基于路径实验数据的扩展卡尔曼滤波定位初始协方差估计与分析

《Franklin Open》：Initial Covariance Estimation and Analysis for EKF Localization Using Route-Based Experimental Data

本文针对扩展卡尔曼滤波（EKF）在移动机器人定位中初始协方差矩阵难以准确设定的问题，提出了一种基于Turtlebot3 Burger平台在三种导航路径（方形、曲线、菱形）上进行实验和仿真数据驱动的初始协方差矩阵系统化构建方法。研究结果表明，该方法推导出的初始协方差（如方形路径位置协方差为0.001022847，方向协方差为0.019544504）能显著提升EKF性能，实现高达98.47%和99.97%的不确定性降低，为移动机器人SLAM（同步定位与建图）提供了更可靠、可复现的初始化方案，具有重要的理论价值和实践指导意义。

　　

在移动机器人导航领域，精确的状态估计是实现自主移动的核心挑战。扩展卡尔曼滤波（EKF， Extended Kalman Filter）作为一种经典的非线性状态估计算法，被广泛应用于机器人同步定位与建图（SLAM， Simultaneous Localization and Mapping）中。然而，EKF的性能在很大程度上依赖于其初始状态协方差矩阵的准确设定。传统做法往往采用单位矩阵、随机值或现有数据集的数值进行初始化，这些方法缺乏系统性的理论依据和实验支持，容易导致滤波器收敛不稳定、定位不一致等问题，尤其在面对复杂多变的实际导航环境时，其局限性更为突出。因此，如何科学地、实验性地推导出可靠的初始协方差矩阵，成为提升EKF定位精度和鲁棒性的关键，这也是本研究着力解决的问题。相关研究成果已发表在《Franklin Open》期刊上。

为了系统回答初始协方差矩阵的优化问题，马来西亚彭亨大学的研究团队Muhammad Haniff Gusrial、Nur Aqilah Othman等人开展了一项深入的研究。他们以Turtlebot3 Burger移动机器人作为实验平台，精心设计了方形（Square）、曲线（Curve）和菱形（Diamond）三种具有不同旋转特性的导航路径。研究团队综合运用了硬件平台（Turtlebot3 Burger机器人及其内置传感器）配置、软件环境（ROS Kinetic， Gazebo仿真）搭建、实验与仿真数据采集、EKF算法实现以及基于大量重复导航实验的协方差计算与统计分析等关键技术方法。通过对比分析从实验数据、仿真数据和制造商提供的参数三种不同来源推导出的初始协方差矩阵在EKF中的表现，旨在找到一种可靠且通用的初始协方差 formulation 方法。

3.4. EKF implementation

研究团队利用开源的Robot Pose EKF包实现了EKF算法，该算法融合了里程计和IMU（惯性测量单元）数据来估计机器人的位姿（位置和方向）。EKF通过一阶泰勒展开对非线性系统进行线性化，并假设噪声为高斯分布，递归地估计系统状态。研究的核心是初始化阶段使用的6×6初始协方差矩阵P_k-1，其对角线元素为从不同路径导航数据中计算出的位置和方向方差，非对角线元素（交叉协方差）设为零，假设状态变量初始不相关以简化计算。

4.1. Covariance published

EKF实施后的结果表明，所有路径导航10轮后发布的协方差都呈现出类似的趋势：收敛的协方差代表机器人线性运动期间的不确定性，而异常值（outliers）则对应机器人旋转运动时产生的不确定性。这与此前研究者的发现一致。研究通过Rqt Multiplot Plugin生成了九种不同初始协方差矩阵（三种路径各对应实验、仿真和制造商三种来源）在EKF实现后的协方差结果图，直观展示了协方差的行为模式。

4.2. Covariance analysis

对发布的协方差进行定量分析发现，不同路径和不同初始协方差来源下，EKF滤波后的最终协方差（End Covariance）均比初始值有显著降低。通过计算改进百分比（Improvement percentage = (初始协方差 - 最终协方差) / 初始协方差 × 100%），发现从实验设置获得的初始协方差在EKF实现后改进效果最佳。例如，在方形路径上，实验得到的初始位置协方差（0.001022847）经过EKF处理后降至0.000015687，改进百分比高达98.47%；方向协方差从0.019544504降至0.000005417，改进百分比达99.97%。分析还表明，菱形路径由于包含60°和120°等多种旋转角度，其产生的协方差异常值峰值（最高达3000 μm2）通常高于方形路径（600 μm2）和曲线路径（2000 μm2），表明其导航过程中的不确定性更高。相比之下，方形路径由于运动模式规则（90°直角转弯），其协方差行为最稳定。

4.3. Rotation analysis

研究人员进一步分析了旋转运动特性（旋转角度、旋转时间）对协方差峰值差异的影响。计算表明，曲线路径的旋转（非定点，沿弧线运动）总时间最长（66.66秒/圈），方形路径（62.84秒/圈）和菱形路径（62.82秒/圈）的旋转时间接近但短于曲线路径。更长的旋转时间往往导致更大的协方差异常值，表明在旋转运动期间存在更大的不确定性，这与曲线和菱形路径观测到的更高协方差峰值相符。

4.4. End covariance analysis

文章列出了所有九种情况下的最终6×6协方差矩阵。由于本研究专注于2D导航，只关注x, y位置和yaw（偏航角）方向上的协方差值（对应于矩阵中的P₀₀, P₁₁, P₅₅），而忽略z轴、roll（横滚角）和pitch（俯仰角）方向的值。最终协方差矩阵显示，初始不确定性在经过EKF递归估计后得到了有效抑制。

4.5. Findings summary

研究总结指出，从实验设置获得的初始协方差矩阵在EKF应用中表现最优。其中，方形路径产生的初始协方差最为可靠，其不确定性低（收敛峰和异常值峰为450 μm2），旋转运动涉及时间较短，EKF改进率高（位置和方向分别提升98.47%和99.97%），且在整个导航过程中协方差行为稳定。曲线路径的协方差行为相对不稳定，而菱形路径则略显稳定。

4.6. Comparative Evaluation with Existing Studies

通过与现有研究的比较，本文指出其提出的基于实验数据推导初始协方差的方法，不同于需要递归调整协方差（增加计算负荷）或需要先验系统误差知识的现有方法。本方法通过实验数据直接确定协方差，无需改变算法结构或迭代调整，在保持实时计算效率的同时，实现了较低的不确定性（约450 μm2）和高达98%-99%的收敛精度提升。

本研究通过系统的实验设计和数据分析，成功地提出并验证了一种基于路径实验数据推导EKF初始协方差矩阵的有效方法。其主要结论在于：基于实际导航数据（特别是方形路径实验数据）经验性推导的初始协方差矩阵，能够为EKF提供更准确、更可靠的初始化，显著降低状态估计的不确定性，改善滤波器的收敛行为和整体定位性能。这项研究的重要意义在于填补了EKF初始协方差矩阵系统性实验化 formulation 的方法学空白，为移动机器人定位，特别是SLAM应用，提供了一个更具鲁棒性和场景适应性的初始化基础。所确定的具体协方差值（如方形路径实验值：位置方差0.001022847，方向方差0.019544504）可作为Turtlebot3 Burger等类似移动机器人平台上实施EKF或SLAM时的实用参考参数，增强了算法性能的可预测性和可重复性。未来工作可将此方法推广至其他机器人平台和更动态的环境中，进一步验证其普适性。