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

Description

本発明は、制御装置、制御方法及びロボット装置に関する。

カメラ画像とロボットの座標の関係を求めるキャリブレーション作業を必要としないで、高精度な部品の位置決めを行う技術として、ビジュアルサーボ技術が知られている。しかしながら、ビジュアルサーボ技術は、一般的に部品を所定の位置に移動させる軌道を決定することが出来ないため、部品同士が接触することを前提とする組み立て作業にビジュアルサーボ技術を適用すると、組み立ての過程で過大な力が生じ、部品を破壊してしまうことがあった。

一方、組み立てを行うロボットにおいて、部品を破壊しないように組み立てを行うために、コンプライアントモーション制御技術が知られている。例えば、特許文献１には、ワークの組付作業を自動的に行う組立用ロボットの制御方法であって、組付動作を開始する前に視覚情報に基づいて組立用ロボットの動作を補正する段階と、組付動作を開始した後に力覚情報に基づいて組立用ロボットの動作を補正する段階と、を有することを特徴とする組立用ロボットの制御方法が開示されている。

特開平８−１７４４５７号公報

ここで、特許文献１の制御方法では、力覚情報に基づいて力を制御するロボットの軸を、部品と環境との関係からその向きを決定することができない。そのため、部品が被組み付け部品と接触して、部品と環境との関係が想定した関係からずれると、力の制御が所望の精度を満たさず、部品を破壊してしまうという問題があった。

そこで本発明は、上記問題に鑑みてなされたものであり、対象物が他の物体と接触しても対象物を破壊することを防ぐことを可能とする制御装置、制御方法及びロボット装置を提供することを課題とする。

（１）本発明は前記事情に鑑みなされたもので、本発明の一態様は、ロボットが把持する対象物の撮像画像に基づいて前記対象物と前記対象物を組み付ける被組み付け部品との相対的な位置関係を検出し、検出した前記位置関係に基づいて前記ロボットのマニピュレーターを制御する第１制御信号を生成するビジュアル制御部と、前記ビジュアル制御部が検出した前記位置関係と前記対象物にかかっている力を示す負荷情報とに基づいて、前記マニピュレーターを制御する第２制御信号を生成し、生成した前記第２制御信号と前記ビジュアル制御部が生成した前記第１制御信号との両方に基づいて、前記ロボットのマニピュレーターを制御するコンプライアントモーション制御部と、を備えることを特徴とする制御装置である。
この構成によれば、制御装置は、ロボットが把持する対象物の撮像画像に基づいて対象物と対象物を組み付ける被組み付け部品との相対的な位置関係を検出し、検出した当該位置関係に基づいて第１制御信号を生成し、検出した当該位置関係と対象物にかかっている力を示す負荷情報とに基づいて第２制御信号を生成し、生成した第２制御信号と第１制御信号との両方に基づいてロボットのマニピュレーターを制御する。これにより、制御装置は、対象物の撮像画像に基づいて対象物を移動させつつ、対象物が他の物体と接触しても対象物に過大な力がかからないように対象物を移動させることができる。
その結果、制御装置は、対象物を破壊することなく対象物を適切な位置へ移動させることができる。

（２）上記に記載の制御装置において、本発明の一態様は、前記コンプライアントモーション制御部は、前記ビジュアル制御部が検出した位置関係と前記負荷情報とに基づいて、ロボットのマニピュレーターを制御する第２制御信号を生成する負荷変位変換部と、前記第１制御信号に基づいて制御信号を生成する軌道生成部を備え、前記第１制御信号と前記軌道生成部が生成した制御信号とに基づいて、前記ロボットのマニピュレーターを制御することを特徴とする。
この構成によれば、制御装置は、コンプライアントモーション制御部による制御とビジュアル制御部による制御とを常に実行することができ、ロボットが把持する対象物に過大な力がかかることを常に防ぎつつ、対象物を目標の位置に向かって移動させることができる。

（３）上記に記載の制御装置において、本発明の一態様は、前記軌道生成部は、前記第１制御信号を、前記負荷変位変換部が生成する第２制御信号の生成周期と同じ生成周期となるように補間した前記制御信号を生成することを特徴とする。
この構成によれば、制御装置は、コンプライアントモーション制御部による制御とビジュアル制御部による制御とを常に実行することができ、ロボットが把持する対象物に過大な力がかかることを常に防ぎつつ、対象物を目標の位置に向かって移動させることができる。

（４）上記に記載の制御装置において、本発明の一態様は、力センサーが検出した前記ロボットに働く力に基づいて、前記対象物にかかっている力を示す負荷情報を算出する負荷算出部を備え、前記負荷変位変換部は、前記負荷算出部が算出した負荷情報に基づいて、前記第２制御信号を生成することを特徴とする。
この構成によれば、制御装置は、力センサーが検出した前記ロボットに働く力に基づいて、前記対象物にかかっている力を示す負荷情報を算出し、その負荷情報に基づいて、前記第２制御信号を生成するので、コンプライアントモーション制御部による制御を実行することができ、ロボットが把持する対象物に過大な力がかかることを防ぐことができる。

（５）上記に記載の制御装置において、本発明の一態様は、前記負荷変位変換部は、
前記負荷情報を、前記力センサーを基準とするセンサー座標系から前記対象物を基準とする物体座標系に変換する第１座標変換部と、前記第１座標変換部によって変換された後の負荷情報に対して、前記対象物の質量とダンパ係数とばね定数とに基づいて決定される伝達関数を乗じる伝達関数乗算部と、前記伝達関数乗算部が乗じることにより得られた負荷情報を前記物体座標系から前記ロボットを基準とするロボット座標系に変換することで、前記第２制御信号を生成する第２座標変換部と、を備えることを特徴とする。
この構成によれば、制御装置は、ロボット座標系において、力センサーが検出した力に
応じてロボットを動かす位置を決定することができるので、ロボットが把持する対象物に過大な力がかかることを防ぐことができ、対象物を破壊することを防止することができる。

（６）上記に記載の制御装置において、本発明の一態様は、前記ビジュアル制御部は、
現在の対象物を撮像した現在画像と、該対象物を目標の位置に移動させた状態のゴール画像との比較に基づいて、前記第１制御信号を生成することを特徴とする。
この構成によれば、制御装置は、現在画像とゴール画像との比較に基づいて第１制御信
号を生成し、この第１制御信号に基づいてロボットのマニピュレーターを制御するので、対象物を目標の位置に向かうように移動させることができる。

（７）上記に記載の制御装置において、本発明の一態様は、前前記対象物が目標の位置に到達したか否か判定する判定部を備え、前記コンプライアントモーション制御部は、前記判定部により前記対象物が目標の位置に到達したと判定されるまで、前記ロボットのマニピュレーターの制御を続けることを特徴とする。
この構成によれば、制御装置は、対象物が目標の位置に到達するまで、対象物の撮像画像に基づいて対象物を移動させつつ、対象物が他の物体と接触しても対象物に過大な力がかからないように対象物を移動させることができる。その結果、制御装置は、対象物を破壊せずに対象物を組み立てることができる。

（８）上記に記載の制御装置において、本発明の一態様は、前記対象物が目標の位置に到達したか否か、及び前記対象物にかかっている力が目標の力に到達したか否か判定する判定部を備え、前記コンプライアントモーション制御部は、前記判定部により前記対象物が目標の位置に到達したと判定され、かつ前記対象物にかかっている力が目標の力に到達したと判定されるまで、前記ロボットのマニピュレーターの制御を続けることを特徴とする。
この構成によれば、制御装置は、対象物が目標の位置に到達するまで、対象物の撮像画像に基づいて対象物を移動させつつ、対象物が他の物体と接触しても対象物に過大な力がかからないように対象物を移動させることができる。その結果、制御装置は、対象物を破壊せずに対象物を組み立てることができる。また、制御装置は、少なくともある１つの向きに対して、力を目標の値に近づけるようにマニピュレーターを制御するので、対象物に過大な力がかからないように移動させるだけでなく、ハンドプレス機を用いたプレス作業などを行わせることができる。

