JP6895911B2 - 物体追跡装置、物体追跡方法及び物体追跡用コンピュータプログラム - Google Patents

Description

本発明は、車両の周囲にある物体を追跡する物体追跡装置、物体追跡方法及び物体追跡用コンピュータプログラムに関する。

車両の衝突回避といった運転支援あるいは自動運転制御を実現するために、車両の周囲を走行する他の車両といった、車両の周囲にある物体の挙動を正確に測定することが求められている。そこで、例えば、レーダあるいはLaser Imaging Detection and Ranging (LIDAR)といった、車両に搭載され、かつ、車両の周囲の物体までの距離を検知する測距センサを用いて、車両の周囲の物体を追跡することが研究されている。

このような測距センサでは、測距センサから照射される、レーザあるいはミリ波などの測距用信号を反射し易い物体の位置が測距点として検出されることがある。特に、車両の後端には、左端側と右端側の２か所にリフレクタが設けられている。そのため、車両周囲の他の車両の追跡中において、特に、車両と他の車両間の距離が遠い場合には、他の車両の各リフレクタが測距点として検出される。しかし、場合によっては、追跡中に、二つのリフレクタのうちの一方と測距センサとの間に他の物体（例えば、追跡中の他の車両以外の車両）が進入することで、その一方のリフレクタが検知されなくなることがある。また、車両から追跡中の他の車両までの距離が遠かったり、他の車両の向きに応じてリフレクタの向きが変わると、何れかのリフレクタにより反射された測距用信号の強度が低すぎてそのリフレクタが検知されなくなることがある。このように、他の車両の追跡中において何れかのリフレクタの検出状態が変化すると、特に、検出状態が変化する前後において検出された測距点の位置が急激に変化するため、他の車両を正確に追跡することが困難となる。

また、車両と追跡中の他の車両間の距離が近い場合には、車両と他の車両の相対的な位置関係（個々の車両の相対的な向きの差も含む）の変化に応じて、測距用信号を反射する他の車両上の位置も変化することがある。このような場合も、他の車両を正確に追跡することが困難となる。

そこで、追跡対象となる車両の形状を表すモデルを仮定し、そのモデルを用いてその車両を追跡する技術が提案されている（例えば、特許文献１及び２、及び非特許文献１を参照）。

特開２０１６−１４８５１４号公報 特開２０１７−１２９４４６号公報

A. Petrovskaya他、「Model Based Vehicle Detection and Tracking for Autonomous Urban Driving」、Autonomous Robots、Vol. 26、pp.123-139、2009年

しかし、特許文献１または非特許文献１に記載の技術では、追跡対象となる車両は、矩形形状のモデルで表される。しかし、測距点として車両に設けられたリフレクタだけが検知されるような場合には、この技術では、追跡対象となる車両において測距用信号を反射する位置が正確にモデル化されず、その結果として、追跡対象となる車両を正確に追跡することが困難なことがある。また、特許文献２に記載の技術では、レーザを反射した計測点をクラスタリングすることで得られたクラスタを形状モデルに当てはめた上で形状モデルの移動が追跡される。そのため、この技術では、追跡中に測距用信号が反射される位置が変化するような場合には、追跡中に得られる計測点の分布が当てはめた形状モデルと必ずしも整合しないので、追跡対象となる車両を正確に追跡することが困難なことがある。

そこで、本発明は、車両の周囲の物体を正確に追跡できる物体追跡装置、物体追跡方法及び物体追跡用コンピュータプログラムを提供することを目的とする。

請求項１の記載によれば、本発明の一つの形態として、物体追跡装置が提供される。この物体追跡装置は、車両（１０）に取り付けられた測距部（２）により得られた測距データから、車両（１０）の周囲に位置する追跡対象物体の位置を表す少なくとも一つの測距点を検出する物体検出部（２１）と、追跡対象物体上の位置ごとの測距点となる確からしさを表す形状モデルに従って、直近の所定期間中に得られた測距点のそれぞれについての追跡対象物体のその所定期間における軌跡に基づいて求められる追跡対象物体を基準とする対象座標系における測距点の位置が追跡対象物体上となる確率の和が最大となるように、その軌跡及び形状モデルを最尤推定する物体追跡部（２２〜２４）とを有する。
本発明による物体追跡装置は、上記の構成を有することにより、車両の周囲の物体を正確に追跡することができる。

また請求項２の記載によれば、形状モデルは、追跡対象物体の進行方向と平行な方向における、測距点となる確からしさの標準偏差と、追跡対象物体の進行方向と直交する方向における位置ごとの測距点である確からしさとを含むことが好ましい。
これにより、物体追跡装置は、追跡対象物体上の位置ごとの測距点となる確からしさを適切、かつ、簡単化して表すことができるので、最尤推定の際の演算量を削減しつつ、車両周囲の物体を正確に追跡できる。

あるいは、請求項３の記載によれば、形状モデルは、追跡対象物体を２次元格子状の複数の区間ごとに分割したときの複数の区間のそれぞれごとの測距点となる確からしさを含むことが好ましい。
これにより、物体追跡装置は、追跡対象物体の追跡中において測距点となる位置が追跡対象物体の進行方向に沿って変化する場合でも、追跡対象物体上の位置ごとの測距点となる確からしさを適切に評価できるので、車両周囲の物体を正確に追跡できる。

