JPS63150183A - ロボツトの運動制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔目次〕 概要 産業上の利用分野 従来の技術 発明が解決しようとする問題点 問題点を解決するための手段　（第１図）作用 実施例 （ａ）　　一実施例の説明 （第２図、第３図、第４図） 山）　他の実施例の説明 発明の効果 〔概要〕 目標速度指令ベクトルと、検出手段の物体検出出力によ
って作成した回避速度指令ベクトルとでロボットの障害
物回避制御を行う運動制御方法において、障害物回避に
よる予定の経路からのずれを修正するため、現在位置に
基いて目標点に向かう該目標速度指令ベクトルを生成す
ることにより、障害物回避と経路復帰制御を行うもので
ある。

〔産業上の利用分野〕

本発明は、マニピュレータ型ロボットに物体検出センサ
を設け、物体検出センサの出力で障害物回避制御を行う
ロボットの運動制御方法に関し、特に経路復帰を障害物
回避後向動的に行いうるロボ７）の運動制御方法に関す
る。

マニピュレータ型ロボットは、組立て等の種々の作業自
動化に利用されている。

このようなロボットの＆跡制御として、自動的に障害物
を回避する能力を持たせることにより、安全性の向上、
ティーチングに要するコストの軽減、作業性の向上、作
業レイアウトの制約の軽減を図ることが行われている。

このような障害物回避のためには、ロボットに力覚（触
覚）、視覚、超音波センサ等を設け、障害物をこれらセ
ンサが検出した場合にロボットを障害物を回避するよう
に運動制御するものである。

〔従来の技術〕

第５図は従来技術の説明図である。

図において、Ｐｉは開始位置、Ｐｉ＋１が目標位置とし
、ロボットは開始位置Ｐｉから目標位置Ｐｉ＋１へ向か
う予定の経路ＲＰを運動ずべく目標速度ベクトルＶｄが
ＰｌからＰ　ｉ＋Ｔ方向へ向けて与えられたものとする
。

ロボットの進行により、２７点で進行方向に障害物４を
認知すると、センサの出力に基いて速度ベクトルをＶｄ
’の如く修正し、障害物４を回避し、更に２２点で進行
方向に障害物４を認知することによって、再び速度ベク
トルをＶｄ’の如く修正する。

障害物が認知されなくなったＤ点に達すると再び元の目
標速度ベクトルＶｄに戻す。

この目標速度ベクトルＶｄば、一般に開始位置Ｐｉから
目標位置Ｐ　ｉ、　＋　１へ移動する距離分与えられる
ので、ロボットは障害物４を回避したことにより、点Ｐ
４で停止する。

これを目標位置Ｐ　ｉ　＋　１に到達させるには、２４
点から目標点Ｐｉ＋１へ向かう１ｔｉｌｔ跡を再ブラニ
ングし、速度ベクトルＶｄＳを生成し、位置決めさせて
いた。

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

しかしながら、従来の運動制御方法では、速度ベクトル
をセンサフィードバンクによって変更することによって
障害物４の回避は可能であるが、与えられた目標位置に
このままでは到達できないという問題が生じる。

これを解決するには、軌道の再プラニングを要し、再プ
ラニングには、障害物に再び衝突しないよう巧妙に行う
必要があるため、広範囲の状況を把握できるセンサや信
号処理系を要し、ロボットの動作速度が低下し、又コス
ト的にも好ましくないという８題があった。

本発明は、障害物を回避しつつ、再プラニングを行わな
くとも目標位置に到達しうるロボットの運動制御方法を
提供することを目的とする。

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

第１図は本発明の原理説明図である。

図中、第５図で示したものと同一のものは同一の記号で
示しである。

本発明では、目標速度ベクトルＶｄｏ−Ｖｃ１５を現在
位置Ｐ　ｏ　”　Ｐ　５に基いて目標位置ｐ＋＋１に向
かうように生成し、これと、センサ出力からの修正（回
避）速度ベクトルＶｆ、〜Ｖｆ４との合成ＶＣ１〜Ｖｃ
５によってロボットの運動制御するようにしている。

〔作用〕

