JPH09323280A - マニピュレータの制御方法及びシステム - Google Patents

Abstract

(57)【要約】 【課題】オペレータが先端効果器を遠隔対象物体に対し
て目測や経験に頼らず適切に操作して位置決めを行うこ
とができるマニピュレータの制御方法及びこれを実施す
るための制御システムを提供する。 【解決手段】対象物体Ｏに対するマニピュレータの位置
・姿勢を操作するに当り、予め３Ｄマーカー３が装備さ
れた先端効果器２をアーム１の先端に設置するととも
に、ステレオカメラ４を用意して置き、３Ｄマーカー３
と対象物体Ｏとを同時にステレオカメラ４で撮影し、コ
ントローラ５にて対象物体Ｏの特徴点と３Ｄマーカー３
の参照パターンの画面上の位置とを追跡し、先端効果器
２上に任意に選んだ参照座標系Σf が対象物体Ｏ上の特
徴点から定義される基準座標系ΣBに一致するようにマ
ニピュレータのアーム１の位置決め制御をする。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、ステレオカメラを
用いてロボットアーム等の先端効果器を遠隔対象物体に
対して所望の相対位置・姿勢に操作する際の位置決め制
御を可能にするマニピュレータの制御方法及びこれを実
施するための制御システムに関する。

【０００２】

【従来の技術】従来、大型クレーンや消防車の梯子等
（マニピュレータ）を遠隔操作して先端部（先端効果
器）を適切に位置決めするには、オペレータの目測に基
づく経験に頼るところが大きかった。よって、マニピュ
レータを自在に操作するには、高い熟練度が必要とな
り、初心者に困難な作業であった。そこで、マニピュレ
ータのアームの位置決めを容易にするために、カメラで
対象物体とアームの先端効果器とを同時にとらえて、こ
れらの特徴点を追跡し、両者を画像面上で一致させるサ
ーボ制御系を利用したマニピュレータの制御システムが
提案されている。

【０００３】

【発明が解決しようとする課題】しかしながら、前記マ
ニピュレータの制御システムでは、カメラで撮影された
画像から対象物体と先端効果器との両特徴点を認識する
必要があるので、その処理に多大な時間を要し、実用化
に至っていないのが現状である。

【０００４】ここにおいて本発明の解決すべき主要な目
的は、次の通りである。本発明の第１の目的は、オペレ
ータが先端効果器を遠隔対象物体に対して目測や経験に
頼らず適切に操作して位置決めを行うことができるマニ
ピュレータの制御方法及びこれを実施するための制御シ
ステムを提供せんとするものである。

【０００５】本発明の第２の目的は、対象物体上に定義
された基準座標系に対して先端効果器の位置・姿勢を立
体的に容易に把握して相対的に並進・回転運動させるこ
とができ、柔軟性の高い位置決め作業を効率的に行うこ
とができるマニピュレータの制御方法及びこれを実施す
るための制御システムを提供せんとするものである。

【０００６】本発明のその他の目的は、明細書、図面、
特に特許請求の範囲の各請求項の記載から自ずと明らか
となろう。

【０００７】

【課題を解決するための手段】本発明は、オペレータが
先端効果器を所望の相対位置・姿勢に操作する際、ステ
レオカメラでとらえた対象物体に対して先端効果器の位
置・姿勢を画像認識し易い３Ｄマーカーにて立体的にと
らえて誤差修正のフィードバック制御をすることを特徴
とする。これにより、オペレータが先端効果器を遠隔対
象物体に対して目測や経験に頼らず適切に操作して位置
決めを行うことができる。さらに、具体的詳細に述べる
と、当該課題の解決は、本発明が次に列挙するそれぞれ
の新規な特徴的構成手法及び手段を採用することにより
前記目的を達成する。

【０００８】すなわち、本発明方法の第１の特徴は、対
象物体に対するマニピュレータの位置・姿勢を操作する
に当り、アーム先端の先端効果器を当該対象物体との相
対関係で適正な位置・姿勢に位置決め制御する際、当該
相対関係の位置・姿勢の撮影と射影幾何等解析を容易に
するため前記先端効果器に一体取付けた３Ｄマーカーの
位置・姿勢の撮影と射影幾何等解析を通して当該先端効
果器の位置・姿勢を間接的に算出処理してなるマニピュ
レータの制御方法にある。

