JP2008126333A - 脚式移動ロボット及びその歩行計画方法 - Google Patents

Abstract

【課題】脚式移動ロボットに段差が存在する歩行面を移動させる場合に、足平を妥当な位置に着地させることが可能な歩行計画方法及び脚式移動ロボットを提供する。
【解決手段】まず、足平１３１の底面を着地させる床面上の候補位置を決定する。次に、決定した候補位置に基づいて足平１３１の底面を着地させる場合に、足平１３１の底面の接地領域の中心部Ｆｃと対向する床面上の対応中心部を求める。続いて、接地領域に対応する床面上の対応領域中に、対応中心部を含む基準平面に対して凹段差が存在するか否かを判定する。そして、凹段差が存在すると判定した場合には、床面上の対応領域において凹段差が占める面積を所定の面積以下とするように足平１３１の底面を着地させる候補位置を修正する。
【選択図】図８

Description

本発明は、体幹部に接続された複数の脚リンクを有し、複数の脚リンクを用いた歩行動作により移動可能なロボット（以下、脚式移動ロボットと呼ぶ）の歩行計画方法に関する。特に、本発明は、脚式移動ロボットが移動する歩行面の段差の状態に応じた着地位置の決定方法に関する。

脚式移動ロボットの歩行計画を行う装置及び方法が従来から提案されている。例えば、特許文献１には、ロボットが位置している基準面と当該基準面と略平行な平面からなる平面群を入力画像から検出し、さらに、平面群を構成する各平面に存在する障害物を認識することによって、検出された平面群を構成する各平面についての障害物情報を有する環境地図群を作成する環境認識装置が開示されている。さらに、特許文献１には、当該環境認識装置によって得られた環境地図群を参照して、ロボットが現在位置から目的位置まで移動するための移動経路を決定する経路計画装置が開示されている。具体的には、当該経路計画装置は、ロボットが位置している基準平面に障害物が存在する場合に、この基準平面上で障害物を回避する移動経路のみを考慮するのではなく、基準平面と平行な他の平面（例えば基準平面に存在する障害物の上面を含む平面）を用いる移動経路も経路候補に加えて経路計画を行う。これによって、例えば、階段の上り下り等によって高さの異なる平面を経由して目的位置に到達する移動経路を計画することができる。
特開２００５−９２８２０号公報

上述したように、ステレオ画像及び距離画像などの入力画像をもとに脚式移動ロボットが歩行を行う歩行面に存在する段差を検出し、検出された段差の上り下りを伴う移動経路を決定する歩行計画装置及び歩行計画方法が従来存在している。しかしながら、これらの従来技術では、脚式移動ロボットの脚リンクの適切な着地位置の決定方法が不十分であり、脚式移動ロボットに段差が存在する歩行面を移動させる場合に、本来移動可能な平面を移動不可能と判断してしまうおそれがある。

上述した特許文献１には、ロボットが位置している現在の平面から、現在の平面と高さが異なる他の平面に移動可能か否かを判断する際に、他の平面がロボットが移動できるほど広くない場合には、当該他の平面に移動可能と判断しないようすべきであるとの記載がされている。しかしながら、検出した平面の広さが具体的にどのような条件である場合にロボットが移動可能と判断するのかについて具体的な開示はない。例えば、脚リンクの先端に設けられる足平の面積より広い平面のみを移動可能な平面と判断すれば、安全な移動経路を決定できる可能性が高い。しかしながら、足平の下に段差が存在しても安定した着地が可能である場合が想定されるため、このような安全性の高い移動経路が目標位置に速やかに到達するために適切な経路であるとは限らない。

本発明は上述した事情を考慮してなされたものであり、脚式移動ロボットに段差が存在する歩行面を移動させる場合に、脚式移動ロボットの脚リンクの先端に設けられた足平を妥当な位置に着地させることが可能な歩行計画方法及び脚式移動ロボットを提供することを目的とする。

本発明の第１の態様にかかる歩行計画方法は、複数の脚部を備え、該複数の脚部の各々の下端に足首関節を介して足平部が設けられており、前記足平部の底面を床面上に接地させる動作を繰り返すことで床面上を歩行する脚式移動ロボットの歩行計画方法である。本方法では、まず、前記床面上において、前記底面を接地させる候補位置を決定する。次に、前記候補位置に基づいて前記底面を着地させる場合に、前記底面の接地領域の中心部と対向する前記床面上の対応中心部を求める。続いて、前記接地領域に対応する前記床面上の対応領域中に、前記対応中心部を含む基準平面に対して凹段差が存在するか否かを判定する。そして、前記凹段差が存在すると判定した場合には、前記対応領域において前記凹段差が占める面積を所定の面積以下とするように前記候補位置を修正する。

このような方法によって、足平を着地させようとする候補位置において凹段差の存在が検出された場合には、凹段差の上への足平のはみ出しを許容して着地位置を決定することができる。これにより、足平底面の接地領域全体の面積より広い領域のみを足平が着地可能な領域に選択する厳格な方法に比べて、足平の着地位置として選択可能な領域を拡大できる。つまり、足平着地位置の妥当な決定を行うことができる。

また、前記第１の態様にかかる歩行計画方法は前記対応領域中に、前記対応中心部を含む平面に対して凸段差が存在するか否かをさらに判定し、前記凸段差が存在すると判定した場合には、前記凸段差を避け、かつ、前記凸段差と前記足平部の間に所定の隙間を設けた位置に前記候補位置を修正することが望ましい。このように、凸段差との間で所定の隙間を設けて足平を着地させることにより、脚式移動ロボットの歩行時に足平を持ち上げる際に足平と凸段差との間に間隔が生じるため、足平の凸段差へのひっかかりを防止することができる。

