JP2008264947A

JP2008264947A - 平面検出方法、及び移動ロボット

Info

Publication number: JP2008264947A
Application number: JP2007112284A
Authority: JP
Inventors: Yutaka Takaoka; 豊高岡
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2007-04-20
Filing date: 2007-04-20
Publication date: 2008-11-06
Anticipated expiration: 2027-04-20
Also published as: JP4844459B2

Abstract

【課題】平面検出に要する処理時間を短縮することができる平面検出方法、及び移動ロボットを提供すること。
【解決手段】本発明にかかる平面検出方法は、移動ロボット１００の移動状態に応じて、サンプリング対象領域が異なる複数のテンプレートより任意のテンプレートを選択するステップと、選択されたテンプレートにより定められたサンプリング対象領域において、３次元データ群から２点の３次元データの組をサンプリングし、該２点の３次元データの組に基づいて平面を検出するステップを有するものである。
【選択図】図２

Description

本発明は平面検出方法、及び移動ロボットに関する。

自律移動ロボットは、自ら移動経路の環境を認識して、目的地へと移動する。このため、移動ロボットは、環境中に存在する平面を正確かつ高速に検出する必要がある。特に、移動ロボットが歩行ロボットである場合には、平面検出に要する処理時間が高速移動の制約となるため、より短時間で正確に平面を検出することが重要となる。
従来より、環境から平面を検出する数多くの手法が提案されている。多数の計測点（３次元位置データ群）から平面の方程式を表す３つのパラメータ（平面パラメータ）を検出する手法として、ハフ変換法やランダムサンプリング法が良く知られている。ハフ変換を利用する手法として、例えば特許文献１には、３次元位置データ群から３点以上のデータをサンプリングして平面パラメータを算出し、この平面パラメータを投票空間に直接投票することによって平面を検出する技術が開示されている。
また、２点ランダムサンプリング法は、３次元位置データ群から２点のデータの組をランダムにサンプリングして、２点のデータの組からなるベクトルより平面の法線ベクトルを算出し、この法線ベクトルに沿って平面までの距離を探索することによって平面を検出するものである。
特開２００３−２７１９７５号公報

しかしながら、特許文献１記載の技術では、ノイズの多い３次元位置データ群からであっても平面を安定して抽出することができるものの、３次元位置データ群から３次元位置データをランダムにサンプリングするため、平面検出のために多大な処理時間を必要とするという問題点がある。
一方、２点ランダムサンプリング法は、ハフ変換を利用する手法と比較して、正確かつ安定して平面を抽出できるという利点を有する。例えば、環境に階段などの複数の平面が存在する場合には、ハフ変換による手法に比べて、より多くの平面を安定して正確に検出することができる。
しかし、２点ランダムサンプリング法においても特許文献１記載の技術と同様に３次元位置データ群から３次元位置データをランダムにサンプリングするため、平面検出のために多大な処理時間を必要とするという問題点がある。
本発明は、かかる課題を解決するためになされたものであり、平面検出に要する処理時間を短縮することができる平面検出方法、及び移動ロボットを提供することを目的とする。

本発明にかかる平面検出方法は、平面を検出する平面検出方法であって、移動ロボットの移動状態に応じて、サンプリング対象領域が異なる複数のテンプレートより任意のテンプレートを選択するステップと、選択されたテンプレートにより定められたサンプリング対象領域において、３次元データ群から２点の３次元データの組をサンプリングし、該２点の３次元データの組に基づいて平面を検出するステップとを有するものである。

これにより、移動ロボットの移動状態に応じて、平面検出のために不要となる領域をサンプリングせずに済む。このため、平面検出に要する処理時間を短縮することができる。

また、前記移動ロボットの過去の移動状態における平面情報に基づいて前記テンプレートを選択するようにしてもよい。

これにより、過去の平面情報より、移動ロボットが移動し次のタイミングにおいて集中的に平面検出処理を実行したいサンプリング領域を特定することができる。このため、特定されたサンプリング対象領域を含むテンプレートを自律的かつ容易に選択することができる。

さらにまた、前記サンプリングされた２点の３次元データの組の個数に基づいて前記テンプレートを選択するようにしてもよい。

これにより、テンプレート上においてサンプリングされた２点の３次元データの組の個数に基づいて、測定環境に対するその選択されたテンプレートの適合度合を判断することができる。即ち、環境に応じたより適切なテンプレートを選択させることができる。

また、前記テンプレートは、予めサンプリング対象領域が定められ前記移動ロボットに搭載されたものであるようにしてもよい。

これにより、環境に応じて予めサンプリング領域を定めたテンプレートを移動ロボットが保持することにより、速やかにテンプレートを選択することができる。