【０００９】本発明方法の第２の特徴は、前記本発明方
法の第１の特徴における前記対象物体と前記先端効果器
との相対関係の撮影が、ステレオカメラにて実行してな
るマニピュレータの制御方法にある。

【００１０】本発明方法の第３の特徴は、前記本発明方
法の第１又は第２の特徴における前記対象物体と前記先
端効果器との相対関係の射影幾何等解析が、前記ステレ
オカメラの画像の前記対象物体の特徴点及び前記３Ｄマ
ーカーの参照パターンの座標位置を数値的に割出し所定
関係式を通し算出処理してなるマニピュレータの制御方
法にある。

【００１１】本発明方法の第４の特徴は、対象物体に対
するマニピュレータの位置・姿勢を操作するに当り、予
め３Ｄマーカーが装備された先端効果器を前記マニピュ
レータのアーム先端に設置するとともに、ステレオカメ
ラを用意して置き、当該ステレオカメラで前記３Ｄマー
カーと前記対象物体とを同時に撮影して、前記対象物体
の特徴点と前記３Ｄマーカーの参照パターンの画面上の
位置とを追跡し、前記マニピュレータの先端効果器上に
任意に選んだ参照座標系が前記対象物体上の特徴点から
定義される基準座標系に一致するように前記マニピュレ
ータのアームの位置決めを制御するマニピュレータの制
御方法にある。

【００１２】本発明方法の第５の特徴は、対象物体に対
するマニピュレータの位置・姿勢を操作するに当り、予
め３Ｄマーカーが装備された先端効果器をマニピュレー
タのアーム先端に設置するとともに、当該３Ｄマーカー
と前記対象物体とを同時に撮影するステレオカメラを用
意して置き、先ず、前記ステレオカメラの画像面上にお
ける前記対象物体の複数の特徴点から基準座標系を定義
し、かつ前記先端効果器上の参照座標系の位置・姿勢を
指定し、次に前記対象物体の特徴点と前記３Ｄマーカー
の参照パターンの画面上の位置とを追跡し、前記ステレ
オカメラによる左右両画面上の前記３Ｄマーカーの参照
パターンの位置をカメラ座標系にて求めた後、前記カメ
ラ座標系に対する３Ｄマーカー座標系の相対位置・姿勢
を算出する一方、前記３Ｄマーカー座標系の相対位置・
姿勢に基づいて前記先端効果器上の参照座標系の射影を
算出し、当該算出した前記参照座標系の射影を前記ステ
レオカメラの左右両画面上にかき込み、続いて、前記先
端効果器上の参照座標系の射影画像を６次元のアーム特
徴量ベクトルに写像する一方、前記対象物体の基準座標
系の射影画像を６次元の対象物特徴量ベクトルに写像し
てアーム特徴量ベクトル及び対象物特徴量ベクトルを求
めた後、両ベクトルが一致するようにマニピュレータを
制御するマニピュレータの制御方法にある。

【００１３】本発明方法の第６の特徴は、前記本発明方
法の第１、第２、第３、第４又は第５の特徴における前
記３Ｄマーカーが、斜方６面体あるいは直方体の同一平
面上にない４つ以上の頂点に参照パターンが配置されて
なるマニピュレータの制御方法にある。

【００１４】本発明方法の第７の特徴は、前記本発明方
法の第３、第４、第５又は第６の特徴における前記３Ｄ
マーカーの参照パターンの画面上の位置が、予め３Ｄマ
ーカーに取り付けられている参照パターンの画像を比較
用イメージとして記憶させておき、前記ステレオカメラ
で撮影した画像と相関をとることにより算出されるマニ
ピュレータの制御方法にある。

【００１５】本発明方法の第８の特徴は、前記本発明方
法の第１、第２、第３、第４、第５、第６又は第７の特
徴における前記マニピュレータが、産業用ロボットのア
ーム、クレーン又は消防車の梯子であるマニピュレータ
の制御方法にある。

【００１６】本発明方法の第９の特徴は、前記本発明方
法の第３、第４、第５、第６、第７又は第８の特徴にお
ける前記３Ｄマーカーの参照パターンが、模様が描かれ
たパターン板と、当該パターン板を明るくする光源とを
備えたマニピュレータの制御方法にある。

