(57)【要約】【課題】カメラの撮像画像による床面の形状を認識するための演算処理を軽減しつつ、ロボットの適切な移動制御を行なうことを可能とする脚式移動ロボットを提供する。【解決手段】ロボット１の移動時に、該ロボットの現在の動作制御のために使用している床面形状の情報により表される床面である現状想定床面に対するロボット１の脚体５Ｌ，５Ｒの複数歩分の着地予定位置を設定し、その各着地予定位置毎に、該着地予定位置の近辺に、ロボット１に搭載されたカメラ２７Ｌ，２７Ｒの撮像画像に投影する画像投影領域Ａ(i)を設定する。設定した各画像投影領域Ａ(i)をカメラ２７Ｌ，２７Ｒの撮像画像の撮像画像に投影してなる部分画像領域の画像に基づいて、各部分画像領域毎に、該部分画像領域内に撮像されている実際の床面である実床面部分領域Ａr(i)の形状を表す形状パラメータ（ｎ(i)，ｄ(i)）を推定する。【選択図】図５
2008246609.tif 000002
【特許請求の範囲】
【請求項１】
ロボットの進行方向前方の床面を含む外界を撮像するカメラを搭載し、該カメラの撮像画像に基づき前記床面の形状を推定しつつ移動する脚式移動ロボットにおいて、
前記ロボットの移動時に、該ロボットの現在の動作制御のために使用している床面形状の情報により表される床面である現状想定床面に対する該ロボットの脚体の複数歩分の着地予定位置を設定する着地予定位置設定手段と、
前記設定された各着地予定位置毎に、該着地予定位置の近辺に前記カメラの撮像画像に投影する平坦な画像投影領域を設定する画像投影領域設定手段と、
前記設定された各画像投影領域を前記カメラの撮像画像に投影してなる部分画像領域の画像に基づいて、各部分画像領域毎に、該部分画像領域内に撮像されている実際の床面である実床面部分領域の形状を表す形状パラメータを推定する床面形状推定手段とを備えたことを特徴とする脚式移動ロボット。
【請求項２】
前記各画像投影領域は、前記設定された着地予定位置における前記ロボットの脚体の接地面を含む領域であることを特徴とする請求項１記載の脚式移動ロボット。
【請求項３】
前記各部分画像領域内に撮像されている実床面部分領域の形状を表す形状パラメータは、少なくとも該実床面部分領域を包含する平面の空間的な姿勢を表す姿勢パラメータと、該平面の空間的な位置を表す位置パラメータとを含むことを特徴とする請求項１または２記載の脚式移動ロボット。
【請求項４】
前記カメラは２つのカメラから成り、前記床面形状推定手段は、前記設定された各画像投影領域毎に、該画像投影領域に対応する一方のカメラの部分画像領域の画像を、他方のカメラの部分画像領域の画像に一致する画像に変換し得る射影変換行列を特定するように、該画像投影領域に対応する実床面部分領域の形状パラメータを推定することを特徴とする請求項３記載の脚式移動ロボット。
【請求項５】
前記床面形状推定手段は、前記設定された画像投影領域のうちの任意の１つの画像投影領域に対応する実床面部分領域の形状パラメータを推定するとき、該部分画像領域に対応する実床面部分領域の形状パラメータを変数パラメータとし、その変数パラメータの値に応じて定まる仮の射影変換行列により前記一方のカメラの部分画像領域の画像を変換してなる画像と、前記他方のカメラの部分画像領域の画像とを比較しつつ、該変数パラメータの値により規定される平面を前記実床面部分領域を包含する平面に収束させるように該変数パラメータの値を初期値から更新していく収束演算処理を実行し、その収束演算処理により得られた該変数パラメータの収束値を、該部分画像領域に対応する実床面部分領域の形状パラメータの推定値として決定することを特徴とする請求項４記載の脚式移動ロボット。
【請求項６】
前記床面形状推定手段は、前記変数パラメータの初期値により規定される平面が前記実床面部分領域を包含する平面に対して傾斜した平面になることを該初期値が満たすべき制約条件とし、該初期値を該制約条件を満たす値に設定する手段を備えることを特徴とする請求項５記載の脚式移動ロボット。
【請求項７】
前記変数パラメータの初期値により規定される平面は、該平面の水平面に対する傾斜角度が、前記実床面分領域を包含する平面の水平面に対する実際の傾斜角度が採り得る値の範囲から逸脱するようにあらかじめ定められた所定角度となる平面であることを特徴とする請求項６記載の脚式移動ロボット。
【請求項８】
前記推定された各実床面部分領域の形状パラメータに基づいて、前記ロボットの進行方向前方の実際の床面の傾斜度合いが変化する傾斜変化部を特定する傾斜変化部特定手段をさらに備えることを特徴とする請求項３〜７のいずれか１項に記載の脚式移動ロボット。
【請求項９】
前記傾斜変化部特定手段は、各画像投影領域にそれぞれ対応する実床面部分領域を前記ロボットに近い側の第１手前側グループと該第１手前側グループよりもロボットから遠い側の第１奥側グループとに分類すると共に、その分類を、該第１手前側グループに属する実床面部分領域の形状パラメータの分散と第１奥側グループに属する実床面部分領域の形状パラメータの分散とが最小になるように行なう手段と、
その分類により得られた第１手前側グループに属する実床面部分領域の形状パラメータの仮代表値と第１奥側グループに属する実床面部分領域の形状パラメータの仮代表値とを決定し、それらの仮代表値により規定される２つの平面の相違の度合いの大小を判断する手段と、
該２つの平面の相違の度合いが大きいと判断されたとき、該２つの平面をそれぞれ規定する仮代表値に基づき、前記傾斜変化部の仮値としての仮傾斜変化部を決定する手段と、
前記第１手前側グループに属する実床面部分領域のうち、前記仮傾斜変化部に近接した実床面部分領域を除外した実床面部分領域の形状パラメータと前記第１奥側グループに属する実床面部分領域のうち、前記仮傾斜変化部に近接した実床面分領域を除外した実床面部分領域の形状パラメータとに基づいて前記傾斜変化部を特定する第１傾斜変化部特定手段とを備えることを特徴とする請求項８記載の除いた脚式移動ロボット。
【請求項１０】
前記第１傾斜変化部特定手段は、前記第１手前側グループに属する実床面部分領域のうち、前記仮傾斜変化部に近接した実床面部分領域を除外した実床面部分領域の形状パラメータの代表値と、前記第１奥側グループに属する実床面部分領域のうち、前記仮傾斜変化部に近接した実床面分領域を除外した実床面部分領域の形状パラメータの代表値とを決定し、それらの代表値により規定される２つの平面の交線を前記傾斜変化部として特定することを特徴とする請求項９記載の脚式移動ロボット。
【請求項１１】
前記傾斜変化部特定手段は、前記仮代表値により規定される２つの平面の相違の度合いが小さいと判断されたとき、前記各画像投影領域にそれぞれ対応する実床面部分領域を、所定の位置よりも前記ロボットに近い側の第２手前側グループと該第２手前側グループよりもロボットから遠い側の第２奥側グループとに分類する手段と、その分類により得られた第２奥側グループに属する実床面部分領域の形状パラメータと前記現状想定床面のうちのロボットの現在の接地箇所の近辺の床面形状の情報とに基づいて前記傾斜変化部を特定する第２傾斜変化部特定手段とを備えることを特徴とする請求項９または１０記載の脚式移動ロボット。
【請求項１２】
前記第２傾斜変化部特定手段は、前記第２奥側グループに属する実床面部分領域の形状パラメータの代表値を決定する手段と、前記現状想定床面のうちのロボットの現在の接地箇所の近辺の床面と前記第２奥側グループに属する実床面部分領域の形状パラメータの代表値により規定される平面との相違の度合いの大小を判断する手段とを備え、該床面および平面の相違の度合いが大きいと判断したとき、前記第２手前側グループの実床面部分領域のうちの最も第２奥側グループ寄りの実床面部分領域に対応する前記着地予定位置と前記第２奥側グループの実床面部分領域のうちの最も第２手前側グループ寄りの実床面部分領域に対応する前記着地予定位置との間の中間位置を前記傾斜変化部の存在位置として該傾斜変化部を特定することを特徴とする請求項１１記載の脚式移動ロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの進行方向前方の床面を含む外界を撮像するカメラを搭載し、該カメラの撮像画像に基づき前記床面の形状を認識しつつ移動する脚式移動ロボットに関する。
【背景技術】
【０００２】
この種の脚式移動ロボットに関する従来の技術としては、例えば、特許文献１あるいは非特許文献１に見られる技術が知られている。
【０００３】
特許文献１に見られる技術では、カメラによりロボット前方の広角域（例えば１８０度範囲）の外界を撮像し、その撮像画像から、撮像範囲の全域にわたって地形や物体の形状、大きさ、位置を認識する。そして、その認識した情報をロボットのアーム（脚体）の動作制御に反映させる。
【０００４】
また、非特許文献１に見られる技術では、ステレオカメラの撮像画像から、ロボットの周辺の床面をグリッド状に分割してなる各分割域における高さを計測して、該床面の高さマップを作成し、ロボットが移動可能な床面と、避けるべき障害物とを区別して認識するようにしている。
【特許文献１】特開２００２−１６６３８０号公報
【非特許文献１】「A Floor and Obstacle Height Map for 3D Navigation of a Humanoid Robot」／Jens-Steffen Gutmann, Masaki Fukuchi, and Masahiro Fujita／Proceedings of the 2005 IEEE／International Conference on Robotics and Automation／Barcelona, Spain, April 2005
【発明の開示】
【発明が解決しようとする課題】
【０００５】
前記特許文献１および非特許文献１に見られる技術は、一般的に言えば、カメラにより撮像された画像中の床面の全領域にわたって、床面の形状を認識し、その認識した床面の形状をロボットの移動制御に利用するものである。
【０００６】
しかしながら、これらの技術では、撮像画像中の床面の全領域にわたって、床面の形状を認識するため、１フレームの画像に対して多大な演算処理を必要とし、その演算処理に要する時間が長くなりやすい。このため、特にロボットを高速で移動させるような場合には、床面の形状の認識するための演算処理が間に合わず、ロボットの移動制御を適切に行なうことが困難となる恐れがあった。
【０００７】
本発明は、かかる背景に鑑みてなされたものであり、カメラの撮像画像による床面の形状を認識するための演算処理を軽減しつつ、ロボットの適切な移動制御を行なうことを可能とする脚式移動ロボットを提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明の脚式移動ロボットは、かかる目的を達成するために、ロボットの進行方向前方の床面を含む外界を撮像するカメラを搭載し、該カメラの撮像画像に基づき前記床面の形状を推定しつつ移動する脚式移動ロボットにおいて、
前記ロボットの移動時に、該ロボットの現在の動作制御のために使用している床面形状の情報により表される床面である現状想定床面に対する該ロボットの脚体の複数歩分の着地予定位置を設定する着地予定位置設定手段と、
前記設定された各着地予定位置毎に、該着地予定位置の近辺に前記カメラの撮像画像に投影する平坦な画像投影領域を設定する画像投影領域設定手段と、
前記設定された各画像投影領域を前記カメラの撮像画像に投影してなる部分画像領域の画像に基づいて、各部分画像領域毎に、該部分画像領域内に撮像されている実際の床面である実床面部分領域の形状を表す形状パラメータを推定する床面形状推定手段とを備えたことを特徴とする（第１発明）。
【０００９】
この第１発明によれば、ロボットの将来の複数歩分の着地予定位置にそれぞれ対応する部分画像領域毎に、その実床面部分領域の形状パラメータが推定される。つまり、カメラの撮像画像内の部分画像領域に限定して、該部分画像領域内の実床面部分領域の形状パラメータが推定される。このため、その推定処理の演算負荷を軽減できる。そして、その部分画像領域は、ロボットの将来の着地予定位置に対応して設定されるので、ロボットの移動動作に必要な範囲で、実際の床面の形状を特定し得る形状パラメータを推定することができることとなる。従って、第１発明によれば、カメラの撮像画像による床面の形状を認識するための演算処理を軽減することができると共に、ロボットの適切な移動制御を行なうことが可能となる。
【００１０】
この第１発明では、前記各画像投影領域は、前記設定された着地予定位置における前記ロボットの脚体の接地面を含む領域であることが好ましい（第２発明）。
【００１１】
この第２発明によれば、ロボットの将来の脚体の接地面を含んでその近辺の実床面の形状を推定することが可能となる。
【００１２】
また、前記第１発明または第２発明では、前記各部分画像領域内に撮像されている実床面部分領域の形状を表す形状パラメータは、少なくとも該実床面部分領域を包含する平面の空間的な姿勢を表す姿勢パラメータと、該平面の空間的な位置を表す位置パラメータとを含むことが好ましい（第３発明）。
【００１３】
この第３発明によれば、各実床面部分領域の形状パラメータの推定値から、ロボットの進行方向前方の実際の床面が、どの位置でどのように傾斜しているかを認識できることとなる。なお、姿勢パラメータとしては、例えば実床面部分領域を包含する平面の法線ベクトルなど、該平面の法線方向を表すパラメータが挙げられる。また、位置パラメータとしては、例えば、空間内のある特定の代表点から実床面部分領域を包含する平面までの距離、あるいは、該平面上の任意の点の空間的な位置を表すパラメータが挙げられる。
【００１４】
この第３発明では、例えば、前記カメラは２つのカメラから成り、前記床面形状推定手段は、前記設定された各画像投影領域毎に、該画像投影領域に対応する一方のカメラの部分画像領域の画像を、他方のカメラの部分画像領域の画像に一致する画像に変換し得る射影変換行列を特定するように、該画像投影領域に対応する実床面部分領域の形状パラメータを推定する（第４発明）。
【００１５】
ここで、画像投影領域に対応する一方のカメラの部分画像領域の画像を、他方のカメラの部分画像領域の画像に一致する画像に変換し得る射影変換行列は、該部分画像領域に対応する実床面部分領域を包含する平面の空間的な姿勢および位置に依存する。従って、該射影変換行列を特定すれば、実床面部分領域の形状パラメータ（姿勢パラメータおよび位置パラメータ）を特定できる。
【００１６】
この第４発明では、より具体的には、前記床面形状推定手段は、例えば、前記設定された画像投影領域のうちの任意の１つの画像投影領域に対応する実床面部分領域の形状パラメータを推定するとき、該部分画像領域に対応する実床面部分領域の形状パラメータを変数パラメータとし、その変数パラメータの値に応じて定まる仮の射影変換行列により前記一方のカメラの部分画像領域の画像を変換してなる画像と、前記他方のカメラの部分画像領域の画像とを比較しつつ、該変数パラメータの値により規定される平面を前記実床面部分領域を包含する平面に収束させるように該変数パラメータの値を初期値から更新していく収束演算処理を実行し、その収束演算処理により得られた該変数パラメータの収束値を、該部分画像領域に対応する実床面部分領域の形状パラメータの推定値として決定する（第５発明）。
【００１７】
この第５発明によれば、実床面部分領域の形状パラメータの推定値を探索的に求めることができる。この場合、前記変数パラメータの値を更新する都度、その変数パラメータの値に応じて定まる仮の射影変換行列により前記一方のカメラの部分画像領域の画像を変換する処理と、その変換してなる画像と他方のカメラの部分画像領域の画像とを比較する処理とが必要となるが、それらの処理は、両カメラの撮像画像内の部分的な領域に限定されるので、それらの処理を比較的短時間で行なうことができる。
【００１８】
この第５発明では、前記床面形状推定手段は、前記変数パラメータの初期値により規定される平面が前記実床面部分領域を包含する平面に対して傾斜した平面になることを該初期値が満たすべき制約条件とし、該初期値を該制約条件を満たす値に設定する手段を備えることが好ましい（第６発明）。
【００１９】
すなわち、詳細は後述するが、本願発明者の知見によれば、前記収束演算処理で更新していく前記変数パラメータの初期値は、それにより規定される平面（以下、初期平面ということがある）が、実床面部分領域を包含する平面（以下、実平面ということがある）と平行となる場合には、初期平面と実平面との空間的な位置の誤差が大きくなるに伴い、収束演算処理の繰り返し演算の回数が急減に増大する傾向がある。さらに、初期平面と実平面との空間的な位置との誤差がある程度大きくなると、前記変数パラメータの値をある値に収束させることができなくなる。つまり、前記変数パラメータの初期値を、初期平面と実平面とが平行となるような値に設定した場合には、それらの初期平面と実平面との空間的な位置も大きな誤差が生じないように、該初期値を設定しなければ、収束演算処理の演算処理負荷が急減に増大し、あるいは、パラメータの値を収束させることができなくなる。
【００２０】
一方、初期平面が、実平面に対してある程度傾斜するように、射影変換行列のパラメータの初期値を設定した場合には、収束演算処理の繰り返し演算の回数は、それらの初期平面と実平面との空間的な位置の誤差が比較的大きくても、急激に増加することはなく、比較的少ない繰り返し演算の回数で、前記パラメータの値を収束させることができる。
【００２１】
従って、第６発明によれば、前記初期値により規定される初期平面と実平面との位置の誤差が比較的大きくても、収束演算処理の収束性を確保しつつ、演算負荷を軽減することが可能となる。
【００２２】
ここで、ロボットを移動させようとする床面の、水平面からの傾斜角度は、一般に、ある範囲内の値の傾斜角度であり、その傾斜角度が採り得る値の範囲は、ロボットの仕様やロボットの動作環境等に依存して定まる。
【００２３】
そこで、前記第６発明では、前記変数パラメータの初期値により規定される平面を、該平面の水平面に対する傾斜角度が、前記実床面分領域を包含する平面の水平面に対する実際の傾斜角度が採り得る値の範囲から逸脱するようにあらかじめ定められた所定角度となる平面とすることが好ましい（第７発明）。
【００２４】
この第７発明によれば、ロボットを移動させようとする実際の床面における実床面部分領域の傾斜度合いによらずに、前記所定角度を、当該範囲を逸脱した値に定めておき、前記初期平面の傾斜角度が該所定角度となるように該初期平面を規定する前記パラメータの初期値を設定することで、前記制約条件を確実に満たす初期値を設定することができる。
【００２５】
前記第３〜第７発明では、前記推定された各実床面部分領域の形状パラメータに基づいて、前記ロボットの進行方向前方の実際の床面の傾斜度合いが変化する傾斜変化部を特定する傾斜変化部特定手段をさらに備えることが好ましい（第８発明）。
【００２６】
このように、傾斜変化部を特定することで、ロボットの移動時に、該傾斜変化部を避けてロボットの脚体を着地させるようなロボットの歩容を作成することが可能となる。
【００２７】
この第８発明では、より具体的には、前記傾斜変化部特定手段は、例えば、各画像投影領域にそれぞれ対応する実床面部分領域を前記ロボットに近い側の第１手前側グループと該第１手前側グループよりもロボットから遠い側の第１奥側グループとに分類すると共に、その分類を、該第１手前側グループに属する実床面部分領域の形状パラメータの分散と第１奥側グループに属する実床面部分領域の形状パラメータの分散とが最小になるように行なう手段と、
その分類により得られた第１手前側グループに属する実床面部分領域の形状パラメータの仮代表値と第１奥側グループに属する実床面部分領域の形状パラメータの仮代表値とを決定し、それらの仮代表値により規定される２つの平面の相違の度合いが大きいか否かを判断する手段と、
該２つの平面の相違の度合いが大きいと判断されたとき、該２つの平面をそれぞれ規定する仮代表値に基づき、前記傾斜変化部の仮値としての仮傾斜変化部を決定する手段と、
前記第１手前側グループに属する実床面部分領域のうち、前記仮傾斜変化部に近接した実床面部分領域を除外した実床面部分領域の形状パラメータと前記第１奥側グループに属する実床面部分領域のうち、前記仮傾斜変化部に近接した実床面分領域を除外した実床面部分領域の形状パラメータとに基づいて前記傾斜変化部を特定する第１傾斜変化部特定手段とを備える（第９発明）。
【００２８】
この第９発明によれば、第１手前側グループに属する実床面部分領域の形状パラメータの分散（ばらつき度合い）と、第１奥側グループに属する実床面部分領域の形状パラメータの分散（ばらつき度合い）とが最小になるように、各画像投影領域にそれぞれ対応する実床面部分領域を分類するので、各グループに属する実床面部分領域の相互の形状パラメータの近似度合いが高いものとなる。そして、各グループ毎の実床面部分領域の形状パラメータの仮代表値（例えば、平均値）により規定される２つの平面の相違の度合い（例えば、２つの平面の傾斜度合いの差）が大きい場合には、第１手前側グループに属する実床面部分領域と、第１奥側グループに属する実床面部分領域との間の箇所付近に傾斜変化部が存在すると考えられる。そこで、これらの各グループ毎の形状パラメータの仮代表値に基づいて仮傾斜変化部を決定する。この場合、例えば、各グループ毎の形状パラメータの仮代表値により規定される２つの平面の交線を仮傾斜変化部として決定すればよい。
【００２９】
ただし、前記の如く分類された第１手前側グループの実床面部分領域のうちの奥側の実床面部分領域、あるいは、第１奥側グループの実床面部分領域のうちの手前側の実床面部分領域は、その内部に実際の傾斜変化部を含んでいる場合がある。そして、この場合には、該実床面部分領域の形状パラメータの推定値は信頼性が低い。そこで、第９発明では、前記第１手前側グループに属する実床面部分領域のうち、前記仮傾斜変化部に近接した実床面部分領域を除外した実床面部分領域の形状パラメータと前記第１奥側グループに属する実床面部分領域のうち、前記仮傾斜変化部に近接した実床面分領域を除外した実床面部分領域の形状パラメータとに基づいて前記傾斜変化部を特定する。これにより、傾斜変化部を適切に特定することができる。
【００３０】
この第９発明では、前記第１傾斜変化部特定手段は、前記第１手前側グループに属する実床面部分領域のうち、前記仮傾斜変化部に近接した実床面部分領域を除外した実床面部分領域の形状パラメータの代表値（例えば平均値）と、前記第１奥側グループに属する実床面部分領域のうち、前記仮傾斜変化部に近接した実床面分領域を除外した実床面部分領域の形状パラメータの代表値（例えば平均値）とを決定し、それらの代表値により規定される２つの平面の交線を前記傾斜変化部として特定すればよい（第１０発明）。
【００３１】
また、前記第９発明または第１０発明では、前記傾斜変化部特定手段は、前記仮代表値により規定される２つの平面の相違の度合いが小さいと判断されたとき、前記各画像投影領域にそれぞれ対応する実床面部分領域を、所定の位置よりも前記ロボットに近い側の第２手前側グループと該第２手前側グループよりもロボットから遠い側の第２奥側グループとに分類する手段と、その分類により得られた第２奥側グループに属する実床面部分領域の形状パラメータと前記現状想定床面のうちのロボットの現在の接地箇所の近辺の床面形状の情報とに基づいて前記傾斜変化部を特定する第２傾斜変化部特定手段とを備えることが好ましい（第１１発明）。
【００３２】
すなわち、前記仮代表値により規定される２つの平面の相違の度合いが小さいと判断される状況は、主に、当該２つの平面が概ね一致している状況、換言すれば、第１手前側グループに属する実床面部分領域と第１奥側グループに属する実床面部分領域とがいずれの概ね同一平面上に存在すると見なせる状況である。ただし、例えば、実際の床面の傾斜度合いが、ロボットの進行方向に沿って徐々に変化しているような場合にも、前記仮代表値により規定される２つの平面の相違の度合いが小さいと判断される場合がある。そこで、第１１発明では、上記の如く各画像投影領域にそれぞれ対応する実床面部分領域を、第２手前側グループと第２奥側グループとに分類し、その分類により得られた第２奥側グループに属する実床面部分領域の形状パラメータと前記現状想定床面のうちのロボットの現在の接地箇所の近辺の床面形状の情報とに基づいて前記傾斜変化部を特定する。このようにすることにより、実際の床面の傾斜度合いが、ロボットの進行方向に沿って徐々に変化しているような場合でも、その傾斜度合いの変化を近似するようにして傾斜変化部を段階的に特定することが可能となる。
【００３３】
なお、実床面部分領域を、第２手前側グループと第２奥側グループとに分類するための所定の位置は、前記着地予定位置設定手段で設定される着地予定位置の歩数目により規定したり、あるいは、ロボットの現在位置からの距離により規定すればよい。
【００３４】
この第１１発明では、より具体的には、前記第２傾斜変化部特定手段は、例えば、前記第２奥側グループに属する実床面部分領域の形状パラメータの代表値を決定する手段と、前記現状想定床面のうちのロボットの現在の接地箇所の近辺の床面と前記第２奥側グループに属する実床面部分領域の形状パラメータの代表値により規定される平面との相違の度合いの大小を判断する手段とを備え、該床面および平面の相違の度合いが大きいと判断したとき、前記第２手前側グループの実床面部分領域のうちの最も第２奥側グループ寄りの実床面部分領域に対応する前記着地予定位置と前記第２奥側グループの実床面部分領域のうちの最も第２手前側グループ寄りの実床面部分領域に対応する前記着地予定位置との間の中間位置を前記傾斜変化部の存在位置として該傾斜変化部を特定するようにすればよい（第１２発明）。
【００３５】
これにより、傾斜変化部を適切に特定できる。
【００３６】
なお、本発明では、前記現状想定床面のうち、少なくともロボットの進行方向前方で、前記特定された傾斜変化部よりも奥側の現状想定床面を、前記特定された傾斜変化部と、奥側グループ（第１奥側グループまたは第２奥側グループ）に属する実床面部分領域の形状パラメータの代表値とに応じて修正することが望ましい。この場合、傾斜変化部、前記特定された傾斜変化部と、奥側グループ（第１奥側グループまたは第２奥側グループ）に属する実床面部分領域の形状パラメータの代表値とにより表される推定床面（奥側グループに属する実床面部分領域の全体を近似する推定床面）との相違度合い（例えば、傾斜度合いの差）が所定量よりも大きい場合に、現状想定床面を修正することが望ましい。
【発明を実施するための最良の形態】
【００３７】
本発明の一実施形態を図１〜図１１を参照して説明する。
【００３８】
まず、図１および図２を参照して本実施形態における脚式移動ロボットの概要構成を説明する。図１は、本実施形態における脚式移動ロボットとしての２足歩行ロボットの外観斜視図、図２は該ロボットの制御システムの概要を示すブロック図である。
【００３９】
この２足歩行ロボット１は、人型ロボットであり、上体（基体）３、左右一対の脚体５Ｌ，５Ｒ、左右一対の腕体７Ｌ，７Ｒ、および頭部９を備える。なお、本実施形態の説明では、符号「Ｌ」、「Ｒ」は、それぞれロボット１の前方に向かって左側の要素、右側の要素を表す符号として使用する。ただし、左右を特に区別する必要が無いときには、符号「Ｌ」、「Ｒ」を省略する。
【００４０】
脚体５Ｌ，５Ｒは、互いに同じ構造のものであり、それぞれ上体３の下端部の左寄りの部分、右寄りの部分から股関節１１を介して延設されている。そして、各脚体５は、その長手方向の中間部に膝関節１３を有する。また、各脚体５の先端部（下端部）には、足首関節１５を介して接地部としての足平１７が装着されている。本実施形態では、各脚体５の股関節１１、膝関節１３、および足首関節１５は、それぞれ３自由度、１自由度、２自由度を有する。従って、足平１７は、上体３に対して６自由度を有する。
【００４１】
腕体７Ｌ，７Ｒは、互いに同じ構造のものであり、それぞれ上体３の上部の左側部、右側部から肩関節１９を介して延設されている。そして、各腕体７は、その長手方向の中間部に肘関節２１を有する。また、各腕体７の先端部には、手首関節２３を介してハンド（手先部）２５が装着されている。本実施形態では、肩関節１９、肘関節２１および手首関節２３はそれぞれ３自由度、１自由度、２自由度を有する。従って、ハンド２５は、上体３に対して６自由度を有する。
【００４２】
頭部９は、上体３の上端部に搭載されている。この頭部９には、左右一対のカメラ２７Ｌ，２７Ｒが内蔵されている。これらのカメラ２７Ｌ，２７Ｒは例えばＣＣＤカメラにより構成されている。これらのカメラ２７Ｌ，２７Ｒは、頭部９の正面（概ねロボット１の前方）の外界の映像を撮像するものである。そして、カメラ２７Ｌ，２７Ｒは、それらの光軸が頭部９の正面に向かってほぼ同方向に向くように頭部９に固定されている。また、頭部９には、上体３に対して該頭部９を可動とする関節機構（図示省略）が内蔵されている。その関節機構によって、頭部９は、カメラ２７Ｌ，２７Ｒと一体に、上体３に対して可動とされている。例えば、頭部９は、上体３の体幹軸（概ね上下方向の軸）まわりの回転動作と、左右方向の軸まわりの回転動作とが可能とされている。なお、頭部９は、上体３に固定されていてもよい。
【００４３】
また、上体３の背面部には、ボックス２９が装着されており、このボックス２９には、後述する制御装置３３等が収容されている。
【００４４】
ロボット１には、その動作（歩行など）を制御するために図２に示す制御システムが備えられている。この制御システムは、各脚体５、各腕体７および頭部９の上記した各関節をそれぞれ駆動する関節アクチュエータ３１と、この関節アクチュエータ３１を介してロボット１の動作（運動）を制御する制御装置３３と、ロボット１の各種の内部状態量を検出するセンサ３５とを有する。なお、図２では、関節アクチュエータ３１を１つだけ記載しているが、該関節アクチュエータ３１は、各脚体５、各腕体７および頭部９の上記した各関節毎に備えられている。また、各関節アクチュエータ３１は電動モータ等により構成される。
【００４５】
センサ３５は、詳細な図示は省略するが、ロボット１の所定の部位、例えば上体３の３軸方向の加速度を検出する加速度センサ、該上体３の３軸まわりの角速度を検出するレートセンサ、該上体３の傾斜角度（鉛直方向に対する傾斜角度）を検出する傾斜計、ロボット１の各足平１７に床側から作用する床反力（３軸方向の並進力および３軸まわりのモーメント）を検出する力センサ（６軸力センサ）などから構成される。
【００４６】
制御装置３３は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路、関節アクチュエータ３１の駆動回路などを含む電子回路ユニットである。この制御装置３３には、前記カメラ２７Ｌ，２７Ｒのそれぞれの撮像画像が入力されると共に、前記センサ３５の検出信号が入力される。さらに、制御装置３３には、ロボット１の遠隔操縦装置（図示省略）などからロボット１の移動速度および移動方向（進行方向）を規定する指令（以下、速度・方向指令という）が入力される。そして、制御装置３３は、これらの入力されたデータを使用して、所要の演算処理を実行し、ロボット１の目標歩容を生成する。さらに、制御装置３３は、生成した目標歩容に応じて各脚体５の股関節１１、膝関節１３、足首関節１５など、ロボット１に備えた各関節の目標動作を決定する。そして、制御装置３３は、その目標動作に従って、各関節アクチュエータ３１を制御し、ロボット１を移動させる。
【００４７】
また、本実施形態では、制御装置３３は、ロボット１を移動させようとする床に対して固定されたグローバル座標系（ワールド座標系）での該ロボット１の自己位置を、前記センサ３５（例えば加速度センサおよびレートセンサ）の検出信号などに基づいて逐次認識するようにしている。なお、ロボット１の自己位置は、適宜の外界センサを使用して認識するようにしてもよい。例えば、ロボット１を移動させようとする床などにランドマークを付設しておき、カメラ２７Ｌ，２７Ｒの撮像画像中のランドマークに基づいてロボット１の自己位置を認識するようにしてもよい。
【００４８】
また、本実施形態では、制御装置３３は、カメラ２７Ｌ，２７Ｒの撮像画像を使用して、ロボット１を移動させようとする床面の形状を推定する処理も実行する。
【００４９】
なお、本実施形態では、ロボット１の速度・方向指令を遠隔操縦装置から制御装置３３に入力するようにしているが、該速度・方向指令、あるいはこれを規定するロボット１の移動計画を制御装置３３にあらかじめティーチングしておいてもよい。
【００５０】
次に、ロボット１の制御装置３３が実行する制御処理のうち、本発明に関連した制御処理を図３〜図１１を参照して詳説する。制御装置３３は、ロボット１を前記速度・方向指令に応じて移動させる制御処理（ロボット１の動作制御処理）を実行しながら、カメラ２７Ｌ，２７Ｒの撮像画像を基に、ロボット１の進行方向前方における床面の形状を逐次推定する。
【００５１】
図３は、この床面の形状の推定処理（以下、床面形状推定処理という）を示すフローチャートである。この床面形状推定処理は、ロボット１の動作制御と並行して、所定の演算処理周期で逐次実行される処理である。
【００５２】
ここで、以降の説明で用いる用語に関して補足しておく。床面形状推定処理における各演算処理周期をフレーム、現在の演算処理周期を今回フレーム、その１つ前の演算処理周期を前回フレームという。また、制御装置３３がロボット１の動作制御のために現時点で想定している床面（ロボット１の現在位置をその領域内に含む床面）を現状想定床面、実際の床面を実床面、床面形状推定処理で推定された床面を推定床面という。
【００５３】
現状想定床面は、ロボット１の動作制御のために必要な床面形状の情報により規定される床面である。その床面形状の情報は、グローバル座標系における斜面や水平面の位置、斜面の傾斜度合いなどであり、ロボット１の移動開始前に、制御装置３３に入力される。制御装置３３は、基本的には、ロボット１を移動させようとする床面が、現状想定床面に一致していると見なして該ロボット１の動作制御を行なう。この場合、本実施形態では、ロボット１の進行方向前方における現状想定床面は、推定床面との差異（例えば、それらの床面の傾斜角度の差）がある程度大きい場合に、推定床面に応じて更新される。なお、ロボット１の移動開始時におけるロボット１の接地箇所の近辺における現状想定床面は水平面であり、実床面にほぼ一致しているものとする。
【００５４】
また、ロボット１の両足平１７，１７のうち、ロボット１の移動時に、空中に持ち上げる（離床させる）足平１７を遊脚足平１７、他方の足平１７（ロボット１の自重を床に支える脚体５の足平１７）を支持脚足平１７という。ロボット１を連続的に移動させるとき、遊脚足平１７および支持脚足平１７は交互に切り換わる。
【００５５】
また、本実施形態では、カメラ２７Ｌ，２７Ｒのうちの一方のカメラの撮像画像をステレオ画像における基準画像とし、他方のカメラの撮像画像をステレオ画像における参照画像とする。以降の説明では、便宜上、例えばカメラ２７Ｌの撮像画像を基準画像とし、該カメラ２７Ｌを基準カメラ２７Ｌということがある。
【００５６】
以上を前提として、床面形状推定処理を説明する。図３を参照して、この床面形状推定処理では、まず、ＳＴＥＰ１において、制御装置３３は、カメラ２７Ｌ，２７Ｒから左右の各撮像画像（基準画像および参照画像）を取得する。
【００５７】
次いで、ＳＴＥＰ２において、制御装置３３は、入力された前記速度・方向指令と、現状想定床面の形状と、ロボット１の目標歩容を生成するために該制御装置３３が使用している歩容パラメータとに基づいて、ロボット１の遊脚足平１７の将来の着地予定位置を１歩目からＮ歩目まで複数歩分、決定する。１歩目の着地予定位置は、ロボット１の現在の遊脚足平１７の着地予定位置である。このＳＴＥＰ２の処理は、本発明における着地予定位置設定手段を構成する処理である。
【００５８】
本実施形態では、ＳＴＥＰ２で決定する着地予定位置は、例えば１歩目から７歩目までの７歩分の着地予定位置である（Ｎ＝７である）。それらの着地予定位置は、前記グローバル座標系での位置である。また、それらの着地予定位置は、足平１７の代表点の位置を意味する。
【００５９】
ここで、本実施形態では、制御装置３３がロボット１の目標歩容を生成するために使用する歩容パラメータには、１歩目の歩容（以下、今回歩容ということがある）における遊脚足平１７の着地予定位置（現在の遊脚足平１７の着地予定位置）と、その次の歩容である２歩目の歩容（以下、次回歩容ということがある）における遊脚足平１７の着地予定位置（現在の遊脚足平１７の次の遊脚足平１７の着地予定位置）と、今回歩容に続く仮想的な周期的歩容（２歩分の歩容を１周期分の歩容とする周期的な歩容）である定常歩容における１歩毎の足平１４の着地位置とを規定する歩容パラメータを含む。これらの歩容パラメータにより規定される今回歩容および次回歩容の遊脚足平１７の着地予定位置並びに定常歩容における着地位置は、ロボット１に対して固定された座標系（例えばロボット１の現在の支持脚足平１７の接地面内の所定の代表点を原点とする座標系）での位置である。
【００６０】
これらの歩容パラメータにより規定される今回歩容および次回歩容の遊脚足平１７の着地予定位置は、制御装置３３に入力された前記速度・方向指令と、現状想定床面（ロボット１の現在の接地箇所の近辺の現状想定床面）の形状情報とから、あらかじめ定められた所定の規則に従って決定される。この場合、今回歩容および次回歩容の遊脚足平１７の着地予定位置は、それらの位置で該遊脚足平１７を現状想定床面に接地させるように設定される。また、今回歩容に続く仮想的な周期的歩容である定常歩容に関しては、その各周期（２歩分の期間）の１歩目の歩容における支持脚足平１７の着地位置と遊脚足平１７の着地位置とは、それらの間の相対的な位置関係が、次回歩容の支持脚足平１１の着地位置（すなわち、今回歩容の遊脚足平１７の着地予定位置）と次回歩容の遊脚足平１７の着地予定位置との間の相対的位置関係に合致するように決定される。また、定常歩容の各周期の２歩目の歩容における支持脚足平１７の着地位置（すなわち、定常歩容の１歩目の歩容における遊脚足平１７の着地予定位置）と遊脚足平１７の着地位置とは、それらの間の相対的な位置関係が、今回歩容の支持脚足平１７の着地位置と今回歩容の遊脚足平１７の着地予定位置との間の相対的な位置関係に合致するように決定される。
【００６１】
なお、定常歩容は、今回歩容の生成条件（ロボット１の移動を安定化させる上で好ましい条件）を決定するために生成される仮想的な歩容であり、ロボット１の現実の動作制御に使用される歩容ではない。また、定常歩容の１周期分の歩数は、２歩分である必要はなく、３歩以上の歩数を１周期分の歩数としてもよい。このような定常歩容に関しては、例えば本願出願によるＰＣＴ国際公開公報ＷＯ／０２／４０２２４などに詳細に説明されているので、本明細書での詳細な説明は省略する。
【００６２】
そして、ＳＴＥＰ２では、制御装置３３は、３歩目までの着地予定位置を、該制御装置３３がロボット１の目標歩容（今回歩容）の生成のために使用している上記歩容パラメータに基づいて決定する。具体的には、該歩容パラメータにより規定される１歩目の着地予定位置（今回歩容の遊脚足平１７の着地予定位置）および２歩目の着地予定位置（次回歩容の遊脚足平１７の着地予定位置）をグローバル座標系に変換したものを、そのまま、２歩目までの足平１４の着地予定位置として決定する。また、上記歩容パラメータにより規定される定常歩容の１周期目の２歩目の着地位置をグローバル座標系に変換したものを３歩目の着地予定位置として決定する。
【００６３】
さらに、制御装置３３は、４歩目からＮ歩目（本実施形態ではＮ＝７）までの着地予定位置については、入力された前記速度・方向指令と、現状想定床面の形状情報とから、所定の規則に従って決定する。その規則は、１歩目および２歩目の着地予定位置を決定するための規則と同じである。なお、これらの着地予定位置を決定するに際しては、現在の速度・方向指令と、現状想定床面とが、そのまま一定に保持されると仮定される。
【００６４】
ＳＴＥＰ２の処理による１歩目からＮ歩目までの着地予定位置の設定例を図５、図６（ａ），（ｂ）、図７、図８（ａ），（ｂ）を参照して説明する。図５は、ロボット１および実床面などをロボット１の矢状面（側面視）で示した説明図、図６（ａ），（ｂ）は、図５に示す実床面などを鉛直方向上方から見た図、図７は、ロボット１および実床面などをロボット１の矢状面（側面視）で示した説明図、図８（ａ），（ｂ）は、図７に示す実床面などを鉛直方向上方から見た図である。この場合、図５では、ロボット１が現時点で接地している実床面が水平面であると共に、ロボット１の進行方向前方のある位置Ｑ１から遠方側の実床面が水平面から斜面（図では昇り斜面）に変化している例を示している。また、図７では、ロボット１が現時点で接地している実床面が斜面（図では昇り斜面）であると共に、ロボット１の進行方向前方のある位置Ｑ２から遠方側の実床面が斜面から水平面に変化している例を示している。なお、「斜面」は、水平面に対して傾斜した床面を意味する。また、本実施形態の説明では、ロボット１の進行経路上のある区間における床面（実床面または現状認識床面または推定床面）が、その区間内で実質的に一定の傾斜角度（水平面に対する傾斜角度）を有する斜面となっている場合において、ロボット１の進行経路上での該斜面の両端を斜面エッジという。該斜面エッジは、本発明における傾斜変化部に相当するものである。そして、それらの斜面エッジのうち、ロボット１の移動開始地点により近い側の斜面エッジを斜面開始エッジ、他方の斜面エッジを斜面終了エッジという。例えば、図５に示す実床面では、該実床面が水平面から斜面に変化する位置Ｑ１における該水平面と斜面との交線が斜面開始エッジとなる。また、図７に示す実床面では、該実床面が斜面から水平面に変化する位置Ｑ２における該斜面と水平面との交線が斜面終了エッジとなる。
【００６５】
実床面が図５に示すような床面である場合において、現状想定床面が水平面（ロボット１が現時点で接地している実床面（水平面）に連続する水平面）であるとする。この状況では、例えば、図５および図６（ａ）で破線で示すように、１歩目からＮ歩目（Ｎ＝７）までの遊脚足平１７(1)〜１７(N)の着地予定位置が設定される。この場合、ロボット１の１歩毎の歩幅は、ほぼ一定である。なお、本実施形態の説明では、添え字（ｉ）（ｉ＝１，２，……，Ｎ）は、ｉ歩目の着地予定位置に対応するものであることを意味する。
【００６６】
また、実床面が図５に示すような床面である場合において、現状想定床面が実床面にほぼ一致しているものとする。この場合には、例えば図６（ｂ）に破線で示すように、各歩数目の着地予定位置におけるロボット１の遊脚足平１７(1)〜１７(N)（Ｎ＝７）が斜面開始エッジに載らず、且つ、該斜面開始エッジに近づき過ぎないように、１歩目からＮ歩目までの着地予定位置が設定される。
【００６７】
また、実床面が図７に示すような床面である場合において、現状認識床面が斜面（ロボット１が現時点で接地している実床面（斜面）に連続する斜面）であるとする。この状況では、例えば、図７および図８（ａ）で破線で示すように、１歩目からＮ歩目（Ｎ＝７）までの遊脚足平１７(1)〜１７(N)の着地予定位置が設定される。この場合、ロボット１の１歩毎の歩幅は、ほぼ一定である。ただし、その歩幅は、図５および図６（ａ）の場合（現状認識床面が水平面である場合）よりも小さな歩幅とされる。
【００６８】
また、実床面が図７に示すような床面である場合において、現状認識床面が実床面にほぼ一致しているものとする。この場合には、例えば図８（ｂ）に破線で示すように、各歩数目の着地予定位置におけるロボット１の遊脚足平１７(1)〜１７(N)（Ｎ＝７）が斜面終了エッジに載らず、且つ、該斜面終了エッジに近づき過ぎないように、１歩目からＮ歩目までの着地予定位置が設定される。
【００６９】
以上のようにＮ歩目までの各着地予定位置は、遊脚足平１７が斜面開始エッジあるいは斜面終了エッジに載るのを避けるように設定される。
【００７０】
補足すると、本実施形態では、３歩目の着地予定位置を定常歩容の歩容パラメータに対応させて決定したが、４歩目以降の着地予定位置の決定の仕方と同様に３歩目の着地予定位置を決定してもよい。Ｎ歩目までの各着地予定位置は、基本的には、速度・方向指令および現状想定床面が将来も現状に維持されると仮定して、Ｎ歩目までのロボット１の平均的な移動速度と移動方向とが速度・方向指令に合致し、また、両脚体５，５どうしの干渉等に関する幾何学的な制約条件を満足しつつ、現状想定床面上でロボット１を安定に移動させ得るように設定すればよい。
【００７１】
なお、ロボット１の現在の遊脚足平１７が着地するまでは、速度・方向指令と、現状想定床面とが一定に保持されている限り、いずれのフレームにおいても、ＳＴＥＰ２で設定される１歩目からＮ歩目までの各着地予定位置は同じになる。一方、現在の遊脚足平１７が着地するまでに、速度・方向指令が変化し、あるいは、現状想定床面が更新された場合には、ＳＴＥＰ２で設定される１歩目からＮ歩目までの各着地予定位置で設定される各着地予定位置は、経時的に変化する。また、現在の遊脚足平１７が着地した後には、少なくともＮ歩目の着地予定位置は、新規に設定されることとなる。
【００７２】
上記のようにＳＴＥＰ２の処理を実行した後、制御装置３３は、次にＳＴＥＰ３の処理を実行する。このＳＴＥＰ３の処理においては、制御装置３３は、ＳＴＥＰ２で決定した１歩目からＮ歩目（Ｎ＝７）までの各着地予定位置の近辺に画像投影領域Ａ(i)（ｉ＝１，２，……，Ｎ）を設定する。各画像投影領域Ａ(i)は、後述するようにカメラ２７Ｌ，２７Ｒの各撮像画像に投影される領域である。そして、該画像投影領域Ａ(i)は、現状想定床面上に設定される平面領域である。なお、このＳＴＥＰ３の処理は、本発明における画像投影領域設定手段を構成する処理である。
【００７３】
図９は、各画像投影領域Ａ(i)の設定例を示す図である。この図９には、現状想定床面に垂直な方向で見た画像投影領域Ａ(i)（ｉ＝１，２，……，Ｎ）と、ｉ歩目の着地予定位置で現状想定床面に接地させた足平１７(i)とを図示している。図示の如く、画像投影領域Ａ(i)は、それに対応する着地予定位置で現状想定床面に足平１７(i)（図示例では右側足平１７Ｒ）の底面の平坦面の全体を接地させたときの該足平１７(i)の接地面の近辺で該接地面を包含するように設定される。
【００７４】
より具体的には、本実施形態では、画像投影領域Ａ(i)は、例えば長方形状に設定される。この場合、ロボット１の進行方向における画像投影領域Ａ(i)の長さＬ１は、該進行方向における足平１７(i)の接地面の長さＬ３よりも若干大きいものとされている。また、ロボット１の進行方向と直交する方向における画像投影領域Ａ(i)の長さＬ２は、現状想定床面に対する足平１７(i)の接地状態を維持したまま、他方の足平１７（図９に示す例では、二点鎖線で示す左側足平１７Ｌ）を足平１７(i)と左右方向に並列させて現状想定床面に接地させたとした場合に、両足平１７，１７の接地面が両者とも、画像投影領域Ａ(i)に包含されるような長さ（ロボット１の進行方向で見た左側足平１７Ｌの左側縁と、右側足平１７Ｒの右側縁との間隔Ｌ４よりも若干大きい長さ）に設定される。
【００７５】
なお、図９に示す例では、説明の便宜上、足平１７の底面の全体が接地面（平坦面）であるように記載しているが、足平１７の底面の全体が平坦面である必要はない。例えば、足平１７の底面のうちの前端部および後端部が上向きに湾曲しており、該前端部および後端部を除く底面が平坦面になっていてもよい。あるいは、足平１７の底面に複数の面一の平坦面が形成されていてもよい。これらの場合には、足平１７の接地面は、該足平１７の底面の一部分となる。そして、その場合には、ｉ歩目の画像投影領域は、足平１７(i)の全体を現状想定床面に投影した領域のうち、該足平１７(i)の接地面以外の領域がはみ出すように設定されていてもよい。
【００７６】
また、画像投影領域Ａ(i)は、必ずしも長方形状に設定する必要はない。また、例えば、図９に仮想線で示すＡ(i)’のように、足平１７(i)の接地面を包含し、且つ、該接地面よりも若干大きな面積を有する領域（その領域内に１つの足平１７(i)の接地面だけを包含するような領域）をｉ歩目の画像投影領域として設定するようにしてもよい。
【００７７】
上記のように画像投影領域Ａ(i)を設定したとき、例えば、図５に示す状況で、現状想定床面が水平面である場合には、図５および図６（ａ）に示す如く、１歩目からＮ歩目（Ｎ＝７）までの画像投影領域Ａ(1)〜Ａ(N)が設定される。また、現状想定床面が、図５の実床面にほぼ一致している場合には、図６（ｂ）に示す如く、１歩目からＮ歩目までの画像投影領域Ａ(1)〜Ａ(N)が設定される。
【００７８】
また、例えば図７に示す状況で、現状想定床面が斜面である場合には、図７および図８（ａ）に示す如く、１歩目からＮ歩目（Ｎ＝７）までの画像投影領域Ａ(1)〜Ａ(N)が設定される。また、現状想定床面が、図７の実床面にほぼ一致する場合には、図８（ｂ）に示す如く、１歩目からＮ歩目までの画像投影領域Ａ(1)〜Ａ(N)が設定される。
【００７９】
次いで、制御装置３３は、ＳＴＥＰ４の処理を実行する。このＳＴＥＰ４の処理では、制御装置３３は、ＳＴＥＰ２で設定した各画像投影領域Ａ(i)（ｉ＝１，２，……，Ｎ）をカメラ２７Ｌ，２７Ｒの各撮像画像に投影する。なお、この場合、各画像投影領域Ａ(i)を各カメラ２７の撮像画像に投影するために、制御装置３３は、グローバル座標系でのカメラ２７Ｌ，２７Ｒの位置および姿勢、あるいは、カメラ２７Ｌまたは２７Ｒに対して固定された座標系での各画像投影領域Ａ(i)の位置および姿勢を、グローバル座標系でのロボット１の現在位置と、該ロボット１の各関節の変位量（検出値または目標値）とに基づいて認識する。なお、「姿勢」は、空間的な向きを意味する。
【００８０】
ＳＴＥＰ４の処理により、例えば図１０に例示する如く、各撮像画像中に、各画像投影領域Ａ(i)を該撮像画像に投影してなる部分画像領域Ａc(i)が得られる。なお、図１０は、右カメラ２７Ｒの撮像画像を示している。
【００８１】
各部分画像領域Ａc(i)内の画像は、ロボット１の進行方向前方に存在する実床面の部分領域の画像である。以降、各画像測定領域Ａc(i)内に画像として捉えられている実床面の部分領域を実床面部分領域Ａr(i)という。
【００８２】
補足すると、画像投影領域Ａ(i)における現状想定床面が実床面に一致している場合には、該画像投影領域Ａ(i)に対応する実床面部分領域Ａr(i)は、画像投影領域Ａ(i)に一致する。一方、画像投影領域Ａ(i)内の現状想定床面が実床面に一致していない場合には、該画像投影領域Ａ(i)に対応する部分画像領域Ａc(i)内の画像、すなわち、実床面部分領域Ａr(i)の画像は、実床面のうちの、画像投影領域Ａ(i)内の各点と各カメラ２７の光学中心とを結ぶ直線上に位置する部分領域の画像となる。例えば、図５に示す状況で、現状想定床面が水平面である場合には、６歩目および７歩目の画像投影領域Ａ(6)，Ａ(7)にそれぞれ対応する実床面部分領域Ａr(6)，Ａr(7)は、それぞれ図５に示す如く、実床面のうちの斜面内の部分領域となる。そして、これらの実床面部分領域Ａr(6)，Ａr(7)の画像がそれぞれ部分画像領域Ａc(6)，Ａc(7)内の画像となる。同様に、図７に示す状況で、現状想定床面が斜面である場合には、６歩目および７歩目の画像投影領域Ａ(6)，Ａ(7)にそれぞれ対応する実床面部分領域Ａr(6)，Ａr(7)は、それぞれ図７に示す如く、実床面のうちの水平面内の部分領域となる。
【００８３】
なお、１歩目からＮ歩目（Ｎ＝７）までの全ての画像投影領域Ａ(i)に対応する部分画像領域Ａc(i)が、各カメラ２７の実際の撮像画像内に含まれるとは限らない。
【００８４】
次いで、制御装置３３は、ＳＴＥＰ５の処理を実行する。この処理は、各画像投影領域Ａ(i)に対応する部分画像領域Ａc(i)内に捉えられている実床面、すなわち、実床面部分領域Ａr(i)の形状を表す形状パラメータを推定する処理である。このＳＴＥＰ５の処理は、本発明における床面形状推定手段を構成する処理である。
【００８５】
ここで、この推定処理の概要を説明しておく。本実施形態では、各画像投影領域Ａ(i)に対応する実床面部分領域Ａr(i)が、ある１つの平面に包含される領域であると見なし、該実床面部分領域Ａr(i)の空間的（３次元的）な姿勢を表す床面姿勢パラメータと、該実床面部分領域Ａr(i)の法線方向における該実床面部分領域Ａr(i)の空間的な位置を表す床面位置パラメータとの組を該実床面部分領域Ａr(i)の形状パラメータとして用いる。
【００８６】
一般に、空間内の任意の平面は、その法線方向と、その法線方向における該平面の位置とから一義的に特定される。そこで、本実施形態では、実床面部分領域Ａr(i)の床面姿勢パラメータとして、該実床面部分領域Ａr(i)を包含する平面の法線ベクトル（単位ベクトル）ｎ(i)を使用する。また、本実施形態では、ロボット１のある代表点と実床面部分領域Ａr(i)を包含する平面との距離ｄ(i)（以下、平面距離ｄ(i)という）を床面位置パラメータとして使用する。この場合、平面距離ｄ(i)に関するロボット１の代表点として、例えば、カメラ２７Ｒ，２７Ｌのうちの基準カメラ２７Ｌの光学中心を使用する。
【００８７】
従って、ＳＴＥＰ５の処理は、各実床面部分領域Ａr(i)毎に、それを包含する平面の法線ベクトルｎ(i)と平面距離ｄ(i)との組（ｎ(i)，ｄ(i)）を実床面部分領域Ａr(i)の形状パラメータとして推定する処理である。なお、本実施形態では、ＳＴＥＰ５で推定される法線ベクトルｎ(i)は、各フレームにおいて、例えばカメラ２７Ｌ，２７Ｒに対して固定された３次元座標系（以下、カメラ座標系という）における各座標軸成分の組として記述される。そのカメラ座標系は、例えば、基準カメラ２７Ｌの光学中心を原点とする３次元直交座標系である。この場合、平面距離ｄ(i)は、カメラ座標系の原点から実床面部分領域Ａr(i)を包含する平面までの距離となる。
【００８８】
前記図５および図７には、上記の如く定義される形状パラメータの例を図示している。図５中のｎ(2)、ｄ(2)は、２歩目の画像投影領域Ａ(2)に対応する実床面部分領域Ａr(2)（図示例では、Ａr(2)はＡ(2)にほぼ一致している）の形状パラメータとしての法線ベクトルｎ(2)と、平面距離ｄ(2)とを例示している。同様に、図５中のｎ(6)、ｄ(6)は、６歩目の画像投影領域Ａ(6)に対応する実床面部分領域Ａr(6)の形状パラメータとしての法線ベクトルｎ(6)と、平面距離ｄ(6)とを例示している。また、図７中のｎ(3)、ｄ(3)は、３歩目の画像投影領域Ａ(3)に対応する実床面部分領域Ａr(3)（図示例では、Ａr(3)はＡ(3)にほぼ一致している）の形状パラメータとしての法線ベクトルｎ(3)と、平面距離ｄ(3)とを例示している。同様に、図７中のｎ(6)、ｄ(6)は、６歩目の画像投影領域Ａ(6)に対応する実床面部分領域Ａr(6)の形状パラメータとしての法線ベクトルｎ(6)と、平面距離ｄ(6)とを例示している。
【００８９】
さらに、ＳＴＥＰ５の処理では、各実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）を、カメラ２７Ｌ，２７Ｒのそれぞれの撮像画像間の射影変換を利用した収束演算処理により推定する。その収束演算処理では、参照画像（カメラ２７Ｒの撮像画像）中の部分画像領域Ａc(i)内の画像を、形状パラメータ（ｎ(i)，ｄ(i)）の値に応じて定まる射影変換行列によって変換してなる画像を、基準画像（カメラ２７Ｌの撮像画像）中の部分画像領域Ａc(i)内の画像と比較しつつ、それらの画像がほぼ一致するように、（ｎ(i)，ｄ(i)）の値をある初期値から徐々に更新していく。換言すれば、（ｎ(i)，ｄ(i)）を変数パラメータとし、その変数パラメータの値により規定される平面を、実床面部分領域Ａr(i)を包含する平面に収束させるように、該変数パラメータの値をある初期値から更新していく（ひいては、該変数パラメータの値により定まる射影変換行列を更新していく）。この収束演算処理による変数パラメータの最終的な収束値が、実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の推定値として得られる。なお、その変数パラメータの収束値より定まる射影変換行列は、参照画像中の部分画像領域Ａc(i)内の画像を、基準画像中の部分画像領域Ａc(i)内の画像に一致する画像に変換し得る射影変換行列としての意味を持つ。
【００９０】
図４は、このＳＴＥＰ５の処理の詳細を示すフローチャートである。この処理は、ＳＴＥＰ３で設定された各画像投影領域Ａ(i)毎に実行される処理である。以降、このＳＴＥＰ５の処理の説明では、着地予定位置の歩数目を示す参照符号ｉは、１〜Ｎ（Ｎ＝７）のうちの任意の１つの値を意味するものとして使用する。また、前回フレームで設定された各画像投影領域Ａ(1)〜Ａ(N)を前回画像投影領域Ａ_p(1)〜Ａ_p(N)と称する。添え字「_p」は前回フレームに関するものであるという意味で使用する。
【００９１】
ＳＴＥＰ５の処理では、制御装置３３は、まず、ＳＴＥＰ５０１において、Ｎ個の前回画像投影領域Ａ_p(1)〜Ａ_p(N)の中から、今回フレームにおけるｉ歩目の着地予定位置に対応する画像投影領域Ａ(i)に最も近い前回画像投影領域Ａ_px（Ａ_pxはＡ_p(1)〜Ａ_p(N)のうちの１つ）を探索する。ここで、「最も近い」というのは空間的な位置が最も近いことを意味する。より具体的には、制御装置３３は、画像投影領域Ａ(i)の中心点と、Ｎ個の前回画像投影領域Ａ_p(1)〜Ａ_p(N)のそれぞれの中心点との間の距離を、それらの中心点のグローバル座標系での位置に基づいて算出し、その距離が最小となる１つの前回画像投影領域を、Ａ(i)に最も近い前回画像投影領域Ａ_pxとする。以降、この前回画像投影領域Ａ_pxを近接前回画像投影領域Ａp_xという。
【００９２】
次いで、ＳＴＥＰ５０２に進んで、制御装置３３は、画像投影領域Ａ(i)と、近接前回画像投影領域Ａ_pxとの位置差が所定の閾値以上であるか否かを判断する。なお、Ａ(i)とＡ_pxとの位置差は、ＳＴＥＰ５０１で算出した、Ａ(i)の中心点とＡ_pxの中心点との距離である。そして、制御装置３３は、この判断結果が否定的である場合には、ＳＴＥＰ５０３、５０４の処理を実行し、肯定的である場合には、ＳＴＥＰ５０５〜５１０の処理を実行する。
【００９３】
補足すると、ＳＴＥＰ５０１〜５１０の処理は、後述する収束演算処理における（ｎ(i)，ｄ(i)）の初期値を設定するための処理である。
【００９４】
ここで、ＳＴＥＰ５０２の判断結果が否定的となる状況は、画像投影領域Ａ(i)と近接前回画像投影領域Ａ_pxとが重なる部分を有し、Ａ(i)に対応する実床面部分領域Ａr(i)を包含する平面と、Ａ_pxに対応する実床面部分領域Ａr_pxを包含する平面とが一致、もしくはほぼ一致することが高い確度で予測される状況である。逆に、ＳＴＥＰ５０２の判断結果が肯定的となる状況は、画像投影領域Ａ(i)と近接前回画像投影領域Ａ_pxとが重なる部分とが重なる部分を持たず（換言すれば前回フレームにおいて既に形状パラメータの推定がなされた実床面画像領域Ａr_pxとの関連性が無く）、Ａ(i)に対応する実床面部分領域Ａr(i)を包含する平面の空間的な姿勢および位置を現時点では高い確度で予測することが困難な状況を意味する。なお、ＳＴＥＰ５０２の判断結果が肯定的となる状況は、今回フレームで新たな歩数目（前回フレームでのＮ歩目の次の歩数目）の着地予定位置に対応する画像投影領域が設定された場合や、前回フレームと今回フレームとの間でロボット１の進行方向前方の現状想定床面が更新された場合等に発生する。
【００９５】
ＳＴＥＰ５０２の判断結果が否定的である場合のＳＴＥＰ５０３、５０４の処理では、画像投影領域Ａ(i)に対応する実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）を収束演算処理により推定するために、その初期値（n0(i)，ｄ0(i)）が設定される。より具体的には、ＳＴＥＰ５０３では、制御装置３３は、近接前回画像投影領域Ａ_pxに対応して、前回フレームで推定された形状パラメータ（ｎ_px，ｄ_px）（これは、前回フレームにおけるカメラ座標系で記述される）を今回フレームにおけるカメラ座標系に変換し、その変換後の形状パラメータを（ｎ_px'，ｄ_px')とする。なお、前回フレームのカメラ座標系と今回フレームのカメラ座標系との間の関係は、前回フレームと今回フレームとの間の期間におけるロボット１の各関節の変位量に基づいて認識される。
【００９６】
次いで、ＳＴＥＰ５０４において、制御装置３３は、上記の如く求めた（ｎ_px'，ｄ_px')を、実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の初期値（n0(i)，ｄ0(i)）として設定する。このように、（n0(i)，ｄ0(i)）を設定するということは、実床面部分領域Ａr(i)を包含する平面の初期値を、近接前回画像投影領域Ａ_pxに対応する実床面部分領域Ａr_pxを包含する平面に一致させるということを意味する。
【００９７】
一方、ＳＴＥＰ５０２の判断結果が肯定的である場合のＳＴＥＰ５０５〜５１０の処理では、（ｎ(i)，ｄ(i)）の初期値（n0(i)，ｄ0(i)）が次のように設定される。すなわち、ＳＴＥＰ５０５において、制御装置３３は、近接前回画像投影領域Ａ_pxに対応して、前回フレームで推定された形状パラメータ（ｎ_px，ｄ_px）により規定される平面πxをグローバル座標系で求める（グローバル座標系での平面πxの位置および姿勢を求める）。より具体的には、例えば、前回フレームのカメラ座標系で記述された形状パラメータ（ｎ_px，ｄ_px）を、前記ＳＴＥＰ５０３と同じ処理によって、今回フレームにおけるカメラ座標系に変換する。そして、その変換後の形状パラメータ（ｎ_px’，ｄ_px’）と、今回フレームにおけるカメラ座標系とグローバル座標系との間の関係とに基づいて、平面πxをグローバル座標系で記述する。あるいは、前回フレームのカメラ座標系で記述された形状パラメータ（ｎ_px，ｄ_px）と、前回フレームにおけるカメラ座標系とグローバル座標系との間の関係とに基づいて、平面πxをグローバル座標系で記述する。なお、各フレームにおけるカメラ座標系とグローバル座標系との間の関係は、各フレームにおけるロボット１のグローバル座標系上の位置と、該フレームにおける該ロボット１の各関節の変位量とに基づいて認識できる。
【００９８】
次いで、ＳＴＥＰ５０６に進んで、制御装置３３は、画像投影領域Ａ(i)の中心点と、基準カメラ２７Ｌの光学中心とを通る直線Ｌ(i)をグローバル座標系で求める。すなわち、該直線Ｌ(i)をグローバル座標系で記述する。
【００９９】
次いで、ＳＴＥＰ５０７に進んで、制御装置３３は、平面πxと直線Ｌ(i)との交点Ｐ(i)を求める。すなわち、交点Ｐ(i)のグローバル座標系での位置を求める。なお、例えば、今回フレームにおけるカメラ座標系での交点Ｐ(i)の位置を求めておき、それをグローバル座標系に変換するようにしてもよい。
【０１００】
次いで、ＳＴＥＰ５０８に進んで、上記の如く求めた交点Ｐ(i)を通って、水平面から所定角度θだけ傾いた平面πa(i)をグローバル座標系で求める。該所定角度θは、本実施形態では、例えば４５度である。この場合、交点Ｐ(i)を通って、所定角度θだけ傾いた平面は無数に存在するが、そのいずれの平面をπa(i)として求めてもよい。一例として、本実施形態では、交点Ｐ(i)を通ってグローバル座標系のＹ軸方向あるいはロボット１の左右方向に平行な水平直線を含んで、水平面から所定角度θだけ傾いた平面（ロボット１から見て昇り斜面または降り斜面となる平面）をπa(i)として求めるようにした。なお、平面πa(i)が通る（平面πa(i)上に存在する）点Ｐ(i)は、必ずしも上記のように設定せずともよく、例えば画像投影領域Ａ(i)内の任意の点をＰ(i)として設定してもよい。また、例えば、近接前回画像投影領域Ａ_pxに対応して前回フレームで推定された形状パラメータ（ｎ_px，ｄ_px）を今回フレームにおけるカメラ座標系に変換してなる（ｎ_px’，ｄ_px’）のｄ_px'を、と同じ値にｄ0(i)を設定するようにしてもよい。この場合には、平面πa(i)が通る点Ｐ(i)を決定する必要はない。
【０１０１】
次いで、Ｓ５０９に進んで、今回フレームにおけるカメラ座標系で平面πa(i)を表す形状パラメータ（ｎa(i)，ｄa(i)）を求める。ｎa(i)は、πa(i)のカメラ座標系での法線ベクトル（単位ベクトル）であり、ｄa(i)は、今回フレームにおける基準カメラ２７Ｌの光学中心と平面πa(i)との距離である。
【０１０２】
次いで、ＳＴＥＰ５１０に進んで、（ｎa(i)，ｄa(i)）を実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の初期値（n0(i)，ｄ0(i)）として設定する。
【０１０３】
このように、ＳＴＥＰ５０２の判断結果が肯定的となると場合、すなわち、Ａ(i)に対応する実床面部分領域Ａr(i)を包含する平面の空間的な姿勢および位置を現時点では高い確度で予測することが困難な状況である場合には、形状パラメータ（ｎ(i)，ｄ(i)）の初期値（n0(i)，ｄ0(i)）は、それにより規定される平面が水平面に対して所定角度θだけ傾いた平面になるように設定される。
【０１０４】
この場合の初期値（n0(i)，ｄ0(i)）の設定例を図１１を参照して説明する。図１１はその説明図であり、ロボット１や実床面などを矢状面（側面視）で示している。この例では、今回フレームにおける６歩目の画像投影領域Ａ(6)に最も近い近接前回画像投影領域Ａ_pxが前回フレームにおける５歩目の画像投影領域Ａ_p(5)であるとする。また、この例では、前回フレームにおける５歩目の着地予定位置の近辺での実床面と現状想定床面とがほぼ一致しており、ひいては、近接前回画像投影領域Ａ_px（＝Ａ_p(5)）に対応する実床面部分領域Ａr_p(5)がＡ_pxにほぼ一致しているものとしている。
【０１０５】
この場合には、今回フレームにおける６歩目の画像投影領域Ａ(5)に関して、図中に示す平面πx（実床面部分領域Ａr_p(5)を包含する平面）が、ＳＴＥＰ５０５において求められる。そして、図中に示す直線Ｌ(6)がＳＴＥＰ５０６において求められる。さらに、図中に示す点Ｐ(6)が、ＳＴＥＰ５０７において求められる。なお、この例では、点Ｐ(6)は、６歩目の画像投影領域Ａ(6)の中心点に一致している。そして、図中に示す平面πa(6)が、ＳＴＥＰ５０８で求められる。この平面πa(6)は、点Ｐ(6)を通り、水平面に対して所定角度θ（例えば４５度）だけ傾いている。さらに、この平面πa(6)を表す形状パラメータ（ｎa(6)，ｄa(6)）、すなわち、平面πa(6)の法線ベクトルｎa(6)と、今回フレームにおける基準カメラ２７Ｌの光学中心から平面πa(6)までの平面距離ｄa(6)との組がＳＴＥＰ５０９で求められる。そして、この形状パラメータ（ｎa(6)，ｄa(6)）が、６歩目の画像投影領域Ａ(6)に対応する実床面部分領域Ａr(6)の形状パラメータ（ｎ(6)，ｄ(6)）の初期値（ｎ0(6)，ｄ0(6)）として設定される。
【０１０６】
なお、ＳＴＥＰ５０２の判断結果が肯定的となると場合に、形状パラメータ（ｎ(i)，ｄ(i)）の初期値（n0(i)，ｄ0(i)）を上記のように設定する理由については、後述する。
【０１０７】
以上説明したＳＴＥＰ５０１〜５１０の処理が、（ｎ(i)，ｄ(i)）の初期値（n0(i)，ｄ0(i)）を設定するための処理である。
【０１０８】
以上のように初期値（n0(i)，ｄ0(i)）を設定した後、ＳＴＥＰ５１２に進んで、制御装置３３は、実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の仮値として、初期値（n0(i)，ｄ0(i)）を設定する。なお、このＳＴＥＰ５１２からの処理が、形状パラメータ（ｎ(i)，ｄ(i)）を推定するための収束演算処理である。この収束演算処理は、本実施形態では、ガウスニュートン法の収束演算処理である。
【０１０９】
次いで、ＳＴＥＰ５１３に進んで、更新前誤差量ｅ(k-1)を初期化する。ここで初期化される更新前誤差量ｅ(k-1)は、後述するループ処理における最初のＳＴＥＰ５１６の処理で求められる誤差量ｅ(k)と比較するものであり、その誤差量ｅ(k)よりも確実に大きな誤差量となるような過大な誤差量に設定される。
【０１１０】
次いで、ＳＴＥＰ５１４からＳＴＥＰ５２２までの処理を所要の条件が満たされるまで繰り返すループ処理が実行される。このループ処理では、形状パラメータ（ｎ(i)，ｄ(i)）は、変数パラメータとしての意味を持ち、適宜、その値が更新される。以降、そのループ処理における各回のＳＴＥＰ５１４〜５２２の処理をループ内演算処理、現在実行中のループ内演算処理を今回ループ内演算処理、その１回前のループ内演算処理を前回ループ内演算処理という。また、今回ループ内演算処理で求められる値と、前回ループ内演算処理で求められる値とを区別するために、必要に応じて前者の値に(k)を付し、後者の値に(k-1)を付する。また、（ｎ(i)，ｄ(i)）は、その値が確定されるまでは、変数パラメータ（ｎ(i)，ｄ(i)）という。
【０１１１】
ＳＴＥＰ５１４において、制御装置３３は、変数パラメータ（ｎ(i)，ｄ(i)）の現在値から、カメラ２７Ｌ，２７Ｒ間の射影変換行列Ｈ(i)を算出する。該射影変換行列Ｈ(i)は、次式（１）により与えられる行列である。なお、ここでは、Ｈ(i)は、正規化されたカメラ座標系で表現されている。
【０１１２】
Ｈ(i）＝Ｒ＋ｔ・ｍ(i)T        ……（１）
但し、ｍ(i)≡ｎ(i)／ｄ(i)
ここで、Ｒは、カメラ２７Ｌ，２７Ｒ間の回転行列、ｔはカメラ２７Ｌ，２７Ｒのそれぞれの光学中心間の並進ベクトル（ここでは縦ベクトル）である。これらは、カメラ２７Ｌ，２７Ｒの相対的な位置および姿勢関係などの内部パラメータに依存するものであり、あらかじめキャリブレーションによって特定されている。また、ｍ(i)は式（１）の但し書きで定義される如く、法線ベクトルｎ(i）を平面距離ｄ(i)により除算したベクトル（ここでは縦ベクトル）である。なお式（１）中の「Ｔ」は転置を意味する。
【０１１３】
式（１）から明らかなように、射影変換行列Ｈ(i)は、変数パラメータ（ｎ(i)，ｄ(i)）の値によって規定される。
【０１１４】
次いで、Ｓ５１５に進んで、制御装置３３は、カメラ２７Ｌ，２７Ｒの撮像画像のうちの参照画像（カメラ２７Ｒの撮像画像）における画像投影領域Ａ(i)の投影画像、すなわち該参照画像における部分画像領域Ａc(i)内の画像を、上記の如く求めた射影変換行列Ｈ(i)により変換し、その変換により得られた画像を変換画像として得る。
【０１１５】
次いで、ＳＴＥＰ５１６に進んで、制御装置３３は、基準画像（カメラ２７Ｌの撮像画像）における画像投影領域Ａ(i)の投影画像、すなわち該基準画像における部分画像領域Ａc(i)内の画像と、上記ＳＴＥＰ５１５で求めた変換画像との間の誤差量ｅ(k)を求める。誤差量ｅ(k)は、基準画像における部分画像領域Ａc(i)内の画像と、上記ＳＴＥＰ５１５で求めた変換画像との輝度分布の差や形状の差に応じた量である。
【０１１６】
ここで、変数パラメータ（ｎ(i)，ｄ(i)）の現在値により規定される平面が、実床面部分領域Ａr(i)を包含する平面と精度よく合致している場合には、ＳＴＥＰ５１５で求めた変換画像は、基準画像における部分画像領域Ａc(i)内の画像に一致またはほぼ一致する。この場合には、誤差量ｅ(k)は、０もしくは微小なものとなる。従って、誤差量ｅ(k)は、（ｎ(i)，ｄ(i)）の現在値により規定される平面（換言すれば、（ｎ(i)，ｄ(i)）の現在値により定まる射影変換行列Ｈ(i)によって規定される平面）と、実床面部分領域Ａr(i)を包含する平面との間の相違の度合いを表す指標である。換言すれば、誤差量ｅ(k)は、（ｎ(i)，ｄ(i)）現在値の、真値に対する誤差を表す指標である。
【０１１７】
次いで、ＳＴＥＰ５１７に進んで、制御装置３３は、現在の誤差量ｅ(k)が前回ループ内演算処理におけるＳＴＥＰ５１６で求めた誤差量である更新前誤差量ｅ(k-1)よりも小さいか否かを判断する。なお、今回ループ内演算処理が、１回目である場合には、誤差量ｅ(k)と比較する更新前誤差量ｅ(k-1)は、前記ＳＴＥＰ５１３で初期化された値である。従って、この場合には、ＳＴＥＰ５１７の判断結果は常に肯定的となる。
【０１１８】
補足すると、今回ループ内演算処理が、１回目である場合には、ＳＴＥＰ５１７の判断処理を省略し、ＳＴＥＰ５１８からの処理を実行するようにしてもよい。その場合には、ＳＴＥＰ５１３の処理は不要である。
【０１１９】
ＳＴＥＰ５１７の判断結果が肯定的である場合には、ＳＴＥＰ５１８に進んで、制御装置３３は、ｎ(i)，ｄ(i)のそれぞれの修正量Δｎ(i）、Δｄ(i)を求める。なお、これらの修正量Δｎ(i）、Δｄ(i)は、誤差量ｅ(k)などに応じて決定される。
【０１２０】
次いで、ＳＴＥＰ５１９に進んで、現在のｅ(k)が所定量よりも小さいか否か、すなわち、ｅ(k)が十分に小さいか否かを判断する。
【０１２１】
そして、この判断結果が否定的である場合には、ＳＴＥＰ５２０に進んで、制御装置３３は、ＳＴＥＰ５１８で求めた修正量Δｎ(i）、Δｄ(i)がそれぞれ所定量よりも小さいか否かを判断する。
【０１２２】
この判断結果が否定的である場合には、ＳＴＥＰ５２１に進んで、制御装置３３は、変数パラメータ（ｎ(i)，ｄ(i)）の現在値を（ｎs(i)，ｄs(i)）の値として設定する（記憶保持する）と共に、更新前誤差量ｅ(k-1)に現在の誤差量ｅ(k)を代入する（現在の誤差量ｅ(k)を次回のループ内演算処理に関する更新前誤差量として設定する）。
【０１２３】
さらに、ＳＴＥＰ５２２に進んで、制御装置３３は、変数パラメータ（ｎ(i)，ｄ(i)）の現在値をＳＴＥＰ５１８で求めた修正量Δｎ(i）、Δｄ(i)により更新する。これにより、今回ループ内演算処理が終了する。そして、制御装置３３は、ＳＴＥＰ５１４からのループ内演算処理を再開する。
【０１２４】
一方、ＳＴＥＰ５１７の判断結果が否定的であるとき、または、ＳＴＥＰ５１９の判断結果が肯定的であるとき、またはＳＴＥＰ５２０の判断結果が肯定的であるときには、ループ内演算処理は終了し（ＳＴＥＰ５１４〜５２２のループ処理から抜け出す）、実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の推定値が確定される。換言すれば、参照画像における実床面部分領域Ａr(i）の画像（部分画像領域Ａc(i)内の画像）を射影変換行列Ｈ(i)により変換してなる変換画像を、基準画像における実床面部分領域Ａr(i）の画像（部分画像領域Ａc(i)内の画像）に合致させ得るような射影変換行列Ｈ(i)を規定する変数パラメータ（ｎ(i)，ｄ(i)）の値が確定される。
【０１２５】
具体的には、今回ループ内演算処理におけるＳＴＥＰ５２０の判断結果が肯定的となる状況は、変数パラメータ（ｎ(i)，ｄ(i)）がほぼ、ある値に収束しており、（ｎ(i)，ｄ(i)）の更新をさらに繰り返しても、（ｎ(i)，ｄ(i)）の値がほとんど変化しない考えられる状況である。この場合には、ＳＴＥＰ５２３に進んで、制御装置３３は、変数パラメータ（ｎ(i)，ｄ(i)）の現在値を今回ループ内演算処理のＳＴＥＰ５１８で求めた修正量Δｎ(i）、Δｄ(i)により更新する。そして、制御装置３３は、ＳＴＥＰ５２４において、この更新後の変数パラメータ（ｎ(i)，ｄ(i)）の値を、実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の推定値として確定する。なお、この場合、ＳＴＥＰ５２３の処理を省略し、今回ループ内演算処理のＳＴＥＰ５１４で使用した変数パラメータ（ｎ(i)，ｄ(i)）の値をそのまま、実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の推定値としてもよい。
【０１２６】
また、今回ループ内演算処理におけるＳＴＥＰ５１９の判断結果が肯定的となる状況は、今回ループ内演算処理におけるＳＴＥＰ５１４で使用した変数パラメータ（ｎ(i)，ｄ(i)）により規定される平面が、実床面部分領域Ａr(i)を包含する平面に十分に一致していると考えられる状況である。この場合には、前記ＳＴＥＰ５２４に進んで、制御装置３３は、変数パラメータ（ｎ(i)，ｄ(i)）の現在値、すなわち、今回ループ内演算処理のＳＴＥＰ５１４で使用した（ｎ(i)，ｄ(i)）を、実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の推定値として確定する。
【０１２７】
また、今回ループ内演算処理におけるＳＴＥＰ５１７の判断結果が否定的である場合には、ループ内演算処理をさらに繰り返しても、変数パラメータ（ｎ(i)，ｄ(i)）が発散する可能性がある。そこで、この場合には、ＳＴＥＰ５２５に進んで、制御装置３３は、前回ループ内演算処理におけるＳＴＥＰ５２１で設定した（ｎs(i)，ｄs(i)）、すなわち前回ループ内演算処理におけるＳＴＥＰ５１４で使用した変数パラメータ（ｎ(i)，ｄ(i)）の値を実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の推定値とする。
【０１２８】
以上が、ＳＴＥＰ５の処理の詳細である。このＳＴＥＰ５の処理により、各画像投影領域Ａ(i)に対応する部分画像領域Ａc(i)内に画像として捉えれている実床面、すなわち、実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の推定値が得られることとなる。
【０１２９】
ここで、前記ＳＴＥＰ５０２の判断結果が肯定的である場合に、（ｎ(i)，ｄ(i)）の初期値（ｎ0(i)，ｄ0(i)）に対応する平面πa(i)を前記した如く設定した理由を以下に説明する。
【０１３０】
本願発明者は、次のような実験を行なった。すなわち、形状パラメータ（ｎ(i)，ｄ(i)）が既知である実床面部分領域Ａr(i)（ロボット１の前方のある位置における領域）に対して、初期値（ｎ0(i)，ｄ0(i)）のうちの、ｎ0(i)を実床面部分領域Ａr(i)の実際の法線ベクトルｎ(i)に一致させた場合（平面πa(i)を実床面部分領域Ａr(i)の平面と平行にした場合）と、ｎ0(i)を実床面部分領域Ａr(i)の実際の法線ベクトルｎ(i)に対して傾けた場合（平面πa(i)を実床面部分領域Ａr(i)の平面に対して傾けた場合）とで、ｄ0(i)の値を複数の値に設定し、前記ＳＴＥＰ５１２〜５２４の収束演算処理を実行した。そして、それぞれの場合において、収束演算処理が完了するまでの前記ループ内演算処理の実行回数を計測した。
【０１３１】
その計測結果の一例を図１２（ａ），（ｂ）に示す。図１２（ａ）は、初期値（ｎ0(i)，ｄ0(i)）に対応する平面πa(i)を実床面部分領域Ａr(i)の平面と平行にした場合における、ｄ0(i)の設定値の誤差（実際のｄ(i)に対する誤差）と、ループ内演算処理の実行回数との関係を示すグラフ、図１２（ｂ）は、初期値（ｎ0(i)，ｄ0(i)）に対応する平面πa(i)を実床面部分領域Ａr(i)に対して４５度だけ傾けた場合における、ｄ0(i)の設定値の誤差（実際のｄ(i)に対する誤差）と、ループ内演算処理の実行回数との関係を示すグラフである。なお、これらの例では、実床面部分領域Ａr(i)は水平面である。また、いずれの図でも、横軸は、ｄ0(i)の設定値の誤差、縦軸はループ内演算処理の実行回数である。
【０１３２】
図１２（ａ）に示すように、平面πa(i)を実床面部分領域Ａr(i)の平面と平行にした場合には、ｄ0(i)の設定値の誤差が大きくなるに伴い、ループ内演算処理の回数が急激に増加する。また、ｄ0(i)の設定値の誤差がある程度大きくなると、変数パラメータ（ｎ(i)，ｄ(i)）をある値に収束させることができなくなる。これに対して、図１２（ｂ）に示すように、平面πa(i)を実床面部分領域Ａr(i)の平面に対して傾けた場合には、ｄ0(i)の設定値の誤差が大きくなっても、ループ内演算処理の回数はさほど増加せず、しかも、変数パラメータ（ｎ(i)，ｄ(i)）をある値に収束させることができる。このことから、ループ内演算処理の実行回数をできるだけ少なくしつつ、効率よく実際の（ｎ(i)，ｄ(i)）を推定するためには、初期値（ｎ0(i)，ｄ0(i)）に対応する平面πa(i)は、実床面部分領域Ａr(i)の平面に対して傾けておくことが望ましいと考えられる。そして、このようにした場合には、ｄ0(i)の設定値は、実際のｄ(i)の値に十分に近い値である必要がなく、比較的大雑把な値でよい。
【０１３３】
一方、本実施形態におけるロボット１のような脚式移動ロボットでは、一般に、該ロボットを移動させようとする床面の傾斜角度（水平面に対する傾斜角度）は、該ロボットの関節などの機構的な制約や仕様上の制約によって、ある許容範囲内の傾斜角度に制限される。また、ロボットの動作環境があらかじめ決められた環境に制限されている場合には、その環境内の床面の傾斜角度の範囲はあらかじめ決まっている。このように、ロボットを移動させようとする床面の傾斜角度が採り得る値は、ある範囲内の値であり、その範囲は、一般に既知である。そして、ロボットを移動させようとする床面の傾斜角度は、一般的には、高々、１０度程度である。
【０１３４】
従って、初期値（ｎ0(i)，ｄ0(i)）に対応する平面πa(i)を、ロボットを移動させようとする床面の傾斜角度が採り得る値の範囲を逸脱するような角度で傾けるようにすれば、平面πa(i)を、事実上、常に実床面部分領域Ａr(i)の平面に対して傾けることができる。例えば、平面πa(i)を水平面に対して４５度傾けるようにすれば、平面πa(i)を、事実上、常に実床面部分領域Ａr(i)の平面に対して傾けることができる。
【０１３５】
このようなことから、本実施形態では、ＳＴＥＰ５０２の判断結果が肯定的である場合に、初期値（ｎ0(i)，ｄ0(i)）に対応する平面πa(i)が、実床面部分領域Ａr(i)の平面に対して傾斜することを初期値（ｎ0(i)，ｄ0(i)）が満たすべき制約条件とし、その制約条件を満たすように、平面πa(i)を水平面に対して所定角度θだけ傾けることとした。そして、その所定角度θの例として４５度を採用した。
【０１３６】
なお、θは、前記したように床面の傾斜角度が採り得る値の範囲を逸脱していればよいが、好ましくは、該範囲内の任意の傾斜角度との角度差の絶対値が、ある所定値以上となる傾斜角度であることが望ましい。本実施形態では、θ＝４５度としたので、実床面の傾斜角度が高々、１０度であるとした場合、平面πa(i)の傾斜角度と実床面部分領域Ａr(i)の平面の傾斜角度との差は、３５度以上になる。但し、平面πa(i)の傾斜角度と実床面部分領域Ａr(i)の平面の傾斜角度との差は、必ずしも、３５度以上である必要はなく、例えば、２０度以上であってもよい。
【０１３７】
以上説明したＳＴＥＰ５の処理を各実床面部分領域Ａr(i)毎に実行することで、各実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）を効率よく短時間で推定することができる。
【０１３８】
なお、本実施形態では、ＳＴＥＰ５０２の判断結果が肯定的である場合にのみ、ＳＴＥＰ５０５〜ＳＴＥＰ５１０の処理により初期値（ｎ0(i)，ｄ0(i)）を設定する（πa(i)を設定する）ようにしたが、ＳＴＥＰ５０２の判断結果が否定的となる場合にも、ＳＴＥＰ５０５〜ＳＴＥＰ５１０と同様の処理を実行して、初期値（ｎ0(i)，ｄ0(i)）を設定するようにしてもよい。換言すれば、ＳＴＥＰ５０２〜ＳＴＥＰ５０４の処理を省略してもよい。
【０１３９】
図３の説明に戻って、制御装置３３は次に、ＳＴＥＰ６の処理を実行する。このＳＴＥＰ６では、制御装置３３は、各実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）の推定値にフィルタリング（１次遅れフィルタなど）を施し、ノイズ成分を除去する。これにより、形状パラメータ（ｎ(i)，ｄ(i)）の値がノイズなどの影響で過剰に変動するのを防止する。なお、以降の説明では、各実床面部分領域Ａr(i)毎の形状パラメータ（ｎ(i)，ｄ(i)）は、このＳＴＥＰ６のフィルタリング処理を施した推定値を意味するものとする。
【０１４０】
次いで、制御装置３３は、ＳＴＥＰ７〜１６の処理を実行し、今回フレームの処理を終了する。このＳＴＥＰ７〜１６の処理では、ロボット１の進行方向前方の実床面の斜面エッジ（斜面開始エッジまたは斜面終了エッジ）の特定などが行なわれる。なお、このＳＴＥＰ７〜１６の処理は、本発明における傾斜変化部特定手段を構成する処理である。
【０１４１】
具体的には、まず、ＳＴＥＰ７において、制御装置３３は、各実床面部分領域Ａr(i)（ｉ＝１，２，……，Ｎ）を、以下に説明する評価関数が最小となるように、手前側グループと奥側グループとの２つのグループに分類する。手前側グループは、ロボット１により近い側のグループを意味し、奥側グループは、手前側グループよりもロボット１から遠い側のグループを意味する。ＳＴＥＰ７の処理は、別の言い方をすれば、ＳＴＥＰ２で着地予定位置を決定した１歩目からＮ歩目までの中から、ある歩数目（Ｋ歩目）を以下に説明する評価関数が最小となるように決定し、実床面部分領域Ａr(i)（ｉ＝１，２，……，Ｎ）を、１歩目からＫ歩目までの着地予定位置に対応する実床面部分領域Ａr(1)〜Ａr(K)からなる手前側グループと、Ｋ＋１歩目からＮ歩目までの着地予定位置に対応する実床面部分領域Ａr(K+1)〜Ａr(N)からなる奥側グループとに分類する処理である。
【０１４２】
その分類のために使用する評価関数として、本実施形態では、次式（２）により表される評価関数を使用する。
【０１４３】
【数１】
2008246609.tif 000003
【０１４４】
なお、式（２）中のｍavfは、より正確には、１歩目からＫ歩目までの実床面部分領域の形状パラメータ（ｎ(i)，ｄ(i)）（ｉ＝１〜Ｋ）の法線ベクトルｎ(1）〜n(K)から、球面線形補間（slerp）によって求められる中心ベクトル（法線ベクトルｎ(1）〜n(K)の平均の向きを持つ単位ベクトル）を、平面距離ｄ(1)〜ｄ(K)の加重平均値により除算してなるベクトルである。同様に、ｍavbは、Ｋ＋１歩目からＮ歩目までの実床面部分領域の形状パラメータ（ｎ(i)，ｄ(i)）（ｉ＝Ｋ＋１〜Ｎ）の法線ベクトルｎ(K+1）〜n(N)から、球面線形補間（slerp）によって求められる中心ベクトル（法線ベクトルｎ(K+1）〜n(N)の平均の向きを持つ単位ベクトル）を、平面距離ｄ(K+1)〜ｄ(N)の加重平均値により除算してなるベクトルである。
【０１４５】
この式（２）における右辺の第１項は、Ａr(1)〜Ａr(K)のそれぞれ形状パラメータの分散（ばらつき度合い）としての意味を持ち、右辺の第２項は、Ａr(K+1)〜Ａr(N)のそれぞれの形状パラメータの分散（ばらつき度合い）としての意味を持つ。そして、ＳＴＥＰ７の処理では、この評価関数の値が最小になるようにＫの値を定め、上記の如く実床面部分領域Ａr(i)（ｉ＝１，２，……，Ｎ）を手前側グループと奥側グループとの２つのグループに分類する。このように、Ａr(1)〜Ａr(N)を手前側グループと、奥側グループとに分類することにより、それぞれのグループにおける形状パラメータの分散が最小になるように、換言すれば、それぞれのグループにおける形状パラメータの相互の近似度合いが最も高くなるように、Ａr(1)〜Ａr(N)が分類されることとなる。
【０１４６】
このように実床面部分領域Ａr(i)（ｉ＝１，２，……，Ｎ）を手前側グループと奥側グループとに分類したとき、例えば図５に示す状況では、式（２）の評価関数が最小となるＫの値は、「５」となり、１歩目から５歩目までの実床面部分領域Ａr(1)〜Ａr(5)が手前側グループ、６歩目から７歩目までの実床面部分領域Ａr(6)，Ａr(7)が奥側グループとして、実床面部分領域が分類されることとなる。また、例えば図７に示す状況では、５歩目の画像投影領域Ａ(5)に対応する実床面部分領域Ａr(5)内に斜面エッジ（斜面終了エッジ）が存在することから、式（２）の評価関数が最小となるＫの値は、「４」または「５」となる。この場合には、５歩目の実床面部分領域Ａr(5)は、手前側グループに属する場合と、奥側グループに属する場合とがある。ただし、１歩目から４歩目までの実床面部分領域Ａr(1)〜Ａr(4)が手前側グループに属し、６歩目から７歩目までの実床面部分領域Ａr(6)，Ａr(7)は奥側グループに属する。
【０１４７】
補足すると、ＳＴＥＰ７で分類される手前側グループおよび奥側グループは、それぞれ、本発明における第１手前側グループ、第１奥側グループに相当するものである。
【０１４８】
次いで、ＳＴＥＰ８に進み、制御装置３３は、上記の如く分類した各グループに属する実床面部分領域の形状パラメータの代表値を求め、その代表値から、手前側グループに対応する平面（以下、手前側代表平面という）と奥側グループに対応する平面（以下、奥側代表平面という）との角度差を算出する。なお、手前側代表平面は、Ａr(1)〜Ａr(K)のそれぞれを包含する平面を代表的に近似する平面としての意味を持ち、奥側代表平面は、Ａr(K+1)〜Ａr(N)のそれぞれを包含する平明を代表的に近似する平面としての意味を持つ。
【０１４９】
具体的には、ＳＴＥＰ８では、手前側グループに属するＡr(1)〜Ａr(K)のそれぞれの形状パラメータにおける法線ベクトルｎ(1)〜ｎ(K)の中心ベクトルｎavfと、平面距離ｄ(1)〜ｄ(K)の加重平均値ｄavfとの組（ｎavf，ｄavf）が手前側グループの形状パラメータの代表値として求められる。なお、この場合、ｎavf／ｄavfは、式（２）におけるｍavfと同じである。
【０１５０】
同様に、奥側グループに属するＡr(K+1)〜Ａr(N)のそれぞれの形状パラメータにおける法線ベクトルｎ(K+1)〜ｎ(N)の中心ベクトルｎavbと、平面距離ｄ(K+1)〜ｄ(N)の加重平均値ｄavbとの組（ｎavb，ｄavb）が奥側グループの形状パラメータの代表値として求められる。なお、この場合、ｎavb／ｄavbは、式（２）におけるｍavbと同じである。
【０１５１】
そして、制御装置３３は、上記の如く求めた（ｎavf，ｄavf）、（ｎavb，ｄavb）によりそれぞれ規定される平面を手前側代表平面、奥側代表平面とし、これらの平面の角度差を算出する。
【０１５２】
補足すると、ＳＴＥＰ８で求める角度差は、手前側代表平面と奥側代表平面との相違の度合いを表す指標としての意味を持つ。その指標としては、両平面の角度差以外に、上記加重平均値ｄavf，ｄavbの差や、前記式（２）に関して説明したｍavfとｍavbとのベクトル空間における距離などを使用してもよい。
【０１５３】
また、ＳＴＥＰ８で求められる形状パラメータの代表値（ｎavf，ｄavf）、（ｎavb，ｄavb）は、本発明における仮代表値に相当するものである。
【０１５４】
次いで、ＳＴＥＰ９に進み、制御装置３３は、ＳＴＥＰ８で求めた角度差が所定の閾値以上であるか否かを判断する。この判断結果が肯定的である場合には、ＳＴＥＰ１０に進み、制御装置３３は、手前側代表平面と奥側代表平面との交差箇所に斜面エッジが存在しているものとして、仮の斜面エッジを算出する。具体的には、制御装置３３は、手前側代表平面を規定する形状パラメータの代表値（ｎavf，ｄavf）と、奥側代表平面を規定する形状パラメータの代表値（ｎavb，ｄavb）とから、それらの平面の交線を算出し、それを仮の斜面エッジとする。
【０１５５】
なお、この仮の斜面エッジは、本発明における仮傾斜変化部に相当するものである。
【０１５６】
次いで、ＳＴＥＰ１１に進み、制御装置３３は、手前側代表平面と奥側代表平面とに関する形状パラメータの代表値と、斜面エッジとを再計算する。具体的には、制御装置３３は、手前側グループに属する実床面部分領域Ａr(1)〜Ａr(K)から、仮の斜面エッジとの距離が所定距離よりも短い実床面部分領域（仮の斜面エッジに近接している実床面部分領域）を除いたものを改めて手前側グループとする。同様に、奥側グループに属するＡr(K+1)〜Ａr(N)から、仮の斜面エッジとの距離が所定距離よりも短い実床面部分領域を除いたものを改めて奥側グループとする。このようにするのは、仮の斜面エッジに近接している実床面部分領域は、実床面のうちの傾斜が異なる２つの平面に跨っている可能性があるからである。そして、このように改めて定めた手前側グループの各実床面部分領域の形状パラメータから、前記ＳＴＥＰ８と同じ手法でそれらの形状パラメータの代表値（ｎavf，ｄavf）を求め、それを改めて、手前側代表平面の形状パラメータの代表値とする。同様に、改めて定めた奥側側グループの各実床面部分領域の形状パラメータから、前記ＳＴＥＰ８と同じ手法でそれらの形状パラメータの代表値（ｎavb，ｄavb）を求め、それを改めて、奥側代表平面の形状パラメータの代表値とする。そして、制御装置３３は、改めて求めた（ｎavf，ｄavf），（ｎavb，ｄavb）によりそれぞれ規定される手前側代表平面と奥側代表平面との交線を斜面エッジとして求める。
【０１５７】
なお、このようにして求められる手前側代表平面と、奥側代表平面とは、推定床面としての意味を持つ。より詳しくは、ロボット１から見て、斜面エッジよりも手前側の推定床面が手前側代表平面により表され、斜面エッジよりも奥側の推定床面が奥側代表平面により表される。この場合、ＳＴＥＰ８の処理で、仮の斜面エッジに近接した実床面部分領域を除外して、手前側代表平面と奥側代表平面とを規定する形状パラメータ（ｎavf，ｄavf），（ｎavb，ｄavb）を改めて求めるので、斜面エッジの手前側および奥側の推定床面の精度を高めることができると共に、該斜面エッジを精度よく特定することができる。
【０１５８】
補足すると、ＳＴＥＰ１１の処理は、本発明における第１傾斜変化部特定手段を構成をする処理である。
【０１５９】
次いで、ＳＴＥＰ１２に進んで、制御装置３３は斜面パラメータを更新する。この斜面パラメータは、ロボット１の進行経路に沿った推定床面の巨視的な形状を表すパラメータである。ＳＴＥＰ１２では、より具体的には、ＳＴＥＰ１１で求められた斜面エッジに応じて、ロボット１の現在位置における推定床面（ロボット１の現在の接地箇所の近辺の推定床面で平面状の床面。以下、現在接地推定床面という）の終了位置（ロボット１の進行方向前方の終了位置）を表す斜面パラメータが更新される。
【０１６０】
また、ロボット１から見て、現在接地推定床面の終了位置より奥側の推定床面（現在接地推定床面と傾斜度合いが異なる床面）の開始位置およびその傾斜度合いを表す斜面パラメータが、ＳＴＥＰ１１で求められた奥側代表平面を規定する形状パラメータ（ｎavb，ｄavb）に応じて更新される。
【０１６１】
一方、ＳＴＥＰ９の判断結果が否定的である場合には、ＳＴＥＰ１３〜ＳＴＥＰ１６の処理が実行される。ここで、ＳＴＥＰ９の判断結果が否定的となる状況は、主に、各実床面部分領域Ａr(i)（ｉ＝１，２，……，Ｎ）が、ほぼ同一平面上に存在しているような状況である。ただし、ロボット１の進行方向前方の実床面の傾斜度合いが徐々に変化しているような場合には、ＳＴＥＰ９に判断結果が否定的となるような場合もある。このような状況に対応するために、ＳＴＥＰ９に判断結果が否定的である場合には、制御装置３３は、以下に説明するような処理を実行する。
【０１６２】
すなわち、まず、ＳＴＥＰ１３において、制御装置３３は、各実床面部分領域Ａr(i)（ｉ＝１，２，……，Ｎ）を所定の位置で手前側グループと奥側グループとに分類する。具体的には、制御装置３３は、例えば、１歩目からあらかじめ定めた歩数目（Ｋx歩目。例えば３歩目）までの実床面部分領域Ａr(1)〜Ａr(Kx)を手前側グループ、Ｋx＋１歩目からＮ歩目までの実床面部分領域Ａr(Kx+1)〜Ａr(N)を奥側グループとする。なお、歩数で分類する代わりに、例えば、ロボット１の進行方向で、該ロボット１の現在位置からの距離が所定距離以下となる画像投影領域Ａ(1)〜Ａ(Kx)に対応する実床面部分領域を手前側グループ、他の実床面部分領域Ａr(Kx+1)〜Ａr(N)を奥側グループとするようにしてもよい。
【０１６３】
補足すると、ＳＴＥＰ１３で分類される手前側グループおよび奥側グループは、それぞれ、本発明における第２手前側グループ、第２奥側グループに相当するものである。
【０１６４】
次いで、ＳＴＥＰ１４に進んで、制御装置３３は、ロボット１の現在の動作制御のために使用している該ロボット１の現在の接地箇所の近辺の現状想定床面（以下、現在接地想定床面という）と、ＳＴＥＰ１３で決定された奥側グループに対応する平面（奥側代表平面）との角度差を算出する。具体的には、ＳＴＥＰ１４では、奥側グループに属するＡr(Kx+1)〜Ａr(N)のそれぞれの形状パラメータにおける法線ベクトルｎ(Kx+1)〜ｎ(N)の中心ベクトルｎavbと、平面距離ｄ(Kx+1)〜ｄ(N)の加重平均値ｄavbとの組（ｎavb，ｄavb）により規定される平面を奥側代表平面として求める。そして、この奥側代表平面と、現在接地想定床面との角度差を算出する。
【０１６５】
補足すると、ＳＴＥＰ１４で求める角度差は、現在接地想定床面と、奥側グループ代表平面との相違の度合いを表す指標としての意味を持つ。その指標はＳＴＥＰ８の場合と同様に、角度差以外の指標を用いてもよい。
【０１６６】
また、例えば本願出願人が特開２００１−３２２０７６号公報に提案したように、ロボットの接地箇所の近辺の床面形状を、床反力の検出値などを基に推定するようにした場合には、その推定された床面形状により表される床面をＳＴＥＰ１４の処理で現在接地想定床面として用いるようにしてもよい。
【０１６７】
次いで、ＳＴＥＰ１５に進んで、制御装置３３は、ＳＴＥＰ１４で求めた角度差の絶対値が所定の閾値以上であるか否かをＳＴＥＰ９と同様に判断する。
【０１６８】
そして、この判断結果が否定的である場合には、奥側代表平面は、現在接地想定床面とほぼ同等の傾斜度合いを有し、該現在接地想定床面に連続した平面であると考えられる。そして、この場合には、制御装置３３は、推定床面に関する斜面パラメータを更新することなく、今回フレームの処理を終了する。
【０１６９】
また、ＳＴＥＰ１５の判断結果が肯定的である場合には、制御装置３３は、奥側代表平面が現在接地想定床面に対して傾斜した平面であると見なし、斜面エッジを算出する。この場合、例えば、Ｋx歩目の着地予定位置における足平１７の踵の位置と、Ｋx＋１歩目の着地予定位置における足平１７の踵の位置との中点を通ってロボット１の進行方向に直交する線を斜面エッジとする。
【０１７０】
補足すると、ＳＴＥＰ１４〜１６の処理は、本発明における第２傾斜変化部特定手段を構成する処理である。
【０１７１】
次いで、ＳＴＥＰ１２に進んで、推定床面に関する斜面パラメータを更新する。より具体的には、ＳＴＥＰ１６で求められた斜面エッジに応じて、現在接地推定床面の終了位置（ロボット１の進行方向前方の終了位置）を表す斜面パラメータが更新される。また、ロボット１から見て、現在接地推定床面の終了位置より奥側の推定床面（現在接地推定床面と傾斜度合いが異なる床面）の開始位置および傾斜度合いを表す斜面パラメータが、ＳＴＥＰ１４で求められた奥側代表平面を規定する形状パラメータ（ｎavb，ｄavb）に応じて更新される。この場合、奥側代表平面は、ロボット１から見て、斜面エッジよりも奥側の推定床面としての意味を持つ。
【０１７２】
以上が本実施形態における床面形状推定処理の詳細である。
【０１７３】
本実施形態では、ロボット１の動作制御に使用する現状想定床面は、上記のように決定される推定床面の斜面パラメータに応じて適宜更新される。この場合、本実施形態では、ロボット１の動作制御の安定性を高めるために、現在接地想定床面の傾斜度合いに関するパラメータは、現状に維持される。そして、現在接地想定床面よりも奥側（現在接地推定床面の終了位置よりも奥側）の現状想定床面については、該現状想定床面と推定床面との傾斜度合いの差が所定値以上（例えば１．５度以上）である場合に、当該奥側の現状想定床面の斜面エッジの位置や傾斜度合いに関するパラメータが、推定床面に合致するように更新される。なお、現在接地想定床面の傾斜度合いに関する斜面パラメータを、現状に維持することは必須ではなく、現在接地推定床面の傾斜度合いに合わせて更新するようにしてもよい。
【０１７４】
以上説明した本実施形態のロボット１では、ロボット１の複数歩分の着地予定位置に対応する実床面部分領域Ａr(i)毎に、カメラ２７Ｌ，２７Ｒの撮像画像を使用して、各実床面部分領域Ａr(i)の形状パラメータ（ｎ(i)，ｄ(i)）を推定する。すなわち、カメラ２７Ｌ，２７Ｒの撮像画像内の実床面のうちの、ロボット１の移動動作に必要な部分領域に限定して、実床面の形状の推定が行なわれる。このため、カメラ２７Ｌ，２７Ｒの撮像画像による床面の形状を認識するための演算処理を軽減しつつ、ロボット１の適切な移動制御を行なうことができる。
【０１７５】
なお、以上説明した実施形態では、２足歩行ロボット１を例に採って説明したが、３つ以上の脚体を有する脚式移動ロボットについても本発明を適用できることはもちろんである。
【図面の簡単な説明】
【０１７６】
【図１】本発明の一実施形態における脚式移動ロボットである２足歩行ロボットの外観斜視図。
【図２】図１のロボットに備えた制御システムの概要構成を示すブロック図。
【図３】実施形態における床面形状の推定処理を示すフローチャート。
【図４】図３のＳＴＥＰ５の処理の詳細を示すフローチャート。
【図５】ロボットの足平の複数歩分の着地予定位置と画像投影領域との設定例を説明するための図。
【図６】図６（ａ），（ｂ）はロボットの足平の複数歩分の着地予定位置と画像投影領域との設定例を説明するための図。
【図７】ロボットの足平の複数歩分の着地予定位置と画像投影領域との設定例を説明するための図。
【図８】図８（ａ），（ｂ）はロボットの足平の複数歩分の着地予定位置と画像投影領域との設定例を説明するための図。
【図９】ロボットの足平と画像投影領域との関係を示す図。
【図１０】画像投影領域を撮像画像に投影した状態を状態を示す図。
【図１１】実床面部分領域の形状パラメータを推定するための収束演算処理における形状パラメータの初期値の設定例を説明するための図。
【図１２】図１２（ａ），（ｂ）は、実床面部分領域の形状パラメータを推定するための収束演算処理における形状パラメータの初期値の設定手法を説明するためのグラフ。
【符号の説明】
【０１７７】
１…２足歩行ロボット（脚式移動ロボット）、５Ｌ，５Ｒ…脚体、２７Ｌ，２７Ｒ…カメラ、３３…制御装置、ＳＴＥＰ２…着地予定位置設定手段、ＳＴＥＰ３…画像投影領域設定手段、ＳＴＥＰ５…床面形状推定手段、ＳＴＥＰ７〜１６…傾斜変化部特定手段、ＳＴＥＰ１１…第１傾斜変化部特定手段、ＳＴＥＰ１４〜１６…第２傾斜変化部特定手段、Ａ(i)…画像投影領域、Ａc(i)…部分画像領域、Ａr(i)…実床面部分領域。
【図１】
2008246609.tif 000004
【図２】
2008246609.tif 000005
【図３】
2008246609.tif 000006
【図４】
2008246609.tif 000007
【図５】
2008246609.tif 000008
【図６】
2008246609.tif 000009
【図７】
2008246609.tif 000010
【図８】
2008246609.tif 000011
【図９】
2008246609.tif 000012
【図１０】
2008246609.tif 000013
【図１１】
2008246609.tif 000014
【図１２】
2008246609.tif 000015