JP6820762B2 - 位置推定装置 - Google Patents

Description

本開示は、位置推定装置に関する。

特許文献１には、車両の現在位置を推定する位置推定装置が開示されている。この位置推定装置は、拡張カルマンフィルタ、すなわち車両の位置に関する初期値及び予測式を用いて、車両の現在位置を推定する。初期値としては、ＧＮＳＳ衛星からの測位情報に基づく車両の現在位置が使用される。

拡張カルマンフィルタ等の時系列フィルタを用いる構成では、過去に推定された現在位置の推定結果の履歴を引き継いで現在位置の推定が行われる。そのため、一度誤った位置推定が行われると、推定精度は悪化する傾向にあり、推定精度の向上は難しい。

そこで、特許文献１記載の位置推定装置では、車両の現在位置の推定精度が算出され、算出された推定精度がしきい値と比較される。そして、推定精度がしきい値の範囲外である場合、再度、ＧＮＳＳ衛星からの測位情報に基づく車両の現在位置を初期値として、拡張カルマンフィルタを用いた位置推定が行われる。

特開２０１２−１８５０１１号公報

一般に、ＧＮＳＳ衛星のような車両等の移動体の外部の装置を介して移動体の現在位置が推定される場合、推定結果の履歴に基づき現在位置が推定される場合と比較して推定に時間がかかる。特許文献１記載の構成では、推定精度の低下を抑制するために、現在位置の推定精度がしきい値の範囲外になった際に、ＧＮＳＳ衛星からの測位情報による位置推定が行われる。そのため、推定結果の出力が求められる所定の出力周期内に位置推定が完了しない場合がある。例えば、車両の自動運転における走行予定経路の生成に車両の現在位置の推定結果が用いられる場合、出力周期内で位置推定が完了しないと、自動運転が継続できないことが起こり得る。

本開示の一局面は、移動体の現在位置の履歴に基づき移動体の現在位置を推定する構成において、位置推定の継続性を向上させ、かつ、推定精度の低下を抑制することを目的としている。

本開示の一態様は、移動体に搭載され、移動体の現在位置を推定する位置推定装置（１４）であって、推定情報取得部（Ｓ２０１）と、第１推定部（１５，Ｓ２１３，Ｓ２１５，Ｓ２２０）と、第２推定部（１６，Ｓ２０２，Ｓ２０３，Ｓ２０５）と、尤度算出部（Ｓ２１６）と、尤度判定部（Ｓ２１７，Ｓ２１８，Ｓ２２１）と、を備える。推定情報取得部は、移動体の外部の装置から現在位置を推定するための情報である推定情報を取得する。第１推定部は、現在位置を推定する第１推定部であって、過去に推定した現在位置の履歴を表す履歴情報に基づく現在位置の推定を周期的に実行する。第２推定部は、第１推定部による現在位置の推定と並行して、推定情報に基づく現在位置の推定を周期的に実行する。尤度算出部は、第１推定部による推定結果及び第２推定部による推定結果のそれぞれについて尤度を算出する。尤度判定部は、第１推定部による推定結果の尤度及び第２推定部による推定結果の尤度のうちいずれの尤度が高いかを判定し、尤度がより高い推定結果を現在位置の推定結果として出力する。

このような構成によれば、第１推定部及び第２推定部による現在位置の推定が並行して行われる。そして、第１推定部の推定結果の尤度が第２推定部の推定結果の尤度を下回った場合、第１推定部の推定結果に代えて尤度がより高い第２推定部の推定結果が出力される。したがって、位置推定の継続性を向上させ、かつ、推定精度の低下を抑制することができる。

なお、この欄及び特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本開示の技術的範囲を限定するものではない。

位置推定システムの構成を示すブロック図である。 位置推定処理のフローチャートである。 第１推定手法による推定結果の尤度の推移の一例を表す図である。 第２推定手法による推定結果の尤度の推移の一例を表す図である。 本開示の位置推定装置による推定結果の尤度の推移の一例を表す図である。

以下、図面を参照しながら、本開示を実施するための形態を説明する。
［１．構成］
図１に示す位置推定システム１は、車両に搭載され、推定情報取得装置１１、周辺観測装置１２、地図ＤＢ１３、位置推定装置１４及び車両制御装置１９を備える。以下では、位置推定システム１が搭載された車両を「自車両」という。

推定情報取得装置１１は、ＧＮＳＳ受信機１１１、車速センサ１１２、加速度センサ１１３及び操舵角センサ１１４を備える。
ＧＮＳＳ受信機１１１は、ＧＮＳＳにおける測位用の人工衛星からの測位情報及びその推定誤差をアンテナを介して取得する。なお、ＧＮＳＳは、グローバル・ナビゲーション・サテライト・システムのことである。

車速センサ１１２は、自車両の速度を測定するためのセンサである。
加速度センサ１１３は、自車両の加速度を測定するためのセンサである。
操舵角センサ１１４は、自車両の操舵角を測定するためのセンサである。

