JP2021105584A - 位置推定方法及び位置推定装置 - Google Patents

Abstract

【課題】センサにより取得した自車両の周囲の物体の３次元位置情報が十分でない場合の自車両の位置の推定精度の低下を軽減する。【解決手段】位置推定方法では、自車両１に搭載されたセンサ１１により自車両１の周囲の物体２０の自車両１に対する３次元位置情報である第１位置情報２１を取得し（Ｓ３）、自車両１へのセンサ１１の取付位置に基づいて、自車両１が走行する路面２４の自車両１に対する３次元位置情報である第２位置情報２５ａ〜２５ｄを生成し（Ｓ７）、自車両１の周囲の物体２０及び自車両１が走行する路面２４の既知の３次元位置情報を有する地図情報１２と、第１位置情報２１及び第２位置情報２５ａ〜２５ｄとを比較して自車両の位置を推定する（Ｓ８）。【選択図】図３

Description

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

下記特許文献１には、ＬＩＤＡＲ（Light Detection and Ranging、Laser Imaging Detection and Ranging）等のセンサを自車両に搭載し、センサにより検出された情報と地図情報とを比較して、自車両の現在位置の推定を行う技術が記載されている。

国際公開第２０１８／１８００９６号パンフレット

しかしながら、センサにより取得した自車両の周囲の物体の３次元位置情報が十分でないと、鉛直方向の自車両の位置の推定精度や、ロール角、ピッチ角の推定精度が低下するおそれがある。この結果、水平方向の自車両の位置の推定精度にも影響を及ぼす恐れがある。
本発明は、センサにより取得した自車両の周囲の物体の３次元位置情報が十分でない場合の自車両の位置の推定精度の低下を軽減することを目的とする。

本発明の一態様による位置推定方法では、自車両に搭載されたセンサにより自車両の周囲の物体の自車両に対する３次元位置情報である第１位置情報を取得し、自車両へのセンサの取付位置に基づいて、自車両が走行する路面の自車両に対する３次元位置情報である第２位置情報を生成し、自車両の周囲の物体及び自車両が走行する路面の既知の３次元位置情報を有する地図情報と、第１位置情報及び第２位置情報とを比較して自車両の位置を推定する。

本発明の一形態によれば、センサにより取得した自車両の周囲の物体の３次元位置情報が十分でない場合の自車両の位置の推定精度の低下を軽減できる。

実施形態の位置推定装置を搭載した車両の概略構成の一例のブロック図である。 センサによる３次元位置情報を用いた位置推定において鉛直方向の推定精度が低下する理由の説明図である。 センサによる３次元位置情報を用いた位置推定において鉛直方向の推定精度が低下する理由の説明図である。 センサによる３次元位置情報を用いた位置推定において水平方向の推定精度が低下する理由の説明図である。 実施形態の位置推定方法の一例の説明図である。 位置推定装置の概略構成の一例のブロック図である。 自車両の移動量に基づく点群データの補正の説明図である。 仮想点群の設定範囲の第１例の説明図である。 仮想点群の設定範囲の第２例の説明図である。 仮想点群の設定範囲の第３例の説明図である。 仮想点群の設定範囲の第４例の説明図である。 仮想点群の設定範囲の第５例の説明図である。 点群データ及び仮想点群データと地図点群データとを比較して自車両の位置を推定する処理の一例の説明図である。 実施形態の位置推定方法の一例のフローチャートである。

以下、図面を参照して、本発明の実施形態を説明する。図面の記載において、同一又は類似の部分には同一又は類似の符号を付し、重複する説明を省略する。各図面は模式的なものであり、現実のものとは異なる場合が含まれる。以下に示す実施形態は、本発明の技術的思想を具体化するための装置や方法を例示するものであって、本発明の技術的思想は、下記の実施形態に例示した装置や方法に特定するものでない。本発明の技術的思想は、特許請求の範囲に記載された技術的範囲内において、種々の変更を加えることができる。

（構成）
図１を参照する。以下、移動体の一例として車両の現在位置を推定する場合について説明するが、本発明は車両に限らず様々な移動体の現在位置の推定に広く適用することができる。
自車両１には、位置推定装置１０が搭載される。位置推定装置１０は、物体センサ１１と、地図データベース１２と、測位装置１３と、車両センサ１４と、コントローラ１５を備える。図１において地図データベースを「地図ＤＢ」と表記する。図４においても同様である。

物体センサ１１は、自車両１の周囲の物体表面の位置を検出して、検出位置の３次元位置情報を出力するセンサである。
例えば物体センサ１１は、ＬＩＤＡＲや、レーザレーダ、ミリ波レーダ、ソナーなどの測距センサを備えてもよい。また、物体センサ１１は、自車両１に搭載されたカメラであってもよい。例えば物体センサ１１として、自車両１の周囲の物体を水平方向に走査して物体の３次元位置情報を取得するセンサを使用してもよい。

