JPS583002A - ロボツトの制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 本発明はロボットの制御方式に関し、特にロボットノア
ームやマニビュし一夕の各関節を回転制御する場合、こ
れを精度よく滑らかに回転制御できるようにしたロボッ
トの制御方式に関する。

従来、ロボットのアームやマニピュレータの制御方式と
しては、関節角を指示値として与える位置制御方式中角
速度を指示値として与える速度制御方式が多く採用され
ている。関節角中角速度は。

通常直交座標や円筒座標郷の空間座標上の位置や速度を
変換して求められる。

例えば第１図において演算処理装置１に対し。

直交座標上で始点Ｘｏ　（Ｘｏ　ｒ　Ｙ＠　＋　ｉ＠、
αＯ９βＯ９Ｔ、）と終点ＸＮ（ｘＨ，ｙｗ、　ｚＨ，
ａＨ，β□γｍ）　（ここでα。

β、ｒはオイラー角）が指示されたとき、演算処理装置
１は上記データから、各”＋　１＋　”軸の方向の移動
が同時に終了するような、纂２図（イ）Ｋ示す如き速度
曲線を作成し、それにもとづき、第２図（ロ）Ｋ示す如
きＸｏからＸＮへの推移曲線を作成する。

そして演算処理装置１はこれにもとづき、第２図（）→
に示す如く各関閣毎のアーム移動量である関節角θをサ
ンプリングタイムに４ｇＫ演算して、このサンプリング
タイム毎の関節角へを速度指示値発生部２に伝達する。

速度指示値発生部２は、との。

演算処理装置１から演算された関節角＾とエンコ値を速
度関数発生部５に＠遅し、これＫよ）速度関数発生０部
３は自薦しているカウンタのカウント数と前記指示値Ｊ
ｉを比較し、指示値θｌが大きいときＫこれに応じた出
力をヰじ、　Ｖ／Ｆコンバータ５に伝達する。Ｖ／Ｆコ
ンバータ５は速度関数発生部５からのアナログ信号をデ
ィジタル信号に変換し。

比較位置カウンタ５に出力する。一方速度関数発生部３
よ抄出力したアナログ信号は、微分器４を鮭肉して加速
度としてモータ７に伝達される。比較カウンタ６はＶ／
Ｆコンバータ５がらのディジタル信号をカウントしこれ
を積算してその結果を関節角θ飄として出方する。エン
コーダ位置カウンタ８はモータ７の現在位置を知らせる
パルスθｈヲｓ速度指示値発生部２及び位置制御部１０
Ｋｉｉ続されたＤ／Ａコンバータ９に出力する。そして
関節角θｈの情報と、エンコーダ位置カウンタ８がらの
現在位置を示す関節角θｉとの差分ｔｉＤ／Ａコンバー
タ９を経て位置制御部１ｏに入方される。そして位置制
御部１０では関節角θにとθに′との位置偏差が零にな
るようにモータ７を駆動制御する。

しかしながらこのような制御方式によればサーボ制御が
不充分のため本出願人は特願１１１４５５−１８７８８
７号「ロボットの制御方式」として、第３図に示す如く
、サンプリングタイムに時点では演算処理装置１から次
のサンプリングタイムに＋１時点での関節角θ−十１を
出力し、これと現時点での、関節角θ駄′とを比較しそ
の位置偏差によ抄制御を行なうことを提案した。これＫ
よれば精度が非常に向上するもののなおかつ次のような
間層が存在する。

すなわち、一般に関節角と空間座標上の位置とは非線形
の関係があるので変換に長い時間を要する。それ故、現
在値を読出してそれに４とづき計算する場合には細かい
サンプリング周期で関節角や角速度を指示することがで
きず、各サンプリング時点での関節角や角速度の指示値
の不連続性が大きくな９アームを滑らかに動かすことが
できなかった。また各サンプリング点の間ではアームの
位置が指示軌跡からずれ、移動経路を重視するような作
業では大きな問題となっている。

