JPH035605B2 - - Google Patents

Description

【発明の詳細な説明】 (1) 発明の技術分野 本発明は、多関節型ロボツトの軌跡制御方式に
係り、特に、６自由度多関節型ロボツトを直交座
標系でスムーズに移動させる軌跡制御方式に関す
る。

(2) 技術の背景 汎用ロボツトの研究は、人間に似た動作をいか
に自動的に機械の手にやらせるかということに向
けられている。したがつて、汎用ロボツトの腕
は、コンピユータで制御され、多関節を有するも
のである。いわゆる多関節型ロボツトでは、どの
関節をどれだけ動かしたら手先がどこの位置まで
くるかを直観的に知ることは容易ではない。した
がつて、各関節の回転量を知つて、腕や手先の各
部の位置をコンピユータにより、ただちに、すな
わちリアルタイムに、計算できなくてはならず、
一般にマトリツクスを用いた方法、すなわち座標
変換行列を用いて計算する方法がとられる。

各関節がすべて回転の自由度で構成される６自
由度多関節型ロボツトを直交座標系で移動させる
場合の問題点を第１図の構成のロボツトを例に説
明する。なお、ロボツトの軌跡制御で使用される
座標変換演算に関しては、例えば、「人工の手の
運動制御に関する研究」日本機械学論文集（Ｃ
編）、45巻391号（昭和54.3）P314〜P322、に詳
しい。

第１図ａに示す関節角度ベクトルΘ＝（θ₁、θ₂、
θ₃、θ₄、θ₅、θ₆）を与えて、第１図ｂに示す手先
の位置・姿勢ベクトルＰ＝（ｘ、ｙ、ｚ、α、β、
γ）を求めるには、各関節に固定された座標系間
のトータルな座標変換行列を用いる。

第１図ａ，ｂのO₀−x₀y₀z₀はロボツトに対して
設定された絶対固定座標系、O_i−X_iY_iZ_i（ｉ＝１
〜５）は各関節に固定された座標系、O_h−x_hy_hz_h
は手先に固定された座標系である。

O_h−x_hy_hz_hからO₀−x₀y₀z₀への座標変換行列Ａ
は Ａ＝A₁A₂A₃A₄A₅A_h ………(1) と計算される。。ここでA_i（ｉ＝１〜５及びｉ＝
ｈ）はO_i−X_iY_iZ_iからO_i-1−X_i-1Y_i-1Z_i-1への変換
行列（ただし、A_hはO_h−x_hy_hz_hからO₅−x₅y₅z₅
への変換行列）であり、O_i−x_iy_iz_iでのある点の
座標値をＸ→ｉとすると X^i-1 １＝A_iXⁱ １ ………(2) より定義され A₁＝cosθ₁ sinθ₁ ０ ０−sinθ₁ cosθ₁ ０ ００ ０ １ ００ ０ l₁ １ A₂＝cosθ₁ ０ −sinθ₂ ００ １ ０ ０sinθ₂ cosθ₂ ０l₂・cosθ₂ ０ −l₂・sinθ₂ １ A₃＝θ₃ ０ −sinθ₃ １０ １ ０ ０sinθ₃ ０ cosθ₃ ０l₃・cosθ₃ ０ −l₃・sinθ₃ １ A₄＝１ ０ ０ ００ cosθ₄ sinθ₄ ００ −sin₄ cosθ₄ ０l₄ ０ ０ １ A₅＝cosθ₅ ０−sinθ₅ ００ １ ０ ０sinθ₅ ０ cosθ₅ ０l₅・cosθ₅ ０ −l₅・sinθ₅ １ A_h＝１ ０ ０ ００ cosθ₆ sinθ₆ ００ −sinθ₆ cosθ₆ ０l₅ ０ ０ １ ………(3) となる。

したがつて、関節角度ベクトルΘが与えられた
とき、絶対座標系O₀−x₀y₀z₀での手先の位置X⁰
＝（ｘ、ｙ、ｚ）は、手先に固定された座標系O_h
−x_hy_hz_hでのそのの位置をX^h＝（x_h、y_h、z_h）と
すると X^O １＝ＡX^h １ ………(4) により計算される。

