JP7498150B2 - 移動体及び移動体の経路決定方法 - Google Patents

Description

本発明は、移動体及び移動体の経路決定方法に関する。

従来、移動体の経路決定に関する技術文献として、特開２０１２－２４３０２９号公報が知られている。この公報には、現在位置から最終目標地に至るための大域経路を生成する移動体において、前方に障害物を検出した場合に障害物を回避して大域経路に復帰する局地経路の候補を複数生成することが示されている。移動体は、複数生成した局地経路の候補のそれぞれについて、最終目標地となす角度、移動可能速度、障害物までの距離から評価値を計算し、評価値が最大となる候補を障害物回避のための局地経路として採用する。また、この公報には経路候補の生成にＤＷＡ［Dynamic Window Approach］を用いることが示されている。

特開２０１２－２４３０２９号公報

Fox, D.Burgard, W. Thrun, S. "The dynamic windows approach to collision avoidance.IEEE Robotics and automation magazine 4(1) (1997)"

しかしながら、上述した従来の移動体では、比較的単純な経路生成しか行うことができず、隘路進入などの場合にスムースに移動できる経路を得ることができないと言う問題があった。

本発明の一態様は、現在位置から目標位置に向かって走行する移動体であって、移動体の周囲に物体が存在しないと仮定して予め設定された第一設定時間内に移動体が現在位置から走行可能な第一区間候補を複数探索する第一区間候補探索部と、移動体の周囲に物体が存在しないと仮定して予め設定された第二設定時間内に移動体が第一区間候補の終点から走行可能な第二区間候補を第一区間候補の終点ごとに複数探索する第二区間候補探索部と、目標位置の存在する方向である目標方向を認識する目標方向認識部と、移動体の周囲の物体であって移動体の移動の障害となる障害物の位置を検出する障害物位置認識部と、目標方向及び障害物の位置に基づいて、複数の第一区間候補及び複数の第二区間候補の中から移動体が走行する経路として用いられる第一区間及び第二区間を決定する経路決定部と、を備え、第一区間候補探索部は、予め設定された移動体の並進速度及び移動体の回転速度の複数の組み合わせに基づいて、並進速度及び／又は回転速度の条件が異なる第一区間候補を複数探索し、第二区間候補探索部は、予め設定された移動体の並進速度及び移動体の回転速度の複数の組み合わせに基づいて、並進速度及び／又は回転速度の条件が異なる第二区間候補を複数探索し、経路決定部は、決定した第一区間の終点を起点とする複数の第二区間候補における移動体の並進速度である第二並進速度と第一区間における移動体の並進速度である第一並進速度との差分が第二並進速度閾値未満となるように第二区間を決定すると共に、決定した第一区間の終点を起点とする複数の第二区間候補における移動体の回転速度である第二回転速度と第一区間における移動体の回転速度である第一回転速度との差分が第二回転速度閾値未満となるように第二区間を決定する。

本発明の一態様に係る移動体によれば、第一区間候補と第二区間候補を少なくとも含む複数段階の経路探索を行い、移動体が走行する経路として用いられる第一区間及び第二区間を決定することで、一段階の経路探索しか行わない従来の技術と比べて、柔軟性が高くスムースに移動体が走行できる経路を得ることができる。

本発明の一態様に係る移動体において、現在位置における移動体の並進速度を認識する走行状態認識部を更に備え、経路決定部は、現在位置における移動体の並進速度と複数の第一区間候補における移動体の並進速度である第一並進速度との差分が第一並進速度閾値未満となるように第一区間を決定してもよい。
この移動体によれば、現在位置における移動体の並進速度と複数の第一区間候補における移動体の並進速度である第一並進速度との差分が第一並進速度閾値未満となるように第一区間を決定するので、並進速度の変化を考慮しない場合と比べて、移動体がスムースに移動できる第一区間を選択することができる。

本発明の一態様に係る移動体において、現在位置における移動体の回転速度を認識する走行状態認識部を更に備え、経路決定部は、現在位置における移動体の回転速度と複数の第一区間候補における移動体の回転速度である第一回転速度との差分が第一回転速度変化閾値未満となるように第一区間を決定してもよい。
この移動体によれば、現在位置における移動体の回転速度と複数の第一区間候補における移動体の回転速度である第一回転速度との差分が第一回転速度変化閾値未満となるように第一区間を決定するので、回転速度の変化を考慮しない場合と比べて、移動体がスムースに移動できる第一区間を選択することができる。

本発明の一態様に係る移動体において、経路決定部は、目標方向及び障害物の位置に基づいて第一区間候補の障害物最接近距離及び第一区間候補の目標方向の有効距離を算出し、第一区間候補の障害物最接近距離が大きいほど第一区間候補の評価値を大きな値として算出すると共に、第一区間候補の有効距離が大きいほど第一区間候補の評価値を大きな値として算出し、目標方向及び障害物の位置に基づいて第二区間候補の障害物最接近距離及び第二区間候補の目標方向の有効距離を算出し、第二区間候補の障害物最接近距離が大きいほど第二区間候補の評価値を大きな値として算出すると共に、第二区間候補の有効距離が大きいほど第二区間候補の評価値を大きな値として算出し、複数の第一区間候補の中から最も評価値の高い第一区間候補を第一区間として決定し、第一区間の第一回転速度と第二回転速度との差分が第二回転速度閾値未満となり、第一区間の第一並進速度と第二並進速度との差分が第二並進速度閾値未満となる複数の第二区間候補の中から、最も評価値の高い第二区間候補を第二区間として決定し、障害物最接近距離とは、第一区間候補又は第二区間候補において障害物と最も接近した箇所と障害物との距離であり、有効距離とは、第一区間候補又は第二区間候補を目標方向に射影した場合の長さであってもよい。

本発明の他の態様は、現在位置から目標位置に向かって走行する移動体の経路決定方法であって、移動体の周囲に物体が存在しないと仮定して予め設定された第一設定時間内に移動体が現在位置から走行可能な第一区間候補を複数探索する第一区間候補探索ステップと、移動体の周囲に物体が存在しないと仮定して予め設定された第二設定時間内に移動体が第一区間候補の終点から走行可能な第二区間候補を第一区間候補の終点ごとに複数探索する第二区間候補探索ステップと、目標位置の存在する方向である目標方向を認識する目標方向認識ステップと、移動体の周囲の物体であって移動体の移動の障害となる障害物の位置を検出する障害物位置認識ステップと、目標方向及び障害物の位置に基づいて、複数の第一区間候補及び複数の第二区間候補の中から移動体が走行する経路として用いられる第一区間及び第二区間を決定する経路決定ステップと、を含み、第一区間候補探索ステップにおいて、予め設定された移動体の並進速度及び移動体の回転速度の複数の組み合わせに基づいて、並進速度及び／又は回転速度の条件が異なる第一区間候補を複数探索し、第二区間候補探索ステップにおいて、予め設定された移動体の並進速度及び移動体の回転速度の複数の組み合わせに基づいて、並進速度及び／又は回転速度の条件が異なる第二区間候補を複数探索し、経路決定ステップにおいて、決定した第一区間の終点を起点とする複数の第二区間候補における移動体の並進速度である第二並進速度と第一区間における移動体の並進速度である第一並進速度との差分が第二並進速度閾値未満となるように第二区間を決定すると共に、決定した第一区間の終点を起点とする複数の第二区間候補における移動体の回転速度である第二回転速度と第一区間における移動体の回転速度である第一回転速度との差分が第二回転速度閾値未満となるように第二区間を決定する。

