在仿真下使用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]文章来源:https://www.toymoban.com/news/detail-490983.html
与真实无人机高度吻合文章来源地址https://www.toymoban.com/news/detail-490983.html
到了这里,关于ROS EKF 机器人位姿估计功能包:robot_pose_ekf | 仿真环境实践的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!