JP5652738B2 - 衝突判定システム及び衝突判定方法 - Google Patents

Description

本発明は、低推力のモータで駆動されるアームが障害物に当たったことを検出する衝突判定システム及び衝突判定方法に関する。

製造現場等で安定した品質で効率的な生産を行うために産業用ロボットが重要な役割を担っている。利用範囲も、自動車車体へのスポット溶接や塗装のみでなく電子回路基板へのパーツの組立、製品の箱詰めなど、拡大を続けてきている。

産業用ロボットを利用する製造現場等では、安全領域を確保するためにセンサを用いてロボットアームと作業者との接触を防止している。

特許文献１（特表２００９−５４５４５７号）には、可動式機械要素を備える機械と作業者との衝突を防止する監視装置が開示されている（特許文献１の段落［０００１］）。この監視装置は、画像センサによって安全領域への物体の進入を監視し、安全領域への物体の進入が検知された場合にロボットアームの駆動を制御し、物体との衝突を防止する（特許文献１の段落［００１０］、［００１２］）。

特許文献２（特開２００１−２０８５０４号）には、医用寝台装置の障害物検出システムが開示されている。このシステムは、医療装置のＣ字形アームが患者や術者、付属装置への接触を防止するために、静電容量型距離センサを利用する。このセンサは、電磁界の強度を検出して周囲の物体の存在を検出する（特許文献２の段落［０００９］）。

特表２００９−５４５４５７号 特開２００１−２０８５０４号

特許文献１，２は、アームが作業者などに当たった場合に作業者などに強い衝撃を与え得ることを前提として、作業者へのアームの衝突を回避することを目的としている。特許文献１，２ではこの目的を安全領域を確保することで解決することから、アームの周囲を監視するためのセンサを利用する必要がある。従来の技術では、このようなセンサを備えると、センサで検出した情報の処理やそれに基づいたアームの制御が必要で、システムが高価になってしまう。

そこで、アームが作業者などへ当たっても危害を加えない程度に、アームを低推力のモータで動作させ、作業者などへ当たった場合には安全側へアームを退避するように制御することが考えられる。さらに、システムの製造コストを低減するために、センサを利用せずとも、低推力のモータによるアームが作業者などに当たったことを判定する技術が必要である。

そこで、本発明は、アームなどの可動部周囲の状況を監視するセンサを用いることなく、低推力のモータによる可動部が作業者や物などの障害物に当ったことを判定する衝突判定システム及び衝突判定方法を提供することを目的とする。

上記目的を達成するために、本発明の衝突判定システムは、
可動部を駆動するための低推力のモータの回転を検出する検出部と、
前記可動部が所定の動作を行うために、前記検出部からの検出信号に基づく現在位置と要求動作に基いた目的位置との差分が減るよう前記モータをフィードバック制御する制御部と、
前記モータの駆動状態を監視する判断部と、
を備え、
前記判断部が、
前記モータの出力量として前記検出部からの検出信号に基いて前記可動部の回転角度及び速度を算出し、
算出した回転角度及び速度の各差分に対してそれぞれ係数を乗じて加算することにより位置情報を電圧値で算出し、その算出した電圧値と前記制御部による前記モータへの制御量としての出力電圧との誤差を計測タイミング毎に算出し、その誤差を累積し、その累積誤差と所定の値とを比較して前記可動部が障害物に衝突したかを判定する。

上記目的を達成するために、低推力のモータで駆動される可動部が障害物に衝突したことを検出する衝突判定方法であって、
前記モータの回転に関する検出信号に基いて前記可動部の回転角度及び速度を、前記モータの出力量として算出し、算出した回転角度及び速度の各差分に対してそれぞれ係数を乗じて加算することで位置情報を電圧値で算出し、計測タイミング毎にその算出した電圧値と前記モータの制御量としてモータの出力電圧との誤差を算出し、計測タイミング毎の各誤差を累積する算出工程と、
前記算出工程で求めた累積誤差と所定の値とを比較して前記可動部が障害物に衝突したかを判定する判断工程と、
を備える。

本発明によれば、可動部の可動域を監視するセンサを設けていないので、センサの情報に基づいた可動部の制御などが不要で、システムの製造コストを低減することができる。

本発明の実施形態に係る機械式駆動装置の平面図である。 本発明の実施形態に係る衝突判定システムのブロック図である 本発明の実施形態に累積誤差制御を説明するための図である。 従来の電流値制御を説明するための図である。

以下、図面に基づいて本発明の実施形態を説明する。
図１は本発明の実施形態に係る機械式駆動装置１の平面図である。機械式駆動装置１は駆動装置２とこの駆動装置２に一端部を支持された棒状のアーム３とを備えている。このアーム３は１自由度を有し、所定の動作、例えば外部から指定された要求動作に基づいて駆動装置２によって鉛直軸周りに回転する。駆動装置２は、アーム３が作業者や物などの障害物５に当たっても危害を加えないように、低推力のＤＣモータ、ＡＣモータ或いはステッピングモータで構成されている。例えば、８０Ｗ程度のモータを利用する。

