JP2005100143A

JP2005100143A - モータ駆動装置の制御方法及び制御装置

Info

Publication number: JP2005100143A
Application number: JP2003333797A
Authority: JP
Inventors: Yoshiharu Nishida; ▲吉▼晴西田; Yoshihide Inoue; 芳英井上
Original assignee: Kobe Steel Ltd
Current assignee: Kobe Steel Ltd
Priority date: 2003-09-25
Filing date: 2003-09-25
Publication date: 2005-04-14

Abstract

【課題】外界との衝突発生時に安全な位置にモータ駆動装置を停止させる。
【解決手段】多軸のモータ駆動装置の制御装置は、モータ駆動装置が障害物３１と衝突した衝突点３２での作業座標系における衝突時の速度ベクトルＶｃに対して逆向きである逆方向速度ベクトルＶｒ（＝−Ｖｃ）を実現するために必要な、各軸の退避速度ｄθ_ｉ／ｄｔを算出する退避速度器２１と、度退避速度ｄθ_ｉ／ｄｔに基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出器２２を備える。
【選択図】図４

Description

本発明は、溶接ロボットを含む各種ロボットのようなモータが被駆動体を駆動する多軸のモータ駆動装置において、被駆動体と外界との衝突時にモータ駆動装置を安全な位置に停止させるための制御に関する。

ロボットなどのモータ駆動装置の被駆動体が治具などの外界と衝突すると、被駆動体自体や外界が破損する。そのため、被駆動体と外界との衝突を検知し、被駆動体を安全な位置に停止させるための種々の提案がなされている。例えば、特許文献１には、衝突検知時に衝突時の速度と逆方向の速度指令をモータ駆動装置に与えることが記載されている。特許文献２には、衝突検知時に所定量逆方向に移動した位置に被駆動体を停止させることが記載されている。特許文献３には、衝突検知時に逆方向の制動トルク指令をモータ駆動装置に与えることが記載されている。特許文献４には、衝突検知前の位置指令データをもとに衝突検知後の位置データを変更することが記載されている。特許文献５には、衝突を検知した軸は逃動作、他の主軸は停止、手首軸は外力に対して自在に可動とし、所定時間後に全軸を拘束する制御方法が記載されている。さらに、特許文献６には、衝突検知時に外乱を緩和する回転方向にモータ駆動装置を駆動し、逆方向に所定時間制動トルク指令を与えることが記載されている。

上記特許文献１から６に記載された制御は、以下のように大別される。まず、特許文献１、２、及び３に記載のものは、衝突時の速度と逆方向に各軸を駆動する。次に、特許文献４に記載のものは、各軸を衝突前の位置に戻す。また、特許文献５に記載のものは、衝突時に各軸をその場で停止させ、あるいは自在に可動させる。さらに、特許文献６に記載のものは、外乱を緩和する方向に各軸を駆動する。

特許文献１、２、及び３に記載されているように衝突時の速度と逆方向に各軸を駆動した場合、必ずしも衝突を緩和する方向とはならず、安全な位置に停止できるとは限らない。

また、特許文献４に記載されているように衝突前の位置に各軸を戻した場合、モータ駆動装置や周りの環境が衝突によって変化していなければ問題はない。しかし、いずれかの軸が衝突によって駆動不能となったり、治具が衝突によって破損した場合、衝突前の位置に戻ったとしても完全に安全である保障はない。

さらに、特許文献５に記載されているように、各軸をその場で停止あるいは自在に可動とする場合、自在に可動な状態となり得るのはロボットの手首軸などだけで、主軸はその場で停止するしかない。特許文献５に記載の方法では、衝突を検知した軸のみ逃げ動作を行うようにしているが、衝突を検知した軸が実際に衝突した軸である確証はなく、衝突検知した軸の逃げ動作によって、さらなる再衝突を招くおそれがある。また、衝突を検知しなかった軸はその場で停止するが、衝突を検知した軸が逃げ動作を実施しても、他の軸に対する衝突による影響が緩和される保証はなく、停止することによって、衝突による破損を広げる可能性がある。

さらにまた、特許文献６に記載されているように外乱を緩和する方向に駆動した場合、軸単位では外乱を緩和する方向に向かおうとするが、衝突時の外乱の向きに応じて逃げ方向を決定したのでは、ロボットなどの多軸のモータ駆動装置ではかえって衝突を悪化させる可能性がある。

衝突発生時の各軸の停止方向や停止位置には関係ないが、衝突後の被害を低減するための種々の制御も提案されている。例えば、特許文献７には、外乱が閾値以上ならトルクリミットを減少することが記載されている。また、特許文献８には、衝突検知時にゲインを小さくし、トルクを制限することが記載されている。さらに、特許文献９には、衝突時に位置ゲインを小さくすることが記載されている。しかし、これらの制御は衝突時の根本的な安全を確保するものではない。

また、外力に応じたモータ駆動制御に関する提案としては、特許文献１０に外乱に応じて力制御指令を位置指令に加算することが記載されている。さらに、特許文献１１に外乱に応じたコンプライアンス制御量に対して動作方向を制限しながら位置指令に加算することが記載されている。しかし、これはいずれも衝突時を想定したものではなく、衝突時における外界との接触の影響（接触による拘束や障害物からの反力や摩擦力の影響）や、安全な退避方向は考慮されておらず、衝突時の安全を確保するものではない。
特開平３−３６８７号公報特開平６−２４５５６１号公報特開平６−２８４７６４号公報特開平１１−２４５１９１号公報特開平１１−２７７４８３号公報特開２０００−２７１８８６号公報特開平６−２９２３７９号公報特開平１０−１８０６６３号公報特開平１１−１０５８０号公報特開平１０−１５６７７１号公報特開平１１−４２５７５号公報

上記従来の問題に鑑み、本発明は、外界との衝突発生時に安全な位置にモータ駆動装置を停止させることを課題とする。

第１の発明は、多軸のモータ駆動装置の制御方法において、上記モータ駆動装置が外界と衝突した衝突点での作業座標系における衝突時の速度ベクトルに対して逆向きである逆方向速度ベクトルを実現するために必要な、各軸の退避速度又は位置増分を算出し、上記退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法を提供する。

第１の発明において、衝突時の各軸の速度と符号が逆で大きさが同じである逆方向各軸速度又は位置増分を算出し、上記逆方向各軸速度又は位置増分に全ての軸で同一の正の定数を乗算して、上記各軸の退避速度又は位置増分を算出してもよい。

また、第１の発明において、衝突時の各軸の速度と、上記衝突点の上記作業座標系へのヤコビ行列とから、上記衝突時の速度ベクトルを算出し、上記衝突点の上記作業座標系へのヤコビ行列の逆行列情報と、上記衝突時の速度ベクトルとから上記各軸の退避速度又は位置増分を算出してもよい。

第２の発明は、多軸のモータ駆動装置の制御方法において、上記モータ駆動装置と外界の衝突検知時の各軸の外乱情報を取得し、上記衝突検知時の各軸の外乱情報に全ての軸で同一の正の定数を乗算して、衝突から退避するための各軸の退避速度又は位置増分を算出し、上記退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法を提供する。

第２の発明において、上記衝突検知時の各軸の外乱情報と、上記退避速度又は位置増分とのうちの少なくとも一方を、上記モータ駆動装置と外界との衝突点での作業座標系に対する各軸速度のヤコビ行列内で使用される各軸の長さ情報により補正してもよい。

第３の発明は、多軸のモータ駆動装置の制御方法において、上記モータ駆動装置が外界と衝突した衝突点での作業座標系における外力ベクトル方向を求め、上記外力ベクトル方向に沿って衝突点を移動させるために必要な各軸の退避速度又は位置増分を算出し、上記退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法を提供する。

第４の発明は、多軸のモータ駆動装置の制御方法において、上記モータ駆動装置が外界と衝突した衝突点での作業座標系における弾性変位ベクトルに沿って、衝突点を移動させるために必要な各軸の退避速度又は位置増分を算出し、上記算出された退避速度又は位置増分に基づいて、各軸の位置、速度、トルクを制御することを特徴とする、モータ駆動装置の制御方法を提供する。

第５の発明は、多軸のモータ駆動装置が外界と衝突した衝突点を移動させる制御方法において、上記衝突検知時の作業座標系での速度ベクトルに対して逆向きである逆方向速度ベクトルと、上記衝突検知時の各軸の外乱情報に基づいて求めた上記衝突点での作業座標系での速度ベクトルと、上記衝突点での作業座標系における外力ベクトルと、上記衝突点での作業座標系における弾性変位ベクトルとのうち、いずれか２つ以上のベクトル方向によって内包されるベクトル方向に、上記衝突点を移動させるために必要な退避各軸速度又は位置増分を算出し、上記退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法を提供する。

上記衝突点に代えて、各軸のリンク先端点と、最先端の軸に設けられた治具取付用のフランジ面とを含む衝突点として想定される点を使用してもよい。

第６の発明は、衝突を検知するための評価値を算出し、該評価値と閾値の比較により外界との衝突を検知する、多軸のモータ駆動装置の制御方法において、第１の評価値と第１の閾値との比較により、いずれかの軸と外界との衝突が検知されると、上記第１の評価値と上記第１の閾値よりも衝突検知の判断が成立しやすい第２の閾値との比較、上記第１の評価値よりも衝突の検知感度が高い第２の評価量と上記第１の閾値との比較、又は上記第２の評価値と上記第２の閾値との比較に基づいて、上記第１の評価値と第１の閾値との比較により外界との衝突が検知された軸以外に外界と衝突した可能性のある軸を推定し、上記推定の結果に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法を提供する。

