JP2008250516A - 射影変換収束演算処理方法 - Google Patents

Abstract

【課題】２つのカメラの撮像画像内の物体の平坦部を観測するために、それらの撮像画像間の射影変換を利用した収束演算処理を行なう方法において、その収束演算処理の収束性を確保しつつ、演算負荷を軽減する。
【解決手段】収束演算処理における射影変換行列の所定種類のパラメータ（n(i)，ｄ(i)）の初期値（n0(i)，ｄ0(i)）により規定される平面πa(i)が、観測対象の物体の平坦部を包含する実際の平面に対して傾斜した平面になることを該初期値（n0(i)，ｄ0(i)）が満たすべき制約条件とし、該初期値（n0(i)，ｄ0(i)）を該制約条件を満たす値に設定する。
【選択図】図４

Description

本発明は、２つのカメラの撮像画像間の射影変換を利用して、該撮像画像中の物体の平坦部（平面領域）を観測するための収束演算処理方法に関する。

２つの撮像カメラの撮像画像中に、平坦部を有する物体が存在する場合、一方の撮像画像（参照画像）を、ある適当な射影変換行列により変換したとき、その変換後の画像内における物体の平坦部の画像を、他方の撮像画像（基準画像）内における物体の平坦部の画像に合致させることができる。この場合、このような画像変換を行い得る射影変換行列は、物体の平坦部を包含する平面の空間的な姿勢（例えば法線方向）と位置（例えばカメラの光学中心からの距離）とに依存する。換言すれば、該射影変換行列は、撮像画像内の物体の平坦部を包含する平面の空間的な位置および姿勢を規定する。

これを利用して、２つの撮像画像中の物体の平坦部を観測する（平坦部の姿勢などを測定する）技術として、従来、例えば、非特許文献１に見られる技術が知られている。この技術では、車両を走行させる道路面としての平面を抽出するために、２つの車載カメラの撮像画像間の射影変換行列の適当なテンプレートを複数用意しておき、それらのテンプレートを射影変換行列の初期値とする収束演算処理により、上記した画像変換を行い得る射影変換行列を推定する。なお、射影変換行列の初期値としてのテンプレートを複数用意するということは、換言すれば、該射影変換行列により規定される平面を表すパラメータの初期値を複数用意するということを意味する。

この非特許文献１に見られる技術は、収束演算処理における射影変換行列の初期値、あるいは、測定しようとする平面の初期値を複数用意しておくことで、測定しようとする平面に対応する射影変換行列の推定処理のロバスト性を高め、また、その推定処理の発散や不適切な射影変換行列が推定される（実際の平面とは異なる平面が推定される）のを防止しようとするものである。
「ステレオ動画像を利用した平面領域抽出による障害物検出」／関 晃仁，奥富 正敏／情報処理学会論文誌：コンピュータビジョンとイメージメディア／vol.45，No.SIG13(CVIM10)，Dec.2004

しかしながら、非特許文献１の技術では、各初期値により規定される平面と、測定しようとする実際の平面との位置の誤差（カメラの光学中心からの距離の誤差）が十分に小さいことを前提としている。このため、それらの位置の誤差が比較的大きい場合には、射影変換行列が十分に収束するまでに必要な繰り返し演算の回数が多大になったり、あるいは、射影変換行列を収束させることができなくなる恐れがあった。

また、複数の初期値に対して収束演算処理を行なう必要があることから、最終的に射影変換行列を確定するまでの演算負荷が過大なものとなりやすいという不都合があった。

本発明はかかる背景に鑑みてなされたものであり、２つのカメラの撮像画像内の物体の平坦部を観測するために、それらの撮像画像間の射影変換を利用した収束演算処理を行なう射影変換収束演算処理方法において、その収束演算処理の収束性を確保しつつ、演算負荷を軽減できる方法を提供することを目的とする。

