JP2006318301A - 適応型ビジュアルフィードバック制御方法 - Google Patents

Abstract

【課題】 ３次元空間の動きに対応するとともに、目標軌道へ高精度に追従することが可能で、煩雑なパラメータを必要としないリンク機構のビジュアルフィードバック制御方法を提供する。
【解決手段】 ロボット１の動作を制御する際に、撮像装置で取得した情報を基に判定した実際のハンド・アーム位置ｒ（ｑ_ｋ）と、目標位置ｒ_ｄ（ｔ_ｋ）との差に基づくフィードバック制御（図の中段）と目標位置ｒ_ｄ（ｔ_ｋ）の差分に基づくフィードフォワード制御（図の上段）に加えて、将来の目標位置ｒ_ｄ（ｔ_ｋ＋１）から求めた目標関節角ｑ_ｄ（ｔ_ｋ＋１）と実際の現在の関節角ｑ（ｔ_ｋ）との差分に基づいて制御を行う予見制御項（図の下段）を追加する。さらにヤコビアン行列をヤコビアン行列推定部２０３でオンライン推定する。
【選択図】 図３

Description

本発明は、ロボットハンド・アーム等の多関節を有するリンク機構の制御方法に関し、特に、撮像装置で取得した画像を基にして制御を行うのに適した適応型ビジュアルフィードバック制御方法に関する。

ロボットアイ等のカメラで撮影した画像情報を基にしてロボットハンド等のリンク機構を有する機械装置の作動をフィードバック制御するビジュアルフィードバック制御という制御手法が知られている（例えば、特許文献１参照）。

特許文献１記載の技術では、画像座標系上の対象物体の特徴部分の動き（２次元速度ベクトル）を作業座標系上の特徴部分の動きに変換する際に必要となる局所的な２次元速度ベクトル座標変化行列（画像ヤコビアン行列）を作業座標系上の対象物体の動きから推定して対象物体に対する機械装置の作動を制御している。
特開平１１−８５２３５号公報

しかしながら、この技術では、作業座標系上の対象物体の動きは所定の拘束平面上の動きに限定されており、３次元空間における動きに対して対応することができない。また、ビジュアルフィードバック制御系においては、視覚センサとして通常、テレビカメラ等のビデオフレームレートで画像を取り込むセンサ、画像処理系が使用されるが、実際の作業物体の動きが制御系に伝達されるまでには、ビデオフレームレート以上の遅れを生ずることは避けられないため、フィードバックループが遅くなり、制御入力が過大となって不安定になりやすく、追従誤差も大きくなりやすいという問題点がある。

そこで本発明は、３次元空間の動きに対応するとともに、目標軌道へ高精度に追従することが可能で、煩雑なパラメータを必要としないリンク機構のビジュアルフィードバック制御方法を提供することを課題とする。

上記課題を解決するため、本発明にかかるビジュアルフィードバック制御方法は、撮像装置で取得した画像を画像処理して得られた情報を基にして多関節を有するリンク機構のフィードバック制御を行う方法であって、リンク機構の目標位置姿勢を基に予見制御項を用いるとともに、リンク機構の位置姿勢の変分と関節角度の変分との関係を記述するヤコビアン行列を追従誤差から推定し、推定したヤコビアン行列を用いて制御指令値を求める適応型ビジュアルフィードバック制御方法であることを特徴とする。

制御時にフィードバック制御項に加えて予見制御項を追加することで目標軌道からのずれを修正する。また、ヤコビアン行列を追従誤差から推定して、制御指令値を求めることで、煩雑なパラメータ同定を回避している。

本発明によれば、予見制御項を用いることで、制御入力が過大になるのを抑制し、安定した制御を実現できるため、目標軌道への追従性の精度を向上させることができる。また、ヤコビアン行列のオンライン推定を行い、煩雑なパラメータ同定を回避することで、制御入力の計算が容易になるとともに、３次元空間の動きへの適用も可能としている。

以下、添付図面を参照して本発明の好適な実施の形態について詳細に説明する。説明の理解を容易にするため、各図面において同一の構成要素に対しては可能な限り同一の参照番号を附し、重複する説明は省略する。

図１は、本発明に係るロボットハンドの適応型ビジュアルフィードバック制御方法を実現するロボットハンドを有するロボットの概略構成図であり、図２は、制御装置のブロック構成図である。

図１に示されるように、ロボット１００の胴体８には、左右双方のアーム７Ｌ、７Ｒを介して、ロボットハンド１Ｒ、１Ｌが取り付けられている。この胴体８は、移動式または固定式の台座に取り付けられている。胴体８の頭部には、視覚センサであるカメラアイ３を備えている。ハンド１、アーム７の各関節には、関節駆動用のモータと、関節角度を検出するエンコーダポテンションメータが配置されている。

