JPH0944253A - 駆動切換制御装置 - Google Patents

Abstract

(57)【要約】 【課題】 制御モードの切り換えの際に生じるモータ指
令値の突変を防ぐ。 【解決手段】 制御モードの切り換えの際に、切り換え
直前のモータ指令値と、切り換えられた制御補償手段の
切り換え直前の出力値との差分を、オフセット補償値Δ
ＯＦＳの初期値として切り換え設定手段により算出す
る。切り換え時オフセット生成手段では、初期値として
与えられたΔＯＦＳを修正時間内で徐々に０に近づける
ように減少させて加算手段に出力する。加算手段では、
制御補償手段の出力値とオフセットとを加算して、切り
換え手段を介してモータに出力する。これにより、切り
換え時におけるモータ指令値の突変を防ぐことができ、
修正時間経過後には本来の制御モードに戻ることができ
る。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は駆動系を制御する駆
動切換制御装置に係り、特に、駆動切り換えを滑らかに
接続するために切り換え直前と切り換え直後の駆動指令
値を一致させるように制御することにより駆動切り換え
時に伴う衝撃を無くした駆動切換制御装置に関するもの
である。

【０００２】

【従来の技術】ロボット等のハンド駆動用アクチュエー
タ等を、例えば位置と力（トルク）のような２以上の情
報に基づいて制御することにより、人間の手や腕と同等
な柔軟性を持った機能を目指した技術として特開平５−
２５２７７３号公報に開示された技術などがある。

【０００３】上記従来の技術は、関節トルクセンサを用
いたトルク制御ループと速度制御ループといった２つ以
上の制御回路を並列して備え、モータを制御する場合
に、２つの制御ループを作業の目的に合わせて切り換え
ることにより動作対象物に対して最適な制御を実現しよ
うとするものである。

【０００４】しかし、単純に制御系を切り換えるだけで
は、切り換え時において制御回路の指令値及び現在値も
同時に切り換わり、切り換え後の制御回路の出力が切り
換え前と一致せずに不連続的に突変し、これによりモー
タにショックを与える、という問題がある。

【０００５】このような切り換え時におけるモータへの
ショックを軽減する技術として、特公平６−８５１３７
号公報に開示された技術などがある。同公報に開示され
た位置と力の制御技術は、トルク制御及び速度制御等の
２つ以上の制御回路を並列し、作業の目的に応じて制御
回路を切り換えることによりモータを制御する場合、ト
ルク偏差及び位置偏差にそれぞれ重み付けした和を０と
するようにモータ制御量を決定して制御を容易にすると
共に、これらの重みを変更するモード変更の際に、制御
回路の指令値を制御回路の現在値に置き換えることによ
りスムーズで安定な制御を実現しようとするものであ
る。

【０００６】

【発明が解決しようとする課題】しかしながら、上記特
公平６−８５１３７号公報に開示された従来技術では、
モータ指令値そのものを切り換えの前後で一致させてい
るわけではないので、切り換え時のショックを完全には
無くすことができない。また、重み（ゲイン）を変える
ので、各制御回路の安定性が保証されず、切り換えに要
する時間がかかるという新たな問題点が生じる。

【０００７】本発明は上記従来の問題点を解消するため
になされたもので、制御ループのゲインを一定に保ちな
がらモータ等の駆動手段への指令値そのものを切り換え
の直前と直後で一致若しくは略一致させるようにするこ
とによって、切り換え時の駆動手段へのショックを完全
に無くし、安定かつ高速な切り換え制御を可能とした駆
動切換制御装置を提供することを目的とする。

【０００８】

【課題を解決するための手段】上記目的を達成するため
に請求項１の発明は、駆動指令値に基づいて駆動制御さ
れる駆動手段と、前記駆動手段により動作される可動部
材と、前記可動部材の動作に伴う複数の運動状態量を各
々検出する検出手段と、前記可動部材における前記複数
の運動状態量の目標値を各々設定する目標値設定手段
と、前記検出手段により検出された各々の運動状態量
が、前記目標値設定手段により設定された各々の目標値
に一致するように、前記駆動手段を制御するための指令
値を所定の演算則に従って各々算出する複数の制御補償
手段と、前記複数の制御補償手段のいずれか１つを選択
し、選択された制御補償手段が算出する指令値を、前記
駆動指令値として前記駆動手段に出力するように切り換
える切り換え手段と、前記切り換え手段による切り換え
直前の駆動指令値に基づいて、切り換え直後の駆動指令
値を補償するための補償値を演算し、該補償値に基づい
て切り換え直後の駆動指令値を、切り換え直前の駆動指
令値若しくはその近傍の値となるように制御する制御手
段と、を含むように構成したものである。

【０００９】請求項２の発明は、請求項１の前記制御手
段が、前記切り換え直前の駆動指令値と、前記選択され
た制御補償手段における前記所定の演算則の逆演算則と
に基づいて、前記選択された制御補償手段の制御対象と
なる運動状態量の目標値の補償値を演算し、該補償値に
基づいて、切り換え直後に設定される目標値を、前記選
択された制御補償手段の切り換え直後の指令値が切り換
え直前の駆動指令値若しくはその近傍となるような目標
値に補正することを特徴とする。

【００１０】請求項３の発明は、請求項１の前記制御手
段が、前記切り換え直前の駆動指令値と、前記選択され
た制御補償手段の切り換え直前の指令値との差を補償値
として演算し、該補償値を所定時間内で徐々に０に近づ
けると共に、前記選択された制御補償手段の指令値に加
算して、前記駆動指令値として出力することを特徴とす
る。

【００１１】請求項１の発明では、切り換え手段が、複
数の制御補償手段のいずれか１つを選択し、選択された
制御補償手段が算出する指令値を、駆動指令値として駆
動手段に入力するように切り換える。複数の制御補償手
段が例えば、トルク制御補償手段と速度制御補償手段と
である場合を考える。ロボットアームにとって、一般
に、ロボットアームが外部環境と接触しうる時は、外部
環境に倣うことのできるトルク制御補償手段とすること
が望ましく、一方、高い位置サーボ剛性を望む時は、位
置ゲインを十分に上げることのできる、速度制御補償手
段とすることが望ましい。

【００１２】制御補償手段の切り換え判定は、例えば、
あらかじめ作成された外部環境モデルとロボットアーム
の現在位置を比較することにより、外部環境の接近領域
に入ると予想される時は、トルク制御補償手段に切り換
え、それ以外は速度制御補償手段とすることが考えられ
る。他の判定手段として視覚装置、近接センサ等を用い
てロボットアームと外部環境の接近状態をモニタし、そ
れをもとに、最適の制御補償手段を選択判定することも
可能である。

【００１３】この切り換えの際に、切り換え直前と直後
の駆動指令値とを一致させるためには、切り換え手段に
より選択された制御補償手段の切り換え直後の指令値
が、切り換え直前の他の制御補償手段による駆動指令値
と一致すれば良い。

【００１４】そこで、制御手段では、切り換え手段によ
る切り換え直前の駆動指令値に基づいて、切り換え直後
の駆動指令値を補償するための補償値を演算し、該補償
値に基づいて切り換え直後の駆動指令値を、切り換え直
前の駆動指令値若しくはその近傍の値となるように制御
する。

【００１５】請求項２の発明は、請求項１の前記制御手
段が、まず、切り換え直前の駆動指令値と、選択された
制御補償手段における前記所定の演算則の逆演算則とに
基づいて、選択された制御補償手段の制御対象となる運
動状態量の目標値の補償値を演算する。例えば、選択さ
れた制御補償手段が切り換え直前の駆動指令値を演算出
力するような目標値を逆演算則によって逆算し、該目標
値と切り換え直後において目標値演算手段が設定するは
ずの本来の目標値との差を補償値として演算する。そし
て、該補償値に基づいて、切り換え直後に設定される目
標値を、選択された制御補償手段の切り換え直後の指令
値が切り換え直前の駆動指令値若しくはその近傍となる
ような目標値に補正する。

