JPH0398101A - ロボットの制御方法及び装置 - Google Patents

Abstract

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

Description

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

［従来の技術］ 力・トルクセンサを用いて位置姿勢指令の修正量を求め
、ロボットアームを指定されたバネ定数を持ったバネと
して動作させる従来のロボット制御方式として、例えば
、第５図に示すように、ｊ．Ａ．Ｍａｐｌｅｓ及びＪ．
Ｊ．Ｂｅｃｋｅｒによる「ロボットマニピュレー夕の力
制御の実験（Ｅｘｐｅｒｉｍｅｎｔｓ　ｉｎ　Ｆｏｒｃ
ｅＣｏｎｔｒｏｌ　ｏｆ’　Ｒｏｂｏｔｌｃ　Ｍａｎｉ
ｐｕｌａｔｏｒｓ）　Ｊ　　（ｐｒｏｃ．ＩＥＥＥ　Ｃ
ａｎｔ．　Ｒｏｂｏｔｌｃ　ａｎｄ　Ａｕｔｏｍａｔｉ
ｏｎ，１９８Ｂ）なる文献がある。

図において、１はロボットアーム、２はアーム１の手首
部に組み込まれた力・トルクセンサ、３は位置姿勢制御
部、４はバネ制御演算部、５は積分器、６は合成部、７
はスイッチ、８は位置姿勢指令、９は軌道生成部である
。

次に動作について説明する。ロボットアーム１がバネと
して動作しないときはスイッチ７はオフとなっている。

この場合、積分器５の積分量つまり修正量は変化せず固
定である。軌道生成部９から位置姿勢指令８が出力され
る。この位置姿勢指令８は合成部６で積分器５の修正量
と合成され、その合成位置姿勢指令を位置姿勢制御部３
へ与え、ロボットアーム１は制御される。

ロボットアーム１がバネとして動作しないモードからバ
ネとして動作するモードに変化するとき、スイッチ７は
オンになる。この場合、力・トルクセンサ２の出力を用
いてバネ制御演算部４で、バネとして動作するための位
置姿勢の微小修正量を求める。これを積分器５で累積し
て位置姿勢修正量を得る。

位置姿勢指令８がバネ中心位置となり、力がつりあった
状態では積分器５の積分量がバネのたわみに相当する。

［発明が解決しようとする課題］ しかしながら、従来のロボット制御方式によれば、バネ
として動作しないモードからバネとして動作するモード
に変化するとき、すなわちスイッチ７がオフからオンに
なるときに軌道指令はそのままで、積分量と作用してい
る力・トルクの関係がつりあっていなければバネ制御演
算部４からの微小修正量が積分器５で累積され、軌道指
令の修正量が変化するため、モードが変化しただけでロ
ボットアーム１が移動してしまい、危険であった。

本発明は、上記のような課題を解決するためになされた
もので、バネとして動作するモードに変化したとき、対
象物からどのような力・トルクが作用していても、ロボ
ットアームが移動することなく、バネとして動作するモ
ードに変化できるロボットの制御方式を得ることを目的
とする。

［課題を解決するための手段］ 本発明に係るロボットの制御方式は、対象物から受ける
力・トルクに応じて軌道から変位するロボットの制御方
式において、ロボットアームの移動すべき位置姿勢指令
を生成する軌道生成部と、前記ロボットアームに固定さ
れ、対象物から受けている力・トルクを計測する力・ト
ルクセンサと、前記軌道生成部の位置姿勢指令をバネ中
心とじて前記ロボットアームを指定されたバネ定数を持
ったバネとして動作させる制御手段と、バネ動作の有効
または無効を指示する指示手段とを備え、バネ動作が無
効の状態から有効状態に変化する指示が発せられたとき
に、そのときの力・トルク計測値と指定されたバネ定数
からバネ中心位置姿勢を求め、前記軌道生成部の指令と
するように構成したものである。

［作　用］ 本発明におけるロボットの制御方式は、現在の手先効果
器が受けている力・トルクとバネ定数から、バネ中心位
置姿勢である軌道指令を設定し直すので、バネ動作の有
効が指示されたときに、バネとしてつりあった状態とな
るように軌道指令を初期設定する。そのため、ロボット
アームは移動することなくバネとして動作するモードに
変化できる。それ以後は、バネ制御演算部からの微小修
正量によりロボ′ットアームはバネの動作モードで制御
される。