推定情報取得装置１１は、ＧＮＳＳ受信機１１１を介して取得された測位情報及び推定誤差並びに車速センサ１１２、加速度センサ１１３及び操舵角センサ１１４の測定結果の各種情報のそれぞれを、自車両の現在位置を推定するための情報である推定情報として位置推定装置１４に出力する。

周辺観測装置１２は、自車両の周辺環境を検出する各種センサとして画像センサ１２１及びレーダセンサ１２２を備える。
画像センサ１２１は、自車両の前後左右の所定範囲を撮像して撮影画像を取得する。そして、画像センサ１２１は、撮影画像を画像処理することで、自車両の前後左右に存在する物体の形状や大きさ、自車両との相対位置等を検出する。

レーダセンサ１２２は、自車両の前後左右の所定領域内に例えばミリ波やレーザ光や超音波等に代表されるレーダ波を送信し、その反射波を受信することで、自車両の前後左右に存在する物体と自車両との相対位置及び相対速度等を検出する。

周辺観測装置１２は、画像センサ１２１及びレーダセンサ１２２による検出結果を、自車両の周辺環境を表す情報である環境情報として位置推定装置１４に出力する。
地図ＤＢ１３は、道路網及びランドマークの情報を含む地図情報を保持するデータベースである。本実施形態でいうランドマークは、あらかじめ位置が特定されている陸上の目印であり、ランドマークと自車両との相対的な位置から自車両の位置を特定できるものである。例えば、ランドマークは、あらかじめ位置が特定されている特徴的な建造物や標識などである。また、地図情報に含まれるランドマークの情報には、ランドマークの形状、大きさ及び絶対位置の情報が含まれる。地図ＤＢ１３は、地図情報を位置推定装置１４に出力する。

位置推定装置１４は、ＣＰＵと、ＲＡＭ、ＲＯＭ、フラッシュメモリ等の半導体メモリ（以下、メモリ）と、を有する周知のマイクロコンピュータを中心に構成される。位置推定装置１４の各種機能は、ＣＰＵが非遷移的実体的記憶媒体に格納されたプログラムを実行することにより実現される。この例では、メモリが、プログラムを格納した非遷移的実体的記憶媒体に該当する。また、このプログラムの実行により、プログラムに対応する方法が実行される。なお、位置推定装置１４を構成するマイクロコンピュータの数は１つでも複数でもよい。

位置推定装置１４は、入力された推定情報、環境情報及び地図情報を用いて自車両の現在位置を推定する。具体的には、位置推定装置１４は、後述する図２に示す位置推定処理を実行することで実現される機能の構成として、第１推定部１５、第２推定部１６、尤度算出部１７及び尤度判定部１８を備える。位置推定装置１４を構成するこれらの要素を実現する手法はソフトウェアに限るものではなく、その一部又は全部の要素を、論理回路やアナログ回路等を組み合わせたハードウェアを用いて実現してもよい。

第１推定部１５は、履歴情報に基づく現在位置の推定を周期的に実行する。ここでいう履歴情報とは、過去に推定した自車両の現在位置及びその推定誤差の時系列の履歴を表す情報である。

第１推定部１５は、次のように自車両の現在位置を推定する。すなわち、第１推定部１５は、まず、履歴情報に基づき、現在時刻において自車両が位置する可能性がある範囲である第１探索範囲Ｃ１を絞り込む。そして、第１推定部１５は、推定精度を向上させるため、環境情報に含まれるランドマークの情報と、地図情報に含まれる自車両周辺のランドマークの情報と、をマッチングさせて第１探索範囲Ｃ１の中から自車両の現在位置を推定する。

具体的には、第１推定部１５は、次のように自車両の現在位置を推定する。すなわち、第１推定部１５は、まず、自車両の仮の現在位置を基準、具体的には、中心とする所定範囲を第１探索範囲Ｃ１として設定する。仮の現在位置は、位置推定が周期的に実行される中での２周期目以降であれば、履歴情報に含まれる自車両の現在位置を用いて推定される。具体的には、本実施形態では、仮の現在位置は、１周期前の時刻に推定された自車両の現在位置として推定される。しかし、仮の現在位置はこれに限られない。仮の現在位置は、例えば、１周期前の時刻に推定された自車両の現在位置を、車速センサ１１２、加速度センサ１１３、操舵角センサ１１４などから取得した車速、加速度あるいは操舵角に基づいて補正した、現在時刻における自車両の予測位置として推定されてもよい。一方、履歴情報がない１周期目であれば、仮の現在位置は、例えば、ＧＮＳＳ受信機１１１により取得された測位情報による現在位置、詳細には、後述する第２推定部１６により推定された現在位置として推定される。次に、第１推定部１５は、履歴情報に含まれる誤差範囲の履歴から現在時刻の自車両の現在位置についての誤差範囲を算出する。そして、第１推定部１５は、仮の現在位置を中心とする半径が誤差範囲程度の範囲を第１探索範囲Ｃ１として設定する。

