JP7407947B2

JP7407947B2 - 車両制御装置

Info

Publication number: JP7407947B2
Application number: JP2022541367A
Authority: JP
Inventors: 弘明北野; 翔太亀岡
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2020-08-04
Filing date: 2020-08-04
Publication date: 2024-01-04
Anticipated expiration: 2040-08-04
Also published as: WO2022029878A1; US20230258826A1; JPWO2022029878A1; DE112020007484T5

Description

本開示は車両に搭載され、車両を制御するための車両制御装置に関する。

車両の運転支援を行う、または車両の自動走行を行うためには、車両が走行すべき道路を検出し、車両が走行すべき経路である走行経路を生成し、生成した走行経路に従って走行するように車両を制御する必要がある。

従来の運転支援システムには、カメラによる区画線検知を利用したものが普及しつつある。しかしながらカメラを利用したシステムでは、そもそもカメラで見える範囲しか検知できず、また区画線がかすれている場合および雨天等により区画線を検出しにくい場合がある。

そこで、ＧＮＳＳ（Global Navigation Satellite System）による衛星測位結果と高精度に測量されている地図データとを用いて、道路を検出するシステムが考えられている。特に近年では、準天頂衛星およびネットワーク型ＲＴＫ（Real Time Kinematic）に代表される高精度に車両位置を測位するシステムも実用化されつつある。

特許文献１では航法衛星からの信号を受信して自車両の位置を測位する衛星航法部と、自車両の方位角と所定の位置からの相対移動量とに基づいて自車両の位置を測位する自律航法部と、自車両が走行する道路に係る地図データを保有する地図データベースと、衛星航法部の測位データが車両制御に使用可能か否かを判定する測位データ使用可否判定部とを備えた測位システムが開示されている。測位データ使用可否判定部は、航法衛星の捕捉数が測位可能な数以上であって、捕捉した航法衛星による測位精度が所定以上の精度である条件下において、衛星航法部の測位データと自律航法部の測位データとの差分、および衛星航法部の測位データと地図データベースの地図データとの差分に基づいて、衛星航法部の測位データが車両制御に使用可能か否かを判定する。また、判定結果に応じて衛星航法部の測位データと自律航法部の測位データのどちらを使用するかを選択する。

特開２０１７－３３９５号公報

特許文献１に開示の技術では、衛星航法部の測位データに問題があった場合に、自律航法部の測位データを使用するだけであって、自律航法部自体が持つ誤差については考慮されていないため、自車両の位置推定の精度に課題があった。

本開示は上記のような問題を解決するためになされたものであり、車両の位置推定の精度を向上させた車両制御装置を提供することを目的とする。

本開示における車両制御装置は、衛星測位装置と自律センサとを用いて車両の位置を推定し、前記車両を制御する車両制御装置であって、前記衛星測位装置から、測位解を求めた測位方式である測位解の状態を含む第１のデータを取得し、前記第１のデータを処理して衛星測位結果として出力する衛星測位結果処理部と、前記自律センサから、前記車両の状態量を示す第２のデータを取得し、前記第２のデータに含まれる第１の誤差を補正して補正データとして出力するセンサ補正部と、前記センサ補正部から出力される前記補正データに基づいて、慣性測位演算を行い、慣性測位結果を出力する慣性測位部と、前記慣性測位部から出力される前記慣性測位結果を用いて測位演算を行うと共に、前記自律センサが出力する前記第２のデータの補正量の推定をするための予測観測値を演算して出力する観測値予測部と、前記観測値予測部から出力される前記予測観測値と、前記衛星測位結果処理部から出力される衛星測位結果との誤差を推定して第２の誤差として出力し、前記第２の誤差に基づいて演算した前記自律センサの補正量を出力する誤差推定部と、前記観測値予測部から出力される前記予測観測値と、前記誤差推定部から出力される前記第２の誤差と、に基づいて、前記予測観測値を補正し、補正後の測位結果として出力する測位補正部と、前記測位補正部から出力される前記補正後の測位結果を用いて、前記車両を道路に沿って走行させる車両制御部と、を備え、前記誤差推定部は、前記測位解の状態に応じて、誤差推定パラメータを変更する。

本開示の車両制御装置によれば、誤差推定部において、測位解の状態に応じて、誤差推定パラメータを変更することで、自律センサの誤差の補正を精度良く行うことができ、車両の位置の推定の精度を向上させて、車両制御の精度を向上させることができる。

実施の形態１に係る車両制御装置を含む車両の全体構成を示す図である。実施の形態１に係る車両制御装置の構成を示すブロック図である。実施の形態１に係る車両制御装置での処理を説明するフローチャートである。実施の形態１の車両制御装置を実現するハードウェア構成を示す図である。実施の形態１の車両制御装置を実現するハードウェア構成を示す図である。

＜実施の形態１＞
＜装置構成＞
図１は実施の形態１の車両制御装置を搭載した車両１の全体構成を示す図である。図１に示されるように車両１は、前輪の２つのタイヤを操作するハンドル２に操舵アクチュエータ３が取り付けられている。

操舵アクチュエータ３は、例えば、ＥＰＳ（電動パワーステアリング）用モータおよびＥＣＵ（Electronic Control Unit）を備え、操舵アクチュエータ３は車両制御装置９からの操舵指令に従って動作することで、ハンドル２および前輪の回転を制御することができる。

