JP2016218673A - 経路計画装置、及び経路計画方法 - Google Patents

Abstract

【課題】全方位に移動可能な全方位移動体が、ずれ難い移動経路を計画する経路計画装置を提供する。【解決手段】全方位移動体が移動する環境内の環境地図情報を記憶する記憶手段９１と、記憶手段に記憶された環境地図情報と、全方位移動体の自己位置と、全方位移動体が移動する目標位置と、に基づいて、自己位置から前記目標位置までの全方位移動体の各経路候補を算出する経路候補算出手段９２と、全方位移動体が前記経路候補算出手段により算出された各経路候補を移動する際にかかる移動コストを算出し、該各経路候補の中から最小の移動コストとなる経路候補を選択し、該選択した経路候補に基づいて、全方位移動体の移動経路を計画する経路計画手段９３と、を備える。移動コストは、全方位移動体の移動制御のパラメータであって、全方位移動体の経路候補からのずれ易さに関するパラメータに基づき算出するコストを含む。【選択図】図３

Description

本発明は、全方位に移動可能な全方位移動体の移動経路を計画する経路計画装置、及び経路計画方法に関するものである。

例えば、物体の衝突可能性と経路長に関する移動コストを考慮して経路探索を行い、移動体の目標位置までの経路を計画する経路計画装置が知られている（特許文献１参照）。

特開２００５−０３２１９６号公報

上記経路計画装置は、移動体の経路からのずれ易さに関する移動コストを考慮して経路探索を行っていない。このため、移動体は、経路計画装置により計画された経路から大きくずれる虞がある。

本発明は、このような問題点を解決するためになされたものであり、全方位移動体がずれ難い経路を計画できる経路計画装置、及び経路計画方法を提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、全方位に移動可能な全方位移動体の移動経路を計画する経路計画装置であって、前記全方位移動体が移動する環境内の環境地図情報を記憶する記憶手段と、前記記憶手段に記憶された環境地図情報と、前記全方位移動体の自己位置と、前記全方位移動体が移動する目標位置と、に基づいて、前記自己位置から前記目標位置までの前記全方位移動体の各経路候補を算出する経路候補算出手段と、前記全方位移動体が前記経路候補算出手段により算出された各経路候補を移動する際にかかる移動コストを算出し、該各経路候補の中から最小の移動コストとなる経路候補を選択し、該選択した経路候補に基づいて、前記全方位移動体の移動経路を計画する経路計画手段と、を備え、前記移動コストは、前記全方位移動体の移動制御のパラメータであって、前記全方位移動体の経路候補からのずれ易さに関する前記パラメータに基づき算出するコストを含む、ことを特徴とする経路計画装置である。
この一態様において、前記全方位移動体は、移動を行う下体部と、該下体部の上方側に旋回動可能に連結された上体部と、を有しており、前記経路計画手段は、前記全方位移動体の経路候補からのずれ易さに関するコストとして、前記上体部に対する下体部の回転の角速度の積分値を算出してもよい。
この一態様において、前記経路計画手段は、前記経路候補の経路長に関するコストと、前記全方位移動体の経路候補からのずれ易さに関するコストに重み付け係数を乗算した値と、を加算して、前記移動コストを算出してもよい。
この一態様において、前記下体部を移動させる駆動輪の駆動軸と、前記上体部に対する下体部の回転の旋回軸と、のオフセット量、あるいは、駆動輪間の幅が大きいほど、前記重み付け係数を大きく設定してもよい。
上記目的を達成するための本発明の一態様は、全方位に移動可能な全方位移動体の移動経路を計画する経路計画方法であって、前記全方位移動体が移動する環境内の環境地図情報を記憶するステップと、前記記憶された環境地図情報と、前記全方位移動体の自己位置と、前記全方位移動体が移動する目標位置と、に基づいて、前記自己位置から前記目標位置までの前記全方位移動体の各経路候補を算出するステップと、前記全方位移動体が前記算出された各経路候補を移動する際にかかる移動コストを算出し、該各経路候補の中から最小の移動コストとなる経路候補を選択し、該選択した経路候補に基づいて、前記全方位移動体の移動経路を計画するステップと、を含み、 前記移動コストは、前記全方位移動体の移動制御のパラメータであって、前記全方位移動体の経路候補からのずれ易さに関する前記パラメータに基づき算出するコストを含む、ことを特徴とする経路計画方法であってもよい。

本発明によれば、全方位移動体がずれ難い経路を計画できる経路計画装置、及び経路計画方法を提供することを主たる目的とする。

