ROS导航功能包:ros-planning/navigation

中国大学mooc:Navigation 工具包说明

一、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包目录中​​​​​​​

<launch>
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom"/>
    <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="true"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
  </node>
</launch>

参数:

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 文件如下:

<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>

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 使用了!

<launch>
   <node pkg="rbx1_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
   	<remap from="input" to="/robot_pose_ekf /odom_combined"/>
   	<remap from="output" to="/odom_ekf"/>
   </node>
</launch>

这个节点就订阅/robot_pose_ekf /odom_combined 下的位置估计信息然后发布/odom_ekf

/odom_ekf 和/odom 的信息类型相同

回复:

<node pkg="move_base" type="move_base" name="move_base" output="screen"> 
  <remap from="odom" to="odom_ekf" />
 </node> 
 
在move_base将odom改为odom_ekf,主要是在源码中回调函数的消息类型是nav::Odometry,消息类型需要一致,如果没有用odom_ekf.py,应该会报错