JP4567308B2 - 衝突検知方法及び衝突検知装置 - 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^２Θ_other/dt^２,dΘ/dt,Θ)とにより、上記式（４）から外乱ｄを時々刻々と算出する。外乱算出器１２により算出された外乱ｄはパワー算出器１３に入力される。パワー算出器１３は、演算器１４、第１及び第２の自乗演算器１５，１６、及び加算器１７を備え、外乱ｄから外乱パワーＰ_ｄを算出する。

外乱パワー算出器１１で算出される外乱パワーＰ_ｄは以下の式（５）で定義される。

ここでｄ’は外乱変化速度、ω_ｎはロボットの固有振動の角速度である。

外乱ｄが固有振動数で振動している場合の外乱ｄとその変化速度ｄ’は、それぞれ以下の式（６），（７）で表される。Ａは振幅である。

式（６），（７）から振幅の自乗Ａ^２は以下の式（８）で表される。

式（８）と式（５）から明らかなように、外乱パワーＰ_ｄは外乱ｄに含まれる固有振動成分を考慮した衝突評価量である。また、式（５）において外乱変化速度の自乗ｄ'^２に乗じる係数１/ω_ｎ ^２は、式（８）から得られた。ただし、式（５）において外乱変化速度の自乗ｄ'^２に乗じる係数は、１/ω_ｎ ^２に限定されず適宜設定することができる。

次に、図１の衝突検知装置によって実行される衝突検知方法を説明する。まず、外乱算出器１２はコントローラ１から入力されるトルクτとエンコーダ４から入力される関節角度θとから、外乱ｄを時々刻々と算出する。次に、外乱算出器１２から入力された外乱ｄから、パワー算出器１３の第１の自乗演算器１５が外乱ｄの自乗値ｄ^２を算出する。また、演算器１４が外乱算出器１２から入力された外乱ｄから外乱変化速度ｄ’を算出し、さらにこの外乱変化速度ｄ’に係数１/ω_ｎを乗じた値ｄ'×（１/ω_ｎ）を算出する。次に、第２の自乗演算器１６が自乗値ｄ'^２×（１/ω_ｎ ^２）を算出する。加算器１７は自乗値ｄ^２と自乗値ｄ'^２×（１/ω_ｎ ^２）を加算して、外乱パワーＰ_ｄを算出する。時々刻々と算出される外乱パワーＰ_ｄはコントローラ１に出力され、閾値と比較される。コントローラ１は外乱パワーＰ_ｄが閾値以上となると衝突であると判断し、ロボット３の動作停止等の必要な制御を実行する。

図３の実線は、外乱算出器１２により算出される外乱ｄと、外乱パワーＰ_ｄの平方根√Ｐ_ｄとを示している。外乱パワーＰ_ｄの平方根をとっているのは、外乱ｄと次元を揃えて比較するためであり、コントローラ１による閾値判定の際に外乱パワーＰ_ｄの平方根をとる必要はなく、図３における閾値ＴＨの自乗値ＴＨ^２を外乱パワーＰ_ｄの閾値として使用すればよい。図３では、外乱ｄは閾値ＴＨを下回っているが、外乱パワーＰ_ｄの平方根は閾値ＴＨを上回っている。従って、外乱パワーＰ_ｄを衝突評価量として用いることにより、振動する外乱ｄを適性に処理し、外乱ｄを衝突評価量とした場合には検知できない衝突を早期に検知できる。

（第２実施形態）
上記式（２），（４）では、加速度やトルクを用いて外乱ｄを算出しているので、算出された外乱ｄには多くのノイズが含まれる。このノイズの影響を除去して安定した衝突検知を実現するために、外乱オブザーバなどを使用した高周波除去特性を示すフィルタ処理でノイズ除去をすることが従来から知られている。この種の従来の外乱オブザーバでは、外乱ｄを一定値外乱（ｄの時間微分値が０）と仮定して構成しており、このようにして構成された外乱オブザーバの外乱推定値ｄ＾の動特性は以下の式（９）で与えられ、外乱ｄに対して１次遅れ特性を示す（図４参照）。

ここでｓはラプラス演算子、ｄ＾(ｓ)とｄ(ｓ)はそれぞれ外乱推定値ｄ＾と外乱ｄのラブラス変換、Ｔ_dは外乱推定値の収束時定数である。

これに対し、本実施形態では、外乱パワーＰ_ｄやその平方根√Ｐ_ｄにフィルタ処理を行う。外乱パワーＰｄに式（９）と同様のフイルタ処理を施すと、以下の式（１０）により外乱パワーＰ_ｄの推定値Ｐ_ｄ＾が得られる。

同様に、外乱パワーの平方根√Ｐ_ｄに式（９）と同様のフィルタ処理を施すと、以下の式（１１）により外乱パワーＰ_ｄの平方根の推定値√Ｐ_ｄ＾が得られる。

これらフィルタリングにより得られた推定値Ｐ_ｄ＾，√Ｐ_ｄ＾が衝突評価量として使用される。

図５は、外乱パワーの平方根√Ｐ_ｄに対して式（１１）のフイルタ処理を施すようにした衝突検知装置を示し、外乱パワー平方根推定器１８とコントローラ１により構成される。外乱パワー平方根推定器１８は、外乱パワー算出器１１、平方根算出器１９、及びフィルタ２０を備えている。外乱パワー算出器１１は、トルクτと関節角度θから式（５）によって時々刻々と外乱パワーＰ_ｄを算出する。平方根算出器１９は外乱パワー算出器１１から入力される外乱パワーＰ_ｄの平方根を算出する。算出された平方根√Ｐ_ｄに対してフィルタ２０によって式（１１）のフィルタ処理が施され、外乱パワーの平方根の推定値√Ｐ_ｄ＾が算出される。この推定値√Ｐ_ｄ＾はコントローラ１に出力され、閾値と比較される。

図６は、図３に示す外乱ｄに対して式（９）のフィルタ特性を持つオブザーバを構成して得られる推定値ｄ＾と、図３に示す外乱ｄに対して外乱パワー平方根推定器１８で式（１１）のフィルタ処理を行うことで得られる外乱パワーの平方根の推定値√Ｐ_ｄ＾とを示す。外乱パワーの平方根√Ｐ_ｄに式（１１）のフィルタ処理を施すことにより、ノイズの影響を受けない安定した衝突検知が可能となり、衝突検知のための閾値ＴＨを下げることができる。

