JPS5822687A - 多自由度運動機構の制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 本発明は、産業用ロボットのような複数の動作軸を結合
した多自由縦運動機構の高速かつ高精度な軌道制御方式
に属するものである。

従来この種の制御方式としては、予め記憶しである位置
指令値を所定の時間間隔で順次指令することに工ってサ
ーボモータを駆動し、各動作軸毎に、その現在位置との
位置偏差を減少するように速変および位置のフィードバ
ック制御を行なうことに；す、装置の先端を目的軌跡に
沿って連続的に制御して行くものがある。この方式は、
制御系が各動作軸毎に構成されているため、制御装置の
構成が簡単になるという利点を有しているが、高速で大
変位の移動を伴う動作をさせる際には、各動作軸相互間
の干渉による負荷変動が生じ九り。

遠心カヤ重力などの影響で蛇行や行き過ぎなどが生じ、
高速、馬槽寂な軌道制御は出麹である。

これに対し、所定の初期状■から終端状態へ高速で移動
させるための指令値を、予め各動作軸相互間の干渉、遠
心力１重力等の影響にＬる変動トルク分を考慮して求め
、これを公称指令値として記憶要素に記憶すると同時に
、この公称指令値による運動軌跡を公称軌道として記憶
しておき、所定の時間間隔で公称指令値を次々に出力し
、各動作軸の位置と速変の現在値との偏差を減少させる
ようにフィードバック制御を行なうものがある。

この方式は、所定の初期状態から終端状態への軌道制御
が高速、高精度で行なえる九め、限定された軌跡の軌道
制御には有効であるが、反面、多数の目標位置がある場
合には、その各場合について公称軌道、公称指令値を記
憶させることは、記憶要素が大形かつ高価となって実現
出麹である。Ｊまた、装置の先端の位置が予めすべて定
まっている場合にしか適用できない。

本発明は、従来技術のこれらの欠点を解決するためにな
されたものであり、その目的は、動作すべき初期状態と
終端状態とが公称軌道の中に含まれていなくても、高速
かつ高精度な軌道制御を行なうことが可能な多自由度運
動機構の制御方式を提供することにある。

このような目的を連成するために、本発明は。

動作すべき初期状態と終端状ａＫ対し、複数の公称軌道
の中から近いものを選択し、選択した公称軌道の初期状
態および終端状１と動作すべき初期状態および終端状態
との差から補正軌道を求め、この補正軌道と公称軌道と
から計画軌道を演算し。

またその動作時間を、入力に対するマージンの最も厳し
い動作軸を基準として設定するものである。

以下、実櫓例を用い七本発明の詳細な説明する。

第１図は、３自由変を有する運動機構の一例を示す構成
図である。同図において、１．２．３で示す各動作軸が
、それぞれ矢印で示す方向に動作することにニジ、先端
４を、初期状ｌｌＳから終端状１６まで連続軌道７に沿
って移動させる。こ０１うな多自由ば運動機構の運動方
程式は、各動作軸１．２．３の変位量を１１（Ｕｓθ２
（ｋ）　＊　ｔ３（ｔ）とし、先端４の変位量を＃（ｔ
）コ〔θ１（ｔ）ｌθ２（ｔ）。

＃３（ｔ）　〕’　　として１次のように示される。

Ｊ（ａ（ｔ））＠ｊ（ｔ）＋ＩＰ（＃（ｔ）、ミ（ｔ）
、冨）＝ｕ（ｔ）・・・・・（１）式 ここで、　Ｊ（＄（ｔ））Ｈθ（１）値によって変化す
る（３Ｘ３）の慣性行列、　Ｆ（ｔ（ｔ）　；θ（ｔ）
：ｇ）はθ（１）　　とその時間変化車０（ｔ）および
重力加速度ｔＫよって変化する（３Ｘ１　）のベクトル
で、遠心カヤコリオリの力および重力の影響によって生
じる変動トルク、Ｕ（ｔ）は、各動作軸を駆動するサー
ボモータの入力をＵｌ　（ｔ）　＊　Ｕｚ　（ｔ）　＊
　Ｕｓ　（ｔ）としてυ（ｔ）冨（Ｕｉ　（ｔ）　＊　
Ｕ２（ｔ）　＊　Ｕ３（ｔ））”で示されるベクトルで
ある。