車両１は、車両制御装置９から入力された操舵指令値に従って操舵アクチュエータ３が制御され、車両１が道路に沿って走行するように操舵制御が行われる。

また、車両１は、衛星４からの信号を受信するアンテナ５、衛星測位装置６、道路情報記憶装置７、車両１に搭載され、ヨーレートセンサおよび車速センサ等の車両の状態量を検出する自律センサ８を備えている。

衛星４は、例えば、複数のＧＰＳ（Global Positioning System）衛星により構成されるが、ＧＰＳ衛星に限定されるものではなく、ＧＬＯＮＡＳＳ（Global Navigation Satellite System）等の他の測位衛星を用いることもできる。

アンテナ５は衛星４からの衛星信号を受信し、受信した信号を衛星測位装置６へと送信する。

衛星測位装置６は、外界センサであり、例えばＧＮＳＳ受信機（ＧＮＳＳセンサ）で構成され、アンテナ５で受信した衛星信号を処理し、車両１の位置、方位角および測位解の状態を道路情報記憶装置７および車両制御装置９へ送信する。

ＧＮＳＳセンサには、出力の設定によってＧＮＳＳセンサ内で測位演算した測位演算結果の他、測位演算を行う前のＧＮＳＳの観測データを測位生データとして出力する機能を有するものがある。測位生データには、擬似距離観測値、ドップラー観測値、搬送波位相観測値が含まれており、これらの観測値については衛星が配信する周波数帯域ごと（例えばＬ１帯、Ｌ２帯、Ｌ５帯など）について得られる。

測位衛星には米国のＧＰＳの他に、ロシアのＧＬＯＮＡＳＳ（Global Navigation Satellite System）、欧州のＧａｌｉｌｅｏ、日本のＱＺＳＳ（Quasi-Zenith Satellite System）、中国のＢｅｉｄｏｕ、インドのＮａｖＩＣ（Navigation Indian Constellation）などがあり、実施の形態１の車両制御装置９は、これら全てを利用することが可能である。

本実施の形態１における衛星測位装置６は、検出する位置を高精度とするために、近年普及しつつある準天頂衛星からの補正信号および図示されないネットワーク端末によるインターネット経由での補正情報に基づき、単独測位、ＤＧＰＳ（Differential Global Positioning System）測位、ＲＴＫ測位、ネットワーク型ＲＴＫ測位などの測位方式の何れを行うことも可能であり、上述した測位解の状態とは上記の測位方式のうちどの測位方式による結果であるかを表している。

単独測位とは、４機以上の測位衛星から受信した擬似距離観測値を用いて、測位を行う衛星測位方式の一種である。

ＤＧＰＳ測位とは、静止衛星型衛星航法補強システム（ＳＢＡＳ：satellite-based augmentation system）、電子基準点および私設の固定局から生成できる衛星測位誤差補強データを用いて、測位演算することにより、単独測位と比較して精度の高い衛星測位結果を得ることが可能な測位方式である。

ＲＴＫ測位とは、電子基準点および私設の固定局の衛星生データを移動局に転送し、基地局近傍の衛星測位誤差要因を除去して高精度な衛星測位が可能となる測位方式である。ＲＴＫ測位では、アンビギュイティーと呼ばれる整数変数が高い信頼度で求まった場合、ｃｍ級の精度で測位が可能である。このときの測位解はＦｉｘ解と呼称され、アンビギュイティーが求まらなかった場合はＦｌｏａｔ解が出力される。

ネットワーク型ＲＴＫ測位とは、ネットワークを用いて基地局設置相当の衛星測位データを取得し、高精度測位を行う測位方式である。衛星測位装置６では、例えばネットワークを用いて上記衛星測位誤差要因が得られている場合に、アンビギュイティーが求まった場合にはＦｉｘ解を、アンビギュイティーが求まらなかった場合にはＦｌｏａｔ解を出力する。また、上記衛星測位誤差要因が得られていない場合には、上記衛星測位誤差補強データを用いてＤＧＰＳ測位により求まった測位解を出力する。さらに、上記衛星測位誤差要因および上記衛星測位誤差補強データが得られていない場合には、単独測位により求まった測位解を出力する。測位解がネットワーク型ＲＴＫ測位方式で求められたものであれば、測位解の状態とはネットワーク型ＲＴＫ測位方式を指す。測位解がＤＧＰＳ測位方式で求められたものであれば、測位解の状態とはＤＧＰＳ測位方式を指す。測位解が単独測位方式で求められたものであれば、測位解の状態とは単独測位方式を指す。すなわち、測位解の状態とは、測位解を求めるための測位方式そのものを指す。

また、外乱に対してよりロバストな位置検出をするためにヨーレートセンサおよび車速センサと組み合わせて自車両の位置および方位角を算出し、デッドレコニングが可能な構成とすることもできる。

車両制御装置９は、操舵アクチュエータ３へ送信する操舵指令値を生成するＥＣＵとして構成され、衛星測位装置６から入力された位置、方位角および測位解の状態、道路情報記憶装置７から入力された道路情報、自律センサ８から入力された車速およびヨーレートに基づいて操舵指令値として目標とする操舵角を出力する。

道路情報記憶装置７は、衛星測位装置６から入力された位置および方位角より、車両１が走行する道路情報、例えば車線中央の緯度および経度の点群情報、車線数、曲率等を出力する。また、衛星測位装置６から入力される方位角を用いて、自車両または、その近傍を原点とする座標系へ変換した道路情報を出力することもできる。

