JP7275406B1

JP7275406B1 - 通過地点生成装置

Info

Publication number: JP7275406B1
Application number: JP2022557725A
Authority: JP
Inventors: 裕基吉田
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2022-05-11
Filing date: 2022-05-11
Publication date: 2023-05-17
Anticipated expiration: 2042-05-11
Also published as: JPWO2023218560A1; WO2023218560A1

Abstract

本開示は通過地点生成装置に関し、移動体の到達すべき通過地点を生成する通過地点生成装置であって、前記移動体の周辺環境情報と前記移動体の状態量に基づいて、少なくとも位置の状態量を含む複数の目標状態候補を演算する目標状態候補生成部と、前記複数の目標状態候補の周辺までの中間地点において、いずれの目標状態候補においても到達可能な局所的な目標状態である局所目標状態を演算し、局所目標状態を通過地点として出力する局所目標状態生成部と、を備える。

Description

本開示は、車両等の自動運転を実現するための通過地点を生成する通過地点生成装置に関する。

近年、自動運転の自動車および自律搬送台車などの自律移動体の開発が進んでいる。これらの自律移動体は決められた目標位置を含む目標状態へ到達するように走行制御される。しかし場合によっては、外界センサ等によって得られる情報によって、目標位置が通過不可であることが目標位置に接近して初めて認知されることがある。例えば、料金所ゲートを通過するシーンおいて、対象車が料金所ゲートから充分な距離がある段階で目標の料金所ゲートを選定した場合、当初目標としていた料金所ゲートに接近して初めて、その料金所ゲートが稼働していないこと、または料金所内に故障車が止まっていたということが分かるシーンが考えられる。このようなシーンでは目標位置を変更することが必要になる。例えば、特許文献１に開示されているように、センサ等で得られる動的情報に応じて、選択したゲートを修正することにより、より好適なゲートの通過を実現する技術が開発されている。

国際公開第２０１８／１４２５６１号

特許文献１においては、静的情報に基づいて複数のゲートの中から第１のゲートを選択し、その後、動的情報に基づいて、第１のゲートの選択結果を第２のゲートへと修正し、ゲート選択結果に修正処理を加えることで、より好適なゲートを選択する技術およびゲートの通過確率が上がるような車両制御システムを提案している。

しかし、第１のゲートを目指して走行している途中で第１のゲートから大きく離れたゲートに修正された場合、または第１のゲート到達直前に他のゲートへと修正された場合、車両は軌道修正に急なハンドル制御が必要になったり、修正後のゲートへ到達できる軌道が見つからないという課題が発生する。

本開示は上記のような問題を解決するためになされたものであり、目標状態の変更が余儀なくされた状況においても、急な軌道修正が発生せず、変更後の目標状態に到達できる通過地点生成装置を提供することを目的とする。

本開示に係る通過地点生成装置は、移動体の到達すべき通過地点を生成する通過地点生成装置であって、前記移動体の周辺環境情報と前記移動体の状態量に基づいて、複数の走行可能位置から目標位置候補を複数選定し、選定した前記目標位置候補を含む複数の目標状態候補を演算する目標状態候補生成部と、前記複数の目標状態候補の周辺までの中間地点において、いずれの目標状態候補においても到達可能な局所的な目標状態である局所目標状態を演算し、前記局所目標状態を前記通過地点として出力する局所目標状態生成部と、を備える、通過地点生成装置。

本開示に係る通過地点生成装置によれば、目標状態の変更に備えて、複数の目標状態候補の周辺までの中間地点において、いずれの目標状態候補においても到達可能な目標状態である局所目標状態を演算し、局所目標状態を通過地点とすることで、目標状態の変更が発生した場合でも、急な軌道修正をすることなく対応でき、変更後の目標状態に到達できる。

本開示に係る実施の形態１の通過地点生成装置を搭載した移動体の構成を示すブロック図である。料金所の走行車線に移動体が進入しようとする状態を模式的に示す図である。本開示に係る実施の形態１の通過地点生成装置の動作の一例を示すフローチャートである。目標状態候補集合を生成する処理を示す概念図である。目標状態候補を説明する概念図である。到達可能境界を説明する概念図である。目標状態候補生成部の変形例の構成を示すブロック図である。走行可能状態までの仮想的な軌道を生成する処理を説明する概念図である。目標状態候補演算部の変形例での処理を説明する概念図である。局所目標状態生成部での局所目標状態を演算する処理を説明する概念図である。局所目標状態生成部での局所目標状態を演算する処理を説明する概念図である。局所目標状態生成部での局所目標状態を演算する処理を説明する概念図である。局所目標状態を領域とした場合の概念図である。局所目標状態を確率分布とした場合の概念図である。目標状態候補に重みを付けた場合の局所目標状態を演算する処理を説明する概念図である。仮想的な軌道に重みを付けた場合の局所目標状態を演算する処理を説明する概念図である。軌道生成部において移動体の目標軌道を生成する処理を説明する概念図である。軌道生成部において目標状態候補の中から目標軌道を生成する処理を説明する概念図である。急な局所目標状態の変更の例を示す概念図である。急な局所目標状態の変更の例を示す概念図である。急な局所目標状態の変更に制限をかける処理を説明する概念図である。本開示に係る実施の形態２の通過地点生成装置を搭載した移動体の構成を示すブロック図である。大域目標状態生成部での大域的目標状態を生成する処理を説明する概念図である。目標状態候補演算部での処理を説明する概念図である。目標状態候補演算部の変形例での処理を説明する概念図である。本開示に係る実施の形態３の通過地点生成装置を搭載した管制サーバーの構成を示すブロック図である。管制サーバーと移動体との間の通信により局所的な目標状態を管制サーバーから移動体に送信されるシステムを示す概念図である。実施の形態１～３の通過地点生成装置を実現するハードウェア構成を示す図である。実施の形態１～３の通過地点生成装置を実現するハードウェア構成を示す図である。

＜実施の形態１＞
図１は、本開示に係る実施の形態１の通過地点生成装置を搭載した移動体１の構成を示すブロック図である。

図１に示す移動体１は、移動体１の走行すべき周辺環境情報および目的地、移動体１の自己状態を取得する自律センサ情報取得部１００と、自律センサ情報取得部１００から得られる情報に基づき、移動体１の到達すべき通過地点を生成する通過地点生成装置２００と、通過地点生成装置２００によって生成された局所目標状態に基づいて、局所目標状態に到達するように移動体１の運動を制御する運動制御部３００とを備えている。なお、自律センサ情報取得部１００で得られる情報は、前処理部１０を介して通過地点生成装置２００に入力されるが、前処理部１０の機能については後に説明する。また、通過地点生成装置２００によって生成された局所目標状態は、後処理部２０を介して運動制御部３００に入力されるが、後処理部２０の機能については後に説明する。

自律センサ情報取得部１００は、周辺環境情報取得部１１０、および自己状態取得部１２０を有している。

周辺環境情報取得部１１０は、移動体周辺の壁、他の移動体等の移動障害物の相対的な位置および速度、方位角、障害物のない走行可能な空間情報を取得する。これらは、例えば移動体に取り付けられているミリ波レーダー、カメラ、ＬｉＤＡＲ（Light Detection and Ranging）、ソナー等によって取得する。

周辺環境情報取得部１１０は、移動体１が走行目標とする少なくとも１つ以上の走行可能位置、走行速度、走行方位角等の情報も取得する。これらは、例えばユーザーが予め指定した情報、移動体１が持つ地図情報の所定の位置等から情報を取得する。ここで、移動体１が持つ地図情報は、高精度地図、カーナビ地図、ＳＬＡＭ（Simultaneous Localization and Mapping）等で生成した点群地図等を指す。

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