第７の発明は、多軸のモータ駆動装置が外界と衝突した衝突点を移動させる制御方法において、衝突時の各軸の速度、外乱情報、弾性変形角度、又は軸力を求め、固有振動数を含む特定周波数成分を除去して上記衝突時の各軸の速度、外乱情報、弾性変形角度、又は軸力を補正し、補正後の各軸の速度、外乱情報、弾性変形角度、又は軸力に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法を提供する。

第８の発明は、衝突を検知し、１つ以上の手法により衝突後の退避方向を決定する多軸のモータ駆動装置の制御方法において、上記１つ以上の手法によって決定された退避各軸速度又は位置増分の向きと一致しない向きに対しては、位置制御又は速度制御の比例ゲインを第１の値に設定し、積分ゲインを０相当に設定し、又はトルクの上下限値の範囲を第１の範囲に設定し、上記１つ以上の手法によって決定された全ての退避各軸速度又は位置増分の向きと一致する向きに対しては、上記位置制御又は速度制御の比例ゲインを上記第１の値よりも大きい第２の値に設定し、上記積分ゲインを上記０相当よりも大きい値に設定し、又は上記トルクの上下限値の範囲を第１の範囲よりも大きい第２の範囲に設定する、ことを特徴とする、モータ駆動装置の制御方法を提供する。

第９の発明は、多軸のモータ駆動装置の制御装置において、上記モータ駆動装置が外界と衝突した衝突点での作業座標系における衝突時の速度ベクトルに対して逆向きである逆方向速度ベクトルを実現するために必要な、各軸の退避速度を算出する退避速度算出手段と、上記退避速度で算出された退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段とを備えることを特徴とする、モータ駆動装置の制御装置を提供する。

第１０の発明は、多軸のモータ駆動装置の制御装置において、上記モータ駆動装置の外界との衝突検知時の各軸の外乱情報を算出する外乱情報算出手段と、上記外乱情報算出手段により算出された上記衝突検知時の各軸の外乱情報に全ての軸で同一の正の定数を乗算し、衝突から退避するための各軸の退避速度を算出する退避速度算出手段と、上記退避速度算出手段によって算出された退避速度に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段とを備えることを特徴とする、モータ駆動装置の制御装置を提供する。

第１１の発明は、多軸のモータ駆動装置の制御装置において、上記モータ駆動装置が外界と衝突した衝突点での作業座標系における外力ベクトル方向を算出する外力ベクトル算出手段と、上記外力ベクトル方向に沿って、衝突点を移動させるために必要な各軸の退避速度を算出する退避速度算出手段と、上記退避速度算出手段によって算出された退避速度に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段とを備えることを特徴とする、モータ駆動装置の制御装置を提供する。

第１２の発明は、多軸のモータ駆動装置の制御装置において、上記モータ駆動装置が外界と衝突した衝突点での作業座標系における弾性変位ベクトルに沿って、衝突点を移動させるために必要な各軸の退避速度又は位置増分を算出する退避速度又は位置増分算出手段と、上記退避速度又は位置増分算出手段によって算出された該退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段とを備えることを特徴とする、モータ駆動装置の制御装置を提供する。

第１３の発明は、多軸のモータ駆動装置の制御装置において、上記モータ駆動装置の外界との衝突を検知する手段と、上記衝突時の作業座標系での速度ベクトルに対して逆向きである逆方向速度ベクトルと、上記衝突検知時の各軸の外乱情報に基づいて求めた衝突点での作業座標系における速度ベクトルと、上記衝突点での作業座標系における外力ベクトルと、上記衝突点での作業座標系における弾性変位ベクトルとのうち、いずれか２つ以上のベクトル方向によって内包されるベクトル方向に、上記衝突点を移動させるために必要な各軸の退避速度を算出する、退避速度算出手段と、上記退避速度算出手段によって算出された退避速度に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段とを備えることを特徴とする、モータ駆動装置の制御装置を提供する。

第１４の発明は、衝突を検知するための評価値を算出する手段と、該評価値と閾値の比較により外界との衝突を検知する検知する検知手段とを備える、多軸のモータ駆動装置の制御装置において、上記検知手段による第１の評価値と第１の閾値との比較により、いずれかの軸と外界との衝突が検知されると、上記第１の評価値と上記第１の閾値よりも衝突検知の判断が成立しやすい第２の閾値との比較、上記第１の評価値よりも衝突の検知感度が高い第２の評価量と上記第１の閾値との比較、又は上記第２の評価値と上記第２の閾値との比較に基づいて、上記第１の評価値と第１の閾値との比較により外界との衝突が検知された軸以外に外界と衝突した可能性のある軸を推定する、推定手段と、上記推定手段による推定結果に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段とを備えることを特徴とする、モータ駆動装置の制御装置を提供する。

第１５の発明は、多軸のモータ駆動装置の制御装置において、衝突時の各軸の速度、外乱情報、弾性変形角度、又は軸力を求める手段と、固有振動数を含む特定周波数成分を除去して上記衝突時の各軸の速度、外乱情報、弾性変形角度、又は軸力を補正する補正手段と、補正手段により補正された各軸の速度、外乱情報、弾性変形角度、又は軸力に基づいて、各軸の位置、速度、又はトルクを算出する指令算出手段とを備えることを特徴とする、モータ駆動装置の制御装置を提供する。

本発明では、逆方向速度ベクトル、外乱情報、外力ベクトル、又は弾性変位ベクトルに基づいて、衝突時の各軸の位置、速度、又はトルクを制御するので、モータ駆動装置が外界と衝突した際に、衝突を緩和する安全な位置に確実にモータ駆動装置を退避させることができる。

（第１実施形態）
図１は、モータ駆動装置の一例である３リンクのマニピュレータを示し、リンク１〜３と、パラレル駆動型の関節軸Ｓ１，Ｓ２と、シリアル駆動型の関節軸（以下、単に軸という。）Ｓ３とを備える。また、図１においてθ１〜θ３は各軸の関節角度を示す。

この３リンクのマニピュレータにおいて図２に示されるような衝突が発生した場合、マニピュレータと障害物（外界）３１との衝突点３２における作業座標系での速度は、衝突時の各関節角速度（各軸速度）による衝突点３２の速度の合成速度として記述される。

この時、単に各軸速度と逆方向に所定の速度で駆動し（上述の特許文献１参照。）、あるいは単に各軸速度と逆方向に所定量移動した位置に停止した場合（上述の特許文献２参照）を図３示す。全ての軸Ｓ１〜Ｓ３の移動量は同一としている。この図３から明らかなように、単軸毎の判断により退避するべくとった退避行動によって、結果として得られた衝突点３２での合成速度が、障害物方向へ移動することとなってしまい、よりいっそう衝突による被害を悪化させることになる。換言すれば、単軸毎の衝突時の速度の向きによって退避方向を決定しても、安全な方向に退避できるとは限らず、作業座標系において、衝突点３２が障害物３１から離れる方向に退避することが重要である。

本実施形態は、障害物３１から離れる方向への退避の一例として、衝突時における衝突点３２の作業座標系での速度ベクトルＶｃと逆方向の速度ベクトルＶｒ（逆方向速度ベクトル）を各軸速度dθｖｉ/dｔ（添字ｉは第ｉ目の軸Ｓｉを示す。）によって実現するものである。

逆方向速度ベクトルＶｒを実現するために必要な各軸速度dθｖｉ/dｔは、以下の式（１）で示すように衝突時の各軸速度dθｃｉ/dｔに、所定の負の係数−Ｗｖ（Ｗｖは全軸同じ値）を乗じることで算出できる。

図４は本実施形態のモータ駆動装置の制御装置を示す。コントローラ１１は軌道計画１２を実現するための指令値としてトルクτをマニピュレータ１３に出力し、それに従ってマニピュレータ１３のモータ（図示せず）が駆動され。エンコーダ１４により検出された各軸の関節角度θｉはコントローラ１１にフィードバックされる。また、モータ制御装置は、外乱算出器１５、評価量算出器１６、及び比較器１７を備えている。外乱算出器１５はコントローラ１１から入力されるトルクτ、エンコーダ１４から入力される関節角度θｉを使用して各軸の外乱ｄｉを時々刻々と算出する。評価量算出器１６は外乱ｄｉを使用して衝突検知のための評価量を算出し、この算出値は比較器１７により閾値と比較される。比較器１７の比較結果より、衝突評価量が閾値以上であれば、コントローラ１１は衝突であると判定し、マニピュレータ１３を安全な位置に停止させるための必要な処理を実行する。

衝突時の処理を実行するために、コントローラ１は退避速度算出器２１と、指令算出器２２とを備える。

退避速度算出器２１は、逆方向速度ベクトルＶｒを実現するために必要な各軸の速度（退避速度）dθｖｉ/dを上記式（１）により算出する。退避速度算出器２１は、まず衝突時の各軸の速度dθｃｉ/dｔに−１を乗じ、速度dθｃｉ/dｔと符号が逆で大きさが同じである逆方向各軸速度−dθｃｉ/dｔを算出する。次に、退避速度算出器２１は、逆方向各軸速度−dθｃｉ/dｔに全ての軸で同一の正しい定数Ｗｖを乗じて、各軸の退避速度dθｖｉ/dを算出する。

