JP2013066996A - 接触状態推定装置及び軌道生成装置 - Google Patents

Abstract

【課題】脚式移動ロボットの足平及び床面など、異なる物体同士の未来の接触状態を高精度で推定しうる装置を提供する。
【解決手段】仮想物体（例えばロボットの足平）の少なくとも一部が実在物体（例えば床）の内部に入り込んだ状態、すなわち、仮想物体の表面上にある複数の仮想点の少なくとも一部が実在物体の内部にある状態が仮定されうる。実在物体の内部及び外部のそれぞれにおいて、仮想点が実在物体の表面又は表皮部分から離れて内部の深い箇所にあって座標値偏差Δｚ_iが大きいほど、コストＥ_iも高い値に算定される。合計コストＥ＝Σ_iＥ_iが最小値又は極小値に近づけられるような仮想点の座標値の組み合わせＺ＾が探索される。
【選択図】 図６

Description

本発明は、異なる物体のそれぞれの表面形状に関する情報に基づき、当該異なる物体の接触状態を推定する装置、及び当該推定結果に基づき、物体の位置及び姿勢の時系列を表わす位置姿勢軌道を生成する装置に関する。

脚式移動ロボットが凹凸面を歩行する際、その姿勢を安定に制御する観点から、足平裏面の異なる箇所に設けられている距離センサの出力値に基づき、足平裏面の当該異なる箇所のそれぞれと地面との接地度を算出する手法が提案されている（特許文献１参照）。

接地度は、足平（正確には足平裏面）が接地していない状態では「０」又は正の微小値εであり、足平が十分に接地している状態では「１」であると定義されている。また、これらの中間の状態では、接地度は０又は当該微小値εと１との間で連続的に値が変化するように定義されている。

特開２００９−１０７０３２号公報

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

そこで、本発明は、脚式移動ロボットの足平及び床面など、異なる物体同士の未来の接触状態を高精度で推定しうる装置及び当該推定結果を用いて物体の位置及び姿勢の制御用の軌道を生成する装置を提供することを課題とする。

本発明の接触状態推定装置は、実物体までの距離を計測するための距離計測要素と、前記距離計測要素により計測された距離に基づき、基準座標系における前記実物体の表面にある各実点の座標値を測定するように構成されている第１演算処理要素と、前記基準座標系において指定形状の仮想物体の位置及び姿勢を仮定することにより、前記仮想物体の表面にあって前記各実点に対応する各仮想点の座標値を仮定するように構成されている第２演算処理要素と、前記第１演算処理要素により測定された前記基準座標系における前記各実点の座標値と、前記第２演算処理要素により仮定された前記基準座標系における前記各仮想点の座標値との偏差であって、前記仮想点が前記実物体の内部に存在する場合は正値として定義され、前記仮想点が前記実物体の外部に存在する場合は負値として定義されている座標値偏差を変数とし、０以上である所定の均衡値において最小値又は極小値を有し、前記均衡値の正側の定義域において増加関数として定義されるコスト関数にしたがって、前記実点と前記仮想点との各組み合わせについてのコストの合計値である合計コストを計算するように構成されている第３演算処理要素とを備え、前記第２演算処理要素が、前記第３演算処理要素により計算された前記合計コストを最小値又は極小値に近づけるような、前記基準座標系における前記各仮想点の座標値の組み合わせを、前記仮想物体に相当する第１物体と、前記実物体としての第２物体との接触状態として探索するように構成されていることを特徴とする。

本発明の第１態様としての接触状態推定装置によれば、仮想物体の少なくとも一部が実物体の内部に入り込んだ状態、すなわち、仮想物体の表面上にある複数の仮想点の少なくとも一部が実物体の内部にある状態が仮定されうる。実物体の内部及び外部のそれぞれにおいて、仮想点が実物体の表面又は表皮部分（厚さは均衡値の大きさに比例する。）から離れて内部の深い箇所にあるほど、当該仮想点と実物体の表面にある実点との座標値偏差が大きい正値に算定され、コストも高い値に算定されうる。

このため、合計コストが最小値又は極小値に近づけられるような仮想点の座標値の組み合わせが探索されることで、実物体の内部にあった仮想点が実物体の表面若しくは表皮部分又は実物体の外側に変位させられる。そして、当該変位後に実点の座標値と一致している仮想点の座標値の組み合わせにより、実物体の表面に対する、仮想物体の表面の接触箇所が推定されうる。なお、一の実物体と他の実物体との接触状態は、例えば当該一の物体の表面における当該他の物体との接触範囲の位置、姿勢及び面積のうち少なくとも１つによって表わされる。

これにより、仮想物体に相当する他の実物体（第１物体）が、各実点の座標値の測定対象となった一の実物体（第２物体）を変形させずに当該一の実物体にめり込んでいる（異なる実物体が共通の実空間を占有している）という実現不可能な状態が接触状態として探索される事態が確実に回避される。その結果、当該一の実物体と当該他の実物体との、未来における接触状態が高精度で推定されうる。また、接触状態の推定に際して、実点座標値が測定されればよく、実物体の表面の姿勢が測定される必要はないので、その分だけ計算負荷が軽減される。なお、仮想物体の指定形状は、仮想点が定義されるその表面が平面のほか任意形状の曲面になるように、任意に変更されうる。

