JPH03250307A - 剛物体の経路制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明は、例えばロボットアーム等の剛物体を、特に指
定した目標軌道に高速応答させ、しかも誤差な（追従さ
せるための経路制御装置に関するものである。

［従来の技術］ 従来のロボットアームの経路制御法の多くは数学モデル
の線形化の考え方に基づいている。そして、非線形力で
ある重力、遠心力、コリオリカ等は、数学モデルを使っ
てフィードフォワード制御によって補償されていた。

上記の如き制御方法として、例えば「日本ロボット学会
誌；Ｖｏｌ、１．　ＮＯ，、２Ｊに記載されているｒ拘
束条件付制御によるロボットアームの経路制御Ｊが知ら
れている。この制御方法は、制御対象であるロボットア
ームにまず非干渉化制御を施し、その系の外側にさらに
最適Ｉ型す−ボ系を構成する制御系設計法となっている
。ここで、非干渉化制御とは、制御系に２つ以上の入力
、例えば、位置指令入力と速度指令入力とを独立して入
力し、夫々独立して、位置制御と速度制御制御を行なう
というものである。そして、この拘束条件を課した制御
方法では、ロボットアームの数学モデルが正確に把握で
きるならば、その数学モデルに基づいて設計された制御
系は、目標軌道に正確に追従することが保証されている
。

［発明が解決しようとしている課題］ しかしながら、上記の制御系では、制御対象であるロボ
ットアームの正確な数学モデルが求められる事を前提と
している為、実機への適用には問題があった。

即ち、通常ロボットアームでは、制御系設計のための簡
略化、物理定数の不確かさ、非線形特性の影響等によっ
て、得られる数学モデルにはモデル誤差が生じるのは不
可避である。しかるに、従来例のｒ拘束条件付制御によ
るロボットアームの経路制御」では、ロボットアームに
対して施される一段目の非干渉化制御の実現が、正確な
数学モデルの導出を前提とし、このモデルの正確さに強
く依存するのである。従って、現実には、不正確な数学
モデルしか得ることはできない為、非干渉化制御は不完
全となり、この不完全な非干渉化制御系に対して、最適
１型サーボ系を組んでも、正確な目標起動への追従が実
現できないばかりか、安定性への影響も大きい。

以下、従来の制御設計法とその問題点を数式と数値実験
を示して詳細に説明する。

ロボットアームの運動方程式はよく知られているように
次式で与えられる。

Ｍ　　−ｅ＋Ｃ＋Ｄ　　−ＩＪ＋Ｇ＝ｕ・・・・・・　
（１） ここで、ｅは関節角ベクトルであり、θの１階の時間微
分を、ｅは２階の時間微分を表わす。Ｍは慣性行列であ
って、関節角ベクトルｅの関数でもある。また、Ｃは遠
心力及びコリオリカを表現する項であり、θとｅの関数
である。さらに、Ｄは粘性摩擦計数行列である。Ｇは重
力項でありθの関数である。Ｕは入力トルクのベクトル
である以下、説明を簡略化するために、２リンク水平ロ
ボツトアームについて（１）式を用いてアームの運動を
記述した場合の従来技術の問題点を説明する。水平アー
ムの場合は、重力項Ｇは零とお（。第２図はこの２リン
ク水平ロボツトアームの作業座標系を説明する図である
。このロボットアームは第１アームの長さがβ１．関節
角がθ８、第２アームの長さがρ２．関節角θ２である
。そして、第２図に示すように、Ｘ＝２８の直線軌道ニ
沿って、３’　＝　Ｖ　ｙの一定速度で、第２アームの
先端を追従させるようにして運動させた時の拘束条件は
、 ｘ　　＝　　ｆ　　Ｉ（θ　）　　＝　　１２　　、ｓ
ｉｎθ　、＋Ａ　　ａｓｉｎ（θ　１＋θ　２）（２ａ
） ｙ　＝　ｆ　ｚ（ｅ　）　＝−Ｑ　＋ＣＱＳθ１−１２
２ｃ０３（θ１＋θ２）（２ｂ） δｆ２（θ）　Ｔ ｙ・「　　　　］θ　　　　・・・（２ｃ）ａθ で表現される。かかる運動は、例えば、溶接ロボットの
アームを、直線上に並ぶ溶接位置に沿って移動するもの
を想定すればよい。

さて、 平衡位置θ。

の近傍で、 ｅ＝θ＋八〇。

