JP6314410B2 - ロボット制御装置、ロボットシステム、ロボット、ロボット制御方法、及びプログラム - Google Patents

Description

本発明は、ロボット制御装置、ロボットシステム、ロボット、ロボット制御方法、及びプログラムに関する。

特許文献１には、カメラが取り付けられたロボットアームを対象物まで移動させる移動制御を行う装置について記載されている。当該装置は、対象物が検知されていない場合、対象物の位置に基づいて予め設定された目標位置までの経路に沿って、マニュピュレータを移動させるティーチングプレイバック制御を実行する。また、当該装置は、ティーチングプレイバック制御を切り替えるための切り替え条件が満たされた場合、視覚サーボ制御を実行する。

特開２０１１−９３０５５号公報

上記の特許文献１では、ある切り替え条件が満たされた場合に、ティーチングプレイバック制御を視覚サーボ制御に切り替えている。しかしながら、特許文献１のように２つの異なる制御をあるタイミングで急に切り替えると、ロボットアームの関節を動かすモーターに過負荷が発生したり、ロボットアームに大きな振動が発生したりすることがある。例えば、切り替え前の制御によるロボットアームの移動方向と、切り替え後の制御によるロボットアームの移動方向とに大きな差分があると、モーターの過負荷や大きな振動が発生する。

そこで、本発明は、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することを目的とする。

上記の課題を解決するための本発明の第一の態様は、ロボットの可動部のエンドポイントを移動させる第一指令値をそれぞれ出力する複数の第一制御部と、前記複数の第一制御部により出力されるそれぞれの第一指令値を足し合わせた第二指令値を用いて前記可動部を制御する第二制御部と、を有することを特徴とするロボット制御装置である。第一の態様によれば、複数の制御の指令値を足し合わせた指令値によりロボットを制御することができるため、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することができる。

前記複数の第一制御部はそれぞれ、第一の同期タイミングをトリガーとして前記エンドポイントを移動させる指令値を生成する本体処理を実行し、第二の同期タイミングをトリガーとして前記本体処理により生成した前記指令値又は前記指令値に基づいて計算した指令値を前記第一指令値として前記第二制御部に出力する出力処理を実行し、前記第二制御部は、前記第二の同期タイミングをトリガーとして前記出力処理により出力される前記第一指令値を足し合わせることにより前記第二指令値を生成する、ことを特徴としてもよい。このようにすれば、各制御の本体処理の処理時間や終了タイミングが異なっていても、各制御の第一指令値を同期したタイミングで取得することができる。

前記複数の第一制御部の本体処理は、それぞれ処理時間が異なり、前記出力処理は、当該出力処理を実行する際に前記本体処理が実行されている場合、前記本体処理よりも優先して実行され、前記実行されている本体処理より前に実行された本体処理により生成された指令値に基づいて計算した指令値を前記第一指令値として出力する、ことを特徴としてもよい。このようにすれば、出力処理時に本体処理が終了していなくても第一指令値を確実に出力することができるため、第一指令値を欠落させることなくロボットを制御することができる。

前記出力処理は、前記本体処理よりも優先して実行される場合、前記実行されている本体処理より前に実行された本体処理により生成された指令値を、前記実行されている本体処理の開始後に発生した前記第二の同期タイミングの回数に応じた割合で指令値を計算し、前記第一指令値として出力する、ことを特徴としてもよい。このようにすれば、本体処理の時間が長い制御の第一指令値によりエンドポイントが急に移動するのを抑制することができる。

前記第一の同期タイミングは、第一の周期で発生し、前記第二の同期タイミングは、前記第一の同期タイミングから、前記第一の同期タイミングの間隔よりも短い所定時間後に発生し、前記複数の第一制御部の本体処理のうち一つの本体処理の処理時間は、前記第一の周期よりも長い、ことを特徴としてもよい。このようにすれば、一定周期で第一指令値を取得してロボットを制御することができる。

前記複数の第一制御部は、設定された経路に従って前記エンドポイントを移動させる第一指令値を生成する位置制御と、前記エンドポイントが目標位置にあるときの前記エンドポイントを含む画像である目標画像、および前記エンドポイントが現在位置にあるときの前記エンドポイントを含む画像である現在画像に基づいて、前記エンドポイントを前記現在位置から前記目標位置に移動させる第一指令値を生成する視覚サーボ制御とであり、前記位置制御の本体処理の処理時間は、前記第一の周期よりも短く、前記視覚サーボ制御の本体処理の処理時間は、前記第一の周期よりも長い、ことを特徴としてもよい。このようにすれば、位置制御と視覚サーボ制御とを並列に動作させることができる。

前記複数の第一制御部は、前記エンドポイントが目標位置にあるときの前記エンドポイントを含む画像である目標画像、および前記エンドポイントが現在位置にあるときの前記エンドポイントを含む画像である現在画像に基づいて、前記エンドポイントを前記現在位置から前記目標位置に移動させる第一指令値を生成する視覚サーボ制御と、前記可動部に作用する力を検出する力検出部からの出力に基づいて前記エンドポイントを移動させる第一指令値を生成するインピーダンス制御とであり、前記インピーダンス制御の本体処理は、前記第一の周期よりも短く、前記視覚サーボ制御の本体処理は、前記第一の周期よりも長い、ことを特徴としてもよい。このようにすれば、インピーダンス制御と視覚サーボ制御とを並列に動作させることができる。

前記複数の第一制御部が出力する第一指令値を保持する記憶部を有し、前記インピーダンス制御は、前記記憶部から他の制御が出力した第一指令値を取得し、当該取得した第一指令値を用いて逆ヤコビ行列の要素を設定する、ことを特徴としてもよい。このようにすれば、インピーダンス制御に必要な他の制御の第一指令値を簡単に取得することができる。

前記複数の第一制御部の処理のそれぞれは、それぞれについて予め設定された開始条件が満たされた場合に開始され、それぞれについて予め設定された終了条件が満たされた場合に終了し、前記第二制御部は、前記複数の第一制御部による処理が実行されている場合に、前記複数の第一制御部のうち一部の第一制御部の処理が終了した場合には、当該終了した第一制御部から最後に出力された第一指令値と、終了していない他の第一制御部から出力される第一指令値とを用いて前記第二指令値を生成する、ことを特徴としてもよい。このようにすれば、終了した制御による第一指令値の結果を欠落させることなく、第二指令値の連続性を保つことができる。

前記複数の第一制御部の処理は、予め設定された開始条件が満たされた場合に同時に開始され、予め設定された終了条件が満たされた場合に同時に終了する、ことを特徴としてもよい。このようにすれば、一つの開始条件及び終了条件により、複数の第一制御部の処理を開始及び終了させることができるため、複数の制御の並列動作の制御を簡単にすることができる。

前記複数の第一制御部は、それぞれについて設定された所定の重みで前記第一指令値に重み付けをして出力する、ことを特徴としてもよい。このようにすれば、ロボットの作業内容などに応じて、モーターの過負荷やロボットの振動を抑制するためにより柔軟な制御を行うことができる。

上記の課題を解決するための本発明の第二の態様は、可動部と、前記可動部のエンドポイントを移動させる第一指令値をそれぞれ出力する複数の第一制御部と、前記複数の第一制御部により出力されるそれぞれの第一指令値を足し合わせた第二指令値を用いて前記可動部を制御する第二制御部と、を有することを特徴とするロボットである。第二の態様によれば、複数の制御の指令値を足し合わせた指令値によりロボットを制御することができるため、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することができる。

上記の課題を解決するための本発明の第三の態様は、可動部を有するロボットと、前記可動部のエンドポイントを移動させる第一指令値をそれぞれ出力する複数の第一制御部と、前記複数の第一制御部により出力されるそれぞれの第一指令値を足し合わせた第二指令値を用いて前記可動部を制御する第二制御部と、を有することを特徴とするロボットシステムである。第三の態様によれば、複数の制御の指令値を足し合わせた指令値によりロボットを制御することができるため、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することができる。

上記の課題を解決するための本発明の第四の態様は、ロボットの可動部のエンドポイントを移動させる第一指令値をそれぞれ出力する複数の第一制御ステップと、前記複数の第一制御ステップにより出力されるそれぞれの第一指令値を足し合わせた第二指令値を用いて前記可動部を制御する第二制御ステップと、を含むことを特徴とするロボット制御方法である。第四の態様によれば、複数の制御の指令値を足し合わせた指令値によりロボットを制御することができるため、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することができる。

上記の課題を解決するための本発明の第五の態様は、ロボット制御装置のプログラムであって、前記ロボットの可動部のエンドポイントを移動させる第一指令値をそれぞれ出力する複数の第一制御部と、前記複数の第一制御部により出力されるそれぞれの第一指令値を足し合わせた第二指令値を用いて前記可動部を制御する第二制御部として、前記ロボット制御装置を機能させることを特徴とする。第五の態様によれば、複数の制御の指令値を足し合わせた指令値によりロボットを制御することができるため、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することができる。

上記の課題を解決するための本発明の第六の態様は、ロボットの予め定められた動作経路に関する情報、及び力検出部からの情報の少なくとも一方と、第一の時点で撮像部から取得した情報とに基づいて、前記ロボットを第一の位置へ動作させ、前記動作経路に関する情報、及び前記力検出部からの情報の少なくとも一方と、前記第一の時点よりも後に前記撮像部から取得した情報とに基づいて、前記ロボットを第二の位置に動作させる、ことを特徴とするロボット制御装置である。これにより、少なくとも二つの異なる制御が並列に動作するため、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することができる。

上記の課題を解決するための本発明の第七の態様は、ロボットであって、前記ロボットの予め定められた動作経路に関する情報、及び力検出部からの情報の少なくとも一方と、第一の時点で撮像部から取得した情報とに基づいて、前記ロボットを第一の位置へ動作させ、前記動作経路に関する情報、及び前記力検出部からの情報の少なくとも一方と、前記第一の時点よりも後に前記撮像部から取得した情報とに基づいて、前記ロボットを第二の位置に動作させる、ことを特徴とするロボットである。これにより、少なくとも二つの異なる制御が並列に動作するため、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することができる。

上記の課題を解決するための本発明の第八の態様は、ロボットとロボット制御装置を備えるロボットシステムであって、前記ロボット制御装置は、前記ロボットの予め定められた動作経路に関する情報、及び力検出部からの情報の少なくとも一方と、第一の時点で撮像部から取得した情報とに基づいて、前記ロボットを第一の位置へ動作させ、前記動作経路に関する情報、及び前記力検出部からの情報の少なくとも一方と、前記第一の時点よりも後に前記撮像部から取得した情報とに基づいて、前記ロボットを第二の位置に動作させる、ことを特徴とするロボットシステムである。これにより、少なくとも二つの異なる制御が並列に動作するため、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することができる。

本発明の第一実施形態に係るロボットシステム１の概略構成の一例を示す図である。 ロボットシステム１の機能構成の一例を示す図である。 ロボット制御装置１０における各制御の並列処理の一例を説明する図である。 移動指令を生成する各タイミングにおけるビジュアルサーボ制御の指令値の決定方法の一例を説明する図である。 ロボット制御装置１０の機能を実現するハードウェア構成の一例を示す図である。 主制御部の動作の一例を示すフロー図（その１）である。 主制御部の動作の一例を示すフロー図（その２）である。 主制御部の動作の一例を示すフロー図（その３）である。 位置制御部の動作の一例を示すフロー図である。 インピーダンス制御部の動作の一例を示すフロー図である。 ビジュアルサーボ制御部の動作の一例を示すフロー図（その１）である。 ビジュアルサーボ制御部の動作の一例を示すフロー図（その２）である。 第一実施形態の第一変形例に係る位置制御部の動作の一例を示すフロー図である。 本発明の第二実施形態に係るロボットシステム１の機能構成の一例を示す図である。 統合制御部（位置制御、インピーダンス制御、及びビジュアルサーボ制御）の動作の一例を示すフロー図である。 位置制御部の動作の一例を示すフロー図である。 インピーダンス制御部の動作の一例を示すフロー図である。 第二実施形態の第一変形例に係る統合制御部（位置制御、及びビジュアルサーボ制御）の動作の一例を示すフロー図である。 第二実施形態の第二変形例に係る統合制御部（インピーダンス制御、及びビジュアルサーボ制御）の動作の一例を示すフロー図である。 第二実施形態の第三変形例に係る統合制御部（位置制御、及びインピーダンス制御）の動作の一例を示すフロー図である。

