JPS6077210A - 空間運動機構の制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】

廉Ｊ」３列生舟金−顆 本発明は、物体のハンドリングやＩＭ送を行う産業ロボ
ットあるいはマニプレータなどのような複数の動作軸を
連結した空間運動機構の高速・高精度な駆動制御方法に
関するものである。 従】Ｕ１肛 従来のこの種の空間運動機構の制御方法のうち、簡易な
制御装置で実現できるものとして、予め記憶されている
指令値に基づいてモータを駆動し、各動作軸の現在の位
置及び速度と指令値との偏差を減少させるように各動作
軸毎に位置と速度のフィードバック制御を行い、装置の
先端を目的とする軌道に沿って連続的に制御していく方
法がある。この制御方法は、各動作軸毎に制御系を構成
するのみで、複雑な演算を必要としないため制御装置は
簡単になるという利点はある。そして、この制御方法は
、先端負荷路が知られており、且つ比較的低速度で比較
的短い距離の所定のりＬ道に沿って空間運動機構の先端
を移動させる場合、その利点を十分に発揮できる。しか
し、高速度で大変位の移動を伴う動作をさせる際には、
各動作軸相互間の干渉により負荷変動が生じたり、遠心
力や重力の影響で蛇行したり行き過ぎなどが生し、高速
・高精度な軌道制御が困難である。 もう１つの軌道制御方法としては、各動作軸相互間の干
渉、遠心力、重力、摩擦力の影響によって生しる変動ト
ルクを打ち消すように、目標軌道を表す各動作軸の位置
、速度、加速度からこのｏＬ道を実現するモータ入力を
運動方程式に基づき演算し、これをフィードフォワード
し、その上で位置と速度のフィードハック制御を行うも
のがある。この方法は、変動トルクを打ち消す作用があ
るため前記の方法と比較し高精度な軌道制御が実現でき
る利点がある。 しかし、ハンドで質量・慣性モーメントが未知の物体を
把持した場合、あるいは、各動作軸における固体摩擦が
変動する場合には、変動トルクを打消すモータ入力今正
確に演算できないため、高精度・高速な軌道制御が実現
できない。 更にもう１つの軌道制御方法は、特開昭５８−１４３９
８５号として出願公開された特願昭５７−２６４５８号
「多自由度運動機構の制御方式」に開示されているもの
で、未知な物体の質量をモータ入力と前回サンプリング
時点での負荷推定量から推定し、この推定値を用いて変
動トルクを打消すモータに演算し、これをフィードフォ
ワードし、その上で位置と速度のフィードバック制御す
るものである。 しかしながら、そこに開示されている実施例は、ハント
が物体の重心を把持することを前提としており、ハント
の自由度を除くと腕の自由度が２の場合であり、腕の自
由度が３にＩ上の機構やハンドが物体の重心を把持して
ない場合には上記出願の方法を適用することは困難であ
る。すなわち、自由度が２の空間運動機構においてハン
ドが物体の重心を把持している場合は、平面運動であり
、３次元の慣性行列を把持しておく必要はないが、３自
由度以上の空間運動機構のり