また、前記第１の態様にかかる歩行計画方法における前記凸段差又は前記凹段差の存在判定を、前記床面に存在する複数の平面を認識することによって作成された環境地図データを参照し、法線ベクトルが互いに異なる複数の平面が前記対応領域に存在するか否かに基づいて行うことが望ましい。このように異なる平面が存在するか否かを判定することで、段差の探索速度を向上させることができる。つまり、異なる平面が存在しなければ段差が存在しないことを速やかに判定できる。

本発明の第２の態様にかかる歩行計画方法は、複数の脚部を備え、該複数の脚部の各々の下端に足首関節を介して足平部が設けられており、前記足平部の底面を床面上に接地させる動作を繰り返すことで床面上を歩行する脚式移動ロボットの歩行計画方法であって、本方法は、以下の（１）〜（９）の処理を行うことを特徴とする。
（１）歩幅候補値を初期歩幅に設定する。
（２）前記底面を着地させる候補位置を、前記歩幅候補値を用いて算出する。
（３）前記候補位置に基づいて前記底面を着地させる場合に、前記底面の接地領域の中心部と対向する前記床面上の対応中心部を求める。
（４）前記接地領域に対応する床面上の対応領域中に、前記対応中心部を含む基準平面に対して凸段差又は凹段差が存在するか否かを判定する。
（５）前記対応領域において前記対応中心部より前方に前記凸段差が存在すると判定した場合は、前記凸段差の境界と前記足平部との隙間が少なくとも第１の値となる位置まで前記歩幅候補値を縮小する。
（６）前記対応領域において前記対応中心部より前方に前記凹段差が存在すると判定した場合は、前記凹段差の境界から前方側への前記底面のはみ出し量が第２の値以下となる位置まで前記歩幅候補値を縮小する。
（７）前記対応領域において前記対応中心部より後方に前記凸段差が存在すると判定した場合は、前記凸段差の境界から前方側への前記底面のはみ出し量が第３の値以下となる位置まで前記歩幅候補値を縮小する。
（８）前記対応領域において前記対応中心部より後方に前記凹段差が存在すると判定した場合は、前記凹段差の境界から前方側への前記底面のはみ出し量が第４の値以下となる位置まで前記歩幅候補値を縮小する。
（９）前記対応領域において凸段差及び凹段差が検出されなくなるまで、（２）〜（８）の処理を繰り返すことによりより得られる歩幅候補値に基づいて、前記足平部の着地位置を決定する。

つまり前記第２の態様にかかる方法では、歩行面に存在する段差を検出し、検出された段差のタイプを４つのパターンに分類し、検出された段差の分類結果に応じて足平の着地位置を修正することとした。このようなアルゴリズムを採用することによって、足平の着地位置の決定処理を体系的に行うことができ、足平の着地位置の計画を速やかに実行することができる。

本発明の第３の態様にかかる脚式移動ロボットは、床面上を歩行する脚式移動ロボットであって、体幹部と、前記体幹部に連結される複数の脚部と、前記複数の脚部の各々の下端に足首関節を介して設けられる複数の足平部と、前記歩行面を視覚的に認識する視覚センサと、前記視覚センサによる取得情報に基づいて、前記床面の３次元地図データを作成する地図作成部と、前記３次元地図データを用いて前記複数の足平部の着地位置を決定する歩行計画部とを備える。さらに、前記歩行計画部は、前記床面上において、前記足平部の底面を着地させる候補位置を決定し、前記候補位置に基づいて前記底面を着地させる場合に、前記底面の接地領域の中心部と対向する前記床面上の対応中心部を求め、前記接地領域に対応する床面上の対応領域中に、前記対応中心部を含む基準平面に対して凹段差が存在するか否かを、前記３次元地図データを参照して判定し、前記凹段差が存在すると判定した場合には、前記対応領域において前記凹段差が占める面積を所定の面積以下とするように前記候補位置を修正することを特徴とする。

このような構成によって、足平を着地させようとする候補位置において凹段差の存在が検出された場合には、凹段差の上への足平のはみ出しを許容して着地位置を決定することができる。これにより、足平底面の接地領域全体の面積より広い領域のみを足平が着地可能な領域に選択する厳格な方法に比べて、足平の着地位置として選択可能な領域を拡大できる。つまり、足平着地位置の妥当な決定を行うことができる。

本発明により、脚式移動ロボットに段差が存在する歩行面を移動させる場合に、足平を妥当な位置に着地させることが可能な歩行計画方法及び脚式移動ロボットを提供できる。

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

発明の実施の形態１．
本実施の形態にかかるロボット１００は、２つの脚リンクを有する脚式移動ロボットである。さらに、ロボット１００は歩行計画装置１を有する。歩行計画装置１は、ロボットが歩行する歩行面の３次元情報を含む環境地図データを参照して、ロボット１００の２つの脚リンクそれぞれの先端に設けられた足平の着地位置を決定する装置である。ロボット１００は、歩行計画装置１によって決定された足平着地位置を実現する動作データを生成し、生成された動作データをもとにロボット１００の関節群をアクチュエータによって駆動することで歩行する。

まず始めに図１を参照してロボット１００の関節自由度について説明する。図１は、ロボット１００を関節と関節間を繋ぐリンクによって示したモデル図である。ロボット１００は、頭部１０１、２本の脚リンクＬＲ及びＬＬ、２本の腕リンクＡＲ及びＡＬ、並びに、これらが連結される体幹部ＢＤにより構成されている。