図２は、料金所ＦＳの手前の左右の車線境界ＬＢで規定される走行車線に、移動体１が進入しようとする状態を模式的に示す図である。図２においては、料金所ゲートＧＴを通過する際の走行可能位置、走行速度、走行方位角の一例を示す。図２において、ｉを添字とすると、（ｘ_ｐｉ,ｙ_ｐｉ）は走行可能位置、ｖ_ｐｉは各走行可能位置における走行速度、θ_ｐｉは各走行可能位置における走行方位角である。

走行可能位置の集合は以下の数式（１）で表される。

走行速度の集合は以下の数式（２）で表される。

走行方位角の集合は以下の数式（３）で表される。

ここで、添字ｆはその環境における終端の数字であり、図２の例であれば終端の数字は７となる。

図２の例では走行可能位置が複数存在している場合に、走行速度、走行方位角も複数個存在しているが、１つとすることもでき、走行可能位置の数に対応している必要はない。

以下、個々の走行可能位置、走行速度および走行方位角により走行可能状態が規定され、走行可能位置の集合、走行速度の集合、走行方位角の集合のまとまりを走行可能状態集合と呼称する。

なお、図２では、☆印は走行可能位置を、矢印の向きは走行方位角を、矢印の長さは走行速度を模式的に表しており、これは、他の平面図においても共通である。

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

図２の例では現在の移動体１の状態量は（ｘ_ｅ,ｙ_ｅ,θ_ｅ,ｖ_ｅ）で表され、ｘ_ｅは移動体１の位置のｘ座標、ｙ_ｅは移動体１の位置のｙ座標、θ_ｅは移動体１の方位角、ｖ_ｅは移動体１の速度である。また、移動体１の状態量は位置、方位角および速度に限定されず、少なくとも位置の状態量を含んでいれば足りる。また、加速度、躍度、高度、ヨーレート、回転角速度、舵角等のその他の移動体１の状態量を含むことも可能である。

ここで、図１の説明に戻る。図１に示されるように、通過地点生成装置２００は、目標状態候補生成部２１０および局所目標状態生成部２２０を有している。

目標状態候補生成部２１０は、移動体１が通過状態の集合、すなわち、移動体１が存在する環境にある全て料金所ゲートＧＴの中から、当初の走行目標として選定していた目標状態、すなわちゲートが走行不可となった場合に、周辺環境情報取得部１１０から取得した少なくとも１つ以上の走行可能状態、および自己状態取得部１２０から取得した移動体１の現在の状態に基づいて新たに決定した目標状態を、移動体１が選定した目標状態の変更のための目標状態候補として生成する。ここで、目標状態とは、移動体１が最終的に通過したい１つの目標の状態を指す。

移動体１は、目標状態候補の中では目標状態の変更を実行することができ、変更可能な目標状態候補を保持しておく。これにより、目標状態候補以外の通過状態への途中変更を制限することで、当初目標としていた目標状態から大きく離れた位置の想定外の目標状態に目標状態が変更されることを防止することができる。

目標状態は、移動体１の位置、速度および方位角に限定されず、少なくとも位置の状態量を含んでいれば足りる。また、加速度、躍度、高度、ヨーレート、回転角速度、舵角等その他の移動体１の状態量を含むことも可能である。

局所目標状態生成部２２０は、目標状態候補生成部２１０から取得する少なくとも１つ以上の目標状態候補の周辺まで移動する際の中間地点において、いずれの目標状態候補においても到達可能であるような１つの局所的な目標状態を演算する。

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

軌道生成部３１０は、少なくとも、局所目標状態生成部２２０で生成された局所的な目標状態に到達するまでに移動体１が走行すべき経路と速度で構成される軌道を生成し、生成された軌道に沿って移動体１を制御するように制御量演算部３２０に生成した軌道を出力する。この際、軌道生成部３１０で生成する軌道は速度情報および時間情報を含まない経路を生成することもできる。

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

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

次に、図２を参照しつつ、通過地点生成装置２００の動作の一例を図３に示すフローチャートを用いて説明する。以下では、料金所ゲートを走行する場合について説明する。

まず、通過地点生成装置２００での処理に先立って、前処理部１０において、自律センサ情報取得部１００の周辺環境情報取得部１１０で取得された周辺環境情報に基づいて、移動体１が現在走行している領域が料金所付近かどうかの領域判定を行う（ステップＳ１０１）。

図２において、料金所ＦＳの付近である領域は、例えば区画線ＢＬがなくなる地点より任意の距離手前の位置から走行可能位置、すなわち料金所ゲートＧＴを通過するまでの領域とすることも可能であり、区画線ＢＬのない領域または走行可能位置から任意の距離以内の領域とすることも可能である。

移動体１が現在走行している領域が料金所付近かどうかの領域判定は、移動体１が持つ高精度地図およびカーナビ地図などの地図情報の料金所の位置情報を用いたり、移動体１に搭載されている前方カメラから取得した画像を画像処理して判断することができる。

ステップＳ１０１において移動体１が料金所付近を走行していると判定された場合（Ｙｅｓの場合）は、通過地点生成装置２００は、周辺環境情報取得部１１０から移動体周辺の障害物情報と走行可能状態集合を取得し、自己状態取得部１２０から移動体１の現在の状態（自己状態）を取得する（ステップＳ１０２）。一方、移動体１が料金所を走行していない場合（Ｎｏの場合）は、周辺環境情報取得部１１０で取得された道路の車線の情報に基づいて、軌道生成部３１０において移動体１が走行すべき軌道を生成する。

ステップＳ１０２において、自己状態、周辺環境情報、走行可能状態集合を取得した後は、目標状態候補生成部２１０において、周辺環境情報取得部１１０および自己状態取得部１２０から取得した情報に基づいて複数の目標状態の候補（目標状態候補集合）を生成する（ステップＳ１０３）。この処理の概念図を図４に示す。

＜目標状態候補生成部での処理＞
図４は、料金所ＦＳの手前の走行車線に、移動体１が進入しようとする状態を模式的に示しており、図４において、ｉを添字とすると、（ｘ_ｃｉ,ｙ_ｃｉ）は目標位置候補、ｖ_ｃｉは目標速度候補、θ_ｃｉは目標方位角候補である。

目標位置候補の集合は以下の数式（４）で表される。

目標速度候補の集合は以下の数式（５）で表される。

目標方位角候補の集合は以下の数式（６）で表される。

ここで、添字ｅはその環境における終端の数字であり、図４の例であれば終端の数字は３となる。

図４の例では目標位置候補が複数存在している場合に、目標速度候補、目標方位角の候補も複数個存在しているが、１つとすることもでき、目標位置候補の数に対応している必要はない。

以下、個々の目標位置候補、目標速度候補および目標方位角候補により目標状態候補が規定され、目標位置候補の集合、目標速度候補の集合、目標方位角候補の集合のまとまりを目標状態候補集合と呼称する。

図４の例では、目標状態候補は、図２に示した周辺環境情報取得部１１０から得られた走行可能状態集合の中から選択した３つの走行可能状態であり、☆印を黒く塗りつぶして示している。換言すれば、図２に示した走行可能状態集合を３つの走行可能状態に絞ったものを目標状態候補集合としている。

