A Generalized Extended Kalman Filter Implementation for the Robot Operating System 翻译

Author：	三木之杨
Date：	2021.09.02
Keywords:	融合定位、IMU、GPS、Odom、EKF、UKF
Description：	这是robot localization包的论文，旨在通过论文了解该包原理，后续会有具体的源码分析及包的具体使用。

目录

摘要

I. INTRODUCTION

II. MOTIVATION

III. EXTENDED KALMAN FILTER NODE

A. Extended Kalman Filter Algorithm

IV. EXPERIMENTS

A. Loop Closure Accuracy

B. Infrequent GPS

V. DISCUSSION

VI. CONCLUSION AND FUTURE WORK

REFERENCES

---

 原文地址：Paper Title (use style: paper title) (ros.org)

摘要

        移动机器人精确的状态估计往往需要多个传感器的数据融合。因此，执行传感器融合的软件应该支持广泛的异构传感器阵列。本文提出了一种用于机器人操作系统(ROS)的软件包robot_localization。该包目前包含一个扩展卡尔曼滤波器(EKF)的实现。它可以支持来自多种传感器类型的无限数量的输入，并允许用户自定义哪些传感器数据字段与当前状态估计融合。在这项工作中，我们激励我们的设计决策，讨论实现细节，并提供来自真实世界测试的结果。

Keywords—sensor fusion; extended Kalman filter; localization; Robot Operating System

---

I. INTRODUCTION

        对所有移动机器人来说，一个关键的挑战是能否回答我在哪里? 这个问题的答案可以通过机器人的传感器得到，它可以提供本体感受和外部感受数据。然而，传感器是不完善的，它们的测量容易产生误差。通过融合多个传感器的数据，我们可以得到一个整体的位置估计，其误差可以小于单独使用一个传感器。通常情况下，更多的传感器输入数据将产生更精确的位置估计。因此，在移动机器人平台上执行传感器融合的任何软件都能够接收平台上的所有可用数据，这一点至关重要。此外，软件应该易于使用和高度可定制，从而为用户提供更大的灵活性，同时允许他们关注更高级别的行为。

        在本文中，我们介绍了我们的软件包robot_localization，为机器人操作系统(ROS)[2]。ROS是一个开源的机器人框架，已经被世界各地的学术界、工业界和军方广泛采用。我们的软件解决了广泛移动机器人的传感器融合需求，并允许与这些平台的快速集成。

        在第二节中，我们详细介绍了创建robot_localization包的动机。在第三节中，我们描述了扩展卡尔曼滤波(EKF) [3] ROS节点，ekf_localization_node。第四节详细介绍了我们在先锋3移动机器人上进行的实验。第五节提供了对实验的总结讨论，第六节最后介绍了用于其他平台的应用程序，并详细介绍了扩展ekf_localization_node和robot_localization包的计划。

---

II. MOTIVATION

        ROS社区开发并贡献了大量的软件，以促进机器人的开发和实践，目前已有2000多个软件包可用。虽然存在其他执行状态估计的包，但由于各种原因，它们通常难以应用于新问题:

• 有限的传感器输入：机器人装备了越来越多的传感器，现有的ROS包需要用户付出大量努力才能成功集成所有传感器的数据。

• 仅限于2D估算：对于一些在平面室内环境中运行的无人地面车辆(ugv)来说，ROS包可以在二维环境中估计车辆的状态，足以满足预期的应用。然而，这些软件包不足以估计3D操作平台的状态，如无人机(uav)、水下无人驾驶车辆(uuv)和户外操作的ugv。

• 有限的ROS消息支持：ROS中的传感器数据通常来自用户无法控制的硬件驱动程序包。如果状态估计节点不支持给定的消息类型，用户必须要么修改驱动程序的源，要么创建一个中间节点，以将消息数据复制到受支持的消息类型中。