＜第一実施形態＞
本発明の第一実施形態について、図面を参照しながら説明する。

図１は、本発明の第一実施形態に係るロボットシステム１の概略構成の一例を示す図である。図１に示すように、ロボットシステム１は、ロボット制御装置１０と、ロボット２０と、撮像装置３０とを備える。ロボット制御装置１０は、ロボット２０及び撮像装置３０と通信可能に接続される。

ロボット制御装置１０は、ロボット２０の全体を制御する。また、ロボット制御装置１０は、撮像装置３０による撮像を制御する。

ロボット２０は、ロボット制御装置１０からの制御信号に従って動作し、作業を行う。ロボットの作業内容は、特に限定されないが、例えば、作業台Ｔ上でワークＷ１をワークＷ２の穴Ｈに嵌める作業などがある。ワークは、作業の対象物と呼ぶことができる。

ロボット２０は、一以上のジョイント（関節ともいう。）２３及び一以上のリンク２４を含むアーム２２と、アーム２２の先端部に設けられたハンド２６と、アーム２２の先端部とハンド２６との間（手首部分と呼ぶこともできる。）に設けられた力覚センサー２５と、を有する。

力覚センサー２５は、例えば、ハンド２６に作用する力や、モーメントを検出する。力覚センサー２５の出力は、ロボット制御装置１０に送られ、ロボット制御装置１０によるロボット２０のインピーダンス制御などに用いられる。力覚センサー２５としては、例えば、並進３軸方向の力成分と、回転３軸回りのモーメント成分の６成分を同時に検出することができる６軸力覚センサーを用いることができる。もちろん、力覚センサー２５の軸数は特に限定されず、例えば３軸でもよい。なお、力覚センサー２５は、力検出部と呼ぶこともできる。

ハンド２６は、例えば、複数の指を備え、少なくとも２本の指で対象物Ｗ（Ｗ１、Ｗ２）を把持することができる。ハンド２６は、アーム２２の先端部に対して着脱可能であってもよい。なお、ハンド２６は、エンドエフェクターの一種ということができる。エンドエフェクターは、対象物を把持したり、持ち上げたり、吊り上げたり、吸着したり、ワークを加工したりするための部材である。エンドエフェクターは、ハンド、フック、吸盤など、様々な形態をとることができる。また、エンドエフェクターは、一本のアームに対して複数設けるようにしてもよい。

アーム２２、力覚センサー２５、及びハンド２６を含むユニットを、可動部やマニピュレータと呼ぶこともできる（以下では、可動部と呼ぶ。）。図１の例では、ロボット２０は可動部２１を２本有する。可動部２１には、ジョイント２３やハンド２６等の各部を動作させるため、例えば、アクチュエーター（図示せず）が備えられる。アクチュエーターは、例えば、サーボモーターやエンコーダーなどを備える。エンコーダーが出力するエンコーダー値（回転情報とも呼べる。）は、例えば、ロボット制御装置１０や後述するロボット２０の駆動制御部２００（図２参照）によるロボット２０のフィードバック制御などに用いられる。

ロボット２０は、ロボット制御装置１０から与えられる制御命令に従って、各ジョイント２３を連動させて駆動することにより、アーム２２の先端部などに設定された注目位置（以下、エンドポイントという。）を、所定の可動範囲内で自在に移動させたり自由な方向へ向けたりすることができる。また、ハンド２６で対象物などを把持したり解放したりすることができる。

なお、エンドポイントの位置は、アームの先端部に限定されず、例えば、エンドエフェクターの先端部などに設定されてもよい。

撮像装置３０は、ロボット２０の作業領域（例えば、作業台Ｔ上の可動部２１により作業可能な範囲である３次元空間）を撮像して、画像データを生成する。図１の例では、２台の撮像装置３０がそれぞれ異なる角度で作業台Ｔ上に設置されている。撮像装置３０は、例えば可視光カメラ、赤外線カメラ等を採用することができる。撮像装置３０により撮像された画像はロボット制御装置１０に入力される。

上記のロボットシステム１の構成は、本実施形態の特徴を説明するにあたって主要構成を説明したのであって、上記した構成例に限られない。また、一般的なロボットシステムが備える構成を排除するものではない。

例えば、図１ではジョイント数が６個（６軸）の例が示されているが、ジョイントの数（「軸数」ともいう）やリンクの数を増減させてもよい。また、ジョイント、リンク、ハンド等の各種部材の形状、大きさ、配置、構造等も適宜変更してよい。

また、例えば、撮像装置３０の設置位置は、特に限定されず、天井や壁に設置されてもよい。また、撮像装置３０に替えて又は加えて、ロボット２０のアーム２２の先端部分、胴体部、あるいは頭部などに、撮像装置を設けるようにしてもよい。また、例えば、撮像装置３０は、ロボット２０に接続されるようにしてもよい。この場合には、撮像装置３０により撮像された画像は、ロボット２０を介してロボット制御装置１０に入力される。また、ロボット制御装置１０は、ロボット２０に内蔵されるようにしてもよい。

図２は、ロボットシステム１の機能構成の一例を示す図である。

ロボット制御装置１０は、主制御部１００と、位置制御部１１０と、インピーダンス制御部１２０と、ビジュアルサーボ（視覚サーボと呼んでもよい）制御部１３０と、センサー情報取得部１４０と、画像取得部１５０とを有する。

なお、位置制御部１１０、インピーダンス制御部１２０、およびビジュアルサーボ制御部１３０のそれぞれを「第一制御部」と呼び、主制御部１００を「第二制御部」、と呼んでもよい。

主制御部１００は、各制御部（位置制御部１１０、インピーダンス制御部１２０、及びビジュアルサーボ制御部１３０）が、各制御の移動指令値を計算する処理のタイミングを同期させる。また、主制御部１００は、各制御部が移動指令値を主制御部１００に対して出力する処理のタイミングを同期させる。この同期制御については、詳細に後述する。

また、主制御部１００は、各制御部（位置制御部１１０、インピーダンス制御部１２０、及びビジュアルサーボ制御部１３０）に対して、各制御の移動指令値に掛ける重みを設定する。主制御部１００は、各制御部から移動指令値（重み付けされた移動指令値）を取得し、合成する。また、主制御部１００は、合成した移動指令値に基づいて、エンドポイントの位置、すなわち可動部２１を移動させるように、ロボット２０に当該合成した移動指令値を含む制御命令を出力する。なお、「合成する」とは、例えば各指令値を足し合わせることであるが、他の計算方法であってもよい。

第一実施形態では、各制御部（位置制御部１１０、インピーダンス制御部１２０、及びビジュアルサーボ制御部１３０）による各制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）は、それぞれ個別に任意のタイミングで開始及び終了することができる。従って、ある時点では、各制御のうちいずれか一つが単独動作している場合もあるし、各制御のうちいずれか二つ以上が並列動作している場合もある。各制御の開始条件及び終了条件は、例えば予めユーザーにより設定される。ロボット制御装置１０は、例えば、キーボード等の入力装置９５や通信インターフェイス９４を介して、ユーザーから開始条件及び終了条件の設定を受け付ける。開始条件及び終了条件の内容は特に限定されず、例えば、経路上の任意の位置、時刻、対象物とエンドポイントの距離、などを使用することができる。

また、第一実施形態では、各制御部（位置制御部１１０、インピーダンス制御部１２０、及びビジュアルサーボ制御部１３０）が出力する移動指令値に掛ける重みは、例えば予めユーザーにより設定される。主制御部１００は、例えば、キーボード等の入力装置９５や通信インターフェイス９４を介して、ユーザーから重みの設定を受け付ける。より具体的には、各制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）の重みを、それぞれ重みα、重みβ、重みγとする。主制御部１００は、上記の各制御の重みのそれぞれの設定を受け付ける。もちろん、各制御の重みは等しくてもよい。

上記のように各制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）のうち複数の制御の並列動作を設定可能とすることにより、制御を、単独動作から並列動作に切り替えたり、並列動作から単独動作に切り替えたりすることができる。これにより、ある制御の単独動作から他の制御の単独動作に切り替えることにより発生するモーターの過負荷やロボットの振動を抑制することができる。また、各制御の移動指令値に重みを掛けられるようにすることで、ロボットの作業内容などに応じて、モーターの過負荷やロボットの振動を抑制するために、より柔軟な設定を行うことができる。

なお、本実施形態で用いる「並列」とは、複数の動作、制御、処理等を、複数の演算装置を用いて完全に並列で実行する場合だけでなく、一つの演算装置を用いてタスクの優先制御等により実質的に並列で実行する場合も含む。

位置制御部１１０は、予め設定されたロボットの動作経路に沿ってエンドポイントを移動させる位置制御を実行する。

例えば、位置制御部１１０は、経路に関する情報を取得する。経路とは、予め教示により設定された１以上の教示位置を、予め設定された所定の順番で結ぶことで形成される経路のことである。経路に関する情報には、例えば、各教示位置の座標や、当該経路上における各教示位置の順番を特定する情報が含まれる。経路に関する情報は、例えばメモリー等の記憶部に保持される。位置制御部１１０は、例えば、キーボード等の入力装置９５を介して、ユーザーから経路に関する情報の設定を受け付けることができる。

また、例えば、位置制御部１１０は、取得した経路に関する情報に基づいて、エンドポイントの軌道、すなわち、エンドポイントの移動量及び移動方向を時系列で決定する。また、位置制御部１１０は、決定した移動量及び移動方向に基づいて、エンドポイントの次回の移動位置（ジョイント２３に設けられた各アクチュエーターの目標角度）を決定する。また、位置制御部１１０は、目標角度だけエンドポイントを移動させるような移動指令値を生成し、主制御部１００に出力する。重みが設定されている場合は、位置制御部１１０は、生成した移動指令値に重みを掛けてから主制御部１００に出力する。

インピーダンス制御部１２０は、ロボット２０の力覚センサー２５からのセンサー情報（力情報やモーメント情報を示すセンサー値）に基づいて、インピーダンス制御（力制御、あるいは力覚制御ともいう。）を行う。インピーダンス制御は、ロボットの手先（ハンド２６等）に外から力を加えた場合に生じる機械的なインピーダンス（慣性、減衰係数、剛性）を、目的とする作業に都合の良い値に設定するための位置と力の制御手法である。例えば、ロボットのエンドエフェクター部に質量と粘性係数と弾性要素が接続されるモデルにおいて、目標として設定した質量と粘性係数と弾性係数で物体に接触するようにする制御である。

例えば、インピーダンス制御部１２０は、センサー情報取得部１４０を介して力覚センサー２５から取得したセンサー情報に基づいて、目標として設定したインピーダンス（目標インピーダンス）となるように、エンドポイントの移動量及び移動方向を時系列に決定する。また、インピーダンス制御部１２０は、決定した移動量及び移動方向に基づいて、次回の移動位置（ジョイント２３に設けられた各アクチュエーターの目標角度）を決定する。また、インピーダンス制御部１２０は、目標角度だけエンドポイントを移動させるような移動指令値を生成し、主制御部１００に出力する。重みが設定されている場合は、インピーダンス制御部１２０は、生成した移動指令値に重みを掛けてから主制御部１００に出力する。

なお、エンドエフェクターに加わる力やモーメントを検出する方法は力覚センサーを用いる方法に限られない。例えば、アーム２２の各軸トルク値からエンドエフェクターに及ぼす外力を推定することもできる。したがって、インピーダンス制御を行うためには、直接または間接的にエンドエフェクターに加わる力を取得する手段を、アーム２２が有していればよい。なお、インピーダンス制御に替えて、ハイブリッド制御やコンプライアンス制御など他の力制御を行うようにしてもよい。