なお、図１では衛星測位装置６と道路情報記憶装置７とは別個の構成としたが、１つの構成の中で両方の処理を行う構成とすることもできる。

次に、実施の形態１の車両制御装置９の構成について、図２を参照して説明する。図２は、実施の形態１の車両制御装置９の構成を示すブロック図である。

図２に示すように車両制御装置９は、衛星測位結果処理部１０、慣性測位部１１、誤差推定部１２、センサ値補正部１３、棄却判定部１４、測位補正部１５、車両状態推定部１６、車両制御部１７および観測値予測部１８を備えている。また、車両制御装置９の全体を統御する内部制御部２０を備えている。なお、内部制御部２０と各部位との接続関係は煩雑になるので図示は省略している。また、車両制御装置９には、衛星測位装置６、道路情報記憶装置７および自律センサ８が接続されている。

衛星測位結果処理部１０は、測位演算および自律センサ８のセンサ値の補正量を推定するために必要な衛星測位装置６からの出力である緯度、経度、高度、方位、測位解の状態を含む測位結果（第１のデータ）を受信し、車両制御装置９内で使用可能に処理して衛星測位結果として誤差推定部１２に出力する。

センサ値補正部１３は、自律センサ８から出力されるセンサ値（第２のデータ）を取得し、センサ値に含まれるスケールファクタ、バイアスなどのセンサ誤差を補正して出力する。さらに後述する車両状態推定部１６で行う測位処理の遅れを補償するため、入力をバッファリングし、補正されたセンサ値（補正データ）を遅れ時間分だけ遅らせて慣性測位部１１に出力する。

慣性測位部１１は、補正されたセンサ値を用いて、車両１の測位結果である位置、姿勢、速度などの慣性測位演算を行い、慣性測位結果を観測値予測部１８に出力する。

観測値予測部１８は、慣性測位部１１から入力された慣性測位結果を用いて、測位演算および自律センサ８が出力する状態量データの補正量の推定をするために必要な予測観測値を演算し、誤差推定部１２に出力する。

誤差推定部１２は、衛星測位結果処理部１０からの衛星測位結果と、観測値予測部１８からの予測観測値との誤差を推定し、推定された誤差を棄却判定部１４へと出力する。

棄却判定部１４では入力された推定誤差に基づき衛星測位結果を棄却するか否かを判定し、判定結果を車両状態推定部１６および車両制御部１７に出力し、推定誤差をセンサ値補正部１３および測位補正部１５に出力する。

測位補正部１５では、棄却判定部１４より入力された推定誤差を用いて、観測値予測部１８より入力された予測観測値を補正し、補正後の測位結果として車両状態推定部１６へ出力する。

車両状態推定部１６では、測位補正部１５より入力された補正後の測位結果に対して、測位処理により発生する遅れ時間を補償した車両状態量を車両制御部１７へと出力する。

＜動作＞
次に、図３に示すフローチャートを用いて実施の形態１の車両制御装置９の処理フローについて説明する。

車両制御装置９が動作を開始すると、まず、内部制御部２０の制御により、慣性測位の初期値、または誤差推定部１２で利用されている現在の慣性測位結果を取得する（ステップＳ１）。なお、車両制御装置９の電源投入直後など、現在の慣性測位結果が取得できない場合は、ＧＮＳＳセンサからの概算の測位結果を利用する、または、慣性測位の初期値として予め定めた値を使用することができる。

次に、センサ値補正部１３は、自律センサ８からセンサ値を取得する（ステップＳ２）。すなわち、自律センサ８は、車両の車速を計測する車速計、車両の加速度および角速度を計測する慣性計測装置（ＩＭＵ：Inertial Measurement Unit）、車両の操舵角度を計測する操舵角計などのセンサを有しており、ＩＭＵから加速度および角速度、車速計から車速を取得する。

車速計は、車両１が有する車輪に取り付けられ、車輪の回転速度を検出するパルスセンサからの出力を用いて、車両の車速に変換する機能を有する。

ＩＭＵは、車両１の屋根または室内に設置され、車両座標系における加速度、角速度を検出する機能を有する。ＩＭＵにはＭＥＭＳ（Micro Electric Mechanical System）および光ファイバージャイロ（Fiber Optic Gyroscope）などを組み込んだものなどが市販されている。

次に、センサ値補正部１３において、自律センサ８のセンサ値を補正する（ステップＳ３）。さらに、後述する測位処理の遅れ時間を補償するため、自律センサ８のセンサ値のバッファリングを行う。

すなわち、衛星測位装置６では受信した衛星信号を処理し、それを車両制御装置９に送信する過程および車両制御装置９で受信する過程で遅れ時間が発生する。遅れ時間が大きくなると、最終的に制御の安定性および制御性能が低下してしまうため、遅れ時間を補償する必要がある。実施の形態１の車両制御装置９では、これらの遅れを一定時間として、その遅れ時間に相当するサンプル分をバッファリングし、遅らせた自律センサ８のセンサ値を用いて誤差推定を行う。これにより、衛星測位結果と自律センサ８との間で発生する時間のずれが解消され、推定精度が向上する。

＜自律センサのセンサ値の補正＞
以下では自律センサ８として、車速計および車両の両軸方向の角速度（以下、ヨーレートと呼称）センサを用いるものとし、以下の、数式（１）および数式（２）で表されるようなセンサ誤差モデルを用いて補正する場合を説明する。

