JP6922486B2 - ロボットの制御装置 - Google Patents

Description

本発明は、ロボットを制御する制御装置に関する。

従来、ロボットに外力が作用する場合に、ロボットの発生するトルクを制限値で制限して、ロボットの位置の偏差を許容しつつロボットの位置をフィードバック制御する制御装置がある（特許文献１参照）。特許文献１に記載の制御装置は、ロボットが作業者に衝突したと判定した場合に、トルクの制限値を０にすることで、作業者がロボットを手で押して動かすことができるようにしている。

特許第４０５５０９０号公報

しかしながら、特許文献１に記載の制御装置では、ロボットが作業者に衝突した場合に、ロボットがすぐに減速するとは限らず、作業者が負傷するおそれがある。また、作業者がロボットを手で押すことをやめると、ロボットの位置は元の指令位置にフィードバック制御されるため、ロボットが作業者に再衝突するおそれがある。

本発明は、こうした課題を解決するためになされたものであり、その主たる目的は、ロボットが作業者に衝突した場合に、ロボットを速やかに減速させるとともに、ロボットと作業者の再衝突を抑制することのできるロボットの制御装置を提供することにある。

上記課題を解決するための第１の手段は、
各関節が各モータにより駆動されるロボットを制御する制御装置であって、
前記ロボットが物体に衝突したことを検知する衝突検知部と、
前記衝突検知部により前記ロボットが物体に衝突したことが検知された場合に、前記各モータを減速させるように駆動する減速制御を実行する第１制御部と、
前記第１制御部により前記減速制御が実行された後、電源から前記各モータへの電力供給を停止させ且つ前記各モータにおいて電力入出力端子間を短絡させるダイナミックブレーキを所定期間実行する第２制御部と、
を備える。

上記構成によれば、ロボットの制御装置は、各関節が各モータにより駆動されるロボットを制御する。

ここで、衝突検知部により、ロボットが物体に衝突したことが検知される。そして、衝突検知部によりロボットが物体（作業者等）に衝突したことが検知された場合に、第１制御部により、各モータを減速させるように駆動する減速制御が実行される。すなわち、ロボットが物体に衝突した場合に、各モータを加速させる加速トルクを０にしたり、各モータの回転速度を維持する維持トルクを０にしたりするのではなく、各モータを減速させるように駆動する。このため、ロボットが作業者に衝突した場合に、ロボットを速やかに減速させることができ、作業者が負傷することを抑制することができる。

第１制御部により減速制御が実行された後、第２制御部により、各モータへの電力供給を停止させ且つ各モータにおいて電力入出力端子間を短絡させるダイナミックブレーキが所定期間実行される。このため、作業者は、ダイナミックブレーキが実行されている所定期間中に、ロボットを押して動かしたり、ロボットから離れたりすることができる。さらに、所定期間中は、ロボットの位置が衝突時の位置に制御されることがない。したがって、ロボットが作業者に再衝突することを抑制することができる。しかも、ダイナミックブレーキの実行中は、作業者がロボットを押して動かす際に抵抗が生じるため、ロボットが急激に動かされることを抑制することができる。

第２の手段では、前記第１制御部は、前記ロボットの速度が所定速度よりも低くなるまで、又は前記減速制御の実行期間が判定期間よりも長くなるまで、前記減速制御を実行する。

上記構成によれば、ロボットの速度が所定速度よりも低くなるまで、又は減速制御の実行期間が判定期間よりも長くなるまで、減速制御が実行されるため、ロボットを確実に減速させることができる。

第３の手段では、各抵抗を介して前記各モータにおいて前記電力入出力端子間を開閉する各抵抗スイッチを備え、前記第１制御部は、前記減速制御として、前記各モータを減速させるように駆動し、且つ前記各抵抗スイッチを閉じる。

上記構成によれば、ロボットの制御装置は、各抵抗を介して各モータにおいて電力入出力端子間を開閉する各抵抗スイッチを備えている。このため、各抵抗スイッチを閉じることにより、各抵抗を介して各モータにおいて電力入出力端子間を短絡させるダイナミックブレーキを実行することができる。そして、第１制御部は、減速制御として、各モータを減速させるように駆動し、且つ各抵抗スイッチを閉じる。このため、各モータの減速駆動に加えて、ダイナミックブレーキを実行することができ、ロボットが作業者に衝突した場合に、ロボットを更に速やかに減速させることができる。なお、このダイナミックブレーキは、モータへの電力供給を継続している状態でも実行することができる。

第４の手段では、各抵抗を介して前記各モータにおいて前記電力入出力端子間を開閉する各抵抗スイッチを備え、前記第１制御部は、前記減速制御として、前記各モータを減速させるように駆動し、且つ前記各抵抗スイッチを開く。

上記構成によれば、第１制御部は、減速制御として、各モータを減速させるように駆動し、且つ各抵抗スイッチを開く。この場合であっても、各モータの減速駆動が実行され、ロボットが作業者に衝突した場合に、ロボットを速やかに減速させることができる。

第５の手段では、第１スイッチング素子と第４スイッチング素子との直列接続体と、第２スイッチング素子と第５スイッチング素子との直列接続体と、第３スイッチング素子と第６スイッチング素子との直列接続体と、が並列接続されたブリッジ回路を有し、前記第１，第２，第３スイッチング素子が前記電源の高電位側に接続され、前記第４，第５，第６スイッチング素子が前記電源の低電位側に接続され、各直列接続体における２つの前記スイッチング素子の間が前記モータのＵ相，Ｖ相，Ｗ相にそれぞれ接続された３相インバータと、前記電源と前記３相インバータとの間に並列接続された平滑コンデンサと、を備え、前記第２制御部は、前記ダイナミックブレーキとして、前記各モータへの電力供給を停止させ且つ前記各抵抗スイッチを閉じる第１ダイナミックブレーキを実行する。

上記構成によれば、電源と各モータとの間に並列接続された平滑コンデンサにより、電源から供給される電圧が平滑化される。ここで、各モータへの電力供給を停止させ且つ第４，第５，第６スイッチング素子を閉じることで、各モータにおいて電力入出力端子間を短絡させるダイナミックブレーキを実行すると以下の問題が生じる。すなわち、電源から各モータへの電力供給を停止させたとしても、平滑コンデンサに電荷が残っている。そして、第４，第５，第６スイッチング素子を閉じるダイナミックブレーキでは、平滑コンデンサに残った電荷を放電させることができず、平滑コンデンサに残った電荷がその後に放電されるおそれがある。

