JP6818708B2 - マニピュレータシステム、制御装置、制御方法、およびプログラム - Google Patents

Description

実施形態は、マニピュレータシステム、制御装置、制御方法、およびプログラムに関する。

従来、エンドエフェクタを目標位置に移動させるマニピュレータシステムが知られている。

特開２０１２−１１４９８号公報

この種のマニピュレータシステムでは、例えば、障害物を回避しながら移動することが可能なより改善されたマニピュレータシステムが得られれば、有意義である。

実施形態のマニピュレータシステムは、マニピュレータと、マニピュレータを作動させるアクチュエータと、マニピュレータが障害物を避けながら移動するようアクチュエータを制御する制御装置と、を備える。制御装置は、マニピュレータの第一対象点の移動速度ベクトルと障害物の表面の第二対象点での法線ベクトルとの角度が９０°以内となるよう、アクチュエータを制御する。

図１は、実施形態のマニピュレータの模式的かつ例示的な構成図である。 図２は、実施形態の制御装置の模式的かつ例示的なブロック図である。 図３は、実施形態の制御装置による制御手順を示す模式的かつ例示的なフローチャートである。 図４は、第１実施形態のアクチュエータ制御部によるアクチュエータの制御によって移動するマニピュレータの位置および姿勢の経時変化を示す模式的かつ例示的な図である。 図５は、第１実施形態の第１変形例の制御装置の模式的かつ例示的なブロック図である。 図６は、第２実施形態の制御装置の模式的かつ例示的なブロック図である。 図７は、第３実施形態の制御装置の模式的かつ例示的なブロック図である。

以下、マニピュレータシステムの例示的な実施形態が開示される。以下に示される実施形態の構成や制御（技術的特徴）、ならびに当該構成や制御によってもたらされる作用および結果（効果）は、一例である。

また、以下の実施形態および変形例には、同様の構成要素が含まれている。以下では、それら同様の構成要素には共通の符号が付与されるとともに、重複する説明が省略される場合がある。また、本明細書において、序数は、構成要素等を区別するために便宜上付与されており、優先順位や順番を示すものではない。

［第１実施形態］
［マニピュレータシステム］
図１は、マニピュレータ１０の構成図である。マニピュレータ１０は、図１には不図示の制御装置２０（図２参照）とともに、マニピュレータシステム１００を構成している。マニピュレータ１０は、ロボットアームとも称されうる。

マニピュレータ１０は、複数のアーム１１が関節１２において回動可能に連結された所謂多関節アームとして構成されている。制御装置２０は、図１には不図示のアクチュエータ１４（図２参照）の作動を制御することにより、各関節１２における二つのアーム１１の角度を変化させ、これによりマニピュレータ１０の位置や姿勢を変化させる。関節１２は、例えば、回転関節や、ボールジョイント等である。なお、マニピュレータ１０は、直動関節を含んでもよい。

マニピュレータ１０は、その先端に、エンドエフェクタ１３を有している。制御装置２０による制御対象点Ｐｃは、例えば、エンドエフェクタ１３上に設定される。

制御装置２０は、制御対象点Ｐｃが所定の位置や姿勢となるよう、アクチュエータ１４を制御する。なお、制御対象点Ｐｃは、一点でもよいし、複数点でもよいし、エンドエフェクタ１３上に設定されなくてもよい。

アクチュエータ１４は、少なくとも一つの関節１２における二つのアーム１１の角度を変化させる。アクチュエータ１４は、制御装置２０が生じる電気的な制御信号によって駆動される。アクチュエータ１４は、例えば、モータや、人工筋肉等である。

また、制御装置２０は、詳細には後述するが、マニピュレータ１０が障害物３０と干渉しないよう、言い換えると、干渉回避の制約条件を満たすよう、アクチュエータ１４を制御する。本実施形態では、制御装置２０は、マニピュレータ１０の第一対象点Ｐｐの移動速度ベクトルｄｘ_ｐ／ｄｔと、障害物３０の表面３１の第二対象点Ｐｒでの法線ベクトルｎ_ｒと、の角度が９０°以内となるよう、アクチュエータ１４を制御する。これにより、第一対象点Ｐｐが第二対象点Ｐｒおよび表面３１に近付くのを抑制することができ、ひいては、マニピュレータ１０が障害物３０と接触するのを抑制することができる。干渉回避の制約条件は、第一制約条件の一例である。