ロボット１００の頭部１０１には、外界の３次元の点群データ、つまり距離画像データを取得する視覚センサ１０１が設けられている。頭部１０１を支持する首関節は、ロール方向の関節１０２、ピッチ方向の関節１０３及びヨー方向の関節１０３を有する。右腕リンクＡＲは、肩のピッチ方向の関節１０５、肩のロール方向の関節１０６、上腕のヨー方向の関節１０７、肘のピッチ方向の関節１０８、手首のヨー方向の関節１０９を有し、右腕リンクＡＲの末端には手部１４１Ｒが設けられている。なお、手部１４１Ｒの機構は、保持する物体の形状、種別等によっては決定すればよく、例えば、複数本の指を有する多関節かつ多自由度を有する構造としてもよい。

左腕リンクＡＬは、右腕リンクＡＲと同様の構造を有する。具体的には、左腕リンクＡＬは、５つの関節１１０乃至１１４を有し、その末端に手部１４１Ｌを有する。

右脚リンクＬＲは、腰のヨー方向の関節１１８、腰のピッチ方向の関節１１９、腰のロール方向の関節１２０、膝のピッチ方向の関節１２１、足首のピッチ方向の関節１２２、足首のロール方向の関節１２３を有する。足首の関節１２３の下部には、歩行面と接する足平１３１Ｒが設けられている。

左脚リンクＬＬは、右脚リンクＬＲと同様の構造を有する。具体的には、左脚リンクＬＬは、６個の関節１２４乃至１２９を有し、その末端に足平１３１Ｌを有する。

体幹部１４３は、ヨー方向の関節１１５、ロール方向の関節１１６及びピッチ方向の関節１１７を有する。

続いて以下では、ロボット１００を歩行させるための制御系について説明する。ロボット１００の制御系の構成を図２に示す。図２において、視覚センサ１０は、上述したように、ロボット１００の外界の距離画像データを取得する。具体的には、レーザレンジファインダ等のアクティブ距離センサによって距離画像データを取得する。なお、ＣＣＤ（Charge Coupled Device）イメージセンサ又はＣＭＯＳ（Complementary Metal Oxide Semiconductor）イメージセンサ等の撮像素子を備えた複数のカメラを備え、これら複数のカメラによって撮影した画像データを用いて距離画像データを生成してもよい。具体的には、複数のカメラによって撮影した画像データから対応点を検出し、ステレオ視によって対応点の３次元位置を復元する。ここで、複数の撮影画像における対応点の探索は、複数の撮影画像に対する時空間微分の拘束式を用いた勾配法や相関法等の公知の手法を適用して行えばよい。

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

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

広範囲にわたる環境地図を生成する場合には、視覚センサ１０、つまり頭部１０１を動かすことによって複数の異なる視点における距離画像データを取得し、複数視点での距離画像データを統合した後の距離画像データをもとに環境地図を生成する。ここで、複数視点で得られた距離画像データの統合は、視覚センサ１０による距離画像データ取得時の首関節の角度に基づいて複数の距離画像データを位置合わせすることにより行う。または、複数の距離画像データから対応点を求めることによって、距離画像データ相互の位置合わせを行ってもよい。

歩行計画部１２は、環境地図生成部１１によって作成された環境地図を参照して目標位置を決定し、決定した目標位置に到達するための足平１３１Ｒ及びＬの着地位置を算出する。なお、歩行計画部１２による足平着地位置の決定方法の詳細は後述する。

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

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

なお、ロボット１００が一歩前進するたびに、歩行計画装置１による足平１３１の着地位置の決定を繰り返してもよいし、複数歩単位で足平１３１Ｒ及びＬの着地位置を決定しておき、これに基づいて歩行動作を行ってもよい。

続いて以下では、歩行計画部１２における足平１３１Ｒ及びＬの着地位置の決定手順について、図４及び５に示すフローチャートを用いて説明する。始めにステップＳ１０１では、足平１３１を一歩踏み出す際の歩幅Ｗを基準歩幅Ｗｒに設定する。なお、この後の説明において単に足平１１３０と示した場合は、２つの足平１３１Ｒ及びＬのいずれか、具合的には、着地位置の決定対象とすべき遊脚の足平を示すものとする。

ステップＳ１０２では、歩幅Ｗで遊脚の足平１３１を踏み出したと仮定した場合に、足平拡大領域Ｒａ内において歩行面の凹凸を探索する。ここで、足平拡大領域Ｒａとは、図６（ａ）に示すように、足平１３１の接地領域と、足平１３１の前後方向にそれぞれ幅Ｌａだけ拡大した領域とを合わせた領域である。

具体的な歩行面の凹凸探索は、例えば、以下のように行うことができる。まず、環境地図を参照することによって、足平拡大領域Ｒａと接する歩行面に平面ＩＤが異なる２つの平面が存在するか否かを判定する。このように平面ＩＤの比較を始めに行うことにより、凹凸探索の速度を向上させることができる。つまり、平面ＩＤが同一であれば、凹凸が存在しないことを速やかに判定できる。また、平面ＩＤの違いによって凹凸が存在する可能性がある領域を速やかに判定することがきる。

足平拡大領域Ｒａと接する歩行面に異なる２つの平面が存在する場合には、足平中心Ｆｃが接する平面に足平１３１が着地されると仮定したときの足平１３１の高さｚｉと、足平中心Ｆｃが接する平面と異なるもう一方の平面の高さｚｐとの差分を所定の閾値Ｈｓと比較する。このときの高さｚｉと高さｚｐの差分の絶対値が閾値Ｈｓより大きくなる場合には、足平中心Ｆｃが接する平面から見て凸段差又は凹段差となる平面が存在すると判定される。ここで、足平中心Ｆｃとは、足首関節１２２及び１２３又は１２８及び１２９の中心位置を意味する。すなわち、ｚｐ−ｚｉが閾値Ｈｓより大きければ、足平中心Ｆｃから見て凸段差となる平面が存在することを判定できる。一方、ｚｉ−ｚｐが閾値Ｈｓより大きければ、足平中心Ｆｃから見て凹段差となる平面が存在することを判定できる。なお以下では、足平中心Ｆｃから見て凸段差となる平面を単に"凸段差"と呼び、足平中心Ｆｃから見て凹段差となる平面を単に"凹段差"と呼ぶ。

