odom&imu融合用于Cartographer建图

任务动机：使用odom&imu融合用于Cartographer建图，提高Cartographer建图时的匹配精度，同时降低运算消耗。

任务描述：使用odom&imu融合用于Cartographer建图，实验对比验证融合对建图匹配精度的影响。

1. odom的作用

        里程计是记录机器人与起始位置相对pose的模块，同时它还提供了机器人实时的线速度、角速度以及这些状态量的不确定性。一个精准的里程计对于建图和定位性能的提高有着重要作用。它不仅可以提供先验位姿，提高建图时的匹配精度，还可以降低运算消耗。

2. 原始odom

2.1 航迹推演

        以平面轮式移动机器人为例，通常会在机器人的每个轮子上安装编码器，它可以记录轮子转动的角度。在此基础上，结合机器人的轮子半径就可以计算出轮子的线速度，再使用轮间距进而计算得到机器人的速度和角速度。使用该速度在较短时间dt内进行积分即可以得到机器人在该dt内的运动累积[1]。如图1所示，记Vr，Vr分别为左右轮的线速度，V为机器人底盘的线速度，W为机器人底盘的角速度，l为机器人底盘的轮间距，r为机器人的运动半径。

图1（来自参考文献1）

        可以得到以下关系：

图2（来自参考文献1）

2.2 弊端

        单纯地使用编码器和底盘参数进行航迹推演，由于经过了多层的递进，过程中必然会累积较多的误差，随着时间的积累，里程计的积累误差会越来越大。如图3是使用激光slam建图得到的机器人真实路线（紫）和编码器里程计的路线（红）。

图3

2.3 融合imu

        在里程计航迹推演的误差中，航向角的误差是影响较为严重的一项。原因在于它不仅体现了机器人的朝向，还影响着机器人坐标(x,y)的计算。因此计算出一个比较精准的角度信息对于提高里程计的准确度来说至关重要。IMU可以提供较为准确的角度和角速度，因此一种提高里程计准确度的思路是将IMU和编码器里程计融合。EKF（扩展卡尔曼滤波）是一种较为常见的传感器融合方法。

3. EKF

        卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。[2]

        首先要计算预测值、预测值和真实值之间误差协方差矩阵。下式中，是k时刻系统状态的预测，A是状态转移矩阵，B控制矩阵，uk-1是控制量。是k时刻系统的状态最终的估计结果。P’k是K时刻机器人状态的不确定性的预测，Pk-1是k-1时刻机器人的不确定性，Q是状态转移过程中引入的不确定性。

        有了这两个就能计算卡尔曼增益K，再然后得到估计值，其中H是观测矩阵，指的是观测量和状态量之间的关系，R是指观测时引入的不确定性。

        最后还要计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备。

        由于卡尔曼滤波只适用于线性系统，而很多待估计的场景都是非线性的，比如我们常见的机器人运动模型，其运动数学模型符合牛顿第二定律：Xt+1=Vt+1/2*a*t2，另外航迹推演部分还包含了求正弦余弦的部分，因此其模型是非线性的，需要对此模型进行线性化的近似。

ROS&EKF：ROS navigation stack中集成了使用ekf融合里程计和imu的package。

3.1 robot_pose_ekf

        robot_pose_ekf是最初的ekf融合package，但由于代码可读性差、可扩展性差以及使用不方便，已经被robot_localization替代了。

3.2 robot_localization

        https://github.com/cra-ros-pkg/robot_localization

        相比robot_pose_ekf,robot_localization支持更多的传感器类型，支持更多的传感器数量，不仅使用方便，而且代码结构清爽，可读性强。

3.3 安装

1. 安装ros-kinetic-geographic-msgs

sudo apt-get install ros-kinetic-geographic-msgs
sudo apt-get remove --purge geographiclib-doc geographiclib-tools

2. 下载 https://sourceforge.net/projects/geographiclib/  并cmake 安装（默认路径usr/local）

3. 改写robot_localization的CMakeLists.txt

# set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/")                   
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/local/share/cmake/GeographicLib/")

find_package(GeographicLib REQUIRED COMPONENTS STATIC) 中的STATIC去掉

4. catkin_make

3.4 使用

1. 准备好要融合的数据

1.1 由编码器和底盘参数推演得到的odom

1.1.1 odom数据的格式为nav_msgs/Odometry

1.1.2 协方差矩阵的配置：用户可以在完成航迹推演后，在地面上设定一定的长度，控制机器人行走该长度然后读取odom的推演结果，多次实验然后统计数据，得到odom的不确定性。

1.2 imu数据

1.2.1 imu的数据格式为ensor_msgs/Imu

1.2.2 一般imu模块厂商会在驱动中给出imu的协方差，因此不需要用户自己统计

1.2.3 发布imu的tf：imu在车身上的位置要以静态tf的形式发布出来，子坐标系为imu的topic中的frame_id。

2. 配置yaml

        以ekf_template.yaml为例

frequency: 30 #表示状态更新的频率

silent_tf_failure: false

sensor_timeout: 0.1

#我们使用的是2d车，因此开启2d模式

two_d_mode: true

transform_time_offset: 0.0

transform_timeout: 0.0

print_diagnostics: true