したがって本発明の目的は、このような問題を改善する
ために、被制御体を指示軌跡に偏差なく追従させるため
に、サンプリングされた数個の関節角や角速度に対して
その間を補間することにより、より小さなサンプル周期
で指示値を生成することができ、被制御体が各サンプリ
ング点を滑らかに積置よ〈移動制御１できるようにした
ロボットの制御方式を提供することＫｌる。

そしてこの目的を達成させるために１本発明では、移動
する被制御体のあらかじめ定められた指示軌跡上のサン
プリング位置の情報を演算して関節角や角速度を生成す
ゐ関数発生手段を有するロボット制御方式において、指
示軌跡上の数個のサンプリング位置からその間の補間値
を演算する軌跡補間手段を設けたことを４！微とする。

以下本発明の一奥施例を第４図ないし第６図にもとづき
説明する。第４図は本発明の７実施例構成図であ抄、第
５図および第６図はその動作状態説明図である。

図中、他択と陶符号部は同一部分を示し、１′は演算処
理装置であって、第１図における演算処理装置１に対応
し、１１は座標変換部、１２は軌跡補間部である。

演算処理部１′は始点Ｘ・（ｘｏ＋ｙ・、２＠、α・、
β・。

γ０）と終点ＸＮ（ｘ）、、　ｙＨ，ｇ）１．　ａｙ、
β麗、γＮ）　（ただしα、β、γはオイラこ角）が与
えられたとき。

これらのデータから各ｘ、Ｙ＋”＋α、β、γ軸方向の
移動が同時に終了するような、第５図（イ）で示す速度
１綜を作成し、それにもとづき始点Ｘｏから終点式への
移動曲線上の点をサンプル周期△Ｔごとに計算する。そ
してｍ時点（ｍ：５）だけ、先読みしたサンプリング位
ＩＩＩ　Ｘｋ＋ｓａを座ｍ俊換１１１１１に出力する屯
のである。この例ではサンプリング時点にのとき、ｍ■
４のに＋４時点の位置を先読みし。

座標変換部１１に出力する。

座標変換部１１は、演算処理部１′から出力されたサン
プリング位置Ｘｋ＋１に応じて各関節角＃に＋。

（ロボットが６関節屋アームのときＫはれ十□〇ｚ＋、
・・・θ：＋ｍ）Ｋ変換し、軌跡補間部１２に出力する
。

軌跡補間部１２は、第５図（）→のＸ印に示す如く。

各サンプル時点の中間部分における各関節角の値θ−＊
Ｉ＋１　（！＝Ｏ、ｐ−１）を演算するものである。そ
してこの補間値を演算する場合には、σに一／　、ｅｌ
ｈ−１＋ｓ　。

・−°θｋ　”・θに＋＋ａ−１１θに＋ｓａのｊｌ＝
Ｚ十ｍ＋１（ｎ≧５）個の点を用いてθにとθに＋１と
の間を補関し、前記σｋ　ｌ＋＊（ｔ−ｏ、ｐ−１）を
演算する。いまｌ！−１，ｍｗ３の場合について、第６
図にもとづき説明する。

まず６十Ｍ　＋　５ｈｙｓおよび６十１時点の１次黴係
数り＋ｔを求める。

姑、　θ−十１−θｈ九十 一三碇−・・・・・・・・・・・・・・・・・・・・・
・・・　（１）ａｌ−保二転　　　１１１１１１０９６
．８０６８１１０１１．６０８９０６．（２）△Ｔ δ。−〇・−＾　　　、１１０１１９９５１１１０１１
６１１、−１−１．＝　ｆｉｌΔＴ ８に４１　ａｍ　　ｌ　　　θに＋１　−　θｋａｔ　
　Ｉ　　・・・・・・・・・・・・・・・・・・・・・
　　（４１８ｏ−ｏ（初期値）　・・・・・・・・・・
・・・・・・・・・・・・・・・・・（５）Ｓ８＋０ま
たは８にヶ１＋０ならｄ ５ｋｍＯかつＳｋ＋１　ｓ＋Ｉ　Ｑならばｔｈ＋ｔ　−
、（へ＋θに÷、）　・・・・・・叫・・・川・・（７
）次に補間係数ｃｋ、ｄｋｌｅｋを求め、これを使用し
てθにとへ＋１０間の点θに、ｌ＋１０−〇、・・・　
ｐ−１）を求める。