さらにまた、前記２点の３次元データの組は、前記テンプレートにより定められたサンプリング対象領域において、予めランダムにサンプリングされたものであるようにしてもよい。

これにより、２点の３次元データの組をランダムにサンプリングする処理を繰り返さずに済み、より高速に２点の３次元データの組を抽出することができる。

本発明にかかる移動ロボットは、所定の環境内を移動する移動ロボットであって、前記移動ロボットの移動状態に応じて、サンプリング対象領域が異なる複数のテンプレートより任意のテンプレートを選択するテンプレート選択部と、選択されたテンプレートにより定められたサンプリング対象領域において、３次元データ群から２点の３次元データの組をサンプリングし、該２点の３次元データの組に基づいて平面を検出する平面検出部とを備えるものである。

また、前記テンプレート選択部は、前記移動ロボットの過去の移動状態における平面情報に基づいて前記テンプレートを選択するようにしてもよい。

さらにまた、前記テンプレート選択部は、前記サンプリングされた２点の３次元データの組の個数に基づいて前記テンプレートを選択するようにしてもよい。

本発明によれば、平面検出に要する処理時間を短縮することができる平面検出方法、及び移動ロボットを提供することを目的とする。

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。尚、各図面において、同一要素には同一の符号を付しており、説明の明確化のため、必要に応じて重複説明を省略する。

発明の実施の形態１．
本実施の形態にかかる移動ロボットは、２つの脚リンクを有する脚式移動ロボットである。さらに、移動ロボットは平面検出装置１を有する。平面検出装置１は、移動ロボットの移動状態に応じて、サンプリング対象領域が異なる複数のテンプレートより任意のテンプレートを選択し、選択されたテンプレートにより定められたサンプリング対象領域において、３次元データ群から２点の３次元データの組をサンプリングし、２点の３次元データの組に基づいて平面を検出する装置である。
移動ロボットは、平面検出装置１によって検出された平面に基づいて移動ロボットが歩行する歩行面の３次元情報を含む環境地図データを決定し、移動ロボットの２つの脚リンクそれぞれの先端に設けられた足平の着地位置を決定する。
さらに、移動ロボットは、決定された足平着地位置を実現するための動作データを生成し、生成された動作データをもとに移動ロボットの関節群をアクチュエータによって駆動することで歩行する。

図１を用いて、本発明にかかる移動ロボット１００の構成について説明する。図１は、移動ロボット１００を側方（移動ロボット１００の移動する方向を向いて左方向）から見た様子を概略的に表す概略図であり、移動ロボット１００に搭乗者Ｐが搭乗している様子を表している。図１に示すように、視覚センサとしてのレーザレンジファインダ１０が移動ロボット１００に取り付けられる。
尚、図１においては、説明の便宜上、移動体１が進行する向き（前後方向）をｘ軸、移動体１が進行する方向について水平方向に直交する向き（左右方向）をｙ軸、移動体の移動する平面から鉛直方向に延びる向き（上下方向）をｚ軸とし、これらの３軸からなる座標系を用いて説明する。すなわち、図１中において、前記ｘ軸は紙面に向かって左右方向、ｙ軸は紙面の奥行方法、ｚ軸は紙面中の上下方向を示す。

図１に示すように、移動ロボット１００は、搭乗者Ｐが搭乗する載置台（図示せず）を含む上体部１０１と、上体部１０１を支持する腰部（図示せず）を介して回動自在に固定される脚部１０４と、腰部に取り付けられたレーザレンジファインダ１０とにより構成される。
脚部１０４は、右脚１０２及び左脚１０３により構成され、右脚１０２は、右膝関節１０２ｃ、右足首関節１０２ｅ、右足平１０２ｆを備え、同様に、左脚１０３は、左膝関節１０３ｃ、左足首関節１０３ｅ、左足平１０３ｆを備える。
図示しないモータからの駆動力が、同じく図示しないプーリ及びベルトを介して伝達されることで、各関節が所望の角度に駆動され、その結果、右脚１０２及び左脚１０３に所望の動きをさせることができる。

移動ロボット１００の上体部１０１には、腰部（図示せず）を介して移動ロボット１００が歩行する環境の距離画像データを検出するレーザレンジファインダ１０が設けられている。より具体的には、例えば図１においては、レーザレンジファインダ１０を、水平方向から移動ロボット１００の足元に向けて傾斜させて設置し、移動ロボット１００の足元近傍を含む前方に向けて赤外線などのレーザ光を照射し、その反射光を受光させる。
このようにすると、移動ロボット１００の足元近傍の距離画像データについて高い分解能を確保することができ、密度の高い足元近傍の距離画像データを取得することができる。

