固定翼无人机阵型变换过渡轨迹生成

C++代码实现

1. 输入参数

  • UAV 数量：$N$

  • 初始／目标槽位（在参考骨架坐标系下）：

    $$ \{s_i^{\rm init}\}_{i=1}^N,\quad \{s_i^{\rm goal}\}_{i=1}^N,\quad s_i\in\mathbb R^2 $$

  • 骨架参考轨迹（全局坐标系）：

    $$ r(t) = \bigl(x_{\rm ref}(t),\,y_{\rm ref}(t)\bigr),\quad t\in[0,T], $$

    已保证二阶连续，可直接计算

    $$ \dot r(t),\quad \psi_{\rm ref}(t)=\arctan2\bigl(\dot y_{\rm ref},\,\dot x_{\rm ref}\bigr),\quad \|\dot r(t)\|=V. $$

  • 飞行／安全约束：

    $$ V=\text{常定空速},\quad R_{\min}=\text{最小转弯半径},\quad d_{\rm safe}=\text{最小安全间距}. $$

  • 规划参数：

    $$ T=\text{过渡总时长},\quad \Delta t=\text{采样周期},\quad N_{\rm samp}=\frac{T}{\Delta t}+1. $$

2. 预处理

  1. 曲率可行性 骨架曲率

    $$ \kappa_{\rm ref}(t) =\frac{\dot x_{\rm ref}\,\ddot y_{\rm ref} - \ddot x_{\rm ref}\,\dot y_{\rm ref}} {(\dot x_{\rm ref}^2+\dot y_{\rm ref}^2)^{3/2}} \le \frac1{R_{\min}} \quad\forall t. $$

    若不满足，需先对 $r(t)$ 做平滑/重参数化。（轨迹规划已保证）

  2. 队形安全性 对任意 $i\neq j$ 及 $\alpha\in[0,1]$，插值槽位

    $$ s_i(\alpha)=(1-\alpha)\,s_i^{\rm init} + \alpha\,s_i^{\rm goal}, $$

    必须保证

    $$ \bigl\|s_i(\alpha)-s_j(\alpha)\bigr\|\ge d_{\rm safe} \quad\forall\alpha,\;i\neq j. $$

    如不满足，可先对插值路径（例如分两段：先拉开／再旋转）做调整。（具体方法见后）

  3. 插值权函数 选一个端点零速度的平滑函数

    $$ \alpha(\tau)\;=\;\tau^2(3-2\tau),\qquad \tau=\frac{t}{T}\in[0,1], $$

    满足 $\alpha(0)=0,\;\alpha(1)=1,\;\alpha'(0)=\alpha'(1)=0$。

3. 阵型变换的形变算法

既然初／末队形都是相对于同一参考轨迹 $r(t)$，我们可将整个过渡视为一段“形变”：

阵型变换的形变过程是让编队在参考骨架上平滑地从初始槽位过渡到目标槽位。

3.1 背景与目标

  • 骨架参考轨迹

    $$ r(t)=(x_{\rm ref}(t),\,y_{\rm ref}(t)),\quad t\in[0,T], $$

    已保证二阶连续，且恒定空速 $\|\dot r(t)\|=V$，航向为

    $$ \psi_{\rm ref}(t)=\arctan2\bigl(\dot y_{\rm ref}(t),\,\dot x_{\rm ref}(t)\bigr). $$

  • 编队槽位 每架机 $i$ 在参考骨架坐标系下有一个初始相对位置 $s_i^{\rm init}$ 和目标相对位置 $s_i^{\rm goal}$，都为二维向量。

  • 算法目的 在整个过渡过程中，让每架机沿着“骨架 + 随时间变化的偏移”轨迹飞行，既保持与骨架同步，又完成自身槽位的从初到末的平滑过渡。

3.2 平滑过渡权函数 $\alpha(t)$

我们需要一个从 0 平滑增长到 1 的标量函数 $\alpha:[0,T]\to[0,1]$，且在端点有零一阶导数，保证过渡“起止”阶段无速度突变：

