JP7418196B2 - 走行軌跡推定方法及び走行軌跡推定装置 - Google Patents

Description

本発明は、車両が走行した走行軌跡を推定する走行軌跡推定方法及び走行軌跡推定装置に関する。

従来から地図データを更新する方法が知られており、地図データ更新方法として特許文献１が開示されている。この特許文献１では、ＧＰＳデータとオドメトリを用いて車両の自車位置を検出し、走行時における走行軌跡を演算していた。

特開２００５－９８８５３号公報

しかしながら、ＧＰＳ信号は必ずしも安定して取得できるとは限らない。そのため、上述した従来の地図データ更新方法では、ＧＰＳ信号を安定して取得できない状況において、車両の走行軌跡を精度よく推定することができないという問題点があった。

そこで、本発明は、上記課題に鑑みて成されたものであり、ＧＰＳ信号を安定して取得できない状況であっても、車両の走行軌跡を精度よく推定することのできる走行軌跡推定方法及び走行軌跡推定装置を提供することを目的とする。

本発明の一態様に係る走行軌跡推定方法及びその装置は、走行データに基づいて車両が走行した走行軌跡を算出し、車両周辺の環境情報と地図情報に記録された環境情報とを照合して車両の基準位置を検出する。そして、基準位置と走行軌跡との差である車両位置誤差を算出し、走行軌跡上にある車両位置のうち、基準位置が検出された車両位置では車両位置誤差に基づいて走行軌跡を補正する。一方、基準位置が検出されなかった車両位置では車両位置の前後に隣接する車両位置の間の移動量を示す相対変位と車両位置におけるオドメトリとの差である変位誤差を算出し、変位誤差に基づいて走行軌跡を補正する。

本発明によれば、ＧＰＳ信号を安定して取得できない状況であっても、車両の走行軌跡を精度よく推定することができる。

図１は、本発明の一実施形態に係る走行軌跡推定装置を備えた走行軌跡推定システムの構成を示すブロック図である。 図２は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の補正方法を説明するための図である。 図３は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の補正方法を説明するための図である。 図４は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の補正方法を説明するための図である。 図５は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の推定処理の処理手順を示すフローチャートである。

以下、本発明の一実施形態について、図面を参照して説明する。図面の記載において同一部分には同一符号を付して詳細な説明は省略する。

［走行軌跡推定システムの構成］
図１を参照して、本実施形態に係る走行軌跡推定装置を備えた走行軌跡推定システムの構成を説明する。図１に示すように、走行軌跡推定システム１００は、走行軌跡推定装置１と、ＧＰＳ受信機３と、ＩＭＵ（慣性計測装置）５と、カメラ７と、レーザレンジファインダー９と、データベース１１とを備える。この他にミリ波レーダーや通信機等を備えていてもよい。また、本実施形態では、走行軌跡推定システム１００が車両に搭載されている場合について説明するが、走行軌跡推定装置１については車両に搭載されていなくてもよい。

ＧＰＳ受信機３は、人工衛星からの電波を受信することにより、地上における自車両の現在地を検出し、検出したデータをデータベース１１に出力する。

ＩＭＵ５は、３軸のジャイロセンサと３方向の加速度計から構成され、３次元の角度（または角速度）と加速度を検出する。また、ＩＭＵ５は、車輪速センサを備えていてもよい。そして、ＩＭＵ５は、検出したセンサ値を用いてオドメトリを算出する。オドメトリは、車両の単位時間当たりの移動量であり、ＩＭＵ５で検出されたデータや車両に搭載されたセンサ群で検出されたセンサ値を用いて算出することができる。ＩＭＵ５は、検出したデータと算出したオドメトリをデータベース１１に出力する。

カメラ７は、ＣＣＤ（Charge-Coupled Device）やＣＭＯＳ（Complementary Metal Oxide Semiconductor）などの撮像素子を有する。カメラ７は、自車両に搭載され、自車両の周囲を撮影する。カメラ７は画像処理機能を有しており、撮影した画像から車両周囲の環境情報を検出する。環境情報には、物標と道路構造が含まれている。物標とは、道路や歩道に設けられた物体であり、例えば信号機や電柱、交通標識等である。また、道路構造とは、道路の構成物であり、例えば白線や黄色線等の道路の区画線や停止線の他に道路端部や縁石、中央分離帯等である。カメラ７は、検出したデータをデータベース１１に出力する。