数式（１）は、車速の真値Ｖ_ｔに対して、車速のスケールファクタｓ_νが掛かっており、数式（２）は、ヨーレート真値γ_ｔに対して、ヨーレートセンサのバイアスｂ_γが重畳し、ヨーレートのスケールファクタｓ_γが掛かっているモデルである。

この例においては、後述する誤差推定部１２において、ｓ_ν、ｓ_γおよびｂ_γのそれぞれ推定値ｓ_νe、ｓ_γeおよびｂ_γｅがセンサ誤差として推定される。センサ値補正部１３においては、誤差推定部１２で推定でされたセンサ誤差の推定値を用いて、以下の数式（３）および（４）によって自律センサ８のセンサ値を補正する。

数式（３）および（４）において、それぞれＶ_ｅおよびγ_ｅは、補正後の車速およびヨーレートである。なお、以上説明したセンサ誤差モデルについては一例であり、他のセンサ誤差モデルを利用しても良い。

ここで、図３のフローチャートの説明に戻り、慣性測位部１１においてステップＳ４の処理を行う。すなわち、補正されたセンサ値と車両の運動モデルを用いて慣性測位部１１において慣性測位演算を行う。慣性測位演算の具体的な演算方法として、車両は概ね平面内を移動するものとして、モデル化する。なお、以下ではＧＲＳ８０（Geodetic Reference System 1980）の楕円体に準拠した航法座標系で表現するものとする。まず、以下の数式（５）で表されるような状態変数を定義する。

数式（５）におけるｙ_ｄは、慣性測位に関する状態変数をまとめた慣性測位に関する状態ベクトルを表している。また、λ_ｄは慣性測位演算で得られた緯度、φ_ｄは慣性測位演算で得られた経度、ｈ_ｄは慣性測位演算で得られた楕円体高、ψ_ｄは慣性測位演算で得られた方位を表す。

この状態変数は、以下の数式（６）で表されるような運動モデルによりモデル化されるものとする。

数式（６）におけるｙ_ｄ・は、慣性測位に関する状態ベクトルを時間微分したベクトルを表している。また、ｇ（ｙ_ｄ，ｕ）はｙ_ｄ、uを入力とする非線形関数を、ｕは入力変数Ｖ、γをまとめた入力ベクトルであり、ｕ＝[Ｖγ]^Ｔを表している。

また、数式（６）におけるＮは卯酉半径を表し、Ｍが子午半径を表しており、それぞれ、以下の数式（７）および（８）で定義される。

補正されたセンサ値を数式（６）に代入し、時々刻々積分を行うことによって慣性測位結果を得ることができる。積分の方法はルンゲ・クッタ法（Runge-Kutta method）などの手法がよく利用される。なお、慣性航法の緯度、経度、高度などの座標は、車両の航法中心の座標としている。

衛星測位装置６が、例えばＧＮＳＳ受信機（ＧＮＳＳセンサ）で構成される場合、慣性測位で得られた情報を用いて、ＧＮＳＳの座標を更新する。以下ではＧＮＳＳセンサの座標更新について説明する。

＜ＧＮＳＳセンサの座標更新＞
ここで、図３のフローチャートの説明に戻り、観測値予測部１８においてステップＳ５の処理を行う。すなわち、ＧＮＳＳセンサで得られる観測値は、アンテナ５の緯度、経度、高度などの座標情報である。以下では、ＧＮＳＳセンサの観測値を（λ_ｍ，φ_ｍ，ｈ_ｍ，ψ_ｍ）とする。一方、慣性測位結果もこれらの座標情報が得られるが、慣性測位結果は車両の航法中心の座標であるため、車両航法中心からアンテナ５の位置までのオフセット量を用いて、ＧＮＳＳセンサの観測値を予測する。すなわち、車両の航法座標系で表現された車両航法中心からアンテナ５までのオフセット量を（Δｘ，Δｙ，Δｚ）とすると、予測されたＧＮＳＳセンサの観測値（λ_ｐ，φ_ｐ，ｈ_ｐ，ψ_ｐ）は、慣性測位値ｙ_ｄ（λ_ｄ，φ_ｄ，ｈ_ｄ，ψ_ｄ）とオフセット量ｖ（Δｘ，Δｙ，Δｚ）から、座標変換関数ｃ（ｙ_ｄ，ｖ）により以下の数式（９）のように求めることができる。

ここで、図３のフローチャートの説明に戻り、ステップＳ６の処理を行う。すなわち、誤差推定部１２においては、衛星測位装置６から得られた衛星測位結果とステップＳ５で得られた予測観測値との誤差（第２の誤差）を推定し、推定した誤差に基づいて自律センサ８の補正されたセンサ値、すなわち自律センサ補正量を演算し、自律センサ補正量はセンサ値補正部１３に、推定された誤差は棄却判定部１４へと出力する。

次に、ステップＳ７の処理を行う。すなわち、誤差推定部１２は、衛星測位装置６から受信した衛星測位結果が更新されているか否かを判定する。すなわち、慣性測位部１１は、ステップＳ４で得られた衛星測位結果と、前回のサンプリング時に衛星測位装置６から受信した衛星測位結果とを比較し、両者が同じであれば、衛星測位装置６からのデータが更新されていないと判定し、両者が異なっていれば、衛星測位装置６からのデータが更新されていると判定する。そして、衛星測位装置６からのデータが更新されている場合（Ｙｅｓの場合）は、ステップＳ１０に移行する。一方、更新されていない場合（Ｎｏの場合）は、慣性測位部１１におけるステップＳ８の処理に移行する。