本発明の他の態様に係る移動体の経路決定方法によれば、第一区間候補と第二区間候補を少なくとも含む複数段階の経路探索を行い、経路として用いられる第一区間及び第二区間を決定することで、一段階の経路探索しか行わない従来の技術と比べて、柔軟性が高くスムースに移動体が走行できる経路を得ることができる。

本発明の一態様及び他の態様によれば、一段階の経路探索しか行わない従来の技術と比べて、柔軟性が高くスムースに移動体が走行できる経路を得ることができる。

一実施形態に係る移動体を示すブロック図である。 （ａ）複数の第一区間候補の探索結果の一例を示す図である。（ｂ）複数の第一区間候補から延びる第二区間候補（１候補のみ）の探索結果の一例を示す図である。（ｃ）複数の第一区間候補から延びる複数の第二区間候補の探索結果の一例を示す図である。 目標方向の有効距離を説明するための図である。 （ａ）経路決定の一例を示す図である。（ｂ）経路決定の他の例を示す図である。 移動体の経路決定処理の一例を示すフローチャートである。 （ａ）第一並進速度評価処理の一例を示すフローチャートである。（ｂ）第一回転速度評価処理の一例を示すフローチャートである。 （ａ）第二並進速度評価処理の一例を示すフローチャートである。（ｂ）第二回転速度評価処理の一例を示すフローチャートである。 （ａ）従来の技術により探索された経路の一例を示す図である。（ｂ）本実施形態に係る移動体による経路の一例を示す図である。

以下、本発明の実施形態について図面を参照して説明する。

図１は、一実施形態に係る移動体を示すブロック図である。図１に示す移動体１は、自動車、パーソナルモビリティ、ロボットなど現在位置から目標位置に向かって地上を走行する移動物体である。移動体１は、自律走行する車両（自動運転車両）、自律走行するパーソナルモビリティ、又は自律走行ロボットであってもよい。

［移動体の構成］
本実施形態に係る移動体１の構成について図面を参照して説明する。図１に示すように、移動体１は、移動体１を制御する制御装置１０を備えている。

制御装置１０は、ＣＰＵ[Central Processing Unit]及び記憶部を有する電子制御ユニットである。制御装置１０は、一般的なコンピュータとして構成されてもよい。記憶部は、ＲＯＭ[Read Only Memory]及び/又はＲＡＭ[Random Access Memory]であってもよく、ＲＡＩＤ（RedundantArrays of Inexpensive Disks）構成とされたＨＤＤ［Hard Disk Drive］やＳＳＤ［Solid State Drive］などの記憶媒体であってもよい。制御装置１０は、外部と通信するためのネットワークカードなどの通信デバイスを備えていてもよい。制御装置１０は、ユーザと情報をやり取りするための入出力デバイスを備えていてもよい。

制御装置１０には、ＧＮＳＳ［Global Navigation Satellite System］受信部２、レーダセンサ３、カメラ４、速度センサ５、加速度センサ６、回転速度センサ７、及び移動体駆動部８が接続されている。

ＧＮＳＳ受信部２は、移動体１に設けられ、四個以上のＧＮＳＳ衛星から信号を受信することにより移動体１の位置（例えば移動体１の緯度及び経度）を測定する。ＧＮＳＳ受信部２は、測定した移動体１の位置情報を制御装置１０へ送信する。また、移動体１は、必ずしもＧＮＳＳ受信部２を有する必要はない。

レーダセンサ３は、移動体１に設けられ、電磁波を用いて移動体１の周囲の障害物を検出する検出機器である。レーダセンサ３は、ミリ波レーダであってもよく、ＬｉＤＡＲ［Light Detection and Ranging］であってもよい。レーダセンサ３は、移動体１の周囲の障害物の検出結果を制御装置１０に送信する。

カメラ４は、移動体１に設けられ、移動体１の周囲を撮像する撮像機器である。カメラ４は、移動体１の周囲の撮像画像を制御装置１０に送信する。カメラは、単眼カメラであってもよく、距離情報を取得できるステレオカメラやＴＯＦ［Time of Flight］カメラであってもよい。なお、移動体１は、レーダセンサ３とカメラ４のうち少なくとも一方を備えていればよい。

速度センサ５は、移動体１の並進速度を検出する検出機器である。移動体１の並進速度とは、移動体１の位置変化の速度である。速度センサ５は、例えば移動体１の車輪速から並進速度を検出する。なお、移動体１は車輪移動に限られない。移動体１は、例えば二足歩行や四足歩行であってもよい。速度センサ５は、移動体１の並進速度の検出結果を制御装置１０に送信する。

加速度センサ６は、移動体１の加速度を検出する検出機器である。加速度センサ６は、移動体１の前後方向の加速度と横方向の加速度を区別して検出してもよい。加速度センサ６は、移動体１の加速度の検出結果を制御装置１０に送信する。なお、移動体１は必ずしも加速度センサ６を備える必要はない。

回転速度センサ７は、移動体１の回転速度を検出する検出機器である。回転速度センサ７は、移動体１の回転速度の検出結果を制御装置１０に送信する。なお、移動体１の回転速度に代えてヨーレート（回転角速度）を送信するヨーレートセンサを用いてもよい。

移動体駆動部８は、移動体１を走行させるための駆動機器である。移動体駆動部８は、例えば移動体１の車輪を駆動させる各種のアクチュエータを含んで構成される。移動体駆動部８の構成は特に限定されず、移動体１の種類に応じた構成が採用される。

移動体駆動部８は、移動体１が車両である場合、エンジンアクチュエータ、ブレーキアクチュエータ、及び操舵アクチュエータを含んでもよい。エンジンアクチュエータは、制御装置１０からの制御信号に応じて車両のエンジンに対する空気の供給量（スロットル開度）を制御し、車両の駆動力を制御する。なお、車両がハイブリッド車である場合には、エンジンに対する空気の供給量の他に、動力源としてのモータに制御装置１０からの制御信号が入力されて当該駆動力が制御される。車両が電気自動車である場合には、動力源としてのモータに制御装置１０からの制御信号が入力されて当該駆動力が制御される。

ブレーキアクチュエータは、制御装置１０からの制御信号に応じてブレーキシステムを制御し、車両の車輪へ付与する制動力を制御する。ブレーキシステムとしては、例えば、液圧ブレーキシステムを用いることができる。操舵アクチュエータは、制御装置１０からの制御信号に応じて車両の電動パワーステアリングシステムのうち操舵トルクを制御するアシストモータの駆動を制御する。これにより、操舵アクチュエータは車両の操舵トルクを制御する。