そして、第１推定部１５は、環境情報に含まれるランドマークの情報と、地図情報に含まれる自車両周辺の所定範囲内のランドマークの情報と、をマッチングする。具体的には、第１推定部１５は、画像センサ１２１により検出されたランドマークの形状や大きさ等の情報と、地図情報に保持されたランドマークの形状や大きさ等の情報と、を比較し、ランドマークの類似度を判定する。そして、類似度がしきい値以上である場合、ランドマークが同一のものであると判定する。そして、第１推定部１５は、環境情報に含まれるランドマークと自車両との相対位置の情報と、地図情報に含まれるランドマークの絶対位置の情報と、から算出した自車両の絶対位置を現在位置として推定する。これにより、第１探索範囲Ｃ１の中から自車両の現在位置が推定される。なお、地図情報からランドマークの情報が読み出される上記自車両周辺の所定範囲とは、第１探索範囲Ｃ１を含む範囲であり、本実施形態では、第１探索範囲Ｃ１を周辺観測部１２、特に画像センサ１２１により観測可能な範囲の分だけ広げた範囲である。つまり、第１探索範囲Ｃ１内のどの位置に自車両が位置しているとしても、その位置から検出されたランドマークが含まれるような範囲である。しかし、上記自車両周辺の所定範囲は、これに限られるものではなく、例えば、第１探索範囲Ｃ１と同範囲であってもよい。

この第１推定部１５による位置推定には、カルマンフィルタやパーティクルフィルタ等の時系列フィルタが用いられる。また、第１推定部１５による位置推定は、履歴情報に基づき行われるため、１出力周期内で行うことができる。ここでいう出力周期とは、現在位置の推定結果の出力が求められる所定の周期である。また、第１推定部１５による位置推定は、後述する第２推定部１６による測位情報を用いる位置推定よりも精度が良い。なお、本実施形態では自車両は自動運転による走行が可能であり、第１推定部１５及び第２推定部１６の推定結果は、走行予定経路の生成に用いられる。自動運転中の自車両は、この走行予定経路に沿って走行する。

第２推定部１６は、推定情報に基づく自車両の現在位置の推定を周期的に実行する。この第２推定部１６は、履歴情報に代えて測位情報を用いて位置推定を行う点で、第１推定部１５と相違する。

第２推定部１６は、次のように自車両の現在位置を推定する。すなわち、第２推定部１６は、まず、測位履歴に基づき現在時刻において自車両が位置する可能性がある範囲である第２探索範囲Ｃ２を絞り込む。そして、第２推定部１６は、推定精度を向上させるため、環境情報に含まれるランドマークの情報と、地図情報に含まれる自車両周辺のランドマークの情報と、をマッチングさせて第２探索範囲Ｃ２の中から自車両の現在位置を推定する。

具体的には、第２推定部１６は、次のように自車両の現在位置を推定する。すなわち、第２推定部１６は、まず、自車両の仮の現在位置を基準、具体的には、中心とする所定範囲を第２探索範囲Ｃ２として設定する。仮の現在位置は、推測航法を用いて推定される。具体的には、測位情報の表す現在位置を、車速センサ１１２、加速度センサ１１３、操舵角センサ１１４などから取得した車速、加速度あるいは操舵角に基づいて補正した、現在時刻における自車両の予測位置として推定される。次に、第２推定部１６は、ＧＮＳＳ受信機１１１により取得された測位情報の推定誤差などから現在時刻の自車両の現在位置についての誤差範囲を算出する。そして、第２推定部１６は、仮の現在位置を中心とする半径が誤差範囲程度の範囲を第２探索範囲Ｃ２として設定する。

そして、第２推定部１６は、環境情報に含まれるランドマークの情報と、地図情報に含まれる自車両周辺の所定範囲内のランドマークの情報と、を第１推定部１５と同様の手法でマッチングする。これにより、第２探索範囲Ｃ２の中から自車両の現在位置が推定される。特に、この第２推定部１６による位置推定には、時系列フィルタが用いられない。また、第２推定部１６による位置推定の演算周期は、複数の出力周期にまたがってもよい。

尤度算出部１７は、第１推定部１５の推定結果及び第２推定部１６の推定結果のそれぞれについて尤度を算出する。尤度の算出については後述する。
尤度判定部１８は、第１推定部１５の推定結果の尤度及び第２推定部１６の推定結果の尤度のうちいずれの尤度が高いかを判定する。そして、尤度判定部１８は、尤度がより高い推定結果を自車両の現在位置の推定結果として車両制御装置１９に出力する。

