JP2011027623A - 位置姿勢計測方法及び装置 - Google Patents

Abstract

【課題】 カメラ及び距離センサを用いて高精度に物体の位置姿勢を計測する。
【解決手段】 物体の二次元画像の画像特徴と、保持された物体の位置姿勢における三次元形状モデルが二次元画像に投影される投影像とのずれを算出する。三次元座標情報と、保持された位置姿勢における三次元形状モデルとのずれを算出する。双方のずれの尺度を、同等の尺度になるように変換して、保持された位置姿勢を補正する。
【選択図】 図５

Description

本発明は、三次元形状が既知である物体の位置姿勢を計測する技術に関する。

近年のロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットが、ハンドなどのエンドエフェクタによって部品を把持して組立を行う。この組立には、把持の対象となる部品とロボット（ハンド）との間の相対的な位置姿勢を計測する必要がある。

位置姿勢を計測する方法として、二次元画像上から検出される特徴や距離画像に対して物体の三次元形状モデルをあてはめるモデルフィッティングによる計測がある。二次元画像に対するモデルフィッティングでは、物体の位置姿勢に基づいて三次元形状モデルを画像上に投影したときの投影像が、検出された特徴に当てはまるように位置姿勢を推定する。距離画像に対するモデルフィッティングでは、距離画像の各点を三次元座標を持つ三次元点群に変換し、三次元空間において三次元形状モデルが三次元点群に当てはまるように位置姿勢を推定する。

しかし、二次元画像上での特徴の検出位置や点群の三次元座標には、画素の量子化誤差やブラー、特徴検出アルゴリズムの精度、カメラ間の対応等に起因する誤差が存在する。そこで、多数の計測情報（画像上の特徴や点群）に含まれる計測誤差の影響を平均化する等、位置姿勢の計測精度を向上させる取り組みがなされている。

日浦，山口，佐藤，井口，"距離画像と濃淡画像の融合による任意形状物体の実時間追跡"，電子情報通信学会論文誌Ｄ−ＩＩ，ｖｏｌ．Ｊ８０−ＤＩＩ，ｎｏ．１１，ｐｐ．２９０４−２９１１，１９９７． Ｒ．Ｙ．Ｔｓａｉ，"Ａ ｖｅｒｓａｔｉｌｅ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｈｉｇｈ−ａｃｃｕｒａｃｙ ３Ｄ ｍａｃｈｉｎｅ ｖｉｓｉｏｎ ｍｅｔｒｏｌｏｇｙ ｕｓｉｎｇ ｏｆｆ−ｔｈｅ−ｓｈｅｌｆ ＴＶ ｃａｍｅｒａｓ ａｎｄ ｌｅｎｓｅｓ，"ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ｖｏｌ．ＲＡ−３，ｎｏ．４，１９８７． Ｌ．Ｖａｃｃｈｅｔｔｉ，Ｖ．Ｌｅｐｅｔｉｔ，ａｎｄ Ｐ．Ｆｕａ，"Ｃｏｍｂｉｎｉｎｇ ｅｄｇｅ ａｎｄ ｔｅｘｔｕｒｅ ｉｎｆｏｒｍａｔｉｏｎ ｆｏｒ ｒｅａｌ−ｔｉｍｅ ａｃｃｕｒａｔｅ ３Ｄ ｃａｍｅｒａ ｔｒａｃｋｉｎｇ，"Ｐｒｏｃ．３ｒｄ ＩＥＥＥ／ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ’０４），ｐｐ．４８−５７，２００４． Ｗ．Ｈｏｆｆ ａｎｄ Ｔ．Ｖｉｎｃｅｎｔ，"Ａｎａｌｙｓｉｓ ｏｆ ｈｅａｄ ｐｏｓｅ ａｃｃｕｒａｃｙ ｉｎ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ，"ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｖｉｓｕａｌｉｚａｔｉｏｎ ａｎｄ Ｃｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ，ｖｏｌ．６，ｎｏ．４，ｐｐ．３１９−３３４，２０００．

高精度に位置姿勢を計測する方法として、明示的な特徴検出は行わず、濃淡画像と距離画像の勾配を利用した位置姿勢の推定するがある（非特許文献１参照）。つまり、運動による明度変化、距離変化を滑らかであると仮定し、濃淡画像の明度変化と距離画像の距離変化から勾配法に基づき物体の姿勢パラメータを算出する。しかし、濃淡画像の二次元と距離画像の三次元では、尺度が異なり、有効に組み合わせることが難しく、人手によってチューニングすることでしか、対応することができないという課題があった。

本発明は、以上の課題を鑑みてなされたものであり、二次元画像から得られる計測情報と、距離画像から得られる計測情報を巧妙に併用して位置姿勢推定することにより、さまざまな物体の位置姿勢を高精度に計測することを目的とする。

上記の目的は、以下の方法によって達成される。