固有振動周波数成分からなる外乱ｄに式（９）のフィルタ処理を施した場合もノイズの影響が低減されるが、得られる外乱推定値ｄ＾の最大値はフィルタ処理によって大きく減衰する。具体的には、図６の外乱推定値ｄ＾の最大値は、図３に示す外乱ｄの最大値の約５０％に減衰している。そのため、図６では閾値ＴＨを図３の約０．５５倍に設定しているにもかかわらず、外乱推定値ｄ＾の最大値は閾値ＴＨを下回っており、衝突を検知できない。

これに対して、外乱パワーＰ_ｄは固有振動成分などの高周波成分がほとんど含まず、より低い周波数成分から構成されているため、式（１１）のフィルタ処理による高周波除去特性の影響が大幅に緩和される。具体的には、図６に示す外乱パワーの平方根の推定値√Ｐ_ｄ＾の最大値は、外乱パワーの平方根√Ｐ_ｄの約７０％であり、閾値ＴＨを大きく上回っている。

このようにパワーＰ_ｄには固有振動成分をほとんど含んでいないため、フィルタ処理による振幅減衰の影響が外乱ｄをフィルタ処理する場合と比較して大幅に緩和される。従って、フィルタ処理により得られる推定値√Ｐ_ｄ＾，Ｐ_ｄ＾は閾値を大きく上回り、外乱パワーＰ_ｄそのものを衝突評価量とする場合よりも、さらに安定した衝突検知が可能となる。また、非負値とした上でフィルタ処理することによって、フィルタによる外乱の振幅低減を抑制しつつ、ノイズを除去することができる。

（第３実施形態）
図７及び図８は、第３実施形態に係る衝突検知装置を示す。この第３実施形態は、外乱絶対値Ａ_ｄの推定値Ａ_ｄ＾を衝突評価量として使用する。

衝突検知装置は、外乱絶対値Ａ_ｄの推定値Ａ_ｄ＾を算出する外乱絶対値推定器２２と、推定値Ａ_ｄ＾を閾値と比較して衝突を判定するコントローラ１により構成される。外乱絶対値推定器２２は外乱絶対値算出器２３とフィルタ２５を備えている。また、外乱絶対値算出器２３は外乱算出器１２と、絶対値算出器２４とを備えている。

外乱算出器１２がトルクτと関節角度θから時々刻々と算出する外乱ｄは絶対値算出器２４に出力され、絶対値算出器２４は以下の式（１２）の外乱絶対値Ａ_ｄを算出する。

この式（１２）に示すように、外乱ｄと同符合の値としてｓｉｇｎ（ｄ）を外乱ｄに乗じることで、非負値である外乱絶対値Ａ_ｄを算出している。

外乱絶対値Ａ_ｄはフィルタ２５に出力され、以下の式（１３）のフィルタ処理により外乱絶対値Ａ_ｄの推定値Ａ_ｄ＾が算出される。

推定値Ａ_ｄ＾はコントローラ１に出力され、閾値と比較される。

図９は、図３に示す外乱ｄに対して式（９）のフィルタ特性を持つオブザーバを構成して得られる推定値ｄ＾と、図３に示す外乱ｄから外乱絶対値推定器２２が式（１２），（１３）で算出した外乱絶対値Ａ_ｄの推定値Ａ_ｄ＾を示す。閾値ＴＨは図６と同一に設定している。外乱推定値ｄ＾が閾値を上回らないため衝突を検知できないのに対して、推定値Ａ_ｄ＾は閾値を上回り衝突を検知できる。図６の外乱パワーＰ_ｄの推定値Ｐ_ｄ＾が閾値ＴＨを大きく上回っているのと比較すると、図９において外乱絶対値Ａ_ｄの推定値Ａ_ｄ＾が閾値ＴＨを上回る程度は小さい。従って、外乱絶対値Ａ_ｄの推定値Ａ_ｄ＾を衝突評価量として使用すると、外乱パワーＰ_ｄの推定値Ｐ_ｄ＾を使用する場合よりも検知の安定性が少し劣るが、外乱ｄの推定値ｄ＾を使用する場合よりも安定した検知が可能である。

（第４実施形態）
図１０及び図１１は、第４実施形態に係る衝突検知装置を示す。この第４実施形態は、上記第３実施形態よりも検出精度を向上させて、第２実施形態のように外乱パワーＰ_ｄの推定値Ｐ_ｄ＾を使用する場合に近付けるために、外乱変化速度ｄ’の絶対値Ａ_ｄ’の推定値Ａ_ｄ’＾を衝突評価量として使用する。

衝突検知装置は、外乱変化速度ｄ’の絶対値Ａ_ｄ’の推定値Ａ_ｄ’＾を算出する外乱変化速度絶対値推定器２６と、推定値Ａ_ｄ’＾を閾値と比較して衝突を判定するコントローラ１により構成される。外乱変化速度絶対値推定器２６は外乱変化速度絶対値算出器２７とフィルタ２８を備えている。また、外乱変化速度絶対値算出器２７は外乱算出器１２と速度絶対値算出器２９とを備えている。さらに、速度絶対値算出器２９は速度算出器３０と絶対値算出器３１とを備えている。

外乱算出器１２がトルクτと関節角度θから時々刻々と算出する外乱ｄは速度絶対値算出器２９に出力される。速度絶対値算出器２９の速度算出器３０が外乱変化速度ｄ’を算出し、絶対値算出器３１に算出する。絶対値算出器３１は以下の式（１４）の外乱変化速度ｄ’の絶対値Ａ_ｄ’を算出する。

この式（１４）に示すように、外乱変化速度ｄ’と同符号の値としてｓｉｇｎ（ｄ’）を外乱変化速度ｄ’に乗じることで、非負値である絶対値Ａ_ｄ’を算出する。

外乱変化速度ｄ’の絶対値はＡ_ｄ’フィルタ２８に出力され、以下の式（１５）のフィルタ処理により外乱変化速度ｄ’の絶対値Ａ_ｄ’の推定値Ａ_ｄ’＾が算出される。

推定値Ａ_ｄ’＾はコントローラ１に出力され、閾値と比較される。第２実施形態では外乱の非負値としてフィルタ処理するが、本実施形態では外乱速度の非負値としてフィルタ処理している。非負値とした上でフィルタ処理することにより、フィルタによって振幅減衰を抑制しつつ、ノイズを除去することができる。

