特許 7599558 軌道計画装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-12-05

(45)【発行日】2024-12-13

(54)【発明の名称】軌道計画装置

(51)【国際特許分類】

   G05D 1/43 20240101AFI20241206BHJP

   G01C 21/20 20060101ALI20241206BHJP

【ＦＩ】

G05D1/43

G01C21/20

【請求項の数】 28

(21)【出願番号】P 2023523703

(86)(22)【出願日】2021-05-24

(86)【国際出願番号】 JP2021019503

(87)【国際公開番号】W WO2022249218

(87)【国際公開日】2022-12-01

【審査請求日】2023-04-20

(73)【特許権者】

【識別番号】000006013

【氏名又は名称】三菱電機株式会社

(74)【代理人】

【識別番号】100088672

【弁理士】

【氏名又は名称】吉竹 英俊

(74)【代理人】

【識別番号】100088845

【弁理士】

【氏名又は名称】有田 貴弘

(72)【発明者】

【氏名】吉田 裕基

(72)【発明者】

【氏名】亀岡 翔太

(72)【発明者】

【氏名】伊藤 凜

(72)【発明者】

【氏名】北野 弘明

(72)【発明者】

【氏名】富永 健太

【審査官】松本 泰典

(56)【参考文献】

【文献】国際公開第２０２１／０５９３９２（ＷＯ，Ａ１）

【文献】国際公開第２０１７／１９９７５１（ＷＯ，Ａ１）

【文献】特開２０１３－４１５０６（ＪＰ，Ａ）

【文献】特開２０１２－１４５９９８（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／４３

Ｇ０１Ｃ ２１／２０

(57)【特許請求の範囲】

【請求項1】

移動体の軌道を計画する軌道計画装置であって、
前記移動体の周辺情報に基づいて、前記移動体の走行可能領域を演算する走行可能領域演算部と、
少なくとも前記移動体の目標位置を含む目標状態量を演算する目標状態演算部と、
少なくとも前記移動体の現在の状態量および前記移動体の現在位置と前記目標位置との間の１つ以上の位置における前記移動体の状態量を予測することで、複数の軌道候補を生成する状態予測部と、
前記目標状態量と前記走行可能領域とに基づいた重み付けを行い、重みに基づいて、前記複数の軌道候補を評価して評価結果を出力する予測状態評価部と、
前記評価結果に基づいて、前記複数の軌道候補から前記軌道を生成し、前記軌道に基づいて前記移動体を制御する運動制御部に対し、前記軌道を出力する軌道生成部と、を備える軌道計画装置。

【請求項2】

前記評価結果に基づいて、前記複数の軌道候補から軌道点を演算する軌道点演算部をさらに備え、
前記軌道生成部は、前記軌道点で構成される点列を前記軌道として生成する、請求項１に記載の軌道計画装置。

【請求項3】

前記走行可能領域演算部は、
現在よりも前に演算された過去の走行可能領域の形状の時系列変化に基づいて、現在の走行可能領域よりも未来の走行可能領域を予測して予測走行可能領域とし、前記現在の走行可能領域と前記予測走行可能領域とを合わせることで前記走行可能領域を延長する、請求項１記載の軌道計画装置。

【請求項4】

前記走行可能領域演算部は、
現在よりも前に演算された過去の走行可能領域の外側の障害物の種類に基づいて、現在の走行可能領域よりも未来の走行可能領域を予測して予測走行可能領域とし、前記現在の走行可能領域と前記予測走行可能領域とを合わせることで前記走行可能領域を延長する、請求項１記載の軌道計画装置。

【請求項5】

前記走行可能領域演算部は、
前記過去の走行可能領域の前記外側の障害物が、静止障害物の場合は、未来においても前記静止障害物より先の領域は走行不可の領域のままと判断し、前記外側の障害物が移動障害物の場合は、未来において、前記移動障害物の移動方向と移動速度に基づいて、前記走行可能領域を延長し、前記外側の障害物が無い領域では、前記移動体の速度に基づいて前記走行可能領域を延長する、請求項４記載の軌道計画装置。

【請求項6】

前記走行可能領域演算部は、
周辺環境情報に基づいて、前記移動体が将来走行できない領域を予測し、前記領域を除外して前記走行可能領域とする、請求項１記載の軌道計画装置。

【請求項7】

前記目標状態演算部は、
前記目標状態量に前記移動体の目標速度を含める、請求項１記載の軌道計画装置。

【請求項8】

前記目標状態演算部は、
前記目標状態量に前記移動体の目標方位角を含める、請求項７記載の軌道計画装置。

【請求項9】

前記目標状態演算部は、
前記目標状態量に前記目標方位角の方向に対して垂直な方向の位置である目標横位置を含める、請求項８記載の軌道計画装置。

【請求項10】

前記目標状態演算部は、
前記目標位置が前記走行可能領域の外側にある場合に、前記走行可能領域において前記目標位置に最も近い位置を前記目標位置として再設定する、請求項１記載の軌道計画装置。

【請求項11】

前記目標状態演算部は、
前記走行可能領域の形状に応じて、前記目標速度に対して上限値を設定する、請求項７記載の軌道計画装置。

【請求項12】

前記状態予測部は、
１つ以上の入力値に対し前記走行可能領域の形状に応じた上限値を設定し、前記１つ以上の入力値を前記移動体の運動モデルに入力することで前記複数の軌道候補を演算する、請求項１記載の軌道計画装置。

【請求項13】

前記状態予測部は、
１つ以上の入力値に対し前記走行可能領域の形状に応じて前記１つ以上の入力値の分布を変え、前記１つ以上の入力値を前記移動体の運動モデルに入力することで前記複数の軌道候補を演算する、請求項１２記載の軌道計画装置。

【請求項14】

前記状態予測部は、
前記走行可能領域の形状に応じて前記入力値の数を変える、請求項１２または請求項１３記載の軌道計画装置。

【請求項15】

前記状態予測部は、
１つ以上の入力値に対し前記目標状態量と前記移動体の前記現在の状態量との乖離度に応じて前記入力値の分布を変え、前記入力値を前記移動体の運動モデルに入力することで前記複数の軌道候補を演算する、請求項１記載の軌道計画装置。

【請求項16】

前記状態予測部は、前記乖離度に応じて前記入力値の数を変える、請求項１５記載の軌道計画装置。

【請求項17】

前記予測状態評価部は、
前記目標状態量と前記移動体の前記現在の状態量との偏差に対して重み付け係数を設定することで前記複数の軌道候補を評価する請求項１記載の軌道計画装置。

【請求項18】

前記予測状態評価部は、
前記目標状態量と前記移動体の前記現在の状態との偏差に対して重み付けを設定し、前記移動体の方位角と前記目標方位角との偏差に応じて前記移動体の前記方位角に対する重み付け係数を変えることで前記複数の軌道候補を評価する、請求項８記載の軌道計画装置。

【請求項19】

前記予測状態評価部は、
前記複数の軌道候補に対して重み付けをして重み付けされた軌道候補とし、前記重み付けされた軌道候補の重みに基づいて前記複数の軌道候補を評価する、請求項１記載の軌道計画装置。

【請求項20】

前記予測状態評価部は、
前記走行可能領域の外側にある前記複数の軌道候補に対し、重み付け係数を０として前記重み付けされた軌道候補とする、請求項１９記載の軌道計画装置。

【請求項21】

前記予測状態評価部は、
前記走行可能領域の外側にある前記複数の軌道候補に対し、前記走行可能領域の境界からの距離に応じて重み付け係数を変えて前記重み付けされた軌道候補とする、請求項１９記載の軌道計画装置。

【請求項22】

前記予測状態評価部は、
前記走行可能領域の内側にある前記複数の軌道候補に対し、前記走行可能領域の境界からの距離に応じて重み付け係数を変えて前記重み付けされた軌道候補とする、請求項１９記載の軌道計画装置。

【請求項23】

前記走行可能領域演算部は、
周辺環境情報に基づいて、前記移動体が将来走行できない領域を予測し、
前記予測状態評価部は、前記領域にある前記複数の軌道候補に対し、重み付け係数を予め設定された値よりも小さくして前記重み付けされた軌道候補とする、請求項１９記載の軌道計画装置。

【請求項24】

前記予測状態評価部は、
前記複数の軌道候補に対して重み付けをして重み付けされた軌道候補とし、
前記予測走行可能領域の内側の前記複数の軌道候補の重み付け係数を、前記現在の走行可能領域の内側の前記複数の軌道候補の前記重み付け係数よりも小さくして前記重み付けされた軌道候補を生成し、前記重み付けされた軌道候補の重みに基づいて前記複数の軌道候補を評価する、請求項３または請求項４に記載の軌道計画装置。

【請求項25】

前記予測状態評価部は、
前記複数の軌道候補に対し重み付けをして重み付けされた軌道候補とし、
前回演算された前記軌道点に対して、跨ぎ禁止線を跨ぐような位置に前記重み付けされた軌道候補がある場合には、前記複数の軌道候補の重み付け係数を他の前記複数の軌道候補の前記重み付けの係数よりも小さくして前記重み付けされた軌道候補を生成し、前記重み付けされた軌道候補の重みに基づいて前記複数の軌道候補を評価する、請求項２記載の軌道計画装置。

【請求項26】

前記予測状態評価部は、
前記複数の軌道候補に対し重み付けをして重み付けされた軌道候補とし、
前記軌道点演算部は、
前記重み付けされた軌道候補に対して加重平均値を演算することで前記軌道点とする、請求項２記載の軌道計画装置。

【請求項27】

前記予測状態評価部は、
前記複数の軌道候補に対し重み付けをして重み付けされた軌道候補とし、
前記軌道点演算部は、
前記重み付けされた軌道候補のうち、最も重み付け係数が大きい前記重み付けされた軌道候補を前記軌道点とする、請求項２記載の軌道計画装置。

【請求項28】

前記予測状態評価部は、
前記複数の軌道候補に対して前回演算された前記軌道点の状態量または前記移動体の現在の状態量との差に応じて重み付けをして重み付けされた軌道候補とする、請求項２記載の軌道計画装置。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、軌道計画装置に関し、特に、車両等の自動運転を実現するための動作を計画する軌道計画装置に関する。

【背景技術】

【0002】

近年、自動車の自動運転および搬送台車などの自律移動システムの開発が進んでいる。自律移動システムでは、移動体が走行すべき軌跡と速度で構成される軌道を生成し、生成された軌道に沿って移動体が走行するように制御される。軌道計画は、多くのシーンにおいて道路の中央および磁気マーカーなどの誘導体に沿うような軌道計画が立てられる。しかし場合によっては、道路の白線が無い料金所付近、未舗装路での走行シーンおよび誘導体を使用しない自律搬送台車が目的地へ移動するシーンにおいては、このような情報は使用できない。このようなシーンでは、走行すべき目印情報の無い空間上を、障害物を回避しつつ目的地へ到達することができる軌道が必要であり、例えば、特許文献１に開示されるように走行すべき目印情報が無くとも軌道計画を実現する技術が開発されている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１２－１４５９９８号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

