JP7373448B2 - 自己位置推定方法および自己位置推定装置 - Google Patents

Description

本発明は、自己位置推定方法および自己位置推定装置に関する。

特許文献１には、自車両の位置（自己位置）を推定する方法が開示されている。当該方法では、ＬＲＦ（レーザレンジファインダ）を用いて検出したポイントクラウドと、カメラを用いて取得した色情報とに基づいて目印物標（ランドマーク）を検出し、地図情報とマッチングすることにより自己位置を推定する。この推定方法では、色情報を用いることによって、検出したランドマークが地図情報に含まれる他のランドマークに誤ってマッチングされることを抑制して、自己位置推定精度を向上させている。

特開２０１３－２５７７４２号公報

しかしながら、色情報を加味することでランドマークのマッチング精度が高められたとしても、局所解に陥る等により、検出したランドマークが地図情報に含まれる他のランドマークに誤ってマッチングされる可能性がある。この場合、検出したランドマークが地図上の他のランドマークに誤ってマッチングされていたとしても、マッチング自体は行われているので、マッチング結果に基づいて自己位置の推定を行う際に、自己位置の推定が実質的に失敗することを予測することが困難である。

本発明は、マッチング結果に基づいた自己位置の推定が失敗することを予測することができる技術を提供することを目的とする。

本発明の一実施形態による自己位置推定方法は、自車両周辺の障害物の位置を所定範囲毎に設定されたグリッド毎に検出する障害物位置検出ステップと、検出された障害物の位置と、少なくとも障害物の位置が記憶された地図情報とをマッチングし、当該マッチングの結果に基づいて自己位置を推定する自己位置推定ステップと、検出された障害物の位置と地図情報に記憶された障害物の位置との一致度をグリッド毎に算出する一致度算出ステップと、を含み、算出された一致度のグリッド間の相関関係に基づいて、自己位置を正しく推定することができなくなることを予測する失敗予測ステップを実行する。

本発明によれば、検出された障害物の位置と地図情報に記憶された障害物の位置との一致度をグリッド毎に算出することにより、当該グリッド間の相関関係に基づいて自己位置の推定が失敗することを予測することができる。

図１は、位置実施形態の自己位置推定装置の概略構成図である。 図２は、ＮＤＴマッチングによる自己位置推定方法を説明する図であって、特にＮＤＴマッチング開始時の状態を示す図である。 図３は、ＮＤＴマッチングによる自己位置推定方法を説明する図であって、特にＮＤＴマッチング終了時の状態を示す図である。 図４は、マハラノビス距離マップとマスクとを説明する図である。 図５は、識別器に対する学習処理に用いる訓練データの収集方法を説明する図である。 図６は、一実施形態の識別器の構成を説明する図である。 図７は、一実施形態の失敗猶予時間算出処理を説明するフローチャートである。

［第１実施形態］
図１は、本発明の第１実施形態に係る自己位置推定方法を実現する自己位置推定装置１０を説明する図である。本実施形態の自己位置推定装置１０は、車両に搭載され、当該車両の現在位置を推定する。自己位置推定装置１０が適用された車両を以下では「自車両」と称する。

図１に示すように、自己位置推定装置１０は、後述する自己位置推定部１１、一致度算出部１２、地図情報記憶部１３、失敗予測部１４、識別器１４ａ、報知部１５等の機能部を有するコントローラ１と、物体検出手段２と、角速度センサ（ヨーレートセンサ）３と、車速センサ４と、を含んで構成される。コントローラ１が有する各機能部の詳細については後述する。

物体検出手段２は、自車両の周囲環境に存在する物体（障害物）の位置を検出する。物体検出手段２は、例えば、センサ（ライダ（ＬｉＤＡＲ）、レーダ）又は、カメラ（ステレオカメラ）である。本実施形態の物体検出手段２はライダにより構成されるものとし、以下では物体検出手段２をライダ２とも称する。ライダ２は、車両に搭載され、自車両の周囲にレーザを照射し、その反射光を捉えることにより自車両周辺に存在する物体の３次元点群データ（ポイントクラウド）を取得する。このポイントクラウドは、対象物体の３次元位置を表すポイントの集合であって、自車両周辺に存在する物体までの距離と、自車両からみた当該物体が存在する方角とを
検出することができる。

角速度センサ（ヨーレートセンサ）３は、自車両の状態（車両状態）を検出する手段として構成され、自車両の旋回時等におけるヨー角の変化の割合（ヨーレート）を示す角速度を検出する。

車速センサ４は、車両状態を検出する手段として構成され、自車両の車速を検出する。

コントローラ（コンピュータ）１は、例えば、中央演算装置（ＣＰＵ）、読み出し専用メモリ（ＲＯＭ）、ランダムアクセスメモリ（ＲＡＭ）、および、入出力インタフェース（Ｉ／Ｏインタフェース）から構成される。また、コントローラ１が備えるＲＯＭには、以下に説明する各機能部がそれぞれに有する各機能を実行するためのプログラムが格納されている。換言すれば、コントローラ１は、記憶媒体として備わるＲＯＭに格納された各種プログラムを実行することによって、以下に説明する自己位置推定部１１、一致度算出部１２、失敗予測部１４等の各機能部の機能を実現するように構成される。ただし、各機能部のすべての機能は、必ずしもコントローラ１によって実現される必要はなく、機能部毎に適宜選択された複数のコントローラによって実現されるように構成されてもよい。また、ＲＯＭ及びＲＡＭに係る記憶媒体は、後述の地図情報等を記憶する地図情報記憶部１３としても機能する。また、当該記憶媒体には、後述する識別器１４ａ等が記憶されてもよい。

