本文是发表于IEEE Sensors Journal（卷21，期20，2021年10月15日）的一项原创性研究论文，题目为《Distributed Cooperative Localization based on Bearing-Only Sensors》。主要作者为杨其帆、杨学荣、杨亚军和杨阳（IEEE会员）。杨其帆、杨学荣和杨阳来自中山大学航空航天学院（深圳，518106），杨亚军来自航天工程大学（北京，101416）。

一、 研究的学术背景

本研究的核心科学领域属于机器人学与自主系统，特别是针对多移动机器人（智能体）在全球定位系统（GPS）拒止环境下的协同定位（Cooperative Localization, CL）问题。协同定位技术允许多个智能体通过相互之间的相对测量和稀疏的绝对位置信息（如已知地标），来克服惯性测量单元（IMU）误差累积的问题，从而实现稳定的自定位。

在GPS拒止环境（如室内、城市峡谷、水下或行星表面）中，为无人机集群等应用提供可靠的位置感知是至关重要的。光学相机因其低成本、轻量化和低功耗的特性，被广泛用作机载传感器。然而，光学相机本质上是纯方位角（Bearing-Only）传感器，即它们只能提供目标相对于自身的方位角度信息，而无法直接获得距离或完整的相对姿态。这给协同定位带来了特殊的挑战：如何基于仅有的角度观测，设计一个完全分布式、可处理高非线性、并保证估计一致性的算法？

传统的集中式协同定位算法需要一个中心节点处理所有信息，存在通信和处理成本高、可扩展性和鲁棒性差的问题。分布式算法则要求每个智能体独立处理自身状态估计，但需处理智能体间状态估计的强相关性，以避免一致性（Consistency）问题（即估计误差的协方差小于或等于真实误差协方差）。现有的一些分布式方法，要么需要大量内存记录所有相关性并进行全连通通信，要么依赖于相对位姿（包含位置和朝向）测量，要么无法保证在切换拓扑（即通信和观测关系动态变化）下的性能。

因此，本研究旨在解决一个开放性问题：设计一种完全分布式的、仅基于方位角测量的、适用于切换拓扑且可证明一致的协同定位算法。本研究的目标是提出一种算法，使每个智能体在仅装备IMU和光学相机的情况下，通过相互之间的方位角观测和有限通信，实现高精度的自定位。

二、 研究的详细工作流程

本研究主要提出了一种新的分布式协同定位算法，其核心在于将协方差交集（Covariance Intersection, CI）方法泛化至纯方位角测量模型，并采用高斯混合模型（Gaussian Mixture Model, GMM）来处理测量的高度非线性。整个工作流程可分为算法框架设计、被动更新过程、主动更新过程、协方差矩阵调整以及数值仿真验证五个主要环节。

1. 算法核心框架与假设条件：算法基于一种分步更新（Stepwise Update） 机制。在每个更新周期，一个智能体可以同时扮演“观察者（Observer）”和“被观察者（Observed）”两种角色。当智能体i观测到智能体j时（记作(i→j)_k），触发两个更新过程：(a) 被观察者j利用来自观察者i的信息更新其位置估计（被动更新）；(b) 观察者i利用对被观察者j的观测更新其自身朝向估计（主动更新）。该机制由方位角测量模型的特性决定，因为测量本身不包含被观测者朝向的信息。

   算法有效运行依赖于几个关键假设，以确保绝对位置信息能够在群体中传播：(1) 至少存在一个智能体（锚点）能通过地标或GPS获得绝对位置信息；(2) 每个智能体的通信范围大于其测量范围；(3) 群体的相对测量图是连通的；(4) 每个智能体在一段时间内都会扮演两种角色以更新其全部状态；(5) 智能体间的相对运动不能是退化运动（Degenerate Motions），即那些不会引起相对方位角变化的运动（如相向或背向直线运动），否则会导致观测模糊和估计误差漂移。

