特許 6954553 自律走行車両、コントローラ、コンピュータプログラム、自律走行車両の制御方法

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

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

(11)【特許番号】6954553

(24)【登録日】2021年10月4日

(45)【発行日】2021年10月27日

(54)【発明の名称】自律走行車両、コントローラ、コンピュータプログラム、自律走行車両の制御方法

(51)【国際特許分類】

   G05D 1/02 20200101AFI20211018BHJP

   B62D 6/00 20060101ALI20211018BHJP

【ＦＩ】

   G05D1/02 WZYW

   B62D6/00

【請求項の数】7

【全頁数】18

(21)【出願番号】特願2016-226682(P2016-226682)

(22)【出願日】2016年11月22日

(65)【公開番号】特開2018-84899(P2018-84899A)

(43)【公開日】2018年5月31日

【審査請求日】2019年8月30日

【新規性喪失の例外の表示】特許法第３０条第２項適用 平成２８年 ５月２８日，農業食料工学会第７５回年次大会

(73)【特許権者】

【識別番号】593006630

【氏名又は名称】学校法人立命館

(73)【特許権者】

【識別番号】501203344

【氏名又は名称】国立研究開発法人農業・食品産業技術総合研究機構

(74)【代理人】

【識別番号】100111567

【弁理士】

【氏名又は名称】坂本 寛

(74)【代理人】

【識別番号】110000280

【氏名又は名称】特許業務法人サンクレスト国際特許事務所

(72)【発明者】

【氏名】深尾 隆則

(72)【発明者】

【氏名】村上 則幸

【審査官】 藤崎 詔夫

(56)【参考文献】

【文献】 特開平０７−１５６８１８（ＪＰ，Ａ）

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

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

【文献】 特開２０１５−００１９０６（ＪＰ，Ａ）

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

Ｇ０５Ｄ １／０２

Ｂ６２Ｄ ６／００

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

【請求項1】

自律走行車両であって、
コントローラと、
前記自律走行車両の回頭角を計測するセンサと、
を備え、
前記コントローラは、
前記自律走行車両と仮想的な参照車両との左右方向の位置の差である横偏差と、前記自律走行車両の回頭角と前記参照車両の回頭角との差である第１回頭角偏差と、を算出する処理と、
前記第１回頭角偏差に対する非線形化を行って第２回頭角偏差を求める非線形化処理と、
前記第２回頭角偏差を変換して第３回頭角偏差を求める変換処理と、
前記横偏差及び前記第３回頭角偏差に基づいて、前記参照車両に追従するように、前記自律走行車両の走行を制御する制御処理と、
を実行するよう構成され、
前記非線形化処理は、前記第１回頭角偏差の絶対値が所定値よりも小さい場合には、前記第２回頭角偏差の絶対値を前記第１回頭角偏差の絶対値よりも小さくする処理であり、
前記変換処理は、前記第２回頭角偏差と、前記横偏差及び前記自律走行車両の速度の関数の値と、を加算して前記第３回頭角偏差を求める処理である
自律走行車両。

【請求項2】

前記非線形化処理は、前記第１回頭角偏差の絶対値が所定値よりも小さい範囲においては、前記第２回頭角偏差の絶対値を前記第１回頭角偏差の絶対値よりも小さくするための非線形関数を用いて、前記第２回頭角偏差を求める処理であり、
前記非線形関数は、前記範囲内における前記第２回頭角偏差が前記範囲外における前記第２回頭角偏差に対してなめらかに連続するよう設定されている
請求項１記載の自律走行車両。

【請求項3】

前記非線形関数は、前記範囲外における前記第２回頭角偏差を、前記第１回頭角偏差と同じ値にするよう構成されている
請求項２記載の自律走行車両。

【請求項4】

自律走行車両であって、
コントローラと、
前記自律走行車両の回頭角を計測するセンサと、
を備え、
前記コントローラは、
前記自律走行車両の回頭角と仮想的な参照車両の回頭角との差である回頭角偏差に対する非線形化処理と、
非線形化処理された前記回頭角偏差に基づいて、前記参照車両に追従するように、前記自律走行車両の走行を制御する制御処理と、
を実行するよう構成され、
前記非線形化処理は、前記回頭角偏差の絶対値が所定値よりも小さい場合には、前記回頭角偏差の絶対値を小さくする処理であり、
前記コントローラは、非線形化処理された前記回頭角偏差の変換処理を更に実行するよう構成され、
前記変換処理は、非線形化処理された前記回頭角偏差と、前記自律走行車両の位置と前記参照車両の位置との左右方向における差である横偏差の関数の値と、を加算して、変換された回頭角偏差を求め、
前記制御処理は、変換された前記回頭角偏差に基づいて実行され、
前記関数は、ｈ（ｚ）＝Ａｚ／（１＋Ｂｚ^２）であり、
ｈ（ｚ）は、ｚｈ（ｚ）＞０，∀ｚ≠０，かつｈ（０）＝０を満たす関数であり、
Ａ，Ｂは、正の値をとる設定パラメータであり、
ｚ＝ｅ_２ｖであり、
ｅ_２は、横偏差であり、
ｖは、前記自律走行車両の速度である
自律走行車両。