本発明では、機械式駆動装置１がアーム３と障害物５との衝突を回避するように構成されていないため、アーム３が要求動作に従って動作している過程で、アーム３の動作領域に例えば作業者などの障害物５が進入し、アーム３が障害物５に衝突した場合に衝突を検出して、アーム３の動作を停止或いはアーム３を障害物５から退避するよう動作させる必要がある。そこで、本発明の実施形態に係る衝突判定システム１０により衝突を検出する。

図２は本発明の実施形態に係る衝突判定システム１０のブロック図である。衝突判定システム１０は、検出部２０と制御部３０と判断部４０とを備えている。

検出部２０はモータ２Ａの回転を検出する。例えば、検出部２０はロータリーエンコーダとして構成されている。このロータリーエンコーダからの信号が制御部３０や判断部４０に送られる。

制御部３０は、アーム３に所定の動作、例えば外部からの要求動作を行わせるためにモータ２Ａを制御する。その際、制御部３０は検出部２０からの信号に基づいてモータの回転位置などを求め、モータ２Ａをフィードバック制御する。図示を省略するが、制御部は比較器と増幅器とを備え、例えば制御部３０はロータリーエンコーダからの信号に基づいて現在位置を求め、要求動作に基づいた目的位置とセンサで得た現在位置とを比較して、その差分が減るようにモータを制御する。

判断部４０は、モータの駆動状態としてモータの位置を監視する。この監視作業として、判断部４０は、モータに対して与えた制御量とモータの出力量との誤差、つまり目的位置と現在位置との差分の検出を行う。この誤差の検出は、所定時間内に予め決められた回数分行われる。そして、判断部は、所定時間内に誤差を累積して累積誤差を算出する。このような累積誤差の算出が所定時間毎に行われる。さらに判断部４０は、累積誤差が所定値を超えた場合にアーム３が障害物５に衝突したと判断する。

判断部４０は、モータの駆動状態を監視するために、モータ位置制御の２つのパラメータ、つまり、モータ２Ａの出力量としての位置情報とモータ２Ａに対して与えた制御量としての出力電圧とを比較する。ここで、位置情報とはモータ２Ａの出力電圧と比較できるように電圧値に換算した値に相当する。モータ２Ａに加えた電圧に対するアーム３の運動量とには誤差が生じるが、制御部によって要求動作は行われる。しかしながら、このような誤差に加えて、アーム３が障害物に衝突した場合には、通常の動作よりも大きな誤差が生じることになる。そこで、本実施形態では、モータ２Ａの出力量としての位置情報と出力電圧との誤差を所定時間内に複数回求めてモータの駆動状態からアームの状態を評価する。

具体的には、所定時間当たりの誤差の累積（以下、累積誤差と呼ぶ。）Σ［位置情報−出力電圧］を算出する。この累積誤差が所定の値を超えた場合、判断部４０は、モータ２Ａが正常な動作を行ってないと判定、つまりアーム３が障害物５に当たったと判断する。

モータ２Ａの位置情報は検出部２０からの信号に基づいて算出される。判断部４０は、モータ２Ａの出力量として、検出部２０からの信号を利用してアーム３の回転角度としてのモータの回転角度とアーム３の速度とを算出し、さらにこれらの回転角度と速度とに係数を乗じて位置情報を算出する。そして、判断部４０は、この位置情報と出力電圧との誤差を所定時間内の計測タイミング毎に計測し、誤差を積算して累積誤差を算出する。具体的には、累積誤差は下記の式として定義される。

ここで、図１中の実線は現在のアーム３を示し、一点鎖線は現在よりも一段階前の過去（以下、前回と呼ぶ。）のアーム３を示し、二点鎖線は現在よりも二段階前のアーム３を示しており、式中のＶ_cur,nは現在の速度、Ｖ_pas,nは前回の速度、Ａ_cur,nは現在のモータ２Ａの回転角度、Ａ_pas,nは前回のモータ２Ａの回転角度、Ｏ_pas,nは前回の出力電圧、ｃｏｕｎｔは単位時間当たりの計測回数、Ｐは位置係数、Ｄは速度係数である。単位時間としては、例えば０．１秒である。

このような累積誤差を算出するために、判断部４０は、図２に示すように位置算出部４１と速度算出部４２と衝突判定部４３とを備えている。

位置算出部４１は、現在のモータ２Ａの回転角度Ａ_cur,nを算出する。位置算出部４１は、検出部２０としての例えばロータリーエンコーダからの信号をモータ２Ａの回転角度Ａ_cur,nに換算する。この回転角度Ａ_cur,nによってアーム３の位置が特定される。このように算出された回転角度Ａ_cur,nは、記憶部５０に保存される。