レーザレンジファインダー９は、光センサ技術であるＬｉＤＡＲ（Light Detection and Ranging）を用いて車両周囲の環境情報を検出するセンサである。具体的に、レーザレンジファインダー９は、レーザ光をある角度範囲内で走査し、その時の反射光を受光して、レーザ発射時点と反射光の受光時点との間の時間差を計測して自車両に対する物標や道路構造の相対距離や方向（すなわち、自車両に対する相対位置）などを検出する。レーザレンジファインダー９は、検出したデータをデータベース１１に出力する。尚、レーザレンジファインダー９は、車両周囲の物標の車両に対する相対位置を検出するセンサであれば、ＬｉＤＡＲ以外であっても良く、例えばレーザレーダあるいは音波レーダなどのレーザレンジファインダー以外のセンサを用いてもよい。

また、走行軌跡推定装置１は、車両に搭載されたセンサ群（図示せず）に接続されている。例えば、アクセルセンサ、ステアリングセンサ、ブレーキセンサ、車速センサ等に接続されて、これらのセンサ群から出力されるセンサ値を取得することができる。

データベース１１は、走行データや環境情報、地図情報等を格納している。走行データは、ＧＰＳ受信機３で検出された車両の位置情報と、ＩＭＵ５で検出された車両の角度や加速度等と、ＩＭＵ５で算出されたオドメトリを含んでいる。環境情報は、カメラ７とレーザレンジファインダー９で検出された物標の位置や道路構造等の情報を含んでいる。地図情報は、カーナビゲーション装置等に記憶されている情報であり、物標情報や施設情報などの経路案内に必要となる各種データを含んでいる。さらに、地図情報は、高精度な地図情報であり、道路や歩道に設けられた信号機や電柱などの立体物、あるいは車線境界線等の物標の地図上の位置情報や道路の車線数や道路境界線、停止線等の道路構造情報を含んでいる。データベース１１は、走行軌跡推定装置１の要求に応じて、格納している情報を走行軌跡推定装置１へ出力する。また、データベース１１は、走行軌跡の推定に必要となるその他の情報も記憶している。

尚、データベース１１は、サーバから定期的に最新の地図情報を入手して、保有する地図情報を更新する。本実施形態では、地図情報として第一地図（高精度地図、車両のルート案内に用いる地図）を備えており、第一地図を補足するものとして、第二地図を備えていてもよい。この第二地図は、第一地図だけでは不完全な場合や、より詳細な地図情報が必要な場合に、車両や外部のサーバで作成される。

走行軌跡推定装置１は、車両が走行した走行軌跡を推定する。走行軌跡推定装置１は、車両が事前に走行して検出しておいた走行データをデータベース１１から取得し、取得した走行データを用いて、過去に車両が走行した走行軌跡を推定する。推定された走行軌跡は、地図データを更新するために使用される。例えば、推定された走行軌跡を用いて物標の位置を計算し、この計算結果を用いて地図データに記録された物標の位置を更新する。

また、走行軌跡推定装置１は、ＧＰＳ受信機３、ＩＭＵ５、カメラ７、レーザレンジファインダー９、データベース１１から取得したデータを処理する制御部を備えており、例えばＩＣ、ＬＳＩ等によって構成される。走行軌跡推定装置１は、これを機能的に捉えた場合、走行軌跡算出部２１と、地図照合部２３と、走行軌跡補正部２５と、走行軌跡調整部２７に分類することができる。

尚、走行軌跡推定装置１は、マイクロコンピュータ、マイクロプロセッサ、ＣＰＵを含む汎用の電子回路とメモリ等の周辺機器から構成されている。このような走行軌跡推定装置１の各機能は、１または複数の処理回路によって実装することができる。処理回路は、例えば電気回路を含む処理装置等のプログラムされた処理装置を含み、また実施形態に記載された機能を実行するようにアレンジされた特定用途向け集積回路（ＡＳＩＣ）や従来型の回路部品のような装置も含んでいる。