物体の位置姿勢を計測する位置姿勢計測方法であって、画像入力手段が、前記物体の二次元画像を入力する画像入力工程と、三次元座標情報入力手段が、前記物体の表面の三次元座標情報を入力する三次元座標情報入力工程と、特徴検出手段が、前記二次元画像から画像特徴を検出する特徴検出工程と、二次元画像ずれ算出手段が、前記検出された画像特徴と、予め保持された三次元形状モデルを、予め保持された前記物体の位置姿勢で、前記二次元画像に投影したときの投影像の投影特徴との、第１のずれを算出する二次元画像ずれ算出工程と、三次元空間ずれ算出手段が、前記三次元座標情報の三次元特徴と、前記保持された位置姿勢における前記三次元形状モデルのモデル特徴との、第２のずれを算出する三次元空間ずれ算出工程と、同等尺度変換手段が、前記第１のずれの尺度及び／又は前記第２のずれの尺度を、同等の尺度になるように変換する同等尺度変換工程と、位置姿勢補正手段が、前記第１のずれ及び前記第２のずれに基づいて、前記保持された位置姿勢を補正する位置姿勢補正工程とを有することを特徴とする位置姿勢計測方法。

本発明により、二次元画像から得られる計測情報と、距離画像から得られる計測情報を巧妙に併用して位置姿勢推定することにより、さまざまな物体の位置姿勢を高精度に計測することができる。

本実施形態における位置姿勢計測装置の構成を示す図である。 本実施形態における三次元形状モデルを説明する図である。 第１の実施形態における位置姿勢計測処理のフローチャートである。 画像からのエッジ検出を説明する図である。 第１の実施形態における位置姿勢算出手段の構成を示す図である。 第１の実施形態における位置姿勢算出処理のフローチャートである。 線分の投影像と検出されたエッジの関係を説明する図である。 画像上の誤差を三次元空間中の誤差に近似する方法を説明する図である。 第３の実施形態における具体的な位置姿勢算出処理のフローチャートである。

以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。

［第１の実施形態］
第１の実施形態では、二次元画像から得られる計測情報（画像特徴）と、距離画像から得られる計測情報（三次元点群）を同時に利用したモデルフィッティングにより物体の位置姿勢を計測する。前述した二次元画像または距離画像を利用する方法はどちらも「各計測情報について、画像上もしくは三次元空間中での誤差が位置姿勢を補正することによって相殺されるように、位置姿勢の補正値を未知変数とした線形方程式を立式する」方法である。そのため、二次元及び三次元の計測情報の夫々について線形方程式を立式し、連立方程式を解くことによって双方の計測情報を同時に利用した位置姿勢推定を行うことができる。しかしながら、画像上での誤差と三次元空間中での誤差は評価尺度が異なるため、どちらか一方の計測情報の影響が極端に強くなり、同時に利用する利点が薄れる。そこで本実施形態では、二次元画像上の誤差が三次元空間中の誤差に相当するように評価尺度を統一する。

図１は、本実施形態における位置姿勢計測装置１の構成を示している。同図に示すように、位置姿勢計測装置１は、三次元形状モデル保持部１１０、概略位置姿勢入力部１２０、二次元画像入力部１３０、画像特徴検出部１４０、三次元座標情報入力部１５０、位置姿勢算出部１６０から構成されている。また、位置姿勢計測装置１には、二次元画像撮像部１００、三次元座標情報計測部１０５が接続されている。以下、位置姿勢計測装置１を構成する各部について説明する。

二次元画像撮像部１００は、通常の二次元画像を撮像するカメラである。撮像される二次元撮像画像は濃淡画像であってもよいしカラー画像であってもよい。

二次元画像入力部１３０は、二次元画像撮像部１００が撮像する画像を位置姿勢計測装置１に入力する。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは事前にキャリブレーションしておいてもよい（非特許文献２参照）。

三次元座標情報計測部１０５は、計測対象である物体表面上の点の三次元情報を計測する。本実施形態では、三次元座標情報計測部１０５として距離画像を出力する距離センサを用いる。距離画像は、各画素が奥行きの情報を持つ画像である。距離センサとしては、対象に照射したレーザ光の反射光をカメラで撮像し、三角測量により距離を計測するアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。これらのアクティブ式の距離センサは、表面のテクスチャが少ない物体が対象であるとき好適である。また、ステレオカメラが撮像する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。パッシブ式の距離センサは、表面のテクスチャが十分ある物体を対象とする場合に好適である。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。

三次元座標情報入力部１５０は、三次元座標情報計測部１０５が計測した三次元情報を取得する。既知である距離センサとカメラの相対的な位置姿勢をもとに、距離画像の各画素をカメラ座標系における三次元座標情報である点群データに変換して位置姿勢計測装置１に入力する。距離センサとカメラは相互に固定されており、両者の相対的な位置姿勢は変化しないと仮定し、事前にキャリブレーションしておいてもよい。例えば、三次元形状が既知であるキャリブレーション物体を様々な方向から観察し、二次元画像に基づいたキャリブレーション物体の位置姿勢と、距離画像に基づいたキャリブレーション物体の位置姿勢の差をもとに、相対的な位置姿勢を求める。

なお、カメラによる画像の撮像と、距離センサによる距離の計測は同時に行われると仮定する。ただし、対象物体が静止している場合など、位置姿勢計測装置１と対象物体との位置姿勢が変化しない場合には、必ずしも同時に行う必要はない。

三次元形状モデル保持部１１０は、位置姿勢を計測する対象である物体の三次元形状モデルを保持する。本実施形態では、物体を線分と面から構成される三次元形状モデルとして記述する。

