JPH07314363A - 多関節マニピュレータの運動制御方法及び装置 - Google Patents

Abstract

(57)【要約】 【目的】特異点及びその近傍においても安定に動作可能
な多関節マニピュレータ先端の直交座標における速度制
御を可能とする多関節マニピュレータの運動制御方法及
び装置を提供する。 【構成】比較指令部３と速度制御装置１″とマニピュレ
ータ機構２を順次一連システム化した多関節マニピュレ
ータの運動制御系において、速度制御装置１″は、現在
関節角度操作量信号ｄに基づき演算したヤコビ行列の近
似逆行列に、入力した速度指令値ｖ_c を乗算して関節駆
動速度指令ｂ″としての 【外１】 【外２】 基づき、各関節α〜ζに取付けられたサーボモータの関
節角速度ｃ″を積分し、現在関節角速度操作量信号ｄを
出力する積分部１ｃ″とで構成することを特徴とする。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、産業用ロボットなどに
用いられる多関節マニピュレータの運動制御方法及びそ
の実施に直接しようする装置に関する。

【０００２】

【従来の技術】従来より用いられている産業用ロボット
には、広い範囲における多自由度の動作が実現可能であ
ることから、多関節マニピュレータが広く用いられてい
る。特に３次元空間における自由度では位置と姿勢に関
して６自由度であるため、６自由度の多関節マニピュレ
ータが使われることが多い。

【０００３】一般的に当該マニピュレータは、それぞれ
独立して駆動される１自由度を有する複数の関節およ
び、上記関節を接続する複数のリンクにより構成される
というシリアルリンク構造をしている。また、この産業
用多関節マニピュレータとしては、各関節の駆動装置と
して速度制御を行えるものが多い。

【０００４】産業用ロボットには外界に固定された作業
対象に対して各種作業をすることが要求されることが多
く、この場合、作業対象に対して固定された直交座標系
において指示された動作を実現しなければならない。直
交座標系において表現された動作を行うためには、直交
座標系において指定された速度を当該マニピュレータ先
端（以下、先端と省略する）に生じさせる必要がある。

【０００５】ところが、各関節の駆動速度と、各関節が
駆動された結果生じる先端の速度との間には、キネマテ
ィクスと呼ばれる関係がある。すなわち、関節角速度ベ
クトルθに対する先端の速度ベクトルｖは

【数１】 と表される。ここで、Ｊ（θ）はヤコビ行列と呼ばれ、
瞬時の関節角θに依存する。

【０００６】Ｊ（θ）は関節角θの取る値によってはノ
ン・フルランク（non-full rank ）になる。Ｊ（θ）が
ノン・フルランク（non-full rank ）になる、関節角お
よびその時の先端の位置を特異点という。一般に（１）
式の関係は順キネマティクス（kinematics）と呼ばれ
る。これに対して（１）式の逆変換、すなわち、先端の
速度ベクトルｖに対する関節角速度ベクトルθを表す関
係は逆キネマティクスと呼ばれるが、特異点では、Ｊ
（θ）の逆行列が存在しないことにより、（１）式の厳
密な逆変換は存在しない。

【０００７】Ｊ（θ）がフルランク（full rank ）のと
き、逆キネマティクスは

【数２】 と表される。マニピュレータの先端の直交座標系におけ
る速度を制御する方法としては、従来、（２）式の関係
を用いて、直交座標系における速度指令値から、関節角
速度の指令値へ変換し、この指令値にしたがって関節角
を速度制御する分解動作速度制御が用いられてきた。

【０００８】図５に分解動作速度制御の原理を説明する
ブロック線図を示す。この制御法については、例えば、
ロボティクスＰ．２５５（Ｋ．Ｓ．フー他著，本田庸悟
他訳，日刊工業新聞社刊）に記述されている。同図中、
ａは先端速度指令動作信号、ｂは関節駆動速度指令、ｃ
は関節角速度、ｄは関節角度操作信号、ｅは先端速度、
１は速度制御装置、１ａは逆ヤコビ行列演算部、１ｂは
速度制御部、１ｃは積算部、２はマニピュレータ機構で
ある。