前記第３演算処理要素が、前記均衡値との偏差の絶対値が同じである一方、変数値が前記均衡値より大きい場合の値が、変数値が前記均衡値よりも小さい場合の値よりも大きくなるように定義されている関数を前記コスト関数として用いて、前記実点と前記仮想点との各組み合わせについての前記コストを計算するように構成されていることが好ましい。

当該構成の接触状態推定装置によれば、均衡値との偏差の絶対値が同じである一方、変数値が均衡値より大きい場合の値が、変数値が均衡値より小さい場合の値よりも大きくなるようなコスト関数にしたがってコスト及び合計コストが計算される。すなわち、均衡値を挟んで正側及び負側の定義域で非対称的なコスト関数にしたがってコスト及び合計コストが計算される。例えば、均衡値の正側の定義域における増加率を表わす勾配が、均衡値の負側の定義域における減少率を表わす勾配よりも急になるような非対称的なコスト関数にしたがってコスト及び合計コストが計算される。

このため、実物体の内部にある仮想点を実物体の表面又は表皮部分に近づけることが、実物体の外部にある仮想点を実物体の表面又は表皮部分に近づけることよりも優先された形で各仮想点の座標値の組み合わせが探索されうる。その結果、前記のような実現不可能な状態が仮想物体と実物体との接触状態として探索される事態が確実に回避される。その結果、当該一の実物体と当該他の実物体との、未来における接触状態が高精度で推定されうる。

前記第３演算処理要素が、前記均衡値の負側の定義域のうち少なくとも前記均衡値よりも小さい指定値以下の定義域において０以上の一定値として定義されている関数を前記コスト関数として用いて、前記実点と前記仮想点との各組み合わせについての前記コストを計算するように構成されていることが好ましい。

当該構成の接触状態推定装置によれば、一定値が０である場合、実物体の外部にあって実物体の表面からある程度離れている仮想点のコストが一律に０に評価される。これにより、合計コストを最小値又は極小値に近づけるような仮想点の座標値の組み合わせの探索処理に要する計算負荷が軽減される。そして、前記のように実際にはありえない仮想物体と実物体との接触状態が探索される事態が確実に回避される。その結果、当該一の実物体と当該他の実物体との、未来における接触状態が高精度で推定されうる。

一定値が正値である場合、実物体の外部にあって実物体の表面からある程度離れている仮想点のコストが一律に当該正値に評価される。これにより、仮想物体の表面が一の実物体の表面又は表皮部分から離れている状態でも、合計コストを最小値又は極小値に近づけるような仮想点の座標値の組み合わせの探索処理を促進させることができる。その結果、当該一の実物体と当該他の実物体との、未来における接触状態が高精度かつ迅速に推定されうる。

前記第２演算処理要素が、前記基準座標系において前記仮想物体の位置及び姿勢の異なる組み合わせのそれぞれを仮定することにより、前記各仮想点の座標値の組み合わせのそれぞれを仮定した上で、前記第３演算処理要素により計算された前記合計コストを最小値又は極小値に近づけるような、前記基準座標系における前記各仮想点の座標値の組み合わせを探索するように構成されていることが好ましい。

当該構成の接触状態推定装置によれば、仮想物体の異なる初期位置および初期姿勢の組み合わせのそれぞれに応じて、一の実物体と、仮想物体と同形状の他の実物体との、未来における複数の接触状態が高精度で推定されうる。

前記コスト関数が、前記座標値偏差を変数とする係数関数と、前記座標値偏差の絶対値の正値の冪乗との積として定義されていることが好ましい。

当該構成の接触状態推定装置によれば、コストが係数関数をバネ係数とし、座標値偏差の絶対値を当該バネ変形量とする仮想的なバネの弾性エネルギーとして表現される。

前記距離計測要素の測定精度の高低に応じて幅の広狭が定まる前記座標値偏差の誤差分布関数に応じて、前記幅が０であれば前記座標値偏差が０である場合の値が０となる一方、前記幅が広いほど前記座標値偏差が０である場合の値が大きい正値となるように前記係数関数が定義されていることが好ましい。

当該構成の接触状態推定装置によれば、座標値偏差が本来的には正値であるものの、距離計測要素の測定精度が低いために負値であると判断され、その結果として当該仮想点を実物体の内部に残したまま仮想面と実物体表面との接触状態が推定されるような事態が回避されうる。

前記仮想物体から前記実物体に作用する仮想押圧力と前記実物体から前記仮想物体に作用する仮想抗力との偏差が、前記座標値偏差を変数とするとともに前記均衡値において０になる関数として定義され、前記コスト関数が、前記偏差の大きさの正の冪乗として定義されていることが好ましい。

前記仮想押圧力が正又は負の一定値として定義され、前記仮想抗力が前記座標値偏差を変数とし、０以下の値域を有する減少関数又は０以上の値域を有する増加関数として定義されていることが好ましい。

当該構成の接触状態推定装置によれば、仮想物体と実物体との仮想的な作用・反作用の法則にしたがって、当該仮想物体と当該実物体との接触状態が推定されうる。

本発明の軌道生成装置は、本発明の接触状態推定装置を備え、前記第３演算処理要素により推定された前記第１物体と前記第２物体との複数の接触状態のうち、一群の接触状態を複数の接触状態候補として選定した上で、前記複数の接触状態候補に応じた接触状態が実現されるように、前記第１物体の位置及び姿勢の時系列を表わす位置姿勢軌道を生成するように構成されていることを特徴とする。