図２は本実施形態における三次元形状モデルを説明する図である。三次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図２（ａ）に示すように、観察対象物体２０１の三次元形状モデルは点Ｐ１〜点Ｐ１４の１４点から構成される。また図２（ｂ）に示すように、観察対象物体２０１の三次元形状モデルは線分Ｌ１〜Ｌ１６により構成されている。図２（ｃ）に示すように、点Ｐ１〜点Ｐ１４は三次元座標値によって表される。また図２（ｄ）に示すように、線分Ｌ１〜Ｌ１６は、線分を構成する点のＩＤによって表される。さらに三次元幾何モデルは、面の情報を保持する。各面は、各面を構成する点のＩＤによって表される。図２に示す三次元形状モデルでは、直方体を構成する６つの面の情報が記憶されている。三次元形状モデルは、位置姿勢算出部１６０において物体の位置姿勢を算出する際に用いられる。

概略位置姿勢入力部１２０は、位置姿勢計測装置１に対する物体の位置姿勢の概略値を入力する。ここで位置姿勢計測装置１に対する物体の位置姿勢とは、カメラ座標系における物体の位置姿勢を表す。しかしながら、カメラ座標系に対する相対的な位置姿勢が既知でかつ変化しなければ、位置姿勢計測装置１のいずれの部分を基準としてもよい。

本実施形態では、位置姿勢計測装置１は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置姿勢として用いる。しかしながら、位置姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置姿勢の計測をもとに物体の速度や角速度を時系列フィルタにより推定し、過去の位置姿勢と推定された速度・加速度から現在の位置姿勢を予測したものでもよい。

また、他のセンサによる物体の位置姿勢の計測が可能である場合には、該センサによる出力値を位置姿勢の概略値として用いてもよい。センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置姿勢を計測する磁気式センサであってもよい。また、物体上に配置されたマーカをシーンに固定されたカメラによって撮像することにより位置姿勢を計測する光学式センサであってもよい。その他、６自由度の位置姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢があらかじめわかっている場合にはその値を概略値として用いる。

画像特徴検出部１４０は、二次元画像入力部１３０から入力された二次元画像から画像特徴を検出する。本実施形態では画像特徴としてエッジを検出する。

位置姿勢算出部１６０は、画像特徴検出部１４０で検出された画像特徴に対して、三次元形状モデル保持部１１０に保持される三次元形状モデルを当て嵌める。また、三次元座標情報入力部１５０によって入力された三次元点群に対しても、三次元形状モデルを当て嵌める。これの当て嵌めによって、物体の位置姿勢を計測する。

図３は、第１の実施形態における位置姿勢の計測の処理手順を示すフローチャートである。

ステップＳ３０１において、位置姿勢計測装置１（カメラ）に対する物体の位置姿勢の概略値を概略位置姿勢入力部１２０により三次元計測装置１に入力する。前述したように、本実施形態では、前の時刻において計測された位置姿勢を用いる。

ステップＳ３０２において、モデルフィッティングにより物体の位置姿勢を算出するための計測情報を取得する。具体的には、対象物体の二次元画像と三次元情報を取得する。

本実施形態では、三次元座標情報計測部１０５は三次元情報として距離画像を出力するものとする。各画素に濃淡値やカラー値が記録されている二次元画像と異なり、距離画像は各画素に視点位置からの奥行き値が記録されている。二次元画像撮像部１００によって撮像された二次元画像は二次元画像入力部１３０を介して位置姿勢計測装置１に入力される。また、三次元座標情報計測部１０５から出力された距離画像は、三次元座標情報入力部１５０を介して位置姿勢計測装置１に入力される。前述のように、距離画像はカメラ座標系における三次元座標情報である三次元点群データに変換されて入力される。距離画像から三次元点群への変換は、距離画像の各画素について、画素位置に対応する視線ベクトルに奥行き値を乗算することにより行う。

ステップＳ３０３において、ステップＳ３０２で入力された二次元画像上での画像特徴の検出を行う。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配の極値となる。

図４は、本実施形態におけるエッジ検出を説明する図である。まず、ステップＳ３０１において入力された計測対象物体の概略位置姿勢と校正済みの二次元画像撮像部１００の内部パラメータを用いて、三次元形状モデルを構成する各線分の画像上への投影像を算出する。線分の投影像は画像上でも線分となる。

次に、画像上で等間隔になるように投影された線分４０１上に制御点４０２を設定し、制御点４０２ごとに、投影された線分４０１の法線方向４０３に１次元のエッジ４０４の検出を行う（図４（ａ））。エッジは画素値の濃度勾配４０５の極値として検出されるため、図４（ｂ）に示すように、近傍にエッジが存在する場合には複数のエッジ４０６が検出されることがある。本実施形態では、検出されるエッジをすべて仮説として保持する（非特許文献３参照）。

図５は、本実施形態における位置姿勢算出手段の構成内容である。

ステップＳ３０４において、ステップＳ３０３で検出されたエッジ及びステップＳ３０２で入力された三次元点群に三次元形状モデルを当てはめることにより、計測対象物体の位置姿勢を算出する。

二次元画像ずれ算出部５０１は、画像上で検出されたエッジである画像特徴と、推定される位置姿勢に基づいて画像上に投影される線分である投影特徴との距離を算出する。

三次元空間ずれ算出部５０２は、点群データを構成する各点である三次元特徴と、位置姿勢に基づいて撮像部１５０の座標系に変換される面であるモデル特徴との距離を算出する。

