JP2009258779A

JP2009258779A - 移動ロボット及び足跡計画方法

Info

Publication number: JP2009258779A
Application number: JP2008103629A
Authority: JP
Inventors: Yutaka Takaoka; 豊高岡
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2008-04-11
Filing date: 2008-04-11
Publication date: 2009-11-05

Abstract

【課題】信頼性の高い足跡計画を短時間で実行することができると共に、自然な動作で移動可能な移動ロボット及び足跡計画方法を提供する。
【解決手段】本発明に係る移動ロボット１００は、脚部の下端に足平を設け、計画した着地位置に足平を着地させて移動する移動ロボットであって、移動始点Ｓから移動終点Ｇへと至る移動ロボット１００の移動経路Ｒを計画する経路計画部１３と、計画した移動経路Ｒ上において移動始点Ｓから所定の距離にサブゴールＳＧを設定するサブゴール設定部１４と、移動始点Ｓから設定したサブゴールＳＧに到達する足平の着地位置を計画する足跡計画部１５と、を備える。
【選択図】図２

Description

本発明は、計画した着地位置に足平を着地させて移動する移動ロボット及びその足跡計画方法に関する。

近時、自ら移動経路の環境を認識して目的地へと移動する自律移動ロボットが開発されている。このような自律移動ロボットでは、移動経路上に存在する障害物を検出し、検出結果に基づいて目的地へと移動する。

移動ロボットが３次元空間内を移動する脚式歩行ロボットである場合には、移動ロボットは、環境の認識結果に基づいて足平を着地可能な領域を判断し、足平の着地位置を順次計画することで歩行を実現する。尚、本明細書中において以下では、移動ロボットの脚部の下端に設けた足平の着地位置を順次計画することを足跡計画と称する。

非特許文献１には、足の歩幅を考慮して、計画した移動経路に沿って足の着地位置を決定する足跡計画手法が開示されている。非特許文献１に記載の足跡計画手法では、足平の着地姿勢は、その着地位置における計画した移動経路の接線方向に設定する。

非特許文献２には、ステップセットを用いた足跡計画手法が開示されている。例えば、移動ロボットの右足が立脚である場合には、次の一歩である左足が遷移可能な位置姿勢をその歩幅などから求めることができる。そこで、非特許文献２に開示された足跡計画手法では、足平の一歩に関して遷移可能な次の一歩の集合を予め求めておき、それら次の一歩の集合をステップセットとして保持する。そして、ステップセットに含まれる一歩を組み合わせた探索枝の中から、最もコストの低い一歩の組合せを探索することで、最適な足跡計画を行うものである。
左部浩太郎他：「二足歩行ロボットによるステレオ画像を用いた障害物回避と歩行計画」，第８回画像センシングシンポジウム予稿集，(２００２−６)。 "ＰｌａｎｎｉｎｇＢｉｐｅｄＮａｖｉｇａｔｉｏｎＳｔｒａｔｅｇｉｅｓｉｎＣｏｍｐｌｅｘＥｎｖｉｒｏｎｍｅｎｔｓ，" Ｊ．Ｃｈｅｓｔｎｕｔｔ，Ｊ．Ｋｕｆｆｎｅｒ，Ｋ．Ｎｉｓｈｉｗａｋｉ，ａｎｄＳ．Ｋａｇａｍｉ，Ｐｒｏｃｅｅｄｉｎｇｓｏｆｔｈｅ２００３ＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｆｅｒｅｎｃｅｏｎＨｕｍａｎｏｉｄＲｏｂｏｔｓ，Ｏｃｔｏｂｅｒ，２００３．

しかしながら、非特許文献１記載の足跡計画手法では、移動始点から移動終点へと至る全ての着地位置を計画するため足跡計画に多くの処理時間を必要とし、現実的にはオンラインで毎歩更新することが困難なものであった。また、足跡計画を予め実行しておくことで計算コストを減少させることができるものの、計画した足跡計画に基づいてロボットを移動させた場合には、実際には、事前に計画した足平の着地位置が干渉して移動することができないことがある。即ち、予め計画した移動経路に沿って足平の着地位置を計画しているため、例えば急カーブの移動経路をロボットが移動する場合などには、ロボットの足平の着地位置同士が必ずしも遷移可能なものになるとは限らず、予め実行した足跡計画では信頼性が不十分なことがある。