第一対象点Ｐｐおよび第二対象点Ｐｒは、干渉回避の制約条件を設定する対象となる点であり、例えば、障害物３０とマニピュレータ１０との相対的な位置や姿勢に基づいて設定されうる。第一対象点Ｐｐとしては、例えば、エンドエフェクタ１３上の点や、関節１２上の点が選択されうる。また、第一対象点Ｐｐとして、例えば、いずれかのアーム１１の両端間の中間位置、言い換えるとアーム１１を挟む二つの関節１２間の中央位置が、選択されてもよい。

また、第一対象点Ｐｐとしては、一点が選択されてもよいし、複数点が選択されてもよい。また、第一対象点Ｐｐとしては、一例としては、障害物３０と干渉する確率の高い点が選択され、制御対象点Ｐｃとは必ずしも一致しない。第二対象点Ｐｒは、表面３１上の一点が選択されてもよいし、表面３１上の複数点が選択されてもよい。なお、図１では、第一対象点Ｐｐと第二対象点Ｐｒとが接した状態で描かれているが、実際の干渉回避制御では、制御装置２０は、第一対象点Ｐｐと第二対象点Ｐｒとが接しないよう、アクチュエータ１４を制御する。この場合、第一対象点Ｐｐと第二対象点Ｐｒとがより確実に接触しないための第一対象点Ｐｐと第二対象点Ｐｒとの距離のマージンや第一対象点Ｐｐと第二対象点Ｐｒとの距離の下限値等の距離条件が設定されてもよい。また、障害物３０を表面３１から距離のマージンに相当する高さだけ膨張させた仮想障害物に対して距離が０以上となる距離条件が設定されてもよい。

［制御装置および制御方法］
図２は、制御装置２０のブロック図である。図２に示されるように、制御装置２０は、目標取得部２１、障害物情報取得部２２、マニピュレータ情報取得部２３、およびアクチュエータ制御部２４を有している。

目標取得部２１は、制御対象点Ｐｃの目標情報を取得する。目標情報は、例えば、制御対象点Ｐｃの次のステップ（タイムステップ）における速度や角速度等である。目標情報は、例えば、予め設定されており、その場合、目標取得部２１は、制御装置２０が有している不図示の記憶部から、目標情報を取得することができる。

障害物情報取得部２２は、マニピュレータ１０が動作する範囲内において、避けるべき障害物３０の障害物情報を取得する。障害物情報は、例えば、障害物３０の表面３１の位置や形状等を示す情報である。障害物情報取得部２２は、ＲＧＢ−Ｄセンサや、カメラ、接触センサ、距離センサのような障害物センサ１５から、障害物情報を取得することができる。また、障害物情報取得部２２は、予め既知の障害物情報を持つデータベースや、ＬＩＤＡＲ（light detection and ranging、laser imaging detection and ranging）等から、障害物情報を取得してもよい。

マニピュレータ情報取得部２３は、マニピュレータ情報を取得する。マニピュレータ情報は、例えば、マニピュレータ１０の位置や姿勢を示す情報であって、例えば、各関節１２における角度である。マニピュレータ情報取得部２３は、ロータリエンコーダや、カメラ、角度センサ、距離センサのようなマニピュレータセンサ１６から、マニピュレータ情報を取得することができる。

アクチュエータ制御部２４は、目標情報、障害物情報、およびマニピュレータ情報に基づいて、マニピュレータ１０と障害物３０とが干渉しないことを制約条件として制御対象点Ｐｃの目標情報に対する移動の最適解を算出する。そして、アクチュエータ制御部２４は、当該最適解に沿ってマニピュレータ１０が移動するよう、アクチュエータ１４を制御する。

図３は、制御装置２０による制御手順を示すフローチャートである。図３に示されるように、制御装置２０にあっては、目標取得部２１が目標情報を取得し（Ｓ１０）、障害物情報取得部２２が障害物情報を取得し（Ｓ１１）、マニピュレータ情報取得部２３がマニピュレータ情報を取得する（Ｓ１２）。図３のフローにおいて、Ｓ１０〜Ｓ１２の順番は入れ替わってもよい。そして、アクチュエータ制御部２４が、干渉回避の制約条件下で制御対象点Ｐｃの目標情報に対する移動の最適解を算出し（Ｓ１３）、当該最適解に沿ってマニピュレータ１０が移動するようアクチュエータを制御する（Ｓ１４）。