なお、識別器１４ａや地図情報等が記憶される記憶媒体は、必ずしもコントローラ１が備える記憶媒体として予め構成される必要はない。例えば、自己位置推定装置１０が適用される車両の外部における任意の場所に設置されたサーバ（又は、インターネットのクラウド上に設けられたクラウドサーバ）に設けられてもよい。この場合には、コントローラ１は、不図示のネットワーク回線等を介して当該サーバにアクセスして、サーバに格納された地図情報等を無線通信により適時に取得可能に構成されてよい。

自己位置推定部１１は、少なくとも物体検出手段２が検出した自車両周辺に存在する物体の自車両に対する相対位置に基づいて、走行中または停車中の自車両の位置（自車両の位置を以下では「自己位置」と称する）を推定する自己位置推定ステップを実行する。自己位置推定の手法としては、スキャンマッチングやマップマッチング等の公知の手法が用いられてよい。本実施形態では、スキャンマッチングのアルゴリズムを利用するものとする。また、本実施形態の自己位置推定部１１は、角速度センサ３が検出した角速度と車速センサ４が検出した車速とに基づくいわゆるオドメトリによる自己位置推定手法とスキャンマッチング等とを組み合わせて自己位置を推定するように構成されてもよい。

自己位置推定部１１が行うスキャンマッチングの具体的な手法としてはＩＣＰ（Iterative Closest Point）マッチング、ＮＤＴ（Normal Distribution Transform）マッチング等があるが、本実施形態では、ＮＤＴマッチングを用いる例について説明する。以下、図２から図４を参照して、自己位置推定部１１による自己位置推定方法について説明する。

ＮＤＴマッチングを実行するにあたって、自己位置推定装置１０は、まず事前の準備として、車両が走行し得る環境（周囲環境）に存在する構造物をライダ（ＬｉＤＡＲ）等でスキャンすることにより得たポイントクラウドに基づいて作成された地図情報を取得しておく必要がある。当該地図情報は、自己位置推定装置１０が自己位置を推定する際に参照される地図情報である。

図２は、ＮＤＴマッチングによる特に開始時点における自己位置推定方法を説明する図である。

図示された全体を３×５の計１５領域に区画する白抜きの矩形領域は、所定範囲毎に設定されたグリッドである。本実施形態における一つのグリッドは、周囲環境における１ｍ×１ｍの領域に対応するように設定されている。ただし、グリッドの数、及び、一つのグリッドに対応する周囲環境の大きさは適宜設定されてよい。また、図示するグリッド毎に配置された様々な形状の楕円（正規分布２０１）は、ＮＤＴマッチングを開始する事前にライダ等でスキャンすることにより得たポイントクラウドの分布を正規分布で表現したものである。本実施形態では、このグリッドと、グリッド毎に配置された正規分布２０１とを含む地図情報が、自己位置推定部１１による自己位置推定を開始する前に地図情報記憶部１３に予め記憶されている。なお、地図情報記憶部１３に事前に記憶された当該地図情報を以下では「基本地図」とも称する。

そして、図２でスキャンポイント２００として示される複数の矩形形状部は、自己位置推定部１１によるＮＤＴマッチング開始時に、車両位置２０２に存在する自車両がライダ２を用いて検出したポイントクラウドである。すなわち、図２で示されるのは、車両位置２０２に存在する自車両が検出したスキャンポイント２００が基本地図上に投影された図である。また、図においてスキャンポイント２００と正規分布２０１とが線で結ばれているように、スキャンポイント２００は、最近傍のグリッドに配置された正規分布２０１に対応づけられる。

なお、ＮＤＴマッチング開始時の初期位置である車両位置２０２は、角速度センサ３が検出した角速度と車速センサ４が検出した車速とに基づくいわゆるオドメトリによる公知の自己位置推定手法を用いて適宜修正されてよい。

そして、自己位置推定部１１は、ＮＤＴマッチングによって、対応づけられたスキャンポイント２００と正規分布２０１との間のマハラノビス距離を最小化するようにして車両位置２０２を修正することで、図３に示す車両位置２０３を得ることができる。

図３は、ＮＤＴマッチングによる自己位置推定方法を説明する図であって、特にＮＤＴマッチング終了時の状態を示す図である。自己位置推定部１１は、全グリッドにおけるスキャンポイント２００と正規分布２０１との間のマハラノビス距離の総和を最小化して最適化することにより、自車両の位置を車両位置２０２から車両位置２０３に修正することができる。図中のスキャンポイント２００に付された数字は、最適化完了時点における各スキャンポイント２００のマハラノビス距離を示している。ただし、当該数字は、単にスキャンポイント２００と正規分布２０１との間の絶対的な距離を示す数字である必要は必ずしもなく、所定条件に基づいて重みづけがされた数字であってもよい。例えば、車両の進行方向に平行な方向における距離よりも、車両の進行方向に垂直な方向における距離の方がより長く評価されるなどしてもよい。また、図示されているマハラノビス距離を示す数字は自然数のみであるが、これは図を簡素化するために選択されたにすぎない。マハラノビス距離は、実際には小数点以下の数字を含んでいてもよいし、例えば０～１の数字範囲の中であらわされてもよい。なお、マハラノビス距離の最小化に基づくＮＤＴマッチングの詳細については以下の非特許文献に開示される。