特許文献１においては、障害物の存在しない走行可能領域に内接する複数の円の中心点を通る線で構成される道なり方向に基づいて、走行経路を生成する方法が採られている。この場合、例えば空港のような非常に広い走行可能領域では、内接する円を決定できないため、走行経路を生成できず目的地へ到達できない。また、料金所付近のような、複雑な形状で横幅の変化が大きい走行可能領域では、正しい道なり方向を演算できず目的地へ到達できない。

【0005】

本開示は、上記のような問題を解決するためになされたものであり、空港および料金所付近などのように走行可能領域が複雑な場合においても、走行可能領域を通って目的地へ到達することができる軌道計画装置を提供することを目的とする。

【課題を解決するための手段】

【0006】

本開示に係る軌道計画装置は、移動体の軌道を計画する軌道計画装置であって、前記移動体の周辺情報に基づいて、前記移動体の走行可能領域を演算する走行可能領域演算部と、少なくとも前記移動体の目標位置を含む目標状態量を演算する目標状態演算部と、少なくとも前記移動体の現在の状態量および前記移動体の現在位置と前記目標位置との間の１つ以上の位置における前記移動体の状態量を予測することで、複数の軌道候補を生成する状態予測部と、前記目標状態量と前記走行可能領域とに基づいた重み付けを行い、重みに基づいて、前記複数の軌道候補を評価して評価結果を出力する予測状態評価部と、前記評価結果に基づいて、前記複数の軌道候補から前記軌道を生成し、前記軌道に基づいて前記移動体を制御する運動制御部に対し、前記軌道を出力する軌道生成部と、を備えている。

【発明の効果】

【0007】

本開示に係る軌道計画装置によれば、移動体の目標位置を含む目標状態量と走行可能領域とに基づいて、目標状態量までの軌道を評価し、評価結果に基づいて軌道を生成するため、走行可能領域が複雑な場合においても、走行可能領域を通って目的地へ到達することができる。

【図面の簡単な説明】

【0008】

【図1】実施の形態１の軌道計画装置を搭載した移動体の概略構成の一例を示すブロック図である。

【図2】実施の形態１における走行可能領域の一例を示す図である。

【図3】実施の形態１における目標状態量の一例を示す図である。

【図4】実施の形態１の軌道計画装置において軌道点演算部が演算する軌道点の一例を示す図である。

【図5】実施の形態１の軌道計画装置において軌道生成部が生成する軌道の一例を示す図である。

【図6】実施の形態１の軌道計画装置の動作を説明するフローチャートである。

【図7】実施の形態１の軌道計画装置において情報取得部からの取得する情報の一例を示す図である。

【図8】実施の形態１の軌道計画装置において情報取得部から取得した情報を移動体座標系に変換した一例を示す図である。

【図9】実施の形態１の軌道計画装置における走行可能領域の予測の一例を示す図である。

【図10】実施の形態１の軌道計画装置における走行可能領域の予測の他の例を示す図である。

【図11】実施の形態１の軌道計画装置における走行可能領域の一例を示す図である。

【図12】実施の形態１の軌道計画装置における走行可能領域の一例を示す図である。

【図13】実施の形態１の軌道計画装置における走行可能領域の一例を示す図である。

【図14】実施の形態１の軌道計画装置における目標状態量の一例を示す図である。

【図15】実施の形態１の軌道計画装置における再設定された目標状態量の一例を示す図である。

【図16】実施の形態１の軌道計画装置における目標状態量のうち速度の上限値を設定する一例を示す図である。

【図17】実施の形態１の軌道計画装置における目標状態量のうち速度の上限値を設定する一例を示す図である。

【図18】実施の形態１の軌道計画装置においてパーティクルフィルタにより予測された状態量の一例を示す図である。

【図19】実施の形態１の軌道計画装置において走行可能領域の形状に応じて設定された入力値の一例を示す図である。

【図20】実施の形態１の軌道計画装置において走行可能領域の形状に応じて設定された入力値の一例を示す図である。

【図21】実施の形態１の軌道計画装置において移動体の現在の状態量と目標状態量との乖離が大きい場合の入力値の設定の一例を示す図である。

【図22】実施の形態１の軌道計画装置における観測変数の一例を示す図である。

【図23】実施の形態１の軌道計画装置における走行可能領域外のパーティクルの重み付けの一例を示す図である。

【図24】実施の形態１の軌道計画装置における走行可能領域内のパーティクルの重み付けの一例を示す図である。

【図25】実施の形態１の軌道計画装置における走行可能領域内のパーティクルの重み付けの一例を示す図である。

【図26】実施の形態１の軌道計画装置における走行可能領域内のパーティクルの重み付けの一例を示す図である。

【図27】実施の形態１における予測された走行可能領域内のパーティクルの重み付けの一例を示す図である。

【図28】実施の形態１の軌道計画装置における評価の重みの設定の一例を示す図である。

【図29】実施の形態１の軌道計画装置において目標状態量に到達する複数の軌道点を得る処理の一例を示す図である。

【図30】実施の形態１の軌道計画装置において目標状態量に到達するまでの軌道生成の一例を示す図である。

【図31】実施の形態１の軌道計画装置において目標状態量に到達するまでの軌道生成の一例を示す図である。

【図32】実施の形態２の軌道計画装置を搭載した移動体の概略構成の一例を示すブロック図である。

【図33】実施の形態２の軌道計画装置において移動体と目標位置とを結ぶ多項式の導出方法を説明する図である。

【図34】実施の形態２の軌道計画装置において移動体と目標位置とを結ぶ多項式の導出方法を説明する図である。

【図35】実施の形態２の軌道計画装置において移動体と目標位置とを結ぶ多項式の導出方法を説明する図である。

【図36】予測状態量が、現在の移動体の状態量と大きく乖離している場合の重み付けを説明する図である。

【図37】予測状態量が、前回演算された軌道点の状態量と大きく乖離している場合の重み付けを説明する図である。

【図38】実施の形態１および２の軌道計画装置を実現するハードウェア構成を示す図である。

【図39】実施の形態１および２の軌道計画装置を実現するハードウェア構成を示す図である。

【発明を実施するための形態】

【0009】

＜実施の形態１＞
図１は、実施の形態１の軌道計画装置を搭載した移動体１の概略構成の一例を示すブロック図である。移動体１は、移動体１の到達すべき目的地情報、移動体１の周辺環境情報、および移動体１の自己状態を取得する情報取得部１００から得られる情報に基づき、移動体１の通るべき軌道を生成する軌道計画装置２００と、軌道計画装置２００によって生成された軌道に基づいて移動体１の運動を制御する運動制御部３００とを備えている。

【0010】

情報取得部１００は、目標値取得部１１０、自己状態取得部１２０および周辺環境取得部１３０を有している。

【0011】

目標値取得部１１０は、移動体が到達すべき目標位置、目標速度、目標方位角などの情報を取得する。目標値取得部１１０は、例えば管制から受信したインフラ情報、ユーザーが予め指定した情報、移動体が持つ地図情報の所定の位置などから情報を取得する。ここで、移動体が持つ地図情報は、高精度地図とは異なり、カーナビ地図、ＳＬＡＭ（Simultaneous Localization and Mapping）等で生成した点群地図等を指す。

【0012】

目標位置は、例えば料金所における、ゲートの入り口あるいはバーの位置、高速道路における退避位置、トーイングトラクターにおける飛行機の前輪部、ユーザーが指定した移動体１の位置などが挙げられる。目標速度は、例えば、法定速度、ユーザーが予め設定した指定の速度などが挙げられる。目標方位角は、目標位置を通過する際の目標の角度であり、例えば、ゲートを通過する際のゲートに対する垂直方向の向きなどが挙げられる。

【0013】

自己状態取得部１２０は、移動体自身の現在の状態を取得する。自己状態取得部１２０は、例えば、速度センサ、加速度センサ、慣性計測装置、操舵角センサ、操舵トルクセンサ、ヨーレートセンサおよび全地球衛星測位システム（ＧＮＳＳ：Global Navigation Satellite System）センサなどが挙げられる。ここで慣性計測装置は、以下においてＩＭＵ（Inertial Measurement Unit）センサと呼称する。

【0014】

周辺環境取得部１３０は、移動体周辺の壁、移動障害物の位置および速度、方位角、障害物の無い走行可能な空間情報を取得する。周辺環境取得部１３０は例えば、ミリ波レーダー、カメラ、ＬｉＤＡＲ（Light Detection and Ranging）、ソナー、車車間通信装置、および路車間通信装置などが挙げられる。

【0015】

軌道計画装置２００は、走行可能領域演算部２１０、目標状態演算部２２０、状態予測部２３０、予測状態評価部２４０、軌道点演算部２５０および軌道生成部２６０を有している。

【0016】

走行可能領域演算部２１０は、前記周辺環境取得部１３０から取得した移動体１の周辺情報を基に、障害物が存在しない、移動体１が走行可能である走行可能領域を演算する。図２には、走行可能領域の一例を示す。図２において、移動体１の進行方向左側に静止障害物ＳＯＢが存在し、進行方向右側から移動障害物ＭＯＢが、左右の車線境界ＬＢで規定される走行車線に進入しようとしており、静止障害物ＳＯＢおよび移動障害物ＭＯＢが存在しない走行可能領域ＴＡが太線で示されている。図２に示されるように、走行可能領域ＴＡとは、必ずしも道路の白線等の車線境界ＬＢで規定される走行車線に限定されるものではない。

【0017】

目標状態演算部２２０は、目標値取得部１１０からの情報に基づいて、移動体１が到達するべき目的地における目標状態量を演算する。この目標状態量は、少なくとも移動体１の目標位置を含んでいる。図３に目標状態量の一例を示す。

【0018】

図３はＥＴＣゲートの手前のような白線が無い道路を示しており、ここでの車線境界ＬＢは白線ではなく壁またはガードレールなどである。図３において目標状態量ＴＧは、時刻ｔでの目標位置の座標（ｘ_ｔ,ｙ_ｔ）、目標方位角θ_ｔ、目標速度ｖ_ｔを含んでいる。なお、現在の移動体１の状態量は（ｘ_ｅ,ｙ_ｅ,θ_ｅ,ｖ_ｅ ）である。

【0019】

状態予測部２３０は、少なくとも移動体１の現在の状態量および移動体１の現在位置と目標位置との間の１つ以上の位置における移動体１の状態量を予測することで、１つ以上の軌道候補を生成する。そのために、例えば移動体の運動モデルを用いた状態推定演算によって、所定の複数の入力を前記移動体の運動モデルに入力し、複数の入力分の少なくとも１ステップ先、すなわち制御周期における１サンプリング時間先の状態量を予測することで、１つ以上の軌道候補を生成する。本実施の形態においては、状態推定手法の一例として、パーティクルフィルタを用いる。

【0020】

