JP5896058B2 - 制御装置、制御方法及びロボット - Google Patents

Description

本発明は制御装置及び制御方法に関し、特にロボットの衝突時における制御方法及び当該制御方法を用いる制御装置に関する。また、当該制御方法を利用するロボットに関する。

通常、ロボット制御やメカトロニクス制御の分野では、メカ振動が励起されることを防ぐためにローパスフィルタ（ＬＰＦ）を介してモータアンプへのトルク指令が行われる。また、力やトルクの検出信号にもノイズ除去のためにＬＰＦが用いられることがある。

特許文献１には、外乱負荷トルクの信号に対してローパスフィルタ演算を行うことで動作補償を行う技術が開示されている。特許文献１の制御装置は、ロボットマニピュレータを駆動するサーボモータの電流値とサーボモータの回転速度とを検出し、モータ電流値とモータ回転速度からロボットマニピュレータに係る外乱負荷トルクを検出する。そして、当該制御装置は、検出した外乱負荷トルクの信号のうち周波数の低い信号のみ透過するローパスフィルタ演算を行って動作補償を行い、動作補償演算結果からロボットマニピュレータの位置指令値を算出してコンプライアンス動作を行う。

当該構成とすることで、外乱負荷トルクの信号の中に周波数が高い雑音信号が入っても誤動作でコンプライアンス動作しないという効果を得ることができる。

特開平１１−０４２５７６号公報

背景技術に関するＬＰＦは、図７に示すように、トルク指令の方向について特性が変わらないため、衝突によって生じる押し付け力又は押し付けトルクを増やす方向にも減らす方向にも応答が同一となる。

このように、トルク指令に対する応答がトルク指令方向に依存しないため、衝突によって生じる押し付け力又は押し付けトルクを速やかに減らすことができなかった。

上記課題を鑑み、本発明は、未知の環境への衝突が生じるような場合において、衝突によって生じる押し付け力又は押し付けトルクを速やかに減らすことを可能とする制御装置及び制御方法並びにロボットを提供することを目的とする。

本発明の一態様である制御装置は、マニピュレータを駆動する駆動手段に対して前記駆動を指令する駆動指令信号を生成する生成手段と、前記生成手段で生成された前記駆動指令信号の第１遮断周波数より高い周波数帯域を減衰又は遮断する第１フィルタと、前記マニピュレータに生じる所定の物理量を検出する検出手段における検出結果に基づいて前記マニピュレータに衝突が発生したかを判断する衝突判断手段と、前記衝突の方向を判断する衝突方向判断手段と、前記衝突の方向に基づいて前記第１フィルタの前記第１遮断周波数を設定する設定手段と、を具備する。

また、本発明の一態様である制御方法は、マニピュレータに生じる所定の物理量を検出する検出手段における検出結果に基づいて前記マニピュレータに衝突が発生したかを判断する衝突判断ステップと、前記衝突が発生したと判断された場合に前記衝突の方向を判断する衝突方向判断ステップと、前記衝突の方向に基づいて第１遮断周波数を設定する設定ステップと、マニピュレータを駆動する駆動手段に対して前記駆動を指令する駆動指令信号を生成する生成ステップと、前記生成ステップで生成された前記駆動指令信号の前記設定ステップで設定された前記第１遮断周波数より高い周波数帯域を減衰又は遮断する第１フィルタステップと、を有する。

また、本発明の一態様であるロボットは、マニピュレータと、前記マニピュレータを駆動する駆動手段と、前記駆動手段に対して前記駆動を指令する駆動指令信号を生成する生成手段と、前記生成手段で生成された前記駆動指令信号の第１遮断周波数より高い周波数帯域を減衰又は遮断する第１フィルタと、前記マニピュレータに生じる所定の物理量を検出する検出手段と、前記検出手段における検出結果に基づいて前記マニピュレータに衝突が発生したかを判断する衝突判断手段と、前記衝突の方向を判断する衝突方向判断手段と、前記衝突の方向に基づいて前記第１フィルタの前記第１遮断周波数を設定する設定手段と、を具備する。

本発明によれば、未知の環境への衝突が生じるような場合において、衝突によって生じる押し付け力又は押し付けトルクを速やかに減らすことを可能とすることができる。

実施の形態１に係るロボットの（a）力センサによる衝突検出、（ｂ）トルクセンサによる衝突検出、の場合を説明する図である。 実施の形態１に係る制御装置の構成を示すブロック図である。 実施の形態１に係る制御装置の動作の流れを示すフローチャート図である。 実施の形態２に係るロボットの全体構成を示すブロック図である。 実施の形態２に係る制御装置の動作の流れを示すフローチャート図である。 実施の形態２に係るローパスフィルタのフィルタ特性の一例を示す図である。 従来のローパスフィルタのフィルタ特性の一例を示す図である。

