(57)【特許請求の範囲】
【請求項１】
移動体に設けられた回動可能なセンサの回転動作をコストとして加味する移動体の経路計画方法であって、
前記回動可能なセンサのセンサ方向を取得し、
目的地からグリッド単位に探索枝を伸長させ経路点を生成し、
過去に生成された経路点がセンサ計測範囲に収まるように探索枝を伸長させセンサ方向を決定するとともに移動速度を決定し、
前記取得したセンサ角度に基づいて計算したセンサの回転コストと移動体の移動時間コストを合計した合計コストを算出して、その探索結果を探索キューに保存し、
前記探索枝が移動体の現在地に到達しない場合には、前記探索キューに保存された探索結果に基づき前記合計コストが最小となる探索枝を選択して新たな経路点を生成し、
前記探索枝が移動体の現在地に到達するまで、前記生成された経路点から探索枝の伸長を繰り返し行う、
移動体の経路計画方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の経路計画方法に関する。
【背景技術】
【０００２】
近年、自律して移動する移動体が開発されている。移動体は、現在地と目的地の間に障害物が計測された場合に、迂回点を設定し回避行動を行う。このとき、迂回点の生成位置は角度によって制御されるので、安定して迂回させることができる。
【０００３】
特許文献１または特許文献２には、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索方法が開示されている。
【０００４】
特許文献３には、格子領域を示す縦横に区切られたグリッドマップを作成し、移動始点から距離に応じたポテンシャル値を設定し、移動始点から目標グリッドまでの最短経路を探索する移動体について開示されている。移動体は、その探索した経路上のグリッドについてのコストの総和を求める。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１１−１２８７５８号公報
【特許文献２】特開２００９−０２５９７４号公報
【特許文献３】特開２０１１−２２７８０７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
一般に、視野角の広いセンサは視野角の狭いセンサに比べて高コストであり、搭載性が劣ることが多い。したがって自律移動体に搭載させるセンサに、視野角の狭いセンサを利用したいという要望があるが、センサの方向を固定しているのみでは、十分に移動経路上の障害物を検出することができず、安全な障害物回避を行うことができない場合がある。また視野を広げるために、センサをyaw軸方向に回転させ、左右を見渡すように移動することも考えられるが、センサを単純に左右に回転させると不必要な方向まで計測してしまい、本当に観測したい領域を効率的に計測できない。
【課題を解決するための手段】
【０００７】
本発明にかかる移動体の経路計画方法は、移動体に設けられた回動可能なセンサの回転動作をコストとして加味する移動体の経路計画方法であって、前記回動可能なセンサのセンサ方向を取得し、目的地から探索枝を伸長させ経路点を生成し、過去に生成された経路点がセンサ計測範囲に収まるように探索枝を伸長させセンサ方向を決定するとともに移動速度を決定し、前記取得したセンサ角度に基づいて計算したセンサの回転コストと移動体の移動時間コストの合計コストを算出し、前記探索枝が移動体の現在地に到達しない場合には、前記合計コストが最小となる探索枝を選択して新たな経路点を生成し、前記探索枝が移動体の現在地に到達するまで探索枝の伸長を繰り返し行う。
これにより、最初の移動時にセンサの向きに対して効率的な経路を生成し、障害物がないことを確認することができることを保障する経路の中で、最小コストの経路が算出できる。
【発明の効果】
【０００８】
安全と効率を両立した移動体の移動経路を生成することができる。
【図面の簡単な説明】
【０００９】
【図１】実施の形態１にかかる移動体の構成要素を示した図である。
【図２】実施の形態１にかかる移動体の外観を示した図である。
【図３】実施の形態１にかかる移動体を走行させるフローチャートである。
【図４】実施の形態１にかかる移動体の上面図である。
【図５】実施の形態１にかかる経路計画に関する詳細なフローチャートである。
【図６Ａ】実施の形態１にかかる経路点の生成例を示す図である。
【図６Ｂ】実施の形態１にかかる経路点の生成例を示す図である。
【図７Ａ】実施の形態１にかかる探索枝の伸長方向を示す図である。
【図７Ｂ】実施の形態１にかかる探索枝の伸長方向を示す図である。
【図８Ａ】実施の形態１にかかるセンサ方向と経路点の関係を示した図である。
【図８Ｂ】実施の形態１にかかるセンサ方向と経路点の関係を示した図である。
【図９Ａ】実施の形態１にかかるセンサ方向と経路点の関係を示した図である。
【図９Ｂ】実施の形態１にかかるセンサ方向と経路点の関係を示した図である。
【発明を実施するための形態】
【００１０】
実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。図１は、移動体１の構成要素を示した図である。図２は、移動体１の外観を示した図である。移動体１は、目的地設定部１１と、自己位置推定部１２と、センサ駆動部１３と、障害物センサ１４と、既知地図記憶部１５と、地図結合部１６と、経路計画部１７と、駆動制御部１８と、を備える。なお移動体１は、全方位に走行可能な全方位台車であるものとして説明する。
【００１１】
目的地設定部１１は、移動体の目的地を設定する。例えば目的地設定部１１における目的地の設定は、ＧＵＩや、音声認識等のユーザインターフェースを用いて行う。目的地設定部１１は、設定された目的地を、経路計画部１７に出力する。
【００１２】
自己位置推定部１２は、移動体の自己位置を推定する。自己位置推定部１２は、推定した移動体１の現在地を、経路計画部１７に出力する。
【００１３】
センサ駆動部１３は、障害物センサ１４の向きをyaw方向に回動させるように駆動する。またセンサ駆動部１３は、障害物センサ１４の向いた方向を、経路計画部１７に出力する。
【００１４】
障害物センサ１４は、移動体１の周囲の障害物の有無を検出する。障害物センサ１４は、例えばカメラ等である。障害物センサ１４は、取得した障害物データを、地図結合部１６に出力する。なお、以下では障害物センサ１４を、センサと記載する場合がある。
【００１５】
既知地図記憶部１５は、事前に作成された静的障害物が登録された地図情報を記憶する。既知地図記憶部１５は、記憶している地図情報を地図結合部１６に出力する。
【００１６】
地図結合部１６は、既知地図記憶部１５から入力された地図情報と、障害物センサ１４に取得された障害物データを統合して、計画用地図を経路計画部１７に出力する。
【００１７】
経路計画部１７は、経路計画を算出する。具体的には経路計画部１７は、目的地設定部１１により設定された目的地情報と、自己位置推定部１２から入力した現在地と、センサ駆動部１３から入力した障害物センサ１４の向きの情報と、地図結合部１６から入力した計画用地図とを統合し、経路を算出する。経路計画部１７は、駆動制御部１８に経路計画を出力する。
【００１８】
駆動制御部１８は駆動部の動作を制御し、移動体１に、経路計画部１７から入力した経路上を走行させる。
【００１９】
次に、移動体１を走行動作について説明する。図３は、移動体１を自律走行させる場合のフローチャートである。なお実施例の前提条件として、計算を簡潔にするために、上面図から見たような２次元平面で考える。図４は、移動体１を上面からみた図であり、実際は四角錐形状となるセンサ計測範囲を、三角形と仮定する。
【００２０】
目的地設定部１１は、目的地を取得する（ステップＳ１１）。目的地設定部１１は、インターフェースを介して取得された目的地を、経路計画で使用する地図の座標系における表現に変換する。
【００２１】
自己位置推定部１２は、現在位置を取得する（ステップＳ１２）。ここで、自己位置推定部１２により取得された現在位置を、経路計画のスタート位置とする。
【００２２】
障害物センサ１４は、障害物の位置を取得する（ステップＳ１３）。障害物センサ１４により取得された障害物は、地図に登録され、経路計画に使用される。
【００２３】
センサ駆動部１３は、移動体のベース本体に対して、障害物センサ１４がどの方向を向いているかを表す角度を取得する（ステップＳ１４）。
【００２４】
経路計画部１７は、障害物センサ１４の向いた方向を考慮して経路計画を行う（ステップＳ１５）。図５は、経路計画を行うための詳細なフローチャートである。
【００２５】
経路計画部１７は、目的地をスタートとして設定する（ステップＳ１５１）。図６は、経路点の生成例を示している。図６Ａは、移動体１の現在地をスタートとして、目的地に向かって経路点を生成する場合の例であり、図６Ｂは、目的地をスタートとして設定し、移動体の現在地を目的地として設定して経路点を生成する場合の例である。経路計画部１７が、図６Ａのように移動体１の現在地をスタートとして経路を探索すると、探索中にどの方向にセンサを向けるべきかが不明となる。そこで経路計画部１７は、図６Ｂのように目的地をスタートとして設定して経路を探索することで、移動体１の現在地の前方の経路が判明し、センサを向ける方向を決定できる。
【００２６】
経路計画部１７は、探索枝を伸長させることで経路を生成する（ステップＳ１５２）。図７Ａ及び図７Ｂは、グリッドマップを設定し、前回の移動方向に基づいて次の移動方向の探索枝を伸長させる例である。経路計画部１７は、辿ってきたグリッドは通らず、移動体１は急に曲がらないといった条件を加味することで、次の移動方向の探索枝を３方向に絞り、ランダムに選択する。図７Ａは、前回の枝が左下から来た場合であり、探索枝を右、右上、上の３方向に伸ばしている。図７Ｂは、前回の枝が左から来た場合であり、探索枝を右下、右、右上の３方向に伸ばしている。
ただし例外として、移動体１の状態がその場旋回状態である場合には、枝を伸ばさないこととする。その場旋回については、後に詳述する。
【００２７】
経路計画部１７は、障害物センサ１４の向きを決定する（ステップＳ１５３）。具体的には、経路計画部１７は、探索された過去の点をセンサ計測範囲に収めるよう、障害物センサ１４の向きを決定する。なお、探索された過去の点とは、移動体１にとって本来の行き先となる点である。経路計画部１７は、障害物センサ１４の向きは、以下の４つの条件をすべて満たすように決定する。
【００２８】
条件１として、経路計画部１７は、センサ計測範囲に一つ先の経路点を優先的に含むよう経路を生成する。図８Ａは、センサ方向の例を示した図である。図８Ａにおいて、点ａにおけるセンサ計測範囲を確認するものとすると、一つ先の経路点とは点ｂのことである。また優先的に含むとは、一つ先の点を含まず、他の点を含むということがないように、という意味である。
条件２として、経路計画部１７は、センサ計測範囲に移動先のできるだけ多くの経路点を含むよう経路を生成する。図８Ａでは、点aの計測範囲の中に、点ｂと点ｃが含まれた状態を示している。ここで仮に、点aにおける計測範囲に点ｄを含めようとする場合には、点ｂが計測範囲から漏れるため、条件１を満たさなくなる。
条件３として、経路計画部１７は、センサ計測範囲に経路点が含まれる場合であっても、障害物センサ１４と経路点の間に遮蔽物がある場合には計測範囲に含まれないものとみなして経路を生成する。図８Ｂは、遮蔽物がある場合のセンサ範囲について示した図である。図８Ｂのセンサ計測範囲のうち、実線で囲んだ部分が障害物を加味したセンサ計測範囲であり、点線で囲んだ部分が通常のセンサ計測範囲である。図８Ｂにおいて、遮蔽物がなければ点ｈは計測範囲内に含まれるところ、遮蔽物があるために、点ｈは計測範囲に含まれないものとして判定される。
条件４として、経路計画部１７は、前回のセンサ角度に基づいて、最大角度偏差を超えないように経路を生成する。この条件は、トルクや回転数など、移動体１の動作において物理的な制限があることによる条件である。
【００２９】
経路計画部１７は、移動体１の移動速度を決定する（ステップＳ１５４）。具体的には、経路計画部１７は、移動速度を、以下の３つの条件を全て満たすように決定する。
【００３０】
条件１として、経路計画部１７は、センサ計測範囲に含まれる点の数に基づいて移動速度を決定する。例えば、計測最大距離をＤ、経路点の間隔をｄ、ステップＳ１５３で求めた計測範囲に含まれる経路点の数をｎとすると、移動速度は、移動速度＝最大速度×ｎ×（Ｄ／ｄ）により決定することができる。ここで、経路点の間隔とはグリッド地図の解像度と同様の意味である。また、Ｄ／ｄは、計測範囲に含まれる経路点の最大個数の目安を示す。
条件２として、経路計画部１７は、条件１で求めた移動速度と前回求めた移動速度の差が最大速度偏差を超える場合には、最大速度偏差で制限を掛けた移動速度を用いるよう決定する。これは、移動体１に設けられた車輪の速度や加速度などに、物理的な制限があるためである。
条件３として、経路計画部１７は、経路点が計測範囲に一つも含まれない場合には、並進速度を０として、センサ回転と同じ方向へ旋回速度のみを与えるように計画する。これは、経路点を観測することを最優先とするためである。
【００３１】
経路計画部１７は、コストを算出する（ステップＳ１５５）。ここでコストは式（１）により求める。
【数１】
0005831356.tif 000002
ここで、Ｃθは、センサ回転−コスト変換係数であり、ＣＴは時間−コスト変換係数であり、
【数２】
0005831356.tif 000003
は、前回の探索枝のセンサ角度と、現在の探索枝のセンサ角度の差の絶対値であり、
【数３】
0005831356.tif 000004
は、現在の探索枝から前回の探索枝まで移動するのにかかる時間である。
【数４】
0005831356.tif 000005
は、フローチャートのステップＳ１５３で算出した、速度と探索点の距離間隔から近似的に求めることが可能である。この場合、
【数５】
0005831356.tif 000006
ただし、(xi,yi)はｉ番目の経路点の位置であり、viはｉ番目の経路点における移動体１の速度である。ただし、ステップＳ１５３の条件３により、その場回転指令が与えられるのは式（５）のようになる。
【数６】
0005831356.tif 000007
ただし、
【数７】
0005831356.tif 000008
は、センサの回転の最大速度であり、ωmaxは移動体旋回の最大速度である。
【００３２】
経路計画部１７は、探索枝が目的地に到達したか否かを判定する（ステップＳ１５６）。探索枝が目的地に到達していれば（ステップＳ１５６でＹｅｓ）、探索キューをクリアして処理を終了し、次の経路計画に備える（ステップＳ１５７）。ここで探索キューとは、スタートから複数に枝分かれした探索途中の点が保存されるバッファである。
探索枝が目的地に到達していなければ（ステップＳ１５６でＮｏ）、ステップＳ１５８に進む。
【００３３】
経路計画部１７は、探索枝を探索キューに登録する（ステップＳ１５８）。なお経路計画部１７は、探索した全ての探索枝を探索キューに保存する。すなわち、複数回ステップＳ１５８が実行された場合には、以前にステップＳ１５８において保存された探索キューの情報を残した状態で、新たに保存を行う。
【００３４】
経路計画部１７は、探索キューに保存されている探索結果をコスト順にソートする（ステップＳ１５９）。これにより、経路計画部１７が探索した探索枝のうち、最初に目的地にたどり着いた探索枝を最適経路であることを保証する。
【００３５】
経路計画部１７は、探索キューに保存されている複数の探索枝のうち、最小コストの探索枝を選択する（ステップＳ１５１０）。その後、ステップＳ１５２に戻り、処理を繰り返し行う。
【００３６】
経路計画部１７は、ステップＳ１５で計画した経路を駆動制御部１８に出力する（ステップＳ１６）。
【００３７】
駆動制御部１８は、経路計画部１７で行った経路計画に基づき、移動体１を移動させる。また、センサ駆動部１３は、経路計画部１７で行った経路計画に基づいて、センサの向きを回転させる（ステップＳ１７）。
【００３８】
経路計画部１７は、移動体１の移動中に目的地の変更があるか否かを判定する（ステップＳ１８）。目的地の変更がある場合には、（ステップＳ１８でＹｅｓ）、ステップＳ１１に戻り、目的地を更新して処理を繰り返し行う。目的地の変更がない場合には（ステップＳ１８でＮｏ）、ステップＳ１９に進む。
【００３９】
経路計画部１７は、移動体１が目的地に到着したか否かを判定する（ステップＳ１９）。目的地に到着した場合には（ステップＳ１９でＹｅｓ）、移動を終了する。目的地に到着していない場合には（ステップＳ１９でＮｏ）、ステップＳ１２に戻り、処理を繰り返し行う。
【００４０】
これにより、最初の移動時に、センサの向きに対して効率的な経路を生成することが可能であって、不要なセンサの回転を発生させることがない。また、障害物がないことを確認することを保証する回路の中で、最小コストの経路が算出でき、安全の確保と高効率を実現できる。また、障害物がある環境下において、移動体を安全に移動させることができる。
【００４１】
具体的には、遮蔽物がある環境下において移動体１を移動させる場合には、遮蔽物を考慮することで計測範囲に含まれる経路点の数が減少する。そのため、移動体１の移動速度が遅くなり、到達時間が長くなり、経路のコストが大きくなることから、移動体１は、障害物を大回りして障害物を見渡せる経路が選ばれる可能性が高くなる。したがって、より安全に移動体を移動させることができる。
【００４２】
また、移動体が全方位移動台車である場合には、同等コストとなる最適経路が複数存在するが、センサ方向を加味することで、唯一の効率的な経路に絞ることが可能となる。例えば図９Ａは、センサの向いている方向と経路の方向が異なる場合であって、この経路が選択された場合、移動体は、センサ回転とその場旋回の動作を行った後に移動を開始する。図９Ｂは、センサの向いている方向と直近の経路の方向が同方向の場合であって、移動体はすぐに移動を開始することができる。本実施の形態では、センサの回転角度をコストに加味しているため、図９Ｂの経路が選択されることになる。
【００４３】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上記実施の形態では移動体１は全方位台車として説明したが、一般的な２輪駆動タイプの移動体であっても適用可能である。その場合、センサの向きを計算する基準である移動体の本体が、接線方向を向くように移動制御されるため、センサの向きを計算する上で、移動体の旋回方向を考慮する必要がある。
また上記実施の形態では、経路点が計測範囲に含まれるか否かの判定を実施しているが、より安全に移動体を走行させる場合には、経路点の大きさを移動体サイズに膨張させた経路円を生成し、経路円と計測範囲との包含関係を調べることが望ましい。
また上記実施の形態では、Ｃtotalは角度偏差の総和を考慮していたが、センサ回転は最大偏差を考慮するだけにとどめ、コストには含めないことが考えられる。すなわち、
【数８】
0005831356.tif 000009
を、
【数９】
0005831356.tif 000010
と簡略化して用いることができる。この場合、センサ方向を大きく回転させる可能性はあるが、到達時間のみを考慮するため、移動体の移動時間はより最適化される。
【符号の説明】
【００４４】
１    移動体
１１    目的地設定部
１２    自己位置推定部
１３    センサ駆動部
１４    障害物センサ
１５    既知地図記憶部
１６    地図結合部
１７    経路計画部
１８    駆動制御部
【図１】
0005831356.tif 000011
【図２】
0005831356.tif 000012
【図３】
0005831356.tif 000013
【図４】
0005831356.tif 000014
【図５】
0005831356.tif 000015
【図６Ａ】
0005831356.tif 000016
【図６Ｂ】
0005831356.tif 000017
【図７Ａ】
0005831356.tif 000018
【図７Ｂ】
0005831356.tif 000019
【図８Ａ】
0005831356.tif 000020
【図８Ｂ】
0005831356.tif 000021
【図９Ａ】
0005831356.tif 000022
【図９Ｂ】
0005831356.tif 000023