特許 6951728 物体追跡装置

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

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

(11)【特許番号】6951728

(24)【登録日】2021年9月29日

(45)【発行日】2021年10月20日

(54)【発明の名称】物体追跡装置

(51)【国際特許分類】

   G01S 13/66 20060101AFI20211011BHJP

   G01S 13/931 20200101ALI20211011BHJP

【ＦＩ】

   G01S13/66

   G01S13/931

【請求項の数】6

【全頁数】17

(21)【出願番号】特願2017-33(P2017-33)

(22)【出願日】2017年1月4日

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

(43)【公開日】2018年4月26日

【審査請求日】2019年12月26日

(31)【優先権主張番号】特願2016-203094(P2016-203094)

(32)【優先日】2016年10月14日

(33)【優先権主張国】JP

(73)【特許権者】

【識別番号】504160781

【氏名又は名称】国立大学法人金沢大学

(74)【代理人】

【識別番号】100098545

【弁理士】

【氏名又は名称】阿部 伸一

(74)【代理人】

【識別番号】100087745

【弁理士】

【氏名又は名称】清水 善廣

(74)【代理人】

【識別番号】100106611

【弁理士】

【氏名又は名称】辻田 幸史

(74)【代理人】

【識別番号】100189717

【弁理士】

【氏名又は名称】太田 貴章

(72)【発明者】

【氏名】菅沼 直樹

(72)【発明者】

【氏名】永野 聖巳

【審査官】 佐藤 宙子

(56)【参考文献】

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

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

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

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

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

【文献】 米国特許出願公開第２００４／００１２５１６（ＵＳ，Ａ１）

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

Ｇ０１Ｓ ７／００− ７／６４

Ｇ０１Ｓ １３／００−１７／９５

Ｇ０８Ｇ １／１６

Ｂ６０Ｒ ２１／００−２１／０１７

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

【請求項1】

自車両周辺の移動する物体の状態を時系列的に推定する物体追跡装置であって、
前記物体の観測情報を取得する観測情報取得手段と、
前記観測情報取得手段が取得した前記観測情報を用いて前記物体の追跡情報の作成と管理を行う追跡情報作成管理手段とを備え、
前記観測情報取得手段は、前記観測情報として前記自車両と前記物体との距離、角度又は相対速度を取得する複数の観測情報取得部からなり、
前記追跡情報作成管理手段は、
追跡情報作成部と、
追跡リスト部と、
追跡リスト共有部と、
対応付け部とを有し、
前記追跡情報作成部は、各前記観測情報取得部が取得したそれぞれの前記観測情報ごとに、前記物体に付与する識別用のＩＤと前記物体の次時刻における推定状態を含む前記追跡情報を作成し、
前記追跡リスト部は、前記追跡情報作成部が作成した前記追跡情報を前記観測情報取得部ごとの追跡リストに記録し、
前記追跡リスト共有部は、観測領域が隣接する一方の前記観測情報取得部と他方の前記観測情報取得部の前記追跡リストを比較し、一方の前記観測情報取得部の前記追跡リストに、他方の前記観測情報取得部の前記追跡リストに存在しない前記ＩＤが付与された前記追跡情報が存在する場合には、存在しない前記ＩＤが付与された前記追跡情報を他方の前記観測情報取得部の前記追跡リストに複製する共有処理を行い、
前記対応付け部は、前時刻における前記追跡情報と次時刻における前記観測情報を前記観測情報取得部ごとに対応付け、
前記追跡リスト部は、前記追跡リスト共有部によって他方の前記観測情報取得部の前記追跡リストに複製された前記追跡情報について、前記対応付け部が対応付けできた場合は、複製された前記追跡情報を更新して他方の前記観測情報取得部の前記追跡リストに保持することを特徴とする物体追跡装置。

【請求項2】

前記観測情報取得部が、前記自車両の前方及び後方に設置され、
前記後方に設置された前記観測情報取得部の数よりも、前記前方に設置された前記観測情報取得部の数のほうが多いことを特徴とする請求項１に記載の物体追跡装置。

【請求項3】

前記観測情報取得部が、前記自車両の前方、後方及び側方に設置されていることを特徴とする請求項１に記載の物体追跡装置。

【請求項4】

前記追跡情報作成管理手段は、前記物体が移動物であるか否かを判断する移動物判断部を備え、
前記追跡リスト共有部は、前記移動物判断部によって移動物であると判断された前記物体の前記追跡情報について前記共有処理を行うことを特徴とする請求項１から請求項３のいずれか１項に記載の物体追跡装置。

【請求項5】

前記追跡リスト部は、前記対応付け部が対応付けできなかった場合は、複製された前記追跡情報を他方の前記観測情報取得部の前記追跡リストから削除することを特徴とする請求項１から請求項４のいずれか１項に記載の物体追跡装置。

【請求項6】

前記追跡情報作成管理手段は、複数の前記追跡リストに同じ前記ＩＤが付与された前記追跡情報が存在する場合には、その中から一つの前記追跡情報を選択する追跡情報選択部と、前記追跡情報選択部が選択した前記追跡情報を、前記自車両を制御又は監視する制御監視手段に前記追跡情報を送信する追跡情報送信部とを備え、
前記追跡情報選択部は、前記物体が現実世界に存在する事後確率が最も大きい前記追跡情報を選択することを特徴とする請求項１から請求項５のいずれか１項に記載の物体追跡装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自車両周辺に位置する先行車両などの移動物体を追跡する物体追跡装置に関する。

【背景技術】

【0002】

近年、交通事故の削減や交通渋滞の緩和、誰もが利用できる移動手段の提供を目的として自動運転自動車の開発が盛んに行われている。そのため自動ブレーキやAdaptive Cruise Control（ACC）など、先行車両を追跡して自動車の加速・操舵・制動のいずれかの操作を自動で行うシステムが次々に実現されている。この先行車両の追跡には車両前方に取り付けられたセンサが用いられる。
例えば特許文献１には、車両前方にレーダ装置を搭載し、走行車両からの反射信号を利用して、複数ある受信チャンネル間の利得差と位相差の初期状態からの変化を監視して異常判断を行うことが開示されている。
また、特許文献２には、車両前方にミリ波レーダ装置を搭載し、検知領域を３種類の領域に分割し、３種類の領域のうち被測定物が存在する領域がいずれかに応じて、ミリ波レーダ装置のトラッカフィルタのフィルタゲインを可変設定することが開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１１−１２７９１０号公報

【特許文献2】特開２００１−２４２２４２号公報

【発明の概要】

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

【0004】

しかし、車両前方に取り付けられたセンサだけでは、交差点や曲率の大きなカーブを曲がる先行車両などを継続して追跡することが難しく、衝突回避のための判断が遅れる可能性がある。
そこで、例えば複数のセンサにより構成される全方位センサシステムを車両に搭載し、先行車両や後続車両、交差車両の継続した追跡を目指すことが考えられる。ここで「追跡」は、時系列的な状態（位置・速度・加速度・角度・ヨーレート等）推定を意味する。そのため継続した追跡を実現すれば、先行車両などの未来の軌道を予測することで継続して衝突の危険を察知できるため、より多くのシーンにおいて安全運転の支援が可能となる。
しかし、全方位センサから得られる観測情報は膨大であるため、全ての観測情報を用いて追跡処理を行うとリアルタイム処理が難しい。

【0005】

そこで本発明は、先行車両等の移動物体の距離、角度又は相対速度を時系列的に計測する観測情報取得部（センサ）を複数備え、継続したリアルタイムな追跡を行う物体追跡装置を提供することを目的とする。

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

【0006】

請求項１記載の本発明の物体追跡装置は、自車両周辺の移動する物体の状態を時系列的に推定する物体追跡装置であって、前記物体の観測情報を取得する観測情報取得手段と、前記観測情報取得手段が取得した前記観測情報を用いて前記物体の追跡情報の作成と管理を行う追跡情報作成管理手段とを備え、前記観測情報取得手段は、前記観測情報として前記自車両と前記物体との距離、角度又は相対速度を取得する複数の観測情報取得部からなり、前記追跡情報作成管理手段は、追跡情報作成部と、追跡リスト部と、追跡リスト共有部と、対応付け部とを有し、前記追跡情報作成部は、各前記観測情報取得部が取得したそれぞれの前記観測情報ごとに、前記物体に付与する識別用のＩＤと前記物体の次時刻における推定状態を含む前記追跡情報を作成し、前記追跡リスト部は、前記追跡情報作成部が作成した前記追跡情報を前記観測情報取得部ごとの追跡リストに記録し、前記追跡リスト共有部は、観測領域が隣接する一方の前記観測情報取得部と他方の前記観測情報取得部の前記追跡リストを比較し、一方の前記観測情報取得部の前記追跡リストに、他方の前記観測情報取得部の前記追跡リストに存在しない前記ＩＤが付与された前記追跡情報が存在する場合には、存在しない前記ＩＤが付与された前記追跡情報を他方の前記観測情報取得部の前記追跡リストに複製する共有処理を行い、前記対応付け部は、前時刻における前記追跡情報と次時刻における前記観測情報を前記観測情報取得部ごとに対応付け、前記追跡リスト部は、前記追跡リスト共有部によって他方の前記観測情報取得部の前記追跡リストに複製された前記追跡情報について、前記対応付け部が対応付けできた場合は、複製された前記追跡情報を更新して他方の前記観測情報取得部の前記追跡リストに保持することを特徴とする。
請求項２記載の本発明は、請求項１に記載の物体追跡装置において、前記観測情報取得部が、前記自車両の前方及び後方に設置され、前記後方に設置された前記観測情報取得部の数よりも、前記前方に設置された前記観測情報取得部の数のほうが多いことを特徴とする。
請求項３記載の本発明は、請求項１に記載の物体追跡装置において、前記観測情報取得部が、前記自車両の前方、後方及び側方に設置されていることを特徴とする。
請求項４記載の本発明は、請求項１から請求項３のいずれか１項に記載の物体追跡装置において、前記追跡情報作成管理手段は、前記物体が移動物であるか否かを判断する移動物判断部を備え、前記追跡リスト共有部は、前記移動物判断部によって移動物であると判断された前記物体の前記追跡情報について前記共有処理を行うことを特徴とする。
請求項５記載の本発明は、請求項１から請求項４のいずれか１項に記載の物体追跡装置において、前記追跡リスト部は、前記対応付け部が対応付けできなかった場合は、複製された前記追跡情報を他方の前記観測情報取得部の前記追跡リストから削除することを特徴とする。
請求項６記載の本発明は、請求項１から請求項５のいずれか１項に記載の物体追跡装置において、前記追跡情報作成管理手段は、複数の前記追跡リストに同じ前記ＩＤが付与された前記追跡情報が存在する場合には、その中から一つの前記追跡情報を選択する追跡情報選択部と、前記追跡情報選択部が選択した前記追跡情報を、前記自車両を制御又は監視する制御監視手段に前記追跡情報を送信する追跡情報送信部とを備え、前記追跡情報選択部は、前記物体が現実世界に存在する事後確率が最も大きい前記追跡情報を選択することを特徴とする。

【発明の効果】

【0007】

本発明によれば、先行車両等の移動物体の距離、角度又は相対速度を時系列的に計測する観測情報取得部（センサ）を複数備え、継続したリアルタイムな追跡を行う物体追跡装置を提供できる。

【図面の簡単な説明】

【0008】

【図1】本発明の一実施例による物体追跡装置が搭載される自車両を示す図

【図2】自車両に観測情報取得手段と追跡情報作成管理手段と制御監視手段を搭載した状態を示す概略構成図

【図3】コスト行列処理とその他の処理に要する時間及びコスト行列の大きさ（要素数）を示す図

【図4】追跡情報の共有による処理時間の削減結果を示す図

【図5】追跡情報の共有処理の概念図

【図6】共有処理のフローチャート図

【図7】実験における先行車両の走行経路を示す図

【図8】実験における先行車両の追跡経路を示す図

【図9】各フレームの処理時間と観測情報数の関係を示す図

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

【0009】

本発明の第１の実施の形態による物体追跡装置は、物体の観測情報を取得する観測情報取得手段と、観測情報取得手段が取得した観測情報を用いて物体の追跡情報の作成と管理を行う追跡情報作成管理手段とを備え、観測情報取得手段は、観測情報として自車両と物体との距離、角度又は相対速度を取得する複数の観測情報取得部からなり、追跡情報作成管理手段は、追跡情報作成部と、追跡リスト部と、追跡リスト共有部と、対応付け部とを有し、追跡情報作成部は、各観測情報取得部が取得したそれぞれの観測情報ごとに、物体に付与する識別用のＩＤと物体の次時刻における推定状態を含む追跡情報を作成し、追跡リスト部は、追跡情報作成部が作成した追跡情報を観測情報取得部ごとの追跡リストに記録し、追跡リスト共有部は、観測領域が隣接する一方の観測情報取得部と他方の観測情報取得部の追跡リストを比較し、一方の観測情報取得部の追跡リストに、他方の観測情報取得部の追跡リストに存在しないＩＤが付与された追跡情報が存在する場合には、存在しないＩＤが付与された追跡情報を他方の観測情報取得部の追跡リストに複製する共有処理を行い、対応付け部は、前時刻における追跡情報と次時刻における観測情報を観測情報取得部ごとに対応付け、追跡リスト部は、追跡リスト共有部によって他方の観測情報取得部の追跡リストに複製された追跡情報について、対応付け部が対応付けできた場合は、複製された追跡情報を更新して他方の観測情報取得部の追跡リストに保持するものである。本実施の形態によれば、複数の観測情報取得部で周辺の物体を観測するため、観測範囲を大きくできる。また、観測情報取得手段で取得したすべての観測情報を用いて追跡処理を行うのではなく、各観測情報取得部で取得した観測情報のそれぞれに対して追跡処理を行うため、追跡処理にかかる時間を短くできリアルタイム処理が可能となる。さらに、観測領域が隣接する観測情報手段の追跡リスト同士で追跡情報を共有することにより先行車両を見失うことなく継続して追跡できる。
本発明の第２の実施の形態は、第１の実施の形態による物体追跡装置において、観測情報取得部が、自車両の前方及び後方に設置され、後方に設置された観測情報取得部の数よりも、前方に設置された観測情報取得部の数のほうが多いものである。本実施の形態によれば、特に先行車両を追跡する場合は、観測すべき物体が自車両の側方又は後方よりも前方に位置することが多いため、車両前方側の観測領域を大きくすることで、先行車両を見失うことなく継続して追跡できる。
本発明の第３の実施の形態は、第１の実施の形態による物体追跡装置において、観測情報取得部が、自車両の前方、後方及び側方に設置されているものである。本実施の形態によれば、自車両周辺を全方位（３６０°）にわたって観測できるため、観測領域に死角がない。
本発明の第４の実施の形態は、第１から第３のいずれか１つの実施の形態による物体追跡装置において、追跡情報作成管理手段は、物体が移動物であるか否かを判断する移動物判断部を備え、追跡リスト共有部は、移動物判断部によって移動物であると判断された物体の追跡情報について共有処理を行うものである。本実施の形態によれば、何らかの物体ではあるが静止物、又は移動物か静止物か不明の物体については共有処理を行わないことで、処理時間を短縮できる。
本発明の第５の実施の形態は、第１から第４のいずれか１つの実施の形態による物体追跡装置において、追跡リスト部は、対応付け部が対応付けできなかった場合は、複製された追跡情報を他方の観測情報取得部の追跡リストから削除するものである。本実施の形態によれば、不要な情報を削除することで、処理時間を短縮できる。
本発明の第６の実施の形態は、第１から第５のいずれか１つの実施の形態による物体追跡装置において、追跡情報作成管理手段は、複数の追跡リストに同じＩＤが付与された追跡情報が存在する場合には、その中から一つの追跡情報を選択する追跡情報選択部と、追跡情報選択部が選択した追跡情報を、自車両を制御又は監視する制御監視手段に追跡情報を送信する追跡情報送信部とを備え、追跡情報選択部は、物体が現実世界に存在する事後確率が最も大きい追跡情報を選択するものである。本実施の形態によれば、物体を精度良く追跡できる。

【実施例】

【0010】

