(57)【要約】【課題】  画像サンプリングタイミングと関節角サンプリングタイミングの適切な対応関係を選択するロボット制御システム、ロボット、ロボット制御方法及びプログラム等を提供すること。【解決手段】  ロボット制御システムは、複数の画像サンプリングタイミングにおける画像特徴量情報を取得する画像特徴量情報取得部（特徴量ベクトル取得部２０）と、複数の関節角サンプリングタイミングであって、前記画像サンプリングタイミングと非同期のサンプリングタイミングにおいて、ロボットの関節角情報を取得する関節角情報取得部（関節角ベクトル取得部１０）と、画像サンプリングタイミングと関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での画像特徴量と関節角情報に基づいて、ロボットを制御するロボット制御部２００を含む。【選択図】      図５
2014140941.tif 000002
【特許請求の範囲】
【請求項１】
複数の画像サンプリングタイミングにおける画像特徴量情報を取得する画像特徴量情報取得部と、
複数の関節角サンプリングタイミングであって、前記画像サンプリングタイミングと非同期のサンプリングタイミングにおいて、ロボットの関節角情報を取得する関節角情報取得部と、
前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での前記画像特徴量情報と前記関節角情報に基づいて、前記ロボットを制御するロボット制御部と、
を含むことを特徴とするロボット制御システム。
【請求項２】
請求項１において、
前記ロボット制御部は、
前記選択対応関係での前記画像特徴量情報と前記関節角情報に基づいて、前記選択対応関係でのヤコビアンを推定し、推定した前記ヤコビアンに基づいて前記ロボットを制御することを特徴とするロボット制御システム。
【請求項３】
請求項２において、
前記ロボット制御部は、
前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の前記対応関係の各対応関係において前記ヤコビアンを推定し、推定した前記ヤコビアンの評価値を求め、
前記評価値の比較処理に基づいて、複数の前記対応関係の中から前記選択対応関係を選択することを特徴とするロボット制御システム。
【請求項４】
請求項３において、
前記ロボット制御部は、
前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の前記対応関係の各対応関係において正規化方程式を作成し、作成した前記正規化方程式の解である最小自乗解を求めることで前記ヤコビアンを推定することを特徴とするロボット制御システム。
【請求項５】
請求項４において、
前記ロボット制御部は、
推定した前記ヤコビアンを用いて、前記ヤコビアンを推定したタイミングの次のタイミングである比較タイミングにおける前記画像特徴量情報を算出し、算出した前記画像特徴量情報と、前記画像特徴量情報取得部で取得された前記比較タイミングでの前記画像特徴量情報とに基づいて、前記評価値を求めることを特徴とするロボット制御システム。
【請求項６】
請求項４において、
前記ロボット制御部は、
推定した前記ヤコビアンを用いて、前記正規化方程式の自乗誤差を前記評価値として求めることを特徴とするロボット制御システム。
【請求項７】
請求項４乃至６のいずれかにおいて、
前記ロボット制御部は、
前記画像特徴量情報の差分により表されるベクトル又は行列をＦとし、前記関節角情報の差分により表される行列をＢとし、前記ヤコビアンを表すベクトル又は行列をＪとして、
Ｆ＝ＢＪ  ・・・・・式（１）
の最小自乗解を与える前記正規化方程式を作成し、
前記行列Ｂに対して、ＢＴＢの前記固有ベクトルを列ベクトルとする行列をＶとし、ＢＢＴの前記固有ベクトルを前記列ベクトルとする行列をＵとし、ＢＴＢの固有値λの逆数を対角要素とする行列をdiag［１／λ］とした場合に、
Ｂ＋＝Ｖdiag［１／λ］ＶＴＢＴ  ・・・・・式（２）
又は
Ｂ＋＝ＢＴＵdiag［１／λ］ＵＴ  ・・・・・式（３）
により、前記行列Ｂの前記一般化逆行列Ｂ＋を求め、求めた前記一般化逆行列Ｂ＋と前記式（１）から、
Ｊ＝Ｂ＋Ｆ  ・・・・・式（４）
に従って前記行列Ｊを求めることで前記ヤコビアンを推定することを特徴とするロボット制御システム。
【請求項８】
請求項４乃至７のいずれかにおいて、
前記ロボット制御部は、
前記ロボットのエンドポイント位置と前記関節角情報とを対応づけるロボットヤコビアンを求め、求めた前記ロボットヤコビアンに基づいて前記正規化方程式を作成することを特徴とするロボット制御システム。
【請求項９】
請求項８において、
前記ロボット制御部は、
基準タイミングでの前記ロボットヤコビアンと、所与のタイミングでの前記ロボットヤコビアンとの比較処理に基づいて、前記所与のタイミングでの前記画像特徴量情報の差分値と前記関節角情報の差分値の組についての重み付けを設定し、設定された重み付けを用いて前記正規化方程式を作成することを特徴とするロボット制御システム。
【請求項１０】
請求項８又は９において、
前記ロボット制御部は、
前記ロボットヤコビアンを解析的に求め、求めた前記ロボットヤコビアンに基づいて前記正規化方程式を作成することを特徴とするロボット制御システム。
【請求項１１】
請求項１乃至１０のいずれかにおいて、
前記画像特徴量情報取得部における前記画像サンプリングタイミングのサンプリング周波数をｆＡ、前記関節角情報取得部における前記関節角サンプリングタイミングの前記サンプリング周波数をｆＢとした場合に、
前記関節角情報取得部は、
ｆＢ≧ｆＡを満たす前記サンプリング周波数で前記関節角情報を取得することを特徴とするロボット制御システム。
【請求項１２】
複数の画像サンプリングタイミングにおける画像特徴量情報を取得する画像特徴量情報取得部と、
複数の関節角サンプリングタイミングにおけるロボットの関節角情報を取得する関節角情報取得部と、
前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での前記画像特徴量情報と前記関節角情報に基づいて、前記ロボットを制御するロボット制御部と、
を含むことを特徴とするロボット制御システム。
【請求項１３】
請求項１乃至１２のいずれかに記載のロボット制御システムを含むことを特徴とするロボット。
【請求項１４】
複数の画像サンプリングタイミングにおける画像特徴量情報を取得し、
複数の関節角サンプリングタイミングであって、前記画像サンプリングタイミングと非同期のサンプリングタイミングにおいて、ロボットの関節角情報を取得し、
前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での前記画像特徴量と前記関節角情報に基づいて、前記ロボットを制御することを特徴とするロボット制御方法。
【請求項１５】
複数の画像サンプリングタイミングにおける画像特徴量情報を取得する画像特徴量情報取得部と、
複数の関節角サンプリングタイミングであって、前記画像サンプリングタイミングと非同期のサンプリングタイミングにおいて、ロボットの関節角情報を取得する関節角情報取得部と、
前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での前記画像特徴量と前記関節角情報に基づいて、前記ロボットを制御するロボット制御部として、
コンピューターを機能させることを特徴とするプログラム。
【請求項１６】
複数の画像サンプリングタイミングにおける画像特徴量情報を取得し、
複数の関節角サンプリングタイミングであって、前記画像サンプリングタイミングと非同期のサンプリングタイミングにおいて、アームの関節角情報を取得し、
前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での前記画像特徴量と前記関節角情報に基づいて、前記アームを制御することを特徴とするロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御システム、ロボット、ロボット制御方法及びプログラム等に関する。
【背景技術】
【０００２】
ロボットの応用においては、より正確に期待する位置へロボットを移動させることが要求される。例えば、産業用用途においては、部品を把持した手先を、より正確に組み付け位置へ移動させることが要求される。従来の場合、この要求はより剛性の高いロボットをより正確に校正することによって実現されてきた。
【０００３】
しかし、どれだけ剛性を高くしても、発熱による熱膨張等、種々の要因により高精度での作業を迅速に行うことが困難な場合が考えられる。また、正確な校正作業には時間を要するという問題もある。
【０００４】
そのため、目標位置（目標状態）と、現在位置（現在状態）との差分をロボットの動きへフィードバックするフィードバック制御が広く用いられている。この場合、目標位置と現在位置との差分を、ロボットの動きへフィードバックするための変換処理に、ヤコビアンと呼ばれるパラメーターを用いる手法が知られている。
【０００５】
例えば特許文献１には、ロボットをヤコビアンを使って動作させながら、実際の動作が想定した動作とどの程度異なっているのかという情報を用いてヤコビアンを再推定するon-the-fly方式を用いたロボット制御手法が開示されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００６−３１８３０１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
目標位置や現在位置を表す特徴量ベクトルを［ｆ］とし、ロボットの関節角ベクトルを［θ］とする。この場合、ヤコビアン［Ｊ］は特徴量ベクトルの差分［Δｆ］及び関節角ベクトルの差分［Δθ］とを、［Δｆ］＝［Ｊ］［Δθ］と関連づけるものである。フィードバック処理を用いたロボット制御においては、［Δｆ］が目標値及び実測値等から求められた場合に、［Δθ］を制御パラメーターとして算出する必要があることから、ヤコビアンの逆行列である逆ヤコビアンを用いることになる。
【０００８】
特許文献１では、所望のロボットの動作と実際のロボットの動作の差である追従誤差ｅを定義し、それを用いてヤコビアンをon-the-flyで推定する手法が開示されている。この手法によれば、ロボットがある姿勢（つまり関節角ベクトルがある状態）のときに、特徴量ベクトルがどのような値であったか、というデータの組を得ることができれば、それを用いてヤコビアンを推定することができる。
【０００９】
しかし、ここには同期という問題がある。つまり特許文献１等の手法によりヤコビアンを計算するためには、「関節角ベクトルがある状態の時に、特徴量ベクトルがどのような値であったか」というデータが必要である。もしも関節角ベクトルと特徴量ベクトルとの時間的な対応が取れていない場合、つまり同期が取れていない場合には、特許文献１の手法であれば追従誤差ｅを正確に求めることはできず、その結果、ヤコビアンを正確に求めることはできない。
【００１０】
特許文献１等のように同期がとれていることを仮定しても問題ないとするためには、ロボット制御系から撮像系（ここでは特徴量として撮像画像から取得される画像特徴量情報を想定している）へ同期信号、シャッター信号を送るという手法を用いることが考えられる。しかしこれには、新たにコストの問題や、配線などの問題が発生する。
【課題を解決するための手段】
【００１１】
本発明の幾つかの態様によれば、画像サンプリングタイミングと関節角サンプリングタイミングの適切な対応関係を選択するロボット制御システム、ロボット、ロボット制御方法及びプログラム等を提供することができる。
【００１２】
本発明の一態様は、複数の画像サンプリングタイミングにおける画像特徴量情報を取得する画像特徴量情報取得部と、複数の関節角サンプリングタイミングであって、前記画像サンプリングタイミングと非同期のサンプリングタイミングにおいて、ロボットの関節角情報を取得する関節角情報取得部と、前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での前記画像特徴量情報と前記関節角情報に基づいて、前記ロボットを制御するロボット制御部と、を含むロボット制御システムに関係する。
【００１３】
本発明の一態様では、画像サンプリングタイミングと関節角サンプリングタイミングの間の複数の対応関係の中から適切な対応関係（選択対応関係）を選択し、当該選択対応関係での画像特徴量情報と関節角情報を用いてロボット制御を行う。よって、ロボットやカメラに対して特定のハードウェア等を追加することなく、データ間の同期をとることができ、非同期の場合に比べて高精度でのロボット制御を行うこと等が可能になる。
【００１４】
また、本発明の一態様では、前記ロボット制御部は、前記選択対応関係での前記画像特徴量情報と前記関節角情報に基づいて、前記選択対応関係でのヤコビアンを推定し、推定した前記ヤコビアンに基づいて前記ロボットを制御してもよい。
【００１５】
これにより、ヤコビアンを用いてロボット制御を行うこと等が可能になる。
【００１６】
また、本発明の一態様では、前記ロボット制御部は、前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の前記対応関係の各対応関係において前記ヤコビアンを推定し、推定した前記ヤコビアンの評価値を求め、前記評価値の比較処理に基づいて、複数の前記対応関係の中から前記選択対応関係を選択してもよい。
【００１７】
これにより、複数の対応関係の各対応関係におけるヤコビアンを推定し、推定したヤコビアンの評価値に基づいて選択対応関係を決定すること等が可能になる。
【００１８】
また、本発明の一態様では、前記ロボット制御部は、前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の前記対応関係の各対応関係において正規化方程式を作成し、作成した前記正規化方程式の解である最小自乗解を求めることで前記ヤコビアンを推定してもよい。
【００１９】
これにより、正規化方程式の解である最小自乗解としてヤコビアンを推定すること等が可能になる。
【００２０】
また、本発明の一態様では、前記ロボット制御部は、推定した前記ヤコビアンを用いて、前記ヤコビアンを推定したタイミングの次のタイミングである比較タイミングにおける前記画像特徴量情報を算出し、算出した前記画像特徴量情報と、前記画像特徴量情報取得部で取得された前記比較タイミングでの前記画像特徴量情報とに基づいて、前記評価値を求めてもよい。
【００２１】
これにより、ヤコビアンを用いて推定した画像特徴量情報と、実際に取得された画像特徴量情報とを用いてヤコビアンの評価値を求めること等が可能になる。
【００２２】
また、本発明の一態様では、前記ロボット制御部は、推定した前記ヤコビアンを用いて、前記正規化方程式の自乗誤差を前記評価値として求めてもよい。
【００２３】
これにより、最小自乗解として求められたヤコビアンを自乗誤差を求める式に戻すことで、ヤコビアンの評価値を求めること等が可能になる。
【００２４】
また、本発明の一態様では、前記ロボット制御部は、前記画像特徴量情報の差分により表されるベクトル又は行列をＦとし、前記関節角情報の差分により表される行列をＢとし、前記ヤコビアンを表すベクトル又は行列をＪとして、Ｆ＝ＢＪの最小自乗解を与える前記正規化方程式を作成し、前記行列Ｂに対して、ＢＴＢの前記固有ベクトルを列ベクトルとする行列をＶとし、ＢＢＴの前記固有ベクトルを前記列ベクトルとする行列をＵとし、ＢＴＢの固有値λの逆数を対角要素とする行列をdiag［１／λ］とした場合に、Ｂ＋＝Ｖdiag［１／λ］ＶＴＢＴ、又はＢ＋＝ＢＴＵdiag［１／λ］ＵＴにより、前記行列Ｂの前記一般化逆行列Ｂ＋を求め、求めた前記一般化逆行列Ｂ＋から、Ｊ＝Ｂ＋Ｆに従って前記行列Ｊを求めることで前記ヤコビアンを推定してもよい。
【００２５】
これにより、行列Ｂの一般化逆行列Ｂ＋を高速且つ安定して求めること等が可能になる。
【００２６】
また、本発明の一態様では、前記ロボット制御部は、前記ロボットのエンドポイント位置と前記関節角情報とを対応づけるロボットヤコビアンを求め、求めた前記ロボットヤコビアンに基づいて前記正規化方程式を作成してもよい。
【００２７】
これにより、ロボットヤコビアンを用いることで、より高精度に画像特徴量ヤコビアンを推定すること等が可能になる。
【００２８】
また、本発明の一態様では、前記ロボット制御部は、基準タイミングでの前記ロボットヤコビアンと、所与のタイミングでの前記ロボットヤコビアンとの比較処理に基づいて、前記所与のタイミングでの前記画像特徴量情報の差分値と前記関節角情報の差分値の組についての重み付けを設定し、設定された重み付けを用いて前記正規化方程式を作成してもよい。
【００２９】
これにより、２つのロボットヤコビアンの比較処理の結果得られる重み付けを用いて正規化方程式を作成することで、より高精度に画像特徴量ヤコビアンを推定すること等が可能になる。
【００３０】
また、本発明の一態様では、前記ロボット制御部は、前記ロボットヤコビアンを解析的に求め、求めた前記ロボットヤコビアンに基づいて前記正規化方程式を作成してもよい。
【００３１】
これにより、解析的に求めたロボットヤコビアンを用いて、より高精度に画像特徴量ヤコビアンを推定すること等が可能になる。
【００３２】
また、本発明の一態様では、前記画像特徴量情報取得部における前記画像サンプリングタイミングのサンプリング周波数をｆＡ、前記関節角情報取得部における前記関節角サンプリングタイミングの前記サンプリング周波数をｆＢとした場合に、前記関節角情報取得部は、ｆＢ≧ｆＡを満たす前記サンプリング周波数で前記関節角情報を取得してもよい。
【００３３】
これにより、関節角サンプリングタイミングのサンプリング周波数が、画像サンプリングタイミングのサンプリング周波数より大きい場合を対象として処理を行うこと等が可能になる。
【００３４】
また、本発明の他の態様は、複数の画像サンプリングタイミングにおける画像特徴量情報を取得する画像特徴量情報取得部と、複数の関節角サンプリングタイミングにおけるロボットの関節角情報を取得する関節角情報取得部と、前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での前記画像特徴量情報と前記関節角情報に基づいて、前記ロボットを制御するロボット制御部と、を含むロボット制御システムに関係する。
【００３５】
また、本発明の他の態様は、上記のロボット制御システムを含むロボットに関係する。
【００３６】
また、本発明の他の態様は、複数の画像サンプリングタイミングにおける画像特徴量情報を取得し、複数の関節角サンプリングタイミングであって、前記画像サンプリングタイミングと非同期のサンプリングタイミングにおいて、ロボットの関節角情報を取得し、前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での前記画像特徴量と前記関節角情報に基づいて、前記ロボットを制御するロボット制御方法に関係する。
【００３７】
また、本発明の他の態様は、上記の各部としてコンピューターを機能させるプログラムに関係する。
【００３８】
また、本発明の他の態様は、複数の画像サンプリングタイミングにおける画像特徴量情報を取得し、複数の関節角サンプリングタイミングであって、前記画像サンプリングタイミングと非同期のサンプリングタイミングにおいて、アームの関節角情報を取得し、前記画像サンプリングタイミングと前記関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での前記画像特徴量と前記関節角情報に基づいて、前記アームを制御するロボットに関係する。
【図面の簡単な説明】
【００３９】
【図１】図１（Ａ）〜図１（Ｃ）はヤコビアンとその具体例を説明する図。
【図２】look-then-move型のヤコビアン推定手法を説明する図。
【図３】図３（Ａ）は同期データの例、図３（Ｂ）〜図３（Ｄ）は非同期データの例。
【図４】特徴量ベクトルと関節角ベクトルの組み合わせの探索例。
【図５】ロボット制御システムの構成例。
【図６】図６（Ａ）、図６（Ｂ）はロボット制御システムを含むロボットの構成例。
【図７】図７（Ａ）、図７（Ｂ）は複数の組み合わせを用いたヤコビアン推定の手法を説明する図。
【図８】一般化逆行列を算出するブロックのシステム構成例。
【図９】評価処理の一例を説明する図。
【図１０】第１の実施形態のロボット制御処理を説明するフローチャート。
【図１１】画像特徴量ヤコビアンとロボットヤコビアンの関係を説明する図。
【図１２】ロボットヤコビアンを用いた重み付け後の連立方程式の例。
【図１３】第２の実施形態のロボット制御処理を説明するフローチャート。
【発明を実施するための形態】
【００４０】
以下、本実施形態について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。
【００４１】
１．本実施形態の手法
まず本実施形態の手法について説明する。ロボットの応用においては、より正確に期待する位置へ移動させることが要求される。例えば、産業用用途においては、部品を把持した手先を、より正確に組み付け位置へ移動させることが要求される。
【００４２】
従来の場合、この要求は、より剛性の高いロボットを、より正確に校正することによって実現されてきた。つまり、剛性の低さは制御の困難な非線形性の原因となるため、より剛性の高いロボットが要求されてきた。また、ロボットの剛性が高くとも残る種々の誤差に対応するために、より正確な校正が要求されてきた。
【００４３】
しかし、このような対応には多くの制限や限界がある。例えば、ロボットを駆動するモーターが発熱することは避けられず、それによる熱膨張などのために数１００ミクロン単位での誤差が発生する。これを避けるためには、予め、ロボットを熱平衡状態とするためのウォームアップが必要となる。つまり高精度が要求される場合に、作業をすぐに開始できないという制限がある。また、ウォームアップ後の校正作業も、非常に作業時間を要求するものである。また、個々のロボットの設置状況に対応して、個々のロボットそれぞれに対して個別に校正を行う必要もあった。
【００４４】
これらに対応するための方法としては、何らかのセンサーからの出力をロボットの制御処理に組み込んだフィードバック制御を行う方法がある。例えば、カメラなどの撮像手段により、目標位置（状態）と現在位置（状態）との差分を検出し、それをロボットの姿勢、動きへフィードバックすることにより、ロボットの非線形性などへ対応する方法である。この方法が理想的に動作するならば、熱膨張などの変動要因に対しても容易に対応が可能であり、更には校正作業なども不要とすることが可能である。
【００４５】
上記の「目標位置と現在位置との差分」を「ロボットの動きへフィードバック」するための変換処理にはいくつかの種類が存在する。その中でも最も直接的に変換を行う手法は、ヤコビアンと呼ばれるパラメーターを用いる手法である。
【００４６】
目標位置や現在位置を表現する特徴量をまとめたものを特徴ベクトル［ｆ］とし、ロボットの姿勢を表現するロボットの関節角をまとめたものを関節角ベクトル［θ］とする。これらは一般的には非線形なベクトル関数［Ｆ］を用いて関連付けられる。ヤコビアン、あるいはヤコビ行列［Ｊ］は、ある点［θ０］の周りで、このベクトル関数を展開した１次の項である。つまり、ヤコビアンとは図１（Ａ）又は図１（Ｂ）に示したように、関節角空間での微少変位を、特徴量空間へと写像するものとなる。なお、本実施形態では特徴量として画像特徴量情報を用いるため、ヤコビアンとしては図１（Ｂ）に示した画像特徴量ヤコビアンを用いることになる。そして、ヤコビアンを表す式としては、例えば図１（Ｃ）に示した式が考えられる。ただし第２の実施形態で後述するように、画像特徴量ヤコビアンの推定精度の向上のために、図１（Ａ）に示したロボットヤコビアンを解析的に求めて処理に用いてもよい。
【００４７】
目標とする特徴量ベクトルを［ｆ０］、現在の特徴量ベクトルを［ｆ］とすると、下式（１）が得られる。
【００４８】
［ｆ］−［ｆ０］＝［Δｆ］＝［Ｊ］［Δθ］  ・・・・・（１）
従って、［Ｊ］の一般化逆行列（あるいは疑似逆行列とも呼ばれる）［Ｊ］−１を求めることができれば、下式（２）が得られる。
【００４９】
［Δθ］＝［Ｊ］−１［Δｆ］  ・・・・・（２）
上式（２）で得られる［Δθ］（或いは［Δθ］にゲイン等を乗じた値）を、現在のロボットの姿勢である［θ］に加算する処理を行う、つまり［Δθ］分だけロボットの関節角を動作させることにより、［Δｆ］を小さくすることができ、ロボットを目標とする位置や状態に近づけることが可能となる。
【００５０】
従来、look-then-move型のヤコビアン推定手法として、例えば図２に示した手法を用いる例が見られた。つまり、特定の関節を微小に変化させた場合の特徴量ベクトルの変化は、ヤコビアン行列の特定の列の値に対応する。これを、すべての関節について行えば、ヤコビアン行列を求めることが可能となる。実際にこのようにして求めたヤコビアンでロボットを制御することが可能である。またこのように特定の関節を単独で動作させるのではなく、複数の関節を同時に動作させても、その動作が線形独立であれば、同様の処理を行うことが可能である。
【００５１】
しかし、このような方法においては、ヤコビアンを推定する動作と、実際にロボットを目標位置へ動かす動作が分離されるという問題がある。つまり、止まって、見て、ヤコビアンを推定し、それを用いてロボットを動作させる、と言う動作が繰り返されることになる。これはロボットの動作速度を著しく遅くするという問題がある。
【００５２】
それに対して特許文献１では、所望のロボットの動作と実際のロボットの動作の差である追従誤差ｅを定義し、それを用いてヤコビアンをon-the-flyで推定する手法が開示されている。この手法によれば、ロボットがある姿勢（つまり関節角ベクトルがある状態）のときに、特徴量ベクトルがどのような値であったか、というデータの組を得ることができれば、それを用いてヤコビアンを推定することができる。
【００５３】
つまり従来手法においては、図３（Ａ）に示したように画像データ（ビジュアルサーボ等を想定しているため、当該画像データから特徴量ベクトルを取得する）と、その画像データを取得したときのロボット姿勢（関節角ベクトル）がタイミング的にあっている同期データであるということを仮定している。
【００５４】
ロボット制御系から撮像系へ同期信号、シャッター信号を送るという手法を用いることで、従来手法のように同期を仮定しても問題ないと考えられる。しかしこれには、コストの問題や、配線などの問題が発生する。そして同期信号、シャッター信号等を用いないものとすれば、図３（Ｂ）〜図３（Ｄ）を用いて後述するように、データ間の非同期が生じることになり、正確なヤコビアンの推定が困難となる。
【００５５】
そこで本出願人は、高コストで複雑な機器を用いない場合にも、特徴量ベクトルと関節角ベクトルとの間の同期を取って、正確にヤコビアンを推定する手法を提案する。具体的には、データ間は図３（Ｂ）〜図３（Ｄ）を用いて後述するように、原点ずれや揺らぎ、或いはその両方に起因して非同期となる。そこでここでは、原点ずれの発生しうる範囲及び揺らぎの発生しうる範囲を設定し、当該範囲で考えられる特徴量ベクトルと関節角ベクトルとの間の複数の組み合わせについてヤコビアンを推定する。例えば、あるタイミングの特徴量ベクトル（画像データ）に対して、４つの関節角ベクトル（関節角データ）が対応するような揺らぎが発生する場合には、図４に示したように、いくつもの組み合わせの候補が考えられる中、そのうちの最適な組み合わせ（図４の例であれば太い線で示した矢印により表される組み合わせ）を探索する問題を解くことになる。
【００５６】
具体的には、複数の対応関係の各対応関係で推定した複数のヤコビアンを、所与の評価手法で評価し、最適なヤコビアンを当該タイミングでのヤコビアンとして採用する。なお、本実施形態は、画像サンプリングタイミングの周期性及び関節角サンプリングタイミングの周期性は仮定した上での処理となる。
【００５７】
ただし、本実施形態の手法では、所与のタイミングでのヤコビアンを決定するためには、ある程度の数の組み合わせ（例えば数万通りの組み合わせ）についてヤコビアンを推定する必要が生じる。そのため１回１回のヤコビアン推定の高速性が要求されることになる。そこで本出願人は、後述する式（１３）を用いて高速且つ安定して（ロバストに）一般化逆行列を求め、求めた一般化逆行列を用いてヤコビアンを推定する手法を合わせて提案する。
【００５８】
以下、第１の実施形態及び第２の実施形態について説明する。第１の実施形態では、下式（１３）を用いたヤコビアンの推定を含め、基本的な手法について説明する。第２の実施形態では、解析的に求めたロボットヤコビアンを用いて、ヤコビアン（狭義には画像特徴量ヤコビアン）の推定精度を向上させる第１の実施形態の変形例について説明する。
【００５９】
２．第１の実施形態
第１の実施形態について説明する。ロボット制御システムの構成例について説明した後、画像特徴量情報と関節角情報の複数の対応関係を設定する手法について説明する。そして設定された各対応関係におけるヤコビアンを推定する手法を説明し、推定した複数のヤコビアンの中から適切なヤコビアンを選択する（つまり適切な対応関係を選択する）手法について説明する。最後に図１０を用いて本実施形態の処理の詳細を説明する。
【００６０】
２．１  システム構成例
図５に本実施形態のロボット制御システムのシステム構成例を示す。ロボット制御システムは、関節角ベクトル取得部（関節角情報取得部）１０と、特徴量ベクトル取得部（画像特徴量情報取得部）２０と、記憶部３０と、ロボット制御部２００とを含む。ただし、ロボット制御システム及びロボット制御システムを構成する各部は、図５の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。
【００６１】
関節角ベクトル取得部１０は、ロボットの関節角情報を取得する。関節角情報は種々の形式により表現可能であるが、例えば１回の関節角サンプリングタイミングにおいて、ロボットの自由度（関節数）に相当する個数の値を要素とする関節角ベクトルを取得してもよい。具体的には、７自由度ロボットであれば１つの関節角情報は、θ０〜θ６の７つの値を要素として有する関節角ベクトルである。
【００６２】
特徴量ベクトル取得部２０は、センサー値等から特徴量情報を取得する。特徴量情報はロボットの位置（或いは状態）を表現するものであるが、本実施形態ではビジュアルサーボ等の画像を用いた制御を想定しているため、撮像した画像から取得される画像特徴量を特徴量情報とする。なお、取得した画像に対して所与の画像処理を行うことで特徴量情報を取得する場合が考えられるように、特徴量情報の取得には何らかの前処理が必要となる場合もある。本実施形態の特徴量ベクトル取得部２０はそのような前処理を行うものであってもよい。或いは画像そのもの（例えば各画素の画素値の集合）を画像特徴量情報として取得してもよい。
【００６３】
記憶部３０は、種々の情報を記憶するものであり、その機能はＲＡＭ等のメモリーやＨＤＤ（ハードディスクドライブ）などにより実現できる。記憶部３０は、関節角ベクトル列記憶部３１と、特徴量ベクトル列記憶部３３と、目標特徴量記憶部３５を含む。
【００６４】
関節角ベクトル列記憶部３１は、複数の関節角ベクトルを例えばＦＩＦＯの形で記憶する。本実施形態では、後述するように図７（Ｂ）に示した式（或いはそれに相当する式）を解くことでヤコビアンを取得することを想定しているため、関節角ベクトルの差分値が図７（Ａ）に示したように少なくとも７つ（広義にはロボットの自由度に対応するだけ）必要となる。よってここでは、関節角ベクトル列記憶部３１は少なくとも８つの関節角ベクトルを記憶する必要がある。
【００６５】
特徴量ベクトル列記憶部３３は、複数の特徴量ベクトルを例えばＦＩＦＯの形で記憶する。関節角ベクトル列記憶部３１と同様に、特徴量ベクトル列記憶部３３は少なくとも８つの特徴量ベクトルを記憶する必要がある。
【００６６】
目標特徴量記憶部３５は、目標となる特徴量を記憶する。本実施形態ではフィードバック処理を行うロボット制御手法を想定しているため、各タイミングでの特徴量情報が、目標特徴量記憶部３５に記憶されている目標特徴量に近づくように（差が小さくなるように）制御を行うことになる。
【００６７】
ロボット制御部２００は、画像サンプリングタイミングと関節角サンプリングタイミングの適切な対応関係を選択するとともに、選択対応関係でのヤコビアンに基づいてロボットの制御を行う。ロボット制御部２００は、対応関係設定部２１０と、ヤコビアン推定部２２０と、評価処理部２３０と、逆ヤコビアン算出部２４０と、目標差分ベクトル算出部２５０と、行列・ベクトル積算出部２６０と、制御データ出力部２７０とを含む。
【００６８】
対応関係設定部２１０は、データ間の非同期の要因（ここでは原点ずれと揺らぎ）に基づいて、画像サンプリングタイミングと関節角サンプリングタイミングの複数の対応関係を設定する。
【００６９】
ヤコビアン推定部２２０は、対応関係設定部２１０で設定された複数の対応関係の各対応関係におけるヤコビアンを推定する。本実施形態では下式（１３）等を用いてヤコビアンを推定するものであり、詳細については後述する。
【００７０】
評価処理部２３０は、ヤコビアン推定部２２０において推定された複数の（狭義には対応関係設定部２１０で設定された全ての対応関係に相当する数の）ヤコビアンについて評価値を算出し、最適なヤコビアンを決定するとともに、当該最適なヤコビアンに対応するデータ間の対応関係を選択対応関係とすることでデータ間の同期をとる。
【００７１】
逆ヤコビアン算出部２４０は、評価処理部２３０により決定された最適なヤコビアンの逆ヤコビアン（上式（２）の［Ｊ］−１に相当）を算出する。逆ヤコビアンの算出には種々の手法が知られており、本実施形態でも従来手法のいずれかを用いてもよい。ただし、ヤコビアン推定部２２０における一般化逆行列を求める処理（下式（１３））は、ヤコビアンを入力とし、その一般化逆行列である逆ヤコビアンを求める処理に用いることも可能である。つまり、本実施形態の逆ヤコビアン算出部２４０は、下式（１３）を用いて逆ヤコビアンを算出してもよい。
【００７２】
目標差分ベクトル算出部２５０は、処理タイミングでの画像特徴量情報と、目標特徴量記憶部３５に記憶された目標特徴量情報との差分を算出する。目標差分ベクトル算出部２５０の出力は上式（２）の［Δｆ］に相当する。
【００７３】
行列・ベクトル積算出部２６０は、逆ヤコビアン算出部２４０から出力された逆ヤコビアン［Ｊ］−１と、目標差分ベクトル算出部２５０から出力された目標差分ベクトル［Δｆ］との積を算出する。つまりここでは上式（２）の［Δθ］を求める処理を行う。
【００７４】
制御データ出力部２７０は、求めた［Δθ］、或いはそれにゲイン等を乗じた値を制御データとして出力する。具体的には、ロボットに対して各関節を制御データに相当する角度だけ変更する指示を出力することになる。
【００７５】
図６（Ａ）に本実施形態のロボット制御システムを含むロボット（ロボットシステム）の例を示す。このロボットシステムは、制御装置３００（上述のロボット制御システムに相当）と、ロボット本体３１０（広義にはロボット）とを含む。制御装置３００はロボット本体３１０の制御処理を行う。具体的には、ロボット本体３１０に含まれる駆動部（例えば関節毎に設けられる関節駆動モーター等）に対して、関節角を上記［Δθ］に相当する角度だけ変化させる指示を、上記制御データとして出力することで、ロボット本体３１０が目標位置（目標状態）に移行する制御を行う。ロボット本体３１０は、アーム３２０及びハンド（把持部）３３０を有する。そして制御装置３００からの動作指示にしたがって動作する。例えば、図示しないパレットに載せられたワークを把持したり、移動するなどの動作を行う。また、図示しない撮像装置で取得された撮像画像情報に基づいて、ロボット本体３１０の姿勢やワークの位置などの情報が検出され、検出された情報が制御装置３００に送られる。
【００７６】
本実施形態のロボット制御システムは例えば図６（Ａ）の制御装置３００に設けられ、例えば制御装置３００のハードウェアやプログラムによりロボット制御システムが実現される。そして本実施形態のロボット制御システムによれば、制御装置３００などの制御ハードウェアに対する性能要求を低減できると共に、ロボット本体３１０を高い応答性で動作させることなどが可能になる。
【００７７】
なお、図６（Ａ）では、ロボット本体３１０（ロボット）と制御装置３００（ロボット制御システム）とが別体に構成されているが、図６（Ｂ）に示すようにロボット本体３１０と制御装置３００とが一体に構成されていてもよい。具体的には図６（Ｂ）では、ロボット本体３１０を支えるベースユニット部に制御装置３００が格納されている。そして、ベースユニット部には車輪等が設けられ、ロボット全体が移動可能な構成となっている。また図６（Ａ）は単腕型のロボットの例であるが、本実施形態のロボットは図６（Ｂ）に示すような双腕型等の多腕型のロボットであってもよい。
【００７８】
２．２  対応関係設定処理
対応関係設定部２１０で行われる、画像サンプリングタイミングと関節角サンプリングタイミングの対応関係の設定処理について説明する。ここでの画像サンプリングタイミングとは、画像データ（及びそれに基づく画像特徴量情報）が取得されるタイミングを表し、具体的なレートはカメラの性能等に依存することになるが、例えば１０ミリ秒程度の周期であることが考えられる。また、関節角サンプリングタイミングとは、ロボットの姿勢データである関節角情報が取得されるタイミングを表し、例えば１ミリ秒程度の周期である。
【００７９】
本実施形態では、画像サンプリングタイミングの周期性、及び関節角サンプリングタイミングの周期性を仮定する。つまり、同一のカメラを同一の条件で用いている限り、画像特徴量情報の取得間隔が９ミリ秒になったり、１１ミリ秒になったりという変動は起こらないものとしており、実際のカメラやロボットの特性を考慮してもこの仮定による問題は考えにくい。
【００８０】
同期がとれた理想的なデータ（同期データ）であれば、画像サンプリングタイミングと関節角サンプリングタイミングの関係は図３（Ａ）に示したようになり、矢印で結ばれたタイミングの画像特徴量情報と関節角情報を処理に用いればよい。これに対して本実施形態では、何らかの遅延に起因して原点ずれが生じる可能性を考慮する。具体的には図３（Ｂ）に示したように、ある関節角サンプリングタイミングの関節角情報が、当該関節角サンプリングタイミングと同時や直近ではなく、離れた時点の画像サンプリングタイミングの画像特徴量情報に対応することが考えられる。つまり、所与の画像サンプリングタイミングに対応する関節角サンプリングタイミングとしては、ある程度の範囲（例えば関節角サンプリング周期が１ミリ秒であり、原点ずれの範囲が５０ミリ秒であるとすれば５０サンプリング分）を対応関係の候補として設定する必要がある。
【００８１】
また、カメラとロボットの動作周期が適当な整数比にならない場合等には、揺らぎに起因する非同期も考慮する必要がある。例えば、安価なＮＴＳＣ型のカメラのフレームレートは毎秒２９．９７フレームであるし、ロボットの動作周期も１２８マイクロ秒×８の１．０２４ミリ秒の場合もある。さらにはハードウェアに起因して揺らぎが発生する場合もある。具体的には、原点ずれがない場合であっても図３（Ｃ）に示したように、所与の画像サンプリングタイミングに対応する関節角サンプリングタイミングが、同時のものではなく、その前後にずれることが考えられる。例えば関節角サンプリング周期が１ミリ秒であり、相対的な周期揺らぎを±１ミリ秒とすれば、前後１サンプルを含めた３サンプリング分については対応関係の候補として設定する必要がある。
【００８２】
本実施形態では、図３（Ｄ）に示したように原点ずれと揺らぎの両方を考慮して対応関係を設定する。例えば最初の処理タイミングにおいては、原点ずれを考慮する必要があるため、画像サンプリングタイミングと関節角サンプリングタイミングの対応関係としては原点ずれの範囲に相当する数（上記の例であれば５０個）を設定する。一方、一回最適なヤコビアンが推定されれば、その際の原点ずれの程度はわかり、以降の処理においても当該原点ずれの値を用いることができる。つまり一度原点を合わせてしまえば、それ以降は揺らぎだけを考えればよいので、画像サンプリングタイミングと関節角サンプリングタイミングの対応関係としては揺らぎの範囲に相当する数（上記の例であれば３個）を設定する。
【００８３】
ただし、本実施形態では後述するように、ヤコビアンの推定において画像特徴量情報の差分値と関節角情報の差分値の組を少なくとも７つ用いるものとしている。つまり対応関係設定部２１０においては、少なくとも８組の画像サンプリングタイミングと関節角サンプリングタイミングの対応関係を（狭義には全て網羅するように）設定する必要がある。
【００８４】
この場合、最初の処理タイミングにおいては、原点ずれを考慮する必要があるため、８組のうち１組の対応関係について原点ずれに相当するだけの数（例えば５０通り）を設定し、他の７組の対応関係について揺らぎに相当するだけの数（例えばそれぞれ３通りの３７通り）を設定する。つまり、具体的な数値例でいえば５０×３７通りの対応関係を設定し、設定した対応関係をヤコビアン推定部２２０に出力する。一方、原点合わせが終わっているのであれば、８組の対応関係についてそれぞれ揺らぎに相当するだけの数（例えばそれぞれ３通りの３８通り）を設定すればよい。
【００８５】
２．３  ヤコビアンの算出処理
対応関係設定部２１０で設定された複数の対応関係の各対応関係におけるヤコビアンを推定する手法について説明する。以下の説明では、特徴量情報（特徴量ベクトル）としてｆ０〜ｆ５からなる６次元ベクトルを考え、関節角情報（関節角ベクトル）としてθ０〜θ６からなる７次元ベクトルを考えるものとするが、これに限定されず任意の特徴量情報、関節角情報に対して以下の手法を適用可能である。
【００８６】
ヤコビアンは上式（１）を満たすものであるから、下式（３）が成り立つ。
【００８７】
【数１】
2014140941.tif 000003
ここでΔｆ０〜Δｆ５は２つの（狭義には隣り合うタイミングでの）特徴量ベクトルの差分を表し、Δθ０〜Δθ６は２つの関節角ベクトルの差分を表すものである。つまり対応関係設定部２１０により、対応付けされた画像特徴量情報と関節角情報の組が２組設定されれば、２つの画像特徴量情報の差分をとることで１つのΔｆ０〜Δｆ５が求められ、２つの関節角情報の差分をとることで１つのΔθ０〜Δθ６が求められるため、上式（３）を設定することができる。
【００８８】
ここで下式（４）のように両辺の転置を考えることで下式（５）が成り立つ。
【００８９】
［Δｆ］Ｔ＝［Δθ］Ｔ［Ｊ］Ｔ  ・・・・・（４）
【００９０】
【数２】
2014140941.tif 000004
上式（５）に示したように、Δｆ０〜Δｆ５及びΔθ０〜Δθ６が既知の値として取得することができた場合には、それらの値を用いてＪを求めることになる。しかし、上式（５）では未知数がＪ００〜Ｊ５６の４２個あるのに対して、方程式の数は６個である。よって条件が十分でないため近似的に解くことしかできない。そのため、本実施形態では１次独立な成分を持つ式を７組以上まとめることで、より正確な解を求めるものとする。
【００９１】
具体的には、対応関係の設定処理において説明したように、画像特徴量情報と関節角情報の組を少なくとも８組対応づける対応関係を設定する。そして隣り合う画像特徴量情報同士の差分、及び隣り合う関節角情報同士の差分を求めることで、Δｆ０〜Δｆ５及びΔθ０〜Δθ６をそれぞれ７組取得することができる。
【００９２】
これを示したものが図７（Ａ）、図７（Ｂ）である。図７（Ａ）の（１）〜（７）に示したように、Δｆ０〜Δｆ５の各組を行要素とする７行の行列、及びΔθ０〜Δθ６の各組を行要素とする７行の行列を考えれば、それらの行列は図７（Ｂ）に示した式を満たすことになる。この場合、４２個の未知数に対して４２個の方程式（ここでは上述したようにそれぞれが線形独立であることを前提としている）を設定することができるため、ヤコビアンを正確に求めることが可能である。
【００９３】
また、対応関係設定部２１０により、対応付けされた画像特徴量情報と関節角情報の組を９組以上設定することで、Δｆ０〜Δｆ５及びΔθ０〜Δθ６をそれぞれ８組以上取得するものとしてもよい。この場合、未知数に対して条件式が過剰となるため、全ての方程式を満足するＪを求めることができない可能性が出てくる。ここで、ｘがｎ次元ベクトル、ｙがｍ次元ベクトル、Ａがｍ×ｎの行列である場合に、下式（６）の線形連立方程式を考える。
【００９４】
ｙ＝Ａｘ  ・・・・・（６）
この場合、下式（７）で与えられる自乗誤差を最小にする最小自乗解は、下式（８）の正規化方程式の解として与えられるため、最小自乗解ｘ＊は下式（９）となる。
【００９５】
||ｙ−Ａｘ||２＝（ｙ−Ａｘ）Ｔ（ｙ−Ａｘ）  ・・・・・（７）
ＡＴＡｘ＝ＡＴｙ  ・・・・・（８）
ｘ＊＝（ＡＴＡ）−１ＡＴｙ  ・・・・・（９）
上式（９）は同時に、一般的には正方行列ではないＡの逆行列（一般化逆行列）Ａ＋が下式（１０）で与えられることを示している。
【００９６】
Ａ＋＝（ＡＴＡ）−１ＡＴ  ・・・・・（１０）
つまり、行列とベクトルという違いはあるが、Δｆにより決定される行列をｙ、Δθにより決定される行列をＡ、ヤコビアンにより決定される行列をｘとすれば、図７（Ｂ）の式は上式（６）の形式であると考えることができ、ヤコビアンは正規化方程式の解である最小自乗解として求めることが可能である。
【００９７】
ここで上式（９）、（１０）は、行列Ａの一般化逆行列Ａ＋を求めることができれば、ｘ＊＝Ａ＋ｙにより最小自乗解を求めることができるということを示しており、一般化逆行列は上式（１０）の形式に限定されるものではない。
【００９８】
例えば、任意のｍ×ｎ行列Ａは、下式（１１）の形式に特異値分解を行うことができる
Ａ＝ＵΣＶＴ  ・・・・・（１１）
ここで、Ｕはｍ次元の正規直交基底であり、Ｖはｎ次元の正規直交基底であり、Σは特異値を対角項に持つ行列となる。なお、Ｕは正方行列ＡＡＴのｍ個の固有ベクトルを列ベクトルとして有する行列、Ｖは正方行列ＡＴＡのｎ個の固有ベクトルを列ベクトルとして有する行列であり、ΣはＵとＶで共有される固有値の平方根を対角項に持つ行列であると言い換えることができる。
【００９９】
このことから、上式（１１）のＡの逆行列と同様の性質を持つ行列として下式（１２）のＡ＋を考えることが可能となる。
【０１００】
Ａ＋＝ＶΣ−１ＵＴ  ・・・・・（１２）
つまり、上式（１１）のＡとしてΔθにより決定される行列（以下行列Ｂと記載する）の特異値分解を行い、上式（１２）に従って行列Ｂの一般化逆行列Ｂ＋を求めてもよい。この場合にも、ヤコビアンを表す最小自乗解は、ｘ＊＝Ａ＋ｙ（具体的には［Ｊ］Ｔ＝Ｂ＋［Δｆ］）により求めることが可能である。
【０１０１】
しかし上式（１０）により一般化逆行列を求める手法には安定的に逆ヤコビアンを求めることができないという問題が生じうる。上式（１０）では、ＡＴＡの逆行列（ＡＴＡ）−１を求める必要がある。ここで、ＡＴＡは正方行列ではあるものの、そのランク（階数）は対象とする具体的な行列の値（ここではΔθの値）に依存するものであり、場合によってはフルランクではない。その場合、逆行列（ＡＴＡ）−１を適切に求めることができないため、一般化逆行列も適切なものとならない。ロボットの制御においては、不適切なパラメーターによる制御は衝突等の深刻な問題を生じうるものであるから、上式（１０）を一般化逆行列の算出に用いることはできない。
【０１０２】
また、上式（１２）により一般化逆行列を求める手法では、通常の逆行列処理を行わないため、安定的に（ロバストに）一般化逆行列を求めることができるものの、計算量が大きく高速処理が難しいという問題が生じうる。例えば、行列Ｂの行の数（上記の例ではｍ）は対応付けされた画像特徴量情報と関節角情報の組の数に対応し、列の数（上記の例ではｎ）はロボットの自由度（例えば関節数）に対応する。この際、ヤコビアンの推定精度を高めるためにｍの値をｎに比べて大きいものとすると、上記Ｕとして巨大な行列を求める必要があり、計算量も膨大なものになってしまうという問題がある。
【０１０３】
そこで本実施形態では、下式（１３）により行列Ａの一般化逆行列Ａ＋を求めるものとする。下式（１３）で、diag［１／λ］とは下式（１４）に示すように固有値λの逆数を対角値に持つ行列を表す。ただし、λ＝０の場合にはその逆数も０であるものとする。
【０１０４】
Ａ＋＝Ｖdiag［１／λ］ＶＴＡＴ  ・・・・・（１３）
【０１０５】
【数３】
2014140941.tif 000005
上式（１３）を用いれば、上式（１０）に示した例とは異なり、通常の逆行列処理を行う必要がない。よって、行列Ｂのランクによらず一般化逆行列Ｂ＋を安定的に求めることが可能である。また、上式（１２）に示した例とは異なり、正規直交基底Ｕを求める必要がない。つまりｍの値が大きくなったとしても、ｍ次元の行列を計算する必要がなくなるため高速に一般化逆行列Ｂ＋を求めることが可能である。
【０１０６】
なお、用いるロボット等の設定によっては、ｍ及びｎの値は異なるものになる。よって、上述の例とは異なりｍに比べてｎが非常に大きくなるケースにおいては、下式（１５）を用いて一般化逆行列を求めてもよい。この場合にも、安定的に一般化逆行列Ｂ＋を求めることが可能であり、且つ非常に大きいｎ次元の行列を扱う必要がないため高速で処理を行うことが可能である。
【０１０７】
Ａ＋＝ＡＴＵdiag［１／λ］ＵＴ  ・・・・・（１５）
つまりヤコビアン推定部２２０は、関節角ベクトル列記憶部３１及び特徴量ベクトル列記憶部３３から、対応関係設定部２１０により対応づけられた関節角ベクトルと特徴量ベクトルをそれぞれ８つ以上読み出し、その差分をとることでΔｆ０〜Δｆ５及びΔθ０〜Δθ６をそれぞれ７組以上取得する。そして７組以上のΔθ０〜Δθ６（図７（Ｂ）の例ではθ００〜θ６６）から決定される行列Ｂの一般化逆行列Ｂ＋を求める。
【０１０８】
ヤコビアン推定部２２０は、一般化逆行列取得部を含んでもよく、一般化逆行列取得部は、図８に示したように、分散行列算出部２２１と、固有値算出部２２２と、逆数算出部２２３と、固有ベクトル行列算出部２２４と、転置行列算出部２２５−１，２２５−２と、行列・ベクトル積算出部２２６と、行列積算出部２２７−１，２２７−２を含む。
【０１０９】
分散行列算出部２２１は、行列Ｂを取得し、分散行列を求める。ここでの分散行列とは、上式（１３）を用いる場合であればＢＴＢであり、上式（１５）を用いる場合であればＢＢＴである。算出した分散行列は、固有値算出部２２２と、固有ベクトル行列算出部２２４に出力される。
【０１１０】
固有値算出部２２２は、分散行列の固有値λを算出する。逆数算出部２２３は、固有値λの逆数１／λを算出する。ただし、λ＝０の場合には、当該固有値の逆数も０であるものとする。
【０１１１】
固有ベクトル行列算出部２２４は、分散行列の固有ベクトルを算出し、固有ベクトルからなる固有ベクトル行列を算出する。固有ベクトル行列とは、上式（１３）のＶ又は上式（１５）のＵに相当する。
【０１１２】
行列・ベクトル積算出部２２６は、固有ベクトル行列算出部２２４からの出力であるＶ又はＵと、逆数算出部２２３の出力であるdiag［１／λ］の積を求める。具体的には、上式（１３）のうちのＶdiag［１／λ］、又は上式（１５）のうちのＵdiag［１／λ］を求める。
【０１１３】
転置行列算出部２２５−１は、固有ベクトル行列算出部２２４の出力の転置行列を求める。具体的にはＶＴ又はＵＴを求める。行列積算出部２２７−１は、行列・ベクトル積算出部２２６の出力と、転置行列算出部２２５−１の出力の積を求める。具体的には、上式（１３）のうちのＶdiag［１／λ］ＶＴ、又は上式（１５）のうちのＵdiag［１／λ］ＵＴを求める。
【０１１４】
また、転置行列算出部２２５−２は、行列Ｂの転置行列ＢＴを求める。行列積算出部２２７−２は、行列積算出部２２７−１の出力と、転置行列算出部２２５−２の出力の積を求める。具体的には、Ｖdiag［１／λ］ＶＴＢＴ又はＢＴＵdiag［１／λ］ＵＴを求めることになり、行列積算出部２２７−２の出力が、行列Ｂの一般化逆行列Ｂ＋となる。
【０１１５】
Δθにより決定される行列Ｂの一般化逆行列Ｂ＋を求めることができたら、求めた一般化逆行列Ｂ＋をΔｆにより決定される行列に左からかけることで、ヤコビアンを求めることが可能である。
【０１１６】
２．４  最適な対応関係の選択処理
上述の手法により、対応関係設定部２１０で設定された複数の対応関係（例えば５０×３７通りの対応関係）のそれぞれについてヤコビアンが推定される。次にその複数のヤコビアンの中から最適なヤコビアン、及びそれに対応する最適な対応関係（選択対応関係）を選択する。
【０１１７】
具体的には各ヤコビアンについて評価値（誤差情報）を求め、当該評価値の比較処理を行うことで最適なヤコビアン等を選択する。評価値算出処理の一例を図９に示す。図９に示すように、特徴量情報のうち最新のもの（図９のＡ１）については評価用情報として用い、対応関係の設定やヤコビアンの推定は行わない。そして、それ以前の特徴量情報に対して、関節角情報との対応付けを行い（ここでは対応づけられた画像特徴量情報と関節角情報の組を９組設定することで、ΔｆとΔθの組を８組設定している）、上述の手法によりヤコビアンを推定する。
【０１１８】
ヤコビアンが推定されたら、当該ヤコビアンを用いて１つ前のタイミングのデータから最新の画像特徴量を推定する。具体的には１つ前の関節角情報の差分値に対して、上式（１）に示したようにその際のヤコビアンを作用させれば、最新の画像サンプリングタイミングでの画像特徴量情報を推定することができる（厳密には最新の画像特徴量と１タイミング前の画像特徴量の差分がまず求まり、そこから最新の画像特徴量を求めることになる）。さらに具体的には、画像サンプリングタイミング（Ａ２）に対応する関節角サンプリングタイミング（対応関係設定部２１０での設定によるが、例えばＢ２）での関節角情報と、その１つ前の関節角情報（Ａ３に対応するＢ３）との差分値にヤコビアンを作用させればよい。ここで、推定したヤコビアンが適切であるほど、当該ヤコビアンを用いて推定した最新タイミングでの画像特徴量情報（推定値）は、実測値である画像特徴量情報に近くなるはずである。よって本実施形態では、例えば推定した画像特徴量情報と、実測した画像特徴量情報との誤差（各成分の差分の自乗和等）を評価値とすればよい。
【０１１９】
この評価値算出処理を推定した複数のヤコビアンについて行い、評価値が最適なもの（ここでは誤差情報であるため値が最小のもの）を最適なヤコビアンとして選択し、その際の対応関係を選択対応関係とする。
【０１２０】
なお、ヤコビアンの評価値は上述のものには限定されない。上述の手法により求めたヤコビアンは、上式（７）の自乗誤差を最小にする最小自乗解である。よって、求められたヤコビアンを上式（７）のｘに代入して値を計算すれば、当該ヤコビアンを用いた場合の自乗誤差の値を求めることができる。ヤコビアンが適切であるほど自乗誤差の値は小さいと考えられるため、上式（７）の自乗誤差の値をヤコビアンの評価値としてもよい。この場合も、評価値が最小のものを最適なヤコビアンとして選択し、その際の対応関係を選択対応関係とする。
【０１２１】
ただし、図７（Ａ）、図７（Ｂ）に示したように、画像特徴量情報と関節角情報の組を７組（広義にはロボットの自由度に対応する数の組）だけ用いた場合には、未知数の数と、互いに１次独立な方程式の数が一致することになり、図７（Ｂ）の式で表される連立方程式をそのまま解くことが可能になる。この場合、上式（６）の等号が成立するのであるから、上式（７）の自乗誤差は必ず０となる。つまり、条件を過不足なく設定した場合には、推定される複数のヤコビアンの上式（７）による自乗誤差は全て０になってしまい、その妥当性について比較することができない。そのため、この場合の評価値としては上式（７）の自乗誤差ではなく、図９に示した手法により求めた値を用いるとよい。一方、方程式の数が過剰な場合には、一般的には上式（６）の等号は成立せず、上式（７）の自乗誤差の値として０以外の有意な値を取得することができる。その場合の評価値は、図９の手法により求めてもよいし、上式（７）の自乗誤差の値を用いてもよいということになる。
【０１２２】
２．５  処理の詳細
本実施形態の処理の流れを図１０のフローチャートを用いて説明する。この処理が開始されると、まず最小評価値、及び最小評価値に対応するヤコビアンである推定ヤコビアンを初期化する（ステップＳ１０１，ステップＳ１０２）。そして、対応関係設定部２１０で設定される全対応関係（例えば５０×３７通りの対応関係）を検討範囲として、当該検討範囲の全てを検討したかを判定する（ステップＳ１０３）。
【０１２３】
検討していない対応関係がある場合（ステップＳ１０３でＮｏの場合）には、未検討の対応関係を１つ選択し（ステップＳ１０４）、選択した対応関係での画像特徴量情報と関節角情報とを用いて上述した手法によりヤコビアンを推定する（ステップＳ１０５）。そして推定したヤコビアンを用いて評価値を算出する（ステップＳ１０６）。具体的には、図９の手法、或いは上式（７）により自乗誤差を求めればよい。
【０１２４】
その後、求められた評価値とその時点で保持されている最小評価値との比較処理を行う（ステップＳ１０７）。ここで、求められた評価値が最小評価値よりも小さい場合（ステップＳ１０７でＹｅｓの場合）には、最小評価値をステップＳ１０６求めた評価値で更新し（ステップＳ１０８）、推定ヤコビアンをステップＳ１０５で推定したヤコビアンで更新する（ステップＳ１０９）。一方、求められた評価値が最小評価値以上の場合（ステップＳ１０７でＮｏの場合）には、最小評価値、推定ヤコビアンの更新を行わずにステップＳ１０３に戻る。
【０１２５】
ステップＳ１０４〜ステップＳ１０９の処理を繰り返し、全ての対応関係について検討を行った場合には、ステップＳ１０３の判定でＹｅｓとなり、最小評価値か初期値から更新されているかを判定する（ステップＳ１１０）。最小評価値が更新されていない場合には、適切な対応関係を選択できなかったものとして処理を終了する。一方、最小評価値が更新されている場合には、ステップＳ１１０時点での最小評価値を与える推定ヤコビアンが最適なヤコビアンであるものとして出力するとともに、当該ヤコビアンに対応する対応関係を選択対応関係とする（ステップＳ１１１）。
【０１２６】
以上の本実施形態では、ロボット制御システムは図５に示したように、複数の画像サンプリングタイミングにおける画像特徴量情報を取得する画像特徴量情報取得部（特徴量ベクトル取得部）２０と、複数の関節角サンプリングタイミングにおけるロボットの関節角情報を取得する関節角情報取得部（関節角ベクトル取得部）１０と、画像サンプリングタイミングと関節角サンプリングタイミングの間の複数の対応関係の中から選択された選択対応関係での画像特徴量情報と関節角情報に基づいて、ロボット（例えば図６（Ａ）のロボット本体３１０）を制御するロボット制御部２００を含む。なお、複数の関節角サンプリングタイミングは、画像サンプリングタイミングと非同期のサンプリングタイミングであることが想定される。
【０１２７】
ここで、画像特徴量情報とは画像特徴量を表す情報であり、例えば画像特徴量を表す値を要素として持つ特徴量ベクトルであってもよい。この場合、特徴量ベクトルの要素は画像データから取得される種々の値が考えられ、エッジを表す情報、空間周波数を表す情報、画素値そのもの等であってもよい。また、関節角情報とはロボットの関節角を表す情報であり、各関節の角度を表す値を要素として持つ関節角ベクトルであってもよい。関節角ベクトルとは、狭義にはロボットの自由度に相当する数の値を、その要素として持つことになる。
【０１２８】
これにより、関節角サンプリングタイミングと画像サンプリングタイミングの間（つまりは関節角情報と画像特徴量情報との間）の適切な対応関係を選択し、データ間の同期を取ることが可能になる。高性能なカメラを用いる等の条件が満たされない場合、図３（Ｂ）に示した原点ずれや図３（Ｃ）に示した揺らぎ等に起因する非同期が発生する可能性が高いため、なんらその点を考慮せずにデータ間の同期を仮定してしまうと、適切なロボット制御は困難である。そこで本実施形態では、データ間の適切な対応関係を選択した上で、選択された対応関係での画像特徴量情報及び関節角情報を用いてロボット制御を行うものとする。
【０１２９】
また、ロボット制御部２００は、選択対応関係での画像特徴量情報と関節角情報に基づいて、選択対応関係でのヤコビアンを推定し、推定したヤコビアンに基づいてロボットを制御してもよい。
【０１３０】
これにより、ヤコビアンを用いたロボット制御を行うことが可能になる。上述したように、ヤコビアンは所与の空間での微少変位（差分）を他の空間での微少変位（差分）として写像するものであるから、画像特徴量情報を２つ以上用いて画像特徴量情報の差分を求めるとともに、関節角情報を２つ以上用いて関節角情報の差分を求める必要がある。よってここでは、画像特徴量情報と関節角情報を少なくとも２組以上対応づける対応関係を選択することが想定される。
【０１３１】
また、ロボット制御部２００は、画像サンプリングタイミングと関節角サンプリングタイミングの間の複数の対応関係の各対応関係においてヤコビアンを推定し、推定したヤコビアンの評価値を求め、評価値の比較処理に基づいて、複数の対応関係の中から選択対応関係を選択してもよい。
【０１３２】
これにより、各対応関係においてヤコビアンを推定し、推定したヤコビアンの妥当性を判定することで、最適な対応関係（上記選択対応関係）を選択することが可能になる。ヤコビアンを用いたロボット制御では、ロボット制御信号（具体的には関節角をどれだけ変更すればよいかを指示する情報）がヤコビアンにより決定されることから、ヤコビアンが正確か否かはロボット制御の精度を決定する重要な指標となる。よってここでは、複数の対応関係を設定した場合に、それらについての（狭義にはその全てについての）ヤコビアンを算出し、当該ヤコビアンの評価値を用いて選択対応関係を決定するものとする。
【０１３３】
また、ロボット制御部２００は、画像サンプリングタイミングと関節角サンプリングタイミングの間の複数の対応関係の各対応関係において正規化方程式を作成し、作成した正規化方程式の解である最小自乗解を求めることでヤコビアンを推定してもよい。
【０１３４】
これにより、正規化方程式を解くことでヤコビアンを推定することが可能になる。ヤコビアンはその定義より上式（１）が成り立ち、その表現形式を変えることで上式（４）等が成り立つ。この場合、上式（６）の最小自乗解が正規化方程式の解として与えられることが知られていることから、本実施形態においても正規化方程式を解くことで、最小自乗解としてヤコビアンを推定する。なお、上式（４）は上式（１）の両辺の転置行列を求めたものであるが、他の形式により上式（１）を変形し、上式（６）に相当する連立方程式を求めてもよい。
【０１３５】
また、ロボット制御部２００は、推定したヤコビアンを用いて、ヤコビアンを推定したタイミングの次のタイミングである比較タイミングにおける画像特徴量情報を算出し、算出した画像特徴量情報と、画像特徴量情報取得部２０で取得された比較タイミングでの画像特徴量情報とに基づいて、評価値を求めてもよい。
【０１３６】
ここで、比較タイミングとはヤコビアンの推定タイミングの次のタイミングを表し、例えば図９のＡ１に相当する。推定したヤコビアンを、当該ヤコビアンに対応するタイミングでの関節角情報の差分値（Δθ）に作用させることで、当該タイミングと次のタイミングの間での画像特徴量情報の差分値（Δｆ）を推定することができる。
【０１３７】
これにより、図９に示した手法により推定したヤコビアンの評価値を求めることが可能になる。Ａ１で取得された画像特徴量情報はいわば実測値であり、Ｂ２とＢ３の関節角情報の差分に対してヤコビアンを作用させて得られた画像特徴量情報はいわば推定値である。ヤコビアンの精度が高いほど、推定値は実測値に近くなると考えられるため、実測値と推定値の差に応じて評価値を決定すればよい。例えば、値が小さいほど評価が高くなるような評価値を用いるのであれば、ヤコビアンの評価値は実測値と推定値の差を表す値（例えば画像特徴量情報（特徴量ベクトル）の各要素の差の自乗和等を評価値とすればよい。この場合、未知数の数に対して条件式が過不足ない場合（図７（Ｂ）に示した場合等）であっても、適切に評価値を求めることができる。
【０１３８】
また、ロボット制御部２００は、推定したヤコビアンを用いて、正規化方程式の自乗誤差を評価値として求めてもよい。
【０１３９】
これにより、上式（７）により表される自乗誤差をヤコビアンの評価値とすることが可能になる。複数のヤコビアンは全て最小自乗解として求められるものであるが、当該最小自乗解を上式（７）に代入した場合の自乗誤差の値についてはヤコビアンに応じて異なるものである。この場合、自乗誤差の値が小さいほど当該ヤコビアンの推定精度は高いと考えることができるため、上式（７）の自乗誤差の値をヤコビアンの評価値とすることができる。なおこの場合にも、評価値の値が小さいほど、ヤコビアンの妥当性が高いということになる。
【０１４０】
また、ロボット制御部２００は、画像特徴量情報の差分により表されるベクトル又は行列をＦとし、関節角情報の差分により表される行列をＢとし、ヤコビアンを表すベクトル又は行列をＪとして、Ｆ＝ＢＪの最小自乗解を与える正規化方程式を作成し、行列Ｂに対して、ＢＴＢの固有ベクトルを列ベクトルとする行列をＶとし、ＢＢＴの固有ベクトルを前記列ベクトルとする行列をＵとし、ＢＴＢの固有値λの逆数を対角要素とする行列をdiag［１／λ］とした場合に、Ｂ＋＝Ｖdiag［１／λ］ＶＴＢＴ、又はＢ＋＝ＢＴＵdiag［１／λ］ＵＴにより、行列Ｂの一般化逆行列Ｂ＋を求め、求めた一般化逆行列Ｂ＋から、Ｊ＝Ｂ＋Ｆに従って行列Ｊを求めることでヤコビアンを推定してもよい。
【０１４１】
これにより、一般化逆行列Ｂ＋を求める際に、上式（１０）により求める場合と異なり一般的な逆行列演算を行う必要がなくなり、行列Ｂのランクによらず安定して（ロバストに）処理を行うことが可能になる。また、上式（１２）の特異値分解を行う手法と比較した場合、上式（１２）では正規直交基底Ｕ，Ｖの両方を演算に用いる必要があるが、本実施形態の手法であればどちらか一方で逆ヤコビアンの演算が可能である。画像特徴量情報と関節角情報の組を何組用いるか等に応じてＵ，Ｖの大きさが決定されるが、本実施形態の手法によればそれらの状況に合わせて演算量の少ない正規直交基底を適宜選択することができるため、上式（１２）に比べて高速に一般化逆行列Ｂ＋を求めることが可能になる。
【０１４２】
また、画像特徴量情報取得部２０における画像サンプリングタイミングのサンプリング周波数をｆＡ、関節角情報取得部１０における関節角サンプリングタイミングのサンプリング周波数をｆＢとした場合に、関節角情報取得部１０は、ｆＢ≧ｆＡを満たすサンプリング周波数で関節角情報を取得してもよい。
【０１４３】
これにより、関節角情報が画像特徴量情報に比べて高い頻度で取得される場合に本実施形態の処理を行うことが可能になる。この場合、数の少ない画像サンプリングタイミングを基準として、当該画像サンプリングタイミングと対応関係を持ちうる複数の関節角サンプリングタイミングを探索することで最適な対応関係を選択することで効率的に処理を行うことが可能となる。揺らぎの範囲が狭い場合には、１つの画像特徴量情報に対応する関節角情報の数が少ない（上述した図３（Ｃ）の例であれば１つの画像特徴量情報に対しては、３つの関節角情報を考慮すればよい）ため、処理対象とする必要のない関節角サンプリングタイミングについては処理をスキップすることが可能となるためである。
【０１４４】
また、以上の本実施形態は上記のロボット制御システムを含むロボットに適用できる。
【０１４５】
ここでのロボット（ロボットシステム）は、図６（Ａ）に示したようにロボット本体３１０と、制御装置３００（上記のロボット制御システムに相当）により構成されてもよい。なお、ロボット本体３１０と制御装置３００は、図６（Ｂ）に示したように一体として構成されてもよい。また、本実施形態におけるロボット制御とは、狭義にはアームの制御であってもよい。
【０１４６】
なお、本実施形態のロボット制御システム等は、その処理の一部または大部分をプログラムにより実現してもよい。この場合には、ＣＰＵ等のプロセッサーがプログラムを実行することで、本実施形態のロボット制御システム等が実現される。具体的には、非一時的な情報記憶媒体に記憶されたプログラムが読み出され、読み出されたプログラムをＣＰＵ等のプロセッサーが実行する。ここで、情報記憶媒体（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＤＶＤ、ＣＤ等）、ＨＤＤ（ハードディスクドライブ）、或いはメモリー（カード型メモリー、ＲＯＭ等）などにより実現できる。そして、ＣＰＵ等のプロセッサーは、情報記憶媒体に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち、情報記憶媒体には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。
【０１４７】
３．第２の実施形態
本実施形態では解析的に求めたロボットヤコビアンに基づいて正規化方程式に重み付けを行い、ヤコビアンの推定精度を向上させる実施例について説明する。
【０１４８】
３．１  ロボットヤコビアン
第１の実施形態で説明したように、ヤコビアンの推定には画像特徴量情報と関節角情報の組を複数対応づける必要がある。これは言い換えれば、所与のタイミングでのヤコビアンを求める際に、ある程度の時間（例えば７タイミング前までのデータを用い、画像サンプリングタイミングが１０ミリ秒周期であれば７０ミリ秒）のデータを用いる必要があるということになる。この間、ヤコビアンが一定であれば、推定されるヤコビアンの精度に問題はないが、上記時間の中でもロボット姿勢の変動等によりヤコビアンは変化しうる。一定時間におけるデータを用い１つのヤコビアンを求めようとするのに、その間でヤコビアンが変動しうるのであれば、推定されたヤコビアンの精度に問題が生じうる。しかし、第１の実施形態ではこの点について特に考慮されてはいない。そこで第２の実施形態ではロボットヤコビアンを用いて精度向上を図るものとする。
【０１４９】
まずロボットヤコビアンについて説明する。ヤコビアンとは、多変数の非線形ベクトル関数を所与の点の近傍で展開したときの１次の係数を表すものである。つまりヤコビアンとは、ある座標系での微小変位を、非線形ベクトル関数で写像される座標系にどのように写像されるか線形近似を与えるものということもできる。
【０１５０】
よって、非線形ベクトル関数（及びそれにより写像される座標系）は種々考えられ、ヤコビアンとしては関節角空間での微少変位を手先位置空間へ写像するロボットヤコビアン（図１（Ａ））を考えることもできるし、関節角空間での微少変位を画像特徴量空間へ写像する画像特徴量ヤコビアン（図１（Ｂ））を考えることもできる。なお、特徴量情報として画像特徴量情報を用いている以上、第１の実施形態の手法により求めているヤコビアンは画像特徴量ヤコビアンである。
【０１５１】
ここで、ビジュアルサーボ等のように画像特徴量情報に基づいてロボットの手先姿勢を制御することができるのは、画像特徴量情報と手先姿勢の間での変換処理が可能である（言い換えれば一方を他方に写像する関数を定義することができる）ということである。つまり、関節角空間での微少変位を画像特徴量空間に写像することを考えた場合、直接写像する（画像特徴量ヤコビアンＪθ―ｆによる変換）ものと、関節角空間での微少変位を手先姿勢空間に写像した上で（ロボットヤコビアンＪθ―ｐによる変換）、手先姿勢空間での微少変位を画像特徴量空間に写像する（便宜的にヤコビアンＪｐ−ｆと表記する）ものとは同等のものと考えることができ、下式（１６）が成り立つ。つまり、画像特徴量ヤコビアンは、ロボットヤコビアンを用いて図１１に示したように分解することが可能である。
【０１５２】
Ｊθ―ｆ＝Ｊθ―ｐＪｐ−ｆ  ・・・・・（１６）
ここで、ビジュアルサーボ等においては、カメラの位置は通常固定されるものであり、ロボットの手先姿勢が同一であれば画像特徴量情報も同一のものとできることが一般的である。つまり、ヤコビアン推定に用いる情報の取得期間（上記例では７０ミリ秒）で画像ヤコビアンＪθ―ｆが変化する場合には、ロボットヤコビアンＪθ―ｐの変化が支配的であると考えられる。
【０１５３】
そして、ロボットヤコビアンＪθ―ｐは、ロボットが関節角に対して理論通りのリンク径として応答をする場合には、解析に求めることが可能であることが知られている。つまり本実施形態では、解析的に求めたロボットヤコビアンに基づいて、画像ヤコビアンＪθ―ｆの変動度合いを推定し、推定結果に応じて重み付けをすることで画像ヤコビアンＪθ―ｆの推定精度を向上させる。なお、ロボットヤコビアンを解析的に求めるための手法としては種々のものが知られているため、詳細な説明は省略する。実際のロボットは理想的な動作を行うものではなく、解析的に求められるロボットヤコビアンには誤差が生じうるが、本実施形態でのロボットヤコビアンは重み係数の設定を行うためのものであり、ロボットヤコビアンに誤差が生じたとしても特に問題とはならない。
【０１５４】
３．２  重み付けした正規化方程式
本実施形態では、ロボットヤコビアンを用いて重み係数を算出し、当該重み係数を用いて正規化方程式を作成するものとする。具体的には、対応関係設定部２１０により対応関係が設定された場合には、関節角情報の差分値を用いて、各差分ベクトル（関節角ベクトルの差分と、画像特徴量ベクトルの差分）に対応するロボットヤコビアンを解析的に求める。
【０１５５】
そして所与の差分ベクトルに対応するロボットヤコビアンを基準ロボットヤコビアンとする。各差分ベクトルに対する重み付けは、当該差分ベクトルに対応するロボットヤコビアンと、基準ロボットヤコビアンとの差異により設定すればよい。差異が大きいほど、ロボットヤコビアンの変動が大きいということであるから、対応する差分ベクトルについての重みは小さくなる（処理結果への当該差分ベクトルの寄与が小さくなる）ように設定する。
【０１５６】
具体的には、図７（Ａ）の（１）のロボットヤコビアンを基準とした場合に、（１）〜（７）のそれぞれについて下式（１７）により相違度を求め、下式（１８）により正規化のための値を求める。そして重み係数としては下式（１９）等の値を用いればよい。なお、（１）同士を比較する場合には、相違度は０となるため重み係数は１となる。また、ロボットヤコビアン同士の相違度が大きいほど、重み係数は０に近づく。
【０１５７】
Err（Ｊｉ，Ｊｊ）＝Σｋ（abs（Ｊｉ（ｋ）−Ｊｊ（ｋ）））  ・・・・・（１７）
N=sqrt（Σｊ（Err（Ｊ０，Ｊｊ））２）  ・・・・・（１８）
ｗｉ＝１．０−abs（Err（Ｊ０，Ｊｉ））／Ｎ  ・・・・・（１９）
このようにすれば、図７（Ｂ）で示した連立方程式の代わりに、図１２に示した式を用いることが可能になる。図１２の式が得られた後は、第１の実施形態と同様に当該式の最小自乗解を、正規化方程式の解として求めればよい。具体的には、重み付けがされた後のΔθにより決定される行列Ｂに対して、一般化逆行列を求める処理を行えばよい。
【０１５８】
３．３  処理の詳細
本実施形態の処理の流れを図１３のフローチャートを用いて説明する。図１３から明らかなように、ステップＳ２０１〜ステップＳ２０４は、図１０のステップＳ１０１〜ステップＳ１０４と同様であり、ステップＳ２０８〜ステップＳ２１３は、図１０のステップＳ１０６〜Ｓ１１１と同様であるため詳細な説明は省略する。
【０１５９】
本実施形態では、ステップＳ２０４で１つの対応関係が選択された後に、その際の情報を用いて解析的にロボットヤコビアンを計算する（ステップＳ２０５）。そして上式（１７）〜（１９）等の手法により、ロボットヤコビアンに基づいて重み係数を算出する（ステップＳ２０６）。そして図１２に示したように重み係数を用いて正規化方程式を作成し、当該正規化方程式の解である最小自乗解としてヤコビアンを推定する（ステップＳ２０７）。
【０１６０】
ヤコビアンが推定された後の処理は第１の実施形態と同様である。
【０１６１】
以上の本実施形態では、ロボット制御部２００は、ロボットのエンドポイント位置と関節角情報とを対応づけるロボットヤコビアンを求め、求めたロボットヤコビアンに基づいて正規化方程式を作成してもよい。また、ロボット制御部２００は、ロボットヤコビアンを解析的に求め、求めた前記ロボットヤコビアンに基づいて前記正規化方程式を作成してもよい。
【０１６２】
これにより、（狭義には解析的に）求めたロボットヤコビアンに基づいて正規化方程式を作成し、当該正規化方程式に基づいて画像特徴量ヤコビアンを推定することが可能になる。上述したように、画像特徴量ヤコビアンの推定に用いるデータはある程度の期間にわたって取得されたものを用いる必要があり、当該期間内で画像特徴量ヤコビアンが変化している場合には、その変化したタイミングに対応する差分値（画像特徴量情報及び関節角情報の差分値）は画像特徴量ヤコビアンの推定において精度低下等の悪影響を及ぼす可能性がある。上述したように、画像特徴量ヤコビアンを、ロボットヤコビアンを含む複数の項の積として表現した場合、画像特徴量ヤコビアンの変動度合いは主としてロボットヤコビアンの変動度合いにより表される。つまり、ロボットヤコビアンの変動が大きいほど、その際の差分値の画像特徴量ヤコビアンの推定処理での寄与を小さくすることで、より精度の高い処理が可能になる。
【０１６３】
また、ロボット制御部２００は、基準タイミング（例えば上式（１９）でのタイミング０）でのロボットヤコビアンと、所与のタイミング（上式（１９）でのタイミングｉであり、狭義にはｉ＝０〜７）でのロボットヤコビアンとの比較処理に基づいて、当該所与のタイミングでの画像特徴量情報の差分値（Δｆ）と関節角情報の差分値（Δθ）の組についての重み付けを設定し、設定された重み付けを用いて正規化方程式を作成してもよい。
【０１６４】
これにより、重み付けを用いて正規化方程式を作成することで、画像特徴量ヤコビアンの推定精度を向上させることが可能になる。重み付けの例としては、上式（１７）〜（１９）により求められた重み係数を用いてもよく、その場合図１２に示した式の最小自乗解を解くことでヤコビアンを求めることになる。なお、ロボットヤコビアンの変動が大きいほど重み係数の値が小さくなればよく、その具体的な算出手法は上式（１７）〜（１９）に限定されるものではない。
【符号の説明】
【０１６５】
１０  関節角ベクトル取得部、２０  特徴量ベクトル取得部、３０  記憶部、
３１  関節角ベクトル列記憶部、３３  特徴量ベクトル列記憶部、
３５  目標特徴量記憶部、２００  ロボット制御部、２１０  対応関係設定部、
２２０  ヤコビアン推定部、２２１  分散行列算出部、２２２  固有値算出部、
２２３  逆数算出部、２２４  固有ベクトル行列算出部、
２２５−１，２２５−２  転置行列算出部、２２６  行列・ベクトル積算出部、
２２７−１，２２７−２  行列積算出部、２３０  評価処理部、
２４０  逆ヤコビアン算出部、２５０  目標差分ベクトル算出部、
２６０  行列・ベクトル積算出部、２７０  制御データ出力部、３００  制御装置、
３１０  ロボット本体、３２０  アーム
【図１】
2014140941.tif 000006
【図２】
2014140941.tif 000007
【図３】
2014140941.tif 000008
【図４】
2014140941.tif 000009
【図５】
2014140941.tif 000010
【図６】
2014140941.tif 000011
【図７】
2014140941.tif 000012
【図８】
2014140941.tif 000013
【図９】
2014140941.tif 000014
【図１０】
2014140941.tif 000015
【図１１】
2014140941.tif 000016
【図１２】
2014140941.tif 000017
【図１３】
2014140941.tif 000018