(57)【要約】【課題】ジンバルリンクを有するロボットの関節機構による動作を簡易なアルゴリズムで高精度に解析することができるロボットの関節機構の制御装置を提供する。【解決手段】固定位置算出手段により、第１モータ１９１および第２モータ１９２のモータ角θ１およびθ２から、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂを算出しておき、仮ロッド長算出手段１２が、一端位置１７１ａ，１７２ｂの前回値を用いて、第１ロッド１７１および第２ロッド１７２の仮ロッド長Ｌ´を算出する。そして、ロッド不定位置決定手段１６が、この仮ロッド長Ｌ´と実際のロッド長Ｌとの偏差であるロッド偏差ΔＤを０に収束させることで、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａを決定する。最終角度出力手段１７が、一端位置１７１ａ，１７２ａから、メインリンク１５０の姿勢角α，βや手１８０の姿勢角を算出して出力する。【選択図】図１
2011131304.tif 000002
【特許請求の範囲】
【請求項１】
互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
前記ジンバルリンクの一方の回転軸に連結された第１ロッドおよび第２ロッドと、
前記第１ロッドを変位させる第１アクチュエータと、
前記第２ロッドを変位される第２アクチュエータと
を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
前記第１アクチュエータの所定時間当たりの変化量である第１アクチュエータ変化量および、前記第２アクチュエータの所定時間当たりの変化量である第２アクチュエータ変化量と、前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量との間の関係を規定する関数を逐次算出する関数算出手段と、
前記第１姿勢角変化量および第２姿勢角変化量から算出される前記第１ロッドおよび第２ロッドの一端位置と、該第１ロッドおよび第２ロッドの他端位置の前回値とのそれぞれの差として算出され、または、前記第１アクチュエータ変化量および第２アクチュエータ変化量から算出される前記第１ロッドおよび第２ロッドの他端位置と、該第１ロッドおよび第２ロッドの一端位置の前回値とのそれぞれの差として算出される仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
前記関数算出手段により算出された関数に基づく前記第１姿勢角変化量および第２姿勢角変化量または、前記第１アクチュエータ変化量および第２アクチュエータ変化量により前記前回値を修正して、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの位置を決定するロッド位置決定手段と
を備えることを特徴とするロボットの関節機構の制御装置。
【請求項２】
互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
前記ジンバルリンクの一方の回転軸に一端が連結された第１ロッドおよび第２ロッドと、
前記第１ロッドの他端に連結された第１モータと、
前記第２ロッドの他端に連結された第２モータと
を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
前記第１モータの所定時間当たりの変化量である第１モータ角変化量および、前記第２モータの所定時間当たりの変化量である第２モータ角変化量と、前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量との間の関係を規定したヤコビ行列を逐次算出する行列算出手段と、
前記第１モータ角変化量から前記第１ロッドの他端位置を逐次算出すると共に、前記第２モータ角変化量から前記第２ロッドの他端位置を逐次算出するロッド他端位置算出手段と、
前記ロッド他端位置算出手段により算出された第１ロッドおよび第２ロッドの他端位置と、該第１ロッドおよび第２ロッドの一端位置の前回値とのそれぞれの差である、仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
前記行列算出手段より算出されたヤコビ行列に基づいて算出される前記第１姿勢角変化量および第２姿勢角変化量により前記第１ロッドおよび第２ロッドの一端位置の前回値を修正することにより、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの一端位置を決定するロッド一端位置決定手段と、
前記ロッド一端位置決定手段より決定された第１ロッドおよび第２ロッドの一端位置から前記結合体の前記第１回転軸回りの第１姿勢角および第２回転軸回りの第２姿勢角を算出する姿勢角算出手段と
を備えることを特徴とするロボットの関節機構の制御装置。
【請求項３】
互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
前記ジンバルリンクの一方の回転軸に一端が連結された第１ロッドおよび第２ロッドと、
前記第１ロッドの他端に連結された第１モータと、
前記第２ロッドの他端に連結された第２モータと
を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量と、前記第１モータの所定時間当たりの変化量である第１モータ角変化量および、前記第２モータの所定時間当たりの変化量である第２モータ角変化量との間の関係を規定した逆ヤコビ行列を逐次算出する逆行列算出手段と、
前記第１姿勢角変化量および第２姿勢角変化量から前記第１ロッドおよび第２ロッドの一端位置を算出するロッド一端位置算出手段と、
前記ロッド一端位置算出手段により算出された第１ロッドおよび第２ロッドの一端位置と、該第１ロッドおよび第２ロッドの他端位置の前回値とのそれぞれの差である、仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
前記逆行列算出手段より算出された逆ヤコビ行列に基づいて算出される第１モータ角変化量および第２モータ角変化量により前記第１ロッドおよび第２ロッドの他端位置の前回値を修正することにより、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの他端位置を決定するロッド他端位置決定手段と、
前記ロッド他端位置決定手段より決定された第１ロッドおよび第２ロッドの他端位置から前記第１モータの第１モータ角および第２モータの第２モータ角を算出するモータ角算出手段と
を備えることを特徴とするロボットの関節機構の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ジンバルリンクを有するロボットの関節機構の制御装置に関する。
【背景技術】
【０００２】
従来、ジンバルリンクを有するロボットの関節機構としては、下記特許文献１に示すように、互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、ジンバルリンクの一方の回転軸に一端が連結された第１ロッドおよび第２ロッドと、第１ロッドの他端に連結された第１モータと、第２ロッドの他端に連結された第２モータとを備えて前記ジンバルリンクに結合した結合体を第１回転軸および第２回転軸回りに動作可能なロボットの関節機構が本願出願人により提案されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００１−３１５０８７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ここで、かかるシンバルリンクを有するロボットの関節機構では、第１モータおよび第２モータの２つのモータの回転角であるモータ角に応じて、ジンバルリンクがその第１回転軸および第２回転軸の２つの回転軸回りに変化する。
【０００５】
そのため、かかるジンバルリンクを有するロボットの関節機構に対して、第1モータおよび第２モータのモータ角から結合体の姿勢角を決定すること、また逆に、結合体の姿勢角から第１モータおよび第２モータのモータ角を決定することを、幾何学的に解析することは困難である。
【０００６】
すなわち、第１モータおよび第２モータのモータ角の変化量と結合体の姿勢角の変化量とをヤコビ行列により規定することができても、結合体の姿勢角（または、第１モータおよび第２モータのモータ角）を決定するためには、多数の条件が必要となり、幾何学的に解を一意に求めることは困難である。
【０００７】
さらに、仮に幾何学的に解を一意に求めた場合でもその値が前回値と比較して、突飛な値となる可能性があり、これにより関節機構の動作を完全に解析することはできない。
【０００８】
そこで、本発明は、以上の点に鑑み、ジンバルリンクを有するロボットの関節機構による動作を簡易なアルゴリズムで高精度に解析することができるロボットの関節機構の制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
第１発明のロボットの関節機構の制御装置は、互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
前記ジンバルリンクの一方の回転軸に連結された第１ロッドおよび第２ロッドと、
前記第１ロッドを変位させる第１アクチュエータと、
前記第２ロッドを変位される第２アクチュエータと
を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
前記第１アクチュエータの所定時間当たりの変化量である第１アクチュエータ変化量および、前記第２アクチュエータの所定時間当たりの変化量である第２アクチュエータ変化量と、前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量との間の関係を規定する関数を逐次算出する関数算出手段と、
前記第１姿勢角変化量および第２姿勢角変化量から算出される前記第１ロッドおよび第２ロッドの一端位置と、該第１ロッドおよび第２ロッドの他端位置の前回値とのそれぞれの差として算出され、または、前記第１アクチュエータ変化量および第２アクチュエータ変化量から算出される前記第１ロッドおよび第２ロッドの他端位置と、該第１ロッドおよび第２ロッドの一端位置の前回値とのそれぞれの差として算出される仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
前記関数算出手段により算出された関数に基づく前記第１姿勢角変化量および第２姿勢角変化量または、前記第１アクチュエータ変化量および第２アクチュエータ変化量により前記前回値を修正して、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの位置を決定するロッド位置決定手段と
を備えることを特徴とする。
【００１０】
第１発明のロボットの関節機構の制御装置によれば、第１ロッドおよび第２ロッドの一端位置または他端位置を決定しておくと共に、未定の第１ロッドおよび第２ロッドの他端位置または一端位置の前回値を用いて、第１ロッドおよび第２ロッドの仮ロッド長を算出しておく。そして、この仮ロッド長を第１ロッドおよび第２ロッドのロッド長という普遍的な値へ収束させることで、第１ロッドおよび第２ロッドの一端位置を複雑な幾何学計算を行うことなく、簡易に決定することができる。
【００１１】
また、ここでの前記修正量は、前記関数に基づく第１アクチュエータ変化量および第２アクチュエータ変化量または、前記結合体の前記第１姿勢角変化量および第２姿勢角変化量に基づくため、高精度に第１ロッドおよび第２ロッドの位置を決定することができる。
【００１２】
さらに、第１ロッドおよび第２ロッドの位置は、その前回値に基づいて、修正されるため、第１ロッドおよび第２ロッドの位置に連続性を持たせることができる。
【００１３】
このように、第１発明のロボットの関節機構の制御装置によれば、ジンバルリンクを有するロボットの関節機構の動作を簡易なアルゴリズムで高精度に解析することができる。
【００１４】
第２発明のロボットの関節機構の制御装置は、互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
前記ジンバルリンクの一方の回転軸に一端が連結された第１ロッドおよび第２ロッドと、
前記第１ロッドの他端に連結された第１モータと、
前記第２ロッドの他端に連結された第２モータと
を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
前記第１モータの所定時間当たりの変化量である第１モータ角変化量および、前記第２モータの所定時間当たりの変化量である第２モータ角変化量と、前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量との間の関係を規定したヤコビ行列を逐次算出する行列算出手段と、
前記第１モータ角変化量から前記第１ロッドの他端位置を逐次算出すると共に、前記第２モータ角変化量から前記第２ロッドの他端位置を逐次算出するロッド他端位置算出手段と、
前記ロッド他端位置算出手段により算出された第１ロッドおよび第２ロッドの他端位置と、該第１ロッドおよび第２ロッドの一端位置の前回値とのそれぞれの差である、仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
前記行列算出手段より算出されたヤコビ行列に基づいて算出される前記第１姿勢角変化量および第２姿勢角変化量により前記第１ロッドおよび第２ロッドの一端位置の前回値を修正することにより、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの一端位置を決定するロッド一端位置決定手段と、
前記ロッド一端位置決定手段より決定された第１ロッドおよび第２ロッドの一端位置から前記結合体の前記第１回転軸回りの第１姿勢角および第２回転軸回りの第２姿勢角を算出する姿勢角算出手段と
を備えることを特徴とする。
【００１５】
第２発明のロボットの関節機構の制御装置によれば、第１モータおよび第２モータのモータ角変化量から、第１ロッドおよび第２ロッドの他端位置を決定しておくと共に、第１ロッドおよび第２ロッドの一端位置の前回値を用いて、第１ロッドおよび第２ロッドの仮ロッド長を算出しておく。そして、この仮ロッド長を第１ロッドおよび第２ロッドのロッド長という普遍的な値へ収束させることで、第１ロッドおよび第２ロッドの一端位置を複雑な幾何学計算を行うことなく、簡易に決定することができる。
【００１６】
また、ここでの修正量は、ヤコビ行列から算出される結合体の姿勢角の変化量に基づくため、高精度に第１ロッドおよび第２ロッドの一端位置を決定することができる。
【００１７】
さらに、第１ロッドおよび第２ロッドの一端位置は、その前回値に基づいて、修正されるため、第１ロッドおよび第２ロッドの一端位置に連続性を持たせることができ、ひいては、この一端位置により決定される結合体の姿勢にも連続性を持たせることができる。
【００１８】
このように、第２発明のロボットの関節機構の制御装置によれば、ジンバルリンクを有するロボットの関節機構の動作を簡易なアルゴリズムで高精度に解析することができる。
【００１９】
第３発明のロボットの関節機構の制御装置は、互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
前記ジンバルリンクの一方の回転軸に一端が連結された第１ロッドおよび第２ロッドと、
前記第１ロッドの他端に連結された第１モータと、
前記第２ロッドの他端に連結された第２モータと
を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量と、前記第１モータの所定時間当たりの変化量である第１モータ角変化量および、前記第２モータの所定時間当たりの変化量である第２モータ角変化量との間の関係を規定した逆ヤコビ行列を逐次算出する逆行列算出手段と、
前記第１姿勢角変化量および第２姿勢角変化量から前記第１ロッドおよび第２ロッドの一端位置を算出するロッド一端位置算出手段と、
前記ロッド一端位置算出手段により算出された第１ロッドおよび第２ロッドの一端位置と、該第１ロッドおよび第２ロッドの他端位置の前回値とのそれぞれの差である、仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
前記逆行列算出手段より算出された逆ヤコビ行列に基づいて算出される第１モータ角変化量および第２モータ角変化量により前記第１ロッドおよび第２ロッドの他端位置の前回値を修正することにより、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの他端位置を決定するロッド他端位置決定手段と、
前記ロッド他端位置決定手段より決定された第１ロッドおよび第２ロッドの他端位置から前記第１モータの第１モータ角および第２モータの第２モータ角を算出するモータ角算出手段と
を備えることを特徴とする。
【００２０】
第３発明のロボットの関節機構の制御装置によれば、結合体の姿勢角から、第１ロッドおよび第２ロッドの一端位置を決定しておくと共に、第１ロッドおよび第２ロッドの他端位置の前回値を用いて、第１ロッドおよび第２ロッドの仮ロッド長を算出しておく。そして、この仮ロッド長を第１ロッドおよび第２ロッドのロッド長という普遍的な値へ収束させることで、第１ロッドおよび第２ロッドの他端位置を複雑な幾何学計算を行うことなく、簡易に決定することができる。
【００２１】
また、ここでの修正量は、逆ヤコビ行列から算出される第１モータおよび第２モータのモータ角の変化量に基づくため、高精度に第１ロッドおよび第２ロッドの他端位置を決定することができる。
【００２２】
さらに、第１ロッドおよび第２ロッドの他端位置は、その前回値に基づいて、修正されるため、第１ロッドおよび第２ロッドの他端位置に連続性を持たせることができ、ひいては、この他端位置により決定される第１モータおよび第２モータのモータ角にも連続性を持たせることができる。
【００２３】
このように、第３発明のロボットの関節機構の制御装置によれば、ジンバルリンクを有するロボットの関節機構の動作を簡易なアルゴリズムで高精度に解析することができる。
【図面の簡単な説明】
【００２４】
【図１】本実施形態のロボットの関節機構の制御装置の構成説明図。
【図２】図１の制御装置が適用される関節機構を示す図。
【図３】図１の制御装置が適用される関節機構を示す分解図。
【図４】図１の制御装置が適用される関節機構を示す図。
【図５】図１のロボットの関節機構の制御装置の処理内容（キネマティック計算処理）を示すフローチャート。
【図６】図１のロボットの関節機構の制御装置の処理内容（逆キネマティック計算処理）を示すフローチャート。
【発明を実施するための形態】
【００２５】
次に、本発明のロボットの関節機構の制御装置の実施形態について、図１〜図６を参照して説明する。
【００２６】
図１に示すように、ロボットの関節機構の制御装置１は、関節機構を動作させるモータと関節機構に結合された結合体の動作との間の関係を解析する装置であって、ロッド固定位置算出手段１１と、仮ロッド長算出手段１２と、ロッド偏差算出手段１３と、ヤコビ・逆ヤコビ行列算出手段１４と、修正量算出手段１５と、ロッド不定位置決定手段１６と、最終角度出力手段１７とを備える。
【００２７】
なお、以下、モータの動作から結合体の動作を解析することをキネマティクス演算、結合体の動作からモータへの動作の解析を逆キネマティクス演算という。
【００２８】
図２に示すように、制御装置１が適用されるロボットの関節機構１００は、人間型ロボットの手首の関節機構であって、腕のリンク１２０と、腕のリンク１２０に連結された手首の関節部１３０と、手首の関節部１３０に連結された結合体である手１８０と、手１８０の縦振りおよび横振り動作を行う駆動装置１９０とを備える。
【００２９】
具体的には、図３に示すように、腕のリンク１２０に形成された対向部１２１ａ，１２１ａに支持されたジンバルリンク１４０と、シンバルリンク１４０に連結されたメインリンク１５０と、メインリンク１５０に連結された第１ロッド１７１および第２ロッド１７２と、第１ロッド１７１および第２ロッド１７２にそれぞれ連結された駆動装置１９０の第１モータ１９１および第２モータ１９２と備える。
【００３０】
ジンバル１４０は、腕のリンク１２０に形成された対向部１２１ａ，１２１ａに縦軸部１４１が支持されて、ジンバルリンク１４０にメインリンク１５０を介して連結された手１８０が横振り自在となっている。また、ジンバルリンクの横軸部１４２がメインリンク１５０に連結されており、手１８０が縦振り自在となっている。なお、手１８０は、メインリンク１５０の副軸部にサブリンク１６０を介して連結されている。
【００３１】
第１ロッド１７１および第２ロッド１７２は、一端がメインリンク１５０にユニバーサルジョイント１５１および１５２を介して連結され、他端が第１モータ１９１の出力アーム１９１ａおよび第２モータ１９２の出力アーム１９２ａに連結されている。
【００３２】
次に、ロボットの関節機構の制御装置１の各構成要素について、図４を参照して具体的に説明する。なお、以降の座標原点は、図４（ａ）および（ｂ）に示すように、第１モータ１７１および第モータ１７２の回転軸上でこれらの中点とする。
【００３３】
ロッド固定位置算出手段１１は、キネマティック演算では、第１モータ１９１のモータ角θ１と第１ロッド１７１の他端位置１７１ｂとを変換すると共に、第２モータ１９２のモータ角θ２と第２ロッド１７２の他端位置１７２ｂとを変換する。
【００３４】
また、ロッド固定位置算出手段１１は、逆キネマティック演算では、手１８０の姿勢角であるメインリンク１５０の姿勢角α，βと、第１ロッド１７１の一端位置１７１ａおよび第２ロッド１７２の一端位置１７２ａとを相互に変換する。
【００３５】
なお、ロッド固定位置算出手段１１は、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂと、これらの一端位置１７１ａ，１７２ａをベクトル演算により特定するものとする。
【００３６】
仮ロッド長算出手段１２は、キネマティック演算では、第１モータ１９１および第２モータ１９２のモータ角θ１，θ１が与えられた場合に、ロッド固定位置算出手段１１により算出される第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂ（固定位置）と、第１ロッド１７１および第２ロッド１７２の一端位置の前回値（不定位置）との差を、それぞれ第１ロッドおよび第２ロッドの仮のロッド長として仮第１ロッド長Ｌ１´および仮第２ロッド長Ｌ２´を算出する。
【００３７】
また、仮ロッド長算出手段１２は、逆キネマティック演算では、手１８０の姿勢角であるメインリンク１５０の姿勢角α，βが与えられた場合に、ロッド固定位置算出手段１１により算出される第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａ（固定位置）と、第１ロッド１７１および第２ロッド１７２の他端位置の前回値（不定位置）との差を、それぞれ第１ロッドおよび第２ロッドの仮のロッド長として仮第１ロッド長Ｌ１´および仮第２ロッド長Ｌ２´を算出する。
【００３８】
ロッド偏差算出手段１３は、（キネマティック演算、逆キネマティック演算のいずれの場合にも）仮ロッド長算出手段１２により算出された仮第１ロッド長Ｌ１´と、実際の第１ロッドのロッド長Ｌ１との偏差ΔＤ１を算出すると共に、仮ロッド長算出手段１２により算出された仮第２ロッド長Ｌ２´と、実際の第２ロッドのロッド長Ｌ２との偏差ΔＤ２を算出する。
【００３９】
ヤコビ・逆ヤコビ行列算出手段１４は、キネマティック演算では、第１モータ１９１および第２モータ１９２のモータ角の微小時間変化量Δθ１，Δθ２と、メインリンク１５０の姿勢角度の微小時間変化量Δα，Δβとが線形関係となることに基づいて、Δθ１，Δθ２からΔα，Δβへの関係の比例関数行列をヤコビ行列として算出する。また、逆キネマティック演算では、Δα，ΔβからΔθ１，Δθ２への関係の比例関数行列を逆ヤコビ行列として算出する。
【００４０】
具体的に、ヤコビ・逆ヤコビ行列算出手段１４は、ロボットの関節機構１００の各リンクの長さと各ジョイントの回転行列を逐次的に求めることでヤコビ行列を算出する。また、ヤコビ・逆ヤコビ行列算出手段１４は、ヤコビ行列に基づいて、逆ヤコビ行列の逆行列として数学的に算出する。
【００４１】
修正量算出手段１５は、キネマティック演算では、ヤコビ・逆ヤコビ行列算出手段１４により算出されたヤコビ行列から算出される、メインリンク１５０の姿勢角度の微小時間変化量Δα，Δβを、第１ロッド１７１の一端位置１７１ａおよび第２ロッド１７２の一端位置１７２ａの微小変化量に変換する。そして、変換した微小変化量を、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａの前回値に加算する修正量とする。
【００４２】
また、修正量算出手段１５は、逆キネマティック演算では、ヤコビ・逆ヤコビ行列算出手段１４により算出された逆ヤコビ行列から算出される、第１モータおよび第２モータのモータ角の微小時間変化量Δθ１，Δθ２を、第１ロッド１７１の他端位置１７１ｂおよび第２ロッド１７２の他端位置１７２ｂの微小変化量に変換する。そして、変換した微小変化量を、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂの前回値に加算する修正量とする。
【００４３】
ロッド不定位置決定手段１６は、キネマティック演算では、修正量算出手段１５により算出された修正量を第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａの前回値に加算することで、第１ロッドおよび第２ロッドのロッド偏差ΔＤ１，ΔＤ２が０またはこれに相当する微小値に収束する場合の一端位置１７１ａ，１７２ａを、その今回値として決定する。
【００４４】
また、ロッド不定位置決定手段１６は、逆キネマティック演算では、修正量算出手段１５により算出された修正量を第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂの前回値に加算することで、第１ロッドおよび第２ロッドのロッド偏差ΔＤ１，ΔＤ２が０（またはこれに相当する微小値）に収束する場合の他端位置１７１ｂ，１７２ｂを、その今回値として決定する。
【００４５】
最終角度出力手段１７は、キネマティック演算では、ロッド不定位置決定手段１６により決定された第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａ（今回値）から、メインリンク１５０の姿勢角α，β（必要に応じて、これを変換した手１８０の姿勢角）を算出して出力する。
【００４６】
また、最終角度出力手段１７は、逆キネマティック演算では、ロッド不定位置決定手段１６により決定された第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂ（今回値）から、第１モータ１９１および第２モータ１９２のモータ角θ１およびθ２を算出して出力する。
【００４７】
次に、図５に示すフローチャートを参照して、本実施形態のロボットの関節機構の制御装置１によるキネマティック演算について説明する。
【００４８】
まず、ロボットの関節機構の制御装置１は、第１モータ１９１のモータ角θ１および第２モータ１９２のモータ角θ２が与えられると（図５／ＳＴＥＰ１０）、ロッド固定位置算出手段１１が、これに対応した第１ロッド１７１の他端位置１７１ｂおよび第２ロッド１７２の他端位置１７２ｂを算出する（図５／ＳＴＥＰ１１）。
【００４９】
次いで、仮ロッド長算出手段１２が、ＳＴＥＰ１１で算出した第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂと、一端位置１７１ａ，１７２ａの前回値との差を、それぞれ第１ロッド１７１および第２ロッド１７２の仮のロッド長（仮第１ロッド長Ｌ１´および仮第２ロッド長Ｌ２´）として算出する（図５／ＳＴＥＰ１２）。
【００５０】
次いで、ロッド偏差算出手段１３が、仮ロッド長算出手段１２により算出された仮第１ロッド長Ｌ１´と、実際の第１ロッドのロッド長Ｌ１との偏差ΔＤ１を算出すると共に、仮ロッド長算出手段１２により算出された仮第２ロッド長Ｌ２´と、実際の第２ロッドのロッド長Ｌ２との偏差ΔＤ２を算出する（図５／ＳＴＥＰ１３）。
【００５１】
次いで、ロッド不定位置決定手段１６が、ＳＴＥＰ１３で算出したロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっているか、すなわち、０（またはこれに相当する微小値）に収束しているかを判定する（図５／ＳＴＥＰ１４）。
【００５２】
そして、ロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっていない場合には（図５／ＳＴＥＰ１４・・ＮＯ）、ヤコビ・逆ヤコビ算出手段１４が、ヤコビ行列を算出する（図５／ＳＴＥＰ１５）。
【００５３】
次いで、修正量算出手段１５が、ＳＴＥＰ１５で算出したヤコビ行列に基づいて、メインリンク１５０の姿勢角度の微小時間変化量Δα，Δβを算出し、これに基づいて、第１ロッド１７１の一端位置１７１ａおよび第２ロッド１７２の一端位置１７２ａの修正量（微小変化量）を算出する（図５／ＳＴＥＰ１６）。
【００５４】
次いで、ロッド不定位置決定手段１６が、ＳＴＥＰ１６で算出した修正量を、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａおよび１７２ａに加算する修正を施して（図５／ＳＴＥＰ１７）、ＳＴＥＰ１２にリターンする。
【００５５】
そして、修正後の一端位置１７１ａおよび１７２ａに基づいて算出されるロッド偏差が収束範囲となるまでこれら一連の処理を繰り返し（図５／ＳＴＥＰ１２〜１７）、ロッド不定位置決定手段１６は、ロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっていると判定した場合には（図５／ＳＴＥＰ１４・・ＹＥＳ）、このときの第１ロッド１７１および第２ロッド１７２の一端位置１７１ａおよび１７２ａを今回値に決定する（図５／ＳＴＥＰ１８）。
【００５６】
次いで、最終角出力手段１７が、ＳＴＥＰ１８で決定された第１ロッド１７１および第２ロッド１７２の一端位置１７１ａおよび１７２ａから、メインリンク１５０の姿勢角α,β、さらには必要に応じて対応する手１８０の姿勢角を算出して、これらを出力し（図５／ＳＴＥＰ１９）、一連の処理を終了する。
【００５７】
以上が、本実施形態のロボットの関節機構の制御装置１によるキネマティック演算である。かかるキネマティクス演算によれば、第１モータ１９１および第２モータ１９２のモータ角θ１およびθ２から、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂを決定しておくと共に、一端位置１７１ａ，１７２ｂの前回値を用いて、
第１ロッド１７１および第２ロッド１７２の仮ロッド長Ｌ´を算出しておき、この仮ロッド長Ｌ´を実際のロッド長Ｌという普遍的な値へ収束させることで（ΔＤ→０）、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａを複雑な幾何学計算を行うことなく、簡易に決定することができる。
【００５８】
また、ここでの修正量は、ヤコビ行列から算出されるメインリンク１５０の姿勢角の変化量Δα，Δβに基づくため、高精度に第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａを決定することができる。
【００５９】
さらに、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａは、その前回値に基づいて、修正されるため、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａに連続性を持たせることができ、この一端位置により決定されるメインリンク１５０、さらには手１８０の動作にも連続性を持たせることができる。
【００６０】
次に、図６に示すフローチャートを参照して、本実施形態のロボットの関節機構の制御装置１による逆キネマティック演算について説明する。
【００６１】
まず、ロボットの関節機構の制御装置１は、メインリンク１５０の姿勢角α，β（手１８０の姿勢角が与えられた場合には、これをメインリンク１５０の姿勢角に変換した値）が与えられると、（図６／ＳＴＥＰ３０）、ロッド固定位置算出手段１１が、これに対応した第１ロッド１７１の一端位置１７１ａおよび第２ロッド１７２の一端位置１７２ａを算出する（図６／ＳＴＥＰ３１）。
【００６２】
次いで、仮ロッド長算出手段１２が、ＳＴＥＰ３１で算出した第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａと、他端位置１７１ｂ，１７２ｂの前回値との差を、それぞれ第１ロッド１７１および第２ロッド１７２の仮のロッド長（仮第１ロッド長Ｌ１´および仮第２ロッド長Ｌ２´）として算出する（図６／ＳＴＥＰ３２）。
【００６３】
次いで、ロッド偏差算出手段１３が、仮ロッド長算出手段１２により算出された仮第１ロッド長Ｌ１´と、実際の第１ロッドのロッド長Ｌ１との偏差ΔＤ１を算出すると共に、仮ロッド長算出手段１２により算出された仮第２ロッド長Ｌ２´と、実際の第２ロッドのロッド長Ｌ２との偏差ΔＤ２を算出する（図６／ＳＴＥＰ３３）。
【００６４】
次いで、ロッド不定位置決定手段１６が、ＳＴＥＰ３３で算出したロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっているか、すなわち、０（またはこれに相当する微小値）に収束しているかを判定する（図６／ＳＴＥＰ３４）。
【００６５】
そして、ロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっていない場合には（図６／ＳＴＥＰ３４・・ＮＯ）、ヤコビ・逆ヤコビ算出手段１４が、逆ヤコビ行列を算出する（図６／ＳＴＥＰ３５）。
【００６６】
次いで、修正量算出手段１５が、ＳＴＥＰ３５で算出した逆ヤコビ行列に基づいて、第１モータ１９１のモータ角の微小時間変化量Δθ１と、第２モータ１９２のモータ角の微小時間変化量Δθ２とを算出し、これに基づいて、第１ロッド１７１の他端位置１７１ｂおよび第２ロッド１７２の他端位置１７２ｂの修正量（微小変化量）を算出する（図６／ＳＴＥＰ３６）。
【００６７】
次いで、ロッド不定位置決定手段１６が、ＳＴＥＰ３６で算出した修正量を、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂおよび１７２ｂに加算する修正を施して（図６／ＳＴＥＰ３７）、ＳＴＥＰ３２にリターンする。
【００６８】
そして、修正後の他端位置１７１ｂおよび１７２ｂに基づいて算出されるロッド偏差が収束範囲となるまでこれら一連の処理を繰り返し（図６／ＳＴＥＰ３２〜３７）、ロッド不定位置決定手段１６は、ロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっていると判定した場合には（図６／ＳＴＥＰ３４・・ＹＥＳ）、このときの第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂおよび１７２ｂを今回値に決定する（図６／ＳＴＥＰ３８）。
【００６９】
次いで、最終角出力手段１７が、ＳＴＥＰ３８で決定された第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂおよび１７２ｂから、第１モータ１９１のモータ角θ１と第２モータ１９２のモータ角θ２とを算出して、これらを出力し（図６／ＳＴＥＰ１９）、一連の処理を終了する。
【００７０】
以上が、本実施形態のロボットの関節機構の制御装置１による逆キネマティック演算である。かかる逆キネマティクス演算によれば、手１８０の姿勢角やメインリンク１５０の姿勢角α，βから、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａを決定しておくと共に、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂの前回値を用いて、第１ロッド１７１および第２ロッド１７２の仮ロッド長Ｌ´を算出しておき、この仮ロッド長Ｌ´を第１ロッドおよび第２ロッドのロッド長Ｌという普遍的な値へ収束させることで、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂを複雑な幾何学計算を行うことなく、簡易に決定することができる。
【００７１】
また、ここでの修正量は、逆ヤコビ行列から算出される第１モータおよび第２モータのモータ角の変化量Δθ１，Δθ２に基づくため、高精度に第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂを決定することができる。
【００７２】
さらに、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂは、その前回値に基づいて、修正されるため、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂに連続性を持たせることができ、ひいては、この他端位置１７１ｂ，１７２ｂにより決定される第１モータおよび第２モータのモータ角θ１，θ２にも連続性を持たせることができる。
【００７３】
このように、本実施形態のロボットの関節機構の制御装置１によれば、ジンバルリンクを有するロボットの関節機構の動作を簡易なアルゴリズムで高精度に解析することができる。
【００７４】
尚、本実施形態においては、第１ロッド１７１および第２ロッド１７２の仮ロッド長Ｌ１´，Ｌ２´と、これらの実際のロッド長Ｌ１，Ｌ２との差から、ロッド偏差ΔＤ１，Ｄ２を０に収束させるようにして、第１ロッド１７１および第２ロッド１７２の不定位置（キネマティック演算では、一端位置１７１ａ，１７２ａであり、逆キネマティック演算では、他端位置１７１ｂ，１７２ｂ）を決定したが（図５／ＳＴＥＰ１４、図６／ＳＴＥＰ３４）、これに限定されるものではなく、仮ロッド長Ｌ１´，Ｌ２´を直接実際のロッド長Ｌ１，Ｌ２に収束させるようにしてもよい。
【００７５】
また、本実施形態では、第１ロッド１７１および第２ロッド１７２を、第１モータ１９１および第２モータ１９２により動作させる構成について説明したが、これに限定されるものではなく、例えば、アクチュエータにより第１ロッド１７１および第２ロッド１７２を動作させるように構成してもよい。
【符号の説明】
【００７６】
１‥ロボットの関節機構の制御装置、１１‥ロッド固定位置算出手段（ロッド他端位置算出手段、ロッド一端位置算出手段）、１２‥仮ロッド長算出手段、１３‥ロッド偏差算出手段、１４‥ヤコビ・逆ヤコビ算出手段（関数算出手段、行列算出手段、逆行列算出手段）、１５‥修正量算出手段、１６‥ロッド不定位置決定手段（ロッド位置決定手段、ロッド一端位置決定手段、ロッド他端位置決定手段）、１７‥最終角度出力手段（姿勢角算出手段、モータ角算出手段）、１４０‥ジンバルリンク、１７１‥第１ロッド、１７２‥第２ロッド、１９１‥第１モータ（第１アクチュエータ）、１９２‥第２モータ（第２アクチュエータ）。
【図１】
2011131304.tif 000003
【図２】
2011131304.tif 000004
【図３】
2011131304.tif 000005
【図４】
2011131304.tif 000006
【図５】
2011131304.tif 000007
【図６】
2011131304.tif 000008