JP7028223B2

JP7028223B2 - 自己位置推定装置

Info

Publication number: JP7028223B2
Application number: JP2019132950A
Authority: JP
Inventors: 大輝森; 義和服部; 豪軌杉浦; 裕之山口
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2019-07-18
Filing date: 2019-07-18
Publication date: 2022-03-02
Anticipated expiration: 2039-07-18
Also published as: JP2021018112A

Description

本発明は、自己位置推定装置に係り、特に、センサから出力される情報に基づいて自車両の位置を推定する自己位置推定装置に関する。

車両の姿勢角及び位置に係る情報は、運転支援技術及び予防安全技術にとって最も重要な情報である。例えば、車体のロール角及びピッチ角は、重力加速度成分を補償する制御入力（操舵角、アクセル及びブレーキ）を決定する際に活用でき、より安定した車両の制御が可能になる。さらに、位置推定は車両の位置制御に必要不可欠である。

特に現在の自動運転技術はＧＰＳ（Global Positioning System）又はＬＩＤＡＲ（Laser Imaging Detection and Ranging）等の車両以外の状況(外界状況)を検知する外界センサの情報が自車両の位置を推定する際に必須となっている。そして、かかる外界センサが機能しなくなった際のフェールセーフとして慣性航法（デッドレコニング）が重要な課題となっている。

デッドレコニングは、自車両の位置を直接検出するのではなく、走行時の車両の挙動を示す３軸の角速度（ピッチレート、ロールレート、ヨーレート）と３軸の加速度（前後加速度、横加速度、上下加速度）とが検出可能なＩＭＵ（Inertial Measurement Unit：慣性計測装置）、車速センサ及び操舵角センサ等の車両自身の状態を検知する内界センサで検出した自車両の移動の蓄積に基づいて自車両の位置を推定する慣性航法の方式である。

外界センサは雨や霧、夜間や逆光といった外界の環境によってその精度が大きく左右されやすい。デッドレコニングは、外界センサが全て使用不可になり内界センサは全て使用可能という状況を想定している。しかしながら、センサの各々で得意とする環境及び苦手とする環境は異なっており、外界センサが全て使用不可になり、内界センサのみ使用可能という状況は、一例に過ぎない。逆に外界センサは良好に機能しているが、内界センサが機能しなくなる場合も十分に想定され得る。

外界センサ及び内界センサを問わず、センサの各々が装置として正常であっても、外界の環境による攪乱（環境外乱）により、特定のセンサが異常値を出力する場合がある。図１１は、正しい推定位置６０、ＧＰＳに基づいた推定位置６２、ＬＩＤＡＲに基づいた推定位置６４、外界センサの一種である車載カメラ（撮像装置）が環境外乱により異常値を出力した場合に当該撮像装置で取得した車両周囲の画像に基づいた推定位置６８、及び撮像装置が出力した異常値と他のセンサの出力値とに基づいた推定位置６６の一例を示した概略図である。

図１１に示したように、異常値を出力する撮像装置で取得した車両周囲の画像に基づいた推定位置６８は、正常な推定位置６０から大きく外れる。また、撮像装置が出力した異常値と他のセンサの出力値とに基づいた推定位置６６も、正しい推定位置６０からは離れている。

正しい推定位置を得るには、装置としては正常であっても、異常値を出力するセンサを除外することが求められるが、当該センサを特定するには、各々のセンサに対して特有の評価方法を設定する必要があった。

特許文献１には、外部センサ及びＧＰＳセンサ等の複数の情報に基づいて車両の自己位置を推定すると共に各種センサ間の相関を算出し、センサ間の相関及び出力位置の相関の値に基づいて自己位置が正しく推定できているかどうかを判断する自動運転車両の制御システムの発明が開示されている。

特許文献２には、ＧＰＳに異常が生じた場合に、ＬＩＤＡＲを用いて位置の計測を行う自己位置推定装置の発明が開示されている。

特許文献３には、ＧＰＳセンサからの情報の途絶を検知した場合に、ＧＰＳセンサ以外のセンサを用いて自己位置の推定を継続すると共に、推定した自己位置の精度が基準値以下になると、運転者への車両制御の移行又は車両の自動停車を判断する自動走行制御装置の発明が開示されている。

特開２０１８－７７７７１号公報特開２０１７－９７４７９号公報特開２０１６－１９２０２８号公報

しかしながら、特許文献１に記載の発明は、入力される数多くの情報のうちの１の情報が劣化しただけで自己位置推定の精度が劣化するおそれがある。また、精度を評価する際に用いられる相関値の閾値は、複数回の走行から算出されるので、走行データが無い場所については自己位置推定の信頼性が担保できないという問題があった。

特許文献２に記載の発明は、ＧＰＳに異常が生じた場合に、車両の前後方向に連続しているような土砂の山やガードレール等の周辺環境をＬＩＤＡＲが常に認識ができることが前提となっているため、ＬＩＤＡＲが取得した周辺環境の情報が劣化すると異常検知ができなくなるという問題があった。