【００１７】本発明装置の第１の特徴は、アーム先端に
先端効果器が設けられたマニピュレータの制御システム
において、前記先端効果器に装備された３Ｄマーカー
と、対象物体と前記３Ｄマーカーとを同時に撮影するス
テレオカメラと、マニピュレータの操作時に前記対象物
体の特徴点と前記３Ｄマーカーの参照パターンの画面上
の位置とを追跡し、かつ前記マニピュレータの先端効果
器上に任意に選んだ参照座標系が前記対象物体上の特徴
点から定義される基準座標系に一致するように前記マニ
ピュレータを制御するコントローラと、を有するマニピ
ュレータの制御システムにある。

【００１８】本発明装置の第２の特徴は、アーム先端に
先端効果器が設けられたマニピュレータの制御システム
において、前記先端効果器に装備された３Ｄマーカー
と、対象物体と前記３Ｄマーカーとを同時に撮影するス
テレオカメラと、コントローラとを備え、前記コントロ
ーラは、前記マニピュレータの操作時に前記ステレオカ
メラからの画像情報を読み取って前記対象物体の特徴点
と前記３Ｄマーカーの参照パターンの画像面上の位置を
追尾する画像追跡器と、前記画像追跡器からの追尾情報
をもとにカメラ座標系に対する３Ｄマーカー座標系の相
対位置・姿勢を算出する３Ｄマーカー位置・姿勢推定器
と、前記３Ｄマーカー座標系の相対位置・姿勢に基づい
て前記先端効果器上の参照座標系の射影を算出し、当該
参照座標系の射影を前記ステレオカメラの左右両画面上
にかき込む参照座標描画器と、前記先端効果器上の参照
座標系の射影を前記先端効果器のカメラ座標系に対する
位置・姿勢と一対一の対応を有する特徴量ベクトルに変
換するアーム特徴量算出部と、前記画像追跡器から出力
された前記対象物体の特徴点を当該対象物体のカメラ座
標に対する位置・姿勢と一対一の対応を有する対象物特
徴量ベクトルに変換する対象物特徴量算出部と、前記ア
ーム特徴量算出部と前記対象物特徴量算出部からの出力
の差をもとにマニピュレータの先端部の速度・角速度指
令を算出するフィードバック制御器とからなるマニピュ
レータの制御システムにある。

【００１９】本発明装置の第３の特徴は、前記本発明装
置の第２の特徴における前記コントローラの画像追跡器
及び３Ｄマーカー位置・姿勢推定器の代わりに、前記ス
テレオカメラの画像を入力として前記３Ｄマーカーの参
照パターンの画像面上の位置を予め用意しておいた比較
用パターンと相関をとりつつ追尾する画像追跡器と、前
記画像追跡器からの出力をもとにカメラ座標系に対する
３Ｄマーカー座標系の相対位置・姿勢を推定する３Ｄマ
ーカー位置・姿勢推定器と、を有するマニピュレータの
制御システムにある。

【００２０】本発明装置の第４の特徴は、前記本発明装
置の第１、第２又は第３の特徴における前記３Ｄマーカ
ーが、斜方６面体あるいは直方体の同一平面上にない４
つ以上の頂点に参照パターンが配置されてなるマニピュ
レータの制御システムにある。

【００２１】本発明装置の第５の特徴は、前記本発明装
置の第１、第２、第３又は第４の特徴における前記マニ
ピュレータが、産業用ロボットのアーム、クレーン又は
消防車の梯子であるマニピュレータの制御システムにあ
る。

【００２２】本発明装置の第６の特徴は、前記本発明装
置の第１、第２、第３、第４又は第５の特徴における前
記３Ｄマーカーの参照パターンが、模様が描かれたパタ
ーン板と、当該パターン板を明るくする光源とを備えた
マニピュレータの制御システムにある。

【００２３】

【発明の実施の形態】以下、本発明の実施の形態を、そ
の装置例、方法例及び応用例に基づいて説明する。 （装置例）図１は本装置例のマニピュレータの制御シス
テムαの構成を示し、図２は本装置例で使用される３Ｄ
マーカーの構成を示す。なお、以下に述べる対象物体Ｏ
の位置・姿勢は３次元空間上での対象物体Ｏの位置（３
次元）と方向（３次元）を表す。

【００２４】本装置例で使用されるマニピュレータは、
アーム１の先端に先端効果器２が設けられている。本装
置例のマニピュレータの制御システムαは、図１に示す
ように、３Ｄマーカー３と、ステレオカメラ４と、コン
トローラ５とからなる。なお、マニピュレータは、産業
用ロボットのアームの他、クレーンや消防車の梯子等で
良く、本発明は遠隔操作するアームを備えているシステ
ムに適用することができる。

