JPH06143172A - 冗長マニピュレータの制御方式 - Google Patents

Abstract

(57)【要約】 【目的】 ７自由度以上の冗長マニピュレータの制御方
式に関し、サンプリング周期を上げて実時間で動作させ
ることができるようにする。 【構成】 ７自由度のマニピュレータ１０の各関節のう
ち、１個の関節、例えば第３関節を冗長自由度制御用関
節として割り当て、それ以外の６個の関節を手先運動制
御用関節として割り当てる。この６個の手先運動制御用
関節によって、６自由度を持つ手先の運動が決まる。冗
長自由度制御手段３は、手先の位置及び姿勢の目標値ｙ
_d と、パラメータ化した第３関節の関節角θ３とを用い
て逆運動学方程式を解き、その計算で得られた各関節角
度等を用いて評価関数のポテンシャル値を求め評価す
る。その評価を満足したとき、パラメータである第３関
節の関節角θ３を決定し、速度指令値θ３^* を出力す
る。一方、手先運動制御手段５は、現在の関節角度θ₁
〜θ₇ と、手先の運動目標値ｙ_d ^* とを用いて逆運動学
方程式を解き、手先運動制御用関節の速度指令値を求め
る。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は冗長マニピュレータの制
御方式に関し、特に７自由度以上の冗長マニピュレータ
の制御方式に関する。

【０００２】産業用に用いられているロボットの多く
は、最大でも６自由度のマニピュレータが主である。こ
れは、手先によって対象物の位置決めをするに際して、
６自由度あれば十分であるからである。一方、人間の腕
は７自由度を有する。人間はこれによって、例えば手先
で対象物を位置決めしながら、同時に肘の位置を制御し
て障害物をかわしたりすることができる。最近では、人
間の腕のように冗長な自由度を有するロボットアームも
産業界に登場しつつある。

【０００３】

【従来の技術】従来、冗長でないアームの、例えば手先
速度制御を行おうとした場合、手先の目標速度軌道をｙ
_d ^* （＝∂ｙ_d ／∂ｔ、以下微分記号として「＊」を用
いる。また、「ｄ」は目標値を示す。）とすると、一般
に、

【０００４】

【数２】ｙ^* ＝Ｊ・θ^* ・・・・・（２） ここで、ｙ^* ：手先速度 θ^* ：各関節角速度 Ｊ ：ヤコビアン が成り立つから、

【０００５】

【数３】θ_d ^* ＝Ｊ^-1・ｙ_d ^* ・・・・・（３） によって各関節の速度軌道θ_d ^* （＝∂θ_d ／∂ｔ）を
求め、アームの関節角速度制御系に逐次指令を与えるこ
とで実現されてきた。

【０００６】冗長マニピュレータの場合、（２）式は成
り立つが、Ｊが正方行列でないため、（３）式は成り立
たず、

【０００７】

【数４】 θ_d ^* ＝Ｊ⁺ ・ｙ_d ^* ＋（Ｉ−Ｊ⁺ ・Ｊ）・ｋ・・・・・（４） ここで、ｋ ：ｎ次の任意ベクトル Ｊ⁺ ：Ｊの疑似逆行列 ｎ ：関節数 Ｉ ：ｎ×ｎの単位行列 を解く必要がある。冗長自由度を持つため、（３）式の
ようにｙ_d ^* からθ_d ^*は一意には決まらず、ｋを適当
に選ぶことによって、θ_d が決定される。

【０００８】ここで、評価関数を、

【０００９】

【数５】ｐ＝Ｖ（θ）・・・・・（５） とし、

【００１０】

【数６】 ｋ＝ξｋｐ ・・・・・（６ａ） ξ＝［ξ₁ ，ξ₂ ，ξ₃ ，・・，ξ_n ］・・・・（６ｂ） ξ１＝Ｖ（θ）／θ１ ・・・・（６ｃ） （６ａ）式より、ｋを決定すると、（５）式の評価関数
ｐを考慮しながら、手先の運動（目標速度軌道）ｙ_d ^*
を実現する各関節の速度軌道θ_d ^* を（４）式により決
定することができる。

【００１１】なお、その場合、評価関数ｐは、冗長自由
度を利用することにより、特異点を回避できるようにし
たり、関節のリミット範囲を回避できるように設定す
る。整理すると、（４）式は下記の二つの要求（ａ），
（ｂ）から一つの解（速度軌道θ_d ^* ）を求める式にな
っている。