この点、上記構成によれば、第２制御部により、ダイナミックブレーキとして、各モータへの電力供給を停止させ且つ各抵抗スイッチを閉じる第１ダイナミックブレーキが実行される。このため、第１ダイナミックブレーキによりロボットを速やかに減速させるとともに、平滑コンデンサに残っている電荷を抵抗により熱として消費させることができる。特に、第３の手段を前提とする場合は、第１制御部による減速制御において、既に各抵抗スイッチが閉じられているため、制御を簡素化することができる。

第６の手段では、前記第２制御部は、前記ダイナミックブレーキとして、前記第１ダイナミックブレーキを実行した後に、前記各モータへの電力供給を停止させ且つ前記第４，第５，第６スイッチング素子を閉じる第２ダイナミックブレーキを実行する。

一般に、各抵抗スイッチを閉じる第１ダイナミックブレーキによるロボットの制動力は、第４，第５，第６スイッチング素子を閉じる第２ダイナミックブレーキによるロボットの制動力よりも小さい。このため、第１ダイナミックブレーキの実行中は、第２ダイナミックブレーキの実行中と比較して、作業者がロボットを押して動かし易くなる。その結果、例えば作業者がロボットと物体との間に挟まれた状況において、作業者がロボットと物体との間から離脱し易くなる。しかしながら、第１ダイナミックブレーキを所定期間継続すると、ロボットと衝突して慌てた作業者により強く押されることで、ロボットが動き過ぎるおそれがある。その場合、作業者によりはねのけられたロボットが、物体に衝突したり、他の作業者に衝突したりするおそれがある。

この点、上記構成によれば、第２制御部は、第１ダイナミックブレーキを実行した後に、前記各モータへの電力供給を停止させ且つ第４，第５，第６スイッチング素子を閉じる第２ダイナミックブレーキを実行する。したがって、第１ダイナミックブレーキにより平滑コンデンサに残った電荷を消費させた後に、第１ダイナミックブレーキよりも制動力の大きい第２ダイナミックブレーキが実行されるため、作業者に押されてロボットが動き過ぎることを抑制することができる。ひいては、作業者によりはねのけられたロボットが、物体に衝突したり、他の作業者に衝突したりする二次災害を抑制することができる。

第２制御部によりダイナミックブレーキが実行された後に、ロボットの位置が直ちに衝突時の位置に制御されると、ロボットが作業者に再衝突するおそれがある。

この点、第７の手段では、前記第２制御部による前記ダイナミックブレーキの実行中に前記ロボットが移動させられた方向へ、前記第２制御部により前記ダイナミックブレーキが実行された後に前記ロボットを所定距離移動させる退避制御を実行する第３制御部を備えている。このため、衝突時の位置からロボットを一旦退避させることができ、ロボットが作業者に再衝突することを抑制することができる。

第２制御部によるダイナミックブレーキの実行中にロボットが移動させられた距離は、ロボットを衝突時の位置から作業者が遠ざける必要性の高さを反映している可能性が高い。

この点、第８の手段では、前記第３制御部は、前記第２制御部による前記ダイナミックブレーキの実行中に前記ロボットが移動させられた距離に基づいて、前記退避制御における前記所定距離を設定するといった構成を採用している。したがって、ロボットを衝突時の位置から作業者が遠ざける必要性の高さに応じて、退避制御におけるロボットを移動させる距離を適切に設定することができる。

第２制御部によるダイナミックブレーキの実行中にロボットが移動させられた速度は、ロボットを衝突時の位置から作業者が遠ざける必要性の高さを反映している可能性が高い。

この点、第９の手段では、前記第３制御部は、前記第２制御部による前記ダイナミックブレーキの実行中に前記ロボットが移動させられた速度に基づいて、前記退避制御における前記所定距離を設定するといった構成を採用している。したがって、ロボットを衝突時の位置から作業者が遠ざける必要性の高さに応じて、退避制御におけるロボットを移動させる距離を適切に設定することができる。

第２制御部によるダイナミックブレーキの実行中にロボットが移動させられた加速度は、ロボットを衝突時の位置から作業者が遠ざける必要性の高さを反映している可能性が高い。

この点、第１０の手段では、前記第３制御部は、前記第２制御部による前記ダイナミックブレーキの実行中に前記ロボットが移動させられた加速度に基づいて、前記退避制御における前記所定距離を設定するといった構成を採用している。したがって、ロボットを衝突時の位置から作業者が遠ざける必要性の高さに応じて、退避制御におけるロボットを移動させる距離を適切に設定することができる。

第３制御部により退避制御が実行された後のロボットの位置から、直線的な軌道により衝突時のロボットの位置まで制御されると、作業者の体が衝突時の位置に残っていた場合に、ロボットが作業者に再衝突するおそれがある。

この点、第１１の手段では、前記第３制御部により前記退避制御が実行された後の前記ロボットの位置から、前記第１制御部による前記減速制御が終了した時の前記ロボットの位置まで、円弧状の軌道により前記ロボットの位置を制御する第４制御部を備えている。したがって、作業者の体が衝突時の位置に残っていたとしても、ロボットが作業者に再衝突することを抑制することができる。

第１２の手段では、前記第４制御部は、前記第２制御部による前記ダイナミックブレーキの実行中に前記ロボットが移動させられた速度に基づいて、前記円弧状の軌道の曲率半径を設定する。

上記構成によれば、第２制御部によるダイナミックブレーキの実行中にロボットが移動させられた速度に基づいて、円弧状の軌道の曲率半径が設定される。このため、ダイナミックブレーキの実行中にロボットが移動させられた速度に応じて、退避制御が実行された後のロボットの位置から減速制御が終了した時のロボットの位置まで制御する円弧状の軌道を適切に設定することができる。

ロボット、コントローラ、及びティーチングペンダントの概要を示す図。 コントローラ及びその周辺の電気的構成を示すブロック図。 衝突時制御の手順を示すフローチャート。 退避軌道を示す模式図。 復帰軌道を示す模式図。

