JP7392785B1 - 車両位置推定装置及び車両位置推定方法 - Google Patents

Abstract

【課題】多数のセンサを用いることなく車両の位置を精度良く推定する。【解決手段】車両位置推定装置１は、車両の走行中の車両の位置データ、方位角データ及び速度データを、所定の時間間隔で取得するデータ取得部３２と、最新の位置データ、方位角データ及び速度データと、過去に取得した複数の位置データ、方位角データ及び速度データとを比較して、最新の位置データ、方位角データ及び速度データの各々の異常度を求める異常度算出部３４と、位置異常度、方位角異常度、及び速度異常度うちの少なくとも一つの異常度が所定の閾値以下である場合に、車両の位置を推定する位置推定部３６を備える。【選択図】図１

Description

本発明は、車両の位置を推定する車両位置推定装置及び車両位置推定方法に関する。

車両の自動運転制御や運転支援制御等には、高精度な自車位置推定が求められる。下記の特許文献１には、ＩＭＵ（Inertial Measurement Unit）、ＧＰＳ（Global Positioning System）センサ、操舵角センサ、ヨーレートセンサ等を組み合わせて車両位置を推定する技術が開示されている。

特開２０２２－１１５６３５号公報

しかし、上記の技術では、多数のセンサで検出を行う必要があるため、車両位置を推定する処理を行う際の消費電力が大きくなってしまう。また、ＩＭＵのような高価なセンサを多数用いるため、コストも高くなってしまう。

そこで、本発明はこれらの点に鑑みてなされたものであり、多数のセンサを用いることなく車両の位置を精度良く推定することを目的とする。

本発明の第１の態様においては、車両の走行中の前記車両の位置データ、方位角データ及び速度データを、所定の時間間隔で取得するデータ取得部と、最新の前記位置データ、前記方位角データ及び前記速度データと、過去に取得した複数の前記位置データ、前記方位角データ及び前記速度データとを比較して、最新の前記位置データ、前記方位角データ及び前記速度データの各々の異常度を求める異常度算出部と、前記位置データの異常度を示す位置異常度、前記方位角データの異常度を示す方位角異常度、及び前記速度データの異常度の示す速度異常度うちの少なくとも一つの異常度が所定の閾値以下である場合に、前記車両の位置を推定する位置推定部と、を備える、車両位置推定装置を提供する。

また、前記位置推定部は、最新の前記位置データ、前記方位角データ及び前記速度データのうちの前記異常度が前記閾値以下であるデータを用いて、前記車両の位置を推定することとしてもよい。

また、前記位置推定部は、最新の前記位置データ、前記方位角データ及び前記速度データのうちの前記異常度が前記閾値以下であるデータを観測値としてカルマンフィルタに入力して補正した位置を、前記車両の位置として推定することとしてもよい。

また、前記位置推定部は、最新の前記位置データ、前記方位角データ及び前記速度データの各々の異常度が前記閾値よりも大きい場合には、前記車両の位置を推定しないこととしてもよい。

また、前記異常度算出部は、最新の前記位置データの過去に取得した複数の位置データの各々との差分の総和から、前記位置異常度を求め、最新の前記方位角データの過去に取得した複数の方位角データの各々との差分の総和から、前記方位角異常度を求め、最新の前記速度データの過去に取得した複数の速度データの各々との差分の総和から、前記速度異常度を求めることとしてもよい。

また、前記データ取得部は、前記車両に搭載された一つの検出センサが検出した検出データと、地図データとをマッチングして、前記位置データ及び前記方位角データを取得することとしてもよい。

本発明の第２の態様においては、車両の走行中の前記車両の位置データ、方位角データ及び速度データを、所定の時間間隔で取得するステップと、最新の前記位置データ、前記方位角データ及び前記速度データと、過去に取得した複数の前記位置データ、前記方位角データ及び前記速度データとを比較して、最新の前記位置データ、前記方位角データ及び前記速度データの各々の異常度を求めるステップと、前記位置データの異常度を示す位置異常度、前記方位角データの異常度を示す方位角異常度、及び前記速度データの異常度の示す速度異常度うちの少なくとも一つの異常度が所定の閾値以下である場合に、前記車両の位置を推定するステップと、を有する、車両位置推定方法を提供する。