【００１６】請求項３の発明では、請求項１の制御手段
が、まず、切り換え直前の駆動指令値と、選択された制
御補償手段の切り換え直前の指令値との差を補償値とし
て演算する。そして、該補償値を所定時間内で徐々に０
に近づけると共に、選択された制御補償手段の指令値に
加算して、駆動指令値として出力する。

【００１７】上記各発明により、切り換えの直前と直後
で駆動指令値が一致若しくは略一致し、駆動手 段に衝
撃を与えることなく切り換えを滑らかにすることができ
る。

【００１８】

【発明の実施の形態】

（第１の実施の形態）以下、本発明の実施の形態を図面
に基づいて詳細に説明する。

【００１９】本発明の第１の実施の形態に係る駆動切換
制御装置の構成ブロックを図１に示す。図１に示すよう
に、この駆動切換制御装置の駆動系は、可動部材として
のリンク２６と、このリンク２６を駆動するためのモー
タドライバ及びモータ２２と、を含んで構成されてい
る。モータドライバ及びモータ２２では、入力されたモ
ータ指令値に応じてモータドライバがモータのトルクや
角速度を調節制御する。また、このリンク２６は、例え
ば多軸マニピュレータのように多関節からなるアーム等
からなり、以下、これを前提として説明する。

【００２０】リンク２６とモータドライバー及びモータ
２２との間には、モータ２２のトルクをリンク２６に伝
達するための減速器等から構成される伝達手段２４が介
在されている。なお、リンク２６が多関節からなるアー
ムの場合には、モータドライバ及びモータ２２では、各
関節を関節軸回りに回転させるためのモータが各々用意
されている。そして、モータドライバは、各関節毎のモ
ータ指令値に応じて各々のモータを制御し、伝達手段２
４を介して各関節にトルクを伝達する。

【００２１】さらに、この伝達手段２４とリンク２６と
の間には、リンク２６のアームの関節軸回りに作用する
関節トルクτを検出するトルク検出手段２５が各関節毎
に各々設けられている。ここで、トルク検出手段２５を
伝達手段２４の後、すなわち出力側に取り付けたのは、
伝達手段２４に存在する摩擦分を補償するためである。
なお、アームの各関節において関節軸回りで検出される
トルクや角速度等の物理量を「関節角レベル」と形容す
る。

【００２２】また、モータドライバ及びモータ２２にお
ける各モータには、各モータの回転角度を検出し、該回
転角度に基づいてリンク２６の各関節におけるアーム角
度ｑを演算する角度検出手段２３がそれぞれ取り付けら
れている。後述するように、この角度検出手段２３によ
り検出された各々のアーム角度ｑに基づいて、リンク２
６のアームの手先における移動距離を演算することがで
きる。なお、関節角レベルと区別するため、アームの手
先に関する位置等の物理量を「手先レベル」と形容す
る。

【００２３】以下においては、１回微分（位置に対し速
度、角度に対し角速度、速度に対し加速度等）の表記を
「’」または「・」を付して記載する。すなわち、位置
を表す符号（例えばＰ）に対し、速度は「’」または
「・」、加速度は「”」または「・・」を付して記載す
るが、これらはそれぞれ同一内容を意味するものであ
る。

【００２４】そして、本駆動切換制御装置の制御系は、
リンク２６のアームの手先レベルにおいて実現される複
数の物理量の目標値を各々生成する指令値生成手段１
と、この指令値生成手段１により生成された手先レベル
の目標値を実現するため、モータドライバー及びモータ
２２をフィードバック制御するサーボ演算手段３０と、
を含んで構成されている。後述するように、このサーボ
演算手段３０は、関節角レベルでトルク及び角速度を制
御するトルク制御モードと速度制御モードの２つの制御
モードを備え、これらのモードを切り換えて駆動系を制
御することができる。

【００２５】指令値生成手段１は、リンク２６のアーム
の手先レベルにおける各物理量、すなわち、目標力Ｆd
、目標位置Ｐd 、目標速度Ｐd ’、及び目標加速度Ｐd
”を、それぞれ微分、積分関係を満足するように生成
する目標力生成手段２、目標位置生成手段４、目標速度
生成手段５、及び目標加速度生成手段７とから構成され
ている。

【００２６】また、サーボ演算手段３０は、検出された
関節角レベルの関節トルク値τが、指令値生成手段１に
より生成された手先レベルの目標値を関節角レベルに変
換して得られる関節トルク指令値τ_d に一致するよう
に、Ｇct演算則に従ってモータドライバー及びモータ２
２への指令値を演算出力するトルク制御補償手段１８
と、検出された関節角レベルの関節速度値ｑ’が、指令
値生成手段１により生成された手先レベルの目標値を関
節角レベルに変換して得られる関節速度指令値ｑ_d’に
一致するように、Ｇcv演算則に従ってモータドライバー
及びモータ２２への指令値を演算出力する速度制御補償
手段１９と、を含んで構成されている。なお、これらの
制御補償手段のモータドライバー及びモータ２２への指
令値は、多関節アームの場合、各関節毎に演算出力され
る。

【００２７】トルク制御補償手段１８の入力端には、現
在の関節トルク値τを検出するトルク検出手段２５の出
力端と、生成された目標値に基づいて各関節の関節トル
ク指令値τ_d を演算する関節トルク変換手段１４の出力
端とが接続されている。

【００２８】関節トルク変換手段１４は、手先レベルで
の目標力Ｆd 、目標位置Ｐd と手先レベルの現在位置Ｐ
との重み付け偏差、目標速度Ｐd ’と現在速度Ｐ’との
重み付け偏差、及び目標加速度Ｐd ”の補償値との和
に、現在のアーム角度ｑに基づいて計算された転置ヤコ
ビ行列Ｊ^T （^T は転置行列を示す）を乗じることによ
り、関節トルク指令値τ_d を演算する演算手段であり、
その入力端には、加算手段１２の出力端が接続されてい
る。なお、このように目標力Ｆd の他に、位置偏差及び
速度偏差等に比例する力を加算して関節トルク指令値τ
_d を演算する場合はコンプライアンス制御を行う場合で
ある。

【００２９】加算手段１２は、入力された値を加算する
演算手段であり、その入力端には、目標力生成手段２、
目標位置と手先レベルの現在位置の重み付け偏差を演算
する位置偏差手段９、目標速度と手先レベルの現在速度
の重み付け偏差を演算する速度偏差演算手段１０、及び
加速度補償値を演算する加速度補償手段１１の出力端が
接続されている。

【００３０】位置偏差演算手段９は、生成された目標位
置Ｐd から手先レベルに変換された現在位置Ｐを減算し
た偏差を求める減算器９Ａと、該偏差にゲインＫpwf を
乗じる乗算器９Ｂと、該偏差にゲインＫpwv を乗じる乗
算器９Ｃと、から構成されている。そして、減算器９Ａ
の入力端には、目標位置生成手段４の出力端と、順変換
演算手段１７の出力端とが接続され、この減算器９Ａの
出力端には、乗算器９Ｂと乗算器９Ｃとが各々並列に接
続されると共に、乗算器９Ｂの出力端は加算手段１２の
入力端に、乗算器９Ｃの出力端は加算手段１３の入力端
にそれぞれ接続されている。

【００３１】順変換演算手段１７は、各関節毎に検出さ
れた現在のアーム角度ｑに、該アーム角度ｑに基づいて
計算された順変換行列Λを乗じることによりアームの手
先レベルの現在位置Ｐに変換する演算手段であり、その
入力端には、角度検出手段２３の出力端が接続されてい
る。

【００３２】速度偏差演算手段１０は、生成された目標
速度Ｐd ’から手先レベルに変換された現在速度Ｐ’を
減算した偏差を求める減算器１０Ａと、この減算器１０
Ａから出力された該偏差にゲインＫdwを乗じる乗算器１
０Ｂとから構成されている。そして、減算器１０Ａの入
力端には、目標速度生成手段５の出力端と、ヤコビ行列
乗算手段１６の出力端とが接続されている。