【００２５】３Ｄマーカー３は、先端効果器２に装備さ
れている。その構成は、図２に示すように、互いに直交
する長さＬｍの３本の腕６ａ、６ｂ、６ｃの先端と原点
にステレオカメラ４により追跡されるべき参照パターン
７を合計４つ設けてなる。すなわち、参照パターン７は
立方体の同一平面上にない４つの頂点（原点、ｘ軸、ｙ
軸、ｚ軸）に配置される。

【００２６】なお、３Ｄマーカー３は、直方体あるいは
斜方６面体上の頂点に参照パターン７を配置した構造に
しても良いが、カメラ座標系に対する３Ｄマーカー３の
位置・姿勢を求めるためには４つ以上の参照パターン７
を必要とする。本装置例の３Ｄマーカー３の参照パター
ン７は、市松模様のパターン板７ａと、パターン板７ａ
を明るくする発光ダイオード７ｂとを備えている。

【００２７】ステレオカメラ４は、対象物体Ｏと３Ｄマ
ーカー３を同時に撮影する。コントローラ５は、画像追
跡器８、３Ｄマーカー位置・姿勢推定器９、参照座標描
画器１０、アーム特徴量算出器１１、対象物特徴量算出
器１２、及びフィードバック制御器１３からなる。

【００２８】画像追跡器８は、マニピュレータの操作時
にステレオカメラ４からの画像情報を読み取って左右両
画面上の対象物体Ｏの特徴点と３Ｄマーカー３の参照パ
ターン７の画像面上の位置を追尾する。３Ｄマーカー位
置・姿勢推定器９は、画像追跡器８からの追尾情報をも
とにカメラ座標系に対する３Ｄマーカー座標系の相対位
置・姿勢を推定する。

【００２９】参照座標描画器１０は、予め設定された３
Ｄマーカー座標系の相対位置・姿勢情報、画像追跡器８
の出力情報、及び３Ｄマーカー位置・姿勢推定器９の出
力情報を得て先端効果器２上の参照座標系の射影を算出
し、かつ参照座標系の射影をステレオカメラ４から得ら
れた画像の左右両画面上にかき込む。アーム特徴量算出
器１１は、先端効果器２上の参照座標系の射影を先端効
果器２のカメラ座標系に対する位置・姿勢と一対一の対
応を有する特徴量ベクトルに変換する。

【００３０】対象物特徴量算出器１２は、画像追跡器８
から出力された対象物体Ｏの特徴点を対象物体Ｏのカメ
ラ座標に対する位置・姿勢と一対一の対応を有する対象
物特徴量ベクトルに変換する。フィードバック制御器１
３は、アーム特徴量算出部１１と対象物特徴量算出部１
２からの出力の差をもとにマニピュレータのアーム１の
先端部の速度・角速度指令を算出する。

【００３１】以上説明した本装置例のマニピュレータの
制御システムαによれば、図４に示すように、対象物体
Ｏと３Ｄマーカー３を同時にとらえたステレオ画像を利
用して、先端効果器２上に任意に選んだ参照座標系Σf
が対象物体Ｏ上の特徴点f1、f2、f3から定義される基準
座標系ΣB に一致するようにマニピュレータのアーム１
を制御する。ここで、図４はステレオカメラ４にてとら
えられた３Ｄマーカー３と対象物体Ｏを表す。

【００３２】（方法例）次に前記装置例を使用したマニ
ピュレータの制御方法の方法例について具体的に説明す
る。 （１）先ず、マニピュレータを操作する際にステレオカ
メラ４により撮影された左右の画像平面Ｉｍを、モニタ
画面に表示し、オペレータはモニタ画面を参照しながら
対象物体Ｏの特徴点を選択する。そして、画像平面Ｉｍ
上における対象物体Ｏの複数の特徴点から基準座標系Σ
B を定義し、先端効果器２上の参照座標系Σf の位置・
姿勢を指定し、参照座標系Σf の位置・姿勢情報を参照
座標描画器１０へ入力する。

