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

Abstract

(57)【要約】 【目的】 自律移動ロボットの位置を単眼視カメラによ
り撮像したワークの画像情報から迅速且つ高精度に演算
して求める。 【構成】 カメラ４により撮像されたワークの画像特徴
点を画像処理部７により演算する。最小二乗推定部１０
により、カメラ４の概略位置に応じた見え方パターンを
選択してカメラ行列を最小二乗推定により求めて推定透
視変換行列を演算する。量子化探索部１１により、所定
の密度で設定された量子化空間点を有する空間探索領域
を設定し、その各量子化空間点においてワークをカメラ
により撮影したときに推定画像特徴点を得るための透視
変換行列を演算すると共に、画像特徴点とのずれを誤差
評価関数により演算する。所定の要求誤差精度を満たす
場合に、そのときの量子化空間点および姿勢パラメータ
をカメラの位置と姿勢として出力する。これにより、迅
速且つ高精度で検出することができる。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

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

【０００４】

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

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

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

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

【０００８】

【課題を解決するための手段】本発明の視覚に基く三次
元位置および姿勢の認識方法は、形状を示す三次元絶対
座標が既知である対象物体を撮像手段により撮影したと
きの画像情報に基いてその対象物体の形状の特徴を示す
二次元の画像特徴点を抽出する画像処理工程と、前記撮
像手段の概略位置と姿勢とに基いて、前記画像処理工程
で抽出した画像特徴点と記憶手段にあらかじめ記憶され
た前記対象物体の見え方のパターンとの間の特徴点の対
応リストを選択すると共に、その対応リストに基いて前
記撮像手段の現在位置と姿勢とにおける前記対象物体の
見え方を表すカメラ行列の最小二乗推定を行って推定透
視変換行列を求める最小二乗推定工程と、前記撮像手段
と前記対象物体との概略的な推定距離に応じて必要な演
算精度に対応する密度の量子化空間点を有する探索領域
を設定する探索領域設定工程と、前記探索領域の量子化
空間点において前記撮像手段により前記対象物体を撮影
したときに得られるべき二次元の画像特徴点を算出する
ための透視変換行列を、あらかじめ記憶手段に記憶され
た前記対象物体の三次元絶対座標に対する前記量子化空
間点の推定位置と前記撮像手段の撮像姿勢を示す姿勢パ
ラメータとに基いて演算する透視変換行列演算工程と、
この透視変換行列演算工程にて求められた前記透視変換
行列と前記最小二乗推定工程において求められた推定透
視変換行列との間の適合度を示す評価誤差値を所定の誤
差評価関数に基いて演算する誤差演算工程と、この誤差
演算工程にて求められた前記評価誤差値を前記探索領域
に対応して設定された要求誤差精度と比較し、前記評価
誤差値が前記要求誤差精度よりも小さくなることを判定
条件として、前記探索領域設定工程，前記透視変換行列
演算工程および前記誤差演算工程を繰り返し実行させる
誤差判定工程と、この誤差判定工程において前記判定条
件を満たしたときに、そのときの量子化空間点の三次元
絶対座標と前記撮像手段の姿勢パラメータとを出力する
データ出力工程とから構成したところに特徴を有する。

【０００９】また、本発明の視覚に基く三次元位置およ
び姿勢の認識装置は、画像情報を出力する撮像手段と、
形状を示す三次元絶対座標が既知である対象物体を前記
撮像手段により撮影したときの画像情報に基いてその対
象物体の形状の特徴を示す二次元の画像特徴点を抽出す
る画像処理手段と、前記撮像手段の概略位置と姿勢とに
基いて、前記画像処理手段により抽出した画像特徴点と
記憶手段にあらかじめ記憶された前記対象物体の見え方
のパターンとの間の特徴点の対応リストを選択すると共
に、その対応リストに基いて前記撮像手段の現在位置と
姿勢とにおける前記対象物体の見え方を表すカメラ行列
の最小二乗推定を行って推定透視変換行列を求める最小
二乗推定手段と、前記撮像手段と前記対象物体との概略
的な推定距離に応じて必要な演算精度に対応する密度の
量子化空間点を有する探索領域を設定する探索領域設定
手段と、前記探索領域の量子化空間点において前記撮像
手段により前記対象物体を撮影したときに得られるべき
二次元の画像特徴点を算出するための透視変換行列を、
あらかじめ記憶手段に記憶された前記対象物体の三次元
絶対座標に対する前記量子化空間点の推定位置と前記撮
像手段の撮像姿勢を示す姿勢パラメータとに基いて演算
する透視変換行列演算手段と、この透視変換行列演算手
段により求められた前記透視変換行列と前記最小二乗推
定手段により求められた推定透視変換行列との間の適合
度を示す評価誤差値を所定の誤差評価関数に基いて演算
する誤差演算手段と、この誤差演算手段にて求められた
前記評価誤差値を前記探索領域に対応して設定された要
求誤差精度と比較し、前記評価誤差値が前記要求誤差精
度よりも小さくなることを判定条件として、前記探索領
域設定手段，前記透視変換行列演算手段および前記誤差
演算手段に前記演算を繰り返し実行させる誤差判定手段
と、この誤差判定手段により前記判定条件が満たされた
ときに、そのときの量子化空間点の三次元絶対座標と前
記撮像手段の姿勢パラメータとを出力するデータ出力手
段とから構成したところに特徴を有する。

