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

Description

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

複数の検出部によって計測した物標の位置と、学習地図上の物標の位置の距離を最小化するような二次元変換を決定することで、車両の位置と向きとを推定する自己位置推定装置が提案されている（特許文献１参照）。

特開２０１５－１４８６０１号公報

特許文献１に記載の技術によれば、複数の検出部によって計測した車両座標系での物標の位置に基づいて、最尤推定により地図データ上での車両の位置及び向きを推定する。そのため、複数の検出部のうち一の検出部の測定誤差が大きい場合、当該一の検出部による測定結果に引きずられて、最尤推定によって求めた車両の位置及び向きの精度が低下してしまうという問題がある。

本発明は、上記問題に鑑みてなされたものであり、その目的とするところは、複数の検出部による測定結果を組み合わせて、地図データ上での車両の位置及び向きを精度よく推定できる自己位置推定方法及び自己位置推定装置を提供することにある。

上述した問題を解決するために、本発明の一態様に係る自己位置推定方法及び自己位置推定装置は、車両の周囲の物標を検出する複数の検出部からの測定結果を用いて、地図データ上での車両の位置及び向きを推定する際に、車両の車両座標系において車両の前後方向、車両の幅方向にそれぞれ沿った物標の位置とそれらの誤差、車両から物標に向かう向きの角度とその誤差を、それぞれ推定し、推定された誤差が最小となる車両の前後方向に沿った物標の位置、車両の幅方向に沿った物標の位置、車両から物標に向かう向きの角度をそれぞれ選択し、選択した位置および角度に基づいて車両の位置及び向きを推定する。

本発明によれば、複数の検出部のうち一の検出部の測定結果が他の検出部による測定結果と比較して大きく異なってしまう場合であっても、複数の検出部による測定結果を組み合わせて、車両の位置及び向きを精度よく推定できる。

図１は、本発明の第１実施形態に係る自己位置推定装置の構成を示すブロック図である。 図２は、本発明の第１実施形態に係る自己位置推定装置の処理手順を示すフローチャートである。 図３は、複数の検出部による検出結果と、推定される物標の位置との関係を示す模式図である。 図４は、本発明の第２実施形態に係る自己位置推定装置の構成を示すブロック図である。 図５は、本発明の第２実施形態に係る自己位置推定装置によるプロファイルの作成方法を説明するための図である。 図６は、本発明の第２実施形態に係る自己位置推定装置による誤差を有する検出部の特定方法を説明するための図である。 図７は、本発明の第２実施形態に係る自己位置推定装置の処理手順を示すフローチャートである。 図８は、本発明の第２実施形態に係る自己位置推定装置による検出部の特定処理の処理手順を示すフローチャートである。 図９は、本発明の第２実施形態に係る自己位置推定装置による効果を説明するための図である。

［第１実施形態］
次に、図面を参照して、本発明の実施の形態を詳細に説明する。説明において、同一のものには同一符号を付して重複説明を省略する。

［自己位置推定装置の構成］
図１は、本実施形態に係る自己位置推定装置の構成を示すブロック図である。図１に示すように、本実施形態に係る自己位置推定装置は、複数の検出部４０と、コントローラ１００とを備える。なお、自己位置推定装置は、図示しない車両に搭載される。

検出部４０は、車両の位置を決定するための基準となる物標の、車両との相対的な位置関係を出力する。

検出部４０の例として、車両に搭載されたカメラによって撮像した画像に基づいて、車両周囲の白線を認識する白線検出部、停止線を検出する停止線検出部などが挙げられる。この他にも、車両に搭載されたレーダあるいはライダによって道路標識や建物などの車両周囲の立体物（静止物体）を検出するランドマーク検出部、人工衛星からの電波を受信することにより、地上における車両の位置を検出する衛星測位システム（ＧＰＳ）に基づく位置検出部などがある。また、検出部４０としては、車両モデルから車両の位置を推定する位置推定部も含まれる。例えば、車両モデルとしては、車両の車輪速と操舵角からオドメトリを求めて車両の位置を推定するモデルが考えられる。なお、白線や停止線等の車両周囲の道路上の表示や道路標識、建物等の車両周囲の立体物等の、静止した目標物を以下では物標と記載する。

コントローラ１００（制御部、処理部の一例）は、ＣＰＵ（中央処理装置）、メモリ、及び入出力部を備える汎用のマイクロコンピュータである。コントローラ１００には、自己位置推定装置として機能させるためのコンピュータプログラム（自己位置推定プログラム）がインストールされている。コンピュータプログラムを実行することにより、コントローラ１００は、自己位置推定装置が備える複数の情報処理回路（３１、３３、７１、７３、７５、９１、９３）として機能する。

なお、ここでは、ソフトウェアによって自己位置推定装置が備える複数の情報処理回路（３１、３３、７１、７３、７５、９１、９３）を実現する例を示す。ただし、以下に示す各情報処理を実行するための専用のハードウェアを用意して、情報処理回路（３１、３３、７１、７３、７５、９１、９３）を構成することも可能である。また、複数の情報処理回路（３１、３３、７１、７３、７５、９１、９３）を個別のハードウェアにより構成してもよい。更に、情報処理回路（３１、３３、７１、７３、７５、９１、９３）は、車両にかかわる他の制御に用いる電子制御ユニット（ＥＣＵ）と兼用してもよい。

コントローラ１００は、複数の情報処理回路（３１、３３、７１、７３、７５、９１、９３）として、推定部３１、誤差推定部３３、第１選択部７１、第２選択部７３、第３選択部７５、出力部９１、誤差統合部９３を備える。

推定部３１は、検出部４０の出力結果に基づいて、検出部４０による検出対象となっている物標の、車両座標系から見た、車両の前後方向に沿った位置、車両の幅方向に沿った位置、及び、車両の前後方向を基準として車両から物標に向かう向きの角度を推定する。なお、以下の前提として、複数の検出部４０による出力結果は、同一の物標に対する出力結果であるとする。

推定部３１による推定は、複数ある検出部４０ごとに行われる。すなわち、推定部３１は、検出部４０ごと、且つ、車両の前後方向の位置（第１座標値）、車両の幅方向の位置（第２座標値）、物標へ向かう向きを示す角度（第３座標値）ごとに、推定結果を出力する。

誤差推定部３３は、検出部４０の出力結果に基づいて、推定部３１による出力結果に対する誤差を推定する。より具体的には、検出部４０の出力結果に基づいて、検出部４０ごとに、車両の前後方向の位置に含まれる誤差（第１誤差）、車両の幅方向の位置に含まれる誤差（第２誤差）、物標へ向かう向きを示す角度に含まれる誤差（第３誤差）ごとに、推定結果を出力する。