ビジュアルサーボ制御部１３０は、撮像装置３０から取得した画像に基づいて、目標物との相対的な位置の変化を視覚情報として計測し、それをフィードバック情報として用いることによって目標物を追跡する制御手法であるビジュアルサーボを実行して、可動部２１を移動させる。なお、第一実施形態では、ビジュアルサーボとして、視差が生じるような２枚の画像を利用して画像を立体として認識させるステレオグラム等の方法を用いて計算した対象の３次元位置情報に基づいてロボットを制御する位置ベース法を採用する。なお、ビジュアルサーボとしては、目標の画像から抽出した特徴量と現在の画像から抽出した特徴量とに基づいてロボットを制御する画像ベース法を採用してもよい。

例えば、ビジュアルサーボ制御部１３０は、画像取得部１５０を介して撮像装置３０からエンドポイントを含む画像を取得する。取得した画像からエンドポイントを認識してもよい。なお、ビジュアルサーボ制御部１３０が行う画像認識処理は、一般的な様々な技術を用いることができるため、説明を省略する。

また、例えば、ビジュアルサーボ制御部１３０は、取得した画像（以下、現在画像という）と、エンドポイントが目標位置にある時の画像（以下、目標画像という）とに基づいて、エンドポイントの軌道、すなわちエンドポイントの移動量及び移動方向を時系列に決定する。なお、目標画像は、予め取得したものをメモリー等の記憶部に格納しておけばよい。また、ビジュアルサーボ制御部１３０は、決定したエンドポイントの移動量及び移動方向に基づいて、次回の移動位置（ジョイント２３に設けられた各アクチュエーターの目標角度）を決定する。また、ビジュアルサーボ制御部１３０は、目標角度だけエンドポイントを移動させるような移動指令値を生成し、主制御部１００に出力する。重みが設定されている場合は、ビジュアルサーボ制御部１３０は、生成した移動指令値に重みを掛けてから主制御部１００に出力する。

なお、関節を持つロボット２０では、各関節の角度を決定すると、フォワードキネマティクス処理によりエンドポイントの位置は一意に決定される。つまり、Ｎ関節ロボットではＮ個の関節角度により１つの位置を表現できることになるから、当該Ｎ個の関節角度の組を１つの目標関節角度とすれば、エンドポイントの軌道を目標関節角度の集合と考えることができる。よって、各制御部から出力される移動指令値は、位置に関する値であってもよいし、関節の角度に関する値（目標角度）であってもよい。主制御部１００が出力する移動指令値についても同様である。

センサー情報取得部１４０は、ロボット２０の力覚センサー２５から出力されるセンサー情報（検出されたセンサー値など）を取得する。なお、センサー情報取得部１４０は、力検出部と呼ぶこともできる。

画像取得部１５０は、撮像装置３０が撮像した画像を取得し、ビジュアルサーボ制御部１３０などに出力する。

ロボット２０は、駆動制御部２００を有する。

駆動制御部２００は、ロボット制御装置１０から出力された移動指令値を含む制御命令と、アクチュエーターのエンコーダー値及びセンサーのセンサー値等とに基づいて、エンドポイントの位置が移動指令値が示す目標位置となるように、アクチュエーターを駆動させる。なお、エンドポイントの現在位置は、例えばアクチュエーターにおけるエンコーダー値等から求めることができる。

例えば、駆動制御部２００は、ロボット制御装置１０から移動指令値（目標角度）を取得する。駆動制御部２００は、各ジョイント２３に設けられた各アクチュエーターのエンコーダー値等に基づいて現在角度を取得し、目標角度と現在角度の差分（偏差角度）を算出する。また、駆動制御部２００は、偏差角度に基づいてアーム２２の移動速度を算出し（例えば、偏差角度が大きいほど移動速度を速くする）、算出した移動速度で算出した偏差角度だけ可動部２１を移動させる。

次に、ロボット制御装置１０で行われる各制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）の並列処理について説明する。

図３は、ロボット制御装置１０における各制御の並列処理の一例を説明する図である。図３では、三つの制御（位置制御、インピーダンス制御、ビジュアルサーボ制御）が並列動作する場合を示している。

主制御部１００は、所定の周期（例えば、１ミリ秒）で、各制御部（位置制御部１１０、インピーダンス制御部１２０、及びビジュアルサーボ制御部１３０）に、移動指令値を計算する処理（以下、「本体処理」ともいう）の実行を許可する同期信号Ｔ１を送る。なお、同期信号Ｔ１を送ることは、各制御部に本体処理の開始を指示しているともいえる。

また、主制御部１００は、同期信号Ｔ１から所定時間（例えば、８００マイクロ秒）の経過後に、各制御部（位置制御部１１０、インピーダンス制御部１２０、及びビジュアルサーボ制御部１３０）に、移動指令値を出力する処理（以下、「出力処理」ともいう）の実行を許可する同期信号Ｔ２を送る。所定時間は、同期信号Ｔ１の所定の周期よりも短い。なお、同期信号Ｔ２を送ることは、各制御部に出力処理の開始を指示しているともいえる。

ここで、同期信号Ｔ１からその直後の同期信号Ｔ２までの間隔（上記の例では、８００マイクロ秒）は、位置制御部１１０の本体処理及びインピーダンス制御部１２０の本体処理の両方を実行するために、十分な時間が設定される。一方、一般的に、ビジュアルサーボ制御は、負荷の高い画像処理を含むため、位置制御やインピーダンス制御に比べて、処理時間が長い。そのため、ビジュアルサーボ制御部１３０の本体処理は、同期信号Ｔ１の１周期よりもよりも長い間実行される。

位置制御部１１０の本体処理及びインピーダンス制御部１２０の本体処理は、同期信号Ｔ１の受信をトリガーとして実行可能状態となり、当該同期信号Ｔ１からその直後の同期信号Ｔ２までの間に実行が開始及び終了され、待機状態となる。一方、ビジュアルサーボ制御部１３０の本体処理は、同期信号Ｔ１を受信した際に待機状態であれば、実行可能状態となり、実行が開始されるが、同期信号Ｔ１を受信した際に待機状態でない場合には、実行中の本体処理が継続される。

位置制御部１１０の本体処理及びインピーダンス制御部１２０の本体処理は、当該本体処理により計算した移動指令値をメモリー等の記憶部に保持する。すなわち、位置制御及びインピーダンス制御では、最新の一回分の移動指令値が保持される。一方、ビジュアルサーボ制御部１３０の本体処理は、当該本体処理により計算した移動指令値をメモリー等の記憶部に保持するのに加え、前回の本体処理により計算した移動指令値を引き続き記憶部に保持する。すなわち、ビジュアルサーボ制御では、最新の一回分の移動指令値と、前回の一回分の移動指令値とが保持される。これらの二つ移動指令値は、後述する補間処理で使用される。

同期信号Ｔ２からその直後の同期信号Ｔ１までの間隔（上記の例では、２００マイクロ秒）は、位置制御部１１０の出力処理、インピーダンス制御部１２０の出力処理、及びビジュアルサーボ制御部１３０の出力処理の全てを実行するために十分な時間が設定される。さらに、同期信号Ｔ２からその直後の同期信号Ｔ１までの間隔は、各制御の出力処理に加え、各制御の移動指令値を合成してロボット２０に出力する主制御部１００の指令生成処理を実行するために十分な時間でもある。

位置制御部１１０の出力処理、インピーダンス制御部１２０の出力処理、及びビジュアルサーボ制御部１３０の出力処理は、同期信号Ｔ２の受信をトリガーとして実行可能状態となり、当該同期信号Ｔ２からその直後の同期信号Ｔ１までの間に実行が開始及び終了され、待機状態となる。ただし、ビジュアルサーボ制御部１３０は、同期信号Ｔ２を受信して出力処理を実行しようとするときに、本体処理が実行中である場合には、本体処理の実行を中断して優先的に出力処理を実行し、出力処理の終了後本体処理の実行を再開する。

位置制御部１１０の出力処理は、最後に記憶部に保持された移動指令値を、主制御部１００に出力する。また、インピーダンス制御部１２０の出力処理は、最後に記憶部に保持された移動指令値を、主制御部１００に出力する。図３に示すように、位置制御部１１０及びインピーダンス制御部１２０の出力処理は、同期信号Ｔ２の直前の同期信号Ｔ１をトリガーに実行が開始された本体処理の結果を、主制御部１００に出力することになる。

一方、ビジュアルサーボ制御部１３０の出力処理は、最後に記憶部に保持された移動指令値とその一つ前に保持された移動指令値とに基づいて補間処理を行い、補間処理後の移動指令値を主制御部１００に出力する。図３に示すように、ビジュアルサーボ制御では、最後に記憶部に保持された移動指令値は、同期信号Ｔ２の直前の同期信号Ｔ１をトリガーに実行が開始された本体処理の結果ではなく、同期信号Ｔ２の直前の同期信号Ｔ１よりも前の同期信号Ｔ１をトリガーに実行が開始された本体処理の結果となる。補間処理については、図４を参照して後に説明する。

主制御部１００の指令生成処理は、同期信号Ｔ２をトリガーとして、各制御部（位置制御部１１０、インピーダンス制御部１２０、及びビジュアルサーボ制御部１３０）の出力処理からの移動指令値を取得し、これらの移動指令値を合成することでロボット２０に出力する移動指令値を生成する。

上述のように第一実施形態では、各制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）の処理を、本体処理と出力処理に分けている。また、各制御の出力処理は一定周期で実行される。また、各制御の出力処理では、最後に実行された本体処理の結果に基づいて移動指令値が出力される。このようにすることで、本体処理の処理時間が異なる複数の制御を並列動作させても、各制御の移動指令値の欠落を発生させることなくロボットへの移動指令値を生成することができる。なお、図３を用いて三つの制御（位置制御、インピーダンス制御、ビジュアルサーボ制御）が並列動作する場合を説明したが、三つの制御のいずれか複数の組み合わせで並列動作を行う場合も、同様である。

ビジュアルサーボ制御部１３０が行う補間処理について説明する。図４は、移動指令を生成する各タイミングにおけるビジュアルサーボ制御の指令値の決定方法の一例を説明する図である。

ビジュアルサーボ制御部１３０は、同期信号Ｔ２を受信した場合に、出力位置カウンターＣを１加算する。また、本体処理が終了した場合には、出力位置カウンターＣをリセット（０に設定）する。図４の例では、本体処理の開始から終了までの間、出力位置カウンターＣは、５回カウントされるため、出力位置カウンターの最大値Ｃｍａｘは５となる。

また、ビジュアルサーボ制御部１３０は、上述のように、本体処理が終了した場合に、当該本体処理により計算した移動指令値をメモリー等の記憶部に保持する（Ｈｃ）のに加え、前回の本体処理により計算した移動指令値を引き続き記憶部に保持する（Ｈｐ）。

ビジュアルサーボ制御部１３０の出力処理では、同期信号Ｔ２を受信する度に、その時点で保持されている最後の移動指令値Ｈｃと、Ｈｃの一つ前の移動指令値Ｈｐと、出力位置カウンターＣとに基づいて、下記の式（１）により出力指令値を決定し、主制御部１００に出力する。
出力指令値＝（（Ｈｃ−Ｈｐ）×Ｃ／Ｃｍａｘ）＋Ｈｐ ・・・（１）

仮に、同期信号Ｔ２を受信する度に、最後に保持された移動指令値のみを出力指令値として出力したとすると、次の本体処理の実行が終了するまでは同じ移動指令値が出力され続ける。そして、当該次の本体処理が終了した場合には、出力指令値の値は急に当該次の本体処理の移動指令値に変更される。このような制御を行うと、移動指令値の差分が大きい場合などには、エンドポイントの急な移動を伴うおそれがある。