車両制御装置１９は、位置推定装置１４から入力された推定結果等を用いて自車両の走行を制御する。具体的には、車両制御装置１９は、入力された推定結果等を用いて自動運転の走行予定経路を生成する。そして、車両制御装置１９は、生成した走行予定経路に従い自車両の走行を制御するアクチュエータを動作させることで自車両を走行させる。

［２．処理］
次に、位置推定装置１４が実行する位置推定処理について図２のフローチャートを用いて説明する。本実施形態では、自車両の運転が手動運転から自動運転に切り替わると位置推定装置１４が起動し、位置推定処理が開始される。また、位置推定装置１４の出力周期はＴ０であるとする。

Ｓ２０１で、位置推定装置１４は、推定情報取得装置１１から推定情報を取得する。
続くＳ２０２で、位置推定装置１４は、取得した測位情報などの推定情報を使用し、推測航法により自車両の位置（以下、初期位置）Ａ１及びその誤差範囲Ｂ１を測定する。ここでいう初期位置Ａ１は、前述した第２推定部１６による位置推定の仮の現在位置である。なお、初期位置Ａ１及び誤差範囲Ｂ１の取得時刻をＴ１とする。

続くＳ２０３で、位置推定装置１４は、初期位置Ａ１を中心とする半径が誤差範囲Ｂ１程度である範囲を第２探索範囲Ｃ２として設定する。
続くＳ２０４で、位置推定装置１４は、周辺観測装置１２から環境情報を取得する。また、位置推定装置１４は、地図ＤＢ１３から地図情報における自車両周辺の所定範囲の情報を取得する。

続くＳ２０５で、位置推定装置１４は、第２推定手法による自車両の現在位置の推定を行う。具体的には、位置推定装置１４は、環境情報に含まれるランドマークＬ１の情報と、地図情報に含まれる自車両周辺の所定範囲内のランドマークＬ２の情報と、をマッチングする。このマッチングにより第２探索範囲Ｃ２の中から自車両の現在位置についての推定位置Ａ２が得られる。この第２推定手法は、第２推定部１６による位置推定手法に相当する。第２推定手法による位置推定が完了までにかかる時間、換言すればＳ２０２、Ｓ２０３及びＳ２０５の処理に要する時間Ｔａは、出力周期Ｔ０よりも大きくなってもよい。以下、最新の推定位置Ａ２が得られた時刻をＴ２とする。なお、位置推定装置１４は、Ｓ２０２、Ｓ２０３及びＳ２０５の処理を実行することで、第２推定部１６として機能する。

位置推定装置１４は、Ｓ２０５を実行すると、Ｓ２０１に戻り、Ｓ２０１以降の処理を繰り返す。
一方、位置推定装置１４は、前述したＳ２０１〜Ｓ２０５の処理と並行して、以下に述べるＳ２１１〜Ｓ２２２の処理を実行する。

Ｓ２１１で、位置推定装置１４は、Ｓ２０５で得られた第２推定手法による最新の推定位置Ａ２を取得する。この時、現在時刻Ｔは、時刻Ｔ２と同時刻又はそれよりも後の時刻である。

Ｓ２１１を実行すると、位置推定装置１４は、以下に述べるＳ２１２の処理及びＳ２１３〜Ｓ２１５の処理を並行して実行する。
Ｓ２１２で、位置推定装置１４は、Ｓ２１１で取得した第２推定手法による推定位置Ａ２をデッドレコニングにより補正する。すなわち、Ｓ２１１で取得した推定位置Ａ２は時刻Ｔ２での推定位置であるため、現在時刻Ｔにおける推定位置は、Ａ２に時刻Ｔ２から現在時刻Ｔまでの間のデッドレコニング結果Ｄを積算して加えた位置Ａ３＝Ａ２＋Ｄとして推定される。デッドレコニング結果Ｄは、推定情報取得装置１１によって１出力周期Ｔ０ごとに最新のものが取得される。位置推定装置１４は、Ｓ２１２を実行すると、後述するＳ２１６に移行する。

一方、Ｓ２１３で、位置推定装置１４は、前述した手法で、履歴情報に基づき第１探索範囲Ｃ１を設定する。
続くＳ２１４で、位置推定装置１４は、周辺観測装置１２から環境情報を取得する。また、位置推定装置１４は、地図ＤＢ１３から地図情報における自車両周辺の所定範囲の情報を取得する。

Ｓ２１５で、位置推定装置１４は、第１推定手法による自車両の現在位置の推定を行う。具体的には、位置推定装置１４は、環境情報に含まれる自車両周辺のランドマークＬ１の情報と、地図情報に含まれる自車両周辺の所定範囲のランドマークＬ３の情報と、をマッチングする。このマッチングにより第１探索範囲Ｃ１の中から自車両の現在位置についての推定位置Ａ４が得られる。第１推定手法による位置推定が完了までにかかる時間、換言すればＳ２１３及びＳ２１５の処理に要する時間Ｔａは、出力周期Ｔ０以下でなければならない。この第１推定手法は、第１推定部１５による位置推定手法に相当する。位置推定装置１４は、Ｓ２１５を実行すると、Ｓ２１６に移行する。

