JPS607504A - アクチユエ−タ信号を補正する方法及び非線形力学制御ル−プ - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔発明の技術分野〕 本発明は、マニピュレータの非線形力学に関してアクチ
ュエータ信号を補正する方法及び非線形力学制御ループ
に関する。

〔従来技術〕

現在、世界では、プログラムされたマニピュレ−タすな
わちロボットの開発が急速に推し進められている。発明
者Ｈａ　ｈｎによる米国特許第３，９０９゜６００号に
示すような最近の工業用ロボットは、ダイナミック制御
法を用いている。しかし、このダイナミック制御法は、
様々の可動部分の複雑な非線形力学を無視しており、そ
のため最適なレスポンスが得られなかった。現在多くの
優れたダイナミック制御において計算上の問題点となっ
ていることは、所定のジヨイント速度及び角度に関して
、目標とするジヨイント加速度を発生するのに必要なア
クチュエータトルク及び／またはカを計算するアルゴリ
ズムである。

たとえば、“ｎ　”リンクマニピュレータの力学は、次
のｎ非線形微分方程式により表わされる。

Ｔ＝Ｊ（ｑ）イ＋ｆ（ｑ、ｃｘ）−４−ｇ（ｑ）　（１
）ここで、 ｑ＝ジヨイント角度（変位）のｎＸ１ベクトルＪ（ｑ）
−ｎｘＨ慣性マトリックス ｆ（ｑ、請求心及びコリオリ（Ｃｏｒｉｏｌｉｓ）　）
ルク（力）のｎＸ１ベクトル ｇ（ｑ）−重力トルク（カ）のｎＸ１ベクトルＴ−アク
チュエータトルク（力）のｎＸ１ベクトル しかし、代表的な６リンクマニピユレータにおいては、
式１の関数Ｊ、ｆ、ｇは非常に複雑な非線形式になって
しまう。このため、提案されている多くの制御法では、
方程式１をｎ個の線形微分方程式に分割している。分割
フィードバックは次のようになる。

Ｔ＝　Ｊ（（１）ｕ＋ｆ（（１，（１）＋ｇ（（１）　
（２）ここで、 Ｕ＝目標とするジヨイント加速度のｎＸ１ベクトル 式２は式１で与えられた元の系を次の線形分割系に変換
する。

弐３で示した系は、標準的な制御技術により簡単に制御
することができる。

式２は、Ｊ、ｆ、ｇがジヨイント角度により決まるので
、マニピュレータが移動する時に常に変化する。従って
、いつでも分割を行なうには、分割トルクＴｆ、実時間
で連続的に再計算し力ければなら彦い。また、望ましく
ないダイナミック作用が生じるのを避けるには、計算回
数は、１秒描９約１００回を要する。しかし、小数点の
加算及び乗算が非常に多いため、このことは制御を行カ
リ上での問題点となっている。

最近１５年間で、式２の計算を行なう多くの方法が提案
されている。これら技術の広範囲にわた１１月の第６９
頁〜７６頁 １１ｅｒｂａｃｈ　によるＩ’　Ａ　Ｒｅｃｕｒｓｉｖ
ｅ　ＬａｇａｎｇｉａｎＦｏｒｍｕｌａｔｉｏｎ　ｏｆ
　Ｍａｎｉｐｕｌａｔｏｒ　Ｄｙｎａｍｉｃａ　ｊにお
いて行なわれている。ラグランシュの方程式に基づいた
、問題の最も優れた公式化は、１９６５年８月、ノース
ウェスタン大学におけるＪ、Ｊ。

Ｕｉｃｋｅｒによる哲学学位論文ｒａｎ　ｔｈｅ　Ｄｙ
ｎａｍｉｃＡｎａｌ）’ｓｉｓ　ｏｆ　５ｐａｔｉａｌ
　Ｌｉｎｋａｇｅｓ　Ｕｓｉｎｇ　４　Ｘ４　Ｍａｔｒ
ｉｃｅａＪ及びスタッフォード大学のＡ、Ｉ。

Ｍｅｍｏ　１７７．１９６９年９月におけるＭ、Ｅ、Ｋ
ａｈｎによる［Ｔｈｅ　Ｎｅａｒ　−Ｍｉｎｉｍｕｍ　
−Ｔｉｍｅ　Ｃｏｎｔｒｏｌｏｆ　０ｐｅｎ　Ｌｏｏｐ
　Ａｒｔｉｃｕｌａｔｅｄ　Ｋｉｎｅｍａｔｉｃ　Ｃｈ
ａ−１ｎｓＪとに依存している。しかし、残念ながら、
（Ｕｉｃｋｅｒ　／　Ｋａｈｎ）公式は、代表的力６リ
ンクマニビュレータに対しては軌道点当り約１１８，０
００もの加算と乗算とを要するため、実時間の計算には
適していない。一方、１９８０年６月Ａｓ胤Ｊｏｕｒ−
Ｃｏｎｔｒｏｌ　ｒ　Ｖｏｌ、１０２　＋Ｎｎ２　＋　
第６９頁〜７６頁におけるＪ、Ｙ、Ｓ、Ｌｕｈ＋Ｍ０Ｍ
、　Ｗａｌｋｅｒ　及びＲｏＰ。

Ｃ，Ｐａｕｌによる「ｏｎ　Ｌｉｎｅ　Ｃｏｍｐｕｔａ
ｔｉｏｎａｌ　Ｓｃ−ｈｅｍｅ　ｆｏｒ　Ｍｅｃｈａｎ
ｉｃａｌ　ＭａｎｉｐｕｌａｔｏｒｓＪにおいて、ラグ
ランシュの方程式を用いて６リンクマニピユレータに関
する式２の計算時間が７．９秒かかることを示している
。しかし、これは、現在のシステムに関する０、０１秒
の目標より約７９０倍も長い。

現在知られている最も速く精密な計算法は、上記論文に
おいてＬｕｈ他により示された再帰ニュートン−オイラ
ー技術である。この方法は、マニピュレータベースから
最後のリンクに向けて、マニピュレータリンクの速度及
び加速度を先ず計算し、その後、最後のリンクからベー
スに向けて、リンクに作用するトルク及び力を計算して
いる。全計算はリンク座標で行なわれ、これにより、必
要とされる座標交換の数はかなり減少し、かつ６リンク
マニビユレータに関する演算数は１５９０に減少する。

従って、この技術は、心安な演算を行なうのに０．０３
３５秒しかかからない。Ｈｏ１ｌｅｒｂａｃｈは、Ｌｕ
ｈ他による再帰ニュートン−オイラー公式は、非常にわ
ずかな改変しかできないと結論しているが、このＨｏ１
ｌｅｒｂａｃｈの結論は、非常に一般的でかつ様々な構
成及び任意数のリンクを有するマニピュレータを操作す
ることのできる公式に対してだけ適用できる。

