有关规划算法历史与两篇 IEEE 论文。

规划算法发展历史

第一阶段：早期探索与经典图搜索（1990年代前）

这一阶段的算法主要为静态、已知环境中的路径规划服务，核心思想是将连续空间离散化，用图搜索寻找最优路径。

  • 基于图搜索的方法：以 Dijkstra 和 A* 算法为代表。
  • 基于几何的方法：Voronoi 图被用于构建远离障碍物的路径，Dubins 曲线则解决了固定翼无人机的最小转弯半径约束下的路径生成问题。
  • 最优控制理论：可追溯到1950年代，最初应用于固定翼飞机和航天器的轨道优化。早期方法如直接配点法，将轨迹优化问题转化为非线性规划问题求解。

这一阶段的算法环境假设理想，但为后续研究奠定了数学与几何基础。

第二阶段：采样式与智能优化算法兴起（1990年代末 - 2010年）

随着环境复杂度提升，基于采样的方法和仿生智能算法成为主流，以应对高维空间和复杂约束。

  • 基于采样的规划：概率路图法（PRM, 1996）和快速扩展随机树（RRT, 1998）是里程碑式的工作。PRM通过构建路网图实现多次查询，RRT则通过随机树生长快速找到可行路径。后续的 RRT* 和 PRM* 保证了算法的渐近最优性。
  • 智能优化算法：受自然现象启发，蚁群算法（ACO, 1996）、粒子群优化（PSO, 1995）、遗传算法（GA）等被广泛用于航迹规划。这些算法通过模拟群体智能或进化过程，在复杂约束下寻找近似最优解。
  • 人工势场法（APF）：将目标点设为引力源、障碍物设为斥力源，引导无人机运动。该方法计算简单，但易陷入局部极小值。

这一时期，算法开始考虑更复杂的约束，但动态环境和实时性仍是巨大挑战。

第三阶段：基于优化的轨迹规划成熟（2010年代 - 2020年代）

研究者意识到，仅靠路径搜索无法满足无人机对动力学可行性和平滑性的要求，轨迹优化成为核心。规划系统普遍采用“前端路径搜索 + 后端轨迹优化”的架构。

  • 前端路径搜索：RRT*、A*等算法负责在复杂环境中快速找到初始可行路径。
  • 后端轨迹优化：对前端路径进行平滑和动力学约束优化。代表性工作包括：
    • 最小化Snap轨迹生成：通过最小化位置的四阶导数（Snap），生成光滑、动力学可行的多项式轨迹。
    • 飞行走廊与凸优化：将无碰撞空间分解为一系列凸多面体（飞行走廊），在走廊内进行轨迹优化，将问题转化为凸优化问题求解。
    • MINCO轨迹类：由浙江大学高飞团队提出，用路点和时间参数化轨迹，将高维优化问题降维，实现毫秒级实时重规划。GCOPTER 和 EGO-Planner 等知名规划器均基于此框架。
    • 模型预测控制（MPC）：将轨迹跟踪与避障结合，在每个控制周期求解有限时域内的最优控制序列，对模型不确定性有一定鲁棒性。

这一阶段，算法在静态复杂环境中已相当成熟，GCOPTER 等框架能实现高速、大场景下的自主飞行。

第四阶段：面向动态环境与学习驱动的规划（2020年代至今）

当前研究前沿聚焦于动态、不确定环境，核心挑战是非合作式动态障碍物的运动预测与避障。

  • 动态环境下的时空联合规划：
    • 反应式规划：速度障碍法（VO）、动态窗口法（DWA）等，仅根据当前障碍物信息避让，缺乏预见性，轨迹易振荡。
    • 基于概率优化的规划：将障碍物位置建模为概率分布，通过机会约束或集合有界方法处理不确定性。
    • 时空联合优化：将时间维度纳入规划，预测动态障碍物未来轨迹，进行状态-时间空间的联合优化。代表性方法包括将动态障碍物未来轨迹构建为“时空胶囊”进行碰撞检测，或使用 MADER 等异步规划器。
  • 深度强化学习（DRL）的引入：
    • DRL通过与环境交互学习策略，无需精确模型，为动态避障提供了新思路。DQN、DDPG、TD3、SAC 等算法被用于无人机路径规划。优势在于能处理高维感知输入和复杂动态环境，但面临样本效率低、训练不稳定、安全验证难等挑战。