本発明では、修正速度ベクトルＶｆと目標速度ベクトル
Ｖｄとの合成速度ベクトルによって運動制御しているの
で、障害物回避ができるとともに、目標速度ベクトルＶ
ｄ自体を時々刻々の現在位置から目標位置に向かうベク
トル方向に修正しているので、障害物回避しても自動的
に目標位置に到達させることができる。

〔実施例〕

（ａｌ　　一実施例の説明 第２図は本発明の詳細な説明図である。

図中、第４図で示したものと同一のものは同一の記号で
示しである。

１ｔよマニピュレータであり、多関節型のもの、１１は
力検出装置であり、第３図に示すハンド１０と手首部の
間に設けられ、ハンド１０に（−１与される外力の方向
と大きさを検出するもの、Ｉ２はアクチュエータであり
、モータで構成され、マニピュレータ１の各関節を駆動
するものであり、この例ではマニピュレータ１が６軸の
多関節型で構成されていることから６軸分のアクチュエ
ータを備えるもの、１３はエンコーダであり、各軸のア
クチュエータ１２による関節変位θ（θ］、θ２、θ３
、θ４、θ５、θ６）を検出するもの、１４はサーボ制
御部であり、後述する演算プロセンサから与えられる各
軸の指令速度に追従する様に各軸のアクチュエータ１２
をサーボ制御するものである。

従って、マニピュレータ（ロボット）工は６軸の多関節
型で構成され、ハンド１０と手首部の間に力センサ（力
検出装置）１１が設けられている。

２は演算プロセッサであり、例えばＤＳＰ　（デる如く
ポストプロセッサから最終目標関節変位ｅｏｉ、移動速
度の最大値１Ｖｊｌ（ｊ＝＋、−ｎ）、経路通過点Ｐｊ
　　（ｊ−＋、−ｎ）が入力され、マニピュレータ１か
ら現在の関節変位θを取り込み、力センサ】１の検出出
力ＦＨ（ハンド座標系での直交座標成分）を取り込み、
回避速度演算、現在位置演算、目標速度演算等を行い、
各サンプリングタイム毎に各軸の指令速度をサーボ制御
部１４に与えるものである。

２０は座標変換演算部であり、力センサ１１の出力（反
力）ＦＨを後述する基準座標系の反力ＦＲに座標変換演
算するもの、２１は回避速度演算部であり、反力ＦＲか
ら反力方向の回避速度指令ベクトルＶｆ（基準座標系）
を演算するもの、２２は現在位置演算部であり、各関節
の現在の変位θから基準座標系でのハンドの現在位置Ｐ
ｎを演算するもの、２３は速度ベクトル生成演算部であ
り、現在位置Ｐｎと目標位置ｐｉ＋１と、１Ｖｊ１に基
いて、後述する（式）によって目標位置Ｐ＋　４−　＋
に向かう目標速度ベクトルＶｄｉを演算するもの、２４
は合成速度ベクトル演算部であり、基準座標系のＶｄｉ
、Ｖｆを加算して、基準座標系での合成速度指令ベクト
ル７尺を演算するもの、２５は座標変換演算部であり、
基準座標系での速度指令ベク］・ルＶ尺をハンド座標系
の速度指令ベクｌ−ルＶ１１に変換するもの、２６は関
節速度指令演算部であり、ハンド座標系の速度指令ベク
トルＶＨを各関節の速度指令ベクトルθに変換するもの
、２７は開数発生演算部であり、各関節の速度指令ベク
トルθの立上り、立下りを予定の加速、減速カーブで規
定して各関節の指令速度を演算するものである。

尚、各演算部２０〜２７は演算プロセッサ２が行う演算
をブロックとして示したものである。

３はポストプロセッサであり、与えられたティーチング
データから通過点Ｐ　ｉ、　Ｐ　ｉ　＋　１−Ｐ　ｉ十
ｍ、各経路の移動速度の最大値Ｖｊ（Ｖｉ、Ｖｉ　４−
１−Ｖ　ｉ　＋ｍ）を演算し、これらと最終目標関節位
置Ｏｏｉを予め演算プロセッサ２に定数としてセットし
且つマニピュレータの現在の関節変位ｅを取り込んで各
関節の現在位置を把握するものであり、経路演算部３０
を持つものである。