本発明の実施の形態について以下に図面を参照して説明する。以下の説明は、本発明の好適な実施の形態を示すものであって、本発明の範囲が以下の実施の形態に限定されるものではない。以下の説明において、同一の符号が付されたものは実質的に同様の内容を示している。

（実施の形態１）
以下、図面を参照して本発明の実施の形態について説明する。図１は、本実施の形態１に係るロボット１０においてアーム１２が障害物に衝突する様子を示した図である。

モータ１１は、後述する制御装置２０からの駆動指令信号に基づいてアーム１２を駆動する。

アーム１２は、モータ１１に関節軸１３を介して接続されている。なお、図１（ａ）は、アーム１２に力センサ１４が設置されている場合を示しており、図１（ｂ）は、関節軸１３にトルクセンサ１５が設置されている場合を示している。

力センサ１４は、アーム１２に加わる外力を検出することで障害物に衝突したことを検出する。また、トルクセンサ１５は、関節軸１３に生じるトルクを検出することで障害物に衝突したことを検出する。

以下の説明では、アーム１２に関節軸１３を含めた全体部分をマニピュレータと称して説明する。また、本明細書で用いるマニピュレータには、ロボットのアーム部分の他、脚に該当するレッグ部分や、首に該当するヘッド部分など、モータ等の駆動装置に接続された様々な形態の可動部を含むものとする。

また、以下の説明では、力センサ１４又はトルクセンサ１５を纏めて、マニピュレータにかかる所定の物理量を検出する検出手段であるとして、センサ１６と称して説明する。

また、以下の説明では、図１に示した反時計方向の回転を正方向と定義し、この方向に回転した時に障害物に衝突して発生する力／トルクの方向を正方向と定義して説明する。

また、本明細書で用いるロボットには、産業用ロボットのように、ある程度自律的に連続した自動作業を行う機械や、人型ロボットや動物型ロボットのように人間や動物に近い形および機能を持つ機械などを含むものとする。

次に、ロボット１０の制御系について説明する。図２は、ロボット１０を制御する制御装置２０の構成を示すブロック図である。制御装置２０は、マニピュレータであるアーム１２を駆動する駆動手段であるモータ１１に対して駆動指令を行うための駆動指令信号を出力し、センサ１６より測定する物理量の測定値である検出信号を入力する。また、外部より動作指令信号を入力し、当該動作指令信号に基づいて対応する駆動指令信号を生成して出力する。ここでは、制御装置２０は、具体的にセンサ１６より力又はトルクの検出結果である検出信号を入力する。

制御装置２０は、駆動指令信号生成部２１と、第１フィルタ部２２と、衝突判断部２３と、衝突方向判断部２４と、フィルタ設定部２５と、を具備する。

駆動指令信号生成部２１は、マニピュレータを駆動するモータ１１に対して駆動を指令する駆動指令信号を生成する。具体的には、駆動指令信号生成部２１は、トルクを発生させるトルク指令信号を出力する。モータ１１は、トルク指令信号に基づいてトルクを発生する。

ここで、駆動指令信号生成部２１は、制御モードに応じて駆動指令信号を生成する。制御モードには、通常動作時における通常モードに加えて、衝突発生時における衝突対処モードがある。当該制御モードは、衝突判断部１１４からの制御モード切り替え指示に基づいて、通常モードから衝突対処モードに切り替えられる。

第１フィルタ部２２は、駆動指令信号生成部２１で生成された駆動指令信号の第１遮断周波数（第１カットオフ周波数）より高い周波数帯域を減衰又は遮断し、当該カットオフ周波数より低い周波数帯域を透過する。第１フィルタ部２２は、高周波成分を取り除いたフィルタ後の駆動指令信号を出力する。

衝突判断部２３は、センサ１６より入力する検出信号に基づいてアーム１２に衝突が発生したかを判断する。例えば、アーム１２が障害物に衝突することによって生じる力やトルクをセンサ１６が検出し、衝突判断部２３が、当該センサ１６からの検出信号と通常動作時における力やトルクの値とを比較することで衝突の有無を判断する。

衝突判断部２３は、衝突が発生したと判断した場合に、駆動指令信号生成部２１における制御モードを通常モードから衝突対処モードに切り替える。衝突判断部２３より制御モード切り替え指示信号を入力した駆動指令信号生成部２１は、制御モードを通常モードから衝突対処モードに切り替える。

衝突方向判断部２４は、センサ１６からの検出信号に基づいて衝突の方向を判断する。ここで、衝突方向判断部２４は、センサ１６からの検出信号の正負の値に応じて衝突の方向の正負を判断する。