本発明の射影変換収束演算処理方法は、かかる目的を達成するために、２つのカメラの撮像画像中に捉えられている物体の平坦部を観測するために、前記２つのカメラのうちの一方のカメラの撮像画像内の物体の画像を両カメラ間の射影変換行列により変換してなる変換画像と、他方のカメラの撮像画像内の物体の画像とを比較しつつ、該射影変換行列の所定種類のパラメータの値により規定される平面を前記物体の平坦部を包含する平面に収束させるように、該パラメータの値を更新していく収束演算処理を実行する射影変換収束演算処理方法において、前記収束演算処理における前記パラメータの初期値により規定される平面が、前記物体の平坦部を包含する実際の平面に対して傾斜した平面になることを該初期値が満たすべきの制約条件とし、該初期値を該制約条件を満たす値に設定したことを特徴とする。

ここで、本願発明者は、射影変換収束演算処理方法に関して種々様々の検討を行なった結果、次のことを知見した。すなわち、具体的な例は後述するが、前記収束演算処理で更新していく射影変換行列のパラメータの初期値は、それにより規定される平面（以下、初期平面ということがある）が、実際の物体の平坦部を包含する平面（以下、実平面ということがある）と平行となる場合には、初期平面と実平面との空間的な位置の誤差が大きくなるに伴い、収束演算処理の繰り返し演算の回数が急減に増大する傾向がある。さらに、初期平面と実平面との空間的な位置との誤差がある程度大きくなると、前記パラメータの値をある値に収束させることができなくなる。つまり、射影変換行列のパラメータの初期値を、初期平面と実平面とが平行となるような値に設定した場合には、それらの初期平面と実平面との空間的な位置も大きな誤差が生じないように、該初期値を設定しなければ、収束演算処理の演算処理負荷が急減に増大し、あるいは、パラメータの値を収束させることができなくなる。

一方、初期平面が、実平面に対してある程度傾斜するように、射影変換行列のパラメータの初期値を設定した場合には、収束演算処理の繰り返し演算の回数は、それらの初期平面と実平面との空間的な位置の誤差が比較的大きくても、急激に増加することはなく、比較的少ない繰り返し演算の回数で、前記パラメータの値を収束させることができる。

そこで、本発明は、前記した通り、前記収束演算処理における前記パラメータの初期値により規定される平面（初期平面）が、前記物体の平坦部を包含する実際の平面（実平面）に対して傾斜した平面になることを該初期値が満たすべきの制約条件とし、該初期値を該制約条件を満たす値に設定した。

このように前記パラメータの初期値を設定することにより、該初期値により規定される初期平面が実平面に対して傾斜した平面となるので、それらの平面の位置の誤差が比較的大きくても、収束演算処理の収束性を確保しつつ、演算負荷を軽減することが可能となる。

かかる本発明では、前記パラメータの初期値により規定される平面は、例えば、該平面の水平面に対する傾斜角度が、前記物体の平坦部を包含する実際の平面の水平面に対する傾斜角度が採り得る値の範囲から逸脱するようにあらかじめ定められた所定角度となる平面である。

すなわち、一般に、観測対象とする物体の平坦部を包含する平面の傾斜角度（水平面に対する傾斜角度）が採り得る値は、ある範囲内の値となる。従って、前記所定角度を、当該範囲を逸脱した値に定めておき、前記初期平面の傾斜角度が該所定角度となるように該初期平面のを規定する前記パラメータの初期値を設定することで、前記制約条件を確実に満たす初期値を設定することができる。

本発明の一実施形態を図１〜図１１を参照して説明する。なお、本実施形態は、本発明の射影変換収束演算処理を、脚式移動ロボットを移動させる床の床面形状を推定する処理に適用した場合の実施形態である。従って、その床が、本発明における物体に相当するものである。

まず、図１および図２を参照して本実施形態における脚式移動ロボットの概要構成を説明する。図１は、本実施形態における脚式移動ロボットとしての２足歩行ロボットの外観斜視図、図２は該ロボットの制御システムの概要を示すブロック図である。