走行軌跡算出部２１は、車両が走行したときに検出した走行データをデータベース１１から取得し、取得した走行データに基づいて、車両が走行した走行軌跡を算出する。具体的に、走行軌跡算出部２１は、ＧＰＳ受信機３で検出した車両位置を、単位時間当たりの移動量であるオドメトリの分だけ移動させていくことによって、車両の走行軌跡を算出する。ここで算出される走行軌跡は、時刻毎の車両位置と車両の姿勢との集合によって表される。車両の姿勢とは、車両の角度、すなわち車両の向きである。例えば、走行軌跡は、時刻ｔｉ（ｉ＝１～ｎ）における座標（ｘ、ｙ）と車両の向きθの集合によって表される。

地図照合部２３は、車両が走行したときに検出した車両周辺の環境情報と地図情報をデータベース１１から取得し、取得した車両周辺の環境情報と地図情報に記録された環境情報とを照合して車両の基準位置を検出する。

具体的に、地図照合部２３は、レーザレンジファインダー９によって検出された車両周辺の物標の車両に対する相対位置であるＬｉＤＡＲデータと、地図情報に点群データとして記録された物標の地図上の位置とをマップマッチングによって照合して、地図上における車両の基準位置を検出する。また、地図情報に記録された白線や縁石等の道路構造を抽出し、車両周辺のＬｉＤＡＲデータからも白線や縁石等の道路構造を抽出して、マップマッチングによって照合し、地図上における車両の基準位置を検出してもよい。地図照合部２３は、ＮＤＴ（Normal Distributions Transform）のような点群データのマッチング手法を用いて基準位置を検出しているので、検出された基準位置は、オドメトリを用いて求められた走行軌跡よりも正確な位置である可能性が高い。そこで、本実施形態では、この基準位置を所定の間隔で検出し、検出された基準位置に近づけるように走行軌跡を補正している。尚、地図照合部２３は、マップマッチングの他にも、カメラ７で検出された物標や道路構造を地図情報と照合して車両の基準位置を検出してもよい。

こうして基準位置が検出されると、地図照合部２３は、基準位置と走行軌跡との差である車両位置誤差を算出する。例えば、図２に示すように、走行軌跡上において、車両が時刻ｔ１に車両位置Ｘ１に位置し、時刻ｔ２に車両位置Ｘ２に位置し、時刻ｔ３に車両位置Ｘ３に位置している。そして、時刻ｔ１における基準位置Ｙ１と、時刻ｔ３における基準位置Ｙ３がそれぞれ検出されている。このような場合に、地図照合部２３は、基準位置Ｙ１と車両位置Ｘ１の差である車両位置誤差ｄ１を算出する。同様に、基準位置Ｙ３と車両位置Ｘ３の差である車両位置誤差ｄ３を算出する。尚、車両位置誤差ｄ１、ｄ３は、車両位置の差と車両の向き（角度）の差で表される。

地図照合部２３は、所定時間毎または所定距離毎に基準位置を検出している。基準位置を検出する間隔が広すぎると、走行軌跡の推定精度が低下し、間隔が狭すぎると、推定の制約（条件）が多くなって走行軌跡の推定が難しくなる。そこで、所定時間と所定距離の設定方法については、走行軌跡の推定精度を高くすることのできる基準位置の間隔を予め実験やシミュレーションによって求めておき、その間隔となるように所定時間と所定距離を設定すればよい。

また、基準位置は、車両が所定の走行状態のときに検出されるようにしてもよい。例えば、所定の走行状態は、車両の停止状態または定速走行状態または直進状態のいずれかの走行状態である。このように車両が停止や定速走行、直進している場合には、マップマッチングによる照合を正確に行うことができるので、基準位置も正確に検出することができる。そこで、地図照合部２３は、このような所定の走行状態のときに基準位置を検出する。

さらに、地図照合部２３は、車両の姿勢が変化した後に、車両が定速直進走行に移行した時点で、基準位置を検出してもよい。例えば、車両が交差点を左折した場合には、交差点の前で基準位置を検出し、さらに交差点を左折した後にも基準位置を検出できれば、交差点の前後で基準位置を検出できるので、走行軌跡をより正確に推定することができる。そこで、車両が交差点やカーブを通過したことを検出するために、地図照合部２３は、車両の姿勢が変化したことを検出する。さらに、交差点の途中やカーブの途中で基準位置を検出することがないように、地図照合部２３は、車両が定速直進走行に移行した時点で基準位置を検出する。これにより、交差点やカーブを完全に通過した後に、基準位置を検出することができる。

