知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2025-04-14
(45)【発行日】2025-04-22
(54)【発明の名称】物体追跡装置
(51)【国際特許分類】
G01S 13/72 20060101AFI20250415BHJP
G01S 13/931 20200101ALN20250415BHJP
【ＦＩ】
G01S13/72
G01S13/931
【請求項の数】
5
(21)【出願番号】P 2024530916
(86)(22)【出願日】2023-06-28
(86)【国際出願番号】 JP2023023958
(87)【国際公開番号】W WO2024005063
(87)【国際公開日】2024-01-04
【審査請求日】2024-08-23
(31)【優先権主張番号】P 2022107182
(32)【優先日】2022-07-01
(33)【優先権主張国・地域又は機関】JP
(73)【特許権者】
【識別番号】000004260
【氏名又は名称】株式会社デンソー
(74)【代理人】
【識別番号】110000578
【氏名又は名称】名古屋国際弁理士法人
(72)【発明者】
【氏名】米田 雅樹
(72)【発明者】
【氏名】小川 高志
【審査官】藤田 都志行
(56)【参考文献】
【文献】米国特許出願公開第２０２２／０１０５９４６（ＵＳ，Ａ１）
【文献】欧州特許出願公開第３５６１５４２（ＥＰ，Ａ１）
【文献】中国特許出願公開第１１２９４６６２５（ＣＮ，Ａ）
【文献】特開２０１９－１５２５７５（ＪＰ，Ａ）
【文献】特開２０２１－００４７３７（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０１Ｓ ７／００－ ７／４２
Ｇ０１Ｓ １３／００－１３／９５
ＪＳＴＰｌｕｓ／ＪＳＴ７５８０／ＪＳＴＣｈｉｎａ（ＪＤｒｅａｍＩＩＩ）
(57)【特許請求の範囲】
【請求項1】
センサ（５）により、周辺環境を所定のサイクルにてセンシングし、当該センシングによって得られた情報に基づいて、ターゲットである物体（３）を追跡する物体追跡装置（１）であって、前記センサによって得られた前記物体を示す観測点の情報を用いて、現サイクルにおける前記物体の状態を示す状態量を推定するように構成された追跡処理部（９）を備え、
前記追跡処理部は、
前記物体の状態量として、少なくとも前記物体の位置と前記物体の形状とに関する前記状態量を用いるものであり、
前サイクルにて推定した前記物体の状態量に基づいて、前記現サイクルの前記物体から得られた前記観測点を抽出するように構成された観測点抽出部（３１）と、
前記前サイクルにて推定した前記物体の状態量に基づいて、前記観測点が期待できる予測する位置に、少なくとも一つの予測点を生成するように構成された予測点生成部（３３）と、
前記現サイクルの前記物体から得られた前記観測点と、前記予測点生成部で生成した前記予測点と、をスキャンマッチングにより位置合わせするように構成された位置合わせ部（３５）と、
前記現サイクルから得られた前記観測点と前記位置合わせされた前記予測点から、前記観測点に対応する前記予測された位置における前記物体の対応点を求め、前記観測点と前記対応点との残差を算出するように構成された残差計算部（３７）と、
前記残差計算部にて算出した残差に基づいて、前記物体の状態量を更新するように構成された状態更新部（３９）と、
を備えた、物体追跡装置。
【請求項2】
請求項１に記載の物体追跡装置であって、前記位置合わせ部は、前記現サイクルの前記物体から得られた前記観測点から、前記物体の輪郭上又は前記輪郭の所定範囲内の近傍にある前記観測点を抽出し、前記抽出した前記観測点を前記スキャンマッチングに用いるように構成された、
物体追跡装置。
【請求項3】
請求項１に記載の物体追跡装置であって、前記位置合わせ部は、前記スキャンマッチングの位置合わせの過程で、前記予測点生成部で生成した前記予測点のうち、前記センサにより観測できる前記予測点を選択するように見直しを行うように構成された、
物体追跡装置。
【請求項4】
請求項１に記載の物体追跡装置であって、前記位置合わせ部は、前記センサにより観測できる前記観測点の点群の空間的な広がりを判断し、前記空間的な広がりが所定範囲以下の場合には、前記スキャンマッチングを行わないように構成された、
物体追跡装置。
【請求項5】
請求項１に記載の物体追跡装置であって、前記位置合わせ部は、前記センサにより観測できる前記観測点の点群の空間的な広がりを判断し、前記空間的な広がりが所定範囲以下の場合には、前記所定範囲以下でない場合に比べて、前記観測点を用いて前記状態量を更新する際に、前記観測点の重み付けを低減するように構成された、
物体追跡装置。
【発明の詳細な説明】
【関連出願の相互参照】
【0001】
本国際出願は、２０２２年７月１日に日本国特許庁に出願された日本国特許出願第２０２２－１０７１８２号に基づく優先権を主張するものであり、日本国特許出願第２０２２－１０７１８２号の全内容を本国際出願に参照により援用する。
【技術分野】
【0002】
本開示は、車両等の物体を追跡する技術に関する。
【背景技術】
【0003】
従来、レーダー等のセンサを用いて、自車両の周囲の他車両等の物体（即ち、ターゲット）を追跡（即ち、トラッキング）する技術として、図１７の上図左側に示すように、ターゲットを一つの質点と仮定して追跡する質点追跡の技術が知られている。
【0004】
また、近年では、質点追跡による課題（即ち、追跡が不安定になるという課題）を改善するために、図１７の上図右側に示すように、ターゲットの運転状態に加えて、ターゲットの形状も含めてモデルに織り込んで追跡する拡張物体追跡（即ち、ＥＯＴ）が知られている。なお、ＥＯＴは、Extended Object Trackingの略であり、この技術は、例えば、非特許文献１等に開示されている。
【0005】
しかし、この拡張物体追跡の技術においては、図１７の中図に示すように、ターゲットや自車両の動きが急に変化した場合や、追跡初期のように、ターゲットの観測情報が十分でない場合には、ターゲットの追跡を精度良く行えないことがあった。
【0006】
この対策として、例えば、下記特許文献１に記載の技術が提案されている。この技術とは、図１７の下図に示すように、ある時刻で予測された追跡対象の状態分布から、観測点の確からしさを尤度関数として定義し、観測点の数に併せて尤度関数を補正（即ち、重みを調整）した後で状態分布を更新することで、疎な観測情報での追跡精度の劣化および追跡失敗を抑制する技術である。
【0007】
この特許文献１の公報には、パーティクルフィルタで取りうる予測分布を振って、予測誤差が生じたときに対応することが示されている。つまり、予測状態分布の仮説を複数生成することで、追跡性の改善（例えば、トラッキングロス等の改善）を図ることが記載されている。
【先行技術文献】
【特許文献】
【0008】
【文献】K.Granstrom,M.Baum, and S.Reuter,“Extended object tracking:Intro-duction,overview, and applications,”Journal of Advances in Information Fusion,vol.12,no.2,2017.
【文献】特開２０２１－６７４８２号公報
【発明の概要】
【0009】
発明者の詳細な検討の結果、従来の技術について、下記のような課題が見出された。
具体的には、特許文献１に記載の技術では、予測状態分布の仮説を複数生成するので、そのための演算の負荷が大きくなるという問題がある。
具体的には、特許文献１に記載の技術では、予測状態分布の仮説を複数生成するので、そのための演算の負荷が大きくなるという問題がある。
【0010】
つまり、自車両のコンピュータ等の電子制御装置では、その演算能力には制限があるので、上述のように予測状態分布の仮説を複数生成する場合には、演算の負担が大きいという問題がある。
【0011】
すなわち、電子制御装置にて演算を行う場合において、限られた計算量の制約下では、生成できる仮説数は限定されるので、対応できる予測ズレの範囲には限界があり、精度の高い追跡を行うことは容易ではない。特に、ターゲットの状態量を示す状態ベクトルが高次元になる場合は、予測ズレの範囲が顕著に狭くなるという問題がある。
【0012】
本開示の一局面は、物体を精度良く追跡することができるとともに、物体を追跡する際の演算の負荷を低減できる技術を提供することが望ましい。
本開示の一態様は、センサ（５）により、周辺環境を所定のサイクルにてセンシングし、当該センシングによって得られた情報に基づいて、ターゲットである物体（３）を追跡する物体追跡装置（１）に関する。
本開示の一態様は、センサ（５）により、周辺環境を所定のサイクルにてセンシングし、当該センシングによって得られた情報に基づいて、ターゲットである物体（３）を追跡する物体追跡装置（１）に関する。
【0013】
物体追跡装置は、追跡処理部（９）を備える。追跡処理部は、センサによって得られた物体を示す観測点の情報を用いて、現サイクルにおける物体の状態を示す状態量を推定するように構成されており、物体の状態量として、少なくとも物体の位置と物体の形状とに関する状態量を用いる。
【0014】
追跡処理部（９）は、観測点抽出部（３１）と予測点生成部（３３）と位置合わせ部（３５）と残差計算部（３７）と状態更新部（３９）とを備える。
追跡処理部は、センサによって得られた物体を示す観測点の情報を用いて、現サイクルにおける物体の状態を示す状態量を推定するように構成されている。
追跡処理部は、センサによって得られた物体を示す観測点の情報を用いて、現サイクルにおける物体の状態を示す状態量を推定するように構成されている。
【0015】
観測点抽出部は、前サイクル（即ち、現サイクルの直前のサイクル）にて推定した物体の状態量に基づいて、現サイクルの物体から得られた観測点を抽出するように構成されている。なお、初期サイクルの場合は、得られた観測点全てを抽出するように構成されていてもよい。
【0016】
予測点生成部は、前サイクルにて推定した物体の状態量に基づいて、観測点が期待できる予測する位置に、少なくとも一つの予測点を生成するように構成されている。
位置合わせ部は、現サイクルの物体から得られた観測点と、予測点生成部で生成した予測点と、をスキャンマッチングにより位置合わせするように構成されている。
位置合わせ部は、現サイクルの物体から得られた観測点と、予測点生成部で生成した予測点と、をスキャンマッチングにより位置合わせするように構成されている。
【0017】
残差計算部は、現サイクルから得られた観測点と位置合わせされた予測点から、観測点に対応する予測された位置における物体の対応点を求め、観測点と対応点との残差を算出するように構成されている。
【0018】
状態更新部は、残差算出部にて算出した残差に基づいて、物体の状態量を更新するように構成されている。
このような構成により、本開示の一態様の物体追跡装置では、物体を精度良く追跡することができるとともに、物体を追跡する際の演算の負荷を低減することができる。
このような構成により、本開示の一態様の物体追跡装置では、物体を精度良く追跡することができるとともに、物体を追跡する際の演算の負荷を低減することができる。
【0019】
つまり、本開示では、従来のように、予測を複数振るのではなく、観測点と予測形状（即ち、予測点）とのズレ（即ち、対応関係）を計算した残差を用いることにより、物体の追跡のための演算を効率的に行うことができる。従って、物体の追跡を行う際の演算の負荷を低減でき、好適に物体を追跡することができる。
【0020】
また、この欄及び請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本開示の技術的範囲を限定するものではない。
【図面の簡単な説明】
【0021】
【図1】第１実施形態の物体追跡装置の全体構成を示すブロック図である。
【図2】第１実施形態の物体追跡装置を機能的に示すブロック図である。
【図3】第１実施形態の物体追跡装置の制御処理を示すフローチャートである。
【図9】スキャンマッチングの方法を示す説明図である。
【図12】残差ベクトルの算出方法を示す説明図である。
【図13】状態量の更新の方法を示す説明図である。
【図15】第３実施形態の物体追跡の手順のうち、観測点の広がりが少ない状態を示す説明図である。
【図16】第３実施形態の物体追跡の手順のうち、矩形の面積と観測点の共分散との関係を示す説明図である。
【図17】従来技術の説明図である。
【発明を実施するための形態】
【0022】
以下、本開示の例示的な実施形態について図面を参照しながら説明する。
［１．第１実施形態］
本第１実施形態では、車両（例えば、自車両）に搭載された物体追跡装置を例に挙げて説明する。即ち、センサにより、自車両の周囲の周辺環境を所定のサイクルにて走査（即ち、センシング）し、当該センシングによって得られた情報に基づいて、ターゲットである物体を追跡する物体追跡装置を例に挙げて説明する。
［１．第１実施形態］
本第１実施形態では、車両（例えば、自車両）に搭載された物体追跡装置を例に挙げて説明する。即ち、センサにより、自車両の周囲の周辺環境を所定のサイクルにて走査（即ち、センシング）し、当該センシングによって得られた情報に基づいて、ターゲットである物体を追跡する物体追跡装置を例に挙げて説明する。
【0023】
［１－１．全体構成］
まず、本第１実施形態の物体追跡装置１の全体構成を、図１に基づいて説明する。
図１に示すように、物体追跡装置１は、車両（例えば、自車両）に搭載されて、自車両の周囲の他車両等の物体（即ち、ターゲット）３を追跡する装置である。
まず、本第１実施形態の物体追跡装置１の全体構成を、図１に基づいて説明する。
図１に示すように、物体追跡装置１は、車両（例えば、自車両）に搭載されて、自車両の周囲の他車両等の物体（即ち、ターゲット）３を追跡する装置である。
【0024】
つまり、物体追跡装置１は、後に詳述するように、ターゲット３を観測した少なくとも１つの観測点から、広がりのあるターゲット３を認識してターゲット３の追跡処理を行うＥＯＴを実現する装置である。
【0025】
なお、ここでは、ターゲット３として、自動車等の車両を例に挙げているが、それ以外にも、バイクや歩行者などの移動する物体が挙げられる。また、一定の形状を有する静止物も挙げられる。
【0026】
この物体追跡装置１は、自車両の周囲をセンシングする周囲センサである例えば測距センサ５、自車両の状態を検出する挙動センサ群７、ターゲット３の追跡を行うための処理を行う追跡処理部９等を備える。
【0027】
測距センサ５としては、ターゲット３の位置に関する情報、例えば、物体追跡装置１とターゲット３との距離や、物体追跡装置１に対するターゲット３の方向の情報が得られる各種のセンサが挙げられる。
【0028】
例えば、周知の、レーダー（Ｒａｄｅｒ）、ライダー（ＬｉＤＡＲ）、ソナー（Ｓｏｎａｒ）、ステレオカメラ、単眼カメラのうち、少なくとも１種が挙げられる。なお、前記センサとしては、空間における位置を示す３次元情報（ｘ、ｙ、ｚ）を観測できるものや、ドップラー速度や信号強度を観測できるものも採用できる。
【0029】
この測距センサ５は、ターゲット３のセンシング（即ち、レーダー等により走査して検出すること）と、ターゲット３にて反射した部分を示す反射点群の観測とを、所定時間間隔（即ち、所定のサイクル）で繰り返す。そして、後述するように、追跡処理部９では、特定時刻ｋにおいて測距センサ５により反射点群が、ターゲット３に対する同時刻ｋでの少なくとも一つの観測点として認識される。
【0030】
挙動センサ群７としては、慣性計測ユニット（即ち、ＩＭＵ）やオドメトリなど、自車両の移動量等の挙動を検出できる各種のセンサを採用できる。この挙動センサ群７により検出される情報としては、例えば、アクセルペダルの操作量、ブレーキペダルの操作量、操舵角、車両速度、車両加速度などが挙げられる。
【0031】
追跡処理部９は、ターゲット３の追跡等の各種の処理を行う制御部１１と、各種のデータやプログラムを記憶する記憶部１３と、を備えたマイクロコンピュータを中心にして構成された電子制御装置である。
【0032】
詳しくは、制御部１１は、周知のプロセッサ（例えば、ＣＰＵ１１ａ）を備え、記憶部１３は、例えば、ＲＡＭ又はＲＯＭ等の半導体メモリ（以下、メモリ１３ａとする）を備える。
【0033】
制御部１１の機能は、非遷移的実体的記録媒体（即ち、メモリ１３ａ）に格納されたプログラムをＣＰＵ１１ａが実行することにより実現される。また、このプログラムが実行されることで、プログラムに対応する方法が実行される。
【0034】
なお、制御部１１の各種機能を実現する手法はソフトウェアに限るものではなく、その一部又は全部の要素について、一つあるいは複数のハードウェアを用いて実現してもよい。例えば、上記機能がハードウェアである電子回路によって実現される場合、その電子回路は多数の論理回路を含むデジタル回路、又はアナログ回路、あるいはこれらの組合せによって実現してもよい。
【0035】
＜追跡処理部の機能的な構成＞
ここで、追跡処理部９の機能的な構成について説明する。なお、制御部１１は、追跡のための演算処理を行う部分である。
ここで、追跡処理部９の機能的な構成について説明する。なお、制御部１１は、追跡のための演算処理を行う部分である。
【0036】
図２に示すように、追跡処理部９（詳しくは、制御部１１）は、機能的に、観測点抽出部３１と予測点生成部３３と位置合わせ部３５と残差計算部３７と状態更新部３９と状態予測部４１とを有する。
【0037】
追跡処理部９は、測距センサ５によって得られたターゲット３を示す観測点の情報を用いて、現サイクルにおけるターゲット３の状態を示す状態量（即ち、状態ベクトル）を推定するように構成されている。なお、状態量として、少なくともターゲット３の位置とターゲット３の形状とに関する状態量を用いることができる。
【0038】
観測点抽出部３１は、前サイクル（即ち、現サイクルの直前のサイクル）にて推定したターゲット３の状態量に基づいて、現サイクルのターゲット３から得られた観測点を抽出するように構成されている。
【0039】
予測点生成部３３は、前サイクルにて推定したターゲット３の状態量に基づいて、観測点が期待できる予測する位置に、少なくとも一つの予測点を生成するように構成されている。
【0040】
位置合わせ部３５は、現サイクルのターゲット３から得られた観測点と、予測点生成部３３で生成した予測点と、をスキャンマッチングにより位置合わせするように構成されている。
【0041】
残差計算部３７は、現サイクルから得られた観測点と位置合わせされた予測点から、観測点に対応する予測された位置における物体の対応点を求め、観測点と対応点との残差（即ち、残差ベクトル）を算出するように構成されている。
【0042】
状態更新部３９は、残差計算部３７にて算出した残差に基づいて、ターゲット３の状態量を更新するように構成されている。
状態予測部４１は、状態更新部３９で更新した状態量に基づいて、次サイクル（即ち、現サイクルの直後のサイクル）の状態量を予測するように構成されている。
状態予測部４１は、状態更新部３９で更新した状態量に基づいて、次サイクル（即ち、現サイクルの直後のサイクル）の状態量を予測するように構成されている。
【0043】
［１－２．追跡処理部による全体の処理］
次に、図３のフローチャートに基づいて、本第１実施形態の物体追跡装置１の全体的な処理の概略の手順を説明する。なお、各ステップの内容については、後に詳述する。
次に、図３のフローチャートに基づいて、本第１実施形態の物体追跡装置１の全体的な処理の概略の手順を説明する。なお、各ステップの内容については、後に詳述する。
【0044】
まず、図３のステップ（以下、Ｓ）１００では、直近の所定期間で得られた観測点に基づいて、ターゲット３の運動と形状の初期値を設定する。
続くＳ１１０では、現時刻におけるターゲット３の運動と形状を予測する。
続くＳ１１０では、現時刻におけるターゲット３の運動と形状を予測する。
【0045】
続くＳ１２０では、対応付いた観測点から、ターゲット３の輪郭（即ち、実際輪郭）上にあると期待される観測点を抽出する。
続くＳ１３０では、予測したターゲット３の形状の輪郭（即ち、予測輪郭）から期待される観測点（即ち、予測点）をサンプリングする。
続くＳ１３０では、予測したターゲット３の形状の輪郭（即ち、予測輪郭）から期待される観測点（即ち、予測点）をサンプリングする。
【0046】
続くＳ１４０では、予測点と観測点とを、スキャンマッチングによって位置合わせする。
続くＳ１５０では、観測点と対応する予測輪郭上の対応点を計算する。
続くＳ１５０では、観測点と対応する予測輪郭上の対応点を計算する。
【0047】
続くＳ１６０では、観測点と対応点とから残差（即ち、残差ベクトル）を求め、現時刻の状態量（即ち、状態ベクトル）の更新を行う。
その後、更新された状態量を用いて、前記Ｓ１１０に戻って、同様な処理を繰り返す。
その後、更新された状態量を用いて、前記Ｓ１１０に戻って、同様な処理を繰り返す。
【0048】
［１－３．追跡処理部による各処理の内容］
次に、図３の追跡処理部９における各処理（即ち、Ｓ１００～Ｓ１６０）の内容について、順を追って詳細に説明する。
次に、図３の追跡処理部９における各処理（即ち、Ｓ１００～Ｓ１６０）の内容について、順を追って詳細に説明する。
【0049】
（１）Ｓ１００の処理の内容
このステップでは、直近の所定期間で得られた観測点に基づいて、ターゲット３の運動とターゲット３の形状の初期値を設定する。
このステップでは、直近の所定期間で得られた観測点に基づいて、ターゲット３の運動とターゲット３の形状の初期値を設定する。
【0050】
ここで、運動には、ターゲット３の質点における位置や速度（即ち、速度の大きさや方向）を含み、形状には、ターゲット３の大きさ（即ち、ターゲット３の空間的な広がり）や向き（即ち、形状においてターゲット３がどちらを向いているかの向き）を含むとする。なお、ターゲット３の大きさは一定であるので、形状としては、ターゲット３の大きさ（例えば、横幅や奥行や高さなど）及び向きを考慮してもよい。なお、位置としては、所定の座標系（例えば、東西南北を規定した座標系や、自車両等の所定の車両を基準とした座標系など）における座標が挙げられる。
【0051】
図４に示すように、レーダー等の測距センサ５を用いて、例えば、１サイクル目におけるセンシングによって、同図左下のターゲット３における複数の観測点が得られ、２サイクル目のセンシングよって、同図右上のターゲット３における複数の観測点が得られたとする。なお、観測点とは、レーダー波等のターゲット３における反射点である。図４では、観測点を、内部に細かいメッシュを付した丸で示してある。なお、他の図面においても、同様な内部にメッシュを付した丸は観測点を示している。
【0052】
この場合、例えば、１サイクル目の複数の観測点の重心（即ち、重心１）から、１サイクル目のターゲット３の位置が分かり、２サイクル目の複数の観測点の重心（即ち、重心２）から、２サイクル目のターゲット３の位置が分かる。また、重心１から重心２への移動ベクトルから、ターゲット３の速度（即ち、ターゲット３の運動）が分かる。
【0053】
また、これとは別に、ターゲット３を上方から見た場合（即ち、平面視）のターゲット３の外周形状（即ち、ターゲット３の大きさを含む輪郭）が予め分かっている場合について説明する。例えば、同図に四角枠で示すように、ターゲット３の輪郭が長方形の場合には、前記複数の観測点をターゲット３の輪郭に合わせるように配置すること（即ち、フィッティングすること）により、ターゲット３の平面視での形状（即ち、大きさや向き）が分かる。即ち、同図の四角枠で示すように、ターゲットの平面視における形状が分かり、また、各ターゲット３の形状から各ターゲット３の重心が分かる。
【0054】
このように、ターゲット３の１サイクル目及び２サイクル目における重心の位置からターゲット３の位置が分かり、また、重心の移動ベクトルにより、ターゲット３の運動が分かる。
【0055】
従って、例えば、１サイクル目のターゲット３の重心を、ターゲット３の位置の初期値として設定でき、また、その移動ベクトルから得られる速度を、２サイクル目のターゲット３の速度の初期値として設定できる。
【0056】
なお、以下では、ターゲット３の平面形状として、例えば車両の外周形状（即ち、輪郭）に合わせた長方形を例に挙げて説明する。
（２）Ｓ１１０の処理の内容
このステップでは、現時刻におけるターゲット３の運動と形状を予測する。
（２）Ｓ１１０の処理の内容
このステップでは、現時刻におけるターゲット３の運動と形状を予測する。
【0057】
図５に示すように、１サイクル前の時刻（即ち、前時刻）におけるターゲット３の位置や速度が分かっている場合には、現サイクルにおけるターゲット３の位置や速度が予測可能である。なお、同図では、予測したターゲットの輪郭を破線で示す。
【0058】
つまり、前時刻におけるターゲット３の位置から前時刻におけるターゲット３の速度で移動したと予測することにより、予測したターゲット３の位置が分かる。なお、予測したターゲット３の位置における速度は、前時刻と速度であるとしてもよい。
【0059】
また、同図では、実際のターゲット３の移動状態を実線の矢印で示し、現時刻において実際に観測された観測点を大小の丸で示し、現時点の実際のターゲット３の輪郭（即ち、矢印の先の輪郭）を実線で示す。
【0060】
なお、実際に観測された複数の観測点のうち、ターゲット３の輪郭に位置する観測点を、ターゲット３の内部に位置する観測点よりも大きく示してある。なお、通常は、輪郭（即ち、外周）から多くの観測点が得られる。
【0061】
（３）Ｓ１２０の処理の内容
このステップでは、対応付いた観測点から、ターゲット３の輪郭上にあると期待される観測点を抽出する。
このステップでは、対応付いた観測点から、ターゲット３の輪郭上にあると期待される観測点を抽出する。
【0062】
図６に示すように、実際のターゲット３の位置と予測したターゲット３の位置がずれている場合があるので、通常は、実際のターゲット３の位置は、予測したターゲット３の位置の近傍、例えば、予測したターゲット３から所定の距離の範囲等の接続範囲内にあると推定する。
【0063】
そして、接続範囲内の複数の観測点（即ち、図６の大小の丸で表現される観測点）を、現時刻における実際のターゲット３の観測点として選択する。
更に、これらの観測点のうち、実際のターゲット３の輪郭（即ち、実際輪郭）上又は輪郭の近傍にあると期待される観測点（即ち、図６にて内部にメッシュを付した大きな丸で表現される観測点）を抽出する。つまり、ターゲット３の輪郭は長方形であるので、長方形の枠上又はその近傍にあると思われる観測点を抽出する。
更に、これらの観測点のうち、実際のターゲット３の輪郭（即ち、実際輪郭）上又は輪郭の近傍にあると期待される観測点（即ち、図６にて内部にメッシュを付した大きな丸で表現される観測点）を抽出する。つまり、ターゲット３の輪郭は長方形であるので、長方形の枠上又はその近傍にあると思われる観測点を抽出する。
【0064】
（４）Ｓ１３０の処理の内容
このステップでは、予測したターゲット３の輪郭（即ち、予測輪郭）から期待される観測点（即ち、予測点）をサンプリングする。
このステップでは、予測したターゲット３の輪郭（即ち、予測輪郭）から期待される観測点（即ち、予測点）をサンプリングする。
【0065】
図７に示すように、ここでは、予測した四角枠状のターゲット３の輪郭上において、所定に間隔毎に、予測点を設定する。図７では、予測点を内部に複数のドットを付した丸で示してある。なお、他の図面においても、同様なドットを付した丸は予測点を示している。
【0066】
（５）Ｓ１４０の処理の内容
このステップでは、予測輪郭の予測点と実際輪郭において抽出された観測点（即ち、実際輪郭に沿った観測点）とを、スキャンマッチングによって位置合わせする。
このステップでは、予測輪郭の予測点と実際輪郭において抽出された観測点（即ち、実際輪郭に沿った観測点）とを、スキャンマッチングによって位置合わせする。
【0067】
図８に示すように、公知のスキャンマッチングの手法により、予測点と観測点とを位置合わせすることにより、予測輪郭と実際輪郭とをできる限り重ね合わせるようにする。
例えば、図９に示すように、予測点と観測点とが１対１の対応関係となるように、予測点の並進移動（即ち、平行移動）や回転移動を行って、複数の観測点と複数の予測点とをできる限り近づけるような処理（即ち、幾何変換の推定と遷移の処理）を行う。
例えば、図９に示すように、予測点と観測点とが１対１の対応関係となるように、予測点の並進移動（即ち、平行移動）や回転移動を行って、複数の観測点と複数の予測点とをできる限り近づけるような処理（即ち、幾何変換の推定と遷移の処理）を行う。
【0068】
つまり、複数の予測点の配置による形状（例えば、Ｌ字形状）と複数の抽出された観測点の配置による形状（例えば、Ｌ字形状）とが、できる限り一致するように、例えば、複数の予測点の配置による形状を維持したままで、複数の予測点を移動させる。
【0069】
ここで、スキャンマッチングの基本的な内容について説明する。
スキャンマッチングの手法としては、例えば、３次元形状の位置合わせの基本的な手法として広く用いられているＩＣＰアルゴリズムを使うことができる。ＩＣＰアルゴリズムは、予測点群をソース点群、観測点をターゲット点群としたとき、最近傍点による対応付けの計算と、対応点からの幾何変換の推定の２つの処理を収束条件を満足するまで交互に繰り返すことで、最終的な位置合わせを行う手法である。
スキャンマッチングの手法としては、例えば、３次元形状の位置合わせの基本的な手法として広く用いられているＩＣＰアルゴリズムを使うことができる。ＩＣＰアルゴリズムは、予測点群をソース点群、観測点をターゲット点群としたとき、最近傍点による対応付けの計算と、対応点からの幾何変換の推定の２つの処理を収束条件を満足するまで交互に繰り返すことで、最終的な位置合わせを行う手法である。
【0070】
このスキャンマッチングの手法は、下記文献［１］、文献［２］に記載されている。なお、下記文献［１］は、ＩＣＰアルゴリズムの元論文を示しており、この文献［１］には、詳細なステップが記載されている。また、文献［２］はＩＣＰアルゴリズムの概要を説明したチュートリアルである。
文献［１］：Besl,Paul J. and Neil D.McKay.“A Method for Registration of 3-D Shapes.”IEEE Trans. Pattern Anal. Mach. Intell.14 (1992): 239-256.
文献［２］：増田健,“Icpアルゴリズム,”情報処理学会研究報告,vol.2009,no.2,pp.1-8,2009.
（６）Ｓ１５０の処理の内容
このステップでは、観測点と対応する予測輪郭上の対応点を計算して求める。
文献［１］：Besl,Paul J. and Neil D.McKay.“A Method for Registration of 3-D Shapes.”IEEE Trans. Pattern Anal. Mach. Intell.14 (1992): 239-256.
文献［２］：増田健,“Icpアルゴリズム,”情報処理学会研究報告,vol.2009,no.2,pp.1-8,2009.
（６）Ｓ１５０の処理の内容
このステップでは、観測点と対応する予測輪郭上の対応点を計算して求める。
【0071】
図１０に示すように、スキャンマッチングにされた観測点と予測点とに対して、例えば放射投影法によって、観測点と対応する予測輪郭上の予測点（即ち、対応点）を求める。
例えば、予測したターゲット３の質点の位置を放射の中心として、実際輪郭上の観測点に向けて直線を引き、その直線上にある又はその直線と最も近い位置にある観測点と予測点とを１対１に対応付ける。
例えば、予測したターゲット３の質点の位置を放射の中心として、実際輪郭上の観測点に向けて直線を引き、その直線上にある又はその直線と最も近い位置にある観測点と予測点とを１対１に対応付ける。
【0072】
なお、放射投影法については、例えば、下記文献［３］に記載のように公知技術である。
文献［３］：M.Michaelis,P.Berthold,T.Luettel,D.Meissner,and H.J.Wuensche,“Extended object tracking with an improved measurement-to-contour association,”Proc.2020 23rd Int.Conf.Inf.Fusion,FUSION 2020,no.July,2020,doi:10.23919/FUSION45008.2020.9190439.
この観測点に対応づけられた予測点が、図１１に示すように、予測したターゲット３の予測輪郭における対応点である。
文献［３］：M.Michaelis,P.Berthold,T.Luettel,D.Meissner,and H.J.Wuensche,“Extended object tracking with an improved measurement-to-contour association,”Proc.2020 23rd Int.Conf.Inf.Fusion,FUSION 2020,no.July,2020,doi:10.23919/FUSION45008.2020.9190439.
この観測点に対応づけられた予測点が、図１１に示すように、予測したターゲット３の予測輪郭における対応点である。
【0073】
（７）Ｓ１６０の処理の内容
このステップでは、図１１に示すように、観測点と対応点（即ち、観測点と対応した予測点）とから残差ベクトルを求め、現時刻のターゲット３の状態量の更新を行う。
このステップでは、図１１に示すように、観測点と対応点（即ち、観測点と対応した予測点）とから残差ベクトルを求め、現時刻のターゲット３の状態量の更新を行う。
【0074】
なお、ターゲット３の状態量は、周知のように、状態ベクトルとして示すことができる。
例えば、ターゲット３の状態量を、横方位位置ｘ、縦方位位置ｙ、横方向速度ｖｘ、縦方向速度ｖｙで表す場合には、これらの状態量を要素として、ベクトル表現した状態ベクトルは、Ｘ＝［ｘ、ｙ、ｖｘ、ｖｙ］Ｔで表すことができる。なお、例えば、横方向は車幅方向であり、縦方向は車幅方向に垂直な方向である。
例えば、ターゲット３の状態量を、横方位位置ｘ、縦方位位置ｙ、横方向速度ｖｘ、縦方向速度ｖｙで表す場合には、これらの状態量を要素として、ベクトル表現した状態ベクトルは、Ｘ＝［ｘ、ｙ、ｖｘ、ｖｙ］Ｔで表すことができる。なお、例えば、横方向は車幅方向であり、縦方向は車幅方向に垂直な方向である。
【0075】
ここで、残差ベクトルについて説明する。
図１２に示すように、行列で表現される残差（即ち、Δ）は、現時刻におけるターゲット３の状態量の予測値（即ち、予測した推定値）ｘｔと、観測点を示す値（即ち、観測値）Ｚとの差Δ＝Ｚ－Ｈ（ｘｔ）である。
図１２に示すように、行列で表現される残差（即ち、Δ）は、現時刻におけるターゲット３の状態量の予測値（即ち、予測した推定値）ｘｔと、観測点を示す値（即ち、観測値）Ｚとの差Δ＝Ｚ－Ｈ（ｘｔ）である。
【0076】
Ｈは、観測モデルである。残差δのｉ番目の要素が、残差ベクトルδｉになる。なお、予測値ｘｔのｘの上には図１２に示すようにチルダ「～」が付記されるが、ここでは省略して示している。
【0077】
例えば、予測点（群）をＰ、それに属するｉ番目の点をｐｉ、観測点（群）をＺ、それに属するｊ番目の点をｚｊとする。また、事前に求めた対応付けの情報から、ｐｉとｚｊとが対応づいたとする。このとき、残差ベクトルδｉは、δｉ＝ｚｊ－ｐｉ（即ち、点ｚｊと点ｐｉのもつ情報の差分：ベクトルｚｊとベクトルｐｉの差）として計算される。
【0078】
ここで、ｐｉやｚｊが持つ情報として、例えば点の位置情報（ｘ、ｙ）がある。なお、点の速度情報（ｖｘ、ｖｙ）などの他の次元が加えられても良い。
なお、下記文献［４］には、B-splineモデルにおける対応付け情報からの残差ベクトルを求める具体的な計算方法が記載されている。
なお、下記文献［４］には、B-splineモデルにおける対応付け情報からの残差ベクトルを求める具体的な計算方法が記載されている。
【0079】
文献［４］：H.Kaulbersch,J.Honer, and M.Baum,“A Cartesian B-Spline Vehicle Model for Extended Object Tracking,”2018 21st Int. Conf.Inf.Fusion,FUSION 2018,no.December 2019, pp.1771-1778,2018,doi:10.23919/ICIF.2018.8455717.
また、下記文献［５］には、Star-convexモデルにおける点の速度情報（Doppler Range Rate）を考慮した残差の定式化について記載がある。
また、下記文献［５］には、Star-convexモデルにおける点の速度情報（Doppler Range Rate）を考慮した残差の定式化について記載がある。
【0080】
文献［５］：K.Thormann,M.Baum, and J.Honer,“Extended Target Tracking Using Gaussian Processes with High-Resolution Automotive Radar,”2018 21st Int.Conf.Inf.Fusion,FUSION 2018,no.December 2019,pp.1764-1770,2018, doi:10.23919/ICIF.2018.8455630.
次に、現時刻の状態量の更新の手法について説明するが、公知技術（例えば、特開２０２１－４７３７号公報等参照）であるので、簡単に説明する。
次に、現時刻の状態量の更新の手法について説明するが、公知技術（例えば、特開２０２１－４７３７号公報等参照）であるので、簡単に説明する。
【0081】
図１３に示すように、現時刻におけるターゲット３の状態量の予測値ｘｔと、観測点の観測値Ｚから、残差Δ＝Ｚ－Ｈ（ｘｔ）を計算し、周知の非線形カルマンフィルタ（例えば、拡張カルマンフィルタやシグマポイントカルマンフィルタなど）を使い、物体の状態量を更新する。
【0082】
そして、現時刻の状態量を更新した後に、更新された状態量を用いて、前記ステップ１１０に戻って、同様な処理を繰り返す。つまり、更新された情報量を用いて、次サイクルのターゲット３の状態量を予測する。
【0083】
なお、次サイクルのターゲット３の状態量を予測する処理については公知である。例えば、次のサイクル（時刻ｔ）のターゲット３の状態量の予測値ｘｔは、前のサイクル（時刻ｔ－１）におけるターゲット３の状態量ｘ（ｔ－１）と、ターゲット３の状態遷移を表す運動モデル（下記式［数１］参照）と、を用いて計算することができる。
【0084】
【数1】
【0085】
［１－４．効果］
本第１実施形態によれば、以下の効果が得られる。
本第１実施形態では、追跡処理部９により、測距センサ５によって得られたターゲット３を示す観測点の情報を用いて、現サイクルにおけるターゲット３の状態を示す状態量（即ち、状態ベクトル）を推定する。観測点抽出部３１では、前サイクルにて推定したターゲット３の状態量に基づいて、現サイクルのターゲット３から得られた観測点を抽出する。予測点生成部３３では、前サイクルにて推定したターゲット３の状態量に基づいて、観測点が期待できる予測する位置に、少なくとも一つの予測点を生成する。位置合わせ部３５では、現サイクルのターゲット３から得られた観測点と、予測点生成部３３で生成した予測点と、をスキャンマッチングにより位置合わせする。残差計算部３７では、現サイクルから得られた観測点と位置合わせされた予測点から、観測点に対応する予測された位置におけるターゲット３の対応点を求め、観測点と対応点との残差（即ち、残差ベクトル）を算出する。状態更新部３９では、残差に基づいて、ターゲット３の状態量を更新する。状態予測部４１では、更新した状態量に基づいて、次サイクルの状態量を予測する。
本第１実施形態によれば、以下の効果が得られる。
本第１実施形態では、追跡処理部９により、測距センサ５によって得られたターゲット３を示す観測点の情報を用いて、現サイクルにおけるターゲット３の状態を示す状態量（即ち、状態ベクトル）を推定する。観測点抽出部３１では、前サイクルにて推定したターゲット３の状態量に基づいて、現サイクルのターゲット３から得られた観測点を抽出する。予測点生成部３３では、前サイクルにて推定したターゲット３の状態量に基づいて、観測点が期待できる予測する位置に、少なくとも一つの予測点を生成する。位置合わせ部３５では、現サイクルのターゲット３から得られた観測点と、予測点生成部３３で生成した予測点と、をスキャンマッチングにより位置合わせする。残差計算部３７では、現サイクルから得られた観測点と位置合わせされた予測点から、観測点に対応する予測された位置におけるターゲット３の対応点を求め、観測点と対応点との残差（即ち、残差ベクトル）を算出する。状態更新部３９では、残差に基づいて、ターゲット３の状態量を更新する。状態予測部４１では、更新した状態量に基づいて、次サイクルの状態量を予測する。
【0086】
このような構成により、本第１実施形態の物体追跡装置１では、ターゲット３を精度良く追跡することができるとともに、ターゲット３を追跡する際の演算の負荷を低減することができる。
【0087】
つまり、本第１実施形態では、従来のように、予測を複数振るのではなく、観測点と予測形状（即ち、予測点）とのズレ（即ち、対応関係）を計算することにより、効率的に計算ができる。従って、ターゲット３の追跡を行う際の演算の負荷を低減できるとともに、精度良くターゲット３を追跡することができる。
【0088】
［１－５．対応関係］
次に、本第１実施形態と本開示との関係について説明する。
物体追跡装置１は物体追跡装置に対応し、ターゲット３はターゲットに対応し、測距センサ５はセンサに対応し、追跡処理部９は追跡処理部に対応し、観測点抽出部３１は観測点抽出部に対応し、予測点生成部３３は予測点生成部に対応し、位置合わせ部３５は位置合わせ部に対応し、残差計算部３７は残差計算部に対応し、状態更新部３９は状態更新部に対応する。
次に、本第１実施形態と本開示との関係について説明する。
物体追跡装置１は物体追跡装置に対応し、ターゲット３はターゲットに対応し、測距センサ５はセンサに対応し、追跡処理部９は追跡処理部に対応し、観測点抽出部３１は観測点抽出部に対応し、予測点生成部３３は予測点生成部に対応し、位置合わせ部３５は位置合わせ部に対応し、残差計算部３７は残差計算部に対応し、状態更新部３９は状態更新部に対応する。
【0089】
［２．第２実施形態］
第２実施形態は、基本的な構成は第１実施形態と同様であるため、以下では主として第１実施形態との相違点について説明する。なお、第１実施形態と同じ符号は、同一構成を示すものであって、先行する説明を参照する。
第２実施形態は、基本的な構成は第１実施形態と同様であるため、以下では主として第１実施形態との相違点について説明する。なお、第１実施形態と同じ符号は、同一構成を示すものであって、先行する説明を参照する。
【0090】
本第２実施形態のハード構成は、第１実施形態と同じであるので、その説明は省略する。
本第２実施形態では、スキャンマッチングの手法に特徴があるので、スキャンマッチングの手法について説明する。
本第２実施形態では、スキャンマッチングの手法に特徴があるので、スキャンマッチングの手法について説明する。
【0091】
本第２実施形態では、スキャンマッチングの位置合わせの過程で、予測点生成部３３で生成した予測点のうち、センサ（例えば、測距センサ５）から観測できる点を逐次的に見直す処理を行う。以下、詳細に説明する。
【0092】
［２－１．処理の内容］
図１４Ａの左端の図に示すように、センシングにより実際輪郭に沿った観測点が得られ、また、予測輪郭（例えば、長方形の枠）に沿った予測点が算出された場合を考える。ここでは、長方形の枠に沿った全ての予測点（即ち、丸で示す予測点）のうち、センサから見て、例えばレーダー波の照射範囲において反射波が得られると考えられる範囲の予測点（即ち、内部に複数のドットを付した丸で示す予測点）を選択する。
図１４Ａの左端の図に示すように、センシングにより実際輪郭に沿った観測点が得られ、また、予測輪郭（例えば、長方形の枠）に沿った予測点が算出された場合を考える。ここでは、長方形の枠に沿った全ての予測点（即ち、丸で示す予測点）のうち、センサから見て、例えばレーダー波の照射範囲において反射波が得られると考えられる範囲の予測点（即ち、内部に複数のドットを付した丸で示す予測点）を選択する。
【0093】
ここで、図１４Ａの左端の図に示すように、実際輪郭に沿った観測点（即ち、内部に細かいメッシュを付した丸）と予測輪郭の予測点（即ち、内部に複数のドットを付した丸）とに対して、前記文献［３］に記載のスキャンマッチングによって、そのまま位置合わせを行うと、場合によっては、図１４Ｂに示すように、適切な位置合わせが行えないことがある。
【0094】
つまり、実際輪郭の観測点は、実際輪郭の右下側の２つの側面の観測点であるが、予測輪郭の予測点は、予測輪郭の左側の２つ側面の予測点であるので、これらの観測点と予測点とのスキャンマッチングを行っても、観測点と予測点とを正しく対応付けることができないことがある。
【0095】
そこで、本第２実施形態では、まず、図１４Ａの左から２番目の図に示すように、スキャンマッチングに用いる予測点を取り直す処理を行う。
具体的には、スキャンマッチングに用いる予測点として、センサから直接に検出できる扇状の範囲（例えば、レーダー波の照射範囲）の予測点を取り直す処理を行う。つまり、予測輪郭に沿った全ての予測点のうち、センサから観測できる点を逐次的に見直すようにする。
具体的には、スキャンマッチングに用いる予測点として、センサから直接に検出できる扇状の範囲（例えば、レーダー波の照射範囲）の予測点を取り直す処理を行う。つまり、予測輪郭に沿った全ての予測点のうち、センサから観測できる点を逐次的に見直すようにする。
【0096】
【0097】
また、本第２実施形態では、スキャンマッチングにおいて、誤ったターゲット３側面等に対応付くことを防ぐことができる。よって、適切な残差計算が可能となるため、追跡精度を向上できる。
【0098】
なお、ターゲット３の予測形状（即ち、予測輪郭）のうち、観測できる位置を対応付けの計算に採用すること自体は、前記文献［３］に記載があるが、本第２実施形態においては、スキャンマッチングの過程で、予測点を取りなおすこと（即ち、逐次的に見直すこと）に特徴がある。
【0099】
［３．第３実施形態］
第３実施形態は、基本的な構成は第１実施形態と同様であるため、以下では主として第１実施形態との相違点について説明する。なお、第１実施形態と同じ符号は、同一構成を示すものであって、先行する説明を参照する。
第３実施形態は、基本的な構成は第１実施形態と同様であるため、以下では主として第１実施形態との相違点について説明する。なお、第１実施形態と同じ符号は、同一構成を示すものであって、先行する説明を参照する。
【0100】
本第３実施形態のハード構成は、第１実施形態と同じであるので、その説明は省略する。
本第３実施形態は、観測点が少ない場合の処理を示している。
本第３実施形態は、観測点が少ない場合の処理を示している。
【0101】
［３－１．処理の内容１］
本第３実施形態の一つの例では、センサ（例えば、測距センサ５）から観測できる点（即ち、観測点群）の空間的な広がりを判断し、その広がりが一定以下の狭い範囲である場合には、スキャンマッチングを行わないようにする。なお、空間的な広がりとしては、平面視における広がりを採用してもよい。
本第３実施形態の一つの例では、センサ（例えば、測距センサ５）から観測できる点（即ち、観測点群）の空間的な広がりを判断し、その広がりが一定以下の狭い範囲である場合には、スキャンマッチングを行わないようにする。なお、空間的な広がりとしては、平面視における広がりを採用してもよい。
【0102】
つまり、図１５に示すように、観測点群の広がりが狭い場合には、観測点と予測点との対応付けが難しく、適切にスキャンマッチングが行えない可能性があるので、そのような場合には、スキャンマッチングを行わないようにする。
【0103】
これにより、不適切なスキャンマッチングに起因する物体の追跡性の低下を抑制できる。
［３－２．処理の内容２］
本第３実施形態の他の例では、センサから観測できる点（即ち、観測点群）の空間的な広がりを判断し、その広がりが一定以下の狭い範囲である場合には、状態量を更新する際に、観測点の情報である観測情報の重みを低下させる。
［３－２．処理の内容２］
本第３実施形態の他の例では、センサから観測できる点（即ち、観測点群）の空間的な広がりを判断し、その広がりが一定以下の狭い範囲である場合には、状態量を更新する際に、観測点の情報である観測情報の重みを低下させる。
【0104】
つまり、実際輪郭等の幾何学的な特徴（例えば、四角形の枠の形状）が得られないことを判断し、スキャンマッチングが破綻する可能性があることを想定し、そのような場合には、観測情報の重みを低下させる。
【0105】
以下、詳細に説明する。
観測ノイズが、共分散行列Ｒｔで表現でき、ゼロ平均の多変数正規分布に従うとする。観測が独立であるときは、下記式［数２］に示すように、共分散行列Ｒｔは対角行列として定義される。なお、ここで、観測ノイズとは、例えば、測距センサ５から得られる測距情報の誤差（例えば、距離の誤差やドップラー速度の誤差）のことを意味する。
観測ノイズが、共分散行列Ｒｔで表現でき、ゼロ平均の多変数正規分布に従うとする。観測が独立であるときは、下記式［数２］に示すように、共分散行列Ｒｔは対角行列として定義される。なお、ここで、観測ノイズとは、例えば、測距センサ５から得られる測距情報の誤差（例えば、距離の誤差やドップラー速度の誤差）のことを意味する。
【0106】
【数2】
【0107】
観測点が１点しかない場合は、空間的な広がりが無い（即ち、面積ゼロ）と判断できるため、共分散行列の成分に非常に大きな値（例えば、１０σ２）を設定する。２点以上の場合は、例えば点群に外接する矩形の面積を計算し、その面積に応じて、例えば図１６に示すような関係で共分散行列の成分を調整する。
【0108】
このように、幾何的な特徴が得られないことを判断し、スキャンマッチングが破綻するような場合に、観測ノイズを増大させ、相対的に観測情報の重みを低下させ、予測重視な推定を行うこと（即ち、予測点を用いた推定を重視すること）を行うことで、ロバスト性向上を図ることがでる。
【0109】
例えば、下記文献［６］（下記ＵＲＬ参照）の「非線形カルマンフィルタの基礎」の「命題２（非線形カルマンフィルタ）」に示される下記式［数３］を用いた処理を行うことにより、観測情報の重みを低下させ、予測重視な推定を行うことができる。
【0110】
文献［６］:https://www.jstage.jst.go.jp/article/sicejl/56/9/56_638/_pdf
【0111】
【数3】
【0112】
つまり、公知の前記［数３］に記載の式に、上述した共分散行列Ｒｔを用いて、拡張カルマンフィルタにおけるカルマンゲインＫを計算する。ここでは、共分散行列Ｒｔが分母項にくるため、共分散行列Ｒｔが大きいと、カルマンゲインＫが小さくなる。そして、カルマンゲインＫが小さくなることにより、観測重みが低下する。
【0113】
このように、観測点を用いた推定よりも、予測点を用いた推定を重視することで、そうでない場合に比べて、物体の追跡性を向上させることができる。
［４．他の実施形態］
以上、本開示の実施形態について説明したが、本開示は、上記実施形態に限定されることなく、種々の形態を採り得ることは言うまでもない。
［４．他の実施形態］
以上、本開示の実施形態について説明したが、本開示は、上記実施形態に限定されることなく、種々の形態を採り得ることは言うまでもない。
【0114】
（４ａ）本開示に記載の物体追跡装置は、車両等の自動運転システムや監視システムなどに適用することができる。
（４ｂ）本開示に記載の物体追跡装置は、コンピュータプログラムにより具体化された一つ乃至は複数の機能を実行するようにプログラムされたプロセッサおよびメモリを構成することによって提供された専用コンピュータにより、実現されてもよい。
（４ｂ）本開示に記載の物体追跡装置は、コンピュータプログラムにより具体化された一つ乃至は複数の機能を実行するようにプログラムされたプロセッサおよびメモリを構成することによって提供された専用コンピュータにより、実現されてもよい。
【0115】
あるいは、本開示に記載の物体追跡装置は、一つ以上の専用ハードウェア論理回路によってプロセッサを構成することによって提供された専用コンピュータにより、実現されてもよい。
【0116】
もしくは、本開示に記載の物体追跡装置は、一つ乃至は複数の機能を実行するようにプログラムされたプロセッサおよびメモリと一つ以上のハードウェア論理回路によって構成されたプロセッサとの組み合わせにより構成された一つ以上の専用コンピュータにより、実現されてもよい。
【0117】
また、コンピュータプログラムは、コンピュータにより実行されるインストラクションとして、コンピュータ読み取り可能な非遷移有形記録媒体に記憶されてもよい。物体追跡装置に含まれる各部の機能を実現する手法には、必ずしもソフトウェアが含まれている必要はなく、その全部の機能が、一つあるいは複数のハードウェアを用いて実現されてもよい。
【0118】
（４ｃ）上述した物体追跡装置の他、当該物体追跡装置のコンピュータを機能させるためのプログラム、このプログラムを記録した半導体メモリ等の非遷移有形記録媒体、物体
追跡方法など、種々の形態で本開示を実現することもできる。
追跡方法など、種々の形態で本開示を実現することもできる。
【0119】
（４ｄ）上記各実施形態における１つの構成要素が有する複数の機能を、複数の構成要素によって実現したり、１つの構成要素が有する１つの機能を、複数の構成要素によって実現したりしてもよい。また、複数の構成要素が有する複数の機能を、１つの構成要素によって実現したり、複数の構成要素によって実現される１つの機能を、１つの構成要素によって実現したりしてもよい。また、上記各実施形態の構成の一部を省略してもよい。また、上記各実施形態の構成の少なくとも一部を、他の実施形態の構成に対して付加または置換してもよい。
［本明細書が開示する技術思想］
［項目１］
センサ（５）により、周辺環境を所定のサイクルにてセンシングし、当該センシングによって得られた情報に基づいて、ターゲットである物体（３）を追跡する物体追跡装置（１）であって、
前記センサによって得られた前記物体を示す観測点の情報を用いて、現サイクルにおける前記物体の状態を示す状態量を推定するように構成された追跡処理部（９）を備え、
前記追跡処理部は、
前記物体の状態量として、少なくとも前記物体の位置と前記物体の形状とに関する前記状態量を用いるものであり、
前サイクルにて推定した前記物体の状態量に基づいて、前記現サイクルの前記物体から得られた前記観測点を抽出するように構成された観測点抽出部（３１）と、
前記前サイクルにて推定した前記物体の状態量に基づいて、前記観測点が期待できる予測する位置に、少なくとも一つの予測点を生成するように構成された予測点生成部（３３）と、
前記現サイクルの前記物体から得られた前記観測点と、前記予測点生成部で生成した前記予測点と、をスキャンマッチングにより位置合わせするように構成された位置合わせ部（３５）と、
前記現サイクルから得られた前記観測点と前記位置合わせされた前記予測点から、前記観測点に対応する前記予測された位置における前記物体の対応点を求め、前記観測点と前記対応点との残差を算出するように構成された残差計算部（３７）と、
前記残差計算部にて算出した残差に基づいて、前記物体の状態量を更新するように構成された状態更新部（３９）と、
を備えた、物体追跡装置。
［本明細書が開示する技術思想］
［項目１］
センサ（５）により、周辺環境を所定のサイクルにてセンシングし、当該センシングによって得られた情報に基づいて、ターゲットである物体（３）を追跡する物体追跡装置（１）であって、
前記センサによって得られた前記物体を示す観測点の情報を用いて、現サイクルにおける前記物体の状態を示す状態量を推定するように構成された追跡処理部（９）を備え、
前記追跡処理部は、
前記物体の状態量として、少なくとも前記物体の位置と前記物体の形状とに関する前記状態量を用いるものであり、
前サイクルにて推定した前記物体の状態量に基づいて、前記現サイクルの前記物体から得られた前記観測点を抽出するように構成された観測点抽出部（３１）と、
前記前サイクルにて推定した前記物体の状態量に基づいて、前記観測点が期待できる予測する位置に、少なくとも一つの予測点を生成するように構成された予測点生成部（３３）と、
前記現サイクルの前記物体から得られた前記観測点と、前記予測点生成部で生成した前記予測点と、をスキャンマッチングにより位置合わせするように構成された位置合わせ部（３５）と、
前記現サイクルから得られた前記観測点と前記位置合わせされた前記予測点から、前記観測点に対応する前記予測された位置における前記物体の対応点を求め、前記観測点と前記対応点との残差を算出するように構成された残差計算部（３７）と、
前記残差計算部にて算出した残差に基づいて、前記物体の状態量を更新するように構成された状態更新部（３９）と、
を備えた、物体追跡装置。
【0120】
［項目２］
項目１に記載の物体追跡装置であって、
前記位置合わせ部は、前記現サイクルの前記物体から得られた前記観測点から、前記物体の輪郭上又は前記輪郭の所定範囲内の近傍にある前記観測点を抽出し、前記抽出した前記観測点を前記スキャンマッチングに用いるように構成された、
物体追跡装置。
項目１に記載の物体追跡装置であって、
前記位置合わせ部は、前記現サイクルの前記物体から得られた前記観測点から、前記物体の輪郭上又は前記輪郭の所定範囲内の近傍にある前記観測点を抽出し、前記抽出した前記観測点を前記スキャンマッチングに用いるように構成された、
物体追跡装置。
【0121】
［項目３］
項目１または項目２に記載の物体追跡装置であって、
前記位置合わせ部は、前記スキャンマッチングの位置合わせの過程で、前記予測点生成部で生成した前記予測点のうち、前記センサにより観測できる前記予測点を選択するように見直しを行うように構成された、
物体追跡装置。
項目１または項目２に記載の物体追跡装置であって、
前記位置合わせ部は、前記スキャンマッチングの位置合わせの過程で、前記予測点生成部で生成した前記予測点のうち、前記センサにより観測できる前記予測点を選択するように見直しを行うように構成された、
物体追跡装置。
【0122】
［項目４］
項目１から項目３までのいずれか１項に記載の物体追跡装置であって、
前記位置合わせ部は、前記センサにより観測できる前記観測点の点群の空間的な広がりを判断し、前記空間的な広がりが所定範囲以下の場合には、前記スキャンマッチングを行わないように構成された、
物体追跡装置。
項目１から項目３までのいずれか１項に記載の物体追跡装置であって、
前記位置合わせ部は、前記センサにより観測できる前記観測点の点群の空間的な広がりを判断し、前記空間的な広がりが所定範囲以下の場合には、前記スキャンマッチングを行わないように構成された、
物体追跡装置。
【0123】
［項目５］
項目１から項目４までのいずれか１項に記載の物体追跡装置であって、
前記位置合わせ部は、前記センサにより観測できる前記観測点の点群の空間的な広がりを判断し、前記空間的な広がりが所定範囲以下の場合には、前記所定範囲以下でない場合に比べて、前記観測点を用いて前記状態量を更新する際に、前記観測点の重み付けを低減するように構成された、
物体追跡装置。
項目１から項目４までのいずれか１項に記載の物体追跡装置であって、
前記位置合わせ部は、前記センサにより観測できる前記観測点の点群の空間的な広がりを判断し、前記空間的な広がりが所定範囲以下の場合には、前記所定範囲以下でない場合に比べて、前記観測点を用いて前記状態量を更新する際に、前記観測点の重み付けを低減するように構成された、
物体追跡装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】