本系列的前4篇文章主要介紹了深度學(xué)習(xí)技術(shù)在無人駕駛環(huán)境感知中的應(yīng)用，包括交通標(biāo)志識別，圖像與點云3D目標(biāo)檢測。關(guān)于環(huán)境感知部分的介紹本系列暫且告一段落，后續(xù)如有需要再進(jìn)行補(bǔ)充。
現(xiàn)在我們開啟新的篇章，在本文中將會介紹無人駕駛的定位部分，我們將使用仿真的Radar和LiDAR數(shù)據(jù)對自行車進(jìn)行追蹤。這里使用到了兩種傳感器數(shù)據(jù)，因此我們需要進(jìn)行數(shù)據(jù)融合，同時由于兩種傳感器工作原理不同，我們需要分別應(yīng)用卡爾曼濾波與擴(kuò)展卡爾曼濾波技術(shù)。
本文主要參考了Udacity《無人駕駛工程師》課程相關(guān)項目，同時也參考了知乎作者陳光的分享，在此一同表示感謝。

文章目錄

• • • 1. 毫米波雷達(dá)簡介
    • 2. 激光雷達(dá)簡介
    • 3. 卡爾曼濾波直觀理解
    • • 3.1 預(yù)測
      • 3.2 更新
    • 4. 傳感器數(shù)據(jù)融合
    • • 4.1 線性卡爾曼濾波實現(xiàn)
      • • （1）初始化
        • （2）預(yù)測
        • （3）更新
      • 4.2 非線性卡爾曼濾波實現(xiàn)
      • • （1）預(yù)測
        • （2）更新
      • 4.3 數(shù)據(jù)融合

1. 毫米波雷達(dá)簡介

毫米波雷達(dá)是自動駕駛汽車常用的傳感器之一，目前市場上常用的毫米波雷達(dá)有大陸ARS408雷達(dá)，測距范圍最遠(yuǎn)能達(dá)到250米。

毫米波雷達(dá)的工作原理是多普勒效應(yīng)，輸出值是極坐標(biāo)下的測量結(jié)果。

如下圖所示，毫米波雷達(dá)能夠測量障礙物在極坐標(biāo)下的距離$\rho$，方向夾角 $\phi$，以及徑向速度$\dot{\rho }$。

---

2. 激光雷達(dá)簡介

激光雷達(dá)與毫米波雷達(dá)不同，它的測量原理是光沿直線傳播。

能直接獲得障礙物在笛卡爾坐標(biāo)系下$x$方向、$y$方向和$z$方向上的距離。目前市場上常用的激光雷達(dá)有Velodyne激光雷達(dá)，國內(nèi)有Robosense激光雷達(dá)以及大疆公司旗下的激光雷達(dá)。激光雷達(dá)根據(jù)線束的多少，分為16線，32線，64線，以及128線激光雷達(dá)。

---

3. 卡爾曼濾波直觀理解

網(wǎng)上介紹卡爾曼濾波原理的書和資料有很多，這里從直觀上來對其進(jìn)行介紹，幫助大家理解，具體數(shù)學(xué)公式推導(dǎo)可查閱相關(guān)論文。

先一句話概括卡爾曼濾波的思想：根據(jù)上一時刻的狀態(tài)$x_{t?1}$，預(yù)測當(dāng)前時刻的狀態(tài)$x_{pre}$，將預(yù)測的狀態(tài)$x_{pre}$與當(dāng)前時刻的測量值$z_t$進(jìn)行加權(quán)更新，更新后的結(jié)果為最終的追蹤結(jié)果$x_t$。

以一個常見的小車運動為例來介紹。如下圖所示：有輛小車在道路上水平向右側(cè)勻速運動，在左側(cè)$o$點安裝了傳感器，傳感器每隔1秒測量一次小車的位置$s$和運動速度$v$。

這里用向量$x_t$來表示小車在$t$時刻的運動狀態(tài)，該向量$x_t$也是最終的輸出結(jié)果，被稱作狀態(tài)向量（state vector），則狀態(tài)向量為：
$$x_t=\left[\begin{array}{c}{s}_t\\ v_t\end{array}\right]$$