そこで、ビジュアルサーボ制御部１３０は、上記のように、同期信号Ｔ２の各トリガーについて、最後に終了した本体処理の移動指令値と、その一つ前に終了した本体処理の移動指令値との差分を、実行中の本体処理の開始時からの同期信号Ｔ２の回数に応じた割合で出力するようにしている。このようにすることで、他の制御よりも本体処理の時間が長いビジュアルサーボの移動指令値によりエンドポイントが急に移動するのを抑制することができる。

図５は、ロボット制御装置１０の機能を実現するハードウェア構成の一例を示す図である。

ロボット制御装置１０は、例えば、図１０に示すような、ＣＰＵ（Central Processing Unit）等の演算装置９１と、ＲＡＭ（Random Access Memory）などの主記憶装置９２と、ＨＤＤ（Hard Disk Drive）等の補助記憶装置９３と、有線又は無線により通信ネットワークと接続するための通信インターフェイス（Ｉ／Ｆ）９４と、マウス、キーボード、タッチセンサーやタッチパネルなどの入力装置９５と、液晶ディスプレイなどの表示装置９６と、ＤＶＤ（Digital Versatile Disk）などの持ち運び可能な記憶媒体に対する情報の読み書きを行う読み書き装置９７と、を備えるコンピューター９０で実現することができる。

例えば、主制御部１００と、位置制御部１１０と、インピーダンス制御部１２０と、ビジュアルサーボ制御部１３０と、センサー情報取得部１４０と、画像取得部１５０の機能は、補助記憶装置９３などから主記憶装置９２にロードされた所定のプログラムを演算装置９１が実行することで実現される。入力部は、例えば、演算装置９１が入力装置９５を利用することで実現される。記憶部は、例えば、演算装置９１が主記憶装置９２又は補助記憶装置９３を利用することで実現される。ロボット２０、撮像装置３０、及び他の機器との通信機能は、例えば、演算装置９１が通信Ｉ／Ｆ９４利用することで実現される。なお、上記の所定のプログラムは、例えば、読み書き装置９７により読み取られた記憶媒体からインストールされてもよいし、通信Ｉ／Ｆ９４を介してネットワークからインストールされてもよい。

ロボット２０の駆動制御部２００は、例えば、演算装置、記憶装置、駆動回路などを備えるＡＳＩＣ（Application Specific Integrated Circuit）を備えるコントローラー基板等により実現することができる。

上述したロボットシステム１の機能構成は、ロボットシステム１の構成を理解容易にするために、主な処理内容に応じて分類したものである。構成要素の分類の仕方や名称によって、本願発明が制限されることはない。ロボットシステム１の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、１つの構成要素がさらに多くの処理を実行するように分類することもできる。また、各構成要素の処理は、１つのハードウェアで実行されてもよいし、複数のハードウェアで実行されてもよい。

また、ロボット制御装置１０及びロボット２０の機能及び処理の分担は、図示した例に限られない。例えば、ロボット制御装置１０の少なくとも一部の機能は、ロボット２０に含まれ、ロボット２０により実現されてもよい。また、例えば、ロボット２０の少なくとも一部の機能は、ロボット制御装置１０に含まれ、ロボット制御装置１０により実現されてもよい。

次に、ロボット制御装置１０の動作について説明する。上述のように、各制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）は、それぞれ個別に、予め設定された開始条件にしたがって開始され、予め設定された終了条件に従って終了される。また、同期信号Ｔ１の間隔は、１ミリ秒であり、同期信号Ｔ１から８００マイクロ秒後に同期信号Ｔ２が出力されるものとする。

図６、図７、及び図８は、主制御部の動作の一例を示すフロー図（その１、その２、その３）である。

図６〜８に示すフローが開始されると、主制御部１００は、初期化処理を行う（ステップＳ１０１）。初期化処理では、主制御部１００は、例えば、同期信号の出力タイミングを決定する各種タイマーの開始、各種変数への初期値設定などを行う。

それから、主制御部１００は、１ミリ秒毎のタイマー割り込みにより発生するタイマーイベントを待機し、当該タイマーイベントが発生した場合に処理をステップＳ１０３に進める（ステップＳ１０２）。

それから、主制御部１００は、ＰｏｓＥｎａｂｌｅフラグ、ＩｍｐＥｎａｂｌｅフラグ、及びＶｓＥｎａｂｌｅフラグをチェックする（ステップＳ１０３）。ＰｏｓＥｎａｂｌｅフラグ、ＩｍｐＥｎａｂｌｅフラグ、及びＶｓＥｎａｂｌｅフラグは、位置制御、インピーダンス制御、及びビジュアルサーボ制御が有効であるか無効であるかを示す変数である。なお、各Ｅｎａｂｌｅフラグは、初期値は「偽（無効）」であり、各制御が開始された場合に「真（有効）」に設定される（図９のステップＳ２０２、図１０のステップＳ３０３、図１１のステップＳ４０２）。

それから、主制御部１００は、前回のタイマーイベント発生時の各Ｅｎａｂｌｅフラグが全て「偽」であり、かつ、今回のタイマーイベント発生時の各Ｅｎａｂｌｅフラグのいずれか一つ以上が「真」であるか否かを判定する（ステップＳ１０４）。なお、主制御部１００は、各Ｅｎａｂｌｅフラグについて、前回のタイマーイベント発生時の値と、今回のタイマーイベント発生時の値とを主記憶装置９２等の記憶部を使用して管理している。ステップＳ１０４により、例えばロボットの一連の作業に係る動作を開始するか否かの初回判定が行われる。なお、ステップＳ１０１の初期化後、最初にステップＳ１０４が実行される場合は、前回のタイマーイベント発生時の各Ｅｎａｂｌｅフラグは全て「偽」であるものとして初回判定が行われる。

前回のタイマーイベント発生時の各Ｅｎａｂｌｅフラグが全て「偽」であり、かつ、今回のタイマーイベント発生時の各Ｅｎａｂｌｅフラグのいずれか一つ以上が「真」である場合（ステップＳ１０４：Ｙ）、主制御部１００は、ロボット２０の駆動制御部２００から各ジョイントの現在角度（現在位置）を取得し、ＲｅｍａｉｎＯｕｔｐｕｔ［］に代入する（ステップＳ１０５）。ＲｅｍａｉｎＯｕｔｐｕｔ［］は、制御の基準となる基準位置を示す変数である。

前回のタイマーイベント発生時の各Ｅｎａｂｌｅフラグが全て「偽」であり、かつ、今回のタイマーイベント発生時の各Ｅｎａｂｌｅフラグのいずれか一つ以上が「真」である、という条件を満たさない場合（ステップＳ１０４：Ｎ）、又は、ステップＳ１０５の終了後、主制御部１００は、ＰｏｓＦｉｎｉｓｈフラグが「真」であるか否かを判定する（図７、ステップＳ１０６）。ＰｏｓＦｉｎｉｓｈフラグは、位置制御が終了したか否かを示す変数である。ＰｏｓＦｉｎｉｓｈフラグは、初期値は「偽」であり、位置制御が終了した場合に「真」に設定される。

ＰｏｓＦｉｎｉｓｈフラグが「真」である場合（ステップＳ１０６：Ｙ）、主制御部１００は、ＰｏｓＯｕｔｐｕｔ［］の値をＲｅｍａｉｎＯｕｔｐｕｔ［］の値に加算して、ＲｅｍａｉｎＯｕｔｐｕｔ［］の値を更新する（ステップＳ１０７）。ＰｏｓＯｕｔｐｕｔ［］は、位置制御が出力する出力指令値（ここでは、基準位置からの差分を示す相対位置）を示す変数である。また、主制御部１００は、ＰｏｓＯｕｔｐｕｔ［］を初期化（例えば０に設定）する（ステップＳ１０８）。また、主制御部１００は、ＰｏｓＥｎａｂｌｅフラグを「偽」に変更する（ステップＳ１０９）。また、主制御部１００は、ＰｏｓＦｉｎｉｓｈフラグを「偽」に変更する（ステップＳ１１０）。

ＰｏｓＦｉｎｉｓｈフラグが「真」でない場合（ステップＳ１０６：Ｎ）、又は、ステップＳ１１０の終了後、主制御部１００は、ＩｍｐＦｉｎｉｓｈフラグが「真」であるか否かを判定する（ステップＳ１１１）。ＩｍｐＦｉｎｉｓｈフラグは、インピーダンス制御が終了したか否かを示す変数である。ＩｍｐＦｉｎｉｓｈフラグは、初期値は「偽」であり、インピーダンス制御が終了した場合に「真」に設定される。

ＩｍｐＦｉｎｉｓｈフラグが「真」である場合（ステップＳ１１１：Ｙ）、主制御部１００は、ＩｍｐＯｕｔｐｕｔ［］の値をＲｅｍａｉｎＯｕｔｐｕｔ［］の値に加算して、ＲｅｍａｉｎＯｕｔｐｕｔ［］の値を更新する（ステップＳ１１２）。ＩｍｐＯｕｔｐｕｔ［］は、インピーダンス制御が出力する出力指令値（ここでは、基準位置からの差分を示す相対位置）を示す変数である。また、主制御部１００は、ＩｍｐＯｕｔｐｕｔ［］を初期化（例えば０に設定）する（ステップＳ１１３）。また、主制御部１００は、ＩｍｐＥｎａｂｌｅフラグを「偽」に変更する（ステップＳ１１４）。また、主制御部１００は、ＩｍｐＦｉｎｉｓｈフラグを「偽」に変更する（ステップＳ１１５）。

ＩｍｐＦｉｎｉｓｈフラグが「真」でない場合（ステップＳ１１１：Ｎ）、又は、ステップＳ１１５の終了後、主制御部１００は、ＶｓＦｉｎｉｓｈフラグが「真」であるか否かを判定する（ステップＳ１１６）。ＶｓＦｉｎｉｓｈフラグは、ビジュアルサーボ制御が終了したか否かを示す変数である。ＶｓＦｉｎｉｓｈフラグは、初期値は「偽」であり、ビジュアルサーボ制御が終了した場合に「真」に設定される。

ＶｓＦｉｎｉｓｈフラグが「真」である場合（ステップＳ１１６：Ｙ）、主制御部１００は、ＶｓＯｕｔｐｕｔ［］の値をＲｅｍａｉｎＯｕｔｐｕｔ［］の値に加算して、ＲｅｍａｉｎＯｕｔｐｕｔ［］の値を更新する（ステップＳ１１７）。ＶｓＯｕｔｐｕｔ［］は、ビジュアルサーボ制御が出力する出力指令値（ここでは、基準位置からの差分を示す相対位置）を示す変数である。また、主制御部１００は、ＶｓＯｕｔｐｕｔ［］を初期化（例えば０に設定）する（ステップＳ１１８）。また、主制御部１００は、ＶｓＥｎａｂｌｅフラグを「偽」に変更する（ステップＳ１１９）。また、主制御部１００は、ＶｓＦｉｎｉｓｈフラグを「偽」に変更する（ステップＳ１２０）。

ＶｓＦｉｎｉｓｈフラグが「真」でない場合（ステップＳ１１６：Ｎ）、又は、ステップＳ１２０の終了後、主制御部１００は、移動指令値を生成する（図８、ステップＳ１２１）。具体的には、主制御部１００は、基準位置であるＲｅｍａｉｎＯｕｔｐｕｔ［］に、相対位置であるＰｏｓＯｕｔｐｕｔ［］、ＩｍｐＯｕｔｐｕｔ［］、及びＶｓＯｕｔｐｕｔ［］を加算し、合計値を移動指令値としてロボット２０に出力する。ステップＳ１２１の処理は、図３の指令生成処理に相当する。

なお、第一実施形態では、ＲｅｍａｉｎＯｕｔｐｕｔ［］、ＰｏｓＯｕｔｐｕｔ［］、ＩｍｐＯｕｔｐｕｔ［］、ＶｓＯｕｔｐｕｔ［］等の各種変数は、主制御部１００や各制御部（位置制御部１１０、インピーダンス制御部１２０、及びビジュアルサーボ制御部１３０）から参照できるように主記憶装置９２等の記憶部に保持される。