【００３３】ヤコビ行列乗算手段１６は、各関節毎に検
出された現在のアーム角速度ｑ’に、アーム角度ｑに基
づいて計算されたヤコビ行列Ｊを乗算することにより、
アームの手先レベルの速度Ｐ’に変換する演算手段であ
り、その入力端には、微分手段２７の出力端が接続され
ている。微分手段２７は、検出された各関節毎のアーム
角度ｑを１回微分することにより角速度ｑ’に変換する
演算手段であり、その入力端には、角度検出手段２３の
出力端が接続されている。

【００３４】加速度補償手段１１は、各関節毎に検出さ
れた現在のアーム角度ｑに基づいて慣性値Ｍw を計算
し、目標加速度生成手段７から入力された加速度指令値
Ｐ_d ”に該慣性値Ｍw を乗じることにより加速度補償値
を演算する演算手段であり、その入力端には、目標加速
度生成手段７が接続されている。すなわち、関節トルク
指令値τ_d に加速度補償を行うことが可能となってい
る。

【００３５】以上のコンプライアンス制御も同時に行う
時のトルク制御モード時のサーボ演算手段３０の演算
（以下、「サーボ演算」という）の内容を式で示すと、
以下のようになる。なお、このサーボ演算は、所定の周
期（サーボサンプル周期）毎に離散的に演算されるもの
とし、ｋ番目にサンプルされる演算を第ｋサーボサンプ
ルとする（サンプル番号ｋは自然数）。

【００３６】ここで、第ｋサーボサンプルの手先におけ
る目標力をＦｄ（ｋ）、目標位置をＰｄ（ｋ）、目標速
度をＰd ’（ｋ）、目標加速度をＰd"（ｋ）、現在位置
をＰ（ｋ）、現在速度をＰ’（ｋ）とする。

【００３７】まず、目標位置と現在位置との偏差Ｐe
（ｋ）は、 Ｐe （ｋ）＝Ｐd （ｋ）−Ｐ（ｋ） で与えられる。これより、目標速度と現在速度との偏差
Ｐe ’（ｋ）は、上式の両辺を微分すれば、 Ｐe ’（ｋ）＝Ｐd ’（ｋ）−Ｐ’（ｋ） で与えられる。

【００３８】この時、関節トルク変換手段１４によって
演算される関節トルク指令値τ_d （ｋ）は、 τ_d （ｋ）＝Ｊ^T ｛Ｋpwf Ｐe （ｋ）＋ＫdwＰe ’（ｋ） ＋Ｍw Ｐd"（ｋ）＋Ｆd （ｋ）｝ ・・・（１） で与えられる。

【００３９】そして、トルク制御補償手段１８の演算則
をＧct、検出された現在の関節トルク値をτ（ｋ）とす
れば、モータ２２への指令値ｉ_d （ｋ）は、 ｉ_d （ｋ）＝ Ｇct（τ_d （ｋ）−τ（ｋ）） ・・・（２） となる。

【００４０】なお、トルク制御補償手段１８では、通常
ＰＩＤ（比例、積分、微分）制御が行われる。

【００４１】速度制御補償手段１９の入力端には、現在
のアーム速度値ｑ’を演算する微分手段２７の出力端
と、生成された目標値に基づいて各関節の関節速度指令
値ｑ_d’を演算する関節速度変換手段１５の出力端とが
接続されている。

【００４２】関節速度変換手段１５は、手先レベルでの
目標速度Ｐd ’と目標位置Ｐd と手先レベルの現在位置
Ｐとの重み付け偏差との和に、現在のアーム角度ｑに基
づいて計算された逆ヤコビ行列Ｊ^-1（^-1は逆行列を示
す）を乗じることにより関節角レベルに変換された関節
速度指令値ｑ_d ’を演算する手段であり、その入力端に
は、加算手段１３の出力端が接続されている。

【００４３】加算手段１３は、入力された値を加算する
演算手段であり、その入力端には、目標速度生成手段
５、及び前述した乗算器９Ｃの出力端が接続されてい
る。なお、速度制御のみを行う場合は、乗算器９Ｃのゲ
インＫpwv を０とすることによっても可能である。

【００４４】以上のように、位置制御も同時に行う時の
速度制御モード時の演算式は、第ｋサーボサンプルの関
節角レベルの速度指令値ｑ_d ’（ｋ）とすれば ｑ_d ’（ｋ）＝Ｊ^-1｛Ｋpwv Ｐe （ｋ）＋Ｐd ’（ｋ）｝ ・・・（３） で与えられる。

【００４５】そして、速度制御補償手段１９の演算則を
Ｇcv、現在の速度値をｑ’（ｋ）とすれば、モータ２２
への指令値ｉ_d （ｋ）は、 ｉ_d （ｋ） ＝ Ｇcv（ｑ_d ’（ｋ）−ｑ’（ｋ）） ・・・（４） で与えられる。

【００４６】通常、ゲインＫpwv はＫpwf より十分大き
くすることができる。すなわち、速度制御モード時の方
がトルク制御モード時より十分大きな位置ループゲイン
とすることができ、位置サーボ剛性を高め、位置精度を
上げることが可能である。

【００４７】なお、速度制御補償償手段１９でも、通常
ＰＩＤ制御が行われる。これらのトルク制御補償手段１
８及び速度制御補償手段１９の出力端は切り換え手段２
０の入力端に接続されている。

【００４８】切り換え手段２０は、制御モードの切り換
えを指示する切り換え信号に応じて、トルク制御補償手
段１８の指令値と速度制御補償手段１９の指令値のいず
れかを、加算手段２１を介してモータドライバ及びモー
タ２２へ出力するように切り換える手段であり、切り換
え設定手段８の切り換え信号の出力端と接続されてい
る。

【００４９】切り換え設定手段８は、これに接続されて
いる切り換え判定手段２９の判定結果に応じて制御モー
ドの切り換えを切り換え手段２０に指示すると共に、制
御モードの切り換え時に、モード切り換えに伴うモータ
２２への指令値の変動を補償するための力補償値及び速
度補償値を計算する手段である。これらの補償値を演算
するために必要となる現在トルク値、現在速度値及びモ
ータ指令値を得るために、切り換え設定手段８の入力端
には、トルク検出手段２５、微分手段２７、及び切り換
え手段２０の出力端が接続されている。また、演算した
トルク補償値及び速度補償値を出力する切り換え設定手
段８の出力端には、目標力生成手段２、及び目標速度生
成手段５の入力端が接続されている。なお、これらの補
償値に基づいて目標値を補正する詳細な処理については
後述する。

【００５０】切り換え判定手段２９では、リンク２６の
動作に伴う運動状態量に応じて、トルク制御モードと速
度制御モードとのいずれが制御に有効であるかの判定を
行い、その判定の結果を切り換え設定手段８に伝達す
る。

【００５１】さらに、本実施の形態の駆動切換制御装置
では、アームにかかる重力や運動することにより発生す
るコリオリ力、遠心力などの非線形トルク分をモータ２
２への指令値に加算し補償するため、非線形トルク演算
手段２８が設けられている。この非線形トルク演算手段
２８により演算された補償トルク値は、加算手段２１に
よってサーボ演算手段３０が出力したモータ指令値に加
算される。これにより、非線形トルク分がモータ指令値
に反映されて、最適な制御が可能となる。なお、非線形
トルク分の影響が小さい場合は、非線形トルク演算手段
２８を省くことが可能である。

【００５２】次に、第ｋサーボサンプルにおける制御モ
ードの切換処理を図２のフローチャートにより説明す
る。

【００５３】まず、切り換え判定手段２９により制御モ
ードの切り換えか否かを判定する（ステップ１００）。

【００５４】制御モードの切り換えではないと判定した
場合（ステップ１００否定判定）、切り換え設定手段８
においてトルク制御モードか否かを判定する（ステップ
１０８）。

【００５５】トルク制御モードと判定した場合（ステッ
プ１０８肯定判定）、指令値生成手段１において各目標
値の生成を行う（ステップ１１０）。すなわち、目標力
生成手段２が目標力Ｆd （ｋ）を、目標位置生成手段４
が目標位置Ｐd （ｋ）を、目標速度生成手段５が目標速
度Ｐd ’（ｋ）を、そして、目標加速度生成手段７が目
標加速度Ｐd ”（ｋ）をそれぞれ生成する。