同等尺度変換部５０３は、算出した双方の距離に基づいて位置姿勢の最適化を行う。より具体的には、二次元画像上での点と直線の符号付距離を算出し、三次元空間中での点と平面との符号付距離を算出し、双方の符号付距離を物体の位置姿勢の関数として線形近似する。そして、符号付距離を０としたときに各計測情報について成り立つ線形方程式を用意する。

位置姿勢補正部５０４は、この線形方程式を連立方程式として解くことにより、物体の位置姿勢の微小変化を求め、位置姿勢を補正する。この処理を繰り返すことにより最終的な位置姿勢を算出する。

前述したように、画像上の距離と三次元空間中での距離は尺度が異なるため、単純に連立方程式を解くだけでは計測情報のどちらか一方に寄与率が偏ってしまう。そのような場合、２種類の計測情報を利用する利点が薄れ、双方を用いることによる精度向上が期待できなくなる。そこで、本実施形態では、二次元画像上の距離を三次元空間中の距離に変換し、尺度を統一することによって寄与率が偏ることをなくす。以下、位置姿勢算出の処理手順について順に説明する。

図６はステップＳ３０４における位置姿勢算出の具体的な処理手順を説明するフローチャートである。

本ステップでは、計測情報に三次元形状モデルが当てはまるように、非線形最適化手法の一つであるＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いて計測対象物体の位置姿勢（以下、６次元ベクトルｓで表す）の概略値を反復演算によって繰り返し補正する。なお、計測対象物体の位置姿勢の算出方法はＧａｕｓｓ−Ｎｅｗｔｏｎ法に限るものではない。例えば、より計算がロバストであるＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法によって行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。

ステップＳ６０１において、初期化処理を行う。ここでは、ステップＳ３０１で得られた計測対象物体の概略の位置姿勢を位置姿勢算出における概略値として入力する。

ステップＳ６０２において、三次元形状モデルと計測情報の対応付けを行う。

まず、画像特徴の対応付けを行う。ステップＳ３０３では、各制御点に関して複数のエッジが仮説として検出された。本ステップでは、検出された複数のエッジのうち、位置姿勢の概略値に基づいて投影された線分に画像上で最も近いエッジを制御点と対応付ける。

次に、点群データの対応付けを行う。まず、位置姿勢の概略値に基づいて三次元形状モデルもしくは点群データを座標変換し、点群データ中の各点につき、三次元空間において最も近傍する面を探索し対応付ける。

ステップＳ６０３において、位置姿勢を算出するための係数行列と誤差ベクトルの算出を行う。ここで係数行列の各要素は、エッジについては画像上での点と直線の距離、点群データについては三次元空間中での点と平面の距離を位置姿勢の関数としたときの、位置姿勢の各要素に関する一次の偏微分係数である。誤差ベクトルは、エッジについては投影された線分と検出されたエッジの画像上での符号付き距離、点群データについては点とモデルの面の三次元空間中での符号付き距離である。

次に、係数行列の導出について説明する。

図７は、線分の投影像と検出されたエッジの関係を説明する図である。図７では、画像の水平方向、垂直方向をそれぞれｕ軸７０１、ｖ軸７０２としている。ある制御点７０３（投影された各線分を画像上で等間隔に分割した点）の画像上での座標７０４を（ｕ０，ｖ０）、該制御点が所属する線分の画像上での傾きをｕ軸７０１に対する傾きθ７０５と表す。傾きθ７０５は、線分７０６の両端の三次元座標をｓに基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。該線分７０６の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、該制御点７０３の対応点７０７の画像上での座標７０８を（ｕ’，ｖ’）とする。ここで、対応点７０７の座標７０８（ｕ’，ｖ’）を通り、傾きがθ７０５である直線（図７の破線）上の点（ｕ，ｖ）は、
ｕ ｓｉｎθ−ｖ ｃｏｓθ＝ｄ ・・・（１）
と表せる（θは定数とする）。ここで、
ｄ＝ｕ’ｓｉｎθ−ｖ’ｃｏｓθ
（定数）である。

制御点７０３の画像上での位置は計測対象物体の位置姿勢により変化する。また、計測対象物体の位置姿勢の自由度は６自由度である。すなわちｓは６次元ベクトルであり、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す三次元ベクトルなどによって表現される。位置姿勢により変化する点の画像上での座標（ｕ，ｖ）は、座標７０４（ｕ_０，ｖ_０）の近傍で１次のテイラー展開によって（２）のように近似できる。但し△ｓ_ｉ（ｉ＝１，２，・・・，６）はｓの各成分の微小変化を表す。

位置姿勢の概略値と実際の位置姿勢の差がそれほどないと仮定すると、正しいｓによって得られる制御点の画像上での位置は（１）が表す直線上にあると仮定できる。（２）によって近似されるｕ、ｖを（１）に代入することにより、（３）が得られる。

ただし、
ｒ＝ｕ_０ｓｉｎθ−ｖ_０ｃｏｓθ
（定数）である。（３）はステップＳ６０２において対応付けが行われたすべてのエッジについて立式することができる。