（ａ）一実施形態に係る全方位移動体の概略的構成を示す正面図である。（ｂ）一実施形態に係る全方位移動体の概略的構成を示す側面図である。（ｃ）一実施形態に係る全方位移動体の概略的構成を示す上面図である。 一実施形態に係る全方位移動体の概略的システム構成を示すブロック図である。 一実施形態に係る経路計画装置の概略的構成を示すブロック図である。 経路候補の選択方法を説明するための図である。 経路候補の選択方法を説明するための図である。 一実施形態に係る経路計画方法のフローを示すフローチャートである。 経路からずれが生じる一例を示す図である。

以下、図面を参照して本発明の実施形態について説明する。
本発明の一実施形態に係る経路計画装置は、全方位に移動可能な全方位移動体に搭載され、その移動経路を計画するものである。図１（ａ）乃至（ｃ）は、本発明の一実施形態に係る全方位移動体の概略的構成を示す図である。

全方位移動体１は、例えば、アクティブキャスタ式の全方位移動ロボットとして構成されている。全方位移動体１は、移動を行う下体部２と、下体部２の上方側に旋回動可能に連結された上体部３と、を有している。下体部２は、一対の補助輪２１と一対の駆動輪２２が設けられている。上体部３は、下体部２に対して、旋回軸（Ｙａｗ回転軸）を中心にして相対回動する。駆動輪２２の駆動軸と上体部３の旋回軸とはオフセットしている（交差しない）。全方位移動体１は、駆動軸と旋回軸とを独立して駆動することで、全方位移動を実現する。全方位移動体１は、例えば、駆動軸による駆動輪２２の回転差に加えて、旋回軸による上体部３の相対回動によって全方位移動を実現する。

なお、全方位移動体１は、アクティブキャスタ式の移動ロボットとして構成されているが、これに限定されない。全方位移動体１は、例えば、オムニホイール式の移動ロボットとして構成されてもよく、全方位に移動可能な任意の移動ロボットとして構成されてもよい。

図２は、本実施形態に係る全方位移動体の概略的システム構成を示すブロック図である。全方位移動体１は、各駆動輪２２を駆動する車輪駆動部４と、上体部３を下体部２に対して相対回動させる上体駆動部５と、各駆動輪２２の駆動軸の回転情報を検出する回転センサ６と、上体部３の旋回軸の回転情報を検出する回転センサ７と、車輪駆動部４及び上体駆動部５を制御する制御装置８と、移動経路を計画する経路計画装置９と、を備えている。

車輪駆動部４及び上体駆動部５は、例えば、モータ、減速機などにより構成されている。車輪駆動部４は、制御装置８からの制御信号に応じて、各駆動軸を回転させ各駆動輪２２を駆動する。上体駆動部５は、制御装置８からの制御信号に応じて、旋回軸を回転させ上体部３を下体部２に対して相対回動させる。制御装置８は、経路計画装置９により計画された経路に従って全方位移動体１が移動するように、車輪駆動部４及び上体駆動部５の駆動を制御する。

回転センサ６は、例えば、エンコーダなどにより構成されており、各駆動軸に設けられている。回転センサ７は、例えば、ポテンショメータなどにより構成されており、旋回軸に設けられている。

制御装置８及び経路計画装置９は、例えば、演算処理等を行うＣＰＵ（Central Processing Unit）８ａ、９ａ、ＣＰＵ８ａ、９ａによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ８ｂ、９ｂ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）８ｃ、９ｃなどからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ８ａ、９ａ、メモリ８ｂ、９ｂ及びインターフェイス部８ｃ、９ｃは、データバスなどを介して相互に接続されている。

図３は、本実施形態に係る経路計画装置の概略的構成を示すブロック図である。本実施形態に係る経路計画装置９は、全方位移動体１が移動する環境内の環境地図情報を記憶する記憶部９１と、全方位移動体１の経路候補を算出する経路候補算出部９２と、全方位移動体１の移動経路を計画する経路計画部９３と、備えている。

記憶部９１は、記憶手段の一具体例である。記憶部９１は、例えば、メモリなどにより構成されている。
経路候補算出部９２は、経路候補算出手段の一具体例である。経路候補算出部９２は、例えば、記憶部９１に記憶された環境地図情報と、全方位移動体１が移動する目標位置と、全方位移動体１の自己位置と、に基づいて、自己位置から目標位置までの全方位移動体１の各経路候補（障害物を回避して短距離で移動可能な低コストの経路など）を算出する。経路候補算出部９２は、例えば、Ａ^＊（スター）やダイクストラ法などを用いて経路候補を算出する。ここで、コストとは、全方位移動体１の最善の経路を見つけるための指標であり、例えば、自己位置から目標位置までの経路長が長いほど、そのコストが増加する。経路候補算出部９２は、各駆動輪２２の回転情報に基づいて、全方位移動体１の自己位置をオドメトリにより算出する。目標位置は、記憶部９１などに予め設定されている。