由于傳感器自身測量誤差的存在，無法直接獲取小車的真實位置，只能獲取在真值附近的一個近似值，可以假設(shè)測量值在真值附近服從高斯分布，如下圖所示，橙色部分為測量值。

在初始的時候，由于不存在上一時刻的狀態(tài)，我們設(shè)初始的狀態(tài)向量為測量值，因此有：

$$x_1=z_1=\left[\begin{array}{c}{s}_1\\ v_1\end{array}\right]$$

---

3.1 預(yù)測

卡爾曼濾波大致分為兩步，第一步為狀態(tài)預(yù)測（prediction）。現(xiàn)在我們已經(jīng)有了小車第1秒的狀態(tài)$x_1$，可以預(yù)測小車在第2秒的狀態(tài)，小車所處位置假設(shè)如下圖所示。

由于小車是做勻速運動，因此小車在第2秒時的預(yù)測狀態(tài)為：
$$x_{pre}=\left[\begin{array}{c}{s}_1+v_1\\ v_1\end{array}\right]$$

---

3.2 更新

現(xiàn)在我們已經(jīng)預(yù)測了小車在第2秒的狀態(tài)，同時傳感器也測量出小車在第2秒時的位置，測量結(jié)果用$z_2$表示，則：
$$z_2=\left[\begin{array}{c}{s}_2\\ v_2\end{array}\right]$$
由于傳感器本身存在著測量噪聲，測量結(jié)果存在很大不確定性，將小車預(yù)測位置與測量值進(jìn)行比較，如下圖所示。

不難發(fā)現(xiàn)，小車的真實位置應(yīng)該處于測量值與預(yù)測值之間。

對測量值與預(yù)測值進(jìn)行加權(quán)，加權(quán)后的結(jié)果如下圖所示，綠色部分即為小車真值分布區(qū)域。

這樣，根據(jù)第2秒的預(yù)測值和測量值，我們就能得到第2秒的狀態(tài)向量$x_2$。同理，按照上述預(yù)測、更新的過程，我們就能得到第3秒，第4秒…第n秒的狀態(tài)向量$x_n$。

上面是卡爾曼濾波的直觀理解，下面給出其數(shù)學(xué)公式。這里留個伏筆，在下一節(jié)部分=結(jié)合代碼再介紹其中每個字母的含義。

---

4. 傳感器數(shù)據(jù)融合

不知道大家有沒有想過這樣一個問題？既然毫米波雷達(dá)與激光雷達(dá)都能測量障礙物的位置，為什么還需要對傳感器數(shù)據(jù)進(jìn)行融合操作呢。

這是因為在自動駕駛中，使用單一傳感器進(jìn)行障礙物跟蹤，往往都有著很大的局限性。激光雷達(dá)測量更精準(zhǔn)，但是其無法得到速度信息；毫米波雷達(dá)能夠得到障礙物速度信息，但是其位置測量精度不如激光雷達(dá)。如果能將二者有效結(jié)合，就能精準(zhǔn)得到障礙物位置和速度信息，因此數(shù)據(jù)融合技術(shù)孕育而生。

在毫米波雷達(dá)與激光雷達(dá)進(jìn)行數(shù)據(jù)融合時，因為兩個傳感器工作原理的不同，其相應(yīng)的技術(shù)處理細(xì)節(jié)也不同。激光雷達(dá)可直接使用線性卡爾曼來進(jìn)行障礙物跟蹤；而毫米波雷達(dá)則需要使用非線性卡爾曼來進(jìn)行物體跟蹤。

這里分為三個小節(jié)來實現(xiàn)，一是線性卡爾曼濾波的實現(xiàn)；二是非線性卡爾曼濾波的實現(xiàn)；三是對二者進(jìn)行數(shù)據(jù)融合。

---

4.1 線性卡爾曼濾波實現(xiàn)

在正式寫代碼處理問題時，我們先熟悉下要處理的傳感器數(shù)據(jù)，這里要處理的是激光雷達(dá)與毫米波雷達(dá)數(shù)據(jù)。

