ekf 传感器数据融合

最新推荐文章于 2024-06-30 16:29:42 发布

人非人1991

最新推荐文章于 2024-06-30 16:29:42 发布

阅读量1.6k

点赞数 1

分类专栏： Linux C/C++ 文章标签： ekf ros

Linux C/C++ 专栏收录该内容

13 篇文章 1 订阅

订阅专栏

robot_pose_ekf.launch 默认配置： 2d odom imu vo

</launch>

2.1.1 订阅主题

odom（编码器）
(nav_msgs/Odometry)2D pose (轮式里程计): 二维姿态包含机器人平面中的坐标和朝向以及方位协方差。平面机器人中，其中z, roll and pitch 忽略。

imu_data（IMU）
(sensor_msgs/Imu)3D orientation (used by the IMU): 提供包含相对世界坐标系的Roll, Pitch 和 Yaw 角度。 Roll 和 Pitch 角是绝对角度，Yaw是相对角度。协方差矩阵决定了方向的不确定性， robot pose ekf 在仅仅接收此msg时不会工作，它需要 'vo' 或'odom' 主题。

vo（视觉里程计）
(nav_msgs/Odometry)3D pose (used by Visual Odometry): 此主题包含机器人全方向及相应协方差信息，当传感器只测量部分3维信息时，需要制定一个较大的协方差用来忽略此项数据。

The robot_pose_ekf节点不需要三个主题同时有效，每个主题数据都会产生一个位置估计及协方差。主题的频率也不一定一致，也具有不同的延迟。数据间歇收发，这时节点将自动检测使用有效的传感数据。添加自主的传感数据参照GPS的示例即可。 http://wiki.ros.org/robot_pose_ekf/Tutorials/AddingGpsSensor

2.1.2 发布的主题

robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)
《滤波器的输出 (the estimated 3D robot pose)》

2.1.3 TF变换

odom_combined → base_footprint\

3. Robot Pose EKF如何工作

3.1 位置解析

所有传感器发送的数据依据自己的世界坐标系，每个世界坐标系将随着时间漂移，因此，各个传感器发送的绝对位置无法直接比较，节点使用每个传感器的相对坐标系更新滤波器。

3.2 协方差解析

随着机器人行动，机器人全局位置的不确定性越来越大，协方差也将逐渐增大而无界限。因此在位置本身发布协方差是毫无意义的，取而代之的是传感器数据发布随时间变化的协方差， *注意通过外部传感器观测将减少估计的不确定性，不过这是定位技术而不是轨迹推演。

3.3 Timing时间

假设滤波器上一刻在t_0时间更新，直到某一传感器数据的时间戳在t_0之后获取，滤波器才会工作。. The node will not update the robot pose filter until at least one measurement ofeach sensor arrived with a timestamp later than t_0. When e.g. a message was 当一个odom传感器接受时间 t_1 > t_0，IMU的数据时间戳在t_2 > t_1 > t_0，在上一刻所有传感器数据都就绪的时间戳滤波器将会更新。本例在 t_1具有odom数据而IMU数据是t_0 到 t_2间线性插入的，滤波器将会更新t_0 到 t_1间的相对odom和IMU数据。