JP5959899B2 - 接触状態推定装置 - Google Patents

Description

本発明は、距離画像センサ及び２つ以上の画像センサを用いて被写体までの距離等を推定する装置に関する。

距離画像センサを通じて取得された距離画像と、ステレオカメラのそれぞれを通じて取得された距離画像とを融合することにより、３次元画像を取得する手法が提案されている（特許文献１参照）。

複数種類の既知形状のパッチのうちいずれか１種のパッチによって床面の複数の散在する局所領域のそれぞれを近似する（パッチマップを作成する）とともに、パッチマップを集め、脚式移動ロボット用空間マップを生成する手法が提案されている（非特許文献１参照）。

特開平０９−００５０５０号公報

"Curved Surface Contact Patches with Quantified Uncertainty", Marsette Vona and Dimitrios Kanoulas, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, Pages: 1439-1446

しかし、地面から浮いていた足平が接地した直後における、当該足平と地面との接触状態が予測されないため、足平が実際に接地した後における当該接地状態によっては、ロボットの姿勢を安定に制御することが困難となる可能性がある。

そこで、本発明は、脚式移動ロボットの足平及び床面など、異なる物体同士の未来の接触状態を高精度で推定しうる装置を提供することを課題とする。

本発明は、実物体を撮像することにより指定物理量を画素値として有する複数のピクセルによって構成される一対の画像のそれぞれを取得するように構成されている一対の画像センサと、
前記一対の画像センサのうち一方の画像センサにより取得された一方の画像の対象領域を構成する複数のピクセルのそれぞれに対して割り当てられる、前記実物体までの距離を取得するように構成されている距離画像センサと、を用いて前記実物体の表面と、指定形状の仮想物体の表面である仮想面との接触状態を、前記実物体と前記仮想物体に相当する他の実物体との接触状態として推定する装置に関する。

本発明の接触状態推定装置は、前記対象領域において各ピクセルに割り当てられる第１偏差を主変数とする第１コスト関数の合計を第１コストとして算定するように構成されている第１演算処理要素と、前記対象領域において各ピクセルに割り当てられる第２偏差又は前記第１及び第２偏差の両方を主変数とする第２コスト関数の合計を第２コストとして算定するように構成されている第２演算処理要素と、前記第１コストと前記第２コストとの統合コストを最小値又は極小値に近づけるように前記実物体の表面に接触する際の前記仮想面の位置及び姿勢を探索するように構成されている第３演算処理要素とを備えていることを特徴とする。

「第１偏差」が、前記距離画像センサにより取得された前記実物体までの距離により位置が定まる実点と、当該実点を前記一方の画像センサの視線方向について前記仮想面に投影した結果としての仮想点との間隔の長短に応じて絶対値の大小が定まり、かつ、前記一方の画像センサを基準として前記仮想点が前記実点より遠くに位置する場合は正値である一方、前記一方の画像センサを基準として前記仮想点が前記実点より近くに位置する場合は負値である変数として定義されている。「第２偏差」が、前記一方の画像のピクセルが有する前記指定物理量と、前記仮想面の位置及び姿勢に応じた形で当該一方の画像のピクセルに対応する、他方の画像センサにより取得された他方の画像のピクセルが有する前記指定物理量との偏差の大小に応じて大小が定まる変数として定義されている。

「第１コスト関数」及び「第２コスト関数」のそれぞれが、主変数の値が０である場合に最小値又は極小値を示し、かつ、正の定義域において増加関数となる関数として定義されている。

本発明の接触状態推定装置によれば、仮想物体の少なくとも一部が実物体の内部に入り込んだ状態が設定された場合、第１偏差が正値となる。このため、当該設定によりある仮想点が実物体の表面から離れて内部の深い箇所にあるほど、第１コスト又は第１及び第２コストの両方が高く評価され、統合コストも高く評価される。

このため、統合コストが最小値又は極小値に近づけられるように仮想面の位置及び姿勢が探索されることにより、仮想点の全部が実物体の内部にはなく、かつ、実物体表面上に位置する仮想点群により定まる仮想面の位置及び姿勢が、当該仮想面の実物体に対する接触状態として推定されうる。

これにより、撮像対象となった一の実物体と、仮想物体に相当する他の実物体とのそれぞれにより、共通の実空間が同時に占有されているという実現不可能な状態が両実物体の接触状態として探索される事態が確実に回避される。仮想物体に相当する他の実物体とは、仮想面と形状及びサイズが同じである表面を有する又は当該表面を適応的に形成しうる実物体を意味する。そして、実物体に対する仮想物体の未来における接触状態が高精度で推定されうる。

本発明の接触状態推定装置において、前記第１コスト関数が、前記第１偏差の絶対値の正の冪乗と、前記第１偏差を主変数とし、かつ、値域が０以上である関数であって、絶対値が同一であっても正の定義域における値が負の定義域における値よりも大きい第１係数関数との積により定義され、前記第２コスト関数が、前記第２偏差の絶対値の正の冪乗と、前記第１偏差又は前記第２偏差を主変数とし、かつ、値域が０以上である関数であって、絶対値が同一であっても正の定義域における値が負の定義域における値よりも大きい第２係数関数との積により定義されていることが好ましい。

当該構成の接触状態推定装置によれば、正負の定義域で非対称的な第１及び第２コスト関数のうち一方又は両方にしたがって、第１及び第２コストのうち一方又は両方が計算されるように、第１及び第２係数関数が定義されている。第１コストは、一方の画像の対象領域に含まれる各ピクセルにおいて第１係数関数の値をバネ係数とする仮想的なバネ群の、第１偏差を変形量とする弾性エネルギーの合計に相当する。同様に、第２コストは、一方の画像の対象領域に含まれる各ピクセルにおいて第２係数関数の値をバネ係数とする仮想的なバネ群の、第２偏差を変形量とする弾性エネルギーの合計に相当する。

このため、実物体の内部にある仮想点を実物体の表面に近づけることが、実物体の外部にある仮想点を実物体の表面に近づけることよりも優先された形で仮想面の位置及び姿勢が探索されうる。これにより、撮像対象となった実物体及び仮想物体に相当する他の実物体のそれぞれにより共通の実空間が同時に占められているような実現不可能な状態が両物体の接触状態として探索される事態が確実に回避される。そして、実物体に対する仮想物体の未来における接触状態が高精度で推定されうる。

本発明の接触状態推定装置において前記第１及び第２係数関数のうち一方又は両方が少なくとも正の定義域において増加関数として定義されていることが好ましい。

当該構成の接触状態推定装置によれば、前記仮想的なバネのバネ係数の値は、当該バネの変形量が大きいほど大きい値になる。このため、仮想面の位置及び姿勢の設定により、ある仮想点が実物体の表面から離れて内部の深い箇所に置かれるほど、当該仮想的なバネの弾性エネルギーに相当する第１コスト又は第１及び第２コストの両方がさらに高く評価され、統合コストも高く評価される。

その結果、実物体の内部にある仮想点を実物体の表面により迅速又は強力に近づけるように仮想面の位置及び姿勢が探索されうる。そして、実物体に対する仮想物体の未来における接触状態が高精度かつ高速で推定されうる。