特許文献３に記載の発明は、ＧＰＳセンサの途絶検知はするものの、他のセンサの異常検知はしていないため、ＧＰＳセンサ以外のセンサが同時に途絶又は当該センサの出力値が劣化すると自動運転継続時間が短くなり、さらに、複数のセンサからの出力値が劣化した場合は、自動運転継続時間が極めて短くなるため緊急停車になるおそれがある。また、複数センサの協調による自己位置の推定が十分に達成できていないという問題があった。

本発明は、上記問題に鑑みてなされたものであり、出力値が異常なセンサを識別し、当該センサを除外することにより、自己位置推定の精度を担保できる自己位置推定装置を実現することを目的とする。

上記目的を達成するために、請求項１に記載の自己位置推定装置は、車両の位置推定に必要な観測量を検出して出力する複数の各々異なる検出部と、前記車両の位置を含む状態量の予測値を算出し、前記状態量の予測値に基づいて、前記複数の検出部の各々が検出した観測量の予測値を算出する事前推定部と、前記複数の検出部の各々について、前記検出部が新たに検出して出力した観測量と前記事前推定部が算出した観測量の予測値とに基づいて、異常値を出力する検出部であるか否かを識別する異常識別部と、前記異常識別部で異常値を出力する検出部でないと識別された前記検出部の各々によって検出して出力した前記観測量と、前記事前推定部が算出した前記検出部の各々の前記観測量の予測値との差分に基づいて、前記事前推定部によって算出した前記状態量の予測値を補正する位置推定部と、所定時間前から現在までの、異常値を出力する検出部でないと識別された前記複数の検出部の各々についての、前記補正された前記状態量の予測値から観測方程式を用いて計算される前記観測量と、前記検出部が新たに検出して出力した観測量との誤差に基づいて、観測方程式の誤差の広がりを更新する更新部と、を含み、前記位置推定部は、観測方程式の誤差の広がりを更に用いて、前記事前推定部によって算出した前記状態量の予測値を補正する。

また、請求項２に記載の自己位置推定装置は、請求項１に記載の自己位置推定装置において、前記事前推定部は、更に、前記状態量の誤差の広がりを算出し、前記状態量の誤差の広がりに基づいて前記複数の検出部の各々が検出する観測量の誤差の広がりを算出し、前記異常識別部は、前記複数の検出部の各々について、前記検出部が新たに検出して出力した観測量と、前記事前推定部が算出した観測量の予測値と、前記検出部が検出する前記観測量の誤差の広がりとに基づいて、異常値を出力する検出部であるか否かを識別する。

また、請求項３に記載の自己位置推定装置は、請求項１又は２に記載の自己位置推定装置において、前記異常識別部は、更に、異常値を出力する検出部でないと識別された前記検出部の各々によって検出して出力した前記観測量からなる観測値を構成し、異常値を出力する検出部でないと識別された前記検出部の各々の前記観測量の予測値からなる観測値の予測値を構成し、異常値を出力する検出部でないと識別された前記検出部の各々が検出する前記観測量の誤差の広がりからなり、前記観測方程式の誤差の広がりを示す観測量の誤差共分散行列を構成し、異常値を出力する検出部でないと識別された前記検出部の各々が検出する前記観測量と、前記状態量との共分散からなる、状態量と観測量との共分散行列を構成し、前記位置推定部は、前記構成された前記観測量の誤差共分散行列、及び前記状態量と観測量との共分散行列に基づいて、ゲインを計算し、
前記ゲイン、及び前記構成された前記観測値と前記観測値の予測値との差分に基づいて、前記状態量の予測値を補正する。

また、請求項４に記載の自己位置推定装置は、請求項１～３のいずれか１項に記載の自己位置推定装置において、前記検出部は、走行時の車両の挙動を示す角速度及び加速度、前記車両の前後速度、前記車両の操舵角、車両周辺に照射した電磁波の反射に基づいて前記車両の周辺の情報、前記車両の周辺の画像情報、並びに衛星から測位に係る情報を各々取得し、前記位置推定部は、前記検出部で取得した各々の情報のいずれかに基づいて測位する。

本発明によれば、出力値が異常なセンサを識別し、当該センサを除外することにより、自己位置推定の精度を担保できるという効果を奏する。

本発明の実施の形態に係る自己位置推定装置の一例を示したブロック図である。本発明の実施の形態における座標系を示した概略図である。本発明の実施の形態における変数の一例を示した説明図である。車両の重心における前後速度及び横速度、後輪車軸中心における前後速度及び横速度、前輪実舵角、重心から前輪車軸中心までの距離、重心から後輪車軸中心までの距離の関係を示した説明図である。横滑りを含む車両の運動と目標経路との関係の一例を示した説明図である。横滑りを含む車両の運動と目標経路との関係の一例を示した説明図である。本発明の実施の形態に係る自己位置推定装置の演算装置の機能ブロック図の一例である。本発明の実施の形態に係る自己位置推定装置の演算装置の事前推定部の機能ブロック図の一例である。本発明の実施の形態に係る自己位置推定装置の演算装置のセンサ異常判定部の機能ブロック図の一例である。本発明の実施の形態に係る自己位置推定装置の演算装置のフィルタリング部の機能ブロック図の一例である。正しい推定位置と、各種センサに基づいた推定位置の一例を示した概略図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。図１に示すように、本実施の形態に係る自己位置推定装置１０は、後述する演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、撮像装置２２が取得した画像情報から目標経路と車両との相対横位置、車両のヨー角及び目標経路の曲率を算出する画像情報処理部２０と、画像情報処理部２０が算出した横位置偏差、ヨー角偏差及び曲率、車速センサ２４が検出した車両前後速度、ＩＭＵ２６が検出した車両の方位角の偏差及び加速度、操舵角センサ２８が検出した車両の操舵角、ＧＰＳ３０で検出した車両の現在位置及び現在のヨー角（方位角）、ＬＩＤＡＲ３２で検出した車両の現在位置及び現在のヨー角、並びにＶ２Ｘ通信部３４が無線通信で取得した情報が各々入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいて車両の自己位置の推定の演算を行なうコンピュータ等で構成された演算装置１４と、演算装置１４で演算された車両の位置等を表示するＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。

