JP2005193311A

JP2005193311A - 冗長マニピュレータの制御方法

Info

Publication number: JP2005193311A
Application number: JP2003435493A
Authority: JP
Inventors: Atsushi Nakajima; 厚中島; Isao Yamaguchi; 功山口; Osamu Okamoto; 修岡本; Yoshiaki Okami; 嘉彰狼
Original assignee: Japan Aerospace Exploration Agency JAXA
Current assignee: Japan Aerospace Exploration Agency JAXA
Priority date: 2003-12-26
Filing date: 2003-12-26
Publication date: 2005-07-21
Anticipated expiration: 2023-12-26
Also published as: US20050143860A1; US7756606B2; JP4735795B2

Abstract

【課題】複数の冗長関節を持ち得る一般的な構造の冗長マニピュレータであっても、複数の関節から一つ又は複数の冗長関節を適切に割り当てて、高速で逆キネマティクス問題の解を得る冗長マニピュレータの制御方法を提供する。
【解決手段】関節を冗長関節と非冗長関節とに任意に切り分けし（Ｓ１）、切り分けされた冗長関節の関節角をパラメータとして初期値を設定する（Ｓ３）。非冗長関節の関節角が逆キネマティクス計算によって数値解として解かれる（Ｓ４）。パラメータとして与えられる冗長関節の関節角と、変化したパラメータに応じて逆キネマティクス計算で求まる非冗長関節の関節角とを用いて定義された評価関数又は拘束条件（Ｓ２）に基づいて関節角の組の最適解を求め（Ｓ５）〜（Ｓ７）、最適解が手先位置の目標範囲をカバーする（Ｓ８）まで、拘束条件の緩和を行って最適解を求める手順を繰り返す。
【選択図】図１

Description

この発明は、非冗長関節と冗長関節とから成る関節を介して複数のアームを順次繋いで構成される冗長マニピュレータにおいて、その手先位置と姿勢とを条件として与えたときに各関節が取るべき関節角を定める冗長マニピュレータの制御方法に関する。

従来、複数の関節を介して複数のアームを順次繋いで構成し、手先に取り付けた各種のツールで種々の作業を行わせるマニピュレータの開発が行われてきている。多関節を有するマニピュレータに所定の作業をさせようとするときには、マニピュレータの手先の位置と姿勢を制御する必要がある。各関節の角度を指定するときに手先が占める位置と方向は行列計算によって一義的に決定することができ、この問題は、運動学（キネマティクス）問題と称されている。これに対してマニピュレータの手先の位置と姿勢を条件（３次元での条件となるので、６個の変数）として設定したときに、各関節角度をどのように決定するかは、逆運動学（逆キネマティクス）問題として知られている。マニピュレータに作業をさせるには逆運動学問題の解を求める計算が必須であるが、逆キネマティクス問題においては、どのような形式のマニピュレータでも成立する一般的な解析解は無いとされている。逆に、実用となり得るマニピュレータやロボットは、逆キネマティクス問題の解析解が得られる構造を持つものが多数である。また、逆キネマティクス問題の解を得るのに時間がかかり過ぎるのでは、時々刻々変化させるべき手先の位置・姿勢に各関節角を追いつかせる制御を行うことができない。それゆえ、マニピュレータに正確で高速な作業を行わせるには、オンライン・リアルタイムで逆キネマティクス問題の数値解を求めことが研究されている。

一方、３次元空間の場合には、位置と方向とはそれぞれ３つの変数によって決定される。ロール軸を含めて７個以上の関節を有するマニュピュレータは、手先の位置と方向を定めるのに必要な６個を超える数の関節を含んでいる。そのような余剰の関節は冗長関節と称されており、冗長関節を一つ以上含むマニュピュレータは冗長マニュピュレータと呼ばれている。実用マニピュレータにおいては、手先の位置とロール軸以外の方向とを定めておいて、その状態で手先を自身のロール軸の回りに回転動作させることで必要な作業をさせることが行われている。その場合、マニピュレータの手先の関節がするロール軸回転は手先の位置や他の軸回りの回転とは独立に扱うことができるので、逆キネマティクス問題として方向を定めるべき独立な変数は、１つ減って２つとすることができる。従って、逆キネマティクス問題は、３次元空間における位置を定める３つの変数と合わせた、計５個の変数を解く問題となる（非特許文献１参照）。冗長マニュピュレータにおいても、冗長関節とその関節角を特定すればロール軸以外の５個の関節すべてが非冗長関節である非冗長マニュピュレータとなり、上記のように、通常、５個の関節角を逆キネマティクス問題として解くことに帰着する。この逆キネマティクス問題を解くに際して、手先の位置と姿勢の統合ベクトルを定義し、ニュートン・ラフソン法による繰り返しによる収束計算によってマニピュレータの関節角の数値解を得ることが提案されている。
狼嘉彰；岡本修；柴田智哉、第４４回自動制御連合講演会講演集、Ｎｏ．０１−２５３、２００１年１１月２２日、ｐ４６２−４６３