【００５６】トルク制御モードにおける目標値が生成さ
れると、サーボ演算手段３０において、（１）式で表さ
れる関節トルク指令値τ_d （ｋ）が演算され、指令値τ
_d （ｋ）と現在トルク値τ（ｋ）とが一致するようにト
ルク制御フィードバック演算が行われる（ステップ１１
４）。すなわち、トルク制御補償手段１８が（２）式で
表されるモータ指令値ｉ_d （ｋ）を出力しモータドライ
バを介してモータ２２のトルク制御を行う。

【００５７】トルク制御モードではないと判定した場合
（ステップ１０８否定判定）、すなわち、速度制御モー
ドの場合、指令値生成手段１において、目標位置生成手
段４が目標位置Ｐd （ｋ）を、目標速度生成手段５が目
標速度Ｐd ’（ｋ）をそれぞれ生成する（ステップ１１
２）。

【００５８】速度制御モードにおける目標値が生成され
ると、サーボ演算手段３０において、（３）式で表され
る関節速度指令値ｑ_d ’（ｋ）が演算され、指令値
ｑ_d ’（ｋ）と現在速度値ｑ（ｋ）とが一致するように
速度制御フィードバック演算が行われる（ステップ１１
６）。すなわち、速度制御補償手段１９が（４）式で表
されるモータ指令値ｉ_d （ｋ）を出力してモータドライ
バ及びモータ２２の速度制御を行う。

【００５９】一方、制御モードの切り換えと判定した場
合（ステップ１００肯定判定）、切り換え設定手段８で
速度制御モードからトルク制御モードに切り換えか否か
を判定する（ステップ１０２）。

【００６０】トルク制御モードに切り換えと判定した場
合（ステップ１０２肯定判定）、切り換え設定手段８に
より速度制御モードからトルク制御モードへの切り換え
補償値演算を実行する（ステップ１０４）。この切り換
え補償値演算は、次のようにして行われる。

【００６１】切り換え直前のモータ指令値をｉ_dc、検出
された関節トルク値をτ（ｋ）とする。そして、切り換
え直後のモータ指令値ｉ_dc ^* を切り換え直前のモータ指
令値ｉ_dcの近傍の値とするために、切り換え時の関節角
レベルトルク指令値τ_dcを、 τ_dc ＝ ｛Ｇct^-1ｉ_dc ｝＋τ（ｋ） ・・・（５） とする。

【００６２】この時、前記（２）式に（５）式を代入す
ると、 ｉ_dc ^* ＝ Ｇct｛Ｇct^-1ｉ_ct＋τ（ｋ）−τ（ｋ）｝＝ｉ_dc となり、ｉ_dc ^* がｉ_dcと一致する。但し、実際は計算誤
差、計測誤差等のために、両者を完全に一致させること
は困難であるが、ｉ_dc ^* をｉ_dcの近傍の値とすることが
できる。

【００６３】なお、アームが６自由度である場合、上記
（５）式は、次の（５−ａ）式のように表される。

【００６４】

【数１】 また、演算則Ｇcti はＰＩＤ制御の場合、 Ｇcti ＝ａ_i ＋ｂ_i ／ｓ＋ｓ・ｃ_i （ｉ＝１〜６） となる。但し、ａ_i 、ｂ_i 、ｃ_i は定数、ｓは微分演算
子として表される。

【００６５】ここで、τ_dcを満たす力制御のみを行う場
合の手先レベル力指令値Ｆdcを、 Ｆdc ＝ Ｊ^-Tτ_dc ・・・（６） とするが、位置、速度制御を同時に行う場合の手先レベ
ル力指令値Ｆdcを、 Ｆdc＝Ｊ^-Tτ_dc−Ｋpwf Ｐe （ｋ）−ＫdwＰe ’（ｋ）−Ｍ_w Ｐd ”（ｋ） ・・・（７） とする。

【００６６】そして、本来の手先レベル力指令値Ｆd
（ｋ）との差ΔＦdcを、 ΔＦdc ＝ Ｆd （ｋ） − Ｆdc ・・・（８） とする。

【００６７】このようにして、切り換え設定手段８によ
りΔＦdcを求め、この補償値を用いて、以下に示すよう
に目標力生成手段２において目標力の補正を行う（ステ
ップ１１０）。

【００６８】目標力生成手段２では、目標力を補正する
ために、ΔＦdcを用いて、現在の目標力Ｆds^* を、 Ｆds^* ＝ Ｆds − ΔＦdc ・・・（９） のように補正を行う。

【００６９】そして、現在の目標力Ｆds^* （ｋ＝０）か
らＴe サーボサンプル後の最終目標力Ｆdeを用いて第ｋ
サーボサイクルの目標力Ｆd ^* （ｋ）を、例えば、 Ｆd ^* （ｋ）＝Ｆds^* ＋｛（Ｆde−Ｆds^* ）／Ｔe ｝・ｋ ・・・（１０） のように生成する。

【００７０】また、最終目標力Ｆdeに達する前にさらに
切り換えが入った場合は、現時点におけるＦd ^* （ｋ）
を目標力Ｆdsとして新たに目標力の補正を行う。

【００７１】なお、単軸回転（１自由度）の場合、アー
ム長をＬとすると、ヤコビ行列Ｊは、Ｊ＝Ｌ（即ち、Ｊ
^-1＝１／Ｌ）と置き換えることができるので、上記
（６）〜（１０）式の代わりに、それぞれ（６’）〜
（１０’）式に基づいて制御すれば良い。

【００７２】 Ｆdc ＝ （１／Ｌ）τ_dc ・・・（６’） Ｆdc ＝（１／Ｌ）τ_dc−ｋ_pwf ｇ_e (k) −ｋ_dwｇ_e '(k)−ｍ_w ｇ_d "( k) ・・・（７’） ΔＦdc ＝ （τ_d (k) − τ_dc）／Ｌ ・・・（８’） τ_d (k) ／Ｌ ＝ τ_ds／Ｌ＋（τ_de／Ｌ−τ_ds／Ｌ）ｋ／Ｔe ・・・（９’） τ_ds／Ｌ ＝ τ_ds／Ｌ − （τ_d (k) − τ_dc）／Ｌ ・・・（１０’） 以上のようにして目標トルクを補正した後、トルク制御
補償手段１８において、補正された目標トルクに基づい
て演算されたトルク指令値τ_d （ｋ）によるトルク制御
モードのフィードバック演算を実行する（ステップ１１
４）。なお、補正された目標力の生成開始と同時に切り
換え設定手段８は切り換え手段２０をトルク制御補償手
段１８側に切り換える。

【００７３】トルク、速度及び位置制御を同時に行う場
合には、現在位置、及び現在速度を目標位置、目標速度
に一致させ、目標加速度を０として、そこから軌道計画
を行っても良い。この場合、（７）式ではなく、簡単な
（６）式を用いることができる。

【００７４】これら、位置、速度、加速度は通常と同
様、微分、積分関係を満足するように目標位置生成手段
４、目標速度生成手段５、目標加速度生成手段７におい
て生成される。詳しい速度の生成方法は次の速度制御モ
ードへの切り換え時で説明する。

【００７５】トルク制御モードから速度制御モードに切
り換える場合（ステップ１０２否定判定）、切り換え設
定手段８において、次のような切り換え補償値演算を実
行する（ステップ１０６）。

【００７６】切り換え直前のモータ指令値をｉ_dc、関節
速度をｑ’（ｋ）とする。切り換え直後のモータ指令値
ｉ_dc ^* を切り換え直前のモータ指令値ｉ_dcの近傍の値と
するために、切り換え時の関節角レベル速度指令値
ｑ_dc’を ｑ_dc’ ＝ ｛Ｇcv^-1ｉ_dc｝＋ｑ’（ｋ） ・・・（１１） とする。

【００７７】この時、（４）式に（１１）式を代入する
と、 ｉ_dc ^* ＝ Ｇcv｛Ｇcv^-1ｉ_dc＋ｑ’（ｋ）−ｑ’（ｋ）｝＝ ｉ_dc となり、ｉ_dc ^* がｉ_dcと一致する。但し、実際は計算誤
差、計測誤差等のために、両者を完全に一致させること
は困難であるが、ｉ_dc ^* をｉ_dcの近傍の値とすることが
できる。