【００１０】

【作用】本発明の視覚に基く三次元位置および姿勢の認
識方法によれば、対象物体に対して移動接近して作業を
行う装置等に搭載した撮像手段により、三次元絶対座標
が既知である対象物体が撮影されると、画像処理工程に
おいては、その画像情報に基いて対象物体の形状の特徴
を示す二次元の画像特徴点を抽出する。

【００１１】続いて、最小二乗推定工程においては、前
記撮像手段の概略位置と姿勢とに基いて、前記画像処理
工程で抽出した画像特徴点と記憶手段にあらかじめ記憶
された前記対象物体の見え方のパターンとの間の特徴点
の対応リストを選択し、この後、その対応リストに基い
て前記撮像手段の現在位置と姿勢とにおいて前記対象物
体の見え方を表す変換行列としてのカメラ行列の最小二
乗推定を行って推定透視変換行列を求める。

【００１２】次に、探索領域設定工程において、撮像手
段とその対象物体との概略的な推定距離に応じて必要な
演算精度に対応する密度の量子化空間点を有する探索領
域を設定し、続く透視変換行列演算工程において、その
探索領域の量子化空間点に前記撮像手段を配置して前記
対象物体を撮影したときに得られるべき二次元の画像特
徴点を算出するための透視変換行列を、あらかじめ記憶
手段に記憶された前記対象物体の三次元絶対座標に対す
る前記量子化空間点の推定位置と前記撮像手段の撮像姿
勢を示す姿勢パラメータとに基いて演算する。

【００１３】そして、誤差演算工程において、透視変換
行列演算工程にて求められた前記透視変換行列と前記最
小二乗推定工程において求められた推定透視変換行列と
の間の適合度を示す評価誤差値を所定の誤差評価関数に
基いて演算する。これは、各行列における対応する要素
のずれを演算するもので、評価誤差値が小さい程両行列
間の適合度合が高いことを示している。

【００１４】次に、誤差判定工程において、上述の誤差
演算工程にて求められた前記評価誤差値を前記探索領域
に対応して設定された要求誤差精度と比較し、前記評価
誤差値が前記要求誤差精度よりも小さくなることを判定
条件として、前記探索領域設定工程，前記透視変換行列
演算工程および前記誤差演算工程を繰り返し実行させ、
最終的に、誤差判定工程にて上記条件に適合するように
なったときには、データ出力工程において、そのときの
量子化空間点の三次元絶対座標と前記撮像手段の姿勢パ
ラメータとを出力するようになり、実際の撮像手段の三
次元絶対座標に最も近い量子化空間点を近似点として求
めることができると共に、撮像手段の光軸のずれ等を表
す姿勢パラメータを検出することができるようになり、
これらのデータに基いて対象物体に対する装置の移動制
御などを適確に行うことができるようになるものであ
る。

【００１５】また、本発明の視覚に基く三次元位置およ
び姿勢の認識装置によれば、上述の各工程を実施する手
段により、同様の動作を行なわせることができ、実際の
撮像手段の三次元絶対座標に最も近い量子化空間点を近
似点として求めることができると共に、撮像手段の光軸
のずれ等を表す姿勢パラメータを検出することができる
ようになり、これらのデータに基いて対象物体に対する
装置の移動制御などを適確に行うことができるようにな
るものである。

【００１６】

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

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

【００１８】最小二乗推定手段としての最小二乗推定部
１０は、画像処理部７にて抽出された画像特徴点が与え
られると、これに基いて後述するようにして、カメラ４
によりワーク５を撮影したときにそのカメラ４の位置お
よび姿勢に対応して得られるべき見え方のパターンを選
択すると共に、カメラ４の現在位置と姿勢とに対するカ
メラ行列の最小二乗推定を行い、この結果得られる推定
透視変換行列Ｍｓを演算して求める。

【００１９】量子化空間探索部１１は、探索領域設定手
段としての探索領域設定部１２，透視変換行列演算手段
としての透視変換行列演算部１３，誤差演算手段として
の誤差演算部１４，誤差判定手段としての誤差判定部１
５およびデータ出力手段としてのデータ出力部１６から
構成されている。

【００２０】この場合、探索領域設定部１２は、探索領
域設定工程として、作業エリア２のワーク５に対する自
律移動ロボット１の概略位置情報に基いた密度の複数の
量子化空間点Ｅを有する空間探索領域Ｓを設定する。透
視変換行列演算部１３は、透視変換行列演算工程とし
て、設定された空間探索領域Ｓの量子化空間点Ｅにおい
てカメラ４によりワーク５を撮影したときに画像信号か
ら得られる画像内における画像特徴点の座標を求めるた
めの透視変換行列Ｍを、自律移動ロボット１の概略位置
情報およびカメラ４の姿勢パラメータに応じて演算す
る。

