(57)【要約】【課題】ロボットの逆運動学の解を、反復演算手法により、より効率的に短時間で求めることができるロボット制御システム等の提供。【解決手段】ロボット制御システムは、ロボット１００の目標エンドポイント位置を取得するエンドポイント位置取得部２０と、ロボット１００の真正モデルの情報を記憶する真正モデル記憶部２２と、真正モデルを近似した近似モデルの情報を記憶する近似モデル記憶部２４と、近似モデルと目標エンドポイント位置を用いて、近似解を求め、近似解と真正モデルを用いて、反復法による反復演算処理を行うことで、ロボット１００の逆運動学における解を求める処理部３０を含む。【選択図】図５
2013132731.tif 000002
【特許請求の範囲】
【請求項１】
ロボットの目標エンドポイント位置を取得するエンドポイント位置取得部と、
前記ロボットの真正モデルの情報を記憶する真正モデル記憶部と、
前記真正モデルを近似した近似モデルの情報を記憶する近似モデル記憶部と、
前記近似モデルと前記目標エンドポイント位置を用いて、近似解を求め、前記近似解と前記真正モデルを用いて、反復法による反復演算処理を行うことで、前記ロボットの逆運動学における解を求める処理部と、
を含むことを特徴とするロボット制御システム。
【請求項２】
請求項１において、
前記処理部は、
前記近似解を初期値として設定して、前記反復演算処理を行うことを特徴とするロボット制御システム。
【請求項３】
請求項１において、
前記処理部は、
前記近似解を前記真正モデルの関節角に設定することで、近似解によるエンドポイント位置を求め、
前記近似解によるエンドポイント位置と前記目標エンドポイント位置との間の誤差に基づいて、前記目標エンドポイント位置を修正することで、修正目標エンドポイント位置を求め、
前記修正目標エンドポイント位置と前記近似モデルを用いて、第２の近似解を求め、
前記第２の近似解と前記真正モデルを用いて、前記反復演算処理を行うことで、前記ロボットの逆運動学における解を求めることを特徴とするロボット制御システム。
【請求項４】
請求項１において、
前記近似モデル記憶部は、
前記真正モデルを近似したモデルとして、第１〜第mの近似モデルの情報を記憶し、
前記処理部は、
前記第１〜第mの近似モデルと前記目標エンドポイント位置を用いて、第１〜第mの近似解を求め、
前記第１〜第mの近似解の中から選択された近似解を初期値として設定して、前記反復演算処理を行うことで、前記ロボットの逆運動学における解を求めることを特徴とするロボット制御システム。
【請求項５】
請求項４において、
前記処理部は、
前記第１〜第mの近似モデルのうちの第ｉ（１≦ｉ≦m）の近似モデルと前記目標エンドポイント位置を用いて、第ｉの近似解を求め、
前記第ｉの近似解を前記真正モデルの関節角に設定することで、近似解による第ｉのエンドポイント位置とヤコビアンを求め、
前記第ｉのエンドポイント位置と前記目標エンドポイント位置との間の第１の誤差と、前記ヤコビアンを用いて、関節角を補正し、補正後の関節角を前記真正モデルの関節角に設定することで、第ｉの補正エンドポイント位置を求め、
前記第ｉの補正エンドポイント位置と前記目標エンドポイント位置の間の第２の誤差に基づいて選択された近似解を、前記初期値として設定することを特徴とするロボット制御システム。
【請求項６】
請求項１において、
前記処理部は、
前記近似解を前記真正モデルの関節角に設定することで、近似解によるエンドポイント位置を求め、
前記近似解によるエンドポイント位置と現在のエンドポイント位置の中から、選択されたエンドポイント位置に対応する関節角を、前記初期値に設定して、前記反復演算処理を行うことで、前記ロボットの逆運動学における解を求めることを特徴とするロボット制御システム。
【請求項７】
請求項６において、
前記処理部は、
前記近似解によるエンドポイント位置と前記目標エンドポイント位置との間の第１の誤差を求め、
前記現在のエンドポイント位置と前記目標エンドポイント位置との間の第２の誤差を求め、
前記第１の誤差の方が前記第２の誤差よりも小さい場合には、前記近似解によるエンドポイント位置に対応する関節角を、前記初期値に設定し、
前記第２の誤差の方が前記第１の誤差よりも小さい場合には、前記現在のエンドポイント位置に対応する関節角を、前記初期値に設定することを特徴とするロボット制御システム。
【請求項８】
請求項６において、
前記処理部は、
前記ロボットのポイント・ツー・ポイント制御が行われた場合には、前記近似解によるエンドポイント位置に対応する関節角を、前記初期値に設定し、
前記ロボットのコンティニュアス制御が行われた場合には、前記現在のエンドポイント位置に対応する関節角を、前記初期値に設定することを特徴とするロボット制御システム。
【請求項９】
請求項６乃至８のいずれかにおいて、
過去のエンドポイントの動作履歴位置である動作履歴エンドポイント位置を記憶する動作履歴記憶部を含み、
前記処理部は、
前記近似解によるエンドポイント位置と前記現在のエンドポイント位置と前記動作履歴エンドポイント位置の中から、選択されたエンドポイント位置に対応する関節角を、前記初期値に設定して、前記反復演算処理を行うことで、前記ロボットの逆運動学における解を求めることを特徴とするロボット制御システム。
【請求項１０】
請求項９において、
前記処理部は、
前記近似解によるエンドポイント位置と前記目標エンドポイント位置との間の第１の誤差を求め、
前記現在のエンドポイント位置と前記目標エンドポイント位置との間の第２の誤差を求め、
前記動作履歴エンドポイント位置と前記目標エンドポイント位置との間の第３の誤差を求め、
前記第１の誤差の方が前記第２の誤差、前記第３の誤差よりも小さい場合には、前記近似解によるエンドポイント位置に対応する関節角を、前記初期値に設定し、
前記第２の誤差の方が前記第１の誤差、前記第３の誤差よりも小さい場合には、前記現在のエンドポイント位置に対応する関節角を、前記初期値に設定し、
前記第３の誤差の方が前記第１の誤差、前記第２の誤差よりも小さい場合には、前記動作履歴エンドポイント位置に対応する関節角を、前記初期値に設定することを特徴とするロボット制御システム。
【請求項１１】
請求項１乃至１０のいずれかにおいて、
前記近似モデル記憶部は、
前記真正モデルの関節のオフセットが省略されたモデルの情報を、前記近似モデルの情報として記憶することを特徴とするロボット制御システム。
【請求項１２】
請求項１乃至１１のいずれかに記載のロボット制御システムと、
前記ロボットと、
を含むことを特徴とするロボットシステム。
【請求項１３】
ロボットの真正モデルの情報を記憶し、
前記真正モデルを近似した近似モデルの情報を記憶し、
前記ロボットの目標エンドポイント位置を取得し、
前記近似モデルと前記目標エンドポイント位置を用いて、近似解を求め、前記近似解と前記真正モデルを用いて、反復法による反復演算処理を行うことで、前記ロボットの逆運動学における解を求め、
求められた解に基づいて前記ロボットを制御することを特徴とするロボット制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御システム、ロボットシステム及びロボット制御方法等に関する。
【背景技術】
【０００２】
ロボットの各関節角から手先位置姿勢（エンドポイント位置）を求める処理である順運動学の処理は容易な処理である。一方、手先位置姿勢から各関節角を求める処理である逆運動学の処理は非常に困難な処理である。特に、ロボットのアームの構成が複雑になると、逆運動学は解析的には処理できず、逐次近似を行う反復法を用いるしかない。
【０００３】
しかしながら、反復法による逆運動学の処理には、一般的に以下のような課題がある。
【０００４】
第１に、反復法による逆運動学の処理は、全体としての処理量が大きくなり、処理に長い時間が必要となる。例えば、解析解を求める処理に比較して、数百倍から数万倍以上の処理量が必要となる。
【０００５】
第２に、反復法による逆運動学の処理では、有限回の反復で、解へ収束しない場合がある。即ち、処理時間や反復回数を制限した場合に、所望の精度の解が得られない。
【０００６】
第３に、反復法による逆運動学の処理では、条件を満足する複数個の解が存在する場合に、どの解へ収束するかの制御が困難である。例えば解析解が存在しない場合、そもそも解が幾つあるのかの推定も困難である。
【０００７】
第４に、適切な解法が、ロボットの構造に強く依存する。即ち、解析解も含め、そのロボットに最適な解法は発見的にしか決められない。
【０００８】
第５に、そもそも指定された条件での解が存在するかどうかを、事前に判断できない。即ち、単に収束しないだけなのか、解が存在しないのかを区別できない。
【０００９】
第６に、解が存在したとしても、そこに収束するかどうかは、設定した初期状態に強く依存する。即ち、悪い初期状態からスタートすれば、収束しないか、或いは非常に時間がかかってしまう。
【００１０】
これらの課題を改善・解決する従来技術としては、例えば特許文献１、２、３、非特許文献１、２に開示される技術が知られている。
【先行技術文献】
【特許文献】
【００１１】
【特許文献１】特開平６−３２０４５１号公報
【特許文献２】特開平２−２０５４８９号公報
【特許文献３】特開平４−３６９００４号公報
【非特許文献】
【００１２】
【非特許文献１】「マニピュレータ逆運動学問題の補間解法に関する研究」計測自動制御学会東北支部、第２１５回（２００４年５月２７日）、資料番号２１５−１５
【非特許文献２】「Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法による可解性を問わない逆運動学」、第２７回日本ロボット学会学術講演会、ＲＳＪ２００９ＡＣ２Ｋ１−０３（２００９年）
【発明の概要】
【発明が解決しようとする課題】
【００１３】
特許文献１の手法は、発見的な手法に拠らず、グレブナー基底を用いることにより、汎用的な手法として、且つ、効率的に逆運動学の解を求めようとするものである。
【００１４】
特許文献２の手法は、一般的な６自由度のロボットの逆運動学の解を、３関節の逆運動学を逐次的に行うことにより、汎用的な方法として、且つ、効率的に解を求めようとするものである。
【００１５】
特許文献３の手法は、新たな制御変数λを導入し、それを用いて逆運動学の解を、より効率的に求めようとするものである。
【００１６】
非特許文献１の手法は、予め、関節角と手先位置姿勢との対応関係を順運動学において求めておき、それの有限個の組み合わせを記憶しておき、逆運動学の処理時には、その組み合わせの中から適当な初期値を選択することにより、逆運動学の解を、より効率的に求めようとするものである。
【００１７】
非特許文献２の手法は、Levenberg-Marquardt法と言う手法を用いることにより、実質的に収束性を保証した上で、逆運動学の解を、より効率的に求めようとするものである。
【００１８】
しかしながら、特許文献１の手法では、複数個の解が存在する場合においては、処理に必要とする演算時間、メモリー使用量が非常に増大するという問題がある。近年のロボットにおいては、複数個の解が存在する方が一般的であるので、この問題は深刻である。
【００１９】
特許文献２の手法においては、対象が６自由度のロボットに限定され、それ以外の場合への適用が困難であるという問題がある。近年のロボットにおいては、手先に対する姿勢の自由度を改善するために、７以上の自由度を持つものも多い。
【００２０】
特許文献３の手法においては、複数個の解が存在する場合に、解の選択性などに問題がある。具体的には、各々の関節角以外に、複数個の解のいずれかを指定するための補助的なパラメーターを別途必要とする場合が多い。
【００２１】
非特許文献１の手法においては、ロボットの自由度が増えた場合、或いは精度を改善するために予め持っているデーターの刻みを細かくした場合、そのデーターを記憶するためのメモリーの使用量が急速に増大するという問題がある。
【００２２】
非特許文献２の手法においては、反復回数は少ないが、反復１回あたりの処理が複雑になり、結局のところ処理時間が長くなるという問題がある。
【００２３】
本発明の幾つかの態様によれば、ロボットの逆運動学の解を、反復演算手法により、より効率的に短時間で求めることができるロボット制御システム、ロボットシステム及びロボット制御方法等を提供できる。
【課題を解決するための手段】
【００２４】
本発明の一態様は、ロボットの目標エンドポイント位置を取得するエンドポイント位置取得部と、前記ロボットの真正モデルの情報を記憶する真正モデル記憶部と、前記真正モデルを近似した近似モデルの情報を記憶する近似モデル記憶部と、前記近似モデルと前記目標エンドポイント位置を用いて、近似解を求め、前記近似解と前記真正モデルを用いて、反復法による反復演算処理を行うことで、前記ロボットの逆運動学における解を求める処理部とを含むロボット制御システムに関係する。
【００２５】
本発明の一態様によれば、ロボットの真正モデルと、真正モデルを近似した近似モデルが用意され、近似モデルと目標エンドポイント位置を用いて、近似解が求められる。そして、求められた近似解と真正モデルを用いて、反復法による反復演算処理を行うことで、ロボットの逆運動学における解が求められるようになる。このように近似モデルによる近似解を利用して反復演算処理を行うことで、ロボットの逆運動学の解を、反復法により、より効率的に短時間で求めることが可能になる。
【００２６】
また本発明の一態様では、前記処理部は、前記近似解を初期値として設定して、前記反復演算処理を行ってもよい。
【００２７】
このように、近似モデルによる近似解を、反復演算処理の初期値に設定すれば、反復法おける初期値の決定等を効率良く行うことが可能になる。
【００２８】
また本発明の一態様では、前記処理部は、前記近似解を前記真正モデルの関節角に設定することで、近似解によるエンドポイント位置を求め、前記近似解によるエンドポイント位置と前記目標エンドポイント位置との間の誤差に基づいて、前記目標エンドポイント位置を修正することで、修正目標エンドポイント位置を求め、前記修正目標エンドポイント位置と前記近似モデルを用いて、第２の近似解を求め、前記第２の近似解と前記真正モデルを用いて、前記反復演算処理を行うことで、前記ロボットの逆運動学における解を求めてもよい。
【００２９】
このようにすれば、近似解と真正モデルの解の差異を、予め目標エンドポイント位置に対して修正しておくことができ、ロボットの逆運動学の解を、より効率的に短時間で求めることが可能になる。
【００３０】
また本発明の一態様では、前記近似モデル記憶部は、前記真正モデルを近似したモデルとして、第１〜第mの近似モデルの情報を記憶し、前記処理部は、前記第１〜第mの近似モデルと前記目標エンドポイント位置を用いて、第１〜第mの近似解を求め、前記第１〜第mの近似解の中から選択された近似解を初期値として設定して、前記反復演算処理を行うことで、前記ロボットの逆運動学における解を求めてもよい。
【００３１】
このようにすれば、１つの近似モデルを用いる場合に比べて、反復法に使用される初期値として、より適切な初期値を設定することが可能になる。
【００３２】
また本発明の一態様では、前記処理部は、前記第１〜第mの近似モデルのうちの第ｉ（１≦ｉ≦m）の近似モデルと前記目標エンドポイント位置を用いて、第ｉの近似解を求め、前記第ｉの近似解を前記真正モデルの関節角に設定することで、近似解による第ｉのエンドポイント位置とヤコビアンを求め、前記第ｉのエンドポイント位置と前記目標エンドポイント位置との間の第１の誤差と、前記ヤコビアンを用いて、関節角を補正し、補正後の関節角を前記真正モデルの関節角に設定することで、第ｉの補正エンドポイント位置を求め、前記第ｉの補正エンドポイント位置と前記目標エンドポイント位置の間の第２の誤差に基づいて選択された近似解を、前記初期値として設定してもよい。
【００３３】
このようにすれば、反復法の例えば最初のステップ分の処理を行うことで、目標エンドポイント位置に最も近くづくような近似解を、反復演算処理の初期値として設定できるようになる。
【００３４】
また本発明の一態様では、前記処理部は、前記近似解を前記真正モデルの関節角に設定することで、近似解によるエンドポイント位置を求め、前記近似解によるエンドポイント位置と現在のエンドポイント位置の中から、選択されたエンドポイント位置に対応する関節角を、前記初期値に設定して、前記反復演算処理を行うことで、前記ロボットの逆運動学における解を求めてもよい。
【００３５】
このようにすれば、例えば近似解によるエンドポイント位置を用いるよりも現在のエンドポイント位置を用いる方が、短時間で効率的に解を求めることができる場合に、これに適切に対処できるようになる。
【００３６】
また本発明の一態様では、前記処理部は、前記近似解によるエンドポイント位置と前記目標エンドポイント位置との間の第１の誤差を求め、前記現在のエンドポイント位置と前記目標エンドポイント位置との間の第２の誤差を求め、前記第１の誤差の方が前記第２の誤差よりも小さい場合には、前記近似解によるエンドポイント位置に対応する関節角を、前記初期値に設定し、前記第２の誤差の方が前記第１の誤差よりも小さい場合には、前記現在のエンドポイント位置に対応する関節角を、前記初期値に設定してもよい。
【００３７】
このようにすれば、目標エンドポイント位置との間の誤差が小さくなる方のエンドポイント位置を選択して、選択されたエンドポイント位置に対応する関節角を、逆運動学における解を求める際の初期値に設定できるようになる。
【００３８】
また本発明の一態様では、前記処理部は、前記ロボットのポイント・ツー・ポイント制御が行われた場合には、前記近似解によるエンドポイント位置に対応する関節角を、前記初期値に設定し、前記ロボットのコンティニュアス制御が行われた場合には、前記現在のエンドポイント位置に対応する関節角を、前記初期値に設定してもよい。
【００３９】
このようにすれば、ポイント・ツー・ポイント制御やコンティニュアス制御が行われた場合に、各制御に最適なエンドポイント位置の関節角を初期値に設定して、逆運動学により解を求めることが可能になる。
【００４０】
また本発明の一態様では、過去のエンドポイントの動作履歴位置である動作履歴エンドポイント位置を記憶する動作履歴記憶部を含み、前記処理部は、前記近似解によるエンドポイント位置と前記現在のエンドポイント位置と前記動作履歴エンドポイント位置の中から、選択されたエンドポイント位置に対応する関節角を、前記初期値に設定して、前記反復演算処理を行うことで、前記ロボットの逆運動学における解を求めてもよい。
【００４１】
このようにすれば、近似解によるエンドポイント位置や現在のエンドポイント位置に加えて、動作履歴エンドポイント位置も加味して、逆運動学の解を求めるための初期値を設定できるようになる。
【００４２】
また本発明の一態様では、前記処理部は、前記近似解によるエンドポイント位置と前記目標エンドポイント位置との間の第１の誤差を求め、前記現在のエンドポイント位置と前記目標エンドポイント位置との間の第２の誤差を求め、前記動作履歴エンドポイント位置と前記目標エンドポイント位置との間の第３の誤差を求め、前記第１の誤差の方が前記第２の誤差、前記第３の誤差よりも小さい場合には、前記近似解によるエンドポイント位置に対応する関節角を、前記初期値に設定し、前記第２の誤差の方が前記第１の誤差、前記第３の誤差よりも小さい場合には、前記現在のエンドポイント位置に対応する関節角を、前記初期値に設定し、前記第３の誤差の方が前記第１の誤差、前記第２の誤差よりも小さい場合には、前記動作履歴エンドポイント位置に対応する関節角を、前記初期値に設定してもよい。
【００４３】
このようにすれば、目標エンドポイント位置との間の誤差が小さくなるエンドポイント位置を選択して、選択されたエンドポイント位置に対応する関節角を、逆運動学における解を求める際の初期値に設定できるようになる。
【００４４】
また本発明の一態様では、前記近似モデル記憶部は、前記真正モデルの関節のオフセットが省略されたモデルの情報を、前記近似モデルの情報として記憶してもよい。
【００４５】
また本発明の他の態様は、上記のいずれかに記載のロボット制御システムと、前記ロボットとを含むロボットシステムに関係する。
【００４６】
また本発明の他の態様は、ロボットの真正モデルの情報を記憶し、前記真正モデルを近似した近似モデルの情報を記憶し、前記ロボットの目標エンドポイント位置を取得し、前記近似モデルと前記目標エンドポイント位置を用いて、近似解を求め、前記近似解と前記真正モデルを用いて、反復法による反復演算処理を行うことで、前記ロボットの逆運動学における解を求め、求められた解に基づいて前記ロボットを制御するロボット制御方法に関係する。
【図面の簡単な説明】
【００４７】
【図１】図１（Ａ）〜図１（Ｃ）は、ロボットの関節や順運動学や逆運動学の説明図。
【図２】図２（Ａ）〜図２（Ｄ）は、ロボットの関節のオフセットの説明図。
【図３】図３（Ａ）〜図３（Ｃ）も、ロボットの関節のオフセットの説明図。
【図４】図４（Ａ）〜図４（Ｃ）は、真正モデル、近似モデルの説明図。
【図５】本実施形態の第１の実施例のロボット制御システムの構成例。
【図６】ロボット制御システムの具体的な構成例。
【図７】図７（Ａ）、図７（Ｂ）はロボットシステムの一例。
【図８】図８（Ａ）〜図８（Ｄ）は第１の実施例の手法の説明図。
【図９】第１の実施例の詳細な処理を示すフローチャート。
【図１０】本実施形態の第２の実施例のロボット制御システムの構成例。
【図１１】図１１（Ａ）〜図１１（Ｄ）は第２の実施例の手法の説明図。
【図１２】第２の実施例の詳細な処理を示すフローチャート。
【図１３】本実施形態の第３の実施例のロボット制御システムの構成例。
【図１４】図１４（Ａ）〜図１４（Ｃ）は第３の実施例の手法の説明図。
【図１５】第３の実施例の詳細な処理を示すフローチャート。
【図１６】本実施形態の第４の実施例のロボット制御システムの構成例。
【図１７】本実施形態の第５の実施例のロボット制御システムの構成例。
【図１８】図１８（Ａ）〜図１８（Ｃ）は第４、第５の実施例の手法の説明図。
【図１９】第４の実施例の詳細な処理を示すフローチャート。
【図２０】第５の実施例の詳細な処理を示すフローチャート。
【発明を実施するための形態】
【００４８】
以下、本発明の好適な実施の形態について詳細に説明する。なお以下に説明する本実施形態は特許請求の範囲に記載された本発明の内容を不当に限定するものではなく、本実施形態で説明される構成の全てが本発明の解決手段として必須であるとは限らない。
【００４９】
１．逆運動学
ロボットの制御において、各関節の角度から全体の姿勢を求める処理は順運動学と呼ばれる。一方、その逆に、所望の姿勢を実現する各関節の角度を求める処理は逆運動学と呼ばれる。逆運動学は、ロボットを実用的に用いる上で非常に重要な処理である。
【００５０】
しかしながら、一般的に順運動学の処理は、実現の容易な処理であるが、逆運動学の処理は、実現の非常に困難な処理となる。順運動学の解は、基本的には座標系の原点の移動と、座標系の回転を逐次的に計算することにより、一意に得られる。一方、逆運動学の解を得るためには、一般的には、非線形の連立方程式を解く必要がある。この解が解析的に得られるのは、ロボットの構造が簡単な場合に限られ、且つ、その場合も、発見的な方法が必要となる。
【００５１】
ロボット（マニピュレーター）への逆運動学や順運動学の適用を考えると、その手先の位置と姿勢を指定するためには、６つのパラメーターが必要となる。つまり、ロボットの手先の３次元座標のパラメーター（Ｘ，Ｙ，Ｚ）と、（Ｘ，Ｙ，Ｚ）のそれぞれの軸の周りの回転角度のパラメーター（Ｕ，Ｖ，Ｗ）とが必要になり、（Ｘ，Ｙ，Ｚ，Ｕ，Ｖ，Ｗ）の６つのパラメーターが必要になる。従って、一般的には、ロボットにも少なくとも６つの自由度が要求されることになる。
【００５２】
任意の自由度のロボットにおいて、各関節角の組を指定すれば、それに対応して１組の（Ｘ，Ｙ，Ｚ，Ｕ，Ｖ，Ｗ）が求まる。つまり、順運動学の解は一意に求まる。しかし、６自由度のロボットに１組の（Ｘ，Ｙ，Ｚ，Ｕ，Ｖ，Ｗ）を与えて各関節角を求める場合を考えても、その解が１つであるとは限らない。むしろ、ロボットの構造の部分的な対称性などにより、複数の解が存在する場合の方が一般的である。また、近年は、ロボットに、より自由な姿勢をとらせるために、７自由度のロボットを用いる場合が多くなっており、この場合は、（Ｘ，Ｙ，Ｚ，Ｕ，Ｖ，Ｗ）を指定しても、解が存在する場合には無限個の解が存在することになる。
【００５３】
例えば図１（Ａ）に、単純な６自由度のロボットアーム（広義にはロボット）の例を示す。このロボットアームは、関節Ｊ１〜Ｊ６とこれらの関節により接続される複数のリンクを有する。ここで関節Ｊ１、Ｊ４、Ｊ６は、図１（Ｂ）に示すように軸回転関節であり、関節Ｊ２、Ｊ３、Ｊ５は旋回関節である。
【００５４】
図１（Ｃ）に示すように、関節Ｊ１〜Ｊ７の関節角のパラメーター（θ１，θ２，θ３，θ４，θ５，θ６，θ７）から、手先位置姿勢（広義にはエンドポイント位置）のパラメーター（Ｘ，Ｙ，Ｚ，Ｕ，Ｖ，Ｗ）を求める処理が、順運動学である。一方、手先位置姿勢のパラメーター（Ｘ，Ｙ，Ｚ，Ｕ，Ｖ，Ｗ）から、関節角のパラメーター（θ１，θ２，θ３，θ４，θ５，θ６，θ７）を求める処理が、逆運動学である。
【００５５】
図１（Ａ）のような単純なロボットアームの逆運動学の解は、解析的に求めることができる。一方、６自由度のロボットにおいては、同一の手先姿勢に対し、部分対称である複数の解が存在する。また、７自由度のロボットにおいては、手先の拘束条件は６つであるので、解が存在する場合には無限個の解が存在する。この場合に、アームアングルと呼ばれる付加的な拘束条件を与えたとしても、部分対称である複数の解が存在する。具体的には、基本解に対して、基部反転で２通りの解、肘反転で２通りの解、手首反転で２通りの解が存在する。従って、これらの各反転の組み合わせで、同一の手先位置姿勢に対して８通りの解が存在する。
【００５６】
逆運動学の解析解が得られる条件のうち、最も基本的なものは、図２（Ａ）のように回転軸が一点で交わる場合である。図２（Ａ）では、関節Ｊ１とＪ３の回転軸は紙面に平行であり、これらの回転軸は、関節Ｊ２の中心の交点で交わる。関節Ｊ２の回転軸は紙面に垂直であり、この回転軸も、上記と同一の交点で交わる。これは、詳細な説明は省略するが、Ｚ-Ｘ-Ｚオイラー角などにより、任意の３次元的な回転が表現できることに関連する。
【００５７】
一方、逆運動学の解析解が通常は得られない条件は、図２（Ｂ）のように関節にオフセットが存在する場合である。これは、１つの関節の回転により、複数の関節位置姿勢の変化が生じるためである。例えば図２（Ａ）では、手先（エンドポイント）を固定し、関節Ｊ３が軸回転しても、関節Ｊ２の位置は変化しない。一方、図２（Ｂ）のようにオフセットが存在する場合、つまり関節Ｊ３の回転軸が関節Ｊ２の中心を通らない場合には、関節Ｊ３が軸回転すると、関節Ｊ２の位置が変化してしまう。
【００５８】
図２（Ｃ）のように関節にオフセットが存在する場合でも、例えば、図２（Ｄ）のように、数学的に等価な、オフセットが省略されたものに変換することが可能である。これは、関節Ｊ３の回転によっても、関節Ｊ２とＪ４の間の距離が変化せず、且つ、関節Ｊ３、Ｊ４、Ｊ５の接続のように、解析解が得られ、且つ、自由度の高い構造が存在するからである。しかし、このような変換が可能かどうかはロボットアームの構造に強く依存し、仮に変換が可能であったとしても、その変換は試行錯誤的に決めるしかない。
【００５９】
以上のように、オフセットの存在は、逆運動学の処理を困難にする。しかしながら、オフセットには、実用上において有利な効果が存在する。例えば、図３（Ａ）、図３（Ｂ）に示すような関節の可動範囲の拡大である。現実のロボットアームには、モーター、構造体の大きさがあるために、例えば図３（Ａ）のような場合は、関節Ｊ１、Ｊ３の大きさが原因で、関節Ｊ２の可動範囲に制限が発生する。一方、図３（Ｂ）のようにオフセットを加えることにより、関節Ｊ２の可動範囲を広くすることが可能となる。
【００６０】
また、図３（Ａ）の場合には、回転軸を１点に集中させるために、その周辺の機械的な構造が複雑になり、その結果、重量が増大してしまう。一方、図３（Ｂ）のような構造においては、相対的に単純な機械的な構造を採用することができ、その結果、重量を軽減することが可能であるという効果も存在する。
【００６１】
本実施形態では、例えば図３（Ｃ）に示すようなロボットアーム（ロボット）を想定する。図３（Ｃ）は、逆運動学の解が解析的に求め難い、或いは求められない構造の一例である。当然であるが、本実施形態の手法は、このような構造のロボットに特化したものではない。
【００６２】
本実施形態では、図４（Ａ）の真正モデルＭＴを近似したモデルとして、解析処理（解析解を得る処理）が可能な図４（Ｂ）、図４（Ｃ）に示すような近似モデルＭＡ（近似解析モデル）を用意する。図４（Ｂ）、図４（Ｃ）の近似モデルＭＡは、図４（Ａ）の真正モデルＭＴからオフセット（回転軸のオフセット）を省略したモデル（取り除いたモデル）である。図４（Ｂ）の近似モデルＭＡでは、オフセットを単に取り去っているが、図４（Ｃ）の近似モデルＭＡでは、関節間を結ぶリンクの長さとして、オフセット部の対角線の長さを用いている。なお、近似モデルは図４（Ｂ）、図４（Ｂ）に示すようなモデルに限定されず、種々の変形実施が可能である。例えば全ての関節についてのオフセットを省略するのではなく、一部の関節についてのオフセットを省略したモデルを近似モデルとしてもよい。
【００６３】
２．第１の実施例
図５に、本実施形態の第１の実施例のロボット制御システム（及びこれを含むロボットシステム）の構成例を示す。図５は、第１の実施例及び後述する第２〜第５の実施例を包含する本実施形態の基本的な構成例でもある。なお、本実施形態のロボット制御システム（ロボットシステム）は図５の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。
【００６４】
図５のロボット制御システム（ロボット制御装置）は、目標エンドポイント位置取得部２０と、真正モデル記憶部２２と、近似モデル記憶部２４と、処理部３０を含む。また、アーム駆動制御部等として機能するロボット制御部８０を含むことができる。そして、このロボット制御システムとロボット１００（ロボットアーム）とにより本実施形態のロボットシステムが実現される。
【００６５】
エンドポイント位置取得部（手先位置姿勢取得部）２０は、ロボット１００（ロボットアーム）の目標エンドポイント位置を取得する。目標エンドポイント位置（目標エンドエフェクター位置）は、ロボット１００のエンドポイント（エンドエフェクター）の移動目標となる位置（姿勢）であり、後述する軌道情報（計画軌道）により特定されるものである。
【００６６】
なお、以下ではロボット１００（ロボットアーム）の手先位置姿勢（Ｘ，Ｙ，Ｚ，Ｕ，Ｖ，Ｗ）を、エンドポイント位置として説明する。本実施形態の処理対象となる手先位置姿勢は、エンドポイント位置に対応する手先位置（Ｘ，Ｙ，Ｚ）であってもよいし、手先位置（Ｘ，Ｙ，Ｚ）及び手先姿勢（Ｕ，Ｖ，Ｗ）の両方であってもよい。
【００６７】
真正モデル記憶部２２は、ロボット１００の真正モデルの情報（真正モデルの関節やリンクの情報）を記憶する。近似モデル記憶部２４は、真正モデルを近似した近似モデルの情報（近似モデルの関節やリンクの情報）を記憶する。具体的には、近似モデル記憶部２４は、真正モデルの関節のオフセットが省略されたモデルの情報を、近似モデルの情報として記憶する。例えば前述の図４（Ａ）に示すように真正モデルＭＴがオフセットを有する場合には、図４（Ｂ）、図４（Ｃ）に示すようにオフセットが省略された近似モデルＭＡを用意し、その情報を近似モデル記憶部２４に記憶する。なお、真正モデル記憶部２２に記憶される情報と近似モデル記憶部２４に記憶される情報のうち重複する部分については共用して記憶するようにしてもよい。
【００６８】
処理部３０は、反復法による反復演算処理を行うことで、ロボット１００の逆運動学における解を求める処理などを行う。具体的には処理部３０は、近似モデルと目標エンドポイント位置（手先位置や足先位置）を用いて、近似解を求める。この近似解は、例えばロボット１００の関節角（関節変数）の近似解（狭義には近似解析解。以下、同様）である。そして処理部３０は、近似解と真正モデルを用いて、反復法による反復演算処理を行うことで、ロボット１００の逆運動学における解を求める。つまり、ロボット１００の関節角（関節変数）の解を求める。例えば処理部３０は、近似解を初期値として設定して、反復演算処理を行うことで、逆運動学における解を求める。この処理部３０の機能は、プロセッサー又はＡＳＩＣなどのハードウエアー回路や、プロセッサー等において動作するプログラムなどにより実現できる。
【００６９】
処理部３０は、近似解演算部４０、反復演算処理部５０、収束判定部５２、エラー処理部５４を含む。
【００７０】
近似解演算部４０は、ロボットの関節角の近似解を求める処理を行う。具体的には、近似モデル記憶部２４からの近似モデル（近似解析モデル）と、目標エンドポイント位置取得部２０からの目標エンドポイント位置（手先位置姿勢）とを用いた解析的な処理により、ロボットの各関節角の近似解析解を求める処理を行う。例えば、目標エンドポイント位置が引数の変数となり、関節角が解の変数となる関数式（非線形連立方程式等）により、関節角の近似解析解を求める。この場合に、例えば三角関数などの非線形関数を多項式に変換して近似解析解を求めることができる。また、アームアングルなどの付加的な拘束条件を与えるようにしてもよい。例えば６自由度のロボットアームについては、６つの関節のうち連続した３つの関節が全て回転関節であり、且つ、それらの関節の３つの回転軸の延長線が一点で交わるような構造の場合には、例えば２組の３次元４次連立方程式に帰着させることで、逆運動学の解を解析的に求めることができる。
【００７１】
反復演算処理部５０は、逆運動学における解を反復法により求める処理を行う。収束判定部５２は、反復法による反復演算処理の収束判定を行う。エラー処理部５４は、反復演算処理においてエラーが発生した場合の処理を行う。
【００７２】
例えば、反復演算処理部５０は、ヤコビアンを用いた数値計算や、最適化計算、探索法等により、逆運動学の解（関節変数の解）を求める。例えばヤコビアンを用いた逆運動学やクォータニオンの逆運動学のアルゴリズムでは、全ての関節の角度を同時に少しずつ更新しながら解を探索する。一方、ＣＣＤ（Cyclic-Coordinate-Descent）法では、１回の計算ステップにおいては、１つの関節の最適角度のみを求め、それを全ての関節に順番に繰り返すことで、ロボットアーム全体としての逆運動学の解を探索する。ＰＩＫ（Particle-IK）法では、関節角ではなく関節位置を直接操作することで逆運動学の解を求める。
【００７３】
例えば下式（１）に示すように、ｍ個の変数を入力とし、ｎ個の出力を与える関数ｆを考える。或いは、ｍ個の変数を入力とし、１つの出力を与える関数ｆをｎ組用意する。
【００７４】
【数１】
2013132731.tif 000003
この場合に、ヤコビアン（ヤコビ行列）は、下式（２）に表される行列である。
【００７５】
【数２】
2013132731.tif 000004
つまり、ヤコビアンは、ｘの「ある値」の近傍において、ｘとｙの関係を線形近似したものである。従って、一般的には、ｘの「ある値」が変化すれば、ヤコビアンも変化する。
【００７６】
ここで、ｘを関節角、ｙをロボットのエンドポイント位置（狭義には手先位置姿勢。以下も同様）とすれば、上式（２）のヤコビアン（ヤコビ行列）は、ある姿勢において各関節が微小に変化した場合の、それに伴うエンドポイント位置の微小な変化を与える関係式となる。ここで、各関節角の微小変化と、エンドポイント位置の微小変化との対応は、その姿勢に応じて変化する。つまり、ヤコビアンは、姿勢の変化に対応して、その都度、計算する必要がある。
【００７７】
エンドポイント位置（手先位置姿勢）から関節角を求める逆運動学の処理は、一般的には非常に難しい処理であるが、下式（３）のように、ヤコビアンの逆行列である逆ヤコビアンを求めることができれば、エンドポイント位置の微小な変化を各関節角の微小な変化に変換する関係式を求めることができる。
【００７８】
【数３】
2013132731.tif 000005
一般的には、ヤコビアンは正方行列ではなく、且つ、ｒａｎｋ（階数）も低く、通常の意味の逆行列は存在しない。例えば７軸のロボットでは、手先の自由度は６つしかないので、７×７ではなく、７×６の行列になってしまう。但し、通常の意味の逆行列と類似した性質を持つ一般化逆行列は存在し得る。この一般化逆行列による逆ヤコビアンＪ−１により、エンドポイント位置の微小な変化を各関節角の微小な変化に変換することができる。例えばヤコビアンを用いた逆運動学の１つのアルゴリズム例では、現在のエンドポイント位置から目標エンドポイント位置へと延びる変位ベクトルΔＰを計算する。次に、逆ヤコビアンＪ−１を用いて、関節角の変位ベクトルΔθ＝ΔＰＪ−１を計算する。そして、θ＝θ＋Δθにより関節角ベクトルを更新する。そして更新後のエンドポイント位置と目標エンドポイント位置との差異が許容誤差範囲内ならば、解が収束したと判定して、反復演算処理を終了する。この場合の収束判定処理は収束判定部５２により行われる。解が収束した場合には、その解は関節角としてロボット制御部８０に出力される。一方、解が収束しなかった場合には、エラー処理部５４が、エラー時用の処理を行う。
【００７９】
ロボット制御部８０は、処理部３０により求められた解に基づいて、ロボット１００の制御を行う。具体的には、解として求めれた関節角を、ロボット１００の各関節角に設定するフィードバック制御を行い、ロボット制御を実現する。
【００８０】
図６に、本実施形態のロボット制御システム（ロボットシステム）の具体的な構成例を示す。このロボット制御システムは、力制御部１２、目標値出力部６０、ロボット制御部８０を含む。またロボットシステムは、このロボット制御システムと、ロボット１００（力覚センサー１０）により構成される。
【００８１】
目標値出力部６０は、ロボット（狭義にはマニピュレーター）のフィードバック制御の目標値を出力する。この目標値に基づいてロボット１００のフィードバック制御が実現される。
【００８２】
目標値出力部６０は、軌道生成部６２とインバースキネマティクス処理部６４を含む。軌道生成部６２は、ロボットの軌道情報を出力する。軌道情報は、ロボットのエンドエフェクター部（エンドポイント）の位置情報（Ｘ，Ｙ，Ｚ）と、各座標軸回りでの回転角度情報（Ｕ，Ｖ，Ｗ）を含むことができる。インバースキネマティクス処理部６４は、軌道生成部６２からの軌道情報に基づいてインバースキネマティクス処理（逆運動学処理）を行い、例えばロボットの関節角情報を目標値として出力する。例えば、図６の軌道生成部６２が図５の目標エンドポイント位置取得部２０に相当し、インバースキネマティクス処理部６４が処理部３０に相当する。
【００８３】
力制御部１２（狭義にはインピーダンス制御部）は、力覚センサー１０からのセンサー情報に基づいて力制御（力覚制御）を行って、目標値の補正値を出力する。更に具体的には、力制御部１２（インピーダンス制御部）は、力覚センサー１０からのセンサー情報（力情報、モーメント情報）に基づいてインピーダンス制御（或いはコンプライアンス制御）を行う。力制御は、例えば、従来の位置制御に対して、力のフィードバックが加わった制御である。インピーダンス制御は、外力に対するエンドエフェクター部（手先）の変位の生じやすさ（機械インピーダンス）を、制御により望ましい状態にする手法である。具体的には、ロボットのエンドエフェクター部の質量に粘性係数と弾性要素が接続されるモデルにおいて、目標として設定した粘性係数と弾性係数で物体に接触するようにする制御である。また力覚センサー１０は、ロボット１００が出している力の反力として受けている力やモーメントを検出するセンサーである。この力覚センサー１０は、通常、ロボット１００のアームの手首部分に取り付けられ、検出された力やモーメントは、センサー情報として、各種の力制御（インピーダンス制御）に用いられる。
【００８４】
ロボット制御部８０は、目標値出力部６０からの目標値に基づいて、ロボットのフィードバック制御を行う。具体的には、力制御部１２からの補正値により補正された目標値に基づいて、ロボットのフィードバック制御を行う。例えば補正値により補正された目標値と、ロボット１００からのフィードバック信号に基づいて、ロボット１００のフィードバック制御を行う。例えばロボット制御部８０は、複数の駆動制御部８２-1〜８２-N（狭義にはモーター制御部）を含み、ロボット１００が有する駆動部１０２-1〜１０２-Nに対して、その制御信号を出力する。ここで駆動部１０２-1〜１０２-Nは、ロボット１００の各関節を動かすための駆動機構であり、例えばモーター等により実現される。
【００８５】
図７（Ａ）に本実施形態のロボット制御システムを含むロボットシステムの例を示す。このロボットシステムは、制御装置３００（ロボット制御システム）と、ロボット本体３１０（広義にはロボット）とを含む。制御装置３００はロボット本体３１０の制御処理を行う。具体的には、動作シーケンス情報（シナリオ情報）に基づいてロボット本体３１０を動作させる制御を行う。ロボット本体３１０は、アーム３２０及びハンド（把持部）３３０を有する。そして制御装置３００からの動作指示にしたがって動作する。例えば、図示しないパレットに載せられたワークを把持したり、移動するなどの動作を行う。また、図示しない撮像装置で取得された撮像画像情報に基づいて、ロボット本体３１０の姿勢やワークの位置などの情報が検出され、検出された情報が制御装置３００に送られる。
【００８６】
本実施形態のロボット制御システムは例えば図７（Ａ）の制御装置３００に設けられ、例えば制御装置３００のハードウェアーやプログラムによりロボット制御システムが実現される。そして本実施形態のロボット制御システムによれば、制御装置３００などの制御ハードウェアーに対する性能要求を低減できると共に、ロボット本体３１０を高い応答性で動作させることなどが可能になる。
【００８７】
なお、図７（Ａ）では、ロボット本体３１０（ロボット）と制御装置３００（ロボット制御システム）とが別体に構成されているが、図７（Ｂ）に示すようにロボット本体３１０と制御装置３００とが一体に構成されていてもよい。具体的には図７（Ｂ）では、ロボット本体３１０を支えるベースユニット部に制御装置３００が格納されている。そして、ベースユニット部には車輪等が設けられ、ロボット全体が移動可能な構成となっている。また図７（Ａ）は単腕型のロボットの例であるが、本実施形態のロボットは図７（Ｂ）に示すような双腕型等の多腕型のロボットであってもよい。
【００８８】
次に本実施形態の第１の実施例の手法の詳細について説明する。第１の実施例では、対象となるロボットを近似し、解析的に逆運動学の解を求めることが可能な近似モデル（近似解析モデル）を用意する。例えばロボットの真正モデルでは、図２（Ｂ）に示すようなオフセットが存在する場合に、近似モデルでは図２（Ａ）に示すようにこのオフセットを取り去る。即ち、図２（Ｂ）の真正モデルでは、関節Ｊ３の回転軸が関節Ｊ２の関節中心を通らず、オフセットを有しているが、図２（Ａ）の近似モデルでは、関節Ｊ３の回転軸が関節Ｊ２の関節中心を通る。また関節Ｊ１の回転軸も関節Ｊ２の関節中心を通る。このため、図２（Ａ）の近似モデルを用いることで、解析的に解を求めることができる。そして、指定された目標エンドポイント位置（目標手先位置姿勢）を入力とし、近似モデルを用いて解析的な処理を行い、解析解である近似解を求める。そして、例えばこの近似解を初期値とし、反復法により、対象となるロボットの逆運動学の解を求める。即ち、近似解を、真正モデルの関節角の初期値に設定して、反復法による反復演算処理を行って、逆運動学の解を求める。ここで、対象となるロボットから近似モデルを求める手法の一例は、ロボットが持つオフセットの長さを零とするものである。また、ロボットの逆運動学の解を求める手法の一例は、ヤコビアン（逆ヤコビアン）を用いる手法である。
【００８９】
具体的には、まず、図８（Ａ）に示すように、目標エンドポイント位置（手先位置姿勢）を与える。目標エンドポイント位置は、例えば図６の軌道生成部６２からの軌道情報に基づいて取得される。
【００９０】
次に図８（Ｂ）に示すように近似モデルＭＡと目標エンドポイント位置を用いて近似解を求める。即ち、近似モデルＭＡの目標エンドポイント位置を図８（Ａ）の位置に設定して、解析的な処理により、関節角の解析解を近似解として求める。近似モデルは解析解を求めることが可能なモデルになっているため、その関節角の解析解を求めることができる。
【００９１】
次に、図８（Ｃ）に示すように、求められた近似解を初期値に設定する。具体的には、真正モデルＭＴの各関節角に、近似モデルにより求められた各関節角の近似解を設定する。近似モデルが良いモデルであれば、近似解は真の解に近いはずであり、図８（Ｃ）の真正モデルＭＴのエンドポイント位置と目標エンドポイント位置との間の誤差は小さいはずである。
【００９２】
そして、このように真正モデルＭＴの関節角の初期値に、近似モデルＭＡにより得られた近似解を設定して、図８（Ｄ）に示すように反復法による反復演算処理を行う。つまり、図８（Ｃ）の誤差を反復法により修正し、逆運動学における解を求める。反復演算処理が収束すれば、それが解となる。即ち、エンドポイント位置と目標エンドポイント位置との間の誤差が許容誤差範囲内になったら、解は収束したと判定され、その時の解が、逆運動学により求めるべきロボットの各関節角になる。
【００９３】
図９は第１の実施例の詳細な処理を表すフローチャートである。まず、目標エンドポイント位置を取得する（Ｓ１）。次に、近似モデルにより近似解を求める（Ｓ２）。即ち、近似モデルを用いた解析的な処理により、解析解である近似解を求める。
【００９４】
次に、解は得られたか否かを判断し、得られなかった場合には処理が失敗したと判断する（Ｓ３、Ｓ７）。一方、得られた場合には、得られた近似解を初期値として用いて、反復法で解を求める（Ｓ４）。具体的には、真正モデルの各関節角の初期値として近似解を設定し、反復法による反復演算処理により逆運動学における解を求める。そして、解が収束した場合には、処理が成功したと判断し（Ｓ５、Ｓ６）、その時の解をロボットの各関節角に設定して、ロボット制御を行う。
【００９５】
以上の本実施形態の手法によれば、複雑な構造を持つロボットの逆運動学の解を、反復法により、より効率的に短時間で求めることが可能になる。また本実施形態によれば、従来の反復法の初期値決めに必要であったような大きな記憶容量を不要にすることができる。また本実施形態によれば、従来の反復法においては困難であったロボットの姿勢制御、冗長自由度制御を容易に行うことが可能となる。以上により、冗長自由度を持ち、オフセットなどの複雑な構造を持つロボットに対して、より高度、高速な制御を実現することが可能になる。
【００９６】
３．第２の実施例
図１０に本実施形態の第２の実施例のロボット制御システム（及びこれを含むロボットシステム）の構成例を示す。なお、ロボット制御システムの構成やその構成要件について、第１の実施例と重複する部分については、適宜、その説明を省略する。
【００９７】
図１０のロボット制御システムは、目標エンドポイント位置取得部２０と、真正モデル記憶部２２と、近似モデル記憶部２４と、処理部３０を含む。またロボット制御部８０を含むことができる。
【００９８】
図１０の処理部３０は、近似解（近似解析解）を真正モデルの関節角（関節変数）に設定することで、近似解によるエンドポイント位置（手先位置姿勢）を求める。即ち、近似モデルにより得られた近似解を、真正モデルの各関節角に設定して、その時の真正モデルのエンドポイント位置を近似解によるエンドポイント位置として求める。次に、処理部３０は、近似解によるエンドポイント位置と目標エンドポイント位置との間の誤差に基づいて、目標エンドポイント位置を修正することで、修正目標エンドポイント位置を求める。即ち、当該誤差の分だけ目標エンドポイント位置をずらす修正を行い、エンドポイント位置をずらした位置を、修正目標エンドポイント位置とする。
【００９９】
そして処理部３０は、修正目標エンドポイント位置と近似モデルを用いて、第２の近似解を求める。具体的には、修正目標エンドポイント位置と近似モデルを用いた解析的処理を行って、関節角の解析解を第２の近似解として求める。そして、処理部３０は、求められた第２の近似解と真正モデルを用いて、反復演算処理を行うことで、ロボット１００の逆運動学における解を求める。具体的には、第２の近似解を真正モデルの関節角の初期値として設定して、反復法の反復演算処理を行うことで、関節角の解を逆運動学における解として求める。
【０１００】
以上の処理を行うために、図１０の処理部３０は、近似解演算部４０、フォワードキネマティクス処理部４２、目標エンドポイント位置修正部４３、反復演算処理部５０、収束判定部５２、エラー処理部５４を含む。
【０１０１】
具体的には、近似解演算部４０は、第１の実施例と同様に、近似モデル記憶部２４からの近似モデルと、目標エンドポイント位置取得部２０からの目標エンドポイント位置を用いて、近似解を求める。そしてフォワードキネマティクス処理部４２は、真正モデル記憶部２２からの真正モデルの関節角に対して、求められた近似解を設定することで、フォワードキネマティクス処理（順運動学処理）により、真正モデルのエンドポイント位置を近似解によるエンドポイント位置として求める。次に、目標エンドポイント位置修正部４３は、近似解によるエンドポイント位置と目標エンドポイント位置との間の誤差に基づいて、目標エンドポイント位置を修正する。そして、近似解演算部４０は、修正目標エンドポイント位置と近似モデルを用いて、解析的処理を行って、第２の近似解を求める。次に、反復演算処理部５０は、第２の近似解と真正モデルを用いて、反復演算処理を行うことで、逆運動学における解を求める。そして、収束判定部５２は、この反復演算処理において解が収束したか否かを判定し、解が収束した場合には、その解を、ロボット１００の関節角としてロボット制御部８０に出力する。一方、解が収束しなかった場合には、エラー処理部５４がエラー時用の処理を行う。
【０１０２】
次に本実施形態の第２の実施例の手法の詳細について説明する。第２の実施例では、対象となるロボットを近似し、解析的に逆運動学の解を求めることが可能な近似モデルを用意する。そして、第１の実施例と同様に、指定された目標エンドポイント位置（目標手先位置姿勢）を入力とし、近似モデルを用いて解析的な処理を行い、解析解である近似解を求める。
【０１０３】
次に、求められた近似解を初期値とし、対象となるロボットのエンドポイント位置を求める。そして、求められたエンドポイント位置と指定された目標エンドポイント位置との差異を誤差として求める。次に、指定された目標エンドポイント位置から、上記の誤差を引き算し、それを修正目標エンドポイント位置とする。そして、修正目標エンドポイント位置を入力として、近似モデルで第２の近似解を求める。次に、この第２の近似解を初期値として、反復法により、対象となるロボットの逆運動学の解を求める。
【０１０４】
この第２の実施例の手法は、近似解を与える近似モデルを２回用いるものである。つまり、真正モデルを用いた場合に起こる、近似解と真正モデルによる解との差異を、予め、目標エンドポイント位置に対して補正しておくことにより、より真の解に近い初期値を得るものである。これは、修正された目標エンドポイント位置に対する第２の近似解の方が、真正モデルを用いた場合の解に近いはずであるという考えに基づいている。
【０１０５】
具体的には、図１１（Ａ）に示すように、第１の実施例と同様に、近似モデルＭＡと目標エンドポイント位置を用いて第１の近似解を求める。そして、第１の近似解を真正モデルＭＴの関節角に設定して、近似解によるエンドポイント位置を求める。
【０１０６】
次に、図１１（Ｂ）に示すように、求められた近似解によるエンドポイント位置（真正モデルのエンドポイント位置）と目標エンドポイント位置との間の誤差（差異）を求める。そして、求められた誤差を用いて、目標エンドポイント位置を修正して、修正目標エンドポイント位置を求める。
【０１０７】
次に、図１１（Ｃ）に示すように、修正目標エンドポイント位置と近似モデルＭＡを用いて、第２の近似解を求める。即ち、近似モデルＭＡの目標エンドポイント位置を修正目標エンドポイント位置に設定して、解析的な処理により、関節角の解析解を第２の近似解として求める。このようにして修正された目標エンドポイント位置に対する第２の近似解の方が、真正モデルＭＴを用いた場合の解に近いはずである。
【０１０８】
次に、図１１（Ｄ）に示すように、求められた第２の近似解を初期値に設定する。具体的には、真正モデルＭＴの各関節角に、近似モデルにより求められた各関節角の第２の近似解を設定する。そして、反復法による反復演算処理を行い、逆運動学における解を求める。反復演算処理が収束すれば、それが解となり、逆運動学により求めるべきロボットの各関節角になる。
【０１０９】
図１２は第２の実施例の詳細な処理を表すフローチャートである。まず、目標エンドポイント位置を取得する（Ｓ１１）。次に、近似モデルにより第１の近似解を求める（Ｓ１２）。即ち、近似モデルに対する解析的な処理により、解析解である第１の近似解を求める。
【０１１０】
次に、解は得られたか否かを判断し、得られなかった場合には処理が失敗したと判断する（Ｓ１３、Ｓ２１）。一方、得られた場合には、得られた第１の近似解と真正モデルを用いて、エンドポイント位置を求める（Ｓ１４）。具体的には、真正モデルの各関節角に第１の近似解を設定し、フォワードキネマティクス処理により、近似解によるエンドポイント位置を求める。そして、求められた近似解によるエンドポイント位置と目標エンドポイント位置との誤差（差異）に基づいて、目標エンドポイント位置を修正し、修正目標エンドポイント位置を求める（Ｓ１５）。
【０１１１】
次に、近似モデルにより修正目標エンドポイント位置に対する第２の近似解を求める（Ｓ１６）。即ち、近似モデルを用いた解析的な処理により、解析解である第２の近似解を求める。そして、解は得られたか否かを判断し、得られなかった場合には処理が失敗したと判断する（Ｓ１７、Ｓ２１）。一方、得られた場合には、得られた第２の近似解と真正モデルを用いて、反復法で解を求める（Ｓ１８）。具体的には、真正モデルの各関節角の初期値として第２の近似解を設定し、反復法による演算処理により逆運動学における解を求める。そして、解が収束した場合には、処理が成功したと判断し（Ｓ１９、Ｓ２０）、その時の解をロボットの各関節角に設定して、ロボット制御を行う。一方、解が得られなかった場合には、処理が失敗したと判断し、エラー処理等を行う（Ｓ２１）。
【０１１２】
以上の第２の実施例の手法によれば、より真の解に近い第２の近似解を初期値に設定して、反復法により逆運動学の解を求めることができる。従って、冗長自由度を持ち、オフセットなどの複雑な構造を持つロボットに対して、より高度、高速な制御を実現することが可能になる。
【０１１３】
４．第３の実施例
図１３に本実施形態の第３の実施例のロボット制御システム（及びこれを含むロボットシステム）の構成例を示す。なお、ロボット制御システムの構成やその構成要件について、第１、第２の実施例と重複する部分については、適宜、その説明を省略する。
【０１１４】
図１３のロボット制御システムは、目標エンドポイント位置取得部２０と、真正モデル記憶部２２と、近似モデル記憶部２４と、処理部３０を含む。またロボット制御部８０を含むことができる。
【０１１５】
図１３の近似モデル記憶部２４は、真正モデルを近似したモデルとして、第１〜第mの近似モデルＭＡ-1〜ＭＡ-m（ｍは２以上の整数）の情報を記憶する。処理部３０は、第１〜第mの近似モデルＭＡ-1〜ＭＡ-mと目標エンドポイント位置を用いて、第１〜第mの近似解を求める。そして、第１〜第mの近似解の中から選択された近似解を初期値として設定して、反復演算処理を行うことで、ロボット１００の逆運動学における解を求める。
【０１１６】
具体的には、処理部３０は、第１〜第mの近似モデルＭＡ-1〜ＭＡ-mのうちの第ｉ（１≦ｉ≦m）の近似モデルと目標エンドポイント位置を用いて、第ｉの近似解を求める。即ち、第ｉの近似モデルと目標エンドポイント位置を用いた解析的処理により、解析解である第ｉの近似解を求める。
【０１１７】
次に処理部３０は、求められた第ｉの近似解を真正モデルの関節角に設定することで、近似解による第ｉのエンドポイント位置とヤコビアンを求める。そして、第ｉのエンドポイント位置と目標エンドポイント位置との間の第１の誤差と、ヤコビアン（逆ヤコビアン）を用いて、関節角を補正する。即ち、ヤコビアンを用いた逆運動学の反復法による反復演算処理を、例えば１回（広義には所定回数）の処理ループだけ、実行する。例えばヤコビアンを用いた逆運動学の反復法では、前述したように、エンドポイント位置の変位（変位ベクトル）と逆ヤコビアンを用いて、関節角の変位（変位ベクトル）を求める。第３の実施例では、エンドポイント位置の変位に相当する第１の誤差と逆ヤコビアンとを用いて求められた関節角の変位の分だけ、関節角を補正する。処理部３０は、この補正後の関節角を真正モデルの関節角に設定することで、第ｉの補正エンドポイント位置を求める。そして、求められた第ｉの補正エンドポイント位置と目標エンドポイント位置の間の第２の誤差に基づいて選択された近似解を、初期値として設定する。具体的には、上述した処理を第１〜第mの近似モデルＭＡ-1〜ＭＡ-mの各近似モデルを用いて行い、第１〜第ｍの補正エンドポイント位置を求める。そして、求められた第１〜第ｍの補正エンドポイント位置と目標エンドポイント位置との間の第２の誤差を求め、第２の誤差が例えば最も小さくなる補正エンドポイント位置に対応する近似解を選択する。そして、選択された近似解を、真正モデルの関節角の初期値として設定して、反復法の反復演算処理を行うことで、逆運動学における解を求める。
【０１１８】
以上の処理を行うために、図１３の処理部３０は、近似解演算部４０-1〜４０-m、近似解選択部４４、反復演算処理部５０、収束判定部５２、エラー処理部５４を含む。
【０１１９】
具体的には、近似解演算部４０-1〜４０-mは、近似モデル記憶部２４からの第１〜第mの近似モデルＭＡ-1〜ＭＡ-mと、目標エンドポイント位置取得部２０からの目標エンドポイント位置を用いて、第１〜ｍの近似解を求める。即ち、各近似解演算部４０-1〜４０-mは、第１の実施例と同様の手法により近似解を求める。そして、近似解選択部４４は、求められた第１〜第mの近似解の中から、例えば誤差が最も少ないと評価される近似解を選択する。例えば、第１〜第mの近似解のうちの第ｉの近似解を真正モデルの関節角に設定することで、近似解による第ｉのエンドポイント位置とヤコビアンが求められ、第ｉのエンドポイント位置と目標エンドポイント位置との間の第１の誤差と、ヤコビアンを用いて、関節角が補正され、補正後の関節角を真正モデルの関節角に設定することで、第ｉの補正エンドポイント位置が求められたとする。この場合には近似解選択部４４は、第１〜第mの補正エンドポイント位置の各第ｉの補正エンドポイント位置と目標エンドポイント位置の間の第２の誤差に基づいて、近似解を選択する。例えば第２の誤差が最も少ない補正エンドポイント位置を選択する。そして、反復演算処理部５０は、選択された近似解と真正モデルを用いて、反復演算処理を行うことで、逆運動学における解を求める。即ち、選択された近似解を真正モデルの初期値に設定して、反復演算処理を行って、関節角についての解を求める。そして収束判定部５２は、解が収束した場合には、その解を、ロボット１００の関節角としてロボット制御部８０に出力する。
【０１２０】
次に本実施形態の第３の実施例の手法の詳細について説明する。第３の実施例では、対象となるロボットを近似し、解析的に逆運動学の解を求めることが可能な複数個の近似モデルを用意する。複数個の近似モデルは、例えば各アームの長さが互いに異なるモデルである。具体的には、図４（Ｂ）、図４（Ｃ）のように、アームの長さの設定が異なる近似モデルなどを想定できる。或いは、オフセットの省略の仕方や、拘束条件が互いに異なる近似モデルであってもよい。
【０１２１】
そして、第１の実施例と同様に、指定された目標エンドポイント位置（目標手先位置姿勢）を入力とし、複数の近似モデルを用いて解析的な処理を行い、解析解である複数の近似解を求める。
【０１２２】
次に、これらの複数の近似解の中から、以下の基準で最適なものを選択し、それを初期値とし、対象となるロボットのエンドポイント位置（手先位置姿勢）を求める。具体的には、指定された目標エンドポイント位置と、複数の近似解のそれぞれに対応するエンドポイント位置との間の第１の誤差（第１の差異）を求める。また、それぞれのエンドポイント位置（手先位置姿勢）におけるヤコビアンを求める。そして、上記の第１の誤差と上記のヤコビアンにより、角関節角の補正を行う。次に、補正された関節角により、補正エンドポイント（補正手先位置姿勢）を求める。そして、目標エンドポイント位置と補正エンドポイント位置との間の第２の誤差（第２の差異）を求める。そして、第２の誤差が最も小さいものに対応する近接解を初期値とし、反復法により、対象となるロボットの逆運動学の解を求める。
【０１２３】
この第３の実施例の手法は、近似解を与える複数個の近似モデルを用意するものである。つまり、複数の近似モデルを用意し、更に、その中のどの近似モデルが良いかという判断を、反復法の最初の１ステップ分の処理を行って、その結果が最も目標に近いものを選択するという手法である。単純に、近似解の結果が目標に近いか遠いではなく、真の解に効率良く近づく可能性が高そうな解を、上記の１ステップ分の処理の結果に基づいて、選択するという手法である。
【０１２４】
まず、ヤコビアンについて簡単に説明する。簡単化のために１次元の場合として説明する。図１４（Ａ）において、ＴＲは、エンドポイント位置に対する関節角の解を表すものであり、真正モデルに対応するものである。近似解に対応するＰ１でのヤコビアンは、Ｐ１における接線（微分値）ＴＧに相当する。このヤコビアンを用い、目標位置（目標エンドポイント位置）へ移動するための角度変位を求めると、図１４（Ｂ）のようになる。具体的には、ヤコビアンを表す接線ＴＧと目標位置（目標エンドポイント位置）との交点Ｐ２から、補正近似解が求まる。この補正近似解は、近似解よりもＰ３の解に近くなっており、目標位置と補正位置（補正エンドポイント位置）と間の誤差は小さい。一方、図１４（Ｃ）では、近似解に対応するＰ４でのヤコビアンに相当する接線ＴＧにより、Ｐ５の補正近似解が求まる。図１４（Ｃ）では、図１４（Ｂ）に比べて、目標位置と補正位置との間の誤差は大きくなっている。また図１４（Ｃ）では、その後の反復演算処理の収束も悪いと考えられる。即ち、補正近似解のところでのヤコビアンを求めて更に補正を行うと、ヤコビアンに対応する接線と目標位置との交点は、解から離れる方向になるため、反復演算処理の収束が悪くなる。
【０１２５】
解の良し悪しを判定することは非常に難しい。例えば、単純に最初の近似解の誤差の大小だけでは、判断を誤る可能性が高い。一方、第３の実施例の手法のようにヤコビアンで一度補正をした後の誤差で判断する手法も、必ずしも最良の手法であるとは言うことはできない。しかし、実験的には、解への収束性の良い場合は、最初の２、３回の補正で真の解の近傍に急速に接近する。一方、収束性の悪い場合には、そのような特性は見られない。従って、この意味においては、本実施形態の第３の実施例の手法のように、補正を１回行った後の誤差を用いて判断する手法は有効であると考えられる。
【０１２６】
図１５は第３の実施例の詳細な処理を表すフローチャートである。まず、目標エンドポイント位置を取得する（Ｓ３１）。次に、全ての近似モデルについての処理を行ったか否かを判断し、行っていない場合には近似モデルを１つ選択する（Ｓ３２、Ｓ３３）。そして、第１の実施例と同様に、選択された近似モデルにより近似解（近似解析解）を求める（Ｓ３４）。
【０１２７】
次に、解は得られたか否かを判断し（Ｓ３５）、得られなかった場合にはステップＳ３２に戻る。一方、得られた場合には、第２の実施例と同様に、得られた近似解と真正モデルを用いて、エンドポイント位置を求める（Ｓ３６）。具体的には、真正モデルの各関節角に近似解を設定し、フォワードキネマティクス処理により、近似解によるエンドポイント位置を求める。具体的には図１４（Ｂ）のＰ１における近似位置を求める。更に、得られた近似解と真正モデルを用いて、ヤコビアン（逆ヤコビアン）を求める（Ｓ３７）。これは図１４（Ｂ）のＰ１における接線ＴＧを求めることに相当する。
【０１２８】
次に、求めたエンドポイント位置と目標エンドポイント位置との間の第１の誤差と、ヤコビアン（逆ヤコビアン）を用いて、関節角を補正する（Ｓ３８）。これは、図１４（Ｂ）において、近似位置（近似解によるエンドポイント位置）と目標位置（目標エンドポイント位置）と接線ＴＧ（ヤコビアン）により補正近似解（補正後の関節角）を求めることに相当する。
【０１２９】
次に、補正後の関節角と真正モデルを用いて、補正エンドポイント位置を求める（Ｓ３９）。これは、図１４（Ｂ）において、ＴＲ（真正モデル）と、補正近似解（補正後の関節角）から、補正位置（補正エンドポイント位置）を求めることに相当する。そして、補正エンドポイント位置と目標エンドポイント位置との間の第２の誤差と、補正後の関節角を記憶部に記憶する（Ｓ４０）。即ち図１４（Ｂ）の目標位置と補正位置の間の誤差と、補正近似解を記憶する。
【０１３０】
ステップＳ３２で全ての近似モデルについての処理が行われたと判断されると、最も目標エンドポイント位置に近い解を与えた関節角を、初期値として選択する（Ｓ４１）。具体的には、図１４（Ｂ）において、補正位置と目標位置との間の第２の誤差が最も小さくなる補正近似解を、初期値として選択する。この選択処理は、ステップＳ４０で記憶された各近似モデルの第２の誤差と補正後の関節角を用いることで、実現できる。
【０１３１】
次に、選択した初期値と真正モデルを用いて、反復法により解を求める（Ｓ４２）。具体的には、真正モデルの各関節角に対して、選択した初期値を設定し、反復法による演算処理により逆運動学における解を求める。そして、解が収束した場合には、処理が成功したと判断し（Ｓ４３、Ｓ４４）、その時の解をロボットの各関節角に設定して、ロボット制御を行う。一方、解が得られなかった場合には、処理が失敗したと判断し、エラー処理等を行う（Ｓ４５）。なお、図１５のステップＳ３７〜Ｓ３９が、ステップＳ４２の反復演算処理の１ステップ分の処理に相当する。
【０１３２】
以上の第３の実施例の手法によれば、複数の近似モデルを用いることで、より真の解に近い補正関節角（補正近似解）を初期値に設定して、反復法により逆運動学の解を求めることができる。従って、冗長自由度を持ち、オフセットなどの複雑な構造を持つロボットに対して、より高度、高速な制御を実現することが可能になる。
【０１３３】
５．第４、第５の実施例
図１６、図１７に本実施形態の第４、第５の実施例のロボット制御システム（及びこれを含むロボットシステム）の構成例を示す。なお、ロボット制御システムの構成やその構成要件について、第１、第２、第３の実施例と重複する部分については、適宜、その説明を省略する。
【０１３４】
図１６、図１７のロボット制御システムは、目標エンドポイント位置取得部２０と、真正モデル記憶部２２と、近似モデル記憶部２４と、処理部３０を含む。またロボット制御部８０を含むことができる。また図１７では、更に動作履歴記憶部２６が設けられる。
【０１３５】
例えば図１６の第４の実施例の処理部３０は、近似解を真正モデルの関節角に設定することで、近似解によるエンドポイント位置を求める。具体的には、フォワードキネマティクス処理によりエンドポイント位置を求める。そして処理部３０は、近似解によるエンドポイント位置と現在のエンドポイント位置の中から、選択されたエンドポイント位置（手先位置姿勢）に対応する関節角を、初期値に設定して、反復演算処理を行うことで、ロボットの逆運動学における解を求める。例えば処理部３０は、現在エンドポイント位置取得部４５を有しており、この現在エンドポイント位置取得部４５が、ロボット制御部８０から、現在のエンドポイント位置や関節角を取得する。例えば、現在の姿勢でのアームのエンドエフェクター部の位置やアームの関節角を取得する。そして、初期値選択部４６が、近似解によるエンドポイント位置と現在のエンドポイント位置の中から選択されたエンドポイント位置に対応する関節角を、反復演算処理に使用される初期値として選択する。
【０１３６】
更に具体的には処理部３０は、近似解によるエンドポイント位置と目標エンドポイント位置との間の第１の誤差を求める。また現在のエンドポイント位置と目標エンドポイント位置との間の第２の誤差を求める。そして処理部３０は、第１の誤差の方が第２の誤差よりも小さい場合には、近似解によるエンドポイント位置に対応する関節角（近似解である関節角）を、初期値に設定する。一方、第２の誤差の方が第１の誤差よりも小さい場合には、現在のエンドポイント位置に対応する関節角（現在のアームの関節角）を、初期値に設定する。
【０１３７】
或いは、処理部３０は、ロボットのポイント・ツー・ポイント制御が行われた場合には、近似解によるエンドポイント位置に対応する関節角を、初期値に設定する。一方、ロボットのコンティニュアス制御が行われた場合には、現在のエンドポイント位置に対応する関節角を、初期値に設定する。
【０１３８】
ここで、ポイント・ツー・ポイント制御は、ＰＴＰ制御と呼ばれるものであり、例えば飛び飛びの点のポーズ（姿勢）だけを指定した命令により行われる制御である。例えば動作経路上の代表地点を教示し、地点のみを制御し、各地点に向かう動作経路（移動経路）については問題としない制御である。一方、コンティニュアス制御とは、ＣＰ制御と呼ばれるものであり、例えば全軌道又は全経路を指定した命令により行われる制御である。例えば、定められた地点間を連続的に補間し、ロボットアームを追従させる。この制御を行う場合には、連続した動作経路を教示することになる。
【０１３９】
図１７の第５の実施例のロボット制御システムは、動作履歴記憶部２６を更に含む。この動作履歴記憶部２６は、過去のエンドポイントの動作履歴位置である動作履歴エンドポイント位置やその時の関節角を記憶する。例えば動作履歴記憶部２６はＦＩＦＯにより構成され、各タイミングでのエンドポイント位置や関節角が動作履歴データとしてＦＩＦＯに順次記憶されて行く。
【０１４０】
そして図１７の処理部３０は、近似解によるエンドポイント位置と、現在エンドポイント位置取得部４５からの現在のエンドポイント位置と、動作履歴記憶部２６からの動作履歴エンドポイント位置の中から、選択されたエンドポイント位置に対応する関節角を、初期値に設定して、反復演算処理を行うことで、ロボットの逆運動学における解を求める。この初期値の選択は初期値選択部４６により行われる。
【０１４１】
具体的には処理部３０は、近似解によるエンドポイント位置と目標エンドポイント位置との間の第１の誤差を求める。また現在のエンドポイント位置と目標エンドポイント位置との間の第２の誤差を求める。更に、動作履歴エンドポイント位置と目標エンドポイント位置との間の第３の誤差を求める。そして処理部３０は、第１の誤差の方が第２の誤差、第３の誤差よりも小さい場合には、近似解によるエンドポイント位置に対応する関節角を、初期値に設定する。また第２の誤差の方が第１の誤差、第３の誤差よりも小さい場合には、現在のエンドポイント位置に対応する関節角を、初期値に設定する。また、第３の誤差の方が第１の誤差、第２の誤差よりも小さい場合には、動作履歴エンドポイント位置に対応する関節角を、初期値に設定する。即ち、近似解によるエンドポイント位置と、現在のエンドポイント位置と、動作履歴エンドポイント位置の中から、目標エンドポイント位置との誤差が最小となるエンドポイント位置を選択して、そのエンドポイント位置に対応する関節角（当該手先位置姿勢時の関節角）を、反復演算処理における初期値に設定する。
【０１４２】
次に本実施形態の第４、第５の実施例の手法の詳細について説明する。第４の実施例の手法では、指定された目標エンドポイント位置（目標手先位置姿勢）と現在のエンドポイント位置（現在の手先位置姿勢）を比較し、それらの間の差異（誤差）が小さい場合には、現在のエンドポイント位置での関節角を、初期値として反復法により解を求める。具体的には、近似解によるエンドポイント位置と現在のエンドポイント位置とのどちらが目標エンドポイント位置に近いかを判断し、近い方の関節角を初期値として採用する。一方、第５の実施例では、ロボットは、過去にとったエンドポイント位置（手先位置姿勢）とその際の関節角を所定個数だけ記憶する履歴データを持ち、指定された目標エンドポイント位置（目標手先位置指定）と履歴データを比較し、その差異（誤差）が最小、且つ、その差異が所定の差異よりも小さい場合、その最小である履歴データを初期値として、反復法により解を求める。即ち、過去の動作履歴を記憶する動作履歴記憶部２６を有し、近似モデルによる近似解、現在位置、動作履歴の中から、目標位置に最も近いものを初期値として採用する。
【０１４３】
例えば図１８（Ａ）は、近似解によるエンドポイント位置の関節角が初期値として設定される場合の例である。例えばＰＴＰ制御（ポイント・ツー・ポイント制御）では、ＰＴＰ制御によるジャンプ前の現在のエンドポイント位置と、ジャンプ後の目標エンドポイント位置との間は距離的に離れている場合が多く、その間の動作経路は問わない制御となる。この場合には図１８（Ａ）に示すように、近似解である関節角を初期値に設定して、逆運動学の反復演算処理を行う。このようにすれば、ＰＴＰ制御のように現在のエンドポイント位置と目標エンドポイント位置の間が距離的に離れる場合に、近似解により目標エンドポイント位置に近づいて、残りの誤差を反復演算処理により修正することが可能になる。
【０１４４】
図１８（Ｂ）は、現在のエンドポイント位置に対応する関節角（現在のアームの関節角）が初期値として設定される場合の例である。例えばＣＰ制御（コンティニュアス制御）では、現在のエンドポイント位置と目標エンドポイント位置との間が距離的に近い場合が多く、動作経路の指定地点が細かく設定される。この場合には図１８（Ｂ）に示すように、現在のエンドポイント位置に対応する関節角を初期値に設定して、逆運動学の反復演算処理を行う。このようにすれば、ＣＰ制御のように現在のエンドポイント位置と目標エンドポイント位置の間が距離的に近くて、細かく制御される場合に、近似解を初期値として用いる場合に比べて、より効率的に短時間で逆運動学の解を求めることが可能になる。
【０１４５】
図１８（Ｃ）は、動作履歴エンドポイント位置に対応する関節角が初期値として設定される場合の例である。例えば、過去の動作履歴エンドポイント位置の中に、近似解によるエンドポイント位置よりも、目標エンドポイント位置に近いエンドポイント位置がある場合には、その動作履歴エンドポイント位置を選択する。そして、選択された動作履歴エンドポイント位置に対応する関節角を初期値に設定して、逆運動学の反復演算処理を行う。このようにすることで、場合によっては、近似解を初期値に設定する場合に比べて、より効率的に短時間で逆運動学の解を求めることも可能になる。
【０１４６】
図１９は、第４の実施例の詳細な処理を示すフローチャートである。まず、目標エンドポイント位置を取得する（Ｓ５１）。そして、第１の実施例と同様に、近似モデルにより近似解を求める（Ｓ５２）。
【０１４７】
次に、解が得られたか否かを判断し（Ｓ５３）、得られなかった場合には、現在の関節角（現在のエンドポイント位置に対応する関節角）を初期値として選択する（Ｓ６２）。一方、解が得られた場合には、得られた近似解を初期値として用いて、反復法で解を求める（Ｓ５４）。そして、解が得られたか否かを判断し（Ｓ５５）、得られなかった場合には、現在の関節角を初期値として選択する（Ｓ６２）。一方、得られた場合には、得られた近似解と真正モデルにより、近似解によるエンドポイント位置を求める（Ｓ５６）。例えばフォワードキネマティクス処理によりエンドポイント位置を求める。
【０１４８】
次に、求められたエンドポイント位置と目標エンドポイント位置との間の第１の誤差を求める（Ｓ５７）。また、現在のエンドポイント位置を取得し（Ｓ５８）、現在のエンドポイント位置と目標エンドポイント位置との間の第２の誤差を求める（Ｓ５９）。
【０１４９】
次に、ステップＳ５７で求められた第１の誤差と、ステップＳ５９で求められた第２の誤差を比較する（Ｓ６０）。そして、第１の誤差の方が第２の誤差よりも小さい場合には、図１８（Ａ）に示すように近似解を初期値として選択する（Ｓ６１）。一方、第２の誤差の方が第１の誤差よりも小さい場合には、図１８（Ｂ）に示すように現在の関節角を初期値として選択する（Ｓ６２）。
【０１５０】
図１９は、第５の実施例の詳細な処理を示すフローチャートである。図１９のステップＳ７１〜Ｓ７９の処理は、図１８のステップＳ５１〜Ｓ５９の処理と同様であるため説明を省略する。
【０１５１】
ステップＳ７９の後、動作履歴エンドポイント位置と目標エンドポイント位置との間の誤差を、第３の誤差として求める（Ｓ８０）。次に、ステップＳ７７で求められた第１の誤差が最小か否かを判断し（Ｓ８１）、第１の誤差が最小である場合には、図１８（Ａ）に示すように近似解の関節角を初期値として選択する（Ｓ８２）。一方、第１の誤差が最小ではない場合には、第２の誤差が最小か否かを判断する（Ｓ８３）。そして、第２の誤差が最小である場合には、図１８（Ｂ）に示すように現在の関節角を初期値として選択する（Ｓ８４）。一方、第２の誤差が最小ではない場合には、誤差最小の動作履歴データを初期値として選択する（Ｓ８５）。例えば図１８（Ｃ）において、複数の動作履歴エンドポイント位置のうち、目標エンドポイント位置との間の誤差が最小となる動作履歴エンドポイント位置を選択し、選択された動作履歴エンドポイント位置に対応する関節角を初期値として選択する。
【０１５２】
なお、上記のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語（エンドポイント位置、近似解等）と共に記載された用語（手先位置姿勢、近似解析解等）は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また本実施形態の実施例及び変形例の全ての組み合わせも、本発明の範囲に含まれる。またロボット制御システム、ロボットシステムの構成・動作、近似解の演算手法、逆運動学の解の演算手法、近似モデルや真正モデルの構成等も、本実施形態で説明したものに限定されず、種々の変形実施が可能である。
【符号の説明】
【０１５３】
ＭＴ  真正モデル、ＭＡ（ＭＡ-1〜ＭＡｍ）  近似モデル、
１０  力覚センサー、１２  力制御部（インピーダンス制御部）、
２０  目標エンドポイント位置取得部、２２  真正モデル記憶部、
２４  近似モデル記憶部、２６  動作履歴記憶部、
３０  処理部、４０（４０-1〜４０-m）  近似解演算部、
４２  フォワードキネマティクス処理部、４３  目標エンドポイント位置修正部、
４４  近似解選択部、４５  現在エンドポイント位置取得部、４６  初期値選択部、
５０  反復演算処理部、５２  収束判定部、５４  エラー処理部、
６０  目標値出力部、６２  軌道生成部、６４  インバースキネマティクス処理部、
８０  ロボット制御部、８２-1〜８２-N  駆動制御部、
１００  ロボット、１０２-1〜１０２-N  駆動部、
３００  制御装置、３１０  ロボット本体、３２０  アーム、３３０  ハンド
【図１】
2013132731.tif 000006
【図２】
2013132731.tif 000007
【図３】
2013132731.tif 000008
【図４】
2013132731.tif 000009
【図５】
2013132731.tif 000010
【図６】
2013132731.tif 000011
【図７】
2013132731.tif 000012
【図８】
2013132731.tif 000013
【図９】
2013132731.tif 000014
【図１０】
2013132731.tif 000015
【図１１】
2013132731.tif 000016
【図１２】
2013132731.tif 000017
【図１３】
2013132731.tif 000018
【図１４】
2013132731.tif 000019
【図１５】
2013132731.tif 000020
【図１６】
2013132731.tif 000021
【図１７】
2013132731.tif 000022
【図１８】
2013132731.tif 000023
【図１９】
2013132731.tif 000024
【図２０】
2013132731.tif 000025