なお、本実施形態において外乱変化速度ｄ’に変えて所定時間内の外乱の変化量を使用してもよい。

（第５実施形態）
図１２は、第５実施形態に係る衝突検知装置を示す。外乱変化速度絶対値Ａ_ｄ’やその推定値Ａ_ｄ’＾をそのまま衝突評価量として使用した場合、外乱ｄやその推定値ｄ＾を使用するよりも高精度の検知が可能であるが、第１実施形態や第２実施形態のように外乱パワーＰ_ｄやその推定値Ｐ_ｄ＾を使用する場合よりは検出精度は低い。第５実施形態では、以下の式（１６）で定義される絶対値和Ｂ_ｄを衝突評価量として使用する。

衝突検知装置は、以下の式（１６）で定義される絶対値和Ｂ_ｄを算出する絶対値和算出器３３と、絶対値和Ｂ_ｄを閾値と比較して衝突を判定するコントローラ１により構成される。

この式（１６）に示すように、絶対値和Ｂ_ｄは式（１２）の外乱絶対値Ａ_ｄと、式（１４）の外乱変化速度の絶対値Ａ_ｄ’に係数１／ω_ｎを乗じたものの和である。

絶対値和算出器３３は、外乱算出器１２、絶対値算出器２４、速度絶対値算出器２９、乗算器３４、及び加算器３５を備えている。

外乱算出器１２がトルクτと関節角度θから時々刻々と算出する外乱ｄは、絶対値算出器２４と速度絶対値算出器２９に出力される。絶対値算出器２４は上述の式（１２）により外乱絶対値Ａ_ｄを算出する。速度絶対値算出器２９は外乱変化速度ｄ’を算出し、さらに上述の式（１４）により外乱変化速度ｄ’の絶対値Ａ_ｄ’を算出する。外乱変化速度ｄ’の絶対値Ａ_ｄ’は乗算器３４に入力され１/ω_ｎが乗算される。外乱絶対値Ａ_ｄと１/ω_ｎを乗じた絶対値Ａ_ｄ’は加算器３５に出力され、式（１６）により絶対値和Ｂ_ｄが算出される。絶対値和Ｂ_ｄはコントローラ１に出力され、閾値と比較される。

図１３及び図１４は、図３に示す外乱ｄに対して絶対和算出器３３が式（１６）により算出した絶対値和Ｂ_ｄを１／√２倍した値を、外乱ｄ、外乱絶対値Ａ_ｄ、及び外乱パワーＰ_ｄの平方根√Ｐ_ｄと共に示す。絶対値和Ｂ_ｄを１／√２倍しているのは、外乱ｄと大きさを揃えて比較するためであり、コントローラ１による閾値判定の際に絶対値和Ｂ_ｄを１／√２倍する必要はなく、図３における閾値ＴＨの１／√２倍を絶対値和Ｂ_ｄの閾値として使用すればよい。外乱ｄや外乱絶対値Ａ_ｄが閾値ＴＨを上回らないのに対し、外乱パワーＰ_ｄの平方根√Ｐ_ｄと同様に絶対値和Ｂ_ｄは閾値ＴＨを上回り衝突を検知できる。外乱パワーＰ_ｄの平方根√Ｐ_ｄと比較すると、絶対知和Ｂ_ｄが閾値ＴＨを上回る程度は小さい。従って、絶対知和Ｂ_ｄを衝突評価量として使用すると、外乱パワーＰ_ｄを使用する場合よりも少し劣るが、外乱ｄや外乱絶対値Ａ_ｄを使用する場合よりも安定性の高い検知が可能である。

（第６実施形態）
図１５は、第６実施形態に衝突検知装置を示す。第６実施形態では、絶対値和Ｂ_ｄに対して以下の式（１７）のフィルタ処理を施して得られる絶対値和Ｂ_ｄの推定値Ｂ_ｄ＾を衝突評価量として使用する。

衝突検知装置は、第５実施形態の絶対値和算出器３３にさらにフィルタ３８を追加した絶対値和推定器３７と、絶対値和Ｂ_ｄの推定値Ｂ_ｄ＾を閾値と比較して衝突を判定するコントローラ１により構成される。

絶対値和算出器３３はトルクτと関節角度θから式（１６）の絶対値和Ｂ_ｄを時々刻々と算出し、フィルタ３８に出力する。フィルタ３８は式（１７）により推定値Ｂ_ｄ＾を算出する。推定値Ｂｄ＾はコントローラ１に出力され、閾値と比較される。

図１６は、図３に示す外乱ｄに対して絶対値和推定器３７が式（１７）により算出した絶対値和Ｂ_ｄの推定値Ｂ_ｄ＾を１／√２倍した値を、外乱推定値ｄ＾、外乱パワーＰ_ｄの推定値Ｐ_ｄ＾の平方根√Ｐ_ｄ＾、及び外乱絶対値Ａ_ｄの推定値Ａ_ｄ＾と共に示す。絶対値和Ｂ_ｄを１／√２倍しているのは、上述の図１３及び図１４と同様に外乱ｄと大きさを揃えて比較するためである。外乱推定値ｄ＾が閾値ＴＨを上回らず、外乱絶対値Ａ_ｄの推定値Ａ_ｄ＾がかろうじて閾値ＴＨを上回るのに対し、絶対値和Ｂ_ｄの推定値Ｂ_ｄ＾は外乱パワーＰ_ｄの推定値Ｐ_ｄ＾と同様に閾値ＴＨを大きく上回っている。従って、絶対値和Ｂ_ｄの推定値Ｂ_ｄ＾を衝突評価量として使用することで、高精度の検知が可能となる。

