ROS源代码阅读（12）——robot_pose_ekf

最新推荐文章于 2023-10-25 11:49:28 发布

qzyj99

最新推荐文章于 2023-10-25 11:49:28 发布

阅读量1.7k

点赞数

分类专栏： ROS代码分析文章标签： python

本文链接：https://blog.csdn.net/qzyj99/article/details/122138002

版权

ROS代码分析专栏收录该内容

14 篇文章 12 订阅

订阅专栏

2021SC@SDUSC
ROS源代码阅读（12）

这一篇我们主要看robot_pose_ekf。

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

robot_pose_ekf的使用：

EKF节点的默认启动文件位于robot_pose_ekf包目录中。启动文件包含多个可配置参数：

freq: 滤波器更新和发布频率。注意：频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高每次位姿评估的精度。

sensor_timeout: 当传感器停止向滤波器发送信息时，滤波器在没有传感器的情况下等待多长时间才重新开始工作。

odom_used, imu_used, vo_used: 确认是否输入。

节点就是robot_pose_ekf
订阅的话题有：

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节点不要求所有三个传感器源一直可用。每个源给出位态估计和协方差。源以不同的速率和延迟进行操作。源可以随着时间的推移出现并消失，节点将自动检测并使用可用的传感器。

发布的话题：

robot_pose_ekf/odom_combined ([geometry_msgs/PoseWithCovarianceStamped][6])

滤波器输出 (评估的3D机器人位姿)。

对于工作原理，通过阅读源码可以大概了解，向滤波器节点发送信息的所有传感器源都有自己的参考坐标系，并且随着时间推移都可能出现漂移现象。因此，不同传感器发送的绝对位姿不能相互比较。节点使用每个传感器的相对位姿差来更新扩展卡尔曼滤波器。
随着机器人的移动，其在参考坐标系中的位姿的不确定性越来越大。同时随着时间的推移，协方差将会无限增长。因此，在位姿本身上公布协方差是没有意义的，而传感器源则会随时间的变化来公布协方差，即速度的协方差。
机器人姿势过滤器最后在时间t_0更新。在每个传感器的至少一次测量到达时间晚于t_0的时间戳之后，节点将不会更新机器人姿态滤波器。
当例如收到有关时间戳t_1> t_0的odom主题的消息，并且在时间戳t_2> t_1> t_0的imu_data主题上，过滤器现在将更新为关于所有传感器的信息可用的最新时间，在这种情况下为时间T_1。直接给出t_1处的odom姿态，并且通过在t_0和t_2之间的imu姿态的线性插值来获得t_1上的imu姿态。在t_0和t_1之间，使用odom和imu的相对姿势来更新机器人姿势过滤器。

对于其中用到的协方差，几点理解：

1)协方差用于描述两个变量之间线性相关的强度。

2)在机器人的应用中，一般都是用多维度的高斯分布来描述机器人的状态。其均值是一个状态列向量。如 ( x y θ ) \left(\begin{array}{l}{x} \ {y} \ {\theta}\end{array}\right) ⎝⎛xyθ⎠⎞是用坐标x,y和偏航角来描述机器人的状态。而各维度的方差用协方差矩阵来表征。协方差矩阵统一描述不同维度间的协方差。它是一个对称矩阵，而且对角线是各个维度上的方差（表征对应维度样本数据的离散程度）。

3）当某维度的协方差为0时说明该维度上的数据一点都不离散，是完全不波动的数据。在后面计算卡尔曼增益 K = P ′ H T S − 1 K=P^{\prime} H^{T} S^{-1} K=P′HTS−1时，因为 P ′ P^{\prime} P′某维度等于0，则计算的该维度的卡尔曼增益 K K K也将为0。根据卡尔曼滤波算法公式 x = x ′ + K y P = ( I − K H ) P ′ \begin{array}{l}{x=x^{\prime}+K y} \ {P=(I-K H) P^{\prime}}\end{array} x=x′+KyP=(I−KH)P′
更新的状态量与上一次的状态量保持一致，协方差也保持和上一状态的一致。由此可以看出此时卡尔曼滤波是不起作用的。