JPH03240104A - ロボットの倣い制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔概　　要〕 未知形状の作業対象物の表面を滑らかに倣うために、作
業座標系をオンラインで自動的に設定するロボットの倣
い制御装置に関し、 未知形状の作業対象物の表面を滑らかに倣うために、作
業座標系をオンラインで自動的に設定することを目的と
し、 目標力ベクトルの方向の単位ベクトルｎを第１列、該ベ
クトルｎに直交し、かつロボットの手先ルの外積■×■
＝■を第２列とする作業座標系から基準座標系への変換
行列を用いて、前記ロボットの手先の目標値ｌと現在位
置との偏差と、該ロボットの力の目標値と手先の対象物
への作用力との偏差とによって、該ロボットの位置と力
を制御する位置と力の混成制御装置を備えたシステムに
おいて、前記ロボットの作業対象物表面上に指定される
倣い作業の開始点Ｔ＋　と終了点Ｔ２、および点ＴＩと
Ｔｚの間の該倣い作業の経由点Ｔ３に対して、該３点Ｔ
＋　、Ｔｚ　、Ｔ３で決定される平面と前記作業対象物
表面との交線を前記倣い作業の経路とする倣い経路設定
手段を有するように構成する。

〔産業上の利用分野〕

本発明はロボットの制御方式に係り、さらに詳しくは未
知形状の作業対象物の表面を滑らかに倣うために、作業
座標系をオンラインで自動的に設定するロボットの倣い
制御装置に関する。

〔従来の技術〕

ロボットの手先をある力で押し付けながら対象物表面に
沿って移動させる倣い作業においては、ロボット手先に
取り付けられたプローブと作業対象物との接触点におけ
る接平面に対して、その法線方向に力制御、また接平面
内で位置制御を行うように設定される。

第１２図は倣い作業のような位置と力の混成制御におけ
る作業座標系の設定の例である。同図において、ロボッ
トアーム１に力覚センサ２を介して取り付けられたプロ
ーブ３と作業対象物４との接触点において、力制御方向
は対象物４の表面の法線方向の単位ベクトルｎで、また
位置制御方向は単位ベクトルＯＶａとによって示される
。本発明ではプローブ３の移動方向を単位ベクトルａの
方向に一致させるものとする。

第１３図はロボットの位置と力の混成制御装置の構成例
のブロック図である。同図においてＰｌ。

はロボットの各関節の変位から算出されるロボットの手
先の現在位置、ｐ　ｏｒは手先の目標位置、Ｐ−は作業
座標系の原点、Ｆ□はロボットに装着された力覚センサ
で検出されるロボット手先と作業対象物との間の作用力
、Ｆ　ｏｒは目標力、ｅは関節速度指令である。ここで
例えばｐ　ｓｒの添字１ｒ１　は例えばロボット本体の
固定位置を原点とする基準座標系表示であることを示し
ている。

ロボットの基準座標系で記述される位置偏差は位置偏差
生成器５によって、また力偏差は力偏差生成器６によっ
て生成され、それぞれ変換行列ＲＴによって、例えばロ
ボット手先位置を原点とし、手先と作業対象物との間の
押し付けなどの拘束関係で定義される作業座標系に変換
される。ここでここでＲＴの“Ｔｏは位置行列を示し、
ｎ、０゜ａは互いに右手系で直交する単位ベクトルを示
す。

作業座標系に変換された位置偏差および力偏差からそれ
ぞれ位置補償器７、および力補償器８によって速度指令
が生成されるが、位置補償器７の出力には位置選択行列
Ｓ、９が、また力補償器８の出力には力選択行列５ｒｌ
Ｏが掛けられて、位置制御方向と力制御方向が分離され
た速度指令としてＶｐｗおよび■１が生成される。ここ
で例えばことを示す０例えば並進ｎ方向を力制御、並進
０およびａ方向を位置制御する場合には位置選択行列Ｓ
、および力選択行列Ｓｆは次のように与えられる。

Ｓ、＝　　ｄｔ−−（０１１）　　　　　　　（２）Ｓ
ｔ−ｄｔ−−（１００）　　　　　　　（３）ここで“
ｄ　Ｌａｇ′は括弧内を主対角線要素とする対角線行列
を示す。

