JP4223911B2 - 衝突検知方法及び衝突検知装置 - Google Patents

Description

本発明は、溶接ロボットを含む各種ロボットのようなモータが被駆動体を駆動するモータ駆動装置において、被駆動体と外界の衝突を検知するための衝突検知方法及び装置に関する。

従来、モータ駆動装置の被駆動体の外界に対する衝突を検知するための種々の方法が提案されている。これらの方法では、モータ駆動装置が正常に動作しているときに必要とされる必要駆動トルクと実トルク（あるいは指令トルク）との差を外乱とし、その推定値、微分値、絶対値、予測値など外乱を用いて算出された評価値が閾値を超えたことにより衝突を検知している。

例えば、特許文献１には、外乱推定オブザーバによる外乱推定値が閾値を超えたことにより衝突を検知することが記載されている。また、特許文献２には、外乱推定値に速度を乗じた値が閾値を超えたことにより衝突を検知することが記載されている。特許文献３には、外乱推定値が閾値を所定時間超えたことにより衝突を検知することが記載されている。特許文献４には、外乱推定値の微分値が閾値を超えたことにより衝突を検知することが記載されている。特許文献５及び特許文献６には、外乱推定値とその微分値のいずれかが閾値を超えたことにより衝突を検知することが記載されている。特許文献７には、外乱と閾値の差の符号により衝突を検知することが記載されている。特許文献８には、弾性変形モデルを用いて算出した外乱推定値が閾値を超えたことにより衝突を検知することが記載されている。特許文献９には、外乱の絶対値が閾値を超えたことにより衝突を検知することが記載されている。特許文献１０には、必要駆動トルクを基準に閾値を設定し、実トルクがその閾値を超えたことにより衝突を検知することが記載されている。この特許文献１０に記載の方法は、直接外乱を算出していないが、必要駆動トルクと実トルクを比較しているので、外乱の大きさを評価していることと等価である。さらに、特許文献１１には、衝突の検知ではないが、外乱予測値が閾値を超えたことにより障害物を検知することが記載されている。

ここで外乱について説明する。ロボットの運動方程式モデルは一般に以下の式で表される。

ここで、Θはロボットの関節角度ベクトル（モータ回転角度ベクトルと１対１に比例）、Ｊ(Θ)はロボットの慣性行列、Ｃ(dΘ/dt,Θ)は重力、遠心コリオリ力、摩擦などを含むベクトル、Τはトルクベクトル、Ｄが外乱ベクトルである。式（１）から外乱ベクトルＤは以下の式（２）で算出することができる。

モータ制御の分野では一般的に単軸毎に外乱が導出されることが多い。今、ロボットのある軸に注目して式（１），（２）式を書き下せば、以下の式（３），（４）が得られる。

ここでθとτとｄはある軸の関節角度とトルクと外乱、ｊ(Θ)はある軸の慣性、ｃ(d^２Θ_other/dt^２,dΘ/dt,Θ)はある軸に対応するベクトルＣの要素と他の軸から受ける慣性力（ただしΘ_otherはθを除いた他の軸の関節角度を要素とするベクトル）である。

ｊ(Θ)やｃ(d^２Θ_other/dt^２,dΘ/dt,Θ)が既知であれば、式（４）により関節加速度d^２θ/dt^２とトルクτとから外乱ｄを導出することができる。ｊ(Θ)やｃ(d^２Θ_other/dt^２,dΘ/dt,Θ)の厳密な値は既知でないが、同定などで得られたモデルに基づいてかなり正確に導出することが可能であるので、実際にはモデルから得られたｊ(Θ)やｃ(d^２Θ_other/dt^２,dΘ/dt,Θ)を用いて、式（４）から外乱ｄが算出される。

また、モータ駆動装置を駆動させるために必要な必要駆動トルクは、式（１）あるいは式（３）の運動方程式モデルから推定可能で、式（１）左辺あるいは式（３）左辺の和として計算できる。すなわち、必要駆動トルクベクトルΤ＾とある軸に関する必要駆動トルクτ＾はそれぞれ以下の式（５），（６）で計算できる。

式（２），（４）〜（６）から明らかなように、一般に外乱は必要駆動トルクと実トルクの差として記述され、以下の式（２）’，（４）’で与えられる。

式（１）〜（６）のΘあるいはθ、dΘ/dtあるいはdθ/dt、d^２Θ/dt^２あるいはd^２θ/dt^２、及びΤあるいはτは、指令値、目標値、実測値、推定値、及び計算値のいずれを用いてもよい。

