JP7274366B2 - 自己位置推定方法及び自己位置推定装置 - Google Patents

Description

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

従来、自車両に対する自車両周囲の物標の相対位置に基づいて、自車両の自己位置の推定値を校正する技術が知られている。物標に基づいて自己位置を校正するタイミングは、物標を検出した直後であるため、誤差補正量が多い場合には校正直後に自己位置が突然大きく補正される現象、いわゆる位置飛びの問題があった。
このため下記特許文献１の自車位置校正装置は、自車の進行先の基準地物の設置地点の所定距離前の補正開始地点から当該設置地点に対応する補正終了地点までの区間を複数の補正区間に分割する。複数の補正区間毎に異なる誤差補正カーブに従って補正終了地点において誤差補正量に到達するよう段階的に自車位置校正処理を実行する。

特開２０１４－０６６６３６号公報

しかしながら、特許文献１の自車位置校正装置においては、進路変更等により補正区間の途中で自車両が予定経路から外れると、新たに基準地物を検出するまで自己位置の推定ができなくなるという問題がある。
本発明は、周囲の物標の相対位置に基づいて自車両の自己位置の推定値を校正する際の推定値の急変を抑制すると共に、自車両の進路に関わらずに自己位置の推定を継続することを目的とする。

本発明の一態様によれば、自車両に対する自車両の周囲の物標の相対位置を検出し、予め記憶した物標の地図上の位置と相対位置とに基づき自車両の自己位置を推定する自己位置推定方法が与えられる。自己位置推定方法では、自車両の移動量に基づいて自車両の自己位置の予測値を算出し、物標の相対位置の検出結果に基づく観測値を生成し、相対位置の検出タイミングにおいて、観測値に応じて予測値を補正して自車両の自己位置を推定し、検出タイミングからの自車両の移動量に基づいて観測値を更新した更新観測値を算出し、相対位置の検出タイミングの合間において、更新観測値に応じて予測値を補正して自車両の自己位置を推定する。

本発明によれば、周囲の物標の相対位置に基づいて自車両の自己位置の推定値を校正する際の推定値の急変を抑制すると共に、自車両の進路に関わらずに自己位置の推定を継続できる。

第１実施形態の自己位置推定装置の一例の概略構成図である。 自己位置推定における位置飛びの説明図である。 第１実施形態の自己位置推定方法の説明図である。 第１実施形態の自己位置推定方法の一例のフローチャートである。 第２実施形態の自己位置推定装置の一例の概略構成図である。

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

（第１実施形態）
（構成）
図１を参照する。以下、移動体の一例として車両の現在位置を推定する場合について説明するが、本発明は車両に限らず様々な移動体の現在位置の推定に広く適用することができる。
自己位置推定装置１は車両（以下「自車両」と表記する）に搭載され、自車両の現在位置を推定する。自己位置推定装置１は、物標検出部１０、地図データベース１１、移動量検出部１２、コントローラ１３を備える。なお、添付する図面において地図データベースを「地図ＤＢ」と表記する。

物標検出部１０は、カメラ、ＬＩＤＡＲ（Light 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）等を有していてもよい。

コントローラ１３は、移動量検出部１２が検出した自車両の相対的な移動量ΔＰに基づいて、デッドレコニング技術やオドメトリを用いて自車両の自己位置を推定する。
また、物標検出部１０がランドマークを検出すると、コントローラ１３は自車両に対するランドマークの相対位置Ｐｃと、地図データベース１１に記憶されたランドマークの地図上の位置である地図座標Ｐｍ（ｘｍ，ｙｍ）とに基づいて、自車両の自己位置を校正する。

上述の通り、ランドマークの検出位置に基づいて自車両の自己位置の推定値を校正すると、推定位置の急変（いわゆる位置飛び）が発生することがある。
図２Ａを参照して位置飛びの問題を説明する。横軸は自車両の移動時間を表し、縦軸は自車両の自己位置の推定値を表す。ここでは、理解の容易のため２次元の自己位置推定ではなく１次元の自己位置推定の例を示す。

丸印は、自車両の移動量に基づいてデッドレコニング技術等で推定した自己位置を表し、星印は、ランドマーク（物標）の検出位置により校正（検出）した自己位置を表す。
図２Ａに示すとおり、移動量に基づく自己位置（丸印）は比較的滑らかに推移するのに対し、ランドマークで校正した自己位置（星印）は直前の自己位置（丸印）から急変し、位置飛びが発生している。
このため、実施形態の自己位置推定方法では、ランドマークの検出タイミングだけでなく、ランドマークの検出タイミングの合間においても、直前のランドマークの検出結果を利用して自己位置の推定結果を補正する。