【請求項5】

コントローラであって、
前記コントローラは、
自律走行車両と仮想的な参照車両との左右方向の位置の差である横偏差と、前記自律走行車両の回頭角と前記参照車両の回頭角との差である第１回頭角偏差と、を算出する処理と、
前記第１回頭角偏差に対する非線形化を行って第２回頭角偏差を求める非線形化処理と、
前記第２回頭角偏差を変換して第３回頭角偏差を求める変換処理と、
前記横偏差及び前記第３回頭角偏差に基づいて、前記参照車両に追従するように、前記自律走行車両の走行を制御する制御処理と、
を実行するよう構成され、
前記非線形化処理は、前記第１回頭角偏差の絶対値が所定値よりも小さい場合には、前記第２回頭角偏差の絶対値を前記第１回頭角偏差の絶対値よりも小さくする処理であり、
前記変換処理は、前記第２回頭角偏差と、前記横偏差及び前記自律走行車両の速度の関数の値と、を加算して前記第３回頭角偏差を求める処理である
コントローラ。

【請求項6】

コンピュータに、
自律走行車両と仮想的な参照車両との左右方向の位置の差である横偏差と、前記自律走行車両の回頭角と前記参照車両の回頭角との差である第１回頭角偏差と、を算出する処理と、
前記第１回頭角偏差に対する非線形化を行って第２回頭角偏差を求める非線形化処理と、
前記第２回頭角偏差を変換して第３回頭角偏差を求める変換処理と、
前記横偏差及び前記第３回頭角偏差に基づいて、前記参照車両に追従するように、前記自律走行車両の走行を制御する制御処理と、
を実行させるコンピュータプログラムであって、
前記非線形化処理は、前記第１回頭角偏差の絶対値が所定値よりも小さい場合には、前記第２回頭角偏差の絶対値を前記第１回頭角偏差の絶対値よりも小さくする処理であり、
前記変換処理は、前記第２回頭角偏差と、前記横偏差及び前記自律走行車両の速度の関数の値と、を加算して前記第３回頭角偏差を求める処理である
コンピュータプログラム。

【請求項7】

自律走行車両の制御方法であって、
前記制御方法は、
前記自律走行車両と仮想的な参照車両との左右方向の位置の差である横偏差と、前記自律走行車両の回頭角と前記参照車両の回頭角との差である第１回頭角偏差と、を算出し、
前記第１回頭角偏差に対する非線形化を行って第２回頭角偏差を求める非線形化処理をし、
前記第２回頭角偏差を変換して第３回頭角偏差を求める変換処理をし、
前記横偏差及び前記第３回頭角偏差に基づいて、前記参照車両に追従するように、前記自律走行車両の走行を制御する
を含み、
前記非線形化処理は、前記第１回頭角偏差の絶対値が所定値よりも小さい場合には、前記第２回頭角偏差の絶対値を前記第１回頭角偏差の絶対値よりも小さくする処理であり、
前記変換処理は、前記第２回頭角偏差と、前記横偏差及び前記自律走行車両の速度の関数の値と、を加算して前記第３回頭角偏差を求める処理である
制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自律走行車両等に関する。

【背景技術】

【0002】

車両の自律走行には、車両の状態を計測することが求められる（例えば、特許文献１参照）。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１５−３６８４０号公報

【発明の概要】

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

【0004】

車両の自律走行には、例えば、経路追従（Path Following）制御が用いられることがある。経路追従制御は、仮想的な参照車両の軌道に、実車両を追従させる制御である。経路追従制御では、実車両と参照車両との相対誤差が用いられる。相対誤差には、例えば、横偏差と回頭角偏差とが含まれる。

【0005】

回頭角偏差を求めるため、実車両の回頭角が計測される。車両の回頭角は、例えば、車両の方位角として計測される。方位角の計測には、例えば、磁気方位を計測する磁気センサが用いられる。

【0006】

回頭角の計測においては、計測誤差が生じることがある。旋回角の計測誤差は、自律走行の精度を低下させる。例えば、高価な磁気センサでも微小ながら計測誤差が生じ、安価なセンサであればより計測誤差が大きくなる。

【0007】

回頭角の計測誤差によって生じる自律走行の精度低下を抑制することが望まれる。

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

【0008】