本発明によれば、多数のセンサを用いることなく車両の位置を精度良く推定できるという効果を奏する。

車両位置推定装置１の構成を示す図である。 車両Ｖに設けられたＬｉＤＡＲ１２を示す模式図である。 車両位置推定処理の流れを示すフローチャートである。

＜車両位置推定装置の構成＞
本発明に係る車両位置推定装置の構成について、図１及び図２を参照しながら説明する。

図１は、車両位置推定装置１の構成を示す図である。車両位置推定装置１は、車両に搭載されており、走行中の自車両の位置を推定する。車両位置推定装置１は、記憶部２０と制御部３０を有する。

記憶部２０は、コンピュータのＢＩＯＳ（Basic Input Output System）等を格納するＲＯＭ（Read Only Memory）、作業領域となるＲＡＭ（Random Access Memory）を含む。また、記憶部２０は、ＯＳ（Operating System）やアプリケーションプログラム、当該アプリケーションプログラムの実行時に参照される種々の情報を格納するＨＤＤ（Hard Disk Drive）やＳＳＤ（Solid State Drive）等の大容量記憶装置である。

制御部３０は、ＣＰＵ（Central Processing Unit）やＧＰＵ（Graphics Processing Unit）等のプロセッサである。制御部３０は、記憶部２０に記憶されたプログラムを実行することによって、データ取得部３２、異常度算出部３４及び位置推定部３６として機能する。

データ取得部３２は、走行中の車両の位置データ、方位角データ及び速度データを、所定の時間間隔で取得する。例えば、データ取得部３２は、車両の自動運転制御や、車線維持等の運転支援制御が行われている際に、車両の位置データ、方位角データ及び速度データを順次取得する。なお、所定時間は、一例として１（ｓ）であるが、これに限定されない。

データ取得部３２は、所定の時間間隔で取得した位置データ、方位角データ及び速度データを、記憶部２０に記憶させる。このため、位置データ、方位角データ及び速度データは、時系列データとして記憶部２０に記憶されている。

データ取得部３２は、車両の位置データとして、例えば車両の重心を原点とした平面座標系での位置データを取得する。データ取得部３２は、方位角データとして、例えば車両が旋回する際の方位角を示す方位角データを取得する。データ取得部３２は、速度データとして、車両が走行中の車速を示す車速データを取得する。

データ取得部３２は、車両の周辺環境を検出する環境検出センサが検出した検出データと、地図データとをマッチングすることで、車両の位置データを取得する。例えば、データ取得部３２は、環境検出センサが検出した３次元の検出データと、３次元の地図データとを、公知のＮＤＴ（Normal Distribution Transform）アルゴリズムを用いてマッチングすることで、車両の位置データを取得する。この際、データ取得部３２は、マッチングした３次元の位置データを２次元の位置データに変換することで、平面座標系での車両の位置データを取得する。データ取得部３２は、マッチング結果を利用することで、車両の方位角データも取得できる。

地図データは、例えば記憶部２０に予め記憶されている。ただし、これに限定されず、データ取得部３２は、車両が走行中に、外部サーバから地図データを取得してもよい。この場合、データ取得部３２は、最新の地図データを用いて、車両の位置データを取得できる。

データ取得部３２は、車両Ｖに搭載された一つの環境検出センサが検出した検出データと、地図データとをマッチングして、位置データ及び方位角データを取得する。環境検出センサは、本実施形態では図２に示すＬｉＤＡＲ（Light Detection And Ranging）１２である。

図２は、車両Ｖに設けられたＬｉＤＡＲ１２を示す模式図である。ＬｉＤＡＲ１２は、車両Ｖの車体の上方に一つのみ設けられている。具体的には、ＬｉＤＡＲ１２は、トラックである車両Ｖのキャブ１０の上方の中央部に、車両Ｖの進行方向の前方を向くように設けられている。この場合には、ＬｉＤＡＲ１２が高い位置に設置されることになるため、車両Ｖの前方に他の車両が走行していても、周辺環境を適切に検出できる。