具体的には、実施形態の自己位置推定方法では、例えば自車両の走行開始地点やランドマークの相対位置Ｐｃの検出結果に基づく観測値の初回値等の任意の基準位置からの自車両の移動量ΔＰに基づいて自車両の自己位置の予測値Ｐｐを算出する。すなわち自車両の自己位置の予測値Ｐｐは任意の時点における自車両の位置を基準位置とし、基準位置からの自車両の移動量を移動量ΔＰに基づいて算出して求められた自己位置であり、基準位置を除いて自車両の移動量のみから予測された自己位置である。また、ランドマークの相対位置Ｐｃの検出結果に基づく観測値（自己位置）を生成する。
相対位置Ｐｃの検出タイミングでは、観測値に応じて予測値Ｐｐを補正して自車両の自己位置Ｐｅを推定する。
相対位置Ｐｃの検出タイミングの合間（相対位置Ｐｃが検出されてから次に相対位置Ｐｃが検出されるまでの間）では、相対位置Ｐｃの検出タイミングからの自車両の移動量を移動量ΔＰに基づいて算出し、相対位置Ｐｃと自車両の移動量に基づいて観測値を更新した更新観測値を算出し、更新観測値に応じて予測値Ｐｐを補正して自車両の自己位置Ｐｅを推定する。

図２Ｂを参照して第１実施形態の自己位置推定方法を説明する。
星印は、ランドマークの相対位置Ｐｃの検出結果に基づく観測値として算出された自車両の自己位置を示す。自己位置の観測値は、自車両に対するランドマークの相対位置Ｐｃとランドマークの地図上の位置Ｐｍ（ｘｍ，ｙｍ）とに基づいて決定できる。
三角印は、ランドマークの相対位置Ｐｃの検出結果に基づく観測値に対して、ランドマークの相対位置Ｐｃを検出した時点からの自車両の移動量に基づいて観測値（星印）を更新した更新観測値を示す。

丸印は、基準位置からの自車両の移動量に基づいて予測した自己位置の予測値Ｐｐを、観測値（星印）又は更新観測値（三角印）に応じて補正した自己位置の推定値Ｐｅである。
相対位置Ｐｃの検出タイミングでは観測値（星印）に応じて予測値Ｐｐを補正し、検出タイミングの合間では更新観測値（三角印）に応じて予測値Ｐｐを補正する。

このように第１実施形態の自己位置推定方法では、相対位置Ｐｃの検出タイミングだけでなく、検出タイミングの合間においても、相対位置Ｐｃの検出結果に応じて予測値Ｐｐを補正する。
これにより、相対位置Ｐｃの検出結果に基づいて補正するタイミングが分散されるので１回ごとの補正量を低減できる。このため、図２Ｂに示すように予測値Ｐｐを補正した推定値Ｐｅの急な変動が抑制され、位置飛びを軽減できる。
さらに、更新観測値は自車両の移動量に基づいて算出されるので、自車両の進路に関わらずに更新観測値を算出できる。このため、自車両の進路に関わらずに継続して予測値Ｐｐを補正して、自己位置の推定値Ｐｅを算出することができる。

なお、図２Ｂの例では、ランドマークの相対位置Ｐｃの検出結果に基づく観測値として自車両の自己位置の観測値を生成した。しかしながら、観測値は必ずしも自己位置の観測値でなくともよい。
観測値は、自車両周囲のランドマークの相対位置Ｐｃの検出結果に基づいて自車両の予測値Ｐｐを補正するために利用可能な測定値であれば足りる。例えば以下の説明では、ランドマークの相対位置Ｐｃそれ自身を観測値として使用する。

以下、コントローラ１３の機能について詳述する。図１を参照する。コントローラ１３は、予測部２０と、更新観測値生成部２１と、スイッチ（ＳＷ）２２と、座標変換部２３と、マッチング部２４と、減算器２５と、ゲイン設定部２６と、乗算器２７と、更新部２８を備える。
予測部２０、更新観測値生成部２１、スイッチ２２、座標変換部２３、マッチング部２４、減算器２５、ゲイン設定部２６、乗算器２７及び更新部２８の機能は、例えば、コントローラ１３が備えるプロセッサが記憶装置に格納されたコンピュータプログラムを実行することによって実現されてもよく、ＦＰＧＡ等のＰＬＤなどのロジックで実現されてもよい。