レーザレンジファインダ１０は、床面上の障害物を含む外部環境の距離画像データを取得する。尚、距離画像データとは、環境に存在する計測対象までの距離値を各画素の画素値として有する２次元データである。後述するように、距離画像データの座標変換を行うことにより、外界の３次元の点群データを生成することができる。

さらに、レーザレンジファインダ１０は、移動ロボット１００の腰部に備えられており、鉛直軸を中心に揺動可能な構成となっている。これにより、移動ロボット１００の移動方向前方をスキャンすることができる。
このようにすると、上面視では、レーザ光によるスキャン範囲を移動ロボット１００の移動方向前方に向かって、左右に広がった扇形形状となるように構成することができ、移動に必要な領域のみを効率的にスキャンすることができる。

ここで、レーザレンジファインダ１０が、２次元の距離画像データから３次元の点群データを生成する方法について説明する。
まず、レーザレンジファインダ１０を左右に揺動せずに固定した場合に、レーザ光を床面に垂直に移動方向前方に向かって上下に照射することで、床面に垂直な平面上における障害物の距離画像データを取得する。そして、レーザレンジファインダ１０を移動ロボット１００の鉛直軸を中心に、移動方向前方に向かって左右に揺動しながらスキャンすることで、複数の異なる平面上における距離画像データを取得する。次いで、レーザレンジファインダ１０を揺動可能に支持する関節の角度に基づいて複数の距離画像データを座標変換して位置合わせする。これにより、複数平面上で得られた距離画像データの統合を行うことができる。または、複数の距離画像データから対応点を求めることによって、距離画像データ相互の位置合わせを行ってもよい。
このようにして、前方の領域に含まれる３次元の距離画像データを簡便な構成によって容易に取得することができる。

続いて以下では、本実施の形態にかかる移動ロボット１００の制御系について説明する。移動ロボット１００の制御系の構成を図２に示す。図２に示すように、平面検出装置１は、テンプレート選択部１２及び平面検出部１３を有する。

図２において、レーザレンジファインダ１０は、上述したように、移動ロボット１００の外部環境の距離画像データを取得する。より具体的には、例えば図３に示すように、移動ロボット１００が移動方向（ここではｘ軸方向）へ移動する際に、レーザレンジファインダ１０より移動方向前方に向けてレーザ光を照射することにより、移動方向前方の距離画像データを検出する。
尚、視覚センサとして、レーザレンジファインダに代えてステレオカメラを用いてもよい。つまり、ＣＣＤ（Charge Coupled Device）イメージセンサ又はＣＭＯＳ（Complementary Metal Oxide Semiconductor）イメージセンサ等の撮像素子を備えた複数のカメラを備え、これら複数のカメラによって撮影された画像の視差を利用して距離画像データを生成してもよい。より具体的には、複数のカメラによって撮影した画像データから対応点を検出し、ステレオ視によって対応点の３次元位置を復元する。ここで、複数の撮影画像における対応点の探索は、複数の撮影画像に対する時空間微分の拘束式を用いた勾配法や相関法等の公知の手法を適用して行えばよい。

３次元位置データ生成部１１は、距離画像データの座標変換を行って３次元データ群としての３次元位置データ群を生成する。尚、３次元位置データ群とは、距離画像データに含まれる多数の計測点の位置ベクトルを３次元直交座標系で表したデータの集合である。
例えば図４の画像は、移動ロボット１００により生成される３次元位置データ群の一例を示している。尚、図４は、床面に平行なｘｙ平面に３次元位置データ群がプロットされたものであり、プロットされた各点について高さ情報（ｚ軸方向の位置情報）が存在する。
ここで、図４の画像をフレームと呼ぶ。移動ロボット１００により処理対象となる現在のフレームを現フレームと呼び、現フレームの以前に生成されたフレームを前フレームと呼ぶ。移動ロボット１００が移動することで、前フレームと現フレームとでは３次元位置データ群が変化する。

また、３次元位置データ生成部１１は、オドメトリを用いて、生成した３次元位置データの歪みを補正する。より具体的には、レーザレンジセンサ１０によるスキャン中に、移動ロボット１００が移動することによって発生する検出誤差を補正する。即ち、レーザレンジセンサ１０によるスキャン中に移動ロボット１００が移動するため、検出される床面形状データは、移動ロボット１００の移動量に応じた検出誤差を含む。オドメトリにより、各関節軸に設けられたエンコーダなどの出力に基づいて移動ロボット１００の移動量を算出して、移動ロボット１００が移動することによる検出誤差を補正する。
さらにまた、３次元位置データ生成部１１は、移動ロボット１００の上体部１０１に設けられたジャイロ（図示せず）からの出力に基づいて、３次元位置データ群のピッチ方向の歪みを補正する。即ち、移動ロボット１００の腰部に設けられたジャイロ（図示せず）の出力からピッチ方向の回転成分を検出し、３次元位置データのピッチ方向の検出誤差を補正する。
これにより、歩行ロボットにおいて３次元位置データに与える影響が大きいピッチ方向の検出誤差を補正することができる。