本発明の軌道生成装置によれば、前記のように高精度で推定される接触状態が実現されるような位置姿勢軌道が生成されるので、当該軌道にしたがって第１物体の位置及び姿勢が制御されることにより、第１物体を第２物体に対して所望の形態で断続的又は連続的に接触させることができる。

本発明の軌道生成装置が、脚式移動ロボットの基体から延設されている複数の脚体の相互干渉を回避するように、前記第１物体としての前記複数の脚体のそれぞれの着床部分の位置姿勢軌道を生成することが好ましい。

本発明の軌道生成装置が、前記複数の脚体のそれぞれの姿勢変化量が所定の許容範囲に収まるように、前記第１物体としての前記複数の脚体のそれぞれの着床部分の位置姿勢軌道を生成することが好ましい。

本発明の実施形態としての接触状態推定装置の構成説明図。 接触状態推定装置の使用例に関する説明図。 接触状態推定装置により実行される演算処理を表わすフローチャート。 係数関数の定義に関する説明図。 コスト関数の定義に関する説明図。 コストの意義に関する説明図。 接触状態の推定結果に関する説明図。 接触状態の推定結果に関する説明図。 推定接触状態に基づく接触状態軌道の生成に関する説明図。 推定接触状態及び接触状態候補の生成に関する説明図。 撮像範囲の調節に関する説明図。 撮像範囲の調節結果に関する説明図。

（構成）
図１に示されている接触状態推定装置は、距離画像センサ（距離計測要素）１と、距離画像センサ１からの出力信号が入力されるコンピュータ（プログラマブルコンピュータ）２により構成されている。コンピュータ２は演算処理装置（ＣＰＵにより構成されている。）、記憶装置（メモリにより構成されている。）及び入出力回路を備えている。

接触状態推定装置は、例えば、図２に示されているように脚式移動ロボットＲに搭載され、その足平ｆｔ（左右を区別する場合は「Ｒ」又は「Ｌ」を付記する。）等の脚体の一部と床面との接触状態を推定するために利用される。脚式移動ロボットＲの構成としては、特許第３６７４７８８号公報等、本出願人が提案している構成が採用されうる。また、接触状態推定装置は、ロボットアームにおいて手の平又はアームの一部と対象物体との接触状態を推定するために利用される。さらに、接触状態推定装置は車両に搭載され、車両のタイヤと路面との接触状態を推定するために利用される。

距離画像センサ１は、たとえばＴＯＦ（Time-of-Flight）距離画像センサであり、各ピクセルが距離計測値を画素値として有する距離画像を取得する。距離画像センサ１の平面的に配列されている撮像素子群により「距離画像座標系」が定義される。距離画像センサ１は、左右一対の画像センサ（ステレオカメラ）によって構成されていてもよい。一対の画像センサが有する視差が利用されることにより、基準座標系における実物体（被写体）の表面にある各実点の座標値が測定される。

コンピュータ２は、後述する演算処理を実行するように構成されている第１演算処理要素２１と、第２演算処理要素２２と、第３演算処理要素２３とを備えている。単一の演算処理装置が、当該３つの演算処理要素２１〜２３として機能してもよいし、複数の演算処理装置が、相互通信により連携しながら当該３つの演算処理要素２１〜２３として機能していてもよい。

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

（機能）
前記構成の接触状態推定装置によって実行される接触状態推定方法について説明する。

第１演算処理要素２１により、実物体である被写体の表面（例えば床面）にある実点の座標値が測定される（図３／ＳＴＥＰ１）。

具体的には、距離画像センサ１からの出力信号に基づき、距離画像座標系を構成する第ｉピクセル（ｉ＝１〜ｎ）の画素値である距離ｚ_iが、実点座標値（正確にはｚ成分）として測定される。２次元の距離画像座標系に対して直交する方向をｚ方向とする３次元直交座標系が基準座標系として採用される。

距離画像センサ１の光学中心と物体表面との距離ｌが測定されるので、距離画像において光軸に対して角度ψをなす位置にある画素の画素値（ｚ成分）はｌｃｏｓψと定義される。このため、距離画像センサ１がキャリブレーションされている場合、当該定義にしたがってｚ成分ｚ_iが計算されうる。

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

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

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

第２演算処理要素２２により、指定形状の仮想物体の表面にある第ｉ仮想点の座標値ｚ_i＾が仮定される（図３／ＳＴＥＰ２）。

具体的には、ロボットＲの足平裏面（仮想物体の表面）の第ｉ仮想点の基準位置ｑ_iが設定されることにより、足平裏面の基準座標系における基準位置及び基準姿勢が設定される。足平裏面が平面状である場合、基準位置ｑ_iは関係式（０１）によって定義される。

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

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

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

さらに、足平裏面の基準座標系における位置及び姿勢を表わす平面パラメータｍ＝^t（ｍ_x，ｍ_y，ｍ_z）が仮定される。

平面パラメータｍは,平面を構成する任意の基準位置ｑ_iとその距離ｚに基づき, 関係式（０１１）に従って定義される。

mq_i=z..(011)。

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

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

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

Z^=^tQm ..(03)。

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

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

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