予測部２０は、移動量検出部１２が検出した自車両の移動量ΔＰに基づいて自車両の自己位置の予測値Ｐｐを算出する。
具体的には、予測部２０は、前回の自己位置の推定値Ｐｅを推定した時点から現在までの自車両の相対的な移動量ΔＰを、移動量検出部１２から取得する。予測部２０は、前回の自己位置の推定値Ｐｅに移動量ΔＰを加えて現在の自己位置の予測値Ｐｐ（ｘｐ，ｙｐ）及び方位角θを算出する処理を繰り返す。この工程は、カルマンフィルタの予測ステップの処理に相当する。

更新観測値生成部２１は、ランドマークの相対位置Ｐｃの検出タイミングにおいて、相対位置Ｐｃの検出結果を観測値として取得する。更新観測値生成部２１は、相対位置Ｐｃの検出タイミングからの自車両の相対的な移動量を移動量検出部１２から取得する。
更新観測値生成部２１は、相対位置Ｐｃの検出タイミングの合間に予測部２０が予測値Ｐｐを算出するタイミングにおいて、自車両の移動量に基づいて観測値Ｐｃを更新して更新観測値Ｐｄ（ｘｄ，ｙｄ）を算出する。
具体的には、ランドマークの相対位置Ｐｃの検出タイミングからの自車両の相対的な移動量を、ランドマークの相対位置（観測値）Ｐｃから減じた差分を、更新観測値Ｐｄとして算出する。

スイッチ２２は、ランドマークの相対位置（観測値）Ｐｃ及び更新観測値Ｐｄを切り替えて、座標変換部２３に入力する。
具体的には、相対位置Ｐｃの検出タイミングにおいて観測値Ｐｃを座標変換部２３に入力する。また、相対位置Ｐｃの検出タイミングの合間に予測部２０が予測値Ｐｐを算出するタイミングでは、更新観測値Ｐｄを座標変換部２３に入力する。

座標変換部２３は、自車両に対するランドマークの相対位置（車両座標系で表された位置）である観測値Ｐｃ及び更新観測値Ｐｄの座標を、地図データベース１１の地図座標系上の座標へ変換する。地図座標系上の座標に変換された観測値Ｐｃ及び更新観測値Ｐｄを「地図座標系観測値Ｐｍ’」と表記する。
具体的には、予測値Ｐｐ（ｘｐ，ｙｐ）及び方位角θに基づいて次式により観測値Ｐｃ（ｘｃ，ｙｃ）を地図座標系観測値Ｐｍ’（ｘｍ’，ｙｍ’）へ変換する。

ｘｍ’＝ｃｏｓθ×ｘｃ－ｓｉｎθ×ｙｃ＋ｘｐ
ｙｍ’＝ｓｉｎθ×ｘｃ＋ｃｏｓθ×ｙｃ＋ｙｐ
なお、地図座標系上の座標に変換された観測値Ｐｃは、特許請求の範囲に記載の「物標の地図座標系上の第１位置」の一例である。

また、次式により更新観測値Ｐｄ（ｘｄ、ｙｄ）を地図座標系観測値Ｐｍ’（ｘｍ’，ｙｍ’）へ変換する。
ｘｍ’＝ｃｏｓθ×ｘｄ－ｓｉｎθ×ｙｄ＋ｘｐ
ｙｍ’＝ｓｉｎθ×ｘｄ＋ｃｏｓθ×ｙｄ＋ｙｐ
地図座標系上の座標に変換された更新観測値Ｐｄは、特許請求の範囲に記載の「物標の地図座標系上の第２位置」の一例である。
座標変換部２３は、地図座標系観測値Ｐｍ’をマッチング部２４と減算器２５に出力する。

マッチング部２４は、地図座標系観測値Ｐｍ’と、地図データベース１１に記憶された同じ種類のランドマークの地図座標とを比較し、最も近いランドマーク同士をマッチングさせる。このとき、最も近いランドマークであっても所定閾値以上離れたランドマークはマッチングしない。例えば、所定閾値は１ｍであってよい。マッチング部２４は、地図座標系観測値Ｐｍ’とマッチングしたランドマークの地図座標Ｐｍ（ｘｍ，ｙｍ）を減算器２５に出力する。