本実施の形態に係る外界センサである撮像装置２２は車載カメラ等であり、一例として、撮影により取得した車両周辺の画像情報を解析して道路の白線等を検出する。又は、画像と高精度地図とのマッチングから現在位置の座標及び車両の方位角を算出してもよい。ＬＩＤＡＲ３２は、一例として、車両周辺に照射したパルス状のレーザ（電磁波）の散乱光から道路の白線等を検出する。

続いて、車両２００の挙動に係る座標系を図２に示したように定義する。地球座標系２０４は地球平面を基準として重力加速度方向とｚ_eとが平行で、ｙ_eが北方向を向いている座標系である。路面座標系２０６は、ｚ_rが車両２００の重心を通り路面に垂直な方向に向き、ｘ_rは車両進行方向に向いている座標系である。車体座標系２０８は車体バネ上に固定された座標系で、ｚ_vは車体鉛直上方向、ｘ_vは車体進行方向を向いている。従って、車両２００の前後方向は、車体座標系２０８のｘ軸に平行な方向となる。後述するように、本実施の形態では、車体座標系２０８の基準点を車両２００の重心ではなく、車両２００の後輪の車軸の車幅方向の中心とする。

また、オイラー姿勢角であるロール角φ、ピッチ角θ及びヨー角ψは、地球座標系２０４に対して、図２に示したように定義される。例えば、ロール角φはｘ軸まわりの回転角であり、ピッチ角θは、y軸まわりの回転角であり、ヨー角ψは、z軸まわりの回転角である。また、ロール角φ、ピッチ角θ及びヨー角ψの各々は、右ネジの方向（図２では、各々の矢印方向）の回転で正の値を示す。本実施の形態では、便宜上、後述するヨー角偏差は基準座標系を路面座標系２０６とし、さらに、本来は地球座標系２０４に対して定義されるオイラー姿勢角を、車体座標系２０８に対して、ロール角φ_v、ピッチ角θ_v及びヨー角ψ_vと定義する。以後、単に、ロール角φ、ピッチ角θ及びヨー角ψと記した場合は、基本的に、車体座標系２０８に対して定義された姿勢角であるとする。

図３は、本実施の形態における変数の一例を示した説明図である。本実施の形態では、車両２００の前後速度Ｕ、車両２００の横速度Ｖ及び車両２００の上下速度Ｗの各々を定義する。Ｕはｘ軸、Ｖはｙ軸及びＷはｚ軸に各々平行する。

また、車両２００のロール角φ、ピッチ角θ、ヨー角ψに対応するＩＭＵ２６の出力値は、角速度であるロールレートＰ、ピッチレートＱ、ヨーレートＲと定義する。

従来はＩＭＵ２６、ジャイロセンサ等の精度が不十分であったこともあり、ヨーレートＲの推定にも車両運動モデルを活用していた。しかし近年、安価なＩＭＵ２６に用いられるＭＥＭＳジャイロの精度が向上していると共に、車速センサ及び操舵角センサ等の複数センサが車両２００に搭載されることで特にヨーレートＲについては補正が容易になった。

本実施の形態では、車両運動モデルのヨーレートは使用せず、ＩＭＵ２６が検出したヨーレートＲの値を使用する。

本実施の形態では、目標経路２２０と車両２００の運動との関係等を用いて、状態方程式ｆ（ｘ）を定義し、演算装置１４における事前推定とフィルタリングのステップとで活用する。事前推定及びフィルタリングのステップの詳細については、後述する。

車両２００の状態量を示す状態変数ｘは、下記のように定義する。

上記のｘでは、地球座標系２０４において、基準となる地点から車両２００までの相対的な位置を示すＥＮ座標での位置を(Ｅ_v Ｎ_v)とする。Ｅ_vは当該地点から経度方向の位置を、Ｎ_vは当該地点から緯度方向の位置を、各々メートルで表したものである。

また、上記ｘでは、車両２００の姿勢角をロール、ピッチ、ヨーの順に(φ_v θ_v ψ_v)とし、車体座標系２０８における速度を前後速度、横速度の順で(Ｕ_v Ｖ_v)とし、回転角速度をロールレート、ピッチレート、ヨーレートの順で(Ｐ_v Ｑ_v Ｒ_v)とし、操舵角をδとし、路面上の白線に対する横位置、ヨー角を(ε_v φ_v)とし、白線の曲率をρとする。