指令算出器２２は、退避速度算出器２１が算出した各軸の退避速度dθｖｉ/dに基づいてマニピュレータ１３を制御して衝突点を移動させる。この制御は各軸の位置、速度、及びトルクのいずれにより実行してもよい。

図２で示す衝突に対して退避速度算出器２１により算出された退避速度dθｖｉ/dｔを衝突検知時の退避各軸速度指令値としたときの結果を図５に示す。退避速度dθｖｉ/dｔを速度指令として与える退避行動によって、衝突点３２での退避時の速度ベクトルは必ず逆方向ベクトルＶｒとなり、衝突の影響を緩和する方向に退避することができる。この図４では所定の負の定数−Ｗｖとして−１を与えたときの例であるが、上述のようにＷｖが正ならば、退避時に同様の速度ベクトルを得ることができる。

また、衝突時の各軸の位置θｃｉ（ｉはＳｉ軸の位置を示す）に、位置増分として退避速度dθｖｉ/dｔを加算し、以下の式（１）’で算出された各軸の位置ないしは関節角度θｖｉを、衝突検知後の退避各軸位置指令値としても、同様に衝突点を安全な方向に移動させることができる。

ここで注意すべき点は、衝突時の各軸速度と逆向きに所定量移動、あるいは所定速度で移動することは、決して衝突点３２の逆方向ベクトルＶｒ方向への移動を保証するものではないということである。上記図５の例から明らかであるが、逆方向ベクトルの方向に移動するためには、向きだけではなく、その移動させる移動量や速度の大きさが重要であり。特許文献１，２に記載のもののように所定量移動するのではなく、向きと大きさを制御することによって初めて逆方向ベクトルＶｒに沿って衝突点３２を移動させることができる。

（第２実施形態）
第２実施形態は退避速度算出器２１が各軸の退避速度dθｖｉ/dｔを算出する方法が第１実施形態と異なる。

退避速度算出器２１による各軸の退避速度dθｖｉ/dｔの算出方法は、第１実施形態のものに限定されず、衝突点１での衝突時の作業座標系での速度ベクトルＶｃを算出すれば、逆方向速度ベクトルＶｒを実現するために必要な各軸の退避速度dθｖｉ/dｔの決定方法は多数考えられる。

衝突時の作業座標系での速度ベクトルＶｃは、衝突点３２あるいは衝突点として考えられる点（リンクの先端点など）における、関節座標系から作業座標系へのヤコビ行列Ｊｂと、衝突時の関節角速度（各軸速度）から以下の式（２）により導出することができる（ただし^Ｔは転置であることを示す）。

衝突時の速度ベクトルＶｃが与えられれば、例えばＪｂの擬似逆行列Ｊｂ^Ｔ×（Ｊｂ×Ｊｂ^Ｔ）^−１を用いて、以下の式（３）から各軸の退避速度dθｖｉ/dｔを決定することができる。

図１に示す３リンクのマニピュレータにおいては（ほとんどの垂直多関節ロボットにおいて同様のことがいえる）、先端の姿勢（図ではリンク３の向いている方向）の変化速度を限定しなければ、逆方向速度ベクトルＶｒを実現するために必要な各軸の退避速度dθｖｉ/dｔの組み合わせは無限に存在し、その決定方法も無限に存在する。式（３）よって導出された退避速度dθｖｉ/dｔは、無限に存在する組み合わせの中で、［dθｖ１/dｔ dθｖ２/dｔ dθｖ３/dｔ・・・］^Ｔのユークリッドノルムを最小にするdθｖｉ/dｔの組み合わせとなる。

式（３）によって決定された退避速度dθｖｉ/dｔを用いて図１のマニピュレータ１３を制御した時の結果を図６に示す。この図６から明らかなように、第１実施形態（図５参照）と同様に、衝突点における（合成速度）速度ベクトルが衝突時と逆向きになり、衝突を緩和する方向にマニピュレータ１３を退避させることができる。

図３の例では軸Ｓ２は衝突時の速度と反対向き（正転方向）に駆動されているが、図５に示すように本実施形態では軸Ｓ１の衝突点３２で逆方向速度ベクトルＶｒを実現するために、軸Ｓ２が衝突時の速度と同じ向き（逆転方向）に駆動され、本実施形態の制御方法が特許文献１，２などと大きく異なることが分かる。

式（１），（３）式から算出された各軸速度に対して、係数ｗ１，ｗ３を乗算した線形和として退避速度dθｖｉ/dｔを与え、以下の式（３）’から得られる退避速度dθｖｉ/dｔに基づいて制御しても、作業座標系において逆方向速度ベクトルを実現することができる。ただしｗ１，ｗ３は正負いずれでもよいが、ｗ１＋ｗ３＞０を満たすものとする。

（第３実施形態）
第３実施形態も退避速度算出器２１が退避速度dθｖｉ/dｔを算出する方法が第１実施形態と異なる。

本実施形態では、式（３）ではなく、いずれかの軸Ｓｋの速度をｖｋに固定し、その軸Ｓｋを除くヤコビ行列Ｊｂｋの逆行列Ｊｂｋ^−１を用いて以下の式（４）により各軸の退避速度によりdθｖｉ/dｔを決定する。

速度ｖｋを固定する軸Ｓｋは、衝突時の関節角速度が最も遅い軸（図２では軸Ｓ１が該当する。）、作用する外乱と速度の向きが同じ軸（図２では軸Ｓ２が該当する。）、あるいは衝突時の関節角速度が最も速い軸（図２では軸Ｓ３が該当する）などの観点で選ぶことができる。衝突時の関節角速度が最も遅い軸は、衝突時の速度が遅い場合には衝突に関する寄与が少ないと推定することができる。また、作用する外乱と速度の向きが同じ軸は衝突に関する寄与が少ないと推定することができる。さらに、衝突時の関節角速度が最も速い軸は退避時の速度変化が最も大きいと推定することができる。

図２に示す衝突が発生した場合に軸Ｓｋとして軸Ｓ３を選び、ｖ３＝０として式（４）から算出した各軸の退避速度ｄθｖ／ｄｔに基づいてマニピュレータ１３を退避させた例を図７に示す。この図７から明らかなように、第１実施形態（図５参照）と同様に、衝突点における（合成速度）速度ベクトルが衝突時と逆向きになり、衝突を緩和する方向にマニピュレータ１３を退避させることができる。また、軸Ｓ３の速度ｖ３を０とすることにより、衝突前後での軸Ｓ３における速度の変化が、第１実施形態（図５参照）や第２実施形態（図６参照）に比べて小さくなる。速度変化が小さい程制御による応答遅れが短くなり、速度変化が大きい程トルクの飽和などによって実際の速度が指令値に近付く時間が長くなるので、より素早く衝突後の退避行動をとることができる。

（第４実施形態）
本発明の第４実施形態は、退避速度算出器２１が外乱情報に基づいて各軸の退避速度を算出する点が第１から第３実施形態と異なる。

図２の衝突において、図８に示すように障害物３１からの反力と摩擦力が外力としてマニピュレータ１３に作用した場合、外力によって各軸に作用する外乱モーメントｄｉ（以降、外乱と呼ぶ）が発生する。図９は各軸毎に外乱ｄｉを緩和する方向にマニピュレータ１３を所定速度で駆動した場合（特許文献６参照）を示す。この図９の例では衝突点３２は障害物３１から離れる方向に退避することができる。しかし、図１０に示すような２リンクのマニピュレータ１３に対して、衝突時に外力として反力と摩擦力が作用する場合、外乱ｄｉを緩和する方向に所定速度で駆動しても、図１１に示すように衝突点３２で合成速度は障害物３１に向かう。このように各軸の外乱ｄｉを緩和する方向にマニピュレータを駆動する退避行動では、衝突による被害を悪化させる場合がある。

本実施形態では退避速度算出器２１に外乱算出器１５から外乱ｄｉが入力される。退避速度算出器２１は、以下の式（５）に示すように、衝突時の各軸外乱情報ｄｉに所定の正の係数Ｗｄ（Ｗｄは全軸同じ値）を乗算して、各軸の退避速度ｄθｄｉ/ｄｉを算出する。

図１２は図８に示す衝突において、式（５）により算出された各軸の退避速度ｄθｄｉ/ｄｔでマニピュレータ１３を駆動した結果を示す。同様に、図１３は図１０に示す衝突において、式（５）により算出された各軸の退避速度ｄθｄｉ/ｄｔでマニピュレータ１３を駆動した結果を示す。正の係数Ｗｄとして１を使用した。これら図１２及び図１３から明らかなように、式（５）により算出したｄθｄｉ/ｄｔを速度指令として与える退避行動によって、図８及び図１０に示す衝突においても、衝突を緩和する方向にマニピュレータ１３を退避させることができる。

なお、図１２及び図１３では、上述のように式（５）の正の係数Ｗｄとして１を与えているが、係数Ｗｄが正であれば同様に退避時の速度ベクトルを得ることができる。

また、衝突時の各軸の位置θｃｉに、位置増分としてdθｄｉ/dｔを加算し、以下の式（５）’で算出されたθｄｉを衝突検知後の退避各軸位置指令値として与えてもよい。

