(57)【要約】【課題】走行軌跡や物標の位置に誤差がある場合でも自車両の自己位置を精度よく推定する。【解決手段】本発明の自己位置推定方法は、走行軌跡モデルと物標モデルを用いて自車両の自己位置を推定する方法であり、走行軌跡モデルの誤差と物標モデルの誤差を算出し、走行軌跡モデルの誤差と物標モデルの誤差の大きさに応じて、走行軌跡モデルから出力された算出結果と物標モデルから出力された算出結果のどちらを用いて、物標位置の推定を開始するか決定する。【選択図】図１０
2019190994.tif 000002
【特許請求の範囲】
【請求項１】
自車両のオドメトリにより得られた走行軌跡データを入力し、前記オドメトリにより得られた走行軌跡データを予め定めたモデルにより調整し、調整した結果を自車両のトラジェクトリデータとして出力する走行軌跡モデルと、
自車両のセンサにより得られた物標データを入力し、前記センサにより得られた物標データを予め定めたモデルにより調整し、調整した結果を自車両の周囲の物標データとして出力する物標モデルと
を用いて自車両の周囲の物標位置を推定し、前記物標位置と地図情報とを照合することによって自車両の自己位置を推定する自己位置推定装置の自己位置推定方法において、
前記走行軌跡モデルの誤差と前記物標モデルの誤差を算出し、
前記走行軌跡モデルの誤差と前記物標モデルの誤差の大きさに応じて、前記走行軌跡モデルから出力された算出結果と前記物標モデルから出力された算出結果のどちらを用いて、前記物標位置の推定を開始するか決定することを特徴とする自己位置推定方法。
【請求項２】
前記走行軌跡モデルと前記物標モデルのうち誤差の小さいほうのモデルから出力された算出結果を、前記走行軌跡モデルと前記物標モデルのもう一方の入力として前記物標位置の推定を開始することを特徴とする請求項１に記載の自己位置推定方法。
【請求項３】
前記走行軌跡モデルの誤差と前記物標モデルの誤差に対して、それぞれ所定の閾値を設定し、
前記走行軌跡モデルの誤差と前記物標モデルの誤差が前記所定の閾値より大きいか否かによって、前記走行軌跡モデルから出力された算出結果と前記物標モデルから出力された算出結果のどちらを用いて、前記物標位置の推定を開始するか決定することを特徴とする請求項１に記載の自己位置推定方法。
【請求項４】
前記物標モデルの誤差が前記所定の閾値より大きい場合には、前記走行軌跡モデルから出力された算出結果を、前記物標モデルの入力として前記物標位置の推定を開始することを特徴とする請求項３に記載の自己位置推定方法。
【請求項５】
前記走行軌跡モデルの誤差が前記所定の閾値より大きい場合には、前記物標モデルから出力された算出結果を、前記走行軌跡モデルの入力として前記物標位置の推定を開始することを特徴とする請求項３または４に記載の自己位置推定方法。
【請求項６】
前記走行軌跡モデルの誤差と前記物標モデルの誤差が前記所定の閾値より大きい場合には、前記走行軌跡モデルから出力された算出結果を、前記物標モデルの入力として物標位置の推定を開始することを特徴とする請求項３〜５のいずれか１項に記載の自己位置推定方法。
【請求項７】
自車両の車速が所定値未満である場合には、前記物標モデルから出力された算出結果を、前記走行軌跡モデルの入力として物標位置の推定を開始することを特徴とする請求項３〜６のいずれか１項に記載の自己位置推定方法。
【請求項８】
自車両のオドメトリにより得られた走行軌跡データを入力し、前記オドメトリにより得られた走行軌跡データを予め定めたモデルにより調整し、調整した結果を自車両のトラジェクトリデータとして出力する走行軌跡モデルと、
自車両のセンサにより得られた物標データを入力し、前記センサにより得られた物標データを予め定めたモデルにより調整し、調整した結果を自車両の周囲の物標データとして出力する物標モデルと
を用いて自車両の周囲の物標位置を推定し、前記物標位置と地図情報とを照合することによって自車両の自己位置を推定する自己位置推定装置において、
前記自己位置推定装置は、
前記走行軌跡モデルの誤差と前記物標モデルの誤差を算出し、
前記走行軌跡モデルの誤差と前記物標モデルの誤差の大きさに応じて、前記走行軌跡モデルから出力された算出結果と前記物標モデルから出力された算出結果のどちらを用いて、前記物標位置の推定を開始するか決定することを特徴とする自己位置推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、走行軌跡モデルと物標モデルとを用いて自車両の自己位置を推定する自己位置推定方法及び自己位置推定装置に関する。
【背景技術】
【０００２】
従来より、カーナビゲーション装置などに格納される地図データを補正する方法が知られている。特許文献１に記載された発明は、ＧＰＳを用いた走行軌跡データと、地図データとを比較し、走行軌跡データに対する地図データのずれを補正する。これにより、地図上の物標の位置などが補正され、補正された物標の位置を用いて、車両の自己位置を算出している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−３１０１９８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ここで、上述した従来の装置では、走行軌跡データに対して地図データを補正しており、走行軌跡データに誤差がないことを前提としている。しかしながら、実際には走行軌跡データにも誤差が含まれているため、自車両の自己位置を精度よく推定することができないという問題点があった。
【０００５】
そこで、本発明は、上記課題に鑑みて成されたものであり、自車両の自己位置を精度よく推定することのできる自己位置推定方法及び自己位置推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明の一態様に係る自己位置推定方法及びその装置は、走行軌跡モデルの誤差と物標モデルの誤差を算出する。そして、走行軌跡モデルの誤差と物標モデルの誤差の大きさに応じて、走行軌跡モデルから出力された算出結果と物標モデルから出力された算出結果のどちらを用いて、自車の周囲の物標情報の推定を開始するか決定する。
【発明の効果】
【０００７】
本発明によれば、自車両の自己位置を精度よく推定することができる。
【図面の簡単な説明】
【０００８】
【図１】図１は、本発明の第１実施形態に係る自己位置推定装置の構成を示すブロック図である。
【図２】図２は、本発明の第１実施形態に係る自己位置推定装置で用いる走行軌跡モデルを説明するための図である。
【図３】図３は、本発明の第１実施形態に係る自己位置推定装置で用いる物標モデルを説明するための図である。
【図４】図４は、本発明の第１実施形態に係る自己位置推定装置によるトラジェクトリの補正処理の処理手順を示すフローチャートである。
【図５】図５は、本発明の第１実施形態に係る自己位置推定装置によるトラジェクトリの補正方法を説明するための図である。
【図６】図６は、本発明の第１実施形態に係る自己位置推定装置によるトラジェクトリの補正方法を説明するための図である。
【図７】図７は、本発明の第１実施形態に係る自己位置推定装置による物標位置の補正処理の処理手順を示すフローチャートである。
【図８】図８は、本発明の第１実施形態に係る自己位置推定装置による物標位置の補正方法を説明するための図である。
【図９】図９は、本発明の第１実施形態に係る自己位置推定装置による物標位置の補正方法を説明するための図である。
【図１０】図１０は、本発明の第１実施形態に係る自己位置推定装置による自己位置推定処理の処理手順を示すフローチャートである。
【図１１】図１１は、従来の地図情報に記録された白線の誤差を説明するための図である。
【図１２】図１２は、本発明の第２実施形態に係る自己位置推定装置による自己位置の推定を開始するときに用いるモデルの決定方法を説明するための図である。
【図１３】図１３は、本発明の第２実施形態に係る自己位置推定装置による自己位置推定処理の処理手順を示すフローチャートである。
【発明を実施するための形態】
【０００９】
［第１実施形態］
以下、本発明の第１実施形態について、図面を参照して説明する。図面の記載において同一部分には同一符号を付して説明を省略する。
【００１０】
［自己位置推定装置の構成］
図１を参照して、本実施形態に係る自己位置推定装置の構成を説明する。図１に示すように、自己位置推定装置１は、カメラ２と、レーザレンジファインダー３と、ＧＰＳ受信機４と、ジャイロセンサ５と、地図データベース６と、コントローラ１０と、通信機７を備える。また、自己位置推定装置１は車両に搭載されており、搭載される車両は自動運転機能を備え、走行支援を行う車両である。
【００１１】
カメラ２は、ＣＣＤ（charge-coupled device）やＣＭＯＳ（complementary metal oxide semiconductor）などの撮像素子を有する。カメラ２は、自車両に搭載され、自車両の周囲を撮影する。カメラ２は、画像処理機能を有しており、撮影した画像から物標などを検出する。物標とは、道路や歩道に設けられた物体であり、例えば信号機や電柱、交通標識等である。また、物標には道路構造が含まれている。道路構造とは、道路の構成物であり、例えば白線や黄色線等の道路の区画線や停止線の他に道路端部や縁石、中央分離帯等である。カメラ２は、検出したデータをコントローラ１０に出力する。尚、物標は、静止している物体であればよく、例えば、建物、建物の特徴部位などでもよい。
【００１２】
レーザレンジファインダー３は、自車両の周囲の物標を検出するセンサである。具体的に、レーザレンジファインダー３は、レーザ光をある角度範囲内で走査し、その時の反射光を受光して、レーザ発射時点と反射光の受光時点との間の時間差を検出する。レーザレンジファインダー３は、自車両に対する物標の相対距離や方向などを検出し、検出したデータをコントローラ１０に出力する。レーザレンジファインダー３は、ボンネット、バンパー、ナンバープレート、ヘッドライト、サイドミラー等の周辺に設置される。
【００１３】
ＧＰＳ受信機４は、人工衛星からの電波を受信することにより、地上における自車両の現在地を検出する。ＧＰＳ受信機４は、検出したデータをコントローラ１０に出力する。
【００１４】
ジャイロセンサ５は、自車両の重心の鉛直軸周りのヨーレート（回転角速度）を検出し、検出したデータをコントローラ１０に出力する。
【００１５】
また、自己位置推定装置１は、車両に搭載されたセンサ群（図示せず）に接続されている。例えば、アクセルセンサ、ステアリングセンサ、ブレーキセンサ、車速センサ、加速度センサ、車輪速センサ等に接続されて、これらのセンサ群から出力されるセンサ値を取得することができる。
【００１６】
地図データベース６は、カーナビゲーション装置等に記憶されているデータベースであり、物標情報や施設情報など経路案内に必要となる各種データを記憶している。地図データベース６は、高精度な地図であり、道路の車線数、道路境界線等の道路構造情報を含む物標情報を記憶している。地図データベース６は、コントローラ１０の要求に応じて地図情報をコントローラ１０に出力する。また、物標データや走行軌跡データ、トラジェクトリデータ等の情報も記憶している。
【００１７】
尚、物標データ等の各種データは必ずしも地図データベース６から取得するものに限定する必要はなく、自車両が備えるセンサによって取得したものでもよい。また、車車間通信、路車間通信を用いて取得してもよい。例えば、物標データなどの各種データがサーバ２５に記憶されている場合には、コントローラ１０は、通信により随時これらのデータを取得することができる。また、コントローラ１０は、サーバ２５から定期的に最新の地図情報を入手して、保有する地図情報を更新することができる。尚、本実施形態では、地図データベース６として第一地図（高精度地図、車両のルート案内に用いる地図）を備えており、第一地図を補足するものとして、第二地図を備えていてもよい。この第二地図は、第一地図だけでは不完全な場合や、より詳細な地図情報が必要な場合に、車両や外部のサーバ２５で作成される。
【００１８】
コントローラ１０は、カメラ２、レーザレンジファインダー３、ＧＰＳ受信機４、ジャイロセンサ５、地図データベース６から取得したデータを処理する回路であり、例えばＩＣ、ＬＳＩ等によって構成される。コントローラ１０は、これを機能的に捉えた場合、走行軌跡モデル作成部１１と、物標モデル作成部１２と、トラジェクトリ算出部１３と、物標位置算出部１４と、トラジェクトリ補正部１５に分類することができる。さらに、コントローラ１０は、物標算出部１６と、物標位置補正部１７と、モデル誤差判定部１８と、マッチング部１９と、自己位置推定部２０に分類することができる。
【００１９】
尚、コントローラ１０は、マイクロコンピュータ、マイクロプロセッサ、ＣＰＵを含む汎用の電子回路とメモリ等の周辺機器から構成されている。そして、特定のプログラムを実行することにより、自己位置推定装置１として動作する。このようなコントローラ１０の各機能は、１または複数の処理回路によって実装することができる。処理回路は、例えば電気回路を含む処理装置等のプログラムされた処理装置を含み、また実施形態に記載された機能を実行するようにアレンジされた特定用途向け集積回路（ＡＳＩＣ）や従来型の回路部品のような装置も含んでいる。
【００２０】
走行軌跡モデル作成部１１は、自車両の走行データを用いて走行軌跡モデルを作成する。走行軌跡モデルは、自車両のオドメトリにより得られた走行軌跡データが入力されると、オドメトリにより得られた走行軌跡データを予め定めたモデルにより調整し、調整した結果を自車両のトラジェクトリデータとして出力する。オドメトリは、車両の単位時間当たりの移動量であり、車両に搭載されたセンサ群から得られる各種センサ値を用いて算出することができる。また、走行軌跡モデルは、図２に示すように、所定区間（図２に示す１点鎖線で囲まれた区間）における走行軌跡上の複数の点Ｘｉ（ｉ＝１〜ｎ）で構成される。各点Ｘｉは、座標（ｘ，ｙ）と車両の向きθ（進行方向）とから成る。
【００２１】
走行軌跡モデル作成部１１は、自車両の走行軌跡から座標（ｘ，ｙ）と車両の向きθを多数取得し、取得した走行データを用いて各点Ｘｉの走行軌跡を学習してモデル化し、走行軌跡が再現可能となるように走行軌跡モデルを作成する。また、走行軌跡モデルは、自車両の走行支援を実行する前に自車両の走行軌跡を学習して生成される。尚、所定区間は、所定の走行時間や走行シーンで区切られる区間である。走行時間は、例えば３〜４秒である。また、走行シーンは、例えば交差点の入口から出口までのシーン、右折または左折のシーンである。このように走行時間または走行シーンで区切る理由は、走行時間が長い場合や走行シーンが複雑な場合には、様々なデータが入り込むため走行軌跡モデルの精度が低下するおそれがあるためである。
【００２２】
走行軌跡モデル作成部１１は、所定の走行時間や走行シーンで区切られた区間の走行軌跡モデルを作成することで、高精度な走行軌跡モデルを生成することができる。尚、走行軌跡モデル作成部１１は、必ずしも自車両の走行データを用いる必要はなく、サーバ２５に集められた自車両以外の他車両の走行データを用いてもよく、走行データを使わずに予め作成されたモデルを用いてもよい。また、走行軌跡モデル作成部１１は、自車両の走行支援を実行する前にモデルを作成してもよく、また、自車両の走行支援を実行している過程で得られた走行データを用いてモデルを作成してもよい。さらに、走行軌跡モデル作成部１１は、走行している過程で得られた走行データを用いてリアルタイムにモデルを更新してもよい。
【００２３】
具体的に、走行軌跡モデル作成部１１は、取得したデータから所定区間の各点Ｘｉ毎の平均位置を求める。次に、走行軌跡モデル作成部１１は、取得した走行データから分散共分散行列を計算し、主成分ベクトルを求める。このとき、走行軌跡モデル作成部１１は、主成分ベクトルの本数として走行軌跡を十分に表現できるだけの本数を求める。次に、走行軌跡モデル作成部１１は、主成分ベクトルを列に並べ、行列Ａを作成する。行列Ａは、軌跡制御係数である。走行軌跡モデル作成部１１は、モデル各点の平均位置と、行列Ａ及び行列Ａの制御パラメータであるqとを足し合わせて走行軌跡モデルＸを作成する。走行軌跡モデルＸは、式（１）、（２）を用いて次のように表される。
【００２４】
【数１】
2019190994.tif 000003
【数２】
2019190994.tif 000004
【００２５】
走行軌跡モデル作成部１１は、作成した走行軌跡モデルＸをサーバ２５に保存してもよいし、自車両に保存してもよい。尚、走行軌跡モデルＸは、一つだけ生成してもよいし、複数生成してもよい。また、所定区間は、５秒間、１０秒間などの時間で区切ってもよいし、動作で区切ってもよい。動作で区切る場合の例としては、交差点に進入してから出るまでを１つの区間とする。尚、走行軌跡モデル作成部１１は、車車間通信を使って他車両に走行軌跡モデルＸを保存してもよい。
【００２６】
物標モデル作成部１２は、自車両の周囲の物標を検出した物標データを用いて物標モデルを作成する。物標モデルは、自車両のセンサにより得られた物標に関する物標データが入力されると、センサにより得られた物標データを予め定めたモデルにより調整し、調整した結果を自車両の周囲の物標データとして出力する。また、物標モデルは、図３に示すように、所定区間（図３に示す１点鎖線で囲まれた区間）における白線上の複数の点Ｌｉ（ｉ＝１〜ｎ）で構成される。尚、図３では物標の一例として白線を用いている。各点Ｌｉは、座標（ｘ，ｙ）と白線の方向φとから成る。
【００２７】
物標モデル作成部１２は、物標データから座標（ｘ，ｙ）と白線の方向φを多数取得し、取得したデータを用いて各点Ｌｉの白線を学習してモデル化し、白線が再現可能となるように物標モデルを作成する。また、物標モデルは、自車両の走行支援を実行する前に自車両周囲の物標を学習して生成される。尚、点Ｌｉ＝（ｘ，ｙ）は、一定区間の白線を、例えば１０ｃｍ間隔で分割したｉ番目の白線の位置である。
【００２８】
具体的に、物標モデル作成部１２は、取得したデータから所定区間の各点Ｌｉ毎の平均位置を求める。次に、物標モデル作成部１２は、取得したデータから分散共分散行列を計算し、主成分ベクトルを求める。このとき、物標モデル作成部１２は、主成分ベクトルの本数として物標を十分に表現できるだけの本数を求める。次に、物標モデル作成部１２は、主成分ベクトルを列に並べ、行列Ｂを作成する。行列Ｂは、軌跡制御係数である。物標モデル作成部１２は、モデル各点の平均位置と、行列Ｂ及び行列Ｂの制御パラメータであるｐとを足し合わせて物標モデルＬを作成する。物標モデルＬは、式（１）、（２）の走行軌跡モデルと同様に、式（３）、（４）を用いて次のように表される。
【００２９】
【数３】
2019190994.tif 000005
【数４】
2019190994.tif 000006
【００３０】
物標モデル作成部１２は、作成した物標モデルＬをサーバ２５に保存してもよいし、自車両に保存してもよい。尚、物標モデルＬは、一つだけ生成してもよく、複数生成してもよい。また、所定区間は、５秒間、１０秒間などの時間で区切ってもよい。尚、物標モデル作成部１２は、車車間通信を使って他車両に物標モデルＬを保存してもよい。また、物標モデル作成部１２は、自車両の走行支援を実行する前にモデルを作成してもよく、また、自車両の走行支援を実行している過程で得られた物標データを用いてモデルを作成してもよい。さらに、物標モデル作成部１２は、走行している過程で得られた物標データを用いてリアルタイムにモデルを更新してもよい。
【００３１】
ここで、車両は道路上の白線に沿って走行するので、走行軌跡と白線の形状との間には相関があり、近似したものとなる。したがって、走行軌跡モデルと物標モデルとの間にも相関があり、近似したものとなる。そのため、走行軌跡モデルの制御係数Ａと物標モデルの制御係数Ｂとの間には相関があり、走行軌跡モデルの制御パラメータｑと物標モデルの制御パラメータｐとの間にも相関がある。
【００３２】
トラジェクトリ算出部１３は、サーバ２５に記憶されている走行軌跡モデルＸを用いてトラジェクトリデータを算出する。トラジェクトリとは、走行軌跡モデルＸを用いて算出される自車両の走行軌跡である。トラジェクトリ算出部１３は、車両の向きθと制御パラメータｑを所定値（例えば、０）で初期化し、走行軌跡モデルＸに入力してトラジェクトリを算出する。また、制御パラメータｑについては、オドメトリの軌跡から逆算した値で初期化してもよい。自車両の走行軌跡は、オドメトリを使用して取得することも可能である。オドメトリとはタイヤの回転角と回転角速度に応じて移動体の移動距離と方向を求める方法である。しかし、オドメトリを使用した場合、タイヤスリップなどによる誤差を回避できない可能性があり、走行軌跡の推定精度が低下するおそれがある。本実施形態では走行軌跡モデルＸを用いたトラジェクトリを使用するので、走行軌跡の推定精度が低下することを抑制することができる。
【００３３】
尚、トラジェクトリ算出部１３は、物標を取得する前のトラジェクトリについて、オドメトリを用いて算出してもよい。すなわち、自車両のオドメトリにより得られた走行軌跡データに基づいてトラジェクトリデータを算出してもよい。また、トラジェクトリ算出部１３は、オドメトリから算出したトラジェクトリを用いて物標を取得してもよく、物標を取得した後のトラジェクトリを、走行軌跡モデルを用いて算出してもよい。
【００３４】
物標位置算出部１４は、トラジェクトリ算出部１３によって算出されたトラジェクトリを用いて地図上における物標の位置を算出する。物標位置算出部１４は、トラジェクトリ上の各点から物標までの距離を取得し、この距離を取得した時の自車両の向きθを取得する。この各点とは、複数の位置であればよく、同じ位置であっても複数回、物標までの距離及び車両の向きを取得できればよい。物標位置算出部１４は、物標までの距離、及び車両の向きθを用いて物標の位置を算出する。
【００３５】
トラジェクトリ補正部１５は、物標位置算出部１４によって算出された物標の位置と地図情報に記録されている物標の位置の差分を求め、この差分を走行軌跡モデルの誤差として推定する。そして、トラジェクトリ補正部１５は、物標位置算出部１４によって算出された物標の位置と地図情報に記録されている物標の位置が一致するようにトラジェクトリを補正する。例えば、ガウスニュートン法等の最適化手法を使用して、車両の向きθと制御パラメータｑを調整しながら繰り返し計算の中で徐々に真値に近づけていく方法を用いて、トラジェクトリを補正する。
【００３６】
トラジェクトリ補正部１５は、算出された物標の位置から、各点で取得した物標までの距離及び車両の向きを拘束条件として与え、走行軌跡モデルＸを用いて、トラジェクトリを補正する。これにより、物標までの距離及び車両の向きを取得した各点の位置に誤差が生じていたとしても（例えば、ＧＰＳ誤差やオドメトリ誤差）、トラジェクトリ補正部１５はトラジェクトリを正確に算出することができる。そのため、ひいては車両の自己位置を正確に算出できるようになる。さらに、トラジェクトリ補正部１５は、この処理を繰り返すことによって、さらにトラジェクトリを正確に算出することができる。
【００３７】
また、トラジェクトリ補正部１５は、トラジェクトリ上の各点から算出した物標の位置の分散を求め、分散が閾値よりも大きい場合に車両の向きθと制御パラメータｑを調整してトラジェクトリを補正してもよい。分散とは、散らばり具合を示す指標であり、一般的な手法を用いて求めることができる。分散が閾値より大きい場合には、平均値から遠く離れたデータが多く、散らばりが大きいことを意味する。一方、分散が閾値以下の場合には、平均値に近いデータが多く、散らばりが小さいことを意味する。換言すれば、分散が閾値以下の場合、物標の位置を精度よく推定できたことを意味する。一方、分散が閾値より大きい場合には、物標の位置を精度よく推定できていないことを意味する。尚、閾値は、実験やシミュレーションを通じて求めてもよく、また任意に設定してもよい。そして、トラジェクトリ補正部１５は、分散が閾値以下になるまで車両の向きθと制御パラメータｑを繰り返し調整してトラジェクトリを補正する。
【００３８】
そして、トラジェクトリ補正部１５は、上述したトラジェクトリの補正を行った結果、算出された物標の位置と、地図情報に記録された物標の位置が一致したと判定すると、トラジェクトリの補正を終了する。一致したか否かの判定方法としては、ガウスニュートン法等の最適化手法を用いる場合には、収束して真値に近づいたと判定した場合に、算出された物標の位置と地図情報に記録された物標の位置が一致したと判定する。また、分散を用いる場合には、分散が閾値以下となった場合に、算出された物標の位置と地図情報に記録された物標の位置が一致したと判定する。
【００３９】
物標算出部１６は、サーバ２５に記憶されている物標モデルＬを用いて道路構造を含む物標を算出する。物標算出部１６は、車両の向きθと制御パラメータｐを所定値（例えば、０）で初期化し、物標モデルＬに入力して物標を算出する。また、物標算出部１６は、自車両のセンサにより得られた物標データに基づいて自車両の周囲の物標データを算出してもよい。そして、物標算出部１６は、算出された物標データを、自車両のオドメトリにより得られた走行軌跡データを基準にして地図上に配置して、物標の位置を算出する。
【００４０】
物標位置補正部１７は、物標算出部１６によって算出された物標の位置と地図情報に記録されている物標の位置の差分を求め、この差分を物標モデルの誤差として推定する。そして、物標位置補正部１７は、物標算出部１６によって算出された物標の位置と地図情報に記録されている物標の位置が一致するように、物標データを補正する。例えば、ガウスニュートン法等の最適化手法を使用して、車両の向きθと制御パラメータｐを調整しながら繰り返し計算の中で徐々に真値に近づけていく方法を用いて、物標データを補正する。物標位置補正部１７は、トラジェクトリを拘束条件として与え、物標モデルＬを用いて、物標データを補正する。
【００４１】
また、物標位置補正部１７は、物標の位置の分散を求め、分散が閾値よりも大きい場合に制御パラメータｐを調整して物標の位置を補正してもよい。なお、閾値は、実験やシミュレーションを通じて求めてもよく、また任意に設定してもよい。そして、物標位置補正部１７は、分散が閾値以下になるまで制御パラメータｐを繰り返し調整して物標の位置を補正する。また、車両の向きθと制御パラメータｐの両方を調整して物標の位置を補正してもよい。
【００４２】
そして、物標位置補正部１７は、上述した物標の位置の補正を行った結果、算出された物標の位置と、地図情報に記録された物標の位置が一致したと判定すると、物標の位置の補正を終了する。一致したか否かの判定方法としては、ガウスニュートン法等の最適化手法を用いる場合には、収束して真値に近づいたと判定した場合に、算出された物標の位置と地図情報に記録された物標の位置が一致したと判定する。また、分散を用いる場合には、分散が閾値以下となった場合に、算出された物標の位置と地図情報に記録された物標の位置が一致したと判定する。
【００４３】
モデル誤差判定部１８は、走行軌跡モデルの誤差と物標モデルの誤差の大きさに応じて、走行軌跡モデルから出力された算出結果と物標モデルから出力された算出結果のどちらを用いて、物標位置の推定を開始するか決定する。特に、本実施形態では、走行軌跡モデルと物標モデルのうち誤差の小さいほうのモデルから出力された算出結果を、走行軌跡モデルと物標モデルのもう一方の入力として物標位置の推定を開始する。
【００４４】
例えば、モデル誤差判定部１８は、走行軌跡モデルの誤差と物標モデルの誤差を比較して、走行軌跡モデルの誤差が物標モデルの誤差より小さい場合には、走行軌跡モデルから出力された算出結果を、物標モデルの入力として物標位置の推定を開始する。また、物標モデルの誤差が走行軌跡モデルの誤差より小さい場合には、物標モデルから出力された算出結果を、走行軌跡モデルの入力として物標位置の推定を開始する。
【００４５】
マッチング部１９は、物標の位置の補正が終了すると、物標位置補正部１７によって補正された物標の位置を地図情報とマッチングし、マッチング度合いが所定値以上となるまでトラジェクトリの補正と物標の位置の補正を繰り返し行うように制御する。このとき、補正された物標の位置だけではなく、補正されたトラジェクトリについても地図情報とのマッチングを行ってもよい。
【００４６】
具体的に、マッチング部１９は、補正された物標の位置を地図情報と重ね合わせて、照合して、地図情報に記録されている物標の位置と一致するか否かをマップマッチングによって判定する。例えば、補正された白線の位置の座標と地図情報に記録されている白線の位置の座標の差分を求め、その差分が所定値以下となるか否かによって判定する。そして、マッチング部１９は、補正された物標の位置と、地図情報に記録されている物標の位置が一致せずにマッチング度合いが所定値未満であると判定した場合には、トラジェクトリの補正と物標の位置の補正が繰り返し行われるように制御する。一方、補正された物標の位置と、地図情報に記録されている物標の位置が一致してマッチング度合いが所定値以上であると判定した場合には、地図情報とのマッチングを終了する。マッチングを終了すると、マッチング部１９は、地図データベース６へリンクして、補正後の物標の位置を地図データベース６へ記録する。
【００４７】
自己位置推定部２０は、マッチング部１９によって地図データベース６に記録された物標の位置を用いて自己位置を推定する。本実施形態において地図上の物標の位置は精度よく推定されているため、自車両から物標までの距離と方向を用いることにより、自己位置推定部２０は自己位置を精度よく推定することができる。尚、物標の位置を用いて車両の自己位置を推定する場合とは、物標の周辺を走行した場合に、物標までの距離と方向をセンサにより検出し、検出した距離と方向、及び取得していた物標の位置から、地図上の自車両の位置を推定することを意味する。
【００４８】
通信機７は、サーバ２５と通信するための装置である。
【００４９】
［トラジェクトリの補正処理］
次に、図４〜６を参照してトラジェクトリの補正方法を説明する。図４は、トラジェクトリの補正処理を示すフローチャートである。
【００５０】
図４に示すように、ステップＳ１において、トラジェクトリ算出部１３は、物標までの距離Ｌと物標の方向φとオドメトリを取得する。
【００５１】
ステップＳ２において、トラジェクトリ算出部１３は、車両の向きθと制御パラメータｑを所定値（例えば、０）で初期化する。
【００５２】
ステップＳ３において、トラジェクトリ算出部１３は、初期化された車両の向きθと制御パラメータｑを、走行軌跡モデルＸに入力してトラジェクトリを算出する。図５に示すトラジェクトリ２１は、初期のトラジェクトリである。また、トラジェクトリ算出部１３は、物標モデルにより出力された物標データを、走行軌跡モデルの入力としてもよい。
【００５３】
図５において点線で示す座標はマップ座標系であり、実線で示す座標は車両座標系である。車両座標系におけるｙ軸は自車両の進行方向を示し、ｘ軸は進行方向に直交する横方向を示す。図５に示す座標（ｘ１，ｙ１）〜座標（ｘ５，ｙ５）は、トラジェクトリ２１上の自己位置である。図５に示す車両の向きθ１〜θ５は、マップ座標系からのヨー角を示す。角度Φ１〜Φ５は、車両座標系から見た自車両の進行方向に対する物標３０の方向を示す角度である。距離Ｌ１〜Ｌ５は、車両座標系から見た自車両から物標３０までの距離である。尚、本実施形態では、所定の座標系を用いて説明しているが、マップ座標系や車両座標系に限定する必要はなく、その他の座標系を用いてもよく、また一つの座標系を用いてもよい。
【００５４】
図５に示すように、自車両は、座標（ｘ１，ｙ１）〜座標（ｘ５，ｙ５）の区間を走行中にカメラ２やレーザレンジファインダー３を用いて自車両から物標３０までの距離Ｌ１〜Ｌ５を計測する。自車両は、距離Ｌ１〜Ｌ５を計測した時刻と同時刻にジャイロセンサ５を用いて車両の向きθ１〜θ５を検出する。図５に示す例では、異なる５つの走行地点から物標３０までの距離と物標３０の方向を取得する様子を示しているが、５点に限定する必要はない。尚、角度Φ１〜Φ５についてもジャイロセンサ５を用いて取得することができる。
【００５５】
ステップＳ４において、物標位置算出部１４は、ステップＳ３で算出されたトラジェクトリデータを基準にして物標データを地図上に配置して、物標の位置を算出する。このとき、物標データとして、物標モデルにより出力された物標データを用いてもよい。トラジェクトリ２１上において、ｉ番目の時刻の自己位置をＸｉとすると、自己位置Ｘｉは、式（５）を用いて次のように表される。
【００５６】
【数５】
2019190994.tif 000007
【００５７】
物標３０の位置を（Ｃｘｉ，Ｃｙｉ）とすると、物標３０の位置（Ｃｘｉ，Ｃｙｉ）は、式（６）を用いて次のように表される。
【００５８】
【数６】
2019190994.tif 000008
【００５９】
式（６）における（Ｌｘｉ，Ｌｙｉ）は、ｉ番目の時刻における自車両から物標３０までの距離であり、角度Φの要素を含む。式（６）に示すように物標位置算出部１４は、自車両から物標３０までの距離（Ｌｘｉ，Ｌｙｉ）を、車両の向きθｉだけ回転させる。そして、物標位置算出部１４は、トラジェクトリ２１上の座標（ｘｉ，ｙｉ）に距離を加算し、座標（ｘｉ，ｙｉ）分平行移動してマップ座標系に変換することで各点毎に物標３０の位置を算出する。
【００６０】
ステップＳ５において、トラジェクトリ補正部１５は、ステップ４で算出された物標の位置と地図情報に記録されている物標の位置の差分を求める。すなわち、ステップＳ４でトラジェクトリデータを基準にして配置された物標データと地図情報との差分を算出する。尚、差分ではなく、物標の位置の分散を求めてもよい。また、ここで算出された差分は、走行軌跡モデルの誤差として推定される。
【００６１】
ステップＳ６において、トラジェクトリ補正部１５は、ステップＳ５で算出された差分が所定値以下であるか否かを判定する。差分が所定値以下である場合にはトラジェクトリの補正処理は終了し、トラジェクトリデータを出力する。一方、差分が所定値より大きい場合にはステップＳ７に進む。
【００６２】
ステップＳ７において、トラジェクトリ補正部１５は、差分が小さくなるようにトラジェクトリデータを予め定めたモデルによって調整し、差分が所定値より小さくなるまでトラジェクトリデータの調整を繰り返し行う。すなわち、ステップＳ４で算出された物標の位置と地図情報に記録されている物標の位置が一致するようにトラジェクトリの補正を繰り返し行う。例えば、ガウスニュートン法等の最適化手法を使用して、車両の向きθと制御パラメータｑを調整しながら繰り返し計算の中で徐々に真値に近づけていく方法でトラジェクトリ２１を補正する。その結果、図５に示すトラジェクトリ２１は、図６に示すように補正される。図５ではトラジェクトリ２１が左右にずれて算出されていたが、図６ではトラジェクトリ２１が滑らかな線に補正されている。
【００６３】
ここで、制御パラメータｑを調整する際に、制御パラメータｑは制御パラメータｐと相関があるため、制御パラメータｐの値に基づいて制御パラメータｑの値を設定し、調整していくようにする。これにより、トラジェクトリを補正する処理に要する時間を短縮することができる。さらに、制御パラメータｑを調整する範囲を絞ることができるので、ロバスト性を高めることができる。
【００６４】
［物標位置の補正処理］
次に、図７〜９を参照して物標位置の補正方法を説明する。図７は、物標位置の補正処理を示すフローチャートである。
【００６５】
図７に示すように、ステップＳ１０において、物標算出部１６は、自車両のセンサにより得られた物標までの距離Ｌと物標の方向φと、トラジェクトリを取得する。トラジェクトリは、トラジェクトリ補正部１５によって補正されたものであっても、その他のトラジェクトリであってもよい。
【００６６】
ステップＳ２０において、物標算出部１６は、車両の向きθと制御パラメータｐを所定値（例えば、０）で初期化する。
【００６７】
ステップＳ３０において、物標算出部１６は、初期化された車両の向きθと制御パラメータｐを、物標モデルＬに入力して物標を算出する。図８では、物標の一例として白線４０を示しており、白線４０は初期の白線である。また、物標算出部１６は、走行軌跡モデルにより出力されたトラジェクトリデータを、物標モデルＬの入力としてもよい。
【００６８】
図８に示すように、自車両は、座標（ｘ１，ｙ１）〜座標（ｘ５，ｙ５）の区間を走行中にカメラ２やレーザレンジファインダー３を用いて自車両から白線４０までの距離Ｌ１〜Ｌ５を計測する。自車両は、距離Ｌ１〜Ｌ５を計測した時刻と同時刻にジャイロセンサ５を用いて車両の向きθ１〜θ５を検出する。図８に示す例では、異なる５つの走行地点から白線４０までの距離と白線４０の方向を取得する様子を示しているが、５点に限定する必要はない。尚、角度Φ１〜Φ５についてもジャイロセンサ５を用いて取得することができる。
【００６９】
ステップＳ４０において、物標算出部１６は、自車両のオドメトリにより得られた走行軌跡データを基準にして物標データを地図上に配置して、物標の位置を算出する。このとき、走行軌跡データとして、走行軌跡モデルにより出力されたトラジェクトリデータを用いてもよい。白線４０上において、ｉ番目の時刻の白線の位置をＬｉとすると、白線の位置Ｌｉは、式（７）を用いて次のように表される。
【００７０】
【数７】
2019190994.tif 000009
【００７１】
白線４０の位置を（Ｃｘｉ，Ｃｙｉ）とすると、白線４０の位置（Ｃｘｉ，Ｃｙｉ）は、式（８）を用いて次のように表される。
【００７２】
【数８】
2019190994.tif 000010
【００７３】
式（８）における（Ｌｘｉ，Ｌｙｉ）は、ｉ番目の時刻における自車両から白線４０までの距離であり、角度Φの要素を含む。式（８）に示すように物標算出部１６は、自車両から白線４０までの距離（Ｌｘｉ，Ｌｙｉ）を、車両の向きθｉだけ回転させる。そして、物標算出部１６は、トラジェクトリ２１上の座標（ｘｉ，ｙｉ）に距離を加算し、座標（ｘｉ，ｙｉ）分平行移動してマップ座標系に変換することで各点毎に白線４０の位置を算出する。
【００７４】
ステップＳ５０において、物標位置補正部１７は、ステップＳ４０で算出された物標の位置と地図情報に記録されている物標の位置の差分を求める。すなわち、ステップＳ４０で走行軌跡データを基準にして配置された物標データと地図情報との差分を算出する。尚、差分ではなく、物標の位置の分散を求めてもよい。また、ここで算出された差分は、物標モデルの誤差として推定される。
【００７５】
ステップＳ６０において、物標位置補正部１７は、ステップＳ５０で算出された差分が所定値以下であるか否かを判定する。差分が所定値以下である場合には物標の補正処理は終了し、物標データを出力する。一方、差分が所定値より大きい場合にはステップＳ７０に進む。
【００７６】
ステップＳ７０において、物標位置補正部１７は、差分が小さくなるように物標データを予め定めたモデルによって調整し、差分が所定値より小さくなるまで物標データの調整を繰り返し行う。すなわち、ステップＳ４０で算出された物標の位置と地図情報に記録されている物標の位置が一致するように物標の補正を繰り返し行う。例えば、ガウスニュートン法等の最適化手法を使用して、車両の向きθと制御パラメータｐを調整しながら繰り返し計算の中で徐々に真値に近づけていく方法で白線４０を補正する。その結果、図８に示す白線は、図９に示すように補正される。図８では白線４０が左右にずれて算出されていたが、図９では白線４０が滑らかな線に補正されている。
【００７７】
ここで、制御パラメータｐを調整する際に、制御パラメータｐは制御パラメータｑと相関があるため、制御パラメータｑの値に基づいて制御パラメータｐの値を設定し、調整していくようにする。これにより、物標の位置を補正する処理に要する時間を短縮することができる。さらに、制御パラメータｐを調整する範囲を絞ることができるので、ロバスト性を高めることができる。
【００７８】
［自己位置推定処理の手順］
次に、図１０のフローチャートを参照して、本実施形態に係る自己位置推定装置１による自己位置推定処理の処理手順を説明する。
【００７９】
ステップＳ１０１において、カメラ２及びレーザレンジファインダー３は、図５に示すようにそれぞれ異なる走行地点から物標３０までの距離Ｌｉと車両の向きθｉを取得する。本実施形態において、ｉは時刻を示し、その値は１〜５である。
【００８０】
ステップＳ１０２において、トラジェクトリ算出部１３は、車両の向きθと制御パラメータｑを所定値で初期化する。
【００８１】
ステップＳ１０３において、トラジェクトリ算出部１３は、ステップＳ１０２で設定した初期値を走行軌跡モデルＸに入力し、図５に示すような初期のトラジェクトリ２１を算出する。そして、物標位置算出部１４は、算出されたトラジェクトリ２１を用いて地図上の物標３０の位置を算出する。すなわち、トラジェクトリデータを基準にして物標データを地図上に配置して物標３０の位置を算出する。物標の位置が算出されると、トラジェクトリ補正部１５は、算出された物標の位置と地図情報に記録されている物標の位置との差分を求め、この差分を走行軌跡モデルの誤差として算出する。
【００８２】
ステップＳ１０４において、物標算出部１６は、自車両のセンサにより得られた物標データを物標モデルＬに入力して自車両の周囲の物標データを算出する。そして、算出した物標データを、自車両のオドメトリにより得られた走行軌跡データを基準にして地図上に配置して物標の位置を算出する。例えば、図８に示すように、トラジェクトリ２１を基準にして地図上に白線４０を配置し、白線４０の位置を算出する。物標の位置が算出されると、物標位置補正部１７は、算出された物標の位置と地図情報に記録されている物標の位置との差分を求め、この差分を物標モデルの誤差として算出する。尚、ステップＳ１０３では物標３０を例にして説明し、ステップＳ１０４では白線４０を例にして説明したが、これはあくまで一例であり、ステップＳ１０３、１０４では異なる物標に対して誤差を算出するのではなく、同一の物標に対して誤差を算出する。
【００８３】
ステップＳ１０５において、モデル誤差判定部１８は、ステップＳ１０３で算出された走行軌跡モデルの誤差と、ステップＳ１０４で算出された物標モデルの誤差とを比較し、物標モデルの誤差が走行軌跡モデルの誤差より小さいか否かを判定する。判定した結果、物標モデルの誤差が走行軌跡モデルの誤差より小さい場合には、ステップＳ１０６に進んで、物標モデルから出力された算出結果を走行軌跡モデルの入力として物標位置の推定を開始する。一方、物標モデルの誤差が走行軌跡モデルの誤差以上である場合には、ステップＳ１１０に進んで、走行軌跡モデルから出力された算出結果を物標モデルの入力として物標位置の推定を開始する。
【００８４】
ステップＳ１０６において、トラジェクトリ算出部１３は、走行軌跡モデルＸを用いてトラジェクトリを算出する。具体的に、トラジェクトリ算出部１３は、物標モデルから出力された物標データを、走行軌跡モデルの入力としてトラジェクトリを算出する。例えば、ステップＳ１０４や後述するステップＳ１１１で算出された物標の位置を走行軌跡モデルに入力してトラジェクトリを算出する。この場合、トラジェクトリ算出部１３は、物標の位置から逆算してトラジェクトリを算出する。また、トラジェクトリ算出部１３は、ステップＳ１０２で設定した初期値を走行軌跡モデルＸに入力し、図５に示すような初期のトラジェクトリ２１を算出してもよい。さらに、自車両のオドメトリにより得られた走行軌跡データに基づいてトラジェクトリデータを算出してもよい。
【００８５】
ステップＳ１０７において、物標位置算出部１４は、ステップＳ１０６で算出されたトラジェクトリを用いて地図上の物標の位置を算出する。まず、物標位置算出部１４は、トラジェクトリ２１上の各点から物標３０までの距離Ｌｉを取得し、この距離を取得した時の車両の向きθｉを取得する。そして、物標位置算出部１４は、物標３０までの距離Ｌｉ及び車両の向きθｉを用いて各点毎に物標３０の位置を算出する。すなわち、トラジェクトリデータを基準にして物標データを地図上に配置して物標３０の位置を算出する。このとき、後述する物標モデルにより出力された物標データを、地図上に配置する物標データとして用いてもよい。
【００８６】
ステップＳ１０８において、トラジェクトリ補正部１５は、ステップＳ１０７で算出された物標３０の位置と地図情報に記録されている物標の位置が一致しているか否かを判定する。判定方法としては、算出された物標３０の位置と地図情報に記録されている物標の位置の差分を求め、その差分が所定の閾値以下であるか否かによって判定すればよい。また、算出された物標３０の位置の分散を算出し、分散が所定の閾値以下であるか否かによって判定してもよい。トラジェクトリ補正部１５は、算出された物標３０の位置と地図情報に記録されている物標の位置が一致していないと判定した場合にはステップＳ１０９へ進む。一方、算出された物標３０の位置と地図情報に記録されている物標の位置が一致していると判定された場合にはトラジェクトリの補正を終了してステップＳ１１０へ進む。
【００８７】
ステップＳ１０９において、トラジェクトリ補正部１５は、車両の向きθと制御パラメータｑを調整してトラジェクトリ２１を補正する。以後、ステップＳ１０６〜Ｓ１０９において、トラジェクトリ補正部１５は、ガウスニュートン法等の最適化手法を使用して、車両の向きθと制御パラメータｑを調整しながら繰り返し計算の中で徐々に真値に近づけていく方法を用いてトラジェクトリを補正する。例えば、物標３０の位置と地図情報に記録されている物標の位置の差分が小さくなるように、予め定めたモデルによってトラジェクトリデータを調整する。
【００８８】
ステップＳ１１０において、物標算出部１６は、物標モデルＬを用いて物標データを算出する。具体的に、物標算出部１６は、走行軌跡モデルから出力された算出結果を、物標モデルの入力として物標データを算出する。例えば、ステップＳ１０３、１０７で走行軌跡モデルにより出力された物標の位置を、物標モデルＬの入力として物標データを算出する。また、物標算出部１６は、車両の向きθと制御パラメータｐを物標モデルＬに入力して物標データを算出してもよい。さらに、物標算出部１６は、自車両のセンサにより得られた物標データを物標モデルＬに入力して、自車両の周囲の物標データを算出してもよい。
【００８９】
ステップＳ１１１において、物標算出部１６は、自車両のオドメトリにより得られた走行軌跡データを基準にして物標データを地図上に配置して、物標の位置を算出する。このとき、走行軌跡データとして、ステップＳ１０３、１０６で走行軌跡モデルにより出力されたトラジェクトリデータを用いてもよい。例えば、図８に示すように、トラジェクトリ２１を基準にして地図上に白線４０を配置し、白線４０の位置を算出してもよい。
【００９０】
ステップＳ１１２において、物標位置補正部１７は、ステップＳ１１１で算出された白線４０の位置と地図情報に記録されている白線の位置が一致しているか否かを判定する。判定方法としては、算出された白線４０の位置と地図情報に記録されている白線の位置の差分を求め、その差分が所定値以下であるか否かによって判定する。また、算出された白線４０の位置の分散を算出し、分散が所定の閾値以下であるか否かによって判定してもよい。物標位置補正部１７は、算出された白線４０の位置と地図情報に記録されている白線の位置が一致していないと判定した場合にはステップＳ１１３へ進む。一方、算出された白線４０の位置と地図情報に記録されている白線の位置が一致していると判定された場合には白線の位置の補正を終了してステップＳ１１４へ進む。
【００９１】
ステップＳ１１３において、物標位置補正部１７は、制御パラメータｐを調整して白線４０を補正する。以後、ステップＳ１１０〜ステップＳ１１３において、物標位置補正部１７は、ガウスニュートン法等の最適化手法を使用して、制御パラメータｐを調整しながら繰り返し計算の中で徐々に真値に近づけていく方法を用いて白線４０の位置を補正する。例えば、白線４０の位置と地図情報に記録されている白線の位置の差分が小さくなるように、予め定めたモデルによって物標データを調整する。
【００９２】
ステップＳ１１４において、マッチング部１９は、補正された白線の位置を地図情報とマッチングし、マッチング度合いが所定値以上であるか否かを判定する。具体的に、マッチング部１９は、補正された白線の位置を地図情報と重ね合わせて、照合して、地図情報に記録されている白線と一致するか否かを判定する。そして、補正された白線が地図情報と一致せずにマッチング度合いが所定値未満であると判定された場合には、ステップＳ１０６に戻ってトラジェクトリの補正と白線の位置の補正を繰り返し行う。尚、ステップＳ１１４からステップＳ１０６に戻った場合には、補正された物標の位置に基づいてトラジェクトリを算出し、物標の位置を算出する。一方、補正された白線が地図情報と一致してマッチング度合いが所定値以上であると判定された場合には、地図情報とのマッチングを終了してステップＳ１１５に進む。
【００９３】
このように、本実施形態では、ステップＳ１０６、１０７の走行軌跡モデルによって出力された算出結果を、ステップＳ１１０、１１１の物標モデルの入力としている。また、ステップＳ１１０、１１１の物標モデルによって出力された算出結果を、ステップＳ１０６、１０７の走行軌跡モデルの入力としている。したがって、本実施形態に係る自己位置推定装置１では、走行軌跡モデルと物標モデルのいずれか一方により出力された算出結果を、走行軌跡モデルと物標モデルのもう一方の入力としている。
【００９４】
ステップＳ１１５において、マッチング部１９は、地図データベース６へリンクして、補正後の白線の位置を地図データベース６へ記録する。また、白線の位置が補正されたことにより、その他の道路構造や物標の位置を補正する必要がある場合には、それらの位置についても補正して記録する。
【００９５】
ステップＳ１１６において、自己位置推定部２０は、ステップＳ１１５で地図データベース６に記録された白線４０の位置を用いて自己位置を推定し、本実施形態に係る自己位置の推定処理を終了する。本実施形態の自己位置の推定処理では、地図上の白線４０の位置を精度よく推定しているので、自車両から白線４０までの距離と方向を用いることにより、自己位置推定部２０は自己位置を精度よく推定することができる。
【００９６】
［第１実施形態の効果］
以上詳細に説明したように、本実施形態に係る自己位置推定装置１では、走行軌跡モデルの誤差と物標モデルの誤差を算出する。そして、走行軌跡モデルの誤差と物標モデルの誤差の大きさに応じて、走行軌跡モデルから出力された算出結果と物標モデルから出力された算出結果のどちらを用いて、物標位置の推定を開始するか決定する。これにより、走行軌跡モデルの誤差と物標モデルの誤差を考慮して物標位置の推定を開始できるので、自車両の自己位置を精度よく推定することができる。
【００９７】
例えば、図１１に示すように、実際の道路上の白線９１と地図情報に記録された白線９３との間には数ｃｍ、例えば１０ｃｍ程度の位置ずれが存在する場合がある。したがって、実際の道路環境において、地図情報をそのまま適用して白線の位置を推定し、車両の自己位置を算出すると、自己位置と実際の道路との間には誤差が発生することになる。加えて、地図によっては、物標情報が不足しており、白線等の物標との相対的な位置関係が分からず、自車両の位置を正確に推定することができない場合がある。
【００９８】
しかしながら、本実施形態に係る自己位置推定装置１では、走行軌跡モデルの誤差と物標モデルの誤差を考慮して物標位置の推定を開始するので、地図情報に誤差がある場合や地図情報が不足している場合でも、自車両の自己位置を正確に推定することができる。
【００９９】
また、本実施形態に係る自己位置推定装置１では、走行軌跡モデルと物標モデルのうち誤差の小さいほうのモデルから出力された算出結果を、走行軌跡モデルと物標モデルのもう一方の入力として物標位置の推定を開始する。これにより、誤差の小さいほうのモデルから出力された算出結果に基づいて物標位置の推定を開始できるので、自車両の自己位置を精度よく推定できるとともに、より早く自己位置を推定することが可能となる。
【０１００】
さらに、自己位置を精度よく推定できることにより、例えば、自動運転制御や運転支援制御を実行する場合に、車両の位置を正確に制御できるので、実際の物標に合った適切な位置制御を実行できるようになる。また、実際の物標に合った適切な走行支援を実行できるので、乗員に与える違和感を抑制した自動運転制御や運転支援制御を実行することができる。
【０１０１】
［第２実施形態］
以下、本発明の第２実施形態について、図面を参照して説明する。図面の記載において、第１実施形態と同一の部分には同一符号を付して説明を省略する。
【０１０２】
［自己位置推定装置の構成］
本実施形態に係る自己位置推定装置では、モデル誤差判定部１８によって行われる処理が第１実施形態と相違しているだけで、自己位置推定装置のその他の構成は図１に示す第１実施形態と同一である。
【０１０３】
モデル誤差判定部１８は、走行軌跡モデルの誤差と物標モデルの誤差の大きさに応じて、走行軌跡モデルから出力された算出結果と物標モデルから出力された算出結果のどちらを用いて、物標位置の推定を開始するか決定する。特に、本実施形態では、走行軌跡モデルの誤差と物標モデルの誤差に対して、それぞれ所定の閾値を設定する。そして、走行軌跡モデルの誤差と物標モデルの誤差が所定の閾値より大きいか否かによって、走行軌跡モデルから出力された算出結果と物標モデルから出力された算出結果のどちらを用いて、物標位置の推定を開始するか決定する。
【０１０４】
尚、設定される閾値は、算出された物標の位置の補正が不要であるか否かを判定するための閾値であり、実験やシミュレーションを通じて設定してもよく、また任意に設定してもよい。
【０１０５】
ここで、図１２を参照して、走行軌跡モデルの算出結果と物標モデルの算出結果のどちらを用いて物標位置の推定を開始するか決定する方法を説明する。モデル誤差判定部１８は、図１２のＡに示すように、走行軌跡モデルの誤差と物標モデルの誤差がいずれも所定の閾値以下である場合には、算出された物標の位置の補正は不要であると判定する。
【０１０６】
また、図１２のＢに示すように、モデル誤差判定部１８は、物標モデルの誤差が所定の閾値より大きい場合には、走行軌跡モデルから出力された算出結果を、物標モデルの入力として物標位置の推定を開始すると判定する。また、図１２のＣに示すように、走行軌跡モデルの誤差が所定の閾値より大きい場合には、物標モデルから出力された算出結果を、走行軌跡モデルの入力として物標位置の推定を開始すると判定する。
【０１０７】
さらに、図１２のＤに示すように、走行軌跡モデルの誤差と物標モデルの誤差がいずれも所定の閾値より大きい場合には、走行軌跡モデルから出力された算出結果を、物標モデルの入力として物標位置の推定を開始すると判定する。一般的に、物標の位置ずれよりもオドメトリの誤差のほうが小さくなる。そこで、走行軌跡モデルの誤差と物標モデルの誤差がいずれも所定の閾値より大きい場合には、走行軌跡モデルを優先させて、走行軌跡モデルから出力された算出結果を用いて物標位置の推定を開始する。
【０１０８】
尚、モデル誤差判定部１８は、自車両の車速が所定値未満である場合には、物標モデルから出力された算出結果を、走行軌跡モデルの入力として物標位置の推定を開始してもよい。車速が極めて低速になると、トラジェクトリの誤差が大きくなる傾向があるため、自車両の車速が所定値未満となった場合には、物標モデルを優先させて、物標モデルから出力された算出結果を用いて物標位置の推定を開始する。所定値については、トラジェクトリの誤差が大きくなる車速を予め実験やシミュレーションを通じて求めておき、その値に設定すればよい。
【０１０９】
また、車速だけではなく、自車両が走行している道路がカーブしている場合にも、トラジェクトリの誤差が大きくなる傾向がある。そこで、自車両がカーブを走行している場合にも、物標モデルを優先させて、物標モデルから出力された算出結果を用いて物標位置の推定を開始するようにしてもよい。
【０１１０】
［自己位置推定処理の手順］
次に、図１３のフローチャートを参照して、本実施形態に係る自己位置推定装置１による自己位置推定処理の処理手順を説明する。ただし、本実施形態に係る自己位置推定処理では、図１０のステップＳ１０５を、図１３のステップＳ２０５、２０６に変更したことが相違しているだけで、その他の処理は第１実施形態と同一である。そのため、その他のステップの詳細な説明は省略する。
【０１１１】
図１３に示すように、ステップＳ２０１〜２０４の処理が実行されて、走行軌跡モデルの誤差と物標モデルの誤差がそれぞれ算出される。そして、ステップＳ２０５において、モデル誤差判定部１８は、補正が必要であるか否かを判定する。すなわち、ステップＳ２０３で算出された走行軌跡モデルの誤差と、ステップＳ２０４で算出された物標モデルの誤差の少なくとも１つが所定の閾値より大きいか否かを判定する。
【０１１２】
図１２のＡに示すように、モデル誤差判定部１８は、走行軌跡モデルの誤差と物標モデルの誤差がいずれも所定の閾値以下である場合には、補正は不要であると判定して、ステップＳ２１５に進む。一方、走行軌跡モデルの誤差と物標モデルの誤差のいずれか、または両方が所定の閾値より大きい場合、すなわち図１２のＢ、Ｃ、Ｄの場合には、補正が必要であると判定して、ステップＳ２０６に進む。
【０１１３】
ステップＳ２０６において、モデル誤差判定部１８は、物標モデルから出力された算出結果を用いて自己位置の推定を開始するか否かを判定する。モデル誤差判定部１８は、走行軌跡モデルの誤差が所定の閾値より大きい場合、すなわち図１２のＣの場合には、物標モデルから出力された算出結果を用いて物標位置の推定を開始すると判定してステップＳ２０７へ進む。
【０１１４】
一方、モデル誤差判定部１８は、物標モデルの誤差が所定の閾値より大きい場合、すなわち図１２のＢの場合には、走行軌跡モデルから出力された算出結果を用いて物標位置の推定を開始すると判定してステップＳ２１１へ進む。また、走行軌跡モデルの誤差と物標モデルの誤差がいずれも所定の閾値より大きい場合、すなわち図１２のＤの場合にも、走行軌跡モデルから出力された算出結果を用いて物標位置の推定を開始すると判定してステップＳ２１１へ進む。この後、ステップＳ２０７〜２１７の処理が実行されて、自己位置が推定されると、本実施形態に係る自己位置の推定処理は終了する。
【０１１５】
［第２実施形態の効果］
以上詳細に説明したように、本実施形態に係る自己位置推定装置１では、走行軌跡モデルの誤差と物標モデルの誤差に対して、それぞれ所定の閾値を設定する。そして、走行軌跡モデルの誤差と物標モデルの誤差が所定の閾値より大きいか否かによって、走行軌跡モデルから出力された算出結果と物標モデルから出力された算出結果のどちらを用いて、物標位置の推定を開始するか決定する。これにより、走行軌跡モデルの誤差と物標モデルの誤差を考慮して物標位置の推定を開始できるので、自車両の自己位置を精度よく推定することができる。
【０１１６】
また、本実施形態に係る自己位置推定装置１では、物標モデルの誤差が所定の閾値より大きい場合には、走行軌跡モデルから出力された算出結果を、物標モデルの入力として物標位置の推定を開始する。これにより、物標モデルの誤差が大きい場合でも、走行軌跡モデルから出力された算出結果を用いて物標位置の推定を開始できるので、自車両の自己位置を精度よく推定することができる。
【０１１７】
さらに、本実施形態に係る自己位置推定装置１では、走行軌跡モデルの誤差が所定の閾値より大きい場合には、物標モデルから出力された算出結果を、走行軌跡モデルの入力として物標位置の推定を開始する。これにより、走行軌跡モデルの誤差が大きい場合でも、物標モデルから出力された算出結果を用いて物標位置の推定を開始できるので、自車両の自己位置を精度よく推定することができる。
【０１１８】
また、本実施形態に係る自己位置推定装置１では、走行軌跡モデルの誤差と物標モデルの誤差が所定の閾値より大きい場合には、走行軌跡モデルから出力された算出結果を、物標モデルの入力として物標位置の推定を開始する。これにより、走行軌跡モデルと物標モデルの両方の誤差が大きい場合には、より誤差が小さいと考えられる走行軌跡モデルから出力された算出結果を用いて物標位置の推定を開始できるので、自車両の自己位置を精度よく推定することができる。
【０１１９】
また、本実施形態に係る自己位置推定装置１では、自車両の車速が所定値未満である場合には、物標モデルから出力された算出結果を、走行軌跡モデルの入力として物標位置の推定を開始する。これにより、自車両の車速が低くてトラジェクトリの誤差が大きくなる可能性が高い場合に、物標モデルから出力された算出結果を用いて物標位置の推定を開始できるので、自車両の自己位置を精度よく推定することができる。
【０１２０】
尚、補正した物標を記録して、次回もしくは次回以降の走行支援に用いることも可能である。これにより、補正した物標データを用いて走行支援を実行できるので、次回もしくは次回以降の走行支援を実行したときに、実際の物標に合った適切な走行支援を実行することができる。また、補正した物標データを記録することにより、同じ道路を走行した時の物標推定が不要になるため、計算負荷を低減することができる。
【０１２１】
尚、上述の実施形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計などに応じて種々の変更が可能であることは勿論である。
【符号の説明】
【０１２２】
１  自己位置推定装置
２  カメラ
３  レーザレンジファインダー
４  ＧＰＳ受信機
５  ジャイロセンサ
６  地図データベース
７  通信機
１０  コントローラ
１１  走行軌跡モデル作成部
１２  物標モデル作成部
１３  トラジェクトリ算出部
１４  物標位置算出部
１５  トラジェクトリ補正部
１６  物標算出部
１７  物標位置補正部
１８  モデル誤差判定部
１９  マッチング部
２０  自己位置推定部
２５  サーバ
３０  物標
４０  白線
【図１】
2019190994.tif 000011
【図２】
2019190994.tif 000012
【図３】
2019190994.tif 000013
【図４】
2019190994.tif 000014
【図５】
2019190994.tif 000015
【図６】
2019190994.tif 000016
【図７】
2019190994.tif 000017
【図８】
2019190994.tif 000018
【図９】
2019190994.tif 000019
【図１０】
2019190994.tif 000020
【図１１】
2019190994.tif 000021
【図１２】
2019190994.tif 000022
【図１３】
2019190994.tif 000023