做无人机集群项目的人大概率都经历过这个尴尬单机定位明明很准一编队飞就露馅了。GPS信号在楼间闪断、两架飞机互相遮挡、飞控输出的位置和邻居的真实位置对不上……这些问题的本质其实只有一个——每架无人机都在独自估算“我在哪”却没有人利用“我们之间的相对关系”。分布式协同定位Distributed Cooperative LocalizationDCL解决的就是这个问题让集群里每一架无人机不仅用自己的传感器还把邻居的位置观测当成自己的信息源通过局部通信把整个编队的定位精度整体抬上去。这也是我在做编队避障、物流配送和协同建图时最终把方案从单机EKF/GNSS组合导航切换到分布式因子图框架的原因。这篇文章把我实践过的分布式协同定位在无人机集群中的落地思路完整拆一遍包含原理推导、matlab仿真代码的模块拆解、调参经验和真机移植时踩过的坑需要做集群定位或者毕设选型的朋友可以参考。1. 单机定位撑不起集群场景分布式协同定位到底解决了什么1.1 集群场景下单机定位的三个硬伤先说最直观的问题绝对定位精度。消费级GNSS单点定位的水平精度通常在三到五米即使在开阔地也很难稳定进一米以内。RTK可以做到厘米级但基站覆盖范围有限在楼群、树冠、桥梁附近容易失锁。视觉里程计和激光里程计在没有明显特征的环境里会漂移尤其无人机长时间匀速直线飞行时位置误差会慢慢累积回看轨迹往往是一条越来越歪的弧线。第二个问题藏在编队里相对误差。两架无人机各自跑单机定位谁都以为自己飞得很直但它们的绝对误差方向可能完全相反。表现在编队上就是队形“扭动”——名义队形是正方形实际拍出来是菱形。你反复调飞控的编队参数也没用因为问题不在控制层而在导航层。第三个问题最容易被忽略单机方案完全没有利用“邻居关系”。编队内无人机之间的距离通常只有几十米这段距离的测量手段非常多UWB测距精度可以做到十厘米级视觉相对位姿在近距离也能达到厘米级。这些相对观测在定位里面就是现成的强约束单机定位时它们全部被浪费掉了。1.2 “几何约束”如何抬升整个编队的精度我经常用一个比喻解释协同定位的收益一群人摸黑站在一个大操场上每个人都拿着自己的计步器走了一会儿大家的位置误差都很大。但只要他们彼此拉着手知道自己和旁边人的相对距离整个队伍的形状就会慢慢被“锁”住。每一条相对测距就是一条看不见的拉手绳子。数学上的逻辑其实并不复杂。单机定位时绝对观测误差服从某一方差位置估计的不确定度是一个误差椭圆加入一条相对测距约束之后这个椭圆会在测距方向上被压缩。编队里相对观测越多、几何关系越丰富整个集群的位置不确定度就越小。尤其对垂直方向误差改善明显——GNSS的高程精度本来就差而UWB测距大多对三维距离敏感等于给高度方向也加了一根绳子。那为什么一定要“分布式”不是有集中式方案吗集中式方案一样能利用相对观测把全局所有数据传到地面站统一做因子图优化即可。但集群规模一大集中式的问题就出来了通信链路全部指向地面站形成瓶颈地面站故障时全系统瘫痪。分布式协同定位的思路是“每个节点只跟邻居通信”每架无人机维护自己的因子图本地计算然后通过一致性迭代把结果逐步收敛到全局最优。代价是收敛需要多轮通信和计算但换来了整个系统没有单点故障、通信量不会随编队规模爆炸、节点即插即用。这对三十架以上规模的集群尤其有利。2. 数学框架因子图为什么比EKF更适合做集群定位2.1 从贝叶斯滤波到因子图状态空间到底长什么样定义第 i 架无人机在时刻 k 的状态为 x_i^k [p_i^k; v_i^k; q_i^k]其中 p 是三维位置v 是速度q 是姿态四元数。整个集群的状态就是所有无人机在所有时刻状态的集合。定位问题在贝叶斯框架下等价于已知所有传感器观测 z求状态 x 的最大后验估计。如果把各时刻的状态作为变量节点把传感器观测和运动学关系作为因子节点就得到一张因子图。每个因子代表一个约束整个问题转化为min_x Σ_j || h_j(x) - z_j ||²_{Σ_j}也就是非线性最小二乘优化。因为因子图天然适合稀疏结构——每架无人机只和邻居有关系并不需要全互联——所以它比EKF更适合集群定位。EKF维护的是整个集群的稠密协方差矩阵规模是 N 架 × T 个时刻的全状态复杂度随节点数近似三次方增长因子图配合增量平滑一次只更新受影响的一小部分变量计算量接近局部更新。2.2 分布式求解的两条主流路线ADMM与一致性高斯牛顿有了全局优化目标难点就变成“怎么在每架无人机只跟邻居通信的前提下求解”。我实践过两条路线。第一条是交替方向乘子法。把全局代价函数拆成每架无人机的本地代价加上“本地状态与邻居状态一致”的约束通过引入对偶变量迭代求解。ADMM收敛快但对罚参数比较敏感参数没调好会出现振荡。适合通信带宽充裕、需要高精度收敛的场景。第二条是带平均一致性迭代的分布式高斯牛顿法。思路更直白每个节点先基于自己的本地因子图计算一个状态增量然后把增量发给邻居所有邻居的增量做加权平均再用平均后的增量更新状态。重复多轮最终收敛到接近集中式优化的结果。这个方法胜在代码简单、逻辑好调代价是收敛需要的迭代次数更多、对网络拓扑的连通性要求更高。我给的matlab仿真工程里用的就是第二种因为它最容易在仿真里看清“信息是怎么在编队里传播的”。2.3 通信拓扑对收敛的影响不是所有连接方式都一样同样是六架无人机全连接、环形连接、链式连接收敛速度差别非常大。全连接每轮迭代信息传播最广收敛最快但每架无人机每轮要收五份邻居消息环形连接省流量但一条误差信息要绕一圈才能传遍全队链式连接最省可是收敛明显变慢而且一旦中间断连编队直接分成两段。我在仿真里跑过一个典型场景固定迭代二十轮| 通信拓扑 | 迭代20轮后位置RMSE | 每轮单机消息量 | | --- | --- | | 全连接 | 0.87m | 5条 | | 环形 | 1.12m | 2条 | | 链式 | 1.78m | 2条 |所以实际使用时我会建议至少保证每个节点有两个以上邻居形成网状结构。只要拓扑联合连通分布式算法就能收敛只是收敛速度不同。3. matlab仿真工程拆解从因子图构建到一致性迭代3.1 仿真场景与参数设定我这次配套的matlab工程参考了实际项目里的典型配置六架无人机一百秒航迹包含直线加速、转弯和高度变化。GNSS观测噪声水平设为水平三米、垂直五米UWB相对测距噪声设为零点二米IMU积分采用简单预积分模型。通信半径设八十米在部分时刻故意安排编队散开让拓扑发生动态变化检验算法在链路中断和恢复时的表现。初始航迹和真实状态由仿真器生成观测数据由真值叠加噪声产生。很多人仿真时喜欢把轨迹设得很规整我认为这是个误区——过于规整的轨迹会让所有方法看起来都很好。我必须加入足够的机动让误差真正暴露出来。3.2 工程目录与四个核心模块代码工程按四个模块组织dcl_sim/ initScenario.m % 生成航迹、观测、邻居关系 buildFactorGraph.m % 构建每架无人机的本地因子图 dclSolver.m % 分布式一致性求解器 evalMetrics.m % 误差统计与绘图initScenario 负责生成真值轨迹、模拟GNSS和UWB观测、按通信半径计算邻居关系。buildFactorGraph 是核心建模模块负责把一段航迹和一串观测转成因子图对象。dclSolver 实现前面说的一致性迭代求解。evalMetrics 负责计算RMSE、画轨迹对比图以及输出与集中式求解的误差差距。3.3 本地因子图构建的代码逻辑下面给出简化后的本地因子图构建片段。正式代码会因你使用的工具箱API不同而略有差异重点看逻辑% 构建第 i 架无人机的本地因子图 function gf buildLocalGraph(i, xHist, zAbs, zRel, neighInfo) gf factorGraph(); % 因子图容器可替换为gtsam或nav工具箱 N size(xHist, 2) - 1; for k 1:N % 运动学约束相邻时刻状态变化需符合IMU预积分/控制输入 gf.addFactor(motionFactor(xHist(:,k), xHist(:,k1)), ... Noise, Q_imu); % 单机绝对观测GNSS可用时加入 if zAbs.valid(i, k) gf.addFactor(absPositionFactor(xHist(:,k), ... zAbs.val(:,k)), Noise, R_gps); end % 相对观测与邻居j在k时刻的UWB测距 for j neighInfo(i).list if zRel.valid(i, j, k) gf.addFactor(rangeFactor(xHist(:,k), ... xHist_neighbor(j, k) ), ... Range, zRel.val(i,j,k), ... Noise, R_uwb); end end end end这里有两个细节容易踩坑。第一相对观测因子要挂到邻居状态变量的副本上。每个节点需要缓存邻居最近一次发送的状态因为UWB观测时间戳可能落后于本地时间。第二Q_imu和R_gps的取值会影响收敛结果协方差给得过大等于“不信任”这条信息给得过小又会过度拟合噪声。我一般用实际传感器标定的方差做初值再乘一个一到二的经验系数。3.4 一致性迭代求解器如何工作dclSolver的核心是“本地计算增量 邻居平均 带步长更新”的循环。简化的代码如下% 分布式一致性高斯牛顿迭代 for iter 1:maxIter for i 1:N % 本地因子图在当前线性化点下求解 dx0 localSolve(gf{i}, x{i}); % 收集所有邻居的上一轮增量 acc dx0; for j neighbors{i} acc acc dx_from_neighbor{j}; end % 平均增量作为本地更新方向 dxi acc / (degree(i) 1); x{i} x{i} alpha * dxi; % alpha为步长 end % 一轮结束后向邻居广播自己的增量 exchangeIncrements(dx, neighbors); end这段代码背后的思想是每架无人机先“自言自语”优化一次再听邻居怎么说最后取一个折中方向往前走。多轮迭代之后全编队的状态会收敛到一个公共解上。虽然理论上需要无限轮才能精确收敛到集中式结果但实际中迭代二十到三十轮精度就能到可用范围。我用三点验证仿真代码的正确性。第一把邻居列表清空算法退化为单机平滑结果必须与单机EKF大体一致。第二把所有观测噪声设为零估计结果应基本等于真值。第三写一个集中式全局因子图求解器所有数据放一个图里解对比分布式结果误差应在一两个数量级以内。这三个检验通过才能说算法实现没问题。4. 仿真结果与参数敏感性实测数据能说明什么问题4.1 精度收益协同比单机定位到底好多少在刚才的仿真参数下我跑了一组对比。单机GNSS组合导航的水平RMSE约二点八七米垂直RMSE约四点二米接入分布式协同定位之后水平RMSE降到零点九一米垂直RMSE降到一点一三米。这个提升主要来自UWB相对测距的约束编队越密集、几何越丰富收益越明显。方案水平RMSE垂直RMSE最大编队偏差单机GNSSIMU2.87m4.21m5.6m分布式协同全连接0.91m1.13m1.8m分布式协同环形连接1.12m1.45m2.4m值得注意的是垂直方向的改善幅度往往大于水平方向。因为GNSS高程本身误差大而UWB测距是三维距离观测对高度也有约束能力。如果你做的是需要精确高度的应用协同定位带来的收益会更直观。4.2 丢包与动态拓扑真实环境下的鲁棒性仿真里我还故意做了丢包测试每轮一致性迭代的消息按一定概率丢弃观察最终定位误差。结果很有意思10%丢包几乎没有影响20%丢包才看到轻微劣化50%丢包下仍能保持比单机定位更好的精度。原因是相对观测本身有多轮迭代信息在网络里是冗余传播的少量丢包不会造成致命影响。丢包率水平RMSE相对单机提升0%0.91m68%10%0.95m67%20%1.06m63%50%1.49m48%这个结论让我在实际工程里放宽了心UWB通信偶发丢包不会毁掉整个定位系统只要后端的迭代设计合理。4.3 计算量与扩展性从6架到12架我还测了不同集群规模下的计算量。因为每架无人机的因子图只包含自己和邻居的变量所以本地计算量基本与邻居数量成正比而不是与集群总数成正比。集群规模每架无人机平均邻居数单轮迭代平均耗时达到收敛需要的轮次3架218ms15轮6架326ms22轮12架441ms28轮真实瓶颈在网络通信每轮迭代都要广播状态增量消息频率决定整体收敛速度。仿真里我直接在内存数组中模拟消息交换所以看不出通信压力真机上这就是最大的工程挑战。5. 从matlab仿真到真机移植绕不开的工程问题5.1 时间同步与消息过期相对观测最容易被忽略的前提matlab仿真里所有观测都是同一个时间基准生成的真机完全不是这样。UWB测距模块输出的“此刻距离”和飞控IMU推算的“此刻位置”之间可能隔着几十毫秒的延迟。如果各机时钟没对齐一条本来很可靠的相对测距会变成带时延误差的坏观测。解决思路有三层。第一层尽量做时间同步最简单的是给所有机载设备装PTP或者GPS授时模块把时间基准统一到微秒级。第二层每条观测都带时间戳算法侧按时间戳对齐而不是按接收顺序。第三层如果实在同步不了可以把时钟偏差作为状态量放进因子图里在线估计。这也是我在实际项目里推荐的兜底方案。5.2 异步更新与动态拓扑算法要适应“有人进来有人出去”仿真里的邻居关系是每个时刻固定计算好的真机上节点会加入、退出、链路会中断恢复。我最开始按同步迭代实现算法每轮必须等所有邻居的消息结果一架无人机掉线整个编队就卡住了。后来改成事件驱动每架无人机按自己的节奏更新收到邻居消息就放进缓存用时间戳最新的数据参与下一轮计算。新邻居加入时要给它一个合理的初始协方差不能设得太大或太小。太大会让它长时间对编队“不信任”太小则会让错误初值污染全局。我习惯在新节点刚入网时只让它发送信息而暂时不把它邻居发来的信息计入本地优化前几秒预估稳定后再切换为正常模式。5.3 坐标系与标定松耦合与紧耦合之间的那道坎相对观测来自UWB模块而位置状态定义在NED坐标系下。UWB返回的距离是天线之间的欧氏距离如果天线安装位置跟飞控惯导原点不重合距离观测里就混入了杠杆臂误差。视角做视觉相对位姿时相机坐标系和机体坐标系之间的外参标定更麻烦一个小角度偏差在几十米距离上就会放大成米级误差。我的建议是先做一次严格的外参标定把UWB天线相位中心和相机光心相对飞控原点的平移、旋转全部量出来并在观测模型里补偿。如果时间紧张至少也要在matlab仿真里把杠杆臂误差建模进去看看它对你的精度预算影响有多大。5.4 参数调优的实践顺序我的调试路线最后聊一下参数调优。很多人拿到代码就急着把迭代轮数和罚参数调到很大结果要么收敛慢要么振荡。我的习惯是从简到繁每个环节单独验证。先把相对观测全断掉确认单机部分正确这步输出的RMSE应该和单机定位基本一致。然后只开一条测距边比如编队里两架飞机之间的距离观测看误差有没有被这条边拉下来。如果误差反而变大大概率是观测模型或时间戳对齐出了问题。最后再逐步把整个拓扑铺满每加一个节点就观察一次误差变化。我实测下来一致性迭代步长 alpha 在0.5到0.9之间表现最好接近1.0容易振荡小于0.3则收敛太慢。信息衰减因子用于降低旧消息的权重典型值在0.1左右。这些参数不是理论推导出来的而是从仿真和试飞数据里调出来的建议按你的数据重新标定。我个人调试这类算法的顺序已经固定成习惯了先断再连先单边再全网每次只引入一个变量。出问题能立刻定位到是观测、通信还是优化器的问题。这套方法帮我避开了很多“最后发现是坐标轴转置”的尴尬。分布式协同定位的技术难点其实很少在数学层面更多是“信息什么时候到、到了之后怎么合并”这些工程细节。把这一层想透了matlab里的矩阵操作和因子图调用都是收尾工作。