第３演算処理要素２３により、座標値偏差（距離残差）Δｚ_i＝ｚ_i−ｚ_i＾に基づき、コスト関数Ｅ（Δｚ_i）にしたがってコストＥ_iが算定され、その和Σ_iＥ_iが合計コストＥとして算定される（図３／ＳＴＥＰ３）。座標値偏差Δｚ_iは、仮想点が実物体の外部にある場合は正値である一方、仮想点が実物体の内部にある場合は負値である。コスト関数Ｅ（Δｚ_i）は、座標値偏差Δｚ_iを変数とする係数関数ｗ（Δｚ_i）と、座標値偏差Δｚ_iの絶対値の二乗の積として関係式（１０）にしたがって定義される。当該冪乗数は「２」に限られず、「０．５」「１．５」「３」など、任意の正値に設定されていてもよい。

E(Δz_i)=w(Δz_i)|Δz_i|² ..(10)。

コストＥ_iの算定手順は任意に選択されてもよい。例えば、座標値偏差Δｚ_iに基づき、コスト関数Ｅ（Δｚ_i）にしたがってコストＥ_iが算定されてもよい。座標値偏差Δｚ_iに基づき、係数関数ｗ（Δｚ_i）にしたがって係数が算定され、これとは別個に座標値偏差の絶対値｜Δｚ_i｜の冪乗（２乗）の値が算定され、その上で当該算定結果が掛け合わせられることによりコストＥ_iが算定されてもよい。

係数関数ｗ（Δｚ_i）は、例えば関係式（１２）によって定義される。図４（ａ）に実線で示されているように、係数関数ｗ（Δｚ_i）は、負の定義域（ΔＺ_i＜０）において０である一方、０以上の定義域において座標値偏差Δｚ_iの増加関数である。

w(Δz_i)=0 (if Δz_i<0), w(Δz_i)=c₀Δz_i(0<c₀) (if 0≦Δz_i) ..(12)。

係数関数ｗ（Δｚ_i）は、例えば関係式（１４）によって定義されていてもよい。この場合、図４（ｂ）に実線で示されているように、係数関数ｗ（Δｚ_i）は、負の指定値−ｃ₂（ｃ₂＞０）以下の定義域において０である一方、負の指定値−ｃ₂を超えている定義域において増加関数である。

w(Δz_i)=0 (if Δz_i≦-c₂), or w(Δz_i)=(c₁/c₂)Δz_i+c₂ (if -c₂<Δz_i) ..(14)。

係数関数ｗ（Δｚ_i）は、例えば関係式（１６）によって定義されていてもよい。図４（ｃ）に実線で示されているように、係数関数ｗ（Δｚ_i）は、座標値偏差Δｚ_iの増加関数である。

w(Δz_i)=log(1+exp(αΔz_i-β)) (α>0, β>0) ..(16)。

係数関数ｗ（Δｚ_i）は、実点座標値ｚ_iの測定誤差、ひいては座標値偏差Δｚ_iの誤差が勘案された上で定義されていてもよい。例えば、関係式（２０）で定義されるように、元の係数関数ｗ_o（Δｚ_i）と、座標値偏差Δｚ_iの誤差分布関数ｐ（ｔ）との畳み込み積分の結果が、係数関数ｗ（Δｚ_i）として定義されていてもよい。積分区間は［−∞，Δｚ_i］である。

w(Δz_i)=∫dt {-w(t-Δz_i)}p(t) ..(20)。

誤差分布関数ｐ（ｔ）は、例えば関係式（２２）で表わされるようなガウス分布関数（平均値ｍ＝０）により定義される。標準偏差σはあらかじめ設定されてもよく、距離画像センサ１からその測定精度を表わす信号を受信し、当該信号に基づいて設定されてもよい。

p(t)=1/{(2π)^1/2σ}exp{-t²/2σ²} ..(22)。

元の係数関数ｗ_o（Δｚ_i）が関係式（１２）により定義されている場合、係数関数ｗ（Δｚ_i）は図４（ａ）に破線で示されているような変化特性を示す。この係数関数ｗ（Δｚ_i）は、元の係数関数ｗ_o（Δｚ_i）（実線参照）と比較して０以上の定義域で増加関数である点で一致する一方、０付近の負の定義域においても増加関数であってΔｚ_i＝０において０ではなく正の値を有する点で相違することがわかる。

図４（ｂ）（ｃ）に実線で示されている係数関数ｗ（Δｚ_i）の変化特性は、図４（ａ）に破線で示されている係数関数ｗ（Δｚ_i）の変化特性を近似的に表現しているともいえる。

係数関数ｗ（Δｚ_i）が関係式（１２）にしたがって定義されている場合、コスト関数Ｅ（Δｚ_i）は、図５（ａ）に示されているように、負の定義域（均衡値＝０の場合の負側の定義域）で０である一方、正の定義域（均衡値＝０の場合の正側の定義域）において増加関数である。また、コスト関数Ｅ（Δｚ_i）は、正負の定義域において非対称的な変化特性を示す。すなわち、変数Δｚ_iの絶対値が同一であっても、当該変数Δｚ_iが正値である場合のコスト関数Ｅ（Δｚ_i）の値が、当該変数Δｚ_iが負値である場合のコスト関数Ｅ（Δｚ_i）の値よりも大きくなる。

