【群体智能集群控制工程实践】第7章 无人机蜂群核心功能实现

第 7 章　无人机蜂群核心功能实现

所属卷册：第三卷　无人机蜂群控制工程实践

第 6 章搭好了蜂群系统的骨架（平台、链路、软件分层），本章把第二卷的算法"填充"进骨架，实现蜂群的三类核心功能：编队飞行（7.1）、协同避障（7.2）与任务执行（7.3）。三类功能对应算法层级的递进：编队飞行是"保持几何构型运动"的底层能力，协同避障是"在动态环境中不撞"的安全能力，任务执行是"针对目标对象工作"的作战/作业能力。本章的每一小节都遵循统一模式：算法选择（引用前文结论）→ 工程实现要点（参数、流程、失效处理）→ 验收指标。需要预先说明的是，本章大量内容是"工程正确性"问题——同样的算法，参数与流程不当会在真机上呈现完全不同的结果，因此各节都给出了可执行的实现与测试步骤。蜂群功能的真实系统验证可对标已有开源与论文报道的实飞蜂群（如室外受限空间数百架次规模的优化蜂拥实飞[1]）的工程结论。

7.1　集群编队飞行控制

7.1.1　二维/三维编队生成

编队生成指集群从"起飞散布状态"收敛到"期望几何构型"的过程，分为两步：初始队形形成（从散乱到成形）与编队保持（成形后随队运动）。工程上队形生成采用"虚拟领航者 + 偏移一致/势场"的组合：虚拟领航者提供队形参考（第 4.1.2.2 节偏移一致式(4.7)的工程实例），势场保证收敛过程不碰撞。

7.1.1.1　基于虚拟领航者的队形初始化方法

队形初始化输入为队形定义表（成员 ID → 队形位姿 $({\mathbf{r}}_i^{\mathrm{init}},{\psi }_i)$），输出为各机"开始跟踪编队状态"的判据。虚拟领航者方案在每机本地生成队形参考：

$${\mathbf{p}}_i^d(t)={\mathbf{p}}_v(t)+\mathbf{R}({\psi }_v){\mathbf{r}}_i,{\mathbf{e}}_i(t)={\mathbf{p}}_i(t)-{\mathbf{p}}_i^d(t)\text{\hspace{1em}(7.1)}$$

其中 $({\mathbf{p}}_v,{\psi }_v)$ 为虚拟领航者位姿（由一致性算法在机间共识，或由长机本地发布——注意第 6.2 章架构中它应在协调层作为一条"队形参考主题"存在），${\mathbf{r}}_i$ 为该机在队形中的相对位置（本体系）。初始化流程的工程要点：

虚拟领航者相对于物理长机的优势在于无单点故障与支持任意队形几何，其队形收敛的一致性与稳定性结论见文献[2]；第 4 章的偏移一致(4.7)与领航-跟随(4.3)分别对应"无显式领航"与"有物理领航"两种编队形态，本节方案可退化为任一形态。

7.1.1.2　编队保持的 PID 参数整定流程

编队保持执行层采用位置环 PID（在 PX4/ArduPilot 中即位置控制器，或以第 6.1.1.1 节 MAVLink 位置指令注入）。PID 整定必须从动力学等效参数出发而非盲目试凑：位置环近似为二阶系统，位置-速度通道增益与自然频率、阻尼比满足

$$k_p={\omega }_n^2,k_d=2\zeta {\omega }_n\text{\hspace{1em}(7.2)}$$

（对双通道 PID 的 P、D 通道；I 通道负责静态偏差消除，其时间常数取 $T_i\approx 5\sim 10/{\omega }_n$）。整定流程分五步：

工程上三个高频坑：其一，"PID 增益大=响应快"的误区——位置环增益受内环带宽与执行器饱和共同约束（式(3.11)），放大 $k_p$ 超过饱和预算只会让飞机在饱和-退饱和间振荡；其二，I 通道在编队场景要弱——编队误差主要由 D/PD 通道收敛，I 通道过大在队形切换时会造成积分超调（对应第 3.2.1.1 节 windup 的编队版），必要时切换期间冻结积分；其三，定位噪声直接决定可实现的精度——位置环 $k_d$ 放大速度噪声，RTK 与视觉定位的噪声差异会让"同一套 PID"表现迥异，验收必须带真实定位源。