Ｓ２１６で、位置推定装置１４は、Ｓ２１２で取得した第２推定手法による推定結果Ａ３及びＳ２１５で取得した第１推定手法による推定結果Ａ４のそれぞれについて尤度を算出する。ここで、尤度には、推定結果Ａ３及び推定結果Ａ４のそれぞれにつき、各推定結果周辺の地図情報と環境情報との一致度、すなわちマッチング率が用いられる。具体的には例えば下記のマッチング率を用いることが考えられる。すなわち、地図ＤＢ１３から推定位置Ａ３を基準とする所定範囲内のランドマークＬ４の大きさや形状等の情報を得る。また、地図ＤＢ１３から推定位置Ａ４を基準とする所定範囲内のランドマークＬ５の大きさや形状等の情報を得る。そして、得られたランドマークＬ４及びＬ５の大きさや形状等の情報を周辺観測装置１２により観測された自車両周辺のランドマークＬ１の大きさや形状等の情報と各々マッチングし、この時のマッチング率を尤度として用いることが考えられる。なお、位置推定装置１４は、Ｓ２１６の処理を実行することで、尤度算出部１７として機能する。

続くＳ２１７で、位置推定装置１４は、第１推定手法による推定結果Ａ４の方が第２推定手法による推定結果Ａ３よりも尤度が高いか否かを判定する。
位置推定装置１４は、第１推定手法による推定結果Ａ４の方が尤度が高いと判定した場合、Ｓ２１８で、第１推定手法による推定結果Ａ４を自車両の現在位置の推定結果として車両制御装置１９に出力する。位置推定装置１４は、Ｓ２１８を実行すると、後述するＳ２２２に移行する。

一方、Ｓ２１７で、位置推定装置１４は、第１推定手法による推定結果Ａ４の方が尤度が低い、つまり、第２推定手法による推定結果Ａ３の方が尤度が高いと判定した場合、Ｓ２１９で、初期化条件が満たされているか否かを判定する。すなわち、位置推定装置１４は、第２推定手法による推定結果Ａ３の方が尤度が高いと判定した場合、第１推定手法に用いられる時系列の履歴情報を初期化する。そして、位置推定装置１４は、第２推定手法による推定結果Ａ３を初期値として第１推定手法による現在位置の推定を再開する。ただし、本実施形態では、第２推定手法による推定結果Ａ３の方が尤度が高いと判定された場合に即座に履歴情報が初期化されるのではなく、所定の条件である初期化条件が更に満たされていると判定された場合、履歴情報が初期化される。本実施形態では、初期化条件は、位置推定処理が実行されてから一度初期化が行われている場合、直近の初期化から一定期間経過したことである。なお、位置推定処理が実行されてから一度も初期化が行われていない場合、初期化条件は満たされていると判定される。

位置推定装置１４は、初期化条件が満たされていると判定した場合、Ｓ２２０で第２推定手法による推定結果Ａ３を用いて第１推定手法に用いられる履歴情報を初期化し、Ｓ２２１に移行する。なお、位置推定装置１４は、Ｓ２１３、Ｓ２１５及びＳ２２０の処理を実行することで、第１推定部１５として機能する。

一方、位置推定装置１４は、初期化条件が満たされていると判定した場合、Ｓ２２０を飛ばしてＳ２２１に移行する。
Ｓ２２１で、位置推定装置１４は、第２推定手法による推定結果Ａ３を自車両の現在位置の推定結果として車両制御装置１９に出力し、Ｓ２２２に移行する。なお、位置推定装置１４は、Ｓ２１７、Ｓ２１８及びＳ２２１の処理を実行することで、尤度判定部１８として機能する。

Ｓ２２２で、位置推定装置１４は、時刻Ｔから１出力周期Ｔ０前の時刻Ｔ−Ｔ０から現在時刻Ｔまでの間に第２推定手法による推定位置Ａ３が更新されたか否かを判定する。位置推定装置１４は、推定位置Ａ３が更新されたと判定した場合、前述したＳ２１１に移行し、第２推定手法による最新の推定位置Ａ３を取得する。一方、位置推定装置１４は、推定位置Ａ３が更新されていないと判定した場合、前述したＳ２１１を飛ばしてＳ２１２及びＳ２１３に移行する。

一方、位置推定装置１４は、前述したＳ２０１〜Ｓ２０５の処理及びＳ２１１〜Ｓ２２２の処理と並行して、以下に述べるＳ２９９の処理を実行する。
Ｓ２９９で、位置推定装置１４は、当該位置推定装置１４の動作を継続するか否かを判定する。本実施形態では、位置推定装置１４は、自車両の運転が自動運転である場合、動作を継続すると判定する。一方、位置推定装置１４は、自車両の運転が手動運転である場合、つまり、自車両の運転が自動運転から手動運転に切り替わったと判定した場合、動作を継続しないと判定する。

