JPH04192008A - 多関節ロボット旋回制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】

ｒ産業上の利用分野】 本発明は多関節ロボット旋回制御方式に関する。

【従来の技術】

多関節ロボットに於いてアームを旋回させるときのトル
ク干渉の問題を、あらゆる旋回形態について解析するの
は難しい。この為従来は駆動モータに対する負荷が最大
になるところの最悪の旋回形態を想定し、この形態を基
にして各アームの最大角速度、該最大角速度に至るまで
の最小時定数等を定め、これらの限界を超えないように
アーム旋回のプログラムを設定している。

【発明が解決しようとする課題】

この為、前記最悪の旋回形態以外の多くの旋回形態では
、ロボットの駆動系が最大能力を発揮出来ない。従って
それだけ余分に旋回時間が掛っており作業効率が悪い。

【課題を解決するための手段】

そこで本発明では、旋回開始後の或る時点に於ける各ア
ームについての目標角度に対する現在角度の誤差、前記
或る時点に於ける各アームの角速度、及び前記移動開始
の時点から前記或る時点までの時間の総和で構成される
評価関数を極大にし、且つ該関数についてのハミルトン
関数を極大にするという２つの条件を満足するトルク曲
線を、種々のトルク曲線の中から選択し、該選択したト
ルク曲線又は該曲線から誘導した関数に従って前記各ア
ームの旋回を制御することとし上記課題の解決をはかる
。

【作用】

上記手段で選択されたトルク曲線は、上記酸る停止位置
から目標の位置まで各アームを旋回させるときに要する
時間について最適化が図られたものである。それ故この
トルク曲線又はこの曲線から誘導した関数に従って旋回
制御をすれば所要時間は最短になる。

【実施例】

