JPS626306A - 多関節ア−ムの制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は多関節アー°ムの制御方法に係り、特に移動体
に搭載され、障害物のある狭あい部で移動体の走行に同
期して上下または左右関節からなる多関節アームを制御
するのに好適な多関節アームの制御方法に関するもので
ある。

〔発明の背景〕

従来、移動体に搭載された多関節アームの制御方法につ
いては、米国原子力学会（ＡＮＳ）の１９８４悪環境下
でのロボットと遠隔操作に関するトピックス会議予稿集
（ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　ｔｈｅ　１９８４Ｎ
ａｔｉｏｎａｌ　Ｔｏｐｉｃａｌ　Ｍｅａｔｉｎｇ　ｏ
ｎ　ＲＯＢＯＴＩＣ３ＡＮＤＲＥＭＯＴＥ　ＨＡＮＤＬ
ＩＮＧ　ＩＮ　ＨＯ３ＴＩＬＥ　ＥＮＶＩＲＯＮＭＥＮ
ＴＳ）　ニおける浅野氏による「多関節ロボットの制御
システムＪ　　（Ｃｏｎｔｏｒｏｌ　Ｓｙｓｔｅｍｓ　
ｆｏｒ　ａ　Ｍｕｅｔｉ−ｊｏｉｎｔＲｏｂｏｔ　）　
　（ｐ　３７５〜ｐ　３８２）と題する文献において論
じられている。

本発明を適用しようとする多関節アームは、第２図に示
すように、少なくとも２つ以上の上下旋回節あるいは左
右旋回節から構成される。第２図に示すものは、左右旋
回節と上下旋回節が交互に配列された１１関節の多関節
アームである。各関節２ｉ　（ｉ＝１．２，３．・・・
、１２）の座標Ｔ。

＝（Ｘ□ｙｉＺｉ）”とすれば（Ｔは転置行列を示し。

Ｔ１□は固定節で、想像上の関節で、アームの先端Ｓに
対応する）、各アーム長３１は、その長さをＱ、として
ベクトルＡ、で表わすことができ、ここに、ＭＪは座標
変換マトリックスで、左右旋回節の場合は、その旋回量
をαＪとすれば、で、上下旋回節の場合は、その旋回量
をβ、とすれば、 で表現できる。また、１つの関節に左右旋回節と上下旋
回節の２自由度が存在すれば、このときのＭＪ　は、α
Ｊ、βｊを用いて次の（４）式で表現できる。

・・・・・・（４） これにより、各関節の位置ＪＪは、各関節のベクトルの
和であるから、次の（５）式となる。

したがって、（５）式より多関節アームでは目標点を与
えられていたとしても、その冗長自由度のために一意的
にその姿勢を決めることができない。

従来の方法では、この冗長自由度を拘束するために各関
節が通るべき経路を指定し、すべての関節が常に経路上
にくるように各関節の制御量をサンプリング（制御）周
期毎に決定していた。すなわち、制御周期Ｔ１毎に根本
側の関節（第１関節側）から、経路上にくるように関節
ｉの位置Ｊ。

を決め、それを満足するように（５）式により制御量α
、またはβ１を決定していた。しかし、この方法では、
次のような問題がある。

（１）すべての関節に対して、制御周期毎に（５）式を
用いて各関節の制御量を決定しているため、計算時間が
長く、目標点に到達するまでに時間がかかり、オンライ
ン制御には向かない。

（２）到達時間短縮のため、既知の経路に対して予め各
関節の制御量を計算し記憶させたとしても。

そのための多くのメモリ容量が必要となる。

（３）未知障害物を検出した時、経路を修正しなければ
ならないが、経路の再構成がむずかしく。

（１）により新たな制御量の決定のため多関節アームの
再制御のための時間がかかる。

〔発明の目的〕

本発明の目的は、上記した欠点をなくし、計算量やメモ
リ量が少なく、未知障害物検出時の経過の修正がしやす
く、かつ、最短時間で目的地に到達せしめることができ
る多関節アームの制御方法を提供することにある。

〔発明の概要〕

本発明の特徴は、障害物等により各関節が通過する指定
通過点を設け、隣接する２つの上記指定通過点で決まる
目標値を最小関節数および最小制御量で満足するように
２つの当該指定通過点間にある関節の制御量を定め、か
つ、制御量を決定するときは、上記関節が上記指定通過
点上にきたときとし、その間は補間により制御量を決定
するようにした点にある。

〔発明の実施例〕

以下本発明の方法の一実施例を第１図〜第１０図を用い
て詳細に説明する。なお、理解を容易にするために、ま
ず、２次元平面上で説明し、その後、３次元への拡張法
について述べる。