この２足歩行ロボット１は、人型ロボットであり、上体（基体）３、左右一対の脚体５Ｌ，５Ｒ、左右一対の腕体７Ｌ，７Ｒ、および頭部９を備える。なお、本実施形態の説明では、符号「Ｌ」、「Ｒ」は、それぞれロボット１の前方に向かって左側の要素、右側の要素を表す符号として使用する。ただし、左右を特に区別する必要が無いときには、符号「Ｌ」、「Ｒ」を省略する。

脚体５Ｌ，５Ｒは、互いに同じ構造のものであり、それぞれ上体３の下端部の左寄りの部分、右寄りの部分から股関節１１を介して延設されている。そして、各脚体５は、その長手方向の中間部に膝関節１３を有する。また、各脚体５の先端部（下端部）には、足首関節１５を介して接地部としての足平１７が装着されている。本実施形態では、各脚体５の股関節１１、膝関節１３、および足首関節１５は、それぞれ３自由度、１自由度、２自由度を有する。従って、足平１７は、上体３に対して６自由度を有する。

腕体７Ｌ，７Ｒは、互いに同じ構造のものであり、それぞれ上体３の上部の左側部、右側部から肩関節１９を介して延設されている。そして、各腕体７は、その長手方向の中間部に肘関節２１を有する。また、各腕体７の先端部には、手首関節２３を介してハンド（手先部）２５が装着されている。本実施形態では、肩関節１９、肘関節２１および手首関節２３はそれぞれ３自由度、１自由度、２自由度を有する。従って、ハンド２５は、上体３に対して６自由度を有する。

頭部９は、上体３の上端部に搭載されている。この頭部９には、左右一対のカメラ２７Ｌ，２７Ｒが内蔵されている。これらのカメラ２７Ｌ，２７Ｒは例えばＣＣＤカメラにより構成されている。これらのカメラ２７Ｌ，２７Ｒは、頭部９の正面（概ねロボット１の前方）の外界の映像を撮像するものである。そして、カメラ２７Ｌ，２７Ｒは、それらの光軸が頭部９の正面に向かってほぼ同方向に向くように頭部９に固定されている。また、頭部９には、上体３に対して該頭部９を可動とする関節機構（図示省略）が内蔵されている。その関節機構によって、頭部９は、カメラ２７Ｌ，２７Ｒと一体に、上体３に対して可動とされている。例えば、頭部９は、上体３の体幹軸（概ね上下方向の軸）まわりの回転動作と、左右方向の軸まわりの回転動作とが可能とされている。なお、頭部９は、上体３に固定されていてもよい。

また、上体３の背面部には、ボックス２９が装着されており、このボックス２９には、後述する制御装置３３等が収容されている。

ロボット１には、その動作（歩行など）を制御するために図２に示す制御システムが備えられている。この制御システムは、各脚体５、各腕体７および頭部９の上記した各関節をそれぞれ駆動する関節アクチュエータ３１と、この関節アクチュエータ３１を介してロボット１の動作（運動）を制御する制御装置３３と、ロボット１の各種の内部状態量を検出するセンサ３５とを有する。なお、図２では、関節アクチュエータ３１を１つだけ記載しているが、該関節アクチュエータ３１は、各脚体５、各腕体７および頭部９の上記した各関節毎に備えられている。また、各関節アクチュエータ３１は電動モータ等により構成される。

センサ３５は、詳細な図示は省略するが、ロボット１の所定の部位、例えば上体３の３軸方向の加速度を検出する加速度センサ、該上体３の３軸まわりの角速度を検出するレートセンサ、該上体３の傾斜角度（鉛直方向に対する傾斜角度）を検出する傾斜計、ロボット１の各足平１７に床側から作用する床反力（３軸方向の並進力および３軸まわりのモーメント）を検出する力センサ（６軸力センサ）などから構成される。

