JPH03111184A - ロボット制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明は力・トルクセンサを用いてロボットアームをバ
ネとして動作させ組立作業や加工作業を実行するロボッ
トの制御方法に関するものである。

［従来の技術］ 力・トルクセンサを用いて位置姿勢指令の修正量を求め
、ロボットアームを指定されたバネ定数を持ったバネと
して動作させる従来のロボット制御方法を用いたロボッ
ト制御装置の構成図として、例えば第４図に示すような
ものがある。これはｒＰｒｏｃ、　ＩＥＥＥ　Ｃｏｎｒ
、Ｉ？ｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ
”Ｅｘｅｒｆｅｎｔｓ　ｉｎ　Ｆｏｒｃｅ　Ｃｏｎｔｒ
ｏｌ　ｏｆＲｏｂｏｔｌｃ　Ｍａｎｊｐｕｌａｔｏｒｓ
　　　（Ｊ、Ａ、Ｍａｐｌｅｓ、Ｊ、Ｊ、Ｂｅｃｋｅｒ
、　１９８Ｂ　）　Ｊに示されるものである。図におい
て、（１）はロボットアーム、（２）はアームの手首部
に組こまれたカ・トルクセンサ、（３）は作業を行なう
手先効果器、（４）は軌道生成部、（５）はバネ変位演
算部、（６）はバネ制御演算部、（７）は合成部、（８
）は位置姿勢制御部、（９）は座標逆変換部、（１０）
は関節位置制御部、（１１）はロボットアーム（１）の
関節位置を計測する関節位置検出器、（１２）は座標正
変換部である。

次に動作について説明する。

軌道生成部（４）においては、手先効果器（３）が外力
を受けないときに通るべき基準位置姿勢指令を生成する
。すなわちこれがバネの中心位置姿勢となる。ロボット
アーム（１）の直交座標系で表わした現在位置姿勢は、
関節位置検出器（ＩＩ）で検出された関節位置を座標正
変換部（１２）で座標変換して得られる。バネ変位演算
部（５）は、基準位置姿勢指令と現在の位置姿勢から現
在ロボットアーム（１）がバネとしてどれだけ変位して
いるかを演算し、位置姿勢偏差を出力する。バネ制御演
算部（６〉は、位置姿勢偏差と現在の位置姿勢と力・ト
ルクセンサ（２）で検出された手先効果器（３）が現在
受けている力・トルクとから、バネとして動作するため
の位置姿勢修正量を演算する。そして合成部（７）で基
準位置姿勢指令を位置姿勢修正量たけ修正した合成位置
姿勢指令を求め、位置姿勢制御部（８）へ指令する。位
置姿勢制御部（８）では、座標逆変換部（９）において
直交座標系での位置姿勢指令を関節位置指令に変換し、
関節位置制御部〈１０）で（］１）の関節位置検出器で
計測した関節位置を用いて位置制御しロボットアーム（
１１）を移動する。以上の演算かマイクロコンピュータ
を用いたサンプリング制御により実行されている。

［発明が解決しようとする課題］ 従来のロボット制御方法は、以上のように行なわれてい
たので、バネとして動作させるには多くの演算を行なう
必要があり、マイクロコンピュータによる制御サンプリ
ング周期が長くなってむだ時間の影響により、バネとし
ての動作の応答特性が悪くなるのが問題となっていた。

本発明は、バネとして動作させるだめの演算量を減らし
制御サンプリング周期を短くすることにより、応答特性
のよいロボット制御方法を提供しようとするものである
。

［課題を解決するための手段］ 本発明に係るロボット制御方法は、バネ中心位置姿勢か
らの変位量として、位置姿勢修正量を時間的に遅らせた
ものを用い、ロボットアームの現在の位置姿勢として合
成位置姿勢指令を時間的に遅らせたものを用いるように
した。

［作用］ 本発明におけるロボットの制御方法では、座標正変換お
よびバネ変位の演算を無くすことが可能になる。座標正
変換には三角関数や積和の浮動小数点演算が多く含まれ
、またバネ変位の演算も多くの積和演算が必要であるが
、本発明によれば、これらの演算を省略することができ
る。