図１７及び図１８は、図３の場合とは異なり外乱ｄに含まれる固有振動成分が減衰しない場合の外乱ｄ、外乱パワーＰ_ｄの平方根√Ｐ_ｄ、外乱絶対値Ａ_ｄ、及び絶対値和Ｂ_ｄを１／√２倍した値を示す。また、図１９は、この場合の外乱推定値ｄ＾、外乱パワーＰ_ｄの推定値Ｐ_ｄ＾、外乱絶対値Ａ_ｄの推定値Ａ_ｄ＾、及び絶対値和Ｂ_ｄの推定値Ｂ_ｄ＾を示す。外乱推定値ｄ＾が閾値ＴＨに達しないの対し、本実施形態の絶対値和Ｂ_ｄの推定値Ｂ_ｄ＾は閾値を大きく上回っている。従って、外乱ｄに含まれる固有振動が減衰しない場合でも、絶対値和Ｂ_ｄの推定値Ｂ_ｄ＾を衝突評価量として使用すれば、衝突を検知することができる。同様に、第２実施形態の外乱パワーＰ_ｄの推定値Ｐ_ｄ＾や、第３実施形態の外乱絶対値Ａ_ｄの推定値Ａ_ｄ＾を衝突評価量として使用すれば、外乱ｄに含まれる固有振動が減衰しない場合でも、衝突を検知することができる。

（第７実施形態）
以上の説明では、外乱ｄや外乱変化速度ｄ’などが固有振動で振動し、その他の周波数成分が含まれない状況について示してきた。しかし、実際には外乱ｄや外乱変化速度ｄ’は、固有振動以外の他の周波数成分も含まれていることが多い。第１実施形態から第６実施形態を、固有振動以外の他の周波数成分も含まれる場合にも適用するためには、外乱ｄや外乱速度ｄ’から固有振動周波数の外乱成分ｄ_ｎや固有振動周波数の外乱速度成分ｄ_ｎ’を抜きだし、これらを外乱ｄや外乱ｄ’に代えて使用すればよい。

図２０に示す第７実施形態では、第１実施形態の外乱パワーＰ_ｄの算出において外乱ｄに代えて固有振動周波数の外乱成分ｄ_ｎを使用する。具体的には、外乱パワー成分算出器４３は、図１に示す外乱パワー算出器１１の外乱算出器１２（図２参照）を、図２１に示す固有振動数の外乱成分ｄ_ｎを算出する外乱成分算出器４１に置換した構成である。さらに具体的には、外乱成分算出器４１は、外乱算出器１２に固有振動周波数の外乱成分を抜き出すためのバンドパスフィルタ４２を追加した構成である。

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

固有振動数の外乱成分ｄ_ｎは、パワー算出器１３に入力される。パワー算出器１３は、以下の式（１９）で示すように固有振動数の外乱成分ｄ_ｎから固有振動数の外乱パワー成分Ｐ_ｄｎを算出する。式（１９）においてｄ_ｎ’は固有振動数の外乱変化速度成分である。

外乱パワー成分Ｐ_ｄｎはコントローラ１に入力され、閾値と比較される。

図２２は、第７実施形態の外乱パワー算出器１１が算出した固有振動周波数の外乱パワー成分Ｐ_ｄｎの平方根√Ｐ_ｄｎを、外乱ｄと共に示す。図３に外乱パワーＰ_ｄと同様に、外乱パワー成分Ｐ_ｄｎの平方根をとっているのは単に外乱ｄと次元を揃えて比較するためである。図２２では、外乱ｄは閾値ＴＨを下回っているが、外乱パワー成分Ｐ_ｄｎは閾値ＴＨを上回り、衝突が検知される。

（第８実施形態）
図２３に示す本発明の第８実施形態は、第７実施形態の外乱パワー成分算出器４３にさらにフィルタ４５を追加した構成の外乱パワー成分推定器４４を備えている。フィルタ４４は外乱パワー成分算出器４３により算出された外乱パワー成分Ｐ_ｄｎに対し、以下の式（２０）のフィルタ処理を施して外乱パワー成分Ｐ_ｄｎの推定値Ｐ_ｄｎ＾を算出する。

このフィルタ処理により得られる推定値Ｐ_ｄｎ＾は衝突評価量としてコントローラ１に出力され、閾値と比較される。

図２４は、図３に示す外乱ｄに対して外乱パワー成分推定器４４が式（２０）のフィルタ処理により算出した外乱パワー成分Ｐ_ｄｎの推定値Ｐ_ｄｎ＾の平方根√Ｐ_ｄｎ＾を、外乱推定値Ｐ＾と共に示す。推定値Ｐ_ｄｎ＾の平方根をとっているのは単に外乱ｄと次元を揃えて比較するためである。図２４では、外乱ｄは閾値ＴＨを下回っているが、外乱パワー成分Ｐ_ｄｎの推定値Ｐ_ｄｎ＾は図２２の外乱パワー成分Ｐ_ｄｎと同様に閾値ＴＨを上回り、衝突が検知される。

（第９実施形態）
以上の説明では、衝突時の固有振動に着目した衝突検知について述べた。しかし、ロボット３が穏やかに外界と接触し、徐々に外界に押し付けられていくような場合、外乱ｄは図３に示すように振動することなく、図２５に示すようにランプ状に変化する。第９実施形態は、このような外乱ｄがランプ状に変化する場合の衝突を検知するものである。

図２６を参照すると、衝突検知装置は外乱算出器１２、速度演算器４６、予測時間算出器４７、及び予測時間算出器４７の算出した閾値到達予測時間Ｔ_ｐと閾値とを比較して衝突を判定するコントローラ１により構成される。

外乱算出器１２はトルクτと関節角度θから時々刻々と外乱ｄを算出し、速度演算器４６は外乱ｄから外乱変化速度ｄ’を算出する（図２７のステップＳ１参照）。外乱ｄと外乱変化速度ｄ’は予測時間算出器４７に入力される。

予測時間算出器４７は、以下の式（２１）により外乱ｄが上限閾値ｄ_ulmt又は下限閾値ｄ_llmtに到達する予測時間（閾値到達予測時間）Ｔ_ｐを算出する（図２７のステップＳ２〜Ｓ６参照）。

閾値到達予測時間Ｔ_ｐはコントローラ１に出力され、所定の時間閾値Ｔ_lmtと比較される。以下の式（２１）’に示すように閾値到達予測時間Ｔ_ｐが時間閾値Ｔ_lmt以下となると、衝突検知であると判断される（図２７のステップＳ７参照）。

衝突を検知してからロボット３が減速停止するまでに要する時間と同じ又はそれよりも長い時間に時間閾値Ｔ_ｌｍｔを設定しておけば、外乱ｄが閾値を超える前にロボット３を停止させることが可能である。換言すれば、外乱ｄは閾値を超えることがない。外乱ｄやその推定値ｄ＾を使用する衝突検知では、ロボットが停止するまでに、外乱ｄが閾値を超える危険性が高いので閾値を低めに設定していた。これに対して本実施形態では外乱ｄが閾値を超えることがないため、外乱ｄとして許容可能な最大値を上限閾値ｄ_ulmt又は下限閾値ｄ_llmtとするなど、従来よりも大きめの閾値を設定可能であり、閾値が低いために発生する誤検知をなくすことができる。