また、地図照合部２３は、車両周辺の環境情報と地図情報に記録された環境情報との一致度が低い車両位置では、基準位置を検出しないようにする。例えば、レーザレンジファインダー９で検出した複数の物標間の相対位置関係と、地図情報に記録された複数の物標の相対位置関係とを照合したときに、その一致度が所定の一致度に満たない場合には、誤検出である可能性が高いので、そのような車両位置では基準位置を検出しないようにする。これにより、正確な環境情報を検出できた車両位置のみで、基準位置を検出するので、走行軌跡の推定精度を向上させることができる。

走行軌跡補正部２５は、走行軌跡算出部２１で算出された走行軌跡を補正する。特に、走行軌跡補正部２５は、走行軌跡上にある車両位置のうち、基準位置が検出された車両位置では、地図照合部２３で算出された車両位置誤差に基づいて走行軌跡を補正する。

例えば、図２に示すように、走行軌跡上にある車両位置のうち、車両位置Ｘ１、Ｘ３では基準位置Ｙ１、Ｙ３が検出されており、車両位置誤差ｄ１、ｄ３が算出されている。そこで、走行軌跡補正部２５は、時刻ｔ１では、車両位置Ｘ１を車両位置誤差ｄ１だけ移動させる。その結果、図３に示すように、車両位置Ｘ１は基準位置Ｙ１の位置に移動する。同様に、車両位置Ｘ３も車両位置誤差ｄ３だけ移動させて基準位置Ｙ３の位置に移動する。

一方、走行軌跡補正部２５は、基準位置が検出されなかった車両位置では、その車両位置の前後に隣接する車両位置に基づいて走行軌跡を補正する。例えば、図２に示すように、時刻ｔ２の車両位置Ｘ２では基準位置が検出されていない。基準位置は、上述したように、所定時間毎または所定距離毎に検出されており、車両が所定の走行状態のときに検出される場合もある。そのため、走行軌跡上にある車両位置のすべてで基準位置が検出されるわけではない。そこで、車両位置Ｘ２のように基準位置が検出されなかった車両位置では、走行軌跡補正部２５は、車両位置Ｘ２の前後に隣接する車両位置Ｘ１、Ｘ３に基づいて走行軌跡を補正する。

具体的に、走行軌跡補正部２５は、前後に隣接する車両位置の間の移動量を示す相対変位と車両位置におけるオドメトリとの差である変位誤差を算出し、この変位誤差に基づいて走行軌跡を補正する。

例えば、図３に示すように、車両位置Ｘ３は車両位置誤差ｄ３だけ移動しているので、車両位置Ｘ２におけるオドメトリＢ２の先端の位置からずれている。この場合に、走行軌跡補正部２５は、前後に隣接する車両位置Ｘ２とＸ３の間の移動量を示す相対変位Ａ２を算出し、この相対変位Ａ２とオドメトリＢ２との差である変位誤差Ｃ２を算出する。相対変位とオドメトリは車両位置と車両の向き（角度）で表されるので、変位誤差は車両位置の差と車両の向き（角度）の差で表される。

そして、走行軌跡補正部２５は、車両位置Ｘ３の位置に近づけるように、変位誤差Ｃ２の分だけ車両位置Ｘ２を移動させる。これにより、車両位置Ｘ２は、図４に示すように車両位置Ｘ３と並ぶような位置に移動する。車両位置Ｘ３はすでに基準位置Ｙ３の位置に移動されているので、正確な位置である可能性が高い。したがって、車両位置Ｘ３の位置に近づけるように車両位置Ｘ２を移動させれば、車両位置Ｘ２も正確な位置に補正することが可能となる。

同様に、走行軌跡補正部２５は、図３に示すように、前後に隣接する車両位置Ｘ１とＸ２の間の移動量を示す相対変位Ａ１を算出し、この相対変位Ａ１と車両位置Ｘ１におけるオドメトリＢ１との差である変位誤差Ｃ１を算出する。

