(57)【特許請求の範囲】
【請求項１】
複数のリンクが夫々関節で連結されて、必要とされる自由度よりも高い自由度を有する冗長なマニピュレータの制御方法において、
マニピュレータの先端を目標位置へ移動させる際の基端から先端までの形状を決めるに際して、移動前のマニピュレータの全体形状を基準としてマニピュレータを構成する各リンクを該リンクの延在方向のみに伸縮可能な弾性体（リンクの長さを自然長とし、適宜な弾性係数を有する）と見なし、リンクを接続する全ての関節の関節角および基端との連結部を固定した状態でマニピュレータの先端を目標位置へ移動させ、全ての関節および基端との連結部の固定を解くことで伸張もしくは収縮した各リンクが自然長に復帰した場合の全体形状をシミュレートし、マニピュレータの先端を目標位置へ移動させたシミュレート結果の全体形状と一致するように各リンクの関節角および基端との連結部の角度を制御することで、マニピュレータの先端を目標位置へ移動させるようにしたことを特徴とするマニピュレータの制御方法。
【請求項２】
上記リンクに適用する弾性体の弾性係数は、非線形の特性を有するものと設定してシミュレートするようにしたことを特徴とする請求項１に記載のマニピュレータの制御方法。
【請求項３】
マニピュレータの先端を目標位置へ移動させるに際して障害物がある場合、この障害物を各弾性体に対して距離に反比例する外力として導入することにより、マニピュレータの先端を目標位置へ移動させた結果をシミュレートし、移動後のマニピュレータの全体形状を決定するようにしたことを特徴とする請求項１又は請求項２に記載のマニピュレータの制御方法。
【請求項４】
マニピュレータの関節が動作不能となった場合、故障した関節の両側にある一組のリンクを合わせて仮想リンクとし、関節が動かなくなった状態での仮想リンクの両端を自然長とすると共に仮想リンクの両端を結ぶ直線状で伸縮可能な弾性体を、仮想リンクの見なし弾性体として導入することにより、移動前のマニピュレータの全体形状を決定するようにしたことを特徴とする請求項１〜請求項３の何れか１項に記載のマニピュレータの制御方法。
【請求項５】
マニピュレータの先端を目標位置へ移動させた場合における全体形状のシミュレートは、複数の演算処理手段により分散的に行うようにしたことを特徴とする請求項１〜請求項４の何れか１項に記載のマニピュレータの制御方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、工場での組立・塗装等に供される産業用マニピュレータ、宇宙空間において検査、組立作業等を行う宇宙用マニピュレータの制御方法に関するものである。特に、必要とされる自由度を超える自由度を有する、いわゆる冗長マニピュレータの制御に対して有効である。
【０００２】
【従来の技術】
従来は、複数のリンクを関節で連結したマニピュレータの手先（先端）を目標とする位置へ移動するために必要な各関節角を計算する場合、マニピュレータの関節角の変化量からマニピュレータ手先の位置の変化量を算出する、いわゆるヤコビ行列を作成し、その逆変換に相当する逆ヤコビ行列を計算することにより、関節角を求めるのが一般的であった。
【０００３】
この逆ヤコビ行列は、マニピュレータの有する自由度と必要とする自由度が同じマニピュレータについては一意に求めることができるが、必要とされる自由度を超える自由度を有する、いわゆる冗長マニピュレータでは、ヤコビ行列が長方行列となるため一意に求めることができない。そこで、これまでは設計者がいくつかの拘束条件を設定し、特異値分解やラグランジュの未定乗数法などを用いて、疑似逆行列を算出していた。
【０００４】
【発明が解決しようとする課題】
しかしながら、冗長マニピュレータの制御方法として、上述したような制御方法をとる場合、必要とされる自由度とマニピュレータの有する自由度の差、いわゆる冗長度が大きくなるに従い、算出が煩雑になる問題があった。また、マニピュレータの部分的な故障などに適応するためには、特殊な計算を用いる必要があった。更に、これらの計算は集中して行う必要があり、近年一般化しつつある分散的制御構造になじまなかった。
【０００５】
柔軟な運動や障害物の自由な回避、部分的故障への対応などを目的として、冗長マニピュレータの開発が盛んになってきているのが近来の傾向で、特に宇宙空間での衛星の検査などにおいては、対象物を巧みに回り込むことが可能で、部分的な故障が発生した後も作業を続行できる、自由度の非常に大きなマニピュレータが開発されつつある。このような宇宙空間での利用を考えた場合、マニピュレータの制御に供される計算機の能力は、放射線やごく低温など劣悪な環境条件により、非常に限られている。このため、非常に大きな自由度を有するマニピュレータの関節角算出を集中的に行うには限界がある。このため、現在計算機を関節とともにモジュール構成にすることで、故障に対してロバストにし、なおかつ分散的に制御を行うことで、個々の計算能力の制約を回避する方向で開発が進められている。このことからも、マニピュレータの関節角を決定する手法としても、こうした分散制御構造を容易に適用できることが望ましい。
【０００６】
そこで、本発明は、冗長度の大きなマニピュレータや故障したマニピュレータにも容易に適用可能で、近年発達しつつある分散制御構造によるマニピュレータ制御を実現することのできる、マニピュレータの関節角決定手法を採用したマニピュレータの制御方法を提案する。
【０００７】
【課題を解決するための手段】
上記課題を解決するために、請求項１に係る発明は、複数のリンクが夫々関節で連結されて、必要とされる自由度よりも高い自由度を有する冗長なマニピュレータの制御方法において、マニピュレータの先端を目標位置へ移動させる際の基端から先端までの形状を決めるに際して、移動前のマニピュレータの全体形状を基準としてマニピュレータを構成する各リンクを該リンクの延在方向のみに伸縮可能な弾性体（リンクの長さを自然長とし、適宜な弾性係数を有する）と見なし、リンクを接続する全ての関節の関節角および基端との連結部を固定した状態でマニピュレータの先端を目標位置へ移動させ、全ての関節および基端との連結部の固定を解くことで伸張もしくは収縮した各リンクが自然長に復帰した場合の全体形状をシミュレートし、マニピュレータの先端を目標位置へ移動させたシミュレート結果の全体形状と一致するように各リンクの関節角および基端との連結部の角度を制御することで、マニピュレータの先端を目標位置へ移動させるようにしたことを特徴とする。
【０００８】
また、請求項２に係る発明は、上記請求項１に記載のマニピュレータの制御方法において、リンクに適用する弾性体の弾性係数は、非線形の特性を有するものと設定してシミュレートするようにしたことを特徴とする。
【０００９】
また、請求項３に係る発明は、上記請求項１又は請求項２に記載のマニピュレータの制御方法において、マニピュレータの先端を目標位置へ移動させるに際して障害物がある場合、この障害物を各弾性体に対して距離に反比例する外力として導入することにより、マニピュレータの先端を目標位置へ移動させた結果をシミュレートし、移動後のマニピュレータの全体形状を決定するようにしたことを特徴とする。
【００１０】
また、請求項４に係る発明は、上記請求項１〜請求項３の何れか１項に記載のマニピュレータの制御方法において、マニピュレータの関節が動作不能となった場合、故障した関節の両側にある一組のリンクを合わせて仮想リンクとし、関節が動かなくなった状態での仮想リンクの両端を自然長とすると共に仮想リンクの両端を結ぶ直線状で伸縮可能な弾性体を、仮想リンクの見なし弾性体として導入することにより、移動前のマニピュレータの全体形状を決定するようにしたことを特徴とする。
【００１１】
また、請求項５に係る発明は、上記請求項１〜請求項４の何れか１項に記載のマニピュレータの制御方法において、マニピュレータの先端を目標位置へ移動させた場合における全体形状のシミュレートは、複数の演算処理手段により分散的に行うようにしたことを特徴とする。
【００１２】
【発明の実施の形態】
次に、添付図面に基づいて、本発明に係るマニピュレータの制御方法の実施形態を説明する。これに先立ち、まず、本発明に係るマニピュレータの制御方法で用いる関節角の決定手法の概要を説明する。
【００１３】
自然長が夫々Ｌ１からＬＮのバネＮ本をつなげたシステム（以下連結バネと呼ぶ）は、外界から継続的な力が加わらないかぎり、それぞれ長さＬ１からＬＮを満たす形状に収束する。但し、全てのバネは、一次元でのみ伸縮するものとし、撓んだり歪んだりすることのない特性を有するものである。このバネが非線形な特性を持っている場合、この収束は線形な特性をもつバネに比べて速やかに収束する。
【００１４】
ここで、マニピュレータを構成する各リンクの距離がＬ１からＬＮであるＮリンクマニピュレータを考える。Ｎ本のバネの連結部（関節）が角度を変えることが出来ない状態として、連結バネの一方をマニピュレータの付け根の位置に、もう一方をマニピュレータの手先の目標位置に各々固定した後、関節が自由に曲がるように開放すると、連結バネの根元から先端までが全バネの総延長（Ｌ１＋Ｌ２＋…ＬＮ）内にあれば、各バネの伸びや収縮によって各バネが長さＬ１からＬＮを満たす形状に収束する。このときの形状（各関節の関節角）は目標位置を実現するマニピュレータの形状にほかならない。
【００１５】
いわば、本発明に係るマニピュレータの制御方法における関節角の決定手法は、従来の如く制御すべき関節角を直接的に求めるのではなく、弾性体に見立てたリンクの長さを変形させて先端を目標位置に移動させたときの変形状態から各リンクが自然長に復したときの形状をシミュレートし、形状変化に伴う関節角の変化量を以てマニピュレータの制御を行うのであるから、先端部の移動前におけるマニピュレータの全体形状が既知であれば、先端部の移動後におけるマニピュレータの全体形状も一意に決めることができる。
【００１６】
したがって、本発明で採用したマニピュレータの関節角決定手法を用いれば、従来の方法で必要であったマニピュレータの形状や関節の故障、障害物などを逆ヤコビ行列に導入する必要がなく、制御系は単に変位が与えられたバネの時間発展をシミュレーションするだけで形状を決定することができるので、計算を容易にすることができ、非常に大きな自由度差を持つマニピュレータへの適用が可能になる。しかも、変位を与えたバネの時間発展は、バネに発生する変位と両端のバネからの外力のみを計算することで解くことができるので、形状の決定に要する計算を複数の計算機により分散的に行うことができ、分散的制御構造による実現が非常に容易であり、計算能力の限られた宇宙用の計算機でも非常に多くの自由度を有するマニピュレータの形状を計算することができる。
【００１７】
次に、図１に基づいて、上述したようにマニピュレータを連結バネに見立てたシミュレーションを行うための実施形態を説明する。ここでは、説明を簡単にするために２次元のマニピュレータシステムに本発明を適用して説明する。なお、このアルゴリズム自体は非常に単純なので３次元への拡張も容易である。また、ここでは軌道上で対象衛星を検査することを想定しているので、任意の初期状態にあるマニピュレータのエンドエフェクターを現在位置から相対移動するタスクを考える。
【００１８】
まず、図１（ａ）に示すようなマニピュレータ１は、第１リンクＬ１と第２リンクＬ２を関節Ｊ１により連結し、第２リンクＬ２と第３リンクＬ３を第２関節Ｊ２で連結し、第３リンクＬ３と第４リンクＬ４を第３関節Ｊ３で連結し、第４リンクＬ４と第５リンクＬ５とを第４関節Ｊ４で連結し、第５リンクＬ５と第６リンクＬ６とを第５関節Ｊ５で連結し、第６リンクと第７リンクとを第６関節Ｊ６で連結して可動腕部を構成し、該可動腕部の基端（第１リンクＬ１の非関節側）をベース２に、可動腕部の先端（第７リンクＬ７の非関節側）にエンドエフェクター３を設けたものである。
【００１９】
これを連結バネに見立てる場合、図１（ｂ）のように、第１リンクＬ１を第１弾性体４１に、第２リンクＬ２を第２弾性体４２に、第３リンクＬ３を第３弾性体４３に、第４リンクＬ４を第４弾性体４４に、第５リンクＬ５を第５弾性体４５に、第６リンクＬ６を第６弾性体４６に、第７リンクＬ７を第７弾性体４７に、各々置き換え、第１〜第９関節Ｊ１〜Ｊ６は固定状態とする。なお、第１〜第７弾性体４１〜４７は、見立てた各リンクの両端をつなぐ直線状（１次元）でのみ伸縮可能な伸縮方向規制手段と適宜な弾性係数を有するコイルスプリングとで等価機能を表す。また、第１〜第７弾性体４１〜４７の自然長は、それぞれ第１〜第７リンクＬ１〜Ｌ７の長さと同じである。
【００２０】
なお、第１〜第７弾性体４１〜４７の有弾性機能は、コイルスプリングに限定されるものではなく、シミュレート可能な弾性素材であれば、どのようなものを適用しても構わないが、本実施形態においては、自然長がＬ0 で、ある時点でのバネ長がＬである非線形な粘弾性をもつバネを用いるものとした。このバネのダイナミクスは以下の通りである。下式において、Ｋ1 は非線形成分の弾性係数、Ｋ2 は線形成分の弾性係数、Ｄはバネの運動に伴う摩擦係数であり、これらのパラメータを適宜に設定することで、所望の弾性体の物理特性を再現できる。なお、弾性バネは変位により与えられたエネルギーを摩擦の効果によって徐々に放出し、自然長に収束するものであるから、摩擦係数Ｄをパラメータの一つとして導入した。
【００２１】
【数１】
0003643867.tif 000002
【００２２】
マニピュレータ１のエンドエフェクター３を相対移動させる指令を送る場合、連結バネ系の他の部分（第１〜第６関節Ｊ１〜Ｊ６およびベース２と第１リンクＬ１との連結部）を固定した状態で、先端に対して指定方向の変位を与え、その状態でバネ全体を開放し、連結バネ全体が定常状態に落ち着くまでをシミュレートし、変位が一定値以下（非線形バネでは揺り戻しによるブレが顕著となって、完全停止までシミュレートするのは無駄であるため）になった状態におけるバネの屈曲角を関節角として指示する。かくして、マニピュレータ１に対する適正な制御を、処理負荷の少ない演算によって速やかに実現できる。
【００２３】
なお、バネが自然長に収束までに要する演算サイクルは、変位の大きさに依存するが、例えば１０mm程度の変位であれば、１００サイクルで十分収束状態に到達できる。この程度の演算処理能力があれば、関節角を決定するためのシミュレートに支障はないので、各リンク毎に演算処理手段を設けておき、これらで分散的に演算処理を行うようにシステムを組み上げることも容易である。
【００２４】
また、故障した関節（固定）を含むマニピュレータの場合は、故障関節の前後に現在の距離を自然長とする強力な非線形バネを導入することで表現する。具体的には、図２（ａ）に示すように、第４関節Ｊ４が故障して現在の関節角に固定されて制御不能となっている場合、故障した第４関節Ｊ４の両側にある一組のリンク（第４リンクＬ４と第５リンクＬ５）を合わせて仮想リンクＬ４−５（第３関節Ｊ３と第５関節Ｊ５とに接続される仮想的なリンク）とする。
【００２５】
この仮想リンクＬ４−５を含むマニピュレータを連結バネに見立てると、図２（ｂ）のようになり、上述した図１（ｂ）との相違は、第４弾性体４４，第４関節Ｊ４，第５弾性体４５を仮想弾性体４Ｖと見なす点である。すなわち、仮想リンクＬ４−５に対応する仮想弾性体４Ｖは、仮想リンクＬ４−５の両端（関節Ｊ３への接続端から関節Ｊ５の接続端までの直線距離）を自然長とすると共に仮想リンクＬ４−５の両端を結ぶ直線状で伸縮可能な弾性体である。このように捉えれば、上述したと同様の方法により、エンドエフェクター３の移動位置に応じた第１〜第３関節Ｊ１〜Ｊ４および第５，第６関節Ｊ５，Ｊ６の関節角を決定できる。尚、故障した関節が複数存在する場合は、夫々について仮想弾性体を見立ててゆけば良いし、故障した関節が連続する場合は、故障関節に隣接する駆動可能な関節を両端とする仮想弾性体を導入すれば良い。
【００２６】
また、関節部の物理的構造などから関節角限界がある場合のマニピュレータ制御に際しては、故障関節同様に関節前後に非線形バネの結合を導入することで等価機能を表現できる。このとき、関節角限界を表現するための弾性体の特性としては、一定長さ以上では全く力を発生せず、ある距離以上縮まると急峻に反発力が発生する特性（関節が関節角限界に近づくと、それ以上の屈曲を妨げるように働く力）を持つように設定する。
【００２７】
また、障害物を回避しつつマニピュレータを動作させる関節角を決定するときには、障害物を、弾性体に対して作用する外力（障害物から各弾性体までの距離に反比例する外力）として導入することにより、変形後のマニピュレータ形状のシミュレーションに適用できる。具体的には、図３（ａ）に示すように、マニピュレータ１の近傍に障害物５が存在する場合、図３（ｂ）のように障害物５の位置と各関節（例えば、障害物５に近い第３関節Ｊ３〜第５関節Ｊ５）を結ぶ外力５Ｖ１〜５Ｖ３を仮想することで、障害物５との距離を一定以上に保つ形状を計算することができる。なお、障害物５からの外力を全ての関節に作用させるものとしても良いが、距離に反比例する外力としては実質的に影響が無いと見なせる関節にまで外力を含めた演算を行うことは意味がないので、作用する外力に或る程度のしきい値を設定し、そのしきい値以上の外力が作用する関節にのみ外力を作用させて演算を行えば十分である。
【００２８】
図４は、本アルゴリズムを全長５００mm、５自由度の２次元マニピュレータに適用し、全関節３０度の初期条件から上下左右にそれぞれ２０mm相対移動させたときの形状と各バネの伸びの時間を表している。このシミュレーションでは、非線形バネの自然長Ｌ0 ＝１０、各パラメータを、Ｋ1 ＝１０００，Ｋ2 ＝０，Ｄ＝１０に設定した。図４（ａ）はマニピュレータの手先を前後左右４ヶ所に移動した際、連結バネが収束した形状（４種類）をそれぞれ線画により示している。これらが本制御方法で計算された結果である。また、図４（ｂ）はこの過程での各バネの伸びの時間変化を示している。手先が移動しバネに変位が与えられたあと、速やかに減少し、バネの伸びが０すなわち自然長に収束していることがわかる。このことは、本制御方法で非常に速やかに結果の形状が導き出されたことを示している。
【００２９】
図４（ｂ）では、まず演算サイクル１０１の時点でマニピュレータの手先を前方向に２０mm移動させている。このときに各バネに発生した変位は、演算サイクルにして６０以内でほぼ完全に収束している。同様の操作を演算サイクル２０１ではマニピュレータの手先を後方向に２０mm、演算サイクル３０１ではマニピュレータの手先を上方向に２０mm、演算サイクル２０１ではマニピュレータの手先を下方向に２０mm、それぞれ移動させているが、いずれの場合も６０サイクル以内におおむね収束している。
【００３０】
図５は、モジュール数を増やし、同じアルゴリズムを１０自由度の２次元マニピュレータに適用した例である。自由度数の増加によらず、全く同じアルゴリズムが安定に動作していることがわかる。
【００３１】
図６と図７は１０自由度の２次元マニピュレータについて、非線形バネのバネ定数であるＫ1 とＫ2 を変えて、バネの非線形性が結果にどう影響するかを示したものである。両図を比較すれば分かる通り、非線形バネを用いた場合は線形バネを用いた場合に比して定常状態に収束する速度が速い。その反面、非線形性を高くし過ぎると変位によって生じる振動が大きくなり、屈曲が局在した形状に陥りやすいことが確認された。なお、見なし弾性体の特性を如何に選定するか（弾性係数はもとより、線形性・非線形性の何れを選定するか）は、いわば設計上のノウハウに属するものであり、特に限定されるものではない。
【００３２】
【発明の効果】
以上説明したように、本願請求項１に係るマニピュレータの制御方法によれば、従来の方法で必要であったマニピュレータの形状を逆ヤコビ行列に導入する必要がなく、制御系は単に変位が与えられたバネの時間発展をシミュレーションするだけで移動後のマニピュレータの全体形状を決定することができるので、演算を容易にすることができ、非常に大きな自由度差を持つマニピュレータを用いる場合に、特に特に有効である。
【００３３】
しかも、変位を与えたバネの時間発展は、バネに発生する変位を計算することで解くことができるので、形状の決定に要する計算を複数の計算機により分散的に行うことができ、分散的制御構造による実現が非常に容易であり、計算能力の限られた宇宙用の計算機でも、非常に多くの自由度を有するマニピュレータの形状を計算することができる。
【００３４】
また、請求項２に係るマニピュレータの制御方法によれば、弾性体が非線形の特性を有するものとすることで、弾性体の自然長への復帰が速やかとなり、シミュレーション結果を得るまでの演算サイクルを短縮化できるという利点がある。
【００３５】
また、請求項３に係るマニピュレータの制御方法によれば、弾性体に作用する外力として障害物を導入することにより、目標点への先端移動に伴う関節角の変化量を得るための形状特定シミュレーションをそのまま適用できる。
【００３６】
また、請求項４に係るマニピュレータの制御方法によれば、関節の故障に際して、故障した関節を含む仮想リンクを導入することにより、目標点への先端移動に伴う関節角の変化量を得るための形状特定シミュレーションをそのまま適用でき、従来必要であった特殊な計算を用いる必要がなく、これらの計算は集中して行う必要もない。
【００３７】
また、請求項５に係るマニピュレータの制御方法によれば、目標点への先端移動に伴う関節角の変化量を得るための形状特定シミュレーションを複数の演算処理手段により分散的に行うものとしたので、演算能力が比較的低いＣＰＵを各リンクに分散配置してマニピュレータの制御を行うようなシステムとすることができる。
【図面の簡単な説明】
【図１】（ａ）は、２次元マニピュレータの概略図である。（ｂ）は、（ａ）のマニピュレータを連結バネに置き換えた概念図である。
【図２】（ａ）は、一部の関節が故障した２次元マニピュレータの概略図である。（ｂ）は、（ａ）のマニピュレータを連結バネに置き換えた概念図である。
【図３】（ａ）は、障害物が近傍に存在する２次元マニピュレータの概略図である。（ｂ）は、（ａ）のマニピュレータを連結バネに置き換えた概念図である。
【図４】（ａ）は、５自由度の平面マニピュレータを連結バネに置き換えてシミュレートしたときに、４点の先端位置の移動に対して、それぞれ得られたマニピュレータの全体形状を線画により示した図である。（ｂ）は、（ａ）における先端位置の移動毎にバネが自然長へ収束する過程での各バネの伸びの変化を示した図である。
【図５】（ａ）は、１０自由度の平面マニピュレータを連結バネに置き換えてシミュレートしたときに、４点の先端位置の移動に対して、それぞれ得られたマニピュレータの全体形状を線画により示した図である。（ｂ）は、（ａ）における先端位置の移動毎にバネが自然長へ収束する過程での各バネの伸びの変化を示した図である。
【図６】（ａ）は、１０自由度の平面マニピュレータを、線形な特性を有する連結バネに置き換えてシミュレートしたときに、４点の先端位置の移動に対して、それぞれ得られたマニピュレータの全体形状を線画により示した図である。（ｂ）は、（ａ）における先端位置の移動毎にバネが自然長へ収束する過程での各バネの伸びの変化を示した図である。
【図７】（ａ）は、１０自由度の平面マニピュレータを、非線形度を過度に強めた特性を有する連結バネに置き換えてシミュレートしたときに、４点の先端位置の移動に対して、それぞれ得られたマニピュレータの全体形状を線画により示した図である。（ｂ）は、（ａ）における先端位置の移動毎にバネが自然長へ収束する過程での各バネの伸びの変化をを示した図である。
【符号の説明】
１      マニピュレータ
２      ベース
３      エンドエフェクター
Ｌ１〜Ｌ７    第１〜第７リンク
Ｌ４−５      仮想リンク
Ｊ１〜Ｊ６    第１〜第６関節
４１〜４７    第１〜第７弾性体
４Ｖ          仮想弾性体
５      障害物
５Ｖ１〜５Ｖ３    外力
【図１】
0003643867.tif 000003
【図２】
0003643867.tif 000004
【図３】
0003643867.tif 000005
【図４】
0003643867.tif 000006
【図５】
0003643867.tif 000007
【図６】
0003643867.tif 000008
【図７】
0003643867.tif 000009