へ＝ｔｌ　＋△Ｕ とおき、 線形近似を行なうと、 次式の状態方程式 が得られる。

（３ｂ） 但し、 である。

ここで、 （３ａ） （３ｂ）式を、 文献 「システム制御理論 （実数出版。

１９７９） に掲載 されている非干渉制御の手順に従って非干渉化すると、
θ、＝ｎπ（ｎ＝ｏ、±１）の特異姿勢を除外して非干
渉化が可能となり、次式で表現されるようになる。

＊＝Ａｘ＋Ｂｗ △ｕ＝　　［０，Ｄ］ ｘ＋ＶＩＪ−’ｗ （４） 但し、 ｘ＝［△ＯＴ、△θＴ］Ｔ、Ｊ＝「シ゛］２ である。また、Ｗは新しい入力である。（４）式は、積
分器だけから成るシステムである。即ち、（４）式で表
わされる系は、上式△Ｕの補償入力によって、ロボット
アームを、物理定数に全（依存しない系に整形している
のである。

しかし、実際には、△Ｕの補償入力の完全性は、得られ
るロボットアームの数学モデルが不確実なので期待する
ことはできない。

さて、（４）式で表現された系は漸近安定ではないので
、次に最適■型す−ボ系を（４）式に対して付加する。

結局、ロボットアームへの入力Ｕは、 ｕ＝’Ｑ＋△Ｕ ＝　［０，Ｄ］　ｘ−￥７１３−’Ｆ　Ｐ　＋ＰＪ　−
’Ｋ　ｖ・・・・・・　（５ａ） ？＝　　［０，ｖ、］”　　−［△Ｘ、△ｙ］”・・・
・・・　（５ｂ） で表現される。

但し、 Ｐ＝［△Ｘ。

Δ文、△ｙ、△ｙ］” Ｋ　＝　ｄｉａｇ（ ｋｌ＋ に２ ） である。ここで、ｆ、、に、は、重みをＱ、Ｒとするリ
カツテイ方程式を解いて得られる。実用上は、実際の軌
道偏差δＸ、δｙを計測して、次式を補償人力Ｕが得ら
れる。

ｕ＝　［０，Ｄ］　ｘ−￥ＡＪ−’Ｆｐｎ　＋ＷＪ−’
Ｋｖ・・・・・・（６ａ） ■＝［−δｘ、ｖう一δｙ］Ｔ （６ｂ） 但し、 ｐｎ　”　［δＸ、δＸ、δｙ、δｙ］Ｔδｘ＝ｆ＋（
ｅ）−氾、 δｘ＝ｊ＋（θ）０ δｙ＝ｊｚＣｅ）　θ である。以上が、ｒ拘束条件付制御によるロボットアー
ムの経路制御Ｊの内容であり、（６）式に示された制御
システムの全体図は第３図となる。

以上、文中で既に述べたように、（４）式の非干渉化制
御を実現するフィードバックゲインには、ロボットアー
ムの物理パラメータ（慣性係数Ｍや粘性摩擦係数り等）
が使われている。しかし、その物理パラメータは、何等
かの手段を使って推定した値であり、公称値でしかない
。ロボットアームの物理パラメータの真値ではないので
ある。即ち、物理パラメータには推定誤差が不可避的に
伴なう。そこで、この推定誤差に原因する不安定性の問
題と、軌道追従精度への影響が大きな課題として浮かび
あがってくるのである。

さて、求められた物理パラメータに推定誤差が伴なって
いると言う状況をシミュレートするために、Ｍ、ゴαＭ
、Ｄｎ＝βＤとおく。但し、Ｍは慣性行列の真値、Ｍ、
は慣性行列の推定値、αはスカラー倍率、Ｄは粘性摩擦
係数行列（対角行列）、Ｄ、は粘性摩擦係数行列の推定
値、βはスカラー倍率である。

先づ、粘性摩擦係数行列には、推定誤差が無く（β＝１
）、慣性行列にのみ推定誤差がある（β≠１）とき、ａ
を変化させたときの平衡点Ａ、　Ｂに関する根の軌道を
第４図に示す。ここで、平衡点Ａ、Ｂは、（ｘ、ｙ）座
標で、それぞれ、（ＸＡ　、３’Ａ）＝　（０；　３．
−０．３）。

（ＸＢ　、　ｙｓ　）＝　（０，３，０，３）［ｍｌの
位置である。同図より、αが大きい程に制御系は安定で
あり、αが小さくなるとダンピングの劣化したものとな
る事が分る。尚、第４図で、ｘ印は、α＝１、即ち、粘
性摩擦係数行列に推定誤差が無いのみならず、慣性行列
にも推定誤差がない時の極の位置である。

