JP5371733B2 - ７軸多関節ロボットの制御方法及び制御プログラム - Google Patents

Description

本発明は、７軸多関節ロボットの制御方法及び制御プログラムに関し、特に手首の位置を移動させる際に７軸多関節ロボットの形態の急激な変化を抑える制御方法及び制御プログラムに関する。

近年、産業用ロボットの制御技術は、コンピュータ技術の進歩に伴って進歩しており、これに伴って産業用ロボットに要求される動作はますます複雑かつ高度化しており、またその精度についてもより高速かつ高精度化が必要とされている。例えば、従来から産業用ロボットとして広く用いられている６軸多関節ロボットの場合、ある決まった位置に移動する用途としては十分であるが、移動空間に存在するさまざまな障害物を巧みに回避しながら複雑な作業を行うことが困難であった。そこで、６軸多関節ロボットに更に１軸（冗長軸）を追加した７軸多関節ロボット（例えば、特許文献１を参照）の開発が近年盛んになってきている。

７軸多関節ロボットの制御方法としては、従来の６軸多関節ロボットの場合と同様に、ロボットアーム先端の手首の位置がたどるべき経路上の点を順次指示する方法や、当該手首の初期位置及び目標位置のみを指示して途中の経路を問わない方法が用いられている。なお、これらの方法が用いられる場合には、当該手首の位置及び姿勢（手首座標）から各関節角（関節角座標）を求める逆変換（座標変換）が行われる。

特開平６−３１５８８０号公報

ところで、７軸多関節ロボットの場合、６軸多関節ロボットと異なって冗長軸（１軸）が存在するので、同じ手首の位置において逆変換により求められる各関節角は一義的に定まらない。

このため、手首の位置をある点Ａから別の点Ｂに移動させる場合、例えば点Ａと点Ｂとにおける第１関節の関節角が１８０°異なるというように、当該点Ａの座標に基づく逆変換により求められかつ特定された各関節角と、当該点Ｂの座標に基づく逆変換により求められかつ特定された各関節角とが大きく乖離してしまい、ロボットの形態が急激に変化するという問題があった。

なお、特許文献１に開示された技術は、先端制御点の位置や姿勢を元のままで肘の部分を回転させる方法であり、手首の位置を移動させる際にロボットの形態の急激な変化を抑制することについては何ら示唆も開示もされていない。

本発明は、このような課題を解決するためになされたもので、その目的は、手首の位置を移動させる際にロボットの形態の急激な変化を抑えることである。

上記の課題を解決するための本発明に係る７軸多関節ロボットの制御方法は、先端に設けられた手首と基端から当該手首に向かって順に設けられた７つの関節とを具備し、前記７つの関節が回転軸をそれぞれ有しかつそれぞれの回転軸の周りに次の関節を回転させるように構成された７軸多関節ロボットの制御方法であって、前記７つの回転軸のうちのいずれか１つを冗長軸に、残りの回転軸のうちの３つを基軸に、かつ当該３つの基軸のうちのいずれか１つを変数軸に定め、かつ、前記基端から前記手首までの直線距離と前記基端に最も近い前記回転軸の延在方向における前記基端から前記手首までの距離とに基づく手首の位置と前記変数軸を有した関節の関節角とに関して定式化された４次方程式を解いて前記手首の位置を前記３つの基軸を有した各関節の関節角に逆変換するステップを含み、前記４次方程式は、その４つの解にそれぞれ対応する４つの第１求解演算式を用いて解くことが可能なものであり、かつ、前記７軸多関節ロボットが採り得る形態は、前記４つの解ひいては前記４つの第１求解演算式に対応して定まるものであり、電源オン時に、当該電源オン時における前記７軸多関節ロボットの初期形態に基づいて、前記４つの第１求解演算式のうちいずれか１つを特定して記憶し、前記電源オンの後、前記記憶した１つの第１求解演算式を用いて前記逆変換するステップを遂行して前記手首の目標位置から前記３つの基軸を有した各関節の関節角を算定し、それにより、前記７軸多関節ロボットの動作を制御する、ものである。

上記の制御方法であって、前記電源オン時に、前記４つの第１求解演算式を用いて前記４次方程式を解いて前記４つの解を求め、当該４つの解それぞれについて当該電源オン時における前記手首の位置を前記３つの基軸を有した各関節の関節角の少なくともいずれか１つに逆変換し、当該１つの基軸を有した関節の関節角と前記電源オン時の前記７軸多関節ロボットの初期形態に対応する前記３つの基軸のうちの１つを有した関節の関節角とを比較することにより、前記４次方程式の４つの第１求解演算式のうちいずれか１つを特定して記憶する、としてもよい。

上記の制御方法であって、全ての互いに隣り合う前記関節の回転軸が互いに垂直であり、前記基端から前記手首に向かって順に設けられた７つの関節を、それぞれ、第１関節、第２関節、第７関節、第３関節、第４関節、第５関節、及び第６関節と定義した場合、前記冗長軸は前記第７関節の回転軸であり、前記３つの基軸は前記第１関節の回転軸、前記第２関節の回転軸、及び前記第３関節の回転軸であり、かつ前記変数軸は前記第３関節の回転軸である、としてもよい。

上記の制御方法によれば、４次方程式の４つの解を求めるための４つの第１求解演算式と、４つの解に応じてとり得る４つの形態とを予め対応付けておき、電源オン時の７軸多関節ロボットの初期形態に基づいて、４つの第１求解演算式のうちいずれか１つを、電源オンより後の逆変換処理において使用する。この結果、手首の位置を目標位置に移動する際に、７軸多関節ロボットの形態の急激な変化を抑えることが可能となる。さらに、電源オン時の７軸多関節ロボットの初期形態を特定するためには、４次方程式の４つの第１求解演算式それぞれについて１回限り演算すればよい。電源オンより後の逆変換処理においては、特定された１つの第１求解演算式以外の残りの３つの第１求解演算式については演算しなくても済むので、逆変換処理に要する演算量を抑えることができ、７軸多関節ロボットの制御の高速化が図られることとなる。

上記の制御方法であって、前記４つの第１求解演算式を用いて前記４次方程式を解く際に、フェラーリの解法に基づいた前記４次方程式を解くための分解方程式として定式化された３次方程式の３つの解に含まれる１つの実数解を求め、かつ当該求めた前記３次方程式の１つの実数解を用いて前記４つの第１求解演算式を演算する、としてもよい。

上記の制御方法であって、前記３次方程式の３つの解に含まれる１つの実数解を求める際に、前記３次方程式の判別式が０又は正の場合には、カルダノの解法により求まる前記３次方程式の１つの実数解と２つの虚数解のうち当該１つの実数解に対応した当該カルダノの解法に基づく第２求解演算式を使用し、前記３次方程式の判別式が負の場合には、ビエタの解法により求まる前記３次方程式の３つの実数解のうち絶対値が同一かつ符号の異なる２つの実数解以外の１つの実数解に対応した当該ビエタの解法に基づく第３求解演算式を使用する、としてもよい。

上記の制御方法によれば、４次方程式を解くためにはフェラーリの解法に基づいた当該４次方程式を解くための分解方程式として定式化された３次方程式を解くことになるが、当該３次方程式であるので３つの解が必ず生じる。よって、３次方程式の３つの解に必ず含まれる１つの実数解を求めるように予めしておけば、４次方程式を解く際に３次方程式の３つの解が無秩序に選ばれることがなくなるので、４次方程式の４つの解を定常的に求めることが可能となる。この結果、７軸多関節ロボットについての急激な変動をさらに抑えることができる。また、３次方程式を解く場合に１つの実数解のみを求めることにしたので、複素数演算をしなくて済み、演算量を抑えることができ、システムへの実装が容易なものとなる。

上記の課題を解決するための本発明に係る７軸多関節ロボットの制御プログラムは、先端に設けられた手首と基端から当該手首に向かって順に設けられた７つの関節とを具備し、前記７つの関節が回転軸をそれぞれ有しかつそれぞれの回転軸の周りに次の関節を回転させるように構成された７軸多関節ロボットの制御方法をコンピュータに遂行させる、７軸多関節ロボットの制御プログラムであって、前記制御方法は、前記７つの回転軸のうちのいずれか１つを冗長軸に、残りの回転軸のうちの３つを基軸に、かつ当該３つの基軸のうちのいずれか１つを変数軸に定め、かつ、前記基端から前記手首までの直線距離と前記基端に最も近い前記回転軸の延在方向における前記基端から前記手首までの距離とに基づく手首の位置と前記変数軸を有した関節の関節角とに関して定式化された４次方程式を解いて前記手首の位置を前記３つの基軸を有した各関節の関節角に逆変換するステップを含み、前記４次方程式は、その４つの解にそれぞれ対応する４つの第１求解演算式を用いて解くことが可能なものであり、かつ、前記７軸多関節ロボットが採り得る形態は、前記４つの解ひいては前記４つの第１求解演算式に対応して定まるものであり、電源オン時に、当該電源オン時における前記７軸多関節ロボットの初期形態に基づいて、前記４つの第１求解演算式のうちいずれか１つを特定して記憶し、前記電源オンの後、前記記憶した１つの第１求解演算式を用いて前記逆変換するステップを遂行して前記手首の目標位置から前記３つの基軸を有した各関節の関節角を算定し、それにより、前記７軸多関節ロボットの動作を制御する、ものである。

