JP6941039B2 - 軌道計画装置、軌道計画方法、およびプログラム - Google Patents

軌道計画装置、軌道計画方法、およびプログラム Download PDF

Info

Publication number
JP6941039B2
JP6941039B2 JP2017227756A JP2017227756A JP6941039B2 JP 6941039 B2 JP6941039 B2 JP 6941039B2 JP 2017227756 A JP2017227756 A JP 2017227756A JP 2017227756 A JP2017227756 A JP 2017227756A JP 6941039 B2 JP6941039 B2 JP 6941039B2
Authority
JP
Japan
Prior art keywords
parameter
arrival point
fixed length
point
nearest neighbor
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2017227756A
Other languages
English (en)
Other versions
JP2019096269A (ja
Inventor
高野　岳
岳 高野
真人 大林
真人 大林
敬祐 宇土
敬祐 宇土
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Denso IT Laboratory Inc
Original Assignee
Denso IT Laboratory Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Denso IT Laboratory Inc filed Critical Denso IT Laboratory Inc
Priority to JP2017227756A priority Critical patent/JP6941039B2/ja
Publication of JP2019096269A publication Critical patent/JP2019096269A/ja
Application granted granted Critical
Publication of JP6941039B2 publication Critical patent/JP6941039B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Description

本発明は、人間が運転操作しなくても自動で走行できる自動車、自動運転車の軌道計画に関する。
自動運転車の軌道計画の１つに、経路長ｓに対しての曲率κを計画する方法がある。これは曲率κと前輪操舵角δに強い関数関係があるためである。図１は、前後輪のスリップを無視したバイシクルモデルを示す図である。この図において、ｌは車両のホイール長である。
前後輪のスリップを無視したバイシクルモデルは次の微分方程式で表される。
Figure 0006941039
 なお、式（４）は車の最小回転半径に起因する条件である。
ここで車両の状態である姿勢（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）への軌道を計画する方法である。
本発明のプログラムは、３本のクロソイド曲線からなる軌道を計画するためのプログラムであって、コンピュータに、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、３本の前記クロソイド曲線の全長を固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索ステップと、前記粗探索ステップで求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップとを実行させる。
本発明のプログラムは、前記粗探索ステップにて、前記終端までの距離が所定の閾値以下となる最近傍到達点を与えるサンプルのパラメータが存在しなかった場合には、入力された条件を満たす解が存在しないと判定してもよい。
本発明のプログラムは、３本のクロソイド曲線からなる軌道を計画するためのプログラムであって、コンピュータに、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、３本の前記クロソイド曲線の全長を第１の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも手前にある内周到達点を求め、前記内周到達点の中で前記終端に最も近い最近傍内周到達点を得たパラメータを第１パラメータとして求める第１パラメータ算出ステップと、３本の前記クロソイド曲線の全長を前記第１の固定長より長い第２の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも奥にある外周到達点を求め、前記外周到達点の中で前記終端に最も近い最近傍外周到達点を得たパラメータを第２パラメータとして求める第２パラメータ算出ステップと、３本の前記クロソイド曲線の全長を前記第１の固定長と前記第２の固定長の中間の中間固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して最近傍内周到達点及び最近傍外周到達点を求め、前記最近傍内周到達点を得たパラメータを第３パラメータ、前記最近傍外周到達点を得たパラメータを第４パラメータとして求める第３・第４パラメータ算出ステップと、前記第２パラメータで与えられる最近傍外周到達点と前記第４パラメータで与えられる最近傍外周到達点とを比べ、前記第４パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第２の固定長とし、前記第２パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べ、前記第３パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第１の固定長とする固定長更新ステップと、前記固定長更新ステップにて前記第１の固定長または前記第２の固定長を更新しつつ、最近傍外周到達点および最近傍内周到達点を繰り返し求め、前記第１の固定長と前記第２の固定長の差が所定の閾値以下となったときに、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めるパラメータ決定ステップと、前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップとを実行させる。
本発明のプログラムは、前記固定長更新ステップにおいて、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べて、前記第１パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めてもよい。
本発明のプログラムは、前記第１パラメータ算出ステップにおける計算によって前記内周到達点が見つからなかった場合には、前記始端と前記終端とをつなぐクロソイド曲線が存在しないと判定してもよい。
本発明のプログラムは、前記第２パラメータ算出ステップにおける計算によって前記外周到達点が見つからなかった場合には、前記外周到達点が見つかるか、前記第２の固定長が所定の閾値以上になるまで、前記第２の固定長を長くして外周到達点を求める処理を行い、前記第２の固定長が所定の閾値以上になったときは、前記始端と前記終端とをつなぐクロソイド曲線が存在しないと判定してもよい。
本発明のプログラムにおいて、前記到達点関数は、前記車両の旋回曲率をパラメータとして含み、前記旋回曲率の最大値を拘束条件として有してもよい。
本発明の軌道計画方法は、軌道計画装置によって３本のクロソイド曲線からなる軌道を計画するための方法であって、前記軌道計画装置は、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、前記軌道計画装置は、前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、前記軌道計画装置は、３本の前記クロソイド曲線の全長を固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索ステップと、前記軌道計画装置は、前記粗探索ステップで求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップとを備える。
本発明の軌道計画方法は、軌道計画装置によって、３本のクロソイド曲線からなる軌道を計画するための方法であって、前記軌道計画装置は、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、前記軌道計画装置は、始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、前記軌道計画装置は、３本の前記クロソイド曲線の全長を第１の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも手前にある内周到達点を求め、前記内周到達点の中で前記終端に最も近い最近傍内周到達点を得たパラメータを第１パラメータとして求める第１パラメータ算出ステップと、前記軌道計画装置は、３本の前記クロソイド曲線の全長を前記第１の固定長より長い第２の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも奥にある外周到達点を求め、前記外周到達点の中で前記終端に最も近い最近傍外周到達点を得たパラメータを第２パラメータとして求める第２パラメータ算出ステップと、前記軌道計画装置は、３本の前記クロソイド曲線の全長を前記第１の固定長と前記第２の固定長の中間の中間固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して最近傍内周到達点及び最近傍外周到達点を求め、前記最近傍内周到達点を得たパラメータを第３パラメータ、前記最近傍外周到達点を得たパラメータを第４パラメータとして求める第３・第４パラメータ算出ステップと、前記軌道計画装置は、前記第２パラメータで与えられる最近傍外周到達点と前記第４パラメータで与えられる最近傍外周到達点とを比べ、前記第４パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第２の固定長とし、前記第２パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べ、前記第３パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第１の固定長とする固定長更新ステップと、前記軌道計画装置は、前記固定長更新ステップにて前記第１の固定長または前記第２の固定長を更新しつつ、最近傍外周到達点および最近傍内周到達点を繰り返し求め、前記第１の固定長と前記第２の固定長の差が所定の閾値以下となったときに、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めるパラメータ決定ステップと、前記軌道計画装置は、前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップとを備える。以下、この粗探索方法を「２分探索法」という。
本発明の軌道計画装置は、３本のクロソイド曲線からなる軌道を計画するための軌道計画装置であって、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力部と、前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶した記憶部と、３本の前記クロソイド曲線の全長を固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記記憶部から読み出した前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索部と、前記粗探索部で求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化部とを備える。
本発明の別の態様の軌道計画装置は、３本のクロソイド曲線からなる軌道を計画するための軌道計画装置であって、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力部と、始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶した記憶部と、複数の組合せをサンプルのパラメータを前記記憶部から読み出した前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索部と、前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化部とを備え、前記粗探索部は、上述した２分探索法を実行する。
本発明によれば、クロソイド長が可変の場合の始端の姿勢と終端の姿勢から、軌道を計画することができる。
前後輪のスリップを無視したバイシクルモデルを示す図である。 ある初期状態にある車両を１つのマイルストーンに乗せる経路長を引数とする操舵計画の例を示す図である。 ３連クロソイドの軌道モデルを示す概念図である。 第１の実施の形態の軌道計画装置の構成を示す図である。 第１の実施の形態の軌道計画装置の動作を示すフローチャートである。 第１の実施の形態における初期値決定の処理を示すフローチャートである。 第２の実施の形態における粗探索処理の動作を示す図である。 固定全長下限ｈｔｏｔａｌＬＢを求める処理を示す図である。 固定全長上限ｈｔｏｔａｌＵＢを求める処理を示す図である。 （ａ）内周到達点の範囲を示す図である。（ｂ）外周到達点の範囲を示す図である。 第１実施例の軌道計画を行う始端と終端のデータを示す図である。 ｈｔｏｔａｌ＝｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒ（ｈｔｏｔａｌ）を示す図である。 ｈ１ ※，ｈ２ ※，ｈ３ ※，θｄ，１ ※から微小変化した次式で与えられる４パラメータｈ１ ※，ｈ２ ※，ｈ３ ※＋Δｈ３，θｄ，１ ※＋Δθｄ，１により、Ｐａｒｒ＋ΔＰａｒｒに近い位置に到達点を変更した図である。 （ａ）ｈｔｏｔａｌ＝１・｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を示す図である。（ｂ）ｈｔｏｔａｌの長さを１・｜Ｐ１−Ｐ０｜から増やし、ｈｔｏｔａｌ＝２・｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を示す図である。 ｈｔｏｔａｌＬＢとｈｔｏｔａｌＵＢの更新処理を行ったときの到達点の分布を示す図である。 繰り返し最適化手法（主双対内点法）を行って得られた軌道を示す図である。 別の各ケースにおいて計算された軌道を示す図である。
以下、本発明の実施の形態の軌道計画装置および軌道計画方法について図面を参照して説明する。まず、本実施の形態の軌道計画装置を説明する前提として、３連クロソイドの軌道について述べるが、まずは、３連クロソイドによる軌道のモデル化の準備段階として、ｘｙ座標原点を始点とする１セグメントのクロソイドの軌道について述べる。
（１セグメントのクロソイドの軌道）
この種のクロソイドの軌道は始端及び終端の位置と始端及び終端における車両の向きと曲率で一意に決定される。例えば、始端での向きと曲率を（θα、κα）、終端での向きと曲率を（θβ、κβ）とすると、クロソイドの１セグメント全経路長ｈは次式で表される。
Figure 0006941039
 １セグメント全経路長が定まるので、曲率κの軌道は次式で表される。ただし、Ｓは１セグメント全経路長hによる規格化経路長とする（Ｓ＝ｓ／ｈ）
Figure 0006941039
 ここでクロソイドの１セグメントにおける向きの変化θｄ＝θβ−θαを導入すると，次式を得る。
Figure 0006941039
 車両向きθの軌道、経路長ｓに対する曲線の変化率η、sharpnessは、次式で表される。
Figure 0006941039
曲線の変化率η、sharpnessは、１クロソイドセグメントにおいて定義から明らかなように一定である。車両向きθの軌道が決まれば式（１）（２）より、座標原点からの移動量を表すＰ（Ｓ）＝［ｘ（Ｓ），ｙ（Ｓ）］Ｔの軌道が次式で決定される。
Figure 0006941039
 式（１２）において、Ｓ＝１とおき、１セグメントクロソイドのｘｙ移動量Ｐｄ＝［ｘｄ，ｙｄ］Ｔが次式で与えられる。
Figure 0006941039
このＰｄ＝（θα，κα，ｈ，θｄ）を関数として、３連クロソイドによる軌道のモデル化で使用する。軌道計画の数値解法のために、式（１２）（１５）の計算について述べる。ただし、ＣＲ（・・・）、ＳＲ（・・・）以外の要素の数値計算は容易であるので省略する。ＣＲ（・・・）、ＳＲ（・・・）の計算は３つのケースに分ける。
［ケース１（θｄ−ｈκα＝０かつκα＝０）］
このケースは、直線運動に相当する。このケースも、定義上、クロソイドに含まれる。
Figure 0006941039
［ケース２（θｄ−ｈκα＝０かつκα≠０）］
このケースは、円運動に相当する。このケースも、定義上、クロソイドに含まれる。
Figure 0006941039
［ケース３（θｄ−ｈκα≠０）］
このケースは、フレネル積分（Ｆｒｅｓｎｅｌ ｉｎｔｅｇｒａｌ）を含むため、計算が難しい。数値積分を使う方法もあるが、十分な精度を出すためには，計算量が大きくなる。以下の式を用いて計算する。
フレネル積分に相当するのは式（２３）（２４）の部分であり，近似の有効な領域の違う２つの近似公式を切り替えて用いる。
Figure 0006941039
 ただし、
Figure 0006941039
Figure 0006941039
Figure 0006941039
以上の準備で、１セグメントクロソイドのｘｙ移動量Ｐｄ＝（θα，κα，ｈ，θｄ）は，セグメントの始端での向きθαと曲率κα、１セグメント全経路長ｈ、１セグメントでの向き変化θｄの４変数で関数として定義され、計算可能なことが示された。
引き続き、クロソイド長が可変の場合の３連クロソイドによる始端での姿勢（posture）から終端での姿勢（posture）への軌道計画を数値最適化問題として定式化する。
（３連クロソイドの軌道）
３連クロソイドの軌道の数値最適化問題への定式化は、次のアプローチをとる。
・第１のクロソイドの始端の姿勢を（ｘ０，ｙ０，θ０，κ０）とする。
・曲率θと向きκが連続となるように、３セグメントのクロソイドを接続する。
・第３のクロソイドの終端の姿勢を（ｘａｒｒ，ｙａｒｒ，θ１，κ１）とする。
・ｘａｒｒ＝ｘ１，ｙａｒｒ＝ｙ１となるまで、第１〜第３のクロソイドのパラメータを｜κ｜≦κｌｉｍｉｔの制約の下で数値最適化する。
図３は、このアプローチによる３連クロソイドの軌道モデルを示す概念図である。第ｋセグメント（ｋ＝１，２，３・・・）に関する変数を、以下のように定義する。
Figure 0006941039
進行する車両の軌道としては、前輪の舵角は経路に対して連続的に変化する必要がある。このため、車両の向きθと車両の曲率κは、セグメント間の端点において連続する。まず、車両向きθについて考えると、各セグメントの両端点での連続性から以下の式を満たす必要がある。
Figure 0006941039
Figure 0006941039
式（２５）〜式（４０）をまとめると、次の式を得る。
Figure 0006941039
８変数ｈ１，ｈ２，ｈ３，θｄ，１，θｄ，２，θｄ，３，κα，２，κα，３が式（４１）を満たせば、式（２５）〜式（４０）で定める軌道は、軌道計画問題の車両向きと曲率の連続性に関する条件を満たす。ここで、式（４１）は４本の等式拘束であり、本質的なパラメータは８変数から４変数に絞ることができる。本発明では、（ｈ１，ｈ２，ｈ３，θｄ，１）を３連クロソイドセグメントの４パラメータに選ぶ（図３参照）。各クロソイドのθｄ，２，θｄ，３，κα，２，κα，３，θα，２，θα，３は、式（４１）からパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の関数として次式で決定される。
Figure 0006941039
ｈ１（ｈ２＋ｈ３）≠０であれば、式（４２）〜式（４７）の右辺は、不定になることはない。本実施の形態では、ｈ１＞０，ｈ２＞０，ｈ３＞０を想定しているので、この条件を満たす。式（４４）より、｜κα，２｜＜κｌｉｍｉｔは、ｈ１，θｄ，１に関する以下の不等式拘束に変換される。
Figure 0006941039
式（４５）より、｜κα，３｜＜κｌｉｍｉｔは、ｈ１，ｈ２，ｈ３，θｄ，１に関する以下の不等式拘束に変換される。
Figure 0006941039
パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）を決めると、式（４２）〜式（４７）より、θα，ｋ，κα，ｋ，θｄ，ｋ（ｋ＝１，２，３）が定まる。さらに、３連クロソイドセグメントの到達点Ｐａｒｒが式（１５）より以下のように計算される。
Figure 0006941039
つまり（ｈ１，ｈ２，ｈ３，θｄ，１）のパラメータを使えば、車両向き及び曲率に関する条件は自動的に満たされるので、位置に関する条件Ｐａｒｒ＝Ｐ１を満たせば、所望の始端の姿勢（posture）から終端の姿勢（posture）への軌道が得られる。以下、（ｈ１，ｈ２，ｈ３，θｄ，１）によるパラメータ表現を４パラメータ表現という。
Ｐａｒｒ（ｈ１，ｈ２，ｈ３，θｄ，１）＝Ｐ１となる（ｈ１，ｈ２，ｈ３，θｄ，１）を探索する問題は数値最適化問題として定式化される。目的関数ｆｏｂｊ（Ｐａｒｒ）をＰａｒｒ−Ｐ１のユークリッド２ノルムの二乗値に選ぶ。このとき、数値最適化問題として以下のように記述できる。
Figure 0006941039
 上記の最適化問題を解いて，最適化の結果、
Figure 0006941039
 が達成できれば、解となる軌道が得られる。つまり、所望の始端の姿勢（posture）から終端の姿勢（posture）への軌道が得られる。
（第１実施の形態）
式（５５）のような最適化問題には、疑似ニュートン法に基づく、主双対内点法（ＰＤＩＰＭ）や遂次２次計画法（ＳＱＰ）などの繰り返し最適化手法が適用可能である。しかし、式（５５）の最適化問題は非線形非凸であるため、不適切な初期値を選ぶと、解が求まらないという問題が生じる。本実施の形態は、式（５５）の最適化問題において、繰り返し最適化手法に用いる適切な初期値を求める。
図４は、第１の実施の形態の軌道計画装置１の構成を示す図である。図４に示すように第１の実施の形態の軌道計画装置１は、入力部１０と、粗探索部１１と、最適化部１２と、出力部１３と、記憶部１４とを有している。
入力部１０は、軌道計画の条件として車両の始端および終端のデータの入力を受け付ける機能を有する。具体的には、車両の始端の姿勢（位置、向き、曲率）と、終端の姿勢（位置、向き、曲率）である。
記憶部１４には、始端から到達点を計算する到達点関数が記憶されている。到達点関数は、上述した式（５３）で示した関数であり、（ｈ１，ｈ２，ｈ３，θｄ，１）の４つのパラメータを有する関数である。
粗探索部１１は、到達点関数に適当なサンプルパラメータを適用して求めた到達点の中から、終端に最も近い最近傍到達点を得たパラメータを求める。粗探索部１１は、ｈｔｏｔａｌ＝ｈ１＋ｈ２＋ｈ３を固定した際のパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の集合により、次式で定義される到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を求める。
Figure 0006941039
この集合が空であれば、適切な初期値はなしと判定し、空でなければ、到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）の中で終端位置Ｐ１に最も近い要素（最近傍到達点）に対応する４パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）を繰り返し最適化手法の初期値として用いる。なお、式中のΔθｆａｎは、到達点Ｐａｒｒが存在すべき角度範囲を規定するものであり、例えば、π／４である。
最適化部１２は、粗探索部１１にて求めたパラメータを初期値として用いて、上述した式（５５）を満たすパラメータの最適化処理を行うことにより、到達点の姿勢が終端の姿勢になるようなパラメータを求める機能を有する。このパラメータにより車両が通るべき軌道が計画される。続いて、最適化部１２は、求めたパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）から、各位置における旋回曲率κも求める機能を有する。
出力部１３は、最適化部１２にて求めたパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）および旋回曲率κを車両の制御を行う自動運転制御部（図示せず）に対して出力する。
図５は、第１の実施の形態の軌道計画装置１の動作を示すフローチャートである。軌道計画装置１は、軌道計画の条件として車両の始端および終端のデータの入力を受け付ける（Ｓ１０）。軌道計画装置１は、記憶部１４から到達点関数を読み出し（Ｓ１１）、到達点関数を用いて初期値を決定する（Ｓ１２）。
図６は、第１の実施の形態における初期値決定の処理を示すフローチャートである。粗探索部１１は、到達点関数に適当なパラメータを設定して、終端に最も近い到達点（最近傍到達点）を得ることができるパラメータをラフに探索する。具体的には、粗探索部１１は、３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）を始端位置である始点Ｐ０から終端位置である終点Ｐ１までの直線距離ｈｔｏｔａｌに固定し（Ｓ２０）、４つのパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の組合せを、例えば１０００通り選び、各パラメータを到達点関数に適用したときの到達点を求める（Ｓ２１）。
粗探索部１１が到達点の近傍到達点を見つけた場合には（Ｓ２２でＹＥＳ）、終点Ｐ１の最近傍の到達点を得たパラメータを初期値として決定する（Ｓ２３）。もし、近傍到達点が見つからなかった場合には（Ｓ２２でＮＯ）、始端から終端に至る軌道が存在しないと判断する（Ｓ２４）。
図５に戻って説明を続ける。軌道計画装置１は初期値を決定すると、決定された初期値を用いてパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の最適化を行い、始端から終端への軌道を描く最適パラメータを算出する（Ｓ１３）。また、軌道計画装置１は、求めた最適パラメータに基づいて、各位置における旋回曲率κも求める。続いて、軌道計画装置１は、パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）および旋回曲率κを自動運転制御部に対して出力する（Ｓ１４）。
以上、軌道計画装置１の構成及び動作について説明したが、上記した軌道計画装置１のハードウェアの例は、ＣＰＵ、ＲＡＭ、ＲＯＭ、ハードディスク、ディスプレイ、キーボード、マウス、通信インターフェース等を備えたコンピュータである。上記した各機能を実現するモジュールを有するプログラムをＲＡＭまたはＲＯＭに格納しておき、ＣＰＵによって当該プログラムを実行することによって、上記した軌道計画装置１が実現される。このようなプログラムも本発明の範囲に含まれる。
（第２実施の形態）
次に、第２の実施の形態の軌道計画装置について説明する。第２の実施の形態の軌道計画装置の基本的な構成は、第１の実施の形態の軌道計画装置１の構成と同じであるが（図４）、粗探索部１１における探索処理が異なる。
第２の実施の形態における粗探索の処理の概要は、以下のとおりである。到達点の分布が始点Ｐ０から見て終点Ｐ１の手前に生じるように３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）を固定する。例えば、３連クロソイドの始点Ｐ０から終点Ｐ１までの直線距離とすれば、到達点の分布は始点Ｐ０から見て終点Ｐ１の手前に生じることになる。この固定長をｈｔｏｔａｌＬＢとする。
また、到達点の分布が始点Ｐ０から見て終点Ｐ１の奥に生じるように３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）を固定する。例えば、始点Ｐ０から終点Ｐ１までの直線距離の２倍とすれば、通常、到達点の分布は始点Ｐ０から見て終点Ｐ１の奥に生じることになる。この固定長をｈｔｏｔａｌＵＢとする。
そして、ｈｔｏｔａｌＬＢ≦ｈｔｏｔａｌＵＢの関係を保ちながら，２分探索により、ｈｔｏｔａｌＬＢ、ｈｔｏｔａｌＵＢを更新し、その差ｈｔｏｔａｌＵＢ−ｈｔｏｔａｌＬＢが十分小さくなったところで、到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌＬＢ）、ＳＰａｒｒＧ（ｈｔｏｔａｌＵＢ）の中で終端位置Ｐ１に最も近い要素に対応する４パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）を繰り返し最適化手法の初期値として用いる。
図７は、第２の実施の形態における粗探索処理の動作を示す図である。粗探索部１１は、３連クロソイドの全長を下限ｈｔｏｔａｌＬＢに固定し、適当に選んだ複数のパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の組合せについて到達点を求め、到達点の中から最近傍内周到達点を求める（Ｓ４０）。ここで、最近傍内周到達点とは、図１０（ａ）に示す範囲に含まれる到達点であって、終点Ｐ１に最も近い到達点である。なお、扇形の中心角θｆａｎは、あらかじめ設定された値であり、例えば、π／４とする。続いて、粗探索部１１は、３連クロソイドの全長を上限ｈｔｏｔａｌＵＢに固定し、適当に選んだ複数のパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の組合せについて到達点を求め、到達点の中から最近傍外周到達点を求める（Ｓ４１）。ここで、最近傍内周到達点とは、図１０（ｂ）に示す範囲に含まれる到達点であって、終点Ｐ１に最も近い到達点である。
図８は、最近傍内周到達点を求める処理を示す図である。粗探索部１１は、始点Ｐ０と終点Ｐ１の直線距離を３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）の下限ｈｔｏｔａｌＬＢとし（Ｓ４０）、この条件を満たす複数のパラメータを適当に選んで、そのパラメータに対応する到達点を計算する（Ｓ４１）。粗探索部１１は、求めた到達点の中に、内周到達点があるか否かを判定し（Ｓ４２）、内周到達点がある場合には（Ｓ４２でＹＥＳ）、その内周到達点のうちで、終点Ｐ１に最も近い、最近傍内周到達点と対応するパラメータを返す（Ｓ４３）。内周到達点の有無の判定において、内周到達点がないと判定された場合には（Ｓ４２でＮＯ）、始端と終端とをつなぐ３連クロソイドの解が存在しないと判定する（Ｓ４４）。
図９は、最近傍外周到達点を求める処理を示す図である。粗探索部１１は、始点Ｐ０と終点Ｐ１の直線距離に定数Ｃ２（＞１）を乗じた値を３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）の上限ｈｔｏｔａｌＵＢとし（Ｓ５０）、この条件を満たす複数のパラメータを適当に選んで、そのパラメータに対応する到達点を計算する（Ｓ５１）。粗探索部１１は、求めた到達点の中に、外周到達点があるか否かを判定し（Ｓ５２）、外周到達点がある場合には（Ｓ５２でＹＥＳ）、その外周到達点のうちで、終点Ｐ１に最も近い、最近傍外周到達点と対応するパラメータを返す（Ｓ５３）。外周到達点の有無の判定において、外周到達点がないと判定された場合には（Ｓ５２でＮＯ）、上限ｈｔｏｔａｌＵＢを更新して上述の処理を繰り返す。
具体的には、始点Ｐ０と終点Ｐ１の直線距離に定数Ｃ３（＞０）を乗じた値を上限ｈｔｏｔａｌＵＢに加算して、新たな上限ｈｔｏｔａｌＵＢとする（Ｓ５４）。この新たな上限ｈｔｏｔａｌＵＢが所定の閾値より大きくない場合には（Ｓ５５でＮＯ）、新たな上限ｈｔｏｔａｌＵＢを用いて外周到達点を求める処理を行う（Ｓ５１）。新たな上限ｈｔｏｔａｌＵＢが所定の閾値より大きい場合には（Ｓ５５でＹＥＳ）、始端と終端とをつなぐ３連クロソイドの解が存在しないと判定する（Ｓ５６）。
図７に戻って、粗探索部１１が行う動作について説明する。粗探索部１１は、上限ｈｔｏｔａｌＵＢと下限ｈｔｏｔａｌＬＢとの差分の距離が所定の閾値より小さいか否かを判定する（Ｓ３２）。上限ｈｔｏｔａｌＵＢと下限ｈｔｏｔａｌＬＢとの差分の距離が閾値より小さい場合には（Ｓ３２でＹＥＳ）、３連クロソイドの全長を上限ｈｔｏｔａｌＵＢに固定して求めた最近傍外周到達点と、下限ｈｔｏｔａｌＬＢに固定して求めた最近傍内周到達点のうち、終点Ｐ１に近い到達点を与えたパラメータを初期値とする（Ｓ３９）。
最初の段階では、上限ｈｔｏｔａｌＵＢと下限ｈｔｏｔａｌＬＢとの差分の距離は閾値より小さくはないので（Ｓ３２でＮＯ）、ステップＳ３３の処理に進む。粗探索部１１は、上限ｈｔｏｔａｌＵＢと下限ｈｔｏｔａｌＬＢとの距離の中間固定長を算出し（Ｓ３３）、この中間固定長を用いて、最近傍外周到達点と、最近傍内周到達点を求める（Ｓ３４）。
続いて、粗探索部１１は、３連クロソイドの全長を上限ｈｔｏｔａｌＵＢに固定して求めた最近傍外周到達点よりも、中間固定長に固定して求めた最近傍外周到達点の方が終点Ｐ１に近いかを判定する（Ｓ３５）。この判定結果、中間固定長で求めた方が終点Ｐ１に近い場合には（Ｓ３５でＹＥＳ）、粗探索部１１は、中間固定長を新たな上限ｈｔｏｔａｌＵＢに設定する（Ｓ３６）。
上限ｈｔｏｔａｌＵＢで求めた最近傍外周到達点の方が終点Ｐ１に近い場合には（Ｓ３５でＮＯ）、粗探索部１１は、３連クロソイドの全長を下限ｈｔｏｔａｌＬＢに固定して求めた最近傍内周到達点よりも、中間固定長に固定して求めた最近傍内周到達点の方が終点Ｐ１に近いかを判定する（Ｓ３７）。この判定の結果、中間固定長で求めた方が終点Ｐ１に近い場合には（Ｓ３７でＹＥＳ）、粗探索部１１は、中間固定長を新たな下限ｈｔｏｔａｌＬＢに設定し（Ｓ３８）、粗探索部１１は、上限ｈｔｏｔａｌＵＢと下限ｈｔｏｔａｌＬＢとの差分の距離が所定の閾値より小さいか否かを判定する処理（Ｓ３２）に戻る。
３連クロソイドの全長を下限ｈｔｏｔａｌＬＢに固定して求めた最近傍外周到達点よりも、中間固定長に固定して求めた最近傍外周到達点の方が終点Ｐ１に近いか否かの判定において、下限ｈｔｏｔａｌＬＢで求めた方が終点Ｐ１に近い場合には（Ｓ３７でＮＯ）、３連クロソイドの全長を上限ｈｔｏｔａｌＵＢに固定して求めた最近傍外周到達点と、下限ｈｔｏｔａｌＬＢに固定して求めた最近傍内周到達点のうち、終点Ｐ１に近い到達点を与えたパラメータを初期値とする（Ｓ３９）。
以下では、具体的に始端および終端における車両の姿勢のデータを与えて、軌道計画を行った例を示す。
（第１実施例）
図１１は、第１実施例の軌道計画を行う始端と終端のデータを示す図である。始端の位置Ｐ０（ｘ０，ｙ０）は（０，０）、終端の位置Ｐ１（ｘ１，ｙ１）は（２５．６１，４．３９）である。始端における車両の向きθ０は０［ｒａｄ］、車両の旋回曲率κ０は０［１／ｍ］であり、終端における車両の向きθ１は０．７８５４［ｒａｄ］、車両の旋回曲率κ１は０．０６６７［１／ｍ］である。
上で説明したとおり、３連クロソイドの全長（ｈ１＋ｈ２＋ｈ３）をｈｔｏｔａｌに固定したときのパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の集合により、次式で定義される到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を考える。
Figure 0006941039
図１２は、図１１のケースにおいて、ｈｔｏｔａｌ＝｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を示す図である。ただし、ここでΔθｆａｎは、π／４とした。また、ＳＰａｒｒＧ（ｈｔｏｔａｌ）は厳密には無限集合であるため、近似計算により、次のように求める。
（１）（ｈ１，ｈ２，ｈ３，θｄ，１）の下記の集合Ｓ４Ｐａｒａ（ｈｔｏｔａｌ）からサンプリングした有限のパラメータ集合ＳＲＳ４Ｐａｒａ（ｈｔｏｔａｌ）を求める。なお、θＲａｎｇｅは、あらかじめ定めたパラメータであり、ここではｐｉ／２としている。
Figure 0006941039
（２）Ｓ４Ｐａｒａ（ｈｔｏｔａｌ）の全要素（ｈ１，ｈ２，ｈ３，θｄ，１）∈Ｓ４Ｐａｒａ（ｈｔｏｔａｌ）に対して，Ｐａｒｒ（ｈ１，ｈ２，ｈ３，θｄ，１）を求め、このうち、式（７０）の到達点ＳＰａｒｒＧ（ｈｔｏｔａｌ）の定義を満たす要素Ｐａｒｒの集合をＳＰａｒｒＧ（ｈｔｏｔａｌ）とする。図１２に示すように、ＳＰａｒｒＧ（ｈｔｏｔａｌ）は、おおよそＰ１−Ｐ０の方向にある到達点集合となる。
式（１５）より、Ｐｄ（θα，κα，ｈ，θｄ）は、θα，κα，ｈ，θｄに関して連続であり、式（４２）〜（４７）より、θｄ，２，θｄ，３，κα，２，κα，３，θα，２，θα，３は（ｈ１，ｈ２，ｈ３，θｄ，１）に関して連続であることから、式（５３）が示すＰａｒｒ（ｈ１，ｈ２，ｈ３，θｄ，１）は、ｈ１，ｈ２，ｈ３，θｄ，１に関して連続である。
したがって、ある４パラメータｈ１ ※，ｈ２ ※，ｈ３ ※，θｄ，１ ※に対する到達点ＰａｒｒＰａｒｒ（ｈ１，ｈ２，ｈ３，θｄ，１）の近傍位置Ｐａｒｒ＋ΔＰａｒｒを考えると，これに対応する４パラメータｈ１，ｈ２，ｈ３，θｄ，１はｈ１ ※，ｈ２ ※，ｈ３ ※，θｄ，１ ※を微小に変化したもので得られる。例えば，具体的には図１３に示すように，ｈ１ ※，ｈ２ ※，ｈ３ ※，θｄ，１ ※から微小変化した次式で与えられる４パラメータｈ１ ※，ｈ２ ※，ｈ３ ※＋Δｈ３，θｄ，１ ※＋Δθｄ，１により、Ｐａｒｒ＋ΔＰａｒｒに近い位置に到達点を変更することができる。
Figure 0006941039
逆に言うと，式（５５）の最適化問題に解（ｈ１ ※，ｈ２ ※，ｈ３ ※，θｄ，１ ※）があれば，その近傍には（ｈ１ ※，ｈ２ ※，ｈ３ ※，θｄ，１ ※）から変化したパラメータ集合に対応する到達点集合が存在する。
ＳＰａｒｒＧ（ｈｔｏｔａｌ）はこの到達点集合に対応するので、ＳＰａｒｒＧ（ｈｔｏｔａｌ）が空であれば、間接的に式（５５）の最適化問題が解を持たないことが推測される。
そこで，第１実施例では，ＳＰａｒｒＧ（ｈｔｏｔａｌ）が空集合ならば、適切な初期値がないと判断する。ＳＰａｒｒＧ（ｈｔｏｔａｌ）に要素があれば、終点Ｐ１の最近傍到達点であるＰａｒｒ∈ＳＰａｒｒＧ（ｈｔｏｔａｌ）に対応するパラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）を初期値に使う。
この方法によれば、ランダムに初期値を選んだ場合に比べて、終点Ｐ１に近い位置から繰り返し最適化手法がスタートできるために、極小値に捕らわれることが少ない。また、いきなり繰り返し最適化手法を適用した場合に、解の有無にかかわらず、問題の非凸性により収束せずに計算が終了しないこともあるが、本実施例では、到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を有限のパラメータ集合ＳＲＳ４ｐａｒａ（ｈｔｏｔａｌ）から近似計算するため、必ず有限時間で解の有無が推定される。
（第２実施例）
始点Ｐ０から到達点Ｐａｒｒの距離｜Ｐａｒｒ−Ｐ０｜は、３連クロソイドセグメントが直線の場合に最大となるので、常に以下の式が成り立つ。
｜Ｐａｒｒ（ｈ１，ｈ２，ｈ３，θｄ，１）−Ｐ０｜≦ｈ１＋ｈ２＋ｈ３＝ｈｔｏｔａｌ
図１４（ａ）は、ｈｔｏｔａｌ＝１・｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を示す図であり、先に示した図１２と同じである。図１４（ａ）に見られるように、ＳＰａｒｒＧ（ｈｔｏｔａｌ）のすべての要素は必ず始点Ｐ０からみて終点Ｐ１の手前に分布しており、上記の式を裏付けている。図１４（ｂ）は、ｈｔｏｔａｌの長さを１・｜Ｐ１−Ｐ０｜から増やし、ｈｔｏｔａｌ＝２・｜Ｐ１−Ｐ０｜とした場合の到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）を示す図である。この場合、到達点集合ＳＰａｒｒＧ（ｈｔｏｔａｌ）には始点Ｐ０からみて終点Ｐ１の奥に分布する。
図１４（ａ）及び図１４（ｂ）に示す結果は、式（５５）が最適解（ｈ１ ※，ｈ２ ※，ｈ３ ※，θｄ，１ ※）を持つときの３連クロソイドの全長ｈｔｏｔａｌ ※をｈｔｏｔａｌ ※＝ｈ１ ※＋ｈ２ ※＋ｈ３ ※とする。ｈｔｏｔａｌ＜ｈｔｏｔａｌ ※とｈｔｏｔａｌを短くすると、ＳＰａｒｒＧ（ｈｔｏｔａｌ）はＰ０から見てＰ１の手前に分布するものが発生する。逆に、ｈｔｏｔａｌ＞ｈｔｏｔａｌ ※とｈｔｏｔａｌを長くすると、ＳＰａｒｒＧ（ｈｔｏｔａｌ）は始点Ｐ０から見て終点Ｐ１の奥に分布するものが発生する。
ＳＰａｒｒＧ（ｈｔｏｔａｌ）が終点Ｐ１の手前に分布するｈｔｏｔａｌを下限ｈｔｏｔａｌＬＢ、ＳＰａｒｒＧ（ｈｔｏｔａｌ）が終点Ｐ１の奥に分布するｈｔｏｔａｌを上限ｈｔｏｔａｌＵＢとする。第２実施例は、下限ｈｔｏｔａｌＬＢと上限ｈｔｏｔａｌＵＢとのギャップを２分探索により狭めることで、より良い初期値を得るものである。
始点Ｐ０からみて終点Ｐ１の手前に分布する到達点を説明するために、次式で定義される内周到達点の集合ＳＰａｒｒＧＩｎ（ｈｔｏｔａｌ）とそれに対応する４パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の集合ＳＲＳ４ｐａｒｒＩｎ（ｈｔｏｔａｌ）を導入する。
Figure 0006941039
式（２００）は、理論的には無限の要素を持つ集合となるが、第１実施例で示したように、サンプリングした有限のパラメータ集合ＳＲＳ４ｐａｒｒ（ｈｔｏｔａｌ）から求めた近似有限集合とする。
さらに、ＳＰａｒｒＧＩｎ（ｈｔｏｔａｌ）のうち、終点Ｐ１に最も近い最近傍内周到達点をＰｐａｒＧＩｎＮＮ（ｈｔｏｔａｌ）、その４パラメータを（ｈ１，ｈ２，ｈ３，θｄ，１）ＩｎＮＮ（ｈｔｏｔａｌ）とする。式で書くと以下のようになる。
Figure 0006941039
同様に、始点Ｐ０からみて終点Ｐ１の奥に分布する到達点に関して説明するために，次式で定義される，到達点集合ＳＰａｒｒＧＯｕｔ（ｈｔｏｔａｌ）とそれに対応する４パラメータ（ｈ１，ｈ２，ｈ３，θｄ，１）の集合ＳＲＳ４ｐａｒａＯｕｔ（ｈｔｏｔａｌ）を導入する。
Figure 0006941039
同様にＳＰａｒｒＧＯｕｔ（ｈｔｏｔａｌ）のうち、終点Ｐ１に最も近い最近傍外周到達点をＰｐａｒＧＯｕｔＮＮ（ｈｔｏｔａｌ）、その４パラメータを（ｈ１，ｈ２，ｈ３，θｄ，１）ＯｕｔＮＮ（ｈｔｏｔａｌ）とする。式で書くと以下のようになる。
Figure 0006941039
ここで示した最近傍内周到達点、最近傍外周到達点を用いて、図７に示す２分探索の処理によって探索範囲を狭めていき、複数のサンプルパラメータの中から、終点Ｐ１の最近傍に到達する４パラメータを求める。
図１５は、図１１に示したケースに対して、上記したアルゴリズムによってｈｔｏｔａｌＬＢとｈｔｏｔａｌＵＢの更新処理を行ったときの到達点の分布を示す図である。図１６は、上記した処理によって求めた初期値を用いて、繰り返し最適化手法（主双対内点法）を行って得られた軌道を示す図である。図１６に示すように、適切な初期値からスタートすることによって、式（５５）に示す最適化問題を適切に解けることが確認された。
また、図１１以外の３つのケースにおいて、本実施の形態の方法で取得した初期値を用いて軌道計画を行った例を示す。
Figure 0006941039
図１７は、上記の各ケースにおいて計算された軌道である。図１７に示すようにいずれも適切に繰り返し最適化が行われ、軌道が得られた。
本発明は、車両の軌道を計画する装置等として有用である。
１ 軌道計画装置
１０ 入力部
１１ 粗探索部
１２ 最適化部
１３ 出力部
１４ 記憶部

Claims (11)

  1. 接続点において曲率が連続する３本のクロソイド曲線からなる軌道を計画するためのプログラムであって、コンピュータに、
    始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、
    前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、３本の前記クロソイド曲線の全長を前記始端から前記終端までの固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索ステップと、
    前記粗探索ステップで求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップと、
    を実行させるプログラム。
  2. 前記粗探索ステップにて、前記終端までの距離が所定の閾値以下となる最近傍到達点を与えるサンプルのパラメータが存在しなかった場合には、入力された条件を満たす解が存在しないと判定する請求項１に記載のプログラム。
  3. 接続点において曲率が連続する３本のクロソイド曲線からなる軌道を計画するためのプログラムであって、コンピュータに、
    始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、
    前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、３本の前記クロソイド曲線の全長を前記始端から前記終端までの第１の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも手前にある内周到達点を求め、前記内周到達点の中で前記終端に最も近い最近傍内周到達点を得たパラメータを第１パラメータとして求める第１パラメータ算出ステップと、
    ３本の前記クロソイド曲線の全長を前記第１の固定長より長い第２の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも奥にある外周到達点を求め、前記外周到達点の中で前記終端に最も近い最近傍外周到達点を得たパラメータを第２パラメータとして求める第２パラメータ算出ステップと、
    ３本の前記クロソイド曲線の全長を前記第１の固定長と前記第２の固定長の中間の中間固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して最近傍内周到達点及び最近傍外周到達点を求め、前記最近傍内周到達点を得たパラメータを第３パラメータ、前記最近傍外周到達点を得たパラメータを第４パラメータとして求める第３・第４パラメータ算出ステップと、
    前記第２パラメータで与えられる最近傍外周到達点と前記第４パラメータで与えられる最近傍外周到達点とを比べ、前記第４パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第２の固定長とし、前記第２パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べ、前記第３パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第１の固定長とする固定長更新ステップと、
    前記固定長更新ステップにて前記第１の固定長または前記第２の固定長を更新しつつ、最近傍外周到達点および最近傍内周到達点を繰り返し求め、前記第１の固定長と前記第２の固定長の差が所定の閾値以下となったときに、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めるパラメータ決定ステップと、
    前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップと、
    を実行させるプログラム。
  4. 前記固定長更新ステップにおいて、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べて、前記第１パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求める請求項３に記載のプログラム。
  5. 前記第１パラメータ算出ステップにおける計算によって前記内周到達点が見つからなかった場合には、前記始端と前記終端とをつなぐクロソイド曲線が存在しないと判定する請求項４に記載のプログラム。
  6. 前記第２パラメータ算出ステップにおける計算によって前記外周到達点が見つからなかった場合には、前記外周到達点が見つかるか、前記第２の固定長が所定の閾値以上になるまで、前記第２の固定長を長くして外周到達点を求める処理を行い、前記第２の固定長が所定の閾値以上になったときは、前記始端と前記終端とをつなぐクロソイド曲線が存在しないと判定する請求項４に記載のプログラム。
  7. 前記到達点関数は、前記車両の旋回曲率をパラメータとして含み、前記旋回曲率の最大値を拘束条件として有する請求項１乃至６のいずれかに記載のプログラム。
  8. 軌道計画装置によって、接続点において曲率が連続する３本のクロソイド曲線からなる軌道を計画するための方法であって、
    前記軌道計画装置は、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、
    前記軌道計画装置は、前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、前記軌道計画装置は、３本の前記クロソイド曲線の全長を前記始端から前記終端までの固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索ステップと、
    前記軌道計画装置は、前記粗探索ステップで求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップと、
    を備える軌道計画方法。
  9. 軌道計画装置によって、接続点において曲率が連続する３本のクロソイド曲線からなる軌道を計画するための方法であって、
    前記軌道計画装置は、始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力ステップと、
    前記軌道計画装置は、始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶部から読み出し、前記軌道計画装置は、３本の前記クロソイド曲線の全長を前記始端から前記終端までの第１の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも手前にある内周到達点を求め、前記内周到達点の中で前記終端に最も近い最近傍内周到達点を得たパラメータを第１パラメータとして求める第１パラメータ算出ステップと、
    前記軌道計画装置は、３本の前記クロソイド曲線の全長を前記第１の固定長より長い第２の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも奥にある外周到達点を求め、前記外周到達点の中で前記終端に最も近い最近傍外周到達点を得たパラメータを第２パラメータとして求める第２パラメータ算出ステップと、
    前記軌道計画装置は、３本の前記クロソイド曲線の全長を前記第１の固定長と前記第２の固定長の中間の中間固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して最近傍内周到達点及び最近傍外周到達点を求め、前記最近傍内周到達点を得たパラメータを第３パラメータ、前記最近傍外周到達点を得たパラメータを第４パラメータとして求める第３・第４パラメータ算出ステップと、
    前記軌道計画装置は、前記第２パラメータで与えられる最近傍外周到達点と前記第４パラメータで与えられる最近傍外周到達点とを比べ、前記第４パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第２の固定長とし、前記第２パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べ、前記第３パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第１の固定長とする固定長更新ステップと、
    前記軌道計画装置は、前記固定長更新ステップにて前記第１の固定長または前記第２の固定長を更新しつつ、最近傍外周到達点および最近傍内周到達点を繰り返し求め、前記第１の固定長と前記第２の固定長の差が所定の閾値以下となったときに、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めるパラメータ決定ステップと、
    前記軌道計画装置は、前記パラメータ決定ステップにて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点における車両の位置、向き、曲率で指定される姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化ステップと、
    を備える軌道計画方法。
  10. 接続点において曲率が連続する３本のクロソイド曲線からなる軌道を計画するための軌道計画装置であって、
    始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力部と、
    前記始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶した記憶部と、
    ３本の前記クロソイド曲線の全長を前記始端から前記終端までの固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記記憶部から読み出した前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索部と、
    前記粗探索部で求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化部と、
    を備える軌道計画装置。
  11. 接続点において曲率が連続する３本のクロソイド曲線からなる軌道を計画するための軌道計画装置であって、
    始端における車両の位置、向き、曲率で指定される姿勢のデータと、終端における車両の位置、向き、曲率で指定される姿勢のデータの入力を受け付ける入力部と、
    始端の姿勢から到達点における車両の位置、向き、曲率で指定される姿勢を求めるための関数であって、少なくともクロソイド曲線の長さをパラメータとして含む到達点関数を記憶した記憶部と、
    複数の組合せをサンプルのパラメータを前記記憶部から読み出した前記到達点関数に適用して到達点を求め、前記到達点の中で前記終端に最も近い最近傍到達点を得たパラメータを求める粗探索部と、
    前記粗探索部にて求めたパラメータを初期値として、前記到達点関数の前記パラメータを変動させ、前記到達点の姿勢が前記終端の姿勢に一致するような最適パラメータを求める最適化部と、
    を備え、
    前記粗探索部は、
    ３本の前記クロソイド曲線の全長を前記始端から前記終端までの第１の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも手前にある内周到達点を求め、前記内周到達点の中で前記終端に最も近い最近傍内周到達点を得たパラメータを第１パラメータとして求める第１パラメータ算出ステップと、
    ３本の前記クロソイド曲線の全長を前記第１の固定長より長い第２の固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して前記終端よりも奥にある外周到達点を求め、前記外周到達点の中で前記終端に最も近い最近傍外周到達点を得たパラメータを第２パラメータとして求める第２パラメータ算出ステップと、
    ３本の前記クロソイド曲線の全長を前記第１の固定長と前記第２の固定長の中間の中間固定長として前記クロソイド曲線のそれぞれの長さの複数の組合せをサンプルのパラメータとし、前記サンプルのパラメータを前記到達点関数に適用して最近傍内周到達点及び最近傍外周到達点を求め、前記最近傍内周到達点を得たパラメータを第３パラメータ、前記最近傍外周到達点を得たパラメータを第４パラメータとして求める第３・第４パラメータ算出ステップと、
    前記第２パラメータで与えられる最近傍外周到達点と前記第４パラメータで与えられる最近傍外周到達点とを比べ、前記第４パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第２の固定長とし、前記第２パラメータで与えられる最近傍外周到達点の方が前記終端に近い場合には、前記第１パラメータで与えられる最近傍内周到達点と前記第３パラメータで与えられる最近傍内周到達点とを比べ、前記第３パラメータで与えられる最近傍内周到達点の方が前記終端に近い場合には、前記中間固定長を新たな第１の固定長とする固定長更新ステップと、
    前記固定長更新ステップにて前記第１の固定長または前記第２の固定長を更新しつつ、最近傍外周到達点および最近傍内周到達点を繰り返し求め、前記第１の固定長と前記第２の固定長の差が所定の閾値以下となったときに、最近傍外周到達点と最近傍内周到達点のうち、前記終端に近い方の最近傍到達点を与えたパラメータを求めるパラメータ決定ステップと、
    を実行する軌道計画装置。
JP2017227756A 2017-11-28 2017-11-28 軌道計画装置、軌道計画方法、およびプログラム Active JP6941039B2 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2017227756A JP6941039B2 (ja) 2017-11-28 2017-11-28 軌道計画装置、軌道計画方法、およびプログラム

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2017227756A JP6941039B2 (ja) 2017-11-28 2017-11-28 軌道計画装置、軌道計画方法、およびプログラム

Publications (2)

Publication Number Publication Date
JP2019096269A JP2019096269A (ja) 2019-06-20
JP6941039B2 true JP6941039B2 (ja) 2021-09-29

Family

ID=66971877

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2017227756A Active JP6941039B2 (ja) 2017-11-28 2017-11-28 軌道計画装置、軌道計画方法、およびプログラム

Country Status (1)

Country Link
JP (1) JP6941039B2 (ja)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114111788A (zh) * 2021-11-09 2022-03-01 武汉乐庭软件技术有限公司 一种基于多段回旋线的轨迹规划的方法、设备及存储设备
CN114506343A (zh) * 2022-03-02 2022-05-17 阿波罗智能技术(北京)有限公司 轨迹规划方法、装置、设备、存储介质及自动驾驶车辆
CN114714351B (zh) * 2022-04-06 2023-06-23 上海工程技术大学 用于移动机械臂的抗饱和目标跟踪控制方法及控制***

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3916537B2 (ja) * 2002-09-04 2007-05-16 株式会社三菱総合研究所 道路線形設計方法、道路線形設計装置およびコンピュータ読取可能な道路線形設計プログラム
JP5555838B2 (ja) * 2010-01-13 2014-07-23 マッスル株式会社 軌道生成方法および軌道生成装置
JP6247622B2 (ja) * 2014-09-29 2017-12-13 日立建機株式会社 管制制御装置
US9746854B2 (en) * 2015-04-24 2017-08-29 Autonomous Solutions, Inc. System and method for controlling a vehicle
JP6636820B2 (ja) * 2016-02-22 2020-01-29 東洋製罐株式会社 巻締装置

Also Published As

Publication number Publication date
JP2019096269A (ja) 2019-06-20

Similar Documents

Publication Publication Date Title
JP6941039B2 (ja) 軌道計画装置、軌道計画方法、およびプログラム
CN110260867B (zh) 一种机器人导航中位姿确定、纠正的方法、设备及装置
CN108073176B (zh) 一种改进型D*Lite车辆动态路径规划方法
CN113448335A (zh) 路径规划方法和装置、车辆和可读存储介质
Ostafew et al. Visual teach and repeat, repeat, repeat: Iterative learning control to improve mobile robot path tracking in challenging outdoor environments
CN109434831B (zh) 机器人运行方法、装置、机器人、电子设备及可读介质
CN109990787B (zh) 一种机器人在复杂场景中规避动态障碍物的方法
JP5901504B2 (ja) 宇宙船の動きを制御するシステム及び方法
JP2017016645A (ja) 半自律車両、及び、半自律車両を制御する方法
CN110220528A (zh) 一种基于a星算法的自动驾驶无人车双向动态路径规划方法
JP2007249363A (ja) 移動体の軌道追従制御システム及び軌道追従制御方法
KR20150121931A (ko) 하이브리드 경로 생성 방법을 이용한 무인 지상 차량 경로 제어 시스템
Van Parys et al. Online distributed motion planning for multi-vehicle systems
CN111915106B (zh) 一种路径生成方法、装置、晶面机及存储介质
Jian et al. A global-local coupling two-stage path planning method for mobile robots
Koyuncu et al. A probabilistic B-spline motion planning algorithm for unmanned helicopters flying in dense 3D environments
CN115077534B (zh) 一种基于b样条曲线的agv路径规划方法
Artuñedo et al. Smooth path planning for urban autonomous driving using OpenStreetMaps
JP6808106B1 (ja) 加工プログラム変換装置、数値制御装置、加工プログラム変換方法および機械学習装置
JP5993784B2 (ja) 経路修正装置
CN117873115A (zh) 一种基于slam的自主导航避障路径规划方法
JP5923973B2 (ja) 経路修正装置
CN116009558A (zh) 一种结合运动学约束的移动机器人路径规划方法
CN114115268B (zh) 路径生成方法、装置、交通工具及存储介质
JP6131587B2 (ja) 経路修正装置

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20200309

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20210217

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20210302

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20210330

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20210713

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20210831

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20210903

R150 Certificate of patent or registration of utility model

Ref document number: 6941039

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150