JPS6024603A - ロボツトの制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明はロボットの制御方法に係り、特に多関節型ロボ
ット・アームを周囲の障害物を回避しながら動作させる
のに好適な制御方法に関する。

〔発明の背景〕

従来、多関節型ロボットの位置決め方法として次のよう
なものがある。

（１）　人間がロボットの先端を目標軌跡に沿って移動
させ、その際の各関節の動作速度を順次記憶させておき
、ロボットの自動動作時にこの記憶を順次読み出しこの
値に従ってロボットの動作を制御する、いわゆる「直接
教示方式」。

（２）　ロボット先端位置を所定の空間座標系で指示し
、これをロボットの各関節角度に座標変換し、この値に
基づいてロボットの動作を制御する、いわゆる「間接教
示方式」。

しかし、「直接教示方式」を冗長関節をもつ多関節型ロ
ボットの制御に用いた場合、ロボット先端位置を目標軌
跡上に固定しただけではロボット・アーム全体の姿勢が
一つに定まらないため、ロボット先端を目標位置に固定
しつつロボット・アーム全体の姿勢をも所望の形状に保
持しなければならず、教示作業が極めて繁雑で困難なも
のになるという問題があった。

また、「間接教示方式」に関しても、ロボット先端位置
を制御する実用的な手法が提案されてはいるものの（特
願昭５７−２１５４２５号）、アーム全体の姿勢を任意
に制御できるものではないため、周囲の障害物を回避し
ながらロボットを動作させるｊ局舎には適用が困難であ
った。

〔発明の目的〕

本発明の目的は、上記のような従来技術の欠点を解消し
、複雑な評価関数、拘束柔性等を用いることなく、リア
ルタイムで冗長関節を有する多関節型ロボットを指定さ
れた軌跡に沿って、周囲の障害物を回避しつつ目標位置
へ誘導するために要するロボットの各関節に対する操作
量を自動的に創成し得るロボットの制御方法を提供する
ことにあるＯ 〔発明の概要〕 上記目的を達成するため、本発明は、冗長関節を有する
多関節型ロボットの先端を目標軌跡に沿って目標位置へ
誘導する制御方法において、前記多関節型ロボットの作
業空間を形成する構造物、および前記多関節型ロボット
の幾何モデル（以下、これら幾何モデルをそれぞれモデ
ル・ロボット、モデル構造物と呼ぶ）を用い、作業空間
座標系で指示された前記多関節型ロボットの移動速度を
前記モデル・ロボットの関節座標系に座標変換し、この
値に従って該モデル・ロボットを単位時間動作させ、前
記モデル構造物との衝突が発生するか否かを該モデル・
ロボットの先端リンク側から順次調べ、衝突が生じない
場合には該モデル・ロボットの関節動作速度に従って現
実の前記多関節型ロボットを動作させ、もし衝突が生じ
ているならば、衝突リンクより先端側のリンクの位置を
変えずに、該衝突リンクと前記モデル構造物との衝突を
回避させるための該衝突リンクの前記作業空間座標系に
おける移動速度を、前記モデル・ロボットの関節動作速
度に座標変換し、この値に基づき該モデル・ロボットを
単位時間動作させて衝突回避動作を行い、再び該モデル
・ロボットと前記モデル構造物との衝突を先端側のリン
クから順次調べ、この結果に応じて前記衝突回避動作を
行い、衝突が前記モデル・ロボットの全てのリンクにお
いて回避されるまでこれを繰返し、最後の衝突回避動作
終了時における前記モデル・ロボットの関節位置の初期
関節位置に対する増分値から前記多関節型ロボットの実
稼働時の目標関節動作速度をめ、これに従って現実の前
記多関節型ロボットを動作さぜることによシ、現実の前
記作業空間における前記構造物への衝突を回避しながら
前記目標軌跡に沿って、前記多関節型ロボットの先端を
前記目標位置へ移動制御できるようにしたことを特徴と
する。