手首の位置を移動する際に７軸多関節ロボットの形態の急激な変化を抑制することができる。

図１は、本発明の実施の形態に係る７軸多関節ロボットとロボット制御装置とによるロボットシステムの構成を示した図である。 図２は、図１に示した７軸多関節ロボットのリンク構造を模式的に表した図である。 図３は、４次方程式の４つの第１求解演算式のいずれか１つを特定する処理の流れを表すフローチャートである。 図４は、記憶された４次方程式の４つの求解方程式のうちのいずれか１つを用いて実行される逆変換処理の流れを示すフローチャートである。 図５は、６軸多関節ロボットの第２関節と第３関節との間に第７関節が設けられた７軸多関節ロボットの構成を示した図である。 図６は、４次方程式の４つの解に応じて採り得る７軸多関節ロボットの４つの形態を示した図である。

以下、本発明の好ましい実施の形態を、図面を参照しながら説明する。なお、以下では全ての図を通じて同一又は相当する要素には同一の参照符号を付して、その重複する説明を省略する。

（本発明の概念）
最初に本発明の概念を説明する。

図５は、基端から先端の手首に向って順に第１乃至第６回転軸Ａ１乃至Ａ６を有した第１乃至第６関節ＪＴ１乃至ＪＴ６を具備する６軸多関節ロボットにおいて、第２関節ＪＴ２（第２回転軸Ａ２）と第３関節ＪＴ３（第３回転軸Ａ３）との間に第７回転軸ＪＴ７を有した第７関節ＪＴ７を設けた場合の７軸多関節ロボット１００の構成を示した図である。なお、図５において、７軸多関節ロボット１００の手首を構成する第４乃至第６関節ＪＴ４〜ＪＴ６（手首軸：第４乃至第６回転軸Ａ４〜Ａ６）は、図面から省略している。

７軸多関節ロボット１００を対象とした逆変換を行う場合、手首の姿勢を規定する手首軸（Ａ４〜Ａ６）はひとまず無視しておき、当該手首の位置を規定する残りの４つの回転軸Ａ１、Ａ２、Ａ７、Ａ３のうち、１つの回転軸（例えば、第７回転軸Ａ７）を冗長軸とし、かつ残りの３つの回転軸（例えば、第１乃至第３回転軸Ａ１〜Ａ３）を基軸とし、当該基軸を有した各関節の関節角を未知変数として取り扱う手法が考えられる。

ここで、図５に示す通り、基準座標系（ＸＹＺ座標系）における原点Ｏ（０，０，０）と７軸多関節ロボット１００の手首の位置Ｐとの間の直線距離をＲｈ、当該基準座標系における手首の位置ＰのＺ軸方向の距離（基端に最も近い第１回転軸の延在方向における基端から手首までの距離）をＺｈとして表すこととする。さらに、基準座標系における手首の位置Ｐを（Ｘｐ，Ｙｐ，Ｚｐ）と表したときに、Ｒｈ及びＺｈについて次式が成立する。

Ｒｈ^２＝Ｘｐ^２＋Ｙｐ^２＋Ｚｐ^２ ・・・式（１−１）
Ｚｈ＝Ｚｐ ・・・式（１−２）
また、Ｒｈ、Ｚｈは、上記の式（１−１）及び（１−２）とは別に、７軸多関節ロボット１００の各リンク長と第１乃至第３及び第７回転軸Ａ１〜Ａ３、Ａ７を有した第１乃至第３及び第７関節ＪＴ１〜ＪＴ３、ＪＴ７の関節角θ１、θ２、θ３、θ７を用いて表すことができる。

よって、これらの式を整理すると、次式の通り変数ｔ（＝ｔａｎθ３）についての４次方程式を得ることが出来る。

ｔ^４＋ａ・ｔ^３＋ｂ・ｔ^２＋ｃ・ｔ＋ｄ＝０ ・・・（２）
なお、式（２）の４次方程式における変数ｔの解は、以下の４つの第１求解演算式（３−１）〜（３−４）から求められる解ｔ１、ｔ２、ｔ３、ｔ４の計４通り存在する。

ｔ１＝（−Ｍ１−（Ｍ１^２−４・Ｎ１）^０．５）／２−ａ／４ ・・・（３−１）
ｔ２＝（−Ｍ１＋（Ｍ１^２−４・Ｎ１）^０．５）／２−ａ／４ ・・・（３−２）
ｔ３＝（−Ｍ２−（Ｍ２^２−４・Ｎ２）^０．５）／２−ａ／４ ・・・（３−３）
ｔ４＝（−Ｍ２＋（Ｍ２^２−４・Ｎ２）^０．５）／２−ａ／４ ・・・（３−４）
よって、４つの解ｔ１乃至ｔ４それぞれについて“θ３（ｎ）＝ａｔａｎ（ｔ（ｎ））：但し、ｎ＝１〜４”を算定することにより、関節角θ３として４つの解θ３（１）、θ３（２）、θ３（３）、θ３（４）を得ることができる。なお、関節角θ３が求まれば、関節角θ１及び関節角θ２についても一義的に求められるので、関節角θ３の４つの解θ３（１）、θ３（２）、θ３（３）、θ３（４）に対応した関節角θ１の４つの解θ１（ｎ）（但し、ｎ＝１〜４）及び関節角θ２の４つの解θ２（ｎ）（但し、ｎ＝１〜４）が求められる。この結果、同一の手首の位置Ｐであっても、７軸多関節ロボット１００の形態としては、図６に示されるような４つの形態が考えられるので、７軸多関節ロボット１００の合理的な動作が行われるためには、７軸多関節ロボットの形態の急激な変化を抑えることが求められる。

ところで、式（２）の４次方程式の４つの解ｔ１乃至ｔ４に関する第１求解演算式（式（３−１）〜式（３−４））の中で、Ｍ１、Ｍ２、Ｎ１、Ｎ２は、式（２）の４次方程式の左辺中の（ａ，ｂ，ｃ，ｄ）、及び４次方程式の「フェラーリの解法」に基づいた式（２）の４次方程式の分解方程式である３次方程式の解ｕによって求められる。なお、この３次方程式は、
ｐ＝−３ａ^２／８＋ｂ ・・・（４−１）
ｑ＝ａ^３／８−ａｂ／２＋ｃ ・・・（４−２）
ｒ＝−３ａ^４／２５６＋ａ^２ｂ／１６−ａｃ／４＋ｄ ・・・（４−３）
とおいた場合に、次式のように、変数ｙについての３次方程式として表される。

８ｙ^３−４ｐｙ^２−８ｒｙ＋４ｐｒ−ｑ^２＝０ ・・・（５）
しかしながら、式（５）の３次方程式の解としてｕ１、ｕ２、ｕ３の３つが得られるので、これらの解ｕ１乃至ｕ３のうちどの解が用いられても、式（２）の４次方程式の解ｔ１、ｔ２、ｔ３、ｔ４を求めることができるという問題が生じる。そこで、式（５）の３次方程式を解く機会毎に、３つの解ｕ１、ｕ２、ｕ３のうち異なるものが用いられてしまうと、式（２）の４次方程式の４つの解ｔ１、ｔ２、ｔ３、ｔ４の組合せが変化するという問題が発生する。

例えば、以下の表１は式（５）の３次方程式の３つの解ｕ１、ｕ２、ｕ３それぞれを用いたときの式（２）の４次方程式の４つの解ｔ１、ｔ２、ｔ３、ｔ４から得られる関節角θ３の組合せの例を示した表である。表１において、式（５）の３次方程式の解ｕ１を採用した場合、式（２）の４次方程式の４つの解ｔ１、ｔ２、ｔ３、ｔ４から得られる関節角θ３の組合せは（−４３°，５１°，−２０°，２８°）である。一方、３次方程式の解ｕ２を採用した場合、４次方程式の４つの解ｔ１、ｔ２、ｔ３、ｔ４から得られる関節角θ３の組合せは（−４３°，−２０°，５１°，２８°）であり、３次方程式の解ｕ１を採用した場合と比べて、４次方程式の解ｔ２、ｔ３に対応する関節角θ３が真逆になっている。あるいは、３次方程式の解ｕ３を採用した場合、４次方程式の解ｔ１、ｔ２、ｔ３、ｔ４から得られる関節角θ３の組合せは（−４３°，−２０°，５１°，２８°）であり、３次方程式の解ｕ１を採用した場合と比べて、４次方程式の解ｔ１、ｔ４に対応する関節角θ３が真逆になっている。