以下、本発明の一実施例による物体追跡装置について説明する。本実施例による物体追跡装置は、自車両周辺に存在する移動物体の位置・速度・加速度・角度・ヨーレート等といった状態を時系列的に推定する。
図１は本実施例による物体追跡装置が搭載される自車両を示す図、図２は同自車両に観測情報取得手段と追跡情報作成管理手段と制御監視手段を搭載した状態を示す概略構成図である。
自車両１０の上方にアルミフレームを設置して各種センサを取付けている。センサの一つとしてＧＮＳＳ複合航法システム２１が搭載されており、ＧＮＳＳ（全地球航法衛星システム）から情報が十分に得られる環境において、１００［Ｈｚ］で自車両１０の３次元位置及び姿勢が計測可能である。さらに、他のセンサである全方位ＬＩＤＡＲ２２から得られる赤外線反射率を利用した自車両１０の自己位置推定も導入している（菅沼直樹,林悠太郎,永田大記,高橋謙太,“高齢過疎地域における自動運転自動車の市街地公道実証実験概要”,自動車技術会学術講演会 講演予稿集,No.14-15,pp.390-394,2015、山本大貴，菅沼直樹，“高解像度赤外線反射率画像を用いた自動運転自動車の自己位置推定”，第23回日本機械学会交通・物流部門大会講演論文集,pp.320-330,2014．）。この自己位置推定を用いることにより、ＧＮＳＳから情報が十分に得られない環境においても自車両１０の高精度な位置情報を取得することが可能である。
また、図２に示すように、さらに他のセンサとして、複数のセンサ（観測情報取得部）３１〜３９を自車両１０の前バンパー及び後バンパーの内部に設置することにより構成される略全方位センサシステム（観測情報取得手段）３０が取り付けられている。センサ３１〜３９としては、それぞれの観測領域（検知範囲）において先行車両等の物体までの距離・角度・相対速度を取得する、ＬＩＤＡＲ、超音波センサ、ミリ波レーダ、又はステレオカメラを用いることが望ましい。本実施例においては、センサ３１〜３９にミリ波レーダを用いることで、略全方位センサシステム（観測情報取得手段）３０として略全方位ミリ波レーダシステムを構成している。略全方位ミリ波レーダシステム３０は、自車両１０の前バンパーの内部に設置された第１のミリ波レーダ３１、第２のミリ波レーダ３２、第３のミリ波レーダ３３、第４のミリ波レーダ３４、第７のミリ波レーダ３７、第８のミリ波レーダ３８及び第９のミリ波レーダ３９と、車両１０の後バンパーの内部に設置された第５のミリ波レーダ３５及び第６のミリ波レーダ３６とからなる。図２において、１〜９の数字が付与された扇形は、各センサ（ミリ波レーダ）３１〜３９の観測領域を概略的に示している。
略全方位ミリ波レーダシステム３０は、観測周期２０［Ｈｚ］で周辺を観測する。また、１つのミリ波レーダ３１〜３９で最大５０個の観測情報（物体までの距離・角度・相対速度）を取得する、すなわち全体で最大４５０個の観測情報を取得する。特に物体が先行車両の場合は、観測すべき物体が自車両１０の側方又は後方よりも前方に位置することが多いため、自車両１０の前方の観測領域を大きくすることで、物体を見失うことなく継続して追跡できる。
なお、先行車両等の物体に対する観測領域をさらに大きくするため、ミリ波レーダを自車両１０の前方、後方及び側方に設置し、全方位ミリ波レーダシステムとして構成することが好ましい。この場合は、自車両１０の周辺を全方位（３６０°）にわたって観測できるため、観測領域に死角がない。

【0011】

本実施例による物体追跡装置は、図２に示すように、追跡情報作成管理手段４０と制御監視手段５０を備える。
追跡情報作成管理手段４０は、追跡情報作成部４１、追跡リスト部４２、リスト共有部４３、移動物判断部４４、対応付け部４５、追跡情報選択部４６及び追跡情報送信部４７を有し、略全方位センサシステム３０が取得した観測情報を用いて物体の追跡情報の作成と管理を行う。制御監視手段５０は、追跡情報作成管理手段４０が作成及び管理する追跡情報に基づいて自車両１０の制御及び監視を行う。

【0012】