非特許文献：P. Biber and W. Strasser, "The normal distributions transform: a new approach to laser scan matching," Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003), 2003, pp. 2743-2748 vol.3.

ただし、本実施形態の自己位置推定部１１は、スキャンマッチングを実行するにあたって必ずしもＮＤＴマッチングを行う必要はなく、例えば、ＩＣＴマッチングを行ってもよい。また、必ずしもマハラノビス距離を使用する必要はなく、例えば、ユークリッド距離を使用してもよい。

なお、自己位置推定装置１０が適用される車両は、自己位置推定部１１が検出した自己位置と設定された目的地とに基づいて自己位置から目的地まで走行する予定の経路（走行経路）を設定し、設定された走行経路に沿って車両が走行することを支援する走行支援ステップを実行するように構成されてよい。走行支援の手法は特に限定されず、公知の手法が採用されてよい。例えば、走行支援ステップでは、上記の自己位置と目的地とに基づき、自車両が走行する車線の車線境界データと地図座標系の道路構造データとを統合した統合道路構造データを用いて自車両の走行経路を設定し、設定した走行経路に沿って走行するように自車両を制御してもよい。すなわち、走行支援ステップは、自車両のアクセル、ブレーキ及びステアリングが自動的に制御されるいわゆる自動走行を実現するものであり、乗員は当該車両に乗った状態で、少なくとも自己位置推定部１１による自己位置推定が成功している間はハンドルから手を離した状態で目的地まで移動することが可能となる。なお、目的地とは車両の乗員によって設定される最終目的地、最終目的地までの経路上の地点、自車両前方所定距離の前方注視点等の、車両が到達すべき地点を意味する。

次に、一致度算出部１２について説明する。一致度算出部１２は、自己位置推定部１１による自己位置推定結果に基づいて、スキャンポイント２００と正規分布２０１との一致度をグリッド毎に算出する。本実施形態の一致度は、グリッド毎のマハラノビス距離の平均値であらわされ、値が小さいほどよく一致している（一致度が高い）と判断されてよい。また、一致度算出部１２は、当該一致度を算出するにあたって、マハラノビス距離マップ３０１とマスク３０２とを生成する。

図４は、マハラノビス距離マップ３０１とマスク３０２とを説明する図である。

図４（ａ）は、グリッド毎の一致度を示す図（マハラノビス距離マップ３０１）である。図示するように、スキャンポイント２００と正規分布２０１との一致度はグリッド毎に算出される。例えば、マハラノビス距離マップ３０１の左端上段のグリッドの一致度を示す数字１．５は、対応する図３の左端上段のグリッド内の４つのスキャンポイント２００のマハラノビス距離の平均値を算出することにより求められる。なお、図示する数字は小数点以下を四捨五入した数である。

さらに、一致度算出部１２は、図３で示すＮＤＴマッチング終了時の状態を示す地図情報に基づいて、グリッド毎のスキャンポイント２００の有無を示すマスク３０２を生成する。

図４（ｂ）は、マスク３０２を説明する図である。マスク３０２は、図３で示すＮＤＴマッチング終了時の状態を示す地図情報、および、マハラノビス距離マップ３０１に対応して、当該地図情報及びマハラノビス距離マップ３０１と同サイズのマスクとして生成される。マスク３０２は、スキャンポイント２００が１点も含まれていないグリッドと、１点以上含まれているグリッドとを選別して、グリッド毎に前者には０を後者には１を割り当てることで生成される。すなわち、図４（ｂ）で示すマスク３０２では、スキャンポイント２００が１点も存在しないグリッドには０が示され、スキャンポイント２００が１点以上存在しているグリッドには１が示されている。

ここで、マスク３０２を生成する理由を説明する。マハラノビス距離マップ３０１では、自車両の周囲環境において障害物が検出されない領域、すなわち最適化されたスキャンポイント２００が一つも検出されない領域に対応するグリッドの一致度は最も高く評価される。例えば、マハラノビス距離が０以上の数字で表されている場合には、障害物が検出されない領域に対応するグリッドの一致度は０となる。一方で、障害物が検出された領域において、スキャンポイント２００と正規分布２０１とが良好に一致している領域に対応するグリッドの一致度は当然高く評価される。そうすると、スキャンポイント２００と正規分布２０１とが一致している領域に対応するグリッドの一致度は、障害物が存在しない領域と同様に０となり得る。すなわち、マハラノビス距離マップ３０１のみの情報では、障害物が無い領域に対応するグリッドの一致度と、スキャンポイント２００と正規分布２０１とが一致している領域に対応するグリッドの一致度は双方とも０となるため、両者を区別することができない。そこで、これらを区別するために、最適化されたスキャンポイント２００が存在しない領域に対応するグリッドと、最適化されたスキャンポイント２００が存在する領域に対応するグリッドとを区分けするマスク３０２が生成される。そして、マハラノビス距離マップ３０１にマスク３０２を適用することにより、障害物が検出されない領域に対応するグリッドと、スキャンポイント２００と正規分布２０１とが一致している領域（障害物を正確に検出した領域）に対応するグリッドとが判別されたマハラノビス距離マップを得ることができる。