手先を空間移動させる場合には、その移動を高速化する
ために、例えばロボットの出し得る最高速度■□８をそ
の高さとする台形状の速度関数が速度発生器１１により
フィードフォワードの速度指令■。いとして生成される
。そしてこの■。０は積分器１２を介して位置偏差生成
器５にも入力される。これは積分器１２の出力Ｉ　（Ｖ
。、（ａ））、すなわち移動開始位置ＰＣ，，からの移
動すべき量によって時々刻々の手先のあるべき位置を求
め、それを現在位置Ｐ１．．と比較するためである。

加算部１３により３つの速度指令の和としてロボットの
手先が出すべき作業座標系記述の速度Ｖ。ｗ＋Ｖ、、＋
Ｖｆ、が生成され、変換行列Ｒ１４によって基準座標系
記述に変換される。そしてその速度から逆ヤコビ行列Ｊ
−’１５を用いて関節速度指令ｅが生成される。ここで
逆ヤコビ行列Ｊ−１はロボットの各関節の微小時間に対
する微小角変位（関節速度）６とロボット手先の微小変
位（速度）■とを Ｖ　＝　Ｊ　ｅ　　　　　　　　　　　　　　　　　（
４）によって関係づけるヤコビ行列Ｊの逆行列であって
、ヤコビ行列Ｊの各要素はロボットの関節の構造によっ
て決定される。

〔発明が解決しようとする課題〕

第１２．１３図のような位置と力の混成制御において実
際のプローブ３の先端拘束状態と設定された作業座標系
との関係にずれが生じると接触力は目標値通りには制御
されないという問題点がある。例えばベクトルａが対象
物４との接線よりも内部に向いていると、ベクトルｎ方
向の位置ずれが修正されるまで過大な接触力が生じ、ま
たベクトルａが外部に向いていると逆に接触力が過小に
なるか、またはプローブ３が対象物４の表面から浮いた
状態になる。

従ってロボットによって対象物４の表面を安定した接触
力で滑らかに倣う場合には、プローブ３の先端拘束状態
に応じてベクトルｎ、ｏ、ａをオンラインで変化させる
必要があり、対象物表面の形状が判明していなければ作
業座標系を設定することができず、また判明していても
倣い動作中に刻々と変化する接平面、およびその法線を
オフラインで準備しておかねばならないという問題点が
あった。

本発明は、未知形状の作業対象物の表面を滑らかに倣う
ために、作業座標系をオンラインで自動的に設定するこ
とを目的とする。

〔課題を解決するための手段〕

第１図は本発明の原理ブロック図である。同図ベクトル
ｎを第１列、ベクトルｎに直行し、かつロボット１６の
手先移動方向の単位ベクトルａを第３列、ａとｎとの外
積０を第２列とする作業座標系から基準座標系への変換
行列を用いて、ロボット１６の手先の目標位置と現在位
置との偏差と、ロボット１６の力の目標値と手先の作業
対象物への作用力との偏差とによって、ロボット１６の
位置と力を制御する位置と力の混成制御装置１７を備え
たロボットシステムにおける倣い制御装置の原理ブロッ
ク図である。

第１図において、倣い経路設定手段１８はロボット１６
の作業対象物表面上に指定される倣い作業の開始点ＴＩ
とその終了点Ｔ２、および点ＴＩとＴｚとの間の倣い作
業の経由点Ｔ３に対して、３点Ｔ＋　、Ｔｚ　、Ｔ３で
決定される平面Φと作業対象物表面との交線を倣い作業
の経路として設定する。平面Φに対して、その法線ベク
トルΦを３点Ｔ’＋　、　Ｔｚ　、およびＴ３から求め
ることにより、倣い経路は法線ベクトルΦをもち、かつ
点Ｔ＋を含む平面Φ上に存在することになる。

〔作　　用〕

本発明においては、倣い作業の開始点ＴＩ、経由点Ｔ３
、および終了点Ｔ２によって決定される平面Φと対象物
表面との交線が、倣い作業の経路として倣い経路設定手
段１８によって設定され、その後プローブの先端が倣い
作業の開始点ＴＩに接触するようにロボットアームの移
動が行われる。