例えば物体センサ１１は、取得した自車両１の周囲の物体表面の自車両に対する３次元位置情報を、３次元の点群データとしてコントローラ１５へ出力する。物体センサ１１により取得される３次元位置情報は、特許請求の範囲に記載の「第１位置情報」の一例である。なお、物体センサ１１により取得される３次元位置情報は、物体センサ１１の位置（すなわち自車両１の位置）を基準とする相対位置情報であり、自車両１の位置を中心とする車両座標系で表現される。
なお、物体センサ１１による３次元位置情報は、必ずしも点群データに限定される必要はなく、例えば画像特徴点や明示的なランドマークの位置情報であってもよい。

地図データベース１２は、カメラ、ＬＩＤＡＲや、レーザレーダ、ミリ波レーダ、ソナーなどセンサを用いて取得された道路及びその周辺に存在する既知の物体や道路の路面の位置を示す３次元位置情報により構成される地図情報のデータベースである。地図データベース１２は、フラッシュメモリ、ハードディスクドライブなどの所定の記憶装置に記憶される。地図データベース１２を構成する３次元位置情報は、例えば３次元点群データや、３次元点群データから位置推定のために抽出された各種形態の情報（特徴点、物標、ランドマーク、統計による近似値）であってよい。

地図データベース１２に格納される３次元位置情報は、所定の処理座標系（例えば、地図座標系や世界座標系）で表現されており、地図上の位置を示す。一方で、物体センサ１１により取得される３次元位置情報は、物体センサ１１の位置（すなわち自車両１の位置）を基準とする相対位置情報であり、自車両１の位置を中心とする車両座標系で表現される。
以下、地図情報を構成する点群データを「地図点群データ」と表記し、物体センサ１１により取得される点群データを「センサ点群データ」と表記する。

測位装置１３は、自車両１の現在位置を測定する。測位装置１３は、例えば全地球型測位システム（ＧＮＳＳ）受信機を備えてよい。ＧＮＳＳ受信機は、例えば地球測位システム（ＧＰＳ）受信機等であり、複数の航法衛星から電波を受信して自車両１の現在位置を測定する。

車両センサ１４は、自車両１に搭載され、自車両１から得られる様々な情報（車両信号）を検出する。車両センサ１４には、例えば、自車両１の走行速度（車速）を検出する車速センサ、自車両１が備える各タイヤの回転速度を検出する車輪速センサ、自車両１の３軸方向の加速度（減速度を含む）を検出する３軸加速度センサ（Ｇセンサ）、操舵角（転舵角を含む）を検出する操舵角センサ、自車両１に生じる角速度を検出するジャイロセンサ、ヨーレートを検出するヨーレートセンサ、自車両１のアクセル開度を検出するアクセルセンサと、運転者によるブレーキ操作量を検出するブレーキセンサが含まれる。

コントローラ１５は、物体センサ１１によるセンサ点群データと地図データベース１２に格納される地図点群データとに基づいて、自車両１の現在の３次元位置と３軸方向の向き（すなわち６自由度の位置及び姿勢）を推定する電子制御ユニット（ＥＣＵ：Electronic Control Unit）である。
以下、コントローラ１５による自車両１の現在の３次元位置と３軸方向の向きの推定を、単に「位置推定」と表記することがある。

コントローラ１５は、プロセッサ１６と、記憶装置１７等の周辺部品とを含む。プロセッサ１６は、例えばＣＰＵ（Central Processing Unit）やＭＰＵ（Micro-Processing Unit）であってよい。
記憶装置１７は、半導体記憶装置や、磁気記憶装置、光学記憶装置等を備えてよい。記憶装置１７は、レジスタ、キャッシュメモリ、主記憶装置として使用されるＲＯＭ（Read Only Memory）及びＲＡＭ（Random Access Memory）等のメモリを含んでよい。
以下に説明するコントローラ１５の機能は、例えばプロセッサ１６が、記憶装置１７に格納されたコンピュータプログラムを実行することにより実現される。

なお、コントローラ１５を、以下に説明する各情報処理を実行するための専用のハードウエアにより形成してもよい。
例えば、コントローラ１５は、汎用の半導体集積回路中に設定される機能的な論理回路を備えてもよい。例えばコントローラ１５はフィールド・プログラマブル・ゲート・アレイ（ＦＰＧＡ：Field-Programmable Gate Array）等のプログラマブル・ロジック・デバイス（ＰＬＤ：Programmable Logic Device）等を有していてもよい。

