racecar仿真竞赛经验总结（四）- AMCL与小车定位

前文链接：

racecar仿真竞赛经验总结（一） - 前言

racecar仿真竞赛经验总结（二）- racecar仿真模型介绍

racecar仿真竞赛经验总结（三）- 激光SLAM建图

由于比赛禁止使用gazebo中直接发布的小车位置坐标，只允许使用车模上有的相机、10HZ激光雷达和100HZ的IMU进行定位。为保证定位精度，博主采用rf2o_laser_odometry将小车的激光雷达数据转为能标识小车所在位置的里程计信息，同时为了提升精度，将此里程计信息通过robot_pose_ekf融合IMU数据生成更精准的里程计信息。最后搭配AMCL解决小车的定位问题。

rf2o_laser_odometry

源码包github链接：rf2o_laser_odometry

算法概述

RF2O是一种基于平面激光扫描的2D里程表估算方法。 对于没有精确里程表的移动机器人很有用。RF2O是一种快速精确的方法，可以从连续的范围扫描中估算激光雷达的平面运动。 对于每个扫描点，其根据传感器速度制定范围流量约束方程，并最小化所得几何约束的鲁棒函数以获得运动估计。 与传统方法相反，此方法不搜索对应关系，而是以密集3D视觉测距法为基础，基于扫描梯度执行密集扫描对齐。RF2O的极低计算成本（在单个CPU内核上为0.9毫秒）加上较高的精确度，使其成为那些需要平面测距的机器人应用的合适方法。

视频Demo

从激光雷达数据获得里程计信息的方法除了rf2o_laser_odometry之外，laser_scan_matcher也是一个很不错的选择！

节点

订阅主题 - Subscribed Topics

• laser_scan (sensor_msgs/LaserScan)D

  Laser scans to process. (This topic can be remapped via the ~laser_scan_topic parameter)

• tf (tf/tfMessage)

  Transforms

发布主题 - Published Topics

• odom (nav_msgs/Odometry)

  Odometry estimations as a ROS topic. (This topic can be remapped via the ~odom_frame_id parameter)

• tf (tf/tfMessage)

  Publishes the transform from the \base_link (which can be remapped via the ~base_frame_id parameter) to \odom (which can be remapped via the ~odom_frame_id parameter).

参数

注意

• 由于rf2o_laser_odometry包发出的/odom主题中的pose和twist是不包含covariance的，而之后使用的robot_pose_ekf需要用到协方差矩阵，因此这里需要提供一个静止和运动时的协方差矩阵才可以正常工作，以下的协方差矩阵可供参考（使用不同的协方差矩阵对其效果有很大影响！可以自行尝试）

#define ODOM_POSE_COVARIANCE  {1e-3, 0, 0, 0, 0, 0, 
							   0, 1e-3, 0, 0, 0, 0, 
							   0, 0, 1e6, 0, 0, 0, 
							   0, 0, 0, 1e6, 0, 0,
							   0, 0, 0, 0, 1e6, 0,
							   0, 0, 0, 0, 0, 1e3}

#define ODOM_POSE_COVARIANCE2 {1e-9, 0, 0, 0, 0, 0, 
							   0, 1e-3, 1e-9, 0, 0, 0,
							   0, 0, 1e6, 0, 0, 0,
                               0, 0, 0, 1e6, 0, 0,
                               0, 0, 0, 0, 1e6, 0,
                               0, 0, 0, 0, 0, 1e-9}

#define ODOM_TWIST_COVARIANCE {1e-3, 0, 0, 0, 0, 0, 
							   0, 1e-3, 0, 0, 0, 0,
                               0, 0, 1e6, 0, 0, 0, 
                               0, 0, 0, 1e6, 0, 0, 
                               0, 0, 0, 0, 1e6, 0, 
                               0, 0, 0, 0, 0, 1e3}

#define ODOM_TWIST_COVARIANCE2 {1e-9, 0, 0, 0, 0, 0,
								0, 1e-3, 1e-9, 0, 0, 0, 
								0, 0, 1e6, 0, 0, 0,
                                0, 0, 0, 1e6, 0, 0,
                                0, 0, 0, 0, 1e6, 0,
                                0, 0, 0, 0, 0, 1e-9}