ように愛換できる。

ｘ　（ｔ）　＝ｆ　（Ｘ　（ｔ）　ｓ　ｕ　（ｔ）　）
・・・・・（２）式 第２図は、本発明を第１図の運動機構に適用した場合の
一実施例を示すブロック図である。同図において、８は
記憶装置、９は演算要素、１０はＤ直変換要素、１１は
電力増幅器、１２はテーボモータ、１３は位置検出要素
、１４は波形整形要素、１！Ｓは位置演算要素、１６は
速変演算要素、１Ｔはタイマである。

ここで、記憶要素８には、先端４が、代表的な１　　　
・１　　　・１ 初期状１Ｘ　！　＝　（θｌ！９０２Ｘ　ｅ　’３１　
＋　’ＩＸ　＊θ２！書・１ θ３ｘ）？　　と終端状１１ｘｔ−（’ｈｔｖθ２１．
θ３？ｓθ１ｙ　＊　’２ｙ　＊　’３ｔ）’に対して
高速に駆動されるように、予め指令値を求めてこれを公
称指令値ＴＪｏｙ（１）として記憶させると共に、この
公称指令値Ｕｏ、（ｔ）によって実現される軌道と動作
時間とを。

それぞれ公称軌道０゜、（１）　、θ。ν（ｔ）、θ。

、（ｔ）（もしくはＸ。？（１）と’　ｏｒ（’））と
公称動作時間Ｔｏｙ　として記憶させておく。ここでは
ｌ−１〜１０として、各１について、サーボモータに加
える入力エネルギの総和 ”１　”　４”　Ｕ”（ｔ）Ｕ（Ｄ　ｄｔ　　　　１１
　＠　＠　１１１１　（３）式を最小にし、かつ各サー
ボモータの許容最大入力Ｕｊｒａａｘ（ｊ＝１〜３）以
下となるように、公称指令値Ｕｏｒ（ｔ）と公称動作時
間Ｔ、Ｐを予め計算して記憶させておく。この求解は、
例えば共役勾配法等の繰返し計算に１って実現できる。

記憶要素８には更に、フィードバックゲイン行列に’（
３Ｘ３行列）の各要素の値を各ｉに対応させて記憶させ
ておく。Ｋ　の各要素の値は、外乱等に対抗して公称軌
道ｘｏＰ（ｔ　）を実現し得るように設定しておく。

これに対し、演算要素９は以下のステップで順次演算を
行なう。

第１ステツプ 装置の先ｙｓ４の現在の状節を初期状態！工＝（＃１エ
　１　’２１　？　θ３エ　、θ１工、θ２．．θ３工
）？とじ、外部工す指示される目標とする状態を終端状
１１ｘｔ＝（θＩＴ　　Ｉ　　’２Ｔ　　ｓ　　θ３テ
　、　θ１！　、　θ２！１θ、？）？　として、複数
の公称軌道の中から、各動作軸における初期状帽の距惜
と終端状態の距−の加算値、即ち、 ＋Ｉ’ｊｔ−θＪ　ｒ　Ｉ　＋　ｌθＪ？−θｊ！１）
・・（４）式が最小となるような公称軌道を選択し、こ
の公弥軌１およびそれに対応する公称指令値、公称動作
時間、フィードバックゲインをそれぞれＸ。ｒ（ｔ）　
１Ｕｏｙ（ｔ）　＊　Ｔｏｙ（ｔ）　＊　Ｋとする。

第２ステツプ 各動作軸毎に、公称指令値の絶対値の最大値とサーボモ
ータに許容される最大入力値σＪｍａｘとの比に、各軸
の動作すべき移動量と第１ステツプで選択した公称軌道
の場合の移動量との比を乗じ。

その積の最大値の平方根に更に公称動作時間Ｔｏ。

を乗じて動作時間Ｔｇｏｙを演算する。

（ｊ；１〜３）　　　・・６５）式 α＝ｍａｘ〔βｌ、β２．β３〕　　　　・・・・・（
６）式Ｔ、０Ｐ＝ｖＴＴｏν・・・・・（７）式％式％ 目標とする初期状１１Ｘｚと選択し九公称軌道の初期状
ＩＩＩ　ｘ　ｘとの差と、目標とする終端状ＩＩ　ｘ　
ｔと前記公称軌道の終端状Ｉｔ　Ｘ　ｔとの差とから、
各動作軸毎に、時間の３次多項式で示されるものとし九
場合の補正軌道の係数ａＪｊ（Ａｗｌ〜４）を演算する
。即ち、第１軸の補正軌道ノ＃ｊ（ｔ）、ノ＃ｊ（ｔ）
。