次に、コントローラ１５による位置推定の一例を説明する。
図２Ａ及び図２Ｂを参照する。物体センサ１１により取得されるセンサ点群データ２１と地図点群データとを照合して自車両１の位置推定を行う場合に、取得したセンサ点群データ２１が十分でないことにより、自車両１の鉛直方向位置が不明になるおそれがある。
例えば、ＬＩＤＡＲには、視野角が狭いセンサ（いわゆるラインスキャナ）などがあり、このセンサで得られる情報は平面状に分布する点群情報となり、例えば鉛直方向に関する情報が縮退する。

すると、例えば図２Ａ及び図２Ｂに示すように、鉛直方向に沿った形状変化が少ない物体２０のセンサ点群データ２１が得られた場合に、このセンサ点群データ２１と地図点群データとを比較しても自車両１の鉛直方向位置が不明になり、鉛直方向の自車両１の位置の推定精度が低下するおそれがある。

このように、自車両１の鉛直方向位置の精度が低下すると、水平方向位置の推定精度の低下を招く恐れがある。図２Ｃを参照してその理由の一例を説明する。
例えば、鉛直方向位置が異なる物体表面２２及び２３は、物体センサ１１による測距方向における位置が異なる。このため、自車両１の鉛直方向位置の精度が低下したことにより、誤って物体表面２２のセンサ点群データ２１と物体表面２３の地図点群データとを比較して自車両１の位置を推定すると、水平方向位置の推定精度も低下する。

そこで、実施形態の位置推定方法では、自車両１の下には必ず路面があり、路面の位置は自車両１への物体センサ１１の取付位置に基づいて推定できることに着目する。
図３を参照する。コントローラ１５は、自車両１への物体センサ１１の取付位置に基づいて、自車両１が走行する路面２４の３次元位置情報２５ａ、２５ｂ、２５ｃ及び２５ｄを仮想的に生成する。３次元位置情報２５ａ〜２５ｄは、特許請求の範囲に記載の「第２位置情報」の一例である。
３次元位置情報２５ａ〜２５ｄは、物体センサ１１により取得される３次元位置情報と同様に、物体センサ１１の位置（すなわち自車両１の位置）を基準とする相対位置情報であり、自車両１の位置を中心とする車両座標系で表現される。

例えばコントローラ１５は、路面２４への自車両１の接地点から物体センサ１１までの高さＨに基づいて、３次元位置情報２５ａ〜２５ｄを生成してよい。
例えばコントローラ１５は、３次元位置情報２５ａ〜２５ｄを、路面２４の３次元位置を表す３次元点群データとして生成する。以下、コントローラ１５によって生成された路面２４の点群データを「仮想点群データ」と表記する。

コントローラ１５は、物体センサ１１により取得される３次元位置情報及びコントローラ１５が生成した路面２４の３次元位置情報と地図情報とを比較して、自車両１の位置を推定する。
例えばコントローラ１５は、センサ点群データ及び仮想点群データと、地図点群データ
と、を比較して自車両１の位置を推定する。

このように、実施形態の位置推定方法では、物体センサ１１により取得される３次元位置情報２１と地図情報との比較に加えて、物体センサ１１の取付位置に基づいて仮想的に生成した路面２４の３次元位置情報と２５ａ〜２５ｄ地図情報とを比較して自車両１の位置を推定する。これにより地図情報と比較する鉛直方向に関する位置情報を増加できる。
この結果、物体センサ１１による３次元位置情報が十分でなくても、鉛直方向の自車両１の位置の推定精度や、ロール角、ピッチ角の推定精度の低下を軽減できる。また、鉛直方向の位置精度の低下に伴う水平方向の位置精度の低下も軽減できる。

以下、位置推定装置１０の機能の一例を詳しく説明する。以下、物体センサ１１により得られる３次元位置情報や地図データベース１２を構成する３次元位置情報が３次元点群データである例について説明する。しかしながら本発明はこのような形態に限定されるものではなく、上記の通り３次元位置情報として画像特徴点、物標、ランドマーク、統計による近似値を用いることができる。
図４を参照する。コントローラ１５は、点群データ取得部３０と、点群データ格納部３１と、移動量演算部３２と、点群データ補正部３３と、仮想点群追加部３４と、位置推定部３５を備える。

点群データ取得部３０は、物体センサ１１からセンサ点群データ２１を取得する。点群データ取得部３０は、取得したセンサ点群データ２１を点群データ格納部３１に格納する。例えば点群データ格納部３１は、図１に示す記憶装置１７に確保された記憶領域であってよい。地図データベース１２が記憶される記憶装置に、点群データ格納部３１のための記憶領域を確保してもよい。
移動量演算部３２は、前回の処理サイクルでセンサ点群データ２１を取得した時点ｔｐから今回の処理サイクルでセンサ点群データ２１を取得する時点ｔｃまでの自車両１の移動量を演算する。例えば移動量演算部３２は、時点ｔｐから時点ｔｃまでの自車両１の位置と姿勢（方向）の変化を自車両１の移動量として演算する。
例えば、移動量演算部３２は、測位装置１３による測位結果や、車両センサ１４からの車両信号によるオドメトリに基づいて自車両１の移動量を演算する。

