本文做者: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】