本発明の接触状態推定装置において、前記第１及び第２係数関数のうち一方又は両方が負の定義域又は負の指定値未満の定義域において０である関数として定義されていることが好ましい。

当該構成の接触状態推定装置によれば、実物体の外部にある仮想点又は実物体の表面からある程度離れている仮想点のコストが一律に「０」に評価される。これにより、統合コストを最小値又は極小値に近づけるような仮想点の座標値の組み合わせの探索処理に要する計算負荷が軽減される。

本発明の接触状態推定装置において、前記第２係数関数の主変数としての前記第２偏差が、前記一対の画像センサのそれぞれにより撮像された、実空間における同一箇所の前記指定物理量は同一であるという拘束条件にしたがって、前記指定物理量の偏差が換算された結果としての前記一対の画像センサの視差残差又は当該視差残差に相当する距離残差として定義されていてもよい。

本発明の接触状態推定装置において、前記距離画像センサが、前記実物体までの距離を画素値として有する複数のピクセルによって構成される距離画像を取得するように構成され、前記第１演算処理要素が、前記一方の画像センサ及び前記距離画像センサの相対的な配置関係に応じて、前記距離画像を構成する各ピクセルの画素値に応じた距離が、前記一方の画像の前記対象領域を構成する複数のピクセルに対して画素値として割り当てるように構成されていてもよい。

本発明の接触状態推定装置において、前記第１及び第２係数関数のうち一方又は両方が負の定義域又は負の指定値未満の定義域において正の指定値である関数として定義されていることが好ましい。

本発明の接触状態推定装置において、前記第１演算処理要素は、前記対象領域を前記仮想物体の表面に投影した領域に基準点を定め、該基準点または該基準点に対応した実点から所定の範囲にある実点を選択し、前記第１偏差を定めることが好ましい。

本発明の一実施形態としての接触状態推定装置の構成説明図。 本発明の接触状態推定装置の使用態様に関する説明図。 本発明の接触状態装置の機能説明図。 基準画像座標系及び距離画像座標系の相関関係に関する説明図。 実点、仮想面及び仮想点に関する説明図。 係数関数に関する説明図。 コスト関数に関する説明図。 仮想的なバネに関する説明図。 距離残差及びエピポーラ拘束条件に関する説明図。 足平裏面及び床面の接触状態推定結果の説明図（第１実施形態）。 基準画像座標系における足平裏面及び床面の接触状態推定結果に関する説明図（第１実施形態）。 マスクを用いた本発明の接触状態推定装置の機能説明図。 マスクの設定方法に関する説明図。 マスクに関する説明図。 足平裏面及び床面の接触状態推定結果の説明図（第２実施形態）。 基準画像座標系における足平裏面及び床面の接触状態推定結果に関する説明図（第２実施形態）。

（構成）
図１に示されている接触状態推定装置は、距離画像センサ１０と、基準画像センサ１１と、参照画像センサ１２と、コンピュータ２０とを備えている。基準画像センサ１１及び参照画像センサ１２は「一対の（又は左右一対の）画像センサ」を構成する。基準画像センサ１１は「一方の画像センサ」を構成し、参照画像センサ１２は「他方の画像センサ」を構成する。接触状態推定装置は、例えば、図２に示されているように脚式移動ロボットに搭載され、その足平等の脚体の一部と床面との接触状態を推定するために利用される。

接触状態推定装置が、同一の指定物理量を画素値とする画像を取得可能な３つ以上の画像センサを構成要素として備え、コンピュータ２０が当該３つ以上の画像センサのうち２つを一対の画像センサとして選択するように構成されていてもよい。

脚式移動ロボットの構成としては、特許第３６７４７８８号公報等、本出願人が提案している構成が採用されうる。また、接触状態推定装置は、ロボットアームにおいて手の平又はアームの一部と対象物体との接触状態を推定するために利用される。さらに、接触状態推定装置は車両に搭載され、車両のタイヤと路面との接触状態を推定するために利用される。

距離画像センサ１０はたとえばＴＯＦ（Time-of-Flight）距離画像センサであり、各ピクセルが距離計測値Ｄｓ’（図４参照）を画素値として有する１次距離画像を取得する。距離画像センサ１０の撮像面又は平面上に配列されている撮像素子群により「１次距離画像座標系」が定義される。

基準画像センサ１１は可視光ステレオカメラの一方のカメラ（例えば右側カメラ）であり、各ピクセルが少なくとも輝度（指定物理量）を画素値として有する基準画像を取得する。基準画像センサ１１の光軸方向がＺ軸方向として定義されている（図４参照）。基準画像センサ１１の撮像面又は平面上に配列されている撮像素子群により「基準画像座標系」が定義される。基準画像座標系の横方向がＸ軸方向として定義され、縦方向がＹ軸方向として定義される。

参照画像センサ１２は可視光ステレオカメラの他方のカメラ（例えば左側カメラ）であり、基準画像と同じく各ピクセルが少なくとも輝度を画素値として有する参照画像を取得する。参照画像センサ１２の撮像面又は平面上に配列されている撮像素子群により「参照画像座標系」が定義される。

距離画像センサ１０、基準画像センサ１１及び参照画像センサ１２のそれぞれのカメラパラメータ（内部パラメータ及び外部パラメータ）は既知であり、接触状態推定装置を構成するコンピュータＣＣのメモリに保存されている。たとえば、１次距離画像座標系及び基準画像座標系の間の座標変換を表わす回転行列及び並進行列又はこれに等価なクォータニオンはメモリに保存されている。同様に、基準画像座標系及び参照画像座標系の間の座標変換を表わす回転行列及び並進行列又はこれに等価なクォータニオンはメモリに保存されている。

接触状態推定装置がロボットに搭載されている場合、１次距離画像座標系、基準画像座標系及び参照画像座標系のそれぞれのロボット座標系に対する位置及び姿勢は、ロボットの挙動を表わす順キネマティクスモデルにしたがって算出された上でメモリに保存される。

ロボット座標系は、ロボットの質量中心（たとえば基体に含まれている。）が原点として定義され、ロボットの上方が＋ｘ方向として定義され、右方向が＋ｙ方向として定義され、前方が＋ｚ方向として定義されている。ロボット座標系の世界座標系における位置及び姿勢は、たとえば、ロボットの行動計画により定義される。

接触状態推定装置を構成するプログラマブルコンピュータ２０は、距離画像センサ１０、基準画像センサ１１及び参照画像センサ１２のそれぞれからの画像信号を対象として後述する演算処理を実行するように構成されている第１演算処理要素２１と、第２演算処理要素２２と、第３演算処理要素２３とを備えている。単一のプロセッサ（演算処理装置）が当該３つの演算処理要素２１〜２３として機能してもよいし、複数のプロセッサが相互通信により連携しながら当該３つの演算処理要素２１〜２３として機能してもよい。