点群データ補正部３３は、前回の処理サイクル及びそれ以前に（すなわち今回の処理サイクルでセンサ点群データ２１を取得する時点ｔｃよりも前に）取得され点群データ格納部３１に格納されたセンサ点群データ２１を、移動量演算部３２が演算した移動量で補正して、点群データ格納部３１に蓄積する。
図５を参照して点群データ補正部３３によるセンサ点群データ２１の補正処理の一例を説明する。参照符号１は、今回の処理サイクルでセンサ点群データ２１を取得する時点ｔｃにおける自車両の位置を示す。

一方で、破線４０は前回の処理サイクルでセンサ点群データ２１を取得した時点ｔｐにおける自車両１の位置を示す。丸プロット２１は、時点ｔｐにおいてセンサ点群データが示す位置を示す。
上記の通り、センサ点群データは車両座標系上の座標、すなわち自車両１を基準とする相対位置で表現されている。このため、時点ｔｐから時点ｔｃまでの間に参照符号１で示す位置へ自車両１が移動すると、センサ点群データ２１が示す位置は、四角プロット４１で示す位置へ変化する。時点ｔｃの自車両１の位置を基準とする相対位置４１は、時点ｔｐの位置４０を基準とする相対位置２１と同一である。
点群データ補正部３３は、四角プロット４１で示す相対位置を表すセンサ点群データ２１を、時点ｔｃにおける自車両１の位置を基準にして丸プロット２１で示す相対位置を示す点群データへ補正する。

図４を参照する。仮想点群追加部３４は、自車両１への物体センサ１１の取付位置に基づいて、自車両１が走行する路面２４の位置を示す仮想点群データ２５ａ〜２５ｄを仮想的に生成する。仮想点群データ２５ａ〜２５ｄは、自車両１の位置推定において地図点群データと比較する点群データに追加される。仮想点群データ２５ａ〜２５ｄを総称して「仮想点群データ２５」と表記すことがある。
仮想点群追加部３４は、仮想点群データ２５の鉛直方向の座標を、路面２４への自車両１の接地点から物体センサ１１までの高さＨに基づいて演算する。

仮想点群追加部３４は、例えば図６Ａ〜６Ｃ及び図７Ｂに示す水平面内範囲４２に１つ以上の仮想点群データ２５を生成る。例えば仮想点群追加部３４は、これらの範囲４２に分布するように、仮想点群データ２５を生成してよい。
例えば、仮想点群追加部３４は、仮想点群データ２５の水平面内位置を範囲４２内に位置するように適宜決定し、高さＨに基づいて鉛直方向位置を演算してよい。

図６Ａを参照する。例えば仮想点群追加部３４は、自車両１の下の路面２４はほぼ平面であると仮定して、自車両１の直下の範囲４２を、仮想点群データ２５の生成範囲として設定してよい。この場合、例えば仮想点群追加部３４は、自車両１の直下の範囲４２に等間隔に分布するように仮想点群データ２５を配置してもよく、不均等に仮想点群データ２５を配置してもよい。例えば、自車両１の接地部位の位置に近いほど密に、遠くなるほど疎らになるように仮想点群データ２５を配置してもよい。
また、例えば仮想点群追加部３４は、図６Ｂに示すように、自車両１の直下の範囲に所定のマージンを加えた範囲４２を、仮想点群データ２５の生成範囲として設定してよい。
この場合、例えば仮想点群追加部３４は、マージンが加えられた範囲４２に等間隔に分布するように仮想点群データ２５を配置してもよく、不均等に仮想点群データ２５を配置してもよい。例えば、自車両１の直下の位置に近いほど密に、遠くなるほど疎らになるように仮想点群データ２５を配置してもよい。また、自車両１の接地部位の位置に近いほど密に、遠くなるほど疎らになるように仮想点群データ２５を配置してもよい。

また図６Ｃに示すように、例えば自車両１が占める車幅方向の位置範囲に亘って延在し、自車両１の現在位置から後方に延びる範囲４２に分布するように、仮想点群データ２５を生成してよい。
この場合、例えば仮想点群追加部３４は、後方に延びる範囲４２に等間隔に分布するように仮想点群データ２５を配置してもよく、不均等に仮想点群データ２５を配置してもよい。例えば、自車両１の直下の位置に近いほど密に、遠くなるほど疎らになるように仮想点群データ２５を配置してもよい。また、自車両１の接地部位の位置に近いほど密に、遠くなるほど疎らになるように仮想点群データ２５を配置してもよい。
また例えば、仮想点群追加部３４が、自車両１の直下の範囲４２に仮想点群データ２５を生成し、点群データ補正部３３が、自車両１の移動量で仮想点群データ２５を補正して蓄積することにより、図６Ｃに示す範囲４２に分布する仮想点群データ２５を生成してもよい。