（９）本発明の一態様は、ロボットが把持する対象物の撮像画像に基づいて前記対象物と前記対象物を組み付ける被組み付け部品との相対的な位置関係を検出し、検出した前記位置関係と前記対象物にかかっている力を示す負荷情報とに基づいて、ビジュアルサーボによる前記ロボットの移動制御とインピーダンス制御による前記ロボットの移動制御を並行して実行することを特徴とする制御装置である。
この構成によれば、制御装置は、ロボットが把持する対象物の撮像画像に基づいて前記対象物と前記対象物を組み付ける被組み付け部品との相対的な位置関係を検出し、検出した前記位置関係と前記対象物にかかっている力を示す負荷情報とに基づいて、ビジュアルサーボによる前記ロボットの移動制御とインピーダンス制御による前記ロボットの移動制御を並行して実行する。これにより、制御装置は、対象物の撮像画像に基づいて対象物を移動させつつ、対象物が他の物体と接触しても対象物に過大な力がかからないように対象物を移動させることができる。

（１０）本発明の一態様は、ビジュアル制御部が、ロボットが把持する対象物の撮像画像に基づいて前記対象物と前記対象物を組み付ける被組み付け部品との相対的な位置関係を検出し、検出した前記位置関係に基づいて前記ロボットのマニピュレーターを制御する第１制御信号を生成する手順と、コンプライアントモーション制御部が、前記ビジュアル制御部が検出した前記位置関係と前記対象物にかかっている力を示す負荷情報とに基づいて、前記マニピュレーターを制御する第２制御信号を生成し、生成した前記第２制御信号と前記ビジュアル制御部が生成した前記第１制御信号との両方に基づいて、前記ロボットのマニピュレーターを制御する手順と、を有することを特徴とする制御方法である。

（１１）本発明の一態様は、ロボットが把持する対象物の撮像画像に基づいて前記対象物と前記対象物を組み付ける被組み付け部品との相対的な位置関係を検出し、検出した前記位置関係に基づいて前記ロボットのマニピュレーターを制御する第１制御信号を生成するビジュアル制御部と、前記ビジュアル制御部が検出した前記位置関係と前記対象物にかかっている力を示す負荷情報とに基づいて、前記マニピュレーターを制御する第２制御信号を生成し、生成した前記第２制御信号と前記ビジュアル制御部が生成した前記第１制御信号との両方に基づいて、前記ロボットのマニピュレーターを制御するコンプライアントモーション制御部と、を備えることを特徴とするロボット装置である。

第１の実施形態におけるロボット装置の概略斜視図である。 第１の実施形態における座標系について説明するための図である。 第１の実施形態における制御装置の概略ブロック図である。 第１の実施形態におけるゴール画像の一例である。 第１の実施形態における動作制御部の概略ブロック図である。 第１の実施形態における補間部の処理を説明するための図である。 第１の実施形態におけるビジュアル制御部の概略ブロック図である。 第１の実施形態における負荷変位変換部の概略ブロック図である。 センサー座標系と物体座標系を説明するための図である。 第１の実施形態におけるロボット制御部の概略ブロック図である。 第１の実施形態における位置指示信号が示す位置を説明するための図である。 第１の実施形態における制御装置の処理の流れの一例を示すフローチャートである。 第２の実施形態におけるロボット装置の概略斜視図である。 第２の実施形態における制御装置の概略ブロック図である。 第２の実施形態における制御装置の処理の流れの一例を示すフローチャートである。 第２の実施形態におけるロボット装置の概略斜視図である。

以下、本発明の実施形態について、図面を参照して詳細に説明する。
［第１の実施形態］
図１は、第１の実施形態におけるロボット装置１の概略斜視図である。図１に示すように、ロボット装置１は、制御装置１０、多関節ロボット（以下、ロボットともいう）２０を備える。なお、図１における部品や構造等の縮尺は、図を明瞭なものとするために実際のものとは異なっている。

制御装置１０は、ロボット２０の動作を制御する。制御装置１０による制御の詳細は後述する。
ロボット２０は、地面に固定された支持台２０ａ、旋回可能及び回転可能に支持台２０ａに連結されたマニピュレーター部（マニピュレーター）２０ｂ、マニピュレーター部２０ｂに連結された把持部２０ｃ、力センサー２０ｄ及び撮像装置３０を備える。また、把持部２０ｃは開閉機能を有する。これにより、把持部２０ｃは対象となる物体（以下、対象物という）を把持することができる。本実施形態では、対象物の一例として部品２００を用いて説明する。

マニピュレーター部２０ｂは、例えば６軸の垂直多関節型のマニピュレーターであり、把持部２０ｃが把持または挟持する部品２００の位置および向きを自在に変更することができる。なお、本実施形態においては、把持または挟持することを「把持する」として説明する。ロボット２０は、制御装置１０による制御によって、マニピュレーター部２０ｂ、把持部２０ｃのうちいずれか一つまたは組み合わせを動かす。

力センサー２０ｄは、把持部２０ｃに加わる力とモーメントを予め決められたレート（本実施形態では、一例として１ｋＨｚ）で検出する。力センサー２０ｄは、例えば６軸の力センサーである。力センサー２０ｄは、検出した力とモーメントを示す把持部負荷情報を制御装置１０へ出力する。
なお、ロボット２０の自由度は６軸によるものに限られない。また、支持台２０ａは、床、壁、天井等、地面に対して固定された場所に設置してもよい。
また、不図示のエンコーダー２１は、ロボット２０の各関節の角度を検出し、検出した各関節の角度を制御装置１０へ送信する。

部品（対象物）２００は、被組み付け部品２１０に組み付けられる部品である。部品２００は、一例として、直方体または立方体である。
被組み付け部品２１０は、部品２００が組み合わされる穴２１１を有している。なお、穴２１１は、被組み付け部品２１０を貫通していても、貫通していなくてもよい。
なお、符号２２０は、後述するように、部品２００と被組み付け部品２１０との接触状態を、２次元で表す場合に用いる平面である。

撮像装置３０は、例えば、部品２００と被組み付け部品２１０の穴２１１との嵌め合わせた状態が確認できる位置に設置されている。撮像装置３０は、例えばＣＣＤ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）カメラである。撮像装置３０は、予め決められたフレームレート（本実施形態では、一例として３０Ｈｚ）で部品２００と被組み付け部品２１０の穴２１１が一枚の画像に収まるように撮像する。撮像装置３０は、撮像により得られた撮像画像（以下、カメラ画像ともいう）を示す画像データを制御装置１０へ出力する。

図２は、第１の実施形態における座標系について説明するための図である。同図において、ΣＲはマニピュレーター部２０ｂを基準とするロボット座標系（ＸＹＺ座標系）、ΣＳは力センサー２０ｄを基準とするセンサー座標系（ｘ’ｙ’ｚ’座標系）、ΣＯは物体を基準とする物体座標系（ｘｙｚ座標系）、ΣＣは撮像装置３０を基準とするカメラ座標系（ｕｖｗ座標系）である。

図３は、第１の実施形態における制御装置１０の概略ブロック図である。制御装置１０は、把持指示部１１と、開始位置移動指示部１２と、位置姿勢検出部１３と、ゴール画像
生成部１４と、記憶部１５と、動作制御部１６と、画像取得部１７と、類似度算出部１８と、判定部１９と、負荷算出部４０とを備える。

第１の実施形態における制御装置１０は、ビジュアルサーボのループの中にインピーダンス制御を組み込むことで、部品２００を破壊しないで、部品２００と被組み付け部品２１０が接触した組み付け後の状態にする。より詳しくは、ビジュアルサーボ技術を用いて、部品２００をテンプレート画像と同じ状態にするにあたって、撮像装置３０が撮像した画像データを用いて現在の部品２００の位置及び姿勢を検出する。制御装置１０は、検出した部品２００の位置及び姿勢に基づいて、インピーダンス制御によってロボット２０を動かす際に、その動作を規定する軸を決定する。

把持指示部１１は、部品２００を把持部２０ｃで把持することを指示する把持指示信号をロボット２０へ送信する。これにより、ロボット２０は、部品２００を把持部２０ｃで把持する。
開始位置移動指示部１２は、マニピュレーター部２０ｂを制御して、部品２００を所定の組み付け作業開始位置に移動する。