各演算処理要素が担当演算処理を実行するように「構成されている」とは、各演算処理要素を構成するＣＰＵ等の演算処理装置が、ＲＯＭ、ＲＡＭ等のメモリ又は記録媒体から必要な情報に加えてソフトウェアを読み出し、当該情報に対して当該ソフトウェアにしたがって演算処理を実行するように「プログラムされている」ことを意味する。

（機能）
前記構成の接触状態推定装置によって実行される接触状態推定方法について説明する。本実施形態では、ロボットの将来における足平の裏面（仮想面）と床面（実物体表面）との接触状態が推定される。そのほか、ロボットがアームの先端にハンドを有する場合、当該ハンドの手の平面又は指先部分の表面と、当該ロボットが遂行すべきタスクの対象となるコップ等の実物体との接触状態が推定されてもよい。

距離画像センサ１０による距離計測値Ｄｓ（ｓ）を画素値として有する複数のピクセルによって構成される「２次距離画像」が取得される（図３／ＳＴＥＰ０２）。「ｓ」は基準画像座標系におけるピクセル位置のうち、基準画像における対象領域であるＲＯＩ（Region of Interest）に含まれ、かつ、距離計測値Ｄｓが割り当てられたピクセル位置（量子化された点の位置）、正確にはその座標値を意味する。

具体的には、距離画像センサ１０を通じて「１次距離画像（距離画像）」が取得される。１次距離画像を構成する複数のピクセルｓ’（図４参照）には、被写体までの距離計測値Ｄｓ’（図５参照）が画素値として割り当てられている。

続いて１次距離画像座標系のピクセル位置ｓ’が、基準画像座標系に座表変換された結果としての点位置ｓ”が求められる（図４参照）。１次距離画像座標系を基準とした観測点Ｐｓの位置は、ベクトル^ｐ’＝Ｄｓ’^ｅ’（ｓ’）により表わされる。「^」はベクトルを意味する（以下同じ。）。「^ｅ’（ｓ’）」は、１次距離画像座標系のピクセル位置ｓ’を通る距離画像センサ１０の視線方向を示す単位ベクトルである。

１次距離画像座標系から基準画像座標系への座標変換を表わす回転行列Ｒ及び並進行列Ｔに基づき、基準画像座標系を基準とした観測点Ｐｓの位置を表わすベクトル^ｐ”＝Ｒ^ｐ’＋Ｔが計算される。回転行列Ｒ及び並進行列Ｔは、予めメモリに保存されている。回転行列Ｒ及び並進行列Ｔは、数学的にこれと等価なクォータニオンによって定義されていてもよい。

当該ベクトル^ｐ”及びその奥行き方向成分（Ｘ−Ｙ座標系である基準画像座標系に対して垂直なＺ方向の成分）であるＺｓ”に基づき、基準画像座標系において観測点Ｐｓに対応する座標位置ｓ”＝（１／Ｚｓ”）^ｐ”が求められる。

基準画像座標系において当該座標位置ｓ”（図４白丸参照）は、一般的にピクセル位置ｓ（図４黒丸参照）に一致しない。このため、最近傍点補間法、双線形補間法又はバイキュービックスプライン法など、公知の補間方法にしたがって、基準画像座標系の各ピクセル位置ｓに対して、座標位置ｓ”に対応する距離Ｚｓ”に基づいて定まる距離（実点距離）Ｚｓが割り当てられる。

そして、基準画像座標系の各ピクセル位置ｓに対して、２次距離画像座標系を基準とした観測点Ｐｓの位置を表わすベクトル^ｐ＝Ｄｓ^ｅ（ｓ）のノルムＤｓ（ｓ）が画素値として割り当てられる。「^ｅ（ｓ）」は、２次距離画像座標系のピクセル位置ｓを通り観測点Ｐｓに向かう視線方向を示す単位ベクトルである。

ただし、距離画像センサ１０及び基準画像センサ１１のそれぞれの解像度の相違等を考慮して、基準画像座標のすべてのピクセル位置に距離計測値が割り当てられる必要はない。これにより、２次距離画像が取得される。

また、基準画像センサ１１を通じて、少なくとも輝度（指定物理量）を画素値として有する複数のピクセルにより構成される「基準画像」が取得される（図３／ＳＴＥＰ０４）。

さらに、参照画像センサ１２を通じて、基準画像と同様に少なくとも輝度を画素値として有する複数のピクセルにより構成される「参照画像」が取得される（図３／ＳＴＥＰ０６）。

同一時刻において取得され、メモリに保存されている同一時刻における２次距離画像、基準画像及び参照画像を対象として次に説明する一連の処理が実行される。なお、距離画像センサ１０、基準画像センサ１１及び参照画像センサ１２が完全に同期していない場合、同一時刻における２次距離画像、基準画像及び参照画像が、若干異なった又は略同一の時刻のそれぞれにおいて取得されてもよい。

まず、基準画像座標系のピクセル位置ｓについてロボットの足平裏面（仮想物体の表面）が「仮想面」として設定される（図３／ＳＴＥＰ０８）。具体的には、仮想面上にある第ｉ仮想点の基準位置（ベクトル）^ｑ_iが設定されることにより、足平裏面の基準座標系における基準位置及び基準姿勢が設定される。足平裏面が平面状である場合、基準位置^ｑ_iは関係式（００１）によって定義される。

^q_i=^t(x_i, y_i, 1) ..(001 )。

ここで「^t」は転置を表わす。（ｘ_i，ｙ_i）は２次距離画像座標系において距離計測値ｚ_iを画素値として有するピクセル位置を表わす。基準位置群Ｑ＝（^ｑ₁，‥^ｑ_i，‥^ｑ_n）によって画定される範囲の面積及び形状は、記憶装置に保存されている足平裏面の面積及び形状に加えて、距離に応じて一義的に定められる。

一方、足平裏面の基準位置及び基準姿勢は任意に変更されうる。例えば、基準位置^ｑ_iのｘ成分に所定値が一律に加算又は減算されることにより、基準座標系における足平裏面の基準位置がｘ方向に当該所定値だけ変更されうる。また、基準位置^ｑ_iのｘ成分及びｙ成分のそれぞれに、ｚ軸回りの所定角度θの回転を表わす因子ｃｏｓθ又はｓｉｎθが一律に乗算されることにより、基準座標系における足平裏面のｘｙ平面における基準姿勢が変更されうる。

さらに、足平裏面の基準座標系における位置及び姿勢を表わす平面パラメータ^ｍ（ｓ）＝^t（ｍ₁，ｍ₂，ｍ₃）が設定される。平面パラメータ^ｍ（ｓ）の初期値は任意であってもよいが、後述するように平面パラメータ^ｍ（ｓ）は前回値が修正されることにより今回値が設定される。

２次距離画像座標系（２次元）に対して直交する方向をｚ方向とする３次元直交座標系が基準座標系として採用される（図５参照）。基準画像センサ１１の光学中心と物体表面との距離Ｄｓが取得されているので（図３／ＳＴＥＰ０２及び図５参照）、距離画像において光軸に対して角度ψをなす位置にある画素の画素値Ｚｓ（ｚ成分）はＤｓｃｏｓψと定義される。このため、距離画像センサ１０がキャリブレーションされている場合、当該定義にしたがってｚ成分Ｚｓ（ｓ）が計算されうる。