ステップＳ８では、誤差推定部１２で演算された自律センサ補正量およびステップＳ４で得られた慣性測位演算の結果を測位補正部１５に出力する。なお、自律センサ補正量が得られなかった場合には、自律センサ補正量は前回の演算結果の値を出力し、慣性測位演算の結果は、ステップＳ４で得られた慣性測位演算の結果を測位補正部１５に出力する。

＜誤差の推定方法＞
以下、誤差推定部１２における誤差の推定方法を説明する。まず、推定対象となる変数を、緯度、経度、高度、方位、車速スケールファクタ、ヨーレートスケールファクタ、ヨーレートバイアスとして、以下の数式（１０）で表される状態ベクトルｘを定義する。

車速のスケールファクタｓ_νおよびヨーレートのスケールファクタｓ_γが微小であるとすると、数式（３）および（４）より、車速の真値Ｖ_ｔおよびヨーレートの真値γ_ｔは、それぞれ以下の数式（１１）および（１２）で近似できる。

車速のスケールファクタｓ_ν、ヨーレートのスケールファクタｓ_γおよびヨーレートセンサのバイアスｂ_γの動的なモデルを、以下の数式（１３）、（１４）および（１５）で表す。すなわち現在の状態から次の状態を予測する１次マルコフ過程で駆動するものとする。

数式（１３）～（１５）において、ｓ_ν・はｓ_νの時間微分、ｓ_γ・はｓ_γの時間微分、ｂ_γ・はｂ_γの時間微分である。また、車速スケールファクタのプロセス雑音Ｗｓ_νは、車速スケールファクタの時間遷移に関する雑音であり、ヨーレートスケールファクタのプロセス雑音Ｗｓ_γは、ヨーレートスケールファクタの時間遷移に関する雑音であり、ヨーレートバイアスのプロセス雑音Ｗｂ_γは、ヨーレートバイアスの時間遷移に関する雑音である。

数式（１３）～（１５）をまとめると、状態方程式は以下の数式（１６）で表すことができる。

数式（１６）において、ｘ_・は、状態ベクトルｘを時間微分したベクトルを表す。また、ｕは、以下の数式（１７）で表すことができる入力ベクトルである。

数式（１６）を状態方程式、数式（９）をＧＮＳＳセンサによる観測方程式として状態ベクトルｘを推定することで衛星測位演算および、自律センサ８の誤差を推定することができる。

数式（１６）の状態方程式および数式（９）の観測方程式は、状態ベクトルに関して非線形となるため、測位演算、自律センサ８の誤差を推定するには非線形の状態推定を適用する必要がある。非線形の状態推定の手法として、パーティクルフィルタまたは逐次モンテカルロ法と呼称される粒子フィルタ、および拡張カルマンフィルタなどの公知の手法が適用可能である。これらの手法は、確率的に最も確からしい状態を推定する手法であり、状態推定問題にはよく利用される。

以下では、拡張カルマンフィルタを用いた手法について説明する。カルマンフィルタではシステムに付随するノイズはガウス分布に従うという仮定の下で状態変数の推定が行われるが、粒子フィルタと比較して、計算負荷が小さく演算回路が小さくて済むので実装上有利である。

＜拡張カルマンフィルタによる状態推定＞
状態ベクトルの事前推定値ｘ_ｂの周りで数式（１６）を１次テイラー展開すると、以下の数式（１８）で表すことができる。

数式（１８）において、ｗはプロセス雑音であり、δｘは以下の数式（１９）で表すことができる誤差状態ベクトルである。

また、数式（１８）において、Ｆａは以下の数式（２０）で表すことができる。

ＧＮＳＳセンサによる観測方程式ｚを以下の数式（２１）のように表す。

なお、観測方程式ｚは、状態ベクトルｘとｕの関数として表現することが可能であり、上記の状況では全て以下の数式（２２）のように書けるものとする。

数式（２２）を状態ベクトルｘの事前推定値ｘ_ｂの周りでテイラー展開すると、以下の数式（２３）および（２４）で表すことができる。

数式（２３）において、出力ベクトルｚは先に示した数式（２２）で表される観測方程式となる。

また、数式（２４）においてＨは、観測方程式を、状態ベクトルｘに関して１次テイラー展開し、ｘとして事前推定値ｘ_ｂを代入した行列であり、以下の数式（２５）で表される。

なお、行列Ｈは、解析的に求めるか、数値微分を用いることにより計算可能である。

数式（１８）および数式（２４）を自律センサ８のサンプリング時間Δｔで離散化し、離散時刻をｋとすると、それぞれ以下の数式（２６）および数式（２７）となる。

数式（２６）および数式（２７）において、Ｆは、時刻ｋに関する誤差状態ベクトルδｘ_ｋに関する状態遷移行列でありＦ＝（１＋Ｆ_ａ・ｄｔ）で表され、ｗ_ｋ＝ｗ・Δｔで表される。ν_ｋは、各観測値に対応したセンサノイズである。プロセスノイズｗおよびセンサノイズν_ｋは、カルマンフィルタのパラメータであり、事前の測定値などを用いて設定が可能である。