$$ \alpha(\tau) = 3\tau^2 - 2\tau^3,\quad \tau = \frac tT. $$
  • $\alpha(0)=0,\;\alpha(1)=1$
  • $\alpha'(0)=\alpha'(1)=0$ （无加速突变）
  • $\alpha''(0)=\alpha''(1)=0$（无加加速突变，二阶光滑）

3.3 插值偏移量 $\Delta_i(t)$

每架机的相对槽位随时间平滑插到目标：

$$ \Delta_i(t) =(1-\alpha(\tfrac tT))\,s_i^{\rm init} \;+\;\alpha(\tfrac tT)\,s_i^{\rm goal}. $$
  • $t=0$ 时 $\Delta_i=s_i^{\rm init}$，$t=T$ 时 $\Delta_i=s_i^{\rm goal}$。
  • 中间阶段 $\Delta_i(t)$ 按三次多项式插值，保持一阶、二阶连续。

3.4 世界坐标下的旋转映射

在骨架处的航向 $\psi_{\rm ref}(t)$ 为当前参考航线方向，我们构造旋转矩阵

$$ R_c(t) =\begin{bmatrix} \cos\psi_{\rm ref}(t) & -\sin\psi_{\rm ref}(t)\\ \sin\psi_{\rm ref}(t) & \cos\psi_{\rm ref}(t) \end{bmatrix}. $$

它把“参考坐标系下”的偏移 $\Delta_i(t)$ 投影到全局坐标系。

3.5 过渡轨迹生成

要让每架机“自己跑”出那条带偏移的轨迹，并满足动力学约束，正确的做法是 从它自己的轨迹导数 来计算航向和速度：

  1. 位置

    $$ p_i(t) = r(t) + R_c(t)\,\Delta_i(t). $$

  2. 速度向量

$$ \dot{p}_i(t)=\underbrace{\dot{r}(t)}_{\substack{\text { 骨妿切向 } \\=V\left(\cos \psi_{\mathrm{ref}}, \sin \psi_{\mathrm{ref}}\right)}}+\underbrace{\dot{R}_c(t) \Delta_i(t)}_{\substack{\text { 骨架旋转 } \\ \text { 对偏移的贡献 }}}+\underbrace{R_c(t) \dot{\Delta}_i(t)}_{\substack{\text { 偏移本身 } \\ \text { 的变化 }}} . $$

其中

  • $\dot R_c(t) = \dot\psi_{\rm ref}(t)\,R_c(t)\begin{pmatrix}0&-1\\1&0\end{pmatrix}$,

  • $\dot\Delta_i(t)=\tfrac{1}{T}\,\alpha'\bigl(\tfrac tT\bigr)\,(s_i^{\rm goal}-s_i^{\rm init})$.

  • 骨架切向 $\dot r$：提供了主推进力，让整个编队朝前飞。

  • 骨架旋转作用 $\dot R_c\,\Delta$：当骨架改变航向时，挂在它两侧的槽位也会被“甩”出去或拉回来，相当于在飞机上产生离心或向心加速度。

  • 偏移变化项 $R_c\,\dot\Delta$：就是我们要飞机从“前后排”变成“并列排”时，在骨架切向以外再加一个横向速度分量。

  1. 速度幅值

    $$ v_i(t) = \|\dot p_i(t)\|. $$

  2. 航向角

    $$ \psi_i(t) = \arctan2\bigl(\dot p_{i,y}(t),\,\dot p_{i,x}(t)\bigr). $$

这样，每架机既能沿着 骨架大方向 前进，又能利用“骨架旋转”和“偏移变化”带来的额外横向速度分量，完成从纵列到横列的真正转向与移动。

离线／在线生成：先按上面公式算出每时刻的 $p_i(t)$、$\psi_i(t)$、$v_i(t)$。

跟踪控制：把 $\{p_i,\psi_i,v_i\}$ 当作三维参考，对每架机运行前馈＋反馈控制（如纯追踪或 MPC），让飞机在动力学允许范围内尽量贴合。

结论：

  • 在编队变形阶段，不要把 $\psi_i$ 简化成骨架航向；
  • 要 分开 计算骨架切向、骨架旋转对偏移的“甩出/拉回”项，以及偏移自身的变化，再合成真实的速度向量；
  • 再由速度向量求出 $v_i(t)$ 和 $\psi_i(t)$，才能让飞机既跟着骨架跑，又在骨架两侧实现真正的移动。