（第５実施形態）
図１４は、図９と同様に２リンクであるがリンク２が短いショートアーム２リンクマニピュレータ１３が障害物３１と衝突した状態を示す。障害物３１と衝突しているのは短いリンク２であり、外力として反力と摩擦力が作用している。この時、リンク２が短くなっているため、リンク２のリンク長に比例して、以下の２種類の影響がある。まず、軸Ｓ２に作用する外乱が小さい（影響１）。また、軸Ｓ２による衝突点３２（リンク２の先端）の速度が小さい（影響２）。

図１５は、図１４に示す衝突において第４実施形態の式（５）で与えられる各軸の退避速度ｄθｄｉ/ｄｔでマニピュレータ１３を駆動した結果を示す。この図１５と図１３を比較すれば明らかなように、上記リンク長に比例した２種類の影響により、軸Ｓ２による衝突点３２の速度がリンク２のリンク長の自乗に比例して小さくなっている。そして、軸Ｓ２による衝突点３２の速度が小さくなったことで、退避時の速度ベクトルが障害物３１の方向へ向いている。

本実施形態では、退避速度算出器２１は、上記２種類の影響を考慮して各軸の退避速度を算出する。

まず、図１４に示す衝突における影響１に対する対策としては、以下の式（６）に示すように、衝突時の各軸の外乱ｄｉを各軸のリンク長Ｌｉで除算して補正する。

この式（６）により算出された各軸の外乱ｄ_Ｌｉに対して、以下の式（７）に示すように第４実施形態と同様に所定の正の係数Ｗ_Ｌ（Ｗ_Ｌは全軸同じ値）を乗算する。

さらに、影響２に対する対策として、式（７）により得られた各軸の速度dθ_Ｌｉ/dｔを、以下の式（８）に示すようにさらにＬｉで除算し、得られたdθ_ＬＬｉ/dｔを、衝突検知時の各軸の退避速度とする。

図１６は、図１４に示す衝突において式（６）〜（８）によって算出された退避速度dθ_ＬＬｉ/dｔを用いてマニピュレータ１３を制御した結果を示す。この図１６から明らかなように、dθ_ＬＬｉ/dｔを速度指令として与える退避行動によって、衝突点での退避時の速度ベクトルは必ず衝突時の速度ベクトルの逆方向となり、衝突を緩和する方向にマニピュレータ１３を退避させることができる。

また、衝突時の各軸の位置θｃｉに、位置増分としてdθ_ＬＬｉ/dｔを加算し、以下の式（８）’で算出されたθ_ＬＬｉを衝突検知後の退避各軸位置指令値として与えてもよい。

本実施形態では、式（６），（８）においてリンク長Ｌｉによる除算を実行するので、結果的には第４実施形態の各軸の退避速度ｄθｉ/ｄｔをリンク長Ｌｉの自乗で除算していることになる。しかし、式（６），（８）のいずれか一つのみを実施することによっても、より確実にマニピュレータを安全な方向に退避させることができる。

（第６実施形態）
本発明の第６実施形態は、退避速度算出器２１が見かけ上のリンク長さを使用して上記リンク長さに比例する２種類の影響を除去する点が第５実施形態と異なる。図１７は一般的な垂直多関節ロボットにおける見かけ上のリンク長を示している。

図１の３リンクのマニピュレータ１３を例に見かけ上のリンク長を使用した補正について説明する。ヤコビ行列Ｊｂは以下の式（９）で表される。

この式（９）において、ｊｂｉｎはｉ行ｎ列のＪｂの要素、Ｌ１，Ｌ２，Ｌ３は各軸Ｓ１〜Ｓ３のリンク長、Ｌｐ１，Ｌｐ２，Ｌｐ３は各軸Ｓ１〜Ｓ３の見かけ上のリンク長さである。また、Ｌｐ１＝Ｌ１，Ｌｐ３＝Ｌ３、Ｌｐ２＝√{Ｌ２^２＋Ｌ２^２×Ｌ３×ｃｏｓ(θ３)＋Ｌ３}、かつφ２＝ｔａｎ^−１{Ｌ３×ｓｉｎ(θ３)／(Ｌ２＋Ｌ３×ｃｏｓ(θ３)))とする。

式（９）から明らかなように、見かけ上のリンク長Ｌｐｉは以下の式（１０）により与えられる。‖＊‖は＊のユークリッドノルムを示す。

この見かけ上のリンク長Ｌｐｉの大きさに比例して以下の２種類の影響がある。まず、外力によってＳｉ軸に作用する外乱が変化する（影響１’）。また、Ｓｉ軸の関節角速度による衝突点の速度が変化する（影響２’）。従って、以下の式（６）”〜（８）”に示すように、第５実施形態の式（６）〜（８）におけるリンク長Ｌｉに代えて、見かけ上のリンク長Ｌｐｉを使用して衝突時の外乱ｄｉなどを補正することで、より正確にリンク長に比例する影響を除去することができる。

なお、式（６）”，（８）”のいずれか一つのみを実施することによっても、第４実施形態よりもより確実にマニピュレータを安全な方向に退避させることができる。

（第７実施形態）
本発明の第７実施形態では、退避速度算出器２１は衝突時の外力ベクトルの方向に退避するように各軸の退避速度を算出する。

図１４の衝突において図示の摩擦力よりも大きな摩擦力が作用した場合、第５実施形態の式（６），（８）や第６実施形態の式（６）”，（８）”で得られる退避速度dθ_ＬＬｉ/dｔに基づく制御では、衝突点３２での合成速度が、障害物方向へ移動することとなってしまい、よりいっそう衝突による被害を悪化させる可能性がある。一方、第１から第３実施形態の逆方向ベクトルの方向への退避では、衝突によるモータ駆動装置や障害物の破損や変形がない限り、必ず安全な方向に退避することができる。しかし、衝突検知時に既に破損変形があった場合には、衝突時の速度と逆方向に駆動しても再衝突の可能性が、必ず安全な方向に退避できるとは限らない。

衝突による破損変形などがあった場合においても、それ以上の破損を防ぎ、より安全な方向に退避する一つの方法としては、モータ駆動装置に作用する外力の作業座標系でのベクトル（外力ベクトルＦ）方向に退避すれば、必ず安全な方向に退避することができる。外力は障害物から受ける反力や摩擦力などによって構成され、そのベクトル方向は常に障害物から離れる方向にある。従って、外力ベクトルＦの方向（外力を緩和する方向）に退避すれば必ず、障害物から離れる方向にモータ駆動装置が駆動され、安全な方向に移動することができる。

各軸に作用する外乱ｄｉは、仮想仕事の定理から外力ベクトルＦとヤコビの転置行列Ｊｂ^Ｔを用いて以下の式（１０）で表される。

また、各軸速度dθｉ/dｔによる衝突点の作業座標系における速度ベクトルＶ_Ｃは以下の式（１１）で与えられる。

所定のスカラ正定数Ｗｆに対して以下の式（１２）で与えられるdθｆｉ/dｔを衝突検知時の各軸の退避速度とすると、作業座標系での衝突点の速度ベクトルＶｆは、式（１０）〜（１２）より以下の式（１３）で与えられる。

式（１３）から明らかなように、作業座標系での衝突点の速度ベクトルＶｆの方向は、外力ベクトルＦの方向と一致している。従って、dθｆｉ/dｔを各軸の退避速度とすれば、外力ベクトルＦの方向に退避することができる。

本実施形態では、退避速度算出器２１は式（１０）〜（１２）により、各軸の退避速度dθｆｉ/dｔを算出し、指令値算出器２２は算出された退避速度dθｆｉ/dｔに基づいて、各軸についての位置指令値、速度指令値、又はトルク指令値を決定する。なお、式（１０）の演算で使用される外乱ｄｉは外乱算出器１５から退避速度算出器２１に入力される。

また、衝突時の各軸の位置θｃｉに、位置増分としてdθｆｉ/dｔを加算し、以下の式（１３）’で算出されたθｆｉを、衝突検知後の退避各軸位置指令値として与えてもよい。

図１０及び図１３の２リンクのマニピュレータ１３のヤコビ行列Ｊｂは以下の式（１４）で与えられる。

この時、ヤコビ行列Ｊｂの逆行列Ｊｂ^−１は以下の式（１５）で与えられる。

式（１５）から式（１２）式は、以下の式（１２）’で与えられる。

あるいは、１／｛１−ｃｏｓ^２(θ２−θ１)｝^２は１以上のスカラー量であるため、１／｛１−ｃｏｓ^２(θ２−θ１)｝^２を正の係数Ｗｆに含めれば、式（１２）は以下の式（１２）”，（１２）’’’で与えられる。

これらの式（１２）”，（１２）’’’に示すように、リンク長Ｌｉの逆数、及び行列Ｈにより外力ベクトルＦ方向に退避するための各軸の退避速度dθｆｉ/dｔを算出することができる。