［発明の実施例］ 第１図はこの発明の一実施例に係わるロボット制御装置
を示す構成図である。図において、（１）〜（１］）は
従来装置と同一または相当部品、（１３）および（１４
）は時間遅れ要素である。

第２図は座標系の定義を示す説明図、（ａ＞図の（１５
）はワールド座標系（以下Ｗ系と記述）で、ロボットの
作業環境に設定された直交座標系である。

また（ｌ〕）図の（１６）は手先効果器（３）に固定さ
れた直交座標系でエンドエフェクタ座標系（以下Ｅ系と
記述）とし、このＥ系をＷ系で記述することによりロボ
ットアームの現在の位置姿勢を表わす。

次に動作について説明する。

以下の説明においてサンプリング周期をＴとし、ｋは時
刻ｔ　＝　ｋ’　Ｔの値であることを示すものとする。

また、ロボットアームの位置はＥ系の原点位置をＷ系で
表わした３行１列のベクトルＰで、姿勢はＥ系の各座標
軸をＷ系で表わした３行３列の行列Ｒで表わし、関節位
置は６行１列のベクトルθで表わすものとする。

まず、ロボットアームの現在位置姿勢を求める方法につ
いて述べる。軌道生成部（４）により指令された基準位
置および姿勢をそれぞれＰｃ（ｋ）、Ｒｅ（ｋ）とする
。また、このときバネ制御演算部（６）で求まった位置
姿勢修正量をＰｓ（ｋ）およびＲｓ（ｋ）とし、合成部
（７）において求められた合成位置姿勢指令をＰｍ（ｋ
）およびＲｍ（ｋ）とすると、Ｐ　　ｎｌ（ｋ）＝　　
Ｐ　　ｃ（ｋ）十　Ｐ　　５（ｋ）　　　　　　　　　
　　　　　　（１）Ｐ　　ｍ（ｋ）＝　　Ｐ　　ｃ（ｋ
）＊　　Ｐ　　５（ｋ）　　　　　　　　　　　　　　
　（２）となる。

この合成位置姿勢指令Ｐｍ（ｋ）およびＲｍ（ｋ）を位
置姿勢制御部（８）の指令とする。位置姿勢制御部（８
）においては、まず座標逆変換部（９）により関節位置
指令θｒ（ｋ）に変換する。関節位置制御部（１０）は
関節位置検出器（１１）で計測されるロボットアームの
各関節の実際の位置θａ　（ｋ）を関節位置指令θｒ（
ｋ）と一致させるようにサーボ制御を行なう。

すなわち、サーボ系の追従遅れ時間経過した後に、各関
節の位置は関節位置指令にほぼ一致する。よって、サー
ボ系の追従遅れ時間をサンプリング周期のほぼ０倍（ｎ
≧１）であるとすると、θａ（ｋＥ−θｒ（ｋ−ｎ）　
　　　　　　　　　（３）となる。

これらを座標正変換し直交座標系での位置姿勢に変換し
たとすると、 Ｐ　ａ（ｋ）＃　Ｐ　Ｍ（ｋ−ｎ）　　　　　　　　　
　　　　　　（４）Ｒａ（ｋ）　’、　Ｒｍ（ｋ−ｎ）
　　　　　　　　　　　　　　　（５）となる。よって
、時刻ｋＴにおけるロボットアムの現在位置姿勢は時刻
（ｋ−ｎ）Ｔの合成位置姿勢指令値で近似できることが
イつかる。そこで、時間遅れ要素（１３）により、合成
部（７）の出力を時間ｎＴだけ遅らせればロボットアー
ムの現在の位置姿勢が得られることになる。時間遅れ要
素（＋３）はマイクロコンピュータのメモリを用いて簡
単に実現できる。例えば、第３図に示すようにメモリ内
にｎ＋１個の連続したバッファを用意する。サンプリン
グ時間毎に合成位置姿勢指令ＰｍおよびＲ１１をバッフ
ァ０からバッファｎまで順に格納する。バッファｎまて
到達すればバッファ０に戻る。