debug: false

debug_out_file: /path/to/debug/file.txt

publish_tf: false

publish_acceleration: false

#我们只用此package来融合odom和imu作为更准的odom，因此world_frame配置为 odom，将map_frame注释掉。如果在地图中定位，那map_frame就配成map，world也配成map，如果在地球上定位，那map_frame就配成map，world配成world。

#map_frame: map                  # Defaults to "map" if unspecified

odom_frame: odom                 # Defaults to "odom" if unspecified

base_link_frame: base_footprint  # Defaults to "base_link" if unspecified

world_frame: odom                # Defaults to the value of odom_frame if unspecified

odom0: /odom

# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.

建议：

• 如果里程计同时提供位置（Position)和线速度(Linear Velocities)，就将线速度信息融合进来。
• 如果里程计同时提供方向(Orentation)和角速度(Angular Velocties)，就将方向信息融合进来。

        实测证实确实如此。如果底盘航迹推演的position和odom速度角速度都来自同一个传感器：编码器，因此不适合都用上。

odom0_config: [false,  false,  false,

                              false, false, true,

                             true, true, false, #y的速度肯定是0，所以他是个挺好的观测数据，用上也不错

                              false, false, false,

       false, false, false]

odom0_queue_size: 2

odom0_nodelay: false

#differential ：比如有两个传感器，他们都带有朝向信息，但是相差比较多，这时候我们可以用比较准的那个。而剩下的那个传感器就把differential模式打开，意思是用这个朝向结合delta_t计算出速度再用上。由于imu的朝向角比编码器积分得到的角度准很多，因此我们融合imu的yaw，odom的yaw作为速度使用。

odom0_differential: true

#不管odom自身的pose相关的数值是多少，我们的用法都是和第一帧数据的相对值

odom0_relative: true

odom0_pose_rejection_threshold: 5

odom0_twist_rejection_threshold: 1

imu0: /imu

imu0_config: [false, false, false,

             false, false,  true,

             false, false, false,

             false,  false,  true,

             true,  false,  false]

imu0_nodelay: false

imu0_differential: true

#应该打开。机器人上电时刻应为0

imu0_relative: true  

imu0_queue_size: 5

imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names

imu0_twist_rejection_threshold: 0.8                #

imu0_linear_acceleration_rejection_threshold: 0.8  #

imu0_remove_gravitational_acceleration: true

use_control: false

stamped_control: false

control_timeout: 0.2

control_config: [true, false, false, false, false, true]

acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]

deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]

acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]

deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

process_noise_covariance: [0.1, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,

                          0,    0.1, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,

                          0,    0,    0.1, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,

                          0,    0,    0,    0.1, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,

                          0,    0,    0,    0,    0.1, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,

                          0,    0,    0,    0,    0,    0.1, 0,     0,     0,    0,    0,    0,    0,    0,    0,

                          0,    0,    0,    0,    0,    0,    0.1, 0,     0,    0,    0,    0,    0,    0,    0,

                          0,    0,    0,    0,    0,    0,    0,     0.1, 0,    0,    0,    0,    0,    0,    0,

                          0,    0,    0,    0,    0,    0,    0,     0,     0.1, 0,    0,    0,    0,    0,    0,

                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0.1, 0,    0,    0,    0,    0,

                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.1, 0,    0,    0,    0,

                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.1, 0,    0,    0,

                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.1, 0,    0,

                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.1, 0,

                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.1]

initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,

                             0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,

                             0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,

                             0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,

                             0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,

                             0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,

                             0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,

                             0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,

                             0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,

                             0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,

                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,

                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,

                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,

                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,

                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

3.5 融合实验效果

参数\融合效果\融合对象	odom
	结论：角度只用imu的，可见首尾交错了，也不是很准。
	结论：角度用imu和odom的融合，效果比较好。
	结论：imu的vyaw作用不大。

        以得出结论，角度用imu和odom的融合，效果比较好。跟原始的odom相比，融合后的odom可以首尾接近相连，相比直接只使用编码器得到的里程计准确了很多。

4. 融合的odom用于cartographer建图

4.1 用于前端

        以Cartographer为例，cartographer中使用三种数据源去做位姿预估：odom、imu、匹配的pose。当使用纯激光建图时，某时刻预估的pose本身就依赖于前几个关键帧的pose，它由前几个pose的首尾之差除以时间计算得来。假如前几个关键帧的pose是错误的话，那么会恶性循环地影响匹配定位。例如，机器人在一个长走廊环境中，由于激光观测到的特征不够丰富，激光帧间匹配会容易形成定位停留在原地的现象。如图所示。

        不使用odom且激光特征不丰富时：

        使用odom进行位姿预测：

4.2 用于后端

        Cartographer中还将odom用在了后端优化，在node和node之间算对应的odom的差，对比node之间的delta pose得到残差。但是odom本身就是一种不太准确的信息，放入后端去优化node的pose，反而会引入多余的误差。因此经过测试，在将odom从后端删除之后，可以得到较好的建图效果。

5. 参考文献

[1]航迹推演（Odometry） https://blog.csdn.net/heyijia0327/article/details/47021861

[2]How a kalmanfilter works, in pictures https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/