JP7318521B2 - 推定装置、推定方法、推定プログラム - Google Patents

Description

本開示は、車両の位置を含む状態量を、推定する推定技術に関する。

従来、外界へのビーム照射により観測したターゲットからのビーム反射を受けて光学画像を出力する光学センサが、車両には搭載されている。こうした光学センサから所定の照射レンジでのビーム照射により出力される光学画像に基づくことで、車両の状態量が推定される。

さて、特許文献１に開示される技術では、光学センサとしてのＬＩＤＡＲセンサによる走査結果に基づいて走行状況が決定され、照射レンジとなる走査範囲の方位が当該決定状況に依存するように制御されている。

特表２０１９―５０７３２６号公報

しかし、光学センサの出力する光学画像が状態量の推定に利用される場合、特許文献１の開示技術のように走査範囲の方位が変更されると、状態量の推定精度が変動してしまう。この推定精度の変動原理について、説明する。

図２８に示すように、光学センサのビーム直交方向においてターゲットの測距に生じる誤差Ｅｍｏは、前後方向Ｘの前後観測誤差（図示しない）と左右方向Ｙの左右観測誤差Ｅｏｙに座標変換可能である。これら前後観測誤差と左右観測誤差Ｅｏｙとは、走査範囲におけるビームステアリング角度に応じて、個別に変動する。このような現象は、ビーム平行方向においてターゲットの測距に生じる誤差についても、同様に発現する。

こうした知見から、走行状況に依存して走査範囲の方位が変更される場合、当該方位変更に伴って前後観測誤差と左右観測誤差Ｅｏｙとの分布が変化する。その結果、前後観測誤差及び左右観測誤差Ｅｏｙの影響した光学画像に基づく状態量推定では、前後方向Ｘと左右方向Ｙとにおいて方位変更に伴う推定誤差の変動が惹起されてしまう。

本開示の課題は、状態量の推定精度を高める推定装置を、提供することにある。本開示の別の課題は、状態量の推定精度を高める推定方法を、提供することにある。本開示のさらに別の課題は、状態量の推定精度を高める推定プログラムを、提供することにある。

以下、課題を解決するための本開示の技術的手段について、説明する。尚、特許請求の範囲及び本欄に記載された括弧内の符号は、後に詳述する実施形態に記載された具体的手段との対応関係を示すものであり、本開示の技術的範囲を限定するものではない。

本開示の第一態様は、
外界へのビーム照射により観測したターゲット（６）からのビーム反射を受けて光学画像（Ｉｓ）を出力する光学センサ（３０）を、搭載した車両（４）の位置を含む状態量（Ｚ）を、推定する推定装置（１）であって、
前後方向（Ｘ）に沿う基準方位（Ｂｓ）から側方へ広がる照射レンジ（Θｓ）でのビーム照射に応じてターゲットまでの距離値がデータ化されて光学センサから出力される光学画像に基づいて、状態量を推定する状態量推定部（１００）と、
状態量の推定に生じる推定誤差であって、光学画像に対する地図情報（Ｉｍ）のマッチング処理による観測残差（Δｏｘ，Δｏｙ）に対する推定誤差として、前後方向（Ｘ）における前後推定誤差（Ｅｅｘ）及び左右方向（Ｙ）における左右推定誤差（Ｅｅｙ）を取得する推定誤差取得部（１２０）と、
光学センサによるターゲットの観測に生じる観測誤差であって、マッチング処理による観測残差に応じた観測誤差として、前後方向（Ｘ）における前後観測誤差（Ｅｏｘ）及び左右方向（Ｙ）における左右観測誤差（Ｅｏｙ）を、照射レンジにおける照射タイミング（ｔ）毎のビームステアリング角度（θ）に対応付けて取得する観測誤差取得部（１６０）と、
前後推定誤差及び左右推定誤差のうち大きい側を大側推定誤差と定義し、前後観測誤差及び左右観測誤差のうち小さい側を小側観測誤差と定義すると、大側推定誤差と小側観測誤差とが同方向となるビームステアリング角度に集中する指向パターン（Ｐｓｄ）へ、照射レンジでの照射タイミング毎のビームステアリング角度に応じた照射密度の分布であるビームパターン（Ｐｓ）を調整するビーム調整部（１８０）とを、備える。

本開示の第二態様は、
外界へのビーム照射により観測したターゲット（６）からのビーム反射を受けて光学画像（Ｉｓ）を出力する光学センサ（３０）を、搭載した車両（４）の位置を含む状態量（Ｚ）を、推定するためにプロセッサ（１２）により実行される推定方法であって、
前後方向（Ｘ）に沿う基準方位（Ｂｓ）から側方へ広がる照射レンジ（Θｓ）でのビーム照射に応じてターゲットまでの距離値がデータ化されて光学センサから出力される光学画像に基づいて、状態量を推定する状態量推定プロセス（Ｓ１０）と、
状態量の推定に生じる推定誤差であって、光学画像に対する地図情報（Ｉｍ）のマッチング処理による観測残差（Δｏｘ，Δｏｙ）に対する推定誤差として、前後方向（Ｘ）における前後推定誤差（Ｅｅｘ）及び左右方向（Ｙ）における左右推定誤差（Ｅｅｙ）を取得する推定誤差取得プロセス（Ｓ２０）と、
光学センサによるターゲットの観測に生じる観測誤差であって、マッチング処理による観測残差に応じた観測誤差として、前後方向（Ｘ）における前後観測誤差（Ｅｏｘ）及び左右方向（Ｙ）における左右観測誤差（Ｅｏｙ）を、照射レンジにおける照射タイミング（ｔ）毎のビームステアリング角度（θ）に対応付けて取得する観測誤差取得プロセス（Ｓ４０）と、
前後推定誤差及び左右推定誤差のうち大きい側を大側推定誤差と定義し、前後観測誤差及び左右観測誤差のうち小さい側を小側観測誤差と定義すると、大側推定誤差と小側観測誤差とが同方向となるビームステアリング角度に集中する指向パターン（Ｐｓｄ）へ、照射レンジでの照射タイミング毎のビームステアリング角度に応じた照射密度の分布であるビームパターン（Ｐｓ）を調整するビーム調整プロセス（Ｓ５０）とを、含む。

本開示の第三態様は、
外界へのビーム照射により観測したターゲット（６）からのビーム反射を受けて光学画像（Ｉｓ）を出力する光学センサ（３０）を、搭載した車両（４）の位置を含む状態量（Ｚ）を、推定するために記憶媒体（１０）に格納され、プロセッサ（１２）に実行させる命令を含む推定プログラムであって、
命令は、
前後方向（Ｘ）に沿う基準方位（Ｂｓ）から側方へ広がる照射レンジ（Θｓ）でのビーム照射に応じてターゲットまでの距離値がデータ化されて光学センサから出力される光学画像に基づいて、状態量を推定させる状態量推定プロセス（Ｓ１０）と、
状態量の推定に生じる推定誤差であって、光学画像に対する地図情報（Ｉｍ）のマッチング処理による観測残差（Δｏｘ，Δｏｙ）に対する誤差として、前後方向（Ｘ）における前後推定誤差（Ｅｅｘ）及び左右方向（Ｙ）における左右推定誤差（Ｅｅｙ）を取得させる推定誤差取得プロセス（Ｓ２０）と、
光学センサによるターゲットの観測に生じる観測誤差であって、マッチング処理による観測残差に応じた観測誤差として、前後方向（Ｘ）における前後観測誤差（Ｅｏｘ）及び左右方向（Ｙ）における左右観測誤差（Ｅｏｙ）を、照射レンジにおける照射タイミング（ｔ）毎のビームステアリング角度（θ）に対応付けて取得させる観測誤差取得プロセス（Ｓ４０）と、
前後推定誤差及び左右推定誤差のうち大きい側を大側推定誤差と定義し、前後観測誤差及び左右観測誤差のうち小さい側を小側観測誤差と定義すると、大側推定誤差と小側観測誤差とが同方向となるビームステアリング角度に集中する指向パターン（Ｐｓｄ）へ、照射レンジでの照射タイミング毎のビームステアリング角度に応じた照射密度の分布であるビームパターン（Ｐｓ）を調整させるビーム調整プロセス（Ｓ５０）とを、含む。

これら第一～第三態様のビームパターンは、前後方向に沿う基準方位から側方へ広がる光学センサの照射レンジにおいて、特定のビームステアリング角度に集中する指向パターンへと調整される。ここで特定のビームステアリング角度によると、状態量推定に生じる前後推定誤差及び左右推定誤差のうち大側推定誤差と、ターゲット観測に生じる前後観測誤差及び左右観測誤差のうち小側観測誤差とが、同方向となる。これによれば、大側推定誤差を低減可能なターゲットの観測確率が高いビームステアリング角度に対して、ビーム照射を集中させ得る。故に、指向パターンに従うビーム照射に応じて光学センサから出力の光学画像に基づいた状態量推定では、大側推定誤差を低減させて状態量の推定精度を高めることが可能となる。

一実施形態による推定装置と共に車両に搭載される光学センサを説明するための模式図である。 一実施形態による推定装置の全体構成を示すブロック図である。 一実施形態による推定装置の具体的構成を示すブロック図である。 図１～３の光学センサを説明するための模式図である。 図３の状態量推定ブロックを詳細に示すブロック図である。 図３の観測誤差取得ブロック及びビーム調整ブロックを詳細に示すブロック図である。 図６の最短距離特定サブブロックを説明するための模式図である。 図６の測距誤差取得サブブロックを説明するための模式図である。 図６の測距誤差取得サブブロックを説明するための模式図である。 図６の実測誤差取得サブブロックを説明するための模式図である。 図６の実測誤差取得サブブロックを説明するための模式図である。 図６の観測誤差平滑サブブロックを説明するための模式図である。 図６の観測誤差平滑サブブロックを説明するための模式図である。 図６のゲイン設定サブブロックを説明するための模式図である。 図６のゲイン設定サブブロック及びパターン更新サブブロックを説明するための模式図である。 図６のゲイン設定サブブロック及びパターン更新サブブロックを説明するための模式図である。 図６のゲイン制限サブブロックを説明するための模式図である。 図６のゲイン制限サブブロックを説明するための模式図である。 図６のパターン更新サブブロックを説明するための模式図である。 図６のパターン更新サブブロックを説明するための模式図である。 図６のパターン更新サブブロックを説明するための模式図である。 図６のパターン更新サブブロックを説明するための模式図である。 図６のパターン更新サブブロックを説明するための模式図である。 一実施形態による推定方法のフローを示すフローチャートである。 図２４のＳ１０を詳細に示すフローチャートである。 図２４のＳ４０を詳細に示すフローチャートである。 図２４のＳ５０を詳細に示すフローチャートである。 従来の課題を説明するための模式図である。

以下、一実施形態を図面に基づいて説明する。

図１，２に示すように、一実施形態による推定装置１は、地図ユニット２及びセンサユニット３と共に、車両４に搭載される。車両４は、推定装置１による運動推定結果に基づいて走行する、例えば高度運転支援車又は自動運転車等である。尚、水平面上の車両４における水平方向のうち、当該車両４の直進方向が、前後方向Ｘと定義される。また、水平面上の車両４における水平方向のうち、当該車両４の直進方向に対して直交する横方向が、左右方向Ｙと定義される。

図２，３に示すように地図ユニット２は、地図情報Ｉｍを非一時的に格納又は記憶する、例えば半導体メモリ、磁気媒体及び光学媒体等のうち、少なくとも一種類の非遷移的実体的記憶媒体（non-transitory tangible storage medium）を主体に、構成される。地図ユニット２は、車両４の高度運転支援又は自動運転制御に利用される、ロケータのデータベースであってもよい。地図ユニット２は、車両４の運転をナビゲートする、ナビゲーション装置のデータベースであってもよい。地図ユニット２は、車両４の運転をナビゲートする、ナビゲーション装置のデータベースであってもよい。地図ユニット２は、これらのデータベース等のうち複数種類の組み合わせにより、構成されてもよい。

地図ユニット２は、例えば外部センタとの通信等により、最新の地図情報Ｉｍを取得して記憶する。地図情報Ｉｍは、車両４の走行環境を表す情報として、二次元又は三次元にデータ化されている。地図情報Ｉｍは、例えば道路自体の位置、形状及び路面状態等のうち、少なくとも一種類を表した道路情報を、少なくとも含んでいる。地図情報Ｉｍは、例えば道路に付属する標識及び区画線の位置並びに形状等のうち、少なくとも一種類を表した標示情報を、含んでいてもよい。地図情報Ｉｍは、例えば道路に面する建造物及び信号機の位置並びに形状等のうち、少なくとも一種類を表した構造物情報を、含んでいてもよい。

図１～３に示すようにセンサユニット３は、光学センサ３０を少なくとも含んで構成される。光学センサ３０は、例えば車両４の運動推定等に利用可能な、いわゆるＬＩＤＡＲ（Light Detection and Ranging / Laser Imaging Detection and Ranging）である。光学センサ３０は、車両４の外界へのビーム照射により観測したターゲット６からのビーム反射を受けて、光学画像Ｉｓを出力する。本実施形態の光学センサ３０は、車両４におけるフロント部分の中央に配置されることで、前方から左右両側方に跨って外界を走査する。

図３に示すように光学センサ３０は、ビーム素子３００、撮像素子３０１及び撮像回路３０２を、それぞれ有している。撮像回路３０２は、ビーム素子３００及び撮像素子３０１を制御する。具体的に撮像回路３０２は、図１，４に破線矢印で示すように車両４の外界とへ向かうレーザビームを、ビーム素子３００から実質一定強度のパルス光状に、断続照射させる。このとき、光学センサ３０の走査範囲となる照射レンジΘｓは、前後方向Ｘに沿う基準方位Ｂｓから側方へと広がる範囲に、設定される。特に本実施形態の照射レンジΘｓには、車両４の前方を向く基準方位Ｂｓを中心として、ビームステアリング軸Ａｓまわりの左右両側にそれぞれ同一又は相異の設定角度ずつをあけた、臨界端方位Ｂｃまでの範囲が与えられている。尚、光学センサ３０において照射レンジΘｓの起点となるビームステアリング軸Ａｓは、水平面上の車両４における鉛直方向、又は当該鉛直方向に対する傾斜方向に設定される。

撮像回路３０２は、照射レンジΘｓ内においてビームステアリング軸Ａｓまわりのビーム照射方向を決めるビームステアリング角度θを、一定時間間隔の断続的な照射タイミングｔ毎に切り替える。このとき撮像回路３０２は、図４に示すように照射タイミングｔ毎のビームステアリング角度θを等間隔又は不等間隔に規定することで、照射レンジΘｓ内でのビームの照射密度を等密度又は不等密度に分布させる。そこで撮像回路３０２は、各照射タイミングｔのビームステアリング角度θ間での間隔によって決まる、照射密度の分布パターンを光学センサ３０のビームパターンＰｓとして制御する。尚、本実施形態の照射レンジΘｓにおけるビームステアリング角度θは、零値（０）となる基準方位Ｂｓよりも左側領域では負（マイナス）の値、基準方位Ｂｓよりも右側領域では正（プラス）の値として、定義される。また、説明の理解を容易にするために図４は、照射タイミングｔ毎のビームステアリング角度θのうち、一部のみを例示している。

図３に示す撮像回路３０２は、撮像素子３０１における縦横二次元配列の複数画素を、各照射タイミングｔのビームステアリング角度θに対応した走査ライン（即ち、縦列）毎に、順次露光する。このように順次露光のローリングシャッタを採用する撮像回路３０２は、撮像素子３０１において露光された走査ラインの各画素により、外界からのビーム反射を感知する。撮像回路３０２は、走査ラインの各画素において照射タイミングｔからビーム反射を感知するまでのビーム飛行時間の半値を、撮像素子３０１からターゲット６までの距離値へと変換する。撮像回路３０２は、変換した距離値を走査ライン毎の各画素に関連付けしてデータ化することで、いわゆる距離画像又は点群画像としての光学画像Ｉｓを生成する。

尚、撮像素子３０１において走査ライン毎の各画素により感知されるビーム反射強度に応じた輝度値が、それら各画素に関連付けして距離値と共にデータ化されることで、光学画像Ｉｓが生成されてもよい。また撮像素子３０１には、断続的なビーム照射の中断期間に感知する外光に応じて車両４の外界を撮像する機能が、備えられていてもよい。この場合、撮像素子３０１において走査ライン毎の各画素により感知される外光強度に応じた輝度値が、それら各画素に関連付けして距離値と共にデータ化されることで、光学画像Ｉｓが生成されてもよい。

図２に示すように推定装置１は、例えばＬＡＮ（Local Area Network）、ワイヤハーネス及び内部バス等のうち少なくとも一種類を介して、センサユニット３と接続されている。推定装置１は、車両４の高度運転支援又は自動運転制御を実行する、運転制御専用のＥＣＵ（Electronic Control Unit）であってもよい。推定装置１は、車両４の高度運転支援又は自動運転制御に利用される、ロケータのＥＣＵであってもよい。推定装置１は、車両４の運転をナビゲートする、ナビゲーション装置のＥＣＵであってもよい。推定装置１は、光学センサ３０の撮像回路３０２により兼用されてもよい。推定装置１は、これらのＥＣＵ及び回路３０２等のうち、後述の機能を分担する複数種類の組み合わせにより、構成されてもよい。

推定装置１は、メモリ１０及びプロセッサ１２を少なくとも一つずつ含んだ、専用のコンピュータである。メモリ１０は、コンピュータにより読み取り可能なプログラム及びデータを非一時的に格納又は記憶する、例えば半導体メモリ、磁気媒体及び光学媒体等のうち、少なくとも一種類の非遷移的実体的記憶媒体（non-transitory tangible storage medium）である。プロセッサ１２は、例えばＣＰＵ（Central Processing Unit）、ＧＰＵ（Graphics Processing Unit）及びＲＩＳＣ（Reduced Instruction Set Computer）－ＣＰＵ等のうち、少なくとも一種類をコアとして含む。

プロセッサ１２は、メモリ１０に格納された推定プログラムに含まれる複数の命令を、実行する。これにより推定装置１は、車両４の位置を含む状態量を推定するための機能ブロックを、図３に示すように複数構築する。このように推定装置１では、車両４の状態量を推定するためにメモリ１０に格納された推定プログラムが複数の命令をプロセッサ１２に実行させることで、複数の機能ブロックが構築される。複数の機能ブロックには、状態量推定ブロック１００、推定誤差取得ブロック１２０、更新判定ブロック１４０、観測誤差取得ブロック１６０及びビーム調整ブロック１８０が含まれる。

状態量推定ブロック１００は、照射レンジΘｓでのビーム照射に応じて光学センサ３０から出力される光学画像Ｉｓに基づいて、車両４の状態量Ｚを推定する。そのために状態量推定ブロック１００は、図５に示すように相異なる機能のサブブロック１０１，１０２を、有している。

マッチングサブブロック１０１には、光学センサ３０により生成された最新の光学画像Ｉｓが、入力される。マッチングサブブロック１０１には、地図ユニット２に記憶された最新の地図情報Ｉｍが、入力される。マッチングサブブロック１０１は、光学画像Ｉｓに対する地図情報Ｉｍのマッチング処理により、観測残差Δｏｘ，Δｏｙを取得する。

具体的にマッチングサブブロック１０１では、光学画像Ｉｓにおいてターゲット６を観測した画素に対応する反射点が、観測点と認識される。そこでマッチングサブブロック１０１は、光学画像Ｉｓにおけるターゲット６の観測点に関して、方向Ｘ，Ｙでの直交座標を算出する。マッチングサブブロック１０１では、地図情報Ｉｍにおいてターゲット６の観測点に対応した座標点が、マッチング点と定義される。特に本実施形態のマッチング点は、ターゲット６の観測点との距離が最短となる座標点を、意味する。マッチングサブブロック１０１は、地図情報Ｉｍにおけるマッチング点に関して、方向Ｘ，Ｙでの直交座標を算出する。マッチングサブブロック１０１は、各方向Ｘ，Ｙでの観測残差Δｏｘ，Δｏｙのベクトル値を、観測点の直交座標とマッチング点の直交座標との差分演算により算出する。このとき観測残差Δｏｘ，Δｏｙは、光学画像Ｉｓに対応した方向Ｘ，Ｙ毎の残差画像ΔＩ（後述の図１０，１１を参照）として、メモリ１０に保存されてもよい。

フィルタリングサブブロック１０２には、マッチングサブブロック１０１でのマッチングにより取得された最新の観測残差Δｏｘ，Δｏｙが、入力される。フィルタリングサブブロック１０２は、このマッチング結果としての観測残差Δｏｘ，Δｏｙに基づいたフィルタリング処理により、車両４の方向Ｘ，Ｙでの状態量Ｚの推定値を、位置ｘ，ｙを含んで取得する。

具体的にフィルタリングサブブロック１０２は、観測残差Δｏｘ，Δｏｙを入力とする推定フィルタとして、例えば拡張カルマンフィルタ、パーティクルフィルタ、無香料カルマンフィルタ及びアンサンブルカルマンフィルタ等のうち、少なくとも一種類を用いる。フィルタリングサブブロック１０２は、推定フィルタに観測残差Δｏｘ，Δｏｙを通すことで、下記式１に従う状態量Ｚのベクトル値と共に、下記２に従う誤差共分散行列Ｍを算出する。ここで式１においてｘ，ｙは、それぞれ方向Ｘ，Ｙでの車両４の直交座標位置である。式１においてｖｘ，ｖｙは、それぞれ方向Ｘ，Ｙでの車両４の走行速度である。式２においてσｘ，σｙは、それぞれ方向Ｘ，Ｙでの直交座標位置の誤差分散である。式２においてσｖｘ，σｖｙは、それぞれ方向Ｘ，Ｙでの走行速度の誤差分散である。

図３に示すように推定誤差取得ブロック１２０には、フィルタリングサブブロック１０２により取得された最新の誤差共分散行列Ｍが、入力される。推定誤差取得ブロック１２０は、状態量推定ブロック１００での状態量推定に生じる誤差として、誤差共分散行列Ｍに基づく推定誤差Ｅｅｘ，Ｅｅｙを、誤差共分散解析により取得する。ここで推定誤差Ｅｅｘは、車両４の前後方向Ｘにおける前後推定誤差として定義される一方、推定誤差Ｅｅｙは、車両４の左右方向Ｙにおける左右推定誤差として定義される。

具体的に推定誤差取得ブロック１２０は、誤差共分散行列Ｍの対角成分のうち前後方向Ｘでの位置誤差分散σｘ及び速度誤差σｖｘを用いた下記式３に従って、前後推定誤差Ｅｅｘを算出する。推定誤差取得ブロック１２０は、誤差共分散行列Ｍの対角成分のうち左右方向Ｙでの位置誤差分散σｙ及び速度誤差σｖｙを用いた下記式４に従って、左右推定誤差Ｅｅｙを算出する。ここで式３，４においてα，βは、例えば実車試験結果又はシミュレーション結果等に基づく設計パラメータである。前後方向Ｘに関する式３の設計パラメータαと、左右方向Ｙに関する式４の設計パラメータαとは、同一であってもよし、相異していてもよい。また、前後方向Ｘに関する式３の設計パラメータβと、左右方向Ｙに関する式４の設計パラメータβとについても、同様である。

図３に示すように更新判定ブロック１４０には、推定誤差取得ブロック１２０により取得された最新の推定誤差Ｅｅｘ，Ｅｅｙが、入力される。更新判定ブロック１４０は、光学センサ３０の照射レンジΘｓにおけるビームパターンＰｓを更新するか否かを、推定誤差Ｅｅｘ，Ｅｅｙに基づいて判定する。

具体的に更新判定ブロック１４０は、各方向Ｘ，Ｙの推定誤差Ｅｅｘ，Ｅｅｙを、それぞれ許容範囲と対比することで、監視する。その結果、双方の推定誤差Ｅｅｘ，Ｅｅｙが各々の許容範囲内に収まっている間は、光学センサ３０にビームパターンＰｓを維持させる維持判定を、更新判定ブロック１４０が下す。この維持判定の場合、後続ブロック１６０，１８０へは処理が進まずに、状態量推定ブロック１００へと処理が戻ることで、光学センサ３０によりビームパターンＰｓが維持される。一方、推定誤差Ｅｅｘ，Ｅｅｙのうち少なくとも一方が各々の許容範囲から値の大きい側に外れた場合には、光学センサ３０によりビームパターンＰｓを更新させる更新判定を、更新判定ブロック１４０が下す。この更新判定の場合、後続ブロック１６０，１８０へと処理が進むことで、後に詳述するビーム調整ブロック１８０から出力の指向パターンＰｓｄに従ってビームパターンＰｓが光学センサ３０により更新される。

ここで各推定誤差Ｅｅｘ，Ｅｅｙの許容範囲は、推定精度上の許容可能な値の上限を閾値として、当該閾値以下の数値範囲に設定されてもよいし、推定精度上の許容困難な値の下限を閾値として、当該閾値未満の数値範囲に設定されてもよい。前後方向Ｘに関する推定誤差Ｅｅｘの許容範囲と、左右方向Ｙに関する推定誤差Ｅｅｙの許容範囲とは、同一であってもよし、相異していてもよい。

図３に示す観測誤差取得ブロック１６０は、光学センサ３０によるターゲット６の観測に生じる誤差として、光学画像Ｉｓに基づく観測誤差Ｅｏｘ，Ｅｏｙを取得する。ここで観測誤差Ｅｏｘは、車両４の前後方向Ｘにおける前後観測誤差として定義される一方、観測誤差Ｅｏｙは、車両４の左右方向Ｙにおける左右観測誤差として定義される。これらの観測誤差Ｅｏｘ，Ｅｏｙを、いずれも理論と実測の二系統によって取得するために観測誤差取得ブロック１６０は、図６に示すように相異なる機能のサブブロック１６１，１６２，１６３，１６４，１６５，１６６を、有している。

最短距離特定サブブロック１６１には、光学センサ３０により生成された最新の光学画像Ｉｓが、入力される。最短距離特定サブブロック１６１は、光学画像Ｉｓにおけるターゲット６までの最短距離観測点を、照射タイミングｔ毎のビームステアリング角度θに対応付けて特定する。

具体的には、図７（ａ）に示すように最短距離特定サブブロック１６１は、光学画像Ｉｓにおいて照射タイミングｔ毎のビームステアリング角度θと個別に対応する走査ライン（即ち、縦列）の各々から、ビーム反射点までの距離が最短（即ち、距離値が最小）の最短距離画素Ｉｓｏを探索する。最短距離特定サブブロック１６１では、各走査ラインでの最短距離画素Ｉｓｏに対応する反射点が、照射タイミングｔ毎にビームステアリング角度θと対応付けの最短距離観測点として認識される。図７（ｂ），（ｃ）に示すように、最短距離特定サブブロック１６１は、探索した走査ライン毎の最短距離画素Ｉｓｏでの距離値を、最短距離観測点での最短距離値Ｉｓｒのベクトル値として抽出する。最短距離特定サブブロック１６１は、探索した走査ライン毎の最短距離画素Ｉｓｏを識別する画素番号を、最短距離インデックスＩｓｉのベクトル値として抽出する。尚、説明の理解を容易にするために図７は、光学画像Ｉｓにおける一部の画素のみを例示している。

図６に示す測距誤差取得サブブロック１６２は、ターゲット６に対するビーム照射及びビーム反射に応じた測距に生じる誤差として、理論に基づく測距誤差Ｅｍｏ，Ｅｍｐを取得する。ここで図８に示すように測距誤差Ｅｍｏは、照射タイミングｔ毎にビームステアリング角度θの変化するビームに対して直交した、ビーム直交方向Ｄｍｏにおける直交測距誤差として定義される。この直交測距誤差Ｅｍｏは、ターゲット６におけるビーム反射点までの距離に依存し、特に当該距離が短いほど小さくなる。図９に示すように測距誤差Ｅｍｐは、照射タイミングｔ毎にビームステアリング角度θの変化するビームに対して平行な、ビーム平行方向Ｄｍｐにおける平行測距誤差として定義される。この平行測距誤差Ｅｍｐは、ビーム照射及びビーム反射に要するビーム飛行時間の計測において、一定に生じる誤差である。

具体的に、図６に示すように測距誤差取得サブブロック１６２には、最短距離特定サブブロック１６１により取得された最新の最短距離値Ｉｓｒが、入力される。測距誤差取得サブブロック１６２は、照射タイミングｔ毎のビームステアリング角度θに対応した各走査ラインでの直交測距誤差Ｅｍｏのベクトル値を、最短距離値Ｉｓｒを用いた下記式５に従って算出する。ここで式５においてＥａｎは、例えばビームステアリング角度θを調整するアクチュエータ等に起因した、光学センサ３０に固有の角度誤差である。測距誤差取得サブブロック１６２は、照射タイミングｔ毎のビームステアリング角度θに対応した各走査ラインでの平行測距誤差Ｅｍｐのベクトル値を、ビームステアリング角度θには依らない一定値に固定する。このとき測距誤差取得サブブロック１６２は、例えばビーム飛行時間を計測するカウンタ等に起因した、光学センサ３０に固有の計測誤差を、平行測距誤差Ｅｍｐとして設定する。

図６に示すように理論誤差取得サブブロック１６３には、測距誤差取得サブブロック１６２により取得された最新の測距誤差Ｅｍｏ，Ｅｍｐが、入力される。理論誤差取得サブブロック１６３は、測距誤差Ｅｍｏ，Ｅｍｐからの座標変換により、観測誤差Ｅｏｘ，Ｅｏｙの各理論値Ｅｏｘｔ，Ｅｏｙｔを取得する。以下、観測誤差Ｅｏｘ，Ｅｏｙの各理論値Ｅｏｘｔ，Ｅｏｙｔは、理論観測誤差Ｅｏｘｔ，Ｅｏｙｔとも表記される。

具体的に理論誤差取得サブブロック１６３には、後に詳述するビーム調整ブロック１８０からフィードバックされる指向パターンＰｓｄのうち、照射タイミングｔ毎に調整される最新のビームステアリング角度θが、入力される。理論誤差取得サブブロック１６３は、各走査ラインに対応する最新のビームステアリング角度θ毎に、最短距離画素Ｉｓｏ（即ち、最短距離観測点）での理論観測誤差Ｅｏｘｔ，Ｅｏｙｔのベクトル値を、下記式６に従う座標変換により算出する。

図３，６に示すように実測誤差取得サブブロック１６４には、マッチングサブブロック１０１により取得された最新の観測残差Δｏｘ，Δｏｙが、入力される。実測誤差取得サブブロック１６４には、最短距離特定サブブロック１６１により取得された最新の最短距離インデックスＩｓｉが、入力される。実測誤差取得サブブロック１６４は、観測残差Δｏｘ，Δｏｙに基づくことで、観測誤差Ｅｏｘ，Ｅｏｙの各実測値Ｅｏｘｍ，Ｅｏｙｍを取得する。以下、観測誤差Ｅｏｘ，Ｅｏｙの各実測値Ｅｏｘｍ，Ｅｏｙｍは、実測観測誤差Ｅｏｘｍ，Ｅｏｙｍとも表記される。

具体的に実測誤差取得サブブロック１６４は、照射タイミングｔ毎のビームステアリング角度θに対応する各走査ラインにおいて、最短距離インデックスＩｓｉに対応する観測残差Δｏｘを、図１０に示すように前後方向Ｘに関して抽出する。実測誤差取得サブブロック１６４は、照射タイミングｔ毎のビームステアリング角度θに対応する各走査ラインにおいて、最短距離インデックスＩｓｉに対応する観測残差Δｏｙを、図１１に示すように左右方向Ｙに関して抽出する。実測誤差取得サブブロック１６４は、こうして抽出された最短距離インデックスＩｓｉに対応する最短距離画素Ｉｓｏ（即ち、最短距離観測点）での観測残差Δｏｘ，Δｏｙを、それぞれ実測観測誤差Ｅｏｘｍ，Ｅｏｙｍに設定する。尚、図１０，１１は、説明の理解を容易にするために、観測残差Δｏｘ，Δｏｙを残差画像ΔＩとした場合の、一部の画素のみを例示している。

図６に示すように観測誤差合成サブブロック１６５には、理論誤差取得サブブロック１６３により取得された理論観測誤差Ｅｏｘｔ，Ｅｏｙｔが、入力される。観測誤差合成サブブロック１６５には、実測誤差取得サブブロック１６４により取得された実測観測誤差Ｅｏｘｍ，Ｅｏｙｍが、入力される。観測誤差合成サブブロック１６５は、理論観測誤差Ｅｏｘｔ，Ｅｏｙｔに対して、それぞれ実測観測誤差Ｅｏｘｍ，Ｅｏｙｍを合成することで、照射タイミングｔ毎のビームステアリング角度θに対応した各走査ラインでの観測誤差Ｅｏｘ，Ｅｏｙを取得する。

具体的に観測誤差合成サブブロック１６５は、理論観測誤差Ｅｏｘｔ及び実測観測誤差Ｅｏｘｍを用いた下記式７に従う重み付き合成演算により、前後観測誤差Ｅｏｘを算出する。観測誤差合成サブブロック１６５は、理論観測誤差Ｅｏｙｔ及び実測観測誤差Ｅｏｙｍを用いた下記式８に従う重み付き合成演算により、左右観測誤差Ｅｏｙを算出する。ここで式７，８においてγは、例えば実車試験結果又はシミュレーション結果等に基づく重み付けパラメータである。前後方向Ｘに関する式７の重み付けパラメータγと、左右方向Ｙに関する式８の重み付けパラメータγとは、同一であってもよし、相異していてもよい。

図６に示すように観測誤差平滑サブブロック１６６には、観測誤差合成サブブロック１６５により取得された観測誤差Ｅｏｘ，Ｅｏｙが、入力される。観測誤差平滑サブブロック１６６は、照射レンジΘｓでの任意のビームステアリング角度θに対して、観測誤差Ｅｏｘ，Ｅｏｙを平滑化する。

具体的に観測誤差平滑サブブロック１６６は、照射タイミングｔ毎のビームステアリング角度θに対応した各走査ラインでの観測誤差Ｅｏｘ，Ｅｏｙを、平滑化フィルタに通す。平滑化フィルタは、図１２に示すように連続するビームステアリング角度θを変数とした、数次の多項式関数により前後観測誤差Ｅｏｘの分布を近似する。平滑化フィルタは、図１３に示すように連続するビームステアリング角度θを変数とした、別な数次の多項式関数により左右観測誤差Ｅｏｙの分布を近似する。

図３に示すビーム調整ブロック１８０は、照射レンジΘｓにおけるビームパターンＰｓを、照射密度の指向パターンＰｓｄへと調整する。そのためにビーム調整ブロック１８０は、図６に示すように相異なる機能のサブブロック１８１，１８２，１８３を、有している。

図３，６に示すようにゲイン設定サブブロック１８１には、推定誤差取得ブロック１２０により取得された最新の推定誤差Ｅｅｘ，Ｅｅｙが、入力される。ゲイン設定サブブロック１８１は、後に詳述するパターン更新サブブロック１８３に供される調整ゲインδ，εを、推定誤差Ｅｅｘ，Ｅｅｙ同士の比率に基づいて、設定する。

具体的にゲイン設定サブブロック１８１では、推定誤差Ｅｅｘ，Ｅｅｙを用いた下記式９により、推定比率Ｒｅが定義される。ゲイン設定サブブロック１８１は、この推定比率Ｒｅに応じて調整ゲインδ，εを調整する。このとき調整ゲインδ，εは、例えば図１４に示すようなゲインデーブルに従って調整されてもよいし、図示はしない演算式に従って調整されてもよい。

前後推定誤差Ｅｅｘが左右推定誤差Ｅｅｙよりも大きくなることで推定比率Ｒｅが１を下回る図１５（ａ），１６（ａ）の場合、ゲイン設定サブブロック１８１が第一調整ゲインδに正（プラス）の値を与えると共に、第二調整ゲインεにも正（プラス）の値を与える。このとき、第一調整ゲインδ及び第二調整ゲインεのうち少なくとも前者については、推定比率Ｒｅが小さいほど、絶対値が大きくなるように可変設定される。左右推定誤差Ｅｅｙが前後推定誤差Ｅｅｘよりも大きくなることで推定比率Ｒｅが１を上回る図１５（ｂ），１６（ｂ）の場合、ゲイン設定サブブロック１８１が第一調整ゲインδに負（マイナス）の値を与えると共に、第二調整ゲインεには正（プラス）の値を与える。このとき、第一調整ゲインδ及び第二調整ゲインεのうち少なくとも前者については、推定比率Ｒｅが大きいほど、絶対値が大きくなるように可変設定される。

図６に示すようにゲイン制限サブブロック１８２には、ゲイン設定サブブロック１８１により設定された調整ゲインδ，εが、入力される。ゲイン制限サブブロック１８２は、調整ゲインδ，εに対してリミッタ処理を施す。

具体的には図１７に示すように、第一調整ゲインδの絶対値が第一閾値μを上回る場合、ゲイン制限サブブロック１８２が当該ゲインδの絶対値を当該閾値μに制限する。図１８に示すように、第二調整ゲインεの絶対値が第二閾値ρを上回る場合、ゲイン制限サブブロック１８２が当該ゲインεの絶対値を当該閾値ρに制限する。これらの場合以外にゲイン制限サブブロック１８２は、ゲイン設定サブブロック１８１による設定値のままに、調整ゲインδ，εを確定する。

図３，６に示すようにパターン更新サブブロック１８３には、観測誤差平滑サブブロック１６６により平滑化された観測誤差Ｅｏｘ，Ｅｏｙが、入力される。パターン更新サブブロック１８３には、ゲイン制限サブブロック１８２によりリミッタ処理の施された調整ゲインδ，εが、入力される。パターン更新サブブロック１８３は、観測誤差Ｅｏｘ，Ｅｏｙ同士の比率に基づくと共に、調整ゲインδ，εにも基づくことで、照射レンジΘｓでのビームパターンＰｓを最新の指向パターンＰｓｄへと更新する。

具体的にパターン更新サブブロック１８３では、図１９に示す観測誤差Ｅｏｘ，Ｅｏｙを用いた下記式１０により、図２０に示す観測比率Ｒｏが定義される。パターン更新サブブロック１８３は、上述の推定比率Ｒｅに基づく調整ゲインδ，εと共に、この観測比率Ｒｏを用いた下記式１１に従う密度関数Ｆｄにより、照射レンジΘｓでの照射密度更新値を算出する。ここで式１１の密度関数Ｆｄは、観測比率Ｒｏとその比率Ｒｏに対する調整ゲインδ，εとに基づくことで、照射レンジΘｓでの任意のビームステアリング角度θに関して照射密度更新値を、図２１に示すように関数化した演算式となる。ここで式１１においてθｌは、照射レンジΘｓのうち基準方位Ｂｓに対する左側、即ち負（マイナス）側の臨界端方位Ｂｃにおけるビームステアリング角度θの値である。式１１においてθｒは、照射レンジΘｓのうち基準方位Ｂｓに対する右側、即ち正（プラス）側の臨界端方位Ｂｃにおけるビームステアリング角度θの値である。式１１の一部として、観測比率Ｒｏ及び調整ゲインδ，εを用いた線形演算部分は、下記式１２を満たす。これらのことから式１１では、照射レンジΘｓでのビーム振幅により、照射密度更新値が正規化されている。

パターン更新サブブロック１８３は、密度関数Ｆｄにより算出した照射密度の更新値を、照射レンジΘｓでの任意のビームステアリング角度θに関して累積した、累積分布関数Ｆｃへ図２２に示すように変換する。パターン更新サブブロック１８３は、累積分布関数Ｆｃにおいて等間隔の各照射タイミングｔに対応させるビームステアリング角度θを、図２３に示すようにサンプリングする。パターン更新サブブロック１８３は、照射タイミングｔ毎にサンプリングしたビームステアリング角度θ間での最新間隔によって決まる、照射密度更新値の分布パターンを指向パターンＰｓｄとして出力する。尚、説明の理解を容易にするために図２３は、照射タイミングｔ毎のビームステアリング角度θのうち、一部のみを例示している。また図２３は、図４のビームパターンＰｓを与える指向パターンＰｓｄの例を、代表的に表している。

