本文做者:Apollo開發者社區javascript
多傳感器融合又稱多傳感器信息融合(multi-sensor information fusion),有時也稱多傳感器數據融合(multi-sensor date fusion)。就是利用計算機技術未來自多傳感器或多源的信息和數據,在必定的準則下加以自動分析和綜合,以完成所須要的決策和估計而進行的信息處理過程。java
多傳感器信息融合技術的基本原理就像人的大腦綜合處理信息的過程同樣,將各類傳感器進行多層次、多空間的信息互補和優化組合處理,最終產生對觀測環境的一致性解釋。在這個過程當中要充分地利用多源數據進行合理支配與使用,而信息融合的最終目標則是基於各傳感器得到的分離觀測信息,經過對信息多級別、多方面組合導出更多有用信息。這不只是利用了多個傳感器相互協同操做的優點,並且也綜合處理了其它信息源的數據來提升整個傳感器系統的智能化。算法
雖然自動駕駛在全球範圍內已掀起浪潮,可是從技術方面而言依然存在挑戰。目前自動駕駛的痛點在於穩定可靠的感知及認知,包括清晰的視覺、優質的算法、多傳感器融合以及高效強大的運算能力。據分析,由自動駕駛引起的安全事故緣由中,相關傳感器的可能誤判也成爲了主要緣由之一。多個傳感器信息融合、綜合判斷無疑成爲提高自動駕駛安全性及賦能車輛環境感知的新趨勢。編程
本文由百度Apollo智能汽車事業部 自動駕駛工程師——陳光撰寫,對實現多傳感器融合技術進行了詳細講解,但願感興趣的開發者能經過閱讀這篇文章有所收穫。安全
如下,ENJOY 微信
1、前言網絡
在《開發者說 | 手把手教你寫卡爾曼濾波器》的分享中,以激光雷達的數據爲例介紹了卡爾曼濾波器(KF)的七個公式,並用C++代碼實現了激光雷達障礙物的跟蹤問題;在《開發者說 | 手把手教你寫擴展卡爾曼濾波器》的分享中,以毫米波雷達的數據爲例,介紹了擴展卡爾曼濾波器(EKF)是如何處理非線性問題的。數據結構
不管是卡爾曼濾波器處理線性問題仍是擴展卡爾曼濾波器處理非線性問題,它們都只涉及到單一傳感器的障礙物跟蹤。單一傳感器有其侷限性,好比激光雷達測量位置更準,但沒法測量速度;毫米波雷達測量速度準確,但在位置測量的精度上低於激光雷達。爲了充分利用各個傳感器的優點,多傳感器融合的技術應運而生。框架
因爲多傳感器融合爲KF和EKF的進階內容,推薦讀者先閱讀《開發者說 | 手把手教你寫卡爾曼濾波器》和《開發者說 | 手把手教你寫擴展卡爾曼濾波器》,瞭解卡爾曼濾波器的基本理論。ide
本文將以激光雷達和毫米波雷達檢測同一障礙物時的數據爲例,進行多傳感器融合技術的講解。同時,會提供《優達學城(Udacity)無人駕駛工程師學位》中所使用的多傳感器數據,並提供讀取數據的代碼,方便你們學習。
多傳感器融合的輸入數據可經過如下連接獲取。
連接:
https://pan.baidu.com/s/1zU9_SgZkMfs75_sXt2Odzw
提取碼:umb8
下載代碼並解壓後,可在data目錄下發現一個名爲sample-laser-radar-measurement-data-2.txt的文件,這就是傳感器融合的輸入數據。數據的測量場景是將檢測障礙物頻率相同的激光雷達和毫米波雷達固定在原點(0,0)處,隨後兩個雷達交替觸發,對同一運動的障礙物進行檢測。
障礙物的運動軌跡以下圖所示,圖中綠線爲障礙物運動的真實運動軌跡(Ground Truth),橙色的點表示的是多傳感器的檢測結果。
▲障礙物的運動軌跡
因爲激光雷達和毫米波雷達是交替觸發檢測的,所以系統收到的傳感器數據也是交替的。這裏的數據除了提供傳感器的測量值外,還提供了障礙物位置、速度的真值,真值可用於評估融合算法的好壞。前十幀(行)數據以下圖所示:
每幀數據的第一個字母表示該數據來自於哪個傳感器,L是Lidar的縮寫,R是Radar的縮寫。
Lidar只可以測量位置,字母L以後的數據依次爲障礙物在X方向上的測量值(單位:米),Y方向上的測量值(單位:米),測量時刻的時間戳(單位:微秒);障礙物位置在X方向上的真值(單位:米),障礙物位置在Y方向上的真值(單位:米);障礙物速度在X方向上的真值(單位:米/秒),障礙物速度在Y方向上的真值(單位:米/秒)。
Radar可以測量徑向距離、角度和速度,字母R以後的數據依次爲障礙物在極座標系下的距離(單位:米),角度(單位:弧度),鏡像速度(單位:米/秒),測量時刻的時間戳(單位:微秒);障礙物位置在X方向上的真值(單位:米),障礙物位置在Y方向上的真值(單位:米);障礙物速度在X方向上的真值(單位:米/秒),障礙物速度在Y方向上的真值(單位:米/秒)。
使用以上規則對輸入數據進行解析,有些相似於無人駕駛技術中的驅動層。sample-laser-radar-measurement-data-2.txt就像傳感器經過CAN或以太網發來的數據,這裏的解析規則就像解析CAN或網絡數據時所用的協議。
編程時,我先讀取了sample-laser-radar-measurement-data-2.txt文件,將每一行數據按照「協議」解析後,將觀測值轉存在結構體MeasurementPackage內,將真值轉存在結構體GroundTruthPackage內。隨後再用一個循環對這些數據進行遍歷,將它們一幀幀地輸入到算法中,具體代碼能夠參看main.cpp函數。
MeasurementPackage的內部構造以下所示:
//@ filename: /interface/measurement_package.h #ifndef MEASUREMENT_PACKAGE_H_ #define MEASUREMENT_PACKAGE_H_ #include "Eigen/Dense" class MeasurementPackage { public: long long timestamp_; enum SensorType{ LASER, RADAR } sensor_type_; Eigen::VectorXd raw_measurements_; }; #endif /* MEASUREMENT_PACKAGE_H_ */
GroundTruthPackage的內部構造以下所示:
//@ filename: /interface/ground_truth_package.h #ifndef GROUND_TRUTH_PACKAGE_H_ #define GROUND_TRUTH_PACKAGE_H_ #include "Eigen/Dense" class GroundTruthPackage { public: long timestamp_; enum SensorType{ LASER, RADAR } sensor_type_; Eigen::VectorXd gt_values_; }; #endif /* GROUND_TRUTH_PACKAGE_H_ */
因爲激光雷達和毫米波雷達的傳感器數據有所差別,所以算法處理時也有所不一樣,先羅列出一個初級的融合算法框架,以下圖所示。
▲圖片出處:優達學城(Udacity)無人駕駛工程師學位
首先讀入傳感器數據,若是是第一次讀入,則須要對卡爾曼濾波器的各個矩陣進行初始化操做;若是不是第一次讀入,證實卡爾曼濾波器已完成初始化,直接進行狀態預測和狀態值更新的步驟;最後輸出融合後的障礙物位置、速度。
咱們將以上過程寫成僞代碼,方便咱們理解隨後的開發工做。
▲傳感器融合僞代碼
經過對比《開發者說 | 手把手教你寫卡爾曼濾波器》和《開發者說 | 手把手教你寫擴展卡爾曼濾波器》中的KF與EKF的代碼會發現,對於勻速運動模型,KF和EKF的狀態預測(Predict)過程是同樣的;KF和EKF惟一區別的地方在於測量值更新(Update)這一步。在測量值更新中,KF使用的測量矩陣H是不變的,而EKF的測量矩陣H是Jacobian矩陣。
所以,咱們能夠將KF和EKF寫成同一個類,這個類中有兩個Update函數,分別命名爲KFUpdate和EKFUpdate。這兩個函數對應着僞代碼中第12行的「卡爾曼濾波更新」和第15行的「擴展卡爾曼濾波更新」。
根據卡爾曼濾波器的預測和測量值更新這兩大過程,咱們將算法邏輯細化,以下圖所示。
▲圖片出處:優達學城(Udacity)無人駕駛工程師學位
接下來咱們合併KF和EKF的跟蹤算法的代碼,將他們封裝在一個名爲KalmanFilter的類中,方便後續調用,改寫後的KalmanFilter類以下所示:
//@ filename: /algorithims/kalman.h #pragma once #include "Eigen/Dense" class KalmanFilter { public: KalmanFilter(); ~KalmanFilter(); void Initialization(Eigen::VectorXd x_in); bool IsInitialized(); void SetF(Eigen::MatrixXd F_in); void SetP(Eigen::MatrixXd P_in); void SetQ(Eigen::MatrixXd Q_in); void SetH(Eigen::MatrixXd H_in); void SetR(Eigen::MatrixXd R_in); void Prediction(); void KFUpdate(Eigen::VectorXd z); void EKFUpdate(Eigen::VectorXd z); Eigen::VectorXd GetX(); private: void CalculateJacobianMatrix(); // flag of initialization bool is_initialized_; // state vector Eigen::VectorXd x_; // state covariance matrix Eigen::MatrixXd P_; // state transistion matrix Eigen::MatrixXd F_; // process covariance matrix Eigen::MatrixXd Q_; // measurement matrix Eigen::MatrixXd H_; // measurement covariance matrix Eigen::MatrixXd R_; };
//@ filename: /algorithims/kalman.cpp #include "kalmanfilter.h" KalmanFilter::KalmanFilter() { is_initialized_ = false; } KalmanFilter::~KalmanFilter() { } void KalmanFilter::Initialization(Eigen::VectorXd x_in) { x_ = x_in; } bool KalmanFilter::IsInitialized() { return is_initialized_; } void KalmanFilter::SetF(Eigen::MatrixXd F_in) { F_ = F_in; } void KalmanFilter::SetP(Eigen::MatrixXd P_in) { P_ = P_in; } void KalmanFilter::SetQ(Eigen::MatrixXd Q_in) { Q_ = Q_in; } void KalmanFilter::SetH(Eigen::MatrixXd H_in) { H_ = H_in; } void KalmanFilter::SetR(Eigen::MatrixXd R_in) { R_ = R_in; } void KalmanFilter::Prediction() { x_ = F_ * x_; Eigen::MatrixXd Ft = F_.transpose(); P_ = F_ * P_ * Ft + Q_; } void KalmanFilter::KFUpdate(Eigen::VectorXd z) { Eigen::VectorXd y = z - H_ * x_; Eigen::MatrixXd Ht = H_.transpose(); Eigen::MatrixXd S = H_ * P_ * Ht + R_; Eigen::MatrixXd Si = S.inverse(); Eigen::MatrixXd K = P_ * Ht * Si; x_ = x_ + (K * y); int x_size = x_.size(); Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size); P_ = (I - K * H_) * P_; } void KalmanFilter::EKFUpdate(Eigen::VectorXd z) { double rho = sqrt(x_(0)*x_(0) + x_(1)*x_(1)); double theta = atan2(x_(1), x_(0)); double rho_dot = (x_(0)*x_(2) + x_(1)*x_(3)) / rho; Eigen::VectorXd h = Eigen::VectorXd(3); h << rho, theta, rho_dot; Eigen::VectorXd y = z - h; CalculateJacobianMatrix(); Eigen::MatrixXd Ht = H_.transpose(); Eigen::MatrixXd S = H_ * P_ * Ht + R_; Eigen::MatrixXd Si = S.inverse(); Eigen::MatrixXd K = P_ * Ht * Si; x_ = x_ + (K * y); int x_size = x_.size(); Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size); P_ = (I - K * H_) * P_; } Eigen::VectorXd KalmanFilter::GetX() { return x_; } void KalmanFilter::CalculateJacobianMatrix() { Eigen::MatrixXd Hj(3, 4); // get state parameters float px = x_(0); float py = x_(1); float vx = x_(2); float vy = x_(3); // pre-compute a set of terms to avoid repeated calculation float c1 = px * px + py * py; float c2 = sqrt(c1); float c3 = (c1 * c2); // Check division by zero if(fabs(c1) < 0.0001){ H_ = Hj; return; } Hj << (px/c2), (py/c2), 0, 0, -(py/c1), (px/c1), 0, 0, py*(vx*py - vy*px)/c3, px*(px*vy - py*vx)/c3, px/c2, py/c2; H_ = Hj; }
KalmanFilter類封裝了卡爾曼濾波的七個公式,專門用於實現障礙物的跟蹤。爲了使代碼儘量解耦且結構清晰,咱們新建一個名爲SensorFusion的類。這個類的做用是將數據層和算法層隔離開。即便外部傳入的數據結構(接口)發生變化,修改SensorFusion類便可完成數據的適配,而不用修改KalmanFilter算法部分的代碼,加強算法的複用性。
在SensorFusion類中添加一個函數Process()用於傳入觀測值,並在函數Process()中調用算法。以下圖所示,在k+1時刻收到激光雷達數據時,根據k時刻的狀態完成一次預測,再根據k+1時刻的激光雷達的觀測數據實現測量值更新(KFUpdate);在k+2時刻收到毫米波雷達的數據時,根據k+1時刻的狀態完成一次預測,再根據k+2時刻的毫米波雷達的觀測數據實現測量值更新(EKFUpdate)。
▲圖片出處:優達學城(Udacity)無人駕駛工程師學位
將以上過程轉換爲代碼,在SensorFusion中實現,以下所示:
//@ filename: /algorithims/sensorfusion.h #pragma once #include "interface/measurement_package.h" #include "kalmanfilter.h" class SensorFusion { public: SensorFusion(); ~SensorFusion(); void Process(MeasurementPackage measurement_pack); KalmanFilter kf_; private: bool is_initialized_; long last_timestamp_; Eigen::MatrixXd R_lidar_; Eigen::MatrixXd R_radar_; Eigen::MatrixXd H_lidar_; };
//@ filename: /algorithims/sensorfusion.cpp #include "sensorfusion.h" SensorFusion::SensorFusion() { is_initialized_ = false; last_timestamp_ = 0.0; // 初始化激光雷達的測量矩陣 H_lidar_ // Set Lidar's measurement matrix H_lidar_ H_lidar_ = Eigen::MatrixXd(2, 4); H_lidar_ << 1, 0, 0, 0, 0, 1, 0, 0; // 設置傳感器的測量噪聲矩陣,通常由傳感器廠商提供,如未提供,也可經過有經驗的工程師調試獲得 // Set R. R is provided by Sensor supplier, in sensor datasheet // set measurement covariance matrix R_lidar_ = Eigen::MatrixXd(2, 2); R_lidar_ << 0.0225, 0, 0, 0.0225; // Measurement covariance matrix - radar R_radar_ = Eigen::MatrixXd(3, 3); R_radar_ << 0.09, 0, 0, 0, 0.0009, 0, 0, 0, 0.09; } SensorFusion::~SensorFusion() { } void SensorFusion::Process(MeasurementPackage measurement_pack) { // 第一幀數據用於初始化 Kalman 濾波器 if (!is_initialized_) { Eigen::Vector4d x; if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) { // 若是第一幀數據是激光雷達數據,沒有速度信息,所以初始化時只能傳入位置,速度設置爲0 x << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0; } else if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) { // 若是第一幀數據是毫米波雷達,能夠經過三角函數算出x-y座標系下的位置和速度 float rho = measurement_pack.raw_measurements_[0]; float phi = measurement_pack.raw_measurements_[1]; float rho_dot = measurement_pack.raw_measurements_[2]; float position_x = rho * cos(phi); if (position_x < 0.0001) { position_x = 0.0001; } float position_y = rho * sin(phi); if (position_y < 0.0001) { position_y = 0.0001; } float velocity_x = rho_dot * cos(phi); float velocity_y = rho_dot * sin(phi); x << position_x, position_y, velocity_x , velocity_y; } // 避免運算時,0做爲被除數 if (fabs(x(0)) < 0.001) { x(0) = 0.001; } if (fabs(x(1)) < 0.001) { x(1) = 0.001; } // 初始化Kalman濾波器 kf_.Initialization(x); // 設置協方差矩陣P Eigen::MatrixXd P = Eigen::MatrixXd(4, 4); P << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 1000.0; kf_.SetP(P); // 設置過程噪聲Q Eigen::MatrixXd Q = Eigen::MatrixXd(4, 4); Q << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; kf_.SetQ(Q); // 存儲第一幀的時間戳,供下一幀數據使用 last_timestamp_ = measurement_pack.timestamp_; is_initialized_ = true; return; } // 求先後兩幀的時間差,數據包中的時間戳單位爲微秒,處以1e6,轉換爲秒 double delta_t = (measurement_pack.timestamp_ - last_timestamp_) / 1000000.0; // unit : s last_timestamp_ = measurement_pack.timestamp_; // 設置狀態轉移矩陣F Eigen::MatrixXd F = Eigen::MatrixXd(4, 4); F << 1.0, 0.0, delta_t, 0.0, 0.0, 1.0, 0.0, delta_t, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; kf_.SetF(F); // 預測 kf_.Prediction(); // 更新 if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) { kf_.SetH(H_lidar_); kf_.SetR(R_lidar_); kf_.KFUpdate(measurement_pack.raw_measurements_); } else if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) { kf_.SetR(R_radar_); // Jocobian矩陣Hj的運算已包含在EKFUpdate中 kf_.EKFUpdate(measurement_pack.raw_measurements_); } }
完成傳感器數據的讀取和融合算法的編寫後,咱們將二者組合起來,寫在main.cpp中,輸出每一幀的障礙物融合結果。
//@ filename: main.cpp #include #include #include #include #include #include "Eigen/Eigen" #include "interface/ground_truth_package.h" #include "interface/measurement_package.h" #include "algorithims/sensorfusion.h" int main(int argc, char* argv[]) { // 設置毫米波雷達/激光雷達輸入數據的路徑 // Set radar & lidar data file path std::string input_file_name = "../data/sample-laser-radar-measurement-data-2.txt"; // 打開數據,若失敗則輸出失敗信息,返回-1,並終止程序 // Open file. if failed return -1 & end program std::ifstream input_file(input_file_name.c_str(), std::ifstream::in); if (!input_file.is_open()) { std::cout << "Failed to open file named : " << input_file_name << std::endl; return -1; } // 分配內存 // measurement_pack_list:毫米波雷達/激光雷達實際測得的數據。數據包含測量值和時間戳,即融合算法的輸入。 // groundtruth_pack_list:每次測量時,障礙物位置的真值。對比融合算法輸出和真值的差異,用於評估融合算法結果的好壞。 std::vector measurement_pack_list; std::vector groundtruth_pack_list; // 經過while循環將雷達測量值和真值所有讀入內存,存入measurement_pack_list和groundtruth_pack_list中 // Store radar & lidar data into memory std::string line; while (getline(input_file, line)) { std::string sensor_type; MeasurementPackage meas_package; GroundTruthPackage gt_package; std::istringstream iss(line); long long timestamp; // 讀取當前行的第一個元素,L表明Lidar數據,R表明Radar數據 // Reads first element from the current line. L stands for Lidar. R stands for Radar. iss >> sensor_type; if (sensor_type.compare("L") == 0) { // 激光雷達數據 Lidar data // 該行第二個元素爲測量值x,第三個元素爲測量值y,第四個元素爲時間戳(納秒) // 2nd element is x; 3rd element is y; 4th element is timestamp(nano second) meas_package.sensor_type_ = MeasurementPackage::LASER; meas_package.raw_measurements_ = Eigen::VectorXd(2); float x; float y; iss >> x; iss >> y; meas_package.raw_measurements_ << x, y; iss >> timestamp; meas_package.timestamp_ = timestamp; measurement_pack_list.push_back(meas_package); } else if (sensor_type.compare("R") == 0) { // 毫米波雷達數據 Radar data // 該行第二個元素爲距離pho,第三個元素爲角度phi,第四個元素爲徑向速度pho_dot,第五個元素爲時間戳(納秒) // 2nd element is pho; 3rd element is phi; 4th element is pho_dot; 5th element is timestamp(nano second) meas_package.sensor_type_ = MeasurementPackage::RADAR; meas_package.raw_measurements_ = Eigen::VectorXd(3); float rho; float phi; float rho_dot; iss >> rho; iss >> phi; iss >> rho_dot; meas_package.raw_measurements_ << rho, phi, rho_dot; iss >> timestamp; meas_package.timestamp_ = timestamp; measurement_pack_list.push_back(meas_package); } // 當前行的最後四個元素分別是x方向上的距離真值,y方向上的距離真值,x方向上的速度真值,y方向上的速度真值 // read ground truth data to compare later float x_gt; float y_gt; float vx_gt; float vy_gt; iss >> x_gt; iss >> y_gt; iss >> vx_gt; iss >> vy_gt; gt_package.gt_values_ = Eigen::VectorXd(4); gt_package.gt_values_ << x_gt, y_gt, vx_gt, vy_gt; groundtruth_pack_list.push_back(gt_package); } std::cout << "Success to load data." << std::endl; // 部署跟蹤算法 SensorFusion fuser; for (size_t i = 0; i < measurement_pack_list.size(); ++i) { fuser.Process(measurement_pack_list[i]); Eigen::Vector4d x_out = fuser.kf_.GetX(); // 輸出跟蹤後的位置 std::cout << "x " << x_out(0) << " y " << x_out(1) << " vx " << x_out(2) << " vy " << x_out(3) << std::endl; } }
將融合結果與真值繪製在同一座標系下,便可看到融合的實際效果,以下圖所示。
▲融合結果與真值對比
由圖能夠看出,融合結果與真值基本吻合,這只是定性的分析,缺少定量的描述。隨後課程介紹了一種定量分析的方式——均方根偏差(RMSE,Root Mean Squared Error),其計算方式是預測值與真實值誤差的平方與觀測次數n比值的平方根,以下公式所示:
▲均方根偏差計算公式
分別計算x、y、vx和vy的RMSE,若是RMSE的值越小,則證實結果與真值越接近,跟蹤效果越好。
《優達學城(Udacity)無人駕駛工程師學位》在課程中提供了一個比較方便的可視化工具,用於實時顯示障礙物的跟蹤狀態和RMSE的值。以下視頻所示,視頻中紅點和藍點分別表示Radar和Lidar的測量位置,綠點表示多傳感器融合後的輸出結果,真值爲小車所在的位置。
以上就是多傳感器融合的技術原理和編程思路。完整版的代碼和搭建Linux開環境的方法,能夠關注微信公衆號【自動駕駛乾貨鋪】,回覆【傳感器融合】獲取。
做爲入門課程,本次分享僅僅介紹了激光雷達和毫米波雷達對單一障礙物的融合。實際工程開發時,多傳感器融合算法工程師除了要掌握融合算法的理論和編碼外,還要學習不一樣傳感器(激光雷達、毫米波雷達、攝像機等)的數據特性和相應的障礙物檢測算法,這樣才能在各類傳感器之間遊刃有餘。
*《優達學城(Udacity)無人駕駛工程師學位》
【http://link.zhihu.com/target=https%3A//cn.udacity.com/course/self-drivingcar-engineer--nd013】