例えば、図３には、複数の検出部４０からの出力結果として、第一の検出部４０からの第一の出力結果（位置Ｐ１（Ｘ１，Ｙ１）、誤差範囲Ｒ１）と、第二の検出部４０からの第二の出力結果（位置Ｐ２（Ｘ２，Ｙ２）、誤差範囲Ｒ２）とが示されている。推定部３１による推定は複数ある検出部４０ごとに行われ、推定部３１は、車両の前後方向の位置（第１座標値）として「位置Ｐ１の座標値Ｘ１」「位置Ｐ２の座標値Ｘ２」を推定結果として出力している。また、推定部３１は、車両の幅方向の位置（第２座標値）として「位置Ｐ１の座標値Ｙ１」「位置Ｐ２の座標値Ｙ２」を推定結果として出力している。

また、図３では、誤差推定部３３は、推定部３１による推定結果「座標値Ｘ１」「座標値Ｘ２」「座標値Ｙ１」「座標値Ｙ２」に含まれる誤差として、「誤差ΔＸ１」「誤差ΔＸ２」「誤差ΔＹ１」「誤差ΔＹ２」を推定結果として出力している。ここで、「誤差ΔＸ１」「誤差ΔＸ２」は第１誤差であり、「誤差ΔＹ１」「誤差ΔＹ２」は第２誤差である。

図３では、第３座標値、第３誤差については明示していないが、推定部３１及び誤差推定部３３は、物標へ向かう向きを示す角度についても同様に推定結果を出力する。また、図３では、複数の検出部４０の出力結果として２つのみが示されているが、それ以上の個数の出力結果が存在する場合であっても、推定部３１及び誤差推定部３３は同様に処理を行う。

なお、誤差推定部３３による誤差の推定には、種々の方法が存在する。例えば、検出部４０自身が出力する誤差情報に基づいて誤差を推定するものであってもよいし、検出部４０ごとの特性に基づく誤差モデルに基づいて、誤差を推定するものであってもよい。また、検出部４０の出力結果だけでなく、車両の走行軌跡を表現する車両モデルをコントローラ１００内部に保持しておき、当該車両モデルに基づいて推定される物標の位置と、検出部４０自身が出力する誤差情報、検出部４０ごとの特性に基づく誤差モデルに基づいて、誤差を推定するものであってもよい。

誤差推定部３３で用いる誤差モデルには、種々のモデルが存在する。例えば、検出部４０がカメラを含む場合、カメラの物標に対する移動速度と発生する誤差との関係を実測して、誤差モデルは作成されるものであってもよい。ここで、一般に、移動速度が大きくなるほど、発生する誤差は大きくなるため、移動速度の関数として、実測された誤差を評価する近似式として誤差モデルを作成することが可能である。

また、カメラの物標までの撮像距離と発生する誤差の関係を実測して、誤差モデルは作成されるものであってもよい。

上記の他にも、カメラのレンズパラメータに基づいて計算によりカメラをモデル化し、モデル化したカメラの特性に基づいて、誤差モデルは作成されるものであってもよい。

車両の走行軌跡を表現する車両モデルについても、種々のモデルが存在する。例えば、オドメトリおよび車両に車載される衛星測位システムに基づいて車両の走行軌跡を実測しておき、所定時間経過後の車両の位置を予測するモデルであってもよい。

第１選択部７１は、推定部３１によって推定された複数の第１座標値の中で、第１誤差が最小となる第１座標値を、第１選択値として選択する。また、最小となる第１誤差を最小第１誤差として選択する。図３に示す例では、第１選択部７１は、第１誤差である「誤差ΔＸ１」「誤差ΔＸ２」のうち、最小の第１誤差は「誤差ΔＸ１」であるので、複数の第１座標値である「座標値Ｘ１」「座標値Ｘ２」の中から、「誤差ΔＸ１」に対応付けられる「座標値Ｘ１」を、第１選択値として選択している。また、第１選択部７１は、「誤差ΔＸ１」を最小第１誤差として選択している。

第２選択部７３は、推定部３１によって推定された複数の第２座標値の中で、第２誤差が最小となる第２座標値を、第２選択値として選択する。また、最小となる第２誤差を最小第２誤差として選択する。図３に示す例では、第２選択部７３は、第２誤差である「誤差ΔＹ１」「誤差ΔＹ２」のうち、最小の第２誤差は「誤差ΔＹ２」であるので、複数の第２座標値である「座標値Ｙ１」「座標値Ｙ２」の中から、「誤差ΔＹ２」に対応付けられる「座標値Ｙ２」を、第２選択値として選択している。また、第２選択部７３は、「誤差ΔＹ２」を最小第２誤差として選択している。

第３選択部７５は、推定部３１によって推定された複数の第３座標値の中で、第３誤差が最小となる第３座標値を、第３選択値として選択する。また、最小となる第３誤差を最小第３誤差として選択する。第３選択部７５は、第１選択部７１及び第２選択部７３と同様に、第３選択値及び最小第３誤差を、複数の第３座標値に基づいて選択する。

出力部９１は、第１選択部７１、第２選択部７３、第３選択部７５によってそれぞれ選択された第１選択値、第２選択値、第３選択値に基づいて、検出部４０による検出対象となっている物標の位置及び角度を推定する。その他、出力部９１は、推定した物標の位置及び角度に基づいて、車両の位置及び向きを推定する。すなわち、出力部９１は、物標位置を含む地図データを予め記憶し、第１選択部７１、第２選択部７３、第３選択部７５で選択された第１選択値、第２選択値、第３選択値と地図データとを照合して、地図データ上における、車両の位置及び向きを推定する。

図３に示す例では、出力部９１は、第１選択値である「座標値Ｘ１」と、第２選択値である「座標値Ｙ２」を、物標の位置ＰＥ（Ｘ１，Ｙ２）であると推定している。

図３では、第３選択値については明示していないが、出力部９１は、物標へ向かう向きを示す角度についても、第３選択値に基づいて同様に推定する。

誤差統合部９３は、第１選択部７１、第２選択部７３、第３選択部７５によってそれぞれ選択された最小第１誤差、最小第２誤差、最小第３誤差に基づいて、出力部９１によって推定された物標の位置及び角度に関する誤差を推定する。その結果、出力部９１によって推定された、地図データ上における車両の位置及び向きに関する誤差を推定する。

