特許 6645936 状態推定装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6645936

(24)【登録日】2020年1月14日

(45)【発行日】2020年2月14日

(54)【発明の名称】状態推定装置

(51)【国際特許分類】

   G01C 21/28 20060101AFI20200203BHJP

   G08G 1/0969 20060101ALI20200203BHJP

【ＦＩ】

   G01C21/28

   G08G1/0969

【請求項の数】11

【全頁数】14

(21)【出願番号】特願2016-172034(P2016-172034)

(22)【出願日】2016年9月2日

(65)【公開番号】特開2018-36225(P2018-36225A)

(43)【公開日】2018年3月8日

【審査請求日】2019年1月22日

(73)【特許権者】

【識別番号】000004695

【氏名又は名称】株式会社ＳＯＫＥＮ

(73)【特許権者】

【識別番号】000004260

【氏名又は名称】株式会社デンソー

(74)【代理人】

【識別番号】110000578

【氏名又は名称】名古屋国際特許業務法人

(72)【発明者】

【氏名】立石 宏次朗

(72)【発明者】

【氏名】川嵜 直輝

(72)【発明者】

【氏名】鈴木 俊輔

(72)【発明者】

【氏名】水野 広

(72)【発明者】

【氏名】小林 健二

(72)【発明者】

【氏名】田中 雄介

(72)【発明者】

【氏名】小西 將貴

【審査官】 田中 将一

(56)【参考文献】

【文献】 特開２００４−０４５２２７（ＪＰ，Ａ）

【文献】 特開２０１６−０５０８４０（ＪＰ，Ａ）

【文献】 特開２０１６−１２６６０５（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｃ ２１／００ − ２１／３６

Ｇ０１Ｃ ２３／００ − ２５／００

Ｇ０８Ｇ １／００ − ９９／００

(57)【特許請求の範囲】

【請求項1】

車両に搭載され当該車両の位置及び進行方向を含む車両状態を、あらかじめ設定された周期を有する観測サイクルごとに繰り返し推定する状態推定装置（１）であって、
前回の観測サイクルで推定された前記車両状態である推定状態を用いて予測した、今回の観測サイクルでの前記車両状態である予測状態を生成するように構成された状態予測部（Ｓ１１０）と、
前記予測状態に含まれる前記車両の位置である予測位置に従って、前記予測位置の周辺に存在する地物を表す地物データ及び道路形状を表す形状データを含む地図データを取得するように構成された地図取得部（Ｓ１２０）と、
前記観測サイクルごとに、光学センサ（５）の検出結果から、前記地物データに少なくとも設置位置が登録されている地物である位置地物及び道路形状を規定する地物である形状地物を検出するように構成された地物検出部（Ｓ１４０、Ｓ２００）と、
前記位置地物と前記車両との相対的な位置関係を検出するように構成された関係検出部（Ｓ１５０）と、
前記車両が前記予測位置に存在するものとして、前記関係検出部により検出された前記位置地物と前記車両との相対的な位置関係によって、同一物であると推定される前記位置地物と前記地物データとの対応付けを行うように構成された対応付け部（Ｓ１６０、Ｓ２３０）と、
前記車両に対する前記位置地物の相対位置と、前記予測位置に対する前記対応付け部により当該位置地物に対応づけられた前記地物データの表す位置の相対位置との差を補正量として算出するように構成された補正算出部（Ｓ２１０）と、
前記車両の進行方向に沿った位置を縦位置として、前記車両に対する前記位置地物の相対位置と、前記予測位置に対する前記対応付け部により当該位置地物に対応づけられた前記地物データの表す位置の相対位置との差を補正量として、当該補正量を用いて前記予測位置を補正することにより、今回の測定サイクルでの前記車両の少なくとも縦位置を推定するように構成された第１推定部（Ｓ１７０）と、
前記補正算出部により算出された補正量のうち、縦位置の補正量である縦補正量があらかじめ設定された補正量閾値以下である場合には、前記予測位置を比較位置とし、前記縦補正量が前記補正量閾値よりも大きい場合には、前記予測位置の縦位置を前記縦補正量で補正した地点を前記比較位置として、前記地図取得部にて取得された前記比較位置での前記形状データが表す道路形状と、前記地物検出部にて検出された形状地物が示す道路形状との位置合わせによって、前記車両の車幅方向に沿った位置である横位置及び前記車両の進行方向である車両方位の少なくとも一方の推定を行うように構成された第２推定部（Ｓ２４０、Ｓ２５０）と、
を備え、前記第１推定部及び前記第２推定部での推定結果を、今回の観測サイクルでの前記推定状態とする、状態推定装置。