また、非特許文献２記載の足跡計画手法では、遷移可能な次の一歩の集合であるステップセットを用いているため計画された次の一歩が干渉することはないものの、予め計算したステップ集合であるためその微調整が困難であり、足平の着地位置を環境に応じて柔軟に所望の位置に計画することができないという問題がある。例えばユーザがロボットに段差を登らせたいと考えたときには、当該段差の幅や位置に応じた適切な足平の着地位置が必要となるが、予め計算した離散的なステップセットでは、当該適切な足平の着地位置を必ずしも有しているわけではない。この結果、ロボットは上手く足平を着地することができず、段差を登って移動することができないことがある。ここで、段差を登るように足跡計画を実行することができない場合には、当該段差を避けるような足跡計画を実行することも可能ではあるが、本来は移動可能であるにもかかわらずに当該段差を避けて移動することになるため、ロボットの動作はユーザから見て不自然な動作となる。また、ステップセット集合に多数の一歩の集合を保持することでこのような不自然な足跡計画を回避する手法が考えられるものの、ステップセット集合の生成処理には多くの計算時間を要し、さらには、これらステップセットを記憶するために多くのメモリ容量を必要とする。このため、ステップセットに含まれる一歩を組み合わせた探索枝の探索処理にも多くの処理時間を必要とすることになり、現実的には、オンラインで毎歩更新することは困難である。

特に、移動ロボットが３次元空間内を移動する脚式歩行ロボットである場合には、車輪型の移動ロボットとは異なり、足平の着地位置を計画する必要がある。しかし、上述した従来の移動ロボットでは足跡計画に要する計算コストが高く、オンラインで毎歩更新することが困難であるという問題があった。また、移動ロボットが環境に存在する段差などの障害物を乗り越えて移動する場合には、計画された足跡計画に基づいて移動すると不自然な動作になる場合があるという問題があった。

本発明は、かかる課題を解決するためになされたものであり、信頼性の高い足跡計画を短時間で実行することができると共に、自然な動作で移動可能な移動ロボット及び足跡計画方法を提供することを目的とする。

本発明に係る移動ロボットは、脚部の下端に足平を設け、計画した着地位置に前記足平を着地させて移動する移動ロボットであって、移動始点から移動終点へと至る前記移動ロボットの移動経路を計画する経路計画部と、前記計画した移動経路上において前記移動始点から所定の距離にサブゴールを設定するサブゴール設定部と、前記移動始点から前記設定したサブゴールに到達する前記足平の着地位置を計画する足跡計画部と、を備えるものである。

このように、移動終点までの全ての足平の着地位置を計画するのではなく、移動始点から所定の距離に設定したサブゴールまでの足跡計画を実行することで、サブゴールまでの遷移可能性を確保した足平の着地位置を短時間で計画することができるため、環境に対して信頼性の高い一歩をオンラインで計画することができる。

また、前記サブゴール設定部で設定した前記サブゴールの配置を調整するサブゴール調整部を更に備え、前記サブゴール調整部は、前記足跡計画部において、前記移動始点から前記サブゴール設定部で設定した前記サブゴールに到達する足平の着地位置が計画不能な場合に、該足平の着地位置が計画可能となるように前記サブゴール設定部で設定した前記サブゴールの配置を調整し、前記足跡計画部は、前記移動始点から前記サブゴール調整部で調整した前記サブゴールに到達する足平の着地位置を計画するようにしてもよい。これにより、サブゴールまでの足跡計画が計画不能である場合に、当該サブゴールの配置を調整することで、足跡計画の成功確率を上昇させることができるため、自然な動作で移動可能な足跡計画を実行することができる。

さらにまた、前記足跡計画部で計画した前記足平の着地位置を記憶する記憶部を更に備え、前記サブゴール調整部での前記サブゴール調整処理の実行時間が所定の処理時間を超えた場合には、少なくとも１制御周期前に計画して前記記憶部に記憶した前記足平の着地位置に基づいて移動するようにしてもよい。サブゴールまでの足跡計画が計画不能であり、当該サブゴールの配置の調整処理が所定の時間内に終了しなかった場合には、以前の足跡計画における結果を利用することによって、移動ロボットが急停止することを防ぎつつ時間を確保することで、フェールセーフすることができる。