［実施例］ 以下、本発明の一実施例を図により説明する。

第１図は本発明の一実施例を示すロボットの制御装置の
構成図である。図において、１はロボットアーム、２は
アーム１の手首部に紐み込まれた力トルクセンサ、３は
位置姿勢制御部、４はバネ制御演算部、５はバネ制御演
算部４の出力である微小移動指令を積分して修正量をつ
くる積分器、６は軌道指令と修正量を合成して合成位置
姿勢指令を生成する合成部、７は積分器５への入力をオ
ン・オフするスイッチ、８は軌道指令、９は軌道生成部
、１０はバネ制御の有効または無効を指示する指示装置
、１１は積分器５の初期値演算部、１２は軌道生成部９
の初期値演算部、１３は積分器５への人力をオン・オフ
するスイッチ、１４は軌道生成部９への入力をオン・オ
フするスイッチ、１５は手先効果器、１６は作業対象物
である。

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

まず、バネ動作について説明する。いま、指定されたバ
ネ定数が６行６列の行列Ｋｓで与えられ、力・トルクセ
ンサ２により計測された対象物１６から手先効果器１５
が受けている力・トルクを６行１列のベクトルＦａとし
、バネの中心と考える軌道生成部９から与えられた位置
姿勢指令と実際のロボットアーム１の位置姿勢の偏差を
６行１列のベクトルΔＸであらわすと、 Ｆａ−ＫｓΔｘ　　　　　　　　　　　　　（１）なる
関係が成り立っていれば、バネ定数Ｋｓのバネとしてロ
ボットアーム１は動作していることになる。

これを図で示すと、第２図のようになり、点線で示され
る位置姿勢が軌道生成部９の指令であり、手先効果器１
５が対象物１６から力・トルクＦａを受け、ΔＸ離れた
実線の位置姿勢に移っていれば、式（１）のバネとなっ
ている。

そこで、第１図のバネ制御演算部４は現在の位置姿勢偏
差と指定されたバネ定数を乗じて、バネとして発生して
いるべき力・トルクを求め、これと力・トルクセンサ２
で計測された実際の力・トルクとを比較し、それらが等
しくなるように位置姿勢の微小修正量を求める。位置の
微小修正量は３行１列のベクトルΔＰ，姿勢の微小修正
量は３行３列の回転行列ΔＲで得られる。積分器５はこ
の微小修正量を積分して位置姿勢修正量を求める。

位置の修正量を３行１列のベクトルＰｓ，姿勢の修正量
を３行３列の行列Ｒ　ｓ’とすると、位置修正量の積分
はベクトルの和、姿勢修正量の積分は行列の積で行われ
、 Ｐｓ　（ｋ）＝Ｐｓ　（ｋ　　１）十ΔＰ（２）Ｒｓ　
（ｋ）＝Ｒｓ　（ｋ−１）＊ΔＲ（３）となる。

軌道生成部９で生成される軌道指令８の位置指令を３行
１列のベクトル−Ｐｃ，姿勢指令を３行３列の行列Ｒｃ
とすると、合成部６では、Ｐｍ　（ｋ）−Ｐｃ　（ｋ）
　十Ｐｓ　（ｋ）　　　（４）Ｒｍ　（ｋ）＝Ｒｃ　（
ｋ）＊Ｒｓ　（ｋ）　　　（５）により、３行１列の合
成位置指令Ｐｍと、３行３列の合成姿勢指令Ｒｍを求め
る。

位置姿勢制御部３は、各関節を駆動してこの合成位置姿
勢にロボットアーム１を移動する。

以上のようにしてバネ動作は実現される。

次に、バネ動作が無効な場合の動作について説明する。

いま、時刻ｋＴにバネ動作無効の指示が発せられたとす
る。バネ動作が無効な状態では、スイッチ７はオフとな
り、積分器５にはバネ制御演算部４からの微小修正量は
入力されなくなり、積分量は一定に保たれる。時刻ｋＴ
以後の位置姿勢制御部３への指令は次のように軌道指令
に一定の修正を行ったものになる。

Ｐｍ　（ｎ）−Ｐｃ　（ｎ）＋Ｐｓ　（ｋ）　　　（６
）Ｒｍ　（ｎ）　一Ｒｅ　（ｎ）＊Ｒｓ　　（ｋ）　　
　（７）ここで、ｎ＞ｋ，Ｐｓ　（ｋ）及びＲｓ　（ｋ
）は時刻ｋＴの修正量で、以後一定である。

