JP2769947B2 - マニピュレータの位置・姿勢制御方法 - Google Patents
マニピュレータの位置・姿勢制御方法Info
- Publication number
- JP2769947B2 JP2769947B2 JP4148654A JP14865492A JP2769947B2 JP 2769947 B2 JP2769947 B2 JP 2769947B2 JP 4148654 A JP4148654 A JP 4148654A JP 14865492 A JP14865492 A JP 14865492A JP 2769947 B2 JP2769947 B2 JP 2769947B2
- Authority
- JP
- Japan
- Prior art keywords
- posture
- manipulator
- hand
- control
- equation
- 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
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
- B25J19/021—Optical sensing devices
- B25J19/023—Optical sensing devices including video camera means
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
- B25J9/162—Mobile manipulator, movable base with manipulator arm mounted on it
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/37—Measurements
- G05B2219/37563—Ccd, tv camera
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/39—Robotics, robotics to robotics hand
- G05B2219/39394—Compensate hand position with camera detected deviation, new end effector attitude
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/39—Robotics, robotics to robotics hand
- G05B2219/39417—6-DOF
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/40—Robotics, robotics mapping to robotics vision
- G05B2219/40298—Manipulator on vehicle, wheels, mobile
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/40—Robotics, robotics mapping to robotics vision
- G05B2219/40613—Camera, laser scanner on end effector, hand eye manipulator, local
Description
【０００１】
【産業上の利用分野】本発明は、移動体に備えられ、種
々の動作を行うマニピュレータの位置及び姿勢を制御す
る方法に関する。
々の動作を行うマニピュレータの位置及び姿勢を制御す
る方法に関する。
【０００２】
【従来の技術】工場での生産性の向上及び安全性の確保
を実現するために、種々の作業用のロボットが開発され
ており、例えば、その先端に手先効果器を設けた多関節
型のマニピュレータを、自律走行する移動体である台車
に搭載した移動ロボットがある。このような移動ロボッ
トにおいては、前記台車の走行制御と、前記マニピュレ
ータの手先効果器の位置・姿勢制御とを行うことによっ
て、作業対象物に対してマニピュレータにて種々の作業
を行うものが考えられている。
を実現するために、種々の作業用のロボットが開発され
ており、例えば、その先端に手先効果器を設けた多関節
型のマニピュレータを、自律走行する移動体である台車
に搭載した移動ロボットがある。このような移動ロボッ
トにおいては、前記台車の走行制御と、前記マニピュレ
ータの手先効果器の位置・姿勢制御とを行うことによっ
て、作業対象物に対してマニピュレータにて種々の作業
を行うものが考えられている。
【０００３】また、このような移動ロボットでは、作業
対象物を撮像するためのCCD カメラ等のカメラを手先効
果器に取付け、該カメラで撮像した画像情報に基づいて
マニピュレータの手先効果器の位置・姿勢制御を行うも
のがある。
対象物を撮像するためのCCD カメラ等のカメラを手先効
果器に取付け、該カメラで撮像した画像情報に基づいて
マニピュレータの手先効果器の位置・姿勢制御を行うも
のがある。
【０００４】カメラの画像情報に基づいて手先効果器の
位置・姿勢制御を行う移動ロボットでは、次に示すよう
な方法でその制御を行っていた。まず、カメラにて作業
対象物を撮像し、その画像情報に基づいて作業座標系に
おける作業対象物の位置及び姿勢を求める。次いで、求
めた作業対象物の位置及び姿勢に基づいて、手先効果器
が作業対象物をつかむためのマニピュレータの位置及び
姿勢を実現する各関節の目標関節角度を求める。そし
て、各関節の関節角度が、求めた目標関節角度に一致す
るように関節角度の比例制御を行い、最終的に手先効果
器が作業対象物をつかむ動作を行うようになっていた。
位置・姿勢制御を行う移動ロボットでは、次に示すよう
な方法でその制御を行っていた。まず、カメラにて作業
対象物を撮像し、その画像情報に基づいて作業座標系に
おける作業対象物の位置及び姿勢を求める。次いで、求
めた作業対象物の位置及び姿勢に基づいて、手先効果器
が作業対象物をつかむためのマニピュレータの位置及び
姿勢を実現する各関節の目標関節角度を求める。そし
て、各関節の関節角度が、求めた目標関節角度に一致す
るように関節角度の比例制御を行い、最終的に手先効果
器が作業対象物をつかむ動作を行うようになっていた。
【０００５】
【発明が解決しようとする課題】前述の如きカメラで撮
像した画像情報に基づいて手先効果器の位置・姿勢制御
を行う従来の移動ロボットにおいては、作業対象物が動
かないという仮定のもとにマニピュレータの関節角度の
比例制御を行っているので、その比例制御中に作業対象
物と、台車とが相対移動する場合には、その移動に対応
するような目標関節角度の変更ができないため、作業対
象物をつかむことができない等、有効な位置・姿勢制御
が行えないという問題があった。
像した画像情報に基づいて手先効果器の位置・姿勢制御
を行う従来の移動ロボットにおいては、作業対象物が動
かないという仮定のもとにマニピュレータの関節角度の
比例制御を行っているので、その比例制御中に作業対象
物と、台車とが相対移動する場合には、その移動に対応
するような目標関節角度の変更ができないため、作業対
象物をつかむことができない等、有効な位置・姿勢制御
が行えないという問題があった。
【０００６】本発明は斯かる事情に鑑みてなされたもの
であり、相対移動する作業対象物に対しても有効な位置
・姿勢制御を行うことを可能とするリアルタイム制御系
を実現するためのマニピュレータの位置・姿勢制御方法
を提供することを目的とする。
であり、相対移動する作業対象物に対しても有効な位置
・姿勢制御を行うことを可能とするリアルタイム制御系
を実現するためのマニピュレータの位置・姿勢制御方法
を提供することを目的とする。
【０００７】
【課題を解決するための手段】第１の発明に係るマニピ
ュレータの位置・姿勢制御方法は、左，右に夫々独立に
駆動される車輪を備えた移動体に搭載され、マニピュレ
ータの手先の周りの所定範囲の画像を上方から撮像する
撮像手段を備えたマニピュレータにおける関節の角度を
制御することにより前記手先を目標の位置・姿勢に制御
する方法において、前記移動体の走行中に、手先の位置
・姿勢の目標値と、手先の現在の位置・姿勢と、移動体
の現在の位置・姿勢とに基づいて、マニピュレータの関
節の目標関節角を求め、該目標関節角と、別途求めたマ
ニピュレータの関節角の実測値と、前記撮像手段からリ
アルタイムに得られる画像情報に含まれている水平面内
での手先の位置・姿勢と作業対象物のそれとの偏差とに
基づいて水平面内での前記手先の位置・姿勢をリアルタ
イムで制御することを特徴とする。
ュレータの位置・姿勢制御方法は、左，右に夫々独立に
駆動される車輪を備えた移動体に搭載され、マニピュレ
ータの手先の周りの所定範囲の画像を上方から撮像する
撮像手段を備えたマニピュレータにおける関節の角度を
制御することにより前記手先を目標の位置・姿勢に制御
する方法において、前記移動体の走行中に、手先の位置
・姿勢の目標値と、手先の現在の位置・姿勢と、移動体
の現在の位置・姿勢とに基づいて、マニピュレータの関
節の目標関節角を求め、該目標関節角と、別途求めたマ
ニピュレータの関節角の実測値と、前記撮像手段からリ
アルタイムに得られる画像情報に含まれている水平面内
での手先の位置・姿勢と作業対象物のそれとの偏差とに
基づいて水平面内での前記手先の位置・姿勢をリアルタ
イムで制御することを特徴とする。
【０００８】第２の発明に係るマニピュレータの位置・
姿勢制御方法は、前記目標関節角と、関節角の実測値
と、手先の位置・姿勢と作業対象物のそれとの偏差とに
基づく制御は、下式で表わされる制御則に従って行われ
ることを特徴とする。
姿勢制御方法は、前記目標関節角と、関節角の実測値
と、手先の位置・姿勢と作業対象物のそれとの偏差とに
基づく制御は、下式で表わされる制御則に従って行われ
ることを特徴とする。
【０００９】
【数２】
【００１０】
【作用】第１の発明にあっては、撮像手段からリアルタ
イムで得られる画像情報に含まれる水平面内での手先の
位置・姿勢と作業対象物との位置偏差に基づく制御と、
関節の角度の目標値に基づく制御との２種類の制御を、
結合させることにより、手先の位置・姿勢の３次元的な
リアルタイムの制御が行われ、しかも画像情報には水平
面内での手先の位置・姿勢をリアルタイムで表す情報が
含まれているため相対移動する作業対象物に対しても同
じくリアルタイムでの効果的な手先の位置・姿勢制御が
行える。
イムで得られる画像情報に含まれる水平面内での手先の
位置・姿勢と作業対象物との位置偏差に基づく制御と、
関節の角度の目標値に基づく制御との２種類の制御を、
結合させることにより、手先の位置・姿勢の３次元的な
リアルタイムの制御が行われ、しかも画像情報には水平
面内での手先の位置・姿勢をリアルタイムで表す情報が
含まれているため相対移動する作業対象物に対しても同
じくリアルタイムでの効果的な手先の位置・姿勢制御が
行える。
【００１１】第２の発明にあっては、作業座標系の位置
（高さ）及び姿勢に関しては制御を行うが、平面上での
制御は行わず、一方、作業座標系での平面上での制御と
を分担して行うことにより不調和が解消される。
（高さ）及び姿勢に関しては制御を行うが、平面上での
制御は行わず、一方、作業座標系での平面上での制御と
を分担して行うことにより不調和が解消される。
【００１２】
【実施例】以下本発明をその実施例を示す図面に基づい
て具体的に説明する。図１は本発明に係るマニピュレー
タの位置・姿勢制御方法の実施に使用する移動ロボット
の斜視図である。
て具体的に説明する。図１は本発明に係るマニピュレー
タの位置・姿勢制御方法の実施に使用する移動ロボット
の斜視図である。
【００１３】図中１は自律走行を行う移動体である台車
であり、該台車１は、箱型の車体10を、左右一対の駆動
輪11（右駆動輪のみ図示）と、前後左右に配されたキャ
スタ12,12 （２つのみ図示）とにて支持してなる。前記
左駆動輪11及び右駆動輪11は各別のモータ（図示せず）
によって独立的に駆動されるようになっており、台車１
は運動方程式と拘束条件式とに支配されて運動する、所
謂非ホロノーム系である。台車１の車体10の上部には、
連結された複数の回転式の関節21,21,…を有し、その先
端部に手先効果器20を備えた多関節型のマニピュレータ
２が備えられている。前記関節21,21,…の夫々は、サー
ボモータ（図示せず）によって駆動されるようになって
いる。前記マニピュレータ２の先端部には、手先効果器
20の近傍を上方から撮像するカメラ（CCD カメラ）３が
取付けられている。
であり、該台車１は、箱型の車体10を、左右一対の駆動
輪11（右駆動輪のみ図示）と、前後左右に配されたキャ
スタ12,12 （２つのみ図示）とにて支持してなる。前記
左駆動輪11及び右駆動輪11は各別のモータ（図示せず）
によって独立的に駆動されるようになっており、台車１
は運動方程式と拘束条件式とに支配されて運動する、所
謂非ホロノーム系である。台車１の車体10の上部には、
連結された複数の回転式の関節21,21,…を有し、その先
端部に手先効果器20を備えた多関節型のマニピュレータ
２が備えられている。前記関節21,21,…の夫々は、サー
ボモータ（図示せず）によって駆動されるようになって
いる。前記マニピュレータ２の先端部には、手先効果器
20の近傍を上方から撮像するカメラ（CCD カメラ）３が
取付けられている。
【００１４】また、左駆動輪11及び右駆動輪11には夫
々、その回転に応じてパルス信号を発振するパスジェネ
レータよりなる走行速度検出器（図示せず）が設けられ
ている。そして、前記マニピュレータ２の各関節21,21,
…には、その関節角度を検出する角度検出器（図示せ
ず）が設けられている。
々、その回転に応じてパルス信号を発振するパスジェネ
レータよりなる走行速度検出器（図示せず）が設けられ
ている。そして、前記マニピュレータ２の各関節21,21,
…には、その関節角度を検出する角度検出器（図示せ
ず）が設けられている。
【００１５】また、図中、ΣS は台車１及びマニピュレ
ータ２と無関係な任意の点（例えば床面の任意の点）に
設定された絶対座標系、ΣH はマニピュレータ２の手先
効果器20の位置・姿勢を表現するために手先効果器20に
設定された手先座標系、Σvは台車１に設定された移動
座標系、Σw は作業対象物４の位置・姿勢を表現するた
めに作業対象物４に設定された作業座標系である。な
お、以下の説明は、マニピュレータ２が６自由度（６関
節）を有するものと仮定して行う。
ータ２と無関係な任意の点（例えば床面の任意の点）に
設定された絶対座標系、ΣH はマニピュレータ２の手先
効果器20の位置・姿勢を表現するために手先効果器20に
設定された手先座標系、Σvは台車１に設定された移動
座標系、Σw は作業対象物４の位置・姿勢を表現するた
めに作業対象物４に設定された作業座標系である。な
お、以下の説明は、マニピュレータ２が６自由度（６関
節）を有するものと仮定して行う。
【００１６】まず、マニピュレータ２の各関節21,21,…
の微小回転と、移動座標系Σv から見た手先効果器20の
微小並進，微小回転との関係について説明する。
の微小回転と、移動座標系Σv から見た手先効果器20の
微小並進，微小回転との関係について説明する。
【００１７】マニピュレータ２の各関節21,21,…が微小
回転したときに、マニピュレータ２の手先効果器20の位
置・姿勢が微小並進，微小回転したとする。そのときの
マニピュレータ２の各関節21,21,…の微小回転角を表す
行列は下記(1) 式で表され、また、そのときの手先効果
器20の微小並進量は、移動座標系Σv から見た微小並進
ベクトルとして下記(2) 式で表され、さらに、そのとき
の手先効果器20の微小回転は、移動座標系Σv から見た
微小回転ベクトルとして下記(3) 式で表される。
回転したときに、マニピュレータ２の手先効果器20の位
置・姿勢が微小並進，微小回転したとする。そのときの
マニピュレータ２の各関節21,21,…の微小回転角を表す
行列は下記(1) 式で表され、また、そのときの手先効果
器20の微小並進量は、移動座標系Σv から見た微小並進
ベクトルとして下記(2) 式で表され、さらに、そのとき
の手先効果器20の微小回転は、移動座標系Σv から見た
微小回転ベクトルとして下記(3) 式で表される。
【００１８】
【数３】
【００１９】
【数４】
【００２０】
【数５】
【００２１】また、マニピュレータ２の各関節21,21,…
の関節角を表す行列ｑ＝［ｑ1 ，ｑ2 ，ｑ3 ，ｑ4 ，ｑ
5 ，ｑ6 ］T と、移動座標系Σv から見た手先効果器20
の位置・姿勢との関係は下記(4) 式で表される。
の関節角を表す行列ｑ＝［ｑ1 ，ｑ2 ，ｑ3 ，ｑ4 ，ｑ
5 ，ｑ6 ］T と、移動座標系Σv から見た手先効果器20
の位置・姿勢との関係は下記(4) 式で表される。
【００２２】
【数６】
【００２３】この(4) 式の両辺を微分すると、手先効果
器20の位置・姿勢の時間微分値と、関節角の時間微分値
との関係は、ヤコビ行列を用いた下記(5) 式の如き関係
式で表される。
器20の位置・姿勢の時間微分値と、関節角の時間微分値
との関係は、ヤコビ行列を用いた下記(5) 式の如き関係
式で表される。
【００２４】
【数７】
【００２５】また、前記(5) 式の関係式は、前記(1),
(3) 式を用いると、下記(6) 式の如き行列式で表すこと
ができる。
(3) 式を用いると、下記(6) 式の如き行列式で表すこと
ができる。
【００２６】
【数８】
【００２７】次に、マニピュレータ２の静力学関係につ
いて説明する。図２はマニピュレータ２の静力学関係を
説明するための説明図である。図２においては、手先効
果器20に、その上方から下方へ向かって作用する外力Ｐ
と、その旋回方向に作用する外力モーメントＭとが生じ
た状態を表しており、マニピュレータ２の形状は簡略化
して表してある。
いて説明する。図２はマニピュレータ２の静力学関係を
説明するための説明図である。図２においては、手先効
果器20に、その上方から下方へ向かって作用する外力Ｐ
と、その旋回方向に作用する外力モーメントＭとが生じ
た状態を表しており、マニピュレータ２の形状は簡略化
して表してある。
【００２８】図２に示される如くマニピュレータ２に対
して作用する外力Ｐは下記(7) 式の行列で表され、ま
た、外力モーメントＭは下記(8) 式の行列で表される。
して作用する外力Ｐは下記(7) 式の行列で表され、ま
た、外力モーメントＭは下記(8) 式の行列で表される。
【００２９】
【数９】
【００３０】
【数１０】
【００３１】このとき、マニピュレータ２の各関節21,2
1,…で発生するトルクを表す行列τ＝［τ1 ，τ2 ，τ
3 ，τ4 ，τ5 ，τ6 ］T と、マニピュレータ２の各関
節21,21,…の関節角を表す行列ｑ＝［ｑ1 ，ｑ2 ，
ｑ3 ，ｑ4 ，ｑ5 ，ｑ6 ］T との関係は、以下に説明す
るように仮想仕事の原理を利用して表すことができる。
1,…で発生するトルクを表す行列τ＝［τ1 ，τ2 ，τ
3 ，τ4 ，τ5 ，τ6 ］T と、マニピュレータ２の各関
節21,21,…の関節角を表す行列ｑ＝［ｑ1 ，ｑ2 ，
ｑ3 ，ｑ4 ，ｑ5 ，ｑ6 ］T との関係は、以下に説明す
るように仮想仕事の原理を利用して表すことができる。
【００３２】外力Ｐ及び外力モーメントＭによりマニピ
ュレータ２に加えられた仕事Ｗ1 は、外力Ｐ及び外力モ
ーメントＭによる手先効果器20の位置・姿勢の変位（移
動座標系から見た変位）を表すベクトルを下記(9) 式の
如くすると、下記(10)式の如く表される。
ュレータ２に加えられた仕事Ｗ1 は、外力Ｐ及び外力モ
ーメントＭによる手先効果器20の位置・姿勢の変位（移
動座標系から見た変位）を表すベクトルを下記(9) 式の
如くすると、下記(10)式の如く表される。
【００３３】
【数１１】
【００３４】
【数１２】
【００３５】また、そのときにこれらの外力及びトルク
により関節角がｄｑだけ回転したとすると、マニピュレ
ータ２の内部でなされた仕事Ｗ2 は下記(11)式の如く表
される。
により関節角がｄｑだけ回転したとすると、マニピュレ
ータ２の内部でなされた仕事Ｗ2 は下記(11)式の如く表
される。
【００３６】
【数１３】
【００３７】静力学においては、前記(10)式で表される
仕事Ｗ1 と、前記(11)式で表される仕事Ｗ2 とが等しい
ので、前記(10),(11) 式より、外力Ｐ及び外力モーメン
トＭによりマニピュレータ２に加えられた仕事と、その
ときにマニピュレータ２が発生すべき仕事との関係は、
下記(12)式の如き関係となる。
仕事Ｗ1 と、前記(11)式で表される仕事Ｗ2 とが等しい
ので、前記(10),(11) 式より、外力Ｐ及び外力モーメン
トＭによりマニピュレータ２に加えられた仕事と、その
ときにマニピュレータ２が発生すべき仕事との関係は、
下記(12)式の如き関係となる。
【００３８】
【数１４】
【００３９】また、この(12)式は、前記(6) 式を用いて
下記(13)式の如く表すことができる。
下記(13)式の如く表すことができる。
【００４０】
【数１５】
【００４１】この(13)式を式変形すると、手先効果器20
に外力Ｐ及び外力モーメントＭが作用するときの各関節
21,21,…で発生するトルクは、ヤコビ行列の転置行列を
用いて下記(14)式の如く表すことができる。
に外力Ｐ及び外力モーメントＭが作用するときの各関節
21,21,…で発生するトルクは、ヤコビ行列の転置行列を
用いて下記(14)式の如く表すことができる。
【００４２】
【数１６】
【００４３】次に、水平面（以下ｘ−ｙ平面という）内
における外力Ｐxyの作用について説明する。図３はｘ−
ｙ平面内における外力Ｐxyの作用を説明するための説明
図である。図３においては、手先効果器20を、ｘ−ｙ平
面内に作用する外力Ｐxyにて所定方向に引っ張った場合
にその外力Ｐxyをｘ軸方向の外力Ｐx と、ｙ軸方向の外
力Ｐy とに分解した状態を表してある。
における外力Ｐxyの作用について説明する。図３はｘ−
ｙ平面内における外力Ｐxyの作用を説明するための説明
図である。図３においては、手先効果器20を、ｘ−ｙ平
面内に作用する外力Ｐxyにて所定方向に引っ張った場合
にその外力Ｐxyをｘ軸方向の外力Ｐx と、ｙ軸方向の外
力Ｐy とに分解した状態を表してある。
【００４４】手先効果器20が外力Ｐxyにて引っ張られた
場合、その外力Ｐxyは、図３に示される如く、力の分解
によってｘ軸方向の外力Ｐx と、ｙ軸方向の外力Ｐy と
に分解できる。ここで、外力Ｐx ，Ｐy がバネの伸びに
よって発生したものであると仮定すると、外力Ｐxyは、
前記バネのｘ軸方向の伸びδｘ，前記バネのｙ軸方向の
伸びδｙ及び前記バネのバネ定数ｋによって、下記(15)
式の如く表すことができる。
場合、その外力Ｐxyは、図３に示される如く、力の分解
によってｘ軸方向の外力Ｐx と、ｙ軸方向の外力Ｐy と
に分解できる。ここで、外力Ｐx ，Ｐy がバネの伸びに
よって発生したものであると仮定すると、外力Ｐxyは、
前記バネのｘ軸方向の伸びδｘ，前記バネのｙ軸方向の
伸びδｙ及び前記バネのバネ定数ｋによって、下記(15)
式の如く表すことができる。
【００４５】
【数１７】
【００４６】このように表される外力Ｐxyを前記(15)式
に代入することによって求められるトルクを表す行列τ
は、外力Ｐxyを発生させるためにマニピュレータ２の各
関節21,21,…に発生すべきトルクである。
に代入することによって求められるトルクを表す行列τ
は、外力Ｐxyを発生させるためにマニピュレータ２の各
関節21,21,…に発生すべきトルクである。
【００４７】次に、カメラ３によるマニピュレータ２の
手先効果器20の位置制御方法について説明する。
手先効果器20の位置制御方法について説明する。
【００４８】まず、カメラ３にて撮像される画像の座標
系（以下カメラ座標系ΣC という）について説明する。
図４はカメラ座標系ΣC を説明するための説明図であ
る。図中５はカメラ３にて撮像される画像を表したモニ
タ画面であり、モニタ画面５上にカメラ座標系ΣC が固
定されているものとする。このようなカメラ座標系ΣC
では、カメラ座標系ΣC から見た作業座標系Σw （作業
対象物４）の位置が転置行列（δｘ，δｙ）T にて表さ
れる。
系（以下カメラ座標系ΣC という）について説明する。
図４はカメラ座標系ΣC を説明するための説明図であ
る。図中５はカメラ３にて撮像される画像を表したモニ
タ画面であり、モニタ画面５上にカメラ座標系ΣC が固
定されているものとする。このようなカメラ座標系ΣC
では、カメラ座標系ΣC から見た作業座標系Σw （作業
対象物４）の位置が転置行列（δｘ，δｙ）T にて表さ
れる。
【００４９】図５は手先座標系ΣH の座標軸とカメラ座
標系ΣC の座標軸とが一致する場合のカメラ座標系ΣC
を説明するための説明図である。手先座標系ΣH の座標
軸とカメラ座標系ΣC の座標軸とが一致するようにカメ
ラ３が取付けられている場合、手先効果器20と作業対象
物４との位置変位は、カメラ座標系ΣC から見た作業座
標系Σw （作業対象物４）の位置（δｘ，δｙ）T その
ものであると言える。このδｘ，δｙを前記(15)式に代
入することによって求められる外力Ｐxyは、図５中に示
される如き方向に作用する力であり、この外力Ｐxyを前
記(14)式に代入することによって求められるトルクを表
す行列τは、手先効果器20を、Ｐxyに相当する力で作業
対象物４へ近づけるためにマニピュレータ２の各関節2
1,21,…に発生すべきトルクである。
標系ΣC の座標軸とが一致する場合のカメラ座標系ΣC
を説明するための説明図である。手先座標系ΣH の座標
軸とカメラ座標系ΣC の座標軸とが一致するようにカメ
ラ３が取付けられている場合、手先効果器20と作業対象
物４との位置変位は、カメラ座標系ΣC から見た作業座
標系Σw （作業対象物４）の位置（δｘ，δｙ）T その
ものであると言える。このδｘ，δｙを前記(15)式に代
入することによって求められる外力Ｐxyは、図５中に示
される如き方向に作用する力であり、この外力Ｐxyを前
記(14)式に代入することによって求められるトルクを表
す行列τは、手先効果器20を、Ｐxyに相当する力で作業
対象物４へ近づけるためにマニピュレータ２の各関節2
1,21,…に発生すべきトルクである。
【００５０】もし、カメラ３によって、δｘ，δｙのみ
ならず、目標の位置・姿勢に対する任意の位置・姿勢の
偏差が測定可能であるならば、以上説明した如き原理に
基づいて手先効果器20の位置・姿勢を目標の位置・姿勢
に補正することが可能である。カメラの撮像結果に基づ
いて求められる、カメラ座標系ΣC から見た手先効果器
20の位置・姿勢の偏差を表すベクトルを下記(16)式の如
く表すと、その位置・姿勢の偏差を解消するためにマニ
ピュレータ２の各関節21,21,…で発生すべきトルクを表
す行列τは、前記(14)式を用いて下記(17)式の如く求め
ることができる。
ならず、目標の位置・姿勢に対する任意の位置・姿勢の
偏差が測定可能であるならば、以上説明した如き原理に
基づいて手先効果器20の位置・姿勢を目標の位置・姿勢
に補正することが可能である。カメラの撮像結果に基づ
いて求められる、カメラ座標系ΣC から見た手先効果器
20の位置・姿勢の偏差を表すベクトルを下記(16)式の如
く表すと、その位置・姿勢の偏差を解消するためにマニ
ピュレータ２の各関節21,21,…で発生すべきトルクを表
す行列τは、前記(14)式を用いて下記(17)式の如く求め
ることができる。
【００５１】
【数１８】
【００５２】
【数１９】
【００５３】次に、マニピュレータ２の制御則について
説明する。マニピュレータ２の各関節21,21,…は全て、
前記サーボモータを含むサーボ系を介して駆動され、各
関節21,21,…を駆動するためのサーボ系への入力電圧を
表すベクトルＶは、サーボ系の基本的な制御則に前記(1
7)式の考え方（カメラ３の画像情報に基づいて手先効果
器20の位置・姿勢の偏差を解消する制御）を付加する
と、下記(18)式の如く求められる。
説明する。マニピュレータ２の各関節21,21,…は全て、
前記サーボモータを含むサーボ系を介して駆動され、各
関節21,21,…を駆動するためのサーボ系への入力電圧を
表すベクトルＶは、サーボ系の基本的な制御則に前記(1
7)式の考え方（カメラ３の画像情報に基づいて手先効果
器20の位置・姿勢の偏差を解消する制御）を付加する
と、下記(18)式の如く求められる。
【００５４】
【数２０】
【００５５】前記(18)式において、右辺第１項は、目標
関節角の指令値に基づく各関節21,21,…の位置フィード
バックを表す項であり、前記目標関節角と関節角の実測
値との偏差に所定の制御ゲインを乗じたものである。ま
た、右辺第２項は、カメラ３の画像情報に基づく各関節
21,21,…の位置・姿勢の補正を表す項であり、前述の如
きヤコビ行列に、カメラ座標系ΣC から見た手先効果器
20の位置・姿勢の偏差を表すベクトルと、所定の制御ゲ
インを乗じたものである。即ち、前記(18)式は、右辺第
１項による位置フィードバックと、右辺第２項による位
置・姿勢の補正とを同時に行う制御則である。
関節角の指令値に基づく各関節21,21,…の位置フィード
バックを表す項であり、前記目標関節角と関節角の実測
値との偏差に所定の制御ゲインを乗じたものである。ま
た、右辺第２項は、カメラ３の画像情報に基づく各関節
21,21,…の位置・姿勢の補正を表す項であり、前述の如
きヤコビ行列に、カメラ座標系ΣC から見た手先効果器
20の位置・姿勢の偏差を表すベクトルと、所定の制御ゲ
インを乗じたものである。即ち、前記(18)式は、右辺第
１項による位置フィードバックと、右辺第２項による位
置・姿勢の補正とを同時に行う制御則である。
【００５６】しかし、前記(18)式の制御則の右辺第１項
の制御によって得られる手先効果器20の位置・姿勢は、
移動ロボットの位置・方位の誤差のため、誤差を含んだ
位置・姿勢であるといえる。この位置・姿勢の誤差が、
つまり、前記(18)式の右辺第２項の手先効果器20の位置
・姿勢の偏差を表すベクトルになるわけであり、このた
め、前記(18)式のように、右辺第１項による位置フィー
ドバックと、右辺第２項による位置・姿勢の補正とを同
時に行うことは、右辺第１項と、右辺第２項とには、以
下の理由による不調和が発生する。
の制御によって得られる手先効果器20の位置・姿勢は、
移動ロボットの位置・方位の誤差のため、誤差を含んだ
位置・姿勢であるといえる。この位置・姿勢の誤差が、
つまり、前記(18)式の右辺第２項の手先効果器20の位置
・姿勢の偏差を表すベクトルになるわけであり、このた
め、前記(18)式のように、右辺第１項による位置フィー
ドバックと、右辺第２項による位置・姿勢の補正とを同
時に行うことは、右辺第１項と、右辺第２項とには、以
下の理由による不調和が発生する。
【００５７】このような右辺第１項と、右辺第２項との
不調和を避ける方法としては、まず、位置・姿勢の誤差
を生じさせる右辺第１項を省略することが考えられる。
その場合は、右辺第２項におけるベクトルδｘi の全て
の要素を正確に求めることが必要である。つまり、視覚
認識による正確な３次元計測が可能であることが必要条
件となる。
不調和を避ける方法としては、まず、位置・姿勢の誤差
を生じさせる右辺第１項を省略することが考えられる。
その場合は、右辺第２項におけるベクトルδｘi の全て
の要素を正確に求めることが必要である。つまり、視覚
認識による正確な３次元計測が可能であることが必要条
件となる。
【００５８】しかし、現実的には、ベクトルδｘi の全
ての要素を観測できるセンサはなく、また逆に作業座標
系Σw の全ての座標軸についての位置・姿勢偏差おフィ
ードバックする必要がない場合もある。本発明で考えて
いるマニピュレータでは、移動ロボットの移動は作業座
標系Σw のｘ−ｙ平面内に限られること、台車１の走行
路面の凹凸及びリンク，減速機のたわみを無視できるこ
と、重力による影響は既に補償されていることを仮定す
れば、ベクトルδｘi のｚ軸方向の位置偏差及びｘ，ｙ
軸回りの姿勢偏差は零である。従って、カメラ３は、常
に鉛直下方を撮像するように制御されるので、カメラ３
が撮像した２次元画像よりｘ，ｙ軸方向の位置偏差及び
ｚ軸回りの姿勢偏差が正確に測定できる。
ての要素を観測できるセンサはなく、また逆に作業座標
系Σw の全ての座標軸についての位置・姿勢偏差おフィ
ードバックする必要がない場合もある。本発明で考えて
いるマニピュレータでは、移動ロボットの移動は作業座
標系Σw のｘ−ｙ平面内に限られること、台車１の走行
路面の凹凸及びリンク，減速機のたわみを無視できるこ
と、重力による影響は既に補償されていることを仮定す
れば、ベクトルδｘi のｚ軸方向の位置偏差及びｘ，ｙ
軸回りの姿勢偏差は零である。従って、カメラ３は、常
に鉛直下方を撮像するように制御されるので、カメラ３
が撮像した２次元画像よりｘ，ｙ軸方向の位置偏差及び
ｚ軸回りの姿勢偏差が正確に測定できる。
【００５９】このため、これらの偏差をカメラフィード
バック（カメラ３からの情報のフィードバック）により
制御し（前記(18)式の右辺第２項による制御）、その他
の偏差を位置・姿勢フィードバックにより制御（前記(1
8)式の右辺第１項による制御）することを考える。
バック（カメラ３からの情報のフィードバック）により
制御し（前記(18)式の右辺第２項による制御）、その他
の偏差を位置・姿勢フィードバックにより制御（前記(1
8)式の右辺第１項による制御）することを考える。
【００６０】しかし、このとき、次のような不調和が起
こる。もし、前記(18)式において、制御ゲインＫ1 ≠
０，制御ゲインＫ2 ＝０の場合には、位置・姿勢フィー
ドバックによる制御系そのものであるから、手先効果器
20は、図５の手先座標系ΣH の位置に制御され、停止す
る。また、逆に、前記(18)式において、制御ゲインＫ1
＝０，制御ゲインＫ2 ≠０の場合には、手先効果器20は
作業座標系Σw の原点に移動する。従って、制御ゲイン
Ｋ1 及び制御ゲインＫ2 のどちらもが値を持つ前記(18)
式の制御則では、手先効果器20は、手先座標系ΣH の原
点と作業座標系Σw の原点との間に移動し、作業座標系
Σw の原点には制御されない。
こる。もし、前記(18)式において、制御ゲインＫ1 ≠
０，制御ゲインＫ2 ＝０の場合には、位置・姿勢フィー
ドバックによる制御系そのものであるから、手先効果器
20は、図５の手先座標系ΣH の位置に制御され、停止す
る。また、逆に、前記(18)式において、制御ゲインＫ1
＝０，制御ゲインＫ2 ≠０の場合には、手先効果器20は
作業座標系Σw の原点に移動する。従って、制御ゲイン
Ｋ1 及び制御ゲインＫ2 のどちらもが値を持つ前記(18)
式の制御則では、手先効果器20は、手先座標系ΣH の原
点と作業座標系Σw の原点との間に移動し、作業座標系
Σw の原点には制御されない。
【００６１】このように、前記(18)式の右辺第１項を省
略した制御則には問題があるので、本発明では、右辺第
１項と右辺第２項とで制御を分担させることを考えた。
前記(18)式の右辺第１項における目標関節角の導出の元
をたどると、それは、作業座標系Σw から見た手先効果
器20の目標位置・姿勢を表すベクトル WｒHd i である。
即ち、移動ロボットの台車１の位置・方位を考慮して、
手先効果器20がとるべき位置・姿勢を表すベクトル Wｒ
H i が求められ、前記(4) 式の変換の逆変換を経て目標
関節角が求められるのである。
略した制御則には問題があるので、本発明では、右辺第
１項と右辺第２項とで制御を分担させることを考えた。
前記(18)式の右辺第１項における目標関節角の導出の元
をたどると、それは、作業座標系Σw から見た手先効果
器20の目標位置・姿勢を表すベクトル WｒHd i である。
即ち、移動ロボットの台車１の位置・方位を考慮して、
手先効果器20がとるべき位置・姿勢を表すベクトル Wｒ
H i が求められ、前記(4) 式の変換の逆変換を経て目標
関節角が求められるのである。
【００６２】そこで、右辺第１項は、作業座標系Σw の
ｚ軸方向及び姿勢に関しては制御を行うが、作業座標系
Σw のｘ，ｙ軸方向に関しては制御を行わない、即ち、
手先効果器20の位置・姿勢のｘ，ｙ軸方向については目
標値を持たないようにする制御を行うようにする。これ
は、言い換えれば、作業座標系Σw のｚ軸方向及び姿勢
に関しては右辺第１項にて制御を行い、一方、作業座標
系Σw のｘ，ｙ軸方向に関しては右辺第２項にて制御を
行うことにより、右辺第１項と右辺第２項とで制御を分
担することである。このようにすれば、右辺第１項と右
辺第２項との不調和が解消される。
ｚ軸方向及び姿勢に関しては制御を行うが、作業座標系
Σw のｘ，ｙ軸方向に関しては制御を行わない、即ち、
手先効果器20の位置・姿勢のｘ，ｙ軸方向については目
標値を持たないようにする制御を行うようにする。これ
は、言い換えれば、作業座標系Σw のｚ軸方向及び姿勢
に関しては右辺第１項にて制御を行い、一方、作業座標
系Σw のｘ，ｙ軸方向に関しては右辺第２項にて制御を
行うことにより、右辺第１項と右辺第２項とで制御を分
担することである。このようにすれば、右辺第１項と右
辺第２項との不調和が解消される。
【００６３】次に、右辺第１項と右辺第２項とで制御を
分担する方法について説明する。マニピュレータ２のｉ
τ時刻における各関節21,21,…の関節角を表す行列ｑi
と、ｉτ時刻の絶対座標系ΣS における台車１の位置・
方位を表す同時変換行列とに関連して、所定の方法によ
り、ｉτ時刻の作業座標系Σw における手先効果器20の
位置・姿勢を表す同時変換行列を求めることができる。
また、この結果に関連して所定の方法により、ｉτ時刻
の作業座標系Σw における手先効果器20の位置・姿勢を
表すベクトル WｒH i が求められる。
分担する方法について説明する。マニピュレータ２のｉ
τ時刻における各関節21,21,…の関節角を表す行列ｑi
と、ｉτ時刻の絶対座標系ΣS における台車１の位置・
方位を表す同時変換行列とに関連して、所定の方法によ
り、ｉτ時刻の作業座標系Σw における手先効果器20の
位置・姿勢を表す同時変換行列を求めることができる。
また、この結果に関連して所定の方法により、ｉτ時刻
の作業座標系Σw における手先効果器20の位置・姿勢を
表すベクトル WｒH i が求められる。
【００６４】もし、ベクトル WｒH i とベクトル WｒHd
i とが等しいという条件が成り立つならば、行列ｑi と
行列ｑd i とが等しくなるため、前記(18)式の右辺第１
項は値を持たず、右辺第１項を省略したことに等しい。
これにより、ベクトル WｒH i のｘ，ｙ軸成分である W
ｒx i ， Wｒy i が、ベクトル WｒHd i のｘ，ｙ軸成分
である Wｒxd i ， Wｒyd i と常に等しくなるように、 W
ｒxd i ， Wｒyd i に Wｒx i ， Wｒy i を与えることと
すると、右辺第１項は、作業座標系Σw のｚ軸方向及び
姿勢に関しては制御を行うが、作業座標系Σw のｘ，ｙ
軸方向に関しては制御を行わないという、右辺第１項と
右辺第２項との制御の分担が成り立つ。
i とが等しいという条件が成り立つならば、行列ｑi と
行列ｑd i とが等しくなるため、前記(18)式の右辺第１
項は値を持たず、右辺第１項を省略したことに等しい。
これにより、ベクトル WｒH i のｘ，ｙ軸成分である W
ｒx i ， Wｒy i が、ベクトル WｒHd i のｘ，ｙ軸成分
である Wｒxd i ， Wｒyd i と常に等しくなるように、 W
ｒxd i ， Wｒyd i に Wｒx i ， Wｒy i を与えることと
すると、右辺第１項は、作業座標系Σw のｚ軸方向及び
姿勢に関しては制御を行うが、作業座標系Σw のｘ，ｙ
軸方向に関しては制御を行わないという、右辺第１項と
右辺第２項との制御の分担が成り立つ。
【００６５】このような、前記(18)式の右辺第１項と右
辺第２項との制御の分担を考慮した作業座標系Σw から
見た手先効果器20の目標位置・姿勢を表すベクトル p W
ｒHd i は、下記(19)式にて表される。
辺第２項との制御の分担を考慮した作業座標系Σw から
見た手先効果器20の目標位置・姿勢を表すベクトル p W
ｒHd i は、下記(19)式にて表される。
【００６６】
【数２１】
【００６７】ここで、前記(19)式の右辺第２項の第１行
列を、以下の説明においてスイッチング行列Ｓと呼ぶこ
とにする。このスイッチング行列Ｓは、１が立った方向
については制御を行わないものである。例えば、手先効
果器20の姿勢について制御を行わないような、目標位置
・姿勢を表すベクトル p WｒHd i を求めたい場合、その
スイッチング行列Ｓは下記(20)式の如く表される。
列を、以下の説明においてスイッチング行列Ｓと呼ぶこ
とにする。このスイッチング行列Ｓは、１が立った方向
については制御を行わないものである。例えば、手先効
果器20の姿勢について制御を行わないような、目標位置
・姿勢を表すベクトル p WｒHd i を求めたい場合、その
スイッチング行列Ｓは下記(20)式の如く表される。
【００６８】
【数２２】
【００６９】また、前記(19)式をスイッチング行列Ｓを
用いて表すと、下記(21)式の如くなる。
用いて表すと、下記(21)式の如くなる。
【００７０】
【数２３】
【００７１】ここで、前記(19)式のベクトル p WｒHd i
に基づいて前述の如く求められるマニピュレータ２の目
標関節角を行列 pｑd i で表し、また、前記(18)式の右
辺第２項が、その右辺第１項が行わない方向に対して制
御を行うことを考慮すると、各関節21,21,…を駆動する
ためのサーボ系への入力電圧を表すベクトルＶは、最終
的に下記(22)式の如くする。
に基づいて前述の如く求められるマニピュレータ２の目
標関節角を行列 pｑd i で表し、また、前記(18)式の右
辺第２項が、その右辺第１項が行わない方向に対して制
御を行うことを考慮すると、各関節21,21,…を駆動する
ためのサーボ系への入力電圧を表すベクトルＶは、最終
的に下記(22)式の如くする。
【００７２】
【数２４】
【００７３】前記(22)式の右辺第１項は、前述の如く得
られる行列 pｑd i にて表される目標関節角と、関節角
の実測値との偏差に所定の制御ゲインを乗じたものであ
り、作業座標系Σw のｚ軸方向及び姿勢に関してのみ制
御を行う。また、右辺第２項は、ヤコビ行列に、カメラ
座標系ΣC から見た手先効果器20の位置・姿勢の偏差を
表すベクトルと、作業座標系Σw のｘ，ｙ軸方向に関し
てのみ制御を行わせるスイッチング行列Ｓと、所定の制
御ゲインとを乗じたものであり、作業座標系Σw のｘ，
ｙ軸方向に関してのみ制御を行う。即ち、前記(22)式
は、右辺第１項による作業座標系Σw のｚ軸方向及び姿
勢の制御と、右辺第２項による作業座標系Σw のｘ，ｙ
軸方向の制御とが調和して行われる制御則である。
られる行列 pｑd i にて表される目標関節角と、関節角
の実測値との偏差に所定の制御ゲインを乗じたものであ
り、作業座標系Σw のｚ軸方向及び姿勢に関してのみ制
御を行う。また、右辺第２項は、ヤコビ行列に、カメラ
座標系ΣC から見た手先効果器20の位置・姿勢の偏差を
表すベクトルと、作業座標系Σw のｘ，ｙ軸方向に関し
てのみ制御を行わせるスイッチング行列Ｓと、所定の制
御ゲインとを乗じたものであり、作業座標系Σw のｘ，
ｙ軸方向に関してのみ制御を行う。即ち、前記(22)式
は、右辺第１項による作業座標系Σw のｚ軸方向及び姿
勢の制御と、右辺第２項による作業座標系Σw のｘ，ｙ
軸方向の制御とが調和して行われる制御則である。
【００７４】次に、前述の如きマニピュレータ２の位置
・姿勢制御を実施するための制御系の構成について説明
する。図６はマニピュレータ２の位置・姿勢制御を行う
制御系の模式的ブロック図である。
・姿勢制御を実施するための制御系の構成について説明
する。図６はマニピュレータ２の位置・姿勢制御を行う
制御系の模式的ブロック図である。
【００７５】図中61は、作業座標系Σw で表された手先
効果器20の位置・姿勢の目標値を設定する位置・姿勢目
標値設定部であり、該位置・姿勢目標値設定部61で設定
された目標位置・姿勢の情報は、位置・姿勢目標値決定
部64へ与えられるようになっている。
効果器20の位置・姿勢の目標値を設定する位置・姿勢目
標値設定部であり、該位置・姿勢目標値設定部61で設定
された目標位置・姿勢の情報は、位置・姿勢目標値決定
部64へ与えられるようになっている。
【００７６】また、台車位置・姿勢演算部62では、台車
１の走行状態に基づいて台車１の現在の位置・姿勢を求
めるようになっており、この台車位置・姿勢演算部62で
求められた台車１の位置・姿勢の情報は、手先位置・姿
勢演算部63及び関節駆動情報演算部65へ与えられるよう
になっている。手先位置・姿勢演算部63では、マニピュ
レータ２の現在の関節角の情報（検出情報）と、台車位
置・姿勢演算部62から与えられた台車１の位置・姿勢の
情報とに基づいて手先効果器20の現在の位置・姿勢を求
めるようになっており、この手先位置・姿勢演算部63で
求められた位置・姿勢の情報は、位置・姿勢目標値決定
部64へ与えられるようになっている。
１の走行状態に基づいて台車１の現在の位置・姿勢を求
めるようになっており、この台車位置・姿勢演算部62で
求められた台車１の位置・姿勢の情報は、手先位置・姿
勢演算部63及び関節駆動情報演算部65へ与えられるよう
になっている。手先位置・姿勢演算部63では、マニピュ
レータ２の現在の関節角の情報（検出情報）と、台車位
置・姿勢演算部62から与えられた台車１の位置・姿勢の
情報とに基づいて手先効果器20の現在の位置・姿勢を求
めるようになっており、この手先位置・姿勢演算部63で
求められた位置・姿勢の情報は、位置・姿勢目標値決定
部64へ与えられるようになっている。
【００７７】位置・姿勢目標値決定部64では、位置・姿
勢目標値設定部61から与えられる目標位置・姿勢の情報
と、手先位置・姿勢演算部63から与えられる現在の位置
・姿勢の情報とに基づき、前述の(21)式を用いて、制御
に用いるべき手先効果器20の位置・姿勢の目標値を決定
するようになっており、その決定された位置・姿勢の目
標値は関節駆動情報演算部65へ与えられるようになって
いる。
勢目標値設定部61から与えられる目標位置・姿勢の情報
と、手先位置・姿勢演算部63から与えられる現在の位置
・姿勢の情報とに基づき、前述の(21)式を用いて、制御
に用いるべき手先効果器20の位置・姿勢の目標値を決定
するようになっており、その決定された位置・姿勢の目
標値は関節駆動情報演算部65へ与えられるようになって
いる。
【００７８】関節駆動情報演算部65では、台車位置・姿
勢演算部62から与えられた台車１の位置・姿勢の情報を
考慮して、位置・姿勢目標値決定部64から与えられた位
置・姿勢の目標値を実現すべく手先効果器20がとる位置
・姿勢を求め、前述の如き(4) 式の変換の逆変換を経て
マニピュレータ２の各関節21,21,…の目標関節角を求め
るようになっている。ここで求められた目標関節角の情
報は、サーボ系入力電圧演算部67に与えられるようにな
っている。
勢演算部62から与えられた台車１の位置・姿勢の情報を
考慮して、位置・姿勢目標値決定部64から与えられた位
置・姿勢の目標値を実現すべく手先効果器20がとる位置
・姿勢を求め、前述の如き(4) 式の変換の逆変換を経て
マニピュレータ２の各関節21,21,…の目標関節角を求め
るようになっている。ここで求められた目標関節角の情
報は、サーボ系入力電圧演算部67に与えられるようにな
っている。
【００７９】また、カメラ３の撮像情報は、画像処理部
66に与えられるようになっており、画像処理部66では、
カメラ３の撮像情報を画像処理してカメラ座標系ΣC か
ら見た手先効果器20の位置・姿勢の偏差を求めるように
なっている。ここで求められた位置・姿勢の偏差の情報
は、サーボ系入力電圧演算部67に与えられるようになっ
ている。
66に与えられるようになっており、画像処理部66では、
カメラ３の撮像情報を画像処理してカメラ座標系ΣC か
ら見た手先効果器20の位置・姿勢の偏差を求めるように
なっている。ここで求められた位置・姿勢の偏差の情報
は、サーボ系入力電圧演算部67に与えられるようになっ
ている。
【００８０】サーボ系入力電圧演算部67では、前述の如
き情報の他に、マニピュレータ２の各関節21,21,…の関
節角の実測値が与えられるようになっており、この関節
角の実測値の情報と、関節駆動情報演算部65から与えら
れる目標関節角と、画像処理部66から与えられる手先効
果器20の位置・姿勢の偏差の情報とに基づき、前記(22)
式を用いてサーボ系への入力電圧を求めるようになって
いる。サーボ系入力電圧演算部67で求められたサーボ系
への入力電圧は、マニピュレータ２のサーボ系に与えら
れるようになっている。
き情報の他に、マニピュレータ２の各関節21,21,…の関
節角の実測値が与えられるようになっており、この関節
角の実測値の情報と、関節駆動情報演算部65から与えら
れる目標関節角と、画像処理部66から与えられる手先効
果器20の位置・姿勢の偏差の情報とに基づき、前記(22)
式を用いてサーボ系への入力電圧を求めるようになって
いる。サーボ系入力電圧演算部67で求められたサーボ系
への入力電圧は、マニピュレータ２のサーボ系に与えら
れるようになっている。
【００８１】このように構成された制御系によって、マ
ニピュレータ２は、サーボ系入力電圧演算部67から与え
られた入力電圧によって駆動される。
ニピュレータ２は、サーボ系入力電圧演算部67から与え
られた入力電圧によって駆動される。
【００８２】以上の如き制御においては、マニピュレー
タ２の手先効果器20は、カメラ３の画像情報に基づく制
御と、関節角度の指令値の情報に基づく制御との２種類
の制御を、夫々の制御の特徴を活かすように結合された
形式で、マニピュレータ２の各関節21,21,…の関節角度
が制御されるが、この関節角度の制御については、水平
面内での手先の位置・姿勢をリアルタイムで表す前記画
像情報が含まれているため、マニピュレータ２に対して
相対移動する作業対象物４に対しても有効な位置・姿勢
制御が行える。
タ２の手先効果器20は、カメラ３の画像情報に基づく制
御と、関節角度の指令値の情報に基づく制御との２種類
の制御を、夫々の制御の特徴を活かすように結合された
形式で、マニピュレータ２の各関節21,21,…の関節角度
が制御されるが、この関節角度の制御については、水平
面内での手先の位置・姿勢をリアルタイムで表す前記画
像情報が含まれているため、マニピュレータ２に対して
相対移動する作業対象物４に対しても有効な位置・姿勢
制御が行える。
【００８３】
【発明の効果】以上詳述した如く、第１の発明に係るマ
ニピュレータの位置・姿勢制御方法においては、左，右
に独立した駆動輪を備える移動体上のマニピュレータに
対し、移動体の走行中、移動体の位置・姿勢データに基
づく制御と撮像手段から常時リアルタイムで得られる画
像情報に含まれる水平面内での手先の位置・姿勢と作業
対象物のそれとの位置偏差に基づく制御とを結合させ
て、関節の角度を制御するがことが出来て、相対移動す
る作業対象物に対しても同じくリアルタイムで効果的な
手先の位置・姿勢制御を行うことができる優れた効果を
奏する。
ニピュレータの位置・姿勢制御方法においては、左，右
に独立した駆動輪を備える移動体上のマニピュレータに
対し、移動体の走行中、移動体の位置・姿勢データに基
づく制御と撮像手段から常時リアルタイムで得られる画
像情報に含まれる水平面内での手先の位置・姿勢と作業
対象物のそれとの位置偏差に基づく制御とを結合させ
て、関節の角度を制御するがことが出来て、相対移動す
る作業対象物に対しても同じくリアルタイムで効果的な
手先の位置・姿勢制御を行うことができる優れた効果を
奏する。
【００８４】第２の発明に係るマニピュレータの位置・
姿勢制御方法においては、目標関節角と関節角の実測値
との偏差に制御ゲインを乗じた作業座標系での位置・姿
勢に関した制御と、ヤコビ行列に撮像手段からの画像情
報で得た手先の位置・姿勢の偏差と作業座標系の二次元
平面上での制御を行わせる行列Ｓと制御ゲインを乗じた
作業座標系での二次元平面上での制御とを、これらの調
和を維持しつつ実行することが出来る。
姿勢制御方法においては、目標関節角と関節角の実測値
との偏差に制御ゲインを乗じた作業座標系での位置・姿
勢に関した制御と、ヤコビ行列に撮像手段からの画像情
報で得た手先の位置・姿勢の偏差と作業座標系の二次元
平面上での制御を行わせる行列Ｓと制御ゲインを乗じた
作業座標系での二次元平面上での制御とを、これらの調
和を維持しつつ実行することが出来る。
【図１】本発明に係るマニピュレータの位置・姿勢制御
方法の実施に使用する移動ロボットの斜視図である。
方法の実施に使用する移動ロボットの斜視図である。
【図２】マニピュレータの静力学関係を説明するための
説明図である。
説明図である。
【図３】ｘ−ｙ平面内における外力の作用を説明するた
めの説明図である。
めの説明図である。
【図４】カメラ座標系を説明するための説明図である。
【図５】手先座標系の座標軸とカメラ座標系の座標軸と
が一致する場合のカメラ座標系を説明するための説明図
である。
が一致する場合のカメラ座標系を説明するための説明図
である。
【図６】マニピュレータの位置・姿勢制御を行う制御系
の模式的ブロック図である。
の模式的ブロック図である。
１ 台車 ２ マニピュレータ ３ カメラ 20 手先効果器 21 関節 64 位置・姿勢目標値決定部 65 関節駆動情報演算部
───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.6，ＤＢ名) B25J 5/00,13/08
Claims (2)
- 【請求項１】 左，右に夫々独立に駆動される車輪を備
えた移動体に搭載され、マニピュレータの手先の周りの
所定範囲の画像を上方から撮像する撮像手段を備えたマ
ニピュレータにおける関節の角度を制御することにより
前記手先を目標の位置・姿勢に制御する方法において、 前記移動体の走行中に、手先の位置・姿勢の目標値と、
手先の現在の位置・姿勢と、移動体の現在の位置・姿勢
とに基づいて、マニピュレータの関節の目標関節角を求
め、該目標関節角と、別途求めたマニピュレータの関節
角の実測値と、前記撮像手段からリアルタイムに得られ
る画像情報に含まれている水平面内での手先の位置・姿
勢と作業対象物のそれとの偏差とに基づいて水平面内で
の前記手先の位置・姿勢をリアルタイムで制御すること
を特徴とするマニピュレータの位置・姿勢制御方法。 - 【請求項２】 前記目標関節角と、関節角の実測値と、
手先の位置・姿勢と作業対象物のそれとの偏差とに基づ
く制御は、下式で表わされる制御則に従って行われるこ
とを特徴とする請求項１記載のマニピュレータの位置・
姿勢制御方法。 【数１】
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP4148654A JP2769947B2 (ja) | 1992-05-15 | 1992-05-15 | マニピュレータの位置・姿勢制御方法 |
| US08/058,732 US5523663A (en) | 1992-05-15 | 1993-05-07 | Method for controlling a manipulator relative to a moving workpiece |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP4148654A JP2769947B2 (ja) | 1992-05-15 | 1992-05-15 | マニピュレータの位置・姿勢制御方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH05318354A JPH05318354A (ja) | 1993-12-03 |
| JP2769947B2 true JP2769947B2 (ja) | 1998-06-25 |
Family
ID=15457642
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP4148654A Expired - Fee Related JP2769947B2 (ja) | 1992-05-15 | 1992-05-15 | マニピュレータの位置・姿勢制御方法 |
Country Status (2)
| Country | Link |
|---|---|
| US (1) | US5523663A (ja) |
| JP (1) | JP2769947B2 (ja) |
Families Citing this family (44)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2764397B1 (fr) * | 1997-06-09 | 1999-10-01 | Aerospatiale | Procede et dispositif de prises de vues spheriques pour la realisation d'une matrice d'images 3d d'un objet |
| GB9803364D0 (en) | 1998-02-18 | 1998-04-15 | Armstrong Healthcare Ltd | Improvements in or relating to a method of an apparatus for registering a robot |
| GB2347741A (en) | 1998-11-05 | 2000-09-13 | Cyberoptics Corp | Electronics assembly apparatus with improved imaging system |
| US6194860B1 (en) | 1999-11-01 | 2001-02-27 | Yoder Software, Inc. | Mobile camera-space manipulation |
| US6538244B1 (en) | 1999-11-03 | 2003-03-25 | Cyberoptics Corporation | Pick and place machine with improved vision system including a linescan sensor |
| US6535291B1 (en) * | 2000-06-07 | 2003-03-18 | Cyberoptics Corporation | Calibration methods for placement machines incorporating on-head linescan sensing |
| KR100478452B1 (ko) * | 2002-06-12 | 2005-03-23 | 삼성전자주식회사 | 이동 로봇의 위치 및 방향 인식 장치 및 방법 |
| KR100468857B1 (ko) * | 2002-11-21 | 2005-01-29 | 삼성전자주식회사 | 2차원 형상에 대한 투사 불변형 표현자를 이용한핸드/아이 캘리브레이션 방법 |
| JP3975959B2 (ja) * | 2003-04-23 | 2007-09-12 | トヨタ自動車株式会社 | ロボット動作規制方法とその装置およびそれを備えたロボット |
| JP2006236031A (ja) * | 2005-02-25 | 2006-09-07 | Seiko Epson Corp | ロボット軌跡制御方法及び装置並びにロボット軌跡制御方法のプログラム |
| CA2633733C (en) * | 2005-12-30 | 2013-12-17 | Goldwing Nominees Pty Ltd | An automated brick laying system for constructing a building from a plurality of bricks |
| WO2007149183A2 (en) * | 2006-05-25 | 2007-12-27 | Braintech Canada, Inc. | System and method of robotically engaging an object |
| US8437535B2 (en) | 2006-09-19 | 2013-05-07 | Roboticvisiontech Llc | System and method of determining object pose |
| US20080181485A1 (en) * | 2006-12-15 | 2008-07-31 | Beis Jeffrey S | System and method of identifying objects |
| US7957583B2 (en) * | 2007-08-02 | 2011-06-07 | Roboticvisiontech Llc | System and method of three-dimensional pose estimation |
| CN101511321B (zh) * | 2007-08-03 | 2012-11-28 | 松下电器产业株式会社 | 按摩机 |
| WO2009026641A1 (en) * | 2007-08-28 | 2009-03-05 | Goldwing Nominees Pty Ltd | System and method for precise real-time control of position and orientation of tooling |
| KR101479233B1 (ko) | 2008-05-13 | 2015-01-05 | 삼성전자 주식회사 | 로봇 및 그 협조작업 제어방법 |
| US8559699B2 (en) * | 2008-10-10 | 2013-10-15 | Roboticvisiontech Llc | Methods and apparatus to facilitate operations in image based systems |
| JP5083194B2 (ja) * | 2008-12-18 | 2012-11-28 | 株式会社デンソーウェーブ | ロボットのキャリブレーション方法及びロボットの制御装置 |
| US8209065B2 (en) * | 2009-01-21 | 2012-06-26 | The Boeing Company | Steering logic for spacecraft slew maneuvers |
| US8457791B2 (en) * | 2009-03-10 | 2013-06-04 | GM Global Technology Operations LLC | Method for dynamically controlling a robotic arm |
| JP4741691B2 (ja) * | 2009-06-15 | 2011-08-03 | ファナック株式会社 | ロボットの異常監視機能を備えたロボットシステム |
| WO2012077449A1 (ja) * | 2010-12-07 | 2012-06-14 | 住友化学株式会社 | ハニカム構造体の搬送装置、及び、ハニカム構造体の封口方法、並びにハニカム構造体の製造方法 |
| KR101305819B1 (ko) * | 2012-01-04 | 2013-09-06 | 현대자동차주식회사 | 착용식 로봇의 사용자 조작 의도 토크 추출방법 |
| DE102012208095A1 (de) * | 2012-05-15 | 2013-11-21 | Kuka Laboratories Gmbh | Mobiler Roboter und Verfahren zum Betreiben eines mobilen Roboters |
| JP6307431B2 (ja) * | 2012-05-25 | 2018-04-04 | 学校法人立命館 | ロボット制御装置、ロボット制御方法、プログラム、記録媒体、ロボットシステム |
| US9457473B2 (en) * | 2012-06-20 | 2016-10-04 | Irobot Corporation | Suspended robot systems and methods for using same |
| WO2014179864A1 (en) * | 2013-05-06 | 2014-11-13 | Socpra Sciences Et Genie S.E.C. | Control of actuators in a robot |
| CN104827480A (zh) * | 2014-02-11 | 2015-08-12 | 泰科电子(上海)有限公司 | 机器人系统的自动标定方法 |
| CN105094051B (zh) * | 2014-04-15 | 2018-03-09 | 睿励科学仪器（上海）有限公司 | 运动平台系统的平面定位补偿方法 |
| JP6485620B2 (ja) * | 2014-09-03 | 2019-03-20 | 株式会社チトセロボティクス | ロボット制御システムおよびロボット制御方法 |
| DE102015212171B3 (de) * | 2015-06-30 | 2016-06-30 | Kuka Roboter Gmbh | Verfahren zum Steuern eines Manipulatorsystems |
| BR112019000728B1 (pt) | 2016-07-15 | 2023-03-28 | Fastbrick Ip Pty Ltd | Veículo que incorpora máquina de assentamento de tijolos |
| BR112019000722B1 (pt) | 2016-07-15 | 2023-03-28 | Fastbrick Ip Pty Ltd | Lança extensível telescópica para transportar item e lança dobrável |
| CN111095355B (zh) | 2017-07-05 | 2023-10-20 | 快砖知识产权私人有限公司 | 实时定位和定向跟踪器 |
| AU2018317937B2 (en) | 2017-08-17 | 2023-11-23 | Fastbrick Ip Pty Ltd | Communication system for an interaction system |
| CN111226090B (zh) | 2017-08-17 | 2023-05-23 | 快砖知识产权私人有限公司 | 具有改进的横滚角测量的激光跟踪器 |
| JP7013766B2 (ja) * | 2017-09-22 | 2022-02-01 | セイコーエプソン株式会社 | ロボット制御装置、ロボットシステム、及び制御方法 |
| EP3694793B1 (en) | 2017-10-11 | 2023-11-08 | Fastbrick IP Pty Ltd | Machine for conveying objects |
| US10060857B1 (en) * | 2017-11-16 | 2018-08-28 | General Electric Company | Robotic feature mapping and motion control |
| WO2020261881A1 (ja) * | 2019-06-27 | 2020-12-30 | パナソニックＩｐマネジメント株式会社 | エンドエフェクタの制御システムおよびエンドエフェクタの制御方法 |
| CN110919658B (zh) * | 2019-12-13 | 2023-03-31 | 东华大学 | 一种基于视觉和多坐标系闭环转换的机器人标定方法 |
| US20220134577A1 (en) * | 2020-10-30 | 2022-05-05 | Dmg Mori Co., Ltd. | Image processing method, image processing apparatus, robot-mounted transfer device, and system |
Family Cites Families (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4146924A (en) * | 1975-09-22 | 1979-03-27 | Board Of Regents For Education Of The State Of Rhode Island | System for visually determining position in space and/or orientation in space and apparatus employing same |
| EP0108549B1 (en) * | 1982-10-29 | 1988-01-07 | Kabushiki Kaisha Toshiba | Control system of multi-joint arm robot apparatus |
| JPS62292376A (ja) * | 1986-06-11 | 1987-12-19 | 東京エレクトロン株式会社 | 無人搬送車 |
| JP2532098B2 (ja) * | 1987-06-19 | 1996-09-11 | トキコ株式会社 | 工業用ロボツトの教示方法 |
| US4833383A (en) * | 1987-08-13 | 1989-05-23 | Iowa State University Research Foundation, Inc. | Means and method of camera space manipulation |
| DE3741632A1 (de) * | 1987-12-05 | 1989-06-22 | Noell Gmbh | Verfahren und vorrichtung zum erkennen und ansteuern eines raumzieles |
| US4965499A (en) * | 1987-12-31 | 1990-10-23 | Westinghouse Electric Corp | Parametric path modeling for an optical automatic seam tracker and real time robotic control system |
| JPH0213804A (ja) * | 1988-07-01 | 1990-01-18 | Fanuc Ltd | 視覚センサにおけるノミナル設定方式 |
| US5159249A (en) * | 1989-05-16 | 1992-10-27 | Dalila Megherbi | Method and apparatus for controlling robot motion at and near singularities and for robot mechanical design |
| US5055755A (en) * | 1989-05-31 | 1991-10-08 | Kabushiki Kaisha Toshiba | Distribution control apparatus |
| JPH03281182A (ja) * | 1990-03-28 | 1991-12-11 | Shinko Electric Co Ltd | 移動ロボットの座標補正方法 |
-
1992
- 1992-05-15 JP JP4148654A patent/JP2769947B2/ja not_active Expired - Fee Related
-
1993
- 1993-05-07 US US08/058,732 patent/US5523663A/en not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH05318354A (ja) | 1993-12-03 |
| US5523663A (en) | 1996-06-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP2769947B2 (ja) | マニピュレータの位置・姿勢制御方法 | |
| TWI673150B (zh) | 機器人教示方法及機器臂控制裝置 | |
| JP5114019B2 (ja) | エフェクタの軌道を制御するための方法 | |
| JP6458052B2 (ja) | 自走式関節ロボット | |
| JP2019093481A (ja) | ロボットシステム及びロボットシステムの制御方法 | |
| Siradjuddin et al. | A position based visual tracking system for a 7 DOF robot manipulator using a Kinect camera | |
| JP2001157975A (ja) | ロボットの制御装置 | |
| KR20180069031A (ko) | 로봇의 다이렉트 교시방법 | |
| JPH0527125B2 (ja) | ||
| JP2013049102A (ja) | ロボットの制御装置及びロボットの姿勢決定方法 | |
| Li et al. | A hybrid visual servo control method for simultaneously controlling a nonholonomic mobile and a manipulator | |
| US20210268660A1 (en) | Robot Control Method And Robot System | |
| CN112706165A (zh) | 一种面向轮式移动机械臂的跟踪控制方法及系统 | |
| JP4396553B2 (ja) | ロボット制御装置，コンピュータプログラム | |
| Sharma et al. | On the observability of robot motion under active camera control | |
| JP2013094935A (ja) | ロボットアーム装置 | |
| Yoo et al. | A study on a mobile platform-manipulator welding system for horizontal fillet joints | |
| CN114378827B (zh) | 一种基于移动机械臂整体控制的动态目标跟踪抓取方法 | |
| Luo et al. | On-line adaptive control for minimizing slippage error while mobile platform and manipulator operate simultaneously for robotics mobile manipulation | |
| JP3223288B2 (ja) | 移動マニピュレータの位置・姿勢制御方法 | |
| Hanh et al. | Implement contour following task of objects with unknown geometric models by using combination of two visual servoing techniques | |
| Lang et al. | Visual servoing with LQR control for mobile robots | |
| JP2014124734A (ja) | ロボットおよび動作軌道制御システム | |
| Guangyao et al. | Visual servo control of omnidirectional mobile manipulator | |
| JP3937078B2 (ja) | ロボットの制御装置及び制御方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |