(57)【特許請求の範囲】
【請求項１】
実物体を撮像することにより指定物理量を画素値として有する複数のピクセルによって構成される一対の画像のそれぞれを取得するように構成されている一対の画像センサと、
前記一対の画像センサのうち一方の画像センサにより取得された一方の画像の対象領域を構成する複数のピクセルのそれぞれに対して割り当てられる、前記実物体までの距離を取得するように構成されている距離画像センサと、を用いて前記実物体の表面と、指定形状の仮想物体の表面である仮想面との接触状態を、前記実物体と前記仮想物体に相当する他の実物体との接触状態として推定する装置であって、
前記対象領域において各ピクセルに割り当てられる第１偏差を主変数とする第１コスト関数の合計を第１コストとして算定するように構成されている第１演算処理要素と、
前記対象領域において各ピクセルに割り当てられる第２偏差又は前記第１及び第２偏差の両方を主変数とする第２コスト関数の合計を第２コストとして算定するように構成されている第２演算処理要素と、
前記第１コストと前記第２コストとの統合コストを最小値又は極小値に近づけるように前記実物体の表面に接触する際の前記仮想面の位置及び姿勢を探索するように構成されている第３演算処理要素とを備え、
前記第１偏差が、前記距離画像センサにより取得された前記実物体までの距離により位置が定まる実点と、当該実点を前記一方の画像センサの視線方向について前記仮想面に投影した結果としての仮想点との間隔の長短に応じて絶対値の大小が定まり、かつ、前記一方の画像センサを基準として前記仮想点が前記実点より遠くに位置する場合は正値である一方、前記一方の画像センサを基準として前記仮想点が前記実点より近くに位置する場合は負値である変数として定義され、
前記第２偏差が、前記一方の画像のピクセルが有する前記指定物理量と、前記仮想面の位置及び姿勢に応じた形で当該一方の画像のピクセルに対応する、他方の画像センサにより取得された他方の画像のピクセルが有する前記指定物理量との偏差の大小に応じて大小が定まる変数として定義され、
前記第１及び第２コスト関数のそれぞれが、主変数の値が０である場合に最小値又は極小値を示し、かつ、正の定義域において増加関数となる関数として定義されていることを特徴とする接触状態推定装置。
【請求項２】
請求項１記載の接触状態推定装置において、
前記第１コスト関数が、前記第１偏差の絶対値の正の冪乗と、前記第１偏差を主変数とし、かつ、値域が０以上である関数であって、絶対値が同一であっても正の定義域における値が負の定義域における値よりも大きい第１係数関数との積により定義され、
前記第２コスト関数が、前記第２偏差の絶対値の正の冪乗と、前記第１偏差又は前記第２偏差を主変数とし、かつ、値域が０以上である関数であって、絶対値が同一であっても正の定義域における値が負の定義域における値よりも大きい第２係数関数との積により定義されていることを特徴とする接触状態推定装置。
【請求項３】
請求項２記載の接触状態推定装置において、
前記第１及び第２係数関数のうち一方又は両方が少なくとも正の定義域において増加関数として定義されていることを特徴とする接触状態推定装置。
【請求項４】
請求項３記載の接触状態推定装置において、
前記第１及び第２係数関数のうち一方又は両方が負の定義域又は負の指定値未満の定義域において０である関数として定義されていることを特徴とする接触状態推定装置。
【請求項５】
請求項２〜４のうちいずれか１つに記載の接触状態推定装置において、
前記第２係数関数の主変数としての前記第２偏差が、前記一対の画像センサのそれぞれにより撮像された、実空間における同一箇所の前記指定物理量は同一であるという拘束条件にしたがって、前記指定物理量の偏差が換算された結果としての前記一対の画像センサの視差残差又は当該視差残差に相当する距離残差として定義されていることを特徴とする接触状態推定装置。
【請求項６】
請求項１〜５のうちいずれか１つに記載の接触状態推定装置において、
前記距離画像センサが、前記実物体までの距離を画素値として有する複数のピクセルによって構成される距離画像を取得するように構成され、
前記第１演算処理要素が、前記一方の画像センサ及び前記距離画像センサの相対的な配置関係に応じて、前記距離画像を構成する各ピクセルの画素値に応じた距離が、前記一方の画像の前記対象領域を構成する複数のピクセルに対して画素値として割り当てるように構成されていることを特徴とする接触状態推定装置。
【請求項７】
請求項３記載の接触状態推定装置において、
前記第１及び第２係数関数のうち一方又は両方が負の定義域又は負の指定値未満の定義域において正の指定値である関数として定義されていることを特徴とする接触状態推定装置。
【請求項８】
請求項１〜７のうちいずれか１つに記載の接触状態推定装置において、
前記第１演算処理要素は、前記対象領域を前記仮想物体の表面に投影した領域に基準点を定め、該基準点または該基準点に対応した実点から所定の範囲にある実点を選択し、前記第１偏差を定めることを特徴とする接触状態推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、距離画像センサ及び２つ以上の画像センサを用いて被写体までの距離等を推定する装置に関する。
【背景技術】
【０００２】
距離画像センサを通じて取得された距離画像と、ステレオカメラのそれぞれを通じて取得された距離画像とを融合することにより、３次元画像を取得する手法が提案されている（特許文献１参照）。
【０００３】
複数種類の既知形状のパッチのうちいずれか１種のパッチによって床面の複数の散在する局所領域のそれぞれを近似する（パッチマップを作成する）とともに、パッチマップを集め、脚式移動ロボット用空間マップを生成する手法が提案されている（非特許文献１参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平０９−００５０５０号公報
【非特許文献】
【０００５】
【非特許文献１】“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は関係式（００１）によって定義される。
【００５２】
^qi=t(xi, yi, 1) ..(001 )。
【００５３】
ここで「t」は転置を表わす。（ｘi，ｙi）は２次距離画像座標系において距離計測値ｚiを画素値として有するピクセル位置を表わす。基準位置群Ｑ＝（^ｑ1，‥^ｑi，‥^ｑn）によって画定される範囲の面積及び形状は、記憶装置に保存されている足平裏面の面積及び形状に加えて、距離に応じて一義的に定められる。
【００５４】
一方、足平裏面の基準位置及び基準姿勢は任意に変更されうる。例えば、基準位置^ｑiのｘ成分に所定値が一律に加算又は減算されることにより、基準座標系における足平裏面の基準位置がｘ方向に当該所定値だけ変更されうる。また、基準位置^ｑiのｘ成分及びｙ成分のそれぞれに、ｚ軸回りの所定角度θの回転を表わす因子ｃｏｓθ又はｓｉｎθが一律に乗算されることにより、基準座標系における足平裏面のｘｙ平面における基準姿勢が変更されうる。
【００５５】
さらに、足平裏面の基準座標系における位置及び姿勢を表わす平面パラメータ^ｍ（ｓ）＝t（ｍ1，ｍ2，ｍ3）が設定される。平面パラメータ^ｍ（ｓ）の初期値は任意であってもよいが、後述するように平面パラメータ^ｍ（ｓ）は前回値が修正されることにより今回値が設定される。
【００５６】
２次距離画像座標系（２次元）に対して直交する方向をｚ方向とする３次元直交座標系が基準座標系として採用される（図５参照）。基準画像センサ１１の光学中心と物体表面との距離Ｄｓが取得されているので（図３／ＳＴＥＰ０２及び図５参照）、距離画像において光軸に対して角度ψをなす位置にある画素の画素値Ｚｓ（ｚ成分）はＤｓｃｏｓψと定義される。このため、距離画像センサ１０がキャリブレーションされている場合、当該定義にしたがってｚ成分Ｚｓ（ｓ）が計算されうる。
【００５７】
基準座標系としては、ロボット座標系又は世界座標系が採用されてもよい。例えば、距離画像センサ１０がロボットに搭載されている場合、距離画像座標系のロボット座標系に対する位置及び姿勢は、ロボットの挙動を表わす順キネマティクスモデルにしたがって算出された上で記憶装置に保存される。距離画像座標系のロボット座標系に対する位置及び姿勢は、並進行列及び回転行列又はこれに等価のクォータニオンによって定義される。
【００５８】
ロボット座標系は、ロボットの質量中心（たとえば基体に含まれている。）が原点として定義され、ロボットの上方が＋ｘ方向として定義され、右方向が＋ｙ方向として定義され、前方が＋ｚ方向として定義されている（図２参照）。ロボット座標系の世界座標系における位置及び姿勢は、たとえば、記憶装置に保存されているロボットの行動計画により定義される。
【００５９】
２次距離画像座標系を基準とした３次元直交座標系における座標値が、記憶装置に保存されている当該行列を用いて座標変換されることで、ロボット座標系における座標値が算定されうる。２次距離画像座標系を基準とした３次元直交座標系における座標値が、記憶装置に保存されている当該行列及び行動計画を用いて座標変換されることで、世界座標系における座標値が算定されうる。
【００６０】
平面パラメータ^ｍは、足平裏面の形状Ｑと、実点座標値群Ｚ＝t（ｚ1，‥，ｚi，‥ｚn）と、ｎ次単位行列Ｉとに基づき、関係式（００２）にしたがって定義される。
【００６１】
m=(QItQ)-1QIZ ..(002)。
【００６２】
そして、関係式（００３）にしたがって算定されるベクトル^Ｚの各成分が、第ｉ仮想点座標値Ｐｃ（ｓ）（正確にはそのｚ成分Ｚｃ（ｓ））として仮定される（図５参照）。
【００６３】
^Z=tQm ..(003)。
【００６４】
仮想物体の表面形状は、本発明の装置のユーザによって平面のほか任意形状の曲面に変更されうる。例えば、曲面パラメータｍ＝t（ｍ11，ｍ12，ｍ21，ｍ22，ｍ3）が設定されることにより、関係式（０２２）によって表現される曲面を表面として有するように仮想物体の形状が設定されてもよい。
【００６５】
m11x2+m12x+ my21y2+m22y+ m3z=α ..(022)。
【００６６】
この場合、例えば、曲面パラメータ又は関係式（０２２）によって表面形状が表現される手の平と、手すりなどの物体との接触状態が推定されうる。
【００６７】
第１演算処理要素２１が、実点Ｐｓ（ｓ）と仮想点Ｐｃ（ｓ）との間隔｜Ｄｓ（ｓ）−Ｄｃ（ｓ）｜の広狭に応じて定まる第１偏差ｅ1（ｓ）に基づき、第１係数関数ｗ1（ｅ1）にしたがって第１係数ｗ1（ｓ）を算定する（図３／ＳＴＥＰ１２）。
【００６８】
例えば、第１偏差ｅ1（ｓ）として、実点距離Ｚｓ（ｓ）と仮想点距離Ｚｃ（ｓ）との距離残差｜Ｚｓ（ｓ）−Ｚｃ（ｓ）｜が採用される（図５参照）。
【００６９】
「実点」とは、２次距離画像座標系（基準画像座標系のうち一部である対象領域に相当する。）におけるピクセル位置ｓ＝（ｕ，ｖ）の画素値Ｄｓ（ｓ）に応じて実空間位置が定まる点Ｐｓ（ｓ）＝Zs^ｓを意味する（図５参照）。＾ｓ＝（ｕ，ｖ，１）はｓの位置を表わすベクトルである。また「仮想点」とは、基準画像センサ１１の視線方向について実点Ｐｓ（ｓ）を仮想面に投影した点を意味する（図５参照）。
【００７０】
距離残差｜Ｄｓ（ｓ）−Ｄｃ（ｓ）｜の大小に応じて幾何学的関係から一義的に定まるさまざまな距離が、第１偏差ｅ1（ｓ）として採用されてもよい。例えば、当該間隔｜Ｄｓ（ｓ）−Ｄｃ（ｓ）｜そのもののほか、実点Ｐｓ（ｓ）がＺ方向について仮想面に投影された結果としての点と、仮想点Ｐｃ（ｓ）との指定方向についての間隔が第１偏差ｅ1（ｓ）として採用されてもよい。また、仮想点Ｐｃ（ｓ）がＺ方向について被写体に相当する平面に投影された結果としての点と、実点Ｐｓ（ｓ）との指定方向についての間隔が第１偏差ｅ1（ｓ）として採用されてもよい。
【００７１】
第１係数ｗ1（ｅ1（ｓ））は、第１偏差ｅ1を主変数とし、関係式（１０１）によって定義されている第１係数関数（従変数）ｗ1（ｅ1）にしたがって算定される。この第１係数関数ｗ1（ｅ1）は、図６（ａ）に示されているように第１偏差ｅ1の増加関数である。第１係数関数ｗ1（ｅ1）は、負の定義域（ｅ1＜０）において０以上かつ正の指定値ε1未満の値域を有する一方、０以上の定義域（ｅ1≧０）において正の指定値ε1以上の値域を有する。
【００７２】
w1(e1)=log(1+exp(αe1-β)), (α>0, β>0) ..(101)。
【００７３】
第１係数ｗ1（ｅ1（ｓ））が関係式（１０２）によって定義されている第１係数関数ｗ1（ｅ1）にしたがって算定されてもよい。この第１係数関数ｗ1（ｅ1）は、図６（ｂ）に示されているように、負の定義域において０以上かつ正の指定値ε1未満の値域を有する一方、０以上の定義域において正の指定値ε1以上の値域を有する。第１係数関数ｗ1（ｅ1）は、負の指定値−ε2（ε2＞０）以下の定義域において０である一方、負の指定値−ε2を超えている定義域において増加関数である。
【００７４】
w1(e1)=0 (if e1≦-ε2), or w1(e1)=(ε1/ε2)e1+ε1(if -ε2<e1) ..(102)。
【００７５】
第１係数ｗ1（ｅ1（ｓ））が関係式（１０３）によって定義されている第１係数関数ｗ1（ｅ1）にしたがって算定されてもよい。この第１係数関数ｗ1（ｅ1）は、図６（ｃ）に示されているように、０未満の定義域において０である一方、０以上の定義域において増加関数である。
【００７６】
w1(e1)=0 (if e1≦0), or w1(e1)=ε0e1, (0<ε0) (if 0< e1) ..(103)。
【００７７】
第１演算処理要素２１が、第１係数ｗ1（ｅ1（ｓ））及び第１偏差ｅ1（ｓ）に基づき、第１コスト関数Ｅ1（ｅ1）（本発明の「第１コスト関数」に相当する。）にしたがって第１コストＥ1を算定する（図３／ＳＴＥＰ１２）。
【００７８】
第１コスト関数Ｅ1（ｅ1）は、例えば関係式（１１０）に示されているように、第１係数関数ｗ1（ｅ1）と、第１偏差の絶対値｜ｅ1｜の二乗との積として定義されている。このため、第１コストＥ1は、第１係数ｗ1（ｓ）をバネ係数とし、仮想点を実点に近づけるような、図８に示されている仮想的なバネ群の弾性エネルギーの合計に相当する。当該冪乗数は「２」に限られず、「０．５」「１．５」「３」など、任意の正値に設定されていてもよい。
【００７９】
E1(e1)＝w1(e1)|e1|2 ..(110)。
【００８０】
第１偏差ｅ1に基づき、第１係数関数ｗ1（ｅ1）にしたがって第１係数ｗ1が算定され（図３／ＳＴＥＰ１０参照）、これとは別個に第１偏差の絶対値｜ｅ1｜の冪乗（２乗）の値が算定され、その上で当該算定結果が掛け合わせられることにより第１コストＥ1が算定される（図３／ＳＴＥＰ１２参照）。これに代えて、第１偏差ｅ1に基づき、第１コスト関数Ｅ1（ｅ1）にしたがって第１コストＥ1が算定されてもよい。
【００８１】
第１コストＥ1は、基準画像座標系における対象領域（２次距離画像座標系）に属するピクセルｓについて、第１コスト関数Ｅ1（ｅ1）の値の合計を表わす関係式（１２０）にしたがって算定される。「Σｓ」は、基準画像座標系における対象領域（２次距離画像座標系）に属するピクセルｓについての和を意味する。
【００８２】
E1＝ΣsE1(e1(s)) ..(120)。
【００８３】
第１偏差ｅ1（ｓ）は仮想面の平面パラメータ^ｍ（ｓ）を変数とする関数であるため、第１偏差ｅ1（ｓ）に基づいて算出される第１コストＥ1も仮想面の平面パラメータ^ｍ（ｓ）を変数とする関数となる。
【００８４】
第１係数関数ｗ1（ｅ1）が関係式（１０１）にしたがって定義されている場合（図６（ａ）参照）、第１コスト関数Ｅ1（ｅ1）は、図７（ａ）に示されているようにｅ1＝０で極小値０を有し、かつ、正の定義域（ｅ1＞０）において増加関数となる。また、第１コスト関数Ｅ1（ｅ1）は、正負の定義域において非対称的な変化特性を示す。すなわち、正の変数値ｅ1＝ｃ（＞０）に基づく第１コスト関数の値Ｅ1（ｃ）が、これと絶対値が等しい負の変数値−ｃに基づく第１コスト関数の値Ｅ1（−ｃ）よりも大きくなる。
【００８５】
第１係数関数ｗ1（ｅ1）が関係式（１０２）にしたがって定義されている場合（図６（ｂ）参照）、第１コスト関数Ｅ1（ｅ1）は、図７（ｂ）に示されているようにｅ1＝０で極小値０を有し、かつ、正の定義域において増加関数となる。また、第１コスト関数Ｅ1（ｅ1）は、正負の定義域−ε2＜ｅ1＜ε2において対称的な変化特性を示す。第１コスト関数Ｅ1（ｅ1）は、負の指定値以下の負の定義域ｅ1≦−ε2においては０となる。
【００８６】
第１係数関数ｗ1（ｅ1）が関係式（１０３）にしたがって定義されている場合（図６（ｃ）参照）、第１コスト関数Ｅ1（ｅ1）は、図７（ｃ）に示されているように負の定義域において０となり、正の定義域において増加関数となる。
【００８７】
第２演算処理要素２２が、基準画像センサ１１及び参照画像センサ１２のそれぞれが、同一の仮想面を撮像しているという仮定下で、参照画像を基準画像座標系に座標変換することにより変換画像を生成する（図３／ＳＴＥＰ１４）。具体的には、基準画像座標系のピクセル位置ｓに対して、参照画像座標系のうち当該仮想面の位置及び姿勢に応じた視差が存在する場合におけるピクセル位置ｓrefの輝度が割り当てられた画像が変換画像として取得される。
【００８８】
第２演算処理要素２２が、第２係数ｗ2（ｅ1（ｓ））及び第２偏差ｅ2（ｓ）に基づき、第２コスト関数Ｅ2（ｅ1，ｅ2）にしたがって第２コストＥ2を算定する（図３／ＳＴＥＰ１６）。基準画像座標系及び変換画像座標系の同一ピクセル位置ｓにおける画素値である指定物理量の偏差、例えば輝度残差ΔＩ（ｓ）が第２偏差ｅ2（ｓ）として採用される。可視光カメラを通じて取得される色（ＲＧＢ値）、赤外光カメラを通じて取得される温度など、可視光カメラを通じて取得される輝度とは異なる物理量が指定物理量として採用されてもよい。
【００８９】
第２コスト関数Ｅ2（ｅ1，ｅ2）は、例えば関係式（２１０）に示されているように、第２係数関数ｗ2（ｅ1）と、第２偏差の絶対値｜ｅ2｜の二乗との積として定義されている。このため、第２コストＥ2は、第２係数ｗ2（ｅ1）をバネ係数とし、「同一時刻において基準画像センサ１１及び参照画像センサ１２のそれぞれにより撮像された同一箇所の指定物理量（ここでは輝度）が同一である」という拘束条件にしたがって仮想点を実点に近づけるような、仮想的なバネの弾性エネルギーに相当する（図８参照）。すなわち、第２コストＥ2は、当該仮想的なバネ群の弾性エネルギーの合計に相当する。当該冪乗数は「２」に限られず、「０．５」「１．５」「３」など、任意の正値に設定されていてもよい。
【００９０】
E2(e1, e2)＝w 2(e1)|e2|2 ..(210)。
【００９１】
第１偏差ｅ1に基づき、第２係数関数ｗ2（ｅ1）にしたがって第２係数ｗ2が算定され（図３／ＳＴＥＰ１０参照）、これとは別個に第２偏差の絶対値｜ｅ2｜の冪乗（２乗）の値が算定され、その上で当該算定結果が掛け合わせられることにより第１コストＥ1が算定される（図３／ＳＴＥＰ１２参照）。これに代えて、第１偏差ｅ1及び第２偏差ｅ2に基づき、第２コスト関数Ｅ2（ｅ1，ｅ2）にしたがって第２コストＥ2が算定されてもよい。
【００９２】
第２コストＥ2は、基準画像座標系における対象領域（２次距離画像座標系）に属するピクセルｓについて、第２コスト関数Ｅ2（ｅ1，ｅ2）の値の合計を表わす関係式（２２０）にしたがって算定される
E2＝ΣsE2(e1(s), e2(s)) ..(220)。
【００９３】
第１偏差ｅ1（ｓ）と同様に第２偏差ｅ2（ｓ）は仮想面の平面パラメータ＾ｍ（ｓ）を変数とする関数であるため、第１偏差ｅ1（ｓ）及び第２偏差ｅ2（ｓ）に基づいて算出される第２コストＥ2も仮想面の平面パラメータ＾ｍ（ｓ）を変数とする関数となる。
【００９４】
第２係数関数ｗ2（ｅ1）は、第１係数関数ｗ1（ｅ1）と同一であってもよく、異なっていてもよい。例えば、第１係数関数ｗ1（ｅ1）及び第２係数関数ｗ2（ｅ1）がともに関係式（１０１）にしたがって定義されていてもよい。第１係数関数ｗ1（ｅ1）が関係式（１０１）にしたがって定義される一方、第２係数関数ｗ2（ｅ1）が関係式（１０２）にしたがって定義されていてもよい。第２係数関数ｗ2の主変数として第１偏差ｅ1に代えて第２偏差ｅ2が用いられてもよい。
【００９５】
輝度残差ΔＩ（ｓ）は前記拘束条件にしたがって求められる。簡単のため、基準画像センサ１１及び参照画像センサ１２が、等しい内部パラメータを持ち互いの光軸が平行になるよう配置された平行ステレオの場合について説明する。
【００９６】
基準画像センサ１１及び参照画像センサ１２が平行ステレオの関係にない場合、図９（ａ）に示されているように視差残差Δｕは、参照画像センサ１２に距離計測値Ｄｓ（ｓ）及び距離候補値Ｄｃ（ｓ）のそれぞれを投影した結果としてのピクセル位置の間隔となる。視差残差Δｕは、実点距離Ｚｓと、仮想点距離Ｚｃと、両画像センサ１１及びＣ２の基線長Ｌとに基づき、関係式（２０１）にしたがって定義される（図５参照）。
【００９７】
Δu=L{(1/Zs)-(1/Zc)} ..(201)。
【００９８】
「ｕ」は基準画像座標系（又は参照画像座標系）における横方向の位置を表わす座標値である。平行ステレオの場合、ｕ軸の方向はエピポーラ線と平行になる。基準画像と、視差残差Δｕが存在するという仮定下での参照画像との輝度残差ΔＩは、エピポーラ拘束条件を考慮に入れて、関係式（２０２）により表現される（図９（ｂ）参照）。
【００９９】
ΔI=(∂I/∂u)^eep^Δu ..(202)。
【０１００】
「^ｅep」は基準画像座標系におけるエピポーラ線の方向を表わす単位ベクトルである。「(∂I/∂u)」は輝度勾配を表すベクトルである。特に平行ステレオの場合、ｕ軸方向のみの輝度勾配が有効となる。
【０１０１】
関係式（２０２）は、前記拘束条件にしたがって視差残差Δｕ（ｓ）が輝度残差ΔＩ（ｓ）に変換されることを表わしている。視差残差Δｕ（ｓ）は、距離計測値Ｄｓに応じて基準画像座標系（２次元距離座標系）のピクセルｓを参照画像座標系に対して投影した結果としての位置と、距離候補値Ｄｃに応じて基準画像座標系のピクセルｓを参照画像座標系に投影した結果としての位置との間隔である。
【０１０２】
輝度残差ΔＩ（ｓ）のほか、輝度残差ΔＩ（ｓ）が関係式（２０２）にしたがって換算された視差残差Δｕ又は輝度残差ΔＩ（ｓ）が関係式（２０２）（２０３）にしたがって換算された距離残差ΔＺ（ｓ）が第２偏差ｅ2（ｓ）として用いられてもよい。関係式（２０３）は、Δｕ＝（ｄｕ／ｄＺｃ）ΔＺ及びΔＺ＝Ｚｓ−Ｚｃという関係式にしたがって関係式（２０１）が変形されることにより得られる。
【０１０３】
Δu(s)=-(L/Zc2)ΔZ(s) ..(203)。
【０１０４】
関係式（２０２）及び（２０３）にしたがって、距離残差ΔＺ（ｓ）＝Ｚｓ（ｓ）−Ｚｃ（ｓ）が存在する場合の輝度残差ΔＩ（ｓ）が計算されうる。
【０１０５】
第３演算処理要素２３が、関係式（３０１）又は（３０２）で表わされている第１コストＥ1及び第２コストＥ2の線形和を統合コストＥとして算出する。前記のように第１コストＥ1及び第２コストＥ2はともに仮想面の平面パラメータ＾ｍを変数とする関数であるため、統合コストＥも平面パラメータ^ｍの関数Ｅ（^ｍ）として定義される。
【０１０６】
E(^m)=E1(^m)+E2(^m) ..(301)。
【０１０７】
E(^m)=χE1(^m)+(1−χ)E2(^m), (0<χ<1)..(302)。
【０１０８】
そして、第３演算処理要素２３は、最小二乗法又は仮想面の平面パラメータを（∂Ｅ（^ｍ）／∂^ｍ）に応じた分だけ逐次変動させる勾配法にしたがって、統合コストＥを最小値にする平面パラメータ^ｍを探索する（図３／ＳＴＥＰ１８）。これにより、仮想面（足平裏面）の位置及び姿勢が推定される。
【０１０９】
統合コストＥの前回値と今回値との差が閾値以下になる等、平面パラメータ^ｑが一定の収束条件を満足しているか否かに応じて、仮想面の探索が終了したか否かが判定される（図３／ＳＴＥＰ２０）。当該判定結果が否定的である場合（図３／ＳＴＥＰ２０‥ＮＯ）、前回平面パラメータ^ｍ（ｋ）（ｋは平面パラメータの更新回数を表わす指数である。）が勾配法にしたがって更新された結果としての今回平面パラメータ^ｍ（ｋ＋１）が設定される（図３／ＳＴＥＰ０８）。その上で前記した一連の処理が繰り返される（図３／ＳＴＥＰ１０〜ＳＴＥＰ２０）。
【０１１０】
その一方、当該判定結果が肯定的である場合（図３／ＳＴＥＰ２０‥ＹＥＳ）、第３演算処理要素２３が、その時点での平面パラメータ^ｍによって定義される仮想面の位置及び姿勢を、足平裏面又は当該足裏面と床面との接合面の位置及び姿勢として推定する（図３／ＳＴＥＰ２２）。
【０１１１】
これにより、例えば、図１０（ａ）〜（ｄ）のそれぞれに示されているように足平裏面が床面に接触している状態が、未来における足平裏面と床面との接触状態として推定される。図１１（ａ）〜（ｄ）には、図１０（ａ）〜（ｄ）に示されている接触状態（接触箇所に格子線が付されている）が表現されている距離画像が示されている。
【０１１２】
（作用効果）
本発明の接触状態推定装置によれば、第１偏差ｅ1（ｓ）が実点Ｐｃと仮想点Ｐｓとの距離残差｜Ｚｃ（ｓ）−Ｚｓ（ｓ）｜の大小に応じて大小が定まるように定義されている（図５参照）。第１偏差ｅ1は、基準画像センサ１１を基準として仮想点Ｐｃが実点Ｐｓより遠くに位置する場合は正値である一方、これとは逆に仮想点Ｐｃが実点Ｐｓより近くに位置する場合は負値である（図８参照）。
【０１１３】
第２偏差ｅ2が、基準画像のピクセルｓと、仮設定された仮想面の位置及び姿勢に応じた形で当該基準画像のピクセルｓに対応する、参照画像のピクセルとの輝度残差（指定物理量の偏差）の大小に応じて大小が定まるように定義されている（図９（ａ）参照）。
【０１１４】
第１コスト関数（第１コスト関数）Ｅ1（ｅ1）が、主変数ｅ1の値が０である場合に最小値又は極小値を示し、かつ、正の定義域において増加関数となる従変数として定義されている（関係式（１１０）及び図７（ａ）〜（ｃ）参照）。第２コスト関数（第２コスト関数）Ｅ2（ｅ1，ｅ2）が、主変数ｅ2の値が０である場合に最小値又は極小値を示し、かつ、正の定義域において増加関数となる関数として定義されている（関係式（２１０）及び図７（ａ）〜（ｃ）参照）。
【０１１５】
第１コストＥ1（ｅ1）は、基準画像（一方の画像）の対象領域（２次距離画像）に含まれる各ピクセルｓにおいて第１係数関数ｗ1（ｓ）の値をバネ係数とする仮想的なバネ群の、第１偏差ｅ1（ｓ）を変形量とする弾性エネルギーの合計に相当する（図８参照）。同様に、第２コストＥ2（ｅ2）は、基準画像の対象領域に含まれる各ピクセルｓにおいて第２係数関数ｗ2（ｓ）の値をバネ係数とする仮想的なバネ群の、第２偏差ｅ2（又は第２偏差ｅ2に基づいて前記拘束条件にしたがって求められる距離残差ΔＺ）を変形量とする弾性エネルギーの合計に相当する（同）。
【０１１６】
仮想物体の少なくとも一部が実物体の内部に入り込んだ状態が設定された場合、基準画像の対象領域の一部又は全部のピクセルｓにおける第１偏差ｅ1（ｓ）が正値となり、第２偏差ｅ2（ｓ）は０ではなく正又は負値となる。このため、当該設定によりある仮想点が実物体の表面から離れて内部の深い箇所に置かれるほど、第１コストＥ1及び第２コストＥ2が高く評価され、統合コストＥも高く評価される。
【０１１７】
そして、統合コストＥが最小値又は極小値に近づけられるように仮想面の位置及び姿勢が探索されることにより、仮想面上の仮想点群の全部が実物体の内部にはなく、かつ、当該仮想点群の少なくとも一部が実物体表面に位置するような仮想面の位置及び姿勢が、当該仮想面の実物体の表面に対する接触状態として推定されうる（図１０及び図１１参照）。
【０１１８】
これにより、撮像対象となった実物体（床）と、仮想物体に相当する他の実物体（足平）とのそれぞれにより共通の実空間が同時に占められているという実現不可能な状態が両物体の接触状態として探索される事態が確実に回避される（図８参照）。そして、一の実物体表面（床面）に対する他実物体表面（足平裏面）の未来における接触状態が高精度で推定されうる。
【０１１９】
第１係数関数ｗ1（ｅ1）が、主変数ｅ1の絶対値が同一であっても当該主変数ｅ1が正値である場合の値が、当該主変数ｅ1が負値である場合よりも大きなくなるように定義されている（関係式（１０１）〜（１０３）及び図６（ａ）〜（ｃ）参照）。第２係数関数ｗ2（ｅ1）が、主変数ｅ1の絶対値が同一であっても当該主変数ｅ1が正値である場合の値が、当該主変数ｅ1が負値である場合よりも大きなくなるように定義されている。
【０１２０】
これにより、正負の定義域で非対称的な第１コスト関数Ｅ1（ｅ1）にしたがって第１コストＥ1が計算され、かつ、正負の定義域で非対称的な第１コスト関数Ｅ1（ｅ1）にしたがって第２コストＥ2が計算される（図７（ａ）〜（ｃ）参照）。前記のように、第１係数関数ｗ1（ｅ1）及び第２係数関数ｗ2（ｅ1）の値は仮想的なバネのバネ係数に相当する。
【０１２１】
このため、実物体の内部にある仮想点を実物体の表面に近づけることが、実物体の外部にある仮想点を実物体の表面に近づけることよりも優先された形で仮想面の位置及び姿勢が探索されうる。これにより、撮像対象となった一の実物体及び仮想物体に相当する他の物体のそれぞれにより共通の実空間が同時に占められているという実現不可能な状態が両物体の接触状態として探索される事態が確実に回避される。そして、当該一の実物体表面に対する当該他の実物体表面の未来における接触状態が高精度で推定されうる。
【０１２２】
第１係数関数ｗ1（ｅ1）及び第２係数関数ｗ2（ｅ1）のうち一方又は両方が少なくとも正の定義域において増加関数として定義されている。これにより、前記仮想的なバネのバネ係数が、当該バネの変形量が大きくなるほど大きい値になる。このため、仮想面の位置及び姿勢の設定により、ある仮想点が実物体の表面から離れて内部の深い箇所に置かれるほど、当該仮想的なバネの弾性エネルギーに相当する第１コストＥ1及び第２コストＥ2の両方がさらに高く評価され、統合コストＥも高く評価される。
【０１２３】
このため、実物体の内部にある仮想点を実物体の表面により迅速又は強力に近づけるように仮想面の位置及び姿勢が探索されうる。これにより、一の実物体に対する他の実物体の未来における接触状態が高精度かつ高速で推定されうる。
【０１２４】
第１係数関数ｗ1（ｅ1）及び第２係数関数ｗ2（ｅ1）のうち一方又は両方が負の定義域又は負の指定値未満の定義域において０である関数として定義されうる（関係式（１０２）（１０３）及び図６（ｂ）（ｃ）参照）。この場合、実物体の外部にある仮想点又は実物体の表面からある程度離れている仮想点のコストが一律に「０」に評価される（図７（ｂ）（ｃ）参照）。これにより、統合コストＥを最小値又は極小値に近づけるような仮想点の座標値の組み合わせの探索処理に要する計算負荷が軽減される。
【０１２５】
（本発明の他の実施形態）
図１２に示されているように、ロボットが歩行経路に下降段差部分が存在する場合、距離画像センサ１０、基準画像センサ１１及び参照画像センサ１２のそれぞれにとって、当該段差部分により下段部分の一部が死角となるオクルージョンが発生する場合がある。このようなオクルージョンの影響を軽減又は解消するため、３次元マスク及び必要に応じて外挿補間処理が利用される。
【０１２６】
具体的には、図１３に示されている１次距離画像座標系の一部であるＲＯＩの中心又は重心等の代表点の画素値Ｄｓを与える実点または対応する仮想点が基準点として定義される（図５参照）。
【０１２７】
そして、仮想面の中心又は重心等の代表点の位置が基準点の位置と一致するように仮想面が設定され、当該仮想面を包含する立体がマスクとして定義される。図１４（ａ）に示されているように基準点を中心とし、実空間における足平裏面に相当する略矩形状の輪郭（破線参照）を有する仮想面を包含する球状のマスクが定義される。マスクの形状は、図１４（ｂ）に示されているように略円柱状又は円盤状であってもよく、図１４（ｃ）に示されているように略矩形柱状又は矩形板状であってもよい。
【０１２８】
図１２に示されているように、ＲＯＩのうち距離計測値（画素値）Ｄｓが存在する複数の実点（●参照）のそれぞれのうち、実空間においてマスクに包含されているもののみが選択される。図１３には、ＲＯＩのうち、距離計測値が存在し、かつ、マスクに包含される実点に相当する領域に斜線が付されている。
【０１２９】
そして、外挿補間法にしたがって、マスクに包含されている他の領域における実点（○参照）が補完される。その結果、基準点が下段部分に存在し、当該基準点の近傍に段差由来のオクルージョンが発生していても、足平裏面と床面との接触状態が高精度で推定されうる（図１５（ａ）及び図１６（ａ）参照）。別の言い方をすると、外挿補間処理の実行要否に応じて、オクルージョンの発生有無が判定されうる。基準点が上段部分に存在する場合も、前記実施形態と同様に足平裏面と床面との接触状態が高精度で推定される（図１５（ｂ）（ｃ）及び図１６（ｂ）（ｃ）参照）。また、画素値Ｄｓを有する複数の実点のうち、マスクに包含されているもののみが選択されるため、計測誤差をの大きい画素値Ｄｓを有する実点を除くことができ、足平裏面と床面との接触状態が高精度で推定される。
【０１３０】
関係式（１０２）により定義されている第１係数関数ｗ1（ｅ1）（図６（ｂ）参照）は、負の指定値−ε2（ｃ2＞０）以下の定義域において、０ではなく正の指定値であってもよい。この場合、実物体の外部にあって実物体の表面からある程度離れている仮想点のコストＥが一律に「正の指定値」に評価される。これにより、仮想物体の表面が一の実物体の表面から離れている状態でも、統合コストＥを最小値又は極小値に近づけるような仮想面の探索処理を促進させることができる。その結果、当該一の実物体と当該他の実物体との、未来における接触状態が高精度かつ迅速に推定されうる。
【０１３１】
さらに、足平裏面の基準位置及び基準姿勢と、平面パラメータ又は曲面パラメータとの複数の組み合わせが仮定され、当該組み合わせのそれぞれに応じて、実物体と仮想物体に相当する他の実物体との、未来における複数の接触状態が推定されてもよい。
【符号の説明】
【０１３２】
１０‥距離画像センサ、１１‥基準画像センサ（一方の画像センサ）、１２‥参照画像センサ（他方の画像センサ）、２０‥コンピュータ、２１‥第１演算処理要素、２２‥第２演算処理要素、２３‥第３演算処理要素。
【図１】
0005959899.tif 000002
【図２】
0005959899.tif 000003
【図３】
0005959899.tif 000004
【図４】
0005959899.tif 000005
【図５】
0005959899.tif 000006
【図６】
0005959899.tif 000007
【図７】
0005959899.tif 000008
【図８】
0005959899.tif 000009
【図９】
0005959899.tif 000010
【図１０】
0005959899.tif 000011
【図１１】
0005959899.tif 000012
【図１２】
0005959899.tif 000013
【図１３】
0005959899.tif 000014
【図１４】
0005959899.tif 000015
【図１５】
0005959899.tif 000016
【図１６】
0005959899.tif 000017