制御装置３３は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路、関節アクチュエータ３１の駆動回路などを含む電子回路ユニットである。この制御装置３３には、前記カメラ２７Ｌ，２７Ｒのそれぞれの撮像画像が入力されると共に、前記センサ３５の検出信号が入力される。さらに、制御装置３３には、ロボット１の遠隔操縦装置（図示省略）などからロボット１の移動速度および移動方向（進行方向）を規定する指令（以下、速度・方向指令という）が入力される。そして、制御装置３３は、これらの入力されたデータを使用して、所要の演算処理を実行し、ロボット１の目標歩容を生成する。さらに、制御装置３３は、生成した目標歩容に応じて各脚体５の股関節１１、膝関節１３、足首関節１５など、ロボット１に備えた各関節の目標動作を決定する。そして、制御装置３３は、その目標動作に従って、各関節アクチュエータ３１を制御し、ロボット１を移動させる。

また、本実施形態では、制御装置３３は、ロボット１を移動させようとする床に対して固定されたグローバル座標系（ワールド座標系）での該ロボット１の自己位置を、前記センサ３５（例えば加速度センサおよびレートセンサ）の検出信号などに基づいて逐次認識するようにしている。なお、ロボット１の自己位置は、適宜の外界センサを使用して認識するようにしてもよい。例えば、ロボット１を移動させようとする床などにランドマークを付設しておき、カメラ２７Ｌ，２７Ｒの撮像画像中のランドマークに基づいてロボット１の自己位置を認識するようにしてもよい。

また、本実施形態では、制御装置３３は、カメラ２７Ｌ，２７Ｒの撮像画像を使用して、ロボット１を移動させようとする床面の形状を推定する処理も実行する。

なお、本実施形態では、ロボット１の速度・方向指令を遠隔操縦装置から制御装置３３に入力するようにしているが、該速度・方向指令、あるいはこれを規定するロボット１の移動計画を制御装置３３にあらかじめティーチングしておいてもよい。

次に、ロボット１の制御装置３３が実行する制御処理のうち、本発明に関連した制御処理を図３〜図１１を参照して詳説する。制御装置３３は、ロボット１を前記速度・方向指令に応じて移動させる制御処理（ロボット１の動作制御処理）を実行しながら、カメラ２７Ｌ，２７Ｒの撮像画像を基に、ロボット１の進行方向前方における床面の形状を逐次推定する。

図３は、この床面の形状の推定処理（以下、床面形状推定処理という）を示すフローチャートである。この床面形状推定処理は、ロボット１の動作制御と並行して、所定の演算処理周期で逐次実行される処理である。

ここで、以降の説明で用いる用語に関して補足しておく。床面形状推定処理における各演算処理周期をフレーム、現在の演算処理周期を今回フレーム、その１つ前の演算処理周期を前回フレームという。また、制御装置３３がロボット１の動作制御のために現時点で想定している床面（ロボット１の現在位置をその領域内に含む床面）を現状想定床面、実際の床面を実床面、床面形状推定処理で推定された床面を推定床面という。

現状想定床面は、ロボット１の動作制御のために必要な床面形状の情報により規定される床面である。その床面形状の情報は、グローバル座標系における斜面や水平面の位置、斜面の傾斜度合いなどであり、ロボット１の移動開始前に、制御装置３３に入力される。制御装置３３は、基本的には、ロボット１を移動させようとする床面が、現状想定床面に一致していると見なして該ロボット１の動作制御を行なう。この場合、本実施形態では、ロボット１の進行方向前方における現状想定床面は、推定床面との差異（例えば、それらの床面の傾斜角度の差）がある程度大きい場合に、推定床面に応じて更新される。なお、ロボット１の移動開始時におけるロボット１の接地箇所の近辺における現状想定床面は水平面であり、実床面にほぼ一致しているものとする。

また、ロボット１の両足平１７，１７のうち、ロボット１の移動時に、空中に持ち上げる（離床させる）足平１７を遊脚足平１７、他方の足平１７（ロボット１の自重を床に支える脚体５の足平１７）を支持脚足平１７という。ロボット１を連続的に移動させるとき、遊脚足平１７および支持脚足平１７は交互に切り換わる。