【００２１】誤差演算部１４は、誤差演算工程として、
最小二乗推定部１０にて求められた推定透視変換行列Ｍ
ｓと透視変換行列演算部１３にて演算された透視変換行
列Ｍとの間の誤差を各行列要素の二乗誤差和として求め
る誤差評価関数に基いて演算する。誤差判定部１５は、
誤差判定工程として、演算された二乗誤差和が要求誤差
精度εよりも小さいか否かを判定し、小さいときにはそ
のときのカメラ４の位置および姿勢のデータを出力端子
１７に出力し、大きいときには再び探索領域設定部１２
に戻って上述の各部において演算を実行させる。そし
て、データ出力部１６は、データ出力工程として、上述
の判定をした結果に基いてそのときの空間探索領域Ｓに
おける量子化空間点Ｅを出力する。

【００２２】また、量子化空間探索部１１には、後述す
る各種データが記憶される記憶手段としてのメモリ１８
が接続され、その各種データの書き込みおよび読み出し
動作が行えるようになっていると共に、外部情報入力端
子１９が設けられ、自律移動ロボット１の走行制御を行
う図示しない制御部あるいは図示しない位置検出センサ
などから自律移動ロボット１の概略位置を示す信号が入
力されるようになっている。この場合、メモリ１８にあ
らかじめ記憶されるデータとしては、特徴点の対応リス
ト，ワーク５および作業エリア２の三次元絶対座標デー
タ，カメラ４のパラメータ，量子化空間点Ｅのリストと
その座標データなどである。

【００２３】次に、本実施例の作用について図３ないし
図１０をも参照して説明する。三次元位置認識装置は、
その動作の流れとして図３に示すようなステップにした
がって自律移動ロボット１の存在位置を認識する。ま
ず、図２の状態において、自律移動ロボット１が軌道上
を移動してきて作業エリア２に差し掛かると、カメラ４
の撮影シーン内に作業エリア２が入って撮影されると、
画像処理部７は、カメラ４の画像信号を入力するように
なる（ステップＴ１）。この場合、自律移動ロボット１
は、所定軌道上を所定方向に移動しており、また移動を
制御する制御部の内部情報などにより概略の停止位置が
わかっているので、その停止予定位置において対象物体
としてのワーク５がカメラ４の視野内に入るように設定
することができる。

【００２４】次に、カメラ４により撮影されたワーク５
の画像信号は、画像処理部７の前処理計算部８にて前処
理としての雑音除去処理（ステップＴ２）およびワーク
５の画像の切り出し処理（ステップＴ３）を実行し、こ
の後、特徴点抽出部９にてその画像信号に基いて次のよ
うにしてｍ個の画像特徴点Ｑｊの集合Ｑを求めて出力す
るようになる（ステップＴ４）。ここで、画像特徴点Ｑ
ｊの集合Ｑは次のように定義される。 Ｑ＝｛Ｑｊ；ｊ＝１，２，…，ｍ｝ …（１） Ｑｊ＝（ｕｊ，ｖｊ） …（２） ｕ，ｖ；表示画面上における画素単位の二次元座標の各
要素値を示す

【００２５】この画像特徴点Ｑｊの抽出に際しては、外
部情報として入力端子１９から与えられる概略位置情報
に基いて次のように演算を行う。すなわち、概略位置情
報として、例えば、自律移動ロボット１の移動経路を記
憶した作業計画，走行制御により移動した空間移動の履
歴情報などが入力されるので、これらの外部情報に基い
て、自律移動ロボット１の中心位置の概略的な三次元絶
対座標として、絶対座標で表した概略位置Ａ０、 Ａ０＝（Ｘ0 ，Ｙ0 ，Ｚ0 ） …（３） を得ることができる。なお、概略位置Ａ０の三次元絶対
座標は、作業エリア２の所定の位置を原点Ｏｐとして表
現したものである。

【００２６】次に、メモリ１８にあらかじめ記憶されて
いる作業エリア２に載置されたワーク５の種々の見え方
パターン（図４（ａ）ないし（ｆ）参照）の中から、概
略位置Ａ０の値に対応する見え方パターンを選択する
（ステップＴ５）。いま、例えば、概略位置Ａ０に対応
する見え方パターンが図４（ｂ）に対応しているときに
は、その見え方パターンに現れているワーク５の三次元
の特徴点Ｐｉの集合、 Ｐ＝｛Ｐｉ；ｉ＝１，２，…，ｎ｝ …（４） に対応する画像特徴点Ｑｊを求めて順次抽出する。抽出
した画像特徴点Ｑｊはメモリ１８に記憶される。これ
は、例えば、図９（ａ）に示すように、作業エリア２と
ワーク５との特徴点Ｐｉの座標が記述されたメモリ１８
の座標データと、同図（ｂ）に示す画像信号により得ら
れた画像特徴点Ｑｊとの間の対応関係を上述の見え方パ
ターン（図４（ｂ）参照）に対応させて選択し、これに
基いて特徴点の対応リストを選択するのである。