パーティクルフィルタとは、確率密度分布による時系列データの予測手法であり、逐次モンテカルロ法と呼称される場合もある。また、状態推定演算としてのパーティクルフィルタは、複数のパーティクルによって状態の確率密度分布を近似するものであり、例えば、ある状態量を持つパーティクルが多ければ、その状態推定演算としてパーティクルフィルタを用いることで、全体の確率密度分布を推定することが可能となるため、局所最適解を出力する頻度を小さくすることができる。

【0021】

予測状態評価部２４０は、各予測状態量、すなわち各パーティクルに重み付けをすることで重み付けされた軌道候補とし、重み付けされた軌道候補を重みに基づいて評価して評価結果を出力する。このとき、目標状態演算部２２０で演算した目標状態量と走行可能領域演算部２１０において演算した走行可能領域に基づいての重み付けを行う。当該重みは、後述する軌道点演算部２５０において、軌道点を演算する際、各予測状態量の重みの値を基に、状態予測部２３０において予測された複数の状態量を加重平均することで尤もらしい状態量を演算することができる。

【0022】

例えば、走行可能領域外の状態量の重み付け係数を０とすれば、加重平均の演算の際に、その状態量に重み付け係数０が乗算され、走行可能領域外の軌道点となることを防ぐことができ、走行可能領域ＴＡ内の軌道であることが保証された軌道を生成できる。

【0023】

軌道点演算部２５０は、軌道候補から軌道点を演算する。具体的には、軌道点演算部２５０は、状態予測部２３０で予測された予測状態量を、予測状態評価部２４０により付与された重みに応じて加重平均し、加重平均された状態量を軌道点とする演算を行う。当該演算の概念図を図４に示す。

【0024】

図４に示されるように、走行可能領域ＴＡおよびその近傍に重み付け係数が０の状態量のパーティクル群Ｇ０、重み付け係数が低い状態量のパーティクル群ＧＬおよび重み付け係数が高い状態量のパーティクル群ＧＨがあり、重み付け係数が高い状態量のパーティクル群ＧＨの加重平均された状態量を軌道点ＴＰとしている。また、予測状態評価部２４０により付与された重みが最も高い状態量を軌道点とすることもできる。

【0025】

軌道生成部２６０は、評価結果に基づいて、軌道候補から軌道を生成し、生成した軌道に基づいて移動体１を制御する運動制御部３００に対し、軌道を出力する。具体的には、軌道生成部２６０は、軌道点演算部２５０で演算された、各離散時間ごとの軌道点から構成される点列を生成軌道として運動制御部３００へ出力する。生成軌道の概念図を図５に示す。

【0026】

図５に示されるように、走行可能領域ＴＡには離散時間ｔ_１、ｔ_２、ｔ_３、ｔ_４およびｔ_５におけるそれぞれの軌道点ＴＰ１、ＴＰ２、ＴＰ３、ＴＰ４およびＴＰ５が示されており、５つの軌道点より生成軌道ＧＴが得られる。

【0027】

運動制御部３００は、制御量演算部３１０およびアクチュエータ制御部３２０を有している。

【0028】

制御量演算部３１０は、軌道生成部２６０で生成された軌道を目標軌道として、目標軌道に沿って走行するための移動体１への目標制御値を演算し、アクチュエータ制御部３２０に出力する。

【0029】

アクチュエータ制御部３２０は、移動体１に搭載されたコントローラであり、制御量演算部３１０で演算された目標制御値に移動体が追従するように、アクチュエータを動作させる。アクチュエータとしては、例えばステアリング、駆動用モータおよびブレーキが挙げられる。

【0030】

次に、実施の形態１の軌道計画装置２００の動作の一例を図６に示すフローチャートを用いて説明する。以下では、パーティクルフィルタを用いる場合について説明する。なお、以下において「１ステップ」とは、制御周期における１サンプリング時間のことを指す。

【0031】

まず、軌道計画装置２００の入力情報として、目標値取得部１１０より目標位置、目標速度、目標方位角等の目標値を取得し、自己状態取得部１２０より移動体の位置、速度、方位角等の自己状態を取得し、周辺環境取得部１３０より、走行可能領域の各頂点座標、移動障害物の位置、速度等の周辺環境情報を取得する（ステップＳ１０１）。このときの入力情報の概念図を図７に示す。

【0032】

図７において、走行可能領域ＴＡを規定する複数の頂点ＶＴＡの座標はｘ座標が、ｘ_ｆ１，ｘ_ｆ２，ｘ_ｆ３・・・ｘ_ｆｉで表され、ｙ座標が、ｙ_ｆ１，ｙ_ｆ２，ｙ_ｆ３・・・ｙ_ｆｉで表される。

【0033】

目標状態量ＴＧは、目標位置の座標（ｘ_ｔ,ｙ_ｔ）、目標方位角θ_ｔ、目標速度ｖ_ｔを含み、現在の移動体１の自己状態量はｘ座標がｘ_ｅ、ｙ座標がｙ_ｅ、速度はｖ_ｅ、方位角はθ_ｅで表される。

【0034】

また、各移動障害物ＭＯＢの座標はｘ座標がｘ_O１，ｘ_O２，ｘ_O３・・・ｘ_Oｉで表され、ｙ座標がｙ_O１，ｙ_O２，ｙ_O３・・・ｙ_Oｉで表され、速度はｖ_O１，ｖ_O２，ｖ_O３・・・ｖ_Oｉで表され、方位角はθ_O１，θ_O２，θ_O３・・・θ_Oｉで表される。

【0035】

図７では、障害物の存在により走行可能領域ＴＡの一部が欠けており、障害物の輪郭に沿って複数の頂点ＶＴＡが表されている。

【0036】

なお、本実施の形態では、走行可能領域ＴＡの情報として走行可能領域ＴＡの各頂点を抽出するが、円または楕円といった線の情報を用いることもできる。

【0037】

また、自己状態取得部１２０より得た自己状態を用いることで、図８に示されるように移動体１の位置を原点、移動体１の向きをｘ軸、移動体の向きに垂直な方向をｙ軸とした移動体座標系に変更された値を用いることもできる。以下では当該移動体座標系の値を用いるものとする。

【0038】

図８に示されるように移動体座標系では、複数の頂点ＶＴＡの座標はＸ座標が、Ｘ_ｆ１，Ｘ_ｆ２，Ｘ_ｆ３・・・Ｘ_ｆｉで表され、Ｙ座標が、Ｙ_ｆ１，Ｙ_ｆ２，Ｙ_ｆ３・・・Ｙ_ｆｉで表される。

【0039】

目標状態量ＴＧは、目標位置の座標（Ｘ_ｔ,Ｙ_ｔ）、目標方位角Θ_ｔ、目標速度Ｖ_ｔを含み、現在の移動体１の自己状態量はＸ座標がＸ_ｅ、Ｙ座標がＹ_ｅ、速度はＶ_ｅ、方位角はΘ_ｅで表される。

【0040】

また、各移動障害物ＭＯＢの座標はＸ座標がＸ_O１，Ｘ_O２，Ｘ_O３・・・Ｘ_Oｉで表され、Ｙ座標がＹ_O１，Ｙ_O２，Ｙ_O３・・・Ｙ_Oｉで表され、速度はＶ_O１，Ｖ_O２，Ｖ_O３・・・Ｖ_Oｉで表され、方位角はΘ_O１，Θ_O２，Θ_O３・・・Θ_Oｉで表される。

【0041】

目標値情報に関する各値の座標変換には、以下の数式（１）が使用される。

【0042】

【数1】

【0043】

移動障害物情報に関する各値の座標変換には、以下の数式（２）が使用される。

【0044】

【数2】

【0045】

走行可能領域ＴＡの各頂点に関する各値の座標変換には、以下の数式（３）が使用される。

【0046】

【数3】

【0047】

ここで、図６のフローチャートの説明に戻る。ステップＳ１０１において周辺環境情報を取得した後は、走行可能領域演算部２１０において、周辺環境取得部１３０から取得した情報を基に、障害物が存在せず移動体１が走行可能である走行可能領域ＴＡを演算する（ステップＳ１０２）。本実施の形態では、図８に示した移動体座標系における走行可能領域ＴＡの各頂点ＶＴＡに関するＸ座標Ｘ_ｆ１～Ｘ_ｆｉおよびＹ座標Ｙ_ｆ１～Ｙ_ｆｉを軌道生成に用いる。

【0048】

なお、走行可能領域ＴＡの各頂点ＶＴＡに関するＸ座標Ｘ_ｆ１～Ｘ_ｆｉおよびＹ座標Ｙ_ｆ１～Ｙ_ｆｉを軌道生成に用いる代わりに、過去に演算した走行可能領域ＴＡの形状の時系列変化に基づいて、現在演算した走行可能領域よりも後の時間での走行可能領域を予測して予測走行可能領域とし、現在演算した走行可能領域と予測走行可能領域とを合わせたものを走行可能領域とし、軌道生成に用いることもできる。

【0049】

図９は、走行可能領域ＴＡの形状の時系列変化に基づいて走行可能領域を予測する方法を表す概念図である。図９においては、時刻Ｔとして現在の時刻をｔとし、それよりも制御周期における１サンプリング時間前をｔ－１、さらに１サンプリング時間前をｔ－２とし、時刻ｔよりも１サンプリング時間後をｔ＋１としている。

【0050】

図９において、移動体１は矢印の方向に前進しており、走行可能領域ＴＡは、前方の障害物の存在により部分的に欠けている。時刻ｔ－２の過去の走行可能領域ＴＡ、時刻ｔ－１の過去の走行可能領域ＴＡから時刻ｔの現在の走行可能領域ＴＡの経時変化から、移動体１が進んだ分だけ手前方向に変化する部分ＮＰ１と、自車が進んでも変化しない、または変化が少ない部分ＮＰ２とが存在することが判る。部分ＮＰ１は静止障害物である可能性が高く、部分ＮＰ２は移動障害物である可能性が高い。このような情報から、時刻ｔ＋１での未来の走行可能領域を予測すると、図９の右端の図におけるハッチングを付した領域が予測走行可能領域ＥＴＡとなり、時刻ｔの現在の走行可能領域ＴＡと予測走行可能領域ＥＴＡとを合わせることで現在の走行可能領域ＴＡを延長することができる。

【0051】

予測走行可能領域ＥＴＡを用いることで、現在認知できている外界センサにより得られた走行可能領域よりも遠くの領域まで軌道を生成することができる。当該領域は、未来において走行可能領域となる見込みがある。

【0052】

また、走行可能領域ＴＡ外の障害物の種類に基づいて、予測走行可能領域を演算し、軌道生成に用いることもできる。

【0053】

図１０は、障害物の種類に基づいて走行可能領域を予測する方法を表す概念図である。なお、障害物の種類とは、例えば、壁、停止他車両および移動他車両などが挙げられる。障害物が移動他車両の場合は、周辺環境取得部１３０は、移動他車両の位置だけでなく速度なども取得する。そして、走行可能領域演算部２１０は、移動他車両の位置および速度などに基づいて予測走行可能領域を演算する。よって、予測走行可能領域は、現在演算された走行可能領域に入っていない領域も含まれる。これは、現在は走行不可と判定された領域であっても、その領域が将来は走行可能と判定されることを意味する。