そして、走行軌跡補正部２５は、車両位置Ｘ１の位置に近づけるように、変位誤差Ｃ１の分だけ車両位置Ｘ２を移動させる。これにより、車両位置Ｘ２は、図４に示すように車両位置Ｘ１と並ぶような位置に移動する。車両位置Ｘ１はすでに基準位置Ｙ１の位置に移動されているので、正確な位置である可能性が高い。したがって、車両位置Ｘ１の位置に近づけるように車両位置Ｘ２を移動させれば、車両位置Ｘ２も正確な位置に補正することが可能となる。

ただし、車両位置Ｘ１、Ｘ３のように基準位置が検出されている場合には、車両位置Ｘ１、Ｘ３は車両位置誤差ｄ１、ｄ３だけ移動しているので、変異誤差Ｃ１、Ｃ２と車両位置誤差ｄ１、ｄ３は等しくなる。図３では、車両位置Ｘ２の前後に隣接する車両位置Ｘ１、Ｘ３は、いずれも基準位置が検出されているが、実際には、基準位置が検出された車両位置の間には、基準位置の検出されていない車両位置が多く存在している。

そこで、実際の走行軌跡の補正では、基準位置が検出された車両位置の近くにある車両位置から順に、変異誤差を算出して車両位置を移動させていき、最終的に基準位置が検出された車両位置の間にあるすべての車両位置を移動させて走行軌跡の補正を完了する。このように、走行軌跡補正部２５は、基準位置が検出された車両位置をまず移動させ、その後に基準位置が検出された車両位置の間にある車両位置を順番に移動させていくことによって、走行軌跡を補正する。

走行軌跡調整部２７は、走行軌跡補正部２５で補正された走行軌跡の誤差が小さくなるように調整し、調整された走行軌跡を、走行軌跡の推定結果として出力する。具体的に、走行軌跡調整部２７は、地図照合部２３によって算出された車両位置誤差と、走行軌跡補正部２５によって算出された変位誤差をすべて加算して誤差総和を算出し、この誤差総和が小さくなるように走行軌跡を調整する。

例えば、走行軌跡調整部２７は、ガウスニュートン法やＬＭ法等の最適化手法を使用して、車両位置Ｘｉ（ｉ＝１～ｎ）を移動させながら繰り返し計算の中で徐々に誤差総和が小さくなるように走行軌跡を調整する。そして、誤差総和が予め設定された所定値以下になると、走行軌跡調整部２７は、走行軌跡の調整が収束したと判定して、調整された走行軌跡を、走行軌跡の推定結果として出力する。

［走行軌跡の推定処理］
次に、図５を参照して、本実施形態に係る走行軌跡推定装置１による走行軌跡の推定処理を説明する。図５は、走行軌跡の推定処理を示すフローチャートである。

図５に示すように、ステップＳ１において、走行軌跡算出部２１は、車両が走行したときに検出した走行データをデータベース１１から取得する。また、地図照合部２３は、車両が走行したときに検出した環境情報と地図情報をデータベース１１から取得する。

ステップＳ３において、走行軌跡算出部２１は、ステップＳ１で取得した走行データに基づいて、車両が走行した走行軌跡を算出する。ここで算出される走行軌跡は、車両が過去に走行した走行軌跡であり、車両位置をオドメトリの分だけ移動させていくことによって走行軌跡を算出する。走行軌跡は、時刻毎の車両位置と車両の姿勢との集合によって表され、具体的に時刻ｔｉ（ｉ＝１～ｎ）における座標（ｘ、ｙ）と車両の向きθの集合によって表される。例えば、図２に示すように、走行軌跡は、時刻ｔ１の車両位置Ｘ１、時刻ｔ２の車両位置Ｘ２、時刻ｔ３の車両位置Ｘ３の集合によって表されている。ただし、車両位置Ｘｎには、車両の向きθｎが含まれている。

ステップＳ５において、地図照合部２３は、車両周辺の環境情報と地図情報に記録された環境情報とを照合して車両の基準位置を検出する。具体的に、地図照合部２３は、レーザレンジファインダー９によって検出された車両周辺のＬｉＤＡＲデータである車両周辺の物標の自車両に対する相対位置と、地図情報に記録された点群データである地図上の物標の位置とをマップマッチングによって照合して、地図上における車両の基準位置を検出している。