数式（２６）および数式（２７）を用いて、カルマンフィルタの処理アルゴリズムを適用すれば、離散時刻ｋでの誤差状態ベクトルの推定値δｘ_ｅ，ｋを求めることが可能である。

＜時間発展処理＞
時間発展処理とは、自律センサ８のサンプリング時間ごとに実行する処理である。時刻ｋにおける状態ベクトルの事前推定値ｘ_ｂ，ｋを時刻ｋにおける慣性測位結果ｙ_ｄ，ｋおよび自律センサ誤差ｅ_{ｓｅｎｓｏｒ，ｋ}を用いて以下の数式（２８）で表す。

時刻ｋにおける誤差状態ベクトルの事前推定値をδｘ_ｂ，ｋ、誤差共分散行列をＰ_ｋ（ｎ×ｎ行列）、事前誤差共分散行列をＰ_ｂ，ｋ（ｎ×ｎ行列）とすると、事前推定値δｘ_ｂ，ｋおよび事前誤差共分散行列Ｐ_ｂ，ｋは、それぞれ以下の数式（２９）および数式（３０）として時間発展処理を行う。

数式（３０）において、Ｑは、ｗ_ｋの分散を対角成分としたプロセスノイズの共分散行列（ｎ×ｎ行列）である。電源投入直後などにおいては誤差共分散行列の初期値が必要であるが、初期値には０以上を満たす任意のスカラー値αとｎ×ｎの単位行列Ｉ_ｎ×ｎを用いて以下の数式（３１）で表されるＰ_ｋ－１などがよく利用される。また、δｘ_ｂ，ｋの初期値としては、δｘ_ｂ，ｋの要素を全て０としたベクトルが利用される。

＜観測更新処理＞
外界センサによる観測値が得られた時刻で、以下の数式（３２）、（３３）および（３４）で定義される観測更新処理を行う。

数式（３２）～（３４）において、δｘ_ｅ，ｋは誤差状態ベクトルの推定値、Ｒはセンサノイズの共分散行列（ｐ×ｐ行列）、Ｇ_ｋはカルマンゲインである。

また、δｚ_ｋは、ｚ_ｍ，ｋを時刻ｋにおける実観測値とし、ｚ_ｐ，ｋを予測観測値として以下の数式（３５）で表されるベクトルである。

このようにすれば、時刻ｋにおける誤差状態ベクトルの推定値δｘ_ｅ，ｋが求まるため、状態ベクトルｘ_ｋの推定値ｘ_ｅ，ｋは、以下の数式（３６）として求めることができる。

また、ＧＮＳＳセンサの測位解は、単独測位、ＤＧＰＳ測位、ＲＴＫ測位、ネットワーク型ＲＴＫ測位などの測位方式によって変わり、衛星測位結果の位置精度が異なるため、測位解の精度が低いほど誤差推定パラメータであるセンサノイズの共分散行列Ｒの要素の値を大きくすると推定結果が良くなる。しかしながら、衛星測位結果のうち方位の精度はＧＮＳＳ信号のドップラー観測値を利用できるため、マルチパス等の影響による精度の劣化が低い。そのため、測位解が変化した場合もセンサノイズの共分散行列Ｒの要素のうち方位ψに係る要素は変更しなくても良い。これによりセンサノイズの共分散行列Ｒの位置の要素を大きくし、センサノイズの共分散行列Ｒの方位の要素を変更しない、またはセンサノイズの共分散行列Ｒの位置の要素より増加割合を小さくすることで、よりセンサのモデルと合致した推定となり、推定精度が向上する。

ここで、図３のフローチャートの説明に戻り、棄却判定部１４におけるステップＳ１０の処理を行う。すなわち、棄却判定部１４は、ステップＳ６で得られた推定誤差に基づいて衛星測位結果の棄却の判定を行う。

数式（３４）の誤差共分散行列Ｐ_ｋは、状態ベクトルの真値と推定値の差に関する分布を表しており、この値を用いれば外界センサの異常値を判定することが可能である。一般的に車両の車輪に取り付けられた車輪速パルスにより、車輪回転数が高精度に出力されるため、マルチパス等の影響を受けやすい衛星測位結果と比較すると、短時間での位置の信頼性が高くなる。そのため、例えば、誤差共分散行列Ｐ_ｋの緯度、経度に対する要素を抽出し、固有値解析を行うことで誤差楕円と呼ばれる楕円を求め、誤差楕円の範囲内にＧＮＳＳセンサのセンサ値が含まれているのであれば、その値は観測値として利用し、含まれていない場合は異常値として棄却し、観測値として使用しないなどの棄却機構を構成することが可能である。これにより、精度の低い観測値を棄却でき、推定精度を向上することができる。

また、観測値が更新されない状態が継続されると誤差共分散行列Ｐ_ｋから計算される誤差楕円の半径が時間と共に大きくなる。すなわち、観測値が得られないためプロセスノイズの共分散行列Ｑに基づいて所定の確率で定められる状態、この例では緯度、経度の範囲が大きくなっていく。そのため道路情報記憶装置７より送られてきた道路情報の走行車線と誤差楕円とを比較し、誤差楕円が走行車線をはみ出していると判断される場合には、車両制御部１７に車両制御を停止させる、または他のカメラおよびＬｉＤＡＲ（Light Detection and Ranging）等の他のセンサを用いた制御に切り替える指示を出力する。これにより、推定精度が低下した場合にも安全に車両を制御することができる。