未来的核心挑战在于，如何在动态、非合作、高不确定性的环境中，实现安全、高效且可验证的实时自主规划。

Reinforcement Learning-Based Optimal Formation Tracking for UAVs With Safety Constraints

研究原因

传统 UAV 编队控制已经能解决“跟得上”，但不一定解决“安全 + 最优”。

本文针对固定翼多无人机，设计了一个能兼顾编队跟踪、避碰、输入约束非对称和外部扰动的控制方案，再用强化学习近似求解最优控制策略。

本文的 RL 采用 Critic-only Reinforcement Learning。只训练 critic，利用 critic 的梯度计算 actor/control policy。

固定翼 UAV 动力学模型

每架 UAV 的位置：

$$ \eta_i=[x_i,y_i,z_i]^\mathsf{T} $$

满足：

$$ \dot x_i=v_i\cos\theta_i\cos\psi_i+d_{xi} $$

$$ \dot y_i=v_i\cos\theta_i\sin\psi_i+d_{yi} $$

$$ \dot z_i=v_i\sin\theta_i+d_{zi}. $$

其中：

  • $v_i$：速度；
  • $\psi_i$：航向角；
  • $\theta_i$：俯仰角；
  • $d_{xi},d_{yi},d_{zi}$：外部扰动。

另外 autopilot 内部动态：

$$ \dot v_i=(\rho_v+\Delta\rho_v)(v_i^c-v_i) $$

$$ \dot\psi_i=(\rho_\psi+\Delta\rho_\psi)(\psi_i^c-\psi_i) $$

$$ \dot\theta_i=(\rho_\theta+\Delta\rho_\theta)(\theta_i^c-\theta_i) $$

这里的

$$ \Delta\rho_v,\Delta\rho_\psi,\Delta\rho_\theta $$

表示 autopilot 参数的不确定性。

$\rho$ 的物理意义可以理解为 UAV autopilot 的响应速率/闭环带宽参数。它决定了实际飞行状态 $v_i,\psi_i,\theta_i$ 跟随上层给出的指令 $v_i^c,\psi_i^c,\theta_i^c$ 有多快。

定义编队跟踪误差

本文采用领导-跟随模型。作者引入虚拟 leader：

$$ \eta_r=[x_r,y_r,z_r]^\mathsf{T} $$

它沿着预先产生的参考轨迹运动。

第 $i$ 架 UAV 希望相对于 leader 保持：

$$ \eta_{ir} $$

这样定义编队跟踪误差：

$$ e_i=\eta_i-\eta_{ir}-\eta_r $$

同时定义相对飞行状态：

$$ \Delta\zeta_i=\zeta_i-\zeta_r $$

其中：

$$ \zeta_i=[v_i,\psi_i,\theta_i]^\mathsf{T}. $$

所以最终状态：

$$ \boxed{ X_i= \begin{bmatrix} e_i\\ \Delta\zeta_i \end{bmatrix} \in\mathbb R^6 } $$

控制：

$$ \boxed{ u_i= [v_i^c,\psi_i^c,\theta_i^c]^\mathsf{T} \in\mathbb R^3. } $$

位置误差 $e_i=\eta_i-\eta_{ir}-\eta_r$：无人机相对期望编队位置的跟踪误差。

飞行状态误差 $\Delta\zeta_i=\zeta_i-\zeta_r$：速度、航向角和俯仰角相对虚拟领航机的误差。

把它们合成增广状态 $X_i=[e_i^\mathsf{T},\Delta\zeta_i^\mathsf{T}]^\mathsf{T}$，写成

$$ \dot X_i=F_i(X_i)+G_i(X_i)u_i+d_i . $$

原本耦合的编队跟踪任务被整理为每架无人机的误差系统，再针对这个系统设计控制器。

跟踪误差的安全约束

作者规定：

$$ -k_i^{o}(t)<e_i^o<k_i^o(t), \qquad o=x,y,z. $$

即：

$$ |e_i^x|<k_i^x,\qquad |e_i^y|<k_i^y,\qquad |e_i^z|<k_i^z. $$

