JPH0569354A

JPH0569354A - 多関節リンク機構制御装置

Info

Publication number: JPH0569354A
Application number: JP23150291A
Authority: JP
Inventors: Tomoyuki Hamada; 朋之浜田; Takeshi Shibuya; 竹志渋谷
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 1991-09-11
Filing date: 1991-09-11
Publication date: 1993-03-23

Abstract

(57)【要約】【目的】多関節リンク機構の運動制御装置に冗長自由度
が存在しても計算可能で、特異点の近傍でも計算が破綻
せず、解の連続性が保たれる汎用的制御装置を提供す
る。【構成】関節とリンク機構の先端位置を含む平面より目
標位置が上にある場合に関節角度を正の方向に修正し、
下にある場合に負の方向に修正する複数の演算装置１，
２，３と、関節角度と座標変換行列を記憶する共有メモ
リ装置１０を設ける。【効果】汎用的な一つの演算手順により冗長自由度を持
つものも含めて様々なリンク機構の制御を行える。ま
た、特異姿勢の近くでも連続的な機構制御を実現でき
る。さらに、並列計算により高速に制御データを計算で
きる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は複数の関節をもつリンク
機構の運動の制御装置に関する。

【０００２】

【従来の技術】多関節リンク機構の制御では、リンク機
構の先端の位置・姿勢が与えられ、それを実現する関節
角度を計算し、これを制御機構に与えることにより運動
の制御を行うことが多い。この先端位置・姿勢から関節
角度を計算する逆変換計算には従来以下のような方法が
とられている。

【０００３】第一の方法は、関節角度と先端位置の関係
を与える連立方程式を直接解いて、角度を求める方法で
ある。

【０００４】第二の方法は、この連立方程式をヤコビア
ンにより局所線形近似し、ある初期値から繰返し計算に
より再帰的に解を求める方法である。

【０００５】また、リンク機構先端の位置・姿勢は六個
のパラメータにより表現される。よってリンク機構の自
由度（独立な関節の数）が七以上存在する場合（冗長自
由度がある場合）には、一般に解を一意に決定すること
はできない。従って、運動エネルギの最小化など何らか
の拘束条件を導入し、疑似的に自由度を落すことにより
計算を行っている。

【０００６】

【発明が解決しようとする課題】第一の方法では、連立
方程式は多くの三角関数を含む非線形な方程式となるの
で、解を解析的に求めるのは困難であることが多い。解
が求まる場合についても機構の軸構成に依存した計算手
順となるので汎用性が確保されない。

【０００７】第二の方法では、ヤコビアンが正則である
ことが条件となる。ヤコビアンが正則でなくなる特異姿
勢の近傍では、数値計算が破綻しやすく、解が不連続と
なることが多い。これが連続的な機構制御の障害となっ
ている。

【０００８】冗長自由度がある場合には、導入する拘束
条件をうまく選ぶ必要がある。

【０００９】本発明の目的は、冗長自由度が存在する場
合にも計算可能で、特異点の近傍でも計算が破綻せず、
解の連続性が保たれた制御装置を提供することにある。

【００１０】

【課題を解決するための手段】上記目的は、各関節ごと
に独立した角度の増減判定規範を与え、協調的に解を計
算する関節角修正量計算手段により達成される。

【００１１】即ち、各関節ｉに関してある時刻ｔにおけ
る関節角度をθ_i(t)、先端位置を目標位置により近付け
るθ_iの修正量をΔθ_i(t) とするとき、時刻ｔ＋１にお
ける関節角度θ_i(t+1)を

【００１２】

【数１】

【００１３】により与える。この操作を各関節ごとに独
立して繰返し行うことにより先端位置を目標位置に近付
ける。

【００１４】今、ベクトルｚ_i，ｒ_i，ｘ_iを図３に示す
ようにそれぞれ第ｉ番目の関節Ｊ_iの回転軸方向単位ベ
クトル、Ｊ_iから先端位置Ｒへのベクトル、Ｊ_iから目標
位置Ｘへのベクトルとすると、Δθ_i(t) は、例えばΔ
θ_iは以下のように決めればよい。

【００１５】

【数２】

【００１６】但し、α_iは正の修正量、|（z_i r_i x_i）|
は、ベクトルｚ_i，ｒ_i，ｘ_iを列とする３×３行列の行
列式である。

