深入理解如何不费吹灰之力搭建一个无人驾驶车（三）2D-小车拓展部分（AMCL+EKF）（EKF协方差矩阵如何写？）

吴家征

已于 2022-01-21 17:59:45 修改

阅读量5.1k

点赞数 14

分类专栏：无人驾驶文章标签：人工智能无人驾驶 ros slam 概率机器人学

于 2019-03-05 23:32:09 首次发布

本文链接：https://blog.csdn.net/qq_38588806/article/details/88212774

版权

无人驾驶专栏收录该内容

11 篇文章 45 订阅

订阅专栏

三、2D小车定位拓展

本文讲一下不是很难的高中部分，主要是AMCL及EKF，两种定位方法

3.1 AMCL定位

首先讲一下AMCL算法，自适应蒙特卡罗粒子滤波定位，其实在gmapping中，我们就要用轮式里程计的odom产生粒子，有些东西，讲大家可能不懂，可以见概率机器人或机器人学状态估计，在贝叶斯滤波中，置信度的更新是靠观测，即一个归一化系数*P(Zt|xt)*预测置信度bel(xt)，这个预测bel一般上面有一横，CSDN不知道怎么打，这个预测bel是由运动方程及上一刻的置信度积分得到，总之，粒子滤波就是让符合观测P(Xt|Zt)的现在的粒子（预测的bel）更容易下一次被采样到，其实就是用离散的方法去近似连续，从而减少计算量，但粒子滤波容易出现粒子缺乏和粒子冗余，故我们用Augmented MCL改善了粒子缺乏，用KLD-MCL改善了粒子冗余。
原生粒子滤波如下：
在这里插入图片描述
如何根据运动模型采样粒子呢，对t-1时刻的粒子知道车子向x方向运动了10米，那么把t-1时刻的粒子的x坐标都加10，这是最简单的做法，但是根据里程计运动模型：实际上运动到那后你还要加上一随机增量，这个随机增量服从高斯分布，且协方差由下面的公式决定：

这个很关键，我们下一篇说。
那么如何通过计算机程序从一个高斯分布采样呢，高斯分布如下：（图来自百度百科）
在这里插入图片描述
我们把-3到3的函数拉成一条线，每段的长度等于其函数的高度及y值，然后生成一个随机数，看他落在哪个区间就好了，这种是用直方图的方法（使用时常取一段区间，用面积表示拉成直线的长度），但我希望我是直接建立从概率到值的映射，这里用Box-Muller方法：
随机抽出两个从[0,1]均匀分布的数字u和v。然后：参考这篇文章
ssd
则Z1，Z2服从标准正态分布，若指定期望为E，方差为V，则令：
Z= Z * V + E;
也可以使用c++ 的 random库
Augmented MCL如下：
在这里插入图片描述
KLD MCL如下：

Augmented MCL说白了就是因为比方说位姿服从高斯分布，向两边延伸概率越来越小，但粒子滤波却不能在那里产生粒子，如果此时你把机器人蒙上眼睛和雷达抱到另一个地方了，那你就悲剧了，粒子难以收敛，所以AugmentedMCL改进就是如果粒子难以收敛到一点附近，那么我们就在全局随机产生一些粒子，越不收敛因随机产生的粒子越多，具体见概率机器人定位那章。

KLD MCL说白了就是如果传感器太牛逼了，最后粒子都收敛到一点附近，那这一点精确度已经够高了，粒子数对精确度提升不大，所以我们希望减少一些粒子，这个衡量单位是在一个栅格中的粒子。

AMCL就是这两者结合，你也可以尝试去自定义粒子分布的形状，这些概率机器人都有，就不说了，蛮简单的。

要使用AMCL你需要用mapserver先保存gmapping或其他算法建的地图，在linux命令行输入

rosrun map_server map_saver -f mymap

之后在launch文件加上你保存地图的路径并运行movebase：

    <arg name="map_file" default="$(find amcl)/maps/class.yaml"/>
	<node pkg="map_server" name="map_server"  type="map_server" args="$(arg map_file)" />
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
	<rosparam file="$(find mb)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find mb)/param/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    <rosparam file="$(find mb)/param/local_costmap_params.yaml" command="load" ns="local_costmap"/>
    <rosparam file="$(find mb)/param/global_planner_params.yaml" command="load" ns="GlobalPlanner"/>
    <rosparam file="$(find mb)/param/dwa_local_planner_params.yaml" command="load" ns="DWAPlannerROS"/>
</node>

在加上AMCL的launch

<node pkg="amcl" type="amcl" name="amcl" output="screen">
	<!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="min_particles" value="500"/>   
    <param name="max_particles" value="5000"/>
    <param name="kld_err" value="0.05"/>   
    <param name="kld_z" value="0.99"/>  
    <param name="update_min_d" value="0.2"/>   
    <param name="update_min_a" value="0.5"/>   
    <param name="resample_interval" value="1"/>  
    <param name="transform_tolerance" value="0.1"/>  
    <param name="recovery_alpha_slow" value="0.0"/> 
    <param name="recovery_alpha_fast" value="0.0"/>  
    <param name="gui_publish_rate" value="10.0"/> 
    <param name="save_pose_rate" value="0.5"/> 
    <param name="use_map_topic" value="false"/>  
    <param name="first_map_only" value="false"/>  
    <param name="laser_min_range" value="-1.0"/> 
    <param name="laser_max_range" value="-1.0"/>  
    <param name="laser_max_beams" value="30"/>   
    <param name="laser_z_hit" value="0.5"/> 
    <param name="laser_z_short" value="0.05"/> 
    <param name="laser_z_max" value="0.05"/>
    <param name="laser_z_rand" value="0.5"/> 
    <param name="laser_sigma_hit" value="0.2"/> 
    <param name="laser_lambda_short" value="0.1"/> 
    <param name="laser_likehood_max_dist" value="2.0"/> 
    <param name="laser_model_type" value="likelihood_field"/>
    <param name="odom_model_type" value="diff"/> 
    <param name="odom_alpha1" value="0.2"/> 
    <param name="odom_alpha2" value="0.2"/>
    <param name="odom_alpha3" value="0.8"/> 
    <param name="odom_alpha4" value="0.2"/> 
    <param name="odom_alpha5" value="0.1"/> 
    <param name="odom_frame_id" value="odom"/>  
    <param name="base_frame_id" value="base_link"/> 
    <param name="global_frame_id" value="map"/> 
    <param name="tf_broadcast" value="true"/> 
    <param name="initial_pose_x" value="0.0"/> 
    <param name="initial_pose_y" value="0.0"/>
    <param name="initial_pose_a" value="0.0"/> 
    <param name="initial_cov_xx" value="0.5*0.5"/>
    <param name="initial_cov_yy" value="0.5*0.5"/>
    <param name="initial_cov_aa" value="(π/12)*(π/12)"/>
</node>

详见amcl参数含义及《概率机器人》及gmapping的解释，比如KLD那个参数就是概率机器人KLD MCL里的，这些参数都很简单，看名字都能懂，如果实在不懂点赞给我发信息，谢谢。

之后启动rviz发一个目标就能进行导航了，不过地图是直接出现了，就是你之前建好的地图，且不能更新。

3.2 EKF定位及协方差矩阵如何写

EKF全称是拓展卡尔曼滤波，那么卡尔曼滤波(KF)是个什么东西呢

对，位姿的x方向里程估计的数据有这两组，我们更愿意相信哪个数据呢？是不是数据二，因为它方差更小，这就是KF的原理，KF系属于高斯滤波（连续贝叶斯滤波），但是假设分布时高斯分布在实际情况往往是极其不准确的，因为你建立的误差模型可能不是服从高斯分布的，那你就必须通过计算把你的误差模型转换为高斯分布，这是极其麻烦的，所以现在都是用粒子滤波（离散贝叶斯滤波）较多，具体见下图：引用自概率机器人：
假设置信度预测就是最左边那个分布，而观测P(Zt|Xt)就是最右边那个分布，KF的结果是将这两者叠加得到中间实线那个分布，我们知道高斯分布曲线越胖，方差越大，所以我们通过这种高斯滤波方法让对位姿估计的不确定性降低了，但是当机器人继续运动的时候，置信度分布曲线还是会变胖，所以我们要继续高斯滤波，这样循环往复。
KF系属贝叶斯滤波其公式为：

