四.卡尔曼滤波器(EKF)开发实践之四: ROS系统位姿估计包robot_pose_ekf详解

本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分五个小节介绍:

一.卡尔曼滤波器开发实践之一: 五大公式

二.卡尔曼滤波器开发实践之二:  一个简单的位置估计卡尔曼滤波器    

三.卡尔曼滤波器(EKF)开发实践之三:  基于三个传感器的海拔高度数据融合  

四.卡尔曼滤波器(EKF)开发实践之四:  ROS系统位姿估计包robot_pose_ekf详解   也就是本文

五.卡尔曼滤波器(EKF)开发实践之五:  编写自己的EKF替换robot_pose_ekf中EKF滤波器

--------------------------------正文开始------------------------------------------
        在我的工作学习中,只要涉及到智能汽车,智能机器人,经常会接触到基于多路传感器的位姿估计,比如ROS系统自带的,用于评估机器人的 3D 位姿的扩展卡尔曼滤波算法包: robot_pose_ekf.

        这个包用于评估机器人的 3D 位姿，使用了来自不同源的位姿测量信息，它可以使用带有 6D（3D position and 3D orientation）模型信息的扩展卡尔曼滤波器来整合来自*里程计(odom)， 惯性传感器(IMU),视觉里程计(VO),以及GPS定位的数据信息。 根据ROS官方文介绍: robot_pose_ekf包采用松耦合方式融合不同传感器信息实现位姿估计。

        下面我就从扩展卡尔曼滤波器(ekf)的五大公式角度,对robot_pose_ekf做详细分析.

        分析重点我打算全部放在这个包EKF部分,对于这个包的基于ROS系统的入口代码,以及各种topic的订阅和发布,TF的监听和发布,已经从传感器收到位姿信息如何初处理,本文不做介绍.另外之这个包支持odom.imu,vo,gps四路传感器位姿信息融合,这里我只以常用的odom和imu两路位姿融合来介绍.

一. odom和imu传感器位姿信息输入

        odom传感器数据格式: odom( x, y, z, roll,pitch,yaw): 其中,x,y作为智能车在平面地图的x和y坐标;z坐标忽略恒等于0; roll(车身纵向翻滚角)和pitch(车身俯仰角)也不做考虑恒等于0. yaw(偏航角)作为智能车在平面地图上左右转向方向角. 即: odom( x, y, 0, 0,0,yaw),共6个测量值数据.   此外odom传感器还提供自身的噪音协方差矩阵,为一个 6x6矩阵.

        imu惯性传感器数据格式: imu(roll,pitch,yaw): 其中roll(车身纵向翻滚角)和pitch(车身俯仰角)也不做考虑恒等于0. yaw(偏航角)作为智能车在平面地图上左右转向方向角. 即: imu( 0,0,yaw),共3个测量值数据.   此外imu传感器还提供自身的噪音协方差矩阵,为一个 3x3矩阵.

二. robot_pose_ekf位姿包系统状态列向量和

       系统状态列向量:X_k(6x1)= ( x, y, z, roll,pitch,yaw), 实际有效值为: X_k(6x1)= ( x, y, 0, 0,0,yaw). 理由同上odom.

        最终,返回的结果: 最优估计值向量:  也是6维列向量 ( x, y, 0, 0,0,yaw) 

三. robot_pose_ekf位姿包的核心:  BFL::ExtendedKalmanFilter

     先讲BFL(贝叶斯滤波器库),这是一个开源的贝叶斯滤波器库,里面包含有扩展的卡尔曼滤波器(ExtendedKalmanFilter). 根据robot_pose_ekf包据介绍,该包采用松耦合方式融合不同传感器信息实现位姿估计. 注意这个松耦合,这有别于前面文章中我介绍的高内聚的整体融合方式.

        下面我就结合关键代码,介绍下robot_pose_ekf包是如何采用松耦合的方式使用BFL::ExtendedKalmanFilter的(详细robot_pose_ekf代码大家可以网上下载):

1. EKF初始化,及先验状态向量定义:

源码位置: odom_estimation.cpp

所在方法: OdomEstimation::initialize(const Transform& prior, const Time& time)

    // set prior of filter
    ColumnVector prior_Mu(6); //--->X_k-1 状态向量(6)先验(初始)估计值(x,y,z,roll,pitch,yaw)
    decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6));
    SymmetricMatrix prior_Cov(6); //--->P_k-1   初始预测矩阵6x6,
    for (unsigned int i=1; i<=6; i++) {
      for (unsigned int j=1; j<=6; j++){
	if (i==j)  prior_Cov(i,j) = pow(0.001,2);
	else prior_Cov(i,j) = 0;
      }
    }
    prior_  = new Gaussian(prior_Mu,prior_Cov); //关于先验状态值的二维高斯分布
    filter_ = new ExtendedKalmanFilter(prior_); //Construction of the Filter

    // remember prior
    addMeasurement(StampedTransform(prior, time, output_frame_, base_footprint_frame_));

如代码和注释所示:

先验状态列向量:  (x,y,z,roll,pitch,yaw),  即状态值个数: n = 6. 

 定义预测过程的不确定性协方差矩阵

 

同时定义先验估计高斯函数: prior_  = new Gaussia(prior_Mu,prior_Cov),  注: 最后,ekf的最优估计值和最优协方差矩阵也将基于才高斯分布获取: 

filter_->PostGet()->ExpectedValueGet();

filter_->PostGet()->CovarianceGet();

依据先验估计高斯定义EKF:  filter_ = new ExtendedKalmanFilter(prior_); 

最后,保存第一个odom位姿数据,等待imu数据到来,才开始数据融合.

2. 松耦合的系统模式和传感器模型定义

源码位置: odom_estimation.cpp

所在方法: OdomEstimation::OdomEstimation()

<1> 系统模型system

    // create SYSTEM MODEL
    ColumnVector sysNoise_Mu(6);  sysNoise_Mu = 0;//预测模型状态向量X_k(6)
    SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;//预测噪音协方差矩阵Q_k(6x6)
    for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
    Gaussian system_Uncertainty(sysNoise_Mu/*均值:预测向量nx1*/, sysNoise_Cov/*预测噪音协方差矩阵nxn*/);//预测二维高斯分布

	//非线性模型没有F_k,B_k. 但求得的雅各比矩阵, 相当于F_k和B_k的结合
	sys_pdf_   = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty/*指定预测模型高斯分布函数*/);//非线性概率密度函数
    sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);//整个系统的预测模型

 定义系统预测模型列向量: x_k(6) = (0,0,0,0,0,0),  对应(x,y,z,roll,pitch,yaw)

 定义系统预测模型噪音协方差矩阵: 

 

定义系统模型非线性概率密度函数(PDF)和系统模型:

sys_pdf_  和 sys_model_

这一步sys_model_定义,其实对应EKF的公式<1>和公式<2>: 

和

Note:  现在只是定义了模型和必要的矩阵和向量, 真正的执行公式<1>和公式<2>,要等到针对sys_model_做更新(update)时才执行.

不对,上面的矩阵和向量定义是不是差点什么?  对,还不见和

其实,​预测矩阵​​​​​​和控制矩阵就在 sys_pdf_ 里面以雅各比矩阵形式被定义,而做为控制向量,在OdomEstimation::update()即ekf针对sys_model_更新时才提供,现在可以先剧透给你,为2维列向量u_k(2x1)=(v, θ), 其中v是小车速度,θ为偏航角度.  还可以剧透的是, v和θ在整个生命周期全是0.即:u_k(2x1)=(0, 0).

未完待续......