(57)【要約】【課題】  随時状況に応じて閾値を必要最低限の値に変更することにより、誤検知の回避と早期の衝突検知を両立する。【解決手段】  衝突検知装置は、外乱ｄと関連する衝突評価量を算出する評価量算出器２２、衝突を検知するために衝突秤量Ｃｉと閾値ｔｈｉを比較する比較器２３、及び運動方程式モデルに関する位置情報、速度情報、及び加速度情報のうち少なくとも一つと、運動方程式モデルのパラメータ誤差範囲とに基づいて、閾値ｔｈｉを算出する閾値算出器２４を備える。【選択図】図１
2005102427.tif 000002
【特許請求の範囲】
【請求項１】
モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体と外界の衝突を検知する、モータ駆動装置の衝突検知方法において、
上記モータ駆動装置の運動方程式モデルのパラメータ誤差範囲を設定し、
上記運動方程式モデルに関する位置情報、速度情報、及び加速度情報のうち少なくとも一つと、上記パラメータ誤差範囲とに基づいて、上記閾値を変更することを特徴とする、モータ駆動装置の衝突検知方法。
【請求項２】
上記位置情報、速度情報、及び加速度情報のうち少なくとも一つと、上記パラメータ誤差範囲とに基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、
上記駆動力誤差範囲に基づいて、上記閾値を変更することを特徴とする、請求項１に記載の衝突検知方法。
【請求項３】
上記運動方程式モデルを、上記位置情報、速度情報、及び加速度情報のうちの少なくとも一つによって算出可能な情報量行列と、上記運動方程式モデルのパラメータを要素にもつパラメータベクトルとの積によって記述し、
上記パラメータ誤差範囲を要素にもつパラメータ誤差範囲ベクトルと、上記情報量行列の各要素の絶対値を要素にもつ情報量絶対値行列との積から、上記駆動力誤差範囲を要素にもつ駆動力誤差範囲ベクトルを算出し、
上記駆動力誤差範囲ベクトルの各要素を用いて、上記閾値を変更することを特徴とする、請求項２に記載の衝突検知方法。
【請求項４】
モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体と外界の衝突を検知する、モータ駆動装置の衝突検知方法において、
モータを駆動するために必要な必要駆動力又は外乱を算出又は推定するために用いられる、上記モータ駆動装置の運動方程式モデルに関する位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して情報誤差範囲を設定又は算出し、
上記情報誤差範囲に基づいて、上記閾値を変更することを特徴とする、モータ駆動装置の衝突検知方法。
【請求項５】
上記情報誤差範囲に基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、
上記駆動力誤差範囲に基づいて、上記閾値を変更することを特徴とする、請求項４に記載の衝突検知方法。
【請求項６】
上記位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して、実システムにおける真の値と選択された情報との時間ずれの範囲を設定し、
上記時間ずれの範囲から上記情報誤差範囲を算出することを特徴とする、請求項４又は請求項５に記載の衝突検知方法。
【請求項７】
モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体の衝突を検知する、モータ駆動装置の衝突検知方法において、
上記モータ駆動装置に対する駆動指令値が、実現困難な指令値である場合、衝突を検知しないことを特徴とする、モータ駆動装置の衝突検知方法。
【請求項８】
モータによって駆動される被駆動体の衝突を検知するモータ駆動装置の衝突検知装置において、
上記モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出する衝突評価量算出手段と、
上記被駆動体の衝突を検知するために、上記衝突評価量と閾値とを比較する比較手段と、
上記モータ駆動装置の運動方程式モデルのパラメータ誤差範囲を設定し、上記運動方程式モデルに関する位置情報、速度情報、及び加速度情報のうち少なくとも一つと、上記パラメータ誤差範囲とに基づいて、上記閾値を算出する閾値算出手段と
を備えることを特徴とする、モータ駆動装置の衝突検知装置。
【請求項９】
モータによって駆動される被駆動体の衝突を検知するモータ駆動装置の衝突検知装置において、
上記モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出する衝突評価量算出手段と、
上記被駆動体の衝突を検知するために、上記衝突評価量と閾値とを比較する比較手段と、
モータを駆動するために必要な必要駆動力又は外乱を算出又は推定するために用いられる、上記モータ駆動装置の運動方程式モデルに関する位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して情報誤差範囲を設定又は算出し、上記情報誤差範囲に基づいて、上記閾値を算出する閾値算出手段と
を備えることを特徴とする、モータ駆動装置の衝突検知装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、溶接ロボットを含む各種ロボットのようなモータが被駆動体を駆動するモータ駆動装置において、被駆動体と外界の衝突を検知するための衝突検知方法及び装置に関する。
【背景技術】
【０００２】
従来、モータ駆動装置の被駆動体の外界に対する衝突を検知するための種々の方法が提案されている。これらの方法では、モータ駆動装置が正常に動作しているときに必要とされる必要駆動トルクと実トルク（あるいは指令トルク）との差を外乱とし、その推定値、微分値、絶対値、予測値など外乱を用いて算出された評価値が閾値を超えたことにより衝突を検知している。
【０００３】
例えば、特許文献１には、外乱推定オブザーバによる外乱推定値が閾値を超えたことにより衝突を検知することが記載されている。また、特許文献２には、外乱推定値に速度を乗じた値が閾値を超えたことにより衝突を検知することが記載されている。特許文献３には、外乱推定値が閾値を所定時間超えたことにより衝突を検知することが記載されている。特許文献４には、外乱推定値の微分値が閾値を超えたことにより衝突を検知することが記載されている。特許文献５及び特許文献６には、外乱推定値とその微分値のいずれかが閾値を超えたことにより衝突を検知することが記載されている。特許文献７には、外乱と閾値の差の符号により衝突を検知することが記載されている。特許文献８には、弾性変形モデルを用いて算出した外乱推定値が閾値を超えたことにより衝突を検知することが記載されている。特許文献９には、外乱の絶対値が閾値を超えたことにより衝突を検知することが記載されている。特許文献１０には、必要駆動トルクを基準に閾値を設定し、実トルクがその閾値を超えたことにより衝突を検知することが記載されている。この特許文献１０に記載の方法は、直接外乱を算出していないが、必要駆動トルクと実トルクを比較しているので、外乱の大きさを評価していることと等価である。さらに、特許文献１１には、衝突の検知ではないが、外乱予測値が閾値を超えたことにより障害物を検知することが記載されている。
【０００４】
ここで外乱について説明する。ロボットの運動方程式モデルは一般に以下の式で表される。
【０００５】
【数１】
2005102427.tif 000003
【０００６】
ここで、Θはロボットの関節角度ベクトル（モータ回転角度ベクトルと１対１に比例）、Ｊ(Θ)はロボットの慣性行列、Ｃ(dΘ/dt,Θ)は重力、遠心コリオリ力、摩擦などを含むベクトル、Τはトルクベクトル、Ｄが外乱ベクトルである。式（１）から外乱ベクトルＤは以下の式（２）で算出することができる。
【０００７】
【数２】
2005102427.tif 000004
【０００８】
モータ制御の分野では一般的に単軸毎に外乱が導出されることが多い。今、ロボットのある軸に注目して式（１），（２）式を書き下せば、以下の式（３），（４）が得られる。
【０００９】
【数３】
2005102427.tif 000005
【００１０】
【数４】
2005102427.tif 000006
【００１１】
ここでθとτとｄはある軸の関節角度とトルクと外乱、ｊ(Θ)はある軸の慣性、ｃ(d２Θother/dt２,dΘ/dt,Θ)はある軸に対応するベクトルＣの要素と他の軸から受ける慣性力（ただしΘotherはθを除いた他の軸の関節角度を要素とするベクトル）である。
【００１２】
ｊ(Θ)やｃ(d２Θother/dt２,dΘ/dt,Θ)が既知であれば、式（４）により関節加速度d２θ/dt２とトルクτとから外乱ｄを導出することができる。ｊ(Θ)やｃ(d２Θother/dt２,dΘ/dt,Θ)の厳密な値は既知でないが、同定などで得られたモデルに基づいてかなり正確に導出することが可能であるので、実際にはモデルから得られたｊ(Θ)やｃ(d２Θother/dt２,dΘ/dt,Θ)を用いて、式（４）から外乱ｄが算出される。
【００１３】
また、モータ駆動装置を駆動させるために必要な必要駆動トルクは、式（１）あるいは式（３）の運動方程式モデルから推定可能で、式（１）左辺あるいは式（３）左辺の和として計算できる。すなわち、必要駆動トルクベクトルΤ＾とある軸に関する必要駆動トルクτ＾はそれぞれ以下の式（５），（６）で計算できる。
【００１４】
【数５】
2005102427.tif 000007
【００１５】
【数６】
2005102427.tif 000008
【００１６】
式（２），（４）〜（６）から明らかなように、一般に外乱は必要駆動トルクと実トルクの差として記述され、以下の式（２）’，（４）’で与えられる。
【００１７】
【数７】
2005102427.tif 000009
【００１８】
【数８】
2005102427.tif 000010
【００１９】
式（１）〜（６）のΘあるいはθ、dΘ/dtあるいはdθ/dt、d２Θ/dt２あるいはd２θ/dt２、及びΤあるいはτは、指令値、目標値、実測値、推定値、及び計算値のいずれを用いてもよい。
【００２０】
図９は、従来の衝突検知装置を一般化して示す。コントローラ１は軌道計画２を実現するための指令値としてトルクτをモータ駆動装置であるロボット３に出力し、それに従ってロボット３のモータ（図示せず）が駆動されて被駆動体であるアームやリスト（図示せず）が移動する。エンコーダ４により検出された各軸の関節角度θはコントローラ１にフィードバックされる。
【００２１】
式（４）に基づいて外乱算出器５が外乱ｄを算出し、この外乱ｄから評価量算出器６が衝突評価量を算出する。算出された衝突評価量は比較器７により閾値算出器８の算出した閾値と比較される。比較器７の比較結果から衝突が検知されると、コントローラ１が装置停止等の衝突時の処理を実行する。
【００２２】
上記特許文献１に記載のものは、図９の評価量算出器６が外乱ｄにフィルタ処理を実行するもの相当し、比較器７は外乱推定値ｄ＾が閾値を超えたことによって衝突を検知する。また、上記特許文献２に記載のものは、評価量算出器６がフィルタ処理後の外乱ｄにさらにモータ速度を乗算するものに相当する。さらに、上記特許文献４に記載のものは評価量算出器６が外乱ｄを微分操作するものに、上記特許文献９に記載のものは評価量算出器６が外乱ｄの絶対値を算出するものに、それぞれ相当する。上記特許文献９に記載のものでは、外乱ｄと閾値を比較する代わりに、ｄ＋τ＾（＝τ）と（閾値＋τ＾）を比較しているが、τ＾をそれぞれから引けば外乱ｄと閾値を比較しているのと実質は同じである。
【００２３】
閾値算出器８により設定される閾値を小さくすることにより、早期に衝突を検知することができる。しかし、閾値を小さくすると、誤検知の可能性が高まるので、状況に応じた閾値の変更が必要である。閾値の変更に関しては、上記特許文献６に、低速時に閾値を下げることが記載されている。また、上記特許文献１０に、ロボットの手先負荷、必要駆動トルクを算出するモデルのパラメータ推定の到達レベル、必要駆動トルクとその差分の線形和、加速度指令と加加速度指令の線形和、位置指令と速度指令と加速度指令と実位置と実速度と実加速度の線形和、停止中かどうか、ロボットの作業などによって閾値を変更することが記載されている。また、特許文献１２には、必要駆動トルクに応じて閾値を変更することが記載されている。さらに、特許文献１３には低温時に閾値を変更することが記載されている。さらにまた、特許文献１４には、溶接ロボットの溶接区間において閾値を変更することが記載されている。
【特許文献１】特開平３−１９６３１３号公報
【特許文献２】特開平９−３０５２３５号公報
【特許文献３】特開平９−３０８２８３号公報
【特許文献４】特開平１１−２８２５４０号公報
【特許文献５】特開平１１−７０４９０号公報
【特許文献６】特開２００１−３５３６８７号公報
【特許文献７】特開平１１−７７５８０号公報
【特許文献８】特開２０００−５２２８６号公報
【特許文献９】特開２００２−２８３２７６号公報
【特許文献１０】特開２００３−２５２７２号公報
【特許文献１１】特開平６−２８９９３８号公報
【特許文献１２】特開２００１−１１７６１８号公報
【特許文献１３】特開平１１−１５５１１号公報
【特許文献１４】特開平１１−２９１１９０号公報
【発明の開示】
【発明が解決しようとする課題】
【００２４】
しかし、上記閾値を変更する従来技術は、誤検知が生じない限界まで閾値を下げているとはいえない。
【００２５】
閾値を下げられない理由としては、運動方程式モデルから算出される必要駆動トルクや駆動力情報の誤差が挙げられる。仮にモデルが実システムと完全に一致し、必要駆動トルクを算出するためにモデルに入力される位置情報、速度情報、及び加速度情報が真値と完全に一致していれば、算出された必要駆動トルクは実際のシステムに作用している真のトルクと完全に一致し、式（２）’，（４）’で算出される外乱は衝突しない限り常に０となり、閾値を０とすることができる。しかし、このような理想的な状況は現実にはありえず、算出された必要駆動トルクや駆動力情報は以下の要因による誤差を含み、必要駆動トルクや駆動力情報から算出される外乱にも誤差が存在する。
【００２６】
１）運動方程式モデルのパラメータ誤差（誤差要因１）
２）必要駆動トルク算出に用いられる位置や速度や加速度情報の誤差（誤差要因２）
３）駆動力情報τあるいはΤの誤差（誤差要因３）
４）運動方程式モデルで考慮されていない構造的なモデル化誤差（誤差要因４）
【００２７】
そこで、本発明は、必要駆動トルクと駆動力情報の誤差を誤差要因ごとに系統立てて扱い、随時状況に応じて閾値を必要最低限の値に変更することにより、誤検知の回避と早期の衝突検知を両立することを課題とする。
【課題を解決するための手段】
【００２８】
本明細書において、位置情報は関節角度ベクトルと個々の軸の関節角度を含む。また、速度情報は関節角度ベクトルと個々の角度の１階時間微分、すなわち関節速度ベクトルと個々の軸の関節速度を含む。さらに、加速度情報は関節角度の２階時間微分、すなわち関節加速度ベクトルと個々の軸の関節加速度を含む。さらにまた、駆動力情報はトルクベクトル、個々の軸のトルク、外乱ベクトル、及び個々の軸の外乱を含む。位置情報、速度情報、加速度情報、及び駆動力情報は、指令値、目標値、実測値、推定値、及び計算値のいずれであってもよい。
【００２９】
第１の発明は、モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体と外界の衝突を検知する、モータ駆動装置の衝突検知方法において、上記モータ駆動装置の運動方程式モデルのパラメータ誤差範囲を設定し、上記運動方程式モデルに関する位置情報、速度情報、及び加速度情報のうち少なくとも一つと、上記パラメータ誤差範囲とに基づいて、上記閾値を変更することを特徴とする、モータ駆動装置の衝突検知方法を提供する。
【００３０】
具体的には、上記位置情報、速度情報、及び加速度情報のうち少なくとも一つと、上記パラメータ誤差範囲とに基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、上記駆動力誤差範囲に基づいて、上記閾値を変更する。
【００３１】
さらに具体的には、上記運動方程式モデルを、上記位置情報、速度情報、及び加速度情報のうちの少なくとも一つによって算出可能な情報量行列と、上記運動方程式モデルのパラメータを要素にもつパラメータベクトルとの積によって記述し、上記パラメータ誤差範囲を要素にもつパラメータ誤差範囲ベクトルと、上記情報量行列の各要素の絶対値を要素にもつ情報量絶対値行列との積から、上記駆動力誤差範囲を要素にもつ駆動力誤差範囲ベクトルを算出し、上記駆動力誤差範囲ベクトルの各要素を用いて、上記閾値を変更する。
【００３２】
第２の発明は、モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体と外界の衝突を検知する、モータ駆動装置の衝突検知方法において、モータを駆動するために必要な必要駆動力又は外乱を算出又は推定するために用いられる、上記モータ駆動装置の運動方程式モデルに関する位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して情報誤差範囲を設定又は算出し、上記情報誤差範囲に基づいて、上記閾値を変更することを特徴とする、モータ駆動装置の衝突検知方法を提供する。
【００３３】
具体的には、上記情報誤差範囲に基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、上記駆動力誤差範囲に基づいて、上記閾値を変更する。
【００３４】
あるいは、上記位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して、実システムにおける真の値と選択された情報との時間ずれの範囲を設定し、上記時間ずれの範囲から上記情報誤差範囲を算出する。
【００３５】
第３の発明は、モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体の衝突を検知する、モータ駆動装置の衝突検知方法において、上記モータ駆動装置に対する駆動指令値が、実現困難な指令値である場合、衝突を検知しないことを特徴とする、モータ駆動装置の衝突検知方法を提供する。
【００３６】
第４の発明は、モータによって駆動される被駆動体の衝突を検知するモータ駆動装置の衝突検知装置において、上記モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出する衝突評価量算出手段と、上記被駆動体の衝突を検知するために、上記衝突評価量と閾値とを比較する比較手段と、上記モータ駆動装置の運動方程式モデルのパラメータ誤差範囲を設定し、上記運動方程式モデルに関する位置情報、速度情報、及び加速度情報のうち少なくとも一つと、上記パラメータ誤差範囲とに基づいて、上記閾値を算出する閾値算出手段とを備えることを特徴とする、モータ駆動装置の衝突検知装置を提供する。
【００３７】
第５の発明は、モータによって駆動される被駆動体の衝突を検知するモータ駆動装置の衝突検知装置において、上記モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出する衝突評価量算出手段と、上記被駆動体の衝突を検知するために、上記衝突評価量と閾値とを比較する比較手段と、モータを駆動するために必要な必要駆動力又は外乱を算出又は推定するために用いられる、上記モータ駆動装置の運動方程式モデルに関する位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して情報誤差範囲を設定又は算出し、上記情報誤差範囲に基づいて、上記閾値を算出する閾値算出手段とを備えることを特徴とする、モータ駆動装置の衝突検知装置を提供する。
【発明の効果】
【００３８】
本発明では、モータ駆動装置の運動方程式のパラメータ誤差範囲、モータを駆動するために必要な必要駆動力、又は外乱を算出又は推定するために用いられる運動方程式モデルに関する位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに基づいて、衝突評価量の閾値を変更することで、随時状況に応じて閾値を必要最低限の値とし、誤検知の回避と早期の衝突検知を両立することができる。
【発明を実施するための最良の形態】
【００３９】
次に、添付図面を参照して本発明の実施の形態を詳細に説明する。各図において、同一の要素には同一の符号を付している。
【００４０】
（第１実施形態）
第１実施形態は、運動方程式モデルのパラメータ誤差（誤差要因１）に応じて閾値を変更するものである。
【００４１】
必要駆動トルクＴ＾を算出する式（５）は、パラメータを要素とするパラメータベクトルPと、加速度情報である関節加速度ベクトルd２Θ/dt２、速度情報である関節速度ベクトルdΘ/dt、及び位置情報である関節角度Θなどの非線形関数として定義される情報量行列Ｘ(d２Θ/dt２,dΘ/dt,Θ)との積を用いて、以下の式（７）のように変形できる。
【００４２】
【数９】
2005102427.tif 000011
【００４３】
例えば、図２に示す２つの関節１１，１２とアーム１３，１４を備える単純の２リンクの平行駆動型マニピュレータの運動方程式モデルについて、パラメータベクトルＰの第１要素は第１軸の慣性パラメータ、第２要素は第２軸の慣性パラメータ、第３要素は第１軸と第２軸の干渉に関する慣性パラメータ、第４要素は第１軸の重力パラメータ、第５要素は第２軸の重力パラメータ、第６要素は第１軸の粘性摩擦パラメータ、第７要素は第１軸の固体摩擦パラメータ、第８要素は第２軸の粘性摩擦パラメータ、第９要素は第２軸の固体摩擦パラメータとすれば、情報量行列Ｘ(d２Θ/dt２,dΘ/dt,Θ)の各要素は以下の式（８）のようになる。ただし情報量行列Ｘ(d２Θ/dt２,dΘ/dt,Θ)のｉ行ｎ列要素をｘｉｎ(d２Θ/dt２,dΘ/dt,Θ)とする。
【００４４】
【数１０】
2005102427.tif 000012
【００４５】
また、図３に示すようにモータ１５とアーム１６を連結する減速機１７が弾性変形する１リンクの弾性軸マニピュレータについて、Ｐの第１要素はアームの慣性パラメータｊｌ、第２要素はアームの重力パラメータｇｌ、第３要素はアームの粘性摩擦パラメータｆｌ、第４要素はアームの固体摩擦パラメータｃｌ、第５要素はモータの慣性パラメータｊｍ、第６要素はモータの粘性摩擦パラメータｆｍ、第７要素はモータの固体摩擦パラメータｃｍとすれば、情報量行列Ｘ(d２Θ/dt２,dΘ/dt,Θ)の各要素は以下の式（９）で与えられる。
【００４６】
【数１１】
2005102427.tif 000013
【００４７】
一般の産業用ロボットでは、モータの回転角θｍは計測可能であるが、アームの回転角θｌは計測されない。しかし軸力τａについて、以下の式（１０）の関係が成り立つ。
【００４８】
【数１２】
2005102427.tif 000014
【００４９】
また、減速機のバネ定数ｋについて、以下の式（１１）の関係が成り立つ。
【００５０】
【数１３】
2005102427.tif 000015
【００５１】
これらの式（１０），（１１）から、モータの関節角度θｍが既知ならば関節角度θ１を算出あるいは推定可能である。逆に、関節角度θ１が与えられば、式（１０），（１１）からモータの回転角度θｍを算出あるいは推定可能である。
【００５２】
例えば、関節角度θｌとして目標値を与えた場合、式（１０）により軸力τａを算出し、算出された軸力τａと関節角度θｌとから式（１１）により関節角度θｌの目標値を実現するために必要なモータの回転角度θｍが算出できる。また、モータの回転角度θｍとして計測値を与えた場合、式（１０），（１１）に基づいて関節角度θｌをシミュレートすることにより、関節角度θｌを算出できる。式（１０），（１１）に基づいてオブザーバを構成すれば関節角度θｌを推定することもできる。
【００５３】
ここでは１リンクの弾性軸マニピュレータについて記述したが、多リンクについても同様のことが言える。
【００５４】
上記式（７）は、パラメータベクトルＰに対して線形であるため、パラメータ誤差ΔＰと、それに起因する必要駆動トルク誤差ΔΤ＾の間には以下の式（１２）の関係が成り立つ。
【００５５】
【数１４】
2005102427.tif 000016
【００５６】
さらに、必要駆動トルク誤差ΔΤ＾の第ｉ要素をΔτｉ＾、パラメータ誤差ΔＰの第ｎ要素をΔｐｎとすれば、式（７）から以下の式（１３），（１４）の関係が成り立つ。ただし、｜・｜は絶対値、Σｎはｎに対するサメンションとする。
【００５７】
【数１５】
2005102427.tif 000017
【００５８】
【数１６】
2005102427.tif 000018
【００５９】
ここでパラメータ誤差ΔＰの要素Δｐｎは未知であるが、その絶対値｜Δｐｎ｜の上限値Δｐｕｎは見積もることが可能である。この上限値Δｐｕｎを用いれば、式（１４）式から以下の式（１５）が得られる。
【００６０】
【数１７】
2005102427.tif 000019
【００６１】
式（１５）のΔτｕｉは当然算出可能で、必要駆動トルク誤差ΔＴ＾の要素の｜Δτｉ＾｜の上限値にもなっている。また、この上限値Δτｕｉを第ｉ要素とする必要駆動トルク誤差範囲ベクトルΔΤｕは、｜ｘｉｎ(d２Θ/dt２,dΘ/dt,Θ)｜をｉ行ｎ列要素とする情報量絶対値行列Ｘａ(d２Θ/dt２,dΘ/dt,Θ)と、上限値Δｐｕｎを第ｎ要素とするパラメータ誤差範囲ベクトルΔＰｕの積として、以下の式（１６）で記述される。
【００６２】
【数１８】
2005102427.tif 000020
【００６３】
式（１６）より、必要駆動トルク誤差範囲ベクトルΔΤｕの各要素Δτｕｉを得ることもできる。
【００６４】
式（１５）又は式（１６）より得られた必要駆動トルク誤差範囲ベクトルΔΤｕの各要素Δτｕｉを定数倍し、個々の軸に対する外乱そのもの又は外乱を使用した演算により得られる衝突評価量の閾値ｔｈｉを以下の式（１７）で算出することができる。
【００６５】
【数１９】
2005102427.tif 000021
【００６６】
この閾値ｔｈｉを使用することで、パラメータ誤差によって生じる必要駆動トルクの誤差によって誤検知することなく、衝突を検知することができる。
【００６７】
図１は、本実施形態の衝突検知装置を示す。コントローラ１は軌道計画２を実現するための指令値としてトルクτをモータ駆動装置であるロボット３に出力し、それに従ってロボット３のモータ（図示せず）が駆動されて被駆動体であるアームやリスト（図示せず）が移動する。エンコーダ４により検出された各軸の関節角度θはコントローラ１にフィードバックされる。衝突検知装置は、外乱算出器２１、評価量算出器２２、比較２３、閾値算出器２４、及びパラメータ誤差上限設定器２５を備える。
【００６８】
外乱算出器２１はコントローラ１から入力されるトルクτ、ロボット３のエンコーダ４から入力される関節角度θ、予め導出されたｊ(Θ)、及び予め導出されたｃ(d２Θother/dt２,dΘ/dt,Θ)とにより、上記式（４）から各軸の外乱ｄｉを時々刻々と算出する。外乱算出器２１で算出された外乱ｄｉは評価量算出器２２に出力される。
【００６９】
評価量算出器２２は、外乱算出器２１より入力される外乱ｄｉから衝突検知の判定に使用する衝突評価量Ｃｉを算出する。衝突評価量Ｃｉは、例えば外乱ｄｉ自体、又は外乱ｄｉの絶対値｜ｄｉ｜であってもよく、以下の式（１８）や式（１９）により算出してもよい。なお、ｄｉ’は個々の軸の外乱変化速度である。
【００７０】
【数２０】
2005102427.tif 000022
【００７１】
【数２１】
2005102427.tif 000023
【００７２】
評価量算出器２２の算出する衝突評価量Ｃｉは比較器２３に入力される。比較器２３の比較結果より衝突評価量Ｃｉが閾値ｔｈｉ以上であれば、コントローラ１は衝突であると判定し、ロボット３の動作停止等の必要な制御を実行する。
【００７３】
閾値算出器２４には、エンコーダ４から回転角度θが入力され、式（１５），（１６）により必要駆動トルク誤差範囲ベクトルΔΤｕの各要素Δτｕｉを算出し、このΔτｕｉから式（１７）により閾値ｔｈｉを時々刻々と算出する。式（１５），（１６）によるΔτｕｉにより算出に使用されるパラメータ誤差範囲ベクトルΔＰの各要素（パラメータ誤差の上限値）ΔＰｕｎはパラメータ誤差上限設定器２５に予め記憶されており、閾値算出器２４に出力される。
【００７４】
特許文献１２に記載の衝突検知では各軸の必要駆動トルクτｉ＾に応じて閾値ｔｈｉを変更しているが、必要駆動トルクτｉ＾が０の時にもパラメータ誤差による必要駆動トルクτｉ＾の誤差は存在する。例えば、以下の式（２０）で与えられる必要駆動トルクτ１＾のパラメータ誤差による誤差Δτ１は、以下の式（２１）で与えられる。Δ＊は各パラメータの誤差である。
【００７５】
【数２２】
2005102427.tif 000024
【００７６】
【数２３】
2005102427.tif 000025
【００７７】
式（２０）のτ１＾が０となっても、Δτ１が０になるとは限らない。特に多リンクのマニピュレータにおいては、他の軸からの干渉力と自身の軸の慣性力などが互いに相殺しあい、必要駆動トルクτｉ＾が０になることは常に発生する。また必要駆動トルクτｉ＾が０の状態が長く続く（τｉ＾’も０となる）場合も往々にして発生する。このような場合、特許文献１２による閾値変更量は０となり、パラメータ誤差に起因する必要駆動トルクの誤差Δτｉには対応できず、閾値のオフセット分を大きくし、対処するしかない。
【００７８】
これに対して本実施形態では、パラメータ誤差による影響をモデルに基づいて適正に判断することが可能で、閾値のオフセット成分を大きくして衝突検知が遅れるような状態を回避することができる。
【００７９】
（第２実施形態）
第１実施形態では、閾値算出器２４は式（１５）により、全ｎ個の要素に対してサメンションして上限値Δτｕｉを求めている。しかし、以下の式（２２）に示すように、特定のｎ個の要素（ｎの全体集合に対する部分集合Ｎの要素、ただしＮは１つ以上の要素をもつ）に対してのみサメンションを行い、それ以外の要素についてはオフセット分で対処することも可能である。
【００８０】
【数２４】
2005102427.tif 000026
【００８１】
ただし通常運転時にとりうる任意の関節加速度d２Θ/dt２、関節速度dΘ/dt、及び関節角度Θに対して、オフセット分≧Σｎ≠Ｎ｛ｘｉｎ(d２Θ/dt２,dΘ/dt,Θ)×Δｐｎ｝を満たし、Σｎ∈Ｎは集合Ｎに属する全てのｎに対するサメンションで、Σｎ≠Ｎは集合Ｎの補集合に属する全てのｎに対するサメンションとする。
【００８２】
式（２２）で与えられた上限値Δτｕｉは式（１５）のそれと比べて大きな値になるが、パラメータ誤差を陽に扱わない従来技術と比較すると、集合Ｎに属するパラメータ誤差を陽に扱っているので、上限値Δτｕｉのオフセット分が明らかに小さい。すなわち式（２２）によって得られた上限値Δτｕｉによって閾値ｔｈｉを変更すれば、従来よりも閾値を小さく設定することができる。
【００８３】
（第３実施形態）
第３実施形態は、情報誤差、すなわち位置情報、速度情報、及び加速度情報の誤差（誤差要因２）に対応して閾値を変更するものである。
【００８４】
位置情報である関節角度Θ、速度情報である関節速度dΘ/dt、及び加速度情報である関節加速度d２Θ/dt２にそれぞれ誤差ΔΘ、ΔdΘ/dt、Δd２Θ/dt２が存在し、情報量行列Ｘ(d２Θ/dt２,dΘ/dt,Θ)がΘ、dΘ/dt、及びd２Θ/dt２について偏微分可能である場合、各誤差ΔΘ、ΔdΘ/dt、Δd２Θ/dt２による必要駆動トルク誤差ΔΤ＾は式（７）から以下の式（２３）で与えられる。
【００８５】
【数２５】
2005102427.tif 000027
【００８６】
また、必要駆動トルク誤差ΔＴ＾の第ｉ要素をΔτｉ＾は次式で与えられる。
【００８７】
【数２６】
2005102427.tif 000028
【００８８】
これらの式（２３），（２４）において、右辺第１項は位置情報誤差ΔΘによるトルク誤差成分、右辺第２項は速度情報誤差ΔdΘ/dtによるトルク誤差成分、右辺第３項は加速度情報誤差Δd２Θ/dt２によるトルク誤差成分である。情報誤差ΔΘ，ΔdΘ/dt，Δd２Θ/dt２は未知であり、式（２３），（２４）から必要駆動トルク誤差ΔＴやその要素Δτｉ＾を直接算出することはできない。しかし、情報誤差ΔΘ，ΔdΘ/dt，Δd２Θ/dt２の範囲は特定することが可能である。各情報誤差Δθｑ，Δdθｑ/dt，Δd２θｑ/dt２の絶対値の上限値をそれぞれΔθｕｑ，Δdθｕｑ/dt，Δd２θｕｑ/dt２とすれば、式（２４）から以下の式（２５）の関係が得られる。
【００８９】
【数２７】
2005102427.tif 000029
【００９０】
この式（２５）で与えられる算出可能な上限値Δτｕｉから上記式（１７）により衝突評価量Ｃｉの閾値ｔｈｉを算出すれば、情報量誤差によって生じる必要駆動トルクの誤差によって誤検知することなく、衝突を検知することができる。
【００９１】
図４に示す本実施形態の衝突検知装置は、パラメータ誤差上限設定器２５（図１参照）に代えて、式（２５）により上限値Δτｕｉを算出する情報誤差上限算出器２７を備えている。閾値算出器２４は情報誤差上限算出器２７の算出した上限値Δτｕｉから式（１７）により衝突評価量Ｃｉの閾値ｔｈｉを算出する。比較器２３により衝突評価量Ｃｉと閾値ｔｈｉを比較した結果、衝突評価量Ｃｉが閾値ｔｈｉ以上であれば、コントローラ１は衝突であると判定し、ロボット３の動作停止等の必要な制御を実行する。本実施形態の衝突検知装置のその他の構成及び作用は図１に示す第１実施形態のものと同様である。
【００９２】
例えば、２リンクの平行駆動型マニピュレータに関する式（８）のｘｉｎについて、dθ１/dt−Δdθｕ１/dt＞０あるいはdθ１/dt＋Δdθｕ１/dt＜０を満たすものとすれば、式（２４）のΔτ１と式（２５）のΔτｕ１は、それぞれ以下の式（２６），（２７）で表される。
【００９３】
【数２８】
2005102427.tif 000030
【００９４】
【数２９】
2005102427.tif 000031
【００９５】
式（２７）で得られたΔτｕ１によって第１軸の閾値ｔｈ１を変更すれば、各情報誤差Δθｑ，Δdθｑ/dt，Δd２θｑ/dt２によって生じるトルク誤差によって誤検知することなく、衝突を検知することができる。
【００９６】
次に、第３実施形態の変形例を説明する。式（２７）ではΔτ１に関係する全ての情報誤差の影響を加算してΔτｕ１を導出しているが、情報誤差の全てではなく、いくつかの情報誤差の影響のみを加算し、他の情報誤差の影響はオフセット分として扱ってもよい。
【００９７】
例えば、第２軸の制御性が悪く、加速度情報誤差Δd２θ２/dt２が大きく変動する場合、その影響のみを取り出し、以下の式（２８）でΔτｕ１を与えても良い。ただしオフセット分は通常運転時に他の情報誤差がΔτ１に与える影響よりも大きな値とする。
【００９８】
【数３０】
2005102427.tif 000032
【００９９】
情報誤差の影響をモデルに基づいて陽に扱わっていない従来の方法では、加速度情報誤差Δd２θ２/dt２によるトルク誤差を正確に見積ることができず、オフセット誤差としてより大きな値を設定するしかない。従って、式（２８）でΔτｕ１を与えたとしても、従来の方法よりも閾値の範囲を小さく設定することが可能である。例えば、式（２８）から明らかなように、θ２＝θ１の場合、Δd２θ２/dt２が存在しても、それによるトルク誤差は０であり、式（２８）から算出されるΔτｕ１も小さくなるが、加速度情報誤差Δd２θ２/dt２によるトルク誤差をオフセット分とする従来の方法では閾値を小さくすることはできない。
【０１００】
式（２８）は加速度情報誤差のみを扱っているが、一般に位置情報誤差Δθｉは他の情報誤差Δdθｉ/dt，Δd２θｉ/dt２に比べて小さく、トルク誤差への影響（定格トルクの数％以下）も少ないので、簡易化のために省略することは可能である。また、速度情報誤差Δdθｉ/dtは、加速度情報誤差Δd２θｉ/dt２に比べて小さいが、トルク誤差への影響は、後述の固体摩擦項の存在によって大きな値となる。このため速度情報誤差Δdθｉ/dtによるトルク誤差に関しては、少なくとも固体摩擦の項を考慮しておくことが望ましい。
【０１０１】
式（２６），（２７）の導出時に、dθ１/dt−Δdθｕ１/dt＞０あるいはdθ１/dt＋Δdθｕ１/dt＜０なる仮定をおいた。この仮定が満足されないdθ１/dt−Δdθｕ１/dt≦０≦dθ１/dt＋Δdθｕ１/dtの時、関節速度dθ１/dtの値は０となる可能性がある。関節速度dθ１/dtが０を横切ると、式（８）の情報量行列Ｘ(d２Θ/dt２,dΘ/dt,Θ)の固体摩擦の項ｘ１７が不連続変化するため、項ｘ１７は関節速度dθ１/dtで偏微分ができない。
【０１０２】
このように情報量行列の要素ｘｉｎが関節速度Δdθｑ/dtに対して不連続にあるいは急峻に変化するような場合、式（２５）中の｜∂ｘｉｎ／∂(dθｑ/dt)｜×Δdθｕｑ/dtの代わりに、以下の式（２９）の値を使用してもよい。
【０１０３】
【数３１】
2005102427.tif 000033
【０１０４】
式（２９）は、関節速度誤差Δdθｑ/dtが｜Δdθｑ/dt｜≦Δdθｕ１/dtの範囲で変化した時のｘｉｎの変化量最大値を示している。
【０１０５】
式（２９）は速度情報誤差Δdθｑ/dtに対するｘｉｎの変化量最大値であるが、ｘｉｎが位置情報誤差Δθｑや加速度情報誤差Δd２θｑ/dt２に対して不連続にあるいは急峻に変化する場合、式（２９）と同様に位置情報誤差Δθｑや加速度情報誤差Δd２θｑ/dt２に対するｘｉｎの変化量最大値を求めることができる。また、ｘｉｎの変化が緩やかな場合に、情報誤差Δdθｑ/dt，Δθｑ，Δd２θｑ/dt２に対するｘｉｎの変化量最大値を式（２５）で使用してもよい。
【０１０６】
dθ１/dt−Δdθｕ１/dt≦０≦dθ１/dt＋Δdθｕ１/dtの時に式（２９）を固体摩擦項ｘ１７＝ｓｉｇｎ(dθ１/dt)に適用すれば、ｘ１７の変化量最大値は２となる。従って、dθ１/dt−Δdθｕ１/dt≦０≦dθ１/dt＋Δdθｕ１/dtの時の式（２７）は、以下の式（２７）’で与えられる。
【０１０７】
【数３２】
2005102427.tif 000034
【０１０８】
固体摩擦はモータ定格トルクに対して数十％を占めることがあり、式（２７）’式の右辺最終項（固体摩擦による影響）の値は決して小さくなく、衝突検知の閾値において考慮すべき重要な値である。
【０１０９】
（第４実施形態）
第３実施形態では、各情報誤差Δθｑ，Δdθｑ/dt，Δd２θｑ/dt２が既に与えられている場合を示した。
【０１１０】
確かに各情報量θｑ，dθｑ/dt，d２θｑ/dt２として検出値を用いた場合、検出ノイズなどの影響により情報誤差Δθｑ，Δdθｑ/dt，Δd２θｑ/dt２が発生し、検出ノイズの大きさは予め見積もっておくことができる。しかし、これら検出値には検出器における検出遅れがミリセカンドオーダーで存在することがある。慣性の軽いモータでは、数ミリセカンドで定格速度近くまで加速する可能性があり、ミリセカンドオーダーの遅れでも、真の実値とのずれは大きなものとなる。また、情報量θｑ，dθｑ/dt，d２θｑ/dt２として、目標値を用いた場合、定常状態での制御誤差（真の実値との誤差）は予め見積もっておくことができるが、過渡状態での応答遅れによる誤差はさらに大きな値となる。さらに、情報量θｑ，dθｑ/dt，d２θｑ/dt２として予測値や算出値などを用いた場合も同様に、定常状態での予測誤差や算出誤差は予め見積もれるが、過渡状態での遅れなどによる誤差はさらに大きな値となる。このように、遅れなどによる時間のずれによって生じる誤差は定常状態ではほとんど発生しないが、過渡状態では大きくなる。
【０１１１】
通常運転時における過渡状態での情報誤差を予め大きめに見込み、情報誤差のオフセット分として与えることも可能である。しかし、過渡状態での誤差をオフセット分として与えた場合、誤差が小さくなる定常状態において、過大な誤差評価を行っていることとなり、衝突検知が遅れてしまう。
【０１１２】
そこで、本実施形態では、検出ノイズなどの定常状態で発生する誤差は、予め各情報誤差のオフセット分として与えておき、時間のずれなどによって発生する誤差成分を随時計算することによって、各情報誤差の範囲を最小限に設定し、ひいては閾値の範囲を最小限にして早期に衝突を検知する。
【０１１３】
時間のずれによる各情報誤差の大きさを直接特定することは難しいが、時間のずれそのものの範囲は予め見積ることは可能である。情報量θｑ，dθｑ/dt，d２θｑ/dt２の時間のずれをφ(θｑ)，φ(dθｑ/dt)，φ(d２θｑ/dt２)、時間のずれの絶対値の最大値をφｕ(θｑ)，φｕ(dθｑ/dt)，φｕ(d２θｑ/dt２)とする。時間のずれによる各情報誤差は、以下の式（３０）で表される。
【０１１４】
【数３３】
2005102427.tif 000035
【０１１５】
各情報誤差の絶対値｜Δθq｜,|Δｄ θｑ/ｄｔ|，｜Δｄ２θｑ/ｄｔ２｜と、各情報量の時間のずれの絶対値の最大値φｕ(θｑ),φｕ(ｄθｑ/ｄｔ),φｕ(ｄ２θｑ/ｄｔ２)との間には、以下の式（３１）の関係が成立する。
【０１１６】
【数３４】
2005102427.tif 000036
【０１１７】
この式（３１）式では、高次の微分の項まで含んでいるが、実際に各情報誤差の絶対値｜Δθｑ｜，｜Δdθｑ/dt｜，｜Δd２θｑ/dt２｜を算出するには、いずれかの次数で打ち切る必要がある。この場合、与えられた各情報誤差の絶対値の上限値は近似値となるため、情報量の時間のずれの絶対値の最大値をｕ(θｑ)，φｕ(dθｑ/dt)，φｕ(d２θｑ/dt２)に係数を乗じ、あるいはオフセット分を加えて大小関係を保つ必要がある。例えば、１次の項までで打ち切れば、以下の式（３２）のようになる。
【０１１８】
【数３５】
2005102427.tif 000037
【０１１９】
式（３２）のオフセット分に、検出ノイズなどの定常状態での誤差分を上乗せしておくことによって、各情報誤差の上限値Δθｕｑ，Δdθｕｑ/dt，Δd２θｕｑ/dt２を得ることができる。これらの上限値Δθｕｑ，Δdθｕｑ/dt，Δd２θｕｑ/dt２から式（２５）により各軸のトルクの誤差Δτｉが得られ、これらの誤差Δτｉから式（２０）により閾値ｔｈｉを算出することができる。
【０１２０】
（第５実施形態）
第４実施形態では、式（３２）に示すように各情報の微分値を用いて各情報誤差の上限値Δθｕｑ，Δdθｕｑ/dt，Δd２θｕｑ/dt２を算出したが、微分値を用いなくても各情報誤差を見積ることができる。
【０１２１】
図５は、モータ駆動装置の制御系構成の一例である。上位の位置指令値設定器３０、微分器３１Ａ，３１Ｂ、及び必要駆動力算出器３２から与えられた各情報の指令値に対してフィルタ３３Ａ，３３Ｂ，３３Ｃ，３３Ｄでフィルタ処理を行い、平滑化された目標値に基づいて位置速度制御器３４及び電流制御器３５によりモータ３６が制御される。
【０１２２】
この時、各目標値、あるいは目標値に追従すべく制御されている検出値、あるいは真の実値は、フィルタ処理によって指令値に対して時間が遅れている。逆にいえば、指令値は他の値に対して時間が進んでおり、他の値に対する未来予測値と考えることができる。
【０１２３】
図６を参照すると、駆動トルクの算出に検出値を用いた場合、現在の検出値に対応する指令値に対して、検出値の時間ズレ範囲での最小最大値を算出すれば、その最小最大値の範囲の中に、真の実値が含まれていることになる。従って、この最小最大値の範囲が検出値の誤差範囲となる。誤差範囲が明らかであれば、各情報量の誤差の上限値Δθｕｑ，Δdθｕｑ/dt，Δd２θｕｑ/dt２は、その誤差範囲における誤差絶対値の最大値として求めることができ。すなわち、検出値に対して、時間の進んだ値（この例では指令値）の過去の推移を用いて、検出値の未来の挙動を予測し、予め与えれた時間ズレの範囲から検出値の誤差範囲を算出することが可能である。このようにして得られた上限値Δθｕｑ，Δdθｕｑ/dt，Δd２θｕｑ/dt２から式（２５）により各軸のトルクの誤差Δτｉが得られ、これらの誤差Δτｉから式（２０）により閾値ｔｈｉを算出することができる。
【０１２４】
（第６実施形態）
以上の説明では、パラメータ誤差と情報誤差を個別に扱ってきたが、双方の誤差がともに存在した場合、双方による誤差を加算し、閾値を変更すればよい。図７は本実施形態の衝突検知装置を示す。パラメータ誤差上限設定器２５がパラメータ誤差によるトルク誤差上限として式（１５）を用い、情報誤差上限算出器２７が情報誤差によるトルク誤差上限として式（２５）を用いる場合、トータルでのトルク誤差の上限値Δτｕｉ＾は以下の式（３３）で与えられる。閾値算出器２４は、式（３３）で与えられる上限値Δτｕｉ＾から式（２０）により閾値ｔｈｉを変更する。
【０１２５】
【数３６】
2005102427.tif 000038
【０１２６】
（第７実施形態）
モータ駆動システムに対する駆動指令値（位置指令値、速度指令値、加速度指令値、及び駆動力指令値）に以下の状況が生じた場合、モータは駆動指令値通りに動作することができない。
【０１２７】
・駆動指令値を実現するために必要な必要駆動トルクが、モータ出力の最大トルクを上回る。
・駆動指令値を実現するためには、モータの最大速度を上回る。
・駆動指令値を実現するためには、モータの動作制限位置を上回る。
【０１２８】
一方、モータ駆動装置は単軸単位で制御されることが多く、式（６）により必要駆動トルクτ＾が単軸単位で算出される。式（６）における他の軸からの干渉トルクｃ(d２Θother/dt２,dΘ/dt,Θ)は、他の軸の情報に基づいて算出されるため、指令値を算出する上位のＣＰＵにおいて、指令値ベースで算出されることが多い。従って、駆動指令値が実現困難で、他の軸が指令値どおり動作しなければ、指令値ベースで算出した干渉トルクｃ(d２Θother/dt２,dΘ/dt,Θ)の値と実際の値の間には大きなずれが生じ、干渉トルクｃ(d２Θother/dt２,dΘ/dt,Θ)を使用して算出される必要駆動トルクτ＾、ひいては必要駆動トルクτ＾を使用して算出される外乱ｄにも大きな誤差が生じる。すなわち、干渉トルクｃ(d２Θother/dt２,dΘ/dt,Θ)の誤差によって、算出された外乱ｄにも大きな誤差が発生し、その外乱ｄによって検知される衝突検知も誤った検知をする。
【０１２９】
そこで、図８に示す本実施形態の衝突検知装置では、駆動指令値検査器４０を設け、以下の条件のいずれか１つが成立する場合、すなわちモータが駆動指令値どおりに動作できない状況に陥った場合には、干渉トルクｃ(d２Θother/dt２,dΘ/dt,Θ)から算出される外乱情報に基づく衝突検知を停止し、誤検知を回避する。
【０１３０】
・駆動指令値を実現するために必要な必要駆動トルクが、モータ出力の最大トルクを上回る。
・駆動指令値を実現するためには、モータの最大速度を上回る。
・駆動指令値を実現するためには、モータの動作制限位置を上回る。
【０１３１】
上記３種類の条件のいずれかがいったん成立した場合には、駆動指令値が実現可能な値になったとしても、駆動指令値どおりにモータが駆動されるまでは、衝突検知を停止する。モータが駆動指令値どおりに駆動されたと判断する条件としては、以下が考えられる。
【０１３２】
・モータの位置誤差や速度誤差が許容範囲に入る。
・駆動指令値が実現困難な状況から、実現可能な状況に遷移してから、一定の時間を設ける。
【０１３３】
次に、駆動力情報τ，Τの誤差（誤差要因３）に対応した閾値の変更について説明する。駆動力情報として、トルク指令値を用いた場合、電流制御器の制御誤差や制御応答遅れなどによる時間ずれが発生する。また、駆動力情報として、トルク検出値（通常モータ電流から逆算される。）を用いた場合、モータ電流の検出誤差、検出遅れ、あるいはモータ電流の変換計算による誤差などによって、誤差が発生する。
【０１３４】
制御誤差、検出誤差、あるいは計算誤差については、オフセット分として考慮するしかない。しかし、制御応答遅れや検出遅れについては、例えば図６を参照して説明した第５実施形態の方法で算出される駆動力の時間ずれによる誤差の上限値を閾値変更に使用することができる。
【０１３５】
なお、運動方程式モデルで考慮されていない構造的なモデル化誤差（誤差要因４）については、モデル化誤差に対する情報が全くない場合、閾値のオフセット分を大きくして対処するしか考えられない。
【図面の簡単な説明】
【０１３６】
【図１】本発明の第１実施形態の衝突検知装置を示すブロック図である。
【図２】２リンクの平行駆動型マニピュレータを示す概略図である。
【図３】１リンクの弾性マニピュレータを示す概略図である。
【図４】本発明の第３実施形態の衝突検知装置を示すブロック図である。
【図５】モータ駆動装置の制御系構成の一例を示すブロック図である。
【図６】指令値、検出値、及び真の実値の時間変化を示すグラフである。
【図７】本発明の第６実施形態の衝突検知装置を示すブロック図である。
【図８】本発明の第７実施形態の衝突検知装置を示すブロック図である。
【図９】従来の衝突検知装置を示すブロック図である。
【符号の説明】
【０１３７】
１  コントローラ
２  軌道計画
３  ロボット
４  エンコーダ
５  外乱算出器
６  評価量算出器
７  比較器
８  閾値算出器
１１，１２  関節
１３，１４  アーム
２１  外乱算出器
２２  評価量算出器
２３  比較器
２４  閾値算出器
２５  パラメータ誤差上限設定器
２７  情報誤差上限算出器
３０  位置指令値設定器
３１Ａ，３１Ｂ  微分器
３２  必要駆動力算出器
３３Ａ，３３Ｂ，３３Ｃ，３３Ｄ
３４  位置速度制御器
３５  電流制御器
３６  モータ
４０  駆動指令値検査器
【図１】
2005102427.tif 000039
【図２】
2005102427.tif 000040
【図３】
2005102427.tif 000041
【図４】
2005102427.tif 000042
【図５】
2005102427.tif 000043
【図６】
2005102427.tif 000044
【図７】
2005102427.tif 000045
【図８】
2005102427.tif 000046
【図９】
2005102427.tif 000047