JPH0816845B2

JPH0816845B2 - 多関節形アームロボットの軌跡制御方法

Info

Publication number: JPH0816845B2
Application number: JP2325781A
Authority: JP
Inventors: 祐次郎清水; 哲司林
Original assignee: 工業技術院長
Priority date: 1990-11-29
Filing date: 1990-11-29
Publication date: 1996-02-21
Anticipated expiration: 2011-02-21
Also published as: JPH04195403A

Description

【発明の詳細な説明】〈産業上の利用分野〉本発明は、原子力プラント内の点検補修ロボット等と
して使用される多関節形アームロボットの軌跡制御方法
に関する。

〈従来の技術〉従来、冗長度を有する多関節形アームロボットの軌跡
制御を行うには、非正則なヤコビ行列を数値解法で解か
なければならず、これをリアルタイムで実行するには高
価な計算機が必要であった。

そこで、煩雑な数値解法を省略する方法としてヤコビ
行列の冗長度を仮想的に殺して正則行列化して求める方
法が知られている。

その一例について、第２図、第３図、第５図、第６図
を参照して説明する。

第２図、第３図は先端にハンドを有する３自由度の平
面アームロボットの一例を示すものである。両図に示す
ように、三つの関節角θ₁,θ₂,θ_３を制御することによ
り、ｘ−ｙ平面内におけるアーム先端位置、つまりハン
ドの位置を任意に移動させることができる。しかし、平
面上で、ハンドの位置のみを制御する場合には、２自由
度あれば充分であり、１軸冗長である。

ここで、ハンドの位置、角関節角は次の（１）（２）
式のように表すものとする。

この様に表すと、関節角から、ハンドの位置は解析的に次の（３）式のように求められる。

（３）式の両辺を時間で微分して速度の関係式として
示すと、次の（４）式の様に表される。

ここで、はヤコビ行列であり、３軸の場合、次の（５）式のよう
に表示される。

一方、制御装置としては、第５図に示すように、操縦
桿20、各関節角速度演算部30及びサーボ機構40とから構
成されている。この各関節角速度演算部30はヤコビ行列
発生器31、小行列式演算機32及び逆ヤコビ演算器33とか
ら構成されている。

ところで、上記制御装置で実際に制御できるのは、多
関節形アームロボットの関節角或いは関節角速度であるから、上記（５）式を関節角速度について解く必要がある。つまり、ハンド速度から関節角速度への変換をしなければならない。

もし、上記（５）式ヤコビ行列が正則行列、則ち冗長
度がない行列であれば、であるから、が求められ、（６）式のように関節角速度が求まる。

しかし、冗長度がある場合は、ヤコビ行列は２×３で
あり、正則ではない。

また、が正則でなくとも、疑似逆行列を用いれば、上記（６）式の形に解けるが、疑似逆行列
を求めるのは計算量が膨大となり問題である。

そこで、から冗長列を一時的に取り除いて正則化した行列をとする。

例えば、（５）式のであれば、一列が冗長であるので、次の（７）（８）
（９）式に示すの三つの正則行列が定義できる。但し、ｓ＝１〜３とする。

このを用いれば、の条件のもとで、次の（10）（11）（12）式に示すよう
に関節角速度について解くことが出来る。

但し、除かれた冗長項に対する関節角速度_Ｓは０と
する。

この場合、どのを用いるかは、その行列式の絶対値の最も大きなものを使用するとすれば、の特異点を回避できる。

そこで、このような導出過程を基にして、従来の制御
装置では多関節形アームロボットを制御するようにして
いる。

即ち、オペレータ10が操縦桿20を操作してハンド速度が指令値として制御装置に与えられると、各関節角速度
演算部30では、第６図に示すフローチャートに従って、
関節角速度を演算する。

先ず、ヤコビ行列演算器31は各サンプリング時点のハ
ンド速度とから、ヤコビ行列式を演算する。

次いで、小行列式演算器32はヤコビ行列式から冗長列を取り除いた正則行列と、その行列式を計算する。

引き続き、逆ヤコビ演算器33は正則行列のうち、その行列式の絶対値の最も大きなものを最適正則行列J_maxとし、その逆行列
J_max ^-1を計算する。

そして、より、各関節角速度_１〜_３を求める。

尚、除かれた関節角速度_Ｓは０とする。

このように演算された各関節角速度_１〜_３が目標
値としてサーボ機構40に出力され、多関節形アームロボ
ットが制御される。

〈発明が解決しようとする課題〉上述した従来の制御方法では、多関節形アームロボッ
トの冗長度を仮想的に殺して正則化しているので、非正
則なヤコビ行列を数値解法する必要がなく、高価な計算
機が不要であるが、しかし、冗長度を殺しているので多
関節形アームロボットの姿勢を好ましい方向に変えてい
くことは出来なかった。