图 7-1 建议插入位：单机阶跃响应曲线族（$\zeta =0.5/0.8/1.2$）与"编队切换后误差包络回稳"示意图。

7.1.2　动态队形变换

7.1.2.1　队形切换的平滑过渡轨迹规划

队形变换（如侦察纵队 → 搜索横队）工程上按第 4.1.2.2 节的参数化过渡框架实现：队形参数 $\mathbf{\lambda }$ 从 0（旧队形）渐变到 1（新队形），每机期望位置 $p_i^d(\mathbf{\lambda }(t))$ 沿过渡曲线滑动。规划对象是 $\mathbf{\lambda }(t)$，约束为：平滑性（$\mathbf{\lambda }$ 二阶连续，切换中无加速度跳变——与第 4.1.1.1 节多项式轨迹同构，可对 $\mathbf{\lambda }$ 做最小急动度参数化）与时间一致性（全网同步的过渡时钟，或由过渡指令的时间戳+持续时间广播实现，各机按同一 $\mathbf{\lambda }$ 时刻表执行，否则过渡构型在机间不同步）。过渡时间的选取由"期望速度上限 $v_{\mathrm{trans}}$ 与路径最长成员位移"决定：$T_{\mathrm{trans}}={\max }_i\Vert {\mathbf{r}}_i^{\mathrm{new}}-{\mathbf{r}}_i^{\mathrm{old}}\Vert /v_{\mathrm{trans}}$，过渡时长取整拍以对齐控制周期。

图 7-2 建议插入位：队形变换的参数化过渡示意——中间构型序列与成员轨迹，标注最长位移成员与过渡时钟。

7.1.2.2　变换过程中的防碰撞约束处理

队形变换是蜂群碰撞风险最高的阶段：成员互相穿越，期望轨迹可能相交。防碰撞约束处理的工程流程是"先验证、后执行、再兜底"：

防碰撞性能的验收以蒙特卡洛扰动注入为准：在标称过渡上叠加定位噪声与风扰各跑 $M$ 次，统计最小间距分布，要求"最小间距的 3σ 下界仍大于硬性安全间距"——这是蜂群安全性验收的一般方法论（与第 8.1 节测试框架共用）。

7.2　集群协同避障

7.2.1　局部感知避障

7.2.1.1　激光雷达/视觉传感器的数据融合方法

避障感知的典型配置为"激光雷达（或深度相机）+ 视觉相机"互补：激光/深度在暗光与近距离可靠、提供稠密距离，视觉提供纹理与远距离检测。融合的工程目标不是"更漂亮的点云"，而是带不确定度的障碍物状态估计（位置、速度、几何、置信度），其方法分层为：

• 数据级/特征级融合（点云对齐、目标框关联）：多传感器目标关联用最近邻/匈牙利匹配（时间戳对齐后），关联失败的目标进入暂存区等待再次确认——单帧误检不应产生避障动作（避障误动作的代价是偏航与掉队）；
• 决策级融合：多源障碍候选按"类型-距离-置信度"表决产生避障输入。异构传感器的分歧处理原则是保守取近：距离估计以最近源为准（宁可早避不晚避），速度估计以最可信源为准（激光多普勒优于视觉光流）；
• 状态层融合：多源量测进入统一的障碍物跟踪器（卡尔曼/扩展卡尔曼，见 7.2.1.2），输出时间一致的障碍状态。多传感器融合的一般方法学综述见文献[3]。

工程实现的两个关键环节：标定（相机-激光外参、时延补偿——毫米级外参误差在 50 m 处产生米级投影误差，且机载振动使外参漂移，需在线校准或定期标定）与退化场景管理（雨雾中激光点云稀疏、逆光下视觉失效，融合器必须显式管理"单传感器退化"状态——降级为剩余可用源并降低避障速度上限，而非继续等额融合）。

7.2.1.2　动态障碍物的轨迹预测模型

避障决策需要障碍物的未来轨迹。工程上的标准预测模型按复杂度递进：恒速（CV）模型（障碍运动平滑时最优性价比）、恒定转率（CT）模型（机动目标，转弯率经滤波估计）与意图模型（配合通信的智能体，用其广播意图替代外推——见 5.2.1.2）。CV 模型的离散形式为