以下本発明の詳細を図示実施例に基いて説明する。ここ
では２軸の水平多関節ロボットを例に取って説明をする
。先ず解析の為の定数、変数を第９図に示すように定義
する。なお図に於てＡ１及びＡ２は第１及び第２アーム
である。 次に評価関数、ハミルトン関数を導出するために２軸の
水平多関節ロボットの状態方程式を求める。なお後述の
式２７がここで求めようとしている状態方程式である。 先ずＣ１軸の重心位置は である。 又θ、軸の重心位置は である。 次にθ、軸の重心点の移動速度は ＝　１ｇ屑（ｓｉｎ（Ｃ１）２＋　ｃｏｓ（Ｃ１）２）
　　　　　　　　　（３）である。 公式５ｉｎ（ｘ）２＋　ｃｏｓ（ｘ）２＝　１　ヨＦ）
υ２＝１ｇ圀　　　　　　　　　　　　　　　（４）と
なる。 ０２軸の重心点の移動速度は ＝　ｌ屑＋　２１１１ｇ２列５ｉｎ（Ｃ２＋θ１）Ｓｉ
ｎ（Ｃ１）＋　２１１１ｇ２ｊ”、　ｃｏｓ（Ｃ２＋θ
１）００８（Ｃ１）＋　２ｊｌ１ｇ２＆１６２　ｓ堕θ
２＋θ１）翁（Ｃ１）＋　２２１２ｇ２＃１７２　ｃｏ
ｓ（Ｃ２＋θ１）ｃｏｓ（Ｃ１）である。 公式ｃｏｓ（Ｘ　）ｃｏｓ（Ｙ）　＋　５ｉｎ（Ｘ）ｓ
ｉｎ（Ｙ）　＝　ｃｏｓ（Ｘ−）’）よりυ至＝　１２
．好＋２１１１ｇ２り：ｃｏｓ（Ｃ２）　＋　２１１１
ｇ２り１り２ｃｏｓ（Ｃ２）＋１９詞＋２１ｆ）撓り２
　＋　１９撓　　　　　　（６）となる。 並進運動エネルギーＱｔは、 Ｑｔ　＝　＞ｍ１ｖ：＋−＞ｍ何 ”　”””Ａｅｅ　＋　ｍ２１：θに＋　２ｍ２１１１
９２ｆ）：　Ｃ０８（Ｃ２）＋２ｍ２１１１ｇ２沙１１
９２　ｃｏｓ（Ｃ２）　＋　ｍ２１ｇｐ３：＋　２ｍ２
１ｇ１ｆ）１ｊ）２　＋　ｍ２１ｇ勢）／２　　　　　
　（７）である。 又回転運動エネルギーＱｒは、 Ｑｒ＝Ｔｊ１θＦ＋Ｈｊ２（θｌ十秒２）２＝（ｊ１外
十ｊ２外＋２ｊ２υ１δ２＋ｊ２列）／２　　　　　　
　（ｓ）故に全体の運動エネルギーは、 Ｑ　＝　Ｑｔ＋Ｑ、　　　　　　　　　　　　　　　　
　（９）＝　（ｍ１１ｇ；８７　＋ｍ２１ｉｅｊ　＋　
２ｍ２１１１ｇ２列ｃｏｓ（Ｃ２）＋２ｍ２１１１ｇ２
Ｓ１＆２　ｃｏｓ（Ｃ２）　＋　ｍ２１ｄｌｌ’、　＋
　２ｍ２１ｇ’、０１ｅ２”　”２７ｇ％ｚ　”　ｊ１
’：　＋　ｊ２ｅｒ　＋　２ｊ２ｅ１ｅ２　＋　ｊ２ｅ
Ｄ／２　（ＩＱ）となる。 ここで式１１のようにα、β、γを定義して式１０を整
理すると、 Ｑ＝＜α列＋β（外＋２υ２＋列） ＋２７汐１ｃｏｓ（θ２）（抄１＋め））／２　　　　
　　　　　（１２）となる。 ところでラグランシュの方程式より である。 式９をこの式１３に代入すると、 を得る。 この式１４．１５を、加速度項を変数とする形に整理す
ると、 となる。 但し上式に於て である。 ここで ＋７β列＋　７２ｊ’：　ｃｏｓθ２　ｓｉｎθ２）　
　　　（２０）（−（β＋７　ｃｏｓθ２Ｘ７（２＆ｌ
十島０２　ｓｉｎθ２）−（α十β＋２７　ｃｏｓθ２
）７勾ｓｔｎθ２）　　（２１）として一般的な状態方
程式の形に纏めると、となる。この式２７が２軸の水平
多関節ロボットの状態方程式である。 本発明のテーマは、この式２７の右辺のｕｌ＋ｕ２、即
ちθ１軸、θ２軸に印加されるトルク曲線について、移
動に要する時間が最小になるように最適化を図ることに
ある。 ここではポンドリヤーギンらが完成した変分法の理論で
ある「最大原理」を用いてこのテーマを解決する。 一般的に最短時間を求めることを目的にするとき最大原
理により与えられる評価関数はである。 本発明ではこの評価関数を具体化する終端条件として次
の３条件を設定した。 （１）目標位置に着いていること。 （２）目標位置に於て角速度が「０」であること。 （３）移動所要時間が最小になること。 二の３条件を与えて式２８を展開すると、Ｍｌ（ｘｌ（
ｔバーθｉ）２＋　Ａｌ２（ｚ２（ｔバーθ三）２＋　
Ａｌ３ｘ３（ｔ／）２＋　Ｍ４ｚ４（ｔ／）２＋な　　
　（２９）となる。 なおこの式２９に於て、 θ、′、θ２゜は目標角度、 Ｍｌ、　Ｍ　２　、　Ｍ　ｓ　、　Ｍ　４は勾配法に於
ける重み付けの為の整数、 ｔ、は移動開始時点をＯとしたときの経過時間である。 又、ＭＬ、ＭＮにかかる関数は時点１．に於ける現在角
度と目標角度の差、 Ｍ　ｓ　、　Ｍ　＜にかかる関数は時点１．に於ける角
速度を示す。夫々二乗したのはこれら差や角速度が絶対
値に於て最小になる解を得るためである。 即ちこの評価関数は、旋回開始後の或る時点に於ける各
アームについての目標角度に対する現在角度の誤差、前
記酸る時点に於ける各アームの角速度、及び前記移動開
始の時点から前記酸る時点までの時間の総和で構成され
る。 而してＵ□＋ｕ２について種々の関数（トルクの変化パ
ターン）を仮定し、それらについてこの評価関数２９の
値を求め、この値が極小を示したときの関数がＵ工、ｕ
、の最適解、即ち移動に要する時間が最小の移動形態を
与える解である。 そこで次にこの最適解を求めるための手順を説明する。 先ずこの評価関数についてのハミルトン関数を求めると
、 Ｈ＝　−１＋Ｐ１：ｌ：ｓ＋Ｐ２Ｚ４＋ＰｄｘＣ：Ｊ：
２＋　：Ｔ：３＋　：！：＜）となる。 又随伴方程式は となる。 式２９の各項をｔ、に関して偏微分して求めるところの
終端条件は、 となる。 さて求めるＵ□、ｕ２の最適解、即ち或る位置θ１゜θ
２から目標位置θｔ、θ２１へ最短時間で移動するため
の最適解は、このハミルトン関数に極大値を与え、且つ
前記評価関数に極小値を与えるところのｕ　ｌ＋　　ｕ
　２である。 この最適解は次の手順で求められる。 （１）先ず１．を仮定する。 （２）ｕＬ＋　　ｕｎを仮定する。 （３）このＵ工、ｕ、に基いて式２７を解く。 これによりｘ１〜ｘ４の関数（曲線）が与えられる。 （４）与えられたＸ、〜ｘ４から式３２（終端条件）が
決定される。 （５）この終端条件を用いて式３１（随伴方程式）の各
関数を求める。 （６）この随伴方程式を用いてハミルトン関数（式３０
）Ｈを求める。 （７）（１）に戻って別のｉ　Ｉ＋　　ｕｌ＋　　ｕｌ
を仮定し、このｔ、、ｕ工、ｕｌについて（３）から（
６）を実行する。 （８）求められたハミルトン関数Ｈの値を最初のものと
比較し、Ｈの極大の方向が何れであるかを探る。探索に
あたっては黄金分割法その他適宜の手法を用いる。 （９）Ｈを極大にする関数（ｕ、ｕｌ）が探索できたら
、この関数が評価関数（式２９）を極小にするか否かを
検査する。この条件が満足されればこのときの関数ｕ　
Ｌ＋　ｕ　２が求める最適解である。 （１０）条件が満たされないときは（１）に戻って探索
をやり直す。 このようにして求めた最適解ｕ　ｌ　＋　　ｕｌに依っ
て第１及び第２アームＡｌ、Ａ２が移動された例（１）
を第１図から第４図に、同じく例（２）を第５図から第
８図に示す。又従来方式の移動例を第１０図に示す。 なお上記最適解ｕ　Ｌ＋　　ｕ　２は１つの移動形態、
即ち或る位置θ１．θ２から或る目標位置θげ、θ？に
移動する個々の形態毎に存在する。従って実施に当って
は、これら４つの角度の最大と最小の間を例えば夫々５
等分して得られる６２５通りの移動形態について、予め
それに就いての最適解ｕｌ＋ｕ２を求め、これを曲線の
サンプリングデータの形で、或いはフーリエ級数の形で
テーブルとして記憶素子に格納しておき、移動制御を実
施する度に該当するｕ　ｌ＋　　ｕｌを呼出すようにし
てもよい。 又上位の電子計算機にこれらθ１．θ２．θｔ、θ、。 を供給し、上位の電子計算機でｕ　Ｌ＋　　ｕ　２を探
索してこれを送り返すようにしてもよい。 なお関数ｕ　ｌ＋　　ｕ　２に代え、これから誘導した
他の関数、例えば第３図、第４図、第７図、第８図の様
な関数を制御に用いてもよい。

