JPH07146121A - 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置 - Google Patents

Abstract

(57)【要約】 【目的】 既知対象物体を撮影するカメラの位置姿勢を
高速且つ高精度でロバストに認識できるようにする。 【構成】 ワークを撮影するカメラ４からの信号により
その画像特徴点を画像処理部７で計算する。量子化空間
探索部１０の各部の計算処理により、カメラ４の位置姿
勢ベクトルｓの複数の探索パラメータに対して、画像特
徴点と予測した推定画像特徴点との間の誤差を計算して
最小値をとる局所最適値を求めることにより、各探索パ
ラメータについて逐次的独立探索を実行し、これを多重
スケールで巡回的に所定回数繰り返すことでカメラ４の
最適位置姿勢を示すデータを計算して認識する。高速且
つ高精度でロバストに位置姿勢の認識を行うことができ
る。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、自律移動ロボットなど
に搭載され単眼視カメラにより撮影した既知対象の画像
情報に基いて単眼視カメラの三次元位置および姿勢を認
識するようにした視覚に基く三次元位置および姿勢の認
識方法ならびに視覚に基く三次元位置および姿勢の認識
装置に関する。

【０００２】

【従来の技術】近年、例えば、自律移動ロボット等のよ
うに、形状および絶対座標位置が既知である対象物体に
対して接近するように移動し、アームにより対象物体を
把持するといった能動的操作を行うようにしたものがあ
る。このような自律移動ロボット等においては、ＣＣＤ
カメラなどの撮像手段を搭載してその二次元的な画像情
報に基いて対象物体に対する自己の三次元的な相対位置
を認識することが必要になる。この場合、位置認識を行
うための装置においては、高精度且つ迅速に計算を行っ
て三次元的な位置のデータを求めることが要求され、し
かも安価に実現できる構成が望まれている。

【０００３】そこで、従来では、比較的簡単なものとし
て、例えば、特開平３−１６６０７２号公報あるいは特
開平３−１６６０７３号公報に示されるようなものが考
えられている。すなわち、これらにおいては、対象装置
に固定された形状および寸法が既知の特殊な幾何学形状
（マーカと呼ぶ）をカメラにより撮影し、そのマーカの
二次元的な画像情報に基いて、その重心を計算する等の
方法により対象装置に対するカメラの相対的な三次元の
位置関係を求めるようにしたものである。

【０００４】

【発明が解決しようとする課題】しかしながら、上述の
ような従来方法によると、対象装置にマーカを設ける必
要があるため、その設置のための準備をその都度行う必
要があると共に、マーカに対してカメラの光軸を常に垂
直に設定する必要があるため、任意の姿勢をとるカメラ
に対しては適用が困難となる不具合がある。

【０００５】このような不具合を解消し得る理論的手法
として、一般には、透視ｎ点問題において最小二乗推定
した透視変換行列の成分からカメラの位置および姿勢を
表すパラメータを逆算する方法がコンピュータビジョン
の手法として存在する。この方法によれば、カメラの光
軸と対象物体を記述する絶対座標の原点との間の関係が
未知の場合でも、原理的にはカメラの位置および姿勢を
表すパラメータを求めることが可能となる。

【０００６】ところが、このようなコンピュータビジョ
ンの手法においては、実際にコンピュータを用いて明示
的に計算する場合に、適用する式が非常に複雑になるこ
とに加えてその計算量が膨大なものとなるため、迅速な
計算処理が困難になり実用的には適用が難しくなるもの
である。

【０００７】本発明は、上記事情に鑑みてなされたもの
で、その目的は、カメラの光軸と既知形状の対象物体を
記述する絶対座標の原点との間の関係が未知の場合で
も、カメラにより撮影した二次元の画像情報に基いて、
比較的簡単かつ迅速にカメラの位置と姿勢を表すパラメ
ータとを求めることができるようにした視覚に基く三次
元位置と姿勢の認識方法ならびに視覚に基く三次元位置
と姿勢の認識装置を提供することにある。

【０００８】

【課題を解決するための手段】本発明の視覚に基く三次
元位置および姿勢の認識方法は、形状を示す三次元絶対
座標が既知である対象物体を撮像手段により撮影したと
きの画像信号に基いてその対象物体の形状の特徴を示す
二次元の画像特徴点を抽出する画像処理ステップと、前
記対象物体に対する前記撮像手段の位置姿勢を示す複数
のパラメータを探索パラメータとし、外部情報により与
えられる推定距離に応じたパラメータ探索開始点を設定
すると共にその推定距離に応じたパラメータ探索幅で量
子化したパラメータ探索点を有する空間探索領域を設定
する探索領域設定ステップと、この探索領域設定ステッ
プにて設定された前記空間探索領域のパラメータ探索点
で決まる位置から前記撮像手段により前記対象物体を撮
影したときに得られるべき二次元の推定画像特徴点を算
出するための透視変換行列を、あらかじめ記憶手段に記
憶された前記対象物体の三次元絶対座標に対する前記量
子化空間点の推定位置と前記撮像手段の初期探索パラメ
ータとに基いて計算する透視変換行列計算ステップと、
この透視変換行列計算ステップにて求められた前記透視
変換行列を用いて前記パラメータ探索点における前記対
象物体の二次元の前記推定画像特徴点を算出する透視変
換計算ステップと、前記画像処理ステップにて抽出され
た前記対象物体の二次元の画像特徴点と前記透視変換計
算ステップにて算出された二次元の前記推定画像特徴点
との対応関係を求めると共に、それら対応関係が得られ
た画像特徴点と推定画像特徴点との間の適合度を示す評
価誤差値を所定の誤差評価関数に基いて計算する誤差計
算ステップと、前記複数の探索パラメータのうちの指定
された探索パラメータに対して前記探索領域内で、前記
各推定画像特徴点に対する画像特徴点の評価誤差値が最
小となるような前記パラメータ探索点を局所最適パラメ
ータとして求めて、あらたにパラメータ探索点として設
定する局所最適パラメータ計算ステップと、この局所最
適パラメータ計算ステップを、あらかじめ指定された順
序で前記複数のパラメータのすべてについて逐次実行す
ると共に、これを所定回数繰り返して実行し、そのとき
の前記評価誤差値が所定の要求誤差精度よりも小さいと
きに、得られた複数のパラメータ探索値を前記位置姿勢
ベクトルの複数のパラメータとして決定する最適位置姿
勢データ決定ステップとを有する構成としたところに特
徴を有する。