ここで、アーム７は、ハンド１の手首部分まででそれぞれ７自由度を有している。またハンド１は、母指とこれに対向する３指を有しており、このうち、母指１０は、４つの関節ごとにそれぞれモータが配置された４自由度のリンク系である。他の３指は、最先端の関節を除く３つの関節ごとにそれぞれモータが配置され、最先端の関節は、隣接する関節と連動する構成とされている。したがって、４関節でそれぞれ３自由度のリンク系を構成する。つまり、ロボットハンド１全体の自由度は１３となる。

制御装置は、ＲＡＭ、ＲＯＭ、ＣＰＵ等で構成される制御ＥＣＵ２を中心に構成されており、対象物およびロボットハンド１の画像を撮影するカメラアイ３の出力画像から画像認識処理により、対象物およびロボットハンド１の位置・姿勢を認識する画像認識部２０と、画像認識結果を基にして把持位置・把持姿勢の目標値を求める位置姿勢演算部２１と、目標値に応じてロボットハンド１の動きを制御するロボット制御部２２とを有している。ロボット制御部２２には、各エンコーダポテンションメータ１６と、各指の指先に配置されている触覚センサ１５の出力信号が入力され、モータドライバ４を制御することで、各モータ１４の動きを制御して、ロボットハンド１とアーム７の動きを制御する。

制御装置は、このような構成に限られるものではなく、指示された姿勢となるようロボットハンド１、アーム７の動きを制御する制御部と、画像認識装置や把持姿勢の演算部を、別体としてもよい。また、これらの演算部・制御部は、ハードウェア的に区分されていてもソフトウェア的に区分されていてもよい。

制御ＥＣＵ２によって行われる制御（適応型ビジュアルフィードバック制御）の制御系を示すブロック図を図３に示す。ここで、現在時刻をｔ_ｋで表し、前回タイムステップの時刻をｔ_ｋ−１、次回タイムステップの時刻をｔ_ｋ＋１で表すものとする。

目標軌道設定部２０１からは、現時点と将来（次回タイムステップ）の目標ハンド（アーム）位置ｒ_ｄ（ｔ_ｋ）、ｒ_ｄ（ｔ_ｋ＋１）がそれぞれ出力される。軌道計画部２０２には、このうち、将来の目標ハンド位置ｒ_ｄ（ｔ_ｋ＋１）が入力され、関節角の将来の目標値ｑ_ｄ（ｔ_ｋ＋１）が出力される。一方、現在のアーム関節角ｑ（ｔ_ｋ）は、エンコーダポテンションメータ１６により検出され、現在のハンド位置ｒ（ｑ_ｋ）は画像処理に基づいて画像認識部２０から出力される。アーム関節の駆動角速度（ｑ（ｔ_ｋ）のｑ上に記号・を付して表す。）はアーム関節角ｑ（ｔ_ｋ）を位置姿勢演算部２１内で時間微分することで得られる。

各モータへの指令値ｕ（ｔ_ｋ）は、３つの項、フィードバック制御項、フィードフォワード制御項、予見制御項からなる。

フィードバック制御項は、目標軌道設定部２０１から出力される目標ハンド位置ｒ_ｄ（ｔ_ｋ）と実際のハンド位置ｒ（ｑ_ｋ）との差分を所定の係数Ｋ倍し、後述するヤコビアン行列の推定値（Ｊｋ上に記号＾を付して表す。）の疑似逆行列（ヤコビアン行列の推定値に上添字＋を付して表す。）を乗じて得た値として定義される。この項は制御閉ループの安定性を確保する項である。

フィードフォワード制御項は、目標軌道設定部２０１から出力される目標ハンド位置ｒ_ｄ（ｔ_ｋ）を時間微分して、フィードバック制御項と同様にヤコビアン行列の推定値の疑似逆行列を乗じて得た値として定義される。この項は、ハンド１、アーム７の動作を目標軌道に追従させるための項である。

予見制御項は、軌道計画部２０２から出力される目標関節角ｑ_ｄ（ｔ_ｋ＋１）と実際の現在の関節角ｑ（ｔ_ｋ）との差分に対して、単位行列から後述するヤコビアン行列の推定値の疑似逆行列にヤコビアン行列の推定値行列を乗じた行列を減じた行列を乗じて得た値として定義される。この項は追従精度を向上させるとともに、制御入力を抑制して、安定した制御を実現するための項である。

式（１）は、この指令値ｕ（ｔ_ｋ）を表す式であり、右辺の第１項〜第３項がそれぞれフィードバック制御項、フィードフォワード制御項、予見制御項に該当する。

次に、ヤコビアン行列の推定について説明する。追従誤差をｅとし、ｅを式（２）により定義する。

このとき、ヤコビアン行列は、式（３）で示される。

ブロイデン法を適用して、式（２）と式（３）から推定式を導出すると、式（４）が導かれる。