例えば、誤差統合部９３は、推定された物標の位置及び角度に関する誤差が、最小第１誤差、最小第２誤差、最小第３誤差の合計であるとして推定する。

［自己位置推定装置の処理手順］
次に、本実施形態に係る自己位置推定装置による自己位置推定の処理手順を、図２のフローチャートを参照して説明する。図２に示す自己位置推定の処理は、車両のイグニッションがオンされると開始し、イグニッションがオンとなっている間、繰り返し実行される。

まず、ステップＳ１０１において、検出部４０は、車両の位置を決定するための基準となる物標の、車両との相対的な位置関係を出力する。そして、推定部３１は、検出部４０の出力結果に基づいて、検出部４０による検出対象となっている物標の、車両座標系から見た、車両の前後方向に沿った位置、車両の幅方向に沿った位置、及び、車両の前後方向を基準として車両から物標に向かう向きの角度を推定する。

ステップＳ１０３において、誤差推定部３３は、検出部４０の出力結果に基づいて、推定部３１による出力結果に対する誤差を推定する。

ステップＳ１０５において、第１選択部７１は、推定部３１によって推定された複数の第１座標値の中で、第１誤差が最小となる第１座標値を、第１選択値として選択する。また、最小となる第１誤差を最小第１誤差として選択する。

ステップＳ１０７において、第２選択部７３は、推定部３１によって推定された複数の第２座標値の中で、第２誤差が最小となる第２座標値を、第２選択値として選択する。また、最小となる第２誤差を最小第２誤差として選択する。

ステップＳ１０９において、第３選択部７５は、推定部３１によって推定された複数の第３座標値の中で、第３誤差が最小となる第３座標値を、第３選択値として選択する。また、最小となる第３誤差を最小第３誤差として選択する。

ステップＳ１１１において、出力部９１は、第１選択部７１、第２選択部７３、第３選択部７５に基づいて、地図データ上における、車両の位置及び向きを推定する。

ステップＳ１１３において、誤差統合部９３は、出力部９１によって推定された、地図データ上における車両の位置及び向きに関する誤差を推定する。

［実施形態の効果］
以上詳細に説明したように、本実施形態に係る自己位置推定方法及び自己位置推定装置は、車両の周囲の物標を検出する複数の検出部からの測定結果を用いて、地図データ上での車両の位置及び向きを推定する際に、複数の検出部ごとに、車両の車両座標系において、車両の前後方向に沿った物標の位置を第１座標値として推定し、第１座標値が有する誤差を第１誤差として推定し、車両の幅方向に沿った物標の位置を第２座標値として推定し、第２座標値が有する誤差を第２誤差として推定し、車両の前後方向を基準として車両から物標に向かう向きの角度を第３座標値として推定し、第３座標値が有する誤差を第３誤差として推定する。そして、推定された複数の第１座標値の中で第１誤差が最小となる第１座標値を、第１選択値として選択し、推定された複数の第２座標値の中で第２誤差が最小となる第２座標値を、第２選択値として選択し、推定された複数の第３座標値の中で第３誤差が最小となる第３座標値を、第３選択値として選択し、第１選択値、第２選択値、第３選択値に基づいて、車両の位置及び向きを推定する。

これにより、複数の検出部のうち一の検出部の測定結果が他の検出部による測定結果と比較して大きく異なってしまう場合であっても、複数の検出部による測定結果の中から、誤差が最も小さいと推定される量（車両の前後方向の位置（第１座標値）、車両の幅方向の位置（第２座標値）、物標へ向かう向きを示す角度（第３座標値））同士を組み合わせ、車両の位置及び向きを精度よく推定できる。

最尤推定による方法では、複数の検出部による複数の測定結果の中に、誤差の大きな測定結果が混入してしまう場合に、誤差の大きな測定結果に引きずられて、推定された車両の位置及び向きの精度が悪化してしまうことがある。結局のところ、最尤推定による方法は、測定結果が無限個の極限では正しい結果を与えるが、現実の測定では有限個の検出部しか利用することができないため、有限個の測定結果しか得られない。そのため、最尤推定による方法では、測定結果の誤差が問題となりうるのである。これと比較して、本実施形態による方法によれば、誤差の大きな測定結果に引きずられて、推定された車両の位置及び向きの精度が悪化してしまうことを抑制できる。

また、本実施形態に係る自己位置推定方法及び自己位置推定装置において、第１選択値が有する第１誤差、第２選択値が有する第２誤差、第３選択値が有する第３誤差の合計を算出するものであってもよい。これにより、推定された車両の位置及び向きについての誤差を評価することができる。また、車両モデルを介して将来の車両の位置及び向きについての誤差を評価するための基となるデータとすることができる。

さらに、本実施形態に係る自己位置推定方法及び自己位置推定装置において、検出部ごとの誤差モデルを用いて、当該検出部における第１誤差、第２誤差、第３誤差をそれぞれ推定するものであってもよい。検出部毎に用意された誤差モデルを使用して誤差を推定するため、検出部毎の特性を考慮した誤差の推定を行うことができる。また、検出部が誤差モデルから離れた挙動を示した場合に、検出部が故障していること検知したり、もしくは、検出部が何らかの原因により通常の条件下で測定を行うことができていないことを検知したりできる。

また、本実施形態に係る自己位置推定方法及び自己位置推定装置において、検出部がカメラである場合、誤差モデルは、カメラの移動速度と発生する誤差の間の関係を実測して作成されたものであってもよい。さらには、誤差モデルは、カメラの撮像距離と発生する誤差の関係を実測して作成されたものであってもよい。車両の移動に伴ってカメラに生じる誤差を、実測値に基づいて正確に評価でき、車両の位置及び向きを、より精度よく推定できる。

さらに、本実施形態に係る自己位置推定方法及び自己位置推定装置において、誤差モデルは、カメラのレンズパラメータに基づいて計算によりモデル化して作成されたものであってもよい。車両の移動に伴ってカメラに生じる誤差を、モデル化したカメラの特性に基づいて正確に評価でき、車両の位置及び向きを、より精度よく推定できる。

また、本実施形態に係る自己位置推定方法及び自己位置推定装置において、車両の車両モデルに基づいて、車両の位置及び向きを推定するものであってもよい。また、車両モデルは、オドメトリおよび車両に車載される衛星測位システムに基づいて実測して作成されたものであってもよい。車両モデルに基づいて推定することにより、車両の走行軌跡と推定された車両の位置及び向きとの間のズレ量を誤差の評価に利用することができ、車両の位置及び向きの推定を改善できる。

