JP5184502B2 - ロボットの関節機構の制御装置 - Google Patents

Description

本発明は、ジンバルリンクを有するロボットの関節機構の制御装置に関する。

従来、ジンバルリンクを有するロボットの関節機構としては、下記特許文献１に示すように、互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、ジンバルリンクの一方の回転軸に一端が連結された第１ロッドおよび第２ロッドと、第１ロッドの他端に連結された第１モータと、第２ロッドの他端に連結された第２モータとを備えて前記ジンバルリンクに結合した結合体を第１回転軸および第２回転軸回りに動作可能なロボットの関節機構が本願出願人により提案されている。

特開２００１−３１５０８７号公報

ここで、かかるシンバルリンクを有するロボットの関節機構では、第１モータおよび第２モータの２つのモータの回転角であるモータ角に応じて、ジンバルリンクがその第１回転軸および第２回転軸の２つの回転軸回りに変化する。

そのため、かかるジンバルリンクを有するロボットの関節機構に対して、第1モータおよび第２モータのモータ角から結合体の姿勢角を決定すること、また逆に、結合体の姿勢角から第１モータおよび第２モータのモータ角を決定することを、幾何学的に解析することは困難である。

すなわち、第１モータおよび第２モータのモータ角の変化量と結合体の姿勢角の変化量とをヤコビ行列により規定することができても、結合体の姿勢角（または、第１モータおよび第２モータのモータ角）を決定するためには、多数の条件が必要となり、幾何学的に解を一意に求めることは困難である。

さらに、仮に幾何学的に解を一意に求めた場合でもその値が前回値と比較して、突飛な値となる可能性があり、これにより関節機構の動作を完全に解析することはできない。

そこで、本発明は、以上の点に鑑み、ジンバルリンクを有するロボットの関節機構による動作を簡易なアルゴリズムで高精度に解析することができるロボットの関節機構の制御装置を提供することを目的とする。

第１発明のロボットの関節機構の制御装置は、互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
前記ジンバルリンクの一方の回転軸に連結された第１ロッドおよび第２ロッドと、
前記第１ロッドを変位させる第１アクチュエータと、
前記第２ロッドを変位される第２アクチュエータと
を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
前記第１アクチュエータの所定時間当たりの変化量である第１アクチュエータ変化量および、前記第２アクチュエータの所定時間当たりの変化量である第２アクチュエータ変化量と、前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量との間の関係を規定する関数を逐次算出する関数算出手段と、
前記第１姿勢角変化量および第２姿勢角変化量から算出される前記第１ロッドおよび第２ロッドの一端位置と、該第１ロッドおよび第２ロッドの他端位置の前回値とのそれぞれの差として算出され、または、前記第１アクチュエータ変化量および第２アクチュエータ変化量から算出される前記第１ロッドおよび第２ロッドの他端位置と、該第１ロッドおよび第２ロッドの一端位置の前回値とのそれぞれの差として算出される仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
前記関数算出手段により算出された関数に基づく前記第１姿勢角変化量および第２姿勢角変化量または、前記第１アクチュエータ変化量および第２アクチュエータ変化量により前記前回値を修正して、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの位置を決定するロッド位置決定手段と
を備えることを特徴とする。

第１発明のロボットの関節機構の制御装置によれば、第１ロッドおよび第２ロッドの一端位置または他端位置を決定しておくと共に、未定の第１ロッドおよび第２ロッドの他端位置または一端位置の前回値を用いて、第１ロッドおよび第２ロッドの仮ロッド長を算出しておく。そして、この仮ロッド長を第１ロッドおよび第２ロッドのロッド長という普遍的な値へ収束させることで、第１ロッドおよび第２ロッドの一端位置を複雑な幾何学計算を行うことなく、簡易に決定することができる。

また、ここでの前記修正量は、前記関数に基づく第１アクチュエータ変化量および第２アクチュエータ変化量または、前記結合体の前記第１姿勢角変化量および第２姿勢角変化量に基づくため、高精度に第１ロッドおよび第２ロッドの位置を決定することができる。

さらに、第１ロッドおよび第２ロッドの位置は、その前回値に基づいて、修正されるため、第１ロッドおよび第２ロッドの位置に連続性を持たせることができる。