また、前記足跡計画部で計画した複数歩分の前記足平の着地位置のうち、前記移動始点から開始する開始一歩分の足平の着地位置に基づいて移動するようにしてもよい。複数歩分の足平の着地位置を確保し、サブゴールまでの遷移可能性が満足されているそのうちの一歩分の足平の着地位置を利用することで、足跡計画の信頼性を向上させることができ、また、一歩のみを利用することで応答性を向上させることができる。

さらにまた、前記足跡計画部で１制御周期前に計画した複数歩分の前記足平の着地位置のうち、前記移動始点から開始する開始一歩分の着地位置を新たな移動始点として設定し、前記サブゴール設定部は、前記計画した移動経路上において、前記新たな移動始点から所定の距離にサブゴールを設定し、前記足跡計画部は、前記新たな移動始点から前記設定したサブゴールに到達する前記足平の着地位置を計画するようにしてもよい。計画した一歩分の足平の着地位置から、次のサブゴールへと足跡計画を実行することで、足跡計画を認識が可能な所定の範囲に限定することができるため、認識率をより向上させることができると共に、より効率的に足跡計画を実行することができる。

また、前記移動ロボットが移動手段としての２つの脚部を備え、２足歩行運動により移動を行うものとすると好適である。

また、本発明は方法によっても具現化され、本発明に係る足跡計画方法は、計画した着地位置に足平を着地させて移動する移動ロボットの足跡計画方法であって、移動始点から移動終点へと至る前記移動ロボットの移動経路を計画するステップと、前記計画した移動経路上において前記移動始点から所定の距離にサブゴールを設定するステップと、前記移動始点から前記設定したサブゴールに到達する前記足平の着地位置を計画するステップと、を備えるものである。

本発明によれば、信頼性の高い足跡計画を短時間で実行することができると共に、自然な動作で移動可能な移動ロボット及び足跡計画方法を提供することを目的とする。

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

発明の実施の形態１．
本実施の形態１に係る移動ロボットは、脚部の下端に足平を設け、計画した着地位置に足平を着地させて移動する。移動ロボットは、移動始点から移動終点へと至る移動ロボットの移動経路を計画する経路計画部と、その計画した移動経路上において移動始点から所定の距離にサブゴールを設定するサブゴール設定部と、移動始点から設定したサブゴールに到達する足平の着地位置を計画する足跡計画部と、を備える。

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

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

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

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

さらに、レーザレンジファインダ１０は、移動ロボット１００の腰部に備えられており、水平軸を中心に揺動可能な構成となっている。これにより、移動ロボット１００の移動方向前方をスキャンすることができる。より具体的には、例えば図１においては、移動方向前方（ここではｘ軸方向）に向けて、レーザレンジファインダ１０が移動ロボット１００の水平軸（ここではy軸）を中心として上下に揺動可能に取り付けられている。このようにすると、側面視では、レーザ光によるスキャン範囲を移動ロボット１００の移動方向前方に向かって、上下に広がった扇形形状となるように構成することができ、移動に必要な領域のみを効率的にスキャンすることができる。

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

続いて以下では、本実施の形態１に係る移動ロボット１００の制御系について説明する。移動ロボット１００の制御系の構成を図２に示す。図２に示すように、移動ロボット１０の移動計画装置１は、経路計画部１３と、サブゴール設定部１４と、足跡計画部１５と、サブゴール調整部１６と、を備える。また、移動ロボット１００は、移動計画装置１で計画した足跡計画結果３１を記憶する記憶部３０を備える。

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

３次元位置データ生成部１１は、距離画像データの座標変換を行って床面形状データとしての３次元位置データ群を生成する。尚、３次元位置データ群とは、距離画像データに含まれる多数の計測点の位置ベクトルを３次元直交座標系で表したデータの集合である。