位置姿勢検出部１３は、撮像装置３０に部品２００及び被組み付け部品２１０を撮像させ、撮像により得られた画像データを取得する。位置姿勢検出部１３は、取得した画像データを用いて、部品２００及び被組み付け部品２１０の位置及び姿勢を検出する。具体的には、例えば、位置姿勢検出部１３は、予め部品２００及び被組み付け部品２１０の取り得る位置及び姿勢の全ての状態において撮像されたカメラ画像をそれぞれ取得し、取得したカメラ画像を位置及び姿勢と関連付けて保持しておく。そして、位置姿勢検出部１３は、例えば、撮像装置３０から取得した画像データと保持しているカメラ画像とを比較し、最も近いカメラ画像に対応する位置及び姿勢を抽出することで、部品２００及び被組み付け部品２１０の位置及び姿勢を検出する。
そして、位置姿勢検出部１３は、検出した被組み付け部品２１０の位置及び姿勢を示す情報を画像データとともにゴール画像生成部１４へ出力する。
なお、本実施形態における位置姿勢検出部１３は、予め部品の位置及び姿勢を変化させた状態のカメラ画像を取得しておいて、それらとの比較により現在の部品の位置及び姿勢を検出したが、これに限ったものではない。位置姿勢検出部１３は、部品の位置及び姿勢を変化させた状態をＣＧ（Ｃｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ）で再現して、それらとの比較により現在の位置及び姿勢を検出してもよい。

ゴール画像生成部１４は、位置姿勢検出部１３が検出した被組み付け部品２１０の位置及び姿勢に基づいて、ゴール画像を生成する。ここで、ゴール画像は、対象物を目標の位置に移動させた場合の画像であり、本実施形態では、一例として、部品２００が被組み付け部品２１０に組み付けられた状態の画像である。具体的には、例えば、ゴール画像生成部１４は、被組み付け部品２１０の取り得る位置及び姿勢の全ての状態において、部品２００を組み付けた後のカメラ画像をあらかじめ取得し保持しておく。そして、例えば、ゴール画像生成部１４は、その中から位置姿勢検出部１３によって検出された撮像装置３０で取得した画像に写った被組み付け部品２１０の位置及び姿勢に最も近いカメラ画像をゴール画像として抽出する。

ゴール画像生成部１４は、生成したゴール画像を動作制御部１６へ出力する。ゴール画像生成部１４は、生成したゴール画像を記憶部１５に記憶させる。これにより、記憶部１５は、ゴール画像を保持する。また、記憶部１５には、部品２００の３次元モデルＩｍが予め記憶されている。

なお、記憶部１５は、予め被組み付け部品２１０を保持していてもよい。その場合、ゴール画像生成部１４は、以下のようにしてゴール画像を生成してもよい。ゴール画像生成部１４は、部品２００及び被組み付け部品２１０の３次元モデルを記憶部１５から読み出す。そして、ゴール画像生成部１４は、位置姿勢検出部１３が検出した被組み付け部品２１０の位置及び姿勢に被組み付け部品２１０の３次元モデルをモデル空間において配置する。そして、ゴール画像生成部１４は、部品２００を組み付けた後の相対位置及び姿勢になるよう部品２００の３次元モデルをそのモデル空間に配置する。そして、ゴール画像生成部１４は、得られたモデル空間の画像をゴール画像としてもよい。

負荷算出部４０は、力センサー２０ｄから入力された把持部負荷情報から、部品２００にかかっている負荷（例えば、力及びモーメント）を算出し、算出した負荷を示す負荷情報を動作制御部１６へ出力する。

動作制御部１６は、撮像装置３０に現在の対象物を撮像させ、撮像により得られた現在画像を撮像装置３０から取得する。そして、動作制御部１６は、ゴール画像生成部１４から入力されたゴール画像と撮像装置３０から取得した現在画像との差分と、負荷算出部４０から入力された負荷情報（例えば、部品２００にかかる力）とに基づいて、ロボット２０の把持部２０ｃの位置を移動させる。これにより、部品２００がロボットの把持部２０ｃの位置に応じて移動する。動作制御部１６の処理の詳細は後述する。動作制御部１６は、ロボット２０の把持部２０ｃの位置を移動させた後に、移動制御した旨を示す移動制御済信号を画像取得部１７へ出力する。

すなわち、動作制御部１６は、視覚情報に基づくロボットの移動制御と力覚情報に基づく該ロボットの移動制御を並行して実行する。換言すれば、動作制御部１６は、ビジュアルサーボによるロボットの移動制御とインピーダンス制御による該ロボットの移動制御を並行して実行する。そして、動作制御部１６は、判定部１９によりロボットが把持する部品２０が目標の位置に到達したと判定されるまで、ロボット２０の制御を続ける。

画像取得部１７は、動作制御部１６から移動制御済信号を受信した場合、撮像装置３０に部品２００を撮像させる。そして、画像取得部１７は、撮像装置３０が撮像により得られた第２画像データを取得する。そして、画像取得部１７は、第２画像データを類似度算出部１８へ出力する。

類似度算出部１８は、記憶部１５からゴール画像を読み出す。類似度算出部１８は、画像取得部１７から入力された第２画像データと、ゴール画像との類似度を算出する。類似度算出部１８は、具体的には、例えば、正規化相互相関関数（ＮＣＣ：Ｎｏｒｍａｌｉｚｅｄ Ｃｒｏｓｓ−Ｃｏｒｒｅｌｅｔｉｏｎ）を類似度として算出してもよい。ここで、一例として類似度が高いほど、現在の部品２００の位置及び姿勢が、目標とする位置及び姿勢に近い。

判定部１９は、ロボット２０が把持する部品２０が目標の位置に到達したか否か判定する。具体的には、例えば、判定部１９は、算出した類似度が予め決められた閾値より大きいか否か判定する。判定部１９は、算出した類似度が予め決められた閾値より大きい場合、現在の部品２００の位置及び姿勢が目標となる位置及び姿勢になったと判定する。一方、判定部１９は、算出した類似度が予め決められた閾値以下の場合、現在の部品２００の位置及び姿勢が目標となる位置及び姿勢になっていないと判定する。そして、制御装置１０は、動作制御部１６の処理に戻る。

図４は、第１の実施形態におけるゴール画像の一例である。同図のゴール画像は、図１の符号２２０で示された平面における部品２００が被組み付け部品２１０に組み付けられた状態を示している。ゴール画像生成部１４は、一例として、図４に示すゴール画像を生成する。

図５は、第１の実施形態における動作制御部１６の概略ブロック図である。動作制御部１６は、ビジュアル制御部１１０と、コンプライアントモーション制御部１５０とを備える。
ビジュアル制御部１１０は、ロボットが把持する対象物の撮像画像に基づいてロボットのマニピュレーターを制御する第１制御信号を生成する。本実施形態では、第１制御信号は、一例として、ロボット２０を動かす先の相対的な位置を示す信号とする。その場合、ビジュアル制御部１１０は、現在の対象物を撮像した現在画像と、該対象物を目標の位置に移動させた状態のゴール画像との比較に基づいて、第１制御信号を生成する。
なお、本実施形態では、第１制御信号は、一例として、ロボット２０を動かす先の相対的な位置を示す信号としたが、これに限らず、ロボット２０を動かす速度などであってもよい。

そして、ビジュアル制御部１１０は、例えば、撮像装置３０の撮像レートに応じて予め決められた時間毎に第１制御信号を制御信号生成部１３０へ出力する。本実施形態では、一例として、撮像装置３０の撮像レートを３０フレーム／秒とし、ビジュアル制御部１１０は、一例として１フレーム撮像するのにかかる時間である３３ｍｓ毎に、第１制御信号をコンプライアントモーション制御部１５０の後述する制御信号生成部１３０へ出力する。

また、ビジュアル制御部１１０は、撮像画像に基づいて、部品（対象物）２００と被組み付け部品２１０との位置関係を検出する。具体的には、例えば、ビジュアル制御部１１０は、現在の対象物を撮像した現在画像と、該対象物を目標の位置に移動させた状態のゴール画像とのに基づいて、部品（対象物）２００と被組み付け部品２１０との位置関係を検出する。そして、ビジュアル制御部１１０は、検出した位置関係を示す位置関係信号をコンプライアントモーション制御部１５０の後述する負荷変位変換部１２０へ出力する。これにより、後述する負荷変位変換部１２０は、位置関係信号に基づいてインピーダンス制御の動作を規定する軸を決定する。

コンプライアントモーション制御部１５０は、ビジュアル制御部１１０が生成した第１制御信号と、ロボット２０に働く力を示す負荷情報に基づいてロボット２０のマニピュレーター部２０ｂを制御する。本実施形態におけるコンプライアントモーション制御部１５０は、一例として、インピーダンス制御技術を用いて、ロボット２０のマニピュレーター部２０ｂを制御する。ここで、コンプライアントモーション制御部１５０は、負荷変位変換部１２０と、制御信号生成部１３０と、ロボット制御部１４０とを備える。