従って、式（５）の３次方程式における３つの解ｕ１、ｕ２、ｕ３の中で常に同じ解（例えば、解ｕ１）を採用し続け、かつ式（２）の４次方程式における４つの解ｔ１、ｔ２、ｔ３、ｔ４の中で常に同じ解（例えば、解ｔ１）を採用し続けなければ、７軸多関節ロボット１００の形態が大きく変化してしまう。詳述すると、表１はある手首位置Ｐｘに対応する３つの解ｕ１〜ｕ３毎の４つの解ｔ１、ｔ２、ｔ３、ｔ４を表しているものとし、かつ手首位置Ｐｘに関して４つの解ｔ１〜ｔ４の中から例えば解ｔ１が選択されたものとする。この場合において、手首位置Ｐｘから少しずれた手首位置Ｐｙについて４つの解ｔ１〜ｔ４を演算により求めたところ、表１に示される値はそれぞれ少しずつずれた値となる。手首位置Ｐｙに関し、手首位置Ｐｘの場合と同様に解ｔ１（少しずつずれた値）を選択すれば問題ない。しかし、解ｔ１（少しずつずれた値）以外の例えば解ｔ２（少しずつずれた値）を選択したとすると、関節角θ３、及びこの関節角θ３を用いて算出される関節角θ１、θ２の値が手首位置Ｐｘのときの値と大きく異なってしまう。この結果、第１乃至第３関節ＪＴ１〜ＪＴ３及び第７関節ＪＴ７の移動量が大きくなるので、このような事態を避ける必要がある。

そこで、７軸多関節ロボット１００の形態の急激な変化を抑えるための方法の一つとして、各関節角の条件（±９０°以内であるか等）やその組合せに基づいて図６に示すような４つの形態の中で今どの形態となっているのかを常時把握するようにし、かつその把握した形態が得られるような一つの解を４つの解ｔ１、ｔ２、ｔ３、ｔ４の中から選択するという制御方法が考えられる。しかしながら、逆変換処理が行われる毎に、上記のような複雑な条件の判別が必要となり、逆変換処理が複雑になるという問題が生じる。

その他の方法としては、式（２）の４次方程式を解くために必要となる式（５）の３次方程式の３つの解ｕ１、ｕ２、ｕ３の中から常時同じ解（例えば、解ｕ１）を採用することで、４つの第１求解演算式（式（３−１）乃至式（３−４））と図６に示すような４つの形態とをあらかじめ１対１に対応付けておくという方法が考えられる。しかしながら、この場合には、式（５）の３次方程式の解法として複素数を取り扱う必要が生じてくるので、システムへの実装が困難なものとなる。

そこで、本発明者は、４つの第１求解演算式（式（３−１）乃至式（３−４））と図６に示すような４つの形態とをあらかじめ１対１に対応付けるとともに、式（５）の３次方程式について常に同じ解を採用し続ける方法として、３次方程式の解法として広く知られている「カルダノの解法」と「ビエタの解法」とを使い分けることを考えた。両者の解法は、ともに３つの解を得ることができるが、次の表２に示すように、３次方程式の判別式Ｄの条件（０より小、０と等しい、０より大）によっては負の√演算、つまり複素数演算を要する場合がある。なお、３次方程式の判別式Ｄの条件としては、次の３つの条件Ｃ１、Ｃ２、Ｃ３が挙げられる。

条件Ｃ１（Ｄ＜０）：３つの実数解が得られる
条件Ｃ２（Ｄ＝０）：１つの実数解（３重解）が得られる
条件Ｃ３（Ｄ＞０）：１つの実数解と２つの虚数解（共役複素数）が得られる

表２に示すとおり、「カルダノの解法」を採用した場合、条件Ｃ１の場合には３つの実数解ｕ１〜ｕ３の全てにおいて負の√演算（複素数演算）が必要であり、条件Ｃ３の場合には２つの虚数解ｕ２、ｕ３において負の√演算が必要となり、かつ条件Ｃ２の場合には負の√演算が不要となるという特徴がある。

「ビエタの解法」を採用した場合、条件Ｃ２の場合には３重解である実数解ｕ１において負の√演算が必要となる場合があり、条件Ｃ３の場合には全ての解（実数解ｕ１、虚数解ｕ２、ｕ３）において負の√演算が必要となる場合があり、かつ条件Ｃ１の場合には負の√演算が不要となるという特徴がある。

７軸多関節ロボットを制御するロボット制御装置の制御プログラム上、負の√演算を演算することは複雑かつ困難であるので、「カルダノの解法」と「ビエタの解法」とを上手く使い分ける必要がある。そこで、本出願人は、条件Ｃ２と条件Ｃ３との場合には、負の√演算を要さずに実数解ｕ１が求められる「カルダノの解法」を用いればよいことに気付いた。

一方、条件Ｃ１の場合には、負の√演算を必要としない「ビエタの解法」を用いればよいことになるが、この場合、３つの実数解（ｕ１〜ｕ３）が求められるので、それらの３つの実数解から１つの実数解を選定する必要がある。さらに、ロボットの形態変化を抑えるべく条件Ｃ１〜Ｃ３のいずれが成立しても常に同一の解ｕ１を採用し続ける必要があるため、「ビエタの解法」により求められた３つの実数解（ｕ１〜ｕ３）のうちのどの解が「カエダノの解法」により求められた１つの実数解ｕ１と対応づけられたものであるかを判断する必要がある。そこで、実数解ｕ２と実数解ｕ３は±が異なるものの絶対値が同じであるという特徴を利用して、両者ではない解を実数解ｕ１として採用することとした。

この結果、３次方程式の判別式Ｄの条件Ｃ１〜Ｃ３のいずれが成立しても、負の√演算が行われずに済み、かつ条件Ｃ１〜Ｃ３のいずれが成立しても同一の実数解ｕ１が常時求められることとなる。さらに、この実数解ｕ１を常時採用して４次方程式の４つの解ｔ１、ｔ２、ｔ３、ｔ４の組合せが求められることになるので、７軸多関節ロボットの形態の急激な変化が抑制される。

（実施の形態）
次に、上述の本発明の概念に基づく本発明の実施の形態を説明する。
［ロボットシステムの構成］
図１は、本発明の実施の形態に係る７軸多関節ロボットとロボット制御装置とによるロボットシステムの構成を示した図である。図２は、図１に示した７軸多関節ロボット１００のリンク構造を模式的に表した図である。

図１及び図２に示すように、７軸多関節ロボット１００は、先端に設けられた手首と所定の基端から当該手首に向かって順に設けられた７つの関節ＪＴ１〜ＪＴ７とを具備し、７つの関節ＪＴ１〜ＪＴ７が第１乃至第７回転軸Ａ１〜Ａ７をそれぞれ有しかつそれぞれの回転軸の周りに次の関節を回転させるように構成されている。なお、本実施の形態においては、７軸多関節ロボット１００は、基台２が作業スペース下側の床面に設置される所謂床置き型の垂直多関節ロボットとして構成されている。さらに、７軸多関節ロボット１００は、基端から手首に向って順に設けられた６つの関節ＪＴ１〜ＪＴ６を具備し、６つの関節ＪＴ１〜ＪＴ６が第１乃至第６回転軸Ａ１〜Ａ６をそれぞれ有し、かつそれぞれの回転軸の周りに次の関節を回転させるように構成された６軸多関節ロボットにおいて、第２関節ＪＴ２（第２回転軸Ａ２）と第３関節ＪＴ３（第３回転軸Ａ３）との間に第７関節ＪＴ７（第７回転軸Ａ７）が設けられ、第７関節ＪＴ７の第７回転軸Ａ７は第２回転軸Ａ２及び第３回転軸Ａ３に対して垂直であることにより構成されている。

なお、７つの関節及び７つの回転軸に付与された符号は便宜上付したものであり、７つの関節及び７つの回転軸それぞれを識別可能であれば如何なる符号であってもよい。

本実施の形態では、後述するＸ０軸、Ｙ０軸、Ｚ０軸による直交座標系を「基準座標系」と呼び、これをΣ０と表記する。また、後述するＸ６軸、Ｙ６軸、Ｚ６軸による直交座標系を「手首座標系」と呼び、これをΣＲと表記する。Σ０の原点に対するΣＲ原点のＸ０軸、Ｙ０軸、Ｚ０軸方向への変位を「手首位置」と呼び、Σ０に対するΣＲのＸ０軸、Ｙ０軸、Ｚ０軸周りの角変位を「手首姿勢」と呼ぶこととする。

