配置Odometry-navigation2
Kong Liangqian Lv6
 2021-09-09 15:59:23  

翻译至https://navigation.ros.org/setup_guides/odom/setup_odom.html

在本指南中，我们将研究如何将机器人的odometry系统与Nav2集成。首先，我们将简要介绍一下里程计，以及Nav2正确运行需要发布的必要消息和转换。接下来，我们将展示如何在两种不同的情况下设置里程计。

在第一种情况下，我们将展示如何在一个有车轮编码器的机器人上配置里程计。

在第二种情况下，我们将使用Gazebo构建一个演示，在sam_bot(我们在前一节中构建的机器人)上模拟一个运行的里程计系统。之后，我们将讨论如何使用robot_localization包融合各种里程计来源，以提供平滑的里程计。最后，我们还将展示如何使用robot_localization发布odom => base_link变换。

Odometry Introduction

里程计系统根据机器人的运动提供了机器人的姿态和速度的局部精确估计。里程表信息可以从各种来源获得，如IMU、LIDAR、RADAR、VIO和轮编码器。需要注意的是，imu随时间漂移，而轮式编码器随移动距离漂移，因此它们经常被一起使用来抵消彼此的负面特性。

odom坐标系和与之相关的变换使用机器人的里程计系统发布连续的定位信息，但随着时间或距离(取决于传感器的形态和漂移)变得不那么准确。尽管如此，机器人仍然可以利用这些信息在其附近导航(例如避免碰撞)。为了在一段时间内获得一致准确的里程数信息，地图框架提供了全球准确的信息，用于校正odom框架。

如之前所讨论的那样，odom坐标系连接了整个系统，而Nav2则需要odom-base_link变换

整个变换可以通过tf2_braodcaster或者一个框架来完成，比如robot_localization，接下来我们会主要讨论这个功能包

除了上段中提到的所需要的odom => base_link变换，Nav2也需要有一个发布者用于发布nav_msgs/Odometry，因为这个消息里包含着机器人的速度消息

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
# This represents estimates of position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id
# The twist in this message should be specified in the coordinate frame given by the child_frame_id

# Includes the frame id of the pose parent.
std_msgs/Header header

# Frame id the pose is pointing at. The twist is in this coordinate frame.
string child_frame_id

# Estimated pose that is typically relative to a fixed world frame.
geometry_msgs/PoseWithCovariance pose

# Estimated linear and angular velocity relative to child_frame_id.
geometry_msgs/TwistWithCovariance twist

此消息告诉我们机器人位姿和速度的估计值。 Header消息在给定坐标系中提供带时间戳的数据。 pose消息提供机器人相对于 header.frame_id 中指定的框架的位置和方向。 twist消息给出相对于 child_frame_id 中定义的框架的线速度和角速度。

Setting Up Odometry on your Robot

因为配置机器人的选择和可能性会非常多，因此这里不会单独提供一种指定的配置方法，我们会给提供一些基本的概念和有用的资源拉来帮助大家配置机器人

首先，我们将使用一个带有轮式编码器作为其里程计源的机器人示例。 请注意，Nav2 不需要车轮编码器，但它在大多数设置中很常见。 设置里程计的目标是计算里程计信息并在 ROS 2 上发布 nav_msgs/Odometry 消息和 odom => base_link 转换。

为了计算这个信息，我们需要在代码中加入把轮式编码器转化为里程计的过程。类似于如下的式子

1
2
linear = (right_wheel_est_vel + left_wheel_est_vel) / 2
angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation;

right_wheel_est_vel和left_wheel_est_vel分别是左轮和右轮的速度，wheel_separation为两个轮子的距离。

另外一种可以发布odom信息的方法是利用ros2_control框架。ros2_control下面有很多的功能包，对于轮式编码器，ros2_control有一个差分驱动控制器diff_drive_controller，他利用geometry_msgs/Twist来发布cmd_vel话题，计算里程信息，发布odom话题，里面包含nav_msgs/Odometry消息

对于其他类型的传感器，他们的应该都有相对应的文档描述如何发布里程计信息，需要记住的是Nav2需要有 nav_msgs/Odometry 消息以及odom=>base_link的坐标变换。这将是你设置自己的里程计系统时候的目标

Simulating an Odometry System using Gazebo