そこで、冗長度を生かした制御を行うには、疑似逆行
列を用いた場合、次の（13）式に基礎を置くことができ
る。

ここで、ベクトルに任意に取られるので、例えば、多関節形アームロボッ
トの姿勢が好ましい方向に動くようにベクトルをコントロールすれば、目的とする軌跡を保ちつつ、冗
長度を利用して多関節形アームロボットの姿勢を変える
ことが出来る。

ところが、この場合も、疑似逆行列を求める演算が必
要となり、その演算量が膨大となる欠点がある。

このように従来では、非正則なヤコビ行列から冗長度
を仮想的に除いて正則化した行列を利用して制御する
と、冗長度を生かした制御が出来ず、また、冗長度を生
かした制御をするために疑似逆行列を用いた演算を採用
すると、演算量が膨大となり、高価な計算機が必要にな
っていた。

本発明は、上記従来技術に鑑みてなされたものであ
り、高価な計算機を必要とせず、しかも、冗長度を生か
して多関節形アームロボットの軌跡を制御できる方法を
提供することを目的とするものである。

〈課題を解決するための手段〉斯かる目的を達成する本発明の構成は１軸の冗長度を
有するｎ軸の多関節形アームロボットの軌跡制御におい
て、冗長度を含めたヤコビ行列を求めると共に該ヤコビ行列から第ｓ軸（ｓ＝１〜ｎ）
を冗長として除いたｎ個の正則行列を求めて、これらの正則行列のうちから、行列式の絶対値の最も大きなものを最適正則行列J_maxとして選び、その
逆行列J_max ^-1をハンド速度に掛けて主関節角速度を求め、更に正則行列の行列式から定義される冗長速度に任意の定数ｋを掛けて前記主関節角速度に加えて関節角目標速度として出力することを特徴とする。

〈作用〉本発明では、冗長度を生かした制御を行うために前述
した（13）式を利用するのであるが、疑似逆行列に冗長自由度分の０ベクトルを加えたを代入する。

例えば、前述した（７）〜（９）式のに対しては次のように求められる。

従って、J_S′^-1は次式のように定義される。

但し、上記（17）（18）（19）式は、第１行、第２
行、第３行に０ベクトルをそれぞれ付加したものであ
る。

このに代えて、前述した（13）式、に代入すると、次のよう
になる。

さて、は第ｓ軸変数_Ｓに対する行ベクトルを０ベクトルとし
たものであるがこの_Ｓを（ｎ＋１）行い、についても_Ｓに関する第ｓ列の列ベクトルを（ｎ＋
１）に移動させても、一般性は失わない。

例えば、但し、はｎ次元の列ベクトル、はｎ次元の行ベクトル、det（ｎ＋１）はの行列式の値である。

もともと、の逆行列であり、次のように変形される。

ここで、（ｉ＝１〜ｎ）は、行列から第ｉ列を除いたに対し、第ｉ列にを入れた行列の行列式の値である。則ち、の列に関して余因子行列の逆行列を行に関して展開すると各余因子はJ_n+1のときにについて展開するのと同じになる（各余因子b_ikはｉ列
要素とｋ行要素（ｋ＝１〜ｎ）を除いた小行列式から求
められる。つまり、の情報は使わず、J_i′とJ_n+1の共通要素から求められ
る。

よって最後の変形は、行列式の次に入れ換えることを意味し、この結果、符号がｎ−
ｉ回変り、J_iと同じ表現を得る。

従って、よって、は次のようになる。

このように、ｓ＝ｎ＋１として上記式を導いたが、ｓ
を１〜ｎとしても、ほぼ同様な結果となる。平面３軸の
場合は、次のようになる。

以上から、を用いた場合、の定数倍を速度ベクトルをに加えても、出力であるは同一である。

即ち、の定数倍として、ロボット姿勢が好ましい姿勢をとるよ
うに、に加算することが出来る。

この定数の決め方は、何らかの評価関数を定義して決めれば良い。例えば、として、が大きい方向となるようにすれば、特異点を回避し、且
つ、ロボットの姿勢が好ましい方向に動くことになる。

例えば、３軸の場合、（33）〜（35）式のうち、detJ
₁〜detJ₃の絶対値が最も大きいものの式を用いてを求めるようにすると良い。

また、定数c_iは関節角速度のリミット値を越えず、且
つが増加する方向に符号をとると良い。

〈実施例〉以下、本発明について、図面に示す実施例を参照して
詳細に説明する。