次に、制御装置１０の機能的構成について説明する。図１に示すように、制御装置１０は、走行状態認識部１１、目標方向認識部１２、第一区間候補探索部１３、第二区間候補探索部１４、障害物位置認識部１５、経路決定部１６、及び移動体制御部１７を有している。なお、以下に説明する制御装置１０の機能の一部は、移動体１と通信可能なサーバにおいて演算される態様であってもよい。

走行状態認識部１１は、移動体１の走行状態を認識する。走行状態には、移動体１の並進速度と回転速度が含まれる。走行状態には、移動体１の加速度が含まれてもよく、ヨーレートが含まれてもよい。走行状態認識部１１は、例えば速度センサ５、加速度センサ６、及び回転速度センサ７の検出結果に基づいて移動体１の走行状態を認識する。

目標方向認識部１２は、移動体１の現在位置と予め設定された目標位置とに基づいて、移動体１の目標方向を認識する。目標位置とは、移動体１の到達目標として予め設定された位置である。目標位置は、例えば移動体１の現在位置を含む平面座標に目標座標値を入力することで設定される。目標位置は、移動体１のユーザ（乗員や管理者など）が設定してもよく、移動体１が予め決められた条件に沿って自律的に設定してもよい。目標位置の設定方法は特に限定されない。

目標方向とは、移動体１の現在位置を基準としたときの目標位置の存在する方向である。目標方向は、現在位置と目標位置とを結ぶ直線の延在方向に相当する。目標方向認識部１２は、一例として、移動体１の現在位置を基準とする平面座標上における目標位置（目標座標値）から目標方向を認識する。

なお、目標方向認識部１２は、地図情報を用いて目標方向を認識してもよい。目標方向認識部１２は、ＧＮＳＳ受信部２による移動体１の位置情報から地図上における移動体１の現在位置を認識する。目標方向認識部１２は、デッドレコニングやＳＬＡＭ［Simultaneous Localization and Mapping］の技術を用いて地図上における移動体１の現在位置を認識してもよい。目標方向認識部１２は、地図上における移動体１の現在位置と地図上に設定された目標位置とに基づいて、目標方向を認識することができる。

その他、目標方向認識部１２は、目標位置に置かれた磁気マーカーと移動体１に備えられた磁気マーカー受信機とによって、目標方向を認識してもよい。

第一区間候補探索部１３は、移動体１の現在位置を起点として、第一区間の候補である第一区間候補を複数探索する。第一区間とは、移動体１の走行する経路を構成する区間である。第一区間候補は、第一区間の候補である。第一区間候補は、予め設定された第一設定時間内に移動体１が現在位置から走行可能な区間として探索される。第一設定時間は、１秒であってもよく、３秒であってもよく、５秒であってもよく、１０秒であってもよく、１５秒であってもよく、１分であってもよい。第一設定時間は、移動体１の制御装置１０における制御周期に対応する時間であってもよい。第一設定時間は、任意の時間を設定することができる。

第一区間候補探索部１３は、ＤＷＡ［Dynamic Window Approach］として、第一設定時間内に移動体１が現在位置から走行可能な第一区間候補を探索する。ＤＷＡについては、“Fox, D. Burgard, W. Thrun, S.:The dynamicwindows approach to collision avoidance. IEEE Robotics and automation magazine4(1) (1997)”に記載がある。評価関数などについては後述する。

第一区間候補探索部１３は、例えば、予め設定された第一速度条件に基づいて、第一区間候補の探索を行う。第一速度条件は、一例として、ユーザが予め設定した並進速度と回転速度の組み合わせである。第一速度条件は、例えば、曲率が大きいため回転速度が一定値以上になると並進速度が低くなるように設定されてもよい。第一速度条件は、移動体１の走行性能に応じて設定することができる。第一区間候補探索部１３は、並進速度及び／又は回転速度が異なっていれば、重なるように複数の第一区間候補を探索してもよい。

第一区間候補探索部１３は、移動体１が周囲に物体の存在しない平面を走行すると仮定して、第一区間候補を複数探索する。また、第一区間候補探索部１３は、例えば角度条件（例えばsinやcosによる条件）を付与することで、複数の第一区間候補が適切に離れるように探索を行ってもよい。

図２（ａ）は、複数の第一区間候補の探索結果の一例を示す図である。図２（ａ）に、第一区間候補ＰＡ、第一区間候補ＰＡの起点Ｓａ（移動体１の現在位置）、第一区間候補ＰＡの終点（到達点）Ｅａを示す。図２（ａ）ではＸＹ平面座標系を用い、起点Ｓａが（Ｘ，Ｙ）：（０，０）に対応する。図２（ａ）に示すように、第一区間候補探索部１３は、移動体１の現在位置を起点Ｓａとして移動体１の前方に広がる複数の第一区間候補ＰＡを探索する。

なお、図２（ａ）では移動体１を前方にのみ移動可能として複数の第一区間候補ＰＡを示しているが、移動体１は前方以外の方向に移動可能であってもよい。例えば移動体１が後方に移動可能である場合には、起点Ｓａを中心としてＸ軸方向に反転した複数の第一区間候補ＰＡ（Ｙ軸のマイナス方向に広がる複数の第一区間候補ＰＡ）を追加で探索することができる。移動体１が３６０°任意の方向に移動可能である場合には、起点Ｓａを中心とした３６０°の方向で図２（ａ）と同様に広がる複数の第一区間候補ＰＡを追加で探索してもよい。第一区間候補ＰＡの探索数には上限が設定されていてもよい。

第二区間候補探索部１４は、複数の第一区間候補ＰＡの終点Ｅａをそれぞれ起点として、第二区間の候補である第二区間候補を複数探索する。第二区間とは、第一区間に続いて移動体１の走行する経路を構成する区間である。第二区間候補は、予め設定された第二設定時間内に移動体１が第一区間候補ＰＡの終点Ｅａから走行可能な区間として探索される。

第二設定時間は、１秒であってもよく、３秒であってもよく、５秒であってもよく、１０秒であってもよく、１５秒であってもよく、１分であってもよい。第二設定時間は、移動体１の制御装置１０における制御周期に対応する時間であってもよい。第二設定時間は、第一設定時間と同じ時間であってもよく、異なる時間であってもよい。第二設定時間は、第一設定時間より長い時間であってもよい。第二設定時間は、任意の時間を設定することができる。

第二区間候補探索部１４は、ＤＷＡとして第二設定時間内に移動体１が第一区間候補ＰＡの終点Ｅａから走行可能な第二区間候補を探索する。第二区間候補探索部１４は、例えば、予め設定された第二速度条件に基づいて第二区間候補の探索を行う。第二速度条件は、一例として、ユーザが予め設定した並進速度と回転速度の組み合わせである。第二速度条件は、第一速度条件と同じであってもよく、異なっていてもよい。第二区間候補探索部１４は、並進速度及び／又は回転速度が異なっていれば、重なるように複数の第二区間候補を探索してもよい。