それから、主制御部１００は、各制御部（位置制御部１１０、インピーダンス制御部１２０、及びビジュアルサーボ制御部１３０）に、移動指令値を計算する処理（本体処理）の実行を許可する同期信号Ｔ１を出力する（ステップＳ１２２）。また、主制御部１００は、タイマー等を用いて、同期信号Ｔ１の出力から８００マイクロ秒待機し、当該時間が経過した場合に処理をステップＳ１２４に進める（ステップＳ１２３）。

それから、主制御部１００は、各制御部（位置制御部１１０、インピーダンス制御部１２０、及びビジュアルサーボ制御部１３０）に、移動指令値を出力する処理（出力処理）の実行を許可する同期信号Ｔ２を出力する（ステップＳ１２４）。

それから、主制御部１００は、制御を終了するか否かを判定する（ステップＳ１２５）。主制御部１００は、例えば、前回のタイマーイベント発生時の各Ｅｎａｂｌｅフラグのいずれか一つ以上が「真」であり、かつ、ステップＳ１２５時点における各Ｅｎａｂｌｅフラグの全てが「偽」であるか否かを判定し、この条件を満たす場合に、制御を終了すると判定する。すなわち、主制御部１００は、ロボットの一連の作業に係る動作が終了したか否かを判定する。もちろん、制御を終了する条件は、上記の条件に限定されない。

制御を終了しない場合（ステップＳ１２５：Ｎ）、主制御部１００は、処理をステップＳ１０２（図６）に戻す。一方、制御を終了する場合（ステップＳ１２５：Ｙ）、主制御部１００は、図６〜８に示すフローを終了する。

上記のように、主制御部１００は、Ｆｉｎｉｓｈフラグが「真」である（ステップＳ１０６：Ｙ、ステップＳ１１１：Ｙ、ステップＳ１１６：Ｙ）制御については、当該制御の出力指令値Ｏｕｔｐｕｔ［］を、基準位置ＲｅｍａｉｎＯｕｔｐｕｔ［］に加算し（ステップＳ１０７、Ｓ１１２、Ｓ１１７）、当該制御の出力指令値を初期化している（ステップＳ１０８、Ｓ１１３、Ｓ１１８）。このようにすることで、ある制御が終了した場合であっても、当該制御が出力した最終的な相対位置により基準位置が更新されるため、当該制御による移動指令の結果を欠落させることなく、ステップＳ１２１で計算する移動指令値の連続性を保つことができる。

図９は、位置制御部の動作の一例を示すフロー図である。図９に示すフローは、例えば予め設定された位置制御の開始条件が満たされた場合に開始される。

図９に示すフローが開始されると、位置制御部１１０は、軌道計画を行う（ステップＳ２０１）。例えば、位置制御部１１０は、経路に関する情報に基づいて、各時刻におけるエンドポイントの位置を求め、各時刻における目標角度を決定する。また、位置制御部１１０は、ＰｏｓＥｎａｂｌｅフラグを「真」に変更する（ステップＳ２０２）。

それから、位置制御部１１０は、主制御部１００からの同期信号Ｔ１を待機し、同期信号Ｔ１を受信した場合に処理をステップＳ２０４に進める（ステップＳ２０３）。

それから、位置制御部１１０は、ステップＳ２０１で計画した軌道に基づいて今回出力すべき目標角度を特定し、当該目標角度に基づいて移動指令値を生成し（ステップＳ２０４）、主記憶装置９２等の記憶部に保持する（ステップＳ２０５）。

それから、位置制御部１１０は、主制御部１００からの同期信号Ｔ２を待機し、同期信号Ｔ２を受信した場合に処理をステップＳ２０７に進める（ステップＳ２０６）。

それから、位置制御部１１０は、ステップＳ２０５で記憶部に保持した移動指令値を取得し、ＰｏｓＯｕｔｐｕｔ［］に代入する（ステップＳ２０７）。重みが設定されている場合は、位置制御部１１０は、取得した移動指令値に重みを掛け、ＰｏｓＯｕｔｐｕｔ［］に代入する。ステップＳ２０７の処理は、図３の位置制御部の出力処理に相当する。

それから、位置制御部１１０は、位置制御を終了するか否かを判定する（ステップＳ２０８）。位置制御部１１０は、例えば、経路上の最終的な目標位置への移動指令値を出力したか否かを判定し、この条件を満たす場合に、位置制御を終了すると判定する。位置制御の終了条件は、特に限定されず、例えば、エラーが発生した場合であってもよい。

位置制御を終了しない場合（ステップＳ２０８：Ｎ）、位置制御部１１０は、処理をステップＳ２０３に戻す。一方、位置制御を終了する場合（ステップＳ２０８：Ｙ）、位置制御部１１０は、ＰｏｓＦｉｎｉｓｈフラグを「真」に変更し（ステップＳ２０９）、図９に示すフローを終了する。

図１０は、インピーダンス制御部の動作の一例を示すフロー図である。図１０に示すフローは、例えば予め設定されたインピーダンス制御の開始条件が満たされた場合に開始される。

図１０に示すフローが開始されると、インピーダンス制御部１２０は、パラメーター（例えば、目標として設定した質量と粘性係数と弾性係数）を取得し（ステップＳ３０１）、力覚センサー２５からのセンサー情報に応じて前記パラメーターによりエンドポイントがどのように変化するかを決定するフィルター処理で用いるフィルター係数を計算する（ステップＳ３０２）。また、位置制御部１１０は、ＩｍｐＥｎａｂｌｅフラグを「真」に変更する（ステップＳ３０３）。

それから、インピーダンス制御部１２０は、主制御部１００からの同期信号Ｔ１を待機し、同期信号Ｔ１を受信した場合に処理をステップＳ３０５に進める（ステップＳ３０４）。

それから、インピーダンス制御部１２０は、パラメーター（例えば、目標として設定した質量と粘性係数と弾性係数）に変更があったか否かを判定する（ステップＳ３０５）。パラメーターの変更は、例えば、入力装置９５を介してユーザーから受け付けることができる。パラメーターの変更があった場合（ステップＳ３０５：Ｙ）、インピーダンス制御部１２０は、フィルター処理で用いるフィルター係数を更新する（ステップＳ３０６）。ステップＳ３０５及びＳ３０６の処理により、インピーダンス制御実行中の任意のタイミングでインピーダンス制御の挙動を変更することができるため、柔軟にロボットの動作を調整することができる。

パラメーターに変更がない場合（ステップＳ３０５：Ｎ）、又は、ステップＳ３０６の終了後、インピーダンス制御部１２０は、力覚センサー２５からのセンサー情報に基づいてフィルター処理を行って、今回出力すべき目標角度を特定し、当該目標角度に基づいて移動指令値を生成し（ステップＳ３０７）、主記憶装置９２等の記憶部に保持する（ステップＳ３０８）。

なお、インピーダンス制御では、センサー情報に応じて、エンドポイントの移動量及び移動方向を表す中間データが算出される。また、算出した移動量及び移動方向（ｄＸ、ｄＹ、ｄＺ）を目標角度（ＩｍｐＯｕｔｐｕｔ［］）へ変換するために、例えば式（１）に示すような逆ヤコビ行列が用いられる。

ここで、逆ヤコビ行列の要素を求めるためには、インピーダンス制御による移動量及び移動方向を考慮せずに、他の制御（位置制御、及びビジュアルサーボ制御）から出力される移動指令値に基づいて生成される移動指令値（目標角度）が必要となる。第一実施形態では、記憶部には、基準位置ＲｅｍａｉｎＯｕｔｐｕｔ［］と、他の制御（位置制御、及びビジュアルサーボ制御）の出力指令値とが保持されているため、インピーダンス制御部１２０は、これらの保持されている値を簡単に参照して逆ヤコビ行列の要素を求め、設定することができる。

それから、インピーダンス制御部１２０は、主制御部１００からの同期信号Ｔ２を待機し、同期信号Ｔ２を受信した場合に処理をステップＳ３１０に進める（ステップＳ３０９）。

それから、インピーダンス制御部１２０は、ステップＳ３０８で記憶部に保持した移動指令値を取得し、ＩｍｐＯｕｔｐｕｔ［］に代入する（ステップＳ３１０）。重みが設定されている場合は、インピーダンス制御部１２０は、取得した移動指令値に重みを掛け、ＩｍｐＯｕｔｐｕｔ［］に代入する。ステップＳ３１０の処理は、図３のインピーダンス制御部の出力処理に相当する。

それから、インピーダンス制御部１２０は、インピーダンス制御を終了するか否かを判定する（ステップＳ３１１）。インピーダンス制御部１２０は、例えば、センサー情報の値が指定された値を超えたか否かを判定し、この条件を満たす場合に、インピーダンス制御を終了すると判定する。インピーダンス制御の終了条件は、特に限定されず、例えば、センサー情報の値のエラーが発生した場合であってもよい。

インピーダンス制御を終了しない場合（ステップＳ３１１：Ｎ）、インピーダンス制御部１２０は、処理をステップＳ３０４に戻す。一方、インピーダンス制御を終了する場合（ステップＳ３１１：Ｙ）、インピーダンス制御部１２０は、ＩｍｐＦｉｎｉｓｈフラグを「真」に変更し（ステップＳ３１２）、図１０に示すフローを終了する。

図１１及び図１２は、ビジュアルサーボ制御部の動作の一例を示すフロー図（その１、その２）である。図１１に示すフローは、例えば予め設定されたビジュアルサーボ制御の開始条件が満たされた場合に開始される。図１１に示すフローと図１２に示すフローは、別のタスクとして実行される。また、図１２のステップＳ４５１〜Ｓ４５５の処理が実行中であっても、図１１のステップＳ４０４〜Ｓ４０９の処理は優先して実行される。

図１１に示すフローが開始されると、ビジュアルサーボ制御部１３０は、撮像装置３０の初期化を行う（ステップＳ４０１）。また、ビジュアルサーボ制御部１３０は、ＶｓＥｎａｂｌｅフラグを「真」に変更する（ステップＳ４０２）。それから、ビジュアルサーボ制御部１３０は、本体処理（図１２）を起動する（ステップＳ４０３）。

それから、ビジュアルサーボ制御部１３０は、主制御部１００からの同期信号Ｔ２を待機し、同期信号Ｔ２を受信した場合に処理をステップＳ４０５に進める（ステップＳ４０４）。

それから、ビジュアルサーボ制御部１３０は、出力位置カウンターに１加算する（ステップＳ４０５）。それから、ビジュアルサーボ制御部１３０は、ステップＳ４５３（図１２）で記憶部に保持されている最後の移動指令値及び当該最後の移動指令値の一つ前の移動指令値と、ステップＳ４０５で更新された出力位置カウンターとに基づいて、出力指令値を求める（ステップＳ４０６）。出力指令値の計算方法は、図４を用いて上述したとおりである。

それから、ビジュアルサーボ制御部１３０は、ステップＳ４０６で計算した出力指令値を取得し、ＶｓＯｕｔｐｕｔ［］に代入する（ステップＳ４０７）。重みが設定されている場合は、ビジュアルサーボ制御部１３０は、取得した出力指令値に重みを掛け、ＶｓＯｕｔｐｕｔ［］に代入する。ステップＳ４０５〜Ｓ４０７の処理は、図３のビジュアルサーボ制御部の出力処理に相当する。

それから、ビジュアルサーボ制御部１３０は、ビジュアルサーボ制御を終了するか否かを判定する（ステップＳ４０８）。ビジュアルサーボ制御部１３０は、例えば、現在画像と目標画像とが一致したか否かを判定し、この条件を満たす場合に、ビジュアルサーボ制御を終了すると判定する。ビジュアルサーボ制御の終了条件は、特に限定されず、例えば、エラーが発生した場合であってもよい。