図９は、従来の衝突検知装置を一般化して示す。コントローラ１は軌道計画２を実現するための指令値としてトルクτをモータ駆動装置であるロボット３に出力し、それに従ってロボット３のモータ（図示せず）が駆動されて被駆動体であるアームやリスト（図示せず）が移動する。エンコーダ４により検出された各軸の関節角度θはコントローラ１にフィードバックされる。

式（４）に基づいて外乱算出器５が外乱ｄを算出し、この外乱ｄから評価量算出器６が衝突評価量を算出する。算出された衝突評価量は比較器７により閾値算出器８の算出した閾値と比較される。比較器７の比較結果から衝突が検知されると、コントローラ１が装置停止等の衝突時の処理を実行する。

上記特許文献１に記載のものは、図９の評価量算出器６が外乱ｄにフィルタ処理を実行するもの相当し、比較器７は外乱推定値ｄ＾が閾値を超えたことによって衝突を検知する。また、上記特許文献２に記載のものは、評価量算出器６がフィルタ処理後の外乱ｄにさらにモータ速度を乗算するものに相当する。さらに、上記特許文献４に記載のものは評価量算出器６が外乱ｄを微分操作するものに、上記特許文献９に記載のものは評価量算出器６が外乱ｄの絶対値を算出するものに、それぞれ相当する。上記特許文献９に記載のものでは、外乱ｄと閾値を比較する代わりに、ｄ＋τ＾（＝τ）と（閾値＋τ＾）を比較しているが、τ＾をそれぞれから引けば外乱ｄと閾値を比較しているのと実質は同じである。

閾値算出器８により設定される閾値を小さくすることにより、早期に衝突を検知することができる。しかし、閾値を小さくすると、誤検知の可能性が高まるので、状況に応じた閾値の変更が必要である。閾値の変更に関しては、上記特許文献６に、低速時に閾値を下げることが記載されている。また、上記特許文献１０に、ロボットの手先負荷、必要駆動トルクを算出するモデルのパラメータ推定の到達レベル、必要駆動トルクとその差分の線形和、加速度指令と加加速度指令の線形和、位置指令と速度指令と加速度指令と実位置と実速度と実加速度の線形和、停止中かどうか、ロボットの作業などによって閾値を変更することが記載されている。また、特許文献１２には、必要駆動トルクに応じて閾値を変更することが記載されている。さらに、特許文献１３には低温時に閾値を変更することが記載されている。さらにまた、特許文献１４には、溶接ロボットの溶接区間において閾値を変更することが記載されている。
特開平３−１９６３１３号公報 特開平９−３０５２３５号公報 特開平９−３０８２８３号公報 特開平１１−２８２５４０号公報 特開平１１−７０４９０号公報 特開２００１−３５３６８７号公報 特開平１１−７７５８０号公報 特開２０００−５２２８６号公報 特開２００２−２８３２７６号公報 特開２００３−２５２７２号公報 特開平６−２８９９３８号公報 特開２００１−１１７６１８号公報 特開平１１−１５５１１号公報 特開平１１−２９１１９０号公報

しかし、上記閾値を変更する従来技術は、誤検知が生じない限界まで閾値を下げているとはいえない。

閾値を下げられない理由としては、運動方程式モデルから算出される必要駆動トルクや駆動力情報の誤差が挙げられる。仮にモデルが実システムと完全に一致し、必要駆動トルクを算出するためにモデルに入力される位置情報、速度情報、及び加速度情報が真値と完全に一致していれば、算出された必要駆動トルクは実際のシステムに作用している真のトルクと完全に一致し、式（２）’，（４）’で算出される外乱は衝突しない限り常に０となり、閾値を０とすることができる。しかし、このような理想的な状況は現実にはありえず、算出された必要駆動トルクや駆動力情報は以下の要因による誤差を含み、必要駆動トルクや駆動力情報から算出される外乱にも誤差が存在する。

１）運動方程式モデルのパラメータ誤差（誤差要因１）
２）必要駆動トルク算出に用いられる位置や速度や加速度情報の誤差（誤差要因２）
３）駆動力情報τあるいはΤの誤差（誤差要因３）
４）運動方程式モデルで考慮されていない構造的なモデル化誤差（誤差要因４）

そこで、本発明は、必要駆動トルクと駆動力情報の誤差を誤差要因ごとに系統立てて扱い、随時状況に応じて閾値を必要最低限の値に変更することにより、誤検知の回避と早期の衝突検知を両立することを課題とする。