また、図４の例のように、各目標状態候補は、隣り合う走行可能状態を選択しているように、目標状態候補集合の各状態量は、互いに近い値の塊とする。この塊とは、隣接する各目標状態が予め設定された所定範囲内となるような状態量の集合である。

目標状態候補がいずれも近くに位置するため、目標状態の変更による軌道修正がより滑らかに実行できる。

また、目標状態候補は、図５に示すように、いずれも移動体１の運動学的に到達可能な到達可能領域Ｒ０内にあって到達可能性が保証されている状態量とする。

すなわち、図５においては到達可能領域Ｒ０を規定する複数の到達可能境界Ｒ０Ｂが〇印で示されており、互いに前後する到達可能境界Ｒ０Ｂどうしを繋ぐことで到達可能境界線となり、２つの到達可能境界線で挟まれた領域が到達可能領域Ｒ０となる。

いずれの目標状態候補も到達可能性が保証されているため、移動体１がいずれかの目標状態に到達する可能性が高くなる。

図５の例では、到達可能領域Ｒ０に含まれる目標状態候補は４つであり、これらで目標状態候補集合を構成する。なお、図５の例では、到達可能領域Ｒ０内の４つの目標状態候補の全てが選択されているが、全てを選択する必要はなく、任意の個数分を選択することもできる。

到達可能境界Ｒ０Ｂは、例えば以下の数式（７）のような簡易な移動体の動力学モデルに、任意の初期状態、例えば、任意の速度、任意のスタート位置、任意の方位角等から、任意の加速度、任意の最大舵角速度、すなわち移動体１における制限値を繰り返し入力して得られた仮想移動体の離散的な予測軌道によって表現できる。この表現の概念図を図６に示す。

数式（７）において、ｔは時刻変数、ｄｔは制御周期における１サンプリング時間、ｕｍａｘは最大舵角速度入力であり、左右方向に２パターンの回転方向があるものとする。αは任意の加速度入力、δは舵角、Ｌはホイールベース、βは移動体横滑り角である。ここで、最大舵角速度入力は、例えば自動車のハンドルの１秒間あたりの回転角度（ｄｅｇ／ｓｅｃ）の最大値で定義され、安全性を考慮した値に設定される。

図６の例では、スタート位置を区画線ＢＬがなくなる料金所エリアの開始位置とし、当該スタート位置ＳＴＰに仮想移動体ＶＭＶが初期の状態量（ｘ_ｔ０,ｙ_ｔ０,θ_ｔ０,ｖ_ｔ０）で存在していると仮定している。そして、任意の入力値を時間毎に繰り返して仮想移動体の動作を規定する数式（７）に与えることで、図６に示されるような仮想移動体ＶＭＶの各時間毎の離散的な予測軌道を得ることができる。ここで、任意の入力値とは、到達可能領域Ｒ０を規定する予測軌道を得ることができる程度に異なった値である。

本実施の形態１では、入力値の上限値を繰り返し入力することで得た離散的な予測軌道、すなわち図６に向かって左側の複数の仮想移動体ＶＭＶで表される軌道と、入力値の下限値を繰り返し入力することで得た離散的な予測軌道、すなわち図６に向かって右側の複数の仮想移動体ＶＭＶで表される軌道とを到達可能境界とする。

ここで、入力値の上限値とは、例えば最大舵角速度入力ｕｍａｘ、または最大加速度入力であり、場合によっては最大舵角速度入力ｕｍａｘと加速度αを０に固定して入力したり、舵角速度ｕを固定して、加速度αを等加速度として入力したりする。また、入力値の下限値とは、例えば最小舵角速度入力、または最小加速度入力である。

なお、入力値の上限値および下限値は、移動体１が持つ性能限界に関する値に限らず、乗り心地を考慮した値またはユーザーが任意に決定した値とすることができる。

乗り心地を考慮した値の一例として、加速度、ジャーク、回転速度、回転加速度を所定値未満とすることが挙げられる。また、これらのうちの少なくとも１つを所定値未満とすることもできる。この所定値は、予め設定される固定値とすることもでき、ユーザーによって調整可能な値とすることもできる。

入力値の上限値および下限値を乗り心地を考慮した値とすることで、いずれかの目標状態に、乗り心地良く到達できる可能性が高くなる。

また、仮想移動体の動力学モデルは数式（７）に限らず、四輪を二輪に近似した動力学モデルである二輪モデルおよび対象とする移動体毎の動力学モデル等、別のモデルを用いることもできる。

また、到達可能境界Ｒ０Ｂは、動力学モデルで予測した軌跡に限らず、ユーザーが任意に設定したスプライン曲線、クロソイド曲線、ｎ次の多項式曲線、直線等で設定することもできる。

なお、到達可能領域Ｒ０は固定のスタート位置から演算した静的な領域としているが、時々刻々と移動する移動体１の現在の状態をスタート位置として移動体が移動するたび繰り返して演算して得られる動的な領域とすることもできる。

＜目標状態候補生成部の変形例での処理＞
図１に示した目標状態候補生成部２１０は、周辺環境情報取得部１１０から取得した少なくとも１つ以上の走行可能状態、および自己状態取得部１２０から取得した移動体１の現在の状態に基づいて新たに決定した目標状態を目標状態候補として生成するものとして説明したが、図７に示すような構成とすることもできる。

図７に示すように目標状態候補生成部２１０は、各走行可能状態を走行する、あるいは到達するまでの仮想的な軌道を生成する仮想軌道生成部２１１と、仮想的な軌道について移動体１の動力学的な制約に基づいて評価する仮想軌道評価部２１２と、制約を破らない仮想的な軌道で到達できる走行可能状態を目標状態候補として選択する目標状態候補演算部２１３とを備えた構成とすることができる。以下、当該構成で目標状態候補を生成する方法について説明する。

図８は、仮想軌道生成部２１１において走行可能状態までの仮想的な軌道を生成する処理を説明する概念図である。図８において、ｉを添字とすると、ｔｒｊ_ｉはｉ番目の走行可能状態までの仮想的な軌道を表している。

ここで、初期状態ＩＳは、区画線ＢＬがなくなる料金所エリアの開始位置とし、状態量（ｘ_０,ｙ_０,θ_０,ｖ_０）とする。

仮想的な軌道は、例えば数式（８）のような多項式で表現され以下の数式（９）から数式（１４）で表される初期状態および各走行可能状態における境界条件の連立方程式を解くことで各係数を導出することができる。

数式（８）において、ｊは多項式の次数、ｃは各次数の係数であり本実施の形態１では次数は５次とする。

ここで、ｆ'（ｘ）はｆ（ｘ）の一階微分、ｆ''（ｘ）はｆ（ｘ）の二階微分を表し、数式（９）は、初期状態における位置に関する境界条件、数式（１０）は、各走行可能状態における位置に関する境界条件、数式（１１）は、初期状態における傾きに関する境界条件、数式（１２）は、各走行可能状態における傾きに関する境界条件、数式（１３）は、初期状態における曲率に関する境界条件、数式（１４）は各走行可能状態における曲率に関する境界条件を表す。

目標状態候補を、移動体１が目標状態に到達可能な仮想的な軌道上に置くことで、いずれかの目標状態に到達する可能性が上が高くなる。

本実施の形態１では、仮想軌道生成部２１１で生成する仮想的な軌道は、多項式の２点境界値問題によって生成しているが、ポテンシャル法、モデル予測制御等のモデルベース手法、ＲＲＴ（Rapidly Exploring Random Tree）等のサンプリング手法およびＡ*探索アルゴリズム等のグラフ探索アルゴリズムなど、他の手法で生成することもできる。