而KF的伪代码为：
ekf
是不是就是贝叶斯滤波啊。而EKF是因为，比方说，你运动轨迹是一个圆，由运动模型如下，见《概率机器人》

是不是我们会由平移及角度计算X方向及Z方向的增量（用cos，sin这些三角函数），但其不是线性的，那我们将其泰勒展开，保留前两项（最多到一阶），之前无法知道KF的A和B和C这三个参数无法算协方差，展开后我知道了，于是就可以算协方差了。
而EKF伪代码如下：

另一种常用的KF系算法就UKF，通过无损变换进行滤波，伪代码如下：

UKF肯定比EKF好些，但是因为假定高斯分布，准确性在现实情况不高，需要一种比较好的误差模型构造支持。
ROS中我们常用到robot_pose_ekf这个包，其实我发现CSDN上没有一篇讲的清楚的，因为一些东西卡了很久，他需要的东西如下，首先是wheel_odom包括协方差矩阵，（不需要到base_link的tf，这个很重要），imu包括协方差矩阵，imu到tf的信息,vo,gps，后面两个可选，有了这些运行这个包就OK
launch如下定义：

 <launch>
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom_ekf"/>
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="true"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
  </node>
 </launch>

然后关键是要建立正确的误差模型。这里我根据里程计运动模型写了个协方差矩阵的计算，它是6*6的矩阵，即(x,y,z,r,p,y)3个直角坐标，3个旋转角。若这6个相互独立，那么协方差矩阵只有对角线有值，(注：如果是单纯EKF建议直接写雅可比矩阵的值)，代码如下：

    for(int i = 0 ; i < 36 ; i++){
		odom_.pose.covariance[i] = 0;
    }

    //i*6+j  因为是二维就只有xy及转角θ
    odom_.pose.covariance[0] = cov_a3x_*delta_x*delta_x + cov_a4x_*(delta_pre_theta_*delta_pre_theta_+delta_theta*delta_theta); //x
    odom_.pose.covariance[7] = cov_a3y_*delta_y*delta_y + cov_a4y_*(delta_pre_theta_*delta_pre_theta_+delta_theta*delta_theta); //y
    odom_.pose.covariance[14] = pow(0.1); //z
    odom_.pose.covariance[21] = pow(0.1); //r
    odom_.pose.covariance[28] = pow(0.1); //p
    odom_.pose.covariance[35] = cov_a1_*(delta_theta*delta_theta) + cov_a2_*(delta_dis*delta_dis); //theta

	if( !delta_x || !delta_y || !delta_theta ){
		odom_.pose.covariance[0] = pow(0.1);
    	odom_.pose.covariance[7] = pow(0.1);
 		odom_.pose.covariance[35] = pow(0.1);
	}
    odom_pub_.publish(odom_);
    delta_pre_theta_ = delta_theta; //在下一次算的时候能用到上一次的数据

公式来自概率机器人里程计运动模型

而imu的误差模型包括噪声(Bias and Noise)、尺度因子(Scale errors)和轴偏差(Axis misalignments)。在ROS上协方差矩阵是只需提供(r,p,y)的方差，若这6个相互独立，那么协方差矩阵只有对角线有值。校准见imu误差模型。
然后把协方差公式写入代码即可。
好了EKF就讲完了，希望大家多多关注。下一章讲如何从头到尾用ROS自己创建一个slam算法

吴家征

关注

14
点赞
踩
97

收藏

觉得还不错? 一键收藏
4
评论
深入理解如何不费吹灰之力搭建一个无人驾驶车（三）2D-小车拓展部分（AMCL+EKF）（EKF协方差矩阵如何写？）

三、2D小车定位拓展本文讲一下不是很难的高中部分，主要是AMCL及EKF，两种定位方法3.1 AMCL定位首先讲一下AMCL算法，自适应蒙特卡罗粒子滤波定位，其实在gmapping中，我们就要用轮式里程计的odom产生粒子，有些东西，讲大家可能不懂，可以见概率机器人或机器人学状态估计，在贝叶斯滤波中，置信度的更新是靠观测，即一个归一化系数*P(Zt|xt)*预测置信度bel(xt)，这个预测...
复制链接

扫一扫