撮像部１５０の座標系（カメラ座標系）で表される点群の三次元座標は、計測対象物体の位置姿勢ｓを用いて計測対象物体の座標系における三次元座標（ｘ，ｙ，ｚ）に変換される。概略の位置姿勢によって、点群データ中のある点が計測対象物体座標（ｘ_０，ｙ_０，ｚ_０）に変換されるとする。（ｘ，ｙ，ｚ）は計測対象物体の位置姿勢により変化するものであり、（ｘ_０，ｙ_０，ｚ_０）の近傍で１次のテイラー展開によって（４）のように近似できる。

点群データ中のある点に、ステップＳ６０２において対応付けられた三次元幾何モデルの面の、計測対象物体座標系における方程式をａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ^２＋ｂ^２＋ｃ^２＝１、ａ，ｂ，ｃ，ｅは定数）とする。正しいｓによって変換される（ｘ，ｙ，ｚ）は、平面の方程式ａｘ＋ｂｙ＋ｃｚ＝ｅを満たすと仮定する。（４）を平面の方程式に代入すると（５）が得られる。

ただし、
ｑ＝ａｘ_０＋ｂｙ_０＋ｃｚ_０
（定数）とした。（５）はＳ６０２において対応付けが行われたすべての点群データについて立式することができる。

（３）、（５）はｓの各成分の微小変化△ｓ_ｉ（ｉ＝１，２，・・・，６）についての方程式であるため、（６）のような△ｓ_ｉに関する線形連立方程式を立てることができる。

繰り返しになるが、（６）の右辺の誤差ベクトルは、エッジについては画像上での符号付距離、点群については三次元空間中での符号付距離であるため、尺度が一致していない。そこで、画像上での誤差にエッジの奥行きを乗じることにより三次元空間中の誤差に近似する。具体的には、エッジについての方程式の両辺に、エッジの奥行きを乗じることによって尺度を三次元空間中の距離に統一する。二次元画像からは奥行き情報を得ることはできないため、エッジの奥行きは何らかの近似的な方法により求める必要がある。

図８は、本実施形態において画像上の誤差８０１を三次元空間中の誤差８０２に近似する方法を説明する図である。本実施形態では、位置姿勢の概略値によって算出された各制御点８０３の視点８０４からの奥行き８０５を、画像上の誤差８０１に乗じることによって三次元空間中の誤差８０２に変換する。また、奥行き８０５の代わりに、三次元空間中の制御点８０３から画像面８０６におけるエッジ８０７を通過する視線ベクトルに対して下ろした垂線の長さとなるようなスケーリング係数を乗じてもよい。解くべき連立方程式は（７）のようになる。

ｚ_１、ｚ_２・・・は各エッジの奥行きを表している。
ここで（７）を（８）のように表す。
Ｊ△ｓ＝Ｅ ・・・（８）
線形連立方程式の係数行列Ｊを算出するため偏微分係数の算出を行う。

ステップＳ６０４において、（８）をもとに、行列Ｊの一般化逆行列（Ｊ^Ｔ・Ｊ）−１・Ｊ^Ｔを用いて位置姿勢の補正値△ｓを最小二乗基準で求める。しかしながら、エッジや点群データには誤検出などによる外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジや点群データでは、（７）の右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きい情報には小さな重みを与え、誤差が小さい情報には大きな重みを与える。重みは例えば（９）に示すようなＴｕｋｅｙの関数により与える。

ｃ_１，ｃ_２は定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差が大きい情報には小さな重みを与え、誤差が小さい情報には大きな重みを与える関数であればなんでもよい。各計測情報（エッジまたは点群データ）に対応する重みをｗ_ｉとする。ここで（１０）のように重み行列Ｗを定義する。

重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗ_ｉが入る。この重み行列Ｗを用いて、（８）を（１１）のように変形する。
ＷＪ△ｓ＝ＷＥ ・・・（１１）
（１１）を（１２）のように解くことにより補正値△ｓを求める。

ステップＳ６０５において、ステップＳ６０４において算出された位置姿勢の補正値△ｓにより、位置姿勢の概略値を補正する。
ｓ←ｓ＋△ｓ
ステップＳ６０６において、収束判定を行い、収束していれば終了し、そうでなければステップＳ６０２に戻る。収束判定では、補正値△ｓがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判定する。このように、収束するまで繰り返すことで、位置姿勢が算出できる。

以上述べたように、第１の実施形態では、二次元画像上の誤差を三次元空間中の誤差に近似的に変換し、二次元画像と点群データの誤差を同一とみなせる尺度に統一して同時に利用する位置姿勢の計測方法について説明した。

［変形例１−１］
第１の実施形態では、二次元画像上の誤差を三次元空間中の誤差に変換する際に、画像上のエッジの奥行きとして、対応する制御点の位置姿勢の概略値から算出される奥行きを用いている。しかしながら、エッジの奥行きは他の方法によって求めてもよい。例えば、距離センサが密な距離情報を計測可能であり、二次元画像上の各画素に対応する距離情報を距離センサから得ることができる場合には、距離センサが計測する距離情報をエッジの奥行きとして用いてもよい。

［変形例１−２］
第１の実施形態では、画像上で検出されるエッジについて、奥行きを個別に算出していた。しかしながら、例えば計測対象物体が位置姿勢計測装置から十分に離れていて物体全体を一つの奥行きで表現できる場合などには、平均的な奥行きを用いてもよい。平均的な奥行きは、個々の制御点の奥行きから求めてもよいし、距離画像から求めてもよい。平均的な奥行きを用いることにより、誤対応や距離の計測誤差に起因する外れ値の影響を軽減することが期待される。