ここで示したダイナミックマニピュレータ制御は、はと
んどの工業用マニピュレータは幾何学的に比較的簡単で
、かつ４〜７個のリンクを有しているという事実に基づ
いて行なわれている。また高レベル言語における計算時
間は、インプリメンテーションにより決まる、Ｌｕｈ他
による技術を用いた計算時間よりも約５〜１０倍短い。

提案されている制御は、マニピュレータを２つの部分に
分けている。第１の３リンク（ベースから数える）は古
典（非−寸トリックス）ラグランシュの方程式を用いて
モデル化し、その速度はリンク座標において計算される
。残りのリンク（すなわちリスト）は再帰ニュートン−
オイラ一方程式を用いてモデル化される。

〔発明の概要〕

本発明は、非線形ダイナミック制御ループを有する複数
リンクの機械的マニピュレータすなわちロボットの制御
システムに関する。この非線形ダイナミック制御ループ
は、第１線形ダイナミツク制御ループにより発生された
信号に加算されろ補正係数を計算し、マニピュレータの
非線形力学に関してアクチュエータ信号を補正する。こ
の第１制御ループは、適当な記憶及び計算能力を有しか
つ各リンクの目標運動を表わす信号を発生する中央処理
装置と、各マニピュレータリンクを作動するアクチュエ
ータを制御するアクチュエータ信号を発生する線形ダイ
ナミック制御ループと、各マニピュレータリンクの位置
と運動に応答しかつ中央処理装置と線形ダイナミック制
御ループへのフィードバック信号を発生するセンサとを
有している。本発明は、マニピュレータの非線形力学に
関して線形制御信号を補正する非線形ダイナミック制御
ループを有し、この非線形ダイナミック制御ループは、
第１の３リンクに対してはＬａｇｒａｎｇｔａｎの力学
を用いて、かつ残りのリンクに対しては再帰ニュートン
−オイラーの力学を用いて、中央処理装置により発生さ
れた運動信号とフィートノくツク信号とに応じてマニピ
ュレータリンクを作動する各アクチュエータに関する補
正係数を発生する非線形力学制御コンピュータと、マニ
ピュレータ非線形力学に関してアクチュエータ信号を補
正するよう上記補正係数とそれに関係したアクチュエー
タ信号とを加算する装置とを有している。

このマニピュレータ制御システムの利点は、従来の線形
制御システムよりも効率が良＜、シかも本発明による非
線形力学に関する計算速度は、従来の非線形制御法より
も５〜１０倍速いということである。

〔実施例〕

以下、添付の図面に基づいて本発明の実施例に関して説
明する。

ダイナミックマニピュレータ制御は、マニピュレータを
２つの部分に分けて行なっている。第１の部分は、３つ
のリンクを有する第１の３リンクから成り、第２の部分
は、残りのリンク全部から成っている。第１図は、発明
者Ｈｏｈｎによる米国特許第３，９０９．６００号に示
したロボットアームに相当する代表的なロボットアーム
１０の第１の３リンクを示している。リンクの座標系は
、リンク３を除いては、前述したＨｏ１ｌｅｒｂａｃｈ
により示された標準規格を用いている。リンク３は、そ
の端部における座標原点と、縦軸に沿つれｚ軸とを常に
有している。第１の３リンク１，２．３は、図示のよう
にベース１８により支持されている。

第１リンク１は、垂直軸（ｚｏ）のまわりで回転できる
ようにベース１８に取りつけられてお９、第２リンク２
は、第１水平軸（ｚｌ）のまわ９で回転できるように第
１リンク１の端部に取りつけられており、さらに第３リ
ンク３は、第２水平軸（ｚ２）のまわりで回転できるよ
うにリンク２の端部に取りつけられている。リンク３の
２軸（ｚ３）は、図示するように縦軸に沿っている。Ｘ
とＹ軸は、右手の系を構成し、Ｘは鉛直面におる。この
モデルにおいては、リストが取りつけられているリンク
３の端部に作用する力及びモーメントにより、ロボット
アーム１０の２つの部分間は標準的な境界を成している
。従って、どのような第１の３リンクアームやどのタイ
プのリストにも常に適合することができる。しかし、前
述したような標準規格では、リンク３の座標がリンク３
の端部におけるジヨイントの形式及び配向によって決ま
るので、このような改変はでさない。第２図はリンク間
のジヨイントを示している。通常、ジヨイントには、ウ
ェストジヨイント１１．ショルダジョイン１−１２゜エ
ルボ−ジヨイント１３．リストジヨイント１４等がある
。

第３図は、代表的な自由リスト２０を示している。この
座標系は、リンク３の座標系である。図示のように、リ
スト２０は、リンク３の端部に取りつけられ第３リンク
の軸（Ｚ３）に関し回転ａ［能な第４リンク４と、第４
リンク４の端部に取りつけられ第４リンクの軸（ｚ４）
に関して回転可能な第５リンク５と、さらにリンク５の
軸（ｚ５）に関して回転可能なリンク６とを有している
。

リンクを作動するアクチュエータの要するりスト２０の
アクチュエータトルク（力）は、上述したようにＬｕｈ
他による再帰ニュートン−メイラ−の方程式を用いて計
算される。計算は、ベース座標〜リンク３座標に対して
行Ａわれ、その速度及び加速度は、第１の３リンクのパ
ラメータ及び変数の簡単な関数となっている。計算は、
リンク４から開始し、最後のリンクまでの速度及び加速
度を計算し、さらに最後のリンクから始まってリンク４
を作動するアクテユエーｚｔでのアクチュエータトルク
（力）の計算を行方う。大抵のマニピュレータリストは
簡単であるので、方程式は各リンクに関して明白に示す
ことができる。このよう万全式化は、一般の再帰ニュー
トン−オイラーアルゴリズムが要する演算の４０％以下
で済む。また、非線形力学に関する計算時間は、プログ
ラム実行の最適化によりさらに短縮される。第１の３リ
ンクモデル及びリストモデルは非常に簡単であるので、
これらモデルはプログラムにおいて記号形式で表わされ
る。従って、当業者には周知のように同類項や、マニピ
ユレーション量を節約する他のオペレーションを分類す
ることができる。

アクチュエータトルクの他、非線形力学制御は、リスト
２０を支持するため、リンク３の端部に要する力とモー
メントを計算する。これらの力とモーメントは、リンク
３の標準座標において常に計算される。

非線形力学制御の第２部分は、Ｌａｇｒａｎｇｉａｎ力
学を用いてリンク１，２．３に関するアクチュエータの
トルク（力）を計算する。これらトルり（力）は主に４
つの作用に起因している。

