JPH0758442B2 - ロボツトの運動制御方法 - Google Patents
ロボツトの運動制御方法Info
- Publication number
- JPH0758442B2 JPH0758442B2 JP61252754A JP25275486A JPH0758442B2 JP H0758442 B2 JPH0758442 B2 JP H0758442B2 JP 61252754 A JP61252754 A JP 61252754A JP 25275486 A JP25275486 A JP 25275486A JP H0758442 B2 JPH0758442 B2 JP H0758442B2
- Authority
- JP
- Japan
- Prior art keywords
- vector
- route
- speed command
- command vector
- robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【発明の詳細な説明】 〔目 次〕 概 要 産業上の利用分野 従来の技術 発明が解決しようとする問題点 問題点を解決するための手段（第１図） 作 用 実施例 （ａ） 一実施例の説明（第２図，第３図） （ｂ） 他の実施例の説明 発明の効果 〔概 要〕 目標速度指令ベクトルと，検出手段の物体検出出力によ
つて作成した回避速度指令ベクトルとでロボツトの障害
物回避制御を行なう運動制御方法において，障害物回避
による予定の経路からのずれを修正するため，現在位置
から予定の経路への垂線方向の経路復帰速度指令ベクト
ルを作成して運動制御することによつて，経路復帰のた
めの演算時間を短縮したものである。
つて作成した回避速度指令ベクトルとでロボツトの障害
物回避制御を行なう運動制御方法において，障害物回避
による予定の経路からのずれを修正するため，現在位置
から予定の経路への垂線方向の経路復帰速度指令ベクト
ルを作成して運動制御することによつて，経路復帰のた
めの演算時間を短縮したものである。
本発明は，マニピユレータ型ロボツトに物体検出センサ
を設け，物体検出センサの出力で障害物回避制御を行な
うロボツトの運動制御方法に関し，特に経路復帰のため
の指令を高速に演算しうるロボツトの運転制御方法に関
する。
を設け，物体検出センサの出力で障害物回避制御を行な
うロボツトの運動制御方法に関し，特に経路復帰のため
の指令を高速に演算しうるロボツトの運転制御方法に関
する。
マニピユレータ型ロボツトは、組立て等の種々の作業自
動化に利用されている。
動化に利用されている。
係るロボツトは，予定の経路を運動すべく，目標速度ベ
クトルが与えられるが，係る経路に障害物が存在する
と，その運動が防げられる。このため，マニピユレータ
にハンドに加わる力を検出する力覚センサ等の物体検出
センサを設け，係るセンサの出力により回避速度ベクト
ルを発生して，障害物を自動的に回避する障害物回避制
御が行なわれている。
クトルが与えられるが，係る経路に障害物が存在する
と，その運動が防げられる。このため，マニピユレータ
にハンドに加わる力を検出する力覚センサ等の物体検出
センサを設け，係るセンサの出力により回避速度ベクト
ルを発生して，障害物を自動的に回避する障害物回避制
御が行なわれている。
この障害物回避を行なうと、必然的に予定の経路からず
れるため，経路に復帰させるための経路復帰の制御が必
要となる。
れるため，経路に復帰させるための経路復帰の制御が必
要となる。
第４図は従来技術の説明図である。
ロボツト１はマニピユレータのハンド10に、ハンド10に
加わる力を検出する力センサ11が設けられており、ハン
ド10を予定の通過点Pi（Xi,Yi,Zi）,Pi+1,Pi+2…を通過
するように制御するものを示している。
加わる力を検出する力センサ11が設けられており、ハン
ド10を予定の通過点Pi（Xi,Yi,Zi）,Pi+1,Pi+2…を通過
するように制御するものを示している。
ハンド10が通過点Piから通過点Pi+1へ直線運動するよう
に経路RPが決定され，経路RP中に障害物４があるものと
する。
に経路RPが決定され，経路RP中に障害物４があるものと
する。
経路RPを直線運動するには，目標速度ベクトルVd（vx,v
y,vz）は，通過点PiからPi+1へ向う経路RPでの予じめ設
定した最大速さをVimaxとし，ハンド10の現在位置
をPn（Xn,Yn,Zn）とすると，次式で与えられる。
y,vz）は，通過点PiからPi+1へ向う経路RPでの予じめ設
定した最大速さをVimaxとし，ハンド10の現在位置
をPn（Xn,Yn,Zn）とすると，次式で与えられる。
即ち，最大速度と通過点Pi+1へ向う単位方向ベクトルの
積で得られる。
積で得られる。
従つて，点ｉから速度Vd0で進行してきたハンド10が位
置P1で障害物４に当つたときには、その反力を力センサ
11により検出し，反力方向に障害物回避の回避速度ベク
トルVf1（Vfx1,Vfy1,Vfz1）を生成し，ハンド10をVd1と
Vf1との合成で運動制御する。
置P1で障害物４に当つたときには、その反力を力センサ
11により検出し，反力方向に障害物回避の回避速度ベク
トルVf1（Vfx1,Vfy1,Vfz1）を生成し，ハンド10をVd1と
Vf1との合成で運動制御する。
このような速度指令ベクトルVd,Vfは，プロセツサの演
算によつて作成され，各サンプリングタイム毎に演算を
行なうから，演算時間中に，ハンド10が前述のVd1とVf1
の合成によつて図のP2の位置に行つたとすると，次の目
標速度指令ベクトルVd2は，第（１）式のPnにP2を代入
して演算により求める。
算によつて作成され，各サンプリングタイム毎に演算を
行なうから，演算時間中に，ハンド10が前述のVd1とVf1
の合成によつて図のP2の位置に行つたとすると，次の目
標速度指令ベクトルVd2は，第（１）式のPnにP2を代入
して演算により求める。
同様に，ハンド10がVd2で進行して，位置P3で再び障害
物４に当つた場合は，目標速度指令ベクトルVd3と回避
速度指令ベクトルVf3とで運動して，障害物回避と，経
路復帰制御が行なわれる。
物４に当つた場合は，目標速度指令ベクトルVd3と回避
速度指令ベクトルVf3とで運動して，障害物回避と，経
路復帰制御が行なわれる。
従来技術においては，第（１）式で明らかな如く，常に
通過点Pi+1に向う方向の目標速度指令ベクトルVdを演算
することによつて，回避速度ベクトルVfによつて障害物
回避動作した結果に伴なう経路RPからのずれを修正して
経路復帰制御するものであつた。
通過点Pi+1に向う方向の目標速度指令ベクトルVdを演算
することによつて，回避速度ベクトルVfによつて障害物
回避動作した結果に伴なう経路RPからのずれを修正して
経路復帰制御するものであつた。
しかしながら，従来技術では，通過点Pi+1に向う目標速
度指令ベクトルVdを第（１）式によつて演算する必要が
あり，第（１）式中のPi+1−Pnは平方根演算とな
る。即ち，Pi+1−Pnは次式で示される。
度指令ベクトルVdを第（１）式によつて演算する必要が
あり，第（１）式中のPi+1−Pnは平方根演算とな
る。即ち，Pi+1−Pnは次式で示される。
このような目標速度指令ベクトルVdの演算に，平方根演
算が含まれていると，目標速度指令ベクトルVdの演算に
時間がかかり，障害物回避において特に重要とされてい
る目標速度指令ベクトル生成のリアルタイム性を大きく
損ねるという問題が生じていた。
算が含まれていると，目標速度指令ベクトルVdの演算に
時間がかかり，障害物回避において特に重要とされてい
る目標速度指令ベクトル生成のリアルタイム性を大きく
損ねるという問題が生じていた。
即ち，障害物回避においては，目標速度指令ベクトル及
び回避速度指令ベクトルを力センサの出力及びバンドの
位置に応じて，出来るだけ高速に，即ち，リアルタイム
に作成することが，応答性の良い円滑な障害物回避制御
及び経路復帰制御に必要となるが，リアルタイムに速度
ベクトルを作成できないため，ロボツトのスピードを遅
くしないと，円滑な障害物回避制御及び経路復帰制御が
困難となつていた。
び回避速度指令ベクトルを力センサの出力及びバンドの
位置に応じて，出来るだけ高速に，即ち，リアルタイム
に作成することが，応答性の良い円滑な障害物回避制御
及び経路復帰制御に必要となるが，リアルタイムに速度
ベクトルを作成できないため，ロボツトのスピードを遅
くしないと，円滑な障害物回避制御及び経路復帰制御が
困難となつていた。
本発明は，速度指令ベクトルをリアルタイムに作成し
て，円滑な障害物回避制御及び経路復帰制御の可能なロ
ボツトの運動制御方法を提供することを目的とする。
て，円滑な障害物回避制御及び経路復帰制御の可能なロ
ボツトの運動制御方法を提供することを目的とする。
第１図は本発明の原理説明図である。
図中，第４図で示したものと同一のものは同一の記号で
示してあり,Vk,Vk1,Vk2,Vk3,Vk4,Vk5は経路復帰指令速
度ベクトルであり，ロボツト（ハンド）の現在位置から
予定の経路RPへの垂線方向の成分を有し，その垂線の距
離ｌに比例した大きさのものである。
示してあり,Vk,Vk1,Vk2,Vk3,Vk4,Vk5は経路復帰指令速
度ベクトルであり，ロボツト（ハンド）の現在位置から
予定の経路RPへの垂線方向の成分を有し，その垂線の距
離ｌに比例した大きさのものである。
本発明は，通過点PiとPi+1の経路RP内であれば、現在位
置Pnに依らず，目標速度指令ベクトルVdiの方向を一定
とし，障害物回避により経路RPからはずれたときは，現
在位置Pnから経路RPへの垂直方向に経路復帰速度ベクト
ルVkが生成され，一定の目標速度指令ベクトルVdiと
し，回避速度指令ベクトルVfと，経路復帰速度ベクトル
Vkとの合成によつて運動制御しようとするものである。
置Pnに依らず，目標速度指令ベクトルVdiの方向を一定
とし，障害物回避により経路RPからはずれたときは，現
在位置Pnから経路RPへの垂直方向に経路復帰速度ベクト
ルVkが生成され，一定の目標速度指令ベクトルVdiと
し，回避速度指令ベクトルVfと，経路復帰速度ベクトル
Vkとの合成によつて運動制御しようとするものである。
即ち、本発明は、物体の存在を検出する検出手段を備え
たロボツトを、予定の経路（RP）に従って運動するよう
与えた目標速度指令ベクトル（Vdi）と、該検出手段の
出力に従って作成する該物体から回避する方向の回避速
度指令ベクトル（Vf）とによって運動制御するロボツト
の運動制御方法において、該目標速度指令ベクトルと該
ロボツトの現在位置ベクトルと該ロボツトの通過点位置
ベクトルとにより、該ロボツトの現在位置から該経路
（RP）上への垂線方向で且つ該垂線の距離に比例した大
きさの経路復帰速度指令ベクトル（Vk）を作成し、 該目標速度指令ベクトル（Vdi）と該回避速度指令ベク
トル（Vf）と該経路復帰速度ベクトル（Vk）との合成に
よって運動制御することを特徴としている。
たロボツトを、予定の経路（RP）に従って運動するよう
与えた目標速度指令ベクトル（Vdi）と、該検出手段の
出力に従って作成する該物体から回避する方向の回避速
度指令ベクトル（Vf）とによって運動制御するロボツト
の運動制御方法において、該目標速度指令ベクトルと該
ロボツトの現在位置ベクトルと該ロボツトの通過点位置
ベクトルとにより、該ロボツトの現在位置から該経路
（RP）上への垂線方向で且つ該垂線の距離に比例した大
きさの経路復帰速度指令ベクトル（Vk）を作成し、 該目標速度指令ベクトル（Vdi）と該回避速度指令ベク
トル（Vf）と該経路復帰速度ベクトル（Vk）との合成に
よって運動制御することを特徴としている。
本発明では、経路復帰方向の目標速度指令ベクトルを生
成する代りに，経路復帰速度ベクトルVkを生成してい
る。
成する代りに，経路復帰速度ベクトルVkを生成してい
る。
即ち，従来の経路復帰方向の目標速度指令ベクトルは，
経路方向の速度ベクトル，即ち目標速度指令ベクトルV
diと，これと垂直の速度ベクトル，即ち経路復帰速度べ
クトルVkに分解できる。
経路方向の速度ベクトル，即ち目標速度指令ベクトルV
diと，これと垂直の速度ベクトル，即ち経路復帰速度べ
クトルVkに分解できる。
従つて，速度ベクトルVdiを一定とし，速度ベクトルVk
を現在位置Pnに応じて生成してやれば，障害物回避によ
つて生じる路RPからの位置ずれを修正でき，速度ベクト
ルVdiの方向は位置によらず，一定のため，単位方向ベ
クトルを位置に応じて演算しなくてよいから，複雑な平
方根演算を要せず，演算時間が短縮できる。
を現在位置Pnに応じて生成してやれば，障害物回避によ
つて生じる路RPからの位置ずれを修正でき，速度ベクト
ルVdiの方向は位置によらず，一定のため，単位方向ベ
クトルを位置に応じて演算しなくてよいから，複雑な平
方根演算を要せず，演算時間が短縮できる。
即ち，第１図（Ａ）の例では，ハンドが障害物４に当た
らない時は,Vf＝Vk＝０のため,Vdiで運動し続ける。一
方，障害物４に当つた場合，例えば位置P1,P2,P3,P4で
は,Vfが発生し，且つ位置に応じてVkが生成されるから,
Vdi,Vf,Vkにより運動し，障害物４を回避し終つたと
き，位置P5では,Vf＝０のため,VdiとVkで運動する。Vk
は，現在位置Pnと経路RPとの距離ｌに比例した大きさの
ため，ロボツト（ハンド）が経路RP上に復帰した際に０
となり，従つて，ハンドはVdiで定速運動する。
らない時は,Vf＝Vk＝０のため,Vdiで運動し続ける。一
方，障害物４に当つた場合，例えば位置P1,P2,P3,P4で
は,Vfが発生し，且つ位置に応じてVkが生成されるから,
Vdi,Vf,Vkにより運動し，障害物４を回避し終つたと
き，位置P5では,Vf＝０のため,VdiとVkで運動する。Vk
は，現在位置Pnと経路RPとの距離ｌに比例した大きさの
ため，ロボツト（ハンド）が経路RP上に復帰した際に０
となり，従つて，ハンドはVdiで定速運動する。
（ａ）一実施例の説明 第２図は本発明の一実施例説明図である。
図中，第４図で示したものと同一のものは同一の記号で
示してあり,12はアクチユエータであり，モータで構成
され，マニピユレータ１の各関節を駆動するものであ
り，この例ではマニピユレータ１が６軸の多関節型で構
成されていることから６軸分のアクチユエータを備える
もの,13はエンコーダであり，各軸のアクチユエータ12
による関節変位（θ1,θ2,θ3,θ4,θ5,θ６）を検出
するもの,14はサーボ制御部であり，後述する演算プロ
セツサから与えられる各軸の指令速度に追従する様に各
軸のアクチユエータ12をサーボ制御するものである。
示してあり,12はアクチユエータであり，モータで構成
され，マニピユレータ１の各関節を駆動するものであ
り，この例ではマニピユレータ１が６軸の多関節型で構
成されていることから６軸分のアクチユエータを備える
もの,13はエンコーダであり，各軸のアクチユエータ12
による関節変位（θ1,θ2,θ3,θ4,θ5,θ６）を検出
するもの,14はサーボ制御部であり，後述する演算プロ
セツサから与えられる各軸の指令速度に追従する様に各
軸のアクチユエータ12をサーボ制御するものである。
従つて，マニピユレータ（ロボツタ）１は６軸の多関節
型で構成され，ハンド10と手首部の間に力センサ（力検
出装置）11が設けられている。
型で構成され，ハンド10と手首部の間に力センサ（力検
出装置）11が設けられている。
２は演算プロセツサであり，例べばDSP（デジタルシグ
ナルプロセツサ）で構成され，後述する如くホストプロ
セツサから最終目標関節変位0i,目標速度指令ベクト
ルVdi,復帰ゲインαi,通過位置Pi,内積値の逆数（Vdi・
Vdi）-1が入力され，マニピユレータ１から現在の関節
変位を取り込み，力センサ11の検出出力FH（ハンド座
標系での直交座標成分）を取り込み，回避速度演算，現
在位置演算，経路復帰速度演算等を行ない，各サンプリ
ングタイム毎に各軸の指令速度をサーボ制御14に与える
ものである。
ナルプロセツサ）で構成され，後述する如くホストプロ
セツサから最終目標関節変位0i,目標速度指令ベクト
ルVdi,復帰ゲインαi,通過位置Pi,内積値の逆数（Vdi・
Vdi）-1が入力され，マニピユレータ１から現在の関節
変位を取り込み，力センサ11の検出出力FH（ハンド座
標系での直交座標成分）を取り込み，回避速度演算，現
在位置演算，経路復帰速度演算等を行ない，各サンプリ
ングタイム毎に各軸の指令速度をサーボ制御14に与える
ものである。
20は座標変換演算部であり，力センサ11の出力（反力）
FHを後述する基準座標系の反力FRに座標変換演算するも
の,21は回避速度演算部であり，反力FRから反力方向を
回避速度指令ベクトルVf（基準座標系）を演算するも
の,22は現在位置演算部であり，各関節の現在の変位
から基準座標系でのハンドの現在位置Pnを演算するも
の,23は経路復帰速度演算部23であり，後述する如く，
目標速度指令ベクトルVdiを発するとともに,Vdi,復帰ゲ
インαi,内積の逆数（Vdi・Vdi）-1,現在位置Pn,通過点
Piから，経路復帰速度指令ベクトルVkを演算するもの,2
4は合成速度ベクトル演算部であり,Vdi,Vk,Vfを加算し
て基準座標系での速度指令ベクトルVRを演算するもの,2
5は座標変換演算部であり，基準座標系での速度指令ベ
クトルVRをハンド座標系の速度指令ベクトルVHに変換す
るもの,26は関節速度指令演算部であり，ハンド座標系
の速度指令ベクトルVHを各関節の速度指令ベクトル に変換するもの,27は関数発生演算部であり，各関節の
速度指令ベクトル の立上り，立下りを予定の加速，減速カーブで規定して
各関節の指令速度を演算するものである。
FHを後述する基準座標系の反力FRに座標変換演算するも
の,21は回避速度演算部であり，反力FRから反力方向を
回避速度指令ベクトルVf（基準座標系）を演算するも
の,22は現在位置演算部であり，各関節の現在の変位
から基準座標系でのハンドの現在位置Pnを演算するも
の,23は経路復帰速度演算部23であり，後述する如く，
目標速度指令ベクトルVdiを発するとともに,Vdi,復帰ゲ
インαi,内積の逆数（Vdi・Vdi）-1,現在位置Pn,通過点
Piから，経路復帰速度指令ベクトルVkを演算するもの,2
4は合成速度ベクトル演算部であり,Vdi,Vk,Vfを加算し
て基準座標系での速度指令ベクトルVRを演算するもの,2
5は座標変換演算部であり，基準座標系での速度指令ベ
クトルVRをハンド座標系の速度指令ベクトルVHに変換す
るもの,26は関節速度指令演算部であり，ハンド座標系
の速度指令ベクトルVHを各関節の速度指令ベクトル に変換するもの,27は関数発生演算部であり，各関節の
速度指令ベクトル の立上り，立下りを予定の加速，減速カーブで規定して
各関節の指令速度を演算するものである。
尚，各演算部20〜27は演算プロセツサ２が行なう演算を
ブロツクとして示したものである。
ブロツクとして示したものである。
３はホストプロセツサであり，与えられたテイーチング
データから通過点Pi,Pi+1…Pi+m,各経路の目標速度指令
ベクトルVdi,Vdi+1…Vdi+m,各経路の復帰ゲインαi,内
積の逆数（Vdi・Vdi）-1,…（Vdm・Vdm）-1を演算し，
これらと最終目標関節変位oiを予じめ演算プロセツサ
２に定数としてセツトし且つマニピユレータの現在の関
節変位を取り込んで各関節の現在位置を把握するもの
であり，経路演算部30を持つものである。
データから通過点Pi,Pi+1…Pi+m,各経路の目標速度指令
ベクトルVdi,Vdi+1…Vdi+m,各経路の復帰ゲインαi,内
積の逆数（Vdi・Vdi）-1,…（Vdm・Vdm）-1を演算し，
これらと最終目標関節変位oiを予じめ演算プロセツサ
２に定数としてセツトし且つマニピユレータの現在の関
節変位を取り込んで各関節の現在位置を把握するもの
であり，経路演算部30を持つものである。
従つて，この実施例では，ハンドの移動位置，速度は，
作業者に理解し易い直交基準座標系（Pn,VR）で扱い，
これをハンド座標系に変換し（VH），更に関節座標系で
の各関節速度 に変換することによつて多関節マニピユレータを速度制
御するものである。
作業者に理解し易い直交基準座標系（Pn,VR）で扱い，
これをハンド座標系に変換し（VH），更に関節座標系で
の各関節速度 に変換することによつて多関節マニピユレータを速度制
御するものである。
これらの関係は，第３図の座標系関係説明図に示す如
く，ロボツトの基準座標系のX0−Y0−Z0直交座標に対
し，ハンド10のハンド座標系は直交座標としてベクトル
，，で表わされ,i番目の関節Jiによる座標変換行
列はAiは，関節Jiの変位θｉの関節として表わされる。
この時，基準座標系でのハンド10の位置はPn（Xn,Yn,
Zn）であり,A0,A1,A2,…,A6とマニピユレータの最後の
アームからのハンド10の方向ベクトルで表わされる。
く，ロボツトの基準座標系のX0−Y0−Z0直交座標に対
し，ハンド10のハンド座標系は直交座標としてベクトル
，，で表わされ,i番目の関節Jiによる座標変換行
列はAiは，関節Jiの変位θｉの関節として表わされる。
この時，基準座標系でのハンド10の位置はPn（Xn,Yn,
Zn）であり,A0,A1,A2,…,A6とマニピユレータの最後の
アームからのハンド10の方向ベクトルで表わされる。
次に，第２図実施例の動作について説明する。
ホストプロセツサ３は，テイーチングデータを受ける
と，通過点Pi…Pi+m,目標速度指令ベクトルVi…Vi+m,内
積の逆襲（Vdi,Vdi）-1…（Vdm,Vdm）-1を演算し，これ
らと復帰ゲインαi,最終目標関節変位0iを演算プロセ
ツサ２に定数としてセツトし，起動する。
と，通過点Pi…Pi+m,目標速度指令ベクトルVi…Vi+m,内
積の逆襲（Vdi,Vdi）-1…（Vdm,Vdm）-1を演算し，これ
らと復帰ゲインαi,最終目標関節変位0iを演算プロセ
ツサ２に定数としてセツトし，起動する。
演算プロセツサ２では，現在位置演算部22で各関節変位
から基準座標系でのハンド10の現在位置Pnを次式によ
り演算する。
から基準座標系でのハンド10の現在位置Pnを次式によ
り演算する。
尚,A0,A1,…,A6は各々基準座標系から関節J1の座標系へ
関節J1の座標系からJ2の座標系へ，…関節J5の座標系か
ら関節J6の座標系への変換行列であり，各関節角度に
より定義される。
関節J1の座標系からJ2の座標系へ，…関節J5の座標系か
ら関節J6の座標系への変換行列であり，各関節角度に
より定義される。
経路復帰速度演算部23は，与えられた目標速度ベクトル
Vdiを演算部24へ送り込むとともに，次式により，現在
位置Pnから経路復帰速度ベクトルVkを演算する。
Vdiを演算部24へ送り込むとともに，次式により，現在
位置Pnから経路復帰速度ベクトルVkを演算する。
先づ，第１図（Ｂ）に示す如く，位置Pnから通過点Pi→
Pi+1の経路RP上に下した垂線の足の座標Prnは， である。
Pi+1の経路RP上に下した垂線の足の座標Prnは， である。
又,VkをPnと経路RPとの距離ｌに比例して与えるから，
復帰ゲインαｉを用いて， となる。
復帰ゲインαｉを用いて， となる。
（５）式中，αi,Pi,Vdi,（Vdi,Vdi）-1は定数で与えら
れているから，現在位置Pnを得ることによつて（５）式
は簡単に演算できる。
れているから，現在位置Pnを得ることによつて（５）式
は簡単に演算できる。
又，力センサ11の出力（反力）FHを取り込み，座標変換
演算部20で，次式により基準座標系表示の反力FRに変換
する。
演算部20で，次式により基準座標系表示の反力FRに変換
する。
次に反力FRを回避速度演算部21が二次遅れ演算して，反
力方向の回避速度ベクトルVfを求める。
力方向の回避速度ベクトルVfを求める。
合成速度ベクトル演算部24は，各速度ベクトルVdi,Vk,V
fを合成し，速度指令ベクトルVRをえる。
fを合成し，速度指令ベクトルVRをえる。
この速度指令ベクトルVRは基準座標系のため，ハンド座
標系の速度指令ベクトルVHに座標変換演算部25で次式に
より得られる。
標系の速度指令ベクトルVHに座標変換演算部25で次式に
より得られる。
尚，（６）式，（７）式の，，は，マニピユレー
タの各関節の構成で決定される前述の座標変換行列A0,
…A6と，関節変位で求められる。
タの各関節の構成で決定される前述の座標変換行列A0,
…A6と，関節変位で求められる。
更に，ハンド座標系の速度指令ベクトルVHと関節速度 の関係は， で表わされ,Jはマニピユレータのヤコビ行列である。
（８）式より，特異点以外では， で与えられるから，関節速度指令演算部26は，（９）式
により関節速度 を演算する。
（８）式より，特異点以外では， で与えられるから，関節速度指令演算部26は，（９）式
により関節速度 を演算する。
この関節速度 関数発生演算部27に与えられ，立上り，立下りを所定の
加減速カーブで変換した各関節の指令速度を演算し，サ
ーボ制御部14へ与える。これを各サンプリングタイム毎
に演算プロセツサ２は実行し，リアルタイムにマニピユ
レータ１を速度制御する。
加減速カーブで変換した各関節の指令速度を演算し，サ
ーボ制御部14へ与える。これを各サンプリングタイム毎
に演算プロセツサ２は実行し，リアルタイムにマニピユ
レータ１を速度制御する。
又，現在位置PnがPi+1に達すると，次の目標速度ベクト
ルVdi+1,復帰ゲインαi+1…を用いて演算を行なう。
ルVdi+1,復帰ゲインαi+1…を用いて演算を行なう。
このようにして，経路RPでは，目標速度ベクトルVdiは
一定とし，障害物が存在しないと,Vf＝Vk＝０であり，
障害物に当たり，力センサ11による反力FHが発生する
と，回避速度ベクトルVfが演算により発生し，これとと
もに回路RPからの位置ずれにより第（５）式によつて復
帰速度ベクトルVkが演算され，これらの速度ベクトルV
di,Vf,Vkの合成で速度制御が行なわれる。この場合,Vk
を求める第（５）式は，単なるベクトル乗算であり，平
方根演算を含んでいないので，リアルタイムにVkを演算
できる。
一定とし，障害物が存在しないと,Vf＝Vk＝０であり，
障害物に当たり，力センサ11による反力FHが発生する
と，回避速度ベクトルVfが演算により発生し，これとと
もに回路RPからの位置ずれにより第（５）式によつて復
帰速度ベクトルVkが演算され，これらの速度ベクトルV
di,Vf,Vkの合成で速度制御が行なわれる。この場合,Vk
を求める第（５）式は，単なるベクトル乗算であり，平
方根演算を含んでいないので，リアルタイムにVkを演算
できる。
又，演算に時間がかからないから，汎用のDSPを用い
て，リアルタイムに速度指令作成ができる。
て，リアルタイムに速度指令作成ができる。
（ｂ） 他の実施例の説明 上述の実施例では,6軸多関節のマニピユレータで説明し
たが，直交座標型マニピユレータやスカラ型マニピユレ
ータに適用してもよく，例えば，直交座標型のもので
は，座標系の変換は必要ない。又，障害物検出を力セン
サで行つているが，他の周知の力覚センサ，距離センサ
党を用いてもよい。
たが，直交座標型マニピユレータやスカラ型マニピユレ
ータに適用してもよく，例えば，直交座標型のもので
は，座標系の変換は必要ない。又，障害物検出を力セン
サで行つているが，他の周知の力覚センサ，距離センサ
党を用いてもよい。
以上本発明を実施例により説明したが，本発明は本発明
の主旨に従い種々の変形が可能であり，本発明からこれ
らを排除するものではない。
の主旨に従い種々の変形が可能であり，本発明からこれ
らを排除するものではない。
以上説明した様に、本発明によれば」次の効果を奏す
る。
る。
障害物回避において、速度指令ベクトルをリアルタイ
ムに作成できるから、障害物回避と経路復帰制御を円滑
に行うことができ、応答性を改善し、ロボツトのスピー
ドを遅くすることなく、これら制御を実行できる。
ムに作成できるから、障害物回避と経路復帰制御を円滑
に行うことができ、応答性を改善し、ロボツトのスピー
ドを遅くすることなく、これら制御を実行できる。
目標速度と現在位置と通過点位置により経路復帰速度
指令ベクトルを作成するので、高速に速度指令ベクトル
が作成でき、障害物をなめるような障害物回避経路で移
動でき、より障害物を回避しても高速に移動できる。
指令ベクトルを作成するので、高速に速度指令ベクトル
が作成でき、障害物をなめるような障害物回避経路で移
動でき、より障害物を回避しても高速に移動できる。
第１図は本発明の原理説明図， 第２図は本発明の一実施例説明図， 第３図は本発明の一実施例座標系関係説明図， 第４図は従来技術の説明図である。 図中,1……マニピユレータ（ロボツト）， 10……ハンド， 11……力センサ（検出手段）， ２……演算プロセツサ， ３……ホストプロセツサ。
フロントページの続き (72)発明者 吉田 豊 神奈川県川崎市中原区上小田中1015番地 富士通株式会社内 (56)参考文献 特開 昭60−10309（ＪＰ，Ａ)
Claims (1)
- 【請求項１】物体の存在を検出する検出手段を備えたロ
ボツトを、予定の経路（RP）に従って運動するよう与え
た目標速度指令ベクトル（Vdi）と、 該検出手段の出力に従って作成する該物体から回避する
方向の回避速度指令ベクトル（Vf）とによって運動制御
するロボツトの運動制御方法において、 該目標速度指令ベクトルと該ロボツトの現在位置ベクト
ルと該ロボツトの通過点位置ベクトルとにより、該ロボ
ツトの現在位置から該経路（RP）上への垂線方向で且つ
該垂線の距離に比例した大きさの経路復帰速度指令ベク
トル（Vk）を作成し、 該目標速度指令ベクトル（Vdi）と該回避速度指令ベク
トル（Vf）と該経路復帰速度指令ベクトル（Vk）との合
成によって運動制御することを特徴とするロボツトの運
動制御方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP61252754A JPH0758442B2 (ja) | 1986-10-23 | 1986-10-23 | ロボツトの運動制御方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP61252754A JPH0758442B2 (ja) | 1986-10-23 | 1986-10-23 | ロボツトの運動制御方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPS63106009A JPS63106009A (ja) | 1988-05-11 |
| JPH0758442B2 true JPH0758442B2 (ja) | 1995-06-21 |
Family
ID=17241819
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP61252754A Expired - Lifetime JPH0758442B2 (ja) | 1986-10-23 | 1986-10-23 | ロボツトの運動制御方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH0758442B2 (ja) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08197342A (ja) * | 1995-01-18 | 1996-08-06 | Mazda Motor Corp | 自動組付装置 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS6010309A (ja) * | 1983-06-30 | 1985-01-19 | Hitachi Ltd | ロボツトハンドの経路補間方法 |
-
1986
- 1986-10-23 JP JP61252754A patent/JPH0758442B2/ja not_active Expired - Lifetime
Also Published As
| Publication number | Publication date |
|---|---|
| JPS63106009A (ja) | 1988-05-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP0086950A1 (en) | Method of controlling an industrial robot | |
| JPS59107884A (ja) | ロボツトの制御方式 | |
| JPH0457688A (ja) | 慣性系におけるロボットの制御装置 | |
| JPH0413726B2 (ja) | ||
| CN117245663A (zh) | 绳驱空间柔性机械臂力位型融合柔顺控制方法及系统 | |
| JPH0693209B2 (ja) | ロボツトの円弧補間姿勢制御装置 | |
| JPH0758442B2 (ja) | ロボツトの運動制御方法 | |
| JPS63150183A (ja) | ロボツトの運動制御方法 | |
| JP2003062775A (ja) | 人間型ハンドロボット用教示システム | |
| JPS63149707A (ja) | ロボツトの速度指令生成方法 | |
| JPH0630012B2 (ja) | 産業用ロボツトの制御方法 | |
| JP2737325B2 (ja) | ロボットの軌道生成方式 | |
| JP3078884B2 (ja) | 倣い制御装置 | |
| JPS6016385A (ja) | ロボツトの制御方法 | |
| JPH0889859A (ja) | 塗装用ロボットの動作教示方法及び装置 | |
| Connolly et al. | Cooperating manipulator control using dual quaternion coordinates | |
| JP3089027B2 (ja) | ロボット倣い制御装置 | |
| JPS6010309A (ja) | ロボツトハンドの経路補間方法 | |
| JPS5810197B2 (ja) | エンカツソウジユウソウチ | |
| JP2909074B2 (ja) | ロボットアームの動作シミュレータ装置 | |
| JPS6024603A (ja) | ロボツトの制御方法 | |
| JPH077300B2 (ja) | ロボット制御装置 | |
| JPH07104687B2 (ja) | ロボットの経路制御方法 | |
| JP2773778B2 (ja) | 力制御ロボットの倣い装置 | |
| JP2926623B2 (ja) | 位置ずれ補正装置 |