【００２７】さて、最小二乗推定部１０においては、上
述のように選択された対応リストに基いてカメラ４の現
在位置と姿勢に対するカメラ行列Ｃの最小二乗推定を行
う（ステップＴ６）。この場合、カメラ行列Ｃは、次式
（５），（６），（７）に基いて設定されるもので、
（ｕ，ｖ，ｗ）は斉次座標系における画像表示面の１点
を表し、（Ｕ，Ｖ）は画像表示面の座標軸で表した座標
値であり、（ｘ，ｙ，ｚ）は三次元絶対座標空間におい
て対応する１点の座標である。つまり、カメラ行列Ｃは
三次元絶対空間の座標と画像表示面上との間の変換を行
うためのパラメータである。なお、ｗはカメラ４による
スケールファクタを表している。

【００２８】

【数１】

【００２９】そして、式（５），（６）から、 ｕ−Ｕｗ＝０ ， ｖ−Ｖｗ＝０ …（８） が導かれ、また、斉次座標の性質を考慮すると、式
（７）から次式（９），（１０），（１１）が得られ
る。

【００３０】

【数２】

【００３１】次に、式（９），（１０），（１１）を式
（８）に代入すると、次式（１２），（１３）が得られ
る。

【００３２】

【数３】 これらの式から次式（１４），（１５）のようにスケー
ルファクタとしてのｗを消去して変形することができ
る。

【００３３】

【数４】 この結果から、次式（１６），（１７）で示される評価
関数を導出することができ、Ｅｕ，Ｅｖを最小化するよ
うな次式（１８），（１９）で示される条件のもとでカ
メラ行列Ｃの各要素を係数とする線形方程式を得て最終
的にカメラ行列Ｃを決定することができる。

【００３４】

【数５】 そして、このようにして得られたカメラ行列Ｃ（広義の
透視変換行列）の推定結果を推定透視変換行列Ｍｓとし
て記憶する。

【００３５】次に、量子化空間探索部１１においては、
探索領域設定部１２により、概略位置Ａ０（Ｘ0 ，Ｙ0
，Ｚ0 ）を基準とした空間探索領域Ｓｃ、 Ｓｃ（ｃ＝１，２，…，Ｃ） …（２０） を設定する（ステップＴ７）。この空間探索領域Ｓｃ
は、図５に示すように、所定の密度で格子状に量子化し
て配列された複数の量子化空間点Ｅ（ｋ，ｃ）、 Ｅ（ｋ，ｃ）（ｋ＝１，２，…，Ｋ、ｃ＝１，２，…，Ｃ） …（２１） を含んで構成される。

【００３６】この場合、作業エリア２の原点Ｏｐを基準
とした絶対座標系におけるＸ軸方向（作業エリア２に対
向する方向）の座標値Ｘ＝ＸL1の平面内で、カメラ４は
ワーク５を向いていると仮定できる。ここで、ＸL1はカ
メラ４の視点の位置を示すＸ座標値の推定値であり、こ
の値は自律移動ロボット１の概略位置Ａ０のＸ方向の座
標値Ｘ0 とカメラ４のロボット上の相対座標から決定で
きる。また、本実施例における自律移動ロボット１の場
合には、概略位置Ａ０の高さ方向の座標値Ｚ0は、一定
の値から大きく外れることがないと想定されるので、カ
メラ４のＺ方向の座標を求めるための探索範囲は実質的
に狭い範囲に絞り込むことができる。

【００３７】次に、カメラ４の各種パラメータの探索範
囲を設定する（ステップＴ８）。ここで、カメラ４のパ
ラメータとしては、図６に対応関係を示すように、 （ａ）カメラ４の姿勢パラメータＣＰ（α，β，γ）、 （ｂ）カメラ４の焦点距離特性ｆ （ｃ）カメラ４の位置（ｒ，θ，φ） がある。そして、この場合において、（ｂ）の焦点距離
特性ｆはカメラ４の特性によりあらかじめ決まっている
ので、例えば、メモリ１８内にデータとして記憶させて
おくことができる。また、（ｃ）のカメラ４の位置
（ｒ，θ，φ）の値は以降の演算工程にて決めることが
できる値である。

【００３８】したがって、ここでは、カメラ４のワーク
５の中心位置からの光軸のずれを表している姿勢パラメ
ータＣＰ（α，β，γ）の探索範囲を決定すれば良い。
すなわち、カメラ４の姿勢パラメータＣＰの探索範囲
は、例えば図７に示すように、γ＝０の場合に対して、
ワーク５の所定位置としての原点Ｏｐの画像位置Ｏｐｓ
（画像情報に含まれていない場合には含まれている特徴
点を原点とする座標系に変換する）が画像の表示画面上
における画素単位の二次元座標（ｕ，ｖ）において、例
えば（Ｎａ，Ｎｂ）であるとすると、次式よりαおよび
βの推定値が計算できる。