なお、本実施形態において、閾値到達予測時間Ｔ_ｐの算出に外乱ｄや外乱変化速度ｄ’の予測値ｄ＾，ｄ’＾を使用してもよい。

第９実施形態の変形例について説明すると、衝突の検知は式（２１）に限定されない。ｄ_ｕｌｍｔ＝ｄ_ｌｍｔ、ｄ_ｌｌｍｔ＝−ｄ_ｌｍｔとし、式（２１）の両辺にａｂｓ(ｄ’)を乗じると、以下の式（２２）が得られる。

この式（２２）を使用すれば、閾値到達予測時間Ｔ_ｐを算出することなく、外乱ｄと外乱変化速度ｄ’を使用して直接的に衝突を検知することができる。なお、本実施形態において、予め定められた所定時間後の外乱を予測しそれに基づいて衝突を検知してもよい。

（第１０実施形態）
モータ駆動系には、トルクリップルや減速機の角度伝達誤差などによって周期的な外乱が加わる。衝突による固有振動とは異なり、これらの周期的外乱は衝突とは全く関係がなく、衝突を検知するために算出された外乱においてはノイズとして作用する。従って、トルクリップルや減速機の角度伝達誤差などによる周期的外乱を除去した外乱に基づいて衝突検知を実施すれば、より高精度な衝突検知を行うことができる。

今、トルクリップルや減速機の角度伝達誤差の周期をω_ｆとすれば、その周期の外乱成分は式（１８）のω_ｎをω_ｆに置き換えた、以下の式（２３）で推定される。

以下の式（２４）に示すように、式（２３）で推定される外乱成分ｄ_ｆをもとの外乱ｄから引くことで、トルクリップル等（周期ω_ｆ）による周期成分を除去した外乱成分ｄ_ｃが得られる。

あるいは外乱成分ｄ_ｆを求めることなく、外乱成分ｄ_ｃを直接求めるためには、１から式（１８）の伝達関数を引いて得られる、以下の式（２５）の伝達関数を有するフィルタを使用すればよい。

式（２３），（２４）から導出された外乱成分ｄ_ｃと、式（２５）から算出された外乱成分ｄ_ｃとは同一の値となる。

図２８は、図２５のランプ状の外乱にさらにトルクリップルや角度伝達誤差による周期的な外乱が付加された外乱ｄと、この外乱ｄに対する式（２４）の外乱成分ｄ_ｃを示す。この図２８から明らかなように、式（２４）の外乱成分ｄ_ｃではトルクリップルや減速機の角度伝達誤差による周期的外乱が除去されるので、外乱成分ｄ_ｃを使用することでほとんどそれらの周期的外乱が存在しない状態と変わりなく衝突を検知することができる。

（第１１実施形態）
以上の説明では、固有振動数で振動する外乱（図３参照）と、ランプ状の外乱（図２５参照）を個別に取り扱ったが、これら２種類の外乱が同時に発生する場合がある。

この場合、固有振動周期の外乱成分ｄ_ｎを除去した外乱成分ｄ_ｏに対して第９又は第１０実施形態の方法を適用することができる。また、固有振動周期の外乱成分ｄ_ｎに対して第１から第８実施形態の方法を適用することができる。本実施形態は、上記２種類の外乱が同時に作用する場合によりに安定した、早期の衝突検知を実現するために、次の衝突評価量Ｃ_ｄを導入する。

まず、外乱_ｄから式（１８）の固有振動周期の外乱成分ｄ_ｎを除去した外乱成分ｄ_ｏを、以下の式（２６）により算出する。外乱_ｄに代えてその推定値ｄ＾を使用してもよい。

式（２５）と同様の以下の式（２７）により外乱成分ｄ_ｏを算出してもよい。

次に、式（２６）又は式（２７）で得られる外乱成分ｄ_ｏの絶対値Ａ_ｄｏは、以下の式（２８）で表される。

評価値Ｃ_ｄは、以下の式（２９）に示すように、絶対値Ａ_ｄｏと式（１９）の固有振動周期Ｐ_ｄｎの平方根√Ｐ_ｄｎの和として定義される。

このＣ_ｄを衝突評価量とし、評価量が閾値を超えたことによって衝突を検知する。

図２９を参照すると、本実施形態の衝突検知装置は、評価量Ｃ_ｄを算出する評価量算出器４９と、評価量Ｃ_ｄを閾値と比較して衝突を判定するコントローラ１により構成される。評価量算出器４９は、外乱パワー平方根算出器５０と、外乱成分絶対値算出器５１とを備える。外乱パワー平方根算出器５０は、第１実施形態の外乱パワー算出器１１にさらにバンドパスフィルタ５２と平方根算出器５３を付加した構成であり、外乱算出器１２がトルクτと関節角度θより時々刻々と算出する外乱ｄから、固有振動周期の外乱パワー成分の平方根√Ｐｄ_ｎを算出する。一方、外乱成分絶対値算出器５１は、外乱パワー平方根算出器５０から入力される外乱ｄと外乱成分ｄ_ｎと式（２６）の外乱成分ｄ_ｏを算出する減算器５５と、減算器５５から出力される外乱成分ｄ_ｏの絶対値Ａ_ｄｏ（式（２８））を算出する絶対値算出器５６とを備える。外乱パワー平方根算出器５０から出力される固有振動周期の外乱パワー成分の平方根√Ｐｄ_ｎと、外乱成分絶対値算出器５１から出力される外乱成分ｄｏの絶対値Ａｄｏが加算器５７で加算され、衝突評価量Ｃ_ｄが算出される。

図３０は、固有振動数で振動する外乱とランプ状の外乱が同時に発生した場合の外乱ｄと、この外乱ｄに対して評価量算出器４９が算出した式（２９）の衝突評価量Ｃ_ｄを示す。外乱ｄが閾値ＴＨに達して衝突が検知される時間と比較すると、本実施形態の衝突評価量Ｃ_ｄを使用することで、検知時間が大幅に短縮され、早期に安定して衝突を検知することができる。