【００１７】

【作用】数２に示した行列式は

【００１８】

【数３】

【００１９】のように変形できる。これは図３に示すよ
うに関節の回転軸ｚ_iと先端位置Ｒにより決まる平面Ｓ
_riに対して、目標位置Ｘが上（表側）にある場合には正
となり、下（裏側）にある場合には負となる。従って、
数１のΔθ_iは、目標位置がＳ_riより上にある場合には
θ_i を増加し、下（裏側）にある場合には減少させる操
作を各関節について独立に行いながら先端を目標位置に
近づけていく。

【００２０】ベクトルｚ_i，ｒ_i，ｘ_iは、時刻ｔにおけ
る各関節の角度θ(t)と目標位置Ｘが与えられれば計算
できるので

【００２１】

【数４】

【００２２】と表現できる。但し、

【００２３】

【数５】

【００２４】とする。以後数４を関節制御関数、数５を
符号判定条件と呼ぶことにする。数４の構造は、制御入
力である関節修正量が状態量である関節角度と先端位置
により陽に表現される形になっている。よって、計算手
順が単純で様々なタイプの制御特性を容易に組み込むこ
とができる。また、陽な表現形式であるため、ヤコビア
ンを用いた場合のような逆行列計算が必要ないので特異
点の近傍でも計算が破綻せず、解の連続性が保たれる。

【００２５】数４の構造より関節の修正量決定手順を関
節ごとに独立に与えることができる。よって、冗長自由
度が存在する場合にも計算可能である。

【００２６】数２にはリンク機構の軸構成固有の計算手
順は含まれていないので、本発明は任意の軸構成のリン
ク機構に対して適用可能な汎用性を持っている。

【００２７】

【実施例】関節制御関数の数４は、単に時刻ｔにおける
関節角度θ(ｔ)と目標位置Ｘ(ｔ)から関節角修正量Δθ
_i を決めることを指定するだけで、関数ｆの形としては
符号判定条件数５が指定する条件を満たす関数であれば
どのような形であっても良い。本発明の実施例として、
ｆが可変ゲイン型フィードバックタイプの制御を与える
式である場合を以下に示す。

【００２８】まず、関節制御関数として

【００２９】

【数６】

【００３０】なる式を考える。ここでθ_eiは目標位置に
対する先端位置のずれ量を関節角におけるずれ量に換算
した値、ｋ_i(χ) はフィードバックゲインであり、フィ
ードバックゲインは関節感度χ_iの関数となっている。
θ_ei、ｋ_i(χ_i)、χ_iはそれぞれ以下のように計算され
る。

【００３１】今、ベクトルｎ_ri、ｎ_xiとして以下のもの
を考えると

【００３２】

【数７】

【００３３】

【数８】

【００３４】ｎ_riは、図３において回転軸ｚ_i と先端位
置Ｒにより決定される平面Ｓ_riの単位法線ベクトルとな
り、ｎ_xi は、図３において回転軸ｚ_iと目標位置Ｘによ
り決定される平面Ｓ_xiの単位法線ベクトルとなる。これ
より、先端位置のずれ量の関節角度換算値θ_eiは以下の
式で与えられる。

【００３５】

【数９】

【００３６】但し、atan2(x,y)は、x＝sinθ，y＝cosθ
なるθを与える関数である。また、ｎ_ri＝０、またはｎ
_xi＝０の場合にはθ_ei＝０とする。θ_eiは符号判定条件
(５)を満たす関数である。

【００３７】次に、関節感度χ_iは以下のように定義さ
れる。

【００３８】図３において、関節ｉから先端位置Ｘへの
ベクトルｘ_iが回転軸ｚ_iとほとんど平行である場合に
は、関節ｉの回転は先端位置を目標位置へ近付ける動き
にほとんど寄与しない。その一方で、ｘ_i のわずかな変
位に対して関節ｉが大きく変化し、不必要な回転運動が
生じることがある。そこで、先端位置の動きに対する各
関節の寄与の程度として関節感度を定義し、これにより
フィードバックゲインを調整する。

【００３９】関節感度χ_iは以下の式で与えられる。

【００４０】

【数１０】

【００４１】ここで、ベクトルｚ_iとｘ_iのなす角をφと
すると||ｚ_i||＝１であるので、

【００４２】

【数１１】

【００４３】即ち、関節感度はsinφに他ならない。