減算器２５は、地図座標系観測値Ｐｍ’と地図座標Ｐｍとの差（ｘｍ’－ｘｍ，ｙｍ’－ｙｍ）である予測誤差ｅを算出し、乗算器２７に出力する。
乗算器２７は、ゲイン設定部２６で設定されたゲインｇを予測誤差ｅに乗じ、その結果得られる積（ｇ×ｅ）を更新部２８へ出力する。
ゲイン設定部２６で設定されたゲインｇは、カルマンフィルタに相当し、観測値Ｐｃと更新観測値Ｐｄの観測誤差に応じて設定される。例えば、更新観測値Ｐｄの観測誤差は観測値Ｐｃの観測誤差よりも大きいので、更新観測値Ｐｄの予測誤差ｅに乗じるゲインｇを、観測値Ｐｃの予測誤差ｅに乗じるゲインｇよりも小さな値に設定してよい。

更新部２８は、予測誤差ｅとゲインｇの積（ｇ×ｅ）に応じて、予測部２０が算出した自車両の自己位置の予測値Ｐｐを補正して、自己位置の推定値Ｐｅを推定する。例えば、更新部２８は、積（ｇ×ｅ）によりカルマンフィルタの観測更新式に従って予測値Ｐｐを更新して推定値Ｐｅを推定する。
ここで、ゲインｇを十分小さくすれば、更新部２８における予測値Ｐｐの一回の補正量を制限できる。これにより、予測誤差ｅが徐々に小さくなるように（すなわち、地図座標系観測値Ｐｍ’が地図座標Ｐｍに徐々に近づくように）推定値Ｐｅを推定することができる。このため、観測値Ｐｃによる校正の際の推定値Ｐｅの急変を抑制できる。

（動作）
次に、図３を参照して第１実施形態の自己位置推定方法の一例を説明する。
ステップＳ１において移動量検出部１２は、前回の自己位置の推定値Ｐｅを推定した時点から現在までの自車両の相対的な移動量ΔＰを検出する。
ステップＳ２において予測部２０は、前回の自己位置の推定値Ｐｅに移動量ΔＰを加えて現在の自己位置の予測値Ｐｐを算出する。

ステップＳ３において、現時刻が移動量検出部１２による自車両周囲のランドマーク（）の相対位置Ｐｃの検出タイミングであるか否かを判定する。検出タイミングである場合（ステップＳ３：Ｙ）には処理はステップＳ４に進む。検出タイミングの合間のタイミングである場合（ステップＳ３：Ｎ）には処理はステップＳ５へ進む。

ステップＳ４において移動量検出部１２は、自車両に対するランドマークの相対位置を検出し、相対位置の観測値Ｐｃを生成する。その後に処理はステップＳ６へ進む。
一方でステップＳ５において更新観測値生成部２１は、相対位置の前回の検出タイミングからの自車両の相対的な移動量を観測値Ｐｃから減じて、更新観測値Ｐｄとして算出する。その後に処理はステップＳ６へ進む。

ステップＳ６において座標変換部２３は、観測値Ｐｃ及び更新観測値Ｐｄを地図座標系観測値Ｐｍ’へ変換する。
ステップＳ７においてマッチング部２４は、地図データベース１１から、地図座標系観測値Ｐｍ’とマッチングするランドマークの地図座標Ｐｍを読み出す。
ステップＳ８において更新部２８は、地図座標系観測値Ｐｍ’と地図座標Ｐｍとの間の予測誤差ｅに応じて、予測部２０が算出した自車両の自己位置の予測値Ｐｐを補正して、自己位置の推定値Ｐｅを推定する。

（第１実施形態の効果）
（１）自己位置推定装置１は、自車両に対する自車両の周囲の物標の相対位置を検出し、予め記憶した物標の地図上の位置と相対位置とに基づき自車両の自己位置を推定する。予測部２０は、自車両の移動量ΔＰに基づいて自車両の自己位置の予測値Ｐｐを算出する。物標検出部１０は、物標の相対位置の検出結果に基づく観測値を生成する。更新部２８は、相対位置の検出タイミングにおいて、観測値Ｐｃに応じて予測値Ｐｐを補正して自車両の自己位置Ｐｅを推定する。