データ取得部３２は、車両に搭載された車速センサ１４が検出した速度データを、車速センサ１４から取得する。例えば、車速センサ１４は、ＬｉＤＡＲ１２の検出タイミングと同じタイミングで、車両の速度を検出する。なお、上記では、環境検出センサがＬｉＤＡＲ１２であることとしたが、これに限定されない。例えば、環境検出センサは、ミリ波レーダーやカメラであってもよい。

異常度算出部３４は、データ取得部３２が取得した最新の位置データ、方位角データ及び速度データの異常度を算出する。具体的には、異常度算出部３４は、最新の位置データ、方位角データ及び速度データと、過去に取得して記憶部２０に記憶された時系列データである複数の位置データ、方位角データ及び速度データとを比較して、最新の位置データ、方位角データ及び速度データの各々の異常度を求める。異常度算出部３４が求めた異常度は、最新のデータ（位置データ、方位角データ及び速度データ）の過去の時系列データからの乖離が大きいほど、異常度合いが大きい値となる。このため、異常度が高いデータは、信頼性が低い異常なデータを意味し、異常度が低いデータは、信頼性が高い正常なデータを意味する。

異常度算出部３４は、最新の位置データの過去に取得した複数の位置データの各々との差分の総和から、最新の位置データの異常度を示す位置異常度を求める。例えば、異常度算出部３４は、式（１）を用いて位置異常度ｄ_ｘｙ，ｉを求める。

式（１）において、（ｘ_ｉ、ｙ_ｉ）が最新の位置データである。また、式（１）は、式（２）のように表される。

異常度算出部３４は、最新の方位角データの過去に取得した複数の方位角データの各々との差分の総和から、最新の方位角データの異常度を示す方位角異常度を求める。例えば、異常度算出部３４は、式（３）を用いて方位角異常度ｄ_θを求める。

式（３）において、θ_ｉが最新の方位角データである。また、式（３）は、式（４）のように表される。

異常度算出部３４は、最新の速度データの過去に取得した複数の速度データの各々との差分の総和から、最新の速度データの異常度を示す速度異常度を求める。例えば、異常度算出部３４は、式（５）を用いて速度異常度ｄ_ｖを求める。

式（５）において、ｖ_ｉが最新の速度データである。また、式（５）は、式（６）のように表される。

位置推定部３６は、データ取得部３２が取得した位置データ、方位角データ及び速度データを用いて、車両の位置を推定する。位置推定部３６は、最新の位置データ、方位角データ及び速度データの異常度（位置異常度、方位角異常度及び速度異常度）から、車両の位置を推定するか否かを判定する。具体的には、位置推定部３６は、位置異常度、方位角異常度及び速度異常度の各々が対応する閾値以下であるか否かで、車両の位置を推定するか否かを判定する。

閾値は、位置異常度に対応する位置閾値と、方位角異常度に対応する方位角閾値と、速度異常度に対応する速度閾値である。位置閾値、方位角閾値及び速度閾値は、例えば予め実験等によりデータの信頼性が高いと認められる上限値に設定されている。このため、位置異常度が位置閾値以下である場合には、位置データが正常であるといえる。同様に、方位角異常度が方位角閾値以下である場合には、方位角データが正常であり、速度異常度が速度閾値以下である場合には、速度データが正常である。

位置推定部３６は、位置異常度、方位角異常度及び速度異常度うちの少なくとも一つの異常度が所定の閾値以下である場合に、車両の位置を推定する。すなわち、位置推定部３６は、位置データ、方位角データ及び速度データのうち正常なデータがある場合に、車両の位置を推定する。前述したように車両の環境検出センサがＬｉＤＡＲ１２の一つのみである場合には、複数のセンサを利用する場合に比べて異常なデータが取得される可能性が高まるが、上述したように異常度が閾値以下のデータを用いて車両の位置を推定することで、異常なデータ用いて車両の位置を推定することを抑制できる。

位置推定部３６は、最新の位置データ、方位角データ及び速度データのうちの異常度が閾値以下であるデータを用いて、車両の位置を推定する。すなわち、位置推定部３６は、異常度が閾値以下であるデータのみを用いて、車両の位置を推定する。通常、位置推定部３６は、車両が走行している間継続して位置を推定する。このため、一のタイミングで３つのデータ（位置データ、方位角データ及び速度データ）のうちの一部のデータで車両の位置を推定しても、他のタイミングで正常な３つのデータを用いて車両の位置を推定することで、走行中の車両の位置を精度よく推定できる。