図３０の例では、固有振動の第１回目の波動で衝突評価量Ｃ_ｄが閾値ＴＨに達して衝突が検知されるように閾値ＴＨの値を設定している。これに対して図３１に示すように、固有振動の２回目以降の波動で衝突評価量Ｃ_ｄが閾値ＴＨに達するように閾値ＴＨを変更しても、外乱ｄを使用するよりも早期に衝突を検知可能である。図３０及び図３１に示すように、衝突評価量Ｃ_ｄは常に外乱ｄよりも大きく、衝突評価量Ｃ_ｄを使用すれば外乱ｄを使用するよりも常に早期に衝突を検知できる。

（第１２実施形態）
図３２は、第１２実施形態に係る衝突検知装置を示す。この第１２実施形態では、上記第１１実施形態の衝突評価量Ｃ_ｄが閾値ＴＨに達するまでに要する閾値到達予測時間Ｔ_ｐから衝突を検知する。

衝突検知装置は、第１１実施形態の評価量算出器４９、予測時間算出器５９、速度算出器６０、及び予測時間算出器５９が算出した閾値到達予測時間Ｔ_ｐを閾値と比較して衝突を判定するコントローラ１により構成される。

評価量算出器４９の算出する衝突評価量Ｃ_ｄと、評価量算出器４９が備える絶対値算出器５６の算出する絶対値Ａ_ｄｏが予測時間算出器５９に入力される。また、評価量算出器４９が備える減算器５５の算出する外乱成分ｄ_ｏと、速度算出器６０の算出する外乱成分ｄ_ｏの変化速度ｄ_ｏ’が予測時間算出器５９に入力される。

予測時間算出器５９は、固有振動周期以外の外乱成分ｄ_ｏ、その変化速度ｄ_ｏ’、及び外乱成分ｄ_ｏの絶対値Ａ_ｄｏを用いて、Ｃ_ｄが上限閾値ｄ_ｕｌｍｔ又は下限閾値ｄ_ｌｌｍｔに到達するまでに要する時間の予測値である閾値到達予測時間Ｔ_ｐを以下の式（３０）により算出する。なお、外乱成分ｄ_ｏとその変化速度ｄ_ｏ’に代えて、外乱成分ｄ_ｏの推定値ｄ_ｏ＾と変化速度ｄ_ｏ’の推定値ｄｏ’＾を使用してもよい。

閾値到達予測時間Ｔ_ｐはコントローラ１に出力され、所定の時間閾値Ｔ_lmtと比較される。以下の式（３１）に示すように閾値到達予測時間Ｔ_ｐが時間閾値Ｔ_lmt以下となると、衝突検知であると判断される。

図３３は、固有振動数で振動する外乱とランプ状の外乱が同時に発生し、かつ固有振動成分が減衰しない場合の外乱ｄと、この外乱ｄに対して評価量算出器４９が算出した衝突評価量Ｃ_ｄ、及び外乱ｄに対する式（２６）の外乱成分ｄ_ｏを示す。また、図３３には、式（３０）を使用した算出した衝突評価量Ｃ_ｄの閾値到達予測時間Ｔ_ｐと、式（２１）の外乱ｄと外乱変化速度ｄ’をそれぞれ外乱成分ｄ_ｏとその変化速度ｄ_ｏ’に置き換えて算出した外乱成分ｄ_ｏの閾値到達予測時間Ｔｐとを示す。さらに、図３４は、図３３の外乱に対する、外乱成分ｄｏの変化速度ｄｏ＾、外乱パワーの平方根√Ｐ_ｄ、及び衝突評価量Ｃ_ｄを概略的に示す。

図３３及び図３４から明らかなように、外乱パワーの平方根√Ｐ_ｄを使用すると、閾値到達予測時間Ｔ_ｐが一定値となり衝突を検知できないのに対し、外乱成分ｄ０や衝突評価量Ｃ_ｄを使用して閾値到達時間Ｔ_ｐを算出することで、衝突を検知することができる。外乱成分ｄ_ｏとその変化速度ｄ_ｏ＾を使用して算出した閾値到達予測時間Ｔ_ｐで衝突を検知する場合と比較すると、衝突評価量Ｃ_ｄを使用して算出した閾値到達予測時間Ｔ_ｐで衝突を検知することにより、外乱の固有振動成分が減衰しない場合でも早期に衝突を検知することができる。

Ｃ_ｄ−Ａ_ｄｏ＝√Ｐ_ｄｎであるから、式（３０）は以下の式（３１）’に変形できる。

第９実施形態の変形例と同様に、この式（３１）’の両辺にａｂｓ(ｄｏ’)を乗じると、以下の式（３２）が得られる。

式（３１）を使用すれば、衝突評価量Ｃ_ｄを算出することなく、衝突を検知することができる。また、式（３２）を使用すれば、閾値到達予測時間Ｔ_ｐを算出することなく、直接的に衝突を検知することができる。

（第１３実施形態）
第１２実施形態では、特定周波数（固有周波数成分）を除去した外乱成分である式（２６）又は式（２７）で算出される外乱成分ｄ_ｏとその変化速度ｄ_ｏ’とを使用して閾値到達予測時間Ｔ_ｐを算出している。しかし、外乱成分ｄ_ｏには特定周波数成分以外の高周波成分が含まれているので、閾値到達予測時間Ｔ_ｏは変化速度ｄ_ｏ’の高周波成分により誤差が生じる。そのため外乱成分ｄ_ｏの変化速度ｄ_ｏ’に高周波成分が含まれる場合、その高周波成分を除去した推定値ｄ_ｏ’＾を用いて閾値到達予測時間Ｔ_ｐを算出した方が安定した衝突検知が可能である。

図３５は、図３３と同一の外乱ｄに対する特定周波数と高周波の両成分を除去した推定値ｄｌ＾と、第１２実施形態の式（３０）で外乱成分ｄ_ｏとその変化速度ｄ_ｏ’に代えて、それぞれ推定値ｄｌ＾と、その変化速度ｄｌ’＾を使用して算出した閾値到達予測時間Ｔ_ｐを示す。図３３と図３５を比較すると、現時点での閾値到達予測時間Ｔ_ｐは同じであるが、図３３の外乱成分ｄ_ｏが衝突直後に大きく変化しているのに対して、図３５の推定値ｄｌ＾はランプ状に変化する外乱の特性を良く示しており、衝突直後からより安定した検知が可能となる。