［変形例１−３］
第１の実施形態では、外れ値の影響を軽減するためのロバスト推定において、三次元空間における誤差に基づいて計測情報の重み付けを行っていた。しかしながら、重み付け方法はこれに限るものではなく、重み付けを二次元画像上の誤差に基づいて行ってもよい。すなわち（１３）のように重み付けを行ってもよい。

ここでｃ_３は定数である。

［変形例１−４］
第１の実施形態では、入力される概略の位置姿勢に基づいてエッジ検出を行って複数の仮説を検出し、その後の繰り返しループでは位置姿勢の概略値に基づいて投影される線分に最も近い仮説を制御点に対応するエッジとして選択していた。すなわち、エッジ検出は一回の計測につき一回しか行わなかった。しかしながら、計算時間に余裕がある場合などには、対応点を選択する代わりにエッジ検出を繰り返しループの中に入れて、位置姿勢の概略値が補正されるたびにエッジ検出を行ってもよい。これにより、最初に入力される概略の位置姿勢と真の位置姿勢との差が大きい場合であっても、繰り返し回数が増えるごとに正しい対応のエッジが検出され、高精度な位置姿勢の計測が可能になる。

［第２の実施形態］
第１の実施形態では、画像上の誤差が三次元空間の誤差に相当するように変換を行うことにより、同一の評価尺度のもとで二次元画像と距離画像を同時に用いた位置姿勢の推定を行う方法について説明した。第２の実施形態では、計測情報に三次元幾何モデルを当てはめる問題を最尤推定問題として捉え、発生確率を共通の評価尺度として用いる。第２の実施形態における位置姿勢計測装置の構成や位置姿勢の計測の処理手順は第１の実施形態と同一であるため説明を省略する。

本実施形態では、発生確率とは、ある位置姿勢が与えられたときに、与えられた位置姿勢に基づいて算出される値（予測値）と実際の計測値との誤差が発生する尤度を表すものとする。本実施形態では、画像から検出されるエッジは探索ラインの方向にのみあいまい性を持つと仮定し、エッジの検出誤差が平均０、標準偏差σ_２Ｄの１次元ガウス分布に従うと仮定する。実画像におけるσ_２Ｄを推定するのは困難である。そのため、エッジの検出誤差は主に画像の量子化誤差に起因するとして、例えばσ_２Ｄを１画素に設定する。予測値と計測値との誤差を「エッジと投影された線分との距離」としてｅｒｒ_２Ｄ（スカラ値）とすると、発生確率は（１４）のように表される。

また、距離センサによって計測される三次元点群の計測誤差は、平均０、共分散行列Σの三次元ガウス分布に従うと仮定する。共分散行列Σは３×３行列であり、対角成分には軸内の分散、非対角成分には軸同士の共分散が入る。Σの推定値は、例えば距離センサの仕様として公開されている計測精度をもとに算出することができる。

第１の実施形態で説明した方法（三次元の点と対応する面の距離を最小化する方法）では、面の法線方向のあいまい性のみが位置姿勢の算出に寄与する。そこで、点群の計測誤差の共分散行列から面の法線方向の計測誤差の標準偏差σ_３Ｄを求める。具体的には、面の座標系とカメラ座標系の間の回転行列をＲとして、共分散行列Σに対しＲＴΣＲの変換をかけることにより面の座標系における共分散行列に変換し、法線ベクトル方向の標準偏差を抽出する。位置姿勢に基づく予測値と実際の計測値との間の誤差（三次元の点と面の距離）をｅｒｒ_３Ｄとすると、発生確率は（１５）のように表される。

最尤推定は、各計測情報の発生確率の積

が最大となるように未知パラメータ（ここでは物体の位置姿勢）を推定する。実際には、発生確率の積の対数を取り、その符号を反転した以下の数を最小化する問題に置き換えられる。

このうち第１項と第３項は位置姿勢に依存しない定数であるため、実際には（１６）

を最小化する問題となる。第１の実施形態と第２の実施形態の違いは、位置姿勢算出アルゴリズムにおける係数行列と誤差ベクトルの算出部分である。

以下、（１６）を最小化するための位置姿勢の算出方法について説明する。エッジに関して得られる方程式

には、エッジ検出誤差の標準偏差σ_２Ｄの逆数を乗じる。

点群に関して得られる方程式

については、面の法線方向の計測誤差の標準偏差σ_３Ｄの逆数を方程式に乗じる。その結果得られる線形連立方程式は式１７のようになる。

（１７）をもとに位置姿勢の補正値の算出を行う。その他の処理は第１の実施形態と同一であるので説明を省略する。

以上述べたように、第２の実施形態では、計測情報の発生確率を統一の尺度として、二次元画像と距離画像を同時に用いた位置姿勢の計測を行う方法について説明した。

［変形例２−１］
第２の実施形態では、点群データの各点について、同一の共分散行列Σを利用していた。しかしながら、すべての点情報について共分散行列が同一である必要はない。距離センサが画素や点といった単位で計測の信頼度を出力することが可能である場合には、その信頼度をもとに点ごとに共分散行列を算出し、それを利用することも可能である。