テンプレート選択部１２は、移動ロボット１００の移動状態に応じて、サンプリング対象領域が異なる複数のテンプレートより任意のテンプレートを選択する。尚、テンプレート選択部１２によるテンプレート選択方法の詳細は後述する。

平面検出部１３は、テンプレート選択部１２により選択されたテンプレートにより定められたサンプリング対象領域において、３次元位置データ群から２点の３次元位置データの組をサンプリングし、２点の３次元位置データの組に基づいて平面を検出する。尚、平面検出部１３による平面の検出方法の詳細は後述する。

環境地図生成部１４は、平面検出部１３により検出された平面に基づいて、複数の平面の集合としての環境地図を生成する。さらに具体的に述べると、本実施の形態の環境地図は、２次元平面（ｘｙ平面とする）を格子状に分割したグリッドデータとして生成される。環境地図の各グリッドに対応するデータとして、各グリッドのｘｙ平面に垂直なｚ軸方向の高さ、各グリッドが属する平面を一意に識別可能な平面ＩＤ、及び、各グリッドが属する平面の法線ベクトルが保持される。環境地図生成部１４は、ほぼ同じ高さの隣接するグリッドの集合を１つの平面と認識し、認識した１つの平面に対して一意な平面ＩＤを付与する。

例えば、図５（ａ）に示す環境１６０を移動ロボット１００が歩行する場合、床面に相当する平面Ｐ１１及び障害物１６１の上面に相当する平面Ｐ１２が平面検出装置１により検出される。そして、環境地図生成部１４によって、平面Ｐ１１が移動可能領域として認識され、平面Ｐ１２が障害物領域として認識される。このとき、環境地図生成部１４によって生成される環境地図２００は、図５（ｂ）に示すようになる。
図５（ｂ）の環境地図２００は、各グリッドのデータとして、各平面のそれぞれを一意に識別可能な平面ＩＤ、各グリッドのｚ軸方向の高さ、及び法線ベクトル（ｎａ，ｎｂ，ｎｃ）が保持される。
尚、隣接するグリッド間の平面ＩＤの違い及びｚ軸方向の高さの違いによって右足平１０２ｆ及び左足平１０３ｆの着地位置における路面の凹凸を検出するため、環境地図２００を構成する各グリッドの面積は、右足平１０２ｆ及び左足平１０３ｆの底面の面積より十分に小さくなるよう選択するとよい。

さらに、図５（ｂ）の環境地図２００は、各グリッドのデータとして領域種別を示す情報、つまり移動可能領域であるか障害物領域であるかを示す識別情報が保持される。図５（ｂ）の例では、床面Ｐ１１に対応する領域Ｒ１１が移動可能領域であり、障害物１６１に対応する領域Ｒ１２（図５（ｂ）の斜線領域）が障害物領域である。

歩行計画部１５は、環境地図生成部１４によって生成された環境地図を参照して目標位置を決定し、決定した目標位置に到達するための右足平１０２ｆ及び左足平１０３ｆの着地位置を算出する。

動作生成部１６は、歩行計画部１６によって生成された右足平１０２ｆ及び左足平１０３ｆの着地位置を実現するための動作データを生成する。ここで、動作データとは、移動ロボット１００のＺＭＰ位置、重心位置、右足平１０２ｆ及び左足平１０３ｆの位置及び姿勢、並びに、移動ロボット１００の上体部１０１の位置及び姿勢を含む。

制御部１７は、動作生成部１６によって生成された動作データを入力し、逆運動学演算によって各関節の目標関節角度を算出する。さらに、制御部１７は、算出した目標関節角度とエンコーダ１９によって計測した現在の関節角度をもとに各関節を駆動するためのトルク制御値を算出する。制御部１７によって算出されたトルク制御値に従って各関節を駆動するためのアクチュエータ１８を動作させることにより、移動ロボット１００の歩行が行われる。

続いて以下では、平面検出装置１による平面検出方法の詳細を図６乃至９を用いて説明する。図６は、平面検出装置１による平面検出手順を示すフローチャートである。図７及び８は、テンプレートの作成、及びテンプレートの例を説明するための図である。図９は、テンプレート選択部１２が、移動ロボット１００の移動状態に応じてテンプレートを選択する様子を説明するための図である。