フィルタ設定部２５は、衝突の方向に基づいて第１フィルタ部２２の第１遮断周波数を設定する。衝突方向判断部２４で衝突方向が正方向と判断された場合は、フィルタ設定部２５は、第１フィルタ部２２の正方向の遮断周波数を低く設定し、負方向の遮断周波数を高く設定する。一方、衝突方向判断部２４で衝突方向が負方向と判断された場合は、フィルタ設定部２５は、第１フィルタ部２２の正方向の遮断周波数を高く設定し、負方向の遮断周波数を低く設定する。このようにフィルタを設定することで、迅速に衝突状態から離脱することができる。

次に、制御装置２０の動作について説明する。図３は、制御装置２０の動作の流れを示すフローチャート図である。

制御周期が開始されると、衝突判断部２３は、センサ１６より入力する検出信号に基づいてマニピュレータに衝突が発生したかを判断する（ステップＳ１０１）。

衝突が発生していないと判断した場合（ステップＳ１０１でＮｏ）、衝突判断部２３は、駆動指令信号生成部２１の制御モードを通常モードに維持し、駆動指令信号生成部２１は、通常モードでトルク制御等の各種制御を行うための駆動指令信号を生成する（ステップＳ１０２）。ステップＳ１０２で生成される駆動指令信号は、第１フィルタ２２で第１遮断周波数より大きい周波数成帯域を減衰又は削減するフィルタ処理が行われた後に出力される（ステップＳ１０３）。

一方、衝突が発生したと判断した場合（ステップＳ１０１でＹｅｓ）、衝突判断部２３は、駆動指令信号生成部２１の制御モードを衝突対処モードに切り替える（ステップＳ１０４）。

続いて、衝突方向判断部２４は、上記検出信号で示される力又はトルクの検出値の正負より衝突方向を判断する（ステップＳ１０５）。

次に、フィルタ設定部２５は、衝突方向判断部２４で判断された衝突方向に基づいて第１フィルタ２２の第１遮断周波数を設定する（ステップＳ１０６）。

次に、ステップＳ１０４で衝突対処モードに切り替えられた駆動指令信号生成部２１は、衝突状態を離脱するための制御を行うための駆動指令信号を生成する（ステップＳ１０７）。図１の形態においては、モータ１１によって駆動される関節軸１３まわりの回転動作によってアーム１２に衝突が発生する。従って、駆動指令信号生成部２１は、具体的に衝突状態を離脱するためのトルク制御を行うために、トルク指令信号を生成する。

第１フィルタ２２は、ステップＳ１０７で生成されたトルク指令信号に対して、ステップＳ１０６において設定された第１遮断周波数より大きい周波数帯域を減衰又は遮断する処理を行う（ステップＳ１０８）。

モータ１１は、ステップＳ１０３又はステップＳ１０８で処理されたフィルタ後の駆動指令信号に基づいてアーム１２を駆動する。

以上のように本実施の形態１に係る制御装置によれば、衝突が発生した場合に、衝突対象からマニピュレータが離れる方向に駆動させる指令信号の高周波成分が、通常モードにおける指令信号の高周波成分と比べて残るように第１フィルタの遮断周波数を高く設定する手段を備えることを特徴とする。当該構成とすることで、衝突が検知された場合に、障害対象より離れる方向への駆動指令が駆動手段に速やかに伝わるため、速やかに衝突状態から離脱することができる。

また、衝突判断部における衝突判断はフィルタ前の検出信号を用いて行い、駆動指令信号生成部における駆動指令信号の生成は、フィルタ後の検出信号に基づいて行われることで、適切な衝突状態からの離脱制御を実現することができる。

（実施の形態２）
本実施の形態２に係る制御装置は、駆動指令信号に対する遮断周波数に加えて、センサより入力する検出信号に対する遮断周波数を衝突の方向に基づいて制御することを特徴とする。

本実施の形態２では、具体的に、駆動指令としてトルク指令を行い、検出手段において力又はトルクを検出するものとして説明する。また、駆動手段は、具体的にモータであり、マニピュレータはアームであるとして説明する。但し、本実施の形態２は、当該構成に限定されるものではない。以下、図面を参照して説明する。但し、実施の形態１で既に説明した部分については、発明明確化のため一部説明を省略する。

図４は、本実施の形態２に係るロボット１００の構成を示すブロック図である。ロボット１００は、制御装置（コントローラ）１１０と、モータアンプ１２０と、モータ１３０と、エンコーダ１４０と、アーム１５０と、センサ１６０と、を具備する。