さらに、本実施形態に係る自己位置推定方法及び自己位置推定装置において、車両モデルと、検出部ごとの誤差モデルを用いて、当該検出部における第１誤差、第２誤差、第３誤差を推定するものであってもよい。検出部毎に用意された誤差モデルを使用して誤差を推定するため、検出部毎の特性を考慮した誤差の推定を行うことができる。

また、本実施形態に係る自己位置推定方法及び自己位置推定装置において、過去の時点で選択された、第１選択値、第２選択値、第３選択値に基づいて、車両の位置及び向きを推定するものであってもよい。過去の時点で選択された、第１選択値、第２選択値、第３選択値も推定に利用することで、推定に使用するデータ量を増やすことが可能となり、車両の位置及び向きを、より精度よく推定できる。

さらに、本実施形態に係る自己位置推定方法及び自己位置推定装置において、第１選択値が有する第１誤差、第２選択値が有する第２誤差、第３選択値が有する第３誤差に基づいて、車両の位置及び向きを推定するものであってもよい。これにより、推定された車両の位置及び向きについての誤差を評価することができる。また、車両モデルを介して将来の車両の位置及び向きについての誤差を評価するための基となるデータとすることができる。

［第２実施形態］
次に、図面を参照して、本発明の実施の形態を詳細に説明する。説明において、同一のものには同一符号を付して重複説明を省略する。

［自己位置推定装置の構成］
図４は、本実施形態に係る自己位置推定装置の構成を示すブロック図である。図４に示すように、本実施形態に係る自己位置推定装置は、検出部評価部１５０をさらに備えたことが第１実施形態と相違している。

検出部評価部１５０は、複数の検出部４０の中から、誤差を有する検出部４０を特定し、特定された検出部４０を第１－第３選択部７１－７５にそれぞれ出力する。その結果、第１－第３選択部７１－７５では、特定された検出部４０の測定結果を用いて推定された座標値を除外した上で、第１－第３誤差が最小となる座標値を第１－第３選択値として選択する。検出部評価部１５０は、プロファイル作成部１５２と、検出部特定部１５４とを備えている。

プロファイル作成部１５２は、複数の検出部４０ごとに、車両の車両座標系において、車両の前後方向に沿った物標の速度を第１速度として算出し、所定期間内に算出された複数の第１速度を記憶してプロファイルを作成する。同様に、プロファイル作成部１５２は、車両の幅方向に沿った物標の速度を第２速度として算出し、所定期間内に算出された複数の第２速度を記憶してプロファイルを作成する。また、車両の前後方向を基準として車両から物標に向かう向きの角速度を第３速度として算出し、所定期間内に算出された複数の第３速度を記憶してプロファイルを作成する。さらに、プロファイル作成部１５２は、基準となる速度を記憶した基準プロファイルも作成する。

プロファイル作成部１５２は、まず複数の検出部４０ごとに、検出部４０が検出対象としている物標の速度を算出する。例えば、検出部４０が白線検出部である場合には白線の移動速度を算出し、検出部４０がＧＰＳに基づく車両の位置検出システムである場合には車両の速度を算出する。速度の算出方法は、検出部４０によってそれぞれ異なっているが、基本的には所定時間内における物標の位置の変化量から求めることができる。そして、プロファイル作成部１５２は、車両の前後方向に沿った物標の速度を第１速度、車両の幅方向に沿った物標の速度を第２速度、車両の前後方向を基準として車両から物標に向かう向きの角速度を第３速度として算出する。

次に、プロファイル作成部１５２は、所定期間内に算出された複数の速度を、算出された時刻の順に並べて記録したプロファイルを作成する。例えば、プロファイルには、１０秒間の間に１秒毎に算出された１０個の速度が、算出された時刻の順に並べられて記憶されている。そして、プロファイル作成部１５２は、検出部４０ごとに第１速度と第２速度と第３速度のプロファイルをそれぞれ作成する。

また、プロファイル作成部１５２は、基準となる速度を記憶した基準プロファイルを作成する。基準プロファイルは、検出部４０の中で最も安定していると考えられる検出部の測定結果を用いて算出された速度を記憶したプロファイルである。例えば、車両モデルから推定された速度を記憶したプロファイルを基準プロファイルとすることが考えられる。車両モデルとしては、車両の車輪速と操舵角から求められるオドメトリを用いることができる。車両モデルの他にもＧＰＳによって検出された車両の位置から算出された速度を用いて基準プロファイルを作成してもよい。

次に、検出部特定部１５４は、プロファイルに記憶された第１速度と基準プロファイルに記憶された速度との差分を算出する。同様に、検出部特定部１５４は、プロファイルに記憶された第２速度と基準プロファイルに記憶された速度との差分を算出し、プロファイルに記憶された第３速度と基準プロファイルに記憶された速度との差分を算出する。そして、算出された差分を加算して差分の和を算出し、算出された差分の和が第１閾値より大きくなるか否かを判定する。その結果、算出された差分の和が第１閾値より大きくなる場合には、第１閾値より大きいと判定されたプロファイルを作成するための測定結果を出力した検出部４０を、継続して誤差を有する検出部であると特定する。

ここで、継続して誤差を有する検出部を特定する方法を、図５、６を参照して説明する。図５は、検出部４０の測定結果から算出された速度の時間変化を示す図であり、図６は、プロファイルに記憶された速度と基準プロファイルに記憶された速度との差分の和の時間変化を示す図である。

図５に示すように、プロファイル作成部１５２は、複数の検出部４０の測定結果から検出部４０ごとに速度Ｖａ、Ｖｂ、Ｖｃ、Ｖｒを算出している。速度Ｖａは検出部４０ａの測定結果から算出された速度、速度Ｖｂは検出部４０ｂの測定結果から算出された速度、速度Ｖｃは検出部４０ｃの測定結果から算出された速度である。また、速度Ｖｒは基準となる速度であり、図５ではオドメトリから求めた速度を基準としている。図５に示すように、速度Ｖａは急な減速をしており、速度Ｖｂは緩やかに減速している。一方、速度Ｖｃ、Ｖｒは加速している。

図５において、プロファイル作成部１５２は、現在時刻ｔ０より前の所定期間、例えば１０秒間に算出された速度を記憶してプロファイルを作成する。図５では、速度Ｖａ、Ｖｂ、Ｖｃのプロファイルをそれぞれ作成し、基準プロファイルとして速度Ｖｒのプロファイルを作成する。