まず、ステップＳ１０１では、テンプレート選択部１２が、移動ロボット１００の過去の移動状態における平面情報としての前フレームの平面検出情報を取得する。より具体的には、例えば、前フレームの平面検出結果に含まれる床面、段差、斜面、階段などの平面情報を取得する。

ステップＳ１０２では、テンプレート選択部１２が、前フレームの平面情報に基づいてテンプレートを選択する。前フレームの平面情報より、移動ロボット１００が移動する方向において、平面検出処理を実行する次のタイミングにおいて集中的に平面を検出したい領域を特定することができる。このため、前フレームの平面情報に基づいて、適切なテンプレートを自律的かつ容易に選択することができる。

ここで、図７及び８を用いてテンプレートについて説明する。
図７は、テンプレートに定められるサンプリング対象領域を説明するための図である。図８は、サンプリング対象領域が異なる複数のテンプレートを説明するための図である。本実施の形態においては、サンプリング対象領域が異なる複数のテンプレートが予め生成され、移動ロボット１００に搭載される。これにより、環境に応じて速やかにテンプレートを選択することができる。

平面検出装置１は、テンプレートにより定められたサンプリング対象領域において、後述するように、３次元位置データ群から２点の３次元位置データの組をサンプリングする。例えば図７（ａ）に示すテンプレートＴ１を用いる場合には、サンプリング対象領域Ｓ１（図７（ａ）の斜線領域）においてサンプリングを行う。

テンプレートＴ１上の各領域に対して、３次元位置データ群の各３次元位置データが対応付けされる。テンプレートＴ１上の領域を示すＤａｔａ（ｉ，ｊ）について、ｉ及びｊを指定することにより、Ｄａｔａ（ｉ，ｊ）に対応する３次元位置データ（ｘ，ｙ，ｚ）を得ることができる。
さらに、サンプリング対象領域Ｓ１において、２点の３次元位置データの組が、予めランダムにサンプリングされ設定されている。より具体的には、２点の３次元位置データの組にそれぞれ対応するＤａｔａ（ｉ_１，ｊ_１）及びＤａｔａ（ｉ_２，ｊ_２）について、（ｉ_１，ｊ_１）及び（ｉ_２，ｊ_２）がランダムにサンプリング対象領域Ｓ１内に設定される。

このように、テンプレートについて、まず、ｉ及びｊの範囲を定めることによりサンプリング対象領域Ｓ１の範囲を定める。さらに、サンプリング対象領域Ｓ１内において、複数の（ｉ_１，ｊ_１）及び（ｉ_２，ｊ_２）の組を予めランダムにサンプリングして設定する。従って、テンプレート上に予め設定されたＤａｔａ（ｉ_１，ｊ_１）及びＤａｔａ（ｉ_２，ｊ_２）に対応する３次元位置データの組からなるベクトルがサンプリングされる（例えば図７（ｂ）参照）。
これにより、２点の３次元位置データの組をランダムにサンプリングする処理を繰り返さずに済み、予め設定されたＤａｔａ（ｉ_１，ｊ_１）及びＤａｔａ（ｉ_２，ｊ_２）に対応した３次元位置データの組を用いることで、より高速に２点の３次元位置データの組を抽出することができる。

移動ロボット１００が移動する環境に応じて、平面検出感度を高めたい領域をサンプリング対象領域とするテンプレートが予め複数作成される。即ち、ベクトルサンプリングを集中して実施する領域が異なるテンプレートが、予め複数作成される。
例えば、図８（ａ）に示すテンプレートＴ０は、移動ロボット１００の足元近傍領域をサンプリング対象領域Ｓ０（図８（ａ）の斜線領域）とするテンプレートの一例である。
図８（ｂ）に示すテンプレートＴ１は、移動ロボット１００の進行方向前方において、フレームの中央近傍に段差や斜面などの検出すべき平面がある場合に、その中央近傍の領域をサンプリング対象領域Ｓ１（図８（ｂ）の斜線領域）とするテンプレートの一例である。
図８（ｃ）に示すテンプレートＴ２は、移動ロボット１００の進行方向前方において、フレーム内に段差や斜面などの検出すべき平面がない場合に、進行方向遠方の領域をサンプリング対象領域Ｓ２（図８（ｃ）の斜線領域）とするテンプレートの一例である。
図８（ｄ）に示すテンプレートＴ３は、移動ロボット１００が回転動作を行う場合に、移動ロボット１００の足元近傍領域をサンプリング対象領域Ｓ３（図８（ｄ）の斜線領域）とするテンプレートの一例である。
尚、図８（ｅ）に示すテンプレートＴ４は、サンプリング対象領域を定めていないテンプレートを示している。