さらに、請求項４の記載によれば、物体追跡部（２２〜２４）は、確率の和が最大となるように形状モデルを変化させずに軌跡を推定することと、推定された軌跡に従って算出した対象座標系での所定期間中の測距点のそれぞれの位置における確からしさが高くなるように形状モデルを更新することとを交互に繰り返し、軌跡の更新が収束したときの確率の和の最大値に対応する軌跡を、所定の期間における追跡対象物体の軌跡とすることが好ましい。
これにより、物体追跡装置は、追跡対象物体の軌跡と形状モデルの両方を適切に推定できる。

請求項５の記載によれば、本発明の他の形態として、物体追跡方法が提供される。この物体追跡方法は、車両（１０）に取り付けられた測距部（２）により得られた測距データから、車両（１０）の周囲に位置する追跡対象物体の位置を表す少なくとも一つの測距点を検出するステップと、追跡対象物体上の位置ごとの測距点となる確からしさを表す形状モデルに従って、直近の所定期間中に得られた測距点のそれぞれについての追跡対象物体のその所定期間における軌跡に基づいて求められる追跡対象物体を基準とする対象座標系における測距点の位置が追跡対象物体上となる確率の和が最大となるようにその軌跡及び形状モデルを最尤推定するステップとを含む。
本発明による物体追跡方法は、上記の構成を有することにより、車両の周囲の物体を正確に追跡することができる。

請求項６の記載によれば、本発明のさらに他の形態として、物体追跡用コンピュータプログラムが提供される。この物体追跡用コンピュータプログラムは、車両（１０）に取り付けられた測距部（２）により得られた測距データから、車両（１０）の周囲に位置する追跡対象物体の位置を表す少なくとも一つの測距点を検出するステップと、追跡対象物体上の位置ごとの測距点となる確からしさを表す形状モデルに従って、直近の所定期間中に得られた測距点のそれぞれについての追跡対象物体のその所定期間における軌跡に基づいて求められる追跡対象物体を基準とする対象座標系における測距点の位置が追跡対象物体上となる確率の和が最大となるようにその軌跡及び形状モデルを最尤推定するステップとをコンピュータに実行させる命令を含む。
本発明による物体追跡用コンピュータプログラムは、上記の構成を有することにより、車両の周囲の物体を正確に追跡することができる。

上記各部に付した括弧内の符号は、後述する実施形態に記載の具体的手段との対応関係を示す一例である。

（ａ）及び（ｂ）は、それぞれ、追跡対象物体の追跡中において、測距センサが追跡対象物体上の同じ位置までの距離を測定できない場合の一例を示す図である。 一つの実施形態による物体追跡装置の概略構成図である。 制御部の機能ブロック図である。 形状モデルの一例を表す図である。 物体追跡処理の動作フローチャートである。 変形例による形状モデルの模式図である。

以下、図を参照しつつ、物体追跡装置について説明する。
この物体追跡装置は、車両に搭載され、車両の周囲に位置する物体までの距離を測定することが可能な測距センサから得られた測距データに基づいて、車両の周囲に位置する物体を追跡する。その際、上記のように、追跡対象物体の追跡中において、測距センサが追跡対象物体上の同じ位置までの距離を測定できないことがある。

図１（ａ）及び図１（ｂ）は、それぞれ、追跡対象物体の追跡中において、測距センサが追跡対象物体上の同じ位置までの距離を測定できない場合の一例を示す図である。

図１（ａ）に示される例では、車両１０に搭載された測距センサによる測距データを用いて、車両１０の前方を走行する車両１０１が追跡されているとする。この場合において、時刻(k-1)では、車両１０１の後端の左側及び右側のそれぞれのリフレクタ１０１ａ、１０１ｂにおいて測距センサからの測距用信号が反射される。そのため、車両１０は、リフレクタ１０１ａの位置及びリフレクタ１０１ｂの位置を検出できる。しかし、時刻kでは、車両１０と車両１０１間の間隔が拡がるため、車両１０と車両１０１の間を走行する他の車両１０２によりオクルージョンが生じて、車両１０の測距センサからの測距用信号がリフレクタ１０１ａに届かなくなり、その結果として、リフレクタ１０１ｂの位置のみが検出される。このような場合、車両１０は、車両１０１を正確に追跡できなくなることがある。

また、図１（ｂ）に示される例では、車両１０に搭載された測距センサによる測距データを用いて、車両１０の前方のコーナーを右側から車両１０の前方へ向けて左折してくる車両１０３が追跡されているとする。この場合において、時刻(k-2)、時刻(k-1)、時刻kのそれぞれにおいて、測距センサからの測距用信号を反射する車両１０３上の位置が異なっている。すなわち、時刻(k-2)では、測距センサによる測距可能な角度範囲１１０内に車両１０３の右前方が含まれるので、車両１０３の右前方において測距用信号が反射される。そのため、車両１０３の右前方の位置が検出される。また、時刻(k-1)では、車両１０から見て、車両１０３の向きが変わることで、車両１０３の正面全体において測距センサからの測距用信号が反射されるため、車両１０３の正面の位置が検出される。さらに、時刻kでは、車両１０と車両１０３とはすれ違いつつあり、車両１０３の右後方において測距用信号が反射されるため、車両１０３の右後方の位置が検出される。したがって、この場合も、車両１０は、車両１０１を正確に追跡できなくなることがある。

そこで、この物体追跡装置は、直近の所定期間における、測距センサにより追跡対象物体上で距離が測定される点（以下、測距点と呼ぶ）の集合に基づいて、その所定期間における追跡対象物体の軌跡だけでなく、追跡対象物体上の位置ごとの測距点となる確からしさを表すモデル（以下では、形状モデルと呼ぶ）を最尤推定することで、追跡中において、測距点となる追跡対象物体上の位置が変化しても追跡対象物体を正確に追跡することを可能とする。

図２は、一つの実施形態による物体追跡装置の概略構成図である。図２に示すように、物体追跡装置１は、車両１０に搭載され、測距センサ２、慣性計測装置(IMU)３、車輪速センサ４、及び電子制御ユニット(ECU)５と、コントローラエリアネットワーク（以下、ＣＡＮという）６を介して互いに接続されている。なお、図２では、説明の都合のため、物体追跡装置１など、車両１０に搭載されている各構成要素及び車両１０の形状、サイズ及び配置は、実際のものとは異なっている。

測距センサ２は、測距部の一例であり、例えば、LIDARあるいはレーダセンサを有する。測距センサ２は、例えば、車両１０の前面側に取り付けられる。そして測距センサ２は、所定の測定周期（例えば、30msec〜100msec）で、所定間隔（例えば、1°）の方位ごとに測距センサ２から車両１０の周囲に存在する物体までの距離を測定し、その距離の測定値を表す測距データを、ＣＡＮ６を介して物体追跡装置１へ出力する。

物体追跡装置１は、記憶部１１と、通信部１２と、制御部１３とを有する。
記憶部１１は、例えば、電気的に書き換え可能な不揮発性メモリ及び揮発性メモリなどの半導体メモリを有する。そして記憶部１１は、物体追跡装置１を制御するための各種プログラム、及び、物体追跡処理で利用される各種の情報、例えば、直近の所定期間（例えば、測距センサ２による測定周期の10〜20倍）内に測距センサ２から受信した測距データ、及び、制御部１３による一時的な演算結果などを記憶する。

通信部１２は、測距センサ２、IMU３、車輪速センサ４、及びECU５などとＣＡＮ６を通じて通信する通信インターフェース及びその制御回路を有する。そして通信部１２は、測距センサ２から測距データを受け取り、その測距データを制御部１３へ渡す。また通信部１２は、物体追跡処理を実行する周期ごとに、IMU３から、車両１０の速度及び移動量などを表すオドメトリ情報を取得したり、あるいは、車輪速センサ４から車輪速を取得して、制御部１３へ渡す。あるいはまた、制御部１３は、車両１０に搭載されたGPS受信機（図示せず）から、車両１０の現在位置を表すGPS信号を受信してもよい。

制御部１３は、１個もしくは複数個の図示してないプロセッサ及びその周辺回路を有する。そして制御部１３は、物体追跡装置１全体を制御する。
図３に、制御部１３の機能ブロック図を示す。図３に示すように、制御部１３は、物体検出部２１と、軌跡更新部２２と、形状モデル更新部２３と、収束判定部２４とを有する。このうち、軌跡更新部２２、形状モデル更新部２３及び収束判定部２４は、物体追跡部の一例である。制御部１３が有するこれらの各部は、例えば、制御部１３が有するプロセッサ上で実行されるコンピュータプログラムによって実現される機能モジュールとして実装される。

最初に、制御部１３により実行される物体追跡処理における、追跡対象物体の軌跡と形状モデルの最尤推定の概要について説明する。

追跡対象物体の軌跡を推定する直近の所定期間を[K-τ,K]とする。ここでτは、所定期間の長さを、測距センサ２による測定周期で除した値、すなわち、測距センサ２による測定周期単位の離散時間で表した直近の所定期間の長さを表す。またKは、測距センサ２による最新の測定時刻を表す。ここで、所定期間直前の離散時間p(=K-τ-1)における追跡対象物体の状態x^p、離散時間pにおける形状モデルm^p、及び、所定期間中に取得された測距データから求められる、所定期間中における追跡対象物体の位置の測定結果Z^K-τ:Kに対する、所定期間における追跡対象物体の軌跡y及び形状モデルmの条件付確率p(y,m|x^p,m^p,z^K-τ:K)には、以下の関係が成立する。

したがって、制御部１３は、次式に従って、所定期間における追跡対象物体の軌跡y及び形状モデルmを最尤推定すればよい。

ここで、p(z^K-τ:K|m,y)は、形状モデルがmとなり、かつ、軌跡がyであるときに、所定期間中における追跡対象物体の位置の測定結果がZ^K-τ:Kとなる条件付き確率である。またp(y|X^P)は、所定期間直前の離散時間pにおける追跡対象物体の状態がX^pであるときに軌跡がyとなる条件付き確率である。さらに、p(m|m^p)は、所定期間直前の離散時間pにおける形状モデルがm^pであるときに所定期間における形状モデルがmとなる条件付き確率である。

その際、軌跡yと形状モデルmの最尤推定を簡単化するため、本実施形態では、制御部１３は、次式に従って、軌跡yと形状モデルmを交互に更新する。

すなわち、軌跡更新部２２は、（３−１）式に従って軌跡yを更新し、形状モデル更新部２３は、（３−２）式に従って形状モデルmを更新する。なお、y~は、形状モデルmの更新時において、それ以前に最尤推定された軌跡yを表し、m~は、軌跡yの更新時において、それ以前に最尤推定された形状モデルを表す。すなわち、軌跡更新部２２は、直前の最尤推定により更新された形状モデルm~を固定として、（３−１）式に従って軌跡yを更新する。一方、形状モデル更新部２３は、直前の最尤推定により更新された軌跡y~を固定として、（３−２）式に従って形状モデルmを更新する。そして収束判定部２４は、軌跡y及び形状モデルmの更新が収束したか否かを判定し、収束したと判定したときの最尤推定された軌跡y~を、所定期間における追跡対象物体の軌跡とする。