ａ、リンク１，２．３の並進慣性（Ｔｔ　）ｂ、リンク
ｌ、２．３の回転慣性（Ｔｒ）Ｃ，リンク１，２．３に
作用する重力（Ｔｇ）ｄ、リスト２０を支持するのに要
する力及びトルク（ＴＶ） アクチュエータｉに要する全トルク（Ｔｉ）は次のよう
になる。

Ｔ　ｉ　””　Ｔｔ　ｉ＋　Ｔｒ　ｉ　＋Ｔｇ　ｉ＋Ｔ
ｗｉ　（４）第１の２つの成分は、次のように計算され
枳。

ここで、 Ｖｊｐ＝Ｐ座標に沿ったリンクｊの重心の速度Ｍｊ＝リ
ンクｊの質量 ｑｉ＝リンクｉのジヨイント角度（変位）ｌ　−アクチ
ュエータ数 Ｊ　−リンク数 ｐ　＝ＸＩ７または２ ここで、ｔｌ−リンク１，２．３の回転による運動共有
エネルギ（ｃｏｅｎｅｒｇｙ）で１＋　工’　ｗ２’　
）　＋　ＩｊｘｙｗｊｘｗｊｙＪＺＺ　ＪＺ ＋　Ｉ　ｊ　ｙｚ’ＶＩ　ｊ　ｙ”Ａ’ｊ　ｚ　＋　Ｉ
　ｊｚ　ｘＷ　ｊ　ｚＷ　ｊ　ｘ　〕（７）ここで、 ｗｊｐ−Ｐ座標に関するリンクｊの角速度Ｉｊ−座標系
におけるリンクｊのａｔ　注モーメント 重力による寄与率とリス）Ｔｗによる寄与率は、調査に
より明示し得る。

ラグランシュの方程式では、上述したＬｕｈ他による方
法が必要とする演算の４０％以下で済む。

また、計算時間の節約も、プログラム実行の最適化によ
り実現し得る。第１の３リンク１，２．３に関する式は
、プログラムにおいて記号形式で表わされ、慣性力、求
心力、コリオリカ及び重力に相当する項に分類すること
ができる。リスト２０は、リンク３に与えられる力及び
モーメントにより、第１の３リンクに関する計算におい
て数値形式で表示される。第１の３リンクに関してＬａ
ｇｒ−ａｎｇｉ　ａｎ力学を使用することは、運動連鎖
を開放しないマニピュレータの制御に対して便利である
。

また、第１の３リンクモデルは、力学の基礎原理から直
接的に導びき出されかつ大抵のロボットアームに対して
一般的であるので、本発明の制御は、各種のマニピュレ
ータ構造に対して簡単に対応することができる。

完全なアルゴリズムは、Ｈｏｈｎにより示されたような
ロボットアームを使用して次の７段階により要約するこ
とができる。

１、　リンク３座標においてリストジヨイント１４に作
用する速度及び加速度■３・Ｎ３・÷３・÷３を計算す
る。

ｗ３ｘ＝′０１ｃＯＳ（θ２＋０３） れ工と↓３Ｘは、Ｌｕｈ他による上述した論説［０ｎ−
Ｌｉｎｅ　Ｃｏｍｐｕｔａｔｉｏｎａｌ　Ｓｃｈｅｍｅ
　ｆｏｒＭｅｃｈａｎｉｃａｌ　？ｖｉａｎｉｐｕｌａ
ｔｏｒｓ　Ｊに示すように計算される。ｖ３ｘと÷３ｘ
　の計算の詳細は、ｒｘｉ−ｎｅｍａｔｉｃｓ　ｏｆ　
ｔｈｅ　Ｌｉｎｋ　Ｊと題するセクション■に示されて
いる。

上記計算において、δ２はリンク２の長さであり、δ１
・θ２・θ３は各リンク２，３の測定した角速度であり
、またθｌ、θ２．θ３は第１図に示すように各リンク
１．２．３の測定角度変位である。

Ｖｓｙ　＊　？ｓｙ　＋　Ｎ３ｙ　ｌ　ＷＦｙ　ｌ　ｖ
３ｚｌ　？３ｚｌ　Ｎ３ｚ　１÷３□は同様の方法で計
算できる。

２、上述したＨｏ１ｌｅｒｂａｃｈにょる再帰ニュート
ン−オイラーのアルゴリズムを用いて、リンク３の端部
（リンク３とリンク４の境界）においてリスト２０を支
持するのに要するトルク及び力と、リンク４，５・・・
拳ｅ　ｎのアクチュエータに関するトルク（力）を計算
する。「再帰ニュートンーオイラー力学」と題するセク
ションに示されているこのアルゴリズムの要約は次のと
おりである。

ｗｌ　＝　ｗｉ−１＋ｚ　Ｈ−１θｉ 階　−↓４−１＋Ｊ−１°ｉｉ＋７３−ｘ×Ｊ−ｔ″ｏ
ｉ°ｄｉ−↓ｉ　Ｘ　ｐ　ｉ　十ｗ１　×（町×Ｐ　１
　）　十Ｐ　ｌ　−１″ｒ″ｉ　＝ｗｉＸ（ｗｌＸｒ”
ｉ）＋ｗｉｘｒ”４＋’；ｉＦ　ｉ　”　Ｍｉｒ　ｌ Ｎ１＝Ｉｉ↓１汁ｗｉ×（工１ｗ１） ｉ＝４＋Ｆｚ６ ｆｉ＝Ｆ１十ｆｌ＋１ ｎｉ　＝ｎｉ＋１＋（ｐｉ＋ｒｉ）ＸＦｉ＋Ｎｉ＋ｐｉ
Ｘｆｉ、ＨＴｉ　””　ｚｉ−１ｎｉ ｉ＝６．５．４　（８） ここで、 Ｗｉ　−リンクｉの角速度 番ｉ＝リンクｌの角加速度 Ｆｊ　＝リンクｉの全外力 Ｎｉ　＝リンクｉの全外部トルク Ｔｉ−リンクｉのアクチュエータトルク（力）Ｉｉ　−
質量の中心におけるリンクｉの慣性テンソル ｆｉ＝ミニリンクｉ−１）によりリンクｉに与えられる
力 ｒＢ　＝リンク（ｉ−１）によりリンクに与えられるモ
ーメント ｐｉ＝ミニペース原点からジヨイントｌ座標原点へのベ
クトル ｐ＊ｉ＝座標原点（ｔ−１）から原点ｉへのベクトル ｒｉ　−ベース座標原点からリンクｉの質量の中心への
ベクトル ｒｌ＝座標原点ｉからリンク１の質量の中心へのベクト
ル Ｍｌ　＝リンクｉの質量 ２　＝ジヨイント量＋１の軸に沿ったベクトル ３、式５を用いて、リンク１，２．３の並進慣性による
リンク１．２．３のアクチュエータに要するトルク（力
）Ｔｔｌを計算する。