ただし、マハラノビス距離マップ３０１にマスク３０２を必ず適用させる必要はない。失敗予測時間の予測精度を担保する観点から許容できるのであれば、マスク３０２を生成せず、マハラノビス距離マップ３０１のみに基づいて失敗猶予時間を予測してもよい。本実施形態では、失敗猶予時間の予測精度を高めることを優先して、一致度算出部１２が生成するマハラノビス距離マップ３０１と、当該マハラノビス距離マップ３０１に適用されるマスク３０２とのペアを用いて失敗猶予時間を予測するものとし、当該ペアを、以下では「マスク済みマップ」と称する。マスク済みマップを用いることによって、マハラノビス距離マップ３０１のグリッドのうち、マスク３０２において１が付されたグリッドに対応するグリッドの値のみに基づいて失敗猶予時間を算出することが可能となる。

そして、失敗予測部１４は、一致度算出部１２が生成したマスク済みマップから、自己位置を正しく検出することができなくなること（自己位置の推定が失敗すること）を予測するとともに、自己位置を正しく検出することができなくなるまでにかかる時間（当該時間を以下では「失敗猶予時間」とも称する）を予測する。本実施形態の失敗予測部１４は、学習済みの識別器１４ａを用いて失敗猶予時間を予測するように構成される。なお、識別器１４ａは、識別器という名に代えて、学習済みモデル、分類器、又は、ニューラルネットワークシステム等と称されてもよい。

識別器１４ａは、深層学習（Deep Neural Network）による機械学習手法を用いて学習処理された学習モデル（学習済みモデル）である。本実施形態の識別器１４ａは、コントローラ１が備える記憶媒体に予め記憶される。識別器１４ａの構成、及び生成方法については、図５、図６を参照して後述する。

報知部１５（図１参照）は、少なくとも自車両の乗員に、失敗予測部１４が予測した失敗猶予時間を音声又は映像等で報知する。報知内容は、失敗猶予時間そのものでもよいし、失敗猶予時間の長さに応じて選択されるブザー音、または「至急」等の行動を促す文字情報等でもよい。これにより、乗員は、将来自己位置推定が失敗することを失敗する前に知得することができる。その結果、乗員は、例えばハンドルから手を離した状態で自動走行している時にこのような報知を受けた場合に、自己位置推定の失敗によって自動走行の安全性が低下する前に例えばハンドルを握る等して手動運転に切り替えて、走行の安全性を確保することができる。

以上が本実施形態の自己位置推定装置１０の構成の詳細である。続いて、識別器１４ａの構成および生成方法について説明する。

図５は、識別器１４ａの生成方法の概要を説明する図であって、特に識別器１４ａに対する学習処理に用いる訓練データの収集方法を説明する図である。本実施形態における識別器１４ａを生成するためには、まず、学習処理を行うための訓練データを大量に収集し蓄積する必要がある。本実施形態では、当該訓練データをシミュレーションによって収集する。ただし、訓練データは必ずしもシミュレーションでのみ収集可能なものではなく、実際の車両を走行させて収集することもできる。しかしながら、実際の車両を走行させる場合は、訓練データを収集するためのコストも時間もかかるため、シミュレーションで収集するほうが効率的である。なお、図５で示す各機能部は、自車両が備える上述のコントローラ１が備えていてもよいし、他のコントローラ、例えば、ＣＰＵ等を備え、必要なシミュレーション環境が実現できる一般的なコントローラ（コンピュータ）が備えていてもよい。本実施形態の識別器１４ａは、コントローラ１によって生成されるものとして以下説明する。

図示するとおり、コントローラ１は、地図情報記憶部１００と、物体検出値出力部１０１と、角速度センサ検出値出力部１０２と、車速センサ検出値出力部１０３と、ノイズ印加部１０４と、自己位置推定部１０５と、一致度算出部１０６と、成否判断部１０７と、失敗時間記録部１０８と、時系列変化記憶部１０９と、を含んで構成される。

地図情報記憶部１００は、上述の地図情報記憶部１３と同様に、基本地図を記憶する。

物体検出値出力部１０１は、物体検出手段２の検出値をシミュレーションするための機能部であって、物体検出手段２が検出し得る検出値を模擬した値である障害物センサ検出値を自己位置推定部１０５に出力する。本実施形態では、例えば、ライダ２が検出し得るポイントクラウドを出力するように構成される。

角速度センサ検出値出力部１０２は、角速度センサ３が検出する自車両の角速度をシミュレーションするための機能部であって、角速度センサ３が検出し得る角速度を模擬した値を車両状態センサ検出値の一つとして自己位置推定部１０５に出力する。

車速センサ検出値出力部１０３は、車速センサ４が検出する自車両の車速をシミュレーションするための機能部であって、車速センサ４が検出し得る車速を模擬した値を車両状態センサ検出値の一つとして自己位置推定部１０５に出力する。