【０００９】また、図６に示すような、分解速度制御法
を応用して、直交座標系における位置制御を実現する方
法も提案されている。これは、目標値と現在位置の直交
座標系における位置偏差に応じた直交座標系の速度を、
分解速度制御法を用いて先端に生じさせることにより、
位置誤差をなくすという考えに基づいている。例えば、
多自由度ロボットの仮想コンプライアンス制御（計測自
動制御学会論文集，ｖｏｌ．２２，ｎｏ．３，１９８
６）はこの考え方に基づいている。この手法の特長とし
ては、図６の信号ａが直交座標系における速度指令であ
ることに基づくと、目標位置軌道に応じた速度目標値を
同図信号ｉとしていれることにより、容易にフィードフ
ォワード制御を行えることがある。

【００１０】同図中、ｆは位置目標値、ｇは位置誤差、
ｈは修正速度指令値、ｉは先端速度目標値、ｊは演算先
端位置、ｋは先端位置、３は比較指令部、３ａは減算
器、３ｂは位置誤差フィードバック係数乗算部、３ｃは
加算器、４は関節角−先端位置変換部である。なお、図
５の分解速度制御のブロックダイヤグラムと同一部分は
同一符号を付した。

【００１１】

【発明が解決しようとする課題】しかし、特異点におい
てはＪ^-1（θ）が存在しないため、前記のどちらの方法
に於いても、特異点を通過することができない。また、
特異点近傍においてはＪ^-1（θ）の要素のいくつかが非
常に大きな値になる。その結果（２）式を用いてθを計
算すると、直交座標系における速度指令値が十分小さく
ても、算出された関節角速度指令値が非常に大きくなる
場合があり、特異点近傍においては制御に支障が生じ
る。

【００１２】以上の２つの理由により、（２）式を用い
た手法では、特異点およびその近傍をマニピュレータの
作業領域から除外しなければならないという制約条件が
生じる。一般に特異点はマニピュレータの可動範囲に広
く点在しているため、前記の従来の制御方法はマニピュ
レータの作業範囲に対して著しい制限を与えるという欠
点を有している。

【００１３】前記の従来手法の欠点はＪ^-1（θ）を用い
ることによる。そこで、前記、分解速度制御を用いた直
交座標系における位置制御法において、特異点における
問題を回避するために、図７に示す、ヤコビ行列の逆行
列Ｊ^-1（θ）のかわりにヤコビ行列の転置Ｊ^Ｔ（θ）を
用いた位置制御法が提案されている（特願平５−１２１
０５５）。しかし、ヤコビ行列の転置Ｊ^Ｔ（θ）を用い
る結果、図６の信号ａに相当する図７の信号ｌは、直交
座標系における速度指令値という意味をもたない。

【００１４】すなわち、直交座標系における速度制御計
を用いて位置制御を実現するという意味がなくなり、全
体として安定なレギュレーションを実現する位置制御法
としてのみ意味をもつ。その結果、位置誤差の結果生じ
る先端の速度は位置誤差に比例せず、位置誤差の収束の
仕方、時間とともに変化する目標値に対する追従性もか
ならずしも保証されない、また、速度目標値による速度
のフィードフォワードを行うことができない。

【００１５】同図中、ｂ′は関節駆動速度指令、ｃ′は
関節角速度、ｌは誤差フィードバック指令値、１′は速
度制御装置、１ａ′は転置ヤコビ行列演算部、１ｂ′は
速度制御部、１ｃ′は積分部、３′は比較指令部、３
ａ′は減算器、３ｂ′は位置誤差フィードバック係数乗
算部である。なお、図６の分解速度制御を利用した位置
制御装置のブロックと同一部分は同一符号を付した。

【００１６】以上説明したように、従来、特異点および
その近傍において安定に動作可能な、多関節マニピュレ
ータ先端の直交座標における速度制御法は提案されてい
なかった。ここにおいて、本発明は従来技術のこのよう
な問題点の解決をねらったもので特異点およびその近傍
においても安定に動作可能な、多関節マニピュレータ先
端の直交座標における速度制御を可能とする多関節マニ
ピュレータの運動制御方法および装置を提供せんとする
ものである。

【００１７】