パーティクルフィルタを用いた場合も同様な棄却機構を構成することが可能であり、異常値を棄却することで、より信頼性の高い推定を行うことが可能である。

ステップＳ１０において、衛星測位結果を棄却しないと判定された場合（Ｎｏの場合）は、慣性測位部１１におけるステップＳ８の処理に移行する。ステップＳ８では、ステップＳ６で演算された自律センサ補正量および後述する方法で演算された慣性測位演算の結果を測位補正部１５に出力する。

一方、ステップＳ１０で衛星測位結果を棄却すると判定された場合（Ｙｅｓの場合）は、自律センサ補正量は前回の演算結果の値を出力し、慣性測位演算の結果は、ステップＳ４で得られた慣性測位演算の結果を出力する（ステップＳ１１）。

なお、ステップＳ１～Ｓ１１の処理は、自律センサ８でのサンプリングが行われるごとに繰り返され、ステップＳ１０で衛星測位結果が棄却される状態が所定時間続く場合には、棄却判定部１４は、慣性測位演算の信頼性が低下しているとの情報を車両制御部１７へと出力する。

ステップＳ８における慣性測位演算では、状態ベクトルの推定値を状態ベクトルｘ_ｅとし、以下の数式（３７）のように定義する。

数式（３７）において、λ_e，φ_e，ｈ_eおよびψ_eは、それぞれ緯度、経度、高度および方位の推定値であり、ｓ_νe、ｓ_γeおよびｂ_γｅは、車速スケールファクタ、ヨーレートスケールファクタ、ヨーレートバイアスの推定値である。

ｙ_ｅ＝［λ_eφ_eｈ_eψ_e］^Ｔとすると、測位演算結果ｙ_ｏｕｔは、以下の数式（３８）で表される。

また、自律センサ誤差ｅ_{ｓｅｎｓｏｒ}は、以下の数式（３９）で表され、センサ値補正部１３に入力される。

次に、車両状態推定部１６におけるステップＳ９の処理について説明する。ステップＳ９では、ステップＳ８での処理を経て出力された測位演算結果に対して、ステップＳ３においてバッファリングした自律センサ８のセンサ値を用いて、衛星測位装置６での測位処理の遅れ時間を補償する。具体的には、現在時刻Ｔｋにおいて、遅れ時間Ｔｄの間に車両運動は変わらないものと仮定し、以下の数式（４０）に示すように、バッファリングした自律センサの車速およびヨーレートをステップＳ８で出力される自律センサ補正量で補正し、測位演算結果ｙ_ｏｕｔを初期値として、補正した車速およびヨーレートを遅れ時間Ｔｄで積分して、遅れ時間を補償した後の測位演算結果ｙ_ｃｏｍｐとして車両制御部１７に出力する。

これにより、自律センサ８と衛星測位結果との時間的な差を抑制した状態で推定し、測位演算結果としては実時間に対して遅れが補償されたものを出力でき、位置精度および制御性能の向上が可能となる。

車両制御部１７では、棄却判定部１４、測位補正部１５、車両状態推定部１６および道路情報記憶装置７からの出力に基づき、車両１を道路に沿って走行させる。

具体的には、測位補正部１５によって得られた車両座標および姿勢に基づき、道路情報記憶装置７から得られる自車周辺道路を自車両座標系へと変換し、自車位置と走行すべき道路との偏差をなくすように車両制御を行う。車両制御の手法としては種々提案されているが、例えば位置の偏差のみをフィードバックして制御する方法、自車と道路との角度偏差を用いる方法、さらに道路の曲率を用いる方法等があるが、何れも公知であるので、説明は省略する。

また、先に説明したように、棄却判定部１４における衛星測位結果の棄却が継続し、信頼性が低下した判定された場合には車両制御部１７では、車両制御を停止する、または他のカメラおよびＬｉＤＡＲ等の他のセンサを用いた制御に切り替える。

以上説明したように、実施の形態１の車両制御装置９によれば、センサノイズの共分散行列（誤差推定パラメータ）をＧＮＳＳセンサの測位解の状態に応じて逐次変更することで、自律センサ８自体の誤差、および予測観測値の誤差を精度良く補償でき、自車両の位置を精度良く推定できる。また、測位衛星データの精度が悪くなることで、自車両の位置の推定精度が悪くなった場合でも、車両制御を停止する、または他のセンサを用いた車両制御に切り替えるため、車両制御の精度を維持できる。

＜ハードウェア構成＞
なお、以上説明した実施の形態１の車両制御装置９の各構成要素は、コンピュータを用いて構成することができ、コンピュータがプログラムを実行することで実現される。すなわち、車両制御装置９は、例えば図４に示す処理回路５０により実現される。処理回路５０には、ＣＰＵ（Central Processing Unit）、ＤＳＰ（Digital Signal Processor）などのプロセッサが適用され、記憶装置に格納されるプログラムを実行することで各部の機能が実現される。

なお、処理回路５０には、専用のハードウェアが適用されても良い。処理回路５０が専用のハードウェアである場合、処理回路５０は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field-Programmable Gate Array）、またはこれらを組み合わせたもの等が該当する。

車両制御装置９は、構成要素の各々の機能が個別の処理回路で実現されても良いし、それらの機能がまとめて１つの処理回路で実現されても良い。