こうしてプロファイルが作成されると、次に、検出部特定部１５４は、プロファイルに記憶された速度と基準プロファイルに記憶された速度との差分を算出する。プロファイルは、所定の時刻や所定の時間間隔で速度を記憶しているので、同一の時刻にある速度同士で差分を算出する。例えば、速度Ｖａの時刻ｔ５における速度と速度Ｖｒの時刻ｔ５における速度の差分を算出する。そして、プロファイルに記憶されている速度のすべてについてそれぞれ差分を算出する。図５の場合では、現在時刻ｔ０より前の１０秒間の速度の差分をすべて算出する。

そして、検出部特定部１５４は、算出された差分をすべて加算して差分の和を算出し、算出した差分の和が第１閾値より大きくなるか否かを判定する。図６は、図５の速度Ｖａから算出した差分の和Ｓａと、図５の速度Ｖｂから算出した差分の和Ｓｂと、図５の速度Ｖｃから算出した差分の和Ｓｃを示している。尚、第１閾値は、車両が通常の速度で直進走行している場合に、継続して誤差を有する検出部を特定するために設定された閾値であり、予め実験やシミュレーションによって設定しておけばよい。

図６に示すように、差分の和Ｓａ、Ｓｂは、時刻ｔ０において第１閾値よりも大きくなっている。すなわち、プロファイルの作成期間において、速度Ｖａ、Ｖｂは、基準となる速度Ｖｒから継続して乖離が生じていたことが分かる。したがって、速度Ｖａ、Ｖｂを算出した測定結果を出力した検出部４０は、継続して誤差を有する検出部であると特定することができる。

一方、差分の和Ｓｃは第１閾値以下となっている。すなわち、プロファイルの作成期間において、速度Ｖｃは、基準となる速度Ｖｒからの乖離が常に小さいことが分かる。したがって、速度Ｖｃを算出した測定結果を出力した検出部４０は、誤差が小さい検出部であると特定することができる。

こうして誤差を有する検出部が特定されると、検出部特定部１５４は、特定結果を第１－第３選択部７１－７５に出力する。

第１選択部７１は、特定結果を取得すると、推定部３１で推定された複数の第１座標値の中で、誤差を有すると特定された検出部４０の測定結果を用いて推定された第１座標値を除外する。そして、除外されなかった第１座標値の中から第１誤差が最小となる第１座標値を第１選択値として選択する。

同様に、第２選択部７３は、推定部３１で推定された複数の第２座標値の中で、誤差を有すると特定された検出部４０の測定結果を用いて推定された第２座標値を除外する。そして、除外されなかった第２座標値の中から第２誤差が最小となる第２座標値を第２選択値として選択する。

また、第３選択部７５は、推定部３１で推定された複数の第３座標値の中で、誤差を有すると特定された検出部４０の測定結果を用いて推定された第３座標値を除外する。そして、除外されなかった第３座標値の中から第３誤差が最小となる第３座標値を第３選択値として選択する。

この後、出力部９１は、第１選択部７１、第２選択部７３、第３選択部７５によってそれぞれ選択された第１選択値、第２選択値、第３選択値に基づいて、検出部４０による検出対象となっている物標の位置及び角度を推定する。その他、出力部９１は、推定した物標の位置及び角度に基づいて、車両の位置及び向きを推定する。

［自己位置推定装置の処理手順］
次に、本実施形態に係る自己位置推定装置による自己位置推定の処理手順を、図７のフローチャートを参照して説明する。図７に示す本実施形態の自己位置推定処理では、ステップＳ１０４が追加されたことが、図２に示す第１実施形態の自己位置推定処理と相違している。

ステップＳ１０４において、検出部評価部１５０は、複数の検出部４０の中で、継続して誤差を有する検出部４０を特定し、特定された検出部４０を第１－第３選択部７１－７５にそれぞれ出力する。以下、ステップＳ１０４で実行される検出部の特定処理を、図８のフローチャートを参照して説明する。

図８に示すように、プロファイル作成部１５２は、ステップＳ２０１において、複数の検出部４０ごとに車両の車両座標系において速度を算出し、ステップＳ２０３において、所定期間内に算出された複数の速度を記憶してプロファイルを作成する。また、プロファイル作成部１５２は、ステップＳ２０３において、基準となる速度を記憶した基準プロファイルも作成する。

ステップＳ２０５において、検出部特定部１５４は、プロファイルに記憶された速度と基準プロファイルに記憶された速度との差分を算出し、算出された差分をすべて加算して差分の和を算出する。

ステップＳ２０７において、検出部特定部１５４は、算出された差分の和が第１閾値より大きくなるか否かを判定する。そして、第１閾値より大きいと判定されたプロファイルを作成するための測定結果を出力した検出部４０を、継続して誤差を有する検出部であると特定する。この後、検出部特定部１５４は特定結果を第１－第３選択部７１－７５に出力する。

こうして、誤差を有する検出部が特定されると、ステップＳ１０４で実行される検出部の特定処理は終了する。

この後、図７の自己位置推定処理はステップＳ１０５－Ｓ１０９に進む。ステップＳ１０５－Ｓ１０９において、第１－第３選択部７１－７５は、誤差を有する検出部の特定結果を取得すると、推定部３１で推定された複数の座標値の中で、誤差を有する検出部４０の測定結果を用いて推定された座標値を除外する。そして、除外されなかった座標値の中から誤差が最小となる座標値を選択値として選択する。

こうして選択値が選択されると、ステップＳ１１１、Ｓ１１３の処理が実行されて、本実施形態に係る自己位置推定処理は終了する。

［変形例１］
上述した実施形態では、車両が通常の速度で直進走行している場合について説明しているが、車両がほぼ停止しているような低速の場合や車両が旋回しているような場合には、誤差の大きさが変化する。そのため、車両が低速の場合や旋回している場合には、上述した実施形態の第１閾値を変更する必要がある。

そこで、車両の速度が所定値以下である場合には、第１閾値よりも小さい第２閾値を用いて、継続して誤差を有する検出部４０を特定する。車両が低速で走行している場合には、誤差は小さくなるので、第２閾値は第１閾値よりも小さな値となる。尚、第２閾値は、車両がほぼ停止しているような低速で走行している場合に、誤差を有する検出部４０を特定するために設定された閾値であり、予め実験やシミュレーションによって設定することができる。

また、車両が旋回中である場合には、第１閾値の代わりに第３閾値を用いて、継続して誤差を有する検出部４０を特定する。車両が旋回中であることを判断する方法としては、車両のヨー角を検出して判断すればよい。尚、第３閾値は、車両が通常の速度で旋回している場合に、誤差を有する検出部４０を特定するために設定された閾値であり、予め実験やシミュレーションによって設定することができる。

