JPH03231306A - ロボットの軌跡制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、ロボットの軌跡制御装置に関し、特にロボッ
トアーム先端をＣＰ（コンティニュアス・バス）制御を
用いて制御する場合に、ロボットアーム先端を所望の軌
跡に沿って精度よく移動させることができる装置に関す
る。

〔従来の技術および発明が解決しようとする課題〕 ロボットアーム先端軌跡の制御方法としてＣＰ（コンテ
ィニュアス・バス）制御が広く知られているが、このＣ
Ｐ制御を達成するための一方法として「運動速度分解制
御法」がある。この運動速度分解制御法では、直交座標
系におけるロボットアーム（以下、甲にアームという）
先端の移動速度ベクトルが所定時間ごとに与えられた時
、与えられた移動速度ベクトルをアームの各関節の回動
軸の回転速度に変換し、この変換した回転速度か得られ
るように各回動軸が制御される。

すなわち、いまδ関節の回動軸の回転位置ベクトルをｅ
、直交座標系におけるアーム先端の位置ベクトルをｒと
すると、これら各ベクトルｅ１　ｒの間には、 ｒ−ｆ（ｅ）　　・・・　（１） のごとく表せるような所定の関係がある。この（１）式
の両辺を微分すると、 が得られる。ここで、Ｊ　（θ）は回動軸の回転位置に
依存したヤコビアン行列である。いま、ヤコビアン行列
Ｊ　（ｅ）の逆行列Ｊ−’（ｅ）が存在するものとすれ
ば、（２）式に逆行列Ｊ−１（θ）を乗算して、回転速
度ｅが下記（３）式のように求まる。

上記（３）式をマイクロプロセッサにおいて一定のサン
プリング周期ごとに演算する場合には、サンプリング周
期の間ではヤコビアン行列の変化が小さく一定であるも
のと仮定して、上記（３）式の替りに下記（４）式を用
いることができる。ただし、Δｒはアーム先端位置ベク
トルｒのサンプリング時間中の変化量（ベクトル量）、
Δθは各関節の回転位置ベクトルθのサンプリング時間
中の変化量（ベクトルｊｌ）である。

Δθ−Ｊ−’（ｅ）　　・Δ「・・・（４）また、サン
プリング周期ごとの各関節の目標回転位置は（４）式で
得られたΔθを下記（５）式のように積算することによ
り得られる。

ｅ−θ＋Δθ・・・（５） 以上の演算処理を制御ブロック図に表すと第３図に示す
ようになる。

すなわち、同図に示すようにまずアーム先端の所望軌跡
に応したアーム先端位置ベクトルｒのサンプリング時間
中における変化量Δｒが与えられると、ヤコビアン逆行
列乗算部１１０では、上記（４）式の演算が行われて、
各関節の回転位置ベクトルθのサンプリング時間中にお
ける変化量Δθが求められる。つぎに、得られた関節回
転位置ベクトルｅの変化量Δｅを順次積分する処理（上
記（５）式の演算）が積分処理部１２０で実行され、関
節目標回転位置（ベクトル量）ｅが求められる。しかる
のち、求められた関節目標回転位置（ベクトル量）ｅが
得られるようにサーボ制御部１３０において各関節の回
動軸がサーボ制御される。

この方法ではサンプリング時間の間でのヤコビアン行列
の変化量が無視できる程小さいことを仮定しているが、
実際にはサンプリング時間（周期）が長い場合や、アー
ム先端の移動速度が大きい場合には、この仮定が成り立
たなくなり、（４）式の；１算において誤差が生じるこ
とになる。そして、（４）式における演算誤差は、さら
に（５）式により積ｐされて増大することとなってしま
う。すなわち、第４図に示すように（５）式で得られた
関節の［１標回転位置ｅ・・・を（１）に代入して再変
換されるアーム先端の目標位置ｒ−・、に応じた軌跡１
４０（これを−点鎖線にて示す）は、本来要求されるア
ーム先端の［１標位置ｒ・・に応じた軌跡１　（これを
実線にて示す）からしだいにずれていってしまうことに
なる。