3.6 导数推导（可选，用于验证）

若需更精确地检查速度／加速度约束，可对 $p_i(t)$ 做一阶、二阶求导：

  1. 一阶导数
$$ \dot{p}_i=\dot{r}+\dot{R}_c \Delta_i+R_c \dot{\Delta}_i . $$
  • $\dot r$ 大小恒为 $V$。
  • $\dot R_c = \dot\psi_{\rm ref}\,R_c\,\begin{bmatrix}0&-1\\1&0\end{bmatrix}$。
  • $\dot\Delta_i = \alpha'(t/T)\,\tfrac1T\,(s_i^{\rm goal}-s_i^{\rm init})$.

  1. 端点特性

    • 因为 $\alpha'(0)=\alpha'(T)=0$，故在 $t=0$ 和 $t=T$ 时，$\dot\Delta_i=0$ 且 $\dot R_c\,\Delta_i$ 只有骨架自身的法向分量，保证编队起落平顺。

  2. 速度约束 若额外产生的速度分量过大，可通过调整 $T$ 或缩小插值幅度 $\|s_i^{\rm goal}-s_i^{\rm init}\|$ 来保证总速度 $\|\dot p_i\|\approx V$。

3.7 离散化实现

以采样周期 $\Delta t$ 离散，令 $t_k=k\Delta t$，对 $k=0,\dots,N_{\rm samp}-1$。

这样就得到了每架机的一系列离散参考点$\{p_i^{\rm ref}[k],\,\psi_i^{\rm ref}[k],\,v_i^{\rm ref}[k]\}$，供下游的跟踪控制器（如纯追踪、LQR、MPC）使用。

3.8 核心优点

  1. 全局同步：所有飞机在同一 $t$ 上共享相同的骨架位置和航向，天然保持队形几何。
  2. 平滑过渡：三次插值 $\alpha(t)$ 保证偏移从头到尾一、二阶都连续，无抖动。
  3. 简单高效：只需几行矩阵运算和插值，实时计算量 $O(N)$；易于并行化。
  4. 可控性强：可根据需要改 $\alpha$ 端点阶数或插值曲线，或在 $\Delta_i$ 上加更多约束（如避障偏移）。

如此，单段“形变”算法就通过“骨架 + 时间平滑插值偏移”三步，完成对每架机世界坐标下参考轨迹的生成，既保证了编队整体的同步与平滑，也天然满足了速度、航向连续的飞行器动力学约束。

3.9 举例

下图演示了三架固定翼无人机从“纵一字”（单纵列）队形平滑过渡到“横一字”（并列）队形的过程：

  1. 初始／目标槽位

    • 纵列时，各机相对骨架的初始偏移

      $$ s^{\rm init}_1=(-10,0),\quad s^{\rm init}_2=(0,0),\quad s^{\rm init}_3=(10,0). $$

    • 并列时，最终偏移

      $$ s^{\rm goal}_1=(0,-10),\quad s^{\rm goal}_2=(0,0),\quad s^{\rm goal}_3=(0,10). $$

  2. 骨架参考轨迹 一条沿 $x$ 轴的直线：

    $$ r(t)=(Vt,0),\quad t\in[0,T],\;V=20\text{ m/s},\;T=10\text{ s}. $$

  3. 插值权函数 $\alpha(\tau)=3\tau^2-2\tau^3,\;\tau=t/T$，保证一、二阶导在端点为零。

  4. 偏移插值 每时刻 $t$，第 $i$ 架机的偏移

    $$ \Delta_i(t) = (1-\alpha)\,s_i^{\rm init} + \alpha\,s_i^{\rm goal}. $$

  5. 全局位置 因骨架航向恒为 $0$，不需要旋转矩阵，直接

    $$ p_i^{\rm ref}(t) = r(t) + \Delta_i(t). $$

  6. 轨迹曲线

    • X 方向：直线匀速前进，由 $0$ 匀增至 $V\,T=200$ m；
    • Y 方向：由初始的 $0$ → 终端的 $\pm10$ m，曲线平滑爬升/下降，端点处一阶、二阶均平滑。

  7. 关键性质

    • 端点无速度突变：$\alpha'(0)=\alpha'(1)=0$，保证起飞和落位阶段不会有额外横向速度；
    • 高效计算：只需若干向量插值和加法，在线 $O(N)$ 复杂度；
    • 可扩展性：如需更复杂骨架（PH 曲线），只需在第 2 步替换 $r(t)$ 和航向矩阵 $R_c(t)$。