４、弐６を用いて、リン”、？　１　、２　、３の回転
慣性によるリンク１，２．３のアクチュエータに要する
トルク（力）Ｔｒｌを計算する。

５、　リンク１，２．３に作用する重力によるリンク１
，２．３のアクチュエータに要するトルク（力）Ｔｇｉ
を計算する。

Ｔｇ３＝Ｍｓｇ　［：（ａ　、　＋８３Ｘ　）ｃｍ　（
θ２＋θ３）　Ｌ＋ｙ８１ｆｌ（θ２＋０３）〕 Ｔｇｚ　＝　Ｔｇｓ　十ｇ　［：　Ｍｓ　ｊＬ２ωＳθ
２　＋Ｍ２　［（３２十５ＺＸ）ｃＯ８０２３２ｙ８１
１１θ２〕〕 Ｔｇｘ　＝　０　（９） ここで、８ｉはリンクｉ座標におけるリンクｌの質量の
中心の位置である。

６、　リストがリンク３の端部に与える力及びモーメン
トのバランスをとるためジヨイント１１゜１２．１３の
アクチュエータに要するトルク（力）　Ｔｗｉを計算す
る。

１ｗ１ニー　Ｆ２［ａ２　ｃｏｓθ２＋ａ３ｃＯ８（θ
２＋０３）］＋ＴｘＢｉｎ（θ２＋０３）　十’ｌ’ｙ
　ｃＯ８（θｚ　＋　０３）Ｔｗｚ　＝　Ｔｚ十Ｆ＞（
ａ２ｓｉｎθｓ＋Ｆｙ（ａ３　＋Ｂ２ｃｏｓ０３）ＴＷ
３　＝Ｔ　ｚ　十Ｆｙ　８３　（”）ここで、Ｆ　ｚ　
ｒ　Ｆ　ｙ　＋　Ｆ　ｚ　＋　Ｔ　Ｘ　Ｉ　Ｔ　ｙ　＋
　Ｔ　ｚ　はリンク３及びリンク４間の境界の力及びモ
ーメントである。

７、式４を用いて、リンク１．２．３のアクチュエータ
に要する全トルク（力）Ｔｉ　を計算する。

第４図は、マニピュレータ制御装置の詳細を示したプ日
ツク図でおる。この装置の動部分は、中央処理装置（Ｃ
ＰＵ）１０２と、制御コンソール１０４と、プログラム
記憶装置１０６と、データ記憶装置１０８とから成って
いる。制御コンソール１０４は、当分野忙おいて周知の
ように、マニピュレータを動作するのに必要ガ制御装置
及びディスプレイ装置をオペレータに提供する。さらに
、装置は、マニピュレータ１００に関連したスイッチや
ソレノイド等、マシン・オリエンテッド機能に関係した
マシン人力／出力装置や他のマシンでマニピュレータの
動作を積分すなわち統合するプロセス人力／出力装置の
ような、他の通常の周辺装置を有している。

中央処理装置（ＣＰＵ）１０２の演算プログラムは、プ
ログラム記憶装置１０６に記憶さｔｌこのプログラムを
実行する際中央処理装置１０２が使用するデータは、デ
ータ記憶装置１０８に記憶されている。

データ記憶装置１０８に記憶されたデータは、計算する
際に使用する定数や、実際の位置を検出するセンサ１１
０からのフィードバックデータや、速度や、マニピュレ
ータ１００の様々なリンクの加速度の他、必要に応じて
、プログラムを実行する場合に生じた解などから成って
いる。

制御コンンール１０４からの入力に応じて、中央処理装
置１０２は、データ記憶装置１０８に記憶されたデータ
を使用してプログラム記憶装置１０６に記憶されたプロ
グラムを実行し、マニピュレータ１００の各リンクの目
標運動を表わす電気信号を発生する。

これら目標運動信号は、加算増幅器１１２と、サーボ増
幅器１１４と、電力増幅器１１６から成る通常のダイナ
ミック制御ループにより、マニピュレータの実際の運動
に変換される。加算増幅器１１２は、各リンクの実際の
位置、速度及び加速度を表わすフィードバック信号を、
関連しｆｒ、運動信号から減算シ、各マニピュレータの
リンクに関する差信号を発生する。サーボ増幅器１１４
は、目標路に沿ってマニピュレータの関連したリンクを
動かすのに必要なトルクすなわち力を表わすダイナミッ
クアクチュエータ信号に、各差信号を変換する。各サー
ボ増幅器は、マニピュレータの運動に関するリニア制御
を行なう。また、このサーボ増幅器は旨速でしかも安定
した応答を得るダイナミック補償装置を有している。

電力増幅器１１６は、アクチュエータ信号を、各アクチ
ュエータが必要とする電力レベルにまで増幅し、目標と
するトルクすなわち力を発生する。

電力増幅器１１６からの出力は、目標の運動でマニピュ
レータ１０口の各リンクを動かす各アクチュエータに送
られる。マニピュレータのアクチュエータが電気モータ
でおる場合、電力増幅器１１６の出力は、電気モータを
作動する電気信号である。また、マニピュレータのアク
チュエータが油圧または空気圧装置である場合には、電
力増幅器１１６の出力は、当分時において周知のように
、油圧丑たは空気圧信号である。

上述した制御装置は、マニピュレータ制御装Ｕ゛の現在
の技術的水準を表わしている。さらに前述したように、
これら装置において使用される制御法は、様々なｈ１動
累子の複雑な（非線形）力学を無視しているが、現在、
この方法は、最悪の状況に向けて制御法をプログラムす
ることにより、また各リンクに目標運動を与えるのに組
替な力を使用することにより種々の素子の複雑な力学を
適当に補償することができるので、満足のいくものであ
る。しかし、この方法は、マニピュレータを毒１］御す
るにはあまり有効な方法とは言えず、シカ・もレスポン
スも良くない。

マニビュレ−７１００のダイナミックレスポンスは、サ
ーボ増幅器１１４からのトルり４古号出ブｊを、種々の
運動素子の複雑（非線形）力学に関して補正する第２ダ
イナミック制御ル−プを設けることにより改善できる。

第４図において、この第２り゛イナミツク制御ループは
、マニピュレータの各１ノンクに関する補正系数を計算
する非線形力学市１ｊ御コンピュータ１１８と、サーボ
増幅器１１４からの対応するトルク信号出力に計算した
補正係数を加算する加算増幅器１２０とから成っている
。各リンクの複雑な力学に関して各アクチュエータ信号
を補正する補正係数は、あらかじめ無視されている。