また、ロボット先端のことを「手先」と呼ぶ。本実施の形態では、ツール部材１１の先端が手先となる。また、主に手首姿勢に影響を及ぼす部分を「手首」と呼ぶこととすると、図１に示す７軸多関節ロボット１００においては、後述のようにΣＲの原点は第４乃至第６回転軸Ａ４乃至Ａ６が一点で交わった点とされているので、第４乃至第６関節ＪＴ４乃至ＪＴ６に関する部分が手首となる。他方、手首位置に影響を及ぼすのは第１乃至第３関節ＪＴ１乃至ＪＴ３、及び第７関節ＪＴ７に関する部分である。「手首」には、アタッチメント、ハンド、ツール、エンドエフェクタ等が含まれる。なお、本実施の形態において、「手首」は後述の手首装置１０に対応している。

ΣＲにおける手先位置を把握しておけば、手首位置及び手首姿勢が与えられた場合に、Σ０における手先位置を求めることができる。また、手首姿勢、Σ０における手先位置及び冗長軸の角度が与えられた場合に、これを実現するような第１乃至第６関節ＪＴ１乃至ＪＴ６の関節角θ１乃至θ６を求めることができる。

なお、本実施の形態においては、三軸交点は、手首軸としての第４乃至第６回転軸Ａ４乃至Ａ６の３軸が交わった一点に対応している。三軸交点の特徴としては、ツール部材１１に対する三軸交点の位置はツール部材１１とアーム部材８との相対角（すなわち、第６回転軸Ａ６の回転角）と無関係に一定であり、アーム部材６に対する三軸交点の位置についてもアーム部材６とアーム部材７との相対角（すなわち、第４回転軸Ａ４の回転角）とは無関係に一定である。このようにすれば、手先（ツール部材１１の先端）の所望の位置・姿勢が与えられた場合に、手首軸の位置を実現する手首軸以外の残りの軸を決定した上で、当該手先の所望の位置・姿勢を実現するような手首軸を決定できるという利点がある。

基台２には、旋回台３、アーム部材（リンク）４、５、６、７、８、及びアタッチメント９がこの順に連設されている。なお、アタッチメント９の先端を成すフランジ面には、各種の作業内容に応じて適宜選択されたツール部材１１が着脱可能に取り付けられている。基台２からアタッチメント９までの連設された部材２〜９は互いに相対回転可能となるよう連結されている。

詳述すると、基台２と旋回台３との連結部である第１関節ＪＴ１において基台２に対する旋回台３の第１回転軸Ａ１周りの回転（旋回）が許容されている。なお、基台２の上面中央部には基端として基準座標系（Ｘ０軸、Ｙ０軸、Ｚ０軸による直交座標系）の原点Ｏ（０，０，０）が設定されている（図２参照）。また、第１関節ＪＴ１には、当該第１関節ＪＴ１の関節座標系（Ｘ１軸、Ｙ１軸、Ｚ１軸による直交座標系）が設定されている。

旋回台３の上端部とアーム部材４の一端部との連結部である第２関節ＪＴ２において旋回台３に対するアーム部材４の第２回転軸Ａ２周りの回転（回動）が許容されている。なお、第２関節ＪＴ２には、当該第２関節ＪＴ２の関節座標系（Ｘ２軸、Ｙ２軸、Ｚ２軸による直交座標系）が設定されている。ここで、基準座標系における第２関節ＪＴ２のＺ軸方向の距離（以下、第１リンク長と呼ぶ）をＬ０と表すこととする。また、基準座標系における第１関節ＪＴ１と第２関節ＪＴ２との間におけるＹ軸方向の距離（以下、第２リンク長と呼ぶ）をＬ１と表すこととする。

アーム部材４の他端部とアーム部材５の一端部との連結部である第７関節ＪＴ７においてアーム部材４に対するアーム部材５の第７回転軸Ａ７周りの回転（旋回）が許容されている。なお、第７関節ＪＴ７には、当該第７関節ＪＴ７の関節座標系（Ｘ７軸、Ｙ７軸、Ｚ７軸による直交座標系）が設定されている。

アーム部材５の他端部とアーム部材６の一端部との連結部である第３関節ＪＴ３においてアーム部材５に対するアーム部材６の第３回転軸Ａ３周りの回転（回動）が許容されている。なお、第３関節ＪＴ３には、当該第３関節ＪＴ３の関節座標系（Ｘ３軸、Ｙ３軸、Ｚ３軸による直交座標系）が設定されている。ここで、基準座標系における第２関節ＪＴ２と第３関節ＪＴ３との間におけるＺ軸方向の距離（以下、第３リンク長と呼ぶ）をＬ２と表すこととする。

アーム部材６の他端部とアーム部材７の一端部との連結部である第４関節ＪＴ４においてアーム部材６に対するアーム部材７の第４回転軸Ａ４周りの回転（旋回）が許容されている。なお、第４関節ＪＴ４には、当該第４関節ＪＴ４の関節座標系（Ｘ４軸、Ｙ４軸、Ｚ４軸による直交座標系）が設定されている。ここで、基準座標系における第３関節ＪＴ３と第４関節ＪＴ４との間におけるＺ軸方向の距離（以下、第４リンク長と呼ぶ）をＬ３と表すこととする。また、基準座標系における第３関節ＪＴ３と第４関節ＪＴ４との間におけるＹ軸方向の距離（以下、第５リンク長と呼ぶ）をＬ４と表すこととする。

アーム部材７の他端部とアーム部材８の一端部との連結部である第５関節ＪＴ５においてアーム部材７に対するアーム部材８の第５回転軸Ａ５周りの回転（回動）が許容されている。なお、第５関節ＪＴ５には、当該第５関節ＪＴ５の関節座標系（Ｘ５軸、Ｙ５軸、Ｚ５軸による直交座標系）が設定されている。

アーム部材８の他端部とアタッチメント９の一端部との連結部である第６関節ＪＴ６においてアーム部材８に対するアタッチメント９の第６回転軸Ａ６周りの回転（旋回）が許容される。なお、第６関節ＪＴ６には、当該第６関節ＪＴ６の関節座標系としてＸ６軸、Ｙ６軸、Ｚ６軸による直交座標系が設定されている。このＸ６軸、Ｙ６軸、Ｚ６軸による直交座標系は、第４乃至第６回転軸Ａ４乃至Ａ６が一点で交わった点を原点に持っている。ここで、基準座標系における第５関節ＪＴ５と第６関節ＪＴ６との間におけるＺ軸方向の距離（以下、第６リンク長と呼ぶ）をＬ５と表すこととする。

基台２が床面に適正に設置されると、基台２に最も近い第１回転軸Ａ１は鉛直方向（基台２から第１関節ＪＴ１への延在方向）に指向し、第２回転軸Ａ２は水平方向（床面と平行な方向）に指向する。また、第７回転軸Ａ７は第２回転軸Ａ２と直交する方向であって、かつアーム部材４の延在方向に指向する。また、第３回転軸Ａ３は第７回転軸Ａ７と直交する方向であって、かつ水平方向に指向する。また、第４回転軸Ａ４は第３回転軸Ａ３と直交する方向であって、かつアーム部材６の延在方向に指向する。また、第５回転軸Ａ５は第４回転軸Ａ４と直交する方向であって、かつ水平方向に指向する。また、第６回転軸Ａ６は第５回転軸Ａ５と直交する方向であって、かつアーム部材８の延在方向に指向する。よって、第１乃至第７回転軸Ａ１〜Ａ７は、全ての互いに隣り合う関節の回転軸が互いに垂直となるように配置されている。

アーム部材７、８、及びアタッチメント９は、アタッチメント９に取り付けられるツール部材１１に微細な動作を行わせるための人間の手首に似せた構造体１０（以下、手首装置という。）を成している。第１回転軸Ａ１、第２回転軸Ａ２、第７回転軸Ａ７、及び第３回転軸Ａ３は、ツール部材１１を手首装置１０とともに水平旋回させたり揺動させたりするための回転軸として用いられ、７軸多関節ロボット１００の主軸を成している。第４乃至第６回転軸Ａ４〜Ａ６は、手首装置１０に設定される回転軸であり、所謂ＲＢＲ（Ｒｏｌｌ−Ｂｅｎｄ−Ｒｏｌｌ）型の手首軸を成している。なお、手首装置１０の手首軸は、ＲＢＲ型に限られず、所謂ＢＢＲ（Ｂｅｎｄ−Ｂｅｎｄ−Ｒｏｌｌ）型や３Ｒ（Ｒｏｌｌ−Ｒｏｌｌ−Ｒｏｌｌ）型の手首軸であってもよい。