また、第１図ｂに示すように、座標系O₀−
x₀y₀z₀に対するO_h−x_hy_hz_hの各軸x_h，y_h，z_hの方
向余弦を、順にh₁〔＝（h₁₁、h₂₁、h₃₁）^T〕、h₂〔＝
h₁₁、h₂₂、h₃₂）^T〕、h₃〔＝（h₁₃、h₂₃、h₃₃）^T〕とし
、
座標系O₀−x₀y₀z₀における座標系O_h−x_hy_hz_hの原
点O_hの座標値をX^h ₀〔＝（X_h0、Y_h0、Z_h0）^T〕とする
と、(1)式の行列Ａは Ａ＝h₁₁ h₁₁ h₁₁ ０h₁₂ h₁₂ h₁₂ ０h₁₃ h₁₃ h₁₃ ０Xh₀ Yh₀ Zh₀ １ ………(5) とおくことができる。

したがつて、O₀−x₀y₀z₀における手先の姿勢
α、β、γは α＝tan^-1（h₂₁／h₁₁） β＝−tan^-1（h₃₁／√₁₁＋₂₁） γ＝tan^-1（h₃₂／h₃₃） ………(6) により計算される。

このように関節角度ベクトルΘが与えられたと
き、手先の位置、姿勢ベクトルＰは、(1)式の行列
Ａを求ることによつて計算される。

逆に、Ｐを与えてロボツトの手先を直交座標系
で移動させる場合も、(5)式の等値関係よりΘを求
める。Θは θ₁＝tan^-1〔Y_h0−（l₅＋l₆）sinαcosβ／X_h0−（l₅＋
l₆）cosαcosβ〕 θ₂＝±cos^-1〔R²＋ｌ〓−（l₃＋l₄）²／2l₂R〕＋φ ここでR²＝P²−Q²、φ＝tan^-1（Ｑ／Ｐ） Ｐ＝｛x_h0−（l₅＋l₆）cosαcosβ｝／cosθ₁ Ｑ＝−Z_h0−（l₅＋l₆）cosβ＋l₁ θ₃＝tan^-1〔Ｑ−l₂・sinθ₂／Ｐ−l₂・cosθ₂〕−θ
₂ θ₄＝tan^-1〔cosβ・sin（α−θ₁）／sinβ・cos（
θ₂＋θ₃）−cosβ・cos（α−θ₁）・sin（θ₂＋θ₃）
〕 θ₅＝±cos^-1〔sinβ・sin（θ₂＋θ₃）＋cosβ・cos
（α−θ₁）cos（θ₂＋θ₃）〕 θ₅＝tan^-1〔cos（θ₂＋θ₃）sinθ₄／sin（θ₂＋θ₃
）・sinθ₅−cos（θ₂＋θ₃）cosθ₄cosθ₅〕＋γ……
…(7) により計算される。しかし、このΘの解は存在範
囲を−π＜θ_i≦π（ｉ＝１〜６）に制限してモー
ド８通りある。

例えば同じ状態でθの値が異なる場合を第２図
ａ，ｂ，ｃに示す。すなわちＰに対するΘの解は
８通りあり、写像Ｐ→Θは１対８対応となつてい
る。この解の種類は、ロボツトに行わせる作業内
容によつて決めなければならないが、解の種類を
変更するためめには、特異点を通過しなければな
らない。

手先の位置・姿勢ベクトルＰが次の状態まで変
化したときの微小変化SPとそれに対する関節角
度ベクトルの微少変化δΘの関係は、ヤコビ行列
Ｊを用いて δＰ→＝JδΘ→ ………(8) で表せられる。ここで、Ｊは(7)式の静的な変換式 Ｐ→＝ｆ（Θ→） ………(9) を微分することによつて求められる行列であつ
て、 J_ij＝∂f_i／∂Θ_j （ｉ、ｊ＝１〜６） ………(10) により計算される動的な変換式(8)の係数となつて
いる。従つて、δＰ→が与えられていて、すなわち
直交座標系上の変分（差分）が与えられたとき、
角度変分δΘ→を求めるには、ヤコビ行列Ｊの逆行
J^-1が必要であるが、この逆行列が存在するため
には行列式detJ≠０が成り立たなければならな
い。

特異点とはdetJ＝０となる角度関係であり、そ
の分布は、ロボツトの機構により決定される。第
１図の構成のロボツトの場合、以下のようにな
る。

θ₂＋θ₃／２＝nπ θ₃＝nπ (11) θ₅＝nπ 、、を満足する特異点を示すロボツトの
状態は、第３図に示すようである。