第５図は、慣性行列に推定誤差がなく（α＝１）　粘性
摩擦係数行列にのみ推定誤差がある（β≠１）とき、β
を変化させたときの平衡点Ａに関する根軌跡である。こ
の図からは、βが大きくなると制御系は不安定になるこ
とが了解できる。又、βく１の場合は、安定ではあるが
、応答の遅い制御系となっている。尚、第５図で、ｘ印
は、β＝１、即ち、慣性行列にも推定誤差がないのみな
らず、粘性摩擦係数行列にも推定誤差が無い時の極の位
置である。

第６図は、公称値の真値に対する推定誤差によって、平
衡点に関する閉ループの極が最初に不安定側にある場合
を想定し、最適Ｉ型す−ボ系の設計に於いて、調整パラ
メータとなる重みＱ、Ｑ＝ｄｉａｇ　（Ｑ＋　、Ｑ２　
） を操作して安定化をはかるときの根軌跡である。

実機を対象とした実験や机上の数値実験に際して、通常
行なわれる操作を図示したものと言える。同図より、以
下の事が結論される。

■二重みＱを大きくすることによって、閉ループの不安
定接を安定側に引き込むことができる。しかしダンピン
グの劣化した状態しか実現できない。

■：従って、物理パラメータの推定誤差に原因して平衡
点における安定性が損なわれている場合、重みＱの操作
だけに依存していたのでは、制御系の安定化は可能とな
るものの、所定の軌道追従性能を得ることはできない。

上記諸性質をふまえて、第７Ａ図と第７Ｂ図に、平衡点
ＡからＢに経路追従させた時の関節トルクｕ＋、ｕｚと
、関節角θ１．θ２の時間変化についての数値実験結果
を示す。条件は、ａ＝１．　０．　　　　β＝０．８ の場合で、重みＱを適切に選び、平衡点位置に関する安
定性は確保しである。しかるに、同図に示されているよ
うに、例えばＶつ＝０．６ｍ／ｓのときには、経路追従
させると発散状態に至ると言う問題点が生じている。発
明者による追及の結果、この現象は物理パラメータの推
定誤差にょる外乱によって積分器出力にオフセットが生
ずることが原因となっている。

［課題を解決するための手段］ 本発明は上述の課題を解決する為に成されたものであり
、例えば、ロボットアーム等の剛物体の物理パラメータ
推定に於いて不可避な誤差が発生する場合に、上述のシ
ステムの不安定状態や発散状態が起こることを防止する
ことのできる剛物体の経路制御装置を提案するものであ
る。

上記課題を達成するための本発明の剛物体の経路制御装
置の構成は、 この剛物体の物理パラメータと、外部からの指令とに基
づいて制御量を演算する演算部と、この制御量を前記演
算部の入力にフィードバックする前記演算部を最適制御
するサーボ部と、前記剛物体の速度情報と前記制御量と
を入力情報として、前記物理パラメータの誤差を補償す
るための補償量を演算して出力する補償部であって、こ
の補償量が前記演算部に正帰還されて、前記制御量に加
算されるように、前記演算部に接続された補償部とから
なることを特徴とする。

上記構成の剛物体の経路制御装置によると、前記物理パ
ラメータの誤差が前記演算部に外乱として働く場合に、
前記補償部による正帰還はこの外乱の応答遅れとして働
き、この外乱を収束する方向に作用する。

［実施例］ 以下、本発明の剛物体の経路制御装置をロボットアーム
に適用した実施例を説明する。この実施例の制御装置で
は、具体的には、以下の如き手段を設けている。

（ａ）ロボットアームに対して、物理パラメータの公称
値を使った非干渉化制御を軌道上の平衡点に関して施す
。

（ｂ）その非干渉化制御系に対して、最適Ｉ型す−ボ系
を構築し、２段階制御構造を有する拘束条件付制御系を
実現する。しかし、このままでは、実際上避けることが
できない物理パラメータの推定誤差に原因して、経路追
従精度の劣化と不安定性の問題を引き起こしてしまう。

（Ｃ）そこで、ロボットアームへの制御量と、アーム先
端の速度を入力情報とした補償器を設け、この補償器出
力に適切なゲインを乗じて上記制御量に加算する。