以下、制御部１３の各部について詳細に説明する。

物体検出部２１は、測距センサ２から測距データが得られる度に、その測距データに含まれる、各方位における測距点の集合から、車両１０の周囲に存在する物体についての測距点の集合を検出する。例えば、物体検出部２１は、車両１０からの距離が所定距離（例えば、50m〜100m）以下となる測距点のうち、互いの測距点間の距離が所定値（例えば、0.2〜0.3m）以下となるように各測距点をクラスタリングすることで得られた測距点の集合を、車両１０の周囲に存在する物体についての測距点の集合として検出する。あるいは、物体検出部２１は、LIDARあるいはレーダスキャナによる測距データから、LIDARあるいはレーダスキャナの周囲の物体を検出する様々の手法の何れかに従って、車両１０の周囲に存在する物体についての測距点の集合を検出してもよい。物体検出部２１は、測距点の集合で表される、車両１０の周囲に存在する物体を、追跡対象物体とする。なお、物体検出部２１により車両１０の周囲の物体が最初に検出されたときから、その物体の追跡が開始されればよい。

軌跡更新部２２は、直近の所定期間[K-τ,K]における追跡対象物体の軌跡yを（３−１）式に従って更新することで、軌跡yを最尤推定する。

そこで先ず、（３−１）式を計算するために、軌跡yに含まれる各要素、及び、（３−１）式の右辺の各項を具体的に記述する。

軌跡yには、所定期間[K-τ,K]に含まれる各離散時間k(=K-τ,K-τ+1,...,K-1,K)における、追跡対象物体の状態、すなわち、位置、速度、方位、角速度、加速度、角加速度が含まれる。しかし、これらを全て独立の変数として扱うと、軌跡yの最尤推定の際に扱う変数の数が非常に多くなり、必要な演算量が非常に大きくなる。一方、追跡対象物体が車両である場合、追跡対象物体の運動は、車両の挙動を考慮した運動モデルに従う。すなわち、追跡対象物体の加速度及び角加速度は急激に変化しないと仮定される。このことから、追跡対象物体は、所定期間[K-τ,K]において等加速度かつ等角加速度で運動すると仮定される。したがって、本実施形態では、軌跡yは、次式のように、最新の離散時間Kにおける位置(x^K,y^K)、速度v^K、方位θ^K、角速度ω^K、加速度a^K、角加速度φ^Kをそれぞれ要素とするベクトルで表される。これにより、扱う変数の数が削減されるので、物体追跡処理の演算量が削減される。

この場合、軌跡yに含まれる各離散時間kにおける追跡対象物体の状態X^kは、次式に従って算出される。

ここでhは、測距センサ２の測定周期を表す。

軌跡yが上記のように表されるので、追跡対象物体の方位、角速度、加速度及び角加速度が正規分布に従うと仮定すると、（３−１）式における右辺の第２項に含まれる、条件付確率p(y|X^P)は下記のように近似される。

ここで、θ^p及びω^pは、軌跡yから（５）式に従って算出される離散時間pにおける方位及び角速度である。μ_θ ^p及びσ_θ ^p2は、離散時間pにおける、方位θの平均値及び分散を表す。また、μ_ω ^p及びσ_ω ^p2は、離散時間pにおける、角速度ωの平均値及び分散を表す。同様に、σ_a ²及びσ_φ ²は、それぞれ、加速度及び角加速度の分散を表す。σ_θ ^p、σ_ω ^p、σ_a及びσ_φは、実験的に定められてもよく、例えば、σ_θ ^p=1[deg]、σ_ω ^p=0.1[deg/s]、σ_a=0.1[m/s²]及びσ_φ=0.1[deg/s²]に設定される。

次に、（３−１）式に含まれる右辺の第1項に関して、所定期間中における追跡対象物体の位置の測定結果Z^K-τ:Kに含まれる、各離散時間kにおける測定結果z^kは、測距点の集合{q^k _w,1,q^k _w,2,...,q^k _w,N}を含む。ここで、測距点q^k _w,j(j=1,2,...,N、ただし、Nは1以上の整数)は、離散時間kにおけるj番目の測距点の世界座標系における位置を表す。各測距点の観測は、互いに独立であると仮定されるので、（３−１）式に含まれる右辺の第1項は、下記のように表される。

本実施形態では、各測距点は、車両１０に搭載された測距センサ２により求められるので、世界座標系で表された測距点q^k _w,jを求めるために、軌跡更新部２２は、車両１０を基準とする車両座標系で表された各測距点の位置を、車両座標系の座標から世界座標系の座標に座標変換する。例えば、軌跡更新部２２は、次式で表される座標変換に従って、車両座標系で表された各測距点の座標を世界座標系の座標に変換する。