【００１２】（ａ）手先の運動に関する要求（ｙ_d ^* ） （ｂ）冗長自由度を利用した特異点回避等の要求（ｋ）

【００１３】

【発明が解決しようとする課題】ところで、（４）式を
計算するためには、疑似逆行列Ｊ⁺ を逐次求めなければ
ならないが、これにはＪの特異値分解を求める等計算量
が多く、また、特異点回避のための複雑な計算を行う必
要等もあり、実用的にはサンプリング周期をあまり高く
とれないという問題点があった。

【００１４】また、手先の運動制御を、特にセンサ値の
フィードバックによって行う場合、その制御系のサンプ
リング周期を上げることが、運動制御性能を向上させる
一つのキーポイントとなるが、冗長マニピュレータの場
合は、上述した理由からサンプリング周期を上げること
ができず、実時間で冗長マニピュレータを動作させるこ
とが困難であった。

【００１５】本発明はこのような点に鑑みてなされたも
のであり、サンプリング周期を上げて実時間で冗長マニ
ピュレータを動作させることができる冗長マニピュレー
タの制御方式を提供することを目的とする。

【００１６】

【課題を解決するための手段】図１は本発明の原理ブロ
ック図である。本発明の冗長マニピュレータの制御方式
は、図に示すように、冗長マニピュレータの各関節の中
から割り当てた冗長自由度制御用関節の運動指令値を求
める冗長自由度制御手段３と、その冗長自由度制御用関
節以外の冗長マニピュレータの各関節から成る手先運動
制御用関節の運動指令値を求める手先運動制御手段５か
ら構成される。

【００１７】

【作用】冗長マニピュレータを制御する際のアルゴリズ
ムを、その手先の運動を制御するアルゴリズムと、冗長
自由度を利用する戦略を制御するアルゴリズムとに分離
し、独立させる。この制御方式を実現するために、冗長
自由度制御手段３と、手先運動制御手段５とを設ける。
また、冗長マニピュレータの各関節を、冗長自由度制御
用と、それ以外の関節から成る手先運動制御用関節とに
割り当てる。例えば、ｎ（ｎ＞６）個の関節を持つマニ
ピュレータの場合、（ｎ−６）個の関節を冗長自由度制
御用として割り当て、残りの６個の関節を手先運動制御
用として割り当てる。手先の運動は、６自由度で決定す
ることができるため、６個の関節で制御することができ
る。

【００１８】上記の冗長自由度制御手段３は、冗長自由
度制御用関節の運動指令値を求め、手先運動制御手段５
は、手先運動制御用関節の運動指令値を求める。これら
の運動指令値を受けて、冗長マニピュレータは所定の動
作を行う。

【００１９】このように、冗長マニピュレータ制御のた
めの計算を冗長自由度制御手段３と、手先運動制御手段
５とに分けて行うため、制御周期を別々に設定すること
ができる。したがって、サンプリング周期を上げて実時
間で冗長マニピュレータを動作させることができる。

【００２０】

【実施例】以下、本発明の一実施例を図面に基づいて説
明する。図２にマニピュレータの制御系のハードウェア
のブロック図を示す。制御系はワークステーション２０
０、制御装置２０及び制御されるマニピュレータ１０か
ら構成されている。なお、マニピュレータ１０は７自由
度であり、７個の関節を有するが、図２では説明を簡単
にするために、１個の関節を駆動するモータのみを示
し、制御系も１個の関節を制御する要素のみを図示して
いる。

【００２１】制御装置２０はボード計算機２１，２２を
中心に構成されている。ボード計算機２１，２２は３２
ビットのＲＩＳＣを使用している。ボード計算機２１，
２２には、バス２１ａを介して、ＡＤ変換器２３、ＤＩ
Ｏ（入出力インターフェース）２４、ＤＡ変換器２５、
カウンタ２６、ＡＤ変換器２７が接続されている。

【００２２】ワークステーション２００とボード計算機
２１，２２は通信回線２０１によって結合されており、
ワークステーション２００からはボード計算機２１，２
２にマニピュレータ１０の動作プログラム及び必要なデ
ータが送られる。ボード計算機２１，２２ではこの動作
プログラムに従ってマニピュレータ１０を制御する。