位置推定装置１４は、動作を継続すると判定した場合、Ｓ２９９を再度実行する。一方、位置推定装置１４は、動作を継続しないと判定した場合、位置推定処理を終了する。
［３．効果］
以上詳述した実施形態によれば、以下の効果が得られる。

（１）本実施形態によれば、位置推定の継続性を向上させ、かつ、推定精度の低下を抑制することができる。以下、このことを図３〜図５を用いて説明する。
図３は、第１推定手法の特徴を示している。第１推定手法では、履歴情報に基づき自車両の現在位置が推定されるため、過去の位置推定が精度良く行われている場合には、ほとんど尤度は低下しない。その一方で、真の現在位置が推定結果から外れてしまうと尤度が大きく低下し、多くの場合自然に復帰することは不可能となる。なお、真の現在位置が推定結果から外れてしまう原因としては、本実施形態のように位置推定においてランドマークのマッチングが行われる場合、ランドマークを取り違えてマッチングさせてしまうことなどが挙げられる。図３では、時刻Ｔ３でランドマークの取り違いが起きている。

一方、図４は、第２推定手法の特徴を示している。図中、時間間隔Ｔｂは、第２推定手法の演算周期を示している。第２推定手法は、演算周期ごとに履歴情報に基づかない初期位置Ａ１を用いて位置推定を行うため、一時的に尤度が低下しても推定結果が真の現在位置付近に復帰することが第１推定手法よりも容易である。その一方で、一般に、履歴情報を用いる第１推定手法よりも第２推定手法による位置推定の方が時間がかかる傾向にある。このように演算時間が長いため、推定完了時の自車両の現在位置はある程度過去の現在位置となる。このため、推定位置を真の現在位置に近づけるためにデッドレコニングを用いて推定位置と真の現在位置との差分を補完することが考えられる。しかし、デッドレコニングを行っても、各演算周期の後半になるにつれてデッドレコニングの誤差が蓄積し、尤度が低下していく。その一方で、第２推定手法による位置推定では、短時間(数秒〜十数秒程度)では大きく現在位置の推定を誤ることはない。

一方、本実施形態の位置推定の特徴は図５に示される。図５に示すように、第１推定手法による位置推定と第２推定手法による位置推定とが並行して行われる。そして、第１推定手法による推定結果及び第２推定手法による推定結果のうち、より尤度が高い推定結果が出力される。図５では、円形の破線で囲まれた部分、すなわち、時刻Ｔ５及びＴ６で第１推定手法による推定結果の尤度が第２推定手法による推定結果の尤度を下回り、この時点で第２推定手法による推定結果が自車両の現在位置の推定結果として出力される。

このように、第１推定手法及び第２推定手法による現在位置の推定が並行して行われ、第１推定手法の推定結果の尤度が第２推定手法による推定結果の尤度を下回った場合、第１推定手法による推定結果に代えて尤度がより高い第２推定手法による推定結果が出力される。換言すると、現在位置の推定結果の過去の履歴を用いない第２推定手法による位置推定をバックグラウンドで実行し、第１推定手法の推定が大きく誤りかけたことを第２推定手法による推定結果との尤度比較によって検出する。したがって、位置推定の継続性を向上させ、かつ、推定精度の低下を抑制することができる。

（２）また、本実施形態の位置推定によれば、第１推定手法による推定結果の尤度が第２推定手法による推定結果の尤度よりも低いと判定された場合、第２推定手法における履歴情報が初期化される。そして、第２推定部による現在位置の推定結果を初期値として履歴情報に基づく現在位置の推定が再開される。図５では、時刻Ｔ５及びＴ６で履歴情報の初期化が行われている。

したがって、（ａ）第１推定手法の推定結果の尤度が大幅に低下した場合であっても尤度を向上させることができる。また、（ｂ）初期値の精度が悪く第１推定手法の推定結果の尤度が低かった場合でもより精度の良い初期値に変更することができる。ここで、上記（ｂ）について補足すると、初期値の再設定が行われるＴ＝Ｔ５までは、第１推定手法の初期値の精度が悪く尤度がそれほど高くない。そして、Ｔ＝Ｔ５の直前のＴ＝Ｔ４で第２推定手法による位置推定結果が更新され、第２推定手法の方が第１推定手法の推定結果よりも尤度が高くなる。その結果、第２推定手法の推定結果が初期値に設定され、第１推定手法の尤度が向上する。よって、初期値の精度が悪く、第１推定手法の推定結果の尤度が低かった場合でもより精度の良い初期値に変更することができる。

（３）また、本実施形態の位置推定によれば、第１推定部による推定結果の尤度が第２推定部による推定結果の尤度よりも低いと判定された場合、初期化条件が満たされているか否かが判定される。そして、初期化条件が満たされていると判定された場合、第１推定手法の履歴情報の初期化が行われる。一方、初期化条件が満たされていないと判定された場合、履歴情報の初期化が行われない。