【請求項2】

請求項１に記載の状態推定装置であって、
前記地物検出部は、前記位置地物として標識を検出するように構成された標識検出部（Ｓ１４０、Ｓ２００）を更に備え、
前記関係検出部は、前記車両の車幅方向に沿った位置を横位置として、前記標識検出部により検出された前記標識の横位置の中心である標識中心位置を求める中心検出部（Ｓ１８０）を更に備え、当該中心検出部により検出された標識中心位置を、前記位置地物の横位置とする
状態推定装置。

【請求項3】

請求項２に記載の状態推定装置であって、
前記地物データには前記標識の横幅のデータが含まれ、
前記中心検出部は、前記標識検出部により検出された前記標識の横幅と、前記地物データに含まれる前記標識の横幅のデータとの差があらかじめ決められた横幅の閾値である横幅閾値以下であると判定された場合、前記位置地物の中心位置を前記標識中心位置として検出するように構成された、状態推定装置。

【請求項4】

請求項２に記載の状態推定装置であって、
前記地物データは、前記標識の輪郭形状を表す画像が含まれ、
前記中心検出部は、前記標識検出部により検出された前記標識と前記画像との比較により、前記位置地物が欠損していると判定した場合には、当該画像に基づいて当該欠損している部分の補完を行い、前記標識中心位置を確定するように構成された、状態推定装置。

【請求項5】

請求項２に記載の状態推定装置であって、
前記中心検出部は、前記標識検出部により前記標識の支柱が認識された場合に、当該支柱を標識中心位置とするように構成された、状態推定装置。

【請求項6】

請求項２から請求項５までに記載の状態推定装置であって、
前記地物検出部は、前記形状地物として道路の境界及び車線の境界を示す境界表示を検出するように構成された境界検出部を更に備え、前記第２推定部は、前記標識検出部により前記標識が検出されなかった場合において、前記境界検出部により道路の境界が検出できる場合、前記車両の横位置の推定及び前記車両の進行方向の推定のうち、少なくとも一方の推定を禁止するように構成された、状態推定装置。

【請求項7】

請求項１から請求項６までのいずれか１項に記載の状態推定装置であって、
前記光学センサがステレオカメラである場合に、
前記関係検出部は、前記ステレオカメラを用いた三角測量により距離を測定するように構成された、状態推定装置。

【請求項8】

請求項１から請求項６までに記載の状態推定装置であって、
前記光学センサはレーザレーダである場合に、前記関係検出部は、レーザを出射してから当該レーザを受光するまでの時間により前記車両と前記標識との距離を測定するように構成された、状態推定装置。

【請求項9】

請求項１から請求項６までのいずれか１項に記載の状態推定装置であって、
前記地図データは、さらに地物のサイズを表す地物サイズデータを備え、
前記関係検出部は、前記地図取得部により取得された前記地物サイズデータと前記標識検出部により検出された前記標識のサイズとの比較により前記車両と前記標識との距離を測定するように構成された、状態推定装置。

【請求項10】

請求項１から請求項６までのいずれか１項に記載の状態推定装置であって、
前記地図データは、さらに地物の設置されている高さを表す地物高さデータを備え、
前記関係検出部は、前記地物高さデータと前記光学センサにより測定された前記地物の高さとの比較により前記車両と前記地物との距離を測定するように構成された、状態推定装置。

【請求項11】

請求項１から請求項７までのいずれか１項に記載の状態推定装置であって、
前記関係検出部は、互いに異なる方法で前記地物との距離を検出するように構成された複数の関係検出部と、
前記車両の周辺状況に応じて使用する前記関係検出部を選択するように構成された選択部（Ｓ１５０）と、を更に備える状態推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、車両に搭載され、当該車両の位置を推定する状態推定装置に関する。

【背景技術】

【0002】