従って、この実施例では、ハンドの移動位置、速度は、
作業者に理解し易い直交基準座標系（Ｐｎ’、ＶＲ）で
扱い、これをハンド座標系に変換し、（ＶＨ）、更に関
節座標系での各関節速度ｅに変換することによって多関
節マニピュレータを速度制御するものである。

これらの関係は、第３図（Ａ）の座標系関係説明図に示
す如く、ロボットの基準座標系のＸｏ　−Ｙｏ−Ｚｏ直
交座標に対し、ハンド１０のハンド座標系は直交座標と
してベクトルｎ、０、ａで表わされ、ｉ番目の関節、ｙ
ｉによる座標変換行列Ａｉは、関節Ｊｉの変位θｉの関
数として表わされる。この時、基準座標系でのハンド１
０の位置ばＰｎ　（Ｘｎ、Ｙｎ、Ｚｎ）であり、Ａ、０
．、Ａ、、Ａ２、−１Ａ６とマニピュレータの最後のア
ームからのハンド１０の方向ベクｌ−ルｈで表わされる
。

次に、第２図実施例の動作について第３図動作説明図、
第４回速度パターン説明図を用いて説明する。

ホストプロセッサ３は、ティーチングデータを受けると
、通過点Ｐ　ｉ−Ｐ　ｔ　＋　ｍ　、移動速度の最大値
Ｖｊを演算し、これらと最終目標関節変位θｏ’ｘを演
算プロセッサ２に定数としてセットし、起動する。

演算プロセッサ２では、現在位置演算部２２で各関節変
位θから基準座標系でのハンド１０の現尚、Ａｏ、Ａ＋
、−・、Ａ６は各々基準座標系から関節Ｊ１の座標系へ
関節Ｊ１の座標系からＪ２の座標系へ、−関節Ｊ５の座
標系から関節Ｊ６の座標系への変換行列であり、各関節
角度Ｏにより定義される。

速度ベクトル生成演算部２３は、次式によって、目標位
置Ｐｉ＋１に向かう速度ベクトル■ｄｉを演算する。

即ち、その経路での最大速度ＩＶｊｌと通過点に向かう
単位方向ベクトルの積で与えられる。

又、力センサ１１の出力（反力）ＦＨを取り込み、座標
変換演算部２０で、次式により基準座標系表示の反力Ｆ
Ｒに変換する。

次に反力ＦＲから回避速度演算部２１が回避速度ベクト
ルＶｆを次式により演算する。

Ｖ　ｆ　＝　Ｋ　−Ｆ　Ｒ−−−−−−−−−（４）但
し、Ｋは速度変換パラメータである。

第３図（Ｃ）の力センサによる障害物回避動作説明図に
示す如く、力センサ１１の出力は、物体（障害物）４へ
の押し付げによる微小なたわみ量を電気的に変換したも
のであるから、回避速度ヘクトルｖｒは障害物４の接平
面に垂直方向であり、これによって係る垂直方向の抗力
を発生させ、障害物との衝突を避けることができる。

この時、速度変換パラメータには、第３図（Ｂ）に示す
如く出力ＦＲと回避速度の一次係数であるから、Ｋを図
のａ又はｂに変えることによって、回避速度ベクトルＶ
ｆの大きさを調整できる。

又、第３図（Ｂ）に示す如く、幅りの不感帯を設け、−
りからＤを不感帯とする不感帯特性を設ける。

とする。

これによって、一定の力で押し付けながら障害物を回避
することもできる。

合成速度ベクトル演算部２４は、各速度ベクトルＶｄｉ
及びＶｆを合成し、速度指令ベクトル７尺をえる。

この速度指令ベクトル７尺は基準座標系のため、ハンド
座標系の速度指令ベクトルＶ）Ｉに座標変換演算部２５
で次式により得られる。

尚、（３）式、（６）式のｎ、ｏ、ａは、マニピュレー
タの各関節の構成で決定される前述の座標変換行列Ａ　
ｏ　、−Ａ　ｓと、関節変位○で求められる。

