ROS察微【51】：如何将里程计和 IMU 与 robots_localization 融合

一、简述

笔记 是 ROS 开发人员 Live Class n.51 的附加材料，由 The Construct 的 Alberto Ezquerro 和 Ricardo Tellez 免费创建和提供。 只要您提供本段的副本，您就可以分发此笔记本。

在今天的直播课中，我们将学习以下内容：

1. 为什么需要融合传感器数据进行导航
2. 什么是 robots_localization 包
3. 如何使用robot_localization 包进行传感器融合

此直播课程的先决条件是：

• ROS 概念的基础知识，如主题、发布和订阅、ROS 服务
• 知道如何创建地图以及如何在其中定位机器人。 如果您不知道如何操作，请查看 Live Class n49
• 对机器人的热爱

二、Robotnik 的 Summit XL 机器人

在本课程中，我们将使用 Robotnik 的 Summit XL 机器人。 即具有以下特点的机器人：

• 四个轮子
• 激光游侠
• IMU
• 相机
• 全球定位系统

如果您对此机器人感兴趣，可以在此处找到更多信息。

三、如何使用这个 ROSject

ROSject 是一个 ROS 项目，其打包方式使其包含的所有材料（ROS 代码、Gazebo 模拟和笔记本）都可以仅使用 Web 链接与任何机构共享。 这就是我们对 Live Class 的所有参加者所做的，我们与他们共享了这个 ROSject（这样他们就可以访问他们包含的所有 ROS 材料）。

查看此网络研讨会以了解有关 ROSjects 以及如何创建自己的 ROSjects 的更多信息。

您需要在 ROS 开发工作室 (ROSDS) 拥有一个免费帐户。 获取帐户，然后按照以下指示进行操作。

3.1. 为什么我们需要融合传感器数据进行自主机器人导航？

因为传感器数据有噪声，这会在机器人的定位中产生错误。

在谈到里程计时，里程计越好，机器人的定位就越好。

里程计误差的问题在于它们会随着时间的推移而累积。

3.2.什么是robot_localization包

它是一个包，用于将不同的里程计来源混合成一个更稳定的来源。

3.3.让我们启动Summit XL模拟

为此，您需要转到 Simulations->Summit XL。 之后，您应该得到以下模拟

3.4. 让我们启动为里程计添加噪声的代码

检查 Rviz 里程表的外观：

启动 Rviz 以在地图中定位机器人

rosrun rviz rviz

 在 /home/user/odom_config.rviz 中加载此 rosject 中提供的 Rviz 配置文件

Launch the noisy_odom package

rosrun noisy_odom add_noise.py

四、.让我们配置robot_localization包

4.1 包生成

让我们创建一个新的 ROS 包，我们将其命名为 Summit_odometry。 

cd ~/catkin_ws/src
$ catkin_create_pkg summit_odometry
$ cd summit_odometry
$ mkdir launch
$ mkdir config

 然后创建一个名为 start_filter.launch 的启动文件，其内容如下：

<launch> 

    <!-- Run the EKF Localization node -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
        <rosparam command="load" file="$(find summit_odometry)/config/ekf_localization.yaml"/>
    </node>

</launch>

 基本上，我们在这里所做的是从具有名为 ekf_localization.yaml 的特定配置文件的 robots_localization 包中启动名为 ekf_localization_node 的 ROS 程序（它使用扩展卡尔曼滤波器）。

4.2 配置文件

现在，在这个文件夹中，创建名为 ekf_localization.yaml 的配置文件。 在此文件中，您将放置以下配置。 填写带有问号 (?) 的所有参数。

#Configuation for robot odometry EKF
#
frequency: 50
    
two_d_mode: true
    
publish_tf: false

# Complete the frames section 
odom_frame: ?
base_link_frame: ?
world_frame: ?
map_frame: ?

# Complete the odom0 configuration
odom0: ?
odom0_config: [?, ?, ?,
               ?, ?, ?,
               ?, ?, ?,
               ?, ?, ?,
               ?, ?, ?,]
odom0_differential: false

# Complete the imu0 configuration
imu0: ?
imu0_config: [?, ?, ?,
              ?, ?, ?,
              ?, ?, ?,
              ?, ?, ?,
              ?, ?, ?,]
imu0_differential: false

process_noise_covariance": [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]


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]

4.3 相关的坐标框

• base_link_frame：它是机器人本身的框架，任何传感器都可以参考。 它通常位于机器人的中心。 它和它一起旅行。
• odom_frame：它是用于报告里程计的框架。
• map_frame：它是用于从知道机器人位置的系统报告全局位置的框架。 例如 AMCL 系统。 如果您没有使用任何外部本地化系统，则可以忽略它。
• world_frame：这是参考前两个帧中的哪一个将用于获取机器人在世界中的绝对坐标的帧。

在我们的例子中，这将是：

base_link_frame: base_link
odom_frame: odom
world_frame: odom

4.4  添加传感器以熔断

任何以这些格式生成消息的传感器都可以输入到 robots_localization 包中以估计机器人位置。

