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

Abstract

【課題】可動部周囲の状況を監視するセンサを用いることなく、低推力のモータによる可動部が作業者や物などの障害物に当ったことを判定する衝突判定システム及び衝突判定方法を提供する。
【解決手段】低推力のモータ２Ａで駆動される可動部が障害物に当たったことを検出する判断部４０を備えた衝突判定システム１０であって、判断部４０が、位置制御されたモータ２Ａの位置の誤差を累積して得る累積誤差の算出を所定時間毎に繰り返して行い、累積誤差が所定値を超えた場合に可動部が障害物に当たったと判断する。
【選択図】図２

Description

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

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

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

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

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

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

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

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

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

上記目的を達成するために、本発明の第１の構成は、低推力のモータで駆動される可動部が障害物に当たったことを検出する判断部を備えた衝突判定システムであって、判断部が、モータに対して与えた制御量とモータの出力量との誤差を累積して得る累積誤差の算出を所定時間毎に繰り返して行い、累積誤差が所定値を超えた場合に可動部が障害物に当たったと判断することを特徴としている。

本発明の衝突判定システムは、モータの回転を検出する検出部と、可動部に所定の動作を行わせるために検出部からの信号に基づいてモータをフィードバック制御する制御部と、を備え、判断部は、検出部からの信号と制御部からのモータの出力電圧とに基づいて下記の式の累積誤差を算出する。

ここで、Ｖ_cur,nは現在の速度、Ｖ_pas,nは前回の速度、Ａ_cur,nは現在のモータの回転角度、Ａ_pas,nは過去のモータの回転角度、Ｏは出力電圧、ｃｏｕｎｔは単位時間当たりの診断回数、Ｐは位置係数、Ｄは速度係数である。

上記目的を達成するために、本発明の第２の構成は、低推力のモータで駆動される可動部が障害物に当たったことを検出する衝突判定方法であって、判断部がモータに対して与えた制御量とモータの出力量との誤差を累積して累積誤差を算出する算出工程と、判断部が、累積誤差と所定値とを比較して可動部が障害物に当たっているか否かを判断する判断工程と、を含むことを特徴としている。

本発明の衝突判定方法は、算出工程で、判断部がモータの回転を検出する検出部からの信号とモータをフィードバック制御する制御部からのモータの出力電圧とに基づいて累積誤差を算出する。

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

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

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

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

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

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

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

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

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

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

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

ここで、図１中の実線は現在のアーム３を示し、一点鎖線は現在よりも一段階前の過去（以下、前回と呼ぶ。）のアーム３を示し、二点鎖線は現在よりも二段階前のアーム３を示しており、式中のＶ_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 (4)

1. 低推力のモータで駆動される可動部が障害物に当たったことを検出する判断部を備えた、衝突判定システムであって、
   上記判断部が、上記モータに対して与えた制御量と上記モータの出力量との誤差を累積して得る累積誤差の算出を所定時間毎に繰り返して行い、上記累積誤差が所定値を超えた場合に上記可動部が上記障害物に当たったと判断することを特徴とする、衝突判定システム。
2. 前記モータの回転を検出する検出部と、
   前記可動部に所定の動作を行わせるために上記検出部からの信号に基づいて上記モータをフィードバック制御する制御部と、を備え、
   前記判断部は、上記検出部からの信号と上記制御部からの前記モータの出力電圧とに基づいて下記の式の累積誤差を算出することを特徴とする、請求項１に記載の衝突判定システム。
   

   

   ここで、Ｖ_cur,nは現在の速度、Ｖ_pas,nは前回の速度、Ａ_cur,nは現在のモータの回転角度、Ａ_pas,nは過去のモータの回転角度、Ｏは出力電圧、ｃｏｕｎｔは単位時間当たりの診断回数、Ｐは位置係数、Ｄは速度係数である。
3. 低推力のモータで駆動される可動部が障害物に当たったことを検出する衝突判定方法であって、
   判断部が上記モータに対して与えた制御量と上記モータの出力量との誤差を累積して累積誤差を算出する算出工程と、
   上記判断部が、上記累積誤差と所定値とを比較して上記可動部が上記障害物に当たっているか否かを判断する判断工程と、
   を含むことを特徴とする、衝突判定方法。
4. 前記算出工程で、前記判断部が前記モータの回転を検出する検出部からの信号と前記モータをフィードバック制御する制御部からのモータの出力電圧とに基づいて下記の式の累積誤差を算出することを特徴とする、請求項３に記載の衝突判定方法。
   

   

   ここで、Ｖ_cur,nは現在の速度、Ｖ_pas,nは前回の速度、Ａ_cur,nは現在のモータの回転角度、Ａ_pas,nは過去のモータの回転角度、Ｏは出力電圧、ｃｏｕｎｔは単位時間当たりの診断回数、Ｐは位置係数、Ｄは速度係数である。

Images