第１図は本発明の多関節アームの制御方法の一実施例を
説明するための基本原理を２次元平面上で示したもので
、第２図の関節構成のうち上下旋回節を除去したもので
ある。第１図において、移動体１は関節２□、２２．・
・・、２．と腕３ｔｔａｓ＊・・・。

３６からなる多関節アーム６を搭載し、ｙ軸で示す走行
ルート１０上を速度Ｖで移動する。本発明の制御方法は
、移動体１の走行に協調しながら多関節アーム６の先端
Ｓを障害物１１を回避しながら目標点Ｑ、（Ｑ、）まで
誘導する制御方法を提供するものである。そのために、
本発明では、各関節２□、２□、・・・、２６　が通る
べき指定通過点Ｑを指定する。指定通過点Ｑには５次の
３種類がある。

その第一は、走行ルート１０上にあり、制御開始点を示
す指定通過点Ｑ、　（Ｑ、）で、多関節アーム６の先端
Ｓがこの指定通過点Ｑ６にきたときから各関節２１，２
□、・・・、２．の制御を開始する。その第二は、目的
点を示す目標点Ｑ？で、最後の指定通過点Ｑｉ　　（ｉ
＝１１２．・・・）は、障害物１１により作成された各
関節２□、２２．・・・が通る点である。

第１図では、Ｑｌがこれに相当す。指定通過点Ｑ工＊Ｑ
ｔ＋を間の距離をり、とすれば、次の（６）式を満足す
るように指定通過点Ｑ、を定めなければならない。

ΣＬｔ　＜ΣＵｋ　　　　　・・・・・・（６）ここに
、Ｑｋ　二関節に、に＋１間のアーム長第３図は制御開
始点Ｑ６から目標点Ｑ？までの一連のアルゴリズムを具
体的に示したフローチャートである。

第１図Ｃａ＞に示すような状態、すなわち、多関節アー
ム６の先端Ｓが制御開始点である指定通過点Ｑ、　（Ｑ
、）にきたことを移動体１が持っている位置検出器（図
示せず）により検出する〔第３図（ａ）の処理２０〕。

次に、移動体１が速度Ｖで移動し、関節２．が制御開始
点Ｑ、　（Ｑ、）にきたとする〔第１図（ｂ）〕。この
ときの移動時間Ｔ、を次の（７）式で求める〔第３図（
ａ）の処理５０〕。

Ｑ＠ Ｔ、＝−・・・・・・（７） ■ そして、先端Ｓの目標値γ１は、指定通過点Ｑ、　（Ｑ
、）を原点とする座標系で、Ｑ□点の座標を（Ｘｚｅ　
ｙｌ）とすれば。

ッ、＝　ｔａｎ−・二　　・旧・・（８）ｉ から求めることができる〔第３ｍ　（ａ）の処理６０）
、もし、γ１が各関節の最大旋回角α１．１より小さけ
れば、１つの関節で指定通過点Ｑユに向うことができる
。この場合の先端関節２．の制御量は、α６＝７１とな
る〔第３図（ａ）の処理７０゜８０）、一方、γ１がα
１１．よりも大きい場合には。

第４図（ａ）に示すように、１つの関節では指定通過点
Ｑ□に向うことはできない、先端関節２゜の制御量がα
８＝α１１．とじて、１つの関節では指定通過点Ｑ工に
向うことができないことを示すため、方向フラグＦｉに
“１″を立てる〔第３図（ａ）の処理７０，９０、第３
図（ｂ）＋７）処理１００３゜ 第１図（ａ）と（ｂ）との間の制御量は、次のように制
御周期毎にいちいち計算せずに１次のように線形補間す
る。第１図（ａ）と（ｂ）とのＴ、間に、制御量α６を
動作させれば、すなわち、次の（９）式で示す角速度ω
６で制御すれば、第１図（ａ）からスタートして時間Ｔ
ａ後には、第１図（ｂ）の状態になる〔第３図（ｂ）の
処理１１０３゜ ω、＝ａ、／Ｔ、　　　　　・・・・・・（９）指定通
過点Ｑ、に次にくるのは、第１図（ｂ）かられかるよう
に、関節２．であるから、Ｑｌに次にくる関節Ｎ０　を
示す指標Ｍ、（Ｉ＝１）の内容を“１”減らし、さらに
次にアーム先端の次に向うべき通過指定点がＱｌからＱ
２となることを示す指標工（指定通過点の添字に対応）
を“１＃増加させる〔第３図（ｂ）の処理１２０）。