θす＋！−θｈ　＋　（ｃｈ　Ｔｌ＋１　＋　ｄｋＴ：
＋ｔ　＋　＠に’Ｉ’ｌ＋’ｌ　）△Ｔ−ａｌｔ−）−
１ ＴＩ＋璽−一ΔＴ（ｉ−０，１・・・　ｐ−１）・・・
αＪそして仁のようにして求められた前記θｋ　ｌ＋１
を△Ｔ／ｐの周期で速度指示値発生部２へ出力する。

そして仁の補間点ごとの差に応じた制御が可能になる。

次に第４図の動作について説明する。

ここでは空間座標が直交座標の場合について説明する。

（１）　　まず、演算処ｍ装置１’に対し、第５図（ロ
）Ｋ示す如く、始点Ｘｏ　（ｘｏ　＋　ｖｅ　ｒ　ｔｏ
　＊　ｅｋ　＋βｓ、ｒｅ）と終点ＸＮ（＊ｗ、　７）
ｌ、　ｚｗ＋α１βｗ、γＮ）が与えられ九とき、これ
らのデータから各”＋７＋！＠α、β、ｒ軸方向の移動
が同時に終了するような速度−Ｍ（第５図（イ））を作
成し、それにもとづき始点為から終点ＸＮへの移動曲線
上の点をサンプル周期Δτととに計算する。そして仁の
移動点のうち、サンプリング点で３時点だけ先読みした
に千３時点におけるサンプリング位ＩＮ　Ｘｈ＋ｓを座
標変換部１１に出力する。

（２）　　座標変換部１１はこのＸｋ＋Ｉをうけて各関
節角θに＋ｌに変換する。例えばロボットが６関節薯ア
ームの場合には、このＸｋ十ｍにより関節角θ−＋ｈθ
２ヤ３．θｔ＋、・・・ル＋−を出力し、これらを軌跡
補間部１２に出力する。

１３１　　軌跡補間部１２では、第６図（ロ）Ｋ示す如
く。

この新らしく伝達され九〇に＋１と、それ壕でに伝達さ
れているθ−十嘗Ｈ＋９に＋１　ｅθ−，θに−１等に
４とづ１１゜＃配ｔ１り式に示す如く、θ翫とθ−＋１
、の間の点θ−−十。

（１−０，１・・・ｐ−１）を求める。このようにして
第６図（ロ）における・印点１ｊｋ−＊　’ｊｋ、１　
ｔへ−・・・＃−が関節角θ皺とａｋ＋１との間の補間
点として求められる。そしてこの関節角θｈＫつづき、
これらの補間点（ｈ　Ｉ；θに一；θｈ−・・・を速度
指示値発生部２に出力する。

（４）　　これにより速度指示値発生部２は前配軌跡補
間部１２から得られた関節角６に、ｌ＋１とモータに取
付けられたエンコーダ位置カウンタＢから現位置の値θ
に−とＫよって角速度θ−鑞一（ｅｋ　ｔ＋ｔ−θ′ｋ
Ｉ）ＺべＬＣ＋−ｏ、　１・　ｐ−１）を生成し、この
指示値を速度関数発生部！ｌＫ出力する。そして仁の角
速度指示値１１；　ｔが第６図（）→に示される。