続いて、上記の状態変数ｘに係る状態方程式について説明する。状態方程式は下記の式のように定義される。以下、状態変数ｘの各々に係る状態方程式を導出する。

地球座標系２０４のx軸、y軸及びｚ軸の各々における回転行列を下記のように定義する。

地球座標系２０４における車両２００の速度ｖ_eは、車体座標系２０８の車両２００の速度ｖ_vとオイラー角の定義とに基づいて、下記のように表現できる。

ｖ_v＝(Ｕ_v Ｖ_v)であり、ｖ_vはＥＮ座標での位置(Ｅ_v Ｎ_v)の微分を意味するので、上記の式は下記のようになる。

上記の式から、ＥＮ座標での位置に係る状態方程式である下記の式（１）、（２）が得られる。

また、車両２００の前後速度Ｕvに係る状態方程式は、下記の式（３）のようになる。

また、路面姿勢角のロール角をφ_r、ピッチ角をθ_rとすると、横方向の運動方程式は、次式によって表される。

上記の中のＦ_yはy軸方向に働く力であり、下記の式に示したように、前輪タイヤ横力Ｆ_fと後輪タイヤ横力Ｆ_rとの和で表される。

そして、前輪タイヤ横力Ｆ_f及び後輪タイヤ横力Ｆ_rは次式で表される。

上記式に関連して、図４に示した車両２００の重心ＣＧ周りの速度をv_c=(Ｕ_c、Ｖ_c、Ｗ_c)、ヨーレートをＲ_c、前輪実舵角をδ_w、重心ＣＧから前輪車軸中心２１０までの距離をｌ_f、重心ＣＧから後輪車軸中心２０２までの距離をｌ_r、前輪、後輪のコーナリングスティフネスを各々Ｋ_f、Ｋ_r、車両質量をｍとした。

上記運動方程式のＦ_yに前輪タイヤ横力Ｆ_f及び後輪タイヤ横力Ｆ_rを代入して整理する。

上記式中の変数Ｒ_c、Ｕ_c、Ｖ_cの各々を、Ｒ_v＝Ｒ_c、Ｕ_v＝Ｕ_c、Ｖ_v＝Ｖ_c－ｌ_rＲ_vの関係を用いて後輪車軸中心２０２を基準とした変数Ｒ_v、Ｕ_v、Ｖ_vに変換すると、横速度Ｖ_vに係る状態方程式である下記の式（４）が得られる。なお、式（４）右辺のｄＲ_v/ｄｔの項は、後述する式（１１）として、ヨーレートＲ_vに係る状態方程式を構成する。

また、車両２００の前後速度Ｕ_vの変化率に係る状態方程式は下記の式（５）のようになる。

また、姿勢角φ_v、 θ_v、ψ_vと回転角速度Ｐ_v、Ｑ_v、Ｒ_vとの関係は、回転行列を用いて下記のように表現される。

上式の右辺の行列の逆行列を上式の両辺に乗算すると、下記のようになる。

上記の式より、姿勢角φ_v、 θ_v、ψ_vに係る状態方程式である下記の式（６）、（７）、（８）が得られる。

また、回転角速度であるロールレートＰ_vに係る状態方程式、及び同ピッチレートＱ_vに係る状態方程式は、各々、下記の式（９）、（１０）のようになる。

また、ヨー方向の運動方程式は、下記のように定義される。下記の式中のＦ_fは前輪タイヤ横力であり、同Ｆ_rは後輪タイヤ横力であり、ｌ_fは車両の重心ＣＧから前輪車軸中心２１０までの距離であり、ｌ_rは重心ＣＧから後輪車軸中心２０２までの距離である。

上述のように、前輪タイヤ横力Ｆ_f及び後輪タイヤ横力Ｆ_rは次式で表される。

上記のヨー方向の運動方程式に、前輪タイヤ横力Ｆ_f及び後輪タイヤ横力Ｆ_rを代入すると共に、前述のように、変数Ｒ_c、Ｕ_c、Ｖ_cの各々を、Ｒ_v＝Ｒ_c、Ｕ_v＝Ｕ_c、Ｖ_v＝Ｖ_c－ｌ_rＲ_vの関係を用いて後輪車軸中心２０２を基準とした変数Ｒ_v、Ｕ_v、Ｖ_vに変換すると、ヨーレートＲ_vに係る状態方程式である式（１１）が得られる。

また、操舵角δに係る状態方程式は、下記の式（１２）のようになる。

図５は、横滑りを含む車両２００の運動と目標経路２２０との関係の一例を示した説明図である。目標経路２２０は、一例として、道路の白線の位置を示す。Ｐ_tは目標位置であり、Ｐ_vは車両２００の位置を示す。一般に車両２００は路面に対して横滑りをしており、横速度を有する運動を伴っている。車両２００の挙動に対し、走行車線上の目標位置Ｐ_tは前後方向の速度Ｕ_tのみを有し、横速度を有さない運動をしている。この特徴を利用し、車両２００の運動と目標位置Ｐ_tの運動との差分を導出する事で、車両２００の横速度を推定するのに必要な条件式を導出する事が可能になる。