仮想軌道評価部２１２においては、仮想軌道生成部２１１で生成された仮想的な軌道を動力学的に評価する。例えば、仮想的な軌道について、移動体１の最小旋回半径よりも曲率の大きい箇所がある軌道は動力学的な制約を越えるので、到達不可能な軌道とする、などの到達可能性の評価を行う。また、生成した軌道が周辺環境情報取得部１１０によって得られた外壁等の静的な障害物を跨ぐか、あるいは衝突するかを評価項目に加えることもできる。

また、数式（７）の移動体の動力学モデルを用いて仮想移動体の軌道の予測を行った場合に、移動体１が追従不可であった仮想移動体の軌道、最大舵角速度または舵角等の移動体１の動力学に関する制約を超えてしまう仮想移動体の軌道は、到達不可能な軌道とするような評価とすることができる。

目標状態候補演算部２１３においては、到達可能であると評価された仮想的な軌道で到達できる走行可能状態を目標状態候補とする。この処理の概念図を図９に示す。

図９は、仮想軌道生成部２１１において生成された仮想的な軌道ｔｒｊ_１～ｔｒｊ_７のうち、仮想軌道評価部２１２において到達不可能と評価された軌道に×印を付し、到達可能と評価された軌道に〇印を付しており、目標状態候補は、いずれも到達可能な仮想的な軌道で走行できる状態量となっている。

図９の例では、目標状態候補は４つであり、これらで目標状態候補集合を構成する。なお、図９の例では、目標状態候補は到達可能な仮想的な軌道で走行できる４つ全てが選択されているが、全てを選択する必要はなく、任意の個数分を選択することもできる。任意の個数は、例えばユーザーが設定した最大個数、最小個数、仮想的な軌道で到達できる全ての個数などが挙げられる。

目標状態候補は、仮想軌道評価部２１２において、各目標状態候補に到達する複数の仮想的な軌道を、所定の評価指標に基づいて点数付けを行い、各仮想的な軌道に付与された点数が所定の閾値以上または所定の閾値未満となる軌道で走行する地点を目標状態候補とすることもできる。この評価指標は、例えば、仮想的な軌道の直進性、長さ、曲率の最大値、曲率の変化率の大きさ、ハンドルの切り返し回数等が挙げられる。

上記評価指標において、直進性であれば、ほぼ直進の軌道であれば１０点であり、直進部分がない軌道であれば０点であるなどの配点が想定され、長さ、曲率などについても安全性に基づいた配点を行い、それらの合計点数を所定の閾値と比較することで、目標状態候補とする。

各目標状態候補に到達する複数の仮想的な軌道に点数付けを行うことで、目標状態候補を、設計者およびユーザーの好みによって配点を変えることで変更できる。

仮想軌道評価部２１２での評価方法としては、仮想的な軌道を幾何学的に評価する方法が挙げられる。例えば仮想的な軌道から、それぞれの軌道の最大曲率Ｋｍａｘを算出し、各軌道の点数を１／Ｋｍａｘ１，１／Ｋｍａｘ２，・・・１／Ｋｍａｘｉとするように曲率の値から点数を算出して評価する方法も考えられる。ここで、添字ｉは、各軌道の添字に相当する。

また、軌道の長さであれば、軌道の長さをＬとすると、それぞれの軌道の点数を１／Ｌ１，１／Ｌ２，１／Ｌ３，・・・１／Ｌｉ）とするように長さ値から点数を算出して評価する方法も考えられる。なお、上記において１／最大曲率、１／長さとしているのは、これらの幾何学的要素では、一般的に小さい値の方が安全性の観点からは良好であるためである。

また、上記の最大曲率による点数および長さによる点数を累積して合計点数を所定の閾値と比較することもできるが、重視したい幾何学的要素があれば、重み付けすることもできる。

また、より単純には、軌道の最大曲率の値がａａ～ｂｂの範囲にある場合は７点、ｂｂ～ｃｃの範囲にある場合は６点、ｃｃ～ｄｄの範囲にある場合は５点とするなどの、大まかな配点とすることもできる。上記において数値の大小関係はａａ＜ｂｂ＜ｃｃ＜ｄｄとなっている。

ここで、図３のフローチャートの説明に戻る。ステップＳ１０３において目標状態候補を演算した後は、局所目標状態生成部２２０において、目標状態候補集合の周辺まで移動する際の中間地点において、いずれの目標状態候補にも到達可能であるような１つの局所目標状態を演算する（ステップＳ１０４）。これは、中間地点からスタートすればいずれの目標状態候補にも到達できるとの概念に基づいた処理である。この処理の概念図を図１０に示す。

＜局所目標状態生成部での処理＞
図１０は、料金所ＦＳの手前の走行車線に、移動体１が進入しようとする状態を模式的に示しており、目標状態候補集合の周辺まで移動する際の中間地点において、局所目標状態ＬＴＳが算出された状態を示している。

図１０において、ｉを添字とすると、Ｓ_ｃｉは各目標状態候補を表し、以下の数式（１５）で定義されるが、状態変数は数式（１５）内の変数以外とすることもできる。

また、図１０において（ｘ_ｌ,ｙ_ｌ,θ_ｌ,ｖ_ｌ）は、局所目標状態ＬＴＳの各要素であり、各要素をまとめた局所目標状態Ｓ_ｌは、以下の数式（１６）で定義される。

図１０の例では、目標状態候補は３つであり、これらで目標状態候補集合を構成する。局所目標状態ＬＴＳからスタートすることで、いずれの目標状態候補にも到達可能となっている。

図１０の例では、局所目標状態ＬＴＳは、いずれの目標状態候補にも到達可能な位置としたが、図１１に示すように、局所目標状態ＬＴＳは、目標状態候補集合の中でも中央に位置する目標状態候補まで到達する仮想的な軌道上の任意の位置とすることもできる。

図１１においては、区画線ＢＬがなくなる料金所エリアの開始位置を初期状態ＩＳとし、状態量（ｘ_０,ｙ_０,θ_０,ｖ_０）とする。図１１において、ｔｒｊ_２は２番目の目標状態候補までの仮想的な軌道を表し、ｌ_ｓは、自律センサで検知可能な距離である。図１１に示すように、仮想的な軌道上の任意の位置は、目標状態候補集合から距離ｌ_ｓ手前の位置とし、当該位置を含むｙ方向に平行な線分を目標状態候補集合検知可能地点とし、当該地点と仮想的な軌道ｔｒｊ_２とが交わる位置を局所目標状態ＬＴＳとすることができる。

また、前記仮想的な軌道上の任意の位置は距離ｌ_ｓ分手前の位置よりも近い位置とすることができ、例えば図１２のように目標状態候補集合からの距離ｌ_ｓから任意の距離ｌ_aを引いた位置とすることができる。

図１１において、目標状態候補集合の中の中央に位置する目標状態候補を目指すように局所目標状態ＬＴＳを置いているのは、中央の目標状態候補が走行不可であった場合に、他の目標状態候補に軌道修正する際に、両隣のどちらにも目標状態候補が存在することとなり、軌道修正できる目標状態候補の数が多くなるためである。

また、局所目標状態を目標状態候補集合から自律センサの検知可能な距離ｌ_ｓ手前の位置とすることで、リアルタイムで動的な情報、例えば動的な障害物およびＥＴＣゲートの信号の色の取得が可能となり、これらの動的な情報が検知可能になると、選択した目標状態の変更が発生する可能性が高くなる。このため、例えば、目標ゲートに近づいたら故障車を検知してゲートを変更するなど、目標状態の変更を余儀なくされる状況が増える。