【発明の効果】

以上説明したように、本発明によれば駆動系の能力を最
大限に生かした移動制御が出来る。

【図面の簡単な説明】

図は本発明の一実施例を示し、第１図は本発明による移
動例（１）の動作軌跡を示す線図、第２図は本発明によ
る移動例（１）のトルク曲線を示す線図、第３図は本発
明による移動例（１）の角度変化を示す線図、第４図は
本発明による移動例（１）の角速度変化を示す線図、第
５図乃至第８図は同様に本発明による移動例（２）の動
作軌跡、トルク曲線、角度変化、角速度変化、を夫々示
す線図、第９図は解析用各変数を示す線図、第１０図は
従来方式の移動例を示す線図である。 Ｈ・・・ハミルトン関数、Ｊ・・・評価関数、Ａ１・・
・第１アーム、Ａ２・・・第２アーム。 ｔｌ・・・或る時点、ｕｌ、ｕｌ・・・トルク曲線、θ
１．θ２・・・現在角度、θＩ、θ２＠・・・目標角度
、θ１．θ、・・・角速度。

Claims (1)

【特許請求の範囲】
1. （１）旋回開始後の或る時点に於ける各アームについて
   の目標角度に対する現在角度の誤差、前記或る時点に於
   ける各アームの角速度、及び前記移動開始の時点から前
   記或る時点までの時間の総和で構成される評価関数を極
   小にし、且つ該関数についてのハミルトン関数を極大に
   するという２つの条件を満足するトルク曲線を、種々の
   トルク曲線の中から選択し、該選択したトルク曲線又は
   該曲線から誘導した関数に従って前記各アームの旋回を
   制御することを特徴とする多関節ロボット旋回制御方式
   。