更新観測値生成部２１は、相対位置の検出タイミングからの自車両の移動量に基づいて観測値Ｐｃを更新した更新観測値Ｐｄを算出する。更新部２８は、相対位置の検出タイミングの合間において、更新観測値Ｐｄに応じて予測値Ｐｐを補正して自車両の自己位置Ｐｅを推定する。
このように相対位置の検出タイミングで予測値Ｐｐを補正するだけでなく、その合間のタイミングでも、観測値Ｐｃから算出した更新観測値Ｐｄに基づいて補正する。これにより、観測値Ｐｃの検出結果に基づく補正タイミングを分散できるので、１回の補正量を低減できる。このため、物標の相対位置に基づいて自車両の自己位置の推定値を校正する際の推定値の急変を抑制できる。また、自車両の移動量に基づいて更新観測値Ｐｄを算出するので、自車両の進路に関わらずに更新観測値を算出して自己位置の推定を継続できる。

（２）物標検出部１０は、観測値として物標の相対位置の観測値を生成する。座標変換部２３は、観測値Ｐｃと予測値Ｐｐとに基づき物標の地図座標系上の第１位置Ｐｍ’を予測し、更新観測値Ｐｄと予測値Ｐｐとに基づき物標の地図座標系上の第２位置Ｐｍ’を予測する。
更新部２８は、相対位置の検出タイミングにおいて第１位置Ｐｍ’が予め記憶した物標の地図上の位置Ｐｍに近づくように予測値Ｐｐを補正し、相対位置の検出タイミングの合間において第２位置Ｐｍ’が予め記憶した物標の地図上の位置Ｐｍに近づくように予測値を補正する。
このように、物標検出部１０が検出した物標の地図座標系上の位置Ｐｍ’が、予め記憶した地図上の位置Ｐｍに近づくように、予測値Ｐｐを補正して自己位置Ｐｅの推定することにより、推定値の急変を抑制できる。

（変形例）
上述の通り、物標の相対位置の検出結果に基づく観測値として、自車両の自己位置の観測値を生成してもよい。自車両に対するランドマークの相対位置とランドマークの地図上の位置とに基づいて自己位置の観測値を算出してよい。
この場合、更新部２８は、相対位置の検出タイミングにおいて観測値に近づくように予測値Ｐｐを補正し、相対位置の検出タイミングの合間において更新観測値に近づくように予測値Ｐｐを補正する。
このように自車両の自己位置の観測値を使用しても、第１実施形態の上記効果と同様の効果を得ることができる。

（第２実施形態）
続いて、図４を参照して第２実施形態の自己位置推定装置１を説明する。第１実施形態の構成要素と同様の構成要素には同じ参照符号を付し、同様の機能の説明を省略する。
第２実施形態の自己位置推定装置１は、複数の物標検出部１０ａ、１０ｂ及び１０ｃを備える。
例えば物標検出部１０ａは、カメラを備え、パターン認識処理によって自車両の周囲のランドマークである信号機を検出する。物標検出部１０ａは、自車両に対する信号機の相対位置の観測値Ｐｃａを生成する。

例えば物標検出部１０ｂは、カメラを備え、パターン認識処理によって自車両の周囲のランドマークである標識を検出する。物標検出部１０ｂは、自車両に対する標識の相対位置の観測値Ｐｃｂを生成する。
例えば物標検出部１０ｃは、ＬＩＤＡＲを備え、パターン認識処理によって自車両の周囲の地物を検出する。物標検出部１０ｃは、自車両に対する地物表面の各点の相対位置を表す点群データ、すなわち検出した地物形状のポイントクラウドを観測値Ｐｃｃとして生成する。

さらに、第２実施形態の自己位置推定装置１は、複数の物標検出部１０ａ～１０ｃの検出結果に基づく複数の観測値Ｐｃａ～Ｐｃｃの更新観測値Ｐｄａ、Ｐｄｂ及びＰｄｃをそれぞれ算出する複数の更新観測値生成部２１ａ、２１ｂ及び２１ｃを備える。
更新観測値生成部２１ａは、物標検出部１０ａの検出タイミングから自車両の移動量に基づいて観測値Ｐｃａを更新した更新観測値Ｐｄａを算出する。
更新観測値生成部２１ｂは、物標検出部１０ｂの検出タイミングから自車両の移動量に基づいて観測値Ｐｃｂを更新した更新観測値Ｐｄｂを算出する。
更新観測値生成部２１ｃは、物標検出部１０ｃの検出タイミングから自車両の移動量に基づいて観測値Ｐｃｃを更新した更新観測値Ｐｄｃを算出する。