本実施の形態では、図５のように各変数を設定する。目標経路２２０上の目標位置Ｐ_tは、接線方向の速度成分Ｕ_tのみを持つとして、目標位置Ｐ_tの速度ベクトルをＶ_t=[Ｕ_t ０]^T、目標位置Ｐ_tをＰ_t=[ｘ_t ｙ_t ]^T、地球座標系のx軸方向に対する接線方向の方位角をψ_tと定義する。さらに車両２００は前後方向のみならず横速度を有するとして（車体スリップ角を有する）、車両２００の位置をＰ_t、車両２００の方位角をψ_v、車両２００の各軸方向の速度成分を図５に示したように、ν_v=[Ｕ_v Ｖ_v ]^Tとする。本実施の形態では、後に車体座標系２０８を基準に計測されたセンサ情報との関係を容易にするため、偏差の基準座標系を車体座標系２０８とし、目標位置Ｐ_tの方位角ψ_tと車両２００の方位角をψ_vとの偏差としてφ=ψ_t－ψ_vを定義する。

また、車両２００から目標点Ｐ_tを見た偏差ベクトルＰ_eは、下記のようになる。

上記の式中のＲ_z ^2dは、下記のような２次元のヨー方向の回転行列である。

偏差ベクトルＰ_eを時間微分すると、下記のようになる。なお、下記式中のＲvはヨーレートである。

Ｐ_e＝[ｅ_x、ｅ_y]として、e_xの変化量を０、φ≪１、ｅ_yＲ_v/Ｕ_v≪１とすると、横位置ε_vに係る状態方程式である式（１３）と、ヨー角φ_vに係る状態方程式である式（１４）とが得られる。

また、曲率ρに係る状態方程式は、下記の式（１５）のようになる。

状態方程式である式（１）～（１５）の各々に含まれる誤差ｖの各々を対角成分とした誤差共分散行列Ｑ_nが定義される。

後述するカルマンフィルタでは、状態方程式ｆ（ｘ）に係る上記の微分方程式を単純積分又はルンゲクッタ法を用いて離散化して使用する。従って、関数ｆ（ｘ）の入出力関係を、時間ｔ＝ｋ、ｋ－１に対して、ｘ（ｋ）＝ｆ（ｘ（ｋ－１））となる形で使用する。ただし、撮像装置２２による白線認識の情報がない場合は、式（１３）、（１４）、（１５）は状態方程式として使用しない。

次にセンサによる観測変数（観測量）を下記のように定義する。

本実施の形態では、ＩＭＵ２６により前後加速度ａ_sx、横加速度ａ_sy、ロールレートＰ_s、ピッチレートＱ_s及びヨーレートＲ_sを各々検出する。また、撮像装置２２で取得した画像情報と高精度地図とのマッチングからＥＮ座標Ｅ_cam、Ｎ_camと方位角ψ_camとを算出する。

また、ＧＮＳＳ衛星からの情報に基づいてＥＮ座標Ｅ_gps、Ｎ_gpsを算出し、さらにドップラー速度をＥＮ座標Ｅ_gps、Ｎ_gps の変化率として算出する。

図６は、横滑りを含む車両２００の運動と目標経路２２０との関係の一例を示した説明図である。横滑りが発生している場合、車両２００の後輪車軸中心２０２には横速度Ｖ_rとヨー角φ_sが生じ、目標位置Ｐ_tと後輪車軸中心２０２との間には、相対横位置ε_sが観察される。

本実施の形態では、撮像装置２２等を用いて相対横位置ε_s、ヨー角φ_s及び走路の曲率ρを算出する。そして、操舵角センサ２８により、操舵角δ_sを検出する。

ｙ=ｈ（ｘ）を満たす観測方程式ｈ（ｘ）は、下記の各式のように、状態量（右辺）と観測量（左辺）との対応を定義したものである。

上記観測方程式のうち、横加速度ａ_syに係る観測方程式右辺のｄＶv/ｄｔの項は、横速度Ｖ_vに係る状態方程式である式（４）で表される。センサを増設した場合は、当該センサに対応する観測方程式を設定する。また、撮像装置２２による白線認識の情報が無い場合は、ε_s、φ_s、ρ_sの各々に係る観測方程式は使用しない。

観測方程式の誤差共分散行列Ｒ_nを下記のように定義する。

誤差共分散行列Ｒ_nは観測方程式の誤差とその広がりであるので、観測方程式の不確かさとセンサのノイズを共に含んでいる。

誤差共分散行列Ｒ_nは、各々のσで表される観測方程式の誤差を対角成分に有する誤差共分散行列である。本実施の形態では、誤差共分散行列Ｒ_nは初期値が設定されるが、以後は更新式によって算出される。

以上により、カルマンフィルタを利用するのに必要な状態方程式、観測方程式、及び誤差共分散行列が定義された。

カルマンフィルタは線形・非線形を含め種々の手法が提案されている。本実施の形態では、前述のように状態方程式ｆ（ｘ）が非線形である事を踏まえ、非線形カルマンフィルタを活用する例を示す。中でも状態方程式ｆ（ｘ）についての線形化を要さず、かつ計算負荷が比較的小さいアンセンティッドカルマンフィルタを一例として採用する。状態方程式ｆ（ｘ）が線形の場合は、線形カルマンフィルタを用いてもよいし、アンセンティッドカルマンフィルタ以外の非線形カルマンフィルタを用いてもよい。