ノ＄ｊ（ｔ）は次式で示されるものとし、・・・（８）
式 ％式％） ） ＝＃ｊチーθｊ、から演算する。

第４ステツプ 時刻ｔ：：ｔｋの時に、修正軌道ノ１（ｔｋ）−（ｌθ
１（ｔｓｃ）ｓノボｚ（ｔｋ）１　ｄｌ？３（ｔｋ））
”　　Ｉ　ｊ；（ｔｋ）−（）−１（ｔｋ）ｓ　ノ”ｚ
（ｔｋ）＊　Δみｓ（’ｍ））”　　＊　ｉ″１（ｔｋ
）−（パ１（ｔｋ）ｓノ”＃ｚ（ｔｋ）　、　Ｉ″１ｊ
３（ｔｋ））”　　を演算する。

第５ステツプ 時刻ｔｍｔｋの時に、公称軌道の動作時間を公称動作時
間ＴＯＰから動作時間Ｔｌ０Ｆに変更させる修正公称軌
道＃ｏｙ（ｔｋ）　ｓ　ｌ’ｏｙ（ｔｋ）　ｅθ０Ｆ（
ｔｋ）を演算する。

第６ステツプ 補正軌道と修正公称軌道とを加算して計画軌道−１゜ｙ
（ｔｉｃ）　＊　’ｓ。ｙ（ｔｋ）　＊　＄＊ｏｙ（ｉ
ｋ）を演算する。

第７ステツプ 上記計画軌道を実現する計ｉＩｉ指令値Ｕｓｅｒ（ｔｋ
）を、（１）式から演算する。

第８ステツプ 位置シよび連間の現在値と計画軌道との偏差にフィート
°バックゲインを乗じたものに上記計画指令値を加算し
てＤＡ変換要素１０に出力する。

以上、演算要素９は、第１〜８ステツプをディジタル演
算し、その出力はＤＡＩＩ”換要素１０によりアナログ
信号に変換される。次いで、このアナログ信号は電力増
幅器１１で増幅され、サーボ毫−タ１２に印加される。

サーボモータ１２が駆動されると、位置検出要１１３の
出力波形が波形整形要素１４によってパルス化され、こ
れに基いて位置演算要素１５が位置θ（１）の現在値を
、また速縦演算要素１６が速［＃（１）の現在値を算出
する。これらの位置θ（１）と連間θ（１）とは演算要
素ＩＫ大入力れ、演算要素９は、タイマ１７に設定され
た所定の周期毎に、前記第４ステツプから第８ステツプ
までを繰返し演算してその結果をＤム変換要素１０に出
力する。

以上述べたような演算ステップを瑠むことにより、予め
定めた公称軌道の初期値、終端値以外の初咽値９Ｍ端直
についても、太刀エネルギがほぼ最小に表ると同時に高
速の動作が可能な軌道および指令値が実時間で求められ
る。

第３図は１本発明を第１図の運動機構に適用した場合の
池の実Ｍ１例を示すブロック図である。同図において、
記憶要素１．ＤＡ変換要票１０．電力増幅器１１１丈−
ボモータ１２１位置検出要素１３％波形整形要＄１４．
位置演算要素１５．連間演算要素１６、およびタイ−１
１１はそれぞれ第２図に示したものと同一の構成および
機能を有している。これに対し、演算要素−は、第２図
に示したものとは異なる次のようなステップで演算を行
ない、その結果を記憶装！１１１およびＤ入変換要素に
出力する。

第１ステツプ 装置の先端４の現在の状態を初期状＊ｘｘとし、外部ニ
ジ指示される１橿とする状態を終端状Ｍｓｘｔとし、予
め定めた複数の公称軌道の中から、初期状轢の距虐と終
端状態の距離との加算値、即ち。

ＰＩ３＝ｌＩＸ［ｘｚｌｌ＋１ｌｘｒ　　ＸＴＩＩ　　
・・・・＊（Ｉｌ１式が最小となる公称軌道を１択する
。