【道制御や２自由度の空間
運動機構においてハン１−が物体の重心を把持していな
い場合の軌道制御には、負荷質量のみならず重心位置や
慣性行列の値を正確に把握していることを必要とし、一
方市心位置や１１１性モーメントがわからなければ、負
荷質量を１１）定することばも困難である。したがって
、従来の方法では、３自由度以上の空間運動機構やハン
１−が物体の重心を把持していない場合変動トルクを打
消すモータ入力が演算できず、高速・高精度な！ｌ４Ｉ
Ｌ道制御が実現できない。 光力ＩＮ１直 そこで、本発明は、上述した従来の方法の欠点を除いて
、ハンドが物体の重心を把持していない場合や、３自由
度以上の空間運動機構においても高速・高精度な軌道制
御が実現できる、空間運動機構の制御方法を提供せんと
するものである。 光１銀１戊 すなわち、本発明によるならば、複数の動作軸の各々に
付属して設けられて、各付属の動作軸を駆動するモータ
と、これら各動作軸の状態量を検出するために各動作軸
に対応して設けられた状態量検出要素と、各状態量検出
要素からの出方を受けて各動作軸の位置、速度、および
加速度を算出する検出回路部とを備えた関節式空間運動
機構において、関節トルクと、各動作軸の位置、速度、
加速度と、機構のハンドが把１屋する負荷の質量、重心
位置、慣性行列に対応するパラメータの前サンプリング
時刻での推定値とから、現時刻でのそれらパラメータの
推定を行い、このパラメータの推定値と所定の軌道を示
す状態量とから上記モーフへの入力を得ることを特徴と
する空間運動機構の制御方法が提供される。 このように、ハンドで把持した未知な物体の質量、重心
位置、慣性モーメントに対応するパラメータを各動作軸
の位置、速度、加速度から推定し、この推定１１ηから
モータ入力を演算し、これをもとに制御することにより
、３自由度以上の空間運動機構において、たとえハンｌ
−が物体の重心を把持していなくとも変動１−ルクを正
確に打消すモータ入力を与えることができ、高速・高精
度な軌道制御ができる。 ’ＡＵ色例 以下、添イ１図面を参照して本発明の詳細な説明する。 第１図は、本発明の実施例の５自由度空間運動機構の構
成図であり、基台ＢからのびるアームＡに、リンク１〜
５が関節８〜１２をそれぞれ介して直列に結合され、最
先端のリンク５には、ハンド６が固定され、そのハント
６には、負荷７が把持されている。 各関節８〜１２では図示するようにθ１〜θ５の回転が
得られる。そして、これらの関節は、図示していないモ
ータによって駆動される。 ここで、そのリンク５に固定されているハンＦ’　６に
よって負荷７を把持したとき、ハンド６および負荷７を
リンク５と一体化して見れば、リンク５の質ｆｊｉ　ｍ
　、重心位置Ｓ　＝　Ｃ５ｘ、　Ｓｙ、　Ｓｚｙ′、重
心まわりの慣性行列１１１ｉｊ）　が変動することにな
る。この変動は、各関節でのトルク変動として表れてく
る。 なお、右肩のＴは転置行列を意味し、以下、同様である
。また、以下、質量や位置を除いてｘ、ｙ、ｚ。 ｉ、ｊ、］、２．　３等の添字がないものはベクトルを
表している。 以下、−膜化して説明するため、機構の自由度はｎとし
く（ＩＩ＋一本例ではｎ＝５である）、リンクｉに座標
系〔へ−Ｘ、　ＹＬＺＪ）を設定する。リンクｎに作用
するモーメントの総和をＮ（３Ｘ１ベクトル）、力の総
和をＦ（３Ｘ１ベクトル）とすると、このＮとＦによる
各関節でのトルクの増分 Δτ−（Δ切　（ｎ×１ベクトル）は、の関係がある。 ここで、ｌは５Ｘｎの＋コピアン行列で関節角度θの関
数であり、容易に演算できる。また、 ヶー（ｆ、、Ｘ＋　”Ｖ＋　（、）２）　＋　”　＝　
（ふ８１品７１品２γ。 ■−（Ｖｘ、　ｖｙ、　Ｖｚ）をそれぞれｎ座標系で表
したリンクｎの角速度、角加速度、直線加速度とすると
、Ｆ＝ｍ　（ｖ＋Ｑｘ３＋ωｘ　（ωＸ５））　（２１
Ｎ＝　（ｐ＋５）ｘＦ＋ｒ山＋ωＸ（Ｉω）（３）の関
係がある。ただし、Ｐはｎ−１座標基原点からｎ座標系
原点へのベクトルであり、ｐ、ｓ、Ω、ふ。 ω、Ｆ、Ｎはいずれもｎ座標系で表しだベクトルである
。ここで ｎ＝　（ｆＩ＋　’２２　’３Ｂ　’＋２　’＋３　’
２３　）’　””ｒ　＝　ｍ　Ｃ５ｘ’、　Ｓｙ’、　
Ｓｚ”、　５ｘＳｙ、　５ｘＳｚ＋　５ｙＳｚｙｆ６１
Ｘ＝（Ｃ１、β”　、ＴＴ　Ｙ　（７）とおくと、（１
）式は 八τ−ＲＸ　（（３ン となる。ただし （１４） 度θが与えられれば演算でき、Ｒ／＋（演算できる。Δ
τは、モーフ入力から’ｅｉ’ｌＦもしくはトルク検出
器によっ°ζｉすられる現時刻での関節トルクτと、現
時刻の状態むｆθ１　θ２　θからリンクｎが、りｓい
としたときの関が）１ルクｔ　をめ、Δτ−τ−５によ
って演Ｗ（きる。なお、θ、θ、θからω、ω、■およ
びものａ１ｑ手順は、ＴＩ＋ｅ　Ａｍｅｒｉｃａｎ　Ｓ
ｏ＜：ｆＬｙ　ｏｆ　Ｍｅｃｔ＋ａｎｉｃａ１１ｉＢｉ
ｎｅａｒｓの季刊誌ｒＪｏｒｎａｌ　ｏ（Ｉｌｙｎａｍ
ｉｃ　ＳｙｓｔｅｍｓＭｅＩｌｓｕｒｅｍｅｎＬ、　ａ
ｎｄ　（３ｏｎｔｒｏｌＪの１９８０年６月号、（Ｖｏ
ｌ、　１０２）の第６９−７６頁の、１．　Ｙ、　Ｓ、
　Ｌｕｈ、　Ｍ、　１４゜１Ｊａｌｋａｒ、Ｒ，Ｉ’、
Ｃ，Ｉ’ａｕｌノ（著；　ｌ０ｎ−１，ｉｎｅ　Ｃｏｍ
ｐｕｔａｔ−ｉｏｎａｌ　５ｃｌＩｅ＋ｎｅ　ｆｏｒ　
Ｍｅｃｈａｎｉｃａｌ　Ｍａｎｉｐｕｌａｔｏｒｓ　Ｊ
にのることができる。 以上により、θ、θ、θ、τと（９１，（１０１，（１
１）　。 （１２）　、（１３）　、（１４）式とから未知パラメ
ータＸに関する線形な関係式（８）がサンプリング毎に
得られ、更に、それから（４）、　＋５１．　（６Ｌ　
＋７１式から質量ｍ、重心位置Ｓ、慣）生行列Ｉがめら
れる。 ここで、検出誤差を考慮し推定誤差量を少なくするため
（８）式にオンライン最小２乗法を適用し、Ｘを推定す
る。オンライン最小２乗法のアルゴリズムは、とおくと
、ｉ＝１．・・・、ｎの順に次式でＸを推定する。 Ｘ（Ｎ）＝Ｘ（Ｎ−θ−Ｐ（Ｎ）ｒ、、０−Ａ″ｒＸと
〃−θ−、ｗ；）　（１５）以上は各関節に於ける固体
摩擦があらがしめ測定できているあるいは無視できる程
度としていたが、フ１１（視することができず且つあら
かじめ測定できない場合は、（１）式の関節トルクの増
分Δτは次のように修正する。 ここで、Ｆｒは固体摩擦力で各軸における固体摩擦力の
大きさをｆ！　とすると、 で表される。ここで、 な Δτ＝ｉ文　（２ｏ） を得る。ＲはＲと同じくθ、δ、θより演算でき、この
場合も、推定誤差を少なくするためオンライン最小２乗
法を適用する。 第２図は、本発明の実施例の制御部のブロック図である
。サーボモーター３〜１７ば、関節８〜１２にそれぞれ
付属するサーボモータであり、そして、それぞれの関節
８〜１２の状態量を検出するために状態量検出要素１８
〜２２が設けられている。具体的には、各関節の動きは
、状態量検出要素１８〜２２によって例えば所定の角度
の回転毎に発生ずるパルスとして検出され、このパルス
信号が検出回路部２３へ送られてそこで状態量θ、θ、
θがめられる。サーボモータ１３〜１７にはサーボ回路
部２４〜２８が付属している。主演ｗ部２９は、記１．
ａ部３０とパラメータ同定演算部３】とデータの授受を
行うとともに、サーボ回路部２４〜２８とハス４０を介
してデータの授受をする。更に、サーボ回路部２４〜２
８とパラメータ同定演算部３１は、バス４０を介して主
演算部２９とデータを授受するだけでなく、ハス４０を
介して検出回路部２３からもデータを受ける。 この動作は、まずはしめに目標軌道θｄ、δｄ。 θｄのデータを記憶部３０から主演算部２９に転送する
。 主演算部２９では、この目標軌道を実現するモータ入力
を系の運動方程式に基づき演算し、サーボ回路部２４〜
２８送出する。サーボ回路部２４〜２８は、主演算部２
９から送出されたモータ人力をフィードフォワードし、
その上に検出回路部２３からの出力θ、ｉと主演算部２
９を介しての目標軌道θｄ、δｄとの偏差をフィードバ
ンク成分として加えた信号を主演算９＋＋２９に送出す
るとともにパワー増幅してサーボモータ１３〜１７に印
加する。これによりモータ１３〜１７は動作し、関節が
駆動される。 その結果としての各関節の動きは、−上述した如く状態
量検出要素１８〜２２によって所定の角度の回転毎に発
生ずるパルスとして検出され、このパルス信号から検出
回路部２３において状態量θ、δ、ｄを表ず検出信号が
められる。この検出信号はサーボ回路部２４〜２８及び
パラメータ同定演算部３１に送出される。 パラメータ同定演算部３１では、第３図に示すフローチ
ャートに従って、モータ入力から関節トルクτで演算さ
れる場合は、主演算部２９よりのτと検出回路部からの
θ、δ、ｉとから、Δτをめて更に上述した各式を用い
て９行列を演算してリンクの質量、重心位置、慣性行列
及び固体摩擦に対応するパラメータの推定を行い、その
結果を主演算部に送出する。 主演算部では更新したパラメータの推定値を用いて、次
のサンプリング時刻での目標軌道を実現するモータ入力
を演算する。上記の動作が指定したサンプル回数だけ繰
返される。 このような構成により、未知な物体を把持した場合にお
いても、リンク５の質量、重心位置、慣性行列に対応す
るパラメータを正確に推定することができ、その推定値
を用いて目標軌道を実現するモータ人力を演算するため
、高精度、高速な軌道制御が実現できる。 第４図は本発明の実施例である６自由度空間運動機構の
構成図であり、第１図と異なる所は、リンク３２と関節
３３及び図示していないがこの関節を駆動するモータが
付加していることである。先端のリンク３２にあるハン
）６によって負荷７を把持したとき、リンク３２とハン
ド６及び負荷７とを一体化して見れば、前記の実施例と
同様にリンク３２の質量ｍ、重心位ｉｔｓ　＝　（Ｓｘ
、Ｓｙ、５ｚ）Ｔ、　Ｍ心の回りの慣性行列（２）、（
３）式はｎ＝６とすることによって同様の関係式を得る
。ここで−トコビアン行列の逆行列が存在する場合には
、（１）式は となる。一方、 Ｆ’Ｎ＝ＦＴ（Ｉふ＋ωｘ（Ｔω））　（２２）となる
。ここで、 とおくと（４）、（５）、　（２１）、　（２２）式よ
り（２４ン、（２５）式ζこおいてτ　、τ　、　Ｗ、
Ｑはθ。 Ｂ θ、θよりめることができる。従って、（２４）、（２
５）式に個別にオンライン最小２乗法を適用することに
よって未知パラメータα、βを正確に推定できる。 第５図は第４図の空間運動機構のための本発明の実施例
の制御部のブロック図であり、第２図と異なる点は、６
自由度機構に対応できるように、サーボ回路部３４、サ
ーボモータ３５、状態量検出要素３６を付加した点であ
る。動作として異なる点はパラメータ同定演算部におい
て未知バラメークα、βの推定を分離して行うことであ
る。これによって、パラメータ推定に必要とする演算量
の低減をはかることができる。 光柘」 以上説明したように未知な物体をハンド把持した場合に
おいても、ハンドと物体を含む先端リンクの質量、重心
位置、慣性行列を精密に推定することができ、これによ
って目標軌道を実現するモーフ入力が正確に演算でき、
高精度、高速な軌道制御が実現できる。 また、軌道制御に限らず物体の質量、重心位置、慣性行
列の正確なデータを必要とする他の制御方式にもここで
示したパラメータ同定のアルゴリズが適用できる。