ノイズ印加部１０４は、自己位置推定部１０５による自己位置推定の失敗を促すための機能部であって、物体検出値出力部１０１が出力するポイントクラウド、角速度センサ検出値出力部１０２が出力する角速度、および、車速センサ検出値出力部１０３が出力する車速の少なくともいずれか一つにノイズを印加する。ノイズ印加部は、例えば自己位置推定部１０５による自己位置推定結果（車両位置２０３）にではなく、車両位置２０３を導出する根拠の一つとなるセンサ検出値にノイズを印加することで、各センサ検出値と自己位置推定結果との因果関係を維持することができるので、後述する学習処理によって生成される識別器１４ａの失敗時間予測精度を効率よく高めることが可能となる。なお、シミュレーション時に印加されるノイズパターンは、実際に車両が行う自己位置推定の際に想定されるノイズ要素を網羅するように設定されたものでもよいし、ランダムなものであってもよい。

自己位置推定部１０５は、地図情報記憶部１００に記憶された基本地図と、物体検出値出力部１０１が出力するポイントクラウド、角速度センサ検出値出力部１０２が出力する角速度、および、車速センサ検出値出力部１０３が出力する車速とに基づいて、自車両の自己位置推定をシミュレーションする。自己位置推定部１０５の機能は、入力されるポイントクラウド、角速度、および、車速がシミュレーションにより生成された値である点を除いて、上述した自己位置推定部１１と同様である。

一致度算出部１０６は、上述の一致度算出部１２と同様に、自己位置推定部１０５による自己位置推定結果に基づいて、マハラノビス距離マップ３０１とマスク３０２とのペアにかかるマスク済みマップを生成する。

成否判断部１０７は、自己位置推定部１０５が推定した自己位置（車両位置２０３を参照）が正しい位置であるか否か、換言すれば、自己位置推定が成功したか失敗したかを判断する。成否判断部１０７は、自己位置推定部１０５が推定した車両位置２０３と、予め記憶された正解位置とを比較して、両者の乖離が予め設定された閾値以上になった場合に失敗、閾値未満の場合は成功と判断する。なおここで用いられる正解位置は、ノイズ印加部１０４によるノイズを印加せずにシミュレーションすることにより容易に生成することができる。

失敗時間記録部１０８は、成否判断部１０７による成否判断結果と、一致度算出部１０６が生成したマスク済みマップとに基づいて、成否判断部１０７の成否判断結果と、成否判断がなされたマスク済みマップとが紐づけられた時系列変化データ群を生成する。より詳細には、失敗時間記録部１０８は、成否判断部１０７が失敗と判断した時のマスク済みマップに失敗時間＝０を付与したデータと、失敗時間＝０が付与されたデータの一時刻前のマスク済みマップに失敗時間＝１を付与したデータと、失敗時間＝１が付与されたデータの一時刻前のマスク済みマップに失敗時間＝２を付与したデータと、以下、同様に、Ｎ時刻前のマスク済みマップに失敗時間＝Ｎを付与したデータとからなる時系列データ群を生成して、時系列変化記憶部１０９に記憶する。なお、ここでの一時刻は、自己位置推定部１０５が車両位置２０３を検出する周期（更新周期）に応じた時間が設定されてよい。また、自己位置推定部１０５による車両位置２０３の更新周期は、コントローラ１の処理速度、ライダ２によるスキャンポイント２００の検出周期（サンプリング周波数）等を考慮して適宜設定されてよい。また、Ｎ時刻前から失敗時間＝０までにかかる時間幅も適宜設定されてよく、例えば１０秒程度であってよい。

このような時系列データ群が生成されることにより、ノイズ印加部１０４が印加したノイズに起因して自己位置推定が失敗した場合に、自己位置推定が失敗した時点（失敗時間＝０）におけるマスク済みマップに対してＮ時刻前（失敗時間＝Ｎ）からのマスク済みマップの変化の過程を記録することができる。そして、自己位置推定のシミュレーションをノイズ印加部１０４が印加するノイズパターンを変化させながら多数繰り返すことにより、失敗時間＝０におけるマスク済みマップに対するＮ時刻前からのマスク済みマップの時系列変化パターンを大量に取得することができる。

また、大量の時系列変化パターンが蓄積されることによって、失敗時間＝０に至るまでのマスク済みマップの時系列変化パターン群に、一又は時系列に並んだ複数の新規のマスク済みマップを照合することで当該新規のマスク済みマップが生成されたタイミングからの失敗猶予時間を算出することが可能となる。例えば、時系列に並ぶ第１～第３のマスク済みマップと時系列変化パターン群とを照合した結果、第１～第３のマスク済みマップと失敗時間＝Ｎ～Ｎ－２が付与されたデータとが一致する場合には、第１のマスク済みマップが生成されたタイミングからＮ時刻後に自己位置推定が失敗すると予測することができる。

そして、このようにして得られる失敗猶予時間は、ある一つ、又は時系列に並んだ複数の新規のマスク済みマップに対応する失敗猶予時間の解として、識別器１４ａに学習処理を施す際の教師データとして利用することができる。

そして、時系列変化記憶部１０９に記憶された大量の時系列変化パターン群を訓練データとして活用し、例えば深層学習（Deep Neural Network）等の機械学習手法を適用して機械学習させることによって学習済みモデルとしての識別器１４ａが生成される。