【０００９】また、本発明の視覚に基く三次元位置およ
び姿勢の認識装置は、画像信号を出力する撮像手段と、
対象物体の形状を示す三次元絶対座標データおよび探索
順序を指定するデータが記憶された記憶手段と、前記撮
像手段により前記対象物体を撮影したときの画像信号に
基いてその対象物体の形状の特徴を示す二次元の画像特
徴点を抽出する画像処理手段と、前記対象物体に対する
前記撮像手段の位置姿勢を示す複数のパラメータを探索
パラメータとし、外部情報により与えられる推定距離に
応じたパラメータ探索開始点を設定すると共にその推定
距離に応じたパラメータ探索幅で量子化したパラメータ
探索点を有する空間探索領域を設定する探索領域設定手
段と、この探索領域設定手段により設定された前記空間
探索領域のパラメータ探索点で決まる位置から前記撮像
手段により前記対象物体を撮影したときに得られるべき
二次元の推定画像特徴点を算出するための透視変換行列
を、あらかじめ記憶手段に記憶された前記対象物体の三
次元絶対座標に対する前記量子化空間点の推定位置と前
記撮像手段の初期探索パラメータとに基いて計算する透
視変換行列計算手段と、この透視変換行列計算手段によ
り求められた前記透視変換行列を用いて前記パラメータ
探索点における前記対象物体の二次元の前記推定画像特
徴点を算出する透視変換計算手段と、前記画像処理手段
により抽出された前記対象物体の二次元の画像特徴点と
前記透視変換計算手段により算出された二次元の前記推
定画像特徴点との間の対応関係を求めると共に、それら
対応関係が得られた画像特徴点と推定画像特徴点との間
の適合度を示す評価誤差値を所定の誤差評価関数に基い
て計算する誤差計算手段と、前記複数の探索パラメータ
のうちの指定された探索パラメータに対して前記探索領
域内で、前記各推定画像特徴点に対する画像特徴点の評
価誤差値が最小となるような前記パラメータ探索点を局
所最適パラメータとして求めて、あらたにパラメータ探
索点として設定する局所最適パラメータ計算手段と、こ
の局所最適パラメータ計算手段による計算処理を、あら
かじめ指定された順序で前記複数のパラメータのすべて
について逐次実行すると共に、これを所定回数繰り返し
て実行し、そのときの前記評価誤差値が所定の要求誤差
精度よりも小さいときに、得られた複数のパラメータ探
索値を前記位置姿勢ベクトルの複数のパラメータとして
決定する最適位置姿勢データ決定手段とを設けて構成し
たところに特徴を有する。

【００１０】

【作用】請求項１記載の視覚に基く三次元位置と姿勢の
認識方法によれば、以下のようにして対象物体に対する
撮像手段の位置姿勢が認識されるようになる。すなわ
ち、撮像手段により撮影された対象物体の画像信号が入
力されると、画像処理ステップにおいて、その画像信号
に基づいて対象物体の形状の特徴等を示す二次元の画像
特徴点を抽出するようになる。

【００１１】探索領域設定ステップにおいては、対象物
体に対する撮像手段の位置姿勢を示す複数のパラメータ
を探索パラメータとし、外部情報により与えられる推定
距離に応じてそれらの探索パラメータに対するパラメー
タ探索開始点を設定すると共にその推定距離に応じて決
められるパラメータ探索幅で量子化したパラメータ探索
点を有する空間探索領域を設定するようになる。

【００１２】透視変換行列計算ステップにおいては、上
述の探索領域設定ステップにて設定された空間探索領域
のパラメータ探索点で決まる位置から前記撮像手段によ
り前記対象物体を撮影したときに得られるべき二次元の
推定画像特徴点を算出するための透視変換行列を計算に
より求める。このとき、透視変換行列は、あらかじめ記
憶手段に記憶された対象物体の三次元絶対座標に対する
パラメータ探索点の推定位置と撮像手段のパラメータ探
索開始点とに基いて計算される。

【００１３】誤差計算ステップにおいては、前述の画像
処理ステップにて抽出された対象物体の二次元の画像特
徴点と透視変換計算ステップにて算出された二次元の推
定画像特徴点との対応関係を求めると共に、それら対応
関係が得られた画像特徴点と推定画像特徴点との間の適
合度を示す評価誤差値を所定の誤差評価関数に基いて計
算するようになる。

【００１４】局所最適パラメータ計算ステップにおいて
は、複数の探索パラメータのうちの指定された探索パラ
メータに対して設定されている探索領域内で、各推定画
像特徴点に対する画像特徴点の評価誤差値が最小となる
ようなパラメータ探索点を局所最適パラメータとして求
めて、あらたにパラメータ探索点として設定するように
なる。

【００１５】最適位置姿勢データ決定ステップにおいて
は、上述の局所最適パラメータ計算ステップを、あらか
じめ指定された順序で複数の探索パラメータのすべてに
ついて逐次実行すると共に、これを所定回数繰り返して
実行し、そのときの評価誤差値が所定の要求誤差精度よ
りも小さいときに、得られた複数のパラメータ探索値を
撮像手段の位置姿勢を示す複数のパラメータとして決定
するようになる。

【００１６】この場合、三次元絶対座標が既知である対
象物をある一つの見え方の位置姿勢の範囲内で撮像手段
により撮影している状態では、上述の各ステップの実行
により、撮像手段の位置姿勢を複数のパラメータで表し
た位置姿勢ベクトルに対して、それぞれのパラメータを
逐次独立して探索することを多重スケールで巡回的に所
定回数だけ実施することになるので、最適な位置姿勢を
探索できる確率が非常に高くなり、少ない探索回数であ
りながら高速且つ高精度で撮像手段の位置姿勢を認識で
きるようになるものである。

【００１７】請求項２記載の視覚に基く三次元位置と姿
勢の認識装置によれば、上述の各処理ステップを実行す
る手段を備えているので、それらの各手段により上述の
処理ステップを実行することにより、少ない探索回数で
ありながら高速且つ高精度で撮像手段の位置姿勢を認識
できるようになるものである。

【００１８】

【実施例】以下、本発明を工場内などの軌道上を走行す
る自律移動ロボットに搭載する三次元位置認識装置に適
用した場合の第１の実施例について図１ないし図１０を
参照しながら説明する。

【００１９】全体構成の概略を示す図２において、自律
移動ロボット１は、例えば、工場内などの床面に設けら
れた軌道に沿って作業エリア２間を移動するようになっ
ており、本発明の三次元位置認識装置（図１参照）が搭
載されている。自律移動ロボット１の上部にはアーム３
が設けられ、そのアーム３の先端部には撮像手段として
のカメラ４および対象物体としてのワーク５を把持する
等の作業を行うためのハンド６が配設されている。この
場合、作業エリア２は壁面から窪んだ状態に形成された
凹部に設けられ、所定形状をなすワーク５はその作業エ
リア２の底面２ａの所定位置に載置されている。

【００２０】さて、三次元位置認識装置は図１に示すよ
うに構成される。画像処理手段としての画像処理部７
は、前処理計算部８および特徴点抽出部９からなる。前
処理計算部８は、カメラ４が撮影したワーク５を含むシ
ーンの画像信号を入力して二値化処理等の前処理計算を
行い、特徴点抽出部９は、前処理計算部８により計算さ
れた画像情報に基いて、後述のように、ワーク５の特徴
点に対応する二次元の画像特徴点を抽出するようになっ
ている。