本発明の一の態様は、自律走行車両である。実施形態において、自律走行車両は、コントローラと、前記自律走行車両の回頭角を計測するセンサと、を備える。前記コントローラは、前記自律走行車両の回頭角と仮想的な参照車両の回頭角との差である回頭角偏差に対する非線形化処理と、非線形化処理された前記回頭角偏差に基づいて、前記参照車両に追従するように、前記自律走行車両の走行を制御する制御処理と、を実行するよう構成されている。前記非線形化処理は、前記回頭角偏差の絶対値が所定値よりも小さい場合には、前記回頭角偏差の絶対値を小さくする処理である。

【0009】

本発明の他の態様は、コントローラである。本発明の他の態様は、コンピュータプログラムである。本発明の他の態様は、自律走行車両の制御方法である。

【図面の簡単な説明】

【0010】

【図1】車両のブロック図である。

【図2】経路追従制御の説明図である。

【図3】非線形化関数を示すグラフである。

【図4】コントローラによる処理のフローチャートである。

【図5A】シミュレーション結果を示す図である。

【図5B】シミュレーション結果を示す図である。

【図6A】シミュレーション結果を示す図である。

【図6B】シミュレーション結果を示す図である。

【図7A】シミュレーション結果を示す図である。

【図7B】シミュレーション結果を示す図である。

【図8A】シミュレーション結果を示す図である。

【図8B】シミュレーション結果を示す図である。

【図9A】シミュレーション結果を示す図である。

【図9B】シミュレーション結果を示す図である。

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

【0011】

［１．実施形態の概要］

【0012】

［第１項］
実施形態に係る自律走行車両は、コントローラと、前記自律走行車両の回頭角を計測するセンサと、を備える。前記コントローラは、前記自律走行車両の回頭角と仮想的な参照車両の回頭角との差である回頭角偏差に対する非線形化処理を実行する。前記コントローラは、非線形化処理された前記回頭角偏差に基づいて、前記参照車両に追従するように、前記自律走行車両の走行を制御する制御処理と、を実行する。前記非線形化処理は、前記回頭角偏差の絶対値が所定値よりも小さい場合には、前記回頭角偏差の絶対値を小さくする処理である。回頭角偏差の絶対値が所定値よりも小さい場合には、前記回頭角偏差の絶対値を小さくすることで、回頭角の計測誤差が生じたとしても、自律走行の精度低下を抑制することができる。

【0013】

［第２項］
前記制御処理は、逆最適制御によって走行を制御する処理であるのが好ましい。逆最適制御は、不整地を車両が走行する場合に生じる外乱に対してロバスト性を有する。

【0014】

［第３項］
前記コントローラは、非線形化処理された前記回頭角偏差の変換処理を更に実行するよう構成され、前記変換処理は、非線形化処理された前記回頭角偏差と、前記自律走行車両の位置と前記参照車両の位置との左右方向における差である横偏差の関数の値と、を加算して、変換された回頭角偏差を求め、前記制御処理は、変換された前記回頭角偏差に基づいて実行されるのが好ましい。この場合、変換された回頭角偏差は、横偏差及び回頭角偏差の関数となる。

【0015】

［第４項］
前記関数は、ｈ（ｚ）＝Ａｚ／（１＋Ｂｚ^２）であるのが好ましい。ここで、ｈ（ｚ）は、ｚｈ（ｚ）＞０，∀ｚ≠０，かつｈ（０）＝０を満たす関数であり、Ａ，Ｂは、正の値をとる設定パラメータであり、ｚ＝ｅ_２ｖであり、ｅ_２は、横偏差であり、ｖは、前記自律走行車両の速度である。この場合、コントローラの調整が容易となる。

【0016】

［第５項］
実施形態に係るコントローラは、前記自律走行車両の回頭角と仮想的な参照車両の回頭角との差である回頭角偏差に対する非線形化処理と、非線形化処理された前記回頭角偏差に基づいて、前記参照車両に追従するように、前記自律走行車両の走行を制御する制御処理と、を実行するよう構成されている。前記非線形化処理は、前記回頭角偏差の絶対値が所定値よりも小さい場合には、前記回頭角偏差の絶対値を小さくする処理である。

【0017】

［第６項］
実施形態に係るコンピュータプログラムは、コンピュータに、自律走行車両の回頭角と仮想的な参照車両の回頭角との差である回頭角偏差に対する非線形化処理と、非線形化処理された前記回頭角偏差に基づいて、前記参照車両に追従するように、前記自律走行車両の走行を制御する制御処理と、を実行させる。

【0018】

［第７項］
実施形態に係る自律走行車両の制御方法は、前記自律走行車両の回頭角と仮想的な参照車両の回頭角との差である回頭角偏差に対する非線形化処理をすること、非線形化処理された前記回頭角偏差に基づいて、前記参照車両に追従するように、前記自律走行車両の走行を制御することを含む。前記非線形化処理は、前記回頭角偏差の絶対値が所定値よりも小さい場合には、前記回頭角偏差の絶対値を小さくする処理である。

【0019】

［２．実施形態の詳細］

【0020】