次に、例えばプローブに装着された力覚センサによって
検出される接触力の方向から作業座標系の単位ベクトル
ｎが決定され、また作業座標系の単位ベクトルａは例え
ば倣い作業の開始点Ｔ！における接平面■と平面Φとの
交線ｌの方向のべりれる。

本発明では未知形状の作業対象物の表面に沿って倣い作
業を行うために、設定されている倣い作業座標系の適合
性判定が適宜行われ、すでに設定されている座標系が不
適と判定された時点で再度作業座標系の設定が行われる
０作業座標系の適合性判定は、例えば作業座標系の原点
（最初は開始点ＴＩ）からプローブ先端の現在位置に向
かうベクトルとすでに設定されている移動方向の単位ベ
クトルａとの威す角度がある閾値を越えたか否かで行わ
れる。

このように、本発明においては、設定されている作業座
標系が不適と判定された時点で作業座標系の再設定が行
われる。

〔実　　施　　例〕

第２図は倣い経路設定の実施例である。同図において、
倣い作業の開始点Ｔ＋　と終了点Ｔ２との間に、例えば
経由点Ｔ３１が設定されると、３点Ｔ＋。

Ｔ２．Ｔ：ＩＩによって決まる平面ΦＩと作業対象物４
の表面との交線が倣い経路１として設定され、また倣い
作業の経由点としてＴ２．が指定されると、平面Φ２と
作業対象物４の表面との交線が倣い経路２として設定さ
れる。ここで平面Φに対する法線ベクトルΦは倣い作業
の経由点をＴ３として、次式で与えられる。

すなわち倣い経路はΦを法線ベクトルとして、かつ倣い
作業の開始点Ｔ＋を含む平面Φ上に存在することになる
。

第３図は作業対象物への押し付は制御の実施例である。

同図は倣い作業の開始点Ｔ１における倣い作業のための
最初の接触状態を示している。まず開始点Ｔ１へのプロ
ーブ先端の空間移動が行われ、次に点Ｔ１から作業対象
物が存在する方向に大きさがＩＦ、□　１ｔの仮の目標
力ベクトルが指令され、プローブ先端の並進自由度の拘
束が１の状態で、押し付は制御が行われる。この時板の
目標ベクトルは接触点Ｔ＋における法線方向と一致する
必要はない。

第３図に示すように最初の接触点における押し付けは、
力覚センサによって検出される接触力Ｆ　ｌｌｒの仮の
目標力ベクトル方向への成分がＩＦ、、ｌｔとなった状
態で終了する。ここで添字°ｔ”は並進ベクトルの大き
さを示す。

プローブ先端が移動している間の接触力は摩擦力を含ん
でいるが、第３図はプローブ先端が倣い作業の開始点Ｔ
＋で停止した時には力覚センサによって検出される接触
力Ｆ０は接触点ににおける接平面Ｈの法線方向を表して
おり、対象物に作用する力の反作用力とみなせるために
、作業座標系第４図は倣い経路設定用平面Φと接平面ｎ
との交線ｌの実施例である。同図において、プローブの
先端が開始点Ｔ＋から倣い経路上の任意の点Ｐ、、。

に移動したものとし、この点を倣い作業座標系を設定す
る接触点ＰＣ，（原点）として作業座標系の単位ベクト
ルａの設定法について説明する。同図に示すように、単
位ベクトルａは平面Φと平面■との交線ｌの方向のベク
トルｍに平行となる。こ（７）式で与えられるベクトル
ｍが第４図のように倣る場合には段位ベクトルｍ／　Ｉ
　ｍ　ｌ−をそのまま作業座標系の単位ベクトルａとす
ることができる。

しかしながら、例えば対象物表面が第２図のように上に
凸でなく、凹んでいるような場合には（５）式で求めら
れる法線ベクトルΦが逆向きとなり、その結果ベクトル
ｍもプローブ移動方向と逆向きとなり、その方向の単位
ベクトルをａとすることはできない。

第５図は作業座標系の単位ベクトルａの設定法の実施例
である。同図において、倣い作業の開始点Ｔ＋　と終点
Ｔ２の直線２へのＰｔ１．　ｐｔｚが次式％式％（８） （９） 単位ベクトルａはＰｔｌからＰｔ２へ至る方向の単位作
業座標系の第３の単位ベクトル０は単位ベクトルｎとａ
を用いて次式で与えられる。

