JP3223288B2 - 移動マニピュレータの位置・姿勢制御方法 - Google Patents
移動マニピュレータの位置・姿勢制御方法Info
- Publication number
- JP3223288B2 JP3223288B2 JP23485792A JP23485792A JP3223288B2 JP 3223288 B2 JP3223288 B2 JP 3223288B2 JP 23485792 A JP23485792 A JP 23485792A JP 23485792 A JP23485792 A JP 23485792A JP 3223288 B2 JP3223288 B2 JP 3223288B2
- Authority
- JP
- Japan
- Prior art keywords
- manipulator
- joint
- equation
- link
- moving
- 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
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【発明の詳細な説明】
【０００１】
【産業上の利用分野】本発明は、移動体にマニピュレー
タを備えた移動マニピュレータの位置及び姿勢を制御す
る方法に関する。
タを備えた移動マニピュレータの位置及び姿勢を制御す
る方法に関する。
【０００２】
【従来の技術】工場での生産性の向上及び安全性の確保
を実現するために、種々の作業用のロボットが開発され
ており、例えば、その先端に手先効果器を設けた多関節
型のマニピュレータを、自律走行する移動体である台車
に搭載した移動マニピュレータがある。このような移動
マニピュレータにおいては、前記台車の走行制御と、前
記マニピュレータの手先効果器の位置・姿勢制御とを行
うことによって、作業対象物に対してマニピュレータに
て種々の作業を行うものが考えられている。
を実現するために、種々の作業用のロボットが開発され
ており、例えば、その先端に手先効果器を設けた多関節
型のマニピュレータを、自律走行する移動体である台車
に搭載した移動マニピュレータがある。このような移動
マニピュレータにおいては、前記台車の走行制御と、前
記マニピュレータの手先効果器の位置・姿勢制御とを行
うことによって、作業対象物に対してマニピュレータに
て種々の作業を行うものが考えられている。
【０００３】
【発明が解決しようとする課題】ところが、前述の如き
移動マニピュレータでは、走行中にマニピュレータが高
速運動すると、その運動の反力が台車の駆動輪に作用し
て台車が揺動し、この揺動によってマニピュレータが揺
動し、手先効果器の位置・姿勢が目標の位置・姿勢に制
御できず、目的とする作業ができなくなるという問題が
あった。また、台車が高加速度での加速又は高速度での
旋回を行った場合、その動作の反力がマニピュレータに
作用してマニピュレータが揺動し、手先効果器の位置・
姿勢が目標の位置・姿勢に制御できず、目的とする作業
ができなくなるという問題があった。
移動マニピュレータでは、走行中にマニピュレータが高
速運動すると、その運動の反力が台車の駆動輪に作用し
て台車が揺動し、この揺動によってマニピュレータが揺
動し、手先効果器の位置・姿勢が目標の位置・姿勢に制
御できず、目的とする作業ができなくなるという問題が
あった。また、台車が高加速度での加速又は高速度での
旋回を行った場合、その動作の反力がマニピュレータに
作用してマニピュレータが揺動し、手先効果器の位置・
姿勢が目標の位置・姿勢に制御できず、目的とする作業
ができなくなるという問題があった。
【０００４】本発明は斯かる事情に鑑みてなされたもの
であり、台車の運動と、マニピュレータの運動との間の
干渉を抑制し、精度が高い位置・姿勢制御を行うことを
可能とする移動マニピュレータの位置・姿勢制御方法を
提供することを目的とする。
であり、台車の運動と、マニピュレータの運動との間の
干渉を抑制し、精度が高い位置・姿勢制御を行うことを
可能とする移動マニピュレータの位置・姿勢制御方法を
提供することを目的とする。
【０００５】
【課題を解決するための手段】本発明に係る移動マニピ
ュレータの位置・姿勢制御方法は、左右一対の駆動輪の
駆動によって任意の方向に移動する移動体に、その各リ
ンクが各別に駆動される多関節型のマニピュレータを備
えてなる移動マニピュレータを、前記移動体の位置及び
姿勢を含めて決定された各別の目標角度に前記各リンク
を制御することにより、前記マニピュレータの手先効果
器を目標の位置及び姿勢に制御する移動マニピュレータ
の位置・姿勢制御装置において、前記移動体の進行速度
及び旋回角速度と、前記マニピュレータの各関節の関節
角及び関節角速度とを検出し、前記手先効果器の目標の
位置及び姿勢に基づいて得られる前記移動体の進行及び
旋回加速度の目標値、並びに前記マニピュレータの各関
節の目標角加速度と、検出した移動体の進行速度及び旋
回角速度、並びにマニピュレータの各関節の関節角及び
関節角速度とに関連して、前記目標の位置及び姿勢の実
現のために、前記移動体の進行速度及び旋回角速度を含
めて前記マニピュレータの各リンクに生ぜしめるべき速
度，加速度，角速度及び角加速度の目標値を、前記移動
体への取付け側から先端側へ順次求め、求められた速
度，加速度，角速度及び角加速度の目標値にて前記マニ
ピュレータの各リンクの夫々が運動するために必要であ
る各関節のトルクを、先端側から移動体への取付け側へ
順次求め、更に、前記運動のために必要な前記移動体の
各駆動輪のトルクを求めて、夫々に対して求めたトルク
が生じるように、前記各駆動輪及び各関節を制御するこ
とを特徴とする。
ュレータの位置・姿勢制御方法は、左右一対の駆動輪の
駆動によって任意の方向に移動する移動体に、その各リ
ンクが各別に駆動される多関節型のマニピュレータを備
えてなる移動マニピュレータを、前記移動体の位置及び
姿勢を含めて決定された各別の目標角度に前記各リンク
を制御することにより、前記マニピュレータの手先効果
器を目標の位置及び姿勢に制御する移動マニピュレータ
の位置・姿勢制御装置において、前記移動体の進行速度
及び旋回角速度と、前記マニピュレータの各関節の関節
角及び関節角速度とを検出し、前記手先効果器の目標の
位置及び姿勢に基づいて得られる前記移動体の進行及び
旋回加速度の目標値、並びに前記マニピュレータの各関
節の目標角加速度と、検出した移動体の進行速度及び旋
回角速度、並びにマニピュレータの各関節の関節角及び
関節角速度とに関連して、前記目標の位置及び姿勢の実
現のために、前記移動体の進行速度及び旋回角速度を含
めて前記マニピュレータの各リンクに生ぜしめるべき速
度，加速度，角速度及び角加速度の目標値を、前記移動
体への取付け側から先端側へ順次求め、求められた速
度，加速度，角速度及び角加速度の目標値にて前記マニ
ピュレータの各リンクの夫々が運動するために必要であ
る各関節のトルクを、先端側から移動体への取付け側へ
順次求め、更に、前記運動のために必要な前記移動体の
各駆動輪のトルクを求めて、夫々に対して求めたトルク
が生じるように、前記各駆動輪及び各関節を制御するこ
とを特徴とする。
【０００６】
【作用】本発明にあっては、手先効果器の目標の位置及
び姿勢に基づいて得られる移動体の目標進行加速度及び
マニピュレータの各関節の目標角加速度と、検出した移
動体の進行速度，旋回角速度及びマニピュレータの各関
節の関節角，関節角速度とに関連して、手先効果器を前
記目標の位置及び姿勢に制御するために必要である各リ
ンクの速度，加速度，角速度及び角加速度の含んだ目標
値を、移動体の側から先端側へ順次的に求め、求められ
たこれらの目標値にてマニピュレータの各リンクの夫々
を運動させるために必要となる移動体の各駆動輪のトル
ク及びマニピュレータの各関節のトルクを、マニピュレ
ータの先端側から移動体の側へ順次的に求める。このよ
うにして求めた移動体の各駆動輪のトルクは、マニピュ
レータの各関節のトルクを考慮したものであるため、求
めた各駆動輪のトルクを発生させるべく移動体の各駆動
輪を制御すると、マニピュレータの運動によって移動体
に作用する反力を抑止することができる。また、このよ
うにして求めたマニピュレータの各関節のトルクは、移
動体の各駆動輪のトルクを考慮したものであるため、求
めた各関節のトルクを発生させるべくマニピュレータの
各関節を制御すると、移動体の運動によってマニピュレ
ータに作用する反力を抑止することができる。
び姿勢に基づいて得られる移動体の目標進行加速度及び
マニピュレータの各関節の目標角加速度と、検出した移
動体の進行速度，旋回角速度及びマニピュレータの各関
節の関節角，関節角速度とに関連して、手先効果器を前
記目標の位置及び姿勢に制御するために必要である各リ
ンクの速度，加速度，角速度及び角加速度の含んだ目標
値を、移動体の側から先端側へ順次的に求め、求められ
たこれらの目標値にてマニピュレータの各リンクの夫々
を運動させるために必要となる移動体の各駆動輪のトル
ク及びマニピュレータの各関節のトルクを、マニピュレ
ータの先端側から移動体の側へ順次的に求める。このよ
うにして求めた移動体の各駆動輪のトルクは、マニピュ
レータの各関節のトルクを考慮したものであるため、求
めた各駆動輪のトルクを発生させるべく移動体の各駆動
輪を制御すると、マニピュレータの運動によって移動体
に作用する反力を抑止することができる。また、このよ
うにして求めたマニピュレータの各関節のトルクは、移
動体の各駆動輪のトルクを考慮したものであるため、求
めた各関節のトルクを発生させるべくマニピュレータの
各関節を制御すると、移動体の運動によってマニピュレ
ータに作用する反力を抑止することができる。
【０００７】
【実施例】以下本発明をその実施例を示す図面に基づい
て具体的に説明する。図１は本発明に係る移動マニピュ
レータの位置・姿勢制御方法の実施に使用する移動マニ
ピュレータの斜視図である。
て具体的に説明する。図１は本発明に係る移動マニピュ
レータの位置・姿勢制御方法の実施に使用する移動マニ
ピュレータの斜視図である。
【０００８】図中１は自律走行を行う移動体である独立
２輪駆動方式の台車であり、該台車１は、箱型の車体10
を、左右一対の駆動輪11,11 （右駆動輪のみ図示）と、
前後に配されたキャスタ12,12 （１つのみ図示）とにて
支持してなる。前記駆動輪11,11 は各別のモータ（図示
せず）によって独立的に駆動されるようになっている。
台車１の車体10の上部には、連結された複数の回転式の
関節21,21,…及びリンク22,22,…を有し、その先端部に
手先効果器20を備えた多関節型のマニピュレータ２が備
えられている。前記関節21,21,…の夫々は、サーボモー
タ（図示せず）によって駆動されるようになっている。
２輪駆動方式の台車であり、該台車１は、箱型の車体10
を、左右一対の駆動輪11,11 （右駆動輪のみ図示）と、
前後に配されたキャスタ12,12 （１つのみ図示）とにて
支持してなる。前記駆動輪11,11 は各別のモータ（図示
せず）によって独立的に駆動されるようになっている。
台車１の車体10の上部には、連結された複数の回転式の
関節21,21,…及びリンク22,22,…を有し、その先端部に
手先効果器20を備えた多関節型のマニピュレータ２が備
えられている。前記関節21,21,…の夫々は、サーボモー
タ（図示せず）によって駆動されるようになっている。
【０００９】また、駆動輪11,11 の夫々には、その回転
に応じてパルス信号を発振するパスジェネレータよりな
る走行速度検出器（図示せず）が設けられている。そし
て、前記マニピュレータ２の各関節21,21,…には、その
関節角度を検出する角度検出器（図示せず）が設けられ
ている。
に応じてパルス信号を発振するパスジェネレータよりな
る走行速度検出器（図示せず）が設けられている。そし
て、前記マニピュレータ２の各関節21,21,…には、その
関節角度を検出する角度検出器（図示せず）が設けられ
ている。
【００１０】また、図中、ΣV は台車１に設定された移
動座標系、Σw は移動マニピュレータの走行面（床面）
に設定された作業座標系である。
動座標系、Σw は移動マニピュレータの走行面（床面）
に設定された作業座標系である。
【００１１】まず、台車１の運動学について説明する。
【００１２】移動マニピュレータは、走行用のモータの
トルクを、駆動輪11,11 と走行面との摩擦を介して台車
１へ伝え走行する。その摩擦力は駆動輪11,11 のスリッ
プ率とスリップ角との非線形な関数として表されるが、
そのスリップ率，スリップ角が小さい場合には、滑りが
全くない状態とみなすことができ、摩擦力は拘束力であ
ると仮定することができる。以下、このような仮定が成
り立つ場合について説明する。
トルクを、駆動輪11,11 と走行面との摩擦を介して台車
１へ伝え走行する。その摩擦力は駆動輪11,11 のスリッ
プ率とスリップ角との非線形な関数として表されるが、
そのスリップ率，スリップ角が小さい場合には、滑りが
全くない状態とみなすことができ、摩擦力は拘束力であ
ると仮定することができる。以下、このような仮定が成
り立つ場合について説明する。
【００１３】図２は移動マニピュレータのフレームモデ
ルを示す模式図である。図２において、移動マニピュレ
ータを代表する移動座標系Σv は、その原点を左右の駆
動輪11,11 の中点に設定してあり、そのｘ軸を駆動輪1
1,11 の車軸の方向、そのｙ軸を台車１の前後方向、そ
のｚ軸を台車１の高さ方向に夫々設定してある。また、
作業座標系Σw は走行面に固定してあり、移動マニピュ
レータの位置は、作業座標系Σw に対する移動座標系Σ
v の原点の位置で表され、移動マニピュレータの姿勢
は、移動座標系Σv の各座標軸回りの姿勢角で表され
る。
ルを示す模式図である。図２において、移動マニピュレ
ータを代表する移動座標系Σv は、その原点を左右の駆
動輪11,11 の中点に設定してあり、そのｘ軸を駆動輪1
1,11 の車軸の方向、そのｙ軸を台車１の前後方向、そ
のｚ軸を台車１の高さ方向に夫々設定してある。また、
作業座標系Σw は走行面に固定してあり、移動マニピュ
レータの位置は、作業座標系Σw に対する移動座標系Σ
v の原点の位置で表され、移動マニピュレータの姿勢
は、移動座標系Σv の各座標軸回りの姿勢角で表され
る。
【００１４】移動座標系Σv のｘ軸回りの回転及びｙ軸
回りの回転は、駆動輪11,11 とキャスタ12,12 とによっ
て拘束されており、移動座標系Σv のｘ軸方向の並進及
びｙ軸方向の並進は、夫々駆動輪11,11 の摩擦力と、重
力とにより拘束されていると仮定すると、作業座標系Σ
w で表された移動座標系Σv の並進速度は下記(1) 式に
て表され、その移動座標系Σv の回転角速度は下記(2)
式にて表される。
回りの回転は、駆動輪11,11 とキャスタ12,12 とによっ
て拘束されており、移動座標系Σv のｘ軸方向の並進及
びｙ軸方向の並進は、夫々駆動輪11,11 の摩擦力と、重
力とにより拘束されていると仮定すると、作業座標系Σ
w で表された移動座標系Σv の並進速度は下記(1) 式に
て表され、その移動座標系Σv の回転角速度は下記(2)
式にて表される。
【００１５】
【数１】
【００１６】
【数２】
【００１７】また、移動座標系Σv のｙ軸方向の並進速
度ＶV と、移動座標系Σv のｚ軸回りの角速度ωv とを
下記(3) 式の転置行列で表すと、その転置行列は左右の
駆動輪11,11 の回転角速度を用いて下記(4) 式にて表さ
れる。
度ＶV と、移動座標系Σv のｚ軸回りの角速度ωv とを
下記(3) 式の転置行列で表すと、その転置行列は左右の
駆動輪11,11 の回転角速度を用いて下記(4) 式にて表さ
れる。
【００１８】
【数３】
【００１９】
【数４】
【００２０】移動マニピュレータに働く外力は重力だけ
であり、重力はｚ軸方向のみに働くことを考慮すると、
作業座標系Σw で表された重力加速度ベクトルは、下記
(5)式にて表される。
であり、重力はｚ軸方向のみに働くことを考慮すると、
作業座標系Σw で表された重力加速度ベクトルは、下記
(5)式にて表される。
【００２１】
【数５】
【００２２】この重力加速度ベクトルの働きを考慮する
と、作業座標系Σw で表された移動座標系Σv の並進加
速度ベクトルは前記(1) 式に関連して下記(6) 式にて表
され、また、作業座標系Σw で表された移動座標系Σv
の回転角加速度ベクトルは前記(2) 式に関連して下記
(7) 式にて表される。
と、作業座標系Σw で表された移動座標系Σv の並進加
速度ベクトルは前記(1) 式に関連して下記(6) 式にて表
され、また、作業座標系Σw で表された移動座標系Σv
の回転角加速度ベクトルは前記(2) 式に関連して下記
(7) 式にて表される。
【００２３】
【数６】
【００２４】
【数７】
【００２５】以下の説明においては、マニピュレータ２
のベースリンクを第０リンクとし、第０リンクから手先
効果器20へ向かって各リンク22,22,…を第１リンク，第
２リンク，…（第ｉリンク）と呼ぶこととする。
のベースリンクを第０リンクとし、第０リンクから手先
効果器20へ向かって各リンク22,22,…を第１リンク，第
２リンク，…（第ｉリンク）と呼ぶこととする。
【００２６】第０リンクの重心は台車１の重心でもあ
る。図２に示すように第０リンクの先端に座標系（以下
第０リンク座標系という）Σ0 を固定する。第０リンク
座標系Σ0 で表された第０リンクの重心をベクトル 0Ｐ
G0で表すと、このベクトル 0Ｐ G0と、作業座標系Σw ，
第０リンク座標系Σ0 間の姿勢変換行列を表す行列 WＲ
0 とを用いることにより、作業座標系Σw で表された第
０リンク座標系Σ0 の原点より第０リンクまでの位置ベ
クトルは、下記(8) 式にて表される。
る。図２に示すように第０リンクの先端に座標系（以下
第０リンク座標系という）Σ0 を固定する。第０リンク
座標系Σ0 で表された第０リンクの重心をベクトル 0Ｐ
G0で表すと、このベクトル 0Ｐ G0と、作業座標系Σw ，
第０リンク座標系Σ0 間の姿勢変換行列を表す行列 WＲ
0 とを用いることにより、作業座標系Σw で表された第
０リンク座標系Σ0 の原点より第０リンクまでの位置ベ
クトルは、下記(8) 式にて表される。
【００２７】
【数８】
【００２８】作業座標系Σw で表された第０リンクの重
心の加速度ベクトルは、前記(8) 式の位置ベクトルを用
いて下記(9) 式にて表される。
心の加速度ベクトルは、前記(8) 式の位置ベクトルを用
いて下記(9) 式にて表される。
【００２９】
【数９】
【００３０】但し、前記(9) 式の右辺の変数の夫々は、
下記(10)〜(12)式と、前記(2),(6),(7) 式とを用いるこ
とにより計算される。
下記(10)〜(12)式と、前記(2),(6),(7) 式とを用いるこ
とにより計算される。
【００３１】
【数１０】
【００３２】
【数１１】
【００３３】
【数１２】
【００３４】次に、マニピュレータ２の運動学について
説明する。以下の説明においては、マニピュレータ２の
各リンク22,22,…の、作業座標系Σw に対する角速度，
角加速度及び重心の加速度を漸化式にて表すことを目的
とする。
説明する。以下の説明においては、マニピュレータ２の
各リンク22,22,…の、作業座標系Σw に対する角速度，
角加速度及び重心の加速度を漸化式にて表すことを目的
とする。
【００３５】図３はマニピュレータ２の運動学を説明す
るためのリンク22,22,…の模式的拡大図である。以下の
説明においては、図３に示すように第ｉリンクに第ｉリ
ンク座標系Σi を固定し、第ｉ関節が第ｉ−１リンク座
標系Σi-1 のｚ軸（ベクトル Wｚi-1 ）回りに回転する
ものとする。
るためのリンク22,22,…の模式的拡大図である。以下の
説明においては、図３に示すように第ｉリンクに第ｉリ
ンク座標系Σi を固定し、第ｉ関節が第ｉ−１リンク座
標系Σi-1 のｚ軸（ベクトル Wｚi-1 ）回りに回転する
ものとする。
【００３６】第ｉリンクの関節変数をｑi （ｉ＝1,2,3
…n)で表すと、作業座標系Σw で表された第ｉリンクの
角速度を表すベクトル Wωi は、下記(13)式にて表され
る。
…n)で表すと、作業座標系Σw で表された第ｉリンクの
角速度を表すベクトル Wωi は、下記(13)式にて表され
る。
【００３７】
【数１３】
【００３８】また、前記(13)式を微分すると、作業座標
系Σw にて表された第ｉリンクの角加速度を表すベクト
ルが下記(14)式の如く得られる。
系Σw にて表された第ｉリンクの角加速度を表すベクト
ルが下記(14)式の如く得られる。
【００３９】
【数１４】
【００４０】但し、前記(14)式は、下記(15)〜(19)式の
関係を用いて求められるものである。
関係を用いて求められるものである。
【００４１】
【数１５】
【００４２】
【数１６】
【００４３】
【数１７】
【００４４】
【数１８】
【００４５】
【数１９】
【００４６】次に、第ｉリンクの重心の加速度を求める
ために、まず、作業座標系Σw にて表された第ｉリンク
座標系Σi の原点の位置を表すベクトルと、その原点の
速度を表すベクトルと、その原点の加速度を表すベクト
ルとを求める。原点の位置を表すベクトルは下記(20)式
の如く表され、原点の速度を表すベクトルは下記(21)式
の如く表され、原点の加速度を表すベクトルは下記(22)
式の如く表される。
ために、まず、作業座標系Σw にて表された第ｉリンク
座標系Σi の原点の位置を表すベクトルと、その原点の
速度を表すベクトルと、その原点の加速度を表すベクト
ルとを求める。原点の位置を表すベクトルは下記(20)式
の如く表され、原点の速度を表すベクトルは下記(21)式
の如く表され、原点の加速度を表すベクトルは下記(22)
式の如く表される。
【００４７】
【数２０】
【００４８】
【数２１】
【００４９】
【数２２】
【００５０】ここで、前記(21),(22) 式に含まれる、移
動座標系ΣV にて表された第ｉリンク座標系Σi の原点
の速度を表すベクトルと、移動座標系ΣV にて表された
第ｉリンク座標系Σi の原点の加速度を表すベクトルと
は、次のように求められる。第ｉリンク座標系Σi で表
された第ｉ−１リンク座標系Σi-1 の原点より第ｉリン
ク座標系Σi の原点までの位置ベクトルをベクトル iＰ
i-1,1 とすると、第ｉリンク座標系Σi は第ｉリンクに
固定されているので、ベクトル iＰi-1,1 は定数ベクト
ルとなる。従って、移動座標系ΣV にて表された第ｉリ
ンク座標系Σiの原点の速度を表すベクトルは下記(23)
式の如く表され、また、移動座標系ΣVにて表された第
ｉリンク座標系Σi の原点の加速度を表すベクトルは下
記(24)式の如く表される。
動座標系ΣV にて表された第ｉリンク座標系Σi の原点
の速度を表すベクトルと、移動座標系ΣV にて表された
第ｉリンク座標系Σi の原点の加速度を表すベクトルと
は、次のように求められる。第ｉリンク座標系Σi で表
された第ｉ−１リンク座標系Σi-1 の原点より第ｉリン
ク座標系Σi の原点までの位置ベクトルをベクトル iＰ
i-1,1 とすると、第ｉリンク座標系Σi は第ｉリンクに
固定されているので、ベクトル iＰi-1,1 は定数ベクト
ルとなる。従って、移動座標系ΣV にて表された第ｉリ
ンク座標系Σiの原点の速度を表すベクトルは下記(23)
式の如く表され、また、移動座標系ΣVにて表された第
ｉリンク座標系Σi の原点の加速度を表すベクトルは下
記(24)式の如く表される。
【００５１】
【数２３】
【００５２】
【数２４】
【００５３】また、移動座標系ΣV にて表された第ｉリ
ンク座標系Σi の原点の角速度は下記(25)式の如く表さ
れる。
ンク座標系Σi の原点の角速度は下記(25)式の如く表さ
れる。
【００５４】
【数２５】
【００５５】次に、作業座標系Σw にて表された、第ｉ
リンク座標系Σi の原点より第ｉリンクの重心の位置ま
での位置ベクトルを下記(26)式の如く表すと、作業座標
系Σ w にて表された第ｉリンクの重心の加速度は、前記
(13),(14),(22)を用いて下記(27)式の如く表される。
リンク座標系Σi の原点より第ｉリンクの重心の位置ま
での位置ベクトルを下記(26)式の如く表すと、作業座標
系Σ w にて表された第ｉリンクの重心の加速度は、前記
(13),(14),(22)を用いて下記(27)式の如く表される。
【００５６】
【数２６】
【００５７】
【数２７】
【００５８】次に、マニピュレータ２の逆動力学計算に
ついて説明する。
ついて説明する。
【００５９】図４はマニピュレータ２の逆動力学を説明
するためのリンク22,22,…の模式的拡大図である。ま
ず、第ｉリンクの重心の運動について考える。前記(27)
式にて表される如き並進加速度で運動するために第ｉリ
ンクの重心に加えられるべき力は下記(28)式の如く表さ
れ、また、前記(14)式にて表される如き角加速度及び前
記(13)式にて表される如き角速度で運動するために第ｉ
リンクの重心に加えられるべきモーメントは下記(29)式
の如く表される。
するためのリンク22,22,…の模式的拡大図である。ま
ず、第ｉリンクの重心の運動について考える。前記(27)
式にて表される如き並進加速度で運動するために第ｉリ
ンクの重心に加えられるべき力は下記(28)式の如く表さ
れ、また、前記(14)式にて表される如き角加速度及び前
記(13)式にて表される如き角速度で運動するために第ｉ
リンクの重心に加えられるべきモーメントは下記(29)式
の如く表される。
【００６０】
【数２８】
【００６１】
【数２９】
【００６２】第ｉリンクにおける力の釣り合いは、下記
(30)式の関係を用い、前記(28)式にて表される力と、第
ｉ−１リンクより第ｉリンクに働く力とに関連して下記
(31)式の如く表される。
(30)式の関係を用い、前記(28)式にて表される力と、第
ｉ−１リンクより第ｉリンクに働く力とに関連して下記
(31)式の如く表される。
【００６３】
【数３０】
【００６４】
【数３１】
【００６５】また、第ｉリンクにおけるトルクの釣り合
いは、前記(30)式の関係を用い、前記(29)式にて表され
るトルクと、第ｉ−１リンクより第ｉリンクに働くトル
クとに関連して下記(32)式の如く表される。
いは、前記(30)式の関係を用い、前記(29)式にて表され
るトルクと、第ｉ−１リンクより第ｉリンクに働くトル
クとに関連して下記(32)式の如く表される。
【００６６】
【数３２】
【００６７】前記(28),(29) 式を前記(31),(32) 式に代
入すると、漸化式で表された第ｉリンクにおける力の釣
り合いが下記(33)式の如く表されると共に漸化式で表さ
れた第ｉリンクにおけるトルクの釣り合いが下記(34)式
の如く表される。
入すると、漸化式で表された第ｉリンクにおける力の釣
り合いが下記(33)式の如く表されると共に漸化式で表さ
れた第ｉリンクにおけるトルクの釣り合いが下記(34)式
の如く表される。
【００６８】
【数３３】
【００６９】
【数３４】
【００７０】また、第ｉ関節の発生すべき駆動トルクτ
i は、アクチュエータの粘性抵抗をＣi 、慣性モーメン
トをＩaiとすると、下記(35)式の如く表される。
i は、アクチュエータの粘性抵抗をＣi 、慣性モーメン
トをＩaiとすると、下記(35)式の如く表される。
【００７１】
【数３５】
【００７２】前記(34),(35) 式より、マニピュレータ２
の希望の運動を実現するトルクが求められる。前記(34)
式を前記(35)式に代入した式は、第ｉリンクの運動方程
式でもある。ｉ＝1,2,…,nであり、手先効果器20が外力
を加えられていないとすると、下記(36)式の関係が成り
立つ。
の希望の運動を実現するトルクが求められる。前記(34)
式を前記(35)式に代入した式は、第ｉリンクの運動方程
式でもある。ｉ＝1,2,…,nであり、手先効果器20が外力
を加えられていないとすると、下記(36)式の関係が成り
立つ。
【００７３】
【数３６】
【００７４】次に、台車１の逆動力学計算について説明
する。
する。
【００７５】図５は台車１の逆動力学を説明するための
リンクの模式的拡大図である。移動座標系ΣV の原点に
かかる力は、前記(33)式においてｉ＝０と置き換えるこ
とにより下記(37)式の如く表され、また、移動座標系Σ
V の原点にかかるトルクは、前記(34)式においてｉ＝０
と置き換えることにより下記(38)式の如く表される。
リンクの模式的拡大図である。移動座標系ΣV の原点に
かかる力は、前記(33)式においてｉ＝０と置き換えるこ
とにより下記(37)式の如く表され、また、移動座標系Σ
V の原点にかかるトルクは、前記(34)式においてｉ＝０
と置き換えることにより下記(38)式の如く表される。
【００７６】
【数３７】
【００７７】
【数３８】
【００７８】台車１は、移動座標系ΣV におけるｙ軸方
向（ベクトル WｙV 方向）の並進運動と、移動座標系Σ
V におけるｚ軸回り（ベクトル WｚV 回り）の回転運動
との２つの自由度しか持たないことと、この２つの自由
度に対する台車１の駆動力及び旋回トルクは、左右の駆
動輪11,11 が発生すべき駆動トルクの和及び差より求め
られることとを考慮すれば、これらのトルクと、作業座
標系Σw にて表された移動座標系ΣV の原点にかかる力
との間には下記(39)式に示される如き関係が成り立ち、
また、これらのトルクと、作業座標系Σw にて表された
移動座標系ΣVの原点にかかるトルクとの間には下記(4
0)式に示される如き関係が成り立つ。
向（ベクトル WｙV 方向）の並進運動と、移動座標系Σ
V におけるｚ軸回り（ベクトル WｚV 回り）の回転運動
との２つの自由度しか持たないことと、この２つの自由
度に対する台車１の駆動力及び旋回トルクは、左右の駆
動輪11,11 が発生すべき駆動トルクの和及び差より求め
られることとを考慮すれば、これらのトルクと、作業座
標系Σw にて表された移動座標系ΣV の原点にかかる力
との間には下記(39)式に示される如き関係が成り立ち、
また、これらのトルクと、作業座標系Σw にて表された
移動座標系ΣVの原点にかかるトルクとの間には下記(4
0)式に示される如き関係が成り立つ。
【００７９】
【数３９】
【００８０】
【数４０】
【００８１】前記(39),(40) 式におけるハットτR 及び
ハットτL は、(39),(40) 式の連立方程式の解として得
られるようになっている。
ハットτL は、(39),(40) 式の連立方程式の解として得
られるようになっている。
【００８２】左駆動輪11の駆動系の粘性抵抗をＣL ，右
駆動輪11の駆動系の粘性抵抗をＣRとし、左駆動輪11の
慣性モーメントをＩaL，右駆動輪11の慣性モーメントを
ＩaRとすると、左駆動輪11駆動用のモータの発生すべき
トルクτL は下記(41)式の如く表され、また、右駆動輪
11駆動用のモータの発生すべきトルクτR は下記(42)式
の如く表される。
駆動輪11の駆動系の粘性抵抗をＣRとし、左駆動輪11の
慣性モーメントをＩaL，右駆動輪11の慣性モーメントを
ＩaRとすると、左駆動輪11駆動用のモータの発生すべき
トルクτL は下記(41)式の如く表され、また、右駆動輪
11駆動用のモータの発生すべきトルクτR は下記(42)式
の如く表される。
【００８３】
【数４１】
【００８４】
【数４２】
【００８５】前記(41),(42) 式におけるハットτR ，ハ
ットτL は、ベクトル Wｆ0 ，ベクトル Wｎ0 より求め
られたものであり、ｖV ，ダッシュｖV ，ωV ，ダッシ
ュω V を変数として含んでいる。これらの変数は、前記
(4) 式及びその微分値（両辺を微分して得られる関係
式）を用いて左駆動輪の回転角速度，左駆動輪の回転角
加速度，右駆動輪の回転角速度，右駆動輪の回転角加速
度に変換することができるので、前記(41),(42) 式は入
力トルクと、モータ回転角との関係を表す運動方程式と
も考えられる。
ットτL は、ベクトル Wｆ0 ，ベクトル Wｎ0 より求め
られたものであり、ｖV ，ダッシュｖV ，ωV ，ダッシ
ュω V を変数として含んでいる。これらの変数は、前記
(4) 式及びその微分値（両辺を微分して得られる関係
式）を用いて左駆動輪の回転角速度，左駆動輪の回転角
加速度，右駆動輪の回転角速度，右駆動輪の回転角加速
度に変換することができるので、前記(41),(42) 式は入
力トルクと、モータ回転角との関係を表す運動方程式と
も考えられる。
【００８６】次に、前述の如きマニピュレータ２の位置
・姿勢制御を実施するための制御系の構成について説明
する。図５はマニピュレータ２の位置・姿勢制御を行う
制御系の模式的ブロック図である。
・姿勢制御を実施するための制御系の構成について説明
する。図５はマニピュレータ２の位置・姿勢制御を行う
制御系の模式的ブロック図である。
【００８７】図中３は台車１及びマニピュレータ２の慣
性を表す慣性要素であり、該慣性要素３には、マニピュ
レータ２の関節21,21,…の角加速度の目標値と、台車１
の旋回角加速度の目標値と、台車１の進行加速度の目標
値とが与えられるようになっており、慣性要素３は、こ
れらの値に応じた慣性に基づくトルクを加え合わせ点４
へ与えるようになっている。また、図中５は台車１及び
マニピュレータ２のダイナミクスであり、該ダイナミク
ス５の出力であるマニピュレータ２の関節21,21,…の角
速度及び角度の観測値と、台車１の旋回角速度及び進行
速度の観測値とが、ダイナミクス５への入力トルクを補
償するための補償要素６へ与えられるようになってい
る。この補償要素６は、前述した如き方法により、前記
観測値に基づいて台車１とマニピュレータ２との間の運
動の非干渉化を実現するトルクを求め、そのトルクを前
記加え合わせ点４へ与えるようになっている。加え合わ
せ点４では、慣性要素３からのトルクと、補償要素６か
らのトルクとが加え合わせられ、その加え合わされたト
ルクがダイナミクス５へ与えられるようになっている。
性を表す慣性要素であり、該慣性要素３には、マニピュ
レータ２の関節21,21,…の角加速度の目標値と、台車１
の旋回角加速度の目標値と、台車１の進行加速度の目標
値とが与えられるようになっており、慣性要素３は、こ
れらの値に応じた慣性に基づくトルクを加え合わせ点４
へ与えるようになっている。また、図中５は台車１及び
マニピュレータ２のダイナミクスであり、該ダイナミク
ス５の出力であるマニピュレータ２の関節21,21,…の角
速度及び角度の観測値と、台車１の旋回角速度及び進行
速度の観測値とが、ダイナミクス５への入力トルクを補
償するための補償要素６へ与えられるようになってい
る。この補償要素６は、前述した如き方法により、前記
観測値に基づいて台車１とマニピュレータ２との間の運
動の非干渉化を実現するトルクを求め、そのトルクを前
記加え合わせ点４へ与えるようになっている。加え合わ
せ点４では、慣性要素３からのトルクと、補償要素６か
らのトルクとが加え合わせられ、その加え合わされたト
ルクがダイナミクス５へ与えられるようになっている。
【００８８】以上のような構成の制御系によりマニピュ
レータ２の位置・姿勢制御を行うと、各駆動輪11,11 の
トルクは、各関節21,21,…のトルクを考慮して求められ
たものであるため、求めた各駆動輪11,11 のトルクを各
駆動輪11,11 に発生することにより、このトルクは、マ
ニピュレータ２の運動によって各駆動輪11,11 に作用す
る運動の反力を抑止する働きをする。また、逆にこのよ
うにして求めた各関節21,21,…のトルクは、各駆動輪1
1,11 のトルクを考慮して求められるものであるため、
求めた各関節21,21,…のトルクを各関節に発生させる
と、このトルクは、台車１の運動によってマニピュレー
タ２に作用する運動の反力を抑止する働きをする。この
ような働きによって、台車１とマニピュレータ２との間
の運動の非干渉化が実現される。
レータ２の位置・姿勢制御を行うと、各駆動輪11,11 の
トルクは、各関節21,21,…のトルクを考慮して求められ
たものであるため、求めた各駆動輪11,11 のトルクを各
駆動輪11,11 に発生することにより、このトルクは、マ
ニピュレータ２の運動によって各駆動輪11,11 に作用す
る運動の反力を抑止する働きをする。また、逆にこのよ
うにして求めた各関節21,21,…のトルクは、各駆動輪1
1,11 のトルクを考慮して求められるものであるため、
求めた各関節21,21,…のトルクを各関節に発生させる
と、このトルクは、台車１の運動によってマニピュレー
タ２に作用する運動の反力を抑止する働きをする。この
ような働きによって、台車１とマニピュレータ２との間
の運動の非干渉化が実現される。
【００８９】次に、以上の如き本発明方法を用いて実際
に移動マニピュレータの位置・姿勢制御を行った場合の
結果について説明する。この制御においては、台車上に
１リンクのマニピュレータを備えた移動マニピュレータ
を用いて行ったものであり、その台車を直線走行させつ
つ、マニピュレータのリンクを、図６の如き角加速度パ
ターンにて運動させた。図６は実際の制御に用いたマニ
ピュレータのリンクの角加速度パターンを示すグラフで
あり、縦軸に角加速度、横軸に時間を夫々とり、これら
の関係を示したものである。
に移動マニピュレータの位置・姿勢制御を行った場合の
結果について説明する。この制御においては、台車上に
１リンクのマニピュレータを備えた移動マニピュレータ
を用いて行ったものであり、その台車を直線走行させつ
つ、マニピュレータのリンクを、図６の如き角加速度パ
ターンにて運動させた。図６は実際の制御に用いたマニ
ピュレータのリンクの角加速度パターンを示すグラフで
あり、縦軸に角加速度、横軸に時間を夫々とり、これら
の関係を示したものである。
【００９０】図６の如き角加速度パターンにてマニピュ
レータを運動させた場合の移動マニピュレータの走行軌
跡を図７に示す。図７において、時間ｔ＝０秒から時間
ｔ＝13秒まで直線的に延びた軌跡が台車の軌跡であり、
その台車の軌跡から横に突出しているのが、マニピュレ
ータの軌跡である。この図７にて明らかな如く本発明方
法を用いると、台車とマニピュレータとの間の運動の非
干渉化が実現できる。
レータを運動させた場合の移動マニピュレータの走行軌
跡を図７に示す。図７において、時間ｔ＝０秒から時間
ｔ＝13秒まで直線的に延びた軌跡が台車の軌跡であり、
その台車の軌跡から横に突出しているのが、マニピュレ
ータの軌跡である。この図７にて明らかな如く本発明方
法を用いると、台車とマニピュレータとの間の運動の非
干渉化が実現できる。
【００９１】また、本発明においては、運動方程式を漸
化式にて表し、その漸化式を用いてマニピュレータ２の
逆動力学計算を行うようになっているため、実際の制御
時における制御に関する計算を短時間で実行可能であ
る。
化式にて表し、その漸化式を用いてマニピュレータ２の
逆動力学計算を行うようになっているため、実際の制御
時における制御に関する計算を短時間で実行可能であ
る。
【００９２】
【発明の効果】以上詳述した如く、本発明に係る移動マ
ニピュレータの位置・姿勢制御方法においては、移動体
の各駆動輪のトルクが、マニピュレータの各関節のトル
クを考慮して求められるから、これらのトルクを各駆動
輪に発生させるべく移動体を制御することにより、マニ
ピュレータの運動によって移動体に作用する反力を抑止
でき、また、マニピュレータの各関節のトルクが、移動
体の各駆動輪のトルクを考慮して求められるから、これ
らのトルクを各関節に発生させるべくマニピュレータを
制御することにより、移動体の運動によってマニピュレ
ータに作用する反力を抑止できる。このように、移動体
の運動と、マニピュレータの運動との間の干渉が抑制さ
れて、マニピュレータの手先効果器に対し、精度が高い
位置・姿勢制御を行うことが可能となる等、本発明は優
れた効果を奏する。
ニピュレータの位置・姿勢制御方法においては、移動体
の各駆動輪のトルクが、マニピュレータの各関節のトル
クを考慮して求められるから、これらのトルクを各駆動
輪に発生させるべく移動体を制御することにより、マニ
ピュレータの運動によって移動体に作用する反力を抑止
でき、また、マニピュレータの各関節のトルクが、移動
体の各駆動輪のトルクを考慮して求められるから、これ
らのトルクを各関節に発生させるべくマニピュレータを
制御することにより、移動体の運動によってマニピュレ
ータに作用する反力を抑止できる。このように、移動体
の運動と、マニピュレータの運動との間の干渉が抑制さ
れて、マニピュレータの手先効果器に対し、精度が高い
位置・姿勢制御を行うことが可能となる等、本発明は優
れた効果を奏する。
【図１】本発明に係る移動マニピュレータの位置・姿勢
制御方法の実施に使用する移動マニピュレータの斜視図
である。
制御方法の実施に使用する移動マニピュレータの斜視図
である。
【図２】移動マニピュレータのフレームモデルを示す模
式図である。
式図である。
【図３】マニピュレータの運動学を説明するためのリン
クの模式的拡大図である。
クの模式的拡大図である。
【図４】マニピュレータの逆動力学を説明するためのリ
ンクの模式的拡大図である。
ンクの模式的拡大図である。
【図５】マニピュレータの位置・姿勢制御を行う制御系
の模式的ブロック図である。
の模式的ブロック図である。
【図６】実際の制御に用いたマニピュレータのリンクの
角加速度パターンを示すグラフである。
角加速度パターンを示すグラフである。
【図７】図６の如き角加速度パターンにてマニピュレー
タを運動させた場合の移動マニピュレータの走行軌跡を
表す模式図である。
タを運動させた場合の移動マニピュレータの走行軌跡を
表す模式図である。
１ 台車 ２ マニピュレータ 11 駆動輪 21 関節 22 リンク
───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 平４−331084（ＪＰ，Ａ) 特開 平４−331083（ＪＰ，Ａ) (58)調査した分野(Int.Cl.7，ＤＢ名) B25J 5/00 B25J 9/18 B25J 13/00
Claims (1)
- 【請求項１】 左右一対の駆動輪の駆動によって任意の
方向に移動する移動体に、その各リンクが各別に駆動さ
れる多関節型のマニピュレータを備えてなる移動マニピ
ュレータを、前記移動体の位置及び姿勢を含めて決定さ
れた各別の目標角度に前記各リンクを制御することによ
り、前記マニピュレータの手先効果器を目標の位置及び
姿勢に制御する移動マニピュレータの位置・姿勢制御方
法において、 前記移動体の進行速度及び旋回角速度と、前記マニピュ
レータの各関節の関節角及び関節角速度とを検出し、 前記手先効果器の目標の位置及び姿勢に基づいて得られ
る前記移動体の進行及び旋回加速度の目標値、並びに前
記マニピュレータの各関節の目標角加速度と、検出した
移動体の進行速度及び旋回角速度、並びにマニピュレー
タの各関節の関節角及び関節角速度とに関連して、前記
目標の位置及び姿勢の実現のために、前記移動体の進行
速度及び旋回角速度を含めて前記マニピュレータの各リ
ンクに生ぜしめるべき速度，加速度，角速度及び角加速
度の目標値を、前記移動体への取付け側から先端側へ順
次求め、 求められた速度，加速度，角速度及び角加速度の目標値
にて前記マニピュレータの各リンクの夫々が運動するた
めに必要である各関節のトルクを、先端側から移動体へ
の取付け側へ順次求め、更に、前記運動のために必要な
前記移動体の各駆動輪のトルクを求めて、 夫々に対して求めたトルクが生じるように、前記各駆動
輪及び各関節を制御することを特徴とする移動マニピュ
レータの位置・姿勢制御方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP23485792A JP3223288B2 (ja) | 1992-09-02 | 1992-09-02 | 移動マニピュレータの位置・姿勢制御方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP23485792A JP3223288B2 (ja) | 1992-09-02 | 1992-09-02 | 移動マニピュレータの位置・姿勢制御方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH0679656A JPH0679656A (ja) | 1994-03-22 |
| JP3223288B2 true JP3223288B2 (ja) | 2001-10-29 |
Family
ID=16977441
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP23485792A Expired - Fee Related JP3223288B2 (ja) | 1992-09-02 | 1992-09-02 | 移動マニピュレータの位置・姿勢制御方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3223288B2 (ja) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4821865B2 (ja) * | 2009-02-18 | 2011-11-24 | ソニー株式会社 | ロボット装置及びその制御方法、並びにコンピューター・プログラム |
| JP5962659B2 (ja) | 2011-09-13 | 2016-08-03 | 株式会社安川電機 | 自走ロボットおよび自走台車 |
| DE102011113664A1 (de) * | 2011-09-20 | 2013-03-21 | Simaco GmbH | Verfahren und Vorrichtung zur Homogenisierung von Tinte für Inkjet-Geräte |
| JP5874618B2 (ja) * | 2012-12-05 | 2016-03-02 | トヨタ自動車株式会社 | 移動体装置 |
| CN112199827B (zh) * | 2020-09-24 | 2024-02-27 | 慧灵科技（深圳）有限公司 | 机械臂的动力学仿真方法、装置、电子设备及存储介质 |
| CN117325166A (zh) * | 2023-10-16 | 2024-01-02 | 北京理工大学前沿技术研究院 | 针对行进车辆的机械臂控制方法、系统、介质及设备 |
-
1992
- 1992-09-02 JP JP23485792A patent/JP3223288B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH0679656A (ja) | 1994-03-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP2769947B2 (ja) | マニピュレータの位置・姿勢制御方法 | |
| US6204619B1 (en) | Dynamic control algorithm and program for power-assisted lift device | |
| JP2006136962A (ja) | 移動ロボット | |
| Selekwa et al. | Path tracking control of four wheel independently steered ground robotic vehicles | |
| Utano et al. | Automatic control of bicycles with a balancer | |
| Lee et al. | Development, analysis, and control of series elastic actuator-driven robot leg | |
| Nagarajan et al. | Planning in high-dimensional shape space for a single-wheeled balancing mobile robot with arms | |
| Graettinger et al. | Evaluation and time-scaling of trajectories for wheeled mobile robots | |
| JPWO2007139135A1 (ja) | ロボット及び制御装置 | |
| JP3223288B2 (ja) | 移動マニピュレータの位置・姿勢制御方法 | |
| Ferland et al. | Taking your robot for a walk: Force-guiding a mobile robot using compliant arms | |
| Liu et al. | Active suspension control with consensus strategy for dynamic posture tracking of wheel-legged robotic systems on uneven surfaces | |
| Yoo et al. | A study on a mobile platform-manipulator welding system for horizontal fillet joints | |
| Tachi et al. | Impedance control of a direct-drive manipulator without using force sensors | |
| US5542028A (en) | Method of controlling position and attitude of working robot and its manipulator and apparatus thereof | |
| Staicu | Dynamics equations of a mobile robot provided with caster wheel | |
| JPS6077210A (ja) | 空間運動機構の制御方法 | |
| US5347616A (en) | Method of controlling position and attitude of working robot and its manipulator and apparatus thereof | |
| Winkler et al. | Force-guided motions of a 6-dof industrial robot with a joint space approach | |
| Luo et al. | On-line adaptive control for minimizing slippage error while mobile platform and manipulator operate simultaneously for robotics mobile manipulation | |
| Li et al. | Novel design and modeling of a mobile parallel manipulator | |
| JPH0445304B2 (ja) | ||
| JPS59189416A (ja) | 産業ロボツトの誘導教示方法 | |
| JPH07104725B2 (ja) | マニピュレータの駆動方式 | |
| JP3234928B2 (ja) | マニピュレータの位置・姿勢制御装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |