特許 7605015 追跡装置、追跡方法、追跡プログラム

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

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

(11)【特許番号】

(24)【登録日】2024-12-16

(45)【発行日】2024-12-24

(54)【発明の名称】追跡装置、追跡方法、追跡プログラム

(51)【国際特許分類】

   B60W 40/04 20060101AFI20241217BHJP

   B60W 60/00 20200101ALI20241217BHJP

   G01S 17/66 20060101ALI20241217BHJP

   G01S 17/86 20200101ALI20241217BHJP

   G01S 13/72 20060101ALI20241217BHJP

   G01S 13/86 20060101ALI20241217BHJP

【ＦＩ】

B60W40/04

B60W60/00

G01S17/66

G01S17/86

G01S13/72

G01S13/86

【請求項の数】 8

(21)【出願番号】P 2021082666

(22)【出願日】2021-05-14

(65)【公開番号】P2022175896

(43)【公開日】2022-11-25

【審査請求日】2024-03-19

(73)【特許権者】

【識別番号】000004260

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

(74)【代理人】

【氏名又は名称】矢作 和行

(74)【代理人】

【識別番号】100121991

【弁理士】

【氏名又は名称】野々部 泰平

(74)【代理人】

【識別番号】100145595

【弁理士】

【氏名又は名称】久保 貴則

(72)【発明者】

【氏名】新井 健太郎

【審査官】齊藤 彬

(56)【参考文献】

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

【文献】国際公開第２０２１／０７５３４９（ＷＯ，Ａ１）

【文献】国際公開第２０２１／０７５３３１（ＷＯ，Ａ１）

【文献】中国特許出願公開第１１１６６７５１２（ＣＮ，Ａ）

【文献】中国特許出願公開第１１２０９９３７８（ＣＮ，Ａ）

【文献】中国特許出願公開第１１０８６１６５１（ＣＮ，Ａ）

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

Ｂ６０Ｗ ４０／０４

Ｂ６０Ｗ ６０／００

Ｇ０１Ｓ １７／６６

Ｇ０１Ｓ １７／８６

Ｇ０１Ｓ １３／７２

Ｇ０１Ｓ １３／８６

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

【請求項1】

プロセッサ（１２）を有し、外界センサ系（２）による観測値に基づき移動体（３）の状態値を時系列に推定して前記移動体を追跡する追跡装置であって、
前記プロセッサは、
観測時刻に観測された前記移動体の前記観測値を取得することと、
前記観測時刻における前記移動体の前記状態値を予測することにより、予測状態値を取得することと、
前記観測時刻における前記観測値及び前記予測状態値を変数とした非線形フィルタリングにより、前記観測時刻における前記状態値の真値を推定することとを、実行するように構成され、
前記真値を推定することは、
前記移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、前記外界センサ系からの各前記頂点毎の視認度に応じて設定することと、
各前記頂点での観測誤差を、前記観測時刻における前記観測値及び前記予測状態値に基づき取得することと、
前記観測誤差の共分散を、各前記頂点毎の前記重み係数に基づき取得することとを、含む追跡装置。

【請求項2】

前記重み係数を設定することは、
前記視認度が高い前記頂点ほど、前記重み係数を小さく設定することを、含む請求項１に記載の追跡装置。

【請求項3】

前記重み係数を設定することは、
前記外界センサ系との間に遮蔽物標が存在する前記頂点に対して、前記重み係数を最大値に設定することを、含む請求項１又は２に記載の追跡装置。

【請求項4】

前記重み係数を設定することは、
前記外界センサ系のセンシングエリア外に存在する前記頂点に対して、前記重み係数を最大値に設定することを、含む請求項１～３のいずれか一項に記載の追跡装置。

【請求項5】

前記予測状態値を取得することは、
前記観測時刻よりも前の過去時刻に推定された前記真値に基づき、前記観測時刻における前記予測状態値を取得することを、含む請求項１～４のいずれか一項に記載の追跡装置。

【請求項6】

前記真値を推定することは、
拡張カルマンフィルタを用いた前記非線形フィルタリングにより、前記予測状態値を更新した前記真値を取得することを、含む請求項１～５のいずれか一項に記載の追跡装置。

【請求項7】

外界センサ系（２）による観測値に基づき移動体（３）の状態値を時系列に推定して前記移動体を追跡するために、プロセッサ（１２）により推定される追跡方法であって、
観測時刻に観測された前記移動体の前記観測値を取得することと、
前記観測時刻における前記移動体の前記状態値を予測することにより、予測状態値を取得することと、
前記観測時刻における前記観測値及び前記予測状態値を変数とした非線形フィルタリングにより、前記観測時刻における前記状態値の真値を推定することとを、含み、
前記真値を推定することは、
前記移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、前記外界センサ系からの各前記頂点毎の視認度に応じて設定することと、
各前記頂点での観測誤差を、前記観測時刻における前記観測値及び前記予測状態値に基づき取得することと、
前記観測誤差の共分散を、各前記頂点毎の前記重み係数に基づき取得することとを、含む追跡方法。

【請求項8】

記憶媒体（１０）に記憶され、外界センサ系（２）による観測値に基づき移動体（３）の状態値を時系列に推定して前記移動体を追跡するために、プロセッサ（１２）に実行させる命令を含む追跡プログラムであって、
前記命令は、
観測時刻に観測された前記移動体の前記観測値を取得させることと、
前記観測時刻における前記移動体の前記状態値を予測させることにより、予測状態値を取得させることと、
前記観測時刻における前記観測値及び前記予測状態値を変数とした非線形フィルタリングにより、前記観測時刻における前記状態値の真値を推定させることとを、含み、
前記真値を推定させることは、
前記移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、前記外界センサ系からの各前記頂点毎の視認度に応じて設定させることと、
各前記頂点での観測誤差を、前記観測時刻における前記観測値及び前記予測状態値に基づき取得させることと、
前記観測誤差の共分散を、各前記頂点毎の前記重み係数に基づき取得させることとを、含む追跡プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、移動体を追跡する追跡技術に、関する。

【背景技術】

【0002】

外界センサ系による観測値に基づき移動体の状態値を時系列に推定して移動体を追跡する追跡技術は、広く知られている。追跡技術の一種として非特許文献１には、カルマンフィルタを用いたフィルタリングにより、移動体の状態値推定を時系列に繰り返す手法が、提案されている。

【先行技術文献】

【非特許文献】

【0003】

【文献】3D Multi-Object Tracking: A Baseline and New Evaluation Metrics（ＵＲＬ：https://arxiv.org/pdf/1907.03961.pdf）

【発明の概要】

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

【0004】

しかし、非特許文献１の提案手法は、外界センサ系から移動体の全体が十分に観測されることを、前提としている。そのため、移動体のうち一部の観測が外界センサ系からは困難となる場合、推定される状態値が真値からずれてしまい、追跡精度の低下するおそれがあった。

【0005】

本開示の課題は、移動体に対する追跡精度を高める追跡装置を、提供することにある。本開示の別の課題は、移動体に対する追跡精度を高める追跡方法を、提供することにある。本開示のさらに別の課題は、移動体に対する追跡精度を高める追跡プログラムを、提供することにある。

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

【0006】

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

【0007】

本開示の第一態様は、
プロセッサ（１２）を有し、外界センサ系（２）による観測値に基づき移動体（３）の状態値を時系列に推定して移動体を追跡する追跡装置であって、
プロセッサは、
観測時刻に観測された移動体の観測値を取得することと、
観測時刻における移動体の状態値を予測することにより、予測状態値を取得することと、
観測時刻における観測値及び予測状態値を変数とした非線形フィルタリングにより、観測時刻における状態値の真値を推定することとを、実行するように構成され、
真値を推定することは、
移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、外界センサ系からの各頂点毎の視認度に応じて設定することと、
各頂点での観測誤差を、観測時刻における観測値及び予測状態値に基づき取得することと、
観測誤差の共分散を、各頂点毎の重み係数に基づき取得することとを、含む。

【0008】

本開示の第二態様は、
外界センサ系（２）による観測値に基づき移動体（３）の状態値を時系列に推定して移動体を追跡するために、プロセッサ（１２）により推定される追跡方法であって、
観測時刻に観測された移動体の観測値を取得することと、
観測時刻における移動体の状態値を予測することにより、予測状態値を取得することと、
観測時刻における観測値及び予測状態値を変数とした非線形フィルタリングにより、観測時刻における状態値の真値を推定することとを、含み、
真値を推定することは、
移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、外界センサ系からの各頂点毎の視認度に応じて設定することと、
各頂点での観測誤差を、観測時刻における観測値及び予測状態値に基づき取得することと、
観測誤差の共分散を、各頂点毎の重み係数に基づき取得することとを、含む。

【0009】

本開示の第三態様は、
記憶媒体（１０）に記憶され、外界センサ系（２）による観測値に基づき移動体（３）の状態値を時系列に推定して移動体を追跡するために、プロセッサ（１２）に実行させる命令を含む追跡プログラムであって、
命令は、
観測時刻に観測された移動体の観測値を取得させることと、
観測時刻における移動体の状態値を予測させることにより、予測状態値を取得させることと、
観測時刻における観測値及び予測状態値を変数とした非線形フィルタリングにより、観測時刻における状態値の真値を推定させることとを、含み、
真値を推定させることは、
移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、外界センサ系からの各頂点毎の視認度に応じて設定させることと、
各頂点での観測誤差を、観測時刻における観測値及び予測状態値に基づき取得させることと、
観測誤差の共分散を、各頂点毎の重み係数に基づき取得させることとを、含む。

【0010】

これら第一～第三態様によると、観測時刻における観測値及び予測状態値を変数とする非線形フィルタリングにより、観測時刻における状態値の真値が推定される。このとき、移動体をモデリングした矩形モデルにおける各頂点での観測誤差が、観測時刻における観測値及び予測状態値に基づき取得されると共に、それら各頂点毎の重み係数に基づき当該観測誤差の共分散が取得される。そこで、各頂点毎に外界センサ系からの視認度に応じて重み係数を設定する第一～第三態様によれば、状態値の真値推定に当該視認度が反映され得る。故に、状態値の真値を精確に推定して、移動体に対する追跡精度を高めることが可能となる。

【図面の簡単な説明】

【0011】

【図1】一実施形態による追跡装置の全体構成を示すブロック図である。

【図2】一実施形態による観測値及び矩形モデルを説明するための模式図である。

【図3】一実施形態による追跡装置の機能構成を示すブロック図である。

【図4】一実施形態による追跡方法を示すフローチャートである。

【図5】一実施形態による予測状態値及び矩形モデルを説明するための模式図である。

【図6】一実施形態による推定処理を示すフローチャートである。

【図7】一実施形態による重み設定例を示す模式図である。

【図8】一実施形態による重み設定サブルーチンを示すフローチャートである。

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

【0012】

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

【0013】

図１に示すように一実施形態による追跡装置１は、外界センサ系２による観測値に基づき移動体３の状態値を時系列に推定することで、移動体３を追跡する。そのために追跡装置１は、外界センサ系２と共に、車両４に搭載される。

【0014】

車両４には、手動運転モードとの間での切り替えにより一時的に、又は当該切り替えが実質実行されずに定常的に、自動運転モードが与えられる。自動運転モードは、条件付運転自動化、高度運転自動化、又は完全運転自動化といった、作動時のシステムが全ての運転タスクを実行する自律走行制御により、実現されてもよい。自動運転モードは、運転支援、又は部分運転自動化といった、乗員が一部若しくは全ての運転タスクを実行する高度運転支援制御により、実現されてもよい。自動運転モードは、それら自律走行制御と高度運転支援制御とのいずれか一方、組み合わせ、又は切り替えにより実現されてもよい。

【0015】

外界センサ系２は、車両４の外界に設定されるセンシングエリアＡＳ内を観測することで、当該センシングエリアＡＳでの観測値を出力する。外界センサ系２は、例えＬｉＤＡＲ（Light Detection and Ranging / Laser Imaging Detection and Ranging）、レーダ、カメラ、又はそれらセンシングデバイスのうち少なくとも二種類のフュージョンにより、構成される。

【0016】

外界センサ系２は、所定の追跡周期で観測を繰り返すように、制御される。センシングエリアＡＳ内に移動体３が存在する場合に外界センサ系２からは、当該移動体３に対する観測時刻ｋでの観測値ｚ_ｋが、そうした追跡周期毎に出力される。ここで観測値ｚ_ｋは、図２に模式的に示す物理量を用いた数１により、定義される。数１においてｘ，ｙは、観測空間に定義される直交座標系での移動体３の、それぞれ横方向中心位置及び縦方向中心位置である。数１においてθは、観測空間に定義される直交座標系での移動体３の、横方向に対する方位角である。数１においてｌ，ｗは、観測空間に定義される直交座標系での移動体３の、それぞれ前後方向長さ及び左右方向幅である。

【数1】

【0017】

図１に示す追跡装置１は、例えばＬＡＮ（Local Area Network）、ワイヤハーネス、内部バス、及び無線通信回線等のうち、少なくとも一種類を介して外界センサ系２と接続される。追跡装置１は、少なくとも一つの専用コンピュータを含んで構成される。追跡装置１を構成する専用コンピュータは、車両４の自動運転モードを含む運転制御を担う、運転制御ＥＣＵ（Electronic Control Unit）であってもよい。追跡装置１を構成する専用コンピュータは、車両４の自己状態量を推定する、ロケータＥＣＵであってもよい。追跡装置１を構成する専用コンピュータは、車両４の走行経路をナビゲートする、ナビゲーションＥＣＵであってもよい。追跡装置１を構成する専用コンピュータは、例えば車両４との間で通信可能な外部センタ又はモバイル端末等を構築する、少なくとも一つの外部コンピュータであってもよい。

【0018】

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

【0019】

プロセッサ１２は、メモリ１０に記憶された追跡プログラムに含まれる複数の命令を、実行する。これにより追跡装置１は、移動体３を追跡するための機能ブロックを、複数構築する。このように追跡装置１では、移動体３を追跡するためにメモリ１０に記憶された追跡プログラムが複数命令をプロセッサ１２に実行させることで、複数機能ブロックが構築される。図３に示すように、追跡装置１により構築される複数の機能ブロックには、予測ブロック１００、観測ブロック１１０、及び推定ブロック１２０が含まれる。

【0020】

これら予測ブロック１００、観測ブロック１１０、及び推定ブロック１２０の共同により、追跡装置１が移動体３を追跡する追跡方法のフローを、図４に従って以下に説明する。本フローは、追跡周期毎に実行される。尚、本フローにおいて「Ｓ」とは、追跡プログラムに含まれた複数命令により実行される、複数ステップをそれぞれ意味する。

【0021】

追跡方法のＳ１００において予測ブロック１００は、観測時刻ｋにおける移動体３の状態値を予測することで、図３に示す予測状態値Ｚ_{ｋ｜ｋ－１}及びその誤差共分散Ｐ_{ｋ｜ｋ－１}を取得する。ここで予測状態値Ｚ_{ｋ｜ｋ－１}は、図５に模式的に示す物理量を用いた数２により、定義される。数２においてＸ，Ｙは、観測空間に定義される直交座標系での移動体３の、それぞれ横方向中心位置及び縦方向中心位置である。数２においてΘは、観測空間に定義される直交座標系での移動体３の、横方向に対する方位角である。数２においてＬ，Ｗは、観測空間に定義される直交座標系での移動体３の、それぞれ前後方向長さ及び左右方向幅である。数２においてＶｘ，Ｖｙ（図５での表示は省略）は、観測空間に定義される直交座標系での移動体３の、それぞれ横方向速度及び縦方向中心速度である。

【数2】

【0022】

Ｓ１００における予測ブロック１００は、観測時刻ｋよりも前の過去時刻ｋ－１において推定ブロック１２０により真値として推定された状態値となる推定状態値Ｚ_{ｋ－１｜ｋ－１}、及びその誤差共分散Ｐ_{ｋ－１｜ｋ－１}に対して、時刻変換後の時間更新演算を実行することで、予測状態値Ｚ_{ｋ｜ｋ－１}を予測的に取得する。このとき予測状態値Ｚ_{ｋ｜ｋ－１}及び誤差共分散Ｐ_{ｋ｜ｋ－１}は、過去時刻ｋ－１での推定状態値Ｚ_{ｋ－１｜ｋ－１}及び誤差共分散Ｐ_{ｋ－１｜ｋ－１}をそれぞれ用いて、数３～５により取得される。ここで数４におけるＱは、システムノイズ（プロセスノイズ）の共分散行列である。

【数3】

【数4】

【数5】

【0023】

図４に示す追跡方法のＳ１１０において観測ブロック１１０は、観測時刻ｋにおける移動体３の図３に示す観測値ｚ_ｋを、外界センサ系２から取得する。Ｓ１１０による観測値ｚ_ｋの取得は、外界センサ系２から観測値ｚ_ｋが出力されるのに伴って実行されてもよいし、当該出力の観測値ｚ_ｋがメモリ１０に一旦バッファリングされてから実行されてもよい。また、こうしたＳ１１０による観測値ｚ_ｋの取得は、Ｓ１００による予測状態値Ｚ_{ｋ｜ｋ－１}の取得に対して、並列的（即ち、同時的）に実行されてもよいし、時間を前後して実行されてもよい。

【0024】

図４に示す追跡方法のＳ１２０において推定ブロック１２０は、観測時刻ｋにおける移動体３の状態値を推定することで、図３に示す当該状態値の真値としての推定状態値Ｚ_ｋ｜ｋ及びその誤差共分散Ｐ_ｋ｜ｋを取得する。このとき推定状態値Ｚ_ｋ｜ｋ及び誤差共分散Ｐ_ｋ｜ｋは、観測時刻ｋでの観測値ｚ_ｋ及び予測状態値Ｚ_{ｋ｜ｋ－１}を変数として観測更新演算をする非線形フィルタリングによって、取得される。

【0025】

具体的にＳ１２０における推定ブロック１２０は、図６に示す推定処理を、実行する。推定処理のＳ２００において推定ブロック１２０は、図２，５，７に示すように移動体３を矩形に模式化してモデリングした矩形モデルＭにおける複数頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒ毎に、それぞれ重み係数ｓ_ｆｌ，ｓ_ｂｌ，ｓ_ｆｒ，ｓ_ｂｒを設定する。

【0026】

このときＳ２００における推定ブロック１２０は、外界センサ系２からの各頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒ毎の視認度ω_ｆｌ，ω_ｂｌ，ω_ｆｒ，ω_ｂｒに応じて、それぞれの重み係数ｓ_ｆｌ，ｓ_ｂｌ，ｓ_ｆｒ，ｓ_ｂｒを設定するように、図８に示す重み設定サブルーチンを実行する。本サブルーチンは、頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒの数に対応した回数分、繰り返し実行される。そこで、頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒのうち、本サブルーチンにおいて各回の実行対象となる頂点を、対象頂点ｍという。尚、本サブルーチンの説明では、図７に基づく一部の例示説明を除き、各種変数に下付き文字で示されるサフィックスとして、移動体３の左前方、左後方、右前方、右後方をそれぞれ示すｆｌ，ｂｌ，ｆｒ，ｂｒの表記が、例えば対象頂点ｍの如く省略されている。

【0027】

図８に示す重み設定サブルーチンのＳ３００において推定ブロック１２０は、外界センサ系２の観測原点Ｏと対象頂点ｍとの間に、図７に示すような遮蔽物標ＳＴが存在するか否かを判定する。ここで遮蔽物標ＳＴは、右後方頂点ｍ_ｂｒに関する図７の例示の如く、観測原点Ｏと対象頂点ｍ（同図ではｍ_ｂｒ）との間を結ぶ線分上に、移動体３とは別個に存在する物体であってもよい。遮蔽物標ＳＴは、右前方頂点ｍ_ｆｒに関する図７の例示の如く、観測原点Ｏと対象頂点ｍ（同図ではｍ_ｆｒ）との間を結ぶ線分上に存在する、矩形モデルＭの一辺に対応した移動体３の構成部分であってもよい。尚、観測原点Ｏは、外界センサ系２を構成する単一センサデバイスに設定されるセンシング原点であってもよいし、外界センサ系２を構成する複数センサデバイスのフュージョンによって観測空間に想定される空間原点であってもよい。

【0028】

図８に示すように、Ｓ３００において遮蔽物標ＳＴが存在するとの判定を下した場合には、重み設定サブルーチンがＳ３１０へ移行することで、推定ブロック１２０が視認度ωを最低値に設定する。ここで図７の例示の場合には、遮蔽物標ＳＴに隠れた右後方頂点ｍ_ｂｒ及び右前方頂点ｍ_ｆｒの視認度ω_ｂｒ，ω_ｆｒが、最低値としての「０」に設定される。

【0029】

図８に示すように、Ｓ３００において遮蔽物標ＳＴが存在しないとの判定を下した場合には、重み設定サブルーチンがＳ３２０へ移行することで、対象頂点ｍが外界センサ系２のセンシングエリアＡＳ外に存在するか否かを、推定ブロック１２０が判定する。その結果、対象頂点ｍはセンシングエリアＡＳ外に存在するとの判定が下された場合には、重み設定サブルーチンがＳ３１０へ移行することで、推定ブロック１２０が視認度ωを最低値に設定する。尚、センシングエリアＡＳは、外界センサ系２を構成する単一センサデバイスに設定される視野角であってもよいし、外界センサ系２を構成する複数センサデバイスの視野角同士での重複エリアであってもよい。

【0030】

Ｓ３２０において対象頂点ｍはセンシングエリアＡＳ外に存在しないとの判定を下した場合には、重み設定サブルーチンがＳ３３０へ移行することで、推定ブロック１２０が視認判定角φを取得する。ここで図７に示すように視認判定角φは、対象頂点ｍにおいて対象頂点ｍに接続されている二辺と、観測原点Ｏ及び対象頂点ｍ間を結ぶ線分との、なす角のうち最小角度に定義される。ここで、左後方頂点ｍ_ｂｌに関する図７の例示では最小角度φ_ｂｌ、また左前方頂点ｍ_ｆｌに関する図７の例示では最小角度φ_ｆｌが、それぞれ視認判定角φに対応する。

【0031】

図８に示すように、Ｓ３３０に続く重み設定サブルーチンのＳ３４０において推定ブロック１２０は、視認判定角φが９０度超過であるか否かを、判定する。その結果、視認判定角φが９０度超過である場合には、重み設定サブルーチンがＳ３５０へ移行することで、推定ブロック１２０が視認度ωを最高値に設定する。一方、視認判定角φが９０度以下である場合には、重み設定サブルーチンがＳ３６０へ移行することで、最低値以上且つ最高値以下となる視認度ωを、推定ブロック１２０が数６により設定する。ここで、左後方頂点ｍ_ｂｌに関する図７の例示では９０度超過の視認判定角φ_ｂｌにより視認度ω_ｂｌが最高値としての「１」に設定される一方、左前方頂点ｍ_ｆｌに関する図７の例示では９０度以下の視認判定角φ_ｆｌにより視認度ω_ｆｌが数６での演算値（ｓｉｎφ_ｆｌ）に設定される。

【数6】

【0032】

図８に示すように、Ｓ３１０，Ｓ３５０，Ｓ３６０に続く重み設定サブルーチンのＳ３７０において推定ブロック１２０は、視認度ωが最小値としての「０」であるか否かを、判定する。その結果、視認度ωが０である場合には、重み設定サブルーチンがＳ３８０へ移行することで、推定ブロック１２０が重み係数ｓを最大値ｓ_ｍａｘに設定する。一方、視認度ωが０でない場合には、重み設定サブルーチンがＳ３９０へ移行することで、視認度ωの逆数（１／ω）と、最大値ｓ_ｍａｘとうち小さい側の値に、推定ブロック１２０が重み係数ｓを設定する。特に、重み係数ｓが視認度ωの逆数に設定される場合、視認度ωが高い頂点ｍほど、重み係数ｓが小さく設定されることになる。

【0033】

ここで、遮蔽物標ＳＴに隠れた右後方頂点ｍ_ｂｒ及び右前方頂点ｍ_ｆｒに関する図７の例示では、重み係数ｓが最大値ｓ_ｍａｘに設定される。一方、左後方頂点ｍ_ｂｌ及び左前方頂点ｍ_ｆｌに関する図７の例示では、視認度ωとしての最高値又は数６での演算値に対する逆数（１又は１／ｓｉｎφ_ｆｌ）と、最大値ｓ_ｍａｘとのうち小側の値に、設定されている。尚、重み係数ｓの最大値ｓ_ｍａｘは、後述する観測誤差ｅ_ｎｅｗの共分散Ｓ_ｎｅｗが非線形フィルタリングによって無限大となることを回避可能なガード値に、「１」よりは大きく定義される。

【0034】

このようなＳ２００による重み設定サブルーチンが全頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒに対して完了することで、図６に示ように推定処理がＳ２１０へ移行する。Ｓ２１０において推定ブロック１２０は、各頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒでの観測誤差ｅ_ｎｅｗを、観測時刻ｋにおける観測値ｚ_ｋ及び予測状態値Ｚ_{ｋ｜ｋ－１}に基づき取得する。

【0035】

このときＳ２１０における推定ブロック１２０は、数７の非線形関数ｈ_ｎｅｗ及び数８～１１の行列変換関数により観測値ｚ_ｋの物理量x，ｙ，θ，ｌ，ｗを、矩形モデルＭの各頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒに対して展開した、展開観測値ｚ_ｎｅｗへと変換する。ここで数８～１１においてｘ_ｆｌ，ｘ_ｂｌ，ｘ_ｆｒ，ｘ_ｂｒは、図２に示すように観測値ｚ_ｋの横方向位置ｘを各頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒへ展開した、展開観測値ｚ_ｎｅｗを構成する位置座標となる。数８～１１においてｙ_ｆｌ，ｙ_ｂｌ，ｙ_ｆｒ，ｙ_ｂｒは、図２に示すように観測値ｚ_ｋの縦方向位置ｙを各頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒへ展開した、展開観測値ｚ_ｎｅｗを構成する位置座標となる。

【数7】

【数8】

【数9】

【数10】

【数11】

【0036】

Ｓ２１０における推定ブロック１２０は、数１２の非線形関数ｈ_ｎｅｗ及び数１３～１６の行列変換関数により予測状態値Ｚ_{ｋ｜ｋ－１}の物理量X，Ｙ，Θ，Ｌ，Ｗを、矩形モデルＭの各頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒに対して展開した、展開状態値Ｚ_ｎｅｗへと変換する。ここで数１３～１６においてＸ_ｆｌ，Ｘ_ｂｌ，Ｘ_ｆｒ，Ｘ_ｂｒは、図５に示すように予測状態値Ｚ_{ｋ｜ｋ－１}の横方向位置Ｘを各頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒへ展開した、展開状態値Ｚ_ｎｅｗを構成する位置座標となる。数１３～１６においてＹ_ｆｌ，Ｙ_ｂｌ，Y_ｆｒ，Ｙ_ｂｒは、図５に示すように予測状態値Ｚ_{ｋ｜ｋ－１}の縦方向位置Ｙを各頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒへ展開した、展開状態値Ｚ_ｎｅｗを構成する位置座標となる。

【数12】

【数13】

【数14】

【数15】

【数16】

【0037】

Ｓ２１０における推定ブロック１２０では、このようにして変換された展開観測値ｚ_ｎｅｗ及び展開状態値Ｚ_ｎｅｗを用いる数１７により、図６の如く観測誤差ｅ_ｎｅｗが取得されることとなる。

【数17】

【0038】

Ｓ２１０に続く推定処理のＳ２２０において推定ブロック１２０は、観測誤差ｅ_ｎｅｗの共分散Ｓ_ｎｅｗを、各頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒ毎の重み係数ｓ_ｆｌ，ｓ_ｂｌ，ｓ_ｆｒ，ｓ_ｂｒに基づき取得する。このとき推定ブロック１２０は、重み係数ｓ_ｆｌ，ｓ_ｂｌ，ｓ_ｆｒ，ｓ_ｂｒを用いた数１８により、各頂点ｍ_ｆｌ，ｍ_ｂｌ，ｍ_ｆｒ，ｍ_ｂｒ毎に重み付けされた、観測誤差ｅ_ｎｅｗの８×８共分散行列Ｒ_ｎｅｗを取得する。ここで数１８のＲ’は、横方向位置及び縦方向位置に対する共分散行列であって、予設定により調整可能な調整パラメータである。さらに推定ブロック１２０は、数１２の非線形関数に対する偏微分行列（ヤコビアン）Ｈ_ｎｅｗを、数１９により取得する。こうして推定ブロック１２０では、観測誤差ｅ_ｎｅｗの共分散行列Ｒ_ｎｅｗ及び偏微分行列Ｈ_ｎｅｗと共に、予測状態値Ｚ_{ｋ｜ｋ－１}の誤差共分散Ｐ_{ｋ｜ｋ－１}を用いた数２０により、観測誤差ｅ_ｎｅｗの共分散Ｓ_ｎｅｗが取得される。

【数18】

【数19】

【数20】

【0039】

Ｓ２２０に続く推定処理のＳ２３０において推定ブロック１２０は、拡張カルマンフィルタを用いた非線形フィルタリングにより、予測状態値Ｚ_{ｋ｜ｋ－１}を更新した真値としての推定状態値Ｚ_ｋ｜ｋを、その誤差共分散Ｐ_ｋ｜ｋと共に取得する。このとき推定ブロック１２０は、観測誤差ｅ_ｎｅｗの共分散Ｓ_ｎｅｗ及び偏微分行列Ｈ_ｎｅｗと共に、予測状態値Ｚ_{ｋ｜ｋ－１}の誤差共分散Ｐ_{ｋ｜ｋ－１}を用いた数２１により、拡張カルマンフィルタのカルマンゲインＫ_ｎｅｗを取得する。これにより推定ブロック１２０では、カルマンゲインＫ_ｎｅｗ及び観測誤差ｅ_ｎｅｗと共に、予測状態値Ｚ_{ｋ｜ｋ－１}を用いた数２２により、推定状態値Ｚ_ｋ｜ｋが取得される。さらに推定ブロック１２０では、カルマンゲインＫ_ｎｅｗ及び偏微分行列Ｈ_ｎｅｗと共に、予測状態値Ｚ_{ｋ｜ｋ－１}の誤差共分散Ｐ_{ｋ｜ｋ－１}を用いた数２３により、推定状態値Ｚ_ｋ｜ｋの誤差共分散Ｐ_ｋ｜ｋが取得される。ここで数２３のＩは、単位行列である。

【数21】

【数22】

【数23】

【0040】

図４に示すように、今回の追跡周期での追跡方法により取得された最新の観測時刻ｋにおける推定状態値Ｚ_ｋ｜ｋは、運転制御ＥＣＵへ出力されることで、車両４の自動運転モードを含む運転制御に活用される。また、今回の追跡周期での追跡方法により取得された観測時刻ｋにおける推定状態値Ｚ_ｋ｜ｋは、次回の追跡周期での追跡方法では過去時刻ｋ－１における推定状態値Ｚ_{ｋ－１｜ｋ－１}として、予測ブロック１００によるＳ１００での予測に使用される。

【0041】

（作用効果）
以上説明した本実施形態の作用効果を、以下に説明する。尚、作用効果の説明では、各種変数に下付き文字で示されるサフィックスとして、移動体３の左前方、左後方、右前方、右後方をそれぞれ示すｆｌ，ｂｌ，ｆｒ，ｆｒの表記が、省略されている。

【0042】

本実施形態によると、観測時刻ｋにおける観測値ｚ_ｋ及び予測状態値Ｚ_{ｋ｜ｋ－１}を変数とする非線形フィルタリングにより、観測時刻ｋにおける状態値の真値が推定される。このとき、移動体３をモデリングした矩形モデルＭにおける各頂点ｍでの観測誤差ｅ_ｎｅｗが、観測時刻ｋにおける観測値ｚ_ｋ及び予測状態値Ｚ_{ｋ｜ｋ－１}に基づき取得されると共に、それら各頂点ｍ毎の重み係数ｓに基づき当該観測誤差ｅ_ｎｅｗの共分散Ｓ_ｎｅｗが取得される。そこで、各頂点毎に外界センサ系２からの視認度ωに応じて重み係数ｓを設定する本実施形態によれば、状態値の真値推定に当該視認度ωが反映され得る。故に、状態値の真値である推定状態値Ｚ_ｋ｜ｋを精確に推定して、移動体３に対する追跡精度を高めることが可能となる。

【0043】

本実施形態によると、視認度ωが高い頂点ｍほど、重み係数ｓが小さく設定される。これによれば、外界センサ系２から高い視認度ωの頂点ｍに関しては、共分散Ｓ_ｎｅｗの行列成分が小さくなることで、状態値の真値推定に対する寄与度が高くなる。故に、外界センサ系２からの視認度ωを反映させた精確な真値としての推定状態値Ｚ_ｋ｜ｋを推定して、移動体３に対する追跡精度を高めることが可能となる。

【0044】

本実施形態によると、外界センサ系２との間に遮蔽物標ＳＴが存在する頂点ｍに対して、重み係数ｓが最大値ｓ_ｍａｘに設定される。これによれば、遮蔽物標ＳＴに隠れるために外界センサ系２からの視認度ωが実質零と想定される頂点ｍに関しては、共分散Ｓ_ｎｅｗの行列成分が大きくなることで、状態値の真値推定に対する寄与度が低くなる。故に、外界センサ系２からの視認度ωを反映させた精確な真値としての推定状態値Ｚ_ｋ｜ｋを推定して、移動体３に対する追跡精度を高めることが可能となる。

【0045】

本実施形態によると、外界センサ系２のセンシングエリアＡＳ外に存在する頂点ｍに対して、重み係数ｓが最大値ｓ_ｍａｘに設定される。これによれば、センシングエリアＡＳ外のために外界センサ系２からの視認度ωが実質零と想定される頂点ｍに関しては、共分散Ｓ_ｎｅｗの行列成分が大きくなることで、状態値の真値推定に対する寄与度が低くなる。故に、外界センサ系２からの視認度ωを反映させた精確な真値としての推定状態値Ｚ_ｋ｜ｋを推定して、移動体３に対する追跡精度を高めることが可能となる。

【0046】

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

【0047】

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

【0048】

変形例による追跡装置、追跡方法、及び追跡プログラムは、車両以外へ適用されてもよい。この場合、特に車両以外へ適用される追跡装置は、外界センサ系２と同一の適用対象に搭載又は設置されていてもよいし、外界センサ系２と異なる適用対象に搭載又は設置されていてもよい。

【符号の説明】

【0049】

２：外界センサ系、３：移動体、１０：メモリ、１２：プロセッサ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】