JPS58114888A - ロボツトの軌跡制御方式 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

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

（２）技術の背景 汎用ロボットの研究は、人間に似た動作をいかに自動的
に機械のｆにやらせるかということに向けられている。

したがって、σを川口ボットの腕は、コンピュータで制
御され、多関節を有するものである。いわゆる多関節型
ロボットでは、どの関節をどれだけ動かしたら手先がど
この位置までくるかを直観的に知ることは容易ではない
。したかって、各関節の回転薔を知って、腕や手先の各
部の位置をコンピュータにより、ただちに、すなわちリ
アルタイムに、１翼できなくてはならず、一般にマトリ
ックスを用いた方法、ずなわら座標変換行列を用いて計
算する方法がとられる。

各関節がすべて回転の自由度で構成される６１山度多関
節型ロボットを直交座標系で移動させる場合の問題点を
図１の構成のロボットを例に説明する。

第１図（ａｌに示す関節角度ベクトル０−（θ１゜θ２
．θ３．θ４．θ６．θ６）を与えて、第１図（ｂ）に
示す手先の位置・姿勢ベクトルＰ−（ｘ。

ｙ、ｚ、　　α、β、γ）を求めるには、各関節に固定
された座標系間のトータルな座標変換行列を用いる。

第１図（ａｌ、　（ｂｌの０ｏ−ｘｏｙｏｚｏはロボッ
トに対して設定された絶対固定座標系、０ｌ−Ｘ１Ｙ＋
Ｚ＋　　（ｉ＝１〜５）は各関節に固定された座標系、
Ｏｈ−ｘｈ　ｙｈ　ｚｌ＋は手先に固定された座標系で
ある。

Ｏｈ　ｘｈｙｈｚｈから０−ｘｙｚへの座標変換行列Ａ
は Ａ＝ＡＩＡ２Δ３　Ａ　ａ　Ａ　５Ａ　１．・・・・（
１）と計算される。ここでＡ　＋　　（ｉ　−１〜５及
び１＝ｈ）はＯ１Ｘ　＋　ｙＩＺ　ｌがらＯ＋−１’−
ｘ＋、ｌ　’／　＋、ｌ　２１−１への変換行列であり
、Ｏｏｌ−ｘ１ｙ１２１でのある点の座標値を×１　　
とすると より定義され ９°”「（ｌｚｏ　　°ｉｒ１＆°　２″−ｃｏｓ（７
°］・　・　・　・　・　（３） ヒなる。

したがって、関節角度ベクトルθが与えられたとき、絶
対座標系０ｏ−ｘｏｙｏ２ｏでの手先の位置Ｘ’　＝　
（ｘ、　　ｙ、　　２）は、手先に固定された座標系Ｏ
ｈ　　ＸｈｙｈＺｈでのその位置を１１−（ｘｈ・　）
’ｈ、２ｈ）とすると により計算される。

また、図１（ｂ）に示すようにＸｈ、　　ｙｈ＋　　Ｚ
ｈ軸の０ｏ−ｘ’ｏｙｏｚｏに対する方向余弦を順にｈ
＋、ｈ２．ｈ３とし、０ｏ−ｘｏｙｏｚｏの原点Ｏｈの
座標値をＸ　とすると、（１）式の行列Ａは・　・　・
　・　・　（５） とおくことができる。

したがって、Ｏら−ＸｕｙｏＺｕにおけるｆ、先の姿勢
α、β、Ｔは α＝ｊａｎ−’（ｈ？ｌ／ｈ＋１） β−−１ｄｎ”　　（ｈ　ｓ　　１／　　ＪＴ’Ｖ　ｌ
−；−璽「Ｉ下）ｒ−ｔａｎ″（ｈ　Ｊ　−＋／ｈ　３
３）・・・・・（６） によりδを算される。

このように関節角度べぐトル○が与えられたとき、手先
の位置、姿勢−・クトルｌ）は、（］）式の行列へを求
めることによって計算される。

逆に、Ｐを与えて１１ボツトのｆ−先を直交座標糸で移
動させる場合も、（５）式の等値関係よりＯを求める。