また、３次元位置データ生成部１１は、生成した３次元位置データの歪みをオドメトリを用いて補正する。より具体的には、レーザレンジファインダ１０によるスキャン中に、移動ロボット１００が移動することによって発生する検出誤差を補正する。移動ロボット１００はスキャン中に移動するため、検出される３次元位置データは移動ロボット１００の移動量に応じた検出誤差を含む。このため、オドメトリにより、各関節軸に設けられたエンコーダなどの出力に基づいて移動ロボット１００の移動量を算出して、移動ロボット１００が移動することによって発生する検出誤差を補正する。

さらにまた、３次元位置データ生成部１１は、移動ロボット１００の上体部１０１に設けられたジャイロ（図示せず）からの出力に基づいて、３次元位置データ群のピッチ方向の歪みを補正する。即ち、移動ロボット１００の腰部に設けられたジャイロ（図示せず）の出力からピッチ方向の回転成分を検出し、３次元位置データのピッチ方向の検出誤差を補正する。これにより、歩行ロボットにおいて３次元位置データに与える影響が大きいピッチ方向の検出誤差を補正することができる。

環境地図生成部１２は、３次元位置データ群から平面を検出し、複数の平面の集合としての環境地図を生成する。さらに具体的に述べると、環境地図生成部１２は、まず、３次元位置データ群からの平面検出結果をもとに、床面領域を移動ロボット１００が歩行可能な移動可能領域として認識し、床面とは別の平面が含まれる領域を障害物領域として認識する。尚、床面とは、移動ロボット１００の足平１０２ｆ及び１０３ｆが接している平面を意味する。ただし、このような領域認識は一例である。例えば、床面とは高さが異なる平面や斜面であっても、床面との高さの差が所定の値より小さい場合に足平１０２ｆ及び１０３ｆを着地させることが可能であれば、これらの平面を移動可能領域として認識してもよい。更にまた、検出された平面の面積と足平１０２ｆ及び１０３ｆの面積とを比較した結果によって、その平面を移動可能領域として認識するかを決定してもよい。そして、環境地図生成部１２は、認識された移動可能領域及び障害物領域から、複数の平面の集合としての環境地図を生成する。

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

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

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

移動計画装置１は、経路計画部１３と、サブゴール設定部１４と、足跡計画部１５と、サブゴール調整部１６と、を備えている。そして、移動ロボット１００は、移動計画装置１で計画した足跡計画結果３１を記憶する記憶部３０を備える。

経路計画部１３は、移動始点（スタートＳ）から移動終点（ゴールＧ）へと至る移動ロボット１００の移動経路Ｒを計画する。経路計画方法については、公知の経路計画方法を適用可能であるが、例えば、Ａ＊（エースター）経路探索法を用いて、移動経路を計画する。

サブゴール設定部１４は、計画した移動経路上において、移動始点から所定の距離となる位置にサブゴールを設定する。尚、サブゴール設定部１４によるサブゴールの設定方法の詳細については後述する。

足跡計画部１５は、移動始点からサブゴールへと到達する足平の着地位置を計画する。より具体的には、足跡計画部１５は、環境地図生成部１２によって生成された環境地図を参照して、移動ロボット１００の足平の着地位置を計画し、計画した着地位置に到達するための右足平１０２ｆ及び左足平１０３ｆの着地位置を算出する。ここで、足跡計画部１５による足跡計画方法については、公知の足跡計画方法を適用可能であるが、例えば、経路計画部１３で計画した移動経路に沿って足平の着地位置を計画する。尚、足跡計画部１５による足跡計画方法としては、探索法やロジック配置法など、公知の手法を用いることができる。

サブゴール調整部１６は、サブゴール設定部１４で設定したサブゴールの配置を調整する。より具体的には、サブゴール調整部１６は、足跡計画部１３における足跡計画の結果、移動始点からサブゴール設定部１４で設定したサブゴールへと到達する足平の着地位置が計画不能である場合には、その足平の着地位置が計画可能となるように、サブゴール設定部１４で設定したサブゴールの配置を調整する。尚、サブゴール調整部１６によるサブゴールの調整方法の詳細については後述する。

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

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

続いて以下では、移動計画装置１による制御方法の詳細を図４乃至９を用いて説明する。図４は、移動計画装置１による制御手順を示すフローチャートである。