ビジュアルサーボ制御を終了しない場合（ステップＳ４０８：Ｎ）、ビジュアルサーボ制御部１３０は、処理をステップＳ４０４に戻す。一方、ビジュアルサーボ制御を終了する場合（ステップＳ４０８：Ｙ）、ビジュアルサーボ制御部１３０は、ＶｓＦｉｎｉｓｈフラグを「真」に変更し（ステップＳ４０９）、図１１に示すフローを終了する。

図１２のフローは、ステップＳ４０３（図１１）で本体処理が起動された場合に開始される。まず、ビジュアルサーボ制御部１３０は、主制御部１００からの同期信号Ｔ１を待機し、同期信号Ｔ１を受信した場合に処理をステップＳ４５２に進める（ステップＳ４５１）。

それから、ビジュアルサーボ制御部１３０は、画像取得部１５０から取得した画像から取得した現在画像と目標画像とに基づいて、今回出力すべき目標角度を特定し、当該目標角度に基づいて移動指令値を生成し（ステップＳ４５２）、主記憶装置９２等の記憶部に保持する（ステップＳ４５３）。

上述のように、画像処理を含むステップＳ４５２の処理時間は長い。すなわち、今回のステップＳ４５２の処理が終了するまでは、次回のステップＳ４５１の処理は実行されていないため、同期信号Ｔ１が送信されても無視されることになる。また、ステップＳ４５３では、今回生成された移動指令値と、前回生成された移動指令値とが保持される。

それから、ビジュアルサーボ制御部１３０は、出力位置カウンターを０に変更する（ステップＳ４５４）。

それから、ビジュアルサーボ制御部１３０は、ＶｓＥｎａｂｌｅフラグが「偽」である、又は、ＶｓＦｉｎｉｓｈフラグが「真」であるかを判定する（ステップＳ４５５）。ＶｓＥｎａｂｌｅフラグが「偽」である、又は、ＶｓＦｉｎｉｓｈフラグが「真」である、という条件を満たさない場合（ステップＳ４５５：Ｎ）、ビジュアルサーボ制御部１３０は、処理をステップＳ４５１に戻す。一方、ＶｓＥｎａｂｌｅフラグが「偽」である、又は、ＶｓＦｉｎｉｓｈフラグが「真」である場合（ステップＳ４５５：Ｙ）、ビジュアルサーボ制御部１３０は、図１２に示すフローを終了する。

以上、本発明の第一実施形態について説明した。第一実施形態によれば、制御の急な切り替えを防止し、ロボットの過負荷や振動を抑制することができる。

＜第一実施形態の第一変形例＞
第一実施形態では、図３に示すように、位置制御部の本体処理は、同期信号Ｔ１をトリガーとして実行されている。しかし、同期信号Ｔ２からその直後の同期信号Ｔ１までの間に、位置制御部１１０の本体処理を実行するようにしてもよい。すなわち、位置制御部１１０は、同期信号Ｔ２をトリガーとして、本体処理を実行し、その後、出力処理を実行する。

図１３は、第一実施形態の第一変形例に係る位置制御部の動作の一例を示すフロー図である。図１３において、図９と同じ符号を付した処理は、図９における処理と同一または同様の機能を有するためその詳細な説明を省略する。第一変形例が第一実施形態と異なる点は、図９のステップＳ２０３の代わりにステップＳ２０３Ａを実行する点と、図９のステップＳ２０６が削除された点である。

図１３のフローが開始されると、位置制御部１１０は、ステップＳ２０１を実行し、それから、ステップＳ２０２を実行する。それから、位置制御部１１０は、主制御部１００からの同期信号Ｔ２を待機し、同期信号Ｔ２を受信した場合に処理をステップＳ２０４に進める（ステップＳ２０３Ａ）。それから、位置制御部１１０は、ステップＳ２０４、ステップＳ２０５、ステップＳ２０７を実行する。それから、位置制御部１１０は、位置制御を終了するか否かを判定する（ステップＳ２０８）。位置制御を終了しない場合（ステップＳ２０８：Ｎ）、位置制御部１１０は、処理をステップＳ２０３Ａに戻す。一方、位置制御を終了する場合（ステップＳ２０８：Ｙ）、位置制御部１１０は、ステップＳ２０９を実行し、図１３に示すフローを終了する。

以上、本発明の第一実施形態の第一変形例について説明した。例えば、位置制御の本体処理を同期信号Ｔ２をトリガーとして実行した方が、ビジュアルサーボ制御などの他の制御の本体処理に割り当てられるＣＰＵリソースが増加し、ロボット制御装置１０の全体として処理が高速化される場合が考えられる。第一変形例は、このような場合に有効である。

なお、ロボット制御装置１０は、例えば、入力装置９５を介したユーザーの指示や、ＣＰＵの処理負荷に応じて、位置制御の処理を図９と図１３のいずれかに切り替え可能であってもよい。また、位置制御の本体処理に限らず、インピーダンス制御の本体処理についても、同期信号Ｔ２をトリガーとして実行するようにしてもよい。

＜第二実施形態＞
第一実施形態では、各制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）は、それぞれ個別に任意のタイミングで開始及び終了する。しかし、ロボットの作業内容などによっては、各制御を同時に開始させ、各制御を同時に終了させた方がよい場合が考えられる。第二実施形態では、各制御を同時に開始させ、同時に終了させる。

図６〜８の主制御部１００の処理は、第二実施形態においても同様である。一方、図９〜１２の各制御の処理は、第一実施形態と異なる。第二実施形態では、位置制御部、インピーダンス制御部、及びビジュアルサーボ制御部それぞれの一部の処理が、統合的に実行される。そこで、各制御部の一部の処理を統合的に実行する統合制御部１０５（図１４：本発明の第二実施形態に係るロボットシステム１の機能構成の一例を示す図を参照）を設ける。基本的に第一実施形態と異なる点は、統合制御部１０５と各制御部の処理分担と、処理順序とであるため、詳細な説明は省略する。

図１５は、本発明の第二実施形態に係る統合制御部（位置制御、インピーダンス制御、及びビジュアルサーボ制御）の動作の一例を示すフロー図である。

図１５に示すフローが開始されると、統合制御部１０５は、位置制御における軌道計画を行う（ステップＳ５０１）。それから、統合制御部１０５は、インピーダンス制御におけるパラメーターを取得し（ステップＳ５０２）、インピーダンス制御におけるフィルター係数を計算する（ステップＳ５０３）。それから、統合制御部１０５は、ビジュアルサーボ制御における撮像装置３０の初期化を行う（ステップＳ５０４）。

それから、統合制御部１０５は、ＰｏｓＥｎａｂｌｅフラグ、ＩｍｐＥｎａｂｌｅフラグ、及びＶｓＥｎａｂｌｅフラグを「真」に変更する（ステップＳ５０５）。それから、統合制御部１０５は、位置制御の本体処理（後述する図１６）、インピーダンス制御の本体処理（後述する図１７）、及びビジュアルサーボ制御の本体処理（第一実施形態の図１２と同様）を起動する（ステップＳ５０６）。

それから、統合制御部１０５は、主制御部１００からの同期信号Ｔ２を待機し、同期信号Ｔ２を受信した場合に処理をステップＳ５０８に進める（ステップＳ５０７）。

それから、統合制御部１０５は、ビジュアルサーボ制御における出力位置カウンターに１加算する（ステップＳ５０８）。それから、統合制御部１０５は、ビジュアルサーボ制御について、ステップＳ４５３（図１２）で記憶部に保持されている最後の移動指令値及び当該最後の移動指令値の一つ前の移動指令値と、ステップＳ５０８で更新された出力位置カウンターとに基づいて、ビジュアルサーボ制御の出力指令値を求める（ステップＳ５０９）。

それから、統合制御部１０５は、位置制御について、ステップＳ６０３（図１６）で記憶部に保持された移動指令値を取得し、ＰｏｓＯｕｔｐｕｔ［］に代入する（ステップＳ５１０）。また、統合制御部１０５は、インピーダンス制御について、ステップＳ７０３（図１７）で記憶部に保持された移動指令値を取得し、ＩｍｐＯｕｔｐｕｔ［］に代入する（ステップＳ５１０）。また、統合制御部１０５は、ビジュアルサーボ制御について、ステップＳ５０９で計算した出力指令値を取得し、ＶｓＯｕｔｐｕｔ［］に代入する（ステップＳ５１０）。なお、各制御に重みが設定されている場合には、統合制御部１０５は、各制御の指令値に対応する重みを掛けてから、それぞれのＯｕｔｐｕｔ［］に代入する。

それから、統合制御部１０５は、全制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）を終了するか否かを判定する（ステップＳ５１１）。統合制御部１０５は、例えば、ビジュアルサーボ制御について現在画像と目標画像とが一致したか否かを判定し、この条件を満たす場合に、全制御を終了すると判定する。全制御の終了条件は、特に限定されず、例えば、各制御の終了条件を組み合わせてもよい。

全制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）を終了しない場合（ステップＳ５１１：Ｎ）、統合制御部１０５は、処理をステップＳ５０７に戻す。一方、全制御を終了する場合（ステップＳ５１１：Ｙ）、統合制御部１０５は、ＰｏｓＦｉｎｉｓｈフラグ、ＩｍｐＦｉｎｉｓフラグ、及びＶｓＦｉｎｉｓｈフラグを「真」に変更し（ステップＳ５１２）、図１５に示すフローを終了する。

図１６は、位置制御部の動作の一例を示すフロー図である。図１６のフローは、ステップＳ５０６（図１５）で位置制御の本体処理が起動された場合に開始される。

まず、位置制御部１１０は、主制御部１００からの同期信号Ｔ１を待機し、同期信号Ｔ１を受信した場合に処理をステップＳ６０２に進める（ステップＳ６０１）。

それから、位置制御部１１０は、ステップＳ５０１（図１５）で計画した軌道に基づいて今回出力すべき目標角度を特定し、当該目標角度に基づいて移動指令値を生成し（ステップＳ６０２）、主記憶装置９２等の記憶部に保持する（ステップＳ６０３）。

それから、位置制御部１１０は、ＰｏｓＥｎａｂｌｅフラグが「偽」である、又は、ＰｏｓＦｉｎｉｓｈフラグが「真」であるかを判定する（ステップＳ６０４）。ＰｏｓＥｎａｂｌｅフラグが「偽」である、又は、ＰｏｓＦｉｎｉｓｈフラグが「真」である、という条件を満たさない場合（ステップＳ６０４：Ｎ）、位置制御部１１０は、処理をステップＳ６０１に戻す。一方、ＰｏｓＥｎａｂｌｅフラグが「偽」である、又は、ＰｏｓＦｉｎｉｓｈフラグが「真」である場合（ステップＳ６０４：Ｙ）、位置制御部１１０は、図１６に示すフローを終了する。

図１７は、インピーダンス制御部の動作の一例を示すフロー図である。図１７のフローは、ステップ５０６（図１５）でインピーダンス制御の本体処理が起動された場合に開始される。

まず、インピーダンス制御部１２０は、主制御部１００からの同期信号Ｔ１を待機し、同期信号Ｔ１を受信した場合に処理をステップＳ７０２に進める（ステップＳ７０１）。

それから、インピーダンス制御部１２０は、力覚センサー２５からのセンサー情報に基づいてフィルター処理を行って、今回出力すべき目標角度を特定し、当該目標角度に基づいて移動指令値を生成し（ステップＳ７０２）、主記憶装置９２等の記憶部に保持する（ステップＳ７０３）。

それから、インピーダンス制御部１２０は、ＩｍｐＥｎａｂｌｅフラグが「偽」である、又は、ＩｍｐＦｉｎｉｓｈフラグが「真」であるかを判定する（ステップＳ７０４）。ＩｍｐＥｎａｂｌｅフラグが「偽」である、又は、ＩｍｐＦｉｎｉｓｈフラグが「真」である、という条件を満たさない場合（ステップＳ７０４：Ｎ）、インピーダンス制御部１２０は、処理をステップＳ７０１に戻す。一方、ＩｍｐＥｎａｂｌｅフラグが「偽」である、又は、ＩｍｐＦｉｎｉｓｈフラグが「真」である場合（ステップＳ７０４：Ｙ）、インピーダンス制御部１２０は、図１７に示すフローを終了する。