通过上述示例，你可以清晰地看到“单段形变”算法如何利用平滑插值函数，在同一骨架轨迹上，让整个编队从一列整齐并列成一排，并保证速度、航向的连续性与飞行器的动力学约束。

4. 形变总时长 $T$的估计

要为形变过程选一个合适的总时长 $T$，既要保证骨架轨迹走完，又要让每架机在动力学和安全约束下，平滑地完成横向偏移。下面给出下界估计与经验设定两种思路。

一、下界估计

假设我们仍用三次插值权函数

$\alpha(\tau)=3\tau^2-2\tau^3,\quad \tau=\frac tT,$

其一阶、二阶导的最大值在区间 $[0,1]$ 上分别为

$\max\alpha'(\tau)=1.5,\qquad \max|\alpha''(\tau)|=6.$

又令

$\Delta s_{\max} \;=\;\max_{i}\bigl\|s_i^{\rm goal}-s_i^{\rm init}\bigr\|$

为最大插值位移。

1. 骨架轨迹时长约束

骨架轨迹长度

$L_{\rm skel}=\int_0^T\bigl\|\dot r(t)\bigr\|\,\mathrm dt = V\,T.$

要走完骨架至少需

$T\ge \frac{L_{\rm skel}}{V}.$

2. 横向速度约束

在任意时刻，偏移向量的横向分量速度