ここで、測距点q^k _l,jは、離散時間kにおけるj番目の測距点の車両座標系における座標を表す。またR^k _lw及びT^k _lwは、それぞれ、離散時間kにおける、車両座標系から世界座標系への座標変換を表す回転行列及び並進行列である。軌跡更新部２２は、回転行列R^k _lw及び並進行列T^k _lwを、例えば、離散時間kにおける車両１０のオドメトリ情報、及び、GPS信号などの車両１０の位置を表す情報に基づいて算出する。あるいは、軌跡更新部２２は、回転行列R^k _lw及び並進行列T^k _lwを、Simultaneous Localization and Mapping (SLAM)技術を用いて算出してもよい。

さらに、測距点は、追跡対象物体上に位置する。そのため、（３−１）式に含まれる右辺の第1項の計算を簡単化するためには、世界座標系で表された測距点の位置を、追跡対象物体を基準とする座標系（以下、対象座標系と呼ぶ）の位置で表すことが好ましい。そこで、軌跡更新部２２は、次式で表される座標変換に従って、世界座標系で表された各測距点の座標を対象座標系の座標に変換する。

ここで、測距点q^k _o,j=(q^k _ox,j,q^k _oy,j)は、離散時間kにおけるj番目の測距点の対象座標系における座標を表す。q^k _ox,jは、追跡対象物体の進行方向と直交する方向における座標を表し、q^k _oy,jは、追跡対象物体の進行方向と平行な方向における座標を表す。またR^k _wo及びT^k _woは、それぞれ、離散時間kにおける、世界座標系から対象座標系への座標変換を表す回転行列及び並進行列である。そしてR^k _ow及びT^k _owは、それぞれ、離散時間kにおける、対象座標系から世界座標系への座標変換を表す回転行列及び並進行列である。（９）式に示されるように、回転行列R^k _ow及び並進行列T^k _owは、軌跡yに含まれる、離散時間kにおける追跡対象物体の位置(x^k,y^k)及び方位θ^kから求められる。

また、q^k _ox,j及びq^k _oy,jは、互いに独立しているため、（７）式の右辺に含まれる条件付確率p(q^k _w,j|m,y)は、次式で表される。

最後に、各測距点が、車両１０と面する方向の端部（例えば、追跡対象物体の後端）上に位置すると仮定されることから、追跡対象物体の形状モデルmは、次式のように表される。

ここで、σ_yは、追跡対象物体の進行方向における、測距点となる確からしさの標準偏差を表す。またηは、追跡対象物体の進行方向と直交する方向における、追跡対象物体の重心を表す。そしてb_i(i=1,2,...,M)は、追跡対象物体の進行方向と直交する方向において、追跡対象物体をM個の区間に分割したときの、区間iにおける重み係数を表す。重み係数b_iは、対応する区間が測距点となる確からしさを表し、一般に、測距用信号の反射強度が高くなる区間ほど大きい値となる。

図４は、形状モデルの一例を表す図である。図４において、横軸は、追跡対象物体の進行方向と直交する方向の位置を表し、縦軸は、各位置における重み係数b_iを正規分布で平滑化して得られる値の大きさを表す。グラフ４００は、二つのリフレクタが設けられる車両後端についての形状モデルの一例を表す。この例では、平滑化後の重み係数が極大値となる位置が二つ存在し、極大値となる二つの位置の中間に位置する極小値と極大値間の差は比較的大きい。これは、リフレクタによる測距用信号の反射強度と、リフレクタ以外の部分による測距用信号の反射強度との差が大きいためである。一方、グラフ４０１は、リフレクタが設けられていない方の車両の端部（例えば、車両の前面）についての形状モデルの一例を表す。この例では、車両のその端部全体にわたって比較的同じ程度で測距用信号が反射されるため、極大値と極小値の差は比較的小さくなる。

したがって、（１１）式における、追跡対象物体の進行方向における測距点の位置に関する条件付確率p(q^k _oy,j|m)は、正規分布に従うと仮定することで以下のように表される。

一方、（１１）式における、追跡対象物体の進行方向と直交する方向における測距点の位置に関する条件付確率p(q^k _ox,j|m)は、正規分布に従うと仮定することで以下のように表される。

ここで、ビンμ_iは、区間iの中点を表す。またσ_xは、測距点の観測誤差に応じた標準偏差であり、例えば、0.2[m]に設定される。

以上により、（３−１）式を解く問題は、次式で表されるコスト関数L(y)を最大化することに置き換えることができる。すなわち、軌跡更新部２２は、軌跡yから求められる、直近の所定期間内の各離散時間における測距点ごとの、その測距点の対象座標系上の座標が追跡対象物体上に位置する確率の和が最大化されるように、軌跡yを更新すればよい。

軌跡更新部２２は、（１４）式において、直前の更新で得られた形状モデルの値を適用すればよい。そして軌跡更新部２２は、例えば、準ニュートン法、最急降下法、シミュレーティッドアニーリング法、あるいは、信頼領域法といった最適化手法に従って、軌跡yに含まれる、離散時間Kにおける追跡対象物体の位置、方位、速度、角速度、及び、加速度及び角加速度の少なくとも何れかを更新する。そして軌跡更新部２２は、更新回数が所定回数に達するか、直近の２回の更新のそれぞれにおけるコスト関数L(y)間の差の絶対値が所定値以下となったときに更新を終了すればよい。軌跡更新部２２は、更新が終了した時点において、それまでに得られたコスト関数L(y)の値のうちの最大値に対応する軌跡yを、その最尤推定値y~とすればよい。
軌跡更新部２２は、推定された軌跡y及び対応するコスト関数L(y)の値を記憶部１１に記憶する。