そこで、こうした軌跡すれを除去すべく第５図に示す制
御方法が考えられている。

すなわち、同図に示すように、アーム先端の所望軌跡に
応したに一１回１１のサンプリング周期におけるアーム
先端の位置ベクトルの変化量Δｒ（ｋ−１）が口＋Ｓ値
としてり、えられたものとすると、第３図の場合と同様
にヤコビアン逆行列乗算部２１０では、上記（４）式の
演算が行われて、各関節の回転位置ベクトルｅのサンプ
リング時間中における変化量Δｅが求められ、つぎに、
求められた関節回転位置ベクトルθの変化量Δθを順次
積分する処理（上記（５）式の演算）が積分処理部２２
０て実行され、ｋ−１回のサンプリング時間終了時点に
おける関節目標回転位置（ベクトル量）θが求められる
。しかるのち、求められた関ｒｉ１目標回転位置（ベク
トル量）θが得られるようにサーボ制御部２３０におい
て各関節がサーボ制御される。

第３図の制御と異なる点は以下の通りである。

すなわち、検出部２４０においてに一１回目のサンプリ
ング時間終了時点における関節目標位置θ（ｋ−１）に
基づき上記（１）式の演算が実行されてアーム先端位置
ｒ′が再変換される。一方、［１標値として与えられた
変化量Δｒ（ｋ−１）を積分処理部２５０において積分
して本来の（所望軌跡上の）アーム先端位置ｒ（ｋ−１
）が求められる。そしてこの本来のアーム先端位Ｍｒ（
ｋ−１）から再変換されたアーム先端位置ｒ″　（ｋ−
１）を１％＆算する処理を減算部２６０で実行して、ア
ーム先端の位置ずれ、つまりヤコビアン逆行列乗算部２
１０、積分処理部２２０で行われた（４）（５）式の演
算による演算誤差ε（ｋ−１）を求める（第６図参照）
。そこでつぎのに回目のサンプリング周期では目標値と
して与えられた変化量Δｒ　（ｋ）に前回（ｋ−１回口
）のサンプリング周期において得られた演算誤差ε（ｋ
−１）を加え合わせる処理を加算部２７０で行う。こう
した前回の演算誤差が加味された補正変化量Δ「（ｋ）
＋ε（ｋ−１）に基づきヤコビアン逆行列乗算部２１０
、積分処理部２２０において（４）、（５）式の演算を
行うようにする。この場合、ヤコビアン逆行列Ｊ−’（
ｅ）を求めるに使用する関節回転位置ｅは、ｋ−１回口
のサンプリング時間終了時点における関節［１標位置ｅ
（ｋ−１）を用いている。

いま、第７図に示すようなＸ−Ｙ平面の円周３００内を
アーム先端３１１が移動しうる３リンクのマニピュレー
タ３１０に第３図、第５図に示す１、す御を適用した場
合を考察する。そこで、先端リンクのＸ軸に対する姿勢
角をαに保持して、アーム先端３１］をＸ軸マイナス方
向（矢印３２ｏ）に１サンプリング時間あたり一定距離
Δ「だけ移動させる制御を行わせたとする。すると、第
８図に示すように第３図の制御ではアーム先端軌跡１５
０（サンプリング周期ごとの移動位置をバラ印で示す）
は、移動するにつれて本来の要求軌跡２から次第にずれ
ていくのがわかる。一方、第５図の制御の場合は軌跡２
８０（サンプリング周期ごとの移動位置を三角印で示す
）に示すように次第にずれていくことを抑えることはで
きるが、本来の要求軌跡２に収束することなく定常誤差
が発生していることがわかる。なお、−回［１のサンプ
リング周期において第５図の制御による軌跡誤差が′第
３図の制御による軌跡誤差と変りないのは、回１１では
第５図の検出部２４０でフィーバツク量を検出すること
ができず、このため前回の演算誤差を目標値に加味した
制御を行うことができないためである。

本発明はこうした実情に鑑みてなされたものであり、定
常誤差を除去してロボットアーム先端を所望の軌跡に収
束させる制御を行うことができる装置を提供することを
その目的としている。