第１乃至第７関節ＪＴ１〜ＪＴ７にはそれぞれサーボモータＭ１〜Ｍ７及び位置検出器Ｄ１〜Ｄ７が設けられている。位置検出器Ｄ１〜Ｄ７は、例えば、ロータリーエンコーダで構成されている。上記の各サーボモータＭ１〜Ｍ７を駆動することにより、第１乃至第７関節ＪＴ１〜ＪＴ７においてそれぞれ許容される第１乃至第７回転軸Ａ１〜Ａ７周りの回転が行われる。なお、各サーボモータＭ１〜Ｍ７は互いに独立して駆動することが可能である。また、上記の各サーボモータＭ１〜Ｍ７が駆動されると、上記の各位置検出器Ｄ１〜Ｄ７によって上記の各サーボモータＭ１〜Ｍ７の第１乃至第７回転軸Ａ１〜Ａ７周りの回転位置の検出が行われる。

ロボット制御装置２００は、演算器２１０と記憶器２２０とを少なくとも備えて構成されており、７軸多関節ロボット１００の基台２の周辺に配置されている。ロボット制御装置２００は、例えば、コンピュータで構成され、演算器２１０及び記憶器２２０は、それぞれ、ＣＰＵ及び内部メモリで構成される。なお、ロボット制御装置２００は、７軸多関節ロボット１００と遠隔に配置されていてもよいし、７軸多関節ロボット１００と物理的に着脱可能な形態で接続されてもよい。

ロボット制御装置２００は、７軸多関節ロボット１００の動作を制御する。具体的には、ロボット制御装置２００は、７軸多関節ロボット１００の第１乃至第７関節ＪＴ１〜ＪＴ７が具備するサーボモータのサーボ制御により、アタッチメント９に取り付けられたツール部材１１を任意の位置及び姿勢に任意の経路に沿って移動させる。

なお、ロボット制御装置２００は、ティーチペンダント等の外部装置（図示せず）と接続可能である。この外部装置によって、ロボット制御装置２００は、オペレータの操作に応じて７軸多関節ロボット１００を並進又は回転運動させるための指令が入力される。演算器２１０は、制御時間毎に入力された上記の指令に基づいて、ツール部材１１が当該制御時間の経過後に位置すべき目標位置を算出する。かかる目標位置は、制御時間と予め定められたツール部材１１の制限移動速度等から求まる移動距離に基づいて算出された後、演算器２１０が演算可能となるようにツール座標系（Ｘｔ軸、Ｙｔ軸、Ｚｔ軸による直交座標系）において定義される座標データという形式に変換される。さらに、演算器２１０は、目標位置の座標データの逆変換処理を行い、制御時間の経過後にツール部材１１を目標位置に移動させるために必要となる関節角θ１〜θ７を算定する。そして、演算器２１０は、これらの算定した関節角θ１〜θ７と上記の位置検出器Ｄ１〜Ｄ７により検出される電源オン時の回転位置との偏差に基づき、第１乃至第７関節ＪＴ１〜ＪＴ７に設けられたサーボモータの動作量の指令値を演算し、各サーボモータに供給する。これにより、ツール部材１１が制御時間の経過毎に目標位置に移動されることとなる。
［逆変換式の定式化］
以下では、７軸多関節ロボット１００の逆変換式の定式化を説明する。なお、手首装置１０の姿勢を規定する手首軸（Ａ４〜Ａ６）はひとまず無視しておき、手首装置１０の位置を規定する残りの４つの回転軸Ａ１、Ａ２、Ａ７、Ａ３のうち、第７回転軸Ａ７を冗長軸とし、かつ残りの第１乃至第３回転軸Ａ１〜Ａ３を基軸とし、当該基軸Ａ１〜Ａ３を有した各関節ＪＴ１〜ＪＴ３の関節角θ１〜θ３を未知変数として取り扱う。また、第３回転軸Ａ３を、後述の４次方程式の変数として用いられる関節角θ３を規定する変数軸とする。

まず、基準座標系から見た第１関節ＪＴ１の位置ベクトル［ｘ０，ｙ０，ｚ０］、第２関節ＪＴ２の位置ベクトル［ｘ１，ｙ１，ｚ１］、第３関節ＪＴ３の位置ベクトル［ｘ３，ｙ３，ｚ３］を、それぞれ“ｘｙｚ０”、“ｘｙｚ１”、“ｘｙｚ３”と表し、第ｉ関節ＪＴｉ（第ｉ回転軸Ａｉ）の関節座標系から見た第ｊ関節ＪＴｊ（第ｊ回転軸Ａｊ）の関節座標系を、同次変換行列（座標変換行列）である「^ｉＴ_ｊ」を用いて表したとき、次式が成り立つ。なお、同次変換行列^ｉＴ_ｊとは、位置ベクトルとオイラー角を用いた回転行列とを組み合わせた行列であり、ロボットの運動学の定式化において広く用いられている行列のことである。

ｘｙｚ１＝^１Ｔ_２・^２Ｔ_７・^７Ｔ_３・ｘｙｚ３ ・・・（６−１）
ｘｙｚ０＝^０Ｔ_１・^１Ｔ_２・^２Ｔ_７・^７Ｔ_３・ｘｙｚ３
＝^０Ｔ_１・ｘｙｚ１ ・・・（６−２）
式（６−２）の両辺に（^０Ｔ_１）^−１を左から掛けるとすると、次式が成り立つ。

（^０Ｔ_１）^−１・ｘｙｚ０＝ｘｙｚ１ ・・・（７）
また、式（７）により、第２関節ＪＴ２の位置ベクトルの各要素であるｘ１、ｙ１、ｚ１についてそれぞれ次式の関係が得られる。なお、次式の中で、ｓｉは第ｉ関節の関節角θ_ｉに応じたｓｉｎ（θ_ｉ）を表しており、ｃｉは第ｉ関節の関節角θ_ｉに応じたｃｏｓ（θ_ｉ）を表している。

ｘ１＝ｃ１・ｘ０＋ｓ１・ｙ０ ・・・（８−１）
ｙ１＝−ｓ１・ｘ０＋ｃ１・ｙ０ ・・・（８−２）
ｚ１＝ｚ０−Ｌ０ ・・・（８−３）
ここで、第１関節ＪＴ１の関節座標系を基準とした場合、基準座標系の原点Ｏから手首装置１０の位置に見立てた第３関節ＪＴ３から延設される第４関節ＪＴ４の関節座標系の原点までの距離Ｒｈ及び距離Ｚｈについては、式（８−１）〜式（８−３）で表されたｘ１、ｙ１、ｚ１を用いて、次式の関係が成立する。

Ｒｈ^２＝ｘ１^２＋ｙ１^２＋ｚ１^２＝ｘ０^２＋ｙ０^２＋（ｚ０−Ｌ０）^２ ・・・（９−１）
Ｚｈ＝ｚ０−Ｌ０ ・・・（９−２）
一方、第２関節ＪＴ２の位置ベクトルの各要素であるｘ１、ｙ１、ｚ１については、式（８−１）〜式（８−３）とは別に、式（６−１）により、それぞれ次式の関係が得られる。

ｘ１＝−ｓ３・ｓ７・ｘ３−ｃ３・ｓ７・ｙ３−ｃ７・ｚ３ ・・・（１０−１）
ｙ１＝（ｃ２・ｓ３・ｃ７＋ｓ２・ｃ３）・ｘ３＋（ｃ２・ｃ３・ｃ７−ｓ２・ｓ３）・ｙ３−ｃ２・ｓ７・ｚ３＋Ｌ１＋Ｌ２・ｓ２ ・・・（１０−２）
ｚ１＝（−ｓ２・ｓ３・ｃ７＋ｃ２・ｃ３ ）・ｘ３−（ｓ２・ｃ３・ｃ７＋ｃ２・ｓ３）・ｙ３＋ｓ２・ｓ７・ｚ３＋Ｌ２・ｃ２ ・・・（１０−３）
これらを用いると、式（９−１）におけるＲｈ^２は次式のとおり表すことができる。

Ｒｈ^２＝ｘ１^２＋ｙ１^２＋ｚ１^２
＝２・Ｌ１・［（ｃ３・ｘ３−ｓ３・ｙ３＋Ｌ２）・ｓ２−｛−ｃ７・（ｓ３・ｘ３＋ｃ３・ｙ３）＋ｓ７・ｚ３｝・ｃ２］＋２・Ｌ２・ｃ３・ｘ３−２・Ｌ２・ｓ３・ｙ３＋Ｌ１^２＋Ｌ２^２＋ｘ３^２＋ｙ３^２＋ｚ３^２ ・・・（１１−１）
また、Ｚｈは、ｚ１を変形して次式のとおり表すことができる。

Ｚｈ＝（ｃ３・ｘ３−ｓ３・ｙ３＋Ｌ２）・ｃ２＋｛−ｃ７・（ｓ３・ｘ３＋ｃ３・ｙ３）＋ｓ７・ｚ３｝・ｃ２ ］・ｓ２ ・・・（１１−２）
ここで、
Ｆ１＝ｃ３・ｘ３−ｓ３・ｙ３＋Ｌ２ ・・・（１２−１）
Ｆ２＝−ｃ７・（ｓ３・ｘ３＋ｃ３・ｙ３）＋ｓ７・ｚ３ ・・・（１２−２）
Ｆ３＝２・Ｌ２・ｃ３・ｘ３‐２・Ｌ２・ｓ３・ｙ３＋Ｌ１^２＋Ｌ２^２＋ｘ３^２＋ｙ３^２＋ｚ３^２ ・・・（１２−３）
とおくと、式（１１−１）及び式（１１−２）については、それぞれ次式のとおり変形することができる。