第二区間候補探索部１４も、移動体１が周囲に物体の存在しない平面を走行すると仮定して、第二区間候補を複数探索する。また、第二区間候補探索部１４も、例えば角度条件（例えばsinやcosによる条件）を付与することで、複数の第二区間候補が適切に離れるように探索を行ってもよい。

図２（ｂ）は、第一区間候補（１候補のみ）から延びる複数の第二区間候補の探索結果の一例を示す図である。ここでは、説明を分かりやすくするため、第一区間候補を１候補のみ示している。図２（ｂ）に、第二区間候補ＰＢ、第二区間候補ＰＢの起点Ｓｂ（第一区間候補ＰＡの終点Ｅａと同じ）、第二区間候補ＰＢの終点（到達点）Ｅｂを示す。

図２（ｂ）では、図２（ａ）に示す第一区間候補ＰＡを延長するように複数の第二区間候補ＰＢが探索された場合を示している。第二区間候補探索部１４は、第二区間候補ＰＢの起点Ｓｂにおける移動体１の向きの変更を許容して第二区間候補ＰＢの探索を行ってもよい。

図２（ｃ）は、複数の第一区間候補から延びる複数の第二区間候補の探索結果の一例を示す図である。図２（ｃ）では図を見やすくするため符号を省略する。第二区間候補探索部１４は、例えば図２（ｃ）に示すように、移動体１が第一区間候補ＰＡの終点Ｅａから走行可能な第二区間候補ＰＢを複数探索する。

なお、移動体１が後方に移動可能である場合には、起点Ｓａを中心としてＸ軸方向に反転した複数の第一区間候補ＰＡ及び第二区間候補ＰＢを追加で探索することができる。同様に、移動体１が３６０°任意の方向に移動可能である場合には、起点Ｓａを中心とした３６０°の方向で図２（ｃ）と同様に広がる複数の第一区間候補ＰＡ及び第二区間候補ＰＢを追加で探索してもよい。第二区間候補ＰＢの探索数には上限が設定されていてもよい。

障害物位置認識部１５は、レーダセンサ３の障害物の検出結果及び／又はカメラ４の撮像画像に基づいて、移動体１の周囲の障害物の位置を認識する。障害物とは、移動体１の移動の障害となるものである。障害物には、壁などの構造物、ポールなどの設置物、歩行者や他の移動体などが含まれる。障害物位置認識部１５は、外部サーバ（例えば移動体１が走行中の施設のサーバや交通情報サーバ）からの通信により障害物の位置を認識してもよい。

経路決定部１６は、目標方向認識部１２の認識した目標方向と障害物位置認識部１５の認識した障害物の位置に基づいて、第一区間候補ＰＡ及び第二区間候補ＰＢの中から移動体１が走行する経路として用いる第一区間及び第二区間を決定する。

具体的に、経路決定部１６は、目標方向と障害物の位置に基づいて、第一区間候補ＰＡ及び第二区間候補ＰＢの評価を行う。経路決定部１６は、例えば下記の式（１）として示す評価関数Ｇａを用いて第一区間候補ＰＡ及び第二区間候補ＰＢの評価値を算出する。

上記の式（１）において、Ｇは評価関数、Ｃは障害物最接近距離、Ｌは目標方向の有効距離である。障害物最接近距離Ｃは、評価対象の第一区間候補ＰＡ及び第二区間候補ＰＢにおいて障害物と最も接近した箇所と障害物との距離である。障害物最接近距離Ｃは、評価対象の第一区間候補ＰＡ及び第二区間候補ＰＢが障害物と接触する場合には－∞の値を取る。

Ｌは目標方向における有効距離（例えば第一区間候補ＰＡを目標方向に射影した場合の長さ）である。ここで、図３は、目標方向の有効距離を説明するための図である。図３に、第一区間候補ＰＡ、目標方向Ｔを示す。図３に示すように、目標方向Ｔの有効距離とは、目標方向Ｔにおける第一区間候補ＰＡの進行距離に相当する。評価対象である第一区間候補ＰＡが目標方向Ｔに向かって長い距離を進むほど高い評価となる。一方で、第一区間候補ＰＡが目標方向Ｔに向かって進む距離が短いと低い評価となる。第一区間候補ＰＡが目標方向Ｔと反対側に進む場合には、マイナスの評価となる。ａ及びｂは調整パラメータである。ａ及びｂの値はユーザが任意に設定する。

図４（ａ）は、経路決定の一例を示す図である。図４（ａ）に、移動体１の現在位置Ｓａ、目標位置Ｇ１、目標方向Ｔ１、壁（障害物）Ｗ１～Ｗ３、及び経路候補Ｃ１～Ｃ３を示す。経路候補Ｃ１～Ｃ３は、第一区間候補ＰＡ及び第二区間候補ＰＢの組み合わせにより形成されている。

図４（ａ）に示す状況において、経路決定部１６は、壁Ｗ１～Ｗ３から適切に離間しており、目標方向Ｔ１における有効距離が最も長い経路候補Ｃ３の評価値を最も高く算出する。経路決定部１６は、経路候補Ｃ１，Ｃ２と比べて、壁Ｗ１に多少近づいても目標方向Ｔ１における有効距離が長く、目標位置Ｇ１に近づいている経路候補Ｃ３を高く評価する。

図４（ｂ）は、経路決定の他の例を示す図である。図４（ｂ）に、移動体１の現在位置Ｓａ、目標位置Ｇ２、目標方向Ｔ２、壁（障害物）Ｗ４～Ｗ６、及び経路候補Ｃ４～Ｃ６を示す。図４（ｂ）に示す状況において、経路決定部１６は、壁Ｗ４～Ｗ６から適切に離間しており、目標方向Ｔ２における有効距離が最も長い経路候補Ｃ６の評価値を最も高く算出する。経路決定部１６は、壁Ｗ４に接近し過ぎであり、目標方向Ｔ２における有効距離も短い経路候補Ｃ４，Ｃ５と比べて、目標位置Ｇ２に近づいている経路候補Ｃ６を高く評価する。

経路決定部１６は、並進速度や回転速度の変化を考慮して評価を行ってもよい。具体的に、経路決定部１６は、例えば下記の式（２）として示す評価関数Ｇｂを用いて第一区間候補ＰＡの評価値を算出してもよい。

ｃは調整パラメータである。ｃの値はユーザが任意に設定する。Ａａは第一区間候補用の速度変化判定値である。それ以外の要素は式（１）と同じであるため説明を省略する。速度変化判定値Ａａは、移動体１の並進速度や回転速度の変化が一定閾値以上である場合に評価値を下げるために用いられる。

具体的に、速度変化判定値Ａａは、現在位置における移動体１の並進速度ｖ_ｃと第一区間候補ＰＡにおける移動体１の並進速度である第一並進速度ｖ_１との差分（絶対値）が第一並進速度閾値ｖ_ｍａｘ１以上である場合、－∞の値を取る。