【００２３】ボード計算機２１はマニピュレータ１０の
手先３０の位置、速度の制御及び各関節角の位置、速
度、加速度を計算する。また、後述する冗長関節角の制
御等の全体の制御を行う。各関節角の位置、速度、加速
度及び冗長関節角のデータはボード計算機２２に送られ
る。ボード計算機２２ではこれらのデータを受け取り、
マニピュレータ１０の各関節の位置、速度及び加速度を
制御する。

【００２４】ＡＤ変換器２３はマニピュレータ１０の各
関節の力検出器からの出力をディジタル値に変換する。
ＤＩＯ２４はマニピュレータ１０とボード計算機２１，
２２との間の入出力信号の授受を行うインタフェースで
ある。ＤＡ変換器２５はボード計算機２２からの各関節
の速度指令等を受け取り、モータドライバ２８に送り、
モータドライバ２８はこの指令に従ってモータ１０Ａを
駆動する。

【００２５】モータ１０Ａにはエンコーダ１０Ｂが同軸
に結合されており、モータ１０Ａの回転位置をフィード
バックする。すなわち、エンコーダ１０Ｂはモータの回
転に応じてパルスを発生する。このパルスはカウンタ２
６でカウントされ、ボード計算機２１，２２はこのカウ
ンタ２６を読み取ることにより、モータ１０Ａの回転位
置を認識できる。また、このパルスはＦ／Ｖ（周波数／
速度）変換器２９によって速度に変換され、ＡＤ変換器
２７に送られる。そして、ＡＤ変換器２７によって、デ
ィジタル値に変換される。ボード計算機２１，２２はこ
のディジタル値を読み取ることにより、モータ１０Ａの
速度、言い換えれば各関節の速度を認識することができ
る。

【００２６】勿論、図２では関節は１個のみで示したの
で、ＡＤ変換器２３、ＤＩＯ２４、ＤＡ変換器２５、カ
ウンタ２６、ＡＤ変換器２７等は関節の数に応じた数の
み必要である。ただし、ＤＩＯ２４等は共通に使用する
ように構成することもできる。また、関節を駆動するモ
ータ１０Ａも実際には７個必要であり、モータドライバ
２８、Ｆ／Ｖ変換器２９についても同様である。

【００２７】図３はマニピュレータの構成を示す図であ
る。マニピュレータ１０は７自由度を有し、アーム１１
〜１７、関節１１ａ〜１７ａを有し、アーム１７の先端
には、手首３０が固定されている。また、各関節の回転
方向はθ１〜θ７で示している。

【００２８】ベース１０ａの上端には回転関節である第
１関節１１ａの下側が固定されており、第１関節１１ａ
の上側には第１アーム１１の下端が固定されている。第
１アーム１１の上端には屈曲関節である第２関節１２ａ
の一方が固定されており、第２関節１２ａの他方には第
２アーム１２の下端が固定されている。さらに、第２ア
ーム１２の上端には回転関節１３ａの下側が固定されて
おり、第３関節１３ａの上側には第３アーム１２の下端
が固定されている。第３アームは途中で９０°屈曲して
いる。第３アーム１３の他端には屈曲関節である第４関
節１４ａの一方が固定されている。第４関節１４ａの他
方には第４アーム１４が固定されている。第４アーム１
４も途中で９０°屈曲している。

【００２９】第４アーム１４の上端には回転関節である
第５関節１５ａの下側が固定さている。第５関節１５ａ
の上側には第５アームの下端が固定されている。第５ア
ームの上端には屈曲関節である第６関節１６ａの一方が
固定されており、第６関節１６ａの他方には第６アーム
１６の下端が固定されている。第６アーム１６の上端に
は回転関節である第７関節１７ａの下側が固定されてお
り、第７関節１７ａの上側には第７アーム１７の下端が
固定されている。第７アーム１７の先端には手先３０が
固定されている。すなわち、第７関節１７ａは手先の回
転を制御する。

【００３０】ここで、第１関節１１ａ、第２関節１２
ａ、第３関節１３ａの何れか一つを冗長関節として使用
することができる。次に、図１を用いて、上記ボード計
算機２２において実行される本発明の冗長マニピュレー
タの制御方式を説明する。本発明は、マニピュレータ１
０を制御する際のアルゴリズムを、その手先３０の運動
を制御するアルゴリズムと、冗長自由度を利用する戦略
を制御するアルゴリズムとに分離し、独立させるように
したものである。この制御方式を実現するため、図に示
すように、冗長自由度制御手段３と、手先運動制御手段
５とが設けられる。