基準座標系としては、ロボット座標系又は世界座標系が採用されてもよい。例えば、距離画像センサ１０がロボットに搭載されている場合、距離画像座標系のロボット座標系に対する位置及び姿勢は、ロボットの挙動を表わす順キネマティクスモデルにしたがって算出された上で記憶装置に保存される。距離画像座標系のロボット座標系に対する位置及び姿勢は、並進行列及び回転行列又はこれに等価のクォータニオンによって定義される。

ロボット座標系は、ロボットの質量中心（たとえば基体に含まれている。）が原点として定義され、ロボットの上方が＋ｘ方向として定義され、右方向が＋ｙ方向として定義され、前方が＋ｚ方向として定義されている（図２参照）。ロボット座標系の世界座標系における位置及び姿勢は、たとえば、記憶装置に保存されているロボットの行動計画により定義される。

２次距離画像座標系を基準とした３次元直交座標系における座標値が、記憶装置に保存されている当該行列を用いて座標変換されることで、ロボット座標系における座標値が算定されうる。２次距離画像座標系を基準とした３次元直交座標系における座標値が、記憶装置に保存されている当該行列及び行動計画を用いて座標変換されることで、世界座標系における座標値が算定されうる。

平面パラメータ^ｍは、足平裏面の形状Ｑと、実点座標値群Ｚ＝^t（ｚ₁，‥，ｚ_i，‥ｚ_n）と、ｎ次単位行列Ｉとに基づき、関係式（００２）にしたがって定義される。

m=(QI^tQ)^-1QIZ ..(002)。

そして、関係式（００３）にしたがって算定されるベクトル^Ｚの各成分が、第ｉ仮想点座標値Ｐｃ（ｓ）（正確にはそのｚ成分Ｚｃ（ｓ））として仮定される（図５参照）。

^Z=^tQm ..(003)。

仮想物体の表面形状は、本発明の装置のユーザによって平面のほか任意形状の曲面に変更されうる。例えば、曲面パラメータｍ＝^t（ｍ₁₁，ｍ₁₂，ｍ₂₁，ｍ₂₂，ｍ₃）が設定されることにより、関係式（０２２）によって表現される曲面を表面として有するように仮想物体の形状が設定されてもよい。

m₁₁x²+m₁₂x+ m_y21y²+m₂₂y+ m₃z=α ..(022)。

この場合、例えば、曲面パラメータ又は関係式（０２２）によって表面形状が表現される手の平と、手すりなどの物体との接触状態が推定されうる。

第１演算処理要素２１が、実点Ｐｓ（ｓ）と仮想点Ｐｃ（ｓ）との間隔｜Ｄｓ（ｓ）−Ｄｃ（ｓ）｜の広狭に応じて定まる第１偏差ｅ₁（ｓ）に基づき、第１係数関数ｗ₁（ｅ₁）にしたがって第１係数ｗ₁（ｓ）を算定する（図３／ＳＴＥＰ１２）。

例えば、第１偏差ｅ₁（ｓ）として、実点距離Ｚｓ（ｓ）と仮想点距離Ｚｃ（ｓ）との距離残差｜Ｚｓ（ｓ）−Ｚｃ（ｓ）｜が採用される（図５参照）。

「実点」とは、２次距離画像座標系（基準画像座標系のうち一部である対象領域に相当する。）におけるピクセル位置ｓ＝（ｕ，ｖ）の画素値Ｄｓ（ｓ）に応じて実空間位置が定まる点Ｐｓ（ｓ）＝Zs^ｓを意味する（図５参照）。＾ｓ＝（ｕ，ｖ，１）はｓの位置を表わすベクトルである。また「仮想点」とは、基準画像センサ１１の視線方向について実点Ｐｓ（ｓ）を仮想面に投影した点を意味する（図５参照）。

距離残差｜Ｄｓ（ｓ）−Ｄｃ（ｓ）｜の大小に応じて幾何学的関係から一義的に定まるさまざまな距離が、第１偏差ｅ₁（ｓ）として採用されてもよい。例えば、当該間隔｜Ｄｓ（ｓ）−Ｄｃ（ｓ）｜そのもののほか、実点Ｐｓ（ｓ）がＺ方向について仮想面に投影された結果としての点と、仮想点Ｐｃ（ｓ）との指定方向についての間隔が第１偏差ｅ₁（ｓ）として採用されてもよい。また、仮想点Ｐｃ（ｓ）がＺ方向について被写体に相当する平面に投影された結果としての点と、実点Ｐｓ（ｓ）との指定方向についての間隔が第１偏差ｅ₁（ｓ）として採用されてもよい。

第１係数ｗ₁（ｅ₁（ｓ））は、第１偏差ｅ₁を主変数とし、関係式（１０１）によって定義されている第１係数関数（従変数）ｗ₁（ｅ₁）にしたがって算定される。この第１係数関数ｗ₁（ｅ₁）は、図６（ａ）に示されているように第１偏差ｅ₁の増加関数である。第１係数関数ｗ₁（ｅ₁）は、負の定義域（ｅ₁＜０）において０以上かつ正の指定値ε₁未満の値域を有する一方、０以上の定義域（ｅ₁≧０）において正の指定値ε₁以上の値域を有する。

w₁(e₁)=log(1+exp(αe₁-β)), (α>0, β>0) ..(101)。

第１係数ｗ₁（ｅ₁（ｓ））が関係式（１０２）によって定義されている第１係数関数ｗ₁（ｅ₁）にしたがって算定されてもよい。この第１係数関数ｗ₁（ｅ₁）は、図６（ｂ）に示されているように、負の定義域において０以上かつ正の指定値ε₁未満の値域を有する一方、０以上の定義域において正の指定値ε₁以上の値域を有する。第１係数関数ｗ₁（ｅ₁）は、負の指定値−ε₂（ε₂＞０）以下の定義域において０である一方、負の指定値−ε₂を超えている定義域において増加関数である。

w₁(e₁)=0 (if e₁≦-ε₂), or w₁(e₁)=(ε₁/ε₂)e₁+ε₁(if -ε₂<e₁) ..(102)。

第１係数ｗ₁（ｅ₁（ｓ））が関係式（１０３）によって定義されている第１係数関数ｗ₁（ｅ₁）にしたがって算定されてもよい。この第１係数関数ｗ₁（ｅ₁）は、図６（ｃ）に示されているように、０未満の定義域において０である一方、０以上の定義域において増加関数である。

w₁(e₁)=0 (if e₁≦0), or w₁(e₁)=ε₀e₁, (0<ε₀) (if 0< e₁) ..(103)。

第１演算処理要素２１が、第１係数ｗ₁（ｅ₁（ｓ））及び第１偏差ｅ₁（ｓ）に基づき、第１コスト関数Ｅ₁（ｅ₁）（本発明の「第１コスト関数」に相当する。）にしたがって第１コストＥ₁を算定する（図３／ＳＴＥＰ１２）。