また、速度変化判定値Ａａは、現在位置における移動体１の回転速度ｗ_ｃと第一区間候補ＰＡにおける移動体１の回転速度である第一回転速度ｗ_１との差分（絶対値）が第一回転速度閾値ｗ_ｍａｘ１以上である場合、－∞の値を取る。第一並進速度閾値ｖ_ｍａｘ１及び第一回転速度閾値ｗ_ｍａｘ１は、予め設定された値の閾値である。第一並進速度閾値ｖ_ｍａｘ１は、移動体１の最大並進加速度に制御装置１０の制御周期を乗じた値としてもよい。同様に、第一回転速度閾値ｗ_ｍａｘ１は、移動体１の最大回転加速度に制御装置１０の制御周期を乗じた値としてもよい。

なお、回転速度は回転方向によって区別され、例えば右方向の回転速度を正の値、左方向の回転速度を負の値として表現される。同方向の回転速度の差分を取るときは、値の大きい方から小さい方を差し引いた差分の絶対値を第一回転速度閾値ｗ_ｍａｘ１と比較する。逆方向の回転速度の差分を取るときには、何れも正の値として合計し、その合計値を差分として第一回転速度閾値ｗ_ｍａｘ１と比較する。

一方で、速度変化判定値Ａａは、現在位置における移動体１の並進速度ｖ_ｃと第一並進速度ｖ_１との差分が第一並進速度閾値ｖ_ｍａｘ１未満であり、且つ、現在位置における移動体１の回転速度ｗ_ｃと第一回転速度ｗ_１との差分が第一回転速度閾値ｗ_ｍａｘ１未満である場合、ゼロの値を取る。

経路決定部１６は、上述した速度変化判定値Ａａを含む評価関数Ｇｂを用いることにより、移動体１の並進速度や回転速度の変化が急でスムースに走行できない第一区間候補ＰＡを移動体１が走行する経路から除外することができる。

なお、速度変化判定値Ａａは、並進速度と回転速度のうち一方のみを考慮する態様であってもよい。速度変化判定値Ａａは、現在位置における移動体１の並進速度ｖ_ｃと第一並進速度ｖ_１との差分が第一並進速度閾値ｖ_ｍａｘ１以上である場合にのみ－∞の値を取ってもよく、現在位置における移動体１の回転速度ｗ_ｃと第一回転速度ｗ_１との差分が第一回転速度閾値ｗ_ｍａｘ１以上である場合にのみ－∞の値を取ってもよい。

また、経路決定部１６は、例えば下記の式（３）として示す評価関数Ｇｃを用いて第二区間候補ＰＢの評価値を算出してもよい。

ｃは調整パラメータである。Ａｂは第二区間候補用の速度変化判定値である。それ以外の要素は式（１）と同じであるため説明を省略する。速度変化判定値Ａｂは、第一区間候補ＰＡにおける移動体１の並進速度である第一並進速度ｖ_１と第二区間候補ＰＢにおける移動体１の並進速度である第二並進速度ｖ_２との差分（絶対値）が第二並進速度閾値ｖ_ｍａｘ２以上である場合、－∞の値を取る。

また、速度変化判定値Ａｂは、第一区間候補ＰＡにおける移動体１の回転速度である第一回転速度ｗ_１と第二区間候補ＰＢにおける移動体１の回転速度である第二回転速度ｗ_２との差分（絶対値）が第二回転速度閾値ｗ_ｍａｘ２以上である場合、－∞の値を取る。第二並進速度閾値ｖ_ｍａｘ２及び第二回転速度閾値ｗ_ｍａｘ２は、予め設定された値の閾値である。第二並進速度閾値ｖ_ｍａｘ２は、第一並進速度閾値ｖ_ｍａｘ１と同じ値であってもよく、異なる値であってもよい。第二回転速度閾値ｗ_ｍａｘ２も、第一回転速度閾値ｖ_ｍａｘ２と同じ値であってもよく、異なる値であってもよい。

一方で、速度変化判定値Ａｂは、第一並進速度ｖ_１と第二並進速度ｖ_２との差分が第二並進速度閾値ｖ_ｍａｘ２未満であり、且つ、第一回転速度ｗ_１と第二回転速度ｗ_２との差分が第二回転速度閾値ｗ_ｍａｘ２未満である場合、ゼロの値を取る。

経路決定部１６は、上述した速度変化判定値Ａｂを含む評価関数Ｇｃを用いることにより、移動体１の並進速度や回転速度の変化が急でスムースに走行できない第二区間候補ＰＢを移動体１が走行する経路から除外することができる。

なお、速度変化判定値Ａｂは、並進速度と回転速度のうち一方のみを考慮する態様であってもよい。速度変化判定値Ａｂは、第一並進速度ｖ_１と第二並進速度ｖ_２との差分が第二並進速度閾値ｖ_ｍａｘ２以上である場合にのみ－∞の値を取ってもよく、第一回転速度ｗ_１と第二回転速度ｗ_２との差分が第二回転速度閾値ｗ_ｍａｘ２以上である場合にのみ－∞の値を取ってもよい。

経路決定部１６は、複数の第一区間候補ＰＡ及び複数の第二区間候補ＰＢの中から、最も評価値の高い組み合わせを第一区間及び第二区間として決定する。第一区間及び第二区間は、移動体１の走行する経路として用いられる区間である。

なお、経路決定部１６は、第一区間及び第二区間を段階的に決定してもよい。経路決定部１６は、複数の第一区間候補ＰＡ及び複数の第二区間候補ＰＢの組み合わせのうち評価値（合計値）が最も高いものではなく、複数の第一区間候補ＰＡのうち最も評価値の高いものを第一区間として決定し、当該第一区間の終点を起点とする複数の第二区間候補ＰＢのうち最も評価値の高いものを第二区間として決定してもよい。

経路決定部１６は、現在位置から目標位置に至る経路を求める必要はなく、目標位置に向かう経路（目標位置に至る経路の一部）を求められればよい。経路決定部１６は、一定周期で経路としての第一区間及び第二区間の決定を繰り返す。経路決定部１６は、移動体１が一度決定した第一区間及び第二区間を走行している途中に新たな第一区間及び第二区間の決定（経路の更新）を行ってもよい。

移動体制御部１７は、移動体１の走行を制御する。移動体制御部１７は、経路決定部１６の決定した経路から移動体１が走行するための駆動量（例えば車輪駆動量）を演算する。移動体制御部１７は、演算した駆動量に応じた制御信号を移動体駆動部８に送信することで、経路に沿った移動体１の走行制御を行う。

移動体制御部１７は、例えば左右輪が対向軸上に配置される対向二輪型の移動機構を移動体１が有する場合、左右輪の回転数を演算する。具体的に、移動体制御部１７は、第一区間の並進速度ｖ及び回転速度ｗを用いて下記の式（４）、（５）を用いて左車輪の回転数ｒｌ，右車輪の回転数ｒｒを求めてもよい。