［２．１ 車両］

【0021】

図１は、車両１００を示している。車両１００は、コントローラ１０によって自律走行する。車両１００は、走行する装置であり、例えば、ロボットトラクタである。ロボットトラクタは、自律走行するトラクタである。車両１００は、ロボットトラクタに限られず、他の種類の車両であってもよい。

【0022】

車両１００は、コントローラ１０を備える。コントローラ１０は、プロセッサ１１及びメモリ１２を有するコンピュータを備える。メモリ１２にはプロセッサ１１によって実行されるコンピュータプログラムが格納されている。プロセッサ１１がコンピュータプログラムを実行することで、コンピュータがコントローラ１０として機能する。

【0023】

車両１００は、車両の状態を計測するセンサ２１，２２を備える。センサ２１は、車両１００の回頭角θを計測する。回頭角θは、方位角として検出される。センサ２１は、例えば、磁気方位を計測する磁気センサである。計測された回頭角θは、コントローラ１０に与えられる。センサ２２は、車両１００の位置を計測する。車両１００の位置は、位置座標（ｘ，ｙ）として検出される。センサ２２は、例えば、全地球衛星測位システム（ＧＮＳＳ）受信機である。計測された位置（ｘ，ｙ）は、コントローラ１０に与えられる。

【0024】

コントローラ１０は、制御対象である車両１００への制御入力として、車両１００の舵角φを出力する。ただし、以下の説明では、コントローラ１０は、制御入力として速度ｖ及び旋回角速度ωを扱う。本実施形態の車両１００は、舵角φが変化することにより旋回するため、コントローラ１０は、速度ｖ及び角速度ωを舵角φに変換して出力する。この変換については後述する。

【0025】

コントローラ１０は、舵角φに応じて、車両の舵角を調整するアクチュエータ３０を制御する。アクチュエータ３０は、コントローラ１０からの指令に応じて、操舵輪を動かし、車両１００の回頭角を変化させる。コントローラ１０は、車両１００への制御入力として車両１００の速度ｖを出力してもよい。

【0026】

［２．２ 経路追従制御］
コントローラ１０は、経路追従（Path Following）制御によって車両１００を自律走行させる。図２に示すように、経路追従制御では、仮想的な参照車両２００が用いられる。経路追従制御においては、実車両である車両１００は、参照車両２００に参照入力を与えたときに参照車両２００が描く参照軌道に追従する。図２に示すように、経路追従制御では、車両１００は、参照車両２００の真横に位置して、参照車両２００に追従する。参照軌道は、車両１００が自律走行する場合の望ましい軌道である。

【0027】

車両１００の状態をｑ＝［ｘ，ｙ，θ］^Ｔ、参照車両の状態をｑ_ｒ＝［ｘ_ｒ，ｙ_ｒ，θ_ｒ］^Ｔとすると、それらの運動学モデルは、以下のように表される。

【数1】

【数2】

【0028】

ここで、ｘ，ｙは、ｘｙ座標系における車両１００の位置を示し、θは、車両１００の回頭角である。ｘ_ｒ，ｙ_ｒは、ｘｙ座標系における参照車両２００の位置を示し、θ_ｒは、参照車両２００の回頭角である。ｖ，ωは、車両１００の速度と角速度であり、ｖ_ｒ，ω_ｒは、参照車両２００の速度と角速度である。

【0029】

車両１００と参照車両２００の相対誤差は、以下のように定義される。

【数3】

【0030】

誤差ｅ_１は、車両１００と参照車両２００との前後方向位置の差である。以下では、誤差ｅ_１を、前後偏差という。誤差ｅ_２は、車両１００と参照車両２００との左右方向位置の差である。以下では、誤差ｅ_２を、横偏差という。誤差ｅ_３は、車両１００の回頭角θと参照車両２００の回頭角θ_ｒとの差である。以下では、誤差ｅ_３を、回頭角偏差という。

【0031】

ある初期条件のもと、制御入力としての速度ｖ及び旋回角速度ωを次のように選ぶと、偏差ｅ_１，ｅ_２，ｅ_３は０へ収束する。

【数4】

【数5】

【0032】

ただし、Ｋ_１，Ｋ_２，Ｋ_３は設定パラメータであり、正の定数である。

【0033】

式（３）で示される相対誤差を時間微分することにより、次の式（６）が得られる。

【数6】

【0034】

追従制御を行う場合には、車両１００と参照車両２００との前後方向の位置の差は考えなくてもよいため、制御入力の進行方向成分ｖには任意の速度を与え、ｅ_１＝０，ｅ_１の時間微分＝０とすると、式（６）から、参照車両２００の速度ｖ_ｒが以下のように決定される。

【数7】

【0035】

ここで、旋回の曲率をρとすると、参照車両２００の速度ｖ_ｒ，角速度ω_ｒは、車両１００の速度ｖに応じ、以下のようになる。