【００３９】 α＝２×arctan｛（Ｎａ／ＮH ）× tan（ｗH ／２）｝ …（２２） β＝２×arctan｛（Ｎｂ／ＮV ）× tan（ｗV ／２）｝ …（２３） ただし、ＮH ；画像の表示画面上における水平方向の画
像サイズ ＮV ；画像の表示画面上における垂直方向の画像サイズ ｗH ；カメラの水平画角 ｗV ；カメラの垂直画角 である。

【００４０】この場合、要求誤差精度があまり高くない
場合には、線形近似として、 α＝（Ｎａ／ＮH ）×ｗH …（２４） β＝（Ｎｂ／ＮV ）×ｗV …（２５） という式で代用することもできる。

【００４１】そして、γ＝０でない場合には、ワーク５
の中心の画像位置Ｏｐｓを含む円環状の領域Ｓａ（Ｏｐ
ｓ）内にγの補正をした場合のＯｐｓｃが存在すると考
えられるので、Ｓａ（Ｏｐｓ）内の量子化空間点Ｏｐｓ
ｃについて式（２２），（２３）あるいは式（２４），
（２５）に基いてα，βを定め、 γ＝γｐｓｃ（ＯｐｓとＯｐｓｃとのなす角度） として、以降の探索工程を行うようにする。なお、本実
施例においては、自律移動ロボット１は、略水平な床の
上を移動することと、概略位置Ａ０の座標値を用いて高
精度にカメラ４の回転角を調整できるので、カメラ４の
光軸ずれに対する姿勢パラメータ（α，β，γ）のう
ち、最も広い探索能力を要求されるのは水平方向の角度
ずれを表すαである。したがって、β，γについては微
小範囲の探索を行えば良い。

【００４２】次に、透視変換行列演算部１３にて、空間
探索領域Ｓ１内の初期探索点として量子化空間点Ｅ
（ｋ，１）（ｋ＝１，２，…，Ｋ）のうちの１点である
量子化空間点Ｅ（１，１）を選択して（ステップＴ９）
その三次元絶対座標と、カメラ４の姿勢パラメータＣＰ
（α，β，γ）の初期値とを用いて、 透視変換行列Ｍ（ｋ，ｃ）（ｋ＝１，２，…，Ｋ、ｃ＝１，２，…，Ｃ） …（２６） を演算する（ステップＴ１０）。

【００４３】この場合、透視変換行列Ｍは、次のように
して演算される。すなわち、まず、ワーク５の原点Ｏｐ
に対して設定されている三次元絶対座標（Ｘ，Ｙ，Ｚ）
を基準として、初期探索領域Ｓ１を概略位置Ａ０のＸ座
標値であるＸ＝Ｘ0 の平面内にとり、この初期探索領域
Ｓ１内の三次元絶対座標（Ｘ，Ｙ，Ｚ）を三次元極座標
（ｒ，θ，φ）に変換する透視変換行列Ｍを求める。カ
メラ４の姿勢パラメータＣＰ（α，β，γ）と、焦点距
離特性ｆから求めたカメラ４の視点位置と射影平面に対
する距離ｄとに基いて、図６に示す関係に基いて透視変
換行列Ｍ（ｋ，ｃ）を求める。

【００４４】ここで、透視変換行列Ｍ（ｋ，ｃ）は次の
ように表すことができる。 Ｍ（ｋ，ｃ）＝Ｔ・Ｍ１・Ｍ２・Ｍ３ …（２７） ただし、 Ｔ ；回転，並行移動，拡大，縮小等の変換行列（ここ
では、カメラ４の姿勢パラメータＣＰで表される回転の
演算） Ｍ１；撮影しているワークの射影空間内への透視変換 Ｍ２；射影空間内の平面から射影平面への透視変換 Ｍ３；射影平面から画像の表示面への座標変換 このうち、画像の表示面とワーク５との間の透視変換を
演算するための透視変換行列Ｍ１とＭ２との積および座
標変換行列Ｍ３は、それぞれ、次式（２８），（２９）
のように表すことができる。

【００４５】

【数６】 また、カメラ４の光軸ずれに対応する姿勢パラメータＣ
Ｐ（α，β，γ）に応じた回転の演算を行うための変換
行列Ｔは、前述の図６と図８に示す関係とに基いて次式
（３０），（３１），（３２）のように表すことができ
る。なお、このとき、光軸ずれの変換においては、、ワ
ーク５との距離が変化しないと仮定すると、Ｔ0 ＝Ｔ1
とおくことができるので、次式（３３）の関係が成立す
る。

【００４６】

【数７】 次に、誤差演算部１４において、このようにして演算に
より求めた透視変換行列Ｍ（ｋ，ｒ）と、最小二乗推定
部１０で求めた推定透視変換行列Ｍｓとの間の誤差評価
関数Ｄ（１，１）を計算する（ステップＴ１１）。この
場合、誤差評価関数Ｄ（ｋ，ｃ）は透視変換行列Ｍと推
定透視変換行列Ｍｓとの各行列要素間の二乗誤差の和を
演算するものである。