以下、垂直多関節型ロボットの制御装置に具現化した一実施形態について、図面を参照しつつ説明する。本実施形態のロボットは、例えば産業用ロボットとして機械組立工場などの組立システムにて用いられる。

はじめに、ロボット１０の概要を図１に基づいて説明する。

ロボット１０は、例えば６軸の垂直多関節型ロボットとして構成されている。ロボット１０は、ベース１１、ショルダ部１３、下アーム１５、第１上アーム１６Ａ、第２上アーム１６Ｂ、手首部１７、及びフランジ１８を備えている。ショルダ部１３（第１回転部）は、ベース１１により水平方向に回転可能に支持されている。下アーム１５（第２回転部）は、ショルダ部１３により上下方向に回転可能に支持されている。第１上アーム１６Ａ（第３回転部）は、下アーム１５により上下方向に回転可能に支持されている。第２上アーム１６Ｂ（第４回転部）は、第１上アーム１６Ａにより捻り回転可能に支持されている。手首部１７（第５回転部）は、第２上アーム１６Ｂにより上下方向に回転可能に支持されている。フランジ１８（第６回転部）は、手首部１７により捻り回転可能に支持されている。

ベース１１、ショルダ部１３、下アーム１５、第１上アーム１６Ａ、第２上アーム１６Ｂ、手首部１７及びフランジ１８は、ロボット１０のアームとして機能する。アーム先端であるフランジ１８には、図示は省略するが、エンドエフェクタ（手先）が取り付けられる。ロボット１０の各関節（各回転部）は、対応して設けられた各モータＭ（図２参照）により駆動される。

ティーチングペンダント４０は、例えば作業者（オペレータ）が携帯あるいは手に持って操作可能な程度の大きさで、例えば薄型の直方体状に形成されている。ティーチングペンダント４０は、各種のキースイッチ４１及びディスプレイ４２等を備えている。作業者は、キースイッチ４１により種々の入力操作を行う。ティーチングペンダント４０は、ケーブルを介してコントローラ７０に接続されており、コントローラ７０と通信可能となっている。作業者は、ティーチングペンダント４０を操作して、ロボット１０の動作プログラムの作成、修正、登録、各種パラメータの設定を行うことができる。動作プログラムの修正等を行うティーチングでは、ロボット１０が行う作業において制御点が通過する教示点を教示する。そして、作業者は、コントローラ７０を通じて、ティーチングされた動作プログラムに基づきロボット１０を動作させることができる。換言すれば、コントローラ７０は、予め設定された動作プログラム及びティーチングペンダント４０の操作に基づいて、ロボット１０のアームの動作を制御する。

図２は、コントローラ７０及びその周辺の電気的構成を示すブロック図である。コントローラ７０（ロボットの制御装置）は、電源ユニット２０、インバータ回路３０、及び制御部６０等を備えている。なお、同図では、１つのモータＭに対応する構成を記載しているが、インバータ回路３０、回生抵抗２５、スイッチング素子２６、及び電圧センサ２７は、モータＭ毎に設けられている。

電源ユニット２０（電圧調節部）は、例えば３相２００Ｖの商用交流電源１４（電源）に接続されている。電源ユニット２０は、整流回路２４、コイル、コンデンサ２１、スイッチング素子２２，２３等を備えている。コンデンサ２１（平滑コンデンサ）は、電源１４とインバータ回路３０との間において、電源ライン１５ａ，１５ｂに並列に接続されている。スイッチング素子２２，２３は、電界効果トランジスタ（ＦＥＴ）等で形成されている。スイッチング素子２２は電源ライン１５ａ，１５ｂに直列に接続されており、スイッチング素子２３は電源ライン１５ａ，１５ｂに並列に接続されている。そして、電源ユニット２０は、スイッチング素子２２を閉じた状態で、スイッチング素子２３の開閉をＰＷＭ制御（デューティ制御）することにより、電源１４から電源ライン１５ａへ供給される電源電圧Ｖｂの高さを調節する。

回生抵抗２５（抵抗）及びスイッチング素子２６（抵抗スイッチ）を直列に接続した直列接続体が、電源ライン１５ａ，１５ｂに並列に接続されている。スイッチング素子２６は、電界効果トランジスタ等で形成されている。また、電源ライン１５ａ，１５ｂには、電圧センサ２７が並列に接続されている。電圧センサ２７（電圧検出部）は、電源ライン１５ａ，１５ｂ間の電源電圧Ｖｂを検出する。

電源ライン１５ａ，１５ｂには、各インバータ回路３０が並列に接続されている。各インバータ回路３０（３相インバータ）には、ロボット１０における各関節（各回転部）を駆動する各モータＭが接続されている。各モータＭには、モータＭの回転を減速させる減速機（図示略）が接続されている。各モータＭには、回転位置を検出するエンコーダＥ（位置検出部）が設けられている。また、各モータＭに減速機を介して接続された出力軸（回転軸）には、出力軸の発生トルクを検出するトルクセンサＴｒが設けられている。各モータＭには各モータＭの回転位置を機械的に保持する機械ブレーキ（図示略）が設けられている。機械ブレーキは、電力が供給されることで保持を解除し、電力供給が停止されることで回転位置を保持する。

各インバータ回路３０は、６個のスイッチング素子ＳＷ１〜ＳＷ６を３相ブリッジ接続するとともに、各スイッチング素子と並列にフライホイールダイオード３２を接続した周知のものである。スイッチング素子ＳＷ１〜ＳＷ６（第１〜第６スイッチング素子）は、バイポーラ型パワートランジスタ等で形成されている。各インバータ回路３０は、各モータＭへ供給される電流を検出する電流センサ３５を備えている。