一方、図１１の場合、自律センサの検知可能な距離ｌ_ｓは、図１０の例のように、目標状態候補集合の周辺まで移動する際の中間地点において局所目標状態ＬＴＳを算出する場合よりも、短い距離となるので、局所目標状態ＬＴＳを目標状態候補集合により近い位置とすることができ、当該局所目標状態ＬＴＳに到達後は、最終的な目標状態が走行不可となることを避けることができる。

目標状態候補集合の中でも中央に位置する目標状態候補を目指すように局所目標状態ＬＴＳを置くことで、中央の目標状態候補が走行不可であった場合に、他の目標状態候補に軌道修正する際に、両隣のどちらにも目標状態候補が存在することとなり、軌道修正しやすくなる。

図１０および図１１では、局所目標状態ＬＴＳは１点としたが、図１３に示すように、局所目標状態は領域とすることもできる。

図１３は、料金所ＦＳの手前の走行車線に、移動体１が進入しようとする状態を模式的に示しており、ｌ_ｓは、自律センサで検知可能な距離である。図１３において、ｉを添字とすると、Ａ_ｃｉは各目標状態候補Ｓ_ｃｉに到達可能な到達可能領域であり、Ａ_ｓは目標状態候補集合を自律センサで検知可能なセンサ検知可能領域であり、Ａ_ｌは局所目標領域である。局所目標領域Ａ_ｌは、到達可能領域Ａ_ｃｉと、目標状態候補のセンサ検知可能領域Ａ_ｓとが重なる領域として定義される。

到達可能領域Ａ_ｃｉを構成する到達可能境界は、例えば、初期位置を各目標状態候補とした場合に、移動体１が通過可能な方位とは逆方向に仮想的な移動体が進むと仮定し、数式（７）のような簡易な移動体の動力学モデルに、任意の加速度、任意の最大舵角速度、すなわち移動体１における制限値を繰り返し入力して得られた仮想移動体の離散的な予測軌道によって表現できる。これが、図１３の各到達可能領域を規定する左右の境界である。

各到達可能領域内では、どこからスタートしても目標とする目標状態候補に到達することができ、さらに、局所目標領域Ａ_ｌ内であれば、どこからスタートしても目標状態候補集合のいずれかの目標状態候補に到達することができる。

また、到達可能境界は、動力学モデルで予測した軌跡に限らず、ユーザーが任意に設定したスプライン曲線、クロソイド曲線、ｎ次の多項式曲線、直線等で設定することもできる。

なお、到達可能境界を得るために動力学モデルに入力する入力値の上限値および下限値は、移動体１が持つ性能限界に関する値に限らず、乗り心地を考慮した値またはユーザーが任意に決定した値とすることができる。

図１０および図１１では、局所目標状態ＬＴＳは１点とし、図１３では局所目標領域としたが、図１４に示すように、局所目標状態は、各目標状態候補に到達できる確率分布で表現することもできる。

図１４は、料金所ＦＳの手前の走行車線に、移動体１が進入しようとする状態を模式的に示しており、局所目標状態の確率分布ｆ_ｐ（ｓ_ｉ）は、ハッチングの濃淡で確率の高さを表しており、最も濃い領域が目標状態候補に到達できる確率が最も高く、この領域からスタートすることで、いずれかの目標状態候補に到達する可能性が最も高くなる。

＜目標状態候補への重み付け＞
局所目標状態生成部２２０は、目標状態候補生成部２１０で演算した各目標状態候補に対する所定の評価指標に基づいて重み付けを行い、重みに応じて局所目標状態を演算することができる。この処理の概念図を図１５に示す。

図１５は、料金所ＦＳの手前の走行車線に、移動体１が進入しようとする状態を模式的に示しており、ｉを添字とすると、ｗ_ｉは目標状態候補に付与される重みであり、重さはｗ_１が最も重く、ｗ_３が最も軽く、ｗ_１＞ｗ_２＞ｗ_３となっている。

目標状態候補Ｓ_ｃ１に付与されている重みが最も重いため、局所目標状態ＬＴＳは、目標状態候補Ｓ_ｃ１に近い状態量、すなわち目標状態候補Ｓ_ｃ１寄りに位置しており、局所目標状態ＬＴＳからスタートすると目標状態候補Ｓ_ｃ１に到達する可能性が高くなる。

これにより、局所目標状態を、設計者およびユーザーの好みによって重みを変えることで設定できる。

ここで、重み付けの指標となる各目標状態候補に対する所定の評価指標は、例えば現在の移動体１からの距離が近い候補ほど重みを重くし、また、横幅が広いゲートを通過する候補ほど重みを重くし、ゲート内の経路がカーブであるゲートを通過する候補は重みを軽くするなど、幾何学的な条件に基づいて評価することができる。

また、目標状態候補が通過するゲートの混雑具合、目標状態候補が通過するゲートがＥＴＣゲートか、一旦停止を必要とする一般ゲートか、目標状態候補が通過するゲートが料金所の両端のどちらかにあるかに基づいて評価することができる。

また、目標状態候補が通過するゲートの手前の道路形状が複雑だったり、ゲート通過に要する通過速度等のゲート通過の難易度に基づいて評価することができる。

＜仮想的な軌道への重み付け＞
局所目標状態生成部２２０は、図７に示した目標状態候補生成部２１０における仮想軌道生成部２１１で生成した仮想的な軌道の中でも、各目標状態候補に到達するまでの軌道に対して重み付けを行い、仮想的な軌道の重みを用いて、局所目標状態とすることもできる。この処理の概念図を図１６に示す。

図１６は、区画線ＢＬがなくなる料金所エリアの開始位置を初期状態ＩＳとし、初期状態ＩＳからスタートする仮想的な軌道を模式的に示しており、ｌ_ｓは、自律センサで検知可能な距離である。図１６において（ｘ_ｌ,ｙ_ｌ,θ_ｌ,ｖ_ｌ）は、局所目標状態ＬＴＳの各要素であり、ｉを添字とすると、ｔｒｊ_iはｉ番目の目標状態候補までの仮想的な軌道を表し、ｗ_ｉは仮想的な軌道ｔｒｊ_iに付与される重みであり、（ｘ_ｌｉ,ｙ_ｌｉ）は仮想的な軌道と目標状態候補集合検知可能地点とが交わる位置である。これを仮想軌道上任意点と呼称する。

仮想的な軌道ｔｒｊ_iに付与される重みの重さはｗ_１が最も重く、ｗ_３が最も軽く、ｗ_１＞ｗ_２＞ｗ_３となっている。仮想的な軌道ｔｒｊ_１に付与されている重みが最も重いため、局所目標状態ＬＴＳは、目標状態候補Ｓ_ｃ１に近い状態量、すなわち目標状態候補Ｓ_ｃ１寄りに位置しており、局所目標状態ＬＴＳからスタートすると目標状態候補Ｓ_ｃ１に到達する可能性が高くなる。

このように、局所目標状態を、設計者およびユーザーの好みによって重みを変えることで設定できる。

局所目標状態ＬＴＳは、例えば、数式（１７）および数式（１８）を用いて仮想軌道上任意点と重みの加重平均を取ることで、各仮想的な軌道の重みの大きさを反映させることができる。ただし、重みは１から０の間の値とし、図１６の例であれば終端の添字ｂは３となる。

局所目標状態ＬＴＳのｘ方向の位置ｘ_ｌは、以下の数式（１７）で得ることができる。