図６は、本実施形態の識別器１４ａを説明する図である。図示するように、識別器１４ａは、マハラノビス距離マップ３０１およびマスク３０２に係るマスク済みマップを入力とし、失敗猶予時間を出力するように構成される。また、識別器１４ａは、時系列処理が可能な中間層を有する。識別器１４ａを構成する中間層は適宜選択されてよく、例えば時系列処理が可能な回帰結合（再帰構造）を持ついわゆるリカレントニューラルネットワーク（Recurrent Neural Network）により構成されてよい。本実施形態の識別器１４ａは、中間層のニューラルネットワークアルゴリズムとして、畳み込みニューラルネットワークとＬＳＴＭ（Long Short-Term Memory）とを組み合わせたｃｏｎｖＬＳＴＭ（Convolutional Long Short-Term Memory）が採用される。

このように構成された識別器１４ａは、上述の訓練データおよび教師データに基づく学習処理が施されることによって、中間層を構成する各ノードの重みづけが最適化される。その結果、識別器１４ａは、マスク済みマップのグリッド間の空間的及び時間的な相関関係を捉えて、入力されるマスク済みマップに対して想定される失敗猶予時間を出力することが可能となる。なお、グリッド間の空間的及び時間的な相関関係とは、時系列に並ぶ複数のマスク済みマップから導き出せる相関関係であって、空間的に隣接するグリッド間のそれぞれの一致度の時間的変化の相関関係を示す。

すなわち、図６で示す識別器１４ａは、マスク済みマップに基づいて失敗猶予時間を出力するようにコントローラ１を機能させるための学習済みモデルであって、マスク済みマップが時系列に入力される入力層と、失敗猶予時間を出力する出力層と、時系列に並ぶ複数のマスク済みマップに対する失敗猶予時間を教師データとして重み付け係数が最適化されたニューラルネットワークを備える中間層と、から構成される学習済みモデルである。

このようにして識別器１４ａが生成されると、識別器１４ａは、自己位置推定装置１０の失敗予測部１４の一機能部として、コントローラ１が備える記憶媒体に学習済みモデルとして記憶される。

以上が本実施形態の識別器１４ａを備える自己位置推定装置１０の構成である。以下では、自己位置推定装置１０による自己位置推定方法の流れについて図７を参照して説明する。

図７は、本実施形態の自己位置推定装置１０が実行する失敗猶予時間算出処理を説明するフローチャートである。当該フローチャートで説明される処理は、車両が起動している間、一定の間隔で常時実行されるようにコントローラ１にプログラムされている。

ステップＳ１では、コントローラ１（自己位置推定部１１）が自己位置を推定する。具体的には、コントローラ１は、まず、自車両周辺の障害物の自車両に対する相対位置を示すスキャンポイント２００を所定範囲毎に設定されたグリッド毎に検出する障害物位置検出ステップを実行する。なお、障害物位置検出ステップはコントローラ１の不図示の機能部である障害物位置検出部により実行されてよい。そして、検出されたスキャンポイント２００と、正規分布２０１を含む基本地図とをマッチングすることによって自車両の位置（自己位置）を示す車両位置２０３を推定する自己位置推定ステップを実行する。当該マッチングの手法は特に限定しないが、本実施形態では、上述のとおりＮＤＴマッチングが採用されてよい。ＮＤＴマッチングが終了すると、一致度を算出するために続くステップＳ２の処理が実行される。

ステップＳ２では、コントローラ１（一致度算出部１２）が、自己位置推定部１１が行ったマッチング結果に基づいて、スキャンポイント２００と正規分布２０１との一致度をグリッド毎に算出する一致度算出ステップを実行する。本実施形態の一致度は、上述したとおり、グリッド毎のマハラノビス距離の平均値が示されたマハラノビス距離マップ３０１として生成される。

また、ステップＳ２では、当該グリッドのうち障害物が検出されたグリッドと障害物が検出されなかったグリッドとを区別するためのマスク３０２を生成するマスク生成ステップが実行される。一致度算出部１２によりマハラノビス距離マップ３０１とマスク３０２とが生成されると、続くステップＳ３の処理が実行される。

ステップＳ３では、コントローラ１（失敗予測部１４）が、グリッド毎に算出された一致度の当該グリッド間の相関関係に基づいて、自己位置を正しく推定することができなくなるまでにかかる猶予時間（失敗猶予時間）を予測する失敗予測ステップを実行する。本実施形態の失敗猶予時間は、予め記憶された識別器１４ａを用いて算出される。すなわち、本実施形態の失敗予測部１４は、ステップＳ２で生成されたマハラノビス距離マップ３０１とマスク３０２とに係るマスク済みマップを識別器１４ａに入力し、識別器１４ａから出力される値を失敗猶予時間として算出するように構成される。従って、コントローラ１は、ステップＳ２で生成されたマハラノビス距離マップ３０１とマスク３０２とを識別器１４ａ入力し、識別器１４ａから失敗猶予時間が出力された場合には、出力された失敗猶予時間を乗員に報知するためにステップＳ４の処理を実行する。他方、ステップＳ２で生成されたマハラノビス距離マップ３０１とマスク３０２の入力に対して識別器１４ａから適切な失敗猶予時間が出力されない場合には、失敗が予測されるほどには自己位置推定の精度が悪化していないと判断して失敗猶予時間算出処理を終了する。