次に、第１図（ｂ）から（ｃ）へくる場合を考える。関
節２．が指定通過点Ｑ、（Ｑ、）にきたことを第３図（
Ｑ）の処理１３０で検出する。この時点で移動体１が速
度Ｖで移動し、関節２．が指定通過点Ｑ、　（Ｑ、）に
きたとする〔第１図（ｃ））。

このときの移動時間は、第３図（ａ）の処理５゜と（７
）式で同様に決まり、（９）式を一般化す゛れば、次の
（１０）式となる〔第３図（Ｑ）の処理１４０〕。

Ｔａ”　Ｑｗｘ／　Ｖ　　　　　　　・＝・・（１０）
ここに、Ｍ、：指定通過点Ｑｏ上にきた関節第３図（ａ
）の処理７０において、１つの関節で次の指定通過点Ｑ
１に向うことができれば、すなわち、方向フラグＦ、の
内容が“０”であるならば、第１図（ｃ）に示すように
、先端関節２、は、γ、旋回した状態から真直ぐに、次
の関節２、は、真直ぐの状態からγ１１回させればよい
。

したがって、第３図（Ｑ）の処理１６０に示すように、
関節２．、２．の制御量および角速度を決める。一般的
には、関節２□が指定通過点Ｑ□にきたときは、関節２
□の目標値を７１、関節２　Ｍ！＄１の目標値をＯにす
ればよい、しかし、方向フラグＦ１の内容が１”である
場合は、第４図（ｂ）に示すように、関節２．の目標値
をα１６．、関節２、の目標値をα１から原点を関節２
１に持つ直交座標系〔（１）式により変換〕によって決
まるαｉｉ、Ｉどなる〔第３図（Ｑ）の処理１７０，１
８０〕。この場合、さらに進んで第４図（Ｑ）のように
関節２．が指定通過点Ｑ、　（Ｑ、）にきたとすると、
先端の関節２６　を真直ぐにし、指定通過点Ｑ、（Ｑ、
）上の関節２４を最大旋回角制御し、真中の関節２．を
最大旋回角からその関節２．を原点とする座標系から求
まる目標値へと３つの関節を制御する。この３つの関節
の制御量、角速度の処理を第３図の１９０，２００で示
す。第３図の場合、指定通過点へ向うための最大関節数
を２としたが、最大関節数が３つの場合は、処理１８０
で求めたγ１０、に対して処理７０〔第３図（ａ）〕の
判断を行い、以下、処理７０〜１００に相当する処理を
行う。この場合、指定通過点から２つの関節の旋回量が
α１１．となる、以下、最大関節数が４，５．・・・と
増えて行くときも同じである。しかし、仮りにαｓａｍ
　＝４５″とすると、第５図に示すように、最大関節数
が２つあれば十分である。

３つ以上だと、制御開始点を変更するか〔第５図（ａ）
において、ＱｌからＱ１′　　に変更）、または、指定
通過点を新たに設ける〔第５図（ｂ）のＱｏ、）ことで
対処できる。

次に、何故第４図に示すように指定通過点にある関節の
うち根本から順に制御するかについて説明する。２つの
指定通過点Ｑ、、Ｑ、間の関節の制御方法には種々ある
が、第６図に代表的なものを示す、各制御方法に対して
、制御量および制御関節数を比べてみると、第６図（Ｑ
）に示すように根本側の関節をできるだけ動作させた方
が、最小制御量および最小関節数で目標方向である指定
通過点Ｑ２に多関節アームを制御できる。

以上説明した方法で移動体１が走行ルート１０に沿って
移動するにしたがい、指定通過点Ｑ、（Ｑ、）　　上に
関節２４＃Ｌ？・・・・・・とくる毎に指定通過点Ｑ。

、Ｑ８間のある関節の制御量および角速度を求めること
ができる。

第７図のように、先端ＳがＱ８にきたときは、第７図（
ａ）、（ｂ）に示す２つのケースにしたがい、Ｑｌが座
標原点になるように座標変換し、Ｑ□をＱ、、Ｑ、をＱ
ｌに考えて同様に指定通過点Ｑ、、　Ｑ２間に存在する
各関節の目標値および角速度を求めることができる。

（ｉ）ケース（ａ）の場合の直交座標変換マトリックス
ＭＱ （五）ケース（ｂ）の場合の直交座標変換マトリックス
Ｍ＃ ・・・・・・（１２） このように、（１）式に対応する変換マトリックスの計
算をするのは、多関節アーム６の先端Ｓが指定通過点に
きたときと、処理７０に示すように１つの関節で制御で
きないときの２回である。

しかも、前者の変換マトリックスの（１１）式または（
１２）式は、（１）式に比べて次数が小さい。