制御装置２０による演算処理や制御は、ソフトウエアによって実行されてもよいし、ハードウエアによって実行されてもよい。また、制御装置２０による演算処理や制御には、ソフトウエアによる演算処理や制御とハードウエアによる演算処理や制御とが含まれてもよい。ソフトウエアによる処理の場合にあっては、制御装置２０は、ＲＯＭや、ＨＤＤ、ＳＳＤ、フラッシュメモリ等の記録媒体（記憶媒体）に記憶されたプログラム（アプリケーション）を読み出して実行する。制御装置２０は、プログラムにしたがって作動することにより、制御装置２０に含まれる各部、すなわち、目標取得部２１や、障害物情報取得部２２、マニピュレータ情報取得部２３、アクチュエータ制御部２４等として、機能する。この場合、プログラムには、上記各部に対応するモジュールが含まれる。

プログラムは、それぞれインストール可能な形式または実行可能な形式のファイルでＣＤ−ＲＯＭや、ＦＤ、ＣＤ−Ｒ、ＤＶＤ、ＵＳＢメモリ等の、コンピュータで読み取り可能な記録媒体に記録されて提供されうる。また、プログラムは、通信ネットワークに接続されたコンピュータの記憶部に記憶され、ネットワーク経由でダウンロードされることによって導入されうる。また、プログラムは、ＲＯＭ等に予め組み込まれてもよい。

また、制御装置２０の少なくとも一部がハードウエアによって構成される場合、当該制御装置２０には、例えば、field programmable gate array（ＦＰＧＡ）や、application specific integrated circuit（ＡＳＩＣ）等が含まれうる。

［制約条件付きの最適化問題］
次に、アクチュエータ制御部２４が実行する演算処理および制御について説明する。

マニピュレータ１０の自由度をｎとし、マニピュレータ１０の自由度を示す各関節角度をベクトルｑ＝［ｑ_１，ｑ_２，ｑ_３，…，ｑ_ｎ］^Ｔで表現する。

目標取得部２１から得られる制御対象点Ｐｃの次のステップにおける目標速度をｖ_cとする。これは、制御対象点Ｐｃにおける三次元的な位置を決定する速度ベクトルと制御対象点Ｐｃの姿勢を決定する角速度と、を有する。障害物３０を考えない場合、制御対象点Ｐｃにおけるヤコビ行列Ｊ_c（ｑ）を含む次の式（１）

を用いることにより、演算処理の３次元空間から関節角空間への変換が可能となる。ここに、

は関節角速度を表すベクトルであり、Ｊ_ｃ（ｑ）^＋はＪ_ｃ（ｑ）の擬似逆行列（ヤコビ行列が正方行列であれば、逆行列）を示す。したがって、制御対象点Ｐｃが目標情報になるべく従うよう移動する最適化問題については、以下の式（２）で表せる最小二乗問題の解を導出すればよい。

なお、制御対象点Ｐｃが複数（ｋ個）存在する場合は、対応するヤコビ行列と目標情報を用いて拡大行列系を以下の式（３）のように構成することで、関節角速度を算出することが可能である。

ヤコビ行列は、マニピュレータ１０上の第一対象点Ｐｐの速度、角速度とマニピュレータ１０の関節角の変化の関係を表した行列である。あるマニピュレータ１０上の点ａ（三次元座標をｘ_ａとおく）におけるヤコビ行列Ｊ_ａは、次の式（４）

のように表せる。ここに、関節ｑ_ｉにおける回転軸のベクトルをｚ_ｉと、関節の中心座標をｒ_ｉとおいた。このヤコビ行列Ｊ_ａにおいて、上の行は位置の関係を表し、下の行は姿勢の関係を表す。

マニピュレータ１０上の第一対象点Ｐｐと、障害物３０上の第二対象点Ｐｒとの距離が下限値に近い場合にあっては、マニピュレータ１０は次の動作で障害物３０を回避する必要がある。障害物３０との距離は、障害物情報とマニピュレータ情報中の関節角度より算出することができる。また、障害物情報より、障害物３０上の第二対象点Ｐｒにおける外向きの法線ベクトルｎ_ｒが得られているものとする。