係数関数ｗ（Δｚ_i）が関係式（１４）にしたがって定義されている場合、コスト関数Ｅ（Δｚ_i）は、図５（ｂ）に示されているように、Δｚ_i＝０（均衡値）で極小値０を有し、正の定義域において増加関数である。これにより、Δｚ_iが０近辺にあるとき、Δｚ_iが０に近づきやすくなる。コスト関数Ｅ（Δｚ_i）は、正負の定義域において非対称的な変化特性を示し、負の指定値−ｃ₂以下の定義域（Δｚ_i≦−ｃ₂）においては０である。すなわち、変数Δｚ_iの絶対値が同一であっても、当該変数Δｚ_iが正値である場合のコスト関数Ｅ（Δｚ_i）の値が、当該変数Δｚ_iが負値である場合のコスト関数Ｅ（Δｚ_i）の値よりも大きくなる。

係数関数ｗ（Δｚ_i）が関係式（１６）にしたがって定義されている場合、コスト関数Ｅ（Δｚ_i）は、図５（ｃ）に示されているように、Δｚ_i＝０（均衡値）で極小値０を有し、正の定義域において増加関数である。また、コスト関数Ｅ（Δｚ_i）は、正負の定義域において非対称的な変化特性を示す。

係数関数ｗ（Δｚ_i）が関係式（２０）にしたがって定義され、元の係数関数ｗ_o（Δｚ_i）が関係式（１２）にしたがって定義されている場合、コスト関数Ｅ（Δｚ_i）は図５（ｂ）に示されているのと同様の変化特性を示す。

コストＥ_iは、基準座標系のｚ方向についての実点及び仮想点の距離残差である座標値偏差Δｚ_iの絶対値の二乗に比例している（関係式（０８）参照）。このため、コストＥ_iは、係数関数ｗ（Δｚ_i）をバネ係数とし、仮想点を実点に近づけるような、図６に示されている仮想的なバネの弾性エネルギーに相当する。

関係式（１０１）〜（１０４）のそれぞれに表わされているように、コスト関数Ｅ（Δｚ_i）は、Δｚ_i＝０において最小値又は極小値を示し、かつ、正の定義域においては増加する関数としてさまざまな形で定義されうる。

w(Δz_i)=0 (if Δz_i<0), E(Δz_i)=(Δz_i)²(if 0≦Δz_i) ..(101)。

E(Δz_i)=exp(αΔzi)(Δzi)² ..(102)。

E(Δz_i)=-c₁Δzi (if Δzi≦0), or E(Δz_i)=c₁Δzi (if 0<Δzi) ..(103)。

E(Δz_i)=ε(≧0) (if Δzi<0), or E(Δz_i)=ε+c₁(Δzi)² (if 0≦Δzi) ..(104)。

床から足平ｆｔに作用する仮想抗力（仮想床反力）を定める仮想床反力関数ｆ（Δｚ_i）が関係式（１０５）により定義され、かつ、当該仮想床反力関数ｆ（Δｚ_i）と足平ｆｔから床に作用する仮想踏力（仮想押圧力）又は足平ｆｔに作用する重力ｍｇとに基づいてコスト関数Ｅ（Δｚ_i）が関係式（１０６）により定義されていてもよい。「ｍ」は足平ｆｔを含むロボットＲの質量の一部又は全部を意味する。「ｇ」は重力加速度である。

f(Δz_i)=-log(1+exp(αΔz_i-β)) (α>0, β>0) ..(105)。

E=(mg-Σ_if(Δz_i))²..(106)。

例えば、仮想床反力関数ｆ（Δｚ_i）が関係式（１０５）により定義されている場合、コスト関数Ｅ（Δｚ_i）は、図５（ｄ）に示されているように、ロボットＲに作用する重力及び仮想床反力が釣り合うようなΔｚ_i＝ｃ（＞０）（均衡値）において極小値０を示し、当該均衡値ｃの正側定義域（ｃ＜Δｚ_i）において増加する関数である。

仮想物体（ロボットＲの足平ｆｔ）と実物体（床）とのうち一方又は両方が接触により変形可能であることが勘案されて均衡値が正値に設定されている。このため、均衡値ｃが正であることは、仮想物体が変形を伴わずにその一部が実物体に入り込んでいるような両物体の接触状態の推定を許容することを意味しない。

仮想物体及び実物体のうち一方又は両方の接触部分（足平ｆｔの底部及び床）の柔軟性が高いほど、均衡値が大きく設定されうる。例えば、仮想物体が部分的に実物体の表皮部分に存在する場合、仮想物体の当該部分が実物体の表面形状に沿うように変形している、又は実物体が仮想物体の当該部分の表面形状に沿って変形しているような接触状態が推定されうる。

第２演算処理要素２２により、収束条件が満足されているか否かが判定される（図３／ＳＴＥＰ４）。収束条件としては、合計コストＥが所定の閾値以下であること、合計コストＥが更新の前後で変化した量が閾値以下であること、（∂Ｅ／∂ｍ）の前回値に対する今回値の偏差が閾値以下であることなどの条件が採用される。

収束条件が満足されていないと判定された場合（図３／ＳＴＥＰ４‥ＮＯ）、第２演算処理要素２２により、合計コストＥを最小値又は極小値に近づけるような、仮想点の座標値ｚ_i＾が探索される（図３／ＳＴＥＰ５）。具体的には、最小二乗法（重み付き最小二乗法）にしたがって、平面パラメータｍが算定される。