また仮想点群追加部３４は、図７Ａに示すように路面２４に対する自車両１の接地部位の位置に、仮想点群データ２５ａ及び２５ｂを生成してよい。仮想点群追加部３４は、自車両１への物体センサ１１の取付位置に基づいて自車両１の接地部位の位置を算出する。
また仮想点群追加部３４は、図７Ｂに示すように路面２４に対する自車両１の接地部位の位置の軌跡が占める範囲４２に分布するように、仮想点群データ２５を生成してよい。
例えば、仮想点群追加部３４が、自車両１の接地部位の位置に仮想点群データ２５を生成し、点群データ補正部３３が、自車両１の移動量で仮想点群データ２５を補正して蓄積することにより、図７Ｂに示す範囲４２に分布する仮想点群データ２５を生成してよい。

なお、仮想点群追加部３４は、仮想点群データ２５を生成する際に、点群データ取得部３０が取得したセンサ点群データ２１や、点群データ格納部３１に蓄積されたセンサ点群データ２１に、自車両１の位置推定に十分な路面２４の点群データが含まれているか否かを判定してよい。
仮想点群追加部３４は、十分な路面２４の点群データがセンサ点群データ２１に含まれていない場合に仮想点群データ２５を生成（追加）し、十分な路面２４の点群データがセンサ点群データ２１に含まれている場合には、仮想点群データ２５を生成（追加）しないか、または、生成する仮想点群データ２５を低減してもよい。これにより、地図点群データと比較される仮想点群データ２５が低減される。この結果、計算負荷を軽減できる。

具体的には、仮想点群追加部３４は、物体センサ１１の取付位置から路面２４までの高さＨを求め，高さＨとの高低差が一定値以下のセンサ点群データ２１が所定数以上ある場合に、十分な路面２４のセンサ点群データ２１があると判定してよい。
または、センサ点群データ２１を取得した範囲を等間隔の複数セルに分割し、各セルにおいてセンサ点群データ２１が示す形状が水平面と平行か否かを判定し、センサ点群データ２１が示す形状が水平面と平行であるセルが一定数以上ある場合に、十分な路面２４のセンサ点群データ２１があると判定してもよい。

また、仮想点群追加部３４は、仮想点群データ２５を生成する際に、自車両１の直下の路面２４の平面度を推定してよい。
仮想点群追加部３４は、平面度が許容範囲にある場合（例えば平面度が閾値以上の場合）に仮想点群データ２５を生成（追加）し、平面度が許容範囲にない場合（例えば平面度が閾値未満の場合）には、仮想点群データ２５を生成（追加）しないか、または、生成する仮想点群データ２５を低減してもよい。これにより、地図点群データと比較される仮想点群データ２５が低減される。
このため、路面２４が平坦であるとの仮定が成立しない個所で仮想点群データ２５を生成することによる、位置推定の精度低下を防止できる。

例えば、仮想点群追加部３４は、自車両１の直下の路面２４の形状が平面近似できるかどうかを判断し、平面近似できないと判断した場合は、仮想点群データ２５を生成しない、又は仮想点群データ２５を低減してよい。
具体的には、仮想点群追加部３４は、段差を乗り越えたか、近傍の路面２４の曲率が十分小さいかに基づいて、平面近似できるか否かを判定してよい。仮想点群追加部３４は、加速度センサなどにより鉛直方向に大きな加速度変化が生じたか否かによって段差の乗り越えを判断してよい。仮想点群追加部３４は、鉛直方向加速度やピッチ角の時系列に基づいて路面２４の曲率を推定してよい。

位置推定部３５は、センサ点群データ及び仮想点群データと、地図点群データと、を比較して自車両１の位置を推定する。以下、位置推定部３５による位置推定の一例を説明する。
位置推定部３５は、前回の処理サイクルで推定した自車両１の処理座標系上の位置（すなわち地図上の位置）を、移動量演算部３２が演算した移動量で補正して自車両１の現在の仮位置を決定する。

位置推定部３５は、自車両１の処理座標系上の位置が仮位置であると仮定して、センサ点群データ及び仮想点群データが示す相対位置情報を、処理座標系上の絶対位置情報に変換する。
図８に、処理座標系におけるセンサ点群データ２１ａ、２１ｂ、…及び仮想点群データ２５…の位置を示す。
位置推定部３５は、センサ点群データ２１ａ、２１ｂ、…及び仮想点群データ２５…の各々に対応する地図点群データＭ１、Ｍ２、Ｍ３…の組合せを選択する。

