基于扩展卡尔曼滤波的多传感器融合定位
多传感器融合是一个复杂的问题,本文根据ROS中的robot localization功能包,说明多传感器融合定位的一种实现方式。
卡尔曼滤波
卡尔曼滤波并不复杂,分为预测和更新两个部分:
预测:
X^k1=AX^k−1+BukPk1=APk−1AT+Q\hat{X}_{k}^{1} =A\hat{X}_{k-1}+Bu_{k}\\ P_{k}^{1}=AP_{k-1}A^{T}+Q X^k1=AX^k−1+BukPk1=APk−1AT+Q
更新:
Kk=Pk1HTHPk1HT+RX^k=X^k1+Kk(Zk−HX^k1)Pk=(I−KkH)Pk1K_{k}=\frac{P_{k}^{1}H^{T}}{HP_{k}^{1}H^{T}+R}\\ \hat{X}_{k}=\hat{X}_{k}^{1}+K_{k}(Z_{k}-H \hat{X}_{k}^{1})\\ P_{k}=(I-K_{k}H)P_{k}^{1} Kk=HPk1HT+RPk1HTX^k=X^k1+Kk(Zk−HX^k1)Pk=(I−KkH)Pk1
在实际使用卡尔曼滤波时,第一是确定状态变量和控制变量,一般在机器人定位中,8个状态变量 [x,y,θ,vx,vy,ω,ax,ay][x,y,\theta,v_{x},v_{y},\omega,a_{x},a_{y}][x,y,θ,vx,vy,ω,ax,ay]都可以看成是状态变量,也可以把加速度看成是控制变量,得到加速度的方法可以通过上一篇文章讲的动力学控制中得到,也可以通过速度做差分。如果是都看成状态变量,那么加速度的更新是通过更新步的第二个式子进行更新,预测步的第一个式子是无法更新加速度的,只会保留加速度。
第二个问题是,状态变量的选取,一般来说定位是机器人在地图上的定位,使用的是世界坐标系,即 [x,y,θ][x,y,\theta][x,y,θ]是世界坐标系下的变量,而速度、角速度、加速度等都是通过传感器计算得到的,是机器人坐标系下的变量。从机器人坐标系转到世界坐标系需要通过旋转变换,即会引入三角函数,这样状态方程就是非线性的了,因此需要使用扩展卡尔曼滤波,这时公式需要做修改,AAA矩阵为雅可比矩阵
xk1=f(x^k−1,uk)z=h(xk)H=∂h∂xk∣x^k1,A=∂f∂xk−1∣x^k−11x_{k}^{1}=f(\hat{x}_{k-1},u_{k})\\ z=h(x_{k})\\ H=\frac{\partial h}{\partial x_{k}}|_{\hat{x}_{k}^{1} },A=\frac{\partial f}{\partial x_{k-1}}|_{\hat{x}_{k-1}^{1} } xk1=f(x^k−1,uk)z=h(xk)H=∂xk∂h∣x^k1,A=∂xk−1∂f∣x^k−11例如:
第三个问题是 P0,Q,RP_{0},Q,RP0,Q,R矩阵应该如何选取。首先是RRR矩阵,测量噪声协方差矩阵,一般是传感器的测量噪声,常用的传感器有IMU,激光雷达,摄像头,编码器等,网上都有常用的噪声协方差矩阵,这个不难找。然后是P0P_{0}P0和QQQ,初始的先验误差协方差矩阵和过程噪声矩阵,这个可以参考一些论文或开源的ekf包,里边有。本文参考的robot localization中,这两个矩阵为:
这里的矩阵是15维的:[x,y,z,roll,pitch,yaw,vx,vy,vz,θroll,θpitch,θyaw,ax,ay,az][x,y,z,roll, pitch, yaw, v_{x}, v_{y}, v_{z}, \theta_{roll}, \theta_{pitch}, \theta_{yaw}, a_{x}, a_{y}, a_{z}][x,y,z,roll,pitch,yaw,vx,vy,vz,θroll,θpitch,θyaw,ax,ay,az]
多传感器融合
多传感器融合是一个复杂的方向,具体可以分为紧耦合和松耦合两种融合方式。紧耦合是将各传感器的数据汇总,然后计算出一个位姿,松耦合是各传感器的数据分别计算出一个位姿,然后再进行汇总得到最终位姿。这么说好像有点不太直观,总之紧耦合的特点是各传感器的数据需要同步,而松耦合的传感器是异步的,不需要同步。robot localization功能包中的EKF是松耦合的融合方式。
多传感器融合的第一个问题是数据处理,由于robot localization包是松耦合的方式,因此对每一个传感器数据单独进行处理。ROS中的协方差矩阵分为三类,位姿、速度和加速度,因此所有传感器的原始数据也会拆分成这三类。举例平面机器人如下:
编码器里程计可以计算位置和速度,使用[θ,vx,vy][\theta,v_{x},v_{y}][θ,vx,vy]数据进行融合,首先先对一个传感器的数据进行分类,θ\thetaθ是一组,vx,vyv_{x},v_{y}vx,vy是一组,前者是pose类型的数据,后者是twist类型的数据,处理pose时,将角度值和其协方差放入一个结构体中,作为其中一组测量数据;处理twist时,将两个速度和对应的协方差矩阵放入另一个结构体中,作为另一组测量数据。
数据处理完后进行卡尔曼滤波,预测步没啥好说的,在更新步中,由于第一组测量数据只有角度值,因此进行更新时只更新角度值,即H矩阵只在角度对应的行,斜对角线上的值为1,其余为0。更新完角度后,协方差矩阵P更新,然后不进行预测了,因为都是同一个时间戳的数据,继续更新速度,H矩阵的值改一下,进行第二次更新。
robot localization功能包运行原理
该功能包分为主函数和传感器数据接收回调函数两个线程。
主函数部分:首先是ekf_localization_node.cpp下的main函数:看代码没啥好说的吧,然后进入这个ekf.run()函数。
int main(int argc, char **argv){ros::init(argc, argv, "ekf_navigation_node"); //创建节点RobotLocalization::RosEkf ekf;//类ekf.run();//进入这个函数return EXIT_SUCCESS;}
ekf.run()这个函数在ros_filter.cpp下1742行(行数可能不一样,因为我的robot localization包原代码被我修改过,但大概就是上下几行)。这个函数中第一个重要的部分就是,这个是读取参数,具体读了什么,可以进去看看,主要是你ekf_params.yaml这里设置的内容。注意这里加载了参数后,回调函数就开始起作用了,具体的内容放后边说。
loadParams();
然后下边就是发布者,tf变换啥的,tf变换是把测量数据从一个坐标系变换到另一个坐标系,还有打印一些东西,主要是让你知道的程序为什么报错的,我们直接忽略,接下来是时间。时间的问题在该包中十分重要,这里是获取当前时间戳,ROS的传感器消息都自带时间戳,储存在header中。
ros::Time curTime;ros::Time lastDiagTime = ros::Time::now();
以上是初始化的一部分内容,然后就进入死循环进行不断的更新和预测了。接下来先说说回调函数的内容。
回调函数,说的是接收到传感器数据后进入回调处理的过程。具体的处理方式函数在ros_filter.h 105行的类中有详细的说明,这里列一部分比较有用的。
template<class T> class RosFilter{/*控制量回调:这部分就是前文讲的,将加速度看成是控制量还是状态变量,如果是看成控制量,那就会根据速度差分的方法计算加速度,同时也会加载最大最小加速度限制、加加速度限制的相关参数,该包默认是不使用控制量。*/void controlCallback(const geometry_msgs::Twist::ConstPtr &msg);void controlCallback(const geometry_msgs::TwistStamped::ConstPtr &msg);/*三种类型数据的回调处理,这里和前文说的差不多,但是除了测量值和协方差矩阵外,还加上了很多东西,比如时间戳等等*/void accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg,const CallbackData &callbackData,const std::string &targetFrame);void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,const CallbackData &callbackData,const std::string &targetFrame,const bool imuData);void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,const CallbackData &callbackData,const std::string &targetFrame);/*IMU数据和里程计数据回调函数*/void imuCallback(const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName,const CallbackData &poseCallbackData, const CallbackData &twistCallbackData,const CallbackData &accelCallbackData);void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName,const CallbackData &poseCallbackData, const CallbackData &twistCallbackData);}
进入odometryCallback(),
/*根据时间戳检查是否是过去的测量数据,每次进行预测更新后,lastSetPoseTime_时间戳都会变成使用的测量值的时间戳*/if (msg->header.stamp <= lastSetPoseTime_){···}/*数据分类处理了,先处理pose,再处理twist*/if (poseCallbackData.updateSum_ > 0){// Grab the pose portion of the message and pass it to the poseCallbackgeometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped();posPtr->header = msg->header;posPtr->pose = msg->pose; // Entire pose object, also copies covariancegeometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);poseCallback(pptr, poseCallbackData, worldFrameId_, false);}if (twistCallbackData.updateSum_ > 0){// Grab the twist portion of the message and pass it to the twistCallbackgeometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();twistPtr->header = msg->header;twistPtr->header.frame_id = msg->child_frame_id;twistPtr->twist = msg->twist; // Entire twist object, also copies covariancegeometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);twistCallback(tptr, twistCallbackData, baseLinkFrameId_);}
进入poseCallback(),
if (msg->header.stamp >= lastMessageTimes_[topicName]){// 创建向量和矩阵,向量用来存放测量值,矩阵存放协方差Eigen::VectorXd measurement(STATE_SIZE);Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);measurement.setZero();measurementCovariance.setZero();···//这个if就是进行处理了,具体处理些个啥,有兴趣可以点进去看,if (preparePose(msg,topicName,targetFrame,callbackData.differential_,callbackData.relative_,imuData,updateVectorCorrected,measurement,measurementCovariance)){/*这个函数就是把处理好的数据,measurement这个放入测量数据队列中这样每一个传感器数据就被分解成pose、twist、acc等三类数据存到队列中*/enqueueMeasurement(topicName,measurement,measurementCovariance,updateVectorCorrected,callbackData.rejectionThreshold_,msg->header.stamp);}···}
这样我们的传感器数据经过这样的回调处理后,最后得到一个队列:[imu_pose,imu_twist,odom1_pose,odom1_twist,odom2_pose…]
说回主函数,进入到死循环后,主要有以下几个函数起作用,
while (ros::ok()){// 获取当前时间戳curTime = ros::Time::now();// 将队列里的传感器数据取出,ekf就运行在这个函数中integrateMeasurements(curTime);// Get latest state and publish itnav_msgs::Odometry filteredPosition;···//后面就是一些尾巴的处理,什么坐标系变换啊,发布话题啊,数据重新跟新啊啥的。}
进入integrateMeasurements(curTime);
while (!measurementQueue_.empty() && ros::ok()){//检查队列是否为空,检查队头的数据时间是不是过去的,MeasurementPtr measurement = measurementQueue_.top();if (measurement->time_ > currentTime.toSec()){break;}measurementQueue_.pop();if (useControl_ && restoredMeasurementCount > 0){filter_.setControl(measurement->latestControl_, measurement->latestControlTime_);restoredMeasurementCount--;}// EKF的预测和更新步filter_.processMeasurement(*(measurement.get()));···}
进入filter_.processMeasurement();
if (initialized_){// 时间差🔺tdelta = measurement.time_ - lastMeasurementTime_;if (delta > 0){validateDelta(delta);// 预测步predict(measurement.time_, delta);// Return this to the userpredictedState_ = state_;}// 更新步correct(measurement);}
predict(measurement.time_, delta);在ekf.cpp中
···//前边都是雅可比矩阵的计算// (1) Apply control terms, which are actually accelerations,controlAcceleration_都是0,因为无控制量state_(StateMemberVroll) += controlAcceleration_(ControlMemberVroll) * delta;state_(StateMemberVpitch) += controlAcceleration_(ControlMemberVpitch) * delta;state_(StateMemberVyaw) += controlAcceleration_(ControlMemberVyaw) * delta;//这个加速度也不变,和上一次状态变量的值一样,第(1)步几乎没用state_(StateMemberAx) = (controlUpdateVector_[ControlMemberVx] ?controlAcceleration_(ControlMemberVx) : state_(StateMemberAx));state_(StateMemberAy) = (controlUpdateVector_[ControlMemberVy] ?controlAcceleration_(ControlMemberVy) : state_(StateMemberAy));state_(StateMemberAz) = (controlUpdateVector_[ControlMemberVz] ?controlAcceleration_(ControlMemberVz) : state_(StateMemberAz));// (2) Project the state forward: x = Ax + Bu (really, x = f(x, u)),预测步第一个式子state_ = transferFunction_ * state_;// Handle wrappingwrapStateAngles();// (3) Project the error forward: P = J * P * J' + Q,第二个式子estimateErrorCovariance_ = (transferFunctionJacobian_ *estimateErrorCovariance_ *transferFunctionJacobian_.transpose());estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance);}
correct();也在ekf.cpp中
···// 各个矩阵Eigen::VectorXd stateSubset(updateSize);// x (in most literature)Eigen::VectorXd measurementSubset(updateSize); // zEigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); // REigen::MatrixXd stateToMeasurementSubset(updateSize, state_.rows()); // HEigen::MatrixXd kalmanGainSubset(state_.rows(), updateSize);// KEigen::VectorXd innovationSubset(updateSize);// z - HxstateSubset.setZero();measurementSubset.setZero();measurementCovarianceSubset.setZero();stateToMeasurementSubset.setZero();kalmanGainSubset.setZero();innovationSubset.setZero();for (size_t i = 0; i < updateSize; ++i){// 这里就是改变测量矩阵H的值了,只在测量数据部分有值。measurementSubset(i) = measurement.measurement_(updateIndices[i]);···}···//后面更新就是套公式了
放两张更新时的图,分别是odometry的pose和twist数据进行更新的,state就是目前的状态,11是θ˙z\dot{\theta}_{z}θ˙z数据所在的位置,其它部分是0,是因为这是平面机器人。6是vxv_{x}vx测量数据所在的位置,机器人不是全向的,所有7处vyv_{y}vy总为0。