経路計画部９３は、経路計画手段の一具体例である。経路計画部９３は、全方位移動体１が経路候補算出部９２により算出された各経路候補を移動する際にかかる移動コストを算出する。経路計画部９３は、各経路候補の中から最小の移動コストとなる経路候補を選択する。経路計画部９３は、該選択した経路候補を全方位移動体１の移動経路として計画する。

ところで、経路からのずれ易さに関する移動コストを考慮して経路計画を行わなかった場合、全方位移動体は、その計画された経路から大きくずれる虞がある。特に、アクティブキャスタ式の全方位移動体は、その構造が複雑でアクティブキャスタの駆動軸及び旋回軸の回転情報などの移動制御パラメータと、その移動制御パラメータに応じた動力学特性を考慮して制御を行う必要がある。例えば、全方位移動体が動作する際に、移動指令方向に上体部は向いているが、下体部がその指令方向と反対方向に向いていることがある（図７）。この場合、駆動輪を高速に回転させ、上体部に対して下体部を１８０度回転させることとなる。このとき、駆動輪の滑りなどのより、駆動輪の回転情報に基づいてオドメトリにより算出される全方位移動体の自己位置に誤差が生じる。このように、構造が複雑な全方位移動体は、その駆動軸及び旋回軸の回転情報（回転角、回転角速度、回転角加速度）などの移動制御パラメータによって、経路からのずれ易さが異なる。

これに対して、本実施形態に係る経路計画装置９の経路計画部９３は、全方位移動体１の移動制御のパラメータであって、全方位移動体１の経路候補からのずれ易さに関するパラメータに基づき算出するコスト（以下、内部状態コスト）を含む、移動コストを算出する。そして、経路計画部９３は、算出した各移動コストの中から最小の移動コストとなる経路候補を選択し、該選択した経路候補を全方位移動体１の移動経路として計画する。これにより、全方位移動体１がずれ難い経路を計画できる。

経路計画部９３は、例えば、経路候補算出部９２により算出された各経路候補の追従再生をオフラインで実行し、そのときの移動制御パラメータの変化に基づいて内部状態コストを算出する。経路計画部９３は、仮想上で、経路候補算出部９２により算出された各経路候補に従って全方位移動体１を移動させ（追従再生）、そのときの全方位移動体１の移動制御パラメータ（旋回軸や駆動軸の回転情報など）を記憶部９１に記憶させる。ここで、全方位移動体１は、制御装置８からの制御信号に従って理想的に走行するもと仮定する。そして、経路計画部９３は、記憶部９１に記憶した移動制御パラメータに基づいて、各経路候補の内部状態コストを算出する。

ここで、例えば、経路候補を追従再生したときの、経路候補からのずれに関与する全方位移動体１の移動制御パラメータ、あるいは、経路候補からのずれ量をｇ（ｔ）とすると、内部状態コストＣｄは下記式で表すことができる。
Ｃｄ＝∫ｇ（ｔ）ｄｔ

経路候補ｉに関して、経路長に関するコスト（以下、経路長コスト）をＣｐ_ｉ、内部状態コストをＣｄ_ｉ、内部状態コストＣｄ_ｉの重み付け係数をｋ、とする。

経路計画部９３は、記憶部９１の環境地図情報に基づいて、経路候補ｉの経路長コストＣｐ_ｉを幾何学的に算出する。経路計画部９３は、算出した経路長コストＣｐ_ｉ及び内部状態コストをＣｄ_ｉに基づいて、経路候補ｉの総合の移動コストＣａ_ｉを下記式を用いて算出する。
Ｃａ_ｉ＝Ｃｐ_ｉ＋ｋＣｄ_ｉ＝Ｃｐ_ｉ＋ｋ∫ｇ_ｉ（ｔ）ｄｔ