詳しくは、インバータ回路３０は、スイッチング素子ＳＷ１，ＳＷ４の直列接続体と、スイッチング素子ＳＷ２，ＳＷ５の直列接続体と、スイッチング素子ＳＷ３，ＳＷ６の直列接続体とが並列接続されたブリッジ回路である。そして、スイッチング素子ＳＷ１〜ＳＷ３は、電源１４の正極側（高電位側）と接続され、スイッチング素子ＳＷ４〜ＳＷ６は、電源１４の負極側（低電位側）及びＧＮＤと接続されている。さらに、直列接続体におけるスイッチング素子ＳＷ１とスイッチング素子ＳＷ２との間が、モータＭのＵ相の電力入出力端子に接続されている。同様に、直列接続体におけるスイッチング素子ＳＷ２とスイッチング素子ＳＷ５との間が、モータＭのＶ相の電力入出力端子に接続されており、直列接続体におけるスイッチング素子ＳＷ３とスイッチング素子ＳＷ６との間が、モータＭのＷ相の電力入出力端子に接続されている。

そして、各インバータ回路３０において、スイッチング素子ＳＷ１〜ＳＷ６の開閉がＰＷＭ制御されることにより、各インバータ回路３０から各モータＭへ供給される電圧が制御される。各モータＭへ供給される電流が電流センサ３５により検出される。

制御部６０は、電源制御部６１、軌道生成部６２、位置・速度制御部６３、電流制御部６４、ＰＷＭ回路６５、及びドライブ回路６６等を備えている。

各モータＭに設けられたエンコーダＥの出力は、軌道生成部６２、位置・速度制御部６３、及び電流制御部６４へそれぞれ入力される。軌道生成部６２は、ロボット１０が行う動作、及び各エンコーダＥにより検出された各モータＭの回転位置（各回転部の回転位置）に基づいて、ロボット１０の動作軌道（目標位置）及び各モータＭの目標回転位置（各回転部の目標回転位置）を算出する。位置・速度制御部６３は、軌道生成部６２により算出された各モータＭの目標回転位置、及び検出された各モータＭの回転位置に基づいて、各モータＭの目標回転速度（各回転部の目標回転速度）、及び各モータＭの目標トルクを算出する。電流制御部６４は、位置・速度制御部６３により算出された各モータＭの目標トルク、各電流センサ３５により検出された電流、並びに検出された各モータＭの回転位置に基づいて、インバータ回路３０からモータＭへ供給する目標電流を算出する。ＰＷＭ回路６５は、電流制御部６４により算出された目標電流に基づいて、各インバータ回路３０の各スイッチング素子ＳＷ１〜ＳＷ６の開閉を制御するＰＷＭ信号を出力する。ドライブ回路６６は、ＰＷＭ回路６５から出力されたＰＷＭ信号に基づいて、各インバータ回路３０の各スイッチング素子ＳＷ１〜ＳＷ６を開閉駆動する。これにより、各モータＭ（ロボット１０）を目標回転位置（目標位置）まで動作させるように、各モータＭの駆動が制御される。

コントローラ７０は、各モータＭへの電力供給を停止させ且つ各スイッチング素子２６を閉じる第１ダイナミックブレーキを実行可能である。具体的には、各モータＭが回転中に、各インバータ回路３０の各スイッチング素子ＳＷ１〜ＳＷ６を開き（オフに制御し）、且つスイッチング素子２２を開き、且つスイッチング素子２６を閉じる。これにより、電源１４から各モータＭへの電力供給が停止され、各モータＭにおいて各相の電力入出力端子間がフライホイールダイオード３２、回生抵抗２５、及びスイッチング素子２６を介して短絡される。その結果、各モータＭにおいて発電された電力が、各モータＭ及び各回生抵抗２５で熱として消費されるとともに、各モータＭに制動力が作用する。

また、コントローラ７０は、各モータＭへの電力供給を停止させ且つスイッチング素子ＳＷ４，ＳＷ５，ＳＷ６を閉じる第２ダイナミックブレーキを実行可能である。具体的には、各モータＭが回転中に、各インバータ回路３０のスイッチング素子ＳＷ４，ＳＷ５，ＳＷ６を閉じ（オンに制御し）、且つスイッチング素子２２を開く。なお、このときスイッチング素子２６を開き、各インバータ回路３０のスイッチング素子ＳＷ１，ＳＷ２，ＳＷ３を開く（オフに制御する）。これにより、電源１４から各モータＭへの電力供給が停止され、各モータＭにおいて各相の電力入出力端子間が６スイッチング素子ＳＷ４，ＳＷ５，ＳＷ６を介して短絡される。その結果、各モータＭに流れる電流が、各モータＭで熱として消費されるとともに、各モータＭに制動力が作用する。上記第１ダイナミックブレーキによる各モータＭ（ロボット１０）の制動力は、第２ダイナミックブレーキによる各モータＭの制動力よりも小さくなっている。

また、コントローラ７０は、ロボット１０が物体に衝突したことを検知した場合に、各モータＭを減速させるように駆動する減速制御を実行可能である。具体的には、ロボット１０の指令位置（目標位置）に現在のロボット１０の位置を設定し、指令位置に基づいて各モータＭの目標回転位置を算出し、各モータＭの目標回転位置及び検出された各モータＭの回転位置に基づいて、各モータＭの目標回転速度及び各モータＭの目標トルクを算出する。これにより、回転中の各モータＭを逆回転方向に駆動する減速トルクが発生し、動作中のロボット１０が急激に停止させられる（減速駆動）。ここで、コントローラ７０は、減速制御として、各モータＭの減速駆動に加えて、スイッチング素子２６を閉じるダイナミックブレーキを実行可能である。これにより、各モータＭにおいて各相の電力入出力端子間がフライホイールダイオード３２、回生抵抗２５、及びスイッチング素子２６を介して短絡される。その結果、各モータＭから流れる電流が各回生抵抗２５で熱として消費されるとともに、各モータＭに制動力が作用し、減速駆動のみによる制動力よりも大きな制動力を各モータＭ（ロボット１０）に作用させることができる。

図３は、ロボット１０が物体に衝突した時に実行される衝突時制御の手順を示すフローチャートである。この一連の処理は、ロボット１０を動作させる際に、コントローラ７０により実行される。

まず、ロボット１０の現在位置を取得する（Ｓ１１）。詳しくは、各エンコーダＥの検出値に基づいて、各モータＭの現在の回転位置を取得する。トルクセンサＴｒの検出値に基づいて、ロボット１０の各回転部が発生している現在トルクを取得する（Ｓ１２）。