【課題を解決するための手段】前記課題の解決は、本発
明が次に列挙する新規な特徴的構成手法および装置を採
用することにより達成される。すなわち、本発明の特徴
は、各関節が独立に速度制御される多関節マニピュレー
タのエンドエフェクタ先端の直交座標系において、当該
マニピュレータの先端の速度と各角関節角度の関係を表
すヤコビ行列の近似逆行列を用い、前記直交座標系にお
ける速度成分の指令値に当該ヤコビ行列の近似逆行列を
かけることによって、当該マニピュレータの関節角の駆
動速度指令値に基づき各関節を駆動することにより、当
該マニピュレータ先端の直交座標系における速度成分の
制御を行うに当り、前記近似逆行列として、当該マニピ
ュレータのヤコビ行列の行列式が０となる特異点におい
てはヤコビ行列の疑似逆行列を用い、当該特異点近傍の
点においてはヤコビ行列の逆行列とその近傍点に最も近
い特異点におけるヤコビ行列の疑似逆行列とから合成さ
れる行列を用い、前記特異点および特異点近傍以外では
ヤコビ行列の逆行列を用いてなる多関節マニピュレータ
の運動制御方法である。

【００１８】本発明装置の第１の特徴は、先端位置目標
値としての位置指令値と、先端速度目標値としての速度
指令値と、マニピュレータのサーボモータに内臓の角度
検出器により得られる現在関節角度のフィードバック変
換による演算先端位置としての現在位置との各データの
入力に基づき先端速度指令の動作信号としての速度指令
値を演算出力する比較指令部と、当該先端速度指令動作
信号を入力し前記現在関節角度の操作量信号を算出する
速度制御装置と、当該現在関節角度操作量信号により各
関節を制御されるマニピュレータ機構とを順次一連シス
テム化した多関節マニピュレータの運動制御系におい
て、前記速度制御装置は、前記現在関節角度操作量信号
に基づき演算したヤコビ行列の近似逆行列に、入力した
前記速度指令値を乗算して関節駆動速度指令としての関
節角速度指令ベクトルを生成する近似逆ヤコビ行列演算
部と、当該関節角速度指令ベクトルに基づき、前記各関
数に取付けられているサーボモータの関節角速度を求め
る速度制御部と、当該関節角度を積分し前記現在関節角
度操作量信号を出力する積分部と、で構成してなる多関
節マニピュレータの運動制御装置である。

【００１９】本発明装置の第２の特徴は、前記本発明装
置の第１の特徴における多関節マニピュレータの運動制
御系が、閉ループのフィードバック制御と開ループのフ
ィードフォワード制御の組合せからなる多関節マニピュ
レータの運動制御装置。

【００２０】本発明装置の第３の特徴は、前記本発明装
置の第１又は第２の特徴における現在関節角度の現在位
置への変換が、フィードバック系中に関節角−先端位置
変換部を挿入介接してなる多関節マニピュレータの運動
制御装置。

【００２１】本発明装置のの第４の特徴は、前記第１，
第２，第３又は第４の特徴における比較指令部が、位置
指令値と現在位置との比較位置誤差を算出する減算器
と、当該位置誤差にフィードバック係数を乗算して修正
速度指令値を求める位置誤差フィードバック係数乗算部
と、当該修正速度指令値に速度指令値を加算して先端速
度指令動作信号を演算出力する加算器と、から構成され
る多関節マニピュレータの運動制御装置である。

【００２２】

【作用】本発明は、前記のような新規の手法及び手段を
講じ、特異点ではヤコビ行列の逆行列Ｊ^-1（θ）が存在
しないため、直交座標での速度から関節角速度を求める
のに（２）式を用いることができない。これは、Ｊ
（θ）がノン・フルランクとなる特異点においては、い
かなる関節角速度θを与えても、（１）式で与えられる
ｖでは実現できない速度成分が存在することに起因す
る。

【００２３】また、特異点近傍ではヤコビ行列の逆行列
Ｊ^-1（θ）の要素の一部が非常に大きな値を持ち、特異
点に限りなく近付けると無限大に発散する。そのため、
特異点近傍で（２）式を用いると、関節角速度の指令値
が非常に大きくなり運動の実現に問題が生じる。これ
は、特異点近傍では実現困難な速度成分があるにも関わ
らず、その実現困難な速度成分を無理に実現させようと
することに起因する。

