JPS60263206A - マニピユレ−タの制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〈産業上の利用分野） 本発明は非線形補償機能を有するマニピュレータの制御
装置に関する。

〈従来の技術〉 従来のマニピュレータの制御装置として、例えば特開昭
５８−１６９２０３号公報に示されるようなものがある
。

これは、移動体（ロボット）の制御特性はある範囲内で
は線形であると仮定して、姿勢・動作速度・負荷に応じ
て補償器の制御ゲインを切替えて制御する方式とし、補
償機能を向上させようとしたものである。

〈発明が解決しようとする問題点〉 しかし、一般にマニピュレータの運動は剛体と仮定して
も線形にはなり得す、またアームイナーシャの大きなマ
ニピュレータや、動作速度の速いマニピュレータについ
ては、更に非線形性が強まることから、線形近位に基づ
く制御方式では、極めてアームイナーシャが小さく、か
つ動作速度の遅いマニピュレータにしか適用できない。

そこで本発明は、アームイナーシャの大きいマニピュレ
ータでも、又高速動作を行う時でも、又アクチュエータ
パワー（モータパワー）が充分な上で重量大のワークを
把持して動作する時でも、マニピュレータの持つ強い非
線形性、軸間干渉を少なくすることができ、良好な位置
決め制御のできるマニピュレータの制御装置を提供する
ことを目的とする。

く問題点を解決するための手段と作用〉このため、本発
明では、第１図に示すように、マニピュレータＡの有す
る非線形性を補償してフィードフォワード的にマニピュ
レータＡへの制御入力を設定する非線形補償手段Ｂと、
目標値と実際値との偏差に応じたフィードハック量を設
定して前記制御入力に加算するフィードハック手段Ｃと
を設けて、マニピュレータＡを制御する一方、前記フィ
ードバンク手段Ｃによるフィードバンク量を検出し、順
次検出されるフィードハック量が減少する方向に前記非
線形補償手段Ｂの補償特性を調整する補償特性調整手段
りを設けて、補償特性を適正化する。

〈実施例〉 第２図は本発明の一実施例のシステム構成を示している
。

！　１は制御対象であるマニピュレータである。

２はフィードフォワード補償用コントローラであり、マ
ニピュレータ１の各リンクに関するジヨイントの関節角
及び関節角速度の目標値θｄ、θｄからマニピュレータ
１が有する非線形性（慣性力。

コリオリ・遠心力、粘性摩擦２重力）をフィードフォワ
ード的に補償してマニピュレータ１へのフィードフォワ
ード制御人力ＵＦＦを定め、マニピュレータグイナミソ
クスを線形化する。ここで、補償量は、予め記憶された
マニピュレータ１固有のパラメータ、すなわち各リンク
に設定された座標系から見た各リンクの重心位置Ｓｉ　
（Ｓｉ、、Ｓｉ、。

ＳＬ、）、リンク重量ｍｉ、リンク長１１．各リンクの
慣性モーメント行列Ｊ　＝　−ｄ＋ａｇ　［’　ｔｘ−
Ｉ　ｍｙ。

Ｉｔ２］　（ｉ＝１〜ｎ、ｎ：関節数）を用イ”］計）
Ｅされる。

３はフィードハックコントローラであり、関節角及び関
節角速度の目標値θｄ、θｄと実際値θ。

θとの各々の偏差に応じてフィードバンク量ＵＦＢを定
める。詳しくは、各々の偏差をにθ、に６倍し、これら
を加算して、フィードバック量ＵＦＢを定める。

そして、フィードフォワード補償用コントローラ２によ
るフィードフォワード制御人力ＵＦＦにフィードバック
コントローラ３からのフィードバック量Ｕ□を加算し、
これにより得られた制御人力Ｕをマニピュレータ１に与
えるようにし、フィードフォワード補償で補償し切れな
い部分をフィードハック制御によって目標値との偏差が
ゼロに近づくように制御している。

４はパラメータ修正装置であり、特にマニピュレータ１
のハンドがワークを把持することにより変化する制御特
性を効果的に補償しようとする場合は、ハンドと一体化
した先端リンクのパラメータ、すなわち重心位置Ｓ、、
、リンク重量ｍ７．リンク長Ｘ１１．慣性モーメントＪ
１１等について、フィードフォワード補償用コントロー
ラ２の記憶値を修正し、その補償特性を調整する。ここ
で、修正の方法は、フィードバックコントローラ３によ
るフィードバック量ＵＦＢを順次検出していて、前回の
パラメータ修正の方向とフィードバックｉｔ’ｕ　ｒ　
−の推移とから、フィードバック量Ｕ、８が減少する方
向にパラメータ値を所定値増減する。