上記の（ａ）〜（Ｃ）の構成は、物理パラメータの推定
誤差が制御系に対して外乱として作用する量をロボット
アームへの制御量とアーム先端の速度を入力情報とした
補償器により検出し、この出力に適切なゲインを乗じて
制御量に加算するので、外乱を打ち消す作用がある。よ
り具体的に記述すると以下のようになる。物理パラメー
タに推定誤差がある場合で、かつ上記の補償器を設けた
時に、速度指令Ｖ、から位置制御系の積分器出力Ｖ、ま
での伝達関数は次式となる。

・・・・・・　（７） 但し、φｃ、、（ｓ）は特性方程式、ω。。

器の定数、Ｆはゲインである。また、 ａ＋、１＝（β−１）ＪＩＷＩ−１Ｄ である。但し、Ｍはここで、 Ｊ は補償 ｙＯ Ｖｙ＝− のステップ入力が印加された場合、 ｔ−１■におけ るｖｌの値は、 となる。ゲインＦを Ｆ＝− ｂ。

と選べば、ｙ、（ｏｏ）＝Ｑとすることができる。

ここで、補償器出力をロボットアームの制御量に加算し
ない場合について考察する。かかる場合は、上式におい
てＦ＝Ｏとすればよ（、ｄ２ □　　　　　　　　　・・・　（９） ・・（“）“−９’に＋ となる。β≠１のとき、８．２≠０となり、速度指令Ｖ
　ｙ　”　Ｖ　ｙｏ／　Ｓの入力が位置制御系に対して
は外乱として入力されるため、その制御系の積分器出力
には、（９）式で表わされるオフセット量が現われるの
である。よって、このオフセット量によって、位置偏差
が増大していき、ついにはロボットアームの可動限界に
至ると云う不安定現象が引き起されるのである。もちろ
ん、β：１の場合、即ち粘性摩擦係数行列にパラメータ
誤差がない場合には、ｙ、（ｏｏ）＝：Ｑとなるので、
不安定現象は生じない。

第１図は、第２図に示したＸ＝ρ８の直線軌道に沿って
、ｙ＝ｖ、の一定速度で、第２アームの先端を追従させ
るようにして運動させた時の拘束運動を行なう際の、平
衡点に関して示したロボットアームの経路制御装置の構
成図である。

同図において、１は物理パラメータに推定誤差がある状
態で非干渉化制御を施した時の非干渉化制御装置である
。この非干渉化制御装置１は同図に示されているように
、Ｘ方向の制御系とＸ方向の制御系とを有するが、本実
施例では、Ｘ＝β８の直線軌道に沿って、ｙ＝Ｖｙの一
定速度で、第２アームの先端を追従させるようにして運
動されるために、Ｘ方向については位置制御系として、
Ｘ方向については速度制御系の役割を果たす。即ち、位
置制御系からは、積分器１５の出力であるΔ大と、積分
器１８からの出力△Ｘが８カされる。また、速度制御系
からは、積分器２７の出力である△ｙと、積分器３０か
らの出力△ｙが出力される。乗算器１３は、慣性行列の
誤差を補正するだめに、位置制御系の制御量Ｗ１にスカ
ラ倍率αを乗する。乗算器１３は、慣性行列の誤差を補
正するために、速度制御系の制御量ｗ２にスカラ倍率α
を乗する。乗算器１６は、位置制御系において、粘性摩
擦係数行列の誤差分を補償するために、積分器１５の出
力△Ｘにゲイン （β−１）・ｊ、・ＴＪ５−’、Ｄ　ｊ＋を掛ける。同
じ（、乗算器２８は、速度制御系において、粘性摩擦係
数行列の誤差分を補償するために、積分器２７の出力△
ｙにゲイン （β−１）・Ｊ２・’１１−’ＤＪ　ｚを掛ける。

乗算器１７．２６については後述する。

制御装置１の外側には、最適Ｉ型サーボ装置が組まれて
いる。即ち、このサーボ装置は、位置制御系において、
Ｘ方向の位置偏差△Ｘに状態フィードバックゲインｆ１
を乗じて加算器１２に負帰還する乗算器４０と、Ｘ方向
の位置変化の偏差△Ｘにゲインｆ２を乗じて加算器１２
に負帰還する乗算器４０と、積分器１０の出力ｖ１にゲ
インに、を乗じて加算器１２にフィードフォワードする
乗算器１１と、更に、速度制御系において、Ｘ方向の位
置変化の偏差△ｙにゲインｆ、を乗じて加算器２３に負
帰還する乗算器２９と、積分器２１の出力ｖ２にゲイン
に２を乗じて加算器２３にフィードフォワードする乗算
器２２とからなる。