７自由度以上の冗長マニピュレータの制御の一例として、１個の関節を冗長自由度制御用関節とし、それ以外の６個の関節を手先運動制御用関節として割り当て、６個の手先運動制御用関節によって、６自由度（ロール軸を含む）を有する手先の運動を決定する制御方式が提案されている（特許文献１参照）。この冗長マニピュレータの制御方式によれば、冗長自由度制御手段は、角度検出手段が検出したデータを用いて運動学（順キネマティクス）方程式によって手先の位置及び姿勢を求めている。いずれかの関節を冗長自由度制御用関節として定めればその関節の関節角はパラメータ化された関節角となり、マニピュレータは手先運動制御用関節のみの６自由度となるので、パラメータに依存する解として逆キネマティクス問題の方程式を解くことができる。その計算で得られた各関節角等を用いて評価関数のポテンシャル値を求め、その評価によって、パラメータとして扱われていた冗長自由度制御用関節の関節角が決定され、手先運動制御用関節の速度指令値が出力される。一方、手先運動制御手段は、現在の関節角と運動目標値指令手段からの手先の運動目標値とを用いて逆運動学方程式を解き、手先運動制御用関節の速度指令値が求められる。求められた速度指令値を各関節制御手段に送ることで、マニピュレータの動作が制御される。冗長自由度制御と手先運動制御とを分けて行っており、制御周期を別々に設定してサンプリング周期を短くすることを可能とし、冗長マニピュレータを実時間で動作させることを図っている。
特開平６−１４３１７２号公報（段落［００３０］〜［００４７］，図１、図４、図５）

しかしながら、上記の従来技術においては、冗長関節は一つであり、一つの冗長関節が定められたとしたときにその冗長関節の関節角をどのような大きさに定めるかについては開示がされているが、複数の関節から冗長関節の割り当てを如何に決定するか、あるいは冗長関節が多いときの割り当てについては如何にするかについては、開示がなく、経験的に適当に定められているのが実情である。

そこで、複数の冗長関節を持ち得る一般的な構造の冗長マニピュレータであっても、複数の関節から一つ又は複数の冗長関節を適切に割り当てて、冗長関節を割り当てた後に非冗長関節の関節角を高速で解くことで逆キネマティクス問題の解を得る点で解決すべき課題がある。

この発明の目的は、複数の関節から一つ又は複数の冗長関節を自動的に且つ適切に割り当てることによって、任意の一般的なマニピュレータであっても逆キネマティクス問題の解を高速で得ることを可能にするロボットの制御方法を提供することである。

上記の課題を解決するため、この発明による冗長マニピュレータの制御方法は、冗長関節と非冗長関節とから成る関節を介して複数のアームを手先まで順次繋いで成る冗長マニピュレータの前記手先についてその位置と方向を指定したときに前記各関節が呈すべき関節角を決定して前記手先の作動を制御する冗長マニピュレータの制御方法において、前記関節を前記冗長関節と前記非冗長関節とに切り分けし、切り分けされた前記冗長関節の関節角をパラメータとして変化させたとき、前記パラメータとして与えられる前記冗長関節の前記関節角と、逆キネマティクス計算で解として得られた前記非冗長関節の関節角とを用いた評価関数及び拘束条件に基づいて前記関節角の最適解を求め、前記最適解が前記手先位置の目標範囲をカバーするか否かを判定し、否の判定であるときには前記評価関数の変更又は前記拘束条件の緩和を行って前記最適解を求める手順を繰り返すことから成っている。

この冗長マニピュレータの制御方法においては、冗長マニピュレータは一つ又は複数の冗長関節と非冗長関節とから成る関節を介して複数のアームを手先まで順次繋いで構成されており、前記手先の位置と方向を指定したときに、各関節が呈すべき関節角を決定して手先の位置と方向を含んだ作動を制御している。先ず、関節を冗長関節と非冗長関節とに任意に切り分けし、切り分けされた冗長関節の関節角をパラメータとして初期値を設定する。冗長関節についてはパラメータ値として関節角が設定されるので、非冗長関節の関節角については逆キネマティクス計算によって、例えば数値解として解くことができる。パラメータとして設定された冗長関節の関節角と、逆キネマティクス計算によって解かれた非冗長関節の関節角の組合せは、最適か否かは直ちには不明である。そこで、予め、パラメータを変化させたときに、パラメータとして与えられる冗長関節の関節角と、変化したパラメータに応じて逆キネマティクス計算で求まる非冗長関節の関節角とを用いた評価関数及び拘束条件を定義する。こうした評価関数及び拘束条件に基づいて関節角の組の最適解を求め、最適解が手先位置の目標範囲をカバーする、即ち手先位置の最終的な目標範囲に到達するときには、その最適解を最終的な最適解とする。手先位置の目標範囲をカバーしないときには、評価関数の変更又は拘束条件の緩和を行って、再度、最適解を求める手順を繰り返し、手先位置の目標範囲をカバーする最終的な最適解を求める。