以下では更に詳しく説明する。

よく知られているように、多関節マニピュレータは次の
（１）式の様な非線形性を有しており、高速・高精度な
動作を線形フィードバックで実現するには、この非線形
性が問題となってくる。

Ｊ（θ）　＃＋ｆｆ　（θ、／））　＋Ｖ　（θ）ｔ＋
ｇ、（θ′）−τ　・・・（１） ここに、θ−［θ１．・・・θｎ］ｔ　（θ８　：各ジ
ヨイント関節角、ｎ：ジョイント数） τ＝［τ１．・・・τｌｌ］’　（τｌ　：各ジヨイン
トへの駆動トルク） Ｊ（θ）：慣性モーメント行列 ｆ　（θ、６）：コリオリ・遠心力項 Ｖ（θ）′：各ジヨイントの粘性摩擦 ｇ（θ）：重力型 そこで、例えば次の（２）式の様にフィードフォワード
的に入力トルクを定めてやることで、マニピュレータの
非線形性をある程度取除けて、次の（３）式の様に線形
化され、線形フィードバンク制御をより効果的に行うこ
とができる。

ｒ＝Ｊ　（θ）　［Ｋ１　θ十に２　／）＋ＧＵＦＦ］
十ｆｒ　（Ｌ　／））　＋Ｖ　＜ｅ）　／）＋ｇ　＜１
））　・（２１（但し、Ｋ＋　、ＩＫｚ　、Ｇは定数行
列）θ＝に２／）＋に、θ　＋ＧＵＦＦ　・・・（３）
尚、θ、汐は予め与えられる関節角及び関節角速度の目
標値θｄ、／）ｄを用いている。

ここで、（２）式の様な複雑な計算は、予めめられた、
各リンクに設定された座標系であられされた各リンクの
重心位置Ｓｉ　＝　［Ｓｉ、、Ｓｉｙ、Ｓｉ、］　ｔ。

リンク重量ｍｉ、リンク長！、、慣性モーメント行列Ｊ
、と、リンク座標系間の変換行列とを用いて計算するこ
とができる（Ｌｕｈのアルゴリズム）。

しかし、マニピュレータがワークを把持すると、先端リ
ンクに関する前述のパラメータＳ。１ｍ７゜７！、、’
Ｊ、は変化してしまい、非線形補償の効果は劣化してし
まう。特に重量の大きいワークを把持した時には顕著と
なる。

そこで、先端リンクに関するパラメータ変化を推定し、
より正確な非線形補償を施す必要がある。

ところで、多くの場合、ワークを把持したハンドと先端
リンクを考えると、はぼリンク長方向に対し対称とみな
すことができ、以下では先端リンクに関するパラメータ
を、Ｓ、、＝　［０，０，ｓｚ］ｔ（リンク長方向にＺ
軸をとる）１ｍア、＃、、Ｊ。

−ｄｉａｇ　［’ｌ　）＋、　Ｉ　Ｉｌ＋　Ｉ　ｚ　］
　として、６つのパラメータｌ’ｎ　＋　Ｓｚ　＋　”
ｎ　＋　ＩＩＩＸＩ　Ｉ、、ｙ、ｌ１ｌｌ！　（以下６
つのパラメータをＰ（ｉ）（ｉ＝１〜６）とおく。）を
推定する。

第３図にパラメータ推定のアルゴリズムのフローチャー
トを、また第４図及び第５図にパラメータ推定の様子を
示した。これらの図をもとに説明する。

パラメータ推定の基本的な考え方は次の通りである。６
つのパラメータのうら先ず１つのパラメータｐ　（１１
に注目し、ワークを把持していない時に与えられた初期
パラメータ値Ｐ。（１）を先ずプラス方向に所定値ΔＰ
（１）だけ動かし、非線形補償した結果の制御成績、す
なわち目標値と実際値との偏差に相当するフィードバッ
ク量が前回のパラメータ値（最初の探索はＰ。（ｉ））
で同じ動作をさせた時のフィードバック量より小さくな
ったかどうかで、パラメータ探索の成功、失敗を評価す
る。成功（すなわち前回より小さくなった）ならば、同
じ方向つまりプラス方向に更にΔＰ　（ｉ）動かして行
き、探索開始で失敗したら逆方向にΔＰ　（ｉ）動かし
、成功を続けて失敗したら、前回のパラメータ値と失敗
した時のパラメータ値との間に真のパラメータ値がある
とし、例えば中間値をパラメータ推定値とし、次のパラ
メータＰ　（ｉ＋１＞の探索に移り、同様のことを繰返
す。