［変形例２］
検出部４０の中には安定性が低く、一時的に大きな誤差を含んだ測定結果を出力する場合がある。そのような場合には、プロファイルに記憶された速度の中に全く異なる値の速度が含まれることになる。

そこで、検出部特定部１５４は、プロファイルに記憶された速度の中に１つでも全く異なる値の速度が含まれている場合には、そのプロファイルを作成するための測定結果を出力した検出部４０を、誤差を有する検出部であると特定する。

具体的に、検出部特定部１５４は、プロファイルに記憶された速度の中で第４閾値より大きな速度があるか否かを判定する。そして、第４閾値より大きな速度を有するプロファイルがある場合には、そのプロファイルを作成するための測定結果を出力した検出部を、誤差を有する検出部であると特定する。尚、第４閾値は、大きな誤差を含んだ速度を検出するために設定された閾値なので、他の閾値よりも大きな値に設定されており、予め実験やシミュレーションによって設定することができる。

［変形例３］
誤差を有する検出部４０を特定する方法として、速度の変化率、すなわち加速度の向き（プラスかマイナスか）が異なっているかどうかを判断することによって、検出部４０が誤差を有するかどうかを判断することができる。

そこで、検出部特定部１５４は、プロファイルに記憶された速度についてそれぞれ加速度を算出し、基準プロファイルに記憶された速度についてもそれぞれ加速度を算出する。加速度は、前後の速度の間の変化率を求めることによって算出することができる。そして、プロファイルに記憶された速度から算出された加速度の向きと基準プロファイルから算出された加速度の向きが異なる回数が第５閾値より多いか否かを判定する。その結果、第５閾値より多い場合には、第５閾値より多いプロファイルを作成するための測定結果を出力した検出部を、誤差を有する検出部であると特定する。尚、第５閾値は、継続して誤差を有する検出部において加速度の向きに異常が発生する回数を、予め実験やシミュレーションによって検証して設定しておけばよい。

［変形例４］
プロファイルに記憶された各速度の誤差は小さくても、誤差のある速度が多い場合には、そのようなプロファイルを作成するための測定結果を出力した検出部は誤差を有する検出部であると考えられる。

そこで、検出部特定部１５４は、プロファイルに記憶された速度と基準プロファイルに記憶された速度とをそれぞれ比較する。そして、速度の差が所定値以上である回数が第６閾値より多い場合には、第６閾値より多いプロファイルを作成するための測定結果を出力した検出部を、誤差を有する検出部であると特定する。尚、第６閾値は、継続して誤差を有する検出部において小さな誤差が発生する回数を、予め実験やシミュレーションによって検証して設定しておけばよい。また、小さな誤差の有無を判定するための所定値は、大きな誤差を判定するための第４閾値より小さな値に設定されている。

［実施形態の効果］
以上詳細に説明したように、本実施形態に係る自己位置推定方法及び自己位置推定装置は、物標の速度を算出して所定期間内に算出された複数の速度を記憶してプロファイルを作成し、さらに基準となる速度を記憶した基準プロファイルを作成する。そして、プロファイルに記憶された速度と基準プロファイルに記憶された速度との差分を算出し、算出された差分を加算して差分の和を算出する。その結果、算出された差分の和が第１閾値より大きくなる場合には、そのプロファイルを作成するための測定結果を出力した検出部を特定する。こうして検出部が特定されると、推定された複数の座標値の中で、特定された検出部の測定結果を用いて推定された座標値を除外し、誤差が最小となる座標値を選択値として選択する。

これにより、継続して誤差を有する検出部４０の測定結果を用いて推定された座標値を除外できるので、自己位置を推定するときに車両の位置及び向きを精度よく推定することができる。

特に、本実施形態に係る自己位置推定方法及び自己位置推定装置では、小さな誤差が継続して存在するような検出部４０の測定結果を誤って選択してしまうことを防止できる。その具体例を、図７を参照して説明する。

図７は、小さな誤差が継続して存在するような検出部４０がある場合に、本実施形態の方法を使用した場合と使用しなかった場合の自己位置の推定結果を示している。図７に示すように、地点Ａでは、本実施形態の方法を使用しないで推定した自己位置Ｐ１と、本実施形態の方法を使用して推定した自己位置Ｐ２は同じ位置にある。

この後、時間が経過して地点Ｂの推定結果では、本実施形態の方法を使用しないで推定した自己位置Ｐ１は、本実施形態の方法を使用して推定した自己位置Ｐ２よりも前方に進んでいる。

小さな誤差が継続して存在するような検出部４０がある場合、各時点における誤差は小さいので、小さな誤差を有する検出部４０の測定結果が選択されることになる。しかし、各時点における誤差は小さくても、継続して選択されて誤差が少しずつ拡大すると、地点Ｂの自己位置Ｐ１は、自己位置Ｐ２よりもだいぶ前方に進んでしまうことになる。

これに対して、本実施形態の方法を使用して推定した自己位置Ｐ２は、継続して誤差を有する検出部４０の測定結果を除外して推定されているので、誤差が少しずつ拡大していくことはなく、精度よく推定されている。

この結果、地点Ｃの推定結果では、本実施形態の方法を使用しないで推定した自己位置Ｐ１は交差点で他の車線へはみ出しているが、本実施形態の方法を使用して推定した自己位置Ｐ２は交差点ではみ出すことなく車線内を走行している。

したがって、本実施形態の方法を使用することにより、小さな誤差が継続して存在するような検出部４０が存在している場合であっても、車両の位置及び向きを精度よく推定することができる。

また、本実施形態に係る自己位置推定方法及び自己位置推定装置では、車両の速度が所定値以下である場合には、第１閾値よりも小さい第２閾値を用いている。これにより、車両がほぼ停止しているような低速の場合であっても、車両の位置及び向きを精度よく推定することができる。

さらに、本実施形態に係る自己位置推定方法及び自己位置推定装置では、車両が旋回中である場合には、第１閾値の代わりに第３閾値を用いている。これにより、車両が旋回している場合であっても、車両の位置及び向きを精度よく推定することができる。

また、本実施形態に係る自己位置推定方法及び自己位置推定装置では、プロファイルに記憶された速度の中で第４閾値より大きな速度がある場合には、第４閾値より大きな速度を有するプロファイルを作成するための測定結果を出力した検出部を特定する。これにより、安定性が低い検出部４０によって正常値とは全く異なる測定結果が出力された場合であっても、車両の位置及び向きを精度よく推定することができる。