続いて、ロボット１０の動作軌道に基づいて、ロボット１０の指令位置を計算する（Ｓ１３）。詳しくは、ロボット１０の動作軌道は、作業者によるティーチングや、動作プログラムにより予め設定されている。そして、動作軌道に沿ってロボット１０を動作させるように、各モータＭの指令位置（目標回転位置）を計算する。

続いて、ロボット１０を動作させる（Ｓ１４）。詳しくは、各モータＭの指令位置、及び各エンコーダＥにより検出された各モータＭの現在の回転位置に基づいて、各モータＭの指令速度（目標回転速度）及び指令加速度（目標加速度）を計算する。各モータＭの指令速度及び指令加速度に基づいて、各モータＭの指令トルク（目標トルク）を計算する。そして、各モータＭにより指令トルクを発生させるように、インバータ回路３０の各スイッチング素子ＳＷ１〜ＳＷ６を制御する。

続いて、ロボット１０の動作軌道に基づいて、ロボット１０の停止位置を計算する（Ｓ１５）。この停止位置は、動作軌道の終了位置、あるいは動作軌道の途中においてロボット１０を停止させる位置である。

続いて、現在トルクから指令トルクを引いた差が、危険トルクよりも小さいか否か判定する（Ｓ１６）。この危険トルクは、ロボット１０の安全基準等に基づいて設定されている。この判定において、現在トルクから指令トルクを引いた差が、危険トルクよりも小さいと判定した場合（Ｓ１６：ＹＥＳ）、ロボット１０の動作軌道に基づいてロボット１０の指令位置を更新する（Ｓ１７）。すなわち、ロボット１０が物体（作業者を含む）に衝突していないと判定した場合、ロボット１０の動作軌道に基づいてロボット１０の指令位置を更新する。

続いて、ロボット１０を停止させる必要があるか否か判定する（Ｓ１８）。詳しくは、Ｓ１１の処理で取得したロボット１０の現在位置がＳ１５の処理で計算したロボット１０の停止位置に一致した場合、又はロボット１０を停止させる停止指令がある場合に、ロボット１０を停止させる必要があると判定する。この判定において、ロボット１０を停止させる必要があると判定した場合（Ｓ１８：ＹＥＳ）、この一連の処理を一旦終了する（ＥＮＤ）。一方、この判定において、ロボット１０を停止させる必要がないと判定した場合（Ｓ１８：ＮＯ）、Ｓ１１の処理から再度実行する。

また、Ｓ１６の判定において、現在トルクから指令トルクを引いた差が、危険トルクよりも小さくないと判定した場合（Ｓ１６：ＮＯ）、ロボット１０の指令位置にロボット１０の現在位置を設定する（Ｓ１９）。すなわち、ロボット１０が物体に衝突したと判定した場合、ロボット１０の指令位置にロボット１０の現在位置を設定する。ロボット１０の現在位置は、例えばロボット１０の手先（制御点）の現在位置である。

続いて、タイマーをセットする（Ｓ２０）。すなわち、タイマーによる時間の計測を開始する。そして、タイマーで計測された時間（実行期間）が処理時間よりも長いか否か判定する（Ｓ２１）。この処理時間（判定期間）は、ロボット１０を停止させることが可能な時間に設定されており、固定値でもよいし、ロボット１０の速度に応じた可変値でもよい。例えば、ロボット１０の速度が高いほど、処理時間を長くする。

Ｓ２１の判定において、タイマーで計測された時間が処理時間よりも長くないと判定した場合（Ｓ２１：ＮＯ）、ロボット１０の停止軌道に基づいて、ロボット１０の指令位置を計算する（Ｓ２２）。詳しくは、ロボット１０の指令位置を、Ｓ１９の処理で設定された指令位置にする。そして、指令位置にロボット１０を停止（動作）させるように、各モータＭの指令位置（目標回転位置）を計算する。

続いて、ロボット１０を動作させる（Ｓ２３）。Ｓ２３の処理はＳ１４の処理と同一であるが、ここでは各モータＭの指令加速度（目標加速度）は負の加速度（減速度）となる。そして、各モータＭの指令トルク（目標トルク）は、負の加速トルク（減速トルク）となる。要するに、Ｓ２２，Ｓ２３の処理により、上述した各モータＭの減速駆動を実行する。さらに、上述したように、各モータＭの減速駆動に加えて、スイッチング素子２６を閉じるダイナミックブレーキを実行する。

続いて、ロボット１０の停止軌道に基づいて、ロボット１０の停止位置を計算する（Ｓ２４）。詳しくは、ロボット１０の停止位置を、Ｓ１９の処理で設定された指令位置にする。

続いて、ロボット１０が停止したか否か判定する（Ｓ２５）。詳しくは、計算された停止位置にロボット１０を停止させるために必要な制御が終了したか否か判定する。なお、ロボット１０の速度が所定速度よりも低くなったか否か、あるいはロボット１０の速度を所定速度よりも低くするための制御が終了したか否か判定してもよい。この判定において、ロボット１０が停止していないと判定した場合（Ｓ２５：ＮＯ）、Ｓ２１の処理から再度実行する。一方、この判定においてロボット１０が停止したと判定した場合（Ｓ２５：ＹＥＳ）、又はＳ２１の判定においてタイマーで計測された時間が処理時間よりも長いと判定した場合（Ｓ２１：ＹＥＳ）、指令位置を保存する（Ｓ２６）。具体的には、Ｓ１９の処理で設定された指令位置、すなわち衝突時のロボット１０の位置を保存する。

続いて、タイマーをセットする（Ｓ２７）。そして、タイマーで計測された時間が処理時間よりも長いか否か判定する（Ｓ２８）。この処理時間は、例えばロボット１０と設備との間に手（体の一部）を挟まれた作業者が手を抜くことが可能な時間に設定されており、固定値でもよいし、ロボット１０の速度や加速度に応じた可変値でもよい。例えば、ロボット１０の速度や加速度が高いほど、処理時間を長くする。

Ｓ２８の判定において、タイマーで計測された時間が処理時間よりも長くないと判定した場合（Ｓ２８：ＮＯ）、ダイナミックブレーキを実行する（Ｓ２９）。詳しくは、上述した第１ダイナミックブレーキを第１所定期間実行し、その後に第２ダイナミックブレーキを第２所定期間実行する。