この冗長マニピュレータの制御方法において、前記評価関数は、前記冗長関節の前記関節角の二乗和であるとすることができる。また、前記評価関数は、隣り合う二つの前記関節角の差の二乗和であるとすることができる。マニピュレータの制御において、どの関節角についても値が小さい方が、各関節を作動させるときに効率的である。そこで、評価関数を冗長関節の中立点からの角度変位量である関節角の二乗和として定義し、その二乗和が最小（極小）になように評価することが考えられる。また、評価関数を隣り合う二つの関節角の差の二乗和として定義することで、隣り合う二つの関節角が大きく変化する場合を低く評価し、隣り合う二つの関節で関節角の変化量が小さいときを高く評価することも考えられる。なお、前記評価関数において、前記二乗和は、各項に総和が１となる正の重み係数をそれぞれ乗じて得られる重み係数付き二乗和として、評価関数において、関節毎に重み付けをすることもできる。

この冗長マニピュレータの制御方法において、前記評価関数は、前記各関節角の角速度が与えられたときの手先の感度を与えるヤコビアン行列式の絶対値の逆数であるとすることができる。即ち、ヤコビアン行列式の絶対値の逆数は、手先を単位角速度変更するのに、各関節角の角速度を変更する変更量が少ないほど効果的な制御を行うことができるという指標として捉えることができる。なお、この方法は、関節リミットの拘束条件を考慮することが困難になる場合があるので、関節角の組の最適解を得るための繰返し計算をする場合には、評価関数を変更することもあり得る。

この冗長マニピュレータの制御方法において、前記逆キネマティクス計算と前記関節角の最適解を求める計算で、前記手先の位置と姿勢ベクトルとから成る統合ベクトルを用いることができる。手先位置ベクトルと手先方向ベクトルとを統合した統合ベクトルを用いて、手先の状態を表すことができる。また、その次元は、手先のロール軸回りの方向を除いた、位置について３次元と方向について２次元との合計５次元とすることができる。

前記統合ベクトルは、各要素に総和が１となる正の重み係数をそれぞれ乗じて得られる重み係数付き要素から成る修正された統合ベクトルとすることができる。統合ベクトルを、各要素を重みで補正した修正統合ベクトルとすることで、関節角の最適解を求める際して、統合ベクトルを構成する各要素に重みを持たせることができる。

この冗長マニピュレータの制御方法において、前記逆キネマティクス計算は、前記関節角に初期値を与えた後、ヤコビアンの逆行列に補正前の前記統合ベクトルと目標統合ベクトルとの差ベクトルを乗じた積を補正前の前記関節角に対して差し引いて補正後の前記関節角とし、前記補正後の前記関節角に基づく順キネマティクス計算によって補正後の前記統合ベクトルを得る繰返し計算を行い、前記差ベクトルのノルムが所定の値以下に収束したときの補正後の前記関節角を近似解とするニュートン・ラプソン法によって行うことができる。ヤコビアンの逆行列を用いて補正後の各関節角を求め、補正後の各関節角を用いて順キネマティクス計算によって補正後の統合ベクトルを得ることができる。補正前後の統合ベクトルの差のノルムが所定の値以下に収束するときに、繰返し計算は収束したと判断できるので、補正後の関節角を逆キネマティクス計算の近似解として数値的に求める得ることができる。

この冗長マニピュレータの制御方法において、前記手先のロール角は、他の前記関節角とは独立して決定することができる。手先のロール角は、手先の他の位置と方向の５次元のベクトルが定まった後に、必要な回転量だけ定めることができるので、独立して決定することが可能である。