（５）　　この速度関数発生部３では内蔵されているカ
ウンタのカウント数と角速度指示値θ【Ｉとを比較し、
この角速度指示値が大きいときはカウンタ数を上げ、小
さいときは下げて両者が一致するようにする。そしてこ
の値がＤ／Ａコンバータ３′によりＤ／Ａ変換され、　
Ｖ／Ｆコンバータ５に出力される。

コノｖ２１Ｆｌブンバータ５では前記Ｄ／Ａコンバータ
５′のアナログ出力電圧が周波数に変換されるので。

比較位置カウンタ６はこれをカウントして積算し。

その結果を関節角の指示値θに−として閉ループ制御部
へ出力する。

（６）　　一方、エンコーダ位置カウンタ８では、峰−
夕７の回転角から現在位置を知らせるディジタル信号の
関節角θ′に−を出方し、この現在位置を示す信号θロ
ーと関節角の指示値θに、ｌとの差分がＤ／Ａコンバー
タ９を経由して位置制御部１０に送出される。そしてこ
れにより指示値θλ、昌と現在位置を示す信号θ；−と
の位置偏差が零になるように、モータ７のモータ駆動回
路を制御する。またエンコーダ位置カウンタ８からの出
力信号θ；−紘同時に速度関数発生部２にフィードバッ
クされ、現在位置と補間点とに応じた角速度が速度指示
値発生部２から出力されるもので娶る。

勿論２本発明はロボットのアームの移動経路中移動速度
が任意のパターンにおいても容易に適用できるものであ
る。例えば第７図（イ）では滑らかに動かすために、始
点および終点近傍で一定加速度の加減速を行なう場合で
あり、また第７図（ロ）では加速度の変分を一定とする
場合であるが、いずれの場合でも極めて有効である。

と軌跡補間部とを独立させ、それぞれを処理を並行に行
うように構成したことにより、空間上の位置を各関節角
に変換するのに要する時間よりも十分小さなサンプル周
期で関節角中角速度の指示値を生成することができる。

また補間数を大きくすれば（すなわち０式でｐを大きく
すれば）、従来よシも十分に小さな周期で逐次指示値を
生成することができ、移動中のアームの位置精度を高め
ることが可能である。また始点Ｘｏから終点島へのサン
プリング点数を少なくシ、それに応じて補間数を大きく
するととＫより高速にアームを移動させることができる
。

さらＫまた。軌跡補間部における補間方法としては、前
述の準エルミート補間式を用いる方法の他に、始点や終
点における２階の微係数を用いる５次ａｐｌ　ｌｉｅ補
間式やｍ階の微係数を用いるＢ　−ａｐｌｉｎ＠補間式
を利用する方法等があり、４１定されるものではな・い
。

【図面の簡単な説明】

第１図は従来のロボット制御方式、第２図はその動作説
明図、第３図は既出願の動作説明図、第４図は本発明の
一実施例構成図、第５図および第６図はその動作説明図
、第７図は他の速度・加速度関係図である。 図中、１は演算処理装置、２は速度指示値発生部、３は
速度関数発生部、４は微分器、５は電圧・周波数コンバ
ータ（Ｖ／Ｐコンバータ）、６は比較位置カウンタ、７
はモータ、８はエンコーダ位置カウンタ、９はディジタ
ル・アナレグ・コンバータ（Ｄ／Ａコンバータ）、１０
は位置制御部、１１は座標変換部、１２は軌跡補間部を
それぞれ示す。 特許出願人　　富士通株式会社 代理人弁理士　山　谷　皓　条 ・に　　　　　　　　　　×　　２　　　　　壱四 べ　　　　；　　　　　　　　　　　　　宙ト一一

Claims (1)

【特許請求の範囲】
1. （１）　　移動する被制御体のあらかじめ定められ九指
   示軌跡上のサンプリング位置の情報を演算して関節角や
   角速度を生成する関数の発生手段を有するロボット制御
   方式において、指示軌跡上の数個のサンプリング位置か
   らその間の補間値を演算する軌跡補間手段を設けたこと
   を特徴とする四ボットの制御方式。