【0054】

図１０において、速度ｖ_ｅの移動体１における現在時刻ｔでの現在の走行可能領域ＴＡを左図として示しており、右図には現在よりも時間ｔ_ｘ後の未来の予測走行可能領域ＴＡＸを示している。

【0055】

図１０の左図において、走行可能領域ＴＡの最大認知距離はＬ_ｍａｘであり、走行可能領域ＴＡの最遠部より先には障害物が何も無い領域ＮＲを破線で示している。また、静止障害物ＳＯＢと速度ｖ_Oの移動障害物ＭＯＢの存在により、走行可能領域ＴＡは部分的に欠けている。

【0056】

図１０の右図において、前方に静止障害物ＳＯＢがある地点は、未来においても走行可能とはならない領域Ｒ１であり、前方に移動障害物ＭＯＢがある地点は、未来において移動障害物ＭＯＢの進行方向に走行可能領域が延びるものと仮定し、走行可能領域ＴＡを延長して延長領域Ｒ２とする。延長する長さは、(ｖ_O－ｖ_ｅ)×ｔ_ｘである。また、走行可能領域ＴＡの最大認知距離Ｌ _ｍａｘ に達している地点は、その先も走行可能であると仮定し、走行可能領域ＴＡを延長して延長領域Ｒ３とする。延長する長さは、ｖ_ｅ×ｔ_ｘである。

【0057】

予測走行可能領域ＴＡＸを用いることで、現在認知できている外界センサにより得られた走行可能領域よりも遠くの位置まで軌道を生成することができる。実際の障害物の動きを基にして走行可能領域を予測するので信頼度が高くなる。また、実際の障害物の動きだけでなく、図９に示すように走行可能領域ＴＡの形状の時系列変化も考慮して走行可能領域ＴＡを予測することができる。これにより、更に信頼度が高くなる。

【0058】

また、ステップＳ１０２の走行可能領域ＴＡの演算では、走行可能であっても行き止まりとなりうる領域が存在する場合は、必要に応じて、走行可能領域ＴＡから当該領域を取り除く処理を行う。図１１は当該処理を説明する概念図である。

【0059】

図１１においては、移動体１の前方にＥＴＣゲートなどの狭い通路が存在するシーンを示しており、目標位置ＴＧＰは通過可能なゲート内となっている。他のゲートは走行不可能であり、将来走行できない行き止まりＤＥとなっており、除去領域ＡＲとして走行可能領域ＴＡから取り除かれる。

【0060】

行き止まりとなりうる領域は、例えば、ｘ軸方向の値が目標位置ＴＧＰとほぼ同じ値であるのに、ｙ軸方向の値に乖離がある場所は行き止まりとする。または図１１のような俯瞰図、航空写真などが得られる場合において、目標位置ＴＧＰとの間に壁がある領域、目標位置ＴＧＰが壁に囲まれている領域などを画像処理技術により検出する。インフラ情報を活用し、工事中、使用不可能なＥＴＣゲート等の位置情報を得られた場合に、通行ができない領域を走行可能領域ＴＡと照らし合わせ、走行可能領域ＴＡにおける移動体１が将来走行できない領域を行き止まりとなりうる領域とする。

【0061】

走行可能領域ＴＡ内であっても、目標位置ＴＧＰに到達する前に行き止まりで阻まれ、身動きが取れなくなることを回避する軌道を生成することができる。

【0062】

また、ステップＳ１０２の走行可能領域ＴＡの演算では、走行可能であっても進入禁止とする領域が存在する場合は、必要に応じて、走行可能領域ＴＡから当該領域を取り除く処理を行う。図１２は当該処理を説明する概念図である。

【0063】

図１２においては、移動体１の前方に進入禁止エリアＩＰＡが設けられているシーンを示している。進入禁止エリアＩＰＡとしては工事中などの領域でフェンス等の障害物で囲われていない領域が挙げられる。走行可能領域ＴＡと進入禁止エリアＩＰＡとが重なる領域は、除去領域ＡＲとして走行可能領域ＴＡから取り除かれる。

【0064】

進入禁止エリアＩＰＡは、図１２のような俯瞰図、航空写真などが得られる場合において、工事中などの領域を画像処理技術により検出するなど、移動体１に取り付けられている前方カメラを用いて、工事中といった進入できない領域を画像処理技術により検出する。インフラ情報を活用し、工事などをしている位置情報を得られた場合に、工事中の領域などを走行可能領域と照らし合わせ、走行可能領域ＴＡにおける工事中等の領域を進入禁止エリアＩＰＡとする。

【0065】

また、ステップＳ１０２の走行可能領域ＴＡの演算では、跨いで走行することを禁止する跨ぎ禁止線がある場合、走行可能領域ＴＡと跨ぎ禁止線とが重なる領域のみを走行可能とする処理を行う。図１３は当該処理を説明する概念図である。

【0066】

図１３においては、移動体１の進行方向の左右に跨ぎ禁止線ＮＳＬが設けられているシーンを示している。跨ぎ禁止線ＮＳＬとしては、路面に設けられた、はみ出し禁止の白色実線、追い越しのためのはみ出し禁止の黄色実線が挙げられる。また、交差点付近３０ｍでは車線変更してはいけない、ＥＴＣゲート直前では車線変更してはいけない等のルールも含む。そして、走行可能領域ＴＡと跨ぎ禁止線ＮＳＬとで囲まれた領域は、除去領域ＡＲとして走行可能領域ＴＡから取り除かれ、走行可能領域ＴＡと跨ぎ禁止線とが重なる領域のみが走行可能となる。

【0067】

図１１～図１３を用いて説明したように、走行可能領域演算部２１０は、移動体１の周辺環境情報に基づいて、移動体１が将来走行できない領域を予測し、この領域を除いたものを走行可能領域ＴＡとして演算する。なお、将来走行できない領域は、図１１～図１３に示すような領域に限定されない。

【0068】

ここで、図６のフローチャートの説明に戻る。ステップＳ１０２において走行可能領域ＴＡを演算した後は、目標状態演算部２２０において、目標値取得部１１０で取得された目標値に基づいて、移動体１が到達すべき目標状態量を演算する（ステップＳ１０３）。本実施の形態における目標状態量の概要図を図１４に示す。

【0069】

図１４に示すように、目標状態量として目標位置（Ｘ_ｔ,Ｙ_ｔ）、目標速度Ｖ_ｔ、目標方位角Θ_ｔを用いて算出する目標位置の目標方位角方向に対して垂直な方向の位置である目標横位置Ｙ_Ｌ、目標速度Ｖ_ｔ、目標方位角Θ_ｔに加え、危険領域Ｄ内の移動障害物ＭＯＢとの距離Ｄ_Ｏを目標車間距離Ｄ_ｔ以上とするために、目標車間距離Ｄ_ｔを目標状態量に含めるものとし、目標状態量Ｐ_ｔは以下の数式（４）で表される。

【0070】

【数4】

【0071】

目標状態量に目標速度Ｖ_ｔを含めることで、例えば、法定速度を守るといった任意の速度で走行可能な軌道を生成することができる。

【0072】

目標状態量に目標方位角Θ_ｔを含めることで、例えば、料金所ゲート通過のシーン等の前方の狭い通路に進入する際に、ゲート対して垂直方向の方位角を目標状態量に設定することでゲートに真っ直ぐ進入するような軌道を生成することができる。

【0073】

目標状態量に目標横位置Ｙ_Ｌを含めることで、真横へ移動できない移動体１にとって、目標位置の横方向の成分のみを目標状態量とすることで、早期に目標位置に対する横方向の偏差を縮めることができ、早期に目標位置を目標方位角で通過できる状態にすることができる。

【0074】

図１４に示される危険領域Ｄは、動的な先行車および静止障害物と確保すべき安全な車間距離、および移動体１が動いている際に、移動体１の付近に人および他車両があることで衝突の可能性が高まり、また、衝突すると被害が大きく危険であるため、人および他車両が近づいてはいけない距離として定義される。図１４では危険領域Ｄ内に移動障害物ＭＯＢが存在し、移動障害物ＭＯＢとの距離Ｄ_Ｏは目標車間距離Ｄ_ｔよりも小さくなっている。

【0075】

また、目標状態量Ｐ_ｔは、目標位置（Ｘ_ｔ,Ｙ_ｔ）を用いて以下の数式（５）で表すこともできる。

【0076】

【数5】

【0077】

また、目標状態量Ｐ_ｔは、少なくとも目標位置（Ｘ_ｔ,Ｙ_ｔ）が含まれている場合、目標速度Ｖ_ｔ、目標方位角Θ_ｔおよび目標車間距離Ｄ_ｔを用いないこともできる。これは、目標状態量として最低限目標位置（Ｘ_ｔ,Ｙ_ｔ）が含まれていれば、目標位置に到達可能な軌道を軌道生成部２６０で生成できるからである。

【0078】

また、目標状態演算部２２０は、目標状態量が走行可能領域ＴＡ外にある場合、目標状態量から最も近い走行可能領域ＴＡ内の状態量を、目標状態量として設定するように処理することもできる。図１５は当該処理を説明する概念図である。

【0079】

図１５においては、目標状態量として目標位置（Ｘ_ｔ,Ｙ_ｔ）を用いる場合を示しており、元の目標位置ＯＴＧＰが走行可能領域ＴＡ外に存在している。元の目標位置ＯＴＧＰから直線距離で最も近い走行可能領域ＴＡ内の目標位置は、走行可能領域ＴＡの右側角部辺りとなるので、ここに目標位置ＴＧＰを設定する。

【0080】

このような処理を行うことで、目標位置ＴＧＰが走行可能領域ＴＡ外にある場合と比較して、生成軌道が走行可能領域ＴＡ内に設定されやすくなるので、安全な走行が可能となる。

【0081】

また、目標状態演算部２２０は、走行可能領域演算部２１０において演算された走行可能領域ＴＡの形状に応じて、目標状態量のうち、少なくとも速度に関する目標状態量に上限値を設定することもできる。例えば、高速走行ができないような、走行可能領域ＴＡが狭い場合および走行可能領域ＴＡの認知距離が短い場合において、速度に関する目標状態量に上限値を設定することが挙げられる。

【0082】

図１６は、走行可能領域ＴＡが狭いシーンを示す概念図である。図１６に示されるように、移動体１の進行方向に沿って移動体１の左右の近い距離に静止障害物ＳＯＢが存在している。走行可能領域ＴＡの最大認知距離はＬ_ｍａｘ１であり、静止障害物ＳＯＢの長さと同程度に長いが、その幅は狭い。

【0083】

このような場合は目標位置または目標方位角に関する目標状態量に上限値を設定することで、生成軌道が走行可能領域ＴＡ内に設定されやすくなるので、移動体１の乗員に精神的な負担を与えない走行が可能となる。

【0084】