車両の運転者が行う運転操作の少なくとも一部を車両が支援する運転支援や自動運転において、車両の走行している走路を認識する走路認識に関する技術が知られている。
例えば、特許文献１には、以下のような技術が開示されている。すなわち、あらかじめ設定された観測サイクルごとに車載カメラによって検出される標識や白線などの地物を検出地物として検出する。前回の観測サイクルで推定された、推定位置や速度ベクトルなどの自車両の状態から今回の観測サイクルでの自車両の位置を予測し、予測位置とする。自車両の予測位置に基づき、地図データベースに登録され、検出地物と同一であると推定される地物である地物データを検出地物に対応づける。自車両の予測位置に対する、対応づけられた検出地物及び地物データの両地物の相対的な位置関係が互いに一致するように予測位置を補正することにより、今回の観測サイクルでの自車両の位置及び方位を推定する。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２００８−２４９６３９号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかしながら、特許文献１に記載の装置では、自車両の実際の位置、特に自車両の進行方向に沿った縦位置と予測位置とが大きく異なっている場合、白線等から特定される道路形状により、関係検出部により検出された位置地物と車両との相対的な位置関係と当該位置地物に対応づけられた地物データの表す位置との対応関係に誤りが生じる。これにより、誤った対応関係に基づいて、自車両の状態が推定されると、誤差が生じやすくなるという問題があった。

【0005】

本開示は、縦位置を補正した場合でも車両の横位置及び車両方位がずれることを抑制する技術を提供する。

【課題を解決するための手段】

【0006】

本開示の状態推定装置（１）は、車両に搭載され当該車両の位置及び進行方向を含む車両状態を、あらかじめ設定された周期を有する観測サイクルごとに繰り返し推定し、状態予測部（Ｓ１１０）、地図取得部（Ｓ１２０）、地物検出部（Ｓ１４０、Ｓ２００）、関係検出部（Ｓ１５０）、対応付け部（Ｓ１６０、Ｓ２３０）、補正算出部（Ｓ２１０）、第１推定部（Ｓ１７０）及び第２推定部（Ｓ２４０、Ｓ２５０）を備える。

【0007】

状態予測部は、前回の観測サイクルで推定された車両状態である推定状態を用いて予測した、今回の観測サイクルでの車両状態である予測状態を生成する。地図取得部は、予測状態に含まれる車両の位置である予測位置に従って、予測位置の周辺に存在する地物を表す地物データ及び道路形状を表す形状データを含む地図データを取得する。地物検出部は、観測サイクルごとに、光学センサ（５）の検出結果から、地物データに少なくとも設置位置が登録されている地物である位置地物及び道路形状を規定する地物である形状地物を検出する。関係検出部は、位置地物と車両との相対的な位置関係を検出する。対応付け部は、車両が予測位置に存在するものとして、関係検出部により検出された位置地物と車両との相対的な位置関係によって、同一物であると推定される位置地物と地物データとの対応付けを行う。補正算出部は、車両に対する位置地物の相対位置と、予測位置に対する対応付け部により当該位置地物に対応づけられた地物データの表す位置の相対位置との差を補正量として算出する。第１推定部は、車両の進行方向に沿った位置を縦位置として、車両に対する位置地物の相対位置と、予測位置に対する対応付け部により当該位置地物に対応づけられた地物データの表す位置の相対位置との差を補正量として、当該補正量を用いて予測位置を補正することにより、今回の測定サイクルでの車両の少なくとも縦位置を推定する。第２推定部は、補正算出部により算出された補正量のうち、縦位置の補正量である縦補正量があらかじめ設定された補正量閾値以下である場合には、予測位置を比較位置とし、縦補正量が補正量閾値よりも大きい場合には、予測位置の縦位置を縦補正量で補正した地点を比較位置として、地図取得部にて取得された比較位置での形状データが表す道路形状と、地物検出部にて検出された形状地物が示す道路形状との位置合わせによって、車両の車幅方向に沿った位置である横位置及び車両の進行方向である車両方位の少なくとも一方の推定を行う。状態推定装置は、第１推定部及び第２推定部での推定結果を、今回の観測サイクルでの推定状態とする。

【0008】

このような構成においては、縦補正量が補正量閾値よりも大きい場合には、補正された車両の縦位置における地図データの示す地物の位置及び光学センサにより取得された地物の位置の比較に基づいて車両の横位置及び車両方位を補正する。これにより、縦位置を補正した場合でも車両の横位置及び車両方位がずれることを抑制することができる。

【0009】

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

【図面の簡単な説明】

【0010】

【図1】状態推定装置及び周辺機器の構成を示すブロック図である。