例えば、移動ロボット１００が歩行を開始する時点においては、移動ロボット１００の足元近傍領域において平面検出を正確に実施する必要がある。この場合には、移動ロボット１００の足元近傍領域をサンプリング対象領域とするテンプレート（例えば図８（ａ）参照）を用いてサンプリングを実施することにより、不要な領域において平面検出処理を行わずに済む。このため、足元近傍領域における平面をより効率的に検出することができる。

次いで、図９は、テンプレート選択部１２が、移動ロボット１００の移動状態に応じてテンプレートを選択する一例を説明するための図である。図９は、床面に平行なｘｙ平面に３次元位置データ群がプロットされたものであり、プロットされた各点について高さ情報（ｚ軸方向の位置情報）が存在する。ここでは、移動ロボット１００は白抜矢印Ｄの方向に移動するものとする。
図９（ａ）に示すように、前フレームの平面情報より、移動ロボット１００の進行方向前方に段差や斜面などの検出すべき平面（図９（ａ）において破線で囲まれた領域Ｖ）が存在するものと判定された場合には、テンプレート選択部１２は、フレームの中央近傍の領域Ｕ２をサンプリング対象とするテンプレート（例えば図８（ｂ）に示したテンプレート）を選択する。
さらにまた、例えば図９（ｂ）に示すように、前フレームの平面情報より、移動ロボット１００の進行方向前方に段差や斜面などの検出すべき平面が存在しないものと判定された場合には、テンプレート選択部１２は、進行方向遠方の領域Ｕ３やセンサのセンシング領域の側面領域Ｕ４などをサンプリング対象とするテンプレート（例えば図８（ｃ）に示したテンプレート）を選択する。

図６に戻り説明を続けると、ステップＳ１０３では、平面検出部１３が、ステップＳ１０３において選択されたテンプレートについて、テンプレート上に予め設定されたＤａｔａ（ｉ_１，ｊ_１）及びＤａｔａ（ｉ_２，ｊ_２）に対応する２点の３次元位置データの組みを抽出することにより、ベクトルをサンプリングする。より具体的には、以下の不等式を満足するＤａｔａ（ｉ_１，ｊ_１）及びＤａｔａ（ｉ_２，ｊ_２）に対応する２点の３次元位置データの組みからなるベクトルをサンプリングする。尚、Ｄｉｓｔａｎｃｅ（Ｄａｔａ（ｉ_１，ｊ_１），Ｄａｔａ（ｉ_２，ｊ_２））は、Ｄａｔａ（ｉ_１，ｊ_１）及びＤａｔａ（ｉ_２，ｊ_２）に対応する、それぞれ３次元位置データ（ｘ_１，ｙ_１，ｚ_１）及び（ｘ_２，ｙ_２，ｚ_２）間のユークリッド距離を意味する。また、Ｄ_ｍｉｎ及びＤ_ｍａｘは検出したい平面の範囲に応じて定められるベクトルの大きさに関する条件値であり、予め定められ図示しないメモリに格納されている。
Ｄ_ｍｉｎ＜Ｄｉｓｔａｎｃｅ（Ｄａｔａ（ｉ_１，ｊ_１），Ｄａｔａ（ｉ_２，ｊ_２））＜Ｄ_ｍａｘ
ここで、上記の不等式を満足しないＤａｔａ（ｉ_１，ｊ_１）及びＤａｔａ（ｉ_２，ｊ_２）に対応する２点の３次元位置データの組みは採用されないものとする。

ステップＳ１０４では、テンプレート選択部１２が、ステップＳ１０３においてサンプリングされたベクトルの個数が予め定められた個数以上となっているか否かを判定する。
判定の結果、サンプリングされたベクトルの個数が予め定められた個数以上となっている場合には、ステップＳ１０５へと進む。

一方、ステップＳ１０４における判定の結果、サンプリングされたベクトルの個数が予め定められた個数以上となっていない場合には、ステップＳ１０２へと戻りテンプレートを選択しなおす。ここで、テンプレートを再度選択する場合には、テンプレート選択部１１は、サンプリングされるベクトル数がより多くなるようなテンプレートを選択するように構成されていることが好ましい。

移動ロボット１００の移動に伴って測定環境が変化するため、使用するテンプレートによっては、ベクトルの大きさに関する条件であるＤ_ｍｉｎ及びＤ_ｍａｘを満足して得られるベクトルの個数が変化する。即ち、ベクトルの個数が不十分であったということは、選択されたテンプレートは移動ロボット１００の移動する環境に十分適応したものではなく、ステップＳ１０２において選択されたテンプレートでは平面検出のために必要なベクトルの個数を確保できなかったものと推測される。
このため、条件を満足して得られたベクトルの個数に応じてテンプレートを切替えることにより、環境により適応したテンプレートを選択するように構成する。