この発明は、上記のように構成されているので、ロボットの制御に必要な逆キネマティクス問題の解を数値解として高速計算でき、関節角度の制御をオンライン・リアルタイムで行うことができる。したがって、利用可能なマニピュレータが解析解のある構造を持つものに限られるという制約がなくなり、使用目的に適した任意のロボットを設計・製造することができる。また、ロボット動作時に２点間の直線補間、円弧補間、曲線補間の制御速度と制御精度が向上する。ロボットのＰＴＰ（Poit To Poit；任意位置決め動作）制御、ＣＰ（Continuous Path;直線補間動作）制御、擬似ＣＰ制御において、教示点間の補間精度を向上させることもできる。更には、力制御、インピーダンス制御、力とインピーダンスとのハイブリッド制御等を行う場合であっても、制御の精度を向上させることができる。

以下、添付した図面に基づいて、この発明によるマニピュレータの制御方法の実施例を説明する。図３は、この発明によるマニピュレータの制御方法が適用されるマニピュレータの概略斜視図である。

図３には、マニピュレータの関節角度の定義が示されている。図３（ａ）は、標準的なマニピュレータを模式的に示した図であり、図３（ｂ）はより具体的に示した斜視図である。マニピュレータ１は、図３（ａ）に示すように、アームｒ０を固定リンクとして、アームｒ１〜ｒ６を可動リンクとして順次、関節（各関節角θ１〜θ６を持つ）によって繋がって構成されており、先端に各種の作業を行う手先（ハンド）２を有している。アームｒ０の基点から手先２の基端部までのベクトルがＲ_e で示されており、手先２の単位方向ベクトルｒ_e が示されている。ｘｙｚ直交座標系は、図３（ｂ）に示すように右手系を形成するように定められる。図３（ｂ）に示すマニピュレータ１は、手先２の３軸（図示の例では関節角θ４、θ５及びθ６）が一点Ａで交わる構造を有している。アームｒ４は長さゼロであり、自身の回転軸の回りに回転可能であり、その回転角度は関節角θ４で表される。アームｒ５は、アームｒ４に対して、アームｒ４の回転軸と直交交叉する回転軸の回りに回転可能であり、その回転角度は関節角θ５で表される。更に、アームｒ６は、手先２が関節角θ６で示すように、自身の回転軸の回りにのみ回転可能であり、その回転軸はアームｒ４とアームｒ５の回転軸の交点Ａと交叉している。

順キネマティクス問題とは、マニピュレータ１の各関節角度θ１〜θ６を与えたときに、手先２が占める位置や姿勢を計算することである。式（１）は、第（ｉ−１）リンク座標系に対する第ｉリンク座標系の相対的な方向余弦行列Ｃⁱ を表している。ここで、Ｔ₂ ，Ｔ₃ は、それぞれ、２軸又は３軸回りの基本回転変換を表す方向余弦行列であり、式（２）で示される。

したがって、慣性座標系から第ｉリンク座標系までの方向余弦行列Ｄⁱ は、式（３）で表される。

最終関節はロール軸回転（関節角θ６）であるので、手先２の位置は関節角θ１，θ２，…，θ５に依存し、それらの関数として表される。後述する逆キネマティクス問題は、現在のところ、図１（ｂ）に示すような関節角θ４、θ５及びθ６の各回転軸が一点Ａで交わる構造の場合にのみ解析解があるとされている。

慣性座標系（好ましくは、地球の回転までをも考慮した上での慣性座標系）を｛ｉ｝、手先２の方向を示す単位ベクトルｒ_e を式（４）で表わし、原点から手先２まで延びたベクトルをＲ_e とする。

方向ベクトルｒ_e は単位ベクトルであるので、式（５）に示すように各要素の二乗和は１であり、ｒ_e １〜ｒ_e ３のうち独立変数は２つである。

そこで、方向単位ベクトルｒ_e は、式（６）に示すように二つの変数で表される。

原点から手先２まで延びた手先位置ベクトルＲ_e （＝｛ｉ｝^T Ｒ_e ）は、各アームｒ０〜ｒ５のベクトルの和として、式（７）のように表される。なお、ＤＣＭは方向余弦行列（３×３の正規直交行列）である。

以上のことから、統合ベクトルとして、式（８）に示すように、手先方向単位ベクトルの二つの要素と手先位置の三つの要素とから成る５変数のベクトルＸを定めることができる。