〔課題を解決するための手段および作用〕そこでこの発
明の第１発明では、Ｐめ設定されたロボットアーム先端
の軌跡上の連続する２点間における前記ロボットアーム
先端の１−１標移動速度を順次求め、該目標移動速度に
基づき前記連続する２点を移動させる前記ロボットアー
ムの６回動軸の［１標回転速度を順次演算する第１の演
算手段と、この第１の演算手段で演算された前記目標回
転速度に基づき前記、連続する２点のうちの終点におけ
る前記各回動軸の目標回転位置を順次演算する第２の演
算手段とを資し、前記第２の演算子３段で演算された前
記目標回転位置に基づき前記各回動軸を制御するように
したロボットの軌跡制御装置において、前記第２の演算
手段で演算された各目標回転位置から前記ロボットアー
ム先端位置を順次求め、該求めたロボットアーム先端位
置と前記予め設定された軌跡との位置ずれを順次各終点
ごとに演算する位置ずれ演算手段と、前記位置ずれ演算
手段で順次演算された各終点における位置ずれを累算す
る累算手段と、前記累算手段で累算された累積位置ずれ
に基づいて、つぎの連続する２点間における前記１１樟
移動速度を補正する補正手段とを具え、前記補正手段て
補１された補正目標移動速度に基づき前記第１の演算手
段による演算を行うようにしている。

すなわちかかる構成によれば、第１の演算により前回発
生したロボットアーム先端位置の位置ずれ（演算誤差）
のみに基づきアームの目標移動速度が補正されるのでは
なく、前回までの位置ずれか累算された累積位置ずれに
基づきアームのＬ１標移動速度が補正される。これによ
り、第１の演算手段の演算誤差が低減されて、アーム先
端の移動軌跡を所望軌跡に一致させることができる。

また、この発明の第２発明によれば、予め設定されたロ
ボットアーム先端の軌跡上の連続する２点間における前
記ロボットアーム先端の目標移動速度を順次求め、該目
標移動速度と前記ロボットアームの各回動軸の所定回転
位置とに基づき前記連続する２点を移動させる前記ロボ
ットアームの各回動軸の目標回転速度を順次演算する第
１の演算手段と、この第１の演算手段で演算された前記
「１標回転速度に基づき前記連続する２点のうちの終点
における前記各回動軸の目標回転位置を順次演算する第
２の演算手段とを在し、前記第２の演算手段で演算され
た前記目標回転位置に基づき前記各回動軸を制御するよ
うにしたロボットの軌跡制御装置において、前記第２の
演算手段で演算された連続する２点の終点における目標
回転位置に基いて、つぎの連続する２点間の所定点にお
ける前記８回動軸の回転位置を推定演算する推定演算手
段を具え、前記第１の演算手段は、前記所定回転位置と
して前記推定演算手段で推定演算された推定回転位置に
基いて前記っぎの連続する２点間に関する演算を行うよ
うにしている。

すなわち、かかる構成によれば、第２の演算手段によっ
て前回得られたロボットアームの各回動軸の目標回転位
置に基づき、っぎの連続する２点の中間点における回転
位置が推定される。そして、この推定された中間点にお
ける回転位置に基づき、つぎの連続する２点ｍｌに関し
、第１の′ｅｔ算手段による演算を行えば、この連続す
る２点の始点における回転位置に基づき、第１の演算手
段による演算を行う従来のものに比較して演算誤差が大
幅に低減されることになる。

〔実施例〕

以下、図面を参照して本発明の実施例について説明する
。第１図に実施例装置の制御ブロック図を例示する。

同図に示すようにこの装置は第５図に示す積分処理部２
２０、サーボ演算部２３０、検出部２４０、積分処理部
２５０、減算部２６０および加算部２７０と同様な積分
処理部２０、サーボ演算部３０、検出部４０、積分処理
部５０、減算部６０および加算部７０とから構成される
とともに、さらにこれらにヤコビアン逆行列乗算部２１
０とは異なるヤコビアン逆行列乗算部１０と、積分補償
演算部８０とを付加した構成となっている。この積分補
償演算部８０はさらに積分処理部８１と加算部８２とか
ら構成されており、積分処理部８１は、減算部６０で得
られる演算誤差εを積分ゲインに１を以て積分する処理
を行い、加算部８２はＭ分処理部８１て積分された演算
誤差Σεと減算部６０の出力εとを加え合わせて、加算
結果を加節部７０に出力する処理を行う。