〔発明の実施例〕

以下、本発明を実施例に基づいて詳細に説明する。

第１図は、本発明の一実施例における冗長関節を有する
多関節型ロボットの機構構造の概略図である。

このロボットはｘｙ平面内の位置決めを行うための３つ
の回転型関節Ｒ１，Ｒ２、Ｒ３と、これらを連接する３
つのリンクＬ１　、Ｌ２　、Ｌｓから構成されている。

リンクＬ１　、Ｌ２　、Ｌａの長さは、それぞれｚｔ　
＋　７２　＋　’３であシ、Ｒ４はリンクＬ３の先端、
すなわちロボットの先端位置を表す。

ここで、リンクＬｌの基部、すなわち関節Ｒ。

は直交座標系−ｘｙの原点に位置し、各関節ＲＩ＋Ｒ２
、Ｒ３の回転角度をそれぞれＸ軸からθ１１リンクＬ１
からθ２、リンクＬ２からθ３とすると、ロボットの各
関節（Ｒ＋　、Ｒ２、Ｒ，３）　、および先端几。の位
置Ｘｎは、ロボットの幾何学的構造から次式のように表
される。

ｋ＝１　ｊにｌ ただし、ｘｌ−ｙｌ−０，１１＝２．３．４でＴは転置
行列を表す。

ここで、 とおくと、 Ｘｎ＝ｆ（θ）（３） ただし、 θ−（θｌ　θ２　θ３）Ｔ ｆ＝＝　（ｆ　ｎｌｆ　Ｒ２） と表せる。

（１）式を時間微分すると次式が導かれる。

ただし、ｎ＝２．３．４である。

ここで、 とおくと、（４）式は Ｘｎ、＝Ｊ１１０　（６） ただし、 と書き直せる。

ロボットの関節位置Ｘｎを指令速度Ｘｎに従って移動さ
せるためには、特願昭５７−２１５４２５号に示されて
いるように、（５）式を用いて各関節の角速度θ。を のように制御すればよい。

ただし、 ｊＩ、に−ＪｌｌＣｋ で、Ｃｋは冗長関節を固定するための縮退化行列であシ
、 と定義する。

今、第２図に示すように、円形の障害物１０００回りに
破線で示す円形の軌跡２００に沿ってロボットの先端Ｒ
ｈを移動制御する場合について説明する。

円形の障害物１００の外壁の形状は、関節Ｒ１を原点と
し、障害物１００の中心をｐ（ａ、ｂ）とすると、 で表される。

また、ロボットの先端Ｒｈの指令軌跡ｘｈは、と表され
るので、ロボット先端の指令速度Ｘｈは、と与えられる
。ここで、本実施例の説明において各記号の添字りはロ
ボットの先端を示すものとする。

本実施例においては、まず第１ステツプとして、（１）
式に従って、モデル上でロボットの先端位置Ｘｈを指令
速度Ｘｈにより移動させる。すなわち、と与えられる。

これに従ってモデル・ロボットを単位時間Δｔだけ動作
させると、モデル・ロボットの関節角度は初期（現在）
関節角度θ　から新関節角度θ″＋″へ・変化する。す
なわち、新関節角度θｔ＋７は θ　＝θ　」−θｈΔｔ０２） となる。

シタカッて、モデル・ロボットの各関節位置は、初期（
現在）関節位置Ｘ六から新開節位置Ｘｎへ次式のように
変化する。

一方、ロボットの各関節位置Ｘｎの障害物１００の中心
ｐ（ａ、ｂ）からの距離ｒｎは、ｒ　ｎ−ｒ　ｎ　（Ｘ
　ｎ） と表される。したがって、ロボットの先端位置Ｘｈの軌
跡２００が指足された際、第３図に示すように先端リン
クから順次、障害物１００に最も接近した時のロボット
各関節の障害物中心Ｐからの最小距離ｒＯｎは次式で与
えられる。