【００７８】（１１）式を満たすように関節角レベル速
度指令値ｑ_dc’を設定するために、手先レベルの速度指
令値Ｐdc’は、速度制御のみを行う場合、 Ｐdc’ ＝ Ｊｑ_dc’ ・・・（１２） とする。

【００７９】また、速度制御と位置制御を同時に行う場
合には、 Ｐdc’ ＝ Ｊｑ_dc’− Ｋpwv Ｐe （ｋ） ・・・（１３） とする。

【００８０】そして、本来の手先レベルの速度指令値Ｐ
d ’（ｋ）との差ΔＰdc’を ΔＰdc’＝ Ｐd ’（ｋ） − Ｐdc’ ・・・（１４） とする。

【００８１】このようにして、切り換え設定手段８によ
りΔＰdc’を求め、この値を用いて、目標速度生成手段
５において目標速度の補正を行う（ステップ１１２）。
この補正方法の詳細については、後述する。

【００８２】以上のようにして目標速度値を補正した
後、速度制御補償手段１９において、補正された目標速
度に基づいて演算された速度指令値ｑ_d ’（ｋ）による
速度制御モードのフィードバック演算を実行する（ステ
ップ１１６）。なお、補正された目標速度の生成開始と
同時に切り換え設定手段８は切り換え手段２０を速度制
御補償手段１９側に切り換える。

【００８３】以上のようにトルク制御モード、速度制御
モードへのいずれの切り換えの場合でも、切り換え時
に、切換直前と直後のモータへの指令値がほぼ一致する
ように目標値を演算することにより、モータ指令値に突
変を生じることなく滑らかに切り換えることが可能とな
る。また、サーボサンプル周期以内で切り換えに必要な
演算を行うことにより、次のサーボサンプルから、直ち
に制御モードを変更することが可能である。

【００８４】次に、目標速度生成手段５の目標速度の生
成方法、目標位置生成手段４の目標位置の生成方法、及
び制御モードの切り換え時における目標速度の補正方法
について、以下に説明する。

【００８５】先ず、目標速度及び目標位置の具体的生成
方法について図３乃至図６により説明する。

【００８６】各サーボサイクル毎の目標速度Ｐd ’
（ｋ）は、目標速度生成手段５により通常、一定の加速
サイクルと一定の速度サイクルとからなる台形状の速度
パターンとなるように生成される。速度制御と同時に位
置制御を行う場合には、手先目標位置Ｐd の生成も行わ
れる。この手先目標位置Ｐd は、目標位置生成手段４に
より目標速度と積分関係を満足するように計算される。

【００８７】目標速度生成手段５は、与えられた移動距
離Ｓに対して、図３（Ｂ）に示すような台形速度パター
ンを満足するように目標速度を決める。すなわち、速度
０の状態から目標速度を正の一定加速度で上げていき
（正の加速サイクル）、目標速度が与えられた最高速度
に達すると、この一定速度を移動距離Ｓと最高速度によ
り定められる所定時間維持し（一定速サイクル）、該所
定時間経過後に、目標速度を負の一定加速度で下げる
（負の加速サイクル）ように設定する。なお、図３
（Ｂ）の場合、正の加速サイクルと負の加速サイクルで
は、加速度の絶対値を等しくするように設定する。

【００８８】図３（Ｂ）の場合、与えられた最高速度を
ｖmax 、加速度をａmax 、サーボサンプル周期をｔとす
ると、速度０の状態から開始した正の加速サイクルは、
（１５）式で与えられる第Ｔａサンプルの直前で完了す
るように設定される。

【００８９】 Ｔa ＝ ｖmax ／ａmax ／ｔ ・・・（１５） また、一定速サイクルは、（１５）式で与えられる第Ｔ
a サンプルから開始され、（１６）式で与えられる第Ｔ
c サンプルの直前で完了するように設定される。

【００９０】 Ｔc ＝ Ｓ／ｖmax ／ｔ ・・・（１６） また、負の加速サイクルは、（１６）式で与えられる第
Ｔc サンプルから開始されるが、加速度の絶対値がａma
x に等しく速度ｖmax から０まで減速するので、正の加
速サイクルと同じサンプル数Ｔa だけ継続するように設
定される。すなわち、第（Ｔa ＋Ｔc ）サンプルでこの
サイクルは完了する。これにより、全サイクルのサーボ
サンプル数Ｔe ＝Ｔa ＋Ｔc となる。

【００９１】ここで、位置制御を同時に行う場合は、現
在の目標位置をＰds、Ｔe サンプル後の最終目標位置を
Ｐdeとすると、移動距離Ｓは、 Ｓ ＝ ｜Ｐde − Ｐds｜ ・・・（１７） で与えられる。但し、｜｜は絶対値を示す。

【００９２】そして、図３（Ｂ）に示した台形速度パタ
ーンの目標速度設定では、第ｋサンプルにおける目標速
度Ｐd ’（ｋ）は、各サイクル毎に、それぞれ次のよう
に設定される。

【００９３】０＜ｋ＜Ｔa の時、すなわち、正の加速サ
イクルの時、 Ｐd ’（ｋ） ＝ ａmax ・ｔ・ｋ ・・・（１８ａ） Ｔa ≦ｋ＜Ｔc の時、すなわち、一定速サイクルの時、 Ｐd ’（ｋ） ＝ ｖmax ・・・（１８ｂ） Ｔc ≦ｋ≦Ｔe の時、すなわち、負の加速サイクルの
時、 Ｐd ’（ｋ） ＝ ｖmax −ａmax ・ｔ・（Ｔe −ｋ） ・・・（１８ｃ） 以上のように各サイクル毎に目標速度が設定され、最終
的なアームの移動距離Ｓが達成される。ここで、サンプ
ル番号ｋと移動距離との関係を図３（Ａ）に示す。

【００９４】図３（Ａ）に示すように、正の加速サイク
ル時には、サンプル番号ｋの増加と共に移動距離は急激
に増加する。一定速サイクルになると、サンプル番号ｋ
と共に、移動距離は一定の増加率で増加する。そして、
負の加速サイクルに入ると、サンプル番号ｋと共に、移
動距離はＳに緩やかに近づいていくことが判る。

【００９５】ところが、与えられた移動距離Ｓが小さ
く、一定加速度ａmax で加速すると、最高目標速度ｖma
x に達する前に、Ｓ／２の距離を移動してしまう場合が
ある。すなわち、次式が成立する場合である。

【００９６】 （ａmax ・Ｓ）^1/2 ＜ｖmax ・・・（１９） かかる場合には、図４（Ｂ）に示すように、一定速サイ
クルを省略し、正と負の加速サイクルのみからなる三角
形の速度パターンで目標速度を設定する。

【００９７】図４（Ｂ）の三角速度パターンの場合、速
度０の状態から開始した正の加速サイクルは、次式で与
えられる第Ｔa1サンプルの直前まで継続するように設定
される。

【００９８】 Ｔa1 ＝ （Ｓ／ａmax ）^1/2 ／ｔ ・・・（２０ａ） この時、最高目標速度ｖmax は、（２０ｂ）式のように
下方修正される。

【００９９】 ｖmax ＝ ａmax ・ｔ・Ｔa1 ・・・（２０ｂ） また、負の加速サイクルは、（２０ａ）式で与えられる
第Ｔa1サンプルから始まり、正の加速サイクルと同じサ
ンプル数Ｔa1だけ継続するように設定される。これによ
り、全サイクルのサーボサンプル数Ｔe ＝２・Ｔa1とな
る。

【０１００】そして、図４（Ｂ）に示した三角速度パタ
ーンの目標速度設定では、第ｋサンプルにおける目標速
度Ｐd ’（ｋ）は、各サイクル毎に、それぞれ次のよう
に設定される。