【００２４】そこで、ヤコビ行列Ｊ（θ）の逆行列Ｊ^-1
（θ）の代わりに、以下の式で定義される近似逆行列Ｊ
^# （θ）を用いることにより、特異点およびその近傍で
は、実現不可能なもしくは実現困難な速度成分は生成せ
ずに、適切に実現できる速度成分のみ生成する。

【数３】 ここで、Ｆ（θ）は特異点で０となる、特異点からの距
離を評価する適当な評価関数である。また、θ_s はθか
らもっとも近い特異点、Ｊ⁺ （θ）はヤコビ行列Ｊ
（θ）の疑似逆行列であり、特異点以外ではＪ^-1（θ）
に一致する。

【００２５】そして分解速度制御で使われる前記（２）
式における逆行列Ｊ^-1（θ）に代わり前記（３）式で定
義される近似逆行列Ｊ^# （θ）を用い、

【数４】 により、直交座標系における速度ｖを関節角速度θに変
換する。

【００２６】近似逆行列Ｊ^# （θ）を用いることの妥当
性を示すために、前記（３）式の意味を説明しておく。
疑似逆行列Ｊ⁺ はＪに対して必ず唯一に定められる行列
で、θ ＝Ｊ⁺ v により、ｍｉｎ（‖ｖ−Ｊθ‖₂ ）を
みたす最小のθを与える。つまり、Ｊ⁺ （θ）はＪ
（θ）がフルランクの時、すなわち特異点以外ではＪ^-1
（θ）に一致し、Ｊ（θ）がノン・フルランクの時、す
なわち特異点では上式の意味で最適な近似解を与える、
Ｊ（θ）の近似逆変換である。

【００２７】このことは、求められる直交座標系におけ
る速度ｖに対して、特異点においては、実現可能でかつ
ｖにもっとも近い速度が、Ｊ（θ）Ｊ⁺ （θ）ｖで与え
られることを意味する。一方、特異点以外では、Ｊ
⁺ （θ）はＪ^-1（θ）に一致するため、特異点近傍では
Ｊ^-1（θ）すなわちＪ⁺ （θ）の要素の一部が非常に大
きな値となり、特異点と近付くとともに無限大に発散す
る。それに対し、特異点においてはＪ⁺ （θ）は要素は
有限の値となる。

【００２８】すなわち、Ｊ⁺ （θ）は得意点とその近傍
で不連続になる。このことは、前記（２）式におけるＪ
^-1（θ）のかわりにＪ⁺ （θ）を単純に用いると、特異
点の前後で関節角速度の指令値θが不連続になり制御の
実現上問題となる。そこで、特異点の近傍では、その点
における疑似逆行列Ｊ⁺ （θ）と、その点に最も近い特
異点θ_s における疑似逆行列Ｊ⁺ （θ_s ）の加重和をと
ることにより、連続性を確保する。

【００２９】また、特異点において実現不可能な速度成
分と、その近傍で実現しにくい速度成分の方向はほぼ一
致するので、特異点近傍において前記（３）式の加重和
でＪ^-1を近似することは、その点において実現しやすい
速度成分を重視したこととなる。

【００３０】以上より、分解速度制御で使われる前記
（２）式に代わり前記（３）式で定義される近似逆行列
Ｊ^# （θ）を使った前記（４）式を用いることにより、
入力として実現困難な範囲で速度指令値に近い適切な速
度を実現し、それ以外に時は与えられた速度指令値に一
致した速度を実現する、特異点およびその近傍において
も安定に動作可能な、多関節マニピュレータ先端の直交
座標における速度制御が実現される。

【００３１】

【実施例】

（装置例）本発明の装置例を図面を用いて説明する。図
１は本装置例においてマニピュレータ機構を制御する速
度制御装置の構成ブロック線図、図２は本装置例の構成
システムブロック線図、図３は本装置例を応用した３自
由度の構成例模式図、図４は同・産業用６自由度多関節
ロボットの手首機構を示す斜面図である。

【００３２】図中、Ａは多関節ロボットの手首機構、
１″は速度制御装置、１ａ″は近似逆ヤコビ行列演算
部、１ｂ″は速度制御部、１ｃ″は積分部、ｂ″は関節
駆動速度指令、ｃ″は関節角速度である。なお、図６に
示す従来例と同一部分は同一符号を付して重複説明を避
けた。