負荷変位変換部１２０は、負荷算出部４０から入力された負荷情報（例えば、部品２００にかかっている力）を予め決められたサンプリングレート（本実施形態では、一例として１ｋＨｚ）で取得する。そして、負荷変位変換部１２０は、ビジュアル制御部１１０から入力された位置関係信号と負荷算出部４０から取得した負荷情報とに基づいて、該力に応じてロボット２０を動かす先の相対的な位置を示す第２制御信号を生成する。そして、負荷変位変換部１２０は、予め決められたレート（本実施形態では、一例として１ｋＨｚ）で、生成した第２制御信号ΔＸを制御信号生成部１３０へ出力する。
本実施形態では、第２制御信号ΔＸは、一例として、ロボット２０を動かす先の相対的な位置を示す信号としたが、これに限らず、ロボット２０を動かす速度などであってもよい。

制御信号生成部１３０は、ビジュアル制御部１１０が生成した第１制御信号と負荷変位変換部１２０が生成した第２制御信号とに基づいて、ロボット２０を動かす先の目標位置を示す位置信号Ｘ＿ｄを生成する。制御信号生成部１３０は、生成した位置信号Ｘ＿ｄをロボット制御部１４０へ出力する。

ここで、制御信号生成部１３０は、第１加算部１３１と、補間部（軌道生成部）１３２と、第２加算部１３３とを備える。
第１加算部１３１は、ロボット制御部１４０から入力された把持部２０ｃの現在位置を示す現在位置信号Ｘ＿ｃを取得する。第１加算部１３１は、ビジュアル制御部１１０から入力された第１制御信号と取得した現在位置信号Ｘ＿ｃとを加算して、ビジュアル制御部１１０によってロボット２０が移動された後の位置を示すビジュアル制御位置信号を生成する。第１加算部１３１は、生成したビジュアル制御位置信号を補間部１３２へ出力する。

補間部１３２は、第１加算部１３１から入力されたビジュアル制御位置信号を負荷変位変換部１２０が生成する第２制御信号の生成周期に応じて補間した補間信号Ｘを生成する。本実施形態では、一例として、力センサー２０ｄの取得レートを１ｋＨｚとし、第２制御信号の出力レートを力センサー２０ｄの取得レートと同一の１ｋＨｚであるとした。そのため、補間部１３２は、ビジュアル制御位置信号を１ｋＨｚで補間した補間信号Ｘを生成する。補間部１３２は、生成した補間信号Ｘを第２加算部１３３へ出力する。

第２加算部１３３は、補間部１３２から入力された補間信号Ｘと負荷変位変換部１２０から入力された第２制御信号とを加算し、加算により得られた位置指示信号Ｘ＿ｄをロボット制御部１４０へ出力する。

ロボット制御部１４０は、制御信号生成部１３０から位置指示信号Ｘ＿ｄが入力される毎に、その位置指示信号Ｘ＿ｄが示す位置へロボット２０の把持部２０ｃの位置を移動させる。ロボット制御部１４０は、上記の処理を予め決められた回数行って、ロボット２０の把持部２０ｃの位置を移動させる。この回数は、第２制御信号の生成レート（本実施形態では、１ｋＨｚ）を第１制御信号の生成レート（本実施形態では、３０Ｈｚ）で割った値（本実施形態では、３３回）である。その後に、ロボット制御部１４０は、移動制御した旨を示す移動制御済信号を画像取得部１７へ出力する。
なお、第１及び第２制御信号が速度の場合、ロボット制御部１４０への出力Ｘ＿ｄも速度になるので、その場合ロボット制御部１４０はＸ＿ｄが示す速度にロボット２０を制御する。

また、ロボット制御部１４０は、予め決められたレート（本実施形態では、一例として１ｋＨｚ）で、ロボット２０の把持部２０ｃの現在位置を検出し、検出した把持部２０ｃの現在位置を示す現在位置信号Ｘ＿ｃを第１加算部１３１と補間部１３２へ出力する。

図６は、第１の実施形態における補間部１３２の処理を説明するための図である。同図において、縦軸はロボット座標系におけるＸ座標、横軸は時間［ｍｓ］を示している。同図において、０、１、２、３、…、３０、３１、３２、３３ｍｓ後のＸ座標が、それぞれＸ_０、Ｘ_１、Ｘ_２、Ｘ_３、…、Ｘ_３０、Ｘ_３１、Ｘ_３２、Ｘ_３３であることが示されている。

ビジュアル制御部１１０が出力した第１制御信号が、把持部位置を３３ｍｓでＸ軸に沿ってｘ０からｘ３３まで移動させる際の方向と距離を示す信号だと仮定する。その仮定において、例えば、ビジュアル制御部１１０は、１ミリ秒後の第１制御信号のＸ成分としてＸ１を生成する。同様にして、例えば、ビジュアル制御部１１０は、ｔ1ミリ秒後（但し、ｔ1は１から３３までの整数）の第１制御信号のＸ成分としてＸ_ｔ１を生成する。
なお、補間部１３２は補間信号を生成する際に、現在の位置、速度及び加速度と、移動後の位置、速度及び加速度とを境界条件として、時間に関する５次多項式を用いても良い。

図７は、第１の実施形態におけるビジュアル制御部１１０の概略ブロック図である。ビジュアル制御部１１０は、第１位置検出部１１１と、第２位置検出部１１２と、第１減算部１１３と、移動量算出部１１４と、カメラロボット座標変換部１１５とを備える。
第１位置検出部１１１は、一例として記憶部１５から、予め関連付けて保持された部品２００の取りうる全ての位置及び姿勢と、その状態において撮像されたカメラ画像とを読み出す。そして、第１位置検出部１１１は、ゴール画像生成部１４から入力されたゴール画像Ｉｇと読み出したカメラ画像とを比較して、最も類似度の高い画像に関連付けられた位置及び姿勢を、ゴール画像中の部品２００のカメラ座標ΣＣにおける位置及び姿勢として算出する。第１位置検出部１１１は、算出した位置を示すカメラ座標位置信号Ｕｇを第１減算部１１３へ出力する。

第２位置検出部１１２は、一例として記憶部１５から、予め関連付けて保持された部品２００の取りうる全ての位置及び姿勢と、その状態において撮像されたカメラ画像とを読み出す。そして、第２位置検出部１１２は、撮像装置３０から取得した現在のカメラ画像Ｉｃと読み出したカメラ画像とを比較して、最も類似度の高い画像に関連付けられた位置及び姿勢を、現在の部品２００のカメラ座標ΣＣにおける位置及び姿勢として算出する。第２位置検出部１１２は、算出した位置を示す現在カメラ座標位置信号Ｕｃを第１減算部１１３へ出力する。

第１減算部１１３は、第１位置検出部１１１から入力されたカメラ座標位置信号Ｕｇから現在カメラ座標位置信号Ｕｃを減算し、減算により得られた減算後信号を移動量算出部１１４へ出力する。ここで、減算後信号は、カメラ座標ΣＣにおけるゴール画像中の部品２００の位置と現在の部品の位置の差を示す信号である。

移動量算出部１１４は、一例として、ＰＩＤ制御によって減算後信号からカメラ座標系ΣＣにおける部品２００の移動量を算出する。移動量算出部１１４は、算出した移動量を示す部品移動量信号をカメラロボット座標変換部１１５へ出力する。

カメラロボット座標変換部１１５は、移動量算出部１１４から入力された部品移動量信号が示す部品２００の移動量をカメラ座標系ΣＣからロボット座標系に変換することで、ロボット座標系ΣＲにおける部品２００の移動量を生成する。具体的には、例えば、カメラロボット座標変換部１１５は、カメラ座標系ΣＣからロボット座標系に変換する回転行列を部品２００の移動量に乗じることにより、ロボット座標系ΣＲにおける部品２００の移動量を生成する。カメラロボット座標変換部１１５は、生成したロボット座標系ΣＲにおける部品２００の移動量を示す第１制御信号を制御信号生成部１３０へ出力する。