【００３１】マニピュレータ１０の各関節１１ａ〜１７
ａのうち、１個の関節を冗長自由度制御用関節として割
り当てる。ここでは、第３関節１３ａを冗長自由度制御
用関節とする。それ以外の６個の関節１１ａ、１２ａ、
１４ａ、１５ａ、１６ａ、１７ａを手先運動制御用関節
として割り当てる。この６個の手先運動制御用関節１１
ａ、１２ａ、１４ａ、１５ａ、１６ａ、１７ａによっ
て、６自由度を持つ手先の運動が決まる。

【００３２】上記の冗長自由度制御手段３は、角度検出
手段１（図２のエンコーダ１０Ｂ）から送られてくる現
在の関節角θ１〜θ７を用いて、現在の手先３０の位置
姿勢を求め、その手先３０の位置姿勢とパラメータ化し
た第３関節１３ａの関節角θ３とを用いて逆運動学方程
式を解き、その計算で得られた各関節角を用いて特異点
回避のための評価関数や他の評価関数のポテンシャル値
を求め評価する。その評価を満足したとき、パラメータ
である第３関節１３ａの関節角度θ３を決定し、速度指
令値θ３^* を出力する。なお、特異点回避のための評価
関数を設定した場合については、詳細を後述する。

【００３３】一方、手先運動制御手段５は、角度検出手
段１から送られてくる現在の関節角度θ１〜θ７と、運
動目標値指令手段４からの手先の運動目標値（目標速度
軌道）ｙ_d ^* とを用いて逆運動学方程式を解き、手先運
動制御用関節１１ａ、１２ａ、１４ａ、１５ａ、１６
ａ、１７ａの速度指令値θ１^* 、θ２^* 、θ４^* 、θ５
^* 、θ６^* 、θ７^* を求める。これらの速度指令値θ１
^* 〜θ７^* は、各関節制御手段６に送られ、マニピュレ
ータ１０の動作を制御する。マニピュレータ１０の動作
後の各関節角は、角度検出手段１で検出され、上述した
ように、冗長自由度制御手段３に与えられる。

【００３４】このように、本実施例では、マニピュレー
タ制御のための計算を冗長自由度制御手段３と、手先運
動制御手段５とに分けて行う。このため、制御周期を別
々に設定することができる。したがって、サンプリング
周期を上げて実時間で冗長マニピュレータを動作させる
ことができる。

【００３５】図４は本発明の第２の実施例を示す図であ
る。この実施例では、手先運動制御手段５Ａは正方化し
たヤコビアンを用いて手先の速度指令値を求める。以下
にその演算式について説明する。

【００３６】７自由度のマニピュレータ１０の場合、前
述した（２）式（ｙ^* ＝Ｊ・θ^* ）を詳しく書くと、次
式（７）となる。

【００３７】

【数７】

【００３８】ここで、第３関節１３ａの関節角θ３は、
冗長自由度制御手段３で制御することを前提にし、その
関節角θ３を既知（パラメータ）として関係する項を外
に出すと、次式（８）となる。

【００３９】

【数８】

【００４０】（８）式を改めて簡略化した記法で書く
と、

【００４１】

【数９】 ｙ^* ＝Ｊ66・Θ₆ ^* ＋Ｋ₃ ・θ₃ ^* ・・・・・（９） ここで、Θ₆ ^* ：θ３^* を除いたθベクトル Ｊ66 ：６×６に正方化したヤコビアン Ｋ₃ ：θ３^* に対するヤコビアン したがって、

【００４２】

【数１０】

【００４３】により、θ３以外の関節の運動目標値Θ_6d
^* が求まる。手先運動制御手段５Ａは、演算手段８で求
めたＪ66の逆行列Ｊ66^-1とＫ₃ 、運動目標値指令手段４
からの手先の運動目標値ｙ_d ^* 、及び変換手段７で得ら
れた速度指令値θ₃ ^* を基にして、上記（１０）式を解
くことで、手先運動制御用関節１１ａ、１２ａ、１４
ａ、１５ａ、１６ａ、１７ａの速度指令値θ１^* 、θ２
^* 、θ４^* 、θ５^* 、θ６^* 、θ７^* を求める。すなわ
ち、本実施例では、疑似逆行列を求めることなく、正方
化したヤコビアンを用いて手先の速度指令値θ１ ^* 、θ
２^* 、θ４^* 、θ５^* 、θ６^* 、θ７^* を求めることが
できる。したがって、その分、演算時間を短縮してサン
プリング周期を上げることができる。