• 对传感器数据的控制有限：准确的状态估计通常只需要可用传感器消息的一个子集，因为有故障的传感器或传感器驱动程序无法正确填写协方差值。这需要用户修改数据消息，例如，通过人为地夸大协方差。

        我们从头开始开发了robot_localization包，以克服这些限制，使其尽可能通用。它在3D空间中执行状态估计，允许无限数量的传感器，支持多种标准ROS消息类型，并允许每个传感器控制哪些消息字段与状态估计融合。

---

III. EXTENDED KALMAN FILTER NODE

        我们开发了ekf_localization_node，一个EKF实现，作为robot_localization的第一个组件，robot_localization包最终将包含多个可执行文件(在ROS术语中称为节点)来执行状态估计。这些节点将共享第二节中描述的理想属性，但在状态估计的数学方法上有所不同。在本节中，我们将描述ekf_localization_node的实现细节。

A. Extended Kalman Filter Algorithm

        EKF公式和算法是众所周知的[3,4,5]。我们在这里详述它们是为了传达重要的实施细节。我们的目标是估计移动机器人在一段时间内的完整3D (6DOF)姿态和速度。该过程可以描述为一个非线性动态系统，具有：

        其中x_k为机器人在k时刻的系统状态(即三维姿态)，f为非线性状态转移函数，w_{k−1}为过程噪声，假设其为正态分布。我们的12维状态向量，x​​​，包括机器人的三维位置，三维方向和它们各自的速度。转动值用欧拉角表示。此外，我们还接收测量值：

        其中z_k为k时刻的测量，ℎ为将状态映射到测量空间的非线性传感器模型，v_k为正态分布的测量噪声。算法的第一步，如式(3)和(4)所示，是进行一个预测步骤，将当前状态估计和误差协方差及时向前投影:

        对于我们的应用，f是一个标准的由牛顿力学推导的三维运动学模型。估计误差协方差P，通过f的雅可比矩阵F进行投影得到，然后被过程噪声协方差Q扰动。

然后对式(5)到式(7)进行修正:

        我们使用观测矩阵H、测量协方差R和hatP_k​计算卡尔曼增益。我们利用增益来更新状态向量和协方差矩阵。我们使用Joseph形式的协方差更新方程[6]来保证P_k保持正半定，从而提高滤波器的稳定性。

        标准EKF公式规定H为观测模型函数ℎ的雅可比矩阵。为了支持广泛的传感器阵列，我们假设每个传感器都产生我们所估计的状态变量的测量值。因此，H就是单位矩阵。ekf_localization_node的一个核心特性是，它允许部分更新状态向量，这也是添加到robot_localization中的任何未来状态估计节点的要求。这对于获取不测量状态向量中的每个变量的传感器数据至关重要，而状态向量几乎总是如此。具体来说，当只测量m个变量时，H变成一个m × 12的秩为m的矩阵，其非零值(在本例中为1)仅存在于被测量变量的列中。

        由于对于给定的应用程序[7]，过程噪声协方差Q很难进行调优，因此ekf_localization_node将此矩阵作为参数公开给用户，从而允许进行额外级别的定制。

---