【数8】

【数9】

【0036】

よって、ｅ_２，ｅ_３を０に収束させる入力は、式（５）によって決定されるωのみとなり、このとき、式（６）は、以下のように書き換えられる。

【数10】

【0037】

［２．３ コントローラによる制御則］

【0038】

［２．３．１ 逆最適制御］
車両１００が、舗装された道路ではなく、農地のような不整地を走行する場合、地面の凹凸やすべりによる外乱を受ける。車両１００がトラクタである場合、トラクタが牽引する作業機による振動や抵抗による外乱も受ける。車両１００の経年劣化などによって、モデルの変化及び不確かさによるモデル化誤差も大きくなる。本実施形態では、外乱及びモデル化誤差に対応したロバストな制御則として、逆最適制御法を採用する。コントローラ１０は、逆最適制御によって、車両１００の走行を制御する。

【0039】

最適制御問題は、システム

【数12】

に対して、以下の条件を満足する制御入力ｕ（ｘ）を設計することである。

【0040】

１．ｕ（ｘ）は系を目標状態に漸近収束させる。
２．ｕ（ｘ）は以下の評価関数Ｊを最小化する。

【数13】

【0041】

しかし、この最適制御問題を解くには、Hamilton-Jacobi-Bellman方程式を解く必要がある。そこで、本実施形態では、より容易である以下の逆最適化のアプローチをとる。
１．まずフィードバック則を設計する。
２．その後、式（１３）のような評価関数に対して最適であることが示される。

【0042】

［２．３．２ Sontagの形式による制御］
次式で示されるアフィンシステムを考える。

【数14】

【0043】

ここで、ｆは、ｎ次元のベクトル、ｇはｎ×ｍ行列である。このとき、以下の条件を満たす関数をControl Lyapunov Function（ＣＬＦ）と定義する。

【0044】

（定義：Control Lyapunov Function）すべてのｘ≠０に対して、次の関係を満足する、なめらかで正定かつ放射状に非有界な関数Ｖ（ｘ）は、式（１４）のシステムに対するControl Lyapunov Function（ＣＬＦ）と呼ばれる。

【数15】

このとき、次の定理が成立することが明らかとなっている。

【0045】

（定理：ＣＬＦからの逆最適制御）
ａ（ｘ），ｂ（ｘ），ｐ（ｘ）が以下の式で定義されるとき、

【数16】

【数17】

【数18】

Sontagの形式による次の入力

【数19】

は、システムを漸近安定化し、かつ次の評価関数を最小化する。

【数20】

【0046】

この最適性によって、求められたコントローラは、sectormargin(1/2,∞)とよばれる安定上の利点を持つ。

【0047】

［２．３．３ 逆最適経路追従制御］
本実施形態のコントローラ１０は、経路追従制御を逆最適制御によって行う。以下では、経路追従制御のための逆最適コントローラ１０を説明する。

【0048】

まず、以下の関数を定義する。ただし、Ａ，Ｂは設定パラメータであり、正の値である。

【数21】

ここで、ｈ（ｚ）は、ｚｈ（ｚ）＞０，∀ｚ≠０，かつｈ（０）＝０の条件を満たす関数である。ｈ（ｚ）は、かかる条件を満たす関数であればよく、式（２１）の形式に限定されない。

【数22】

【0049】

以下の式（２３）によって、コントローラ１０は、回頭角偏差ｅ_３を変換する変換処理を行う。実施形態において、変換処理は、回頭角偏差ｅ_３に、式（２１）に示す関数ｈの値を加算して、変換された回頭角偏差ｅ^~_３を求める処理である。ここでは、式（２１）において、ｚ＝ｅ_２ｖであり、ｈは、横偏差ｅ_２の関数となっている。

【数23】

【0050】

ＣＬＦからの逆最適制御では、式（１５）を満たす必要があるが、回頭角偏差ｅ_３は、単独では、式（１５）の関係を満たさない。しかし、ｚｈ（ｚ）＞０となる関数ｈ（ｚ）を、回頭角偏差ｅ_３に加算すると、式（１５）を満たすことができる。式（２３）に示すように、変換された回頭角偏差ｅ^~_３は、横偏差ｅ_２と回頭角偏差ｅ_３の関数として表される。

【0051】

式（２１）によれば、ｅ_２ｖの絶対値が大きい場合には、ｅ_２ｖが、式（２３）に示す回頭角偏差ｅ^~_３に反映される度合は小さくなる。一方、ｅ_２ｖの絶対値が小さい場合には、ｅ_２ｖは、その大きさに応じた値がｅ^~_３に反映される。

【0052】

式（１０）及び式（２３）より、次式が得られる。

【数24】

【0053】

式（２４）に基づくと、以下で示されるＶ_ｉは、Control Lyapunov Function（ＣＬＦ）となる。

【数25】

