はじめまして、ティアフォーで自動運転のソフトウェア開発を行っている村上です。
今回のエントリでは自動運転技術のうち、周りの環境を認識する技術の一部を紹介していきます。
自動運転におけるPerceptionの役割
安全で快適な自動運転を実現するためには、機械が人間と同じように周りの状況を認識し、それらの情報を元にして行動計画、そして車両制御をする必要があります。
上の図におけるPerceptionにおいて、センサー情報から周囲の環境を認識するアルゴリズムが実装されています。
今回は周囲の障害物を認識するための要素に絞って進めていきます。
大きく分けて、Detection、Tracking、Predictionに分かれています。
- Detection: センサー情報から障害物情報を検知
- Tracking: Detectionの結果を時系列で処理することによって、障害物の状態を推定
- Prediction: Tracking結果を元にして、将来の障害物の動きを予測
上記の動画はDetectionとTrackingを可視化したものになります。
障害物検知、それらのクラス分類をDetectorが担い、Trackingではそれらの速度や向きを推定して動画上で可視化しています。
そして、今回はTrackingのアルゴリズムの１つ IMM-UKF-PDA Tracker について紹介します。
IMM-UKF-PDA Tracker
IMMとUKF、PDAを組み合わせたアルゴリズムなので、このような名前になっており、
それぞれ何をしているかについてはこの後説明していきます。
参考文献*1 *2
各障害物の位置情報 から速度、向き、動作モデルの推定を行います。
UKF(Unscented Kalman Filter)
日本語では「無香料カルマンフィルタ」と呼ばれることもあります。
AutowareのObject TrackingではUKFを軸にして障害物の状態を推定します。
UKFでは具体的に各障害物の を推定しています。
問題となるのは、前提としている動作モデルが非線形モデルという点です。（動作モデルについてはIMMのパートで解説）
通常のカルマンフィルタでは非線形モデルを扱うことができません。なので非線形モデルを線形モデルに近似して扱う必要があります。
この非線形モデルを線形化して扱うカルマンフィルタの手法の一つがUnscented Kalman Filterになります。
Unscented Transformと呼ばれる手法で点（シグマポイント）をサンプリングし、非線形モデルでそれらの点を伝播させます。そして伝播させた点から平均と分散を推定します。
このような処理をすることで、非線形モデルの動作モデルに対しても、線形モデルのように変換後の平均と分散を推定することができます。
IMM(Interacting Multiple Model)
IMMは一つの障害物に対して、複数の動作モデルを仮定し、確からしい動作モデルに対して重み付けをしてObject Trackingをする手法になります。
前提となる動作モデルはCV(Constant Velocity), CTRV(Constant Turn Rate and Velocity), Random Motionの３つになります。
それぞれ、まっすぐ進む、曲がりながら進む、動かない動作モデルを表現しています。
それぞれの更新式は以下のようになります。
{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)
\]
\[
{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)
\]
\[
{x_{k+1}} = \left(
\begin{array}{c}
x_{k} \\
y_{k} \\
\ 0 \\
\psi_{k} \\
\ 0
\end{array}
\right)
\]
はkフレームにおけるvelocity.
, はそれぞれkフレームにおけるyaw, yaw rateを表しています。
このように、一つの障害物に対して複数の動作モデルを仮定することで、障害物が複雑な動きをした場合にも対応できるようにしています。
PDA(Probabilistic Data Association)
Data Associationは追跡物体と観測データを関連付けるアルゴリズムを指します。
例えば上記のようなシナリオを考えた時に、"track 1"と"track 2"に対して適切な観測データ（図の"observations"）を割り当てる必要があります。
Probabilistic Data Associationではカルマンフィルタによる予測値と観測データを比べて、確からしい観測データに対して重み付けをしてData Associationをしています。
参考文献*3
まとめ
Autowareでは論文を参考にして上記の要素技術を組み合わせたIMM-UKF-PDA Trackerを実装しています。
また最近はカルマンフィルタのパラメターをオートチューニングする*4やマップのレーン情報を使用してTrackingする*5なども追加で実装しています。機会があればこちらのアルゴリズムに関しても紹介していけたらと思います。
*1:Arya Senna Abdul Rachman. 2017. 3D-LIDAR
*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.