図８は、第１の実施形態における負荷変位変換部１２０の概略ブロック図である。負荷変位変換部１２０は、第１座標変換部１２１と、伝達関数乗算部１２４と、第２座標変換部１２５とを備える。
第１座標変換部１２１は、力センサー２０ｄが検出した力ベクトルＦ＿ｓをセンサー座標系から物体座標系に変換する。ここで、力ベクトルＦ＿ｓは力とモーメントを含むものとする。また、センサー座標系は、力センサー２０ｄが使用する座標系であり、物体座標系は一例として、部品２００を基準とする座標系である。なお、物体座標系は、被組み付け部品２１０を基準とする座標系であっても良いし、部品２００と被組み付け部品２１０の相対的な位置関係を基準とする座標系であっても良い。ここで、第１座標変換部１２１は、第１変換行列算出部１２２と、第１乗算部１２３とを備える。

第１変換行列算出部１２２は、第２変換行列算出部１２６から入力された第２変換行列Ｔに基づいて、センサー座標系ΣＳから物体座標系ΣＯに変換する第１変換行列Ｕを算出する。ここで、予めセンサー座標系ΣＳからロボット座標系ΣＲへの第１の変換式は既知であるので、一例として第１変換行列算出部１２２がこの第１の変換式を予め保持するものとする。

その場合において、第１変換行列算出部１２２は、一例として、以下の処理により第１変換行列Ｕを算出する。第１変換行列算出部１２２は、例えば、後述する第２変換行列算出部１２６から、物体座標系ΣＯからロボット座標系ΣＲへ変換する第２変換行列Ｔを取得する。そして、第１変換行列算出部１２２は、第２変換行列Ｔの逆行列を算出することで、ロボット座標系ΣＲから物体座標系ΣＯへの第２の変換式を算出する。第１変換行列算出部１２２は、第１の変換式と第２の変換式を乗じることにより、第１変換行列Ｕを算出する。そして、第１変換行列算出部１２２は、算出した第１変換行列Ｕを第１乗算部１２３へ出力する。

第１乗算部１２３は、負荷算出部４０から入力された負荷情報に含まれる力ベクトルＦ＿ｓに、第１変換行列算出部１２２から入力された第１変換行列Ｕを乗算し、乗算後の力ベクトルＦ＿ｏを示す力ベクトル信号を伝達関数乗算部１２４へ出力する。
伝達関数乗算部１２４は、第１乗算部１２３から入力された力ベクトル信号が示す乗算後の力ベクトルＦ＿ｏに対して、予め決められた伝達関数を乗じる。力ベクトルＦ＿ｏに含まれる力及びモーメントの６つの要素、及びΔｘ（→）に含まれる位置及び姿勢の６つの要素をそれぞれＦ＿ｏ[i]、Δｘ[i]とすると、予め決められた伝達関数は、次式で表される。

この式は、インピーダンス制御で用いられる伝達関数である。ここで、Δｘの上に矢印→が付された符号をΔｘ（→）と表すこととし、Δｘ（→）は、伝達関数を乗じた後のベクトルを表すものとする。また、ｍ［ｉ］、ｄ［ｉ］及びｋ［ｉ］は、Ｆ＿ｏの各要素Ｆ＿ｏ［ｉ］に対してΔｘ（→）の各要素Δｘ［ｉ］の運動が、慣性質量ｍ［ｉ］、ダンパ係数ｄ［ｉ］及びバネ定数ｋ［ｉ］で特徴付けられる機械インピーダンスを有するように設定する変数であり、要素毎に適宜設定される。伝達関数乗算部１２４は、伝達関数乗算後のベクトルΔｘ（→）を示す乗算後信号を第２座標変換部１２５へ出力する。

第２座標変換部１２５は、伝達関数乗算部１２４から入力された乗算後信号が示すベクトルΔｘ（→）を物体座標系ΣＯからロボット座標系ΣＲに変換することで、第２制御信号ΔＸを生成する。ここで、第２座標変換部１２５は、第２変換行列算出部１２６と、第２乗算部１２７とを備える。
第２変換行列算出部１２６は、ビジュアル制御部１１０から入力された位置関係信号に基づいて、物体座標系ΣＯからロボット座標系ΣＲに変換する第２変換行列Ｔを算出する。第２座標変換部１２５は、例えば、以下の処理により第２変換行列Ｔを算出する。

第２座標変換部１２５は、例えば、ビジュアル制御部１１０に含まれる第１位置姿勢検出部の出力から、物体座標系ΣＯからカメラ座標系ΣＣへの第３の変換式を算出する。そして、第２座標変換部１２５は、例えば、カメラ座標系ΣＣからロボット座標系ΣＲへの第４の変換式を算出する。第２座標変換部１２５は、第３の変換式と第４の変換式を乗じることにより、第２変換行列Ｔを算出する。

第２変換行列算出部１２６は、算出した第２変換行列Ｔを第２乗算部１２７と第１変換行列算出部１２２へ出力する。
第２乗算部１２７は、伝達関数乗算部１２４から入力された乗算後信号に第２変換行列Ｔを乗じることにより、第２制御信号ΔＸを生成する。そして、第２乗算部１２７は、生成した第２制御信号ΔＸを制御信号生成部１３０へ出力する。

図９は、センサー座標系と物体座標系を説明するための図である。同図において、把持部２０ｃが部品２００を把持している状態が示されている。また、同図において、力センサー２０ｄと被組み付け部品２１０が示されている。また、力センサー２０ｄの中心を原点とするセンサー座標系（ｘ´ｙ´ｚ´座標系）が示されている。また、部品２００の一つの端面の中心を原点とする物体座標系（ｘｙｚ座標系）が示されている。第１変換行列Ｕは、このセンサー座標系を物体座標系に変換する変換行列である。

図１０は、第１の実施形態におけるロボット制御部１４０の概略ブロック図である。ロボット制御部１４０は、角度位置変換部１４１と、位置角度変換部１４２と、第２減算部１４３と、増幅部１４４とを備える。
角度位置変換部１４１は、ロボット２０が保持するエンコーダー２１からロボット２０の各関節の角度θｃを取得する。そして角度位置変換部１４１は、取得した各関節の角度θｃから、把持部２０ｃの現在位置を算出し、算出した現在位置を示す現在位置信号Ｘ＿ｃを第１加算部１３１へ出力する。

位置角度変換部１４２は、例えば、制御信号生成部１３０から入力された位置指示信号Ｘ＿ｄが示すロボット２０を動かす先の目標位置から、公知の逆運動学の処理によりその目標位置が実現されるロボット２０の各関節の目標角度を算出する。そして、位置角度変換部１４２は、算出した目標角度を示す目標角度信号を第２減算部１４３へ出力する。

第２減算部１４３は、位置角度変換部１４２から入力された目標角度信号からエンコーダー２１から入力された現在の関節角度を示す現在関節角度信号θｃを減算する。第２減算部１４３は、減算することにより得られた差分信号を増幅部１４４へ出力する。

増幅部１４４は、第２減算部１４３から入力された差分信号を予め決められたゲインＫ_θを乗じて増幅することにより得られた電流信号をロボット２０へ出力する。ここで、電流信号は、例えば、ロボット２０の各関節のモータに与える電流を示す６軸のベクトルである。これにより、ロボット２０は、増幅部１４４から入力された電流信号に基づいて、各関節のモータを駆動することにより、位置指示信号Ｘ＿ｄが示す目標位置へ把持部２０ｃの位置を移動する。

図１１は、第１の実施形態における位置指示信号Ｘ＿ｄが示す位置を説明するための図である。同図の例において、ロボット座標系（ＸＹＺ座標系）において、原点９１を基準として、部品２００の現在位置Ｐと、部品２００の移動先の位置Ｑと、部品２００のスタート座標Ｓ（Ｘ＿０，Ｚ＿０）と部品２００のゴール座標Ｓとが示されている。また、ビジュアル制御部１１０が生成する第１制御信号Ｘが示す第１ベクトル９２、負荷変位変換部１２０が生成する第２制御信号ΔＸが示す第２ベクトル９３及び制御信号生成部１３０が生成する位置指示信号Ｘ＿ｄが示す第３ベクトル９４のそれぞれの一例が示されている。

また、この第１ベクトル９２は、スタート座標Ｓ（Ｘ＿０，Ｚ＿０）とゴール座標Ｇ（Ｘ＿Ｇ，Ｚ＿Ｇ）を結ぶ直線に沿っていることが示されている。すなわち、ビジュアル制御部１１０は、部品２００のスタート座標Ｓから部品２００のゴール座標Ｇへ向かって部品２００を動かすようロボット２０を制御する。ここでは、ビジュアル制御部の出力が、スタート座標Ｓからゴール座標Ｓまで直線の例が示されていますが、必ずしも直線になるものではない。