位置推定部３５は、センサ点群データ２１ａ、２１ｂ、…及び仮想点群データ２５…と、地図点群データＭ１、Ｍ２、Ｍ３…との間の誤差の大きさ（距離）Ｄ１、Ｄ２、Ｄ３…を算出し、誤差の総和Ｓ＝Ｄ１＋Ｄ２＋Ｄ３＋…を算出する。
位置推定部３５は、数値解析により、総和Ｓが最小となる地図点群データＭ１、Ｍ２、Ｍ３…の組合せと自車両１の位置及び姿勢を算出して、自車両１の現在位置（地図上の位置）の推定値として決定する。

また、位置推定部３５は、センサ点群データ２１ａ、２１ｂ、…及び仮想点群データ２５…と、地図点群データＭ１、Ｍ２、Ｍ３…との間の合致度（マッチング度）Ｄｍを算出する。
例えば、位置推定部３５は、最小の総和Ｓが小さいほど高い合致度Ｄｍを算出してよい。例えば、位置推定部３５は、最小の総和Ｓの減少関数に基づいて合致度Ｄｍを算出してよい。また例えば位置推定部３５は、最小の総和Ｓの増加関数の逆数（例えば最小の総和Ｓの逆数）を合致度Ｄｍとして算出してよい。

実際には、位置推定部３５は、既存のＩＣＰ(iterative closest point)、ＮＤＴ(Normal Distribution Transform)、やカルマンフィルタ、拡張カルマンフィルタ、粒子フィルタ、情報フィルタ、あるいは、ニュートン法、レーベンバーク・マルカート法などに基づいて、自車両１の現在位置の推定値と合致度Ｄｍとを算出してよい。

仮想点群追加部３４は、合致度Ｄｍに応じて生成する仮想点群データ２５の密度を調整して（変化させて）もよく、合致度Ｄｍに応じて仮想点群データ２５を生成する水平方向範囲の広さを調整して（変化させて）もよい。
例えば仮想点群追加部３４は、合致度Ｄｍが低いほど、生成する仮想点群データ２５の密度を下げてよい。また、合致度Ｄｍが低いほど、仮想点群データ２５を生成する水平方向範囲を狭くしてもよい。

合致度Ｄｍが低い仮想点群データ２５を多数生成すると、このような仮想点群データ２５に過度にマッチングし、正しい推定を行うことができなくなるからである。また、仮想点群追加部３４は、合致度Ｄｍが所定値より低い場合に仮想点群データ２５にノイズを加え、合致度Ｄｍが引くほどノイズを増やしてもよい。このような方法によっても、合致度Ｄｍが低い仮想点群データ２５との過度のマッチングを抑制できる。
仮想点群データ２５を生成する水平方向範囲を狭くする場合には、例えば車幅方向範囲を狭くしてよい。一般的な路面は、水はけのために路端に向かって表面高さが低くなるように形成されており、車幅方向に向かって自車両１の接地位置から離れるにつれて接地位置との高低差が増加するからである。

仮想点群追加部３４は、今回の処理サイクルで算出した合致度Ｄｍに応じて、次回以降の処理サイクルで生成する仮想点群データ２５の密度及び／又は水平方向範囲を調整しても（変化させても）よい。
または、仮想点群追加部３４は、密度及び／又は水平方向範囲を調整して（変化させて）今回の処理サイクルで用いる仮想点群データ２５を再度生成し、位置推定部３５が位置推定をやり直してもよい。
また、合致度Ｄｍに応じて仮想点群データ２５の密度及び／又は水平方向範囲を小さくする場合には、位置推定部３５は、生成済みの仮想点群データ２５の中から位置推定に用いる仮想点群データ２５を選択して、より小さな密度及び／又は水平方向範囲の仮想点群データ２５を用いて位置推定をやり直してもよい。

（動作）
次に図９を参照して、実施形態の位置推定方法の一例を説明する。
ステップＳ１において移動量演算部３２は、前回の処理サイクルでセンサ点群データ２１を取得した時点ｔｐから今回の処理サイクルでセンサ点群データ２１を取得する時点ｔｃまでの自車両１の移動量を演算する。

ステップＳ２において点群データ補正部３３は、前回の処理サイクル及びそれ以前に（すなわち今回の処理サイクルでセンサ点群データ２１を取得する時点ｔｃよりも前に）取得され点群データ格納部３１に格納されたセンサ点群データ２１を、移動量演算部３２が演算した移動量で補正して、点群データ格納部３１に蓄積する。
ステップＳ３において点群データ取得部３０は、物体センサ１１からセンサ点群データ２１を取得して、点群データ格納部３１に格納する。