一般的なカルマンフィルタの内部では、走行路と横滑りを含む車両２００との平面運動の関係に基づいた状態の事前推定と、事前推定の値をセンサ値で補正するフィルタリングとの２つのステップにより未知数の推定を行っている。

しかしながら、本実施の形態に係る自己位置推定装置１０の演算装置１４では、事前推定とフィルタリングとに加えて、異常値を出力するセンサを判定するセンサ異常判定と観測方程式の誤差共分散行列Ｒ_nの更新とをさらに行う。

図７は、本実施の形態に係る自己位置推定装置１０の演算装置１４の機能ブロック図の一例である。演算装置１４は、状態方程式ｆ（ｘ）を用いて、目標経路２２０である走行車線と横滑りを含む車両２００の平面運動との関係に基づく状態の推定を行うと共に、観測方程式ｈ（ｘ）を用いて、対応するセンサ情報を推定する事前推定部４２と、異常値を出力するセンサを判定するセンサ異常判定部４４と、事前推定部４２が出力した事前推定値を、センサ情報を用いて補正するフィルタリング部４６と、観測方程式の誤差共分散行列Ｒ_nを更新する更新部４８と、を含む。

図７に示した変数及び観測量は、下記の通りである。

図８は、事前推定部４２の機能ブロック図の一例である。図８に示したように、事前推定部４２は、第１アンセンティッド変換部４２Ａと第２アンセンティッド変換部４２Ｂとを含む。

第１アンセンティッド変換部４２Ａは、状態方程式ｆ（ｘ）に基づいて状態量を更新する1回目のアンセンティッド変換(Unscented transfer)を行って、ｘの平均及びｘの誤差とその広がりを示す共分散行列を出力する。

第２アンセンティッド変換部４２Ｂは、第１アンセンティッド変換部４２Ａが出力した状態量ｘの平均及び状態量ｘの共分散行列を用いて、観測方程式ｈ（ｘ）に従って、対応する観測量に変換する２回目のアンセンティッド変換を行う。

なお、アンセンティッド変換の目的は、ある非線形関数ｙ＝ｆ（ｘ）による変換において、下記の観測量ｙの平均及び観測量ｙの誤差とその広がりを示す共分散行列を精度よく求めることにある。