さらに、本実施形態に係る自己位置推定方法及び自己位置推定装置では、プロファイルに記憶された速度についてそれぞれ加速度を算出し、基準プロファイルに記憶された速度についてそれぞれ加速度を算出する。そして、プロファイルの速度から算出された加速度の向きと基準プロファイルから算出された加速度の向きが異なる回数が第５閾値より多い場合には、第５閾値より多いプロファイルを作成するための測定結果を出力した検出部を特定する。これにより、継続して誤差を有する検出部を特定することができるので、車両の位置及び向きを精度よく推定することができる。

また、本実施形態に係る自己位置推定方法及び自己位置推定装置では、プロファイルに記憶された速度と基準プロファイルに記憶された速度とをそれぞれ比較する。そして、速度の差が所定値以上である回数が第６閾値より多い場合には、第６閾値より多いプロファイルを作成するための測定結果を出力した検出部を特定する。これにより、プロファイルに記憶された各速度の誤差は小さくても誤差を含んだ速度が多い場合には、そのような測定結果を出力した検出部を特定できるので、車両の位置及び向きを精度よく推定することができる。

上述の実施形態で示した各機能は、１又は複数の処理回路によって実装されうる。処理回路には、プログラムされたプロセッサや、電気回路などが含まれ、さらには、特定用途向けの集積回路（ＡＳＩＣ）のような装置や、記載された機能を実行するよう配置された回路構成要素なども含まれる。

以上、実施形態に沿って本発明の内容を説明したが、本発明はこれらの記載に限定されるものではなく、種々の変形及び改良が可能であることは、当業者には自明である。この開示の一部をなす論述および図面は本発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施形態、実施例および運用技術が明らかとなろう。

本発明はここでは記載していない様々な実施形態等を含むことは勿論である。したがって、本発明の技術的範囲は上記の説明から妥当な特許請求の範囲に係る発明特定事項によってのみ定められるものである。

３１ 推定部
３３ 誤差推定部
４０ 検出部
７１ 第１選択部
７３ 第２選択部
７５ 第３選択部
９１ 出力部
９３ 誤差統合部
１００ コントローラ
１５０ 検出部評価部
１５２ プロファイル作成部
１５４ 検出部特定部

Claims (18)

1. 車両の周囲の物標を検出する複数の検出部からの測定結果を用いて、地図データ上での前記車両の位置及び向きを推定する自己位置推定方法であって、
   複数の前記検出部ごとに、前記車両の車両座標系において、
   前記車両の前後方向に沿った前記物標の位置を第１座標値として推定し、前記第１座標値が有する誤差を第１誤差として推定し、
   前記車両の幅方向に沿った前記物標の位置を第２座標値として推定し、前記第２座標値が有する誤差を第２誤差として推定し、
   前記車両の前後方向を基準として前記車両から前記物標に向かう向きの角度を第３座標値として推定し、前記第３座標値が有する誤差を第３誤差として推定し、
   推定された複数の第１座標値の中で第１誤差が最小となる第１座標値を、第１選択値として選択し、
   推定された複数の第２座標値の中で第２誤差が最小となる第２座標値を、第２選択値として選択し、
   推定された複数の第３座標値の中で第３誤差が最小となる第３座標値を、第３選択値として選択し、
   前記第１選択値、前記第２選択値、前記第３選択値に基づいて、前記車両の位置及び向きを推定する
   ことを特徴とする自己位置推定方法。
2. 請求項１に記載の自己位置推定方法であって、
   複数の前記検出部ごとに、前記車両の車両座標系において、
   前記車両の前後方向に沿った前記物標の速度を第１速度として算出し、所定期間内に算出された複数の前記第１速度を記憶してプロファイルを作成し、
   前記車両の幅方向に沿った前記物標の速度を第２速度として算出し、所定期間内に算出された複数の前記第２速度を記憶してプロファイルを作成し、
   前記車両の前後方向を基準として前記車両から前記物標に向かう向きの角速度を第３速度として算出し、所定期間内に算出された複数の前記第３速度を記憶してプロファイルを作成し、
   基準となる速度を記憶した基準プロファイルを作成し、
   前記プロファイルに記憶された前記第１速度と前記基準プロファイルに記憶された速度との差分を算出し、算出された前記差分を加算して差分の和を算出し、算出された前記差分の和が第１閾値より大きくなる場合には、前記プロファイルを作成するための測定結果を出力した前記検出部を特定し、
   前記プロファイルに記憶された前記第２速度と前記基準プロファイルに記憶された速度との差分を算出し、算出された前記差分を加算して差分の和を算出し、算出された前記差分の和が第１閾値より大きくなる場合には、前記プロファイルを作成するための測定結果を出力した前記検出部を特定し、
   前記プロファイルに記憶された前記第３速度と前記基準プロファイルに記憶された速度との差分を算出し、算出された前記差分を加算して差分の和を算出し、算出された前記差分の和が第１閾値より大きくなる場合には、前記プロファイルを作成するための測定結果を出力した前記検出部を特定し、
   推定された複数の前記第１座標値の中で、特定された前記検出部の測定結果を用いて推定された第１座標値を除外して、前記第１誤差が最小となる第１座標値を前記第１選択値として選択し、
   推定された複数の前記第２座標値の中で、特定された前記検出部の測定結果を用いて推定された第２座標値を除外して、前記第２誤差が最小となる第２座標値を前記第２選択値として選択し、
   推定された複数の前記第３座標値の中で、特定された前記検出部の測定結果を用いて推定された第３座標値を除外して、前記第３誤差が最小となる第３座標値を前記第３選択値として選択する
   ことを特徴とする自己位置推定方法。
3. 請求項２に記載の自己位置推定方法であって、
   前記車両の速度が所定値以下である場合には、前記第１閾値よりも小さい第２閾値を用いることを特徴とする自己位置推定方法。
4. 請求項２又は３に記載の自己位置推定方法であって、
   前記車両が旋回中である場合には、前記第１閾値の代わりに第３閾値を用いることを特徴とする自己位置推定方法。