【００４７】続いて、現在の量子化空間点Ｅ（１，１）
において姿勢パラメータＣＰ（α，β，γ）のうちγを
そのままにした状態でα，βを変更し、上述と同様にし
て誤差評価関数Ｄ（ｎE ，１）（ｎE ＝１，２，…，Ｎ
E ）を演算する（ステップＴ１２，１３，１０，１
１）。この後、これらの誤差評価関数Ｄ（ｎE ，１）か
ら最適値としてＤE1（αE ，βE ，γE ）を算出する
（ステップＴ１４）。続いて、誤差判定部１５におい
て、演算された誤差評価関数Ｄ（ｋ，１）の値から得ら
れた最適値ＤE1の演算値が、あらかじめ設定されている
検出条件としての要求誤差精度εの値に対して、 ＤE ｋ＜ε …（３４） とした判定条件を満たすか否かを判断する（ステップＴ
１５）。

【００４８】そして、上述の判定条件式（３７）を満た
していない場合には、以上の演算を初期探索領域Ｓ１内
における他の量子化空間点Ｅ（ｋ，１）についても同様
に実行する（ステップＴ１６，Ｔ１７を経てステップＴ
９〜Ｔ１５を実行）。さらに、他の量子化空間点Ｅ
（ｋ，１）について判定条件式（３４）を満たさない場
合には、そのときの最適値ＤE ｋのなかから要求誤差精
度εに一番近い値が得られた量子化空間点Ｅ（ｋ，１）
の位置に基づいて、新たな空間探索領域Ｓ２を設定し
（ステップＴ１８，Ｔ１９）、前述の演算工程を再び実
行する（ステップＴ５〜Ｔ１５）。

【００４９】一方、判定条件式（３４）の条件を満たし
た場合には（ステップＴ１５）、データ出力部１６によ
り、そのときの最適値ＤE ｋに相当する量子化空間点Ｅ
（ｋ，ｃ）の位置（ｒE ，θE ，φE ）の値およびカメ
ラ４の姿勢パラメータ（αE，βE ，γE ）の値を検出
データとして出力端子１７に出力し（ステップＴ２
０）、探索プログラムを終了する。

【００５０】また、最終の空間探索領域ＳＣの設定にも
拘らず判定条件式（３４）を満たさない場合には（ステ
ップＴ１８）、要求誤差精度εを満たす量子化空間点Ｅ
（ｋ，ｃ）が存在しないことをメッセージ出力した（ス
テップＴ２１）後、それまでの間に演算された最適値Ｄ
E ｋのなかから要求誤差精度εに一番近い値が得られた
量子化空間点Ｅ（ｋ，ｃ）の位置（ｒE ，θE ，φE ）
の値およびカメラ４の姿勢パラメータ（αE ，βE ，γ
E ）の値を検出データとして出力端子１７に出力して
（ステップＴ２０）探索プログラムを終了する。

【００５１】なお、上述の場合には、作業エリア２のワ
ーク５の原点Ｏｐから一定の距離（Ｘ＝ＸL1）におい
て、要求誤差精度εを満たす量子化空間点Ｅ（ｋ，１）
を求めるように、空間探索領域Ｓの設定をＳ１，Ｓ２，
…と設定する探索過程を示しているが、例えば、図１０
に示すように、自律移動ロボット１の進行によるワーク
５への近接（Ｘ＜ＸL1）するのに応じてその位置に応じ
た空間探索領域Ｓ（ｋ，ｃ）を設定する探索過程を実行
することもできる。

【００５２】すなわち、例えば、自律移動ロボット１の
進行過程において、原点Ｏｐからの距離Ｘに対応して一
つの空間探索領域Ｓ１を設定し、そのなかの量子化空間
点Ｅ（ｋ，１）のうち要求誤差精度εに一番近いものを
カメラ４の位置および姿勢パラメータを示す仮の検出デ
ータとして求め、その検出データに基いて自律移動ロボ
ット１の移動量を制御し、原点Ｏｐからの距離Ｘがさら
に小さくなったときに新たに空間探索領域Ｓ２を設定し
てさらに要求誤差精度εに一番近い量子化空間点Ｅ
（ｋ，２）を求めるようにし、以下、同様にして、ワー
ク５に自律移動ロボット１が最も近接したときに、最終
的に要求誤差精度εを満たすように探索過程を設定する
ようにしても良い。

