<第一実施形態>
本発明の第一実施形態について、図面を参照しながら説明する。
図1は、本発明の第一実施形態に係るロボットシステム1の概略構成の一例を示す図である。図1に示すように、ロボットシステム1は、ロボット制御装置10と、ロボット20と、撮像装置30とを備える。ロボット制御装置10は、ロボット20及び撮像装置30と通信可能に接続される。
ロボット制御装置10は、ロボット20の全体を制御する。また、ロボット制御装置10は、撮像装置30による撮像を制御する。
ロボット20は、ロボット制御装置10からの制御信号に従って動作し、作業を行う。ロボットの作業内容は、特に限定されないが、例えば、作業台T上でワークW1をワークW2の穴Hに嵌める作業などがある。ワークは、作業の対象物と呼ぶことができる。
ロボット20は、一以上のジョイント(関節ともいう。)23及び一以上のリンク24を含むアーム22と、アーム22の先端部に設けられたハンド26と、アーム22の先端部とハンド26との間(手首部分と呼ぶこともできる。)に設けられた力覚センサー25と、を有する。
力覚センサー25は、例えば、ハンド26に作用する力や、モーメントを検出する。力覚センサー25の出力は、ロボット制御装置10に送られ、ロボット制御装置10によるロボット20のインピーダンス制御などに用いられる。力覚センサー25としては、例えば、並進3軸方向の力成分と、回転3軸回りのモーメント成分の6成分を同時に検出することができる6軸力覚センサーを用いることができる。もちろん、力覚センサー25の軸数は特に限定されず、例えば3軸でもよい。なお、力覚センサー25は、力検出部と呼ぶこともできる。
ハンド26は、例えば、複数の指を備え、少なくとも2本の指で対象物W(W1、W2)を把持することができる。ハンド26は、アーム22の先端部に対して着脱可能であってもよい。なお、ハンド26は、エンドエフェクターの一種ということができる。エンドエフェクターは、対象物を把持したり、持ち上げたり、吊り上げたり、吸着したり、ワークを加工したりするための部材である。エンドエフェクターは、ハンド、フック、吸盤など、様々な形態をとることができる。また、エンドエフェクターは、一本のアームに対して複数設けるようにしてもよい。
アーム22、力覚センサー25、及びハンド26を含むユニットを、可動部やマニピュレータと呼ぶこともできる(以下では、可動部と呼ぶ。)。図1の例では、ロボット20は可動部21を2本有する。可動部21には、ジョイント23やハンド26等の各部を動作させるため、例えば、アクチュエーター(図示せず)が備えられる。アクチュエーターは、例えば、サーボモーターやエンコーダーなどを備える。エンコーダーが出力するエンコーダー値(回転情報とも呼べる。)は、例えば、ロボット制御装置10や後述するロボット20の駆動制御部200(図2参照)によるロボット20のフィードバック制御などに用いられる。
ロボット20は、ロボット制御装置10から与えられる制御命令に従って、各ジョイント23を連動させて駆動することにより、アーム22の先端部などに設定された注目位置(以下、エンドポイントという。)を、所定の可動範囲内で自在に移動させたり自由な方向へ向けたりすることができる。また、ハンド26で対象物などを把持したり解放したりすることができる。
なお、エンドポイントの位置は、アームの先端部に限定されず、例えば、エンドエフェクターの先端部などに設定されてもよい。
撮像装置30は、ロボット20の作業領域(例えば、作業台T上の可動部21により作業可能な範囲である3次元空間)を撮像して、画像データを生成する。図1の例では、2台の撮像装置30がそれぞれ異なる角度で作業台T上に設置されている。撮像装置30は、例えば可視光カメラ、赤外線カメラ等を採用することができる。撮像装置30により撮像された画像はロボット制御装置10に入力される。
上記のロボットシステム1の構成は、本実施形態の特徴を説明するにあたって主要構成を説明したのであって、上記した構成例に限られない。また、一般的なロボットシステムが備える構成を排除するものではない。
例えば、図1ではジョイント数が6個(6軸)の例が示されているが、ジョイントの数(「軸数」ともいう)やリンクの数を増減させてもよい。また、ジョイント、リンク、ハンド等の各種部材の形状、大きさ、配置、構造等も適宜変更してよい。
また、例えば、撮像装置30の設置位置は、特に限定されず、天井や壁に設置されてもよい。また、撮像装置30に替えて又は加えて、ロボット20のアーム22の先端部分、胴体部、あるいは頭部などに、撮像装置を設けるようにしてもよい。また、例えば、撮像装置30は、ロボット20に接続されるようにしてもよい。この場合には、撮像装置30により撮像された画像は、ロボット20を介してロボット制御装置10に入力される。また、ロボット制御装置10は、ロボット20に内蔵されるようにしてもよい。
図2は、ロボットシステム1の機能構成の一例を示す図である。
ロボット制御装置10は、主制御部100と、位置制御部110と、インピーダンス制御部120と、ビジュアルサーボ(視覚サーボと呼んでもよい)制御部130と、センサー情報取得部140と、画像取得部150とを有する。
なお、位置制御部110、インピーダンス制御部120、およびビジュアルサーボ制御部130のそれぞれを「第一制御部」と呼び、主制御部100を「第二制御部」、と呼んでもよい。
主制御部100は、各制御部(位置制御部110、インピーダンス制御部120、及びビジュアルサーボ制御部130)が、各制御の移動指令値を計算する処理のタイミングを同期させる。また、主制御部100は、各制御部が移動指令値を主制御部100に対して出力する処理のタイミングを同期させる。この同期制御については、詳細に後述する。
また、主制御部100は、各制御部(位置制御部110、インピーダンス制御部120、及びビジュアルサーボ制御部130)に対して、各制御の移動指令値に掛ける重みを設定する。主制御部100は、各制御部から移動指令値(重み付けされた移動指令値)を取得し、合成する。また、主制御部100は、合成した移動指令値に基づいて、エンドポイントの位置、すなわち可動部21を移動させるように、ロボット20に当該合成した移動指令値を含む制御命令を出力する。なお、「合成する」とは、例えば各指令値を足し合わせることであるが、他の計算方法であってもよい。
第一実施形態では、各制御部(位置制御部110、インピーダンス制御部120、及びビジュアルサーボ制御部130)による各制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)は、それぞれ個別に任意のタイミングで開始及び終了することができる。従って、ある時点では、各制御のうちいずれか一つが単独動作している場合もあるし、各制御のうちいずれか二つ以上が並列動作している場合もある。各制御の開始条件及び終了条件は、例えば予めユーザーにより設定される。ロボット制御装置10は、例えば、キーボード等の入力装置95や通信インターフェイス94を介して、ユーザーから開始条件及び終了条件の設定を受け付ける。開始条件及び終了条件の内容は特に限定されず、例えば、経路上の任意の位置、時刻、対象物とエンドポイントの距離、などを使用することができる。
また、第一実施形態では、各制御部(位置制御部110、インピーダンス制御部120、及びビジュアルサーボ制御部130)が出力する移動指令値に掛ける重みは、例えば予めユーザーにより設定される。主制御部100は、例えば、キーボード等の入力装置95や通信インターフェイス94を介して、ユーザーから重みの設定を受け付ける。より具体的には、各制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)の重みを、それぞれ重みα、重みβ、重みγとする。主制御部100は、上記の各制御の重みのそれぞれの設定を受け付ける。もちろん、各制御の重みは等しくてもよい。
上記のように各制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)のうち複数の制御の並列動作を設定可能とすることにより、制御を、単独動作から並列動作に切り替えたり、並列動作から単独動作に切り替えたりすることができる。これにより、ある制御の単独動作から他の制御の単独動作に切り替えることにより発生するモーターの過負荷やロボットの振動を抑制することができる。また、各制御の移動指令値に重みを掛けられるようにすることで、ロボットの作業内容などに応じて、モーターの過負荷やロボットの振動を抑制するために、より柔軟な設定を行うことができる。
なお、本実施形態で用いる「並列」とは、複数の動作、制御、処理等を、複数の演算装置を用いて完全に並列で実行する場合だけでなく、一つの演算装置を用いてタスクの優先制御等により実質的に並列で実行する場合も含む。
位置制御部110は、予め設定されたロボットの動作経路に沿ってエンドポイントを移動させる位置制御を実行する。
例えば、位置制御部110は、経路に関する情報を取得する。経路とは、予め教示により設定された1以上の教示位置を、予め設定された所定の順番で結ぶことで形成される経路のことである。経路に関する情報には、例えば、各教示位置の座標や、当該経路上における各教示位置の順番を特定する情報が含まれる。経路に関する情報は、例えばメモリー等の記憶部に保持される。位置制御部110は、例えば、キーボード等の入力装置95を介して、ユーザーから経路に関する情報の設定を受け付けることができる。
また、例えば、位置制御部110は、取得した経路に関する情報に基づいて、エンドポイントの軌道、すなわち、エンドポイントの移動量及び移動方向を時系列で決定する。また、位置制御部110は、決定した移動量及び移動方向に基づいて、エンドポイントの次回の移動位置(ジョイント23に設けられた各アクチュエーターの目標角度)を決定する。また、位置制御部110は、目標角度だけエンドポイントを移動させるような移動指令値を生成し、主制御部100に出力する。重みが設定されている場合は、位置制御部110は、生成した移動指令値に重みを掛けてから主制御部100に出力する。
インピーダンス制御部120は、ロボット20の力覚センサー25からのセンサー情報(力情報やモーメント情報を示すセンサー値)に基づいて、インピーダンス制御(力制御、あるいは力覚制御ともいう。)を行う。インピーダンス制御は、ロボットの手先(ハンド26等)に外から力を加えた場合に生じる機械的なインピーダンス(慣性、減衰係数、剛性)を、目的とする作業に都合の良い値に設定するための位置と力の制御手法である。例えば、ロボットのエンドエフェクター部に質量と粘性係数と弾性要素が接続されるモデルにおいて、目標として設定した質量と粘性係数と弾性係数で物体に接触するようにする制御である。
例えば、インピーダンス制御部120は、センサー情報取得部140を介して力覚センサー25から取得したセンサー情報に基づいて、目標として設定したインピーダンス(目標インピーダンス)となるように、エンドポイントの移動量及び移動方向を時系列に決定する。また、インピーダンス制御部120は、決定した移動量及び移動方向に基づいて、次回の移動位置(ジョイント23に設けられた各アクチュエーターの目標角度)を決定する。また、インピーダンス制御部120は、目標角度だけエンドポイントを移動させるような移動指令値を生成し、主制御部100に出力する。重みが設定されている場合は、インピーダンス制御部120は、生成した移動指令値に重みを掛けてから主制御部100に出力する。
なお、エンドエフェクターに加わる力やモーメントを検出する方法は力覚センサーを用いる方法に限られない。例えば、アーム22の各軸トルク値からエンドエフェクターに及ぼす外力を推定することもできる。したがって、インピーダンス制御を行うためには、直接または間接的にエンドエフェクターに加わる力を取得する手段を、アーム22が有していればよい。なお、インピーダンス制御に替えて、ハイブリッド制御やコンプライアンス制御など他の力制御を行うようにしてもよい。
ビジュアルサーボ制御部130は、撮像装置30から取得した画像に基づいて、目標物との相対的な位置の変化を視覚情報として計測し、それをフィードバック情報として用いることによって目標物を追跡する制御手法であるビジュアルサーボを実行して、可動部21を移動させる。なお、第一実施形態では、ビジュアルサーボとして、視差が生じるような2枚の画像を利用して画像を立体として認識させるステレオグラム等の方法を用いて計算した対象の3次元位置情報に基づいてロボットを制御する位置ベース法を採用する。なお、ビジュアルサーボとしては、目標の画像から抽出した特徴量と現在の画像から抽出した特徴量とに基づいてロボットを制御する画像ベース法を採用してもよい。
例えば、ビジュアルサーボ制御部130は、画像取得部150を介して撮像装置30からエンドポイントを含む画像を取得する。取得した画像からエンドポイントを認識してもよい。なお、ビジュアルサーボ制御部130が行う画像認識処理は、一般的な様々な技術を用いることができるため、説明を省略する。
また、例えば、ビジュアルサーボ制御部130は、取得した画像(以下、現在画像という)と、エンドポイントが目標位置にある時の画像(以下、目標画像という)とに基づいて、エンドポイントの軌道、すなわちエンドポイントの移動量及び移動方向を時系列に決定する。なお、目標画像は、予め取得したものをメモリー等の記憶部に格納しておけばよい。また、ビジュアルサーボ制御部130は、決定したエンドポイントの移動量及び移動方向に基づいて、次回の移動位置(ジョイント23に設けられた各アクチュエーターの目標角度)を決定する。また、ビジュアルサーボ制御部130は、目標角度だけエンドポイントを移動させるような移動指令値を生成し、主制御部100に出力する。重みが設定されている場合は、ビジュアルサーボ制御部130は、生成した移動指令値に重みを掛けてから主制御部100に出力する。
なお、関節を持つロボット20では、各関節の角度を決定すると、フォワードキネマティクス処理によりエンドポイントの位置は一意に決定される。つまり、N関節ロボットではN個の関節角度により1つの位置を表現できることになるから、当該N個の関節角度の組を1つの目標関節角度とすれば、エンドポイントの軌道を目標関節角度の集合と考えることができる。よって、各制御部から出力される移動指令値は、位置に関する値であってもよいし、関節の角度に関する値(目標角度)であってもよい。主制御部100が出力する移動指令値についても同様である。
センサー情報取得部140は、ロボット20の力覚センサー25から出力されるセンサー情報(検出されたセンサー値など)を取得する。なお、センサー情報取得部140は、力検出部と呼ぶこともできる。
画像取得部150は、撮像装置30が撮像した画像を取得し、ビジュアルサーボ制御部130などに出力する。
ロボット20は、駆動制御部200を有する。
駆動制御部200は、ロボット制御装置10から出力された移動指令値を含む制御命令と、アクチュエーターのエンコーダー値及びセンサーのセンサー値等とに基づいて、エンドポイントの位置が移動指令値が示す目標位置となるように、アクチュエーターを駆動させる。なお、エンドポイントの現在位置は、例えばアクチュエーターにおけるエンコーダー値等から求めることができる。
例えば、駆動制御部200は、ロボット制御装置10から移動指令値(目標角度)を取得する。駆動制御部200は、各ジョイント23に設けられた各アクチュエーターのエンコーダー値等に基づいて現在角度を取得し、目標角度と現在角度の差分(偏差角度)を算出する。また、駆動制御部200は、偏差角度に基づいてアーム22の移動速度を算出し(例えば、偏差角度が大きいほど移動速度を速くする)、算出した移動速度で算出した偏差角度だけ可動部21を移動させる。
次に、ロボット制御装置10で行われる各制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)の並列処理について説明する。
図3は、ロボット制御装置10における各制御の並列処理の一例を説明する図である。図3では、三つの制御(位置制御、インピーダンス制御、ビジュアルサーボ制御)が並列動作する場合を示している。
主制御部100は、所定の周期(例えば、1ミリ秒)で、各制御部(位置制御部110、インピーダンス制御部120、及びビジュアルサーボ制御部130)に、移動指令値を計算する処理(以下、「本体処理」ともいう)の実行を許可する同期信号T1を送る。なお、同期信号T1を送ることは、各制御部に本体処理の開始を指示しているともいえる。
また、主制御部100は、同期信号T1から所定時間(例えば、800マイクロ秒)の経過後に、各制御部(位置制御部110、インピーダンス制御部120、及びビジュアルサーボ制御部130)に、移動指令値を出力する処理(以下、「出力処理」ともいう)の実行を許可する同期信号T2を送る。所定時間は、同期信号T1の所定の周期よりも短い。なお、同期信号T2を送ることは、各制御部に出力処理の開始を指示しているともいえる。
ここで、同期信号T1からその直後の同期信号T2までの間隔(上記の例では、800マイクロ秒)は、位置制御部110の本体処理及びインピーダンス制御部120の本体処理の両方を実行するために、十分な時間が設定される。一方、一般的に、ビジュアルサーボ制御は、負荷の高い画像処理を含むため、位置制御やインピーダンス制御に比べて、処理時間が長い。そのため、ビジュアルサーボ制御部130の本体処理は、同期信号T1の1周期よりもよりも長い間実行される。
位置制御部110の本体処理及びインピーダンス制御部120の本体処理は、同期信号T1の受信をトリガーとして実行可能状態となり、当該同期信号T1からその直後の同期信号T2までの間に実行が開始及び終了され、待機状態となる。一方、ビジュアルサーボ制御部130の本体処理は、同期信号T1を受信した際に待機状態であれば、実行可能状態となり、実行が開始されるが、同期信号T1を受信した際に待機状態でない場合には、実行中の本体処理が継続される。
位置制御部110の本体処理及びインピーダンス制御部120の本体処理は、当該本体処理により計算した移動指令値をメモリー等の記憶部に保持する。すなわち、位置制御及びインピーダンス制御では、最新の一回分の移動指令値が保持される。一方、ビジュアルサーボ制御部130の本体処理は、当該本体処理により計算した移動指令値をメモリー等の記憶部に保持するのに加え、前回の本体処理により計算した移動指令値を引き続き記憶部に保持する。すなわち、ビジュアルサーボ制御では、最新の一回分の移動指令値と、前回の一回分の移動指令値とが保持される。これらの二つ移動指令値は、後述する補間処理で使用される。
同期信号T2からその直後の同期信号T1までの間隔(上記の例では、200マイクロ秒)は、位置制御部110の出力処理、インピーダンス制御部120の出力処理、及びビジュアルサーボ制御部130の出力処理の全てを実行するために十分な時間が設定される。さらに、同期信号T2からその直後の同期信号T1までの間隔は、各制御の出力処理に加え、各制御の移動指令値を合成してロボット20に出力する主制御部100の指令生成処理を実行するために十分な時間でもある。
位置制御部110の出力処理、インピーダンス制御部120の出力処理、及びビジュアルサーボ制御部130の出力処理は、同期信号T2の受信をトリガーとして実行可能状態となり、当該同期信号T2からその直後の同期信号T1までの間に実行が開始及び終了され、待機状態となる。ただし、ビジュアルサーボ制御部130は、同期信号T2を受信して出力処理を実行しようとするときに、本体処理が実行中である場合には、本体処理の実行を中断して優先的に出力処理を実行し、出力処理の終了後本体処理の実行を再開する。
位置制御部110の出力処理は、最後に記憶部に保持された移動指令値を、主制御部100に出力する。また、インピーダンス制御部120の出力処理は、最後に記憶部に保持された移動指令値を、主制御部100に出力する。図3に示すように、位置制御部110及びインピーダンス制御部120の出力処理は、同期信号T2の直前の同期信号T1をトリガーに実行が開始された本体処理の結果を、主制御部100に出力することになる。
一方、ビジュアルサーボ制御部130の出力処理は、最後に記憶部に保持された移動指令値とその一つ前に保持された移動指令値とに基づいて補間処理を行い、補間処理後の移動指令値を主制御部100に出力する。図3に示すように、ビジュアルサーボ制御では、最後に記憶部に保持された移動指令値は、同期信号T2の直前の同期信号T1をトリガーに実行が開始された本体処理の結果ではなく、同期信号T2の直前の同期信号T1よりも前の同期信号T1をトリガーに実行が開始された本体処理の結果となる。補間処理については、図4を参照して後に説明する。
主制御部100の指令生成処理は、同期信号T2をトリガーとして、各制御部(位置制御部110、インピーダンス制御部120、及びビジュアルサーボ制御部130)の出力処理からの移動指令値を取得し、これらの移動指令値を合成することでロボット20に出力する移動指令値を生成する。
上述のように第一実施形態では、各制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)の処理を、本体処理と出力処理に分けている。また、各制御の出力処理は一定周期で実行される。また、各制御の出力処理では、最後に実行された本体処理の結果に基づいて移動指令値が出力される。このようにすることで、本体処理の処理時間が異なる複数の制御を並列動作させても、各制御の移動指令値の欠落を発生させることなくロボットへの移動指令値を生成することができる。なお、図3を用いて三つの制御(位置制御、インピーダンス制御、ビジュアルサーボ制御)が並列動作する場合を説明したが、三つの制御のいずれか複数の組み合わせで並列動作を行う場合も、同様である。
ビジュアルサーボ制御部130が行う補間処理について説明する。図4は、移動指令を生成する各タイミングにおけるビジュアルサーボ制御の指令値の決定方法の一例を説明する図である。
ビジュアルサーボ制御部130は、同期信号T2を受信した場合に、出力位置カウンターCを1加算する。また、本体処理が終了した場合には、出力位置カウンターCをリセット(0に設定)する。図4の例では、本体処理の開始から終了までの間、出力位置カウンターCは、5回カウントされるため、出力位置カウンターの最大値Cmaxは5となる。
また、ビジュアルサーボ制御部130は、上述のように、本体処理が終了した場合に、当該本体処理により計算した移動指令値をメモリー等の記憶部に保持する(Hc)のに加え、前回の本体処理により計算した移動指令値を引き続き記憶部に保持する(Hp)。
ビジュアルサーボ制御部130の出力処理では、同期信号T2を受信する度に、その時点で保持されている最後の移動指令値Hcと、Hcの一つ前の移動指令値Hpと、出力位置カウンターCとに基づいて、下記の式(1)により出力指令値を決定し、主制御部100に出力する。
出力指令値=((Hc−Hp)×C/Cmax)+Hp ・・・(1)
仮に、同期信号T2を受信する度に、最後に保持された移動指令値のみを出力指令値として出力したとすると、次の本体処理の実行が終了するまでは同じ移動指令値が出力され続ける。そして、当該次の本体処理が終了した場合には、出力指令値の値は急に当該次の本体処理の移動指令値に変更される。このような制御を行うと、移動指令値の差分が大きい場合などには、エンドポイントの急な移動を伴うおそれがある。
そこで、ビジュアルサーボ制御部130は、上記のように、同期信号T2の各トリガーについて、最後に終了した本体処理の移動指令値と、その一つ前に終了した本体処理の移動指令値との差分を、実行中の本体処理の開始時からの同期信号T2の回数に応じた割合で出力するようにしている。このようにすることで、他の制御よりも本体処理の時間が長いビジュアルサーボの移動指令値によりエンドポイントが急に移動するのを抑制することができる。
図5は、ロボット制御装置10の機能を実現するハードウェア構成の一例を示す図である。
ロボット制御装置10は、例えば、図10に示すような、CPU(Central Processing Unit)等の演算装置91と、RAM(Random Access Memory)などの主記憶装置92と、HDD(Hard Disk Drive)等の補助記憶装置93と、有線又は無線により通信ネットワークと接続するための通信インターフェイス(I/F)94と、マウス、キーボード、タッチセンサーやタッチパネルなどの入力装置95と、液晶ディスプレイなどの表示装置96と、DVD(Digital Versatile Disk)などの持ち運び可能な記憶媒体に対する情報の読み書きを行う読み書き装置97と、を備えるコンピューター90で実現することができる。
例えば、主制御部100と、位置制御部110と、インピーダンス制御部120と、ビジュアルサーボ制御部130と、センサー情報取得部140と、画像取得部150の機能は、補助記憶装置93などから主記憶装置92にロードされた所定のプログラムを演算装置91が実行することで実現される。入力部は、例えば、演算装置91が入力装置95を利用することで実現される。記憶部は、例えば、演算装置91が主記憶装置92又は補助記憶装置93を利用することで実現される。ロボット20、撮像装置30、及び他の機器との通信機能は、例えば、演算装置91が通信I/F94利用することで実現される。なお、上記の所定のプログラムは、例えば、読み書き装置97により読み取られた記憶媒体からインストールされてもよいし、通信I/F94を介してネットワークからインストールされてもよい。
ロボット20の駆動制御部200は、例えば、演算装置、記憶装置、駆動回路などを備えるASIC(Application Specific Integrated Circuit)を備えるコントローラー基板等により実現することができる。
上述したロボットシステム1の機能構成は、ロボットシステム1の構成を理解容易にするために、主な処理内容に応じて分類したものである。構成要素の分類の仕方や名称によって、本願発明が制限されることはない。ロボットシステム1の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、1つの構成要素がさらに多くの処理を実行するように分類することもできる。また、各構成要素の処理は、1つのハードウェアで実行されてもよいし、複数のハードウェアで実行されてもよい。
また、ロボット制御装置10及びロボット20の機能及び処理の分担は、図示した例に限られない。例えば、ロボット制御装置10の少なくとも一部の機能は、ロボット20に含まれ、ロボット20により実現されてもよい。また、例えば、ロボット20の少なくとも一部の機能は、ロボット制御装置10に含まれ、ロボット制御装置10により実現されてもよい。
次に、ロボット制御装置10の動作について説明する。上述のように、各制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)は、それぞれ個別に、予め設定された開始条件にしたがって開始され、予め設定された終了条件に従って終了される。また、同期信号T1の間隔は、1ミリ秒であり、同期信号T1から800マイクロ秒後に同期信号T2が出力されるものとする。
図6、図7、及び図8は、主制御部の動作の一例を示すフロー図(その1、その2、その3)である。
図6〜8に示すフローが開始されると、主制御部100は、初期化処理を行う(ステップS101)。初期化処理では、主制御部100は、例えば、同期信号の出力タイミングを決定する各種タイマーの開始、各種変数への初期値設定などを行う。
それから、主制御部100は、1ミリ秒毎のタイマー割り込みにより発生するタイマーイベントを待機し、当該タイマーイベントが発生した場合に処理をステップS103に進める(ステップS102)。
それから、主制御部100は、PosEnableフラグ、ImpEnableフラグ、及びVsEnableフラグをチェックする(ステップS103)。PosEnableフラグ、ImpEnableフラグ、及びVsEnableフラグは、位置制御、インピーダンス制御、及びビジュアルサーボ制御が有効であるか無効であるかを示す変数である。なお、各Enableフラグは、初期値は「偽(無効)」であり、各制御が開始された場合に「真(有効)」に設定される(図9のステップS202、図10のステップS303、図11のステップS402)。
それから、主制御部100は、前回のタイマーイベント発生時の各Enableフラグが全て「偽」であり、かつ、今回のタイマーイベント発生時の各Enableフラグのいずれか一つ以上が「真」であるか否かを判定する(ステップS104)。なお、主制御部100は、各Enableフラグについて、前回のタイマーイベント発生時の値と、今回のタイマーイベント発生時の値とを主記憶装置92等の記憶部を使用して管理している。ステップS104により、例えばロボットの一連の作業に係る動作を開始するか否かの初回判定が行われる。なお、ステップS101の初期化後、最初にステップS104が実行される場合は、前回のタイマーイベント発生時の各Enableフラグは全て「偽」であるものとして初回判定が行われる。
前回のタイマーイベント発生時の各Enableフラグが全て「偽」であり、かつ、今回のタイマーイベント発生時の各Enableフラグのいずれか一つ以上が「真」である場合(ステップS104:Y)、主制御部100は、ロボット20の駆動制御部200から各ジョイントの現在角度(現在位置)を取得し、RemainOutput[]に代入する(ステップS105)。RemainOutput[]は、制御の基準となる基準位置を示す変数である。
前回のタイマーイベント発生時の各Enableフラグが全て「偽」であり、かつ、今回のタイマーイベント発生時の各Enableフラグのいずれか一つ以上が「真」である、という条件を満たさない場合(ステップS104:N)、又は、ステップS105の終了後、主制御部100は、PosFinishフラグが「真」であるか否かを判定する(図7、ステップS106)。PosFinishフラグは、位置制御が終了したか否かを示す変数である。PosFinishフラグは、初期値は「偽」であり、位置制御が終了した場合に「真」に設定される。
PosFinishフラグが「真」である場合(ステップS106:Y)、主制御部100は、PosOutput[]の値をRemainOutput[]の値に加算して、RemainOutput[]の値を更新する(ステップS107)。PosOutput[]は、位置制御が出力する出力指令値(ここでは、基準位置からの差分を示す相対位置)を示す変数である。また、主制御部100は、PosOutput[]を初期化(例えば0に設定)する(ステップS108)。また、主制御部100は、PosEnableフラグを「偽」に変更する(ステップS109)。また、主制御部100は、PosFinishフラグを「偽」に変更する(ステップS110)。
PosFinishフラグが「真」でない場合(ステップS106:N)、又は、ステップS110の終了後、主制御部100は、ImpFinishフラグが「真」であるか否かを判定する(ステップS111)。ImpFinishフラグは、インピーダンス制御が終了したか否かを示す変数である。ImpFinishフラグは、初期値は「偽」であり、インピーダンス制御が終了した場合に「真」に設定される。
ImpFinishフラグが「真」である場合(ステップS111:Y)、主制御部100は、ImpOutput[]の値をRemainOutput[]の値に加算して、RemainOutput[]の値を更新する(ステップS112)。ImpOutput[]は、インピーダンス制御が出力する出力指令値(ここでは、基準位置からの差分を示す相対位置)を示す変数である。また、主制御部100は、ImpOutput[]を初期化(例えば0に設定)する(ステップS113)。また、主制御部100は、ImpEnableフラグを「偽」に変更する(ステップS114)。また、主制御部100は、ImpFinishフラグを「偽」に変更する(ステップS115)。
ImpFinishフラグが「真」でない場合(ステップS111:N)、又は、ステップS115の終了後、主制御部100は、VsFinishフラグが「真」であるか否かを判定する(ステップS116)。VsFinishフラグは、ビジュアルサーボ制御が終了したか否かを示す変数である。VsFinishフラグは、初期値は「偽」であり、ビジュアルサーボ制御が終了した場合に「真」に設定される。
VsFinishフラグが「真」である場合(ステップS116:Y)、主制御部100は、VsOutput[]の値をRemainOutput[]の値に加算して、RemainOutput[]の値を更新する(ステップS117)。VsOutput[]は、ビジュアルサーボ制御が出力する出力指令値(ここでは、基準位置からの差分を示す相対位置)を示す変数である。また、主制御部100は、VsOutput[]を初期化(例えば0に設定)する(ステップS118)。また、主制御部100は、VsEnableフラグを「偽」に変更する(ステップS119)。また、主制御部100は、VsFinishフラグを「偽」に変更する(ステップS120)。
VsFinishフラグが「真」でない場合(ステップS116:N)、又は、ステップS120の終了後、主制御部100は、移動指令値を生成する(図8、ステップS121)。具体的には、主制御部100は、基準位置であるRemainOutput[]に、相対位置であるPosOutput[]、ImpOutput[]、及びVsOutput[]を加算し、合計値を移動指令値としてロボット20に出力する。ステップS121の処理は、図3の指令生成処理に相当する。
なお、第一実施形態では、RemainOutput[]、PosOutput[]、ImpOutput[]、VsOutput[]等の各種変数は、主制御部100や各制御部(位置制御部110、インピーダンス制御部120、及びビジュアルサーボ制御部130)から参照できるように主記憶装置92等の記憶部に保持される。
それから、主制御部100は、各制御部(位置制御部110、インピーダンス制御部120、及びビジュアルサーボ制御部130)に、移動指令値を計算する処理(本体処理)の実行を許可する同期信号T1を出力する(ステップS122)。また、主制御部100は、タイマー等を用いて、同期信号T1の出力から800マイクロ秒待機し、当該時間が経過した場合に処理をステップS124に進める(ステップS123)。
それから、主制御部100は、各制御部(位置制御部110、インピーダンス制御部120、及びビジュアルサーボ制御部130)に、移動指令値を出力する処理(出力処理)の実行を許可する同期信号T2を出力する(ステップS124)。
それから、主制御部100は、制御を終了するか否かを判定する(ステップS125)。主制御部100は、例えば、前回のタイマーイベント発生時の各Enableフラグのいずれか一つ以上が「真」であり、かつ、ステップS125時点における各Enableフラグの全てが「偽」であるか否かを判定し、この条件を満たす場合に、制御を終了すると判定する。すなわち、主制御部100は、ロボットの一連の作業に係る動作が終了したか否かを判定する。もちろん、制御を終了する条件は、上記の条件に限定されない。
制御を終了しない場合(ステップS125:N)、主制御部100は、処理をステップS102(図6)に戻す。一方、制御を終了する場合(ステップS125:Y)、主制御部100は、図6〜8に示すフローを終了する。
上記のように、主制御部100は、Finishフラグが「真」である(ステップS106:Y、ステップS111:Y、ステップS116:Y)制御については、当該制御の出力指令値Output[]を、基準位置RemainOutput[]に加算し(ステップS107、S112、S117)、当該制御の出力指令値を初期化している(ステップS108、S113、S118)。このようにすることで、ある制御が終了した場合であっても、当該制御が出力した最終的な相対位置により基準位置が更新されるため、当該制御による移動指令の結果を欠落させることなく、ステップS121で計算する移動指令値の連続性を保つことができる。
図9は、位置制御部の動作の一例を示すフロー図である。図9に示すフローは、例えば予め設定された位置制御の開始条件が満たされた場合に開始される。
図9に示すフローが開始されると、位置制御部110は、軌道計画を行う(ステップS201)。例えば、位置制御部110は、経路に関する情報に基づいて、各時刻におけるエンドポイントの位置を求め、各時刻における目標角度を決定する。また、位置制御部110は、PosEnableフラグを「真」に変更する(ステップS202)。
それから、位置制御部110は、主制御部100からの同期信号T1を待機し、同期信号T1を受信した場合に処理をステップS204に進める(ステップS203)。
それから、位置制御部110は、ステップS201で計画した軌道に基づいて今回出力すべき目標角度を特定し、当該目標角度に基づいて移動指令値を生成し(ステップS204)、主記憶装置92等の記憶部に保持する(ステップS205)。
それから、位置制御部110は、主制御部100からの同期信号T2を待機し、同期信号T2を受信した場合に処理をステップS207に進める(ステップS206)。
それから、位置制御部110は、ステップS205で記憶部に保持した移動指令値を取得し、PosOutput[]に代入する(ステップS207)。重みが設定されている場合は、位置制御部110は、取得した移動指令値に重みを掛け、PosOutput[]に代入する。ステップS207の処理は、図3の位置制御部の出力処理に相当する。
それから、位置制御部110は、位置制御を終了するか否かを判定する(ステップS208)。位置制御部110は、例えば、経路上の最終的な目標位置への移動指令値を出力したか否かを判定し、この条件を満たす場合に、位置制御を終了すると判定する。位置制御の終了条件は、特に限定されず、例えば、エラーが発生した場合であってもよい。
位置制御を終了しない場合(ステップS208:N)、位置制御部110は、処理をステップS203に戻す。一方、位置制御を終了する場合(ステップS208:Y)、位置制御部110は、PosFinishフラグを「真」に変更し(ステップS209)、図9に示すフローを終了する。
図10は、インピーダンス制御部の動作の一例を示すフロー図である。図10に示すフローは、例えば予め設定されたインピーダンス制御の開始条件が満たされた場合に開始される。
図10に示すフローが開始されると、インピーダンス制御部120は、パラメーター(例えば、目標として設定した質量と粘性係数と弾性係数)を取得し(ステップS301)、力覚センサー25からのセンサー情報に応じて前記パラメーターによりエンドポイントがどのように変化するかを決定するフィルター処理で用いるフィルター係数を計算する(ステップS302)。また、位置制御部110は、ImpEnableフラグを「真」に変更する(ステップS303)。
それから、インピーダンス制御部120は、主制御部100からの同期信号T1を待機し、同期信号T1を受信した場合に処理をステップS305に進める(ステップS304)。
それから、インピーダンス制御部120は、パラメーター(例えば、目標として設定した質量と粘性係数と弾性係数)に変更があったか否かを判定する(ステップS305)。パラメーターの変更は、例えば、入力装置95を介してユーザーから受け付けることができる。パラメーターの変更があった場合(ステップS305:Y)、インピーダンス制御部120は、フィルター処理で用いるフィルター係数を更新する(ステップS306)。ステップS305及びS306の処理により、インピーダンス制御実行中の任意のタイミングでインピーダンス制御の挙動を変更することができるため、柔軟にロボットの動作を調整することができる。
パラメーターに変更がない場合(ステップS305:N)、又は、ステップS306の終了後、インピーダンス制御部120は、力覚センサー25からのセンサー情報に基づいてフィルター処理を行って、今回出力すべき目標角度を特定し、当該目標角度に基づいて移動指令値を生成し(ステップS307)、主記憶装置92等の記憶部に保持する(ステップS308)。
なお、インピーダンス制御では、センサー情報に応じて、エンドポイントの移動量及び移動方向を表す中間データが算出される。また、算出した移動量及び移動方向(dX、dY、dZ)を目標角度(ImpOutput[])へ変換するために、例えば式(1)に示すような逆ヤコビ行列が用いられる。
ここで、逆ヤコビ行列の要素を求めるためには、インピーダンス制御による移動量及び移動方向を考慮せずに、他の制御(位置制御、及びビジュアルサーボ制御)から出力される移動指令値に基づいて生成される移動指令値(目標角度)が必要となる。第一実施形態では、記憶部には、基準位置RemainOutput[]と、他の制御(位置制御、及びビジュアルサーボ制御)の出力指令値とが保持されているため、インピーダンス制御部120は、これらの保持されている値を簡単に参照して逆ヤコビ行列の要素を求め、設定することができる。
それから、インピーダンス制御部120は、主制御部100からの同期信号T2を待機し、同期信号T2を受信した場合に処理をステップS310に進める(ステップS309)。
それから、インピーダンス制御部120は、ステップS308で記憶部に保持した移動指令値を取得し、ImpOutput[]に代入する(ステップS310)。重みが設定されている場合は、インピーダンス制御部120は、取得した移動指令値に重みを掛け、ImpOutput[]に代入する。ステップS310の処理は、図3のインピーダンス制御部の出力処理に相当する。
それから、インピーダンス制御部120は、インピーダンス制御を終了するか否かを判定する(ステップS311)。インピーダンス制御部120は、例えば、センサー情報の値が指定された値を超えたか否かを判定し、この条件を満たす場合に、インピーダンス制御を終了すると判定する。インピーダンス制御の終了条件は、特に限定されず、例えば、センサー情報の値のエラーが発生した場合であってもよい。
インピーダンス制御を終了しない場合(ステップS311:N)、インピーダンス制御部120は、処理をステップS304に戻す。一方、インピーダンス制御を終了する場合(ステップS311:Y)、インピーダンス制御部120は、ImpFinishフラグを「真」に変更し(ステップS312)、図10に示すフローを終了する。
図11及び図12は、ビジュアルサーボ制御部の動作の一例を示すフロー図(その1、その2)である。図11に示すフローは、例えば予め設定されたビジュアルサーボ制御の開始条件が満たされた場合に開始される。図11に示すフローと図12に示すフローは、別のタスクとして実行される。また、図12のステップS451〜S455の処理が実行中であっても、図11のステップS404〜S409の処理は優先して実行される。
図11に示すフローが開始されると、ビジュアルサーボ制御部130は、撮像装置30の初期化を行う(ステップS401)。また、ビジュアルサーボ制御部130は、VsEnableフラグを「真」に変更する(ステップS402)。それから、ビジュアルサーボ制御部130は、本体処理(図12)を起動する(ステップS403)。
それから、ビジュアルサーボ制御部130は、主制御部100からの同期信号T2を待機し、同期信号T2を受信した場合に処理をステップS405に進める(ステップS404)。
それから、ビジュアルサーボ制御部130は、出力位置カウンターに1加算する(ステップS405)。それから、ビジュアルサーボ制御部130は、ステップS453(図12)で記憶部に保持されている最後の移動指令値及び当該最後の移動指令値の一つ前の移動指令値と、ステップS405で更新された出力位置カウンターとに基づいて、出力指令値を求める(ステップS406)。出力指令値の計算方法は、図4を用いて上述したとおりである。
それから、ビジュアルサーボ制御部130は、ステップS406で計算した出力指令値を取得し、VsOutput[]に代入する(ステップS407)。重みが設定されている場合は、ビジュアルサーボ制御部130は、取得した出力指令値に重みを掛け、VsOutput[]に代入する。ステップS405〜S407の処理は、図3のビジュアルサーボ制御部の出力処理に相当する。
それから、ビジュアルサーボ制御部130は、ビジュアルサーボ制御を終了するか否かを判定する(ステップS408)。ビジュアルサーボ制御部130は、例えば、現在画像と目標画像とが一致したか否かを判定し、この条件を満たす場合に、ビジュアルサーボ制御を終了すると判定する。ビジュアルサーボ制御の終了条件は、特に限定されず、例えば、エラーが発生した場合であってもよい。
ビジュアルサーボ制御を終了しない場合(ステップS408:N)、ビジュアルサーボ制御部130は、処理をステップS404に戻す。一方、ビジュアルサーボ制御を終了する場合(ステップS408:Y)、ビジュアルサーボ制御部130は、VsFinishフラグを「真」に変更し(ステップS409)、図11に示すフローを終了する。
図12のフローは、ステップS403(図11)で本体処理が起動された場合に開始される。まず、ビジュアルサーボ制御部130は、主制御部100からの同期信号T1を待機し、同期信号T1を受信した場合に処理をステップS452に進める(ステップS451)。
それから、ビジュアルサーボ制御部130は、画像取得部150から取得した画像から取得した現在画像と目標画像とに基づいて、今回出力すべき目標角度を特定し、当該目標角度に基づいて移動指令値を生成し(ステップS452)、主記憶装置92等の記憶部に保持する(ステップS453)。
上述のように、画像処理を含むステップS452の処理時間は長い。すなわち、今回のステップS452の処理が終了するまでは、次回のステップS451の処理は実行されていないため、同期信号T1が送信されても無視されることになる。また、ステップS453では、今回生成された移動指令値と、前回生成された移動指令値とが保持される。
それから、ビジュアルサーボ制御部130は、出力位置カウンターを0に変更する(ステップS454)。
それから、ビジュアルサーボ制御部130は、VsEnableフラグが「偽」である、又は、VsFinishフラグが「真」であるかを判定する(ステップS455)。VsEnableフラグが「偽」である、又は、VsFinishフラグが「真」である、という条件を満たさない場合(ステップS455:N)、ビジュアルサーボ制御部130は、処理をステップS451に戻す。一方、VsEnableフラグが「偽」である、又は、VsFinishフラグが「真」である場合(ステップS455:Y)、ビジュアルサーボ制御部130は、図12に示すフローを終了する。
以上、本発明の第一実施形態について説明した。第一実施形態によれば、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することができる。
<第一実施形態の第一変形例>
第一実施形態では、図3に示すように、位置制御部の本体処理は、同期信号T1をトリガーとして実行されている。しかし、同期信号T2からその直後の同期信号T1までの間に、位置制御部110の本体処理を実行するようにしてもよい。すなわち、位置制御部110は、同期信号T2をトリガーとして、本体処理を実行し、その後、出力処理を実行する。
図13は、第一実施形態の第一変形例に係る位置制御部の動作の一例を示すフロー図である。図13において、図9と同じ符号を付した処理は、図9における処理と同一または同様の機能を有するためその詳細な説明を省略する。第一変形例が第一実施形態と異なる点は、図9のステップS203の代わりにステップS203Aを実行する点と、図9のステップS206が削除された点である。
図13のフローが開始されると、位置制御部110は、ステップS201を実行し、それから、ステップS202を実行する。それから、位置制御部110は、主制御部100からの同期信号T2を待機し、同期信号T2を受信した場合に処理をステップS204に進める(ステップS203A)。それから、位置制御部110は、ステップS204、ステップS205、ステップS207を実行する。それから、位置制御部110は、位置制御を終了するか否かを判定する(ステップS208)。位置制御を終了しない場合(ステップS208:N)、位置制御部110は、処理をステップS203Aに戻す。一方、位置制御を終了する場合(ステップS208:Y)、位置制御部110は、ステップS209を実行し、図13に示すフローを終了する。
以上、本発明の第一実施形態の第一変形例について説明した。例えば、位置制御の本体処理を同期信号T2をトリガーとして実行した方が、ビジュアルサーボ制御などの他の制御の本体処理に割り当てられるCPUリソースが増加し、ロボット制御装置10の全体として処理が高速化される場合が考えられる。第一変形例は、このような場合に有効である。
なお、ロボット制御装置10は、例えば、入力装置95を介したユーザーの指示や、CPUの処理負荷に応じて、位置制御の処理を図9と図13のいずれかに切り替え可能であってもよい。また、位置制御の本体処理に限らず、インピーダンス制御の本体処理についても、同期信号T2をトリガーとして実行するようにしてもよい。
<第二実施形態>
第一実施形態では、各制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)は、それぞれ個別に任意のタイミングで開始及び終了する。しかし、ロボットの作業内容などによっては、各制御を同時に開始させ、各制御を同時に終了させた方がよい場合が考えられる。第二実施形態では、各制御を同時に開始させ、同時に終了させる。
図6〜8の主制御部100の処理は、第二実施形態においても同様である。一方、図9〜12の各制御の処理は、第一実施形態と異なる。第二実施形態では、位置制御部、インピーダンス制御部、及びビジュアルサーボ制御部それぞれの一部の処理が、統合的に実行される。そこで、各制御部の一部の処理を統合的に実行する統合制御部105(図14:本発明の第二実施形態に係るロボットシステム1の機能構成の一例を示す図を参照)を設ける。基本的に第一実施形態と異なる点は、統合制御部105と各制御部の処理分担と、処理順序とであるため、詳細な説明は省略する。
図15は、本発明の第二実施形態に係る統合制御部(位置制御、インピーダンス制御、及びビジュアルサーボ制御)の動作の一例を示すフロー図である。
図15に示すフローが開始されると、統合制御部105は、位置制御における軌道計画を行う(ステップS501)。それから、統合制御部105は、インピーダンス制御におけるパラメーターを取得し(ステップS502)、インピーダンス制御におけるフィルター係数を計算する(ステップS503)。それから、統合制御部105は、ビジュアルサーボ制御における撮像装置30の初期化を行う(ステップS504)。
それから、統合制御部105は、PosEnableフラグ、ImpEnableフラグ、及びVsEnableフラグを「真」に変更する(ステップS505)。それから、統合制御部105は、位置制御の本体処理(後述する図16)、インピーダンス制御の本体処理(後述する図17)、及びビジュアルサーボ制御の本体処理(第一実施形態の図12と同様)を起動する(ステップS506)。
それから、統合制御部105は、主制御部100からの同期信号T2を待機し、同期信号T2を受信した場合に処理をステップS508に進める(ステップS507)。
それから、統合制御部105は、ビジュアルサーボ制御における出力位置カウンターに1加算する(ステップS508)。それから、統合制御部105は、ビジュアルサーボ制御について、ステップS453(図12)で記憶部に保持されている最後の移動指令値及び当該最後の移動指令値の一つ前の移動指令値と、ステップS508で更新された出力位置カウンターとに基づいて、ビジュアルサーボ制御の出力指令値を求める(ステップS509)。
それから、統合制御部105は、位置制御について、ステップS603(図16)で記憶部に保持された移動指令値を取得し、PosOutput[]に代入する(ステップS510)。また、統合制御部105は、インピーダンス制御について、ステップS703(図17)で記憶部に保持された移動指令値を取得し、ImpOutput[]に代入する(ステップS510)。また、統合制御部105は、ビジュアルサーボ制御について、ステップS509で計算した出力指令値を取得し、VsOutput[]に代入する(ステップS510)。なお、各制御に重みが設定されている場合には、統合制御部105は、各制御の指令値に対応する重みを掛けてから、それぞれのOutput[]に代入する。
それから、統合制御部105は、全制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)を終了するか否かを判定する(ステップS511)。統合制御部105は、例えば、ビジュアルサーボ制御について現在画像と目標画像とが一致したか否かを判定し、この条件を満たす場合に、全制御を終了すると判定する。全制御の終了条件は、特に限定されず、例えば、各制御の終了条件を組み合わせてもよい。
全制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)を終了しない場合(ステップS511:N)、統合制御部105は、処理をステップS507に戻す。一方、全制御を終了する場合(ステップS511:Y)、統合制御部105は、PosFinishフラグ、ImpFinisフラグ、及びVsFinishフラグを「真」に変更し(ステップS512)、図15に示すフローを終了する。
図16は、位置制御部の動作の一例を示すフロー図である。図16のフローは、ステップS506(図15)で位置制御の本体処理が起動された場合に開始される。
まず、位置制御部110は、主制御部100からの同期信号T1を待機し、同期信号T1を受信した場合に処理をステップS602に進める(ステップS601)。
それから、位置制御部110は、ステップS501(図15)で計画した軌道に基づいて今回出力すべき目標角度を特定し、当該目標角度に基づいて移動指令値を生成し(ステップS602)、主記憶装置92等の記憶部に保持する(ステップS603)。
それから、位置制御部110は、PosEnableフラグが「偽」である、又は、PosFinishフラグが「真」であるかを判定する(ステップS604)。PosEnableフラグが「偽」である、又は、PosFinishフラグが「真」である、という条件を満たさない場合(ステップS604:N)、位置制御部110は、処理をステップS601に戻す。一方、PosEnableフラグが「偽」である、又は、PosFinishフラグが「真」である場合(ステップS604:Y)、位置制御部110は、図16に示すフローを終了する。
図17は、インピーダンス制御部の動作の一例を示すフロー図である。図17のフローは、ステップ506(図15)でインピーダンス制御の本体処理が起動された場合に開始される。
まず、インピーダンス制御部120は、主制御部100からの同期信号T1を待機し、同期信号T1を受信した場合に処理をステップS702に進める(ステップS701)。
それから、インピーダンス制御部120は、力覚センサー25からのセンサー情報に基づいてフィルター処理を行って、今回出力すべき目標角度を特定し、当該目標角度に基づいて移動指令値を生成し(ステップS702)、主記憶装置92等の記憶部に保持する(ステップS703)。
それから、インピーダンス制御部120は、ImpEnableフラグが「偽」である、又は、ImpFinishフラグが「真」であるかを判定する(ステップS704)。ImpEnableフラグが「偽」である、又は、ImpFinishフラグが「真」である、という条件を満たさない場合(ステップS704:N)、インピーダンス制御部120は、処理をステップS701に戻す。一方、ImpEnableフラグが「偽」である、又は、ImpFinishフラグが「真」である場合(ステップS704:Y)、インピーダンス制御部120は、図17に示すフローを終了する。
以上、本発明の第二実施形態について説明した。第二実施形態によれば、3つの制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)を、同時に開始させ、同時に終了させることができる。3つの制御によりロボットを動作させる場合に、開始条件と終了条件が一つであるため、制御が簡単になる。
<第二実施形態の第一変形例>
第二実施形態では、3つの制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)の並列動作を同時に開始させ、同時に終了させる。しかし、ロボットの作業内容などによっては、2つの制御(位置制御、及びビジュアルサーボ制御)を同時に開始させ、同時に終了させるようにしてもよい。
第二実施形態の第一変形例では、統合制御部105の動作が第二実施形態と異なる。従って、異なる点を中心に説明する。
図18は、第二実施形態の第一変形例に係る統合制御部(位置制御、及びビジュアルサーボ制御)の動作の一例を示すフロー図である。図18において、図15と同一の符号を付した処理は、図15における処理と同一または同様の機能を有するためその詳細な説明を省略する。
図18のフローが開始されると、統合制御部105は、位置制御における軌道計画を行う(ステップS501)。それから、統合制御部105は、ビジュアルサーボ制御における撮像装置30の初期化を行う(ステップS504)。それから、統合制御部105は、PosEnableフラグ、及びVsEnableフラグを「真」に変更する(ステップS505A)。それから、統合制御部105は、位置制御の本体処理(図16)、及びビジュアルサーボ制御の本体処理(図12)を起動する(ステップS506A)。
それから、統合制御部105は、主制御部100からの同期信号T2を待機し、同期信号T2を受信した場合に処理をステップS508に進める(ステップS507)。それから、統合制御部105は、出力位置カウンターに1加算する(ステップS508)。それから、統合制御部105は、ビジュアルサーボ制御の出力指令値を求める(ステップS509)。
それから、統合制御部105は、位置制御について、ステップS603(図16)で記憶部に保持された移動指令値を取得し、PosOutput[]に代入する(ステップS510A)。また、統合制御部105は、ビジュアルサーボ制御について、ステップS509で計算した出力指令値を取得し、VsOutput[]に代入する(ステップS510A)。なお、各制御に重みが設定されている場合には、統合制御部105は、各制御の指令値に対応する重みを掛けてから、それぞれのOutput[]に代入する。
それから、統合制御部105は、位置制御、及びビジュアルサーボ制御を終了するか否かを判定する(ステップS511A)。統合制御部105は、例えば、ビジュアルサーボ制御について現在画像と目標画像とが一致したか否かを判定し、この条件を満たす場合に、各制御を終了すると判定する。各制御の終了条件は、特に限定されず、例えば、各制御の終了条件を組み合わせてもよい。
位置制御、及びビジュアルサーボ制御を終了しない場合(ステップS511A:N)、統合制御部105は、処理をステップS507に戻す。一方、位置制御、及びビジュアルサーボ制御を終了する場合(ステップS511A:Y)、統合制御部105は、PosFinishフラグ、及びVsFinishフラグを「真」に変更し(ステップS512A)、図18に示すフローを終了する。
以上、本発明の第二実施形態の第一変形例について説明した。第二実施形態の第一変形例によれば、2つの制御(位置制御、及びビジュアルサーボ制御)を、同時に開始させ、同時に終了させることができる。2つの制御によりロボットを動作させる場合に、開始条件と終了条件が一つであるため、制御が簡単になる。
<第二実施形態の第二変形例>
第二実施形態では、3つの制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)の並列動作を同時に開始させ、同時に終了させる。しかし、ロボットの作業内容などによっては、2つの制御(インピーダンス制御、及びビジュアルサーボ制御)を同時に開始させ、同時に終了させるようにしてもよい。
第二実施形態の第二変形例では、統合制御部105の動作が第二実施形態と異なる。従って、異なる点を中心に説明する。
図19は、第二実施形態の第二変形例に係る統合制御部(インピーダンス制御、及びビジュアルサーボ制御)の動作の一例を示すフロー図である。図19において、図15と同一の符号を付した処理は、図15における処理と同一または同様の機能を有するためその詳細な説明を省略する。
図19のフローが開始されると、統合制御部105は、インピーダンス制御におけるパラメーターを取得し(ステップS502)、インピーダンス制御におけるフィルター係数を計算する(ステップS503)。それから、統合制御部105は、ビジュアルサーボ制御における撮像装置30の初期化を行う(ステップS504)。
それから、統合制御部105は、ImpEnableフラグ、及びVsEnableフラグを「真」に変更する(ステップS505B)。それから、統合制御部105は、インピーダンス制御の本体処理(図17)、及びビジュアルサーボ制御の本体処理(図12)を起動する(ステップS506B)。
それから、統合制御部105は、主制御部100からの同期信号T2を待機し、同期信号T2を受信した場合に処理をステップS508に進める(ステップS507)。それから、統合制御部105は、出力位置カウンターに1加算する(ステップS508)。それから、統合制御部105は、ビジュアルサーボ制御の出力指令値を求める(ステップS509)。
それから、統合制御部105は、インピーダンス制御について、ステップS703(図17)で記憶部に保持された移動指令値を取得し、ImpOutput[]に代入する(ステップS510B)。また、統合制御部105は、ビジュアルサーボ制御について、ステップS509で計算した出力指令値を取得し、VsOutput[]に代入する(ステップS510B)。なお、各制御に重みが設定されている場合には、統合制御部105は、各制御の指令値に対応する重みを掛けてから、それぞれのOutput[]に代入する。
それから、統合制御部105は、インピーダンス制御、及びビジュアルサーボ制御を終了するか否かを判定する(ステップS511B)。統合制御部105は、例えば、ビジュアルサーボ制御について現在画像と目標画像とが一致したか否かを判定し、この条件を満たす場合に、各制御を終了すると判定する。各制御の終了条件は、特に限定されず、例えば、各制御の終了条件を組み合わせてもよい。
インピーダンス制御、及びビジュアルサーボ制御を終了しない場合(ステップS511B:N)、統合制御部105は、処理をステップS507に戻す。一方、インピーダンス制御、及びビジュアルサーボ制御を終了する場合(ステップS511B:Y)、統合制御部105は、ImpFinishフラグ、及びVsFinishフラグを「真」に変更し(ステップS512B)、図19に示すフローを終了する。
以上、本発明の第二実施形態の第二変形例について説明した。第二実施形態の第二変形例によれば、2つの制御(インピーダンス制御、及びビジュアルサーボ制御)を、同時に開始させ、同時に終了させることができる。2つの制御によりロボットを動作させる場合に、開始条件と終了条件が一つであるため、制御が簡単になる。
<第二実施形態の第三変形例>
第二実施形態では、3つの制御(位置制御、インピーダンス制御、及びビジュアルサーボ制御)の並列動作を同時に開始させ、同時に終了させる。しかし、ロボットの作業内容などによっては、2つの制御(位置制御、及びインピーダンス制御)を同時に開始させ、同時に終了させるようにしてもよい。
第二実施形態の第三変形例では、統合制御部105の動作が第二実施形態と異なる。従って、異なる点を中心に説明する。
図20は、第二実施形態の第三変形例に係る統合制御部(位置制御、及びインピーダンス制御)の動作の一例を示すフロー図である。図20において、図15と同一の符号を付した処理は、図15における処理と同一または同様の機能を有するためその詳細な説明を省略する。
図20のフローが開始されると、統合制御部105は、統合制御部105は、位置制御における軌道計画を行う(ステップS501)。それから、統合制御部105は、インピーダンス制御におけるパラメーターを取得し(ステップS502)、インピーダンス制御におけるフィルター係数を計算する(ステップS503)。
それから、統合制御部105は、PosEnableフラグ、及びImpEnableフラグを「真」に変更する(ステップS505C)。それから、統合制御部105は、位置制御の本体処理(図16)、及びインピーダンス制御の本体処理(図17)を起動する(ステップS506C)。
それから、統合制御部105は、主制御部100からの同期信号T2を待機し、同期信号T2を受信した場合に処理をステップS510Cに進める(ステップS507)。
それから、統合制御部105は、位置制御について、ステップS603(図16)で記憶部に保持された移動指令値を取得し、PosOutput[]に代入する(ステップS510C)。また、統合制御部105は、インピーダンス制御について、ステップS703(図17)で記憶部に保持された移動指令値を取得し、ImpOutput[]に代入する(ステップS510C)。なお、各制御に重みが設定されている場合には、統合制御部105は、各制御の指令値に対応する重みを掛けてから、それぞれのOutput[]に代入する。
それから、統合制御部105は、位置制御、及びインピーダンス制御を終了するか否かを判定する(ステップS511C)。統合制御部105は、例えば、位置制御について経路上の最終的な目標位置への移動指令値を出力したか否かを判定し、この条件を満たす場合に、各制御を終了すると判定する。各制御の終了条件は、特に限定されず、例えば、各制御の終了条件を組み合わせてもよい。
位置制御、及びインピーダンス制御を終了しない場合(ステップS511C:N)、統合制御部105は、処理をステップS507に戻す。一方、位置制御、及びインピーダンス制御を終了する場合(ステップS511C:Y)、統合制御部105は、PosFinishフラグ、及びImpFinishフラグを「真」に変更し(ステップS512C)、図20に示すフローを終了する。
以上、本発明の第二実施形態の第三変形例について説明した。第二実施形態の第三変形例によれば、2つの制御(位置制御、及びインピーダンス制御)を、同時に開始させ、同時に終了させることができる。2つの制御によりロボットを動作させる場合に、開始条件と終了条件が一つであるため、制御が簡単になる。
なお、上述した各フロー図の各処理単位は、ロボット制御装置10の処理を理解容易にするために、主な処理内容に応じて分割したものである。処理単位の分割の仕方や名称によって、本願発明が制限されることはない。ロボット制御装置10の処理は、処理内容に応じて、さらに多くの処理単位に分割することもできる。また、1つの処理単位がさらに多くの処理を含むように分割することもできる。
以上、本発明について実施形態を用いて説明したが、本発明の技術的範囲は上記実施形態に記載の範囲には限定されない。上記実施形態に多様な変更または改良を加えることが可能であることが当業者には明らかである。また、そのような変更または改良を加えた形態も本発明の技術的範囲に含まれ得ることが、特許請求の範囲の記載から明らかである。本発明は、ロボットと、ロボット制御装置等とを別個に有するロボットシステムとして提供してもよいし、ロボットにロボット制御装置等が含まれるロボットとして提供してもよいし、ロボット制御装置として提供してもよい。また、本発明は、ロボット等を制御する方法、ロボット等を制御するプログラム、当該プログラムを記憶した記憶媒体として提供することもできる。