• nav_msgs/里程计
• sensor_msgs/Imu
• geometry_msgs/PoseWithCovarianceStamped
• geometry_msgs/TwistWithCovarianceStamped

         而且，最重要的是，您可以拥有多个传感器。 这意味着，例如，您可以让两个不同的传感器（车轮编码器和视觉里程计）或两个不同的惯性测量单元提供里程计。

a. 先指明 关于传感器topic

In our case are:

• /noisy_odom
• /imu/data

b. 配置变量矩阵
        现在，对于每个输入传感器，我们必须指定其消息的哪些组件将在卡尔曼滤波器中合并（融合）以计算最终状态估计。 要指定这一点，您必须填写一个 3x5 值矩阵。 矩阵的含义如下：

[  X,        Y,       Z
  roll,    pitch,    yaw
  X/dt,     Y/dt,    Z/dt
 roll/dt, pitch/dt, yaw/dt
  X/dt2,    Y/dt2,   Z/dt2]

 上述值的含义如下：

X, Y, Z：这些是机器人的 [x,y,z] 坐标。
roll、pitch、yaw：这些是 rpy 轴，它们指定了机器人的方向。
X/dt、Y/dt、Z/dt：这些是机器人的速度。
roll/dt、pitch/dt、yaw/dt：这些是机器人的角速度
X/ 𝑑𝑡2 , Y/ 𝑑𝑡2 , Z/ 𝑑𝑡2 ：这些是机器人的线性加速度：

因此，在这种情况下，我们为卡尔曼滤波器考虑的值是，对于里程计数据：

* X 和 Y 中的线速度，以及 Z 中的角速度。

odom0_config: [false, false, false
 false, false, false
 true,  true,  false
 false, false, true
 false, false, false,]

And for the IMU data:

* yaw(orientation), angular velocity in Z and linear acceleration in X.

 C-协方差矩阵

Use the default values

* **process_noise_covariance**：此参数用于对滤波算法预测阶段的不确定性进行建模。什么！！？？基本上，它用于改进过滤器产生的结果。对角线上的值是状态向量的方差，包括姿态，然后是速度，然后是线性加速度。设置不是强制性的，但您可以通过调整它来获得更好的结果。无论如何，除非您是这方面的专家，否则根本不容易设置。因此，在这种情况下，最好的选择是测试不同的值，看看它们如何改善或降低结果。

* **initial_state_covariance**：估计协方差定义了当前状态估计中的误差。此参数允许设置矩阵的初始值，这将影响滤波器收敛的速度。
这是您应该遵循的规则：如果您正在测量一个变量，请使 initial_estimate_covariance 中的对角线值大于该测量的协方差。因此，例如，如果您测量的相关变量的协方差值为 1e-6，则将 initial_estimate_covariance 对角线值设为 1e-3 或类似的值。

五、运行 robot_localization

现在是时候使用您的配置启动 *robot_localization* 了：

$ roslaunch summit_odometry start_filter.launch

现在在 Rviz 中检查里程计现在是否经过过滤和稳定。 您可以在主题中找到结果：

*/odom/filtered*

六. 连接到真实robot上

关闭模拟窗口以防止干扰。

6.1.在机器人中安装Husarnet通讯包

首先，您需要在机器人本身中安装一个包，该包允许与 ROSDS 进行 IP6 通信。 多亏了 Husarnet，这才成为可能。

该软件包已由我们的团队安装在机器人中。

6.2 在机器人中启动通讯脚本

您需要启动 *set_etc_hosts_mode.py* 脚本以将机器人置于通信模式。

我们的团队已经这样做了。

sudo python set_etc_hosts_mode.py RemoteMode summit

此时，您应该通过发出以下命令向机器人询问其通信地址：

sudo husarnet websetup

同时获取机器人计算机的主机名：

hostname

 6.3. Start the robot network in ROSDS

转到顶部菜单并选择 Real Robot->Robot Network。

几秒钟后，您应该看到您已经启动了 ROSDS 的机器人网络。

6.4. 在 ROSDS 中添加到机器人的连接

现在使用顶部菜单添加一个新的机器人连接，然后选择 Real Robot->Add device。

在将出现的对话框中，指明机器人计算机的主机名，以及您在步骤 1 中获得的通信网址。

几秒钟后，您应该会看到以下配置。

6.5.设置master 

现在您必须向 ROADS 指示 ROS_MASTER_URI 应该指向机器人而不是 ROADS 本身。

* 首先，我们建议终止模拟以防止与真实机器人的主题冲突。

* 然后，前往 **Real Robot** 并点击 **summit** 的圆圈。

至此，ROSDS就连接到了真正的机器人上！！！

您可以检查 shell 中的主题是否与机器人中的主题相对应。

rostopic list

6.6. 重新执行robot_localization 

roslaunch summit_odometry start_filter.launch

6.7. 检查 rviz 结果

rosrun rviz rviz

• 任务结束!!

 原文标题（

ROS Developers Live Class n51 How to fuse odometry & IMU with robot_localization）