形状モデル更新部２３は、形状モデルmを（３−２）式に従って更新することで、形状モデルmを最尤推定する。すなわち、形状モデル更新部２３は、直近の所定期間に含まれる離散時間ごとの各測距点の対象座標系上の位置が測距点となる確からしさを高くするように形状モデルを更新する。

本実施形態では、形状モデル更新部２３は、軌跡更新部２２による直前の更新で得られた追跡対象物体の軌跡の推定値y~にしたがって、（９）式における回転行列R^k _ow及び並進行列T^k _owを更新し、更新された回転行列R^k _ow及び並進行列T^k _owを用いて、（９）式に従って、各離散時間における各測距点q^k _ojの対象座標系における座標を再計算する。

形状モデル更新部２３は、追跡対象物体について追跡を開始した以降の各離散時間における、各測距点の追跡対象物体の進行方向についての対象座標系での座標q^l _oy,j(l=1,2,...,p)と、再計算された各測距点の追跡対象物体の進行方向についての対象座標系での座標q^k _oy,j(k=K-τ,...,K)とから、追跡対象物体の進行方向における位置の標準偏差σ_yを再計算する。

また、形状モデル更新部２３は、再計算された各測距点について、ビンμ_iのうち、その測距点の追跡対象物体の進行方向と直交する方向についての対象座標系での座標q^k _ox,j(k=K-τ,...,K)に最も近いビンμ_nに対応する重み係数b_nに1を加算することで、各重み係数b_iを更新する。さらに、各重み係数b_iが更新されると、形状モデル更新部２３は、追跡対象物体の進行方向と直交する方向についての重心ηを次式に従って更新する。

形状モデル更新部２３は、更新された形状モデルmを最尤推定された形状モデルm~として記憶部１１に記憶する。

収束判定部２４は、軌跡y及び形状モデルmの更新が収束したか否かを判定し、収束したと判定したときの最尤推定された軌跡y~を、所定期間における追跡対象物体の軌跡とする。例えば、収束判定部２４は、前回の軌跡更新部２２による軌跡更新の実行時におけるコスト関数L(y)の値と、最新の軌跡更新部２２による軌跡更新の実行時におけるコスト関数L(y)の値間の変化量が所定の変化量閾値以下となったとき、軌跡更新部２２による軌跡更新の実行回数が所定回数に達したとき、あるいは、最新の軌跡更新部２２による軌跡更新の実行中においてコスト関数L(y)が所定の終了閾値以上となったときに、軌跡y及び形状モデルmの更新が収束したと判定する。そして収束判定部２４は、更新終了時までに得られたコスト関数L(y)の値のうちの最大値に対応する軌跡y及び形状モデルmを、それぞれ、最尤推定された追跡対象物体の軌跡及び形状モデルとする。

図５は、物体追跡処理の動作フローチャートである。制御部１３は、測距センサ２により最新の測距データが得られるごとに、以下に示す動作フローチャートに従って物体追跡処理する。あるいは、制御部１３は、測距センサ２による測定周期の複数倍の周期ごとに、以下に示す動作フローチャートに従って物体追跡処理を実行してもよい。

物体検出部２１は、測距センサ２による最新の測距データから、車両１０の周囲に存在する追跡対象物体上の測距点の集合を検出する（ステップＳ１０１）。

軌跡更新部２２は、追跡対象物体の最新の測距点の集合を含む、直近の所定期間内の各離散時間における測距点の集合から直近の所定期間における追跡対象物体の軌跡yの初期値を設定する（ステップＳ１０２）。例えば、軌跡更新部２２は、最新の測距点の重心に相当する、世界座標系の座標を追跡対象物体の最新の位置(x^K,y^K)とし、前回の物体追跡処理における方位、速度、角速度、加速度及び角加速度を、それぞれ、θ^K、v^K、ω^K、a^K及びφ^Kとする。なお、追跡対象物体について行われる最初の物体追跡処理の場合には、θ^K、v^K、ω^K、a^K及びφ^Kとして、任意の値、例えば、オドメトリ情報から得られる、車両１０自体の最新の方位、速度、角速度、加速度及び角加速度が、θ^K、v^K、ω^K、a^K及びφ^Kとして用いられてもよい。また、軌跡更新部２２は、前回の物体追跡処理により求められた形状モデルmを、軌跡yの更新に利用する最尤推定された形状モデルm~とする（ステップＳ１０３）。なお、軌跡更新部２２は、記憶部１１に予め記憶される、標準の形状モデルを最尤推定された形状モデルm~としてもよい。

軌跡更新部２２は、最尤推定された形状モデルm~に対して、軌跡yから求められる、直近の所定期間内の離散時間ごとの各測距点が追跡対象物体上となる確率の和が最大化されるよう、すなわち、コスト関数L(y)の値が最大化されるよう、軌跡yを最尤推定する（ステップＳ１０４）。また、形状モデル更新部２３は、最尤推定された軌跡y~に基づいて、形状モデルmを更新して、最尤推定された形状モデルm~を求める（ステップＳ１０５）。

収束判定部２４は、軌跡y及び形状モデルmの更新が収束したか否かを判定する（ステップＳ１０６）。更新が収束していない場合（ステップＳ１０６−Ｎｏ）、制御部１３は、ステップＳ１０４以降の処理を繰り返す。一方、更新が収束した場合（ステップＳ１０６−Ｙｅｓ）、収束判定部２４は、確率の和が最大、すなわち、コスト関数L(y)が最大となるときの軌跡yを、直近の所定期間における追跡対象物体の軌跡と推定する（ステップＳ１０７）。その後、制御部１３は、物体追跡処理を終了する。