【００３３】本装置例では、直交座標系における速度指
令ベクトルｖ_c が入力されると、近似ヤコビ逆行列演算
部１ａ″は、マニピュレータのいずれも図示しないサー
ボモータに内蔵の角度検出器により得られる現在関節角
θに基づき、本実施例で提案するヤコビ行列の近似逆行
列Ｊ^# （θ）を演算し、得られた近似逆行列Ｊ^# （θ）
に速度指令ベクトルｖ_c を乗算することにより、

【外１】

【００３４】

【外２】 に内蔵の速度制御部１ｂ″により、マニピュレータ機構
２の各関節α〜ζにとりつけられているサーボモータの
角速度が

【外３】

【外４】

【外５】

【外６】 マニピュレータ機構２により、マニピュレータ先端に速
度ベクトルｖの速度が生じる。なお、簡単のためのサー
ボモータについては速度制御部１ｂ″についてのみ示
し、他は省略してある。

【００３４】次に、具体的な機構を用いてヤコビ行列の
近似逆行列の具体的な構成例を示す。簡単のため、図３
に示した３自由度の機構例を考える。この機構は図４に
例を示したような産業用６自由度多関節ロボットＡの手
首機構としてよく用いられるもので、主な役割は、姿勢
の向きおよび姿勢の変化する角速度を制御することであ
る。したがって、直交座標系における速度として、図２
に示した姿勢角速度ω＝［ω_ｘ，ω_ｙ，ω_ｚ］^Ｔを考え
る。

【００３５】関節角位置を、［θ_ａ，θ_ｂ，θ_ｃ］^Ｔと
すると順キネマティクスは

【数５】 となる。

【００３６】当該（５）式で、Ｓ_a ＝ｓｉｎθ_a ，Ｃ_a
＝ｓｉｎθ_a ，Ｓ_b ＝ｓｉｎθ_b ，Ｃ_b ＝ｓｉｎθ_b 、
の場合

【数６】 である。Ｊ（θ）の行列は、det Ｊ（θ）＝ｓｉｎθ_b
となる。すなわち、θ_b＝０±ｎπが特異点である。

【００３７】特異点以外における疑似逆行列は、

【数７】 となる。

【００３８】特異点θ＝θ_s 、すなわちｓｉｎθ_b ＝０
である時の疑似逆行列は、

【数８】 となる。

【００３９】これより、疑似逆行列Ｊ+ （θ）が、θ＝
θs において不連続になり、θがθs 近傍において無限
大に発散することが確認できる。なお、疑似逆行列の求
め方については、例えば、システム制御のためのマトリ
ックス理論Ｐ．３３３（児玉・須田 著，計測自動制御
学会，１９７８）に記述されている。

【００４０】θ＝θs および近傍において連続かつ有限
になる近似逆行列は以下のようになる。det Ｊ（θ）＝
ｓｉｎθ_b を考慮して、評価関数をＦ（θ）＝ｓｉｎθ
_b とし、Ｊ（θ）の要素の最大値を考慮して適当なθ_o
＞０を与えてＦ_o ＝Ｆ（θ_o）とすることにより、近似
逆行列は

【数９】 となる。

【００４１】このようにして、特異点およびその近傍以
外では近似逆行列Ｊ^# （θ）の性質と速度制御部１ｂ″
の機能により、先端速度指令動作信号ａとしての速度指
令ベクトルｖ_c と等しい速度ベクトルｖの速度をマニピ
ュレータ先端に実現することができる。特異点およびそ
の近傍では、近似逆行列Ｊ^# （θ）の性質により、マニ
ピュレータ機構２に実現できる範囲内で速度指令ベクト
ルｖ_c を近似した速度を先端に生じさせるべく

【外７】 速度制御部１ｂ″の機能により、速度指令ベクトルｖ_c
をマニピュレータ機構２に実現できる範囲内で近似した
速度ベクトルｖの速度をマニピュレータ先端に実現する
ことができる。

【００４２】（方法例）図６または図７で説明した従来
の運動制御手法に相当する、本発明の方法例としての運
転制御法を図２に示す前記装置例を用いて説明する。速
度制御装置１″は既に説明した図１のように構成される
速度制御装置を用いて構成される。