【００２１】量子化空間探索部１０は、探索領域設定手
段としての探索領域決定部１１，透視変換行列計算手段
としての透視変換行列計算部１２，透視変換計算手段と
しての透視変換部１３，誤差計算手段としての誤差評価
関数計算部１４，局所最適パラメータ計算手段および最
適位置姿勢データ決定手段としての最適値決定部１５そ
して、決定された最適位置姿勢データを出力する最終出
力部１６から構成されている。

【００２２】この場合、探索領域決定部１１は、作業エ
リア２のワーク５に対する自律移動ロボット１の概略位
置情報に基いてカメラ４の位置姿勢ベクトルｓ〔γ，
θ，φ，ρ〕の４個（ｈ＝１，２，３，４）のパラメー
タを探索パラメータとして、それらのパラメータ探索開
始点ｓ（１，１），パラメータ探索幅δ（ｋ，ｈ）およ
びパラメータ探索領域を設定する（ｋ（＝１，２，…，
Ｋ）は繰返し探索回数を示す）。なお、これらのパラメ
ータγ，θ，φ，ρについては作用の説明にて詳述す
る。

【００２３】透視変換行列計算部１２は、設定されたパ
ラメータ探索領域のパラメータ探索点ｓ（ｋ，ｈ）にお
いてカメラ４によりワーク５を撮影したときに画像信号
から得られる画像内における画像特徴点の座標を求める
ための透視変換行列Ｍを、自律移動ロボット１の概略位
置情報およびカメラ４の姿勢パラメータに応じて計算す
る。透視変換計算部１３は、求められた透視変換行列Ｍ
を用いてパラメータ探索点ｓ（ｋ，ｈ）のそれぞれの位
置でカメラ４によりワーク５を撮影したときにその特徴
点に対応して得られるべき二次元の推定画像特徴点を計
算する。

【００２４】誤差評価関数計算部１４は、画像処理部７
にて抽出された画像特徴点と前記推定画像特徴点との対
応関係をつけると共に、それらの間の位置の誤差値を二
乗誤差和を求める誤差評価関数Ｄに基いて計算する。最
適値決定部１５は、一つの探索パラメータに対する最適
値を局所最適パラメータとして決定すると共に、この局
所最適パラメータをあらかじめ指定された順序で位置姿
勢ベクトルの各パラメータに対して逐次計算し、これを
所定回数繰り返して実行させることにより、位置姿勢ベ
クトルの複数のパラメータを決定する。

【００２５】また、量子化空間探索部１０には、記憶手
段としてのメモリ１７が接続され、その各種データの書
き込みおよび読み出し動作が行えるようになっている。
この場合、メモリ１７にあらかじめ記憶されるデータと
しては、特徴点マッチングリスト，ワーク５および作業
エリア２の三次元絶対座標データ，カメラ４の焦点距離
ｆ，パラメータ探索点およびパラメータ探索幅のリスト
とその座標データおよび位置姿勢パラメータの探索順序
指定情報などである。

【００２６】また、外部位置情報出力部１８は、カメラ
４の外部位置情報を示すデータを量子化空間探索部１０
に入力するもので、自律移動ロボット１の走行制御を行
う図示しない制御装置あるいは図示しない位置検出セン
サなどから自律移動ロボット１の概略位置を示す信号が
入力されるようになっている。

【００２７】次に、本実施例の作用について図３ないし
図１０をも参照して説明する。三次元位置認識装置は、
その動作の流れとして図３に示すようなフローチャート
の各処理ステップにしたがって自律移動ロボット１の存
在位置を認識する。まず、図２の状態において、自律移
動ロボット１が軌道上を移動してきて作業エリア２に差
し掛かると、カメラ４の撮影シーン内に作業エリア２が
入って撮影されるようになり、画像処理部７は、カメラ
４の画像信号を入力するようになる（ステップＴ１）。

【００２８】この場合、自律移動ロボット１は、所定軌
道上を所定方向に移動しており、作業エリア２に近付く
と、停止位置を指示するセンサからの信号や移動制御情
報等に基づいて、所定の停止領域Ｅ内に停止するように
なっているので、この停止領域Ｅ内に停止した状態では
カメラ４により作業エリア２のワーク５を撮影したとき
に、その視野内にワーク５が十分収まるように設定され
ている。

【００２９】次に、カメラ４により撮影されたワーク５
の画像信号は、画像処理部７の前処理計算部８にて前処
理としての雑音除去処理（ステップＴ２）およびワーク
５の画像の切り出し処理（ステップＴ３）を実行し、こ
の後、特徴点抽出部９にてその画像信号に基いて次のよ
うにしてｍ個の画像特徴点Ｑｊの集合Ｑを求めて出力す
るようになる（ステップＴ４）。ここで、画像特徴点Ｑ
ｊの集合Ｑは式（１），（２）のように定義される。

【００３０】

【数１】

【００３１】この画像特徴点Ｑｊの抽出に際しては、外
部情報出力部１９から与えられる概略位置情報に基いて
次のように計算を行う。すなわち、概略位置情報とし
て、例えば、自律移動ロボット１の移動経路を記憶した
作業計画，走行制御により移動した空間移動の履歴情報
などが入力されるので、これらの外部情報に基いて、自
律移動ロボット１の中心位置の概略的な三次元絶対座標
を得ることができる。そして、自律移動ロボット１の中
心位置に対するカメラ４の相対位置はアーム３の制御位
置情報に基づいて得ることができるから、カメラ４の概
略位置を決定することができる。これにより得られたカ
メラ４の概略位置情報に基づいて、以降の探索過程にお
ける基準となる探索領域として初期探索領域Ｓ（１）を
設定することができる。

【００３２】次に、ステップＴ５，Ｔ６において、探索
領域決定部１１により、カメラ４の各種パラメータの探
索範囲を設定する。ここで、カメラ４の状態を示すパラ
メータとしては、図２および図４に対応関係を示してい
るように、（ａ）カメラ４の姿勢パラメータＣＰ（α，
β，γ），（ｂ）カメラ４の焦点距離特性ｆ，（ｃ）カ
メラ４の位置ベクトル（ｒ，θ，φ）がある。

【００３３】この場合、（ｂ）に示すカメラ４の焦点距
離ｆは、カメラ４の特性によりあらかじめ決まっている
データであり、メモリ１７内に焦点距離データとしてあ
らかじめ記憶されている。姿勢パラメータＣＰの各パラ
メータα，β，γは、カメラ４の光軸をｚ軸，水平方向
をｘ軸そして垂直方向をｙ軸としたときの、ｚ軸周りの
回転角度γ，ｘ軸周りの回転角度α，そしてｙ軸周りの
回転角度βとなる値である。また、カメラ４の位置パラ
メータは、三次元絶対座標系の原点に対するカメラ４の
視点の位置を極座標で示すもので、ｒは距離、θ，φは
ｒの傾き角度を示した値である。