本明細書において、位置情報は関節角度ベクトルと個々の軸の関節角度を含む。また、速度情報は関節角度ベクトルと個々の角度の１階時間微分、すなわち関節速度ベクトルと個々の軸の関節速度を含む。さらに、加速度情報は関節角度の２階時間微分、すなわち関節加速度ベクトルと個々の軸の関節加速度を含む。さらにまた、駆動力情報はトルクベクトル、個々の軸のトルク、外乱ベクトル、及び個々の軸の外乱を含む。位置情報、速度情報、加速度情報、及び駆動力情報は、指令値、目標値、実測値、推定値、及び計算値のいずれであってもよい。

第１の発明は、モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体と外界の衝突を検知する、モータ駆動装置の衝突検知方法において、上記モータ駆動装置の運動方程式モデルのパラメータ誤差範囲を設定し、上記運動方程式モデルに関する位置情報、速度情報、及び加速度情報のうち少なくとも一つと、上記パラメータ誤差範囲とに基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、上記駆動力誤差範囲を定数倍した値に誤差要因による項を加算したものを上記閾値と変更することを特徴とする、モータ駆動装置の衝突検知方法を提供する。

さらに具体的には、上記運動方程式モデルを、上記位置情報、速度情報、及び加速度情報のうちの少なくとも一つによって算出可能な情報量行列と、上記運動方程式モデルのパラメータを要素にもつパラメータベクトルとの積によって記述し、上記パラメータ誤差範囲を要素にもつパラメータ誤差範囲ベクトルと、上記情報量行列の各要素の絶対値を要素にもつ情報量絶対値行列との積から、上記駆動力誤差範囲を要素にもつ駆動力誤差範囲ベクトルを算出し、上記駆動力誤差範囲ベクトルの各要素を定数倍した値に誤差要因による項を加算したものを上記閾値とする

第２の発明は、モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体と外界の衝突を検知する、モータ駆動装置の衝突検知方法において、モータを駆動するために必要な必要駆動力又は外乱を算出又は推定するために用いられる、上記モータ駆動装置の運動方程式モデルに関する位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して情報誤差範囲を設定又は算出し、上記情報誤差範囲に基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、上記駆動力誤差範囲を定数倍した値に誤差要因による項を加算したものを上記閾値とすることを特徴とする、モータ駆動装置の衝突検知方法を提供する。

あるいは、上記位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して、実システムにおける真の値と選択された情報との時間ずれの範囲を設定し、上記時間ずれの範囲から上記情報誤差範囲を算出する。

第３の発明は、モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体の衝突を検知する、モータ駆動装置の衝突検知方法において、上記モータ駆動装置に対する駆動指令値が、実現困難な指令値である場合、衝突を検知しないことを特徴とする、モータ駆動装置の衝突検知方法を提供する。

第４の発明は、モータによって駆動される被駆動体の衝突を検知するモータ駆動装置の衝突検知装置において、上記モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出する衝突評価量算出手段と、上記被駆動体の衝突を検知するために、上記衝突評価量と閾値とを比較する比較手段と、上記モータ駆動装置の運動方程式モデルのパラメータ誤差範囲を設定し、上記運動方程式モデルに関する位置情報、速度情報、及び加速度情報のうち少なくとも一つと、上記パラメータ誤差範囲とに基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、上記駆動力誤差範囲を定数倍した値に誤差要因による項を加算したものを上記閾値とする閾値算出手段とを備えることを特徴とする、モータ駆動装置の衝突検知装置を提供する。

第５の発明は、モータによって駆動される被駆動体の衝突を検知するモータ駆動装置の衝突検知装置において、上記モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出する衝突評価量算出手段と、上記被駆動体の衝突を検知するために、上記衝突評価量と閾値とを比較する比較手段と、モータを駆動するために必要な必要駆動力又は外乱を算出又は推定するために用いられる、上記モータ駆動装置の運動方程式モデルに関する位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して情報誤差範囲を設定又は算出し、上記情報誤差範囲に基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、上記駆動力誤差範囲を定数倍した値に誤差要因による項を加算したものを閾値とする閾値算出手段とを備えることを特徴とする、モータ駆動装置の衝突検知装置を提供する。

本発明では、モータ駆動装置の運動方程式のパラメータ誤差範囲、モータを駆動するために必要な必要駆動力、又は外乱を算出又は推定するために用いられる運動方程式モデルに関する位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに基づいて、衝突評価量の閾値を変更することで、随時状況に応じて閾値を必要最低限の値とし、誤検知の回避と早期の衝突検知を両立することができる。

次に、添付図面を参照して本発明の実施の形態を詳細に説明する。各図において、同一の要素には同一の符号を付している。

（第１実施形態）
第１実施形態は、運動方程式モデルのパラメータ誤差（誤差要因１）に応じて閾値を変更するものである。