したがって、位置推定装置１４全体としての位置推定精度を高めることができ得る。すなわち、第１推定手法が長い時系列の履歴情報を活用する場合、履歴情報の初期化を行うことで履歴が使えなくなり、一時的に位置推定精度が低下する場合がある。この点、本実施形態の位置推定では、初期化条件が満たされていないと判定された場合、履歴情報の初期化が行われない。このため、初期条件の初期化が頻繁に行われることを抑制し、位置推定装置１４全体としての位置推定精度を高めることができ得る。

（４）本実施形態の位置推定によれば、第１推定部１５は、履歴情報に基づき推定した仮の現在位置周辺の第１探索範囲Ｃ１を設定し、環境情報に含まれるランドマークの情報と、地図情報における自車両周辺のランドマークの情報と、をマッチングすることにより第１探索範囲Ｃ１の中から自車両の現在位置を推定する。また、第２推定部１６は、推定情報に基づき推定した仮の現在位置Ａ１周辺の第２探索範囲Ｃ２を設定し、環境情報に含まれるランドマークの情報と、地図情報における自車両周辺のランドマークの情報と、をマッチングすることにより第２探索範囲Ｃ２の中から現在位置を推定する。

したがって、例えば、第１推定部１５が履歴情報を用いるがランドマークのマッチングを行わない場合や、第２推定部１６がランドマークのマッチングを行わず推定航法単独で位置推定を行う場合と比較して、位置推定精度を向上させることができる。

なお、本実施形態では、車両が移動体に相当し、測位用の人工衛星が外部の装置に相当し、測位用の人工衛星からの測位情報が外部の装置から取得される推定情報に相当する。また、Ｓ２０１が推定情報取得部としての処理に相当し、Ｓ２０２、Ｓ２０３及びＳ２０５が第２推定部としての処理に相当し、Ｓ２０４及びＳ２１４が環境情報取得部及び地図情報取得部としての処理に相当し、Ｓ２１３、Ｓ２１５及びＳ２２０が第１推定部としての処理に相当する。Ｓ２１６が尤度算出部としての処理に相当し、Ｓ２１７、Ｓ２１８及びＳ２２１が尤度判定部としての処理に相当し、Ｓ２１９が条件判定部としての処理に相当し、画像センサ１２１及びレーダセンサ１２２がセンサに相当する。

［３．他の実施形態］
以上、本開示を実施するための形態について説明したが、本開示は上述の実施形態に限定されることなく、種々変形して実施することができる。

（１）上記実施形態では、初期化条件は直近の履歴情報の初期化から一定期間経過したことであるが、初期化条件はこれに限られるものではない。初期化条件は、例えば、第２推定手法の推定結果の尤度が第１推定手法の推定結果の尤度を所定値以上上回ることであってもよい。この場合の所定値は、第２推定手法の推定結果の尤度が第１推定手法の推定結果の尤度を大幅に上回ることに対応するように設定されてもよい。

（２）上記実施形態において、第１推定部１５の推定結果の尤度が第２推定部１６の推定結果の尤度よりも低いと判定された場合に、第２推定部１６の推定結果を用いた履歴情報の初期化が行われなくてもよい。すなわち、上記実施形態において、図２のＳ２１９及びＳ２２０の処理が実行されなくてもよい。

（３）上記実施形態では、第１推定手法及び第２推定手法は上記実施形態の手法に限られるものではない。第１推定手法は、例えば、履歴情報を用いるが、ランドマークのマッチングを行わない手法であってもよい。また、第１推定手法は、例えば、ランドマークのマッチングを行わない手法、具体的には、測位情報単独で位置推定を行う手法、推定航法による位置推定手法等であってもよい。

（４）上記実施形態では、推定情報の取得元である外部の装置は測位用の人工衛星であり、推定情報は人工衛星からの測位情報であるが、外部の装置及び推定情報はこれに限られるものではない。外部の装置は、例えば、無線通信の基地局に設けられた、移動体と無線通信可能な無線送信装置であってもよい。そして、推定情報は、例えば、上記無線送信装置と移動体との距離を表す情報である距離情報であってもよい。

（５）上記実施形態では、移動体として車両を例示したが、移動体はこれに限られるものではない。移動体は、例えば、ロボット等であってもよい。
（６）前述した位置推定装置１４の他、当該位置推定装置１４を構成要素とするシステム、当該位置推定装置１４としてコンピュータを機能させるためのプログラム、このプログラムを記憶した半導体メモリ等の非遷移的実体的記憶媒体、履歴情報に基づく移動体の現在位置の推定方法など、種々の形態で本開示を実現することもできる。