$${\mathbf{x}}_o(k+1)=\mathbf{F}{\mathbf{x}}_o(k)+\mathbf{w},\mathbf{F}=\left[\begin{array}{cc}{\mathbf{I}}_2& \Delta t{\mathbf{I}}_2\\ 0_2& {\mathbf{I}}_2\end{array}\right]\text{\hspace{1em}(7.3)}$$

其中状态为障碍物（位置, 速度），$\mathbf{w}$ 为过程噪声（模型不确定度），配合量测更新即标准卡尔曼滤波（KF/EKF），其预测协方差随时间增长：$\mathbf{P}(t)={\mathbf{P}}_0+\mathbf{Q}t$——预测越远越不确定，这正是第 5.2.1.1 节速度障碍"扩边"的量化依据（预测时域越长、扩边越大）。轨迹预测的建模与滤波方法细节见跟踪领域的标准教材[4]。

工程实现的三条经验：其一，预测时域与避障机动能力匹配——预测只用于"决策是否避让、往哪个方向避让"，不用于长时外推；时域取"本机以最大减速度停下所需时间+反应余量"（与第 5.2.1.2 节一致）；其二，噪声参数现场标定——过程噪声 $\mathbf{Q}$ 决定预测可信度（Q 太小则预测"过度自信"，撞上机动目标；太大则扩边过大、机动频繁），标准做法是用记录的真实障碍轨迹做 Q 离线辨识；其三，预测不确定度要传递到下游——避障规划器使用"协方差扩边后的障碍圆盘"而非点障碍，保证"不知道目标往哪拐"时系统自动选择保守机动。

7.2.2　集群协同避障

7.2.2.1　基于一致性协议的避障方向协商

单机避障解决"我不撞障碍物"，集群避障还要解决"我不撞队友且队友也这么想"。若各机独立避障，可能出现"让行方向冲突"（两机同时让向同一侧）与"避障-保队矛盾"（避障机动撕开队形后无法重组）。协同避障的工程做法是在避障机动生成后加一层方向协商：各机把本地避障建议方向 ${\theta }_i^{\mathrm{avoid}}$（由势场/VO 计算的期望偏航）在邻居间交换，用一致性协议对"最终偏航方向"达成一致：

$${\theta }_i^{\mathrm{cmd}}={\theta }_i^{\mathrm{avoid}}-\sum _{j\in {\mathcal{N}}_i}{a}_{ij}{\sigma }_{\delta }({\theta }_i^{\mathrm{avoid}}-{\theta }_j^{\mathrm{avoid}})\text{\hspace{1em}(7.4)}$$

式(7.4)把"本地建议"与"邻居建议的分歧"合并——方向分歧大的成员互相修正，方向建议接近的成员协同保持；${\sigma }_{\delta }$ 限幅保证修正量有界（对应式(3.9)的相对饱和思想）。大规模实飞验证表明，这类"本地建议 + 邻居协商 + 硬核避碰"的层次结构能在受限空间中维持数百架量级无人机群的自主飞行[1]，是经过真机检验的框架。工程实现注意：协商消息必须带意图有效期（协商结果只在 $\Delta T$ 内有效，超时回到独立避障——防止失联成员拖住协商）、以及避障优先级高于保队（方向协商只在"无直接碰撞危险"时运行，危险距离内直接执行本地避让，协商结果留待脱险后重组）。

7.2.2.2　狭窄通道场景的通行优先级分配

狭窄通道（门廊、树冠间隙、楼宇夹缝）是协同避障的极限场景：物理上不容许两机并行，必须排队/分时通过。通行优先级分配机制为：进入通道前，候选成员按"任务紧急度-距通道口距离-队形角色"评分排序，广播"通行意向 + 预计占用窗口 $[t_{\mathrm{enter}},t_{\mathrm{exit}}]$"；冲突窗口重叠的两机按优先级让行——低优先级机在通道口外悬停等待（保持与通道口的安全距离），高优先级机通过后再进入。窗口占用冲突检测的判据为通道入口侧两机距离同时小于"排队距离"且期望通过时间窗口相交。