ステップＳ７において、地図照合部２３は、走行軌跡上にある車両位置の近傍に基準位置が検出されているか否かを判定し、基準位置が検出されている場合にはステップＳ９に進み、基準位置が検出されていない場合にはステップＳ１３に進む。例えば、図２に示すように、車両位置Ｘ１、Ｘ３では基準位置Ｙ１、Ｙ３が検出されているのでステップＳ９に進み、車両位置Ｘ２では基準位置が検出されていないのでステップＳ１３に進む。

地図照合部２３は、まず時刻ｔ１の車両位置Ｘ１の近傍に基準位置が検出されているか否かを判断してステップＳ９～１３の処理を行い、その後は順番に車両位置Ｘ２から車両位置ＸｎまでステップＳ９～Ｓ１３の処理を行う。

ステップＳ９において、地図照合部２３は、基準位置と走行軌跡上の車両位置との差である車両位置誤差を算出する。例えば、図２の車両位置Ｘ１では、基準位置Ｙ１と車両位置Ｘ１の差ｄ１を車両位置誤差として出力する。同様に、時刻ｔ３では、基準位置Ｙ３と車両位置Ｘ３の差ｄ３を車両位置誤差として出力する。

ステップＳ１１において、走行軌跡補正部２５は、基準位置が検出された車両位置について、ステップＳ９で算出された車両位置誤差に基づいて走行軌跡を補正する。例えば、図２に示すように、走行軌跡補正部２５は、基準位置Ｙ１、Ｙ３が検出された車両位置Ｘ１、Ｘ３について、車両位置誤差ｄ１、ｄ３だけ車両位置Ｘ１、Ｘ３を移動させて走行軌跡を補正する。

ステップＳ１３において、地図照合部２３は、走行軌跡上のすべての車両位置についてステップＳ７～１３の処理が行われたか否かを判定する。そして、ステップＳ７～１３の処理が行われていない車両位置がある場合にはステップＳ７に戻り、すべての車両位置についてステップＳ７～１３の処理が行われている場合にはステップＳ１５に進む。ステップＳ１５に進む場合には、基準位置が検出されたＸ１、Ｘ３のような車両位置では、図３に示すように車両位置がすべて移動されており、基準位置が検出されなかったＸ２のような車両位置では図３に示すように車両位置が移動されていないままとなっている。

ステップＳ１５において、走行軌跡補正部２５は、基準位置が検出されなかった車両位置について、前後に隣接する車両位置の間の移動量を示す相対変位と車両位置におけるオドメトリとの差である変位誤差を算出する。例えば、図３に示すように、基準位置が検出されなかった車両位置Ｘ２について、前後に隣接する車両位置Ｘ２とＸ３の間の移動量を示す相対変位Ａ２を算出し、この相対変位Ａ２と車両位置Ｘ２におけるオドメトリＢ２との差である変位誤差Ｃ２を算出する。同様に、走行軌跡補正部２５は、変位誤差Ｃ１を算出する。

ステップＳ１７において、走行軌跡補正部２５は、基準位置が検出されなかった車両位置について、ステップＳ１５で算出された変位誤差に基づいて走行軌跡を補正する。走行軌跡補正部２５は、図３に示すように、基準位置が検出されなかった車両位置Ｘ２について、変位誤差Ｃ１、Ｃ２だけ車両位置Ｘ２を移動させることで、図４に示すように走行軌跡を補正する。尚、前後に隣接する車両位置で基準位置が検出されている場合には、変位誤差は車両位置誤差と等しくなる。

ステップＳ１９において、走行軌跡補正部２５は、基準位置が検出されなかった車両位置について、車両位置の補正が終了したか否かを判定する。図３の車両位置Ｘ２のように基準位置が検出されなかったすべての車両位置について、走行軌跡補正部２５は、車両位置の補正が終了したか否かを判定する。そして、車両位置の補正が終了していない場合にはステップＳ１５に戻り、終了している場合にはステップＳ２１に進む。

ステップＳ２１において、走行軌跡調整部２７は、ステップＳ９で算出された車両位置誤差と、ステップＳ１５で算出された変位誤差をすべて加算して誤差総和を算出する。この誤差総和は、車両位置Ｘ１から車両位置Ｘｎまでのすべての車両位置の車両位置誤差と変位誤差を加算したものである。