図１７は、走行可能領域ＴＡの認知距離が短いシーンを示す概念図である。図１７に示されるように、移動体１の進行方向前方の走行可能領域ＴＡの認知距離Ｌ_ｍａｘ２が、図１６に示した走行可能領域ＴＡの最大認知距離Ｌ _ｍａｘ１より非常に小さく、遠くまで認知できない状態となっている。

【0085】

このようなシーンは、移動体１がトーイングトラクターで、前方に牽引対象の飛行機があるような場合、前方の近い位置に壁があるような場合、移動体１に取り付けられているカメラセンサの前方の検知距離が短い場合に想定される。

【0086】

このような場合は速度に関する目標状態量に上限値を設定することで、生成軌道が走行可能領域ＴＡ内に設定されやすくなるので、移動体１の乗員に精神的な負担を与えない走行が可能となる。

【0087】

ここで、図６のフローチャートの説明に戻る。ステップＳ１０３において目標状態量を演算した後は、状態予測部２３０において、移動体の現在の状態に基づきＮ_ｐ個のパーティクルを定義する（ステップＳ１０４）。Ｎ_ｐ個のパーティクルは、それぞれ異なる状態量を有する。Ｎ_ｐは、２以上の整数である。本実施の形態では、パーティクルの状態量Ｐは、移動体の二次元位置Ｘ_ｐおよびＹ_ｐ、方位角Θ_ｐ、速度Ｖ_ｐ、舵角δ_ｐ 、加速度ａ_ｐおよび舵角速度ｕ_ｐで構成され、以下の数式（６）で表されるものとする。

【0088】

【数6】

【0089】

ただし二次元位置Ｘ_ｐおよびＹ_ｐと、方位角Θ_ｐは移動体１の現在位置、現在方位角を基準とした図８の座標系で表すものとする。また、ｎ個目のパーティクルの状態量をＰ_ｎと表記するものとする。全てのパーティクルについて状態変数の初期値は同じ値とし、二次元位置Ｘ_ｐおよびＹ_ｐと方位角Θ_ｐの初期値は０、速度Ｖ_ｐの初期値は現在の移動体１の速度、舵角δ_ｐの初期値は現在の移動体１の舵角、加速度ａ_ｐおよび舵角速度ｕ_ｐの初期値は０とする。また、各パーティクルに対し重みＷを定義し、重みＷの初期値は全パーティクルで等しく、以下の数式（７）で表される値とし、時刻Ｔ_ｐを定義し、初期値０に設定する。

【0090】

【数7】

【0091】

なお、パーティクル数は、走行可能領域ＴＡの形状、面積に応じて可変とすることもでき、目標状態量との乖離度に応じて可変とすることもできる。

【0092】

ステップＳ１０４においてパーティクルを定義した後は、状態予測部２３０において、各パーティクルに対して一様乱数を用いたランダムなパーティクル数分の入力を与えることで、離散時間幅Ｔ_ｄ後の状態量を予測する（ステップＳ１０５）。以下に、パーティクルの状態予測の方法を説明する。

【0093】

パーティクルの状態量予測は、システムモデルを用いて行うが、本実施の形態で用いるモデルを以下に説明する。システムモデルの状態変数はパーティクルの二次元位置Ｘ_ｐおよびＹ_ｐ、方位角Θ_ｐ、速度Ｖ_ｐおよび舵角δ_ｐとし、状態量は以下の数式（８）で表されるものとする。

【0094】

【数8】

【0095】

また、システムモデルへの入力値Ｐ_ｕは、車両の加速度ａと舵角速度ｕとで構成され、以下の数式（９）で表されるものとする。

【0096】

【数9】

【0097】

また、移動体１の横滑り角βは、以下の数式（１０）で表されるものとする。

【0098】

【数10】

【0099】

ここで、システムモデルは移動体１のホイールベースＬを用いて、微分方程式として以下の数式（１１）で表される。

【0100】

【数11】

【0101】

なお、上述したシステムモデルは、四輪を二輪に近似し、力学を考慮しない運動学モデルと言えるが、四輪を二輪に近似した動力学モデルである二輪モデル等、他の車両運動モデルを用いることもできる。

【0102】

システムモデルへの入力変数のうち、加速度ａは予め設定しておいた任意の上限値ａ_ｍｘと任意の下限値ａ_ｍｎについて、以下の数式（１２）を満たす値を、パーティクルごとに一様乱数を用いて決定する。

【0103】

【数12】

【0104】

また、システムモデルへの入力変数のうち、舵角速度ｕに関しては、予め設定しておいた上限値ｕ_ｍｘ（＞０）について、以下の数式（１３）を満たすことを舵角速度ｕの第１の拘束条件とする。

【0105】

【数13】

【0106】

また、舵角に関する上限値δ_ｍｘ（＞０）について、離散時間幅Ｔ_ｄ後の舵角δ’が以下の数式（１４）を満たすことを舵角速度ｕの第２の拘束条件とする。

【0107】

【数14】

【0108】

離散時間幅Ｔ_ｄ後の舵角δ’は、以下の数式（１５）で表される。

【0109】

【数15】

【0110】

このため、第２の拘束条件は、以下の数式（１６）で表される。

【0111】

【数16】

【0112】

システムモデルへの入力値Ｐ_ｕのうち舵角速度ｕは、第１の拘束条件および第２の拘束条件を満たす値を、パーティクルごとに一様乱数を用いて決定する。

【0113】

以上のように、制約である舵角上限値δ_ｍｘに基づいて決定した入力値Ｐ_ｕを用いて、上述したシステムモデルにより離散時間幅Ｔ_ｄ後の状態量Ｐ_ｘ’を予測する。これにより、制約を考慮したパーティクルの状態予測が可能となる。

【0114】

パーティクルの状態量は予測状態量Ｐ_ｘ’と入力値Ｐ_ｕを用いて更新し、以下の数式（１７）で表されるものとする。また、時刻Ｔに離散時間幅Ｔ_ｄを足した値を更新後の時刻とする。

【0115】

【数17】

【0116】

時刻Ｔ_ｄ後の各パーティクルの予測状態量をＰ_ｎｘ’とし、離散時間幅Ｔ_ｄ後の状態予測の概念図を図１８に示す。図１８において、移動体１の前方には時刻Ｔ_ｄ後のｎ個のパーティクルの予測状態量Ｐ_ｎｘ’が示されている。このように、複数のパーティクルを用いて状態量を予測することで、各パーティクルの点と走行可能領域ＴＡの位置関係から、走行可能領域ＴＡの外か中かという評価を行うことができるので、走行可能領域ＴＡ内であることを保証できる軌道を生成できる。

【0117】

なお、状態予測部２３０での演算では、ランダムな入力を与えるために一様分布に従う一様乱数を用いて入力を決定したが、正規分布に従う正規乱数または他の分布に従うランダムな入力を用いることもできる。

【0118】

また、状態予測部２３０での演算では、予め設定しておいた任意の上限値を超えないような入力としていたが、当該入力は走行可能領域ＴＡの形状に従って可変となるような値にすることもできる。

【0119】

図１９は、走行可能領域ＴＡが狭くて長い形状に合わせて入力を設定する場合を説明する概念図である。図１９に示されるように、走行可能領域ＴＡが縦長の形状であれば、予測状態量Ｐ_ｘ’が図示されるような走行可能領域ＴＡ外とならないような舵角速度ｕの入力の上限値を設定するか、舵角速度ｕの入力の分布を狭くする。このような処理を行うことで、走行可能領域ＴＡ内に予測状態量を生成しやすくなる。

【0120】

また、走行可能領域ＴＡが狭くて長い形状の場合は、入力の数を少なくすることで、演算量を少なくすることができる。

【0121】

図２０は、走行可能領域ＴＡが横長の形状に合わせて入力を設定する場合を説明する概念図である。図２０に示されるように、走行可能領域ＴＡが横長の形状であれば、予測状態量Ｐ_ｘ’が図示されるような走行可能領域ＴＡ外とならないような加速度ａの入力の上限値を設定するか、加速度ａの入力の分布を広くするか、入力の数を多くする。このような処理を行うことで、走行可能領域ＴＡ内に予測状態量を生成しやすくなる。

【0122】

また、走行可能領域ＴＡが横長の形状の場合は、入力の数を多くすることで、より適した軌道が得られやすくなる。

【0123】

なお、状態予測部２３０は、移動体１の現在の状態量と目標状態量との乖離が大きい場合には、移動体１の運動モデルへの入力値の数を多くし、乖離度が小さい場合には、入力値の数を少なくすることもできる。図２１は、現在の状態量と目標状態量との乖離が大きい場合の入力値の設定を説明する概念図である。

【0124】

図２１に示すように、方位角の目標状態量Θ_ｔと、移動体１の現在位置での方位角Θ_ｅとの乖離が大きく、舵角を大きくしないと目標位置に到達しない場合には、運動モデルへの入力値として舵角速度を加え、舵角速度が大きくなるように軌道を予測する。

【0125】

この場合、舵角の分布を広く取る。すなわち舵角の上下限値の範囲を広くする。また、入力値の数を多くすることで、より適した軌道が得られやすくなる。これにより、移動体１の現在の状態量と目標状態量との乖離が大きい場合でも、目標位置に到達しやすくなる。一方、乖離が小さい場合には、入力値の数を少なくすることで、演算量が少なくなり演算負荷を低減できる。このように、現在の状態量と目標状態量との乖離の度合いに応じて入力値の数を可変にすることで、移動体１の滑らかな軌道生成と演算負荷の低減とを実現できる。

【0126】

ここで、図６のフローチャートの説明に戻る。ステップＳ１０５において各パーティクルの状態量を予測した後は、予測状態評価部２４０において、各パーティクルの更新後の状態量から観測値を求める（ステップＳ１０６）。観測変数は目標状態演算部２２０で演算した目標状態量に基づき定義する。本実施の形態では、目標位置での目標方位角方向に対して垂直な方向の位置である目標横位置への到達、目標速度の維持、目標方位角への到達および移動障害物からの安全な距離の保持を目標とする。これらの目標に基づき、パーティクルと目標横位置Ｙ_Ｌとの偏差Ｌ_ｅ、パーティクルの速度状態量Ｖ_ｐ、パーティクルの方位角状態量Θ_ｐ、危険領域Ｄへの進入距離Ｄ_ｐを観測変数とし、観測値Ｐ_ｙを以下の数式（１８）で表す。

【0127】

【数18】

【0128】

図２２は各観測変数を示した図である。図２２において、現在の移動体１に対して時刻Ｔ_ｄ後の予測状態量Ｐ_ｘ’を有するパーティクルＰ_ｄを模式的に示している。パーティクルＰ_ｄの位置、方位角に基づいて危険領域Ｄが設定され、危険領域Ｄには移動障害物ＭＯＢが進入している。移動障害物ＭＯＢの危険領域Ｄへの進入距離Ｄ_ｐは、パーティクルＰ_ｄの方位角Θ_ｐと平行な方向の距離で規定する。

【0129】