ステップＳ１０５では、平面検出部１２が、ステップＳ１０３においてサンプリングされたベクトルに基づいて、３次元位置データ群に含まれる平面の法線を探索する。
具体的には、まず、サンプリングされたベクトルを正規化し原点へと移動する。すると、同一平面上に存在するベクトル群は原点を中心とする円盤状に分布するため、その円盤に直交するベクトルの角度を平面の法線として算出する。

ステップＳ１０６では、平面検出部１２が、ステップＳ１０５において探索された平面について、その平面の距離を探索する。
具体的には、まず、原点からステップＳ１０５において探索された平面の法線ベクトルの方向に、平面上に存在するベクトルの密度が予め定めた閾値以上となる平面を探索する。そして、原点から探索された平面までの距離を検出候補平面までの距離とする。

ステップＳ１０７では、平面検出部１２が、３次元位置データ群に含まれる全ての３次元位置データに対して、３次元位置データが属する平面の平面ＩＤをラベリングする。
尚、平面ＩＤは、ステップＳ１０５からＳ１０６までの処理にかけて探索された複数の平面に対応して設定される。

ステップＳ１０８では、平面検出部１２が、各平面とその平面に対応付けられた３次元位置データとの間の誤差が最小となるように、最小二乗法を利用して最小二乗フィッティング処理を実施する。

尚、上述した３次元位置データ生成部１１、テンプレート選択部１２、平面検出部１３及び環境地図生成部１４により実行される処理は、典型的なコンピュータシステムを用いて実現可能である。具体的には、一定の時間間隔で発生するタイマ割り込みに応じて、図６のフローチャートに示した処理をコンピュータシステムに行わせるためのプログラムをコンピュータシステムに実行させればよい。なお、図６のフローチャートに示した処理をコンピュータシステムに実行させるためのプログラムは、１のプログラムである必要はなく、処理内容により分割された複数のプログラムモジュールによって構成されてもよい。

以上のようにして、移動ロボット１００の移動状態に応じて、平面検出のために不要となる領域をサンプリングせずに済む。このため、平面検出に要する処理時間を短縮することができる。
特に、２点ランダムサンプリング法においては、これまで全領域においてベクトルをランダムにサンプリングしていたため、大きさに関する条件を満足するベクトルがサンプリングされる確率が低くなり、効率的に所望の個数のベクトルを抽出することができなかった。さらに、条件を満足するベクトルを所望の個数抽出するまでサンプリング処理を繰り返す必要があったため、平面検出に要する処理時間が非常に大きなものとなっていた。
本実施の形態によれば、予め、サンプリング対象領域における2点の３次元位置データの組を設定し、当該領域において設定された２点の３次元位置データの組みに基づいてサンプリングを実施する。これにより条件を満足したベクトルを効率よくサンプリングすることができるため、２点ランダムサンプリング法をより高速に実施することができる。
このように、平面検出に要する処理時間を短縮することにより、ひいては、平面検出処理が移動ロボットの高速移動の制約となることを緩和させることができる。

また、本実施の形態によれば、移動ロボット１００の移動状態に応じて、平面検出を実施したい領域において効率的にサンプリングを実施させることができるため、より精度の高い平面検出を行うことができる。
移動ロボット１００の移動に伴って測定環境は変化するため、条件を満足して抽出されるベクトルの個数が得られる確率も変化する。このため、全ての領域をサンプリング対象領域とした場合や、同一のテンプレートを用いてサンプリングを実施し続けた場合には、検出すべき領域から所望の個数のベクトルを抽出することができない場合がある。
本実施の形態によれば、測定環境に応じてテンプレートを切替えることで、条件を満足してサンプリングされるベクトルの個数が得られる確率を環境に応じて向上させることができる。即ち、測定環境に応じてテンプレートを切替えることによりサンプリング対象領域を調整し、平面検出のための検出感度を向上させることができる。従って、より精度の高い平面検出を行うことができる。

尚、本実施の形態においては、予め定められたサンプリング対象領域においてサンプリングを実施するため、全領域をサンプリング対象領域とする従来のランダムサンプリング手法を採用した場合と比較して、ランダム性は若干低下する。しかしながら、サンプリング対象領域が異なる複数のテンプレートを用意して、測定環境に応じてテンプレートを切替えて使用することで、ランダム性の低下を緩和させることができる。

尚、上記の例ではサンプリング対象領域の異なる複数のテンプレートが予め生成され移動ロボット１００に搭載されるものとしたが、これに限定されない。例えば、移動ロボット１００の開始時点においてはフレームの全領域をサンプリング対象領域とするテンプレート（例えば図８（ｅ）のテンプレートＴ４）を使用し、移動ロボットが移動を開始した後は、前フレームの平面情報及び移動ロボット１００の移動量に応じて、テンプレートのサンプリング対象領域を更新するようにしてもよい。