また、本実施形態では、カメラ２７Ｌ，２７Ｒのうちの一方のカメラの撮像画像をステレオ画像における基準画像とし、他方のカメラの撮像画像をステレオ画像における参照画像とする。以降の説明では、便宜上、例えばカメラ２７Ｌの撮像画像を基準画像とし、該カメラ２７Ｌを基準カメラ２７Ｌということがある。

以上を前提として、床面形状推定処理を説明する。図３を参照して、この床面形状推定処理では、まず、ＳＴＥＰ１において、制御装置３３は、カメラ２７Ｌ，２７Ｒから左右の各撮像画像（基準画像および参照画像）を取得する。

次いで、ＳＴＥＰ２において、制御装置３３は、入力された前記速度・方向指令と、現状想定床面の形状と、ロボット１の目標歩容を生成するために該制御装置３３が使用している歩容パラメータとに基づいて、ロボット１の遊脚足平１７の将来の着地予定位置を１歩目からＮ歩目まで複数歩分、決定する。１歩目の着地予定位置は、ロボット１の現在の遊脚足平１７の着地予定位置である。

本実施形態では、ＳＴＥＰ２で決定する着地予定位置は、例えば１歩目から７歩目までの７歩分の着地予定位置である（Ｎ＝７である）。それらの着地予定位置は、前記グローバル座標系での位置である。また、それらの着地予定位置は、足平１７の代表点の位置を意味する。

ここで、本実施形態では、制御装置３３がロボット１の目標歩容を生成するために使用する歩容パラメータには、１歩目の歩容（以下、今回歩容ということがある）における遊脚足平１７の着地予定位置（現在の遊脚足平１７の着地予定位置）と、その次の歩容である２歩目の歩容（以下、次回歩容ということがある）における遊脚足平１７の着地予定位置（現在の遊脚足平１７の次の遊脚足平１７の着地予定位置）と、今回歩容に続く仮想的な周期的歩容（２歩分の歩容を１周期分の歩容とする周期的な歩容）である定常歩容における１歩毎の足平１４の着地位置とを規定する歩容パラメータを含む。これらの歩容パラメータにより規定される今回歩容および次回歩容の遊脚足平１７の着地予定位置並びに定常歩容における着地位置は、ロボット１に対して固定された座標系（例えばロボット１の現在の支持脚足平１７の接地面内の所定の代表点を原点とする座標系）での位置である。

これらの歩容パラメータにより規定される今回歩容および次回歩容の遊脚足平１７の着地予定位置は、制御装置３３に入力された前記速度・方向指令と、現状想定床面（ロボット１の現在の接地箇所の近辺の現状想定床面）の形状情報とから、あらかじめ定められた所定の規則に従って決定される。この場合、今回歩容および次回歩容の遊脚足平１７の着地予定位置は、それらの位置で該遊脚足平１７を現状想定床面に接地させるように設定される。また、今回歩容に続く仮想的な周期的歩容である定常歩容に関しては、その各周期（２歩分の期間）の１歩目の歩容における支持脚足平１７の着地位置と遊脚足平１７の着地位置とは、それらの間の相対的な位置関係が、次回歩容の支持脚足平１１の着地位置（すなわち、今回歩容の遊脚足平１７の着地予定位置）と次回歩容の遊脚足平１７の着地予定位置との間の相対的位置関係に合致するように決定される。また、定常歩容の各周期の２歩目の歩容における支持脚足平１７の着地位置（すなわち、定常歩容の１歩目の歩容における遊脚足平１７の着地予定位置）と遊脚足平１７の着地位置とは、それらの間の相対的な位置関係が、今回歩容の支持脚足平１７の着地位置と今回歩容の遊脚足平１７の着地予定位置との間の相対的な位置関係に合致するように決定される。