工程要点：其一，通道口容量感知——进入通道前先由前哨机（或感知最远机）广播"通道净宽/净高测量值"，低于本机安全包络则整队改道（窄通道判据来自 1.2.1 节感知半径与安全间距的联合预算），避免队列走到通道口才发现过不去；其二，等待位置避让队形——等待机群在通道口外展开为"疏散待命队形"（成员间距拉大），避免在狭窄入口处扎堆互相推挤；其三，通过失败回退——窗口内无法完成通过（通道被占、感知异常）时退出队列并重新申请，防止死锁式反复尝试；其四，优先级避免永久饥饿——低优先级机等待超时后提升优先级（老化机制），保证任务公平。

7.3　集群任务执行

7.3.1　协同搜索任务

7.3.1.1　区域覆盖的网格划分方法

协同搜索的首要问题是把大搜索区域划分为各机的工作子区。工程上按先验信息分层处理：先验无信息（盲搜）→ 等面积/等能力划分；先验有概率图（目标概率分布已知）→ 概率加权划分。面积划分的基线方法是条带划分与 Voronoi 划分：

• 条带划分（扫描线搜索的天然配套）：把区域沿扫描方向分成 $N$ 条等宽条带，每机扫一条。宽度由"单机续航-扫描速度"决定：${\ell }_i=L$（全长），$w_i=W/N$；负载均衡由"总扫描时间相等"约束 $w_i\cdot L/v_i=T$ 推广为 $w_i\propto v_i$（异构速度平台按速度分配带宽，对应第 5 章异构能力匹配的思想）；
• Voronoi 划分（加权覆盖/区域监控）：以各机位置为中心划分区域，每点归属最近机，配合 Lloyd 迭代使划分随平台移动自平衡：

$$V_i=\left\{\mathbf{q}\in \mathcal{A}:\Vert \mathbf{q}-{\mathbf{p}}_i\Vert \le \Vert \mathbf{q}-{\mathbf{p}}_j\Vert ,\forall j\right\}\text{\hspace{1em}(7.5)}$$

Voronoi 划分的分布式可计算性与 Lloyd 更新的收敛性使其成为覆盖控制的数学骨干[5]；工程上把"欧氏距离"替换为"加权距离 $\Vert \mathbf{q}-{\mathbf{p}}_i\Vert /v_i$"（速度快的平台多分）即可扩展为异构版本。两种方法的选取原则：扫描型任务（搜救、测绘）用条带，持续监控型任务（巡逻、监视）用 Voronoi+Lloyd；网格粒度由传感器覆盖宽度决定（每格不大于机载传感器幅宽的一半，保证扫描无缝隙）。

图 7-3 建议插入位：条带划分与 Voronoi 划分对比示意（含异构速度加权的 Voronoi），标注覆盖率定义。

7.3.1.2　搜索效率的评估指标与优化

搜索效率必须被量化，否则无法优化。标准的双指标体系为：

• 覆盖率（空间维度）：$C(t)=\left|{\mathcal{A}}_{\mathrm{covered}}(t)\right|/\left|\mathcal{A}\right|$——已覆盖面积占总面积比例，覆盖判定用"最后访问时间"与"最大重访间隔"双重标准（监测型任务重访间隔比覆盖率更重要：某格 10 分钟前扫过但目标此时出现，等于没覆盖）；
• 发现概率（目标维度）：对目标概率图，搜索效率定义为发现概率的期望增长 $\mathbb{E}[\Delta P_{\det }]$——概率型搜索中每次扫描把格内目标存在概率按"检测概率 × 目标存活折扣"更新（贝叶斯/指数衰减更新，对应 7.2.1.2 预测不确定度与搜索模型的结合）。

优化手段按回报排序：路径层（扫描路径的转角最小化——转角损失速度与电池，锯齿扫描的转弯衔接是最大的隐性浪费）、划分层（按概率图加权划分——高概率区先搜、多机重叠搜，对应式(7.5)加权版）、动态层（搜索中目标概率图更新后重划区域边界——概率图更新频率与划分重算频率要解耦，重划周期取分钟级避免震荡）。搜索效率的验收对标"理论基线"：同区域内"单机矩形扫描"的时间是天然基线，多机协同的加速比、覆盖率曲线与概率图下降速率都应相对基线报告——任何"协同搜索"如果加速比低于 1.5 且覆盖率曲线无优势，应先怀疑划分与路径实现而非算法本身。

7.3.2　协同目标跟踪

7.3.2.1　多机观测数据的融合算法