第１図に本発明の一実施例に使用する制御装置を示
す。同図に示す実施例は、第２図に示す３自由度の平面
アームロボットの制御に示すものである。同図に示すよ
うに、平面上でハンドの位置を制御する場合、２自由度
であれば充分であり、１軸冗長である。

ここで、制御装置は、ハンド指令速度指令操縦桿20
と、各関節角速度演算部30と、サーボ機構40とから構成
されている。各関節角速度演算部30は、ヤコビ行列演算
部31、小行列式演算部32、逆ヤコビ演算部33、冗長速度
演算部34及び関節目標速度演算部35とから構成されてい
る。

従って、オペレータ10が操縦桿20を操作してハンド速
度を指令値として制御装置に与えると、各関節角速度演算
部30では、第６図に示すフローチャートに従って関節角
目標速度を演算する。

次いで、小行列式演算器32はヤコビ行列式から冗長列を取り除いた正則行列の行列式を計算する。

引き続き、逆ヤコビ演算器33はの絶対値のうち最も大きなものを最適正則行列J_maxと
し、その逆行列J_max ^-1を計算する。

そして、より、主関節角速度を求める。

尚、除かれた関節角速度は０とする。

更に、本発明では、冗長速度演算器34が、前述した
（36）式に示す冗長速度を求め、この冗長速度の任意の定数ｋを掛けて速度ベクトルを演算する。

定数ｋの正負と大きさは、評価関数により求める。

引き続き、関節目標速度演算器35は、この速度ベクト
ルを加えて、関節角目標速度を演算する。

但し、関節角目標速度が速度リミット値を越えないように調整する。

このように演算された関節角目標速度が目標値としてサーボ機構40に出力され、多関節形アー
ムロボットが制御される。

このように上記実施例では冗長速度に任意の定数ｋを掛けて、関節角速度に加えているので、この任意の定数ｋを調整することに
より、目的とする軌跡を得ながら冗長度を生かして、多
関節形アームロボットの姿勢を好ましい方向に変えるこ
とが可能である。

また、疑似逆行列を使用しないので、演算量が膨大と
なることはない。

尚、本発明では、冗長度が１の場合に限っているが、
これは、冗長度としては、実用上１で充分であり、２以
上の冗長度を持つロボットは、その構造、強度上から作
られることは殆ど無いためである。

従って、ヤコビ行列は、制御入力をｎ次元とすると、（ｎ＋１）軸のロボッ
トに対するものとなり、その行列のサイズはｎ×（ｎ＋
１）となる。即ち、平面３軸の例で言えば、２×３のサ
イズの行列となる。

〈発明の効果〉以上、実施例を参照して具体的に説明したように、本
発明は冗長度を有する多関節形アームロボットの制御に
おいて、全て予め数式解が求められた等を制御に用いるので、擬似逆行列を使用する数値解法
に比較し、演算量が少なく、高価な計算機が不要とな
る。また、冗長度を生かした制御ができるので、特異点
回避がリアルタイムで可能となり、操作性、制御性が格
段に向上する。

【図面の簡単な説明】

第１図は本発明の一実施例に使用する制御装置の構成
図、第２図は本発明が適用される多関節形アームロボッ
トの一例を示す外観図、第３図は第２図に示す多関節形
アームロボットをモデル化して示す説明図、第４図は第
１図に示す制御装置の各軸速度指令演算部の演算過程を
示すフローチャート、第５図は従来の制御装置の構成
図、第６図は従来の制御装置の各軸速度指令演算部の演
算過程を示すフローチャートである。図面中、 10はオペレータ、 20は操縦桿、 30は各関節角速度演算部、 31はヤコビ行列演算部、 32は小行列式演算部、 33は逆ヤコビ演算部、 34は冗長速度演算部、 35は関節目標速度演算部、 40はサーボ機構である。

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁶ 識別記号庁内整理番号ＦＩ技術表示箇所Ｇ０５Ｂ 19/416 Ｇ０５Ｂ 19/403 Ｐ

Claims

【特許請求の範囲】

【請求項１】１軸の冗長度を有する多関節形アームロボ
ットの軌跡制御において、冗長度を含めたヤコビ行列を
求めると共に該ヤコビ行列から冗長度を除去した正則行
列を全て求め、これらの正則行列のうちから、行列式の
絶対値の最も大きなものを最適正則行列として選び、該
最適正則行列の逆行列をハンド速度に掛けて主関節角速
度を演算し、更に正則行列の行列式から定義される冗長
速度に任意の定数を掛けて前記主関節角速度に加えて関
節角目標速度として出力することを特徴とする多関節形
アームロボットの軌跡制御方法。