（Ｒｈ^２−Ｆ３）／（２・Ｌ１）＝Ｆ１・ｓ２−Ｆ２・ｃ２ ・・・（１３−１）
Ｚｈ＝Ｆ１・ｃ２＋Ｆ２・ｓ２ ・・・ （１３−２）
ここで、式（１３−１）及び式（１３−２）それぞれの辺々を２乗して、それぞれの和をとると、次式が成立する。

｛（Ｒｈ^２−Ｆ３）／（２・Ｌ１）｝^２＋Ｚｈ^２＝Ｆ１^２＋Ｆ２^２ ・・・（１４）
そして、式（１４）に対し、式（１１−１）、式（１１−２）、式（１２−１）〜式（１２−３）を代入し、ｓ３、ｃ３について整理すると、次式が成立することとなる。

Ａ・ｃ３^２＋Ｂ・ｓ３^２＋Ｃ・ｓ３・ｃ３＋Ｄ・ｃ３＋Ｅ・ｓ３＋Ｆ＝０ ・・・（１５）
なお、式（１５）中のＡ、Ｂ、Ｃ、Ｄ、Ｅ、Ｆは以下の通りとなる。

Ａ＝４・Ｌ１^２・（ｘ３^２＋ｙ３^２・ｃ７^２）−４・Ｌ２^２・ｘ３^２ ・・・（１６−１）
Ｂ＝４・Ｌ１^２・（ｙ３^２＋ｘ３^２・ｃ７^２）−４・Ｌ２^２・ｙ３^２ ・・・（１６−２）
Ｃ＝４・Ｌ１^２・（−２・ｘ３・ｙ３＋２・ｘ３・ｙ３・ｃ７^２）＋８・Ｌ２^２・ｘ３・ｙ３ ・・・（１６−３）
Ｄ＝４・Ｌ１^２・（２・ｘ３・Ｌ２−２・ｙ３・ｚ３・ｓ７・ｃ７）−４・Ｌ２・ｘ３・Ｋ ・・・（１６−４）
Ｅ＝４・Ｌ１^２・（−２・ｙ３・Ｌ２−２・ｘ３・ｚ３・ｓ７・ｃ７）＋４・Ｌ２・ｙ３・Ｋ ・・・（１６−５）
Ｆ＝４・Ｌ１^２・（Ｌ２^２＋ｚ３^２・ｓ７^２−Ｚｈ^２）−Ｋ^２ ・・・（１６−６）
なお、式（１６−１）〜式（１６−６）中において、Ｌ１は上記の第１リンク長を表しており、Ｌ２は上記の第２リンク長を表している。また、Ｋは次式により得られる値である。

Ｋ＝Ｌ１^２＋Ｌ２^２＋ｘ３^２＋ｙ３^２＋ｚ３^２‐Ｒｈ^２・・・（１７）
式（１６−１）〜式（１６−６）の各値は、ｓ１，ｃ１，ｓ２，ｃ２に無関係な値であり、各関節角の情報がなくても求めることができる。

次に、ｔ＝ｔａｎ（θ３）とおくと、第３関節ＪＴ３の関節角θ３に関するｃ３（余弦）及びｓ３（正弦）について次式が成立する。

ｃ３＝２・ｔ／（１＋ｔ^２）・・・（１８−１）
ｓ３＝（１−ｔ^２）／（１＋ｔ^２）・・・（１８−２）
ここで、式（１８−１）、式（１８−２）を式（１５）に代入し、変数ｔ（＝ｔａｎ（θ３））の次数毎に項を整理すると、次式が成立する。

（Ｂ−Ｅ＋Ｆ）・ｔ^４＋２（−Ｃ＋Ｄ）・ｔ^３＋（２Ａ−Ｂ＋Ｆ）・ｔ^２＋２（Ｃ＋Ｄ ）・ｔ＋（Ｂ＋Ｅ＋Ｆ）＝０ ・・・（１９）
以上のとおり、式（１９）により表される４次方程式を解くことで変数ｔを求め、θ３＝ａｔａｎ（ｔ）を演算することにより、関節角θ３を得ることができる。

一方、関節角θ２については、次のように求めることができる。まず、式（１９）により求められた関節角θ３を、式（１２−１）、式（１２−２）にそれぞれ代入すると、Ｆ１，Ｆ２が求められる。ここで、Ｚｈは既知であるので、式（１３−２）より、関節角θ２は次式により求められる。なお、次式の演算結果としては、式（１３−１）、式（１３−２）を満たすものを選択することとする。

θ２＝ａｔａｎ２（Ｆ２，Ｆ１）±ａｔａｎ２（（Ｆ２^２＋Ｆ１^２−Ｚｈ^２）^０．５，Ｚｈ）・・・（２０）
また、関節角θ_１については、次のように求めることができる。まず、式（８−１）〜（８−３）と式（１０−１）〜（１０−３）とを用いてｘ１を消去すると、次式が成立する。

ｃ１・ｘ０＋ｓ１・ｙ０＝−ｓ３・ｓ７・ｘ３−ｃ３・ｓ７・ｙ３−ｃ７・ｚ３ ・・・（２１）
ここで、式（２１）の右辺をＱとおき、式（２１）の両辺に式（１９）により求められた関節角θ３を代入することで、θ１は次式により求められる。なお、次式の演算結果としては、式（８−１）〜式（８−３）を満たすものを選択することとする。

θ１＝ａｔａｎ２（ｙ０，ｘ０）±ａｔａｎ２（（ｙ０^２＋ｘ０^２−Ｑ^２）^０．５ ， Ｑ）・・・（２２）
以上のようにして、関節角θ１、θ２、θ３に関して、式（１９）、式（２１）、式（２２）のとおり、７軸多関節ロボット１００の逆変換式が定式化される。なお、この逆変換式の定式化は予め行われ、この定式化された逆変換式が制御プログラムに組み込まれている。
［４つの第１求解演算式と４つの形態との対応付け］
つぎに、式（１９）の４次方程式の４つの解を求めるための４つの第１求解演算式と７軸多関節ロボット１００の４つの形態との対応付けについて説明する。

まず、式（１９）により表される４次方程式の各係数を整理して次式に変換する。

ｔ^４＋ａｔ^３＋ｂｔ^２＋ｃｔ＋ｄ＝０ ・・・（２３）
また、
ｐ＝−３ａ^２／８＋ｂ ・・・（２４−１）
ｑ＝ａ^３／８−ａｂ／２＋ｃ ・・・（２４−２）
ｒ＝−３ａ^４／２５６＋ａ^２ｂ／１６−ａｃ／４＋ｄ ・・・（２４−３）
とおく。ここで、４次方程式の解法としてよく知られた「フェラーリの解法」に基づいて、式（２３）の４次方程式の分解方程式である変数ｙについての３次方程式を次式のとおり定式化する。

８ｙ^３−４ｐｙ^２−８ｒｙ＋４ｐｒ−ｑ^２＝０ ・・・（２５）
なお、式（２５）の３次方程式の各係数を整理して次式に変換する。

ｘ^３＋ｆｘ^２＋ｇｘ＋ｈ＝０ ・・・（２６）
ここで、式（２６）式の判別式Ｄは、次式の通り表される。

Ｄ＝ｑ^２＋ｐ^３ ・・・（２７）
なお、ｐ及びｑは次式の通りである。

ｐ＝（ｇ−ｆ^２ ／３）／３ ・・・（２８−１）
ｑ＝（ｈ−ｇ・ｆ／３＋２・ｆ^３／２７）／２ ・・・（２８−２）
また、式（２５）の３次方程式の解は、式（２７）の判別式Ｄの解に基づいた３つの条件Ｃ１、Ｃ２、Ｃ３毎に分別される。

条件Ｃ１（Ｄ＜０の時）：実数解３つ
条件Ｃ２（Ｄ＝０の時）：実数の重解と他の実数解又は実数の３重解
条件Ｃ３（Ｄ＞０の時）：実数解１つと虚数解２つ（共役複素数）
ここで、条件Ｃ２又は条件Ｃ３の場合には、負の√演算の必要がない「カルダノの解法」に従って、式（２５）の３次方程式の解を求めることとする。なお、カルダノの解法とは３次方程式の代表的な解法である。具体的には、まず、
Ｗ１＝（−１＋３^０．５・ｉ）／２ ・・・（２９−１）
Ｗ２＝Ｗ１^２ ・・・（２９−２）
Ｕ１＝−ｑ＋Ｄ^０．５ ・・・（２９−３）
Ｕ２＝−ｑ−Ｄ^０．５ ・・・（２９−４）
とおくと、カルダノの解法における式（２５）の３次方程式の３つの解ｕ１、ｕ２、ｕ３は、次式により求められる。

