JP2000167789A - 多関節ロボットの制御装置 - Google Patents
多関節ロボットの制御装置Info
- Publication number
- JP2000167789A JP2000167789A JP34697898A JP34697898A JP2000167789A JP 2000167789 A JP2000167789 A JP 2000167789A JP 34697898 A JP34697898 A JP 34697898A JP 34697898 A JP34697898 A JP 34697898A JP 2000167789 A JP2000167789 A JP 2000167789A
- Authority
- JP
- Japan
- Prior art keywords
- joint
- angle
- joints
- nth
- manipulator
- 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.)
- Granted
Links
Landscapes
- Manipulator (AREA)
Abstract
(57)【要約】
【課題】 冗長な自由度を持つ多関節ロボットの関節角
度を計算する際の計算量を大幅に低減させることができ
る。 【解決手段】 多関節ロボットの先端関節が目標位置に
距離的に近づくような関節角度を最小２乗法により１軸
づつ求め、求めた目標角度と現在の関節角度の差に比例
した量を１軸づつ関節を回転させることによって、最終
的に先端の関節が目標位置に達するようにしている。こ
のため、関節角度を計算する時の掛け算数（計算）量
は、従来の逆ヤコビ行列を用いた方法ではロボットを構
成する関節数の２乗で増えるのに対し、本発明では、関
節数に比例して増えるため、計算量が著しく減少する。
度を計算する際の計算量を大幅に低減させることができ
る。 【解決手段】 多関節ロボットの先端関節が目標位置に
距離的に近づくような関節角度を最小２乗法により１軸
づつ求め、求めた目標角度と現在の関節角度の差に比例
した量を１軸づつ関節を回転させることによって、最終
的に先端の関節が目標位置に達するようにしている。こ
のため、関節角度を計算する時の掛け算数（計算）量
は、従来の逆ヤコビ行列を用いた方法ではロボットを構
成する関節数の２乗で増えるのに対し、本発明では、関
節数に比例して増えるため、計算量が著しく減少する。
Description
【０００１】
【発明の属する技術分野】本発明は、多関節ロボットの
制御装置に係り、特に、冗長な自由度を持つ多関節ロボ
ットの関節角度を算出する制御装置に関する。
制御装置に係り、特に、冗長な自由度を持つ多関節ロボ
ットの関節角度を算出する制御装置に関する。
【０００２】
【従来の技術】図１は、一般的な冗長な自由度を持つ多
関節ロボット（多関節マニピュレータ）１ｃを示す。こ
こでは、ロボットとマニピュレータは同じ意味で用い
る。この多関節マニピュレータ１ｃは、各関節１ａ，１
ｐ，１ｑ，１ｒ・・・１ｄが多数個のリンク１ｂで連結
され、各関節１ａ，１ｐ，１ｑ，１ｒ・・・１ｄは、そ
れぞれ関節の回転させるためのモータ（図示せず）を内
臓している。多関節マニピュレータ１ｃは、架台１ｅに
根元関節である第１関節１ａが固定され、電源１ｆによ
り駆動するコントローラ１ｇから各関節１ａ，１ｐ，１
ｑ，１ｒ・・・１ｄへの目標位置指令が通信インターフ
ェイス１ｈを通して各関節内のモータに伝達され駆動さ
れるようになっている。
関節ロボット（多関節マニピュレータ）１ｃを示す。こ
こでは、ロボットとマニピュレータは同じ意味で用い
る。この多関節マニピュレータ１ｃは、各関節１ａ，１
ｐ，１ｑ，１ｒ・・・１ｄが多数個のリンク１ｂで連結
され、各関節１ａ，１ｐ，１ｑ，１ｒ・・・１ｄは、そ
れぞれ関節の回転させるためのモータ（図示せず）を内
臓している。多関節マニピュレータ１ｃは、架台１ｅに
根元関節である第１関節１ａが固定され、電源１ｆによ
り駆動するコントローラ１ｇから各関節１ａ，１ｐ，１
ｑ，１ｒ・・・１ｄへの目標位置指令が通信インターフ
ェイス１ｈを通して各関節内のモータに伝達され駆動さ
れるようになっている。
【０００３】このような構造の多関節マニピュレータ１
ｃにおいて、先端関節１ｄの位置を制御するとき、関節
数が７個以上の場合は、従来は、「コロナ社 ロボット
制御基礎理論 吉川恒夫著、ｐ５７−５８、ｐ２２
２」、「多関節マニピュレータの冗長性分解制御、広瀬
茂男著、自動計測学会論文集Ｖｏｌ．２４， No.９，ｐ
９５４−ｐ９５９，１９８８」等に示されているよう
に、一般的には疑似ヤコビ行列を使って式１によりマニ
ピュレータ１ｃを構成する全ての目標関節位置を計算
し、式２により求めた目標関節位置から各関節の目標関
節角度を計算するようにしていた。具体的には以下の通
りである。 q=Jv + v + (I-J v + J v )k （q=[q1,q2, ・・・・・・, q N ]) （式１） q= ∫q dt （式２） 但し、ｑは第１から第Ｎ関節の移動速度行列であり、J
v + は疑似ヤコビ逆行列であり、J v はヤコビ逆行列で
あり、Ｉは単位行列であり、ｋは任意の定数ベクトル行
列であり、ｖは先端の関節の移動速度行列であり、ｑは
第１から第Ｎ関節の位置行列である。
ｃにおいて、先端関節１ｄの位置を制御するとき、関節
数が７個以上の場合は、従来は、「コロナ社 ロボット
制御基礎理論 吉川恒夫著、ｐ５７−５８、ｐ２２
２」、「多関節マニピュレータの冗長性分解制御、広瀬
茂男著、自動計測学会論文集Ｖｏｌ．２４， No.９，ｐ
９５４−ｐ９５９，１９８８」等に示されているよう
に、一般的には疑似ヤコビ行列を使って式１によりマニ
ピュレータ１ｃを構成する全ての目標関節位置を計算
し、式２により求めた目標関節位置から各関節の目標関
節角度を計算するようにしていた。具体的には以下の通
りである。 q=Jv + v + (I-J v + J v )k （q=[q1,q2, ・・・・・・, q N ]) （式１） q= ∫q dt （式２） 但し、ｑは第１から第Ｎ関節の移動速度行列であり、J
v + は疑似ヤコビ逆行列であり、J v はヤコビ逆行列で
あり、Ｉは単位行列であり、ｋは任意の定数ベクトル行
列であり、ｖは先端の関節の移動速度行列であり、ｑは
第１から第Ｎ関節の位置行列である。
【０００４】
【発明が解決しようとする課題】しかしながら、上述の
従来の方法において、疑似ヤコビ行列は、ロボットの関
節数が増えるほど行列のマトリックスが大きくなり、実
時間で計算するには高速の計算機を必要とする。また、
図２に示すような狭隘部である管２ａにマニピュレータ
２ｂを挿入する場合、先端の関節２ｃを管２ａの中心に
沿って動かすことになるが、この場合、疑似ヤコビ行列
を用いた従来の方法を用いると、マニピュレータ２ｂの
一部の関節２ｄが管２ａと接触し挿入できなくなること
がある。これは、管内挿入ではマニピュレータの関節が
動く範囲が制限されるので、管との接触を考慮して関節
の角度を補正するなどの処理が必要となるからである。
しかしながら、この補正処理を上述の式１に追加するよ
うな提案はこれまでなされておらず、問題であった。
従来の方法において、疑似ヤコビ行列は、ロボットの関
節数が増えるほど行列のマトリックスが大きくなり、実
時間で計算するには高速の計算機を必要とする。また、
図２に示すような狭隘部である管２ａにマニピュレータ
２ｂを挿入する場合、先端の関節２ｃを管２ａの中心に
沿って動かすことになるが、この場合、疑似ヤコビ行列
を用いた従来の方法を用いると、マニピュレータ２ｂの
一部の関節２ｄが管２ａと接触し挿入できなくなること
がある。これは、管内挿入ではマニピュレータの関節が
動く範囲が制限されるので、管との接触を考慮して関節
の角度を補正するなどの処理が必要となるからである。
しかしながら、この補正処理を上述の式１に追加するよ
うな提案はこれまでなされておらず、問題であった。
【０００５】そこで本発明は、従来からの問題点を解決
するためになされたものであり、関節角度を計算する際
の計算量を大幅に低減させることができると共に狭隘部
内にも挿入可能な多関節ロボットの制御装置を提供する
ことを目的としている。
するためになされたものであり、関節角度を計算する際
の計算量を大幅に低減させることができると共に狭隘部
内にも挿入可能な多関節ロボットの制御装置を提供する
ことを目的としている。
【０００６】
【課題を解決するための手段】上記の目的を達成するた
めに本発明は、モータを内臓したＮ個の各関節をリンク
を介して連結した多関節ロボットの制御装置であって、
この制御装置が、根元関節である第１関節を他の関節は
固定した状態で所定の微少角度Δθ1 だけ回転させると
き、先端関節である第Ｎ関節の目標位置をＤ（Ｄ1、Ｄ2、
Ｄ3 ）とし、第１関節を回転させる前の第Ｎ関節の現在
位置をＰ（Ｐ1、Ｐ2、Ｐ3 ）とし、第１関節をθ回転させ
たときの第Ｎ関節の位置の変化量をΔＰ（ΔＰ1、ΔＰ2、
ΔＰ3 ）とし、第１関節をθ回転させたときの第Ｎ関節
の位置と目標位置までの距離の２乗値をＥとし、ΔＰ1
＝ｆ1 （θ）、ΔＰ2 ＝ｆ2 （θ）、ΔＰ3 ＝ｆ
3 （θ）とし、Ｅ＝（Ｄ1 −Ｐ1 −ｆ1 （θ））2 ＋
（Ｄ2 −Ｐ2 −ｆ2 （θ））2 ＋（Ｄ3 −Ｐ3 −ｆ
3 （θ））2 を最少とする第１関節の回転量θを最小２
乗法により求めて微少角度Δθ1 とする第１関節の角度
演算手段と、第１関節に隣り合う第２関節から第Ｎ関節
の各々において、各関節を他の関節を固定した状態で所
定の微少角度Δθだけ回転させ、この微少角度Δθを上
記の第１関節の回転角度演算手段と同様に最小２乗法に
より求めて微少角度Δθとする第２関節乃至第Ｎ関節の
角度演算手段と、を有し、Ｅの値が所定のしきい値以下
となるまで第１関節乃至第Ｎ関節の角度演算手段が演算
を繰り返して行い、各関節の回転角度を算出することを
特徴としている。このように、本発明によれば、ロボッ
トの先端関節が目標位置に距離的に近づくような関節角
度を最小２乗法により１軸づつ求め、求めた目標角度と
現在の関節角度の差に比例した量を１軸づつ関節を回転
させることによって、最終的に先端の関節が目標位置に
達するようにしている。このため、関節角度を計算する
時の掛け算数（計算）量は、従来の逆ヤコビ行列を用い
た方法ではロボットを構成する関節数の２乗で増えるの
に対し、本発明では、関節数に比例して増えるため、計
算量が著しく減少する。この結果、本発明によれば、ロ
ボットを構成する関節数が多くなればなるほど、従来の
方法と比較してより大きく関節角度の計算量が減少する
ことになる。
めに本発明は、モータを内臓したＮ個の各関節をリンク
を介して連結した多関節ロボットの制御装置であって、
この制御装置が、根元関節である第１関節を他の関節は
固定した状態で所定の微少角度Δθ1 だけ回転させると
き、先端関節である第Ｎ関節の目標位置をＤ（Ｄ1、Ｄ2、
Ｄ3 ）とし、第１関節を回転させる前の第Ｎ関節の現在
位置をＰ（Ｐ1、Ｐ2、Ｐ3 ）とし、第１関節をθ回転させ
たときの第Ｎ関節の位置の変化量をΔＰ（ΔＰ1、ΔＰ2、
ΔＰ3 ）とし、第１関節をθ回転させたときの第Ｎ関節
の位置と目標位置までの距離の２乗値をＥとし、ΔＰ1
＝ｆ1 （θ）、ΔＰ2 ＝ｆ2 （θ）、ΔＰ3 ＝ｆ
3 （θ）とし、Ｅ＝（Ｄ1 −Ｐ1 −ｆ1 （θ））2 ＋
（Ｄ2 −Ｐ2 −ｆ2 （θ））2 ＋（Ｄ3 −Ｐ3 −ｆ
3 （θ））2 を最少とする第１関節の回転量θを最小２
乗法により求めて微少角度Δθ1 とする第１関節の角度
演算手段と、第１関節に隣り合う第２関節から第Ｎ関節
の各々において、各関節を他の関節を固定した状態で所
定の微少角度Δθだけ回転させ、この微少角度Δθを上
記の第１関節の回転角度演算手段と同様に最小２乗法に
より求めて微少角度Δθとする第２関節乃至第Ｎ関節の
角度演算手段と、を有し、Ｅの値が所定のしきい値以下
となるまで第１関節乃至第Ｎ関節の角度演算手段が演算
を繰り返して行い、各関節の回転角度を算出することを
特徴としている。このように、本発明によれば、ロボッ
トの先端関節が目標位置に距離的に近づくような関節角
度を最小２乗法により１軸づつ求め、求めた目標角度と
現在の関節角度の差に比例した量を１軸づつ関節を回転
させることによって、最終的に先端の関節が目標位置に
達するようにしている。このため、関節角度を計算する
時の掛け算数（計算）量は、従来の逆ヤコビ行列を用い
た方法ではロボットを構成する関節数の２乗で増えるの
に対し、本発明では、関節数に比例して増えるため、計
算量が著しく減少する。この結果、本発明によれば、ロ
ボットを構成する関節数が多くなればなるほど、従来の
方法と比較してより大きく関節角度の計算量が減少する
ことになる。
【０００７】また、本発明においては、さらに、各関節
の表面に設けられた接触センサと、接触センサがその関
節に接続されたリンク部が周囲の障害物に接触したこと
を検知した場合、その関節の回転角度を接触を回避する
方向に所定の微小角度だけ駆動させる接触回避手段とを
有することが好ましい。これにより、ロボットの駆動中
に、マニピュレータの関節の一部が周囲の障害物に接触
したら、その関節を接触を回避する方向に少しづつ回す
簡単な障害物回避ロジックを容易に組み込め、そのこと
により、結果的に障害物の多い管内にもマニピュレータ
を挿入できる。これに対し、従来のヤコビ行列ではマニ
ピュレータの先端位置が目標位置に近づくよう全関節の
目標角度を一括して求めてしまうため、障害物との回避
ロジックを容易に組み込むことができず、障害物の多い
管内にもマニピュレータを挿入するのが難しい。
の表面に設けられた接触センサと、接触センサがその関
節に接続されたリンク部が周囲の障害物に接触したこと
を検知した場合、その関節の回転角度を接触を回避する
方向に所定の微小角度だけ駆動させる接触回避手段とを
有することが好ましい。これにより、ロボットの駆動中
に、マニピュレータの関節の一部が周囲の障害物に接触
したら、その関節を接触を回避する方向に少しづつ回す
簡単な障害物回避ロジックを容易に組み込め、そのこと
により、結果的に障害物の多い管内にもマニピュレータ
を挿入できる。これに対し、従来のヤコビ行列ではマニ
ピュレータの先端位置が目標位置に近づくよう全関節の
目標角度を一括して求めてしまうため、障害物との回避
ロジックを容易に組み込むことができず、障害物の多い
管内にもマニピュレータを挿入するのが難しい。
【０００８】
【発明の実施の形態】次に、図１、図３及び図４によ
り、本発明の第１実施形態を説明する。図１は、本発明
が適用可能な一般的な多数個の関節を連結したＮ自由度
のマニピュレータを示す概略図であり、図３は、本発明
の第１実施形態による関節の回転角度を算出するための
制御用フローチャートであり、図４は、本発明の第１実
施形態を８自由度のマニピュレータに適用したときの関
節の動作状況を示す概略図である。
り、本発明の第１実施形態を説明する。図１は、本発明
が適用可能な一般的な多数個の関節を連結したＮ自由度
のマニピュレータを示す概略図であり、図３は、本発明
の第１実施形態による関節の回転角度を算出するための
制御用フローチャートであり、図４は、本発明の第１実
施形態を８自由度のマニピュレータに適用したときの関
節の動作状況を示す概略図である。
【０００９】図３の制御用フローチャートより、本発明
の第１実施形態による関節の目標角度を決定するための
関節角度算出方法（アルゴリズム）を、図１に示すＮ自
由度の多関節マニピュレータを例にして説明する。図３
に示すように、関節の目標角度を決定する際には、オペ
レータが、マニピュレータ先端の関節１ｄの目標位置を
指定した後、根元の関節１ａから先端の関節１ｄまで順
に１関節づつ、先端の関節１ｄの位置が目標位置に距離
的に近づくように個々の関節を少しずつ回転させ、目標
位置との誤差があるしきい値以下になるまで前述の処理
を繰り返すようにしている。以下、具体的に説明する。
先ず、図３のステップＳ１において、オペレータがコン
トローラ１ｇを使って、マニピュレータ１ｃの先端関節
１ｄの目標位置Ｄ（Ｄ1 、Ｄ2 、Ｄ3 ）を入力する。こ
こで、Ｄ1 、Ｄ2 、Ｄ3 は、それぞれ目標位置Ｄのｘ，
ｙ，ｚ方向成分である。
の第１実施形態による関節の目標角度を決定するための
関節角度算出方法（アルゴリズム）を、図１に示すＮ自
由度の多関節マニピュレータを例にして説明する。図３
に示すように、関節の目標角度を決定する際には、オペ
レータが、マニピュレータ先端の関節１ｄの目標位置を
指定した後、根元の関節１ａから先端の関節１ｄまで順
に１関節づつ、先端の関節１ｄの位置が目標位置に距離
的に近づくように個々の関節を少しずつ回転させ、目標
位置との誤差があるしきい値以下になるまで前述の処理
を繰り返すようにしている。以下、具体的に説明する。
先ず、図３のステップＳ１において、オペレータがコン
トローラ１ｇを使って、マニピュレータ１ｃの先端関節
１ｄの目標位置Ｄ（Ｄ1 、Ｄ2 、Ｄ3 ）を入力する。こ
こで、Ｄ1 、Ｄ2 、Ｄ3 は、それぞれ目標位置Ｄのｘ，
ｙ，ｚ方向成分である。
【００１０】次に、ステップＳ２において、他の関節を
固定したまま、第１関節１ａ（最も根元の関節）だけを
微少角度Δθ1 だけ回転させる。この微少角度Δθ1 の
計算は以下のように行う。 (1) 第１関節をθ（＜＜１）回転させたときの第Ｎ関節
の位置の変化量をΔＰ（ΔＰ1、ΔＰ2、ΔＰ3 ）とする。
ΔＰは従来のｘ，ｙ，ｚ軸まわりの回転行列（多関節ロ
ボットの順キネマティクス）を使って求める。第１関節
を回転させる前の第Ｎ関節の現在位置をＰ（Ｐ1、Ｐ2、Ｐ
3 ）とする。 (2) 第１関節をθ回転させたときの第Ｎ関節の位置と目
標位置までの距離の２乗値をＥとすると、 Ｅ＝（Ｄ1 −Ｐ1 −ΔＰ1 ）2 ＋（Ｄ2 −Ｐ2 −Δ
Ｐ2 ）2 ＋（Ｄ3 −Ｐ3 −ΔＰ3 ）2 を最小にする第１関節の回転量θを最小２乗法で求め
る。この式中のΔＰ1、ΔＰ2、ΔＰ3 はθの関数であり、
ΔＰ1 ＝ｆ1 （θ）、ΔＰ2 ＝ｆ2 （θ）、ΔＰ 3 ＝ｆ
3 （θ）と記述できる。これらを代入して上式を書き換
えると、Ｅ＝（Ｄ1 −Ｐ1 −ｆ1 （θ））2 ＋（Ｄ2 −
Ｐ2 −ｆ2 （θ））2 ＋（Ｄ3−Ｐ3 −ｆ3 （θ））2
と表される。 (3) ｃｏｓθ＝１，ｓｉｎθ＝θの近似をして上式を単
純化し、θで偏微分した値を０とするθを求める。第１
関節の回転量Δθ1 は、このようにして求めたθに定数
Ｋ（Ｋ＜１）を乗じた値、即ち、Δθ1 ＝Ｋθで求ま
る。次に、ステップＳ３において、他の関節を固定した
まま、第１関節１ａに隣り合う第２関節１ｐを微少角度
Δθ2 だけ微少に回転させる。微少角度Δθ2 の計算方
法は、ステップＳ２と同様の要領で行う。以後、第３関
節１ｑ、第４関節１ｒと先端関節である第Ｎ関節１ｄま
で同様の要領で回転させる。ステップＳ４は、第Ｎ関節
１ｄについての処理である。さらに、ステップＳ５にお
いて、先端の関節の現在位置と目標位置までの距離Ｅを
計算し、しきい値Ｅ0 以下か否かを判定し、以下でない
場合には、ステップＳ２に戻り、ＥがＥ0 以下となるま
で、ステップＳ２からステップＳ４の処理を繰り返す。
このようにして、最終的な各関節の目標回転角度が算出
される。
固定したまま、第１関節１ａ（最も根元の関節）だけを
微少角度Δθ1 だけ回転させる。この微少角度Δθ1 の
計算は以下のように行う。 (1) 第１関節をθ（＜＜１）回転させたときの第Ｎ関節
の位置の変化量をΔＰ（ΔＰ1、ΔＰ2、ΔＰ3 ）とする。
ΔＰは従来のｘ，ｙ，ｚ軸まわりの回転行列（多関節ロ
ボットの順キネマティクス）を使って求める。第１関節
を回転させる前の第Ｎ関節の現在位置をＰ（Ｐ1、Ｐ2、Ｐ
3 ）とする。 (2) 第１関節をθ回転させたときの第Ｎ関節の位置と目
標位置までの距離の２乗値をＥとすると、 Ｅ＝（Ｄ1 −Ｐ1 −ΔＰ1 ）2 ＋（Ｄ2 −Ｐ2 −Δ
Ｐ2 ）2 ＋（Ｄ3 −Ｐ3 −ΔＰ3 ）2 を最小にする第１関節の回転量θを最小２乗法で求め
る。この式中のΔＰ1、ΔＰ2、ΔＰ3 はθの関数であり、
ΔＰ1 ＝ｆ1 （θ）、ΔＰ2 ＝ｆ2 （θ）、ΔＰ 3 ＝ｆ
3 （θ）と記述できる。これらを代入して上式を書き換
えると、Ｅ＝（Ｄ1 −Ｐ1 −ｆ1 （θ））2 ＋（Ｄ2 −
Ｐ2 −ｆ2 （θ））2 ＋（Ｄ3−Ｐ3 −ｆ3 （θ））2
と表される。 (3) ｃｏｓθ＝１，ｓｉｎθ＝θの近似をして上式を単
純化し、θで偏微分した値を０とするθを求める。第１
関節の回転量Δθ1 は、このようにして求めたθに定数
Ｋ（Ｋ＜１）を乗じた値、即ち、Δθ1 ＝Ｋθで求ま
る。次に、ステップＳ３において、他の関節を固定した
まま、第１関節１ａに隣り合う第２関節１ｐを微少角度
Δθ2 だけ微少に回転させる。微少角度Δθ2 の計算方
法は、ステップＳ２と同様の要領で行う。以後、第３関
節１ｑ、第４関節１ｒと先端関節である第Ｎ関節１ｄま
で同様の要領で回転させる。ステップＳ４は、第Ｎ関節
１ｄについての処理である。さらに、ステップＳ５にお
いて、先端の関節の現在位置と目標位置までの距離Ｅを
計算し、しきい値Ｅ0 以下か否かを判定し、以下でない
場合には、ステップＳ２に戻り、ＥがＥ0 以下となるま
で、ステップＳ２からステップＳ４の処理を繰り返す。
このようにして、最終的な各関節の目標回転角度が算出
される。
【００１１】図４は、８個全てピッチ軸（ｙ軸まわりの
回転軸）で構成されたマニピュレータに対して、図４の
（４ａ）に示す状態で先端関節４ｈを目標位置４ｉに近
づけるときの、マニピュレータの動作状況を示したもの
であり、状態（４ｅ）に示すように最終的に全関節を使
い先端関節４ｈの目標位置４ｉに移動できることがわか
る。関節４ｆはマニピュレータの根元関節であり、図３
の制御用フローチャートに示す処理により、関節４ｆか
ら関節４ｇと順に関節４ｈまで繰り返し関節が駆動され
る。
回転軸）で構成されたマニピュレータに対して、図４の
（４ａ）に示す状態で先端関節４ｈを目標位置４ｉに近
づけるときの、マニピュレータの動作状況を示したもの
であり、状態（４ｅ）に示すように最終的に全関節を使
い先端関節４ｈの目標位置４ｉに移動できることがわか
る。関節４ｆはマニピュレータの根元関節であり、図３
の制御用フローチャートに示す処理により、関節４ｆか
ら関節４ｇと順に関節４ｈまで繰り返し関節が駆動され
る。
【００１２】本発明は、後述するように、自由度Ｎが大
きいほど従来方法に比べて計算量が小さくなる点で従来
法に比べて極めて有効である。以下、本発明と上述の式
１と式２で示される従来方法の計算量を比較する。 （比較時の前提条件）Ｎ個の関節を持つマニピュレータ
の先端関節を目標位置に移動させる場合の各関節角度の
計算量について比較する。算出時の計算は、ほとんどが
行列演算であるため、演算量は総掛け数で比較する。掛
け数は、マトリックスの成分を１個の文字で置き換えて
算出する。
きいほど従来方法に比べて計算量が小さくなる点で従来
法に比べて極めて有効である。以下、本発明と上述の式
１と式２で示される従来方法の計算量を比較する。 （比較時の前提条件）Ｎ個の関節を持つマニピュレータ
の先端関節を目標位置に移動させる場合の各関節角度の
計算量について比較する。算出時の計算は、ほとんどが
行列演算であるため、演算量は総掛け数で比較する。掛
け数は、マトリックスの成分を１個の文字で置き換えて
算出する。
【００１３】（従来方法と本発明との計算量の比較） (1) 従来の方法（コロナ社、ロボット制御基礎理論、吉
川恒夫著、ｐ５７−５８，ｐ２２２） 上述の式１中の行列間の計算（掛け算を有する部分）
は、下線部を引いた処理ａ（J v + v ）と処理ｂ（ Jv
+ J v ）の部分である。さらに、その計算に関連する掛
け算回数を示す。 q=Jv + v + (I-Jv + J v )k （q=[q1,q2, ・・・・・・, q N ]) （式１） 但し、ｑは第１関節１ａから第Ｎ関節１ｄの移動速度行
列（Ｎ×１行列）であり、J v + は疑似ヤコビ逆行列
（３×Ｎ行列）であり、J v はヤコビ逆行列（Ｎ×３行
列）であり、ｖは先端の関節の移動速度行列（３×１行
列）であり、ｑは第１関節１ａから第Ｎ関節１ｄの位置
行列（Ｎ×１行列）である。式１中のマトリクス間演算
の掛け算回数は、以下の通りとなる。 処理ａ：３Ｎ2 回（＝Ｎ×１×３×Ｎ） 処理ｂ：９Ｎ2 回（＝３×Ｎ×Ｎ×３） マトリックス部分の合計回数：１２Ｎ2 回
川恒夫著、ｐ５７−５８，ｐ２２２） 上述の式１中の行列間の計算（掛け算を有する部分）
は、下線部を引いた処理ａ（J v + v ）と処理ｂ（ Jv
+ J v ）の部分である。さらに、その計算に関連する掛
け算回数を示す。 q=Jv + v + (I-Jv + J v )k （q=[q1,q2, ・・・・・・, q N ]) （式１） 但し、ｑは第１関節１ａから第Ｎ関節１ｄの移動速度行
列（Ｎ×１行列）であり、J v + は疑似ヤコビ逆行列
（３×Ｎ行列）であり、J v はヤコビ逆行列（Ｎ×３行
列）であり、ｖは先端の関節の移動速度行列（３×１行
列）であり、ｑは第１関節１ａから第Ｎ関節１ｄの位置
行列（Ｎ×１行列）である。式１中のマトリクス間演算
の掛け算回数は、以下の通りとなる。 処理ａ：３Ｎ2 回（＝Ｎ×１×３×Ｎ） 処理ｂ：９Ｎ2 回（＝３×Ｎ×Ｎ×３） マトリックス部分の合計回数：１２Ｎ2 回
【００１４】(2) 本発明 行列間の演算は、図３に示す制御用フローチャートのス
テップＳ２からステップＳ４に行われるものであり、ス
テップＳ２の項目(1) で具体的に記載したものにあた
る。図３のステップＳ２における処理をマトリクスＲN
を使った式で記述すると以下の式３になり、計算の主要
部分であるマトリクスの掛け算の部分は、式３中の下線
部の処理ｃ（R N ( ωN )RN-1(ωN-1)・・ R3(ω3) R2(ω
2)R1 (ω1+Δθ1)）と処理ｄ（R1 (ω1+Δθ1)）の部分
である。処理ｃでは、Ｎ個のマトリクスを掛け合わせて
３×３行列をつくり、処理ｄでは処理ｃで求めた３×３
行列と位置行列Ｐ1 と掛け算処理である。さらに、処理
ｃ及び処理ｄにおける掛け算回数を示す。 PN = R N ( ωN )RN-1(ωN-1)・・ R3(ω3) R2(ω2)R1 (ω1+Δθ1)P1（式３） 但し、Ｐ1 は第１関節１ａの現在位置行列（３×１行
列）であり、ＲN (ωN) は第Ｎ関節（先端の関節１
ｄ）についての回転行列（３×３行列）でり、ωNは第
Ｎ関節（先端の関節１ｄ）の現在の関節角度であり、Δ
θ1 は第１関節１ａ（本処理で回転させる軸）の回転角
度である。式３中のマトリクス間演算の掛け算回数は、
以下の通りとなる。 処理ｃ：９Ｎ〔＝（３×３）×Ｎ〕 処理ｄ：９（＝３×３） マトリックス部分の合計回数：９（Ｎ＋１）
テップＳ２からステップＳ４に行われるものであり、ス
テップＳ２の項目(1) で具体的に記載したものにあた
る。図３のステップＳ２における処理をマトリクスＲN
を使った式で記述すると以下の式３になり、計算の主要
部分であるマトリクスの掛け算の部分は、式３中の下線
部の処理ｃ（R N ( ωN )RN-1(ωN-1)・・ R3(ω3) R2(ω
2)R1 (ω1+Δθ1)）と処理ｄ（R1 (ω1+Δθ1)）の部分
である。処理ｃでは、Ｎ個のマトリクスを掛け合わせて
３×３行列をつくり、処理ｄでは処理ｃで求めた３×３
行列と位置行列Ｐ1 と掛け算処理である。さらに、処理
ｃ及び処理ｄにおける掛け算回数を示す。 PN = R N ( ωN )RN-1(ωN-1)・・ R3(ω3) R2(ω2)R1 (ω1+Δθ1)P1（式３） 但し、Ｐ1 は第１関節１ａの現在位置行列（３×１行
列）であり、ＲN (ωN) は第Ｎ関節（先端の関節１
ｄ）についての回転行列（３×３行列）でり、ωNは第
Ｎ関節（先端の関節１ｄ）の現在の関節角度であり、Δ
θ1 は第１関節１ａ（本処理で回転させる軸）の回転角
度である。式３中のマトリクス間演算の掛け算回数は、
以下の通りとなる。 処理ｃ：９Ｎ〔＝（３×３）×Ｎ〕 処理ｄ：９（＝３×３） マトリックス部分の合計回数：９（Ｎ＋１）
【００１５】次に図３のステップＳ３における処理は、
マトリクスＲN を使った式で記述すると式４で表せ、掛
け算が生じるのは下線部の処理ｅ（R N ( ωN )RN-1(ω
N-1)・・ R3(ω3) R2(ω2+Δθ2)）及び処理ｆ（ R2(ω2+
Δθ2)）の部分である。 PN = R N ( ωN )RN-1(ωN-1)・・ R3(ω3) R2(ω2+Δθ2)P2 （式４） ここで、R N ( ωN )RN-1(ωN-1)・・ R3(ω3)は、（式
３）の計算結果が流用できるので新たな計算は生じな
い。さらに、処理ｅ及び処理ｆにおける掛け算回数を示
す。但し、Ｐ1 は第２関節１ｐの現在位置行列（３×１
行列）であり、ＲN (ωN) は第Ｎ関節（先端の関節１
ｄ）についての回転行列（３×３行列）であり、ω N は
第Ｎ関節（先端の関節１ｄ）の現在の関節角度であり、
Δθ2 は第２関節（本処理で回転させる軸）の回転角度
である。式４中のマトリクス間演算の掛け算回数は、以
下の通りとなる。 処理ｅ：０〔第１関節の場合の計算結果を使う〕 処理ｆ：９（＝３×３） マトリックス部分の合計回数：９
マトリクスＲN を使った式で記述すると式４で表せ、掛
け算が生じるのは下線部の処理ｅ（R N ( ωN )RN-1(ω
N-1)・・ R3(ω3) R2(ω2+Δθ2)）及び処理ｆ（ R2(ω2+
Δθ2)）の部分である。 PN = R N ( ωN )RN-1(ωN-1)・・ R3(ω3) R2(ω2+Δθ2)P2 （式４） ここで、R N ( ωN )RN-1(ωN-1)・・ R3(ω3)は、（式
３）の計算結果が流用できるので新たな計算は生じな
い。さらに、処理ｅ及び処理ｆにおける掛け算回数を示
す。但し、Ｐ1 は第２関節１ｐの現在位置行列（３×１
行列）であり、ＲN (ωN) は第Ｎ関節（先端の関節１
ｄ）についての回転行列（３×３行列）であり、ω N は
第Ｎ関節（先端の関節１ｄ）の現在の関節角度であり、
Δθ2 は第２関節（本処理で回転させる軸）の回転角度
である。式４中のマトリクス間演算の掛け算回数は、以
下の通りとなる。 処理ｅ：０〔第１関節の場合の計算結果を使う〕 処理ｆ：９（＝３×３） マトリックス部分の合計回数：９
【００１６】次に図３のステップＳ３における処理は、
マトリクスＲN を使った式で記述すると式５で表せ、掛
け算が生じる部分は式５中の下線部の処理ｇの部分であ
る。さらに、処理ｇにおける掛け算回数を示す。 PN = R N ( ωN + ΔθN )PN （式５） 但し、ＰN は、第Ｎ関節（先端の関節１ｄ）の現在位置
行列（３×１行列）であり、ＲN (ωN ) は、第Ｎ関節
（先端の関節１ｄ）についての回転行列（３×３行列で
あり、ωN は、第Ｎ関節（先端の関節１ｄ）の現在の関
節角度であり、ΔθN は、第２関節１ｐ（本処理で回転
させる軸）の回転角度である。式５中のマトリクス間演
算の掛け算回数は、以下の通りとなる。 処理ｇ：９（＝３×３） マトリックス部分の合計回数：９
マトリクスＲN を使った式で記述すると式５で表せ、掛
け算が生じる部分は式５中の下線部の処理ｇの部分であ
る。さらに、処理ｇにおける掛け算回数を示す。 PN = R N ( ωN + ΔθN )PN （式５） 但し、ＰN は、第Ｎ関節（先端の関節１ｄ）の現在位置
行列（３×１行列）であり、ＲN (ωN ) は、第Ｎ関節
（先端の関節１ｄ）についての回転行列（３×３行列で
あり、ωN は、第Ｎ関節（先端の関節１ｄ）の現在の関
節角度であり、ΔθN は、第２関節１ｐ（本処理で回転
させる軸）の回転角度である。式５中のマトリクス間演
算の掛け算回数は、以下の通りとなる。 処理ｇ：９（＝３×３） マトリックス部分の合計回数：９
【００１７】(3) 従来方法と本発明の計算量の比較 以上、従来方法と本発明を用いた処理におけるマトリク
ス間演算の掛け算回数を比較したが、その合計回数は以
下のようになる。 従来の方法：１２Ｎ2 回 本発明 ：１８Ｎ回（＝９（Ｎ＋１）＋９（Ｎ−１）
回） このように、自由度Ｎが大きいほど、本発明によれば、
従来方法に比べて計算量が著しく小さくなる。
ス間演算の掛け算回数を比較したが、その合計回数は以
下のようになる。 従来の方法：１２Ｎ2 回 本発明 ：１８Ｎ回（＝９（Ｎ＋１）＋９（Ｎ−１）
回） このように、自由度Ｎが大きいほど、本発明によれば、
従来方法に比べて計算量が著しく小さくなる。
【００１８】以上説明したように、本発明の第１実施形
態においては、マニピュレータ先端の関節が目標位置に
距離的に近づくような関節角度を最小２乗法により１軸
づつ求め、求めた目標角度と現在の関節角度の差に比例
した量を１軸づつ関節を回転させることによって、最終
的に先端の関節が目標位置に達するようにしている。関
節角度を計算する時の掛け算数（計算）量は、従来の逆
ヤコビ行列を用いた方法ではマニピュレータを構成する
関節数の２乗で増えるのに対し、本発明の第１実施形態
では、関節数に比例して増えるため、計算量が著しく減
少する。この結果、本発明の第１実施形態によれば、マ
ニピュレータを構成する関節数が多くなればなるほど、
従来の方法と比較してより大きく関節角度の計算量が減
少することになる。
態においては、マニピュレータ先端の関節が目標位置に
距離的に近づくような関節角度を最小２乗法により１軸
づつ求め、求めた目標角度と現在の関節角度の差に比例
した量を１軸づつ関節を回転させることによって、最終
的に先端の関節が目標位置に達するようにしている。関
節角度を計算する時の掛け算数（計算）量は、従来の逆
ヤコビ行列を用いた方法ではマニピュレータを構成する
関節数の２乗で増えるのに対し、本発明の第１実施形態
では、関節数に比例して増えるため、計算量が著しく減
少する。この結果、本発明の第１実施形態によれば、マ
ニピュレータを構成する関節数が多くなればなるほど、
従来の方法と比較してより大きく関節角度の計算量が減
少することになる。
【００１９】次に、図１、図５乃至図７により、本発明
の第２実施形態を説明する。図５及び図６は、狭隘部に
挿入する多関節マニピュレータの例を示しており、図７
は、本発明の第２実施形態による制御用フローチャート
を示している。以下、第２実施形態によるＮ自由度を有
するマニピュレータを図５や図６に示す狭隘部に挿入さ
せる場合の関節角度算出法を説明する。この実施形態に
おいては、図１に示すマニピュレータ１ｃの全関節の表
面に接触センサ（図示せず）を取り付けている。図５
は、原子力発電プラントに設けられた原子炉の上部炉心
構造物５ａの管群中に、先端にカメラ（図示せず）を取
り付けその根元が本体５ｂに取り付けられたマニピュレ
ータ５ｃを挿入する状態を示している。このとき、マニ
ピュレータ５ｃが管群と干渉しないように挿入するため
のマニピュレータ５ｃの関節角度決定が問題となる。図
６は、ガイドワイヤ６ｈの先端にマニピュレータ６ｂを
取り付け、マニピュレータ６ｂの先端に取り付けられた
カメラ６ｃを見ながらオペレータが操作器６ｆを使って
遠隔操作し、屈曲する管６ａ内中にマニピュレータ６ｂ
を挿入していき、マニピュレータ６ｂ自体は、ガイドワ
イヤー送り器６ｇで管６ａ内に押し込まれていく状態を
示している。この図６に示す場合も、図５の場合と同様
に、マニピュレータ６ｂが管６ａと干渉しないように挿
入するためのマニピュレータ６ｂの関節角度決定が問題
となる。
の第２実施形態を説明する。図５及び図６は、狭隘部に
挿入する多関節マニピュレータの例を示しており、図７
は、本発明の第２実施形態による制御用フローチャート
を示している。以下、第２実施形態によるＮ自由度を有
するマニピュレータを図５や図６に示す狭隘部に挿入さ
せる場合の関節角度算出法を説明する。この実施形態に
おいては、図１に示すマニピュレータ１ｃの全関節の表
面に接触センサ（図示せず）を取り付けている。図５
は、原子力発電プラントに設けられた原子炉の上部炉心
構造物５ａの管群中に、先端にカメラ（図示せず）を取
り付けその根元が本体５ｂに取り付けられたマニピュレ
ータ５ｃを挿入する状態を示している。このとき、マニ
ピュレータ５ｃが管群と干渉しないように挿入するため
のマニピュレータ５ｃの関節角度決定が問題となる。図
６は、ガイドワイヤ６ｈの先端にマニピュレータ６ｂを
取り付け、マニピュレータ６ｂの先端に取り付けられた
カメラ６ｃを見ながらオペレータが操作器６ｆを使って
遠隔操作し、屈曲する管６ａ内中にマニピュレータ６ｂ
を挿入していき、マニピュレータ６ｂ自体は、ガイドワ
イヤー送り器６ｇで管６ａ内に押し込まれていく状態を
示している。この図６に示す場合も、図５の場合と同様
に、マニピュレータ６ｂが管６ａと干渉しないように挿
入するためのマニピュレータ６ｂの関節角度決定が問題
となる。
【００２０】図７に示す制御用フローチャートにおいて
は、図３に示す第１実施形態の制御用フローチャート
と、ステップＳ１１、Ｓ１２、Ｓ１５、Ｓ１８、Ｓ１９
が同様であり、ステップＳ１３、Ｓ１４、Ｓ１６、Ｓ１
７が追加されている。図５及び図６に示すようなマニピ
ュレータを狭隘部に挿入させる場合、図７に示す処理で
マニピュレータの関節角度の決定を行うと、以下のよう
になる。即ち、ステップＳ１３において、第１関節をΔ
θ1 回転させることで、第１関節と第２関節のリンク部
が物体に接触するか否かを判定する。接触しない場合に
は、ステップＳ１５に進み、接触する場合には、ステッ
プＳ１４に進み、ΔΔθ 1 ＝−Ｋθ1 （Ｋ＜１）とし物
体と回避方向に第１関節を微少に駆動する。さらに、第
２関節においても、ステップＳ１６、Ｓ１７において、
同様な処理を行う。この実施形態においては、ステップ
Ｓ１２、Ｓ１５、Ｓ１８で、マニピュレータの先端位置
が目標位置に近づくように関節角度が算出されるが、管
にマニピュレータが接触することが十分考えられる。こ
の場合、マニピュレータの一部が管に引っかかりマニピ
ュレータの先端を目標位置に近づけられないほか、マニ
ピュレータを破損させることになる。管との接触有無
は、マニピュレータの全関節の表面に設置した接触セン
サにより検知できる。そこで、上述したように、ステッ
プＳ１３、Ｓ１４、Ｓ１６、Ｓ１７では、管に接触する
関節は、接触を回避する方向に少しだけ回転させるよう
にしている。こうすることで、管と接触していない関節
を動かしマニピュレータの先端が目標位置近づくように
回転させ、管と接触している関節は接触を回避する方向
に回転させ、全体として管との干渉を回避しながらマニ
ピュレータの先端を目標位置に近づけることができる。
は、図３に示す第１実施形態の制御用フローチャート
と、ステップＳ１１、Ｓ１２、Ｓ１５、Ｓ１８、Ｓ１９
が同様であり、ステップＳ１３、Ｓ１４、Ｓ１６、Ｓ１
７が追加されている。図５及び図６に示すようなマニピ
ュレータを狭隘部に挿入させる場合、図７に示す処理で
マニピュレータの関節角度の決定を行うと、以下のよう
になる。即ち、ステップＳ１３において、第１関節をΔ
θ1 回転させることで、第１関節と第２関節のリンク部
が物体に接触するか否かを判定する。接触しない場合に
は、ステップＳ１５に進み、接触する場合には、ステッ
プＳ１４に進み、ΔΔθ 1 ＝−Ｋθ1 （Ｋ＜１）とし物
体と回避方向に第１関節を微少に駆動する。さらに、第
２関節においても、ステップＳ１６、Ｓ１７において、
同様な処理を行う。この実施形態においては、ステップ
Ｓ１２、Ｓ１５、Ｓ１８で、マニピュレータの先端位置
が目標位置に近づくように関節角度が算出されるが、管
にマニピュレータが接触することが十分考えられる。こ
の場合、マニピュレータの一部が管に引っかかりマニピ
ュレータの先端を目標位置に近づけられないほか、マニ
ピュレータを破損させることになる。管との接触有無
は、マニピュレータの全関節の表面に設置した接触セン
サにより検知できる。そこで、上述したように、ステッ
プＳ１３、Ｓ１４、Ｓ１６、Ｓ１７では、管に接触する
関節は、接触を回避する方向に少しだけ回転させるよう
にしている。こうすることで、管と接触していない関節
を動かしマニピュレータの先端が目標位置近づくように
回転させ、管と接触している関節は接触を回避する方向
に回転させ、全体として管との干渉を回避しながらマニ
ピュレータの先端を目標位置に近づけることができる。
【００２１】本発明の第２実施形態においては、１個の
関節について最小２乗法を使い先端の関節が目標位置に
近づくような目標関節角度を求め、求めた目標関節角度
と現在の関節角度との差に比例した角度だけ関節を回
し、次に隣接する関節について同様に目標角度を求めて
関節を動かすといった逐次的な動作でマニピュレータを
動かしている。このため、マニピュレータの駆動中に、
マニピュレータの関節の一部が周囲の障害物に接触した
ら、その関節を接触を回避する方向に少しづつ回す簡単
な障害物回避ロジックを容易に組み込め、そのことによ
り、結果的に障害物の多い管内にもマニピュレータを挿
入できるようにしている。これに対し、従来のヤコビ行
列ではマニピュレータの先端位置が目標位置に近づくよ
う全関節の目標角度を一括して求めてしまうため、障害
物との回避ロジックを容易に組み込むことができず、障
害物の多い管内にもマニピュレータを挿入するのが難し
い。
関節について最小２乗法を使い先端の関節が目標位置に
近づくような目標関節角度を求め、求めた目標関節角度
と現在の関節角度との差に比例した角度だけ関節を回
し、次に隣接する関節について同様に目標角度を求めて
関節を動かすといった逐次的な動作でマニピュレータを
動かしている。このため、マニピュレータの駆動中に、
マニピュレータの関節の一部が周囲の障害物に接触した
ら、その関節を接触を回避する方向に少しづつ回す簡単
な障害物回避ロジックを容易に組み込め、そのことによ
り、結果的に障害物の多い管内にもマニピュレータを挿
入できるようにしている。これに対し、従来のヤコビ行
列ではマニピュレータの先端位置が目標位置に近づくよ
う全関節の目標角度を一括して求めてしまうため、障害
物との回避ロジックを容易に組み込むことができず、障
害物の多い管内にもマニピュレータを挿入するのが難し
い。
【００２２】次に、図８乃至図１２により、本発明の第
３実施形態を説明する。この第３実施実施形態は、第２
実施形態を４本足歩行ロボットに適用したものである。
図８は本発明の第３実施形態が適用された４本足歩行ロ
ボットを示す全体構成図であり、図９は第３実施形態に
よる４本足歩行ロボットが形状の異なる段差を移動して
いる様子を示す斜視図であり、図１０及び図１１は第３
実施形態の４本足歩行ロボットの歩行の際の関節の角度
を算出するための制御用フローチャートであり、図１２
は第３実施形態による４本足歩行ロボットの歩行中の姿
勢を示す斜視図である。図８に示すように、４本足歩行
ロボット８ａは、脚８ｂの１本あたりＮ軸の関節を持
ち、その全関節には接触センサ（図示せず）が取り付け
られている。４本の脚は、固定治具８ｃで連結されてい
る。ここで、関節８ｄは根元関節である第１関節を示
し、関節８ｅは先端関節である第Ｎ関節を示している。
この４本足歩行ロボット８ａへの動作指令は、コントロ
ーラ８ｆから通信インターフェイス８ｇを介してケーブ
ル８ｊから送られる。これらのコントローラ８ｆと通信
インターフェイス８ｇは電源８ｈと接続されている。
３実施形態を説明する。この第３実施実施形態は、第２
実施形態を４本足歩行ロボットに適用したものである。
図８は本発明の第３実施形態が適用された４本足歩行ロ
ボットを示す全体構成図であり、図９は第３実施形態に
よる４本足歩行ロボットが形状の異なる段差を移動して
いる様子を示す斜視図であり、図１０及び図１１は第３
実施形態の４本足歩行ロボットの歩行の際の関節の角度
を算出するための制御用フローチャートであり、図１２
は第３実施形態による４本足歩行ロボットの歩行中の姿
勢を示す斜視図である。図８に示すように、４本足歩行
ロボット８ａは、脚８ｂの１本あたりＮ軸の関節を持
ち、その全関節には接触センサ（図示せず）が取り付け
られている。４本の脚は、固定治具８ｃで連結されてい
る。ここで、関節８ｄは根元関節である第１関節を示
し、関節８ｅは先端関節である第Ｎ関節を示している。
この４本足歩行ロボット８ａへの動作指令は、コントロ
ーラ８ｆから通信インターフェイス８ｇを介してケーブ
ル８ｊから送られる。これらのコントローラ８ｆと通信
インターフェイス８ｇは電源８ｈと接続されている。
【００２３】図９に示すように、４本足歩行ロボット８
ａは、接触センサからの情報を基にして、形状が異なる
階段９ａ（図９（ａ）参照）及び高さの異なる段差９
ｂ，９ｃ（図９（ｂ）（ｃ）参照）を乗り越えながら移
動することができる。次に、図１０乃至図１２により、
第３実施形態による４本足歩行ロボットの歩行の際の関
節の角度を算出するための制御内容を説明する。先ず、
ステップＴ１において、４本足歩行ロボット８ａの移動
方向をオペレータが入力する。次に、ステップＴ２にお
いて、従来手法である静歩行パターンに従って、１本の
移動させる脚１１ａ（遊脚）と３本の４本足歩行ロボッ
トを支える脚１１ｂ（支持脚）を選択する。次に、ステ
ップＴ３において、支持脚の３接地点から構成される三
角形１１ｃ内に４本足歩行ロボットの重心が入るように
その中心１１ｄを移動させる。次に、ステップＴ４にお
いて、遊脚１１ａの先端関節１１ｅを垂直にＡ1 （ｍ
ｍ）だけ持ち上がるように、根元関節１１ｆ（第１関
節）から先端関節１１ｅ（第Ｎ関節）までを１軸づつ順
番にΔθi だけ回転させる。ここで、Δθｉは、図７の
ステップＳ１２，Ｓ１５，Ｓ１８におけるΔθ1，Δθ
2 ，ΔθN と同様な処理により求める。
ａは、接触センサからの情報を基にして、形状が異なる
階段９ａ（図９（ａ）参照）及び高さの異なる段差９
ｂ，９ｃ（図９（ｂ）（ｃ）参照）を乗り越えながら移
動することができる。次に、図１０乃至図１２により、
第３実施形態による４本足歩行ロボットの歩行の際の関
節の角度を算出するための制御内容を説明する。先ず、
ステップＴ１において、４本足歩行ロボット８ａの移動
方向をオペレータが入力する。次に、ステップＴ２にお
いて、従来手法である静歩行パターンに従って、１本の
移動させる脚１１ａ（遊脚）と３本の４本足歩行ロボッ
トを支える脚１１ｂ（支持脚）を選択する。次に、ステ
ップＴ３において、支持脚の３接地点から構成される三
角形１１ｃ内に４本足歩行ロボットの重心が入るように
その中心１１ｄを移動させる。次に、ステップＴ４にお
いて、遊脚１１ａの先端関節１１ｅを垂直にＡ1 （ｍ
ｍ）だけ持ち上がるように、根元関節１１ｆ（第１関
節）から先端関節１１ｅ（第Ｎ関節）までを１軸づつ順
番にΔθi だけ回転させる。ここで、Δθｉは、図７の
ステップＳ１２，Ｓ１５，Ｓ１８におけるΔθ1，Δθ
2 ，ΔθN と同様な処理により求める。
【００２４】次に、ステップＴ５において、第１乃至第
Ｎ関節を回転中に遊脚の先端関節１１ｅが段差に接触す
るか否かを接触センサにより判定する。接触する場合に
は、ステップＴ６に進み、遊脚の先端関節１１ｅが更に
垂直にΔＡ1 （ｍｍ）だけ持ち上がるように、ステップ
Ｔ４及びステップＴ５と同様な処理を繰り返して実行す
る。次に、ステップＴ７に進み、第１乃至第Ｎ関節を回
転中に遊脚の先端以外の関節が段差に接触するか否か又
は可動範囲をオーバするか否かを判定する。ステップＴ
５において接触しないと判定された場合も同様にステッ
プＴ７に進む。ステップＴ７において、抵触するか又は
可動範囲をオーバする場合には、ステップＴ８に進み、
現在回転中の関節を−ｋΔθｉ（但し、ｋ＜１）だけ回
転させ、接触及び可動範囲オーバを回避する。その後、
ステップＴ９に進む。また、ステップＴ７で接触もせず
又は可動範囲もオーバしてない場合にも、ステップＴ９
に進む。次に、ステップＴ９において、先端関節１１ｅ
がＡ1 （ｍｍ）だけ垂直に持ち上げることが終了したか
否かを判定する。終了していない場合には、ステップＴ
４に戻り持ち上げが終了するまで同様な処理を繰り返し
て行う。
Ｎ関節を回転中に遊脚の先端関節１１ｅが段差に接触す
るか否かを接触センサにより判定する。接触する場合に
は、ステップＴ６に進み、遊脚の先端関節１１ｅが更に
垂直にΔＡ1 （ｍｍ）だけ持ち上がるように、ステップ
Ｔ４及びステップＴ５と同様な処理を繰り返して実行す
る。次に、ステップＴ７に進み、第１乃至第Ｎ関節を回
転中に遊脚の先端以外の関節が段差に接触するか否か又
は可動範囲をオーバするか否かを判定する。ステップＴ
５において接触しないと判定された場合も同様にステッ
プＴ７に進む。ステップＴ７において、抵触するか又は
可動範囲をオーバする場合には、ステップＴ８に進み、
現在回転中の関節を−ｋΔθｉ（但し、ｋ＜１）だけ回
転させ、接触及び可動範囲オーバを回避する。その後、
ステップＴ９に進む。また、ステップＴ７で接触もせず
又は可動範囲もオーバしてない場合にも、ステップＴ９
に進む。次に、ステップＴ９において、先端関節１１ｅ
がＡ1 （ｍｍ）だけ垂直に持ち上げることが終了したか
否かを判定する。終了していない場合には、ステップＴ
４に戻り持ち上げが終了するまで同様な処理を繰り返し
て行う。
【００２５】次に、ステップＴ１０において、遊脚１１
ａの先端関節１１ｅが水平移動して一定歩幅Ａ2 （ｍ
ｍ）先の目標位置に近づくように、根元関節１１ｆ（第
１関節）から先端関節１１ｅ（第Ｎ関節）までを１軸づ
つ順番にΔθi だけ回転させる。ここで、Δθｉは、図
７のステップＳ１２，Ｓ１５，Ｓ１８におけるΔθ1 ，
Δθ2 ，ΔθN と同様な処理により求める。このステッ
プＴ１０においては、更に、ステップＴ５〜Ｔ８を実行
しながら、遊脚の先端関節１１ｅがＡ2 （ｍｍ）の水平
移動を終了するまで、このステップにおける処理を繰り
返して行う。次に、ステップＴ１１において、ステップ
Ｔ４とは逆に、遊脚の先端関節１１ｅをΔＡ1 （ｍｍ）
だけ垂直に下げる。この処理を、遊脚の先端関節１１ｅ
が段差面に接触するまで、繰り返して実行する。次に、
ステップＴ１１に進み、オペレータからの停止命令があ
るか否かを判定し、あれば、終了し、無ければ、ステッ
プＴ１３に進む。ステップＴ１３では、別の脚を遊脚と
して、同様の処理を実行する。具体的には、この別の脚
に関して、ステップＴ２に戻り、同様な処理をオペレー
タからの停止命令があるまで繰り返しておこなう。
ａの先端関節１１ｅが水平移動して一定歩幅Ａ2 （ｍ
ｍ）先の目標位置に近づくように、根元関節１１ｆ（第
１関節）から先端関節１１ｅ（第Ｎ関節）までを１軸づ
つ順番にΔθi だけ回転させる。ここで、Δθｉは、図
７のステップＳ１２，Ｓ１５，Ｓ１８におけるΔθ1 ，
Δθ2 ，ΔθN と同様な処理により求める。このステッ
プＴ１０においては、更に、ステップＴ５〜Ｔ８を実行
しながら、遊脚の先端関節１１ｅがＡ2 （ｍｍ）の水平
移動を終了するまで、このステップにおける処理を繰り
返して行う。次に、ステップＴ１１において、ステップ
Ｔ４とは逆に、遊脚の先端関節１１ｅをΔＡ1 （ｍｍ）
だけ垂直に下げる。この処理を、遊脚の先端関節１１ｅ
が段差面に接触するまで、繰り返して実行する。次に、
ステップＴ１１に進み、オペレータからの停止命令があ
るか否かを判定し、あれば、終了し、無ければ、ステッ
プＴ１３に進む。ステップＴ１３では、別の脚を遊脚と
して、同様の処理を実行する。具体的には、この別の脚
に関して、ステップＴ２に戻り、同様な処理をオペレー
タからの停止命令があるまで繰り返しておこなう。
【００２６】この４本足歩行ロボットに適用された本発
明の第３実施形態においては、Ｎ軸ある関節の全てを使
用して先端関節を目標位置に移動させることができるた
め、脚の可動範囲を広げることができる。各関節の関節
表面に取り付けた接触センサ（図示せず）からの情報を
基に各関節の回転量を調整しているため、移動対象であ
る段差の高さや幅のサイズが変化した場合でも、アルゴ
リズムを修正することなく乗り越え移動が可能となる。
また、段差障害物のために脚を構成する関節の一部が可
動範囲をオーバして脚姿勢がきつくなる状況が発生した
場合は、可動範囲を超えた関節を逆向きに回転させて可
能範囲内に戻すと共に別の関節の回転量を調節すること
により、結果的に脚の先端関節を目標位置に近づけ、歩
行による段差移動を可能にしている。従来の４本足歩行
ロボット（第４回日本ロボット学会学術講演会予稿集、
1986、ｐ381 〜ｐ386 ）の多くが３〜４自由度を持つ脚
で構成されているが、本発明の第３実施形態は、それ以
上の自由度を持つ脚で構成されている４本足歩行ロボッ
トの移動時の関節角度算出に関して特に有効である。
明の第３実施形態においては、Ｎ軸ある関節の全てを使
用して先端関節を目標位置に移動させることができるた
め、脚の可動範囲を広げることができる。各関節の関節
表面に取り付けた接触センサ（図示せず）からの情報を
基に各関節の回転量を調整しているため、移動対象であ
る段差の高さや幅のサイズが変化した場合でも、アルゴ
リズムを修正することなく乗り越え移動が可能となる。
また、段差障害物のために脚を構成する関節の一部が可
動範囲をオーバして脚姿勢がきつくなる状況が発生した
場合は、可動範囲を超えた関節を逆向きに回転させて可
能範囲内に戻すと共に別の関節の回転量を調節すること
により、結果的に脚の先端関節を目標位置に近づけ、歩
行による段差移動を可能にしている。従来の４本足歩行
ロボット（第４回日本ロボット学会学術講演会予稿集、
1986、ｐ381 〜ｐ386 ）の多くが３〜４自由度を持つ脚
で構成されているが、本発明の第３実施形態は、それ以
上の自由度を持つ脚で構成されている４本足歩行ロボッ
トの移動時の関節角度算出に関して特に有効である。
【００２７】本発明の第３実施形態による４本足歩行ロ
ボットの脚運びと歩幅（先端関節の移動軌跡）は、従来
の４本足歩行ロボットと同じであるが、本実施形態によ
れば、脚先を目標位置に移動させる際に、遊脚の段差接
触や関節の可動範囲オーバに対する回避を逐次実行して
いるので、従来のものとは異なり、予め段差の場所と形
状（ロボット用の移動マップ）がわかっていなくても、
移動方向だけをオペレータが指示するだけで、接触セン
サにより段差の位置と形状を検知し自動的に階段等を移
動することが可能となる。このように、本実施形態は、
従来から提案されている移動マップをベースとした制御
方法とは異なり、４本足歩行ロボットの正確な自己位置
測定も不要となり、そのため、適用範囲も広範囲とな
る。さらに、１本当り４軸を超える脚を持つ４本足歩行
ロボットの制御はこれまで実施されておらず、本実施形
態の４本足歩行ロボットにおいては、従来の先端関節を
絶えず接地させて移動する歩行とは異なり、膝関節（例
えば第Ｎ−１関節）を接地させた歩行も合わせて行うこ
とにより、従来のものよりも高い段差を乗り越えること
が可能となる。
ボットの脚運びと歩幅（先端関節の移動軌跡）は、従来
の４本足歩行ロボットと同じであるが、本実施形態によ
れば、脚先を目標位置に移動させる際に、遊脚の段差接
触や関節の可動範囲オーバに対する回避を逐次実行して
いるので、従来のものとは異なり、予め段差の場所と形
状（ロボット用の移動マップ）がわかっていなくても、
移動方向だけをオペレータが指示するだけで、接触セン
サにより段差の位置と形状を検知し自動的に階段等を移
動することが可能となる。このように、本実施形態は、
従来から提案されている移動マップをベースとした制御
方法とは異なり、４本足歩行ロボットの正確な自己位置
測定も不要となり、そのため、適用範囲も広範囲とな
る。さらに、１本当り４軸を超える脚を持つ４本足歩行
ロボットの制御はこれまで実施されておらず、本実施形
態の４本足歩行ロボットにおいては、従来の先端関節を
絶えず接地させて移動する歩行とは異なり、膝関節（例
えば第Ｎ−１関節）を接地させた歩行も合わせて行うこ
とにより、従来のものよりも高い段差を乗り越えること
が可能となる。
【００２８】
【発明の効果】以上説明したように本発明によれば、関
節角度を計算する際の計算量を大幅に低減させることが
できると共に狭隘部内にも挿入可能な多関節ロボットの
制御装置を提供することができる。さらに、本発明は、
４本足歩行ロボットにも適用可能である。
節角度を計算する際の計算量を大幅に低減させることが
できると共に狭隘部内にも挿入可能な多関節ロボットの
制御装置を提供することができる。さらに、本発明は、
４本足歩行ロボットにも適用可能である。
【図１】 一般的な冗長な自由度を持つ多関節ロボット
（多関節マニピュレータ）を示す概略図
（多関節マニピュレータ）を示す概略図
【図２】 狭隘部である管内に図１の多関節ロボット
（多関節マニピュレータ）を挿入する場合を示す概略図
（多関節マニピュレータ）を挿入する場合を示す概略図
【図３】 本発明の第１実施形態による関節の回転角度
を算出するための制御用フローチャート
を算出するための制御用フローチャート
【図４】 本発明の第１実施形態を８自由度のマニピュ
レータに適用したときの関節の動作状況を示す概略図
レータに適用したときの関節の動作状況を示す概略図
【図５】 原子力発電プラントに設けられた原子炉の上
部炉心構造物の管群内に挿入された本発明の第２実施形
態が適用可能な多関節マニピュレータを示す概略図
部炉心構造物の管群内に挿入された本発明の第２実施形
態が適用可能な多関節マニピュレータを示す概略図
【図６】 屈曲する管内に挿入された本発明の第２実施
形態が適用可能な多関節マニピュレータを示す概略図
形態が適用可能な多関節マニピュレータを示す概略図
【図７】 本発明の第２実施形態による関節の回転角度
を算出するための制御用フローチャート
を算出するための制御用フローチャート
【図８】 本発明の第３実施形態が適用された４本足歩
行ロボットを示す全体構成図
行ロボットを示す全体構成図
【図９】 本発明の第３実施形態による４本足歩行ロボ
ットが形状の異なる段差を移動している様子を示す斜視
図
ットが形状の異なる段差を移動している様子を示す斜視
図
【図１０】 本発明の第３実施形態の４本足歩行ロボッ
トの歩行の際の関節の角度を算出するための制御用フロ
ーチャート
トの歩行の際の関節の角度を算出するための制御用フロ
ーチャート
【図１１】 本発明の第３実施形態の４本足歩行ロボッ
トの歩行の際の関節の角度を算出するための制御用フロ
ーチャート
トの歩行の際の関節の角度を算出するための制御用フロ
ーチャート
【図１２】 本発明の第３実施形態による４本足歩行ロ
ボットの歩行中の姿勢を示す斜視図である。
ボットの歩行中の姿勢を示す斜視図である。
１ｃ 多関節ロボット（多関節マニピュレータ） １ａ，１ｐ，１ｒ，１ｄ 関節 １ｂ リンク １ｅ 架台 １ｆ 電源 １ｇ コントローラ １ｈ 通信インターフェイス ４ｆ，４ｇ，４ｈ 関節 ４ｉ 目標位置 ５ａ 原子炉の上部炉心構造物 ５ｃ マニピュレータ ６ａ 管 ６ｂ マニピュレータ ６ｆ 操作器 ６ｄ 電源 ６ｅ コントローラ ８ａ ４本足歩行ロボット ８ｂ 脚 ８ｃ 固定治具 ８ｄ 根元関節（第１関節） ８ｅ 先端関節（第Ｎ関節） ８ｆ コントローラ ８ｇ 通信インターフェイス ８ｊ ケーブル 11ａ，11ｂ 脚 11ｄ 中心 11ｆ 根元関節 11ｅ 先端関節
Claims (2)
- 【請求項１】 モータを内臓したＮ個の各関節をリンク
を介して連結した多関節ロボットの制御装置であって、
この制御装置が、 根元関節である第１関節を他の関節は固定した状態で所
定の微少角度Δθ1 だけ回転させるとき、先端関節であ
る第Ｎ関節の目標位置をＤ（Ｄ1、Ｄ2、Ｄ3 ）とし、第１
関節を回転させる前の第Ｎ関節の現在位置をＰ（Ｐ1、Ｐ
2、Ｐ3 ）とし、第１関節をθ回転させたときの第Ｎ関節
の位置の変化量をΔＰ（ΔＰ1、ΔＰ2、ΔＰ3 ）とし、第
１関節をθ回転させたときの第Ｎ関節の位置と目標位置
までの距離の２乗値をＥとし、ΔＰ1 ＝ｆ1 （θ）、Δ
Ｐ2 ＝ｆ2 （θ）、ΔＰ3 ＝ｆ3（θ）とし、Ｅ＝（Ｄ
1 −Ｐ1 −ｆ1 （θ））2 ＋（Ｄ2 −Ｐ2 −ｆ
2 （θ）） 2 ＋（Ｄ3 −Ｐ3 −ｆ3 （θ））2 を最少と
する第１関節の回転量θを最小２乗法により求めて上記
微少角度Δθ1 とする第１関節の角度演算手段と、 第１関節に隣り合う第２関節から第Ｎ関節の各々におい
て、各関節を他の関節を固定した状態で所定の微少角度
Δθだけ回転させ、この微少角度Δθを上記の第１関節
の回転角度演算手段と同様に最小２乗法により求めて上
記微少角度Δθとする第２関節乃至第Ｎ関節の角度演算
手段と、を有し、 上記Ｅの値が所定のしきい値以下となるまで上記第１関
節乃至第Ｎ関節の角度演算手段が演算を繰り返して行
い、各関節の回転角度を算出することを特徴とする多関
節ロボットの制御装置。 - 【請求項２】 さらに、各関節の表面に設けられた接触
センサと、接触センサがその関節に接続されたリンク部
が周囲の障害物に接触したことを検知した場合、その関
節の回転角度を接触を回避する方向に所定の微小角度だ
け駆動させる接触回避手段とを有することを特徴とする
請求項１記載の多関節ロボットの制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP34697898A JP3068577B2 (ja) | 1998-12-07 | 1998-12-07 | 多関節ロボットの制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP34697898A JP3068577B2 (ja) | 1998-12-07 | 1998-12-07 | 多関節ロボットの制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2000167789A true JP2000167789A (ja) | 2000-06-20 |
| JP3068577B2 JP3068577B2 (ja) | 2000-07-24 |
Family
ID=18387105
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP34697898A Expired - Fee Related JP3068577B2 (ja) | 1998-12-07 | 1998-12-07 | 多関節ロボットの制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3068577B2 (ja) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2010001107A3 (en) * | 2008-06-30 | 2010-03-25 | Oliver Crispin Robotics Limited | Method of controlling a robotic arm |
| US8219246B2 (en) | 2001-06-13 | 2012-07-10 | Oliver Crispin Robotics Limited | System and method for controlling a robotic arm |
-
1998
- 1998-12-07 JP JP34697898A patent/JP3068577B2/ja not_active Expired - Fee Related
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8219246B2 (en) | 2001-06-13 | 2012-07-10 | Oliver Crispin Robotics Limited | System and method for controlling a robotic arm |
| WO2010001107A3 (en) * | 2008-06-30 | 2010-03-25 | Oliver Crispin Robotics Limited | Method of controlling a robotic arm |
| EP3839674A1 (en) * | 2008-06-30 | 2021-06-23 | Oliver Crispin Robotics Limited | Method of controlling a robotic arm |
Also Published As
| Publication number | Publication date |
|---|---|
| JP3068577B2 (ja) | 2000-07-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3207728B2 (ja) | 冗長マニピュレータの制御方法 | |
| US6845295B2 (en) | Method of controlling a robot through a singularity | |
| US11358282B2 (en) | System and method for constraint management of one or more robots | |
| KR101479233B1 (ko) | 로봇 및 그 협조작업 제어방법 | |
| JP5025641B2 (ja) | 運動学的特異点補償システムおよび方法 | |
| JP5114019B2 (ja) | エフェクタの軌道を制御するための方法 | |
| US11433541B2 (en) | Automated calibration system and method for a workpiece coordinate frame of a robot | |
| US8606402B2 (en) | Manipulator and control method thereof | |
| CN110682286B (zh) | 一种协作机器人实时避障方法 | |
| US9043030B2 (en) | Manipulator and path generation method thereof | |
| TWI704039B (zh) | 機械臂奇異點控制方法及系統 | |
| US7047107B2 (en) | Robot control apparatus | |
| US7248012B2 (en) | Teaching data preparing method for articulated robot | |
| US20220402127A9 (en) | Multi-angle end effector | |
| Sanchez et al. | Four-arm collaboration: Two dual-arm robots work together to manipulate tethered tools | |
| CN110545965B (zh) | 多关节机器人以及多关节机器人系统 | |
| JP3068577B2 (ja) | 多関節ロボットの制御装置 | |
| US11103994B2 (en) | System and method for natural tasking of one or more robots | |
| Song et al. | Efficient formulation approach for the forward kinematics of the 3-6 Stewart-Gough platform | |
| JP3884928B2 (ja) | 多関節ロボットの姿勢算出方法および姿勢算出装置 | |
| US20230109223A1 (en) | Intelligent clear path | |
| JP4000307B2 (ja) | 多関節ロボットのティーチングデータ作成方法 | |
| CN116021515B (zh) | 一种基于深度强化学习的机械臂容错避障方法 | |
| JP3146463B2 (ja) | 冗長軸マニピュレータの肘移動制御装置 | |
| US11673258B2 (en) | Modular robotic device and method to operate a modular robotic device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20000417 |
|
| S111 | Request for change of ownership or part of ownership |
Free format text: JAPANESE INTERMEDIATE CODE: R313115 |
|
| R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| LAPS | Cancellation because of no payment of annual fees |