その他の実施の形態．
発明の実施の形態１は、２点ランダムサンプリング法によって平面を検出する一例を示したが、本発明はこれに限定されない。例えば、移動ロボット１００の移動状態に応じて、サンプリング対象領域が異なる複数のテンプレートより任意のテンプレートを選択し、選択されたテンプレートにより定められたサンプリング対象領域において、３次元位置データ群から複数の３次元位置データをサンプリングし、サンプリングされた３次元位置データからハフ変換を利用して平面を検出するように構成してもよい。

尚、上記の例では、脚式移動ロボットに本発明を適用した一例を示したが、車輪等のその他の移動手段を有する移動ロボットに対しても本発明を適用可能である。

さらに、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

本発明の実施の形態にかかる移動ロボットの構成を示す側面図である。本発明の実施の形態にかかる移動ロボットが有する制御系のブロック構成図である。本発明の実施の形態にかかる移動ロボットが移動する際にレーザレンジファインダによるスキャンの様子を説明するための図である。本発明の実施の形態にかかる３次元位置データ群の一例を示す図である。本発明の実施の形態にかかる移動ロボットが移動する環境の一例及び環境地図の一例を示す図である。本発明の実施の形態にかかる平面検出装置の処理内容を示すフローチャートである。本発明の実施の形態にかかるテンプレートの一例を示す図である。本発明の実施の形態にかかるサンプリング対象領域の異なるテンプレートを示す図である。本発明の実施の形態にかかるテンプレートの適用例を示す図である。

符号の説明

１平面検出装置、
１０レーザレンジファインダ、１１３次元位置データ生成部、
１２テンプレート選択部、１３平面検出部、１４環境地図生成部、
１５歩行計画部、１６動作生成部、
１７制御部、１８アクチュエータ、１９エンコーダ

１００移動ロボット、１０４脚部、
１０２右脚、１０２ｃ右膝関節、１０２ｅ右足首関節、１０２ｆ右足平、
１０３左脚、１０３ｃ左膝関節、１０３ｅ左足首関節、１０３ｆ左足平、

１６０環境、１６１障害物、Ｐ１１、１２床面、２００環境地図、
Ｐ搭乗者、Ｒ１１、１２領域、Ｖ領域、
Ｔ０〜Ｔ４テンプレート、Ｓ０〜Ｓ３サンプリング対象領域

Claims

平面を検出する平面検出方法であって、
移動ロボットの移動状態に応じて、サンプリング対象領域が異なる複数のテンプレートより任意のテンプレートを選択するステップと、
選択されたテンプレートにより定められたサンプリング対象領域において、３次元データ群から２点の３次元データの組をサンプリングし、該２点の３次元データの組に基づいて平面を検出するステップと
を有する平面検出方法。
前記移動ロボットの過去の移動状態における平面情報に基づいて前記テンプレートを選択する
ことを特徴とする請求項１記載の平面検出方法。
前記サンプリングされた２点の３次元データの組の個数に基づいて前記テンプレートを選択する
ことを特徴とする請求項１又は２記載の平面検出方法。
前記テンプレートは、予めサンプリング対象領域が定められ前記移動ロボットに搭載されたものである
ことを特徴とする請求項１乃至３いずれか１項記載の平面検出方法。
前記２点の３次元データの組は、前記テンプレートにより定められたサンプリング対象領域において、予めランダムにサンプリングされたものである
ことを特徴とする請求項４記載の平面検出方法。
所定の環境内を移動する移動ロボットであって、
前記移動ロボットの移動状態に応じて、サンプリング対象領域が異なる複数のテンプレートより任意のテンプレートを選択するテンプレート選択部と、
選択されたテンプレートにより定められたサンプリング対象領域において、３次元データ群から２点の３次元データの組をサンプリングし、該２点の３次元データの組に基づいて平面を検出する平面検出部と
を備える移動ロボット。
前記テンプレート選択部は、
前記移動ロボットの過去の移動状態における平面情報に基づいて前記テンプレートを選択する
ことを特徴とする請求項６記載の移動ロボット。
前記テンプレート選択部は、
前記サンプリングされた２点の３次元データの組の個数に基づいて前記テンプレートを選択する
ことを特徴とする請求項６又は７記載の移動ロボット。
前記テンプレートは、予めサンプリング対象領域が定められ前記移動ロボットに搭載されたものである
ことを特徴とする請求項６乃至８いずれか１項記載の移動ロボット。
前記２点の３次元データの組は、前記テンプレートにより定められたサンプリング対象領域において、予めランダムにサンプリングされたものである
ことを特徴とする請求項９記載の移動ロボット。