【００５３】このように、本実施例においては、既知形
状の対象物体としての作業エリア２およびワーク５をカ
メラ４により撮像してその画像情報から画像特徴点Ｑｊ
を抽出し、最小二乗推定部１０により、抽出した画像特
徴点とワーク５の見え方のパターンとの間の特徴点の対
応リストを選択してカメラ４の現在位置と姿勢とにおけ
るワーク５の見え方を表すカメラ行列の最小二乗推定を
行って推定透視変換行列Ｍｓを求め、一方、量子化した
空間点を複数個有する空間探索領域Ｓｃを設定すると共
に、カメラ４の姿勢パラメータＣＰ（α，β，γ）を設
定してワーク５をその量子化空間点Ｅ（ｋ，ｃ）におい
て撮像したときに得られるべきワーク５の画像特徴点を
求めるための透視変換行列Ｍ（ｋ，ｃ）を演算し、それ
らの対応関係を求めて誤差評価関数Ｄ（ｋ，ｃ）により
要求誤差精度εを満たすか否かを判定してマッチングを
とることによりカメラ４の位置および姿勢パラメータＣ
Ｐを推定するようにしたので、以下のような効果を得る
ことができる。

【００５４】すなわち、第１に、従来と異なり、特殊マ
ーカを設ける必要がなくなり、マーカ認識用の移動時間
が不要となって演算時間を短縮して高速化を図れる。第
２に、透視変換行列の成分からパラメータを算出する方
法ではなく、所定密度で配置される量子化空間点Ｅを有
する空間探索領域Ｓｃを設定して透視変換行列を求め、
推定透視変換行列との間の適合度を評価するので、演算
量が膨大となることなく迅速に演算処理を実行できる。
第３に、カメラ４の姿勢パラメータＣＰ（α，β，γ）
が未知の場合でも適用できる。第４に、カメラ４の位置
を求めるための演算の精度を、量子化空間点Ｅの取り方
で調整することができる。つまり、例えば、作業内容や
環境の実情を考慮して遠距離においては粗い精度で高速
演算を行わせ、そしてワーク５に近接したときには細か
い精度で正確な演算を行なわせるように設定するという
ように、情況に応じた適切な精度に設定することができ
る。第５に、このように演算を実行するので、対象物体
としてのワーク５に対して、能動的にカメラ４が移動す
る際に、遠距離での光軸合わせ（粗い推定）から近距離
での高精度推定まで、移動系と連動して多段的に用いる
ことができる。

【００５５】

【発明の効果】以上説明したように、本発明の視覚に基
く三次元位置および姿勢の認識方法によれば、画像処理
工程により、形状を示す三次元絶対座標が既知である対
象物体を撮像手段により撮影したときの画像情報に基い
てその対象物体の形状の特徴を示す二次元の画像特徴点
を抽出し、最小二乗推定工程において、撮像手段の概略
位置と姿勢とに基いて、抽出した画像特徴点と記憶手段
に記憶された対象物体の見え方パターンとの間の対応リ
ストを選択し、カメラ行列の最小二乗推定により推定透
視変換行列を求め、探索領域設定工程，透視変換行列演
算工程，誤差演算工程，誤差判定工程およびデータ出力
工程により、撮像手段の概略的な推定位置に応じた探索
領域を設定してその探索領域における複数の量子化され
た空間点における対象物体に対する透視変換行列を演算
して前記推定透視変換行列との誤差を誤差評価関数によ
り評価して要求誤差精度を満たすときにそのときの量子
化空間点を撮像手段の三次元絶対座標位置として推定す
ると共に、撮像手段の姿勢パラメータを姿勢として出力
するようにしたので、撮像手段の光軸ずれがある場合で
も、特殊マーカなどを用いることなく、迅速且つ高精度
に撮像手段の位置と姿勢を認識することができ、しか
も、要求誤差精度の設定の仕方により演算速度と演算精
度との兼ね合いを距離に応じて適切に設定することがで
き、これらのデータに基いて対象物体に対する装置の移
動制御などを適確に行うことができるという優れた効果
を奏するものである。

【００５６】また、本発明の視覚に基く三次元位置およ
び姿勢の認識装置によれば、上述の各工程を実行する手
段により、同様の動作を行なわせることができ、実際の
撮像手段の三次元絶対座標に最も近い量子化空間点を近
似点として求めることができると共に、撮像手段の光軸
のずれ等を表す姿勢パラメータを検出することができる
ようになり、これらのデータに基いて対象物体に対する
装置の移動制御などを的確に行うことができるようにな
るという優れた効果を奏する。

【図面の簡単な説明】

【図１】本発明の一実施例を示す機能的構成のブロック
図

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

【図３】プロセスの流れを説明するフローチャート

【図４】種々の見え方パターン

【図５】空間探索領域と作業エリアとの位置関係を示す
作用説明図

【図６】絶対座標軸とカメラおよび画像の座標軸との関
係を示す図

【図７】画像表示部上のワークとカメラの姿勢パラメー
タとの関係を示す説明図

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

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

【図１０】探索領域をワークとの距離に応じて設定する
場合の作用説明図

【符号の説明】