【００３３】（２）次に、画像追跡器８にて、対象物体
Ｏの特徴点と３Ｄマーカー３の参照パターン７の画面上
の位置とを追跡する。このとき、例えば、図２に示す３
Ｄマーカー３に取り付けられている参照パターン７の画
像を比較用イメージとしてメモリしておき、入力画像と
相関をとることにより画像面上の位置を計算する。な
お、参照パターン７に含まれる２本の直交直径線を検出
しその交点として面像面上の位置を計算してもよい。

【００３４】（３）続いて、３Ｄマーカー位置・姿勢推
定器９において、画像追跡器８の出力情報をもとにカメ
ラ座標系に対する３Ｄマーカー座標系の相対位置・姿勢
を算出（推定）する。３Ｄマーカー位置・姿勢推定器９
で行われる処理は以下の通りである。

【００３５】図５は３Ｄマーカー３のステレオカメラに
よる一画像平面Ｉｍ上への射影を表す。

【外１】

【００３６】

【外２】

【００３７】

【外３】

【００３８】

【外４】

【００３９】

【外５】

【００４０】

【外６】

【００４１】

【外７】

【００４２】

【外８】

【００４３】

【数１】

【００４４】

【数２】

【００４５】

【数３】

【００４６】ここで、

【数４】

【００４７】

【外９】

【００４８】（４）次に、参照座標描画器１０が、３Ｄ
マーカー座標系の相対位置・姿勢情報、画像追跡器８の
出力情報、及び３Ｄマーカー位置・姿勢推定器９の出力
情報をもとに先端効果器２上の参照座標系の射影を算出
し、かつ参照座標系の射影をステレオカメラ４から得ら
れた画像の左右両画面上にかき込む。

【００４９】

【外１０】

【００５０】ここで、左右のカメラ座標系を改めてΣl
、Σr と定義する。Σc をΣl 、Σr と置き換えれ
ば、左右の画像平面Ｉｍ上で参照座標系Σf の射影が求
められる。それぞれの画像平面Ｉｍ上に合成された参照
座標系Σf の原点をｐ_Oi、ｘ、ｙ、ｚ軸の端点をそれぞ
れｐ_Xi、ｐ_Yi、ｐ_Zi（ｉ＝ｌ、ｒ）と定義する。

【００５１】（５）続いて、アーム特徴量算出部１１に
て先端効果器２上の参照座標系Σfの射影を先端効果器
２のカメラ座標系に対する位置・姿勢と一対一の対応を
有する特徴量ベクトルに変換するとともに、対象物特徴
量算出部１２にて画像追跡器８から出力された対象物体
Ｏの特徴点を対象物体Ｏのカメラ座標に対する位置・姿
勢と一対一の対応を有する対象物特徴量ベクトルに変換
する。そして、フィードバック制御器１３にてアーム特
徴量算出部１１と対象物特徴量算出部１２からの出力の
差をもとにマニピュレータの先端部の速度・角速度指令
を算出する。

【００５２】いま、説明の簡素化のため、マニピュレー
タのアーム１の先端効果器２は基準座標系ΣB に対して
任意の並進速度と回転角速度が実現できるよう駆動でき
るものとする。従って、ここで設計すべき視覚フィード
バック則は、対象物体Ｏの基準座標ΣB とアーム１の参
照座標Σf が画像面上で一致するような並進速度及び回
転角速度指令を生成することである。ここで、ｐ_Oi（ｉ
＝ｌ、ｒ）の１階微分を考えれば次のように与えられ
る。

【００５３】

【外１１】

【００５４】

【外１２】

【００５５】また、

【数５】

【００５６】ｐ_Yi、ｐ_Ziの１階微分も同様に求められ
る。

【外１３】

【００５７】位置決めは対象物体Ｏ上に定義された基準
座標系ΣB に先端効果器２の参照座標系Σf を画像面上
で一致させるよう行われるが、参照座標系Σf の原点及
びｘ、ｙ、ｚ軸の端点と直接対応可能な対象物体Ｏの特
徴点が存在するとは限らない。そこで、

【外１４】

【００５８】この写像は３次元空間上のある座標系のス
テレオ画像平面Ｉｍ上への射影をその座標系のカメラ座
標系に対する位置・姿勢と一対一に対応する６次元ベク
トル（特徴量ベクトル）に変換するものである。従っ
て、先端効果器２上の参照座標系Σf と対象物体Ｏ上の
基準座標系ΣB それぞれの射影に対してこの写像を行え
ば、位置決めは両者の特徴量ベクトルを一致させること
で実現される。