【０１０１】０＜ｋ＜Ｔa1の時、すなわち、正の加速サ
イクルの時、 Ｐd ’（ｋ） ＝ ａmax ・ｔ・ｋ ・・・（２１ａ） Ｔa1≦ｋ≦Ｔe の時、すなわち、負の加速サイクルの
時、 Ｐd ’（ｋ） ＝ ｖmax −ａmax ・ｔ・（Ｔe −ｋ） ・・・（２１ｂ） ここで、三角速度パターンの場合における移動距離とサ
ンプル番号との関係を図４（Ａ）に示す。図３（Ａ）と
異なり、一定の増加率を示す一定速サイクルが無いこと
が判る。

【０１０２】なお、位置制御を同時に行う場合には、目
標位置Ｐd （ｋ）は、Ｐd ’（ｋ）を積分して得られ
る。

【０１０３】以上の通常の目標速度の設定では、速度０
の状態から正の加速サイクルを開始したが、制御モード
の切り換え直後に目標速度に補正を加える場合には、図
５（Ｂ）及び図６（Ｂ）に示すように初期目標速度をΔ
Ｐdc’として軌道計画を行う。

【０１０４】図５（Ｂ）に示すように、ΔＰdc’（＜ｖ
max ）が正の時は、ｖmax まで加速する時間が短くて済
むので正の加速サイクルが完了するサンプル番号Ｔa2は
図３（Ｂ）のＴa より小さくなる。一方、図６（Ｂ）に
示すように、ΔＰdc’が負の時は、速度を０にするまで
の時間だけ余計にかかるので正の加速サイクルが完了す
るサンプル番号Ｔa3はＴa より大きくなる。

【０１０５】ここで、Ｔa2、Ｔa3は、 Ｔa2、Ｔa3 ＝（ｖmax − ΔＰdc’）／ａmax ／ｔ ・・・（２２） となるように設定される。

【０１０６】また、目標速度Ｐd ’（ｋ）は、０＜ｋ＜
Ｔa2 又は ０＜ｋ＜Ｔa3の時、すなわち正の加速サイ
クルの時、 Ｐd ’（ｋ）＝ａmax ・ｔ・ｋ＋ΔＰdc’ ・・・（２３） となるように設定される。

【０１０７】なお、ｖmax −ΔＰdc’＜０の場合は、一
旦、ｖmax までの減速を行った後、Ｔa2、Ｔa3サイクル
以降の軌道計画を行う。

【０１０８】Ｔa2又はＴa3サイクル以降の一定速サイク
ルや負の加速サイクル時では、補正を行わない時と同様
に軌道計画を行う。すなわち、最高目標速度ｖmax に達
したら、一定速サイクルを実行し、移動距離がＳに達す
るＴa サイクル前から負の加速サイクルを実行する。ま
た、三角速度パターンとなる条件では、一定速サイクル
を省略し、移動距離がＳに達するＴa1サイクル前から負
の加速サイクルを実行する。

【０１０９】図５（Ｂ）及び図６（Ｂ）の速度パターン
の場合におけるサンプル番号と移動距離との関係をぞれ
ぞれ図５（Ａ）及び図６（Ａ）に示す。

【０１１０】以上述べた第１の実施の形態に示した方法
は、後述する第２の実施の形態のように、トルク制御ル
ープ、速度制御ループの内側に切り換え時のモータ指令
値の差分を打ち消すためのオフセット補償分を持たない
ので、各制御ループの指令値と現在値との間に新たな定
常偏差分が加わらない，という利点がある。

【０１１１】（第２の実施の形態）第１の実施の形態で
は、制御モードの切り換え直前のモータ指令値を与える
関節角レベルの指令値を切り換え後の制御補償手段の逆
特性を用いて求め、さらにこの関節角レベルの指令値を
手先レベルの目標値に逆演算し、この切り換え直前に対
応する目標値と、本来の切り換え直後の目標値との差分
に基づいて切り換え後の目標値を補正演算することによ
り、切り換え直前と直後のモータ指令値をほぼ一致させ
るように制御していた。すなわち、制御補償手段の逆特
性に基づく演算等が必要不可欠であった。

【０１１２】これに対し、目標値そのものを補正する演
算を行わず、切り換え直前のモータ指令値と他方の制御
補償手段の切り換え直前の出力との差分をオフセットと
して切り換え後のモータ指令値に与えることにより、切
り換え直前と直後のモータ指令値を一致させることがで
きる。これを第２の実施の形態として以下に開示する。

【０１１３】図７に、第２の実施の形態に係る駆動切換
制御装置の構成ブロックを示す。なお、第１の実施の形
態と同様の構成要件については、同一の符号を付して説
明を省略する。また、第２の実施の形態でも、第１の実
施の形態における各関節のトルク指令値τ_d 及び速度指
令値ｑ_d ’を演算する構成要件及び指令値生成手段１は
必須の構成要件であるが、図７では図示を省略する。

【０１１４】図７に示すように、第２の実施の形態に係
る駆動切換制御装置では、切り換え後のモータ指令値に
与えられるオフセット補償値を生成する切り換え時オフ
セット生成手段３５と、生成されたオフセット補償値を
各々の制御補償手段のモータ指令値に加算する加算手段
３６及び加算手段３７とを新たに含んで構成されてい
る。

【０１１５】加算手段３６は、切り替え時オフセット生
成手段３５に接続されると共に、トルク制御手段１８と
切り換え手段２０との間に、加算手段３７は、切り替え
時オフセット生成手段３５に接続されると共に、速度制
御手段１９と切り換え手段２０との間に各々介在されて
いる。

【０１１６】また、切り換え設定手段８０は、第１の実
施の形態のように目標値の補正分を演算するのではな
く、後述するオフセット初期値を演算する。この切り換
え設定手段８０は、オフセット初期値を与えるため、切
り換え時オフセット生成手段３５に接続されている。

【０１１７】次に、本の実施の形態の切り換え手順につ
いて図８のフローチャートにより説明する。

【０１１８】まず、サンプル番号ｋを１に設定し（ステ
ップ１５０）、オフセット補償値ΔＯＦＳを０に初期化
する（ステップ１５２）。

【０１１９】次に、切り換え判定手段２９により制御モ
ードを切り換えるか否かの判定を行う（ステップ１５
４）。

【０１２０】制御モードの切り換えと判定した場合（ス
テップ１５４肯定判定）、速度制御モードからトルク制
御モードに切り換えるか否かを判定する（ステップ１５
６）。

【０１２１】速度制御モードからトルク制御モードに切
り換える場合（ステップ１５６肯定判定）、切り換え設
定手段８０によりオフセット補償値ΔＯＦＳの初期値演
算が行われる（ステップ１５８）。このオフセット補償
値ΔＯＦＳの初期値は、切り換え直前のモータ指令値ｉ
d 、すなわち速度制御補償手段１９の出力値と、切り換
え直前のトルク制御補償手段の出力値Ｉτの差分として
次式により求められる。 ΔＯＦＳ ＝ ｉd
− Ｉτ ・・・・（２４） 切り換え設定手段８０では、（２４）式のように演算し
たΔＯＦＳを切り換え時オフセット生成手段３５に出力
し、切り換え手段２０をトルク制御側に切り換える。

【０１２２】次に、指令値生成手段１において、目標
力、目標位置、目標速度、及び目標加速度の生成を行う
（ステップ１６４）。この場合、第１の実施の形態のよ
うに切り換え直前と切り換え直後のモータ指令値を一致
させるための目標値の補正は行われない。

【０１２３】次に、サーボ演算手段３０でトルク制御フ
ィードバック演算を行う（ステップ１６８）。この演算
では、まず関節トルク指令値τ_d が演算され、トルク制
御補償手段１８が現在トルク値τにトルク指令値を一致
させるように、モータ指令値を出力する。この時、切り
換え時オフセット生成手段３５によりオフセット補償値
ΔＯＦＳが加算手段３６に入力され、トルク制御補償手
段１８のモータ指令値に加算されて切り換え手段２０に
入力される。切り換え手段２０は、切り換え設定手段８
０により既にトルク制御側に切り換えられているので、
モータドライバー及びモータ２２に入力されるモータ指
令値は次式で与えられる。

【０１２４】 ｉ_d （ｋ）＝｛Ｇct（τ_d （ｋ）−τ（ｋ））｝＋ΔＯＦＳ ・・・・（２５） この時の各部の信号の変化の様子を図９に示す。図９に
示すように、速度制御モードからトルク制御モードに切
り換えられた時、モータ指令値ｉd とトルク制御補償手
段１８の出力値Ｉτには大きな差がある。従って、この
ままΔＯＦＳが加算されていない（２）式のモータ指令
値を用いた場合、切り換え前後で大きな指令値の変化が
起こりモータ２２にショックを与えることになる。しか
し、（２４）式で与えられる切り換え直前のモータ指令
値とトルク制御補償手段の出力値の差分であるオフセッ
ト補償値ΔＯＦＳが加算された（２５）式のモータ指令
値では、図９に示すように切り換えの前後で指令値ｉd
の突変が起こらない。これにより、切り換えを滑らかに
することができる。

【０１２５】次に、ΔＯＦＳの絶対値が０より大きいか
否かの判定を行う（ステップ１７２）。ΔＯＦＳの絶対
値が０より大きい場合（ステップ１７２肯定判定）、切
り換え時オフセット生成手段３５でΔＯＦＳの絶対値を
所定量減少させて０に近づける（ステップ１７４）。

【０１２６】制御を継続する場合（ステップ１７６否定
判定）、サンプル番号ｋをインクルメントし（ステップ
１７８）、ステップ１５４に戻って切り換えか否かの判
定を行う。切り換えが無い場合（ステップ１５４否定判
定）、再び各目標値の生成（ステップ１６４）、トルク
制御フィードバック演算を行う（ステップ１６８）。こ
の演算で、モータ指令値ｉd に加算されるΔＯＦＳは、
その絶対値を減算されているので、１サーボサンプル前
のトルク制御よりも本来のトルク制御に近づいているこ
とになる。そして再び切り換え時オフセット生成手段３
５でΔＯＦＳが０に近づくように減算する（ステップ１
７４）。

【０１２７】同様の処理をモード切り換えが起こらない
限り繰り返すと、所定の修正時間経過後にΔＯＦＳが０
になる。ΔＯＦＳが０になると、図９に示すように、モ
ータ指令値ｉd がトルク制御補償手段１８の出力値Ｉτ
となってトルク指令値τ_d と実際のトルク出力τはより
一致するようになる。すなわち、本来のトルク制御モー
ドが行われる。

【０１２８】トルク制御モードから速度制御モードへ切
り換える場合（ステップ１５６否定判定）も上記と同様
に行う。

【０１２９】まず、切り換え設定手段８０において、Δ
ＯＦＳの初期値を、切り換え直前のモータ指令値ｉd 、
すなわちトルク制御補償手段１８の出力値と、切り換え
直前の速度制御補償手段１９の出力値Ｉv との差分とし
て次式のように求める（ステップ１６０）。

【０１３０】 ΔＯＦＳ ＝ ｉd − Ｉｖ ・・・・（２６） 切り換え設定手段８０では、（２６）式のように演算し
たΔＯＦＳを切り換え時オフセット生成手段３５に出力
し、切り換え手段２０を速度制御側に切り換える。

【０１３１】次に、指令値生成手段１で目標速度と目標
位置を生成し（ステップ１６６）、サーボ演算手段３０
で速度フィードバック演算を行う（ステップ１７０）。
この際に、切り換え時オフセット生成手段３５では、
（２６）式のオフセット補償値ΔＯＦＳを加算手段３７
に出力する。そして、加算手段３７では、速度制御補償
手段１９とΔＯＦＳとを加算して速度制御側に切り換え
られた切り換え手段２０に出力する。

【０１３２】よって、モータ指令値ｉ_d （ｋ）は、 ｉ_d （ｋ）＝｛Ｇcv（ｑ_d ’（ｋ）−ｑ’（ｋ））｝＋ΔＯＦＳ ・・・・（２７） で与えられる。

【０１３３】これにより、トルク制御モードへ切り換え
る場合と同様に、切り換え時にモータ指令値ｉd と速度
制御補償手段１９の出力値Ｉv が一致していなくても、
モータ指令値ｉd は突変せず、モータ２２への衝撃を無
くすことができる。

【０１３４】その後、修正時間の間でΔＯＦＳを徐々に
０に近づけていく（ステップ１７４）ことにより、モー
タ指令値ｉd が速度制御補償手段１９の出力値Ｉv とな
って速度指令値ｑ_d ’と実際の速度ｑ’はより一致する
ようになる。すなわち、本来の速度制御モードが行われ
る。

【０１３５】以上のように、本実施の形態の場合、切り
換えを滑らかにしてモータへの衝撃を少なくする効果と
共に、各目標値の生成手段にまで逆上って改めて計算し
直す必要がないため、演算量が大幅に減る、という効果
がある。

【０１３６】なお、修正時間中におけるΔＯＦＳの変更
の仕方には種々なパターンがある。例えば、図９に示す
ように時間に比例して変更することも可能であるし、他
の時間関数、例えば２次関数のように変更することも可
能である。また、修正時間は一定でも良いし、ΔＯＦＳ
の初期値の大きさに応じて増減しても良い。

【０１３７】また、本実施の形態において、修正時間の
間、安定性を損なわない範囲で、同時に各制御補償手段
中のゲインを小さい値から徐々に大きい値に変化させる
ことも可能である。こうすることにより、各制御補償手
段の出力を切り換え直後は小さくし、その後、徐々に大
きくすることも可能となり、より滑らかな切り換えが可
能となる。

【０１３８】（第３の実施の形態）第２の実施の形態で
は、オフセット補償手段としての切り換え時オフセット
生成手段３５を各制御ループの外側に別個設けたが、各
制御ループの内側に、各制御ループの指令値と現在値の
偏差の積分手段を備える場合は、この手段をオフセット
補償手段として用いることができる。これを第３の実施
の形態として以下に開示する。

【０１３９】図１０に、第３の実施の形態に係る駆動切
換制御装置の構成ブロックを示す。なお、第１の実施の
形態等と同様の構成要件については、同一の符号を付し
て説明を省略する。

【０１４０】図１０に示すように、トルク制御補償手段
１８には、トルク指令値τ_d と現在トルク値τとの差分
に基づいてトルク制御フィードバック演算を行う補償手
段４１と、積分手段４２とが並列に設けられている。そ
して、補償手段４１と積分手段４２との出力値は加算さ
れて出力される構成になっている。

【０１４１】離散系での積分手段４２は次の操作を行な
う。連続系での積分操作 ＫＩ∫（τｄ−τ）ｄｔ を離散系に置き換えると、サンプル周期をＴＳとして Ｋｉｎｔｅｇ（ｋ）＝ＫＩ＊ＴＳ＊（τｄ（ｋ）−τ（ｋ））＋Ｋｉｎｔｅｇ （ｋ−１） （２８） となる。ここでＫＩは積分ゲインあるいは積分時定数と
言われる。

【０１４２】例えば、トルク制御補償手段において、補
償手段４１と積分手段４２が並列に備えられる場合、 Ｋｉｎｔｅｇ（ｋ）＝ＫＩ＊ＴＳ＊（τｄ（ｋ）−τ（ｋ））＋Ｋｉｎｔｅｇ （ｋ−１） （２９） ｉｄ＝Ｇｃｔ（τｄ（ｋ）−τ（ｋ））＋Ｋｉｎｔｅｇ（ｋ） となる。

【０１４３】また、速度制御補償手段１８には、速度指
令値ｑ_d と現在速度値ｑとの差分に基づいて速度制御フ
ィードバック演算を行う補償手段４３と、積分手段４４
とが並列に設けられている。そして、補償手段４３と積
分手段４４との出力値は加算されて出力される構成にな
っている。

【０１４４】そして、切り換え設定手段８１には、切り
換え手段２０、トルク制御補償手段１８、及び速度制御
補償手段１９の出力端が各々接続されている。そして、
切り換え設定手段８１の出力端は、積分手段４２と、積
分手段４４に各々接続されている。