ステップＳ２３において、走行軌跡調整部２７は、ステップＳ２１で算出された誤差総和が小さくなるように走行軌跡を調整し、収束したか否かを判定する。走行軌跡調整部２７は、ガウスニュートン法やＬＭ法等の最適化手法を使用して、車両位置Ｘｉ（ｉ＝１～ｎ）を移動させながら繰り返し計算の中で徐々に誤差総和が小さくなるように走行軌跡を調整する。そして、誤差総和が予め設定された所定値以下とならない場合には走行軌跡の調整が収束しないと判定してステップＳ３に戻る。一方、誤差総和が予め設定された所定値以下になると、走行軌跡の調整が収束したと判定し、調整された走行軌跡を、走行軌跡の推定結果として出力して本実施形態に係る走行軌跡の推定処理を終了する。

［実施形態の効果］
以上詳細に説明したように、本実施形態に係る走行軌跡推定装置１では、走行データに基づいて車両が走行した走行軌跡を算出し、車両周辺の環境情報と地図情報に記録された環境情報とを照合して車両の基準位置を検出する。そして、基準位置と走行軌跡との差である車両位置誤差を算出し、走行軌跡上にある車両位置のうち、基準位置が検出された車両位置では車両位置誤差に基づいて走行軌跡を補正する。一方、基準位置が検出されなかった車両位置では車両位置の前後に隣接する車両位置に基づいて走行軌跡を補正する。これにより、ＧＰＳ信号を安定して取得できない状況であっても、車両周辺の環境情報から基準位置を検出して走行軌跡を補正できるので、車両の走行軌跡を精度よく推定することができる。

特に、本実施形態に係る走行軌跡推定装置１では、レーザレンジファインダー９で取得したＬｉＤＡＲデータを用いてＮＤＴ（Normal Distributions Transform）のような点群データのマッチング手法で基準位置を検出している。このようにして検出された基準位置は、オドメトリを用いて求められた走行軌跡よりも正確な位置である可能性が高い。したがって、本実施形態に係る走行軌跡推定装置１は、正確な位置に検出された基準位置に基づいて走行軌跡を補正するので、車両の走行軌跡を精度よく推定することができる。

また、本実施形態に係る走行軌跡推定装置１では、所定時間毎または所定距離毎に基準位置を検出する。これにより、予め推定精度が高くなるように設定された所定の間隔で基準位置を検出して走行軌跡を補正するので、車両の走行軌跡を精度よく推定することができる。

さらに、本実施形態に係る走行軌跡推定装置１では、車両が所定の走行状態のときに基準位置を検出する。これにより、マップマッチングによる照合を正確に行うことができる走行状態のときに基準位置を検出することができる。したがって、正確な位置に検出された基準位置を用いて走行軌跡を補正できるので、車両の走行軌跡を精度よく推定することができる。

また、本実施形態に係る走行軌跡推定装置１では、所定の走行状態として、車両の停止状態または定速走行状態または直進状態のいずれかのときに基準位置を検出する。これにより、マップマッチングによる照合を正確に行うことができる停止状態、定速走行状態、直進状態のときに基準位置を検出することができる。したがって、正確な位置に検出された基準位置を用いて走行軌跡を補正できるので、車両の走行軌跡を精度よく推定することができる。

さらに、本実施形態に係る走行軌跡推定装置１では、車両の姿勢が変化した後に車両が定速直進走行に移行した時点で基準位置を検出する。これにより、カーブや交差点のように車両の姿勢が変化した地点の後で基準位置を検出できるので、走行軌跡をより正確に推定することができる。

また、本実施形態に係る走行軌跡推定装置１では、車両周辺の環境情報と地図情報に記録された環境情報との一致度が低い車両位置では、基準位置を検出していない。これにより、正確な環境情報を検出できた車両位置のみで、基準位置を検出するので、走行軌跡の推定精度を向上させることができる。

さらに、本実施形態に係る走行軌跡推定装置１では、時刻毎の車両位置と車両の姿勢との集合によって、走行軌跡を表している。これにより、走行軌跡を時間毎に正確に表すことができるので、車両の走行軌跡を精度よく推定することができる。

また、本実施形態に係る走行軌跡推定装置１では、基準位置が検出されなかった車両位置において、前後に隣接する車両位置の間の移動量を示す相対変位と車両位置におけるオドメトリとの差である変位誤差を算出する。そして、この変位誤差に基づいて走行軌跡を補正する。これにより、基準位置が検出されなかった車両位置でも、前後に隣接する車両位置に基づいて走行軌跡を補正できるので、車両の走行軌跡を精度よく推定することができる。