例えば、図２の歩行面１６０において平面Ｐ７に足平中心Ｆｃが位置している図７（ａ）の例、及び、平面Ｐ４に足平中心Ｆｃが位置している図７（ｂ）の例では、足平中心Ｆｃより前方側でｚｉ−ｚｐが閾値Ｈｓより大きくなった場合に、足平中心Ｆｃより前方側に凹段差の存在が検出される。

上述したステップＳ１０２の探索において足平拡大領域Ｒａ内に凸段差が存在することが検出された場合、ステップＳ１０４〜Ｓ１０９の処理を実行する（ステップＳ１０３）。ステップＳ１０４では、凸段差として検出された平面の前端が足平中心Ｆｃより後方に位置するかどうかを判定する。ステップＳ１０４の判定が成立する場合は、検出された凸段差が、パターンＡ〜Ｄの４種類の段差の存在パターンのうちのパターンＣに該当する段差であると判定する（ステップＳ１０５）。

パターンＣに区分される段差は、図８に示すように、足平中心Ｆｃより後方に凸段差が存在する形態である。パターンＣに区分される凸段差が存在する場所に足平１３１を着地することは、ロボット１００の転倒といった不安定な挙動を招く可能性が高い。このため、凸段差として検出された平面の前端からはみ出し量Ｌｂを余した位置に足平１３１の前端が位置するように歩幅Ｗを縮小する（ステップＳ１０５）。このときの歩幅Ｗの縮小幅Ｓは、図９（ｃ）に示すように、
Ｓ＝ｆ１−Ｌｂ＋ｄ
によって算出することができる。ここで、ｆ１は、足平中心Ｆｃと足平１３１の前端との距離である。また、距離ｄは、足平中心Ｆｃと凸段差の前端との距離である。図３（ａ）に示すようなｘ軸に垂直な段差を仮定すると、距離ｄは、足平中心Ｆｃのｘ座標ｘｃと凸段差の前端のｘ座標ｘｍａｘとの差によって算出できる。

一方、凸段差として検出された平面の前端及び後端が共に足平中心Ｆｃより前方に位置すると判定される場合は、検出された凸段差が、パターンＡ〜Ｄの４種類の段差の存在パターンのうちのパターンＡに該当する段差であると判定する（ステップＳ１０６及びＳ１０７）。

パターンＡに区分される段差は、図８に示すように、足平中心Ｆｃより前方に凸段差が存在する形態である。パターンＡに区分される凸段差が存在する場所に足平１３１を着地すると、足平１３１の着地場所に凸段差が存在することでロボット１００が転倒したり、足平１３１の前端と凸段差との隙間が不十分であるために、足平１３１を再び遊脚として蹴り出す際に凸段差につまずいたりする可能性が高い。このため、凸段差として検出された平面の後端と隙間Ｌａを生じる位置に足平１３１の前端が位置するように歩幅Ｗを縮小する（ステップＳ１０７）。このときの歩幅Ｗの縮小幅Ｓは、図９（ａ）に示すように、
Ｓ＝ｆ１＋Ｌａ−ｄ
によって算出することができる。ここで、ｆ１は、足平中心Ｆｃと足平１３１の前端との距離である。また、距離ｄは、凸段差の後端と足平中心Ｆｃとの距離である。図３（ａ）に示すようなｘ軸に垂直な段差を仮定すると、距離ｄは、凸段差の後端のｘ座標ｘｍｉｎと足平中心Ｆｃのｘ座標ｘｃとの差によって算出できる。

検出された凸段差が、上述したパターンＡ及びＣのいずれにも区分されない場合には、このような凸段差の上に着地することを回避するため、凸段差の後端と隙間Ｌａが生じる位置に足平１３１の前端が位置するように歩幅Ｗを縮小する（ステップＳ１０８）。

ステップＳ１０９では、縮小された歩幅Ｗがゼロ以上であるかを判定し、歩幅Ｗがゼロ以上である場合はステップＳ１０２に戻って処理を繰り返す。一方、歩幅Ｗがゼロより小さくなった場合は、図５に示すステップＳ１１７に分岐して歩幅Ｗをゼロとし、これを確定した歩幅として出力する（ステップＳ１１８）。

次に、上述したステップＳ１０３にて足平拡大領域Ｒａ内に凸段差が検出されなかった場合に実行されるステップＳ１１０からＳ１１６の処理内容について、図５を参照して説明する。ステップＳ１１０では、上述したステップＳ１０２の探索の結果によって、足平縮小領域Ｒｂ内に凹段差が存在するかを判定する。ここで、足平縮小領域Ｒｂとは、足平拡大領域Ｒａ及び足平１３１の接地領域の部分領域であって、図６（ｂ）に示すように、足平１３１の接地領域のうち、足平１３１の前端及び後端からそれぞれ幅Ｌｂだけ縮小した領域である。

ステップＳ１１０にて足平縮小領域Ｒｂに凹段差が存在すると判定された場合、ステップＳ１１１〜Ｓ１１６の処理を実行する。ステップＳ１１１では、凹段差として検出された平面の前端が足平中心Ｆｃより後方に位置するかどうかを判定する。ステップＳ１１１の判定が成立する場合は、検出された凹段差が、パターンＡ〜Ｄの４種類の段差の存在パターンのうちのパターンＤに該当する段差であると判定する（ステップＳ１１２）。