以上説明してきたように、物体追跡装置は、直近の所定期間内の離散時間ごとの各測距点が追跡対象物体上となる確率の和が最大化されるように、直近の所定期間における追跡対象物体の軌跡及び形状モデルを更新する。そのため、この物体追跡装置は、追跡対象物体の追跡の途中において、測距センサからの測距用信号が反射される追跡対象物体上の位置が変化したり、追跡対象物体の一部について測距されなくなっても、追跡対象物体の軌跡を正確に推定できる。

なお、追跡対象物体の速度及び角速度は急激には変化しないと想定される。また、追跡対象物体の加速度及び角加速度は、大きな値とはならないと想定される。そこで変形例によれば、軌跡更新部２２は、（１４）式の代わりに次式で表されるコスト関数L(y)を最大化するように、軌跡yを最尤推定してもよい。

その際、軌跡更新部２２は、（１４）式に従って軌跡yを最尤推定するときと同様の処理を実行すればよい。

（１６）式に従って軌跡yを最尤推定することで、軌跡更新部２２は、追跡対象物体が急加速または急減速したり、急激に旋回するような軌跡が求められることを抑制できるので、追跡対象物体の軌跡をより正確に推定できる。

また、測距センサ２からの測距用信号が反射される、追跡対象物体上の位置が、追跡対象物体の進行方向においても大きく変動することがある。例えば、上記のように、追跡中に、追跡対象物体の進行方向と車両１０の進行方向との角度差が変化する場合に、測距用信号を反射する追跡対象物体上の位置が、追跡対象物体の進行方向に沿って変化することがある。

そこで、他の変形例によれば、形状モデルmは次式で表される。

ここでμ_ij(i,j∈1,2,...,V)は、追跡対象物体の進行方向及びその直交方向のそれぞれにおいて、追跡対象物体をV個の区間に分割したときの、区間(i,j)におけるベルヌーイ分布の値、すなわち、その区間(i,j)が測距点となる確率を表す。またη_x,η_yは、それぞれ、追跡対象物体の進行方向と直交する方向における中心及び追跡対象物体の進行方向における中心を表す。

図６は、この変形例による形状モデルmの模式図である。図６に示されるように、形状モデルmは、２次元格子状の確率の集合６００として表される。そして個々の格子の数値は、その格子に相当する区間における、測距用信号が反射される確率である。

この場合、軌跡更新部２２は、次式で表されるコスト関数L(y)を最大化するように、軌跡yを最尤推定すればよい。

ただし、r^k _ojは、離散時間kにおけるj番目の測距点q^k _oj=(q^k _ox,j,q^k _oy,j)の対象座標系における座標を含む区間を表す。またp(r^k _oj|m)は、形状モデルmについての、区間r^k _ojにおける確率μ_rkojである。

この変形例においても、軌跡更新部２２は、例えば、準ニュートン法、最急降下法、シミュレーティッドアニーリング法、あるいは、信頼領域法といった最適化手法に従って、軌跡yに含まれる、離散時間Kにおける追跡対象物体の位置、方位、速度、角速度、及び、加速度及び角加速度の少なくとも何れかを更新する。そして軌跡更新部２２は、更新回数が所定回数に達するか、直近の２回の更新のそれぞれにおけるコスト関数L(y)間の差の絶対値が所定値以下となったときに更新を終了すればよい。軌跡更新部２２は、更新が終了した時点において、それまでに得られたコスト関数L(y)の値のうちの最大値に対応する軌跡yを、その最尤推定値y~とすればよい。

なお、この変形例においても、軌跡更新部２２は、（１８）式の代わりに、次式で表されるコスト関数L(y)を最大化するように、軌跡yを最尤推定してもよい。

この場合も、（１６）式が用いられる場合と同様に、軌跡更新部２２は、追跡対象物体が急加速または急減速したり、急激に旋回するような軌跡が求められることを抑制できるので、追跡対象物体の軌跡をより正確に推定できる。

また、形状モデル更新部２３は、上記の実施形態と同様に、軌跡更新部２２による直前の更新で得られた追跡対象物体の軌跡の最尤推定値y~にしたがって、（９）式における回転行列R^k _ow及び並進行列T^k _owを更新し、更新された回転行列R^k _ow及び並進行列T^k _owを用いて、（９）式に従って、各離散時間における各測距点q^k _ojの対象座標系における座標を再計算する。そして形状モデル更新部２３は、再計算された各測距点q^k _ojの対象座標系における座標に基づいて、（１７）式で表される形状モデルmを更新すればよい。その際、形状モデル更新部２３は、例えば、S.Thrun他、「Probabilistic robotics」、MIT press、2005年、訳、確率ロボティクス、毎日コミュニケーションズ、2007年、の第9章に記載されている手法に従って、形状モデルmを更新すればよい。

さらに他の変形例によれば、物体検出部２１により複数の物体が検出された場合、制御部１３は、複数の物体のそれぞれごとに、物体追跡処理を実行すればよい。その際、制御部１３は、測距センサ２による最新の測距データにおいて複数の物体が検出されている場合、それ以前に追跡中の物体のそれぞれについて、軌跡から予測される最新の測距時点での追跡中の物体の位置のうち、着目する検出された物体の位置と最も近い位置となる追跡中の物体と着目する検出された物体とが同一であるとして、物体追跡処理を実行すればよい。また制御部１３は、追跡中の物体について、一定期間にわたって測距センサ２により検出されなくなると、追跡を終了すればよい。あるいは、制御部１３は、追跡中の物体の速度が所定の下限値（例えば、5km/h）以下である場合には、その追跡中の物体は静止物体であるとして、追跡を中止してもよい。