【図2】カメラ座標系を示す図である。

【図3】画像座標系を示す図である。

【図4】ステレオカメラにより撮像された画像のイメージを表す図である。

【図5】状態推定処理を示すフローチャートである。

【図6】光学センサにより道路境界が検出できない状態を表した図である。

【図7】中心位置検出処理を示すフローチャートである。

【図8】光学センサによる地物の検出において画角から標識が外れ、補完が必要となる状態を表す図である。

【図9】予測位置における対応付けの方法の概要を表す図である。

【図10】比較位置における再対応付けの方法の概要を表す図である。

【図11】変形例における光学センサにより撮像された画像の例を表す図である。

【発明を実施するための形態】

【0011】

以下、図面を参照しながら、本開示の実施形態を説明する。
［１．第１実施形態］
［１−１．構成］
本実施形態の運転支援システム２は車両に搭載され、図１に示すように状態推定装置１を備える。運転支援システム２は、更に、ＧＮＳＳ受信機３、地図メモリ４、光学センサ５、状態量センサ６及び制御装置７を備えてもよい。以下、状態推定装置１を搭載する車両を自車とする。

【0012】

ＧＮＳＳ受信機３は、ＧＮＳＳ用の人工衛星からの送信電波を、アンテナを介して受信する装置である。ＧＮＳＳ受信機３は、自車の絶対位置を測定し、測定した絶対位置を位置データとして状態推定装置１に出力する。

【0013】

地図メモリ４は、通常のナビゲーションに使用するための自車の走行可能な道路の地図データを記憶している。当該地図データには地物を表す地物データ及び道路形状を表す形状データが含まれる。ここでいう地物データは、車両の走行可能な道路に沿って設置されている道路標識を表す標識データを含む。標識データは、当該道路標識の地図上の位置、標識の種類、自車が走行している道路に対する標識の高さ、標識の大きさ及び標識に表示されている文字、標識を支える支柱のデータを含む。形状データは、自車の走行中の道路の境界又は車線の境界を示す境界表示の位置のデータである境界データを含む。

【0014】

光学センサ５は、自車の前方の地物を含む画像を取得する。ここでいう地物には、自車の前方の道路標識などの位置地物と、道路の境界及び車線の境界などの形状地物とが含まれる。光学センサ５は、自車の前方の地物を表す画像を状態推定装置１に出力する。本実施形態においては、光学センサ５は、水平な同一線上に備えられ、光軸が平行となるように配置された一対のカメラであるステレオカメラである。なお、当該ステレオカメラを構成する一対のカメラの焦点距離は同一であるとする。以下、図２に示すように自車のステレオカメラの設置された位置を基準とした実空間の座標系をカメラ座標系とし、そのカメラ座標系における座標を（Ｘｃ，Ｙｃ，Ｚｃ）で表す。ここで、Ｘｃはステレオカメラが撮像する水平方向の座標を表し、Ｙｃはステレオカメラが撮像する鉛直方向の座標を表し、Ｚｃはステレオカメラが撮像する光軸方向の座標を表す。ここで、Ｘｃは、ステレオカメラの光軸方向を向いた際に左方向に行くほど座標の値は増加し、Ｙｃは、上方向に行くほど座標の値は増加する。これに対して、撮像された画像における座標系を図３に示すように画像座標系とし、その画像座標系における座標を（ｕ，ｖ）で表す。また、画像中心を（ｃｕ，ｃｖ）とする。ここで、ｕは撮像される画像の水平方向における座標を表し、撮像された画像の右方向に行くほど座標の値は増加する。ｖは撮像される画像の鉛直方向における座標を表し、撮像される画像の下方向に行くほど座標の値は増加する。また、ステレオカメラにより図４に示すような画像を取得することができる。

【0015】

状態量センサ６は、自車の車速及びヨーレートなどの自車の状態を検出する。状態量センサ６は、検出した自車の状態を表す自車状態量をＥＣＵに出力する。ここでいう自車状態量とは、自車の速度及び加速度を含み、状態量センサ６は、具体的には、速度センサ及び加速度センサである。

【0016】

制御装置７は、状態推定装置１からの入力に応じて自車のアクセル、ブレーキ及びステアリングの制御を行うことにより自動運転や運転支援などの機能を実現する装置である。
状態推定装置１は入力される地図データ、画像及び自車状態量により、自車の位置を推定し、推定された自車の位置及び当該自車の位置における道路の形状を制御装置７に出力する。