ここで危険領域Ｄは、パーティクルＰ_ｄの方位角Θ_ｐの向きに長辺の向きを傾けた長方形の領域であり、この領域はパーティクルＰ_ｄの前方に距離Ｄ_ｘ、左右に距離Ｄ_ｙずつの長さを有する領域となっている。ここで、距離Ｄ_ｘは、パーティクルＰ_ｄの速度Ｖと予め設定しておいた安全見込時間Ｔ_ｓを用いて、以下の数式（１９）で表される。

【0130】

【数19】

【0131】

また、距離Ｄ_ｙは、予め設定しておいたパラメータＴ_ｓｙを用いて、以下の数式（２０）で表される。

【0132】

【数20】

【0133】

ステップＳ１０６において各パーティクルの観測値を求めた後は、予測状態評価部２４０において、各パーティクルの観測値Ｐ_ｙと、理想観測値Ｐ_ｙｉとの差から、各パーティクルの重みＷを更新する（ステップＳ１０７）。ここで、理想観測値Ｐ_ｙｉとは、仮想的に設定した目標状態量にある移動体１に対する観測値であり、移動体１が目標状態量を満たしている場合に、移動体１は理想状態となる。本実施の形態において理想観測値Ｐ_ｙｉは、目標状態量に基づいて、目標横偏差との偏差Ｌ_ｅｎｏｍ、目標車速Ｖ_ｎｏｍ、目標方位角Θ_ｔｎｏｍおよび目標進入距離Ｄ_ｎｏｍで構成され、以下の数式（２１）で表される。

【0134】

【数21】

【0135】

また、パーティクルの位置が、ステップＳ１０２で演算した走行可能領域ＴＡ外である場合は、各パーティクルの重みを０とする、あるいは走行可能領域ＴＡ内にあるパーティクルよりも低い値とする。走行可能領域ＴＡの内外判定は、例えば、走行可能領域ＴＡの各頂点と移動体１とを結ぶ多角形領域内に、パーティクルの二次元位置Ｘ_ｐおよびＹ_ｐがあるか否かで判定を行う。

【0136】

ここで、上記では走行可能領域ＴＡ外のパーティクルは重みを０としているが、走行可能領域ＴＡ外のパーティクルについて、走行可能領域ＴＡからの逸脱度に応じて、パーティクルに付与する重みを可変とすることもできる。

【0137】

図２３は、走行可能領域ＴＡ外のパーティクルに付与する重みを可変とする処理の概念図である。図２３において、移動体１の前方の走行可能領域ＴＡ内にあるパーティクルＰＷ_４は、重みＷ_４が付与され、走行可能領域ＴＡは重みＷ_４付与領域ＲＷ_４と呼称することができる。

【0138】

また、重みＷ_４付与領域ＲＷ_４の外側には、重みＷ_３付与領域ＲＷ_３が設定され、そこにあるパーティクルＰＷ_３には、重みＷ_３が付与される。また、重みＷ_３付与領域ＲＷ_３のさらに外側には、重みＷ_２付与領域ＲＷ_２が設定され、そこにパーティクルがある場合には、重みＷ_２が付与される。また、重みＷ_２付与領域ＲＷ_２のさらに外側には、重みＷ_１付与領域ＲＷ_１が設定され、そこにあるパーティクルＰＷ_１には、重みＷ_１が付与される。なお、重みはＷ_４が最も重く、重みＷ_３、Ｗ_２、Ｗ_１の順に軽くなる。

【0139】

このように、走行可能領域ＴＡの外側のパーティクル（予測点）を削除せず、走行可能領域ＴＡからの距離に応じて予測状態量の評価を行うことで、外界センサの認知可能範囲の限界等により、実際には走行可能領域ＴＡであっても、外界センサ上は走行可能領域ＴＡ外として認識されている領域についても、軌道候補として残すことができ、残った軌道候補に対して、走行可能領域ＴＡからの距離、すなわち信頼度に応じた予測点の評価ができ、軌道計画が成功しやすくなる。また、走行可能領域ＴＡから離れたパーティクルほど付与する重みを軽くすることで、当該パーティクルに基づいて軌道が生成される可能性を低減でき、生成された軌道の安全性を高めることができる。

【0140】

また、図２３ではパーティクルに付与する重みは不連続な値となっているが、走行可能領域ＴＡからの距離に応じて付与する重みが連続的に変化するような値とすることもできる。

【0141】

また、走行可能領域ＴＡ内のパーティクルについては、走行可能領域ＴＡを規定する境界からの距離に応じて、パーティクルに付与する重みを可変とすることもできる。

【0142】

図２４は、走行可能領域ＴＡ内のパーティクルに付与する重みを可変とする処理の概念図である。図２４において、移動体１の前方の走行可能領域ＴＡ内には、走行可能領域ＴＡを規定する境界側から内側に向けて、順に、重みＷ_１付与領域ＲＷ_１、重みＷ_２付与領域ＲＷ_２、重みＷ_３付与領域ＲＷ_３および重みＷ_４付与領域ＲＷ_４が設定されている。各付与領域での処理は図２３を用いて説明した処理と同じであり、重みはＷ_４が最も重く、重みＷ_３、Ｗ_２、Ｗ_１の順に軽くなる。

【0143】

このように、走行可能領域ＴＡ内の予測状態量において、走行可能領域ＴＡを規定する境界に近づくにつれて、小さくなるような重みの付与を行うことで、走行可能領域ＴＡの境界付近よりも走行可能領域ＴＡの中央に位置する予測状態量の重みが高くなり、走行可能領域ＴＡの境界をなるべく避けるような軌道計画ができ、生成された軌道の安全性を高めることができる。

【0144】

また、図２４ではパーティクルに付与する重みは不連続な値となっているが、走行可能領域ＴＡを規定する境界からの距離に応じて付与する重みが連続的に変化するような値とすることもできる。

【0145】

また、走行可能領域ＴＡ内のパーティクルについては、走行可能領域ＴＡ内であっても、進行すると行き止まりとなりうる地点に予測されたパーティクルは、重みを０または小さな重みを付与するように処理することもできる。図２５は当該処理を説明する概念図である。

【0146】

図２５においては、移動体１の前方にＥＴＣゲートなどの狭い通路が存在するシーンを示しており、目標位置ＴＧＰは通過可能なゲート内となっている。他のゲートは走行不可能であり、行き止まりＤＥとなっており進行すると移動体１が将来走行できない領域となっている。この領域に予測されたパーティクルＰＷは、重みが０（Ｗ＝０）となっている。

【0147】

このように、走行可能領域ＴＡ内であっても、目標位置ＴＧＰに到達する前に行き止まりで阻まれ、身動きが取れなくなることを回避する軌道を生成することができる。

【0148】

なお、行き止まりとなりうるかの判定は、例えば、図２５中の目標位置ＴＧＰからのｘ方向の偏差が小さいかつ、ｙ方向の偏差が大きいパーティクル、すなわち目標位置ＴＧＰから水平方向（ｙ方向）にずれているパーティクルであれば、目標位置の手前の障害物に阻まれていることになる。よって、目標位置ＴＧＰからのｘ方向の偏差が小さいかつ、ｙ方向の偏差が大きいか否かで行き止まりとなりうる地点に予測されたパーティクルか否かを判定することができる。

【0149】

また、図１１を用いて説明したように、行き止まりの箇所を予め走行可能領域ＴＡから除外し、行き止まりとなりうる地点に予測されたパーティクルは走行可能領域ＴＡ外にあるものとして、重みＷを付与することもできる。

【0150】

また、走行可能領域内のパーティクルについては、走行可能領域内に跨ぎ禁止線がある場合、１ステップ前に軌道点演算部２５０で生成された軌道点、または移動体１に対して、跨ぎ禁止線を跨ぐような位置に、ステップＳ１０５で演算したパーティクルがある場合、そのパーティクルの重みＷを０とするか、または小さな重みを付与することもできる。図２６は当該処理を説明する概念図である。

【0151】

図２６においては、移動体１の進行方向の左右に跨ぎ禁止線ＮＳＬが設けられているシーンを示しており、左右の跨ぎ禁止線ＮＳＬで規定される走行車線内に複数の軌道点ＴＰで構成される生成軌道ＧＴが設定されているが、現在、予測状態評価部２４０において評価中のパーティクルの幾つかは、１ステップ前に軌道点演算部２５０で生成された軌道点ＴＰに対して跨ぎ禁止線ＮＳＬを跨ぐような位置に予測されたパーティクルＰＷとなっており、重みが０（Ｗ＝０）となっている。

【0152】

このように、軌道点ＴＰに対して跨ぎ禁止線ＮＳＬを跨ぐような位置に予測されたパーティクルの重みを０とすることで、移動体１が将来走行できない領域に軌道が設定されることを防止でき、生成された軌道の安全性を高めることができる。

【0153】

また、図１３を用いて説明したように、走行可能領域ＴＡと跨ぎ禁止線ＮＳＬとで囲まれた領域は、除去領域ＡＲとして走行可能領域ＴＡから除外し、その地点に予測されたパーティクルは走行可能領域ＴＡ外にあるものとして、重みＷを付与することもできる。

【0154】

また、現在認知できている走行可能領域外のパーティクルについては、予測した走行可能領域がある場合、予測走行可能領域に、当該パーティクルが位置していれば、そのパーティクルの重みＷは０としないように処理することもでき、現在認知できている走行可能領域内のパーティクルの重みよりも小さな重みを付与することもできる。図２７は予測走行可能領域内にあるパーティクルの重み付け処理を説明する概念図である。

【0155】

図２７においては、現在認知できている走行可能領域ＴＡの先に予測走行可能領域ＥＴＡが設けられており、予測走行可能領域ＥＴＡ内にあるパーティクルＰＷ_１には、重みＷ_１が付与される。また、走行可能領域ＴＡ内にあるパーティクルＰＷ_２には、重みＷ_２が付与される。ここで、重みＷ_１は重みＷ_２よりも小さい（Ｗ_１＜Ｗ_２）。

【0156】

予測走行可能領域ＥＴＡを用いることで、現在認知できている外界センサによる走行可能領域ＴＡよりも遠くの位置まで軌道を生成することができる。また、予測走行可能領域ＥＴＡの信頼度は高くないので、現在認知できている外界センサによる走行可能領域ＴＡ内の予測状態量の重みを相対的に大きくすることで、信頼度の高い軌道を生成できる。

【0157】

ここで、ステップＳ１０５において離散時間幅Ｔ_ｄ後の状態量を予測したパーティクルについて、周辺環境取得部１３０から得られる移動障害物の予測軌道を用い、各パーティクル周辺の領域に、同時刻、すなわちパーティクルの予測時間と同じ時間に予測軌道を用いることで得られた予測障害物が存在するかを判断し、存在する場合はそのパーティクルの重みＷを０とする処理を行うこともできる。

【0158】

各パーティクルの更新前の重みＷをＷ_ｐとして再定義する。重みＷは、更新前の重みと尤度ＬＬＶとに比例し、以下の数式（２２）を用いて、全パーティクルの重みの積算値が１となるよう更新する。

【0159】

【数22】

【0160】