2. 被动更新过程（更新被观察者位置）：

   • 研究对象与数据处理：这个过程涉及两个智能体：观察者i（拥有自身的局部状态估计）和被观察者j。其目标是将两个关于j位置的不同信息源进行融合：一个是由j自身IMU传播得到的局部位置估计（高斯分布），另一个是由观察者i基于其自身（不确定的）位置估计和对j的方位角测量所生成的j的位置分布。
   • 核心方法与实验步骤：由于方位角测量与观察者自身状态的非线性耦合，无法直接获得j位置的解析概率密度函数（PDF）。因此，研究者提出了采样方法2（Sampling Method 2）。该方法首先对观察者i的局部状态（位置和朝向）进行采样，同时对方位角测量噪声和未知的观测距离（假设在一个测量范围[mr_min， mr_max]内均匀分布）进行采样。然后，根据相机针孔模型和测量方程（公式2），生成一系列描述被观察者j可能位置的粒子。这些粒子集合本质上表征了在考虑观测者自身不确定性和测量噪声后，j位置的非高斯后验分布。
   • 非线性处理与数据融合：为了后续与高斯分布进行融合，使用期望最大化（Expectation-Maximization, EM）算法将上述粒子集合拟合为一个高斯混合模型（GMM）。观察者i将此GMM参数发送给被观察者j。被观察者j随后使用算法1：Chernoff融合（Algorithm 1: Chernoff Fusion），将这个GMM与其自身维护的（高斯分布）位置估计进行融合。Chernoff融合是广义协方差交集（Generalized CI）的一种实现，用于融合两个非高斯的PDF，并寻找一个最优的融合权重ω以最小化融合后的不确定性（通过最小化Chernoff信息）。该算法涉及构造融合后的GMM分量、利用Sigma点求解权重系数、并通过优化寻找最佳ω，最终输出融合后的位置估计均值和协方差。

3. 主动更新过程（更新观察者朝向）：

   • 研究对象与数据处理：这个过程由观察者i独立完成。其目标是将两个关于i自身朝向的不同信息源进行融合：一个是i自身IMU传播得到的局部朝向估计（高斯分布），另一个是基于其对被观察者j的位置估计（从通信获得）和方位角测量反推得到的自身朝向分布。
   • 核心方法与实验步骤：根据测量模型（公式2），可以反推出观察者的朝向估计（公式11）。但这同样是关于多个随机变量（i和j的位置、测量噪声）的非线性函数。因此，研究者提出了采样方法3（Sampling Method 3）。该方法分别对i和j的局部位置估计（高斯分布）以及方位角测量噪声进行采样，然后根据公式11计算生成一系列描述观察者i可能朝向的粒子。
   • 数据融合：统计这些粒子，发现其朝向分布近似为高斯分布。因此，可以通过计算样本均值和协方差，或再次使用EM算法拟合一个单分量的GMM，来获得一个高斯形式的传播朝向估计及其协方差。随后，观察者i使用算法2：协方差交集融合（Algorithm 2: Covariance Intersection Fusion），将这个传播得到的朝向估计与其自身的局部朝向估计进行融合。CI融合是Chernoff融合在高斯分布下的特例，通过优化权重ω以最小化融合后协方差的迹，来保守地处理两个估计之间的未知相关性。

4. 协方差矩阵调整：

   • 问题与解决方法：通过被动和主动更新，智能体的位置(x，y)和朝向(ψ)被分别更新。然而，状态向量的协方差矩阵中，位置与朝向之间的交叉项（Cross Terms） 在更新过程中未被触及。这些交叉项是在IMU运动传播过程中产生的，反映了位置和朝向在运动学上的耦合。忽略它们可能导致协方差矩阵失去半正定性，破坏估计的一致性。
   • 新颖的调整算法：研究没有采用传统的奇异值分解（SVD）寻找最近半正定矩阵的方法（因为这会扰动所有矩阵元素，可能破坏已保证的一致性），而是提出了一种仅调整交叉项的方法。基本思想是假设被动和主动更新过程不改变位置与朝向间的相关系数。当更新后的协方差矩阵因交叉项不匹配而可能变为负定时，通过引入一个缩放系数γ，仅对交叉项进行缩放（公式17），并求解一个二次方程（公式18）找到使矩阵行列式为零（即处于半正定边界）的γ值（公式19），从而得到一个修正后的半正定协方差矩阵，同时保持了已更新部分（对角线块）的一致性。

5. 整体算法与仿真验证：

   • 算法集成：上述步骤被整合为算法3：纯方位角分布式协同定位（Algorithm 3）。每个智能体在每个采样周期执行：基于IMU的状态传播；若作为观察者检测到测量，则执行采样、GMM拟合，并发送GMM给被观察者；若作为观察者需发起主动更新，则请求被观察者状态、采样、GMM拟合、执行CI融合更新朝向、调整协方差；若作为被观察者收到GMM，则执行Chernoff融合更新位置、调整协方差。
   • 仿真设置与工作流程：研究通过数值仿真验证算法性能。仿真对象为一个由4个机器人组成的团队，其中1个为已知精确位置的锚点机器人，其余3个无外部定位信息。所有机器人进行不同周期的圆周运动以避免退化运动。仿真设置了动态切换的测量拓扑（如图6所示），并在600秒内分为三个阶段，以测试算法在拓扑变化、成员暂时失联后重连等情况下的表现。仿真中，真实运动轨迹每0.1秒更新，自运动（IMU）测量每1秒一次，方位角测量每5秒一次。关键参数包括初始位姿不确定性、测量噪声标准差、采样点数（N1=5000）、GMM分量数（N2=10）等。

三、 研究的主要结果