［変形例２−２］
第２の実施形態では、エッジ検出誤差の分布はすべて同一の標準偏差に従うとしていたが、これに限るものではない。すなわち、画像上で検出されるエッジごとに検出のあいまい性を推定することができる場合には、そのあいまい性をもとに標準偏差を算出し、エッジごとに変えてもよい。エッジ検出のあいまい性は、例えばエッジ検出に利用されるカーネルサイズなどを用いることができる。これによって、あいまいなエッジは小さく重み付けられ、高精度に検出されたエッジは大きく重み付けられるため、より高精度な位置姿勢の算出が可能となる。

［第３の実施形態］
第１、第２の実施形態では、二次元画像及び三次元計測情報を同時に利用して位置姿勢の算出を行った。第３の実施形態では、２種類の計測情報を同時に利用するのではなく、二次元画像のみからの位置姿勢算出と、三次元点群のみからの位置姿勢算出を別々に行い、二つの結果を統合する方法について説明する。第３の実施形態における位置姿勢計測装置の構成や位置姿勢の計測の処理手順は第１の実施形態と同一であるため説明を省略する。

図９は、第３の実施形態における位置姿勢算出の具体的な処理手順を示すフローチャートである。この処理は、図３のフローチャートのステップＳ３０４において実施される処理に相当する。

ステップＳ９０１において、三次元点群のみを利用して物体の位置姿勢を算出する。位置姿勢の算出は、基本的には第１の実施形態において説明した方法と同一であるが、三次元点群データのみを用いて行う点が異なる。三次元点群データをもとに算出される位置姿勢を６次元ベクトルｓ_３Ｄで表す。位置姿勢を算出する同時に、算出された位置姿勢のあいまいさを表す６×６共分散行列Σ_３Ｄを算出する。この共分散行列は、後段の処理において二次元画像をもとに算出される位置姿勢と統合するために用いられる。位置姿勢の共分散行列は次のように算出される。２Ｄ−２Ｄ対応、３Ｄ−３Ｄ対応から位置姿勢を算出する（非特許文献４参照）。

本実施形態では、三次元の点と三次元の面の対応または二次元画像上の点と直線の対応からの共分散行列の算出する方法を示す。点群データの計測誤差の面の法線方向の成分を△ｐとし、その標準偏差をσ_３Ｄとする。σ_３Ｄは第２の実施形態で説明した方法と同じ方法によって算出する。△ｐが位置姿勢の微小変化△ｓ_３Ｄに相当すると仮定すると、一次近似により（１８）が得られる（記号については第１の実施形態参照）。

三次元点群データの各点についての（１８）を統合すると（１９）のようになる。

ただし、

である。（１９）を（２０）のように表す。

（２０）より、最小二乗法により△ｓ_３Ｄが（２１）のように算出される。

△ｓ_３Ｄの共分散行列Σ_３Ｄは次のようになる。ただしＥ［…］は…の期待値を表す。

すなわち、位置姿勢の共分散行列Σ３Ｄは、点群データの計測誤差の標準偏差、三次元平面パラメータ、三次元座標の位置姿勢に関する１次偏微分（ヤコビ行列）から算出する。

ステップＳ９０２において、二次元画像のみを利用して物体の位置姿勢を算出する。位置姿勢の算出は、第１の実施形態における算出方法によりエッジのみを用いて行う。得られる位置姿勢を６次元ベクトルｓ_２Ｄで表す。位置姿勢の算出と同時に、位置姿勢の算出結果のあいまいさを表す６×６共分散行列Σ_２Ｄを算出する。この共分散行列は、後段の処理においてステップＳ９０１において算出された位置姿勢と統合するために用いられる。位置姿勢の共分散行列Σ_２Ｄは次のように算出される。

エッジの探索方向の検出誤差を△ｄとし、その標準偏差をσ_２Ｄとする。σ_２Ｄは第２の実施形態で説明した方法と同じ方法によって算出する。△ｄが位置姿勢の微小変化△ｓ_２Ｄに相当すると仮定すると、一次近似により（２３）が得られる（記号については第１の実施形態参照）。

各エッジについての（２３）を統合すると（２４）のようになる。

ただし、

である。（２４）を（２５）のように表す。

Σ_３Ｄと同様に算出すると、Σ_２Ｄは（２６）のようになる。

すなわち、Σ_２Ｄは、エッジ検出誤差の標準偏差と、画像上に投影された線分の方程式、画像座標の位置姿勢についての１次偏微分（ヤコビ行列）から算出する。

ステップＳ９０３において、三次元計測情報をもとに算出された位置姿勢ｓ３Ｄと二次元画像をもとに算出された位置姿勢ｓ_２Ｄを統合する。すなわち、統合した位置姿勢を表す６次元ベクトルをｓ_{ｆｉｎａｌ}とすると、（２７）のように算出できる。

このように算出することによって、各位置姿勢が持つあいまいさがお互いの計測結果によって補われ、結果的に高精度に位置姿勢が計測されることが期待できる。

以上述べたように、第３の実施形態では、二次元画像と三次元点群データからそれぞれ別々に位置姿勢を算出し、算出された位置姿勢の共分散行列を用いて２つの位置姿勢を統合することによって位置姿勢の計測を行う方法について説明した。