なお、干渉回避の制約条件の演算処理は、最も距離が近い点あるいは最も距離が近付くことが予測される二点（第一対象点Ｐｐおよび第二対象点Ｐｒ）あるいはそのような二点の複数の組み合わせについて実行されうる。演算処理の対象となる第一対象点Ｐｐおよび第二対象点Ｐｒの選定や、その選定の判断基準となる第一対象点Ｐｐと第二対象点Ｐｒとの距離の算出は、マニピュレータ１０のアーム１１や障害物３０の円筒や直方体などの計算しやすい形状に近似した上で算出する、Ｂｏｂｌｏｗ法、Ｌｉｎ／Ｃａｎｎｙ法、Ｇｉｌｂｅｒｔ法などの公知の衝突判定手法を用いて、実行することができる。

第一対象点Ｐｐの次のステップにおける位置速度ベクトル

と、障害物３０の表面３１の第二対象点Ｐｒにおける法線ベクトルｎ_ｒとの内積は、次の式（５）で表せる。

ここに、α_ｐは、二つのベクトルのなす角度である。α_ｐが−９０°以上でありかつ９０°以下の範囲であれば、第一対象点Ｐｐは、障害物３０の無い方向に移動すると考えることができる。言い換えると、次の式（６）

が満たされれば、次のステップにおいて、第一対象点Ｐｐは第二対象点Ｐｒとは接触せず、この部位においてマニピュレータ１０は障害物３０とは干渉しない。ここに、Ｊ_Ｂｐ（ｑ）は、３×ｎのマニピュレータ１０上の第一対象点Ｐｐにおける位置のみを対象にしたヤコビ行列である。

なお、第一対象点Ｐｐおよび第二対象点Ｐｒの複数（ｍ個）の組み合わせについて演算する場合、以下の式（７）のような拡大行列を用いればよい。

アクチュエータ制御部２４は、式（６）で示される干渉回避の制約条件を満たしつつ、式（２）で示される制御対象点Ｐｃの目標情報に対する移動の最適解を算出する。すなわち、アクチュエータ制御部２４は、以下の式（８）で示されるような、制約条件付きの線形最小二乗問題の最適解を算出する。

このような、制約条件付き線形最小二乗問題は、内点法、有効制約法などにより高速に解くことが可能である。また、制約条件には、関節角度の上下限、関節角速度の上下限などを加えることも可能であり、かかる手法は、拡張性が高い手法であると言える。

図４は、アクチュエータ制御部２４によるアクチュエータ１４の制御によって移動するマニピュレータ１０の位置および姿勢の経時変化を示す図である。図４を参照すれば、エンドエフェクタ１３上に設定された制御対象点Ｐｃが目標情報になるべく従うように軌跡Ｐｔに沿って移動する際、マニピュレータ１０が全体的に第二対象点Ｐｒから距離δ（マージン）だけ離間した状態で、すなわち第二対象点Ｐｒと干渉しない状態で移動していることが、理解できよう。なお、図４の例では、第一対象点Ｐｐも、エンドエフェクタ１３上に設定されている。第三対象点Ｐｂは、第二対象点Ｐｒから距離δだけ表面３１の外方に突出した仮想的な対象点（仮想的な障害物の表面）であり、アクチュエータ制御部２４は、仮想的な障害物の表面上の第三対象点Ｐｂとの距離が０以上あるいは０より大きいとする制約条件下で、上記最適化問題を解くことができる。

以上、説明したように、本実施形態では、制御装置２０は、マニピュレータ１０の第一対象点Ｐｐの移動速度ベクトルと障害物３０の表面３１の第二対象点Ｐｒでの法線ベクトルとの角度が９０°以内となるよう、アクチュエータ１４を制御する。このような構成によれば、制御装置２０は、例えば、障害物３０と干渉することなく、マニピュレータ１０を動作させることができる。