このように、第１発明のロボットの関節機構の制御装置によれば、ジンバルリンクを有するロボットの関節機構の動作を簡易なアルゴリズムで高精度に解析することができる。

第２発明のロボットの関節機構の制御装置は、互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
前記ジンバルリンクの一方の回転軸に一端が連結された第１ロッドおよび第２ロッドと、
前記第１ロッドの他端に連結された第１モータと、
前記第２ロッドの他端に連結された第２モータと
を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
前記第１モータの所定時間当たりの変化量である第１モータ角変化量および、前記第２モータの所定時間当たりの変化量である第２モータ角変化量と、前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量との間の関係を規定したヤコビ行列を逐次算出する行列算出手段と、
前記第１モータ角変化量から前記第１ロッドの他端位置を逐次算出すると共に、前記第２モータ角変化量から前記第２ロッドの他端位置を逐次算出するロッド他端位置算出手段と、
前記ロッド他端位置算出手段により算出された第１ロッドおよび第２ロッドの他端位置と、該第１ロッドおよび第２ロッドの一端位置の前回値とのそれぞれの差である、仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
前記行列算出手段より算出されたヤコビ行列に基づいて算出される前記第１姿勢角変化量および第２姿勢角変化量により前記第１ロッドおよび第２ロッドの一端位置の前回値を修正することにより、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの一端位置を決定するロッド一端位置決定手段と、
前記ロッド一端位置決定手段より決定された第１ロッドおよび第２ロッドの一端位置から前記結合体の前記第１回転軸回りの第１姿勢角および第２回転軸回りの第２姿勢角を算出する姿勢角算出手段と
を備えることを特徴とする。

第２発明のロボットの関節機構の制御装置によれば、第１モータおよび第２モータのモータ角変化量から、第１ロッドおよび第２ロッドの他端位置を決定しておくと共に、第１ロッドおよび第２ロッドの一端位置の前回値を用いて、第１ロッドおよび第２ロッドの仮ロッド長を算出しておく。そして、この仮ロッド長を第１ロッドおよび第２ロッドのロッド長という普遍的な値へ収束させることで、第１ロッドおよび第２ロッドの一端位置を複雑な幾何学計算を行うことなく、簡易に決定することができる。

また、ここでの修正量は、ヤコビ行列から算出される結合体の姿勢角の変化量に基づくため、高精度に第１ロッドおよび第２ロッドの一端位置を決定することができる。

さらに、第１ロッドおよび第２ロッドの一端位置は、その前回値に基づいて、修正されるため、第１ロッドおよび第２ロッドの一端位置に連続性を持たせることができ、ひいては、この一端位置により決定される結合体の姿勢にも連続性を持たせることができる。

このように、第２発明のロボットの関節機構の制御装置によれば、ジンバルリンクを有するロボットの関節機構の動作を簡易なアルゴリズムで高精度に解析することができる。

第３発明のロボットの関節機構の制御装置は、互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
前記ジンバルリンクの一方の回転軸に一端が連結された第１ロッドおよび第２ロッドと、
前記第１ロッドの他端に連結された第１モータと、
前記第２ロッドの他端に連結された第２モータと
を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量と、前記第１モータの所定時間当たりの変化量である第１モータ角変化量および、前記第２モータの所定時間当たりの変化量である第２モータ角変化量との間の関係を規定した逆ヤコビ行列を逐次算出する逆行列算出手段と、
前記第１姿勢角変化量および第２姿勢角変化量から前記第１ロッドおよび第２ロッドの一端位置を算出するロッド一端位置算出手段と、
前記ロッド一端位置算出手段により算出された第１ロッドおよび第２ロッドの一端位置と、該第１ロッドおよび第２ロッドの他端位置の前回値とのそれぞれの差である、仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
前記逆行列算出手段より算出された逆ヤコビ行列に基づいて算出される第１モータ角変化量および第２モータ角変化量により前記第１ロッドおよび第２ロッドの他端位置の前回値を修正することにより、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの他端位置を決定するロッド他端位置決定手段と、
前記ロッド他端位置決定手段より決定された第１ロッドおよび第２ロッドの他端位置から前記第１モータの第１モータ角および第２モータの第２モータ角を算出するモータ角算出手段と
を備えることを特徴とする。