ｕ１＝１・Ｕ１^{（１／３）}＋１・Ｕ２^{（１／３）}−ａ／３ ・・・（３０−１）
ｕ２＝Ｗ１・Ｕ１^{（１／３）}＋Ｗ２・Ｕ２^{（１／３）}−ａ／３ ・・・（３０−２）
ｕ３＝Ｗ２・Ｕ１^{（１／３）}＋Ｗ１・Ｕ２^{（１／３）}−ａ／３ ・・・（３０−３）
なお、式（３０−１）〜式（３０−３）中のＵ１^{（１／３）}及びＵ２^{（１／３）}は３つの値が考えられるが、実数の値であるものを用いることとする。つまり、Ｕ１又はＵ２が負の場合には、これらの絶対値の３乗根を求めた上で、それに−１をかけて算定することで、虚数の算定を避けることとする。また、条件Ｃ２又はＣ３の場合には、３つの解（ｕ１、ｕ２、ｕ３）のうち、実数解となる解ｕ１のみを演算することとする。なお、条件Ｃ２又はＣ３の場合において実数解ｕ１を求めるための式（３０−１）は、本発明に係る「第２求解演算式」に対応する。

次に、条件Ｃ１の場合には、「ビエタの解法」により式（２５）の３次方程式の解を求めることとする。なお、ビエタの解法とは三角関数の三倍角の公式を利用したカルダノの解法とは異なる３次方程式の既知の解法である。具体的には、式（２８−１）及び式（２８−２）を用いると、次式が成立する。なお、次式の中のΦは、同次変換行列を定義する場合に用いたオイラー角の一つを表している。

ｃｏｓ３Φ＝−ｑ／｛（−ｐ）^３｝^{（１／２）} ・・・（３１）
また、式（３１）により次式が成立する。

Φ＝ａｒｃｃｏｓ［‐ｑ／｛（‐ｐ）^３｝^{（１／２）}］／３ ・・・（３２）
なお、条件Ｃ１のときには式（２７）の判別式Ｄ＜０であるので、「ｐ＜０」となり、式（３２）の右辺の平方根の中身は正の数となる。よって、負の√演算が行われることはない。ビエタの解法における式（２５）の３次方程式の３つの解ｕ１，ｕ２，ｕ３は、次式により求められる。

ｕ１＝２（−ｐ）^０．５ ・ｃｏｓΦ−ａ／３ ・・・（３３−１）
ｕ２＝２（−ｐ）^０．５ ・ｃｏｓ（Φ＋２π／３）−ａ／３ ・・・（３３−２）
ｕ３＝２（−ｐ）^０．５ ・ｃｏｓ（Φ＋４π／３）−ａ／３ ・・・（３３−３）
この場合、解ｕ１，ｕ２，ｕ３は全て実数解となるが、無条件にどれでも採用していいわけではなく、たとえ条件Ｃ２又はＣ３になっても実数解となるものを選択する必要がある。そこで、条件Ｃ２又はＣ３のときには共役複素数（虚数）となる２つの実数解は絶対値が同一でかつ符号（±）が異なるという特徴を利用して、条件Ｃ１の場合には、３つの解ｕ１，ｕ２，ｕ３のうち絶対値が同一かつ符号が異なる２つの実数解ｕ２，ｕ３以外の残りの１つの実数解ｕ１のみを特定して演算することとする。なお、条件Ｃ１の場合において実数解ｕ１を求めるための式（３３−１）は、本発明に係る「第３求解演算式」に対応する。

そして、以上のように「カルダノの解法」又は「ビエタの解法」により演算された式（２５）の３次方程式の１つの実数解をｕと表す。さらに、Ｍ１、Ｍ２、Ｎ１、Ｎ２を次式のとおり定義する。

Ｍ１＝−（２ｕ−ｐ）^０．５ ・・・（３４−１）
Ｍ２＝（２ｕ−ｐ）^０．５ ・・・（３４−２）
Ｎ１＝（２ｕ−ｐ）^０．５・ｑ／２（２ｕ−ｐ）＋ｕ ・・・（３４−３）
Ｎ２＝−（２ｕ−ｐ）^０．５・ｑ／２（２ｕ−ｐ）＋ｕ ・・・（３４−４）
このとき、式（２０）の４次方程式の４つの解は次式の第１求解演算式により求められる。

ｔ１＝（−Ｍ１−（Ｍ１^２−４・Ｎ１）^０．５）／２−ａ／４ ・・・（３５−１）
ｔ２＝（−Ｍ１＋（Ｍ１^２−４・Ｎ１）^０．５）／２−ａ／４ ・・・（３５−２）
ｔ３＝（−Ｍ２−（Ｍ２^２−４・Ｎ２）^０．５）／２−ａ／４ ・・・（３５−３）
ｔ４＝（−Ｍ２＋（Ｍ２^２−４・Ｎ２）^０．５）／２−ａ／４ ・・・（３５−４）
ここで、４つの解ｔ１〜ｔ４を求めるための４つの第１求解演算式（式（３５−１）〜式（３５−４））を、図６に示したようなロボットの４つの形態と対応付けることとする。なお、４つの解ｔ１〜ｔ４を求めるための４つの第１求解演算式（式（３５−１）〜式（３５−４））は、式（２５）の３次方程式の３つの解ｕ１、ｕ２、ｕ３のうち実数解ｕ１のみが使用されているので、４つの解ｔ１〜ｔ４の組合せは一義的に求められることとなる。つまり、解ｕ１〜ｕ３がランダムに選択されるということがなく、常に同一の実数解ｕ１のみに基づいて４つの第１求解演算式（式（３５−１）〜式（３５−４））の演算が行われて４つの解ｔ１〜ｔ４が求められるので、７軸多関節ロボット１００の形態の急激な変化を抑えることができる。
［形態に応じた４次方程式の解（ｔ１〜ｔ４のいずれか１つ）の特定］
図３は、４つの第１求解演算式（式（３５−１）〜式（３５−４））のうち１つを特定するための処理の流れを示したフローチャートである。

まず、７軸多関節ロボット１００及びロボット制御装置２００の電源をオンしたとき（ステップＳ３０１）、ロボット制御装置２００の演算器２１０は、第１乃至第７関節ＪＴ１〜ＪＴ７に設けられている位置検出器Ｄ１〜Ｄ７により関節角θ１〜θ７を取得する。そして、演算器２１０は、取得した関節角θ１〜θ７を順変換処理することにより、７軸多関節ロボット１００のロボットアームの電源オン時の先端位置（手首装置１０の位置）を算定する（ステップＳ３０２）。

つぎに、演算器２１０は、算定したロボットアームの先端位置に基づいて上述した逆変換処理を行う。具体的には、まず、式（２５）の３次方程式について、条件Ｃ１の場合にはビエタの解法に従って実数解ｕ１のみを算定し、条件Ｃ２、Ｃ３の場合にはカルダノの解法に従って実数解ｕ１のみを算定する。

つぎに、演算器２１０は、式（２０）の４次方程式について、算定した式（２５）の３次方程式の実数解ｕ１のみを使用して、４つの第１求解演算式（式（３５−１）〜式（３５−４））を演算することにより４つの解ｔ１〜ｔ４をそれぞれ算定する（ステップＳ３０３）。また、これらの算定した４つの解ｔ１〜ｔ４を用いて、それぞれに対応する関節角θ３を算定する（ステップＳ３０４）。

そして、演算器２１０は、ステップＳ３０４において算定された４つの関節角θ３の中で、位置検出器Ｄ３により取得した関節角θ３に最も近い関節角θ３を特定するともに、この特定した関節角θ３を算定するときに用いられた、４つの解ｔ１〜ｔ４のうちの１つを特定する。なお、この１つの解を特定するのに、関節角θ３に代えて、関節角θ１又は関節角θ２を用いてもよい。さらに、この特定した１つの解ｔ１〜ｔ４を求めるための１つの第１求解演算式（式（３５−１）〜式（３５−４））を記憶器２２０に記憶する（ステップＳ３０５）。この結果、記憶器２２０に記憶された１つの第１求解演算式（式（３５−１）〜式（３５−４））が、電源オン後の逆変換処理において用い続けられる。

図４は、記憶器２２０に記憶された４つの第１求解演算式（式（３５−１）〜式（３５−４））のうちいずれか１つを用いて実行される逆変換処理の流れを示すフローチャートである。なお、図３に示される電源オン時の処理によって４次方程式の解ｔ１を求めるための第１求解演算式（３５−１）のみが記憶器２２０に記憶されているものとする。