必要駆動トルクＴ＾を算出する式（５）は、パラメータを要素とするパラメータベクトルPと、加速度情報である関節加速度ベクトル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^２Θ_other/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^２の絶対値の上限値をそれぞれΔθ_ｕｑ，Δ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＋Δ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≦０≦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^２)とする。時間のずれによる各情報誤差は、以下の式（３０）で表される。

各情報誤差の絶対値｜Δθq｜,|Δｄ θｑ/ｄｔ|，｜Δｄ^２θｑ/ｄｔ^２｜と、各情報量の時間のずれの絶対値の最大値φ_ｕ(θｑ),φ_ｕ(ｄθｑ/ｄｔ),φ_ｕ(ｄ^２θｑ/ｄｔ^２)との間には、以下の式（３１）の関係が成立する。

この式（３１）式では、高次の微分の項まで含んでいるが、実際に各情報誤差の絶対値｜Δθｑ｜，｜Δ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^２Θ_other/dt^２,dΘ/dt,Θ)は、他の軸の情報に基づいて算出されるため、指令値を算出する上位のＣＰＵにおいて、指令値ベースで算出されることが多い。従って、駆動指令値が実現困難で、他の軸が指令値どおり動作しなければ、指令値ベースで算出した干渉トルクｃ(d^２Θ_other/dt^２,dΘ/dt,Θ)の値と実際の値の間には大きなずれが生じ、干渉トルクｃ(d^２Θ_other/dt^２,dΘ/dt,Θ)を使用して算出される必要駆動トルクτ＾、ひいては必要駆動トルクτ＾を使用して算出される外乱ｄにも大きな誤差が生じる。すなわち、干渉トルクｃ(d^２Θ_other/dt^２,dΘ/dt,Θ)の誤差によって、算出された外乱ｄにも大きな誤差が発生し、その外乱ｄによって検知される衝突検知も誤った検知をする。

そこで、図８に示す本実施形態の衝突検知装置では、駆動指令値検査器４０を設け、以下の条件のいずれか１つが成立する場合、すなわちモータが駆動指令値どおりに動作できない状況に陥った場合には、干渉トルクｃ(d^２Θ_other/dt^２,dΘ/dt,Θ)から算出される外乱情報に基づく衝突検知を停止し、誤検知を回避する。

・駆動指令値を実現するために必要な必要駆動トルクが、モータ出力の最大トルクを上回る。
・駆動指令値を実現するためには、モータの最大速度を上回る。
・駆動指令値を実現するためには、モータの動作制限位置を上回る。

上記３種類の条件のいずれかがいったん成立した場合には、駆動指令値が実現可能な値になったとしても、駆動指令値どおりにモータが駆動されるまでは、衝突検知を停止する。モータが駆動指令値どおりに駆動されたと判断する条件としては、以下が考えられる。

・モータの位置誤差や速度誤差が許容範囲に入る。
・駆動指令値が実現困難な状況から、実現可能な状況に遷移してから、一定の時間を設ける。

次に、駆動力情報τ，Τの誤差（誤差要因３）に対応した閾値の変更について説明する。駆動力情報として、トルク指令値を用いた場合、電流制御器の制御誤差や制御応答遅れなどによる時間ずれが発生する。また、駆動力情報として、トルク検出値（通常モータ電流から逆算される。）を用いた場合、モータ電流の検出誤差、検出遅れ、あるいはモータ電流の変換計算による誤差などによって、誤差が発生する。

制御誤差、検出誤差、あるいは計算誤差については、オフセット分として考慮するしかない。しかし、制御応答遅れや検出遅れについては、例えば図６を参照して説明した第５実施形態の方法で算出される駆動力の時間ずれによる誤差の上限値を閾値変更に使用することができる。

なお、運動方程式モデルで考慮されていない構造的なモデル化誤差（誤差要因４）については、モデル化誤差に対する情報が全くない場合、閾値のオフセット分を大きくして対処するしか考えられない。

本発明の第１実施形態の衝突検知装置を示すブロック図である。 ２リンクの平行駆動型マニピュレータを示す概略図である。 １リンクの弾性マニピュレータを示す概略図である。 本発明の第３実施形態の衝突検知装置を示すブロック図である。 モータ駆動装置の制御系構成の一例を示すブロック図である。 指令値、検出値、及び真の実値の時間変化を示すグラフである。 本発明の第６実施形態の衝突検知装置を示すブロック図である。 本発明の第７実施形態の衝突検知装置を示すブロック図である。 従来の衝突検知装置を示すブロック図である。

符号の説明

