(57)【要約】      （修正有）【課題】クロソイド長が可変の場合の始端の姿勢から終端での姿勢への軌道を計画する方法である。【解決手段】軌道計画のためのプログラムは、繰り返し最適化手法によって、３連クロソイド曲線で表される軌道を最適化するプログラムであって、以下のステップによって最適化を行う。３本のクロソイド曲線の全長を固定長としてクロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、サンプルのパラメータを到達点関数に適用して到達点を求め、到達点の中で終端に最も近い最近傍到達点を得たパラメータを求める粗探索ステップと、粗探索ステップで求めたパラメータを初期値として、到達点関数のパラメータを変動させ、到達点の姿勢が終端の姿勢に一致するような最適パラメータを求める最適化ステップとを実行させる。【選択図】図６
2019096269.tif 000002
【特許請求の範囲】
【請求項１】
３本のクロソイド曲線からなる軌道を計画するためのプログラムであって、コンピュータに、
始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、
前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、３本の前記クロソイド曲線の全長を固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索ステップと、
前記粗探索ステップで求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップと、
を実行させるプログラム。
【請求項２】
前記粗探索ステップにて、前記終端までの距離が所定の閾値以下となる最近傍到達点を与えるサンプルのパラメータが存在しなかった場合には、入力された条件を満たす解が存在しないと判定する請求項１に記載のプログラム。
【請求項３】
３本のクロソイド曲線からなる軌道を計画するためのプログラムであって、コンピュータに、
始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、
前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、３本の前記クロソイド曲線の全長を第１の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも手前にある内周到達点を求め、前記内周到達点の中で前記終端に最も近い最近傍内周到達点を得たパラメータを第１パラメータとして求める第１パラメータ算出ステップと、
３本の前記クロソイド曲線の全長を前記第１の固定長より長い第２の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも奥にある外周到達点を求め、前記外周到達点の中で前記終端に最も近い最近傍外周到達点を得たパラメータを第２パラメータとして求める第２パラメータ算出ステップと、
３本の前記クロソイド曲線の全長を前記第１の固定長と前記第２の固定長の中間の中間固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して最近傍内周到達点及び最近傍外周到達点を求め、前記最近傍内周到達点を得たパラメータを第３パラメータ、前記最近傍外周到達点を得たパラメータを第４パラメータとして求める第３・第４パラメータ算出ステップと、
前記第２パラメータで与えられる最近傍外周到達点と前記第４パラメータで与えられる最近傍外周到達点とを比べ、前記第４パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第２の固定長とし、前記第２パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べ、前記第３パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第１の固定長とする固定長更新ステップと、
前記固定長更新ステップにて前記第１の固定長または前記第２の固定長を更新しつつ、最近傍外周到達点および最近傍内周到達点を繰り返し求め、前記第１の固定長と前記第２の固定長の差が所定の閾値以下となったときに、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めるパラメータ決定ステップと、
前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップと、
を実行させるプログラム。
【請求項４】
前記固定長更新ステップにおいて、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べて、前記第１パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求める請求項３に記載のプログラム。
【請求項５】
前記第１パラメータ算出ステップにおける計算によって前記内周到達点が見つからなかった場合には、前記始端と前記終端とをつなぐクロソイド曲線が存在しないと判定する請求項４に記載のプログラム。
【請求項６】
前記第２パラメータ算出ステップにおける計算によって前記外周到達点が見つからなかった場合には、前記外周到達点が見つかるか、前記第２の固定長が所定の閾値以上になるまで、前記第２の固定長を長くして外周到達点を求める処理を行い、前記第２の固定長が所定の閾値以上になったときは、前記始端と前記終端とをつなぐクロソイド曲線が存在しないと判定する請求項４に記載のプログラム。
【請求項７】
前記到達点関数は、前記車両の旋回曲率をパラメータとして含み、前記旋回曲率の最大値を拘束条件として有する請求項１乃至６のいずれかに記載のプログラム。
【請求項８】
軌道計画装置によって３本のクロソイド曲線からなる軌道を計画するための方法であって、
前記軌道計画装置は、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、
前記軌道計画装置は、前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、前記軌道計画装置は、３本の前記クロソイド曲線の全長を固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索ステップと、
前記軌道計画装置は、前記粗探索ステップで求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップと、
を備える軌道計画方法。
【請求項９】
軌道計画装置によって、３本のクロソイド曲線からなる軌道を計画するための方法であって、
前記軌道計画装置は、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、
前記軌道計画装置は、始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、前記軌道計画装置は、３本の前記クロソイド曲線の全長を第１の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも手前にある内周到達点を求め、前記内周到達点の中で前記終端に最も近い最近傍内周到達点を得たパラメータを第１パラメータとして求める第１パラメータ算出ステップと、
前記軌道計画装置は、３本の前記クロソイド曲線の全長を前記第１の固定長より長い第２の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも奥にある外周到達点を求め、前記外周到達点の中で前記終端に最も近い最近傍外周到達点を得たパラメータを第２パラメータとして求める第２パラメータ算出ステップと、
前記軌道計画装置は、３本の前記クロソイド曲線の全長を前記第１の固定長と前記第２の固定長の中間の中間固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して最近傍内周到達点及び最近傍外周到達点を求め、前記最近傍内周到達点を得たパラメータを第３パラメータ、前記最近傍外周到達点を得たパラメータを第４パラメータとして求める第３・第４パラメータ算出ステップと、
前記軌道計画装置は、前記第２パラメータで与えられる最近傍外周到達点と前記第４パラメータで与えられる最近傍外周到達点とを比べ、前記第４パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第２の固定長とし、前記第２パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べ、前記第３パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第１の固定長とする固定長更新ステップと、
前記軌道計画装置は、前記固定長更新ステップにて前記第１の固定長または前記第２の固定長を更新しつつ、最近傍外周到達点および最近傍内周到達点を繰り返し求め、前記第１の固定長と前記第２の固定長の差が所定の閾値以下となったときに、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めるパラメータ決定ステップと、
前記軌道計画装置は、前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点における車両の位置、向き、曲率で指定される姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップと、
を備える軌道計画方法。
【請求項１０】
３本のクロソイド曲線からなる軌道を計画するための軌道計画装置であって、
始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力部と、
前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶した記憶部と、
３本の前記クロソイド曲線の全長を固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記記憶部から読み出した前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索部と、
前記粗探索部で求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化部と、
を備える軌道計画装置。
【請求項１１】
３本のクロソイド曲線からなる軌道を計画するための軌道計画装置であって、
始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力部と、
始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶した記憶部と、
複数の組合せをサンプルのパラメータを前記記憶部から読み出した前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索部と、
前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化部と、
を備え、
前記粗探索部は、
３本の前記クロソイド曲線の全長を第１の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも手前にある内周到達点を求め、前記内周到達点の中で前記終端に最も近い最近傍内周到達点を得たパラメータを第１パラメータとして求める第１パラメータ算出ステップと、
３本の前記クロソイド曲線の全長を前記第１の固定長より長い第２の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも奥にある外周到達点を求め、前記外周到達点の中で前記終端に最も近い最近傍外周到達点を得たパラメータを第２パラメータとして求める第２パラメータ算出ステップと、
３本の前記クロソイド曲線の全長を前記第１の固定長と前記第２の固定長の中間の中間固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して最近傍内周到達点及び最近傍外周到達点を求め、前記最近傍内周到達点を得たパラメータを第３パラメータ、前記最近傍外周到達点を得たパラメータを第４パラメータとして求める第３・第４パラメータ算出ステップと、
前記第２パラメータで与えられる最近傍外周到達点と前記第４パラメータで与えられる最近傍外周到達点とを比べ、前記第４パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第２の固定長とし、前記第２パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べ、前記第３パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第１の固定長とする固定長更新ステップと、
前記固定長更新ステップにて前記第１の固定長または前記第２の固定長を更新しつつ、最近傍外周到達点および最近傍内周到達点を繰り返し求め、前記第１の固定長と前記第２の固定長の差が所定の閾値以下となったときに、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めるパラメータ決定ステップと、
を実行する軌道計画装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、人間が運転操作しなくても自動で走行できる自動車、自動運転車の軌道計画に関する。
【背景技術】
【０００２】
自動運転車の軌道計画の１つに、経路長ｓに対しての曲率κを計画する方法がある。これは曲率κと前輪操舵角δに強い関数関係があるためである。図１は、前後輪のスリップを無視したバイシクルモデルを示す図である。この図において、ｌは車両のホイール長である。
【０００３】
前後輪のスリップを無視したバイシクルモデルは次の微分方程式で表される。
【０００４】
【数１】
2019096269.tif 000003
なお、式（４）は車の最小回転半径に起因する条件である。
【０００５】
ここで車両の状態である姿勢（posture）をx位置、y位置、車両向きθ、曲率κの４変数からなる状態で定義する。つまり、本明細書において、姿勢（posture）は、位置を含む概念である。道路上を走る自動運転車両の運用形態として、予め道路上に決められたマイルストーンに沿って走行する形態があり得る。曲線路も想定すると、このマイルストーンには位置だけでなく、曲線路に対応した適切な車両向き、曲率が定められる。
【０００６】
図２は、ある初期状態にある車両を１つのマイルストーンに乗せる経路長を引数とする操舵計画の例を示す図である。車両がマイルストーンに到着する時には、位置だけではなく、車両向きと、前輪向きから定まる曲率も曲線路に合わせる必要がある。この操舵計画は、車両の初期の位置、向き、前輪向きからなる姿勢（posture）を始端（Start）として、道路上のマイルストーンにおける姿勢（posture）を終端（End）とする軌道計画として考えることができる。本明細書で対象とする軌道計画は、この種の経路長ｓに対しての曲率κを計画するものを対象とする。この種の軌道計画では、車両が止まることなく進行するために、曲率κ（ｓ）が経路長ｓの連続関数であることが要求される。
【０００７】
以下説明のため、軌道計画の始端（Start）における姿勢（posture）を、車両位置（ｘ，ｙ）＝（ｘ０，ｙ０）、車両向きθ＝θ０、曲率κ＝κ０とする。同様に終端（End）における姿勢（posture）を、車両位置（ｘ，ｙ）＝（ｘ１，ｙ１）、車両向きθ＝θ１、曲率κ＝κ１とする。車両位置を２次元ベクトルとして取り扱うときはＰ＝［ｘ，ｙ］Ｔと表す。
【０００８】
始端、終端の車両位置はそれぞれ、Ｐ０＝［ｘ０，ｙ０］Ｔ、Ｐ１＝［ｘ１，ｙ１］Ｔとなる。以下、始端の位置を「始点」、終端の位置を「終点」という。この種の始端の姿勢（posture）から、終端での姿勢（posture）への軌道計画は、自動運転に限定されるものではなく、多くの研究が行われている（特許文献１、非特許文献１、非特許文献２）。ただし、姿勢（posture）という用語が使われないことがある点に留意する。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開２０１１−１４５７９号公報
【非特許文献】
【００１０】
【非特許文献１】D. H. Shin and S. Singh. “Path generation for robot vehicles using composite clothoid segments,” Carnegie-Mellon Univ. Pittsburgh PA Robotics Inst., No. CMU-TR-90-31, 1990.
【非特許文献２】蘭豊礼, 玉井博文, and 牧野洋. "三連クロソイドによる自由点列補間." 精密工学会誌 76.10 (2010): 1194-1199.
【発明の概要】
【発明が解決しようとする課題】
【００１１】
上記したいずれの文献も、部分問題として、３本のクロソイドによる初期の姿勢（posture）から終端での姿勢（posture）への軌道計画を解いているが、問題設定に制約が与えられている。すなわち、Shinらによる研究ではクロソイドの長さに制約はないが、曲率の経路長による変化率の絶対値が３つのクロソイドにわたりすべて一定とするという強い制約を与えている。蘭らの研究では、３つのクロソイドの長さの比をあらかじめ固定するという強い制約を与えている。
【００１２】
本発明は、上記背景に鑑み、上記のような制約のないクロソイド長が可変の場合の始端の姿勢（posture）から終端での姿勢（posture）への軌道を計画する方法である。
【課題を解決するための手段】
【００１３】
本発明のプログラムは、３本のクロソイド曲線からなる軌道を計画するためのプログラムであって、コンピュータに、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、３本の前記クロソイド曲線の全長を固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索ステップと、前記粗探索ステップで求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップとを実行させる。
【００１４】
本発明のプログラムは、前記粗探索ステップにて、前記終端までの距離が所定の閾値以下となる最近傍到達点を与えるサンプルのパラメータが存在しなかった場合には、入力された条件を満たす解が存在しないと判定してもよい。
【００１５】
本発明のプログラムは、３本のクロソイド曲線からなる軌道を計画するためのプログラムであって、コンピュータに、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、３本の前記クロソイド曲線の全長を第１の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも手前にある内周到達点を求め、前記内周到達点の中で前記終端に最も近い最近傍内周到達点を得たパラメータを第１パラメータとして求める第１パラメータ算出ステップと、３本の前記クロソイド曲線の全長を前記第１の固定長より長い第２の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも奥にある外周到達点を求め、前記外周到達点の中で前記終端に最も近い最近傍外周到達点を得たパラメータを第２パラメータとして求める第２パラメータ算出ステップと、３本の前記クロソイド曲線の全長を前記第１の固定長と前記第２の固定長の中間の中間固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して最近傍内周到達点及び最近傍外周到達点を求め、前記最近傍内周到達点を得たパラメータを第３パラメータ、前記最近傍外周到達点を得たパラメータを第４パラメータとして求める第３・第４パラメータ算出ステップと、前記第２パラメータで与えられる最近傍外周到達点と前記第４パラメータで与えられる最近傍外周到達点とを比べ、前記第４パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第２の固定長とし、前記第２パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べ、前記第３パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第１の固定長とする固定長更新ステップと、前記固定長更新ステップにて前記第１の固定長または前記第２の固定長を更新しつつ、最近傍外周到達点および最近傍内周到達点を繰り返し求め、前記第１の固定長と前記第２の固定長の差が所定の閾値以下となったときに、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めるパラメータ決定ステップと、前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップとを実行させる。
【００１６】
本発明のプログラムは、前記固定長更新ステップにおいて、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べて、前記第１パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めてもよい。
【００１７】
本発明のプログラムは、前記第１パラメータ算出ステップにおける計算によって前記内周到達点が見つからなかった場合には、前記始端と前記終端とをつなぐクロソイド曲線が存在しないと判定してもよい。
【００１８】
本発明のプログラムは、前記第２パラメータ算出ステップにおける計算によって前記外周到達点が見つからなかった場合には、前記外周到達点が見つかるか、前記第２の固定長が所定の閾値以上になるまで、前記第２の固定長を長くして外周到達点を求める処理を行い、前記第２の固定長が所定の閾値以上になったときは、前記始端と前記終端とをつなぐクロソイド曲線が存在しないと判定してもよい。
【００１９】
本発明のプログラムにおいて、前記到達点関数は、前記車両の旋回曲率をパラメータとして含み、前記旋回曲率の最大値を拘束条件として有してもよい。
【００２０】
本発明の軌道計画方法は、軌道計画装置によって３本のクロソイド曲線からなる軌道を計画するための方法であって、前記軌道計画装置は、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、前記軌道計画装置は、前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、前記軌道計画装置は、３本の前記クロソイド曲線の全長を固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索ステップと、前記軌道計画装置は、前記粗探索ステップで求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップとを備える。
【００２１】
本発明の軌道計画方法は、軌道計画装置によって、３本のクロソイド曲線からなる軌道を計画するための方法であって、前記軌道計画装置は、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、前記軌道計画装置は、始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、前記軌道計画装置は、３本の前記クロソイド曲線の全長を第１の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも手前にある内周到達点を求め、前記内周到達点の中で前記終端に最も近い最近傍内周到達点を得たパラメータを第１パラメータとして求める第１パラメータ算出ステップと、前記軌道計画装置は、３本の前記クロソイド曲線の全長を前記第１の固定長より長い第２の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも奥にある外周到達点を求め、前記外周到達点の中で前記終端に最も近い最近傍外周到達点を得たパラメータを第２パラメータとして求める第２パラメータ算出ステップと、前記軌道計画装置は、３本の前記クロソイド曲線の全長を前記第１の固定長と前記第２の固定長の中間の中間固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して最近傍内周到達点及び最近傍外周到達点を求め、前記最近傍内周到達点を得たパラメータを第３パラメータ、前記最近傍外周到達点を得たパラメータを第４パラメータとして求める第３・第４パラメータ算出ステップと、前記軌道計画装置は、前記第２パラメータで与えられる最近傍外周到達点と前記第４パラメータで与えられる最近傍外周到達点とを比べ、前記第４パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第２の固定長とし、前記第２パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べ、前記第３パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第１の固定長とする固定長更新ステップと、前記軌道計画装置は、前記固定長更新ステップにて前記第１の固定長または前記第２の固定長を更新しつつ、最近傍外周到達点および最近傍内周到達点を繰り返し求め、前記第１の固定長と前記第２の固定長の差が所定の閾値以下となったときに、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めるパラメータ決定ステップと、前記軌道計画装置は、前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップとを備える。以下、この粗探索方法を「２分探索法」という。
【００２２】
本発明の軌道計画装置は、３本のクロソイド曲線からなる軌道を計画するための軌道計画装置であって、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力部と、前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶した記憶部と、３本の前記クロソイド曲線の全長を固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記記憶部から読み出した前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索部と、前記粗探索部で求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化部とを備える。
【００２３】
本発明の別の態様の軌道計画装置は、３本のクロソイド曲線からなる軌道を計画するための軌道計画装置であって、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力部と、始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶した記憶部と、複数の組合せをサンプルのパラメータを前記記憶部から読み出した前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索部と、前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化部とを備え、前記粗探索部は、上述した２分探索法を実行する。
【発明の効果】
【００２４】
本発明によれば、クロソイド長が可変の場合の始端の姿勢と終端の姿勢から、軌道を計画することができる。
【図面の簡単な説明】
【００２５】
【図１】前後輪のスリップを無視したバイシクルモデルを示す図である。
【図２】ある初期状態にある車両を１つのマイルストーンに乗せる経路長を引数とする操舵計画の例を示す図である。
【図３】３連クロソイドの軌道モデルを示す概念図である。
【図４】第１の実施の形態の軌道計画装置の構成を示す図である。
【図５】第１の実施の形態の軌道計画装置の動作を示すフローチャートである。
【図６】第１の実施の形態における初期値決定の処理を示すフローチャートである。
【図７】第２の実施の形態における粗探索処理の動作を示す図である。
【図８】固定全長下限ｈｔｏｔａｌＬＢを求める処理を示す図である。
【図９】固定全長上限ｈｔｏｔａｌＵＢを求める処理を示す図である。
【図１０】（ａ）内周到達点の範囲を示す図である。（ｂ）外周到達点の範囲を示す図である。
【図１１】第１実施例の軌道計画を行う始端と終端のデータを示す図である。
【図１２】ｈｔｏｔａｌ＝｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒ（ｈｔｏｔａｌ）を示す図である。
【図１３】ｈ１※，ｈ２※，ｈ３※，θｄ，１※から微小変化した次式で与えられる４パラメータｈ１※，ｈ２※，ｈ３※＋Δｈ３，θｄ，１※＋Δθｄ，１により、Ｐａｒｒ＋ΔＰａｒｒに近い位置に到達点を変更した図である。
【図１４】（ａ）ｈｔｏｔａｌ＝１・｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を示す図である。（ｂ）ｈｔｏｔａｌの長さを１・｜Ｐ１−Ｐ０｜から増やし、ｈｔｏｔａｌ＝２・｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を示す図である。
【図１５】ｈｔｏｔａｌＬＢとｈｔｏｔａｌＵＢの更新処理を行ったときの到達点の分布を示す図である。
【図１６】繰り返し最適化手法（主双対内点法）を行って得られた軌道を示す図である。
【図１７】別の各ケースにおいて計算された軌道を示す図である。
【発明を実施するための形態】
【００２６】
以下、本発明の実施の形態の軌道計画装置および軌道計画方法について図面を参照して説明する。まず、本実施の形態の軌道計画装置を説明する前提として、３連クロソイドの軌道について述べるが、まずは、３連クロソイドによる軌道のモデル化の準備段階として、ｘｙ座標原点を始点とする１セグメントのクロソイドの軌道について述べる。
【００２７】
（１セグメントのクロソイドの軌道）
この種のクロソイドの軌道は始端及び終端の位置と始端及び終端における車両の向きと曲率で一意に決定される。例えば、始端での向きと曲率を（θα、κα）、終端での向きと曲率を（θβ、κβ）とすると、クロソイドの１セグメント全経路長ｈは次式で表される。
【００２８】
【数２】
2019096269.tif 000004
１セグメント全経路長が定まるので、曲率κの軌道は次式で表される。ただし、Ｓは１セグメント全経路長hによる規格化経路長とする（Ｓ＝ｓ／ｈ）
【００２９】
【数３】
2019096269.tif 000005
ここでクロソイドの１セグメントにおける向きの変化θｄ＝θβ−θαを導入すると，次式を得る。 
【００３０】
【数４】
2019096269.tif 000006
車両向きθの軌道、経路長ｓに対する曲線の変化率η、sharpnessは、次式で表される。
【００３１】
【数５】
2019096269.tif 000007
【００３２】
曲線の変化率η、sharpnessは、１クロソイドセグメントにおいて定義から明らかなように一定である。車両向きθの軌道が決まれば式（１）（２）より、座標原点からの移動量を表すＰ（Ｓ）＝［ｘ（Ｓ），ｙ（Ｓ）］Ｔの軌道が次式で決定される。
【数６】
2019096269.tif 000008
式（１２）において、Ｓ＝１とおき、１セグメントクロソイドのｘｙ移動量Ｐｄ＝［ｘｄ，ｙｄ］Ｔが次式で与えられる。 
【数７】
2019096269.tif 000009
【００３３】
このＰｄ＝（θα，κα，ｈ，θｄ）を関数として、３連クロソイドによる軌道のモデル化で使用する。軌道計画の数値解法のために、式（１２）（１５）の計算について述べる。ただし、ＣＲ（・・・）、ＳＲ（・・・）以外の要素の数値計算は容易であるので省略する。ＣＲ（・・・）、ＳＲ（・・・）の計算は３つのケースに分ける。
【００３４】
［ケース１（θｄ−ｈκα＝０かつκα＝０）］
このケースは、直線運動に相当する。このケースも、定義上、クロソイドに含まれる。
【００３５】
【数８】
2019096269.tif 000010
【００３６】
［ケース２（θｄ−ｈκα＝０かつκα≠０）］
このケースは、円運動に相当する。このケースも、定義上、クロソイドに含まれる。
【数９】
2019096269.tif 000011
【００３７】
［ケース３（θｄ−ｈκα≠０）］
このケースは、フレネル積分（Ｆｒｅｓｎｅｌ  ｉｎｔｅｇｒａｌ）を含むため、計算が難しい。数値積分を使う方法もあるが、十分な精度を出すためには，計算量が大きくなる。以下の式を用いて計算する。
フレネル積分に相当するのは式（２３）（２４）の部分であり，近似の有効な領域の違う２つの近似公式を切り替えて用いる。
【数１０】
2019096269.tif 000012
ただし、
【００３８】
【数１１】
2019096269.tif 000013
【数１２】
2019096269.tif 000014
【数１３】
2019096269.tif 000015
【００３９】
以上の準備で、１セグメントクロソイドのｘｙ移動量Ｐｄ＝（θα，κα，ｈ，θｄ）は，セグメントの始端での向きθαと曲率κα、１セグメント全経路長ｈ、１セグメントでの向き変化θｄの４変数で関数として定義され、計算可能なことが示された。
【００４０】
引き続き、クロソイド長が可変の場合の３連クロソイドによる始端での姿勢（posture）から終端での姿勢（posture）への軌道計画を数値最適化問題として定式化する。
【００４１】
（３連クロソイドの軌道）
３連クロソイドの軌道の数値最適化問題への定式化は、次のアプローチをとる。
・第１のクロソイドの始端の姿勢を（ｘ０，ｙ０，θ０，κ０）とする。
・曲率θと向きκが連続となるように、３セグメントのクロソイドを接続する。
・第３のクロソイドの終端の姿勢を（ｘａｒｒ，ｙａｒｒ，θ１，κ１）とする。
・ｘａｒｒ＝ｘ１，ｙａｒｒ＝ｙ１となるまで、第１〜第３のクロソイドのパラメータを｜κ｜≦κｌｉｍｉｔの制約の下で数値最適化する。
【００４２】
図３は、このアプローチによる３連クロソイドの軌道モデルを示す概念図である。第ｋセグメント（ｋ＝１，２，３・・・）に関する変数を、以下のように定義する。
【００４３】
【数１４】
2019096269.tif 000016
【００４４】
進行する車両の軌道としては、前輪の舵角は経路に対して連続的に変化する必要がある。このため、車両の向きθと車両の曲率κは、セグメント間の端点において連続する。まず、車両向きθについて考えると、各セグメントの両端点での連続性から以下の式を満たす必要がある。
【数１５】
2019096269.tif 000017
【数１６】
2019096269.tif 000018
【００４５】
式（２５）〜式（４０）をまとめると、次の式を得る。
【数１７】
2019096269.tif 000019
【００４６】
８変数ｈ１，ｈ２，ｈ３，θｄ，１，θｄ，２，θｄ，３，κα，２，κα，３が式（４１）を満たせば、式（２５）〜式（４０）で定める軌道は、軌道計画問題の車両向きと曲率の連続性に関する条件を満たす。ここで、式（４１）は４本の等式拘束であり、本質的なパラメータは８変数から４変数に絞ることができる。本発明では、（ｈ１，ｈ２，ｈ３，θｄ，１）を３連クロソイドセグメントの４パラメータに選ぶ（図３参照）。各クロソイドのθｄ，２，θｄ，３，κα，２，κα，３，θα，２，θα，３は、式（４１）からパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の関数として次式で決定される。
【数１８】
2019096269.tif 000020
【００４７】
ｈ１（ｈ２＋ｈ３）≠０であれば、式（４２）〜式（４７）の右辺は、不定になることはない。本実施の形態では、ｈ１＞０，ｈ２＞０，ｈ３＞０を想定しているので、この条件を満たす。式（４４）より、｜κα，２｜＜κｌｉｍｉｔは、ｈ１，θｄ，１に関する以下の不等式拘束に変換される。
【数１９】
2019096269.tif 000021
【００４８】
式（４５）より、｜κα，３｜＜κｌｉｍｉｔは、ｈ１，ｈ２，ｈ３，θｄ，１に関する以下の不等式拘束に変換される。
【数２０】
2019096269.tif 000022
【００４９】
パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）を決めると、式（４２）〜式（４７）より、θα，ｋ，κα，ｋ，θｄ，ｋ（ｋ＝１，２，３）が定まる。さらに、３連クロソイドセグメントの到達点Ｐａｒｒが式（１５）より以下のように計算される。
【数２１】
2019096269.tif 000023
【００５０】
つまり（ｈ１，ｈ２，ｈ３，θｄ，１）のパラメータを使えば、車両向き及び曲率に関する条件は自動的に満たされるので、位置に関する条件Ｐａｒｒ＝Ｐ１を満たせば、所望の始端の姿勢（posture）から終端の姿勢（posture）への軌道が得られる。以下、（ｈ１，ｈ２，ｈ３，θｄ，１）によるパラメータ表現を４パラメータ表現という。
【００５１】
Ｐａｒｒ（ｈ１，ｈ２，ｈ３，θｄ，１）＝Ｐ１となる（ｈ１，ｈ２，ｈ３，θｄ，１）を探索する問題は数値最適化問題として定式化される。目的関数ｆｏｂｊ（Ｐａｒｒ）をＰａｒｒ−Ｐ１のユークリッド２ノルムの二乗値に選ぶ。このとき、数値最適化問題として以下のように記述できる。
【００５２】
【数２２】
2019096269.tif 000024
上記の最適化問題を解いて，最適化の結果、
【数２３】
2019096269.tif 000025
が達成できれば、解となる軌道が得られる。つまり、所望の始端の姿勢（posture）から終端の姿勢（posture）への軌道が得られる。
【００５３】
（第１実施の形態）
式（５５）のような最適化問題には、疑似ニュートン法に基づく、主双対内点法（ＰＤＩＰＭ）や遂次２次計画法（ＳＱＰ）などの繰り返し最適化手法が適用可能である。しかし、式（５５）の最適化問題は非線形非凸であるため、不適切な初期値を選ぶと、解が求まらないという問題が生じる。本実施の形態は、式（５５）の最適化問題において、繰り返し最適化手法に用いる適切な初期値を求める。
【００５４】
図４は、第１の実施の形態の軌道計画装置１の構成を示す図である。図４に示すように第１の実施の形態の軌道計画装置１は、入力部１０と、粗探索部１１と、最適化部１２と、出力部１３と、記憶部１４とを有している。
【００５５】
入力部１０は、軌道計画の条件として車両の始端および終端のデータの入力を受け付ける機能を有する。具体的には、車両の始端の姿勢（位置、向き、曲率）と、終端の姿勢（位置、向き、曲率）である。
【００５６】
記憶部１４には、始端から到達点を計算する到達点関数が記憶されている。到達点関数は、上述した式（５３）で示した関数であり、（ｈ１，ｈ２，ｈ３，θｄ，１）の４つのパラメータを有する関数である。
【００５７】
粗探索部１１は、到達点関数に適当なサンプルパラメータを適用して求めた到達点の中から、終端に最も近い最近傍到達点を得たパラメータを求める。粗探索部１１は、ｈｔｏｔａｌ＝ｈ１＋ｈ２＋ｈ３を固定した際のパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の集合により、次式で定義される到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を求める。
【００５８】
【数２４】
2019096269.tif 000026
【００５９】
この集合が空であれば、適切な初期値はなしと判定し、空でなければ、到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）の中で終端位置Ｐ１に最も近い要素（最近傍到達点）に対応する４パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）を繰り返し最適化手法の初期値として用いる。なお、式中のΔθｆａｎは、到達点Ｐａｒｒが存在すべき角度範囲を規定するものであり、例えば、π／４である。
【００６０】
最適化部１２は、粗探索部１１にて求めたパラメータを初期値として用いて、上述した式（５５）を満たすパラメータの最適化処理を行うことにより、到達点の姿勢が終端の姿勢になるようなパラメータを求める機能を有する。このパラメータにより車両が通るべき軌道が計画される。続いて、最適化部１２は、求めたパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）から、各位置における旋回曲率κも求める機能を有する。
【００６１】
出力部１３は、最適化部１２にて求めたパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）および旋回曲率κを車両の制御を行う自動運転制御部（図示せず）に対して出力する。
【００６２】
図５は、第１の実施の形態の軌道計画装置１の動作を示すフローチャートである。軌道計画装置１は、軌道計画の条件として車両の始端および終端のデータの入力を受け付ける（Ｓ１０）。軌道計画装置１は、記憶部１４から到達点関数を読み出し（Ｓ１１）、到達点関数を用いて初期値を決定する（Ｓ１２）。
【００６３】
図６は、第１の実施の形態における初期値決定の処理を示すフローチャートである。粗探索部１１は、到達点関数に適当なパラメータを設定して、終端に最も近い到達点（最近傍到達点）を得ることができるパラメータをラフに探索する。具体的には、粗探索部１１は、３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）を始端位置である始点Ｐ０から終端位置である終点Ｐ１までの直線距離ｈｔｏｔａｌに固定し（Ｓ２０）、４つのパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の組合せを、例えば１０００通り選び、各パラメータを到達点関数に適用したときの到達点を求める（Ｓ２１）。
【００６４】
粗探索部１１が到達点の近傍到達点を見つけた場合には（Ｓ２２でＹＥＳ）、終点Ｐ１の最近傍の到達点を得たパラメータを初期値として決定する（Ｓ２３）。もし、近傍到達点が見つからなかった場合には（Ｓ２２でＮＯ）、始端から終端に至る軌道が存在しないと判断する（Ｓ２４）。
【００６５】
図５に戻って説明を続ける。軌道計画装置１は初期値を決定すると、決定された初期値を用いてパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の最適化を行い、始端から終端への軌道を描く最適パラメータを算出する（Ｓ１３）。また、軌道計画装置１は、求めた最適パラメータに基づいて、各位置における旋回曲率κも求める。続いて、軌道計画装置１は、パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）および旋回曲率κを自動運転制御部に対して出力する（Ｓ１４）。
【００６６】
以上、軌道計画装置１の構成及び動作について説明したが、上記した軌道計画装置１のハードウェアの例は、ＣＰＵ、ＲＡＭ、ＲＯＭ、ハードディスク、ディスプレイ、キーボード、マウス、通信インターフェース等を備えたコンピュータである。上記した各機能を実現するモジュールを有するプログラムをＲＡＭまたはＲＯＭに格納しておき、ＣＰＵによって当該プログラムを実行することによって、上記した軌道計画装置１が実現される。このようなプログラムも本発明の範囲に含まれる。
【００６７】
（第２実施の形態）
次に、第２の実施の形態の軌道計画装置について説明する。第２の実施の形態の軌道計画装置の基本的な構成は、第１の実施の形態の軌道計画装置１の構成と同じであるが（図４）、粗探索部１１における探索処理が異なる。
【００６８】
第２の実施の形態における粗探索の処理の概要は、以下のとおりである。到達点の分布が始点Ｐ０から見て終点Ｐ１の手前に生じるように３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）を固定する。例えば、３連クロソイドの始点Ｐ０から終点Ｐ１までの直線距離とすれば、到達点の分布は始点Ｐ０から見て終点Ｐ１の手前に生じることになる。この固定長をｈｔｏｔａｌＬＢとする。
【００６９】
また、到達点の分布が始点Ｐ０から見て終点Ｐ１の奥に生じるように３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）を固定する。例えば、始点Ｐ０から終点Ｐ１までの直線距離の２倍とすれば、通常、到達点の分布は始点Ｐ０から見て終点Ｐ１の奥に生じることになる。この固定長をｈｔｏｔａｌＵＢとする。
【００７０】
そして、ｈｔｏｔａｌＬＢ≦ｈｔｏｔａｌＵＢの関係を保ちながら，２分探索により、ｈｔｏｔａｌＬＢ、ｈｔｏｔａｌＵＢを更新し、その差ｈｔｏｔａｌＵＢ−ｈｔｏｔａｌＬＢが十分小さくなったところで、到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌＬＢ）、ＳＰａｒｒＧ（ｈｔｏｔａｌＵＢ）の中で終端位置Ｐ１に最も近い要素に対応する４パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）を繰り返し最適化手法の初期値として用いる。
【００７１】
図７は、第２の実施の形態における粗探索処理の動作を示す図である。粗探索部１１は、３連クロソイドの全長を下限ｈｔｏｔａｌＬＢに固定し、適当に選んだ複数のパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の組合せについて到達点を求め、到達点の中から最近傍内周到達点を求める（Ｓ４０）。ここで、最近傍内周到達点とは、図１０（ａ）に示す範囲に含まれる到達点であって、終点Ｐ１に最も近い到達点である。なお、扇形の中心角θｆａｎは、あらかじめ設定された値であり、例えば、π／４とする。続いて、粗探索部１１は、３連クロソイドの全長を上限ｈｔｏｔａｌＵＢに固定し、適当に選んだ複数のパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の組合せについて到達点を求め、到達点の中から最近傍外周到達点を求める（Ｓ４１）。ここで、最近傍内周到達点とは、図１０（ｂ）に示す範囲に含まれる到達点であって、終点Ｐ１に最も近い到達点である。
【００７２】
図８は、最近傍内周到達点を求める処理を示す図である。粗探索部１１は、始点Ｐ０と終点Ｐ１の直線距離を３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）の下限ｈｔｏｔａｌＬＢとし（Ｓ４０）、この条件を満たす複数のパラメータを適当に選んで、そのパラメータに対応する到達点を計算する（Ｓ４１）。粗探索部１１は、求めた到達点の中に、内周到達点があるか否かを判定し（Ｓ４２）、内周到達点がある場合には（Ｓ４２でＹＥＳ）、その内周到達点のうちで、終点Ｐ１に最も近い、最近傍内周到達点と対応するパラメータを返す（Ｓ４３）。内周到達点の有無の判定において、内周到達点がないと判定された場合には（Ｓ４２でＮＯ）、始端と終端とをつなぐ３連クロソイドの解が存在しないと判定する（Ｓ４４）。
【００７３】
図９は、最近傍外周到達点を求める処理を示す図である。粗探索部１１は、始点Ｐ０と終点Ｐ１の直線距離に定数Ｃ２（＞１）を乗じた値を３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）の上限ｈｔｏｔａｌＵＢとし（Ｓ５０）、この条件を満たす複数のパラメータを適当に選んで、そのパラメータに対応する到達点を計算する（Ｓ５１）。粗探索部１１は、求めた到達点の中に、外周到達点があるか否かを判定し（Ｓ５２）、外周到達点がある場合には（Ｓ５２でＹＥＳ）、その外周到達点のうちで、終点Ｐ１に最も近い、最近傍外周到達点と対応するパラメータを返す（Ｓ５３）。外周到達点の有無の判定において、外周到達点がないと判定された場合には（Ｓ５２でＮＯ）、上限ｈｔｏｔａｌＵＢを更新して上述の処理を繰り返す。
【００７４】
具体的には、始点Ｐ０と終点Ｐ１の直線距離に定数Ｃ３（＞０）を乗じた値を上限ｈｔｏｔａｌＵＢに加算して、新たな上限ｈｔｏｔａｌＵＢとする（Ｓ５４）。この新たな上限ｈｔｏｔａｌＵＢが所定の閾値より大きくない場合には（Ｓ５５でＮＯ）、新たな上限ｈｔｏｔａｌＵＢを用いて外周到達点を求める処理を行う（Ｓ５１）。新たな上限ｈｔｏｔａｌＵＢが所定の閾値より大きい場合には（Ｓ５５でＹＥＳ）、始端と終端とをつなぐ３連クロソイドの解が存在しないと判定する（Ｓ５６）。
【００７５】
図７に戻って、粗探索部１１が行う動作について説明する。粗探索部１１は、上限ｈｔｏｔａｌＵＢと下限ｈｔｏｔａｌＬＢとの差分の距離が所定の閾値より小さいか否かを判定する（Ｓ３２）。上限ｈｔｏｔａｌＵＢと下限ｈｔｏｔａｌＬＢとの差分の距離が閾値より小さい場合には（Ｓ３２でＹＥＳ）、３連クロソイドの全長を上限ｈｔｏｔａｌＵＢに固定して求めた最近傍外周到達点と、下限ｈｔｏｔａｌＬＢに固定して求めた最近傍内周到達点のうち、終点Ｐ１に近い到達点を与えたパラメータを初期値とする（Ｓ３９）。
【００７６】
最初の段階では、上限ｈｔｏｔａｌＵＢと下限ｈｔｏｔａｌＬＢとの差分の距離は閾値より小さくはないので（Ｓ３２でＮＯ）、ステップＳ３３の処理に進む。粗探索部１１は、上限ｈｔｏｔａｌＵＢと下限ｈｔｏｔａｌＬＢとの距離の中間固定長を算出し（Ｓ３３）、この中間固定長を用いて、最近傍外周到達点と、最近傍内周到達点を求める（Ｓ３４）。
【００７７】
続いて、粗探索部１１は、３連クロソイドの全長を上限ｈｔｏｔａｌＵＢに固定して求めた最近傍外周到達点よりも、中間固定長に固定して求めた最近傍外周到達点の方が終点Ｐ１に近いかを判定する（Ｓ３５）。この判定結果、中間固定長で求めた方が終点Ｐ１に近い場合には（Ｓ３５でＹＥＳ）、粗探索部１１は、中間固定長を新たな上限ｈｔｏｔａｌＵＢに設定する（Ｓ３６）。
【００７８】
上限ｈｔｏｔａｌＵＢで求めた最近傍外周到達点の方が終点Ｐ１に近い場合には（Ｓ３５でＮＯ）、粗探索部１１は、３連クロソイドの全長を下限ｈｔｏｔａｌＬＢに固定して求めた最近傍内周到達点よりも、中間固定長に固定して求めた最近傍内周到達点の方が終点Ｐ１に近いかを判定する（Ｓ３７）。この判定の結果、中間固定長で求めた方が終点Ｐ１に近い場合には（Ｓ３７でＹＥＳ）、粗探索部１１は、中間固定長を新たな下限ｈｔｏｔａｌＬＢに設定し（Ｓ３８）、粗探索部１１は、上限ｈｔｏｔａｌＵＢと下限ｈｔｏｔａｌＬＢとの差分の距離が所定の閾値より小さいか否かを判定する処理（Ｓ３２）に戻る。
【００７９】
３連クロソイドの全長を下限ｈｔｏｔａｌＬＢに固定して求めた最近傍外周到達点よりも、中間固定長に固定して求めた最近傍外周到達点の方が終点Ｐ１に近いか否かの判定において、下限ｈｔｏｔａｌＬＢで求めた方が終点Ｐ１に近い場合には（Ｓ３７でＮＯ）、３連クロソイドの全長を上限ｈｔｏｔａｌＵＢに固定して求めた最近傍外周到達点と、下限ｈｔｏｔａｌＬＢに固定して求めた最近傍内周到達点のうち、終点Ｐ１に近い到達点を与えたパラメータを初期値とする（Ｓ３９）。
【実施例】
【００８０】
以下では、具体的に始端および終端における車両の姿勢のデータを与えて、軌道計画を行った例を示す。
【００８１】
（第１実施例）
図１１は、第１実施例の軌道計画を行う始端と終端のデータを示す図である。始端の位置Ｐ０（ｘ０，ｙ０）は（０，０）、終端の位置Ｐ１（ｘ１，ｙ１）は（２５．６１，４．３９）である。始端における車両の向きθ０は０［ｒａｄ］、車両の旋回曲率κ０は０［１／ｍ］であり、終端における車両の向きθ１は０．７８５４［ｒａｄ］、車両の旋回曲率κ１は０．０６６７［１／ｍ］である。
【００８２】
上で説明したとおり、３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）をｈｔｏｔａｌに固定したときのパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の集合により、次式で定義される到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を考える。
【数２５】
2019096269.tif 000027
【００８３】
図１２は、図１１のケースにおいて、ｈｔｏｔａｌ＝｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を示す図である。ただし、ここでΔθｆａｎは、π／４とした。また、ＳＰａｒｒＧ（ｈｔｏｔａｌ）は厳密には無限集合であるため、近似計算により、次のように求める。
【００８４】
（１）（ｈ１，ｈ２，ｈ３，θｄ，１）の下記の集合Ｓ４Ｐａｒａ（ｈｔｏｔａｌ）からサンプリングした有限のパラメータ集合ＳＲＳ４Ｐａｒａ（ｈｔｏｔａｌ）を求める。なお、θＲａｎｇｅは、あらかじめ定めたパラメータであり、ここではｐｉ／２としている。
【数２６】
2019096269.tif 000028
【００８５】
（２）Ｓ４Ｐａｒａ（ｈｔｏｔａｌ）の全要素（ｈ１，ｈ２，ｈ３，θｄ，１）∈Ｓ４Ｐａｒａ（ｈｔｏｔａｌ）に対して，Ｐａｒｒ（ｈ１，ｈ２，ｈ３，θｄ，１）を求め、このうち、式（７０）の到達点ＳＰａｒｒＧ（ｈｔｏｔａｌ）の定義を満たす要素Ｐａｒｒの集合をＳＰａｒｒＧ（ｈｔｏｔａｌ）とする。図１２に示すように、ＳＰａｒｒＧ（ｈｔｏｔａｌ）は、おおよそＰ１−Ｐ０の方向にある到達点集合となる。
【００８６】
式（１５）より、Ｐｄ（θα，κα，ｈ，θｄ）は、θα，κα，ｈ，θｄに関して連続であり、式（４２）〜（４７）より、θｄ，２，θｄ，３，κα，２，κα，３，θα，２，θα，３は（ｈ１，ｈ２，ｈ３，θｄ，１）に関して連続であることから、式（５３）が示すＰａｒｒ（ｈ１，ｈ２，ｈ３，θｄ，１）は、ｈ１，ｈ２，ｈ３，θｄ，１に関して連続である。
【００８７】
したがって、ある４パラメータｈ１※，ｈ２※，ｈ３※，θｄ，１※に対する到達点ＰａｒｒＰａｒｒ（ｈ１，ｈ２，ｈ３，θｄ，１）の近傍位置Ｐａｒｒ＋ΔＰａｒｒを考えると，これに対応する４パラメータｈ１，ｈ２，ｈ３，θｄ，１はｈ１※，ｈ２※，ｈ３※，θｄ，１※を微小に変化したもので得られる。例えば，具体的には図１３に示すように，ｈ１※，ｈ２※，ｈ３※，θｄ，１※から微小変化した次式で与えられる４パラメータｈ１※，ｈ２※，ｈ３※＋Δｈ３，θｄ，１※＋Δθｄ，１により、Ｐａｒｒ＋ΔＰａｒｒに近い位置に到達点を変更することができる。
【数２７】
2019096269.tif 000029
【００８８】
逆に言うと，式（５５）の最適化問題に解（ｈ１※，ｈ２※，ｈ３※，θｄ，１※）があれば，その近傍には（ｈ１※，ｈ２※，ｈ３※，θｄ，１※）から変化したパラメータ集合に対応する到達点集合が存在する。
【００８９】
ＳＰａｒｒＧ（ｈｔｏｔａｌ）はこの到達点集合に対応するので、ＳＰａｒｒＧ（ｈｔｏｔａｌ）が空であれば、間接的に式（５５）の最適化問題が解を持たないことが推測される。
【００９０】
そこで，第１実施例では，ＳＰａｒｒＧ（ｈｔｏｔａｌ）が空集合ならば、適切な初期値がないと判断する。ＳＰａｒｒＧ（ｈｔｏｔａｌ）に要素があれば、終点Ｐ１の最近傍到達点であるＰａｒｒ∈ＳＰａｒｒＧ（ｈｔｏｔａｌ）に対応するパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）を初期値に使う。
【００９１】
この方法によれば、ランダムに初期値を選んだ場合に比べて、終点Ｐ１に近い位置から繰り返し最適化手法がスタートできるために、極小値に捕らわれることが少ない。また、いきなり繰り返し最適化手法を適用した場合に、解の有無にかかわらず、問題の非凸性により収束せずに計算が終了しないこともあるが、本実施例では、到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を有限のパラメータ集合ＳＲＳ４ｐａｒａ（ｈｔｏｔａｌ）から近似計算するため、必ず有限時間で解の有無が推定される。
【００９２】
（第２実施例）
始点Ｐ０から到達点Ｐａｒｒの距離｜Ｐａｒｒ−Ｐ０｜は、３連クロソイドセグメントが直線の場合に最大となるので、常に以下の式が成り立つ。
｜Ｐａｒｒ（ｈ１，ｈ２，ｈ３，θｄ，１）−Ｐ０｜≦ｈ１＋ｈ２＋ｈ３＝ｈｔｏｔａｌ
【００９３】
図１４（ａ）は、ｈｔｏｔａｌ＝１・｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を示す図であり、先に示した図１２と同じである。図１４（ａ）に見られるように、ＳＰａｒｒＧ（ｈｔｏｔａｌ）のすべての要素は必ず始点Ｐ０からみて終点Ｐ１の手前に分布しており、上記の式を裏付けている。図１４（ｂ）は、ｈｔｏｔａｌの長さを１・｜Ｐ１−Ｐ０｜から増やし、ｈｔｏｔａｌ＝２・｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を示す図である。この場合、到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）には始点Ｐ０からみて終点Ｐ１の奥に分布する。
【００９４】
図１４（ａ）及び図１４（ｂ）に示す結果は、式（５５）が最適解（ｈ１※，ｈ２※，ｈ３※，θｄ，１※）を持つときの３連クロソイドの全長ｈｔｏｔａｌ※をｈｔｏｔａｌ※＝ｈ１※＋ｈ２※＋ｈ３※とする。ｈｔｏｔａｌ＜ｈｔｏｔａｌ※とｈｔｏｔａｌを短くすると、ＳＰａｒｒＧ（ｈｔｏｔａｌ）はＰ０から見てＰ１の手前に分布するものが発生する。逆に、ｈｔｏｔａｌ＞ｈｔｏｔａｌ※とｈｔｏｔａｌを長くすると、ＳＰａｒｒＧ（ｈｔｏｔａｌ）は始点Ｐ０から見て終点Ｐ１の奥に分布するものが発生する。
【００９５】
ＳＰａｒｒＧ（ｈｔｏｔａｌ）が終点Ｐ１の手前に分布するｈｔｏｔａｌを下限ｈｔｏｔａｌＬＢ、ＳＰａｒｒＧ（ｈｔｏｔａｌ）が終点Ｐ１の奥に分布するｈｔｏｔａｌを上限ｈｔｏｔａｌＵＢとする。第２実施例は、下限ｈｔｏｔａｌＬＢと上限ｈｔｏｔａｌＵＢとのギャップを２分探索により狭めることで、より良い初期値を得るものである。
【００９６】
始点Ｐ０からみて終点Ｐ１の手前に分布する到達点を説明するために、次式で定義される内周到達点の集合ＳＰａｒｒＧＩｎ（ｈｔｏｔａｌ）とそれに対応する４パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の集合ＳＲＳ４ｐａｒｒＩｎ（ｈｔｏｔａｌ）を導入する。
【数２８】
2019096269.tif 000030
【００９７】
式（２００）は、理論的には無限の要素を持つ集合となるが、第１実施例で示したように、サンプリングした有限のパラメータ集合ＳＲＳ４ｐａｒｒ（ｈｔｏｔａｌ）から求めた近似有限集合とする。
【００９８】
さらに、ＳＰａｒｒＧＩｎ（ｈｔｏｔａｌ）のうち、終点Ｐ１に最も近い最近傍内周到達点をＰｐａｒＧＩｎＮＮ（ｈｔｏｔａｌ）、その４パラメータを（ｈ１，ｈ２，ｈ３，θｄ，１）ＩｎＮＮ（ｈｔｏｔａｌ）とする。式で書くと以下のようになる。
【数２９】
2019096269.tif 000031
【００９９】
同様に、始点Ｐ０からみて終点Ｐ１の奥に分布する到達点に関して説明するために，次式で定義される，到達点集合ＳＰａｒｒＧＯｕｔ（ｈｔｏｔａｌ）とそれに対応する４パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の集合ＳＲＳ４ｐａｒａＯｕｔ（ｈｔｏｔａｌ）を導入する。
【数３０】
2019096269.tif 000032
【０１００】
同様にＳＰａｒｒＧＯｕｔ（ｈｔｏｔａｌ）のうち、終点Ｐ１に最も近い最近傍外周到達点をＰｐａｒＧＯｕｔＮＮ（ｈｔｏｔａｌ）、その４パラメータを（ｈ１，ｈ２，ｈ３，θｄ，１）ＯｕｔＮＮ（ｈｔｏｔａｌ）とする。式で書くと以下のようになる。
【数３１】
2019096269.tif 000033
【０１０１】
ここで示した最近傍内周到達点、最近傍外周到達点を用いて、図７に示す２分探索の処理によって探索範囲を狭めていき、複数のサンプルパラメータの中から、終点Ｐ１の最近傍に到達する４パラメータを求める。
【０１０２】
図１５は、図１１に示したケースに対して、上記したアルゴリズムによってｈｔｏｔａｌＬＢとｈｔｏｔａｌＵＢの更新処理を行ったときの到達点の分布を示す図である。図１６は、上記した処理によって求めた初期値を用いて、繰り返し最適化手法（主双対内点法）を行って得られた軌道を示す図である。図１６に示すように、適切な初期値からスタートすることによって、式（５５）に示す最適化問題を適切に解けることが確認された。
また、図１１以外の３つのケースにおいて、本実施の形態の方法で取得した初期値を用いて軌道計画を行った例を示す。
【０１０３】
【表１】
2019096269.tif 000034
【０１０４】
図１７は、上記の各ケースにおいて計算された軌道である。図１７に示すようにいずれも適切に繰り返し最適化が行われ、軌道が得られた。
【産業上の利用可能性】
【０１０５】
本発明は、車両の軌道を計画する装置等として有用である。
【符号の説明】
【０１０６】
１  軌道計画装置
１０  入力部
１１  粗探索部
１２  最適化部
１３  出力部
１４  記憶部
【図１】
2019096269.tif 000035
【図２】
2019096269.tif 000036
【図３】
2019096269.tif 000037
【図４】
2019096269.tif 000038
【図５】
2019096269.tif 000039
【図６】
2019096269.tif 000040
【図７】
2019096269.tif 000041
【図８】
2019096269.tif 000042
【図９】
2019096269.tif 000043
【図１０】
2019096269.tif 000044
【図１１】
2019096269.tif 000045
【図１２】
2019096269.tif 000046
【図１３】
2019096269.tif 000047
【図１４】
2019096269.tif 000048
【図１５】
2019096269.tif 000049
【図１６】
2019096269.tif 000050
【図１７】
2019096269.tif 000051