(57)【要約】【課題】撮像装置が取得した画像、車速、車体のヨーレート及び車体の加速度から車体の姿勢角を推定する車体姿勢角推定装置を実現する。【解決手段】車体周辺の画像情報を取得する撮像装置２２と、撮像装置２２が取得した画像情報から、目標経路と車体との横位置偏差及び車体のヨー角偏差を算出する画像情報処理部２０と、横滑りを含む車体の運動との関係を表す式、及び車体の加速度を含む前後方向又は横方向の車体の運動の関係を表す式を用いて、横位置偏差、ヨー角偏差、車速センサ２４から得られる前後速度、ヨーレートセンサ２６から得られるヨーレート及び加速度センサ２８から得られる加速度を含む実測値から、車体の横速度及び車体の姿勢角を推定する演算装置１４と、を含んでいる。【選択図】図１
2020097316.tif 000002
【特許請求の範囲】
【請求項１】
車体周辺の画像情報を取得する撮像装置と、
前記撮像装置が取得した画像情報から、目標経路と車体との横位置偏差及び前記目標経路と車体のヨー角偏差を算出する画像情報処理部と、
前記目標経路上の目標点の移動と、横滑りを含む前記車体の運動との関係を表す式、及び前記車体の加速度を含む前後方向又は横方向の車体の運動の関係を表す式を用いて、前記横位置偏差、前記ヨー角偏差、車速センサから得られる前後速度、ヨーレートセンサから得られるヨーレート及び加速度センサから得られる加速度を含む実測値から、前記車体の横速度及び前記車体の姿勢角を推定する演算部と、
を含む車体姿勢角推定装置。
【請求項２】
前記目標経路上の目標点の移動と、横滑りを含む前記車体の運動との関係を表す式は、
前記前後速度から、前記ヨーレートと前記横位置偏差との積を減算したものを、前記ヨー角偏差を用いて補正した結果とヨー角偏差の正弦値との積から、前記横位置偏差の変化量と、前記車体のヨー運動に伴う車体重心位置に対する前記撮像装置の横速度とを減算した結果と、前記横速度とが等しい関係を表す式を含み、
前記車体の加速度を含む前後方向又は横方向の車体の運動の関係を表す式は、
前記前後速度の変化量から、前記ヨーレートと前記横速度との積及び重力加速度とピッチ角の正弦値との積を、減算した結果と、前記加速度センサから得られる前後加速度とが等しい関係を表す式を含み、
前記演算部は、前記姿勢角として前記ピッチ角を推定する請求項１に記載の車体姿勢角推定装置。
【請求項３】
前記車体の加速度を含む前後方向又は横方向の車体の運動の関係を表す式は、前記横速度の変化量に、前記ヨーレートと前記前後速度との積及び重力加速度とロール角の正弦値と前記ピッチ角の余弦値との積を、加算した結果と、前記加速度センサから得られる横加速度とが等しい関係を表す式をさらに含み、
前記演算部は、前記姿勢角として前記ピッチ角及び前記ロール角を推定する請求項２に記載の車体姿勢角推定装置。
【請求項４】
前記演算部は、前記ヨーレートに前記ピッチ角の変化量と前記ロール角の正弦値との積を加算した結果を前記ロール角の余弦値と前記ピッチ角の余弦値との積で除算して得た結果と、ヨー角の変化量とが等しい関係を表す式をさらに用いて、前記姿勢角として前記ピッチ角、前記ロール角及び前記ヨー角を推定する請求項３に記載の車体姿勢角推定装置。
【請求項５】
前記演算部は、前記目標経路上の目標点の移動と、横滑りを含む前記車体の運動との関係を表す式に基づいて、前回推定された、前記横速度及び前記姿勢角を含む前記車体の状態量から、次時刻の前記車体の状態量を予測する予測部と、
前記車体の加速度を含む前後方向又は横方向の車体の運動の関係を表す式を含む、予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車体の状態量に対応する、前記横位置偏差、前記ヨー角偏差、前記前後速度、前記ヨーレート及び前記加速度と、それぞれの前記実測値との差分、及び前回推定された前記車体の状態量に基づいて、前記予測部で予測された前記車体の状態量を補正することにより、前記横速度及び前記姿勢角を推定する更新部と、
を含む請求項１〜請求項４の何れか１項に記載の車体姿勢角推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車体姿勢角推定装置に係り、特に、走行路と横滑りを含む車体の運動の関係に基づき、車体の姿勢角を推定する車体姿勢角推定装置に関する。
【背景技術】
【０００２】
走行中の車体は、路面に対して前後運動、横運動及びヨー運動の３軸方向の運動を行うが、かかる運動の際に、車体は、地球座標系（すなわち大地）に対する角度である姿勢角が動的に変化する。例えば、旋回中の車体には、車体が左右方向に傾くロール角が生じる。また、加速している車体、及び減速している車体には、車体の進行方向に車体が傾くピッチ角が生じる。
【０００３】
車体のロール角及びピッチ角を把握できれば、車体の制御に係る操舵角、アクセル開度及びブレーキの強度の各々を適切に決定でき、車両の自動運転において、より安定した制御が可能となる。また、車体の姿勢角の的確な推定により、車両の地図上での移動量がより正確に算出できるので、自動運転において、車両の自己位置の推定がより適切に行える。
【０００４】
特許文献１には、車輪速センサと加速度センサとを用いて、車体の前後勾配を推定する路面勾配推定装置の発明が開示されている。
【０００５】
特許文献２には、車体のヨーレート、横加速度、前後加速度及び操舵角から車体の姿勢角を推定する道路形状予測装置の発明が開示されている。
【０００６】
特許文献３には、車体のヨーレート及び横加速度の値から路面キャンバ角を推定すると共に、車体の運動方程式から車体の横速度を推定する車線維持支援装置の発明が開示されている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２０１７−１４４９７３号公報
【特許文献２】特開２０１４−１５１８５３号公報
【特許文献３】特開２０１７−１３５２０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、特許文献１に記載の発明は、車体の旋回時の横滑りが考慮されていないので、横滑りが顕著になる交差点等を車体が走行する際に、ピッチ角の推定の精度が悪化するおそれがあった。
【０００９】
特許文献２に記載の発明は、使用している運動モデルに環境に対する柔軟性がなく、路面環境が変化した場合に、車体の姿勢角推定の精度が悪化するおそれがあった。
【００１０】
特許文献３に記載の発明は、操舵系の力学モデル及びタイヤの力学モデルを用いているが、特にタイヤの力学モデルは環境による変化が著しく、推定結果に悪影響を及ぼす恐れがある。また、上記のモデルは、車体の仕様に依存する部分が大きく、車体間の汎用性に難があるという問題があった。
【００１１】
本発明は、上記問題に鑑みてなされたものであり、撮像装置が取得した画像、車速、車体のヨーレート及び車体の加速度から車体の姿勢角を精度よく推定する車体姿勢角推定装置を実現することを目的とする。
【課題を解決するための手段】
【００１２】
上記目的を達成するために、請求項１に記載の車体姿勢角推定装置は、車体周辺の画像情報を取得する撮像装置と、前記撮像装置が取得した画像情報から、目標経路と車体との横位置偏差及び前記目標経路と車体のヨー角偏差を算出する画像情報処理部と、前記目標経路上の目標点の移動と、横滑りを含む前記車体の運動との関係を表す式、及び前記車体の加速度を含む前後方向又は横方向の車体の運動の関係を表す式を用いて、前記横位置偏差、前記ヨー角偏差、車速センサから得られる前後速度、ヨーレートセンサから得られるヨーレート及び加速度センサから得られる加速度を含む実測値から、前記車体の横速度及び前記車体の姿勢角を推定する演算部と、を含んでいる。
【００１３】
また、請求項２に記載の車体姿勢角推定装置は、請求項１に記載の車体姿勢角推定装置において、前記目標経路上の目標点の移動と、横滑りを含む前記車体の運動との関係を表す式は、前記前後速度から、前記ヨーレートと前記横位置偏差との積を減算したものを、前記ヨー角偏差を用いて補正した結果とヨー角偏差の正弦値との積から、前記横位置偏差の変化量と、前記車体のヨー運動に伴う車体重心位置に対する前記撮像装置の横速度とを減算した結果と、前記横速度とが等しい関係を表す式を含み、前記車体の加速度を含む前後方向又は横方向の車体の運動の関係を表す式は、前記前後速度の変化量から、前記ヨーレートと前記横速度との積及び重力加速度とピッチ角の正弦値との積を、減算した結果と、前記加速度センサから得られる前後加速度とが等しい関係を表す式を含み、前記演算部は、前記姿勢角として前記ピッチ角を推定する。
【００１４】
また、請求項３に記載の車体姿勢角推定装置は、請求項２に記載の車体姿勢角推定装置において、前記車体の加速度を含む前後方向又は横方向の車体の運動の関係を表す式は、前記横速度の変化量に、前記ヨーレートと前記前後速度との積及び重力加速度とロール角の正弦値と前記ピッチ角の余弦値との積を、加算した結果と、前記加速度センサから得られる横加速度とが等しい関係を表す式をさらに含み、前記演算部は、前記姿勢角として前記ピッチ角及び前記ロール角を推定する。
【００１５】
請求項４に記載の車体姿勢角推定装置は、請求項３に記載の車体姿勢角推定装置において、前記演算部は、前記ヨーレートに前記ピッチ角の変化量と前記ロール角の正弦値との積を加算した結果を前記ロール角の余弦値と前記ピッチ角の余弦値との積で除算して得た結果と、ヨー角の変化量とが等しい関係を表す式をさらに用いて、前記姿勢角として前記ピッチ角、前記ロール角及び前記ヨー角を推定する。
【００１６】
請求項５に記載の車体姿勢角推定装置は、請求項１〜請求項４の何れか１項に記載の車体姿勢角推定装置において、前記演算部は、前記目標経路上の目標点の移動と、横滑りを含む前記車体の運動との関係を表す式に基づいて、前回推定された、前記横速度及び前記姿勢角を含む前記車体の状態量から、次時刻の前記車体の状態量を予測する予測部と、前記車体の加速度を含む前後方向又は横方向の車体の運動の関係を表す式を含む、予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車体の状態量に対応する、前記横位置偏差、前記ヨー角偏差、前記前後速度、前記ヨーレート及び前記加速度と、それぞれの前記実測値との差分、及び前回推定された前記車体の状態量に基づいて、前記予測部で予測された前記車体の状態量を補正することにより、前記横速度及び前記姿勢角を推定する更新部と、を含んでいる。
【発明の効果】
【００１７】
本発明によれば、撮像装置が取得した画像、車速、車体のヨーレート及び車体の加速度から車体の姿勢角を精度よく推定する車体姿勢角推定装置を実現することができるという効果を奏する。
【図面の簡単な説明】
【００１８】
【図１】本発明の第１の実施の形態に係る車体姿勢角推定装置の構成の一例を示すブロック図である。
【図２】本発明の第１の実施の形態における座標系を示した概略図である。
【図３】撮像装置の位置、車両の重心、横位置偏差及びヨー角偏差の関係の一例を示した説明図である。
【図４】横滑りを含む車両の運動と目標経路との関係を示した説明図である。
【図５】本発明の第１の実施の形態に係る車体姿勢角推定装置の入出力の概略を示した説明図である。
【図６】本発明の第１の実施の形態に係る車体姿勢角推定装置の演算装置の機能ブロック図の一例である。
【図７】事前推定部の機能ブロック図の一例である。
【図８】フィルタリング部の機能ブロック図の一例である。
【図９】交差点での右左折時のように、横位置偏差とヨー角偏差とが大きい場合の車両の旋回の一例を示した概略図である。
【図１０】本発明の第２の実施の形態に係る車体姿勢角推定装置の入出力の概略を示した説明図である。
【図１１】本発明の第３の実施の形態に係る車体姿勢角推定装置の入出力の概略を示した説明図である。
【発明を実施するための形態】
【００１９】
［第１の実施の形態］
以下、図面を参照して本発明の実施の形態を詳細に説明する。図１に示すように、本実施の形態に係る車体姿勢角推定装置１０は、後述する演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、撮像装置２２が取得した画像情報から目標経路と車両との横位置偏差、車両のヨー角偏差及び目標経路の曲率を算出する画像情報処理部２０と、画像情報処理部２０が算出した横位置偏差、ヨー角偏差、曲率、車速センサ２４が検出した前後速度、ヨーレートセンサ２６が検出したヨーレート及び加速度センサが検出した加速度が入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいて車体の横速度及び車体の姿勢角の演算を行なうコンピュータ等で構成された演算装置１４と、演算装置１４で演算された姿勢角等を表示するＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。
【００２０】
続いて、車体２００の挙動に係る座標系を図２に示したように定義する。地球座標系２０４は地球平面を基準として重力加速度方向とｚeとが平行で、ｙeが北方向を向いている座標系である。路面座標系２０６は、ｚrが車体２００の重心を通り路面に垂直な方向に向き、ｘrは車両進行方向に向いている座標系である。車体座標系２０８は車体バネ上に固定された座標系で、ｚvは車体鉛直上方向、ｘvは車体進行方向を向いている。
【００２１】
また、オイラー姿勢角であるロール角φ、ピッチ角θ及びヨー角ψは、地球座標系２０４に対して、図２に示したように定義される。例えば、ロール角φはｘ軸まわりの回転角であり、ピッチ角θは、y軸まわりの回転角であり、ヨー角ψは、z軸まわりの回転角である。また、ロール角φ、ピッチ角θ及びヨー角ψの各々は、右ネジの方向（図２では、各々の矢印方向）の回転で正の値を示す。本実施の形態では、便宜上、後述するヨー角偏差は基準座標系を路面座標系２０６とし、さらに、本来は地球座標系２０４に対して定義されるオイラー姿勢角を、車体座標系２０８に対して、ロール角φv、ピッチ角θv及びヨー角ψvと定義する。以後、単に、ロール角φ、ピッチ角θ及びヨー角ψと記した場合は、基本的に、車体座標系２０８に対して定義された姿勢角であるとする。
【００２２】
本実施の形態は、撮像装置２２で取得した画像情報、車速センサ２４で検出した車速、ヨーレートセンサ２６で検出した車体のヨーレート及び加速度センサ２８で検出した車体の加速度を用いて車体２００の横速度（以下、「車体横速度」と略記）及び姿勢角を算出する。本実施の形態では車体横速度の算出に際して、図３にあるように、目標位置Ｐtにおける目標経路２２０である走行車線の曲率ρに加え、センサ位置Ｐsを基準とした目標位置Ｐtに対する横位置偏差ｅ1とヨー角偏差ｅ2とを想定する。なお、センサ位置Ｐsは車体重心Ｐrから車体２００の前方方向に距離（車体重心Ｐrとセンサ位置Ｐsとのオフセット距離）ｌsで隔てられた位置にあり、車載カメラ等の撮像装置２２が設けられている。図３に示したように、センサ位置Ｐsを基準とした車体２００の進行方向の移動量はｘSであり、センサ位置Ｐsを基準とした車体２００の横方向の移動量はｙsである。
【００２３】
次に、車体横速度を推定する際に用いる、横滑りを含む車体２００の運動と目標経路２２０との関係を図４に示す。一般に車体２００は路面に対して横滑りをしており、横速度を有する運動を伴っている。車体２００の挙動に対し、走行車線上の目標位置Ｐtは前後方向の速度のみを有し、横速度を有さない運動をしている。この特徴を利用し、車体２００の運動と目標位置Ｐtの運動との差分を導出する事で、車体横速度を推定するのに必要な条件式を導出する事が可能になる。
【００２４】
本実施の形態では、図３、４のように各変数を設定する。目標経路２２０上の目標位置Ｐtは、接線方向の速度成分Ｕtのみを持つとして、目標位置Ｐtの速度ベクトルをＶt=[Ｕt ０]T、目標位置ＰtをＰt=[ｘt ｙt ]T、地球座標系のx軸方向に対する接線方向のヨー角をψtと定義する。さらに車体２００は前後方向のみならず横速度を有するとして（車体スリップ角を有する）、路面座標系２０６の原点をＰr=[ｘr ｙr ]T、ヨー角をψr、車体２００の各軸方向の速度成分を図４に示したように、νr=[Ｕr Ｖr ]Tとする。本実施の形態では、後に車体座標系２０８を基準に計測されたセンサ情報との関係を容易にするため、偏差の基準座標系を路面座標系２０６とし、ヨー角偏差としてψe=ψt−ψrを定義する。
【００２５】
路面座標系２０６から見た目標位置Ｐtの偏差ベクトルをＰe=[ｘe ｙe ]とすると、当該偏差ベクトルＰeの微分は下記の２式で表される。
2020097316.tif 000003
【００２６】
車載カメラ等の撮像装置２２が取得した車体２００周辺の画像情報を、画像情報処理部２０において既知の手法によって画像解析することにより、目標位置Ｐtにおける走行車線の曲率ρに加え、センサ位置Ｐsを基準とした目標位置Ｐtに対する横位置偏差ｅ1とヨー角偏差ｅ2とが抽出される。撮像装置２２はセンサの一種であるから、当該撮像装置２２で取得した画像情報から抽出した曲率ρ、横位置偏差ｅ1及びヨー角偏差ｅ2をセンサで取得されるセンサ情報とする。さらに、車速センサ２４で検出する車体２００の前後方向の速度である前後速度Ｕr及びヨーレートセンサ２６で検出するヨーレートＲrをセンサ情報として、上記式を変形すると、下記の３式が得られる。
2020097316.tif 000004
【００２７】
さらに車体横速度ＶrとヨーレートＲrとについて上記３式を整理すると、下記の式（１）、（２）、（３）が得られる。
2020097316.tif 000005
【００２８】
上記の式（１）と式（３）から下記の式（４）が求められる。式（４）の右辺の変数は、いずれもセンサより取得可能である。従って、式（４）によれば、撮像装置２２、車速センサ２４及びヨーレートセンサ２６で各々検出したデータに基づいて車体横速度Ｖrを算出できる。
2020097316.tif 000006
【００２９】
式（４）は、前後速度Ｕrから車体２００のヨーレートＲrと横位置偏差ｅ1を乗算したＲrｅ1を減算することで旋回半径の補正を行い、さらにそれをヨー角偏差ｅ2の余弦cosｅ2で除算することで求めた目標経路２２０上の目標位置Ｐtの移動速度Ｕt（式（３）参照）に、ヨー角偏差ｅ2の正弦sinｅ2を乗算して得られる前後速度Ｕrの目標経路２２０に対する垂直成分、言い換えれば、前後速度Ｕrの車体座標系２０８のｙ座標成分であるＵtsinｅ2から、目標経路との横位置偏差ｅ1の変化量（ｅ1の微分値）と、車体２００のヨー運動に伴う車体重心位置Ｐrに対する撮像装置２２の横速度Ｒrｌsを引いたものが、車体２００の横速度Ｖrと一致することを示している。
【００３０】
なお、上記式（４）は、前後速度Ｕrから、ヨーレートＲrと横位置偏差ｅ1との積を減算したものを、ヨー角偏差ｅ2を用いて補正した結果とヨー角偏差ｅ2の正弦値との積から、横位置偏差ｅ1の変化量と、車体２００のヨー運動に伴う車体重心位置Ｐrに対する撮像装置２２の横速度Ｒrｌsとを減算した結果と、車体２００の横速度Ｖrとが等しい関係を表す式の一例である。
【００３１】
加速度センサ２８からは、車体２００の進行方向（ｘ軸方向）の前後加速度ａxが得られ、さらに車体２００の横方向（ｙ軸方向）の加速度である横加速度ａyが得られる。前後加速度ａx及び横加速度ａyの各々を検出するには、前後加速度ａx検出用の加速度センサ２８と、横加速度ａy検出用の加速度センサ２８とを各々備える。又は、前後加速度ａx及び横加速度ａyが共に検出可能な加速度センサ２８を用いる。
【００３２】
加速度センサ２８から得られる前後加速度ａx及び横加速度ａyの関係は、下記の車体２００の加速度を含む前後方向又は横方向の車体の運動の関係を表す式（５）、（６）の通りになる。
2020097316.tif 000007
【００３３】
基本的に、前後加速度ａxは、前後速度Ｕrの微分であるが、車体２００の横方向（ｙ軸方向）の挙動であるヨーレートＲr及び車体横速度Ｖrに影響される。また、重力加速度ｇも、前後加速度ａxに影響する。その結果、前後加速度ａxは、前後速度Ｕrの微分値（変化量）から、上記の式（４）で示される車体横速度ＶrとヨーレートＲrとの積及び重力加速度ｇとピッチ角θの正弦値との積を、減算した値に一致する。
【００３４】
また、基本的に、横加速度ａyは、車体横速度Ｖrの微分であるが、ヨーレートＲr及び前後速度Ｕrに影響される。また、重力加速度ｇも、横加速度ａyの値に影響する。その結果、横加速度ａyは、車体横速度Ｖrの微分値（変化量）に、ヨーレートＲrと前後速度Ｕrとの積及び重力加速度ｇとロール角φの正弦値とピッチ角θの余弦値との積を、加算した値に一致する。
【００３５】
前述のように、車体横速度Ｖrは式（４）を用いて算出でき、前後加速度ａx及び横加速度ａyは、加速度センサ２８で検出できる。従って、式（５）、（６）における未知数はロール角φ及びピッチ角θの２つなので、式（５）、（６）は一種の２元連立方程式となり、未知数であるロール角φ及びピッチ角θを算出できる。
【００３６】
また、オイラー姿勢角のピッチ角θの微分値と、車体方位角であるヨー角ψの微分値は、ピッチレートＱ及びヨーレートＲを用いて下記の式のように表される。
2020097316.tif 000008
【００３７】
本実施の形態では、ピッチレートを検出するセンサの使用を想定していないので、上記２式からピッチレートＱを消去すると、ヨー角ψの微分値（変化量）は下記の式で表される。
2020097316.tif 000009
【００３８】
上記の式は、後述する本発明の第３の実施の形態において、ヨー角ψを算出する際に用いられる。なお、上記の式は、ヨーレートＲにピッチ角θの変化量とロール角φの正弦値との積を加算した結果をロール角φの余弦値とピッチ角θの余弦値との積で除算して得た結果と、ヨー角の変化量とが等しい関係を表している。
【００３９】
図５は、本実施の形態に係る車体姿勢角推定装置１０の入出力の概略を示した説明図である。車体姿勢角推定装置１０を構成する演算装置１４には、画像情報処理部２０から横位置偏差ｅ1とヨー角偏差ｅ2とが入力される。また、車速センサ２４からは前後速度Ｕr、ヨーレートセンサ２６からは車体２００のヨーレートＲr、加速度センサ２８からは車体２００の前後加速度ａxの各々が、演算装置１４に入力される。なお、車体２００の前後方向は、車体座標系２０８のｘ軸に平行な方向である。
【００４０】
演算装置１４は、後述する演算処理によって、横位置偏差ｅ1、ヨー角偏差ｅ2、前後速度Ｕr、ヨーレートＲr及び前後加速度ａxから車体横速度Ｖr及び車体姿勢角である車体ピッチ角θvを推定する。
【００４１】
本実施の形態では、上述の目標経路２２０と車体２００の運動との関係を用いて、状態方程式ｆ（ｘ）を定義し、演算装置１４における事前推定とフィルタリングのステップとで活用する。事前推定及びフィルタリングのステップの詳細については、後述する。
【００４２】
まず、撮像装置２２より得られる目標経路２２０に対する横位置偏差ｅ1、ヨー角偏差ｅ2、前後速度Ｕr、ヨーレートＲr及び前後加速度ａxの情報を用いて、車体横速度Ｖr及びピッチ角θvを推定する場合の状態方程式ｆ（ｘ）は、以下のように定義する。
【００４３】
かかる場合の状態方程式ｆ（ｘ）は、下記のような、時間ｔにおける状態量ｘ（ｔ）に対して、下記の式（７）の関係が成り立つ。
2020097316.tif 000010
2020097316.tif 000011
【００４４】
上記の状態量ｘ（ｔ）に対して、上記式（７）の関係が成り立つ状態方程式ｆ（ｘ）を下記のように立式する。
2020097316.tif 000012
【００４５】
式（８）、（９）は、目標経路２２０と車体２００との運動の関係を示した微分方程式である。
【００４６】
また、本実施の形態では、各状態に対するシステムノイズを下記のように定義する。
2020097316.tif 000013
【００４７】
次にセンサによる観測量を下記のように定義する。
2020097316.tif 000014
【００４８】
ｙ=ｈ（ｘ）を満たす観測方程式ｈ（ｘ）は、下記の各式のように、状態量（右辺）と観測量（左辺）との対応を定義したものである。なお、一番下の式が、車体の前後加速度を含む前後方向の車体の運動の関係を表す式である。
2020097316.tif 000015
【００４９】
ただし、各観測量に対する観測ノイズを下記のように定義する。
2020097316.tif 000016
【００５０】
カルマンフィルタでは、式（８）、（９）を含む状態方程式ｆ（ｘ）の各々を離散化して使用する。従って、関数ｆ（ｘ）の入出力関係を、時間ｔ＝ｋ、ｋ−１に対して、ｘ（ｋ）＝ｆ（ｘ（ｋ−１））となる形で使用する。
【００５１】
図６は、本実施の形態に係る車体姿勢角推定装置１０の演算装置１４の機能ブロック図の一例である。図６に示したように、演算装置１４は、状態方程式ｆ（ｘ）を用いて、目標経路２２０である走行車線と横滑りを含む車体２００の運動との関係に基づく状態の推定を行うと共に、車体の前後加速度を含む前後方向の車体の運動の関係を表す式を含む観測方程式ｈ（ｘ）を用いて、対応するセンサ情報を推定する事前推定部１０２と、事前推定部１０２が出力した事前推定値を、センサ情報を用いて補正するフィルタリング部１０４と、を含む。
【００５２】
カルマンフィルタは線形・非線形を含め種々の手法が提案されている。本実施の形態では、前述のように状態方程式ｆ（ｘ）が非線形である事を踏まえ、非線形カルマンフィルタを活用する例を示す。中でも状態方程式ｆ（ｘ）についての線形化を要さず、かつ計算負荷が比較的小さいアンセンティッドカルマンフィルタを一例として採用する。状態方程式ｆ（ｘ）が線形の場合は、線形カルマンフィルタを用いてもよいし、アンセンティッドカルマンフィルタ以外の非線形カルマンフィルタを用いてもよい。
【００５３】
図７は、事前推定部１０２の機能ブロック図の一例である。図７に示したように、事前推定部１０２は、第１アンセンティッド変換部１０２Ａと第２アンセンティッド変換部１０２Ｂとを含む。
【００５４】
第１アンセンティッド変換部１０２Ａは、状態方程式ｆ（ｘ）に基づいて状態量を更新する1回目のアンセンティッド変換(Unscented transfer)を行って、ｘの平均及びｘの共分散行列を出力する。
【００５５】
第２アンセンティッド変換部１０２Ｂは、第１アンセンティッド変換部１０２Ａが出力した状態量ｘの平均及び状態量ｘの共分散行列を用いて、観測方程式ｈ（ｘ）に従って、対応する観測量に変換する２回目のアンセンティッド変換を行う。
【００５６】
なお、アンセンティッド変換の目的は、ある非線形関数ｙ＝ｆ（ｘ）による変換において、下記の観測量ｙの平均及び共分散行列を精度よく求めることにある。
2020097316.tif 000017
【００５７】
従って、本実施の形態は、平均値と標準偏差とに対応する2n+1個のサンプル(シグマポイント）を用いて、確率密度関数を近似することを特徴とする。
【００５８】
図７に示したように、第１アンセンティッド変換部１０２Ａは、シグマポイント重み係数部１０２Ａ１と、関数変換部１０２Ａ２と、Ｕ変換部１０２Ａ３と、を含む。また、第２アンセンティッド変換部１０２Ｂは、シグマポイント重み係数部１０２Ｂ１と、関数変換部１０２Ｂ２と、Ｕ変換部１０２Ｂ３と、を含む。
【００５９】
第１アンセンティッド変換部１０２Ａのシグマポイント重み係数部１０２Ａ１及び第２アンセンティッド変換部１０２Ｂのシグマポイント重み係数部１０２Ｂ１では、シグマポイントＸi：ｉ＝０、１、２、…２ｎが、下記のように選択される。
2020097316.tif 000018
【００６０】
ただし、スケーリングファクタκは、κ≧０となるように選択する。また、シグマポイントに対する重みは下記のように定義する。
2020097316.tif 000019
【００６１】
第１アンセンティッド変換部１０２Ａの関数変換部１０２Ａ２における、非線形関数ｆ（ｘ）による各シグマポイントの変換は下記のようになる。下記は、第１アンセンティッド変換部１０２Ａの関数変換部１０２Ａ２での状態方程式ｆ（ｘ）による変換だが、第２アンセンティッド変換部１０２Ｂの関数変換部１０２Ｂ２での観測方程式ｈ（ｘ）を用いた変換では、観測値が得られる。
2020097316.tif 000020
【００６２】
第１アンセンティッド変換部１０２ＡのＵ変換部１０２Ａ３では、関数ｆ（ｘ）によって変換された値と、前述の重み係数とを用いて、下記のように状態量ｘの平均値と状態量ｘの共分散行列を算出する。なお、下記式中のＱnはシステムノイズである。
2020097316.tif 000021
【００６３】
第２アンセンティッド変換部１０２ＢのＵ変換部１０２Ｂ３では、下記の計算を行う。
2020097316.tif 000022
【００６４】
図８は、フィルタリング部１０４の機能ブロック図の一例である。フィルタリング部１０４では、Ｕ変換部１０２Ｂ３で計算された、状態量の事前予測値に対応する観測値と、実際に観測された観測値との差を比較し、状態量の予測値を補正する処理を行う。
【００６５】
状態量の予測値を実際に観測された値でフィードバックする処理はカルマンゲイン(Kalman Gain)と呼ばれ、次式で計算される。なお、次式のＲnは観測ノイズである。
2020097316.tif 000023
【００６６】
次に、このカルマンゲインを用いて、状態量の事前予測値を補正する処理を次のように行う。
2020097316.tif 000024
【００６７】
以上の事前推定部１０２及びフィルタリング部１０４の処理を各タイムステップごとに繰り返すことにより、車体横速度Ｖr及び車体ピッチ角θvを推定する。
【００６８】
車体横速度Ｖr及び車体ピッチ角θvを推定する場合、フィルタリング部１０４が出力したｘ（ｋ）、Ｐx（ｋ）を前回値として事前推定部１０２に入力し、上述のように２段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測値を計算する。計算した状態量の予測値は、フィルタリング部１０４でカルマンゲイン及び実際に観測された最新の観測値を用いて補正される。フィルタリング部１０４が出力したｘ（ｋ）、Ｐx（ｋ）は、記憶装置１８に一時的にホールドし、記憶装置１８にホールドしたｘ（ｋ）、Ｐx（ｋ）を前回値として事前推定部１０２に入力する。
【００６９】
この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことによって車体横速度Ｖrと共に車体姿勢角である車体ピッチ角θvを推定する。
【００７０】
従来、車体姿勢角の推定では、車体横速度Ｖrを考慮していない場合が少なくない。図９に示したように、交差点での右左折又はレーンチェンジの場合には、横位置偏差ｅ1とヨー角偏差ｅ2とが増大し、車体横速度Ｖr及びヨーレートＲrが大きくなるため、車体姿勢角の推定に大きな誤差が生じるおそれがある。現在の自動運転技術は、高速道路等の曲率ρが小さい比較的なめらかな道路線形を有する経路を前提としているが、今後は曲率ρが大きい一般道を含めた自動運転を行うには、低速で大きく曲がるような場合でも車体姿勢角の高精度な推定が求められる。
【００７１】
本実施の形態に係る車体姿勢角推定装置１０は、既存の状態方程式に依存せず、かつ車体２００の運動について近似をしていないので、車体２００が目標経路２２０から大きく逸脱した場合でも正確に車体横速度Ｖr及び車体ピッチ角θvを推定することができる。
【００７２】
以上説明したように、本実施の形態に係る車体姿勢角推定装置１０によれば、撮像装置が取得した画像、車速、車体のヨーレート及び車体の加速度から車体２００の車体横速度Ｖr及び車体ピッチ角θvを精度よく算出できる。
【００７３】
［第２の実施の形態］
続いて、本発明の第２の実施の形態について説明する。本実施の形態は、後述するように、演算装置１４の入出力が第１の実施の形態と若干相違するが、車体姿勢角推定装置の構成は、第１の実施の形態に係る車体姿勢角推定装置１０と同一なので、当該構成についての詳細な説明は省略する。
【００７４】
図１０は、本実施の形態に係る車体姿勢角推定装置１０の入出力の概略を示した説明図である。車体姿勢角推定装置１０を構成する演算装置１４には、画像情報処理部２０から横位置偏差ｅ1とヨー角偏差ｅ2とが入力される。また、車速センサ２４からは車体２００の前後速度Ｕr、ヨーレートセンサ２６からは車体２００のヨーレートＲr、加速度センサ２８からは前後加速度ａx及び横加速度ａyの各々が、演算装置１４に入力される。
【００７５】
演算装置１４は、後述する演算処理によって、横位置偏差ｅ1、ヨー角偏差ｅ2、前後速度Ｕr、ヨーレートＲr、前後加速度ａx及び横加速度ａyから車体横速度Ｖr、車体ピッチ角θv及びロール角φvを推定する。
【００７６】
本実施の形態では、第１の実施の形態と同様に、目標経路２２０と車体２００の運動との関係を用いて、状態方程式ｆ（ｘ）を定義し、演算装置１４における事前推定とフィルタリングのステップとで活用する。
【００７７】
本実施の形態の状態方程式ｆ（ｘ）は、下記のような、時間ｔにおける状態量ｘ（ｔ）に対して、下記の式（１０）の関係が成り立つ。
2020097316.tif 000025
2020097316.tif 000026
【００７８】
上記の状態量ｘ（ｔ）に対して、上記式（１０）の関係が成り立つ状態方程式ｆ（ｘ）を下記のように立式する。
2020097316.tif 000027
【００７９】
式（１１）、（１２）は、目標経路２２０と車体２００との運動の関係を示した微分方程式である。
【００８０】
また、本実施の形態では、各状態に対するシステムノイズを下記のように定義する。
2020097316.tif 000028
【００８１】
次にセンサによる観測量を下記のように定義する。
2020097316.tif 000029
【００８２】
ｙ=ｈ（ｘ）を満たす観測方程式ｈ（ｘ）は、下記の各式のように、状態量（右辺）と観測量（左辺）との対応を定義したものである。下から２番目の式が、車体の前後加速度を含む前後方向の車体の運動の関係を表す式であり、一番下の式が、車体の横加速度を含む横方向の車体の運動の関係を表す式である。
2020097316.tif 000030
【００８３】
ただし、各観測量に対する観測ノイズを下記のように定義する。
2020097316.tif 000031
【００８４】
カルマンフィルタでは、式（８）、（９）を含む状態方程式ｆ（ｘ）の各々を離散化して使用する。従って、関数ｆ（ｘ）の入出力関係を、時間ｔ＝ｋ、ｋ−１に対して、ｘ（ｋ）＝ｆ（ｘ（ｋ−１））となる形で使用する。以下、第１の実施の形態と同様に、事前推定部１０２及びフィルタリング部１０４の処理を各タイムステップごとに繰り返すことにより、車体横速度Ｖr、車体ピッチ角θv及びロール角φvを推定する。
【００８５】
車体２００の車体横速度Ｖr、車体ピッチ角θv及びロール角φvを推定する場合も、フィルタリング部１０４が出力したｘ（ｋ）、Ｐx（ｋ）を前回値として事前推定部１０２に入力し、上述のように２段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測値を計算する。計算した状態量の予測値は、フィルタリング部１０４でカルマンゲイン及び実際に観測された最新の観測値で補正される。フィルタリング部１０４が出力したｘ（ｋ）、Ｐx（ｋ）は、記憶装置１８に一時的にホールドし、記憶装置１８にホールドしたｘ（ｋ）、Ｐx（ｋ）を前回値として事前推定部１０２に入力する。
【００８６】
この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、車体２００の車体横速度Ｖr、車体ピッチ角θv及びロール角φvを精度よく推定できる。
【００８７】
本実施の形態は、第1の実施の形態と同様に、既存の状態方程式に依存せず、かつ車体２００の運動について近似をしていないので、車体２００が目標経路２２０から大きく逸脱した場合でも正確に車体横速度Ｖr、車体ピッチ角θv及びロール角φvを正確に推定することができる。
【００８８】
以上説明したように、本実施の形態に係る車体姿勢角推定装置１０によれば、撮像装置が取得した画像、車速、車体のヨーレート及び車体の加速度から車体２００の車体横速度Ｖr、車体ピッチ角θv及びロール角φvを精度よく算出できる。
【００８９】
［第３の実施の形態］
続いて、本発明の第３の実施の形態について説明する。本実施の形態は、後述するように、演算装置１４の入出力が第１の実施の形態と若干相違するが、車体姿勢角推定装置の構成は、第１の実施の形態に係る車体姿勢角推定装置１０と同一なので、当該構成についての詳細な説明は省略する。
【００９０】
図１１は、本実施の形態に係る車体姿勢角推定装置１０の入出力の概略を示した説明図である。車体姿勢角推定装置１０を構成する演算装置１４には、画像情報処理部２０から横位置偏差ｅ1とヨー角偏差ｅ2とが入力される。また、車速センサ２４からは車体２００の前後速度Ｕr、ヨーレートセンサ２６からは車体２００のヨーレートＲr、加速度センサ２８からは前後加速度ａx及び横加速度ａyの各々が、演算装置１４に入力される。
【００９１】
演算装置１４は、後述する演算処理によって、横位置偏差ｅ1、ヨー角偏差ｅ2、前後速度Ｕr、ヨーレートＲr、前後加速度ａx及び横加速度ａyから車体横速度Ｖr、車体ピッチ角θv、ロール角φv及び車体方位角であるヨー角ψvを推定する。
【００９２】
本実施の形態では、第１の実施の形態と同様に、目標経路２２０と車体２００の運動との関係を用いて、状態方程式ｆ（ｘ）を定義し、演算装置１４における事前推定とフィルタリングのステップとで活用する。
【００９３】
本実施の形態の状態方程式ｆ（ｘ）は、下記のような、時間ｔにおける状態量ｘ（ｔ）に対して、下記の式（１３）の関係が成り立つ。
2020097316.tif 000032
【００９４】
上記の状態量ｘ（ｔ）に対して、上記式（１３）の関係が成り立つ状態方程式ｆ（ｘ）を下記のように立式する。
2020097316.tif 000033
【００９５】
式（１４）、（１５）は、目標経路２２０と車体２００との運動の関係を示した微分方程式である。
【００９６】
また、本実施の形態では、各状態に対するシステムノイズを下記のように定義する。
2020097316.tif 000034
【００９７】
次にセンサによる観測量を下記のように定義する。
2020097316.tif 000035
【００９８】
ｙ=ｈ（ｘ）を満たす観測方程式ｈ（ｘ）は、下記の各式のように、状態量（右辺）と観測量（左辺）との対応を定義したものである。
2020097316.tif 000036
【００９９】
ただし、各観測量に対する観測ノイズを下記のように定義する。
2020097316.tif 000037
【０１００】
カルマンフィルタでは、式（８）、（９）を含む状態方程式ｆ（ｘ）の各々を離散化して使用する。従って、関数ｆ（ｘ）の入出力関係を、時間ｔ＝ｋ、ｋ−１に対して、ｘ（ｋ）＝ｆ（ｘ（ｋ−１））となる形で使用する。以下、第１の実施の形態と同様に、事前推定部１０２及びフィルタリング部１０４の処理を各タイムステップごとに繰り返すことにより、車体横速度Ｖr、車体ピッチ角θv、ロール角φv及びヨー角ψvを推定する。
【０１０１】
車体２００の車体横速度Ｖr、車体ピッチ角θv、ロール角φv及びヨー角ψvを推定する場合も、フィルタリング部１０４が出力したｘ（ｋ）、Ｐx（ｋ）を前回値として事前推定部１０２に入力し、上述のように２段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測値を計算する。計算した状態量の予測値は、フィルタリング部１０４でカルマンゲイン及び実際に観測された最新の観測値で補正される。フィルタリング部１０４が出力したｘ（ｋ）、Ｐx（ｋ）は、記憶装置１８に一時的にホールドし、記憶装置１８にホールドしたｘ（ｋ）、Ｐx（ｋ）を前回値として事前推定部１０２に入力する。
【０１０２】
この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、車体２００の車体横速度Ｖr、車体ピッチ角θv、ロール角φv及びヨー角ψvを精度よく推定できる。
【０１０３】
本実施の形態は、第1の実施の形態と同様に、既存の状態方程式に依存せず、かつ車体２００の運動について近似をしていないので、車体２００が目標経路２２０から大きく逸脱した場合でも正確に車体横速度Ｖr、車体ピッチ角θv、ロール角φv及びヨー角ψvを正確に推定することができる。
【０１０４】
以上説明したように、本実施の形態に係る車体姿勢角推定装置１０によれば、撮像装置が取得した画像、車速、車体のヨーレート及び車体の加速度から車体２００の車体横速度Ｖr、車体ピッチ角θv、ロール角φv及びヨー角ψvを精度よく算出できる。
【符号の説明】
【０１０５】
１０   車体姿勢角推定装置
１２   入力装置
１４   演算装置
１６   表示装置
１８   記憶装置
２０   画像情報処理部
２２   撮像装置
２４   車速センサ
２６   ヨーレートセンサ
２８   加速度センサ
１０２ 事前推定部
１０２Ａ      第１アンセンティッド変換部
１０２Ａ１    シグマポイント重み係数部
１０２Ａ２    関数変換部
１０２Ａ３    Ｕ変換部
１０２Ｂ      第２アンセンティッド変換部
１０２Ｂ１    シグマポイント重み係数部
１０２Ｂ２    関数変換部
１０２Ｂ３    Ｕ変換部
１０４ フィルタリング部
２００ 車両
２０４ 地球座標系
２０６ 路面座標系
２０８ 車体座標系
２２０ 目標経路
【図１】
2020097316.tif 000038
【図２】
2020097316.tif 000039
【図３】
2020097316.tif 000040
【図４】
2020097316.tif 000041
【図５】
2020097316.tif 000042
【図６】
2020097316.tif 000043
【図７】
2020097316.tif 000044
【図８】
2020097316.tif 000045
【図９】
2020097316.tif 000046
【図１０】
2020097316.tif 000047
【図１１】
2020097316.tif 000048