そして、さらに、本実施例の制御装置は、速度指令ｖ、
が非干渉化制御装置１の位置制御系に及ぼす外乱を検出
する為の補償器２と、この補償器出力に適切なゲインＦ
を乗じる乗算器４５と、この積出力を位置制御系の制御
量Ｗ＋に加算するための加算器１２とを有する。

上記の構成に於いて、粘性摩擦行列りに同定誤差が無い
場合、即ちβ＝１のとき、乗算器１６゜１７．２６．２
８はオーブンとなるので、制御系全体は、最適１型サー
ボ系のゲインｆ　Ｉ　、ｆ２　。

ｋ、から成る位置制御系と最適■型す−ボ系のゲインｆ
、、に、から成る速度制御系とに分離される。従って、
位置制御系でロボットアーム先端の位置の拘束を行ない
、速度制御系で速度指令ｖｙに対する追従が夫々役割分
担されている。

ところが、β≠１のときは、速度指令■、が、位置制御
系に対しては、乗算器１７のバスを通してゲイン （β−１）・Ｊ、・Ｖｌ−１・　Ｄ−Ｊ　２の外乱とし
て作用する。この外乱は、位置制御系の積分器出力Ｖ１
にオフセットを生じせしめる原因となっている。しかも
このオフセットは速度指令Ｖ、が零になっても即座に零
とはならない為、長く位置制御系に印加され続ける。よ
って、このオフセット量に原因して、位置の拘束がはず
れていき、ついには発散状態に至る。

そこで、速度指令Ｖｙの入力による位置制御系の上記外
乱を、位置制御系の制御量ｗ１とＸ方向の速度Δ大を入
力とする補償器２に導く。補償器２では処理は以下のよ
うである。即ち、乗算器５２により制御量Ｗ、にゲイン
ｂ、を乗じた量と、乗算器５１により速度Δ大にゲイン
ｂ２を乗じた量とが加算器５３により加算されこの和が
特性Ｓ　＋　ω０ の低域通過フィルター５４に通されて、この低域成分が
加算器５５に入力される。この加算器５５の他の入力は
、乗算器５０により速度△Ｘにゲインｂ、を乗じた量で
ある。

この補償器２の出力（即ち、加算器５５の圧力）は、丁
度、位置制御系に対して印加される外乱 （β−１）・Ｊ、・Ｗ’Ｊ−”　　Ｄ−Ｊ　＝の１次遅
れ推定値となっている。外乱の１次遅れ推定値に対して
、乗算器４５により適切なゲインＦを乗じ、これを、さ
らに加算器１２により制御量ｗ１に対して加算すること
で、外乱をキャンセルする構成になっている。

以上、述べてきたように本実施例によれば、制御対象で
あるロボットアームに非干渉化制御を施し、その系に対
して最適■型す−ボ系を構成する拘束条件付制御法にお
いて生ずるところの、物理パラメータの推定誤差に原因
する、不安定現象を速やかに抑圧することができる。不
安定現象が回避される結果として、追従精度を向上させ
ることができる。又、従来の制御系構成のままでは、物
理パラメータの推定を時間を掛けて精密に実施せねばな
らなかったが、本実施例の経路制御装置では、さほど、
精密な推定値を必要としない利点もある。即ち、推定に
要するコストを低減できる効果がある。更には、物理パ
ラメータの中で特に粘性係数行列は経時的に、あるいは
動作状況によって変動するが、そのような変動があって
も制御系全体の特性には影響を与えないロバストなロボ
ットアームの経路制御装置を提供できる効果がある。

本発明は°その主旨を逸脱しない範囲で種々変形が可能
である。その変形を以下に説明する。

第１図に示したロボットアームの経路制御装置は、速度
指令Ｖ、の入力が位置制御系に与える外乱を抑圧する為
に、補償器２を位置制御系内に設け、その出力に適切な
ゲインＦを乗じて、位置制御系の制御量Ｗ、に正帰還す
る構成を示したものである。換言すれば、第１図の実施
例の構成では、位置指令が目標の平衡点で零の場合であ
る。

従って、位置指令入力に零以外の指令が入力される場合
には、この指令が速度制御系に対して外乱を与えること
になる。よって、この場合には、第１図と同様に、補償
器２とゲインＦから成る構成を速度制御内に設けること
により、速度制御の不安定と発散を防止することができ
る。即ち、補償器２は、その目的に応じて、位置制御系
及び速度制御系のいずれか一方若しくは両方に設定する
ことができる。