また、後者については、各腕長が等しければ、処理１７
０の座標変換処理が各関節に対して同じであるから、処
理１７０の処理を第４図（ｂ）に示すように先端関節か
ら１つ手前の関節が指定通過点にきたときのみに一度計
算すればよく、マトリツクス計算の大幅な低減をはかる
ことができる。

上記に示した本発明の制御方法によって制御した場合の
制御終了時の最終形態を第８図に示す。

次に、指定通過点の指定方法について説明する。

この方法には色々考えられるが、第９図にその代表的な
ものを示す、中間９指定通過点Ｑ、　　（ｉ　＝１．２
．・・・）に対して、第９図（ａ）は障害物１１の中間
点とする方法、第９図（ｂ）はΣＬ。

を最小にするように定める方法、第９図（ｃ）はΣγ１
　を最小にするように定める方法である。制御開始点で
ある指定通過点に対しては、第９図（ａ）〜（ｃ）のい
ずれの方法においても指定通過点Ｑ、、　Ｑ、間の制御
関節数を最小にするように定める。なお、前述した本発
明の説明では、第９図（ａ）の方法を用いた。

次に、未知障害物を検出したときの指定通過点Ｑの作成
方法については、多関節アーム６の障害物検出センサを
腕単位に装着しておき、その検出センサによって障害物
を検出した腕を判定し、そのときの各関節の制御量α、
、β１により、（５）式を用いて障害物の位置を検出で
きる。この場合、腕単位で検出しているので、腕長、腕
径などによる検出誤差をγとすれば、検出点を中心に半
径ｒの円（３次元の場合には球）で障害物を表現する。

新たに検出した未知障害物と既知障害物により前述した
方法のいずれかにより指定通過点Ｑ工を作成する。

最後に、いままで２次元平面上において説明したが、３
次元への適用方法について第１０図を用いて説明する。

指定通過点Ｑに対して左右平面上のなす角ｒと上下平面
上のなす角φに分解し、左右旋回節に対してはでを、上
下旋回節に対してはφを目標値（いままでの記述におけ
るγに対応）としてそれぞれ計算すればよい。

〔発明の効果〕

以上説明したように１本発明によれば、簡単なアルゴリ
ズムで、少ない計算機やメモリ容量で未知障害物検出時
の経過修正がしやすく、がっ、最短時間で目的地まで到
達可能とすることができるという効果がある。

【図面の簡単な説明】

第１図は本発明の多関節アームの制御方法の一実施例を
説明するための基本原理を２次元平面上で示した図、第
２図は本発明を適用しようとする多関節アームの関節構
成の一例を示す図、第３図は本発明の一連のアルゴリズ
ムの一例を示すフローチャート、第４図は指定通過点間
の制御量の決定方法の説明図、第５図は指定通過点間で
制御する関節数を低減可能であることを示す図、第６図
は本発明の基本的考え方である根本側関節から順次制御
する理由を説明するための図、第７図は指定通過点にお
ける座標変換を示す図、第８図は第１図の状態で多関節
アームを制御したときの最終形態を示した図、第９図は
指定通過点の指定方法を示す図、第１０図は本発明の３
次元空間への拡張方法を説明するための図である。 １・・・移動体、２□、２□、・・・ｔ　２ｓｔ　４ｔ
ｔ・・・、４゜・・・関節、３１，３□、・・・ｔ　Ｌ
ｔ　５ｔｔ・・・、５．・・・腕、６・・・多関節アー
ム、１０・・・移動体の走行ルート、１１・・・障害物
、Ｑ、　（Ｑ、）・・・制御開始点を示す指定通過点、
Ｑ（（ｉ＝１１２．・・・）・・・指定通過点、Ｑ？・
・・目標点を示す指定通過点、Ｓ・・・多関節アームの
先端。

Claims (1)

1. 【特許請求の範囲】 １、移動体に搭載され、少なくとも左右旋回節あるいは
   上下旋回節を複数個持つ多関節アームにおいて、障害物
   等により各関節が通過する指定通過点を設け、隣接する
   ２つの前記指定通過点で決まる目標値を最小関節数およ
   び最小制御量で満足するように２つの当該指定通過点間
   にある関節の制御量を定め、かつ、制御量を決定すると
   きは、前記関節が前記指定通過点上にきたときとし、そ
   の間は補間により制御量を決定するようにすることを特
   徴とする多関節アームの制御方法。 ２、前記隣接する２つの指定通過点間の関節の制御量を
   決定する方法は、前記指定通過点間にある関節のうち根
   本側の関節から順次制御することにより最小関節数およ
   び最小制御量を満足するようにするものである特許請求
   の範囲第１項記載の多関節アームの制御方法。