下面是激光雷達(dá)與毫米波雷達(dá)交替發(fā)出的前20行數(shù)據(jù)，其中L代表激光雷達(dá)，R代表毫米波雷達(dá)。

L 0 0 1477010443349642 0 0 0 0 R 0 0 0 1477010443349642 0 0 0 0 L 1.559445e+00 -1.385015e-01 1477010444349642 2.098967e+00 5.222280e-02 2.195949e+00 1.093391e-01 R 1.812711e+00 2.619727e-02 2.305732e+00 1477010444349642 2.098967e+00 5.222280e-02 2.195949e+00 1.093391e-01 L 3.890927e+00 -1.341657e-01 1477010445349642 4.291359e+00 2.153118e-01 2.284336e+00 2.263225e-01 R 4.200967e+00 5.202598e-02 2.418127e+00 1477010445349642 4.291359e+00 2.153118e-01 2.284336e+00 2.263225e-01 L 6.863517e+00 4.168175e-01 1477010446349642 6.569422e+00 4.960956e-01 2.363816e+00 3.488471e-01 R 6.456604e+00 7.330529e-02 2.438466e+00 1477010446349642 6.569422e+00 4.960956e-01 2.363816e+00 3.488471e-01 L 9.077331e+00 5.932112e-01 1477010447349642 8.924371e+00 8.992403e-01 2.433593e+00 4.745258e-01 R 8.941596e+00 9.936074e-02 2.529122e+00 1477010447349642 8.924371e+00 8.992403e-01 2.433593e+00 4.745258e-01 L 1.157555e+01 1.666900e+00 1477010448349642 1.134677e+01 1.426997e+00 2.493282e+00 6.007812e-01 R 1.153365e+01 1.242681e-01 2.500995e+00 1477010448349642 1.134677e+01 1.426997e+00 2.493282e+00 6.007812e-01 L 1.359209e+01 2.311915e+00 1477010449349642 1.382695e+01 2.079045e+00 2.542900e+00 7.249468e-01 R 1.380271e+01 1.453491e-01 2.539724e+00 1477010449349642 1.382695e+01 2.079045e+00 2.542900e+00 7.249468e-01 L 1.664559e+01 2.902999e+00 1477010450349642 1.635537e+01 2.852433e+00 2.582842e+00 8.443653e-01 R 1.652890e+01 1.730753e-01 2.728349e+00 1477010450349642 1.635537e+01 2.852433e+00 2.582842e+00 8.443653e-01 L 1.901058e+01 3.705553e+00 1477010451349642 1.892299e+01 3.741610e+00 2.613820e+00 9.564796e-01 R 1.974624e+01 1.973267e-01 2.502012e+00 1477010451349642 1.892299e+01 3.741610e+00 2.613820e+00 9.564796e-01 L 2.133124e+01 4.732053e+00 1477010452349642 2.152152e+01 4.738551e+00 2.636790e+00 1.058912e+00 R 2.213645e+01 2.161499e-01 2.798219e+00 1477010452349642 2.152152e+01 4.738551e+00 2.636790e+00 1.058912e+00

激光雷達(dá)每一幀有7個數(shù)據(jù)，依次是：

• 障礙物在X方向上的測量值（單位：米）；
• Y方向上的測量值（單位：米）；
• 測量時刻的時間戳（單位：微秒）；
• 障礙物位置在X方向上的真值（單位：米）；
• 障礙物位置在Y方向上的真值（單位：米）；
• 障礙物速度在X方向上的真值（單位：米/秒）；
• 障礙物速度在Y方向上的真值（單位：米/秒）。

毫米波雷達(dá)每一幀有8個數(shù)據(jù)，依次是：

• 障礙物在極坐標(biāo)系下的距離（單位：米）；
• 角度（單位：弧度） ；
• 徑向速度（單位：米/秒）；
• 測量時刻的時間戳（單位：微秒）；
• 障礙物位置在X方向上的真值（單位：米）；
• 障礙物位置在Y方向上的真值（單位：米）；
• 障礙物速度在X方向上的真值（單位：米/秒）；
• 障礙物速度在Y方向上的真值（單位：米/秒）。