上記の式（４），（５）におけるＲは移動体１の車輪半径、Ｈは移動体１のトレッド（車輪の中心間距離）である。移動体制御部１７は、上記の式（４），（５）によって算出した左車輪の回転数ｒｌ及び右車輪の回転数ｒｒを制御信号として移動体駆動部８に送信することで、経路に沿った移動体１の走行制御を行う。

［移動体の経路決定方法］
続いて、本実施形態に係る移動体１の経路決定方法について図面を参照して説明する。図５は、移動体１の経路決定処理の一例を示すフローチャートである。

図５に示すように、移動体１の制御装置１０は、Ｓ１として、目標方向認識部１２により移動体１の目標方向を認識する（目標方向認識ステップ）。目標方向認識部１２は、例えば移動体１の現在位置と予め設定された目標位置とに基づいて移動体１の目標方向を認識する。

Ｓ２において、制御装置１０は、第一区間候補探索部１３により第一区間候補ＰＡを複数探索する（第一区間候補探索ステップ）。第一区間候補探索部１３は、移動体１の現在位置を起点として、予め設定された第一設定時間内に移動体１が現在位置から走行可能な第一区間候補ＰＡを複数探索する。

Ｓ３において、制御装置１０は、第二区間候補探索部１４により第二区間候補ＰＢを複数探索する（第二区間候補探索ステップ）。第二区間候補探索部１４は、複数の第一区間候補ＰＡの終点Ｅａを起点として、予め設定された第二設定時間内に移動体１が第一区間候補ＰＡの終点Ｅａから走行可能な第二区間候補ＰＢを複数探索する。

Ｓ４において、制御装置１０は、障害物位置認識部１５により移動体１の周囲の障害物の位置を認識する（障害物位置認識ステップ）。障害物位置認識部１５は、例えばレーダセンサ３の障害物の検出結果及び／又はカメラ４の撮像画像に基づいて、移動体１の周囲の障害物の位置を認識する。

Ｓ５において、制御装置１０は、経路決定部１６により評価値の算出を行う。経路決定部１６は、目標方向と障害物の位置に基づいて、第一区間候補ＰＡ及び第二区間候補ＰＢの評価を行う（評価ステップ）。経路決定部１６は、例えば上述した式（１）として示す評価関数Ｇａを用いて第一区間候補ＰＡ及び第二区間候補ＰＢの評価値を算出する。

Ｓ６において、制御装置１０は、経路決定部１６により経路決定（第一区間及び第二区間の決定）を行う（経路決定ステップ）。経路決定部１６は、例えば複数の第一区間候補ＰＡ及び複数の第二区間候補ＰＢの中から、最も評価値の高い組み合わせを第一区間及び第二区間として決定する。

Ｓ７において、制御装置１０は、移動体制御部１７により移動体１の走行制御を行う（走行制御ステップ）。移動体制御部１７は、制御信号を移動体駆動部８に送信することで、経路決定部１６の決定した経路に沿って移動体１が走行するように制御する。

図６（ａ）は、第一並進速度評価処理の一例を示すフローチャートである。第一並進速度評価処理は、図５に示すフローチャートのＳ５において実行される。

図６（ａ）に示すように、制御装置１０は、Ｓ１０として、経路決定部１６により現在位置における移動体１の並進速度ｖ_ｃと第一区間候補ＰＡにおける移動体１の並進速度である第一並進速度ｖ_１との差分（絶対値）が第一並進速度閾値ｖ_ｍａｘ１以上であるか否かを判定する（第一並進速度判定ステップ）。

制御装置１０は、上記差分が第一並進速度閾値ｖ_ｍａｘ１以上であると判定した場合（Ｓ１０：ＹＥＳ）、Ｓ１１に移行する。制御装置１０は、上記差分が第一並進速度閾値ｖ_ｍａｘ１以上であると判定しなかった場合（Ｓ１０：ＮＯ）、第一並進速度評価処理を終了する。

Ｓ１１において、制御装置１０は、経路決定部１６により第一区間候補ＰＡ（上記Ｓ１０の判定がＹＥＳである第一区間候補ＰＡ）の評価値を－∞とする（第一並進速度評価ステップ）。経路決定部１６は、例えば上述した式（２）の速度変化判定値Ａａの値を－∞とすることで該当する第一区間候補ＰＡの評価値を－∞として第一区間として選ばれないようにする。

図６（ｂ）は、第一回転速度評価処理の一例を示すフローチャートである。第一回転速度評価処理は、図５に示すフローチャートのＳ５において実行される。

図６（ａ）に示すように、制御装置１０は、Ｓ２０として、経路決定部１６により現在位置における移動体１の回転速度ｗ_ｃと第一区間候補ＰＡにおける移動体１の回転速度である第一回転速度ｗ_１との差分（絶対値）が第一回転速度閾値ｗ_ｍａｘ１以上であるか否かを判定する（第一回転速度判定ステップ）。

制御装置１０は、上記差分が第一回転速度閾値ｗ_ｍａｘ１以上であると判定した場合（Ｓ２０：ＹＥＳ）、Ｓ２１に移行する。制御装置１０は、上記差分が第一回転速度閾値ｗ_ｍａｘ１以上であると判定しなかった場合（Ｓ２０：ＮＯ）、第一回転速度評価処理を終了する。

Ｓ２１において、制御装置１０は、経路決定部１６により第一区間候補ＰＡ（上記Ｓ２０の判定がＹＥＳである第一区間候補ＰＡ）の評価値を－∞とする（第一回転速度評価ステップ）。経路決定部１６は、例えば上述した式（２）の速度変化判定値Ａａの値を－∞とすることで該当する第一区間候補ＰＡの評価値を－∞として第一区間として選ばれないようにする。

図７（ａ）は、第二並進速度評価処理の一例を示すフローチャートである。第二並進速度評価処理は、図５に示すフローチャートのＳ５において実行される。

図７（ａ）に示すように、制御装置１０は、Ｓ３０として、経路決定部１６により第一区間候補ＰＡにおける移動体１の並進速度である第一並進速度ｖ_１と第二区間候補ＰＢにおける移動体１の並進速度である第二並進速度ｖ_２との差分（絶対値）が第二並進速度閾値ｖ_ｍａｘ２以上であるか否かを判定する（第二並進速度判定ステップ）。

制御装置１０は、上記差分が第二並進速度閾値ｖ_ｍａｘ２以上であると判定した場合（Ｓ３０：ＹＥＳ）、Ｓ３１に移行する。制御装置１０は、上記差分が第二並進速度閾値ｖ_ｍａｘ２以上であると判定しなかった場合（Ｓ３０：ＮＯ）、第二並進速度評価処理を終了する。

Ｓ３１において、制御装置１０は、経路決定部１６により第二区間候補ＰＢ（上記Ｓ３０の判定がＹＥＳである第二区間候補ＰＢ）の評価値を－∞とする（第二並進速度評価ステップ）。経路決定部１６は、例えば上述した式（３）の速度変化判定値Ａｂの値を－∞とすることで該当する第二区間候補ＰＢの評価値を－∞として第二区間として選ばれないようにする。