ｏ　＝　ａ　Ｘ　ｎ　　　　　　　　　　　　　　　０
０また目標力ベクトルは単位ベクトルｎの方向のベクト
ルとして仮の目標ベクトルの大きさを用いてＦ　ｏｒ＝
　ｌ　Ｆ　ｏｒｌ　　　ｔ　　ｎ　　　　　　　　　　
　　　Ｑ７Ｊ第６図は倣い座標系の適合性判定の実施例
である。

対象物が連続した曲面である場合、作業座標系は原理上
は連続して変化させることになるが、そうすると振動的
な動作となる恐れがある。本発明では第６図に示すよう
に、作業座標系の原点ｐ　ｃｒから手先の現在位置Ｐ０
へ至るベクトルａｓとａベクトルとのなす角度θ１が方
向角誤差閾値θｔｈａを越えたときに、既設作業座標系
は不適であると判定する。すなわち　　　　ゝ ａ　Ｓ＝　　（Ｐａｒ　　Ｐｃｒ）　／　ｌ　Ｐａｒ　
　Ｐｃｒｌ　　　Ｑ’ｌ）なるａｓベクトルにおいて、 ａｓ−ａ≦ｃｏｓ　（θｔｈａ　）　　　　　　　０４
）が真のとき、既設作業座標系は不適である。

また、何らかの原因で接触に不安定が生じたり、ｎ方向
の力制御の追従が遅れることにより、プローブ先端が対
象物から離れることがあり得るので、倣い作業座標系の
適合性判定には、ｎ方向の接触力が充分に存在している
ことを条件とする。

Ｆｍｒ・ｎ≧ξ　（Ｆ、ｒｌ　・ｎ）、　　（０＜ξ〈
１）・　・　・ａつ を満たしているときに限る。

第７図は本発明の倣い制御装置を用いるロボット制御シ
ステムの実施例の全体構成ブロック図である。同図にお
いて、システムはＣＰＵ２０．目標力で押し付は動作、
または目標位置への移動などの１動作を行うための目標
ベクトルＰ。１の生成、作業座標系から基準座標系への
変換行列Ｒの生成、選択行列Ｓｒ、Ｓ−の生成、フィー
ドフォワードの移動速度■□８の生成を行う１動作制御
装置２１、第１３図の位置と力の混成制御装置と関節サ
ーボ制御装置を含むリアルタイム制御装置２２、本発明
の倣い動作制御装置２３、ロボットへの動作命令などを
格納する記憶装置、例えば磁気ディスク２４、キーボー
ド２５、倣い作業の開始点ＴＩ、終了点Ｔ２および経由
点Ｔ３などを記憶するメモリ２６、各関節に取り付けら
れたエンコーダからの信号をディジタル変換するＡ／Ｄ
変換器２７、各関節のモータを駆動するパワーアンプへ
の信号をアナログ変換するＤ／Ａ変換器２８、ＣＲＴ２
９、磁気ディスク２４のインタフェース３０．キーボー
ド２５のインタフェース３１、図示しない力覚センサの
インタフェース３２、ＣＲＴ２９のインタフェース３３
、エンコーダ用のｌ１０３４、およびパワーアンプ用の
ｌ１０３５から構成されている。

第８図は本発明における倣い動作制御の全体処理実施例
のフローチャートである。同図において、処理がスター
トするとまずステップ（Ｓ）３６で第９図で説明する前
処理が行われる。前処理では倣い作業における倣い経路
の設定や、最初の接触点における仮の目標力ベクトルな
どの各条件値の設定が行われる。続いてＳ３７で倣い作
業の開始点Ｔ、への空間移動が、また３３８で開始点Ｔ
＋での仮の目標力ベクトルＦ。□での押し付けが、第７
図の１動作制御装置２１によって各ステップに図示のマ
クロ命令によって実行される。なおＳ３７のマクロ命令
における“ＴＩＭＥ’　は空間移動命令における移動時
間の指定である。