パターンＤに区分される段差は、図８に示すように、足平縮小領域Ｒｂ内であって足平中心Ｆｃより後方に凹段差が存在する形態である。パターンＤに区分される凹段差が存在する場所に足平１３１を着地することは、遊脚の着地時に段差を踏み外すことによってロボット１００が転倒するなどの不安定な挙動を招く可能性が高い。このため、凹段差として検出された平面の前端からはみ出し量Ｌｂを余した位置に足平１３１の前端が位置するように歩幅Ｗを縮小する（ステップＳ１１２）。このときの歩幅Ｗの縮小幅Ｓは、図９（ｄ）に示すように、
Ｓ＝ｆ１−Ｌｂ＋ｄ
によって算出することができる。ここで、ｆ１は、足平中心Ｆｃと足平１３１の前端との距離である。また、距離ｄは、足平中心Ｆｃと凹段差の前端との距離である。図３（ａ）に示すようなｘ軸に垂直な段差を仮定すると、距離ｄは、足平中心Ｆｃのｘ座標ｘｃと凹段差の前端のｘ座標ｘｍａｘとの差によって算出できる。

一方、凹段差として検出された平面の前端及び後端が共に足平中心Ｆｃより前方に位置すると判定される場合は、検出された凹段差が、パターンＡ〜Ｄの４種類の段差の存在パターンのうちのパターンＢに該当する段差であると判定する（ステップＳ１１３及びＳ１１４）。

パターンＢに区分される段差は、図８に示すように、足平縮小領域Ｒｂ内であって足平中心Ｆｃより前方に凹段差が存在する形態である。パターンＢに区分される凹段差が存在する場所に足平１３１を着地することは、遊脚の着地時に段差を踏み外すことによってロボット１００が転倒するなどの不安定な挙動を招く可能性が高い。このため、凹段差として検出された平面の後端からはみ出し量Ｌｂを余した位置に足平１３１の前端が位置するように歩幅Ｗを縮小する（ステップＳ１１４）。このときの歩幅Ｗの縮小幅Ｓは、図９（ｂ）に示すように、
Ｓ＝ｆ１−Ｌｂ−ｄ
によって算出することができる。ここで、ｆ１は、足平中心Ｆｃと足平１３１の前端との距離である。また、距離ｄは、凹段差の後端と足平中心Ｆｃとの距離である。図３（ａ）に示すようなｘ軸に垂直な段差を仮定すると、距離ｄは、凹段差の後端のｘ座標ｘｍｉｎと足平中心Ｆｃのｘ座標ｘｃとの差によって算出できる。

検出された凹段差が、上述したパターンＢ及びＤのいずれにも区分されない場合には、このような凹段差の上に着地することを回避するため、凹段差の後端から隙間Ｌｂを余した位置に足平１３１の前端が位置するように歩幅Ｗを縮小する（ステップＳ１１５）。

ステップＳ１１６では、縮小された歩幅Ｗがゼロ以上であるかを判定し、歩幅Ｗがゼロ以上である場合はステップＳ１０２に戻って処理を繰り返す。一方、歩幅Ｗがゼロより小さくなった場合は、ステップＳ１１７に分岐して歩幅Ｗをゼロとし、これを確定した歩幅として出力する（ステップＳ１１８）。

上述した凸段差及び凹段差の検出に応じた歩幅補正処理を繰り返した結果、足平拡大領域内Ｒａ内の凸段差及び足平縮量領域Ｒｂの凹段差のいずれもが検出されない場合には、このときの歩幅を確定した歩幅として出力する（ステップＳ１１０及びＳ１１８）。

以上に説明したように、検出された段差をパターンＡ〜Ｄのいずれかに分類しながら歩幅の補正を繰り返すことによって、ロボット１００が移動する歩行面に存在する様々な形状の段差に対応することができる。例えば、足平１３１の仮定の着地位置が、図１０の上段に示すような斜度変化が存在する平面Ｐ３とＰ４の境界位置となる場合は、始めの処理ループにおいてパターンＣの凸段差と判定され、平面Ｐ４の後端からはみ出し量Ｌｂを余して歩幅が縮小される。次に、この歩幅の縮小を行った後の着地位置は、図１０の中段に示すように、次の処理ループにおいてパターンＡの凸段差と判定される。このため、図１０の下段に示すように平面Ｐ３の前端と隙間Ｌａを生じる位置まで歩幅が縮小される。

また、例えば、足平１３１の仮定の着地位置が、図１１の上段に示すような階段状の段差が存在する平面Ｐ１とＰ２の境界位置となる場合は、始めの処理ループにおいてパターンＤの凹段差と判定され、平面Ｐ２の後端からはみ出し量Ｌｂを余して歩幅が縮小される。次に、この歩幅の縮小を行った後の着地位置は、図１１の中段に示すように、次の処理ループにおいてパターンＡの凸段差と判定される。このため、図１１の下段に示すように平面Ｐ１の前端と隙間Ｌａを生じる位置まで歩幅が縮小される。

上述したように、歩行計画装置１は、足平１３１を着地させようとする候補位置において凹段差の存在が検出された場合には、凹段差の上への足平１３１のはみ出しを許容して着地位置を決定する。これにより、足平１３１の底面全体の面積より広い領域のみを足平が着地可能な領域に選択する厳格な方法に比べて、足平１３１の着地位置として選択可能な領域を拡大できる。つまり、足平着地位置の妥当な決定を行うことができる。

さらに、歩行計画装置１は、足平１３１を着地させようとする候補位置において、凸段差の存在が検出された場合には、凸段差との間に所定の隙間Ｌａを設けて足平１３１の着地位置を決定する。このように、凸段差との間で所定の隙間を設けて足平１３１を着地させることにより、ロボット１００歩行時に足平１３１を持ち上げる際に、足平１３１と凸段差の間に間隔が生じるため、足平１３１の凸段差へのひっかかりを防止することができる。