従って、本実施の形態は、平均値と標準偏差とに対応する2n+1個のサンプル(シグマポイント）を用いて、確率密度関数を近似することを特徴とする。

図８に示したように、第１アンセンティッド変換部４２Ａは、シグマポイント重み係数部４２Ａ１と、関数変換部４２Ａ２と、Ｕ変換部４２Ａ３と、を含む。また、第２アンセンティッド変換部４２Ｂは、シグマポイント重み係数部４２Ｂ１と、関数変換部４２Ｂ２と、Ｕ変換部４２Ｂ３と、を含む。

第１アンセンティッド変換部４２Ａのシグマポイント重み係数部４２Ａ１及び第２アンセンティッド変換部４２Ｂのシグマポイント重み係数部４２Ｂ１では、シグマポイントＸ_i：ｉ＝０、１、２、…２ｎが、下記のように選択される。なお、Ｐ_xの平方根行列のｉ番目の列は、一例としてコレスキー分解によって算出される。

ただし、スケーリングファクタκは、κ≧０となるように選択する。また、シグマポイントに対する重みは下記のように定義する。

第１アンセンティッド変換部４２Ａの関数変換部４２Ａ２における、非線形関数ｆ（ｘ）による各シグマポイントの変換は下記のようになる。下記は、第１アンセンティッド変換部４２Ａの関数変換部４２Ａ２での状態方程式ｆ（ｘ）による変換だが、第２アンセンティッド変換部４２Ｂの関数変換部４２Ｂ２での観測方程式ｈ（ｘ）を用いた変換では、観測量Ｙ_i（ｋ）が得られる。

第１アンセンティッド変換部４２ＡのＵ変換部４２Ａ３では、関数ｆ（ｘ）によって変換された値と、前述の重み係数とを用いて、下記のように状態量ｘの平均値と状態量ｘの誤差とその広がりである共分散行列を算出する。なお、下記式中のＱ_nは状態方程式の誤差共分散行列である。

第２アンセンティッド変換部４２ＢのＵ変換部４２Ｂ３では、下記の計算を行う。

図９は、センサ異常判定部４４の機能ブロック図の一例である。センサ異常判定部４４は、すべてのセンサの観測量及び誤差共分散に基づいて、各々のセンサの異常度を計算する異常度算出部４４Ａと、閾値に基づいて異常値を出力するセンサを検出する異常検出部４４Ｂと、異常値を出力するセンサを除外するように各種ベクトル及び行列を再構成する再構成部４４Ｃと、を備える。

以下、異常度算出部４４Ａにおいて、一例として、あるセンサＡについての異常度を計算する手法について説明する。

ｙ(ｋ)_iを観測量のｉ番目の要素とし、センサＡの情報が観測量ｙ(ｋ)のｉ＝ｌ_a、…、（ｌ_a＋ｍ_a）番目の要素に記録されているとして、下記のベクトルと行列を定義する。

かかる場合に、異常度は、下記の式の形で定義がされる。

上記の式による異常度の計算を任意のセンサに対して行う事により、各々のセンサの異常度を計算する事ができる。上記の式では、観測量から予測値を引いた偏差を含む観測ベクトルの２乗を、誤差とその広がりを示す共分散行列で除算する。その結果、各変数の分散のみならず、共分散も加味した異常度が算出される。

異常検出部４４Ｂでは、あるセンサＡについて計算された異常度が閾値ａ_a ^thを超えた場合に、当該センサは異常だとみなす。異常度の確率分布は自由度ｍ_aスケールが下記の式で示した１のカイ２乗分布χ²(μ|m_a, 1)に従うことが知られている。

上記の式中のΓはガンマ関数を示し、次式で表される。

確率値α_a以下で発生する値は異常とみなすとすると、閾値ａ_a ^thは下記の式を満たす。

閾値ａ_a ^thは上記の式に基づいて算出するが、演算負荷が問題となるので、閾値ａ_a ^thは予め算出しておいてもよい。又は、確率値α_aに対応した閾値ａ_a ^thが記述されたテーブル(マップ）を予め備えていてもよい。

上記のように、センサの出力値ａ_a(ｋ)が閾値ａ_a ^thを超えた場合は、出力値ａ_a(ｋ)を出力したセンサを異常とみなし、当該センサをフィルタリング部４６で使用しない。なお、閾値ａ_a ^thを可変にして、例えば密集地帯を走行している際には、より厳しい閾値ａ_a ^thに変更するなど、状況に応じて閾値ａ_a ^thを変化させてもよい。

再構成部４４Ｃでは、異常検出部４４Ｂでの異常なセンサと正常なセンサとの識別後に、フィルタリング部４６で使用する各種ベクトルと行列を整理する。異常と識別されたセンサの観測量ｙ(ｋ)における要素番号を除いた要素番号のみを含むベクトルをＩ_T(ｋ)とし、Ｉ_T(ｋ)の要素番号をＩ_T(ｋ)＝[１２ … sizeＩ_T(ｋ) ]として、下記の４式をフィルタリング部４６で使用する。

図１０は、フィルタリング部４６の機能ブロック図の一例である。フィルタリング部４６では、Ｕ変換部４２Ｂ３で計算された、状態量の事前予測値に対応する観測量と、実際に観測された観測量との差を比較し、状態量の予測値を補正する処理を行う。なお、フィルタリング部４６で用いる各種ベクトル及び行列は、再構成４４Ｃによって再構成されたものである。例えば、フィルタリング部４６で用いる観測量は、センサ異常判定部４４で正常と判定されたセンサの出力値である。

状態量の予測値を実際に観測された値でフィードバックする処理はカルマンゲイン(Kalman Gain)と呼ばれ、次式で計算される。なお、次式のＲ_nは観測ノイズである誤差共分散行列Ｒ_nである。

次に、このカルマンゲインを用いて、状態量の事前予測値を補正する処理を次のように行う。

以上の事前推定部４２及びフィルタリング部４６の処理を各タイムステップごとに繰り返すことにより、車両２００の横速度Ｖ_v等を推定する。

一例として、車両２００の横速度Ｖ_vを推定する場合、フィルタリング部４６が出力したｘ（ｋ）、Ｐ_x（ｋ）を前回値として事前推定部４２に入力し、上述のように２段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測量を計算する。計算した状態量の予測値は、フィルタリング部４６でカルマンゲイン及び実際に観測された最新の観測量を用いて補正される。フィルタリング部４６が出力したｘ（ｋ）、Ｐ_x（ｋ）は、記憶装置１８に一時的にホールドし、記憶装置１８にホールドしたｘ（ｋ）、Ｐ_x（ｋ）を前回値として事前推定部４２に入力する。

この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、車両の横速度Ｖ_vを精度よく推定できる。

更新部４８では、観測方程式の誤差共分散行列Ｒ_n(ｋ)の更新を行う。更新部４８では、ある時間ｋ（例えば現在）からｗステップ前の過去からの観測量及び状態量を保存し、保存した観測量及び状態量を用いて誤差共分散行列Ｒ_n(ｋ)を更新する。更新式を次式に示す。

上記の更新式では、過去の観測量と推定値との誤差とその広がりに関する共分散行列を用いている。また、上記の更新式では、異常と判断されたセンサの値も使用して、前回の共分散行列に基づいた値を利用する。

上記の更新式内のＣ_n（ｋ）を求める他の方法として、新しく観測された観測量と推定値とに対する時定数τの１次遅れ系として扱うことも可能である。かかる場合には、下記の更新式を使用する。

上記の更新式を利用する場合は１ステップ前の値のみ必要で、１ステップよりも過去の記録を残しておく必要がない。上記の方法以外にも、ロバスト推定等を用いた誤差共分散行列の算出を行ってもよい。

以上説明したように、本実施の形態に係る自己位置推定装置１０によれば、観測量の予測値とセンサが新たに検出して出力した観測量との偏差に基づいて算出した異常度により異常値を出力するセンサを識別し、当該センサを除外することにより、自己位置推定の精度を担保できる。

一般的にセンサは、自身が装置として正しく動作しているかどうかの判断はできても、環境外乱によるセンサ値の劣化は把握できない場合が多い。センサ値の劣化の例として、ＧＰＳ信号のマルチパスによる位置情報の劣化や、ＬＩＤＡＲ情報と地図情報との誤マッチング等が挙げられる。いずれも出力値はあるがその質が極端に劣化している。

従来の自己位置推定装置では、情報の途絶による欠落等の明らかな情報の劣化には対応ができていたが、情報そのものの質の劣化には対応できていなかった。本実施の形態に係る自己位置推定装置１０は、車両運動モデルによる予測からセンサ値を予測する事によって、取得されたセンサ情報の質を監視する事を可能にした。

各種センサは環境に応じてその情報の質が変化する。センサの異常監視はセンサ情報の外れ値を検出する技術であるが、一方で定常的に精度が向上したり低下したりすることが考えられる。このような長い時間をかけて変化をするようなセンサ値のばらつきに対応するように、カルマンフィルタ内で想定する誤差共分散行列Ｒ_nを逐次更新する方法を導入した。その結果、事前にセンサ値のばらつきを評価する試験を行わなくても、センサ値の精度向上や低下に影響されにくい自己位置推定が可能となる。

１０自己位置推定装置
１２入力装置
１４演算装置
１６表示装置
１８記憶装置
２０画像情報処理部
２２撮像装置
２４車速センサ
２８操舵角センサ
３０ＧＰＳ
３２ＬＩＤＡＲ
３４Ｖ２Ｘ通信部
４２事前推定部
４４センサ異常判定部
４６フィルタリング部
４８更新部
２００車両
２０２後輪車軸中心
２１０前輪車軸中心
２２０目標経路

Claims

車両の位置推定に必要な観測量を検出して出力する複数の各々異なる検出部と、
前記車両の位置を含む状態量の予測値を算出し、前記状態量の予測値に基づいて、前記複数の検出部の各々が検出した観測量の予測値を算出する事前推定部と、
前記複数の検出部の各々について、前記検出部が新たに検出して出力した観測量と前記事前推定部が算出した観測量の予測値とに基づいて、異常値を出力する検出部であるか否かを識別する異常識別部と、
前記異常識別部で異常値を出力する検出部でないと識別された前記検出部の各々によって検出して出力した前記観測量と、前記事前推定部が算出した前記検出部の各々の前記観測量の予測値との差分に基づいて、前記事前推定部によって算出した前記状態量の予測値を補正する位置推定部と、
所定時間前から現在までの、異常値を出力する検出部でないと識別された前記複数の検出部の各々についての、前記補正された前記状態量の予測値から観測方程式を用いて計算される前記観測量と、前記検出部が新たに検出して出力した観測量との誤差に基づいて、観測方程式の誤差の広がりを更新する更新部と、
を含み、
前記位置推定部は、観測方程式の誤差の広がりを更に用いて、前記事前推定部によって算出した前記状態量の予測値を補正する自己位置推定装置。
前記事前推定部は、更に、前記状態量の誤差の広がりを算出し、前記状態量の誤差の広がりに基づいて前記複数の検出部の各々が検出する観測量の誤差の広がりを算出し、
前記異常識別部は、前記複数の検出部の各々について、前記検出部が新たに検出して出力した観測量と、前記事前推定部が算出した観測量の予測値と、前記検出部が検出する前記観測量の誤差の広がりとに基づいて、異常値を出力する検出部であるか否かを識別する請求項１に記載の自己位置推定装置。
前記異常識別部は、更に、異常値を出力する検出部でないと識別された前記検出部の各々によって検出して出力した前記観測量からなる観測値を構成し、
異常値を出力する検出部でないと識別された前記検出部の各々の前記観測量の予測値からなる観測値の予測値を構成し、
異常値を出力する検出部でないと識別された前記検出部の各々が検出する前記観測量の誤差の広がりからなり、前記観測方程式の誤差の広がりを示す観測量の誤差共分散行列を構成し、
異常値を出力する検出部でないと識別された前記検出部の各々が検出する前記観測量と、前記状態量との共分散からなる、状態量と観測量との共分散行列を構成し、
前記位置推定部は、前記構成された前記観測量の誤差共分散行列、及び前記状態量と観測量との共分散行列に基づいて、ゲインを計算し、
前記ゲイン、及び前記構成された前記観測値と前記観測値の予測値との差分に基づいて、前記状態量の予測値を補正する請求項１又は２記載の自己位置推定装置。
前記検出部は、走行時の車両の挙動を示す角速度及び加速度、前記車両の前後速度、前記車両の操舵角、車両周辺に照射した電磁波の反射に基づいて前記車両の周辺の情報、前記車両の周辺の画像情報、並びに衛星から測位に係る情報を各々取得し、
前記位置推定部は、前記検出部で取得した各々の情報のいずれかに基づいて測位する請求項１～３のいずれか１項に記載の自己位置推定装置。