従って、閾値到達予測時間Ｔ_ｐによる衝突検知を行う上では、高周波成分は外乱成分ｄ_ｏや推定値ｄｌ＾にではなく、外乱成分ｄ_ｎに含まれる方が好ましい。外乱成分ｄｎを以下の（３２）’式により算出すれば、外乱成分ｄｎに特定周波数成分に加え高周波成分も含めることができる。

次に、上記特定周波数と高周波の両成分を除去した推定値ｄｌ＾の算出方法について具体的に説明する。図３６は、ランプ状に変化する成分が含まれる外乱ｄに対して、単純に低域通過で高周波除去のフィルタ特性をもつ外乱推定オブザーバ（式（９））によって算出した推定値ｄ＾を示す。図３６から明らかなように推定値ｄ＾はｄに対して式（９）の収束字定数Ｔ_ｄだけ位相が遅れてしまい、衝突検知の判断が遅れることになる。これは、式（９）の外乱オブザーバが外乱変化速度ｄ’＝０と仮定し、外乱ｄをオフセット外乱と仮定して定式化されているためである。これに対して本実施形態では、外乱ｄをランプ状外乱として扱うために、外乱ｄの変化加速度ｄ”＝０と仮定してオブザーバを定式化することにより、以下の式（３３）のランプ状外乱推定オブザーバを構成する。

またｄ”＝０と仮定して定式化した場合、外乱変化速度を推定するランプ状外乱変化速度推定オブザーバは以下の式（３４）で与えられる。

式（３３），（３４）のオブザーバで得られる外乱の推定値ｄｌ＾と、外乱変化速度の推定値ｄｌ’＾を、例えば第１２実施形態の式（３０）において外乱成分ｄ_ｏ（又は推定値ｄ_ｏ＾）とその変化速度ｄ_ｏ’（又は推定値ｄ_ｏ＾）に代えて使用すれば、衝突検知の誤検知低減など、安定化に寄与することができる。

図３６に示すように、式（９）のオブザーバの推定値ｄ＾は外乱ｄに対して移送が遅れるのに対し、式（３３）のランプ状外乱推定オブザーバの推定値ｄｌ＾は速やかに外乱ｄと一致する。

ランプ状に変化する外乱は、衝突時以外にも、一定加速時の粘性摩擦による外乱などがあり、このような外乱に対して上記のランプ状外乱推定オブザーバを適用し、外乱相殺（外乱を打ち消すように外乱推定値をトルクに加算）を行えば、速やかに外乱の影響を除去し、高精度な制御が可能となる。

次に、第７、第１０、及び第１１実施形態の変形例について説明する。式（１８），（２３）に示されるバンドバスフィルタや式（２５），（２７）のノッチフィルタのパラメータΩをΩ＞ω_ｎあるいはω_ｆと設定すれば、ω_ｎ ^２／ΩからΩの間の周波数帯を通過させ、あるいは除去するフィルタとなり、Ω≦ω_ｎあるいはω_ｆと設定すれば、ωの周波数のみを通過させ、あるいは除去するフィルタとなる。固有振動周期やトルクリップル周期や角度伝達誤差周期に不確かさがある場合、Ω＞ω_ｎあるいはω_ｆと設定し、幅を持ってその周期を捕らえることができる。またその周期のみを通過あるいは除去させ、それ以外の周期の外乱を厳密に除去あるいは通過させるためには、Ω≦ω_ｎあるいはω_ｆと設定すればよい。

また、ロボットの固有振動周期はロボットの姿勢により変化し、トルクリップル周期や角度伝達誤差周期は、モータの回転速度によって変化する。式（１８），（２３），（２４）のフィルタにおいて、固有振動周期やトルクリップル周期や角度伝達誤差周期の変化に応じて、ω_ｎやω_ｆを変更することによって、固有振動周期やトルクリップル周期や角度伝達誤差周期の変化に対応することができる。このようにして得られる推定値ｄｎ＾，ｄｆ＾，ｄｃ＾は、当然、上述の衝突検知に使用することも可能であるが、例えばモータトルクにフィードバックすることによって、トルクリップルや角度伝達誤差の影響を相殺するために使用することもできる。

本発明の第１実施形態に係る衝突検知装置を示すブロック図である。 外乱算出器を示すブロック図である。 外乱と外乱パワーの時間変化を示すグラフである。 外乱推定オブザーバを示すブロック図である。 本発明の第２実施形態に係る外乱パワー平方根推定器を示すブロック図である。 外乱の推定値及び外乱パワーの推定値の平方根の時間変化を示すグラフである。 本発明の第３実施形態に係る衝突検知装置を示すブロック図である。 外乱絶対値算出器を示すブロック図である。 外乱の推定値及び外乱絶対値の推定値の時間変化を示すグラフである。 本発明の第４実施形態に係る衝突検知装置を示すブロック図である。 外乱変化速度絶対値算出器を示すブロック図である。 本発明の第５実施形態に係る衝突検知装置を示すブロック図である。 外乱、外乱パワーの平方根、外乱絶対値、及び絶対値和の時間変化を示すグラフである。 図１３の部分拡大図である。 本発明の第６実施形態に係る衝突検知装置を示すブロック図である。 外乱の推定値、外乱パワーの推定値、外乱絶対値の推定値、及び絶対値和の推定値の時間変化を示すグラフである。 外乱に含まれる固有振動成分が減衰しない場合の外乱パワー、外乱絶対値、及び絶対値和の時間変化を示すグラフである。 図１７の部分拡大図である。 外乱に含まれる固有振動成分が減衰しない場合の外乱パワーの推定値、外乱絶対値の推定値、及び絶対値和の推定値の時間変化を示すグラフである。 本発明の第７実施形態に係る衝突検知装置を示すブロック図である。 外乱成分算出器を示すブロック図である。 固有振動数の外乱成分パワー成分の平方根と外乱の時間変化を示すグラフである。 本発明の第８実施形態に係る衝突検知装置を示すブロック図である。 固有振動数の外乱パワー成分の推定値の平方根と外乱の推定値の時間変化を示すグラフである。 ランプ状に変化する外乱と閾値到達予測時間を示す概略的なグラフである。 本発明の第９実施形態に係る衝突検知装置を示すブロック図である。 第９実施形態の衝突検知方法を説明するためのフローチャートである。 周期的な外乱が付加された外乱とこの周期的な外乱を除去した外乱を示すグラフである。 本発明の第１１実施形態の衝突検知装置を示すブロック図である。 ランプ状の外乱と衝突評価量の時間変化を示すグラフである。 ランプ状の外乱に固有振動による外乱が加わった外乱と衝突評価量の時間変化を示すグラフである。 本発明の第１２実施形態に係る衝突検知装置を示すブロック図である。 ランプ状の外乱に固有振動による外乱が加わった外乱、衝突評価量、及び固有振動数の外乱成分を除去した外乱成分の時間変化を示すグラフである。 外乱パーの平方根、衝突評価量、及び固有振動数の外乱成分を除去した外乱成分の推定値の時間変化を示す概略的なグラフである。 ランプ状の外乱に固有振動による外乱が加わった外乱、衝突評価量、及び特定周波数と高周波の両成分を除去した推定値の時間変化を示すグラフである。 ランプ状の外乱、通常の外乱の推定値、及び特定周波数と高周波の両成分を除去した外乱の時間変化を示すグラフである。