ステップＳ４において点群データ補正部３３は、点群データ格納部３１に蓄積されたセンサ点群データ２１に、自車両１の位置推定に十分な路面２４の点群データが含まれているか否かを判定する。十分な路面２４のセンサ点群データ２１がある場合（ステップＳ４：Ｙ）には、点群データ補正部３３が仮想点群データ２５を生成せずに、処理はステップＳ５へ進む。
ステップＳ５において位置推定部３５は、センサ点群データ２１と地図点群データとを比較して自車両１の位置を推定する。その後に処理はステップＳ１０へ進む。

一方で、ステップＳ４において十分な路面２４のセンサ点群データ２１がない場合（ステップＳ４：Ｎ）に処理はステップＳ６へ進む。
ステップＳ６において仮想点群追加部３４は、自車両１の直下の路面２４の平面度を推定し、平面度が許容範囲にあるか否かを判定する。平面度が許容範囲にない場合（ステップＳ６：Ｎ）に、点群データ補正部３３が仮想点群データ２５を生成せずに、処理はステップＳ５へ進む。
ステップＳ５において位置推定部３５は、センサ点群データ２１と地図点群データとを比較して自車両１の位置を推定する。その後に処理はステップＳ１０へ進む。

一方で、ステップＳ６において平面度が許容範囲にある場合（ステップＳ６：Ｙ）に処理はステップＳ７へ進む。
ステップＳ７において仮想点群追加部３４は、自車両１への物体センサ１１の取付位置に基づいて、自車両１が走行する路面２４の位置を示す仮想点群データ２５を仮想的に生成する。
ステップＳ８において位置推定部３５は、センサ点群データ２１及び仮想点群データ２５と、地図点群データと、を比較して自車両１の位置を推定する。また、位置推定部３５は、センサ点群データ２１及び仮想点群データ２５と、地図点群データとの間の合致度（マッチング度）Ｄｍを算出する。

ステップＳ９において仮想点群追加部３４は、合致度Ｄｍに応じて生成する仮想点群データ２５の密度を調整する。及び／又は、合致度Ｄｍに応じて仮想点群データ２５を生成する水平方向範囲を調整する。その後に処理はステップＳ１０へ進む。
ステップＳ１０において位置推定装置１０は、自車両１のイグニッションスイッチ（ＩＧＮ）がオフになったか否かを判定する。イグニッションスイッチがオフにならない場合（ステップＳ１０：Ｎ）に処理はステップＳ１に戻る。イグニッションスイッチがオフになった場合（ステップＳ１０：Ｙ）に処理は終了する。

（実施形態の効果）
（１）点群データ取得部３０は、自車両１に搭載された物体センサ１１により自車両１の周囲の物体の自車両１に対する３次元位置情報である第１位置情報を取得する。仮想点群追加部３４は、自車両１への物体センサ１１の取付位置に基づいて、自車両１が走行する路面２４の自車両１に対する３次元位置情報である第２位置情報を生成する。位置推定部３５は、自車両１の周囲の物体及び自車両１が走行する路面の既知の３次元位置情報を有する地図情報と、第１位置情報及び第２位置情報とを比較して自車両１の位置を推定する。

これにより、第１位置情報のみを用いて自車両１の位置推定を行った場合には鉛直方向の自車両の位置や、ロール角、ピッチ角の推定精度が低くなっても、第１位置情報に加えて第２位置情報を用いて自車両１の位置を推定することにより、鉛直方向位置や、ロール角、ピッチ角の推定精度を向上できる。これにより、鉛直方向位置や、ロール角、ピッチ角の推定精度の低下を軽減できる。

（２）仮想点群追加部３４は、自車両１の接地部位の位置、自車両１の直下の範囲、自車両１の直下の範囲に所定のマージンを加えた範囲、又は自車両１が占める車幅方向の位置範囲の路面２４の第２位置情報を生成する。
路面２４が平坦である場合には、上記範囲において路面２４の第２位置情報を精度良く生成することができる。この結果、鉛直方向位置や、ロール角、ピッチ角の推定精度を向上できる。

（３）移動量演算部３２は、自車両１の移動量を取得する。点群データ補正部３３は、移動量に基づいて第１位置情報を補正して点群データ格納部３１に蓄積する。位置推定部３５は、点群データ格納部３１に蓄積した第１位置情報及び第２位置情報を地図情報と比較して自車両１の位置を推定する。
これにより、複数回の処理サイクルで取得した第１位置情報を用いて自車両１の位置を推定することが可能になるため、自車両１の位置の推定精度を向上できる。