なお、定常歩容は、今回歩容の生成条件（ロボット１の移動を安定化させる上で好ましい条件）を決定するために生成される仮想的な歩容であり、ロボット１の現実の動作制御に使用される歩容ではない。また、定常歩容の１周期分の歩数は、２歩分である必要はなく、３歩以上の歩数を１周期分の歩数としてもよい。このような定常歩容に関しては、例えば本願出願によるＰＣＴ国際公開公報ＷＯ／０２／４０２２４などに詳細に説明されているので、本明細書での詳細な説明は省略する。

そして、ＳＴＥＰ２では、制御装置３３は、３歩目までの着地予定位置を、該制御装置３３がロボット１の目標歩容（今回歩容）の生成のために使用している上記歩容パラメータに基づいて決定する。具体的には、該歩容パラメータにより規定される１歩目の着地予定位置（今回歩容の遊脚足平１７の着地予定位置）および２歩目の着地予定位置（次回歩容の遊脚足平１７の着地予定位置）をグローバル座標系に変換したものを、そのまま、２歩目までの足平１４の着地予定位置として決定する。また、上記歩容パラメータにより規定される定常歩容の１周期目の２歩目の着地位置をグローバル座標系に変換したものを３歩目の着地予定位置として決定する。

さらに、制御装置３３は、４歩目からＮ歩目（本実施形態ではＮ＝７）までの着地予定位置については、入力された前記速度・方向指令と、現状想定床面の形状情報とから、所定の規則に従って決定する。その規則は、１歩目および２歩目の着地予定位置を決定するための規則と同じである。なお、これらの着地予定位置を決定するに際しては、現在の速度・方向指令と、現状想定床面とが、そのまま一定に保持されると仮定される。

ＳＴＥＰ２の処理による１歩目からＮ歩目までの着地予定位置の設定例を図５、図６（ａ），（ｂ）、図７、図８（ａ），（ｂ）を参照して説明する。図５は、ロボット１および実床面などをロボット１の矢状面（側面視）で示した説明図、図６（ａ），（ｂ）は、図５に示す実床面などを鉛直方向上方から見た図、図７は、ロボット１および実床面などをロボット１の矢状面（側面視）で示した説明図、図８（ａ），（ｂ）は、図７に示す実床面などを鉛直方向上方から見た図である。この場合、図５では、ロボット１が現時点で接地している実床面が水平面であると共に、ロボット１の進行方向前方のある位置Ｐ１から遠方側の実床面が水平面から斜面（図では昇り斜面）に変化している例を示している。また、図７では、ロボット１が現時点で接地している実床面が斜面（図では昇り斜面）であると共に、ロボット１の進行方向前方のある位置Ｐ２から遠方側の実床面が斜面から水平面に変化している例を示している。なお、「斜面」は、水平面に対して傾斜した床面を意味する。また、本実施形態の説明では、ロボット１の進行経路上のある区間における床面（実床面または現状認識床面または推定床面）が、その区間内で実質的に一定の傾斜角度（水平面に対する傾斜角度）を有する斜面となっている場合において、ロボット１の進行経路上での該斜面の両端を斜面エッジという。そして、それらの斜面エッジのうち、ロボット１の移動開始地点により近い側の斜面エッジを斜面開始エッジ、他方の斜面エッジを斜面終了エッジという。例えば、図５に示す実床面では、該実床面が水平面から斜面に変化する位置Ｐ１における該水平面と斜面との交線が斜面開始エッジとなる。また、図７に示す実床面では、該実床面が斜面から水平面に変化する位置Ｐ２における該斜面と水平面との交線が斜面終了エッジとなる。

実床面が図５に示すような床面である場合において、現状想定床面が水平面（ロボット１が現時点で接地している実床面（水平面）に連続する水平面）であるとする。この状況では、例えば、図５および図６（ａ）で破線で示すように、１歩目からＮ歩目（Ｎ＝７）までの遊脚足平１７(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)という。この実床面部分領域Ａ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)）は、変数パラメータとしての意味を持ち、適宜、その値が更新される。すなわち、（ｎ(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)からなる奥側グループとに分類する処理である。

