IMU与里程计融合

最新推荐文章于 2024-07-13 09:50:39 发布

Karen_bluu

最新推荐文章于 2024-07-13 09:50:39 发布

阅读量3.1k

点赞数 33

分类专栏： RM视觉导航学习文章标签： ubuntu linux 3d

本文链接：https://blog.csdn.net/Hong_J_0826/article/details/136075483

版权

RM视觉导航学习专栏收录该内容

8 篇文章 2 订阅

订阅专栏

1.概述

实际使用小车的过程中，光有麦轮的里程计计算是不够的，因为实际使用过程中经常会出现轮子打滑和数据出现累计误差的情况，所以我们可以从使用IMU和里程计融合的角度来对系统“升级”。

2.IMU

2.1简述

IMU 一般指6轴传感器，内部包含了3轴陀螺仪和3轴加速度计，3轴就是表示 XYZ 平面下的3个坐标轴。

3轴陀螺仪测量的是每个轴上面的角速度，精度一般为 °/s ，也就是按照这个趋势旋转，每秒钟能走过的度数。

3轴加速度计测量的是每个轴所受的重力加速度，比如传感器水平放置水平地面时，理论上只受到z轴负向的重力加速度，大小为9.8 m/s^2 1

2.2原理

详细了解：http://t.csdnimg.cn/3YFSX

3.IMU数据获取

3.1数据接口