速度算出部４２は、現在の速度Ｖ_cur,nを算出する。速度算出部４２は、検出部２０としての例えばロータリーエンコーダからの信号を現在の速度Ｖ_cur,nに換算する。この速度Ｖ_cur,nによってアーム３の速度が特定される。このように算出された速度Ｖ_cur,nは、記憶部５０に保存される。

衝突判定部４３は、先ず計測タイミング毎に誤差を算出する。この誤差の算出に、衝突判定部４３は、位置算出部４１からの現在のモータ２Ａの回転角度Ａ_cur,nと、記憶部５０からの前回のモータ２Ａの回転角度Ａ_pas,nと、速度算出部４２からの現在の速度Ｖ_cur,nと、記憶部５０からの前回の速度Ｖ_pas,nと、記憶部５０からの前回の出力電圧Ｏ_pas,nと、を利用する。衝突判定部４３はこのような誤差の算出処理を所定時間内に、規定された回数だけ繰り返して、式（１）に従った累積誤差を算出する。

図３は累積誤差の時間変化を示す図である。図３に示すように、モータ２Ａによる制御に従ってアーム３が回転している場合には、誤差は０の値に近接するが、障害物５に衝突した場合には、図３のｔ＝ｔ１で示すように累積誤差が所定の値ＴＨ２を超える。また、図３に示していないが、累積誤差が＋側の値ＴＨ１を越えた場合も、モータ２Ａによる制御に従ってアーム３が回転していない、つまり障害物５に衝突したことになる。

このように、累積誤差が所定の値ＴＨ１，ＴＨ２を超えた場合、判断部４０は、アーム３が障害物５に衝突したと判断する。この場合、判断部４０は、停止信号を制御部３０に送り、アーム３を停止或いは障害物５から後退するようにアーム３を制御する。

このような本発明の累積誤差制御に対して、モータへの電流値による制御が従来知られている。例えば図４に示すように、モータの動作中で最もトルクがかかる部分を確認して、その電流値が所定の値ＴＨ３を超えた場合、異常として制御を止める。このような電流制御は単純な閾値制御であるが、過負荷になるまでに時間がかかり、アームが障害物に接触した直後にアームを停止させることができない。このような電流制御に対して、本発明の実施形態に係る累積誤差制御は、モータ２Ａの制御状況だけを監視するため、アーム３と障害物５との接触を直ちに検出できて、アーム３を素早く停止させることができる。
また、アーム３の可動域を監視するセンサを設けていないので、センサの情報に基づいたアーム３の制御などが不要で、システムの製造コストを低減することができる。

以上説明したが、本発明は発明の趣旨を逸脱しない範囲において様々な形態で実施できる。
本発明は上記実施形態に用途や使用場所が限定されるものではない。例えば、モータによって駆動される可動部は、回動動作を行うアームに限定されるものではなく、直進方向に移動するコンベアやリフターの可動部分であってもよいことは勿論である。

１ ：機械式駆動装置
２ ：駆動装置
２Ａ ：モータ
３ ：アーム
５ ：障害物
１０ ：衝突判定システム
２０ ：検出部
３０ ：制御部
４０ ：判断部
４１ ：位置算出部
４２ ：速度算出部
４３ ：衝突判定部
５０ ：記憶部

Claims (2)

1. 可動部を駆動するための低推力のモータの回転を検出する検出部と、
   前記可動部が所定の動作を行うために、前記検出部からの検出信号に基づく現在位置と要求動作に基いた目的位置との差分が減るよう前記モータをフィードバック制御する制御部と、
   前記モータの駆動状態を監視する判断部と、
   を備え、
   前記判断部が、
   前記モータの出力量として前記検出部からの検出信号に基いて前記可動部の回転角度及び速度を算出し、
   算出した回転角度及び速度の各差分に対してそれぞれ係数を乗じて加算することにより位置情報を電圧値で算出し、その算出した電圧値と前記制御部による前記モータへの制御量としての出力電圧との誤差を計測タイミング毎に算出し、その誤差を累積し、その累積誤差と所定の値とを比較して前記可動部が障害物に衝突したかを判定する、
   衝突判定システム。
2. 低推力のモータで駆動される可動部が障害物に衝突したことを検出する衝突判定方法であって、
   前記モータの回転に関する検出信号に基いて前記可動部の回転角度及び速度を、前記モータの出力量として算出し、算出した回転角度及び速度の各差分に対してそれぞれ係数を乗じて加算することで位置情報を電圧値で算出し、計測タイミング毎にその算出した電圧値と前記モータの制御量としてモータの出力電圧との誤差を算出し、計測タイミング毎の各誤差を累積する算出工程と、
   前記算出工程で求めた累積誤差と所定の値とを比較して前記可動部が障害物に衝突したかを判定する判断工程と、
   を備える、衝突判定方法。