ただし、 ｒ０＝ｒｈ （ｒＨはロボット先端Ｘｈの障害物の中心Ｐがらの距離
を表す） である。

すなわち、ロボットの各関節位置Ｘｎを障害物の中心Ｐ
から０■式で示される最小距離ｒｏｎ以上離せば、ロボ
ットを構成するリンクＬ　１１と障害物１００との衝突
を必ず回避できることになる。

換言すれば、 ｒｎ≧ｒ　％ となるように各関節位置Ｘｎを制御することは、ロボッ
トと障害物１００の衝突を回避するための十分条件を与
えることになる。

そこで、９０式に従ってモデル・ロボットを動作させた
後、第２ステツプとして、ａ□□□式で示される各関節
位置が０９式で与えられる障害物への衝突距離内にある
か否かを、ａａ式を用いて先端側の関節から順次チェッ
クする。その結果、もし、ｒ　ｎ（ｒ　Ｏ，（１６） ただし、 ｒ　＋ｎ≧ｒｏｎ　（ｍａｎ） で、第ｎ　ＩＪンクＬｎが障害物に衝突している場合に
は、第ｍ　ＩＪンクＬｍの位置を移動場せずに第１１リ
ンクＬ　ｎの衝突を回避させる。

そのために、まず、第ｎ＋１関節Ｒｎ＋１からみた第ｎ
　１７ンクＬ。の動作角を衝突回避角ψ。とじて、第４
図に示すように と定義する。

この場合、第１１＋１関節Ｒｎ＋１を基準とした第ｎ関
節Ｒ，の位置は、韻ｉ突回避角ψｎを用いるとと表され
る。従って（＋４１．　（１５＋、　（１８）式より、
（１９ま ただし、 Ｓ　ｎｘ　”’　Ｘ　ｎ＋１＋７ｎＣＯ５ψ、−ａＳｎ
ｙ　−ｙｎ＋１＋Ｌｎｓｌｎψ、−ｂが導刀・れるので
、第ｎ関節Ｒｎの障害物への衝突を回避するためには、 だけ、衝突回避角ψ。を動作させればよい。

すなわち、第ｎ関節Ｒｎの位置をｄＸ。

たけ、単位時間Δｔの間に移動させる。この時の第ｎ関
節Ｒｎの移動速度を衝突回避速度大λ辷＝　ｄＸ　ｎ　
／Δｔ　＠ とじて、これを実現する第１関節Ｒ１がら第ｎ−１関節
Ｒ１ｎ−１の動速度（衝突回避角速度）δ；を（力式を
用いて次のようにめる。

δ；＝ΣμｋＣｋＪ■辷　ａ ｋ＝１ また、第ｎ関節几。は第ｎリンクＬｎよす先端側）ＩＪ
ンク位置を変化させないよう、なる修正角度す。で動作
させる。

したがって、以上の衝突回避関節角速度ｂＳ１および修
正角速度机で、単位時間Δｔだけ各・関節を動作させた
際のモデル・ロボットの障害回避関節角度θｔ　＋、２
　Ｊ　ｔは、 ０　””を−〇　””＋（ｊｎ　十〇ｎ　ｊ　ｎ）Δｔ
５ただし、 となる。

よって、モデル・ロボットの各関節目標位置Ｘ七２Ｊｔ
は（３）式から、 ）（ｔ＋２Δｔ　＝ｆ　ｕｔ＋２Δｔ）＠と定まる。

関節数が本実施例よりも多い場合には、Ｑ４）、　（１
５）。

００式を用いて再度先端側の第ｆｉ　−ｌ　ＩＪンクＬ
ｎ＝１から障害物との衝突をチェックする。もし、衝突
が発生しているならば、上に説明したような衝突回避動
作を行い、衝突置所がなくなるまで同様の処理を行う。