まず、移動計画装置１は、直前の制御周期において計画した複数歩分の足平の着地位置のうち、その最初の一歩の足平の着地位置を新たなスタートＳとして設定する（ステップＳ１０１）。

次いで、経路計画部１３は、環境地図を参照して、スタートＳからゴールＧまでの移動経路Ｒを計画する（ステップＳ１０２）。

次いで、サブゴール設定部１４は、計画された移動経路Ｒ上に、サブゴールＳＧを設定する（ステップＳ１０３）。サブゴールＳＧは、移動経路Ｒ上においてスタートＳから所定の距離Ｄとなる位置に設定する。また、サブゴールＳＧは、障害物の境界線上を避けて設定する。具体的には、障害物の境界線を含む所定の領域（以下、エッジ領域という。）を避けるようにサブゴールＳＧを設定する。

次いで、足跡計画部１５は、スタートＳから設定したサブゴールＳＧまでの足跡計画を実行する（ステップＳ１０４）。即ち、足跡計画部１５は、移動経路Ｒに沿って、移動ロボット１００の右足平１０２ｆ及び左足平１０３ｆの着地位置を算出する。そして、算出した足平の着地位置（足跡計画結果）を記憶部２０に記憶する。

例えば図５に示すように、サブゴール設定部１２は、スタートＳから所定の距離Ｄ＋αとなる位置にサブゴールＳＧを設定する。ここで、スタートＳから所定の距離Ｄ離れた位置は、障害物Ｐ１２のエッジ領域（障害物Ｐ１２の境界線から幅Ｅを有する領域であって、図において斜線で示す領域ＥＲ）に含まれるため、当該エッジ領域を避けるように、所定の距離Ｄ＋αとなる位置にサブゴールＳＧを設定する。そして、足跡計画部１５による足跡計画の結果、移動経路Ｒに沿って、スタートＳからサブゴールＳＧまでの右足平１０２ｆ及び左足平１０３ｆの着地位置が計画される。尚、図に示した例では、右足平１０２ｆは直前の制御周期で計画した一歩の足平の着地位置である。また、図に示した例では、移動ロボット１００が空間中に存在する段差Ｐ１２を乗り越える動作を含む。

このように、ゴールＧまでの足跡計画を実行するのではなく、サブゴールＳＧまでの足跡計画を実行することで、足跡計画を短時間で実行することができる。そして、サブゴールＳＧまでの足平の着地位置は遷移可能性が確保されるため、サブゴールＳＧまでの複数歩分の足平の着地位置のうち、そのうちの一歩分の足平の着地位置を動作計画に用いることで、環境に対して信頼性の高い一歩をオンラインで計画することができる。

さらに、サブゴールＳＧを設定するための所定の距離Ｄについて、例えば１、２歩分相当の距離を所定の距離Ｄとする代わりに、４、５歩分相当の距離を所定の距離Ｄとすることで、移動ロボット１００が実際に移動に使用する最初の一歩以外にも複数歩分の足平の着地位置を確保しておくことができる。これにより、後述するように次の一歩の足跡計画が実現不可能な場合には、それら確保しておいた足平の着地位置を利用することができるため、移動ロボット１００の移動を中断させずに継続することができる。

また、例えば図６（ａ）及び（ｂ）に示すように、移動ロボット１００の移動に伴ってスタートＳ及びサブゴールＧを設定・更新してゆくことで、より効率的に足跡計画を実行することができる。尚、図６では、移動ロボット１００の移動に伴って、移動ロボット１００が認識可能な範囲（スキャン範囲Ｑ）が移動する様子を示している。図６（ａ）に示すように、制御周期ｔにおいて、移動ロボット１００はスタートＳ０からサブゴールＳＧ０まで足跡計画を実行する。そして、図６（ｂ）に示すように、制御周期ｔ＋１において、移動ロボット１００は、制御周期ｔにおいて計画したスタートＳ０からの足跡計画のうち、その最初の一歩分の着地位置を新たなスタートＳ１として設定するとともに、新たにサブゴールＳＧ１を設定して、そのスタートＳ１からサブゴールＳＧ１までの足跡計画を実行する。このように、直前の制御周期で計画した一歩分の足平の着地位置から開始して、次の新たなサブゴールへと足跡計画を実行することで、移動ロボット１００が認識可能な所定の範囲内に限定して足跡計画を実行することができるため、認識率をより向上させることができると共に、より効率的に足跡計画を実行することができる。

