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