JP3431026B2 - 冗長軸を有する７軸以上のマニピュレータの制御方法 - Google Patents
冗長軸を有する７軸以上のマニピュレータの制御方法Info
- Publication number
- JP3431026B2 JP3431026B2 JP11077392A JP11077392A JP3431026B2 JP 3431026 B2 JP3431026 B2 JP 3431026B2 JP 11077392 A JP11077392 A JP 11077392A JP 11077392 A JP11077392 A JP 11077392A JP 3431026 B2 JP3431026 B2 JP 3431026B2
- Authority
- JP
- Japan
- Prior art keywords
- manipulator
- tool
- posture
- axes
- control point
- 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 - Fee Related
Links
Description
【発明の詳細な説明】
【０００１】
【産業上の利用分野】本発明は宇宙、深海、原子力発電
所等の特殊環境、あるいは一般産業において利用される
７軸以上の多関節マニピュレータに関するものである。 【０００２】 【従来の技術】一般にロボットマニピュレータの動作に
おいて、マニピュレータの動作範囲内に障害物があれ
ば、これを回避するようなアームの動きを必要とする。
マニピュレータの肩部分の位置とマニピュレータが実際
に作業するマニピュレータ先端のツール部位置との関係
で、必ずしも障害物を回避することがアームの機構上の
兼ね合いもあって可能であるということではない。この
ことはマニピュレータが機構上多くの冗長となる軸を有
すれば可能となるということがある。しかし通常考えら
れてているマニピュレータは人間の腕に似た６軸ないし
７軸の多関節マニピュレータである。人間の腕であれば
障害物を回避するために肘部分を内側あるいは外側に回
転させて目的を達成させることが可能である場合があ
る。マスタ・スレイブ型マニピュレータではマスタとス
レイブのマニピュレータが機械的に同構造であれば、マ
スタ側で肘を回転させるという動作指令を発生させる機
構が存在するので実現が可能であるが、マスタとスレイ
ブのマニピュレータが機械的に異構造の一般のマスタ構
造の場合（図３参照）は、そんなに簡単ではない。７軸
の冗長軸ロボットであっても、目的の肘関節を回転させ
るという動作は、容易なことではない。とくにマニピュ
レータのツール部の位置と姿勢をもとのままの状態で、
肘部分のみを回転させるということは特に実現の困難な
動作である。従来の６軸マニピュレータの障害物回避の
方法は、肩部分の位置を再度変更するという方法をとる
のが一般的である。また、人間腕形マニピュレータの制
御方式について、特開昭５２−１１１１５９に開示され
ている。この人間腕形マニピュレータの機構は、支持体
に取り付けられた肩部、その肩部から伸びる上腕、上腕
の先端の肘部、その肘部から伸びる前腕、前腕の先端の
手首部、及びその手首部から伸びる手先によって構成さ
れている。 【０００３】 【発明が解決しようとしている課題】しかしながら、肩
部分の位置を再度変更するという方法は動作が滑らかに
連続しなくなる欠点がある。また、従来技術の特開昭５
２−１１１１５９による制御方式では、肩部に直交する
３軸の回転機構を持つマニピュレータの制御方式であ
り、このような構造のマニピュレータでは、動作範囲が
狭くなるという欠点がある。また、従来技術に開示され
ている「マニピュレータの肩の点とツールの制御点を結
ぶ軸に回転する」演算方式では、機構的にマニピュレー
タの肘が動かないという場合がある。本発明は、障害物
回避の問題において、肩部分の位置を変更することな
く、マニピュレータのツール部分の位置や姿勢を変化し
ないで肘部分を内側あるいは外側に回転させて、障害物
との接触を回避する方法を提供することを目的とするも
のである。 【０００４】 【課題を解決するための手段】本発明の冗長軸を有する
７軸以上のマニピュレータの制御方法は、７軸以上の多
関節マニピュレータにおいて、現在のツール部の位置と
姿勢を第１ツール制御点とし、マニピュレータの肩の点
と前記第１ツール制御点以外の点を結ぶ軸に前記第１ツ
ール制御点を回転させる第１の演算と、前記第１の演算
により変換されたツール部の位置と姿勢を第２ツール制
御点とし、前記ツール部に近い方から選んだマニピュレ
ータの３軸目または４軸目までを動かして前記第２ツー
ル制御点の姿勢を前記第１ツール制御点の姿勢に合わせ
る第２の演算と、前記第２の演算により変換されたツー
ル部の位置と姿勢を第３ツール制御点とし、マニピュレ
ータ全軸を使って前記第３ツール制御点の位置を前記第
１ツール制御点の位置に合わせる演算と、をヤコビアン
を用いて順に実行することを特徴とするものである。 【０００５】 【作用】マニピュレータの型としては７軸の人間の腕に
似た多関節型マニピュレータを想定する。今右腕を想定
し肘を外に張った状態を考える（図４参照）。この肘の
前方に柱のような障害物が存在し手には水の入ったコッ
プをもっているとする。この状態でまず肘を下ろし柱と
衝突しないようにして、手を前のほうにもっていけば、
この柱との衝突は避けることが出来る。このことは次の
ようにして考えれば機械マニピュレータでも実現でき
る。まず最初の腕の状態で肩の点と手の平の点より少し
左腕側によった点を結ぶ線を考える。この線を回転軸と
して手の状態を右に回転させる。すると自然に肘部分が
右に回転する。そこで手より近い３関節を使って（肩に
近い４関節は変化せずに）回転以前の姿勢に合わせるよ
うに左に回転する。この状態は最初の手の姿勢と同じで
あるが 手の平の位置が異なる。この位置を最初の位置
に合わせるために、腕７軸を全て使って移動するように
する。結果は最初の手の位置と姿勢は同じで、肘の位置
が前と比べて下がった状態になっている。このことは当
初の目的を達成した状態である。このことをマニピュレ
ータの式を使って論理的に説明する。 【０００６】マニピュレータは一般的にｎ軸のものを想
定するが、具体的には７軸のものを想像すると理解しや
すい。最初のマニピュレータの関節角度が次のようであ
ったとする。 ｛ｑiS｝ この軸角度がつくるマニピュレータのツールの制御点の
姿勢と位置は ｛Ω、ｒ｝ であるとする。そして最終的に得られる関節角度を次の
ようであったとする。 ｛ｑiE｝ マニピュレータの各軸角度とツール部分の制御点での位
置と姿勢の関係は微小変化分で記述すると次のような関
係で示される。 ［ΔｒT 、ΔωT ］T ＝Ｊ＊Δｑ （１） ｎ ：ロボットの軸数 Δ ：微小変化であることを意味する ｒ ：作業空間の位置ベクトル （次元
３） ω ：作業空間の姿勢回転ベクトル （次元
３） Ｊ ：ヤコビアンマトリクス （次元 ６
ｘｎ） ｑ ：マニピュレータジョイント空間での軸の回転ベク
トル（次元 ｎ） ［ ］T ：行列［ ］の転置を意味する。 微小姿勢変化を意味するΔωの意味は次元が３であるの
でそれが示すベクトルを回転軸としてΔωの大きさが微
小回転角度であることを意味している。この関係式はマ
ニピュレータの回転軸がΔｑだけ変化する場合マニピュ
レータのツール部の制御点で位置がΔｒ、姿勢がΔωだ
け変化するということを意味する。逆に作業空間で位置
の変化がΔｒ、姿勢の変化がΔωで与えられれば、それ
を実現するマニピュレータの軸の微小変化Δｑがこの関
係式を解くことによって求めらえることを意味する。こ
の関係式を使って、ツールの位置と姿勢を変えないで、
マニピュレータの肘部分を回転させるには次のようにす
ればよい。これは論理的には次の論理演算Ａ〜Ｃの３段
階より成り立っている。処理フローを図２に示し、以下
具体的に説明する。 【０００７】（論理演算Ａ）話をわかり易くするため
に、右腕のマニピュレータとする。この右腕のマニピュ
レータの肘の部分に障害物があるので肘を内側ないしは
外側に回転させてこの障害物を回避することを考える。
図５において、点Ｓがマニピュレータの肩部分の肩の
点、Ｐがツールの制御点、点Ｑが点Ｐ近傍の左腕側に定
義される点である。つまり左右両肩の点と右腕のツール
の制御点を通る三角形の張る面上にこのＱ点を選択す
る。そこでＳとＱを結ぶ線をｌとしこの軸方向を意味す
る単位ベクトルをｋとする。点Ｐより線ｌに下ろした垂
線の足の点がＨである。ツールの制御点Ｐが線ｌの回り
に描く小さい円を想定している。マニピュレータのツー
ルの制御点の位置と姿勢をこの回転軸ベクトルｋの回り
にΔθだけ回転することを考える。現在のツールの位置
ベクトルをｒ、姿勢を［ｎ，ｏ，ａ］とする（図６参
照、図６はツールが回転軸ベクトルｋの回りに微小角度
Δθだけ位置と姿勢を回転する場合を描いている）。
ｎ，ｏ，ａは姿勢を表す直交３軸を表す単位ベクトルで
ある。ベクトルを回転軸ｋ回りにΔθだけ回転する回転
マトリクスＲは次のように与えられる。 Ｒ＝Ｅ3*cos(Δθ)+ｋ＊ｋT ( １- cos(Δθ))＋[ ｋｘ]*sin(Δθ) （２） Ｅ3 ：３ｘ３の単位マトリクス ［ｋｘ］：ベクトルｋのスキュウマトリクス 回転マトリクスＲが求まったので、マニピュレータのツ
ールの制御点を回転させることを考える。制御点の姿勢
のみを回転軸ｋの回りに回転させると、手首のみが回転
して肘部分が回転しない場合があるので、制御点の位置
ｒと姿勢［ｎ，ｏ，ａ］を回転軸ｋの回りに回転させる
ようにする。今問題にしていることは、ベクトルｋとΔ
θに対応する（１）式の左辺の量［ΔｒT 、ΔωT ］T
の量を如何にして求めるかという問題である。 【０００８】位置の回転 （２）式は回転軸ベクトルｋが原点を起点にしている場
合のマトリクスであるので、制御点Ｐを回転した位置を
求めるには、補正をしてやらねばならない。そのために
点Ｐより線ｌに垂線を下しその点をＨとする。マニピュ
レータのヤコビアンを求める際に利用した、座標系の原
点Ｓをこの点Ｈに平行移動して考える。ベクトルＳＨを
ｒ1 ベクトルＨＰをｒ2 とすると制御点の位置ｒは下記
のようになる。 回転前の位置ｒ＝ｒ1 ＋ｒ2 （３） そこでｒ2 がベクトルｋの回りに回転する場合を計算し
てその結果をｒ1 に加えてやればもとめる回転位置にな
る。 回転後の位置＝ｒ1 ＋Ｒ* ｒ2 （４） 従って微小変位Δｒは下記のように求められる。 Δｒ＝（Ｒ−Ｅ3 ） * ｒ2 （５） 【０００９】姿勢の回転 姿勢の回転は回転軸ベクトルがｋで回転角度がΔθであ
るので簡単に下記のようにもとまる。 Δω＝ｋ* Δθ （６） （５）と（６）の値を（１）式にいれて左辺の値がもと
まる。ヤコビアンＪは回転前のマニピュレータ の姿勢
で求められるので既知の要素からなるマトリクスであ
る。（注意：宇宙ロボットの場合はマニピュレータ登載
の衛星がマニピュレータの動きで影響を受けるのでヤコ
ビアンマトリクスは簡単にもとまらないことがある。想
定して場合は質量に大きな差がある場合である。）
（１）式を解くことによって、マニピュレータの軸の微
小角度ベクトルΔｑが求められる。 Δｑ＝Ｊ# * ［ΔｒT 、ΔωT ］T ＋（Ｅn - Ｊ# Ｊ）* ｙ （７） Ｊ# はＪの疑似逆行列である。Ｅn はｎｘｎの単位マト
リクスである。ｙは任意のベクトルである。（７）式の
右辺の第二項はゼロベクトルとおいてよいので書き直す
と下記のようになる。 Δｑ＝Ｊ# * ［ΔｒT 、ΔωT ］T （８） もう一度整理して説明する。 ａ）右椀のマニピュレータについて考える。 ｂ）マニピュレータのツール制御点Ｐを左腕側に少しず
らした点Ｑを考える。 ｃ）マニピュレータの肩の位置Ｓと点Ｑを結ぶ線を想定
し、これを回転軸ベクトルｋとする。 ｄ）マニピュレータのツールの位置ｒ、姿勢を［ｎ，
ｏ，ａ］としこれを前記ベクトルｋの回りにΔθだけ回
転させる。 ｅ）上の計算を式（５）（６）によって式（１）の左辺
の値を求める。 ｆ）式（１）を解くことによって微少変化Δθに対応す
る、マニピュレータの軸の微少角度ベクトルΔｑを
（８）式のように求める。 ｇ）回転前の軸角度ベクトルｑにΔｑを加えることによ
ってマニピュレータの肘を回転した姿勢を求めることが
できる。 ｈ）ｄ）からｇ）を複数回実行してマニピュレータの肘
を回転させる。この結果のマニピュレータのツールの制
御点の姿勢と位置は ｛ΩA 、ｒA ｝ である。 【００１０】（論理演算Ｂ）式（１）におけるヤコビア
ンＪは、展開して書くと次のようになる。 【００１１】 【数１】 【００１２】ここで 各要素は以下の次元のマトリクス Ｊ11：3 x (n-4) Ｊ12：3 x 4 Ｊ21：3 x (n-4) Ｊ22：3 x 4 最初の軸角度（ｑ1 s 、ｑ2 s ，ｑ3 s ，…, ｑns）が
形成するマニピュレータのツール部の姿勢はΩであっ
た。今の状態の姿勢はΩA である。従って姿勢ΩA をΩ
に合わせることを考える。回転マトリクスＲを考えると Ω＝Ｒ* ΩA （１０） これより Ｒ＝Ω* ΩA ー 1 ＝Ω* ΩA T （１１） この式から、回転マトリクスＲを形成する回転軸ベクト
ルｋB と回転角度θBが計算できる。 Δω＝ｋB * θB ／（分割数ｍB ）とすると、次のよう
な関係がなりたつ。 【００１３】 【数２】 【００１４】ヤコビアンＪは毎回計算するとして、この
関係式は位置を無視すると次の関係式と同じことであ
る。 Δω＝Ｊ22* （Δｑn - 3 ，Δｑn-2 ，Δｑn-1 ，Δｑn ）T （１３） （Δｑn - 3 ，Δｑn-2 ，Δｑn-1 ，Δｑn ）T ＝Ｊ22 # * Δω＋（Ｅ4 −Ｊ22 # * Ｊ22）ｙ1 ＝Ｊ22 # * Δω （１４） 求められた微小角度をもとの軸角度に加える。再度ヤコ
ビアンをＪ22をもとめ上の計算を繰り返すことによっ
て、姿勢がもとのものと一致した、軸角度 ｛ｑiB｝ を求めることができる。 【００１５】（論理演算Ｃ）今のツールの状態は姿勢は
Ωで位置はｒA ' である。姿勢を調整したので位置がｒ
A からｒA ' に代わった。従ってツールの位置をｒA '
よりｒに姿勢を動かさず移せばよい。位置ｒA ' から元
の位置ｒへの方向への微小位置変化分Δｒは Δｒ＝（ｒ−ｒA ' ）／（分割数ｍC ）であるので ［ΔｒT ，０T ］T ＝Ｊ* ［Δｑ1 ，Δｑ2 ，Δｑ3 ，…，Δｑn ］T （１５） これを解くと ［Δｑ1 ，Δｑ2 ，Δｑ3 ，…，Δｑn ］T ＝Ｊ# * ［ΔｒT 、０T ］T ＋（Ｅn −Ｊ# * Ｊ）ｙ ＝Ｊ# * ［ΔｒT 、０T ］T （１６） 得られた微小角度を軸角度に加える。再度ヤコビアンＪ
を計算する。このことをｍC 回繰り返すことによって、
位置と姿勢を最初の ｛ Ω、ｒ｝にもってくることが
できる。ここで求められた軸角度は｛ｑiE｝である。最
初の軸角度｛ｑiS｝と最終的にえられる軸角度｛ｑiE｝
の間を適当な分割数ｍで割って微小角度｛ΔｑRi｝を求
める。この微小軸角度が求まると、 ｑi ＝ｑi ＋ΔｑRi （１７） として分割数（ｍ）だけくり返せば目的のマニピュレー
タの肘回転動作は実現される。注意することは疑似逆行
列Ｊ# が求まらないことがある。この場合はマニピュレ
ータの位置を少しずらしてやることによって可能にな
る。基本的なこのような問題はロボットシミュレータで
もって、扱っているロボットマニピュレータがどの様な
障害物回避動作が可能であるかを事前に検討しておい
て、マニピュレータの構造を決定しなければならないと
いうことがある。 【００１６】 【実施例】図１に、従来の制御装置に本発明の方法を組
み込む場合の実施例を示す。点線で囲んだ部分が本発明
に関係する部分である。マスタ・スレイブ型マニピュレ
ータのマスタ側の操作卓１に肘回転指令ボタン２ａ、２
ｂと、肘最大回転角度θmax を与えるダイヤル、ないし
デジタルスイッチ３を設ける。この角度は肘を最大どの
程度回転させるのかを決定するために使用する。肘回転
指令ボタンに「右回転」「左回転」の名称をあたえる。
「右回転」ボタン２ａを押せばマニピュレータは肘部分
が右回転し、「左回転」ボタン２ｂを押せば肘部分が左
回転するような微少回転角度ΔｑRiをΔｑ計算装置４が
発生し従来の位置制御器５に与える。具体的計算手法は
すでに説明したとおりである。なお、説明は一台のマニ
ピュレータについて述べたが、複数台のマニピュレータ
があれば、その数に見合った肘回転指令ボタンを準備す
ることが必要である。 【００１７】 【発明の効果】以上述べたように、本発明によれば、７
軸以上の冗長軸マニピュレータの障害物回避のマニピュ
レータ肘回転動作が可能になる。また、本発明に記載さ
れているように、マニピュレータの肩の点とツールの制
御点近傍に撰んだ点を結ぶ軸に回転することで、確実に
肘を動かすことが可能となるという格段の効果を奏する
ものである。
所等の特殊環境、あるいは一般産業において利用される
７軸以上の多関節マニピュレータに関するものである。 【０００２】 【従来の技術】一般にロボットマニピュレータの動作に
おいて、マニピュレータの動作範囲内に障害物があれ
ば、これを回避するようなアームの動きを必要とする。
マニピュレータの肩部分の位置とマニピュレータが実際
に作業するマニピュレータ先端のツール部位置との関係
で、必ずしも障害物を回避することがアームの機構上の
兼ね合いもあって可能であるということではない。この
ことはマニピュレータが機構上多くの冗長となる軸を有
すれば可能となるということがある。しかし通常考えら
れてているマニピュレータは人間の腕に似た６軸ないし
７軸の多関節マニピュレータである。人間の腕であれば
障害物を回避するために肘部分を内側あるいは外側に回
転させて目的を達成させることが可能である場合があ
る。マスタ・スレイブ型マニピュレータではマスタとス
レイブのマニピュレータが機械的に同構造であれば、マ
スタ側で肘を回転させるという動作指令を発生させる機
構が存在するので実現が可能であるが、マスタとスレイ
ブのマニピュレータが機械的に異構造の一般のマスタ構
造の場合（図３参照）は、そんなに簡単ではない。７軸
の冗長軸ロボットであっても、目的の肘関節を回転させ
るという動作は、容易なことではない。とくにマニピュ
レータのツール部の位置と姿勢をもとのままの状態で、
肘部分のみを回転させるということは特に実現の困難な
動作である。従来の６軸マニピュレータの障害物回避の
方法は、肩部分の位置を再度変更するという方法をとる
のが一般的である。また、人間腕形マニピュレータの制
御方式について、特開昭５２−１１１１５９に開示され
ている。この人間腕形マニピュレータの機構は、支持体
に取り付けられた肩部、その肩部から伸びる上腕、上腕
の先端の肘部、その肘部から伸びる前腕、前腕の先端の
手首部、及びその手首部から伸びる手先によって構成さ
れている。 【０００３】 【発明が解決しようとしている課題】しかしながら、肩
部分の位置を再度変更するという方法は動作が滑らかに
連続しなくなる欠点がある。また、従来技術の特開昭５
２−１１１１５９による制御方式では、肩部に直交する
３軸の回転機構を持つマニピュレータの制御方式であ
り、このような構造のマニピュレータでは、動作範囲が
狭くなるという欠点がある。また、従来技術に開示され
ている「マニピュレータの肩の点とツールの制御点を結
ぶ軸に回転する」演算方式では、機構的にマニピュレー
タの肘が動かないという場合がある。本発明は、障害物
回避の問題において、肩部分の位置を変更することな
く、マニピュレータのツール部分の位置や姿勢を変化し
ないで肘部分を内側あるいは外側に回転させて、障害物
との接触を回避する方法を提供することを目的とするも
のである。 【０００４】 【課題を解決するための手段】本発明の冗長軸を有する
７軸以上のマニピュレータの制御方法は、７軸以上の多
関節マニピュレータにおいて、現在のツール部の位置と
姿勢を第１ツール制御点とし、マニピュレータの肩の点
と前記第１ツール制御点以外の点を結ぶ軸に前記第１ツ
ール制御点を回転させる第１の演算と、前記第１の演算
により変換されたツール部の位置と姿勢を第２ツール制
御点とし、前記ツール部に近い方から選んだマニピュレ
ータの３軸目または４軸目までを動かして前記第２ツー
ル制御点の姿勢を前記第１ツール制御点の姿勢に合わせ
る第２の演算と、前記第２の演算により変換されたツー
ル部の位置と姿勢を第３ツール制御点とし、マニピュレ
ータ全軸を使って前記第３ツール制御点の位置を前記第
１ツール制御点の位置に合わせる演算と、をヤコビアン
を用いて順に実行することを特徴とするものである。 【０００５】 【作用】マニピュレータの型としては７軸の人間の腕に
似た多関節型マニピュレータを想定する。今右腕を想定
し肘を外に張った状態を考える（図４参照）。この肘の
前方に柱のような障害物が存在し手には水の入ったコッ
プをもっているとする。この状態でまず肘を下ろし柱と
衝突しないようにして、手を前のほうにもっていけば、
この柱との衝突は避けることが出来る。このことは次の
ようにして考えれば機械マニピュレータでも実現でき
る。まず最初の腕の状態で肩の点と手の平の点より少し
左腕側によった点を結ぶ線を考える。この線を回転軸と
して手の状態を右に回転させる。すると自然に肘部分が
右に回転する。そこで手より近い３関節を使って（肩に
近い４関節は変化せずに）回転以前の姿勢に合わせるよ
うに左に回転する。この状態は最初の手の姿勢と同じで
あるが 手の平の位置が異なる。この位置を最初の位置
に合わせるために、腕７軸を全て使って移動するように
する。結果は最初の手の位置と姿勢は同じで、肘の位置
が前と比べて下がった状態になっている。このことは当
初の目的を達成した状態である。このことをマニピュレ
ータの式を使って論理的に説明する。 【０００６】マニピュレータは一般的にｎ軸のものを想
定するが、具体的には７軸のものを想像すると理解しや
すい。最初のマニピュレータの関節角度が次のようであ
ったとする。 ｛ｑiS｝ この軸角度がつくるマニピュレータのツールの制御点の
姿勢と位置は ｛Ω、ｒ｝ であるとする。そして最終的に得られる関節角度を次の
ようであったとする。 ｛ｑiE｝ マニピュレータの各軸角度とツール部分の制御点での位
置と姿勢の関係は微小変化分で記述すると次のような関
係で示される。 ［ΔｒT 、ΔωT ］T ＝Ｊ＊Δｑ （１） ｎ ：ロボットの軸数 Δ ：微小変化であることを意味する ｒ ：作業空間の位置ベクトル （次元
３） ω ：作業空間の姿勢回転ベクトル （次元
３） Ｊ ：ヤコビアンマトリクス （次元 ６
ｘｎ） ｑ ：マニピュレータジョイント空間での軸の回転ベク
トル（次元 ｎ） ［ ］T ：行列［ ］の転置を意味する。 微小姿勢変化を意味するΔωの意味は次元が３であるの
でそれが示すベクトルを回転軸としてΔωの大きさが微
小回転角度であることを意味している。この関係式はマ
ニピュレータの回転軸がΔｑだけ変化する場合マニピュ
レータのツール部の制御点で位置がΔｒ、姿勢がΔωだ
け変化するということを意味する。逆に作業空間で位置
の変化がΔｒ、姿勢の変化がΔωで与えられれば、それ
を実現するマニピュレータの軸の微小変化Δｑがこの関
係式を解くことによって求めらえることを意味する。こ
の関係式を使って、ツールの位置と姿勢を変えないで、
マニピュレータの肘部分を回転させるには次のようにす
ればよい。これは論理的には次の論理演算Ａ〜Ｃの３段
階より成り立っている。処理フローを図２に示し、以下
具体的に説明する。 【０００７】（論理演算Ａ）話をわかり易くするため
に、右腕のマニピュレータとする。この右腕のマニピュ
レータの肘の部分に障害物があるので肘を内側ないしは
外側に回転させてこの障害物を回避することを考える。
図５において、点Ｓがマニピュレータの肩部分の肩の
点、Ｐがツールの制御点、点Ｑが点Ｐ近傍の左腕側に定
義される点である。つまり左右両肩の点と右腕のツール
の制御点を通る三角形の張る面上にこのＱ点を選択す
る。そこでＳとＱを結ぶ線をｌとしこの軸方向を意味す
る単位ベクトルをｋとする。点Ｐより線ｌに下ろした垂
線の足の点がＨである。ツールの制御点Ｐが線ｌの回り
に描く小さい円を想定している。マニピュレータのツー
ルの制御点の位置と姿勢をこの回転軸ベクトルｋの回り
にΔθだけ回転することを考える。現在のツールの位置
ベクトルをｒ、姿勢を［ｎ，ｏ，ａ］とする（図６参
照、図６はツールが回転軸ベクトルｋの回りに微小角度
Δθだけ位置と姿勢を回転する場合を描いている）。
ｎ，ｏ，ａは姿勢を表す直交３軸を表す単位ベクトルで
ある。ベクトルを回転軸ｋ回りにΔθだけ回転する回転
マトリクスＲは次のように与えられる。 Ｒ＝Ｅ3*cos(Δθ)+ｋ＊ｋT ( １- cos(Δθ))＋[ ｋｘ]*sin(Δθ) （２） Ｅ3 ：３ｘ３の単位マトリクス ［ｋｘ］：ベクトルｋのスキュウマトリクス 回転マトリクスＲが求まったので、マニピュレータのツ
ールの制御点を回転させることを考える。制御点の姿勢
のみを回転軸ｋの回りに回転させると、手首のみが回転
して肘部分が回転しない場合があるので、制御点の位置
ｒと姿勢［ｎ，ｏ，ａ］を回転軸ｋの回りに回転させる
ようにする。今問題にしていることは、ベクトルｋとΔ
θに対応する（１）式の左辺の量［ΔｒT 、ΔωT ］T
の量を如何にして求めるかという問題である。 【０００８】位置の回転 （２）式は回転軸ベクトルｋが原点を起点にしている場
合のマトリクスであるので、制御点Ｐを回転した位置を
求めるには、補正をしてやらねばならない。そのために
点Ｐより線ｌに垂線を下しその点をＨとする。マニピュ
レータのヤコビアンを求める際に利用した、座標系の原
点Ｓをこの点Ｈに平行移動して考える。ベクトルＳＨを
ｒ1 ベクトルＨＰをｒ2 とすると制御点の位置ｒは下記
のようになる。 回転前の位置ｒ＝ｒ1 ＋ｒ2 （３） そこでｒ2 がベクトルｋの回りに回転する場合を計算し
てその結果をｒ1 に加えてやればもとめる回転位置にな
る。 回転後の位置＝ｒ1 ＋Ｒ* ｒ2 （４） 従って微小変位Δｒは下記のように求められる。 Δｒ＝（Ｒ−Ｅ3 ） * ｒ2 （５） 【０００９】姿勢の回転 姿勢の回転は回転軸ベクトルがｋで回転角度がΔθであ
るので簡単に下記のようにもとまる。 Δω＝ｋ* Δθ （６） （５）と（６）の値を（１）式にいれて左辺の値がもと
まる。ヤコビアンＪは回転前のマニピュレータ の姿勢
で求められるので既知の要素からなるマトリクスであ
る。（注意：宇宙ロボットの場合はマニピュレータ登載
の衛星がマニピュレータの動きで影響を受けるのでヤコ
ビアンマトリクスは簡単にもとまらないことがある。想
定して場合は質量に大きな差がある場合である。）
（１）式を解くことによって、マニピュレータの軸の微
小角度ベクトルΔｑが求められる。 Δｑ＝Ｊ# * ［ΔｒT 、ΔωT ］T ＋（Ｅn - Ｊ# Ｊ）* ｙ （７） Ｊ# はＪの疑似逆行列である。Ｅn はｎｘｎの単位マト
リクスである。ｙは任意のベクトルである。（７）式の
右辺の第二項はゼロベクトルとおいてよいので書き直す
と下記のようになる。 Δｑ＝Ｊ# * ［ΔｒT 、ΔωT ］T （８） もう一度整理して説明する。 ａ）右椀のマニピュレータについて考える。 ｂ）マニピュレータのツール制御点Ｐを左腕側に少しず
らした点Ｑを考える。 ｃ）マニピュレータの肩の位置Ｓと点Ｑを結ぶ線を想定
し、これを回転軸ベクトルｋとする。 ｄ）マニピュレータのツールの位置ｒ、姿勢を［ｎ，
ｏ，ａ］としこれを前記ベクトルｋの回りにΔθだけ回
転させる。 ｅ）上の計算を式（５）（６）によって式（１）の左辺
の値を求める。 ｆ）式（１）を解くことによって微少変化Δθに対応す
る、マニピュレータの軸の微少角度ベクトルΔｑを
（８）式のように求める。 ｇ）回転前の軸角度ベクトルｑにΔｑを加えることによ
ってマニピュレータの肘を回転した姿勢を求めることが
できる。 ｈ）ｄ）からｇ）を複数回実行してマニピュレータの肘
を回転させる。この結果のマニピュレータのツールの制
御点の姿勢と位置は ｛ΩA 、ｒA ｝ である。 【００１０】（論理演算Ｂ）式（１）におけるヤコビア
ンＪは、展開して書くと次のようになる。 【００１１】 【数１】 【００１２】ここで 各要素は以下の次元のマトリクス Ｊ11：3 x (n-4) Ｊ12：3 x 4 Ｊ21：3 x (n-4) Ｊ22：3 x 4 最初の軸角度（ｑ1 s 、ｑ2 s ，ｑ3 s ，…, ｑns）が
形成するマニピュレータのツール部の姿勢はΩであっ
た。今の状態の姿勢はΩA である。従って姿勢ΩA をΩ
に合わせることを考える。回転マトリクスＲを考えると Ω＝Ｒ* ΩA （１０） これより Ｒ＝Ω* ΩA ー 1 ＝Ω* ΩA T （１１） この式から、回転マトリクスＲを形成する回転軸ベクト
ルｋB と回転角度θBが計算できる。 Δω＝ｋB * θB ／（分割数ｍB ）とすると、次のよう
な関係がなりたつ。 【００１３】 【数２】 【００１４】ヤコビアンＪは毎回計算するとして、この
関係式は位置を無視すると次の関係式と同じことであ
る。 Δω＝Ｊ22* （Δｑn - 3 ，Δｑn-2 ，Δｑn-1 ，Δｑn ）T （１３） （Δｑn - 3 ，Δｑn-2 ，Δｑn-1 ，Δｑn ）T ＝Ｊ22 # * Δω＋（Ｅ4 −Ｊ22 # * Ｊ22）ｙ1 ＝Ｊ22 # * Δω （１４） 求められた微小角度をもとの軸角度に加える。再度ヤコ
ビアンをＪ22をもとめ上の計算を繰り返すことによっ
て、姿勢がもとのものと一致した、軸角度 ｛ｑiB｝ を求めることができる。 【００１５】（論理演算Ｃ）今のツールの状態は姿勢は
Ωで位置はｒA ' である。姿勢を調整したので位置がｒ
A からｒA ' に代わった。従ってツールの位置をｒA '
よりｒに姿勢を動かさず移せばよい。位置ｒA ' から元
の位置ｒへの方向への微小位置変化分Δｒは Δｒ＝（ｒ−ｒA ' ）／（分割数ｍC ）であるので ［ΔｒT ，０T ］T ＝Ｊ* ［Δｑ1 ，Δｑ2 ，Δｑ3 ，…，Δｑn ］T （１５） これを解くと ［Δｑ1 ，Δｑ2 ，Δｑ3 ，…，Δｑn ］T ＝Ｊ# * ［ΔｒT 、０T ］T ＋（Ｅn −Ｊ# * Ｊ）ｙ ＝Ｊ# * ［ΔｒT 、０T ］T （１６） 得られた微小角度を軸角度に加える。再度ヤコビアンＪ
を計算する。このことをｍC 回繰り返すことによって、
位置と姿勢を最初の ｛ Ω、ｒ｝にもってくることが
できる。ここで求められた軸角度は｛ｑiE｝である。最
初の軸角度｛ｑiS｝と最終的にえられる軸角度｛ｑiE｝
の間を適当な分割数ｍで割って微小角度｛ΔｑRi｝を求
める。この微小軸角度が求まると、 ｑi ＝ｑi ＋ΔｑRi （１７） として分割数（ｍ）だけくり返せば目的のマニピュレー
タの肘回転動作は実現される。注意することは疑似逆行
列Ｊ# が求まらないことがある。この場合はマニピュレ
ータの位置を少しずらしてやることによって可能にな
る。基本的なこのような問題はロボットシミュレータで
もって、扱っているロボットマニピュレータがどの様な
障害物回避動作が可能であるかを事前に検討しておい
て、マニピュレータの構造を決定しなければならないと
いうことがある。 【００１６】 【実施例】図１に、従来の制御装置に本発明の方法を組
み込む場合の実施例を示す。点線で囲んだ部分が本発明
に関係する部分である。マスタ・スレイブ型マニピュレ
ータのマスタ側の操作卓１に肘回転指令ボタン２ａ、２
ｂと、肘最大回転角度θmax を与えるダイヤル、ないし
デジタルスイッチ３を設ける。この角度は肘を最大どの
程度回転させるのかを決定するために使用する。肘回転
指令ボタンに「右回転」「左回転」の名称をあたえる。
「右回転」ボタン２ａを押せばマニピュレータは肘部分
が右回転し、「左回転」ボタン２ｂを押せば肘部分が左
回転するような微少回転角度ΔｑRiをΔｑ計算装置４が
発生し従来の位置制御器５に与える。具体的計算手法は
すでに説明したとおりである。なお、説明は一台のマニ
ピュレータについて述べたが、複数台のマニピュレータ
があれば、その数に見合った肘回転指令ボタンを準備す
ることが必要である。 【００１７】 【発明の効果】以上述べたように、本発明によれば、７
軸以上の冗長軸マニピュレータの障害物回避のマニピュ
レータ肘回転動作が可能になる。また、本発明に記載さ
れているように、マニピュレータの肩の点とツールの制
御点近傍に撰んだ点を結ぶ軸に回転することで、確実に
肘を動かすことが可能となるという格段の効果を奏する
ものである。
【図面の簡単な説明】
【図１】本発明を実施する装置例を示す図
【図２】本発明の論理処理のフローを示す図
【図３】マスタ・スレイブ型マニピュレータを示す図で
あり、（ａ）がマスタマニピュレータ（ｂ）がスレイブ
マニピュレータである。 【図４】スレイブマニピュレータ（７軸を有する多関節
型）の右腕側を示す図である。図中の矢印は軸の回転方
向を意味している。 【図５】本発明での位置関係を説明するための図であ
る。 【図６】ツールの位置と姿勢の回転を説明するための図
である。
あり、（ａ）がマスタマニピュレータ（ｂ）がスレイブ
マニピュレータである。 【図４】スレイブマニピュレータ（７軸を有する多関節
型）の右腕側を示す図である。図中の矢印は軸の回転方
向を意味している。 【図５】本発明での位置関係を説明するための図であ
る。 【図６】ツールの位置と姿勢の回転を説明するための図
である。
─────────────────────────────────────────────────────
フロントページの続き
(58)調査した分野(Int.Cl.7，ＤＢ名)
B25J 1/00 - 21/02
G05B 19/18 - 19/46
G05D 3/00 - 3/20
Claims (1)
- (57)【特許請求の範囲】 【請求項１】７軸以上の多関節マニピュレータにおい
て、 現在のツール部の位置と姿勢を第１ツール制御点とし、
マニピュレータの肩の点と前記第１ツール制御点以外の
点を結ぶ軸に前記第１ツール制御点を回転させる第１の
演算と、前記第１の演算により変換されたツール部の位置と姿勢
を第２ツール制御点とし、 前記ツール部に近い方から選
んだマニピュレータの３軸目または４軸目までを動かし
て前記第２ツール制御点の姿勢を前記第１ツール制御点
の姿勢に合わせる第２の演算と、前記第２の演算により変換されたツール部の位置と姿勢
を第３ツール制御点とし、 マニピュレータ全軸を使って
前記第３ツール制御点の位置を前記第１ツール制御点の
位置に合わせる演算と、 をヤコビアンを用いて順に実行することを特徴とする冗
長軸を有する７軸以上のマニピュレータの制御方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP11077392A JP3431026B2 (ja) | 1992-04-02 | 1992-04-02 | 冗長軸を有する７軸以上のマニピュレータの制御方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP11077392A JP3431026B2 (ja) | 1992-04-02 | 1992-04-02 | 冗長軸を有する７軸以上のマニピュレータの制御方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH05285863A JPH05285863A (ja) | 1993-11-02 |
| JP3431026B2 true JP3431026B2 (ja) | 2003-07-28 |
Family
ID=14544232
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP11077392A Expired - Fee Related JP3431026B2 (ja) | 1992-04-02 | 1992-04-02 | 冗長軸を有する７軸以上のマニピュレータの制御方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3431026B2 (ja) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4133843B2 (ja) * | 2004-01-14 | 2008-08-13 | 川崎重工業株式会社 | 産業用ロボット |
| US8423190B1 (en) | 2009-06-11 | 2013-04-16 | Kabushiki Kaisha Yaskawa Denki | Manipulator system |
| JP5612971B2 (ja) | 2010-09-07 | 2014-10-22 | オリンパス株式会社 | マスタスレーブマニピュレータ |
-
1992
- 1992-04-02 JP JP11077392A patent/JP3431026B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH05285863A (ja) | 1993-11-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3207728B2 (ja) | 冗長マニピュレータの制御方法 | |
| JP2874238B2 (ja) | 多関節形ロボットの制御方法 | |
| JP3431026B2 (ja) | 冗長軸を有する７軸以上のマニピュレータの制御方法 | |
| JP3884249B2 (ja) | 人間型ハンドロボット用教示システム | |
| JP4230196B2 (ja) | 位置決め演算方法および位置決め演算装置 | |
| Kheddar et al. | The hidden robot concept-high level abstraction teleoperation | |
| JP3146463B2 (ja) | 冗長軸マニピュレータの肘移動制御装置 | |
| JP3206765B2 (ja) | マスタスレーブマニピュレータの制御方法 | |
| JP2645866B2 (ja) | マニピュレータの制御方法および装置 | |
| JP2552977B2 (ja) | 力フィードバック型多軸操作装置 | |
| JP2507449B2 (ja) | 冗長関節を有するマスタ・スレ―ブマニピュレ―タ及び該マニピュレ―タの制御方法 | |
| JPH0677205B2 (ja) | ロボツトの制御方法 | |
| JP3080791B2 (ja) | マニプレータの直接教示用補助装置 | |
| Parkin | An interactive robotic simulation package | |
| JPH0428507B2 (ja) | ||
| JPH05337860A (ja) | ロボットハンドの教示装置およびロボットハンド | |
| Song et al. | Efficient formulation approach for the forward kinematics of 3-6 parallel mechanisms | |
| KR102624551B1 (ko) | 회전 변위의 제약이 없는 다관절 로봇의 작업공간 유연제어방법 | |
| JPS60218114A (ja) | ロボツトア−ム教示装置 | |
| Reynaerts et al. | Whole-finger manipulation with a two-fingered robot hand | |
| Vertut et al. | Sensor-aided and/or computer-aided bilateral teleoperator system (SCATS) | |
| Mishima et al. | Haptic teleoperation of humanoid robot hand using three-dimensional force feedback | |
| IKWUNZE et al. | Simulation of a Robot Arm that Mimics the Angular Movement and Control of an Authentic Human Arm | |
| JPH03262009A (ja) | 多自由度ロボットの制御方法 | |
| JPH06226667A (ja) | ７軸マニピュレータの制御方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20080523 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090523 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100523 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100523 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110523 Year of fee payment: 8 |
|
| LAPS | Cancellation because of no payment of annual fees |