JP2518699B2

JP2518699B2 - ロボット制御装置

Info

Publication number: JP2518699B2
Application number: JP1249277A
Authority: JP
Inventors: 正弘藤田; 孝夫馬場
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 1989-09-27
Filing date: 1989-09-27
Publication date: 1996-07-24
Anticipated expiration: 2011-07-24
Also published as: JPH03111184A

Description

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

［従来の技術］力・トルクセンサを用いて位置姿勢指令の修正量を求
め、ロボットアームを指定されたバネ定数を持ったバネ
として動作させる従来のロボット制御方法を用いたロボ
ット制御装置の構成図として、例えば第４図に示すよう
なものがある。これは「Proc.IEEE Conf.Robotics and
Automation“Exeriments in Force Control of Robotic
Manipulators"（J.A.Maples,J.J.Becker,1986）」に示
されるものである。図において（１）はロボットアー
ム、（２）はアームの手首部に組こまれた力・トルクセ
ンサ、（３）は作業を行なう手先効果器、（４）は軌道
生成部、（５）はバネ変異演算部、（６）はバネ制御演
算部、（７）は合成部、（８）は位置姿勢制御部、
（９）は座標逆変換部、（10）は関節位置制御部、（1
1）はロボットアーム（１）の関節位置を計測する関節
位置検出器、（12）は座標正交換部である。

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

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

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

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

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

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

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

第２図は座標系の定義を示す説明図、（ａ）図の（1
5）はワールド座標系（以下Ｗ系と記述）で、ロボット
の作業環境に設定された直交座標系である。また（ｂ）
図の（16）は手先効果器（３）に固定された直交座標系
でエンドエフェクタ座標系（以下Ｅ系と記述）とし、こ
のＥ系をＷ系で記述することによりロボットアームの現
在の位置姿勢を表わす。

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

以下の説明においてサンプリング周期をＴとし、ｋは
時刻ｔ＝kTの値であることを示すものとする。また、ロ
ボットアームの位置はＥ系の原点位置をＷ系で表わした
３行１列のベクトルＰで、姿勢はＥ系の各座標軸をＷ系
で表わした３行３列の位列Ｒで表わし、関節位置は６行
１列のベクトルθで表わすものとする。

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

この合成位置姿勢指令Pm（ｋ）およびRm（ｋ）を位置
姿勢制御部（８）の指令とする。位置姿勢制御部（８）
においては、まず座標逆変換部（９）により関節位置指
令θｒ（ｋ）に変換する。関節位置制御部（10）は関節
位置検出器（11）で計測されるロボットアームの各関節
の実際の位置θａ（ｋ）を関節位置指令θｒ（ｋ）と一
致させるようにサーボ制御を行なう。すなわち、サーボ
系の追従遅れ時間経過した後に、各関節の位置は関節位
置指令にほぼ一致する。よって、サーボ系の追従遅れ時
間をサンプリング周期のほぼｎ倍（ｎ≧１）であるとす
ると、 θａ（ｋ）≒θｒ（ｋ−ｎ）（３）となる。

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

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

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

まず、位置の偏差を３行１列のベクトルΔXp（ｋ）と
すると、 ΔXp（ｋ）＝Pa（ｋ）−Pc（ｋ）（６）である。

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

また、バネをＥ系で実現する場合はＥ系で表わした偏
差が必要である。これを^ＥΔXp（ｋ）とすると、式（1
1）および（５）より、^Ｅ ΔXp（ｋ）＝Ra（ｋ）^-1×Δｐ（ｋ） ≒Rm（ｋ−ｎ）^-1×Ps（ｋ−ｎ）（12）となり、時刻（ｋ−ｎ）Ｔの合成姿勢指令と位置修正量
を用い演算できる。

次に姿勢の偏差を表わす行列をRd（ｋ）とすると、 Rd（ｋ）＝Rc（ｋ）^-1×Ra（ｋ）（13）により、基準姿勢指令から実際の姿勢への偏差が求ま
る。式（５）により、 Rd（ｋ）≒Rc（ｋ）^-1×Rm（ｋ−ｍ）（14）さらに、式（８）と同様に、 Rc（ｋ）≒Rc（ｋ−ｎ）（15）と書けるので、式（14）および式（15）より、 Rd（ｋ）≒Rc（ｋ−ｎ）^-1×Rm（ｋ−ｎ）（16）となる。式（２）より、 Rm（ｋ−ｎ）＝Rc（ｋ−ｎ）×Rs（ｋ−ｎ）（17）であるから、式（16）および（17）より、 Rd（ｋ）≒Rs（ｋ−ｎ）（18）となる。

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

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

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

また、このベクトルをＷ系で表わすには、 ΔXr（ｋ）＝Ra（ｋ）×^ＥΔXr（ｋ） ≒Rm（ｋ−ｎ）×^ＥΔXr（ｋ）（22）で得られる。

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

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

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

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

【図面の簡単な説明】

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

Claims

(57)【特許請求の範囲】

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