ここでパターン更新サブブロック１８３では、推定誤差Ｅｅｘ，Ｅｅｙのうち、大きい側が大側推定誤差として、また小さい側が小側推定誤差として、それぞれ定義される。パターン更新サブブロック１８３では、各照射タイミングｔでのビームステアリング角度θ毎の観測誤差Ｅｏｘ，Ｅｏｙのうち、大きい側が大側観測誤として、また小さい側が小側観測誤差として、それぞれ定義される。これらの定義下、指向パターンＰｓｄは式１１の密度関数Ｆｄを満たす。

式１１の密度関数Ｆｄを満たす指向パターンＰｓｄは、推定誤差Ｅｅｘ，Ｅｅｙの視点において、推定比率Ｒｅに基づく調整ゲインδ，εに応じた指向性を、ビーム照射に与える。詳細には、図１５（ａ）の場合に指向パターンＰｓｄは、照射レンジΘｓのうち大側推定誤差Ｅｅｘの方向Ｘに最も近いステアリング角度θとして基準方位Ｂｓでの角度θを含んだ側となる、前方領域において照射密度を高い側にバイアスする。このとき特に、調整ゲインδ，εの設定により指向パターンＰｓｄは、特に推定比率Ｒｅが小さいほど、前方領域における照射密度の高側バイアス率を大きく設定する。さらに図１５（ａ）の場合に指向パターンＰｓｄは、照射レンジΘｓのうち小側推定誤差Ｅｅｙの方向Ｙに最も近いステアリング角度θとして臨界端方位Ｂｃでの角度θを含んだ側となる、左右残りの側方領域において照射密度を低い側にバイアスする。

一方、図１５（ｂ）の場合に指向パターンＰｓｄは、照射レンジΘｓのうち大側推定誤差Ｅｅｙの方向Ｙに最も近いステアリング角度θとして臨界端方位Ｂｃでの角度θを含んだ側となる、左右の側方領域において照射密度を高い側にバイアスする。このとき特に、調整ゲインδ，εの設定により指向パターンＰｓｄは、特に推定比率Ｒｅが大きいほど、各側方領域における照射密度の高側バイアス率を大きく設定する。さらに図１５（ｂ）の場合に指向パターンＰｓｄは、照射レンジΘｓのうち小側推定誤差Ｅｅｘの方向Ｘに最も近いステアリング角度θとして基準方位Ｂｓでの角度θを含んだ側となる、残りの前方領域において照射密度を低い側にバイアスする。

式１１の密度関数Ｆｄを満たす指向パターンＰｓｄは、観測誤差Ｅｏｘ，Ｅｏｙの視点において、観測比率Ｒｏに基づいた別の指向性を、ビーム照射に与える。詳細には、図１５（ａ）の場合に指向パターンＰｓｄは、照射レンジΘｓのうち大側推定誤差Ｅｅｘと同方向Ｘの小側観測誤差Ｅｏｘに対応するビームステアリング角度θにおいて、照射密度を高い側にバイアスする。このとき特に、正の第一調整ゲインδに応じた正の相関により指向パターンＰｓｄは、観測比率Ｒｏが大きいほど、小側観測誤差Ｅｏｘの対応角度θにおける照射密度の高側バイアス率を大きく設定する。さらに図１５（ａ）の場合に指向パターンＰｓｄは、照射レンジΘｓのうち大側推定誤差Ｅｅｘと異方向Ｙの小側観測誤差Ｅｏｙに対応する別のステアリング角度θにおいて、照射密度を低い側にバイアスする。このような指向パターンＰｓｄは、大側推定誤差Ｅｅｘと小側観測誤差Ｅｏｙとがそれぞれ異方向Ｘ，Ｙの異方向角度θよりも、大側推定誤差Ｅｅｘと小側観測誤差Ｅｏｘとが同方向Ｘの同方向角度θに対して、ビーム照射を集中させる。これにより、大側推定誤差Ｅｅｘを低減可能なターゲット６の観測確率が低い異方向角度θよりも、当該観測確率が高い同方向角度θに対して、ビーム照射が集中することになる。

一方、図１５（ｂ）の場合に指向パターンＰｓｄは、照射レンジΘｓのうち大側推定誤差Ｅｅｙと同方向Ｙの小側観測誤差Ｅｏｙに対応するビームステアリング角度θにおいて、照射密度を高い側にバイアスする。このとき特に、負の第一調整ゲインδに応じた負の相関により指向パターンＰｓｄは、観測比率Ｒｏが小さいほど、小側観測誤差Ｅｏｙの対応角度θにおける照射密度の高側バイアス率を大きく設定する。さらに図１５（ｂ）の場合に指向パターンＰｓｄは、照射レンジΘｓのうち大側推定誤差Ｅｅｙと異方向Ｘの小側観測誤差Ｅｏｘに対応する別のステアリング角度θにおいて、照射密度を低い側にバイアスする。このような指向パターンＰｓｄは、大側推定誤差Ｅｅｙと小側観測誤差Ｅｏｘとがそれぞれ異方向Ｙ，Ｘの異方向角度θよりも、大側推定誤差Ｅｅｙと小側観測誤差Ｅｏｙとが同方向Ｙの同方向角度θに対して、ビーム照射を集中させる。これにより、大側推定誤差Ｅｅｙを低減可能なターゲット６の観測確率が低い異方向角度θよりも、当該観測確率が高い同方向角度θに対して、ビーム照射が集中することになる。

以上により光学センサ３０は、図３，６に示すようにパターン更新サブブロック１８３から出力される最新の指向パターンＰｓｄに従って、照射レンジΘｓのビームパターンＰｓを制御する。こうして制御された指向パターンＰｓｄでのビーム照射に応じて、状態量Ｚの次回推定が実行される。このとき生じる推定誤差Ｅｅｘ，Ｅｅｙに基づいて次回更新の判定が下される場合、最新の指向パターンＰｓｄに関するデータが、パターン更新サブブロック１８３から理論誤差取得サブブロック１６３へとフィードバックされることとなる。

ここまで説明したように本実施形態では、状態量推定ブロック１００が「状態量推定部」に相当し、推定誤差取得ブロック１２０が「推定誤差取得部」に相当する。また本実施形態では、観測誤差取得ブロック１６０が「観測誤差取得部」に相当し、ビーム調整ブロック１８０が「ビーム調整部」に相当する。

機能ブロック１００，１２０，１４０，１６０，１８０の共同により、推定装置１が車両４の状態量Ｚを推定する推定方法のフローを、図２４～２７に基づいて以下に説明する。尚、本フローは、車両４が運動推定を必要とする推定タイミング毎、又は光学センサ３０のシャッタフレーム毎に、実行される。また、本フローにおいて「Ｓ」とは、推定プログラムに含まれた複数命令により実行される複数ステップを、意味する。

図２４に示すように、Ｓ１０において状態量推定ブロック１００は、照射レンジΘｓでのビーム照射に応じて光学センサ３０から出力される光学画像Ｉｓに基づいて、車両４の状態量Ｚを推定する。

図２５に示すようにＳ１０のうち、Ｓ１０１においてマッチングサブブロック１０１は、光学画像Ｉｓに対する地図情報Ｉｍのマッチング処理により、観測残差Δｏｘ，Δｏｙを取得する。Ｓ１０のうち、Ｓ１０１に続くＳ１０２においてフィルタリングサブブロック１０２は、このマッチングによる観測残差Δｏｘ，Δｏｙに基づくフィルタリング処理により、車両４の方向Ｘ，Ｙでの状態量Ｚの推定値を取得する。

図２４に示すように、Ｓ１０に続くＳ２０において推定誤差取得ブロック１２０は、状態量推定ブロック１００での状態量推定に生じる誤差として、誤差共分散行列Ｍに基づく推定誤差Ｅｅｘ，Ｅｅｙを取得する。さらに続くＳ３０において更新判定ブロック１４０は、光学センサ３０の照射レンジΘｓにおけるビームパターンＰｓを更新するか否かを、推定誤差Ｅｅｘ，Ｅｅｙに基づいて判定する。Ｓ３０において否定判定、即ちビームパターンＰｓの維持判定が下された場合には、光学センサ３０ではビームパターンＰｓが維持されて、今回フローの実行が終了する。

Ｓ３０において肯定判定、即ちビームパターンＰｓの更新判定が下された場合には、今回フローがＳ４０へ移行する。Ｓ４０において観測誤差取得ブロック１６０は、光学センサ３０によるターゲット６の観測に生じる誤差として、光学画像Ｉｓに基づく観測誤差Ｅｏｘ，Ｅｏｙを取得する。

図２６に示すようにＳ４０のうち、Ｓ４０１において最短距離特定サブブロック１６１は、光学画像Ｉｓにおけるターゲット６までの最短距離観測点を、照射タイミングｔ毎のビームステアリング角度θに対応付けて特定する。Ｓ４０のうち、Ｓ４０１に続くＳ４０２において測距誤差取得サブブロック１６２は、ターゲット６に対するビーム照射及びビーム反射に応じた測距に生じる誤差として、理論に基づく測距誤差Ｅｍｏ，Ｅｍｐを取得する。

Ｓ４０のうち、Ｓ４０２に続くＳ４０３において理論誤差取得サブブロック１６３は、測距誤差Ｅｍｏ，Ｅｍｐからの座標変換により、理論観測誤差Ｅｏｘｔ，Ｅｏｙｔを取得する。Ｓ４０のうち、Ｓ４０３に続くＳ４０４において実測誤差取得サブブロック１６４は、観測残差Δｏｘ，Δｏｙに基づくことで、実測観測誤差Ｅｏｘｍ，Ｅｏｙｍを取得する。