---

對數(shù)據(jù)有了一定了解后，現(xiàn)在我們開始實現(xiàn)線性卡爾曼濾波。

（1）初始化

首先是初始化部分，在這里要初始化卡爾曼濾波的各個變量。

這里使用Eigen庫進(jìn)行初始化，這里我們定義了一個KalmanFilter類，表示為卡爾曼濾波追蹤器，其成員函數(shù)包括初始化，預(yù)測，線性卡爾曼更新，擴(kuò)展卡爾曼更新等，這在面向?qū)ο缶幊讨惺浅Ｊ褂玫囊环N方法。

#ifndef KALMAN_FILTER_H_ #define KALMAN_FILTER_H_ #include "Eigen/Dense"class KalmanFilter { public:// state vectorEigen::VectorXd x_;// state covariance matrixEigen::MatrixXd P_;// state transistion matrixEigen::MatrixXd F_;// process covariance matrixEigen::MatrixXd Q_;// measurement matrixEigen::MatrixXd H_;// measurement covariance matrixEigen::MatrixXd R_;/*** Constructor*/KalmanFilter();/*** Destructor*/virtual ~KalmanFilter();/*** Init Initializes Kalman filter* @param x_in Initial state* @param P_in Initial state covariance* @param F_in Transition matrix* @param H_in Measurement matrix* @param R_in Measurement covariance matrix* @param Q_in Process covariance matrix*/void Init(Eigen::VectorXd &x_in, Eigen::MatrixXd &P_in, Eigen::MatrixXd &F_in,Eigen::MatrixXd &H_in, Eigen::MatrixXd &R_in, Eigen::MatrixXd &Q_in);/*** Prediction Predicts the state and the state covariance* using the process model* @param delta_T Time between k and k+1 in s*/void Predict();/*** Updates the state by using standard Kalman Filter equations* @param z The measurement at k+1*/void Update(const Eigen::VectorXd &z);/*** Updates the state by using Extended Kalman Filter equations* @param z The measurement at k+1*/void UpdateEKF(const Eigen::VectorXd &z);/*** General kalman filter update operations* @param y the update prediction error*/void UpdateRoutine(const Eigen::VectorXd &y);};#endif /* KALMAN_FILTER_H_ */

初始化時，需要初始化狀態(tài)向量$x$，狀態(tài)轉(zhuǎn)移矩陣$F$，狀態(tài)協(xié)方差矩陣$P$，測量矩陣$H$，過程協(xié)方差矩陣$Q$，測量協(xié)方差矩陣$R$，代碼如下：

void KalmanFilter::Init(VectorXd &x_in, MatrixXd &P_in, MatrixXd &F_in,MatrixXd &H_in, MatrixXd &R_in, MatrixXd &Q_in) {x_ = x_in;P_ = P_in;F_ = F_in;H_ = H_in;R_ = R_in;Q_ = Q_in; }

---

（2）預(yù)測

然后是預(yù)測部分，根據(jù)卡爾曼濾波原理，預(yù)測公式為：
$$x^{\prime }=Fx+u$$
這里的$x$為狀態(tài)向量，其乘以狀態(tài)轉(zhuǎn)移矩陣$F$，再加上外部影響$u$，即可得到預(yù)測狀態(tài)向量$x^{\prime }$。

對于激光雷達(dá)來說，其狀態(tài)向量$x$為4維向量，$x,y$方向上的位置和速度：
$$x=?\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}?$$
假設(shè)障礙物做勻速運動，則在經(jīng)過$\delta t$時間后，狀態(tài)向量為：
$$x^{\prime }=?\begin{array}{c}x+v_x?\delta t\\ y+v_y?\delta t\\ v_x\\ v_y\end{array}?$$
如果用矩陣表示的話，則狀態(tài)轉(zhuǎn)移公式為：
$$?\begin{array}{c}x+v_x?\delta t\\ y+v_y?\delta t\\ v_x\\ v_y\end{array}?=?\begin{array}{cccc}1& 0& \delta t& 0\\ 0& 1& 0& \delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}???\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}?$$
因此，狀態(tài)轉(zhuǎn)移矩陣為$(4,4)$的矩陣，我們可以先將$\delta t$定義為1，計算時再根據(jù)實際情況修改：

// Initial transition matrix F_ekf_.F_ = MatrixXd(4, 4);ekf_.F_ << 1, 0, 1, 0,0, 1, 0, 1,0, 0, 1, 0,0, 0, 0, 1;

然后是預(yù)測部分的第二個公式：
$$P^{\prime }=FP{F}^T+Q$$
在公式中，$P$為狀態(tài)協(xié)方差矩陣，表示系統(tǒng)的不確定性，開始時系統(tǒng)的不確定性會很大，但隨著輸入的數(shù)據(jù)越來越多，系統(tǒng)會趨于穩(wěn)定。這里還用到了過程協(xié)方差矩陣$Q$，這表示系統(tǒng)受外部環(huán)境影響的情況，如突然遇到爬坡或路面有凹坑的情況。

對于激光雷達(dá)來說，無法測量障礙物的速度，因此，測量位置的不確定性要小于速度不確定性。因此這里的$P$可以初始化為：

// Initialize state covariance matrix Pekf_.P_ = MatrixXd(4, 4);ekf_.P_ << 1, 0, 0, 0,0, 1, 0, 0,0, 0, 1000, 0,0, 0, 0, 1000;

$Q$代碼如下，這里也是可以調(diào)整的。

// Initialize process noise covariance matrixekf_.Q_ = MatrixXd(4, 4); ekf_.Q_ << 0, 0, 0, 0,0, 0, 0, 0,0, 0, 0, 0,0, 0, 0, 0;

這樣我們就完成了預(yù)測部分，最終我們寫為一個函數(shù)Predict()代碼如下：

void KalmanFilter::Predict() {// Predict the statex_ = F_ * x_;MatrixXd Ft = F_.transpose();P_ = F_ * P_ * Ft + Q_; }

---

（3）更新

最后是卡爾曼濾波的更新部分，更新部分第一個公式為：
$$y=z?H{x}^{\prime }$$
上式計算了測量值$z$與預(yù)測值$x^{\prime }$之間的差值$y$。

由于激光雷達(dá)測量值為$(x_m，y_m)$，與狀態(tài)向量維度不同。在與狀態(tài)向量進(jìn)行計算時，需要乘以一個測量矩陣$H$變?yōu)橥S的狀態(tài)量，上式用矩陣表示為：
$$\left[\begin{array}{c}\delta x\\ \delta y\end{array}\right]=\left[\begin{array}{c}{x}_m\\ y_m\end{array}\right]?\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]??\begin{array}{c}x+v_x?\delta t\\ y+v_y?\delta t\\ v_x\\ v_y\end{array}?$$
因此這里的測量矩陣$H$為：

// Lidar - measurement matrixH_laser_ = MatrixXd(2, 4);H_laser_ << 1, 0, 0, 0,0, 1, 0, 0;

然后是下面兩個公式：
$$\begin{gathered} S=H{P}^{\prime }{H}^T+R \\ K=P^{\prime }{H}^T{S}^{?1} \end{gathered}$$
這兩個公式是求卡爾曼增益$K$，實際就是$y$的權(quán)重。

最后還有兩個公式：
$$\begin{gathered} x=x^{\prime }+Ky \\ P=(I?KH)P^{\prime } \end{gathered}$$
這樣就得到了當(dāng)前幀的狀態(tài)向量與狀態(tài)協(xié)方差矩陣。然后根據(jù)新的狀態(tài)向量$x$和協(xié)方差矩陣$P$可以計算下一幀的狀態(tài)向量，如此反復(fù)。
這里定義測量協(xié)方差矩陣$R$為：

// Initialize measurement covariance matrix - laserR_laser_ = MatrixXd(2, 2);R_laser_ << 0.0225, 0,0, 0.0225;

至此，卡爾曼濾波中五個重要矩陣$F，P，Q，H，R$就已經(jīng)定義完了，更新部分代碼為：