ここで、重み付け係数ｋは、ユーザが任意に設定変更できる。重み付け係数ｋは、経路長コストＣｐ_ｉと内部状態コストＣｄ_ｉとのバランスを考慮して最適に設定する。
例えば、経路候補からのずれが大きくなると予測され、内部状態コストＣｄ_ｉを重視する場合は、重み付け係数ｋを大きく設定する。経路の路面が滑り易い状態、あるいは、全方位移動体１のハードウェア構成が滑り易い設定（遠心力が大きく作用する設定など）、の場合などは、重み付け係数ｋを大きく設定するのが好ましい。より具体的には、経路の路面抵抗が小さくなるほど、旋回軸と駆動軸とのオフセット量が大きいほど、あるいは、駆動輪間の幅が大きいほど、重み付け係数ｋを大きく設定してもよい。一方で、経路候補からのずれが小さいと予測され、内部状態コストＣｄ_ｉを重視しない場合は、重み付け係数ｋを小さく設定する。これにより、経路の路面状態や全方位移動体のハードウェア構成を考慮して、移動コストをより高精度に算出できる。

経路計画部９３は、各経路候補ｉの中から最小の移動コストＣａ_ｉとなる経路候補ｉを選択し、該選択した経路候補ｉを全方位移動体１の移動経路として計画する。なお、移動コストは、経路長コストと内部状態コストと、を含む構成であるが、これに限定されない。移動コストは、例えば、衝突可能性のコストと、経路長コストと、内部状態コストとを含む構成であってもよく、全方位移動体１の移動に影響する任意のコストを含むことができる。

例えば、図４に示す如く、経路候補算出部９２は、自己位置から目標位置までの経路候補（１）〜（３）を算出する。経路候補（１）〜（３）の経路長は同一であるため、その経路長コストも同一となる。しかし、経路候補（１）〜（３）において、全方位移動体１の移動制御パラメータ（旋回軸の回転角度（上体部３の下体部２に対する相対角度）など）は異なるため、その内部状態コストは異なる。

ここで、全方位移動体１は、各駆動輪２２の回転差により方向転換を行う差動二輪駆動を行っている。このため、全方位移動体１の並進速度を一定であると仮定すると、旋回軸を中心とした下体部２の角速度が増加するに従がって駆動輪２２の回転速度が増加し、経路からずれ易くなると推定される。

この例では、上体部３の姿勢は基準座標系に対して固定されている。このため、下体部２の基準座標系に対する回転角度と、上体部３に対する下体部２の回転角度は等価と見なすことができる。以上から、経路計画部９３は、内部状態コストとして、上体部３に対する下体部２の回転の角速度の積分値（旋回軸の角度変化量）を算出する。全方位移動体１は、上述の如く、特に、上体部３に対して下体部２を回転する動作時に、駆動輪２２の滑りが発生し、経路からのずれが生じ易い。すなわち、下体部２の回転の角速度の積分値によって、駆動輪２２の滑りを見積もることができ、経路からのずれ易さを正確に算出できる。

図５に示す如く、経路候補（１）では、上体部３に対して下体部２を１回、時計方向へ９０度回転させている。経路候補（２）では、上体部３に対して下体部２を１回、時計方向へ９０度回転させ、さらに、１回、反時計方向へ９０度回転させている。経路候補（３）では、上体部３に対して下体部２を１回、時計方向へ９０度回転させ、さらに、１回、反時計方向へ９０度回転させている。したがって、経路候補（１）〜（３）の内部状態コストＣｄ_１〜Ｃｄ_３を計算すると、下記関係が成立することが分かる。経路候補（１）の旋回軸の角度変化量が最も小さく、その内部状態コストＣｄ_１が最小となる。
Ｃｄ_１＜Ｃｄ_２＝Ｃｄ_３

なお、経路候補（１）〜（３）の経路長は同一であるため、その経路長コストＣｐ_１〜Ｃｐ_３も同一となる（Ｃｐ_１＝Ｃｐ_２＝Ｃｐ_３）。したがって、経路候補（１）〜（３）の内部状態コストの関係（Ｃｄ_１＜Ｃｄ_２＝Ｃｄ_３）から、経路候補（１）〜（３）の移動コストＣａ１〜Ｃａ３は、（Ｃａ_１＜Ｃａ_２＝Ｃａ_３)の関係になることが分かる。経路計画部９３は、最小の移動コストＣａ_１の経路候補（１）を選択し、該選択した経路候補（１）を全方位移動体１の移動経路として計画する。

図６は、本実施形態に係る経路計画方法のフローを示すフローチャートである。
ユーザは、記憶部９１に全方位移動体１の目標位置を設定する（ステップＳ１０１）。
全方位移動体１は、自律移動を開始する（ステップＳ１０２）。

経路計画装置９の経路候補算出部９２は、記憶部９１に記憶された環境地図情報と、全方位移動体１の目標位置と、全方位移動体１の自己位置と、に基づいて、自己位置から目標位置までの全方位移動体１の各経路候補を算出する（ステップＳ１０３）。経路候補算出部９２は、例えば、経路長コストが所定値以下となる複数の経路候補を算出する。