$v_{\rm cross}(t) =\bigl\|\dot \Delta_i(t)\bigr\| =\frac{\alpha'(\tau)}{T}\,\Delta s_i,$

其最大值约为

$v_{\rm cross,max} =\frac{\max\alpha'\,\Delta s_{\max}}{T} =\frac{1.5\,\Delta s_{\max}}{T}.$

为不超过飞机最大横向速度 $V_{\rm cross,max}$（可由最大倾斜角 $\varphi_{\max}$ 给出， $V_{\rm cross,max}=V\sin\varphi_{\max}$），需

$\frac{1.5\,\Delta s_{\max}}{T}\;\le\;V\sin\varphi_{\max} \quad\Longrightarrow\quad T\;\ge\;\frac{1.5\,\Delta s_{\max}}{V\sin\varphi_{\max}}.$

3. 横向加速度约束

偏移的横向加速度

$a_{\rm cross}(t) =\bigl\|\ddot \Delta_i(t)\bigr\| =\frac{\alpha''(\tau)}{T^2}\,\Delta s_i,$

其峰值约

$a_{\rm cross,max} =\frac{6\,\Delta s_{\max}}{T^2}.$

需满足飞机最大侧向加速度 $a_{\max}=g\tan\varphi_{\max}$：

$\frac{6\,\Delta s_{\max}}{T^2}\;\le\;g\tan\varphi_{\max} \quad\Longrightarrow\quad T\;\ge\;\sqrt{\frac{6\,\Delta s_{\max}}{g\tan\varphi_{\max}}}\,.$

综合下界

$T \;\ge\; \max\Biggl\{ \underbrace{\frac{L_{\rm skel}}{V}}_{\text{骨架时长}}, \;\underbrace{\frac{1.5\,\Delta s_{\max}}{V\sin\varphi_{\max}}}_{\text{横向速度}}, \;\underbrace{\sqrt{\frac{6\,\Delta s_{\max}}{g\tan\varphi_{\max}}}}_{\text{横向加速度}} \Biggr\}.$

取这一最大值为 最小可行 $T$。

二、经验设定

在工程上，还常辅以以下经验方法：

  1. 安全裕度 在上述下界上再乘以一个安全系数 $k\in[1.1,1.5]$，以应对风扰动与数值误差。

  2. 分段时间分配 如果要把整个过渡分为“分散→聚合”“形变”“分散→落位”三段，可先估算每段的 $\Delta s_{\max}$ 并分别套用同样的方法，得到 $T_1,T_2,T_3$，最后令

    $T=T_1+T_2+T_3$

  3. 仿真验证 用离线仿真或快速 MPC 在候选 $T$ 下模拟一次，检查横摆速度、侧向加速度和安全间距是否满足，再微调 $T$。

  4. 经验规则

    • 对于常见编队（队列宽度不超过 20 m，飞行速度 15–25 m/s，倾斜角限幅 25°），一般取 $T\approx 5\text{–}10$ s 即可完成单次变形；
    • 更大队形或更大位移可按比例线性放大。

举例

  • 参考骨架长 $L_{\rm skel}=200$m，$V=20$m/s → 骨架下界 $=10$s。
  • 最大偏移 $\Delta s_{\max}=10$m，$\varphi_{\max}=25°$：
    • 速度约束下界 $\;T\ge1.5\cdot10/(20\sin25°)\approx3.5$ s；
    • 加速度约束下界 $\;T\ge\sqrt{6\cdot10/(9.81\tan25°)}\approx4.2$ s。
  • 取三者最大：$T_{\min}=10$s，加安全系数 1.2 → 最终 $T\approx12$ s。

结论：

  1. 计算下界，确保满足骨架、横向速度与加速度约束；
  2. 乘以安全系数，并结合仿真微调；
  3. 若分段，可对每段套用同样方法，再累加。

如此，便能给出既快速又安全平顺的变形总时长 $T$。

5. 队形安全性不满足情况下的调整

当直接沿线性插值

$$ s_i(α) = (1-α)\,s_i^{\rm init} + α\,s_i^{\rm goal} $$

时，若在某些 $α$ 上 $\|s_i(α)-s_j(α)\| < d_{\rm safe}$，就需要对“插值路径”做以下几种调整：

5.1 先验检测与分段改造

  1. 离散采样检查

    • 在一组离散参数 $\{α_k\}\subset[0,1]$ 上计算所有 $\|s_i(α_k)-s_j(α_k)\|$。
    • 如果存在某段 $[α_a,α_b]$ 导致近距离，就定位需要调整的“危险区间”。

  2. 分段形变 把整个变换分成三段：

    1. 发散（分散）阶段 $[0,α_1]$：先把所有相对位置从初始矩阵各自按径向放大到一个“安全外圈”

      $$ s_i^{(1)}(α) = (1-β(α))\,s_i^{\rm init} + β(α)\,\bigl( \lambda\,s_i^{\rm init}\bigr),\quad \lambda>1 $$

      使最小间距 $\lambda\|s_i^{\rm init}-s_j^{\rm init}\|\ge d_{\rm safe}$。

    2. 形变＋收敛阶段 $[α_1,α_2]$：在这个放大后安全的外圈内，再做初→末槽位的插值。

    3. 微收敛阶段 $[α_2,1]$：再从外圈收敛到目标槽位，保证末端精准对齐。

这样即使直接初→末会碰撞，也先让编队“张开”再“闭合”，始终保持安全距离。

5.2 队形缩放（Scale-Morphing）

将线性插值改为“缩放＋插值”：

$$ s_i(α) = r(α)\;\Bigl[\,\bigl(1-α\bigr)\,\bigl(\lambda\,s_i^{\rm init}\bigr) +α\,\bigl(\lambda\,s_i^{\rm goal}\bigr)\Bigr], $$

其中

$$ r(α)=1 + \mu\,\sin(π\,α) $$

是一个在中间阶段适度增加队形尺度的因子，$\mu>0$ 控制最大放大倍数。

  • 端点 $α=0,1$ 时 $r=1$，不影响初末；
  • 中间 $r>1$，提高间距，避开碰撞。

总结：

根据编队规模和实时性需求，可混合分段形变＋缩放：先 “张” 再 “变” 再 “合”，最简单直接，来确保插值期间始终满足 $\|s_i(α)-s_j(α)\|\ge d_{\rm safe}$。

C++代码实现