さらに、歩行計画装置１は、歩行面に存在する段差を検出し、検出された段差のタイプを上述したパターンＡ〜Ｄの４つのパターンに分類し、検出された段差の分類結果に応じて足平１３１の着地位置を調整することにした。このようなアルゴリズムを採用することによって、足平１３１の着地位置の決定処理を体系的に行うことができるため、足平１３１の着地位置の計画を速やかに実行することができる。

発明の実施の形態２．
発明の実施の形態１おいては、歩行計画装置１が、環境地図を１つの２次元グリッドデータとして作成するものとして説明した。この場合、図４に示したステップＳ１０２において必要となる足平１３１の高さｚｉの計算を、足平拡大領域Ｒａの段差探索の都度行う必要がある。足平１３１の底面の任意の点（ｘｉ，ｙｉ，ｚｉ）の高さｚｉは、足平中心Ｆｃが接する平面の法線ベクトル（ｎａ，ｎｂ，ｎｃ）及び足平中心Ｆｃの位置（ｘｃ，ｙｃ，ｚｃ）によって定まる平面の方程式に、演算対象の探索点のｘｙ座標（ｘｉ，ｙｉ）を代入することによって求めることができる。

本実施の形態にかかる歩行計画装置２は、歩行計画装置１が有する環境地図生成部１１に代えて環境地図生成部２１を有し、歩行計画部１２に代えて歩行計画部２２を有する。環境地図生成部２１は、環境地図生成部１１と同様に平面の検出を行い、検出された平面ごとに２次元グリッドデータを生成し、複数の２次元グリッドデータの集合として環境地図を生成する。このとき、１つの平面に対応するグリッドデータには、各グリッドに対応してｚ軸方向の"現実の高さ"又は"仮の高さ"が保持される。ここで、"現実の高さ"とは現実の歩行面の高さであって、歩行面が現実に存在する領域のグリッドに保持される値である。これに対して"仮の高さ"とは、現実には歩行面が存在しない領域のグリッドに保持される値であって、検出された平面を歩行面が現実には存在しない領域にまで延長したと仮定した場合に算出される平面の高さである。

例えば、図１２に示す歩行面２６０を認識する場合、環境地図生成部２１によって３つの平面Ｐ１１〜Ｐ１３が検出される。さらに、環境地図生成部２１によって、検出された３つの平面に対応して、３つの２次元グリッドデータ３１〜３２を含む環境地図３０が生成される。平面Ｐ１１に対応するグリッドデータ３１は、現実の高さが保持されるグリッド群３１１（図１３の空白で示すグリッド）と仮の高さが保持されるグリッド群３１２（図１３の斜線で示すグリッド）を含む。同様に、平面Ｐ１２に対応するグリッドデータ３２は、現実の高さが保持されるグリッド群３２１と仮の高さが保持されるグリッド群３２２を含む。同様に、平面Ｐ１３に対応するグリッドデータ３３は、現実の高さが保持されるグリッド群３３１と仮の高さが保持されるグリッド群３３２を含む。

歩行計画部２２は、発明の実施の形態１の歩行計画部１１と同様の足平１３１の着地位置の計画を行うが、図４に示したステップＳ１０２において必要な足平１３１の高さｚｉを、環境地図として保持された"現実の高さ"又は"仮の高さ"を読み出すことによって取得する。

本実施の形態では、環境地図生成部２１が、検出された平面毎に２次元グリッドデータを生成するため、発明の実施の形態１の場合に比べて、環境地図を保持するために必要なメモリ量は増大する。しかしながら、歩行計画部２２は、足平拡大領域Ｒａ内に存在する段差を探索する際に、探索点を変更するたびに足平１３１の高さｚｉの計算を行う必要がないため、足平１３１の着地位置の計画を行う際の処理速度を向上させることが可能となる。

その他の実施の形態．
上述した発明の実施の形態１では、説明を簡略化するため、足平１３１の左右方向に沿って段差の境界線が存在する場合に適応する歩行計画装置１の処理について説明した。しかしながら、足平１３１の前後方向に沿って段差の境界線が存在する場合に対して、上述した歩行計画装置１の処理を拡張することは容易である。

具体的には、足平拡大領域Ｒａは、足平１３１の左右方向にも拡大させればよい。また、足平縮小領域Ｒｂは、足平１３１の接地領域に比べて足平１３１の左右方向にも縮小させればよい。また、歩行計画部２２は、足平１３１の前後方向に沿って凹段差の境界線が存在する場合に、所定のはみ出し量Ｌｂ´だけ足平１３１が凹段差上にはみ出すことを許容して着地位置を決定すればよい。さらに、歩行計画部２２は、足平１３１の前後方向に沿って凸段差の境界線が存在する場合に、凸段差と足平１３１との間に所定の隙間Ｌａ´を設けて着地させるよう着地位置を決定してもよい。

つまり、歩行計画装置１は、歩行面上の候補位置に足平１３１を着地させると仮定したときに、足平中心Ｆｃと対向する平面を基準面として、足平１３１の底面に対向する歩行面上の対応領域中に基準面より高さが低い凹段差となる領域が存在する場合には、歩行面上の対応領域において凹段差が占める面積が所定の面積以下になるように、足平１３１を着地させる位置を決定するとよい。ここで、所定の面積は、ロボット１００を支持できる範囲内で適宜設定すればよい。例えば、発明の実施の形態１では、足平の底面の面積より小さい足平縮小領域Ｒｂを定義し、歩行面上の対応領域のうちで足平縮小領域Ｒｂに対応する部分領域に凹段差が含まれないように足平１３１の着地位置を決定している。