【００３４】そして、カメラ４の姿勢パラメータＣＰの
うち、光軸ずれを示す姿勢パラメータαおよびβについ
ては、対象物の中心位置の撮影画面上の座標データに基
づいて求めることができる。すなわち、図５に示すよう
に、撮影画面上でのワーク５の座標系の原点Ｏｐに対し
て、撮影画面上での画像位置Ｏｐｓ（画像情報に含まれ
ていない場合には含まれている特徴点を原点とする座標
系に変換する）が画素単位（ピクセル値）で示した二次
元座標（ｕ，ｖ）において、例えば（Ｎａ，Ｎｂ）であ
るとすると、次に示す式（３），（４）より姿勢パラメ
ータαおよびβの推定値が計算できる。

【００３５】この場合、式（３）に示すαの値に cosβ
の要素が入るのは、初めにｙ軸周りに角度βだけ回転し
たときに、ワーク５の座標系の原点Ｏｐが遠ざかること
によって撮影画面上の画像位置Ｏｐｓの二次元座標デー
タＮａが変動するのを補正しているためである。

【００３６】

【数２】

【００３７】この結果、カメラ４の位置姿勢を示すカメ
ラパラメータとしては、残った未知の４個のパラメータ
γ，θ，φ，ｒを要素とした４次元の位置姿勢ベクトル
ｓ〔γ，θ，φ，ｒ〕を想定し、これらのパラメータを
探索パラメータとして設定すれば良いことになる。な
お、以降の説明においては、パラメータγとパラメータ
ｒとの符号の表記上の混同を避けるために、本明細書中
の表記を符号「ｒ」に代えて符号「ρ」として示し、し
たがって、上述の位置姿勢ベクトルｓ〔γ，θ，φ，
ｒ〕に対して位置姿勢ベクトルｓ〔γ，θ，φ，ρ〕に
置き換えて示す。（図２，図４，図６においては、
「ｒ」のまま表記した。）次に、探索すべき探索パラメ
ータを指定するための変数ｈを「１」に設定して（ステ
ップＴ７）はじめの探索パラメータを「γ」に指定し
（ステップＴ８）、探索領域Ｓ（１）内の１点を探索パ
ラメータγに対するパラメータ探索開始点ｓ（１、１）
として設定する。このパラメータ探索開始点ｓ（１，
１）は、パラメータ探索点ｓ（ｋ，ｈ）の初期値に対応
するものであり、ｋは繰り返し探索回数を示している。

【００３８】また、設定されたパラメータ探索開始点ｓ
（１，１）においては、その探索パラメータγに対する
パラメータ探索幅δ（１，１）が設定され（ステップＴ
１０，Ｔ１１）、探索過程が実行されるようになってい
る。このとき、探索範囲内での探索パラメータの値の変
更は、変数Ｉを順次指定することにより、パラメータ探
索幅δだけ変化されるようになっている。

【００３９】次に、透視変換行列計算部１２において、
パラメータ探索開始点ｓ（１，１）の三次元位置座標お
よび前述の式（３），（４）で求めた姿勢パラメータα
およびβの値，探索パラメータγの初期設定値で作られ
る初期姿勢を用いて、透視変換行列Ｍ（１，１）を計算
する（ステップＴ１２）。

【００４０】この場合、透視変換行列Ｍは、次のように
して計算される。すなわち、まず、ワーク５の原点Ｏｐ
に対して設定されている三次元絶対座標（Ｘ，Ｙ，Ｚ）
を基準として、初期探索領域Ｓ（１）を設定する。この
初期探索領域Ｓ（１）は、自律移動ロボット１の停止領
域Ｅに基づいて設定されるもので、カメラ４の予定撮像
位置である概略位置Ａ０（Ｘ０，Ｙ０，Ｚ０）を極座標
で表した概略位置Ａ０（ρ０，θ０，φ０）を中心とし
て、停止位置精度に対応する位置探索領域として、 ρ０−Δρ１≦ρ≦ρ０＋Δρ２ θ０−Δθ１≦θ≦θ０＋Δθ２ φ０−Δφ１≦φ≦φ０＋Δφ２ の範囲に相当するように設定される。また、姿勢探索領
域に対しては、パラメータαおよびβについては式
（３），（４）で求めた結果を用い、パラメータγにつ
いては、 −Δγ１≦γ≦Δγ２ の範囲に相当するように設定される。

【００４１】そして、カメラ４の姿勢パラメータＣＰ
（α，β，γ）と、焦点距離特性ｆから求めたカメラ４
の視点位置と射影平面に対する距離ｄとに基いて、図６
（「ρ」は「ｒ」として表記している）に示す関係に基
いて透視変換行列Ｍを求める。ここで、透視変換行列Ｍ
は次式（５）のように表すことができる。

【００４２】

【数３】

【００４３】このうち、画像の表示面とワーク５との間
の透視変換を計算するための透視変換行列Ｍ１とＭ２と
の積および座標変換行列Ｍ３は、それぞれ、次式
（６），（７）のように表すことができる。

【００４４】

【数４】

【００４５】また、カメラ４の光軸ずれに対応する姿勢
パラメータＣＰ（α，β，γ）に応じた回転の計算を行
うための変換行列Ｔは、前述の図４（「ρ」は「ｒ」と
して表記している）と図６に示す関係とに基いて、γ，
αそしてβの順に回転する回転変換行列として次式
（８），（９），（１０）のように表すことができる。

【００４６】

【数５】

【００４７】なお、上述の場合、透視変換行列Ｍの計算
過程においては、以下のようにして演算量の削減を図る
ことができる。すなわち、式（５）中の変換行列Ｔに式
（８）を代入し、ここで、（Ｔ１・Ｍ１・Ｍ２）で示さ
れた要素をＭＴと置くと式（５Ａ）のようになる。この
とき、要素ＭＴの値は、式（６）および式（８）から計
算すると式（５Ｂ）のように単純な形の式に変形でき
る。したがって、式（５Ｂ）を式（５Ａ）に代入する
と、透視変換行列Ｍは式（５Ｃ）のようになり、実際の
計算にあたっては、この式（５Ｃ）に基づいて行うこと
により演算量を少なくして演算速度の向上を図ることが
できる。

【００４８】

【数６】

【００４９】次に、ステップＴ１３に進むと、このよう
にして計算により求めた透視変換行列Ｍ（１，１）に基
いて、透視変換計算部１３により、作業エリア２とそこ
に載置されたワーク５のｎ個の特徴点Ｐｉの集合Ｐの各
特徴点Ｐｉについて三次元絶対座標データを透視変換の
計算を行って二次元の画像表示部の座標を求め、式（１
１），式（１２）に示すような各特徴点Ｐｉに対する推
定画像特徴点Ｑｅｉの集合Ｑｅを求める。そして、斉次
座標を用いると、特徴点Ｐｉの集合Ｐから推定画像特徴
点Ｑｅｉの集合Ｑｅへの変換は、式（５）あるいは単純
化した式（５Ｃ）を用いると、式（１３）のように表す
ことができる。

【００５０】

【数７】

