(57)【要約】【課題】自車位置の推定結果のふらつきが続くことを抑制することができる自車位置推定装置を提供する。【解決手段】車両の周囲の物標を認識する物標認識部と、物標の地図上の位置情報を含む物標情報を記憶する物標データベースと、物標認識部による物標の認識結果と物標情報とに基づいて車両の地図上の観測位置を推定する観測位置推定部と、車両の内部センサの検出結果に基づいて過去の自車位置の推定結果から車両の地図上の予測位置を演算する予測位置演算部と、観測位置及び予測位置に基づいて自車位置を推定する自車位置推定部と、自車位置の変動に基づいて、自車位置推定部は、自車位置の推定結果のふらつきが生じていると判定された場合、予測位置に近い自車位置を推定するように自車位置の推定における予測位置の重み付けを重くする。【選択図】図４
2020034441.tif 000002
【特許請求の範囲】
【請求項１】
車両の地図上の位置である自車位置を推定する自車位置推定装置であって、
前記車両の外部センサの検出結果に基づいて、前記車両の周囲の物標を認識する物標認識部と、
前記物標の地図上の位置情報を含む物標情報を記憶する物標データベースと、
前記物標認識部による前記物標の認識結果と前記物標情報とに基づいて、前記車両の地図上の観測位置を推定する観測位置推定部と、
前記車両の内部センサの検出結果に基づいて、過去の前記自車位置の推定結果から前記車両の地図上の予測位置を演算する予測位置演算部と、
前記観測位置及び前記予測位置に基づいて、前記自車位置を推定する自車位置推定部と、
前記自車位置の変動に基づいて、前記自車位置の推定結果のふらつきが生じているか否かを判定するふらつき判定部と、
を備え、
前記自車位置推定部は、前記ふらつき判定部により前記自車位置の推定結果のふらつきが生じていると判定された場合、前記予測位置に近い前記自車位置を推定するように前記自車位置の推定における前記予測位置の重み付けを重くする、自車位置推定装置。
【請求項２】
前記自車位置推定部は、位置用カルマンフィルタを用いて前記観測位置及び前記予測位置から前記自車位置を推定し、前記位置用カルマンフィルタのゲインを変更することで前記予測位置の重み付けを変更する、請求項１に記載の自車位置推定装置。
【請求項３】
車両の地図上の位置である自車位置及び前記自車位置の方位角を推定する自車位置推定装置であって、
前記車両の外部センサの検出結果に基づいて前記車両の周囲の物標を認識する物標認識部と、
前記物標の地図上の位置情報を含む物標情報を記憶する物標データベースと、
前記物標認識部による前記物標の認識結果と前記物標情報とに基づいて、前記車両の地図上の観測位置及び前記車両の地図上の観測方位角を推定する観測位置推定部と、
前記車両の内部センサの検出結果に基づいて、過去の前記自車位置及び過去の前記自車位置の方位角の推定結果から前記車両の地図上の予測位置及び前記車両の地図上の予測方位角を演算する予測位置演算部と、
前記観測位置及び前記観測方位角と前記予測位置及び前記予測方位角とに基づいて、前記自車位置及び前記自車位置の方位角を推定する自車位置推定部と、
前記自車位置の変動又は前記自車位置の方位角の変動に基づいて、前記自車位置の推定結果のふらつきが生じているか否かを判定するふらつき判定部と、
を備え、
前記自車位置推定部は、前記ふらつき判定部により前記自車位置の推定結果のふらつきが生じていると判定された場合、前記予測位置に近い前記自車位置を推定するように前記自車位置の推定における前記予測位置の重み付けを重くすると共に前記予測方位角に近い前記自車位置の方位角を推定するように前記予測方位角の重み付けを重くする自車位置推定装置。
【請求項４】
前記自車位置推定部は、位置用カルマンフィルタを用いて前記観測位置及び前記予測位置から前記自車位置を推定し、前記位置用カルマンフィルタのゲインを変更することで前記予測位置の重み付けを変更する、請求項３に記載の自車位置推定装置。
【請求項５】
前記自車位置推定部は、方位角用カルマンフィルタを用いて前記観測方位角及び前記予測方位角から前記自車位置の方位角を推定し、前記方位角用カルマンフィルタの方位角ゲインを変更することで前記予測方位角の重み付けを変更する、請求項３又は４に記載の自車位置推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自車位置推定装置に関する。
【背景技術】
【０００２】
従来、自車位置推定装置に関する技術文献として、特許第４１６５４０７号公報が知られている。この公報には、移動体の位置を推定する位置推定装置であって、ＧＰＳの推定位置に速度情報を参照して得た測位位置情報と過去のＧＰＳの推定位置の変化から予測された予測位置情報とを取得し、移動体の移動速度などに応じて測位位置情報及び予測位置情報に重み付けそれぞれを行い、重み付けに応じて測位位置情報及び予測位置情報の何れかを選択して表示する装置が示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第４１６５４０７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、車両の地図上の位置である自車位置の推定において、道路上の区画線などの物標を用いることが知られている。物標を用いた自車位置の推定は、車両の外部センサによる物標の認識結果と予め記憶されている物標の地図上の位置情報とを用いて行われる。しかしながら、物標を用いた自車位置推定だけでは、区画線の擦れなどの物標の状態や誤検出などによって自車位置のふらつきが発生するため検討の余地がある。
【０００５】
そこで、本技術分野では、自車位置の推定結果のふらつきが続くことを抑制することができる自車位置推定装置を提供することが望まれている。
【課題を解決するための手段】
【０００６】
上記課題を解決するため、本発明の一態様は、車両の地図上の位置である自車位置を推定する自車位置推定装置であって、車両の外部センサの検出結果に基づいて、車両の周囲の物標を認識する物標認識部と、物標の地図上の位置情報を含む物標情報を記憶する物標データベースと、物標認識部による物標の認識結果と物標情報とに基づいて、車両の地図上の観測位置を推定する観測位置推定部と、車両の内部センサの検出結果に基づいて、過去の自車位置の推定結果から車両の地図上の予測位置を演算する予測位置演算部と、観測位置及び予測位置に基づいて、自車位置を推定する自車位置推定部と、自車位置の変動に基づいて、自車位置の推定結果のふらつきが生じているか否かを判定するふらつき判定部と、を備え、自車位置推定部は、ふらつき判定部により自車位置の推定結果のふらつきが生じていると判定された場合、予測位置に近い自車位置を推定するように自車位置の推定における予測位置の重み付けを重くする。
【０００７】
本発明の一態様に係る自車位置推定装置によれば、区画線の擦れなどの物標の状態や誤検出などによる観測位置のズレの影響から自車位置の推定結果のふらつきが生じているときには、車速、加速度、又はヨーレートなどの内部センサの検出結果から演算された予測位置に近い自車位置を推定するように予測位置の重み付けを重くするので、観測位置と予測位置とを固定した割合で自車位置推定に用いる場合と比べて、自車位置の推定結果のふらつきが続くことを抑制することができる。
【０００８】
本発明の一態様に係る自車位置推定装置において、自車位置推定部は、位置用カルマンフィルタを用いて観測位置及び予測位置から自車位置を推定し、位置用カルマンフィルタのゲインを変更することで予測位置の重み付けを変更してもよい。
この自車位置推定装置によれば、位置用カルマンフィルタを用いることなく、観測位置と予測位置との割合から自車位置を推定する場合と比べて、自車位置の推定精度の向上を図ることができる。また、この場合にはゲインの変更により容易に予測位置の重み付けを変更することができる。
【０００９】
本発明の他の態様は、車両の地図上の位置である自車位置及び自車位置の方位角を推定する自車位置推定装置であって、車両の外部センサの検出結果に基づいて車両の周囲の物標を認識する物標認識部と、物標の地図上の位置情報を含む物標情報を記憶する物標データベースと、物標認識部による物標の認識結果と物標情報とに基づいて、車両の地図上の観測位置及び車両の地図上の観測方位角を推定する観測位置推定部と、車両の内部センサの検出結果に基づいて、過去の自車位置及び過去の自車位置の方位角の推定結果から車両の地図上の予測位置及び車両の地図上の予測方位角を演算する予測位置演算部と、観測位置及び観測方位角と予測位置及び予測方位角とに基づいて、自車位置及び自車位置の方位角を推定する自車位置推定部と、自車位置の変動又は自車位置の方位角の変動に基づいて、自車位置の推定結果のふらつきが生じているか否かを判定するふらつき判定部と、を備え、自車位置推定部は、ふらつき判定部により自車位置の推定結果のふらつきが生じていると判定された場合、予測位置に近い自車位置を推定するように自車位置の推定における予測位置の重み付けを重くすると共に予測方位角に近い自車位置の方位角を推定するように予測方位角の重み付けを重くする。
【００１０】
本発明の他の態様に係る自車位置推定装置によれば、区画線の擦れなどの物標の状態や誤検出などによる観測位置のズレの影響から自車位置の推定結果のふらつきが生じているときには、車速、加速度、又はヨーレートなどの内部センサの検出結果から演算された予測位置に近い自車位置を推定するように予測位置の重み付けを重くするので、観測位置のみを自車位置推定に用いる場合及び観測位置と予測位置とを固定した割合で自車位置推定に用いる場合と比べて、自車位置の推定結果のふらつきが続くことを抑制することができる。また、この自車位置推定装置によれば、自車位置の推定結果のふらつきが生じているときには、予測方位角に近い方位角を推定するように予測方位角の重み付けを重くするので、観測方位角と予測方位角とを固定した割合で自車位置の方位角の推定に用いる場合と比べて、自車位置の方位角のふらつきが続くことを抑制することができる。
【００１１】
本発明の他の態様に係る自車位置推定装置において、自車位置推定部は、位置用カルマンフィルタを用いて観測位置及び予測位置から自車位置を推定し、位置用カルマンフィルタのゲインを変更することで予測位置の重み付けを変更してもよい。
この自車位置推定装置によれば、位置用カルマンフィルタを用いることなく、観測位置と予測位置との割合から自車位置を推定する場合と比べて、自車位置の推定精度の向上を図ることができる。また、この場合にはゲインの変更により容易に予測位置の重み付けを変更することができる。
【００１２】
本発明の他の態様に係る自車位置推定装置において、自車位置推定部は、方位角用カルマンフィルタを用いて観測方位角及び予測方位角から自車位置の方位角を推定し、方位角用カルマンフィルタの方位角ゲインを変更することで予測方位角の重み付けを変更してもよい。
この自車位置推定装置によれば、方位角用カルマンフィルタを用いることなく、観測方位角と予測方位角との割合から自車位置の方位角を推定する場合と比べて、自車位置の方位角の推定精度の向上を図ることができる。また、この場合には方位角ゲインの変更により容易に予測方位角の重み付けを変更することができる。
【発明の効果】
【００１３】
以上説明したように、本発明の一態様又は他の態様に係る自車位置推定装置によれば、自車位置の推定結果のふらつきが続くことを抑制することができる。
【図面の簡単な説明】
【００１４】
【図１】第１実施形態に係る自車位置推定装置を示すブロック図である。
【図２】予測位置及び観測位置に基づく自車位置の推定を説明するための平面図である。
【図３】（ａ）予測位置の演算処理の一例を示すフローチャートである。（ｂ）観測位置を用いた自車位置の推定処理の一例を示すフローチャートである。
【図４】重み付け変更処理の一例を示すフローチャートである。
【図５】第２実施形態に係る自車位置推定装置を示すブロック図である。
【図６】（ａ）予測位置及び予測方位角の演算処理の一例を示すフローチャートである。（ｂ）観測位置及び観測方位角を用いた自車位置及び自車位置の方位角の推定処理の一例を示すフローチャートである。
【図７】第２実施形態における重み付け変更処理の一例を示すフローチャートである。
【発明を実施するための形態】
【００１５】
以下、本発明の実施形態について図面を参照して説明する。
【００１６】
［第１実施形態］
図１に示す第１実施形態に係る自車位置推定装置１００は、乗用車などの車両に搭載され、車両（自車両）の地図上の位置である自車位置の推定を行う装置である。車両の地図上の位置の基準は特に限定されず、予め決められた車両の中心位置を基準としてもよく、車両の重心位置を基準としてもよい。
【００１７】
〈第１実施形態に係る自車位置推定装置の構成〉
図１に示すように、自車位置推定装置１００は、システムを統括的に管理するＥＣＵ[Electronic Control Unit]１０を備えている。ＥＣＵ１０は、ＣＰＵ[CentralProcessing Unit]、ＲＯＭ[Read Only Memory]、ＲＡＭ[Random Access Memory]などを有する電子制御ユニットである。ＥＣＵ１０では、例えば、ＲＯＭに記憶されているプログラムをＲＡＭにロードし、ＲＡＭにロードされたプログラムをＣＰＵで実行することにより各種の機能を実現する。ＥＣＵ１０は、複数の電子ユニットから構成されていてもよい。
【００１８】
ＥＣＵ１０は、ＧＰＳ［Global Positioning System］受信部１、外部センサ２、内部センサ３、地図データベース４、及び物標データベース５と接続されている。
【００１９】
ＧＰＳ受信部１は、３個以上のＧＰＳ衛星から信号を受信することにより、車両の地図上の位置（例えば車両の緯度及び経度）を測定する測定部である。ＧＰＳ受信部１は、測定した車両の位置情報（測定結果）をＥＣＵ１０へ送信する。ＧＰＳ受信部１に代えてＧＮＳＳ［Global Navigation Satellite System］受信部を用いてもよい。
【００２０】
外部センサ２は、車両の周辺の状況を検出する検出機器である。外部センサ２は、後述する物標データベース５に記憶された物標を検出するセンサとしても機能する。外部センサ２は、カメラ、レーダセンサのうち少なくとも一つを含む。
【００２１】
カメラは、車両の外部状況を撮像する撮像機器である。カメラは、車両のフロントガラスの裏側に設けられ、車両前方を撮像する。カメラは、車両の外部状況に関する撮像情報をＥＣＵ１０へ送信する。カメラは、単眼カメラであってもよく、ステレオカメラであってもよい。カメラは、複数台設けられていてもよく、車両の前方以外の方向を撮像してもよい。
【００２２】
レーダセンサは、電波（例えばミリ波）又は光を利用して車両の周辺の物体を検出する検出機器である。レーダセンサには、例えば、ミリ波レーダ又はライダー［LIDAR：Light Detection and Ranging］が含まれる。レーダセンサは、電波又は光を車両の周辺に送信し、物体で反射された電波又は光を受信することで物体を検出する。レーダセンサは、検出した物体情報をＥＣＵ１０へ送信する。物体には、ガードレール、建物等の固定物体の他、歩行者、自転車、他車両等の移動物体が含まれる。
【００２３】
内部センサ３は、車両の走行状態を検出する検出機器である。内部センサ３は、車速センサ、加速度センサ、及びヨーレートセンサを含む。車速センサは、車両の速度を検出する検出器である。車速センサとしては、例えば、車両の車輪又は車輪と一体に回転するドライブシャフトなどに対して設けられ、車輪の回転速度を検出する車輪速センサが用いられる。車速センサは、検出した車速情報（車輪速情報）をＥＣＵ１０に送信する。
【００２４】
加速度センサは、車両の加速度を検出する検出器である。加速度センサは、例えば、車両の前後方向の加速度を検出する前後加速度センサと、車両の横加速度を検出する横加速度センサとを含んでいる。加速度センサは、例えば、車両の加速度情報をＥＣＵ１０に送信する。ヨーレートセンサは、車両の重心の鉛直軸周りのヨーレート（回転角速度）を検出する検出器である。ヨーレートセンサとしては、例えばジャイロセンサを用いることができる。ヨーレートセンサは、検出した車両のヨーレート情報をＥＣＵ１０へ送信する。内部センサ３は、オドメトリなどによって車両の地図上の位置の測定に利用される場合には、車載の位置測定部に相当する。
【００２５】
地図データベース４は、地図情報を記憶するデータベースである。地図データベース４は、例えば、車両に搭載されたＨＤＤ［Hard Disk Drive］などの記憶装置内に形成されている。地図情報には、道路の位置情報（車線の位置情報を含む）、道路形状の情報（曲率など）、交差点及び分岐点の位置情報などが含まれる。なお、地図データベース４は、車両と通信可能なサーバに形成されていてもよい。
【００２６】
物標データベース５は、物標に関する物標情報を記憶するデータベースである。物標とは、地図上の位置情報が既知であり、自車位置推定の基準として利用される物体である。物標には、道路の区画線が含まれる。物標には、一時停止線などの路面標示が含まれてもよく、道路標識、ガードレール、信号機などが含まれてもよい。
【００２７】
物標情報には、物標の地図上の位置情報と物標の外形情報が含まれる。物標の地図上の位置情報には、平面視した場合における物標の地図上の座標位置の情報と、物標の高さ情報とが含まれる。物標の外形情報とは、カメラの撮像画像又はレーダセンサの検出情報から物標を認識（特定）するために用いられる物標の外形に関する情報である。物標の外形情報には、物標の大きさが含まれていてもよい。
【００２８】
物標データベース５は、地図データベース４と一体のデータベースであってもよい。物標データベース５は、車両と通信可能なサーバに形成されていてもよい。
【００２９】
次に、ＥＣＵ１０の機能的構成について説明する。ＥＣＵ１０は、測定位置取得部１１、物標認識部１２、観測位置推定部１３、予測位置演算部１４、自車位置推定部１５、及びふらつき判定部１６を有している。以下に説明するＥＣＵ１０の機能の一部は、車両と通信可能なサーバで実行されてもよい。
【００３０】
測定位置取得部１１は、ＧＰＳ受信部１の測定した車両の位置情報（測定結果）に基づいて、車両の地図上の位置である測定位置を取得する。測定位置取得部１１は、例えば緯度経度の情報として測定位置を取得する。
【００３１】
物標認識部１２は、外部センサ２の検出結果に基づいて、車両の周囲の物標を認識する。物標認識部１２は、一例として、測定位置取得部１１の測定した車両の測定位置を用いて、物標データベース５の物標情報の中から車両の外部センサ２により検出される物標の候補を絞り込み、外部センサ２の検出結果と物標の候補の外形情報とから車両の周囲に存在する物標を認識する。物標認識部１２は、例えばカメラの撮像画像に対してエッジ抽出、ハフ変換、及びパターンマッチングなどの画像処理を行うことにより、区画線などの物標を認識することができる。物標認識は、カメラの撮像画像から認識されてもよく、レーダセンサの物体情報から認識されてもよく、撮像画像と物体情報の両方から認識されてもよい。物標認識部１２は、認識した物標の車両に対する相対位置の認識も行う。
【００３２】
物標認識部１２は、車両の測定位置を常に用いる必要はなく、既に自車位置推定を行っている場合には過去（例えば一つ前）に推定した自車位置を代わりに用いてもよい。更に、物標認識部１２は、必ずしも測定位置又は過去の自車位置を用いる必要はない。物標認識部１２は、車両の周囲に存在する物標の配置のパターンから物標の候補を絞り込んでもよく、道路案内標識の表示内容などを画像認識することにより物標情報の中から当該道路案内標識を特定してもよい。物標の認識方法は限定されず、様々な手法を採用することができる。
【００３３】
観測位置推定部１３は、物標認識部１２による物標の認識結果と物標データベース５の物標情報とに基づいて、車両の地図上の観測位置を推定する。観測位置とは、物標の認識結果から得られる車両の地図上の位置である。観測位置は、物標認識部１２の認識結果から得られる車両及び物標の相対位置と物標情報に含まれる物標の地図上の位置とに基づいて推定される。
【００３４】
観測位置推定部１３は、一例として、車両の走行する車線の左右の区画線と車両との相対位置から、横方向（車線の幅方向）における観測位置を推定する。観測位置推定部１３は、破線状の区画線を構成する各線分の縦方向の端部（車線の延在方向における端部）から、縦方向における観測位置を推定する。観測位置推定部１３は、一時停止線などの路面標示又は道路標識などから縦方向における観測位置を推定してもよい。観測位置推定部１３は、横方向における観測位置と縦方向における観測位置とから、車両の観測位置を推定する。
【００３５】
予測位置演算部１４は、内部センサ３の検出結果に基づいて、過去の自車位置の推定結果から車両の地図上の予測位置を演算する。予測位置とは、いわゆるオドメトリにより予測された車両の地図上の位置である。予測位置演算部１４は、過去の自車位置から現在の車両の地図上の位置としての予測位置を演算する。予測位置演算部１４は、例えば車速センサの検出した車速及びヨーレートセンサの検出したヨーレートに基づいて、過去の自車位置からの車両の車速及びヨーレートの履歴を用いて予測位置の演算を行うことができる。予測位置演算部１４は、車速センサの検出する車速の履歴に代えて加速度センサの検出する加速度の履歴を用いてもよい。
【００３６】
過去の自車位置は、一つ前（例えば１フレーム前）の自車位置推定により推定された自車位置であってもよく、数回前の自車位置推定により推定された自車位置であってもよい。自車位置推定を始めて行う場合においては、過去の自車位置として測定位置を用いてもよい。
【００３７】
自車位置推定部１５は、観測位置推定部１３の推測した観測位置及び予測位置演算部１４の演算した予測位置に基づいて、自車位置を推定する。ここで、図２は、予測位置及び観測位置に基づく自車位置の推定を説明するための平面図である。図２に、車両の左側区画線Ｌ１、車両の右側区画線Ｌ２（破線状の区画線）、車両の隣接車線の区画線Ｌ３、一つ前に推定された過去の自車位置Ｍｐ、車両の予測位置Ｍ１、車両の観測位置Ｍ２、及び車両の自車位置Ｍｚを示す。
【００３８】
図２に示す予測位置Ｍ１は、過去の自車位置Ｍｐからの車両の車速及びヨーレートの履歴を用いて演算された現在の車両の予測位置である。観測位置Ｍ２は、物標認識部１２の認識した左側区画線Ｌ１及び右側区画線Ｌ２の認識結果から観測位置推定部１３によって推定された現在の車両の観測位置である。車両の自車位置Ｍｚは、予測位置Ｍ１及び観測位置Ｍ２から推定された現在の車両の地図上の位置（自車位置の推定結果）である。
【００３９】
図２に示す状況において、自車位置推定部１５は、位置用カルマンフィルタを用いて予測位置Ｍ１及び観測位置Ｍ２から自車位置Ｍｚを推定する。位置用カルマンフィルタでは、カルマンフィルタにおける予想推定値としての予測位置Ｍ１と観測値としての観測位置Ｍ２を用いて、状態推定値として現在の自車位置Ｍｚを推定する。
【００４０】
ここで、拡張カルマンフィルタの一例について説明する。拡張カルマンフィルタは、例えば状態数をｎとして下記の式（１）で表わすことができる。
ｘｕｐ＝ｘｐｒｅ＋Ｋ（ｚ−Ｈ・ｘｐｒｅ）・・・（１）
式（１）において、ｘｕｐは更新後の状態推定値（ｎ行１列）、ｘｐｒｅは予想推定値（ｎ行１列）、Ｋはカルマンゲイン行列（ｎ行ｎ列）、ｚは観測値（ｎ行１列）、Ｈは観測行列（ｎ行ｎ列）である。位置カルマンフィルタにおいて、ｘｕｐは現在の自車位置Ｍｚ、ｘｐｒｅは予測位置Ｍ１、ｚは観測位置Ｍ２に対応する。
【００４１】
位置用カルマンフィルタでは、カルマンゲイン（以後、ゲインと称する）を変更することで、自車位置推定における予測位置Ｍ１の重み付けを変更することができる。例えばカルマンゲインにおけるシステムノイズを小さくすることが予測位置Ｍ１の重み付けを大きくすることになる。また、カルマンゲインにおける観測ノイズを小さくすることが観測位置Ｍ２の重み付けを大きくすることになる。
【００４２】
具体的に、ゲインは下記の式（２）で表わすことができる。
Ｋ＝Ｐｐｒｅ・Ｈｔ（Ｈ・Ｐｐｒｅ・Ｈｔ＋Ｒ）−１・・・（２）
Ｐｐｒｅは予測共分散行列（ｎ行ｎ列）、Ｈｔは観測行列の転置行列（ｎ行ｎ列）、Ｒは観測ノイズ行列である。この観測ノイズ行列の対角成分の値を小さくすることで、自車位置推定における観測値（観測位置Ｍ２）の重みを大きくすることができる。また、予測共分散行列は下記の式（３）で表わすことができる。
Ｐｐｒｅ＝Φ・Ｐｕｐ・Φｔ＋Ｑ・・・（３）
Ｐｐｒｅは現在の予測共分散行列（ｎ行ｎ列）、Ｐｕｐは前回の更新後の共分散行列（ｎ行ｎ列）、Φは状態遷移行列（ｎ行ｎ列）、Φｔは状態遷移行列の転置行列（ｎ行ｎ列）、Ｑはシステムノイズ行列（ｎ行ｎ列）である。このシステムノイズ行列の対角成分の値を小さくすることで、自車位置推定における予想推定値（予測位置Ｍ１）の重みを大きくすることができる。
【００４３】
自車位置推定部１５は、例えば、初期設定として予測位置Ｍ１と観測位置Ｍ２とが５０：５０になるようにゲインを設定する。この場合、図２に示すように、自車位置Ｍｚは、予測位置Ｍ１及び観測位置Ｍ２から等距離の位置（中間位置）として推定される。ゲインの変更について詳しくは後述する。
【００４４】
なお、自車位置推定部１５は、必ずしも位置用カルマンフィルタを用いて自車位置Ｍｚを推定する必要はない。自車位置推定部１５は、カルマンフィルタに代えてパーティクルフィルタを用いてもよい。自車位置推定部１５は、予測位置Ｍ１及び観測位置Ｍ２の間の位置の中から、予め設定された予測位置Ｍ１及び観測位置Ｍ２のそれぞれの重み付けに応じて自車位置Ｍｚを推定してもよい。自車位置推定部１５は、例えば自車位置Ｍｚの推定における予測位置Ｍ１の重み付けが４０であり、観測位置Ｍ２の重み付けが６０である場合、予測位置Ｍ１及び観測位置Ｍ２を結ぶ直線上において観測位置Ｍ２に対する距離と予測位置Ｍ１に対する距離との比が４０：６０になる位置を自車位置Ｍｚとして推定する。
【００４５】
ふらつき判定部１６は、自車位置推定部１５の推定した自車位置Ｍｚの変動に基づいて、自車位置Ｍｚの推定結果のふらつきが生じているか否かを判定する。ふらつき判定部１６は、例えば、車両の車線幅方向における自車位置Ｍｚのふらつきを判定する。
【００４６】
ふらつき判定部１６は、過去の一定回数分の自車位置の推定結果に基づいて、自車位置の推定結果における車両の車線中心からの横位置偏差の最小値と最大値の差分を演算する。ふらつき判定部１６は、差分がふらつき判定閾値以上である場合、自車位置の推定結果のふらつきが生じていると判定する。一定回数は、特に限定されないが、例えば１０回（１０フレーム分）とすることができる。ふらつき判定閾値は、予め設定された値の閾値である。ふらつき判定閾値は例えば１０ｃｍとすることができる。なお、必ずしも車線中心からの横位置偏差を用いる必要はなく、過去の一定回数分の自車位置Ｍｚにおける横位置の変動の最大値を用いてもよい。
【００４７】
ふらつき判定部１６により自車位置の推定結果のふらつきが生じていると判定された場合、自車位置推定部１５は、予測位置Ｍ１に近い自車位置Ｍｚを推定するように自車位置推定における予測位置Ｍ１の重み付けを重くする。自車位置推定部１５は、例えば位置用カルマンフィルタのゲインを変更することで、予測位置Ｍ１の重み付けを重くする。自車位置推定部１５は、一例として、初期設定において５０：５０であった予測位置Ｍ１と観測位置Ｍ２との重み付けの比を予測位置Ｍ１：観測位置Ｍ２＝６０：４０に変更する。
【００４８】
自車位置推定部１５は、自車位置Ｍｚの推定結果のふらつきが生じていると判定される度に、予測位置Ｍ１の重み付けを重くしてもよい。自車位置推定部１５は、自車位置の推定結果のふらつきが生じていると判定される度に、重み付けの比において予測位置Ｍ１が＋１０、観測位置Ｍ２が−１０となるように、予測位置Ｍ１の重み付けを重くする。重み付けの変更には上限を設けてもよい。上限は予め設定することができる。自車位置推定部１５は、予測位置Ｍ１：観測位置Ｍ２＝１００：０を上限としてもよく、予測位置Ｍ１：観測位置Ｍ２＝８０：２０を上限としてもよい。
【００４９】
また、ふらつき判定部１６は、自車位置の推定結果が安定しているか否かを判定する。ふらつき判定部１６は、過去の一定回数分の自車位置の推定結果に基づいて、車両の車線中心からの横位置偏差の最小値と最大値の差分が安定閾値未満である場合、自車位置の推定結果が安定していると判定する。一定回数はふらつき判定時の回数と同一であってもよく、異なる回数であってもよい。安定閾値は、予め設定された値の閾値であり、ふらつき判定閾値より小さい値の閾値である。安定閾値は例えば５ｃｍとすることができる。
【００５０】
ふらつき判定部１６により自車位置の推定結果が安定していると判定された場合、自車位置推定部１５は、観測位置Ｍ２に近い自車位置Ｍｚを推定するように自車位置推定における観測位置Ｍ２の重み付けを重くする。自車位置推定部１５は、例えば位置用カルマンフィルタのゲインを変更することで、観測位置Ｍ２の重み付けを重くする。自車位置推定部１５は、一例として、初期設定において５０：５０であった予測位置Ｍ１と観測位置Ｍ２との重み付けの比を予測位置Ｍ１：観測位置Ｍ２＝４０：６０に変更する。
【００５１】
自車位置推定部１５は、自車位置の推定結果が安定していると判定される度に、観測位置Ｍ２の重み付けを重くしてもよい。自車位置推定部１５は、自車位置の推定結果が安定していると判定される度に、重み付けの比において観測位置Ｍ２が＋１０、予測位置Ｍ１が−１０となるように、観測位置Ｍ２の重み付けを重くする。重み付けの変更には上限を設けてもよい。上限は予め設定することができる。自車位置推定部１５は、予測位置Ｍ１：観測位置Ｍ２＝１００：０を上限としてもよく、予測位置Ｍ１：観測位置Ｍ２＝２０：８０を上限としてもよい。自車位置推定部１５は、変更した重み付けに応じて、予測位置Ｍ１及び観測位置Ｍ２から自車位置Ｍｚを推定する。
【００５２】
〈第１実施形態に係る自車位置推定装置の処理〉
続いて、第１実施形態に係る自車位置推定装置１００の処理について図面を参照して説明する。
【００５３】
図３（ａ）は、予測位置の演算処理の一例を示すフローチャートである。予測位置の演算処理は車両の走行中に実行される。図３（ａ）に示すように、自車位置推定装置１００のＥＣＵ１０は、Ｓ１０として、予測位置演算部１４により車速及びヨーレートの履歴の取得を行う。予測位置演算部１４は、内部センサ３に含まれる車速センサから車両の車速を検出すると共に、内部センサ３に含まれるヨーレートセンサから車両のヨーレートを検出することでそれぞれの履歴を取得する。その後、ＥＣＵ１０はＳ１２に移行する。
【００５４】
Ｓ１２において、ＥＣＵ１０は、予測位置演算部１４により予測位置Ｍ１の演算を行う。予測位置演算部１４は、車速及びヨーレートの履歴に基づいて、過去の自車位置の推定結果から車両の地図上の予測位置Ｍ１を演算する。その後、ＥＣＵ１０は今回の処理を終了する。
【００５５】
図３（ｂ）は、観測位置を用いた自車位置の推定処理の一例を示すフローチャートである。観測位置を用いた自車位置の推定処理は、車両の走行中に実行される。図３（ｂ）に示すように、ＥＣＵ１０は、Ｓ２０として、物標認識部１２により車両の周囲の物標を認識する。物標認識部１２は、外部センサ２の検出結果に基づいて、車両の周囲の物標を認識する。その後、ＥＣＵ１０はＳ２２に移行する。
【００５６】
Ｓ２２において、ＥＣＵ１０は、観測位置推定部１３により観測位置Ｍ２を推定する。観測位置推定部１３は、物標認識部１２による物標の認識結果と物標データベース５の物標情報とに基づいて、車両の地図上の観測位置Ｍ２を推定する。その後、ＥＣＵ１０はＳ２４に移行する。
【００５７】
Ｓ２４において、ＥＣＵ１０は、自車位置推定部１５により位置用カルマンフィルタによる自車位置Ｍｚの推定を行う。自車位置推定部１５は、予測位置演算部１４の演算した予測位置Ｍ１及び観測位置推定部１３の推測した観測位置Ｍ２に基づいて、自車位置Ｍｚを推定する。自車位置推定部１５は、今回の処理の前に予め設定された重み付けに応じて予測位置Ｍ１及び観測位置Ｍ２から自車位置Ｍｚを推定する。
【００５８】
なお、ＥＣＵ１０は、Ｓ２０において、車両の周囲の物標を認識できない場合には今回の処理を終了する。この場合には、ＥＣＵ１０は、予測位置Ｍ１を仮の自車位置として用いてもよい。
【００５９】
図４は、重み付け変更処理の一例を示すフローチャートである。重み付け変更処理は、例えば、自車位置の推定処理が予め決められた回数行われた場合に実行される。重み付け変更処理は、車両の走行距離に応じて実行されてもよく、定期的に行われてもよい。
【００６０】
図４に示すように、ＥＣＵ１０は、Ｓ３０として、ふらつき判定部１６により自車位置の推定結果のふらつきが生じているか否かを判定する。ふらつき判定部１６は、例えば、過去の一定回数分の自車位置の推定結果に基づいて、自車位置の推定結果における車両の車線中心からの横位置偏差の最小値と最大値の差分を演算し、差分がふらつき判定閾値以上である場合に自車位置の推定結果のふらつきが生じていると判定する。ＥＣＵ１０は、自車位置の推定結果のふらつきが生じていると判定された場合（Ｓ３０：ＹＥＳ）、Ｓ３２に移行する。ＥＣＵ１０は、自車位置の推定結果のふらつきが生じていないと判定された場合（Ｓ３０：ＮＯ）、Ｓ３４に移行する。
【００６１】
Ｓ３２において、ＥＣＵ１０は、自車位置推定部１５により自車位置推定における予測位置Ｍ１の重み付けを重くする。自車位置推定部１５は、予測位置Ｍ１に近い自車位置Ｍｚを推定するように自車位置推定における予測位置Ｍ１の重み付けを重くする。自車位置推定部１５は、例えば、位置用カルマンフィルタのゲインを変更することで、予測位置Ｍ１の重み付けを重くする。なお、自車位置推定部１５は、予測位置Ｍ１の重み付けの増加が上限に至っている場合には重み付けを行わない。その後、ＥＣＵ１０は今回の処理を終了する。
【００６２】
Ｓ３４において、ＥＣＵ１０は、ふらつき判定部１６により自車位置の推定結果が安定しているか否かを判定する。ふらつき判定部１６は、例えば、過去の一定回数分の自車位置の推定結果に基づいて、自車位置の推定結果における車両の車線中心からの横位置偏差の最小値と最大値の差分を演算し、差分が安定閾値未満である場合に自車位置の推定結果が安定していると判定する。ＥＣＵ１０は、自車位置の推定結果が安定していると判定された場合（Ｓ３４：ＹＥＳ）、Ｓ３６に移行する。ＥＣＵ１０は、自車位置の推定結果が安定していないと判定された場合（Ｓ３４：ＮＯ）、今回の処理を終了する。
【００６３】
Ｓ３６において、ＥＣＵ１０は、自車位置推定部１５により自車位置推定における観測位置Ｍ２の重み付けを重くする。自車位置推定部１５は、観測位置Ｍ２に近い自車位置Ｍｚを推定するように自車位置推定における観測位置Ｍ２の重み付けを重くする。自車位置推定部１５は、例えば、位置用カルマンフィルタのゲインを変更することで、観測位置Ｍ２の重み付けを重くする。なお、自車位置推定部１５は、観測位置Ｍ２の重み付けの増加が上限に至っている場合には重み付けの増加を行わない。その後、ＥＣＵ１０は今回の処理を終了する。
【００６４】
〈第１実施形態に係る自車位置推定装置の作用効果〉
以上説明した第１実施形態に係る自車位置推定装置１００によれば、区画線の擦れなどの物標の状態や誤検出などによる観測位置Ｍ２のズレの影響から自車位置の推定結果のふらつきが生じているときには、内部センサ３の検出結果から演算された予測位置Ｍ１に近い自車位置Ｍｚを推定するように予測位置Ｍ１の重み付けを重くするので、予測位置Ｍ１と観測位置Ｍ２とを固定した割合で自車位置推定に用いる場合と比べて、自車位置の推定結果のふらつきが続くことを抑制することができる。
【００６５】
また、自車位置推定装置１００によれば、位置用カルマンフィルタを用いた自車位置Ｍｚの推定を行うことで、位置用カルマンフィルタを用いることなく予測位置Ｍ１と観測位置Ｍ２との割合から自車位置Ｍｚを推定する場合と比べて、自車位置Ｍｚの推定精度の向上を図ることができる。また、この場合にはゲインの変更により容易に予測位置Ｍ１の重み付けを変更することができる。
【００６６】
［第２実施形態］
続いて、第２実施形態に係る自車位置推定装置について説明する。図５は、第２実施形態に係る自車位置推定装置を示すブロック図である。図５に示す自車位置推定装置２００は、第１実施形態と比べて、観測方位角及び予測方位角を用いて自車位置Ｍｚの方位角を推定する点が主に異なっている。観測方位角及び予測方位角について詳細は後述する。方位角は地図に対して決められ、例えば北を基準として右周りに正の角度で表わすことができる。なお、方位角は、車両の走行する地図上の道路に対して決められ、道路の延在方向を基準として右周りに正の角度として表わしてもよい。
【００６７】
〈第２実施形態に係る自車位置推定装置の構成〉
以下、第２実施形態に係る自車位置推定装置２００の構成について説明する。第１実施形態と同様又は相当する構成については同じ符号を付して詳細な説明を省略する。
【００６８】
図５に示すように、自車位置推定装置２００のＥＣＵ２０は、第１実施形態のＥＣＵ１０と比べて、観測位置推定部２３、予測位置演算部２４、自車位置推定部２５、及びふらつき判定部２６の機能が異なっている。
【００６９】
観測位置推定部２３は、第１実施形態における観測位置Ｍ２の推定に加えて、観測方位角の推定を行う。観測方位角とは、物標の認識結果から得られる車両の地図上の方位角である。観測方位角は、物標認識部１２の認識結果から得られる車両及び物標の相対位置と物標情報に含まれる物標の地図上の位置とに基づいて推定される。観測方位角は、一つの物標の向き（例えば道路案内標識の正面であるか裏であるか）から推定されてもよく、複数の物標の相対位置を組み合わせることで推定されてもよい。
【００７０】
予測位置演算部２４は、第１実施形態における予測位置Ｍ１の演算に加えて、予測方位角の演算を行う。予測方位角とは、いわゆるオドメトリにより予測された車両の地図上の方位角である。予測位置演算部１４は、過去の自車位置の方位角から現在の車両の地図上の方位角としての予測方位角を演算する。予測位置演算部１４は、例えばヨーレートセンサの検出したヨーレートに基づいて、過去の自車位置の方位角を基準として車両のヨーレートの履歴を用いることで予測方位角の演算を行うことができる。
【００７１】
自車位置推定部２５は、第１実施形態における自車位置Ｍｚの演算に加えて、自車位置Ｍｚの方位角の推定を行う。自車位置推定部２５は、予測方位角及び観測方位角に基づいて、自車位置Ｍｚの方位角を推定する。
【００７２】
自車位置推定部２５は、方位角用カルマンフィルタを用いて予測位置Ｍ１及び観測位置Ｍ２から自車位置Ｍｚの方位角を推定する。方位角用カルマンフィルタでは、カルマンフィルタにおける予想推定値としての予測方位角と観測値としての観測方位角を用いて、状態推定値として現在の自車位置Ｍｚの方位角を推定する。方位角用カルマンフィルタとしては、第１実施形態で述べた拡張カルマンフィルタを応用することができる。
【００７３】
方位角用カルマンフィルタでは、カルマンフィルタゲイン（以後、方位角ゲインと称する）を変更することで、自車位置推定における予測方位角の重み付けを変更することができる。自車位置推定部２５は、例えば、初期設定として予測方位角と観測方位角とが５０：５０になるように方位角ゲインを設定する。この場合、自車位置Ｍｚの方位角は、予測方位角及び観測方位角から等しい角度の方位角として推定される。方位角ゲインの変更について詳しくは後述する。なお、自車位置推定部２５は、更に車線の走行方向を考慮して方位角を推定してもよい。
【００７４】
なお、自車位置推定部２５は、必ずしも方位角用カルマンフィルタを用いて自車位置Ｍｚの方位角を推定する必要はない。自車位置推定部２５は、カルマンフィルタに代えてパーティクルフィルタを用いてもよい。自車位置推定部２５は、予測方位角及び観測方位角の間の角度の中から、予め設定された予測方位角及び観測方位角のそれぞれの重み付けに応じて自車位置Ｍｚの方位角を推定してもよい。自車位置推定部２５は、例えば自車位置の推定における予測方位角の重み付けが４０であり、観測方位角の重み付けが６０である場合、観測方位角となす角度と予測方位角となす角度の比が４０：６０になる方位角を自車位置Ｍｚの方位角として推定する。
【００７５】
ふらつき判定部２６は、自車位置推定部２５の推定した自車位置Ｍｚの方位角の変動に基づいて、自車位置の推定結果（自車位置の方位角の推定結果を含む）のふらつきが生じているか否かを判定する。
【００７６】
ふらつき判定部２６は、過去の一定回数分の自車位置Ｍｚの方位角の推定結果に基づいて、過去の一定回数分の自車位置Ｍｚの方位角から算出したヨーレートの変動（ヨーレートの最大値と最小値の差分）を演算する。ふらつき判定部２６は、自車位置Ｍｚの方位角から算出したヨーレートの変動がふらつき方位角閾値以上である場合、自車位置の推定結果のふらつきが生じていると判定する。一定回数は、特に限定されないが、例えば１０回（１０フレーム分）とすることができる。ふらつき方位角閾値は、予め設定された値の閾値である。ふらつき方位角閾値は例えば０．１ｄｅｇ／ｓとすることができる。
【００７７】
ふらつき判定部２６により自車位置の推定結果のふらつきが生じていると判定された場合、自車位置推定部２５は、予測方位角に近い自車位置Ｍｚの方位角を推定するように自車位置推定における予測方位角の重み付けを重くする。自車位置推定部２５は、例えば方位角用カルマンフィルタの方位角ゲインを変更することで、予測方位角の重み付けを重くする。自車位置推定部２５は、一例として、初期設定において５０：５０であった予測方位角と観測方位角との重み付けの比を予測方位角：観測方位角＝６０：４０に変更する。
【００７８】
自車位置推定部２５は、自車位置の推定結果のふらつきが生じていると判定される度に、予測方位角の重み付けを重くしてもよい。自車位置推定部２５は、自車位置の推定結果のふらつきが生じていると判定される度に、重み付けの比において予測方位角が＋１０、観測方位角が−１０となるように、予測方位角の重み付けを重くする。重み付けの変更には上限を設けてもよい。上限は予め設定することができる。自車位置推定部２５は、予測方位角：観測方位角＝１００：０を上限としてもよく、予測方位角：観測方位角＝８０：２０を上限としてもよい。
【００７９】
また、ふらつき判定部２６は、自車位置の推定結果が安定しているか否かを判定する。ふらつき判定部２６は、過去の一定回数分の自車位置Ｍｚの方位角から算出したヨーレートの変動が方位角安定閾値未満である場合、自車位置の推定結果が安定していると判定する。方位角安定閾値は、予め設定された値の閾値であり、ふらつき方位角閾値より小さい値の閾値である。方位角安定閾値は例えば０．０５ｄｅｇ／ｓとすることができる。
【００８０】
ふらつき判定部２６により自車位置の推定結果が安定していると判定された場合、自車位置推定部２５は、観測方位角に近い自車位置Ｍｚの方位角を推定するように自車位置推定における観測方位角の重み付けを重くする。自車位置推定部２５は、例えば方位角用カルマンフィルタの方位角ゲインを変更することで、観測方位角の重み付けを重くする。自車位置推定部２５は、一例として、初期設定において５０：５０であった予測方位角と観測方位角との重み付けの比を予測方位角：観測方位角＝４０：６０に変更する。
【００８１】
自車位置推定部２５は、自車位置の推定結果が安定していると判定される度に、観測方位角の重み付けを重くしてもよい。自車位置推定部２５は、自車位置の推定結果が安定していると判定される度に、重み付けの比において観測方位角が＋１０、予測方位角が−１０となるように、観測方位角の重み付けを重くする。重み付けの変更には上限を設けてもよい。上限は予め設定することができる。自車位置推定部２５は、予測方位角：観測方位角＝１００：０を上限としてもよく、予測方位角：観測方位角＝２０：８０を上限としてもよい。
【００８２】
自車位置推定部２５は、変更した重み付けに応じて、予測方位角及び観測方位角から自車位置Ｍｚの方位角を推定する。なお、自車位置推定部２５は、予測方位角及び観測方位角の重み付けを、予測位置Ｍ１及び観測位置Ｍ２の重み付けとして自車位置Ｍｚの推定に用いてもよい。
【００８３】
〈第２実施形態に係る自車位置推定装置の処理〉
続いて、第２実施形態に係る自車位置推定装置２００の処理について図面を参照して説明する。
【００８４】
図６（ａ）は、予測位置及び予測方位角の演算処理の一例を示すフローチャートである。予測位置及び予測方位角の演算処理は車両の走行中に実行される。図６（ａ）に示すように、自車位置推定装置２００は、Ｓ４０として、予測位置演算部２４により車速及びヨーレートの履歴を取得する。なお、Ｓ４０の処理は図３（ａ）のＳ１０の処理に対応する。その後、ＥＣＵ２０はＳ４２に移行する。
【００８５】
Ｓ４２において、ＥＣＵ２０は、予測位置演算部２４により予測位置Ｍ１及び予測方位角の演算を行う。予測位置演算部２４は、車速及びヨーレートの履歴に基づいて、過去の自車位置の推定結果から車両の地図上の予測位置Ｍ１を演算する。また、予測位置演算部２４は、ヨーレートの履歴に基づいて、過去の自車位置の方位角の推定結果から車両の地図上の予測方位角を演算する。その後、ＥＣＵ２０は今回の処理を終了する。
【００８６】
図６（ｂ）は、観測位置及び観測方位角を用いた自車位置及び自車位置の方位角の推定処理の一例を示すフローチャートである。当該推定処理は、車両の走行中に実行される。図６（ｂ）に示すように、ＥＣＵ２０は、Ｓ５０として、物標認識部１２により車両の周囲の物標を認識する。物標認識部１２は、外部センサ２の検出結果に基づいて、車両の周囲の物標を認識する。なお、Ｓ５０の処理は図３（ｂ）のＳ２０の処理に対応する。その後、ＥＣＵ２０はＳ５２に移行する。
【００８７】
Ｓ５２において、ＥＣＵ２０は、観測位置推定部２３により観測位置Ｍ２及び観測方位角を推定する。観測位置推定部２３は、物標認識部１２による物標の認識結果と物標データベース５の物標情報とに基づいて、車両の地図上の観測位置Ｍ２及び観測方位角を推定する。その後、ＥＣＵ２０はＳ５４に移行する。
【００８８】
Ｓ５４において、ＥＣＵ２０は、自車位置推定部２５により、位置用カルマンフィルタによる自車位置Ｍｚの推定、及び、方位角用カルマンフィルタによる自車位置Ｍｚの方位角の推定を行う。
【００８９】
自車位置推定部２５は、予測位置演算部２４の演算した予測位置Ｍ１及び観測位置推定部２３の推測した観測位置Ｍ２に基づいて、自車位置Ｍｚを推定する。自車位置推定部２５は、今回の処理の前に予め設定された重み付けに応じて予測位置Ｍ１及び観測位置Ｍ２から自車位置Ｍｚを推定する。同様に、自車位置推定部２５は、予測位置演算部２４の演算した予測方位角及び観測位置推定部２３の推測した観測方位角に基づいて、自車位置Ｍｚの方位角を推定する。自車位置推定部２５は、今回の処理の前に予め設定された重み付けに応じて予測方位角及び観測方位角から自車位置Ｍｚの方位角を推定する。
【００９０】
ＥＣＵ２０は、Ｓ５０において、車両の周囲の物標を認識できない場合には今回の処理を終了する。この場合には、ＥＣＵ２０は、予測位置Ｍ１を仮の自車位置Ｍｚとして用いてもよく、予測方位角を仮の自車位置Ｍｚの方位角として用いてもよい。
【００９１】
図７は、第２実施形態における重み付け変更処理の一例を示すフローチャートである。重み付け変更処理は、例えば、自車位置の推定処理が予め決められた回数行われた場合に実行される。重み付け変更処理は、車両の走行距離に応じて実行されてもよく、定期的に行われてもよい。
【００９２】
図７に示すように、ＥＣＵ２０は、Ｓ６０として、ふらつき判定部２６により自車位置の推定結果のふらつきが生じているか否かを判定する。ふらつき判定部２６は、例えば、過去の一定回数分の自車位置Ｍｚの方位角から算出したヨーレートの変動（ヨーレートの最大値と最小値の差分）を演算する。ふらつき判定部２６は、自車位置Ｍｚの方位角から算出したヨーレートの変動がふらつき方位角閾値以上である場合、自車位置の推定結果のふらつきが生じていると判定する。ＥＣＵ２０は、自車位置の推定結果のふらつきが生じていると判定された場合（Ｓ６０：ＹＥＳ）、Ｓ６２に移行する。ＥＣＵ２０は、自車位置の推定結果のふらつきが生じていないと判定された場合（Ｓ６０：ＮＯ）、Ｓ６４に移行する。
【００９３】
Ｓ６２において、ＥＣＵ２０は、自車位置推定部２５により自車位置推定における予測位置Ｍ１の重み付けを重くすると共に、予測方位角の重み付けを重くする。自車位置推定部２５は、予測方位角に近い自車位置Ｍｚの方位角を推定するように自車位置推定における予測方位角の重み付けを重くする。自車位置推定部２５は、例えば方位角用カルマンフィルタの方位角ゲインを変更することで、予測方位角の重み付けを重くする。なお、自車位置推定部２５は、上限に至っている場合には更なる重み付けの増加を行わない。その後、ＥＣＵ２０は今回の処理を終了する。
【００９４】
Ｓ６４において、ＥＣＵ２０は、ふらつき判定部２６により自車位置の推定結果が安定しているか否かを判定する。ふらつき判定部２６は、例えば、過去の一定回数分の自車位置Ｍｚの方位角から算出したヨーレートの変動が方位角安定閾値未満である場合、自車位置Ｍｚの推定結果が安定していると判定する。ＥＣＵ２０は、自車位置の推定結果が安定していると判定された場合（Ｓ６４：ＹＥＳ）、Ｓ６６に移行する。ＥＣＵ２０は、自車位置の推定結果が安定していないと判定された場合（Ｓ６４：ＮＯ）、今回の処理を終了する。
【００９５】
Ｓ６６において、ＥＣＵ２０は、自車位置推定部２５により自車位置推定における観測位置Ｍ２の重み付けを重くすると共に観測方位角の重み付けを重くする。自車位置推定部２５は、観測方位角に近い自車位置Ｍｚの方位角を推定するように自車位置推定における観測方位角の重み付けを重くする。自車位置推定部２５は、例えば方位角用カルマンフィルタの方位角ゲインを変更することで、観測方位角の重み付けを重くする。なお、自車位置推定部２５は、上限に至っている場合には更なる重み付けを行わない。その後、ＥＣＵ２０は今回の処理を終了する。
【００９６】
〈第２実施形態に係る自車位置推定装置の作用効果〉
以上説明した第１実施形態に係る自車位置推定装置２００によれば、区画線の擦れなどの物標の状態や誤検出などによる観測位置Ｍ２及び観測方位角のズレの影響から自車位置の推定結果のふらつきが生じているときには、内部センサ３の検出結果から演算された予測位置Ｍ１に近い自車位置Ｍｚを推定するように予測位置Ｍ１の重み付けを重くするので、予測位置Ｍ１と観測位置Ｍ２とを固定した割合で自車位置推定に用いる場合と比べて、自車位置の推定結果のふらつきが続くことを抑制することができる。また、自車位置推定装置２００によれば、自車位置の推定結果のふらつきが生じているときには、予測方位角に近い方位角を推定するように予測方位角の重み付けを重くするので、観測方位角と予測方位角とを固定した割合で自車位置Ｍｚの方位角の推定に用いる場合と比べて、自車位置Ｍｚの方位角のふらつきが続くことを抑制することができる。
【００９７】
また、自車位置推定装置２００によれば、位置用カルマンフィルタを用いた自車位置Ｍｚの推定を行うことで、第１実施形態と同様の効果を得ることができる。更に、自車位置推定装置２００によれば、方位角用カルマンフィルタを用いて自車位置Ｍｚの方位角を推定するので、方位角用カルマンフィルタを用いることなく観測方位角と予測方位角との割合から自車位置の方位角を推定する場合と比べて、自車位置Ｍｚの方位角の推定精度の向上を図ることができる。また、この場合には方位角ゲインの変更により容易に予測方位角の重み付けを変更することができる。
【００９８】
以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限定されるものではない。本発明は、上述した実施形態を始めとして、当業者の知識に基づいて種々の変更、改良を施した様々な形態で実施することができる。
【００９９】
自車位置推定装置１００，２００のＥＣＵ１０、２０は、外部センサ２の検出結果の確からしさを演算してもよい。外部センサ２の検出結果の確からしさとは、例えばカメラの画像認識の確からしさである。カメラの認識の確からしさは、カメラの故障判定や画像認識の確からしさ（信頼度など）を演算する周知の手法により求めることができる。外部センサ２の検出結果の確からしさとして、レーダセンサの検出結果の確からしさを用いてもよい。レーダセンサの検出結果の確からしさについても、レーダセンサの故障判定などの周知の手法により求めることができる。また、外部センサ２の検出結果の確からしさは、カメラの撮像画像から認識された物標とレーダセンサの物体情報から認識された物標の整合性を用いて求めてもよい。外部センサ２の検出結果の確からしさは、整合性が低いほど小さい値となる。
【０１００】
ＥＣＵ１０、２０は、外部センサ２の検出結果の確からしさが判定閾値未満である場合、観測位置Ｍ２の重み付けを軽くして、予測位置Ｍ１の重み付けを重くする。判定閾値は予め設定された値の閾値である。ＥＣＵ１０、２０は、位置用カルマンフィルタを用いる場合、ゲインを変更することで重み付けを調整する。自車位置推定装置１００，２００は、外部センサ２の検出結果の確からしさが判定閾値未満である場合、観測方位角の重み付けを軽くして、予測方位角の重み付けを重くしてもよい。ＥＣＵ１０、２０は、方位角用カルマンフィルタを用いる場合、方位角ゲインを変更することで重み付けを調整する。
【０１０１】
第１実施形態に係る自車位置推定装置１００、自車位置Ｍｚの方位角を推定してもよい。この場合、第２実施形態のように観測方位角及び予測方位角の両方を求める必要はなく、観測方位角又は予測方位角の一方をそのまま自車位置Ｍｚの方位角とみなしてもよい。或いは、過去の自車位置から現在の自車位置Ｍｚに向かう方向を自車位置Ｍｚの方位角としてもよい。
【０１０２】
第２実施形態に係る自車位置推定装置２００において、ふらつき判定部２６は、第１実施形態と同様に、自車位置の変動（横位置偏差などの変動）に基づいて、自車位置の推定結果にふらつきが生じているか否かを判定してもよい。
【０１０３】
自車位置推定部１５，２５は、自車位置の推定結果のふらつきが生じていると判定された場合に、予測位置Ｍ１及び／又は予測方位角の重み付けの増加を一回だけ行う態様であってもよい。同様に、自車位置推定部１５，２５は、自車位置の推定結果が安定していると判定された場合に、観測位置Ｍ２及び／又は観測方位角の重み付けの増加を一回だけ行う態様であってもよい。自車位置推定部１５，２５は、定期的又は一定の走行距離を車両が走行する度に、予測位置Ｍ１と観測位置Ｍ２の重み付けを初期設定に戻してもよい。予測方位角と観測方位角の重み付けについても同様である。
【０１０４】
ふらつき判定部１６は、ふらつき判定閾値を複数用いて、ふらつきの程度を段階的に判定してもよい。この場合、ふらつき判定部１６は、自車位置の推定結果のふらつきの程度が大きいほど（例えば過去の一定回数分の自車位置の推定結果における車両の車線中心からの横位置偏差の最小値と最大値の差分が大きいほど）、予測位置Ｍ１の重み付けを大きく増加させる。自車位置の推定結果のふらつきの程度は、例えば過去の一定回数分の自車位置の推定結果における車両の車線中心からの横位置偏差の最小値と最大値の差分と複数のふらつき判定閾値との比較により判定することができる。
【０１０５】
同様に、ふらつき判定部１６は、安定閾値を複数用いて、自車位置の推定結果の安定の程度を段階的に判定してもよい。この場合、ふらつき判定部１６は、自車位置の推定結果の安定の程度が大きいほど（例えば過去の一定回数分の自車位置の推定結果における車両の車線中心からの横位置偏差の最小値と最大値の差分が小さいほど）、観測位置Ｍ２の重み付けを大きく増加させる。これらの点は、第２実施形態における予測方位角及び観測方位角の重み付けについても適用することができる。
【０１０６】
第２実施形態の自車位置推定部２５は、予測方位角及び観測方位角の重み付けと、予測位置Ｍ１及び観測位置Ｍ２との重み付けとを独立して変更してもよい。自車位置推定部２５は、第１実施形態で述べたように、過去の一定回数分の自車位置の変動（横位置偏差など）から自車位置の推定結果のふらつきが生じていると判定された場合には、予測位置Ｍ１及び観測位置Ｍ２の重み付けのみを変更する。また、自車位置推定部２５は、第２実施形態で述べたように、過去の一定回数分の自車位置の方位角から自車位置の推定結果のふらつきが生じていると判定された場合には、予測方位角及び観測方位角の重み付けのみを変更してもよい。
【０１０７】
また、ふらつき判定部１６，２６は、必ずしも自車位置の推定結果が安定しているか否かの判定を行う必要はない。この場合には、自車位置の推定結果による観測位置Ｍ２又は観測方位角の重み付けの増加の処理も必須ではない。
【符号の説明】
【０１０８】
１…ＧＰＳ受信部、２…外部センサ、３…内部センサ、４…地図データベース、５…物標データベース、１０，２０…ＥＣＵ、１１…測定位置取得部、１２…物標認識部、１３，２３…観測位置推定部、１４，２４…予測位置演算部、１５，２５…自車位置推定部、１６，２６…ふらつき判定部、１００，２００…自車位置推定装置。
【図１】
2020034441.tif 000003
【図２】
2020034441.tif 000004
【図３】
2020034441.tif 000005
【図４】
2020034441.tif 000006
【図５】
2020034441.tif 000007
【図６】
2020034441.tif 000008
【図７】
2020034441.tif 000009