【0017】

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

【0018】

状態推定装置１の各種機能を実現する手法はソフトウェアに限るものではなく、その一部又は全部の要素について、一つ又は複数のハードウェアを用いて実現してもよい。例えば、上記機能がハードウェアである電子回路によって実現される場合、その電子回路は多数の論理回路を含むデジタル回路、又はアナログ回路、あるいはこれらの組合せによって実現してもよい。

【0019】

［１−２．処理］
次に、状態推定装置１が一定周期で実行する状態推定処理について、図５のフローチャートを用いて説明する。なお、状態推定処理を開始する前に、状態推定装置１は自車の位置及び車両方位の初期化を行う。すなわちＧＮＳＳ受信機３から取得する位置情報を自車の位置として設定する。

【0020】

Ｓ１１０で状態推定装置１は、前回の推定結果及び状態量センサ６から取得した自車の速度及び加速度に基づいて今回の処理サイクルでの自車の位置及び車両方位を予測し、当該予測された状態を予測状態として生成する。この場合において、前回の推定結果とは、前回の処理サイクルにおける後述するＳ１７０で補正された縦位置のデータ、Ｓ２４０で補正された横位置のデータ及びＳ２５０で補正された車両方位のデータのことである。また、最初の処理サイクルの場合には初期化された値に基づいて今回の処理サイクルにおける自車の位置及び車両方位が予測される。なお、自車の位置及び車両方位の予測方法は公知の技術であり、詳細な説明は省略する。

【0021】

Ｓ１２０で状態推定装置１は、Ｓ１１０で生成された予測状態に含まれる車両の位置である予測位置に従って、地図メモリ４から地図データを取得する。
Ｓ１３０で状態推定装置１は、光学センサ５により撮像された画像を光学センサ５から取得する。

【0022】

Ｓ１４０で状態推定装置１は、Ｓ１３０で取得した画像に含まれている地物を検出する。すなわち、光学センサ５により撮像された画像に対して、あらかじめメモリに記憶されている地物データの表す地物の形状パターンのパターンマッチングを行い、当該パターンマッチングにより検出した地物を検出地物とする。なお、ここでいうパターンマッチングは公知の技術であり、説明を省略する。

【0023】

Ｓ１５０で状態推定装置１は、検出地物の自車に対する縦位置を検出する。具体的には、検出地物の自車に対する縦位置は、光学センサ５であるステレオカメラによって同時に撮像された視差のある画像同士を比較することによって算出される。ここで、検出地物の自車に対する縦位置は、検出地物のステレオカメラにおける光軸方向の座標Ｚｃで表される。座標Ｚｃは、同時に撮像された視差のある画像それぞれに共通して写っている検出地物の横方向の位置の差である視差をｄ、一対のカメラ同士の距離である基線距離をｂ、一対のカメラそれぞれの焦点距離をｆｕとして（１）式で表される。当該式において、一対のカメラ同士の距離ｂ及び焦点距離ｆｕは設計時に決まる既知の値であるので、視差ｄから、検出地物と自車との距離がＺｃで表される。

【0024】

【数1】

Ｓ１６０で状態推定装置１は、地物の対応付けを行う。具体的には、車両が予測位置に存在するものとして、検出地物に含まれる検出標識及び検出境界と地物データに含まれる標識データ及び形状データに含まれる境界データとの位置関係の比較を行う。比較により導出される車両との相対的な位置関係によって、検出標識及び検出境界それぞれが同一物に該当すると推定される標識データ及び境界データの示す標識及び境界を特定することで対応付けを行う。

【0025】

Ｓ１７０で状態推定装置１は、縦位置の補正を行う。具体的には、自車の進行方向に沿った位置を縦位置として、Ｓ１２０で取得した地図データから特定される予測位置に対する検出標識の縦位置とＳ１５０で状態推定装置１により測定された検出標識の縦位置との差分を縦補正量として算出する。状態推定装置１は、当該縦補正量だけ予測位置の表す縦位置を補正し、比較位置として設定する。

【0026】