このように生成された観測値Ｐｃａ～Ｐｃｃ及び更新観測値Ｐｄａ～Ｐｄｃは、順次、座標変換部２３に入力されて、それぞれ地図座標系観測値Ｐｍ’に変換される。
マッチング部２４は、地図座標系観測値Ｐｍ’の各々とマッチングするランドマークの地図座標Ｐｍを地図データベース１１から読み出し、減算器２５は、地図座標系観測値Ｐｍ’と地図座標Ｐｍとの間の予測誤差ｅが算出する。更新部２８は、予測誤差ｅとゲインｇの積（ｇ×ｅ）に応じて予測値Ｐｐを補正して、自己位置の推定値Ｐｅを推定する。

以下の説明において、物標検出部１０ａ～１０ｃを総称して「物標検出部１０」と表記することがある。更新観測値生成部２１ａ～２１ｃを総称して「更新観測値生成部２１」と表記することがある。
また、観測値Ｐｃａ～Ｐｃｃを総称して「観測値Ｐｃ」と表記することがある。更新観測値Ｐｄａ～Ｐｄｃを総称して「更新観測値Ｐｄ」と表記することがある。

物標検出部１０ａ～１０ｃによる相対位置の検出周期は異なっていてもよく、同じであってもよい。
物標検出部１０ａ～１０ｃの検出周期が異なる場合、観測値Ｐｃａ～Ｐｃｃの発生頻度に差が生じることになる。
このように発生頻度の異なる観測値Ｐｃａ～Ｐｃｃに基づいて自己位置の推定値Ｐｅを推定すると、発生頻度の高い観測値Ｐｃに応じて予測値Ｐｐを補正する頻度が高くなるため、発生頻度の低い観測値Ｐｃが自己位置の推定値Ｐｅに反映されにくくなる。すなわち、検出周期が比較的長い物標検出部１０の検出結果が推定値Ｐｅに反映されにくくなる。

そこで発生頻度の低い観測値Ｐｃを重み付けして推定値Ｐｅに反映され易くなるようにすると、今度は発生頻度の低い観測値Ｐｃによる補正で推定値Ｐｅが大きく変動し、位置飛びを生じさせることになる。
このため、第２実施形態の自己位置推定装置１は、検出周期がより長い物標検出部１０の観測値Ｐｃについてはより多くの更新観測値Ｐｄを発生させてよい。これにより、検出周期の異なる物標検出部１０ａ～１０ｃの検出結果が、推定値Ｐｅに与える影響の違いを低減する。

具体的には、比較的長い検出周期の物標検出部１０による観測値Ｐｃの更新観測値Ｐｄを算出する更新観測値生成部２１は、比較的短い検出周期の物標検出部１０による観測値Ｐｃの更新観測値Ｐｄを算出する他の更新観測値生成部２１よりも、より高い頻度で更新観測値を算出してよい。
すなわち、比較的長い検出周期の物標検出部１０による観測値Ｐｃの更新観測値Ｐｄを算出する更新観測値生成部２１は、比較的短い検出周期の物標検出部１０による観測値Ｐｃの更新観測値Ｐｄを算出する他の更新観測値生成部２１よりも、より多くの更新観測値を算出してよい。

これにより、例えば、単位時間あたりの観測値Ｐｃａ～Ｐｃｃの生成回数Ｎｃａ、Ｎｃｂ、Ｎｃｃの比（Ｎｃａ：Ｎｃｂ：Ｎｃｃ）よりも、単位時間あたり観測値Ｐｃａ～Ｐｃｃと更新観測値Ｐｄａ～Ｐｄｃの生成回数の和の比（Ｎｃａ＋Ｎｄａ：Ｎｃｂ＋Ｎｄｂ：Ｎｃｃ＋Ｎｄｃ）を１に近付けることができる。
この結果、検出周期の異なる物標検出部１０ａ～１０ｃの検出結果が、推定値Ｐｅに与える影響の違いを低減できる。

また、物標検出部１０ａ～１０ｃの観測値Ｐｃａ～Ｐｃｃや、その更新観測値Ｐｄａ～Ｐｄｃの信頼度（すなわち測定誤差）は、物標検出部１０ａ～１０ｃの特性や、作動状況、検出状態に応じてそれぞれ異なる。
このため、ゲイン設定部２６は、観測値Ｐｃａ～Ｐｃｃと更新観測値Ｐｄａ～Ｐｄｃの信頼度を設定する。例えば、物標検出部１０ａ～１０ｃの特性や、作動状況、検出状態に応じてこれらの信頼度を設定してよい。