第３発明のロボットの関節機構の制御装置によれば、結合体の姿勢角から、第１ロッドおよび第２ロッドの一端位置を決定しておくと共に、第１ロッドおよび第２ロッドの他端位置の前回値を用いて、第１ロッドおよび第２ロッドの仮ロッド長を算出しておく。そして、この仮ロッド長を第１ロッドおよび第２ロッドのロッド長という普遍的な値へ収束させることで、第１ロッドおよび第２ロッドの他端位置を複雑な幾何学計算を行うことなく、簡易に決定することができる。

また、ここでの修正量は、逆ヤコビ行列から算出される第１モータおよび第２モータのモータ角の変化量に基づくため、高精度に第１ロッドおよび第２ロッドの他端位置を決定することができる。

さらに、第１ロッドおよび第２ロッドの他端位置は、その前回値に基づいて、修正されるため、第１ロッドおよび第２ロッドの他端位置に連続性を持たせることができ、ひいては、この他端位置により決定される第１モータおよび第２モータのモータ角にも連続性を持たせることができる。

このように、第３発明のロボットの関節機構の制御装置によれば、ジンバルリンクを有するロボットの関節機構の動作を簡易なアルゴリズムで高精度に解析することができる。

本実施形態のロボットの関節機構の制御装置の構成説明図。 図１の制御装置が適用される関節機構を示す図。 図１の制御装置が適用される関節機構を示す分解図。 図１の制御装置が適用される関節機構を示す図。 図１のロボットの関節機構の制御装置の処理内容（キネマティック計算処理）を示すフローチャート。 図１のロボットの関節機構の制御装置の処理内容（逆キネマティック計算処理）を示すフローチャート。

次に、本発明のロボットの関節機構の制御装置の実施形態について、図１〜図６を参照して説明する。

図１に示すように、ロボットの関節機構の制御装置１は、関節機構を動作させるモータと関節機構に結合された結合体の動作との間の関係を解析する装置であって、ロッド固定位置算出手段１１と、仮ロッド長算出手段１２と、ロッド偏差算出手段１３と、ヤコビ・逆ヤコビ行列算出手段１４と、修正量算出手段１５と、ロッド不定位置決定手段１６と、最終角度出力手段１７とを備える。

なお、以下、モータの動作から結合体の動作を解析することをキネマティクス演算、結合体の動作からモータへの動作の解析を逆キネマティクス演算という。

図２に示すように、制御装置１が適用されるロボットの関節機構１００は、人間型ロボットの手首の関節機構であって、腕のリンク１２０と、腕のリンク１２０に連結された手首の関節部１３０と、手首の関節部１３０に連結された結合体である手１８０と、手１８０の縦振りおよび横振り動作を行う駆動装置１９０とを備える。

具体的には、図３に示すように、腕のリンク１２０に形成された対向部１２１ａ，１２１ａに支持されたジンバルリンク１４０と、シンバルリンク１４０に連結されたメインリンク１５０と、メインリンク１５０に連結された第１ロッド１７１および第２ロッド１７２と、第１ロッド１７１および第２ロッド１７２にそれぞれ連結された駆動装置１９０の第１モータ１９１および第２モータ１９２と備える。

ジンバル１４０は、腕のリンク１２０に形成された対向部１２１ａ，１２１ａに縦軸部１４１が支持されて、ジンバルリンク１４０にメインリンク１５０を介して連結された手１８０が横振り自在となっている。また、ジンバルリンクの横軸部１４２がメインリンク１５０に連結されており、手１８０が縦振り自在となっている。なお、手１８０は、メインリンク１５０の副軸部にサブリンク１６０を介して連結されている。

第１ロッド１７１および第２ロッド１７２は、一端がメインリンク１５０にユニバーサルジョイント１５１および１５２を介して連結され、他端が第１モータ１９１の出力アーム１９１ａおよび第２モータ１９２の出力アーム１９２ａに連結されている。

次に、ロボットの関節機構の制御装置１の各構成要素について、図４を参照して具体的に説明する。なお、以降の座標原点は、図４（ａ）および（ｂ）に示すように、第１モータ１７１および第モータ１７２の回転軸上でこれらの中点とする。