また、同図において、部品２００に力Ｆが加わっていることが示されている。第２ベクトルの方向は、この力Ｆが加わっている方向である。すなわち、負荷変位変換部１２０は、力Ｆが加わっている方向へ部品２００を動かすようロボット２０を制御する。
また、第３ベクトル９４は、第１ベクトル９２と第２ベクトル９３とを加算したベクトルである。同図の例において、制御信号生成部１３０は、現在位置Ｐから第３ベクトル９４が示す位置Ｑへ部品２００を動かすようロボット２０を制御する。

図１２は、第１の実施形態における制御装置２０の処理の流れの一例を示すフローチャートである。まず、把持指示部１１は、部品２００を把持部２０ｃで把持させる（ステップＳ１０１）。次に、開始位置移動指示部１２は、マニピュレーター部２０ｂを制御して、部品２００を所定の組み付け作業開始位置に移動する（ステップＳ１０２）。位置姿勢検出部１３は、撮像装置３０で取得した画像を用いて、部品２００及び被組み付け部品２１０の位置及び姿勢を検出する（ステップＳ１０３）。

次に、検出した被組み付け部品２１０の位置及び姿勢から、部品２００の組み付け後のゴール画像を生成する（ステップＳ１０４）。ビジュアル制御部１１０は、ゴール画像と現在画像との差分に基づいて、ビジュアルサーボによる第１の動作指示信号を生成する（ステップＳ１０５）。次に、負荷変位変換部１２０は、部品２００と被組み付け部品２１０の位置及び姿勢からインピーダンス制御による第２の動作指示信号を生成する（ステップＳ１０６）。

次に、動作制御部１６は、第１の動作指示信号と第２の動作指示信号とに基づいて、動作指示信号を生成し、ロボットの把持部２０ｃの位置を移動させる（ステップＳ１０７）。次に、画像取得部１７は、撮像装置３０で撮像した画像を取得する（ステップＳ１０８）。次に、類似度算出部１８は、画像取得部１７で取得した画像と、ゴール画像との類似度を算出する（ステップＳ１０９）。

次に、判定部１９は、類似度算出部１８が算出した類似度が予め決められた閾値よりも大きいか否か判定する（ステップＳ１１０）。その類似度が予め決められた閾値以下の場合（ステップＳ１１０ ＮＯ）、制御装置１０は、ステップＳ１０５の処理に戻る。一方、その類似度が予め決められた閾値より大きい場合（ステップＳ１１０ ＹＥＳ）、制御装置１０は、本フローチャートの処理を終了する。

以上、第１の実施形態におけるロボット装置１における制御装置１０の動作制御部１６は、視覚情報に基づくロボット２０の移動制御と力覚情報に基づくロボット２０の移動制御を並行して実行することにより、部品２００を把持するロボット２０の移動を制御する。このように構成することで、制御装置１０は、カメラ座標系ΣＣとロボット座標系ΣＯの関係を求めるキャリブレーション作業を必要とせずに高精度な部品２００の位置決めを行いつつ、組み立ての過程で部品２００と被組み付け部品２１０が接触しても過大な力が生じないようにすることができる。その結果、制御装置１０は、部品２００を破壊せずに被組み付け部品２１０に組み付けることができる。

より詳細には、制御装置１０において、ビジュアル制御部１１０が、現在の部品（対象物）２００を撮像した現在画像と、部品（対象物）２００を目標の位置に移動させた場合のゴール画像との差分に基づいて、ロボット２０を動かした後の位置を示す第１制御信号を生成する。また、負荷変位変換部１２０は、力センサー２０ｄが検出した力に基づいて、該力に応じてロボット２０を動かした後の位置を示す第２制御信号を生成する。そして、制御信号生成部１３０は、ビジュアル制御部１１０が生成した第１制御信号と負荷変位変換部１２０が生成した第２制御信号とに基づいて、ロボット２０を動かした後の位置を指示する位置指示信号を生成する。そして、動作制御部１６は、制御信号生成部が生成した制御信号に基づいて、ロボット２０の移動を制御する。

これにより、制御装置１０は、部品（対象物）２００が障害物に接触しても、部品（対象物）２００が障害物に沿うように制御信号を生成するので、対象物が障害物に沿うようにロボット２０を制御することができる。これにより、ロボット２０は、部品（対象物）２００を破壊せずに部品（対象物）２００を最終的な目的位置まで移動させることにより、部品（対象物）２００を組み立てることができる。

また、本実施形態において、ビジュアル制御部１１０は、撮像画像に基づいて、部品（対象物）２００と部品（対象物）２００を組み付ける被組み付け部品２１０との位置関係を検出する。そして、コンプライアントモーション制御部１５０の負荷変位変換部１２０は、ビジュアル制御部１１０が検出した位置関係と負荷情報とに基づいてロボット２０のマニピュレーターを制御する第２制御信号を生成する。そして、コンプライアントモーション制御部１５０は、ビジュアル制御部１１０が生成した第１制御信号と負荷変位変換部１２０が生成した第２制御信号とに基づいて、ロボット２０のマニピュレーター２０ｂを制御する。

これにより、制御装置２０は、ビジュアル制御部１１０が位置決めを行うために部品２１０と被組み付け部品２１０との位置関係を検出するので、コンプライアントモーション制御部１５０は、その位置関係に基づいてインピーダンス制御の動作を規定する軸を決定することで、適切なコンプライアントモーションの制御ができる。

また、接触を前提とするロボット２０の作業において、ビジュアル制御部１１０は、現在の対象物を撮像した現在画像と、該対象物を目標の位置に移動させた状態のゴール画像との比較に基づいて、第１制御信号を生成する。これにより、動作制御部１６は、対象物を最終的な位置まで移動するようロボット２０を制御することができる。それゆえ、制御装置１０は、ゴール画像を教示されることで対象物２００を被組み付け部品２１０に組み付けるように制御できるので、接触を前提とするロボット２０の作業に対して教示を簡易にすることができる。

また、制御装置１０は、部品（対象物）２００の姿勢が変わっても、その部品（対象物）２００の姿勢に応じて、負荷変位変換部１２０が制御する座標軸を変更するので、部品（対象物）２００に過度な力が働くことを防止でき、部品（対象物）２００を破壊することを防止することができる。

また、制御装置１０において、位置姿勢検出部１３による被組み付け部品２１０の位置及び姿勢の検出と、ゴール画像生成部１４によるゴール画像の生成を随時行うようにすることで、被組み付け部品２１０が移動しても、その移動した位置に部品（対象物）２００を移動するようにロボット２０を制御する。これにより、制御装置１０は、被組み付け部品２１０が移動しても部品（対象物）２００を被組み付け部品２１０に組み付けることができる。

例えば、ベルトコンベアーを完全に等速で動かすことは難しいが、被組み付け部品２１０がベルトコンベアー上に配置されて非等速で移動しても、制御装置１０は、被組み付け部品２１０の移動に追従して、部品（対象物）２００を移動するようにロボット２０を制御するので、部品（対象物）２００を被組み付け部品２１０に組み付けることができる。

［第２の実施形態］
続いて、本発明の第２の実施形態について説明する。図１３は、第２の実施形態におけるロボット装置１ｂの概略斜視図である。なお、図１と共通する要素には同一の符号を付し、その具体的な説明を省略する。図１３のロボット装置１ｂの構成は、図１のロボット装置１の構成に対して、制御装置１０が、制御装置１０ｂに変更されたものとなっている。第２の実施形態における制御装置１０ｂは、収束判定処理が第１の実施形態の制御装置１０とは異なる。

図１４は、第２の実施形態における制御装置１０ｂの概略ブロック図である。なお、図３と共通する要素には同一の符号を付し、その具体的な説明を省略する。図１４の制御装置１０ｂの構成は、図３の制御装置１０の構成に対して、選択部４１が追加され、負荷算出部４０が負荷算出部４０ｂに、判定部１９が判定部１９ｂに変更されたものになっている。

負荷算出部４０ｂは、第１の実施形態における負荷算出部４０と同様の機能を有するが以下の点で異なる。負荷算出部４０ｂは、部品２００にかかる負荷（例えば、力及びモーメント）を示す負荷情報を判定部１９ｂにも出力する。
選択部４１は、部品２００に設定された座標軸毎に、類似度と負荷情報のうちいずれの指標を用いて収束判定するかを示す選択信号を生成し、生成した選択信号を判定部１９ｂへ出力する。