収束条件が満足されていると判定された場合（図３／ＳＴＥＰ４‥ＹＥＳ）、第２演算処理要素２２により、その段階における仮想点の座標値ｚ_i＾に基づき、実物体と、仮想物体と同一形状の表面を有する他の実物体との接触状態が推定される（図３／ＳＴＥＰ６）。

（作用効果）
前記機能を発揮する接触状態推定装置によれば、仮想物体（例えばロボットＲの足平ｆｔ）と、実物体（例えば床）とが共通の実空間を同時に占有するという、当該仮想物体が実物体である場合には実現不可能な状態が仮定されうる。すなわち、仮想物体の表面上にある複数の仮想点の少なくとも一部が実物体の内部にある状態が仮定されうる（図６参照）。

実物体の内部及び外部のそれぞれにおいて、仮想点が実物体の表面又は表皮部分から離れて内部の深い箇所にあるほど、当該仮想点と実物体の表面にある実点との座標値偏差Δｚ_iが大きい正値に算定され、コストＥ_iも高い値に算定される（図５（ａ）〜（ｄ）参照）。表皮部分は均衡値が大きくなるほど厚くなるように定義される。

このため、合計コストＥ＝Σ_iＥ_iが最小値（又は極小値）に近づけられるような平面パラメータｍ、さらには平面パラメータｍによって表わされる仮想点の座標値の組み合わせＺ＾が探索されることで、実物体の内部にあった仮想点を実物体の外側又は表面上若しくは表皮部分に変位させ、当該変位後に実物体表面又は実物体の表皮部分にある仮想点の座標値の組み合わせにより、一の実物体の表面に対する仮想物体の表面の接触箇所が決定されうる（図７及び図８参照）。

関係式（１４）により定義される係数関数ｗ（Δｚ_i）が採用されることで、実物体の外部にあって実物体の表面からある程度離れている仮想点のコストＥ_iが一律に「０」に評価されうる（図５（ｂ）参照）。これにより、合計コストを最小値又は極小値に近づけるような仮想点の座標値の組み合わせの探索処理に要する計算負荷が軽減される。

関係式（１６）により定義される係数関数ｗ（Δｚ_i）が採用されることで、正の定義域における増加率を表わす勾配が、負の定義域における減少率を表わす勾配よりも急になるような、正負の定義域で非対称的なコスト関数Ｅ（Δｚ_i）にしたがってコストＥ_i及び合計コストＥが計算される（図５（ｃ）参照）。このため、実物体の内部にある仮想点を実物体の表面に近づけることが、実物体の外部にある仮想点を実物体の表面に近づけることよりも優先された形で各仮想点の座標値の組み合わせよりも探索されうる。

係数関数ｗ（Δｚ_i）が、距離画像センサ１の測定精度の高低に応じて幅の広狭（標準偏差σの大小）が定まる座標値偏差の誤差分布関数ｐ（ｔ）と、元の係数関数ｗ_o（Δｚ_i）との畳み込み積分の結果として定義されている（関係式（２０）（２２）参照）。すなわち、係数関数ｗ（Δｚ_i）は、幅σが０であれば座標偏差値Δｚ_iが０である場合の値ｗ（０）が０となる一方、幅σが広いほど座標偏差値Δｚ_iが０である場合の値ｗ（０）が大きい正値となるように定義されている。

これにより、座標値偏差Δｚ_iが本来的には正値であるものの、距離画像センサ１の測定精度が低いために負値であると判断され、その結果として当該仮想点を実物体の内部に残したまま仮想物体と実物体との接触状態が推定されるような事態が回避されうる。

畳み込みにより得られた当該係数関数ｗ（Δｚ_i）を近似表現するように関係式（１４）における負の指定値−ｃ₂又は関係式（１６）における係数α及びβの値が設定されてもよい。その結果、同様に距離画像センサ１の測定精度に由来して、当該仮想点を実物体の内部に残したまま仮想面と実物体表面との接触状態が推定されるような事態が回避されうる。

そして、仮想物体に相当する他の実物体が、各実点の座標値の測定対象となった一の実物体を変形させることなく当該実物体にめり込んでいるという実現不可能な状態が接触状態として探索される事態が確実に回避される（図６参照）。すなわち、当該一の実物体及び当該他の実物体が共通の実空間を同時に占有するという実現不可能な状態が接触状態として探索される事態が確実に回避される。仮想物体に相当する他の実物体とは、仮想面と形状及びサイズが同じである表面を有する又は当該表面を適応的に形成しうる実物体を意味する。その結果、当該一の実物体と当該他の実物体との、未来における接触状態が高精度で推定されうる。

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

また、接触状態の推定に際して、実点座標値が測定されればよく、実物体の表面の姿勢が測定される必要はないので、その分だけ計算負荷が軽減される。

（本発明の他の実施形態）
関係式（１４）により定義される係数関数ｗ（Δｚ_i）は、負の指定値−ｃ₂（ｃ₂＞０）以下の定義域において、０ではなく正値であってもよい。この場合、実物体の外部にあって実物体の表面からある程度離れている仮想点のコストが一律に当該正値に評価される。これにより、仮想物体の表面が一の実物体の表面から離れている状態でも、合計コストを最小値又は極小値に近づけるような仮想点の座標値の組み合わせの探索処理を促進させることができる。その結果、当該一の実物体と当該他の実物体との、未来における接触状態が高精度かつ迅速に推定されうる。

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