【００４４】このχ_iを用いることにより、フィードバ
ックゲインｋ_i(χ_i)は

【００４５】

【数１２】

【００４６】により計算する。但し、ｋ_i0は標準ゲイン
で正の定数である。ｋ_i(χ_i)は非負の値をとる。

【００４７】以上により、数５は具体的に

【００４８】

【数１３】

【００４９】の形で与えられる。

【００５０】次にｎ_ri，ｎ_xi等のベクトルを計算するた
めに必要なｚ_i，ｒ_i，ｘ_iの計算手順を示す。これらは
以下の式により与えられる。

【００５１】

【数１４】

【００５２】

【数１５】

【００５３】

【数１６】

【００５４】ここで、Ｒ_iはリンク機構の根元から関節
Ｊ_iまでのベクトルであり、Ｒ_n+1 は根元から先端まで
のベクトルとする。ｅ_zはワールド座標系Ｆ_Wのｚ軸方向
単位ベクトルである。Ｂ_i は関節位置を原点として、関
節の回転軸方向をｚ軸とする関節座標系Ｆ_iとワールド
座標系Ｆ_Wとの間の回転変換行列である。Ｆ_WとＦ_iの間
の座標変換マトリックスをＡ_iとすると、Ｒ_iはＡ_i の右
端列ベクトルの上三要素で与えられ、Ｂ_iはＡ_iの左上３
×３部分行列で与えられる。即ち、

【００５５】

【数１７】

【００５６】

【数１８】

【００５７】となる。

【００５８】またＦ_iＦ_i+1への座標変換マトリックスを
Ｔ_iとすると、Ｔ_iは関節角θ_i の関数となり

【００５９】

【数１９】

【００６０】なる関係がある。

【００６１】以上の関係を整理するとθ_i(ｔ)からθ
_i(ｔ＋１)を計算する具体的な手順は以下のように示さ
れる。

【００６２】

【数２０】

【００６３】

【数２１】

【００６４】即ち、時刻ｔにおける関節角度θ(ｔ)を用
いてＡ_i(ｔ)（i＝1...n)を計算し、Ａ_i(ｔ)とＡ_n+1(ｔ)
及びＸ(ｔ)を用いて各関節制御関数ｆ_i を計算する。計
算手順をチャートにまとめたものを図４に示す。図中、
４０１は、時刻ｔのデータ、４０２は時刻ｔ＋１のデー
タ、４０３はデータを参照するタイミング、４０４はデ
ータが書き換わるタイミングを示す。

【００６５】この計算手順による一ステップ当たりの計
算時間Δｔは、ｔ_f を数２０の計算に要する時間、ｔ_A
を数２１の計算に要する時間とすると

【００６６】

【数２２】

【００６７】となる。Δｔは関節数ｎに比例して長くな
る。

【００６８】今、Δｔを十分小さく取ることができると
すると、Ａ_i(ｔ)とＡ_i(ｔ＋１)はほとんど等しいとみな
すことができるので、数２１は

【００６９】

【数２３】

【００７０】と変形できる。これにより計算手順は図５
に示すようになり、計算時間Δｔは

【００７１】

【数２４】

【００７２】となり、ｎにかかわらず一定とすることが
できる。

【００７３】以上の計算手順を実行する装置の構成を図
１に示す。

【００７４】図において１，２，３は時刻ｔにおける関
節角度θ_i(ｔ) ，目標位置Ｘ(ｔ)，座標変換マトリクス
Ａ_i(ｔ)，Ａ_n+1(ｔ)より数２０，数２３に従ってＡ
_i+1(ｔ)と次の時刻ｔ＋１における関節角度θ_i(ｔ＋１)
を計算する演算装置である。この演算装置は制御対象
である多関節リンク機構の関節の数だけ用意する。１０
は各計算ステップにおけるリンク機構の情報を記憶する
ための共有記憶装置である。その内部は記憶される情報
に応じて領域が分けられている。４，６，８はθ_i(ｔ)
とＸ(ｔ)よりベクトルｚ_i，ｒ_i，ｘ_iを得るための座標
変換行列の時刻ｔにおける値Ａ_i(ｔ) を記憶するための
記憶領域である。５，７，９は時刻ｔにおける関節角度
θ_i(ｔ) を記憶するための領域である。