第４図は以上の基本的考え方に従ってパラメータ探索し
た時の制御状態のうちフィードバンク制御量ＵＦＢの状
態を時系列で示したものである。横軸ハパラメータ推定
時刻にで例えば１つの繰返し動作にかかる時間を１０分
割したものを示し、縦軸はフィードバック制御量ＵＦＢ
の絶対値を示している。時刻に＝０で静止状態から動作
を開始し、時刻に＝１０までに目標に位置決めしている
。図中Ｏ印は前回の繰返し動作中にパラメータ修正して
いる時のフィードバンク制御量Ｕ　ｒ　Ｂ　Ｔｋ）の値
を示し、Ｘ印は今回の繰返し動作中にパラメータ修正を
している時のフィードバンク制御量Ｕ　ｒ　ｍ　（ｋ）
の値を示している。

第３図の推定フローチャートと併せて推定アルゴリズム
を説明して行く。

先ず時刻に＝０であるパラメータＰ　（ｉ）に対しＰ（
１１＋　Ａ　Ｐ　（ｉ）と微動させ（第３図の７ｏ−の
３４）、他のパラメータＰＵ）（ｊｆ−ｉ）と合わせ、
非線形補償の計算（式（２）をＬｕｈのアルゴリズムで
計算）を行ってフィードフォワード制御入力ＯＦＦを設
定し出力した（Ｓ６）ものとする。

次の時刻に＝１ではフィードバンク量Ｕ□（１）を読込
み（Ｓｌ）、パラメータ修正していない時の同じ動作時
のフィードバック量ＵＦｌｌとの差ΔＵ（１）＝　ｌ　
ＵＦＩ（１）　ｌ　’　ｌ　ＵＦＩ（１１１をめ、この
ΔＵ　（１１が正か否か、つまりパラメータ修正してフ
ィードバック量（の絶対値）が減ったか否かを判定する
（Ｓ２）。

ΔＵが負であれば、パラメータ探索が失敗したとし、初
めての探索であれば微動方向を変え（８１２〜５１４）
、成功後の失敗であれば、真のパラメータＰ　（ｉｌの
値は前回の値と今回修正して失敗した値の中間にあると
し、Ｐ（１）’ＡΔＰ　（ｉ）をパラメータ推定値とし
て（Ｓ　８）　、Ｐ（ｉ）の探索を終え、他のパラメー
タＰ　（ｉｌ１）の探索に移る（Ｓ９）。

そして全ての６つのパラメータの探索が終ったか否かを
判定しく５１０）、終わったら再び最初のパラメータか
ら探索し直す（Ｓ　１１）。この理由は、パラメータ探
索中に瞬時外乱が混入しパラメータ探索が誤る可能性が
あり、常に繰返すことにより最適パラメータ推定値を得
ることができるからである。

さてＳ２でΔＵ　（１１が正の場合は、パラメータ探索
が成功している可能性があるとする。但し、前回他のパ
ラメータＰ（ｉ−１）の修正が成功していれば、ΔＵ（
１）は当然圧となるので、ΔＵ（ｋ）の正負だけで、今
回のパラメータＰ　（１１の探索が成功し゛　た七は言
えない、。

そこで、時刻に＝０のΔＵ　（０）よりΔＵ（１）が大
きければ成功、小さければ失敗とする（Ｓ３）。

失敗した時は前記のような処理（３７〜５１１）を経て
、次のパラメータＰ　（ｉｌ１）の探索に移る。

成功した時は、同じ微動方向に更にΔＰ　ｆｉ）微動さ
せ、探索を繰返す（３４〜Ｓ６）。