Ｓ４０のうち、Ｓ４０４に続くＳ４０５において観測誤差合成サブブロック１６５は、理論観測誤差Ｅｏｘｔ，Ｅｏｙｔに対して、それぞれ実測観測誤差Ｅｏｘｍ，Ｅｏｙｍを合成することで、観測誤差Ｅｏｘ，Ｅｏｙを取得する。Ｓ４０のうち、Ｓ４０５に続くＳ４０６において観測誤差平滑サブブロック１６６は、照射レンジΘｓでの任意のビームステアリング角度θに対して、観測誤差Ｅｏｘ，Ｅｏｙを平滑化する。

図２４に示すように、４０に続くＳ５０においてビーム調整ブロック１８０は、照射レンジΘｓにおけるビームパターンＰｓを、最新の照射密度を決める指向パターンＰｓｄに調整する。

図２７に示すようにＳ５０のうち、Ｓ５０１においてゲイン設定サブブロック１８１は、推定誤差Ｅｅｘ，Ｅｅｙ同士の推定比率Ｒｅに基づいて調整ゲインδ，εを設定する。Ｓ５０のうち、Ｓ５０１に続くＳ５０２においてゲイン制限サブブロック１８２は、調整ゲインδ，εに対するリミッタ処理を実施する。

Ｓ５０のうち、Ｓ５０２に続くＳ５０３においてパターン更新サブブロック１８３は、観測誤差Ｅｏｘ，Ｅｏｙ同士の観測比率Ｒｏと調整ゲインδ，εとに基づくことで、照射レンジΘｓでのビームパターンＰｓを最新の指向パターンＰｓｄへ更新する。その結果、光学センサ３０ではビームパターンＰｓが指向パターンＰｓｄに制御されると共に、理論誤差取得サブブロック１６３へと指向パターンＰｓｄがフィードバックされて、今回フローの実行が終了する。

Ｓ５０３において更新される指向パターンＰｓｄは推定誤差視点では、照射レンジΘｓのうち大側推定誤差の方向に最も近いステアリング角度θ側の領域では、高い側に照射角度をバイアスする。推定誤差視点において指向パターンＰｓｄはまた、照射レンジΘｓのうち小側推定誤差の方向に最も近いステアリング角度θ側の領域では、低い側に照射角度をバイアスする。一方、観測誤差視点において指向パターンＰｓｄは、照射レンジΘｓのうち大側推定誤差と同方向の小側観測誤差に対応するビームステアリング角度θでは、高い側に照射密度をバイアスする。指向パターンＰｓｄは観測誤差視点ではまた、照射レンジΘｓのうち大側推定誤差と異方向の小側観測誤差に対応するビームステアリング角度θでは、低い側に照射密度をバイアスする。これらの観測誤差視点による指向パターンＰｓｄは、大側推定誤差と小側観測誤差とが同方向となるビームステアリング角度θに対してビーム照射が集中するように、指向性を与えることとなる。このような両視点のバイアスが式１１の密度関数Ｆｄにより相俟って合成されることで、パターン更新サブブロック１８３から光学センサ３０に指令される最新の指向パターンＰｓｄが決まることとなる。

ここまで説明したように本実施形態では、Ｓ１０が「状態量推定プロセス」に相当し、Ｓ２０が「推定誤差取得プロセス」に相当する。また本実施形態では、Ｓ４０が「観測誤差取得プロセス」に相当し、Ｓ５０が「ビーム調整プロセス」に相当する。

（作用効果）
以上説明した本実施形態の作用効果を、以下に説明する。

本実施形態のビームパターンＰｓは、前後方向Ｘに沿う基準方位Ｂｓから側方へ広がる光学センサ３０の照射レンジΘｓにおいて、特定のビームステアリング角度θに集中する指向パターンＰｓｄへと調整される。ここで特定のビームステアリング角度θによると、状態量Ｚの推定に生じる前後推定誤差Ｅｅｘ及び左右推定誤差Ｅｅｙのうち大側推定誤差と、ターゲット６の観測に生じる前後観測誤差Ｅｏｘ及び左右観測誤差Ｅｏｙのうち小側観測誤差とが、同方向となる。これによれば、大側推定誤差を低減可能なターゲット６の観測確率が高いビームステアリング角度θに対して、ビーム照射を集中させ得る。故に、指向パターンＰｓｄに従うビーム照射に応じて光学センサ３０から出力の光学画像Ｉｓに基づいた状態量Ｚの推定では、大側推定誤差を低減させて状態量Ｚの推定精度を高めることが可能となる。

本実施形態による指向パターンＰｓｄは、大側推定誤差と同方向の小側観測誤差に対応するビームステアリング角度θでの照射密度を高い側、また大側推定誤差と異方向の小側観測誤差に対応するビームステアリング角度θでの照射密度を低い側に、バイアスする。これによれば、大側推定誤差を低減可能なターゲット６の観測確率が低いビームステアリング角度θよりも、大側推定誤差を低減可能なターゲット６の観測確率が高いビームステアリング角度θに対して、優先的にビーム照射が集中し得る。故に、指向パターンＰｓｄに従うビーム照射に応じての光学画像Ｉｓに基づくことで、大側推定誤差の低減効果、ひいては状態量Ｚの推定精度を高めることが可能となる。

本実施形態によると、照射タイミングｔ毎のビームステアリング角度θに対応付けて取得されるターゲット６の最短距離観測点では、ビーム直交方向Ｄｍｏの成分が最小となる観測誤差Ｅｏｘ，Ｅｏｙからは、大側推定誤差と同方向の小側観測誤差を正しく取得できる。これによれば、小側観測誤差のビームステアリング角度θを狙って集中させたビーム照射に応じての光学画像Ｉｓに基づくことで、大側推定誤差の低減効果、ひいては状態量Ｚの推定精度を高めることが可能となる。

本実施形態によると、ターゲット６の観測に生じる直交測距誤差Ｅｍｏ及び平行測距誤差Ｅｍｐからの座標変換により各理論値Ｅｏｘｔ，Ｅｏｙｔが高精度に取得され得る観測誤差Ｅｏｘ，Ｅｏｙからは、大側推定誤差と同方向の小側観測誤差を正しく取得できる。これによれば、小側観測誤差のビームステアリング角度θを狙って集中させたビーム照射に応じての光学画像Ｉｓに基づくことで、大側推定誤差の低減効果、ひいては状態量Ｚの推定精度を高めることが可能となる。

本実施形態によると、直交測距誤差Ｅｍｏ及び平行測距誤差Ｅｍｐから観測誤差Ｅｏｘ，Ｅｏｙの各理論値Ｅｏｘｔ，Ｅｏｙｔへの座標変換は、ビーム調整ブロック１８０のＳ５０からフィードバックされる指向パターンＰｓｄに基づくことで、正確性を増す。これによれば、大側推定誤差と同方向となる小側観測誤差のビームステアリング角度θを適確に狙って、ビーム照射を集中させることができる。故に、そうしたビーム照射に応じての光学画像Ｉｓに基づくことで、大側推定誤差の低減効果を担保して状態量Ｚの推定精度を高めることが可能となる。

本実施形態によると、光学画像Ｉｓにおけるターゲット６の観測点と地図情報Ｉｍにおけるマッチング点との観測残差Δｏｘ，Δｏｙに基づくことで、高精度に取得され得る観測誤差Ｅｏｘ，Ｅｏｙの各実測値Ｅｏｘｍ，Ｅｏｙｍは、それら誤差の各理論値Ｅｏｘｔ，Ｅｏｙｔと合成される。これによれば、理論と実測の二系統により正確性を増して合成される観測誤差Ｅｏｘ，Ｅｏｙからは、大側推定誤差と同方向となる小側観測誤差のビームステアリング角度θを適確に狙って、ビーム照射を集中させることができる。故に、そうしたビーム照射に応じての光学画像Ｉｓに基づくことで、大側推定誤差の低減効果を担保して状態量Ｚの推定精度を高めることが可能となる。

本実施形態によると、前後観測誤差Ｅｏｘ及び左右観測誤差Ｅｏｙがビームステアリング角度θに対して平滑化される。これによれば、ビームステアリング角度θに対して観測誤差Ｅｏｘ，Ｅｏｙが外乱によりばらつくことに起因して、大側推定誤差と同方向となる小側観測誤差の取得精度が悪化する事態を、抑制できる。故に、そうした小側観測誤差のビームステアリング角度θを狙って集中させたビーム照射に応じての光学画像Ｉｓに基づくことで、外乱による大側推定誤差の増大を抑制し得る。したがって、状態量Ｚの高い推定精度を確保することが可能となる。

本実施形態によると、前後観測誤差Ｅｏｘと左右観測誤差Ｅｏｙとの観測比率Ｒｏに基づいて照射密度がビームステアリング角度θに関して関数化されることで、その密度関数Ｆｄを満たす指向パターンＰｓｄへとビームパターンＰｓが調整される。これによれば、密度関数Ｆｄにおいて観測比率Ｒｏにより決まる小側観測誤差のうち、大側推定誤差と同方向誤差のビームステアリング角度θを正しく反映した指向パターンＰｓｄに従って、当該角度θへとビーム照射を集中させることができる。故に、そうしたビーム照射に応じての光学画像Ｉｓに基づくことで、大側推定誤差の低減効果、ひいては状態量Ｚの推定精度を高めることが可能となる。

本実施形態によると、前後推定誤差Ｅｅｘと左右推定誤差Ｅｅｙとの推定比率Ｒｅに基づくことで、密度関数Ｆｄの観測比率Ｒｏに対する調整ゲインδ，εが設定される。これにより密度関数Ｆｄを満たす指向パターンＰｓｄには、観測誤差Ｅｏｘ，Ｅｏｙだけでなく、推定誤差Ｅｅｘ，Ｅｅｙ自体も反映され得る。故に、そうした指向パターンＰｓｄに従うビーム照射に応じての光学画像Ｉｓに基づくことで、大側推定誤差を適確に低減して状態量Ｚの推定精度を高めることが可能となる。

本実施形態によるビームパターンＰｓは、前後推定誤差Ｅｅｘ及び左右推定誤差Ｅｅｙのうち少なくとも一方が許容範囲から大きい側に外れる場合に、更新される。これにより推定誤差Ｅｅｘ，Ｅｅｙの小さい間は、ビームパターンＰｓを安定させてロバスト性を高める一方で、推定誤差Ｅｅｘ，Ｅｅｙの大きい状況となれば、ビームパターンＰｓを更新して状態量Ｚの推定精度を高めることが可能となる。