（接触状態の推定結果に基づく軌道生成）
前記構成の接触状態推定装置を用いて第１物体の位置姿勢軌道を生成するように軌道生成装置が構成されてもよい。第１物体の第２物体に対する推定接触状態ｇｓｔｋ（ｋ＝１，２，‥）のうち、空間的に離散的又は連続的（準連続的）な一群の局所領域の推定接触状態が接触状態候補ｇｃｄｊ（ｊ＝１，２，‥）として選定される。当該選定された接触状態候補ｇｃｄｊに基づき、第１物体の位置及び姿勢の時系列を表わす位置姿勢軌道が生成される。

例えば、図９（ａ）に示されているように、ロボットＲ（図２参照）の両足平ｆｔ（第１物体）と、床（第２物体）との複数の局所領域の接触状態ｇｓｔ１〜ｇｓｔ７が前記のように推定される。また、複数の局所領域の推定接触状態ｇｓｔ１〜ｇｓｔ７のうち、空間的に離散的な一群の局所領域の推定接触状態ｇｓｔ２、ｇｓｔ５及びｇｓｔ７が複数の接触状態候補ｇｃｄ１〜ｇｃｄ３（斜線箇所参照）として選択される。

ロボットＲの両脚体が相互に干渉しないという第１の条件が満たされるように接触状態候補ｇｃｄｊが選択される。両脚体の干渉回避手法としては、特許公報第４２２５９６９号に記載されている手法が採用されうる。

そのほか、遊脚の足平ｆｔと床との接触範囲が、支持脚の足平ｆｔを包含する所定範囲から外れているような推定接触状態ｇｓｔｋが当該遊脚の接触状態候補ｇｓｔｊとして選択される。所定範囲としては、支持脚の足平ｆｔの中心、又は当該中心から前後左右にずれた点を中心とする球などの任意形状の３次元空間に包含される範囲が採用される。右足平ｆｔ＿Ｒが支持脚側の足平である場合、右足平ｆｔ＿Ｒの中心よりも右側又は右斜め前方にずれた点を中心とする球等に包含される範囲が所定範囲として定義される。左足平ｆｔ＿Ｌが支持脚側の足平である場合、左足平ｆｔ＿Ｌの中心よりも左側又は左斜め前方にずれた点を中心とする球等に包含される範囲が所定範囲として定義される。

第１物体の複数の位置姿勢軌道が生成された場合、各脚体の関節角度変化量により定義される姿勢変化が、所定の許容範囲に収まるという第２条件が満たされるような一の位置姿勢軌道にしたがって第１物体の位置及び姿勢が制御されてもよい。

そして、図９（ｂ）に示されているように、接触状態候補ｇｃｄ１〜ｇｃｄ３に相当する接触状態が実現されるように、右足平ｆｔ＿Ｒの位置姿勢軌道（黒矩形参照）及び左足平ｆｔ＿Ｌの位置姿勢軌道（白矩形参照）が生成される。各足平ｆｔの複数の位置姿勢軌道が生成された場合、ロボットＲの脚体関節角度の可変範囲及び歩幅等の物理的制約条件が満たされるような一の位置姿勢軌道が選択される。その上で、当該一の位置姿勢軌道にしたがって各足平ｆｔと床面との接触状態が実現されるように、ロボットＲの脚体等の動作が制御される。

図１０（ａ）には、ロボットＲの各足平ｆｔと上り段差がある床との局所領域の推定接触状態ｇｓｔｋ及び接触状態候補ｇｃｄｊ（斜線箇所参照）が示されている。図１０（ｂ）には、ロボットＲの各足平ｆｔと下り階段との局所領域の推定接触状態ｇｓｔｋ及び接触状態候補ｇｃｄｊ（斜線箇所参照）が示されている。推定接触状態ｇｓｔｋに応じた足平ｆｔと床との接触範囲が楕円又は矩形により示されている。図１０（ａ）（ｂ）に示されているように、床面における各推定接触状態ｇｓｔｋに応じた推定接触範囲は相互に重複していてもよい。

（撮像範囲調節）
接触状態推定範囲の変更又は拡張のため、距離画像センサ１の位置及び姿勢のうち一方又は両方が変更されることにより、その撮像範囲が変更されてもよい。例えば、図１１に示されているように、距離画像センサ１が頭部に搭載されているロボットＲが基体（上体）を前傾させる（白矢印参照）。これにより、撮像範囲が、元の撮像範囲（破線参照）よりもロボットＲに近い領域（足元領域）を含むように調節される（斜線箇所参照）。

図１２（ａ）には、ロボットＲの基体前傾前の距離画像センサ１による撮像画像が示されている。図１２（ｂ）には、ロボットＲの基体前傾後の距離画像センサ１による撮像画像が示されている。基体前傾後の撮像画像（図１２（ｂ））の下側には、基体前傾前の撮像画像（図１２（ａ））には含まれていなかった各足平ｆｔ＿Ｌ及びｆｔ＿Ｒの先端部分（つま先部分）が含まれている。基体前傾の前後で床面に付されているマークＭの撮像画像における位置が変化している。

１‥距離画像センサ（距離計測要素）、２‥コンピュータ、２１‥第１演算処理要素、２２‥第２演算処理要素、２３‥第３演算処理要素。