経路計画部９３は、経路候補算出部９２により算出された各経路候補の追従再生をオフラインで実行し、各経路候補の内部状態コストを算出する（ステップＳ１０４）。
経路計画部９３は、各経路候補の経路長コストを算出し、該算出した経路長コストと、内部状態コストに重み付け係数を乗算した値と、を加算して各経路候補の移動コストを算出する（ステップＳ１０５）。

経路計画部９３は、各経路候補の中から最小の移動コストとなる経路候補を選択し、該選択した経路候補を全方位移動体１の移動経路として計画する（ステップＳ１０６）。
制御装置８は、経路計画装置９の経路計画部９３により計画された移動経路に従って全方位移動体１が移動するように、車輪駆動部４及び上体駆動部５の駆動を制御する（ステップＳ１０７）。

以上、本実施形態に係る経路計画装置９の経路計画部９３は、全方位移動体１の移動制御のパラメータであって、全方位移動体１の経路候補からのずれ易さに関するパラメータに基づき算出するコストを含む、移動コストを算出する。そして、経路計画部９３は、各経路候補の中から最小の移動コストとなる経路候補を選択し、該選択した経路候補を全方位移動体１の移動経路として計画する。これにより、全方位移動体１がずれ難い経路を計画できる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
また、本発明は、例えば、図６に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ 全方位移動体、２ 下体部、３ 上体部、４ 車輪駆動部、５ 上体駆動部、６ 回転センサ、７回転センサ、８ 制御装置、９ 経路計画装置、２１ 補助輪、２２ 駆動輪、９１ 記憶部、９２ 経路候補算出部、９３ 経路計画部

Claims (5)

1. 全方位に移動可能な全方位移動体の移動経路を計画する経路計画装置であって、
   前記全方位移動体が移動する環境内の環境地図情報を記憶する記憶手段と、
   前記記憶手段に記憶された環境地図情報と、前記全方位移動体の自己位置と、前記全方位移動体が移動する目標位置と、に基づいて、前記自己位置から前記目標位置までの前記全方位移動体の各経路候補を算出する経路候補算出手段と、
   前記全方位移動体が前記経路候補算出手段により算出された各経路候補を移動する際にかかる移動コストを算出し、該各経路候補の中から最小の移動コストとなる経路候補を選択し、該選択した経路候補に基づいて、前記全方位移動体の移動経路を計画する経路計画手段と、を備え、
   前記移動コストは、前記全方位移動体の移動制御のパラメータであって、前記全方位移動体の経路候補からのずれ易さに関する前記パラメータに基づき算出するコストを含む、
   ことを特徴とする経路計画装置。
2. 請求項１記載の経路計画装置であって、
   前記全方位移動体は、移動を行う下体部と、該下体部の上方側に旋回動可能に連結された上体部と、を有しており、
   前記経路計画手段は、前記全方位移動体の経路候補からのずれ易さに関するコストとして、前記上体部に対する下体部の回転の角速度の積分値を算出する、
   ことを特徴とする経路計画装置。
3. 請求項２記載の経路計画装置であって、
   前記経路計画手段は、前記経路候補の経路長に関するコストと、前記全方位移動体の経路候補からのずれ易さに関するコストに重み付け係数を乗算した値と、を加算して、前記移動コストを算出する、
   ことを特徴とする経路計画装置。
4. 請求項３記載の経路計画装置であって、
   前記下体部を移動させる駆動輪の駆動軸と、前記上体部に対する下体部の回転の旋回軸と、のオフセット量、あるいは、駆動輪間の幅が大きいほど、前記重み付け係数を大きく設定する、
   ことを特徴とする経路計画装置。
5. 全方位に移動可能な全方位移動体の移動経路を計画する経路計画方法であって、
   前記全方位移動体が移動する環境内の環境地図情報を記憶するステップと、
   前記記憶された環境地図情報と、前記全方位移動体の自己位置と、前記全方位移動体が移動する目標位置と、に基づいて、前記自己位置から前記目標位置までの前記全方位移動体の各経路候補を算出するステップと、
   前記全方位移動体が前記算出された各経路候補を移動する際にかかる移動コストを算出し、該各経路候補の中から最小の移動コストとなる経路候補を選択し、該選択した経路候補に基づいて、前記全方位移動体の移動経路を計画するステップと、を含み、
   前記移動コストは、前記全方位移動体の移動制御のパラメータであって、前記全方位移動体の経路候補からのずれ易さに関する前記パラメータに基づき算出するコストを含む、
   ことを特徴とする経路計画方法。

Images