制御装置１１０は、制御モードに応じて、位置制御、速度制御、トルク制御、力制御等の各種制御を行う。制御装置１１０は、少なくとも通常モードと衝突対処モードの２つの制御モードを有しており、制御モードに応じた制御を行う。

モータアンプ１２０は、制御装置１１０より出力されるフィルタ後のトルク指令信号に応じてモータ１３０の電流を制御することで指令に応じたトルクを発生させる。

モータ１３０は、モータアンプ１２０によって駆動され、トルクを発生する。

エンコーダ１４０は、モータ１３０の回転に基づいてアーム１５０の現在位置を求め、位置信号として制御装置１１０に出力する。

アーム１５０は、モータ１３０に関節軸を介して結合され、外部環境との力のやり取りを行う。

センサ１６０は、アーム１５０と外部環境との間で生じる力又はトルクを検出する。なお、センサ１６０は、当該力を検出する力センサ及びトルクを検出するトルクセンサの２つのセンサから構成されていても良い。センサ１６０で検出された力又はトルクに関する信号は、検出信号として制御装置１１０に出力される。

次に、制御装置１１０について詳しく説明する。制御装置１１０は、駆動指令信号生成部１１１と、第１フィルタ１１２と、第２フィルタ１１３と、衝突判断部１１４と、衝突方向判断部１１５と、フィルタ設定部１１６と、を具備する。第２フィルタ１１３以外は、それぞれ実施の形態１における駆動指令信号生成部２１、第１フィルタ２２、衝突判断部２３、衝突方向判断部２４、フィルタ設定部２５、と略同一の機能を有しているため一部説明を省略する。

駆動指令信号生成部１１１は、外部より入力する動作指令信号、エンコーダ１４０より入力する位置信号、第２フィルタ部１１３より入力するフィルタ後検出信号等に基づいて、次の制御周期で行う各種制御を行うためのトルク指令信号を生成する。ここで、駆動指令信号生成部１１１は、衝突判断部１１４より制御モード切り替え指示信号を入力した場合は、制御モードを通常の各種制御を行う通常モードから衝突対処モードに変更する。

駆動指令信号生成部１１１は、衝突対処モードでは、衝突が発生した場合に衝突状態から離脱するための所定のアルゴリズムに従って、トルク指令信号を生成する。

第１フィルタ部１１２及び第２フィルタ部１１３は、それぞれフィルタ設定部１１６によって個別に設定される遮断周波数でフィルタ処理を行う。第１フィルタ部１１２は、フィルタ後のトルク指令信号をモータアンプ１２０へ出力する。第２フィルタ部１１３は、フィルタ後の検出信号を駆動指令信号生成部１１１に出力する。

第１フィルタ部１１２は、トルク指令信号に対して第１遮断周波数以下の周波数帯域を透過させるフィルタであるため、以下の説明ではトルク指令ＬＰＦ（トルク指令ローパスフィルタ）と称することがある。また、第２フィルタ部１１３は、検出信号に対して第２遮断周波数以下の周波数帯域を透過させるフィルタであるため、以下の説明では検出信号ＬＰＦ（検出信号ローパスフィルタ）と称することがある。

衝突判断部１１４は、センサ１６０からの検出信号に基づいてアーム１５０と外部環境との間の衝突の有無を判断して制御モードを通常モード又は衝突対処モードに切り替える。ここで、衝突判断部１１４は、第２フィルタ部１１３でフィルタ処理される前の検出信号で上記衝突判断を行う。従って、駆動指令信号生成部１１１は、フィルタ前検出信号に基づいて制御モードの切り替えが行われ、フィルタ後検出信号に基づいて、制御モードに応じたトルク指令信号を生成する。

衝突方向判断部１１５は、衝突判断部１１４で衝突が検知された場合に、フィルタ前検出信号の値の正負から衝突方向を判断し、判断結果をフィルタ設定部１１６に出力する。

フィルタ設定部１１６は、衝突方向判断部１１５における判断結果に基づいて第１フィルタ部１１２の第１遮断周波数と、第２フィルタ部１１３の第２遮断周波数をそれぞれ設定する。ここで、フィルタ設定部１１６は、第１遮断周波数と第２遮断周波数を同一の周波数に設定することが可能であるが、個別に最適な周波数をそれぞれ選択して設定することが好ましい。

続いて、ロボット１００における制御動作について説明する。図５は、ロボット１００の制御動作の流れを示すフローチャート図である。

制御周期が開始されると、衝突判断部１１４は、センサ１６０より入力する検出信号に基づいてマニピュレータに衝突が発生したかを判断する（ステップＳ２０１）。