【００５９】ここで、先端効果器２の参照座標Σf に関
するベクトルをアーム特徴量ベクトル

【外１５】

【００６０】

【外１６】

【００６１】

【外１７】

【００６２】このとき、写像を含む全システムは次のよ
うに記述することができる。

【数６】

【００６３】ここで、

【数７】

【００６４】このとき、イメージヤコビアンＪの逆行列
を用いて、次のような視覚フィードバック制御則（フィ
ードバック制御器１３）が構成できる。

【外１８】

【００６５】

【外１９】

【００６６】（応用例）次に、本実施形態の応用例につ
いて説明する。本応用例では、図７に示すように画像追
跡器８は比較用イメージＩｍ' としてメモリしておいた
３Ｄマーカー３の参照パターンの画像を入力画像と相関
をとることにより参照パターン７の画像面上の位置を計
算するものとする。この場合、参照パターン７の画像平
面Ｉｍ上への射影７’は最初は比較用イメージＩｍ' と
一致しているが、マニピュレータのアーム１の動きに応
じて変化することになる。

【００６７】一方、前述した３Ｄマーカー位置・姿勢推
定器９を構成すればカメラ座標系に対するマーカー座標
系の位置・姿勢を計算できるため、これを用いて比較用
イメージＩｍ' をそのときの３Ｄマーカー３の位置・姿
勢で参照パターン７が画像平面Ｉｍ上につくる射影を逐
次求めてこれを比較用パターンとすることができる。あ
るいは、比較用パターンで相関をとって参照パターン７
の位置を求めた後、その位置近傍の画像を新たな比較用
イメージとして逐次更新してもよい。

【００６８】また、３Ｄマーカー３の４つの参照パター
ン７のステレオカメラによる観測が困難になる場合に
は、３Ｄマーカー位置・姿勢推定器９からの情報により
３Ｄマーカー３あるいは参照パターンの向き（例えば図
７のθ）を変更すればよい。ただし、先端効果器２の参
照座標系Σf の位置・姿勢は変化させないため、参照座
標Σf を画像面上に描画する場合にはマーカー座標系に
対する参照座標系Σf の位置・姿勢情報を変更して計算
する。

【００６９】以上本実施形態の代表的な装置例、方法例
及び応用例について説明したが、本実施形態は必ずしも
当該装置例の手段及び当該方法例の手法等だけに限定さ
れるものではない。本実施形態は、その目的を達成し、
後述する効果を有する範囲内において適宜変更して実施
することができるものである。

【００７０】

【発明の効果】以上説明したように、本発明は、ステレ
オカメラでとらえた対象物体に対して所望の相対位置・
姿勢に先端効果器を操作するに当り、先端効果器の位置
・姿勢を画像認識し易い３Ｄマーカーにて立体的にとら
えて誤差の修正をフィードバック制御にて行うので、対
象物体上に定義された基準座標系に対して先端効果器を
相対的に並進・回転運動させることが可能になり、しか
も効率的に柔軟性の高い位置決め作業を行うことができ
るという効果を奏する。したがって、オペレータは目測
や経験に頼らず適切にマニピュレータのアームの位置決
めを行うことができる。

【図面の簡単な説明】

【図１】本発明の装置例のマニピュレータの制御システ
ムの構成を示したブロック図である。

【図２】同上の装置例にて用いる３Ｄマーカーの構成を
示した説明図である。

【図３】３Ｄマーカーの参照パターンの構成を示した概
略図である。

【図４】ステレオカメラにより観測された３Ｄマーカー
と対象物体を示した説明図である。

【図５】３Ｄマーカーのステレオカメラによる一画像平
面上への射影を表した説明図である。

【図６】ステレオカメラと３Ｄマーカーとの間の相対位
置推定の過程を表した説明図である。

【図７】マニピュレータのアームとステレオカメラの相
対位置関係により参照パターンの射影が変化する様子を
表した説明図である。

【符号の説明】

α…マニピュレータの制御システム １…アーム ２…先端効果器 ３…３Ｄマーカー ４…ステレオカメラ ５…コントローラ ６ａ、６ｂ、６ｃ…腕 ７…参照パターン ７ａ…パターン板 ７ｂ…発光ダイオード ７’…参照パターンの射影 ８…画像追跡器 ９…３Ｄマーカー位置・姿勢推定器 １０…参照座標描画器 １１…アーム特徴量算出器 １２…対象物特徴量算出器 １３…フィードバック制御器 Ｉｍ…画像平面 Ｉｍ' …比較用イメージ Σf …参照座標系 ΣB …基準座標系 Ｆ…焦点 Ｏ…対象物体 Ｒ…光軸