局所目標状態ＬＴＳのｙ方向の位置ｙ_ｌは、以下の数式（１８）で得ることができる。

重みは、例えば、移動体１との距離、各目標状態候補に移動するために必要な横移動量、各目標状態候補が通過するゲートの混雑具合、他車の目標状態の選択率、より大域的な目標状態（実施の形態２で説明）との距離および位置関係などに基づいて設定される。なお、数式（１７）および数式（１８）に示すように、ｘ方向に対する重みとｙ方向に対する重みとを同じｗ_ｉとしているが、異なっていてもよい。

ここで、図３のフローチャートの説明に戻る。ステップＳ１０４において局所目標状態を演算した後は、後処理部２０において、移動体が局所目標状態へ接近したかを判定する（ステップＳ１０５）。例えば、移動体１と局所目標状態とのユークリッド距離が閾値未満の場合、または図１３を用いて説明した局所目標領域Ａ_ｌ内に到達した場合に移動体１が局所目標状態に接近したと判定される。

ステップＳ１０５において移動体１が局所目標状態へ接近していない判定された場合（Ｎｏの場合）は、ステップＳ１０６に進み、局所目標状態を目標値として、軌道生成部３１０（図１）において移動体１の目標軌道を生成する。この処理の概念図を図１７に示す。

図１７は、料金所ＦＳの手前の走行車線に、移動体１が進入しようとする状態を模式的に示しており、軌道生成部３１０で生成された移動体１の位置から局所目標状態ＬＴＳまでの移動体１の目標軌道ｔｒｊ_ｔが示されている。

一方、ステップＳ１０５において移動体が局所目標状態へ接近したと判定された場合（Ｙｅｓの場合）は、ステップＳ１０６に進み、軌道生成部３１０において、目標状態候補の中から１つの目標状態までの軌道を目標軌道として生成する。この処理の概念図を図１８に示す。

図１８には、移動体１が当初の目標状態候補に接近した後、３つの目標状態候補から１つの目標状態を選択し、選択された目標状態までの目標軌道ｔｒｊ_ｔが軌道生成部３１０で生成された状態を示している。

この場合、１つの目標状態の選択方法は、例えば、所定の指標に応じて目標状態候補に重み付けを行い、重みが大きい目標状態候補を選択する方法が挙げられる。

ここで、所定の指標とは、例えば、移動体１との距離、各目標状態候補に移動するために必要な横移動量、各目標状態候補が通過するゲートの混雑具合、他車の目標状態の選択率、より大域的な目標状態（実施の形態２で説明）との距離および位置関係などが挙げられる。

また、図１８のように、１つの目標状態候補の付近で故障車ＢＶが存在している場合、または料金所ゲートＧＴの信号が赤信号となって通過できない目標状態候補がある場合は、該当する目標状態候補を除外するという方法が挙げられる。

このように、ステップＳ１０５の判定結果がＹｅｓかＮｏかによって、次のステップＳ１０６で生成される軌道は異なる。すなわち、ステップＳ１０５の判定がＹｅｓの場合には、軌道生成部３１０は、目標状態までの軌道を生成し、ステップＳ１０５の判定がＮｏの場合には、軌道生成部３１０は、局所目標状態までの軌道を生成する。ステップＳ１０６においていずれかの軌道を生成した後は、一連の処理を終了する。なお、生成された軌道に基づいて、運動制御部３００の制御量演算部３２０では目標軌道に沿って走行するための移動体１への目標制御値を演算し、アクチュエータ制御部３３０は、目標制御値に移動体１が追従するように、アクチュエータを動作させるが、これらの動作は公知の技術を用いることができるので、説明は省略する。

ここで、軌道生成部３１０において局所目標状態までの軌道を生成する場合には、移動体１が局所目標状態にある程度近づいた後に、急に局所目標状態を変更すると、移動体１は急な軌道修正が必要になり、また、変更後の局所目標状態に到達可能な軌道が見つからない場合もあるため、このような急な局所目標状態の変更は制限することができる。具体的には、単位時間あたりの状態量の変化が所定量以上となるような目標状態量の変更を制限することができる。

急な局所目標状態の変更を制限することで、移動体１の急な軌道修正および変更後の局所目標状態に到達できる軌道が見つからない状態を防止できる。

このような急な局所目標状態の変更の例を図１９および図２０に示す。図１９には、移動体１の現在位置から、当初の局所目標状態ＬＴＳまでの移動体１の目標軌道ｔｒｊ_ｔが示されているが、急に局所目標状態ＬＴＳＸに変更されると、当初の目標軌道ｔｒｊ_ｔでは変更後の局所目標状態ＬＴＳＸに到達できなくなる。このような場合は、新たな目標軌道ｔｒｊ_ｔを生成することになり、時間を要する。

図２０には、当初の局所目標状態ＬＴＳが料金所ＦＳの近くに存在している場合が示されているが、急に局所目標状態ＬＴＳＸに変更されると、料金所ゲートＧＴの手前の故障車ＢＶの存在により、変更後の局所目標状態ＬＴＳＸに到達できる軌道が見つからない、または算出する時間がない場合がある。

さらに、図３のステップＳ１０１～ステップＳ１０４の処理の期間に制限をかけることもできる。この制限を図２１を用いて説明する。

図２１は、区画線ＢＬがなくなる料金所エリアの開始位置をスタート位置ＳＴＰとし、スタート位置ＳＴＰよりも手前側を領域ＲＡ、スタート位置ＳＴＰよりも料金所ゲートＧＴ側を領域ＲＢとし、図３のステップＳ１０１～ステップＳ１０４の処理は、移動体１が領域ＲＡを走行中に行い、移動体１がスタート位置ＳＴＰを通過した後は、ステップＳ１０１～Ｓ１０４までの処理は行わず、領域ＲＡを走行中に演算された目標状態候補集合と局所目標状態ＬＴＳの値を用いる。

これにより、移動体１がスタート位置ＳＴＰを通過後に目標とする局所目標状態が常に変化したり、移動体１の目標値が振動したりすることを防ぎ、生成した軌道の揺れ、すなわち軌道生成時の目標値の揺れを防止することができる。

なお、スタート位置ＳＴＰは、区画線ＢＬがなくなる地点に限らず、区画線ＢＬがなくなる地点よりも所定の距離手前の地点とすることもできる。

以上説明したように実施の形態１の通過地点生成装置２００によれば、目標通過地点を決定する初めの段階で、目標通過地点が修正されることを考慮して、複数の目標通過地点の候補を決定しておく。また、いずれの目標通過地点にも到達可能である局所目標状態ＬＴＳを中間地点に設定し、中間地点を目指して走行する。このため、中間地点付近の走行時および、それ以前で目標通過地点の変更が発生しても、目標通過地点の変更に臨機応変に対応でき、目標通過地点の通過確率を向上できる。また、目標通過地点が通行不可である場合に、目標通過地点を他の通過地点に変更する動作を、急な軌道修正処理をせずに実施することが可能となる。

＜実施の形態２＞
図２２は、本開示に係る実施の形態２の通過地点生成装置を搭載した移動体１の構成を示すブロック図である。なお、図２２においては、図１を用いて説明した移動体１と同一の構成については同一の符号を付し、重複する説明は省略する。

図２２に示す移動体１は、自律センサ情報取得部１００において、周辺環境情報取得部１１０、自己状態取得部１２０および大域環境情報取得部１３０を有している。