ゲイン設定部２６は、観測値Ｐｃａ～Ｐｃｃ及び更新観測値Ｐｄａ～Ｐｄｃに乗じるゲインｇを、観測値Ｐｃａ～Ｐｃｃ及び更新観測値Ｐｄａ～Ｐｄｃの信頼度に応じてそれぞれ設定する。例えば、ゲイン設定部２６は、より信頼度が高い場合により大きなゲインｇを設定する。
例えば、ゲイン設定部２６は、これらの信頼度をカルマンフィルタにおける観測の共分散行列として設定する。具体的には信頼度の高い観測値や更新観測値に対しては小さな共分散を、信頼度の低い観測値や更新観測値に対しては大きな共分散を設定する。

これにより、信頼度に応じて重み付けされた複数の観測値Ｐｃａ～Ｐｃｃに応じて予測値Ｐｐを補正して自己位置Ｐｅを推定することができる。
また、信頼度に応じて重み付けされた複数の更新観測値Ｐｄａ～Ｐｄｃに応じて予測値Ｐｐを補正して自己位置Ｐｅを推定することができる。

（第２実施形態の効果）
（１）自己位置推定装置１は、ランドマークの相対位置をそれぞれ検出する複数の物標検出部１０ａ～１０ｃの検出結果に基づいて、複数の観測値Ｐｃａ～Ｐｃｃをそれぞれ生成する。更新観測値生成部２１ａ～２１ｃは、複数の物標検出部１０ａ～１０ｃの検出タイミングからの自車両の移動量に基づいて、複数の観測値Ｐｃａ～Ｐｃｃをそれぞれ更新した複数の更新観測値Ｐｄａ～Ｐｄｃを算出する。これにより、複数の物標検出部１０ａ～１０ｃの検出結果に基づいて自車両の自己位置Ｐｅを推定できる。

（２）ゲイン設定部２６は、複数の観測値Ｐｃａ～Ｐｃｃ毎に信頼度を定める。更新部２８は、相対位置の検出タイミングにおいて、信頼度に応じて重み付けされた複数の観測値Ｐｃａ～Ｐｃｃに応じて予測値Ｐｐを補正して自車両の自己位置Ｐｅを推定する。これにより、観測値Ｐｃａ～Ｐｃｃの信頼度に応じた補正量で予測値Ｐｐを補正して自己位置Ｐｅを推定できる。

（３）ゲイン設定部２６は、複数の更新観測値Ｐｄａ～Ｐｄｃ毎に信頼度を定める。更新部２８は、相対位置の検出タイミングの合間において、信頼度に応じて重み付けされた複数の更新観測値Ｐｄａ～Ｐｄｃに応じて予測値Ｐｐを補正して自車両の自己位置Ｐｅを推定する。これにより、更新観測値Ｐｄａ～Ｐｄｃの信頼度に応じた補正量で予測値Ｐｐを補正して自己位置Ｐｅを推定できる。

（４）複数の物標検出部１０ａ～１０ｃは、第１周期でランドマークの相対位置を検出する第１の物標検出部と、第１周期よりも長い第２周期でランドマークの相対位置を検出する第２の物標検出部とを含む。
更新観測値生成部２１ａ～２１ｃのうち第１の更新観測値生成部は、第１の物標検出部の検出タイミングからの自車両の移動量に基づいて、第１の物標検出部の検出結果に基づく観測値を更新した第１更新観測値を算出する。

更新観測値生成部２１ａ～２１ｃのうち第２の更新観測値生成部は、第１更新観測値の算出頻度よりも高い算出頻度で、第２の物標検出部の検出タイミングからの自車両の移動量に基づいて、第２の物標検出部の検出結果に基づく観測値を更新した第２更新観測値を算出する。
これにより、検出周期の異なる物標検出部１０ａ～１０ｃの検出結果が、自己位置Ｐｅの推定結果に与える影響の違いを低減できる。

１…自己位置推定装置、１０、１０ａ～１０ｃ…物標検出部、１１…地図データベース、１２…移動量検出部、１３…コントローラ、２０…予測部、２１、２１ａ～２１ｃ…更新観測値生成部、２２…スイッチ、２３…座標変換部、２４…マッチング部、２５…減算器、２６…ゲイン設定部、２７…乗算器、２８…更新部

Claims (8)