（７）上記実施形態における１つの構成要素が有する複数の機能を、複数の構成要素によって実現したり、１つの構成要素が有する１つの機能を、複数の構成要素によって実現したりしてもよい。また、複数の構成要素が有する複数の機能を、１つの構成要素によって実現したり、複数の構成要素によって実現される１つの機能を、１つの構成要素によって実現したりしてもよい。また、上記実施形態の構成の一部を省略してもよい。また、上記実施形態の構成の少なくとも一部を、他の上記実施形態の構成に対して付加又は置換してもよい。なお、特許請求の範囲に記載した文言によって特定される技術思想に含まれるあらゆる態様が本開示の実施形態である。

１…位置推定システム、１１…推定情報取得装置、１２…周辺観測装置、１４…位置推定装置、１５…第１推定部、１６…第２推定部、１７…尤度算出部、１８…尤度判定部、１９…車両制御装置。

Claims (6)

1. 移動体に搭載され、前記移動体の現在位置を推定する位置推定装置（１４）であって、
   前記移動体の外部の装置から前記現在位置を推定するための情報である推定情報を取得する推定情報取得部（Ｓ２０１）と、
   前記現在位置を推定する第１推定部（１５，Ｓ２１３，Ｓ２１５，Ｓ２２０）であって、過去に推定した前記現在位置の履歴を表す履歴情報に基づく前記現在位置の推定を周期的に実行する第１推定部と、
   前記第１推定部による前記現在位置の推定と並行して、前記推定情報に基づく前記現在位置の推定を周期的に実行する第２推定部（１６，Ｓ２０２，Ｓ２０３，Ｓ２０５）と、
   前記第１推定部による推定結果及び前記第２推定部による推定結果のそれぞれについて尤度を算出する尤度算出部（Ｓ２１６）と、
   前記第１推定部による推定結果の前記尤度及び前記第２推定部による推定結果の前記尤度のうちいずれの前記尤度が高いかを判定し、前記尤度がより高い前記推定結果を前記現在位置の推定結果として出力する尤度判定部（Ｓ２１７，Ｓ２１８，Ｓ２２１）と、
   前記移動体の周辺環境を検出するセンサ（１２１，１２２）から前記周辺環境を表す情報である環境情報を取得する環境情報取得部（Ｓ２０４，Ｓ２１４）と、
   あらかじめ位置が特定されているランドマークの情報を含む地図情報を取得する地図情報取得部（Ｓ２０４，Ｓ２１４）と、
   を備え、
   前記尤度判定部は、前記第１推定部による推定結果及び前記第２推定部による推定結果のそれぞれについて、各推定結果周辺の前記地図情報と前記環境情報との一致度を前記尤度として算出する、位置推定装置。
2. 請求項１に記載の位置推定装置であって、
   前記尤度判定部は、前記地図情報に含まれる前記移動体周辺の前記ランドマークの情報と、前記環境情報に含まれる前記ランドマークの情報と、のマッチング率を前記尤度として算出する、位置推定装置。
3. 請求項１又は請求項２に記載の位置推定装置であって、
   前記外部の装置は、測位用の人工衛星であり、
   前記推定情報取得部は、前記人工衛星からの測位情報を前記推定情報として取得する、位置推定装置。
4. 請求項１から請求項３までのいずれか１項に記載の位置推定装置であって、
   前記第１推定部は、前記履歴情報に基づき推定した仮の前記現在位置周辺の第１探索範囲を設定し、前記環境情報に含まれる前記ランドマークの情報と、前記地図情報に含まれる前記移動体周辺の前記ランドマークの情報と、をマッチングすることにより前記第１探索範囲の中から前記現在位置を推定し、
   前記第２推定部は、前記推定情報に基づき推定した仮の前記現在位置周辺の第２探索範囲を設定し、前記環境情報に含まれる前記ランドマークの情報と、前記地図情報に含まれる前記移動体周辺の前記ランドマークの情報と、をマッチングすることにより前記第２探索範囲の中から前記現在位置を推定する、位置推定装置。
5. 請求項１から請求項４までのいずれか１項に記載の位置推定装置であって、
   前記第１推定部は、前記尤度判定部により当該第１推定部による推定結果の前記尤度が前記第２推定部による推定結果の前記尤度よりも低いと判定された場合、前記履歴情報を初期化し、前記第２推定部による前記現在位置の推定結果を初期値として前記履歴情報に基づく前記現在位置の推定を再開する、位置推定装置。
6. 請求項５に記載の位置推定装置であって、
   前記尤度判定部により前記第１推定部による推定結果の前記尤度が前記第２推定部による推定結果の前記尤度よりも低いと判定された場合、所定の初期化条件が満たされているか否かを判定する条件判定部（Ｓ２１９）を更に備え、
   前記第１推定部は、前記条件判定部により前記初期化条件が満たされていると判定された場合、前記履歴情報の初期化を行い、前記条件判定部により初期化条件が満たされていないと判定された場合、前記履歴情報の初期化を行わない、位置推定装置。

Images