ただし、失敗予測部１４は、必ずしも識別器１４ａを用いて失敗猶予時間を予測するように構成される必要はない。失敗予測部１４は、識別器１４ａに代えて、識別器１４ａに対する訓練データとして大量に蓄積した上述の時系列変化パターン群に基づいて失敗猶予時間を予測するように構成されてもよい。より具体的には、コントローラ１は、識別器１４ａに代えて、マスク済みマップと失敗猶予時間とが対応付けられたマップを記憶する記憶媒体（データベース）を備え、当該マップを参照して失敗猶予時間を算出するように構成されてもよい。なお、当該マップは、訓練データとして収集された上述の時系列変化パターン群に基づいて作成される。この場合、失敗予測部１４は、一致度算出ステップで得た一致度のグリッド間の相関関係を示すマスク済みマップの時系列変化と当該データベースに格納されたマップとを照合することにより失敗猶予時間を算出するように構成されてよい。

ステップＳ４では、コントローラ１（報知部１５）が、失敗猶予時間を乗員に報知する。これにより、乗員は、自己位置推定の失敗によって自動走行の安全性が低下する前に、自己位置推定が失敗するまでの猶予時間を知得することができる。当該報知が行われると、失敗猶予時間算出処理は終了する。

以上、一実施形態の走行支援方法によれば、自車両周辺の障害物の位置を所定範囲毎に設定されたグリッド毎に検出する障害物位置検出ステップと、検出された障害物の位置と、少なくとも障害物の位置が記憶された地図情報（基本地図）とをマッチングし、当該マッチングの結果に基づいて自己位置を推定する自己位置推定ステップと、検出された障害物の位置と地図情報に記憶された障害物の位置との一致度をグリッド毎に算出する一致度算出ステップと、を含み、算出された一致度のグリッド間の相関関係に基づいて、自己位置を正しく推定することができなくなることを予測する失敗予測ステップを実行する。これにより、自己位置推定ステップにて推定される自己位置が正しく推定することができなくなることを予測することができる。より具体的には、失敗予測ステップは、算出された一致度のグリッド間の相関関係の時系列変化に基づいて、自己位置を正しく推定することができなくなるまでの時間である猶予時間（失敗猶予時間）を予測する。これにより、自己位置推定ステップにて推定される自己位置が正しく推定することができなくなるまでにかかる時間を、自己位置推定が失敗する前に予測することができる。

また、一実施形態の走行支援方法によれば、失敗予測ステップでは、学習済みの識別器１４ａに対して、算出された一致度のグリッド間の相関関係（マハラノビス距離マップ３０１）を時系列に入力することにより失敗猶予時間を予測し、識別器１４ａは、一致度のグリッド間の相関関係（マハラノビス距離マップ３０１）が時系列に入力された際に失敗猶予時間を出力するように、教師データを用いた学習処理が施された学習済みモデルである。これにより、学習済みモデルである識別器１４ａを用いてマハラノビス距離マップ３０１の時系列変化から失敗猶予時間を算出することができる。

また、一実施形態の走行支援方法によれば、訓練データは、（シミュレーションにより、）自車両周辺の障害物の位置を示す障害物センサ検出値と、自車両の状態を示す車両状態センサ検出値とに基づく障害物位置検出ステップを実行し、自己位置推定ステップを実行し、一致度算出ステップを実行し、障害物センサ検出値と車両状態センサ検出値とにノイズを印加するノイズ印加ステップを実行し、ノイズが印加されたことに起因して自己位置を正しく推定することができなくなった時に、自己位置が正しく推定できなくなるまでの一致度のグリッド間の相関関係（マハラノビス距離マップ３０１）の時系列変化データ群を記憶する時系列変化記憶ステップを実行することにより収集される。このように、例えば自己位置推定部１０５による自己位置推定結果（車両位置２０３）にではなく、各センサ検出値にノイズが印加されることで、各センサ検出値と自己位置推定結果との因果関係を維持することができるので、識別器１４ａの失敗時間予測精度を効率よく高めることができる訓練データを大量に収集することが可能となる。

また、一実施形態の走行支援方法によれば、教師データは、一致度算出ステップにおいて算出された一致度のグリッド間の相関関係（マハラノビス距離マップ３０１）の時系列変化に対する失敗猶予時間であって、時系列変化記憶ステップにおいて収集された時系列変化データ群と、一致度算出ステップにおいて算出された一致度のグリッド間の相関関係（マハラノビス距離マップ３０１）の時系列変化とを照合することにより算出される。これにより、教師データに基づく学習処理によって、学習済みモデルである識別器１４ａを生成することができる。

また、一実施形態の走行支援方法によれば、失敗予測ステップでは、シミュレーションの結果として予め記憶された自己位置が正しく推定できなくなるまでの一致度のグリッド間の相関関係の時系列変化データ群と、一致度算出ステップで得た一致度のグリッド間の相関関係（マハラノビス距離マップ３０１）の時系列変化とを照合することにより前記猶予時間を予測する。これにより、識別器１４ａを備えなくとも、マハラノビス距離マップ３０１に基づいて失敗猶予時間を算出することができる。

また、一実施形態の走行支援方法によれば、グリッドのうち、障害物が検出されないグリッドと障害物が検出されたグリッドとを区別するマスクを生成するマスク生成ステップをさらに備え、失敗予測ステップでは、マスクにより区別された障害物が検出されたグリッド間の一致度の相関関係に基づいて、失敗猶予時間を予測する。これにより、障害物が検出されない領域に対応するグリッドと、スキャンポイント２００と正規分布２０１とが一致している領域（障害物を正確に検出した領域）に対応するグリッドとが判別されたマハラノビス距離マップに基づいて失敗猶予時間が算出されるので、失敗猶予時間の予測精度を向上させることができる。