有了协方差矩阵后只需要在特定位置将其付给odom.pose.covariance和odom.twist.covariance即可

• 由于rf2o_laser_odometry会发布一个tf变化，这将导致tf树发生改变，这里不需要使用可以在源码中禁止其发布

参数调整

• ~laser_scan_topic (string, default: /laser_scan)

  Topic name where lidar scans are being published.

• ~base_frame_id (string, default: /base_link)

  TF frame name of the mobile robot base. A tf transform from the laser_frame to the base_frame should exist.

• ~odom_frame_id (string, default: /odom)

  TF frame name for published odometry estimations. This same parameter is used to publish odometry as a topic.

• ~freq (double, default: 10.0)

  Odometry publication rate (Hz).

launch文件参考

<launch>
 <node pkg="rf2o_laser_odometry" type="rf2o_laser_odometry_node" name="rf2o_laser_odometry">
  <param name="laser_scan_topic" value="/scan"/>
  <param name="base_frame_id" value="/base_footprint"/>
  <param name="odom_frame_id" value="/odom"/>
  <param name="freq" value="10.0"/>
 </node>
</launch>

robot_pose_ekf

ROS Wiki：http://wiki.ros.org/robot_pose_ekf

源码包github链接：robot_pose_ekf

创客智造教程：https://www.ncnynl.com/archives/201708/1909.html

源码分析：https://blog.csdn.net/zhxue_11/article/details/83828877

问题整理和修改教程：https://blog.csdn.net/shenghuaijing3314/article/details/78220151

概述

robot_pose_ekf是ROS Navigation stack中的一个包，通过扩展卡尔曼滤波器对imu、里程计odom、视觉里程计vo的数据进行融合，来估计平面移动机器人的真实位置姿态，输出odom_combined消息。robot_pose_ekf只适用于平面上的轮式移动机器人，因此odom信息中的z，pitch和roll分量可以被忽略。IMU可以提供车体坐标系相对于世界坐标系的姿态（RPY角），其中Roll和Pitch是绝对角度，因为有重力方向作为参考，而偏航角Yaw则是一个相对角度（如果IMU中没有集成电子罗盘测量地球磁场角作为参考）。IMU姿态的协方差矩阵代表了姿态测量的不确定度。

节点

订阅主题 - Subscribed Topics

• odom（编码器）
  (nav_msgs/Odometry)2D pose (轮式里程计): 二维姿态包含机器人平面中的坐标和朝向以及方位协方差。平面机器人中，其中z, roll and pitch 忽略。

• imu_data（IMU）
  (sensor_msgs/Imu)3D orientation (used by the IMU): 提供包含相对世界坐标系的Roll, Pitch 和 Yaw 角度。 Roll 和 Pitch 角是绝对角度，Yaw是相对角度。协方差矩阵决定了方向的不确定性， robot pose ekf 在仅仅接收此msg时不会工作，它需要 ‘vo’ 或’odom’ 主题。

• vo（视觉里程计）
  (nav_msgs/Odometry)3D pose (used by Visual Odometry): 此主题包含机器人全方向及相应协方差信息，当传感器只测量部分3维信息时，需要制定一个较大的协方差用来忽略此项数据。

The robot_pose_ekf节点不需要三个主题同时有效，每个主题数据都会产生一个位置估计及协方差。主题的频率也不一定一致，也具有不同的延迟。数据间歇收发，这时节点将自动检测使用有效的传感数据。添加自主的传感数据参照GPS的示例即可。 the Adding a GPS sensor tutorial

发布主题 - Published Topics

• robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)
  过滤器的输出（估计的3D机器人姿态）

TF变换

• odom_combined → base_footprint

robot_pose_ekf在输出odom_combined信息同时还会发布相关的坐标变换，输入下面指令查看tf变换关系：

rosrun rqt_tf_tree rqt_tf_tree

可以看出robot_pose_ekf节点会发布base_footprint坐标系相对于odom_combined坐标系的变换：

launch文件参考