更に、ハンド座標系の速度指令ベクトルＶ）Ｉと関節速
度θの関係は、 Ｖ　１４−Ｊθ　　　　　　−一−−−−−（７）テ表
わされ、Ｊはマニピュレータのヤコビ行列である。（７
）式より、特異点以外では、○＝Ｊ−１・Ｖ　Ｈ−−−
−（８） で与えられるから、関節速度指令演算部２６は、を演算
する。

この関節速度６は、関数発生演算部２７に与えられ、立
上り、立下りを所定の加減速カーブで変換した各関節の
指令速度を演算し、サーボ制御部１４へ与える。これを
各サンプリングタイム毎に演算プロセッサ２は実行し、
リアルタイムにマニピュレータ１を速度制御する。

又、目標位置ｐ　ｉ＋１に達すると、次の目標位置Ｐｉ
→２、移動速度Ｖｉ＋２によって同様に制御が行われる
。

このようにして、第１図及び第３図（Ｃ）に示す如く、
目標速度ベクトルＶｄｉとして目標位置ｐｉ→−１方向
のものを発生し、障害物４に当たり、力センサ１１によ
る反力Ｆ　Ｈが発生すると、回避速度ベクトルＶｆが演
算により発生し、ベクトルＶｄｉとＶｆの合成ベクトル
で速度制御が行われる。第４図はこれをＸ１Ｙ方向のス
カラ速度パターンで示しである。

又、障害物回避によって経路のずれが生じても目標速度
ベクトルＶｄｉは目標位置Ｐ　ｉ、　＋　１に向ってい
るので、目標位置に到達できる。回避速度ヘクＩ・ルＶ
ｆは約１０Ｈｚ程度までであるから、ＩＫ　Ｈｚ程度の
デジタル制御系で十分実現できる。

（ｂｌ　　他の実施例の説明 上述の実施例では、６軸多関節のマニピュレータで説明
したが、直交座標型マニピュレータやスカラ型マニビＪ
、レークに適用してもよく、例えば、直交座標型のもの
では、座標系の変換は必要ない。

又、障害物検出を力センサで行っているが、他の周知の
力覚センサ、距離センサ等を用いてもよい。

しかも、経路は、直線に限らず、軌道の通過点を細かく
与えて曲線経路としてもよい。

以上本発明を実施例により説明したが、本発明は本発明
の主旨に従い種々の変形が可能であり、本発明からこれ
らを排除するものではない。

〔発明の効果〕

以上説明した様に、本発明によれば、障害物回避におい
て、マニピュレータの軌跡上の障害物を回避し、且つ目
標位置に到達するこきが可能となり、その際途中で停止
させ、経路を再生成させる必要はないため、高速且つ円
滑に動作できるという効果を奏する。

又、その実現も、通常の制御系の経路生成部やザーボ制
御部に特別な変更や判断処理を追加させる必要はなく、
速度ベクトル生成演算部とセンサ信号処理機能を付与す
ればよく、容易且つ安価に実現できるという効果も奏し
、センサフィードバンク付ロボットに高度な障害物回避
機能を付与できる。

【図面の簡単な説明】

第１図は本発明の原理説明図、 第２図は本発明の詳細な説明図、 第３図は本発明の一実施例動作説明図、第４図は本発明
による速度パターン説明図、第５図は従来技術の説明図
である。 図中、１−マニピュレータ（ロボット）、１０−ハンド
、 １１−力センサ（検出手段）、 ２・−演算プロセッサ、 ３・−ホストプロセッサ。

Claims (1)

1. 【特許請求の範囲】 物体の存在を検出する検出手段（１１）を備えたロボッ
   ト（１）を、目標点に向かう目標速度指令ベクトル（Ｖ
   ｄ）と、該検出手段（１１）に従って作成する該物体か
   ら回避する方向の回避速度指令ベクトル（Ｖｆ）との合
   成によって運動制御するロボットの運動制御方法におい
   て、 該ロボットの現在位置に基づいて該目標点に向かう目標
   速度指令ベクトル（Ｖｄｉ）を生成することを特徴とす
   るロボットの運動制御方法。