IV. EXPERIMENTS

        我们设计并执行了两个实验来评估ekf_localization_node的性能。我们的测试平台是MobileRobots Pioneer 3(图1)。该机器人配备了车轮编码器，可以提供原始里程估算。此外，我们还在平台上安装了带有两个Microstrain 3DM-GX2 imu和两个Garmin GPS 18x单元的传感器套件。传感器安装在定制的钻机上，有助于减少imu的磁干扰，并提高GPS设备的信号质量。我们已经配置了ekf_localization_node来接收来自每个imu的滚转、俯仰、偏航和它们各自的速度，以及来自车轮编码器的x和偏航速度。对于每个GPS，我们定义一个转换，将机器人的世界坐标系坐标(即机器人起始位置的原点所在的坐标系)转换为GPS的UTM坐标，如下所示：

        其中Φ、θ和ψ分别为车辆的初始utm坐标下roll, pitch, yaw。c、s分别为余弦函数和正弦函数，x_{UTM0}、y_{UTM0}、z_{UTM0}为首次上报GPS位置的UTM坐标。在任何随后的时间t，我们将GPS测量转换为机器人的世界坐标系——odom，通过:

        然后配置ekf_localization_node，将转换后的位置与状态估计融合在一起。这个过程对每个GPS都是独立进行的。

        我们从作者所在大楼停车场的平台上收集了原始传感器数据。其路径如图2所示。实验环境从机器人的起点到最远的距离约为110米。机器人是由操纵杆控制和驱动的，因此它的最终位置正好是它开始的地方。这次收集持续了大约777秒。然后通过ROS的rosbag实用程序回放数据，允许我们在同一个收集的数据上进行多个实验。虽然状态是在3D空间中估计的(即考虑到滚动和俯仰，并结合GPS高度测量)，但我们报告的结果是2D的，因为近平面环境使得报告3D信息变得多余(第VI节提供了一个ekf_localization_node应用于无人机3D状态估计问题的示例)。

A. Loop Closure Accuracy

        对于我们的第一个实验，我们感兴趣的是机器人的开始和结束位置之间的距离，如ekf_localization_node所报告的。理想情况下，我们希望结束位置(x, y)值尽可能接近原点(0,0)。我们在多个ekf_localization_node传感器配置中重复实验:

• (1)通过平台的里程计进行航位推算，
• (2)使用单个IMU的融合里程计，
• (3)使用两个IMU的融合里程计;
• (4)融合了两个imu和一个GPS的里程计，
• (5)融合了两个imu和两个GPS单元的里程计。

        如前所述，ekf_localization_node提供了配置其每个传感器中的哪些变量在最终状态估计中实际上被融合的能力。我们在Table I.中列出了传感器的配置。注意，对于这些实验，我们保持每个配置不变。然而，在现实中，一些配置可能会根据传感器包而改变。进一步讨论见第五节。

        结果列于表二。对于每个传感器配置，都报告x和y位置的环路闭合误差。我们的实验设计是这样的:这个错误只是ekf_localization_node在包文件回放停止前最后报告的状态估计。我们还报告了滤波器对x和y的最后估计标准差值。这表明了EKF的误差与现实有多接近。统计是在机器人的世界坐标系中报告的(按照ROS标准，我们表示这个坐标系为Odom)。机器人的起始方向和原点如图2所示。

        我们还对图2到图7中的结果进行了图形描述。在图2中，我们将GPS轨迹的平均值作为机器人的路径真值。这只是作为一个视觉指导，并不被认为是事实。在图3到图7中，我们将估计的路径叠加在可视化的顶部，以供参考。它还展示了表II(对应于图6和图7)的第四行和第五行配置是如何改善平均GPS轨迹的。

        实验的结果在很大程度上是凭直觉得出的。航位推算的结果是最差的，机器人最终报告的位置距离原点超过174米(图3)。我们的实验车车轮编码器存在偏差，直线被报告为轻微右转，导致高度不准确的位置估计。引入一个IMU有助于纠正这个问题，因为IMU的偏航速度误差比先驱者的里程计提高了一个数量级，同时融合了绝对方向(图4)。然而，采集区域包含许多强电磁干扰区域，导致磁力计报告的标题不准确。添加第二个IMU只会稍微改善最终的位置误差，因为它受到同样的干扰，而且实际上它在收集到一半的时候就停止报告数据了(图5)。该传感器故障是多个传感器融合如此强大的一个例子，因为系统可以更优雅地处理故障或不频繁的传感器数据。

        我们可以通过加入单个GPS进一步完善我们的估计(图6)。这有助于约束里程计不准确的线速度估计的影响，并消除IMU干扰导致的糟糕航向估计的影响。添加第二个GPS对最终位置误差的改善没有那么大，但显示了ekf_localization_node从大量传感器输入成功融合数据的能力(图7)。