式（４）を整理すると、次式（５）である。

（５）式のままだと、ｑ_ｋの微分値が略０のときに、推定値が発散するため、１より小さい正の定数εを用いて、式（５）を式（６）のように修正して用いる。

ヤコビアン行列推定部２０３は式（６）に基づいてヤコビアン行列のオンライン推定を行い、出力する。

次に、本発明にかかる制御方法の処理を図４のフローチャートを参照して説明する。最初に、画像認識部２０がカメラアイ３で取得した画像を画像処理することで対象物体の位置を認識する（ステップＳ１）。次に、位置姿勢演算部２１は、アーム７とハンド１の動作経路を計画し（ステップＳ２）、計画した経路を記憶する（ステップＳ３）。この動作経路の計画は、把持時点における接触点の位置座標を設定し、逆運動学的にそのときのハンド１とアーム７の位置・姿勢をそれぞれ算定した後、各ハンド１、アーム７の目標軌道を設定すればよい。

設定後、上述した制御系を利用してハンド１、アーム７の駆動を制御する（ステップＳ４）ことで、アーム７、ハンド１を設定した経路に沿って移動させることで、物体の把持を行う。さらに、物体を把持したまま所定の動作（例えば、物体をある場所から別の場所へと移動させる）を行わせる（ステップＳ５）。所定の動作を完了したら、処理を終了する。

発明者らは、本発明の制御方法による目標軌道への追従性の向上効果を確認するためのシミュレーションを行ったので、以下にその結果について述べる。図５は、シミュレーションで対象とした目標軌道を示している。ＸＹＺ座標系において（３７９．１，−３１１．５，７１４．７）ｍｍの初期位置から（５２９．１，−１６１．５，８６４．７）ｍｍの目標位置まで直線経路でアームを移動させる場合を考える。ここでは、初期位置から目標位置までの移動時間を１０秒に設定し、制御系のサンプリングタイムを４０ｍｓに設定し、予見制御なしと予見制御ありの２種類の制御方法を用いた場合のアームの軌跡を比較した。なお、ヤコビアン行列の初期値として以下の初期値を利用した。

図６（ａ）〜（ｃ）は、それぞれの制御方法における移動軌跡のＸ、Ｙ、Ｚ各座標値を目標軌跡と比較して示したものである。図に示されるように、予見制御を用いないビジュアルフィードバック制御においては、目標軌跡からのずれが大きい。本シミュレーションでは、このずれは発散にまでは至っていないが、最大で目標位置から１００ｍｍ近くはずれており、近くに障害物等が存在した場合には衝突してしまう可能性がある。

これに対して、予見制御を用いたビジュアルフィードバック制御においては、運動開始初期に目標軌跡からのずれが発生するが、早期に収束し、その後のずれはほとんど見られない。初期のずれはヤコビアン行列の初期値のずれに起因するものであり、動きに合わせて適切な設定を行うことでさらに低減することが可能と考えられる。

以上のシミュレーション結果から、本発明の制御方法により目標軌道への追従性を向上させる効果が確認された。また、ヤコビアン行列をオンラインで推定することで、煩雑な同定プロセスを回避しつつ、実際の動きに適合させる効果も確認できた。

ここでは、ロボットアームについてシミュレーションによる検証を行ったが、ロボットハンドについても同様の効果が得られる。また、本発明の適用範囲はこのようなロボットハンド、アームに限られるものではなく、多関節を有するリンク機構の制御として広く活用することが可能である。

本発明に係るロボットハンドの適応型ビジュアルフィードバック制御方法を実現するロボットハンドを有するロボットの概略構成図である。 図１のロボットの制御装置のブロック構成図である。 図２の制御系を示すブロック図である。 図３の制御処理を示すフローチャートである。 シミュレーションで対象とした目標軌道を示す図である。 予見制御の有無の２つの制御における移動軌跡のＸ、Ｙ、Ｚ各座標値を目標軌跡と比較して示したものである。

符号の説明

１…ロボットハンド、３…カメラアイ、４…モータドライバ、７…アーム、８…胴体、１４…モータ、１６…エンコーダポテンションメータ、１５…触覚センサ、２０…画像認識部、２１…位置姿勢演算部、２２…ロボット制御部、１００…ロボット、２０１…目標軌道設定部、２０２…軌道計画部、２０３…ヤコビアン行列推定部。

Claims (1)

1. 撮像装置で取得した画像を画像処理して得られた情報を基にして多関節を有するリンク機構のフィードバック制御を行う方法において、
   リンク機構の目標位置姿勢を基に予見制御項を用いるとともに、リンク機構の位置姿勢の変分と関節角度の変分との関係を記述するヤコビアン行列を追従誤差から推定し、推定したヤコビアン行列を用いて制御指令値を求めることを特徴とする適応型ビジュアルフィードバック制御方法。
   

Images