大域環境情報取得部１３０は、周辺環境情報取得部１１０よりもさらに広範囲の大域的な情報を取得する。この情報には、例えば、車線のルート情報、より大域的な目的地、通過位置および中継地点などの情報が該当し、例えばユーザーが予め指定した情報、移動体１が持つ地図情報の所定のルート情報および位置情報、高精度ロケータおよび車載通信装置等から取得する。

また、移動体１は、通過地点生成装置２００において、大域目標状態生成部２３０、目標状態候補生成部２１０および局所目標状態生成部２２０を有している。

大域目標状態生成部２３０は、大域環境情報取得部１３０から取得した大域的な目的地等の情報に基づいて、周辺環境情報取得部１１０で取得する走行可能状態および目標状態候補生成部２１０で生成する目標状態候補集合よりもさらに大域的な目標状態である大域的目標状態を生成する。この処理の概念図を図２３に示す。

図２３は、移動体１がスタート位置ＳＴＰを通過し、料金所ＦＳに接近しつつある状態を示しており、料金所ＦＳの向こう側には大域的目標状態ＧＴＳが存在している。

図２３において、（ｘ_ｇ,ｙ_ｇ,θ_ｇ,ｖ_ｇ）は大域的目標状態ＧＴＳの各要素であり、（ｘ_ｇ,ｙ_ｇ）は大域的目標位置、ｖ_ｇは大域的目標速度、θ_ｇは大域的目標方位角である。ここで、大域的目標状態は少なくとも位置の情報を含んでいれば足り、速度および方位角の情報を含まないことも可能である。図２３中の目標ルートＴＲは、大域環境情報取得部１３０で取得する情報の一例であり、図２３における大域的目標状態ＧＴＳは、目標ルートＴＲに従って走行するために必要な移動体１の初期位置となる。

図２３の例では、目標状態候補は、走行可能状態集合の中から選択した３つの走行可能状態であり、目標状態候補集合として☆印を黒く塗りつぶして示している。

目標状態候補生成部２１０は、スタート位置ＳＴＰを初期位置とし、初期位置から移動体１の運動学的に到達可能な到達可能領域と、大域的目標状態ＧＴＳに移動体１の運動学的に到達可能な到達可能領域との双方が重なる領域内における走行可能状態集合の中から目標状態候補を生成する。この処理の概念図を図２４に示す。

図２４において、到達可能領域Ｒ１は、スタート位置ＳＴＰから移動体の運動学的に到達可能な領域であり、複数の到達可能境界Ｒ１Ｂが〇印で示されており、互いに前後する到達可能境界Ｒ１Ｂどうしを繋ぐことで到達可能境界線となり、２つの到達可能境界線で挟まれた領域が到達可能領域Ｒ１となる。

到達可能領域Ｒ２は、大域的目標状態ＧＴＳに移動体が運動学的に到達可能な領域であり、複数の到達可能境界Ｒ２Ｂが〇印で示されており、互いに前後する到達可能境界Ｒ２Ｂどうしを繋ぐことで到達可能境界線となり、２つの到達可能境界線で挟まれた領域が到達可能領域Ｒ２となる。目標状態候補は、到達可能領域Ｒ１と到達可能領域Ｒ２の双方が重なる領域内の状態量である。

到達可能領域Ｒ１およびＲ２は、実施の形態１において説明した数式（７）のような簡易な移動体の動力学モデルを用いて得られる仮想移動体の離散的な予測軌道によって到達可能境界Ｒ１ＢおよびＲ２Ｂを導出することで得ることができる。

＜目標状態候補生成部の変形例での処理＞
また、図２２に示した目標状態候補生成部２１０の代わりに、図７に示した目標状態候補生成部２１０のように、仮想的な軌道を生成する仮想軌道生成部２１１と、仮想的な軌道について移動体１の動力学的な制約に基づいて評価する仮想軌道評価部２１２と、制約を破らない仮想的な軌道で到達できる走行可能状態を目標状態候補として選択する目標状態候補演算部２１３とを備えた構成とすることができる。

図２５は、仮想軌道生成部２１１において走行可能状態までの仮想的な軌道を生成する処理を説明する概念図である。図２５において、ｉを添字とすると、ｔｒｊ_biはｉ番目の走行可能状態までの仮想的な軌道であり、ｔｒｊ_aiは、ｉ番目の走行可能状態から大域的目標状態ＧＴＳまでの仮想的な軌道である。

まず、仮想軌道生成部２１１においてスタート位置ＳＴＰから走行可能状態までの仮想的な軌道と、走行可能状態から大域的目標状態ＧＴＳまでの仮想的な軌道をそれぞれ生成し、次に、仮想軌道評価部２１２において仮想的な軌道を動力学的に評価する。最後に、スタート位置ＳＴＰから走行可能状態までの仮想的な軌道と、走行可能状態から大域的目標状態までの仮想的な軌道の双方が到達可能であると評価された、到達可能な２つの軌道の組み合わせで通過できる通過状態を目標状態候補とする。

なお、仮想的な軌道は、実施の形態１において説明した数式（８）のような多項式で表現され、数式（９）～（１４）で表される境界条件の連立方程式を解いて各係数を導出することで得ることができる。

また、目標状態候補は、仮想軌道評価部２１２において、スタート位置ＳＴＰから各目標状態候補に到達する複数の仮想的な軌道、および各目標状態候補から大域的目標状態ＧＴＳに到達する複数の仮想的な軌道について、所定の評価指標に基づいて点数付けを行う。そして、各目標状態候補に到達する仮想的な軌道と各目標状態候補から大域的目標状態ＧＴＳに到達する仮想的な軌道について、双方の点数が閾値以上となる組み合わせで走行できる目標状態を目標状態候補とする。また、仮想的な軌道について点数付けを行う代わりに、実施の形態１で示した重みを付与してもよい。この場合、スタート位置ＳＴＰから各目標状態候補に到達する複数の仮想的な軌道、および各目標状態候補から大域的目標状態ＧＴＳに到達する複数の仮想的な軌道について、重みが付与される。

以上説明したように実施の形態２の通過地点生成装置２００によれば、目標状態への走行確率の向上と、目標状態が走行不可で目標状態を変更するという動作を急な軌道修正をすることなく実施することが可能である。さらに、目標状態に到達した後に、さらにその先の大域的な目標地点を目指す場合に、目標状態が大域的な目標状態を考慮しているため、大域的な目標地点に到達しやすくなる。

＜実施の形態３＞
図２６は、本開示に係る実施の形態３の通過地点生成装置が搭載された管制サーバー２（管制装置）と移動体１の構成を示すブロック図である。なお、図２６においては、図１を用いて説明した移動体１と同一の構成については同一の符号を付し、重複する説明は省略する。

図２６に示す管制サーバー２は、走行エリア情報取得部４００および通過地点生成装置２００を有している。

走行エリア情報取得部４００は、エリア情報取得部４１０を有している。エリア情報取得部４１０は、料金所エリアの走行可能な空間情報、例えば料金所エリアを規定する外壁等の位置情報、移動体１の客観的な状態量、すなわち自車座標系ではなく絶対座標系での状態量、移動障害物の客観的な状態量、移動体１が走行目標とする少なくとも１つ以上の走行可能位置、走行速度、走行方位角等の情報を取得する。これらは走行エリアに設置されている路側センサから情報を収集する。

通過地点生成装置２００は、通過地点生成装置２００は、目標状態候補生成部２１０および局所目標状態生成部２２０を有している。