ロッド固定位置算出手段１１は、キネマティック演算では、第１モータ１９１のモータ角θ１と第１ロッド１７１の他端位置１７１ｂとを変換すると共に、第２モータ１９２のモータ角θ２と第２ロッド１７２の他端位置１７２ｂとを変換する。

また、ロッド固定位置算出手段１１は、逆キネマティック演算では、手１８０の姿勢角であるメインリンク１５０の姿勢角α，βと、第１ロッド１７１の一端位置１７１ａおよび第２ロッド１７２の一端位置１７２ａとを相互に変換する。

なお、ロッド固定位置算出手段１１は、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂと、これらの一端位置１７１ａ，１７２ａをベクトル演算により特定するものとする。

仮ロッド長算出手段１２は、キネマティック演算では、第１モータ１９１および第２モータ１９２のモータ角θ１，θ１が与えられた場合に、ロッド固定位置算出手段１１により算出される第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂ（固定位置）と、第１ロッド１７１および第２ロッド１７２の一端位置の前回値（不定位置）との差を、それぞれ第１ロッドおよび第２ロッドの仮のロッド長として仮第１ロッド長Ｌ１´および仮第２ロッド長Ｌ２´を算出する。

また、仮ロッド長算出手段１２は、逆キネマティック演算では、手１８０の姿勢角であるメインリンク１５０の姿勢角α，βが与えられた場合に、ロッド固定位置算出手段１１により算出される第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａ（固定位置）と、第１ロッド１７１および第２ロッド１７２の他端位置の前回値（不定位置）との差を、それぞれ第１ロッドおよび第２ロッドの仮のロッド長として仮第１ロッド長Ｌ１´および仮第２ロッド長Ｌ２´を算出する。

ロッド偏差算出手段１３は、（キネマティック演算、逆キネマティック演算のいずれの場合にも）仮ロッド長算出手段１２により算出された仮第１ロッド長Ｌ１´と、実際の第１ロッドのロッド長Ｌ１との偏差ΔＤ１を算出すると共に、仮ロッド長算出手段１２により算出された仮第２ロッド長Ｌ２´と、実際の第２ロッドのロッド長Ｌ２との偏差ΔＤ２を算出する。

ヤコビ・逆ヤコビ行列算出手段１４は、キネマティック演算では、第１モータ１９１および第２モータ１９２のモータ角の微小時間変化量Δθ１，Δθ２と、メインリンク１５０の姿勢角度の微小時間変化量Δα，Δβとが線形関係となることに基づいて、Δθ１，Δθ２からΔα，Δβへの関係の比例関数行列をヤコビ行列として算出する。また、逆キネマティック演算では、Δα，ΔβからΔθ１，Δθ２への関係の比例関数行列を逆ヤコビ行列として算出する。

具体的に、ヤコビ・逆ヤコビ行列算出手段１４は、ロボットの関節機構１００の各リンクの長さと各ジョイントの回転行列を逐次的に求めることでヤコビ行列を算出する。また、ヤコビ・逆ヤコビ行列算出手段１４は、ヤコビ行列に基づいて、逆ヤコビ行列の逆行列として数学的に算出する。

修正量算出手段１５は、キネマティック演算では、ヤコビ・逆ヤコビ行列算出手段１４により算出されたヤコビ行列から算出される、メインリンク１５０の姿勢角度の微小時間変化量Δα，Δβを、第１ロッド１７１の一端位置１７１ａおよび第２ロッド１７２の一端位置１７２ａの微小変化量に変換する。そして、変換した微小変化量を、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａの前回値に加算する修正量とする。

また、修正量算出手段１５は、逆キネマティック演算では、ヤコビ・逆ヤコビ行列算出手段１４により算出された逆ヤコビ行列から算出される、第１モータおよび第２モータのモータ角の微小時間変化量Δθ１，Δθ２を、第１ロッド１７１の他端位置１７１ｂおよび第２ロッド１７２の他端位置１７２ｂの微小変化量に変換する。そして、変換した微小変化量を、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂの前回値に加算する修正量とする。

ロッド不定位置決定手段１６は、キネマティック演算では、修正量算出手段１５により算出された修正量を第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａの前回値に加算することで、第１ロッドおよび第２ロッドのロッド偏差ΔＤ１，ΔＤ２が０またはこれに相当する微小値に収束する場合の一端位置１７１ａ，１７２ａを、その今回値として決定する。