また、歩行計画装置１は、歩行面上の候補位置に足平１３１を着地させると仮定したときに、足平中心Ｆｃと対向する平面を基準面として、足平１３１の底面に対向する歩行面上の対応領域中に基準面より高さが高い凸段差となる領域が存在する場合には、検出された凸段差を回避でき、かつ凸段差と足平１３１の間に所定の隙間を有する位置に足平１３１の着地位置を修正するとよい。このような拡張によって、足平１３１の進行方向に対してどのような角度で凸段差又は凹段差の境界線が存在する場合にも、足平１３１の着地位置の妥当な決定が可能である。

また、発明の実施の形態１では、歩行面の凸段差及び凹段差の両方を検出し、これらを回避するように足平１３１の着地位置を修正することとしたが、凸段差及び凹段差のいずれか一方の検出と回避動作のみを行うよう構成することも可能である。少なくとも凹段差の検出と回避処理を実行することにより、足平１３１の着地位置として選択可能な領域を拡大でき、足平着地位置の妥当な決定を行うことができる。

また、発明の実施の形態１では、隙間Ｌａ及びはみ出し量Ｌｂを、足平１３１の前方側と後方側とで共通の値としたが、これらは異なる値でもよい。

また、発明の実施の形態１では、自律移動を行う脚式移動ロボット１００に本発明を適用する場合を説明した。しかしながら、例えば、搭乗者の操縦に従って移動する脚式移動ロボットにも本発明を適用することができる。この場合には、歩行計画部１２が、搭乗者による操縦桿等の操作によって入力された方向に目標位置を設定し、目標位置に向けて足平１３１の着地位置を順次決定するよう構成すればよい。

また、発明の実施の形態１の歩行計画装置１は、最初に最大歩幅Ｗｒを初期歩幅に設定し、凸段差又は凹段差が検出された場合には、歩幅を順次縮小させていくこととした。しかしながら、初期歩幅をロボット１００の最大歩幅より小さい値に設定し、凸段差又は凹段差が検出された場合には、歩幅を広げることで段差を回避する構成としてもよい。例えば、足平１３１の全長に隙間Ｌａを加えた長さだけ最大歩幅より小さい値を初期歩幅に設定し、上述したパターンＢの凹段差又はパターンＣの凸段差を検出した場合には、段差の境界から隙間Ｌａだけ前方に歩幅を広げた位置を足平の着地位置とすることが可能である。

また、発明の実施の形態１のロボット１００が有する足平１３１Ｒ及びＬは、関節を有していないものとして説明した。しかしながら、例えば、足平１３１Ｒ及びＬの先端に関節を介して爪先部を設ける構成としてもよい。このように構成された脚式移動ロボットでは、支持脚であった脚リンクが遊脚となって足平が床面を離れる際に、爪先部のみが接地する姿勢となる場合がある。このため、爪先部のみが接地する姿勢においても凹段差を踏み外すことがないように、はみ出し量Ｌｂが爪先部の長さより小さくなるよう設定すればよい。又は、凹段差上への足平１３１のはみ出しを許容した場合には、爪先部のみが接地する姿勢を禁止して動作データを作成してもよい。

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

本発明にかかる脚式移動ロボットのモデル図である。 本発明にかかる歩行計画装置のブロック構成図である。 本発明にかかる脚式移動ロボットが移動する歩行面の一例及び環境地図の一例を示す図である。 本発明にかかる歩行計画装置の処理内容を示すフローチャートである。 本発明にかかる歩行計画装置の処理内容を示すフローチャートである。 足平拡大領域及び足平縮小領域を説明するための図である。 本発明にかかる歩行計画装置が行う段差検出処理を説明するための図である。 本発明にかかる歩行計画装置が行う段差検出処理を説明するための図である。 本発明にかかる歩行計画装置が行う段差検出処理を説明するための図である。 本発明にかかる歩行計画装置が行う段差検出処理を説明するための図である。 本発明にかかる歩行計画装置が行う段差検出処理を説明するための図である。 本発明にかかる脚式移動ロボットが移動する歩行面の一例を示す図である。 本発明にかかる脚式移動ロボットによって生成される環境地図の一例を示す図である。

符号の説明

１、２ 歩行計画装置
１０ 視覚センサ
１１、２１ 環境地図生成部
１２、２２ 歩行計画部
１３ 動作生成部
１４ 制御部
１５ アクチュエータ
１６ エンコーダ
２０、３０ 環境地図
１００ 脚式移動ロボット
１０１ 頭部
１０２〜１２９ 関節
１３１Ｒ、１３１Ｌ 足平
１４１Ｒ、１４１Ｌ 手部
１６０、２６０ 歩行面
ＬＲ 右脚リンク
ＬＬ 左脚リンク
ＡＲ 右腕リンク
ＡＬ 左腕リンク
ＢＤ 体幹部

Claims (13)

1. 複数の脚部を備え、該複数の脚部の各々の下端に足首関節を介して足平部が設けられており、前記足平部の底面を床面上に接地させる動作を繰り返すことで床面上を歩行する脚式移動ロボットの歩行計画方法であって、
   前記床面上において、前記底面を接地させる候補位置を決定し、
   前記候補位置に基づいて前記底面を着地させる場合に、前記底面の接地領域の中心部と対向する前記床面上の対応中心部を求め、
   前記接地領域に対応する前記床面上の対応領域中に、前記対応中心部を含む基準平面に対して凹段差が存在するか否かを判定し、
   前記凹段差が存在すると判定した場合には、前記対応領域において前記凹段差が占める面積を所定の面積以下とするように前記候補位置を修正する、
   ことを特徴とする歩行計画方法。