实际实现时，作者更倾向于使用实时距离：

$$ k_i \le \frac12 \left( ||\eta_i-\eta_j||-d_{\min} \right). $$

并采用保守形式：

$$ k_i< \frac12 \sqrt{ (x_i-x_j)^2+ (y_i-y_j)^2+ (z_i-z_j)^2 }. $$

这种设计让安全边界能够随相邻 UAV 的距离变化。

设计控制边界函数（CBF）

如果每架机都不能偏离自己的编队位置太远，邻机之间就能保留最小安全距离。

定义安全集合：

$$ S_i= \{X_i:h_i(X_i)\ge0\}. $$

针对上述安全约束设置新的边界函数：

$$ \boxed{B_i^c(X_i)= \sum_{o\in \{x,y,z\}} \frac{(k_i^oe_i^o)^2} {(k_i^o+e_i^o)(k_i^o-e_i^o)}} $$

分母实际上就是 $(k_i^o)^2-(e_i^o)^2$。

有三个关键性质：

$$ B_i^c(0)=0 $$

$$ \inf_{X_i\in Int(S_i)}B_i^c(X_i)>0 $$

$$ X_i\rightarrow\partial S_i \quad\Rightarrow\quad B_i^c(X_i)\rightarrow\infty $$ CBF 在误差为零时为零，误差接近允许边界时迅速增大。在理想最优策略下状态不会逃出安全集合。

加入“安全-性能”权衡参数 $\kappa_i$

论文增加：

$$ \kappa_i B_i^c(X_i) $$

把 CBF 加到代价函数中：

$$ \boxed{ L_i^c= L_{i1} +\kappa_iB_i^c -\frac{\delta^2}{2}d_i^\mathsf{T}d_i } $$

其中：

$$ L_{i1}= X_i^\mathsf{T}R_iX_i+U_i(u_i). $$

  • $\kappa_i$ 大：更加重视安全，动作更保守；
  • $\kappa_i$ 小：更加偏向最优跟踪。

$\kappa_i$ 是一个经验调节参数。

把扰动建模成对手

形成零和微分博弈

$$ \boxed{ \min_{u_i}\max_{d_i} \int_0^\infty L_i^c(X_i,u_i,d_i),dt } $$

其中：

  • UAV controller = 最小化博弈方
  • disturbance = 最大化博弈方

$\min{u_i}$为努力降低成本，$\max{d_i}$为最恶劣情况。

这样可以得到 robust Nash strategy。

作者认为这比把扰动当作“被动不确定性”更适合描述 UAV 的最恶劣情况环境。

建立 HJI 方程

定义安全最优代价函数：

$$ V_{ic}^*(X_i)= \min_{u_i}\max_{d_i} \int_0^\infty L_i^c(X_i,u_i,d_i),dt. $$

价值函数 $V_i^*(X_i)$ 表示从当前误差状态出发，未来长期的“代价”有多大。代价越大，说明当前状态或后续行为越不理想。

论文的运行代价包含四部分： $$ L_{ic}= X_i^\mathsf{T}R_iX_i +U_i(u_i) +\kappa_i B_{ci}(X_i) -\delta_i^2 d_i^\mathsf{T}d_i . $$

  • $X_i^\mathsf{T}R_iX_i$：惩罚编队位置和飞行状态误差；
  • $U_i(u_i)$：惩罚控制输入，同时照顾输入上下界；
  • $\kappa_iB_{ci}(X_i)$：靠近安全边界时增加惩罚；
  • $-\delta_i^2d_i^\mathsf{T}d_i$：在零和博弈中，扰动被视作试图增大代价的对手。 把未来总代价写成价值函数，并用动态规划的最优性条件，就得到 HJI 方程：

$$ \boxed{ 0= \min_{u_i}\max_{d_i}H_i } $$

上式即为 Hanmilton-Jacobi-Isaacs equation。

对应哈密顿量 $H_i$：

$$ H_i= L_i^c + \nabla V_{ic}^* (F_i+G_iu_i+d_i). $$

当前代价 + 价值函数沿系统运动方向的变化率 = 0。由于 $V_i^*$ 未知、方程非线性，很难直接解出来。后面的 critic 就是为了近似它。

