(57)【特許請求の範囲】
【請求項１】
被写体までの距離を計測するように構成されている距離画像センサと、
前記被写体を撮像することにより、当該被写体の指定物理量を画素値として有する複数のピクセルによって構成される基準画像を取得するように構成されている基準画像センサと、
前記被写体を撮像することにより、当該被写体の前記指定物理量を画素値として有する複数のピクセルによって構成されている参照画像を取得するように構成されている参照画像センサと、を用いて前記被写体の表面の位置及び姿勢を推定する装置であって、
前記基準画像の少なくとも一部である対象領域に属する各ピクセルについて、前記距離画像センサによる距離計測値により位置が定まる実点と、当該実点を前記基準画像センサの視線方向について仮想面に投影した結果としての仮想点との間隔の長短に応じて定まる第１偏差と、前記基準画像センサの光軸方向について前記基準画像センサと前記実点及び前記仮想点のうち一方である対象点との距離である対象点距離とに基づき、前記第１偏差について増加関数であり、かつ、前記対象点距離について減少関数である多変数関数として定義されている第１コスト関数にしたがって、第１コストを算定するように構成されている第１演算処理要素と、
前記基準画像の前記対象領域に属する各ピクセルについて、前記基準画像のピクセルの画素値と、前記仮想面の位置及び姿勢に応じた形で当該基準画像のピクセルに対応する前記参照画像のピクセルの画素値との偏差としての第２偏差に基づき、前記第２偏差について増加関数である第２コスト関数にしたがって第２コストを算定するように構成されている第２演算処理要素と、
前記基準画像の前記対象領域に属する複数のピクセルについて、前記第１コストと前記第２コストとの総和である合計コストをその最小値に近づけるように前記仮想面を探索することにより、前記被写体の表面の位置及び姿勢を推定するように構成されている第３演算処理要素とを備えていることを特徴とする環境認識装置。
【請求項２】
請求項１記載の環境認識装置において、
前記第１コスト関数が前記距離画像センサによる前記被写体までの距離の測定結果の確度について減少関数として定義され、
前記第１演算処理要素は、距離の測定結果の確度を認識した上で、前記第１偏差と、前記対象点距離と、当該確度とに基づき、前記第１コスト関数にしたがって前記第１コストを算定するように構成されていることを特徴とする環境認識装置。
【請求項３】
請求項１又は２記載の環境認識装置において、
前記第２コスト関数が前記基準画像センサ及び前記参照画像センサのうち少なくとも一方による画素値の測定結果の確度について増加関数として定義され、
前記第２演算処理要素は、画素値の測定結果の確度を認識した上で、少なくとも前記第２偏差と、当該確度とに基づき、前記第２コスト関数にしたがって前記第２コストを算定するように構成されていることを特徴とする環境認識装置。
【請求項４】
請求項１〜３のうちいずれか１つに記載の環境認識装置において、
前記第３演算処理要素は、最小二乗法にしたがって前記被写体の表面の位置及び姿勢を推定するように構成されていることを特徴とする環境認識装置。
【請求項５】
被写体までの距離を計測するように構成されている距離画像センサと、
前記被写体を撮像することにより、当該被写体の指定物理量を画素値として有する複数のピクセルによって構成される基準画像を取得するように構成されている基準画像センサと、
前記被写体を撮像することにより、当該被写体の前記指定物理量を画素値として有する複数のピクセルによって構成されている参照画像を取得するように構成されている参照画像センサと、を用いて前記被写体の表面の位置及び姿勢を推定する装置であって、
前記距離画像センサにより取得された距離画像の複数のピクセル位置のそれぞれについて、前記基準画像センサ及び前記参照画像センサのそれぞれにより撮像された同一箇所の前記指定物理量は同一であるという拘束条件にしたがって、距離計測値に応じた前記基準画像センサのピクセル位置を参照画像センサに投影したピクセル位置と、距離候補値に応じた前記基準画像センサのピクセル位置を参照画像センサに投影したピクセル位置との偏差である視差残差に基づいて第１偏差を算出するように構成されている第１演算処理要素と、
前記基準画像センサ及び前記参照画像センサが前記距離候補値に応じた視差を有するという仮定に基づき、当該視差に応じた形で当該基準画像のピクセルに対応する前記参照画像のピクセルの画素値を取得した上で、前記基準画像及び前記参照画像の複数のピクセルのそれぞれについて画素値である前記指定物理量の偏差を第２偏差として算出するように構成されている第２演算処理要素と、
前記第１演算処理要素により算出された前記第１偏差と、前記第２演算処理要素により算出された前記第２偏差とに基づき、前記基準画像と前記参照画像との乖離度を算出するととともに、複数の前記距離候補値のうち前記乖離度が最小になるような前記距離候補値を、前記距離画像の各ピクセルにおける距離真値として推定するように構成されている第３演算処理要素とを備えていることを特徴とする環境認識装置。
【請求項６】
請求項５記載の環境認識装置において、
前記第１演算処理要素が、前記視差残差から前記指定物理量の偏差への変換を実行することにより、前記第１偏差を算出することを特徴とする環境認識装置。
【請求項７】
請求項６記載の環境認識装置において、
前記第１演算処理要素が、前記視差残差に対して前記指定物理量のエピポーラライン方向の勾配を乗じることにより、前記変換を実行することにより、前記第１偏差を算出することを特徴とする環境認識装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、距離画像センサ及び２つの画像センサを用いて被写体までの距離等を推定する装置に関する。
【背景技術】
【０００２】
ステレオカメラから得られる輝度情報に基づき、勾配法にしたがって平面パラメータ及び距離を推定する手法が提案されている（非特許文献１参照）。
【０００３】
距離画像センサを通じて取得された距離画像と、ステレオカメラのそれぞれを通じて取得された画像とを統合することにより、距離画像を推定する手法が提案されている（非特許文献２参照）。
【０００４】
具体的には、距離尤度及び輝度尤度に基づいて総合尤度が計算され、複数の距離候補値のうち総合尤度が最高となる距離候補値が、距離画像の各ピクセルにおける距離真値として推定される。
【先行技術文献】
【非特許文献】
【０００５】
【非特許文献１】“Lucas-Kanade 20 Years On : A Unifying Framework Part 1 : The Quantity Approximated, the Warp Update Rule, and the Gradient Descent Approximation”, S. Baker and I.Matthews, International Journal of Computer Vision (2004) Volume: 56, Issue: 3, Pages: 221-255
【非特許文献２】“Fusion of Time-of-Flight Depth and Stereo for High Accuracy Depth Maps”, JieJie Zhu, et al. IEEE (2008)
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、距離尤度及び輝度尤度のそれぞれの重みが、被写体までの距離の遠近とは無関係に経験則にしたがって定められる。このため、測定誤差が大きい遠距離の情報の影響が総合尤度に無視し得ない程度に反映され、距離推定精度が低下する可能性がある。
【０００７】
そこで、本発明は、被写体までの距離を含む当該被写体情報の推定精度の向上を図りうる装置を提供することを課題とする。
【課題を解決するための手段】
【０００８】
本発明は、被写体までの距離を計測するように構成されている距離画像センサと、前記被写体を撮像することにより、当該被写体の指定物理量を画素値として有する複数のピクセルによって構成される基準画像を取得するように構成されている基準画像センサと、前記被写体を撮像することにより、当該被写体の前記指定物理量を画素値として有する複数のピクセルによって構成されている参照画像を取得するように構成されている参照画像センサと、を用いて前記被写体の表面の位置及び姿勢を推定する装置に関する。
【０００９】
本発明の第１態様の環境認識装置は、前記基準画像の少なくとも一部である対象領域に属する各ピクセルについて、前記距離画像センサによる距離計測値により位置が定まる実点と、当該実点を前記基準画像センサの視線方向について仮想面に投影した結果としての仮想点との間隔の長短に応じて定まる第１偏差と、前記基準画像センサの光軸方向について前記基準画像センサと前記実点及び前記仮想点のうち一方である対象点との距離である対象点距離とに基づき、前記第１偏差について増加関数であり、かつ、前記対象点距離について減少関数である多変数関数として定義されている第１コスト関数にしたがって、第１コストを算定するように構成されている第１演算処理要素と、前記基準画像の前記対象領域に属する各ピクセルについて、前記基準画像のピクセルの画素値と、前記仮想面の位置及び姿勢に応じた形で当該基準画像のピクセルに対応する前記参照画像のピクセルの画素値との偏差としての第２偏差に基づき、前記第２偏差について増加関数である第２コスト関数にしたがって第２コストを算定するように構成されている第２演算処理要素と、前記基準画像の前記対象領域に属する複数のピクセルについて、前記第１コストと前記第２コストとの総和である合計コストをその最小値に近づけるように前記仮想面を探索することにより、前記被写体の表面の位置及び姿勢を推定するように構成されている第３演算処理要素とを備えていることを特徴とする。
【００１０】
本発明の第１態様の環境認識装置によれば、第１コスト関数が対象点距離の減少関数として定義されているので、対象点距離が長いほど当該ピクセルの第１コストが小さく評価される。したがって、対象点距離の測定又は推定誤差が大きい蓋然性が高いピクセルの第１コストの合計コストに対する寄与度が小さくなる。これにより、被写体までの距離を含む当該被写体情報の推定精度の向上が図られる。
【００１１】
本発明の第１態様の環境認識装置において、前記第１コスト関数が前記距離画像センサによる前記被写体までの距離の測定結果の確度について減少関数として定義され、前記第１演算処理要素は、距離の測定結果の確度を認識した上で、前記第１偏差と、前記対象点距離と、当該確度とに基づき、前記第１コスト関数にしたがって前記第１コストを算定するように構成されていることが好ましい。
【００１２】
本発明の第１態様の環境認識装置において、前記第２コスト関数が前記基準画像センサ及び前記参照画像センサのうち少なくとも一方による画素値の測定結果の確度について増加関数として定義され、前記第２演算処理要素は、画素値の測定結果の確度を認識した上で、少なくとも前記第２偏差と、当該確度とに基づき、前記第２コスト関数にしたがって前記第２コストを算定するように構成されていることが好ましい。
【００１３】
本発明の第１態様の環境認識装置において、前記第３演算処理要素は、最小二乗法にしたがって前記被写体の表面の位置及び姿勢を推定するように構成されていることが好ましい。
【００１４】
本発明の第２態様の環境認識装置は、被写体までの距離を計測するように構成されている距離画像センサと、前記被写体を撮像することにより、当該被写体の指定物理量を画素値として有する複数のピクセルによって構成される基準画像を取得するように構成されている基準画像センサと、前記被写体を撮像することにより、当該被写体の前記指定物理量を画素値として有する複数のピクセルによって構成されている参照画像を取得するように構成されている参照画像センサと、を用いて前記被写体の表面の位置及び姿勢を推定する装置であって、前記距離画像センサにより取得された距離画像の複数のピクセル位置のそれぞれについて、前記基準画像センサ及び前記参照画像センサのそれぞれにより撮像された同一箇所の前記指定物理量は同一であるという拘束条件にしたがって、距離計測値に応じた前記基準画像センサのピクセル位置を参照画像センサに投影したピクセル位置と、距離候補値に応じた前記基準画像センサのピクセル位置を参照画像センサに投影したピクセル位置との偏差である視差残差に基づいて第１偏差を算出するように構成されている第１演算処理要素と、前記基準画像センサ及び前記参照画像センサが前記距離候補値に応じた視差を有するという仮定に基づき、当該視差に応じた形で当該基準画像のピクセルに対応する前記参照画像のピクセルの画素値を取得した上で、前記基準画像及び前記参照画像の複数のピクセルのそれぞれについて画素値である前記指定物理量の偏差を第２偏差として算出するように構成されている第２演算処理要素と、前記第１演算処理要素により算出された前記第１偏差と、前記第２演算処理要素により算出された前記第２偏差とに基づき、前記基準画像と前記参照画像との乖離度を算出するととともに、複数の前記距離候補値のうち前記乖離度が最小になるような前記距離候補値を、前記距離画像の各ピクセルにおける距離真値として推定するように構成されている第３演算処理要素とを備えていることを特徴とする。
【００１５】
本発明の第２態様の環境認識装置によれば、「同一又は略同一の時刻において基準画像センサ及び参照画像センサのそれぞれにより撮像された同一箇所の前記指定物理量は同一である」という普遍的な条件にしたがって第１偏差が算出される。このため、経験則に基づいて設計された評価関数にしたがって距離候補値の尤度が評価される場合と比較して、距離真値、ひいては距離画像の推定精度の向上を図ることができる。
【００１６】
本発明の第２態様の環境認識装置において、前記第１演算処理要素が、前記視差残差から前記指定物理量の偏差への変換を実行することにより、前記第１偏差を算出することが好ましい。
【００１７】
本発明の第２態様の環境認識装置において、前記第１演算処理要素が、前記視差残差に対して前記指定物理量のエピポーラライン方向の勾配を乗じることにより、前記変換を実行することにより、前記第１偏差を算出することが好ましい。
【図面の簡単な説明】
【００１８】
【図１】本発明の実施形態としての環境認識装置の構成説明図。
【図２】本発明の実施形態としての環境認識方法のフローチャート。
【図３】基準画像座標系及び距離画像座標系の相関関係に関する説明図。
【図４】実点、仮想面及び仮想点に関する説明図。
【図５】基準画像座標系及び参照画像座標系の関係に関する説明図。
【図６】環境認識結果に関する説明図。
【発明を実施するための形態】
【００１９】
（構成）
図１に示されている環境認識装置１は、距離画像センサＣ０と、基準画像センサＣ１と、参照画像センサＣ２とを備えている。環境認識装置１は、たとえば、車両又は脚式移動ロボットなどの移動装置に搭載され、当該移動装置の環境を認識するために利用される。
【００２０】
距離画像センサＣ０はたとえばＴＯＦ（Time-of-Flight）距離画像センサであり、各ピクセルが距離計測値Ｄｓ’（図４参照）を画素値として有する１次距離画像を取得する。距離画像センサＣ０の撮像面又は平面上に配列されている撮像素子群により「１次距離画像座標系」が定義される。
【００２１】
基準画像センサＣ１は可視光ステレオカメラの一方のカメラ（たとえば左側カメラ）であり、各ピクセルが少なくとも輝度（指定物理量）を画素値として有する基準画像を取得する。基準画像センサＣ１の光軸方向がＺ軸方向として定義されている（図４参照）。基準画像センサＣ１の撮像面又は平面上に配列されている撮像素子群により「基準画像座標系」が定義される。基準画像座標系の横方向がＸ軸方向として定義され、縦方向がＹ軸方向として定義される。
【００２２】
参照画像センサＣ２は可視光ステレオカメラの他方のカメラ（たとえば右側カメラ）であり、基準画像と同じく各ピクセルが少なくとも輝度を画素値として有する参照画像を取得する。参照画像センサＣ２の撮像面又は平面上に配列されている撮像素子群により「参照画像座標系」が定義される。
【００２３】
距離画像センサＣ０、基準画像センサＣ１及び参照画像センサＣ２のそれぞれのカメラパラメータ（内部パラメータ及び外部パラメータ）は既知であり、環境認識装置１を構成するコンピュータのメモリに保存されている。たとえば、１次距離画像座標系及び基準画像座標系の間の座標変換を表わす回転行列及び並進行列又はこれに等価なクォータニオンはメモリに保存されている。同様に、基準画像座標系及び参照画像座標系の間の座標変換を表わす回転行列及び並進行列又はこれに等価なクォータニオンはメモリに保存されている。
【００２４】
環境認識装置１がロボットに搭載されている場合、１次距離画像座標系、基準画像座標系及び参照画像座標系のそれぞれのロボット座標系に対する位置及び姿勢は、ロボットの挙動を表わす順キネマティクスモデルにしたがって算出された上でメモリに保存される。
【００２５】
ロボット座標系は、ロボットの質量中心（たとえば基体に含まれている。）が原点として定義され、ロボットの上方が＋ｘ方向として定義され、右方向が＋ｙ方向として定義され、前方が＋ｚ方向として定義されている。ロボット座標系の世界座標系における位置及び姿勢は、たとえば、ロボットの行動計画により定義される。
【００２６】
環境認識装置１は、プログラマブルコンピュータにより構成されており、当該画像を対象として後述する演算処理を実行するように構成されている第１演算処理要素１１と、第２演算処理要素１２と、第３演算処理要素１３とを備えている。当該３つの演算処理要素１１〜１３は、単一のコンピュータにより構成されていてもよいし、物理的に独立している複数のコンピュータにより構成されていてもよい。
【００２７】
各演算処理要素が担当演算処理を実行するように「構成されている」とは、各演算処理要素を構成するＣＰＵ等の演算処理装置が、ＲＯＭ、ＲＡＭ等のメモリ又は記録媒体から必要な情報に加えてソフトウェアを読み出し、当該情報に対して当該ソフトウェアにしたがって演算処理を実行するように「プログラムされている」ことを意味する。
【００２８】
（機能）
前記構成の環境認識装置１によって実行される環境認識方法について説明する。
【００２９】
距離画像センサＣ０による距離計測値Ｄｓ［ｓ］を画素値として有する複数のピクセルによって構成される「２次距離画像」が取得される（図２／ＳＴＥＰ００２）。ここで「ｓ」は基準画像座標系におけるピクセル位置のうち、基準画像において対象領域又はＲＯＩ（Region of Interest）に含まれ、距離計測値Ｄｓが割り当てられたピクセル位置（量子化された点の位置）、正確にはその座標値を意味する。
【００３０】
具体的には、まず距離画像センサＣ０による被写体までの距離計測値Ｄｓ’（図４参照）を画素値として有する複数のピクセルｓ’によって構成される「１次距離画像」が取得される。そして、１次距離画像座標系の各ピクセル位置における距離計測値Ｄｓ’に応じた画素値が、基準画像座標系の各ピクセル位置ｕに割り当てられることにより２次距離画像が取得される。
【００３１】
具体的には、図３に示されているように、１次距離画像座標系のピクセル位置ｓ’が、基準画像座標系に座表変換された結果としての点位置ｓ”が求められる。１次距離画像座標系を基準とした観測点Ｐｓの位置は、ベクトル^ｐ’＝Ｄｓ’^ｅ’（ｓ’）により表わされる。「^」はベクトルを意味する（以下同じ。）。「^ｅ’（ｓ’）」は、１次距離画像座標系のピクセル位置ｓ’を通る距離画像センサＣ０の視線方向を示す単位ベクトルである。
【００３２】
１次距離画像座標系から基準画像座標系への座標変換を表わす回転行列Ｒ及び並進行列Ｔに基づき、基準画像座標系を基準とした観測点Ｐｓの位置を表わすベクトル^ｐ”＝Ｒ^ｐ’＋Ｔが計算される。回転行列Ｒ及び並進行列Ｔは、予めメモリに保存されている。回転行列Ｒ及び並進行列Ｔは、数学的にこれと等価なクォータニオンによって定義されていてもよい。
【００３３】
当該ベクトル^ｐ”及びその奥行き方向成分（Ｘ−Ｙ座標系である基準画像座標系に対して垂直なＺ方向の成分）であるＺｓ”に基づき、基準画像座標系において観測点Ｐｓに対応する座標位置ｓ”＝（１／Ｚｓ”）^ｐ”が求められる。
【００３４】
基準画像座標系において当該座標位置ｓ”（図３白丸参照）は、一般的にピクセル位置ｓ（図３黒丸参照）に一致しない。このため、最近傍点補間法、双線形補間法又はバイキュービックスプライン法など、公知の補間方法にしたがって、基準画像座標系の各ピクセル位置ｓに対して、座標位置ｓ”に対応する距離Ｚｓ”に基づいて定まる距離（実点距離）Ｚｓが割り当てられる。
【００３５】
そして、基準画像座標系の各ピクセル位置ｓに対して、２次距離画像座標系を基準とした観測点Ｐｓの位置を表わすベクトル^ｐ＝Ｄｓ^ｅ（ｓ）のノルムＤｓ（ｓ）が画素値として割り当てられる。「^ｅ（ｓ）」は、２次距離画像座標系のピクセル位置ｓを通る、距離画像センサＣ０の視線方向を示す単位ベクトルである。
【００３６】
ただし、距離画像センサＣ０及び基準画像センサＣ１のそれぞれの解像度の相違等を考慮して、基準画像座標のすべてのピクセル位置に距離計測値が割り当てられる必要はない。これにより、２次距離画像が取得される。
【００３７】
また、基準画像センサＣ１を通じて、少なくとも輝度を画素値として有する複数のピクセルにより構成される「基準画像」が取得される（図２／ＳＴＥＰ００４）。
【００３８】
さらに、参照画像センサＣ２を通じて、基準画像と同様に少なくとも輝度を画素値として有する複数のピクセルにより構成される「参照画像」が取得される（図２／ＳＴＥＰ００６）。
【００３９】
同一時刻において取得され、メモリに保存されている同一時刻における２次距離画像、基準画像及び参照画像を対象として次に説明する一連の処理が実行される。なお、距離画像センサＣ０、基準画像センサＣ１及び参照画像センサＣ２が完全に同期していない場合、同一時刻における２次距離画像、基準画像及び参照画像が、若干異なった又は略同一の時刻のそれぞれにおいて取得されてもよい。
【００４０】
まず、基準画像座標系のピクセル位置ｓについて仮想面（平面）が設定される（図２／ＳＴＥＰ００８）。具体的には、仮想面の位置及び姿勢を表わすベクトル^ｎ／ｄにより定義される平面パラメータ^ｑが設定される。平面パラメータ^ｑの初期値は任意であってもよいが、後述するように平面パラメータ^ｑは前回値が修正されることにより、今回値が設定される。「^ｎ」は仮想面の単位法線ベクトルである（図４参照）。「ｄ」は基準画像センサＣ１の撮像素子から仮想面までの距離である（図４参照）。
【００４１】
第１演算処理要素１１は、実点Ｐｓと仮想点Ｐｃとの間隔｜Ｄｓ−Ｄｃ｜の広狭に応じて定まる第１偏差ｅ1に基づき、第１コスト関数ｆ1にしたがって「第１コスト」を算定する（図２／ＳＴＥＰ０１０）。
【００４２】
図４に示されているように「実点」とは、２次距離画像座標系におけるピクセル位置ｓ＝（ｕ，ｖ）の画素値Ｄｓ（ｓ）に応じて実空間位置が定まる点Ｐｓ＝（１／Ｄｓ）^ｓを意味する。また「仮想点」とは、ピクセル位置^ｓからその画素値Ｄｓ（ｓ）の方向（基準画像センサＣ１の視線方向）について実点Ｐｓを仮想面に投影した点を意味する。
【００４３】
第１偏差ｅ1として、実点距離Ｚｓと仮想点距離Ｚｃとの偏差｜Ｚｓ−Ｚｃ｜が採用される。実点距離Ｚｓは、基準画像センサＣ１の光軸方向（Ｚ方向）について、基準画像センサＣ１と実点Ｐｓとの距離である。仮想点距離Ｚｃは、基準画像センサＣ１と仮想点Ｐｃとの距離である（図４参照）。
【００４４】
なお、実点Ｐｓと仮想点Ｐｃとの間隔｜Ｄｓ−Ｄｃ｜の広狭に応じて幾何学的関係から一義的に定まるさまざまな距離が、第１偏差ｅ1として採用されてもよい。例えば、当該間隔｜Ｄｓ−Ｄｃ｜そのもののほか、実点ＰｓがＺ方向について仮想面に投影された結果としての点と、仮想点Ｐｃとの指定方向についての間隔が第１偏差ｅ1として採用されてもよい。また、仮想点ＰｃがＺ方向について被写体に相当する平面に投影された結果としての点と、実点Ｐｓとの指定方向についての間隔が第１偏差ｅ1として採用されてもよい。
【００４５】
第１コスト関数ｆ1は、第１偏差ｅ1を変数とする連続的又は段階的な増加関数である。すなわち（∂ｆ1／∂ｅ1）＞０又はｆ1（ｅ1＋δｅ1）−ｆ1（ｅ1）＞０（δｅ1＞０）という関係式が成立する。第１コスト関数ｆ1は、たとえば関係式（１０）にしたがって定義されている。
【００４６】
f1(e1)＝Σse12(s) ..(10)。
【００４７】
「Σs」は、２次距離画像座標系における和を意味する。第１偏差ｅ1は仮想面の平面パラメータ^ｑ＝^ｎ／ｄを変数とする関数であるため、第１偏差ｅ1に基づいて算出される第１コストも仮想面の平面パラメータ^ｑを変数とする関数となる。
【００４８】
第１コスト関数ｆ1は、第１偏差ｅ1に加えて、実点距離Ｚｓ及び仮想点距離Ｚｃのうち一方である対象点距離Ｚを変数とする多変数関数であってもよい。第１コスト関数ｆ1は、対象点距離Ｚについて減少関数として定義される。この場合、第１コスト関数ｆ1は、たとえば関係式（１１）にしたがって定義される。
【００４９】
f1(e1, Z)=Σsg1(Z(s))e12(s),
g1(Z)=1/Zn（n>0。たとえばn=1又は2。） ..(11)。
【００５０】
関係式（１１）により定義される第１コスト関数ｆ1は、次に説明するようにその対象点距離Ｚに対する挙動特性が、基準画像センサＣ１及び参照画像センサＣ２の視差残差Δｕのそれと少なくとも部分的に一致又は近似する。したがって、この第１コスト関数ｆ1は、当該視差残差Δｕを一義的に定める幾何学的な関係に鑑みて妥当な形で定義されていることがわかる。ここで、この点について説明する。
【００５１】
ここでは簡単のため、基準画像センサＣ１及び参照画像センサＣ２が、等しい内部パラメータを持ち互いの光軸が平行になるよう配置された平行ステレオの場合について説明する。基準画像センサＣ１及び参照画像センサＣ２が平行ステレオの関係にない場合、図５（ａ）に示されているように視差残差Δｕは参照画像センサＣ２に距離計測値Ｄｓと距離候補値Ｄｃを投影した時のそれぞれのピクセル位置の差となる。
【００５２】
視差残差Δｕは、実点距離Ｚｓと、仮想点距離Ｚｃと、両画像センサＣ１及びＣ２の基線長Ｌとに基づき、関係式（１１１）にしたがって定義される（図４参照）。
【００５３】
Δu=L{(1/Zs)-(1/Zc)} ..(111)。
【００５４】
「ｕ」は基準画像座標系（又は参照画像座標系）における横方向の位置を表わす座標値である。平行ステレオの場合、ｕ軸の方向はエピポーラ線と平行になる。基準画像と、視差残差Δｕが存在するという仮定下での参照画像との輝度の誤差ΔＩは、エピポーラ拘束条件を考慮に入れて、関係式（１１２）により表現される（図５（ｂ）参照）。
【００５５】
ΔI=(∂I/∂u)^eepΔu ..(112)。
【００５６】
「^ｅep」は基準画像座標系におけるエピポーラ線の方向を表わす単位ベクトルである。ここで(∂I/∂u)は輝度勾配を表すベクトルであり、「(∂I/∂u)^eep」はエピポーラ線方向の輝度勾配を表す。特にu軸とエピポーラ線が平行の場合,u軸方向のみの輝度勾配となる。関係式（１１２）にしたがって、距離残差ΔＺ＝Ｚｓ−Ｚｃが存在する場合の輝度残差ΔＩが計算されうる。
【００５７】
関係式（１１２）は、同一時刻において基準画像センサＣ１及び参照画像センサＣ２のそれぞれにより撮像された同一箇所の指定物理量（ここでは輝度）は同一であるという拘束条件にしたがって、距離計測値Ｄｓに応じて基準画像センサＣ１の「ｓ」（基準画像座標系におけるピクセル位置）を参照画像センサＣ２に投影したピクセル位置と、距離候補値Ｄｃに応じて基準画像センサＣ１の「ｓ」を参照画像センサＣ２に投影したピクセル位置との偏差である視差残差Δｕが、輝度残差（指定物理量の偏差）に変換されることを表わしている。
【００５８】
第１演算処理要素１１により、関係式（１１２）にしたがって輝度残差ΔＩが第１偏差ｅ1として算出されてもよい。
【００５９】
Δｕ＝（ｄｕ／ｄＺｃ）ΔＺ及びΔＺ＝Ｚｓ−Ｚｃという関係式にしたがって関係式（１１１）が変形されることにより、視差残差Δｕは関係式（１１３）によって表現される。
【００６０】
Δu=-(L/Zc2)(Zs-Zc) ..(113)。
【００６１】
関係式（１１３）によれば、仮想点距離Ｚｃが対象点距離Ｚとして定義された場合、視差残差Δｕは対象点距離Ｚの逆数（１／Ｚ）のべき乗に比例する項を含む。このため、関係式（１１）の第１コスト関数ｆ1の対象点距離Ｚに対する依存性を、視差残差Δｕのそれと少なくとも部分的に一致又は近似させることができる。したがって、この第１コスト関数ｆ1は、当該視差残差Δｕを一義的に定める、基準画像センサＣ１、参照画像センサＣ２、実点Ｐｓの位置及び仮想点Ｐｃの位置の幾何学的な配置関係に鑑みて妥当な形で定義されていることがわかる。
【００６２】
第１コスト関数ｆ1は、第１偏差ｅ1又は第１偏差ｅ1及び対象点距離Ｚに加えて、距離画像センサＣ０による距離計測値Ｄｓ’の確度ｒ1を変数とする多変数関数であってもよい。第１コスト関数ｆ1は、確度ｒ1について連続的又は段階的な減少関数として定義される。
【００６３】
確度ｒ1（ｓ）は、距離画像センサＣ０の受光強度の増加関数として定義される。すなわち、距離画像座標系におけるピクセル位置ｓの画素値の基礎となった、１次距離画像座標系におけるピクセル位置における距離画像センサの受光強度が高いほど、確度ｒ1（ｓ）の値が大きくなる。この場合、第１コスト関数ｆ1は、たとえば関係式（１２）〜（１５）のうちいずれか１つにしたがって定義される。
【００６４】
f1(e1, Z, r1)=Σsh1(r1(s))g1(Z(s))e12(s),
h1(r1)=1/r1n（n>0。たとえばn=1。） ..(12)。
【００６５】
f1(e1, Z, r1)=h1(Σsr1(s)/N)Σig1(Z(s))e12(s), 
h1(r1)=1/r1n（n>0。たとえばn=1。Σsr1(s)/Nは対象領域に属するピクセル群の確度r1の平均値。） ..(13)。
【００６６】
f1(e1, r1)=Σsh1(r1(s))e12(s) ..(14)。
【００６７】
f1(e1, r1)=h1(Σsr1(s)/N)Σie12(s) ..(15)。
【００６８】
第２演算処理要素１２は、基準画像センサＣ１及び参照画像センサＣ２のそれぞれが、同一の仮想面を撮像しているという仮定下で、参照画像を基準画像座標系に座標変換することにより変換画像を生成する（図２／ＳＴＥＰ０１２）。具体的には、基準画像座標系のピクセル位置ｓ1に対して、参照画像座標系のうち当該仮想面の位置及び姿勢に応じた視差が存在する場合におけるピクセル位置ｓ2の輝度が割り当てられた画像が変換画像として取得される。
【００６９】
第２演算処理要素１２は、基準画像座標系及び変換画像座標系の同一ピクセル位置ｓにおける画素値の偏差としての第２偏差ｅ2に基づき、第２コスト関数ｆ2にしたがって「第２コスト」を算定する（図２／ＳＴＥＰ０１４）。
【００７０】
第２コスト関数ｆ2は、第２偏差ｅ2を変数とする連続的又は段階的な増加関数である。すなわち（∂ｆ2／∂ｅ2）＞０又はｆ2（ｅ2＋δｅ2）−ｆ2（ｅ2）＞０（δｅ2＞０）という関係式が成立する。第２コスト関数ｆ2は、たとえば関係式（２０）にしたがって定義されている。
【００７１】
f2(e2)＝Σse22(s) ..(20)。
【００７２】
第２偏差ｅ2は仮想面の平面パラメータ＾ｑ＝＾ｎ／ｄを変数とする関数であるため、第２偏差ｅ2に基づいて算出される第２コストも仮想面の平面パラメータ＾ｑを変数とする関数となる。
【００７３】
第２コスト関数ｆ2は、第２偏差ｅ2又は第２偏差ｅ2及び対象点距離Ｚに加えて、基準画像センサＣ１及び参照画像センサＣ２のうち少なくとも一方による画素値の確度ｒ2を変数とする多変数関数であってもよい。第２コスト関数ｆ2は、確度ｒ2について連続的又は段階的な減少関数として定義される。
【００７４】
確度ｒ2（ｓ）は、たとえば、画素の微分値が特徴的な値になるか否かという基準にしたがって、高低が定まる関数として定義される。この場合、第２コスト関数ｆ2は、たとえば関係式（２１）又は（２２）にしたがって定義される。
【００７５】
f2(e2, r2)=Σsh2(r2(s))e22(s),
h2(r2)=1/r2m（m>0。たとえばm=1。） ..(21)。
【００７６】
f2(e2, r2)=h2(Σsr2(s)/M)Σse22(s), 
h2(r2)=1/r2m（m>0。たとえばm=1。Σsr2(s)/Mは基準画像センサの対象領域又は参照画像センサにおいてこれに対応する領域に属するピクセル群の確度ｒ2の平均値。） ..(22)。
【００７７】
確度ｒ2（ｓ）は、画素値である輝度の勾配が基準値以下である場合、予め定められている正の所定値とされてもよい。
【００７８】
第３演算処理要素１３は、第１コストと第２コストとの合計コストＣが最小値に近づくように、最小二乗法にしたがって仮想面を探索する（図２／ＳＴＥＰ０１６）。これにより、被写体の表面の位置及び姿勢が推定される。前記のように第１コスト及び第２コストはともに仮想面の平面パラメータ＾ｑを変数とする関数であるため、合計コストＣも関係式（３１）又は（３２）により表現されるように、平面パラメータ^ｑの関数Ｃ（^ｑ）として定義される。
【００７９】
C(^q)=f1(^q)+f2(^q) ..(31)。
【００８０】
C(^q)=αf1(^q)+(1-α)f2(^q), (0<α<1)..(32)。
【００８１】
また、関係式（１１２）及び（１１３）により、輝度残差ΔＩと距離残差ΔＺとの関係が対応付けられている。このため、第１偏差ｅ1が輝度残差ΔＩにより、幾何学的な配置関係に鑑みて妥当な形で、同次元で表現されうる。或いは、第２偏差ｅ2が距離残差ΔＺにより、幾何学的な配置関係に鑑みて妥当な形で、同次元で表現されうる。
【００８２】
そして、仮想面の平面パラメータを（∂Ｃ（^ｑ）／∂^ｑ）に応じた分だけ変動させる勾配法にしたがって、合計コストＣを最小値にする平面パラメータ^ｑが探索される。
【００８３】
合計コストの前回値と今回値との差が閾値以下になる等、平面パラメータ^ｑが一定の収束条件を満足しているか否かに応じて、仮想面の探索が終了したか否かが判定される（図２／ＳＴＥＰ０１８）。当該判定結果が否定的である場合（図２／ＳＴＥＰ０１８‥ＮＯ）、前回平面パラメータ^ｑ（ｋ）（ｋは平面パラメータの更新回数を表わす指数である。）が勾配法にしたがって更新された結果としての今回平面パラメータ^ｑ（ｋ＋１）が設定される（図２／ＳＴＥＰ００８）。その上で前記した一連の処理が繰り返される（図２／ＳＴＥＰ０１０〜ＳＴＥＰ０１８）。
【００８４】
その一方、当該判定結果が肯定的である場合（図２／ＳＴＥＰ０１８‥ＹＥＳ）、第３演算処理要素１３は、その時点での平面パラメータ^ｑによって定義される仮想面の位置及び姿勢を、被写体の表面の位置及び姿勢として推定する（図２／ＳＴＥＰ０２０）。
【００８５】
図６（ａ）には左から順に、基準画像、参照画像及び距離真値を画素値として有する距離画像の一例が示されている。距離画像において、画素値である距離が遠いほど明度が低く、画素値である距離が近いほど明度が高くなっている。図６（ｂ）には、本発明の環境認識方法にしたがって得られた距離画像が示されている。その一方、図６（ｃ）には、従来技術である非特許文献１記載の手法をステレオカメラより得られた輝度情報に適用し得られた距離画像が示されている。
【００８６】
基準画像及び参照画像に含まれる物体表面は実際にはほぼ平面状であるものの（図６（ａ）参照）、従来技術によれば、その表面における文字又は画像などの存在に由来して、物体表面に比較的大きい凹凸が存在することを表わす距離画像が取得される（図６（ｃ）参照）。その一方、本発明によれば、物体表面にわずかな凹凸が存在するもののほぼ平面状であって、実際に近い距離画像が取得されており、環境認識精度の向上が図られていることがわかる（図６（ｂ）参照）。
【００８７】
なお、可視光カメラを通じて取得される色（ＲＧＢ値）、赤外光カメラを通じて取得される温度など、可視光カメラを通じて取得される輝度とは異なる物理量が指定物理情報として距離画像が推定されてもよい。
【００８８】
本発明の環境認識装置１によれば、第１コスト関数が対象点距離Ｚの減少関数として定義されているので、対象点距離Ｚが長いほど当該ピクセルの第１コストが小さく評価される（関係式（１１）〜（１５）参照）。したがって、対象点距離Ｚの測定又は推定誤差が大きい蓋然性が高いピクセルの第１コストの合計コストＣに対する寄与度が小さくなる。これにより、被写体の表面位置および姿勢を表わす平面パラメータ＾ｑの推定精度の向上が図られる。
【００８９】
さらに、合計コストＣが第１偏差ｅ1の二乗和及び対象点距離Ｚの逆数（１／Ｚ）のべき乗、並びに、第２偏差ｅ2の二乗和に基づいて計算される（関係式（１０）〜（１５）及び関係式（２０）〜（２２）参照）。このため、評価関数が当該残差の対数関数又は指数関数である場合（例えば非特許文献２の式（５）参照）と比較して、合計コストの計算量を低減し、その計算速度の向上を図ることができる。
【符号の説明】
【００９０】
１‥環境認識装置、１１‥第１演算処理要素、１２‥第２演算処理要素、１３‥第３演算処理要素、Ｃ０‥距離画像センサ、Ｃ１‥基準画像センサ、Ｃ２‥参照画像センサ。
【図１】
0005515018.tif 000002
【図２】
0005515018.tif 000003
【図３】
0005515018.tif 000004
【図４】
0005515018.tif 000005
【図５】
0005515018.tif 000006
【図６】
0005515018.tif 000007