式（１２）'''からもわかるように、ヤコビの逆行列を用いた場合、第５実施形態の式（６），（８）のような形に式（１２）を変形することが可能であり、この式変形を行った場合には本実施形態は第５実施形態に含まれる。以上から、見かけの上のリンク長Ｌｐｉ（図１０及び図１３のマニピュレータ１３ではリンク長Ｌｉと見かけ上のリンク長Ｌｐｉは一致している。）の逆数によって外乱ｄｉやdθｆｉ/dｔを補正することの正しさを確認できる。

図１８は、図１３の衝突に対し式（１２）’，（１２）”，又は（１２）'''により算出した各軸の退避速度dθｆｉ/dｔに基づく退避の結果を示す。この図１８から明らかなように、dθｆｉ/dｔを速度指令として与える退避行動によって、衝突点での退避時の速度ベクトルは必ず衝突時の速度ベクトルの逆方向となり、衝突を緩和する方向にマニピュレータ１３を退避させることができる。

ここで注目すべき点は、各軸の外乱ｄｉを緩和する向き（図１６参照）と、外力を緩和するために必要な各軸速度の向き（図１８）が必ずしも一致しないことである。図１６の軸Ｓ１は外乱ｄｉを緩和するために正転方向に駆動されているが、図１８の軸Ｓ１は外力を緩和するために逆転方向に駆動されている。すなわち外力を緩和する向きと、各軸に加わる外乱を緩和する向きとは必ずしも一致せず、各軸の外乱を緩和する方向への退避行動（特許文献６参照）が決して安全である保証はない。また外力を緩和するためには、向きだけではなく、その速度や位置増分の大きさも重要であり、向きと大きさを制御することによって初めて安全な方向に退避することができる。

第１から第３実施形態における逆方向速度ベクトルＶｒの場合と同様に、図１に示す３リンクのマニピュレータにおいては（ほとんどの垂直多関節ロボットにおいて同様のことがいえる）、先端の姿勢（図ではリンク３の向いている方向）の変化速度を限定しなければ、外力ベクトルＦ方向に駆動するために必要な各軸速度dθｆｉ/dｔの組み合わせは無限に存在し、その決定方法も無限に存在する。例えば、式（３）と同様に擬似逆行列を用いた以下の式（１６）で導出された退避速度dθｆｉ/dｔは、無限に存在する組み合わせの中で、［dθｆ１/dｔ dθｆ２/dｔ dθｆ３/dｔ・・・］^Ｔのユークリッドノルムを最小にするdθｆｉ/dｔの組み合わせとなる。

また、式（１６）のほかに、第３実施形態の式（４）と同様に、いずれかの軸Ｓｋの速度をｖｋに固定し、その軸Ｓｋを除くヤコビ行列Ｊｂｋの逆行列Ｊｂｋ^−１を用いて以下の式（１７）によりdθｆｉ/dｔを決定することも可能である。

速度を固定する軸の選び方は、第３実施形態で列挙したものの他、例えば外乱ｄｉの小さな軸など（図ではＳ１軸）、外乱に起因した選別方法も考えることができる。

図２に示す衝突が発生した場合に軸Ｓｋとして外乱ｄｉが最も小さい軸であるＳ１軸を選び、ｖ１＝０として式（１７）から算出した各軸の退避速度dθｆｉ/dｔに基づいてマニピュレータ１３を退避させた例を図１９に示す。この図１９から明らかなように、衝突点における（合成速度）速度ベクトルが衝突時と外力ベクトルＦと同方向となり、衝突を緩和する方向にマニピュレータ１３を退避させることができる。

また、外力ベクトルＦ方向に駆動するために必要な各軸速度dθｆｉ/dｔの組み合わせを決定する方法が無限に存在するように、各軸の外乱ｄｉから外力ベクトルＦを算出する方法も無限に存在する。例えば擬似逆行列（Ｊｂ×Ｊｂ^Ｔ）^−１×Ｊｂを用いて以下の式（１８）により外力ベクトルＦを算出することができる。

あるいはヤコビ行列Ｊｂｋの逆行列Ｊｂｋ^−１を用いて以下の式（１８）’により外力ベクトルＦを算出することができる。

式（１０）の関係を満足していれば、式（１８），（１８）’のいずれを使用しても同じ外力ベクトルＦを得ることができる。しかし、一般には外乱ｄｉにはモデル化誤差や計測誤差などによる誤差があり、その誤差の影響によって式（１８），（１８）’のいずれを使用するかで実際には算出される外乱ベクトルＦが異なる。

式（１８）によって算出される外力ベクトルＦは、最小自乗の意味で外乱ｄｉの誤差の影響が最も軽減された値である。また、式（１８）’においても、外乱ｄｉの値が最も小さい軸をＳｋ軸として選ぶ（誤差の影響を最も受けやすい軸をＳｋ軸として選んで、計算から除外する）ことによって、外乱ｄｉの誤差の影響を低減することができる。

なお、上述の第２実施形態においてマニピュレータが図９及び図１３で示す２リンクのマニピュレータである場合には、式（３）のように擬似逆行列を用いる必要はない。２リンクのマニピュレータでは、擬似逆行列と通常の逆行列が一致するため、式（３）は以下の式（３）’と等価である。

また、第７実施形態の式（１５）の関係を適用すれば、式（３）は以下の式（３）''と等価である。