いま、アーム先端の所望軌跡に応じたに一１回］］のサ
ンプリング周期におけるアーム先端の位置ベクトルの変
化量Δｒ’（ｋ−１，）が［」標値として与えられたも
のとすると、この変化量の［１標値Δｒ（ｋ−１）と、
ｋ−１回口のサンプリング開始時点におけるアーム先端
ＩＩ標値ｒ（ｋ−２）と、ｋ−１回「１のサンプリング
終了時点におけるアム先端［１標値ｒ（ｋ−１）との間
には明らかにド記（６）式 ％式％（１） （６） の関係がある。この（６）式の演算は積分処理部５０で
行われる。

一方、ヤコビアン逆行列乗算部１０では、後述するよう
に基本的には上記（４）式と同様の演算が行われて、各
関節の回転位置ベクトルｅのに一１回１−１のサンプリ
ング時間中における変化量Δθ（ｋ−１）が求められる
。つぎに、求められた関節回転位置ベクトルｅの変化量
Δθ（ｋ−１）を積分する演算（上記（５）式の演算に
対応する）、θ（ｋ−１）−〇（ｋ　−２）＋Δθ（ｋ
−１）・・（７） が積分処理部２０て実行され、ｋ−１回［１のサンプリ
ング時間終了時点における関節目標回転位置θ（ｋ−１
）が求められる。しかるのち、求められた関節目標位置
θ（ｋ−１）が得られるようにサーボ制御部３０におい
て各関節がサーボ制御される。

さらに下記（８）式に示すように検出部４０ては上記（
７）式のごとく得られた関節目標回転位置θ（ｋ−１）
に基づき上記（１）式の演算が実行されてアーム先端位
置ｒ−（ｋ−１）が再変換される。

ｒ−（ｋ−１）　　−ｆ　　（θ　（ｋ−１））−（８
）なお、前述するようにこの再変換されたアーム先端位
置ｒ−（ｋ−１）には、演算誤差ε（ｋｌ）が含まれて
おり、本来のアーム［１標位置ｒ（ｋ−１）とは演算誤
差ε（ｋ　−１＞たけずれている。そこで、本来のアー
ム目標位置ｒ（Ｉｃ−１，）から再変換されたアーム目
標位置ｒ−（ｋ−１）を減算する処理（（６）式−（８
）式）か下記（９）式に示すごとく減算部６０で実行さ
れ、上記演算誤差ε（ｋ−１）が求められる。

ε（ｋ−１，）　−ｒ　（ｋ−１）　−ｒ　　（ｋ−１
）・・・（９） さらに積分処理部８１ではいままでの演算誤差を積分す
る下記（１０）式の積分補償演算を行う。

Σε（ｋ−１）−Σε（ｋ−２）＋Ｋｉ・ε（ｋ−１）
・・・（１０） ただしＫｉは積分ゲインである。そして加算部８２は（
９）式によって得られた演算誤差ε（ｋ−１）と、（１
０）式によって得られた演算誤差の積分値Σε（ｋ−１
）とを加え、これを加算部７０に出力する。いま、つぎ
のに回目のサンプリング周期におけるアーム先端の位置
ベクトルの変化量Δｒ　（ｋ）が目標値として与えられ
たものとすると、加算部７０からは、下記（１１）式に
示す補１Ｆ目標値Δｒ　（ｋ）か出力される。

Δｒ　（ｋ）−Δｒ　（ｋ）　＋ε（ｋ−１）　＋Σε
（ｋ　−１，）・・・（１１） つぎに、ヤコビアン逆行列乗算部１０では、上記（４）
式の演算を行うことになるが、ヤコビアン逆行列Ｊ−’
０で使用する各関節の回転位置ベクトルは、ｋ−１回口
のサンプリング終了時点（ｋ回目のサンプリング開始時
点）における目標回転位置ベクトルθ（ｋ−１）ではな
くて、今回（ｋ回目）のサンプリング時間においてアー
ム先端が要求軌跡の連続する２点の中点に存在するとき
の各回動軸の回転位置ベクトルθ（ｋ）を下記（１３）
式によって推定し、この推定した回転位置ベクトルｅ　
（ｋ）を用いるようにしている。