B. Infrequent GPS

        许多机器人接收到不频繁的绝对位置测量，当这些信号缺失时必须保持状态估计。对于我们的第二个实验，我们想要评估当GPS信号不经常到达时滤波器的性能。我们使用与表II第4行相同的传感器配置进行实验，即使用里程计、imu和一个GPS。但是，我们过滤收集日志文件，使GPS数据每120秒才可用一次。我们的目标是确定滤波器如何优雅地处理与当前状态估计相差很大的传感器数据的融合。

        结果如图8所示。GPS定位发生的位置显示在地图上，并导致明显的瞬时位置变化。这些跳跃明显地将状态估计拉向GPS轨道，但卡尔曼增益给当前状态估计一些权重，导致新位置处于当前状态和测量之间。尽管状态估计和测量之间有很大的差异，但滤波器的协方差矩阵保持了它的稳定性，并且x和y方差值显著降低。行驶结束时，车辆闭环(x, y)绝对误差为(12.06,0.52)米。

---

V. DISCUSSION

        再次参考表II的第一行(航位推算)，很明显x和y的估计方差非常大。对于这个特殊的测试，协方差矩阵的条件数增长很快，说明滤波器不稳定。这部分原因在于传感器的配置。偏航与x和y位置之间的强相关性意味着，如果不对偏航或(x, y)进行绝对测量，这些值上的误差将迅速增长。显然，这个问题通过imu解决了，imu提供了绝对偏航测量。

        表II中第2行和第3行(x, y)位置的估计标准差远小于真实位置估计误差。这部分是由于试验车里程计和IMU数据的噪声比其协方差要大引起的。我们也没有调整过程噪声协方差矩阵Q[7]。

        值得注意的是，尽管里程计只能真正测量x和偏航速度，但我们可以推断出更多的信息。由于平台的约束，平台不会获得任何瞬时的z或y速度，也就是说，它不能飞行并且是非完整的。因此，只要适当地设置了测量的协方差，我们就可以将这些数据字段中的零值与我们的估计融合在一起(表I)。一般来说，如果一个量的测量是通过运动学约束隐含的，那么最好将该量视为一个测量值。

        虽然我们的实验只使用了本体感受传感器，但我们的软件设计是这样的，即来自外部感受传感器(如激光扫描仪或摄像机)的输入也可以使用，只要它们产生支持的ROS信息类型。例如，迭代最近点(ICP)算法[8]可以与来自RGBD传感器(如微软Kinect[9])的数据一起使用，以生成额外的里程测量[10]来源。

        虽然此实验中可能的传感器排列数量并不多，但ekf_localization_node(以及robot_localization中的未来节点)的传感器数据定制参数可以产生更大的可能配置集。例如，我们的一个imu已知有一个故障陀螺仪。在这种情况下，我们可以使用那个IMU只报告方向，使用第二个IMU给我们方向和角速度。这种细粒度的控制对于处理已知的故障传感器和故障排除是有用的。

---

VI. CONCLUSION AND FUTURE WORK

        虽然这项工作专注于在近平面环境中操作的机器人，但我们也成功地将ekf_localization_node应用于多个项目，包括地面和空中机器人[11]。特别是，我们已经通过ardrone_autonomy ROS包[12]将该软件与Parrot AR.Drone 2.0四轴飞行器集成(图10)。无人机有基于相机的速度传感、基于相机和气压的高度传感、IMU和GPS。

        我们计划通过多种方式改进ekf_localization_node和robot_localization:

• 协方差覆盖。某些特定机器人平台或传感器的ROS节点为某些协方差矩阵项指定任意值，以表示所讨论的数量未被测量或不可靠。然而，正如第五节所讨论的，用户可能希望将传感器数据合并到传感器实际测量不到的值中。目前，robot_localization中所有节点都使用的传感器数据预处理逻辑，给任何融合方差为零的传感器赋一个小的方差值。虽然这允许测量被融合而不打破过滤器，值应该是可参数化的。
• 支持线性加速度。我们目前没有融合线性加速度在我们的状态估计或解释它在我们的运动学模型。这样做将进一步提高过滤精度。
• 附加的状态估计节点。robot_localization包包含多个节点，用于执行状态估计。我们计划在未来增加新的节点，如无迹卡尔曼滤波[13]节点和粒子滤波[14]节点。

        在本文中，我们为robot_localization ROS包引入了一个广义扩展卡尔曼滤波节点ekf_localization_node。它支持多传感器和高定制性，非常适合各种机器人平台的状态估计问题。作者希望robot_localization能够从ROS社区的反馈和贡献中受益。

---

REFERENCES

[1] J.J.  Leonard  and  H.F.  Durrant-Whyte,  "Mobile  robot  localization  by 
tracking  geometric  beacons," Robotics  and  Automation,  IEEE lon_deg
Transactions on vol. 7, no. 3, pp. 376-382, 1991. 
[2] M.  Quigley,  K.  Conley,  B.  Gerkey,  J.  Faust,  T.  Foote,  J.  Leibs,  R. 
Wheeler and A.Y. Ng, "ROS: an open-source robot operating system," 
ICRA workshop on open source software vol. 3, no. 3.2,  2009. 
[3] G.L. Smith, S.F. Schmidt and L.A. McGee, "Application of statistical 
filter theory to the optimal estimation of position and velocity on board a 
circumlunar vehicle,"  1962. 
[4] R.E.  Kalman,  "A  New  Approach  to  Linear  Filtering  and  Prediction 
Problems," Transactions of the ASME pp. 35-45, 1960. 
[5] G. Welch and G. Bishop, "An introduction to the Kalman filter," 1995. 
[6] G.J.  Bierman  and  C.L.  Thornton,  "Numerical  comparison  of  Kalman 
filter algorithms: Orbit determination case study," Automatica vol. 13, 
no. 1, pp. 23-35, 1977. 
[7] B.J. Odelson, M.R. Rajamani and J.B. Rawlings, "A new autocovariance 
least-squares method for estimating noise covariances," Automatica vol. 
42, no. 2, pp. 303-308, 2006. 
[8] P. Besl and N. McKay, "Method for registration of 3-D shapes," IEEE 
PAMI, 14,  pp. 239-256, 1992. 
[9] Microsoft  Corporation,  “Kinect  for  Windows,”. 
http://www.microsoft.com/en-us/kinectforwindows/,  2014 
[10] A. Milella, and R. Siegwart, "Stereo-based ego-motion estimation using 
pixel  tracking  and  iterative  closest  point," Computer  Vision  Systems, 
2006 ICVS'06. IEEE International Conference on., 2006. 
[11] D.  Stouch,  A.  Ost,  T.  Moore  and  C.  Monnier,  "Robust  Tactical 
Communications  Relay  using  Visual  Object  Detection  on  an 
Autonomous  Mobile  Robot," International  Advanced  Robotics 
Programme's  7th  International  Workshop  on  Robotics  for  Risky 
Environments - Extreme Robotics (IARP RISE-ER 2013) 2013. 
[12] M.  Monajjemi,  "Autonomylab/ardrone  autonomy.  github.  com," 
AutonomyLab/ardrone autonomy  2013. 
[13] S.J. Julier and J. Uhlmann, "A New Extension of the Kalman Filter to 
Nonlinear Systems," AeroSense, Simulation and Controls, Multi Sensor 
Fusion, Tracking and Resource Management II  1997. 
[14] S. Thrun, D. Fox, W. Burgard and F. Dellaert, "Robust Monte Carlo 
Localization for Mobile Robots," Artificial Intelligence vol. 128, no. 1-2, 
pp. 99-141, 2000.