次いで、移動計画装置１は、足跡計画部１５がサブゴールＳＧまでの足跡計画を完了することができたか否かを判定する（ステップＳ１０５）。足跡計画を完了することができた場合には、移動計画装置１は、足跡計画部１５による足跡計画結果を記憶部３０に記憶する。そして、移動ロボット１００は、当該足跡計画のうち、最初の一歩分の足平の着地位置に基づいて動作する。

一方、足跡計画部１５によりサブゴールＳＧまでの足跡計画を完了することができなかった場合には、サブゴール調整部１６は、サブゴール設定部１４で設定したサブゴールＳＧの配置を調整する（ステップＳ１０６）。ここでのサブゴールＳＧの配置の調整方法は様々な手法が考えられるが、移動経路Ｒに沿って、足平の着地位置が計画可能となるようにサブゴール設定部１４で設定したサブゴールＳＧの配置を調整する。即ち、サブゴールＳＧの位置・姿勢を所定量変更する。これにより、サブゴールＳＧまでの足跡計画が計画不能である場合に、当該サブゴールＳＧの配置を調整することで足跡計画の成功確率を上昇させ、自然な動作で移動可能な足跡計画を実行することができる。

例えば図７（ａ）に示すように、サブゴールＳＧの配置によっては、サブゴールＳＧまでの足跡計画を完了させることができない場合がある。そこで、図７（ｂ）に示すように、サブゴール調整部１６によって、サブゴールＳＧをサブゴールＳＧ３へと調整することで、サブゴールＳＧ３までの足跡計画を完了させること可能になる場合がある。サブゴールＳＧの調整方法としては、例えば図８（ａ）に示すようにサブゴールＳＧを移動経路Ｒ上でサブゴールＳＧ４へと移動することができる。尚、図８（ｂ）に示すように、サブゴールＳＧとは高さが異なるエッジ領域ＥＲ１にはサブゴールＳＧ４を移動させないようにすることが好ましい。足跡計画における計算処理の複雑化を回避して、足跡計画の成功率を向上させるためである。

次いで、移動計画装置１は、サブゴール調整部１６でのサブゴールＳＧの調整処理の実行時間が所定の処理時間を超えたか否かを判断する（ステップＳ１０７）。サブゴール調整の処理時間が所定の処理時間を越えなかった場合には、ステップＳ１０４へと戻り、調整されたサブゴールＳＧまでの足跡計画を実行する。

一方で、サブゴール調整の処理時間が所定の処理時間を越えた場合には、サブゴール調整処理の実行を中断して、直前の制御周期において計画した足跡計画結果を用いて動作する（ステップＳ１０８）。即ち、サブゴール調整処理についてタイムアウトした場合には、少なくとも１制御周期前に計画した足跡計画結果のうちで、移動ロボット１００が移動に使用済みの一歩分を除いた、その次の一歩の足平の着地位置に基づいて動作する。このように、サブゴールＳＧまでの足跡計画が計画不能であり、かつ、そのサブゴールＳＧの配置の調整処理がタイムアウトした場合には、以前に計画した足跡計画結果を利用して移動ロボット１００の動作が急停止することを防ぐことで、フェールセーフを実現することができる。