特異点は解の種類の変化点であり、この点を境
にΘ→の動きは不連続となつてしまう。また、特異
点近傍でも、Ｐ→の微小変化によつて、Θ→が大きく
変化する。

従つて、直交座標系でロボツトが連続移動でき
る範囲には制限があることになる。

(3) 従来技術の問題点 従来、ロボツトを広い範囲で移動させる場合、
直交座標系だけを用いるのではなく、Θ→の成分を
座標値とする座標系を用いて解の種類を変更する
など、直交座標系とそれ以外の座標系を併用する
方法がとられている。

(4) 発明の目的 本発明は、６自由度多関節型ロボツトに固有の
特異点の存在をチエツクすることにより、直交座
標系上を大域的にすなわち、全空間を連続的に移
動できるロボツトの軌跡制御方式を提供すること
にある。

(5) 発明の構成 本発明は、計算したロボツトの目標関節角度と
現在関節角度との差から、計算値の解の種類が変
更したかどうかをすなわち、特異点に近くなつた
かどうかをチエツクし、解の種類が変更しすなわ
ち、特異点が近くなつた場合には、目標関節角度
を補正することにより特異点をスムーズに通過で
きる角速度を求めるようにするロボツトの軌跡制
御方式を提供したものである。

(6) 発明の実施例 本発明の制御方式を第４図と共に説明する。。
制御されるロボツトの構成は第１図とする。

演算処理装置１は直交座標系におけるロボツト
の指示径路上の各サンプル時点ｋ＋１での位置・
姿勢ベクトルP_K+1から(7)式を用いて関節角度
Θ_K+1をもとめ、このΘ_K+1とサーボ機構３内のエ
ンコーダ位置カウンタから得られた現時点での関
節角度Θ′_Kとから角速度 θ〓_K＝（θ_K+1−θ′_K）／Δt （Δt：サンプル周期） を生成し、速度関数発生部２へ出力する。

サーボ機構３は、ロボツトの各関節に配置され
ており、速度関数発生部２の出力を追従する。

本実施例において、特異点のチエツクは θ₂＋θ₃／２＝nπとθ₅＝nπの点に限り、θ₃＝nπ
の
点はチエツクせずに解の種類のどちらか一方に定
めておく、すなわち、θ₂からθ₃ を結ぶ直線によつて区切られる領域の一方に定め
ておく。その理由は第３図ｂより第３番目の関節
が一方の領域しか回転できなくでも、ロボツトの
手先の移動範囲に影響がないからである。

前記２点の特異点のチエツクは、ロボツトが θ₂＋θ₃／２がnπを通る時θ₁がずれる。

θ₅がnπを通る時θ₄とθ₆がπずれる。

ことを利用する。このことは、変換式(7)から導か
れることである。アクチユエータ等の機械的動作
速度の上限によつて定まる関節の最高角速度を
θ〓maxとする時、θ₁またはθ₄（θ₆）の変化、すなわ
ち、演算回路１で計算される関節の次の状態と今
の状態の角度の差｜θ_K+1−θ′_K｜が特異点におけ
る角度πと実際のアクチユエータ等の越え得る物
質的な限界角度θ〓max・Δtとの差より大となつた
ところ、すなわち、 ｜θ_K+1−θ′_K｜＞π−θ〓max・Δt となる所を特異点と見なし の場合 θ₁→θ₁±π θ₂→π−θ₂ の場合 θ₄→θ₄±π θ₅→−θ₅ θ₆→θ₆±π のように補正して、角度の特異点における不連続
的な変化を特異点に達する前に回避してしまう。
判別値をθ〓max・Δtとせずにπ−θ〓max・Δtとし
た理由は、特異点近傍の変化を考慮し、上記特異
点における不連続性の回避を特異点の近傍で行う
ようにしたためである。特異点での不連続性を除
去するためには、上述の如く、θ₁〜θ₆を特異点以
前で変化させればよいが、次にアクチユエータ等
を、特異点近傍において、いかに駆動すべきかに
ついて述べる。