ここで、衝突判断部１１４は、一例として、第２フィルタ部１１３を通さずにセンサ１６０より直接入力する検出信号（力／トルクの検出値）と、通常モードにおいて出すべき力／トルクの値（想定検出基準値）との比較を行うことで衝突が発生した否かの判断を行う。

上記検出信号が示す値と基準値との比較の結果、検出値が大きければ、衝突判断部１１４は、衝突が発生したものと判断し（ステップＳ２０１でＹｅｓ）、衝突対処モード時の処理フローであるステップＳ２０４〜ステップＳ２１０の処理に進む。一方、上記検出信号が示す値と基準値との比較の結果、検出値が基準値と略同一の値であるか、又は、当該基準値以下である場合は、衝突判断部１１４は、衝突は発生していないと判断し（ステップＳ２０１でＮｏ）、通常モード時の処理フローであるステップＳ２０２〜ステップＳ２０３の処理に進む。

ステップＳ２０１で衝突が発生していないと判断された場合、駆動指令信号生成部１１１は、通常モードで駆動指令信号を生成する（ステップＳ２０２）。ここでは、駆動指令信号生成部１１１は、外部より入力する動作指令信号で指示される動作内容を行うために現在発生させるべきトルク値を、エンコーダ１４０より入力する位置信号、第２フィルタ部１１３より入力するフィルタ後検出信号、などの信号（情報）に基づいて算出し、トルク指令信号を生成する。また、駆動指令信号生成部１１１は、トルク制御を行うために生成するトルク指令信号以外にも、速度制御、位置制御、力制御、等を行うための各種駆動指令信号を生成することができる。

第１フィルタ部１１２は、ステップＳ２０２で生成された駆動指令信号（トルク指令信号）に対して、低周波数領域を透過させるフィルタ処理を行い、フィルタ後のトルク指令信号をモータアンプ１２０へ出力する（ステップＳ２０３）。ここで、第１フィルタ部１１２は、フィルタ設定部１１６によって設定される遮断周波数に従ってフィルタ処理を行う。

一方、ステップＳ２０１で衝突が発生したと判断された場合、駆動指令信号生成部１１１は、制御モードを通常モードから衝突対処モードに切り替える（ステップＳ２０４）。

具体的には、衝突判断部１１４は、ステップＳ２０１で衝突が発生したと判断した場合、制御モードの切り替え指示信号を駆動指令信号生成部１１１に出力し、駆動指令信号生成部１１１は、当該制御モード切り替え指示信号に基づいて、制御モードを通常モードから衝突対処モードに切り替える。

次に、衝突方向判断部１１５は、衝突方向を判断する（ステップＳ２０５）。具体的には、入力した検出信号（力／トルクの検出値）の正負に基づいて衝突方向を判断する。衝突方向判断部１１５は、衝突判断部１１４における衝突発生判断後に、衝突方向を判断した後は、衝突対処モードを抜けるまで方向判断は不変として処理を省略することができる。すなわち、制御モードが衝突対処モードから通常モードに切り替えられるまでの間は、衝突判断部１１４において衝突が発生したと判断された後に衝突方向判断部１１５が行う衝突方向判断の判断結果を保持し、再度の判断を行わない構成とすることができる。

次に、フィルタ設定部１１６は、ステップＳ２０５における衝突方向判断の判断結果に基づいて、第１フィルタ部１１２及び第２フィルタ部１１３の遮断周波数を設定する（ステップＳ２０６〜ステップＳ２０９）。

ステップＳ２０５における衝突方向判断の結果、正方向の衝突であった場合（ステップＳ２０５でＹｅｓ）、フィルタ設定部１１６は、第１フィルタ部１１２の正方向の駆動指令信号に対する遮断周波数を低く設定し、負方向の駆動指令信号に対する遮断周波数を高く設定する（ステップＳ２０６）。

続いて、フィルタ設定部１１６は、第２フィルタ部１１３の正方向の検出信号に対する遮断周波数を高く設定し、負方向の検出信号に対する遮断周波数を低く設定する（ステップＳ２０７）。

一方、ステップＳ２０５における衝突方向判断の結果、負方向の衝突であった場合（ステップＳ２０５でＮｏ）、フィルタ設定部１１６は、第１フィルタ部１１２の正方向の駆動指令信号に対する遮断周波数を高く設定し、負方向の駆動指令信号に対する遮断周波数を低く設定する（ステップＳ２０８）。

続いて、フィルタ設定部１１６は、第２フィルタ部１１３の正方向の検出信号に対する遮断周波数を低く設定し、負方向の検出信号に対する遮断周波数を高く設定する（ステップＳ２０９）。

なお、ステップＳ２０６、ステップＳ２０７、及び、ステップＳ２０８、ステップＳ２０９は、それぞれ順序を入れ替えることが可能である。