続いて、ロボット１０の現在位置を取得する（Ｓ３０）。Ｓ３０の処理はＳ１１の処理と同一であるが、ここではダイナミックブレーキを実行中に作業者により押し動かされるロボット１０の現在位置が逐次取得される。ロボット１０の各回転部が発生している現在トルクを取得する（Ｓ３１）。Ｓ３１の処理はＳ１２の処理と同一であるが、ここではダイナミックブレーキを実行中に作業者がロボット１０に作用させている現在トルクが逐次取得される。その後、Ｓ２８の処理から再度実行する。

一方、Ｓ２８の判定において、タイマーで計測された時間が処理時間よりも長いと判定した場合（Ｓ２８：ＹＥＳ）、ロボット１０を退避させる退避軌道を設定する（Ｓ３２）。詳しくは、図４に示すように、Ｓ２９の処理によるダイナミックブレーキの実行中にロボット１０が移動させられた方向へ、ダイナミックブレーキが実行された後にロボット１０を所定距離移動させる退避軌道を設定する。さらに、Ｓ２９の処理によるダイナミックブレーキの実行中にロボット１０が移動させられた速度に基づいて、この所定距離を設定する。例えば、ロボット１０が移動させられた速度が高いほど、所定距離を長くする。ロボット１０が移動させられた速度は、Ｓ３０の処理で取得した複数の現在位置に基づいて計算することができる。

続いて、タイマーをセットする（Ｓ３３）。そして、タイマーで計測された時間が処理時間よりも長いか否か判定する（Ｓ３４）。この処理時間は、ロボット１０を退避軌道に沿って退避させることが可能な時間に設定されており、固定値でもよいし、ロボット１０の速度に応じた可変値でもよい。例えば、ロボット１０が移動させられた速度が高いほど上記所定距離が長くされる構成では処理時間を固定値とし、所定距離が固定値の構成ではロボット１０の速度が高いほど処理時間を短くする。

Ｓ３５〜Ｓ４１の処理はＳ１１〜Ｓ１７の処理と同一であるが、Ｓ３７の処理ではロボット１０の退避軌道に基づいて、ロボット１０の指令位置を計算する（Ｓ３７）。そして、Ｓ３７及びＳ３８の処理は、作業者により押された方向へロボット１０を退避させる退避制御である。また、Ｓ４０の判定において、現在トルクから指令トルクを引いた差が、危険トルクよりも小さくないと判定した場合（Ｓ４０：ＮＯ）、Ｓ２７の処理から再度実行する。

また、Ｓ３４の処理において、タイマーで計測された時間が処理時間よりも長いと判定した場合（Ｓ３４：ＹＥＳ）、ロボット１０の位置を衝突時の位置へ復帰させる復帰軌道を設定する（Ｓ４２）。詳しくは、図５に示すように、上記退避制御が実行された後のロボット１０の位置から、上述した減速制御が終了した時のロボット１０の位置まで復帰させる円弧状の復帰軌道を設定する。さらに、Ｓ２９の処理によるダイナミックブレーキの実行中にロボット１０が移動させられた速度に基づいて、この復帰軌道を設定する。例えば、ロボット１０が移動させられた速度が高いほど、復帰軌道の曲率半径を大きくする。

Ｓ４３〜Ｓ５０の処理はＳ１１〜Ｓ１８の処理と同一であるが、Ｓ４５の処理ではロボット１０の復帰軌道に基づいて、ロボット１０の指令位置を計算する（Ｓ４５）。

Ｓ４８の判定において、現在トルクから指令トルクを引いた差が、危険トルクよりも小さくないと判定した場合（Ｓ４８：ＮＯ）、ロボット１０の動力を遮断する（Ｓ５１）。具体的には、ロボット１０への電力供給を停止する。これにより、各モータＭに設けられた機械ブレーキによって、各モータＭの回転位置が保持され、ロボット１０の位置（姿勢）が保持される。その後、この一連の処理を終了する（ＥＮＤ）。

なお、Ｓ１６の処理が衝突検知部としての処理に相当し、Ｓ２２及びＳ２３の処理が第１制御部としての処理に相当し、Ｓ２９の処理が第２制御部としての処理に相当し、Ｓ３７及びＳ３８の処理が第３制御部としての処理に相当し、Ｓ４５及びＳ４６の処理が第４制御部としての処理に相当する。

以上詳述した本実施形態は、以下の利点を有する。

・ロボット１０が作業者（物体）に衝突したことが検知された場合に、各モータＭを減速させるように駆動する減速制御が実行される。すなわち、ロボット１０が物体に衝突した場合に、各モータＭを加速させる加速トルクを０にしたり、各モータＭの回転速度を維持する維持トルクを０にしたりするのではなく、各モータＭを減速させるように駆動する。このため、ロボット１０が作業者に衝突した場合に、ロボット１０を速やかに減速させることができ、作業者が負傷することを抑制することができる。

・減速制御が実行された後、各モータＭへの電力供給を停止させ且つ各モータＭにおいて電力入出力端子間を短絡させるダイナミックブレーキが所定期間実行される。このため、作業者は、ダイナミックブレーキが実行されている所定期間中に、ロボット１０を押して動かしたり、ロボット１０から離れたりすることができる。さらに、所定期間中は、ロボット１０の位置が衝突時の位置に制御されることがない。したがって、ロボット１０が作業者に再衝突することを抑制することができる。しかも、ダイナミックブレーキの実行中は、作業者がロボット１０を押して動かす際に抵抗が生じるため、ロボット１０が急激に動かされることを抑制することができる。

・ロボット１０の速度が所定速度よりも低くなるまで、又は減速制御の実行時間が処理時間よりも長くなるまで、減速制御が実行されるため、ロボット１０を確実に減速させることができる。

・ロボット１０のコントローラ７０は、各回生抵抗２５を介して各モータＭにおいて電力入出力端子間を開閉する各スイッチング素子２６を備えている。このため、各スイッチング素子２６を閉じることにより、各回生抵抗２５を介して各モータＭにおいて電力入出力端子間を短絡させるダイナミックブレーキを実行することができる。そして、減速制御として、各モータＭが減速するように駆動され、且つ各スイッチング素子２６が閉じられる。このため、各モータＭの減速駆動に加えて、ダイナミックブレーキを実行することができ、ロボット１０が作業者に衝突した場合に、ロボット１０を更に速やかに減速させることができる。