また、一実施形態の走行支援方法によれば、失敗予測ステップにおいて予測された失敗猶予時間を報知する報知ステップをさらに含む。これにより、乗員は、将来（例えば何秒後かに）自己位置推定が失敗することを、失敗する前に知得することができる。

以上、本発明の実施形態について説明したが、上記実施形態は本発明の適用例の一部を示したに過ぎず、本発明の技術的範囲を上記実施形態の具体的構成に限定する趣旨ではない。

１…コントローラ（障害物位置検出部、自己位置推定部、一致度算出部、失敗予測部）
１１…自己位置推定部
１２…一致度算出部
１４…失敗予測部
１４ａ…識別器
１５…報知部

Claims (9)

1. 自車両周辺の障害物の位置を所定範囲毎に設定されたグリッド毎に検出する障害物位置検出ステップと、
   検出された前記障害物の位置と、少なくとも前記障害物の位置が記憶された地図情報とをマッチングし、当該マッチングの結果に基づいて自己位置を推定する自己位置推定ステップと、
   検出された前記障害物の位置と前記地図情報に記憶された前記障害物の位置との一致度を前記グリッド毎に算出する一致度算出ステップと、
   算出された前記一致度の前記グリッド間の相関関係に基づいて、前記自己位置を正しく推定することができなくなることを予測する失敗予測ステップと、を含む、
   自己位置推定方法。
2. 請求項１に記載の自己位置推定方法であって、
   前記失敗予測ステップは、前記一致度の前記グリッド間の相関関係の時系列変化に基づいて、前記自己位置を正しく推定することができなくなるまでの時間である猶予時間を算出する、
   自己位置推定方法。
3. 請求項２に記載の自己位置推定方法であって、
   前記失敗予測ステップでは、
   学習済みの識別器に対して、算出された前記一致度の前記グリッド間の相関関係を時系列に入力することにより前記猶予時間を予測し、
   前記識別器は、前記一致度の前記グリッド間の相関関係が時系列に入力された際に前記猶予時間を出力するように、教師データを用いた学習処理が施された学習済みモデルである、
   自己位置推定方法。
4. 請求項３に記載の自己位置推定方法であって、
   前記学習処理は、シミュレーションによって、
   前記自車両周辺の障害物の位置を示す障害物センサ検出値と、自車両の状態を示す車両状態センサ検出値とに基づく前記障害物位置検出ステップを実行し、
   前記自己位置推定ステップを実行し、
   前記一致度算出ステップを実行し、
   前記障害物センサ検出値と前記車両状態センサ検出値とにノイズを印加するノイズ印加ステップを実行し、
   前記ノイズが印加されたことに起因して前記自己位置を正しく推定することができなくなった時に、前記自己位置が正しく推定できなくなるまでの前記一致度の前記グリッド間の相関関係の時系列変化データ群を記憶する時系列変化記憶ステップを実行することにより収集された訓練データを用いて行われる、
   自己位置推定方法。
5. 請求項４に記載の自己位置推定方法であって、
   前記教師データは、
   前記一致度算出ステップにおいて算出された前記一致度の前記グリッド間の相関関係の時系列変化に対する前記猶予時間であって、前記時系列変化記憶ステップにおいて収集された前記時系列変化データ群と、前記一致度算出ステップにおいて算出された前記一致度の前記グリッド間の相関関係の時系列変化とを照合することにより算出される、
   自己位置推定方法。
6. 請求項２に記載の自己位置推定方法であって、
   前記失敗予測ステップでは、
   シミュレーションの結果として予め記憶された前記自己位置が正しく推定できなくなるまでの前記一致度の前記グリッド間の相関関係の時系列変化データ群と、前記一致度算出ステップで得た前記一致度の前記グリッド間の相関関係の時系列変化とを照合することにより前記猶予時間を予測する、
   自己位置推定方法。
7. 請求項２から６のいずれかに記載の自己位置推定方法であって、
   前記グリッドのうち、前記障害物が検出されないグリッドと前記障害物が検出されたグリッドとを区別するマスクを生成するマスク生成ステップをさらに備え、
   前記失敗予測ステップでは、前記マスクにより区別された前記障害物が検出されたグリッド間の前記一致度の相関関係に基づいて、前記猶予時間を予測する
   自己位置推定方法。
8. 請求項２から７のいずれかに記載の自己位置推定方法であって、
   前記失敗予測ステップにおいて予測された前記猶予時間を報知する報知ステップをさらに含む、
   自己位置推定方法。
9. 自車両周辺の障害物の位置を所定範囲毎に設定されたグリッド毎に検出する障害物位置検出部と、
   検出された前記障害物の位置と、少なくとも前記障害物の位置が記憶された地図情報とをマッチングし、当該マッチングの結果に基づいて自己位置を推定する自己位置推定部と、
   検出された前記障害物の位置と前記地図情報に記憶された前記障害物の位置との一致度を前記グリッド毎に算出する一致度算出部と、
   算出された前記一致度の前記グリッド間の相関関係に基づいて、前記自己位置を正しく推定することができなくなることを予測する失敗予測部と、を含む、
   自己位置推定装置。

Images