【００５１】次に、画像処理部７にて抽出されたカメラ
４により撮影されたワーク５の画像特徴点Ｑｊの集合Ｑ
（式（１）参照）の各画像特徴点Ｑｊと、あらかじめ記
憶されたワーク５の三次元の特徴点Ｐｉ（式（４）参
照）との間の対応関係をとった特徴点対応リストを選択
する（ステップＴ１４）。この場合、図７（ａ）に示す
ワーク５の特徴点Ｐｉの位置と、図８（ａ）〜（ｆ）に
示す種々の見え方パターンとの対比から、各画像特徴点
Ｑｊを対応付けると、図７（ｂ）に示すように、画像情
報として撮影画面上の画像特徴点Ｑｊと対応する特徴点
Ｐｉとの対応関係が得られる。そして、以上のような対
応付けの結果に基づいて、ワーク５の見え方のパターン
を決定する。

【００５２】例えば、図８に示す見え方パターンの
（ｂ）に相当する特徴点対応リストが得られた場合に
は、見え方パターン（ｂ）の対応関係の範囲内で、例え
ば、図９（ａ）〜（ｃ）に示すように、画像特徴点Ｑｊ
の位置が推定特徴点Ｑｅｉに対して小さく見える場合
（ａ）、偏って見える場合（ｂ）、傾いて見える場合
（ｃ）などの種々の状態が想定される。つまり、以下の
探索過程においては、図９（ａ）〜（ｃ）などのよう
な、同じ見え方パターンの状態におけるワーク５の形状
のずれを探索することになるのである。

【００５３】続いて、誤差評価関数計算部１４により、
対応関係が得られた画像特徴点Ｑｊと前述の推定画像特
徴点Ｑｅｉの集合Ｑｅ（＝Ｐ・Ｍ）との間の誤差評価関
数Ｄ（１，１）を計算する（ステップＴ１５）。この場
合、誤差評価関数Ｄ（ｋ，ｈ）は、画像特徴点Ｑｊと推
定画像特徴点Ｑｅｉとの間の距離の二乗誤差和に相当す
る値を計算するものである。

【００５４】続いて、ステップＴ１６，Ｔ１７を経て、
探索パラメータγを設定したパラメータ探索範囲内でそ
のパラメータ探索幅δ（１，１）の間隔で量子化された
各パラメータ探索点について上述のステップＴ１１ない
しＴ１５を繰り返し実行することにより探索過程を進め
る。

【００５５】そして、得られた誤差評価関数の値Ｄ
（１，１）の最小値となる局所最適値Ｄmin （１，１）
に対応するパラメータ探索点における探索パラメータγ
の値に基づいて、このときのパラメータ探索範囲におけ
るパラメータ探索点をｓ（１，２）とするようになる
（ステップＴ１８）。なお、この計算過程においては、
探索パラメータであるγの値以外にはパラメータθ，
φ，ρの値をパラメータ初期設定値のまま固定して行う
独立探索として実行するようになっている。

【００５６】続いて、いま求めた位置姿勢のパラメータ
探索点ｓ（１，２）を新たなパラメータ探索開始点とし
て、ステップＴ１９，Ｔ２０にて次の探索パラメータを
θ（ｈ＝２）に設定し、上述のステップＴ８ないしＴ１
７を繰り返すことにより、探索パラメータθについての
局所最適値Ｄmin （１，２）を計算する。

【００５７】この場合、探索パラメータθの値は、θの
パラメータ探索範囲内でパラメータ探索幅をδ（１，
２）として量子化されたパラメータ探索点について上述
のステップＴ８ないしＴ１７を実行することにより計算
される。そして、ステップＴ１８にて探索パラメータθ
についての局所最適値Ｄmin （１，２）が得られると、
これに対応するパラメータ探索点をｓ（１，３）とす
る。

【００５８】以下、探索パラメータφおよびρについて
も、同様にして上述のステップＴ８〜Ｔ１８を繰り返し
実行する。そして、探索パラメータφの局所最適値Ｄmi
n （１，４）に対するパラメータ探索点ｓ（１，４）が
求められた後、探索パラメータρの局所最適値Ｄmin
（２，１）が求められると、このときのパラメータ探索
点ｓ（２，１）は、すべての探索パラメータγ，θ，
φ，ρについて１回の局所最適値Ｄmin を求めたことに
なる。

【００５９】したがって、次にステップＴ１９になる
と、こんどはステップＴ２１に移行することになり、繰
り返し探索回数ｋの値が設定値Ｋに達するまでの間はス
テップＴ２２に移行し、再び上述のステップＴ７〜Ｔ２
２の探索過程を繰り返すようになる。

【００６０】この場合、探索領域Ｓ（ｋ）は、上述の探
索領域Ｓ（１）から新たな探索領域Ｓ（２）に設定され
るようになっている。つまり、各探索パラメータγ，
θ，φおよびρについて１回の探索過程を実行したこと
により、２回目には探索範囲を絞り込んでその範囲内で
精度を上げた狭い幅のパラメータ探索幅δ（２，ｈ）を
設定することになるのである。

【００６１】また、すべての探索パラメータγ，θ，
φ，ρについて順次１回の局所最適値Ｄmin （２，１）
として得られたパラメータ探索点ｓ（２，１）を２回目
の探索過程におけるパラメータ探索開始点ｓ（２，１）
として新たに設定して同様の計算処理を実行するように
なっている。したがって、各探索パラメータγ，θ，
φ，ρについて逐次的且つ独立に探索過程が実行され、
これを巡回的に所定回数Ｋ回繰り返すという計算が行わ
れるのである。

【００６２】ところで、このような探索過程の概念を、
図１０を用いて簡単に説明する。なお、本実施例におい
ては、位置姿勢ベクトルｓ〔γ，θ，φ，ρ〕が４個の
パラメータを持っているので、探索領域空間が４次元と
なって図示する上で無理が生ずるため、ここでは、便宜
的に位置姿勢ベクトルｓを３個の探索パラメータθ，
φ，ρを有する位置姿勢ベクトルｓ〔θ，φ，ρ〕とし
て設定した場合を例にとって説明する。

【００６３】すなわち、図１０においては、位置姿勢ベ
クトルｓ〔θ，φ，ρ〕の各探索パラメータθ，φ，ρ
をそれぞれ探索軸であるθ軸，φ軸，ρ軸とし、三次元
空間内で直交する座標軸として示している。また、この
座標系の原点をパラメータ探索開始点ｓ（１，１）とす
る。

【００６４】そして、まず、探索パラメータφ，ρを初
期探索点の値のまま固定し、探索パラメータθについて
のみ独立に探索過程を実行してそのときの誤差評価値Ｄ
minが最小となるときの位置姿勢ベクトルのパラメータ
探索点ｓ（１，２）を計算する。つまり、θ軸方向（図
中矢印（イ）で示す）に沿って所定のパラメータ探索幅
δ（１，１）で設定されたパラメータ探索点について誤
差評価値Ｄmin を求め、そのときの探索パラメータθの
値を用いた新たなパラメータ探索点ｓ（１，２）を求め
て、１回目の探索パラメータθに対する独立探索を終了
する。