１は自律移動ロボット、２は作業エリア、４はカメラ
（撮像手段）、５はワーク（対象物体）、７は画像処理
部、８は前処理計算部、９は特徴点抽出部、１０は最小
二乗推定部（最小二乗推定手段）、１１は量子化空間探
索部、１２は探索領域設定部（探索領域設定手段）、１
３は透視変換行列演算部（透視変換行列演算手段）、１
４は誤差演算部（誤差演算手段）、１５は誤差判定部
（誤差判定手段）、１６はデータ出力部（データ出力手
段）、１８はメモリ（記憶手段）、１９は外部情報入力
端子である。

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁵ 識別記号 庁内整理番号 ＦＩ 技術表示箇所 Ｇ０５Ｄ 3/12 Ｋ 9179−3Ｈ Ｇ０６Ｆ 15/62 ４１５ 9287−5Ｌ

Claims (2)

【特許請求の範囲】
1. 【請求項１】 形状を示す三次元絶対座標が既知である
   対象物体を撮像手段により撮影したときの画像情報に基
   いてその対象物体の形状の特徴を示す二次元の画像特徴
   点を抽出する画像処理工程と、 前記撮像手段の概略位置と姿勢とに基いて、前記画像処
   理工程で抽出した画像特徴点と記憶手段にあらかじめ記
   憶された前記対象物体の見え方のパターンとの間の特徴
   点の対応リストを選択すると共に、その対応リストに基
   いて前記撮像手段の現在位置と姿勢とにおける前記対象
   物体の見え方を表すカメラ行列の最小二乗推定を行って
   推定透視変換行列を求める最小二乗推定工程と、 前記撮像手段と前記対象物体との概略的な推定距離に応
   じて必要な演算精度に対応する密度の量子化空間点を有
   する探索領域を設定する探索領域設定工程と、 前記探索領域の量子化空間点において前記撮像手段によ
   り前記対象物体を撮影したときに得られるべき二次元の
   画像特徴点を算出するための透視変換行列を、あらかじ
   め記憶手段に記憶された前記対象物体の三次元絶対座標
   に対する前記量子化空間点の推定位置と前記撮像手段の
   撮像姿勢を示す姿勢パラメータとに基いて演算する透視
   変換行列演算工程と、 この透視変換行列演算工程にて求められた前記透視変換
   行列と前記最小二乗推定工程において求められた推定透
   視変換行列との間の適合度を示す評価誤差値を所定の誤
   差評価関数に基いて演算する誤差演算工程と、 この誤差演算工程にて求められた前記評価誤差値を前記
   探索領域に対応して設定された要求誤差精度と比較し、
   前記評価誤差値が前記要求誤差精度よりも小さくなるこ
   とを判定条件として、前記探索領域設定工程，前記透視
   変換行列演算工程および前記誤差演算工程を繰り返し実
   行させる誤差判定工程と、 この誤差判定工程において前記判定条件を満たしたとき
   に、そのときの量子化空間点の三次元絶対座標と前記撮
   像手段の姿勢パラメータとを出力するデータ出力工程と
   からなる視覚に基く三次元位置および姿勢の認識方法。
2. 【請求項２】 画像情報を出力する撮像手段と、 形状を示す三次元絶対座標が既知である対象物体を前記
   撮像手段により撮影したときの画像情報に基いてその対
   象物体の形状の特徴を示す二次元の画像特徴点を抽出す
   る画像処理手段と、 前記撮像手段の概略位置と姿勢とに基いて、前記画像処
   理手段により抽出した画像特徴点と記憶手段にあらかじ
   め記憶された前記対象物体の見え方のパターンとの間の
   特徴点の対応リストを選択すると共に、その対応リスト
   に基いて前記撮像手段の現在位置と姿勢とにおける前記
   対象物体の見え方を表すカメラ行列の最小二乗推定を行
   って推定透視変換行列を求める最小二乗推定手段と、 前記撮像手段と前記対象物体との概略的な推定距離に応
   じて必要な演算精度に対応する密度の量子化空間点を有
   する探索領域を設定する探索領域設定手段と、 前記探索領域の量子化空間点において前記撮像手段によ
   り前記対象物体を撮影したときに得られるべき二次元の
   画像特徴点を算出するための透視変換行列を、あらかじ
   め記憶手段に記憶された前記対象物体の三次元絶対座標
   に対する前記量子化空間点の推定位置と前記撮像手段の
   撮像姿勢を示す姿勢パラメータとに基いて演算する透視
   変換行列演算手段と、 この透視変換行列演算手段により求められた前記透視変
   換行列と前記最小二乗推定手段により求められた推定透
   視変換行列との間の適合度を示す評価誤差値を所定の誤
   差評価関数に基いて演算する誤差演算手段と、 この誤差演算手段にて求められた前記評価誤差値を前記
   探索領域に対応して設定された要求誤差精度と比較し、
   前記評価誤差値が前記要求誤差精度よりも小さくなるこ
   とを判定条件として、前記探索領域設定手段，前記透視
   変換行列演算手段および前記誤差演算手段に前記演算を
   繰り返し実行させる誤差判定手段と、 この誤差判定手段により前記判定条件が満たされたとき
   に、そのときの量子化空間点の三次元絶対座標と前記撮
   像手段の姿勢パラメータとを出力するデータ出力手段と
   からなる視覚に基く三次元位置および姿勢の認識装置。