次に、駆動指令信号生成部１１１は、衝突対処モードで駆動指令信号を生成する（ステップＳ２１０）。具体的に、駆動指令信号生成部１１１は、衝突状態から離脱するために必要となるトルク値を、エンコーダ１４０より入力する位置信号、第２フィルタ部１１３より入力するフィルタ後検出信号、などの信号（情報）に基づいて算出し、トルク指令信号を生成する。

この時、第２フィルタ部１１３より入力するフィルタ後検出信号は、ステップＳ２０７又はステップＳ２０８でフィルタ設定部１１６によって設定された遮断周波数でフィルタ処理が行われている。ステップＳ２０７及びステップＳ２０９では、押し付け力／押し付けトルクが減る方向、すなわち衝突対象から離れる方向の検出が遅くなるように設定されている。当該設定に基づいて検出信号に対するフィルタ処理を行った上で駆動指令信号生成部１１１がトルク指令信号を生成することで、望ましい動作を得ることができる。

この点について、詳細に説明する。衝突が発生した場合、押し付け力／トルクが過大になると対象物や自身の破壊に繋がるため、このような状況を避けることが最優先事項となる。ここで、押し付け力／トルクが減る方向の信号を早く検出してしまうと、押し付け力／トルクの減らしすぎを防ぐために、押し付け力／トルクが減る方向、すなわち、衝突対象から離れる方向のトルク指令が却って小さくなり、望ましい動作とならない。そこで、押し付け力／トルクが減る方向の検出が遅くなるように設定することで、押し付け力／トルクをより早く減らすために、押し付け力／トルクが減る方向のトルク指令が大きくなり、結果的に衝突対象から離れやすくなって望ましい動作が得られることになる。

ステップＳ２１０で生成されたトルク指令信号は、第１フィルタ部１１２においてフィルタ処理が行われた後、モータアンプ１２０へ出力される（ステップＳ２１１）。

ここで、第１フィルタ部１１２の遮断周波数は、ステップＳ２０６及びステップＳ２０８で、フィルタ設定部１１６によって押し付け力／トルクが減る方向、すなわち、衝突対象から離れる方向へのトルク指令が早く実行されるように設定されている。第１フィルタ部１１２が、当該設定に基づいてトルク指令信号にフィルタ処理を行った上でフィルタ後の信号をモータアンプ１２０に出力することで、衝突対象から離れやすくなって望ましい動作が得られることになる。

モータアンプ１２０は、フィルタ処理後のトルク指令信号に基づいてモータ１３０に対する電流制御を行ってモータ１３０を駆動し、モータ１３０は、モータ１２０からの電流制御に従ってアーム１５０を駆動する。

図６は、第１フィルタ部１１２のトルク指令信号に対するフィルタ特性を示している。図７に示す従来のトルク指令ローパスフィルタのフィルタ特性と比較して、衝突した物体から離れる方向の動作のためのトルク指令に対しては早い応答がなされ、衝突した物体に更に押し付ける方向の動作のためのトルク指令に対しては遅い応答となる。

以上各実施の形態で説明したように、本発明の衝突制御装置は、衝突の方向を判断する衝突方向判断部と、衝突の方向に基づいて駆動指令信号に対するローパスフィルタ（ＬＰＦ）のカットオフ周波数を設定するフィルタ設定部と、を具備することを特徴としている。当該構成によれば、衝突発生の事実に加えて衝突方向まで判断し、当該衝突方向が反映されたフィルタ処理が行われて駆動指令が出されることで、衝突状態からの離脱を適切に早めることができる。

ここで、フィルタ設定部は、衝突の方向に対して逆方向にマニピュレータを駆動させる駆動指令信号に対する第１ＬＰＦのカットオフ周波数を、衝突の方向にマニピュレータを駆動させる駆動指令信号に対する第１ＬＰＦのカットオフ周波数と比較して高く設定するとなお良好である。当該設定により、衝突状態からの離脱を最優先としたフィルタ特性を実現できる。

なお、トルクセンサ等の検出部より出力される検出信号の第２カットオフ周波数より高い周波数帯域を減衰又はカットオフする第２ＬＰＦを更に具備し、マニピュレータを駆動する駆動部に対して駆動を指令する駆動指令信号を生成する生成部は、当該第２ＬＰＦにおけるフィルタ処理後の検出信号に基づいて当該駆動指令信号を生成するとメカ振動を更に抑えることができるため良好である。ここで、第１ＬＰＦと同様、フィルタ設定部が、衝突の方向に基づいて前記第２ＬＰＦの第２カットオフ周波数を設定すると良い。