その分類のために使用する評価関数として、本実施形態では、次式（２）により表される評価関数を使用する。

なお、式（２）中のｍ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）とから、それらの平面の交線を算出し、それを仮の斜面エッジとする。

なお、この仮の斜面エッジは、本発明における仮傾斜変化部に相当するものである。

次いで、ＳＴＥＰ１１に進み、制御装置３３は、手前側代表平面と奥側代表平面とに関する形状パラメータの代表値と、斜面エッジとを再計算する。具体的には、制御装置３３は、手前側グループに属する実床面部分領域Ａ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)）を推定する。すなわち、カメラ２７Ｌ，２７Ｒの撮像画像内の実床面のうちの、ロボット１の移動動作に必要な部分領域に限定して、実床面の形状の推定が行なわれる。このため、カメラ２７Ｌ，２７Ｒの撮像画像による床面の形状を認識するための演算処理を軽減しつつ、ロボット１の適切な移動制御を行なうことができる。

なお、以上説明した実施形態では、２足歩行ロボット１を例に採って説明したが、３つ以上の脚体を有する脚式移動ロボットについても本発明を適用できることはもちろんである。

また、前記実施形態では、物体が、カメラを搭載したロボットを移動させる床である場合を例に採って説明したが、本発明の適用対象はこれに限られるものではない。例えば、カメラを搭載した車両が走行する道路を物体とし、その道路の路面形状を推定するような場合にも、本発明を適用することができる。

本発明の一実施形態における脚式移動ロボットである２足歩行ロボットの外観斜視図。 図１のロボットに備えた制御システムの概要構成を示すブロック図。 実施形態における床面形状の推定処理を示すフローチャート。 図３のＳＴＥＰ５の処理の詳細を示すフローチャート。 ロボットの足平の複数歩分の着地予定位置と画像投影領域との設定例を説明するための図。 図６（ａ），（ｂ）はロボットの足平の複数歩分の着地予定位置と画像投影領域との設定例を説明するための図。 ロボットの足平の複数歩分の着地予定位置と画像投影領域との設定例を説明するための図。 図８（ａ），（ｂ）はロボットの足平の複数歩分の着地予定位置と画像投影領域との設定例を説明するための図。 ロボットの足平と画像投影領域との関係を示す図。 画像投影領域を撮像画像に投影した状態を状態を示す図。 実床面部分領域の形状パラメータを推定するための収束演算処理における形状パラメータの初期値の設定例を説明するための図。 図１２（ａ），（ｂ）は、実床面部分領域の形状パラメータを推定するための収束演算処理における形状パラメータの初期値の設定手法を説明するためのグラフ。

符号の説明

２７Ｌ，２７Ｒ…カメラ、Ａr(i)…実床面部分領域（物体の平坦部）、Ｈ(i)…射影変換行列。

Claims (2)

1. ２つのカメラの撮像画像中に捉えられている物体の平坦部を観測するために、前記２つのカメラのうちの一方のカメラの撮像画像内の物体の画像を両カメラ間の射影変換行列により変換してなる変換画像と、他方のカメラの撮像画像内の物体の画像とを比較しつつ、該射影変換行列の所定種類のパラメータの値により規定される平面を前記物体の平坦部を包含する平面に収束させるように、該パラメータの値を更新していく収束演算処理を実行する射影変換収束演算処理方法において、
   前記収束演算処理における前記パラメータの初期値により規定される平面が、前記物体の平坦部を包含する実際の平面に対して傾斜した平面になることを該初期値が満たすべき制約条件とし、該初期値を該制約条件を満たす値に設定したことを特徴とする射影変換収束演算処理方法。
2. 前記パラメータの初期値により規定される平面は、該平面の水平面に対する傾斜角度が、前記物体の平坦部を包含する実際の平面の水平面に対する傾斜角度が採り得る値の範囲から逸脱するようにあらかじめ定められた所定角度となる平面であることを特徴とする請求項１記載の射影変換収束演算処理方法。

Images