・電源１４と各モータＭとの間に並列接続されたコンデンサ２１により、電源１４から供給される電圧が平滑化される。ここで、各モータＭへの電力供給を停止させ且つスイッチング素子ＳＷ４，ＳＷ５，ＳＷ６を閉じることで、各モータＭにおいて電力入出力端子間を短絡させるダイナミックブレーキを実行すると以下の問題が生じる。すなわち、電源１４から各モータＭへの電力供給を停止させたとしても、コンデンサ２１に電荷が残っている。そして、スイッチング素子ＳＷ４，ＳＷ５，ＳＷ６を閉じるダイナミックブレーキでは、コンデンサ２１に残った電荷を放電させることができず、コンデンサ２１に残った電荷がその後に放電されるおそれがある。この点、ダイナミックブレーキとして、各モータＭへの電力供給を停止させ且つ各スイッチング素子２６を閉じる第１ダイナミックブレーキが実行される。このため、第１ダイナミックブレーキによりロボット１０を速やかに減速させるとともに、コンデンサ２１に残っている電荷を回生抵抗２５により熱として消費させることができる。特に、減速制御において、既に各スイッチング素子２６が閉じられているため、制御を簡素化することができる。

・各スイッチング素子２６を閉じる第１ダイナミックブレーキによるロボット１０の制動力は、スイッチング素子ＳＷ４，ＳＷ５，ＳＷ６を閉じる第２ダイナミックブレーキによるロボット１０の制動力よりも小さい。このため、第１ダイナミックブレーキの実行中は、第２ダイナミックブレーキの実行中と比較して、作業者がロボット１０を押して動かし易くなる。その結果、例えば作業者がロボット１０と物体との間に挟まれた状況において、作業者がロボット１０と物体との間から離脱し易くなる。しかしながら、第１ダイナミックブレーキを所定期間継続すると、ロボット１０と衝突して慌てた作業者により強く押されることで、ロボット１０が動き過ぎるおそれがある。その場合、作業者によりはねのけられたロボット１０が、物体に衝突したり、他の作業者に衝突したりするおそれがある。この点、所定期間において、第１ダイナミックブレーキが実行された後に、各モータＭへの電力供給を停止させ且つスイッチング素子ＳＷ４，ＳＷ５，ＳＷ６を閉じる第２ダイナミックブレーキが実行される。したがって、第１ダイナミックブレーキによりコンデンサ２１に残った電荷を消費させた後に、第１ダイナミックブレーキよりも制動力の大きい第２ダイナミックブレーキが実行されるため、作業者に押されてロボット１０が動き過ぎることを抑制することができる。ひいては、作業者によりはねのけられたロボット１０が、物体に衝突したり、他の作業者に衝突したりする二次災害を抑制することができる。

・ダイナミックブレーキが実行された後に、ロボット１０の位置が直ちに衝突時の位置に制御されると、ロボット１０が作業者に再衝突するおそれがある。この点、ダイナミックブレーキの実行中にロボット１０が移動させられた方向へ、ダイナミックブレーキが実行された後にロボット１０を所定距離移動させる退避制御が実行される。このため、衝突時の位置からロボット１０を一旦退避させることができ、ロボット１０が作業者に再衝突することを抑制することができる。

・ダイナミックブレーキの実行中にロボット１０が移動させられた速度は、ロボット１０を衝突時の位置から作業者が遠ざける必要性の高さを反映している可能性が高い。この点、ダイナミックブレーキの実行中にロボット１０が移動させられた速度に基づいて、退避制御における所定距離が設定される。したがって、ロボット１０を衝突時の位置から作業者が遠ざける必要性の高さに応じて、退避制御におけるロボット１０を移動させる距離を適切に設定することができる。

・退避制御が実行された後にロボット１０の位置から、直線的な軌道により衝突時のロボット１０の位置まで制御されると、作業者の体が衝突時の位置に残っていた場合に、ロボット１０が作業者に再衝突するおそれがある。この点、退避制御が実行された後のロボット１０の位置から、減速制御が終了した時のロボット１０の位置まで、円弧状の復帰軌道によりロボット１０の位置が制御される。したがって、作業者の体が衝突時の位置に残っていたとしても、ロボット１０が作業者に再衝突することを抑制することができる。

・ダイナミックブレーキの実行中にロボット１０が移動させられた速度に基づいて、円弧状の復帰軌道の曲率半径が設定される。このため、ダイナミックブレーキの実行中にロボット１０が移動させられた速度に応じて、円弧状の復帰軌道を適切に設定することができる。

上記実施形態を、以下のように変更して実施することもできる。

・電流センサ３５により検出される電流が閾値を超えて変動したことに基づいて、ロボット１０が作業者（物体）に衝突したと判定することもできる。

・Ｓ２２及びＳ２３の処理として、コントローラ７０（第１制御部）は、各スイッチング素子２６を閉じるダイナミックブレーキを実行せず、各モータＭの減速駆動のみを実行してもよい。すなわち、減速制御として、各モータＭを減速させるように駆動し（減速駆動）、且つ各スイッチング素子２６を開く。この場合であっても、各モータＭの減速駆動が実行され、ロボット１０が作業者に衝突した場合に、ロボット１０を速やかに減速させることができる。

・Ｓ２９の処理として、コントローラ７０（第２制御部）は、第１ダイナミックブレーキのみを所定期間実行する、又は第２ダイナミックブレーキのみを所定期間実行するようにしてもよい。

・ダイナミックブレーキの実行中にロボット１０が移動させられた距離は、ロボット１０を衝突時の位置から作業者が遠ざける必要性の高さを反映している可能性が高い。そこで、コントローラ７０（第３制御部）は、ダイナミックブレーキの実行中にロボット１０が移動させられた距離に基づいて、退避制御における所定距離を設定してもよい。こうした構成によれば、ロボット１０を衝突時の位置から作業者が遠ざける必要性の高さに応じて、退避制御におけるロボット１０を移動させる距離を適切に設定することができる。