<launch>
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom_combined"/>  #表示输出的主题：odom_combined
    <param name="base_footprint_frame" value="base_footprint"/>
    <param name="freq" value="30.0"/>                   #表示发布主题的频率 30hz
    <param name="sensor_timeout" value="1.0"/>          #表示超时时间，1.0表示1秒，如果1秒内没有传感器输入，会报错。
    <param name="odom_used" value="true"/>              #表示里程计使能
    <param name="imu_used" value="true"/>               #表示imu使能
    <param name="vo_used" value="false"/>               #视觉里程计
    <param name="debug" value="true"/>
    <param name="self_diagnose" value="false"/>
    <!--remap from="imu_data"  to ="imu_data1"/-->      #取消注释为添加滤波
  </node> 
 </launch>

其他注意事项

robot_pose_ekf中用到的imu和odom数据都需要有协方差矩阵

imu数据的协方差矩阵设置可以参考：https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/gyro.py (官方提供的模型中已经设置好这里可以直接使用)

self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]

底盘运动时odom的协方差矩阵在上文rf2o_laser_odometry时已给出

注意imu信息的协方差矩阵中代表机器人航向角的分量方差为1e-6，而里程计信息的协方差矩阵中机器人姿态分量的协方差为1e3，两个值相差很大。在进行EKF融合时，会更“相信”imu提供的姿态信息，因为其方差更小。比如机器人在转动过程中轮子发生了打滑，用编码器推算出的姿态一直在旋转，而实际姿态（主要由IMU测量得到）却没发生太大变化，这种情况就需要使用信息融合方法来减小误差。协方差矩阵中的参数设置非常重要，要根据传感器手册或者实际使用测量来确定。

在网上看到一个博主写的很不错这里引用一下–博客链接

下图为vrep仿真中robot_pose_ekf的效果，其中蓝色的曲线是使用robot_pose_ekf融合后的机器人运动轨，红色为原始的带噪声的轨迹曲线（这只是一个例子，实际效果怎么样还要调整各种参数）：

另外需要注意的是，robot_pose_ekf会发布base_link到odom的tf变换，因此我们自己的程序中就不用发布了，否则会出现冲突（在tf树中是不能构成回路的，只能有一个父坐标系，但是可以有很多子坐标系 ）。下图是仿真过程中rivz显示的原始odom（黄色箭头）和融合后的odom_combined（红色箭头）信息，以及base_link坐标系和odom坐标系间的变换关系。

AMCL

ROS Wiki：amcl

古月居教程：https://www.guyuehome.com/273

创客智造教程：https://www.ncnynl.com/archives/201708/1911.html

参数调整——csdn：https://www.cnblogs.com/dyan1024/p/7825988.html

其他博客链接：https://magicalsoso.github.io/2020/02/18/TwoWheelDiffChassisSLAM10-Navigation/#amcl%E9%85%8D%E7%BD%AE

关于amcl的相关教程在网上很多，这里就不对其原理再展开描述了，博主主要分享一下amcl参数配置和所遇到的问题

amcl中包含的话题和服务

amcl参数配置

先上我们的amcl配置

<launch>
<arg name="scan_topic" default="scan"/>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <remap from="scan" to="scan_filtered"/>

        <!-- Overall filter parameters -->
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="2000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="update_min_d" value="0.25"/> <!-- Translational movement required before performing a filter update. -->
        <param name="update_min_a" value="0.2"/> <!-- Rotational movement required before performing a filter update. 0.1 represents 5.7 degrees  -->
        <param name="resample_interval" value="1"/> <!-- Number of filter updates required before resampling. -->
        <param name="transform_tolerance" value="0.1"/> <!-- Default 0.1; time with which to post-date the transform that is published, to indicate that this transform is valid into the future. -->
        <param name="recovery_alpha_slow" value="0.001"/> <!-- Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. -->
        <param name="recovery_alpha_fast" value="0.1"/> <!-- Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. -->
        <param name="gui_publish_rate" value="10.0"/> <!-- Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable. -->
        <param name="save_pose_rate"   value="0.3"/>
        <param name="use_map_topic"    value="true"/>
        <param name="first_map_only"   value="false"/>

        <!-- set particles init pose to robot pose -->
        <param name="initial_pose_x" value="-0.5"/> <!-- -0.5 -->
        <param name="initial_pose_y" value="0.0"/>
        <param name="initial_pose_a" value="0.0"/>
        <param name="initial_cov_xx" value="0.09"/>
        <param name="initial_cov_yy" value="0.09"/>
        <param name="initial_cov_aa" value="0.03"/> <!-- pi/12 * pi/12 0.033-->

        <!-- Laser model parameters -->
	<remap from="scan" to="$(arg scan_topic)"/>
	    <param name="laser_min_range" value="-1" />
	    <param name="laser_max_range" value="12" />
        <param name="laser_max_beams" value="60"/>	
        <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_likelihood_max_dist" value="2.0"/>
        <param name="laser_model_type" value="likelihood_field"/>

        <!-- Odometry model parameters -->
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha1" value="0.2"/> <!-- Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion. -->
        <param name="odom_alpha2" value="0.2"/> <!-- Specifies the expected noise in odometry's rotation estimate from translational component of the robot's motion. -->
        <param name="odom_alpha3" value="0.2"/> <!-- Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion. -->
        <param name="odom_alpha4" value="0.2"/> <!-- Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion. -->
        <param name="odom_alpha5" value="0.1"/> <!--only used if model is "omni"-->
        <param name="odom_frame_id" value="odom_combined"/>
        <param name="base_frame_id" value="base_footprint"/> <!-- Change this if you want to change your base frame id. -->
        <param name="global_frame_id" value="map"/>
	    <param name="tf_broadcast"    value="true"/>
    </node>