（他の実施形態）
以上、一実施形態について説明したが、本開示は、当該実施形態に限定して解釈されるものではなく、本開示の要旨を逸脱しない範囲内において種々の実施形態に適用することができる。

変形例の推定装置１は、デジタル回路及びアナログ回路のうち少なくとも一方をプロセッサとして含んだ、専用のコンピュータであってもよい。ここで特にデジタル回路とは、例えば、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field Programmable Gate Array）、ＳＯＣ（System on a Chip）、ＰＧＡ（Programmable Gate Array）、及びＣＰＬＤ（Complex Programmable Logic Device）等のうち、少なくとも一種類である。またこうしたデジタル回路は、プログラムを格納したメモリを、備えていてもよい。

変形例では、車両４に搭載された少なくとも一つの車載コンピュータと、車両４との間にて通信が可能な少なくとも一つの外部センタコンピュータとから、システム的に推定装置１の機能が分担されてもよい。変形例の地図ユニット２は、車両４との間の通信によりアクセスが可能な、少なくとも一つ外部データベースであってもよい。

変形例の光学センサ３０では、前後方向Ｘに沿う基準方位Ｂｓが車両４の後方を向くことで、当該後方から左右両側方に跨って外界を走査してもよい。変形例の光学センサ３０では、各照射タイミングｔでの等間隔又は不等間隔なビームステアリング角度θ毎にビーム強度が調整されることで、指向パターンＰｓｄが更新されてもよい。

変形例では、最短距離の観測点に代わる平均距離の観測点が、サブブロック１６１及びＳ４０１により抽出されて、サブブロック１６２，１６４及びＳ４０２，Ｓ４０４により利用されてもよい。変形例のサブブロック１８３及びＳ５０３では、指向パターンＰｓｄがサブブロック１６３及びＳ４０３へフィードバックされなくてもよい。

変形例では、更新判定ブロック１４０及びＳ３０が省略されてもよい。変形例では、サブブロック１６２，１６３及びＳ４０２，Ｓ４０３と共に、サブブロック１６５及びＳ４０５が省略されてもよい。変形例では、サブブロック１６４及びＳ４０４と共に、サブブロック１６５及びＳ４０５が省略されてもよい。変形例では、サブブロック１６６及びＳ４０６が省略されてもよい。

変形例のサブブロック１８１及びＳ５０１では、調整ゲインδ，εが一定値に設定されてもよい。変形例では、サブブロック１８２及びＳ５０２が省略されてもよい。変形例のサブブロック１８３及びＳ５０３では、密度関数Ｆｄ及び累積分布関数Ｆｃに代えて、例えばテーブル等に基づくことで、指向パターンＰｓｄが更新されてもよい。変形例のサブブロック１８３及びＳ５０３では、照射レンジΘｓのうち小側推定誤差の方向に最も近いステアリング角度θ側の領域では、照射密度を所定密度に保持するように指向パターンＰｓｄが更新されてもよい。

１ 推定装置、４ 車両、６ ターゲット、１０ メモリ、１２ プロセッサ、３０ 光学センサ、１００ 状態量推定ブロック、１２０ 推定誤差取得ブロック、１４０ 更新判定ブロック、１６０ 観測誤差取得ブロック、１８０ ビーム調整ブロック、Ｂｓ 基準方位、Ｄｍｏ ビーム直交方向、Ｄｍｐ ビーム平行方向、Ｅｅｘ 前後推定誤差、Ｅｅｙ 左右推定誤差、Ｅｍｏ 直交測距誤差、Ｅｍｐ 平行測距誤差、Ｅｏｘｍ，Ｅｏｙｍ 実測値・実測観測誤差、Ｅｏｘｔ，Ｅｏｙｔ 理論値・理論観測誤差、Ｆｄ 密度関数、Ｉｍ 地図情報、Ｉｓ 光学画像、Ｐｓ ビームパターン、Ｐｓｄ 指向パターン、Ｒｅ 推定比率、Ｒｏ 観測比率、Ｘ 前後方向、Ｙ 左右方向、Ｚ 状態量、ｔ 照射タイミング、Δｏｘ，Δｏｙ 観測残差、Θｓ 照射レンジ、δ 第一調整ゲイン、ε 第二調整ゲイン、θ ビームステアリング角度

Claims (30)

1. 外界へのビーム照射により観測したターゲット（６）からのビーム反射を受けて光学画像（Ｉｓ）を出力する光学センサ（３０）を、搭載した車両（４）の位置を含む状態量（Ｚ）を、推定する推定装置（１）であって、
   前後方向（Ｘ）に沿う基準方位（Ｂｓ）から側方へ広がる照射レンジ（Θｓ）での前記ビーム照射に応じて前記ターゲットまでの距離値がデータ化されて前記光学センサから出力される前記光学画像に基づいて、前記状態量を推定する状態量推定部（１００）と、
   前記状態量の推定に生じる推定誤差であって、前記光学画像に対する地図情報（Ｉｍ）のマッチング処理による観測残差（Δｏｘ，Δｏｙ）に対する推定誤差として、前後方向（Ｘ）における前後推定誤差（Ｅｅｘ）及び左右方向（Ｙ）における左右推定誤差（Ｅｅｙ）を取得する推定誤差取得部（１２０）と、
   前記光学センサによる前記ターゲットの観測に生じる観測誤差であって、前記マッチング処理による前記観測残差に応じた観測誤差として、前後方向（Ｘ）における前後観測誤差（Ｅｏｘ）及び左右方向（Ｙ）における左右観測誤差（Ｅｏｙ）を、前記照射レンジにおける照射タイミング（ｔ）毎のビームステアリング角度（θ）に対応付けて取得する観測誤差取得部（１６０）と、
   前記前後推定誤差及び前記左右推定誤差のうち大きい側を大側推定誤差と定義し、前記前後観測誤差及び前記左右観測誤差のうち小さい側を小側観測誤差と定義すると、前記大側推定誤差と前記小側観測誤差とが同方向となる前記ビームステアリング角度に集中する指向パターン（Ｐｓｄ）へ、前記照射レンジでの前記照射タイミング毎の前記ビームステアリング角度に応じた照射密度の分布であるビームパターン（Ｐｓ）を調整するビーム調整部（１８０）とを、備える推定装置。
2. 前記ビーム調整部は、前記大側推定誤差と同方向の前記小側観測誤差に対応する前記ビームステアリング角度での照射密度を高い側にバイアスする前記指向パターンであって、前記大側推定誤差と異方向の前記小側観測誤差に対応する前記ビームステアリング角度での照射密度を低い側にバイアスする前記指向パターンへ、前記ビームパターンを調整する請求項１に記載の推定装置。
3. 前記観測誤差取得部は、前記ターゲットまでの最短距離観測点を前記照射タイミング毎の前記ビームステアリング角度に対応付けて特定し、それら最短距離観測点での前記前後観測誤差及び前記左右観測誤差を取得する請求項１又は２に記載の推定装置。
4. 前記観測誤差取得部は、前記光学センサによる前記ターゲットの測距に生じる誤差として、ビーム直交方向（Ｄｍｏ）における直交測距誤差（Ｅｍｏ）及びビーム平行方向（Ｄｍｐ）における平行測距誤差（Ｅｍｐ）からの座標変換により、前記前後観測誤差及び前記左右観測誤差の各理論値（Ｅｏｘｔ，Ｅｏｙｔ）を取得する請求項１～３のいずれか一項に記載の推定装置。
5. 前記観測誤差取得部は、前記ビーム調整部からフィードバックされる前記指向パターンに基づいた前記座標変換により、前記前後観測誤差及び前記左右観測誤差の各理論値（Ｅｏｘｔ，Ｅｏｙｔ）を取得する請求項４に記載の推定装置。
6. 前記状態量推定部は、前記光学画像に対する前記地図情報の前記マッチング処理に基づいて前記状態量を推定し、
   前記観測誤差取得部は、前記ターゲットの観測点と前記地図情報におけるマッチング点との前記観測残差に基づいて前記前後観測誤差及び前記左右観測誤差の各実測値（Ｅｏｘｍ，Ｅｏｙｍ）を取得し、それら各実測値を前記前後観測誤差及び前記左右観測誤差の各理論値（Ｅｏｘｔ，Ｅｏｙｔ）と合成する請求項４又は５に記載の推定装置。
7. 前記観測誤差取得部は、前記ビームステアリング角度に対して前記前後観測誤差及び前記左右観測誤差を平滑化する請求項１～６のいずれか一項に記載の推定装置。
8. 前記ビーム調整部は、前記前後観測誤差と前記左右観測誤差との観測比率（Ｒｏ）に基づいて照射密度を前記ビームステアリング角度に関して関数化した、密度関数（Ｆｄ）を満たす前記指向パターンへ、前記ビームパターンを調整する請求項１～７のいずれか一項に記載の推定装置。
9. 前記ビーム調整部は、前記密度関数において前記観測比率に対する調整ゲイン（δ，ε）を、前記前後推定誤差と前記左右推定誤差との推定比率（Ｒｅ）に基づいて設定する請求項８に記載の推定装置。
10. 前記前後推定誤差及び前記左右推定誤差のうち少なくとも一方が許容範囲から大きい側に外れる場合に前記ビーム調整部は、前記ビームパターンを更新する請求項１～９のいずれか一項に記載の推定装置。
11. 外界へのビーム照射により観測したターゲット（６）からのビーム反射を受けて光学画像（Ｉｓ）を出力する光学センサ（３０）を、搭載した車両（４）の位置を含む状態量（Ｚ）を、推定するためにプロセッサ（１２）により実行される推定方法であって、
    前後方向（Ｘ）に沿う基準方位（Ｂｓ）から側方へ広がる照射レンジ（Θｓ）での前記ビーム照射に応じて前記ターゲットまでの距離値がデータ化されて前記光学センサから出力される前記光学画像に基づいて、前記状態量を推定する状態量推定プロセス（Ｓ１０）と、
    前記状態量の推定に生じる推定誤差であって、前記光学画像に対する地図情報（Ｉｍ）のマッチング処理による観測残差（Δｏｘ，Δｏｙ）に対する推定誤差として、前後方向（Ｘ）における前後推定誤差（Ｅｅｘ）及び左右方向（Ｙ）における左右推定誤差（Ｅｅｙ）を取得する推定誤差取得プロセス（Ｓ２０）と、
    前記光学センサによる前記ターゲットの観測に生じる観測誤差であって、前記マッチング処理による前記観測残差に応じた観測誤差として、前後方向（Ｘ）における前後観測誤差（Ｅｏｘ）及び左右方向（Ｙ）における左右観測誤差（Ｅｏｙ）を、前記照射レンジにおける照射タイミング（ｔ）毎のビームステアリング角度（θ）に対応付けて取得する観測誤差取得プロセス（Ｓ４０）と、
    前記前後推定誤差及び前記左右推定誤差のうち大きい側を大側推定誤差と定義し、前記前後観測誤差及び前記左右観測誤差のうち小さい側を小側観測誤差と定義すると、前記大側推定誤差と前記小側観測誤差とが同方向となる前記ビームステアリング角度に集中する指向パターン（Ｐｓｄ）へ、前記照射レンジでの前記照射タイミング毎の前記ビームステアリング角度に応じた照射密度の分布であるビームパターン（Ｐｓ）を調整するビーム調整プロセス（Ｓ５０）とを、含む推定方法。