void KalmanFilter::Update(const VectorXd &z) {VectorXd z_pred = H_ * x_;VectorXd y = z - z_pred;UpdateRoutine(y); }void KalmanFilter::UpdateRoutine(const VectorXd &y) {MatrixXd Ht = H_.transpose();MatrixXd S = H_ * P_ * Ht + R_;MatrixXd Si = S.inverse();// Compute Kalman gainMatrixXd K = P_ * Ht * Si;// Update estimatex_ = x_ + K * y;long x_size = x_.size();MatrixXd I = MatrixXd::Identity(x_size, x_size);P_ = (I - K * H_) * P_; }

至此，線性卡爾曼濾波部分就已經(jīng)完成了，下一節(jié)我們實現(xiàn)非線性卡爾曼濾波。

---

4.2 非線性卡爾曼濾波實現(xiàn)

非線性卡爾曼濾波是用于解決非線性問題，與線性卡爾曼濾波相同，非線性卡爾曼濾波也分為初始化、預(yù)測、更新三部分。

初始化與線性卡爾曼濾波相似，我們?nèi)匀皇褂肒almanFilter類構(gòu)造一個追蹤器。

（1）預(yù)測

這里再回顧下卡爾曼濾波預(yù)測中的兩個公式：
$$\begin{gathered} x^{\prime }=Fx+u \\ {P}^{\prime }=FP{F}^T+Q \end{gathered}$$
這里仍然使用第一次測量值作為初始狀態(tài)，狀態(tài)轉(zhuǎn)移矩陣為$F$，狀態(tài)協(xié)方差矩陣為$P$，過程協(xié)方差矩陣為$Q$，與上一節(jié)的初始化相同。

---

（2）更新

回顧上一節(jié)，更新部分第一個公式為：
$$y=z?H{x}^{\prime }$$
但是這里我們使用的是毫米波雷達(dá)數(shù)據(jù)，測量得到的數(shù)據(jù)為：距離$\rho$，方向夾角 $\phi$，以及徑向速度$\dot{\rho }$。

這里得到的信息為極坐標(biāo)信息，需要轉(zhuǎn)換為直角坐標(biāo)距離。我們將上式寫成矩陣形式為：
$$y=?\begin{array}{c}\rho \\ \\ \phi \\ \\ \dot{\rho }\end{array}???\begin{array}{c}\sqrt{{\rho }_x^2+{\rho }_y^2}\\ \\ arctan(\frac{{\rho }_y}{{\rho }_x})\\ \\ \frac{{\rho }_x{v}_x{\rho }_y{v}_y}{\sqrt{{\rho }_x^2+{\rho }_y^2}}\end{array}?$$

其中，${\rho }_x，{\rho }_y$為預(yù)測后的位置，$v_x，v_y$為預(yù)測后的速度。這里的位置轉(zhuǎn)換，速度轉(zhuǎn)換為非線性轉(zhuǎn)換。

然后是卡爾曼濾波的后面兩個公式：
$$\begin{gathered} S=H{P}^{\prime }{H}^T+R \\ K=P^{\prime }{H}^T{S}^{?1} \end{gathered}$$

這里在求卡爾曼增益時，需要知道測量矩陣$H$，因為我們測量數(shù)據(jù)為$(3,1)$的列向量，而狀態(tài)向量為$(4,1)$的列向量。因此測量矩陣的維度為$(3,4)$。我們可以寫成下面這個形式：
$$H{x}^{\prime }=?\begin{array}{c}\sqrt{{\rho }_x^2+{\rho }_y^2}\\ \\ arctan(\frac{{\rho }_y}{{\rho }_x})\\ \\ \frac{{\rho }_x{v}_x{\rho }_y{v}_y}{\sqrt{{\rho }_x^2+{\rho }_y^2}}\end{array}?=?\begin{array}{cccc}{H}_{00}& H_{01}& H_{02}& H_{03}\\ H_{10}& H_{11}& H_{12}& H_{13}\\ H_{20}& H_{21}& H_{22}& H_{23}\end{array}???\begin{array}{c}{\rho }_x\\ {\rho }_y\\ v_x\\ v_y\end{array}?$$
顯然，上式兩邊轉(zhuǎn)化為非線性轉(zhuǎn)換，那么如何才能將非線性函數(shù)用近似線性函數(shù)表達(dá)呢？