また、本実施形態では、制御装置２０は、第一対象点Ｐｐにおけるヤコビ行列と表面３１の第二対象点Ｐｒでの法線ベクトルとに基づいて、移動速度ベクトルと法線ベクトルとの内積を０以上とする第一制約条件を含む制約条件付きの、マニピュレータ１０の制御対象点Ｐｃの目標動作の最適化問題を解くことにより、アクチュエータ１４を制御する。このような構成によれば、制御装置２０は、例えば、障害物３０と干渉することなく、かつ制御対象点Ｐｃがその目標情報にできるだけ従って移動するよう、マニピュレータ１０を動作させることができる。

また、本実施形態では、最適化問題は、予め定められた制御対象点Ｐｃの目標動作に対してアクチュエータ１４の作動を最適化する問題である。本実施形態によれば、制御装置２０は、制約条件を満たしつつ、制御対象点Ｐｃが予め定められた目標情報にできるだけ従って移動するよう、マニピュレータ１０を動作させることができる。

［第１変形例］
図５は、本変形例の制御装置２０Ａのブロック図である。本変形例のマニピュレータシステム１００Ａでは、制御装置２０Ａのマニピュレータ情報取得部２３は、アクチュエータ制御部２４の演算処理結果からマニピュレータ情報を取得する。具体的には、マニピュレータ情報取得部２３は、アクチュエータ制御部２４から、当該アクチュエータ制御部２４が算出した各関節１２の角速度を取得し、当該角速度を積分することにより、各関節１２の角度を取得することができる。

よって、本変形例によれば、マニピュレータセンサ１６が無くより簡素な構成のマニピュレータシステム１００Ａによって、上記第１実施形態と同様の効果を得ることができる。

［第２実施形態］
図６は、本実施形態の制御装置２０Ｂのブロック図である。本実施形態のマニピュレータシステム１００Ｂでは、制御装置２０Ｂは、カメラ１７の画像に基づくビジュアルサーボ制御によって、制御対象点Ｐｃを目標位置に移動させる。このため、制御装置２０Ｂは、画像取得部２５、特徴量算出部２６、およびビジュアルサーボ制御部２７を有している。

カメラ１７は、光電変換素子や光学系部品等を有した画像センサであり、例えば、エリアセンサや、ＲＧＢ−Ｄセンサ等である。カメラ１７は、例えば、マニピュレータ１０のエンドエフェクタ１３か、あるいはエンドエフェクタ１３と固定された部位に取り付けられる。

画像取得部２５は、カメラ１７から、目標物を撮影した画像を取得する。

特徴量算出部２６は、画像取得部２５で取得された画像データから画像処理により対象物の特徴を示す特徴量を算出する。

ビジュアルサーボ制御部２７は、主として以下の方程式（９）を解くことにより、制御対象点Ｐｃの速度、加速度を表すベクトルｖ_ｃを算出する。
Ｌｖ_ｃ＝−λｅ ・・・（９）
ここに、Ｌは、画像ヤコビ行列であり、ｅは、ビジュアルサーボを用いて最小化する偏差であり、λは安定性を確保するための正のスカラーである。

ビジュアルサーボ制御の手法としては、特徴量ベース手法と、位置ベース手法とがある。まずは特徴量ベース手法について説明する。

［特徴量ベース手法］
特徴量ベース手法においては、画像上の特徴量Ｘの座標が［ｘ，ｙ］^Ｔで表されるとき、特徴量Ｘに対する画像ヤコビ行列Ｌ_ｘおよび偏差ｅ_ｘは、次の式（１０）で表される。

ここに、［ｘ^＊，ｙ^＊］^Ｔは、特徴量Ｘの目標とする座標であり、Ｚは深さを表すパラメータである。Ｚは未知の値であるが、その推定方法は公知であり、また精度がそれほど高くない場合にあっても制御の安定性が損なわれ難いことが知られている。複数の特徴量を用いてＬ_ｘおよびｅ_ｘを縦に並べた拡大形の画像ヤコビ行列Ｌおよび偏差ｅを用いることにより、画像ヤコビ行列Ｌの疑似逆行列を求め、ベクトルｖ_ｃを算出する。

［位置ベース手法］
位置ベース手法においては、画像特徴量より制御対象点Ｐｃの目標となる位置と姿勢を算出し、現在の位置、姿勢との差を最小化する制御対象点Ｐｃの速度、角速度が算出される。位置偏差をｔ、姿勢偏差をＲとすれば、画像ヤコビ行列Ｌおよび偏差ｅは、以下の式（１１）で表される。