Claims (15)

【特許請求の範囲】
1. 【請求項１】対象物体に対するマニピュレータの位置・
   姿勢を操作するに当り、 アーム先端の先端効果器を当該対象物体との相対関係で
   適正な位置・姿勢に位置決め制御する際、当該相対関係
   の位置・姿勢の撮影と射影幾何等解析を容易にするため
   前記先端効果器に一体取付けた３Ｄマーカーの位置・姿
   勢の撮影と射影幾何等解析を通して当該先端効果器の位
   置・姿勢を間接的に算出処理する、 ことを特徴とするマニピュレータの制御方法。
2. 【請求項２】前記対象物体と前記先端効果器との相対関
   係の撮影は、 ステレオカメラにて実行する、 ことを特徴とする請求項１に記載のマニピュレータの制
   御方法。
3. 【請求項３】前記対象物体と前記先端効果器との相対関
   係の射影幾何等解析は、 前記ステレオカメラの画像の前記対象物体の特徴点及び
   前記３Ｄマーカーの参照パターンの座標位置を数値的に
   割出して所定関係式を通し算出処理する、 ことを特徴とする請求項１又は２に記載のマニピュレー
   タの制御方法。
4. 【請求項４】対象物体に対するマニピュレータの位置・
   姿勢を操作するに当り、 予め３Ｄマーカーが装備された先端効果器を前記マニピ
   ュレータのアーム先端に設置するとともに、ステレオカ
   メラを用意して置き、 当該ステレオカメラで前記３Ｄマーカーと前記対象物体
   とを同時に撮影して、前記対象物体の特徴点と前記３Ｄ
   マーカーの参照パターンの画面上の位置とを追跡し、 前記マニピュレータの先端効果器上に任意に選んだ参照
   座標系が前記対象物体上の特徴点から定義される基準座
   標系に一致するように前記マニピュレータのアームの位
   置決めを制御する、 ことを特徴とするマニピュレータの制御方法。
5. 【請求項５】対象物体に対するマニピュレータの位置・
   姿勢を操作するに当り、 予め３Ｄマーカーが装備された先端効果器をマニピュレ
   ータのアーム先端に設置するとともに、当該３Ｄマーカ
   ーと前記対象物体とを同時に撮影するステレオカメラを
   用意して置き、 先ず、前記ステレオカメラの画像面上における前記対象
   物体の複数の特徴点から基準座標系を定義し、かつ前記
   先端効果器上の参照座標系の位置・姿勢を指定し、 次に前記対象物体の特徴点と前記３Ｄマーカーの参照パ
   ターンの画面上の位置とを追跡し、前記ステレオカメラ
   による左右両画面上の前記３Ｄマーカーの参照パターン
   の位置をカメラ座標系にて求めた後、 前記カメラ座標系に対する３Ｄマーカー座標系の相対位
   置・姿勢を算出する一方、前記３Ｄマーカー座標系の相
   対位置・姿勢に基づいて前記先端効果器上の参照座標系
   の射影を算出し、当該算出した前記参照座標系の射影を
   前記ステレオカメラの左右両画面上にかき込み、 続いて、前記先端効果器上の参照座標系の射影画像を６
   次元のアーム特徴量ベクトルに写像する一方、前記対象
   物体の基準座標系の射影画像を６次元の対象物特徴量ベ
   クトルに写像してアーム特徴量ベクトル及び対象物特徴
   量ベクトルを求めた後、両ベクトルが一致するようにマ
   ニピュレータを制御する、 ことを特徴とするマニピュレータの制御方法。
6. 【請求項６】前記３Ｄマーカーは、 斜方６面体あるいは直方体の同一平面上にない４つ以上
   の頂点に参照パターンが配置されてなる、 ことを特徴とする請求項１、２、３、４又は５に記載の
   マニピュレータの制御方法。
7. 【請求項７】前記３Ｄマーカーの参照パターンの画面上
   の位置は、 予め３Ｄマーカーに取り付けられている参照パターンの
   画像を比較用イメージとして記憶させておき、前記ステ
   レオカメラで撮影した画像と相関をとることにより算出
   する、 ことを特徴とする請求項３、４、５又は６に記載のマニ
   ピュレータの制御方法。