Claims (11)

1. 実物体までの距離を計測するための距離計測要素と、
   前記距離計測要素により計測された距離に基づき、基準座標系における前記実物体の表面にある各実点の座標値を測定するように構成されている第１演算処理要素と、
   前記基準座標系において指定形状の仮想物体の位置及び姿勢を仮定することにより、前記仮想物体の表面にあって前記各実点に対応する各仮想点の座標値を仮定するように構成されている第２演算処理要素と、
   前記第１演算処理要素により測定された前記基準座標系における前記各実点の座標値と、前記第２演算処理要素により仮定された前記基準座標系における前記各仮想点の座標値との偏差であって、前記仮想点が前記実物体の内部に存在する場合は正値として定義され、前記仮想点が前記実物体の外部に存在する場合は負値として定義されている座標値偏差を変数とし、０以上である所定の均衡値において最小値又は極小値を有し、前記均衡値の正側の定義域において増加関数として定義されるコスト関数にしたがって、前記実点と前記仮想点との各組み合わせについてのコストの合計値である合計コストを計算するように構成されている第３演算処理要素とを備え、
   前記第２演算処理要素が、前記第３演算処理要素により計算された前記合計コストを最小値又は極小値に近づけるような、前記基準座標系における前記各仮想点の座標値の組み合わせを、前記仮想物体に相当する第１物体と、前記実物体としての第２物体との接触状態として探索するように構成されていることを特徴とする接触状態推定装置。
2. 請求項１記載の接触状態推定装置において、
   前記第３演算処理要素が、前記均衡値との偏差の絶対値が同じである一方、変数値が前記均衡値より大きい場合の値が、変数値が前記均衡値よりも小さい場合の値よりも大きくなるように定義されている関数を前記コスト関数として用いて、前記実点と前記仮想点との各組み合わせについての前記コストを計算するように構成されていることを特徴とする接触状態推定装置。
3. 請求項１又は２記載の接触状態推定装置において、
   前記第３演算処理要素が、前記均衡値の負側の定義域のうち少なくとも前記均衡値よりも小さい指定値以下の定義域において０以上の一定値として定義されている関数を前記コスト関数として用いて、前記実点と前記仮想点との各組み合わせについての前記コストを計算するように構成されていることを特徴とする接触状態推定装置。
4. 請求項１〜３のうちいずれか１つに記載の接触状態推定装置において、
   前記第２演算処理要素が、前記基準座標系において前記仮想物体の位置及び姿勢の異なる組み合わせのそれぞれを仮定することにより、前記各仮想点の座標値の組み合わせのそれぞれを仮定した上で、前記第３演算処理要素により計算された前記合計コストを最小値又は極小値に近づけるような、前記基準座標系における前記各仮想点の座標値の組み合わせを探索するように構成されていることを特徴とする接触状態推定装置。
5. 請求項１〜４のうちいずれか１つに記載の接触状態推定装置において、
   前記コスト関数が、前記座標値偏差を変数とする係数関数と、前記座標値偏差の絶対値の正値の冪乗との積として定義されていることを特徴とする接触状態推定装置。
6. 請求項５記載の接触状態推定装置において、
   前記距離計測要素の測定精度の高低に応じて幅の広狭が定まる前記座標値偏差の誤差分布関数に応じて、前記幅が０であれば前記座標値偏差が０である場合の値が０となる一方、前記幅が広いほど前記座標値偏差が０である場合の値が大きい正値となるように前記係数関数が定義されていることを特徴とする接触状態推定装置。
7. 請求項１〜４のうちいずれか１つに記載の接触状態推定装置において、
   前記仮想物体から前記実物体に作用する仮想押圧力と前記実物体から前記仮想物体に作用する仮想抗力との偏差が、前記座標値偏差を変数とするとともに前記均衡値において０になる関数として定義され、前記コスト関数が、前記偏差の大きさの正の冪乗として定義されていることを特徴とする接触状態推定装置。
8. 請求項７記載の接触状態推定装置において、
   前記仮想押圧力が正又は負の一定値として定義され、前記仮想抗力が前記座標値偏差を変数とし、０以下の値域を有する減少関数又は０以上の値域を有する増加関数として定義されていることを特徴とする接触状態推定装置。
9. 請求項１〜８のうちいずれか１つに記載の接触状態推定装置を備え、
   前記第３演算処理要素により推定された前記第１物体と前記第２物体との複数の局所領域の接触状態のうち、一群の接触状態を複数の接触状態候補として選定した上で、前記複数の接触状態候補に応じた接触状態が実現されるように、前記第１物体の位置及び姿勢の時系列を表わす位置姿勢軌道を生成するように構成されていることを特徴とする軌道生成装置。
10. 請求項９記載の軌道生成装置において、
    脚式移動ロボットの基体から延設されている複数の脚体の相互干渉を回避するように、前記第１物体としての前記複数の脚体のそれぞれの着床部分の位置姿勢軌道を生成することを特徴とする軌道生成装置。
11. 請求項９記載の軌道生成装置において、
    脚式移動ロボットの基体から延設されている複数の脚体のそれぞれの姿勢変化量が所定の許容範囲に収まるように、前記第１物体としての前記複数の脚体のそれぞれの着床部分の位置姿勢軌道を生成することを特徴とする軌道生成装置。