(57)【要約】【課題】３次元環境再現システムを実現するために、床面上を移動する撮像系の動きパラメータを安定して推定する方法とその装置、およびコンピュータに観測系動き推定方法を実現させるプログラムを記録したコンピュータ読み取り可能な記録媒体を提供することを課題とする。【解決手段】走行する観測系（自走カメラ）の動きを、周囲６特徴の方位角を異なる３位置において観測することにより、床面上を移動する撮像系の動きパラメータを安定して推定する方法とその装置を提供する。【選択図】図３
2006155656.tif 000002
【特許請求の範囲】
【請求項１】
走行する観測系の水平移動、回転角度を推定する方法であって、基準位置における観測系周囲に存在するＮ個所（Ｎ≧６）の方向角を観測するステップ１と、上記Ｎ個所について異なる位置１で再度方向角を観測するステップ２と、上記Ｎ個所についてさらに異なる位置２で再度方向角を観測するステップ３と、以上の３Ｎの方向角を用いて、基準位置に対する位置１および位置２とその位置における鉛直線周りの観測系の回転を求めるステップからなることを特徴とする観測系動き推定方法。
【請求項２】
走行する観測系の水平移動、回転角度を推定する装置であって、観測系周囲に存在するＮ個所（Ｎ≧６）の方向角を観測する手段と、上記手段を異なる観測位置（基準位置、位置１、位置２）について３回動作させた結果を保持するメモリと、記憶された３観測位置における３Ｎの方向角を用いて、基準位置に対する位置１および位置２とその位置における鉛直線周りの観測系の回転を求める手段を備えたことを特徴とする観測系動き推定装置。
【請求項３】
コンピュータに請求項１に記載の観測系動き推定方法を実現させるプログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、３次元環境再現システムの観測系動き推定方法とその装置、およびコンピュータに観測系動き推定方法を実現させるプログラムを記録したコンピュータ読み取り可能な記録媒体に関する。
【背景技術】
【０００２】
３次元環境再現システムでは自走ロボットを所定の位置を走査するよう制御する。
【０００３】
この制御は角度センサーの情報から移動距離、回転を得て行われるが、角度センサーの精度不足や車輪のスリップが避けられず、正しく所定の位置を走行する保証はない。そのために得られた画像から実際に走行した視点位置を補正する必要がある。
【０００４】
画像中の特徴点から、カメラの動きを推定する手法として６特徴点照合による推定方法がある。例えば、非特許文献１に記載されている。
【０００５】
これを撮像装置の校正に関する従来例とする。この概念を図２に示す。画像中で
【０００６】
【数１】
2006155656.tif 000003
【０００７】
が未知数であるとする。
【０００８】
ここで、ｄ０からｄ５まではカメラから物体（この場合、筐体の頂点）までの距離、Ｔｘ、Ｔｙ、Ｔｚはカメラと物体間の相対的並進成分、ωｘ、ωｙ、ωｚは回転成分である。基準位置で観測された６特徴点の座標を数２として定める。
【０００９】
【数２】
2006155656.tif 000004
【００１０】
３次元空間中の特徴点の位置が（数１）と（数２）を既知とすると、任意の視点位置の特徴点投影座標が（数３）として計算できる。
【００１１】
【数３】
2006155656.tif 000005
【００１２】
（数３）が実際には（数４）として観測されたとする。
【００１３】
【数４】
2006155656.tif 000006
【００１４】
（数３）と（数４）は量子化誤差やレンズ系の収差のため、必ずしも一致しない。
【００１５】
（数３）と（数４）のずれを２乗誤差として（数５）に表現し、これを最小化する枠組みで物体−カメラ間の動きΨを決定する。
【００１６】
【数５】
2006155656.tif 000007
【００１７】
この最小化は、（数３）の関数ｆが非線形関数であることから、反復による最小化手法が使われる。（数６）は非線形最小２乗化の手法として広く用いられているＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｔ法による最小化ステップを表している。
【００１８】
【数６】
2006155656.tif 000008
【００１９】
Ｈは近似ヘッセ行列で、Ｉは単位行列∇ｅは勾配ｔは反復数である。λは非負の制御変数で、λが大きな時、最急降下法（収束は遅いが安定）に、λが小さな時、ニュートン法（不安定だが収束は速い）に近づく。
【００２０】
Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｔ法では、λを制御することにより最急降下法とニュートン法の長所が生かせる。なお、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｔ法では、近似ヘッセ行列を以下のように計算する。
【００２１】
【数７】
2006155656.tif 000009
【００２２】
【数８】
2006155656.tif 000010
【００２３】
数８は、数５が２次形式をとっていることから、可能になった近似である。
【００２４】
反復が安定収束であるためには、数６において、勾配∇ｅに係る逆行列は正定値（ｐｏｓｉｔｉｖｅ−ｄｅｆｉｎｉｔｅ）である必要がある。
【００２５】
一般にヘッセ行列は正定値である保証はないが、数７が数８の近似により実対称行列となることから、この場合正定置であることが保証されている。以上２視点間の６特徴点照合により、カメラの並進・回転が求まる。ただし、並進については絶対値ではなく、特徴点までの距離も含めて比として得られる。
【００２６】
なお、６特徴点の照合が必要な理由は、以下のように説明できる。特徴点一つの観測につき水平・垂直の座標が得られることから２つの方程式が立つ。
【００２７】
カメラの動きパラメータ（未知パラメータ）は６個である。特徴点照合が１つにつき、基準座標における特徴点までの距離が１個未知パラメータとして増える。特徴点照合が６のとき、方程式数が未知パラメータ１２と等しくなり、数５の最小化として画像から動きパラメータが求まる。
【非特許文献１】Ｊｕｙａｎｇ  Ｗｅｎｇ，  Ｎａｒｅｎｄｒａ  Ａｈｕｊａ，  ａｎｄ  Ｔｈｏｍａｓ  Ｓ．Ｈｕａｎｇ：“Ｏｐｔｉｍａｌ  ｍｏｔｉｏｎ  ａｎｄ  ｓｔｒｕｃｔｕｒｅ  ｅｓｔｉｍａｔｉｏｎ”，  ＩＥＥＥ  Ｔｒａｎｓ，  Ｐａｔｔｅｒｎ  Ａｎａｌｙｓｉｓ  ａｎｄ  Ｍａｃｈｉｎｅ  Ｉｎｔｅｌｌｉｇｅｎｃｅ，１５（９），ｐｐ８６４−８８４（１９９３）．
【発明の開示】
【発明が解決しようとする課題】
【００２８】
従来例は６特徴点照合により、カメラの並進・回転が求まる。図１のように一般に床面上を移動する撮像系では、自由度が水平移動に拘束され、また回転についても鉛直線を軸に１自由度の回転である。
【００２９】
したがって、２自由度の並進運動と１自由度の回転運動合計３自由度のパラメータ推定では、３特徴点照合により、カメラ移動パラメータが画像より推定できる。しかし、カメラ移動に際して、特徴点座標の垂直方向成分が変化しないことが多い。
【００３０】
言い換えれば、画面上、垂直方向の特徴点座標の僅かな観測誤差が、カメラの運動パラメータを大きく左右する。
【００３１】
本発明は、かかる点に鑑み、床面上を移動する観測系の動きパラメータを安定して推定する方法と装置を提供することを目的とする。
【課題を解決するための手段】
【００３２】
この課題を解決するための第１の発明は、走行する観測系の水平移動、回転角度を推定する方法であって、基準位置における観測系周囲に存在するＮ個所（Ｎ≧６）の方向角を観測するステップ１と、上記Ｎ個所について異なる位置１で再度方向角を観測するステップ２と、上記Ｎ個所についてさらに異なる位置２で再度方向角を観測するステップ３と、以上の３Ｎの方向角を用いて、基準位置に対する位置１および位置２とその位置における鉛直線周りの観測系の回転を求めるステップからなることを特徴とする観測系動き推定方法である。
【００３３】
第２の発明は、走行する観測系の水平移動、回転角度を推定する装置であって、観測系周囲に存在するＮ個所（Ｎ≧６）の方向角を観測する手段と、上記手段を異なる観測位置（基準位置、位置１、位置２）について３回動作させた結果を保持するメモリと、記憶された３観測位置における３Ｎの方向角を用いて、基準位置に対する位置１および位置２とその位置における鉛直線周りの観測系の回転を求める手段からなることを特徴とする観測系動き推定装置である。
【発明の効果】
【００３４】
本発明に係る観測系動き推定方法によれば、走行する観測系（自走カメラ）の動きを、周囲６特徴の方位角を異なる３位置において観測することにより、求めることができる。
【発明を実施するための最良の形態】
【００３５】
以下本発明の実施例について、図面を参照しながら説明する。
【００３６】
（実施例１）
床面上を移動する撮像系の動きパラメータを安定して推定する方法の実施例１について、図３、図４、図５を用いて説明する。はじめに動きパラメータの推定原理を説明する。図３に示すように、カメラが自走することにより、異なる観測位置で、見え方の異なる画像が撮像される。ここで求めたいのは、カメラの並行移動成分（床面上の２自由度）と鉛直方向周りの回転成分（１自由度）の計３自由度のパラメータである。
【００３７】
床面上の移動に対して、最も見え方変化の大きな特徴は、情景中の垂直エッジである（原理的には、垂直エッジだけではなく、特徴点を含む。後述の方向角に変換できる特徴であれば良い）。
【００３８】
本実施例では水平方向の見え方の変化だけを頼りにカメラの動きパラメータを推定する。図４は、撮像装置の位置を真上から俯瞰した図であるが、このように３視点位置でＡの位置を基準とし、新たに位置Ｂおよび位置Ｃで同じ垂直エッジを観測するとする。各位置における十字は、撮像装置中心で定まる局座標である。
【００３９】
基準位置Ａにおいて定めた座標系に対して、Ｂに移動した際のカメラ動きパラメータは（ＴｘＢ、ＴｙＢ、ωＢ）、Ｃに移動した際のカメラ動きパラメータは（ＴｘＣ、ＴｙＣ、ωＣ）とする。
【００４０】
未知数は６である。これを求めるために、各位置における垂直エッジの方位角を求める。観測１つに付き数９が１つ成り立つ。
【００４１】
数９においてｐは位置の識別子でこの場合［Ｂ、Ｃ］のいずれかである。ｉは垂直エッジの識別子であり、０≦ｉ＜Ｎ、Ｎ≧６であることは後述する。
【００４２】
【数９】
2006155656.tif 000011
【００４３】
数９は垂直エッジｉの基準座標Ａからみた距離ｄｉＡと位置ｐまでのカメラ動きパラメータが分かれば視点Ａにおける方位角が計算できることを意味する拘束式である。
【００４４】
さて、ここで２つの視点（例えば、基準点Ａと移動点Ｂ）で、垂直エッジを照合して基準座標から移動点Ｂまでのカメラ動きパラメータを求めようとすると、観測１つにつき拘束式が増えるが、新たに未知パラメータｄｉＡが増える。
【００４５】
そこで、観測位置を３点（この場合に相当し、基準点Ａと移動点ＢとＣ）とすると、観測１に対して拘束式が２つ得られ、６特徴の観測で１２の拘束式、１２の未知パラメータとなり、未知パラメータが定まる。
【００４６】
より一般的には６特徴以上の観測であれば、未知パラメータは全て求まる。
【００４７】
数９を書き換えて数１０とする。
【００４８】
【数１０】
2006155656.tif 000012
【００４９】
ただし、
【００５０】
【数１１】
2006155656.tif 000013
【００５１】
【数１２】
2006155656.tif 000014
【００５２】
【数１３】
2006155656.tif 000015
【００５３】
である。
【００５４】
ここで求めるべきパラメータを数１４とする。
【００５５】
【数１４】
2006155656.tif 000016
【００５６】
これまでの関係式から、Ψが求まれば視点位置Ａ、Ｂ、Ｃ間のカメラ移動が分かることになる。
【００５７】
基準位置Ａにおける観測角を数１５とすると、Ψと数１５から、視点位置Ｂにおける観測角と視点位置Ｃにおける観測角が数１６として導かれる。
【００５８】
【数１５】
2006155656.tif 000017
【００５９】
【数１６】
2006155656.tif 000018
【００６０】
以上は、移動パラメータと視点位置から計算された値であるが、実際に観測された角度を数１７であるとする。
【００６１】
【数１７】
2006155656.tif 000019
【００６２】
ただし、
【００６３】
【数１８】
2006155656.tif 000020
【００６４】
【数１９】
2006155656.tif 000021
【００６５】
である。
【００６６】
ここで、Ψと数１５から得られる数１６を以後の微分処理を容易にするため（数１０参照）、数２０に表現を書き換える。数２０は明示しないまでもΨと基準点Ａにおける観測角（数１５）の関数である。
【００６７】
【数２０】
2006155656.tif 000022
【００６８】
以上に対応して、観測位置Ｂ、Ｃにおける観測角の表現を数１７から数２１に表現を変える。
【００６９】
【数２１】
2006155656.tif 000023
【００７０】
そして、計算により予測されるＢ地点、Ｃ地点の観測角と実測との２乗誤差を数２２で表す。
【００７１】
【数２２】
2006155656.tif 000024
【００７２】
数２２を最小化するΨとしてパラメータが求まる。これは、従来技術で示したＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｔ法の枠組みで数２３の反復として求めることができる。
【００７３】
【数２３】
2006155656.tif 000025
【００７４】
数２３中、Ｈは近似ヘッセ行列（数７、数８と同形）で、Ｉは単位行列∇ｅは勾配である。
【００７５】
ヘッセ行列と勾配を求める微分処理は数１４の未知パラメータについて行われる。
【００７６】
以上の処理ステップをまとめると、図５となる。これを実施例１とする。
【００７７】
（実施例２）
観測系動き推定装置の実施例について、図６を用いて説明する。図６はブロック図であり、１０１は全方位画像から垂直エッジを抽出する特徴抽出部、１０２は垂直エッジの対応関係を求める特徴追跡部、１０３はエッジから図４に示す観測角に変換する６方向抽出部、１０４〜１０５は基準点Ａ、移動点Ｂ、Ｃの角度を記憶するメモリ、１０７は以上の観測から数２３に示した反復演算を行い、数１４のパラメータを求めるカメラ動き計算部である。
【００７８】
この装置によれば、自走しながら３地点で画像上に現れる６本の線分の見える方位角を記憶することにより、その線分（例えば、テーブルや本棚の稜線）までの距離と移動成分の比、および鉛直線周りの回転角を計算することができる。実際に観測した例を図７、図８に示す。
【００７９】
未知パラメータのうち、距離に関する成分（言い換えれば回転以外の成分）は全て比として求まる。
【００８０】
本実施例においては、移動カメラの車輪から得られた距離から数１４のＴｘＢの絶対値を定め、比例演算で絶対距離を求めている。
【００８１】
なお、本実施例では、全方位画像に写る垂直線分を特徴として用いたが、原理的には、図４に示すように３次元中の物体の存在（エッジや角など）を観測位置からの水平面上の方位角に置き換えることができれば、数１４のパラメータを求めることができる。
【００８２】
したがって、撮像対象が全方位画像である必要はなく、また用いる画像特徴も線分に限定する必要はない。
【００８３】
例えば、通常のカメラを用いてその投影面上でレンズ中心に平行な線を仮定し、その線上の特徴移動を追跡することによりカメラ動きパラメータを推定することができる。
【００８４】
さらに、観測手段は画像である必要も無い。方位角の観測であるからレーザー光の照射によってもこの方法および装置は適用できる。
【００８５】
最後に６線分を観測したが、７以上の線分を観測しても、増える未知パラメータ（線分までの距離）の数よりも拘束式が増えるため、数２０の最小化により求めることができる。
【００８６】
なお、本発明をソフトウエアプログラムによって実現し、そのプラグラムをメモリ上に記憶することにより、実行することができる。その場合、そのプログラムをＣＤ−ＲＯＭ等の記録媒体を用いて、あるいはインターネット等の通信回線を用いてプログラムを配信して、本発明を実行することも可能である。
【産業上の利用可能性】
【００８７】
本発明にかかる観測系動き推定方法とその装置、およびコンピュータに観測系動き推定方法を実現させるプログラムを記録したコンピュータ読み取り可能な記録媒体は、床面上を移動する撮像系の動きパラメータを安定して推定することができるため、自走カメラ等に有用である。
【図面の簡単な説明】
【００８８】
【図１】３次元環境再現システム概念図
【図２】画像上の６特徴点照合によるカメラ動きパラメータ推定図
【図３】６垂直線による自走ロボット位置推定概念図
【図４】方向角と自走ロボットの座標系説明図
【図５】観測系動き推定方法の実施例処理手順を示す図
【図６】観測系動き推定装置の実施例ブロック図
【図７】観測系動き推定装置の実施例による処理結果例１を示す図
【図８】観測系動き推定装置の実施例による処理結果例２を示す図
【符号の説明】
【００８９】
１０１    特徴抽出部
１０２    特徴追跡部
１０３    ６方向抽出部
１０４    角度メモリ１
１０５    角度メモリ２
１０６    角度メモリ３
１０７    カメラ動き計算部
【図１】
2006155656.tif 000026
【図２】
2006155656.tif 000027
【図３】
2006155656.tif 000028
【図４】
2006155656.tif 000029
【図５】
2006155656.tif 000030
【図６】
2006155656.tif 000031
【図７】
2006155656.tif 000032
【図８】
2006155656.tif 000033