また、ロッド不定位置決定手段１６は、逆キネマティック演算では、修正量算出手段１５により算出された修正量を第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂの前回値に加算することで、第１ロッドおよび第２ロッドのロッド偏差ΔＤ１，ΔＤ２が０（またはこれに相当する微小値）に収束する場合の他端位置１７１ｂ，１７２ｂを、その今回値として決定する。

最終角度出力手段１７は、キネマティック演算では、ロッド不定位置決定手段１６により決定された第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａ（今回値）から、メインリンク１５０の姿勢角α，β（必要に応じて、これを変換した手１８０の姿勢角）を算出して出力する。

また、最終角度出力手段１７は、逆キネマティック演算では、ロッド不定位置決定手段１６により決定された第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂ（今回値）から、第１モータ１９１および第２モータ１９２のモータ角θ１およびθ２を算出して出力する。

次に、図５に示すフローチャートを参照して、本実施形態のロボットの関節機構の制御装置１によるキネマティック演算について説明する。

まず、ロボットの関節機構の制御装置１は、第１モータ１９１のモータ角θ１および第２モータ１９２のモータ角θ２が与えられると（図５／ＳＴＥＰ１０）、ロッド固定位置算出手段１１が、これに対応した第１ロッド１７１の他端位置１７１ｂおよび第２ロッド１７２の他端位置１７２ｂを算出する（図５／ＳＴＥＰ１１）。

次いで、仮ロッド長算出手段１２が、ＳＴＥＰ１１で算出した第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂと、一端位置１７１ａ，１７２ａの前回値との差を、それぞれ第１ロッド１７１および第２ロッド１７２の仮のロッド長（仮第１ロッド長Ｌ１´および仮第２ロッド長Ｌ２´）として算出する（図５／ＳＴＥＰ１２）。

次いで、ロッド偏差算出手段１３が、仮ロッド長算出手段１２により算出された仮第１ロッド長Ｌ１´と、実際の第１ロッドのロッド長Ｌ１との偏差ΔＤ１を算出すると共に、仮ロッド長算出手段１２により算出された仮第２ロッド長Ｌ２´と、実際の第２ロッドのロッド長Ｌ２との偏差ΔＤ２を算出する（図５／ＳＴＥＰ１３）。

次いで、ロッド不定位置決定手段１６が、ＳＴＥＰ１３で算出したロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっているか、すなわち、０（またはこれに相当する微小値）に収束しているかを判定する（図５／ＳＴＥＰ１４）。

そして、ロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっていない場合には（図５／ＳＴＥＰ１４・・ＮＯ）、ヤコビ・逆ヤコビ算出手段１４が、ヤコビ行列を算出する（図５／ＳＴＥＰ１５）。

次いで、修正量算出手段１５が、ＳＴＥＰ１５で算出したヤコビ行列に基づいて、メインリンク１５０の姿勢角度の微小時間変化量Δα，Δβを算出し、これに基づいて、第１ロッド１７１の一端位置１７１ａおよび第２ロッド１７２の一端位置１７２ａの修正量（微小変化量）を算出する（図５／ＳＴＥＰ１６）。

次いで、ロッド不定位置決定手段１６が、ＳＴＥＰ１６で算出した修正量を、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａおよび１７２ａに加算する修正を施して（図５／ＳＴＥＰ１７）、ＳＴＥＰ１２にリターンする。

そして、修正後の一端位置１７１ａおよび１７２ａに基づいて算出されるロッド偏差が収束範囲となるまでこれら一連の処理を繰り返し（図５／ＳＴＥＰ１２〜１７）、ロッド不定位置決定手段１６は、ロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっていると判定した場合には（図５／ＳＴＥＰ１４・・ＹＥＳ）、このときの第１ロッド１７１および第２ロッド１７２の一端位置１７１ａおよび１７２ａを今回値に決定する（図５／ＳＴＥＰ１８）。

次いで、最終角出力手段１７が、ＳＴＥＰ１８で決定された第１ロッド１７１および第２ロッド１７２の一端位置１７１ａおよび１７２ａから、メインリンク１５０の姿勢角α,β、さらには必要に応じて対応する手１８０の姿勢角を算出して、これらを出力し（図５／ＳＴＥＰ１９）、一連の処理を終了する。