（４）仮想点群追加部３４は、物体センサ１１により検出された路面２４の第１位置情報が自車両１の位置推定に十分であるか否かを判定し、路面２４の第１位置情報が自車両１の位置推定に十分である場合に、地図情報と比較する第２位置情報を低減する。
これにより、第１位置情報によって十分な精度で自車両１の位置を推定できれば、第２位置情報の生成、及び地図情報と第２位置情報との比較を省略して、処理負荷を軽減できる。

（５）仮想点群追加部３４は、路面の平面度を推定し、平面度が閾値未満の場合に地図情報と比較する第２位置情報を低減する。
これにより、路面２４が平坦であるとの仮定が成立しない個所で生成した第２位置情報による位置推定の精度低下を防止できる。

（６）位置推定部３５は、第１位置情報及び第２位置情報と地図情報の合致度を算出する。仮想点群追加部３４は、合致度が比較的低い場合には、合致度が比較的高い場合よりも、地図情報と比較する第２位置情報を生成する範囲又は密度を小さくする。
第２位置情報と地図情報の合致度が低い個所は、第２位置情報を生成するのに適していないと考えられる。したがって、このような箇所では地図情報と比較する第２位置情報を低減することで、合致度を向上させて位置測定の精度を向上できる。

（７）物体センサ１１は、自車両１の周囲の物体を水平方向に走査して第１位置情報を取得する。
本発明によれば、水平方向に走査する物体センサ１１を用いて、鉛直方向に関する情報が縮退した第１位置情報を取得しても、鉛直方向の自車両１の位置の推定精度や、ロール角、ピッチ角の推定精度の低下を軽減できる。また、鉛直方向の位置精度等の低下に伴う水平方向の位置精度の低下も軽減できる。

１…自車両、１０…位置推定装置、１１…物体センサ、１２…地図データベース、１３…測位装置、１４…車両センサ、１５…コントローラ、１６…プロセッサ、１７…記憶装置、３０…点群データ取得部、３１…点群データ格納部、３３…点群データ補正部、３４…仮想点群追加部、３５…位置推定部

Claims (8)

1. 自車両に搭載されたセンサにより前記自車両の周囲の物体の自車両に対する３次元位置情報である第１位置情報を取得し、
   前記自車両への前記センサの取付位置に基づいて、前記自車両が走行する路面の自車両に対する３次元位置情報である第２位置情報を生成し、
   前記自車両の周囲の物体及び前記自車両が走行する路面の既知の３次元位置情報を有する地図情報と、前記第１位置情報及び前記第２位置情報とを比較して前記自車両の位置を推定する、
   ことを特徴とする位置推定方法。
2. 前記自車両の接地部位の位置、前記自車両の直下の範囲、前記自車両の直下の範囲に所定のマージンを加えた範囲、又は前記自車両が占める車幅方向の位置範囲の路面の前記第２位置情報を生成することを特徴とする請求項１に記載の位置推定方法。
3. 前記自車両の移動量を取得し、
   前記移動量に基づいて前記第１位置情報を補正して蓄積し、
   前記第２位置情報及び蓄積した前記第１位置情報と前記地図情報とを比較して前記自車両の位置を推定する、
   ことを特徴とする請求項１又は２に記載の位置推定方法。
4. 前記センサにより検出された前記路面の前記第１位置情報が前記自車両の位置推定に十分であるか否かを判定し、
   前記路面の前記第１位置情報が前記自車両の位置推定に十分である場合に、前記地図情報と比較する前記第２位置情報を低減する、
   ことを特徴とする請求項１〜３の何れか一項に記載の位置推定方法。
5. 前記路面の平面度を推定し、
   前記平面度が閾値未満の場合に前記地図情報と比較する前記第２位置情報を低減する、
   ことを特徴とする請求項１〜４の何れか一項に記載の位置推定方法。
6. 前記第１位置情報及び前記第２位置情報と前記地図情報の合致度を算出し、
   前記合致度が比較的低い場合には、前記合致度が比較的高い場合よりも、前記地図情報と比較する前記第２位置情報を生成する範囲又は密度を小さくする、
   ことを特徴とする請求項１〜５の何れか一項に記載の位置推定方法。
7. 前記センサは、前記自車両の周囲の物体を水平方向に走査して前記第１位置情報を取得することを特徴とする請求項１〜６の何れか一項に記載の位置推定方法。
8. 自車両に搭載されて、前記自車両の周囲の物体の自車両に対する３次元位置情報である第１位置情報を取得するセンサと、
   前記自車両への前記センサの取付位置に基づいて、前記自車両が走行する路面の自車両に対する３次元位置情報である第２位置情報を生成し、前記自車両の周囲の物体及び前記自車両が走行する路面の既知の３次元位置情報を有する地図情報と、前記第１位置情報及び前記第２位置情報とを比較して前記自車両の位置を推定するコントローラと、
   を備えることを特徴とする位置推定装置。

Images