次に、バネ動作が無効な状態から有効な状態に変化した
ときの動作について説明する。

いま、時刻ｋＴに有効の指示が発せられたとする。まず
、積分器５の初期値演算部１１では、指定されたバネ定
数Ｋｓと力・トルクセンサ２で計測された実際の力・ト
ルクＦａとからバネとしてつりあうための位置姿勢偏差
を求める。

式（１）より、 Δｘ　ｍ　Ｋ　ｓ　−ｌＦ　ａ　　　　　　　　　　　
　　（Ｂ）となる。ここで、Ｋｓ−’はＫｓの逆行列を
あらわす。

ただし、ΔＸｐは位置の偏差をあらわす３行１列のベク
トル、ΔＸｒは姿勢の偏差をあらわす３行１列のベクト
ルである。

ΔＸｐと等価な、位置の積分量の初期値Ｐｓ’は以下の
ようになる。

Ｐｓ’　−ΔＸ　ｐ　　　　　　　　　　　　　　（１
０）次に、ΔＸｒと等価な姿勢の積分量Ｒｓ’を求める
。

ΔＸｒは次のように単位ベクトルＫｒと回転角θＳの積
である。７の突起部８とノズル２が接触ΔＸｒ−θｓ　
Ｋ　ｒ　　　　　　　　　　　　（１１）ただし、θｓ
−ｌΔＸｒ Ｋｒ　　一ΔＸｒ　　／　　Ｉ　　ΔＸｒである。

このとき、姿勢の積分量の初期値Ｒｓ’　は、ただし、
Ｋｒ−　［Ｋｘ　　Ｋｙ　　Ｋｚコ１　　（１は転値を
示す） ＳθＢ　，ｍ　ｓｉｎθＳ Ｃθｓ−ｃｏｓθＳ Ｖθｓ−１−ｃｏｓ　　θＳ である。

以上より、初期値Ｐｓ’及びＲｓ’が求まった。

次に、スイッチ１３をオンして、積分器５の積分量を、
位置の積分量はＰｓ’に、姿勢の積分量はＲｓ’　に初
期設定して、スイッチ１３をオフに戻す。すなわち、 Ｐｓ　　（ｋ）−Ｐｓ’　　　　　　　　　　　　　　
　　　（１３）Ｒｓ　　（ｋ）＝Ｒｓ’　　　　　　　
　　　　　　　　　　＜１４）とする。

次に、軌道生成部９の初期値演算部１２では、積分器５
からの位置修正量がＰｓ（ｋ），同じく姿勢修正量がＲ
ｓ　（ｋ）のときに、合成位置姿勢指令が変化しないよ
うな軌道指令の値を求める。

すなわち、バネの中心位置姿勢を求める。

いま、求める軌道指令の位置指令を３行１列のベクトル
Ｐｃ’　　姿勢指令を３行３列の行列Ｒｃ’　　バネ動
作有効の指示が出る直前の合成位置指令を３行１列のベ
クトルＰｍ（ｋ−１）．合成姿勢指令を３行３列の行列
Ｒｍ（ｋ−１）とすると、 Ｐｃ’　　＝Ｐｍ　（ｋ−１）−Ｐｓ　　（ｋ）　　　
　（１５）Ｒｅ’　　−Ｒｍ　（ｋ−１）＊　（Ｒｓ　
　（ｋ））　一Ｉ（ｌＢ〉 となる。ここで、（Ｒｓ　（ｋ））−’はＲｓ　　（ｋ
）の逆行列をあらわす。

次にスイッチ１４をオンして、Ｐｃ’　，Ｒｃ’を軌道
生成部９へ入力し、それぞれ軌道指令の位゛置指令Ｐｃ
（ｋ），軌道指令の姿勢指令Ｒｅ　（ｋ）とした後・、
スイッチ１４をオフに戻す。

すなわち、 Ｐｃ　（ｋ）−Ｐｃ’　　　　　　　　　　　　（１７
）Ｒｃ　（ｋ）＝Ｒｃ’　　　　　　　　　　　　（１
ｇ）次に、合或部６により時刻ｋＴの合成位置姿勢を求
める。

