特開2019-219204(P2019-219204A)IP Force 特許公報掲載プロジェクト 2015.5.11 β版

知財求人 - 知財ポータルサイト「IP Force」

▶ 日産自動車株式会社の特許一覧
特開2019-219204走行軌跡推定方法及び走行軌跡推定装置
<>
  • 特開2019219204-走行軌跡推定方法及び走行軌跡推定装置 図000003
  • 特開2019219204-走行軌跡推定方法及び走行軌跡推定装置 図000004
  • 特開2019219204-走行軌跡推定方法及び走行軌跡推定装置 図000005
  • 特開2019219204-走行軌跡推定方法及び走行軌跡推定装置 図000006
  • 特開2019219204-走行軌跡推定方法及び走行軌跡推定装置 図000007
< >
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】特開2019-219204(P2019-219204A)
(43)【公開日】2019年12月26日
(54)【発明の名称】走行軌跡推定方法及び走行軌跡推定装置
(51)【国際特許分類】
   G01C 21/28 20060101AFI20191129BHJP
   G09B 29/10 20060101ALI20191129BHJP
【FI】
   G01C21/28
   G09B29/10 A
【審査請求】未請求
【請求項の数】6
【出願形態】OL
【全頁数】12
(21)【出願番号】特願2018-115215(P2018-115215)
(22)【出願日】2018年6月18日
(71)【出願人】
【識別番号】000003997
【氏名又は名称】日産自動車株式会社
(71)【出願人】
【識別番号】507308902
【氏名又は名称】ルノー エス.ア.エス.
【氏名又は名称原語表記】RENAULT S.A.S.
(74)【代理人】
【識別番号】100083806
【弁理士】
【氏名又は名称】三好 秀和
(74)【代理人】
【識別番号】100101247
【弁理士】
【氏名又は名称】高橋 俊一
(74)【代理人】
【識別番号】100095500
【弁理士】
【氏名又は名称】伊藤 正和
(74)【代理人】
【識別番号】100098327
【弁理士】
【氏名又は名称】高松 俊雄
(72)【発明者】
【氏名】武田 祐一
(72)【発明者】
【氏名】▲高▼野 博幸
【テーマコード(参考)】
2C032
2F129
【Fターム(参考)】
2C032HB11
2C032HB22
2C032HC08
2C032HD26
2C032HD30
2F129AA03
2F129BB03
2F129BB15
2F129BB33
2F129BB53
2F129CC19
2F129CC20
2F129DD29
2F129EE94
(57)【要約】
【課題】白線等の環境情報が検出できない車両位置がある場合でも車両の走行軌跡を精度よく推定する。
【解決手段】本発明の走行軌跡推定方法は、走行データに基づいて車両が走行した走行軌跡を算出し、車両周辺の環境情報と地図情報に記録された環境情報との差である環境情報誤差を算出し、走行軌跡上にある車両位置のうち、車両周辺の環境情報が検出された車両位置では環境情報誤差に基づいて走行軌跡を補正し、車両周辺の環境情報が検出されなかった車両位置では車両位置の前後に隣接する車両位置に基づいて走行軌跡を補正する。
【選択図】図5
【特許請求の範囲】
【請求項1】
車両が走行した走行軌跡を推定する走行軌跡推定装置の走行軌跡推定方法であって、
前記車両が走行したときに検出した走行データと、前記車両が走行したときに検出した車両周辺の環境情報とを取得し、
前記走行データに基づいて前記車両が走行した走行軌跡を算出し、
前記車両周辺の環境情報と、地図情報に記録された環境情報との差である環境情報誤差を算出し、
前記走行軌跡上にある車両位置のうち、前記車両周辺の環境情報が検出された車両位置では、前記環境情報誤差に基づいて前記走行軌跡を補正し、
前記車両周辺の環境情報が検出されなかった車両位置では、前記車両位置の前後に隣接する車両位置に基づいて前記走行軌跡を補正することを特徴とする走行軌跡推定方法。
【請求項2】
前記走行軌跡は、時刻毎の車両位置と車両の姿勢との集合によって表されることを特徴とする請求項1に記載の走行軌跡推定方法。
【請求項3】
前記車両周辺の環境情報が1つの車両位置に複数検出されている場合、前記地図情報に記録された環境情報に最も特性が近い前記車両周辺の環境情報を用いて、前記環境情報誤差を算出することを特徴とする請求項1または2に記載の走行軌跡推定方法。
【請求項4】
前記車両周辺の環境情報が検出されなかった車両位置では、前後に隣接する車両位置の間の移動量を示す相対変位と、前記車両位置におけるオドメトリとの差である変位誤差を算出し、前記変位誤差に基づいて前記走行軌跡を補正することを特徴とする請求項1〜3のいずれか1項に記載の走行軌跡推定方法。
【請求項5】
前記環境情報誤差と前記変位誤差をすべて加算した誤差総和を算出し、前記誤差総和が小さくなるように、前記補正された走行軌跡を調整することを特徴とする請求項4に記載の走行軌跡推定方法。
【請求項6】
車両が走行した走行軌跡を推定する制御部を備えた走行軌跡推定装置であって、
前記制御部は、
前記車両が走行したときに検出した走行データと、前記車両が走行したときに検出した車両周辺の環境情報とを取得し、
前記走行データに基づいて前記車両が走行した走行軌跡を算出し、
前記車両周辺の環境情報と、地図情報に記録された環境情報との差である環境情報誤差を算出し、
前記走行軌跡上にある車両位置のうち、前記車両周辺の環境情報が検出された車両位置では、前記環境情報誤差に基づいて前記走行軌跡を補正し、
前記車両周辺の環境情報が検出されなかった車両位置では、前記車両位置の前後に隣接する車両位置に基づいて前記走行軌跡を補正することを特徴とする走行軌跡推定装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、車両が走行した走行軌跡を推定する走行軌跡推定方法及び走行軌跡推定装置に関する。
【背景技術】
【0002】
従来から地図データを更新する方法が知られており、地図データ更新方法として特許文献1が開示されている。この特許文献1では、GPSデータとオドメトリを用いて車両の自車位置を検出し、走行時における走行軌跡を演算していた。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開2005−98853号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、上述した従来の地図データ更新方法では、白線等の車両周辺にある環境情報が検出できない場合には、車両の走行軌跡を精度よく推定することができないという問題点があった。
【0005】
そこで、本発明は、上記課題に鑑みて成されたものであり、白線等の車両周辺にある環境情報が検出できない場合でも、車両の走行軌跡を精度よく推定することのできる走行軌跡推定方法及び走行軌跡推定装置を提供することを目的とする。
【課題を解決するための手段】
【0006】
本発明の一態様に係る走行軌跡推定方法及びその装置は、車両が走行したときに検出した走行データと、車両が走行したときに検出した車両周辺の環境情報とを取得し、走行データに基づいて車両が走行した走行軌跡を算出する。そして、車両周辺の環境情報と、地図情報に記録された環境情報との差である環境情報誤差を算出し、走行軌跡上にある車両位置のうち、車両周辺の環境情報が検出された車両位置では、環境情報誤差に基づいて走行軌跡を補正する。一方、車両周辺の環境情報が検出されなかった車両位置では、車両位置の前後に隣接する車両位置に基づいて走行軌跡を補正する。
【発明の効果】
【0007】
本発明によれば、白線等の車両周辺にある環境情報が検出できない場合でも、車両の走行軌跡を精度よく推定することができる。
【図面の簡単な説明】
【0008】
図1図1は、本発明の一実施形態に係る走行軌跡推定装置を備えた走行軌跡推定システムの構成を示すブロック図である。
図2図2は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の補正方法を説明するための図である。
図3図3は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の補正方法を説明するための図である。
図4図4は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の補正方法を説明するための図である。
図5図5は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の推定処理の処理手順を示すフローチャートである。
【発明を実施するための形態】
【0009】
以下、本発明の一実施形態について、図面を参照して説明する。図面の記載において同一部分には同一符号を付して詳細な説明は省略する。
【0010】
[走行軌跡推定システムの構成]
図1を参照して、本実施形態に係る走行軌跡推定装置を備えた走行軌跡推定システムの構成を説明する。図1に示すように、走行軌跡推定システム100は、走行軌跡推定装置1と、GPS受信機3と、IMU(慣性計測装置)5と、カメラ7と、データベース9とを備える。この他にレーザレンジファインダーや通信機等を備えていてもよい。また、本実施形態では、走行軌跡推定システム100が車両に搭載されている場合について説明するが、走行軌跡推定装置1については車両に搭載されていなくてもよい。
【0011】
GPS受信機3は、人工衛星からの電波を受信することにより、地上における自車両の現在地を検出し、検出したデータをデータベース9に出力する。
【0012】
IMU5は、3軸のジャイロセンサと3方向の加速度計から構成され、3次元の角度(または角速度)と加速度を検出する。また、IMU5は、車輪速センサを備えていてもよい。そして、IMU5は、検出したセンサ値を用いてオドメトリを算出する。オドメトリは、車両の単位時間当たりの移動量であり、IMU5で検出されたデータや車両に搭載されたセンサ群で検出されたセンサ値を用いて算出することができる。IMU5は、検出したデータと算出したオドメトリをデータベース9に出力する。
【0013】
カメラ7は、CCD(charge-coupled device)やCMOS(complementary metal oxide semiconductor)などの撮像素子を有する。カメラ7は、自車両に搭載され、自車両の周囲を撮影する。カメラ7は画像処理機能を有しており、撮影した画像から車両周囲の環境情報を検出する。環境情報には、物標と道路構造が含まれている。物標とは、道路や歩道に設けられた物体であり、例えば信号機や電柱、交通標識等である。また、道路構造とは、道路の構成物であり、例えば白線や黄色線等の道路の区画線や停止線の他に道路端部や縁石、中央分離帯等である。カメラ7は、検出したデータをデータベース9に出力する。
【0014】
また、走行軌跡推定装置1は、車両に搭載されたセンサ群(図示せず)に接続されている。例えば、アクセルセンサ、ステアリングセンサ、ブレーキセンサ、車速センサ等に接続されて、これらのセンサ群から出力されるセンサ値を取得することができる。
【0015】
データベース9は、走行データや環境情報、地図情報等を格納している。走行データは、GPS受信機3で検出された車両の位置情報と、IMU5で検出された車両の角度や加速度等と、IMU5で算出されたオドメトリを含んでいる。環境情報は、カメラ7で撮像されて検出された物標や道路構造等の情報を含んでいる。地図情報は、カーナビゲーション装置等に記憶されている情報であり、物標情報や施設情報などの経路案内に必要となる各種データを含んでいる。さらに、地図情報は、高精度な地図情報であり、道路や歩道に設けられた信号機や電柱等の物標情報や道路の車線数や道路境界線、停止線等の道路構造情報を含んでいる。データベース9は、走行軌跡推定装置1の要求に応じて、格納している情報を走行軌跡推定装置1へ出力する。また、データベース9は、走行軌跡の推定に必要となるその他の情報も記憶している。
【0016】
尚、データベース9は、サーバから定期的に最新の地図情報を入手して、保有する地図情報を更新する。本実施形態では、地図情報として第一地図(高精度地図、車両のルート案内に用いる地図)を備えており、第一地図を補足するものとして、第二地図を備えていてもよい。この第二地図は、第一地図だけでは不完全な場合や、より詳細な地図情報が必要な場合に、車両や外部のサーバで作成される。
【0017】
走行軌跡推定装置1は、車両が走行した走行軌跡を推定する。走行軌跡推定装置1は、車両が事前に走行して検出しておいた走行データをデータベース9から取得し、取得した走行データを用いて、過去に車両が走行した走行軌跡を推定する。推定された走行軌跡は、地図データを更新するために使用される。例えば、推定された走行軌跡を用いて物標の位置を計算し、この計算結果を用いて地図データに記録された物標の位置を更新する。
【0018】
また、走行軌跡推定装置1は、GPS受信機3、IMU5、カメラ7、データベース9から取得したデータを処理する制御部を備えており、例えばIC、LSI等によって構成される。走行軌跡推定装置1は、これを機能的に捉えた場合、走行軌跡算出部11と、地図照合部13と、走行軌跡補正部15と、走行軌跡調整部17に分類することができる。
【0019】
尚、走行軌跡推定装置1は、マイクロコンピュータ、マイクロプロセッサ、CPUを含む汎用の電子回路とメモリ等の周辺機器から構成されている。このような走行軌跡推定装置1の各機能は、1または複数の処理回路によって実装することができる。処理回路は、例えば電気回路を含む処理装置等のプログラムされた処理装置を含み、また実施形態に記載された機能を実行するようにアレンジされた特定用途向け集積回路(ASIC)や従来型の回路部品のような装置も含んでいる。
【0020】
走行軌跡算出部11は、車両が走行したときに検出した走行データをデータベース9から取得し、取得した走行データに基づいて、車両が走行した走行軌跡を算出する。具体的に、走行軌跡算出部11は、GPS受信機3で検出した車両位置を、単位時間当たりの移動量であるオドメトリの分だけ移動させていくことによって、車両の走行軌跡を算出する。ここで算出される走行軌跡は、時刻毎の車両位置と車両の姿勢との集合によって表される。車両の姿勢とは、車両の角度、すなわち車両の向きである。例えば、走行軌跡は、時刻ti(i=1〜n)における座標(x、y)と車両の向きθの集合によって表される。
【0021】
地図照合部13は、車両が走行したときに検出した車両周辺の環境情報と地図情報をデータベース9から取得し、取得した車両周辺の環境情報と、地図情報に記録された環境情報との差である環境情報誤差を算出する。例えば、地図照合部13は、取得した車両周辺の環境情報に含まれている白線の位置と、地図情報に記録されている白線の位置の差分を求め、この差分を環境情報誤差として算出する。
【0022】
また、地図照合部13は、車両周辺の環境情報が1つの車両位置に複数検出されている場合、地図情報に記録された環境情報に最も特性が近い車両周辺の環境情報を用いて、環境情報誤差を算出する。例えば、時刻tの車両位置において、車両周辺の環境情報として白線が複数検出され、地図情報には1本の白線しか記録されていない場合には、検出された複数の白線のうち、地図情報に記録された白線に最も特性が近い白線を用いて環境情報誤差を算出する。ここで、特性とは、白線の形状や白線が延びている方向、白線の位置、白線の太さ、色等である。したがって、車両周辺の環境情報として白線が複数検出されている場合に、地図照合部13は、例えば、地図情報に記録された白線の形状と最も近い形状の白線を用いて、環境情報誤差を算出する。また、地図情報に記録された白線の位置に最も近い位置にある白線を用いて、環境情報誤差を算出してもよい。
【0023】
さらに、地図照合部13は、車両周辺の環境情報として白線を検出しているが、地図情報には白線が記録されていない場合には、環境情報誤差を算出しないようにしてもよい。例えば、カメラ7では白線のように検出されている場合でも、地図情報にはゼブラゾーンや縁石しか記録されていない場合がある。このような場合には、誤検知の可能性があるので、環境情報誤差の算出を行わないようにする。
【0024】
走行軌跡補正部15は、走行軌跡算出部11で算出された走行軌跡を補正する。特に、走行軌跡補正部15は、走行軌跡上にある車両位置のうち、車両周辺の環境情報が検出された車両位置では、地図照合部13で算出された環境情報誤差に基づいて走行軌跡を補正する。
【0025】
例えば、図2に示すように、車両が、時刻t1に車両位置X1に位置し、時刻t2に車両位置X2に位置し、時刻t3に車両位置X3に位置している。この場合に、時刻t1では、車両周辺の環境情報として白線位置P1が複数検出されている。そして、地図情報には車両位置X1の近傍に白線L1が記録されている。したがって、車両周辺の環境情報と地図情報に記録された環境情報との差である環境情報誤差は、白線位置P1と白線L1の差分になるので、図2のd1となる。同様に、時刻t3では、車両周辺の環境情報として白線位置P3が複数検出され、地図情報には車両位置X3の近傍に白線L3が記録されているので、環境情報誤差はd3になる。
【0026】
そこで、走行軌跡補正部15は、時刻t1では、白線位置P1を白線L1の位置に合わせるように環境情報誤差d1だけ移動させ、この移動に同期させて車両位置X1も環境情報誤差d1だけ移動させる。この結果、図3に示すように、白線位置P1は白線L1上の位置に移動し、この移動に同期して車両位置X1も移動する。同様に、白線位置P3も白線L3上の位置に移動し、この移動に同期して車両位置X3も移動する。
【0027】
一方、走行軌跡補正部15は、車両周辺の環境情報が検出されなかった車両位置では、この車両位置の前後に隣接する車両位置に基づいて走行軌跡を補正する。例えば、図2に示すように、時刻t2では、車両位置X2の近傍に、車両周辺の環境情報として白線が検出されていない。このような状況は、例えば車両が交差点に進入すると、車両の両側にあった白線が途切れるために起こるものである。そこで、このように白線を検出できない場合に、走行軌跡補正部15は、車両位置X2の前後に隣接する車両位置X1、X3に基づいて走行軌跡を補正する。
【0028】
具体的に、走行軌跡補正部15は、前後に隣接する車両位置の間の移動量を示す相対変位と、車両位置におけるオドメトリとの差である変位誤差を算出し、この変位誤差に基づいて走行軌跡を補正する。
【0029】
例えば、図3に示すように、車両位置X3は環境情報誤差d3だけ移動しているので、車両位置X2におけるオドメトリB2の先端の位置からずれている。この場合に、走行軌跡補正部15は、前後に隣接する車両位置X2とX3の間の移動量を示す相対変位A2を算出し、この相対変位A2とオドメトリB2との差である変位誤差C2を算出する。相対変位とオドメトリは車両位置と車両の向き(角度)で表されるので、変位誤差は車両位置の差と車両の向き(角度)の差で表される。
【0030】
そして、走行軌跡補正部15は、車両位置X3の位置に近づけるように、変位誤差C2の分だけ車両位置X2を移動させる。これにより、車両位置X2は、図4に示すように車両位置X3と並ぶような位置に移動する。車両位置X3はすでに環境情報誤差d3だけ移動されているので、正確な位置である可能性が高い。したがって、車両位置X3の位置に近づけるように車両位置X2を移動させれば、車両位置X2も正確な位置に補正することが可能となる。
【0031】
同様に、走行軌跡補正部15は、図3に示すように、前後に隣接する車両位置X1とX2の間の移動量を示す相対変位A1を算出し、この相対変位A1と車両位置X1におけるオドメトリB1との差である変位誤差C1を算出する。
【0032】
そして、走行軌跡補正部15は、車両位置X1の位置に近づけるように、変位誤差C1の分だけ車両位置X2を移動させる。これにより。車両位置X2は、図4に示すように車両位置X1と並ぶような位置に移動する。車両位置X1はすでに環境情報誤差d1だけ移動されているので、正確な位置である可能性が高い。したがって、車両位置X1の位置に近づけるように車両位置X2を移動させれば、車両位置X2も正確な位置に補正することが可能となる。
【0033】
走行軌跡調整部17は、走行軌跡補正部15で補正された走行軌跡の誤差が小さくなるように調整し、調整された走行軌跡を、走行軌跡の推定結果として出力する。具体的に、走行軌跡調整部17は、地図照合部13によって算出された環境情報誤差と、走行軌跡補正部15によって算出された変位誤差をすべて加算して誤差総和を算出し、この誤差総和が小さくなるように走行軌跡を調整する。
【0034】
例えば、走行軌跡調整部17は、ガウスニュートン法やLM法等の最適化手法を使用して、車両位置Xi(i=1〜n)を移動させながら繰り返し計算の中で徐々に誤差総和が小さくなるように走行軌跡を調整する。そして、誤差総和が予め設定された所定値以下になると、走行軌跡調整部17は、走行軌跡の調整が収束したと判定して、調整された走行軌跡を、走行軌跡の推定結果として出力する。
【0035】
[走行軌跡の推定処理]
次に、図5を参照して、本実施形態に係る走行軌跡推定装置1による走行軌跡の推定処理を説明する。図5は、走行軌跡の推定処理を示すフローチャートである。
【0036】
図5に示すように、ステップS1において、走行軌跡算出部11は、車両が走行したときに検出した走行データをデータベース9から取得する。また、地図照合部13は、車両が走行したときに検出した環境情報と地図情報をデータベース9から取得する。
【0037】
ステップS3において、走行軌跡算出部11は、ステップS1で取得した走行データに基づいて、車両が走行した走行軌跡を算出する。ここで算出される走行軌跡は、車両が過去に走行した走行軌跡であり、車両位置をオドメトリの分だけ移動させていくことによって走行軌跡を算出する。走行軌跡は、時刻毎の車両位置と車両の姿勢との集合によって表され、具体的に時刻ti(i=1〜n)における座標(x、y)と車両の向きθの集合によって表される。例えば、図2に示すように、走行軌跡は、時刻t1の車両位置X1、時刻t2の車両位置X2、時刻t3の車両位置X3の集合によって表されている。ただし、車両位置Xnには、車両の向きθnが含まれている。
【0038】
ステップS5において、地図照合部13は、走行軌跡上にある車両位置の近傍に環境情報が検出されているか否かを判定する。本実施形態では、環境情報の一例として、白線を用いる場合について説明するので、走行軌跡上にある車両位置の近傍に白線が検出されているか否かを判定する。そして、白線が検出されている場合にはステップS7に進み、白線が検出されていない場合にはステップS11に進む。例えば、図2に示すように、車両位置X1、X3では白線位置P1、P3が検出されているのでステップS7に進み、車両位置X2では白線が検出されていないのでステップS11に進む。
【0039】
地図照合部13は、まず時刻t1の車両位置X1の近傍に白線が検出されているか否かを判断してステップS7〜11の処理を行い、その後は順番に車両位置X2から車両位置XnまでステップS7〜S11の処理を行う。
【0040】
また、地図照合部13は、車両周辺の環境情報として白線が検出されていても、地図情報に白線が記録されていない場合には、環境情報が検出されていないと判定し、環境情報誤差を算出しないようにしてもよい。例えば、カメラ7では白線のように検出されていても、地図情報にはゼブラゾーンや縁石しか記録されていない場合がある。このような場合には、誤検知の可能性があるので、白線を検出していないと判定する。
【0041】
ステップS7において、地図照合部13は、車両周辺の環境情報と、地図情報に記録された環境情報との差である環境情報誤差を算出する。具体的に、地図照合部13は、取得した車両周辺の環境情報に含まれている白線の位置を地図座標系に変換し、地図座標系に変換された白線の位置と、地図情報に記録されている白線の位置との差を求め、この差を環境情報誤差として算出する。例えば、図2の車両位置X1では、白線位置P1と地図情報に記録された白線L1の差d1を環境情報誤差として出力する。同様に、時刻t3では、白線位置P3と地図情報に記録された白線L3の差d3を環境情報誤差として出力する。
【0042】
また、地図照合部13は、車両周辺の環境情報が1つの車両位置に複数検出されている場合、地図情報に記録された環境情報に最も特性が近い車両周辺の環境情報を用いて、環境情報誤差を算出する。例えば、時刻tの車両位置において、車両周辺の環境情報として白線が複数検出され、地図情報には1本の白線しか記録されていない場合には、検出された複数の白線のうち、地図情報に記録された白線に最も特性が近い白線を用いて環境情報誤差を算出する。
【0043】
ステップS9において、走行軌跡補正部15は、車両周辺の環境情報が検出された車両位置について、ステップS7で算出された環境情報誤差に基づいて走行軌跡を補正する。例えば、図2に示すように、走行軌跡補正部15は、車両周辺の環境情報として白線が検出された車両位置X1、X3について、環境情報誤差d1、d3だけ車両位置X1、X3を移動させて、図3に示すように走行軌跡を補正する。
【0044】
ステップS11において、地図照合部13は、走行軌跡上のすべての車両位置についてステップS5〜11の処理が行われたか否かを判定する。そして、ステップS5〜11の処理が行われていない車両位置がある場合にはステップS5に戻り、すべての車両位置についてステップS5〜11の処理が行われている場合にはステップS13に進む。ステップS13に進む場合には、白線が検出されたX1、X3のような車両位置では、図3に示すように車両位置がすべて移動されており、白線が検出されなかったX2のような車両位置では、図3に示すように車両位置が移動されていないままとなっている。
【0045】
ステップS13において、走行軌跡補正部15は、車両周辺の環境情報が検出されなかった車両位置について、前後に隣接する車両位置の間の移動量を示す相対変位と、車両位置におけるオドメトリとの差である変位誤差を算出する。例えば、図3に示すように、白線が検出されなかった車両位置X2について、前後に隣接する車両位置X2とX3の間の移動量を示す相対変位A2を算出し、この相対変位A2と車両位置X2におけるオドメトリB2との差である変位誤差C2を算出する。同様に、走行軌跡補正部15は、変位誤差C1を算出する。
【0046】
ステップS15において、走行軌跡補正部15は、車両周辺の環境情報が検出されなかった車両位置について、ステップS13で算出された変位誤差に基づいて走行軌跡を補正する。走行軌跡補正部15は、図3に示すように、車両周辺の環境情報として白線が検出されなかった車両位置X2について、変位誤差C1、C2だけ車両位置X2を移動させることで、図4に示すように走行軌跡を補正する。
【0047】
ステップS17において、走行軌跡補正部15は、車両周辺の環境情報が検出されなかった車両位置について、車両位置の補正が終了したか否かを判定する。図3の車両位置X2のように白線が検出されなかったすべての車両位置について、走行軌跡補正部15は、車両位置の補正が終了したか否かを判定する。そして、車両位置の補正が終了していない場合にはステップS13に戻り、終了している場合にはステップS19に進む。
【0048】
ステップS19において、走行軌跡調整部17は、ステップS7で算出された環境情報誤差と、ステップS13で算出された変位誤差をすべて加算して誤差総和を算出する。この誤差総和は、車両位置X1から車両位置Xnまでのすべての車両位置の環境情報誤差と変位誤差を加算したものである。
【0049】
ステップS21において、走行軌跡調整部17は、ステップS19で算出された誤差総和が小さくなるように走行軌跡を調整し、収束したか否かを判定する。走行軌跡調整部17は、ガウスニュートン法やLM法等の最適化手法を使用して、車両位置Xi(i=1〜n)を移動させながら繰り返し計算の中で徐々に誤差総和が小さくなるように走行軌跡を調整する。そして、誤差総和が予め設定された所定値以下とならない場合には走行軌跡の調整が収束しないと判定してステップS3に戻る。一方、誤差総和が予め設定された所定値以下になると、走行軌跡の調整が収束したと判定し、調整された走行軌跡を、走行軌跡の推定結果として出力し、本実施形態に係る走行軌跡の推定処理を終了する。
【0050】
[実施形態の効果]
以上詳細に説明したように、本実施形態に係る走行軌跡推定装置1では、走行データに基づいて車両が走行した走行軌跡を算出し、車両周辺の環境情報と地図情報に記録された環境情報との差である環境情報誤差を算出する。そして、走行軌跡上にある車両位置のうち、車両周辺の環境情報が検出された車両位置では環境情報誤差に基づいて走行軌跡を補正し、車両周辺の環境情報が検出されなかった車両位置では、この車両位置の前後に隣接する車両位置に基づいて走行軌跡を補正する。これにより、白線等の車両周辺にある環境情報が検出できた場合だけでなく、車両周辺の環境情報が検出できない場合でも、走行軌跡を補正することができるので、車両の走行軌跡を精度よく推定することができる。
【0051】
また、本実施形態に係る走行軌跡推定装置1では、時刻毎の車両位置と車両の姿勢との集合によって、走行軌跡を表している。これにより、走行軌跡を時刻毎に正確に表すことができるので、車両の走行軌跡を精度よく推定することができる。
【0052】
さらに、本実施形態に係る走行軌跡推定装置1では、車両周辺の環境情報が1つの車両位置に複数検出されている場合に、地図情報に記録された環境情報に最も特性が近い車両周辺の環境情報を用いて、環境情報誤差を算出する。これにより、特性が近くて信頼性の高い環境情報を用いて環境情報誤差を算出できるので、車両の走行軌跡を精度よく推定することができる。
【0053】
また、本実施形態に係る走行軌跡推定装置1では、車両周辺の環境情報が検出されなかった車両位置について、前後に隣接する車両位置の間の移動量を示す相対変位と、車両位置におけるオドメトリとの差である変位誤差を算出する。そして、この変位誤差に基づいて走行軌跡を補正する。これにより、白線等の車両周辺にある環境情報が検出できない場合でも、前後に隣接する車両位置に基づいて走行軌跡を補正できるので、車両の走行軌跡を精度よく推定することができる。
【0054】
さらに、本実施形態に係る走行軌跡推定装置1では、環境情報誤差と変位誤差の誤差総和を算出し、この誤差総和が小さくなるように、補正された走行軌跡を調整する。これにより、走行軌跡全体の整合性を取ることができるので、推定された走行軌跡の精度を向上させることができる。
【0055】
尚、上述の実施形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計などに応じて種々の変更が可能であることは勿論である。
【符号の説明】
【0056】
1 走行軌跡推定装置
3 GPS受信機
5 IMU(慣性計測装置)
7 カメラ
9 データベース
11 走行軌跡算出部
13 地図照合部
15 走行軌跡補正部
17 走行軌跡調整部
100 走行軌跡推定システム
図1
図2
図3
図4
図5