robot_localization的EKF论文摘要与平面定位的源码解析

最新推荐文章于 2024-08-15 22:44:52 发布

夏天的一叶

最新推荐文章于 2024-08-15 22:44:52 发布

阅读量1.8k

点赞数 4

分类专栏：传感器融合

本文链接：https://blog.csdn.net/qq_29313679/article/details/105163246

版权

传感器融合专栏收录该内容

6 篇文章 3 订阅

订阅专栏

A Generalized Extended Kalman Filter Implementation for the Robot Operating System

论文摘要

文章的第二部分，讲述了创建robot_localization包的动机
在文章的第三部分，详细描述了ROS的ekf_localization_node的实现
一开始就开发了我们的机器人定位软件包，以克服这些限制，并尽可能通用。它在三维空间中执行状态估计，允许无限数量的传感器，支持多种标准ROS消息类型，并允许每个传感器控制哪个消息字段与状态估计相融合
我们将描述ekf_location节点的实现细节
我们的目标是估计移动机器人的全三维（6自由度）姿态和速度随时间的变化。这个过程可以描述为一个非线性动态系统 $x_{t}=f(x_{t-1})+w_{t-1}$ ,我们的12维状态向量，𝒙，包括车辆的三维姿态、三维方向和各自的速度。旋转值用欧拉角表示
另外测量数据用 $z_{t}=h(x_{t})+v_{t}$ 表示，此处z是t时间的测量值，ℎ是将状态映射到测量空间的非线性传感器模型，且𝒗是正态分布的测量噪声。
算法的第一阶段，如等式（3）和（4）所示，是执行预测步骤，该预测步骤将当前状态估计和误差协方差及时向前投影：

$\hat{x}_{t}=f(x_{t-1})$ 公式（3），

$\hat{P}_{t}=FP_{t-1}F^{T}+Q$ 公式（4），

对于我们的应用，𝑓是从牛顿力学导出的标准三维运动模型。估计误差协方差𝑷通过𝑭（𝑓的雅可比）投影，然后受过程噪声协方差𝑸的扰动。
然后，我们在方程（5）中执行校正步骤
$K=\hat{P}_{t}H^{T}(H\hat{P}_{t}H^{T}+R)^{-1}$ 公式（5），

$x_{t}=\hat{x}_{t}+K\left (z-H\hat{x}_{t} \right )$ 公式（6），

$P_{t}=\left (I-KH \right )\hat{P}_{t}\left ( I-KH \right )^{T}+KRK^{T}$ 公式（7），
我们使用观测矩阵𝑯、测量协方差𝑹和 $\hat{P}_{t}$ 计算卡尔曼增益。我们利用增益来更新状态向量和协方差矩阵。我们使用约瑟夫形式的协方差更新方程[6]通过确保𝑷𝑘保持正半定来提高滤波器的稳定性。
由于过程噪声协方差𝑸对于给定的应用程序可能难以调谐[7]，ekf_本地化_节点将此矩阵作为参数公开给用户，从而允许额外的定制级别。
第四部分实验部分，主要为GPS坐标与odom坐标的变换矩阵T
实验内容。理想情况下，我们希望结束位置（x，y）值尽可能接近（0，0）处的原点。我们在多个ekf_节点传感器配置中重复实验：（1）通过平台的里程计进行航位推算，（2）与单个IMU融合里程计，（3）与两个IMU融合里程计，（4）与两个IMU和一个GPS融合里程计，以及（5）与两个IMU和两个GPS单元融合里程计。结果表格如下
实验结果基本上是凭直觉得出的。航位推算产生最差的性能，机器人的最终报告位置距离原点超过174米（图3）。我们先锋公司的车轮编码器存在偏差，以至于直线报告为轻微右转，导致位置估计高度不准确。
因为IMU的偏航速度误差比先锋的里程计的偏航速度误差提高了一个数量级，而且绝对方位的融合（图4）。然而，采集区域包含许多强电磁干扰区域，导致磁强计报告的航向不准确。第二IMU的加入仅稍微改善了最终位置误差，因为它受到相同的干扰，并且因为它实际上在收集的中途停止了报告数据（图5）。虽然通常是重复实验的一个原因，但这种传感器故障是多个传感器融合如此强大的一个例子，因为系统可以更优雅地处理故障或不频繁的传感器数据。
我们可以通过包含一个GPS进一步完善我们的估计（图6）。这有助于限制里程计不准确的线性速度估计的影响，并消除IMU干扰导致的不良航向估计的影响。添加第二个GPS可以减少最终位置误差的显著改善，但是展示了ekf_定位节点成功地融合来自大量传感器输入的数据的能力（图7）。
结果如图8所示。GPS定位发生的位置显示在地图上，并导致明显的瞬时位置变化。这些跳跃明显地将状态估计拉向GPS轨道，但是Kalman增益给当前状态估计赋予了一定的权重，导致新的位置处于当前状态和测量之间。尽管状态估计和测量之间存在较大的差异，但滤波器的协方差矩阵保持其稳定性，x和y方差值显著减小。
总结。虽然这项工作的重点是在近平面环境下操作的机器人，但我们也成功地在涉及地面和空中机器人的多个项目中应用了ekf_定位节点[11]。特别是，我们通过ardrone_autonomy ROS软件包[12]将该软件与Parrot AR.Drone 2.0四旋翼机集成（图10）。无人机有基于摄像机的速度传感、基于摄像机和气压计的高度传感、IMU和GPS。
后面文章进行源码分析。源码实现理解可以参考以下链接，不过是二维的https://blog.csdn.net/qq_31651031/article/details/86491765
原文的源码解析在下面地址https://blog.csdn.net/qq_29313679/article/details/105229598
上面的平面二维源码中，在测量模型里，使用GPS数据对编码器的航迹推算进行校正，均值和状态协方差矩阵都较好的符合实际运行路径，运行代码得到以下结果，蓝色（实际位置）和红色（EKF定位）相符合，黑色（航迹推算）则偏离实际值