一方、時刻ｋＴにおいて格納するバッファの次のバッフ
ァの値を現在の位置姿勢として用いればよい。第３図は
時刻ｋ　ＴにおいてバッファＤの値を現在位置姿勢とし
て用い、バッファ（口利）に合成位置姿勢指令値を格納
したところを示す。

次にバネ制御演算に必要な位置および姿勢の偏差を求め
る方法について述べる。

軌道生成部（４）から指令される基準位置姿勢指令Ｐ　
ｃ（ｋ）およびＲｅ（ｋ）と、ロボットアームの現在の
位置姿勢Ｐ　ａ（ｋ）およびＲａ（ｋ）との偏差がバネ
のたわみ量に相当する。

まず、位置の偏差を３行１列のベクトルΔＸｐ（ｋ）と
すると、 ΔＸ　ｐ（ｋ）　−Ｐ　ａ（ｋ）　−Ｐ　ｃ（ｋ）　　
　　　　（６）である。

式（４）を用いて、 ΔＸ　ｐ（ｋ）　’ｉ　Ｐ　ｍ（ｋ−ｎ）　−Ｐ　ｃ（
ｋ）　　　　　（７）さらに、作業対象物に接触して力
・トルクを受けるような場合基準位置姿勢指令は、急激
には変化しないので、 Ｐ　ｃ（ｋ）＃　Ｐ　ｃ（ｋ−ｎ）　　　　　　　　　
　（８）と書ける。よって、式（７）およびり８）より
、ΔＸ、ｐ（ｋ）＝　Ｐ　ｍ（ｋ−ｎ）　−Ｐ　ｃ（ｋ
−ｎ）　　　　（９）となる。式（１）より、 Ｐ　ｌ１ｌ（ｋ−ｎ）　＝　Ｐ　ｃ（ｋ−ｎ）　＋　Ｐ
　５（ｋ−ｎ）　　　　（１０）であるから、式（９）
および（１０）より、ΔＸｐ（ｋ）−、Ｐ　５（ｋ−ｎ
）　　　　　　　　　　　　　（１，１）となる。すな
わち、時刻ｋＴにおける位置偏差は時刻（ｋ−ｎ）　Ｔ
における位置修正量で近似できることになる。

また、バネをＥ系で実現する場合はＥ系で表わした偏差
が必要である。これをＥΔＸｐ（ｋ）とすると、式（１
１）および（５）より、 ＥΔＸ　ｐ（ｋ）　−Ｒａ（、ｋ）−’　ｘΔＸｐ（ｋ
）＃　Ｒｍ（ｋ−ｎ）　　’ｘＰｓ（ｋ−ｎ）　　　（
１２）となり、時刻（ｋ−ｎ）Ｔの合成姿勢指令と位置
修正量を用い演算できる。

次に姿勢の偏差を表わす行列をＲｄ（ｋ）とすると、Ｒ
ｄ（ｋ）　＝　Ｒｃ（ｋ）　’　Ｘ　Ｒａ（ｋ）　　　
　　　（１３）により、基準姿勢指令から実際の姿勢へ
の偏差が求まる。式（５）により、 Ｒｄ（ｋ）’；　Ｒｃ（ｋ）　’　Ｘ　ＲＩｌｌ（ｋ−
ｍ）　　　　　（１４）さらに、式（８）と同様に、 Ｒｃ（ｋ）’ｉ　Ｒｃ（ｋ−ｎ）　　　　　　　　　　
（＋５）と書けるので、式（１４）および式（１５）よ
り、Ｒｄ（ｋ）’；　Ｒｃ（ｋ−ｎ）−１ｘ　Ｒｍ（ｋ
−ｎ）　　　　（１６）となる。式（２）より、 Ｒｍ（ｋ−ｎ）−Ｒｃ（ｋ−ｎ）Ｘ　　Ｒ５（ｋ−ｎ）
　　　　　　　　（］−７）であるから、式（１Ｇ）お
よび（１７）より、Ｒｄ（ｋ）’；　Ｒ５（ｋ−ｎ）　
　　　　　　　　　（１８）となる。