1. 自車両に対する前記自車両の周囲の物標の相対位置を検出し、予め記憶した前記物標の地図上の位置と前記相対位置とに基づき前記自車両の自己位置を推定する自己位置推定方法であって、
   前記自車両の移動量に基づいて前記自車両の自己位置の予測値を算出し、
   前記物標の前記相対位置の検出結果に基づく観測値を生成し、
   前記相対位置の検出タイミングにおいて、前記観測値に応じて前記予測値を補正して前記自車両の自己位置を推定し、
   前記相対位置の検出タイミングの合間において、前記検出タイミングからの前記自車両の移動量に基づいて前記観測値を更新した更新観測値を算出するとともに前記更新観測値に応じて前記予測値を補正して前記自車両の自己位置を推定する、
   ことを特徴とする自己位置推定方法。
2. 前記観測値として前記物標の前記相対位置の観測値を生成し、
   前記観測値と前記予測値とに基づき前記物標の地図座標系上の第１位置を予測し、
   前記更新観測値と前記予測値とに基づき前記物標の地図座標系上の第２位置を予測し、
   前記相対位置の検出タイミングにおいて前記第１位置が前記予め記憶した前記物標の地図上の位置に近づくように前記予測値を補正し、前記相対位置の検出タイミングの合間において前記第２位置が前記予め記憶した前記物標の地図上の位置に近づくように前記予測値を補正する、
   ことを特徴とする請求項１に記載の自己位置推定方法。
3. 前記観測値として前記自車両の自己位置の観測値を生成し、
   前記相対位置の検出タイミングにおいて観測値に近づくように前記予測値を補正し、前記相対位置の検出タイミングの合間において前記更新観測値に近づくように前記予測値を補正する、
   ことを特徴とする請求項１に記載の自己位置推定方法。
4. 前記物標の前記相対位置をそれぞれ検出する複数のセンサの検出結果に基づいて、複数の前記観測値をそれぞれ生成し、
   前記複数のセンサのそれぞれの検出タイミングからの前記自車両の移動量に基づいて、前記複数のセンサの検出結果に基づく前記複数の観測値をそれぞれ更新した複数の前記更新観測値を算出する、
   ことを特徴とする請求項１～３の何れか一項に記載の自己位置推定方法。
5. 前記複数の観測値毎に信頼度を定め、
   前記相対位置の検出タイミングにおいて、前記信頼度に応じて重み付けされた前記複数の観測値に応じて前記予測値を補正して前記自車両の自己位置を推定する、
   ことを特徴とする請求項４に記載の自己位置推定方法。
6. 前記複数の更新観測値毎に信頼度を定め、
   前記相対位置の検出タイミングの合間において、前記信頼度に応じて重み付けされた前記複数の更新観測値に応じて前記予測値を補正して前記自車両の自己位置を推定する、
   ことを特徴とする請求項４に記載の自己位置推定方法。
7. 前記複数のセンサは、第１周期で前記相対位置を検出する第１センサと、前記第１周期よりも長い第２周期で前記相対位置を検出する第２センサと、を含み、
   前記第１センサの検出タイミングからの前記自車両の移動量に基づいて、前記第１センサの検出結果に基づく前記観測値を更新した第１更新観測値を算出し、
   前記第１更新観測値の算出頻度よりも高い算出頻度で、前記第２センサの検出タイミングからの前記自車両の移動量に基づいて、前記第２センサの検出結果に基づく前記観測値を更新した第２更新観測値を算出する、
   ことを特徴とする請求項４～６のいずれか一項に記載の自己位置推定方法。
8. 自車両に対する前記自車両の周囲の物標の相対位置を検出し、予め記憶した前記物標の地図上の位置と前記相対位置とに基づき前記自車両の自己位置を推定する自己位置推定装置であって、
   前記物標の前記相対位置を検出するセンサと、
   前記自車両の移動量を検出する移動量検出器と、
   前記移動量に基づいて前記自車両の自己位置の予測値を算出し、前記相対位置の検出タイミングにおいて、前記物標の前記相対位置の検出結果に基づく観測値に応じて前記予測値を補正して前記自車両の自己位置を推定し、前記相対位置の検出タイミングの合間において、前記検出タイミングからの前記自車両の移動量に基づいて前記観測値を更新した更新観測値を算出するとともに前記更新観測値に応じて前記予測値を補正して前記自車両の自己位置を推定するコントローラと、
   を備えることを特徴とする自己位置推定装置。

Images