したがって、各関節の回転位置ベクトルθのに回１１の
サンプリング時間中における変化量Δθ（ｋ）は、下記
（１２）式のごと１くなる。この演算によれば、ヤコビ
アン逆行列Ｊ−’（ト）を求める際に一１回目のサンプ
リング終了時点における位置ベクトルθ（ｋ−１）を代
入する第（４）式の場合に較べて大幅に涜神誤差を低減
できることがわかっている。

Δθ（ｋ）−Ｊ−’　Ｃｅ（ｋ））−Δｒ　（ｋ）・・
（１２） △ θ　（ｋ）−ｅ　　（ｋ−１）＋　　（］／、２）　　
−ｆｅ（ｋ　−１）　　−〇　（ｍ−２）ｌ　　・・・
　（１３）こうして変化量Δθ（ｋ）が求まったならば
、積分処理部２０て（（５）式の演算）による正確な関
節目標回転位置θ（１０を求めることができるようにな
る。しかるのち、求められた関節目標回転位置（ベクト
ル量）θ（１０がｉＵられるようにサーボ制御部３０に
おいて各関節の回動軸かサーボ制御される。

再び、第５図、第１図の各制御を第７図の３リンクマニ
ピユレータ３１０に適用した場合を考察し、これら各制
御による応答を比較する。

第５図の制御の場合は軌跡２８０（サンプリング周期ご
との移動位置を三角印で示す）に示すように本来の要求
軌跡２に収束することなく定常誤差か発生している。し
かし、第１図の制御の場合は、軌跡９０（サンプリング
周期ごとの移動位置をパン印で示す）に示すように定常
誤差は発生せず、本来の要求軌跡２に収束し、正確な軌
跡を措いているのがわかる。

以上説明したように実施例によればアーム先端位置を各
関節回転位置に変換する際に発生する演等：！１差を単
にフィードバックするのではなくて、演算誤差を積分し
て、この累積誤差を目標値に加味するようにしたので、
定常誤差の発生が抑えられ、アーム先端の移動軌跡を要
求軌跡に精度よく致させることができるようになる。

また、実施例によれば関節回転位置に基づきサンプリン
グ時間中の関節位置の変化量を演算する場合に（ヤコビ
アン逆行列の演算）、前回のサンプリング終了時点にお
ける関節位置に基づき演算するのではなくて、今回のサ
ンプリング時間中の関節回転位置を推定演算し、この推
定関節回転位置に基づき上記関節回転位置の変化量を演
算しているので、演算誤差が大幅に低減されることにな
る。したがって、アーム先端の移動軌跡の正確な制御に
大きく寄与することとなる。

なお、実施例では（］３）式の右辺の第２項に示すよう
に１／２を乗算して、ロボットアーム先端が要求軌跡の
連続する２点の中点に存在するときの各回動軸の回転位
置を推定するようにしているが、係数１／２に限定され
ることなく、要は連続する２点間のうち始点を除く、中
間点に対応する回転位置を推定し得る係数であれば任意
である。

さらに（１３）式の演算式に限定されることなく、中間
点に対応する回転位置を推定し得る演算式であればよい
。

また、実施例ては、（１０）式の演算式によって積分補
償演算を行うようにしているが、これに限定されること
なく、前回までの演算誤差を積算し１！Ｉる演算式であ
れば、その内容は任意である。

〔発明の効果〕

以上説明したように本発明によれば、ロボットアーム先
端位置から各関節回転位置を求める演算で発生する演算
誤差が大幅に低減されるので、ロボットアーム先端の移
動軌跡と要求軌跡との間に発生する定常誤差が除去され
て、移動軌跡を要求軌跡に一致させる高精度な制御を行
うことが可能となる。