对控制量求驻点得到最优控制律：

$$ \frac{\partial H_i}{\partial u_i}=0 $$

得到：

$$ \boxed{ u_i^*= \bar\beta_i-\alpha_i \tanh \left( \frac{1}{2}\alpha_i^{-1} G_i^\mathsf{T}\nabla V_{ic}^{*\mathsf{T}} \right) } $$

$\tanh$ 的关键作用是把结果限制在输入范围内。

对扰动量求驻点得到最坏扰动律：

$$ \frac{\partial H_i}{\partial d_i}=0 $$

得到：

$$ \boxed{ d_i^*= \frac{1}{2\delta_i^2} \nabla V_{ic}^{*\mathsf{T}} } $$

它表示扰动会沿着使价值函数增大的方向作用。 $\delta_i$ 决定了扰动在博弈中的权重，也和论文设定的扰动衰减水平有关。

整个控制器的难点已经被集中到一个东西上：求未知的 $V_{ic}^*(X_i)$。

作者假设：

$$ \boxed{ V_{ic}^*(X_i)= w_i^{*\mathsf{T}}\phi_i(X_i) + \bar B_i^c(X_i) + \epsilon_i(X_i) } $$

其中：

  • $w_i^*$：理想但未知的权重；
  • $\phi_i(X_i)$：神经网络的基函数；
  • $\bar B_i^c$：显式写入近似中的安全障碍项；
  • $\epsilon_i$：神经网络无法完全拟合的误差。

安全边界附近的障碍函数变化很快，将障碍项显式保留，critic 就不用独自承担全部安全函数的拟合工作。

CBF 直接进入 Critic

用 critic 神经网络近似价值函数，并从价值函数的梯度计算控制策略。

普通形式可能是：

$$ V^*\approx w^\mathsf{T}\phi(X). $$

本文则设计成：

$$ \boxed{ V^* \approx w^\mathsf{T}\phi(X)+\bar B^c(X) } $$

其中：

$$ \bar B_i^c(X_i)= \kappa_i \sum_{o=x,y,z} \frac{(k_i^oe_i^o)^2} {(k_i^o+e_i^o)(k_i^o-e_i^o)+\sigma_i} $$

这里的 $\sigma_i>0$ 是为了避免数值奇异。

因此 Critic 从训练过程中一开始就知道“越接近安全边界，value 应该越差”，不用等 RL 自己慢慢从数据中学出来。

利用 critic 计算得控制器

实际使用中，理想权重 $w_i^*$ 未知，于是用估计权重 $\hat w_i$：

$$ \hat V_{ic}= \hat w_i^\mathsf{T}\phi_i+\bar B_i^c $$

所以：

$$ \nabla \hat V_{ic}= \hat w_i^\mathsf{T}\nabla\phi_i+ \nabla\bar B_i^c $$

将其代入前面的控制律和扰动律，就得到论文的近似策略：

$$ \boxed{ \hat u_i= \beta_i-\alpha_i \tanh\left( \frac{1}{2}\alpha_i^{-1} G_i^\mathsf{T}\nabla\hat V_{ic}^{\mathsf T} \right) } $$

扰动估计：

$$ \boxed{ \hat d_i= \frac{1}{2\delta_i^2}\nabla\hat V_{ic}^{\mathsf T} } $$

Critic 的权重会影响价值梯度，价值梯度再影响控制指令。

Critic 权重训练

作者利用 HJI 方程本身：理想情况下，把最优策略和价值函数放进哈密顿量后，应满足

$$ H_i(X_i,\nabla V_i^*,u_i^*,d_i^*)=0 $$

因此，可以把哈密顿量的计算值当作残差。

定义哈密顿估计误差： $$ e_i(t)= \hat w_i^\mathsf{T}(t)\phi_i(t)+L_i^c(X_i,\hat u_i,\hat d_i)+\bar B_i^{c0}(t) $$

若价值函数近似和策略都理想，残差应接近零。训练 critic 权重，就是调整 $\hat w_i$，让这个残差变小。

同时，对于历史时刻：

$$ t_1,t_2,\dots,t_l $$

把以前记录的数据重新拿出来计算：

$$ e_i(t_p,t) $$

于是 loss：