第１コスト関数Ｅ₁（ｅ₁）は、例えば関係式（１１０）に示されているように、第１係数関数ｗ₁（ｅ₁）と、第１偏差の絶対値｜ｅ₁｜の二乗との積として定義されている。このため、第１コストＥ₁は、第１係数ｗ₁（ｓ）をバネ係数とし、仮想点を実点に近づけるような、図８に示されている仮想的なバネ群の弾性エネルギーの合計に相当する。当該冪乗数は「２」に限られず、「０．５」「１．５」「３」など、任意の正値に設定されていてもよい。

E₁(e₁)＝w₁(e₁)|e₁|² ..(110)。

第１偏差ｅ₁に基づき、第１係数関数ｗ₁（ｅ₁）にしたがって第１係数ｗ₁が算定され（図３／ＳＴＥＰ１０参照）、これとは別個に第１偏差の絶対値｜ｅ₁｜の冪乗（２乗）の値が算定され、その上で当該算定結果が掛け合わせられることにより第１コストＥ₁が算定される（図３／ＳＴＥＰ１２参照）。これに代えて、第１偏差ｅ₁に基づき、第１コスト関数Ｅ₁（ｅ₁）にしたがって第１コストＥ₁が算定されてもよい。

第１コストＥ₁は、基準画像座標系における対象領域（２次距離画像座標系）に属するピクセルｓについて、第１コスト関数Ｅ₁（ｅ₁）の値の合計を表わす関係式（１２０）にしたがって算定される。「Σ_ｓ」は、基準画像座標系における対象領域（２次距離画像座標系）に属するピクセルｓについての和を意味する。

E₁＝Σ_sE₁(e₁(s)) ..(120)。

第１偏差ｅ₁（ｓ）は仮想面の平面パラメータ^ｍ（ｓ）を変数とする関数であるため、第１偏差ｅ₁（ｓ）に基づいて算出される第１コストＥ₁も仮想面の平面パラメータ^ｍ（ｓ）を変数とする関数となる。

第１係数関数ｗ₁（ｅ₁）が関係式（１０１）にしたがって定義されている場合（図６（ａ）参照）、第１コスト関数Ｅ₁（ｅ₁）は、図７（ａ）に示されているようにｅ₁＝０で極小値０を有し、かつ、正の定義域（ｅ₁＞０）において増加関数となる。また、第１コスト関数Ｅ₁（ｅ₁）は、正負の定義域において非対称的な変化特性を示す。すなわち、正の変数値ｅ₁＝ｃ（＞０）に基づく第１コスト関数の値Ｅ₁（ｃ）が、これと絶対値が等しい負の変数値−ｃに基づく第１コスト関数の値Ｅ₁（−ｃ）よりも大きくなる。

第１係数関数ｗ₁（ｅ₁）が関係式（１０２）にしたがって定義されている場合（図６（ｂ）参照）、第１コスト関数Ｅ₁（ｅ₁）は、図７（ｂ）に示されているようにｅ₁＝０で極小値０を有し、かつ、正の定義域において増加関数となる。また、第１コスト関数Ｅ₁（ｅ₁）は、正負の定義域−ε₂＜ｅ₁＜ε₂において対称的な変化特性を示す。第１コスト関数Ｅ₁（ｅ₁）は、負の指定値以下の負の定義域ｅ₁≦−ε₂においては０となる。

第１係数関数ｗ₁（ｅ₁）が関係式（１０３）にしたがって定義されている場合（図６（ｃ）参照）、第１コスト関数Ｅ₁（ｅ₁）は、図７（ｃ）に示されているように負の定義域において０となり、正の定義域において増加関数となる。

第２演算処理要素２２が、基準画像センサ１１及び参照画像センサ１２のそれぞれが、同一の仮想面を撮像しているという仮定下で、参照画像を基準画像座標系に座標変換することにより変換画像を生成する（図３／ＳＴＥＰ１４）。具体的には、基準画像座標系のピクセル位置ｓに対して、参照画像座標系のうち当該仮想面の位置及び姿勢に応じた視差が存在する場合におけるピクセル位置ｓ_refの輝度が割り当てられた画像が変換画像として取得される。

第２演算処理要素２２が、第２係数ｗ₂（ｅ₁（ｓ））及び第２偏差ｅ₂（ｓ）に基づき、第２コスト関数Ｅ₂（ｅ₁，ｅ₂）にしたがって第２コストＥ₂を算定する（図３／ＳＴＥＰ１６）。基準画像座標系及び変換画像座標系の同一ピクセル位置ｓにおける画素値である指定物理量の偏差、例えば輝度残差ΔＩ（ｓ）が第２偏差ｅ₂（ｓ）として採用される。可視光カメラを通じて取得される色（ＲＧＢ値）、赤外光カメラを通じて取得される温度など、可視光カメラを通じて取得される輝度とは異なる物理量が指定物理量として採用されてもよい。

第２コスト関数Ｅ₂（ｅ₁，ｅ₂）は、例えば関係式（２１０）に示されているように、第２係数関数ｗ₂（ｅ₁）と、第２偏差の絶対値｜ｅ₂｜の二乗との積として定義されている。このため、第２コストＥ₂は、第２係数ｗ₂（ｅ₁）をバネ係数とし、「同一時刻において基準画像センサ１１及び参照画像センサ１２のそれぞれにより撮像された同一箇所の指定物理量（ここでは輝度）が同一である」という拘束条件にしたがって仮想点を実点に近づけるような、仮想的なバネの弾性エネルギーに相当する（図８参照）。すなわち、第２コストＥ₂は、当該仮想的なバネ群の弾性エネルギーの合計に相当する。当該冪乗数は「２」に限られず、「０．５」「１．５」「３」など、任意の正値に設定されていてもよい。

E₂(e₁, e₂)＝w ₂(e₁)|e₂|² ..(210)。

第１偏差ｅ₁に基づき、第２係数関数ｗ₂（ｅ₁）にしたがって第２係数ｗ₂が算定され（図３／ＳＴＥＰ１０参照）、これとは別個に第２偏差の絶対値｜ｅ₂｜の冪乗（２乗）の値が算定され、その上で当該算定結果が掛け合わせられることにより第１コストＥ₁が算定される（図３／ＳＴＥＰ１２参照）。これに代えて、第１偏差ｅ₁及び第２偏差ｅ₂に基づき、第２コスト関数Ｅ₂（ｅ₁，ｅ₂）にしたがって第２コストＥ₂が算定されてもよい。

第２コストＥ₂は、基準画像座標系における対象領域（２次距離画像座標系）に属するピクセルｓについて、第２コスト関数Ｅ₂（ｅ₁，ｅ₂）の値の合計を表わす関係式（２２０）にしたがって算定される
E₂＝Σ_sE₂(e₁(s), e₂(s)) ..(220)。

第１偏差ｅ₁（ｓ）と同様に第２偏差ｅ₂（ｓ）は仮想面の平面パラメータ＾ｍ（ｓ）を変数とする関数であるため、第１偏差ｅ₁（ｓ）及び第２偏差ｅ₂（ｓ）に基づいて算出される第２コストＥ₂も仮想面の平面パラメータ＾ｍ（ｓ）を変数とする関数となる。

