(57)【要約】【課題】処理負担の増加を抑制して、目標コースに精度よく追従することができるようにする。【解決手段】目標コース検出部２６によって、前方カメラ１２によって撮像された画像から、目標コースを検出する。検出点取得部３４によって、目標コース上の２点について、カメラ撮像方向に対する点の方向の俯角及び方位角を取得する。形状演算部４２によって、２点の各々についての俯角及び方位角に基づいて、目標コースの旋回曲率、及び目標コースに対する移動体の横偏差を計算する。旋回制御部４８によって、目標コースの旋回曲率、及び目標コースに対する移動体の横偏差に基づいて、目標コースに追従するように移動体の旋回運動を制御する。【選択図】図３
2016029559.tif 000002
【特許請求の範囲】
【請求項１】
旋回運動により移動位置の変更を行う移動体の旋回運動を制御する移動体制御装置であって、
前記移動体の移動速度を検出する速度検出手段と、
前記移動体の前方又は後方を撮像する撮像手段によって撮像された画像から、車線マーク又は目標コースを検出するコース検出手段と、
前記コース検出手段によって検出された車線マーク又は目標コース上の複数の点の各々について、基準方向に対する前記点の方向の俯角及び方位角を検出する角度検出手段と、
前記角度検出手段によって前記複数の点の各々について検出された俯角及び方位角に基づいて、前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差を計算する形状計算手段と、
前記形状計算手段によって計算された前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差と、前記速度検出手段によって検出された前記移動速度とに基づいて、目標コースに追従するように前記移動体の旋回運動を制御する旋回制御手段と、
を含む移動体制御装置。
【請求項２】
前記複数の点の各々について、前記点の俯角及び前記撮像手段の視点高さに基づいて、曲率あたりの方位角係数である曲率係数を算出し、前記複数の点の各々について、前記点の俯角、方位角、及び前記撮像手段の視点高さに基づいて、横偏差あたりの方位角係数である横偏差係数を計算する形状係数計算手段を更に含み、
前記形状計算手段は、前記角度検出手段によって前記複数の点の各々について検出された俯角及び方位角、並びに前記形状係数計算手段によって前記複数の点の各々について計算された前記曲率係数及び前記横偏差係数に基づいて、前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差を計算する請求項１記載の移動体制御装置。
【請求項３】
前記旋回制御手段は、前記形状計算手段によって計算された前記車線マーク又は目標コースの旋回曲率に応じ、かつ、前記車線マーク又は目標コースに対する前記移動体の横偏差を収束させる旋回ヨーレートを実現するように、前記移動体の旋回運動を制御する請求項１又は２記載の移動体制御装置。
【請求項４】
前記旋回制御手段は、前記形状計算手段によって計算された前記車線マーク又は目標コースの旋回曲率に応じ、かつ、前記車線マーク又は目標コースに対する前記移動体の横偏差を収束させる旋回ヨーレートを実現するための操舵角を算出し、前記移動体の操舵角を制御する請求項３記載の移動体制御装置。
【請求項５】
前記複数の点の各々について、前記点の俯角及び前記撮像手段の視点高さに基づいて、曲率あたりの方位角係数である曲率係数を算出し、前記複数の点の各々について、前記点の俯角、方位角、及び前記撮像手段の視点高さに基づいて、横偏差あたりの方位角係数である横偏差係数を計算する形状係数計算手段と、
前記移動体のヨーレートを検出するヨーレート検出手段と、
前記ヨーレート検出手段によって検出された前記移動体のヨーレート、前記速度検出手段によって検出された前記移動速度、及び前記形状係数計算手段によって前記複数の点の各々について計算された前記横偏差係数に基づいて、前記移動体のスリップ角を算出し、
前記ヨーレート検出手段によって検出された前記移動体のヨーレートと、前記算出された前記移動体のスリップ角と、前記旋回制御手段によって算出された前記操舵角とに基づいて、前記操舵角に対する前記移動体のヨーレートゲイン及び前記操舵角に対する前記移動体のスリップ角ゲインを算出する制御ゲイン算出手段と、を更に含む請求項４記載の移動体制御装置。
【請求項６】
前記形状計算手段によって計算された前記車線マーク又は目標コースの旋回曲率と、前記速度検出手段によって検出された前記移動速度と、前記旋回制御手段によって算出された前記操舵角とに基づいて、前記操舵角に対する前記移動体のヨーレートゲインを算出する制御ゲイン算出手段を更に含む請求項４記載の移動体制御装置。
【請求項７】
前記制御ゲイン算出手段は、前記形状計算手段によって計算された前記車線マーク又は目標コースに対する前記移動体の横偏差に応じて、前記操舵角に対する前記移動体のスリップ角ゲインを算出する請求項６記載の移動体制御装置。
【請求項８】
前記制御ゲイン算出手段によって算出された前記移動体のスリップ角ゲインを用いて算出されるスリップ角に基づいて、前記移動体の姿勢を算出する姿勢算出手段を更に含み、
前記旋回制御手段は、前記制御ゲイン算出手段によって算出された前記移動体のヨーレートゲインに基づいて、前記操舵角を算出し、前記移動体の操舵角を制御し、
前記形状計算手段は、前記角度検出手段によって前記複数の点の各々について検出された俯角及び方位角と、前記姿勢算出手段によって算出された前記移動体の姿勢とに基づいて、前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差を計算する請求項５又は請求項７に記載の移動体制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体制御装置に係り、特に、旋回運動により移動位置の変更を行う移動体の旋回運動を制御する移動体制御装置に関する。
【背景技術】
【０００２】
従来技術では、前方の走行場面情報から追従目標コースや制御パラメータをカルマンフィルタなどの状態空間フィルタにより数値化し、その検出結果に基づいた位置制御により追従走行する（例えば、特許文献１）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平８−２６１７５６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
従来技術では、カルマンフィルタを構成する上で道路形状モデルや車両状態量パラメータなどの推定パラメータを用いて検出精度を高めている。しかし、それらの適合状態や予測的な構成により検出誤差の発生や処理負担が増加する。また、追従制御において、検出した制御パラメータの誤差により制御性能が低下し誤差補償などが必要になる。
【０００５】
本発明は、上記の問題点を解決するためになされたもので、処理負担の増加を抑制して、目標コースに精度よく追従することができる移動体制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記の目的を達成するために本発明に係る移動体制御装置は、旋回運動により移動位置の変更を行う移動体の旋回運動を制御する移動体制御装置であって、前記移動体の移動速度を検出する速度検出手段と、前記移動体の前方又は後方を撮像する撮像手段によって撮像された画像から、車線マーク又は目標コースを検出するコース検出手段と、前記コース検出手段によって検出された車線マーク又は目標コース上の複数の点の各々について、基準方向に対する前記点の方向の俯角及び方位角を検出する角度検出手段と、前記角度検出手段によって前記複数の点の各々について検出された俯角及び方位角に基づいて、前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差を計算する形状計算手段と、前記形状計算手段によって計算された前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差と、前記速度検出手段によって検出された前記移動速度とに基づいて、目標コースに追従するように前記移動体の旋回運動を制御する旋回制御手段と、を含んで構成されている。
【０００７】
本発明に係る移動体制御装置によれば、速度検出手段によって、前記移動体の移動速度を検出する。コース検出手段によって、前記移動体の前方又は後方を撮像する撮像手段によって撮像された画像から、車線マーク又は目標コースを検出する。
【０００８】
そして、角度検出手段によって、前記コース検出手段によって検出された車線マーク又は目標コース上の複数の点の各々について、基準方向に対する前記点の方向の俯角及び方位角を検出する。形状計算手段によって、前記角度検出手段によって前記複数の点の各々について検出された俯角及び方位角に基づいて、前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差を計算する。
【０００９】
そして、旋回制御手段によって、前記形状計算手段によって計算された前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差と、前記速度検出手段によって検出された前記移動速度とに基づいて、目標コースに追従するように前記移動体の旋回運動を制御する。
【００１０】
このように、画像から検出された車線マーク又は目標コース上の複数の点の各々について、基準方向に対する前記点の方向の俯角及び方位角を検出し、前記複数の点の各々について検出された俯角及び方位角に基づいて、前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差を計算し、目標コースに追従するように前記移動体の旋回運動を制御することにより、処理負担の増加を抑制して、目標コースに精度よく追従することができる。
【００１１】
本発明に係る移動体制御装置は、前記複数の点の各々について、前記点の俯角及び前記撮像手段の視点高さに基づいて、曲率あたりの方位角係数である曲率係数を算出し、前記複数の点の各々について、前記点の俯角、方位角、及び前記撮像手段の視点高さに基づいて、横偏差あたりの方位角係数である横偏差係数を計算する形状係数計算手段を更に含み、前記形状計算手段は、前記角度検出手段によって前記複数の点の各々について検出された俯角及び方位角、並びに前記形状係数計算手段によって前記複数の点の各々について計算された前記曲率係数及び前記横偏差係数に基づいて、前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差を計算するようにすることができる。
【００１２】
上記の旋回制御手段は、前記形状計算手段によって計算された前記車線マーク又は目標コースの旋回曲率に応じ、かつ、前記車線マーク又は目標コースに対する前記移動体の横偏差を収束させる旋回ヨーレートを実現するように、前記移動体の旋回運動を制御するようにすることができる。また、上記の旋回制御手段は、前記形状計算手段によって計算された前記車線マーク又は目標コースの旋回曲率に応じ、かつ、前記車線マーク又は目標コースに対する前記移動体の横偏差を収束させる旋回ヨーレートを実現するための操舵角を算出し、前記移動体の操舵角を制御するようにすることができる。
【００１３】
また、上記の移動体制御装置は、前記移動体のヨーレートを検出するヨーレート検出手段と、前記ヨーレート検出手段によって検出された前記移動体のヨーレート、前記速度検出手段によって検出された前記移動速度、及び前記形状係数計算手段によって前記複数の点の各々について計算された前記横偏差係数に基づいて、前記移動体のスリップ角を算出し、前記ヨーレート検出手段によって検出された前記移動体のヨーレートと、前記算出された前記移動体のスリップ角と、前記旋回制御手段によって算出された前記操舵角とに基づいて、前記操舵角に対する前記移動体のヨーレートゲイン及び前記操舵角に対する前記移動体のスリップ角ゲインを算出する制御ゲイン算出手段と、を更に含むことができる。これによって、移動体の特性変化に応じてヨーレートゲイン及びスリップ角ゲインを適応させることができる。
【００１４】
また、上記の移動体制御装置は、前記形状計算手段によって計算された前記車線マーク又は目標コースの旋回曲率と、前記速度検出手段によって検出された前記移動速度と、前記旋回制御手段によって算出された前記操舵角とに基づいて、前記操舵角に対する前記移動体のヨーレートゲインを算出する制御ゲイン算出手段を更に含むことができる。これによって、移動体の特性変化に応じてヨーレートゲインを適応させることができる。
【００１５】
また、上記の制御ゲイン算出手段は、前記形状計算手段によって計算された前記車線マーク又は目標コースに対する前記移動体の横偏差に応じて、前記操舵角に対する前記移動体のスリップ角ゲインを算出することができる。これによって、移動体の特性変化に応じてスリップ角ゲインを適応させることができる。
【００１６】
上記の移動体制御装置は、前記制御ゲイン算出手段によって算出された前記移動体のスリップ角ゲインを用いて算出されるスリップ角に基づいて、前記移動体の姿勢を算出する姿勢算出手段を更に含み、前記旋回制御手段は、前記制御ゲイン算出手段によって算出された前記移動体のヨーレートゲインに基づいて、前記操舵角を算出し、前記移動体の操舵角を制御し、前記形状計算手段は、前記角度検出手段によって前記複数の点の各々について検出された俯角及び方位角と、前記姿勢算出手段によって算出された前記移動体の姿勢とに基づいて、前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差を計算することができる。
【発明の効果】
【００１７】
以上説明したように、本発明の移動体制御装置によれば、画像から検出された車線マーク又は目標コース上の複数の点の各々について、基準方向に対する前記点の方向の俯角及び方位角を検出し、前記複数の点の各々について検出された俯角及び方位角に基づいて、前記車線マーク又は目標コースの旋回曲率、及び前記車線マーク又は目標コースに対する前記移動体の横偏差を計算し、目標コースに追従するように前記移動体の旋回運動を制御することにより、処理負担の増加を抑制して、目標コースに精度よく追従することができる、という効果が得られる。
【図面の簡単な説明】
【００１８】
【図１】カメラ視点位置を基準とし、カメラ撮像方向を基準方向とした角度による視覚座標で示した図である。
【図２】目標コースに追従するための、旋回曲率ρの軌跡を追従する曲率ヨーレートｒｒと、横偏差Ｙｅｒを収束させる横偏差ヨーレートｒｙとを示した図である。
【図３】本発明の第１の実施の形態に係る車両制御装置の構成を示す概略図である。
【図４】本発明の第１の実施の形態に係る車両制御装置の構成を示す概略図である。
【図５】目標コースを示す図である。
【図６】車両の移動を旋回運動により規定することを示す図である。
【図７】直線道路の前方道路画像を示した図である。
【図８】横偏差係数と俯角との関係を示した図である。
【図９】旋回道路の前方道路画像を示した図である。
【図１０】曲率係数と俯角との関係を示した図である。
【図１１】方位角を説明するための図である。
【図１２】旋回半径４００ｍの前方道路形状を模式的に示した図である。
【図１３】旋回路追従時の設定値の例を示した図である。
【図１４】障害物回避制御と回避車線への追従制御を説明するための図である。
【図１５】検出範囲推定ならびに複数点の検出例を示す図である。
【図１６】次期制御コース形状ψｆ１、ψｆ２を予測的に検出する方法を説明するための図である。
【図１７】旋回曲率と横偏差とカメラ位置を示した平面図である。
【図１８】本発明の第１の実施の形態に係る車両制御装置における車両制御処理ルーチンの内容を示すフローチャートである。
【図１９】旋回曲率を算出した結果を示す図である。
【図２０】複合路の追従走行結果を示す図である。
【図２１】複合路の追従走行に車線変更追跡を車線横位置Ｙｄｒｉｖｅとして加算した走行結果を示した図である。
【図２２】本発明の第２の実施の形態に係る車両制御装置の構成を示す概略図である。
【図２３】制御ブロックの構成を示す図である。
【図２４】処理フローを示す図である。
【図２５】ヨーレートゲイン及びスリップ角ゲインを自律適応させる方法を説明するための図である。
【図２６】本発明の第２の実施の形態に係る車両制御装置における車両制御処理ルーチンの内容を示すフローチャートである。
【図２７】ヨーレートゲインを自律適応させる方法を説明するための図である。
【図２８】車体スリップ角とそれに関する要素を示した図である。
【図２９】スリップ角ゲイン適応係数符号とそれに関する要素の符号を示した図である。
【図３０】ヨーレートゲイン及びスリップ角ゲインを自律適応させる方法を説明するための図である。
【図３１】Ckr、Ckbに規定値を使用した場合の複合路の追従走行結果を示す図である。
【図３２】CkrRT,CkbRTを使用した場合の複合路の追従走行結果を示す図である。
【図３３】CkrRTを使用した場合の追従走行結果を示す図である。
【発明を実施するための形態】
【００１９】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２０】
＜第１の実施の形態の概要＞
＜目標コース形状検出（旋回曲率、横偏差の検出）＞
【００２１】
図１は、前方カメラにより前方道路を撮像しカメラ視点位置を基準とし、カメラ撮像方向を基準方向とした角度（縦方向俯角θ、横方向方位角ψ）による視覚座標で示している。車線マーク等に基づいて検出される目標コース上の２点の検出点の座標データψ１、ψ２から旋回曲率ρ＝１／Ｒと横偏差Ｙｅｒを、幾何形状に基づいた代数計算により算出する。
【００２２】
＜目標コース追従（旋回ヨーレート制御）＞
図２では、目標コースに追従するための、旋回曲率ρの軌跡を追従する曲率ヨーレートｒｒと、横偏差Ｙｅｒを旋回曲率軌跡に距離Ｌｙで収束させる横偏差ヨーレートｒｙとを示した。移動体のヨーレート制御を操舵角により行う場合、旋回曲率と横偏差のヨーレートを発生する操舵角ＭａＲ、ＭａＹを、後述する式（２８）（２９）に示す。これらの式は旋回運動の特性からヨーレート操舵角ゲインＣｋｒ、車速Ｖなどから算出される。移動体の位置制御はこれらが合算された操舵角ＭＡによって制御される旋回ヨーレートｒｍによって行われる。
【００２３】
コース形状検出とコース追従は上記の代数式の連続的な計算により実現され、移動体の走行位置制御機構を構築する。
【００２４】
＜車両制御装置の構成＞
図３、図４に示すように、本発明の第１の実施の形態に係る車両制御装置１０は、自車両の前方を撮像する前方カメラ１２と、自車両の速度を検出する車速センサ１４と、自車両のヨーレートを検出するヨーレートセンサ１６と、前方カメラ１２からの前方画像、車速センサ１４からの出力、及びヨーレートセンサ１６からの出力に基づいて、目標コースを追従するように、自車両の操舵角を制御するための操舵機構１８の作動を制御するコンピュータ２０とを備えている。
【００２５】
前方カメラ１２は、車両前方の対象領域を撮像し、画像信号を生成する撮像部（図示省略）、撮像部で生成されたアナログ信号である画像信号をデジタル信号に変換するＡ／Ｄ変換部（図示省略）、及びＡ／Ｄ変換された画像信号を一時的に格納するための画像メモリ（図示省略）を備えている。前方カメラ１２は、対象領域を、所定間隔で繰り返し撮像して、撮像により得られた前方画像を示す画像データをコンピュータ２０に出力する。
【００２６】
車速センサ１４は、自車両の車速を、所定間隔で繰り返し検出して、コンピュータ２０に出力する。
【００２７】
ヨーレートセンサ１６は、自車両のヨーレートを、所定間隔で繰り返し検出して、コンピュータ２０に出力する。
【００２８】
操舵機構１８は、コンピュータ２０による制御に従って、自車両の操舵角を変化させる。
【００２９】
コンピュータ２０は、ＣＰＵと、ＲＡＭと、後述する車両制御処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ２０は、画像取得部２２、車線マーク抽出部２４、目標コース検出部２６、車両情報取得部２８、検出係数演算部３０、検出点基準値演算部３２、検出点取得部３４、形状係数演算部３６、検出係数設定部３８、車線位置演算部４０、形状演算部４２、車両姿勢演算部４４、制御係数演算設定部４６、及び旋回制御部４８を備えている。なお、車線マーク抽出部２４及び目標コース検出部２６は、コース検出手段の一例であり、検出点取得部３４は、角度検出手段の一例であり、形状係数演算部３６は、形状係数計算手段の一例であり、形状演算部４２は、形状計算手段の一例である。
【００３０】
ここで、本実施の形態の原理について説明する。
【００３１】
＜原理＞
図５に示すように車両などの移動体の位置制御では、規定された道路形状により定まる目標コースを設定し、それを追従するように操舵制御される。目標コースの旋回半径は連続的に変化するが、時々刻々の挙動は、図６のような定常旋回として示せる。上記図６のように車両の移動は旋回運動により規定される。
【００３２】
旋回運動は式（１）、（２）で示され、旋回半径Ｒの旋回ヨーレートｒは車速Ｖにより規定される。目標となる旋回ヨーレートｒは車両特性値として定まるヨーレート操舵ゲインＣｋｒに応じた操舵角ＭＡにより制御される。しかし、車両の初期姿勢などにより旋回半径軌跡から横偏差が発生するため、それを補正する操舵の併用が必要となる。
【００３３】
本実施の形態の特徴はコース追従の目標となる旋回曲率ρ＝１／Ｒと横偏差Ｙｅｒを検出し、その旋回曲率を実現する旋回制御と、横偏差を旋回曲率軌跡へ収束させる旋回制御を行う。この目標検出と旋回制御を連続的に継続する。前方道路形状をカメラなどにより検出し、カメラ位置を基準にした視覚座標の幾何形状に基づいた代数計算により旋回曲率と横偏差を算出し、旋回曲率と横偏差の算出値を用いて旋回運動の特性に基づいた旋回制御により車両の位置制御を実現する。
【００３４】
定常旋回の運動式
１／Ｒ＝ｒ／Ｖ    ・・・（１）
ｒ＝Ｃｋｒ＊ＭＡ    ・・・（２）
ａ＝Ｖ２／Ｒ    ・・・（３）
【００３５】
ただし、ａは、旋回加速度であり、Ｃｋｒは、ヨーレート操舵ゲインであり、ＭＡは、操舵角であり、ｒは、旋回ヨーレートであり、ρは、旋回曲率（１／Ｒ）であり、Ｖは、車速である。
【００３６】
＜目標コース検出＞
目標コースは、カメラなどにより入力された前方道路画像から検出される。画像処理を用いて、道路面の濃淡変化などにより抽出された車線マークなどから、目標コースを検出する。本検出方法ではカメラ視点位置を基準値とした視覚座標値で取り扱う。横方向の角度を方位角ψ、縦方向を俯角θとして、検出点を角度表記する。
【００３７】
＜視覚座標としての横偏差の特性＞
図７に、直線道路の前方道路画像を示した。画像処理により抽出した車線マーク、及び車線マークから検出した目標コースを図中破線で示した。視覚座標における横偏差の特性は幾何形状により以下のように定量化される。
【００３８】
旋回曲率ρ＝０の場合、破線で示した検出点の横偏差方位角ψｙと横偏差Ｙｅｒの関係は横偏差係数Ｋｙにより式（４）で示される。
【００３９】
ψｙ＝Ｋｙ＊Ｙｅｒ    ・・・（４）
【００４０】
横偏差係数Ｋｙはカメラ視点高さＨｅと方位角ψと俯角θの関数として式（５）から算出できる。これらの関係を図８に示した。
【００４１】
Ｋｙ＝［ψ／ｔａｎ（ψ）＊ｔａｎ（θ）］／Ｈｅ    ・・・（５）
【００４２】
ただし、Ｋｙは、横偏差係数（ｄｅｇ／ｍ）であり、ψは、検出点方位角（ｄｅｇ）であり、Ｙｅｒは、横偏差（ｍ、目標コースからの偏差）であり、θは、検出点俯角（ｄｅｇ）であり、Ｈｅは、カメラ視点高さ（ｍ）である。
【００４３】
＜視覚座標としての旋回曲率の特性＞
図９に旋回道路の前方道路画像を示した。画像処理により抽出した車線マーク、及び車線マークから検出した目標コースを図中破線で示した。視覚座標における旋回曲率の特性は幾何形状により以下のように定量化される。
【００４４】
Ｙｅｒ＝０である場合、破線で示した検出点の曲率方位角ψｒと旋回曲率ρの関係は曲率係数Ｋｒにより式（６）で示される。
【００４５】
ψｒ＝Ｋｒ＊ρ    ・・・（６）
【００４６】
曲率あたりの方位角係数である曲率係数Ｋｒは、検出点の俯角θとカメラ視点高さＨｅの関数として式（７）、（８）から算出できる。これらの関係を図１０に示した。
【００４７】
Ｋｒ＝ｔａｎ−１（［ＫｒＲ−ｓｑｒｔ（ＫｒＲ２−Ｌθ２）］／Ｌθ）
＊ＫｒＲ＊ｒ２ｄ  ・・・（７）
Ｌθ＝Ｈｅ／ｔａｎ（θ）  ・・・（８）
【００４８】
ただし、Ｋｒは、曲率係数（ｄｅｇ・ｍ）であり、ψｒは、曲率方位角（ｄｅｇ）であり、ＫｒＲは、Ｋｒ算出半径（ｍ）であり、ＫｒＲ＝４００として算出可能である。また、θは、検出点俯角（ｄｅｇ）であり、Ｈｅは、カメラ視点高さ（ｍ）であり、Ｌθは、俯角距離（ｍ）であり、ｒ２ｄは、１８０／π（ｄｅｇ／ｒａｄ）であり、三角関数内の算出式のｒａｄを明示するものである。
【００４９】
Ｋｒ算出半径ＫｒＲは、それまでに検出した走行履歴の旋回半径に基づき設定できる。また、車速の関数として、例えば軌跡制御の対象となる道路の平均的な旋回半径を、Ｋｒ算出半径ＫｒＲとしてもよく、例えば高速道路などを対象とした場合、設定条件の例として、走行車速１００ｋｍ／ｈ、旋回加速度０．２Ｇとすると、式（１）（２）（３）より定まる旋回半径４００ｍを、Ｋｒ算出半径ＫｒＲとしても良い。
【００５０】
＜視覚座標としての方位角の特性＞
視覚座標の方位角の関係を図１１、式（９）に示す。
【００５１】
ψ＝ψｒ＋ψｙ＋ψｃ＋ψｏ    ・・・（９）
ψｃ＝ψｈ＋β    ・・・（１０）
ψｈ＝ｓｉｎ−１（Ｙｖ／Ｖ）＊ｒ２ｄ    ・・・（１１）
Ｙｖ＝ΔＹｅｒ／ｄｔ    ・・・（１１−１）
ψｏ＝ｔａｎ−１（Ｙｏｆｓ／Ｌθ）＊ｒ２ｄ    ・・・（１２）
Ｙｏｆｓ＝Ｙｄｒｉｖｅ−Ｙｌｉｎｅ    ・・・（１２−１）
【００５２】
ただし、ψｃは、車線偏差角であり、ψｈは、Ｙｅｒ差分とサンプリング時間ｄｔから求められた横偏差速度より算出される車体偏差角であり、ψｏは、加算操作となる車線位置偏差角であり、βは、車両特性値である車体スリップ角である。また、Ｖは、車両特性値である車速であり、Ｙｏｆｓは、車線座標の横位置制御目標である車線位置偏差である。また、Ｙｄｒｉｖは、車線変更、回避追跡などの車線横位置であり、Ｙｌｉｎｅは、目標コースの横偏差である。
【００５３】
方位角ψは式（９）のように、目標コースの曲率方位角ψｒ（式（６））、横偏差方位角ψｙ（式（４））、車線偏差角ψｃ（式（１０）、（１１））、車線変更などの追従コースから横偏差を伴う横位置制御による車線位置偏差角ψｏ（式（１２）（１２−１））によって生ずる各々の方位角の線形和と仮定する。
【００５４】
車線偏差角ψｃは車体偏差角ψｈと車体スリップ角βからなり、車体偏差角ψｈは、横偏差Ｙｅｒの速度と車速Ｖから定まる目標コースからの偏差角に相当する、検出した横偏差の走行履歴から算出される。
【００５５】
車両速度Ｖは車両状態量として車速センサにより検出される。
【００５６】
車体スリップ角βは車両特性によって生ずる旋回方向と車体方位角の差となる。車両諸元などにより車体スリップ角βを算出して（後述する式（４５））設定するか、近似的に車体スリップ角βを０とする。また、車両状態量のヨーレートｒ、車速Ｖの検出値から、車体スリップ角βを推定しても良い（後述する式（２１））。
【００５７】
＜視覚座標としての旋回曲率及び横偏差の算出法＞
規定された道路形状や車線変更などの位置制御を実現するコース追随の目標となる旋回曲率と横偏差の算出法を示す。
【００５８】
図１２は、旋回半径４００ｍの前方道路形状を模式的に示した図である。図１２に示すように目標コース上の２点を抽出し、検出方位角ψ１、ψ２とその俯角θ１、θ２を検出する。後述するように、θ１、θ２には、測定状況により検出精度が高くなる検出点の俯角や事前に設定した値を用いる。目標コースが移動体の旋回コース上にある必要はない。
【００５９】
式（１３）に示すように、目標コースの旋回曲率と横偏差の方位角成分を持つ曲率方位角ψｒ、横偏差方位角ψｙ、車線位置偏差角ψｏを加算した形状方位角ψｒｙから旋回曲率と横偏差を算出する。
【００６０】
ψｒｙ＝ψｒ＋ψｙ＋ψｏ  ・・・（１３）
【００６１】
ただし、ψｏ１、ψｏ２は、車線位置偏差角（ｄｅｇ）であり、ψｒｙ１，ψｒｙ２は、形状方位角（ｄｅｇ）である。
【００６２】
検出方位角ψ１、ψ２は、ψｒ、ψｙ、ψｃが加算された値となるので目標コースの旋回曲率と横偏差の方位角成分を持たないψｃ（式（１０）、（１１））をψ１、ψ２から差し引く。車線位置制御に伴う車線位置偏差角ψｏは、俯角位置θ１、θ２の位置の方位角（式（１２））としてψ１、ψ２に加算される。ψｃは方位角情報となるので俯角θによらず同値となる。
【００６３】
式（１４−１）、（１５−１）に示された形状方位角ψｒｙは追従目標となる旋回曲率と横偏差による方位角となる。
【００６４】
ψｒｙ１＝ψ１−ψｃ＋ψｏ１    ・・・（１４−１）
ψｒｙ２＝ψ２−ψｃ＋ψｏ２    ・・・（１５−１）
【００６５】
ただし、ψ１、ψ２は、検出点方位角（ｄｅｇ）であり、ψｃは、車線偏差角（ｄｅｇ）（ψｃ＝ψｃ１＝ψｃ２）である。
【００６６】
このように、ψｃを既知値としてψ１、ψ２から減算し、ψｏ１、ψｏ２を既知値としてψ１、ψ２に加算している。
【００６７】
ψｒｙ１、ψｒｙ２は、θ１、θ２の関数として得られる変換係数Ｋｒ、Ｋｙにより旋回曲率と横偏差の連立式（式（１４）、（１５））として示され、ρとＹｅｒについて式（１４）、（１５）を解くと、式（１６）、（１７）が得られ、旋回曲率と横偏差は式（１６）、（１７）の代数式により算出できる。
【００６８】
ψｒｙ１＝Ｋｒ１＊ρ＋Ｋｙ１＊Ｙｅｒ  ・・・（１４）
ψｒｙ２＝Ｋｒ２＊ρ＋Ｋｙ２＊Ｙｅｒ  ・・・（１５）
【００６９】
【数１】
2016029559.tif 000003
【００７０】
ただし、Ｋｙ、Ｋｒは俯角関数であり、検出点俯角θ１、θ２から式（５）、（７）より算出される。Ｋｒ１、Ｋｒ２は、曲率係数（ｄｅｇ・ｍ）であり、Ｋｙ１、Ｋｙ２は、横偏差係数（ｄｅｇ／ｍ）である。
【００７１】
図１２は２本の車線マークの形状検出した例を示しており、Ｌｉｎｅ１は旋回軌跡と目標コースが一致した場合、Ｌｉｎｅ２の横偏差は２ｍとなり、式（１２−１）の車線位置偏差角Ｙｌｉｎｅを２ｍに設定して追随走行の目標コースとする。この場合、式（１６）（１７）の代数式により（ψｃ＝ψｏ＝０）、ρ（＝１／Ｒ）、Ｙｅｒは以下のように算出される。
【００７２】
Ｌｉｎｅ１：Ｒ＝４００ｍ、Ｙｅｒ＝０ｍ
Ｌｉｎｅ２：Ｒ＝３９８ｍ、Ｙｅｒ＝２ｍ
【００７３】
＜視覚座標としての車体スリップ角の算出法＞
車体スリップ角は一般に車両特性値として車両重量などの車両諸元と車速から算出されるが、検出値から車体スリップ角を推定することも可能である。車両推定値として車速Ｖと旋回ヨーレートｒを測定し、式（１）より車両の旋回曲率を算出する。
【００７４】
軌跡追従のため検出した目標コース上の２点の検出点は検出俯角の特性により、車両測定値に比べ旋回情報を先読み（位置、時間）しており、フィルタ処理などにより車両測定値と同期させた方位角ψｓ１、ψｓ２を算出する。
【００７５】
方位角を車体スリップ角と横偏差方位角から成る特性方位角ψｂｙ（式（１８））として考える。特性方位角ψｂｙは式（１９−１）、（２０−１）に示すように、曲率方位角ψｒ’を式（１）、（６−１）から求め、ψｈを式（１０）（１１）から求め、同期方位角ψｓから差し引くことにより算出する。
【００７６】
ψｂｙ＝β＋ψｙ        ・・・（１８）
ψｒ’＝ｒ／Ｖ＊Ｋｒ      ・・・（６−１）
ψｂｙ１＝ψｓ１−ｒ／Ｖ＊Ｋｒ１−ψｈ    ・・・（１９−１）
ψｂｙ２＝ψｓ２−ｒ／Ｖ＊Ｋｒ２−ψｈ    ・・・（２０−１）
ψｂｙ１＝β＋Ｋｙ１＊Ｙｅｒ’    ・・・（１９）
ψｂｙ２＝β＋Ｋｙ２＊Ｙｅｒ’    ・・・（２０）
【００７７】
ただし、ψｂｙ１、ψｂｙ２は、特性方位角（ｄｅｇ）であり、ψｈは、車体偏差角（ｄｅｇ）であり、βは、車体スリップ角（ｄｅｇ）である。
【００７８】
このように、ｒ／Ｖ＊Ｋｒをψｒ’１、ψｒ’２としてψｓ１、ψｓ２から減算している。また、ψｈを既知値としてψｓ１、ψｓ２から減算している。
【００７９】
特性方位角ψｂｙは、式（１９）、（２０）に示すように車体スリップ角βと横偏差Ｙｅｒ’の連立式となり、βとＹｅｒ’について式（１９）（２０）を解くことにより、式（２１）、（２２）が得られ、式（２１）、（２１）により特性方位角ψｂｙ１、ψｂｙ２とＫｙ１、Ｋｙ２から代数式により車体スリップ角βを算出できる。
【００８０】
【数２】
2016029559.tif 000004
【００８１】
このように、スリップ角βは、ヨーレート、方位角などの検出値から式（２１）の代数式により算出される。
【００８２】
＜旋回軌跡に対する追従制御法＞
車両の位置から測定した目標コース上の２点の座標角度から、目標コースの旋回曲率と目標コースからの車両の横偏差とを検出した結果に基づき、上記図２に示したように目標コース上への車両の位置制御を行う。
【００８３】
旋回曲率ρの曲率ヨーレートｒｒは式（２３）に示されているように旋回曲率ρと車速Ｖの積として定まる。
【００８４】
ｒｒ＝ρ＊Ｖ・・・（２３）
【００８５】
旋回によって生ずる横偏差Ｙｅｒは式（２４）に示される横偏差ヨーレートｒｙの旋回に横偏差収束時間Ｔｙとして設定された時間（式（２６））で目標コース上の位置に収束できる。これらのヨーレートを加算した旋回ヨーレートｒｍの制御により目標コース上への位置制御を行う（式（２５））。
【００８６】
ｒｙ＝２＊Ｙｅｒ／（Ｔｙ２＊Ｖ）    ・・・（２４）
ｒｍ＝ｒｒ＋ｒｙ    ・・・（２５）
Ｔｙ＝Ｌｙ／Ｖ  ・・・（２６）
【００８７】
ただし、ｒｍは、旋回ヨーレート（ｒａｄ／ｓ、ｒｍ＝Ｃｋｒ＊ＭＡ）であり、ｒｒは、曲率ヨーレート（ｒａｄ／ｓ、ｒｒ＝Ｃｋｒ＊ＭａＲ）であり、ｒｙは、横偏差ヨーレート（ｒａｄ／ｓ、ｒｙ＝Ｃｋｒ＊ＭａＹ）であり、Ｔｙは、横偏差収束時定数（ｓ）であり、予め設定される。
【００８８】
旋回ヨーレートｒｍが車両特性により定まるヨーレート操舵角ゲインＣｋｒと操舵角ＭＡの積として制御される場合を示す（式（２））。旋回曲率、横偏差の旋回ヨーレートの特性式（２３）、（２４）、（２）から、各々の操舵角ゲインは式（２８）、（２９）のように定まる。また、追従制御補償として横偏差速度（式（１１−１））を追加し、式（２９−１）のようにしてもよい。Ｋｄｙは速度係数である。
【００８９】
ＭＡ＝ＭａＲ＋ＭａＹ      ・・・（２７）
ＭａＲ＝Ｖ／Ｃｋｒ＊ρ    ・・・（２８）
ＭａＹ＝２／（Ｔｙ２＊Ｖ＊Ｃｋｒ）＊Ｙｅｒ    ・・・（２９）
ＭａＹ＝２／（Ｔｙ２＊Ｖ＊Ｃｋｒ）＊（Ｙｅｒ＋Ｙｖ＊Ｋｄｙ）  ・・・（２９−１）
【００９０】
ただし、Ｃｋｒは、ヨーレート操舵角ゲイン（１／ｓ）であり、ＭＡは、操舵角（ｒａｄ）であり、ＭａＲは、曲率操舵角（ｒａｄ）であり、ＭａＹは、横偏差操舵角（ｒａｄ）である。
【００９１】
操舵角ＭＡは、式（２７）、（２８）、（２９）の代数式により算出される。
【００９２】
このように、旋回曲率ρの検出値から曲率操舵角ＭａＲ、横偏差Ｙｅｒの検出値から横偏差操舵角ＭａＹが算出され、それらを加算した操舵角ＭＡが設定され、旋回ヨーレートｒｍが制御され、目標コースへの追従走行が行われる。
【００９３】
旋回曲率に応じた曲率ヨーレートｒｒと横偏差に応じた横偏差ヨーレートｒｙを操舵角により制御する例を示したが、左右輪の駆動・制動力差により旋回モーメントの制御によっても同様の位置制御が可能である。Ｃｋｒをヨーレート駆動制御動力差ゲイン、ＭａＲを曲率駆動制動力差、ＭａＹを横偏差駆動制動力差、操舵角ＭＡを駆動制動力差に置き換えることにより、駆動制動力差の制御により、目標コース上を追従走行する位置制御が実現できる。
【００９４】
＜コース追従＞
目標コースの検出ならびにコース追従のための係数設定として、旋回路追従時の設定横向き加速度ａｒ、横偏差収束時間Ｔｙ、検出点補正係数Ｋｔが設定される。横向き加速度ａｒは車両特色や走行条件などにより設定される。横偏差収束時間Ｔｙ、検出点補正係数Ｋｔは、車速や車線追従、車線変更などの追従条件に応じて設定される。図１３に、旋回路追従時の設定横向き加速度ａｒを基準とした設定値の例を示した。
【００９５】
旋回中の車速を、設定横向き加速度ａｒから式（３０）により算出した設定車速Ｖｒを超えないように制御する。車速ＶがＶｒを超過したときは、速度制限もしくは警報により減速を誘導する。
Ｖｒ＝ｓｑｒｔ（ａｒ＊Ｒ）    ・・・（３０）
Ｌθ＝Ｖ＊Ｔｙ＊Ｋｔ    ・・・（３１）
【００９６】
追従軌跡の検出点俯角θは、横偏差収束時間Ｔｙ、補正係数Ｋｔ、車速Ｖから式（３１）、式（８）により算出する俯角距離Ｌθ(カメラ視点位置から検出点までの前方距離）を基準距離としてカメラ高さＨｅから設定される。上記図１３に設定車速Ｖｒと横偏差収束時間Ｔｙから算出した例を示した。俯角距離Ｌθは近傍の道路面から検出するが、追従走行において前方車両との車間距離によりLθ近辺の画像取得に影響を受ける場合Ｖ、Ｔｙ、Ｋｔ等の設定補正も可能である。
【００９７】
また、走行車速Ｖと設定横向き加速度ａｒから式（３０）により算出した旋回半径Ｒを式（７）のＫｒＲとして曲率係数を算出しても良い。
【００９８】
＜車線位置制御＞
任意の横位置を追従する場合の目標コースを車線位置偏差角ψｏ（式（１２））に記述する。車線位置制御によりコース取りを行う場合、旋回曲率に応じた車線位置を目標コースに加算して旋回時の車線走行位置を定める。旋回時の軌跡位置による旋回タイミングや余裕感を運転者の感覚に応じた設定や選択により、感覚にあったコース追従を設定する。
【００９９】
例えば、以下の式（３２）に従って、Ｙｄｒｉｖｅを設定する。
【０１００】
Ｙｄｒｉｖｅ＝Ｋｄｒ＊ρ    ・・・（３２）
【０１０１】
ただし、Ｋｄｒは、コース取り係数であり、設定値である（定数、曲率変化関数等にしても良い。）。同様の効果として、操舵角ＭＡに比例した車線位置を式（３２−１）のように車線位置偏差角ψｏに設定してもよい。Ｋｄｇは舵角比例コース取り係数である。
ψｏ＝Ｋｄｇ＊ＭＡ    ・・・（３２−１）
【０１０２】
車線位置制御により車線変更を行う場合には、運転者により車線変更支援の選択により、障害物がなく安全を確認できる場合、車速などに応じて設定した車線変更をする軌跡を目標コースに加算し、運転支援を可能とする。
【０１０３】
例えば、以下の式（３３）に従って、Ｙｄｒｉｖｅを設定する。
【０１０４】
Ｙｄｒｉｖｅ（ｔ）＝Ｙｌｃ＊ｆｌｃ＊ｔ−Ｓｌｃ＊ｓｉｎ（２π＊ｆｌｃ＊ｔ）
・・・（３３）
【０１０５】
ただし、Ｙｌｃは、車線変更幅であり、設定値である（例えば、Ｙｌｃ＝４ｍ）。またｆｌｃは車線変更周波数であり、設定値である（例えば、ｆｌｃ＝０．２Ｈｚ）。Ｓｌｃは、緩和係数であり、設定値である（定数、時間（距離）関数等にしても良い）。
【０１０６】
車線位置制御により、第１の回避支援を行う場合には、前方に障害物を発見し、制動停止が不可能で周囲に障害物がなく回避により安全を確保できる場合、障害物を回避する軌跡を目標コースに加算し、安全支援を可能とする。
【０１０７】
例えば、Ｙｄｒｉｖｅを時間関数とした場合、以下の式（３４）に従って、Ｙｄｒｉｖｅを設定する。
【０１０８】
Ｙｄｒｉｖｅ（ｔ）＝Ｙｅｖ＊ｆｅｖ＊ｔ−Ｓｅｖ＊ｓｉｎ（２π＊ｆｅｖ＊ｔ）
・・・（３４）
【０１０９】
ただし、Ｙｅｖは、回避幅であり、設定値である（例えば、Ｙｅｖ＝１ｍ）。ｆｅｖは、回避周波数であり、設定値である（ｆｅｖ＝０．５Ｈｚ）。Ｓｅｖは、緩和係数であり、設定値である（定数、時間（距離）関数等にしても良い）。
【０１１０】
上記の式（３２）、（３３）、（３４）は軌跡形状ならびに時間関数としたが、車線位置制御における横向き加速度、躍度などに基づいた軌跡形状としても良い。
【０１１１】
また、車線位置制御により、第２の回避支援を行う場合には、前方障害物を認識し、制動停止が不可能で周囲に障害物がなく回避により安全を確保できる場合、障害物、走行レーンや車線ラインなどとの相対位置から回避コースを前方画像上に重ね合わせ（図１４）、目標コースとして、旋回曲率と横偏差を算出し、障害物回避制御と回避車線への追従制御により安全支援を可能とする。上記図１４では、ψ１Ｒ０は、自車走行レーンの右車線マークを示し、ψ１Ｌ０は、左車線マークを示している。また、ψ１Ｒ１は、右接続レーンの右車線マークを示し、ψ１Ｌ１は、左接続レーンの左車線マークを示している。
【０１１２】
＜検出範囲推定及び次期コース検出＞
目標コースを車線マークなどから検出するが、車線マークと道路面との濃淡差の減少などにより検出率の低下が発生する。検出率の低下に対応して以下の検出精度向上の処理が行われる。
【０１１３】
本実施の形態における目標コースの検出法では、目標コースを旋回曲率と横偏差による幾何形状により定量化しており、その逆変換も可能である。車線マークの出現範囲を、式（６）、式（３５）に従って推定した範囲ψｗＲ、ψｗＬに設定する。その範囲内では車線マークの出現確率が高く、検出した車線マークに基づいて検出した目標コースは、信頼性が高い。
【０１１４】
ψｒ＝Ｋｒ＊ρ  ・・・（６）
ψｗＲ＝ψｒ＋Ｋｙ’＊（Ｙｅｒ＋Ｙｗ）
ψｗＬ＝ψｒ＋Ｋｙ’＊（Ｙｅｒ−Ｙｗ）・・・（３５）
【０１１５】
ただし、ρは、検出した曲率（１／ｍ）であり、Ｙｅｒは、検出横偏差（ｍ）であり、Ｋｒは、曲率係数（ｄｅｇ・ｍ、式（７））であり、Ｋｙ’は、近似横偏差係数（ｄｅｇ／ｍ）である。また、ψｗＲ、ψｗＬは、検出方位角範囲を規定する値（ｄｅｇ）であり、Ｙｗは、検出横範囲を規定する値（ｍ、例えば０．５ｍ）である。
【０１１６】
さらに道路面の複数の検出点から複数の車線マークを検出する。通常の車線幅は４ｍ程度であり、走行中の車線幅Ｙｌｗを検出できる。追従中の車線マークの検出確率か低下した場合、式（３６）に示した検出範囲ψｌｗＲ、ψｌｗＬには、対になる車線マークの出現確率が高い。
【０１１７】
ψｌｗＲ＝ψｒ＋Ｋｙ’＊（Ｙｅｒ＋Ｙｌｗ＋Ｙｗ）
ψｌｗＬ＝ψｒ＋Ｋｙ’＊（Ｙｅｒ＋Ｙｌｗ−Ｙｗ）    ・・・（３６）
Ｋｙ’＝θ／Ｈｅ    ・・・（５−１）
【０１１８】
ただし、Ｙｌｗは、検出車線幅（ｍ）であり、ψｌｗＲ、ψｌｗＬは、検出方位角範囲の車線幅（ｄｅｇ）であり、θは、検出点の俯角（ｄｅｇ）であり、Ｈｅは、カメラ視点高さ（ｍ）である。
【０１１９】
検出範囲ψｌｗＲ、ψｌｗＬから複数の車線マークを検出し、目標コースを検出するための車線マークの切り替えや、図１４のような複数車線の車線マークの統合により、車線幅を含むコース形状を信頼性の高い形状データとして用いて検出精度を向上する。また、カメラ位置を通過する目標コース推定も幾何形状から可能である。
【０１２０】
図１５に検出範囲推定ならびに複数点の検出例を示す。これらは式（３１）に示すＬθを俯角の基準として検出し主に旋回制御に直接使用する目標コース形状の検出精度を向上させるため併用する。
【０１２１】
図１６は目標コースの検出点を俯角方向に延長θｆ[１、２]することにより、次期制御コース形状ψｆ１、ψｆ２を予測的に検出する。コース形状の変化などにともなう次期制御の準備設定や現在制御の補償などに用い制御精度を向上させる。θｆ２はθ１として、現在の制御形状の遠方検出点を次期形状の近傍検出点としてもよい。
【０１２２】
また、車両の状態量検出センサによるヨーレートや車速などにより旋回曲率を推定できることを式（１）に示した。これらの測定結果は旋回曲率の補償に適用できる。
【０１２３】
＜曲率係数と横偏差係数の導出式＞
図１７に旋回曲率と横偏差とカメラ位置を平面図で示した。上記図１７に示した幾何形状の特性に基づき曲率係数と横偏差係数を導出する。関係式を式（３７）から式（４２）に示した。曲率係数の導出式を式(４３)、式（７−１）に示した。横偏差係数の導出式を式(５−２）に示した。
【０１２４】
（曲率係数と横偏差係数の算出のための関係式）
Ｒ2 ＝Ｘ2 ＋ Ｙ2        ・・・（３７）
Ｘ＝ Ｌθ        ・・・（３８）
Ｙ ＝ ｓｑｒｔ（Ｒ2−Ｌθ2）    ・・・（３９）
Ｙｒ ＝Ｒ − Ｙ        ・・・（４０）
Ｌθ ＝Ｈｅ ／ ｔａｎ（θ）       ・・・（４１）
Ｙｙ ＝ Ｌθ ＊ ｔａｎ（ψｙ）       ・・・（４２）
【０１２５】
（曲率係数と横偏差係数の導出式）
ψｒ＝ｔａｎ-1（Ｙｒ／Ｌθ）＊ｒ２ｄ
＝ｔａｎ-1 （[ Ｒ−ｓｑｒｔ（Ｒ2−Ｌθ2 ）]／Ｌθ）＊ｒ２ｄ      ・・・（４３）
Ｋｒ＝ψｒ／ρ
＝ｔａｎ-1 （[Ｒ−ｓｑｒｔ（Ｒ2−Ｌθ2 ）]／Ｌθ）＊ｒ２ｄ＊Ｒ・・・（７−１）
Ｋｙ＝ψｙ／Ｙｙ
＝ψｙ／［（Ｈｅ／ｔａｎ（θ））＊ｔａｎ（ψｙ）］
＝［ψｙ／ｔａｎ（ψｙ） ＊ ｔａｎ（θ）］ ／ Ｈｅ     ・・・（５−２）
【０１２６】
＜車両制御装置の各部の処理＞
上記で説明した原理に従って、本実施の形態では、画像取得部２２によって、前方カメラ１２によって撮像された前方画像を取得する。
【０１２７】
車線マーク抽出部２４は、画像取得部２２によって取得した前方画像から、車線マークを抽出する。目標コース検出部２６は、車線マーク抽出部２４によって抽出された車線マークに基づいて、目標コースを検出する。
【０１２８】
車両情報取得部２８は、車速センサ１４によって検出された車速Ｖ、及びヨーレートセンサ１６によって検出されたヨーレートｒを取得する。
【０１２９】
検出係数演算部３０は、形状演算部４２で計算された旋回曲率ρから求められる旋回半径Ｒに基づいて、Ｋｒ算出半径ＫｒＲを算出する。検出係数演算部３０は、形状係数演算部３６によって演算された横偏差係数Ｋｙ１、Ｋｙ２と、車両姿勢演算部４４によって演算された特性方位角ψｂｙ１、ψｂｙ２とに基づいて、上記式（２１）に従って、スリップ角βを演算する。または、制御係数演算設定部４６によって設定された車体スリップ角ゲインＣｋｂと、旋回制御部４８によって演算された操舵角ＭＡから式（４５）に従って車体スリップ角βを演算する。
【０１３０】
検出点基準値演算部３２は、車速Ｖと、制御係数演算設定部４６によって設定されたＴｙ、補正係数Ｋｔとに基づいて、上記式（３１）に従って、俯角距離Ｌθを演算する。また、検出点基準値演算部３２は、俯角距離Ｌθと、カメラ視点高さＨｅとに基づいて、上記式（８）に従って、検出点俯角θを演算する。
【０１３１】
検出点取得部３４は、目標コース検出部２６によって検出された目標コースと、検出点基準値演算部３２によって演算された検出点俯角θとに基づいて、目標コース上の検出点俯角θ付近の２点について、視覚座標（θ１、ψ１）、（θ２、ψ２）を取得する。
【０１３２】
形状係数演算部３６は、検出点取得部３４によって取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、カメラ視点高さＨｅに基づいて、上記式（５）に従って、横偏差係数Ｋｙ１、Ｋｙ２を演算する。形状係数演算部３６は、検出点取得部３４によって取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、検出係数演算部３０によって演算されたＫｒ算出半径ＫｒＲに基づいて、上記式（７）、式（８）に従って、曲率係数Ｋｒ１、Ｋｒ２を演算する。
【０１３３】
検出係数設定部３８は、横向き加速度ａｒ、横偏差収束時間Ｔｙ、及び検出点補正係数Ｋｔを設定すると共に、Ｋｒ算出半径ＫｒＲの初期値を設定する。
【０１３４】
車線位置演算部４０は、例えば、車線位置制御としてコース取りを行う場合、形状演算部４２によって演算された旋回曲率ρ、及びコース取り係数Ｋｄｒに基づいて、上記式（３２）に従って、車線横位置Ｙｄｒｉｖｅを算出する。また、車線位置演算部４０は、検出点取得部３４によって取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、車線横位置Ｙｄｒｉｖｅに基づいて、上記式（１２）、式（１２−１）に従って、車線位置偏差角ψｏ１、ψｏ２を演算する。
【０１３５】
形状演算部４２は、検出点取得部３４によって取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、車両姿勢演算部４４によって演算された車線偏差角ψｃ、及び車線位置演算部４０によって演算された車線位置偏差角ψｏ１、ψｏ２に基づいて、上記式（１４−１）、式（１５−１）に従って、形状方位角ψｒｙ１、ψｒｙ２を演算する。形状演算部４２は、形状方位角ψｒｙ１、ψｒｙ２と、形状係数演算部３６によって演算された曲率係数Ｋｒ１、Ｋｒ２並びに横偏差係数Ｋｙ１、Ｋｙ２とに基づいて、上記式（１６）、式（１７）に従って、旋回曲率ρ及び横偏差Ｙｅｒを演算する。形状演算部４２は、車線マーク抽出部２４によって抽出された車線マークに基づいて、車線幅Ｙｌｗなどの道路形状データを演算する。
【０１３６】
車両姿勢演算部４４は、形状演算部４２によって演算された横偏差Ｙｅｒの、前回値との差分ΔＹｅｒと、車速Ｖとに基づいて、上記式（１１）に従って、車体偏差角ψｈを演算する。車両姿勢演算部４４は、演算された車体偏差角ψｈと、車体スリップ角βとに基づいて、上記式（１０）に従って、車線偏差角ψｃを演算する。車両姿勢演算部４４は、方位角ψ１、ψ２をフィルタ処理などにより車両測定値と同期させた同期方位角ψｓ１、ψｓ２と、形状係数演算部３６によって演算された曲率係数Ｋｒ１、Ｋｒ２と、演算された車体偏差角ψｈと、車速Ｖと、ヨーレートｒとに基づいて、上記式（１９−１）、（２０−１）に従って、特性方位角ψｂｙ１、ψｂｙ２を演算する。
【０１３７】
制御係数演算設定部４６は、以下の式（４４）、（４５−１）に従って、ヨーレート操舵角ゲインＣｋｒ、車体スリップ角操舵角ゲインＣｋｂを演算する。
【０１３８】
Ｃｋｒ＝１／（１＋Ａ＊Ｖ２）＊Ｖ／Ｌ＊１／Ｎｇ    ・・・（４４）
β＝Ｃｋｂ＊ＭＡ      ・・・（４５）
Ｃｋｂ＝（１＋Ａ’＊Ｖ２）／（１＋Ａ＊Ｖ２）＊ｌｒ／Ｌ＊１／Ｎｇ＊ｒ２ｄ
・・・（４５−１）
Ａ＝−（ｍ＊（ｌｆ＊Ｃｆ−ｌｒ＊Ｃｒ））／（２＊Ｌ２＊Ｃｆ＊Ｃｒ）・・・（４６）
Ａ’＝−（ｍ＊ｌｆ）／（２＊Ｌ＊ｌｒ＊Ｃｒ）    ・・・（４７）
【０１３９】
ただし、Ｃｆは前輪コーナリングパワであり、Ｃｒは後輪コーナリングパワであり、Ｌは前輪軸−後輪軸の距離である。また、Ｉｆは前輪軸−重心の距離であり、ｌｒは後輪軸−重心の距離であり、ｍは車両質量である。Ｎｇは、操舵ギア比である。上記の各種値は、車両諸元により算出され、車速のマップデータとしてメモリ保存され、修正されるものである。
【０１４０】
制御係数演算設定部４６は、横偏差収束時定数Ｔｙ、補正係数Ｋｔを車線位置演算部４０の追従条件に応じて設定する。
【０１４１】
旋回制御部４８は、形状演算部４２によって演算された旋回曲率ρ及び横偏差Ｙｅｒと、制御係数演算設定部４６によって演算されたヨーレート操舵角ゲインＣｋｒ及び設定された横偏差収束時定数Ｔｙと、車速Ｖとに基づいて、上記式（２７）〜（２９）に従って、操舵角ＭＡを演算し、演算した操舵角ＭＡに従って、操舵機構１８を制御する。
【０１４２】
＜車両制御装置の作用＞
次に、第１の実施の形態に係る車両制御装置１０の作用について説明する。車両制御装置１０を搭載した車両の走行中に、コンピュータ２０において、図１８に示す車両制御処理ルーチンが実行される。
【０１４３】
まず、ステップ１００において、検出係数設定部３８は、横向き加速度ａｒ、横偏差収束時間Ｔｙ、及び検出点補正係数Ｋｔを設定すると共に、Ｋｒ算出半径ＫｒＲの初期値を設定する。
【０１４４】
そして、ステップ１０２において、車線位置演算部４０は、例えば、車線位置制御としてコース取りを行う場合、後述するステップ１１８において前回演算された旋回曲率ρ、及び予め設定されたコース取り係数Ｋｄｒに基づいて、上記式（３２）に従って、車線横位置Ｙｄｒｉｖｅを算出する。また、車線位置演算部４０は、検出点取得部３４によって取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、車線横位置Ｙｄｒｉｖｅに基づいて、上記式（１２）、式（１２−１）に従って、車線位置偏差角ψｏ１、ψｏ２を演算する。
【０１４５】
ステップ１０４において、車両情報取得部２８は、車速センサ１４によって検出された車速Ｖ、及びヨーレートセンサ１６によって検出されたヨーレートｒを取得する。
【０１４６】
ステップ１０６において、検出係数演算部３０は、ステップ１１８で前回計算された旋回曲率ρから求められる旋回半径Ｒに基づいて、Ｋｒ算出半径ＫｒＲを算出する。検出係数演算部３０は、後述するステップ１１６において前回演算された横偏差係数Ｋｙ１、Ｋｙ２と、後述するステップ１２０において前回演算された特性方位角ψｂｙ１，ψｂｙ２とに基づいて、上記式（２１）に従って、スリップ角βを演算する。また、ステップ１０２において設定した車線横位置Ｙｄｒｉｖｅ、ステップ１０４で取得した車速Ｖに応じてＴｙ、Ｋｔを補償演算する。
【０１４７】
ステップ１０８において、画像取得部２２によって、前方カメラ１２によって撮像された前方画像を取得する。
【０１４８】
ステップ１１０において、車線マーク抽出部２４は、上記ステップ１０８で取得した前方画像から、車線マークを抽出する。そして、目標コース検出部２６は、抽出された車線マークに基づいて、目標コースを検出する。
【０１４９】
ステップ１１２において、検出点基準値演算部３２は、上記ステップ１０４で取得した車速Ｖと、上記ステップ１００で設定されたＴｙ、Ｋｔとに基づいて、上記式（３１）に従って、俯角距離Ｌθを演算する。また、検出点基準値演算部３２は、俯角距離Ｌθと、カメラ視点高さＨｅとに基づいて、上記式（８）に従って、検出点俯角θを演算する。
【０１５０】
ステップ１１４において、検出点取得部３４は、上記ステップ１１０で検出された目標コースと、上記ステップ１１２で演算された検出点俯角θとに基づいて、目標コース上の検出点俯角θ付近の２点について、視覚座標（θ１、ψ１）、（θ２、ψ２）を取得する。
【０１５１】
ステップ１１６において、形状係数演算部３６は、上記ステップ１１４で取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、カメラ視点高さＨｅに基づいて、上記式（５）に従って、横偏差係数Ｋｙ１、Ｋｙ２を演算する。形状係数演算部３６は、上記ステップ１１４で取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、上記ステップ１０６で演算されたＫｒ算出半径ＫｒＲに基づいて、上記式（７）、式（８）に従って、曲率係数Ｋｒ１、Ｋｒ２を演算する。
【０１５２】
ステップ１１８において、形状演算部４２は、上記ステップ１１４で取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、ステップ１２０で前回演算された車線偏差角ψｃ、及び上記ステップ１０２で演算された車線位置偏差角ψｏ１、ψｏ２に基づいて、上記式（１４−１）、式（１５−１）に従って、形状方位角ψｒｙ１、ψｒｙ２を演算する。形状演算部４２は、形状方位角ψｒｙ１、ψｒｙ２と、上記ステップ１０６で演算された横偏差係数Ｋｙ１、Ｋｙ２とに基づいて、上記式（１６）、式（１７）に従って、旋回曲率ρ及び横偏差Ｙｅｒを演算する。形状演算部４２は、上記ステップ１１０で抽出された車線マークに基づいて、車線幅Ｙｌｗを演算する。
【０１５３】
ステップ１２０において、車両姿勢演算部４４は、上記ステップ１１８で演算された横偏差Ｙｅｒの、前回値との差分ΔＹｅｒと、上記ステップ１０４で取得した車速Ｖとに基づいて、上記式（１１）に従って、車体偏差角ψｈを演算する。車両姿勢演算部４４は、車体スリップ角βとに基づいて、上記式（１０）に従って、車線偏差角ψｃを演算する。車両姿勢演算部４４は、方位角ψ１、２をフィルタ処理などにより車両測定値と同期させた同期方位角ψｓ１、ψｓ２と、上記ステップ１１６で演算された曲率係数Ｋｒ１、Ｋｒ２と、演算された車体偏差角ψｈと、車速Ｖと、ヨーレートｒとに基づいて、上記式（１９−１）、（２０−１）に従って、特性方位角ψｂｙ１、ψｂｙ２を演算する。
【０１５４】
ステップ１２２において、制御係数演算設定部４６は、上記式（４４）に従って、ヨーレート操舵角ゲインＣｋｒを演算する。
【０１５５】
ステップ１２４において、旋回制御部４８は、上記ステップ１１８で演算された旋回曲率ρ及び横偏差Ｙｅｒと、上記ステップ１２２で演算されたヨーレート操舵角ゲインＣｋｒ及び横偏差収束時定数Ｔｙと、車速Ｖとに基づいて、上記式（２７）〜（２９）に従って、操舵角ＭＡを演算し、演算した操舵角ＭＡに従って、操舵機構１８を制御し、上記ステップ１０２へ戻る。
【０１５６】
＜実施例＞
次に、実際の検出結果ならびに軌跡追従制御の例を示す。
【０１５７】
＜旋回曲率の検出結果＞
図１９は、Ｘ軸方向へ直線路５０ｍから緩和区間１００ｍを経過し旋回半径４００ｍの定常旋回５０ｍのコース上を移動し、その座標位置から俯角４度と６度の２点の方位角を用いて旋回曲率を算出した結果を示している。ただし、検出条件として、ＫｒＲ＝４００ｍ、θ１＝４度、θ２＝６度、Ｖ＝１００ｋｍ／ｈ、検出周期＝６０Ｈｚを用いた。
【０１５８】
上記図１９に示すように、旋回路ならびに定常旋回における検出曲率は目標コースのコース曲率と一致している。緩和部では俯角θにより先行的に旋回曲率を検出しており、連続的な曲率変化を正しく検出できる。また、クロソイド接続部の曲率変化を緩和しコース追従時の横向き加速度変化を軽減する。
【０１５９】
＜追従操舵の制御結果＞
図２０、図２１は直線路と旋回路の複合路を目標コースとして追従走行した結果を示している。目標コースの形状は図中に示したように直線路Ｓと緩和（クロソイド）Ｃと定常旋回（Ｒ４００）Ｒからなる。車両は前方道路からカメラ測定した目標コース上の２点の方位角ψ１、ψ２と俯角θ１、θ２から旋回曲率１／Ｒと横偏差Ｙｅｒを検出し、各々の旋回ヨーレートを発生する操舵角ＭａＲ、ＭａＹとその加算操舵角ＭＡを算出し、ヨーレートゲインＣｋｒによって発生するヨーレートｒｍによる追従走行を連続的に繰り返す。制御条件として、θ１＝４度、θ２＝６度、Ｔｙ＝０．４ｓｅｃ、Ｖ＝１００ｋｍ／ｈ、検出方位角精度＝０．０５度、制御周期＝１０Ｈｚを用いた。
【０１６０】
図２０に複合路の追従走行結果を示した。検出横偏差Ｙｅｒについては検出方位角精度に応じた測定幅の変動が発生するが、カメラ位置の目標コースからのカメラ車線偏差は１０ｍｍ以下で追従制御している。目標コースの旋回曲率（追従コース曲率）と検出曲率は一致しており、正しく検出している。
【０１６１】
図２１に複合路の追従走行に車線変更追跡を車線横位置Ｙｄｒｉｖｅとして式（３３）に例出した設定値を加算した走行結果を示した。車線変更目標コース上をカメラ車線偏差（目標コースの追跡形状からの横偏差Ｙｅｒとして表示）が精度よく追従している。操舵角ＭＡは道路軌跡追従（図２０参照）に車線変更追従が重畳され制御されている。
【０１６２】
以上説明したように、第１の実施の形態に係る車両制御装置によれば、画像から検出された目標コース上の２点の各々について、カメラ視点からのカメラ撮像方向に対する当該点の方向の俯角及び方位角を検出し、２点の各々について検出された俯角及び方位角に基づいて、目標コースの旋回曲率、及び目標コースに対する自車両の横偏差を計算し、目標コースに追従するように自車両の旋回運動を制御することにより、処理負担の増加を抑制して、目標コースに精度よく追従することができる。
【０１６３】
また、自車両の位置制御を目標コース追従により行う。目標コース形状として旋回曲率と横偏差を検出し、その旋回曲率を実現する旋回制御と横偏差を旋回曲率軌跡へ収束させる旋回制御を行う。この形状検出と旋回制御の継続によりコース追従する。このように、目標コース形状を時々刻々の旋回曲率と横偏差として検出し、旋回運動として規定される挙動制御により、自車両の位置制御を簡便に高い精度で実現することができる。
【０１６４】
次に、第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については同一符号を付して説明を省略する。
【０１６５】
＜第２の実施の形態の概要＞
第１の実施の形態では、車両制御を事前に設定した制御特性や制御係数によって行う。制御対象である車両の特性、例えば重量などが変化した場合、制御対象の応答特性が想定条件と異なり、制御量の過不足などが発生し、制御精度が低下し、最適な位置制御ができなくなる場合がある。
【０１６６】
また、目標コース形状はコースと車両の相対姿勢から検出される。車両の姿勢が正しく検出されないと目標コース形状の測定誤差が発生し、制御精度が低下する。第１の実施の形態では、車両の姿勢，例えば車体スリップ角は操舵角に対するスリップ角ゲインから推定する。スリップ角ゲイン値の精度や制御対象の特性変化に伴うスリップ角ゲインの変化により姿勢検出の精度が低下し、制御の精度が低下する。
【０１６７】
そこで、本実施の形態では、目標コース追従のための車両の位置制御を行う車両制御装置において、実時間で、制御量に対する車両の挙動を検出または推定することにより、車両の応答特性を検出または推定し、制御特性を逐次更新して再設定することにより自律適応し、制御精度ならびに安定性を最適化する。
【０１６８】
＜車両制御装置の構成＞
図２２に示すように、本発明の第２の実施の形態に係る車両制御装置２１０のコンピュータ２２０は、画像取得部２２、車線マーク抽出部２４、目標コース検出部２６、車両情報取得部２８、検出係数演算部３０、検出点基準値演算部３２、検出点取得部３４、形状係数演算部３６、検出係数設定部３８、車線位置演算部４０、形状演算部４２、車両姿勢演算部４４、制御係数設定部２４６、制御係数演算部２５０、及び旋回制御部４８を備えている。なお、検出係数演算部３０が、姿勢算出手段の一例であり、制御係数演算部２５０が、制御ゲイン算出手段の一例である。
【０１６９】
ここで、本実施の形態の原理について説明する。
【０１７０】
＜原理＞
追従する目標コース形状を、前方カメラ１２による前方道路画像により撮影し、画像データから道路形状を定常旋回曲率ρ及び横偏差Yerとして算出する。車両の運動情報として、車速センサ１４、ヨーレートセンサ１６により、車速V、ヨーレートrを計測する。車両の制御を旋回ヨーレート制御によるとし、旋回曲率ρから設定したMaRと、横偏差Yerから設定したMaYとを加算した操舵角MAにより、旋回ヨーレート制御を行う（図２３、図２４参照）。
【０１７１】
図２３に示すブロック図、図２４に示す処理フロー図は、第１の実施の形態と共通であり、図２３の各ブロックの該当式を以下に示す。
【０１７２】
ρoutは、式(16)などで算出され、Youtは、式(17)などで算出され、KdRは、式(28)で算出され、KdYは、式(29)で算出される。また、Ty・Vは、式(26)、(31)などで算出され、Ckrは式(44)などで算出され、Ckbは式(45-1)などで算出される。sは微分演算子を表し、1/sは積分演算子を表している。
【０１７３】
制御対象の車両の車両重量などの特性変化により、操舵角に対するヨーレートゲインCkr、操舵角に対するスリップ角ゲインCkbが変化し、ヨーレートｒやスリップ角βが設定した値と異なり、制御誤差となり、位置精度、安定性が低下する。
【０１７４】
本実施の形態では、実時間でヨーレートゲインCkr、スリップ角ゲインCkbを検出することにより、最適な制御特性を再設定する。
【０１７５】
ヨーレートゲイン、スリップ角ゲインを実時間で測ればよいことは、自明であるが、実時間計測値の使用には処理による時間遅れや、検出要素の相互干渉などの理由で困難であった。本実施の形態では、処理の適正化により、これらの問題を解決し、実時間計測を可能とした。
【０１７６】
以下、第１の実施の形態で説明したCkr、Ckbと区別するため、実時間の検出値をCkrRT、CkbRTと表記する。
【０１７７】
＜車両制御装置の各部の処理＞
上記で説明した原理に従って、本実施の形態では、制御係数演算部２５０は、形状係数演算部３６によって演算された横偏差係数Ｋｙ１、Ｋｙ２と、車両姿勢演算部４４によって演算された特性方位角ψｂｙ１、ψｂｙ２とに基づいて、上記式（２１）に従って、スリップ角βを演算する（βRTとして明示する）。
【０１７８】
制御係数演算部２５０は、ヨーレートセンサ１６によって計測された旋回ヨーレートｒ、演算されたスリップ角βRT、及び旋回制御部４８によって発生させた操舵角MAを用い、以下の式(101)、(102)に従って、ヨーレートゲインCkrRT、スリップ角ゲインCkbRTを演算することにより自律適応する（図２５参照）。
【０１７９】
CkrRT = r / MA                     ・・・(101)
CkbRT= βRT/ MA                    ・・・(102)
【０１８０】
第１の実施の形態では、スリップ角として直接使用する形態で記述しており、そのため、同期方位角値ψs1,ψs2の精度、遅れなどにより、βRTの精度の制約を受けるが、スリップ角ゲインCkbRTとして設定し、操舵角関数のスリップ角とすることにより、スリップ角の高精度推定、ならびに遅れ特性の設定も容易になる。かつ、βRT算出のためのψs1,ψs2の要件精度を低くでき、各種の走行条件での安定した検出が可能になる。スリップ角の検出は、車両の高い精度の位置検出には重要である。本実施の形態のように方位角を用いた形状検出では、スリップ角を画像データから直接検出でき、安定した自律適応を実現する。
【０１８１】
制御係数設定部２４６は、制御係数演算部２５０で演算されたヨーレートゲインCkrRT、スリップ角ゲインCkbRTを用いて、ヨーレートゲインCkr、スリップ角ゲインCkbの再設定を行う。また、制御係数設定部２４６は、制御係数演算設定部４６と同様に、横偏差収束時定数Ｔｙ、補正係数Ｋｔを、車線位置演算部４０の追従条件に応じて設定する。
【０１８２】
旋回制御部４８は、形状演算部４２によって演算された旋回曲率ρ及び横偏差Ｙｅｒと、制御係数設定部２４６によって演算されたヨーレート操舵角ゲインCkrRT及び設定された横偏差収束時定数Ｔｙと、車速Ｖとに基づいて、上記式（２７）、（２８）、（２９）又は（２９−１）に従って、操舵角ＭＡを演算し、演算した操舵角ＭＡに従って、操舵機構１８を制御する。このように、再設定されたCkrRTを用いることにより、位置制御のための操舵角MA、MaR、MaYがつねに最適化される。
【０１８３】
検出係数演算部３０は、制御係数設定部２４６によって設定された車体スリップ角操舵角ゲインＣｋｂRTと、旋回制御部４８によって演算された操舵角ＭＡとから、上記式（４５）に従って車体スリップ角βを演算する。
【０１８４】
車両姿勢演算部４４は、形状演算部４２によって演算された横偏差Ｙｅｒの、前回値との差分ΔＹｅｒと、車速Ｖとに基づいて、上記式（１１）に従って、車体偏差角ψｈを演算する。車両姿勢演算部４４は、演算された車体偏差角ψｈと、検出係数演算部３０によって演算された車体スリップ角βとに基づいて、上記式（１０）に従って、車線偏差角ψｃを演算する。
【０１８５】
形状演算部４２は、検出点取得部３４によって取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、車両姿勢演算部４４によって演算された車線偏差角ψｃ、及び車線位置演算部４０によって演算された車線位置偏差角ψｏ１、ψｏ２に基づいて、上記式（１４−１）、式（１５−１）に従って、形状方位角ψｒｙ１、ψｒｙ２を演算する。
【０１８６】
このように、再設定されたCkbRTを用いることにより、目標コース形状検出のための方位角ψc，ψry1，ψry1がつねに最適化される。
【０１８７】
＜車両制御装置の作用＞
次に、第２の実施の形態に係る車両制御装置２１０の作用について説明する。車両制御装置２１０を搭載した車両の走行中に、コンピュータ２２０において、図２６に示す車両制御処理ルーチンが実行される。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【０１８８】
まず、ステップ１００において、検出係数設定部３８は、横向き加速度ａｒ、横偏差収束時間Ｔｙ、及び検出点補正係数Ｋｔを設定すると共に、Ｋｒ算出半径ＫｒＲの初期値を設定する。そして、ステップ１０２において、車線位置演算部４０は、ステップ１１８において前回演算された旋回曲率ρ、及び予め設定されたコース取り係数Ｋｄｒに基づいて、上記式（３２）に従って、車線横位置Ｙｄｒｉｖｅを算出する。また、車線位置演算部４０は、検出点取得部３４によって取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、車線横位置Ｙｄｒｉｖｅに基づいて、上記式（１２）、式（１２−１）に従って、車線位置偏差角ψｏ１、ψｏ２を演算する。
【０１８９】
ステップ１０４において、車両情報取得部２８は、車速センサ１４によって検出された車速Ｖ、及びヨーレートセンサ１６によって検出されたヨーレートｒを取得する。
【０１９０】
ステップ１０６において、検出係数演算部３０は、ステップ１１８で前回計算された旋回曲率ρから求められる旋回半径Ｒに基づいて、Ｋｒ算出半径ＫｒＲを算出する。検出係数演算部３０は、後述するステップ２７２で前回設定された車体スリップ角ゲインＣｋｂRTと、ステップ１２４で前回演算された操舵角ＭＡとから、上記式（４５）に従って車体スリップ角βを演算する。また、ステップ１０２において設定した車線横位置Ｙｄｒｉｖｅ、ステップ１０４で取得した車速Ｖに応じてＴｙ、Ｋｔを補償演算する。
【０１９１】
ステップ１０８において、画像取得部２２によって、前方カメラ１２によって撮像された前方画像を取得する。
【０１９２】
ステップ１１０において、車線マーク抽出部２４は、上記ステップ１０８で取得した前方画像から、車線マークを抽出する。そして、目標コース検出部２６は、抽出された車線マークに基づいて、目標コースを検出する。
【０１９３】
ステップ１１２において、検出点基準値演算部３２は、上記ステップ１０４で取得した車速Ｖと、上記ステップ１００で設定されたＴｙ、Ｋｔとに基づいて、上記式（３１）に従って、俯角距離Ｌθを演算する。また、検出点基準値演算部３２は、俯角距離Ｌθと、カメラ視点高さＨｅとに基づいて、上記式（８）に従って、検出点俯角θを演算する。
【０１９４】
ステップ１１４において、検出点取得部３４は、上記ステップ１１０で検出された目標コースと、上記ステップ１１２で演算された検出点俯角θとに基づいて、目標コース上の検出点俯角θ付近の２点について、視覚座標（θ１、ψ１）、（θ２、ψ２）を取得する。
【０１９５】
ステップ１１６において、形状係数演算部３６は、上記ステップ１１４で取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、カメラ視点高さＨｅに基づいて、上記式（５）に従って、横偏差係数Ｋｙ１、Ｋｙ２を演算する。形状係数演算部３６は、上記ステップ１１４で取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、上記ステップ１０６で演算されたＫｒ算出半径ＫｒＲに基づいて、上記式（７）、式（８）に従って、曲率係数Ｋｒ１、Ｋｒ２を演算する。
【０１９６】
ステップ１１８において、形状演算部４２は、上記ステップ１１４で取得した２点（θ１、ψ１）、（θ２、ψ２）の各々について、ステップ１２０で前回演算された車線偏差角ψｃ、及び上記ステップ１０２で演算された車線位置偏差角ψｏ１、ψｏ２に基づいて、上記式（１４−１）、式（１５−１）に従って、形状方位角ψｒｙ１、ψｒｙ２を演算する。形状演算部４２は、形状方位角ψｒｙ１、ψｒｙ２と、上記ステップ１０６で演算された横偏差係数Ｋｙ１、Ｋｙ２とに基づいて、上記式（１６）、式（１７）に従って、旋回曲率ρ及び横偏差Ｙｅｒを演算する。形状演算部４２は、上記ステップ１１０で抽出された車線マークに基づいて、車線幅Ｙｌｗを演算する。
【０１９７】
ステップ１２０において、車両姿勢演算部４４は、上記ステップ１１８で演算された横偏差Ｙｅｒの、前回値との差分ΔＹｅｒと、上記ステップ１０４で取得した車速Ｖとに基づいて、上記式（１１）に従って、車体偏差角ψｈを演算する。車両姿勢演算部４４は、上記ステップ１０６で演算された車体スリップ角βとに基づいて、上記式（１０）に従って、車線偏差角ψｃを演算する。車両姿勢演算部４４は、方位角ψ１、２をフィルタ処理などにより車両測定値と同期させた同期方位角ψｓ１、ψｓ２と、上記ステップ１１６で演算された曲率係数Ｋｒ１、Ｋｒ２と、演算された車体偏差角ψｈと、車速Ｖと、ヨーレートｒとに基づいて、上記式（１９−１）、（２０−１）に従って、特性方位角ψｂｙ１、ψｂｙ２を演算する。
【０１９８】
ステップ１２４において、旋回制御部４８は、上記ステップ１１８で演算された旋回曲率ρ及び横偏差Ｙｅｒと、ステップ２７２で前回演算されたヨーレート操舵角ゲインＣｋｒRTと、設定された横偏差収束時定数Ｔｙと、車速Ｖとに基づいて、上記式（２７）、（２８）、（２９）又は（２９−１）に従って、操舵角ＭＡを演算し、演算した操舵角ＭＡに従って、操舵機構１８を制御する。
【０１９９】
そして、ステップ２７０において、制御係数演算部２５０は、上記ステップ１１６で演算された横偏差係数Ｋｙ１、Ｋｙ２と、上記ステップ１２０で演算された特性方位角ψｂｙ１、ψｂｙ２とに基づいて、上記式（２１）に従って、スリップ角βRTを演算する。制御係数演算部２５０は、上記ステップ１０４で取得された旋回ヨーレートｒ、演算されたスリップ角βRT、旋回制御部４８によって発生させた操舵角MAを用い、以下の式(101)、(102)にしたがって、ヨーレートゲインCkrRT、スリップ角ゲインCkbRTを演算する。
【０２００】
ステップ２７２では、上記ステップ２７０で演算されたヨーレートゲインCkrRT、スリップ角ゲインCkbRTを用いて、ヨーレートゲインCkr、スリップ角ゲインCkbを再設定し、上記ステップ１０２へ戻る。
【０２０１】
以上説明したように、第２の実施の形態に係る車両制御装置によれば、車両制御において、旋回ヨーレートの計測値や、前方道路環境を撮影した画像データなどから、制御対象特性値となる操舵角に対するヨーレートゲインや、環境検出特性値となる操舵角に対する車体スリップ角ゲインなどを実時間で検出することにより、制御対象である車両の重量などの諸元が変化しても制御特性を適切な状態に保ち、制御の精度と安定性を維持することができる。また、最適な制御特性を実時間で再設定することにより、制御対象である車両の特性変化にも対応した車両制御特性を実現する。また、事前に想定した条件以外でも、最適な制御特性を実現できる。
【０２０２】
＜第３の実施の形態＞
次に、第３の実施の形態について説明する。なお、第３の実施の形態に係る車両制御装置の構成は、第２の実施の形態と同様の構成となるため、同一符号を付して説明を省略する。
【０２０３】
ここで、本実施の形態の原理について説明する。
【０２０４】
＜原理＞
本実施の形態では、上記式(45-1)のように設定したスリップ角ゲインが適用でき、車両の特性変化によるヨーレートゲインの変化に適応する。
【０２０５】
車速センサ１４により車速Vを計測し、前方カメラ１２による前方道路画像により撮影し、画像データとして道路形状を撮影する構成においては、上記第１の実施の形態で説明したように、上記式（１６）、（１７）に従って検出した目標コース形状の旋回曲率ρならびに横偏差Yer、車速Vとそれらから設定される操舵角MaR、MaYから、ヨーレートゲインCkrRTを適応検出する。
【０２０６】
旋回曲率に応じ操舵角は上記式(28)として設定され、同式からヨーレートゲインは式(103)となる。
【０２０７】
MaR = V / Ckr * ρ               ・・・(28)
Ckr = V *ρ/ MaR                 ・・・(103)
【０２０８】
車両特性変化によりCkrが設定値と異なる場合、制御誤差として横偏差が発生しMaYが式(29)により設定される。制御誤差に相当するMaYを収束させるようにCkrRTを式(104)により設定する。CkrRTが車両特性変化に対応した正値に収束するにつれ、MaY*KyRTは減少し、自律適応できる。ヨーレートゲイン適応係数KyRtは適応条件に応じた適正値を設定すればよい。
【０２０９】
MaY = 2 / ( Ty2* V * Ckr ) * Yer        ・・・(29)
CkrRT = V *ρ/ (MaR + MaY * KyRT )      ・・・(104)
【０２１０】
＜車両制御装置の各部の処理＞
制御係数演算部２５０は、車速センサ１４によって計測された車速Ｖ、形状演算部４２によって演算された旋回曲率ρ、及び旋回制御部４８によって発生させた操舵角MAを用い、上記式(104)にしたがって、ヨーレートゲインCkrRTを演算することにより自律適応する（図２７参照）。
【０２１１】
制御係数設定部２４６は、制御係数演算部２５０で演算されたヨーレートゲインCkrRTを用いて、ヨーレートゲインCkrの再設定を行う。また、制御係数設定部２４６は、制御係数演算設定部４６と同様に、横偏差収束時定数Ｔｙ、補正係数Ｋｔを車線位置演算部４０の追従条件に応じて設定する。また、制御係数設定部２４６は、スリップ角ゲインＣｋｂとして予め定められた値を設定する。
【０２１２】
なお、第３の実施の形態に係る車両制御装置の他の構成及び作用については、第２の実施の形態と同様であるため、説明を省略する。
【０２１３】
以上説明したように、第３の実施の形態に係る車両制御装置によれば、車両の特性変化に応じてヨーレートゲインを適応させることができる。
【０２１４】
＜第４の実施の形態＞
次に、第４の実施の形態について説明する。なお、第４の実施の形態に係る車両制御装置の構成は、第２の実施の形態と同様の構成となるため、同一符号を付して説明を省略する。
【０２１５】
ここで、本実施の形態の原理について説明する。
【０２１６】
＜原理＞
本実施の形態では、車両の特性変化による、ヨーレートゲイン、スリップ角ゲインの変化に適応する。
【０２１７】
車速センサ１４により車速Vを計測し、前方カメラ１２による前方道路画像により撮影し、画像データとして道路形状を撮影する構成においては、上記第１の実施の形態で説明したように、上記式（１６）、（１７）に従って検出した目標コース形状の旋回曲率ρならびに横偏差Yer、車速Vと、それらから設定される操舵角MaR，MaYとから車体スリップ角ゲインCkbRT、ヨーレートゲインCkrRTを適応する。
【０２１８】
ヨーレートゲインは上記式(104)により自律適応する。
【０２１９】
車体スリップ角は旋回中の旋回方向に対する車体方位角の差に相当する。
【０２２０】
目標コース形状の旋回曲率、横偏差は旋回方向と車体方位角が一致する状態で正しく検出でき、車体スリップ角による誤差を防ぐために車体スリップ角ゲインCkbなどにより検出角度を補正する（上記式(10)(13) (14-1)(15-2) (14)(15)(16)(17)(45)などを参照）。
【０２２１】
実際の車体スリップ角ゲインが車両特性変化などにより設定値と誤差が生ずると、旋回曲率、横偏差の検出誤差となる。そこで、本実施の形態では、横偏差を収束させることにより、車体スリップ角ゲインを適応させる。
【０２２２】
旋回時の車体方向と目標コースの旋回方向との方位角度差を車体スリップ角とする。旋回外側方向を外向きスリップ角、符号プラス（＋）で表し、逆の旋回内側方向を内向きスリップ角、符号マイナス（−）とする。スリップ角ゲインもスリップ角と同じ符号を用い、旋回方向、操舵角MAに依存した符号になる。
【０２２３】
また、右旋回に伴う旋回方向と操舵角MAをプラス（＋）とし、スリップ角によって検出される横偏差Yerも右方向を（＋）とする座標系を考える。スリップ角推定が正しく行われないとスリップ角誤差により図２８のようにYerが検出される。
【０２２４】
上記の関係から、車体スリップ角が内向きになる条件は、横偏差Yerに操舵角MAの逆符号（マイナス）を付加した値である、スリップ角偏差sYer（式(105)）がプラス＋になる。逆に同値がマイナス（−）になる条件で車体スリップ角は外向きになる。
【０２２５】
車体スリップ角による横偏差は通常補正されるが、スリップ角ゲインCkbの誤差により横偏差が誤検出される。式(105)のスリップ角偏差sYerがスリップ角偏差規定値Yebeta以下になるようにCkbRTを式(106)、(107)により自律適応する（図２９参照）。スリップ角偏差規定値Yebeta、スリップ角ゲイン適応係数Ckb_Rupは、適応条件に応じた適正値を設定する。
【０２２６】
sYer = Yer * sign(−MA)          ・・・(105)
【０２２７】
車体スリップ角が外向きとなる判定式(106)の成立で、式(107)に従って、CkbRTを（＋）側に増加させる。
【０２２８】
sYer > Yebeta                   ・・・(106)
CkbRT = CkbRT + Ckb_Rup          ・・・(107)
【０２２９】
一方、車体スリップ角が内向きとなる判定式(108)の成立で、式(109)に従って、CkbRTを（−）側に増加させる。
【０２３０】
sYer < ( −Yebeta )                    ・・・(108)
CkbRT = CkbRT − Ckb_Rup           ・・・(109)
【０２３１】
上記式(107)、(109)で設定したCkbRTが、車両特性変化に対応した正値に収束するにつれ、sYerは減少し、自律適応できる。
【０２３２】
また、CkbRTは適宜リセットする。例えば直進状態になった時（横向き加速度が規定値以下となった時）などにリセットすればよい。
【０２３３】
＜車両制御装置の各部の処理＞
制御係数演算部２５０は、車速センサ１４によって計測された車速Ｖ、形状演算部４２によって演算された旋回曲率ρ、及び旋回制御部４８によって発生させた操舵角MAを用い、上記式(104)にしたがって、ヨーレートゲインCkrRTを演算することにより自律適応する（図３０参照）。
【０２３４】
また、制御係数演算部２５０は、形状演算部４２によって演算された横偏差Ｙｅｒと、及び旋回制御部４８によって発生させた操舵角MAとを用い、上記式(105)にしたがって、スリップ角偏差sYerを演算する。制御係数演算部２５０は、演算したスリップ角偏差sYerが、スリップ角偏差規定値Yebetaより大きい場合には、スリップ角ゲインCkbRTを式(107)に従って演算する。また、制御係数演算部２５０は、演算したスリップ角偏差sYerが、−Yebetaより小さい場合には、スリップ角ゲインCkbRTを式(109)に従って演算する。
【０２３５】
制御係数設定部２４６は、制御係数演算部２５０で演算されたヨーレートゲインCkrRT、スリップ角ゲインCkbRTを用いて、ヨーレートゲインCkr、スリップ角ゲインCkbの再設定を行う。また、制御係数設定部２４６は、制御係数演算設定部４６と同様に、横偏差収束時定数Ｔｙ、補正係数Ｋｔを車線位置演算部４０の追従条件に応じて設定する。
【０２３６】
なお、第４の実施の形態に係る車両制御装置の他の構成及び作用については、第２の実施の形態と同様であるため、説明を省略する。
【０２３７】
以上説明したように、第４の実施の形態に係る車両制御装置によれば、車両の特性変化に応じてスリップ角ゲイン及びヨーレートゲインを適応させることができる。
【０２３８】
第４の実施の形態では、スリップ角ゲインの調節と、スリップ角推定による、ヨーレートゲイン推定の自律適応と、旋回曲率と横偏差の検出とをほぼ同時に行なう。これは、前方道路画像の画像データから目標コース形状を旋回曲率ρと横偏差Yerとして検出する検出式の相互関係に応じて、検出時期などを調整することにより可能になる。
【０２３９】
なお、上記の第４の実施の形態では、操舵角MaR、MaYの誤差、横偏差Yerの誤差の収束により、CkbRT、CkrRTを検出し、再設定する場合を例に説明したが、これに限定されるものではなく、旋回曲率誤差などの他の検出値の収束による適正化により、CkbRT、CkrRTや他の制御係数を検出し、再設定しても良い。
【０２４０】
また、上記の第２の実施の形態〜第４の実施の形態では、CkrRT、CkbRTの各々に対して、上限値と下限値を設定し、それを超える場合は適応値を設定するようにしてもよい。例えば、車両挙動が微小な状態（横向き加速度が規定値以下など）での過度な自律適応は行わないようにしてもよい。この場合には、再設定するCkr、Ckbの値を0、または初期設定値に準じた値に設定すればよい。
【０２４１】
また、上記第２の実施の形態〜第４の実施の形態では、検出されたCkrRT、CkbRTの値を、ヨーレートゲイン、スリップ角ゲインとして再設定する場合を例に説明したが、これに限定されるものではなく、前回値との重み付き平均を、ヨーレートゲイン、スリップ角ゲインとして再設定するようにしてもよい。
【０２４２】
また、上記第２の実施の形態〜第４の実施の形態で説明した、ヨーレートゲインCkrRTの算出方法を組み合わせて、ヨーレートゲインを再設定してもよい。また、上記第２の実施の形態と第４の実施の形態で説明した、スリップ角ゲインCkbRTの算出方法を組み合わせて、スリップ角ゲインを再設定してもよい。
【０２４３】
＜実施例＞
次に、第４の実施の形態で説明した手法を用いた軌跡追従制御の結果を示す。
【０２４４】
車両重量ｍを×1.2(基準より20%増加,5名乗車相当)とした場合のコース追従試算の時系列結果を図３１、図３２に示す。追従コースは直進区間(S)と定常旋回区間(R)をクロソイド緩和区間(C)で接続し、コース中央を追従した。また、旋回横向き加速度を0.2Gとし、定常旋回をR400とし、車速を100km/hとした。
【０２４５】
図３１は、Ckr、Ckbに規定値を使用した場合の結果を示す。旋回時に定常横偏差の増加が見られる。
【０２４６】
図３２は、上記第４の実施の形態で説明した方法によりCkrRT（真値：実線，算出値：二点鎖線）,CkbRT（真値：破線，算出値：一点鎖線）を算出し、車両増加状態に適応した場合の、コース追従試算の時系列結果を示す。適応により旋回時の定常横偏差の増加が減少していることが分かる。
【０２４７】
次に、第３の実施の形態で説明した手法を用いた軌跡追従制御の結果を示す。旋回半径に応じて旋回横向き加速度が0.2Gになる車速を設定する。
【０２４８】
車両重量ｍを×1.0(基準状態,1名乗車相当)、×1.2(20%増加,5名乗車相当)とした場合の、コース追従試算の追従誤差の横偏差rms値を図３３に示す。追従条件は図３１、図３２と同条件とし、通常の走行を想定した旋回半径を設定した。旋回半径に応じて走行距離は異なる。
【０２４９】
規定値を用いた場合には設定値誤差により高速（旋回半径大）の追従誤差が増加し、さらに、車両重量の増加に伴い誤差の増加が顕著になる。第３の実施の形態で説明した手法による適応（CkrRT）により追従誤差が減少し、追従精度と安定性の向上が確認できる。
【０２５０】
なお、上記の実施の形態では、前方カメラにより撮像された画像から車線マークを抽出している場合を例に説明したが、これに限定されるものではなく、後方カメラを更に用いて、後方カメラにより撮像された画像から車線マークを抽出するようにしてもよい。
【０２５１】
また、基準方向をカメラの撮像方向とし、目標コース上の２点の角度表記を、カメラ視点からのカメラ撮像方向に対する俯角及び方位角で表わす場合を例に説明したが、これに限定されるものではない。カメラ撮像方向以外の基準方向を用いて、目標コース上の２点の角度表記を、カメラ視点からの基準方向に対する俯角及び方位角で表わすようにしてもよい。
【０２５２】
また、目標コース上の２点について、俯角及び方位角を取得する場合を例に説明したが、これに限定されるものではなく、車線マーク上の２点を検出点として、俯角及び方位角を取得するようにしてもよい。
【０２５３】
また、旋回運動により移動位置の変更を行う移動体であれば、車両以外の移動体に適用してもよい。
【符号の説明】
【０２５４】
１０ 車両制御装置
１２ 前方カメラ
１４ 車速センサ
１６ ヨーレートセンサ
１８ 操舵機構
２０ コンピュータ
２２ 画像取得部
２４ 車線マーク抽出部
２６ 目標コース検出部
２８ 車両情報取得部
３０ 検出係数演算部
３２ 検出点基準値演算部
３４ 検出点取得部
３６ 形状係数演算部
３８ 検出係数設定部
４０ 車線位置演算部
４２ 形状演算部
４４ 車両姿勢演算部
４６ 制御係数演算設定部
４８ 旋回制御部
２１０ 車両制御装置
２２０ コンピュータ
２４６ 制御係数設定部
２５０ 制御係数演算部
【図１】
2016029559.tif 000005
【図２】
2016029559.tif 000006
【図３】
2016029559.tif 000007
【図４】
2016029559.tif 000008
【図５】
2016029559.tif 000009
【図６】
2016029559.tif 000010
【図７】
2016029559.tif 000011
【図８】
2016029559.tif 000012
【図９】
2016029559.tif 000013
【図１０】
2016029559.tif 000014
【図１１】
2016029559.tif 000015
【図１２】
2016029559.tif 000016
【図１３】
2016029559.tif 000017
【図１４】
2016029559.tif 000018
【図１５】
2016029559.tif 000019
【図１６】
2016029559.tif 000020
【図１７】
2016029559.tif 000021
【図１８】
2016029559.tif 000022
【図１９】
2016029559.tif 000023
【図２０】
2016029559.tif 000024
【図２１】
2016029559.tif 000025
【図２２】
2016029559.tif 000026
【図２３】
2016029559.tif 000027
【図２４】
2016029559.tif 000028
【図２５】
2016029559.tif 000029
【図２６】
2016029559.tif 000030
【図２７】
2016029559.tif 000031
【図２８】
2016029559.tif 000032
【図２９】
2016029559.tif 000033
【図３０】
2016029559.tif 000034
【図３１】
2016029559.tif 000035
【図３２】
2016029559.tif 000036
【図３３】
2016029559.tif 000037