非線形力学制御コンピュータ１１８は、中央処理装置１
０２により発生された目標運動信号と、センツー１１０
によし発生されたフィートノくツク信号とを受信する。

その後、非線形力学制御コンピュータ１１８は、Ｈｏ１
ｌｅｒｂａｃｈにより示されたラグランシュの方程式を
用いて、第１の３リンクに関する補正係数を計算しかつ
前述したようにＬｕｈ他により示した再帰ニュートン−
オイラーのアルゴリズムを用いて残りのリンクに関する
補正係数をＷｔ算する。続いて、各リンクに関する計算
した補正係数は、加算増幅器１２０において関連したド
ルレジ信号と加算される。加算増幅器１２０は、第５図
に関して後述するように、サーボ増幅器１１４におシす
る各サーボ増幅器にそれぞれ関連した複数の加算増幅器
から成る。

第５図は、６リンクマニピユレータ用の第２り。

イナミツク制御ループの詳細を示している。非線形力学
制御コンピュータ１１８は、１６ビツトのＩｎｔｅ１８
０８６　、　Ｍｏｔｏｒｏｌａ６８０００マイクログロ
セッサや、最近売り出されたまたは現在開発中の新しい
３２ビツトマイクロプロセツサ等、市販されているマイ
クロプロセッサである。非線形力学制御コンピュータ１
１Ｂは、中央処理装置１０２からの目標運動信号と、各
リンク１〜６にそれぞれ関連したセッサ１１０−１〜１
１０−６がら成るセンサ１１０からのフィードバック信
号とを直接的に受信する。各リンクは、当分野において
周知のように、リンクの現在位置、速度、及び／または
加速度を表わす信号を発生する１つまたは複数のセンサ
を有している。

非線形ダイナミック制御コンピュータ１１８は、目標運
動信号及びフィードバック信号に応じて各サーボ増幅器
１１４〜１〜１１４−６（リンク１〜６にそれぞれ対応
している。）からのアクチュエータ信号出力に関する補
正係数を計算する。ダイナミック制御コンピュータ１１
８からの補正係数信号出力は、図示のように各加算増幅
器１２０−１〜１２０−６（リンク１〜６にそれぞれ対
応している。）において、サーボ増幅器１１４−１〜１
１４−６により発生された各アクチュエータ信号とそれ
ぞれ加算される。加算増幅器１２０−　ｉ〜１２０−６
からの補正されたアクチュエータ信号出力は、電力増幅
器１１６−１〜１１６−６（リンク１〜６にそれぞれ対
応している。）においてそれぞれ増幅され、リンク１〜
６に関連した各アクチュエータ１１７−１〜１１７−６
を駆動する。各センサ１１０−１〜１１０−６は、各リ
ンクの現在の状態を検出し、中央処理装置１０２、加算
増幅器１１２及び非線形力学制御コンピュータ１１８に
フィードバックされるフィードバック信号を発生する。

第６図は、非線形力学制御コンピュータ１１８に関する
プログラムのフローチャートを示している。

プログラムは、ブロック１２２に示すように、中央処理
装置１０２から受信した目標運動信号とセッサ１１０−
１〜１１０−６から受信したフィードバック俗信とを記
録することにより開始する。続いてプログラムはブロッ
ク１２４に示すように、リンク３座標のリンク３の端部
における線形速度■、線形加速度や、角速度Ｗ及び角加
速度÷を計算する。Ｘ軸の線形速度■３Ｘと角速度Ｗ　
３　ｚは、次の式を用いて計算される。

ｖ３ｘ−０２Ｂ２８ｔｌ１０３＋（θ２」−０３）ａ３
　及びＷ　３ｚ−θ、　ｓｉｎ　（θ２＋０３）ここで
、θ１．θ２．θ３はリンク１，２．３の測定角速度で
ちる。ａ２はリンク２の長さである。

角度θ２と０３は、第１図に示されている。線形加速度
やは前述したように、Ｌｕｈ他によるｒｏｎＬｉｎｅ　
Ｃｏｒｎｐｕｔｅｔｉｏｎａｌ　Ｓｃｈｅｍｅ　ｆｏｒ
　Ｍｅｃｈａｎｉｃ−ａｌ　Ｍａｎｉｐｕｌａｔｏｒｓ
、　Ｊに記載されているように計算される。これら計算
手順は、ｒ　Ｋｉｎｅｍａｔｉｃａｏｆ　ｔｈｅ　Ｌｉ
ｎｋｓＪと題する５ｅｃｔｉｏｎｌにおいて詳述されて
訃り、ここでＶｉ＋、とＷｉ＋１はリンク３の端部にお
ける線形変位及び角度変位である。

同様にして、線形及び角速度及び加速度は、ｙ及び２軸
について計算される。これら計算のプログラムは、当分
野において周知であるので、これらの詳細な説明につい
ては、本発明を不明瞭なものとしないため省略する。

次に段階においてブロック１２６に示すように、再帰ニ
ュートン−オイラーの方程式を用いて、リンク３と４間
の境界におけるリスト２０を支持するのに必要な力及び
トルクと、リスト２０のリンク４〜ｎに関するアクチュ
エータ４，５・・・・・ｎに必要なトルク及び／まだは
力とを計算する。

手順の詳細は、Ｈｏ１ｌｅｒｂａｃｈによ−る「Ａ　Ｒ
ｅｃｕｒ−ｓｉｒｅ　Ｌａｇｒａｎｇｉａｎ　Ｆｏｒｍ
ｕｌａｔｉｏｎ　ｏｆ　Ｍａｎｉｐｕｌａ−ｔｏｒ　Ｄ
ｙｎａｍｉｃｓ　ａｎｄ　ａ　Ｃｏｍｐａｒａｔｉｖｅ
　５ｔｕｄｙ　ｏｆＤｙｎａｍｉｃｓ　Ｆｏｒｍｕｌａ
ｔｉｏｎ　ＣｏｎｐｌｅｘｉｔｙＪに示すものと同様で
ある。また、アルゴリズムの詳細は［再帰ニュートンー
オイラー力学」と題するセクションにおいて示されてい
る。

続いて、プログラムはブロック１２８に示すように、リ
ンク１．２．３の慣性によるトルク及び力を計算する。

さらにブロック１３０に示すようにリンク１，２゜３の
回転慣性によるトルクまたは力を計算する。

その後、ブロック１３２に示すように、重力による力を
計算する。重力による力の式は、周知であリ、特定のマ
ニピュレータ用に簡単に導き出すことができる。

この計算に続いて、ブロック１３４に示すように、リス
トがリンク３の端部に与える力及びモーメントをバラン
スするため、リンク１．２．３のアクチュエータに必要
なトルクまたは力Ｔｗｉの計算をする。これら計算は、
式１０を用いて行なわれる。