$$ E_i(\hat w_i)= \frac12 \left[ \frac{e_i(t)^2} {(\phi_i^\mathsf{T}(t)\phi_i(t)+1)^2}+ \sum_{p=1}^{l} \frac{e_i(t_p,t)^2} {(\phi_i^\mathsf{T}(t_p)\phi_i(t_p)+1)^2} \right] $$

然后梯度下降，得到论文的权重更新律：

$$ \boxed{ \dot{\hat{w}}_{i}= -\sigma _{iw} \frac{\partial E_i}{\partial\hat w_i}} $$

  • 第一项：用当前数据修正权重；
  • 求和项：用过去存下来的数据继续修正权重；
  • $\sigma_i^w$：学习率，决定调整快慢；
  • 负号：沿着残差下降的方向更新。

即训练目标同时惩罚当前残差与回放残差。

储存历史样本

定义：

$$ \Pi_i= [\bar\phi_i(t_1), \bar\phi_i(t_2), \dots, \bar\phi_i(t_l)] $$

要求：

$$ \boxed{ \operatorname{rank}(\Pi_i)=m } $$

只要 replay dataset 足够丰富、满秩即可。

Critic 权重不是直接拟合控制量，而是拟合价值函数；控制量由价值函数梯度算出；训练则通过让 HJI 残差变小来调整权重。

三个创新点

CBF + 编队碰撞约束 + 非对称输入约束统一

同时考虑了避免碰撞、执行器约束和编队跟踪。

把扰动建模成“对手”

本文将扰动看作最大化博弈方而非噪声，使得控制器本质上具有最恶劣情况的健壮性。

CBF-aware critic + Experience Replay，避开 Persistent Excitation

传统理论经常要求系统输入/数据必须持续充分激励，但是 UAV 编队飞行时，不一定容易满足这个条件，本文改成储存历史样本，可以在线检查，比 PE 更方便。这是这篇论文在 RL 训练机制上的重要贡献。

Optimization Algorithm of UAVs Task Assignment and Path Planning Based on Dynamic Cluster Particle Swarm Optimization

本文研究的问题聚焦于 UAV 集群的任务分配与路径规划，侧重工程实现。

现状

任务分配问题

信息不完全

每架 UAV 不可能掌握所有其他 UAV 和任务的完整信息，即不完全信息环境。

既竞争又合作

可能存在所有 UAV 都去抢一个高价值任务的情况，但 UAV 又必须合作提高整体任务收益。

传统 PSO 做路径规划容易陷入局部最优

PSO 的优势是参数少、计算简单、收敛快，所以很适合路径规划，但存在搜索精度不足和容易陷入局部最优等问题。

  • MAPPO算法：集群调配
  • DCPSO算法：路径规划

MAPPO

MAPPO 的基本思路：

  • Actor 根据当前状态选择动作，也就是任务决策；
  • Critic 估计当前状态或动作的价值；
  • 用优势函数评估某个动作比平均策略好多少；
  • 用 PPO 的裁剪更新限制策略一次变化太大，从而让训练更稳定。

论文中的 MAPPO 输出的是目标选择策略，运动模型主要用于描述位置变化。

任务分配

多 UAV 任务分配建模为一个离散时间的马尔科夫决策进程。

连续 UAV 运动简单表示成：

$$ \dot{x}=v\cos\varphi $$

$$ \dot{y}=v\sin\varphi. $$

其中：

  • $(x,y)$：UAV 位置；
  • $v$：巡航速度；
  • $\varphi$：航向角。

然后把连续飞行轨迹离散成路径点集合，以方便计算机处理。

论文定义了$y_{i,n}$，表示 UAV $U_n$ 是否完成任务 $i$。

可以理解为：

$$ y_{i,n}= \begin{cases} 1,&U_n\text{执行任务}i;\\ 0,&\text{否则} \end{cases} $$

目标是在满足任务时间：

$$ T<T_{\max} $$

的条件下，让 UAV 集群获得尽可能大的总奖励。本质上是让 $N$ 架 UAV 获得任务路径，在时间限制内最大化总任务收益。

MAPPO 即 Multi-Agent Proximal Policy Optimization，近端策略优化，是 PPO 的多智能体扩展。