Ｓ１８０で状態推定装置１は、中心位置検出処理を行うことにより、検出標識の中心位置である標識中心位置を検出する。中心位置検出処理の詳細は後述する。
Ｓ１９０で状態推定装置１は、Ｓ１８０における中心位置検出処理の結果、検出不適状態であるか否かを判定する。検出不適状態であると判定された場合、Ｓ２００で状態推定装置は道路境界の推定に用いることができる形状地物がＳ１４０で検出されているか否かを判定する。Ｓ２００で、道路境界の推定に用いることができる形状地物が検出されていると判定された場合には、処理を終了する。すなわち、横位置及び車両方位の補正を行わず、処理を終了する。Ｓ２００で、道路境界の推定に用いることができる形状地物が検出されていないと判定された場合には、Ｓ２１０以降の処理を行う。ここでいう、検出地物として、道路境界の推定に用いることができる形状地物が検出されない状態の例を図６に示す。すなわち、当該検出されない状態の例としては、光学センサの検出範囲は２本の実線Ｒ１の内側であるとすると、点線で示したＲ２のように、走行している車両などにより検出範囲が遮られる場合がある。このような場合が、形状地物である道路境界Ｂが検出されない場合の例として挙げられる。

【0027】

Ｓ２１０で、Ｓ１７０で算出した縦補正量があらかじめ決められた閾値以上であるか否かを判定する。
Ｓ２１０で状態推定装置１がＳ１７０で算出された縦補正量があらかじめ決められた閾値より大きいと判定した場合には、Ｓ２２０で状態推定装置１は、地図データの再取得を行う。すなわち、状態推定装置１は、Ｓ１７０で設定された比較位置での、標識データ及び境界データを含む地図データを取得する。

【0028】

Ｓ２３０で、状態推定装置１は、地物の再対応付けを行う。具体的には、Ｓ１６０と同様の処理であるが、車両がＳ１７０で設定された比較位置に存在するものとして、位置関係の比較を行う点で相違する。

【0029】

Ｓ２１０で状態推定装置１がＳ１７０で補正した縦補正量があらかじめ決められた閾値よりも小さいと判定された場合には、状態推定装置１は、Ｓ２２０及びＳ２３０の処理を省略し、Ｓ２４０以降の処理を行う。

【0030】

Ｓ２４０で状態推定装置１は、横位置の補正を行う。具体的には以下のように行う。検出標識に対応する標識データが表す検出標識の比較位置又は予測位置に対する横位置を予測横位置とし、Ｓ１３０で取得した画像に基づいて求められる検出標識の自車に対する横位置を検出横位置とする。予測横位置と検出横位置とを比較することにより行われる。ここでいう、検出標識に対応する標識データとは、Ｓ１６０で対応付けがなされた検出標識又はＳ２３０で再対応付けがなされた検出標識に対応する標識データのことである。画像に基づいた横位置は以下のように求められる。すなわち、検出標識の自車に対する横位置Ｘｃは、検出標識のステレオカメラにおける光軸方向の座標Ｚｃ、検出標識の画像上の座標ｕ、画像中心の横方向の座標ｃｕ及びステレオカメラの焦点距離ｆｕから（２）式を用いて求められる。

【0031】

【数2】

予測横位置と検出横位置との差分を車両の横補正量とする。状態推定装置１は、当該横補正量だけ比較位置又は予測位置の横位置を車両の横方向にシフトさせることにより、横位置を補正する。

【0032】

Ｓ２５０で状態推定装置１は車両方位の補正を行う。すなわち、車両方位の補正は以下のように行われる。比較位置又は予測位置において、光学センサが撮像する自車の周辺の境界表示の仮想的な画像を境界データに基づいて推定する。当該仮想的な画像における道路の境界と実際の光学センサにより撮像される画像における道路の境界との比較を行い、それぞれの道路の境界が一致するように、車両方位を補正する。車両方位の補正方法については、公知の技術であり、説明を省略する。

【0033】

なお、Ｓ１１０が状態予測部、Ｓ１２０が地図取得部、Ｓ１４０及びＳ２００が地物検出部、Ｓ１５０が関係検出部、Ｓ１６０及びＳ２３０が対応付け部、Ｓ１７０が第１補正部、Ｓ１８０が中心検出部、Ｓ２１０が補正算出部、Ｓ２４０及びＳ２５０が第２補正部による処理に相当する。また、地物検出部のうち、標識を検出するものが標識検出部、境界を検出するものが境界検出部に相当する。
＜中心位置検出処理＞
状態推定装置１が行う中心位置検出処理を図７に示す。

【0034】