判定部１９ｂは、第１の実施形態における判定部１９と同様の機能を有するが、以下の点で異なる。判定部１９ｂは、部品２００に設定された座標軸毎に、類似度算出部１８から入力された類似度と負荷算出部４０からの入力された負荷情報のうち選択部４１から入力された選択信号が示す指標に基づいて、部品２００が被組み付け部品２１０に組み付けられたか否か判定する。

具体的には、例えば、ある座標軸の選択信号が類似度を示す場合、判定部１９ｂは、類似度が予め決められた閾値より大きい場合、部品２００が被組み付け部品２１０に組み付けられたと判定する。一方、類似度が予め決められた閾値以下の場合、判定部１９ｂは、部品２００が被組み付け部品２１０に組み付けられていないと判定する。

また、例えば、ある座標軸の選択信号が負荷情報を示す場合、判定部１９ｂは、選択された軸方向の力又はモーメントと、それに対して予め設定された値との差異が所定の値より小さい場合、部品２００が被組み付け部品２１０に組み付けられたと判定する。一方、上記差異が所定の値以上の場合、判定部１９ｂは、部品２００が被組み付け部品２１０に組み付けられていないと判定する。

図１５は、第２の実施形態における制御装置１０ｂの処理の流れの一例を示すフローチャートである。ステップＳ２０１〜Ｓ２０９の処理は、図１２のステップＳ１０１〜Ｓ１０９の処理と同様であるので、その説明を省略する。
次に、ステップＳ２１０において、判定部１９ｂは、選択信号が類似度か否か判定する。選択信号が類似度である場合（ステップＳ２１０ ＹＥＳ）、判定部１９ｂはステップＳ２１１の処理に遷移する。一方、選択信号が類似度でなく負荷情報である場合（ステップＳ２１０ ＮＯ）、判定部１９ｂはステップＳ２１２の処理に遷移する。

次に、ステップＳ２１１において、判定部１９ｂは、類似度が予め決められた閾値より大きいか否か判定する。類似度が予め決められた閾値以下の場合（ステップＳ２１１ ＮＯ）、制御装置１０ｂはステップＳ２０５の処理に戻る。一方、類似度が予め決められた閾値より大きい場合（ステップＳ２１１ ＹＥＳ）、制御装置１０ｂはステップＳ２１３へ遷移する。

次に、ステップＳ２１２において、判定部１９ｂは、選択された軸方向の力又はモーメントと、それに対して予め設定された値との差異が所定の値より小さいか否か判定する。上記差異が所定の値以上の場合（ステップＳ２１２ ＮＯ）、制御装置１０ｂはステップＳ２０５の処理に戻る。一方、上記差異が所定の値より小さい場合（ステップＳ２１２ ＹＥＳ）、制御装置１０ｂはステップＳ２１３へ遷移する。

次に、ステップＳ２１３において、判定部１９ｂは、部品２００に設定された座標軸の全てで判定したか否か判定する。部品２００に設定された座標軸の全てで判定していない場合（ステップＳ２１３ ＮＯ）、判定部１９ｂは、判定する際の座標軸を変更し（ステップＳ２１４）、ステップＳ２１０の処理に戻る。一方、部品２００に設定された座標軸の全てで判定した場合（ステップＳ２１３ ＹＥＳ）、制御装置１０ｂはその処理を終了する。以上で、本フローチャートの処理を終了する。

以上、第２の実施形態のロボット装置１ｂにおける制御装置１０ｂは、第１の実施形態の効果に加えて、判定部１９ｂが座標軸毎に、部品２００が被組み付け部品２１０に組み付けられたか否かの判定に用いる指標を変更することができる。これにより、制御装置１０ｂは、部品２００が被組み付け部品２１０に組み付けられた際の、部品２００と被組み付け部品２１０の相対位置関係を常に一定の高精度になるようにすることができる。
また、ビジュアルサーボによる位置決めの精度を出しにくい、例えば撮像装置３０にとっての奥行き方向については、力又はモーメントの情報を収束判定に適用して被組み付け部品２１０と部品２００との接触を利用して位置決めを行わせることで、高精度な位置決めを実現することが出来る。

［第３の実施形態］
続いて、本発明の第３の実施形態について説明する。第１及び第２の実施形態では、一つのマニピュレーター（マニピュレーター部）を有するロボットについて説明したが、第３の実施形態では、二つのマニピュレーターを備える例を説明する。
図１６は、第２の実施形態におけるロボット装置１ｃの概略斜視図である。図１２に示すように、ロボット装置１ａは、ロボット２０’と、制御装置１０ｃと、本体３０２とを備える。なお、第１の実施形態と同様のものは、同じ符号を用いて説明を省略する。

ロボット２０’は、２つのマニピュレーター部（第１マニピュレーター）２０Ａおよびマニピュレーター部（第２マニピュレーター）２０Ｂ、第１把持部２０Ａｃ、第２把持部２０Ｂｃ、第１力センサー２０Ａｄ、第２力センサー２０Ｂｄ、可動部３０１を備えている。２つのマニピュレーター部２０Ａおよび２０Ｂは、可動部３０１に取り付けられている。可動部３０１は、本体３０２に取り付けられている。第１把持部２０Ａｃの先端には、撮像装置３０が取り付けられている。

第１力センサー２０Ａｄは、第１の実施形態と同様に、第１把持部２０Ａｃに加わる力とモーメントを検出し、検出した力とモーメントを制御装置１０ｃへ送信する。また、第２力センサー２０Ｂｄは、第１力センサー２０Ａｄと同様に、第２把持部２０Ｂｃに加わる力とモーメントを検出し、検出した力とモーメントを制御装置１０ｃへ送信する。

撮像装置３０は、部品２００と被組み付け部品２１０とを撮像できる位置に設置されている。撮像装置３０は、撮像したカメラ画像を示す画像データを制御装置１０ｃへ送信する。

制御装置１０ｃは、マニピュレーター部２０Ａ、マニピュレーター部２０Ｂ、第１把持部２０Ａｃ、第２把持部２０Ｂｃを制御する。制御装置１０ｃの構成は、第１の実施形態の制御装置１０と同様である。なお、本実施形態では、制御装置１０ｃは本体の外部に配置したが、本体３０２の内部に制御装置１０ｃを配置してもよい。

本体３０２は、移動可能なように搬送部３０３Ａおよび３０３Ｂを備えている。搬送部３０３Ａおよび３０３Ｂは、例えば車輪、キャタピラ等である。

次に、制御装置１０ｃは、部品２００と被組み付け部品２１０を第１把持部３０３ａ、第２把持部３０３ｂのそれぞれに把持させて、所定位置に移動するように変更する。それ以降の処理は、被組み付け部品２１０の位置をそのままにして、部品２００を動かして部品２００を被組み付け部品２１０に組み付けた状態にする。この処理は、第１の実施形態における制御装置１０の処理と同様であるので、その説明を省略する。

以上、第３の実施形態のロボット装置１ｃにおける制御装置１０ｃは、第１の実施形態の効果に加えて、第２把持部３０３ｂに被組み付け部品２１０を把持させたまま、部品２００を被組み付け部品２１０に組み付けることができる。これにより、制御装置１０ｃは、被組み付け部品２１０の姿勢がどのような姿勢であっても、部品２００を被組み付け部品２１０に組み付けることができる。

なお、制御装置１０ｃの構成は、第１の実施形態の制御装置１０と同様としたが、これに限らず、制御装置１０ｃの構成は、第２の実施形態の制御装置１０と同様であってもよい。
また、各実施形態では、類似度算出部１８は、類似度を算出し、判定部（１９又は１９ｂ）は類似度に基づいて収束判定したが、これに限ったものではない。例えば、類似度算出部１８は、相違度を算出して、判定部（１９又は１９ｂ）は相違度に基づいて収束判定してもよい。その際、例えば、類似度算出部１８は、ＳＳＤ（Ｓｕｍ ｏｆ Ｓｑｕａｒｅｄ Ｄｉｆｆｅｒｅｎｃｅ）を相違度として算出してもいい。ここで、一例として相違度が低いほど、現在の部品２００の位置及び姿勢が、目標とする位置及び姿勢に近い。