【００４４】図５は本発明の第３の実施例を示す図であ
る。この実施例では、インピーダンス制御等をする場合
の加速度制御に関し、手先の加速度指令値を、上記の第
２の実施例と同様に、正方化したヤコビアンを用いて求
める。以下にその演算式について説明する。

【００４５】上記（９）式でＪ66及びＫ₃ の各要素が、
そのときの関節角の関数であることを考慮して、（９）
式の両辺を時間微分すると、

【００４６】

【数１１】 Ｘ^**＝Ｊ66・Θ₆ ^**＋Ｊ66^* ・Θ₆ ^* ＋Ｋ₃ ・θ３^**＋Ｋ₃ ^* ・θ３^* ・・・・・（１１） となり、Θ₆ ^**について解くと、

【００４７】

【数１２】 Θ₆ ^**＝Ｊ66^-1・（Ｘ^**−Ｊ66^* ・Θ₆ ^* −Ｋ₃ ・θ３^**−Ｋ₃ ^* ・θ３^* ） ・・・・・（１２） したがって、手先運動加速度制御手段９は、演算手段８
Ｂで求めたＪ66の逆行列Ｊ66^-1、Ｊ66^* 、Ｋ₃ 及びＫ₃
^* 、加速度目標値指令手段４０からの手先の加速度目標
値ｙ_d ^**、並びに変換手段７Ｂで得られた速度指令値θ
３^* 及び加速度指令値θ３^**を基にして、上記（１２）
式を解くことで、手先運動制御用関節１１ａ、１２ａ、
１４ａ、１５ａ、１６ａ、１７ａの加速度指令値θ
１^**、θ２^**、θ４^**、θ５^**、θ６^**、θ７^**を求め
る。すなわち、本実施例では、上記第２の実施例の場合
と同様に、疑似逆行列を求めることなく、正方化したヤ
コビアンを用いて手先の加速度指令値θ１^**、θ２^**、
θ４^**、θ５^**、θ６^**、θ７ ^**を求めることができ
る。したがって、その分、演算時間を短縮することがで
き、インピーダンス制御で加速度を制御する場合にも、
サンプリング周期を上げることができる。

【００４８】次に、上述した冗長自由度制御手段３にお
いて、特異点回避のための評価関数を設定した場合につ
いて説明する。図６は冗長関節をパラメータ化して特異
点を回避する処理手順のフローチャートである。図にお
いて、Ｓに続く数値はステップ番号を示す。 〔Ｓ１〕現在の関節角θ１〜θ７を読む。 〔Ｓ２〕現在の関節角θ１〜θ７から、現在の手先の位
置姿勢を計算する。 〔Ｓ３〕第３関節をパラメータ化する。すなわち、第３
関節を特異点を回避するための関節として、その他の関
節の制御で手先の位置姿勢を制御する。 〔Ｓ４〕第３関節を固定し、第３関節を除く他の６個の
関節によって、逆運動方程式を解き、６個の関節の関節
角を計算する。 〔Ｓ５〕計算された各関節の関節角及びパラメータ化し
た第３関節の関節角から、評価関数を求める後述の（１
３）式によって、ポテンシャル値Ｐを求める。 〔Ｓ６〕ポテンシャル値Ｐがしきい値Ｐｋ以下かどうか
判断し、そうであれば、Ｓ８へ、しきい値以下でなけれ
ばＳ７へ進む。 〔Ｓ７〕ポテンシャル値Ｐがしきい値Ｐｋ以下ではない
ので、Ｓ４で求められた各関節の関節角の回転位置では
特異点かあるいはその近傍に入ってしまう可能性があ
る。したがって、計算された各関節の関節角の回転位置
からポテンシャル関数の勾配を求めて、第３関節の関節
角を修正する。その詳細は後述する。 〔Ｓ８〕ポテンシャル値Ｐがしきい値Ｐｋ以下であり、
特異点あるいはその近傍内ではないので、Ｓ４で計算し
た各関節の関節角、及びパラメータで固定した第３関節
の関節角を移動すべき角度として決定する。 〔Ｓ９〕アームを動作させる。すなわち、Ｓ８で決定し
た第３関節の関節角と、現在の第３関節の関節角との差
から第３関節の速度指令値θ３^* を求め、その速度指令
値θ３を出力してアームを動作させる。