Ｓ３１０で状態推定装置１は、Ｓ１３０で取得した画像を取得する。
Ｓ３２０で状態推定装置１は、Ｓ１４０で取得した検出地物の取得を行う。
Ｓ３３０で状態推定装置１は、検出標識の左右両側に欠損があるか否かを判定する。なお、ここでいう欠損とは、図８に示すような画像の一部が光学センサにより正しく認識されなかったものをいう。ここでは、光学センサによる地物の検出において画角から標識が外れ、補完が必要となる状態を例として挙げる。欠損はこれに限定されるものではなく、汚れや検出する環境などの要因により光学センサにより地物の検出が適切にされなかった場合をいう。欠損の方法は、具体的には、状態推定装置１は検出標識の横幅と、標識データの示す横幅とを比較することにより行われる。比較の結果、それぞれの横幅の差分があらかじめ決められた閾値である横幅閾値以下である場合には、状態推定装置１は検出標識の横側に欠損が無いと判定する。

【0035】

Ｓ３３０で状態推定装置１により欠損が無いと判定した場合、Ｓ３４０で、検出標識の横幅の中点を標識中心位置として確定し、処理を終了する。
Ｓ３３０で状態推定装置１は、欠損があると判定した場合、Ｓ３５０で当該欠損が検出標識の片側のみに存在するか否かを判定する。具体的には、状態推定装置１は検出された検出標識の画像と標識データの示す検出標識の画像との比較を行う。

【0036】

Ｓ３５０で状態推定装置１が当該画像の比較により検出標識の片側のみに欠損が存在すると判定した場合には、状態推定装置１は標識データの示す標識の輪郭形状を表す画像から、Ｓ３６０で欠損部分の補完を行う。すなわち、標識データには欠損の無い状態の検出標識が含まれているので、状態推定装置１は欠損がある標識に標識データを重ね合わせることで欠損を補完し、Ｓ３７０で状態推定装置１は、補完された検出標識の横幅の中点を標識中心位置として確定する。

【0037】

Ｓ３５０で状態推定装置１が当該画像の比較により検出標識の両側に欠損が存在すると判定した場合には、Ｓ３８０で、状態推定装置１は、検出標識の支柱が検出されるか否かを判定する。ここでいう支柱とは、検出標識を支える柱のことであり、検出標識の横方向の中心に位置する。すなわち、状態推定装置１は、検出標識の支柱と、標識データの示す支柱との比較を行う。当該比較により標識データの示す支柱が検出された場合、すなわちＳ３８０で状態推定装置１により検出標識の支柱が検出された場合には、Ｓ３９０で状態推定装置１は、支柱の位置を標識中心位置として確定し、処理を終了する。

【0038】

Ｓ３８０で状態推定装置１により検出標識の支柱が検出されたと判定されなかった場合には、Ｓ４００で検出不適状態であると判定し、Ｓ４１０で欠損の有無にかかわらず、検出標識の横幅の中点を標識中心位置として確定し処理を終了する。

【0039】

［１−３．効果］
以上詳述した第１実施形態によれば、以下の効果を奏する。
（１ａ）本実施形態の状態推定装置によれば、縦位置の補正量である縦補正量が大きい場合、補正された自車の縦位置に基づいて自車の横位置及び車両方位を補正することにより、縦位置を補正した場合でも自車の横位置及び車両方位がずれることを抑制することができる。

【0040】

すなわち、図９に示すように、補正前の自車の位置Ｐにおいて、地物Ｍ１を検出したとする。地物Ｍ１に相当する地物データＤ１が地図データに存在した場合、自車と地物Ｍ１との相対的な位置関係を維持したまま、検出された地物Ｍ１に対応する地物データＤ１を重ね合わせるように補正することにより縦位置の補正を行う。当該補正により自車の位置が位置Ｑに補正されたとする。ここで、縦補正量が小さい場合には、補正前の地物データＤ２に基づいて、自車の横位置及び車両の補正を行う。このようにすることによりデータの再取得にかかる時間を削減することができる。また、データの再取得にかかる処理負荷の低減を図ることができる。一方、縦補正量が大きい場合、図１０に示すように、前述と同様の縦位置の補正を行った後、自車が位置Ｑに存在するものとして地物データの再取得を行い、得られた地物データＤ３との対応付けを行い、自車の横位置及び車両方位の補正を行う。縦補正量が大きい場合には、補正前の位置Ｐと補正後の位置Ｑとの自車の横位置及び車両方位のずれが大きいことが予測されるため、データの再取得及び対応付けを行うことでより、自車の状態推定の予測精度を向上させることができる。