［変形例３−１］
以上説明した実施形態・変形例では、二次元画像上の特徴としてエッジを用いていた。しかしながら、二次元画像上の特徴はエッジに限るものではなく、その他の特徴であってもよい。例えば、対象物体の三次元形状モデルを三次元点群データによって表し、画像特徴として特徴点を検出して、特徴点と三次元の点の対応に基づいて位置姿勢を算出してもよい。また、特定の特徴のみ用いるのではなく、複数の特徴（特徴点とエッジ）を位置姿勢の算出に利用してもよい。

［変形例３−２］
以上説明した実施形態・変形例では、三次元情報計測装置として、密（ｄｅｎｓｅ）な距離画像を出力する距離センサの利用を想定していた。しかしながら、三次元情報計測装置はこれに限るものではなく、疎な計測を行うものであってもよい。例えば、スポット光を用いた距離計測装置であってもよい。

［その他の実施形態］
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。また、そのプログラムをコンピュータ読み取り可能な記録媒体に記録して提供してもよい。

本発明は、ロボットの自己位置推定や、ロボットと物体との間の相対的な位置姿勢を推定するのに利用することができる。

Claims (12)

1. 物体の位置姿勢を計測する位置姿勢計測方法であって、
   画像入力手段が、前記物体の二次元画像を入力する画像入力工程と、
   三次元座標情報入力手段が、前記物体の表面の三次元座標情報を入力する三次元座標情報入力工程と、
   特徴検出手段が、前記二次元画像から画像特徴を検出する特徴検出工程と、
   二次元画像ずれ算出手段が、前記検出された画像特徴と、
   予め保持された三次元形状モデルを、予め保持された前記物体の位置姿勢で、前記二次元画像に投影したときの投影像の投影特徴との、
   第１のずれを算出する二次元画像ずれ算出工程と、
   三次元空間ずれ算出手段が、前記三次元座標情報の三次元特徴と、
   前記保持された位置姿勢における前記三次元形状モデルのモデル特徴との、
   第２のずれを算出する三次元空間ずれ算出工程と、
   同等尺度変換手段が、前記第１のずれの尺度及び／又は前記第２のずれの尺度を、同等の尺度になるように変換する同等尺度変換工程と、
   位置姿勢補正手段が、前記第１のずれ及び前記第２のずれに基づいて、前記保持された位置姿勢を補正する位置姿勢補正工程と
   を有することを特徴とする位置姿勢計測方法。
2. 概略位置姿勢入力手段が、前記物体の概略の位置姿勢を入力し、前記物体の位置姿勢として保持する概略位置姿勢入力工程
   を更に有し、
   前記二次元画像ずれ算出工程では、
   前記概略位置姿勢入力工程で入力され保持された前記位置姿勢に基づいて、第１のずれを算出することを特徴とする請求項１に記載の位置姿勢計測方法。
3. 前記二次元画像が、前記物体の撮像画像であることを特徴とする請求項１又は請求項２に記載の位置姿勢計測方法。
4. 前記三次元座標情報は、距離画像から得られる前記物体の表面上の点群の三次元座標であることを特徴とする請求項１乃至請求項３の何れか１項に記載の位置姿勢計測方法。
5. 前記画像特徴が、特徴点又はエッジであることを特徴とする請求項１乃至請求項４の何れか１項に記載の位置姿勢計測方法。
6. 前記画像特徴が、該画像特徴の信頼度に基づいて重み付けされることを特徴とする請求項１乃至請求項５の何れか１項に記載の位置姿勢計測方法。
7. 前記三次元座標情報が、該三次元座標情報の信頼度に基づいて重み付けされることを特徴とする請求項１乃至請求項６の何れか１項に記載の位置姿勢計測方法。
8. 前記同等尺度変換工程では、前記第１のずれを三次元空間の距離に相当する尺度に変換することを特徴とする請求項１乃至請求項７の何れか１項に記載の位置姿勢計測方法。
9. 前記同等尺度変換工程では、第１のずれの尺度及び第２のずれの尺度を、各々のずれの発生確率に変換することを特徴とする請求項１乃至請求項７の何れか１項に記載の位置姿勢計測方法。
10. 請求項１乃至請求項９の何れか１項に記載の位置姿勢計測方法をコンピュータに実行させることを特徴とするプログラム。
11. 請求項１乃至請求項９の何れか１項に記載の位置姿勢計測方法をコンピュータに実行させることを特徴とするプログラムを記録した記録媒体。
12. 物体の位置姿勢を計測する位置姿勢計測装置であって、
    前記物体の二次元画像を入力する画像入力手段と、
    前記物体の表面の三次元座標情報を入力する三次元座標情報入力手段と、
    前記二次元画像から画像特徴を検出する特徴検出手段と、
    前記検出された画像特徴と、
    予め保持された三次元形状モデルを、予め保持された前記物体の位置姿勢で、前記二次元画像に投影したときの投影像の投影特徴との、
    第１のずれを算出する二次元画像ずれ算出手段と、
    前記三次元座標情報の三次元特徴と、
    前記保持された位置姿勢における前記三次元形状モデルのモデル特徴との、
    第２のずれを算出する三次元空間ずれ算出手段と、
    前記第１のずれの尺度及び／又は前記第２のずれの尺度を、同等の尺度になるように変換する同等尺度変換手段と、
    前記第１のずれ及び前記第２のずれに基づいて、前記保持された位置姿勢を補正する位置姿勢補正手段と
    を有することを特徴とする位置姿勢計測装置。