この時、フィルタ設定部は、衝突の方向の検出信号に対する第２ＬＰＦの第２カットオフ周波数を、衝突の方向に対して逆方向の検出信号に対する第２ＬＰＦの第２カットオフ周波数と比較して高く設定するとなお良好である。当該構成とすることで、衝突状態からの離脱を最優先としたフィルタ特性を実現できる。

また、上述のように、衝突判断部における衝突判断はフィルタ前の検出信号を用いて行い、駆動指令信号生成部における駆動指令信号の生成は、フィルタ後の検出信号に基づいて行われることで、適切な衝突状態からの離脱制御を実現することができる。

なお、上記説明では、主に、センサ等の検出部においてトルクを検出し、モータアンプ等の駆動部に出力する駆動指令としてトルク指令を行う場合について説明したがこれに限定されるものではない。例えば、検出部は、アーム等のマニピュレータに生じる振動を検出し、衝突判断部は、検出された振動波形や振幅の大きさ等に基づいて衝突が発生しているかを判断することも可能である。

すなわち、上記説明では、衝突判断手段は、マニピュレータにかかる力又はトルクを検出する検出手段における検出結果に基づいてマニピュレータに衝突が発生したかを判断し、衝突方向判断手段は、マニピュレータにかかる力又はトルクを検出する検出手段における検出結果に基づいて衝突の方向を判断する場合について説明した。一方、本発明は、これに限定されず、様々な物理量を検出するセンサからの情報に基づいて衝突や衝突方向を判断することが可能である。

なお、上記説明では、通常モードから衝突対処モードに制御モードが切り替えられる場合について主に説明したが、衝突状態から離脱された場合には、制御装置は、衝突対処モードから通常モードに適宜切り替える。当該衝突状態から離脱したかどうかは、時間や力／トルクの検出値に所定の基準値を設けておき、衝突対処モードにおける各制御周期において、当該基準値と比較を行うことで判断することが可能である。当該比較の結果、衝突状態が解消している場合は、衝突判断部は、制御モードを衝突対処モードから通常モードに切り替える。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上記説明では、各フィルタ部とフィルタ設定部とを別の機能構成として説明したが、各フィルタ部とフィルタ設定部が一体として形成されていても良い。

すなわち、本発明の制御装置には、マニピュレータに生じる所定の物理量を検出するセンサにおける検出結果に基づいて前記マニピュレータに衝突が発生したかを判断する衝突判断部と、前記衝突の方向を判断する衝突方向判断部と、前記マニピュレータを駆動する駆動部に対して前記駆動を指令する駆動指令信号を生成する生成部と、前記衝突の方向に対して逆方向に前記マニピュレータを駆動させる前記駆動指令信号に対して、前記衝突の方向に前記マニピュレータを駆動させる駆動指令信号よりも高い遮断周波数でフィルタ処理を行うフィルタ部と、を具備する構成が含まれる。より詳細には、前記フィルタ部は、前記生成部で生成される駆動指令信号が、前記衝突方向判断部で判断された前記衝突の方向に対して逆方向に前記マニピュレータを駆動させる駆動指令信号である場合、前記衝突の方向に前記マニピュレータを駆動させる駆動指令信号よりも高い遮断周波数でフィルタ処理を行うことで、より高周波帯域の成分を通過させる。当該構成とすることで、衝突状態からの離脱を早めることができる。

また、検出信号に関するフィルタ部についても同様である。すなわち、本発明の制御装置は、前記衝突の方向に対して逆方向の検出信号に対して、前記衝突の方向の検出信号よりも低い遮断周波数でフィルタ処理を行う検出信号ローパスフィルタ部を具備し、駆動を指令する駆動指令信号を生成する生成部は、前記検出信号ローパスフィルタ部におけるフィルタ処理後の検出信号に基づいてマニピュレータを駆動する駆動部に対して前記駆動指令信号を生成する構成とすることが当然可能である。

また、上述した制御装置における各機能は、個々の装置や電子回路などのハードウェアで実現してもよいし、ＣＰＵ（Central Processing Unit）にコンピュータプログラムを実行させることにより実現されていても良い。また、上述したプログラムは、様々なタイプの非一時的なコンピュータ可読媒体（ｎｏｎ−ｔｒａｎｓｉｔｏｒｙ ｃｏｍｐｕｔｅｒ ｒｅａｄａｂｌｅ ｍｅｄｉｕｍ）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（ｔａｎｇｉｂｌｅ ｓｔｏｒａｇｅ ｍｅｄｉｕｍ）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Ｐｒｏｇｒａｍｍａｂｌｅ ＲＯＭ）、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ ＰＲＯＭ）、フラッシュＲＯＭ、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（ｔｒａｎｓｉｔｏｒｙ ｃｏｍｐｕｔｅｒ ｒｅａｄａｂｌｅ ｍｅｄｉｕｍ）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。また、上述した各実施の形態同士を組み合わせることも可能である。