1. 切换拓扑下的算法一致性：仿真结果表明（如图7所示），在拓扑结构动态切换（分别在200秒和400秒改变）的情况下，所提出的算法能够保持估计的一致性（Consistency）。所有智能体（锚点除外）的位置和朝向估计误差均收敛，并始终保持在3σ边界内。这验证了算法在动态、非全连通网络中的有效性。结果同时显示了分布式算法的典型特征：离信息源（锚点）在测量拓扑中越远的智能体，其估计精度越低。这是因为信息是通过相邻节点的测量逐步传递的，而非全局共享。在200-400秒期间，智能体1与团队失去联系，其估计因IMU误差累积而开始发散；而智能体2和3尽管拓扑改变，仍能保持稳定一致估计。当智能体1在400秒重新加入后，其估计能够迅速被纠正。此外，结果显示朝向估计比位置估计具有更高的稳定性和更快的收敛速度，研究者认为这源于位置估计中使用了近似算法（GMM拟合和Chernoff融合优化）带来的额外保守性。

2. 参数影响的数值分析：研究通过蒙特卡洛仿真，在固定拓扑下分析了关键参数（GMM分量数和采样数）对算法精度和计算成本的影响。

   • GMM分量数的影响（表 I）：增加GMM分量数会显著增加被动更新过程的计算耗时，但对位置估计精度（用均方根误差RMS衡量）的提升存在饱和点。对于靠近锚点的智能体，大约4个分量即可使精度趋于稳定；而对于远离锚点的智能体（如Agent 4），需要更多分量才能达到稳定，因为其观测模型分布更分散（如图3所示）。超过一定数量后，增加分量只会增加计算负担，对精度提升有限。
   • 采样数的影响（表 II）：增加采样点数能同时提升被动更新（位置）和主动更新（朝向）的估计精度，且带来的计算时间增加相对温和。值得注意的是，主动更新过程的计算成本远低于被动更新（不到总过程的1%），因为被动更新中的Chernoff融合优化（公式8）远比主动更新中的CI融合优化（公式13）复杂。同样，远离锚点的智能体需要更多的采样点才能达到与近锚点智能体相当的精度提升幅度。研究强调，过长的拓扑链路（即信息传递路径过长）会导致算法性能下降。

四、 研究的结论与意义

本研究成功提出并验证了一种首个完全分布式的、仅基于方位角测量的协同定位算法。该算法的主要贡献在于： * 科学价值：将协方差交集（CI）方法成功推广至高度非线性的纯方位角测量场景，通过结合高斯混合模型（GMM）和分步更新机制，解决了分布式协同定位中因未知相关性导致的一致性问题。为处理非线性、非高斯估计问题提供了一种基于采样的实用化分布式融合框架。 * 应用价值：使得仅配备低成本IMU和光学相机的机器人集群，在无GPS环境中实现可靠的自定位成为可能。算法不要求全连通通信，能够适应动态变化的网络拓扑（切换拓扑），增强了系统在现实复杂环境中的鲁棒性和可扩展性。这对于大规模无人机编队、水下机器人集群、地下或室内搜索救援等应用具有重要意义。

五、 研究的亮点

1. 方法首创性：据作者所知，这是首次实现完全分布式的纯方位角协同定位方法，填补了该领域的技术空白。
2. 理论保证：算法基于广义CI，在理论上被证明是一致的（Provably Consistent），确保了估计结果的可靠性。
3. 处理高度非线性：创新性地采用GMM来近似由方位角测量产生的复杂非高斯后验分布，并结合采样方法有效地处理了观测者自身状态不确定性与测量噪声的耦合问题。
4. 灵活的更新机制与拓扑适应性：提出的“被动更新位置”和“主动更新朝向”的分步机制，允许智能体在不同时间步、通过与不同邻居的交互来分别更新状态的不同部分，从而自然支持切换拓扑，降低了同步和全连通的要求。
5. 实用的协方差调整方法：提出了一种仅调整协方差矩阵交叉项以保持其半正定性的新方法，在维持算法一致性的同时，计算量远低于传统的SVD方法。

六、 其他有价值的内容

研究在讨论部分指出了算法的局限性及未来方向。与所有基于CI的方法一样，该算法给出的估计是保守的（Conservative），即估计的不确定性可能比实际情况更大。随着拓扑链路的增长，误差会累积放大。此外，随着传感器测量范围增大或误差增大，为了保证实时性，需要更强的计算能力。未来的工作将包括：引入新的融合规则以减少保守性；将方法应用于更复杂的动力学模型和非高斯传感器噪声；所有这些工作都将基于GMM的良好性质和本文提出的基于采方法的便利性而展开。论文附录中提供的详细算法步骤和采样方法，为其他研究者复现和应用该方法提供了清晰的指引。