ここで、追跡情報作成管理手段４０の追跡情報作成部４１における移動物体の状態推定手法の例について説明する。状態推定手法の１つであるカルマンフィルタは、略全方位センサシステム３０により得られる観測情報と１つのモデルを用いて推定対象である物体の状態を逐次的に推定する。そのため、そのモデルに対応しない運動が行われた際の推定精度は大きく劣化する。そこで、複数のモデルを用いて物体の状態を推定するInteracting Multiple Model(ＩＭＭ）法を用いる（R.Helmick,“IMM Estimator with Nearest-Neighbor Joint Probabilistic Data Association”, Yaakov Bar-Shalom and William Dale Blair Edit “Multitarget-Multisensor Tracking: Applications and Advances Volume III” Artech House Publishers, pp.161 - 198．）。
本実施例では等速度モデル、等加速度モデル及び停止モデルの３つのモデルを用いる。等速度モデルでは速度がほぼ一定となる定常移動中は精度よく状態推定を行えるが、加速度がほぼ一定となる発進時や減速時などでは誤差が大きくなる。一方で、等加速度モデルでは加速度を推定する分ノイズが発生しやすく、速度がほぼ一定となる定常移動中は等速度モデルほど精度よく状態推定が行えないが、加速度がほぼ一定となる発進時や減速時などでは精度よく状態推定が行える。また停止モデルを用いることで、略全方位センサシステム３０のノイズにより停止物体が急に動き出す物体と誤推定されることを緩和することができる。ＩＭＭ法では各モデルの尤もらしさを求め、それにより重み付けを行うことで各モデルの誤差を打ち消し合う。そのため、カルマンフィルタでは推定の難しいような複雑な運動をする物体の状態推定には有効とされる。本実施例で用いるモデルの状態変数ｘと入力ｗを式（１）、（２）に示す。

【数1】

【数2】

ここで、式（１）においてｐは絶対座標の位置を示し、ｖ，ａは絶対座標系の軸方向の速度・加速度を示し、上付き文字は絶対座標系のどちらの軸成分かを示す。また、式（２）において上付き文字がどの変数のノイズかを示し、「・」は時間微分を示す。そして、式（１）、（２）を用いた等加速度モデルの状態方程式ｘ_ｔを式（３）に示す。

【数3】

また、等速度モデルの状態方程式は式（３）において加速度が０であり、停止モデルの状態方程式は式（３）において速度と加速度が０である。

【0013】

次に、追跡情報作成管理手段４０の対応付け部４５における割り当て問題の解決手法の例について説明する。ＩＭＭ法ではモデルにより更新された既存追跡物体の状態と略全方位センサシステム３０から得られた観測情報を対応付けることにより状態が更新される。そのため、複数の物体が存在する複雑な環境下において既存追跡物体に対して正しく観測情報を対応付ける必要がある（割り当て問題）。
また、観測情報には既存追跡物体から得られるものだけでなく略全方位センサシステム３０の誤動作やクラッタにより得られるものが存在するため、正しい対応付けを行うための研究が行われている。本実施例ではGlobal Nearest Neighbor（ＧＮＮ）を用いて対応付けを行う（Pavlina Konstantinova & Alexander Udvarev & Tzvetan Semerdjiev, “A Study of a Target Tracking Algorithm Using Global Nearest Neighbor Approach”, International Conference on Computer Systems and Technologies−CompSysTech’2003.）。「ＧＮＮ」とは、全ての対応付けを考慮したコスト行列を生成し、その時刻で最も可能性の高い割り当てを行う手法である。ｍ個の既存追跡物体とｌ個の観測情報を得たとき、大きさはｍ×ｌのコスト行列ｃが式（４）のように生成される。

【数4】

このため、コスト行列は既存追跡物体数と観測情報の数によって大きさを変える。そして、式（４）の各要素にはマハラノビス距離を用いる。モデルにより更新された既存追跡物体ｉの状態と略全方位センサシステム３０から得られたｊ番目の観測情報ｚ^ｊとの残差ベクトルとその共分散ｓ^ｉｊを用いてマハラノビス距離ｄ_ｉｊは式（５）で表される。

【数5】

また閾値定数χ^２（１−α）を定義し、次式（６）を満たすときχ^２検定を満たす。

【数6】

ここで、αは有意水準でありこれを定めることでχ^２（１−α）を定義することができる。χ^２検定を満たしたとき対応付け候補としてコストｃ_ｉｊにはマハラノビス距離が代入される。

【数7】

そして満たさないときには対応付けの可能性のない組み合わせとしてコストが与えられる。

【数8】

生成したコスト行列に対してＭｕｎｋｒｅｓアルゴリズム（Francois Bourgeois, Jean-Claude Lassalle, “An Extension of the Munkres Algorithm for the Assignment Problem to Rectangular Matrices”, Communications of the ACM, Vol.14, No.12, pp.804-806, 1971）を用いることにより対応付けが決定される。１つの観測情報は１つの既存追跡物体にのみ割り当てられる。

【0014】

次に、追跡情報作成管理手段４０における追跡管理に用いるフィルタの例について説明する。略全方位センサシステム３０の誤動作やクラッタにより、既存追跡物体から観測情報が得られなかったり不要に得られたりすることが考えられる。そのため既存追跡物体に対して割り当てが無い可能性や誤った追跡が開始される可能性を考慮する必要がある。そこで本実施例では、Binary Bayes Filterを用いて物体が本当に現実世界に存在する物体であるかを判断する。時刻ｔに事象ｘが発生する確率Ｐ（ｘ_ｔ）としたとき、対数オッズlog-odds(x_t)は次式（９）で表現され、

【数9】

前時刻までの対数オッズ表現の事後確率log-odds(x_1:t-1)と足し合わせることで現在の対数オッズ表現の事後確率log-odds(x_1:t)が求まる。

【数10】

このようにして求めた事後確率を基にフィルタリングを行う手法をBinary Bayes Filter という。また、log-odds(x_1:t)は式（１１）により事後確率Ｐ(x_1:t)に復元できる。

【数11】

本実施例ではＰ(x_t)を物体が存在する確率としてマハラノビス距離ｄ_ｉｊに基づいて計算する。計算式を式（１２）に示す。

【数12】

ここで、Ｐ_max＝０．８と設定している。そして、このＰ（ｘ_ｔ）を用いて物体が存在する事後確率（Track Score）Ｐ_TSを求める。またこれに加えていくつかの条件により追跡開始・継続・終了と移動物体であるかの判断を行う。この条件については後述する。

【0015】

次に、追跡情報作成管理手段４０の移動物判断部４４における追跡管理について説明する。本実施例では追跡開始・継続・終了と移動物体であるかの判断を行うため、Confirmed・Tentative・Deleteの３つの追跡モードを用いる。
Confirmedモードは、その追跡物体が移動物体であると判断されたときに遷移し、追跡を継続するモードである。Tentativeモードは、その観測情報の値が何かしらの物体から得られたものとして識別用のＩＤを与えて追跡を開始したり、何かしらの物体ではあるが移動物体では無いとして追跡を継続したりするモードである。Deleteモードは、追跡物体が観測可能な範囲から外れた場合やノイズにより発生したものであると判断された場合に遷移し、追跡を終了するモードである。各モードの遷移条件を表１に示す。ここで、表１のＰ_cnf，Ｐ_stop，Ｄ_move，Ｐ_dlt，Ｎ_dltは任意のパラメータである。

【表1】

表１に示すように、TentativeモードからConfirmedモードへは、事後確率（Track Score）Ｐ_TSがＰ_Cnfよりも大きいとき、ＩＭＭ法において求められた停止モデルのモデル確率がＰ_stopよりも小さいとき、及び全方位センサシステム３０による初期観測位置からの移動距離がＤ_moveよりも大きいときに遷移する。また、TentativeモードからDeleteモード又はConfirmedモードからDeleteモードへは、事後確率（Track Score）Ｐ_TSがＰ_Dltよりも小さいとき、又は追跡物体と観測情報との対応付けが無いフレーム連続数がＮ_Dltよりも大きいときに遷移する。

【0016】

次に、略全方位ミリ波レーダシステム３０による物体追跡における処理時間の問題について説明する。複数のミリ波レーダ３１〜３９から構成される略全方位ミリ波レーダシステム３０は周辺から膨大な観測情報を取得する。この全ての観測情報を用いて追跡処理を行うと既存追跡物体と観測情報を対応付けるためのコスト行列は巨大化し、リアルタイムな追跡処理が困難となる。例えば、９個のミリ波レーダ３１〜３９を用いて略全方位ミリ波レーダシステム３０を構成し、金沢大学構内において追跡実験を行った場合、取得した最大観測情報数は１８０個であった。取得した観測情報全てを用いて追跡処理を行った場合、観測周期５０［ｍｓ］に対して最大処理時間は２７１［ｍｓ］という結果が得られ、リアルタイム処理が不可能であった。
そこで全ての観測情報を用いるのではなく、各ミリ波レーダ３１〜３９の観測情報を用いてそれぞれで追跡処理を行い、追跡情報を共有した。この結果、最大処理時間を９．２８［ｍｓ］まで抑えることができ、リアルタイム処理が可能となった。全ての観測情報を用いて追跡処理を行ったときのコスト行列から対応付けを見つける処理にかかった時間とその他の処理に要する時間およびコスト行列のサイズ（要素数）を図３に示す。図３において、最も濃い線が「コスト行列のサイズ（要素数）」、最も薄い線が「コスト行列の処理に要する時間（コスト行列から対応付けを見つける処理にかかった時間）」、その中間の濃淡線が「その他の処理に要する時間」を示す。また、全ての観測情報を用いて追跡処理を行った場合（全体処理）と各ミリ波レーダ３１〜３９の観測情報を用いてそれぞれで追跡処理を行い追跡情報を共有する場合（個別処理＋追跡情報共有処理）の処理時間を図４に示す。図４において、濃い線が本実施例による「個別処理＋追跡情報共有処理」、薄い線が従来の「全体処理」を示す。ここで、処理はクロックが「２．８ＧＨｚ」、実装メモリが「４ＧＢ」のＣＰＵで行った。共有処理については後述する。

【0017】

次に、追跡情報作成管理手段４０における追跡情報の共有処理について説明する。略全方位ミリ波レーダシステム３０を構成する各ミリ波レーダ３１〜３９の観測情報を用いてそれぞれで追跡処理を行うと、リアルタイム処理は可能となるが、観測するミリ波レーダ３１〜３９が切り替わる領域で物体をロストしてしまう。そのため、追跡情報の共有処理が必要となる。そこで、以下の３つの仮定を置いて追跡情報の共有を行う。
１）周辺車両（物体）は少なくとも１つのミリ波レーダ３１〜３９で複数回観測されConfirmedモードで追跡される。
２）隣接するミリ波レーダ３１〜３９の観測可能領域は一部重複している。
３）あるミリ波レーダ３１〜３９の観測情報を用いて追跡の開始・継続された物体は次の時刻にそのミリ波レーダ３１〜３９か隣接するミリ波レーダ３１〜３９で観測される。
これらの仮定より、Confirmedモードで追跡された物体の追跡情報を、隣接するミリ波レーダ３１〜３９の観測情報を用いた追跡リストに共有する。なお、Tentativeモードで追跡された物体の追跡情報は隣接するミリ波レーダ３１〜３９同士で共有しないこと、すなわち、観測情報を取得した物体ではあるが静止物、又は移動物か静止物か不明の物体については共有処理を行わないことで、処理時間を短縮できる。但し、Tentativeモードの段階から物体の追跡情報を共有して追跡する場合もあり得る。

【0018】

図５は、追跡情報の共有処理のイメージを示す概念図である。図５において、自車両（実験車両（Experiment vehicle））１０の周囲には物体として先行車両（Preceding vehicle）Ａと交差車両（Crossing vehicle）Ｂが存在する。
第１のミリ波レーダ３１は先行車両Ａの観測情報を取得する。追跡情報作成部４１は第１のミリ波レーダ３１が取得した観測情報を用いて先行車両Ａの追跡情報を作成する。追跡リスト部４２は追跡リスト（TRACKING LIST 1）に先行車両Ａの追跡情報を記録する。移動物判断部４４は先行車両Ａが移動車両であると判断した場合に先行車両Ａの追跡モードをConfirmedモードに遷移させる。また、第９のミリ波レーダ３９は交差車両Ｂの観測情報を取得する。追跡情報作成部４１は第９のミリ波レーダ３９が取得した観測情報を用いて交差車両Ｂの追跡情報を作成する。追跡リスト部４２は追跡リスト（TRACKING LIST 2）に交差車両Ｂの追跡情報を記録する。移動物判断部４４は交差車両Ｂが移動車両であると判断した場合に交差車両Ｂの追跡モードをConfirmedモードに遷移させる。また、第２のミリ波レーダ３２の観測範囲には物体が存在しないため観測情報が取得されず、追跡リスト（TRACKING LIST 3）に先行車両Ａ及び交差車両Ｂの追跡情報は記録されない。
リスト共有部４３は、観測領域が隣接する第１のミリ波レーダ３１と第９のミリ波レーダ３９の追跡リストを比較し、第１のミリ波レーダ３１の追跡リストに交差車両Ｂの追跡情報を複製し、第９のミリ波レーダ３９の追跡リストに先行車両Ａの追跡情報を複製する。これにより、第１のミリ波レーダ３１と第９のミリ波レーダ３９の追跡リストには、先行車両Ａと交差車両Ｂの追跡情報が共有される。また、観測領域が隣接する第１のミリ波レーダ３１と第２のミリ波レーダ３２の追跡リストを比較し、第２のミリ波レーダ３２の追跡リストに先行車両Ａの追跡情報を複製する。これにより、第１のミリ波レーダ３１と第２のミリ波レーダ３２の追跡リストには、先行車両Ａの追跡情報が共有される。

【0019】

共有処理について詳しく説明する。まず、ｉ番目のミリ波レーダＭ^ｉの観測情報のみを用いた追跡リストＴ^ｉ＝｛Ｔ₁ⁱ，Ｔ₂ⁱ…｝とそれに隣接するｊ番目のミリ波レーダＭ^jの観測情報のみを用いた追跡リストＴ^ｊ＝｛Ｔ₁^j，Ｔ₂^j…｝を考える。追跡リスト部４２は、追跡情報をミリ波レーダごとの追跡リストに記録する。すなわち、ｉ番目のミリ波レーダＭ^ｉの観測情報を用いた追跡情報は追跡リストＴ^ｉに記録され、ｊ番目のミリ波レーダＭ^jの観測情報を用いた追跡情報は追跡リストＴ^ｊに記録される。リスト共有部４３は、このＴ^ｉ内のConfirmedモードの追跡情報をＴ^ｊ内の追跡情報と比較し、Ｔ^ｊ内に同じＩＤの追跡情報がなければそのｋ番目の追跡情報Ｔ_kⁱをＴ^ｊに追加（複製）する。そして対応付け部４５は、共有された追跡情報Ｔ_kⁱについて共有先のミリ波レーダＭ^jの観測情報と対応付けを行う。このとき対応付けがある場合は状態の更新を行いＴ_kⁱはＴ^ｊに残り、対応付けが無い場合はＴ^ｊから除去する。このように、追加された追跡情報が共有先のミリ波レーダの観測情報と対応付けされない場合はその追加された追跡情報を除去することで、不要な情報が減り、処理時間を短縮できる。
最後にＴ^ｉとＴ^ｊ内のＴ_kⁱが両方とも更新された場合、追跡情報選択部４６は、物体が現実世界に存在する事後確率（Track Score）が最大のものを追跡情報として選択する。追跡情報送信部４７は、追跡情報選択部４６が選択した事後確率が最大の追跡情報を制御監視手段５０に送信する。事後確率が最大のものを制御監視手段５０に送信する追跡情報として選定することで、物体を精度良く追跡できる。なお、追跡情報選択部４６は、対応付けコストの小さい方を追跡情報として選択することもできる。リスト共有部４３の共有処理のフローチャートを図６に示す。

【0020】

図６において、共有処理が開始されると、ｉ番目のミリ波レーダ（ＭＷＲ）Ｍ^ｉとｊ番目のミリ波レーダＭ^jの観測領域が隣接しているか否かを判断する（ステップ１）。
ステップ１において、ｉ番目のミリ波レーダＭ^ｉとｊ番目のミリ波レーダＭ^jの観測領域が隣接していないと判断された場合には、ステップ１に戻り、隣接するｉ番目のミリ波レーダＭ^ｉとｊ番目のミリ波レーダＭ^jが見つかるまで判断を繰返す。
ステップ１において、ｉ番目のミリ波レーダＭ^ｉとｊ番目のミリ波レーダＭ^jの観測領域が隣接していると判断された場合には、ｉ番目のミリ波レーダＭ^ｉの観測情報を用いて作成された追跡情報が記録された追跡リストＴ^ｉ内に、ｊ番目のミリ波レーダＭ^jを用いて作成された追跡情報が記録された追跡リストＴ^ｊ内に存在しない識別用のＩＤが付与された追跡情報Ｔ_kⁱが存在するか否かを比較する（ステップ２）。
ステップ２において、追跡リストＴ^ｉ内に、追跡リストＴ^ｊ内には存在しない識別用のＩＤが付与された追跡情報Ｔ_kⁱが存在しない場合には、ステップ１に戻る。
ステップ２において、追跡リストＴ^ｉ内に、追跡リストＴ^ｊ内には存在しない識別用のＩＤが付与された追跡情報Ｔ_kⁱが存在する場合には、追跡リストＴ^ｊに追跡情報Ｔ_kⁱを複製して共有する（ステップ３）。
ステップ３の後、全てのミリ波レーダの追跡リスト同士を比較したか否かを判断する（ステップ４）。
ステップ４において、全てのミリ波レーダの追跡リスト同士を比較していないと判断した場合には、ステップ１に戻る。
ステップ４において、全てのミリ波レーダの追跡リスト同士を比較したと判断した場合には、各ミリ波レーダの観測情報と追跡リストを用いてコスト行列を作成し、前時刻における追跡情報と次時刻における観測情報の対応付けを探索する（ステップ５）。
ステップ５の後、ステップ３において共有した追跡情報Ｔ_kⁱに対応付けられる観測情報が共有先のミリ波レーダＭ^jの観測情報に存在するか否かを判断する（ステップ６）。
ステップ６において、共有した追跡情報Ｔ_kⁱに対応付けられる観測情報が共有先のミリ波レーダＭ^jの観測情報に存在する場合には、共有した追跡情報Ｔ_kⁱを更新して追跡リストＴ^ｊ内に保持する（ステップ７）。
ステップ６において、共有した追跡情報Ｔ_kⁱに対応付けられる観測情報が共有先のミリ波レーダＭ^jの観測情報に存在しない場合には、共有した追跡情報Ｔ_kⁱを追跡リストＴ^ｊ内から削除する（ステップ８）。
ステップ７又はステップ８の後、あるＩＤが付与された追跡情報について、複数の追跡リストに同じＩＤが付与された追跡情報が存在するか否かを判断する（ステップ９）。
ステップ９において、複数の追跡リストに、同じＩＤが付与された追跡情報が存在すると判断された場合には、同じＩＤが付与された追跡情報の内、物体が現実世界に存在する事後確率（Track Score）が最も大きい追跡情報を制御監視手段５０に送信する（ステップ１０）。
ステップ９において、複数の追跡リストに、同じＩＤが付与された追跡情報が存在しないと判断された場合には、そのまま追跡情報を制御監視手段５０に送信する（ステップ１１）。
ステップ１０又はステップ１１の後、次のフレームで同様の処理が行われる。

【0021】

次に、本発明の被観測物追跡装置を用いた実験について説明する。金沢大学構内の道路にて、物体を先行車両として実験を行った。先行車両は約１５〜２０km/hで走行しており、実験車両（自車両）１０もほぼ同じ速度で走行して先行車両を追跡する。先行車両は直線道路を走行した後に交差点で右折する。実験車両１０も同様に右折する。追跡実験を行った時間は２０［ｓ］（４００フレーム）である。走行した経路を図７に白矢印で示す。図７における背景は全方位ＬＩＤＡＲ２２から得られる赤外線反射率を利用して生成した地図画像である。そして上述のようにConfirmedモードで追跡された物体の追跡情報を、隣接するミリ波レーダ３１〜３９の観測情報を用いた追跡リストに共有する。ここで、モード遷移のためのパラメータはＰ_cnf＝０．９９９，Ｐ_stop＝０．１００，Ｄ_move＝１．５［ｍ］，Ｐ_dlt＝０．２００，Ｎ_dlt＝３と設定した。

【0022】

次に、実験の結果を示す。実験車両１０は直線道路において車両前方のミリ波レーダ（図２の第１のミリ波レーダ３１）で先行車両を追跡した。その後、先行車両が交差点を右折した際には車両右斜め前のミリ波レーダ（図２の第９のミリ波レーダ３９）と追跡情報を共有することによりロストすることなく追跡を継続した。そして、実験車両が右折を終えて直線に戻った際には車両前方のミリ波レーダと追跡情報を共有することによりロストすることなく追跡を継続した。この結果を図８に示す。図８において太線のプロットが略全方位ミリ波レーダシステム３０の各ミリ波レーダ３１〜３９の観測情報を用いてそれぞれで追跡処理を行い、追跡情報を共有したときの先行車両の軌跡である。また、図８の破線のプロットは実験車両前方に取り付けたミリ波レーダのみで先行車両を追跡した場合の軌跡である。実験車両前方に取り付けたミリ波レーダのみで先行車両を追跡した場合は、先行車両が交差点を右折した後、実験車両も右折を行い前方に先行車両をとらえるまでの間、ロストしてしまう結果となった。
表２にロストしたフレーム数や処理時間などを示す。また、各フレームの処理時間と観測情報数の関係を図９に示す。図９において、濃い線が本実施例による略全方位ミリ波レーダシステム３０を利用し追跡情報の共有を行った追跡処理時間を示し、薄い線が実験車両前方に取り付けたミリ波レーダのみを用いた場合の追跡処理時間を示す。
表２より実験車両前方のミリ波レーダのみを用いた場合、先行車両を９６フレームの間ロストしたことがわかる。つまりミリ波レーダの観測周波数２０［Ｈｚ］を考慮すると４．８［ｓ］の間、実験車両は先行車両を見失い、先行車両が急ブレーキをかけたとしても衝突の危険性を察知することができないといえる。これに対して本実施例による略全方位ミリ波レーダシステム３０の各ミリ波レーダ３１〜３９の観測情報を用いてそれぞれで追跡処理を行い、追跡情報を共有したときの処理時間は、観測情報の数の分、前方のミリ波レーダのみを用いた場合よりは大きくなったが、リアルタイムでの処理でロストの無い効率的かつ効果的な追跡が行えることがわかる。

【表2】

【産業上の利用可能性】

【0023】

本発明による物体追跡装置は、特に自動運転自動車に適用することで、先行車両や後続車両等に対して継続したリアルタイムな追跡を実現できる。

【符号の説明】

【0024】

１０ 自車両
２１ ＧＮＳＳ複合航法システム
２２ 全方位ＬＩＤＡＲ
３０ 略全方位センサシステム（観測情報取得手段）
３１、３２、３３、３４、３５、３６、３７、３８、３９ センサ
４０ 追跡情報作成管理手段
４１ 追跡情報作成部
４２ 追跡リスト部
４３ リスト共有部
４４ 移動物判断部
４５ 対応付け部
４６ 追跡情報選択部
４７ 追跡情報送信部
５０ 制御監視手段

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】