［発明の効果］ 以上説明したように、本発明の経路制御装置の構成は、
剛物体の物理パラメータと外部からの指令とに基づいて
制御量を演算する演算部と；この制御量を前記演算部の
入力にフィードバックする前記演算部を最適制御するサ
ーボ部と；前記剛物体の速度情報と前記制御量とを入力
情報として。

前記物理パラメータの誤差を補償するための補償量を演
算して出力する補償部であって、この補償量が前記演算
部に正帰還されて、前記制御量に加算されるように、前
記演算部に接続された補償部とからなる。

そのために、前記物理パラメータの誤差が前記演算部に
外乱として働く場合に、前記補償部による正帰還はこの
外乱の応答遅れとして働き、この外乱を収束する方向に
作用する。従って、例えば、ロボットアーム等の剛物体
の物理パラメータ推定に於いて不可避な誤差が発生する
場合に、経路制御システムの不安定状態や発散状態が起
こることを防止することが可能となる。

【図面の簡単な説明】

第１図は本発明の実施例を示すロボットアームの経路制
御装置の構成図、 第２図は２リンク水平ロボツトアームの作業座標系を説
明する図、 第３図は拘束条件付制御によるロボットアームの経路制
御を実現する制御装置の構成図、第５図は粘性摩擦行列
の推定誤差Ｂを変化させたときの平衡点Ａに関する根軌
跡を示すグラフ図、 第６図は閉ループの極が不安定側にある場合を想定し、 重みＱを操作したときの根軌跡を示すグ第７Ａ図、第７
Ｂ図は経路追従の数値実験結果を示すグラフ図である。 図中、 １・・・非干渉化制御装置、２・・・補償器、９，１２
゜１４．２０，２３，２５，５３．５５・・・加算器、
１０．１５，１８，２１，２７．３０・・・積分器、１
１．１３，１６，１７，２２，２４，２６，２８．２９
，４０，４１，４５，５０，５１．５２・・・乗算器で
ある。

Claims (4)

【特許請求の範囲】
1. （１）ロボットアーム等の剛物体の駆動経路を制御する
   ための経路制御装置において、 この剛物体の物理パラメータと、外部からの指令とに基
   づいて制御量を演算する演算部と、この制御量を前記演
   算部の入力にフィードバックする前記演算部を最適制御
   するサーボ部と、前記剛物体の速度情報と前記制御量と
   を入力情報として、前記物理パラメータの誤差を補償す
   るための補償量を演算して出力する補償部であって、こ
   の補償量が前記演算部に正帰還されて、前記制御量に加
   算されるように、前記演算部に接続された補償部とから
   なる剛物体の経路制御装置。
2. （２）前記演算部は、位置制御のための第１の制御量を
   演算する第１の演算系と、速度制御のための第２の制御
   量を演算する第２の演算系と、前記第１の演算系と第２
   の演算系の一方の系が演算した制御量が前記号物体駆動
   部分の粘性摩擦係数による他方の系へ及ぼす干渉による
   干渉項を演算して入力する干渉系とを含む事を特徴とす
   る請求項の第１項に記載の剛物体の経路制御装置。
3. （３）前記サーボ部は、位置制御のために、前記第１の
   制御量を積分して該第１の制御量にフィードバックする
   第１のループとからなる第１のサーボループと、速度制
   御のために、前記第２の制御量を積分して該第２の制御
   量にフィードバックする第２のループとからなる第２の
   サーボループとを具備し、 前記演算部の第１の演算系は、前記第１の制御量を積分
   して位置情報の微分項を出力すると共に、この微分項に
   粘性摩擦係数の誤差による補正項を前記第１の制御量に
   正帰還するループとを有し、 前記干渉系は、前記干渉項を上記正帰還ループの前記第
   １の制御量に入力する事を特徴とする請求項の第２項に
   記載の剛物体の経路制御装置。
4. （４）前記補償部は、前記第１の制御量に所定の第１の
   ゲインを乗じたものと、前記微分項に所定の第２のゲイ
   ンを乗じたものを加算する演算手段と、一次応答フィル
   タ処理を施すフィルタ手段と、この一次応答出力に所定
   の第３のゲインを乗じたものを、前記第１の制御量に正
   帰還する手段とを備える事を特徴とする請求項の第３項
   に記載の剛物体の経路制御装置。