続いてＳ３９で第１０図で説明する倣い作業座標系の設
定処理が行われる。この処理では前述のルｎ、ａ、およ
び０の設定などが行われる。Ｓ３９の処理が終了した時
点で倣い作業開始時の作業座標系の設定が完了したこと
になり、Ｓ４０で倣い作業が開始される。すなわち第７
図のメモリ２６に格納されている目標位置Ｐ。ｒを倣い
作業の終了点Ｔ２に、フィードフォワードの移動速度ｖ
、、Ｘを変数５ｐｅｅｄに置換することによって、リア
ルタイム制御装置２２を用いて位置と力の混成制御によ
って倣い動作が実行される。

モして５４１でプローブ先端位置Ｐ、、が目標位置ｐ　
ｏｒ、すなわち倣い作業の終了点Ｔ２に到着したか否か
が、例えばリアルタイム制御装置２２内の移動終了フラ
グを監視することによって判定される。３４１でプロー
ブ先端が倣い作業の終了点Ｔ２に到着していない場合に
は、３４２で第１１図で説明する倣い作業座標系の適合
性判定処理が行われ、適と判定された場合にはＳ４１か
らの処理が繰り返される。

Ｓ４２で倣い座標系が不適と判定された場合には、Ｓ４
３でフィードフォワードの移動速度Ｖ□８とサンプリン
グ周期毎の目標位置を求めるための積分値が共に０とさ
れて、リアルタイム制御装置２２の位置と力の混成制御
による倣い動作が停止される。モしてＳ４４でプローブ
先端が停止した点において、３３９と同様にして倣い作
業座標系の設定処理が行われ、Ｓ４０以降の処理が繰り
返される。

３４１でプローブ先端位置Ｐ０が倣い作業の終了点Ｔ２
に達したと判定された時点で、３４５〜Ｓ４７の空間移
動命令によって点Ｅ＋、Ｅｚ　　・・・を経由してプロ
ーブ先端の点Ｅｎ、例えばホームポジションへの移動が
行われ、処理が終了する。

第９図は第８図の３３６、すなわち前処理の実施例のフ
ローチャートである。同図においてまず３４８で倣い作
業の開始点ＴＩ、終了点Ｔ２、および経由点Ｔ３がダイ
レクトティーチング、またはキーボード２５からの座標
値の入力、あるいはディスク装置２４からの座標値の入
力により設定され、メモリ２６に格納される。続いてＳ
４９で（５）式を用いて平面Φの法線ベクトルΦが算出
され、メモリ２６に格納される。Ｓ５０では倣い動作の
終了後に、例えばホームポジションまでの空間移動にお
ける経由点として任意の個数の点の座標がダイレクトテ
ィーチング、またはキーボード２５あるいはディスク装
置２４からの座標値の入力によって設定され、メモリ２
６に格納される。

その後３５１で倣い作業の開始点Ｔ１における初期の接
触力Ｆ。ｒｌが仮の大目標ベクトルとしてキーボード２
５、またはディスク装置２４からの座標成分値の入力に
よって設定され、メモリ２６に格納される。例えば第３
図に示す拘束状態の場合には、２．方向に押し付ければ
作業対象物への接触ができるので、仮の大目標ベクトル
として次式を人力する。

Ｆｏ、＝　（ＯＯＯ，５）” この時のＩＦ、□　ｌｔが倣い動作における接触力の大
きさとして設定されることになる。

その後Ｓ５２で倣い動作の速度５ｐｅｅｄが、またＳ５
３、Ｓ５４ではそれぞれ座標系の適合判定処理に用いら
れる方向角誤差閾値θｔｈｄ　％および接触判定係数ξ
がキーボード２５、またはディスク装置２４からの入力
によって設定され、メモリ２６に格納されて処理を終了
する。

第１０図は第８図の３３９、すなわち倣い作業座標系設
定処理の実施例のフローチャートである。

同図において、まずＳ５５でプローブ先端の位置Ｐ　ｓ
ｒが座標系の原点ｐ　ｃｒとされ、３５６で（６）式に
よって単位ベクトルｎが設定され、Ｓ５７で平面Φと■
との交線ｌの方向のベクトルｍが設定され、３５８、Ｓ
５９において（８）および（９）式を用いて倣い作業の
開始点Ｔ＋　と終了点Ｔ２の直線ｌへの足Ｐｔｌおよび
ｐｔｚが求められる。