ここに、θおよびｕは、姿勢偏差Ｒを回転軸ｕおよび回転角θで表したベクトルであり、Ｉ_３は、３次元の単位行列であり、ｕ［ｘ］は、ｕの外積行列であり、ｓｉｎｃ（ｘ）＝ｓｉｎ（ｘ）／ｘである。姿勢偏差Ｒおよび位置偏差ｔは、特徴量から基礎行列やホモグラフィ行列を推定することにより算出可能である。

アクチュエータ制御部２４は、ビジュアルサーボ制御部２７から受け取った次のステップにおける制御対象点Ｐｃの位置を目標情報として、上記第１実施形態と同様に、制約条件付きの線形最小二乗問題の最適解を算出し、マニピュレータ１０が当該最適解を満たす位置および姿勢となるよう、アクチュエータ１４を制御する。

以上、説明したように、本実施形態では、最適化問題は、ビジュアルサーボ制御における制御対象点Ｐｃの目標動作に対してアクチュエータ１４の作動を最適化する問題である、本実施形態によれば、制御装置２０Ｂは、制約条件を満たしつつ、制御対象点Ｐｃがビジュアルサーボによって得られた目標情報にできるだけ従って移動するよう、マニピュレータ１０を動作させることができる。

［第３実施形態］
図７は、本実施形態の制御装置２０Ｃのブロック図である。本実施形態のマニピュレータシステム１００Ｃでは、制御装置２０Ｃは、カメラ１７の画像に基づくビジュアルサーボ制御によって、制御対象点Ｐｃを目標位置に移動させる。ただし、アクチュエータ制御部２４は、干渉回避の制約条件を満たしながらビジュアルサーボ制御も含む演算処理を実行し、当該演算処理結果に応じて作動するようアクチュエータ１４を制御する。アクチュエータ制御部２４は、ビジュアルサーボ制御によるタスクと、干渉回避制御（干渉回避の制約条件）によるタスクと、を実行する。ビジュアルサーボ制御によるタスクをビジュアルサーボタスクと称し、干渉回避制御によるタスクを干渉回避タスクと称する。

ビジュアルサーボタスクでは、アクチュエータ制御部２４は、主として以下の方程式（９）を解くことにより、制御対象点Ｐｃの速度、加速度を表すベクトルｖ_ｃを算出する。
Ｌｖ_ｃ＝−λｅ ・・・（９）
よって、以下の式（１２）で表せる最小二乗問題の解を導出すればよい。

したがって、アクチュエータ制御部２４は、以下の式（１３）で示されるような、制約条件付きの線形最小二乗問題の最適解を算出する。

すなわち、アクチュエータ制御部２４は、ビジュアルサーボタスクと、干渉回避タスクと、を考慮した次の式（１４）を解くことにより、各関節１２の角速度を算出することができる。

ここに、Ｉは、単位行列である。

また、アクチュエータ制御部２４は、ビジュアルサーボタスクが、制約条件タスクとしての第一ビジュアルサーボタスクと、最適化タスクとしての第二ビジュアルサーボタスクと、を含み、さらに制約条件タスクが、第一ビジュアルサーボタスクの他に干渉回避タスクを含む場合についても、制約条件を満たす最適解を算出し、マニピュレータ１０が当該最適解にしたがう動作を実行するようアクチュエータ１４を制御することができる。干渉回避タスクにおける制約条件は、第一制約条件の一例であり、第一ビジュアルサーボタスクにおける制約条件は、第二制約条件の一例である。

具体的に、アクチュエータ制御部２４は、第一ビジュアルサーボタスクとして、次の式（１５）、
Ｌ_１ｖ_ｃ＝−λ_１ｅ_１ ・・・（１５）
を制約条件とする。ここに、Ｌ_１はｌ×６行（ｌ＜６）の行列であり、このタスクを達成可能な解が複数あるものとする。

この第一ビジュアルサーボタスクは、ある１つの特徴量（座標［ｘ，ｙ］）を目標位置（座標［ｘ^＊，ｙ^＊］）に一致させようとする特徴量ベース手法により得られる。第一ビジュアルサーボタスクは、以下の式（１６）を解くことに相当する。