位置推定部３６は、最新の位置データ、方位角データ及び速度データの各々の異常度が閾値よりも大きい場合には、車両の位置を推定しない。すなわち、位置推定部３６は、位置異常度が位置閾値よりも大きく、方位角異常度が方位角閾値よりも大きく、速度異常度が速度閾値よりも大きい場合には、最新の位置データ、方位角データ及び速度データが異常なデータであるため、車両の位置を推定しない。これにより、異常なデータを用いて車両の位置を推定することを防止できる。

位置推定部３６は、状態を推定する公知のカルマンフィルタに位置データ、方位角データ及び速度データを入力して、車両の位置を推定する。具体的には、位置推定部３６は、最新の位置データ、方位角データ及び速度データのうちの異常度が閾値以下であるデータを観測値としてカルマンフィルタに入力して補正した位置を、車両の位置として推定する。

＜車両位置推定処理の流れ＞
図３は、車両位置推定処理の流れを示すフローチャートである。図３の処理は、車両が走行を開始して所定時間経過したところから始まる。以下では、所定時間経過する迄に、車両位置推定装置１のデータ取得部３２が取得した位置データ、方位角データ及び速度データが、記憶部２０に時系列データとして記憶されたものとする。

まず、データ取得部３２は、走行中の車両の位置データ、方位角データ及び速度（車速）データを取得する（ステップＳ１０２）。具体的には、データ取得部３２は、ＬｉＤＡＲ１２の検出データと地図データとをマッチングして得られた車両の位置データ及び方位角データと、車速センサ１４によって検出された速度データとを取得する。

次に、異常度算出部３４は、ステップＳ１０２で取得された位置データ、方位角データ及び速度データの異常度を算出する（ステップＳ１０４）。具体的には、異常度算出部３４は、ステップＳ１０２で取得された位置データ、方位角データ及び速度データと、記憶部２０に記憶された時系列データとを比較して、位置異常度、方位角異常度及び速度異常度を算出する

次に、位置推定部３６は、３つの異常度（位置異常度、方位角異常度及び速度異常度）が閾値（位置閾値、方位角閾値及び速度閾値）よりも大きいか否かを判定する（ステップＳ１０６）。ステップＳ１０６において少なくとも一つの異常度が閾値以下である場合には（Ｎｏ）、位置推定部３６は、車両の位置を推定する（ステップＳ１０８）。具体的には、位置推定部３６は、異常度が閾値以下のデータをカルマンフィルタに代入して、車両の位置を推定する。一方で、ステップＳ１０６において３つの異常度の各々が閾値よりも大きい場合には（Ｙｅｓ）、位置推定部３６は、ステップＳ１０８の車両の位置推定を行わず、ステップＳ１０２に戻る。

ステップＳ１０８の車両の位置推定が行われた後は、走行中の車両が駐車する迄（ステップＳ１１０：Ｎｏ）、上述したステップＳ１０２～Ｓ１０８の処理が繰り返される。すなわち、車両位置推定装置１は、所定の時間間隔で取得される位置データ、方位角データ及び速度データを用いて、走行中の車両の位置を継続的に推定する。一方で、車両が駐車すると（ステップＳ１１０：Ｙｅｓ）、車両位置推定装置１は、車両位置推定処理を終了する。

＜本実施形態における効果＞
上述した実施形態の車両位置推定装置１は、最新の位置データ、方位角データ及び速度データと、過去に取得した位置データ、方位角データ及び速度データの時系列データとを比較して、最新の位置データ、方位角データ及び速度データの各々の異常度（位置異常度、方位角異常度及び速度異常度）を求める。そして、車両位置推定装置１は、位置異常度、方位角異常度及び速度異常度うちの少なくとも一つの異常度が所定の閾値以下である場合に、車両の位置を推定する。
これにより、車両位置推定装置１は、位置データ、方位角データ及び速度データのうちの少なくとも一つのデータが正常である場合に、正常なデータを用いて車両の位置を推定することになる。このため、正常でないデータを用いて車両の位置を推定することを抑制できる。特に、車両の周辺環境を検出する環境検出センサ（ＬｉＤＡＲ１２）が一つのみである場合には、取得される位置データや方位角データが異常である（データの異常度が、閾値よりも大きい）ケースが生じうる。これに対して、上述したように異常度が閾値以下のデータを用いて車両の位置を推定することで、多数の検出センサを用いなくても車両の位置を精度よく推定できる。