【００４９】次にＳ７で説明した第３関節の関節角の修
正の詳細について説明する。第３関節の関節角に関して
は、以下のような逐次近似解法による局所最適化によっ
て実行する。

【００５０】まず、ポテンシャル関数Ｐ＝ｆ（ｘ）を
（１３）式とする。

【００５１】

【数１３】

【００５２】なお、図３に示す７自由度のマニピュレー
タの特異点を解析的に求めると以下の通りである。 （ａ）θ２＝０°でθ３＝９０°、θ２＝０°でθ３＝
−９０° （ｂ）θ４≒１３．４°、またはθ４≒１６６．６° （ｃ）θ６＝０°、またはθ６＝１８０° である。これらは、マニピュレータの関節の構造、各ア
ームの長さで異なるが、その詳細は本発明とは直接関係
ないので省略する。

【００５３】したがって、特異点を有する関節は、第２
関節、第３関節、第４関節、第６関節であるので、（１
３）式のｉは２，３，４，６について求めて、この和を
求める。

【００５４】ここで、冗長関節である第３関節の関節角
をｘとし、 ｘ⁰ ：第３関節角の初期値 ｘ^k+1 ＝ｘ^k ＋α^k ・ｄ^k （ｋ＝０，１，２，・・・） ｄ ：探索方向 α ：ステップ幅 とする。

【００５５】まず、ステップ幅を決定する。（１３）式
の評価関数ｆ（ｘ）からステップ幅を決めるφを、

【００５６】

【数１４】 φ（α）＝ｆ（ｘ^k ＋α^k ｄ^k ） ───（１４） とし、これをセカント法と呼ばれる以下の（１５）式で
示される反復公式により求める。

【００５７】

【数１５】

【００５８】次に、探索方向を決定する。探索方向は準
ニュートン法により決定する。準ニュートン法では、以
下の（１６）式を逐次計算することにより探索方向ベク
トルｄを決定する。

【００５９】

【数１６】 ｄ^k ＝−（Ｈ^k ）^-1∇ｆ（ｘ^k ） ───（１６） ただし、Ｈはヘシアン（Ｈｅｓｓｉａｎ）すなわちヘッ
セ行列、∇はラプラシアンである。ここで、行列の系列
｛Ｈ^k ｝は以下の（１７）式によって求められる。

【００６０】

【数１７】

【００６１】このように、冗長自由度制御手段３におい
て、先ず冗長関節を決定し、この関節角をパラメータと
して扱う。これにより、マニピュレータは６自由度にな
り、手先の位置と姿勢の目標値を与えれば、逆運動方程
式から冗長関節以外の関節角は求められる。この冗長関
節の関節角及び他の関節角から特異点を評価する評価関
数のポテンシャル値Ｐを計算する。例えば、この評価関
数は特異点近傍で急激に増加する評価関数とする。そし
て、この評価関数のポテンシャル値Ｐが一定値Ｐｋ以下
のときは、先に求めた冗長関節及び他の関節の関節角を
移動目標値として、関節を制御する。一方、計算したポ
テンシャル値Ｐが一定値Ｐｋ以下でないときは、冗長関
節の関節角を修正して、一定値Ｐｋ以下になるように、
冗長関節の関節角を修正する。これによって、ポテンシ
ャル値Ｐが常に一定値Ｐｋ以下になるようにして、特異
点を回避するように制御する。

【００６２】上記の特異点評価の手法において、評価関
数ｆ（ｘ）を導入し、また、その局所最適化に準ニュー
トン法を用いた。このため、収束性を速めることがで
き、その分、冗長自由度制御手段３でのサンプリング周
期を上げることができる。したがって、マニピュレータ
を実時間で、高速に動かすことができる。

【００６３】