さらに、正の係数Ｗｖに正のスカラ量１／｛１−ｃｏｓ^２(θ２−θ１)｝を含めれば、式（３）は以下の式（３）'''で表すことができる。

（第８実施形態）
本発明の第８実施形態では、退避速度算出器２１は弾性変位ベクトル方向の退避動作を実現する各軸の退避速度を算出する。

第７実施形態の外力ベクトルＦ方向の退避以外に、衝突による破損や変形などがあった場合においても、それ以上の破損を防ぎ、より安全な方向に退避する方法としては、外力による弾性変位のベクトル方向への退避がある。

図２０を参照すると、通常、産業用ロボットなどのモータ駆動装置では、モータ２５とリンク２６を連結する減速機２７がバネ要素として作用し、弾性変形する。図２１に示すように、この種のモータ駆動装置では、衝突時の外力によって弾性変形が発生し、衝突点３２の位置も変位する。この弾性変形による作業座標系での衝突点４の変位を表すベクトル（弾性変位ベクトル）の方向は、必ず障害物３１から離れる方向にある。この点に関し図２１に基づいて説明すると、仮に弾性変位ベクトルが障害物３１の方向（図２１において下向き）に向いていたとすれば、弾性変形しなかった場合の衝突点３２の位置は障害物３１より図において上にあることとなり、障害物３１と接触していないこととなる。すなわち障害物３１と接触していないのに衝突したこととなり、矛盾が生じる。したがって弾性変位ベクトルが障害物３１方向（図において下向き）に向くことはありえず、必ず障害物３１から離れる方向となる。

次に、弾性変位ベクトル方向の退避動作を実現する各軸の退避速度の算出方法を説明する。弾性変形として、図２１に示すような各軸の弾性変形のみを考える。外力による各軸の弾性変形角度εｉは以下のように算出又は推定可能である。まず、外力による弾性変形角度εｉを含めた全ての弾性変形角度εａｉ（外力に加え、自重や慣性力や遠心コリオリ力などによる弾性変形角度εｏｉと外力による弾性変形角度εｉを含む全ての弾性変形角度）を推定可能である。ここで外力以外のモーメント成分ｍｏｉはモータを駆動させるために必要な必要駆動トルクであり、運動方程式モデルから算出あるいは推定可能である。モーメン成分ｍｏｉによる弾性変形角度εｏｉは、予め同定などによって与えることが可能な各軸のバネ定数Ｋｉから以下の式（１９）で表される。

以下の式（２０）に示すように、外力による各軸の弾性変形角度εｉは、全弾性変形角度εａｉからモーメン成分ｍｏｉによる弾性変形角度εｏｉを差し引くことで算出することができる。

また、一般に衝突検知では各軸の外乱ｄｉを算出していることが多く、既に外乱ｄｉが算出されていれば、外乱ｄｉを軸力と仮定し、外乱ｄｉと各軸のバネ定数Ｋｉとから、以下の式（２１）によって外力による各軸の弾性変形角度εｉの大まかな値を算出することができる。

この式（２１）はあくまで近似式であり、弾性変形角度εｉの正確な値を算出するためには、式（２０）や後述する式（３２），（３３）を使用する必要がある。特に、外乱ｄｉと外力による各軸の弾性変形角度εｉの向きは必ずしも一致するとは限らない。

式（２０），（２１）などで算出又は推定された外力による各軸の弾性変形角度εｉと弾性変位ベクトルＥの間には以下の式（２２）の関係が成り立つ。

式（２２）により得られた弾性変位ベクトルＥを用いれば、第１実施形態の式（３）の場合と同様に、以下の式（２３）から各軸の退避速度dθｅｉ/dｔ（ただしＷｅは正のスカラー量）が得られる。

式（２３）より以下の式（２４）の関係が成り立つ。

すなわち、各軸の退避速度dθｅｉ/dｔによる衝突点の速度ベクトルＶｅの方向と、弾性変位ベクトルＥの方向とが一致する。従って、式（２３）から得られるdθｅｉ/dｔを、衝突検知時の退避各軸速度指令値とすることで、衝突を緩和する退避を実現することができる。

衝突時の各軸の位置θｃｉに、位置増分としてdθｅｉ/dｔを加算し、以下の式（２３）’で算出されるθｅｉを、衝突検知後の退避各軸位置指令値として与えてよい。

また、式（２２），（２３）にように弾性変位ベクトルＥを算出しなくても、弾性変形角度εｉから以下の式（２５）により退避速度dθｅｉ/dｔを算出することができる。

式（２２）より以下の式（２６）が成り立つので、式（２５）で算出した退避速度dθｅｉ/dｔを使用しても、衝突点の速度ベクトルＶｅの方向と、弾性変位ベクトルＥの方向とが一致する。

図１０や図１４に示す２リンクのマニピュレータ１３では、擬似逆行列と通常の逆行列が一致するので、以下の式（２５）’に示すように、式（２３）は式（２５）と等価となる。

本実施形態では、退避速度算出器２１は、式（２２），（２３）あるいは式（２５）により各軸の退避速度dθｅｉ/dｔを算出し、指令算出器２２は算出された退避速度dθｅｉ/dｔにより決定した指令値（位置、速度、又はトルク）をマニピュレータ１３に出力する。

図２２は、図２１に示す衝突において式（２５）により算出した各軸の退避速度dθｅｉ/dｔを速度指令として与える退避行動を示す。衝突点３２での退避時の速度ベクトルは弾性変位ベクトルＥの方向と同じ方向となり、マニピュレータ１３は衝突を緩和する方向に退避している。

（第９実施形態）
本実施形態では退避速度算出器２１は、第１実施形態の逆方向速度ベクトルＶｒ（−Ｖｃ）、第７実施形態の外力ベクトルＦ、及び弾性変位ベクトルＥから衝突時の移動方向を算出する。

上述のように逆方向速度ベクトル方向Ｖｒ（モータ駆動装置や障害物に破損がない場合）、外力ベクトルＦ、又は弾性変位ベクトルＥ方向に退避すれば、必ず障害物から離れる方向に動作できる。すなわち、図２３に示すように、これら３つベクトル方向に内包されるベクトルであれば必ず障害物から退避することができる。内包されるベクトル方向Ｖｈは、以下の式（２７）に示すように非負のスカラー量Ｗｖ，Ｗｆ，Ｗｅにより逆方向速度ベクトルＶｒ、外力ベクトルＦ、及び弾性変位ベクトルＥの線形和で与えることができる。

退避速度算出器２１は式（２７）のベクトル方向Ｖｈを実現するように各軸の退避速度ｄθｉ／ｄｔを決定する。

また、上記３つのベクトル、第４実施形態の各軸の外乱ｄｉを緩和する方向の各軸外乱速度（式（５））に基づいて求めた作業座標系での速度ベクトルＶｄを考慮して以下の式（２７）’によりベクトル方向Ｖｈを決定してもよい。Ｗｄは正のスカラー量である。

式（２７），（２７）’における正のスカラー量Ｗｖ，Ｗｆ，Ｗｅ，Ｗｄは、外乱や各軸速度の向きや大きさによって変更してもよい。また、上記４つのベクトルのうち、いずれか２つ以上のベクトルからベクトル方向Ｖｈを決定してもよく、この場合には式（２７）’において使用しないベクトルに乗じるスカラー量を０とすればよい。

（第１０実施形態）
以上の実施形態ではマニピュレータ１３の先端が衝突点であったが、図２４に示すようにリンクの途中で衝突があった場合、厳密に衝突点の位置を特定することは困難である。このような衝突の場合、例えば衝突した軸Ｓ４のリンク先端点やマニピュレータそのもの先端点、計算時間短縮のために治具などを取り付ける治具取り付け用のフランジ面、あるいは主軸の先端点などを衝突点の代わりに代用し、第１から第９実施実施形態を適用してもよい。

（第１１実施形態）
外乱情報を用いて衝突を検知する場合、一般に外乱情報から得られる衝突評価値が閾値を超えたことによって衝突を検知するのであるが、誤検知を防止するためにその閾値は高めに設定されており、どの軸が最小に閾値を超えるか想定がつかない。図２４のような衝突においては、Ｓ４軸が衝突しているのであるが、Ｓ１〜Ｓ３軸で衝突が最初に検知され、その衝突検知軸を中心に衝突検知後の退避運動を決定すれば、Ｓ４軸が適切に退避可能か疑問である。

そこで、本実施形態では衝突検知のための比較器１７は２種類の閾値を用いて、衝突が発生した軸を推定する機能を有する。詳細には、第１の閾値と評価量算出器１６で算出された衝突評価量との比較によりいずれかの軸で衝突が検知されれば、衝突検知用の閾値より低い（衝突検知の判断が成立しやすい）第２の閾値を用いて、衝突した可能性のある軸を検索し、衝突した可能性のある軸の内、最も根元から遠い（先端から近い）軸を選び、その軸において衝突が発生したものとする。この推定結果を用いて退避速度算出器２１と指令算出器２２が、衝突後の退避行動を決定することで、よりいっそう的確な退避行動を行うことができる。

例えば、図２４において軸Ｓ３で最初に衝突が検知されたとする。この時、第２の閾値を用いて、衝突評価値と第２の閾値を比較して、衝突した可能性のある軸を検索した結果、軸Ｓ２，３，４が衝突した可能性があることがわかったとする。この結果から、根元（図２４では軸Ｓ１）から最も遠い軸Ｓ４にて衝突が発生したものとして（例えばＳ４軸のリンク先端が衝突点と仮定して）第１から第９実施形態を適用すればよい。

また、衝突した可能性がある軸よりも先端側の軸（図２４では軸Ｓ４よりも先端側の軸Ｓ５，Ｓ６）については、退避行動をせず、衝突時の位置で停止、あるいは可動自在にするなどの処置を行うことによって、衝突後の再衝突の可能性を低減あるいはそれによる被害を最小限にとどめることができる。

最初の衝突検知に使用された衝突評価量（第１の衝突評価量）よりも衝突の検知感度が高い第２の衝突評価量と上記最初の衝突検知に使用された閾値（第１の閾値）との比較、又は上記第２の評価値と上記第１の閾値よりも衝突検知の判断が成立しやすい第２の閾値との比較に基づいて、外界と衝突した可能性のある軸を推定してもよい。

（第１２実施形態）
衝突した可能性がある軸よりも先の軸については、停止あるいは自在に可動とする代わりに、コントローラ１１が位置や速度の比例ゲインを小さくする、あるいは位置や速度の積分ゲインを０（あるいは０相当の値（０相当の値とは退避行動が終了するまでに積分ゲインによるトルク値が大きく変化しない程度に小さく、あるいの積分ゲインによる積分値の上限値を小さくする））にするなどの対策も有効である。

また、衝突した可能性がある軸より根元の軸に対しては、まず第１から第９実施形態のいずれか１つ以上の方法を選出し、選出された全ての方法による各軸の退避速度の向きを算出する。

選出された全ての方法による退避各軸速度の向きが一致した場合、各軸が退避すべき向きは信頼にたる向きであり、退避各軸速度と逆の向きに対しては、位置あるいは速度あるいは電流制御の比例ゲインを小さく、あるいは積分ゲインを０（あるいは０相当の値）に、あるいはトルクリミットを小さく設定する。一方、退避各軸速度と同じ向きに対しては、ゲインあるいはトルクリミットを逆向きよりも大きく設定することによって、退避すべき向きには高速に移動するとともに、その逆向きには移動しないように制御することができる。

また、選出された全ての方法による退避各軸速度の向きが一致なかった場合、正逆転どちらの向きに移動するのが正しいのか判断が分かれる状況であり、このような疑わしい状況ではどちらの向きに対しても、位置あるいは速度あるいは電流制御の比例ゲインを小さく、あるいは積分ゲインを０（あるいは０相当の値）に、あるいはトルクリミットを小さく設定する。これにより、選出されたいずれかの方法による誤った退避行動の影響を緩和することができる。

上記の内容を系統立てて実施する一つの方法としては、選出された方法によって算出された全ての各軸の退避速度の逆向き方向に対しては、位置、速度、あるいは電流制御の比例ゲインを小さく、積分ゲインを０（あるいは０相当の値）、あるいはトルクリミットを小さく設定すればよい。すなわち、全ての軸の退避速度の逆向きが一致した場合、その向きに対して小さくし、一致しなかった場合両方の向きに対して小さくすればよい。

このようにいずれかの方法によって算出された各軸の退避速度の向きに応じてゲインを変更することによって、誤った退避の向きによる退避行動の影響を緩和しつつ、正しいと思われる退避の向きに関してはより素早く退避することができる。またトルクリミットの中心点を、重力項などのモデルから算出可能なトルク情報をもとに変更すれば、よりいっそうの効果を得ることができる。このような各軸の退避速度あるいは位置増分の向きに応じたゲインあるいはリミット変更は、与えられた退避各軸速度あるいは位置増分の向きの信頼性が重要であり、従来技術のように安全でない方向に退避する可能性があったのでは実現できない。上記第１から第９実施形態により算出される確実に安全な方向に退避できる各軸の退避速度に基づいて、上記変更を行う必要がある。

（第１３実施形態）
図２５に示すように衝突時の外乱が固有振動による振動成分とランプ状の外乱で構成される場合、外乱の向きを反対に検出する可能性がある。外乱情報に基づいて各軸の退避速度を算出する第４実施形態、外乱を使用して求めた外力ベクトルＦに基づいて各軸の退避速度を算出する第７実施形態、及び外乱を使用して求めた弾性変位ベクトルＥ方向に基づいて各軸の退避速度を算出する第８実施形態では、外乱の向きを誤検知すると誤った退避行動をとる可能性がある。

従って、第４、第７、及び第８実施形態では、評価量算出器１６が以下の方法で外乱ｄｉから固有振動含む高周波による振動成分を除去し、退避速度算出器２１は固有振動による振動成分を除去済みの外乱に基づいて各軸の退避速度を算出する構成とすることで、より確実に衝突を緩和する方向への退避動作を実現することができる。

評価量算出器１６は以下の式（２８）で示す固有振動（角速度ω_ｎ）を中心としたバンドパスフィルタ処理を施すバンドパスフィルタを備える。このバンドパスフィルタ処理により、外乱ｄｉから外乱成分ｄｎｉを抜き出す。Ωはフィルタが通過させるバンド幅を指定するパラメータである。

ランプ状成分の外乱成分ｄｌｉは以下の式（２９）で得られる。

（第１４実施形態）
衝突検知時には、ランプ状外乱ｄｌｉよりも振動成分ｄｎｉの方が大きな場合が多い。第１３実施形態において説明したように、振動成分ｄｎｉを除去することによってランプ状外乱ｄｌｉの向きを正しく判別することは可能であるが、衝突検出時の外乱の大きさはランプ状外乱ｄｌｉよりも振動成分ｄｎｉから判別する方がより正しい判定を行うことができる。しかし、振動成分の外乱ｄｎｉの大きさは時々刻々変化するため、振動成分の外乱の大きさを正しく判断することは困難である。

第４、第７、及び第８実施形態において、評価量算出器１６と退避速度算出器２１を以下の構成とすることで、正確に判断した振動成分の外乱の大きさに基づいて、より確実に衝突を緩和する方向への退避動作を実現することができる。

まず、評価量算出器１６は以下の式（３０）で定義される固有振動を含む高周波の振動成分の外乱ｄｎｉに対する外乱パワーＰ_ｄｎｉを算出する。ただし、ｄｎｉ’はｄｎｉの変化速度、ωｎｉはＳｉ軸の固有振動各速度である。

また、退避速度算出器２１は、式（３０）により得られた外乱パワーＰ_ｄｎｉを使用して以下の式（３１）で定義される外乱ｄｃｌｉを算出する。

図２６に外乱ｄｃｌｉの挙動を示す。固有振動周期などによって外乱ｄｃｌｉの大きさは大きく変化せず、またその向きもランプ状外乱ｄｌｉと一致した方向を示し、向き及び大きさも安定して検出可能である。従って、外乱ｄｃｌｉの大きさや向きなどを用いて、退避行動を決定することで、より有効な退避行動を決定することができる。

図２０のような弾性変形する軸を有する場合、その運動方程式モデルは、以下の式（３２）で記述され、第１３実施形態や第１４実施形態の方法でリンクに作用する外乱Ｄを正確に推定することができる。

しかし、従来の衝突検知などに用いられている外乱推定オブザーバでは、Θｌが計測不可能であるため、式（３２）から軸力Ｋ×Θεを消去し、以下の式（３３）で示す運動方程式において、ＤではなくＤ⁻を外乱として扱っている。

そのため、Ｄ⁻に含まれる外乱Ｄ以外の成分の影響によって、外乱の従来の算出あるいは推定値には図２５のような振動成分が重畳される。

また、従来の衝突検知ための外乱としては、式（３３）式を仮定しながら、外乱推定を行う外乱としてＤ⁻＋Ｃｌなどの値を推定しておき、後で推定された外乱値（Ｄ⁻＋Ｃｌ）＾からＣｌ(dΘｍ/dｔ，Θｍ)を差し引くなどの行為を行っているが、結局、式（３３）を仮定して外乱Ｄ⁻を算出推定していることと等価である、あるいはその変形例に過ぎない。

このようなＤ⁻中のＤ以外の成分（固有振動などの振動成分）は、衝突を検知する上で重要な情報であるが、退避行動を決定するためにＤ⁻そのものを用いれば第１３実施形態で説明したように、誤った判断が行われるため、Ｄ⁻から振動成分を除去したＤ（図２４中のランプ状外乱ｄｌｉに相当）や、Ｄと振動成分を分離し、それぞれの情報を有効に活用したｄｃｌｉなどによって退避行動を決定することが重要である。

あるいは外乱を推定する従来技術として、ｄｉ⁻から固有振動成分を除去するものがあるが、衝突時にはｄにもいくらかの振動成分が含まれるため、一律に固有振動成分を除去することが正しいとは言えず、正しくモデルを反映させた第１３実施形態や第１４実施形態により正しい値を推定することが可能である。

同様に、式（３３）のｄｉの代わりにＤ⁻の第ｉ要素ｄｉ⁻を用いた場合、算出された各軸の弾性変形角度εｉ⁻にも振動成分が重畳されるため、第１３実施形態と同様に振動成分の除去、あるいは分離が必要で、分離されたεｉ⁻の振動成分を第１３実施形態の式（２９）と同様に有効に活用し、より正確な退避方向をえることも可能である。

また、ｄｉ⁻を用いた式（３１）では、各軸のｄｉ⁻が軸力Ｋｉ×εｉ⁻と一致すると仮定した式であり、軸力に対して同様の処置（振動成分の除去や除去された振動成分の活用など）を行っても同様の結果を得ることができる。

さらに、上記から自明なとおり、従来の外乱退避に用いられる外乱推定あるいは算出によって得られる外乱値は、いずれも本当の外乱ｄを表しておらず、第１３実施形態や第１４実施形態の外乱値を用いることによって、初めてロボットなどの多軸のモータ駆動装置に対する外乱を正確に算出することが可能であり、従来の退避方法に用いられていた外乱推定値と各軸速度あるいは位置増分による外乱推定値とは明らかに異なり、退避行動に活用すべく情報は、衝突検知などに使用する外乱値ではなく、固有振動などの振動成分を除去した（あるいは振動成分を別途活用した）値や、振動成分の影響を受けない第１３実施形態や第１４実施形態の外乱値の方が好ましい。

（第１６実施形態）
第８実施形態において弾性変位ベクトルＥの算出に使用する各軸の弾性変形角度ε_ｉは、以下の方法で算出することができる。まず、軸力Ｋｉ×εｉを正確に推定することが可能で、例えば算出あるいは推定された軸力Ｋｉ×εｉから以下の式（３４）によって、各軸の弾性変形角度εｉを算出可能である。

軸力Ｋｉ×εｉを算出するためには、式（３０）より得られる以下の式（３５）から算出することができる。

また、上式に基づいて、軸力がオフセット状に変化すると仮定すれば、オフセット外乱推定オブザーバ（通常のオブザーバ）と同様に推定可能である。あるいは式（３５）のＫ×Θεに対して何らかのフィルタリング処理を行い、フィルタリング後の値を用いても同様の結果（フィルタリング処理によるが）を得ることができる。

あるいは軸力がランプ状に変化すると仮定し、そのランプ状に変化する軸力を推定することも可能である。

式（３５）により得られる軸力の算出値あるいは推定値を用いて弾性変位ベクトルＥを算出することで、第８、第９、第１０、及び第１３実施形態において、より確実に衝突を緩和する方向の退避行動を決定することが可能である。

また、式（３３）に基づく軸力や、弾性変形角度εｉを用いたとしても、モデル化誤差などの影響によって、いくらかは振動成分がその推定値あるいは算出値に重畳される可能性がある。このため上記の振動周期成分の除去あるいは分離は、算出あるいは推定される各軸の弾性変形角度εｉや軸力Ｋｉ×εｉの推定値あるいは算出値についても有効に作用する。

３リンクのマニピュレータの一例を示す概略図である。図１のマニピュレータが障害物に衝突した状態を示す概略図である。従来の方法による退避行動を示す概略図である。第１実施形態に係るモータ駆動装置の制御装置を示すブロック図である。第１実施形態によるマニピュレータの退避行動の一例を示す概略図である。第２実施形態によるマニピュレータの退避行動の一例を示す概略図である。第３実施形態によるマニピュレータの退避行動の一例を示す概略図である。図１のマニピュレータの衝突の一例を示す概略図である。図８の衝突に対する従来の退避行動の一例を示す概略図である。２リンクのマニピュレータの衝突の一例を示す概略図である。図１０の衝突に対する従来の退避行動の一例を示す概略図である。第４実施形態によるマニピュレータの退避行動の一例を示す概略図である。第４実施形態によるマニピュレータの退避行動の他の一例を示す概略図である。ショートアーム２リンクマニピュレータが障害物と衝突した状態を示す概略図である。図１４の衝突に対する第４実施形態の退避動作を示す概略図である。図１４に示す衝突に対する第５実施形態の退避動作を示す概略図である。一般的な垂直多関節ロボットにおける見かけ上のリンク長を示す概略図である。図１４の衝突に対する第４実施形態の退避行動を示す概略図である。図２の衝突に対する第７実施形態の退避行動を示す概略図である。１リンクの弾性マニピュレータを示す概略図である。１リンクの弾性マニピュレータが障害物と衝突した状態を示す概略図である。図２０の衝突に対する第８実施形態の退避行動を示す概略図である。第９実施形態のベクトル方向Ｖｈを説明するための概略図である。３リンクのマニピュレータの途中のリンクが障害物に衝突した状態を示す概略図である。ランプ状成分に固有振動成分が重畳された外乱の波形を示すグラフである。外乱パワーを使用して算出した外乱の挙動を示すグラフである。

符号の説明

１，２，３リンク
１１コントローラ
１２軌道計画
１３マニピュレータ
１４エンコーダ
１５外乱算出器
１６評価量算出器
１７比較器
２１退避速度算出器
２２指令算出器
２５モータ
２６アーム
２７減速機
３１障害物
３２衝突点

Claims

多軸のモータ駆動装置の制御方法において、
上記モータ駆動装置が外界と衝突した衝突点での作業座標系における衝突時の速度ベクトルに対して逆向きである逆方向速度ベクトルを実現するために必要な、各軸の退避速度又は位置増分を算出し、
上記退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法。
衝突時の各軸の速度と符号が逆で大きさが同じである逆方向各軸速度又は位置増分を算出し、
上記逆方向各軸速度又は位置増分に全ての軸で同一の正の定数を乗算して、上記各軸の退避速度又は位置増分を算出することを特徴とする、請求項１に記載のモータ駆動装置の制御方法。
衝突時の各軸の速度と、上記衝突点の上記作業座標系へのヤコビ行列とから、上記衝突時の速度ベクトルを算出し、
上記衝突点の上記作業座標系へのヤコビ行列の逆行列情報と、上記衝突時の速度ベクトルとから上記各軸の退避速度又は位置増分を算出することを特徴とする、請求項１に記載のモータ駆動装置の制御方法。
多軸のモータ駆動装置の制御方法において、
上記モータ駆動装置と外界の衝突検知時の各軸の外乱情報を取得し、
上記衝突検知時の各軸の外乱情報に全ての軸で同一の正の定数を乗算して、衝突から退避するための各軸の退避速度又は位置増分を算出し、
上記退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法。
上記衝突検知時の各軸の外乱情報と、上記退避速度又は位置増分とのうちの少なくとも一方を、上記モータ駆動装置と外界との衝突点での作業座標系に対する各軸速度のヤコビ行列内で使用される各軸の長さ情報により補正することを特徴とする、請求項４に記載のモータ駆動装置の制御方法。
多軸のモータ駆動装置の制御方法において、
上記モータ駆動装置が外界と衝突した衝突点での作業座標系における外力ベクトル方向を求め、
上記外力ベクトル方向に沿って衝突点を移動させるために必要な各軸の退避速度又は位置増分を算出し、
上記退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法。
多軸のモータ駆動装置の制御方法において、
上記モータ駆動装置が外界と衝突した衝突点での作業座標系における弾性変位ベクトルに沿って衝突点を移動させるために必要な各軸の退避速度又は位置増分を算出し、
上記算出された退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法。
多軸のモータ駆動装置が外界と衝突した衝突点を移動させる制御方法において、
上記衝突検知時の作業座標系での速度ベクトルに対して逆向きである逆方向速度ベクトルと、上記衝突検知時の各軸の外乱情報に基づいて求めた上記衝突点での作業座標系での速度ベクトルと、上記衝突点での作業座標系における外力ベクトルと、上記衝突点での作業座標系における弾性変位ベクトルとのうち、いずれか２つ以上のベクトル方向によって内包されるベクトル方向に、上記衝突点を移動させるために必要な退避各軸速度又は位置増分を算出し、
上記退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法。
上記衝突点に代えて、各軸のリンク先端点と、最先端の軸に設けられた治具取付用のフランジ面とを含む衝突点として想定される点を使用することを特徴とする、請求項１から請求項８のいずれか１項に記載の、モータ駆動装置の制御方法。
衝突を検知するための評価値を算出し、該評価値と閾値の比較により外界との衝突を検知する、多軸のモータ駆動装置の制御方法において、
第１の評価値と第１の閾値との比較により、いずれかの軸と外界との衝突が検知されると、上記第１の評価値と上記第１の閾値よりも衝突検知の判断が成立しやすい第２の閾値との比較、上記第１の評価値よりも衝突の検知感度が高い第２の評価量と上記第１の閾値との比較、又は上記第２の評価値と上記第２の閾値との比較に基づいて、上記第１の評価値と第１の閾値との比較により外界との衝突が検知された軸以外に外界と衝突した可能性のある軸を推定し、
上記推定の結果に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法。
多軸のモータ駆動装置が外界と衝突した衝突点を移動させる制御方法において、
衝突時の各軸の速度、外乱情報、弾性変形角度、又は軸力を求め、
固有振動数を含む特定周波数成分を除去して上記衝突時の各軸の速度、外乱情報、弾性変形角度、又は軸力を補正し、
補正後の各軸の速度、外乱情報、弾性変形角度、又は軸力に基づいて、各軸の位置、速度、又はトルクを制御することを特徴とする、モータ駆動装置の制御方法。
衝突を検知し、１つ以上の手法により衝突後の退避方向を決定する多軸のモータ駆動装置の制御方法において、
上記１つ以上の手法によって決定された退避各軸速度又は位置増分の向きと一致しない向きに対しては、位置制御又は速度制御の比例ゲインを第１の値に設定し、積分ゲインを０相当に設定し、又はトルクの上下限値の範囲を第１の範囲に設定し、
上記１つ以上の手法によって決定された全ての退避各軸速度又は位置増分の向きと一致する向きに対しては、上記位置制御又は速度制御の比例ゲインを上記第１の値よりも大きい第２の値に設定し、上記積分ゲインを上記０相当よりも大きい値に設定し、又は上記トルクの上下限値の範囲を第１の範囲よりも大きい第２の範囲に設定する、
ことを特徴とする、モータ駆動装置の制御方法。
多軸のモータ駆動装置の制御装置において、
上記モータ駆動装置が外界と衝突した衝突点での作業座標系における衝突時の速度ベクトルに対して逆向きである逆方向速度ベクトルを実現するために必要な、各軸の退避速度を算出する退避速度算出手段と、
上記退避速度で算出された退避速度に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段と
を備えることを特徴とする、モータ駆動装置の制御装置。
多軸のモータ駆動装置の制御装置において、
上記モータ駆動装置の外界との衝突検知時の各軸の外乱情報を算出する外乱情報算出手段と、
上記外乱情報算出手段により算出された上記衝突検知時の各軸の外乱情報に全ての軸で同一の正の定数を乗算し、衝突から退避するための各軸の退避速度を算出する退避速度算出手段と、
上記退避速度算出手段によって算出された退避速度に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段と
を備えることを特徴とする、モータ駆動装置の制御装置。
多軸のモータ駆動装置の制御装置において、
上記モータ駆動装置が外界と衝突した衝突点での作業座標系における外力ベクトル方向を算出する外力ベクトル算出手段と、
上記外力ベクトル方向に沿って、衝突点を移動させるために必要な各軸の退避速度を算出する退避速度算出手段と、
上記退避速度算出手段によって算出された退避速度に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段と
を備えることを特徴とする、モータ駆動装置の制御装置。
多軸のモータ駆動装置の制御装置において、
上記モータ駆動装置が外界と衝突した衝突点での作業座標系における弾性変位ベクトルに沿って、衝突点を移動させるために必要な各軸の退避速度又は位置増分を算出する退避速度又は位置増分算出手段と、
上記退避速度算出手段又は位置増分算出手段によって算出された退避速度又は位置増分に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段と
を備えることを特徴とする、モータ駆動装置の制御装置。
多軸のモータ駆動装置の制御装置において、
上記モータ駆動装置の外界との衝突を検知する手段と、
上記衝突検知時の作業座標系での速度ベクトルに対して逆向きである逆方向速度ベクトルと、上記衝突検知時の各軸の外乱情報に基づいて求めた衝突点での作業座標系における速度ベクトルと、上記衝突点での作業座標系における外力ベクトルと、上記衝突点での作業座標系における弾性変位ベクトルとのうち、いずれか２つ以上のベクトル方向によって内包されるベクトル方向に、上記衝突点を移動させるために必要な各軸の退避速度を算出する、退避速度算出手段と、
上記退避速度算出手段によって算出された退避速度に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段と
を備えることを特徴とする、モータ駆動装置の制御装置。
衝突を検知するための評価値を算出する手段と、該評価値と閾値の比較により外界との衝突を検知する検知する検知手段とを備える、多軸のモータ駆動装置の制御装置において、
上記検知手段による第１の評価値と第１の閾値との比較により、いずれかの軸と外界との衝突が検知されると、上記第１の評価値と上記第１の閾値よりも衝突検知の判断が成立しやすい第２の閾値との比較、上記第１の評価値よりも衝突の検知感度が高い第２の評価量と上記第１の閾値との比較、又は上記第２の評価値と上記第２の閾値との比較に基づいて、上記第１の評価値と第１の閾値との比較により外界との衝突が検知された軸以外に外
界と衝突した可能性のある軸を推定する、推定手段と、
上記推定手段による推定結果に基づいて、各軸の位置、速度、又はトルクの指令を算出する指令算出手段と
を備えることを特徴とする、モータ駆動装置の制御装置。
多軸のモータ駆動装置の制御装置において、
衝突検知時の各軸の速度、外乱情報、弾性変形角度、又は軸力を求める手段と、
固有振動数を含む特定周波数成分を除去して上記衝突時の各軸の速度、外乱情報、弾性変形角度、又は軸力を補正する補正手段と、
補正手段により補正された各軸の速度、外乱情報、弾性変形角度、又は軸力に基づいて、各軸の位置、速度、又はトルクを算出する指令算出手段と
を備えることを特徴とする、モータ駆動装置の制御装置。