【００６５】次に、いま得られたパラメータ探索点ｓ
（１，２）をパラメータ探索開始点として探索パラメー
タφについて同様の探索過程を実行する。今度は、他の
探索パラメータθおよびρを固定した状態で探索パラメ
ータφのみをパラメータ探索幅δ（１，２）に応じて探
索過程を実行するのである。したがって、パラメータ探
索点ｓ（１，２）を探索開始点としてφ軸に沿った方向
に探索を行うことになり（図中矢印（ロ）で示す）、所
定の探索範囲内で得られる局所最適値Ｄmin を求めてそ
のときの探索パラメータφの値を用いた新たなパラメー
タ探索点ｓ（１，３）を求める。

【００６６】次に、同様のことを探索パラメータρにつ
いて実行すると、そのときに得られたパラメータ探索点
ｓ（２，１）は２回目の探索過程におけるパラメータ探
索開始点となり、以下、このような探索過程をＫ回繰り
返し実行することにより、図中矢印（ハ）〜（ヘ）で示
すようにして順次パラメータ探索点ｓ（ｋ，ｈ）が最適
値に向かって探索されて行き、最終的に得られるパラメ
ータ探索点ｓ（Ｋ＋１，１）が最適な位置姿勢ベクトル
ｓ〔θ，φ，ρ〕を与える最適位置姿勢データとなる。

【００６７】なお、上述のようにして各探索パラメータ
についての探索過程を１回実行すると、次の回に行われ
る探索過程においては、その探索範囲が絞り込まれるよ
うになる多重スケールとして巡回的に探索過程を実行す
ることができるので、この探索回数Ｋの値を大きく設定
するほど、検出精度を向上させることができるようにな
って、目標検出精度を高くすることができる。

【００６８】さて、上述のようにして繰り返し回数Ｋだ
け（例えばＫ＝５）上述のステップＴ７〜Ｔ２２を繰り
返し実行すると、ステップＴ２１を経てステップＴ２３
に移行するようになる。ここで、最適値決定部１５は、
最終的に得られたパラメータ探索点ｓ（６，１）に対す
る局所最適値Ｄmin の値が目標となる誤差精度εよりも
小さいか否かを判定し、この条件を満たしている場合に
は、このときのパラメータ探索点ｓ（６，１）に対応す
る位置姿勢ベクトルｓ〔γ，θ，φ，ρ〕および姿勢パ
ラメータとしてステップＴ５で計算した値αおよびβを
最適位置姿勢データとして最終出力部１６から出力する
ようになる（ステップＴ２４）。

【００６９】また、一連の探索過程が終了した結果、局
所最適値Ｄmin の値が誤差精度ε以上となってステップ
Ｔ２３の条件を満たさない場合には、ステップＴ２３か
らステップＴ２５に進み、要求誤差精度εを満たす探索
点ｓが存在しないことをメッセージ出力した後、それま
での間に計算された局所最適値Ｄmin のなかから要求誤
差精度εに一番近い値が得られたパラメータ探索点ｓ
（ｋ，ｈ）の位置姿勢ベクトルｓ〔γ，θ，φ，ρ〕の
値およびカメラ４の姿勢パラメータ（α，β）の値を新
たなパラメータ探索開始点として設定すると共に新たな
探索領域Ｓを設定してステップＴ６に戻り、上述と全く
同様にして探索過程を繰り返し実行するようになる。

【００７０】なお、上述のように探索過程を実行するこ
とにより、例えば、繰り返し回数ｋの値が最終回Ｋのと
きには最終目標検出精度εよりも小さいパラメータ探索
幅δを設定する必要があるが、それ以前の繰り返し回数
ｋ（ｋ＜Ｋ）に対しては、そのときの検出精度に対応し
て目標検出精度よりも大きいパラメータ探索幅δを設定
することができ、これにより、探索の初期段階において
はパラメータ探索幅δを広い幅として概略的な位置姿勢
の計算を行うことにより迅速に探索領域を絞り込み、最
終段階でパラメータ探索幅δを目標検出精度となるよう
に設定することができるようになる。つまり、パラメー
タ探索幅を多重スケールとして探索過程を実行すること
ができるので、探索過程の計算時間を短縮した概略的な
計算を行いながら効率的に精度の高い計算処理を行うこ
とができる。

【００７１】なお、発明者らの実験結果によると、この
ように多重スケールで探索過程を実行することにより、
繰り返し探索回数Ｋの値を「５」とした場合でも、検出
誤差精度を、距離精度±１．０ｍｍ、姿勢精度±０．５
°の位置姿勢決定に対して十分に対応できることがわか
り、高速且つ高精度でロバストに位置姿勢を認識するこ
とができることが確認されている。

【００７２】このように本実施例においては、既知形状
の対象物体としての作業エリア２およびワーク５をカメ
ラ４により撮像してその画像情報から画像特徴点Ｑｊを
抽出し、量子化した空間点を複数個有する空間探索領域
Ｓｃを設定すると共に、カメラ４の位置姿勢ベクトルｓ
〔γ，θ，φ，ρ〕を設定してワーク５をその探索点ｓ
（ｋ，ｈ）において撮像したときに得られるべきワーク
５の推定画像特徴点Ｑｅｉを透視変換行列Ｍ（ｋ，ｈ）
を求めることにより計算し、それらの対応関係を求めて
誤差評価関数Ｄに対する局所最適値Ｄmin を計算して探
索パラメータごとに逐次的な独立探索を実行することを
繰り返し回数Ｋだけ繰り返し実行し、要求誤差精度εを
満たしたときの探索点ｓに対応する位置姿勢ベクトルｓ
〔γ，θ，φ，ρ〕および姿勢パラメータ（α，β）を
最適位置姿勢データとして出力するようにしたので、以
下のような効果を得ることができる。

【００７３】すなわち、第１に、従来と異なり、特殊マ
ーカを設ける必要がなくなり、マーカ認識用の移動時間
が不要となって計算時間を短縮して高速化を図れる。第
２に、現在、明示的に解けていない透視変換行列の成分
からパラメータを算出する方法ではないので、明示的に
は解けるが計算量が膨大となる透視変換行列の最小二乗
推定を必要としないため、迅速に計算処理を実行でき
る。第３に、カメラ４の姿勢パラメータが未知の場合で
も適用できる。

【００７４】第４に、カメラ４の位置姿勢ベクトルｓ
〔γ，θ，φ，ρ〕を求めるための計算において、探索
パラメータ毎の逐次的独立探索を多重スケールで巡回的
に繰り返し実行することにより、少ない探索回数で最適
な位置姿勢を高精度且つロバストに認識することができ
るようになる。

【００７５】第５に、透視変換行列Ｍの式（５）を式
（５Ｃ）のように単純化して実際の計算を行うので、式
（５）のままで計算する場合に比べて大幅な計算量の削
減を図れるので、演算速度の向上を図ることができる。