例えば図９に示すように、移動ロボット１００は、まず、最初の制御周期ｔにおいて、スタートＳ０からサブゴールＳＧ０までの足跡計画を実行する（即ち、それぞれ１歩分の足平の着地位置を示すデータＤ１乃至５を生成する）。次いで、移動ロボット１００は、次の制御周期ｔ＋１において（即ち、移動に伴って新たな環境認識結果を得た場合において）、一歩目の足平の着地位置データＤ１を移動に消費しながら、スタートＳ１からサブゴールＳＧ１までの次の足跡計画を実行する（即ち、データＤ６乃至１０を生成する）。尚、このとき、サブゴールＳＧ１までの足跡計画が完了できなかった場合には、サブゴールＳＧ１を調整するようにしてもよい。次いで、移動ロボット１００は、次の制御周期ｔ＋２において、一歩目の足平の着地位置データＤ６を消費しながら、スタートＳ２からサブゴールＳＧ２までの次の足跡計画を実行する（即ち、データＤ１１乃至１５を生成する）。ここで、サブゴールＳＧ２までの足跡計画を完了させることができず（例えば、データＤ１５を生成することができない場合）、かつ、サブゴールＳＧ２の調整処理についてタイムアウトした場合には、直前の制御周期ｔ＋１で計画したデータＤ７乃至１０を用いて動作を実現する。即ち、移動ロボット１００は、次の制御周期ｔ＋３において、直前の制御周期ｔ＋１で計画したデータＤ７乃至１０をＤ１１乃至１４として採用し、その最初の一歩のデータＤ７を消費しながら、スタートＳ３からサブゴールＳＧ３までの次の足跡計画を実行する（即ち、データＤ１６乃至２０を生成する）。従って図９に示した例では、移動ロボット１００は、足平の着地位置を示すデータＤ１、Ｄ６、Ｄ７（Ｄ１１）、Ｄ１６に基づいて移動する。

尚、上述したステップＳ１０８では、サブゴール調整の処理時間が所定の処理時間を越えた場合には、サブゴール調整処理の実行を中断して、直前の制御周期において計画した足跡計画結果を用いて動作するものとしたが、ステップＳ１０２で計画した移動経路Ｒが環境変化によって変更された場合には、直前の制御周期において計画した足跡計画結果を採用しないものとしてもよい。即ち、移動経路Ｒ自体が変更された場合には、その変更に伴ってサブゴールＳＧも変更されるため、移動ロボット１００は過去に計画された足跡計画結果を利用せずに、変更されたサブゴールＳＧ５までの新たな足跡計画を実行するようにしてもよい。更に、変更されたサブゴールＳＧ５までの新たな足跡計画を完了させることができない場合には、移動ロボット１００を一時的に停止させ、当該停止位置からサブゴールＳＧ５まで足跡計画を実行するようにしてもよい。

以上説明したように、ゴールＧまでの全ての足平の着地位置について足跡計画を実行するのではなく、サブゴールＳＧまでの足跡計画のみを実行することで、サブゴールＳＧまでの遷移可能性を確保した足平の着地位置を短時間で計画することができ、環境に対して信頼性の高い一歩をオンラインで計画することができる。また、サブゴールＳＧまでの足跡計画が計画不能で完了することができなかった場合には、そのサブゴールＳＧの配置を調整することで、足跡計画の成功確率を上昇させることができ、自然な動作で移動可能な足跡計画を実行することができる。更に、サブゴールＳＧまでの足跡計画が計画不能であり、かつ、サブゴールＳＧの配置の調整処理が所定の時間内に終了しなかった場合には、以前の足跡計画における結果を利用することで、フェールセーフを実現することができる。

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

本発明の実施の形態に係る移動ロボットの構成を示す側面図である。本発明の実施の形態に係る移動ロボットが有する制御系のブロック構成図である。本発明の実施の形態に係る移動ロボットが移動する環境の一例及び環境地図の一例を示す図である。本発明の実施の形態に係る移動計画装置の処理内容を示すフローチャートである。本発明の実施の形態に係る移動ロボットの処理の様子を概念的に例示する図である。本発明の実施の形態に係る移動ロボットの処理の様子を概念的に例示する図である。本発明の実施の形態に係る移動ロボットの処理の様子を概念的に例示する図である。本発明の実施の形態に係る移動ロボットの処理の様子を概念的に例示する図である。本発明の実施の形態に係る移動ロボットの足跡計画結果の様子を概念的に例示する図である。

符号の説明

１移動計画装置、
１０レーザレンジファインダ、１１３次元位置データ生成部、
１２環境地図生成部、１３経路計画部、１４サブゴール設定部、
１５足跡計画部、１６サブゴール調整部、
１７動作生成部、１８制御部、１９アクチュエータ、２０エンコーダ、
３０記憶部、３１足跡計画結果、

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