図７（ｂ）は、第二回転速度評価処理の一例を示すフローチャートである。第二回転速度評価処理は、図５に示すフローチャートのＳ５において実行される。

図７（ａ）に示すように、制御装置１０は、Ｓ４０として、経路決定部１６により第一区間候補ＰＡにおける移動体１の回転速度である第一回転速度ｗ_１と第二区間候補ＰＢにおける移動体１の回転速度である第二回転速度ｗ_２との差分（絶対値）が第二回転速度閾値ｗ_ｍａｘ２以上であるか否かを判定する（第二回転速度判定ステップ）。

制御装置１０は、上記差分が第二回転速度閾値ｗ_ｍａｘ２以上であると判定した場合（Ｓ４０：ＹＥＳ）、Ｓ４１に移行する。制御装置１０は、上記差分が第二回転速度閾値ｗ_ｍａｘ２以上であると判定しなかった場合（Ｓ４０：ＮＯ）、第二回転速度評価処理を終了する。

Ｓ４１において、制御装置１０は、経路決定部１６により第二区間候補ＰＢ（上記Ｓ４０の判定がＹＥＳである第二区間候補ＰＢ）の評価値を－∞とする（第二回転速度評価ステップ）。経路決定部１６は、例えば上述した式（３）の速度変化判定値Ａｂの値を－∞とすることで該当する第二区間候補ＰＢの評価値を－∞として第二区間として選ばれないようにする。

以上説明した本実施形態に係る移動体１（及び移動体１の経路決定方法）によれば、第一区間候補ＰＡと第二区間候補ＰＢを少なくとも含むＤＷＡによる多段階の経路探索を行い、目標方向及び障害物の位置に基づいて移動体１が走行する経路として用いられる第一区間及び第二区間を決定することで、一段階の経路探索しか行わない従来の技術と比べて、柔軟性が高くスムースに移動体１が走行できる経路を得ることができる。

ここで、図８（ａ）は、従来の移動体の経路決定方法による経路の一例を示す図である。図８（ａ）に、従来の移動体の現在位置Ｓａ、経路ＣＺ、目標位置Ｇ１０、目標方向Ｔ１０、隘路Ｎ、及び壁（障害物）Ｗ１０～Ｗ１３を示す。図８（ａ）に示すように、一段階の経路探索しか行わない従来の移動体では、壁Ｗ１０と壁Ｗ１１により形成された隘路Ｎに進入する経路を探索できない場合があった。

図８（ｂ）は、本実施形態に係る移動体１による経路の一例を示す図である。図８（ｂ）に、移動体１の経路Ｃ１０を示す。図８（ｂ）に示すように、移動体１によれば、ＤＷＡによる多段階の経路探索を行うことで、隘路Ｎにも対応可能な柔軟性を有しスムースに移動体１が走行できる経路を得ることができる。移動体１は、障害物すり抜けや障害物すれ違いもスムースに実現できる経路を得ることが可能である。

また、移動体１によれば、現在位置における移動体１の並進速度ｖ_ｃと第一区間候補ＰＡにおける移動体１の並進速度である第一並進速度ｖ_１との差分が第一並進速度閾値ｖ_ｍａｘ１未満となるように第一区間を決定することで、並進速度の変化を考慮しない場合と比べて、移動体１がスムースに移動できる第一区間を選択することができる。

同様に、移動体１によれば、現在位置における移動体１の回転速度ｗ_ｃと第一区間候補ＰＡにおける移動体１の回転速度である第一回転速度ｗ_１との差分が第一回転速度閾値ｗ_ｍａｘ１未満となるように第一区間を決定することで、回転速度の変化を考慮しない場合と比べて、移動体がスムースに移動できる第一区間を選択することができる。

また、移動体１によれば、第一区間候補ＰＡにおける移動体１の並進速度である第一並進速度ｖ_１と第二区間候補ＰＢにおける移動体１の並進速度である第二並進速度ｖ_２との差分が第二並進速度閾値ｖ_ｍａｘ２未満となるように第二区間を決定することで、第一区間及び第二区間の並進速度の変化を考慮しない場合と比べて、移動体１がスムースに移動できる第二区間を選択することができる。

同様に、移動体１によれば、第一区間候補ＰＡにおける移動体１の回転速度である第一回転速度ｗ_１と第二区間候補ＰＢにおける移動体１の回転速度である第二回転速度ｗ_２との差分が第二回転速度閾値ｗ_ｍａｘ２未満となるように第二区間を決定することで、第一区間及び第二区間の回転速度の変化を考慮しない場合と比べて、移動体がスムースに移動できる第二区間を選択することができる。

以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限定されるものではない。本発明は、上述した実施形態を始めとして、当業者の知識に基づいて種々の変更、改良を施した様々な形態で実施することができる。

移動体１は、第二区間候補ＰＢの終点Ｅｂを起点として移動体１が予め設定された第三設定時間内に走行可能な第三区間候補を複数探索する第三区間候補探索部を更に有していてもよい。この場合、経路決定部１６は、第二区間候補ＰＢと同様に複数の第三区間候補の評価値を算出して、移動体１の走行する経路としての第三区間を決定する。移動体１は、更に第四区間候補探索部、第五区間候補探索部を有していてもよい。

経路決定部１６は、移動体制御部１７が前回演算した移動体１の駆動量（例えば車輪駆動量）を用いて、第一区間候補ＰＡ及び第二区間候補ＰＢの評価値を算出してもよい。経路決定部１６は、例えば並進速度や回転速度と同様に、前回演算した移動体１の駆動量と第一区間候補ＰＡにおける並進速度の実現に必要な移動体１の駆動量の差が閾値未満となるように第一区間を決定する。

経路決定部１６は、並進速度や回転速度の変化を考慮した評価を行う場合に、必ずしも並進速度の変化が大きい第一区間候補ＰＡの評価値を－∞にする必要はない。経路決定部１６は、現在位置における移動体１の並進速度ｖ_ｃと第一並進速度ｖ_１との差分が第一並進速度閾値ｖ_ｍａｘ１以上の第一区間候補ＰＡを評価対象から除外するだけでもよい。回転速度についても同様である。また第二区間候補ＰＢについても同様である。

経路決定部１６は、第二区間候補ＰＢを評価するときの目標方向を第二区間候補ＰＢの起点Ｓｂ（第一区間候補ＰＡの終点Ｅａ）から見た目標位置の存在する方向としてもよい。経路決定部１６は、第一区間候補ＰＡの場合の目標方向（移動体１の現在位置から見た目標位置の存在する方向）をそのまま第二区間候補ＰＢの評価に用いるのではなく、第二区間候補ＰＢの起点Ｓｂから見た目標位置の存在する方向である目標方向を用いて目標方向の有効距離が長いほど第二区間候補ＰＢの評価値を高く算出する。これにより、経路決定部１６は、移動体の現在位置Ｓａと第二区間候補ＰＢの起点Ｓｂの位置ずれの影響を受けることなく、最終的に目標位置により近づく第二区間候補ＰＢの評価値を高く算出することができる。