ここで、Ｋ_ｉ２，Ｋ_ｉ３は正定数である。Ｋ_ｉ２はｅ_２に対するゲインであり、Ｋ_ｉ３はｅ^~_３に対するゲインである。

【0054】

仮に、式（２５）において、ｅ^~_３がｅ_３である場合には、ｅ_２，ｅ_３に対するゲインＫ_ｉ２，Ｋ_ｉ３を別々に設定することで、ｅ_２，ｅ_３に対するゲインを個別に調整でき問題はない。しかし、Ｖ_ｉがControl Lyapunov Function（ＣＬＦ）であるためには、式（２５）において、ｅ^~_３が採用されている必要がある。ｅ^~_３は、ｅ_２とｅ_３の関数であるため、ゲインＫ_ｉ２，Ｋ_ｉ３を別々に設定しただけでは、ｅ_２，ｅ_３に対するゲインを個別に調整するのは必ずしも容易ではない。

【0055】

例えば、経路追従制御では、回頭角偏差ｅ_３が生じたために横偏差ｅ_２が大きくなった場合に、その回頭角偏差ｅ_３を小さくしようとすると、横偏差ｅ_２を小さくすることができない。一方、横偏差ｅ_２を小さくしようとすると、却って回頭角偏差ｅ_３が大きくなる。経路追従制御では、横偏差ｅ_２と回頭角偏差ｅ_３とにはトレードオフがある。したがって、ｅ_２とｅ_３の関数であるｅ^~_３を、単一のゲインＫ_ｉ３で適切に調整するのは必ずしも容易ではない。

【0056】

これに対して、本実施形態では、式（２１）の関数ｈが有する設定パラメータＡ，Ｂを調整することで、変換された回頭角偏差ｅ^~_３に、横偏差ｅ_２をどの程度反映させるかを調整でき、コントローラ１０を適切に調整するのが容易となる。ここで、式（２１）では、設定パラメータＡは、関数ｈの値の大きさを決定し、設定パラメータＢは、ｅ^~_３にｅ_２ｖの大きさに応じた値が反映されるｅ_２ｖの範囲を決定する。

【0057】

さて、式（２５）で示されるＶ_ｉが、Control Lyapunov Function（ＣＬＦ）であるとき、

【数26】

【数27】

である。

【0058】

式（２６）（２７）を、式（１９）の制御入力を出力するコントローラ１０に適用することで、横偏差ｅ_２及び回頭角偏差ｅ_３を０に収束させる角速度ωが決定される。ただし、速度ｖ_ｒが有界であるには、ｅ_３≠π／２かつｅ_２ωが有界でなければならない。

【0059】

［２．３．４ 入力の変換］
式（１９）の制御入力は、車両１００の速度ｖと旋回角速度ωであるが、実際の車両１００は、操舵輪である前輪の舵角φが変化することにより旋回する。舵角φと、速度ｖ及び旋回確度ωとの関係は、以下のとおりである。ここで、Ｌは、車両１００の前後輪の距離である。

【数28】

【0060】

コントローラ１０は、決定された角速度ωを式（２８）で舵角φに変換し、舵角φに応じてアクチュエータ３０を制御する。

【0061】

［２．３．５ 回頭角偏差ｅ_３の非線形化］
回頭角偏差ｅ_３は、計測される方位角（回頭角）θの計測誤差の影響を受けて、実際の回頭角偏差よりも大きくなることがある。実際よりも大きい回頭角偏差ｅ_３に基づいて制御すると、横偏差ｅ_２を収束させ難くなる。この場合、制御入力を求める場合の回頭角偏差ｅ_３に対するゲインを小さくすれば、その分、方位角θの計測誤差の影響も小さくできる。しかし、単に、回頭角偏差ｅ_３に対するゲインを小さくすると、回頭角偏差ｅ_３が本当に大きい場合に、横偏差ｅ_２を収束させるのがやはり困難になる。

【0062】

本実施形態では、計測誤差の影響を低減するため、コントローラ１０は、回頭角偏差ｅ_３の非線形化処理を行う。回頭角偏差ｅ_３の非線形化処理のための関数ｉ（ｅ_３）は、例えば、以下のように定義される。以下では、関数ｉ（ｅ_３）の値を、非線形化回頭角偏差という。

【数29】

ここで、ａは、関数ｉ（ｅ_３）の原点での傾き、εは非線形化を行う範囲を表し、例えば、以下の条件を満たすように決定される。

【数30】

【数31】

【0063】

関数ｉ（ｅ_３）による非線形化処理は、回頭角偏差ｅ_３の絶対値が所定値εよりも小さい場合には、回頭角偏差の絶対値を小さくする処理である。図３における実線は、関数ｉ（ｅ_３）の例を示す。ここでは，ａ＝０．１，ε＝０．０２０に決定した。なお、図３における点線は、傾き１の直線である。