以上が、本実施形態のロボットの関節機構の制御装置１によるキネマティック演算である。かかるキネマティクス演算によれば、第１モータ１９１および第２モータ１９２のモータ角θ１およびθ２から、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂを決定しておくと共に、一端位置１７１ａ，１７２ｂの前回値を用いて、
第１ロッド１７１および第２ロッド１７２の仮ロッド長Ｌ´を算出しておき、この仮ロッド長Ｌ´を実際のロッド長Ｌという普遍的な値へ収束させることで（ΔＤ→０）、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａを複雑な幾何学計算を行うことなく、簡易に決定することができる。

また、ここでの修正量は、ヤコビ行列から算出されるメインリンク１５０の姿勢角の変化量Δα，Δβに基づくため、高精度に第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａを決定することができる。

さらに、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａは、その前回値に基づいて、修正されるため、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａに連続性を持たせることができ、この一端位置により決定されるメインリンク１５０、さらには手１８０の動作にも連続性を持たせることができる。

次に、図６に示すフローチャートを参照して、本実施形態のロボットの関節機構の制御装置１による逆キネマティック演算について説明する。

まず、ロボットの関節機構の制御装置１は、メインリンク１５０の姿勢角α，β（手１８０の姿勢角が与えられた場合には、これをメインリンク１５０の姿勢角に変換した値）が与えられると、（図６／ＳＴＥＰ３０）、ロッド固定位置算出手段１１が、これに対応した第１ロッド１７１の一端位置１７１ａおよび第２ロッド１７２の一端位置１７２ａを算出する（図６／ＳＴＥＰ３１）。

次いで、仮ロッド長算出手段１２が、ＳＴＥＰ３１で算出した第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａと、他端位置１７１ｂ，１７２ｂの前回値との差を、それぞれ第１ロッド１７１および第２ロッド１７２の仮のロッド長（仮第１ロッド長Ｌ１´および仮第２ロッド長Ｌ２´）として算出する（図６／ＳＴＥＰ３２）。

次いで、ロッド偏差算出手段１３が、仮ロッド長算出手段１２により算出された仮第１ロッド長Ｌ１´と、実際の第１ロッドのロッド長Ｌ１との偏差ΔＤ１を算出すると共に、仮ロッド長算出手段１２により算出された仮第２ロッド長Ｌ２´と、実際の第２ロッドのロッド長Ｌ２との偏差ΔＤ２を算出する（図６／ＳＴＥＰ３３）。

次いで、ロッド不定位置決定手段１６が、ＳＴＥＰ３３で算出したロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっているか、すなわち、０（またはこれに相当する微小値）に収束しているかを判定する（図６／ＳＴＥＰ３４）。

そして、ロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっていない場合には（図６／ＳＴＥＰ３４・・ＮＯ）、ヤコビ・逆ヤコビ算出手段１４が、逆ヤコビ行列を算出する（図６／ＳＴＥＰ３５）。

次いで、修正量算出手段１５が、ＳＴＥＰ３５で算出した逆ヤコビ行列に基づいて、第１モータ１９１のモータ角の微小時間変化量Δθ１と、第２モータ１９２のモータ角の微小時間変化量Δθ２とを算出し、これに基づいて、第１ロッド１７１の他端位置１７１ｂおよび第２ロッド１７２の他端位置１７２ｂの修正量（微小変化量）を算出する（図６／ＳＴＥＰ３６）。

次いで、ロッド不定位置決定手段１６が、ＳＴＥＰ３６で算出した修正量を、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂおよび１７２ｂに加算する修正を施して（図６／ＳＴＥＰ３７）、ＳＴＥＰ３２にリターンする。

そして、修正後の他端位置１７１ｂおよび１７２ｂに基づいて算出されるロッド偏差が収束範囲となるまでこれら一連の処理を繰り返し（図６／ＳＴＥＰ３２〜３７）、ロッド不定位置決定手段１６は、ロッド偏差ΔＤ１およびΔＤ２がいずれも収束範囲内となっていると判定した場合には（図６／ＳＴＥＰ３４・・ＹＥＳ）、このときの第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂおよび１７２ｂを今回値に決定する（図６／ＳＴＥＰ３８）。