协同跟踪中，多架无人机从不同视角观测同一目标，其量测必须在机间融合以获得比单机更稳的目标状态。融合的正确工程做法是先本地滤波、再分布式融合估计：每机本地卡尔曼滤波维护目标状态估计 $({\hat{\mathbf{x}}}_i,{\mathbf{P}}_i)$，周期性地与邻居交换估计并融合。直接对相关估计做简单加权平均会"双计"共同信息（两机共享了同一次量测，融合后协方差虚假偏小，即一致性缺失），工程上采用保守融合：加权指数形式的融合

$${\mathbf{P}}^{-1}=\sum _i{\omega }_i{\mathbf{P}}_i^{-1},\hat{\mathbf{x}}=\mathbf{P}\sum _i{\omega }_i{\mathbf{P}}_i^{-1}{\hat{\mathbf{x}}}_i,\sum _i{\omega }_i\le 1\text{\hspace{1em}(7.6)}$$

式(7.6)在通信图上的迭代实现即分布式一致性估计（对估计/协方差做一致平均），其"去相关"由 ${\omega }_i$ 的总和约束保证——权重的选取与网络拓扑相关，理论上更严格的分布估计框架（含稳定性保证与通信策略）见文献[6]。工程实现要点：其一，融合消息与本地估计解耦——本地滤波以传感器帧率运行，融合消息以低得多的频率广播（10 Hz 量级），避免"融合频率 = 滤波频率"造成的通信负担；其二，航迹标识管理——多机关联同一目标靠全局唯一的航迹 ID（视觉特征、位置门限），ID 分裂/合并事件要有裁决机制（对应 7.3.2.2 的跟踪丢失检测）；其三，融合估计参与编队控制要加平滑——融合估计噪声特性与本地估计不同，直接接入 7.1 节的位置控制会引起指令抖动，需在融合估计与控制指令间加低通。

7.3.2.2　跟踪丢失后的重定位策略

目标跟踪必然面临丢失：目标被遮挡（进入树冠/建筑后）、目标超出视场、目标检测器失效。重定位策略的价值在于把"丢失→重搜"的时间损失压到最小，其工程流程为：

重定位的验收指标是丢失-恢复时间分布与重搜误报率：恢复时间应远小于任务对连续跟踪的时间要求（如目标识别任务的容错丢失时间窗）；重搜误报（找回错误目标）比丢失更危险，找回必须经过"位置连续性 + 外观特征"双重确认（对应 7.2.1.1 的关联与暂存机制）。

本章小结

本章把前两卷算法落到无人机蜂群的三大功能：编队飞行（虚拟领航初始化式(7.1)、PID 整定式(7.2)、防碰撞队形变换）、协同避障（传感器融合、轨迹预测式(7.3)、方向协商式(7.4)、窄通道优先级调度）与任务执行（区域划分式(7.5)、保守融合式(7.6)、搜索/跟踪指标化验收）。贯穿全章的方法论是指标化验收与安全兜底：每个功能都以可测量的指标（误差包络、最小间距分布、覆盖率、恢复时间）验收，每个危险过程（队形变换、通道通过、跟踪丢失）都有"先验证、再执行、最后兜底"的三道防线。第 8 章将给出验证这些功能的完整测试与部署流程。

思考与练习

参考文献

[1] Vásárhelyi G, Virágh C, Somorjai G, et al. Optimized flocking of autonomous drones in confined environments[J]. Science Robotics, 2018, 3(20): eaat3536.
[2] Ren W. Consensus strategies for cooperative control of vehicle formations[J]. IET Control Theory & Applications, 2007, 1(2): 505–512.
[3] Khaleghi B, Khamis A, Karray F O, et al. Multisensor data fusion: a review of the state of the art[J]. Information Fusion, 2013, 14(1): 28–44.
[4] Bar-Shalom Y, Li X R, Kirubarajan T. Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software[M]. New York: Wiley, 2001.
[5] Cortés J, Martínez S, Karataş T, et al. Coverage control for mobile sensing networks[J]. IEEE Transactions on Automatic Control, 2004, 49(9): 1342–1353.
[6] Battistelli G, Chisci L. Kullback-Leibler average, consensus on probability densities, and distributed state estimation with guaranteed stability[J]. Automatica, 2014, 50(3): 707–718.