【０１４５】次に、本実施の形態の作用について説明す
る。切り換え判定手段２９が速度制御モードからトルク
制御モードへの切り換えを判定すると、切り換え設定手
段８１は、切り換え手段２０から出力されているモータ
指令値ｉ_d を入力すると共に、トルク速度制御補償手段
１９の出力Ｉτを入力する。そして、（２４）式のよう
にオフセット補償値ΔＯＦＳの初期値を演算してトルク
制御補償手段１８の積分手段４２に積分初期値として与
える。これは（２９）式において、 Ｋｉｎｔｅｇ（ｋ）＝△ＯＦＳ （３０） とすることに相当し、この時、 ｉｄ＝Ｇｃｔ（τｄ（ｋ）−τ（ｋ））＋△ＯＦＳ （３１） となる。（３１）式は前の（２５）式と同じになり、オ
フセット補償値△ＯＦＳを加えたことと同じになり、切
り換え後にモータ指令値に突変を与えない。その後、切
り換え手段２０をトルク制御側に切り換える。

【０１４６】それ以降は、通常積分操作を（２９）式に
従って行なう。これは、切り換え後の積分初期値（（２
９）式においてＫｉｎｔｅｇ（ｋ−１）に相当）を△Ｏ
ＦＳより開始したことに相当する。

【０１４７】トルク制御モードから速度制御モードへの
切り換えの場合も同様に、切り換え設定手段８１は、切
り換え手段２０から出力されているモータ指令値ｉ_d を
入力すると共に、速度制御補償手段１９の出力Ｉv を入
力する。そして、（２６）式のようにオフセット補償値
ΔＯＦＳの初期値を演算して速度制御補償手段１９の積
分手段４４に積分初期値として与える。その後、切り換
え手段２０を速度制御側に切り換える。

【０１４８】以上のように、制御ループの内側に積分手
段を備えている場合には、積分初期値をオフセット補償
手段として用いることができ、制御モードの切り換え時
におけるモータ指令値の突変を無くし、切り換えを滑ら
かに行うことが可能となる。

【０１４９】積分手段を並列して備える場合は、切り換
え時のためだけに用いる特別な補償手段を別に備える必
要がない。また、その後も通常の補償演算を続けるだけ
で済むという利点を有する。

【０１５０】また、本実施の形態において、オフセット
補償量を小さくしたい場合には、各制御ループの指令値
を現在値に一致させる、或いは近づけることを同時に行
っても良い。

【０１５１】また、サーボサンプル周期以内で切り換え
に必要な演算を行うことにより、次のサーボサンプルか
ら直ちに制御モードを変更することが可能である。

【０１５２】以上が本発明の実施の形態であるが、上記
例のみに限定されるものではない。例えば、トルク制御
と速度制御の２つの制御を切り換える場合だけではな
く、他の物理量に関する制御モードへの切り換え、或い
は３以上の制御モードを有する場合にも本発明を適用で
きる。

【０１５３】その他、制御や処理の流れ等についても任
意好適に変更可能である。

【０１５４】

【発明の効果】以上説明したように請求項１〜請求項３
の発明によれば、切り換え直後の駆動指令値は、切り換
え直前の駆動指令値若しくはその近傍になるように制御
されるので、駆動手段に衝撃を与えることなく切り換え
を滑らかにすることができる、という効果が得られる。

【図面の簡単な説明】

【図１】第１の実施の形態に係る駆動切換制御装置の構
成ブロック図である。

【図２】第１の実施の形態に係る駆動切換制御装置の切
り換え制御の流れを示すフローチャートである。

【図３】（Ａ）は、目標速度設定手段５が台形速度パタ
ーンで目標速度を設定する場合におけるサーボサンプル
番号と目標速度の関係、（Ｂ）は、（Ａ）と同じ条件で
サーボサンプル番号と移動距離との関係を示すグラフで
ある。

【図４】（Ａ）は、目標速度設定手段５が三角速度パタ
ーンで目標速度を設定する場合におけるサーボサンプル
番号と目標速度の関係、（Ｂ）は、（Ａ）と同じ条件で
サーボサンプル番号と移動距離との関係を示すグラフで
ある。

【図５】（Ａ）は、制御モードの切り換えに伴うモータ
指令値の突変を防ぐために台形速度パターンで設定され
る目標速度を正の補償値で補正する場合におけるサーボ
サンプル番号と目標速度の関係、（Ｂ）は、（Ａ）と同
じ条件でサーボサンプル番号と移動距離との関係を示す
グラフである。

【図６】（Ａ）は、制御モードの切り換えに伴うモータ
指令値の突変を防ぐために台形速度パターンで設定され
る目標速度を負の補償値で補正する場合におけるサーボ
サンプル番号と目標速度の関係、（Ｂ）は、（Ａ）と同
じ条件でサーボサンプル番号と移動距離との関係を示す
グラフである。

【図７】第２の実施の形態に係る駆動切換制御装置の構
成ブロック図である。

【図８】第２の実施の形態に係る駆動切換制御装置の切
り換え制御の流れを示すフローチャートである。

【図９】速度制御モードからトルク制御モードに切り換
える場合における、モータ指令値ｉ_d 、トルク制御補償
手段１８の出力値Ｉτ、関節トルク指令値τ_d 、オフセ
ット補償値ΔＯＦＳの時間的変化の様子を示した図であ
る。

【図１０】第３の実施の形態に係る駆動切換制御装置の
構成ブロック図である。

【符号の説明】

１ 指令値生成手段 ８ 切り換設定手段 １８ トルク制御補償手段 １９ 速度制御補償手段 ２０ 切り換え手段 ２２ モータドライバー及びモータ ２３ 角度検出手段 ２５ トルク検出手段 ２６ リンク ２７ 微分手段 ３５ 切り換え時オフセット生成手段 ３６ 加算手段 ３７ 加算手段

───────────────────────────────────────────────────── フロントページの続き (72)発明者 津坂 祐司 愛知県愛知郡長久手町大字長湫字横道41番 地の１ 株式会社豊田中央研究所内 (72)発明者 野村 秀樹 愛知県愛知郡長久手町大字長湫字横道41番 地の１ 株式会社豊田中央研究所内 (72)発明者 佐藤 義正 愛知県豊田市トヨタ町１番地 トヨタ自動 車株式会社内

Claims (3)

【特許請求の範囲】
1. 【請求項１】 駆動指令値に基づいて駆動制御される駆
   動手段と、 前記駆動手段により動作される可動部材と、 前記可動部材の動作に伴う複数の運動状態量を各々検出
   する検出手段と、 前記可動部材における前記複数の運動状態量の目標値を
   各々設定する目標値設定手段と、 前記検出手段により検出された各々の運動状態量が、前
   記目標値設定手段により設定された各々の目標値に一致
   するように、前記駆動手段を制御するための指令値を所
   定の演算則に従って各々算出する複数の制御補償手段
   と、 前記複数の制御補償手段のいずれか１つを選択し、選択
   された制御補償手段が算出する指令値を、前記駆動指令
   値として前記駆動手段に出力するように切り換える切り
   換え手段と、 前記切り換え手段による切り換え直前の駆動指令値に基
   づいて、切り換え直後の駆動指令値を補償するための補
   償値を演算し、該補償値に基づいて切り換え直後の駆動
   指令値を、切り換え直前の駆動指令値若しくはその近傍
   の値となるように制御する制御手段と、 を含む駆動切換制御装置。
2. 【請求項２】 前記制御手段は、 前記切り換え直前の駆動指令値と、前記選択された制御
   補償手段における前記所定の演算則の逆演算則とに基づ
   いて、前記選択された制御補償手段の制御対象となる運
   動状態量の目標値の補償値を演算し、該補償値に基づい
   て、切り換え直後に設定される目標値を、前記選択され
   た制御補償手段の切り換え直後の指令値が切り換え直前
   の駆動指令値若しくはその近傍となるような目標値に補
   正することを特徴とする請求項１の駆動切換制御装置。
3. 【請求項３】 前記制御手段は、 前記切り換え直前の駆動指令値と、前記選択された制御
   補償手段の切り換え直前の指令値との差を補償値として
   演算し、該補償値を所定時間内で徐々に０に近づけると
   共に、前記選択された制御補償手段の指令値に加算し
   て、前記駆動指令値として出力することを特徴とする請
   求項１の駆動切換制御装置。