次いで、最終角出力手段１７が、ＳＴＥＰ３８で決定された第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂおよび１７２ｂから、第１モータ１９１のモータ角θ１と第２モータ１９２のモータ角θ２とを算出して、これらを出力し（図６／ＳＴＥＰ１９）、一連の処理を終了する。

以上が、本実施形態のロボットの関節機構の制御装置１による逆キネマティック演算である。かかる逆キネマティクス演算によれば、手１８０の姿勢角やメインリンク１５０の姿勢角α，βから、第１ロッド１７１および第２ロッド１７２の一端位置１７１ａ，１７２ａを決定しておくと共に、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂの前回値を用いて、第１ロッド１７１および第２ロッド１７２の仮ロッド長Ｌ´を算出しておき、この仮ロッド長Ｌ´を第１ロッドおよび第２ロッドのロッド長Ｌという普遍的な値へ収束させることで、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂを複雑な幾何学計算を行うことなく、簡易に決定することができる。

また、ここでの修正量は、逆ヤコビ行列から算出される第１モータおよび第２モータのモータ角の変化量Δθ１，Δθ２に基づくため、高精度に第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂを決定することができる。

さらに、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂは、その前回値に基づいて、修正されるため、第１ロッド１７１および第２ロッド１７２の他端位置１７１ｂ，１７２ｂに連続性を持たせることができ、ひいては、この他端位置１７１ｂ，１７２ｂにより決定される第１モータおよび第２モータのモータ角θ１，θ２にも連続性を持たせることができる。

このように、本実施形態のロボットの関節機構の制御装置１によれば、ジンバルリンクを有するロボットの関節機構の動作を簡易なアルゴリズムで高精度に解析することができる。

尚、本実施形態においては、第１ロッド１７１および第２ロッド１７２の仮ロッド長Ｌ１´，Ｌ２´と、これらの実際のロッド長Ｌ１，Ｌ２との差から、ロッド偏差ΔＤ１，Ｄ２を０に収束させるようにして、第１ロッド１７１および第２ロッド１７２の不定位置（キネマティック演算では、一端位置１７１ａ，１７２ａであり、逆キネマティック演算では、他端位置１７１ｂ，１７２ｂ）を決定したが（図５／ＳＴＥＰ１４、図６／ＳＴＥＰ３４）、これに限定されるものではなく、仮ロッド長Ｌ１´，Ｌ２´を直接実際のロッド長Ｌ１，Ｌ２に収束させるようにしてもよい。

また、本実施形態では、第１ロッド１７１および第２ロッド１７２を、第１モータ１９１および第２モータ１９２により動作させる構成について説明したが、これに限定されるものではなく、例えば、アクチュエータにより第１ロッド１７１および第２ロッド１７２を動作させるように構成してもよい。

１‥ロボットの関節機構の制御装置、１１‥ロッド固定位置算出手段（ロッド他端位置算出手段、ロッド一端位置算出手段）、１２‥仮ロッド長算出手段、１３‥ロッド偏差算出手段、１４‥ヤコビ・逆ヤコビ算出手段（関数算出手段、行列算出手段、逆行列算出手段）、１５‥修正量算出手段、１６‥ロッド不定位置決定手段（ロッド位置決定手段、ロッド一端位置決定手段、ロッド他端位置決定手段）、１７‥最終角度出力手段（姿勢角算出手段、モータ角算出手段）、１４０‥ジンバルリンク、１７１‥第１ロッド、１７２‥第２ロッド、１９１‥第１モータ（第１アクチュエータ）、１９２‥第２モータ（第２アクチュエータ）。

Claims (3)

1. 互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
   前記ジンバルリンクの一方の回転軸に連結された第１ロッドおよび第２ロッドと、
   前記第１ロッドを変位させる第１アクチュエータと、
   前記第２ロッドを変位される第２アクチュエータと
   を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
   前記第１アクチュエータの所定時間当たりの変化量である第１アクチュエータ変化量および、前記第２アクチュエータの所定時間当たりの変化量である第２アクチュエータ変化量と、前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量との間の関係を規定する関数を逐次算出する関数算出手段と、
   前記第１姿勢角変化量および第２姿勢角変化量から算出される前記第１ロッドおよび第２ロッドの一端位置と、該第１ロッドおよび第２ロッドの他端位置の前回値とのそれぞれの差として算出され、または、前記第１アクチュエータ変化量および第２アクチュエータ変化量から算出される前記第１ロッドおよび第２ロッドの他端位置と、該第１ロッドおよび第２ロッドの一端位置の前回値とのそれぞれの差として算出される仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
   前記関数算出手段により算出された関数に基づく前記第１姿勢角変化量および第２姿勢角変化量または、前記第１アクチュエータ変化量および第２アクチュエータ変化量により前記前回値を修正して、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの位置を決定するロッド位置決定手段と
   を備えることを特徴とするロボットの関節機構の制御装置。