2. 前記候補位置の修正は、前記底面の接地領域のうちで前記中心部を含む部分領域に対応する前記対応領域中の対応部分領域内に前記凹段差が含まれないように前記候補位置を決定することにより行う請求項１に記載の歩行計画方法。
3. 前記凹段差の存在を、前記基準平面に前記底面を接地させると仮定したときに、前記接地領域の高さと前記対応領域の高さとの差分が予め定めた閾値より大きいか否かによって判定する請求項１に記載の歩行計画方法。
4. 前記対応領域中に、前記対応中心部を含む平面に対して凸段差が存在するか否かをさらに判定し、
   前記凸段差が存在すると判定した場合には、前記凸段差を避け、かつ、前記凸段差と前記足平部の間に所定の隙間を設けた位置に前記候補位置を修正する請求項１に記載の歩行計画方法。
5. 前記凸段差の存在を、前記基準平面に前記底面を接地させると仮定したときに、前記接地領域からみた前記対応領域の高さが予め定めた閾値より大きいか否かによって判定する請求項４に記載の歩行計画方法。
6. 前記中心部は、前記足首関節の位置を前記底面に投影した位置である請求項１乃至５のいずれかに記載の歩行計画方法。
7. 前記凸段差又は前記凹段差の存在判定を、前記床面に存在する複数の平面を認識することによって作成された環境地図データを参照し、法線ベクトルが互いに異なる複数の平面が前記対応領域に存在するか否かに基づいて行う請求項１又は４に記載の歩行計画方法。
8. 前記環境地図データは、前記床面に存在する平面毎に作成された２次元グリッドデータの集合であって、前記２次元グリッドデータの各々は、前記床面に存在する平面を仮想的に延長した場合の当該平面の仮想的な高さを保持しており、
   前記２次元グリッドデータを参照して得た前記仮想的な高さを用いて、前記凸段差又は前記凹段差の存在判定を行う請求項７に記載の歩行計画方法。
9. 複数の脚部を備え、該複数の脚部の各々の下端に足首関節を介して足平部が設けられており、前記足平部の底面を床面上に接地させる動作を繰り返すことで床面上を歩行する脚式移動ロボットの歩行計画方法であって、
   歩幅候補値を初期歩幅に設定し、
   前記底面を着地させる候補位置を、前記歩幅候補値を用いて算出し、
   前記候補位置に基づいて前記底面を着地させる場合に、前記底面の接地領域の中心部と対向する前記床面上の対応中心部を求め、
   前記接地領域に対応する床面上の対応領域中に、前記対応中心部を含む基準平面に対して凸段差又は凹段差が存在するか否かを判定し、
   前記対応領域において前記対応中心部より前方に前記凸段差が存在すると判定した場合は、前記凸段差の境界と前記足平部との隙間が少なくとも第１の値となる位置まで前記歩幅候補値を縮小し、
   前記対応領域において前記対応中心部より前方に前記凹段差が存在すると判定した場合は、前記凹段差の境界から前方側への前記底面のはみ出し量が第２の値以下となる位置まで前記歩幅候補値を縮小し、
   前記対応領域において前記対応中心部より後方に前記凸段差が存在すると判定した場合は、前記凸段差の境界から前方側への前記底面のはみ出し量が第３の値以下となる位置まで前記歩幅候補値を縮小し、
   前記対応領域において前記対応中心部より後方に前記凹段差が存在すると判定した場合は、前記凹段差の境界から前方側への前記底面のはみ出し量が第４の値以下となる位置まで前記歩幅候補値を縮小し、
   前記対応領域において凸段差及び凹段差が検出されなくなるまで、前記候補位置の算出処理、前記凸段差及び凹段差の存在判定処理、及び前記歩幅候補値の縮小処理を繰り返すことにより得られる歩幅候補値に基づいて、前記足平部の着地位置を決定する歩幅計画方法。
10. 床面上を歩行する脚式移動ロボットであって、
    体幹部と、
    前記体幹部に連結される複数の脚部と、
    前記複数の脚部の各々の下端に足首関節を介して設けられる複数の足平部と、
    前記床面を視覚的に認識する視覚センサと、
    前記視覚センサによる取得情報に基づいて、前記床面の３次元地図データを作成する地図作成部と、
    前記３次元地図データを用いて前記複数の足平部の着地位置を決定する歩行計画部とを備え、
    前記歩行計画部は、
    前記床面上において、前記足平部の底面を着地させる候補位置を決定し、
    前記候補位置に基づいて前記底面を着地させる場合に、前記底面の接地領域の中心部と対向する前記床面上の対応中心部を求め、
    前記接地領域に対応する前記床面上の対応領域中に、前記対応中心部を含む基準平面に対して凹段差が存在するか否かを、前記３次元地図データを参照して判定し、
    前記凹段差が存在すると判定した場合には、前記対応領域において前記凹段差が占める面積を所定の面積以下とするように前記候補位置を修正することを特徴とする脚式移動ロボット。
11. 前記歩行計画部は、前記基準平面に前記底面を接地させると仮定したときに、前記接地領域の高さと前記対応領域の高さとの差分が予め定めた閾値より大きいか否かによって前記凹段差の存在を判定する請求項１０に記載の脚式移動ロボット。
12. 前記歩行計画部は前記対応領域中に、前記対応中心部を含む平面に対して凸段差が存在するか否かをさらに判定し、前記凸段差が存在すると判定した場合には、前記凸段差を避け、かつ、前記凸段差と前記足平部の間に所定の隙間を設けた位置に前記候補位置を修正する請求項１０に記載の脚式移動ロボット。
13. 前記歩行計画部は、前記基準平面に前記底面を接地させると仮定したときに、前記接地領域からみた前記対応領域の高さが予め定めた閾値より大きいか否かによって前記凸段差の存在を判定する請求項１２に記載の脚式移動ロボット。