ROS EKF 机器人位姿估计功能包：robot_pose_ekf | 仿真环境实践

在仿真下使用robot_pose_ekf

仿真环境为 一个无人机，具备3D POSE里程计数据，和imu数据。



将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="false"/>
  <param name="imu_used" value="true"/>
  <param name="vo_used" value="true"/>

  <remap from="vo" to="/firefly/odometry_sensor1/odometry" />
  <remap from="imu_data" to="/firefly/imu" />
  
</node>

</launch>

即关掉2D轮速里程计
打开imu和vo里程计
并将话题重映射
vo重映射为/firefly/odometry_sensor1/odometry
imu_data重映射为/firefly/imu

启动 robot_pose_ekf 节点

roslaunch robot_pose_ekf robot_pose_ekf.launch

出现如下报错

[ERROR] [1686016442.630112783, 912.440000000]: Covariance specified for measurement on topic vo is zero
[ WARN] [1686016448.129717214, 917.940000000]: Could not transform imu message from firefly/imu_link to base_footprint. Imu will not be activated yet.

第一个问题解决：
第一个问题，vo的数据协方差都是0

rostopic echo /firefly/odometry_sensor1/odometry

打印话题，查看如下

covariance 部分确实是0

原来在仿真环境里面，里程计的噪声设置为0了，

    noise_normal_position="0.01 0.01 0.01"
    noise_normal_quaternion="0.017 0.017 00.017"
    noise_normal_linear_velocity="0.02 0.02 0.02"
    noise_normal_angular_velocity="0.013 0.013 0.013"

把这里的噪声加上即可

协方差这里不再是0

再运行robot_pose_ekf节点，则不会出现下面这个报错

[ERROR] [1686016442.630112783, 912.440000000]: Covariance specified for measurement on topic vo is zero

第二个问题解决：
第二个问题是
imu消息无法从firefly/imu_link 到 base_footprint ， 这是因为仿真的模型没有base_footprint，imu的坐标系是firefly/imu_link。
所以将robot_pose_ekf.launch中的

<param name="base_footprint_frame" value="base_footprint"/>

改为

<param name="base_footprint_frame" value="firefly/imu_link"/>

再运行即不会报任何错误

可以看到 /robot_pose_ekf/odom_combined 话题下面可以正常输出融合后的里程计信息

rostopic hz /robot_pose_ekf/odom_combined

rostopic echo /robot_pose_ekf/odom_combined

无人机在地面的：

header:
seq: 514
stamp:
secs: 33
nsecs: 680000000
frame_id: “odom_combined”
pose:
pose:
position:
x: 0.0017419902403915423
y: 0.043231049081159115
z: 0.06509762290083827
orientation:
x: 0.007851188432969749
y: 0.007545160963110543
z: -0.01976178367618696
w: 0.999745418240147
covariance: [1.0011717677116394e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0011717677116394e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0011717677116394e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.592218287517943e-09, 9.655282989158597e-09, 9.65528301191329e-09, 0.0, 0.0, 0.0, 9.655282989158597e-09, 9.592218264790355e-09, 9.655283011899738e-09, 0.0, 0.0, 0.0, 9.65528301191329e-09, 9.655283011899738e-09, 9.592218287545048e-09]

无人机在空中的

header:
seq: 3389
stamp:
secs: 158
nsecs: 460000000
frame_id: “odom_combined”
pose:
pose:
position:
x: -0.033636298310285875
y: 0.08253540836294387
z: 1.5229299724233456
orientation:
x: 0.019733241066607503
y: 0.012368392749471947
z: -0.19686997503152537
w: 0.9801529650972284
covariance: [2.498272806406021e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.498272806406021e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.498272806406021e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.409062825302344e-07, 2.4083166145225446e-07, 2.4083166315748764e-07, 0.0, 0.0, 0.0, 2.4083166145225446e-07, 2.409062830983563e-07, 2.408316631579213e-07, 0.0, 0.0, 0.0, 2.4083166315748764e-07, 2.408316631579213e-07, 2.409062848035895e-07]

与真实无人机高度吻合