最後に、リンク１，２．３のアクチュエータに関する全
トルクまたは力を、ブロック１３６に示すように計算す
る。これらトルクは、前述したように、式４を用いて計
算される。すなわちＴｉ　＝ＴｔＩ　＋Ｔｒｉ＋Ｔｇｉ
　＋　Ｔｗ３非線形力学力学コンピューメ１１８の最後
の動作は、ブロック１３７に示すように、加算増幅器１
２〇−１〜１２０−６に補正係数信号として、計算した
トルク及び力を出力することである。加算増幅器におい
て、これらはアクチュエータ信号に加算される。

第６図のフローチャートは、ブロック１２４　、１２６
において行なわれる計算が、ブロック１２８　、１３０
゜１３２において行なわれる計算とは独立しており、指
示したシーケンスで行なわ々くてもよいことを示してい
る。また、ブロック１２４　、１２６の計算に必要な計
算時間は、ブロック１２８　、１３０　、１３２の計算
に必要な計算時間とほぼ同じである。従って、これら計
算を行なう計算時間は、第７図に示すマイクロプロセッ
サ１３８及びマイクロプロセッサ１４０のような２つの
マイクロプロセラサラ用い、ブロック１２８　、１３０
　、１３２の計算と並列してブロック１２４　、１２６
の計算を独立して行なうことによシ、半分にすることが
できる。

第７図において、マイクロプロセッサ１３Ｂは、ブロッ
ク１２４　、１２６の再帰ニュートン−オイラーの方程
式計算を用いてリストのアクチュエータの、非線形力学
に関する補正係数を計算し、マイクロプロセッサ１４０
は、ブロック１２８〜１３２のラグランシュの方程式計
算を用いてアームアクチュエータ（リンク１．２．３の
アクチュエータ）の非線形力学に関する補正係数を計算
する。次に、ブロック１３４　、１３６の計算は、ブロ
ック１２８　、１３０　。

１３２において行なわれた計算結果と組み合わせた、マ
イクロプロセッサ１３８におけるブロック１２６の計算
結果を用いて、マイクロプロセッサ１４０ニおいて行な
われる。

マイクロプロセッサ１３８によシ計算されたリストの補
正係数は、加算増幅器１２０−　Ｂとして示された加算
増幅器１２０−４　、１２０−５　、・１１・　１２０
−ｎ等の被数の加算増幅器により、サーボ増幅器１１４
−　Ｂとして示されたサーボ増幅器１１４−４゜１１４
−５．・・・・・　１１４−ｎからの出力に加算される
。各加算増幅器１２０−Ｈの出力は、リンク４．５・・
・・・　ｎ　に関連したアクチュエータ１４２を作動す
る複数の電力増幅器１１６−　Ｂによシそれぞれ」曽幅
される。リンク４，５・・・・１１　ｎに関するセンサ
１１０−　Ｂは、中央処理装置１０２とダイナミック制
御コンピュータ１１Ｂに送る位置及び速度フィードバッ
ク信号を発生する。

同様にして、マイクロプロセッサ１４０により発生され
た、第１の３リンク１，２．３に関する補正係数は、加
算増幅器１２０−　Ａとして示された加算増幅器１２０
−１　、１２０−２　、１２０−３により、サーボ増幅
器１１４−Ａとして示されたサーボ増幅器１１４−１　
、１１４−２　、１１４−３からの出力に加算される。

加算された信号は、アクチュエータ１４４として示され
た、リンク１．２．３のアクチュエータを作動する、電
力増幅器１１６−Ａとして示された各電力増幅器１１６
−１　、１１６−２　、１１６−３において増幅される
。センサ１１０−Ａとして示されたセンサ１１０−１　
、１１０−２　、１１０−３は、前述したように、中央
処理装置とダイナミック制御コンピュータへの位置及び
速度フィートノ（ツク信号を発生する。

第８図は、各マイクロプロセッサ１３８　、１４０によ
シ実行される第６図の７０−チャートに示されたプログ
ラムの一部を示している。第８図に分いて、マイクロプ
ロセッサ１３８は、ブロック１２４゜１２６に関して前
述したように、再帰ニュートン−オイラ一方程式を用い
て、リンク４　、５　＠＠ａ＊＊ｎのトルク及び力を計
算する。ブロック１２６において計算されたリンク４，
５・・拳・・　ｎのトルクは、マイクロプロセッサ１４
０に送られ、ブロック１４６で示すようにリンク１，２
．３のトルクの計算を完了する。また、ブロック１２６
において計算されたトルクは、サーボ増幅器１１４−４
　、１１４−５争・・φ・１１４−　ｎからのアクチュ
エータ信号出力の補正係数として加算増幅器１２０−４
　、１２０−５・・・・１２０−ｎに送られる。同時に
並列して、マイクロプロセッサ１４０は、ブロック１２
８　、１３０　、１３２に示すようにラクランジュの方
程式を用いて並進慣性９回転慣性及び重力のバランスを
とるため、す／り１，２．３のトルクを計算する。その
後、マイクロプロセッサ１３８からの、リンク４，５−
・・・・ｎの計算したトルク及び力は、マイクロプロセ
ッサ１４０に送られ、ここで、ブロック１３４に示すよ
うにリンク３の端部に作用するリンク４，５・・・・１
１　ｎの力のバランスをとるのに必要なトルク及び／ま
たは力を計算する。最後に、リンクド２．３に作用する
全部の力は加算され、各リンクの補正係数として出力さ
れる、リンク１．２．３に作用する非線形トルクを発生
する。

この第２ダイナミツク制御ループを用いて、マニピュレ
ータ制御システムは、さらに精密で有効な方法でマニピ
ュレータシステムの非線形力学に関して補償することが
できる。また、マニピュレータのリンクを２つの組に分
け、かつ第３リンク座標の原点をその端部に配置するこ
とにより、リスト２０から成るリンク４，５・・・・・
ｎに作用する非線形の力は再帰ニュートン−オイラーの
方程式を用いて有効に計算することができ、またリンク
１．２．３に作用する非線形トルクはＬａｇｒａｎｇｉ
ａｎ力学を用いて計算することができる。この非線形ダ
イナミック制御システムは、所要計算時間を減少して、
Ｌｕｈ他により示したようなニュートンーオイラー力学
を用いた場合より約５倍速い０．００３〜０．００４秒
の範囲内で各リンクの所定の補正係数を計算することが
できる。さらに、非線形ダイナミック制御コンピュータ
に２つのマイクロプロセッサを使用することにより、計
算時間は係数約２により減少され、ダイナミックマニピ
ュレーク制御システムは従来の約１０倍速くかつ目標の
０．０１秒以内に入る。