１６０環境、１６１障害物、Ｐ１１、Ｐ１２、Ｐ１３平面、２００環境地図、
Ｐ搭乗者、Ｒ１１、１２領域、Ｒ移動経路、
ＥＲエッジ領域、Ｑスキャン範囲

Claims

脚部の下端に足平を設け、計画した着地位置に前記足平を着地させて移動する移動ロボットであって、
移動始点から移動終点へと至る前記移動ロボットの移動経路を計画する経路計画部と、
前記計画した移動経路上において前記移動始点から所定の距離にサブゴールを設定するサブゴール設定部と、
前記移動始点から前記設定したサブゴールに到達する前記足平の着地位置を計画する足跡計画部と、
を備えた移動ロボット。
前記サブゴール設定部で設定した前記サブゴールの配置を調整するサブゴール調整部を更に備え、
前記サブゴール調整部は、前記足跡計画部において、前記移動始点から前記サブゴール設定部で設定した前記サブゴールに到達する足平の着地位置が計画不能な場合に、該足平の着地位置が計画可能となるように前記サブゴール設定部で設定した前記サブゴールの配置を調整し、
前記足跡計画部は、前記移動始点から前記サブゴール調整部で調整した前記サブゴールに到達する足平の着地位置を計画する
ことを特徴とする請求項１記載の移動ロボット。
前記足跡計画部で計画した前記足平の着地位置を記憶する記憶部を更に備え、
前記サブゴール調整部での前記サブゴール調整処理の実行時間が所定の処理時間を超えた場合には、少なくとも１制御周期前に計画して前記記憶部に記憶した前記足平の着地位置に基づいて移動する
ことを特徴とする請求項２記載の移動ロボット。
前記足跡計画部で計画した複数歩分の前記足平の着地位置のうち、前記移動始点から開始する開始一歩分の足平の着地位置に基づいて移動する
ことを特徴とする請求項１乃至３いずれか１項記載の移動ロボット。
前記足跡計画部で１制御周期前に計画した複数歩分の前記足平の着地位置のうち、前記移動始点から開始する開始一歩分の着地位置を新たな移動始点として設定し、
前記サブゴール設定部は、前記計画した移動経路上において、前記新たな移動始点から所定の距離にサブゴールを設定し、
前記足跡計画部は、前記新たな移動始点から前記設定したサブゴールに到達する前記足平の着地位置を計画する
ことを特徴とする請求項４記載の移動ロボット。
前記移動ロボットが移動手段としての２つの脚部を備え、２足歩行運動により移動を行う
ことを特徴とする請求項１乃至５いずれか１項記載の移動ロボット。
計画した着地位置に足平を着地させて移動する移動ロボットの足跡計画方法であって、
移動始点から移動終点へと至る前記移動ロボットの移動経路を計画するステップと、
前記計画した移動経路上において前記移動始点から所定の距離にサブゴールを設定するステップと、
前記移動始点から前記設定したサブゴールに到達する前記足平の着地位置を計画するステップと、
を備えた足跡計画方法。
前記移動始点から前記設定した前記サブゴールに到達する足平の着地位置が計画不能な場合に、該足平の着地位置が計画可能となるように前記設定した前記サブゴールの配置を調整し、
前記移動始点から前記調整した前記サブゴールに到達する足平の着地位置を計画する
ことを特徴とする請求項７記載の足跡計画方法。
前記サブゴールの調整処理の実行時間が所定の処理時間を超えた場合には、少なくとも１制御周期前に計画して記憶した前記足平の着地位置に基づいて移動する
ことを特徴とする請求項８記載の足跡計画方法。
前記計画した複数歩分の前記足平の着地位置のうち、前記移動始点から開始する開始一歩分の足平の着地位置に基づいて移動する
ことを特徴とする請求項７乃至９いずれか１項記載の足跡計画方法。
１制御周期前に計画した複数歩分の前記足平の着地位置のうち、前記移動始点から開始する開始一歩分の着地位置を新たな移動始点として設定し、
前記計画した移動経路上において、前記新たな移動始点から所定の距離にサブゴールを設定し、
前記新たな移動始点から前記設定したサブゴールに到達する前記足平の着地位置を計画する
ことを特徴とする請求項１０記載の足跡計画方法。