這里我們使用泰勒公式來近似，近似后的泰勒公式為：
$$h(x)\approx h(x_0)+\frac{?h(x_0)}{?x}(x?x_0)$$
這里忽略了二次以上的高階項。對$x$求導(dǎo)后的項為雅克比公式。這里，雅克比公式就是我們的測量矩陣$H$。

最終測量矩陣為：
$$H=?\begin{array}{cccc}\frac{p_x}{\sqrt{{\rho }_x^2+{\rho }_y^2}}& \frac{p_y}{\sqrt{{\rho }_x^2+{\rho }_y^2}}& 0& 0\\ & & & \\ \frac{?p_y}{{\rho }_x^2+{\rho }_y^2}& \frac{?p_x}{{\rho }_x^2+{\rho }_y^2}& 0& 0\\ & & & \\ \frac{p_y(v_x{\rho }_y?v_y{\rho }_x)}{({\rho }_x^2+{\rho }_y^2{)}^{3/2}}& \frac{p_x(v_y{\rho }_x?v_x{\rho }_y)}{({\rho }_x^2+{\rho }_y^2{)}^{3/2}}& \frac{p_x}{\sqrt{{\rho }_x^2+{\rho }_y^2}}& \frac{p_y}{\sqrt{{\rho }_x^2+{\rho }_y^2}}\end{array}?$$
這里對測量矩陣進(jìn)行初始化$H$：

// Radar - jacobian matrixHj_ = MatrixXd(3, 4);Hj_ << 0, 0, 0, 0,0, 0, 0, 0,0, 0, 0, 0;

測量噪聲協(xié)方差矩陣$R$為：

//measurement covariance matrix - radarR_radar_ = MatrixXd(3, 3);R_radar_ << 0.09, 0, 0,0, 0.0009, 0,0, 0, 0.09;

下面是雅克比公式計算函數(shù)：