第４図は、時刻に＝０からパラメータＰ　（１１の探索
を開始し、時刻に＝４までのパラメータ修正は成功し、
時刻に＝５でパラメータ修正した結果、ｋ＝６で探索失
敗し、パラメータＰ　（ｋｌの推定値をＰ（ｉ）　’Ａ
ΔＰ　（１１としている。そして、次のパラメータｐ（
ｉｌ、１）の探索を開始し、時刻に＝７までのパラメー
タ修正は成功し、時刻に＝８で探索失敗して、ｐ　（ｉ
ｌ１）の推定値をＰ　（ｉｌ１）’、４Ｐ（ｉｌ１）と
してｐ　（ｉｌ１）の探索を終了している様子を表して
いる。

第５図は、このときの６つのパラメータＰ（１）（ｉ＝
１〜６）の値の変化を示している。但し、第４図のｉを
１とし、パラメータ探索したＰ　（１）とＰ（２）だけ
示し、Ｐ（３）〜Ｐ（６）は一定値なので省略した。

以上のアルゴリズムを実行するパラメータ修正装置４、
Ｌｕｈのアルゴリズムに基づくフィードフォワード補償
用コントローラ２、および位置偏差。

速度偏差に対し比例フィードバック制御するフィードバ
ンクコントローラ３などは、例えばマイクロコンピュー
タで構成することもできるし、又専用ディスクリートＩ
Ｃ等で構成することもできるが、第６図に６軸マニピユ
レータの場合でマイクロコンピュータで構成した例を示
す。

第６図において、１１はマイクロプロセッサ（ＭＰＵ）
であり、図では電源回路２発振回路、リセント回路等は
省略した。１２は６細分すなわち６つのＤ／Ａ変換器、
１３は電流アンプ、１４はＤＣモータであり、ＭＰＵＩ
Ｉからのトルク指令をＤ／Ａ変換器１２によりアナログ
電圧に変換し、電流アンプ１３を介して各軸のＤＣモー
タ１４を駆動する。１５はパルスジェネレータ、１６は
波形整形回路であり、パルスジェネレータ１５で各ジヨ
イントの角速度を検出し、波形整形回路１６を介してＭ
ＰＵＩＩに入力する。１７はアンプダウンカウンタであ
り、波形整形回路１６を介して入力される各ジヨイント
の角速度から関節角を検出し、ＭＰＵＩＩに入力する。

ここで、ＭＰＵＩＩ内部では各関節の目標関節角θ４゜
角速度θ４をソフトウェアで発生させ、フィードフォワ
ード補償計算、フィードバンク制御計算を行い、各軸の
トルク指令を出力する。

尚、この実施例では先端リンクのパラメータ推定のみ行
ったが、全てのリンクに適用し、モデル誤差を修正し、
より良い非線形補償をすることも可能であり、また各リ
ンクの慣性モーメント行列に全ての要素がパラメータと
して入る場合も適用可能である。

また、この実施例では、フィードバンク量ｔＪＦＲを直
接検出するようにしたが、目標値と実際値との偏差から
検出するようにしてもよく、フィードバックゲインが一
定でない場合はこの方がより正確な制御が可能である。

〈発明、の効果〉 以上説明したように本発明によれば、ワーク把持等によ
り変化するマニピュレータの制御特性を極めて簡単なア
ルゴリズムで推定し、これに対応した補償特性を得るよ
うにしたため、制御特性の変動にかかわらず当初狙って
いた非線形マニピュレータの線形化ができ、最適な線形
フィードバンク制御ができる。よって、マニピュレータ
の強い非線形性にもかかわらず、高速動作を実現でき、
また、未知ワークを把持してもあるいは重量大のワーク
を把持しても、繰返し位置決め精度を向上させることが
できるという効果が得られる。

【図面の簡単な説明】

第１図は本発明の構成を示すブロック図、第２図は本発
明の一実施例を示すシステム構成図、第３図はパラメー
タ推定のアルゴリズムのフローチャート、第４図及び第
５図は第３図のフローに従って推定した時の状態を示す
図、第６図はマイクロコンピュータによるハードウェア
構成例を示す図である。

Claims (1)

【特許請求の範囲】
1. マニピュレータの有する非線形性を補償してフィードフ
   ォワード的にマニピュレータへの制御入力を設定する非
   線形補償手段と、目標値と実際値との偏差に応じたフィ
   ードバンク量を設定して前記制御入力に加算するフィー
   ドバンク手段とを備え、更に、前記フィードバンク手段
   によるフィードバンク量を検出し、順次検出されるフィ
   ードバンク量が減少する方向に前記非線形補償手段の補
   償特性を調整する補償特性調整手段を備えたことを特徴
   とするマニピュレータの制御装置。