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

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 とおくことができるので、式（２１）の
関係が成立する。

【００３９】

【数２】

【００４０】次に、このようにして演算により求めた透
視変換行列Ｍ（ｋ，ｃ）に基いて、透視変換演算部１３
により、作業エリア２とそこに載置されたワーク５のｎ
個の特徴点の集合Ｐの各特徴点Ｐｉ（式（４）参照）を
二次元の画像表示部の座標に透視変換の演算を実行し、
各特徴点Ｐｉに対する推定画像特徴点ＱEiを求める（ス
テップＴ９）。これらの推定画像特徴点ＱEiの集合ＱE
は、 ＱE ＝｛ＱEi；i ＝１，２，…，ｎ｝ （ｎ≧ｍ） …（２２） である。

【００４１】これにより、斉次座標を用いると、特徴点
の集合Ｐから推定画像特徴点の集合ＱE への変換は、式
（１６）から次式のように表すことができる。 （ｕEi，ｖEi，１）＝（Ｘｉ，Ｙｉ，Ｚｉ，１）・Ｍ …（２３） ただし、 （Ｘｉ，Ｙｉ，Ｚｉ）；特徴点Ｐｉの三次元絶対座標 （ｕEi，ｖEi） ；推定画像特徴点ＱEiの表示部上
の二次元座標 である。なお、この場合においては、量子化空間点Ｅ
（１，１）に対して求められた透視変換行列Ｍ（１，
１）を用いて特徴点の集合Ｐの推定画像特徴点の集合Ｑ
E を演算する。

【００４２】次に、画像処理部７にて抽出されたカメラ
４により撮影されたワーク５の画像特徴点の集合Ｑ（式
（１）参照）の各画像特徴点Ｑｊとあらかじめ記憶され
たワーク５の三次元の特徴点Ｐｉ（式（４）参照）との
間の対応関係をとった特徴点対応リストを作成する（ス
テップＴ１０）。これは、図９（ａ），（ｂ）に示すよ
うに、ワーク５の特徴点Ｐｉの位置と、量子化空間点Ｅ
（１，１）に応じた見え方パターン（図４（ｂ）参照）
との対比から、図９（ｂ）に示すように、各画像特徴点
Ｑｊを対応付けると、次の表に示すように画像情報とし
て表示面に表れた画像特徴点Ｑｊと対応する特徴点Ｐｉ
との対応関係が得られる。

【００４３】

【表１】

【００４４】なお、この場合において、特徴点Ｐｉに対
して対応する観測された画像特徴点Ｑｊが全て存在しな
い場合があるので、対応する画像特徴点Ｑｊが存在する
場合に「存在フラグ」が「１」となるようになってい
る。また、抽出の順序は、画像の表示部上で左上から右
下に向かって順次行っている。

【００４５】続いて、誤差演算部１４により、対応関係
が得られた画像特徴点Ｑｊと前述の推定画像特徴点の集
合ＱE （＝Ｐ・Ｍ（１，１））との間の誤差評価関数Ｄ
（１，１）を計算する（ステップＴ１１）。この場合、
誤差評価関数Ｄ（ｋ，ｃ）は画像特徴点Ｑｊと推定画像
特徴点ＱEiとの間の距離に相当する値を演算するもので
ある。続いて、現在つまり１番目の量子化空間点Ｅ
（１，１）において姿勢パラメータＣＰ （α，β，
γ）のうちγをそのままにした状態でα，βを変更し、
上述と同様にして誤差評価関数Ｄ（ｎE ，１）（ｎE ＝
１，２，…，ＮE ）を演算する（ステップＴ１２，１
３，８〜１１）。

【００４６】この後、探索領域Ｓ１中のｋ番目の量子化
空間点Ｅ（ｋ，１）におけるこれらの誤差評価関数Ｄ
（ｎE ，１）から上述の演算を行なうことで最適値とし
てＤE1（αE ，βE ，γE ）を算出する（ステップＴ１
４）。ここで、通常、この最適値の決定は、観測される
全特徴点についてＱE とＱとの間の二乗誤差和を評価関
数とし、その最小値演算を行うことにより実行されるよ
うになっている。

【００４７】続いて、誤差判定部１５において、上記誤
差演算部１４でｋ番目の量子化空間点Ｅ（ｋ，１）につ
いて演算された誤差評価関数Ｄ（ｋ，１）の値から得ら
れた最適値ＤE1（αE，βE，γE）の演算値ＤE ｋが、
探索領域Ｓ１のｋ番目の量子化空間点Ｅ（ｋ，１）の検
出条件としてあらかじめ設定されている要求誤差精度ε
の値に対して、 ＤE ｋ＜ε …（２４） とした判定条件を満たすか否かを判断する（ステップＴ
１５）。

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

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

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

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

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

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

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

【００５５】

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

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

【図面の簡単な説明】

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

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

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

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

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

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

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

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

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

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

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

───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.⁷，ＤＢ名) B25J 3/00 - 3/04 B25J 9/10 - 9/22 B25J 13/00 - 13/08 B25J 19/02 - 19/06 G01B 11/00

Claims (2)

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