数据接口，通过配置可以输出原始 `ros_imu topic

还有更多方法

3.2滤波

ROS提供的相关包imu_tools进行滤波

可以看到complementary_filter_gain_node会订阅该topic，即该topic作为输入滤波得到最终数据(发布/imu/data topic 类型同样为sensor_msgs/Imu)

4.融合方法

4.1直接融合

从imu得到的数据为一个相对角度(主要使用yaw，roll和pitch 后面不会使用到)，使用该角度来替代由编码器计算得到的角度。这个方法较为简单，出现打滑时候因yaw不会受到影响，即使你抬起机器人转动一定的角度，得到的里程也能正确反映出来

4.2卡尔曼滤波

4.2.1概述

官方卡尔曼滤波的包robot_pose_ekf，robot_pose_ekf开启扩展卡尔曼滤波器生成机器人姿态，支持

odom（编码器）
imu_data（IMU）
vo（视觉里程计）还可以支持[GPS](robot_pose_ekf/Tutorials/AddingGpsSensor - ROS Wiki）

4.2.2使用

4.2.2.1配置

可以在 robot_pose_ekf 包目录中找到 EKF 节点的默认启动文件。启动文件包含许多可配置的参数：

freq：过滤器的更新和发布频率。请注意，随着时间的推移，更高的频率会为您提供更多的机器人姿势，但不会提高每个估计的机器人姿势的准确性。
sensor_timeout：当传感器停止向过滤器发送信息时，过滤器应该等待多长时间才能在没有该传感器的情况下继续工作。
odom_used、imu_used vo_used：启用或禁用输入。

可以在启动文件中修改配置，如下所示：

4.2.2.2运行

创建程序包：

  $ rosdep install robot_pose_ekf
  $ roscd robot_pose_ekf
  $ rosmake

Run the robot pose ekf

  $ roslaunch robot_pose_ekf.launch

4.2.3相关节点

4.2.3.1`robot_pose_ekf`

robot_pose_ekf实现了一个扩展的卡尔曼滤波器，用于确定机器人姿态。

4.2.3.2 Subscribed Topics

Odom（nav_msgs/里程计)

2D pose（由车轮里程计使用）：2D 姿势包含机器人在地平面上的位置和方向以及该姿势的协方差。发送此 2D 姿势的消息实际上表示 3D 姿势，但 z、滚动和俯仰被简单地忽略。

imu_data （sensor_msgs/Imu)

3D orientation（由 IMU 使用）：3D 方向提供有关机器人底架相对于世界参考系的滚动角、俯仰角和偏航角的信息。横滚角和俯仰角被解释为绝对角度（因为 IMU 传感器具有重力参考），偏航角被解释为相对角度。协方差矩阵指定了方向测量的不确定度。robot_pose_ekf 在仅接收有关此主题的消息时不会启动;它还需要有关“VO”或“Odom”主题的消息。

vo（nav_msgs/里程计)

3D pose（由视觉里程计使用）：3D pose表示机器人的完整位置和方向以及该pose的协方差。当传感器仅测量 3D 姿势的一部分时（例如，车轮里程计仅测量 2D 姿势），只需在未实际测量的 3D pose部分上指定较大的协方差即可。

robot_pose_ekf节点不要求所有三个传感器源始终可用。每个源都给出了pose估计值和协方差。这些源以不同的速率和不同的延迟运行。源可以随时间推移出现和消失，节点将自动检测并使用可用的传感器。若要添加自己的传感器输入，请查看添加 GPS 传感器教程

4.2.3.3 Published Topics

robot_pose_ekf/odom_combined （geometry_msgs/PoseWithCovarianceStamped)

过滤器的输出（估计的 3Drobot pose）

4.2.3.4提供的`tf` 转换

 odom_combined`→`base_footprint

4.3工作原理

4.3.1Poes解释

所有向滤波器节点发送信息的传感器源都可以有自己的世界参考系，并且每个世界参考系都可以随时间任意漂移。因此，不同传感器发送的absolute Pose无法相互比较。该节点使用每个传感器的relative pose differences来更新扩展的卡尔曼滤波器。

4.3.2协方差解释(Covariance interpretation)

随着机器人四处移动，其在世界参考中pose的不确定性越来越大。随着时间的流逝，协方差将无限增长。因此，发布pose本身的协方差是没有用的，相反，传感器源会发布协方差如何随时间变化，即速度上的协方差。请注意，使用对世界的观察（例如，测量到已知墙壁的距离）将减少机器人姿势的不确定性;然而，这是定位，而不是里程计。

4.3.3Timing

想象一下，机器人姿势过滤器上次更新的时间是t_0。在到达每个传感器的至少一次测量值且时间戳晚于 t_0 之前，节点不会更新机器人姿势过滤器。例如，当在时间戳t_1 > t_0的 odom 主题和时间戳t_2 > t_1 > t_0的 imu_data 主题上收到消息时，过滤器现在将更新到有关所有传感器的信息可用的最新时间，在本例中为时间t_1。直接给出t_1处的 odom 位姿，通过对 t_1 和 t_0 之间的 imu 位姿进行线性插值得到 t_2 处的 imu位姿。机器人姿势过滤器使用odom和imu的相对姿势进行更新，介于 t_0 和 t_1 之间。

上图显示了PR2机器人从给定的初始位置（绿点）开始，驱动并返回初始位置时的实验结果。完美的里程计 x-y 图应显示精确的闭环。蓝线表示车轮里程计的输入，蓝点表示估计的结束位置。红线表示robot_pose_ekf的输出，它结合了车轮里程计和imu的信息，红点是估计的结束位置。

4.3.4 包裹状态

4.3.4.1稳定性

这个包的代码库已经过很好的测试，并且已经稳定了很长时间。然而，随着消息类型的演变，ROS API 一直在变化。在未来的版本中，ROS API 可能会再次更改为简化的单主题界面（请参阅下面的路线图）。

4.3.4.1路线图

该滤波器目前设计用于我们在 PR2 机器人上使用的三个传感器信号（车轮里程计、imu 和 vo）。我们计划使这个包更加通用：未来的版本将能够监听“n”个传感器源，所有源都发布（nav_msgs/里程计）消息。每个源都将在里程计消息中设置 3D 姿势的协方差，以指定它实际测量的 3D 姿势的哪一部分。
我们想将速度添加到扩展卡尔曼滤波器的状态中。