式（１６）で表される第一ビジュアルサーボタスクによれば、例えば、特徴量を対象物の中央にとり、目標位置をカメラの中央に設定すれば、対象物の中央が常にカメラ１７の画像の中央に位置されるような制御が実現される。

また、アクチュエータ制御部２４は、次の式（１７）

で表せる第二ビジュアルサーボタスクの最適解を求める。
ここに、画像ヤコビ行列Ｌ_２、偏差ｅ_２、およびＬ_θｕは、次の式（１８）によって与えられる。

よって、アクチュエータ制御部２４は、さらに干渉回避タスクを追加した以下の式（１９）を解くことにより、マニピュレータ１０の各関節１２の角速度を算出することができる。

このような構成によれば、制御装置２０Ｃは、障害物３０との干渉回避タスクおよび第一ビジュアルサーボタスクを制約条件としながら、制御対象点Ｐｃができるだけ第二ビジュアルサーボタスクに従って移動するよう、マニピュレータ１０を動作させることができる。

なお、本実施形態では、最適化タスクは第二ビジュアルサーボタスクであったが、アクチュエータ制御部２４は、最小二乗問題もしくは線形拘束で表現される最適化タスクと干渉回避タスクとが組み込まれたタスクを解くことができる。具体的には、例えば、予め定められた位置および姿勢の目標情報に対する最適化タスクを、上記第二ビジュアルサーボタスクと入れ替えることができる。

以上、本発明の実施形態を例示したが、上記実施形態は一例であって、発明の範囲を限定することは意図していない。実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、組み合わせ、変更を行うことができる。実施形態は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。また、実施形態の構成や形状は、部分的に入れ替えて実施することも可能である。また、各構成や形状等のスペック（構造や、種類、方向、形式、大きさ、長さ、幅、厚さ、高さ、角度、数、配置、位置、材質等）は、適宜に変更して実施することができる。

１０…マニピュレータ、１１…アーム、１２…関節、２０…制御装置、３０…障害物、３１…表面、１００…マニピュレータシステム、Ｐｃ…制御対象点、Ｐｐ…第一対象点、Ｐｒ…第二対象点。

Claims (7)

1. マニピュレータと、
   前記マニピュレータを作動させるアクチュエータと、
   前記マニピュレータが障害物を避けながら移動するよう前記アクチュエータを制御する制御装置と、
   を備え、
   前記制御装置は、前記マニピュレータの第一対象点の移動速度ベクトルと前記障害物の表面の第二対象点での法線ベクトルとの角度が９０°以内となるよう、前記アクチュエータを制御し、
   前記マニピュレータは、関節型マニピュレータであり、
   前記制御装置は、前記第一対象点におけるヤコビ行列と前記表面の前記第二対象点での法線ベクトルとに基づいて、前記移動速度ベクトルと前記法線ベクトルとの内積を０以上とする第一制約条件を含む制約条件付きの、前記マニピュレータの制御対象点の目標動作の最適化問題を解くことにより、前記アクチュエータを制御する、
   マニピュレータシステム。
2. 前記最適化問題は、予め定められた前記制御対象点の目標動作に対して前記アクチュエータの作動を最適化する問題である、前記アクチュエータを制御する、請求項１に記載のマニピュレータシステム。
3. 前記最適化問題は、ビジュアルサーボ制御における前記制御対象点の目標動作に対して前記アクチュエータの作動を最適化する問題である、請求項１に記載のマニピュレータシステム。
4. 前記制約条件は、前記制御対象点のビジュアルサーボ制御による第二制約条件を含む、請求項１〜３のうちいずれか一つに記載のマニピュレータシステム。
5. 請求項１〜４のうちいずれか一つに記載のマニピュレータシステムに含まれる制御装置。
6. 請求項５に記載の制御装置によってマニピュレータを制御するマニピュレータの制御方法であって、
   前記法線ベクトルを取得し、
   前記移動速度ベクトルと前記法線ベクトルとの角度が９０°以内となるよう、前記アクチュエータを制御する、マニピュレータの制御方法。
7. コンピュータを請求項５に記載の制御装置として機能させるプログラムであって、
   前記法線ベクトルを取得し、
   前記移動速度ベクトルと前記法線ベクトルとの角度が９０°以内となるよう、前記アクチュエータを制御する、プログラム。
   

Images