$\text{Actor}_i$ 决定无人机下一步的任务，而 $\text{Critic}$ 评估 UAV 集群的整体状态情况。

论文采用“集中训练，分别执行”，即训练阶段共享全局信息进行价值评估，而实际执行时每个 agent 根据自己的策略执行的方式展开。

数学原理

Advantage function：

$$ A^\pi(s_t,a_t)= Q^\pi(s_t,a_t)-V^\pi(s_t) $$

广义优势估计 GAE

Generalized Advantage Estimation $A_t^{GAE(\gamma,\lambda)}$

基本形式是：

$$ A_t^{GAE}= \sum_{l=0}^{T-t} (\lambda\gamma)^l\delta_{t+l}. $$

其中：

$$ \delta_t= r_t+\gamma V(s_{t+1})-V(s_t). $$

它实际上平衡了 bias 和 variance，让优势估计更稳定。这部分内容仍属于标准 PPO/MAPPO 框架，并不是本文最主要的创新。

CLIP

MAPPO 的核心 policy objective 来自 PPO：

$$ L_i^{CLIP}(\theta_i)= E_t [ \min( r_t(\theta_i)A_i, \operatorname{clip}(r_t(\theta_i),1-\epsilon,1+\epsilon)A_i ) ]. $$

其中：

$$ r_t(\theta_i)= \frac{ \pi_{\theta_i}(a_t|s_t) }{ \pi_{\theta_{i,\text{old}}}(a_t|s_t) }. $$

这个 ratio 衡量新策略相对于旧策略改变了多少。如果完全不限制：

$$ \pi_{\rm old} \rightarrow \pi_{\rm new} $$

可能一次更新太大，导致训练不稳定。所以：

$$ \operatorname{clip} ( r_t, 1-\epsilon, 1+\epsilon ) $$

相当于给 policy update 加一个“限幅器”，用于控制策略更新幅度。

决策变量 $y_{i,n}\in{0,1}$（第 $i$ 个目标是否由第 $n$ 架执行），目标最大化总收益：

$$\max\ \sum_{i} r_i \sum_{n} y_{i,n},\qquad \text{s.t. 总时间}<T_{\max}$$

MAPPO 的关键是 CTDE：训练时用一个集中式 critic，执行时每架无人机只用局部观测的 actor。论文强调因此"智能体数、目标数增加不改变网络结构复杂度"。

“改进"部分：多个智能体撞到同一航路点时，按随机概率 $P$ 选择，形成博弈关系，声称实现群内竞争与合作的 Nash 均衡。

仿真：目标区 $[-50,50]$ km，TDOA 测量误差 $20$ ns，站址误差 $10^{-3}$ km，相关系数 $0.35$，指标是 GDOP；收益对比 MAPPO 比 PSO/GA 高 12.2%（10机/60目标）和约 41%（15机/90目标）。

DCPSO

人工势场建模，环境代价只有两个势场分量：

$$f_a(x,y)=\varepsilon\big[(x-x_0)^2+(y-y_0)^2\big],\qquad f_r(x,y)=\eta\Big(\frac{1}{p(x,y)}-\frac{1}{a_0}\Big)^{3}$$

滚动时域：窗口含 3 个航迹点，8 方向网格移动，执行一步后窗口前移。

Tent 混沌初始化——利用混沌的随机性/遍历性让初始种群铺满解空间：

$$x_{n+1}=\begin{cases}2x_n,&0\le x_n<0.5\\ 2(1-x_n),&0.5\le x_n<1\end{cases}$$

动态聚类机制（论文的核心创新）：选位置最密集者为簇头 → 距簇头最近的半数归簇 1、其余归簇 2 → 簇间不交互各自迭代 → 合并。论文的解释是簇 1 跟 $P_1$ 落到局部最优 $A_1$，簇 2 跟 $P_0$ 走向全局最优 $A_0$。

仿真：种群 50、学习因子 2、惯性权重 1；4 个多峰低维函数 30 次实验对比 PSO/PIO/SSA/CDPIO，标准差为 0；航迹长度均值 108.84、标准差 0.34、最优 108.34、最差 109.85；5 个静态障碍。复杂度 $O(NDT)$ → $O(NDT\cdot(\text{PSO}+\text{混沌}))$。