AutowareにおけるObject Trackingアルゴリズムの紹介

はじめまして、ティアフォーで自動運転のソフトウェア開発を行っている村上です。

今回のエントリでは自動運転技術のうち、周りの環境を認識する技術の一部を紹介していきます。

自動運転におけるPerceptionの役割

 安全で快適な自動運転を実現するためには、機械が人間と同じように周りの状況を認識し、それらの情報を元にして行動計画、そして車両制御をする必要があります。

 

https://cdn-ak.f.st-hatena.com/images/fotolife/k/kfunaoka/20181203/20181203163921.png

 

上の図におけるPerceptionにおいて、センサー情報から周囲の環境を認識するアルゴリズムが実装されています。

今回は周囲の障害物を認識するための要素に絞って進めていきます。
大きく分けて、Detection、Tracking、Predictionに分かれています。

  • Detection: センサー情報から障害物情報を検知
  • Tracking: Detectionの結果を時系列で処理することによって、障害物の状態を推定
  • Prediction: Tracking結果を元にして、将来の障害物の動きを予測


上記の動画はDetectionとTrackingを可視化したものになります。

障害物検知、それらのクラス分類をDetectorが担い、Trackingではそれらの速度や向きを推定して動画上で可視化しています。

そして、今回はTrackingのアルゴリズムの1つ IMM-UKF-PDA Tracker について紹介します。

IMM-UKF-PDA Tracker

IMMとUKF、PDAを組み合わせたアルゴリズムなので、このような名前になっており、
それぞれ何をしているかについてはこの後説明していきます。
参考文献*1 *2

各障害物の位置情報 (x, y) から速度、向き、動作モデルの推定を行います。

f:id:kosuke-murakami:20181225134810p:plain
UKF(Unscented Kalman Filter)

日本語では「無香料カルマンフィルタ」と呼ばれることもあります。

AutowareのObject TrackingではUKFを軸にして障害物の状態を推定します。

UKFでは具体的に各障害物の(x, y, velocity, yaw, \dot{yaw}) を推定しています。

 

問題となるのは、前提としている動作モデルが非線形モデルという点です。(動作モデルについてはIMMのパートで解説)

通常のカルマンフィルタでは非線形モデルを扱うことができません。なので非線形モデルを線形モデルに近似して扱う必要があります。

f:id:kosuke-murakami:20181225191450p:plain
この非線形モデルを線形化して扱うカルマンフィルタの手法の一つがUnscented Kalman Filterになります。

Unscented Transformと呼ばれる手法で点(シグマポイント)をサンプリングし、非線形モデルでそれらの点を伝播させます。そして伝播させた点から平均と分散を推定します。

f:id:kosuke-murakami:20181225191505p:plain

このような処理をすることで、非線形モデルの動作モデルに対しても、線形モデルのように変換後の平均と分散を推定することができます。

 

IMM(Interacting Multiple Model) 

IMMは一つの障害物に対して、複数の動作モデルを仮定し、確からしい動作モデルに対して重み付けをしてObject Trackingをする手法になります。

前提となる動作モデルはCV(Constant Velocity), CTRV(Constant Turn Rate and Velocity), Random Motionの3つになります。
それぞれ、まっすぐ進む、曲がりながら進む、動かない動作モデルを表現しています。

それぞれの更新式は以下のようになります。

 

Constant Velocity
\[
{x_{k+1}} = \left(
\begin{array}{c}
x_{k} + \upsilon_{k}\Delta t\sin\bigl(\psi_{k}\bigr) \\
y_{k} + \upsilon_{k}\Delta t\cos\bigl(\psi_{k}\bigr) \\
 \upsilon_{k} \\
\psi_{k} \\
\ 0
\end{array}
\right)
\]

 

Constant Turn Rate Velocity

\[
{x_{k+1}} = \left(
\begin{array}{c}
x_{k} + \upsilon_{k}/\dot{\psi_{k}}\bigl(\sin\bigl(\psi_{k} + \dot{\psi_{k}} \Delta t\bigr)-\sin\bigl(\psi_{k}\bigr)\bigr) \\
y_{k} + \upsilon_{k}/\dot{\psi_{k}}\bigl(-\cos\bigl(\psi_{k} + \dot{\psi_{k}} \Delta t\bigr)+\cos\bigl(\psi_{k}\bigr)\bigr) \\
 \upsilon_{k} \\
\psi_{k} +\Delta t\dot{\psi_{k}}\\
\ \dot{\psi_{k}}
\end{array}
\right)
\]

 

Random Motion

\[
{x_{k+1}} = \left(
\begin{array}{c}
x_{k} \\
y_{k}  \\
\ 0 \\
\psi_{k} \\
\ 0
\end{array}
\right)
\]

 \upsilon_{k}はkフレームにおけるvelocity.

 \psi_{k}, \dot{\psi_{k}}, はそれぞれkフレームにおけるyaw, yaw rateを表しています。

 

このように、一つの障害物に対して複数の動作モデルを仮定することで、障害物が複雑な動きをした場合にも対応できるようにしています。

PDA(Probabilistic Data Association)

Data Associationは追跡物体と観測データを関連付けるアルゴリズムを指します。
f:id:kosuke-murakami:20181225185200p:plain

 

例えば上記のようなシナリオを考えた時に、"track 1"と"track 2"に対して適切な観測データ(図の"observations")を割り当てる必要があります。

 

f:id:kosuke-murakami:20181225185915p:plain


Probabilistic Data Associationではカルマンフィルタによる予測値と観測データを比べて、確からしい観測データに対して重み付けをしてData Associationをしています。

参考文献*3


まとめ

Autowareでは論文を参考にして上記の要素技術を組み合わせたIMM-UKF-PDA Trackerを実装しています。


また最近はカルマンフィルタのパラメターをオートチューニングする*4やマップのレーン情報を使用してTrackingする*5なども追加で実装しています。機会があればこちらのアルゴリズムに関しても紹介していけたらと思います。

*1:Arya Senna Abdul Rachman. 2017. 3D-LIDAR Multi Object Tracking for Autonomous Driving. Master's thesis. Delft University of Technology, Delft, Netherlands

*2:M. Schreier. 2016. Bayesian environment representation, prediction, and criticality assessment for driver assistance systems. Ph.D. Dissertation. Technische Universität Darmstadt, Darmstadt, Germany.

*3:Robert Collins. 2012. Vision-Based Tracking. (November 2012). Retrieved November 7, 2012 from http://www.cse.psu.edu/~rtc12/CSE598C/datassocPart1.pdf

*4:Zheng B, Fu P, Li B, and Yuan X. 2018. A Robust Adaptive Unscented Kalman Filter for Nonlinear Estimation with Uncertain Noise Covariance. Sensors(Basel). DOI:10.3390/s18030808.

*5:D. Petrich, T. Dang, D. Kaspar, G. Breuel, and C. Stiller. 2013. Map-based Long Term Motion Prediction for Vehicles in Traffic Environments. International IEEE Conference on Intelligent Transportation Systems (ITSC). pp. 2166–2172.

はじめまして

はじめまして、1日目のエントリを書きました id:kfunaoka です。

株式会社ティアフォーは、2015年12月に設立されて3年目を迎える、自動運転システムや自動運転の運行管理を行うプラットフォームを開発している会社です。 まだ見ぬサービス実現に向けて、先端技術を駆使して技術的な課題を克服すべく、様々なチャレンジを行っています。

そんな取り組みを、これからこのブログを通してご紹介していければと考えています。

最初のエントリとなる今回は、弊社で力を入れている自動運転システム用オープンソースソフトウェア「Autoware」を紹介したいと思います。

続きを読む