いま、合成位置指令を３行１列のベクトルＰｍ，合成姿
勢指令を３行３列の行列Ｒｍとすると、式（４）　．　
　（５）　，　　（１７），　　（１８）より、Ｐｍ　
（ｋ）＝Ｐｃ’　十Ｐｓ　（ｋ）　　　　　（１９）Ｒ
ｍ　（ｋ）　一Ｒｃ’　＊Ｒｓ　（ｋ）　　　　　（２
０）となる。

式（１９）．　　（２０）は、式（１５）．　　（１８
）により、Ｐｍ　（ｋ）　−Ｐｍ　（ｋ−１）　　　　
　　　　（２１）Ｒｍ　（ｋ）　−Ｒｍ　（ｋ−１）　
　　　　　　　（２２）となり、バネ動作有効の指示が
出る直前と同じ位置姿勢指令であり、ロボットアーム１
は動き出すことはない。

次に、スイッチ７をオンにして、以後、最初に説明した
バネ動作有効の状態での制御を行う。

第３図に本発明のロボットアームの動きの説明図を示す
。第３図（ａ）において、Ａ，Ｂ及びＣはあらかじめ計
画されたロボットアーム１が通過すべき地点である。軌
道生成部９はこれらの地点を補間して軌道を生成し、バ
ネ制御が最初から無効であれば実線のような軌道をロボ
ットアーム１は通過する。

次に、地点ＡからＢまではバネ制御が無効で、軌道生或
部９からの指令が地点Ｂに到達したとき、バネ制御が有
効になった場合のロボットアーム１の動きを第３図（ｂ
）に示す。地点Ｂにおいて既に対象物から力・トルクＦ
ｌを受けていた場合でも、地点Ｂにおいてロボットアー
ム１は移動せず、Ｂｌをバネ中心としたバネとして静止
している。

以後、対象物からの力・トルクに応じてバネとして動作
し、例えば実線のように移動する。

第４図に上で述べたＡからＣに至る動作のプログラム例
を示す。

まず、バネ定数の設定を行う。３個の並進のバネ定数と
３個の回転のバネ定数を数値で指定する。

次にバネ動作が無効の状態でＢ点へ直線で移動する。Ｂ
点に達するとバネ動作を有効にする。そしてＣ点に移動
する。

なお、上記実施例では、位置姿勢制御部への指令値を修
正することによりバネ動作を実現する場合について説明
したが、関節速度制御部への速度指令あるいは関節トル
ク制御部へのトルク指令などを変化させることによりバ
ネ動作を実現する場合にも適用可能である。

［発明の効果］ 以上説明したように、本発明によれば、ロボットアーム
がバネとして動作するような状態に変化したときに、対
象物からどのような力・トルクが作用していても、ロボ
ットアームが移動することがなく、バネとして動作する
モードに変化することが可能となる。

【図面の簡単な説明】

第１図は本発明の一実施例によるロボットの制御装置の
構成図、第２図はバネ動作の説明図、第３図はロボット
アームの動きの説明図、第４図はロボットの動作プログ
ラム例を示す図、第５図は従来のロボットの制御方式の
説明図である。 １・・・ロボットアーム　　２・・・力・トルクセンサ
３・・・位置姿勢制御部　　４・・・バネ制御部５・・
・積分器　　　　　　６・・・合成部７・・・スイッチ
　　　　　８・・・位置姿勢指令９・・・軌道生或部 １０・・・バネ制御の有効／無効を指示する指示装置１
１・・・積分器の初期値演算部 １２・・・軌道生成部の初期値演算部 １３．１４・・・スイッチ なお、図中、同一符号は同一または相当部分を示す。

Claims (1)

1. 【特許請求の範囲】 対象物から受ける力・トルクに応じて軌道から変位する
   ロボットの制御方式において、 ロボットアームの移動すべき位置姿勢指令を生成する軌
   道生成部と、前記ロボットアームに固定され、対象物か
   ら受けている力・トルクを計測する力・トルクセンサと
   、前記軌道生成部の位置姿勢指令をバネ中心として前記
   ロボットアームを指定されたバネ定数を持ったバネとし
   て動作させる制御手段と、バネ動作の有効または無効を
   指示する指示手段とを備え、バネ動作が無効の状態から
   有効状態に変化する指示が発せられたときに、そのとき
   の力・トルク計測値と指定されたバネ定数からバネ中心
   位置姿勢を求め、前記軌道生成部の指令とすることを特
   徴とするロボットの制御方式。