</launch>

其中具体的参数含义请自行查阅ros wiki或者其他博客链接，博主分享一下这里面比较重要的参数

• min_particles：

  滤波器中的最少粒子数，值越大定位效果越好，但是相应的会增加主控平台的计算资源消耗。

• max_particles：

  滤波器中最多粒子数，是一个上限值，因为太多的粒子数会导致系统资源消耗过多。

• initial_pose_x、initial_pose_y和initial_pose_a：

  初始位姿均值，用于初始化高斯分布滤波器，initial_pose_参数决定撒出去的初始位姿粒子集范围中心，这里只要把位置定在小车中心即可

• initial_cov_xx、initial_cov_yy和initial_cov_aa：

  初始位姿协方差，用于初始化高斯分布滤波器。initial_cov_参数决定初始粒子集的范围，将其调大会扩大初始化的粒子分布范围，比赛中只要将其范围扩大到赛道宽度左右大小即可

• odom_alpha1~4：

  由于小车模型为阿卡曼模型，amcl中odom_model_type设置为diff故只需前四个即可

  • odom_alpha1 (double, default: 0.2)：指定由机器人运动部分的旋转分量估计的里程计旋转的期望噪声，默认0.2（旋转存在旋转噪声）
  • odom_alpha2 (double, default: 0.2)：机器人运动部分的平移分量估计的里程计旋转的期望噪声，默认0.2（旋转中可能出现平移噪声）
  • odom_alpha3 (double, default: 0.2)：机器人运动部分的平移分量估计的里程计平移的期望噪声，如果你自认为自己机器人的里程计信息比较准确那么就可以将该值设置的很小
  • odom_alpha4 (double, default: 0.2)：机器人运动部分的旋转分量估计的里程计平移的期望噪声，你设置的这4个alpha值越大说明里程计的误差越大

比赛中遇到的坑

首先有朋友可能要问，amcl发布的主题不是有一个/amcl_pose了吗？为什么还要使用rf2o + ekf这样的方法来定位呢？

确实理论上可以使用/amcl_pose发布的信息直接进行定位，但/amcl_pose发布的频率太低了…根本没办法让高速移动的小车实现精准的定位，关于其频率可以使用以下代码进行查看

rostopic hz /amcl_pose

由于当时没有具体搞懂amcl的原理，配置完amcl的launch文件并搭建好tf树后以为已经把amcl用上了，当时的tf树变换如下：

结果最后发现实际使用的还是laser+imu整合出的odometry，效果飘得一批，如下图所示

其中绿色点是小车读到的下一个目标点的位置，黑色点是小车获得的自身位置，可以发现两者在拐弯时偏差很大，当时以为是amcl协方差的问题尝试了半天，也修改了robot_pose_ekf和rf2o_laser_odometry里的与yaw相关的协方差矩阵，效果都不尽人意…后来请教学长才发现tf变换压根没发布，最后是主动监听map->basefootprint的tf变换把amcl纠正偏差的效果用上在解决问题（具体代码在odom_tf_converter包里了）

最后定位结果如下：