【図面の簡単な説明】

第１図は、本発明の方法が適用される５自由度空間運動
機構の構成図、 第２図は、第１図の空間運動機構のために本発明の方法
を実施するための制御部の構成図、第３図は、パラメー
タ同定演算部での処理のフローチャート、 第４図は、本発明の方法が通用される６自由空間運動機
構の構成図、 第５図は、第４図の空間運動機構のために本発明の方法
を実施するための制御部の構成図である。 （主な参照番号） Ａ・・基台、　Ｂ・・支持アーム、 １〜５・・リンク、　６・・ハンド、 ７・・物体、　８〜１２・・サーボモータ、１８〜２２
・・状態量検出要素、２３・・検出回路部、２４〜２８
・・サーボ回路部、　２９・・主演算部、３０・・記憶
部、 ３１・・パラメータ同定演算部、 ３２・・リンク、　３３・・関節、 ３４・・サーボ回路部、　３５・・サーボモータ、３６
・・状態ｍ検出要素、４０・・ハス出願人　日本電信電
話公社 代理人　弁理士　新居　正彦 第２図

Claims (3)

【特許請求の範囲】
1. （１）複数の動作軸の各々に付属して設けられて、各イ
   」屈の動作軸を駆動するモータと、これら各動作軸の伏
   ！占■を検出するために各動作軸に対応して設けられた
   状態量検出要素と、各状態量検出要素からの出力を受り
   て各動作軸の位置、速度、および加速度を算出する検出
   回路部とを備えた関節式の空間運動機構の制御方法にお
   いて、関節１−ルクと、各動作軸の（）′Ｌ置、速度、
   加速度と、機構のハンＩＸが把握する負１１；Ｊの質量
   、重心位置、慣性行列にり１応するパラメータの前サン
   プリング時刻での推定値とから、現時刻Ｃのそれらパラ
   メータの推定を行い、このパラメータの１１１．定値と
   所定の！ｌ！ＩＬ道を示す状態量とから上記モータへの
   入力をｆＪることを特徴とする空間運動機構の制御ａ１
   ）方／１４゜
2. （２）前記のパラメータの推定過程において、負荷の質
   量、重心位置、慣性モーメント及び各動作軸の咋体摩擦
   に対応するパラメータを推定することを特徴とする特許
   請求の範囲第１項記載の空間運動機構の制御方法。
3. （３）前記の空間運動機構の自由度が６以上のときに、
   前記のパラメータの推定過程において、負荷の質量と重
   心位置に対応するパラメータの推定と慣性行列に対応す
   るパラメータの推定とを分離して行うことを特徴とする
   特許請求の範囲第１項記載の空間運動機構の制御方法。