【0041】

（１ｂ）本実施形態の状態推定装置によれば、検出不適状態の場合に、道路の境界が検出されるか否かによって検出された検出中心位置を用いて横位置及び車両方位の補正を行うか、当該補正を行わず処理を終了するかを切り替える。これにより、道路の境界が検出できない状態においては、標識中心位置に基づいて自車の位置を補正することができる。一方で、道路の境界が検出できる状態においては、標識中心位置に基づいた自車の位置の補正を行うことにより自車の位置が誤って補正されることを抑制することができる。よって、自車の位置の補正の精度を向上することができる。

【0042】

（１ｃ）本実施形態の状態推定装置によれば、取得された画像において、検出された標識の標識中心位置の比較を行うことで、補正の元となる画像が正確に取れたか否かを判定する。判定に応じて、補正を行うか否かを変更することにより、状態推定の精度を向上させる。

【0043】

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

【0044】

（２ａ）上記実施形態では光学センサ５はステレオカメラであるが、上記実施形態では、光学センサ５はステレオカメラであるが、光学センサ５の形態はこれに限定されるものではない。例えば、レーザレーダであるとしてもよく、単眼カメラであるとしてもよい。光学センサ５としてレーザレーダを用いた場合、レーザレーダがレーザを出射してから当該レーザを受光するまでの時間により自車と標識との距離を測定してもよい。また、この場合、上記実施形態で撮像された画像の代わりに距離が測定された点の集合である測距点群に基づいて各地物と車両との距離を求めてもよい。

【0045】

（２ｂ）また、上記実施形態では、ステレオカメラを用いた三角測量により、自車と地物との距離を測定したが、自車と地物との距離の測定方法はこれに限定されるものではない。例えば地物データに含まれる標識データは、地物の高さを表す地物高さデータ又は地物のサイズを表す地物サイズデータを含むとしてもよい。これにより、状態推定装置１は地物高さデータ又は地物サイズデータと、図１１に示す光学センサ５により撮像された画像における検出地物の高さＨ又は地物のサイズＳとを比較することで、自車と地物との距離を測定してもよい。なお、図１１では、道路脇に存在する標識を検出地物としているが検出地物の種類は、これに限定されるものではない。ガードレールなどの高さＨ、サイズＳがあらかじめ地物高さデータ及び地物サイズデータとして、記憶されているものであればよい。この場合において、地物の高さＨに基づいて距離を測定する場合には、逆光の場合や、取得した画像の地物部分の輝度が飽和している場合、模様が無い場合など地物のサイズＳが正確に測定できない場合にも自車と地物との距離を測定することができる。また、地物のサイズＳに基づいて距離を測定する場合には、路面が急勾配などで車両と地物との相対的な高さＨが正確に測定できない場合にも自車と地物との距離を測定することができる。また、これらの地物の高さＨに基づいた距離の測定方法と地物のサイズＳに基づいた距離の測定方法とを組み合わせて自車と地物との距離を測定しても良い。または車両の周辺状況に応じて、これらの距離の測定方法を選択するように構成されてもよい。具体的には、光学センサ５から取得した画像や、地図データに含まれる周辺環境や道路の形状の情報に基づいて、距離の測定方法を選択するように構成されてもよい。なお、当該距離の測定方法を選択する処理が選択部に相当し、状態推定処理のＳ１５０で行われるとしてもよい。

【0046】

（２ｃ）上記実施形態では、自車との距離を測定する地物は標識であるとしたが、自車との距離を測定する地物はこれに限定されるものではない。例えば、信号機などでもよい。

【0047】

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

【0048】

（２ｅ）上述した状態推定装置１の他、当該状態推定装置１を構成要素とするシステム、当該状態推定装置１としてコンピュータを機能させるためのプログラム、このプログラムを記録した半導体メモリ等の非遷移的実態的記録媒体、状態推定方法など、種々の形態で本開示を実現することもできる。

【符号の説明】

【0049】

１…状態推定装置、２…自車、３…ＧＮＳＳ受信機、４…地図メモリ、５…光学センサ、６…状態量センサ、７…制御装置、１１…ＣＰＵ、１２…メモリ。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】