Ｏは ここでＲ２−ｐ　’　＋　ＱＮ、φ−ｔａｎ−’　（Ｑ
　／　Ｐ）Ｐ＝（ｘｈｏ　　（文５＋交６）ｃｏｓαｃ
ｏｓβ）／　　ｃｏｓθ１ Ｑ　＝　−Ｚ　ｈｏ　　　（文５＋又６）　　ｓｉｎβ
十又１・　・　・　・　・　・（７） θ５＝±（３）−Ｉ〔し９糾ＣＱ、＋θ３ン↑（３）β
□□□（ｍ−に’１１（３）（０・＋θ１］θ６−ｔａ
ｎ”　［ＡＴｈ（＋）＋＋９１３Ａ；Ｊ、−Ｌｏ（１１
１Ｊｉｌｌｃ６０Ｑ、（ａｐθ、］”・　・　・　・　
・　・　（８） により計算される。しかし、このθの解は存在範囲を一
πくθ１≦π（ｉ＝１〜６）に制限してモード８通りあ
る。

例えば同じ状態でθの値が異なる場合を第２図（ａ）。

（１））、　ＦＣ＋に示す。すなわちｆに対する百の解
るよ８通りあり、写像ｐ−７５は１対８対応となっても
）る。

この解の種類は、ロボットに行わせる作業内容ζこよっ
て決めなければならないが〜解の種類を変更するために
は、特異点を通過しなければならなＬ〜。

ｊを用いて δＰ＝ＪδＯ・・・・・・・・・・・（８）で表せられ
る。ここで、ｊは（７）式の静的な変換式Ｐ＝ｆｌ）　
　　・・・・・・・・・・・＋９１を微分することによ
って求められる行列であって、Ｊ　ＩＪ　　＝　ａｆ　
１／’ａ　Ｏｊ　　　（１、Ｊ　＝１〜６）・・・・・
・　（１０） により計算される動的７変換式（８）の係数となってい
る。従って、δ下が与えられていて、すなわち直交座標
系上の変分（差分）が与えられたとき、角度変分δるを
求めるには、ヤコビ行列ｊの逆行列Ｊ’が必要であるが
、この逆行列が存在するためには行列式ｄｅｔＪ≠０が
成り立たなけれはならない。

特異点とはｄｅｔＪ　＝　Ｏとなる角度関係であり、そ
の分布は、ロボットの機構により決定される。

第１図の構成のロボットの場合、以下のようになる。

■θ２＋θ３　／　２　＝　ｎπ ■θ３＝ｎπ　　　　　　　　　　　　　　　（］１）
■θ５＝ｎπ ■、■、■を満足する特異点を示すロボットの状態は、
１３図に示すようである。

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

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

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

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

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

（６）発明の実施例 本発明の制御ツノ式を第４図と共に説明する。制御され
るロボットの構成は第１図とする。

演算処理装置１は直交座標系における四Ｊノドの指示径
路上の各サンプル時点に＋１での時点　　ＰＫや１から
（７）式を用いて関節角度ＯＫ＋１をもとめ、この○に
や１とサーボ機構３内の工・ンコータ位置カウンタから
得らねた現時点での関節角度Ｏ′にとから角速度 θに−（θにや１−θ′Ｋ）／Δｔ （Δｔ：サンプル周期） を生成し、速度関数発生部２へ出力する。

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

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

前記２点の特異点のチェックは、ロボットが■θ２＋θ
３／２がｎπを通る時θ１がπずれる。

■θ５がｎπを通る時θ４とθ６がπずれる。

ことを利用する。このことは、変換式（７）から導かれ
ることである。アクチュエータ等の機械的動作速度の上
限によって定まる関節の最高角速度をδ□８Ｘとする時
、θ１またはθ・　（θ６）の変化・すなわち、演算回
路１で計算される関節の次の状態と今の状態の角度の差
ｊθた！−θ’Ｋ　　ｌが特異点における角度πと実際
のアクチュエータ等の越え得る物質的な限界角度ｅ　ｍ
ａｘ　・Δｔとの差より大となったところ、ずなわら、 １０に＋１−　θ″ｙ　　ｌ　＞π−δ□ａＸ　、Δｔ
となる所を特異点と見なし ■の場合　θ１−　θ１±π θ２−　π−θ２ ■の場合　θ４　→　θ４±π θ５−→　−θ５ θ６　→　θ６±π のように補正して、角度の特異点における不連続的な変
化を特異点に達する前に回避してしまう。

