(57)【要約】【課題】  ロボットなどのモータによって駆動される被駆動体と外界との衝突を、早期に、かつ安定して検知する。【解決手段】  衝突検知装置１６は、モータ又は被駆動体に加わる外乱を算出する外乱算出器１２、外乱算出器１２によって算出された外乱ｄの絶対値Ａｄを算出する絶対値算出器２４、及び絶対値算出器２４により算出された絶対値Ａｄを閾値と比較して衝突を判定するコントローラ１を備える。【選択図】図１
2005100145.tif 000002
【特許請求の範囲】
【請求項１】
モータによって駆動される被駆動体の衝突を検知する方法において、
上記モータ又は上記被駆動体に加わる外乱を算出又は推定し、
上記算出又は推定された外乱に、該外乱と同符号の値を乗じて非負値を得て、
上記得られた非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出することを特徴とする、衝突検知方法。
【請求項２】
上記衝突評価量に対して高周波除去特性を示すフィルタ処理を行い、
上記フィルタ処理された衝突評価量を用いて上記被駆動体の衝突を検知することを特徴とする、請求項１に記載の衝突検知方法。
【請求項３】
上記算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を抜き出し、
上記抜き出された特定周波数成分又は特定周波数成分を含む高周波成分の外乱を、上記非負値の算出に使用することを特徴とする、請求項１又は請求項２に記載の衝突検知方法。
【請求項４】
上記算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を除去し、
上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱を、上記非負値の算出に使用することを特徴とする、請求項１又は請求項２に記載の衝突検知方法。
【請求項５】
モータによって駆動される被駆動体の衝突を検知する方法において、
上記モータ又は上記被駆動体に加わる外乱の変化量又は変化速度を算出又は推定し、
上記算出又は推定された外乱変化量又は変化速度に、該変化量又は該変化速度と同符号の値を乗じて非負値を得て、
上記得られた非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出することを特徴とする、衝突検知方法。
【請求項６】
上記衝突評価量に対して高周波除去特性を示すフィルタ処理を行い、
上記フィルタ処理された衝突評価量を用いて上記被駆動体の衝突を検知することを特徴とする、請求項５に記載の衝突検知方法。
【請求項７】
上記算出又は推定された外乱変化量又は変化速度から特定周波数成分又は特定周波数成分を含む高周波成分を抜き出し、
上記抜き出された特定周波数成分又は特定周波数成分を含む高周波成分の外乱変化量又は変化速度を、上記非負値の算出に使用する、請求項５又は請求項６に記載の衝突検知方法。
【請求項８】
上記算出又は推定された外乱変化量又は変化速度から特定周波数成分又は特定周波数成分を含む高周波成分を除去し、
上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱変化量又は変化速度を、上記非負値の算出に使用することを特徴とする、請求項５又は請求項６に記載の衝突検知方法。
【請求項９】
モータによって駆動される被駆動体の衝突を検知する方法において、
上記モータ又は被駆動体に加わる外乱の変化量又は変化速度を算出又は推定し、
上記算出又は推定された外乱変化量又は変化速度の自乗値を算出し、
上記モータ又は上記被駆動体に加わる外乱を算出又は推定し、
上記算出又は推定された外乱の自乗値を算出し、
上記算出又は推定された外乱変化量又は変化速度の自乗値に係数を乗じたものと、上記算出又は推定された外乱の自乗値とを加算して外乱パワーを算出し、該外乱パワーに基づいて上記被駆動体の衝突を検知することを特徴とする、衝突検知方法。
【請求項１０】
上記外乱パワーに対して高周波除去特性を示すフィルタ処理を行い、
フィルタ処理された上記外乱パワーを用いて上記被駆動体の衝突を検知することを特徴とする、請求項９に記載の衝突検知方法。
【請求項１１】
上記算出又は推定された外乱と、上記算出又は推定された外乱変化量又は変化速度とから、それぞれ特定周波数成分又は特定周波数成分を含む高周波成分を抜き出し、
上記抜き出された特定周波数成分又は特定周波数成分を含む高周波成分の外乱と、上記抜き出された特定周波数成分又は特定周波数成分を含む高周波成分の外乱変化量又は変化速度とを、それぞれ上記自乗値の算出に使用することを特徴とする、請求項９又は請求項１０に記載の衝突検知方法。
【請求項１２】
上記算出又は推定された外乱と、上記算出又は推定された外乱変化量又は変化速度とから、それぞれ特定周波数成分又は特定周波数成分を含む高周波成分を除去し、
上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱と、上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱変化量又は変化速度とを、それぞれ上記自乗値の算出に使用することを特徴とする、請求項９又は請求項１０に記載の衝突検知方法。
【請求項１３】
モータによって駆動される被駆動体の衝突を検知する方法において、
上記モータ又は上記被駆動体に加わる外乱を算出又は推定し、
上記モータ又は上記被駆動体に加わる外乱の変化量又は変化速度を算出又は推定し、
上記算出又は推定された外乱と、上記算出又は推定された外乱の変化量又は変化速度とに基づいて、上記外乱が予め定められた閾値に到達するまでの時間又は予め定められた所定時間後の外乱を予測し、
上記予測した時間又は外乱を用いて、上記被駆動体の衝突を検知することを特徴とする、衝突検知方法。
【請求項１４】
上記算出又は推定された外乱と、上記算出又は推定された外乱変化量又は変化速度とから、それぞれ特定周波数成分又は特定周波数成分を含む高周波成分を除去し、
上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱と、上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱変化量又は変化速度とを、それぞれ上記外乱が予め定められた閾値に到達するまでの時間又は予め定められた所定時間後の外乱の算出に使用することを特徴とする、請求項１３に記載の衝突検知方法。
【請求項１５】
モータによって駆動される被駆動体の衝突を検知する方法において、
上記モータ又は上記被駆動体に加わる外乱を算出又は推定し、
上記算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を抜き出し、かつ同符号の値を乗じて第１の非負値を得て、
上記算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を除去し、かつ同符合の値を乗じて第２の非負値を得て、
上記第１及び第２の非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出することを特徴とする、衝突検知方法。
【請求項１６】
上記モータ又は被駆動体に加わる外乱の変化量又は変化速度を算出又は推定し、
上記算出又は推定された外乱変化量又は変化速度から特定周波数成分又は特定周波数成分を含む高周波成分を除去し、
上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱変化量又は変化速度と、上記衝突評価量とに基づいて、上記外乱が予め定められた閾値に到達するまでの時間又は予め定められた所定時間後の外乱を予測し、
上記予測した時間又は上記予測した所定時間後の外乱を用いて、上記被駆動体の衝突を検知することを特徴とする、請求項１５に記載の衝突検知方法。
【請求項１７】
モータによって駆動される被駆動体の衝突を検知する方法において、
上記モータ又は上記被駆動体に加わる外乱の変化加速度を０として外乱推定オブザーバ又は外乱変化速度推定オブザーバを構成し、
上記外乱推定オブザーバ又は外乱変化速度推定オブザーバによって外乱又は外乱変化速度を推定し、
上記推定された外乱又は外乱変化速度を用いて、上記被駆動体の衝突を検知することを特徴とする、衝突検知方法。
【請求項１８】
モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、
上記モータ又は上記被駆動体に加わる外乱を算出又は推定する外乱算出・推定手段と、
上記外乱算出・推定手段によって算出又は推定された外乱に、該外乱と同符号の値を乗じて非負値を算出する乗算手段と、
上記乗算手段により算出された非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出する評価量算出手段と
を備えることを特徴とする、衝突検出装置。
【請求項１９】
モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、
上記モータ又は上記被駆動体に加わる外乱の変化量又は変化速度を算出又は推定する外乱変化量算出・推定手段と、
上記外乱変化量算出・推定手段によって算出又は推定された外乱変化量又は変化速度に、該変化量又は該変化速度と同符号の値を乗じて非負値を算出する乗算手段と、
上記乗算手段により算出された非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出する評価量算出手段と
を備えることを特徴とする、衝突検知装置。
【請求項２０】
モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、
上記モータ又は被駆動体に加わる外乱の変化量又は変化速度を算出又は推定する外乱変化量算出・推定手段と、
上記外乱変化量算出・推定手段によって算出又は推定された外乱変化量又は変化速度の自乗値を算出する第１の自乗演算手段と、
上記モータ又は上記被駆動体に加わる外乱を算出又は推定する外乱算出・推定手段と、
上記外乱算出・推定手段によって算出又は推定された外乱の自乗値を算出する第２の自乗演算手段と、
上記第１の自乗演算手段の算出値に係数を乗じたものに、上記第２の自乗演算手段の算出値を加算して外乱パワーを算出する外乱パワー算出手段と、
上記外乱パワー算出手段によって算出された外乱パワーに基づいて、被駆動体の衝突の有無を判定する判定手段と
を備えることを特徴とする、衝突検知装置。
【請求項２１】
モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、
上記モータ又は上記被駆動体に加わる外乱を算出又は推定する外乱算出・推定手段と、
上記モータ又は上記被駆動体に加わる外乱の変化量又は変化速度を算出又は推定する外乱変化量算出・推定手段と、
上記外乱算出・推定手段によって算出又は推定された外乱と、上記外乱変化量算出・推定手段によって算出又は推定された外乱の変化量又は変化速度とに基づいて、上記外乱が予め定められた閾値に到達するまでの時間を予測する予測手段と、
上記予測手段によって予測された時間を用いて、上記被駆動体の衝突の有無を判定する判定手段と
を備えることを特徴とする、衝突検知装置。
【請求項２２】
モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、
上記モータ又は上記被駆動体に加わる外乱を算出又は推定する外乱算出・推定手段と、
上記外乱算出・推定手段によって算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を抜き出し、かつ同符号の値を乗じて第１の非負値を算出する、第１の非負値算出手段と、
上記外乱算出・推定手段によって算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を除去し、かつ同符合の値を乗じて第２の非負値を算出する、第２の非負値算出手段と、
上記第１及び第２の非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出する、衝突評価量算出手段と
を備えることを特徴とする、衝突検知装置。
【請求項２３】
モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、
上記モータ又は上記被駆動体に加わる外乱の変化加速度を０とする外乱推定オブザーバ又は外乱変化速度推定オブザーバを備え、
上記外乱推定オブザーバ又は外乱変化速度推定オブザーバによって推定された外乱又は外乱変化速度を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出する、衝突評価量算出手段と
を備えることを特徴とする、衝突検知装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、溶接ロボットを含む各種ロボットのようなモータが被駆動体を駆動するモータ駆動装置において、被駆動体と外界との衝突を検知するための衝突検知方法及び装置に関する。
【背景技術】
【０００２】
従来、モータ駆動装置の被駆動体の外界に対する衝突を検知するための種々の方法が提案されている。
【０００３】
例えば、特許文献１には、モータ速度とモータトルクから外乱を推定し、外乱推定値がある閾値を越えたことによって、衝突を検知することが記載されている。また、特許文献２には、外乱推定値とモータ速度とを用いて衝突を検知することが記載されている。さらに、特許文献３には、外乱推定値の微分がある閾値を越えたことによって衝突を検知することが記載されている。さらにまた、特許文献４には、外乱推定値と外乱推定値の微分値とを用いて、衝突を検知することが記載されている。上記特許文献１〜４の他にも、外乱を用いた衝突検知方法が種々の文献に記載されている（例えば、特許文献５，６参照）。
【特許文献１】特開平３−１９６３１３号公報
【特許文献２】特開平９−３０５２３５号公報
【特許文献３】特開平１１−２８２５４０号公報
【特許文献４】特開平１１−７０４９０号公報
【特許文献５】特開２００１−３７２８９号公報
【特許文献６】特開２００２−２８３２７６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかし、上記特許文献１に記載の外乱推定値を用いる方法を、減速機などを介してモータが被駆動体を駆動するモータ駆動装置に適用した場合、減速機あるいは被駆動体そのものの固有振動によって外乱が変動するので、衝突検知が遅れてしまう。
【０００５】
また、モータ速度が０、すなわち被駆動体が停止状態にあっても、外界が動くことで被駆動体に衝突する場合がある。しかし、上記特許文献２に記載の外乱推定値とモータ速度を用いる方法では、この種の衝突を検知することができない。
【０００６】
さらに、被駆動体と外界とが徐々に近づいて接触し、被駆動体が外界に対して徐々に押し付けられてゆくような衝突は、特許文献３に記載の外乱推定値の微分値を使用する方法では検知することができない。この種の衝突であっても、外乱が徐々に増大して、装置が破壊する領域に達することがある。
【０００７】
特許文献４に記載の方法は外乱推定値と外乱推定値の微分値を使用するので、外乱推定値のみを使用する特許文献１に記載の方法の場合に問題となる固有振動に起因する衝突検知時間の遅れは解消される。しかし、外乱そのものには大きなノイズが含まれており、外乱変化量ではさらにノイズが増幅されるため、十分な平滑化が必要である。一方、従来知られている通常の平滑化を使用したのでは、平滑化により生じる外乱変化量の位相の遅れにより、検知が遅れ、衝突の早期検知を達成することができない。
【０００８】
特許文献５，６に記載されているような外乱を使用する他の衝突検知方法も、上記特許文献１に記載の方法と同様に、固有振動による外乱変動に起因する衝突検知時間の遅れの問題がある。
【０００９】
外乱は、ロボットやモータなどの力学モデルと実機との差異をトルクあるいは力などで表現したものであるが、ロボットやモータの力学モデルが少なくとも速度の１階微分方程式で記述されるため、外乱そのものには速度の１階微分（すなわち加速度）が含まれ、非常に多くのノイズを含む。外乱速度ではさらに多くのノイズを含む。これらのノイズを除去するために、上記種々の特許文献に記載の従来技術では、オブザーバやフィルタなどの平滑化処理が採用されている。しかし、位相遅れが小さく（検知時間を短くするためには位相遅れを小さくする必要がある）、かつ十分なノイズ除去特性を与えている（誤検知なくすため、さらには閾値を小さくして検知時間を短くするためにはノイズ除去が必要）ものはない。
【００１０】
そこで、本発明は、ロボットなどのモータによって駆動される被駆動体と外界との衝突を、早期に、かつ安定して検知する方法及び装置を提供することを課題とする。
【課題を解決するための手段】
【００１１】
第１の発明は、モータによって駆動される被駆動体の衝突を検知する方法において、上記モータ又は上記被駆動体に加わる外乱を算出又は推定し、上記算出又は推定された外乱に、該外乱と同符号の値を乗じて非負値を得て、上記得られた非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出することを特徴とする、衝突検知方法を提供する。
【００１２】
第１の発明において、上記衝突評価量に対して高周波除去特性を示すフィルタ処理を行い、上記フィルタ処理された衝突評価量を用いて上記被駆動体の衝突を検知してもよい。
【００１３】
また、上記算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を抜き出し、上記抜き出された特定周波数成分又は特定周波数成分を含む高周波成分の外乱を、上記非負値の算出に使用してもよい。
【００１４】
さらに、上記算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を除去し、上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱を、上記非負値の算出に使用してもよい。
【００１５】
第２の発明は、モータによって駆動される被駆動体の衝突を検知する方法において、上記モータ又は上記被駆動体に加わる外乱の変化量又は変化速度を算出又は推定し、上記算出又は推定された外乱変化量又は変化速度に、該変化量又は該変化速度と同符号の値を乗じて非負値を得て、上記得られた非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出することを特徴とする、衝突検知方法を提供する。
【００１６】
第２の発明において、上記衝突評価量に対して高周波除去特性を示すフィルタ処理を行い、上記フィルタ処理された衝突評価量を用いて上記被駆動体の衝突を検知してもよい。
【００１７】
また、上記算出又は推定された外乱変化量又は変化速度から特定周波数成分又は特定周波数成分を含む高周波成分を抜き出し、上記抜き出された特定周波数成分又は特定周波数成分を含む高周波成分の外乱変化量又は変化速度を、上記非負値の算出に使用してもよい。
【００１８】
さらに、上記算出又は推定された外乱変化量又は変化速度から特定周波数成分又は特定周波数成分を含む高周波成分を除去し、上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱変化量又は変化速度を、上記非負値の算出に使用してもよい。
【００１９】
第３の発明は、モータによって駆動される被駆動体の衝突を検知する方法において、上記モータ又は被駆動体に加わる外乱の変化量又は変化速度を算出又は推定し、上記算出又は推定された外乱変化量又は変化速度の自乗値を算出し、上記モータ又は上記被駆動体に加わる外乱を算出又は推定し、上記算出又は推定された外乱の自乗値を算出し、上記算出又は推定された外乱変化量又は変化速度の自乗値に係数を乗じたものと、上記算出又は推定された外乱の自乗値とを加算して外乱パワーを算出し、該外乱パワーに基づいて上記被駆動体の衝突を検知することを特徴とする、衝突検知方法を提供する。
【００２０】
第３の発明において、上記外乱パワーに対して高周波除去特性を示すフィルタ処理を行い、フィルタ処理された上記外乱パワーを用いて上記被駆動体の衝突を検知してもよい。
【００２１】
また、上記算出又は推定された外乱と、上記算出又は推定された外乱変化量又は変化速度とから、それぞれ特定周波数成分又は特定周波数成分を含む高周波成分を抜き出し、上記抜き出された特定周波数成分又は特定周波数成分を含む高周波成分の外乱と、上記抜き出された特定周波数成分又は特定周波数成分を含む高周波成分の外乱変化量又は変化速度とを、それぞれ上記自乗値の算出に使用してもよい。
【００２２】
さらに、上記算出又は推定された外乱と、上記算出又は推定された外乱変化量又は変化速度とから、それぞれ特定周波数成分又は特定周波数成分を含む高周波成分を除去し、上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱と、上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱変化量又は変化速度とを、それぞれ上記自乗値の算出に使用してもよい。
【００２３】
第４の発明は、モータによって駆動される被駆動体の衝突を検知する方法において、上記モータ又は上記被駆動体に加わる外乱を算出又は推定し、上記モータ又は上記被駆動体に加わる外乱の変化量又は変化速度を算出又は推定し、上記算出又は推定された外乱と、上記算出又は推定された外乱の変化量又は変化速度とに基づいて、上記外乱が予め定められた閾値に到達するまでの時間又は予め定められた所定時間後の外乱を予測し、上記予測した時間又は外乱を用いて、上記被駆動体の衝突を検知することを特徴とする、衝突検知方法を提供する。
【００２４】
第４の発明において、上記算出又は推定された外乱と、上記算出又は推定された外乱変化量又は変化速度とから、それぞれ特定周波数成分又は特定周波数成分を含む高周波成分を除去し、上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱と、上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱変化量又は変化速度とを、それぞれ上記外乱が予め定められた閾値に到達するまでの時間又は予め定められた所定時間後の外乱の算出に使用してもよい。
【００２５】
第５の発明は、モータによって駆動される被駆動体の衝突を検知する方法において、上記モータ又は上記被駆動体に加わる外乱を算出又は推定し、上記算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を抜き出し、かつ同符号の値を乗じて第１の非負値を得て、上記算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を除去し、かつ同符合の値を乗じて第２の非負値を得て、上記第１及び第２の非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出することを特徴とする、衝突検知方法を提供する。
【００２６】
第５の発明において、上記モータ又は被駆動体に加わる外乱の変化量又は変化速度を算出又は推定し、上記算出又は推定された外乱変化量又は変化速度から特定周波数成分又は特定周波数成分を含む高周波成分を除去し、上記特定周波数成分又は特定周波数成分を含む高周波成分を除去した外乱変化量又は変化速度と、上記衝突評価量とに基づいて、上記外乱が予め定められた閾値に到達するまでの時間又は予め定められた所定時間後の外乱を予測し、上記予測した時間又は上記予測した所定時間後の外乱を用いて、上記被駆動体の衝突を検知してもよい。
【００２７】
第６の発明は、モータによって駆動される被駆動体の衝突を検知する方法において、上記モータ又は上記被駆動体に加わる外乱の変化加速度を０として外乱推定オブザーバ又は外乱変化速度推定オブザーバを構成し、上記外乱推定オブザーバ又は外乱変化速度推定オブザーバによって外乱又は外乱変化速度を推定し、上記推定された外乱又は外乱変化速度を用いて、上記被駆動体の衝突を検知することを特徴とする、衝突検知方法を提供する。
【００２８】
第７の発明は、モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、上記モータ又は上記被駆動体に加わる外乱を算出又は推定する外乱算出・推定手段と、上記外乱算出・推定手段によって算出又は推定された外乱に、該外乱と同符号の値を乗じて非負値を算出する乗算手段と、上記乗算手段により算出された非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出する評価量算出手段とを備えることを特徴とする、衝突検出装置を提供する。
【００２９】
第８の発明は、モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、上記モータ又は上記被駆動体に加わる外乱の変化量又は変化速度を算出又は推定する外乱変化量算出・推定手段と、上記外乱変化量算出・推定手段によって算出又は推定された外乱変化量又は変化速度に、該変化量又は該変化速度と同符号の値を乗じて非負値を算出する乗算手段と、上記乗算手段により算出された非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出する評価量算出手段とを備えることを特徴とする、衝突検知装置を提供する。
【００３０】
第９の発明は、モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、上記モータ又は被駆動体に加わる外乱の変化量又は変化速度を算出又は推定する外乱変化量算出・推定手段と、上記外乱変化量算出・推定手段によって算出又は推定された外乱変化量又は変化速度の自乗値を算出する第１の自乗演算手段と、上記モータ又は上記被駆動体に加わる外乱を算出又は推定する外乱算出・推定手段と、上記外乱算出・推定手段によって算出又は推定された外乱の自乗値を算出する第２の自乗演算手段と、上記第１の自乗演算手段の算出値に係数を乗じたものに、上記第２の自乗演算手段の算出値を加算して外乱パワーを算出する外乱パワー算出手段と、上記外乱パワー算出手段によって算出された外乱パワーに基づいて、被駆動体の衝突の有無を判定する判定手段とを備えることを特徴とする、衝突検知装置を提供する。
【００３１】
第１０の発明は、モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、上記モータ又は上記被駆動体に加わる外乱を算出又は推定する外乱算出・推定手段と、上記モータ又は上記被駆動体に加わる外乱の変化量又は変化速度を算出又は推定する外乱変化量算出・推定手段と、上記外乱算出・推定手段によって算出又は推定された外乱と、上記外乱変化量算出・推定手段によって算出又は推定された外乱の変化量又は変化速度とに基づいて、上記外乱が予め定められた閾値に到達するまでの時間を予測する予測手段と、上記予測手段によって予測された時間を用いて、上記被駆動体の衝突の有無を判定する判定手段とを備えることを特徴とする、衝突検知装置を提供する。
【００３２】
第１１の発明は、モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、上記モータ又は上記被駆動体に加わる外乱を算出又は推定する外乱算出・推定手段と、上記外乱算出・推定手段によって算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を抜き出し、かつ同符号の値を乗じて第１の非負値を算出する、第１の非負値算出手段と、上記外乱算出・推定手段によって算出又は推定された外乱から特定周波数成分又は特定周波数成分を含む高周波成分を除去し、かつ同符合の値を乗じて第２の非負値を算出する、第２の非負値算出手段と、上記第１及び第２の非負値を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出する、衝突評価量算出手段とを備えることを特徴とする、衝突検知装置を提供する。
【００３３】
第１２の発明は、モータによって駆動される被駆動体の衝突を検知する衝突検知装置において、上記モータ又は上記被駆動体に加わる外乱の変化加速度を０とする外乱推定オブザーバ又は外乱変化速度推定オブザーバを備え、上記外乱推定オブザーバ又は外乱変化速度推定オブザーバによって推定された外乱又は外乱変化速度を用いて、上記被駆動体の衝突を検知するための衝突評価量を算出する、衝突評価量算出手段とを備えることを特徴とする、衝突検知装置を提供する。
【発明の効果】
【００３４】
本発明は、外乱に同符合の値を乗じて得た非負値、外乱の変化量又は変化速度に同符合の値を乗じて得た非負値、外乱パワー、外乱が閾値に到達するまでの時間、又は外乱の特定周波成分の自乗と特定周波数成分を除去した外乱の自乗から算出した衝突評価量を用いて被駆動体の衝突を検知するので、ロボットなどのモータによって駆動される被駆動体と外界との衝突を、早期に、かつ安定して検知することができる。
【発明を実施するための最良の形態】
【００３５】
次に、添付図面を参照して本発明の実施の形態を詳細に説明する。各図面において、同一の要素には同一の符号を付している。
【００３６】
まず、モータで駆動される被駆動体の一種であるロボットを例に外乱について説明する。ロボットの力学モデルは一般に以下の式で表される。
【００３７】
【数１】
2005100145.tif 000003
【００３８】
ここで、Θはロボットの関節角度ベクトル（モータ回転角度ベクトルと１対１に比例）、Ｊ(Θ)はロボットの慣性行列、Ｃ(dΘ/dt,Θ)は重力、遠心コリオリ力、摩擦などを含むベクトル、Τはトルクベクトル、Ｄが外乱ベクトルである。式（１）から外乱ベクトルＤは以下の式（２）で算出することができる。
【００３９】
【数２】
2005100145.tif 000004
【００４０】
モータ制御の分野では一般的に単軸毎に外乱が導出されることが多い。今、ロボットのある軸に注目して式（１），（２）式を書き下せば、以下の式（３），（４）が得られる。
【００４１】
【数３】
2005100145.tif 000005
【００４２】
【数４】
2005100145.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,Θ)を用いて、式（４）から外乱ｄが算出される。
【００４５】
ロボットがワーク（外界）と接触し、弾き飛ばし、あるいは破損させた場合、図３に示すような外乱ｄが発生する。外乱ｄが振動しているのは、接触によってワークから受ける反力によりロボットの固有振動が励起されたためである。このようにワークを弾き飛ばす、あるいは破損させた場合、固有振動によって振動している外乱を適正に判断し、ロボットは速やかに停止する必要がある。上述の特許文献１に記載の方法のように、外乱に対して閾値を設けて外乱そのもので衝突を検知しようとしても、振動する外乱を適正に判断することができない。実際、図３において外乱ｄは閾値ＴＨを超えておらず、衝突とは判断されない。
【００４６】
（第１実施形態）
図１及び図２は、本発明の第１実施形態に係る衝突検知装置を示す。この第１実施形態は、外乱ｄの代わりに外乱パワーＰｄを衝突評価量として衝突を検知する。
【００４７】
図１を参照すると、コントローラ１は軌道計画２を実現するための指令値としてトルクτをモータ駆動装置であるロボット３に出力し、それに従ってロボット３のモータ（図示せず）が駆動されて被駆動体であるアームやリスト（図示せず）が移動する。エンコーダ４により検出された各軸の関節角度θはコントローラ１にフィードバックされる。
【００４８】
本実施形態の衝突検知装置は、外乱パワー算出器１１とコントローラ１により構成される。外乱パワー算出器１１は、コントローラ１から入力されるトルクτと、エンコーダ４から入力される関節角度θとから、外乱パワーＰｄを算出する。外乱パワーＰｄはコントローラ１に出力され、コントローラ１に予め記憶された閾値と比較される。
【００４９】
外乱パワー算出器１１は、外乱算出器１２と、パワー算出器１３とを備える。図２に示すように、外乱算出器１２は、コントローラ１から入力されるトルクτ、ロボット３のエンコーダ４から入力される関節角度θ、予め導出されたｊ(Θ)、及び予め導出されたｃ(d２Θother/dt２,dΘ/dt,Θ)とにより、上記式（４）から外乱ｄを時々刻々と算出する。外乱算出器１２により算出された外乱ｄはパワー算出器１３に入力される。パワー算出器１３は、演算器１４、第１及び第２の自乗演算器１５，１６、及び加算器１７を備え、外乱ｄから外乱パワーＰｄを算出する。
【００５０】
外乱パワー算出器１１で算出される外乱パワーＰｄは以下の式（５）で定義される。
【００５１】
【数５】
2005100145.tif 000007
【００５２】
ここでｄ’は外乱変化速度、ωｎはロボットの固有振動の角速度である。
【００５３】
外乱ｄが固有振動数で振動している場合の外乱ｄとその変化速度ｄ’は、それぞれ以下の式（６），（７）で表される。Ａは振幅である。
【００５４】
【数６】
2005100145.tif 000008
【００５５】
【数７】
2005100145.tif 000009
【００５６】
式（６），（７）から振幅の自乗Ａ２は以下の式（８）で表される。
【００５７】
【数８】
2005100145.tif 000010
【００５８】
式（８）と式（５）から明らかなように、外乱パワーＰｄは外乱ｄに含まれる固有振動成分を考慮した衝突評価量である。また、式（５）において外乱変化速度の自乗ｄ'２に乗じる係数１/ωｎ２は、式（８）から得られた。ただし、式（５）において外乱変化速度の自乗ｄ'２に乗じる係数は、１/ωｎ２に限定されず適宜設定することができる。
【００５９】
次に、図１の衝突検知装置によって実行される衝突検知方法を説明する。まず、外乱算出器１２はコントローラ１から入力されるトルクτとエンコーダ４から入力される関節角度θとから、外乱ｄを時々刻々と算出する。次に、外乱算出器１２から入力された外乱ｄから、パワー算出器１３の第１の自乗演算器１５が外乱ｄの自乗値ｄ２を算出する。また、演算器１４が外乱算出器１２から入力された外乱ｄから外乱変化速度ｄ’を算出し、さらにこの外乱変化速度ｄ’に係数１/ωｎを乗じた値ｄ'×（１/ωｎ）を算出する。次に、第２の自乗演算器１６が自乗値ｄ'２×（１/ωｎ２）を算出する。加算器１７は自乗値ｄ２と自乗値ｄ'２×（１/ωｎ２）を加算して、外乱パワーＰｄを算出する。時々刻々と算出される外乱パワーＰｄはコントローラ１に出力され、閾値と比較される。コントローラ１は外乱パワーＰｄが閾値以上となると衝突であると判断し、ロボット３の動作停止等の必要な制御を実行する。
【００６０】
図３の実線は、外乱算出器１２により算出される外乱ｄと、外乱パワーＰｄの平方根√Ｐｄとを示している。外乱パワーＰｄの平方根をとっているのは、外乱ｄと次元を揃えて比較するためであり、コントローラ１による閾値判定の際に外乱パワーＰｄの平方根をとる必要はなく、図３における閾値ＴＨの自乗値ＴＨ２を外乱パワーＰｄの閾値として使用すればよい。図３では、外乱ｄは閾値ＴＨを下回っているが、外乱パワーＰｄの平方根は閾値ＴＨを上回っている。従って、外乱パワーＰｄを衝突評価量として用いることにより、振動する外乱ｄを適性に処理し、外乱ｄを衝突評価量とした場合には検知できない衝突を早期に検知できる。
【００６１】
（第２実施形態）
上記式（２），（４）では、加速度やトルクを用いて外乱ｄを算出しているので、算出された外乱ｄには多くのノイズが含まれる。このノイズの影響を除去して安定した衝突検知を実現するために、外乱オブザーバなどを使用した高周波除去特性を示すフィルタ処理でノイズ除去をすることが従来から知られている。この種の従来の外乱オブザーバでは、外乱ｄを一定値外乱（ｄの時間微分値が０）と仮定して構成しており、このようにして構成された外乱オブザーバの外乱推定値ｄ＾の動特性は以下の式（９）で与えられ、外乱ｄに対して１次遅れ特性を示す（図４参照）。
【００６２】
【数９】
2005100145.tif 000011
【００６３】
ここでｓはラプラス演算子、ｄ＾(ｓ)とｄ(ｓ)はそれぞれ外乱推定値ｄ＾と外乱ｄのラブラス変換、Ｔdは外乱推定値の収束時定数である。
【００６４】
これに対し、本実施形態では、外乱パワーＰｄやその平方根√Ｐｄにフィルタ処理を行う。外乱パワーＰｄに式（９）と同様のフイルタ処理を施すと、以下の式（１０）により外乱パワーＰｄの推定値Ｐｄ＾が得られる。
【００６５】
【数１０】
2005100145.tif 000012
【００６６】
同様に、外乱パワーの平方根√Ｐｄに式（９）と同様のフィルタ処理を施すと、以下の式（１１）により外乱パワーＰｄの平方根の推定値√Ｐｄ＾が得られる。
【００６７】
【数１１】
2005100145.tif 000013
【００６８】
これらフィルタリングにより得られた推定値Ｐｄ＾，√Ｐｄ＾が衝突評価量として使用される。
【００６９】
図５は、外乱パワーの平方根√Ｐｄに対して式（１１）のフイルタ処理を施すようにした衝突検知装置を示し、外乱パワー平方根推定器１８とコントローラ１により構成される。外乱パワー平方根推定器１８は、外乱パワー算出器１１、平方根算出器１９、及びフィルタ２０を備えている。外乱パワー算出器１１は、トルクτと関節角度θから式（５）によって時々刻々と外乱パワーＰｄを算出する。平方根算出器１９は外乱パワー算出器１１から入力される外乱パワーＰｄの平方根を算出する。算出された平方根√Ｐｄに対してフィルタ２０によって式（１１）のフィルタ処理が施され、外乱パワーの平方根の推定値√Ｐｄ＾が算出される。この推定値√Ｐｄ＾はコントローラ１に出力され、閾値と比較される。
【００７０】
図６は、図３に示す外乱ｄに対して式（９）のフィルタ特性を持つオブザーバを構成して得られる推定値ｄ＾と、図３に示す外乱ｄに対して外乱パワー平方根推定器１８で式（１１）のフィルタ処理を行うことで得られる外乱パワーの平方根の推定値√Ｐｄ＾とを示す。外乱パワーの平方根√Ｐｄに式（１１）のフィルタ処理を施すことにより、ノイズの影響を受けない安定した衝突検知が可能となり、衝突検知のための閾値ＴＨを下げることができる。
【００７１】
固有振動周波数成分からなる外乱ｄに式（９）のフィルタ処理を施した場合もノイズの影響が低減されるが、得られる外乱推定値ｄ＾の最大値はフィルタ処理によって大きく減衰する。具体的には、図６の外乱推定値ｄ＾の最大値は、図３に示す外乱ｄの最大値の約５０％に減衰している。そのため、図６では閾値ＴＨを図３の約０．５５倍に設定しているにもかかわらず、外乱推定値ｄ＾の最大値は閾値ＴＨを下回っており、衝突を検知できない。
【００７２】
これに対して、外乱パワーＰｄは固有振動成分などの高周波成分がほとんど含まず、より低い周波数成分から構成されているため、式（１１）のフィルタ処理による高周波除去特性の影響が大幅に緩和される。具体的には、図６に示す外乱パワーの平方根の推定値√Ｐｄ＾の最大値は、外乱パワーの平方根√Ｐｄの約７０％であり、閾値ＴＨを大きく上回っている。
【００７３】
このようにパワーＰｄには固有振動成分をほとんど含んでいないため、フィルタ処理による振幅減衰の影響が外乱ｄをフィルタ処理する場合と比較して大幅に緩和される。従って、フィルタ処理により得られる推定値√Ｐｄ＾，Ｐｄ＾は閾値を大きく上回り、外乱パワーＰｄそのものを衝突評価量とする場合よりも、さらに安定した衝突検知が可能となる。また、非負値とした上でフィルタ処理することによって、フィルタによる外乱の振幅低減を抑制しつつ、ノイズを除去することができる。
【００７４】
（第３実施形態）
図７及び図８は、第３実施形態に係る衝突検知装置を示す。この第３実施形態は、外乱絶対値Ａｄの推定値Ａｄ＾を衝突評価量として使用する。
【００７５】
衝突検知装置は、外乱絶対値Ａｄの推定値Ａｄ＾を算出する外乱絶対値推定器２２と、推定値Ａｄ＾を閾値と比較して衝突を判定するコントローラ１により構成される。外乱絶対値推定器２２は外乱絶対値算出器２３とフィルタ２５を備えている。また、外乱絶対値算出器２３は外乱算出器１２と、絶対値算出器２４とを備えている。
【００７６】
外乱算出器１２がトルクτと関節角度θから時々刻々と算出する外乱ｄは絶対値算出器２４に出力され、絶対値算出器２４は以下の式（１２）の外乱絶対値Ａｄを算出する。
【００７７】
【数１２】
2005100145.tif 000014
【００７８】
この式（１２）に示すように、外乱ｄと同符合の値としてｓｉｇｎ（ｄ）を外乱ｄに乗じることで、非負値である外乱絶対値Ａｄを算出している。
【００７９】
外乱絶対値Ａｄはフィルタ２５に出力され、以下の式（１３）のフィルタ処理により外乱絶対値Ａｄの推定値Ａｄ＾が算出される。
【００８０】
【数１３】
2005100145.tif 000015
【００８１】
推定値Ａｄ＾はコントローラ１に出力され、閾値と比較される。
【００８２】
図９は、図３に示す外乱ｄに対して式（９）のフィルタ特性を持つオブザーバを構成して得られる推定値ｄ＾と、図３に示す外乱ｄから外乱絶対値推定器２２が式（１２），（１３）で算出した外乱絶対値Ａｄの推定値Ａｄ＾を示す。閾値ＴＨは図６と同一に設定している。外乱推定値ｄ＾が閾値を上回らないため衝突を検知できないのに対して、推定値Ａｄ＾は閾値を上回り衝突を検知できる。図６の外乱パワーＰｄの推定値Ｐｄ＾が閾値ＴＨを大きく上回っているのと比較すると、図９において外乱絶対値Ａｄの推定値Ａｄ＾が閾値ＴＨを上回る程度は小さい。従って、外乱絶対値Ａｄの推定値Ａｄ＾を衝突評価量として使用すると、外乱パワーＰｄの推定値Ｐｄ＾を使用する場合よりも検知の安定性が少し劣るが、外乱ｄの推定値ｄ＾を使用する場合よりも安定した検知が可能である。
【００８３】
（第４実施形態）
図１０及び図１１は、第４実施形態に係る衝突検知装置を示す。この第４実施形態は、上記第３実施形態よりも検出精度を向上させて、第２実施形態のように外乱パワーＰｄの推定値Ｐｄ＾を使用する場合に近付けるために、外乱変化速度ｄ’の絶対値Ａｄ’の推定値Ａｄ’＾を衝突評価量として使用する。
【００８４】
衝突検知装置は、外乱変化速度ｄ’の絶対値Ａｄ’の推定値Ａｄ’＾を算出する外乱変化速度絶対値推定器２６と、推定値Ａｄ’＾を閾値と比較して衝突を判定するコントローラ１により構成される。外乱変化速度絶対値推定器２６は外乱変化速度絶対値算出器２７とフィルタ２８を備えている。また、外乱変化速度絶対値算出器２７は外乱算出器１２と速度絶対値算出器２９とを備えている。さらに、速度絶対値算出器２９は速度算出器３０と絶対値算出器３１とを備えている。
【００８５】
外乱算出器１２がトルクτと関節角度θから時々刻々と算出する外乱ｄは速度絶対値算出器２９に出力される。速度絶対値算出器２９の速度算出器３０が外乱変化速度ｄ’を算出し、絶対値算出器３１に算出する。絶対値算出器３１は以下の式（１４）の外乱変化速度ｄ’の絶対値Ａｄ’を算出する。
【００８６】
【数１４】
2005100145.tif 000016
【００８７】
この式（１４）に示すように、外乱変化速度ｄ’と同符号の値としてｓｉｇｎ（ｄ’）を外乱変化速度ｄ’に乗じることで、非負値である絶対値Ａｄ’を算出する。
【００８８】
外乱変化速度ｄ’の絶対値はＡｄ’フィルタ２８に出力され、以下の式（１５）のフィルタ処理により外乱変化速度ｄ’の絶対値Ａｄ’の推定値Ａｄ’＾が算出される。
【００８９】
【数１５】
2005100145.tif 000017
【００９０】
推定値Ａｄ’＾はコントローラ１に出力され、閾値と比較される。第２実施形態では外乱の非負値としてフィルタ処理するが、本実施形態では外乱速度の非負値としてフィルタ処理している。非負値とした上でフィルタ処理することにより、フィルタによって振幅減衰を抑制しつつ、ノイズを除去することができる。
【００９１】
なお、本実施形態において外乱変化速度ｄ’に変えて所定時間内の外乱の変化量を使用してもよい。
【００９２】
（第５実施形態）
図１２は、第５実施形態に係る衝突検知装置を示す。外乱変化速度絶対値Ａｄ’やその推定値Ａｄ’＾をそのまま衝突評価量として使用した場合、外乱ｄやその推定値ｄ＾を使用するよりも高精度の検知が可能であるが、第１実施形態や第２実施形態のように外乱パワーＰｄやその推定値Ｐｄ＾を使用する場合よりは検出精度は低い。第５実施形態では、以下の式（１６）で定義される絶対値和Ｂｄを衝突評価量として使用する。
【００９３】
衝突検知装置は、以下の式（１６）で定義される絶対値和Ｂｄを算出する絶対値和算出器３３と、絶対値和Ｂｄを閾値と比較して衝突を判定するコントローラ１により構成される。
【００９４】
【数１６】
2005100145.tif 000018
【００９５】
この式（１６）に示すように、絶対値和Ｂｄは式（１２）の外乱絶対値Ａｄと、式（１４）の外乱変化速度の絶対値Ａｄ’に係数１／ωｎを乗じたものの和である。
【００９６】
絶対値和算出器３３は、外乱算出器１２、絶対値算出器２４、速度絶対値算出器２９、乗算器３４、及び加算器３５を備えている。
【００９７】
外乱算出器１２がトルクτと関節角度θから時々刻々と算出する外乱ｄは、絶対値算出器２４と速度絶対値算出器２９に出力される。絶対値算出器２４は上述の式（１２）により外乱絶対値Ａｄを算出する。速度絶対値算出器２９は外乱変化速度ｄ’を算出し、さらに上述の式（１４）により外乱変化速度ｄ’の絶対値Ａｄ’を算出する。外乱変化速度ｄ’の絶対値Ａｄ’は乗算器３４に入力され１/ωｎが乗算される。外乱絶対値Ａｄと１/ωｎを乗じた絶対値Ａｄ’は加算器３５に出力され、式（１６）により絶対値和Ｂｄが算出される。絶対値和Ｂｄはコントローラ１に出力され、閾値と比較される。
【００９８】
図１３及び図１４は、図３に示す外乱ｄに対して絶対和算出器３３が式（１６）により算出した絶対値和Ｂｄを１／√２倍した値を、外乱ｄ、外乱絶対値Ａｄ、及び外乱パワーＰｄの平方根√Ｐｄと共に示す。絶対値和Ｂｄを１／√２倍しているのは、外乱ｄと大きさを揃えて比較するためであり、コントローラ１による閾値判定の際に絶対値和Ｂｄを１／√２倍する必要はなく、図３における閾値ＴＨの１／√２倍を絶対値和Ｂｄの閾値として使用すればよい。外乱ｄや外乱絶対値Ａｄが閾値ＴＨを上回らないのに対し、外乱パワーＰｄの平方根√Ｐｄと同様に絶対値和Ｂｄは閾値ＴＨを上回り衝突を検知できる。外乱パワーＰｄの平方根√Ｐｄと比較すると、絶対知和Ｂｄが閾値ＴＨを上回る程度は小さい。従って、絶対知和Ｂｄを衝突評価量として使用すると、外乱パワーＰｄを使用する場合よりも少し劣るが、外乱ｄや外乱絶対値Ａｄを使用する場合よりも安定性の高い検知が可能である。
【００９９】
（第６実施形態）
図１５は、第６実施形態に衝突検知装置を示す。第６実施形態では、絶対値和Ｂｄに対して以下の式（１７）のフィルタ処理を施して得られる絶対値和Ｂｄの推定値Ｂｄ＾を衝突評価量として使用する。
【０１００】
【数１７】
2005100145.tif 000019
【０１０１】
衝突検知装置は、第５実施形態の絶対値和算出器３３にさらにフィルタ３８を追加した絶対値和推定器３７と、絶対値和Ｂｄの推定値Ｂｄ＾を閾値と比較して衝突を判定するコントローラ１により構成される。
【０１０２】
絶対値和算出器３３はトルクτと関節角度θから式（１６）の絶対値和Ｂｄを時々刻々と算出し、フィルタ３８に出力する。フィルタ３８は式（１７）により推定値Ｂｄ＾を算出する。推定値Ｂｄ＾はコントローラ１に出力され、閾値と比較される。
【０１０３】
図１６は、図３に示す外乱ｄに対して絶対値和推定器３７が式（１７）により算出した絶対値和Ｂｄの推定値Ｂｄ＾を１／√２倍した値を、外乱推定値ｄ＾、外乱パワーＰｄの推定値Ｐｄ＾の平方根√Ｐｄ＾、及び外乱絶対値Ａｄの推定値Ａｄ＾と共に示す。絶対値和Ｂｄを１／√２倍しているのは、上述の図１３及び図１４と同様に外乱ｄと大きさを揃えて比較するためである。外乱推定値ｄ＾が閾値ＴＨを上回らず、外乱絶対値Ａｄの推定値Ａｄ＾がかろうじて閾値ＴＨを上回るのに対し、絶対値和Ｂｄの推定値Ｂｄ＾は外乱パワーＰｄの推定値Ｐｄ＾と同様に閾値ＴＨを大きく上回っている。従って、絶対値和Ｂｄの推定値Ｂｄ＾を衝突評価量として使用することで、高精度の検知が可能となる。
【０１０４】
図１７及び図１８は、図３の場合とは異なり外乱ｄに含まれる固有振動成分が減衰しない場合の外乱ｄ、外乱パワーＰｄの平方根√Ｐｄ、外乱絶対値Ａｄ、及び絶対値和Ｂｄを１／√２倍した値を示す。また、図１９は、この場合の外乱推定値ｄ＾、外乱パワーＰｄの推定値Ｐｄ＾、外乱絶対値Ａｄの推定値Ａｄ＾、及び絶対値和Ｂｄの推定値Ｂｄ＾を示す。外乱推定値ｄ＾が閾値ＴＨに達しないの対し、本実施形態の絶対値和Ｂｄの推定値Ｂｄ＾は閾値を大きく上回っている。従って、外乱ｄに含まれる固有振動が減衰しない場合でも、絶対値和Ｂｄの推定値Ｂｄ＾を衝突評価量として使用すれば、衝突を検知することができる。同様に、第２実施形態の外乱パワーＰｄの推定値Ｐｄ＾や、第３実施形態の外乱絶対値Ａｄの推定値Ａｄ＾を衝突評価量として使用すれば、外乱ｄに含まれる固有振動が減衰しない場合でも、衝突を検知することができる。
【０１０５】
（第７実施形態）
以上の説明では、外乱ｄや外乱変化速度ｄ’などが固有振動で振動し、その他の周波数成分が含まれない状況について示してきた。しかし、実際には外乱ｄや外乱変化速度ｄ’は、固有振動以外の他の周波数成分も含まれていることが多い。第１実施形態から第６実施形態を、固有振動以外の他の周波数成分も含まれる場合にも適用するためには、外乱ｄや外乱速度ｄ’から固有振動周波数の外乱成分ｄｎや固有振動周波数の外乱速度成分ｄｎ’を抜きだし、これらを外乱ｄや外乱ｄ’に代えて使用すればよい。
【０１０６】
図２０に示す第７実施形態では、第１実施形態の外乱パワーＰｄの算出において外乱ｄに代えて固有振動周波数の外乱成分ｄｎを使用する。具体的には、外乱パワー成分算出器４３は、図１に示す外乱パワー算出器１１の外乱算出器１２（図２参照）を、図２１に示す固有振動数の外乱成分ｄｎを算出する外乱成分算出器４１に置換した構成である。さらに具体的には、外乱成分算出器４１は、外乱算出器１２に固有振動周波数の外乱成分を抜き出すためのバンドパスフィルタ４２を追加した構成である。
【０１０７】
バンドパスフィルタ４２は、以下の式（１８）に示す固有振動（角速度ωｎ）を中心としたバンドパスフィルタ処理により、外乱ｄから外乱成分ｄｎを抜き出す。Ωはフィルタが通過させるバンド幅を指定するパラメータである。
【０１０８】
【数１８】
2005100145.tif 000020
【０１０９】
固有振動数の外乱成分ｄｎは、パワー算出器１３に入力される。パワー算出器１３は、以下の式（１９）で示すように固有振動数の外乱成分ｄｎから固有振動数の外乱パワー成分Ｐｄｎを算出する。式（１９）においてｄｎ’は固有振動数の外乱変化速度成分である。
【０１１０】
【数１９】
2005100145.tif 000021
【０１１１】
外乱パワー成分Ｐｄｎはコントローラ１に入力され、閾値と比較される。
【０１１２】
図２２は、第７実施形態の外乱パワー算出器１１が算出した固有振動周波数の外乱パワー成分Ｐｄｎの平方根√Ｐｄｎを、外乱ｄと共に示す。図３に外乱パワーＰｄと同様に、外乱パワー成分Ｐｄｎの平方根をとっているのは単に外乱ｄと次元を揃えて比較するためである。図２２では、外乱ｄは閾値ＴＨを下回っているが、外乱パワー成分Ｐｄｎは閾値ＴＨを上回り、衝突が検知される。
【０１１３】
（第８実施形態）
図２３に示す本発明の第８実施形態は、第７実施形態の外乱パワー成分算出器４３にさらにフィルタ４５を追加した構成の外乱パワー成分推定器４４を備えている。フィルタ４４は外乱パワー成分算出器４３により算出された外乱パワー成分Ｐｄｎに対し、以下の式（２０）のフィルタ処理を施して外乱パワー成分Ｐｄｎの推定値Ｐｄｎ＾を算出する。
【０１１４】
【数２０】
2005100145.tif 000022
【０１１５】
このフィルタ処理により得られる推定値Ｐｄｎ＾は衝突評価量としてコントローラ１に出力され、閾値と比較される。
【０１１６】
図２４は、図３に示す外乱ｄに対して外乱パワー成分推定器４４が式（２０）のフィルタ処理により算出した外乱パワー成分Ｐｄｎの推定値Ｐｄｎ＾の平方根√Ｐｄｎ＾を、外乱推定値Ｐ＾と共に示す。推定値Ｐｄｎ＾の平方根をとっているのは単に外乱ｄと次元を揃えて比較するためである。図２４では、外乱ｄは閾値ＴＨを下回っているが、外乱パワー成分Ｐｄｎの推定値Ｐｄｎ＾は図２２の外乱パワー成分Ｐｄｎと同様に閾値ＴＨを上回り、衝突が検知される。
【０１１７】
（第９実施形態）
以上の説明では、衝突時の固有振動に着目した衝突検知について述べた。しかし、ロボット３が穏やかに外界と接触し、徐々に外界に押し付けられていくような場合、外乱ｄは図３に示すように振動することなく、図２５に示すようにランプ状に変化する。第９実施形態は、このような外乱ｄがランプ状に変化する場合の衝突を検知するものである。
【０１１８】
図２６を参照すると、衝突検知装置は外乱算出器１２、速度演算器４６、予測時間算出器４７、及び予測時間算出器４７の算出した閾値到達予測時間Ｔｐと閾値とを比較して衝突を判定するコントローラ１により構成される。
【０１１９】
外乱算出器１２はトルクτと関節角度θから時々刻々と外乱ｄを算出し、速度演算器４６は外乱ｄから外乱変化速度ｄ’を算出する（図２７のステップＳ１参照）。外乱ｄと外乱変化速度ｄ’は予測時間算出器４７に入力される。
【０１２０】
予測時間算出器４７は、以下の式（２１）により外乱ｄが上限閾値ｄulmt又は下限閾値ｄllmtに到達する予測時間（閾値到達予測時間）Ｔｐを算出する（図２７のステップＳ２〜Ｓ６参照）。
【０１２１】
【数２１】
2005100145.tif 000023
【０１２２】
閾値到達予測時間Ｔｐはコントローラ１に出力され、所定の時間閾値Ｔlmtと比較される。以下の式（２１）’に示すように閾値到達予測時間Ｔｐが時間閾値Ｔlmt以下となると、衝突検知であると判断される（図２７のステップＳ７参照）。
【０１２３】
【数２２】
2005100145.tif 000024
【０１２４】
衝突を検知してからロボット３が減速停止するまでに要する時間と同じ又はそれよりも長い時間に時間閾値Ｔｌｍｔを設定しておけば、外乱ｄが閾値を超える前にロボット３を停止させることが可能である。換言すれば、外乱ｄは閾値を超えることがない。外乱ｄやその推定値ｄ＾を使用する衝突検知では、ロボットが停止するまでに、外乱ｄが閾値を超える危険性が高いので閾値を低めに設定していた。これに対して本実施形態では外乱ｄが閾値を超えることがないため、外乱ｄとして許容可能な最大値を上限閾値ｄulmt又は下限閾値ｄllmtとするなど、従来よりも大きめの閾値を設定可能であり、閾値が低いために発生する誤検知をなくすことができる。
【０１２５】
なお、本実施形態において、閾値到達予測時間Ｔｐの算出に外乱ｄや外乱変化速度ｄ’の予測値ｄ＾，ｄ’＾を使用してもよい。
【０１２６】
第９実施形態の変形例について説明すると、衝突の検知は式（２１）に限定されない。ｄｕｌｍｔ＝ｄｌｍｔ、ｄｌｌｍｔ＝−ｄｌｍｔとし、式（２１）の両辺にａｂｓ(ｄ’)を乗じると、以下の式（２２）が得られる。
【０１２７】
【数２３】
2005100145.tif 000025
【０１２８】
この式（２２）を使用すれば、閾値到達予測時間Ｔｐを算出することなく、外乱ｄと外乱変化速度ｄ’を使用して直接的に衝突を検知することができる。なお、本実施形態において、予め定められた所定時間後の外乱を予測しそれに基づいて衝突を検知してもよい。
【０１２９】
（第１０実施形態）
モータ駆動系には、トルクリップルや減速機の角度伝達誤差などによって周期的な外乱が加わる。衝突による固有振動とは異なり、これらの周期的外乱は衝突とは全く関係がなく、衝突を検知するために算出された外乱においてはノイズとして作用する。従って、トルクリップルや減速機の角度伝達誤差などによる周期的外乱を除去した外乱に基づいて衝突検知を実施すれば、より高精度な衝突検知を行うことができる。
【０１３０】
今、トルクリップルや減速機の角度伝達誤差の周期をωｆとすれば、その周期の外乱成分は式（１８）のωｎをωｆに置き換えた、以下の式（２３）で推定される。
【０１３１】
【数２４】
2005100145.tif 000026
【０１３２】
以下の式（２４）に示すように、式（２３）で推定される外乱成分ｄｆをもとの外乱ｄから引くことで、トルクリップル等（周期ωｆ）による周期成分を除去した外乱成分ｄｃが得られる。
【０１３３】
【数２５】
2005100145.tif 000027
【０１３４】
あるいは外乱成分ｄｆを求めることなく、外乱成分ｄｃを直接求めるためには、１から式（１８）の伝達関数を引いて得られる、以下の式（２５）の伝達関数を有するフィルタを使用すればよい。
【０１３５】
【数２６】
2005100145.tif 000028
【０１３６】
式（２３），（２４）から導出された外乱成分ｄｃと、式（２５）から算出された外乱成分ｄｃとは同一の値となる。
【０１３７】
図２８は、図２５のランプ状の外乱にさらにトルクリップルや角度伝達誤差による周期的な外乱が付加された外乱ｄと、この外乱ｄに対する式（２４）の外乱成分ｄｃを示す。この図２８から明らかなように、式（２４）の外乱成分ｄｃではトルクリップルや減速機の角度伝達誤差による周期的外乱が除去されるので、外乱成分ｄｃを使用することでほとんどそれらの周期的外乱が存在しない状態と変わりなく衝突を検知することができる。
【０１３８】
（第１１実施形態）
以上の説明では、固有振動数で振動する外乱（図３参照）と、ランプ状の外乱（図２５参照）を個別に取り扱ったが、これら２種類の外乱が同時に発生する場合がある。
【０１３９】
この場合、固有振動周期の外乱成分ｄｎを除去した外乱成分ｄｏに対して第９又は第１０実施形態の方法を適用することができる。また、固有振動周期の外乱成分ｄｎに対して第１から第８実施形態の方法を適用することができる。本実施形態は、上記２種類の外乱が同時に作用する場合によりに安定した、早期の衝突検知を実現するために、次の衝突評価量Ｃｄを導入する。
【０１４０】
まず、外乱ｄから式（１８）の固有振動周期の外乱成分ｄｎを除去した外乱成分ｄｏを、以下の式（２６）により算出する。外乱ｄに代えてその推定値ｄ＾を使用してもよい。
【０１４１】
【数２７】
2005100145.tif 000029
【０１４２】
式（２５）と同様の以下の式（２７）により外乱成分ｄｏを算出してもよい。
【０１４３】
【数２８】
2005100145.tif 000030
【０１４４】
次に、式（２６）又は式（２７）で得られる外乱成分ｄｏの絶対値Ａｄｏは、以下の式（２８）で表される。
【０１４５】
【数２９】
2005100145.tif 000031
【０１４６】
評価値Ｃｄは、以下の式（２９）に示すように、絶対値Ａｄｏと式（１９）の固有振動周期Ｐｄｎの平方根√Ｐｄｎの和として定義される。
【０１４７】
【数３０】
2005100145.tif 000032
【０１４８】
このＣｄを衝突評価量とし、評価量が閾値を超えたことによって衝突を検知する。
【０１４９】
図２９を参照すると、本実施形態の衝突検知装置は、評価量Ｃｄを算出する評価量算出器４９と、評価量Ｃｄを閾値と比較して衝突を判定するコントローラ１により構成される。評価量算出器４９は、外乱パワー平方根算出器５０と、外乱成分絶対値算出器５１とを備える。外乱パワー平方根算出器５０は、第１実施形態の外乱パワー算出器１１にさらにバンドパスフィルタ５２と平方根算出器５３を付加した構成であり、外乱算出器１２がトルクτと関節角度θより時々刻々と算出する外乱ｄから、固有振動周期の外乱パワー成分の平方根√Ｐｄｎを算出する。一方、外乱成分絶対値算出器５１は、外乱パワー平方根算出器５０から入力される外乱ｄと外乱成分ｄｎと式（２６）の外乱成分ｄｏを算出する減算器５５と、減算器５５から出力される外乱成分ｄｏの絶対値Ａｄｏ（式（２８））を算出する絶対値算出器５６とを備える。外乱パワー平方根算出器５０から出力される固有振動周期の外乱パワー成分の平方根√Ｐｄｎと、外乱成分絶対値算出器５１から出力される外乱成分ｄｏの絶対値Ａｄｏが加算器５７で加算され、衝突評価量Ｃｄが算出される。
【０１５０】
図３０は、固有振動数で振動する外乱とランプ状の外乱が同時に発生した場合の外乱ｄと、この外乱ｄに対して評価量算出器４９が算出した式（２９）の衝突評価量Ｃｄを示す。外乱ｄが閾値ＴＨに達して衝突が検知される時間と比較すると、本実施形態の衝突評価量Ｃｄを使用することで、検知時間が大幅に短縮され、早期に安定して衝突を検知することができる。
【０１５１】
図３０の例では、固有振動の第１回目の波動で衝突評価量Ｃｄが閾値ＴＨに達して衝突が検知されるように閾値ＴＨの値を設定している。これに対して図３１に示すように、固有振動の２回目以降の波動で衝突評価量Ｃｄが閾値ＴＨに達するように閾値ＴＨを変更しても、外乱ｄを使用するよりも早期に衝突を検知可能である。図３０及び図３１に示すように、衝突評価量Ｃｄは常に外乱ｄよりも大きく、衝突評価量Ｃｄを使用すれば外乱ｄを使用するよりも常に早期に衝突を検知できる。
【０１５２】
（第１２実施形態）
図３２は、第１２実施形態に係る衝突検知装置を示す。この第１２実施形態では、上記第１１実施形態の衝突評価量Ｃｄが閾値ＴＨに達するまでに要する閾値到達予測時間Ｔｐから衝突を検知する。
【０１５３】
衝突検知装置は、第１１実施形態の評価量算出器４９、予測時間算出器５９、速度算出器６０、及び予測時間算出器５９が算出した閾値到達予測時間Ｔｐを閾値と比較して衝突を判定するコントローラ１により構成される。
【０１５４】
評価量算出器４９の算出する衝突評価量Ｃｄと、評価量算出器４９が備える絶対値算出器５６の算出する絶対値Ａｄｏが予測時間算出器５９に入力される。また、評価量算出器４９が備える減算器５５の算出する外乱成分ｄｏと、速度算出器６０の算出する外乱成分ｄｏの変化速度ｄｏ’が予測時間算出器５９に入力される。
【０１５５】
予測時間算出器５９は、固有振動周期以外の外乱成分ｄｏ、その変化速度ｄｏ’、及び外乱成分ｄｏの絶対値Ａｄｏを用いて、Ｃｄが上限閾値ｄｕｌｍｔ又は下限閾値ｄｌｌｍｔに到達するまでに要する時間の予測値である閾値到達予測時間Ｔｐを以下の式（３０）により算出する。なお、外乱成分ｄｏとその変化速度ｄｏ’に代えて、外乱成分ｄｏの推定値ｄｏ＾と変化速度ｄｏ’の推定値ｄｏ’＾を使用してもよい。
【０１５６】
【数３１】
2005100145.tif 000033
【０１５７】
閾値到達予測時間Ｔｐはコントローラ１に出力され、所定の時間閾値Ｔlmtと比較される。以下の式（３１）に示すように閾値到達予測時間Ｔｐが時間閾値Ｔlmt以下となると、衝突検知であると判断される。
【０１５８】
【数３２】
2005100145.tif 000034
【０１５９】
図３３は、固有振動数で振動する外乱とランプ状の外乱が同時に発生し、かつ固有振動成分が減衰しない場合の外乱ｄと、この外乱ｄに対して評価量算出器４９が算出した衝突評価量Ｃｄ、及び外乱ｄに対する式（２６）の外乱成分ｄｏを示す。また、図３３には、式（３０）を使用した算出した衝突評価量Ｃｄの閾値到達予測時間Ｔｐと、式（２１）の外乱ｄと外乱変化速度ｄ’をそれぞれ外乱成分ｄｏとその変化速度ｄｏ’に置き換えて算出した外乱成分ｄｏの閾値到達予測時間Ｔｐとを示す。さらに、図３４は、図３３の外乱に対する、外乱成分ｄｏの変化速度ｄｏ＾、外乱パワーの平方根√Ｐｄ、及び衝突評価量Ｃｄを概略的に示す。
【０１６０】
図３３及び図３４から明らかなように、外乱パワーの平方根√Ｐｄを使用すると、閾値到達予測時間Ｔｐが一定値となり衝突を検知できないのに対し、外乱成分ｄ０や衝突評価量Ｃｄを使用して閾値到達時間Ｔｐを算出することで、衝突を検知することができる。外乱成分ｄｏとその変化速度ｄｏ＾を使用して算出した閾値到達予測時間Ｔｐで衝突を検知する場合と比較すると、衝突評価量Ｃｄを使用して算出した閾値到達予測時間Ｔｐで衝突を検知することにより、外乱の固有振動成分が減衰しない場合でも早期に衝突を検知することができる。
【０１６１】
Ｃｄ−Ａｄｏ＝√Ｐｄｎであるから、式（３０）は以下の式（３１）’に変形できる。
【０１６２】
【数３３】
2005100145.tif 000035
【０１６３】
第９実施形態の変形例と同様に、この式（３１）’の両辺にａｂｓ(ｄｏ’)を乗じると、以下の式（３２）が得られる。
【０１６４】
【数３４】
2005100145.tif 000036
【０１６５】
式（３１）を使用すれば、衝突評価量Ｃｄを算出することなく、衝突を検知することができる。また、式（３２）を使用すれば、閾値到達予測時間Ｔｐを算出することなく、直接的に衝突を検知することができる。
【０１６６】
（第１３実施形態）
第１２実施形態では、特定周波数（固有周波数成分）を除去した外乱成分である式（２６）又は式（２７）で算出される外乱成分ｄｏとその変化速度ｄｏ’とを使用して閾値到達予測時間Ｔｐを算出している。しかし、外乱成分ｄｏには特定周波数成分以外の高周波成分が含まれているので、閾値到達予測時間Ｔｏは変化速度ｄｏ’の高周波成分により誤差が生じる。そのため外乱成分ｄｏの変化速度ｄｏ’に高周波成分が含まれる場合、その高周波成分を除去した推定値ｄｏ’＾を用いて閾値到達予測時間Ｔｐを算出した方が安定した衝突検知が可能である。
【０１６７】
図３５は、図３３と同一の外乱ｄに対する特定周波数と高周波の両成分を除去した推定値ｄｌ＾と、第１２実施形態の式（３０）で外乱成分ｄｏとその変化速度ｄｏ’に代えて、それぞれ推定値ｄｌ＾と、その変化速度ｄｌ’＾を使用して算出した閾値到達予測時間Ｔｐを示す。図３３と図３５を比較すると、現時点での閾値到達予測時間Ｔｐは同じであるが、図３３の外乱成分ｄｏが衝突直後に大きく変化しているのに対して、図３５の推定値ｄｌ＾はランプ状に変化する外乱の特性を良く示しており、衝突直後からより安定した検知が可能となる。
【０１６８】
従って、閾値到達予測時間Ｔｐによる衝突検知を行う上では、高周波成分は外乱成分ｄｏや推定値ｄｌ＾にではなく、外乱成分ｄｎに含まれる方が好ましい。外乱成分ｄｎを以下の（３２）’式により算出すれば、外乱成分ｄｎに特定周波数成分に加え高周波成分も含めることができる。
【０１６９】
【数３５】
2005100145.tif 000037
【０１７０】
次に、上記特定周波数と高周波の両成分を除去した推定値ｄｌ＾の算出方法について具体的に説明する。図３６は、ランプ状に変化する成分が含まれる外乱ｄに対して、単純に低域通過で高周波除去のフィルタ特性をもつ外乱推定オブザーバ（式（９））によって算出した推定値ｄ＾を示す。図３６から明らかなように推定値ｄ＾はｄに対して式（９）の収束字定数Ｔｄだけ位相が遅れてしまい、衝突検知の判断が遅れることになる。これは、式（９）の外乱オブザーバが外乱変化速度ｄ’＝０と仮定し、外乱ｄをオフセット外乱と仮定して定式化されているためである。これに対して本実施形態では、外乱ｄをランプ状外乱として扱うために、外乱ｄの変化加速度ｄ”＝０と仮定してオブザーバを定式化することにより、以下の式（３３）のランプ状外乱推定オブザーバを構成する。
【０１７１】
【数３６】
2005100145.tif 000038
【０１７２】
またｄ”＝０と仮定して定式化した場合、外乱変化速度を推定するランプ状外乱変化速度推定オブザーバは以下の式（３４）で与えられる。
【０１７３】
【数３７】
2005100145.tif 000039
【０１７４】
式（３３），（３４）のオブザーバで得られる外乱の推定値ｄｌ＾と、外乱変化速度の推定値ｄｌ’＾を、例えば第１２実施形態の式（３０）において外乱成分ｄｏ（又は推定値ｄｏ＾）とその変化速度ｄｏ’（又は推定値ｄｏ＾）に代えて使用すれば、衝突検知の誤検知低減など、安定化に寄与することができる。
【０１７５】
図３６に示すように、式（９）のオブザーバの推定値ｄ＾は外乱ｄに対して移送が遅れるのに対し、式（３３）のランプ状外乱推定オブザーバの推定値ｄｌ＾は速やかに外乱ｄと一致する。
【０１７６】
ランプ状に変化する外乱は、衝突時以外にも、一定加速時の粘性摩擦による外乱などがあり、このような外乱に対して上記のランプ状外乱推定オブザーバを適用し、外乱相殺（外乱を打ち消すように外乱推定値をトルクに加算）を行えば、速やかに外乱の影響を除去し、高精度な制御が可能となる。
【０１７７】
次に、第７、第１０、及び第１１実施形態の変形例について説明する。式（１８），（２３）に示されるバンドバスフィルタや式（２５），（２７）のノッチフィルタのパラメータΩをΩ＞ωｎあるいはωｆと設定すれば、ωｎ２／ΩからΩの間の周波数帯を通過させ、あるいは除去するフィルタとなり、Ω≦ωｎあるいはωｆと設定すれば、ωの周波数のみを通過させ、あるいは除去するフィルタとなる。固有振動周期やトルクリップル周期や角度伝達誤差周期に不確かさがある場合、Ω＞ωｎあるいはωｆと設定し、幅を持ってその周期を捕らえることができる。またその周期のみを通過あるいは除去させ、それ以外の周期の外乱を厳密に除去あるいは通過させるためには、Ω≦ωｎあるいはωｆと設定すればよい。
【０１７８】
また、ロボットの固有振動周期はロボットの姿勢により変化し、トルクリップル周期や角度伝達誤差周期は、モータの回転速度によって変化する。式（１８），（２３），（２４）のフィルタにおいて、固有振動周期やトルクリップル周期や角度伝達誤差周期の変化に応じて、ωｎやωｆを変更することによって、固有振動周期やトルクリップル周期や角度伝達誤差周期の変化に対応することができる。このようにして得られる推定値ｄｎ＾，ｄｆ＾，ｄｃ＾は、当然、上述の衝突検知に使用することも可能であるが、例えばモータトルクにフィードバックすることによって、トルクリップルや角度伝達誤差の影響を相殺するために使用することもできる。
【図面の簡単な説明】
【０１７９】
【図１】本発明の第１実施形態に係る衝突検知装置を示すブロック図である。
【図２】外乱算出器を示すブロック図である。
【図３】外乱と外乱パワーの時間変化を示すグラフである。
【図４】外乱推定オブザーバを示すブロック図である。
【図５】本発明の第２実施形態に係る外乱パワー平方根推定器を示すブロック図である。
【図６】外乱の推定値及び外乱パワーの推定値の平方根の時間変化を示すグラフである。
【図７】本発明の第３実施形態に係る衝突検知装置を示すブロック図である。
【図８】外乱絶対値算出器を示すブロック図である。
【図９】外乱の推定値及び外乱絶対値の推定値の時間変化を示すグラフである。
【図１０】本発明の第４実施形態に係る衝突検知装置を示すブロック図である。
【図１１】外乱変化速度絶対値算出器を示すブロック図である。
【図１２】本発明の第５実施形態に係る衝突検知装置を示すブロック図である。
【図１３】外乱、外乱パワーの平方根、外乱絶対値、及び絶対値和の時間変化を示すグラフである。
【図１４】図１３の部分拡大図である。
【図１５】本発明の第６実施形態に係る衝突検知装置を示すブロック図である。
【図１６】外乱の推定値、外乱パワーの推定値、外乱絶対値の推定値、及び絶対値和の推定値の時間変化を示すグラフである。
【図１７】外乱に含まれる固有振動成分が減衰しない場合の外乱パワー、外乱絶対値、及び絶対値和の時間変化を示すグラフである。
【図１８】図１７の部分拡大図である。
【図１９】外乱に含まれる固有振動成分が減衰しない場合の外乱パワーの推定値、外乱絶対値の推定値、及び絶対値和の推定値の時間変化を示すグラフである。
【図２０】本発明の第７実施形態に係る衝突検知装置を示すブロック図である。
【図２１】外乱成分算出器を示すブロック図である。
【図２２】固有振動数の外乱成分パワー成分の平方根と外乱の時間変化を示すグラフである。
【図２３】本発明の第８実施形態に係る衝突検知装置を示すブロック図である。
【図２４】固有振動数の外乱パワー成分の推定値の平方根と外乱の推定値の時間変化を示すグラフである。
【図２５】ランプ状に変化する外乱と閾値到達予測時間を示す概略的なグラフである。
【図２６】本発明の第９実施形態に係る衝突検知装置を示すブロック図である。
【図２７】第９実施形態の衝突検知方法を説明するためのフローチャートである。
【図２８】周期的な外乱が付加された外乱とこの周期的な外乱を除去した外乱を示すグラフである。
【図２９】本発明の第１１実施形態の衝突検知装置を示すブロック図である。
【図３０】ランプ状の外乱と衝突評価量の時間変化を示すグラフである。
【図３１】ランプ状の外乱に固有振動による外乱が加わった外乱と衝突評価量の時間変化を示すグラフである。
【図３２】本発明の第１２実施形態に係る衝突検知装置を示すブロック図である。
【図３３】ランプ状の外乱に固有振動による外乱が加わった外乱、衝突評価量、及び固有振動数の外乱成分を除去した外乱成分の時間変化を示すグラフである。
【図３４】外乱パーの平方根、衝突評価量、及び固有振動数の外乱成分を除去した外乱成分の推定値の時間変化を示す概略的なグラフである。
【図３５】ランプ状の外乱に固有振動による外乱が加わった外乱、衝突評価量、及び特定周波数と高周波の両成分を除去した推定値の時間変化を示すグラフである。
【図３６】ランプ状の外乱、通常の外乱の推定値、及び特定周波数と高周波の両成分を除去した外乱の時間変化を示すグラフである。
【符号の説明】
【０１８０】
１  コントローラ
２  軌道計画
３  ロボット
４  エンコーダ
１１  外乱パワー算出器
１２  外乱算出器
１３  パワー算出器
１４  演算器
１５，１６  自乗演算器
１７  加算器
１８  外乱パワー平方根推定器
１９  平方根算出器
２０  フィルタ
２２  外乱絶対値推定器
２３  外乱絶対値算出器
２４  絶対値算出器
２５  フイルタ
２６  外乱変化速度絶対値推定器
２７  外乱変化速度絶対値算出器
２８  フィルタ
２９  速度絶対値算出器
３０  速度算出器
３１  絶対値算出器
３３  絶対値和算出器
３４  乗算器
３５  加算器
３７  絶対値和推定器
３８  フィルタ
４１  外乱成分算出器
４２  バンドパスフィルタ
４３  外乱パワー成分算出器
４４  外乱パワー成分推定器
４５  フィルタ
４６  速度演算器
４７  予測時間算出器
４９  評価量算出器
５０  外乱パワー平方根算出器
５１  外乱成分絶対値算出器
５２  バンドパスフィルタ
５３  平方根算出器
５４  加算器
５５  減算器
５６  絶対値算出器
５７  加算器
５９  予測時間算出器
６０  速度算出器
【図１】
2005100145.tif 000040
【図２】
2005100145.tif 000041
【図３】
2005100145.tif 000042
【図４】
2005100145.tif 000043
【図５】
2005100145.tif 000044
【図６】
2005100145.tif 000045
【図７】
2005100145.tif 000046
【図８】
2005100145.tif 000047
【図９】
2005100145.tif 000048
【図１０】
2005100145.tif 000049
【図１１】
2005100145.tif 000050
【図１２】
2005100145.tif 000051
【図１３】
2005100145.tif 000052
【図１４】
2005100145.tif 000053
【図１５】
2005100145.tif 000054
【図１６】
2005100145.tif 000055
【図１７】
2005100145.tif 000056
【図１８】
2005100145.tif 000057
【図１９】
2005100145.tif 000058
【図２０】
2005100145.tif 000059
【図２１】
2005100145.tif 000060
【図２２】
2005100145.tif 000061
【図２３】
2005100145.tif 000062
【図２４】
2005100145.tif 000063
【図２５】
2005100145.tif 000064
【図２６】
2005100145.tif 000065
【図２７】
2005100145.tif 000066
【図２８】
2005100145.tif 000067
【図２９】
2005100145.tif 000068
【図３０】
2005100145.tif 000069
【図３１】
2005100145.tif 000070
【図３２】
2005100145.tif 000071
【図３３】
2005100145.tif 000072
【図３４】
2005100145.tif 000073
【図３５】
2005100145.tif 000074
【図３６】
2005100145.tif 000075