时光荏苒，珍惜当下

机器人搭建与调试6：里程计信息融合功能包 robot_pose_ekf

发表于 更新于 分类于 Valine：

  robot_pose_ekf功能包使用带有6D（3D position and 3D orientation）模型信息的扩展卡尔曼滤波器来整合来自轮子里程计，IMU传感器和视觉里程计的数据信息，来估算机器人的3D位姿。其基本思路就是用松耦合方式融合不同传感器信息实现位姿估计。

robot_pose_ekf 功能包介绍

订阅话题

  robot_pose_ekf 默认订阅的topic为：imu_data、odom 和 vo，因此要注意发布消息时topic的名称要对应，否则会起不到滤波作用。不想使用默认名称的话可以用remap元素进行名称重映射。

2D pose (used by wheel odometry): 该2D pose包含了机器人在地面的位置（position）和方位（orientation）信息以及该位姿的协方差（covariance）。用来发送该2D位姿的消息实际上表示了一个3D位姿，只不过把z，pitch和roll分量简单忽略了（平面上的轮式移动机器人）。

3D orientation (used by the IMU): 3D方位提供机器人底座相对于世界坐标系的Roll, Pitch and Yaw偏角。 Roll and Pitch角是绝对角度（因为IMU使用了重力参考），而YAW角是相对角度。 协方差矩阵用来指定方位测量的不确定度。当仅仅收到这个主题消息时，机器人位姿ekf还不会启动，因为它还需要来自主题’vo’或者’odom’的消息。

3D pose (used by Visual Odometry): 3D位姿可以完整表示机器人的位置和方位并给出位姿协方差。当用传感器只测量部分3D位姿(e.g. the wheel odometry only measures a 2D pose)时候, 可以给还未真正开始测量的部分3D位姿先简单指定一个大的协方差。

发布话题

TF 变换

  • odom_combined → base_footprint

在ROS小车中

  • /imu_data话题来自 razor_imu_9dof传感器。
  • /arduino 发布话题 /odom ，消息格式为 nav_msgs/Odometry；
  • /robot_pose_ekf 发布话题 robot_pose_ekf/odom_combined， 消息格式为 geometry_msgs/PoseWithCovarianceStamped。两者消息格式不一样，需要转换格式再使用。

robot_pose_ekf 功能包使用

robot_pose_ekf launch 文件

robot_pose_ekf.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14

<launch>
      <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
        <param name="output_frame" value="odom_combined"/>
        <param name="base_footprint_frame" value="base_footprint"/>
        <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="false"/>
        <param name="debug" value="false"/>
        <param name="self_diagnose" value="false"/>
    </node>
 </launch>
- freq: 滤波器更新和发布频率。注意：频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高每次位姿评估的精度。
- sensor_timeout: 当传感器停止向滤波器发送信息时，滤波器在没有传感器的情况下等待多长时间才重新开始工作。
- odom_used, imu_used, vo_used: 是否订阅各类数据。

TF 树维护

（1）打开终端，运行robot_pose_ekf 节点

$ roscore
$ roslaunch ros_arduino_python arduino.launch （树莓派3B）
$ roslaunch razor_imu_9dof razor-pub.launch （树莓派3B）
$ roslaunch robot_pose_ekf robot_pose_ekf.launch

（2）修改程序

  • 查看tf树
$ rosrun rqt_tf_tree rqt_tf_tree
（图）修改前的TF树

可以看出有两个TF树:

/odom -> /base_link : /odom 来自 /arduino 节点，
/odom_combined -> /base_footprint : /odom_combined 来自 /robot_pose_ekf 节点

这是因为 ros_arduino_bridge 包下 base_controller.py 文件发布了 /odom ，应该注释掉这一段，使用 /robot_pose_ekf 节点发布的tf。

  • 修改 base_controller.py 文件
base_controller.py
1
2
3
4
5
6
7
8
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
	(self.x, self.y, 0), 
	(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
	rospy.Time.now(),
	self.base_frame,
	"odom"
)
  • 修改 robot_pose_ekf.launch 文件
robot_pose_ekf.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
<launch>
	<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
		<param name="output_frame" value="odom"/>
		<param name="base_footprint_frame" value="base_link"/>
		<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="false"/>
		<param name="debug" value="false"/>
		<param name="self_diagnose" value="false"/>
		<remap from="imu_data" to="imu" />
		<!-- 将节点订阅的 imu_data 话题改名为 imu,如果 imu 节点发布的话题是 imu_data 就不用修改 -->
	</node>
 </launch>
output_frame：更改 odom_combined 为 odom
base_footprint_frame：更改 base_footprint 为 base_link
imu节点重映射：将将节点订阅的 imu_data 话题改名为 imu
  • 再查看tf树
（图）修改后的TF树

可以看出 /odom 来自 /robot_pose_ekf ，TF树正确无误，可以进行下一阶段。

添加协方差矩阵

imu数据的协方差矩阵： razor_imu_9dof 的IMU，它有官方提供的软件包，会带有covariance，不需要添加。
底盘运动时odom的协方差矩阵：打开 ros_arduino_bridge包下 python base_controller.py ，添加协方差矩阵。

base_controller.py
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
odom = Odometry()
            odom.header.frame_id = "odom"
            odom.child_frame_id = self.base_frame
            odom.header.stamp = now
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.pose.covariance = [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.twist.twist.linear.x = vxy
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = vth
            odom.twist.covariance = [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]

            self.odomPub.publish(odom)

参考

完
+