最後に、このようにして得られたモデル・ロボットの最
終的関節角θ″ｍａｔと初期関節角θｔとの差分を単位
時間Δｔでの実目標角速度δｔ＋Δｔ０　ｔ＋Δｔ　’
ｔ＋ｍΔｔ θ　−（θ　−θ１）／Δｔ　＠ として、実際のロボットの動作を制御する。

したがって、実稼働時のロボットは、あらかじめモデル
・ロボットを動作させて衝突回避した後の動作位置を移
動目標として制御されるため、障害物に衝突することな
く指令軌跡に沿って移動させることか可能である。

なお、本実施例では、円形の障害物を回避する場合につ
いて説明したが、他の形状の障害物であっても、また、
障害物が複数個存在していても本制御方法が適用できる
ことは明らかである。

第５図は、サンプリング制御を用いた本発明の制御方法
を実現するための制御回路の一実施例を−示す図である
。

指令速度発生器４０１は指令軌跡Ｘｈと初期指令速度Ｖ
に基づいて住０）式からモデル・ロボットの各関節、お
よびリンクの先端の移動指令速度Ｘｎを算出する。座標
変換器４０２は、こ）指令速度３＊ Ｘアを（力式に基づいてモデル・ロボットの関節座０＊ 標系に変換し指令関節角速度θｈをめる。新関節角度計
算器４０３は、ロボットの各関節駆動軸に装着したエン
コーダ５０４から出力される現時点での各関節角度θｔ
から、前記指令関節角速度（目標関節角速度）θｈに基
づいて単位時間Δｔだけ動作した後の新関節角度θ　を
（１２１式を用いてめる。座標変換器４０４は、この新
関節角度θ　から（１）式に基づいて新開節位置Ｘｎ　
を算出する。距離計算器４０５は、この新開節位置Ｘｎ
　から（１４１式に基づいて各関節の円形障害や１００
の中心Ｐからの距離ｒｎをめる。衝突回避角度計算器４
０６は、障害物の中心からの距離ｒｎと、あらかじめ計
算しである、リンクが障害物に衝突しないための関節の
障害物中心からの最小距離ｒ’ｎ’ｅ比較し、もし、衝
突が発生しておれば、先端位置を変えずに衝突を回避す
る衝突回避角度ｄ９）、を（２１式を用いて算出する。

衝突回避速度計算器４０７は、この衝突回避角度ｄψ。

からＣ］）。

＠式に基づいて衝突回避速度大′ｎを算出する。座標変
換器４０８は、Ｑ式に基づいて前記衝突回避、速度大１
をモデル・ロボットの関節座標系に変換し、衝突回避角
速度１　、をめる。修正角速度計算器４０９は、前記衝
突回避角度ｄψ□と前記衝突回避角速度＋　′ｎから（
ハ）式に基づいて衝突しているリンクより先端側のリン
ク位置を変えずに衝突回避するための修正角速度θ。を
算出する。障害回避関節角度計算器４１０は、前記衝突
回避角度仁と前記修正角速度δｎおよび前記新関節角度
θｔ＋″から（至）式に基づいて障害物への衝突を回避
しながら先端を指令軌跡上へ位置決めするための障害回
避関節角度θ　を算出する。実目標関節角速度計算器４
１１は、この障害物回避関節角ｔ＋２Δを 度θ　と現在関節θｔから、実稼働時のロボットヲ障害
物を回避しながら指令軌跡に沿って移動させるための実
目標関節角速度θ　を１２η式に基づいて算出する。サ
ーボ機構５００は、ロボットの各関節を指令速度（実目
標関節角速度）θに従って駆動する速度制御型サーボ機
構である。

このサーボ機構５００は、ロボットの各関節Ｊ。

几２．Ｒ３をそれぞれ駆動するモータ５０２−１゜２．
３と、とのモータの回転速度をそれぞれ検出するタコジ
ェネレータ５０３−１．２．３、およびモータ５０２−
１．２．３をそれぞれ駆動するザーボ増幅器５０１−１
．２．３から構成されている。また、ロボットの各関節
几、　、　Ｒ２、ｇ３にはそれぞれエンコーダ５０４−
１．２．３が装着され、各関節の現在関節角度 θｔ＝（θ１ｔθ２ｔ　θ３ｔ）Ｔ が検出できるようになっている。