また、図５には、処理回路５０がプロセッサを用いて構成されている場合におけるハードウェア構成を示している。この場合、車両制御装置９の各部の機能は、ソフトウェア等（ソフトウェア、ファームウェア、またはソフトウェアとファームウェア）との組み合わせにより実現される。ソフトウェア等はプログラムとして記述され、メモリ５２に格納される。処理回路５０として機能するプロセッサ５１は、メモリ５２（記憶装置）に記憶されたプログラムを読み出して実行することにより、各部の機能を実現する。すなわち、このプログラムは、車両制御装置９の構成要素の動作の手順および方法をコンピュータに実行させるものであると言える。

ここで、メモリ５２は、例えば、ＲＡＭ、ＲＯＭ、フラッシュメモリー、ＥＰＲＯＭ（Erasable Programmable Read Only Memory）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read Only Memory）等の、不揮発性または揮発性の半導体メモリ、ＨＤＤ（Hard Disk Drive）、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Digital Versatile Disc）およびそのドライブ装置等、または、今後使用されるあらゆる記憶媒体であっても良い。

以上、車両制御装置９の各構成要素の機能が、ハードウェアおよびソフトウェア等の何れか一方で実現される構成について説明した。しかしこれに限ったものではなく、車両制御装置９の一部の構成要素を専用のハードウェアで実現し、別の一部の構成要素をソフトウェア等で実現する構成であっても良い。例えば、一部の構成要素については専用のハードウェアとしての処理回路５０でその機能を実現し、他の一部の構成要素についてはプロセッサ５１としての処理回路５０がメモリ５２に格納されたプログラムを読み出して実行することによってその機能を実現することが可能である。

以上のように、車両制御装置９は、ハードウェア、ソフトウェア等、またはこれらの組み合わせによって、上述の各機能を実現することができる。

本開示は詳細に説明されたが、上記した説明は、全ての局面において、例示であって、本開示がそれに限定されるものではない。例示されていない無数の変形例が、本開示の範囲から外れることなく想定され得るものと解される。

なお、本開示は、その開示の範囲内において、実施の形態を適宜、変形、省略することが可能である。

Claims

衛星測位装置と自律センサとを用いて車両の位置を推定し、前記車両を制御する車両制御装置であって、
前記衛星測位装置から、測位解を求めた測位方式である測位解の状態を含む第１のデータを取得し、前記第１のデータを処理して衛星測位結果として出力する衛星測位結果処理部と、
前記自律センサから、前記車両の状態量を示す第２のデータを取得し、前記第２のデータに含まれる第１の誤差を補正して補正データとして出力するセンサ補正部と、
前記センサ補正部から出力される前記補正データに基づいて、慣性測位演算を行い、慣性測位結果を出力する慣性測位部と、
前記慣性測位部から出力される前記慣性測位結果を用いて測位演算を行うと共に、前記自律センサが出力する前記第２のデータの補正量の推定をするための予測観測値を演算して出力する観測値予測部と、
前記観測値予測部から出力される前記予測観測値と、前記衛星測位結果処理部から出力される衛星測位結果との誤差を推定して第２の誤差として出力し、前記第２の誤差に基づいて演算した前記自律センサの補正量を出力する誤差推定部と、
前記観測値予測部から出力される前記予測観測値と、前記誤差推定部から出力される前記第２の誤差と、に基づいて、前記予測観測値を補正し、補正後の測位結果として出力する測位補正部と、
前記測位補正部から出力される前記補正後の測位結果を用いて、前記車両を道路に沿って走行させる車両制御部と、を備え、
前記誤差推定部は、
前記測位解の状態に応じて、誤差推定パラメータを変更する、車両制御装置。
前記誤差推定パラメータは、
センサノイズの共分散行列である、請求項１記載の車両制御装置。
前記車両制御装置は、
前記第２の誤差を用いて、前記衛星測位結果を棄却するか否かを判定する棄却判定部をさらに備え、
前記衛星測位結果を棄却する場合には、前記自律センサの前記補正量を使用せず、
前記衛星測位結果を棄却しない場合には、前記自律センサの前記補正量を使用する、請求項１記載の車両制御装置。
前記誤差推定部は、前記第２の誤差の共分散行列を演算し、
前記棄却判定部は、前記第２の誤差の共分散行列から求めた誤差楕円と、道路情報とに基づいて、前記道路情報における前記車両の走行車線に前記誤差楕円が含まれているか否かを判定し、
前記車両制御部は、前記車両の前記走行車線に前記誤差楕円が含まれていない場合には、前記補正後の測位結果を用いた車両制御を制限する、請求項３記載の車両制御装置。
前記センサ補正部は、
前記第１の誤差が補正された前記第２のデータに対し、前記衛星測位結果処理部からの前記第１のデータの送受信処理に起因する前記衛星測位結果の演算遅れ時間に相当する時間分だけバッファリングする、請求項１記載の車両制御装置。
前記測位補正部から出力される前記補正後の測位結果に対して、前記衛星測位結果の前記演算遅れ時間を補償した車両状態量を出力する車両状態推定部をさらに備え、
前記車両状態推定部は、
前記慣性測位結果を初期値として、前記第１の誤差が補正され、バッファリングされた前記第２のデータを前記演算遅れ時間で積分して、前記演算遅れ時間を補償する、請求項５記載の車両制御装置。