【００４３】まず、関節角度θに、マニピュレータ機構
２を考慮して事前に決定される位置に関する順キネマテ
ィクス演算を施すことにより、マニピュレータ先端の直
交座標における現在位置Ｘが求められる。目標位置軌道
に基づき入力される位置指令値Ｘ_ref と現在位置Ｘから
位置誤差Ｅが算出される。

【００４４】つぎに位置誤差Ｅに、予め設定されたフィ
ードバック係数Ｋ_p が乗算されることにより、修正速度
指令値ｖ_mod が生成される。位置目標値ｆとしての速度
指令値ｖ_ref が事前に得られないとき、もしくは目標位
置が変化しない定点であるときは、修正速度指令値ｖ
_mod が先端速度指令たる速度指令値ｖ_c 動作信号ａとし
て速度制御装置１″へ入力される。速度制御装置１″
は、前記装置例に基づきマニピュレータ先端を直交座標
で速度制御する。

【００４５】このように位置誤差Ｅに比例した速度を先
端に生じさせることにより、マニピュレータ先端の位置
が目標位置に制御される。この時、位置誤差Ｅは直交座
標系において計算されるため、特異点における計算上の
問題は生じない。さらに速度制御装置１″における直交
座標における速度から関節駆動速度指令ｂ″への変換
は、本方法例で案出した方法が用いられるため、特異点
においてもマニピュレータは安定に制御される。

【００４６】また、マニピュレータの機構および各関節
α〜ζで実現できる速度に応じた適切な位置軌道あるい
は速度で指令値が入力されれば、特異点を通過すること
ができる。そして、稼働範囲以外の位置指令値が与えら
れるなど、実現が困難な指令位置が与えられた場合で
も、その結果得られる位置誤差Ｅに対する速度指令値と
して実現可能な速度指令値しか生成しないため、制御的
に破綻をきたすことなく、実現可能な範囲で位置目標値
ｆに近い適切な位置へ制御されることが期待できる。

【００４７】目標位置軌道に応じた速度目標値ｖ_ref が
事前に得られる時は、速度指令値をｖ_ref に修正速度指
令値ｖ_mod を加算することにより、先端速度指令値ｖ_c
動作信号ａを生成し、これが速度制御装置１″へ速度指
令として入力される。このように速度制御装置１″への
入力である先端速度指令値ｖ_c 動作信号ａが先端に生じ
る速度そのもので先端速度目標値ｉによるフィードフォ
ワード制御を行うことが容易にでき、追従誤差を軽減す
ることができる。

【００４８】

【発明の効果】かくして、本発明によれば、従来技術で
は直交座標における速度制御が困難であった特異点およ
びその近傍においても安定に動作可能な、多関節マニピ
ュレータ先端の直交座標における速度の制御法及び装置
が構成可能になる。また、マニピュレータの機構および
各関節で実現できる速度に応じた、適切な位置軌道ある
いは速度で指令値が入力されれば、特異点を通過するこ
とができる。

【００４９】したがって、直交座標におけるマニピュレ
ータの稼働範囲が飛躍的に広まる。また、位置制御の応
用した場合、特異点に関わらず安定な位置制御系が実現
でき、さらに速度目標値をフィードフォワード的に入力
することにより、複雑な演算することなしに、簡単に軌
道追従制度をあげることができ、マニピュレータ作業を
高度化することができるなど優れた効果を奏する。

【図面の簡単な説明】

【図１】本発明の装置例においてマニピュレータ機構を
制御する速度制御装置の構成ブロック線図である。

【図２】同上の構成システムブロック線図である。

【図３】同上を応用した３自由度の機構例模式図であ
る。

【図４】同上、産業用６自由度多関節ロボット手首機構
を示す斜視図である。

【図５】直交座標における速度を制御する従来手法の一
つである分解動作速度制御を説明する速度制御装置の構
成ブロック線図である。

【図６】分解動作速度制御を用いて位置制御系を構成し
た場合の多関節マニピュレータの運動制御装置を説明す
るシステム構成ブロック線図である。

【図７】従来手法で提案されている、ヤコビ行列の転置
を用いた位置制御系の動作を説明する多関節マニピュレ
ータの運転制御装置のシステム構成ブロック線図であ
る。

【符号の説明】