なお、本発明は、ここに開示した実施例−やアルゴリズ
ムに限定されず、本発明の思想に基づいて改変できるこ
とは当業者には明白であろう。

【図面の簡単な説明】 第１図は代表的なマニピュレータの第１の３リンクの概
要図、第２図は各接合点を示したマニピュレータの第１
の３リンクのｇ要因、第３図は代表的ナマニピュレータ
のリストの概要図、第４図はマニピュレータ制御システ
ムのブロック図、第５ＩＡは非線形ダイナミック制御装
装置の詳細を示したブロック図、第６図は非線形力学制
御コンピュータのフローチャートを示す図、第７図は非
線形力学制御コンピュータに２つのマイクロプロセッサ
を用いた非線形力学制御システムのブロック図、第８図
は第７図の非線形ダイナミック制御ループの２つのマイ
クロプロセッサ間の計算の分割を示したフローチャート
を示す図である。 １．２．３．４．５　・・−−リンク、１０・拳・・ロ
ボットアーム、２０・・・・リスト、１００−・・・マ
ニピュレータ、１０２・・・・中央処理装置（ＣＰＵ）
、１０４・・−・制御コンンール、１０６Φ・Φ・プロ
グラム記憶装置、１０８・・・・データ記憶装置、１１
０１１・・・センサ、１１２・・−・加算増幅器、１１
４・・・・サーボ増幅器、１１６・・・・電力増幅器、
１１８・・・・非線形力学制御コンピュータ、１３Ｂ　
、　１４０・・・・マイクロプロセッサ、１４２　、１
４４・・・・アクチュエータリンク。 特許出願人　ザ・ペンデイツクス弓−ボレーション代理
人山川政樹（ほか２名）

Claims (1)