5. 請求項２～４のいずれか一項に記載の自己位置推定方法であって、
   前記プロファイルに記憶された前記第１速度の中で第４閾値より大きな速度がある場合には、前記第４閾値より大きな速度を有するプロファイルを作成するための測定結果を出力した前記検出部を特定し、
   前記プロファイルに記憶された前記第２速度の中で第４閾値より大きな速度がある場合には、前記第４閾値より大きな速度を有するプロファイルを作成するための測定結果を出力した前記検出部を特定し、
   前記プロファイルに記憶された前記第３速度の中で第４閾値より大きな速度がある場合には、前記第４閾値より大きな速度を有するプロファイルを作成するための測定結果を出力した前記検出部を特定する
   ことを特徴とする自己位置推定方法。
6. 請求項２～５のいずれか一項に記載の自己位置推定方法であって、
   前記プロファイルに記憶された前記第１速度についてそれぞれ加速度を算出し、前記基準プロファイルに記憶された速度についてそれぞれ加速度を算出し、前記第１速度から算出された加速度の向きと前記基準プロファイルの速度から算出された加速度の向きが異なる回数が、第５閾値より多い場合には、前記第５閾値より多いプロファイルを作成するための測定結果を出力した前記検出部を特定し、
   前記プロファイルに記憶された前記第２速度についてそれぞれ加速度を算出し、前記基準プロファイルに記憶された速度についてそれぞれ加速度を算出し、前記第２速度から算出された加速度の向きと前記基準プロファイルの速度から算出された加速度の向きが異なる回数が、第５閾値より多い場合には、前記第５閾値より多いプロファイルを作成するための測定結果を出力した前記検出部を特定し、
   前記プロファイルに記憶された前記第３速度についてそれぞれ加速度を算出し、前記基準プロファイルに記憶された速度についてそれぞれ加速度を算出し、前記第３速度から算出された加速度の向きと前記基準プロファイルの速度から算出された加速度の向きが異なる回数が、第５閾値より多い場合には、前記第５閾値より多いプロファイルを作成するための測定結果を出力した前記検出部を特定する
   ことを特徴とする自己位置推定方法。
7. 請求項２～６のいずれか一項に記載の自己位置推定方法であって、
   前記プロファイルに記憶された前記第１速度と前記基準プロファイルに記憶された速度とをそれぞれ比較して、速度の差が所定値以上である回数が第６閾値より多い場合には、前記第６閾値より多いプロファイルを作成するための測定結果を出力した前記検出部を特定し、
   前記プロファイルに記憶された前記第２速度と前記基準プロファイルに記憶された速度とをそれぞれ比較して、速度の差が所定値以上である回数が第６閾値より多い場合には、前記第６閾値より多いプロファイルを作成するための測定結果を出力した前記検出部を特定し、
   前記プロファイルに記憶された前記第３速度と前記基準プロファイルに記憶された速度とをそれぞれ比較して、速度の差が所定値以上である回数が第６閾値より多い場合には、前記第６閾値より多いプロファイルを作成するための測定結果を出力した前記検出部を特定する
   ことを特徴とする自己位置推定方法。
8. 請求項１～７のいずれか一項に記載の自己位置推定方法であって、
   前記第１選択値が有する前記第１誤差、
   前記第２選択値が有する前記第２誤差、
   前記第３選択値が有する前記第３誤差
   の合計を算出する
   ことを特徴とする自己位置推定方法。
9. 請求項１～８のいずれか一項に記載の自己位置推定方法であって、
   前記検出部ごとの誤差モデルを用いて、当該検出部における前記第１誤差、前記第２誤差、前記第３誤差をそれぞれ推定する
   ことを特徴とする自己位置推定方法。
10. 請求項９に記載の自己位置推定方法であって、
    前記検出部がカメラである場合、前記誤差モデルは、前記カメラの移動速度と発生する誤差の間の関係を実測して作成されたものである
    ことを特徴とする自己位置推定方法。
11. 請求項１０に記載の自己位置推定方法であって、
    前記誤差モデルは、前記カメラの撮像距離と発生する誤差の関係を実測して作成されたものである
    ことを特徴とする自己位置推定方法。
12. 請求項１０又は１１に記載の自己位置推定方法であって、
    前記誤差モデルは、前記カメラのレンズパラメータに基づいて計算によりモデル化して作成されたものである
    ことを特徴とする自己位置推定方法。
13. 請求項１～１２のいずれか一項に記載の自己位置推定方法であって、
    前記車両の車両モデルに基づいて、前記車両の位置及び向きを推定する
    ことを特徴とする自己位置推定方法。
14. 請求項１３に記載の自己位置推定方法であって、
    前記車両モデルは、オドメトリおよび前記車両に車載される衛星測位システムに基づいて実測して作成されたものである
    ことを特徴とする自己位置推定方法。
15. 請求項１３又は１４に記載の自己位置推定方法であって、
    前記車両モデルと、前記検出部ごとの誤差モデルを用いて、当該検出部における前記第１誤差、前記第２誤差、前記第３誤差を推定する
    ことを特徴とする自己位置推定方法。
16. 請求項１～１５のいずれか一項に記載の自己位置推定方法であって、
    過去の時点で選択された、前記第１選択値、前記第２選択値、前記第３選択値に基づいて、前記車両の位置及び向きを推定する
    ことを特徴とする自己位置推定方法。
17. 請求項１～１６のいずれか一項に記載の自己位置推定方法であって、
    前記第１選択値が有する前記第１誤差、前記第２選択値が有する前記第２誤差、前記第３選択値が有する前記第３誤差に基づいて、前記車両の位置及び向きを推定する
    ことを特徴とする自己位置推定方法。
18. 車両の周囲の物標を検出する複数の検出部と、
    コントローラと、
    を備える、地図データ上での前記車両の位置及び向きを推定する自己位置推定装置であって、
    前記コントローラは、
    複数の前記検出部ごとに、前記車両の車両座標系において、
    前記車両の前後方向に沿った前記物標の位置を第１座標値として推定し、前記第１座標値が有する誤差を第１誤差として推定し、
    前記車両の幅方向に沿った前記物標の位置を第２座標値として推定し、前記第２座標値が有する誤差を第２誤差として推定し、
    前記車両の前後方向を基準として前記車両から前記物標に向かう向きの角度を第３座標値として推定し、前記第３座標値が有する誤差を第３誤差として推定し、
    推定された複数の第１座標値の中で第１誤差が最小となる第１座標値を、第１選択値として選択し、
    推定された複数の第２座標値の中で第２誤差が最小となる第２座標値を、第２選択値として選択し、
    推定された複数の第３座標値の中で第３誤差が最小となる第３座標値を、第３選択値として選択し、
    前記第１選択値、前記第２選択値、前記第３選択値に基づいて、前記車両の位置及び向きを推定する
    ことを特徴とする自己位置推定装置。
    
    

Images