１０ ロボット １１ モータ
１２ アーム １３ 関節軸
１４ 力センサ １５ トルクセンサ
１６ センサ ２０ 制御装置
２１ 駆動指令信号生成部 ２２ フィルタ部
２３ 衝突判断部 ２４ 衝突方向判断部
２５ フィルタ設定部 ２５ 設定部
１００ ロボット １１０ 制御装置
１１１ 駆動指令信号生成部 １１２ フィルタ部
１１３ フィルタ部 １１４ 衝突判断部
１１５ 衝突方向判断部 １１６ フィルタ設定部
１２０ モータアンプ １３０ モータ
１４０ エンコーダ １５０ アーム
１６０ センサ

Claims (8)

1. マニピュレータに生じる所定の物理量を検出する検出手段より出力された検出信号の遮断周波数より高い周波数帯域を減衰又は遮断するフィルタと、
   前記フィルタによって前記遮断周波数より高い周波数帯域が減衰又は遮断された検出信号に基づいて、前記マニピュレータを駆動する駆動手段に対して前記駆動を指令する駆動指令信号を生成する生成手段と、
   前記検出手段から出力された検出信号が示す検出結果に基づいて前記マニピュレータに衝突が発生したかを判断する衝突判断手段と、
   前記衝突の方向を判断する衝突方向判断手段と、
   前記衝突の方向に応じて、前記フィルタの前記遮断周波数の値を決定する設定手段と、
   を具備する制御装置。
2. 前記設定手段は、前記衝突の方向の前記検出信号に対する前記フィルタの前記遮断周波数を、前記衝突の方向に対して逆方向の前記検出信号に対する前記フィルタの前記遮断周波数と比較して高く設定することを特徴とする、
   請求項１に記載の制御装置。
3. 前記生成手段は、前記衝突判断手段において前記衝突が発生したと判断された場合に、前記フィルタで前記遮断周波数より高い周波数帯域が減衰又は遮断された前記検出信号に基づいて前記駆動指令信号を生成することを特徴とする、
   請求項１又は２に記載の制御装置。
4. 前記生成手段は、前記駆動手段に対するトルク指令であるトルク指令信号を前記駆動指令信号として生成する、
   請求項１乃至３のいずれか１項に記載の制御装置。
5. 前記検出手段は、前記マニピュレータにかかる力又はトルクを検出する、
   請求項１乃至４のいずれか１項に記載の制御装置。
6. マニピュレータに生じる所定の物理量を検出する検出手段より出力された検出信号が示す検出結果に基づいて前記マニピュレータに衝突が発生したかを判断する衝突判断ステップと、
   前記衝突が発生したと判断された場合に前記衝突の方向を判断する衝突方向判断ステップと、
   前記衝突の方向に応じて、遮断周波数の値を決定する設定ステップと、
   前記検出手段より出力された検出信号の遮断周波数より高い周波数帯域を減衰又は遮断するフィルタステップと、
   前記フィルタステップで前記遮断周波数より高い周波数帯域が減衰又は遮断された検出信号に基づいて、前記マニピュレータを駆動する駆動手段に対して前記駆動を指令する駆動指令信号を生成する生成ステップと、
   を有する制御方法。
7. マニピュレータと、
   前記マニピュレータを駆動する駆動手段と、
   前記マニピュレータに生じる所定の物理量を検出し、検出結果を示す検出信号を出力する検出手段と、
   前記検出手段より出力された検出信号の遮断周波数より高い周波数帯域を減衰又は遮断するフィルタと、
   前記フィルタによって前記遮断周波数より高い周波数帯域が減衰又は遮断された検出信号に基づいて、前記駆動手段に対して前記駆動を指令する駆動指令信号を生成する生成手段と、
   前記検出手段より出力された検出信号が示す検出結果に基づいて前記マニピュレータに衝突が発生したかを判断する衝突判断手段と、
   前記衝突の方向を判断する衝突方向判断手段と、
   前記衝突の方向に応じて、前記フィルタの前記遮断周波数の値を決定する設定手段と、
   を具備するロボット。
8. 前記駆動手段は、前記マニピュレータを駆動するモータであり、
   前記マニピュレータは、
   アーム部と、
   前記アーム部と前記モータとを接続する関節軸と、
   を含み、
   前記検出手段は、前記アーム部にかかる力又は前記関節軸にかかるトルクのいずれかを検出する、
   請求項７に記載のロボット。

Images