第２係数関数ｗ₂（ｅ₁）は、第１係数関数ｗ₁（ｅ₁）と同一であってもよく、異なっていてもよい。例えば、第１係数関数ｗ₁（ｅ₁）及び第２係数関数ｗ₂（ｅ₁）がともに関係式（１０１）にしたがって定義されていてもよい。第１係数関数ｗ₁（ｅ₁）が関係式（１０１）にしたがって定義される一方、第２係数関数ｗ₂（ｅ₁）が関係式（１０２）にしたがって定義されていてもよい。第２係数関数ｗ₂の主変数として第１偏差ｅ₁に代えて第２偏差ｅ₂が用いられてもよい。

輝度残差ΔＩ（ｓ）は前記拘束条件にしたがって求められる。簡単のため、基準画像センサ１１及び参照画像センサ１２が、等しい内部パラメータを持ち互いの光軸が平行になるよう配置された平行ステレオの場合について説明する。

基準画像センサ１１及び参照画像センサ１２が平行ステレオの関係にない場合、図９（ａ）に示されているように視差残差Δｕは、参照画像センサ１２に距離計測値Ｄｓ（ｓ）及び距離候補値Ｄｃ（ｓ）のそれぞれを投影した結果としてのピクセル位置の間隔となる。視差残差Δｕは、実点距離Ｚｓと、仮想点距離Ｚｃと、両画像センサ１１及びＣ２の基線長Ｌとに基づき、関係式（２０１）にしたがって定義される（図５参照）。

Δu=L{(1/Zs)-(1/Zc)} ..(201)。

「ｕ」は基準画像座標系（又は参照画像座標系）における横方向の位置を表わす座標値である。平行ステレオの場合、ｕ軸の方向はエピポーラ線と平行になる。基準画像と、視差残差Δｕが存在するという仮定下での参照画像との輝度残差ΔＩは、エピポーラ拘束条件を考慮に入れて、関係式（２０２）により表現される（図９（ｂ）参照）。

ΔI=(∂I/∂u)^e_ep^Δu ..(202)。

「^ｅ_ep」は基準画像座標系におけるエピポーラ線の方向を表わす単位ベクトルである。「(∂I/∂u)」は輝度勾配を表すベクトルである。特に平行ステレオの場合、ｕ軸方向のみの輝度勾配が有効となる。

関係式（２０２）は、前記拘束条件にしたがって視差残差Δｕ（ｓ）が輝度残差ΔＩ（ｓ）に変換されることを表わしている。視差残差Δｕ（ｓ）は、距離計測値Ｄｓに応じて基準画像座標系（２次元距離座標系）のピクセルｓを参照画像座標系に対して投影した結果としての位置と、距離候補値Ｄｃに応じて基準画像座標系のピクセルｓを参照画像座標系に投影した結果としての位置との間隔である。

輝度残差ΔＩ（ｓ）のほか、輝度残差ΔＩ（ｓ）が関係式（２０２）にしたがって換算された視差残差Δｕ又は輝度残差ΔＩ（ｓ）が関係式（２０２）（２０３）にしたがって換算された距離残差ΔＺ（ｓ）が第２偏差ｅ₂（ｓ）として用いられてもよい。関係式（２０３）は、Δｕ＝（ｄｕ／ｄＺｃ）ΔＺ及びΔＺ＝Ｚｓ−Ｚｃという関係式にしたがって関係式（２０１）が変形されることにより得られる。

Δu(s)=-(L/Zc²)ΔZ(s) ..(203)。

関係式（２０２）及び（２０３）にしたがって、距離残差ΔＺ（ｓ）＝Ｚｓ（ｓ）−Ｚｃ（ｓ）が存在する場合の輝度残差ΔＩ（ｓ）が計算されうる。

第３演算処理要素２３が、関係式（３０１）又は（３０２）で表わされている第１コストＥ₁及び第２コストＥ₂の線形和を統合コストＥとして算出する。前記のように第１コストＥ₁及び第２コストＥ₂はともに仮想面の平面パラメータ＾ｍを変数とする関数であるため、統合コストＥも平面パラメータ^ｍの関数Ｅ（^ｍ）として定義される。

E(^m)=E₁(^m)+E₂(^m) ..(301)。

E(^m)=χE₁(^m)+(1−χ)E₂(^m), (0<χ<1)..(302)。

そして、第３演算処理要素２３は、最小二乗法又は仮想面の平面パラメータを（∂Ｅ（^ｍ）／∂^ｍ）に応じた分だけ逐次変動させる勾配法にしたがって、統合コストＥを最小値にする平面パラメータ^ｍを探索する（図３／ＳＴＥＰ１８）。これにより、仮想面（足平裏面）の位置及び姿勢が推定される。

統合コストＥの前回値と今回値との差が閾値以下になる等、平面パラメータ^ｑが一定の収束条件を満足しているか否かに応じて、仮想面の探索が終了したか否かが判定される（図３／ＳＴＥＰ２０）。当該判定結果が否定的である場合（図３／ＳＴＥＰ２０‥ＮＯ）、前回平面パラメータ^ｍ（ｋ）（ｋは平面パラメータの更新回数を表わす指数である。）が勾配法にしたがって更新された結果としての今回平面パラメータ^ｍ（ｋ＋１）が設定される（図３／ＳＴＥＰ０８）。その上で前記した一連の処理が繰り返される（図３／ＳＴＥＰ１０〜ＳＴＥＰ２０）。

その一方、当該判定結果が肯定的である場合（図３／ＳＴＥＰ２０‥ＹＥＳ）、第３演算処理要素２３が、その時点での平面パラメータ^ｍによって定義される仮想面の位置及び姿勢を、足平裏面又は当該足裏面と床面との接合面の位置及び姿勢として推定する（図３／ＳＴＥＰ２２）。

これにより、例えば、図１０（ａ）〜（ｄ）のそれぞれに示されているように足平裏面が床面に接触している状態が、未来における足平裏面と床面との接触状態として推定される。図１１（ａ）〜（ｄ）には、図１０（ａ）〜（ｄ）に示されている接触状態（接触箇所に格子線が付されている）が表現されている距離画像が示されている。

（作用効果）
本発明の接触状態推定装置によれば、第１偏差ｅ₁（ｓ）が実点Ｐｃと仮想点Ｐｓとの距離残差｜Ｚｃ（ｓ）−Ｚｓ（ｓ）｜の大小に応じて大小が定まるように定義されている（図５参照）。第１偏差ｅ₁は、基準画像センサ１１を基準として仮想点Ｐｃが実点Ｐｓより遠くに位置する場合は正値である一方、これとは逆に仮想点Ｐｃが実点Ｐｓより近くに位置する場合は負値である（図８参照）。

第２偏差ｅ₂が、基準画像のピクセルｓと、仮設定された仮想面の位置及び姿勢に応じた形で当該基準画像のピクセルｓに対応する、参照画像のピクセルとの輝度残差（指定物理量の偏差）の大小に応じて大小が定まるように定義されている（図９（ａ）参照）。