第２ステツプ 第イステップで１択した公称軌道Ｘｏｙ（ｔ）お１びこ
れに対応する公称指令値Ｕｏ　ｐ　（ｔ）のまわりで前
記（乃式を線形化し、１凛とする初間状ｌ！１１ＩＸ工
と選択し九公称軌、纏の初期状慢Ｘｚとの差７ｘニーｘ
Ｉ−ｘＩ　ｓ　　お工び１凛とする終端状ｓｘｔと前記
公称軌道との差ΔＸ７二［７Ｘ↑を初期条件および終端
条件として、エネルギが最小となる補正軌道Δｘ（ｔ）
を演算する。即ち、上記公称値のまわりで線形化した系
の運動方程式は次のＬうに示される。

Δｘｃｔ）　＝　ｋ（ｔ）Δｘ（ｔ）　＋　Ｂ（ｔ）Δ
ｕ（ｔ）　　　−−−−−ｕ’Ｊ式そこで、評価関数 テＯν　　　！ ＰＩ３−、／’　　　ｌｕ　（ｔ）ｊｕ（ｔ）ｄｔ　　
　ｅａｓｅｓ（１３式を最小ＫＬ、かつ上述した初期条
件、終端条件を満たす）ｕ（ｔ）を演算する。その厳密
解は。

ハ（ｓ）　＝　−Ｂ’（ｔ）−−（Ｔｏｐ　＊　ｔ　）
　・Ｇ　（０＊　Ｔｏｙ）−”（Φ（Ｔｏｙ、０）ノＸ
ｚ−ΔＸ　Ｔ）　　　　ａ　・・α４式で与えられる。

但し、φ（Ｔｏｙ　ｓ　Ｌ）は、行列微分方程式 ％式％） ・・・・・（１９式 を解き、　Ｊ（ｔ）ツφ（Ｔｏｙ　ｅ　＊　）で与えら
れる。ま九、ａ（ｏ、’ｒｏデ）は１行列微分方程式％
式％（） ｐ（ｏ）：＝−・（零行列）　　・・・・・一式を解き
、Ｐ　（ＴＯ？）　−Ｇ　（０＊　Ｔｅｙ）　　とする
ことによって得られる。上記ＣｌＪ式で示されるｊｕ（
ｔ）の時の軌道ノｘ（ｔ）をα１式によシ演算すれば、
それが求める補正軌道である。

第３ステツプ 公称軌道Ｘ。Ｐ（ｔ）と上記補正軌道＃（１）とを加算
して得られる軌道を単針画軌道 ｘｇｏｐ（ｔ）＝ｘｏｙ（ｔ）＋ノ：１（１）０≦ｔ≦
ＴｏＰ　　　　・・・・Ｃ０７）式とし、この軌道を実
現する単針１ｉ指令値Ｕａｏｙ（ｔ）ｔ−（１）式から
演算する。

第４ステツプ 各動作軸毎に、単針ｄｉ指令値の絶対値の最大値とサー
ボモータに許容される最大入力値Ｕｊｎａａｘとの比を
演算し、その最大値の平方根に、第１ステツプで選択し
た公称軌道に対応する公称動作時間を乗じて動作時間Ｔ
ｌｌ０Ｐを演算する。即ち、動作時間は入力の平方根の
逆数に比例するから、一式の工うに動作時間を定めるこ
とにより１次に述べる計１ｉ１１指令値の最大値と丈−
ボモータに許容される最大値との各動作軸毎の比のうち
、その最大値ｔ−１とすることができ、短時間の制御が
可能となる。

ｇｓステップ 上述し九単針１軌道の動作時間を、公称動作時間ＴＯＦ
から上記動作時間Ｔｓ＋ｏｙに質更させる計画指令値Ｕ
−ｔｏ（ｔ）およびその時の計画軌道θ＠ｏｙ（Ｌ）＠
＃ｇｏｙ（ｔ）　、”Ｊ：ｏｙ（ｔ）を演算し、その結
果を記憶要素１８に記憶させる。

但し、０≦ｔ≦Ｔ６゜。