符号の説明

１ コントローラ
２ 軌道計画
３ ロボット
４ エンコーダ
１１ 外乱パワー算出器
１２ 外乱算出器
１３ パワー算出器
１４ 演算器
１５，１６ 自乗演算器
１７ 加算器
１８ 外乱パワー平方根推定器
１９ 平方根算出器
２０ フィルタ
２２ 外乱絶対値推定器
２３ 外乱絶対値算出器
２４ 絶対値算出器
２５ フイルタ
２６ 外乱変化速度絶対値推定器
２７ 外乱変化速度絶対値算出器
２８ フィルタ
２９ 速度絶対値算出器
３０ 速度算出器
３１ 絶対値算出器
３３ 絶対値和算出器
３４ 乗算器
３５ 加算器
３７ 絶対値和推定器
３８ フィルタ
４１ 外乱成分算出器
４２ バンドパスフィルタ
４３ 外乱パワー成分算出器
４４ 外乱パワー成分推定器
４５ フィルタ
４６ 速度演算器
４７ 予測時間算出器
４９ 評価量算出器
５０ 外乱パワー平方根算出器
５１ 外乱成分絶対値算出器
５２ バンドパスフィルタ
５３ 平方根算出器
５４ 加算器
５５ 減算器
５６ 絶対値算出器
５７ 加算器
５９ 予測時間算出器
６０ 速度算出器

Claims (8)

1. モータによって駆動される被駆動体の衝突を検知する方法において、
   上記モータ又は上記被駆動体に加わる外乱を算出し、
   上記モータ又は上記被駆動体に加わる外乱の変化速度を算出し、
   上記算出された外乱と、上記算出された外乱の変化速度と、被駆動体の固有振動数の角速度とを用いて、上記被駆動体の衝突を検知するための衝突評価量として外乱パワーを以下の式により算出し、
   

   

   この式においてＰ _ｄ は外乱パワー、ｄは算出された外乱、ｄ’は算出された外乱の変化速度、ω _ｎ は被駆動体の固有振動数の角速度であることを特徴とする、衝突検知方法。
2. 上記算出された外乱から被駆動体の固有振動数成分を抜き出し、
   上記算出された外乱の変化速度から被駆動体の固有振動数成分を抜き出し、
   上記抜き出された被駆動体の固有振動数成分の外乱と、上記抜き出された被駆動体の固有振動数成分の外乱の変化速度を、上記外乱パワーの算出に使用することを特徴とする、請求項１に記載の衝突検知方法。
3. 上記算出された外乱から角度伝達誤差を含む周期的外乱に対応する周波数成分を除去し、
   上記算出された外乱の変化速度から角度伝達誤差を含む周期的外乱に対応する周波数成分を除去し、
   上記角度伝達誤差を含む周期的外乱に対応する周波数成分を除去した外乱と、上記角度伝達誤差を含む周期的外乱に対応する周波数成分を除去した外乱の変化速度を、上記外乱パワーの算出に使用することを特徴とする、請求項１に記載の衝突検知方法。
4. 上記衝突評価量に対して高周波除去特性を示すフィルタ処理を行い、
   上記フィルタ処理された衝突評価量を用いて上記被駆動体の衝突を検知することを特徴とする、請求項１に記載の衝突検知方法。
5. モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、
   上記モータ又は上記被駆動体に加わる外乱を算出する外乱算出手段と、
   上記モータ又は上記被駆動体に加わる外乱の変化速度を算出する外乱変化速度算出手段、
   上記算出された外乱と、上記算出された外乱の変化速度と、被駆動体の固有振動数の角速度とを用いて、上記被駆動体の衝突を検知するための衝突評価量として外乱パワーを以下の式により算出して上記被駆動体の衝突を検出し、
   

   

   この式においてＰ _ｄ は外乱パワー、ｄは算出された外乱、ｄ’は算出された外乱の変化速度、ω _ｎ は被駆動体の固有振動数の角速度である衝突評価量算出・評価手段と
   を備えることを特徴とする、衝突検知装置。
6. 上記衝突評価量算出・評価手段は、
   上記算出された外乱から被駆動体の固有振動数成分を抜き出し、
   上記算出された外乱の変化速度から被駆動体の固有振動数成分を抜き出し、
   上記抜き出された被駆動体の固有振動数成分の外乱と、上記抜き出された被駆動体の固有振動数成分の外乱の変化速度を、上記外乱パワーの算出に使用することを特徴とする、請求項５に記載の衝突検知装置。
7. 上記衝突評価量算出・評価手段は、
   上記算出された外乱から角度伝達誤差を含む周期的外乱に対応する周波数成分を除去し、
   上記算出された外乱の変化速度から角度伝達誤差を含む周期的外乱に対応する周波数成分を除去し、
   上記角度伝達誤差を含む周期的外乱に対応する周波数成分を除去した外乱と、上記角度伝達誤差を含む周期的外乱に対応する周波数成分を除去した外乱変化量の変化速度を、上記外乱パワーの算出に使用することを特徴とする、請求項５に記載の衝突検知装置。
8. 上記衝突評価量算出・評価手段は、
   上記衝突評価量に対して高周波除去特性を示すフィルタ処理を行い、
   上記フィルタ処理された衝突評価量を用いて上記被駆動体の衝突を検知することを特徴とする、請求項５に記載の衝突検知装置。

Images