さらに他の変形例によれば、追跡対象物体の軌跡yは、直近の所定期間における個々の離散時間ごとの追跡対象物体の位置(x^k,y^k)、方位θ^k、速度v^k、角速度ω^k、加速度a^k、及び角加速度φ^kをそれぞれ互いに独立した要素として含んでいてもよい。この場合でも、軌跡更新部２２は、軌跡yを最尤推定する際、上記の要素の何れかを、所定の最適化手法に従って変更すればよい。またこの場合でも、隣接する二つの離散時間の間での加速度a^k及び角加速度φ^kの変化量が所定の上限値以下となるような制約が設けられてもよい。

上記の実施形態または変形例による物体追跡装置により求められた車両１０の周囲の物体の軌跡は、例えば、ＣＡＮを介して運転支援システムの制御回路（図示せず）へ送信されてもよい。運転支援システムの制御回路は、例えば、オドメトリ情報から算出される自車両の軌跡とその周囲の物体の軌跡とを比較して、車両１０とその周囲の物体とが所定距離範囲内に近づくと判定すると、車内に設置されたディスプレイまたはスピーカを介して、その周囲の物体と衝突する可能性があることをドライバに通知する。あるいは、運転支援システムの制御回路は、ＥＣＵに、衝突を回避するよう、車両１０の速度を変更したり、あるいは、車両１０の進行方向を変更させる命令を出力してもよい。

以上のように、当業者は、本発明の範囲内で、実施される形態に合わせて様々な変更を行うことができる。

１ 物体追跡装置
２ 測距センサ
３ 慣性計測装置
４ 車輪速センサ
５ 電子制御ユニット
６ コントローラエリアネットワーク
１０ 車両
１１ 記憶部
１２ 通信部
１３ 制御部
２１ 物体検出部
２２ 軌跡更新部
２３ 形状モデル更新部
２４ 収束判定部

Claims (6)

1. 車両（１０）に取り付けられた測距部（２）により得られた測距データから、前記車両（１０）の周囲に位置する追跡対象物体の位置を表す少なくとも一つの測距点を検出する物体検出部（２１）と、
   前記追跡対象物体上の位置ごとの前記測距点となる確からしさを表す形状モデルに従って、直近の所定期間中に得られた前記測距点のそれぞれについての前記追跡対象物体の当該所定期間における軌跡に基づいて求められる前記追跡対象物体を基準とする対象座標系における前記測距点の位置が前記追跡対象物体上となる確率の和が最大となるように前記軌跡及び前記形状モデルを最尤推定する物体追跡部（２２〜２４）と、
   を有する物体追跡装置。
2. 前記形状モデルは、前記追跡対象物体の進行方向と平行な方向における、前記測距点となる確からしさの標準偏差と、前記追跡対象物体の進行方向と直交する方向における位置ごとの前記測距点である確からしさとを含む、請求項１に記載の物体追跡装置。
3. 前記形状モデルは、前記追跡対象物体を２次元格子状の複数の区間ごとに分割したときの前記複数の区間のそれぞれごとの前記測距点となる確からしさを含む、請求項１に記載の物体追跡装置。
4. 前記物体追跡部（２２〜２４）は、前記確率の和が最大となるように前記形状モデルを変化させずに前記軌跡を推定することと、推定された前記軌跡に従って算出した前記対象座標系での前記所定期間中の前記測距点のそれぞれの位置における確からしさが高くなるように前記形状モデルを更新することとを交互に繰り返し、前記軌跡の更新が収束したときの前記確率の和の最大値に対応する前記軌跡を、前記所定の期間における前記追跡対象物体の軌跡とする、請求項１〜３の何れか一項に記載の物体追跡装置。
5. 車両（１０）に取り付けられた測距部（２）により得られた測距データから、前記車両（１０）の周囲に位置する追跡対象物体の位置を表す少なくとも一つの測距点を検出するステップと、
   前記追跡対象物体上の位置ごとの前記測距点となる確からしさを表す形状モデルに従って、直近の所定期間中に得られた前記測距点のそれぞれについての前記追跡対象物体の当該所定期間における軌跡に基づいて求められる前記追跡対象物体を基準とする対象座標系における前記測距点の位置が前記追跡対象物体上となる確率の和が最大となるように前記軌跡及び前記形状モデルを最尤推定するステップと、
   を含む物体追跡方法。
6. 車両（１０）に取り付けられた測距部（２）により得られた測距データから、前記車両（１０）の周囲に位置する追跡対象物体の位置を表す少なくとも一つの測距点を検出するステップと、
   前記追跡対象物体上の位置ごとの前記測距点となる確からしさを表す形状モデルに従って、直近の所定期間中に得られた前記測距点のそれぞれについての前記追跡対象物体の当該所定期間における軌跡に基づいて求められる前記追跡対象物体を基準とする対象座標系における前記測距点の位置が前記追跡対象物体上となる確率の和が最大となるように前記軌跡及び前記形状モデルを最尤推定するステップと、
   をコンピュータに実行させるための物体追跡用コンピュータプログラム。

Images