また、式（５）と式（７）から、統合ベクトルＸは、式（９）に示すように第１〜５関節角の関数として表される。なお、統合ベクトルは、各要素に総和が１となる正の重み係数α_i ( α_i ＞０，Σα_i ＝１）をそれぞれ乗じて得られる重み係数付き要素から成る修正された統合ベクトルとすることができる。各要素に相対的な重み係数を付けることによって大きい重みを付けるほど、最適解を求める際にその要素の収束値を絞り、制御性を一層高めることができる。

式（９）を時間で微分すると、式（１０）のように表される。ここで、∂／∂ｔ（ｒ_e （２，３））は２行×１列の行列、∂／∂ｔ（Ｒ_e ）は３行×１列の行列である。

手先方向単位ベクトルｒ_e と手先位置ベクトルＲ_e の５要素からなる統合ベクトルＸの各関節角θ１〜θ５の角速度が与えられたときの手先２の感度、即ち手先２の移動速度や姿勢角レート（又は角速度）を与えるヤコビ行列（ヤコビアン）Ｊを定義することができる。ヤコビ行列Ｊは、５行×５列の行列であり（即ち、３軸についての位置と方向から成る６変数から最終回転軸方向は除かれている）、式（１１）で表される。なお、オイラー角によって姿勢を表示する場合、姿勢角レートでヤコビ行列を定義すると、ハンドが真上や真下を向いたときにマニピュレータが特異状態となって制御不可能となることがある（特異点問題。ここでは詳細を省略する）。

手先の位置と方向についての統合ベクトルＸの目標値を与えたとき、手先がそうした目標値を取るための関節角θ_n （非冗長関節の関節角；ｎ＝１，２，…，５）を求める計算が逆キネマティクス計算である。関節角θ_n は、ニュートン・ラプソン法を用いて数値解として求めることができる。即ち、統合ベクトルＸの目標値を目標統合ベクトルをＸ_t とし、初期値θ_n0に対する統合ベクトルをＸ₀ とする。このとき、統合ベクトルについてｎ回目の繰返し計算における計算値Ｘ_n と目標統合ベクトルＸ_t との差ベクトルΔＸ_n は、式（１２）のように表される。

差ベクトルΔＸ_n のノルムが設定値を満足するまで、以下の計算が繰り返される。ｉ番目の関節角θ_i のｊ＋１回目の計算値θ_i,j+1 は、前回の計算値θ_i,j にヤコビ行列Ｊと差ベクトルΔＸ_j とを用いて、次のように表される。即ち、手先２の速度や姿勢角速度（ハンド角速度）が与えられたときに、マニピュレータの各関節における関節角速度を計算するときには、式（１３）に示すように、その速度及び姿勢角速度に式（１１）の逆行列としての逆ヤコビ行列Ｊ ^- ¹を乗じることによって関節角速度を計算することができる。

差ベクトルΔＸ_n が予め定められた一定の値以下になったとき、計算は収束したと判定される。以上に述べた計算手法（式（１）〜式（１３））は、冗長マニピュレータの場合においても、冗長関節をパラメータとして扱うときは、非冗長関節（上記５関節とロール軸関節）に対して適用可能であることは明らかである。

図４は、上記の冗長関節を考慮しない（即ち、関節はすべて非冗長関節とする）ときのマニピュレータの制御方法において、手先２の位置と方向を決定する概略的な手順についてまとめたフローチャートである。
このフローチャートによれば、先ず、エントリー（ステップ０。「Ｓ０」と略す。以下、同様）し、関節角すべての初期値（デフォルト値又はユーザ指定）を読み込んだ後、手先２（図１参照）の姿勢と位置ベクトルの定義が行われる（Ｓ１）。即ち、手先２の方向単位ベクトルｒ_e は、式（６）に示すように二つの変数で定義され、手先２の位置ベクトルＲ_e は、式(7) で定義される。
手先２に関する統合ベクトルＸの５×１行列が生成される（Ｓ２）。Ｓ１における定義により、Ｘは、式（８）に示すように、手先方向単位ベクトルｒ_e と手先位置ベクトルＲ_e の５変数とする関数で表される。
エントリーで読み込まれた関節角に基づく順キネマティクス計算によって、手先２の初期値が計算される（Ｓ３）。
次に、逆キネマティクス計算を適用することによって、関節角収束計算が行われる（Ｓ４）。即ち、通常は、Ｓ３で求められた手先２の初期値は目標統合ベクトルＸ_t と異なっているので、手先２の実際の位置と方向を目標統合ベクトルＸ_t で定まる位置と方向とに一致させるために、各関節角が取るべき値を上記したニュートン・ラプソン法を用いた繰返し計算で求める。繰返し計算で目標統合ベクトルＸ_t との差ベクトルΔＸ_n が十分ゼロに収束したと判断されるときの関節角が、求めるべき各関節角とされる。
次いで、手先２のロール角が計算される（Ｓ５）。手先２の位置を方向を得るための各関節角が求まれば、最後に決定することでよい関節角として、手先２のロール軸回りの関節角が計算される。
手先２のロール軸を含むすべての関節角が定まるのでこのフローチャートを終了し離脱する（Ｓ６）。

図４に示すフローチャートのＳ４における関節角収束計算の具体例が図５に示されている。図５は、逆キネマティクス計算が適用されて関節角収束計算を行うサブルーチンのフローチャート（’ＩＮＶＫＩＮ’）である。
このフローチャートにエントリーする（Ｓ１０）と、ニュートン・ラフソン法（以下、「Ｎ−Ｒ法」という）の初期化（Ｓ１１）が行われる。即ち、繰返し回数ＩＴＲＮ＝１と置いて、イタレーションの初期化を行う。
次に、一般化された順キネマティクスサブルーチン「ＲＢＦＷ６Ｍ」を用いて、関節角の現在値に対する手先・方向ベクトル（統合ベクトルの５成分：ＸＸＴ（１）〜ＸＸＴ（５））の計算を行う（Ｓ１２）。
サブルーチン「ＪＡＣＯＢ５」を用いて、関節角の現在値に対するヤコビアン（５×５行列）を計算する（Ｓ１３）。即ち、任意形状と自由度配置に対するヤコビアンが計算される。
サブルーチン「ＮＲ−ＭＴＤ」を用いて、Ｎ−Ｒ法による近似解を計算（新しい関節角（更新値）を求める）（Ｓ１４）。サブルーチン「ＮＲ−ＭＴＤ」は、式（１３）に基づいて更新値（θ_i+1 ）を求めるルーチンである。
一般化された順キネマティクスサブルーチン「ＲＢＦＷ６Ｍ」を用いて、関節角の更新値に対応する手先位置・方向ベクトル（５成分：ＸＸＴ（１）〜ＸＸＴ（５））の計算を行う（Ｓ１５）。
更新後の手先位置・方向ベクトルと目標となる手先位置・方向ベクトルとの誤差ノルムδ＝｜ＸＸＥ（＊）−ＸＸＴ（＊）｜を計算する（Ｓ１６）。
計算初期値の更新を行う（Ｓ１７）。
次に、ＮーＲ法を続行するか否かを判定する（Ｓ１８）。即ち、
｜δ｜＞εであって、且つ
ＩＴＲＮ＜ＩＴＲＮｍａｘ
であると判定されるときＳ１２に戻る。
Ｓ１８において、判定が否定であるときには、方向余弦行列から手先ロール角を計算する（Ｓ１９）。
サブルーチンを出る（Ｓ２０）。

次に、この発明による、冗長関節を含む冗長マニピュレータの場合において、全関節角を定める手順を、図１に示すフローチャートに基づいて説明する。図１は、冗長関節と非冗長関節とから成る冗長マニピュレータの全関節角を決定するためのメインルーチンを示すフローチャートである。
本メインフローチャートがスタートする（３０）と、先ず、冗長関節・非冗長関節の切り分けを行う（Ｓ３１）。本発明による特徴の一つは、冗長関節の個数が一つに限られておらず幾つあってもよいという一般性を備える点にある。冗長関節が複数存在していても、冗長関節・非冗長関節の切り分けを、その組合せに従って行うことができる。
冗長関節を定めるための条件として、評価関数Ｆ（Ｘ）、等式拘束ｇ（Ｘ）、及び付等式拘束ｈ（Ｘ）の定義を行う（Ｓ３２）。即ち、手先２の位置と方向との統合ベクトルＸについて、等式拘束ｇ（Ｘ）及び付等式拘束ｈ（Ｘ）で表される条件、或いは統合ベクトルＸの取り得る範囲で値の大小を評価することになる評価関数Ｆ（Ｘ）を定義する。本発明による特徴の二つ目は、このステップにおいて、任意の評価関数又は拘束条件を扱うことができ、実用性が向上することである。なお、通常、評価関数と拘束条件の両方を用いることはない。
次に、Ｓ３１で切り分けられているすべての冗長関節（一つ又は複数個）についてその又はそれらの関節角の初期値を設定する（Ｓ３３）。
すべての冗長関節について関節角の初期値が設定されているので、既に切り分けられている非冗長関節について、与えられている手先２の位置と方向を得るためのそれらの関節角を逆運動学問題として解く（Ｓ３４）。逆キネマティクスの解法は、既に説明したように、図５に示すサブルーチン’ＩＮＶＫＩＮ’を適用して、Ｎ−Ｒ法による繰返し計算で収束する数値解として解くことができる。
設定値又は数値解としてすべての関節角について値が求まっているので、評価関数Ｆ（Ｘ）によって最適性の計算を行う（Ｓ３５）。評価関数Ｆは、拘束条件ｇ（Ｘ），ｈ（Ｘ）の満足度を評価する関数である。
Ｓ３５での評価関数Ｆ（Ｘ）の計算結果に基づいて、関節角が最適か否かが判定される（Ｓ３６）。関節角が最適であると判定されればループから離脱するが、最適解ではないとの判定であるときは非冗長関節の逆キネマティクス計算（Ｓ３４）に戻るルートを辿る。この場合、戻ることによる繰返し計算回数ｎが最大値ｎｍａｘを超えるか否かが判定される（Ｓ３７）、繰返し計算回数ｎが最大値ｎｍａｘを超えていなければ、冗長関節の関節角が変更される（Ｓ３８）。最適解を求めるまでは、冗長関節の関節角はパラメータとして扱われ、冗長関節の関節角の値が変更される。変更する値の幅等については、適宜にユーザが定義し、設定する。冗長関節の関節角の値を変更した後、Ｓ３４に戻り、変更された冗長関節の関節角で非冗長関節の関節角を再計算する。なお、点線で囲まれた範囲で行われるＳ３４〜Ｓ３８の手順は、先に記載した特許文献１に開示されている手法による手順と同等である。Ｓ３７において繰返し計算回数ｎが最大値ｎｍａｘ以上になると判定されれば、Ｓ３１に戻って冗長関節・非冗長関節の切り分けをやり直す。
Ｓ３６で関節角の最適解が得られた場合には、その最適解が記録される（Ｓ３９）。
Ｓ３９で最適解として記録された関節角が、冗長マニピュレータ１の手先位置の目標範囲をカバーしているか否か、即ち、冗長マニピュレータ１の手先２が最終目標位置（一定の最終目標範囲内も含む）に到達しているか否かが判定される（Ｓ４０）。Ｓ４０における判定がＮＯである場合には、拘束条件の緩和として手先２の出発点から最終目標位置までの途中に逐次置かれる手先目標位置が変更され（Ｓ４１）、非冗長関節の逆キネマティクス計算（Ｓ３４）に戻る。Ｓ４０における判定がＹＥＳの場合には、このフローチャートを終了して離脱する（Ｓ４２）。

メインルーチンのフローチャートのＳ３５における冗長関節角の最適手法による決定サブルーチンの詳細について、図２を参照して説明する。図２は、図１で示したメインルーチンのＳ３５における評価関数の計算の詳細を示すサブルーチンを示すフローチャートであり、サブルーチンへのエントリー（Ｓ５０）の後、評価関数と制約関数（拘束条件）の計算（’ＦＣＣＡＬ’；Ｓ５１）、差分近似による勾配の計算（’ＤＩＦ’；Ｓ５２）、ペナルティ関数値の計算（’ＰＶＡＬ’；Ｓ５３）、収束計算（’ＣＯＮＴＥＳ’；Ｓ５４）、Ｓ５４の計算が収束したか否かの判定（Ｓ５５）、Ｓ５５の判定がＹＥＳ（収束する）であるときのサブプログラムの終了とメインプログラムへの戻り（Ｓ５７）、及びＳ５５の判定がＮＯ（収束せず）の判定である場合において、Ｓ５１へ戻り再度の計算を繰り返すための、解（冗長関節角）と近似ヘッセ行列を更新（Ｓ４６）から成っている。。なお、ヘッセ行列は、多変数の場合の極値の判定をするときに用いられる行列であり、正定性をチェックすることで多変数関数の極値を知ることができるが、計算が複雑であるために、より簡素化した行列として近似ヘッセ行列が定められる。また、この最適手法による決定サブルーチン自体については、例えば、茨城俊秀、福島雅夫、「Ｆｏｒｔｒａｎ７７最適化プログラミング」岩波書店（１９９１年）に詳しく解説されているので、ここではこれ以上の説明を省略する。

冗長関節角の決定には、最適化法の適用が必要である。即ち、ある評価関数Ｆを最小にする（最適化）ように、ｍ個の冗長関節角φ１，φ２，…，φｍが決定される。

ここで、θ１，θ２，…θ５，θ６は逆キネマティクス問題として決定されるべき非冗長関節の関節角である。また、関数Ｆとしては、つぎのような関数が考えられる。
（１）中立点からの角度のずれの和

（２）前ステップとの差（微小時間Δｔにおける角度変化）の和

（３）ヤコビアン行列式の絶対値の逆数

この場合には、Ｊは拘束条件からはずれる。
（４）関節リミットへの接近度を表す関数（多数あり）
マニピュレータの制御において、どの関節角についても値が小さい方が、各関節を作動させるときに効率的である。そこで、評価関数を上記（１）に示すように冗長関節の関節角の二乗和として定義し、その二乗和が最小（極小）になように評価することが考えられる。また、評価関数を隣り合う二つの関節角の差の二乗和として定義することで、隣り合う二つの関節角が大きく変化する場合を低く評価し、隣り合う二つの関節で関節角の変化量が小さいときを高く評価することも考えられる。更に、各項に総和が１となる重み係数をそれぞれ乗じて得られる重み係数付き二乗和で評価関数を定義することで、関節が占める位置に応じて重みを付けることもできる。更にまた、上記（３）で示すように、評価関数を、各関節角の角速度が与えられたときの手先の感度を与えるヤコビアン行列式の絶対値の逆数で定めることもできる。

この発明は、マニピュレータの制御、即ち、各関節角度の制御のみならず、複数の関節とアームとから成る生産ロボット、介護ロボット、愛玩ロボット等のあらゆるロボットの制御に適用可能であることは明らかである。

この発明による冗長マニピュレータの制御方法において、メインプログラムのフローチャートである。図１で示したメインルーチンにおける評価関数の計算の詳細を示すサブルーチンを示すフローチャートである。従来のマニピュレータの概略斜視図である。マニピュレータの制御方法において、一般的なアルゴリズムを示すフローチャートの一例である。マニピュレータの制御方法において実行される、逆キネマティクス計算サブルーチンのフローチャートである。

符号の説明

１マニピュレータ２手先
θ１〜θ６関節角（非冗長関節角） φ１〜φｍ関節角（冗長関節角）
ｒ０アーム（固定リンク）ｒ１〜ｒ６アーム（可動リンク）
Ｒ_e アームｒ０の基点から手先２の基端部までのベクトル
ｒ_e 手先２の単位方向ベクトルＡ手先２の３軸の交点
Ｃⁱ 第ｉリンク座標系の相対的な方向余弦行列
Ｄⁱ 慣性座標系から第ｉリンク座標系までの方向余弦行列
Ｔ₂ ，Ｔ₃ ２軸又は３軸回りの基本回転変換を表す方向余弦行列
Ｘ統合ベクトルＸ_t 目標統合ベクトル ΔＸ_n 差ベクトル
Ｊヤコビ行列（ヤコビアン）

Claims

冗長関節と非冗長関節とから成る関節を介して複数のアームを手先まで順次繋いで成る冗長マニピュレータの前記手先についてその位置と方向を指定したときに前記各関節が呈すべき関節角を決定して前記手先の作動を制御する冗長マニピュレータの制御方法において、前記関節を前記冗長関節と前記非冗長関節とに切り分けし、切り分けされた前記冗長関節の関節角をパラメータとして変化させたとき、前記パラメータとして与えられる前記冗長関節の前記関節角と、逆キネマティクス計算で解として得られた前記非冗長関節の関節角とを用いた評価関数及び拘束条件に基づいて前記関節角の最適解を求め、前記最適解が前記手先位置の目標範囲をカバーするか否かを判定し、否の判定であるときには前記評価関数の変更又は前記拘束条件の緩和を行って前記最適解を求める手順を繰り返すことから成る冗長マニピュレータの制御方法。
前記評価関数は、前記冗長関節の前記関節角の二乗和であることから成る請求項１に記載の冗長マニピュレータの制御方法。
前記評価関数は、隣り合う二つの前記関節角の差の二乗和であることから成る請求項１に記載の冗長マニピュレータの制御方法。
前記評価関数は、前記各関節角の角速度が与えられたときの手先の感度を与えるヤコビアン行列式の絶対値の逆数であることから成る請求項１に記載の冗長マニピュレータの制御方法。
前記逆キネマティクス計算と前記関節角の最適解を求める計算において、前記手先の位置と姿勢ベクトルとから成る統合ベクトルを用いることから成る請求項１に記載の冗長マニピュレータの制御方法。
前記統合ベクトルは、各要素に総和が１となる正の重み係数をそれぞれ乗じて得られる重み係数付き要素から成る修正された統合ベクトルであることから成る請求項５に記載の冗長マニピュレータの制御方法。
前記逆キネマティクス計算は、前記関節角に初期値を与えた後、ヤコビアンの逆行列に補正前の前記統合ベクトルと目標統合ベクトルとの差ベクトルを乗じた積を補正前の前記関節角に対して差し引いて補正後の前記関節角とし、前記補正後の前記関節角に基づく順キネマティクス計算によって補正後の前記統合ベクトルを得る繰返し計算を行い、前記差ベクトルのノルムが所定の値以下に収束したときの補正後の前記関節角を近似解とするニュートン・ラプソン法によって行われることから成る請求項５又は６に記載の冗長マニピュレータの制御方法。
前記手先のロール角は、他の前記関節角とは独立して決定されることから成る請求項１に記載の冗長マニピュレータの制御方法。