8. 【請求項８】前記マニピュレータは、 産業用ロボットのアーム、クレーン又は消防車の梯子で
   ある、 ことを特徴とする請求項１、２、３、４、５、６又は７
   に記載のマニピュレータの制御方法。
9. 【請求項９】前記３Ｄマーカーの参照パターンは、 模様が描かれたパターン板と、当該パターン板を明るく
   する光源とを備えた、 ことを特徴とする請求項３、４、５、６、７又は８に記
   載のマニピュレータの制御方法。
10. 【請求項１０】アーム先端に先端効果器が設けられたマ
    ニピュレータの制御システムにおいて、 前記先端効果器に装備された３Ｄマーカーと、 対象物体と前記３Ｄマーカーとを同時に撮影するステレ
    オカメラと、 マニピュレータの操作時に前記対象物体の特徴点と前記
    ３Ｄマーカーの参照パターンの画面上の位置とを追跡
    し、かつ前記マニピュレータの先端効果器上に任意に選
    んだ参照座標系が前記対象物体上の特徴点から定義され
    る基準座標系に一致するように前記マニピュレータを制
    御するコントローラと、を有する、 ことを特徴とするマニピュレータの制御システム。
11. 【請求項１１】アーム先端に先端効果器が設けられたマ
    ニピュレータの制御システムにおいて、 前記先端効果器に装備された３Ｄマーカーと、対象物体
    と前記３Ｄマーカーとを同時に撮影するステレオカメラ
    と、コントローラとを備え、 前記コントローラは、 前記マニピュレータの操作時に前記ステレオカメラから
    の画像情報を読み取って前記対象物体の特徴点と前記３
    Ｄマーカーの参照パターンの画像面上の位置を追尾する
    画像追跡器と、前記画像追跡器からの追尾情報をもとに
    カメラ座標系に対する３Ｄマーカー座標系の相対位置・
    姿勢を算出する３Ｄマーカー位置・姿勢推定器と、前記
    ３Ｄマーカー座標系の相対位置・姿勢に基づいて前記先
    端効果器上の参照座標系の射影を算出し、当該参照座標
    系の射影を前記ステレオカメラの左右両画面上にかき込
    む参照座標描画器と、前記先端効果器上の参照座標系の
    射影を前記先端効果器のカメラ座標系に対する位置・姿
    勢と一対一の対応を有する特徴量ベクトルに変換するア
    ーム特徴量算出部と、前記画像追跡器から出力された前
    記対象物体の特徴点を当該対象物体のカメラ座標に対す
    る位置・姿勢と一対一の対応を有する対象物特徴量ベク
    トルに変換する対象物特徴量算出部と、前記アーム特徴
    量算出部と前記対象物特徴量算出部からの出力の差をも
    とにマニピュレータの先端部の速度・角速度指令を算出
    するフィードバック制御器とからなる、 ことを特徴とするマニピュレータの制御システム。
12. 【請求項１２】前記コントローラの画像追跡器及び３Ｄ
    マーカー位置・姿勢推定器の代わりに、 前記ステレオカメラの画像を入力として前記３Ｄマーカ
    ーの参照パターンの画像面上の位置を予め用意しておい
    た比較用パターンと相関をとりつつ追尾する画像追跡器
    と、前記画像追跡器からの出力をもとにカメラ座標系に
    対する３Ｄマーカー座標系の相対位置・姿勢を推定する
    ３Ｄマーカー位置・姿勢推定器とを有する、 ことを特徴とする請求項１１に記載のマニピュレータの
    制御システム。
13. 【請求項１３】前記３Ｄマーカーは、 斜方６面体あるいは直方体の同一平面上にない４つ以上
    の頂点に参照パターンが配置されてなる、 ことを特徴とする請求項１０、１１又は１２に記載のマ
    ニピュレータの制御システム。
14. 【請求項１４】前記マニピュレータは、 産業用ロボットのアーム、クレーン又は消防車の梯子で
    ある、 ことを特徴とする請求項１０、１１、１２又は１３に記
    載のマニピュレータの制御システム。
15. 【請求項１５】前記３Ｄマーカーの参照パターンは、 模様が描かれたパターン板と、当該パターン板を明るく
    する光源とを備えた、 ことを特徴とする請求項１０、１１、１２、１３又は１
    ４に記載のマニピュレータの制御システム。