【００７６】なお、上記実施例においては、探索パラメ
ータγ，θ，φ，ρの探索順序をこの順番となるように
あらかじめ指定して逐次的独立探索過程を実行するよう
にしているが、これに限らず、ワーク５に対するカメラ
４の概略位置姿勢に応じて探索順序を柔軟に変更する機
能を設けても良い。つまり、例えば、距離ｒに対するパ
ラメータρが探索範囲を決定づける大きな要素となるよ
うな位置にある場合には、探索パラメータρを探索順序
の第１番目に指定するといったように、探索順序を決定
するための判断基準を設けておくことにより、さらに効
率的な探索過程を実行することができるのである。

【００７７】図１１は本発明の第２の実施例を示すもの
で、以下、第１の実施例と異なる部分について説明す
る。すなわち、このものは、第１の実施例において実行
されるプログラムの内容に加えて図１１に示すステップ
を実行することにより、撮影画面上の原点計算に起因す
る測定誤差に対してロバスト性の向上を図ると共に認識
精度の向上を図るようにしたものである。

【００７８】第１の実施例においては、探索過程の初期
段階でカメラの姿勢パラメータであるαとβの値を画像
情報に基づいて設定し、これを既知データとして取り扱
うようにした。これは、探索すべき６個のパラメータ
α，β，γ，θ，φおよびρに対して実際の計算段階で
４個のパラメータに絞り込むことで演算量を低減して高
速化を図るようにしている。

【００７９】しかし、この場合に、撮影画面の画像デー
タから抽出した対象物座標系原点の二次元座標に誤差が
含まれていると、初期設定として求めた姿勢パラメータ
α，βの値にも誤差が生じていることになる。したがっ
て、この初期設定に基づいて他の４個の探索パラメータ
の巡回独立探索を行う場合には、姿勢パラメータα，β
の探索精度を上回る高精度の位置姿勢認識が達成できな
くなる場合が生ずるのである。

【００８０】そこで、この実施例では、上述の不具合を
解消しながら、しかも６個すべての位置姿勢パラメータ
を探索パラメータとして巡回独立探索過程を実行する場
合に比べて、演算量の増加分を最小限に止めて演算速度
の低下の防止を図るようにしている。

【００８１】まず、第１の実施例における探索過程を実
行するうちに、ステップＴ６が終了すると、続いて図１
１に示すステップＴ２５を実行する。ここでは、繰り返
し探索回数ｋの値があらかじめ設定されたカメラ姿勢パ
ラメータα，βの探索開始回数ｍ回以上となっているか
否かを判断する。ここで、「ＮＯ」と判断されたときに
は、そのままステップＴ７に移行して第１の実施例と同
様の探索過程を実施し、以後、繰り返して探索過程を実
行するうちに、このステップＴ２５で「ＹＥＳ」と判断
されると、ステップＴ２６に分岐するようになる。

【００８２】すなわち、ステップＴ２６にて探索パラメ
ータとしての姿勢パラメータα，βの探索幅を設定し、
ステップＴ２７に進むと姿勢パラメータβの探索を実行
し、続いてステップＴ２８にて姿勢パラメータαの探索
を実行し、この後、ステップＴ７に戻って第１の実施例
と同様にして以降のステップにおける探索過程を実行す
る。この場合、探索回数が大きくなるにしたがってステ
ップＴ２６における探索幅の設定値は細かい値となり、
探索精度が高くなるようにしている。

【００８３】したがって、このような第２の本実施例に
よれば、第１の実施例における効果に加えて、初期段階
で撮影画面上の原点Ｏｐに基づいて求めたカメラ姿勢パ
ラメータα，βの値を探索過程の後半段階で探索パラメ
ータに加えて探索過程を実行するようにしたので、初期
段階で撮影画像原点座標Ｏｐデータに含まれる検出誤差
によるカメラ姿勢パラメータα，βの初期値の誤差を修
正することができ、探索過程の後半段階での実質的な検
出精度をより向上させることができるようになる。ま
た、探索過程の前半段階から全てのパラメータについて
探索を実行する場合に比べて演算速度の低下を招くこと
なく検出精度の向上を図ることができるものである。

【００８４】なお、発明者らは、上述の場合に、繰り返
し探索回数Ｋの値を第１の実施例における回数５回より
もさらに多くした１２回に設定して高精度の探索を実施
し、この場合に、カメラ姿勢パラメータα，βの探索開
始回数ｍを７回として実施した場合に、高精度で良好な
結果が得られることがわかった。

【００８５】この場合、繰り返し探索回数Ｋの値を大き
く設定することは、一般的に検出精度の向上となる反
面、探索時間が長くなるという制約を受けることになる
ので、実際には要求される検出精度と探索時間との兼ね
合いで繰り返し探索回数を設定することになる。そこ
で、短時間での検出精度向上が要求される場合には、本
実施例における探索過程が有効な方法となるものであ
る。

【００８６】なお、上記第２の実施例においては、繰り
返し探索回数Ｋを１２回とし、カメラ姿勢パラメータ
α，βの探索開始回数ｍを７回として実験した場合につ
いて説明したが、これに限らず、繰り返し探索回数Ｋお
よび探索開始回数ｍ（Ｋ＞ｍ＞２）の値は検出精度と探
索時間との兼ね合いで適宜の値に設定することができる
ものである。

【００８７】

【発明の効果】以上説明したように、本発明によれば、
次のような効果を得ることができる。すなわち、請求項
１記載の視覚に基く三次元位置および姿勢の認識方法に
おいては、撮像手段により撮影された対象物体の画像信
号入力に対し、画像処理ステップにおいて二次元の画像
特徴点を抽出し、空間探索領域設定ステップにおいて外
部情報等に基づいた撮像手段と対象物体との概略的な推
定距離に応じて必要な計算精度に対応する密度の量子化
空間点を有する空間探索領域を設定し、探索パラメータ
設定ステップにおいてパラメータ探索値の初期値，探索
点，探索範囲を設定する。

【００８８】そして、透視変換行列計算ステップにおい
て二次元の推定画像特徴点を算出するための透視変換行
列を計算すると共に、透視変換計算ステップにおいて対
象物体の二次元の推定画像特徴点を算出し、誤差計算ス
テップにおいて画像特徴点と推定画像特徴点との対応関
係を求めてそれらの間の評価誤差値を誤差評価関数に基
いて計算し、局所最適パラメータ計算ステップおよび最
適位置姿勢データ決定ステップにおいて、あらかじめ指
定された順序で前記複数のパラメータのすべてについて
局所最適パラメータの逐次的独立探索を実行させ、これ
を所定回数繰り返すことにより、位置姿勢ベクトルの複
数のパラメータとして決定するようにした。

【００８９】これにより、撮像手段の位置姿勢を複数の
パラメータで表した位置姿勢ベクトルに対して、それぞ
れのパラメータを逐次独立して探索することを多重スケ
ールで巡回的に所定回数だけ実施することができ、少な
い探索回数で高速且つ高精度でロバストに撮像手段の位
置姿勢を認識できるという優れた効果を奏する。

【００９０】請求項２記載の視覚に基く三次元位置と姿
勢の認識装置によれば、上述の各処理ステップを実行す
る手段を備えているので、少ない探索回数でありながら
高速且つ高精度でロバストに撮像手段の位置姿勢を認識
できるという優れた効果を奏する。