【発明の効果】以上説明したように本発明では、冗長自
由度制御手段と、手先運動制御手段とを設けて、冗長マ
ニピュレータを制御する際のアルゴリズムを、その手先
の運動を制御するアルゴリズムと、冗長自由度を利用す
る戦略を制御するアルゴリズムとに分離し、独立させ
た。

【００６４】したがって、制御周期を別々に設定するこ
とができ、サンプリング周期を上げて実時間で冗長マニ
ピュレータを動作させることができる。また、冗長自由
度制御手段での特異点評価の手法において、評価関数ｆ
（ｘ）を導入し、また、その局所最適化に準ニュートン
法を用いた。

【００６５】このため、収束性を速めることができ、そ
の分、サンプリング周期を上げることができる。したが
って、冗長マニピュレータを実時間で、高速に動かすこ
とができる。

【図面の簡単な説明】

【図１】本発明の原理ブロック図である。

【図２】マニピュレータの制御系のハードウェアのブロ
ック図である。

【図３】マニピュレータの構成を示す図である。

【図４】本発明の第２の実施例を示す図である。

【図５】本発明の第３の実施例を示す図である。

【図６】冗長関節をパラメータ化して特異点を回避する
処理手順のフローチャートである。

【符号の説明】

１ 角度検出手段 ３ 冗長自由度制御手段 ４ 運動目標値指令手段 ５，５Ａ 手先運動制御手段 ６，６Ａ，６Ｂ 各関節制御手段 ７，７Ｂ 変換手段 ８，８Ｂ 演算手段 ９ 手先運動加速度制御手段 １０ マニピュレータ １１ａ〜１７ａ 第１〜第７関節 １３ａ 冗長関節 ２２ ボード計算機 ３０ 手先 ４０ 加速度目標値指令手段

───────────────────────────────────────────────────── フロントページの続き (72)発明者 渡辺 一郎 神奈川県川崎市中原区上小田中1015番地 富士通株式会社内

Claims (7)

【特許請求の範囲】
1. 【請求項１】 ７自由度以上の自由度を有する冗長マニ
   ピュレータの制御方式において、 前記冗長マニピュレータの各関節の中から割り当てた冗
   長自由度制御用関節の運動指令値を求める冗長自由度制
   御手段（３）と、 前記冗長自由度制御用関節以外の前記冗長マニピュレー
   タの各関節から成る手先運動制御用関節の運動指令値を
   求める手先運動制御手段（５）と、 を有することを特徴とする冗長マニピュレータの制御方
   式。
2. 【請求項２】 前記手先運動制御手段（５）は、疑似逆
   行列を用いずに前記運動指令値を求めることを特徴とす
   る請求項１記載の冗長マニピュレータの制御方式。
3. 【請求項３】 前記手先制御用関節の加速度指令値を求
   める手先運動加速度制御手段を設け、前記手先運動加速
   度制御手段は、疑似逆行列を用いずに前記加速度指令値
   を求めることを特徴とする請求項１記載の冗長マニピュ
   レータの制御方式。
4. 【請求項４】 ７自由度以上の自由度を有する冗長マニ
   ピュレータの制御方式において、 少なくとも１つの冗長関節の関節角をパラメータ化して
   冗長マニピュレータを６自由度に縮退させ、 手先の位置及び姿勢の目標値を与えて逆運動学方程式か
   ら前記冗長関節以外の関節の関節角を決定し、 前記パラメータ化した冗長関節の関節角及び前記冗長関
   節以外の関節の関節角を用いて特異点を回避する評価関
   数のポテンシャル値を計算し、前記ポテンシャル値が所
   定の値以下になるように前記パラメータを修正し、 前記冗長マニピュレータが特異点を回避するように制御
   することを特徴とする冗長マニピュレータの制御方式。
5. 【請求項５】 前記ポテンシャル値の計算及び前記パラ
   メータの修正は、前記冗長マニピュレータの動作時に実
   時間で行うことを特徴とする請求項４記載の冗長マニピ
   ュレータの制御方式。
6. 【請求項６】 前記評価関数は次の式、 【数１】 であることを特徴とする請求項４記載の冗長マニピュレ
   ータの制御方式。
7. 【請求項７】 前記冗長関節角の探索方向とステップ幅
   を逐次近似解法によって決定し、前記探索方向はセカン
   ト法により求め、前記探索方向は準ニュートン法により
   決定することを特徴とする請求項４記載の冗長マニピュ
   レータの制御方式。