特異点近傍では、θ₁とθ₄（θ₆）の変化は不連続
とならないまでも大きな変化をするため、１サン
プル周期Δtで移動する角速度は計算値ではアク
チユエータが駆動できる最高角速度θ〓max以上と
なつてしまう。これに対処するために特異点近傍
において、すなわちθ₁又はθ₄（θ₆）の変化が π−θ〓max・Δt＞｜θ_K+1−θ′_K｜≧θ〓max・Δtと
な
る所では、θ₁又はθ₄（θ₆）を計算値で得られる角
速度θ〓_Kの代わりにアクチユエータの実際の最高角
速度θ〓maxで動かし（θ〓_K＞θ〓max）、その分サン
プ
ル周期を長くするように、｜θ〓_K／θ〓max｜・Δtと
すれば、移動距離を計算値で与えるものとアクチ
ユエータをθ〓maxで動かした場合とで一致させる
ことができる。この場合θ₁またはθ₄（θ₆）以外のθ_i
については、計算値で得られる角速度を｜
θ〓max／θ〓_K｜倍することはもちろんである。

(7) 発明の効果 上述したように本発明では、特異点のチエツク
を行い、またはこれと同時に特異点近傍のチエツ
クをを行うことにより、与えられた直交座標から
各関節の関節角度へと特異点及びその近傍におい
ても、スムーズに変換でき、従つて６自由度多関
節型ロボツトの機構に固有の特異点及びその近傍
を連続的に通過できるので、ロボツトが直交座標
系上を大域的に移動できる。

【図面の簡単な説明】

第１図ａ，ｂは各々発明に適用された６自由度
多関節型ロボツトの構成図とオイラー角で表示さ
れた手先の位置姿勢を示す図、第２図ａ，ｂ，ｃ
は８組の関節角度のモード毎の説明図、第３図
ａ，ｂ，ｃは３種の特異点での関節の状態を示す
図、第４図は速度制御方式の装置構成図。 １……演算処理装置、２……速度関数発生部、
３……サーボ機構、O₀−x₀y₀z₀……ロボツトの絶
対固定座標系、O_i−x_iy_iz_i……各関節に固定され
た座標系（ｉ＝１〜５）、O_h−x_hy_hz_h……手先に
固定された座標系、l_i……第ｉ番目の腕の長さ、
θ_i……第ｉ番目の関節の回転角度、Ｐ……O₀−
x₀y₀z₀上での手先の位置・姿勢を表す６次元ベク
トル、h₁，h₂，h₃……O_h−x_hy_hz_hのｘ，ｙ，ｚ軸
のO₀−x₀y₀z₀に対する方向余弦。

Claims (1)

1. 【特許請求の範囲】 １ 多関節ロボツトのベースに定義した直交座標
   系にて予め指定されている始点より逐次所定の時
   間後に通過するべき補間点を演算し、前記各々の
   補間点におけるロボツトの関節角を前記直交座標
   系からロボツトの形態に固有の関節座標系への座
   標変換演算により求め、前記補間点における関節
   角と移動中の現時点の関節角との差から補間点に
   達するまでに関節を駆動させる速度を求める演算
   を行い、ロボツト先端を終点まで移動させるロボ
   ツトの軌跡制御方式において、前記関節角の差が
   予め設定されている所定量より大きくなつた時、
   関節角が不連続に変化する特異点に近づいたと見
   做し、補間点の関節角を、変化量が少なく、かつ
   先端が同じ位置・姿勢をとることのできる前記座
   標変換の別解に変更することを特徴とするロボツ
   トの軌跡制御方式。 ２ 多関節ロボツトのベースに定義した直交座標
   系にて予め指定されている始点より逐次所定の時
   間後に通過するべき補間点を演算し、前記各々の
   補間点におけるロボツトの関節角を前記直交座標
   系からロボツトの形態に固有の関節座標系への座
   標変換演算により求め、前記補間点における関節
   角と移動中の現時点の関節角との差から補間点に
   達するまでに関節を駆動させる速度を求める演算
   を行い、ロボツト先端を終点まで移動させるロボ
   ツトの軌跡制御方式において、前記関節角の差が
   予め設定されている所定量より大きくなつた時、
   関節角が不連続に変化する特異点に近づいたと見
   做し、補間点の関節角を、変化量が少なく、かつ
   先端が同じ位置・姿勢をとることのできる前記座
   標変換の別解に変更し、また、前記関節角の差が
   前記所定量よりも小さいが、求めた関節駆動速度
   が機構的に駆動可能な最高関節角速度より大きく
   なつた時、前記特異点の近傍に近づいたと見做
   し、補間点への関節駆動速度を前記最高関節角速
   度に設定すると共に、その速度で駆動する時間
   を、速度を低減させた比率分に見合うだけ長く設
   定し直すことを特徴とするロボツトの軌跡制御方
   式。