【図面の簡単な説明】

【図１】本発明の第１の実施例を示す概略的なブロック
構成図

【図２】自律移動ロボットと作業エリアとの位置関係を
示す図

【図３】プロセスの流れを概略的に説明するためのフロ
ーチャート

【図４】透視変換行列を計算するための座標軸の関係を
示す説明図

【図５】撮影画面上のワークとカメラの姿勢パラメータ
との関係を示す説明図

【図６】絶対座標軸とカメラおよび撮影画面の座標軸と
の関係を示す説明図

【図７】（ａ）作業エリアおよびワークの特徴点の定義
と（ｂ）画像信号による画像特徴点との対応関係を示す
作用説明図

【図８】ワークの種々の見え方パターン

【図９】ひとつの見え方パターンにおける異なる見え方
の状態の説明図

【図１０】探索過程の概念的な説明図

【図１１】本発明の第２の実施例を示すプロセスの流れ
を概略的に説明するためのフローチャートの追加ステッ
プ説明図

【符号の説明】

１は自律移動ロボット、４はカメラ（撮像手段）、５は
ワーク（対象物体）、７は画像処理部（画像処理手
段）、８は前処理計算部、９は特徴点抽出部、１０は量
子化空間探索部、１１は探索領域決定部（探索領域設定
手段）、１２は透視変換行列計算部（透視変換行列計算
手段）、１３は透視変換部（透視変換計算手段）、１４
は誤差評価関数計算部（誤差計算手段）、１５は最適値
決定部（局所最適パラメータ計算手段、最適位置姿勢デ
ータ決定手段）、１６は最終出力部、１７はメモリ（記
憶手段）、１８は外部位置情報出力部である。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 小南 哲也 愛知県刈谷市昭和町１丁目１番地 日本電 装株式会社内

Claims (2)

【特許請求の範囲】
1. 【請求項１】 形状を示す三次元絶対座標が既知である
   対象物体を撮像手段により撮影したときの画像信号に基
   いてその対象物体の形状の特徴を示す二次元の画像特徴
   点を抽出する画像処理ステップと、 前記対象物体に対する前記撮像手段の位置姿勢を示す複
   数のパラメータを探索パラメータとし、外部情報により
   与えられる推定距離に応じたパラメータ探索開始点を設
   定すると共にその推定距離に応じたパラメータ探索幅で
   量子化したパラメータ探索点を有する空間探索領域を設
   定する探索領域設定ステップと、 この探索領域設定ステップにて設定された前記空間探索
   領域のパラメータ探索点で決まる位置から前記撮像手段
   により前記対象物体を撮影したときに得られるべき二次
   元の推定画像特徴点を算出するための透視変換行列を、
   あらかじめ記憶手段に記憶された前記対象物体の三次元
   絶対座標に対する前記パラメータ探索点の推定位置と前
   記撮像手段のパラメータ探索開始点とに基いて計算する
   透視変換行列計算ステップと、 この透視変換行列計算ステップにて求められた前記透視
   変換行列を用いて前記パラメータ探索点における前記対
   象物体の二次元の前記推定画像特徴点を算出する透視変
   換計算ステップと、 前記画像処理ステップにて抽出された前記対象物体の二
   次元の画像特徴点と前記透視変換計算ステップにて算出
   された二次元の前記推定画像特徴点との対応関係を求め
   ると共に、それら対応関係が得られた画像特徴点と推定
   画像特徴点との間の適合度を示す評価誤差値を所定の誤
   差評価関数に基いて計算する誤差計算ステップと、 前記複数の探索パラメータのうちの指定された探索パラ
   メータに対して前記探索領域内で、前記各推定画像特徴
   点に対する画像特徴点の評価誤差値が最小となるような
   前記パラメータ探索点を局所最適パラメータとして求め
   て、あらたにパラメータ探索点として設定する局所最適
   パラメータ計算ステップと、 この局所最適パラメータ計算ステップを、あらかじめ指
   定された順序で前記複数のパラメータのすべてについて
   逐次実行すると共に、これを所定回数繰り返して実行
   し、そのときの前記評価誤差値が所定の要求誤差精度よ
   りも小さいときに、得られた複数のパラメータ探索値を
   前記位置姿勢ベクトルの複数のパラメータとして決定す
   る最適位置姿勢データ決定ステップとを有することを特
   徴とする視覚に基く三次元位置および姿勢の認識方法。
2. 【請求項２】 画像信号を出力する撮像手段と、 対象物体の形状を示す三次元絶対座標データおよび探索
   順序を指定するデータが記憶された記憶手段と、 前記撮像手段により前記対象物体を撮影したときの画像
   信号に基いてその対象物体の形状の特徴を示す二次元の
   画像特徴点を抽出する画像処理手段と、 前記対象物体に対する前記撮像手段の位置姿勢を示す複
   数のパラメータを探索パラメータとし、外部情報により
   与えられる推定距離に応じたパラメータ探索開始点を設
   定すると共にその推定距離に応じたパラメータ探索幅で
   量子化したパラメータ探索点を有する空間探索領域を設
   定する探索領域設定手段と、 この探索領域設定手段により設定された前記空間探索領
   域のパラメータ探索点で決まる位置から前記撮像手段に
   より前記対象物体を撮影したときに得られるべき二次元
   の推定画像特徴点を算出するための透視変換行列を、あ
   らかじめ記憶手段に記憶された前記対象物体の三次元絶
   対座標に対する前記量子化空間点の推定位置と前記撮像
   手段の初期探索パラメータとに基いて計算する透視変換
   行列計算手段と、 この透視変換行列計算手段により求められた前記透視変
   換行列を用いて前記パラメータ探索点における前記対象
   物体の二次元の前記推定画像特徴点を算出する透視変換
   計算手段と、 前記画像処理手段により抽出された前記対象物体の二次
   元の画像特徴点と前記透視変換計算手段により算出され
   た二次元の前記推定画像特徴点との間の対応関係を求め
   ると共に、それら対応関係が得られた画像特徴点と推定
   画像特徴点との間の適合度を示す評価誤差値を所定の誤
   差評価関数に基いて計算する誤差計算手段と、 前記複数の探索パラメータのうちの指定された探索パラ
   メータに対して前記探索領域内で、前記各推定画像特徴
   点に対する画像特徴点の評価誤差値が最小となるような
   前記パラメータ探索点を局所最適パラメータとして求め
   て、あらたにパラメータ探索点として設定する局所最適
   パラメータ計算手段と、 この局所最適パラメータ計算手段による計算処理を、あ
   らかじめ指定された順序で前記複数のパラメータのすべ
   てについて逐次実行すると共に、これを所定回数繰り返
   して実行し、そのときの前記評価誤差値が所定の要求誤
   差精度よりも小さいときに、得られた複数のパラメータ
   探索値を前記位置姿勢ベクトルの複数のパラメータとし
   て決定する最適位置姿勢データ決定手段とを有すること
   を特徴とする視覚に基く三次元位置および姿勢の認識装
   置。