第１コスト関数（第１コスト関数）Ｅ₁（ｅ₁）が、主変数ｅ₁の値が０である場合に最小値又は極小値を示し、かつ、正の定義域において増加関数となる従変数として定義されている（関係式（１１０）及び図７（ａ）〜（ｃ）参照）。第２コスト関数（第２コスト関数）Ｅ₂（ｅ₁，ｅ₂）が、主変数ｅ₂の値が０である場合に最小値又は極小値を示し、かつ、正の定義域において増加関数となる関数として定義されている（関係式（２１０）及び図７（ａ）〜（ｃ）参照）。

第１コストＥ₁（ｅ₁）は、基準画像（一方の画像）の対象領域（２次距離画像）に含まれる各ピクセルｓにおいて第１係数関数ｗ₁（ｓ）の値をバネ係数とする仮想的なバネ群の、第１偏差ｅ₁（ｓ）を変形量とする弾性エネルギーの合計に相当する（図８参照）。同様に、第２コストＥ₂（ｅ₂）は、基準画像の対象領域に含まれる各ピクセルｓにおいて第２係数関数ｗ₂（ｓ）の値をバネ係数とする仮想的なバネ群の、第２偏差ｅ₂（又は第２偏差ｅ₂に基づいて前記拘束条件にしたがって求められる距離残差ΔＺ）を変形量とする弾性エネルギーの合計に相当する（同）。

仮想物体の少なくとも一部が実物体の内部に入り込んだ状態が設定された場合、基準画像の対象領域の一部又は全部のピクセルｓにおける第１偏差ｅ₁（ｓ）が正値となり、第２偏差ｅ₂（ｓ）は０ではなく正又は負値となる。このため、当該設定によりある仮想点が実物体の表面から離れて内部の深い箇所に置かれるほど、第１コストＥ₁及び第２コストＥ₂が高く評価され、統合コストＥも高く評価される。

そして、統合コストＥが最小値又は極小値に近づけられるように仮想面の位置及び姿勢が探索されることにより、仮想面上の仮想点群の全部が実物体の内部にはなく、かつ、当該仮想点群の少なくとも一部が実物体表面に位置するような仮想面の位置及び姿勢が、当該仮想面の実物体の表面に対する接触状態として推定されうる（図１０及び図１１参照）。

これにより、撮像対象となった実物体（床）と、仮想物体に相当する他の実物体（足平）とのそれぞれにより共通の実空間が同時に占められているという実現不可能な状態が両物体の接触状態として探索される事態が確実に回避される（図８参照）。そして、一の実物体表面（床面）に対する他実物体表面（足平裏面）の未来における接触状態が高精度で推定されうる。

第１係数関数ｗ₁（ｅ₁）が、主変数ｅ₁の絶対値が同一であっても当該主変数ｅ₁が正値である場合の値が、当該主変数ｅ₁が負値である場合よりも大きなくなるように定義されている（関係式（１０１）〜（１０３）及び図６（ａ）〜（ｃ）参照）。第２係数関数ｗ₂（ｅ₁）が、主変数ｅ₁の絶対値が同一であっても当該主変数ｅ₁が正値である場合の値が、当該主変数ｅ₁が負値である場合よりも大きなくなるように定義されている。

これにより、正負の定義域で非対称的な第１コスト関数Ｅ₁（ｅ₁）にしたがって第１コストＥ₁が計算され、かつ、正負の定義域で非対称的な第１コスト関数Ｅ₁（ｅ₁）にしたがって第２コストＥ₂が計算される（図７（ａ）〜（ｃ）参照）。前記のように、第１係数関数ｗ₁（ｅ₁）及び第２係数関数ｗ₂（ｅ₁）の値は仮想的なバネのバネ係数に相当する。

このため、実物体の内部にある仮想点を実物体の表面に近づけることが、実物体の外部にある仮想点を実物体の表面に近づけることよりも優先された形で仮想面の位置及び姿勢が探索されうる。これにより、撮像対象となった一の実物体及び仮想物体に相当する他の物体のそれぞれにより共通の実空間が同時に占められているという実現不可能な状態が両物体の接触状態として探索される事態が確実に回避される。そして、当該一の実物体表面に対する当該他の実物体表面の未来における接触状態が高精度で推定されうる。

第１係数関数ｗ₁（ｅ₁）及び第２係数関数ｗ₂（ｅ₁）のうち一方又は両方が少なくとも正の定義域において増加関数として定義されている。これにより、前記仮想的なバネのバネ係数が、当該バネの変形量が大きくなるほど大きい値になる。このため、仮想面の位置及び姿勢の設定により、ある仮想点が実物体の表面から離れて内部の深い箇所に置かれるほど、当該仮想的なバネの弾性エネルギーに相当する第１コストＥ₁及び第２コストＥ₂の両方がさらに高く評価され、統合コストＥも高く評価される。

このため、実物体の内部にある仮想点を実物体の表面により迅速又は強力に近づけるように仮想面の位置及び姿勢が探索されうる。これにより、一の実物体に対する他の実物体の未来における接触状態が高精度かつ高速で推定されうる。

第１係数関数ｗ₁（ｅ₁）及び第２係数関数ｗ₂（ｅ₁）のうち一方又は両方が負の定義域又は負の指定値未満の定義域において０である関数として定義されうる（関係式（１０２）（１０３）及び図６（ｂ）（ｃ）参照）。この場合、実物体の外部にある仮想点又は実物体の表面からある程度離れている仮想点のコストが一律に「０」に評価される（図７（ｂ）（ｃ）参照）。これにより、統合コストＥを最小値又は極小値に近づけるような仮想点の座標値の組み合わせの探索処理に要する計算負荷が軽減される。

（本発明の他の実施形態）
図１２に示されているように、ロボットが歩行経路に下降段差部分が存在する場合、距離画像センサ１０、基準画像センサ１１及び参照画像センサ１２のそれぞれにとって、当該段差部分により下段部分の一部が死角となるオクルージョンが発生する場合がある。このようなオクルージョンの影響を軽減又は解消するため、３次元マスク及び必要に応じて外挿補間処理が利用される。

具体的には、図１３に示されている１次距離画像座標系の一部であるＲＯＩの中心又は重心等の代表点の画素値Ｄｓを与える実点または対応する仮想点が基準点として定義される（図５参照）。

そして、仮想面の中心又は重心等の代表点の位置が基準点の位置と一致するように仮想面が設定され、当該仮想面を包含する立体がマスクとして定義される。図１４（ａ）に示されているように基準点を中心とし、実空間における足平裏面に相当する略矩形状の輪郭（破線参照）を有する仮想面を包含する球状のマスクが定義される。マスクの形状は、図１４（ｂ）に示されているように略円柱状又は円盤状であってもよく、図１４（ｃ）に示されているように略矩形柱状又は矩形板状であってもよい。

図１２に示されているように、ＲＯＩのうち距離計測値（画素値）Ｄｓが存在する複数の実点（●参照）のそれぞれのうち、実空間においてマスクに包含されているもののみが選択される。図１３には、ＲＯＩのうち、距離計測値が存在し、かつ、マスクに包含される実点に相当する領域に斜線が付されている。