在本节，我们会利用Gazebo来模拟一个里程计系统。我们会首先配置gazebo和一些必要的功能包，添加gazebo插件来模拟IMU传感器和差分差速里程计系统，则可以有topic进行发布sensor_msgs/Imu和nav_msgs/Odometry，

Adding Gazebo Plugins to a URDF

下面我们会添加IMU传感器和差分差速插件到URDF文件

对我们的机器人，我们将会有一个传感器插件GazeboRosImuSensor),一个传感器插件必须要链接一个link，因此我们创建了一个imu_link与之链接，这个link会被关键`refernence关联到传感器上。

接下来我们会设置topic的名字，并且遵守REP145的规定，设定了initalOrientationAsReference为false，最后为传感器添加一些噪声

下面的代码配置了IMU，建议放在</robot>之上

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
<link name="imu_link">
  <visual>
    <geometry>
      <box size="0.1 0.1 0.1"/>
    </geometry>
  </visual>

  <collision>
    <geometry>
      <box size="0.1 0.1 0.1"/>
    </geometry>
  </collision>

  <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/>
</link>

<joint name="imu_joint" type="fixed">
  <parent link="base_link"/>
  <child link="imu_link"/>
  <origin xyz="0 0 0.01"/>
</joint>

 <gazebo reference="imu_link">
  <sensor name="imu_sensor" type="imu">
   <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
      <ros>
        <namespace>/demo</namespace>
        <remapping>~/out:=imu</remapping>
      </ros>
      <initial_orientation_as_reference>false</initial_orientation_as_reference>
    </plugin>
    <always_on>true</always_on>
    <update_rate>100</update_rate>
    <visualize>true</visualize>
    <imu>
      <angular_velocity>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </z>
      </angular_velocity>
      <linear_acceleration>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </z>
      </linear_acceleration>
    </imu>
  </sensor>
</gazebo>

接下来，我们添加差速驱动插件，配置插件发布名字为/demo/odom的topic。

添加以下代码到IMU插件的</gazebo>之后

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
<gazebo>
  <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
    <ros>
      <namespace>/demo</namespace>
    </ros>

    <!-- wheels -->
    <left_joint>drivewhl_l_joint</left_joint>
    <right_joint>drivewhl_r_joint</right_joint>

    <!-- kinematics -->
    <wheel_separation>0.025</wheel_separation>
    <wheel_diameter>0.2</wheel_diameter>

    <!-- limits -->
    <max_wheel_torque>20</max_wheel_torque>
    <max_wheel_acceleration>1.0</max_wheel_acceleration>

    <!-- output -->
    <publish_odom>true</publish_odom>
    <publish_odom_tf>true</publish_odom_tf> <!-- 区别是什么 -->
    <publish_wheel_tf>true</publish_wheel_tf>

    <odometry_frame>odom</odometry_frame>
    <robot_base_frame>base_link</robot_base_frame>
  </plugin>
</gazebo>

Launch and Build Files

我们现在编辑一下launch文件，使其可以孵化一个机器人sam_bot在gazebo中。因为我们已经开始模拟了 ，因此已经不需要之前的GUI界面了，在launch文件中移除下面的内容

1
2
3
4
5
6
joint_state_publisher_gui_node = launch_ros.actions.Node(
  package='joint_state_publisher_gui',
  executable='joint_state_publisher_gui',
  name='joint_state_publisher_gui',
  condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
)

在 return launch.LaunchDescription中删除下面这一行

1
joint_state_publisher_gui_node,

现在打开package.xml，删除下面这一行

1
<exec_depend>joint_state_publisher_gui</exec_depend>

为了启动gazebo，我们需要在 joint_state_publisher_node, 之前加入下面的代码

1
launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'], output='screen'),

现在添加一个孵化机器人sam_bot的节点，把下面的代码添加到return launch.LaunchDescription([之前

1
2
3
4
5
6
spawn_entity = launch_ros.actions.Node(
  package='gazebo_ros',
  executable='spawn_entity.py',
  arguments=['-entity', 'sam_bot', '-topic', 'robot_description'],
  output='screen'
)

然后在rviz之前添加spawn_entity

1
2
3
4
      robot_state_publisher_node,
      spawn_entity,
      rviz_node
])

Build, Run and Verification

下面我们可以查看一下，/demo/imu和/demo/odom是否已经在ros的中。进入项目根目录

1
2
3
colcon build
. install/setup.bash
ros2 launch sam_bot_description display.launch.py

../../_images/gazebo_sam_bot.png

打开另外一个终端，可以查看

1
ros2 topic list

你应该可以看见 /demo/imu and /demo/odom 在返回的列表中。

也可以通过如下命令查看更多关于topic的内容

1
2
ros2 topic info /demo/imu
ros2 topic info /demo/odom

应该可以看见如下的返回内容

1
2
3
Type: sensor_msgs/msg/Imu
Publisher count: 1
Subscription count: 0
1
2
3
Type: nav_msgs/msg/Odometry
Publisher count: 1
Subscription count: 0

可以看见， /demo/imu 话题应该发布 sensor_msgs/Imu 消息类型， /demo/odom 应该发布 nav_msgs/Odometry消息类型，这两个话题发布者是我们设定的两个插件。同时也要注意，现在两个topic都还没有订阅者。在下一节中，我们会用robot_localization订阅这两个话题，然后返回出一个经过融合后的，光滑的，局部精确的里程计信息给Nav2。

Robot Localization Demo

robot_localization功能包可以把多个传感器里程计的输入进行融合输出一个局部准确的平滑里程计信息。

这些输入可以是 nav_msgs/Odometry、sensor_msgs/Imu、geometry_msgs/PoseWithCovarianceStamped 和 geometry_msgs/TwistWithCovarianceStamped 消息。

一个普通的机器人至少有IMU和轮式编码器来作为获取里程信息，当有多个里程信息提供给robot_localization，robot_localization可以通过状态估计结点来将这些里程信息进行融合，融合的方法为扩展卡尔曼滤波（ekf_node）或者是卡尔曼滤波（ukf_node）。除此之外，这个包还实现了一个navsat_transform_node，即当有GPS的时候，他可以提供一个把世界坐标系到机器人坐标系的转换。

融合的数据会通过 odometry/filtered 和 accel/filtered来进行发布，另外，他也可以在tf中发布odom=>base_link的变换。

如果您的机器人只能提供一个里程计源，那么除了平滑之外，robot_localization 的影响很小。 在这种情况下，另一种方法是通过 tf2 广播器在您的单一里程计节点源中发布转换。 尽管如此，您仍然可以选择使用 robots_localization 来发布变换，并且在输出中仍可能会观察到一些平滑属性。

在本节的其余部分，我们将展示如何使用 robot_localization 来融合 sam_bot 的传感器信息。 它将使用/demo/Imu 上发布的sensor_msgs/Imu 消息和/demo/odom 上发布的nav_msgs/Odometry 消息，然后把融合数据发布在odometry/filtered、accel/filtered 和/tf 上。

Configuring Robot Localization

下面我们将使用robot_localization功能包中的扩展卡尔曼滤波对里程计信息进行融合，并且发布odom=>base_link变换

首先安装一下robot_localization功能包

1
sudo apt install ros-<ros2-distro>-robot-localization

下面，我们利用YAML文件指定ekf-node的参数，在项目的根目录下建立config文件夹，建立文件ekf.yaml，将如下内容

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
### ekf config file ###
ekf_filter_node:
    ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of theinputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
        frequency: 30.0

# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
        two_d_mode: false

# Whether to publish the acceleration state. Defaults to false if unspecified.
        publish_acceleration: true

# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified.
        publish_tf: true

# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
#    to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
#    observations) then:
#     3a. Set your "world_frame" to your map_frame value
#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
#         from robot_localization! However, that instance should *not* fuse the global data.
        map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: base_link  # Defaults to "base_link" ifunspecified
        world_frame: odom           # Defaults to the value of odom_frame if unspecified

        odom0: demo/odom
        odom0_config: [true,  true,  true,
                       false, false, false,
                       false, false, false,
                       false, false, true,
                       false, false, false]

        imu0: demo/imu
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      false, false, false,
                      false, false, false]

在这个配置文件里，我们定义了 frequency, two_d_mode, publish_acceleration, publish_tf, map_frame, odom_frame, base_link_frame, 和 world_frame. 更多配置信息可以查看官网

要将新的传感器输入添加到 ekf_filter_node，只要把数字直接添加到基本名称后面即可（odom、imu、pose、twist）。 在我们的例子中，我们有一个 nav_msgs/Odometry 和一个 sensor_msgs/Imu 作为过滤器的输入，因此我们使用 odom0 和 imu0。 我们将odom0的值设置为demo/odom，也就是发布nav_msgs/Odometry的topic。 同样，我们将imu0的值设置为发布sensor_msgs/Imu的topic，即demo/imu。

您可以使用_config 参数指定过滤器要使用传感器中的哪些值。 该参数取值顺序为x、y、z、roll、pitch、yaw、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。 在我们的示例中，我们将 odom0_config 中的所有内容都设置为 false，除了第 1、2、3 和 12 个条目，这意味着过滤器将仅使用odom0的 x、y、z 和 vyaw 值。

在 imu0_config 矩阵中，您会注意到仅使用了 roll、pitch 和 yaw。 典型的移动机器人级 IMU 还将提供角速度和线性加速度。 为了使robot_localization 正常工作，您不应该在多个相互衍生的领域中融合。 由于角速度在内部融合到 IMU 以提供滚转、俯仰和偏航估计，我们不应该融合用于导出该信息的角速度。 我们也不会融合角速度，因为它在不使用特别高质量（和昂贵）的 IMU 时具有噪声特性。

Launch and Build Files

现在我们添加了ekf_node到launch文件，打开launch/display.launch.py。添加以下结点

1
2
3
4
5
6
7
robot_localization_node = launch_ros.actions.Node(
       package='robot_localization',
       executable='ekf_node',
       name='ekf_filter_node',
       output='screen',
       parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}]
)

然后添加launch 参数到return launch.LaunchDescription([中，

1
2
launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True',
                                            description='Flag to enable use_sim_time'),

最后把robot_localization添加到rviz_node的前面

1
2
3
4
5
      robot_state_publisher_node,
      spawn_entity,
      robot_localization_node,
      rviz_node
]

然后，我们去package.xml里面添加运行依赖

1
<exec_depend>robot_localization</exec_depend>

修改CMakeLists.txt，把config文件添加到编译里面

1
2
3
4
install(
  DIRECTORY src launch rviz config
  DESTINATION share/${PROJECT_NAME}
)

Build, Run and Verification

1
2
3
colcon build
. install/setup.bash
ros2 launch sam_bot_description display.launch.py

Gazebo 和 Rviz 应该被启动，在Rviz里面，你应该看见我们的机器人以及TF坐标系

../../_images/rviz.png

下面我们检查一下 odometry/filtered, accel/filtered,和 /tf是否在系统的topic中，打开另外一个终端

1
ros2 topic list

你应该看见他们在topic的列表中

我们还可以检查一下 subscriber 的数量

1
2
ros2 topic info /demo/imu
ros2 topic info /demo/odom

你会发现他们俩都有一个各自的订阅者

检查ekf_filter_node结点是哪些话题的订阅者

1
ros2 node info /ekf_filter_node

应该可以看见如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
/ekf_filter_node
Subscribers:
  /demo/imu: sensor_msgs/msg/Imu
  /demo/odom: nav_msgs/msg/Odometry
  /parameter_events: rcl_interfaces/msg/ParameterEvent
  /set_pose: geometry_msgs/msg/PoseWithCovarianceStamped
Publishers:
  /accel/filtered: geometry_msgs/msg/AccelWithCovarianceStamped
  /diagnostics: diagnostic_msgs/msg/DiagnosticArray
  /odometry/filtered: nav_msgs/msg/Odometry
  /parameter_events: rcl_interfaces/msg/ParameterEvent
  /rosout: rcl_interfaces/msg/Log
  /tf: tf2_msgs/msg/TFMessage
Service Servers:
   ...

从输出可以看出，ekf_filter_node订阅了/demo/imu和/demo/odom，同时他也发布了odometry/filtered, accel/filtered,和 /tf。

我们也可以检查robot_localization正在发布变换odom=>base_link，

1
ros2 run tf2_ros tf2_echo odom base_link

Conclusion

在本指南中，我们讨论了 Nav2 期望从里程计系统中获得的消息和转换。 我们已经看到了如何设置里程计系统以及如何验证发布的消息。 我们还讨论了如何使用多个里程计传感器来提供使用机器人定位的过滤和平滑的里程计。 我们还检查了 odom => base_link 转换是否由robot_localization 正确发布。

 Comments