【００７５】演算装置１，２，３において実行される演
算手順を図２に示す。

【００７６】今、演算装置２がｉ番目の関節角を制御す
る演算装置であるとすると、２はまず８に記憶されてい
るＡ_i(ｔ)より数１６に従ってｚ_iを計算し（ステップ
１）、４に記憶されているＡ_n+1(ｔ)と８に記憶されて
いるＡ_i(ｔ)より数１４に従ってｒ_iを計算し（ステップ
２）、入力されたＸ(ｔ)とＡ_i(ｔ)より数１５に従って
ｘ_iを計算する（ステップ３）。次に、ｚ_iとｘ_iより数
１０に従ってχ_iを計算し（ステップ４）、ｘ_i，ｒ_i，
ｚ_iより数９に従ってθ_eiを計算する(ステップ５）。そ
して、χ_i，θ_eiより数６に従って関節制御量、即ち、
Δθ_iを計算し（ステップ６）、これを７に記憶されて
いる現在の関節角度θ_i(ｔ) に加えることにより、次の
目標関節角度θ_i(ｔ＋１) を得て、これを７に書き込む
（ステップ７）。また、２は７のθ_i(ｔ)と８のＡ_i(ｔ)
より数１９に従ってＡ_i+1(ｔ) を計算し、これを６に書
き込む（ステップ８）。

【００７７】以上の手順により関節角の制御動作が実行
される。

【００７８】次に本発明を用いた第二の実施例について
説明する。

【００７９】第一の実施例においては、制御装置には一
つの目標位置Ｘ(ｔ)が与えられたが、第二の実施例にお
いては図６に示すように、複数の目標位置Ｘ_j(ｔ)(j=
1...m)を用意し、複数の関節Ｊ_cjの位置を制御する装置
を示す。

【００８０】この場合の関節制御関数は以下の式で与え
られる。

【００８１】

【数２５】

【００８２】ここで、ａ_ijは正の重み付け定数で、ｆ_ij
（θ，Ｘ_j）は、ｃ_j番目の関節Ｊ_cjを先端位置とし、Ｘ
_jを目標位置とした場合の関節制御関数であり、

【００８３】

【数２６】

【００８４】とする。また、Ｊ_cmはリンク機構の先端と
する。

【００８５】これにより、θ_i(ｔ)からθ_i(ｔ＋１)を計
算する具体的な手順数２０は以下のように修正される。

【００８６】

【数２７】

【００８７】図７に制御装置の構成を示す。図におい
て、４から１０までは図１と同様である。５０１,５０
２,５０３は時刻ｔにおける関節角度θ_i(ｔ),目標位置
Ｘ_j(ｔ)（j=1...m），座標変換マトリクスＡ_i(ｔ)，Ａ
_n+1(ｔ) より数２７，数２３に従ってＡ_i+1(ｔ)と次の
ステップｔ＋１における関節角度θ_i(ｔ＋１)を計算す
る演算装置である。

【００８８】演算装置５０１，５０２，５０３において
実行される演算手順を図８に示す。

【００８９】今、演算装置５０２がｉ番目の関節角を制
御する演算装置であるとすると、５０２はまず８に記憶
されているＡ_i(ｔ)より数１６に従ってｚ_iを計算する
（ステップ１）。次にj＝1...mに関して、４に記憶され
ているＡ_n+1(ｔ)とＡ_i(ｔ)より数１４に従ってｒ_iを計
算し（ステップ２）、入力されたＸ_j(ｔ)とＡ_i(ｔ) よ
りｘ_ijを計算し（ステップ３）、ｚ_iとｘ_ijよりχ_ij を
計算し（ステップ４）、ｘ_ij，ｒ_i，ｚ_iよりθ_eijを計
算する（ステップ５）。そして、χ_ij，θ_eijよりΔθ_i
を計算し(ステップ６)、これを７に記憶されている現在
の関節角度θ_i(ｔ)に加えることにより、次の目標関節
角度θ_i(ｔ＋１) を得て、これを７に書き込む（ステッ
プ７）。また、５０２は７のθ_i(ｔ)と８のＡ_i(ｔ)より
数１９に従ってＡ_i+1(ｔ)を計算し、これを６に書き込
む（ステップ８）。

【００９０】以上の手順により複数の目標位置に対する
関節角の制御動作が実行される。

【００９１】以上の実施例では、関節制御関数として、
数６に示したような関節角ずれ量にフィードバックゲイ
ンを掛けた形のものを示したが、数６の構造としては数
５を満たす限り様々なタイプのものを用いることができ
る。

【００９２】以上の実施例で演算装置１，２，３（ある
いは５０１，５０２，５０３）は、独立なものであると
したが、これらの演算装置で行われる演算は一つの演算
装置で行っても良い。但し、その場合高速な演算速度は
望めない。座標変換行列を記憶する領域４，６，８は演
算装置の内部に備えられた記憶領域であっても良い。関
節角度を記憶する領域５，７，９は関節角度検出装置と
関節駆動装置であっても良い。この場合、記憶領域に値
を書き込む操作は関節駆動装置に指令を与えることに対
応し、記憶領域から値を読む操作は関節角度検出装置か
ら値を読み出すことに対応する。

【００９３】この実施例で、先端位置と関節位置を表す
ベクトルＲ_i は演算装置により計算したが、先端位置と
関節位置を検出する位置検出装置によりこれらの値を得
ても良い。

【００９４】次に第三の実施例について図９を用いて説
明する。

【００９５】図において２０１，２０２，２０３は演算
装置、２０４，２０５，２０６は関節角度検出装置、２
０７，２０８，２０９は位置検出装置、１０は多関節リ
ンク機構である。図１０に関節と関節角度検出装置と位
置検出装置の接続関係、及び回転方向の関係を示す。ｉ
番目のリンクを１０１とすると、関節Ｊ_i(１１１）に取
り付けられた位置検出装置Ｓ_i（７）はｉ−１番目のリ
ンクＬ_i-1（１０２）に固定されている。位置検出装置
はリンク機構の先端位置４０１と目標位置４０２を関節
Ｊ_i の回転軸を中心とした円筒上の位置３０１，３０２
として検出し、図１１に示すように円筒上の先端位置３
０１が目標位置３０２に対して右回転の方向にある場合
には＋１、左回転の方向にある場合には−１を出力す
る。

【００９６】演算装置は位置検出装置の出力値が＋１の
場合には関節角度検出装置より検出した角度を微小量Δ
θだけ増加した値をリンク機構のアクチエータに出力す
る。位置検出装置の出力値が−１の場合には関節角度検
出装置より検出した角度を微小量Δθだけ減じた値をリ
ンク機構のアクチエータに出力する。これらの機構が連
続的に動作することによりリンク機構の先端位置を常に
目標位置に近付けることができる。本実施例では、リン
ク機構のアクチエータが一部故障していても、残りの関
節による先端位置の自由度が許す範囲で、正常に動作す
ることができる。

【００９７】

【発明の効果】本発明による制御装置を用いれば、機構
の軸構成に依存せず汎用的な一つの演算手順により様々
なタイプのリンク機構の制御を行える。また、解を解析
的に求めるのが困難な軸構成のリンク機構に対しても制
御を行うことができる。

【図面の簡単な説明】

【図１】本発明の一実施例を示すブロック図である。

【図２】演算装置の動作の説明図。

【図３】本発明の動作原理の説明図。

【図４】制御量の計算手順チャート。

【図５】制御量の改良型計算手順チャート。

【図６】本発明の第二の実施例の説明図。

【図７】本発明の第二の実施例を示すブロック図。

【図８】本発明の第二の実施例の演算装置の動作の説明
図。

【図９】本発明の第三の実施例を示すブロック図。

【図１０】本発明の第三の実施例における関節付近の構
造を示す斜視図。

【図１１】本発明の第三の実施例における位置検出装置
の説明図。

【符号の説明】

１，２，３…演算装置、４，６，８…座標変換行列を記
憶する領域、５，７，９…関節角度を記憶する領域、１
０…共有メモリ装置。

Claims

【特許請求の範囲】

【請求項１】関節に対応した複数の演算装置と、現在の
前記関節の角度と座標変換行列を記憶する共有メモリ装
置とからなり、前記演算装置は各関節に対して共有メモ
リに記憶された現在の前記関節角度と関節間の座標変換
行列と目標位置とを入力とし、前記演算装置群により計
算した先端位置と目標位置の差が小さくなる方向に関節
角度を修正した値を出力し、それに応じて変化する新た
な座標変換行列を出力することを特徴とする多関節リン
ク機構制御装置。