そして、外挿補間法にしたがって、マスクに包含されている他の領域における実点（○参照）が補完される。その結果、基準点が下段部分に存在し、当該基準点の近傍に段差由来のオクルージョンが発生していても、足平裏面と床面との接触状態が高精度で推定されうる（図１５（ａ）及び図１６（ａ）参照）。別の言い方をすると、外挿補間処理の実行要否に応じて、オクルージョンの発生有無が判定されうる。基準点が上段部分に存在する場合も、前記実施形態と同様に足平裏面と床面との接触状態が高精度で推定される（図１５（ｂ）（ｃ）及び図１６（ｂ）（ｃ）参照）。また、画素値Ｄｓを有する複数の実点のうち、マスクに包含されているもののみが選択されるため、計測誤差をの大きい画素値Ｄｓを有する実点を除くことができ、足平裏面と床面との接触状態が高精度で推定される。

関係式（１０２）により定義されている第１係数関数ｗ₁（ｅ₁）（図６（ｂ）参照）は、負の指定値−ε₂（ｃ₂＞０）以下の定義域において、０ではなく正の指定値であってもよい。この場合、実物体の外部にあって実物体の表面からある程度離れている仮想点のコストＥが一律に「正の指定値」に評価される。これにより、仮想物体の表面が一の実物体の表面から離れている状態でも、統合コストＥを最小値又は極小値に近づけるような仮想面の探索処理を促進させることができる。その結果、当該一の実物体と当該他の実物体との、未来における接触状態が高精度かつ迅速に推定されうる。

さらに、足平裏面の基準位置及び基準姿勢と、平面パラメータ又は曲面パラメータとの複数の組み合わせが仮定され、当該組み合わせのそれぞれに応じて、実物体と仮想物体に相当する他の実物体との、未来における複数の接触状態が推定されてもよい。

１０‥距離画像センサ、１１‥基準画像センサ（一方の画像センサ）、１２‥参照画像センサ（他方の画像センサ）、２０‥コンピュータ、２１‥第１演算処理要素、２２‥第２演算処理要素、２３‥第３演算処理要素。

Claims (8)

1. 実物体を撮像することにより指定物理量を画素値として有する複数のピクセルによって構成される一対の画像のそれぞれを取得するように構成されている一対の画像センサと、
   前記一対の画像センサのうち一方の画像センサにより取得された一方の画像の対象領域を構成する複数のピクセルのそれぞれに対して割り当てられる、前記実物体までの距離を取得するように構成されている距離画像センサと、を用いて前記実物体の表面と、指定形状の仮想物体の表面である仮想面との接触状態を、前記実物体と前記仮想物体に相当する他の実物体との接触状態として推定する装置であって、
   前記対象領域において各ピクセルに割り当てられる第１偏差を主変数とする第１コスト関数の合計を第１コストとして算定するように構成されている第１演算処理要素と、
   前記対象領域において各ピクセルに割り当てられる第２偏差又は前記第１及び第２偏差の両方を主変数とする第２コスト関数の合計を第２コストとして算定するように構成されている第２演算処理要素と、
   前記第１コストと前記第２コストとの統合コストを最小値又は極小値に近づけるように前記実物体の表面に接触する際の前記仮想面の位置及び姿勢を探索するように構成されている第３演算処理要素とを備え、
   前記第１偏差が、前記距離画像センサにより取得された前記実物体までの距離により位置が定まる実点と、当該実点を前記一方の画像センサの視線方向について前記仮想面に投影した結果としての仮想点との間隔の長短に応じて絶対値の大小が定まり、かつ、前記一方の画像センサを基準として前記仮想点が前記実点より遠くに位置する場合は正値である一方、前記一方の画像センサを基準として前記仮想点が前記実点より近くに位置する場合は負値である変数として定義され、
   前記第２偏差が、前記一方の画像のピクセルが有する前記指定物理量と、前記仮想面の位置及び姿勢に応じた形で当該一方の画像のピクセルに対応する、他方の画像センサにより取得された他方の画像のピクセルが有する前記指定物理量との偏差の大小に応じて大小が定まる変数として定義され、
   前記第１及び第２コスト関数のそれぞれが、主変数の値が０である場合に最小値又は極小値を示し、かつ、正の定義域において増加関数となる関数として定義されていることを特徴とする接触状態推定装置。
2. 請求項１記載の接触状態推定装置において、
   前記第１コスト関数が、前記第１偏差の絶対値の正の冪乗と、前記第１偏差を主変数とし、かつ、値域が０以上である関数であって、絶対値が同一であっても正の定義域における値が負の定義域における値よりも大きい第１係数関数との積により定義され、
   前記第２コスト関数が、前記第２偏差の絶対値の正の冪乗と、前記第１偏差又は前記第２偏差を主変数とし、かつ、値域が０以上である関数であって、絶対値が同一であっても正の定義域における値が負の定義域における値よりも大きい第２係数関数との積により定義されていることを特徴とする接触状態推定装置。
3. 請求項２記載の接触状態推定装置において、
   前記第１及び第２係数関数のうち一方又は両方が少なくとも正の定義域において増加関数として定義されていることを特徴とする接触状態推定装置。
4. 請求項３記載の接触状態推定装置において、
   前記第１及び第２係数関数のうち一方又は両方が負の定義域又は負の指定値未満の定義域において０である関数として定義されていることを特徴とする接触状態推定装置。
5. 請求項２〜４のうちいずれか１つに記載の接触状態推定装置において、
   前記第２係数関数の主変数としての前記第２偏差が、前記一対の画像センサのそれぞれにより撮像された、実空間における同一箇所の前記指定物理量は同一であるという拘束条件にしたがって、前記指定物理量の偏差が換算された結果としての前記一対の画像センサの視差残差又は当該視差残差に相当する距離残差として定義されていることを特徴とする接触状態推定装置。
6. 請求項１〜５のうちいずれか１つに記載の接触状態推定装置において、
   前記距離画像センサが、前記実物体までの距離を画素値として有する複数のピクセルによって構成される距離画像を取得するように構成され、
   前記第１演算処理要素が、前記一方の画像センサ及び前記距離画像センサの相対的な配置関係に応じて、前記距離画像を構成する各ピクセルの画素値に応じた距離が、前記一方の画像の前記対象領域を構成する複数のピクセルに対して画素値として割り当てるように構成されていることを特徴とする接触状態推定装置。
7. 請求項３記載の接触状態推定装置において、
   前記第１及び第２係数関数のうち一方又は両方が負の定義域又は負の指定値未満の定義域において正の指定値である関数として定義されていることを特徴とする接触状態推定装置。
8. 請求項１〜７のうちいずれか１つに記載の接触状態推定装置において、
   前記第１演算処理要素は、前記対象領域を前記仮想物体の表面に投影した領域に基準点を定め、該基準点または該基準点に対応した実点から所定の範囲にある実点を選択し、前記第１偏差を定めることを特徴とする接触状態推定装置。
   

Images