第６ステツプ タイマ１１に工って設定される所定の時間間隔毎に、位
置および速質の現在値と計画軌道との偏差にフィードバ
ックゲインを乗じた積に計１ｉｉ１１１令値を加算し、
その結果をＤＡ変換要素１０に出力する。この第５ステ
ツプは、動作時間がＴａｏｙ達するまで繰返し行なわれ
る。

以上述ぺ九工うな各ステップの演算を行なうことにより
、予め定めた公称初朝壇、公称終端値以外の初期値、終
端値につ込ても、入力エネルギをほぼ最小に保つと共に
、各サーボモータ〈許容される最大入力以下となる工う
な計Ｉｊｉ指令値、およびその計ｌＩｉｆＭｉ令値に１
って実現される計１軌道が。

共役勾配法等の工つな繰返し収束計算を行なうことなく
、１回のループで極めて短時間に求められる。

以上説明した１うに、本発明に工れば、任意の初期状態
、任意の終端状鴨に対し、公称軌道の中から最も近似す
るものを選択してその公称軌道と補正軌道とから計画軌
道を求め、またその動作時間は指令値が各サーボモータ
に許容される最大入力を超えることのない工うに設定す
ることに１す、最小エネルギ軌道としての公称軌道に対
し％最小エネルギをほぼ維持した計画軌道が実現でき、
ｔた高速の軌道制御が実現できる。また、指令値を多自
由度運動機構の運動方程式自体から演算しているために
高ｆ［な軌道制御が実現できるという優れた効果を有す
る。

【図面の簡単な説明】

第１図は本発明の一実癩列を適用した３自由度運動機構
を示す説明図、第２図は本発明の一実権例を示す制御ブ
ロック図、第３図は本発明の他の実権例を示す制御ブロ
ック図である。 １．２．３・・・・動作軸、４・・・・先端。 ５・・・・初期状態、６・・・・終端状態、Ｔ・・・・
連続軌道％８，１８・・ｅ・記憶要素、Ｓ・・・・演算
要素、１０・・・・Ｉｎ変換要素。 １２・・・・サーボモータ、１３・・・・位置検出要素
、１５・・・・位置演算要素、１６・・・・速度演算要
素、ＩＴ・・・・タイマ。 第１図 ３

Claims (1)

1. 【特許請求の範囲】 α）複数の動作軸と、これら各動作軸を駆動するサーメ
   篭−夕と、各動作軸の位置および速寂を検出する検出要
   素と、予め設定し九複数の公称軌道およびこの公称軌道
   に対応する公称指令値ならびに位置と速覆のフィードバ
   ックゲインを記憶する記憶要素と、＃紀検出要素から得
   る位置および速変の現在値と前記公称軌道および公称指
   令値とからｍ１ｅｔ−ポモータに与える命令値を演算す
   る演算要素とを備え九多自由寂運動機構において、ａｌ
   ｌとする初期状１と終端状態とに対し、前記公称軌道の
   中から、その公称軌道の初期状態と前記目標とする初期
   状態との距離と、公称軌道の終端状態と舖記目標とする
   終端状態との距離との加算値かはぼ最小になる公称軌道
   を選択し、この選択し九公称軌道の初期状態と目標とす
   る初期状態との差および轟咳公称軌道の終端状態と目標
   とする終端状態との差から補正軌道を演算し、この補正
   軌道と前記選択し九公弥軌道とから計画軌道を演算し、
   更にこの計画軌道に対応する計ｉｉ１指令値を演算して
   これら計画軌道と計画指令値とに１って軌道制御を行な
   うことを特徴とする多自由変運動＊＊の制御方式。 （２）計−軌道および計＠指令値は、各動作軸毎に選択
   した公称軌道に対応する公称指令値の最大値とサーボモ
   ータに許容される最大入力値との比と、軌道における移
   動量と前記公称軌道における移動量との比とを乗算し、
   その積の最大値の平方根に比例するように定めた動作時
   間に対応して設定することを特徴とする特許請求の範Ｓ
   第１項記載の条内装置運動機構の制御方式。 （３）計画軌道および計画指令値は、各動作軸毎の指令
   値の最大値とサーボモータに許容される最大入力値との
   比の中の最大値１−１にするように定め良計１動作時間
   に対応して設定する仁とを特徴とする特許請求の範囲第
   １項記載の多自由＆運動機構の制御方式。