2. 互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
   前記ジンバルリンクの一方の回転軸に一端が連結された第１ロッドおよび第２ロッドと、
   前記第１ロッドの他端に連結された第１モータと、
   前記第２ロッドの他端に連結された第２モータと
   を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
   前記第１モータの所定時間当たりの変化量である第１モータ角変化量および、前記第２モータの所定時間当たりの変化量である第２モータ角変化量と、前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量との間の関係を規定したヤコビ行列を逐次算出する行列算出手段と、
   前記第１モータ角変化量から前記第１ロッドの他端位置を逐次算出すると共に、前記第２モータ角変化量から前記第２ロッドの他端位置を逐次算出するロッド他端位置算出手段と、
   前記ロッド他端位置算出手段により算出された第１ロッドおよび第２ロッドの他端位置と、該第１ロッドおよび第２ロッドの一端位置の前回値とのそれぞれの差である、仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
   前記行列算出手段より算出されたヤコビ行列に基づいて算出される前記第１姿勢角変化量および第２姿勢角変化量により前記第１ロッドおよび第２ロッドの一端位置の前回値を修正することにより、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの一端位置を決定するロッド一端位置決定手段と、
   前記ロッド一端位置決定手段より決定された第１ロッドおよび第２ロッドの一端位置から前記結合体の前記第１回転軸回りの第１姿勢角および第２回転軸回りの第２姿勢角を算出する姿勢角算出手段と
   を備えることを特徴とするロボットの関節機構の制御装置。
3. 互いに平行でない第１回転軸と第２回転軸とを有するジンバルリンクと、
   前記ジンバルリンクの一方の回転軸に一端が連結された第１ロッドおよび第２ロッドと、
   前記第１ロッドの他端に連結された第１モータと、
   前記第２ロッドの他端に連結された第２モータと
   を備えて前記ジンバルリンクに結合した結合体を前記第１回転軸および第２回転軸回りに動作可能なロボットの関節機構の制御装置において、
   前記結合体の前記第１回転軸回りの所定時間当たりの変化量である第１姿勢角変化量および、該結合体の前記第２回転軸回りの所定時間当たりの変化量である第２姿勢角変化量と、前記第１モータの所定時間当たりの変化量である第１モータ角変化量および、前記第２モータの所定時間当たりの変化量である第２モータ角変化量との間の関係を規定した逆ヤコビ行列を逐次算出する逆行列算出手段と、
   前記第１姿勢角変化量および第２姿勢角変化量から前記第１ロッドおよび第２ロッドの一端位置を算出するロッド一端位置算出手段と、
   前記ロッド一端位置算出手段により算出された第１ロッドおよび第２ロッドの一端位置と、該第１ロッドおよび第２ロッドの他端位置の前回値とのそれぞれの差である、仮第１ロッド長および仮第２ロッド長を算出する仮ロッド長算出手段と、
   前記逆行列算出手段より算出された逆ヤコビ行列に基づいて算出される第１モータ角変化量および第２モータ角変化量により前記第１ロッドおよび第２ロッドの他端位置の前回値を修正することにより、前記仮ロッド長算出手段により算出された仮第１ロッド長および仮第２ロッド長を前記第１ロッドのロッド長および第２ロッドのロッド長に収束させるように該第１ロッドおよび第２ロッドの他端位置を決定するロッド他端位置決定手段と、
   前記ロッド他端位置決定手段より決定された第１ロッドおよび第２ロッドの他端位置から前記第１モータの第１モータ角および第２モータの第２モータ角を算出するモータ角算出手段と
   を備えることを特徴とするロボットの関節機構の制御装置。

Images