Ａ…多関節マニピュレータ ａ…先端速度指令動作信号 ｂ，ｂ′，ｂ″…関節駆動速度指令 ｃ，ｃ′，ｃ″…関節角速度 ｄ…関節角度操作量信号 ｅ…先端速度 ｆ…位置目標値 ｇ…位置誤差 ｈ…修正速度指令値 ｉ…先端速度目標値 ｊ…演算先端位置 ｋ…先端位置 ｌ…誤差フィードバック指令値 α〜ζ…関節 １，１′，１″…速度制御装置 １ａ…逆ヤコビ行列演算部 １ａ′…転置ヤコビ行列演算部 １ａ″…近似ヤコビ逆行列演算部 １ｂ，１ｂ′，１ｂ″…速度制御部 １ｃ，１ｃ′，１ｃ″…積分部 ２…マニピュレータ機構 ３，３′…比較指令部 ３ａ，３ａ′…減算器 ３ｂ，３ｂ′…位置誤差フィードバック係数乗算部 ３ｃ…加算部 ４…関節角−先端位置変換部

Claims (5)

【特許請求の範囲】
1. 【請求項１】各関節が独立に速度制御される６自由度多
   関節マニピュレータエンドのエフェクタ先端の、直交座
   標系において、 当該マニピュレータの先端の速度と各角関節角速度の関
   係を表すヤコビ行列の近似逆行列を用い、前記直交座標
   系における速度成分の指令値に当該ヤコビ行列の近似逆
   行列をかけることによって、当該マニピュレータの関節
   角の駆動速度指令値に基づき各関節を駆動することによ
   り、当該マニピュレータ先端の前記直交座標系における
   速度成分の制御を行うに当り、 前記近似逆行列として、当該マニピュレータのヤコビ行
   列の行列式が０となる特異点においてはヤコビ行列の擬
   似逆行列を用い、 当該特異点近傍の点においてはその点におけるヤコビ行
   列の逆行列とその点に最も近い特異点におけるヤコビ行
   列の擬似逆行列とから合成される行列を用い、 前記特異点および特異点近傍以外ではヤコビ行列の逆行
   列を用いること、 を特徴とする多関節マニピュレータの運動制御方法。
2. 【請求項２】先端位置目標値としての位置指令値と，先
   端速度目標値としての速度指令値と，マニピュレータの
   サーボモータに内蔵の角度検出器により得られる現在関
   節角度のフィードバック変換による演算先端位置として
   の現在位置との各データの入力に基づき先端速度指令の
   動作信号としての速度指令値を演算出力する比較指令部
   と，当該先端速度指令動作信号を入力し前記現在関節角
   度の操作量信号を算出する速度制御装置と，当該現在関
   節角度操作量信号により各関節を制御されるマニピュレ
   ータ機構とを順次一連システム化した多関節マニピュレ
   ータの運動制御系において、 前記速度制御装置は、 前記現在関節角度操作量信号に基づき演算したヤコビ行
   列の近似逆行列に、入力した前記速度指令値動作信号を
   乗算して関節角速度指令ベクトルを生成する近似逆ヤコ
   ビ行列演算部と、 当該関節角速度指令ベクトルに基づき、前記各関節に取
   付けられているサーボモータの関節角速度を求める速度
   制御部と、 当該関節角度を積分し前記現在関節角度操作量信号を出
   力する積分部と、 で構成することを特徴とする多関節マニピュレータの運
   動制御装置。
3. 【請求項３】多関節マニピュレータの運動制御系は、閉
   ループのフィードバック制御と開ループのフィードフォ
   ワード制御の組合せからなることを特徴とする請求項２
   記載の多関節マニピュレータの運動制御装置。
4. 【請求項４】現在関節角度の現在位置への変換は、フィ
   ードバック系中に関節角−先端位置変換部を挿入介接す
   ることを特徴とする請求項２又は３記載の多関節マニピ
   ュレータの運動制御装置。
5. 【請求項５】比較指令部は、 位置指令値と現在位置との比較位置誤差を算出する減算
   器と、 当該位置誤差にフィードバック係数を乗算して修正速度
   指令値を求める位置誤差フィードバック係数乗算部と、 当該修正速度指令値に速度指令値を加算して先端速度指
   令値動作信号を演算出力する加算器と、 から構成されることを特徴とする請求項２，３又は４記
   載の多関節マニピュレータの運動制御装置。