ここで尤度ＬＬＶは、予め設定しておいたパーティクルの状態量Ｐ_ｘに関する共分散行列Ｑと観測値Ｐ_ｙに関する共分散行列Ｒを用いて、以下の数式（２３）で求められる。

【0161】

【数23】

【0162】

ここで、行列Πは以下の数式（２４）で表される。

【0163】

【数24】

【0164】

ただし、ｎ個目のパーティクルにおける測定行列Ｈの値Ｈ_ｎは、Ｐ_ｘ＝Ｐ_ｘｎとした場合、測定関数ｈを状態量Ｐ_ｘで微分した値とし、以下の数式（２５）で表される。

【0165】

【数25】

【0166】

また、測定関数ｈは状態量Ｐ_ｘから観測値Ｐ_ｙを求める関数であり、以下の数式（２６）で表される。

【0167】

【数26】

【0168】

各パーティクルの観測値と、理想観測値との差に基づく、各パーティクルの重み更新について、移動体１と目標状態量との差、または適用シーンに基づいて、どの項目をどの程度重点的に評価するかという、評価の重みを設定することもできる。図２８は評価の重みの設定処理を説明する概念図である。

【0169】

図２８においては、移動体１が目標横位置Ｙ_Ｌから離れた状態にあり、目標横位置Ｙ_Ｌとの偏差を縮めることが重要視される領域ＩＡ１では、重み更新時に目標横位置Ｙ_Ｌへの偏差が小さいパーティクルの尤度が大きくなるように評価の重みを設定する。

【0170】

このような設定により、例えば、料金所ゲート通過シーンにおいて、目標位置が通過したいゲートであるとき、ゲートに対して垂直で通過する必要があるシーンでは、縦方向の位置に関する目標状態量との差よりも、横方向の位置に関する目標状態量との差を重視することで、縦方向の位置が合うよりも先に、横方向の位置を合わせることができ、ゲートに対して垂直で進入できる位置に早めに到達できるような軌道を生成できる。

【0171】

また、図２８において、移動体１の方位角Θ_ｅと目標状態量ＴＧでの目標方位角Θ_ｔとの偏差が大きい状態にあり、目標方位角Θ_ｔとの偏差を縮めることが重要視される領域ＩＡ２では、重み更新時に目標方位角Θ_ｔとの偏差が小さいパーティクルの尤度が大きくなるように評価の重みを設定する。評価の重みの設定は偏差が小さいパーティクルほど重み付け係数を大きくすることで設定される。

【0172】

このような設定により、例えば、料金所ゲート通過シーンにおいて、目標位置が通過したいゲートであるとき、目標位置から距離が遠い地点においては方位角に関する状態量の評価の重みを低くして、距離が近い地点においては方位角に関する状態量の評価の重みを大きくすることで、目標位置から離れた地点においては軌道の方位角に関する状態量の自由度が高く、目標位置に近づくにつれて方位角に関する状態量を合わせて行くような軌道を生成できる。

【0173】

ここで、図６のフローチャートの説明に戻る。ステップＳ１０７において各パーティクルの重みを更新した後は、予測状態評価部２４０において、各パーティクルの重みに基づき、パーティクルのリサンプリングを行う（ステップＳ１０８）。ただし、パーティクル数の大幅な減少を防ぐため、有効パーティクル数Ｎ _ｅｆｆ が閾値Ｎｔｈｒ以上となる場合にのみリサンプリングを行い、それ以外の場合はこのステップでは何も行わない。

【0174】

ここで、有効パーティクル数Ｎ _ｅｆｆ は以下の数式（２７）で表される。

【0175】

【数27】

【0176】

リサンプリングでは、通常のパーティクルフィルタ同様、経験分布関数から等間隔にサンプリングを行う。リサンプリングを行った場合、以下の数式（２８）に基づいて、各パーティクルの重みは同等として重みのリセットを行う。

【0177】

【数28】

【0178】

ステップＳ１０８においてパーティクルのリサンプリングを行った後は、軌道点演算部２５０において、パーティクルの位置と速度について、予測状態評価部２４０で演算した重みに基づいて、軌道生成部２６０において加重平均値を演算し、その加重平均した、少なくとも位置データと速度データで構成される点を軌道点として、例えば軌道生成部２６０内のメモリ（図示せず）に記憶する（ステップＳ１０９）。

【0179】

また、軌道点は、予測状態評価部２４０で演算した重みに基づいて、重みの最も大きい、すなわち重み付け係数の最も大きいパーティクルを軌道点とすることもできる。

【0180】

また、軌道点は、位置データおよび速度データに限らず、方位角データ、舵角データ等を含めて構成することもできる。

【0181】

また、軌道点は位置データのみの点を軌道点とすることもできる。

【0182】

ステップＳ１０９において軌道点を記憶した後は、時刻Ｔが計画ホライズンＴｔｈｒに達したかどうかを判断する（ステップＳ１１０）。時刻が計画ホライズン未満の場合（Ｎｏの場合）はステップＳ１０４以下の処理を繰り返す。時刻Ｔが計画ホライズンＴｔｈｒ以上である場合（Ｙｅｓの場合）は、軌道生成部２６０が軌道点として記憶していた、各離散時間ごとの位置データと速度データで構成される軌道点の点列を生成軌道として運動制御部３００に出力する。

【0183】

ステップＳ１０４からステップＳ１１０までの演算を時刻が計画ホライズンＴｔｈｒ以上となるまで繰り返して複数の軌道点を得る処理の概念図を図２９に示す。図２９においては、パーティクル数を４個とし、また、図中のパーティクルはリサンプリング後の状態を表している。

【0184】

図２９に示されるように、走行可能領域ＴＡ内に、時刻Ｔ＝Ｔ_１のパーティクル群Ｇ１、時刻Ｔ＝Ｔ_２のパーティクル群Ｇ２および時刻Ｔ＝Ｔ_３のパーティクル群Ｇ３があり、時刻Ｔ＝Ｔ_３では計画ホライズンＴｔｈｒを超えている。パーティクル群Ｇ１～Ｇ３のそれぞれにおける、４個のパーティクルの加重平均された状態量が軌道点ＴＰ１、ＴＰ２およびＴＰ３となっている。これらの軌道点の点列が生成軌道となるので、予測状態評価部２４０による評価に基づいた尤もらしい軌道点を生成できる。

【0185】

また、パーティクル群Ｇ１～Ｇ３のそれぞれにおける重みの最も大きい、すなわち重み付け係数の最も大きいパーティクルを軌道点とすることでも、上記と同様の効果が得られる。

【0186】

以上説明した、本実施の形態の軌道計画装置２００を用いて目標状態量に到達するまでの軌道生成の一例を、図３０および図３１を用いて説明する。図３０および図３１は軌道生成の一例を示す模式図であり、図３０の左図には目標状態量ＴＧから遠い位置に移動体１が存在するときの生成軌道ＧＴ１を模式的に示している。図３０の右図には、左図の生成軌道ＧＴ１を用いて、生成軌道ＧＴ１に沿って前方に進み、左図のときよりも少し前進した位置での生成軌道ＧＴ２を示している。図３１の左図には、図３０の右図の生成軌道ＧＴ２を用いて、生成軌道ＧＴ２に沿って前方に進み、図３０の右図のときよりも少し前進した位置での生成軌道ＧＴ３を示している。図３１の右図には、左図の生成軌道ＧＴ３を用いて、生成軌道ＧＴ３に沿って前方に進み、目標状態量ＴＧに到達した後の生成軌道ＧＴ４を示している。

【0187】

以上説明したように実施の形態１の軌道計画装置２００によれば、移動体１の位置の状態量を含む目標状態量と走行可能領域とに基づいて、目標状態量までの軌道を評価し、評価結果に基づいて軌道を生成するため、走行可能領域が複雑な場合においても、走行可能領域を通って目的地へ到達することができる。

【0188】

＜実施の形態２＞
図３２は、実施の形態２の軌道計画装置を搭載した移動体１の概略構成の一例を示すブロック図である。なお、図３２においては、図１を用いて説明した移動体１と同一の構成については同一の符号を付し、重複する説明は省略する。

【0189】

図３２に示す、移動体１においては、移動体１の通るべき軌道を生成する軌道計画装置２００Ａの構成が、実施の形態１の軌道計画装置２００とは異なっている。すなわち、実施の形態２の軌道計画装置２００Ａは、状態推定にパーティクルフィルタを用いる代わりに、移動体１の現在の位置と目標位置とを通る多項式を状態予測部２３０で演算することによって軌道を生成するため、軌道点演算部２５０を有していない点で軌道計画装置２００とは異なっている。

【0190】

移動体１と目標状態量ＴＧを結ぶ多項式の軌道を演算する方法について、以下では目標状態量ＴＧを目標位置（ｘ_ｇ、ｙ_ｇ）として説明する。図３３は、移動体１と目標位置ＴＧを結ぶ多項式の導出方法を説明する概念図である。

【0191】

図３３のように移動体１の位置を原点、移動体１の向きをｘ軸、移動体の向きに垂直な方向をｙ軸とした場合、移動体１と目標位置ＴＧを結ぶ多項式ｙ＝ｆ（ｘ）は以下の数式（２９）となる。

【0192】

【数29】

【0193】

そして、当該多項式は、移動体１の位置（原点）と目標位置ＴＧを通り、移動体１の位置地点では方向がｘ軸（角度が０度）、また目標位置地点では所定の方向（角度）を向いていなければならないという制約条件を付ける。

【0194】

それを数式で表すと以下の境界条件、すなわち移動体位置地点と目標位置地点の条件の連立方程式を解くことで、各係数ｃ_ｉを求めることができる。当該連立方程式は、以下の数式（３０）および（３１）で表される。ここで、ｘ_０は移動体位置のｘ座標値、ｘ_ｇは目標位置のｘ座標値である。

【0195】

【数30】

【0196】

【数31】

【0197】

次に、多項式を以下の数式（３２）とする場合について説明する。

【0198】

【数32】

【0199】

例えば、移動体位置地点での関数値（ｆ（ｘ_０））が移動体１のｙ座標値であり、移動体位置地点での傾きを表す関数値（ｆ’（ｘ_０））が移動体１の方位角であり、移動体位置地点での曲率を表す関数値（ｆ’’（ｘ _０ ））が０の境界条件を与えてやると、係数Ｃ_０、Ｃ_１、Ｃ_２が一意的に求まり残りのｘ^３の項はランダムに与えることで、図３４に示されるようにランダムの項数分の多項式の軌道ができる。

【0200】

図３４には、複数の障害物ＯＢがある場合に、複数の障害物ＯＢを避けて移動体１と目標位置ＴＧとを結ぶ多項式軌道を得るための３パターンの多項式軌道ＰＴ１、ＰＴ２およびＰＴ３が示されている。

【0201】

多項式軌道ＰＴ１、ＰＴ２およびＰＴ３を与える多項式は、それぞれ以下の数式（３３）、（３４）および（３５）で表される。

【0202】

【数33】

【0203】

【数34】

【0204】

【数35】

【0205】