目標状態候補生成部２１０は、エリア情報取得部４１０で取得された移動体１の客観的な状態量、移動体１が走行目標とする少なくとも１つ以上の走行可能位置、走行速度、走行方位角等の情報に基づいて、新たに決定した目標状態を、目標状態の変更のための目標状態候補として生成する。

局所目標状態生成部２２０で演算した局所的な目標状態の状態量は、移動体１に送信され、運動制御部３００の軌道生成部３１０において、局所的な目標状態に到達するまでに移動体１が走行すべき経路と速度で構成される軌道を生成する。

このように、実施の形態３では、通過地点生成装置２００が管制サーバー２に搭載にされ、管制サーバー２と移動体１との間の通信により、局所的な目標状態が管制サーバー２から移動体１に送信されるシステムとなっている。このシステムの概念図を図２７に示す。

図２７は、料金所ＦＳの手前の走行車線に進入した移動体１と、移動体１を管制制御する管制サーバー２とが通信している状態を模式的に示している。また、料金所エリアには複数の路側センサＲＳが設置されており、それぞれの路側センサＲＳと移動体１が管制サーバー２を介して通信し合っている。

管制サーバー２の走行エリア情報取得部４００は、複数の路側センサＲＳを介して移動体１の客観的な状態量、料金所エリア内の移動障害物ＭＯＢの客観的な状態量等の情報を取得する。

このように、管制サーバー２と移動体１とが通信を行い、管制サーバー２内の通過地点生成装置２００の局所目標状態生成部２２０で演算したは局所的な目標状態の状態量は、通信により移動体１に与えられる。このため、局所目標状態生成部２２０は、管制サーバー２から移動体１への送信に伴う通信遅延を考慮して、目標状態候補、および局所目標状態を生成する。

例えば、実施の形態１において図５を用いて説明した到達可能領域、図８を用いて説明した仮想的な軌道などの目標状態候補を演算するために必要な、移動体１の動力学的な到達可能性については、移動体１の動力学のみでなく通信遅延も数式（７）等の動力学モデルに入れることで、通信遅延を考慮した演算を行う。

また、実施の形態１において、図１０および図１３等の局所目標状態の演算に関わる移動体の動力学的な到達可能性についても、移動体の動力学のみでなく通信遅延も考慮した上で局所目標状態の生成を行う。

例えば、目標状態候補に到達するまでの通信遅延による追従遅れを考慮して局所目標状態の位置をやや手前にする方法などが挙げられる。

以上説明したように実施の形態３の通過地点生成装置２００は、管制サーバー２に搭載されている。このため、移動体１内に通過地点生成装置２００を搭載する場合に比べて、演算処理が移動体１内に集中せず、移動体１内の演算負荷が軽減される。

また、管制サーバー２は移動体１よりも広範囲な領域を監視することができる。そのため、管制サーバー２で認知した情報を活用して通過地点生成装置２００で演算した通過地点は、移動体１で算出した通過地点よりも適した位置になりやすい。

管制サーバー２は複数台の移動体に目標状態を指示することができ、複数台の移動体が協調しながら自動でゲート通過をすることができる。

また、通信遅延が発生する管制制御であっても、いずれの目標状態候補にも到達することができ、料金所ゲートＧＴの通過確率が向上する。

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

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

通過地点生成装置２００は、構成要素の各々の機能が個別の処理回路で実現でき、それらの機能がまとめて１つの処理回路で実現することもできる。

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

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

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

以上のように、通過地点生成装置２００は、ハードウェア、ソフトウェア等、またはこれらの組み合わせによって、上述の各機能を実現することができる。

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

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

Claims

移動体の到達すべき通過地点を生成する通過地点生成装置であって、
前記移動体の周辺環境情報と前記移動体の状態量に基づいて、複数の走行可能位置から目標位置候補を複数選定し、選定した前記目標位置候補を含む複数の目標状態候補を演算する目標状態候補生成部と、
前記複数の目標状態候補の周辺までの中間地点において、いずれの目標状態候補においても到達可能な局所的な目標状態である局所目標状態を演算し、前記局所目標状態を前記通過地点として出力する局所目標状態生成部と、を備える、通過地点生成装置。
前記目標状態候補生成部は、
前記移動体が前記局所目標状態に到達した際に、前記いずれの目標状態候補においても到達可能なように、隣接する各目標状態が所定範囲内となるように互いに近い状態量の集合とする、請求項１記載の通過地点生成装置。
前記目標状態候補生成部は、
前記いずれの目標状態候補も、前記移動体の運動学的に到達可能な到達可能領域内にあるように演算する、請求項１または請求項２記載の通過地点生成装置。
前記目標状態候補生成部は、
前記いずれの目標状態候補も、前記移動体の乗り心地を考慮した移動により到達可能なように演算する、請求項１に記載の通過地点生成装置。
前記複数の目標状態候補の各目標状態よりも大域的な目標状態を生成して大域的目標状態として出力する大域目標状態生成部をさらに備え、
前記目標状態候補生成部は、
前記いずれの目標状態候補も、前記大域的目標状態に到達可能なように演算する、請求項１に記載の通過地点生成装置。
前記複数の目標状態候補のそれぞれは、
前記移動体が料金所ゲートを通過するための目標状態であって、
前記大域的目標状態は、
前記移動体が前記料金所ゲートを通過した後に、前記移動体が目指す位置の状態量を少なくとも含む、請求項５記載の通過地点生成装置。
前記目標状態候補生成部は、
複数の目標状態を通過する複数の仮想的な軌道を生成する仮想軌道生成部と、
前記複数の仮想的な軌道について前記移動体の動力学的な制約に基づいて評価する仮想軌道評価部と、
前記複数の仮想的な軌道のうち、前記制約を破らない仮想的な軌道で到達できる前記複数の目標状態を前記複数の目標状態候補として演算する目標状態候補演算部と、を有する、請求項１に記載の通過地点生成装置。
前記局所目標状態生成部は、
前記複数の仮想的な軌道のうち、前記目標状態候補演算部で演算した前記複数の目標状態候補の中で最も中央に位置する目標状態候補まで到達する仮想的な軌道上の任意の位置を前記局所目標状態とする、請求項１に記載の通過地点生成装置。
前記局所目標状態生成部は、
前記移動体が前記局所目標状態に到達する直前での前記局所目標状態の変更を制限する請求項１に記載の通過地点生成装置。
前記局所目標状態生成部は、
前記複数の目標状態候補について、所定の評価指標に基づいて重み付けを行い、重みの値に基づいて前記局所目標状態を演算する、請求項１に記載の通過地点生成装置。
前記目標状態候補生成部は、
前記複数の仮想的な軌道について、所定の評価指標に基づいて点数付けを行い、仮想的な軌道にそれぞれ付与された点数と閾値との比較を行い、前記閾値以上または前記閾値未満となる前記仮想的な軌道で走行する地点を前記複数の目標状態候補とする、請求項７記載の通過地点生成装置。
前記局所目標状態生成部は、
前記複数の仮想的な軌道のうち、前記目標状態候補演算部で演算した前記複数の目標状態候補まで到達する仮想的な軌道に対して重み付けを行い、前記仮想的な軌道上の任意の位置と重みとを加重平均した状態を前記局所目標状態とする、請求項７記載の通過地点生成装置。
前記通過地点生成装置は、
前記移動体との通信により前記移動体を管制制御する管制装置に搭載され、
前記局所目標状態生成部は、
管制制御の通信遅延を考慮して、前記いずれの目標状態候補にも到達可能なように前記局所目標状態を演算する、請求項１に記載の通過地点生成装置。