本サーボ機構５００によシ前記目標関節角速度、；ｔ＋
Ｊｔに従ってロボットは指令軌跡Ｘｈに沿って障害物を
回避しながら移動制御されることになる。

なお、本実施例においては、専用の演算器を用いて障害
物回避動作を制御する場合について述ベタカ、マイクロ
・コンピュータ等の計算機を用いて本発明の思想を実現
することも可能である。

〔発明の効果〕

以上説明したように、本発明のロボット制御方法によれ
ば、複雑な評価関数、拘束条件等を用いることなく、リ
アルタイムで冗長関節を有する多関節型ロボットを指定
された軌跡に沿って、周囲の障害物を回避しつつ目標位
置へ誘導するために要する多関節型ロボットの各関節に
対する操作量を自動的に創成することができる。

【図面の簡単な説明】

第１図は本発明の一実施例による多関節型ロボットの機
構構造の概略図、第２図は障害物の一例とロボットの指
令軌跡の一例を示す図、第３図は衝突を回避するための
ロボットの関節位置を説明するための図、第４図は衝突
を回避するだめの衝突回避角を説明するための図、第５
図は本発明の一実施例による制御回路のブロック図であ
る。 １００・・・障害物、２０］・・・指令軌跡、４０６・
・・衝突回避角速度器、４０７・・・衝突回避速度計算
器、４０９・・・修正角速度計算器、４１０・・・障害
回避関￥！ｌ　１　日 ％ｚ（Ｒｚン 第　３　図 １００　ＸＬ　（ＦＣｌ、） 第　４　閏

Claims (1)

【特許請求の範囲】
1. １、冗長関節を有する多関節型ロボットの先端を目標軌
   跡に沿って目標位置へ誘導する制御方法において、前記
   多関節型ロボットの作業空間を形成する構造物、および
   前記多関節型ロボットの幾何モデル（以下、これら幾何
   モデルをそれぞれモデル・ロボット、モデル構造物と呼
   ぶ）を用い、作業空間座標系で指示された前記多関節型
   ロボットの移動速度を前記モデル・ロボットの関節座標
   系に座標変換し、この値に従って該モデル・ロボットを
   単位時間動作させ、前記モデル構造物との衝突が発生す
   るか否かを該モデル・ロボットの先端リンク側から順次
   調べ、衝突が生じない場合には該モデル・ロボットの関
   節動作速度に従って現実の前記多関節型ロボットを動作
   させ、もし衝突が生じているならば、衝突リンクより先
   端側のリンクの位置を変えずに、該衝突リンクと前記モ
   デル構造物との衝突を回避芒せるための該衝突リンクの
   前記作業空間座標系における移動速度を、ＡＩＪ記モテ
   ル・ロボットの関節動作速度に座標変換し、この値に基
   づき該モデル・ロボットを単位時間動作させて衝突回避
   動作を行い、再び該モデル・ロボットと前記モデル構造
   物との衝突を先端側のリンクから順次調べ、この結果に
   応じて前記衝突回避動作を行い、衝突が前記モデル・ロ
   ボットの全てのリンクにおいて回避されるまでこれを繰
   返し、最後の衝突回避動作終了時における前記モデル・
   ロボットの関節位置の初期関節位置に対する増分値から
   ｉｆｆ記多関節型ロボットの実稼働時の目標関節動作速
   度をめ、仲れに従って現実の前記多関節型ロボットを動
   作さぜることにより、現実の前記作業空間における前記
   構造物への衝突を回避しながら前記目標軌跡に沿って、
   前記多関節型ロボットの先端を前記目標位置へ移動制御
   できるようにしたことを特徴とするロボットの’＋ＩＪ
   ｌ］御方法。