以上、本発明の第二実施形態について説明した。第二実施形態によれば、３つの制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）を、同時に開始させ、同時に終了させることができる。３つの制御によりロボットを動作させる場合に、開始条件と終了条件が一つであるため、制御が簡単になる。

＜第二実施形態の第一変形例＞
第二実施形態では、３つの制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）の並列動作を同時に開始させ、同時に終了させる。しかし、ロボットの作業内容などによっては、２つの制御（位置制御、及びビジュアルサーボ制御）を同時に開始させ、同時に終了させるようにしてもよい。

第二実施形態の第一変形例では、統合制御部１０５の動作が第二実施形態と異なる。従って、異なる点を中心に説明する。

図１８は、第二実施形態の第一変形例に係る統合制御部（位置制御、及びビジュアルサーボ制御）の動作の一例を示すフロー図である。図１８において、図１５と同一の符号を付した処理は、図１５における処理と同一または同様の機能を有するためその詳細な説明を省略する。

図１８のフローが開始されると、統合制御部１０５は、位置制御における軌道計画を行う（ステップＳ５０１）。それから、統合制御部１０５は、ビジュアルサーボ制御における撮像装置３０の初期化を行う（ステップＳ５０４）。それから、統合制御部１０５は、ＰｏｓＥｎａｂｌｅフラグ、及びＶｓＥｎａｂｌｅフラグを「真」に変更する（ステップＳ５０５Ａ）。それから、統合制御部１０５は、位置制御の本体処理（図１６）、及びビジュアルサーボ制御の本体処理（図１２）を起動する（ステップＳ５０６Ａ）。

それから、統合制御部１０５は、主制御部１００からの同期信号Ｔ２を待機し、同期信号Ｔ２を受信した場合に処理をステップＳ５０８に進める（ステップＳ５０７）。それから、統合制御部１０５は、出力位置カウンターに１加算する（ステップＳ５０８）。それから、統合制御部１０５は、ビジュアルサーボ制御の出力指令値を求める（ステップＳ５０９）。

それから、統合制御部１０５は、位置制御について、ステップＳ６０３（図１６）で記憶部に保持された移動指令値を取得し、ＰｏｓＯｕｔｐｕｔ［］に代入する（ステップＳ５１０Ａ）。また、統合制御部１０５は、ビジュアルサーボ制御について、ステップＳ５０９で計算した出力指令値を取得し、ＶｓＯｕｔｐｕｔ［］に代入する（ステップＳ５１０Ａ）。なお、各制御に重みが設定されている場合には、統合制御部１０５は、各制御の指令値に対応する重みを掛けてから、それぞれのＯｕｔｐｕｔ［］に代入する。

それから、統合制御部１０５は、位置制御、及びビジュアルサーボ制御を終了するか否かを判定する（ステップＳ５１１Ａ）。統合制御部１０５は、例えば、ビジュアルサーボ制御について現在画像と目標画像とが一致したか否かを判定し、この条件を満たす場合に、各制御を終了すると判定する。各制御の終了条件は、特に限定されず、例えば、各制御の終了条件を組み合わせてもよい。

位置制御、及びビジュアルサーボ制御を終了しない場合（ステップＳ５１１Ａ：Ｎ）、統合制御部１０５は、処理をステップＳ５０７に戻す。一方、位置制御、及びビジュアルサーボ制御を終了する場合（ステップＳ５１１Ａ：Ｙ）、統合制御部１０５は、ＰｏｓＦｉｎｉｓｈフラグ、及びＶｓＦｉｎｉｓｈフラグを「真」に変更し（ステップＳ５１２Ａ）、図１８に示すフローを終了する。

以上、本発明の第二実施形態の第一変形例について説明した。第二実施形態の第一変形例によれば、２つの制御（位置制御、及びビジュアルサーボ制御）を、同時に開始させ、同時に終了させることができる。２つの制御によりロボットを動作させる場合に、開始条件と終了条件が一つであるため、制御が簡単になる。

＜第二実施形態の第二変形例＞
第二実施形態では、３つの制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）の並列動作を同時に開始させ、同時に終了させる。しかし、ロボットの作業内容などによっては、２つの制御（インピーダンス制御、及びビジュアルサーボ制御）を同時に開始させ、同時に終了させるようにしてもよい。

第二実施形態の第二変形例では、統合制御部１０５の動作が第二実施形態と異なる。従って、異なる点を中心に説明する。

図１９は、第二実施形態の第二変形例に係る統合制御部（インピーダンス制御、及びビジュアルサーボ制御）の動作の一例を示すフロー図である。図１９において、図１５と同一の符号を付した処理は、図１５における処理と同一または同様の機能を有するためその詳細な説明を省略する。

図１９のフローが開始されると、統合制御部１０５は、インピーダンス制御におけるパラメーターを取得し（ステップＳ５０２）、インピーダンス制御におけるフィルター係数を計算する（ステップＳ５０３）。それから、統合制御部１０５は、ビジュアルサーボ制御における撮像装置３０の初期化を行う（ステップＳ５０４）。

それから、統合制御部１０５は、ＩｍｐＥｎａｂｌｅフラグ、及びＶｓＥｎａｂｌｅフラグを「真」に変更する（ステップＳ５０５Ｂ）。それから、統合制御部１０５は、インピーダンス制御の本体処理（図１７）、及びビジュアルサーボ制御の本体処理（図１２）を起動する（ステップＳ５０６Ｂ）。

それから、統合制御部１０５は、主制御部１００からの同期信号Ｔ２を待機し、同期信号Ｔ２を受信した場合に処理をステップＳ５０８に進める（ステップＳ５０７）。それから、統合制御部１０５は、出力位置カウンターに１加算する（ステップＳ５０８）。それから、統合制御部１０５は、ビジュアルサーボ制御の出力指令値を求める（ステップＳ５０９）。

それから、統合制御部１０５は、インピーダンス制御について、ステップＳ７０３（図１７）で記憶部に保持された移動指令値を取得し、ＩｍｐＯｕｔｐｕｔ［］に代入する（ステップＳ５１０Ｂ）。また、統合制御部１０５は、ビジュアルサーボ制御について、ステップＳ５０９で計算した出力指令値を取得し、ＶｓＯｕｔｐｕｔ［］に代入する（ステップＳ５１０Ｂ）。なお、各制御に重みが設定されている場合には、統合制御部１０５は、各制御の指令値に対応する重みを掛けてから、それぞれのＯｕｔｐｕｔ［］に代入する。

それから、統合制御部１０５は、インピーダンス制御、及びビジュアルサーボ制御を終了するか否かを判定する（ステップＳ５１１Ｂ）。統合制御部１０５は、例えば、ビジュアルサーボ制御について現在画像と目標画像とが一致したか否かを判定し、この条件を満たす場合に、各制御を終了すると判定する。各制御の終了条件は、特に限定されず、例えば、各制御の終了条件を組み合わせてもよい。

インピーダンス制御、及びビジュアルサーボ制御を終了しない場合（ステップＳ５１１Ｂ：Ｎ）、統合制御部１０５は、処理をステップＳ５０７に戻す。一方、インピーダンス制御、及びビジュアルサーボ制御を終了する場合（ステップＳ５１１Ｂ：Ｙ）、統合制御部１０５は、ＩｍｐＦｉｎｉｓｈフラグ、及びＶｓＦｉｎｉｓｈフラグを「真」に変更し（ステップＳ５１２Ｂ）、図１９に示すフローを終了する。

以上、本発明の第二実施形態の第二変形例について説明した。第二実施形態の第二変形例によれば、２つの制御（インピーダンス制御、及びビジュアルサーボ制御）を、同時に開始させ、同時に終了させることができる。２つの制御によりロボットを動作させる場合に、開始条件と終了条件が一つであるため、制御が簡単になる。

＜第二実施形態の第三変形例＞
第二実施形態では、３つの制御（位置制御、インピーダンス制御、及びビジュアルサーボ制御）の並列動作を同時に開始させ、同時に終了させる。しかし、ロボットの作業内容などによっては、２つの制御（位置制御、及びインピーダンス制御）を同時に開始させ、同時に終了させるようにしてもよい。

第二実施形態の第三変形例では、統合制御部１０５の動作が第二実施形態と異なる。従って、異なる点を中心に説明する。

図２０は、第二実施形態の第三変形例に係る統合制御部（位置制御、及びインピーダンス制御）の動作の一例を示すフロー図である。図２０において、図１５と同一の符号を付した処理は、図１５における処理と同一または同様の機能を有するためその詳細な説明を省略する。

図２０のフローが開始されると、統合制御部１０５は、統合制御部１０５は、位置制御における軌道計画を行う（ステップＳ５０１）。それから、統合制御部１０５は、インピーダンス制御におけるパラメーターを取得し（ステップＳ５０２）、インピーダンス制御におけるフィルター係数を計算する（ステップＳ５０３）。

それから、統合制御部１０５は、ＰｏｓＥｎａｂｌｅフラグ、及びＩｍｐＥｎａｂｌｅフラグを「真」に変更する（ステップＳ５０５Ｃ）。それから、統合制御部１０５は、位置制御の本体処理（図１６）、及びインピーダンス制御の本体処理（図１７）を起動する（ステップＳ５０６Ｃ）。

それから、統合制御部１０５は、主制御部１００からの同期信号Ｔ２を待機し、同期信号Ｔ２を受信した場合に処理をステップＳ５１０Ｃに進める（ステップＳ５０７）。

それから、統合制御部１０５は、位置制御について、ステップＳ６０３（図１６）で記憶部に保持された移動指令値を取得し、ＰｏｓＯｕｔｐｕｔ［］に代入する（ステップＳ５１０Ｃ）。また、統合制御部１０５は、インピーダンス制御について、ステップＳ７０３（図１７）で記憶部に保持された移動指令値を取得し、ＩｍｐＯｕｔｐｕｔ［］に代入する（ステップＳ５１０Ｃ）。なお、各制御に重みが設定されている場合には、統合制御部１０５は、各制御の指令値に対応する重みを掛けてから、それぞれのＯｕｔｐｕｔ［］に代入する。

それから、統合制御部１０５は、位置制御、及びインピーダンス制御を終了するか否かを判定する（ステップＳ５１１Ｃ）。統合制御部１０５は、例えば、位置制御について経路上の最終的な目標位置への移動指令値を出力したか否かを判定し、この条件を満たす場合に、各制御を終了すると判定する。各制御の終了条件は、特に限定されず、例えば、各制御の終了条件を組み合わせてもよい。

位置制御、及びインピーダンス制御を終了しない場合（ステップＳ５１１Ｃ：Ｎ）、統合制御部１０５は、処理をステップＳ５０７に戻す。一方、位置制御、及びインピーダンス制御を終了する場合（ステップＳ５１１Ｃ：Ｙ）、統合制御部１０５は、ＰｏｓＦｉｎｉｓｈフラグ、及びＩｍｐＦｉｎｉｓｈフラグを「真」に変更し（ステップＳ５１２Ｃ）、図２０に示すフローを終了する。

以上、本発明の第二実施形態の第三変形例について説明した。第二実施形態の第三変形例によれば、２つの制御（位置制御、及びインピーダンス制御）を、同時に開始させ、同時に終了させることができる。２つの制御によりロボットを動作させる場合に、開始条件と終了条件が一つであるため、制御が簡単になる。

なお、上述した各フロー図の各処理単位は、ロボット制御装置１０の処理を理解容易にするために、主な処理内容に応じて分割したものである。処理単位の分割の仕方や名称によって、本願発明が制限されることはない。ロボット制御装置１０の処理は、処理内容に応じて、さらに多くの処理単位に分割することもできる。また、１つの処理単位がさらに多くの処理を含むように分割することもできる。