・ダイナミックブレーキの実行中にロボット１０が移動させられた加速度は、ロボット１０を衝突時の位置から作業者が遠ざける必要性の高さを反映している可能性が高い。そこで、コントローラ７０（第３制御部）は、ダイナミックブレーキの実行中にロボット１０が移動させられた加速度に基づいて、退避制御における所定距離を設定してもよい。こうした構成によれば、ロボット１０を衝突時の位置から作業者が遠ざける必要性の高さに応じて、退避制御におけるロボット１０を移動させる距離を適切に設定することができる。また、コントローラ７０は、ダイナミックブレーキの実行中にロボット１０が移動させられた距離、速度、及び加速度の少なくとも１つに基づいて、退避制御における所定距離を設定してもよい。

・コントローラ７０（第４制御部）は、退避制御が実行された後のロボット１０の位置から、減速制御が終了した時のロボット１０の位置まで、直線状の軌道によりロボット１０の位置を制御することもできる。その場合であっても、ダイナミックブレーキが実行された後にロボット１０を所定距離移動させる退避制御が実行されるため、衝突時の位置からロボット１０を一旦退避させることができ、ロボット１０が作業者に再衝突することを抑制することができる。

・各インバータ回路３０と各モータＭとの間において、各モータＭの電力入出力端子間を短絡させるように、抵抗とスイッチング素子との直列接続体を接続し、このスイッチング素子を閉じることで、ダイナミックブレーキを実行することもできる。

・ロボット１０として、水平多関節型ロボットを採用することもできる。

１０…ロボット、１４…電源、２１…コンデンサ（平滑コンデンサ）、２２…スイッチング素子、２５…回生抵抗（抵抗）、２６…スイッチング素子（抵抗スイッチ）、３０…インバータ回路（３相インバータ）、６０…制御部、７０…コントローラ（制御装置）、Ｍ…モータ、Ｔｒ…トルクセンサ。

Claims (12)

1. 各関節が各モータにより駆動されるロボットを制御する制御装置であって、
   前記ロボットが物体に衝突したことを検知する衝突検知部と、
   前記衝突検知部により前記ロボットが物体に衝突したことが検知された場合に、前記各モータを減速させるように駆動する減速制御を実行する第１制御部と、
   前記第１制御部により前記減速制御が実行された後、電源から前記各モータへの電力供給を停止させ且つ前記各モータにおいて電力入出力端子間を短絡させるダイナミックブレーキを所定期間実行して前記ロボットが移動させられることを許容する第２制御部と、
   を備えるロボットの制御装置。
2. 前記第１制御部は、前記ロボットの速度が所定速度よりも低くなるまで、又は前記減速制御の実行期間が判定期間よりも長くなるまで、前記減速制御を実行する請求項１に記載のロボットの制御装置。
3. 各抵抗を介して前記各モータにおいて前記電力入出力端子間を開閉する各抵抗スイッチを備え、
   前記第１制御部は、前記減速制御として、前記各モータを減速させるように駆動し、且つ前記各抵抗スイッチを閉じる請求項１又は２に記載のロボットの制御装置。
4. 各抵抗を介して前記各モータにおいて前記電力入出力端子間を開閉する各抵抗スイッチを備え、
   前記第１制御部は、前記減速制御として、前記各モータを減速させるように駆動し、且つ前記各抵抗スイッチを開く請求項１又は２に記載のロボットの制御装置。
5. 第１スイッチング素子と第４スイッチング素子との直列接続体と、第２スイッチング素子と第５スイッチング素子との直列接続体と、第３スイッチング素子と第６スイッチング素子との直列接続体と、が並列接続されたブリッジ回路を有し、前記第１，第２，第３スイッチング素子が前記電源の高電位側に接続され、前記第４，第５，第６スイッチング素子が前記電源の低電位側に接続され、各直列接続体における２つの前記スイッチング素子の間が前記モータのＵ相，Ｖ相，Ｗ相にそれぞれ接続された３相インバータと、
   前記電源と前記３相インバータとの間に並列接続された平滑コンデンサと、を備え、
   前記第２制御部は、前記ダイナミックブレーキとして、前記各モータへの電力供給を停止させ且つ前記各抵抗スイッチを閉じる第１ダイナミックブレーキを実行する請求項３又は４に記載のロボットの制御装置。
6. 前記第２制御部は、前記ダイナミックブレーキとして、前記第１ダイナミックブレーキを実行した後に、前記各モータへの電力供給を停止させ且つ前記第４，第５，第６スイッチング素子を閉じる第２ダイナミックブレーキを実行する請求項５に記載のロボットの制御装置。
7. 前記第２制御部による前記ダイナミックブレーキの実行中に前記ロボットが移動させられた方向へ、前記第２制御部により前記ダイナミックブレーキが実行された後に前記ロボットを所定距離移動させる退避制御を実行する第３制御部を備える請求項１〜６のいずれか１項に記載のロボットの制御装置。
8. 前記第３制御部は、前記第２制御部による前記ダイナミックブレーキの実行中に前記ロボットが移動させられた距離に基づいて、前記退避制御における前記所定距離を設定する請求項７に記載のロボットの制御装置。
9. 前記第３制御部は、前記第２制御部による前記ダイナミックブレーキの実行中に前記ロボットが移動させられた速度に基づいて、前記退避制御における前記所定距離を設定する請求項７又は８に記載のロボットの制御装置。
10. 前記第３制御部は、前記第２制御部による前記ダイナミックブレーキの実行中に前記ロボットが移動させられた加速度に基づいて、前記退避制御における前記所定距離を設定する請求項７〜９のいずれか１項に記載のロボットの制御装置。
11. 前記第３制御部により前記退避制御が実行された後の前記ロボットの位置から、前記第１制御部による前記減速制御が終了した時の前記ロボットの位置まで、円弧状の軌道により前記ロボットの位置を制御する第４制御部を備える請求項７〜１０のいずれか１項に記載のロボットの制御装置。
12. 前記第４制御部は、前記第２制御部による前記ダイナミックブレーキの実行中に前記ロボットが移動させられた速度に基づいて、前記円弧状の軌道の曲率半径を設定する請求項１１に記載のロボットの制御装置。

Images