まず、演算器２１０は、外部装置からツール部材１１の目標位置を取得すると（ステップＳ４０１）、この取得した目標位置を用いて式（２５）の３次方程式の３つの解（ｕ１〜ｕ３）のうち実数解ｕ１のみを算定する（ステップＳ４０２）。

つぎに、演算器２１０は、算定した実数解ｕ１を使用して記憶器２２０に記憶された式（２０）の４次方程式の解ｔ１に関する第１求解演算式（３５−１）を演算する。これにより、式（２０）の４次方程式の解ｔ１が算定される（ステップＳ４０３）。

そして、算定した解ｔ１に対応する関節角θ３を算定する（ステップＳ４０４）。なお、算定した関節角θ３を用いて、式（２２）により関節角θ１が算定され、式（２１）により関節角θ２が算定される（ステップＳ４０５）。

以上のとおり、式（２０）の４次方程式の４つの解ｔ１〜ｔ４を求めるための４つの第１求解演算式（式（３５−１）〜（３５−４））と、４つの解ｔ１〜ｔ４に応じてとり得る４つの形態１〜４とを予め対応付けておき、電源オン時の形態に基づいて４つの第１求解演算式（式（３５−１）〜（３５−４））のうちいずれか１つを、電源オン後の逆変換処理において使用する。この結果、７軸多関節ロボット１００の形態の急激な変化を抑えることが可能となる。

さらに、電源オン時の７軸多関節ロボット１００の初期形態を特定するために、４つの第１求解演算式（式（３５−１）〜（３５−４））についてそれぞれ１回限り演算すればよく、電源オンより後の逆変換処理においては特定された１つの第１求解演算式以外の残りの３つの第１求解演算式については演算しなくても済むので、逆変換処理に要する演算量を抑えることができ、７軸多関節ロボット１００の制御の高速化が図られることとなる。

また、式（２０）の４次方程式を解くためには式（２５）の３次方程式を解くことになるが、当該３次方程式である以上、３つの解ｕ１〜ｕ３が生じてしまう。よって、この３つの解ｕ１〜ｕ３のうちいずれか１つを予め選定しておけば、４次方程式の４つの解ｔ１〜ｔ４の組合せについてのばらつきを抑えることができる。

さらに、電源オン時に４次方程式を解くために３次方程式を解く際に、カルダノの解法とビエタの解法とを使い分けて複素数演算をしなくて済むようにしたので、演算量を抑えることができ、ロボットシステムへの実装が容易なものとなる。

上記説明から、当業者にとっては、本発明の多くの改良や他の実施の形態が明らかである。従って、上記説明は、例示としてのみ解釈されるべきであり、本発明を実行する最良の態様を当業者に教示する目的で提供されたものである。本発明の精神を逸脱することなく、その構造及び／又は機能の詳細を実質的に変更できる。

本発明は、７軸多関節ロボットの逆変換処理にとって有益である。

２ 基台
３ 旋回台
４〜８ アーム部材
９ アタッチメント
１０ 手首装置
１１ ツール部材
ＪＴ１〜ＪＴ７ 第１乃至第７関節
Ａ１〜Ａ７ 第１乃至第７回転軸
Ｍ１〜Ｍ７ サーボモータ
Ｄ１〜Ｄ７ 位置検出器
Ｌ０〜Ｌ５ 第１乃至第６リンク長
１００ ７軸多関節ロボット
２００ ロボット制御装置
２１０ 演算器
２２０ 記憶器

Claims (6)

1. 先端に設けられた手首と基端から当該手首に向かって順に設けられた７つの関節とを具備し、前記７つの関節が回転軸をそれぞれ有しかつそれぞれの回転軸の周りに次の関節を回転させるように構成された７軸多関節ロボットの制御方法であって、
   前記７つの回転軸のうちのいずれか１つを冗長軸に、残りの回転軸のうちの３つを基軸に、かつ当該３つの基軸のうちのいずれか１つを変数軸に定め、かつ、前記基端から前記手首までの直線距離と前記基端に最も近い前記回転軸の延在方向における前記基端から前記手首までの距離とに基づく手首の位置と前記変数軸を有した関節の関節角とに関して定式化された４次方程式を解いて前記手首の位置を前記３つの基軸を有した各関節の関節角に逆変換するステップを含み、
   前記４次方程式は、その４つの解にそれぞれ対応する４つの第１求解演算式を用いて解くことが可能なものであり、かつ、前記７軸多関節ロボットが採り得る形態は、前記４つの解ひいては前記４つの第１求解演算式に対応して定まるものであり、
   電源オン時に、当該電源オン時における前記７軸多関節ロボットの初期形態に基づいて、前記４つの第１求解演算式のうちいずれか１つを特定して記憶し、
   前記電源オンの後、前記記憶した１つの第１求解演算式を用いて前記逆変換するステップを遂行して前記手首の目標位置から前記３つの基軸を有した各関節の関節角を算定し、それにより、前記７軸多関節ロボットの動作を制御する、７軸多関節ロボットの制御方法。
2. 前記電源オン時に、前記４つの第１求解演算式を用いて前記４次方程式を解いて前記４つの解を求め、当該４つの解それぞれについて当該電源オン時における前記手首の位置を前記３つの基軸を有した各関節の関節角の少なくともいずれか１つに逆変換し、当該１つの基軸を有した関節の関節角と前記電源オン時の前記７軸多関節ロボットの初期形態に対応する前記３つの基軸のうちの１つを有した関節の関節角とを比較することにより、前記４次方程式の４つの第１求解演算式のうちいずれか１つを特定して記憶する、請求項１に記載の７軸多関節ロボットの制御方法。
3. 全ての互いに隣り合う前記関節の回転軸が互いに垂直であり、
   前記基端から前記手首に向かって順に設けられた７つの関節を、それぞれ、第１関節、第２関節、第７関節、第３関節、第４関節、第５関節、及び第６関節と定義した場合、前記冗長軸は前記第７関節の回転軸であり、前記３つの基軸は前記第１関節の回転軸、前記第２関節の回転軸、及び前記第３関節の回転軸であり、かつ前記変数軸は前記第３関節の回転軸である、請求項１に記載の７軸多関節ロボットの制御方法。
4. 前記４つの第１求解演算式を用いて前記４次方程式を解く際に、フェラーリの解法に基づいた前記４次方程式を解くための分解方程式として定式化された３次方程式の３つの解に含まれる１つの実数解を求め、かつ当該求めた前記３次方程式の１つの実数解を用いて前記４つの第１求解演算式を演算する、請求項１又は３に記載の７軸多関節ロボットの制御方法。
5. 前記３次方程式の３つの解に含まれる１つの実数解を求める際に、前記３次方程式の判別式が０又は正の場合には、カルダノの解法により求まる前記３次方程式の１つの実数解と２つの虚数解のうち当該１つの実数解に対応した当該カルダノの解法に基づく第２求解演算式を使用し、前記３次方程式の判別式が負の場合には、ビエタの解法により求まる前記３次方程式の３つの実数解のうち絶対値が同一かつ符号の異なる２つの実数解以外の１つの実数解に対応した当該ビエタの解法に基づく第３求解演算式を使用する、請求項４に記載の７軸多関節ロボットの制御方法。
6. 先端に設けられた手首と基端から当該手首に向かって順に設けられた７つの関節とを具備し、前記７つの関節が回転軸をそれぞれ有しかつそれぞれの回転軸の周りに次の関節を回転させるように構成された７軸多関節ロボットの制御方法をコンピュータに遂行させる、７軸多関節ロボットの制御プログラムであって、
   前記制御方法は、
   前記７つの回転軸のうちのいずれか１つを冗長軸に、残りの回転軸のうちの３つを基軸に、かつ当該３つの基軸のうちのいずれか１つを変数軸に定め、かつ、前記基端から前記手首までの直線距離と前記基端に最も近い前記回転軸の延在方向における前記基端から前記手首までの距離とに基づく手首の位置と前記変数軸を有した関節の関節角とに関して定式化された４次方程式を解いて前記手首の位置を前記３つの基軸を有した各関節の関節角に逆変換するステップを含み、
   前記４次方程式は、その４つの解にそれぞれ対応する４つの第１求解演算式を用いて解くことが可能なものであり、かつ、前記７軸多関節ロボットが採り得る形態は、前記４つの解ひいては前記４つの第１求解演算式に対応して定まるものであり、
   電源オン時に、当該電源オン時における前記７軸多関節ロボットの初期形態に基づいて、前記４つの第１求解演算式のうちいずれか１つを特定して記憶し、
   前記電源オンの後、前記記憶した１つの第１求解演算式を用いて前記逆変換するステップを遂行して前記手首の目標位置から前記３つの基軸を有した各関節の関節角を算定し、それにより、前記７軸多関節ロボットの動作を制御する、７軸多関節ロボットの制御プログラム。

Images