【図面の簡単な説明】

第１図は、本発明に係るロボットの軌跡制御装置の実施
例を概念的に示す制御ブロック図、第２図は、第１図に
示す制御装置と第５図に示す制御装置とによって第７図
に示すマニピュレータのアｌ、先端を制御した場合にお
けるアーム先端の移動軌跡の変化の様子を示す比較図、
第３図は、従来のロボットの軌跡制御装置を概念的に示
す制御ブロック図、第４図は、第３図に示す制御装置に
よるロボットアーム先端の移動軌跡の変化の様子を示す
図、第５図は、従来のロボットの軌跡制御装置を概念的
に示す制御ブロック図、第６図は、第５図に示す制御装
置によるロボットアーム先端の移動軌跡の変化の様子を
示す図、第７図は、ＸＹ２次元平面内をアーム先端が移
動する３リンクのマニピュレータを概念的に示す図、第
８図は、第３図に示す制御装置と第５図に示す制御装置
とによって第７図に示すマニピュレータのアーム先端を
制御した場合におけるアーム先端の移動軌跡の変化の様
子を示す比較図である。 １．２・・要求軌跡、］０　　・ヤコビアン逆行列乗算
部、２０．５０．８１・・積分処理部、３０・・サボ演
算部、４０・・検出部、５０・・・積分処理部、６〔］
・減算部、７０．８２・・・加算部、８ｏ・・・補償演
算部、９０・・移動軌跡、３１０・・マニビュレタ、３
１】　・アーム先端。 第２図 １０ ２０ ３０ り 第４図 第５図 第６図

Claims (2)

【特許請求の範囲】
1. （１）予め設定されたロボットアーム先端の軌跡上の連
   続する２点間における前記ロボットアーム先端の目標移
   動速度を順次求め、該目標移動速度に基づき前記連続す
   る２点を移動させる前記ロボットアームの各回動軸の目
   標回転速度を順次演算する第１の演算手段と、この第１
   の演算手段で演算された前記目標回転速度に基づき前記
   連続する２点のうちの終点における前記各回動軸の目標
   回転位置を順次演算する第２の演算手段とを有し、前記
   第２の演算手段で演算された前記目標回転位置に基づき
   前記各回動軸を制御するようにしたロボットの軌跡制御
   装置において、 前記第２の演算手段で演算された各目標回転位置から前
   記ロボットアーム先端位置を順次求め、該求めたロボッ
   トアーム先端位置と前記予め設定された軌跡との位置ず
   れを順次各終点ごとに演算する位置ずれ演算手段と、 前記位置ずれ演算手段で順次演算された各終点における
   位置ずれを累算する累算手段と、前記累算手段で累算さ
   れた累積位置ずれに基づいて、つぎの連続する２点間に
   おける前記目標移動速度を補正する補正手段と を具え、前記補正手段で補正された補正目標移動速度に
   基づき前記第１の演算手段による演算を行うようにした
   ロボットの軌跡制御装置。
2. （２）予め設定されたロボットアーム先端の軌跡上の連
   続する２点間における前記ロボットアーム先端の目標移
   動速度を順次求め、該目標移動速度と前記ロボットアー
   ムの各回動軸の所定回転位置とに基づき前記連続する２
   点を移動させる前記ロボットアームの各回動軸の目標回
   転速度を順次演算する第１の演算手段と、この第１の演
   算手段で演算された前記目標回転速度に基づき前記連続
   する２点のうちの終点における前記各回動軸の目標回転
   位置を順次演算する第２の演算手段とを有し、前記第２
   の演算手段で演算された前記目標回転位置に基づき前記
   各回動軸を制御するようにしたロボットの軌跡制御装置
   において、前記第２の演算手段で演算された連続する２
   点の終点における目標回転位置に基いて、つぎの連続す
   る２点間の所定点における前記各回動軸の回転位置を推
   定演算する推定演算手段 を具え、前記第１の演算手段は、前記所定回転位置とし
   て前記推定演算手段で推定演算された推定回転位置に基
   いて前記つぎの連続する２点間に関する演算を行うよう
   にしたロボットの軌跡制御装置。