12. 前記ビーム調整プロセスは、前記大側推定誤差と同方向の前記小側観測誤差に対応する前記ビームステアリング角度での照射密度を高い側にバイアスする前記指向パターンであって、前記大側推定誤差と異方向の前記小側観測誤差に対応する前記ビームステアリング角度での照射密度を低い側にバイアスする前記指向パターンへ、前記ビームパターンを調整する請求項１１に記載の推定方法。
13. 前記観測誤差取得プロセスは、前記ターゲットまでの最短距離観測点を前記照射タイミング毎の前記ビームステアリング角度に対応付けて特定し、それら最短距離観測点での前記前後観測誤差及び前記左右観測誤差を取得する請求項１１又は１２に記載の推定方法。
14. 前記観測誤差取得プロセスは、前記光学センサによる前記ターゲットの測距に生じる誤差として、ビーム直交方向（Ｄｍｏ）における直交測距誤差（Ｅｍｏ）及びビーム平行方向（Ｄｍｐ）における平行測距誤差（Ｅｍｐ）からの座標変換により、前記前後観測誤差及び前記左右観測誤差の各理論値（Ｅｏｘｔ，Ｅｏｙｔ）を取得する請求項１１～１３のいずれか一項に記載の推定方法。
15. 前記観測誤差取得プロセスは、前記ビーム調整プロセスからフィードバックされる前記指向パターンに基づいた前記座標変換により、前記前後観測誤差及び前記左右観測誤差の各理論値（Ｅｏｘｔ，Ｅｏｙｔ）を取得する請求項１４に記載の推定方法。
16. 前記状態量推定プロセスは、前記光学画像に対する前記地図情報の前記マッチング処理に基づいて前記状態量を推定し、
    前記観測誤差取得プロセスは、前記ターゲットの観測点と前記地図情報におけるマッチング点との前記観測残差に基づいて前記前後観測誤差及び前記左右観測誤差の各実測値（Ｅｏｘｍ，Ｅｏｙｍ）を取得し、それら各実測値を前記前後観測誤差及び前記左右観測誤差の各理論値（Ｅｏｘｔ，Ｅｏｙｔ）と合成する請求項１４又は１５に記載の推定方法。
17. 前記観測誤差取得プロセスは、前記ビームステアリング角度に対して前記前後観測誤差及び前記左右観測誤差を平滑化する請求項１１～１６のいずれか一項に記載の推定方法。
18. 前記ビーム調整プロセスは、前記前後観測誤差と前記左右観測誤差との観測比率（Ｒｏ）に基づいて照射密度を前記ビームステアリング角度に関して関数化した、密度関数（Ｆｄ）を満たす前記指向パターンへ、前記ビームパターンを調整する請求項１１～１７のいずれか一項に記載の推定方法。
19. 前記ビーム調整プロセスは、前記密度関数において前記観測比率に対する調整ゲイン（δ，ε）を、前記前後推定誤差と前記左右推定誤差との推定比率（Ｒｅ）に基づいて設定する請求項１８に記載の推定方法。
20. 前記前後推定誤差及び前記左右推定誤差のうち少なくとも一方が許容範囲から大きい側に外れる場合に前記ビーム調整プロセスは、前記ビームパターンを更新する請求項１１～１９のいずれか一項に記載の推定方法。
21. 外界へのビーム照射により観測したターゲット（６）からのビーム反射を受けて光学画像（Ｉｓ）を出力する光学センサ（３０）を、搭載した車両（４）の位置を含む状態量（Ｚ）を、推定するために記憶媒体（１０）に格納され、プロセッサ（１２）に実行させる命令を含む推定プログラムであって、
    前記命令は、
    前後方向（Ｘ）に沿う基準方位（Ｂｓ）から側方へ広がる照射レンジ（Θｓ）での前記ビーム照射に応じて前記ターゲットまでの距離値がデータ化されて前記光学センサから出力される前記光学画像に基づいて、前記状態量を推定させる状態量推定プロセス（Ｓ１０）と、
    前記状態量の推定に生じる推定誤差であって、前記光学画像に対する地図情報（Ｉｍ）のマッチング処理による観測残差（Δｏｘ，Δｏｙ）に対する推定誤差として、前後方向（Ｘ）における前後推定誤差（Ｅｅｘ）及び左右方向（Ｙ）における左右推定誤差（Ｅｅｙ）を取得させる推定誤差取得プロセス（Ｓ２０）と、
    前記光学センサによる前記ターゲットの観測に生じる観測誤差であって、前記マッチング処理による前記観測残差に応じた観測誤差として、前後方向（Ｘ）における前後観測誤差（Ｅｏｘ）及び左右方向（Ｙ）における左右観測誤差（Ｅｏｙ）を、前記照射レンジにおける照射タイミング（ｔ）毎のビームステアリング角度（θ）に対応付けて取得させる観測誤差取得プロセス（Ｓ４０）と、
    前記前後推定誤差及び前記左右推定誤差のうち大きい側を大側推定誤差と定義し、前記前後観測誤差及び前記左右観測誤差のうち小さい側を小側観測誤差と定義すると、前記大側推定誤差と前記小側観測誤差とが同方向となる前記ビームステアリング角度に集中する指向パターン（Ｐｓｄ）へ、前記照射レンジでの前記照射タイミング毎の前記ビームステアリング角度に応じた照射密度の分布であるビームパターン（Ｐｓ）を調整させるビーム調整プロセス（Ｓ５０）とを、含む推定プログラム。
22. 前記ビーム調整プロセスは、前記大側推定誤差と同方向の前記小側観測誤差に対応する前記ビームステアリング角度での照射密度を高い側にバイアスする前記指向パターンであって、前記大側推定誤差と異方向の前記小側観測誤差に対応する前記ビームステアリング角度での照射密度を低い側にバイアスする前記指向パターンへ、前記ビームパターンを調整させる請求項２１に記載の推定プログラム。
23. 前記観測誤差取得プロセスは、前記ターゲットまでの最短距離観測を前記照射タイミング毎の前記ビームステアリング角度に対応付けて特定させ、それら最短距離観測点での前記前後観測誤差及び前記左右観測誤差を取得させる請求項２１又は２２に記載の推定プログラム。
24. 前記観測誤差取得プロセスは、前記光学センサによる前記ターゲットの測距に生じる誤差として、ビーム直交方向（Ｄｍｏ）における直交測距誤差（Ｅｍｏ）及びビーム平行方向（Ｄｍｐ）における平行測距誤差（Ｅｍｐ）からの座標変換により、前記前後観測誤差及び前記左右観測誤差の各理論値（Ｅｏｘｔ，Ｅｏｙｔ）を取得させる請求項２１～２３のいずれか一項に記載の推定プログラム。
25. 前記観測誤差取得プロセスは、前記ビーム調整プロセスからフィードバックされる前記指向パターンに基づいた前記座標変換により、前記前後観測誤差及び前記左右観測誤差の各理論値（Ｅｏｘｔ，Ｅｏｙｔ）を取得させる請求項２４に記載の推定プログラム。
26. 前記状態量推定プロセスは、前記光学画像に対する前記地図情報の前記マッチング処理に基づいて前記状態量を推定させ、
    前記観測誤差取得プロセスは、前記ターゲットの観測点と前記地図情報におけるマッチング点との前記観測残差に基づいて前記前後観測誤差及び前記左右観測誤差の各実測値（Ｅｏｘｍ，Ｅｏｙｍ）を取得させ、それら各実測値を前記前後観測誤差及び前記左右観測誤差の各理論値（Ｅｏｘｔ，Ｅｏｙｔ）と合成させる請求項２４又は２５に記載の推定プログラム。
27. 前記観測誤差取得プロセスは、前記ビームステアリング角度に対して前記前後観測誤差及び前記左右観測誤差を平滑化させる請求項２１～２６のいずれか一項に記載の推定プログラム。
28. 前記ビーム調整プロセスは、前記前後観測誤差と前記左右観測誤差との観測比率（Ｒｏ）に基づいて照射密度を前記ビームステアリング角度に関して関数化した、密度関数（Ｆｄ）を満たす前記指向パターンへ、前記ビームパターンを調整させる請求項２１～２７のいずれか一項に記載の推定プログラム。
29. 前記ビーム調整プロセスは、前記密度関数において前記観測比率に対する調整ゲイン（δ，ε）を、前記前後推定誤差と前記左右推定誤差との推定比率（Ｒｅ）に基づいて設定させる請求項２８に記載の推定プログラム。
30. 前記前後推定誤差及び前記左右推定誤差のうち少なくとも一方が許容範囲から大きい側に外れる場合に前記ビーム調整プロセスは、前記ビームパターンを更新させる請求項２１～２９のいずれか一項に記載の推定プログラム。

Images