続いてＳ６０で０２）式に従って目標力ベクトルが単位
ベクトルｎの方向に設定され、Ｓ６１でプローブ先端の
移動方向単位ベクトルａと作業座標系の第３の単位ベク
トル０とが００および００式を用いて設定され、Ｓ６２
で力選択行列Ｓｆと位置選択行列Ｓ９との設定が行われ
る。ここで力選択行列Ｓｔはｎ方向だけが１とされ、位
置選択行列Ｓ２はａと０方向とが１とされる。

第１１図は第８図におけるＳ４２、すなわち倣い作業座
標系の適合性判定処理の実施例のフローチャートである
。同図において、Ｓ６３で０３１式によって作業座標系
の原点Ｐ　ｃｒからプローブ先端の現在位置ｐ　ｓｒへ
至るベクトルａＳが求められ、Ｓ６４で（２）および０
つ式が共に成立しているか否かが判定され、共に成立し
ている場合には倣い作業座標系が不適なものとして第８
図において３４３の処理に移行し、圓および０５）式の
両方が成立している場合以外は作業座標系が通と判定さ
れ、第８図において３４１からの処理が繰り返される。

なお、本実施例では第１１図の５６４で０９式のみが成
立しない場合、例えばプローブ先端が対象物から離れた
ときには座標系はそのままで位置と力の混成制御が続行
されるが、０ω式が不成立になったときに移動速度を零
としてプローブを停止させ、ｎ方向への力制御によって
プローブと対象物との接触が再び行われるのを待つ処理
を入れることも可能である。

〔発明の効果〕

以上詳細に説明したように、本発明によれば作業座標系
がオンラインで自動的に設定されることになり、未知形
状の作業対象物の表面を滑らかに倣うことが可能になり
、倣い作業におけるロボットの制御性の向上に寄与する
ところが大きい。

【図面の簡単な説明】

第１図は本発明の原理ブロック図、 第２図は倣い経路の設定の実施例を示す図、第３図は作
業対象物への押し付は制御の実施例を示す図、 第４図は倣い経路設定子面Φと接平面■との交線ｌの実
施例を示す図、 第５図はベクトルａの設定法の実施例を示す図、第６図
は倣い座標系の適合性判定の実施例を示す図、 第７図は本発明の倣い制御装置を用いるロボット制御シ
ステムの実施例の全体構成を示すブロック図、 第８図は倣い制御の全体処理実施例のフローチャート、 第９図は前処理の実施例のフローチャート、第１０図は
倣い作業座標系設定処理の実施例のフローチャート、 第１１図は倣い作業座標系の適合性判定処理の実施例の
フローチャート、 第１２図は倣い作業における座標系設定の例を示す図、 第１３図は位置と力の混成制御装置の構成例を示すブロ
ック図である。 １　・ ２　・ ３　・ ４　・ ２１　・ ２２　・ ２３　・ ２４　・ ２５　・ ２６　・ ロボットアーム、 力覚センサ、 プローブ、 作業対象物、 １動作制御装置、 リアルタイム制御装置、 倣い動作制御装置、 ディスク装置、 キーボード、 メモリ。

Claims (1)

【特許請求の範囲】
1. 目標力ベクトルの方向の単位ベクトル■を第１列、該ベ
   クトル■に直交し、かつロボット（１６）の手先移動方
   向の単位ベクトル■を第３列、該２ベクトルの外積■×
   ■＝■を第２列とする作業座標系から基準座標系への変
   換行列を用いて、前記ロボット（１６）の手先の目標位
   置と現在位置との偏差と、該ロボット（１６）の力の目
   標値と手先の対象物への作用力との偏差とによって、該
   ロボット（１６）の位置と力を制御する位置と力の混成
   制御装置（１７）を備えたシステムにおいて、前記ロボ
   ット（１６）の作業対象物表面上に指定される倣い作業
   の開始点Ｔ＿１と終了点Ｔ＿２、および点Ｔ＿１とＴ＿
   ２の間の該倣い作業の経由点Ｔ＿３に対して、該３点Ｔ
   ＿１、Ｔ＿２、Ｔ＿３で決定される平面と前記作業対象
   物表面との交線を前記倣い作業の経路とする倣い経路設
   定手段（１８）を有することを特徴とするロボットの倣
   い制御装置。