さらに、本実施形態に係る走行軌跡推定装置１では、車両位置誤差と変位誤差をすべて加算した誤差総和を算出し、この誤差総和が小さくなるように、補正された走行軌跡を調整する。これにより、走行軌跡全体の整合性を取ることができるので、推定された走行軌跡の精度を向上させることができる。

尚、上述の実施形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計などに応じて種々の変更が可能であることは勿論である。

１ 走行軌跡推定装置
３ ＧＰＳ受信機
５ ＩＭＵ（慣性計測装置）
７ カメラ
９ レーザレンジファインダー
１１ データベース
２１ 走行軌跡算出部
２３ 地図照合部
２５ 走行軌跡補正部
２７ 走行軌跡調整部
１００ 走行軌跡推定システム

Claims (9)

1. 車両が走行した走行軌跡を推定する走行軌跡推定方法であって、
   前記車両が走行したときに検出した走行データと、前記車両が走行したときに検出した車両周辺の環境情報とを取得し、
   前記走行データに基づいて前記車両が走行した走行軌跡を算出し、
   前記車両周辺の環境情報と地図情報に記録された環境情報とを照合して前記車両の基準位置を検出し、
   前記基準位置と前記走行軌跡との差である車両位置誤差を算出し、
   前記走行軌跡上にある車両位置のうち、前記基準位置が検出された車両位置では、前記車両位置誤差に基づいて前記走行軌跡を補正し、
   前記基準位置が検出されなかった車両位置では、前記車両位置の前後に隣接する車両位置の間の移動量を示す相対変位と前記車両位置におけるオドメトリとの差である変位誤差を算出し、前記変位誤差に基づいて前記走行軌跡を補正することを特徴とする走行軌跡推定方法。
2. 前記基準位置は、所定時間毎または所定距離毎に検出されることを特徴とする請求項１に記載の走行軌跡推定方法。
3. 前記基準位置は、前記車両が所定の走行状態のときに検出されることを特徴とする請求項１または２に記載の走行軌跡推定方法。
4. 前記所定の走行状態は、前記車両の停止状態または定速走行状態または直進状態のいずれかであることを特徴とする請求項３に記載の走行軌跡推定方法。
5. 前記基準位置は、前記車両の姿勢が変化した後に前記車両が定速直進走行に移行した時点で検出されることを特徴とする請求項１または２に記載の走行軌跡推定方法。
6. 前記基準位置は、前記車両周辺の環境情報と前記地図情報に記録された環境情報との一致度が低い車両位置では検出されないことを特徴とする請求項１～５のいずれか１項に記載の走行軌跡推定方法。
7. 前記走行軌跡は、時刻毎の車両位置と車両の姿勢との集合によって表されることを特徴とする請求項１～６のいずれか１項に記載の走行軌跡推定方法。
8. 前記車両位置誤差と前記変位誤差をすべて加算した誤差総和を算出し、前記誤差総和が小さくなるように、前記補正された走行軌跡を調整することを特徴とする請求項１～７のいずれか１項に記載の走行軌跡推定方法。
9. 車両が走行した走行軌跡を推定する制御部を備えた走行軌跡推定装置であって、
   前記制御部は、
   前記車両が走行したときに検出した走行データと、前記車両が走行したときに検出した車両周辺の環境情報とを取得し、
   前記走行データに基づいて前記車両が走行した走行軌跡を算出し、
   前記車両周辺の環境情報と地図情報に記録された環境情報とを照合して前記車両の基準位置を検出し、
   前記基準位置と前記走行軌跡との差である車両位置誤差を算出し、
   前記走行軌跡上にある車両位置のうち、前記基準位置が検出された車両位置では、前記車両位置誤差に基づいて前記走行軌跡を補正し、
   前記基準位置が検出されなかった車両位置では、前記車両位置の前後に隣接する車両位置の間の移動量を示す相対変位と前記車両位置におけるオドメトリとの差である変位誤差を算出し、前記変位誤差に基づいて前記走行軌跡を補正することを特徴とする走行軌跡推定装置。

Images