【0064】

非線形化処理においては、回頭角偏差ｅ_３の絶対値が所定値εよりも小さい範囲、すなわち−ε≦ｅ_３≦εの範囲においては、関数ｉは非線形関数となっており、図３に示すように、非線形化回頭角偏差ｉ（ｅ_３）の絶対値が、回頭角偏差ｅ_３の絶対値よりも小さくなる非線形化が行われる。回頭角偏差ｅ_３の絶対値が小さい範囲では、計測誤差の影響が相対的に大きくなるため、非線形化処理により、回頭角偏差ｅ_３の絶対値を小さくして回頭角偏差ｅ_３を重視しないようにして、計測誤差の影響を小さくする。これにより、横偏差ｅ_２を収束させるのが容易となる。

【0065】

一方、回頭角偏差ｅ_３の絶対値が所定値εよりも大きい範囲、すなわち、−ε＞ｅ_３，ｅ_３＜εの範囲では、関数ｉは線形関数であり、ｉ（ｅ_３）＝ｅ_３となる。つまり、回頭角偏差ｅ_３の絶対値が所定値εよりも大きい場合には、非線形化回頭角偏差ｉ（ｅ_３）は、非線形化処理前の回頭角偏差ｅ_３の値を維持する。回頭角偏差ｅ_３の絶対値が大きい範囲では、計測誤差の影響が相対的に小さくなるため、回頭角偏差ｅ_３の絶対値を小さくすることなく制御入力の決定に用いることで、回頭角偏差ｅ_３が本当に大きい場合に、横偏差ｅ_２を収束させるのが容易となる。

【0066】

傾きａは、計測誤差の大きさに応じて適宜設定される。計測誤差が小さい場合には、傾きａを上記の範囲で大きくすることで、−ε≦ｅ_３≦εの範囲の関数ｉ（ｅ_３）が、回頭角偏差ｅ_３の絶対値を小さくする度合が小さくなる。逆に、計測誤差が大きい場合には、傾きａを上記の範囲で小さくすることで、−ε≦ｅ_３≦εの範囲の関数ｉ（ｅ_３）が、回頭角偏差ｅ_３の絶対値を小さくする度合が大きくなる。

【0067】

コントローラ１０は、傾きａとして予め設定された値を用いても良いし、計測誤差の大きさに応じて傾きａを動的に変更してもよい。計測誤差の大きさは、例えば、コントローラ１０が、カルマンフィルタを用いて推定できる。

【0068】

非線形化範囲εも、計測誤差の大きさに応じて適宜設定される。計測誤差が小さい場合には、非線形化範囲εを上記の範囲で小さくして、非線形化処理の範囲を絞ることができる。逆に、計測誤差の大きさが大きい場合には、非線形化範囲εを上記の範囲で大きくして、非線形化処理の範囲を広げることができる。

【0069】

コントローラ１０は、非線形化範囲εとして予め設定された値を用いても良いし、計測誤差の大きさに応じて非線形化範囲εを動的に変更してもよい。前述のように、計測誤差の大きさは、例えば、コントローラ１０が、カルマンフィルタを用いて推定できる。

【0070】

実施形態において、−ε≦ｅ_３≦εの範囲におけるｉ（ｅ_３）は、−ε＞ｅ_３，ｅ_３＜εの範囲におけるｉ（ｅ_３）＝ｅ_３に対してなめらかに連続する関数として設定されている。−ε≦ｅ_３≦εの範囲における関数ｉ（ｅ_３）と、−ε＞ｅ_３，ｅ_３＜εの範囲における関数ｉ（ｅ_３）と、がなめらかに連続するものであることで、制御が安定化する。

【0071】

式（２９）を用いると、式（２３）は以下のように書き換えられる。

【数32】

【0072】

これにより、式（２４）と同様に次式が得られる。

【数33】

【0073】

式（３３）に基づくと、先の式（２５）で示されるＶ_ｉは、Control Lyapunov Function（ＣＬＦ）となる。このとき、

【数34】

【数35】

である。

【0074】

式（３４）（３５）を、式（１９）の制御入力を出力するコントローラ１０に適用することで、横偏差ｅ_２及び回頭角偏差ｅ_３を０に収束させる角速度ωが決定される。ただし、速度ｖ_ｒが有界であるには、ｅ_３≠π／２かつｅ_２ωが有界でなければならない。

【0075】

［２．４ コントローラによる処理手順］
図４は、経路追従制御を逆最適制御によって行うコントローラ１０による処理手順を示している。ステップＳ１において、コントローラ１０は、横偏差ｅ_２と回頭角偏差ｅ_３を算出する。横偏差ｅ_２は、センサ２２によって計測された位置（ｘ，ｙ）と、参照車両２００の位置（ｘ_ｒ，ｙ_ｒ）と、参照車両２００の回頭角θ_ｒと、から算出される（式（３）参照）。回頭角偏差ｅ_３は、センサ２１によって計測された回頭角θと、参照車両２００の回頭角θｒと、から、回頭角偏差ｅ_３も算出する（式（３）参照）。