以上、本発明を実施の形態を用いて説明したが、本発明の技術的範囲は上記実施の形態に記載の範囲には限定されず、その要旨の範囲内で種々の変形及び変更が可能である。例えば、装置の全部又は一部は、任意の単位で機能的又は物理的に分散・統合して構成することができる。また、複数の実施の形態の任意の組み合わせによって生じる新たな実施の形態も、本発明の実施の形態に含まれる。組み合わせによって生じる新たな実施の形態の効果は、もとの実施の形態の効果を併せ持つ。

１ 車両位置推定装置
３２ データ取得部
３４ 異常度算出部
３６ 位置推定部
１２ ＬｉＤＡＲ
Ｖ 車両

Claims (6)

1. 車両の走行中の前記車両の位置データ、方位角データ及び速度データを、所定の時間間隔で取得するデータ取得部であって、前記車両に搭載された一つの検出センサが検出した検出データと、地図データとをマッチングして、前記位置データ及び前記方位角データを取得するデータ取得部と、
   最新の前記位置データ、前記方位角データ及び前記速度データと、過去に取得した複数の前記位置データ、前記方位角データ及び前記速度データとを比較して、最新の前記位置データ、前記方位角データ及び前記速度データの各々の異常度を求める異常度算出部と、
   前記位置データの異常度を示す位置異常度、前記方位角データの異常度を示す方位角異常度、及び前記速度データの異常度の示す速度異常度うちの少なくとも前記位置異常度及び前記方位角異常度が所定の閾値以下である場合に、前記車両の位置を推定する位置推定部と、
   を備える、車両位置推定装置。
2. 前記位置推定部は、最新の前記位置データ、前記方位角データ及び前記速度データのうちの少なくとも前記異常度が前記閾値以下である前記位置データ及び前記方位角データを用いて、前記車両の位置を推定する、
   請求項１に記載の車両位置推定装置。
3. 前記位置推定部は、最新の前記位置データ、前記方位角データ及び前記速度データのうちの少なくとも前記異常度が前記閾値以下である前記位置データ及び前記方位角データを観測値としてカルマンフィルタに入力して補正した位置を、前記車両の位置として推定する、
   請求項２に記載の車両位置推定装置。
4. 前記位置推定部は、最新の前記位置データ、前記方位角データ及び前記速度データの各々の異常度が前記閾値よりも大きい場合には、前記車両の位置を推定しない、
   請求項１に記載の車両位置推定装置。
5. 前記異常度算出部は、
   最新の前記位置データの過去に取得した複数の位置データの各々との差分の総和から、前記位置異常度を求め、
   最新の前記方位角データの過去に取得した複数の方位角データの各々との差分の総和から、前記方位角異常度を求め、
   最新の前記速度データの過去に取得した複数の速度データの各々との差分の総和から、前記速度異常度を求める、
   請求項１に記載の車両位置推定装置。
6. 車両の走行中の前記車両の位置データ、方位角データ及び速度データを、所定の時間間隔で取得するステップであって、前記車両に搭載された一つの検出センサが検出した検出データと、地図データとをマッチングして、前記位置データ及び前記方位角データを取得するステップと、
   最新の前記位置データ、前記方位角データ及び前記速度データと、過去に取得した複数の前記位置データ、前記方位角データ及び前記速度データとを比較して、最新の前記位置データ、前記方位角データ及び前記速度データの各々の異常度を求めるステップと、
   前記位置データの異常度を示す位置異常度、前記方位角データの異常度を示す方位角異常度、及び前記速度データの異常度の示す速度異常度うちの少なくとも前記位置異常度及び前記方位角異常度が所定の閾値以下である場合に、前記車両の位置を推定するステップと、
   を有する、車両位置推定方法。