また、各実施形態において、一例として、部品（対象物）２００と被組み付け部品２１０とが組みつけられた状態の画像を、対象物を目標の位置に移動させた状態のゴール画像として用いたが、これに限ったものではない。
また、ゴール画像は、複数のゴール画像があってもよい。例えば、第１のゴール画像と第２のゴール画像とがあった場合に、各実施形態の制御装置は、現在画像が第１のゴール画像と一致した後に、第２のゴール画像と一致させるようにしてもよい。

また、複数の装置を備えるシステムが、各実施形態の制御装置（１０、１０ｂ、１０ｃ）の各処理を、それらの複数の装置で分散して処理してもよい。
また、各実施形態の制御装置（１０、１０ｂ、１０ｃ）の各処理を実行するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、当該記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより、制御装置（１０、１０ｂ、１０ｃ）に係る上述した種々の処理を行ってもよい。

なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものであってもよい。また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、フラッシュメモリ等の書き込み可能な不揮発性メモリ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。

さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリ（例えばＤＲＡＭ（Ｄｙｎａｍｉｃ Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ））のように、一定時間プログラムを保持しているものも含むものとする。また、上記プログラムは、このプログラムを記憶装置等に格納したコンピュータシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピュータシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。また、上記プログラムは、前述した機能の一部を実現するためのものであっても良い。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であっても良い。

以上、本発明の実施形態について図面を参照して詳述したが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計等も含まれる。

１、１ｂ、１ｃ ロボット装置
１０、１０ｂ、１０ｃ 制御装置
１１ 把持指示部
１２ 開始位置移動指示部
１３ 位置姿勢検出部
１４ ゴール画像生成部
１５ 記憶部
１６ 動作制御部
１７ 画像取得部
１８ 類似度算出部
１９ 判定部
２０、２０’ ロボット
３０ 撮像装置
２０ａ 支持台
２０ｂ、２０Ａ、２０Ｂ マニピュレーター部（マニピュレーター）
２０ｃ 把持部
２０Ａｃ 第１把持部
２０Ｂｃ 第２把持部
２０ｄ、２０Ａｄ、２０Ｂｄ 力センサー
３０ 撮像装置
４０ 負荷算出部
４１ 選択部
１１０ ビジュアル制御部
１１１ 第１位置検出部
１１２ 第２位置検出部
１１３ 第１減算部
１１４ 移動量算出部
１１５ カメラロボット座標変換部
１２０ 負荷変位変換部
１２１ 第１座標変換部
１２２ 第１変換行列算出部
１２３ 第１乗算部
１２４ 伝達関数乗算部
１２５ 第２座標変換部
１２６ 第２変換行列算出部
１２７ 第２乗算部
１３０ 制御信号生成部
１３１ 第１加算部
１３２ 補間部（軌道生成部）
１３３ 第２加算部
１４０ ロボット制御部
１４１ 角度位置変換部
１４２ 位置角度変換部
１４３ 第２減算部
１４４ 増幅部
１５０ コンプライアントモーション制御部
２００ 部品（対象物）
２１０ 被組み付け部品
３０２ 本体
３０３Ａ、３０３Ｂ 搬送部

Claims (11)

1. ロボットが把持する対象物の撮像画像に基づいて前記対象物と前記対象物を組み付ける被組み付け部品との相対的な位置関係を検出し、検出した前記位置関係に基づいて前記ロボットのマニピュレーターを制御する第１制御信号を生成するビジュアル制御部と、
   前記ビジュアル制御部が検出した前記位置関係と前記対象物にかかっている力を示す負荷情報とに基づいて、前記マニピュレーターを制御する第２制御信号を生成し、生成した前記第２制御信号と前記ビジュアル制御部が生成した前記第１制御信号との両方に基づいて、前記ロボットのマニピュレーターを制御するコンプライアントモーション制御部と、
   を備えることを特徴とする制御装置。
2. 前記コンプライアントモーション制御部は、
   前記ビジュアル制御部が検出した前記位置関係と前記負荷情報とに基づいて前記第２制御信号を生成する負荷変位変換部と、
   前記第１制御信号に基づいて制御信号を生成する軌道生成部を備え、前記第１制御信号と前記軌道生成部が生成した制御信号とに基づいて、前記ロボットのマニピュレーターを制御することを特徴とする請求項１に記載の制御装置。
3. 前記軌道生成部は、前記第１制御信号を、前記負荷変位変換部が生成する第２制御信号の生成周期と同じ生成周期となるように補間した前記制御信号を生成することを特徴とする請求項２に記載の制御装置。
4. 力センサーが検出した前記ロボットに働く力に基づいて、前記対象物にかかっている力を示す負荷情報を算出する負荷算出部を備え、
   前記負荷変位変換部は、前記負荷算出部が算出した負荷情報に基づいて、前記第２制御信号を生成することを特徴とする請求項２または３に記載の制御装置。
5. 前記負荷変位変換部は、
   前記負荷情報を、前記力センサーを基準とするセンサー座標系から前記対象物を基準とする物体座標系に変換する第１座標変換部と、
   前記第１座標変換部によって変換された後の負荷情報に対して、前記対象物の質量とダンパ係数とばね定数とに基づいて決定される伝達関数を乗じる伝達関数乗算部と、
   前記伝達関数乗算部が乗じることにより得られた負荷情報を前記物体座標系から前記ロボットを基準とするロボット座標系に変換することで、前記第２制御信号を生成する第２座標変換部と、
   を備えることを特徴とする請求項４に記載の制御装置。
6. 前記ビジュアル制御部は、現在の対象物を撮像した現在画像と、該対象物を目標の位置に移動させた状態のゴール画像との比較に基づいて、前記第１制御信号を生成することを特徴とする請求項１から請求項５のいずれか一項に記載の制御装置。
7. 前記対象物が目標の位置に到達したか否か判定する判定部を備え、
   前記コンプライアントモーション制御部は、前記判定部により前記対象物が目標の位置に到達したと判定されるまで、前記ロボットのマニピュレーターの制御を続けることを特徴とする請求項１から請求項６のいずれか一項に記載の制御装置。
8. 前記対象物が目標の位置に到達したか否か、及び前記対象物にかかっている力が目標の力に到達したか否か判定する判定部を備え、
   前記コンプライアントモーション制御部は、前記判定部により前記対象物が目標の位置に到達したと判定され、かつ前記対象物にかかっている力が目標の力に到達したと判定されるまで、前記ロボットのマニピュレーターの制御を続けることを特徴とする請求項１から請求項６のいずれか一項に記載の制御装置。
9. ロボットが把持する対象物の撮像画像に基づいて前記対象物と前記対象物を組み付ける被組み付け部品との相対的な位置関係を検出し、検出した前記位置関係と前記対象物にかかっている力を示す負荷情報とに基づいて、ビジュアルサーボによる前記ロボットの移動制御とインピーダンス制御による前記ロボットの移動制御を並行して実行することを特徴とする制御装置。
10. ビジュアル制御部が、ロボットが把持する対象物の撮像画像に基づいて前記対象物と前記対象物を組み付ける被組み付け部品との相対的な位置関係を検出し、検出した前記位置関係に基づいて前記ロボットのマニピュレーターを制御する第１制御信号を生成する手順と、
    コンプライアントモーション制御部が、前記ビジュアル制御部が検出した前記位置関係と前記対象物にかかっている力を示す負荷情報とに基づいて、前記マニピュレーターを制御する第２制御信号を生成し、生成した前記第２制御信号と前記ビジュアル制御部が生成した前記第１制御信号との両方に基づいて、前記ロボットのマニピュレーターを制御する手順と、
    を有することを特徴とする制御方法。
11. ロボットが把持する対象物の撮像画像に基づいて前記対象物と前記対象物を組み付ける被組み付け部品との相対的な位置関係を検出し、検出した前記位置関係に基づいて前記ロボットのマニピュレーターを制御する第１制御信号を生成するビジュアル制御部と、
    前記ビジュアル制御部が検出した前記位置関係と前記対象物にかかっている力を示す負荷情報とに基づいて、前記マニピュレーターを制御する第２制御信号を生成し、生成した前記第２制御信号と前記ビジュアル制御部が生成した前記第１制御信号との両方に基づいて、前記ロボットのマニピュレーターを制御するコンプライアントモーション制御部と、
    を備えることを特徴とするロボット装置。

Images