【0076】

ステップＳ２において、コントローラ１０は、回頭角偏差ｅ_３に対して、非線形化処理を行って、非線形化回頭角偏差ｉ（ｅ_３）を求める。非線形化処理のための関数ｉは、コントローラ１０のメモリ１２に、非線形化処理用のデータ１２１として記憶されている。プロセッサ１１は、非線形化処理用のデータ１２１を読み出して非線形化処理を行う。回頭角偏差ｅ_３に対して非線形化処理が行われるため、前述のように、センサ２１によって計測された回頭角θに計測誤差が含まれていても、その影響が低減される。

【0077】

ステップＳ３において、コントローラ１０は、非線形化回頭角偏差ｉ（ｅ_３）に対して、式（３２）に従った変換処理を行う。変換処理では、非線形化回頭角偏差ｉ（ｅ_３）と、横偏差ｅ_２の関数ｈ（ｅ_２ｖ）の値と、を加算して、変換された非線形化回頭角偏差ｅ^~_３が求められる。変換処理のための設定パラメータＡ，Ｂ（式（２１）参照）は、コントローラ１０のメモリ１２にパラメータ値１２２及びパラメータ値１２３として設定されている。プロセッサ１１は、パラメータ値１２２，１２３を設定パラメータＡ，Ｂとして読み出して、変換処理を行う。設定パラメータＡ，Ｂがあるため、変換された非線形化回頭角偏差ｅ^~_３が適切になる。設定パラメータＡ，Ｂは、ユーザ入力により調整されてもよいし、コントローラ１０自らが、車両１００の走行環境等に応じて調整してもよい。

【0078】

ステップＳ４において、コントローラ１０は、横偏差ｅ_２及び変換された非線形化回頭角偏差ｅ^~_３に基づき、逆最適制御によりｅ_２及びｅ^~_３を収束させる角速度ωを決定する。コントローラ１０は、角速度ωから、式（２８）に基づいて、舵角φを求め、アクチュエータ３０を制御する。

【0079】

［２．５ シミュレーション結果］

【0080】

［２．５．１ 非線形化処理］
図５Ａ，５Ｂ，６Ａ，６Ｂは、回頭角θの計測誤差がある場合において、回頭角偏差ｅ_３に対する非線形化処理の有無による車両１００の状態の違いをシミュレーションした結果を示す。図５Ａは、非線形化処理を行った場合の横偏差ｅ_２及び回頭角偏差ｅ_３を示し、図５Ｂは、非線形化処理を行わない場合の横偏差ｅ_２及び回頭角偏差ｅ_３を示す。図６Ａは、非線形化処理を行った場合の舵角φを示し、図６Ｂは、非線形化処理を行わない場合の舵角φを示す。

【0081】

図５Ｂに示すように、非線形化処理を行わない場合の横偏差ｅ_２は、０．１ｍ程度であったのに対して、図５Ａに示すように、非線形化処理を行った場合の横偏差ｅ_２は、０．０４〜０．０５ｍ程度に低減される。シミュレーションにより、回頭角θの計測誤差があっても、非線形化処理を行うことで、横偏差ｅ_２を抑制できることが確認された。

【0082】

［２．５．２ 設定パラメータＡ，Ｂ］
図７Ａ，７Ｂ、８Ａ，８Ｂ，９Ａ，９Ｂは、式（２１）のパラメータＡ，Ｂを調整した場合の横偏差ｅ_２，回頭角偏差ｅ_３，舵角φを示す。図７Ａ，７Ｂは、Ａ＝１，Ｂ＝１に設定した場合であり、この場合、横偏差ｅ_２は、０．０４ｍ程度である。図８Ａ，８Ｂは、Ａ＝１，Ｂ＝３０に設定した場合であり、この場合、横偏差ｅ_２は、０．０２５ｍ程度である。図９Ａ，９Ｂは、Ａ＝１，Ｂ＝１５に設定した場合であり、この場合、横偏差ｅ_２は、０．０２３ｍ程度である。このように、パラメータＡ、Ｂ、特に、パラメータＢを適切に調整することにより、横偏差ｅ_２を容易に小さくすることができる。

【0083】

なお、本発明は、上記実施形態に限定されるものではなく、様々な変形が可能である。

【符号の説明】

【0084】

１０ コントローラ
１１ プロセッサ
１２ メモリ
２１ センサ
２２ センサ
３０ アクチュエータ
１００ 車両
２００ 参照車両

【図1】

【図2】

【図3】

【図4】

【図5A】

【図5B】

【図6A】

【図6B】

【図7A】

【図7B】

【図8A】

【図8B】

【図9A】

【図9B】