网上收集关于robot_pose_ekf扩展卡尔曼融合包的使用
ROS导航功能包：ros-planning/navigation
中国大学mooc：Navigation 工具包说明
目录
一、csdn：ROS下robot_pose_ekf扩展卡尔曼融合包的使用
二、ROS wiki：带外部传感器的robot_pose_ekf
四、csdn：22.IMU和里程计融合
五、csdn：使用 robot_pose_ekf 对imu和odom进行融合
一、csdn：ROS下robot_pose_ekf扩展卡尔曼融合包的使用
ROS下robot_pose_ekf扩展卡尔曼融合包的使用
有一个imu，而且校准的好（博主使用的是razor 9dof的imu，通过ros包pub一个Imu的data类型）
原先的tf_tree中可以看到这个单单使用里程计的tf图片：（由arduion转换的，底层通信的node里发布的）
由于ekf包会为我们处理好这部分tf，所以不需要我们发布变换了，所以需要将上面这部分发布的代码注释掉！
之后我们需要加一个imu的link（由于我们的比赛中已经有了这个link，所以这一步就不做处理：静态发布imu的tf）
现在开始进行使用ekf包来融合：
- 开启底盘的launch发布odom的topic
- 开启imu的launch发布imu的topic
- 开启运行robot_pose_ekf的launch
为了查看ekf包是否正常工作，使用以下代码查看：
rosservice call /robot_pose_efk/get_status
查看Odometry sensor和IMU sensor 是否is used ，is active来确保ekf包的是否正确使用
如果成功，那么tf图是这样的，odom–base_footprint的转换就是由/robot_pose_ekf发布的
至于融合后的效果，就要看imu的校准度了！！！看效果的话可以用rviz或者导出rosbag包用matlab的plot函数绘出来，matlab可能更直观点，可以看这个：使用Matlab与ROS端通信以及绘制Odom里程计信息
二、ROS wiki：带外部传感器的robot_pose_ekf
带外部传感器的robot_pose_ekf
三、创客智造：ROS与navigation教程-robot_pose_ekf
ROS与navigation教程-robot_pose_ekf
-
Robot Pose EKF 包用于评估机器人的3D位姿，基于来自不同来源的位姿测量信息。它使用带有6D（3D position and 3D orientation）模型信息的扩展卡尔曼滤波器来整合来自轮式里程计，IMU传感器和视觉里程计的数据信息。基本思路就是用松耦合方式融合不同传感器信息实现位姿估计。
如何使用：EKF节点的默认启动文件位于robot_pose_ekf包目录中
参数：
- freq: 滤波器更新和发布频率。注意：频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高每次位姿评估的精度。
- sensor_timeout: 当传感器停止向滤波器发送信息时，滤波器在没有传感器的情况下等待多长时间才重新开始工作。
- odom_used, imu_used, vo_used: 确认是否输入
编译运行：
rosdep install robot_pose_ekf
roscd robot_pose_ekf
rosmake
roslaunch robot_pose_ekf.launch节点：
订阅的话题：
odom ([nav_msgs/Odometry][2])
2D pose (用于轮式里程计)：其包含机器人在地面中的位置（position）和方位（orientation）以及该位姿的协方差。 发送此2D位姿的消息实际上表示3D位姿，但z，pitch和roll分量被简单忽略了。imu_data ([sensor_msgs/Imu][3])
3D orientation (用于IMU):3D方位提供机器人基座相对于地图坐标系的Roll, Pitch and Yaw偏角。Roll and Pitch角是绝对角度（因为IMU使用了重力参考），而YAW角是相对角度。协方差矩阵指定的方位测量的不确定度。当仅仅收到这个主题消息时，机器人位姿ekf还不会启动，因为它还需要来自主题'vo'或者'odom'的消息。vo ([nav_msgs/Odometry][4])
3D pose (用于视觉里程计):3D位置表示机器人的完整位置和方位以及该位姿的协方差。当用传感器只测量部分3D位姿(e.g. the wheel odometry only measures a 2D pose)时候, 可以给还未真正开始测量的部分3D位姿先简单指定一个大的协方差。发布的话题：
robot_pose_ekf/odom_combined ([geometry_msgs/PoseWithCovarianceStamped][6])
滤波器输出 (评估的3D机器人位姿)。tf转换：odom_combined → base_footprint
四、csdn：22.IMU和里程计融合
22.IMU和里程计融合
实际使用中会出现轮子打滑和累计误差的情况，这里单单使用编码器得到里程计会出现一定的偏差，虽然激光雷达会纠正，但一个准确的里程对这个系统还是较为重要。
IMU即为 惯性测量单元，一般包含了三个单轴的加速度计和三个单轴的陀螺仪，简单理解通过加速度二次积分就可以得到位移信息、通过角速度积分就可以得到三个角度
查看话题：/robot_pose_ekf/odom_combined
rostopic info /robot_pose_ekf/odom_combined
类型需要注意下是PoseWithCovarianceStamped并非Odometry,后面会用到这个作为显示，所以还需要一个转换
查看该topic信息可以看到odom_ekf订阅了该topic,再次查看该节点信息可以看到
他会发出一个Odometry的topic,发出一个tf
查看订阅滤波器输出话题的节点：/odom_ekf
rosnode info /odom_ekf 
在
robot_pose_ekf配置时，做了些映射处理，这样可以保证导航层在使用和不用imu的时候无需修改就可以工作,有无imu输出的tf都是odom → base_footprint，发出的里程都是odom，所以将带有imu的里程topic映射为wheel_odom
五、csdn：使用 robot_pose_ekf 对imu和odom进行融合
使用 robot_pose_ekf 对imu和odom进行融合
注意发布消息时 topic 的名称要对应,否则会起不到滤波作用！！！
robot_pose_ekf.launch 文件如下:
- output_frame, base_footprint_frame:用于指定输出 tf 变换中坐标系的名字,默认为 odom_combined 和 base_footprint。
注意使用robot_pose_ekf 进行滤波时传感器的协方差矩阵信息不能空着,否则可能会出现错误,因此要设置合理的值。
- 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 的协方差矩阵如下,参考:
https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/covariances.py
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]
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]
注意 imu 信息的协方差矩阵中代表机器人航向角的分量方差为 1e-6,而里程计信息的协方差矩阵中机器人姿态分量的协方差为1e3,两个值相差很大。在进行 EKF 融合时,会更“相信”imu 提供的姿态信息,因为其方差更小。
比如机器人在转动过程中轮子发生了打滑,用编码器推算出的姿态一直在旋转,而实际姿态(主要由 IMU 测量得到)却没发生太大变化,这种情况就需要使用信息融合方法来减小误差。协方差矩阵中的参数设置非常重要,要根据传感器手册或者实际使用测量来确定。
我们得到了/robot_pose_ekf/odom_combined 消息：
在 movebase 需要订阅/odom 的话题,想让这个消息被 movebase使用
这里主要是看 rbx_bringup 包里提供的一个节点:odom_ekf.py，odom_ekf.py 就是将它们转化用的
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
from nav_msgs.msg import Odometry
class OdomEKF():
def __init__(self):
# Give the node a name
rospy.init_node('odom_ekf', anonymous=False)
# Publisher of type nav_msgs/Odometry
self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=10)
# Wait for the /odom_combined topic to become available
rospy.wait_for_message('input', PoseWithCovarianceStamped)
# Subscribe to the /odom_combined topic
rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
rospy.loginfo("Publishing combined odometry on /odom_ekf")
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.header.frame_id = '/odom'
odom.child_frame_id = 'base_footprint'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
if __name__ == '__main__':
try:
OdomEKF()
rospy.spin()
except:
pass
很简单, /odom和/robot_pose_ekf/odom_combined它们消息类型是不同的,前者是nav_msgs/Odometry,后者是geometry_msgs/PoseWithCovarianceStamped ,两者的区别:后者的内容是前者的一部分
odom_ekf.py 就是将它们转化用的。
转化后还要在发布融合信息的 launch 文件里将/robot_pose_ekf/odom_combined 话题 remap 成/odom_ekf.就可以给 movebase 使用了!
这个节点就订阅/robot_pose_ekf /odom_combined 下的位置估计信息然后发布/odom_ekf
/odom_ekf 和/odom 的信息类型相同
回复：
在move_base将odom改为odom_ekf，主要是在源码中回调函数的消息类型是nav::Odometry,消息类型需要一致，如果没有用odom_ekf.py，应该会报错