以上、本発明について実施形態を用いて説明したが、本発明の技術的範囲は上記実施形態に記載の範囲には限定されない。上記実施形態に多様な変更または改良を加えることが可能であることが当業者には明らかである。また、そのような変更または改良を加えた形態も本発明の技術的範囲に含まれ得ることが、特許請求の範囲の記載から明らかである。本発明は、ロボットと、ロボット制御装置等とを別個に有するロボットシステムとして提供してもよいし、ロボットにロボット制御装置等が含まれるロボットとして提供してもよいし、ロボット制御装置として提供してもよい。また、本発明は、ロボット等を制御する方法、ロボット等を制御するプログラム、当該プログラムを記憶した記憶媒体として提供することもできる。

１：ロボットシステム、１０：ロボット制御装置、２０：ロボット、２１：可動部、２２：アーム、２３：ジョイント、２４：リンク、２５：力覚センサー、２６：ハンド、３０：撮像装置、９０：コンピューター、９１：演算装置、９２：主記憶装置、９３：補助記憶装置、９４：通信Ｉ／Ｆ、９５：入力装置、９６：表示装置、９７：読み書き装置、１００：主制御部、１０５：統合制御部、１１０：位置制御部、１２０：インピーダンス制御部、１３０：ビジュアルサーボ制御部、１４０：センサー情報取得部、１５０：画像取得部、２００：駆動制御部

Claims (13)

1. ロボットの可動部のエンドポイントを移動させる第一指令値をそれぞれ出力する複数の第一制御部と、
   前記複数の第一制御部により出力されるそれぞれの第一指令値を足し合わせた第二指令値を用いて前記可動部を制御する第二制御部と、
   を有し、
   前記複数の第一制御部はそれぞれ、第一の同期タイミングをトリガーとして前記エンドポイントを移動させる指令値を生成する本体処理を実行し、第二の同期タイミングをトリガーとして前記本体処理により生成した前記指令値又は前記指令値に基づいて計算した指令値を前記第一指令値として前記第二制御部に出力する出力処理を実行し、
   前記第二制御部は、前記第二の同期タイミングをトリガーとして前記出力処理により出力される前記第一指令値を足し合わせることにより前記第二指令値を生成し、
   前記第一の同期タイミングは、第一の周期で発生し、
   前記第二の同期タイミングは、前記第一の同期タイミングから、前記第一の同期タイミングの間隔よりも短い所定時間後に発生し、
   前記複数の第一制御部の本体処理のうち少なくとも一つの本体処理の処理時間は、前記第一の同期タイミングから前記第二の同期タイミングが発生するまでの所定時間よりも短く、
   前記複数の第一制御部の本体処理のうち少なくとも一つの本体処理の処理時間は、前記第一の周期よりも長い、
   ことを特徴とするロボット制御装置。
2. 請求項１に記載のロボット制御装置であって、
   前記出力処理は、当該出力処理を実行する際に前記本体処理が実行されている場合、前記本体処理よりも優先して実行され、前記実行されている本体処理より前に実行された本体処理により生成された指令値に基づいて計算した指令値を前記第一指令値として出力する、
   ことを特徴とするロボット制御装置。
3. 請求項２に記載のロボット制御装置であって、
   前記出力処理は、前記本体処理よりも優先して実行される場合、前記実行されている本体処理より前に実行された本体処理により生成された指令値を、前記実行されている本体処理の開始後に発生した前記第二の同期タイミングの回数に応じた割合で指令値を計算し、前記第一指令値として出力する、
   ことを特徴とするロボット制御装置。
4. 請求項３に記載のロボット制御装置であって、
   前記複数の第一制御部は、設定された経路に従って前記エンドポイントを移動させる第一指令値を生成する位置制御と、前記エンドポイントが目標位置にあるときの前記エンドポイントを含む画像である目標画像、および前記エンドポイントが現在位置にあるときの前記エンドポイントを含む画像である現在画像に基づいて、前記エンドポイントを前記現在位置から前記目標位置に移動させる第一指令値を生成する視覚サーボ制御とであり、
   前記位置制御の本体処理の処理時間は、前記第一の同期タイミングから前記第二の同期タイミングが発生するまでの所定時間よりも短く、
   前記視覚サーボ制御の本体処理の処理時間は、前記第一の周期よりも長い、
   ことを特徴とするロボット制御装置。
5. 請求項３に記載のロボット制御装置であって、
   前記複数の第一制御部は、前記エンドポイントが目標位置にあるときの前記エンドポイントを含む画像である目標画像、および前記エンドポイントが現在位置にあるときの前記エンドポイントを含む画像である現在画像に基づいて、前記エンドポイントを前記現在位置から前記目標位置に移動させる第一指令値を生成する視覚サーボ制御と、前記可動部に作用する力を検出する力検出部からの出力に基づいて前記エンドポイントを移動させる第一指令値を生成するインピーダンス制御とであり、
   前記インピーダンス制御の本体処理は、前記第一の同期タイミングから前記第二の同期タイミングが発生するまでの所定時間よりも短く、
   前記視覚サーボ制御の本体処理は、前記第一の周期よりも長い、
   ことを特徴とするロボット制御装置。
6. 請求項５に記載のロボット制御装置であって、
   前記複数の第一制御部が出力する第一指令値を保持する記憶部を有し、
   前記インピーダンス制御は、前記記憶部から他の制御が出力した第一指令値を取得し、当該取得した第一指令値を用いて逆ヤコビ行列の要素を設定する、
   ことを特徴とするロボット制御装置。
7. 請求項１〜４いずれか一項に記載のロボット制御装置であって、
   前記複数の第一制御部の処理のそれぞれは、それぞれについて予め設定された開始条件が満たされた場合に開始され、それぞれについて予め設定された終了条件が満たされた場合に終了し、
   前記第二制御部は、前記複数の第一制御部による処理が実行されている場合に、前記複数の第一制御部のうち一部の第一制御部の処理が終了した場合には、当該終了した第一制御部から最後に出力された第一指令値と、終了していない他の第一制御部から出力される第一指令値とを用いて前記第二指令値を生成する、
   ことを特徴とするロボット制御装置。
8. 請求項１〜４いずれか一項に記載のロボット制御装置であって、
   前記複数の第一制御部の処理は、予め設定された開始条件が満たされた場合に同時に開始され、予め設定された終了条件が満たされた場合に同時に終了する、
   ことを特徴とするロボット制御装置。
9. 請求項１〜８いずれか一項に記載のロボット制御装置であって、
   前記複数の第一制御部は、それぞれについて設定された所定の重みで前記第一指令値に重み付けをして出力する、
   ことを特徴とするロボット制御装置。
10. 可動部と、
    前記可動部のエンドポイントを移動させる第一指令値をそれぞれ出力する複数の第一制御部と、
    前記複数の第一制御部により出力されるそれぞれの第一指令値を足し合わせた第二指令値を用いて前記可動部を制御する第二制御部と、
    を有し、
    前記複数の第一制御部はそれぞれ、第一の同期タイミングをトリガーとして前記エンドポイントを移動させる指令値を生成する本体処理を実行し、第二の同期タイミングをトリガーとして前記本体処理により生成した前記指令値又は前記指令値に基づいて計算した指令値を前記第一指令値として前記第二制御部に出力する出力処理を実行し、
    前記第二制御部は、前記第二の同期タイミングをトリガーとして前記出力処理により出力される前記第一指令値を足し合わせることにより前記第二指令値を生成し、
    前記第一の同期タイミングは、第一の周期で発生し、
    前記第二の同期タイミングは、前記第一の同期タイミングから、前記第一の同期タイミングの間隔よりも短い所定時間後に発生し、
    前記複数の第一制御部の本体処理のうち少なくとも一つの本体処理の処理時間は、前記第一の同期タイミングから前記第二の同期タイミングが発生するまでの所定時間よりも短く、
    前記複数の第一制御部の本体処理のうち少なくとも一つの本体処理の処理時間は、前記第一の周期よりも長い、
    ことを特徴とするロボット。
11. 可動部を有するロボットと、
    前記可動部のエンドポイントを移動させる第一指令値をそれぞれ出力する複数の第一制御部と、
    前記複数の第一制御部により出力されるそれぞれの第一指令値を足し合わせた第二指令値を用いて前記可動部を制御する第二制御部と、
    を有し、
    前記複数の第一制御部はそれぞれ、第一の同期タイミングをトリガーとして前記エンドポイントを移動させる指令値を生成する本体処理を実行し、第二の同期タイミングをトリガーとして前記本体処理により生成した前記指令値又は前記指令値に基づいて計算した指令値を前記第一指令値として前記第二制御部に出力する出力処理を実行し、
    前記第二制御部は、前記第二の同期タイミングをトリガーとして前記出力処理により出力される前記第一指令値を足し合わせることにより前記第二指令値を生成し、
    前記第一の同期タイミングは、第一の周期で発生し、
    前記第二の同期タイミングは、前記第一の同期タイミングから、前記第一の同期タイミングの間隔よりも短い所定時間後に発生し、
    前記複数の第一制御部の本体処理のうち少なくとも一つの本体処理の処理時間は、前記第一の同期タイミングから前記第二の同期タイミングが発生するまでの所定時間よりも短く、
    前記複数の第一制御部の本体処理のうち少なくとも一つの本体処理の処理時間は、前記第一の周期よりも長い、
    ことを特徴とするロボットシステム。
12. ロボットの可動部のエンドポイントを移動させる第一指令値をそれぞれ出力する複数の第一制御ステップと、
    前記複数の第一制御ステップにより出力されるそれぞれの第一指令値を足し合わせた第二指令値を用いて前記可動部を制御する第二制御ステップと、
    を含み、
    前記複数の第一制御ステップはそれぞれ、第一の同期タイミングをトリガーとして前記エンドポイントを移動させる指令値を生成する本体処理を実行し、第二の同期タイミングをトリガーとして前記本体処理により生成した前記指令値又は前記指令値に基づいて計算した指令値を前記第一指令値として前記第二制御ステップに出力する出力処理を実行し、
    前記第二制御ステップは、前記第二の同期タイミングをトリガーとして前記出力処理により出力される前記第一指令値を足し合わせることにより前記第二指令値を生成し、
    前記第一の同期タイミングは、第一の周期で発生し、
    前記第二の同期タイミングは、前記第一の同期タイミングから、前記第一の同期タイミングの間隔よりも短い所定時間後に発生し、
    前記複数の第一制御ステップの本体処理のうち少なくとも一つの本体処理の処理時間は、前記第一の同期タイミングから前記第二の同期タイミングが発生するまでの所定時間よりも短く、
    前記複数の第一制御ステップの本体処理のうち少なくとも一つの本体処理の処理時間は、前記第一の周期よりも長い、
    ことを特徴とするロボット制御方法。
13. ロボット制御装置のプログラムであって、
    ロボットの可動部のエンドポイントを移動させる第一指令値をそれぞれ出力する複数の第一制御部と、
    前記複数の第一制御部により出力されるそれぞれの第一指令値を足し合わせた第二指令値を用いて前記可動部を制御する第二制御部として、
    前記ロボット制御装置を機能させ、
    前記複数の第一制御部はそれぞれ、第一の同期タイミングをトリガーとして前記エンドポイントを移動させる指令値を生成する本体処理を実行し、第二の同期タイミングをトリガーとして前記本体処理により生成した前記指令値又は前記指令値に基づいて計算した指令値を前記第一指令値として前記第二制御部に出力する出力処理を実行し、
    前記第二制御部は、前記第二の同期タイミングをトリガーとして前記出力処理により出力される前記第一指令値を足し合わせることにより前記第二指令値を生成し、
    前記第一の同期タイミングは、第一の周期で発生し、
    前記第二の同期タイミングは、前記第一の同期タイミングから、前記第一の同期タイミングの間隔よりも短い所定時間後に発生し、
    前記複数の第一制御部の本体処理のうち少なくとも一つの本体処理の処理時間は、前記第一の同期タイミングから前記第二の同期タイミングが発生するまでの所定時間よりも短く、
    前記複数の第一制御部の本体処理のうち少なくとも一つの本体処理の処理時間は、前記第一の周期よりも長い、
    ことを特徴とするプログラム。

Images