判別値をθｍａｘ　・Δｔとせずにπ−δｍａｘ　・Δ
ｔとした理由は、特異点近傍の変化を考慮し、上記特異
点における不連続性の回避を特異点の近傍で行うように
したためである。特異点での不連続性を除去するために
は、上述の如く、θ１〜θ６を特異点以前で変化させれ
ばよいが、次にアクチュエータ等を、特異点近傍におい
て、いかに駆動すべきかについて述べる。

特異点近傍では、θ１とθ４　（θ６）の変化は不連続
とならないまでも大きな変化をするため、１サンプル周
期Δｔで移動する角速度は計算値ではアクチュエータが
駆動できる最高角速度δｍａｘ以上となってしまう。こ
れに対処するために特異点近傍において、すなわちθ１
又はθ４　（θδ）の変化が Δｔとなる所では、θ１又はθ４　（θ５）を計算値で
得られる角速度θにの代わりにアクチュエ−ｊＩＩｌａ
×）、その分サンプル周期を長くするように、１δ、／
δｗａｘ　ｌ・Δｔとすれば、移動距離を計算値で与え
られるものとアクチュエータをδｍａｘで動かした場合
とで一致させることができる。この場合０１またはθ４
　（θ６）以外のθ１については、計算値で得られる角
速度を１δｍａｘ　／θに１倍することはもちろんであ
る。

（７）発明の効果 上述したように本発明では、特異点のチェックを演算回
路ｌによって行うと同時に、特異点近傍におけるサーボ
機構３の制御も行なうことにより与えられた直交座標か
ら各関節の関節角度へと特異点及びその近傍においても
、スムーズに変換でき、従って６自由度多関節型ｒＪボ
ットの機構に固有の特異点及びその近傍を通過できるの
で、［１ボツトが直交座標糸」−を人威的に移動できる
。

【図面の簡単な説明】

第１図（ａｌ、　（ｂｌは各々発明に適用された６自由
度多関節型ロボットの構成図とオイラー角で表示された
手先の位置姿勢を示す図、第２図（・＋１．　ｉｂｌ、
　（（：）は８組の関節角度のモード毎の説明図、第３
図（ｄ）５１ｂ＋、　（Ｃ］ｌは３種の特異点での関節
の状態を示す図、第４図は速度制御力式の装置構成図。 １・・・演算処理装置、２・・・速度関数発生部、３・
・・サーボ機構、Ｏｏ　−ｘ　ｏ　ｙ　ｕ　ｚ　、＋・
・・ロボットの絶対固定座標系、０ｌ−ｘｌｙＩｚｌ・
・・各関節に固定された座標系（ｌ−１〜５）、Ｏｈ　
　ＸｈｙｈＺｈ・・・手先に固定された座標系、文、・
・・第１番目の腕のＫさ、θ、・・・第１＃目の関節の
回転角度、Ｐ・・・０−ｘｙｚ上での手先の位置・姿勢
を表す６次元ベクトル、ｈ＋、ｈｐ、ｈ：＋　°’０ｈ
−ｘｌ＋）’ｈｚｈのＸ。 ｙ、ｚ軸の０−ｘｙｚに対ケる方向余弦。

Claims (2)

【特許請求の範囲】
1. （１）多関節型ロボットを直交座標系より移動させる制
   御方式において、ロボットの目標関節角度と現在関節角
   度との差から関節角度が不連続に変化する特異点に近付
   いたことを検知し、各関節の角度を所定量変化させるこ
   とによって特異点を免れる手段を有することを特徴とす
   るロホ・ノド軌跡制御方式。
2. （２）多関節型ロボットを直交座標系より移動させる制
   御方式において、ロボットの目標関節角度と現在関節角
   度との差から関節角度が不連続に変化する特異点に近付
   いたことを検知し、各関節の角度を所定量変化さゼるこ
   とによって特異点を免れる手段と特異点近傍においては
   機械的に可能な最高の角速度で関節を動かしても正しい
   移動を得るようにサンプリング周期を長くする手段とを
   設けることを特徴とするロボット軌跡制御方式。