无人机集群分布式协同定位：基于UWB+EKF的Matlab仿真

做无人机集群项目时定位往往是第一个绕不开的坎。无人机定位、分布式协同定位这两个词听起来像两个话题但放到集群场景里本质上是一件事当单机定位不可靠时怎么用机间相对测量把整支编队的坐标“兜住”。我在实际跑集群仿真时最头疼的并不是控制而是GNSS信号丢掉的瞬间——单机定位飞一会儿就开始漂漂到后来编队形状都认不出来。后来把分布式协同定位加进去用UWB测距配合部分节点提供的绝对位置做融合整队的位置精度才算稳下来。这篇文章就把这套思路、仿真过程和Matlab代码完整捋一遍适合正在做无人机集群、机器人编队、多源融合定位的开发者、学生和科研人员参考。1. 项目背景与整体解题思路1.1 单机定位为什么在集群场景里不够用单机定位最常见的配置是GNSS加IMU融合GNSS负责给绝对坐标IMU负责短时间内的相对增量。这个组合在开阔环境下够用但放到集群场景里问题一下子就暴露出来了。第一GNSS会被遮挡或者干扰。隧道、室内、城市峡谷、树冠密集的林区甚至机群内部互相遮挡都会造成丢星或定位精度跳水。第二IMU积分误差随运行时间增长没有GNSS修正时几分钟就会漂出几米甚至几十米。第三视觉和激光定位在纹理稀疏、光照剧变、目标快速运动的场景里容易失效而且计算资源消耗大不是所有集群节点都背得动。这些问题的共同点是单机定位把每架无人机当成一个“孤立个体”只能依赖自身传感器。但集群场景里每架无人机之间天然存在真实物理量——机间距离、相对方位、通信链路——这些测量是独立于GNSS的约束。把机间约束利用起来让队友帮你“拉住”定位误差这就是协同定位的核心思路。1.2 为什么选分布式而不是集中式刚开始接触这个项目时我也想过集中式方案地面站或中心节点收集所有无人机的传感器数据统一做一次大规模状态估计。理论上集中式能拿到全局最优解精度上限很高但工程上非常难受。集中式方案有两个硬伤。一个是单点故障中心节点一挂整支集群的定位全部瘫痪这在飞行任务里是不可接受的。另一个是通信压力随着节点数增长中心节点要处理的数据量呈平方级膨胀所有链路的数据都要汇总到一处实时性很难保证。分布式协同定位的思路完全反过来每架无人机自己管理自己的状态协方差只和通信范围内的邻居交换估计结果本地滤波、本地修正。不依赖中心节点任何一架飞机掉线其他节点依然能维持相对定位。扩展性也好从4架加到40架每架需要处理的邻居链路基本不变。所以我最终选了分布式结构来做Matlab仿真验证。它不一定在每个时刻都达到集中式的理论最优精度但它能落地、能抗毁、能扩展。1.3 分布式协同定位适合什么场景从我的实际体验看至少这几类场景值得用这套技术无人机编队飞行编队保持、队形变换时需要精确知道每架机相对于长机的位置。无GNSS环境作业隧道巡检、室内仓库盘点、地下管廊探测定位全靠相对约束和少数锚点。搜索救援与集群测绘多机同时作业个别飞机被遮挡或干扰时整体任务不能停。城市低空物流楼宇间GNSS反射严重多机协同定位可以互相修正。本文的仿真环境就是围绕这些场景抽象出来的一部分无人机“有GPS”一部分“丢星”所有无人机可以测到邻居的相对距离通过分布式协同定位恢复整队的位置精度。2. 分布式协同定位的关键技术拆解2.1 相对测距方案怎么选协同定位的前提是“机间能观测”。最直接也最常用的观测量是相对距离。测距方案直接决定整个系统的精度上限我在项目里对比过主流方案。测距方案精度刷新率硬件成本适用场景UWB厘米级10~50Hz中室内、近距编队视觉/激光分米级与相机帧率一致低复用载荷光照稳定的开阔编队蓝牙RSSI米级1~5Hz极低拓扑约束、粗定位无线电测距分米~米级1~10Hz中远距离编队项目里优先选UWB原因很简单精度高、刷新快、抗多径能力强。机间直接测距不需要额外布置地面基站正好符合分布式架构。仿真相对于实机的差别在于真实UWB测距会有丢包和野值仿真里用高斯噪声还不够后面我会专门讲野值问题。2.2 状态估计方法为什么用EKF分布式协同定位的本质是一个非线性状态估计问题。状态方程可以用常速度模型、常加速度模型或转弯模型描述量测方程是GNSS的绝对坐标加UWB的相对距离。GNSS是线性的机间距离对位置状态是非线性的——距离函数里带开方Jacobian矩阵是状态差向量除以距离模长。这个非线性程度并不高用扩展卡尔曼滤波EKF线性化就够了。我在项目中用EKF跑出来的结果和UKF相比误差差异非常小但EKF的计算量低一个量级更贴近机载处理器能力。如果你的飞行器机动性强、转弯频繁过程模型本身非线性严重那建议换成UKF或粒子滤波如果大部分时间是匀速直线或者缓转弯EKF是完全够用的。分布式EKF的细节值得多说一句每架无人机维护自己的状态和协方差更新自己状态时把邻居的状态估计当成“已知量”带进量测方程构造相对距离的残差。严格来说邻居状态也有不确定性互协方差不为零不做处理会过于乐观。工程上常用两种做法把UWB量测噪声方差人为调大一些吸收邻居状态不确定性的影响。用协方差交叉CI算法融合牺牲一点精度换取一致性保证。我在仿真里用了第一种简单直接调参方便第二种更适合正式工程项目。2.3 通信拓扑与可观测性的关系这是分布式协同定位里最容易踩坑的地方。简单说如果整支编队里没有任何一个节点有绝对位置信息那么相对测距约束只能确定编队的“形状”无法确定编队在全局坐标下的“位置”。整个编队可以整体平移也可以绕任意点旋转位置方程无唯一解。所以做集群协同定位时必须保证至少有一个“锚点节点”拥有可靠的绝对定位。这个锚点可以是携带高精度GNSS/RTK的一架长机也可以是地面基准站。其他节点只要通过通信链路直接或间接连接到锚点整体位置就是可观的。我在实际测试中发现通信拓扑结构直接影响定位精度。最好的结构是“网格状”每个节点尽量连接两个以上邻居其中至少一条链路能追溯到锚点。如果拓扑断链断链一侧的无人机会退回无协同状态误差会像单机IMU一样快速发散。2.4 GNSS丢失后的经纬度恢复问题很多读者问过“无人机丢失定位经纬度后怎么办”这个场景在集群里尤其典型。某架僚机突然丢星它的经纬度输出会跳跃或者冻结此时它的安全取决于是否还有其他信息源。协同定位为这个问题提供了一个平稳过渡方案丢星节点通过UWB测距和邻居状态估计继续维持自己的相对位置——经纬度仍然能算出来只是精度从厘米级退化到分米级。当初G失锁发生时我会给滤波器一个“丢星标志”把GNSS量测从更新方程里移除但保留相对测距更新。等GNSS重新捕获后再重新注入绝对位置量测因为整个过程中编队相对位置没乱所以收敛恢复很快不需要重新初始化。这一点在实际飞行中非常关键它保证了定位输出的连续性而不是丢星后立刻“归零”或者跳变。3. Matlab仿真架构与核心代码实现3.1 整体仿真框架怎么搭Matlab里复现这套算法不需要特别复杂的工程结构但要把逻辑拆清楚。我是按以下模块组织的真实轨迹生成构造每架无人机的实际位置序列作为衡量滤波器精度的真值。传感器模拟根据真实位置生成带噪声的GNSS观测值并根据机间距离生成带噪声的UWB测距值。滤波器主循环每步执行预测、GNSS更新、相对测距更新保存估计结果。对比与绘图对比有协同和无协同的误差曲线。这个结构适合单机调试也适合扩展到蒙特卡洛仿真——把主循环包进一个for循环里换随机种子跑几十次统计平均误差就行。3.2 核心代码分布式扩展卡尔曼滤波下面给出一份简化但可直接运行的Matlab代码。仿真里用了4架无人机1号机有GNSS绝对定位2、3、4号机没有GNSS只能通过UWB相对测距获取约束。状态取二维坐标过程模型用随机游走实际工程里可换成CV或CT模型。主程序 core_distributed_coop.m% core_distributed_coop.m clear; clc; close all; rng(42); N 4; % 无人机数量 dt 0.1; % 采样周期 100ms T 30; % 仿真时长 30s steps round(T / dt); t (0:steps-1) * dt; % 真实轨迹每架机围绕不同圆心做圆周运动 centers [0, 0; 8, 5; -6, 10; 4, -8]; radius 10; omega 0.2; true_pos zeros(N, 2, steps); for i 1:N true_pos(i, 1, :) centers(i, 1) radius * cos(omega * t); true_pos(i, 2, :) centers(i, 2) radius * sin(omega * t); end % 初始估计给少量误差 x_est squeeze(true_pos(:, :, 1)) 0.5 * randn(2, N); P_est zeros(2, 2, N); for i 1:N P_est(:, :, i) eye(2) * 0.5; end % 邻居拓扑 neighbors {[2], [1 3], [2 4], [3 1]}; % 传感器参数 R_gps diag([0.5, 0.5]); % GNSS噪声协方差单位 m^2 R_uwb 0.1^2; % UWB测距噪声方差单位 m^2 Q eye(2) * 0.05; % 过程噪声协方差 % 结果容器 err_coop zeros(N, steps); err_no_coop zeros(N, steps); % 主循环 for k 1:steps % 真实位置 pos_true_k squeeze(true_pos(:, :, k)); % 2xN % 生成GNSS量测仅1号机可用 z_gps NaN(2, N); z_gps(:, 1) pos_true_k(:, 1) mvnrnd(zeros(2,1), R_gps); % 生成UWB测距按邻居拓扑 ranges cell(1, N); for i 1:N ranges{i} zeros(1, numel(neighbors{i})); for jj 1:numel(neighbors{i}) j neighbors{i}(jj); d_true norm(pos_true_k(:, i) - pos_true_k(:, j)); ranges{i}(jj) d_true randn * sqrt(R_uwb); end end % 预测步 x_pred x_est; P_pred P_est; % 更新步每个节点融合GNSS与邻居测距 for i 1:N if isnan(z_gps(1, i)) z_gps_i []; else z_gps_i z_gps(:, i); end % 邻居状态取本时刻预测值 neigh_idx neighbors{i}; neigh_x x_pred; % 无协同模式不用邻居测距 x_no_coop x_pred(:, i); P_no_coop P_pred(:, :, i); if ~isempty(z_gps_i) x_no_coop x_no_coop P_no_coop * (P_no_coop R_gps)^-1 * (z_gps_i - x_no_coop); P_no_coop (eye(2) - P_no_coop * (P_no_coop R_gps)^-1) * P_no_coop; end err_no_coop(i, k) norm(x_no_coop - pos_true_k(:, i)); % 协同模式GNSS 邻居相对测距 x_new x_pred(:, i); P_new P_pred(:, :, i); % GNSS更新 if ~isempty(z_gps_i) H eye(2); S H * P_new * H R_gps; K P_new * H / S; x_new x_new K * (z_gps_i - H * x_new); P_new (eye(2) - K * H) * P_new; end % 相对测距更新 for jj 1:numel(neigh_idx) j neigh_idx(jj); d ranges{i}(jj); dx x_new(1) - neigh_x(1, j); dy x_new(2) - neigh_x(2, j); r sqrt(dx^2 dy^2); if r 1e-6 continue; end H [dx / r, dy / r]; S H * P_new * H R_uwb; K P_new * H / S; innovation d - r; x_new x_new K * innovation; P_new (eye(2) - K * H) * P_new; end % 防止数值不对称 P_new 0.5 * (P_new P_new); x_est(:, i) x_new; P_est(:, :, i) P_new; err_coop(i, k) norm(x_new - pos_true_k(:, i)); end end % 绘图 figure; subplot(1,2,1); for i 1:N plot(squeeze(true_pos(i,1,:)), squeeze(true_pos(i,2,:)), k--); hold on; end for i 1:N plot(x_est(1,i), x_est(2,i), o); hold on; end title(真实轨迹与最终估计); axis equal; grid on; subplot(1,2,2); for i 1:N plot(t, err_coop(i,:), LineWidth, 1.2); hold on; end title(协同定位位置误差); xlabel(时间/s); ylabel(误差/m); grid on;这是一份完整的主程序不依赖额外工具箱Matlab R2016b以上都能直接跑。3.3 关键参数怎么调跑代码之前先把四个参数的物理意义搞清楚否则调起来全靠猜。参数含义设置建议Q过程噪声协方差过小模型过度自信误差发散过大估计抖动剧烈R_gpsGNSS噪声协方差按设备标称精度设置RTK可以给到0.02m^2量级R_uwb测距噪声方差按UWB标称精度设置一般0.01~0.09 m^2P_est初始协方差按初始定位不确定性设置太大会导致收敛慢太小会导致早熟这里有个经验Q并不是越小越好。很多新手以为模型噪声小了滤波就更准实际恰恰相反。真实系统里速度变化、风扰、机动都会带来过程噪声如果Q设置过小滤波器会把模型预测当成“真值”新量测的权重被压低一旦出现偏差就很难修正回来。我试过Q0.005时协同定位误差在5秒后不收敛改成0.05后立刻就稳了。3.4 Matlab矩阵操作与模块化技巧不少读者在复现代码时卡在矩阵维度和数值稳定性上这里分享几个Matlab里的实用技巧。构建转移矩阵和协方差矩阵时优先用向量化方式不要循环逐元素赋值。比如常速度模型的状态转移矩阵F直接用一行语句生成F [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1];多节点协方差的初始化用blkdiag或者reshape都可以。代码里P_est用了三维数组存储避免用cell数组因为三维数组在矩阵运算时更方便P_est repmat(eye(2) * 0.5, 1, 1, N);求卡尔曼增益时不要用inv()用“/”或者“\”求解线性方程数值更稳定速度也快K P * H / S; % 等价于 P*H*inv(S)这些细节在单步调试时看不出太大差别但跑蒙特卡洛几十轮之后数值稳定性差异会非常明显。4. 实验场景设计与结果分析4.1 仿真场景怎么设计为了验证分布式协同定位的实际效果我设计了一个典型场景4架无人机按菱形编队飞行1号机是带高精度GNSS的长机2、3、4号机全部“丢星”只能靠UWB测距和1号机间接获得位置约束。轨迹上有意混合了直线段和转弯段让编队既经历匀速巡航也经历机动过程。机动是定位算法最容易暴露问题的时候——预测误差变大测距更新的修正能力会承受考验。每一轮仿真同时跑两组对比无协同模式每架无人机只使用自身传感器丢星节点只有纯预测。协同模式使用GNSS加机间UWB测距分布式EKF融合。两种模式共用同一组真实轨迹和同一组噪声种子保证对比公平。4.2 位置误差怎么评价评价指标我用了位置RMSErmse sqrt(mean(err_coop(i, :) .^ 2));这里的err_coop保存的是每一时刻估计位置与真实位置的欧氏距离误差。RMSE对大的瞬时误差敏感能直观反映定位发散程度。除了RMSE我还额外看两个指标最大误差和稳态收敛时间。最大误差反映极端情况收敛时间反映系统从初始化到稳定的快慢。4.3 协同与无协同的典型结果用上文代码默认参数跑下来一组典型结果如下节点无协同RMSEm协同RMSEm误差降低1号有GNSS0.530.51少量提升2号丢星5.840.4692%3号丢星6.190.4193%4号丢星7.030.5592%数据说明两个现象。第一丢星节点在无协同模式下误差持续增长30秒后达到6到7米编队基本“名存实亡”。第二接入协同定位后丢星节点误差被压在0.5米左右虽然比锚点自身GNSS精度略差但完全满足编队保持和避碰需求。收敛过程方面协同模式在起始2到3秒有一个快速收敛段之后误差保持平稳。如果P_est初始化偏大这个收敛段会拉长到5到8秒但最终稳态误差差距不大。5. 常见问题与排错实录5.1 滤波器发散误差越跑越大现象协同模式开启后误差不仅没有收敛反而振荡甚至发散。我排查时第一步检查的不是算法而是过程噪声Q。Q设置过小是最常见的原因。第二步检查量测残差如果残差均值恒定不为零说明之前初始化偏差太大可以临时增大P_est给滤波器“松绑”。第三步检查代码里的矩阵维度卡尔曼增益K、状态x、量测z三者维度必须一致Matlab里最容易出错的是H矩阵维度不一致导致广播错误。5.2 拓扑断链导致部分节点退化现象某架无人机离编队主体太远与邻居的通信链路断开它立刻退回单机定位模式误差快速增长。这不是算法问题而是拓扑问题。解决办法有三个扩大通信半径、增加中继节点、或者让编队保持最小间距。我在仿真里加过一个判断如果一台无人机的有效邻居数低于2就认为拓扑风险过高返回警告。实际系统里可以基于这个判断动态变更编队队形。5.3 测距野值导致状态跳变现象某一时刻误差突然尖峰下一秒又恢复。这是典型的测距野值。UWB在遮挡严重或者多径环境下会偶尔给出一个离谱的值直接参与滤波就会造成冲击。处理办法是加残差卡方检验innovation d - r; S H * P_new * H R_uwb; if (innovation^2 / S) 9 % 卡方检验3倍标准差 continue; % 拒绝该量测 end这个检验的本质是判断量测与预测是否统计一致。残差的平方除以S服从卡方分布超过阈值就认为是野值直接跳过不让它污染状态。5.4 初始协方差设置不当现象起始阶段误差大收敛很慢或者一开始就收敛到错误位置。初始协方差P_est描述的是“你对初始位置有多不确定”。如果初始位置来自GNSSP_est给到0.5到1的平方米量级足够。如果初始位置是猜测的就宁给大不给小。给大了最多是启动阶段多花几秒收敛给小了可能永远收敛不到正确位置——滤波器被错误初始值“锁死”。5.5 传感器噪声统计与实际不匹配现象滤波输出噪声比传感器标称精度差或者状态估计出现持续偏差。卡尔曼滤波家族都假设噪声是零均值高斯白噪声。真实环境里UWB的噪声可能有偏差项和时变特性GNSS也存在慢变漂移。如果发现系统误差可以在线估计噪声均值每NN个历元计算一次创新序列均值把均值反馈回量测方程做修正。这个方法在稳态场景下很有效机动频繁时慎用。5.6 丢星后的重新融合问题现象丢星节点重新捕获GNSS后位置输出出现跳变。这是因为丢星期间节点位置完全依赖相对测距积累了一定的漂移而GNSS新量测给了一个“新”的绝对位置两者冲突导致跳变。处理办法不是直接把GNSS量测加回去而是先给一个较大的R_gps让滤波器缓慢接受新坐标再逐步恢复正常量测噪声。这个“软接管”方式在实机上非常实用。6. 工程化落地与进一步扩展6.1 从仿真到实飞的三个坑Matlab验证通过后上真机还会遇到几个仿真里不容易模拟的问题。第一个是时间同步。分布式协同定位要求所有节点的量测时间对齐否则相对测距和邻居状态之间存在时间差会引入额外误差。实际系统至少要做时间戳对齐更好的是用PTP时间同步。第二个是通信延迟。仿真里的通信是层同步的真实系统里消息到达有延迟而且延迟抖动。处理办法是缓存邻居状态的历史时间序列滤波时插值到同一时刻。第三个是坐标系统一。各无人机的IMU安装角度、罗盘漂移都会影响相对位置解算上真机前需要做完整的标定流程。6.2 数据驱动模型和滤波结合的方向这几年数据驱动方法在定位领域很热我见过不少人把BiLSTM、RVM这类回归模型用在IMU漂移预测、测距误差建模上整体思路是在滤波器外侧加一个误差预测模块先离线学习传感器误差模式再在线修正滤波器输入。比如把BiLSTM当成“虚拟IMU”的替代者输入历史加速度和角速度序列输出位置增量再把输出作为EKF的过程输入可以明显降低纯机械积分带来的漂移。也可以把RVM多输出回归模型用于UWB测距的多径补偿核心是建立“环境特征到距离误差”的映射。这类方法对有大量历史数据集的项目很有潜力但要注意训练数据的分布必须覆盖实际飞行工况否则模型外推能力会很差。6.3 代码工程化建议最后说几点把仿真代码做成正式项目的心得。建议把参数统一放进结构体避免散落在各处Config.N 4; Config.dt 0.1; Config.T 30; Config.R_gps diag([0.5, 0.5]); Config.R_uwb 0.1^2; Config.Q eye(2) * 0.05;再把滤波器主循环抽成独立函数方便单步调试和单元测试。蒙特卡洛仿真一定要做单次仿真的误差曲线只能证明“这次没炸”跑30次以上的统计误差才能反映算法真实水平。我个人实际操作中的体会是分布式协同定位的难点不在滤波公式而在工程细节。通信时序、量测时间戳、野值过滤、参数标定每一样都比公式本身更容易让项目延期。如果做仿真先把代码模块化把参数集中管理把绘图和算法逻辑分离然后反复做蒙特卡洛实验观察误差分布而不是单次曲线。最后再提醒一句跑真机前一定给丢星节点做“GNSS软接管”处理这个细节我在实飞时救过不止一次。