3. 航迹规划数学基础

多无人机协同航迹规划本质上是一个带约束的高维优化问题：既要为每架无人机生成满足动力学与安全约束的飞行轨迹，又要让整个机群在时间、空间上协调一致。理解其数学基础，才能判断一个规划问题“难在哪里”、该选什么求解器、为什么某些算法会陷入局部最优。本节围绕五块基础展开：图论、凸与非凸优化、最优控制、概率与随机方法、多目标优化。

一、图论基础：把空域抽象成可搜索的网络

图论提供的是“离散化”工具。把连续空域离散为节点集合 V 与边集合 E，航迹规划就转化为在图上寻找满足约束的路径。常用表示有邻接矩阵与邻接表；对稀疏空域（如城市楼宇间），邻接表存储效率更高。边权可定义为距离、能耗、暴露风险或它们的加权和，例如 w(e)=α·d(e)+β·risk(e)+γ·1/alt(e)。

搜索算法中，Dijkstra 适用于非负权单源最短路；A* 引入启发式 h(n)（如到目标的欧氏距离），在 h 满足可采纳性（不高估真实代价）时保证最优。对多机协同，常用“时空图”：节点为 (位置, 时间) 二元组，边表示在 Δt 内从一位置移动到另一位置，从而把“避碰”编码为“不同无人机不得占用同一时空节点”。这是后续协同规划的关键抽象。

二、凸优化与非凸优化：决定求解难度与全局性

若目标函数与可行域均为凸，则问题是凸优化：任何局部最优即全局最优，可用内点法、梯度投影等高效求解，且解具有可预测性。航迹规划中，若把障碍建模为凸多面体、把动力学线性化、把代价设为二次型，就能得到二次规划（QP）或二阶锥规划（SOCP）形式，例如最小化 ∑‖p_k+1−p_k‖² 并满足 A p_k ≤ b 的走廊约束。

但真实问题往往非凸：避障约束“无人机必须在障碍外”是可行域的非凸补集；固定翼的转弯半径、速度与航向耦合也是非凸的。非凸问题存在多个局部极小，梯度类方法结果依赖初值。工程上常用三类策略：一是凸化/序列凸优化（SCP），在当前轨迹附近线性化非凸约束，迭代求解凸子问题；二是把非凸区域分解为若干凸子区域（如走廊分解），在凸块内求解再拼接；三是用全局搜索（如 RRT*、遗传算法）先给出较好初值，再用局部优化精修。

三、最优控制初步：连续时间下的轨迹生成

最优控制把航迹规划写成：给定系统 ẋ=f(x,u,t)，寻找控制 u(t) 使代价 J=Φ(x(t_f))+∫L(x,u,t)dt 最小，并满足边界与路径约束。核心结论来自 Pontryagin 极小值原理（必要条件）与 Hamilton-Jacobi-Bellman 方程（充分条件）。对线性系统与二次代价，LQR 给出解析反馈 u=−Kx；对带约束、带预测的在线规划，模型预测控制（MPC）在每个采样时刻求解有限时域优化，只执行第一步控制，再滚动更新，天然适合多机协同中的避碰与编队保持。

实际实现中，常把连续最优控制离散为非线性规划（NLP）：以 N 个离散点的状态与控制为决策变量，用配点法或打靶法把动力学转为等式约束，再交给 IPOPT、SNOPT 等求解器。例如取 N=30、Δt=0.5 s，则单机决策变量约 30×(3+3)=180 维，三机协同约 540 维，规模可控。

四、概率与随机方法：应对不确定性

真实飞行中风场、GPS 漂移、通信延迟都带随机性。概率方法把状态视为随机变量，用概率密度描述不确定性传播。常用工具包括：扩展卡尔曼滤波（EKF）与粒子滤波做状态估计；机会约束 P(g(x)≤0)≥1−δ 把“必须避障”松弛为“以高概率避障”；蒙特卡洛抽样评估轨迹的碰撞概率与到达时间分布。

采样规划方面，RRT 通过在空间中随机采样并连接无碰撞节点快速探索，RRT* 通过重连与选择父节点渐进最优。其优点是无需梯度、适合高维与非凸环境；缺点是解的质量依赖采样密度，工程上常与局部优化结合：RRT* 给初值，SCP 精修。

五、多目标优化基本理论：协同中的权衡

多无人机协同通常同时关心飞行时间、能耗、被探测风险、机间通信保持与到达时间一致性。这些目标往往冲突，不存在单一最优解，而是 Pareto 最优解集：在不劣化任一目标的前提下无法再改进其他目标。求解思路有三类：加权和法把多目标转为单目标，简单但难以覆盖非凸前沿；ε-约束法固定部分目标上界、优化其余目标，能获得分布更均匀的前沿；进化算法（如 NSGA-II）通过非支配排序与拥挤度距离直接逼近 Pareto 前沿。

工程上还需把“协同”量化为目标或约束：例如要求各机到达时间差 |t_i−t_j|≤ε_t，或把机间距离下界 d_min 作为硬约束。下表给出一个三机协同示例的参数设定（示例数据，用于说明建模方式，非实测结果）：

这五块基础并非孤立：图论负责离散化与搜索，凸/非凸分析决定求解器选择，最优控制提供连续轨迹与反馈律，概率方法处理不确定性，多目标理论完成协同权衡。把它们串起来，才能从“能飞”走向“飞得优、飞得稳、飞得协同”。