1. 【特許請求の範囲】 （１）各リンクの目標運動を表わす運動信号を発生しか
   つ適当な記憶及び計算能力を有する中央処理装置（１０
   ２）と、目標運動に関して各リンクを動かすアクチュエ
   ータを作動するアクチュエータ信号を発生する線形ダイ
   ナミック制御ループ（１１２。 １１４　、１１６　）と、上記中央処理装置（１０２）
   および上記線形ダイナミック制御ループへのフィートノ
   くツク信号を発生する、上記リンクの位置及び運動を検
   出するセンサ（１１０）とを有する、複数リンクマニピ
   ュレータ（１０Ｇ）の制御システムにおいて、マニピュ
   レータの非線形力学に関して上記アクチュエータ信号を
   補正する方法であって；複数リンクマニピュレータの複
   数リンクを、所定数のリンク（１，２，３）から成る第
   １組と少なくとも第４リンクを含む残りのリンクから成
   る第２組とに分割する過程と；再帰ニュートン−オイラ
   ー力学を用いて上記目標運動信号と上記フィードバック
   信号に応じて上記残りのリンクに関する第１組の補正係
   数を計算する過程（１２４、１２６）と；　Ｌａｇｒａ
   ｎ−ｇｉａｎ力学を用いて上記目標運動信号と上記フィ
   ードバック信号と上記第１組の補正係数とに応じて上記
   所定数のリンクに関する第２組の補正係数を計算する過
   程（１２８、１３０、１３２、１３４、１３６）ト；マ
   ニピュレータリンクの非線形力学に関して補正された補
   正アクチュエータ信号を発生するよう、上記第１組の補
   正係数と上記第２組の補正係数をそれらに関するアクチ
   ュエータ信号に加算する過程（１２０）と；それぞれの
   リンクを目標運動で動かすよう上記補正アクチュエータ
   信号でマニピュレータの各アクチュエータを作動する過
   程とから成ることを特徴とするマニピュレータの非線形
   力学に関してアクチュエータ信号を補正する方法。 （２、特許請求の範囲第１項記載の方法において、第１
   組の補正係数を計算する過程（１２４、１２６）は、目
   標運動及びフィードバック信号に応じて、第１組及び第
   ２組のリンク間の境界における線形速度及び線形加速度
   とを計算する過程（１２４）と；上記目標運動とフィー
   ドバック信号に応じて上記第１組及び第２組のリンク間
   の境界における回転速度及び回転加速度を計算する過程
   （１２４）と；再帰＝ユートンーオイラーカ学を用いて
   上記目標運動信号とフィードバック信号と上記コンピュ
   ータ線形及び回転速度及び加速度とに応じて残りのリン
   クのアクチュエータが必要とする非線形トルク及び力を
   計算する過程（１２６）とから成ることを特徴とする方
   法。 （３）特許請求の範囲第２項記載の方法において、第１
   組のリンクは、ベースから始まる第１の３リンクから成
   り、再帰ユユートンーオイラーカ学を用いて非線形トル
   ク及び力を計算する過程（１２６）はｉ＝４〜ｉ＝ｎの
   順方向に次の方程式、すなわちＷ”Ｗｉ−１＋ｚｉ−１
   θｉ Ｗｉ　”Ｗｉ−１＋Ｚｉ−１１１＋Ｗｉ−ＩＸＺｉ−１
   （１ｐｉ　−’−Ｗｉ　Ｘｐｉ　＋Ｗｉ　Ｘ　（ｗｌｘ
   ｐｌ）十Ｉ）１−１”ｒ　ｉ　””　ｗｉＸ（Ｖｌｒｉ
   　Ｘｒ”ｉ　）十’ＷｉＸｒ”ｉ＋”ｐｉＦｌ−Ｍｉ′
   ｒ″ｌ Ｎｉ　””　ｌ１ＶＬｌ’ｉ＋ＮＶｉＸ（Ｉｉｗｉ）を
   解き、かつリンク゛ｎ“′〜リンク４まで逆方向に次の
   方程式、すなわち、 ｆｉ　＝　Ｆｉ＋ｆｉ＋１ ｎｌ　＝　ｎ１Ｌｔ＋（ｐｉ＋ｒｉ　）ＸＦｉ＋Ｎｉ＋
   Ｉ）ｉＸｆｉ　＋−ＩＴｉ　＝　ｚｌ−＋ｎｉ ここで、 Ｗｉ　−リンクｉの角速度 ÷ｌ　−リンクｉの角加速度 Ｆｉ　−リンクｉの全外力 Ｎ１−リンクｉの全外部トルク Ｔ１＝リンクｉのアクチュエータトルク（力）Ｉｉ　−
   質量の中心におけるリンクｉの慣性テンソル ｆｌ　−リンク（ｉ−１）がリンクｉに及ぼす力ｎｉ−
   ミーリンク−１）がリンクｉに及ぼすモーメント ｐｉ＝ミニベース原点からジヨイントｉ座標原点へのベ
   クトル ｐｉ　−座標原点（ｉ−１）から原点ｉへのベクトル ｒｉ＝ヘース座標原点から質量のリンクｌ中心へのベク
   トル ｒｉ　−座標原点から質量のリンクｌ中心へのベクトル Ｍｉ　＝リンクｉの質量 ２　＝ジョイン）ｉ＋１の軸に沿ったベクトルを解くこ
   とを特徴とする方法。 （４）特許請求の範囲第３項記載の方法において、第２
   組の補正係数を計算する過程（１２８、１３０、１３２
   ゜１３４　、１３６　）は、目標運動及びフィードバッ
   ク信号に応じて慣性力のバランスをとるよう、第１の３
   リンク（１，２，３）に伴うアクチュエータに関する非
   線形トルク（Ｔｔｌ）を計ｑ、する過程（１２８）と；
   上記目標運動及びフィードバック信号に応じて回転慣性
   力のバランスをとるよう、上記第１の３リンク（１，２
   ，３）に伴うアクチュエータに関する非線形トルク（Ｔ
   ｒｉ）を計算する過程（１３０）と；第３リンクの端部
   に作用する残りのリンクによるトルクと力のバランスを
   とりかつ上記目標運動とフィードバック信号に応じて重
   力によるトルクと力のバランスをとるよう、第１の３リ
   ンク（１，２゜３）に伴うアクチュエータに関する非線
   形トルク（Ｔｇ、）を計算する過程（１３２）と；上記
   目標運動信号とフィードバック信号と上記残シのリンク
   に関して発生された補正信号とに応じて第３リンクの端
   部に作用する残りのリンクによるトルク及び力のバラン
   スをとるよう、第１の３リンク（１，２゜３）に伴うア
   クチュエータに関するトルク（Ｔ＝ｉ）を計算する過程
   （１３４）と；上記第１の３リンク（ｊ　、２．３　）
   に関する上記補正係数を発生ずるよう上記トルクＴｔｉ
   　、Ｔｒｉ　、Ｔｇｉ　、Ｔｗｉ　を加算する過程（１
   ３６）とから成ることを特徴とする方法。 （５）特許請求の範囲第２項記載の方法において、第１
   組の補正係数を計算する過程（１２４、１２６）は、第
   １マイクロプロセツサ（１３８）において計算され、か
   つ第２組の補正係数を計算する過程の第１の３つの過程
   （１２８、１３０、１３２）は、第１組の補正係数の計
   算と平行して第２マイクロプロセツｔ（１４ｏ）におい
   て計算され、さらに、第２組の補正係数を計算する過程
   の残りの２つの過程（１３４、１３６）は、上記第１及
   び第２マイクロプロセツサの１つにおいてその後計算さ
   れ、上記残りの２つの過程は上記第２マイクロプロセツ
   サ（１４０）において計算されることを特徴とする方法
   。 （６）複数リンクマニピピュレ−１（１００）　、！ｌ
   −１？ニピュレータリンクの目標運動を表わす運動信号
   を発生する適当な記憶及び計算装置を有する中央処理装
   置（１０２）と、各マニピュレータリンクを動かすアク
   チュエータを制御するアクチュエータ信号を発生する線
   形ダイナミック制御ループ（１１２，１１４゜１１６）
   と、中央処理装置（１０２）と線形ダイナミック制御ル
   ープへのフィードバック信号を発生する上記マニピュレ
   ータリンクの位置及び運動に応答するセンサ（１，１０
   ）とから成るマニピュレータ制御システムにおいて、マ
   ニピュータの非線形力学に関してアクチュエータ信号を
   補正する非線形力学制御ループであって；上記中央処理
   装ｆ！（１０２）により発生された運動信号と、フィー
   ドバラ２４１■号とに応じてマニピュレータリンクを動
   かす各アクチュエータに関する補正係数を発生し、がっ
   Ｌａ−ｇｒａｎｇｉａｎ力学を用いて所定数のマニピュ
   レータリンクに関する上記補正係数を計算しがっ再、ｌ
   、；、ｌニュートンーオイラー力学を用いて残りのマニ
   ピュレータリンクに関する補正係数を計算する非ｒ足形
   力学制御コンピュータ装Ｅ、　（１１Ｂ）　ト；マニピ
   ュレータ（１００）の非線形力学に関して補正された補
   正アクチュエータ信号を発生するよう上記補正係数を関
   連したアクチュエータ（Ｈ−号にそれぞれ加算する装置
   （１２０）とから成ること全特徴とする非１１！形力学
   制御ループ。 （７）特許請求の範囲第６項記載の非腺形力孝制イイ（
   １ルーズにおいて、へ所定数のマニピュレータリンクは
   、ペース（１８）から始ま暮第１の３リンク（１、２゜
   、３）であることを特徴とする非線形カ学制御ルーグ。 （８）特許請求の範囲第７項記載の非線形力学制御ルー
   だにおいて、非線形力学制御コンピユータ（１１８）は
   、第３リンク座標で、第３リンクの端部における線形速
   度と線形加速度上角速度と角加速度を計算する第１装置
   （１２４）と；再帰ニュートン−オイラー力学を用いて
   残りのリンクに関する補正係数を計算する第２装置（１
   ２６）と：第１の３リンクに関する並進慣性力のバラン
   スをとるようトルクを計算する第３装置（１２Ｂ）と；
   上記第１の３リンクに関する回転慣性力のバランスをと
   るのに必要ガトルクを計算する第４装置ｃ１３０）と；
   −上記第１の３リンクに関する重力のバランスをとるの
   に必要なトルクを計算する第５装［（１３２）と；上記
   残りのリンクにより上記第３リンクの端部に与えられる
   トルク及び力による上記第１の３リンクのトルクを削η
   ］する第６装置（１３４）と；上記第１の３リンクに関
   する補正係数を発生するよう上記第３〜第６装置により
   計算されたトルクを加算する第７装置ｉＭ：　（１３６
   ）とから成ることを特徴とする非線形力学制御ループ。 （９）特許請求の範囲第８項記載の非線形力学制御ルー
   プにおいて、第３リンク（３）の座標は、第３リンクの
   端部に原点を有していることを特徴とする非線形力学制
   御ループ。 αＱ特許請求の範囲第８項記載の非線形力学制御ループ
   において、非線形力学コンビエ７り（１１Ｂ）ｉｄ：、
   第１及び第２装置から成るｍ１マイクロプロセツサ（１
   ４０）と；第３〜第７装置から成る第２マイクログロセ
   ツサ（１３８）と：上記第１マイクロプロセツザにより
   計算された補正係数を上記第２マイクロプロセツサに送
   る装置（１４６）とから成ることを特徴とする非線形力
   学制御ループ。