上記複数の多項式軌道から目標位置ＴＧへの到達値および多項式軌道が走行可能領域ＴＡの境界から離れており、走行可能領域ＴＡ内にあるか等の評価を行い、最も評価が高い多項式軌道を生成軌道とする。図３４を例に採ると、多項式軌道ＰＴ３が生成軌道となる。

【0206】

また、多項式を上述した数式（３２）とする場合について、以下の方法によっても多項式軌道を得ることができる。

【0207】

例えば、移動体位置地点での関数値（ｆ（ｘ_０））が移動体１のｙ座標値であり、移動体位置地点での傾きを表す関数値（ｆ’（ｘ_０））が移動体１の方位角であり、移動体位置地点での関数値（ｆ（ｘ_ｇ））が目標位置ＴＧのｙ座標値であり、移動体位置地点での関数値（ｆ’（ｘ_ｇ））が目標位置ＴＧの方位角であるとの境界条件を与えてやると、係数Ｃ_３、Ｃ_２、Ｃ_１およびＣ_０を一意的に求めることができる。求められた係数Ｃ_３、Ｃ_２、Ｃ_１およびＣ_０の値の周辺で各係数をランダムに変えることで、図３５のように複数の多項式の軌道ができる。

【0208】

図３５は、複数の障害物ＯＢがある場合に、複数の障害物ＯＢを避けて移動体１と目標位置ＴＧとを結ぶ多項式軌道を得るための３パターンの多項式軌道ＰＴ１、ＰＴ２およびＰＴ３が示されている。

【0209】

多項式軌道ＰＴ１、ＰＴ２およびＰＴ３を与える多項式は、それぞれ以下の数式（３６）、（３７）および（３８）で表される。

【0210】

【数36】

【0211】

【数37】

【0212】

【数38】

【0213】

上記複数の多項式軌道から目標位置ＴＧへの到達値および多項式軌道が走行可能領域ＴＡの境界から離れており、走行可能領域ＴＡ内にあるか等の評価を行い、最も評価が高い多項式軌道を生成軌道とする。図３５を例に採ると、多項式軌道ＰＴ１が生成軌道となる。

【0214】

このように実施の形態２の軌道計画装置２００Ａは、状態推定にパーティクルフィルタを用いる代わりに、移動体１の現在の位置と目標位置とを通る多項式を演算することで生成軌道を得るので、軌道点は演算せず軌道点演算部２５０を有していない。

【0215】

ここで、多項式を演算することで生成軌道を得る方法を採る場合の利点として、演算負荷が低いことが挙げられる。すなわち、上記で説明した連立方程式を解くだけで１つの候補軌道を演算することができるが、パーティクルフィルタを用いる場合は、多数のパーティクルの状態遷移を生成したい軌道の長さ分だけ演算を繰り返す必要があるため、演算負荷が大きい。

【0216】

一方、パーティクルフィルタを用いて生成軌道を得る方法を採る場合の利点としては、走行可能領域ＴＡ内であることを保証する軌道が生成できることが挙げられる。すなわち、パーティクルフィルタは多数の予測点（予測状態量）を使って、１つの軌道点を求める手法であるが、各パーティクルの点と走行可能領域ＴＡの位置関係から、軌道点が走行可能領域ＴＡの外か内かという評価を行うことができるため、走行可能領域ＴＡ内であることを保証できる軌道を生成することができる。一方で多項式を演算することで生成軌道を得る方法では、移動体１と目標位置ＴＧを結ぶ軌道を生成できるものの、「走行可能領域ＴＡ内である」こと、および「障害物を回避する」ことを多項式の数式に入れることができない、または考慮できないので生成した軌道が走行可能領域ＴＡ外となる可能性がある。

【0217】

また、パーティクルフィルタを用いて生成軌道を得る方法を採る場合の利点としては、軌道の探索範囲が広いことが挙げられる。すなわち、多項式を演算することで生成軌道を得る方法では、多項式で表現できる軌道のみしか得ることができないため、小さな探索範囲となるが、パーティクルフィルタを用いる場合は、状態の予測点をベースとする、すなわち点により表現するので探索範囲が広くなる。また、非線形な軌道も得ることができ、多項式で表現できない軌道も生成できる。

【0218】

＜重み付けの変形例＞
以上説明した実施の形態１の予測状態評価部２４０は、各予測状態量、すなわち各パーティクルに重み付けをすることで重み付けされた軌道候補とする。このとき、予測状態量が、現在の移動体１の状態量と大きく乖離している場合、または、前回演算された軌道点の状態量と大きく乖離している場合、このような予測状態量に基づいて演算した軌道点を用いて軌道を生成すると、軌道が急変して移動体１の乗り心地が悪くなる。

【0219】

そこで、予測状態評価部２４０において、予測状態量が、現在の移動体１の状態量と大きく乖離している場合、および、前回演算された軌道点の状態量と大きく乖離している場合は、重み付け係数を小さくすることで、急変するような軌道が生成されることを抑制し、移動体１の乗り心地を改善できる。

【0220】

図３６には、予測状態量が、現在の移動体１の状態量と大きく乖離している場合の重み付けの概念図を示す。図３６に示されるように、現在の移動体１の状態量（ｘ_ｅ,ｙ_ｅ,θ_ｅ,ｖ_ｅ ）に対して大きく乖離しているパーティクル群ＧＳと、現在の移動体１の状態量に対して乖離がそれほど大きくないパーティクル群ＧＭが存在する場合、予測状態評価部２４０は、パーティクル群ＧＳのパーティクルの重み付け係数は小さくする。一方、予測状態評価部２４０は、パーティクル群ＧＭのパーティクルの重み付け係数は小さくしない。

【0221】

ここで、現在の移動体１の状態量と予測状態量（パーティクル）との乖離の程度は、両状態量の差が予め定めた閾値に基づいて判断することができ、閾値よりも大きければ、乖離が大きいとし、閾値以下であれば乖離が小さいと判断することができる。

【0222】

また、パーティクルに対する重み付けは、現在の移動体１の状態量と予測状態量（パーティクル）との差の絶対値に基づいて重み付け係数を変えるようにすることもできる。例えば、両状態量の差の絶対値に基づいて段階的に変化するような重み付け係数を設定しておき、両状態量の差が大きくなれば重み付け係数を段階的に小さくし、両状態量の差が小さくなれば重み付け係数を段階的に大きくすることもできる。

【0223】

図３７には、予測状態量が、前回演算された軌道点の状態量と大きく乖離している場合の重み付けの概念図を示す。図３７に示されるように、前回演算された軌道点ＴＰ２の状態量に対して大きく乖離しているパーティクル群ＧＳと、前回演算された軌道点ＴＰ２の状態量に対して乖離がそれほど大きくないパーティクル群ＧＭが存在する場合、予測状態評価部２４０は、パーティクル群ＧＳのパーティクルの重み付け係数は小さくする。一方、予測状態評価部２４０は、パーティクル群ＧＭのパーティクルの重み付け係数は小さくしない。

【0224】

ここで、前回演算された軌道点ＴＰ２の状態量と予測状態量（パーティクル）との乖離の程度は、両状態量の差が予め定めた閾値に基づいて判断することができ、閾値よりも大きければ、乖離が大きいとし、閾値以下であれば乖離が小さいと判断することができる。

【0225】

また、パーティクルに対する重み付けは、前回演算された軌道点ＴＰ２の状態量と予測状態量（パーティクル）との差の絶対値に基づいて重み付け係数を変えるようにすることもできる。例えば、両状態量の差の絶対値に基づいて段階的に変化するような重み付け係数を設定しておき、両状態量の差が大きくなれば重み付け係数を段階的に小さくし、両状態量の差が小さくなれば重み付け係数を段階的に大きくすることもできる。

【0226】

なお、以上説明した実施の形態１および２に係る軌道計画装置２００および２００Ａの各構成要素は、コンピュータを用いて構成することができ、コンピュータがプログラムを実行することで実現される。すなわち、軌道計画装置２００および２００Ａは、例えば図３８に示す処理回路５０により実現される。処理回路５０には、ＣＰＵ、ＤＳＰ（Digital Signal Processor）などのプロセッサが適用され、記憶装置に格納されるプログラムを実行することで各部の機能が実現される。

【0227】

なお、処理回路５０には、専用のハードウェアが適用されても良い。処理回路５０が専用のハードウェアである場合、処理回路５０は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field-Programmable Gate Array）、またはこれらを組み合わせたもの等が該当する。

【0228】

軌道計画装置２００および２００Ａは、構成要素の各々の機能が個別の処理回路で実現されても良いし、それらの機能がまとめて１つの処理回路で実現されても良い。

【0229】

また、図３９には、処理回路５０がプロセッサを用いて構成されている場合におけるハードウェア構成を示している。この場合、軌道計画装置２００および２００Ａの各部の機能は、ソフトウェア等（ソフトウェア、ファームウェア、またはソフトウェアとファームウェア）との組み合わせにより実現される。ソフトウェア等はプログラムとして記述され、メモリ５２に格納される。処理回路５０として機能するプロセッサ５１は、メモリ５２（記憶装置）に記憶されたプログラムを読み出して実行することにより、各部の機能を実現する。すなわち、このプログラムは、軌道計画装置２００および２００Ａの構成要素の動作の手順および方法をコンピュータに実行させるものであると言える。

【0230】

ここで、メモリ５２は、例えば、ＲＡＭ、ＲＯＭ、フラッシュメモリー、ＥＰＲＯＭ（Erasable Programmable Read Only Memory）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read Only Memory）等の、不揮発性または揮発性の半導体メモリ、ＨＤＤ（Hard Disk Drive）、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Digital Versatile Disc）およびそのドライブ装置等、または、今後使用されるあらゆる記憶媒体であっても良い。

【0231】

以上、軌道計画装置２００および２００Ａの各構成要素の機能が、ハードウェアおよびソフトウェア等のいずれか一方で実現される構成について説明した。しかしこれに限ったものではなく、軌道計画装置２００および２００Ａの一部の構成要素を専用のハードウェアで実現し、別の一部の構成要素をソフトウェア等で実現する構成であっても良い。例えば、一部の構成要素については専用のハードウェアとしての処理回路５０でその機能を実現し、他の一部の構成要素についてはプロセッサ５１としての処理回路５０がメモリ５２に格納されたプログラムを読み出して実行することによってその機能を実現することが可能である。

【0232】

以上のように、軌道計画装置２００および２００Ａは、ハードウェア、ソフトウェア等、またはこれらの組み合わせによって、上述の各機能を実現することができる。

【0233】

本開示は詳細に説明されたが、上記した説明は、全ての局面において、例示であって、本開示がそれに限定されるものではない。例示されていない無数の変形例が、本開示の範囲から外れることなく想定されうるものと解される。

【0234】

なお、本開示は、その開示の範囲内において、各実施の形態を自由に組み合わせたり、各実施の形態を適宜、変形、省略することが可能である。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】

【図22】

【図23】

【図24】

【図25】

【図26】

【図27】

【図28】

【図29】

【図30】

【図31】

【図32】

【図33】

【図34】

【図35】

【図36】

【図37】

【図38】

【図39】