操作环境MATLAB 2024a1、算法描述摘要多无人机协同编队能够通过空间分布和任务分工提高系统的覆盖范围、容错能力与任务执行效率。然而在卫星导航信号受遮挡、干扰或无法使用的环境中无人机之间难以持续获得准确的相对位置信息。测距噪声、非视距传播、随机丢包和传感器偏置还会进一步降低编队稳定性。针对上述问题本文设计了一种基于超宽带多基线测距和鲁棒扩展卡尔曼滤波的三无人机三角编队协同定位与控制方法。系统采用一架领航无人机和两架跟随无人机组成等边三角形编队在领航机上设置双基线超宽带测距节点同时保留两架跟随无人机之间的直接测距链路构成五条测距链路。状态估计部分将两架跟随无人机相对领航机的位置、速度和速度观测偏置统一纳入扩展卡尔曼滤波状态通过归一化新息门限排除严重异常测量并利用Huber权重降低边缘异常值对状态估计的影响。控制部分采用领航轨迹跟踪与跟随相对位置控制相结合的结构并加入参考加速度前馈、相对速度反馈、积分补偿和控制量限幅。仿真结果表明在存在高斯测距噪声、连续非视距误差、随机离群点和测距丢包的条件下系统仍能维持稳定的三角形编队。单次仿真的稳态边长误差均方根为0.0185米两架跟随无人机的稳态相对位置估计均方根分别为0.0291米和0.0277米超宽带测量接收率为96.49%。40次Monte Carlo仿真中稳态边长误差均方根的95%分位为0.0180米说明所设计方法具有较好的定位精度、异常测量抑制能力和统计稳定性。关键词超宽带测距无人机编队相对定位扩展卡尔曼滤波非视距误差轨迹跟踪1 引言无人机编队通过多架飞行器之间的协同可用于巡检、测绘、目标跟踪、区域覆盖、应急搜索和通信中继等任务。与单架无人机相比多无人机系统具有任务并行、空间覆盖范围大和局部故障容忍能力强等特点。但编队系统并不是简单地增加无人机数量。只要相对定位、通信同步或控制分配出现明显误差多架无人机之间就可能产生队形变形、跟踪误差累积甚至碰撞风险。相对导航是多无人机协同中的基础环节。已有研究表明在只有部分无人机能够获得全局位置信息的情况下可利用无人机之间的距离观测实现相对定位并进一步完成覆盖控制或协同任务。UWB收发节点同时具备无线通信和距离测量能力适合用于无人机之间的协同定位。相关研究已经将UWB测距用于单无人机定位、无人机群扩展定位和多无人机相对覆盖控制。卫星导航系统在开阔环境中能够提供全局位置但在厂房、地下空间、建筑物附近或存在强干扰的区域中卫星信号可能衰减或中断。视觉和激光定位通常依赖环境特征、光照条件和有效视场。UWB测距主要利用信号传播时间获得节点间距离在缺少明显视觉特征的环境中仍具有应用价值。多基线布置还可以通过增加空间约束改善单一距离观测无法确定方向的问题。现有研究已验证UWB多节点布置在平面相对定位和室内定位中的可行性。UWB测距并不意味着测量数据始终可靠。当无人机与测距节点之间存在墙体、设备或机体遮挡时信号可能经过反射或绕射后到达接收端测量距离通常出现明显正偏差。若直接将此类测量输入普通卡尔曼滤波器异常数据会通过滤波增益传递到位置和速度状态。相关NLOS鲁棒定位研究通常需要对传播状态、异常偏差或测量可信度进行估计和抑制。针对上述问题本文构建三架无人机等边三角形编队仿真系统。系统不依赖全部无人机的绝对位置而是利用领航机双基线到两架跟随无人机的四条测距链路以及跟随无人机之间的一条测距链路进行协同定位。相对位置估计采用带新息门限和Huber加权的扩展卡尔曼滤波编队控制采用领航—跟随结构。研究重点不是单独追求理想条件下的定位精度而是在丢包、连续NLOS误差、随机离群点和速度偏置共同存在时验证定位与控制闭环的稳定性。2 系统总体方案系统由无人机1、无人机2和无人机3组成。无人机1作为领航机负责沿给定曲线路径运动无人机2和无人机3作为跟随无人机分别位于领航机后方两侧。三架无人机的期望相对位置构成边长为4米的等边三角形。该队形不是固定在全局坐标系中而是跟随领航机速度方向进行旋转因此编队能够在曲线路径上保持合理的朝向。领航机安装两个具有一定横向间距的UWB节点双基线长度设置为1.2米。无人机2分别测量其到两个领航机节点的距离无人机3同样获得两条距离观测。同时无人机2与无人机3之间进行直接测距。系统共获得五个标量距离观测。与单节点测距相比领航机双基线可以提供更丰富的几何约束跟随无人机之间的测距则可直接约束三角形第三条边降低两个跟随目标相对状态同时漂移的可能性。系统仿真周期为0.05秒总时长为60秒。领航机参考轨迹由恒定前向运动和两个不同频率的横向正弦分量构成。该轨迹同时包含直线推进、缓慢转弯和曲率变化可以检验编队在持续转向情况下的队形保持能力。参考轨迹的位置、速度、加速度、航向角、角速度和角加速度均由解析关系直接产生避免数值差分放大噪声。整个系统分为测量层、状态估计层和控制层。测量层生成UWB距离、相对速度和有效性标志状态估计层根据运动模型和观测数据估计两架跟随无人机相对领航机的状态控制层根据期望相对位置与估计相对位置之间的偏差计算控制加速度。无人机运动状态更新后又会影响下一时刻的距离观测从而构成完整的定位与控制闭环。多无人机编队通常需要同时解决轨迹跟踪、相对位置保持和局部信息交互问题。领航—跟随结构能够将全局任务轨迹集中到领航机将跟随无人机的控制目标转化为相对状态跟踪结构清晰且便于工程实现。现有编队研究中领航—跟随控制、分布式一致性控制和基于邻居信息的队形控制均是常用实现路线。3 UWB多基线测距与误差处理仿真中的真实距离由各无人机实际位置和领航机双基线节点位置计算获得。在真实距离基础上系统叠加标准差为0.06米的高斯测距噪声。该噪声用于描述接收时间戳误差、设备内部延迟波动和环境多径引起的小范围随机变化。为了模拟具有时间连续性的NLOS环境本文没有将每一个NLOS误差设置为完全独立的随机事件而是采用两状态马尔可夫过程描述视距和非视距状态。每条链路在任一时刻都处于正常状态或NLOS状态。正常链路按照给定概率进入NLOS状态NLOS链路按照另一概率恢复到正常状态。进入NLOS状态后测距值产生正向偏差并在后续多个采样周期中逐渐变化。该模型比单个采样点的独立脉冲误差更接近遮挡持续存在时的测量现象。除连续NLOS误差外系统还设置了低概率随机离群点。离群点会在正常测距基础上产生额外的较大正偏差用于模拟偶发错误时间戳、错误首径检测或严重多径干扰。系统同时设置基础丢包概率和NLOS附加丢包概率。当链路测量无效时相应距离数据被设置为空值滤波器跳过该链路的测量更新。上述设置使测距数据同时包含小范围随机噪声、持续正偏差、偶发大偏差和数据缺失。普通最小二乘或标准扩展卡尔曼滤波依赖测量噪声近似服从零均值高斯分布而连续NLOS误差明显不满足这一假设。因此必须在滤波更新前判断测量残差是否合理并根据残差大小动态调整测量可信度。本文首先计算每条距离测量的新息并利用预测协方差和测量噪声将新息归一化。考虑到NLOS测距大多表现为正偏差系统采用非对称门限正向新息使用较严格的门限负向新息使用相对宽松的门限。当归一化新息超出门限时该测量直接被拒绝不参与当前时刻的状态修正。对于没有超过拒绝门限、但残差仍然偏大的测量系统利用Huber权重进行软降权。当残差较小时测量权重保持为1滤波器按照正常测量噪声完成更新当残差超过Huber阈值后权重随残差增大而降低。程序通过增大等效测量噪声的方式降低异常观测对应的卡尔曼增益。与单纯使用硬门限相比该方法避免了在门限附近频繁接受或拒绝测量造成的不连续变化。4 鲁棒扩展卡尔曼滤波相对定位方法滤波器状态同时描述无人机2和无人机3相对领航机的位置、速度以及速度观测偏置。每架跟随无人机对应六个状态两架无人机共形成十二维状态向量。将速度偏置纳入状态的原因是若直接把带固定偏差的速度观测当作无偏测量位置估计会持续积累误差。通过偏置状态的随机游走模型滤波器能够在运行过程中逐步分离真实相对速度与传感器偏置。状态预测采用离散运动学模型。相对位置由上一时刻相对位置和相对速度推进相对速度由领航机与跟随无人机之间的控制加速度差进行修正。过程噪声按照连续加速度白噪声模型离散化并对速度偏置设置独立随机游走噪声。该预测方式利用了控制器已经计算出的加速度信息使状态预测不完全依赖距离测量。五条UWB链路对应五个非线性距离观测。无人机2到领航机两个节点的距离由无人机2相对位置与两个基线偏移量决定无人机3采用相同结构无人机2与无人机3之间的距离由两组相对位置之差决定。滤波器根据当前预测状态计算距离预测值并求取距离对状态的局部敏感关系。距离观测更新采用逐链路顺序处理方式。每处理一条有效测量状态和协方差都会立即更新下一条测量使用最新状态继续计算。顺序更新不需要同时构造大型观测矩阵也便于针对每条链路分别执行异常值检测和权重调整。协方差更新采用数值稳定性较高的形式并在每次更新后对协方差矩阵进行对称化处理。这样可以降低浮点计算造成的非对称误差避免长时间仿真后协方差矩阵出现不合理结果。距离测量主要约束相对位置但对相对速度和速度偏置的直接可观测性有限。系统因此增加四个相对速度标量观测分别对应两架跟随无人机在两个坐标方向上的相对速度。速度测量模型将真实相对速度与偏置状态相加。滤波器在完成距离更新后继续处理速度观测并设置五倍标准差门限以避免速度观测中的极端异常值直接破坏状态估计。可靠的相对导航不仅要关注平均误差还要识别测量故障和导航结果失效风险。相关多无人机相对导航研究也强调了故障检测、完整性监测和异常观测隔离的重要性。 本文采用的新息门限、Huber加权和测量使用率统计正是对这一问题的简化工程实现。5 三角形编队与轨迹跟踪控制领航机采用位置与速度反馈控制。控制器以参考加速度为前馈项根据领航机实际位置与期望位置之间的误差产生位置反馈根据实际速度与期望速度之间的误差产生阻尼反馈。该结构能够在跟踪曲线路径时兼顾响应速度和运动平稳性。两架跟随无人机不直接根据自身绝对位置完成编队控制而是根据滤波器输出的相对状态进行控制。每架跟随无人机首先计算自身相对领航机的期望位置和期望速度再与估计的相对位置和相对速度进行比较。控制器由相对位置比例项、相对速度微分项和位置误差积分项组成。积分项主要用于减小速度偏置、模型误差或持续扰动造成的稳态队形误差。但积分增益过大可能导致控制量积累和队形振荡。因此程序对积分状态设置上下限。当误差持续存在时积分项只能在有限范围内变化避免控制器长时间饱和后产生严重超调。在弯曲轨迹上等边三角形队形需要随领航机航向发生旋转。跟随无人机不仅要跟随领航机平移还需要产生由编队角速度和角加速度引起的附加运动。程序根据领航机航向角、角速度和角加速度计算每个编队顶点的期望速度与期望加速度并将跟随点相对领航点的期望加速度作为前馈补偿。若忽略该项编队在直线路段可能保持稳定但在转弯过程中会产生明显的外扩或滞后。跟随无人机最终控制量由领航机控制加速度与相对编队控制加速度叠加得到。三架无人机的控制加速度均设置最大幅值限制。限幅不是简单地分别裁剪两个坐标分量而是在保持原控制方向的前提下限制向量模长因此不会改变控制器给出的主要运动方向。多旋翼编队在动态路径上的控制不能只关注静态几何队形还要同时考虑参考运动的速度和加速度变化。相关研究已经证明引入运动前馈和相对状态反馈有助于提高多旋翼编队的动态轨迹跟踪能力。6 仿真结果与分析仿真首先对三架无人机的实际轨迹和期望轨迹进行对比。结果图显示领航机沿给定曲线路径运动两架跟随无人机分别位于领航机后方两侧。三架无人机实际轨迹与期望轨迹基本重合。在不同时间位置绘制的三角形连接线表明编队能够随路径航向连续旋转没有出现明显的队形翻转或跟随无人机交换位置现象。领航机全时段位置误差均方根为0.0234米进入稳态后的误差均方根为0.0034米。全时段误差包含初始位置和速度偏差引起的过渡过程因此高于稳态结果。该结果说明领航机控制器能够快速消除初始状态误差并稳定跟踪参考路径。UWB测距结果图给出了五条链路的真实距离、带噪测量和被滤波器拒绝的异常测量。多数有效测量分布在真实距离附近NLOS或离群测量则表现为明显正偏差。异常值拒绝标记说明滤波器能够在异常测量进入状态更新之前将其识别。在当前随机种子和参数条件下UWB平均接收率为96.49%NLOS测量拒绝率为100%正常视距测量误拒率约为0.0358%。需要指出这些数值是当前仿真条件下的统计结果不能直接等同于实际硬件系统在所有环境中的性能。两架跟随无人机的相对位置真实值与EKF估计值总体一致。初始阶段由于设置了相对位置、相对速度和偏置初始误差估计结果需要一定时间完成收敛。进入稳态后无人机2相对位置估计误差均方根为0.0291米无人机3为0.0277米。两组结果接近说明双基线结构对两架跟随无人机提供了较为均衡的几何约束。三角形三条边的实际距离均能收敛到4米附近。全时段边长误差均方根为0.0422米稳态边长误差均方根为0.0185米。稳态绝对边长误差的95%分位为0.0352米稳态最大误差为0.0562米。以三条边误差同时持续小于0.08米作为收敛条件单次仿真的编队收敛时间为1.9秒。无人机2和无人机3的稳态轨迹跟踪误差均方根分别为0.0349米和0.0332米。该误差略大于相对位置估计误差原因在于实际跟踪结果还会受到运动过程噪声、控制加速度限幅和控制器动态响应的共同影响。控制量曲线没有持续贴近最大加速度限制说明当前控制参数没有依赖长时间饱和来维持队形。为了避免仅使用一个随机种子得出片面结论程序进一步执行40次Monte Carlo仿真。每次仿真使用不同随机种子重新生成测距噪声、NLOS状态、离群点、丢包和过程扰动。统计结果表明稳态边长误差均方根的中位数为0.0163米95%分位为0.0180米收敛时间的95%分位为2.7525秒。单次结果与Monte Carlo统计结果处于同一数量级没有出现单次仿真表现良好、随机重复后明显失稳的情况。从结果可以看出系统性能并不是依靠完全无丢包或完全视距的理想测量实现的。滤波器允许部分测量缺失并对正偏异常值执行拒绝或降权控制器根据估计相对状态持续修正编队误差。即使某一时刻部分链路无法使用运动模型、速度观测和其他有效UWB链路仍可以共同维持短时间状态估计。但当前结果仍存在明确边界。仿真使用二维质点运动模型没有进一步建立无人机姿态、推力、气动力和电机动态UWB天线位置只考虑平面旋转没有考虑机体俯仰和横滚导致的三维基线变化通信时延、节点时钟漂移和UWB多节点测距调度冲突也没有单独建模。因此该系统适合用于验证协同定位和编队控制算法不能直接替代实际飞行平台的硬件在环和外场测试。7 系统特点第一系统采用领航机双基线和跟随无人机间直接测距相结合的五链路结构。双基线提高了单架跟随无人机相对位置的几何约束能力跟随无人机间链路则直接限制三角形第三条边。第二系统在同一状态估计器中同时估计两架跟随无人机的相对位置、相对速度和速度测量偏置。该结构能够利用两架跟随无人机之间的距离关系完成联合约束避免将两架无人机完全拆分为两个互不关联的定位问题。第三测距模型同时包含高斯噪声、连续NLOS误差、随机离群点和数据丢失。相比只叠加普通白噪声的理想仿真该设置更能检验滤波器面对非高斯异常数据时的稳定性。第四鲁棒EKF采用非对称新息门限和Huber软降权相结合的处理方式。严重异常测量被直接拒绝中等残差测量被降低权重正常测量保持原有更新强度。第五编队参考状态考虑领航轨迹的航向角、角速度和角加速度。三角形队形能够随路径方向旋转而不是始终固定在全局坐标方向。第六控制器包含参考加速度前馈、相对位置反馈、相对速度反馈、有限积分补偿和向量幅值限幅。该结构兼顾了动态跟踪、稳态误差抑制和控制量约束。第七程序同时输出轨迹、UWB测距、EKF相对位置、估计误差、编队边长、控制量和Monte Carlo统计结果并将主要指标保存为CSV和MAT文件便于后续参数对比和重复验证。结论本文完成了一套三无人机UWB协同定位与三角形编队控制仿真系统。系统通过领航机双基线、跟随无人机间测距、相对速度观测和运动模型建立联合相对定位框架并利用新息门限与Huber加权提高扩展卡尔曼滤波对NLOS和离群测量的适应能力。编队控制器根据估计相对状态保持等边三角形队形并跟随连续变化的曲线路径。仿真结果表明所设计方法在存在测距噪声、连续NLOS偏差、离群点和丢包的条件下仍可实现厘米级稳态编队误差。40次Monte Carlo结果进一步说明系统对随机测量扰动具有较稳定的重复性能。后续研究应扩展到三维六自由度无人机模型加入通信时延、时钟误差、测距调度、动态遮挡和避碰约束并通过硬件在环或实际UWB无人机平台验证算法性能。参考文献[1] Zhang, Ziyong, Xiaoling Xu, Jinqiang Cui, and Wei Meng. “Multi-UAV Area Coverage Based on Relative Localization: Algorithms and Optimal UAV Placement.”Sensors, vol. 21, no. 7, 2021, article 2400. DOI: 10.3390/s21072400.[2] Steup, Christoph, Jonathan Beckhaus, and Sanaz Mostaghim. “A Single-Copter UWB-Ranging-Based Localization System Extendable to a Swarm of Drones.”Drones, vol. 5, no. 3, 2021, article 85. DOI: 10.3390/drones5030085.[3] Wang, Mingyang, et al. “UWB Based Relative Planar Localization with Enhanced Precision for Intelligent Vehicles.”Actuators, vol. 10, no. 7, 2021, article 144. DOI: 10.3390/act10070144.[4] Hyun, Jieum, and Hyun Myung. “NR-UIO: NLOS-Robust UWB-Inertial Odometry Based on Interacting Multiple Model and NLOS Factor Estimation.”Sensors, vol. 21, no. 23, 2021, article 7886. DOI: 10.3390/s21237886.[5] Pan, Hao, et al. “An UWB-Based Indoor Coplanar Localization and Anchor Placement Optimization Method.”Neural Computing and Applications, vol. 34, 2022, pp. 16845–16860. DOI: 10.1007/s00521-022-07329-8.[6] Hou, Zhicheng, Gong Zhang, Wenlin Yang, Weijun Wang, and Changsoo Han. “Flatness-Based Aggressive Formation Tracking Control of Quadrotors with Finite-Time Estimated Feedforwards.”Applied Sciences, vol. 11, no. 2, 2021, article 792. DOI: 10.3390/app11020792.[7] Lizzio, Fausto Francesco, Elisa Capello, and Giorgio Guglieri. “A Review of Consensus-Based Multi-Agent UAV Implementations.”Journal of Intelligent Robotic Systems, vol. 106, 2022, article 43. DOI: 10.1007/s10846-022-01743-9.[8] Ambroziak, Leszek, and Maciej Ciężkowski. “Virtual Electric Dipole Field Applied to Autonomous Formation Flight Control of Unmanned Aerial Vehicles.”Sensors, vol. 21, no. 13, 2021, article 4540. DOI: 10.3390/s21134540.[9] Sun, Yuan. “Autonomous Integrity Monitoring for Relative Navigation of Multiple Unmanned Aerial Vehicles.”Remote Sensing, vol. 13, no. 8, 2021, article 1483. DOI: 10.3390/rs13081483.[10] Karras, George C., Charalampos P. Bechlioulis, George K. Fourlas, and Kostas J. Kyriakopoulos. “A Mixed-Initiative Formation Control Strategy for Multiple Quadrotors.”Robotics, vol. 10, no. 4, 2021, article 116. DOI: 10.3390/robotics10040116.2、仿真结果演示3、关键代码展示略4、MATLAB 源码获取WXGZH搜索Matlab程序猿获取