以上より、時刻ｋＴにおける位置姿勢の偏差は時刻（ｋ
−ｎ）　Ｔにおける位置姿勢修正量、で近似できること
がわかる。そこで、時間遅れ要素（１４）によりバネ制
御演算部の出力を時間ｎだけ遅らせれば現在の位置姿勢
の偏差が得られることになる。これも、第３図に示した
のと同時にマイクロコンピュータのメモリを用いて簡単
に実現できる。

最後に、位置姿勢の修正量を求めるバネ制御演算部につ
いて説明する。

バネ制御演算を行なう場合姿勢の偏差を表わす行列Ｔｄ
（ｋ）と等価な３行１列のベクトルを求める必要がある
。これをＥΔＸｒ（ｋ）とすると、（１９） ただし、 である。

また、このベクトルをＷ系で表わすには、ΔＸｒ（ｋ）
＝Ｒａ（ｋ）Ｘ　　ＥΔＸｒ（ｋ）’ｉ　Ｒｍ（ｋ−ｎ
）　ｘ　　ＥΔＸ　ｒ（ｋ）　　　（２２）で得られる
。

位置の偏差ΔＸｐ（ｋ）、姿勢の偏差がΔＸｒ（ｋ）、
力・トルクセンサ（２）で計測された現在の力・トルク
が６行１列のベクトルＦ　ａ（ｋ）であるとき、予ワ め設定されているバネ定数をＫｓとすると、なる関係が
成り立っていれば、バネ定数Ｋｓのバネとしてロボット
アーム（１）は動作していることになる。

そこで、バネ制御演算部（６）では、位置姿勢の偏差と
バネ定数から、 の演算を行ない、バネとして発生すべき力・トルクＦ　
ｒ（ｋ）を求める。

そして、力・トルクセンサ（２）で計測された実際の力
・トルクＦ　ａ（ｋ）と比較し、それらが等しくなるよ
うに、位置姿勢の修正ｍ　Ｐ　５（ｋ）およびＲｓ（ｋ
）を求める。

［発明の効果コ 以上のように、この発明によればバネ動作を実現する上
で必要なロボットアームの現在の位置姿勢およびバネ中
心位置姿勢からの偏差として、修正された位置姿勢指令
および位置姿勢修正量を、時間遅れ要素により１サンプ
リング時間以上遅らせたものを用いるようにしたので、
関節角度から位置姿勢を求める座標正変換演算および式
（６）と式（１３）によるベクトル・行列演算をなくす
ことができ、全体の演算量を大幅に減少させることがで
きる。その結果マイクロコンピュータによる制御サンプ
リング周期が短くなり、応答特性のよいバネ動作を実現
することが可能になる。

【図面の簡単な説明】

第１図はこの発明の一実施例に係わるロボット制御装置
を示す構成図、第２図（ａ）　、　（ｂ）は座標系の定
義を示す説明図、第３図は時間遅れ要素を説明する説明
図、第４図は従来の方法によるロボット制御装置を示す
構成図である。 図において、（１）はロボットアーム、（２）は力・ト
ルクセンサ、（５）は軌道生成部、（７）はバネ制御演
算部である。 なお、図中、同一符号は同一または相当部分を示す。

Claims (1)

1. 【特許請求の範囲】 多関節形ロボットアームに、作業対象物から受ける力お
   よびトルクを計測する力・トルクセンサを取付け、指定
   されたバネ定数をもったバネとしてロボットアームを外
   力に応じて変形するように動作させるロボット制御方法
   において、 バネ中心位置姿勢となる基準位置姿勢を指令する軌道生
   成部と、バネとして動作するための位置姿勢修正量を求
   めるバネ制御演算部と、上記基準位置姿勢指令と上記位
   置姿勢修正量を合成して合成位置姿勢指令をつくる合成
   部と、この合成位置姿勢指令に従ってロボットアームを
   移動する位置姿勢制御部と、上記位置姿勢修正量を時間
   的に遅らせる第１の時間遅れ要素と、上記合成位置姿勢
   指令を時間的に遅らせる第２の時間遅れ要素とを備え、
   上記バネ制御演算部において使用する現在のバネ中心位
   置姿勢からの偏差として第１の時間遅れ要素の出力を用
   い、ロボットアームの現在の位置姿勢として第２の時間
   遅れ要素の出力を用いることを特徴とするロボット制御
   方法。