１…移動体、２…ＧＮＳＳ受信部、３…レーダセンサ、４…カメラ、５…速度センサ、６…加速度センサ、７…回転速度センサ、８…移動体駆動部、１０…制御装置、１１…走行状態認識部、１２…目標方向認識部、１３…第一区間候補探索部、１４…第二区間候補探索部、１５…障害物位置認識部、１６…経路決定部、１７…移動体制御部。

Claims (5)

1. 現在位置から目標位置に向かって走行する移動体であって、
   前記移動体の周囲に物体が存在しないと仮定して予め設定された第一設定時間内に前記移動体が前記現在位置から走行可能な第一区間候補を複数探索する第一区間候補探索部と、
   前記移動体の周囲に物体が存在しないと仮定して予め設定された第二設定時間内に前記移動体が前記第一区間候補の終点から走行可能な第二区間候補を前記第一区間候補の終点ごとに複数探索する第二区間候補探索部と、
   前記目標位置の存在する方向である目標方向を認識する目標方向認識部と、
   前記移動体の周囲の物体であって前記移動体の移動の障害となる障害物の位置を検出する障害物位置認識部と、
   前記目標方向及び前記障害物の位置に基づいて、複数の前記第一区間候補及び複数の前記第二区間候補の中から前記移動体が走行する経路として用いられる第一区間及び第二区間を決定する経路決定部と、
   を備え、
   前記第一区間候補探索部は、予め設定された前記移動体の並進速度及び前記移動体の回転速度の複数の組み合わせに基づいて、前記並進速度及び／又は前記回転速度の条件が異なる前記第一区間候補を複数探索し、
   前記第二区間候補探索部は、予め設定された前記移動体の並進速度及び前記移動体の回転速度の複数の組み合わせに基づいて、前記並進速度及び／又は前記回転速度の条件が異なる前記第二区間候補を複数探索し、
   前記経路決定部は、決定した前記第一区間の終点を起点とする複数の前記第二区間候補における前記移動体の並進速度である第二並進速度と前記第一区間における前記移動体の並進速度である第一並進速度との差分が第二並進速度閾値未満となるように前記第二区間を決定すると共に、
   決定した前記第一区間の終点を起点とする複数の前記第二区間候補における前記移動体の回転速度である第二回転速度と前記第一区間における前記移動体の回転速度である第一回転速度との差分が第二回転速度閾値未満となるように前記第二区間を決定する、移動体。
2. 前記現在位置における前記移動体の並進速度を認識する走行状態認識部を更に備え、
   前記経路決定部は、前記現在位置における前記移動体の並進速度と複数の前記第一区間候補における前記移動体の並進速度である第一並進速度との差分が第一並進速度閾値未満となるように前記第一区間を決定する、請求項１に記載の移動体。
3. 前記現在位置における前記移動体の回転速度を認識する走行状態認識部を更に備え、
   前記経路決定部は、前記現在位置における前記移動体の回転速度と複数の前記第一区間候補における前記移動体の回転速度である第一回転速度との差分が第一回転速度閾値未満となるように前記第一区間を決定する、請求項１に記載の移動体。
4. 前記経路決定部は、前記目標方向及び前記障害物の位置に基づいて前記第一区間候補の障害物最接近距離及び前記第一区間候補の前記目標方向の有効距離を算出し、前記第一区間候補の前記障害物最接近距離が大きいほど前記第一区間候補の評価値を大きな値として算出すると共に、前記第一区間候補の前記有効距離が大きいほど前記第一区間候補の前記評価値を大きな値として算出し、
   前記目標方向及び前記障害物の位置に基づいて前記第二区間候補の障害物最接近距離及び前記第二区間候補の前記目標方向の有効距離を算出し、前記第二区間候補の前記障害物最接近距離が大きいほど前記第二区間候補の評価値を大きな値として算出すると共に、前記第二区間候補の前記有効距離が大きいほど前記第二区間候補の前記評価値を大きな値として算出し、
   複数の前記第一区間候補の中から最も前記評価値の高い前記第一区間候補を前記第一区間として決定し、
   前記第一区間の前記第一回転速度と前記第二回転速度との差分が前記第二回転速度閾値未満となり、前記第一区間の前記第一並進速度と前記第二並進速度との差分が前記第二並進速度閾値未満となる複数の前記第二区間候補の中から、最も前記評価値の高い第二区間候補を前記第二区間として決定し、
   前記障害物最接近距離とは、前記第一区間候補又は前記第二区間候補において前記障害物と最も接近した箇所と前記障害物との距離であり、
   前記有効距離とは、前記第一区間候補又は前記第二区間候補を前記目標方向に射影した場合の長さである、請求項１に記載の移動体。
5. 現在位置から目標位置に向かって走行する移動体の経路決定方法であって、
   前記移動体の周囲に物体が存在しないと仮定して予め設定された第一設定時間内に前記移動体が前記現在位置から走行可能な第一区間候補を複数探索する第一区間候補探索ステップと、
   前記移動体の周囲に物体が存在しないと仮定して予め設定された第二設定時間内に前記移動体が前記第一区間候補の終点から走行可能な第二区間候補を前記第一区間候補の終点ごとに複数探索する第二区間候補探索ステップと、
   前記目標位置の存在する方向である目標方向を認識する目標方向認識ステップと、
   前記移動体の周囲の物体であって前記移動体の移動の障害となる障害物の位置を検出する障害物位置認識ステップと、
   前記目標方向及び前記障害物の位置に基づいて、複数の前記第一区間候補及び複数の前記第二区間候補の中から前記移動体が走行する経路として用いられる第一区間及び第二区間を決定する経路決定ステップと、
   を含み、
   前記第一区間候補探索ステップにおいて、予め設定された前記移動体の並進速度及び前記移動体の回転速度の複数の組み合わせに基づいて、前記並進速度及び／又は前記回転速度の条件が異なる前記第一区間候補を複数探索し、
   前記第二区間候補探索ステップにおいて、予め設定された前記移動体の並進速度及び前記移動体の回転速度の複数の組み合わせに基づいて、前記並進速度及び／又は前記回転速度の条件が異なる前記第二区間候補を複数探索し、
   前記経路決定ステップにおいて、決定した前記第一区間の終点を起点とする複数の前記第二区間候補における前記移動体の並進速度である第二並進速度と前記第一区間における前記移動体の並進速度である第一並進速度との差分が第二並進速度閾値未満となるように前記第二区間を決定すると共に、
   決定した前記第一区間の終点を起点とする複数の前記第二区間候補における前記移動体の回転速度である第二回転速度と前記第一区間における前記移動体の回転速度である第一回転速度との差分が第二回転速度閾値未満となるように前記第二区間を決定する、移動体の経路決定方法。

Images