１ コントローラ
２ 軌道計画
３ ロボット
４ エンコーダ
５ 外乱算出器
６ 評価量算出器
７ 比較器
８ 閾値算出器
１１，１２ 関節
１３，１４ アーム
２１ 外乱算出器
２２ 評価量算出器
２３ 比較器
２４ 閾値算出器
２５ パラメータ誤差上限設定器
２７ 情報誤差上限算出器
３０ 位置指令値設定器
３１Ａ，３１Ｂ 微分器
３２ 必要駆動力算出器
３３Ａ，３３Ｂ，３３Ｃ，３３Ｄ
３４ 位置速度制御器
３５ 電流制御器
３６ モータ
４０ 駆動指令値検査器

Claims (7)

1. モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体と外界の衝突を検知する、モータ駆動装置の衝突検知方法において、
   上記モータ駆動装置の運動方程式モデルのパラメータ誤差範囲を設定し、
   上記運動方程式モデルに関する位置情報、速度情報、及び加速度情報のうち少なくとも一つと、上記パラメータ誤差範囲とに基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、
   上記駆動力誤差範囲を定数倍した値に誤差要因による項を加算したものを上記閾値とすることを特徴とする、モータ駆動装置の衝突検知方法。
2. 上記運動方程式モデルを、上記位置情報、速度情報、及び加速度情報のうちの少なくとも一つによって算出可能な情報量行列と、上記運動方程式モデルのパラメータを要素にもつパラメータベクトルとの積によって記述し、
   上記パラメータ誤差範囲を要素にもつパラメータ誤差範囲ベクトルと、上記情報量行列の各要素の絶対値を要素にもつ情報量絶対値行列との積から、上記駆動力誤差範囲を要素にもつ駆動力誤差範囲ベクトルを算出し、
   上記駆動力誤差範囲ベクトルの各要素を定数倍した値に誤差要因による項を加算したものを上記閾値とすることを特徴とする、請求項１に記載の衝突検知方法。
3. モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体と外界の衝突を検知する、モータ駆動装置の衝突検知方法において、
   モータを駆動するために必要な必要駆動力又は外乱を算出又は推定するために用いられる、上記モータ駆動装置の運動方程式モデルに関する位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して情報誤差範囲を設定又は算出し、
   上記情報誤差範囲に基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、
   上記駆動力誤差範囲を定数倍した値に誤差要因による項を加算したものを上記閾値とすることを特徴とする、モータ駆動装置の衝突検知方法。
4. 上記位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して、実システムにおける真の値と選択された情報との時間ずれの範囲を設定し、
   上記時間ずれの範囲から上記情報誤差範囲を算出することを特徴とする、請求項３に記載の衝突検知方法。
5. モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出し、該衝突評価量と閾値とを比較して上記被駆動体の衝突を検知する、モータ駆動装置の衝突検知方法において、
   上記モータ駆動装置に対する駆動指令値が、実現困難な指令値である場合、衝突を検知しないことを特徴とする、請求項３に記載のモータ駆動装置の衝突検知方法。
6. モータによって駆動される被駆動体の衝突を検知するモータ駆動装置の衝突検知装置において、
   上記モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出する衝突評価量算出手段と、
   上記被駆動体の衝突を検知するために、上記衝突評価量と閾値とを比較する比較手段と、
   上記モータ駆動装置の運動方程式モデルのパラメータ誤差範囲を設定し、上記運動方程式モデルに関する位置情報、速度情報、及び加速度情報のうち少なくとも一つと、上記パラメータ誤差範囲とに基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、上記駆動力誤差範囲を定数倍した値に誤差要因による項を加算したものを上記閾値とする閾値算出手段と
   を備えることを特徴とする、モータ駆動装置の衝突検知装置。
7. モータによって駆動される被駆動体の衝突を検知するモータ駆動装置の衝突検知装置において、
   上記モータ又は被駆動体に加わる外乱と関連する衝突評価量を算出する衝突評価量算出手段と、
   上記被駆動体の衝突を検知するために、上記衝突評価量と閾値とを比較する比較手段と、
   モータを駆動するために必要な必要駆動力又は外乱を算出又は推定するために用いられる、上記モータ駆動装置の運動方程式モデルに関する位置情報、速度情報、加速度情報、及び駆動力情報のうち少なくとも一つに対して情報誤差範囲を設定又は算出し、上記情報誤差範囲に基づいて、上記運動方程式モデルから算出される駆動力の誤差範囲を算出し、上記駆動力誤差範囲を定数倍した値に誤差要因による項を加算したものを閾値とする閾値算出手段と
   を備えることを特徴とする、モータ駆動装置の衝突検知装置。

Images