MatrixXd Tools::CalculateJacobian(const VectorXd& x_state) {MatrixXd Hj(3, 4);// Unroll state parametersfloat px = x_state(0);float py = x_state(1);float vx = x_state(2);float vy = x_state(3);// Pre-compute some term which recur in the Jacobianfloat c1 = px * px + py * py;float c2 = sqrt(c1);float c3 = c1 * c2;// Sanity check to avoid division by zeroif (std::abs(c1) < 0.0001) {std::cout << "Error in CalculateJacobian. Division by zero." << std::endl;return Hj;}// Actually compute Jacobian matrixHj << (px / c2), (py / c2), 0, 0,-(py / c1), (px / c1), 0, 0,py * (vx*py - vy*px) / c3, px * (vy*px - vx*py) / c3, px / c2, py / c2;return Hj;}

最后還有兩個公式：
$$\begin{gathered} x=x^{\prime }+Ky \\ P=(I?KH)P^{\prime } \end{gathered}$$
最終得到更新后的狀態(tài)向量$x$，以及新的狀態(tài)協(xié)方差矩陣$P$。

至此，非線性卡爾曼濾波部分就已經(jīng)完成了，重點是測量矩陣H的計算。

---

4.3 數(shù)據(jù)融合

完成了毫米波雷達(dá)與激光雷達(dá)的預(yù)測與更新，現(xiàn)在是對二者進(jìn)行融合。

下圖所示為毫米波雷達(dá)與激光雷達(dá)數(shù)據(jù)融合的整體框架。
首先是讀入傳感器數(shù)據(jù)，選擇第一幀作為初始值。對于之后的幀，進(jìn)行狀態(tài)預(yù)測，然后根據(jù)傳感器類型進(jìn)行更新，最后得到跟蹤后的狀態(tài)向量。

這里定義了一個數(shù)據(jù)融合類FusionEKF。

class FusionEKF {public:/* Constructor. */FusionEKF();/* Destructor. */virtual ~FusionEKF();/* Run the whole flow of the Kalman Filter from here. */void ProcessMeasurement(const MeasurementPackage &measurement_pack);/* Kalman Filter update and prediction math lives in here. */KalmanFilter ekf_;private:// check whether the tracking toolbox was initialized or not (first measurement)bool is_initialized_;// previous timestamplong long previous_timestamp_;// tool object used to compute Jacobian and RMSETools tools;Eigen::MatrixXd R_laser_;Eigen::MatrixXd R_radar_;Eigen::MatrixXd H_laser_;Eigen::MatrixXd Hj_;float noise_ax;float noise_ay; };

上面有一個很重要的處理函數(shù)ProcessMeasurement()，是對上圖數(shù)據(jù)融合的代碼實現(xiàn)。

void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {/****************************************************************************** Initialization****************************************************************************/if (!is_initialized_){if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {// Extract values from measurementfloat rho = measurement_pack.raw_measurements_(0);float phi = measurement_pack.raw_measurements_(1);float rho_dot = measurement_pack.raw_measurements_(2);// Convert from polar to cartesian coordinatesfloat px = rho * cos(phi);float py = rho * sin(phi);float vx = rho_dot * cos(phi);float vy = rho_dot * sin(phi);// Initialize stateekf_.x_ << px, py, vx, vy;}else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {// Extract values from measurementfloat px = measurement_pack.raw_measurements_(0);float py = measurement_pack.raw_measurements_(1);// Initialize stateekf_.x_ << px, py, 0.0, 0.0;}// Update last measurementprevious_timestamp_ = measurement_pack.timestamp_;// Done initializing, no need to predict or updateis_initialized_ = true;return;}/****************************************************************************** Prediction****************************************************************************/// Compute elapsed time from last measurementfloat dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0;// Update last measurementprevious_timestamp_ = measurement_pack.timestamp_;// Update state transition matrix F (according to elapsed time dt)ekf_.F_(0, 2) = dt;ekf_.F_(1, 3) = dt;// Compute process noise covariance matrixfloat dt_2 = dt * dt;float dt_3 = dt_2 * dt;float dt_4 = dt_3 * dt;ekf_.Q_ << dt_4 / 4 * noise_ax, 0, dt_3 / 2 * noise_ax, 0,0, dt_4 / 4 * noise_ay, 0, dt_3 / 2 * noise_ay,dt_3 / 2 * noise_ax, 0, dt_2 * noise_ax, 0,0, dt_3 / 2 * noise_ay, 0, dt_2 * noise_ay;ekf_.Predict();/****************************************************************************** Update****************************************************************************/if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {// Radar updatesekf_.R_ = R_radar_;ekf_.H_ = tools.CalculateJacobian(ekf_.x_);ekf_.UpdateEKF(measurement_pack.raw_measurements_);} else {// Laser updatesekf_.R_ = R_laser_;ekf_.H_ = H_laser_;ekf_.Update(measurement_pack.raw_measurements_);}// print the outputcout << "x_ = " << ekf_.x_ << endl;cout << "P_ = " << ekf_.P_ << endl; }

至此，激光雷達(dá)與毫米波雷達(dá)融合部分就已經(jīng)完成了。

而在實際開發(fā)時，一輛ADAS汽車通常會安裝多個毫米波雷達(dá)，包括前雷達(dá)，后雷達(dá)，角雷達(dá)；有的還會裝有激光雷達(dá)；本文沒有考慮相機(jī)，而相機(jī)是ADAS汽車最常使用的傳感器。在對這些傳感器進(jìn)行數(shù)據(jù)融合時還需要考慮時間同步與空間同步問題。

以上只是對汽車外部的障礙物進(jìn)行定位，而如果汽車想對自身進(jìn)行定位的話，通常需要安裝慣性測量單元IMU和GPS傳感器，在下一篇博客中將會進(jìn)行介紹。

創(chuàng)作挑戰(zhàn)賽新人創(chuàng)作獎勵來咯，堅持創(chuàng)作打卡瓜分現(xiàn)金大獎

總結(jié)

以上是生活随笔為你收集整理的动手学无人驾驶（5）：多传感器数据融合的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。