ekf 传感器数据融合

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

  
                             
    
    
      
    
    
   
   
 
 

 

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数据。

ekf 传感器数据融合_第1张图片


上图所示,PR2在给定的起点(绿色)开始,里程计图展示了整个闭环,蓝色线表示编码器的输入,蓝点表示终点。红线表示输出的滤波数据,它融合的滤波器和IMU,红点是估计的终点。

4. 软件包状态

4.1 Stability稳定性

代码经过长期测试ok,随着ROS的API更改,将来也需要做调整。

4.2 路标

滤波器主要用于PR2中的三种传感器输入(wheel odometry, imu and vo) ,我们计划开发更为通用的版本,将来的版本支持多个 (nav_msgs/Odometry)传感器消息输入。

5. 教程

robot_pose_ekf/Tutorials/AddingGpsSensor

其上为粘贴内容。


关于把gps信息加入到其中,在现有的方案中是把gps信息转化为这三个输入话题中的一个话题中来使用。


http://wiki.ros.org/robot_pose_ekf/Tutorials/AddingGpsSensor


http://wiki.ros.org/gps_common




你可能感兴趣的:(Linux,C/C++)