固定翼无人机B 样条避障轨迹生成

障碍环境下固定翼无人机B 样条避障轨迹生成算法

C++代码实现

一、输入与预备工作

1. 输入数据

1. 初始离散路径点序列

   $$ \mathcal{P} = \{\,P_0,\,P_1,\,\dots,\,P_N\}, \quad P_i = (x_i,\,y_i,\,v_i,\,a_i), \quad i = 0,1,\dots,N, $$

   由“动力学路径搜索”算法（基于运动原语的混合状态 A*、Kinodynamic A* 等）生成，满足：

   • 每个 $P_i$ 处对应的栅格单元为“空闲”（Free）。
   • 相邻状态间已满足初步的动力学可行性，并避免直接与障碍碰撞。

2. 二维障碍物栅格地图 $\mathcal{G}$

   $$ \mathcal{G}[u,v] \in \{0,1\}, \quad u=1,\dots,M,\;v=1,\dots,N, $$

   其中 1 表示“障碍（Occupied）”，0 表示“空闲（Free）”；

   • 栅格分辨率：$r_{\text{res}}$（如 0.1 m），便于将连续坐标 $(x,y)$ ↔ 栅格索引 $(u,v)$ 转换。
   • 预先计算得到 距离场 $\mathrm{DF}[u,v]$，用以查询任意位置到最近障碍物的欧氏距离。

3. 固定翼飞机动力学参数

   • 最小允许飞行速度：$\bar v_{\min} > 0$。
   • 最大允许飞行速度：$\bar v_{\max} > \bar v_{\min}$。
   • 最大允许加速度：$\bar a > 0$。
   • 最小回转半径： $R_{\min} > 0$，对应最大曲率 $\kappa_{\max} = 1/R_{\min}$。

4. B 样条与离散化参数

   • 样条次数（阶数）：$p = 3$（三次 B 样条）。
   • 时间间隔：$\Delta t > 0$，决定离散化时每一步的时刻增量。

2. 符号与变量定义

• 原始离散路径：$\{P_0,\dots,P_N\}$。

• 相邻点欧氏距离：

  $$ \ell_i = \|P_{i+1} - P_i\|,\quad i=0,\dots,N-1. $$

• 在路径点附近与障碍的最小距离：

  $$ d_c^{(i)} = \min_{x\in \overline{P_iP_{i+1}}}\bigl(\mathrm{距离场}(x)\bigr), \quad i = 0,\dots,N-1. $$

  （可近似取 $\min\{\mathrm{DF}(\lfloor P_i\rfloor),\,\mathrm{DF}(\lfloor P_{i+1}\rfloor)\}$，若栅格足够密。）

• 相邻两点之间允许的最大间距：

  $$ r_{\max}^{(i)} = \min\Bigl\{\tfrac{d_c^{(i)}}{3},\,\bar v_{\max}\,\Delta t\Bigr\}, \quad i=0,\dots,N-1. $$

• 插值后得到的控制点序列：

  $$ \{Q_0,\,Q_1,\,\dots,\,Q_M\},\quad M \ge N, $$

  其中首尾 $\{Q_0,Q_1,Q_2\}$ 与 $\{Q_{M-2},Q_{M-1},Q_M\}$ 将用于打结并固定边界。

• 节点向量（均匀打结）：

  $$ \{\,t_0,\dots,t_{M+p+1}\},\quad t_0=t_1=t_2=t_3,\quad t_{M+1}=t_{M+2}=t_{M+3}=t_{M+4},\quad t_{k+1}-t_k = \Delta t\;\text{（对 }k=p,\dots,M\text{）}. $$

• B 样条轨迹：

  $$ \mathcal{S}(t) \;=\; \bigl(x(t),\,y(t)\bigr) \;=\; \sum_{j=0}^{M} N_{j,3}(t)\,Q_j,\quad t\in [\,t_0,\,t_M\,]. $$

• 离散化输出序列：

  $$ \{\,S_k\}_{k=0}^K,\quad t_k = t_0 + k\,\Delta t,\;K = \lfloor (t_M - t_0)/\Delta t\rfloor, $$$$ S_k = \bigl(x(t_k),\,y(t_k),\,v_x(t_k),\,v_y(t_k),\,a_x(t_k),\,a_y(t_k)\bigr). $$

二、算法步骤

1. 计算原始离散路径每段到障碍的最小距离与最大允许间距

1. 遍历原始路径每段 $(P_i,P_{i+1})$

   • 计算欧氏距离：

     $$ \ell_i \;=\; \|P_{i+1} - P_i\|, \quad i = 0,\dots,N-1. $$

   • 查询距离场：

     • 取近似值

       $$ d_c^{(i)} = \min\bigl\{\mathrm{DF}(\lfloor P_i\rfloor),\;\mathrm{DF}(\lfloor P_{i+1}\rfloor)\bigr\}, $$

       如果需要更精确可选取该线段上若干插值点再查询并取最小值。

     • 要求 $d_c^{(i)} > 0$，否则原始路径点就在障碍内部，需回退到路径搜索阶段重新规划。

2. 计算该段允许的最大间距

   $$ r_{\max}^{(i)} = \min\Bigl\{\tfrac{d_c^{(i)}}{3},\;\bar v_{\max}\,\Delta t\Bigr\}. $$

   • 需要同时满足：

     1. “凸包安全”要 $r_{j,j+1} < d_c^{(i)}/3$；
     2. “速度上限”要 $\ell \le \bar v_{\max}\,\Delta t$。

2. 等距插值生成初始控制点

1. 对每段 $(P_i,\,P_{i+1})$ 判断是否插值

   • 若 $\ell_i \le r_{\max}^{(i)}$，则不插值，仅保留两端点作为控制点；

   • 否则，需要插值：

     1. 计算所需插值数量：

        $$ N_{\text{ins},i} \;=\; \Bigl\lceil \tfrac{\ell_i}{\,r_{\max}^{(i)}}\Bigr\rceil \;-\;1. $$

     2. 在 $\overline{P_iP_{i+1}}$ 上等距插入这 $N_{\text{ins},i}$ 个新点：

        $$ P_i^{(k)} \;=\; P_i \;+\; \frac{k}{\,N_{\text{ins},i}+1\,}\,\bigl(P_{i+1} - P_i\bigr), \quad k = 1,2,\dots,N_{\text{ins},i}. $$

        每个新插值点的位置只包含 $(x,y)$，速度与加速度可用线性插值或初步赋值（将由后端优化整合）。

2. 构造新的控制点序列 $\{Q_j\}$

   • 初始化空序列 $\mathcal{Q}=\emptyset$。

   • 依次遍历 $i=0\dots N-1$：

     1. 将 $P_i$（若 $i=0$ 或上一段未添加，否则已添加）加入 $\mathcal{Q}$。
     2. 若 $N_{\text{ins},i} > 0$，按顺序将 $\{\,P_i^{(1)},\,\dots,\,P_i^{(N_{\text{ins},i})}\}$ 依次加入 $\mathcal{Q}$。
     3. 将 $P_{i+1}$ 暂不加入（下一轮中作为“上一段末尾”再加入），以免重复。

   • 遍历结束后，将最后一个原始点 $P_N$ 加入 $\mathcal{Q}$。

   • 记 $\mathcal{Q} = \{\,Q_0,\,Q_1,\,\dots,\,Q_M\}$，其中

     $$ M = N + \sum_{i=0}^{N-1} N_{\text{ins},\,i}. $$

3. 检查插值后相邻段长

   • 对 $\{Q_0,\dots,Q_M\}$ 序列再次检查：

     $$ r_j = \|\,Q_{j+1} - Q_j\|,\quad j=0,\dots,M-1. $$

     确保对所有 $j$ 有

     $$ \bar v_{\min}\,\Delta t \;\le\; r_j \;\le\; \bar v_{\max}\,\Delta t, \quad r_j < \frac{d_c^{(j)}}{3}, $$

     其中 $d_c^{(j)}$ 取该段控制点对应的最小障碍距离（可近似继承插值前的值），必须同时满足“速度下限”、“速度上限”与“凸包安全”要求。

   • 如果出现某段 $\|Q_{j+1} - Q_j\| < \bar v_{\min}\,\Delta t$，可：

     1. 合并相邻过近的两个点，减少过小段，或
     2. 增大时间间隔 $\Delta t$（如果后端优化允许不同段不同 $\Delta t$，也可局部增加）。

   • 如果出现某段 $\|Q_{j+1} - Q_j\| > r_{\max}^{(j)}$，则需重新在该段插值；循环直到所有相邻段都满足条件为止。

3. 构造三次 B 样条与节点向量

1. 打结与节点向量设置

   • 对已插值控制点序列 $\{Q_0,\dots,Q_M\}$，构造均匀打结节点向量 $\{t_0,\dots,t_{M+p+1}\}$，其中 $p=3$，令：

     $$ \begin{cases} t_0 = t_1 = t_2 = t_3,\\ t_4 = t_0 + \Delta t,\;t_5 = t_0 + 2\Delta t,\;\dots,\;t_{M} = t_0 + (M-3)\,\Delta t,\\ t_{M+1} = t_{M+2} = t_{M+3} = t_{M}. \end{cases} $$

   • 这样保证了：

     1. 首尾打结：$Q_0,Q_1,Q_2,Q_3$ 共用同一个起始节点值，$Q_{M-3},Q_{M-2},Q_{M-1},Q_M$ 共用同一个终止节点值；
     2. 时间均匀：对中间节点，$\Delta t$ 固定不变。

2. 表示初始 B 样条轨迹

   • 定义基函数 $N_{j,3}(t)$（三次均匀 B 样条基函数），则初始轨迹：

     $$ \mathcal{S}_0(t) = \sum_{j=0}^{M} N_{j,3}(t)\,Q_j, \quad t \in [\,t_0,\,t_{M}\,]. $$

   • 根据 B 样条性质，该曲线自动位于所有控制点凸包内，但尚未执行优化，可能不满足“最小回转半径”或“全部凸包无碰撞”要求。

4. 后端优化：凸包避碰、曲率与速度/加速度约束

1. 优化变量

   • 自由控制点：$\{Q_3,\,Q_4,\dots,\,Q_{M-3}\}$，即除去首尾各 3 个打结点后的一批内部点。
   • 固定控制点：$\{Q_0,Q_1,Q_2\}$ 与 $\{Q_{M-2},Q_{M-1},Q_M\}$，用于保持起点/终点及其一阶速度边界不变。

2. 成本函数与约束项 令所有被优化的内部点组成向量 $\mathbf{q} = [\,Q_3^\top,\;Q_4^\top,\;\dots,\;Q_{M-3}^\top\,]^\top$。目标是求解最优 $\mathbf{q}^*$，使得以下成本最小且满足所有不等式约束：

   1. 高阶导数平滑成本 $\,f_{\text{smooth}}$

      $$ f_{\text{smooth}} = \sum_{j=0}^{M-3} \int_{t_0}^{t_M} \bigl\|\tfrac{d^3}{dt^3}\,\mathcal{S}(t)\bigr\|^2\,dt, $$

      通过对三次 B 样条求三阶导数，转换为对控制点的二次型惩罚。该成本鼓励曲线抖动小、平滑度高。

   2. 速度上限与下限软约束 $\,f_{v}$

      • 对每个时刻 $t$，速度向量为

        $$ \dot{\mathcal{S}}(t) = \bigl(\,\dot x(t),\,\dot y(t)\bigr), \quad v(t) = \|\dot{\mathcal{S}}(t)\|. $$

      • 离散化近似：对每个相邻控制点段 $\{Q_j,\,Q_{j+1}\}$，近似速度为

        $$ v_{j} \approx \frac{\|Q_{j+1} - Q_j\|}{\Delta t}. $$

      • 定义对速度上下限的惩罚：

        $$ f_{v} = \sum_{j=0}^{M-1} \begin{cases} \alpha_v\,\bigl(v_j - \bar v_{\max}\bigr)^2, & v_j > \bar v_{\max},\\ \alpha_v\,\bigl(\bar v_{\min} - v_j\bigr)^2, & v_j < \bar v_{\min},\\ 0, & \bar v_{\min} \le v_j \le \bar v_{\max}, \end{cases} $$

        其中 $\alpha_v > 0$ 为权重系数。

   3. 加速度上限软约束 $\,f_{a}$

      • 对每三个连续控制点 $\{Q_j,\,Q_{j+1},\,Q_{j+2}\}$，近似加速度

        $$ a_{j} \approx \frac{\|\,Q_{j+2} - 2\,Q_{j+1} + Q_j\|}{\Delta t^2}. $$

      • 若 $a_j > \bar a$，则产生惩罚：

        $$ f_{a} = \sum_{j=0}^{M-2} \alpha_a\,\max\bigl(0,\;a_j - \bar a\bigr)^2, $$

        权重系数 $\alpha_a > 0$。

   4. 曲率上限软约束 $\,f_{\kappa}$

      • 对每三个连续控制点 $\{Q_j,\,Q_{j+1},\,Q_{j+2}\}$，可近似计算离散曲率：

        $$ \kappa_{j} \;=\; \frac{2\,\bigl|\det\bigl([\;Q_{j+1}-Q_{j},\,Q_{j+2}-Q_{j+1}\bigr])\bigr|} {\|Q_{j+1}-Q_{j}\|\;\cdot\;\|Q_{j+2}-Q_{j+1}\|\;\cdot\;\|Q_{j+2}-Q_{j}\|}\,, $$

        或更精确地在 B 样条连续曲线上采样并计算 $\kappa(t) = \frac{|\dot x\,\ddot y - \ddot x\,\dot y|}{(\dot x^2 + \dot y^2)^{3/2}}$。

      • 定义惩罚：

        $$ f_{\kappa} = \sum_{j=0}^{M-2} \alpha_\kappa\,\max\bigl(0,\,\kappa_{j} - \kappa_{\max}\bigr)^2, $$

        权重系数 $\alpha_\kappa > 0$。

   5. 凸包无碰撞软约束 $\,f_{\text{col}}$

      • 对每四个连续控制点 $\{Q_j,Q_{j+1},Q_{j+2},Q_{j+3}\}$，构造其凸包，并保证任意凸包内点与障碍物距离 $> 0$。

      • 可采取“采样+距离场查询”或“间距阈值近似”。近似方式：

        1. 对这四个点分别查询其离障距离：

           $$ d_{j}^\ell = \mathrm{距离场}(Q_{j+\ell}),\quad \ell=0,1,2,3. $$

        2. 近似认为该凸包内任意点到障碍距离有下界

           $$ d_h \;>\; \min_{\ell=0..3} \bigl\{d_{j}^\ell\bigr\} \;-\; (r_{j,j+1} + r_{j+1,j+2} + r_{j+2,j+3}), $$

           其中 $r_{a,b} = \|Q_a - Q_b\|$。

        3. 若上式右侧 $\le 0$，说明可能碰撞，此时定义惩罚：

           $$ f_{\text{col}} = \sum_{j=0}^{M-3} \alpha_c \,\max\Bigl(0,\; (r_{j,j+1} + r_{j+1,j+2} + r_{j+2,j+3}) - \min_{\ell} d_j^\ell \Bigr)^2, $$

           权重系数 $\alpha_c > 0$。

      • 若需更精确可在每个四点凸包内随机采样若干点，再做距离场查询并累加惩罚。

3. 总成本函数

   $$ J(\mathbf{q}) \;=\; \lambda_{\text{smooth}}\,f_{\text{smooth}} \;+\;\lambda_v\,f_{v} \;+\;\lambda_a\,f_{a} \;+\;\lambda_\kappa\,f_{\kappa} \;+\;\lambda_c\,f_{\text{col}}, $$

   其中 $\{\lambda_{\text{smooth}},\lambda_v,\lambda_a,\lambda_\kappa,\lambda_c\}$ 为各项权重，根据任务需求手动调参。

4. 约束条件（硬性约束）

   • 对所有时刻或离散段强制满足：

     1. “最低速度” $\tilde v_j = \tfrac{\|Q_{j+1}-Q_j\|}{\Delta t} \ge \bar v_{\min}$。
     2. “最大速度” $\tilde v_j \le \bar v_{\max}$。
     3. “最大加速度” $\tilde a_j = \tfrac{\|Q_{j+2} - 2Q_{j+1} + Q_j\|}{\Delta t^2} \le \bar a$。
     4. “最大曲率” $\tilde \kappa_j \le \kappa_{\max}$。
     5. “凸包无碰撞距离” $\min_{\ell} d_j^\ell - (r_{j,j+1} + r_{j+1,j+2} + r_{j+2,j+3}) > 0$。

   • 如果要简化，可将其中部分限制改为“惩罚”加入成本函数，但对一些绝对不能违反的条件（如最低速度、凸包碰撞）建议作为硬性约束来保证可行性。

5. 求解方式

   • 整个优化问题可视作带不等式约束的非线性最小二乘 / 二次规划。常见解决方案：

     1. 梯度下降或拟牛顿法：使用数值微分估算梯度，对自由控制点向量 $\mathbf{q}$ 迭代更新；
     2. Sequential Quadratic Programming (SQP)：将约束拆成线性或二次子问题；
     3. 交替最小二乘：先固定速度/加速度项求解平滑，再修正碰撞与曲率；
     4. 普通非线性优化库：如 IPOPT、NLopt 等，设定成本和约束；

   • 每次迭代需实时查询距离场或复用加速数据结构，以快速计算碰撞惩罚或距离约束。

6. 优化终止条件

   • 当最大迭代次数到达、或成本函数增量低于阈值、或所有硬性约束均满足且“惩罚梯度”接近 0 时，停止迭代，得到最优控制点 $\{Q_j^*\}$。

5. 离散化输出与轨迹跟随

1. 连续 B 样条轨迹

   • 最终得到的 B 样条轨迹：

     $$ \mathcal{S}^*(t) = \sum_{j=0}^M N_{j,3}(t)\,Q_j^*, \quad t\in [\,t_0,\,t_M\,]. $$

2. 离散化时序状态点

   • 令 $K = \bigl\lfloor \frac{t_M - t_0}{\Delta t}\bigr\rfloor$，对每个 $k=0,1,\dots,K$ 计算时刻：

     $$ t_k = t_0 + k\,\Delta t. $$

   • 计算位置、速度、加速度：

     $$ \bigl(x_k,\,y_k\bigr) = \bigl(x(t_k),\,y(t_k)\bigr), \quad \bigl(v_{x,k},\,v_{y,k}\bigr) = \bigl(\dot x(t_k),\,\dot y(t_k)\bigr), $$$$ \bigl(a_{x,k},\,a_{y,k}\bigr) = \bigl(\ddot x(t_k),\,\ddot y(t_k)\bigr). $$

   • 整理为状态向量：

     $$ S_k = \bigl(x_k,\,y_k,\,v_{x,k},\,v_{y,k},\,a_{x,k},\,a_{y,k}\bigr). $$

3. 最终碰撞与动力学检测

   • 对每个离散点 $(x_k,y_k)$ 做栅格查询，确保栅格为“Free”；

   • 对每段速度 $v_k = \sqrt{v_{x,k}^2 + v_{y,k}^2}$ 与加速度 $a_k = \sqrt{a_{x,k}^2 + a_{y,k}^2}$ 做检测：

     $$ \bar v_{\min} \le v_k \le \bar v_{\max}, \quad a_k \le \bar a. $$

   • 对三点连续段可近似计算离散曲率并检测 $\kappa_k \le \kappa_{\max}$。

   • 若有任一点不满足要求，则需回退到“后端优化”阶段，继续迭代或调整插值密度／$\Delta t$。

4. 输出结果

   • 最优控制点列表：$\{\,Q_0^*,\,Q_1^*,\dots,\,Q_M^*\}$。
   • 节点向量：$\{\,t_0,\dots,t_{M+4}\}$。
   • 离散时序状态：$\{\,S_0,\dots,S_K\}$。
   • 将 $\{S_k\}$ 传递给固定翼飞控或轨迹跟踪模块，按照时序运动并执行避障。

三、算法要点与注意事项

1. 插值与参数选择

   • 插值仅在相邻两控制点段长超过 $\min\{d_c/3,\;\bar v_{\max}\,\Delta t\}$ 时进行，以兼顾“凸包安全”与“最高速度”。
   • 同时必须确保插值后每段与“最低速度” $\bar v_{\min}$ 一致：若 $\|Q_{j+1}-Q_j\| < \bar v_{\min}\,\Delta t$，需要不要插值过密或增大 $\Delta t$。

2. 凸包避障近似

   • 直接在四点凸包上做网格采样并查距离场，代价昂贵；常用的近似方法是：

     $$ d_h \;>\; \min_{\ell=0..3} \{\,\mathrm{DF}(Q_{j+\ell})\}\;-\;(r_{j,j+1}+r_{j+1,j+2}+r_{j+2,j+3}). $$

   • 若更严格，可每个凸包随机或规则采若干点，再查询距离确保无碰撞。

3. 曲率与回转半径

   • 固定翼转弯需要空间，若连续点三角区间过陡、过小，也会使曲率超限。
   • 在优化中，“软约束 + 硬约束”并用：若曲率轻微超过，用惩罚项拉平；若严重超过，则直接判为不可行。

4. 速度下限的处理

   • 由于固定翼无法原地悬停，优化时要避免出现“相邻控制点几乎重合”导致速度接近 0 的情况。
   • 可以将“速度下限”做为硬性约束 $\|Q_{j+1} - Q_j\|/\Delta t \ge \bar v_{\min}$，或在成本函数中给出足够大的惩罚权重 $\lambda_v$。

5. 首尾打结保证边界

   • 起点 $Q_0,Q_1,Q_2$ 固定：保证位置与速度与原始离散路径搜索结果一致；
   • 末点 $Q_{M-2},Q_{M-1},Q_M$ 固定：保证到达终点时位置与速度与目标状态一致。

6. 优化收敛与实时性

   • 为提高在线可用性，可先采用“快速近似 + 少量迭代”获得一个较为靠谱的轨迹，再进行后台逐步精化。
   • 如果(1) 某段障碍密集且拐弯半径受限，(2) 初始离散路径过于稀疏，导致大量插值与迭代，可能超时；此时需要在“动力学路径搜索”阶段增密采样，或允许奔近实时（如在云端或更强算力设备上优化）。

四、伪代码示例

函数 GenerateFixedWingBsplineTrajectory(
    原始离散路径 P[0..N], 
    栅格地图 G, 
    距离场 DF, 
    参数 { Δt, ̄v_min, ̄v_max, ̄a, R_min, p=3 }
):
    # 1. 计算每段距离与障碍距离
    for i in 0..N-1:
        ℓ_i ← || P[i+1].pos – P[i].pos ||
        d_c[i] ← min( DF( P[i].pos ), DF( P[i+1].pos ) )
        if d_c[i] ≤ 0: 
            return “路径点与障碍碰撞，需要重新搜索”
        r_max[i] ← min( d_c[i]/3, ̄v_max * Δt )

    # 2. 插值生成初始控制点序列 Q
    Q ← empty list
    for i in 0..N-1:
        if i == 0:
            append Q.push_back( P[0].pos )
        ℓ_i ← || P[i+1].pos – P[i].pos ||
        if ℓ_i ≤ r_max[i]:
            # 直接相邻，无插值
            append Q.push_back( P[i+1].pos )
        else:
            N_ins ← ceil( ℓ_i / r_max[i] ) – 1
            for k in 1..N_ins:
                α ← k / (N_ins + 1)
                new_pt ← P[i].pos + α*(P[i+1].pos – P[i].pos)
                append Q.push_back( new_pt )
            append Q.push_back( P[i+1].pos )

    M ← length(Q) – 1

    # 3. 校验相邻段是否满足最低速度下限
    repeat:
        any_adjust ← false
        for j in 0..M-1:
            r_j ← || Q[j+1] – Q[j] ||
            if r_j < ̄v_min * Δt:
                # 段太短，不满足速度下限，需要合并或增大 Δt
                # 方案 a: 合并 Q[j]与Q[j+1]
                merge_index ← j+1
                remove Q[merge_index]
                M ← M – 1
                any_adjust ← true
                break
                # 方案 b: 增大 Δt 或标记后端调整，视需求而定
        if any_adjust == false:
            break
    # 结束相邻过短段处理

    # 4. 构造节点向量 T，打结 p=3
    T_length ← M + p + 2  # 总节点数
    T[0..T_length-1] ← 0
    for k in p+1 .. M:
        T[k] ← T[k-1] + Δt
    for k in M+1 .. M+p+1:
        T[k] ← T[M]

    # 5. 后端优化
    # 自由变量： Q[3..M-3]，固定 Q[0..2], Q[M-2..M]
    初始化优化变量 X ← [ Q[3], Q[4], …, Q[M-3] ]
    设置权重 { λ_smooth, λ_v, λ_a, λ_κ, λ_c }
    定义成本函数 J(X) 以及硬性约束 C(X)
    使用非线性优化器（如 SQP、IPOPT）求解：
        minimize J(X)
        s.t. C(X) ≤ 0
    得到最优 X*，更新对应 Q* 序列

    # 6. 生成最终 B 样条与离散跟踪点
    S_output ← empty list
    for k in 0..K where K = floor( (T[M] – T[0]) / Δt ):
        t_k ← T[0] + k * Δt
        (x_k, y_k) ← evaluate_Bspline_position( Q*, T, p, t_k )
        (v_xk, v_yk) ← evaluate_Bspline_velocity( Q*, T, p, t_k )
        (a_xk, a_yk) ← evaluate_Bspline_acceleration( Q*, T, p, t_k )
        append S_output.push_back( (x_k, y_k, v_xk, v_yk, a_xk, a_yk) )
        # 可选：对 (x_k,y_k) 再做一次栅格检查，确保无碰撞

    返回 { Q*, T, S_output }

五、算法总结与特点

1. 兼顾安全与动力学

   • 凸包避障：通过对每四点凸包进行距离场估计与三角不等式近似，确保轨迹片段整体远离障碍。
   • 速度上下限与加速度：固定翼必须保持“不能低于最小速度”、“不能高于最大速度”和“不能超过最大加速度”，算法在插值与后端优化时同时考虑这几项约束。
   • 最小回转半径：通过“曲率上限”约束或惩罚，保证生成的轨迹不会在转弯时超过固定翼物理极限。

2. 层次化设计

   • 前端插值：针对原始离散路径进行插值，保证“相邻段长”落在 $[\bar v_{\min}\Delta t,\;\min(\bar v_{\max}\Delta t,\;d_c/3)]$ 之间，初步满足速度上下限与凸包安全。
   • 打结与初始 B 样条：构造三次均匀打结 B 样条，为后端优化提供连续可微的初始轨迹。
   • 后端优化：对所有内部控制点进行联合优化，最小化“高阶抖动”、“速度/曲率/碰撞”惩罚，生成最终满足所有约束的最优轨迹。

3. 适应性与可扩展性

   • 若环境障碍动态变化，可在飞行途中重新调用此流程：

     1. 动力学路径搜索得到新的粗糙离散路径；
     2. 重新插值＋优化快速得到新 B 样条；
     3. 若规划时间紧张，可只做少量迭代并允许权重动态调整，以保证避障迅速。

   • 若需在三维空间（带高度）规划，可将二维 $(x,y)$ 扩展到三维 $(x,y,z)$，再加高度约束与垂直速度/加速度限制；算法核心不变，仅需多维度距离场与三维曲率控制。

4. 实时性考虑

   • 插值阶段复杂度线性于原始路径点数，多数情况下插值点有限，不会爆炸；
   • 后端优化为稀疏二次/非线性规划，可利用稀疏矩阵加速，或限定最大迭代次数以保证时限。
   • 若追求更快，可在前端做更密的路径搜索并缩短后端优化范围（只优化局部热点段）。

C++代码实现

通过上述算法，固定翼无人机可以在二维障碍物栅格地图中，由动力学路径搜索输出的稀疏离散路径点，高效地生成一条既满足“最低飞行速度”、“最大转弯半径”、“凸包无碰撞”又具有“高阶平滑性”的三次 B 样条避障轨迹，并最终离散化为飞行控制器可跟踪的时序状态点序列。

附：带不等式约束非线性优化求解方法对比

下面对四种常见的带不等式约束非线性优化求解思路进行对比，并给出在固定翼 B 样条避障场景中常用的推荐：

1. 梯度下降 / 拟牛顿法（基于数值微分估算梯度）

实现思路

1. 先把所有自由控制点 $\mathbf{q}=[Q_3^\top,\dots,Q_{M-3}^\top]^\top$ 组织为一个向量。

2. 构造总成本函数

   $$ J(\mathbf{q}) \;=\; \lambda_{\text{smooth}}\,f_{\text{smooth}}(\mathbf{q}) \;+\;\lambda_v\,f_{v}(\mathbf{q}) \;+\;\lambda_a\,f_{a}(\mathbf{q}) \;+\;\lambda_\kappa\,f_{\kappa}(\mathbf{q}) \;+\;\lambda_c\,f_{\text{col}}(\mathbf{q}), $$

   其中每一项可以显式写成关于控制点坐标的二次或高阶函数。

3. 将各个不等式约束（如“速度下限”、“凸包距离”）转化为罚函数或投影（Projection）方式。

4. 在每一步迭代中，用数值微分（或自动微分）计算 $\nabla J(\mathbf{q})$，然后沿负梯度方向更新 $\mathbf{q}$，或用拟牛顿（如L-BFGS）估算 Hessian 近似，再做一轮方向搜索。

5. 迭代直至收敛：当 $\|\nabla J\|$ 足够小、或成本下降量低于阈值、且所有软性惩罚趋近于零时停止。

优缺点

• 优点

  1. 实现门槛低：只需要能评估目标函数和梯度（数值或自动微分），易于用通用代码手写。
  2. 灵活可控：可以对每一项惩罚函数权重精细调节，并随时加入新的软约束。
  3. 占用资源少：相比起一轮完整 SQP，纯梯度下降内存和求解器依赖更少，适合对实时性要求不极高、且问题规模中等的情况。

• 缺点

  1. 收敛速度慢：纯梯度下降对条件数敏感，尤其“碰撞惩罚”项在矩阵条件数很差时，容易出现震荡或极其缓慢的收敛。
  2. 不易严格满足强制约束：如果把“最低速度”与“凸包无碰撞”都做成软惩罚的话，最后只能保证“几乎不违反”，但很难精确地把“速度绝对不能小于 $\bar v_{\min}$”或“凸包距离绝对 > 0”当作硬性约束。
  3. 调参烦人：学习率、惩罚权重、步长策略都要大量试验，才能保证既满足安全约束又快速收敛。

适用场景

• 原始控制点数较少（$\mathbf{q}$ 维度几十到几百），对实时性没那么苛刻（能容忍几十到几百次迭代）。
• 开发初期或研究验证阶段，用于快速原型演示与分析不同惩罚权重的影响。
• 环境障碍不复杂，冲突很少出现、对“凸包碰撞”要求并不绝对严格，只需要一个“较优、几乎无碰撞”的结果即可。

2. Sequential Quadratic Programming (SQP)

原理与实现流程

1. 概念 SQP 将原始非线性问题

   $$ \min_{\mathbf{q}}\, J(\mathbf{q}), \quad \text{s.t.}\quad c_i(\mathbf{q}) \le 0\quad(i=1,\dots,m), $$

   通过泰勒展开和拉格朗日对偶，把每次迭代都近似为一个二次规划（Quadratic Program, QP）：

   $$ \begin{aligned} &\min_{\Delta \mathbf{q}} \;\;\frac12\,\Delta \mathbf{q}^\top\,B\,\Delta \mathbf{q} \;+\;\nabla J(\mathbf{q})^\top\,\Delta \mathbf{q},\\ &\text{s.t.}\quad c_i(\mathbf{q}) + \nabla c_i(\mathbf{q})^\top\,\Delta \mathbf{q} \;\le\; 0,\quad i=1,\dots,m. \end{aligned} $$

   其中 $B$ 是某种 Hessian 的近似（例如 BFGS 更新得到的拟牛顿 Hessian），或直接使用目标的精确 Hessian。

2. 迭代流程

   • 初始化 $\mathbf{q}^{(0)}$（来自前端插值结果），构建初始约束值 $c_i(\mathbf{q}^{(0)})$。

   • 在第 $k$ 次迭代：

     1. 用当前 $\mathbf{q}^{(k)}$ 计算 $\nabla J$ 与 $\nabla c_i$。
     2. 构造上述 QP 子问题，求解得到修正步长 $\Delta \mathbf{q}^{(k)}$。
     3. 做线性搜索或信赖域更新，得到下一点 $\mathbf{q}^{(k+1)} = \mathbf{q}^{(k)} + \alpha\,\Delta \mathbf{q}^{(k)}$，并更新 Hessian 近似。
     4. 判断是否收敛：若 $\|\Delta \mathbf{q}^{(k)}\|$ 与 KKT 条件均满足阈值，则停止。

优缺点

• 优点

  1. 收敛速度快：SQP 本质上在可行域内做二次近似，往往每一步能显著降低目标值，且对带稀疏结构的 Hessian 矩阵有良好收敛性。
  2. 天然处理约束：所有不等式约束 $c_i(\mathbf{q})\le 0$ 都可以当作硬约束直接在线性子问题里处理，最终能保证“凸包碰撞”、“最低/最高速度”、“最大加速度/曲率”之类绝对满足。（只要 QP 子问题可行，整个迭代就始终保持可行。）
  3. 易于扩展新约束：如果后续想加上“航向角约束”或“高度限制”，只需往 $c_i(\mathbf{q})$ 中添加对应表达式即可，无需重写整个框架。

• 缺点

  1. 实现复杂度较高：需要构建准确的梯度和 Hessian（或其近似），并调用高效的 QP 求解器。
  2. 对初始点敏感：若初始 $\mathbf{q}^{(0)}$ 距离可行域边界太近（例如某个四点凸包已经碰撞，约束 $c_i(\mathbf{q}^{(0)})>0$），可能导致子问题不可行或难以收敛。
  3. 资源占用相对大：每一步都要求解一个 QP，若控制点维度很大（几百维往上），QP 子问题也会比较大，单次迭代耗时较高。

适用场景

• 真实系统中要求“绝对避障”、曲率、速度严格满足固定翼动力学，不能留有软性惩罚失败的余地。
• 算力足够，或可利用稀疏 QP 求解器加速（如 OSQP、qpOASES 等），能在十几毫秒到百来毫秒内完成一次 QP 求解。
• 对轨迹优化结果有较高精度要求，且对实时性有一定容忍度（整个规划连接在一起通常可在几百毫秒到几秒完成）。

3. 交替最小二乘（Alternating Least Squares）

原理与实现思路

1. 分两步迭代：

   • 步骤 A（平滑子问题）：先固定“碰撞/曲率”软约束，对目标中仅包含 $f_{\text{smooth}}$ 与 $f_v$、$f_a$、$f_\kappa$ 的项做二次最小二乘（或带二次锥约束）求解；
   • 步骤 B（碰撞/曲率修正子问题）：再固定刚刚得到的平滑解，检查凸包碰撞和曲率约束，对那些违反约束的控制点段局部做拉扯/修正。

2. 循环往复，直到“平滑”和“避障/曲率”两方面都满足，或者收敛到一个兼顾二者的折中解。

优缺点

• 优点

  1. 分段处理各自子目标：把平滑度与避障约束“解耦”，每一步只需关心成本里的一小部分，编程实现时较为直接。
  2. 可以利用快速的二次最小二乘库：第一步只要求解带固定权重的二次型优化，多数成熟库（Eigen、Ceres、GTSAM）都能快速完成。
  3. 逐步改进：当环境障碍不太密集时，往往第一步就能得到一个很平滑的轨迹，再做少量碰撞拉扯，就能满足安全要求。

• 缺点

  1. 收敛不保证全局最优：由于“平滑”与“避障”分别分开求解，容易陷入局部折中：在 A 步骤中忽略了碰撞，B 步骤可能需要大幅拉扯，拉扯后平滑又被破坏，需要再回到 A，循环反复且不易收敛。
  2. 对碰撞往往只能做局部修正：B 步骤只针对违反约束的局部片段做移动，可能引入新的速度/曲率违背，需要多次往返修正。
  3. 实现难度适中：虽说单步子问题简单，但要保证交替多步都收敛，需要较多启发式策略（例如在 B 步骤只允许小幅度拉扯、或在 A 步骤加大对曲率的惩罚），调参较麻烦。

适用场景

• 对实时性要求极高，但对最优性要求不严格的场合。
• 环境障碍较稀疏、曲率要求中等，可以容忍“平滑—碰撞—平滑”几轮折中后得到一个可行解。
• 如果只需快速“在线修正”已有轨迹的少量局部违例段，可以把大规模优化拆成若干小片做交替。

4. 使用成熟的非线性优化库（如 IPOPT、NLopt、CasADi、SNOPT 等）

原理与实践

1. 直接构造完整优化模型

   • 将所有自由控制点坐标 $\mathbf{q}$ 设为决策变量（变量维度 $2\times(\!M-5)$），
   • 直接在库里声明成本函数 $J(\mathbf{q})$（可用自动微分或手动推导 Hessian/Jacobian），
   • 直接声明不等式约束 $c_i(\mathbf{q}) \le 0$（包括“$\|Q_{j+1}-Q_j\|/\Delta t \ge \bar v_{\min}$”、“凸包距离 $\min d_j^\ell - (r_{j,j+1} + r_{j+1,j+2} + r_{j+2,j+3}) > 0$” 等），以及任何其他曲率与加速度限制。

2. 调用黑箱求解器

   • 类似于调用 IPOPT：

     import ipopt
     n = 2*(M-5)  # 自由变量维度
     def obj(q): return J(q)            # 返回标量
     def grad(q): return ∇J(q)           # 返回梯度向量
     def cons(q): return [c_1(q),…, c_m(q)]  # 返回长度 m 的约束向量
     def jac(q): return JacobianMatrix(q)   # 返回 m×n 稀疏矩阵
     solver = ipopt.problem(
         n=n, m=m,
         problem_obj=obj, problem_grad=grad,
         problem_cons=cons, problem_jac=jac,
         lb=[…], ub=[…],   # 变量界限
         cl=[…, …], cu=[…, …]  # 约束范围
     )
     q_opt, info = solver.solve(q_init)

   • 求解器自动处理 KKT 条件、线性化子问题、Hessian 更新与线性子问题求解，最后输出可行的 $\mathbf{q}^*$ 与拉格朗日乘子信息。

优缺点

• 优点

  1. 成熟稳定、参数少：只需编写一次目标与约束表达式，求解器自动做可行域维护、自动微分、稀疏矩阵处理等，开发工作量大大降低。
  2. 支持稀疏结构：大多数库都能自动识别并利用 Hessian/Jacobian 的稀疏性，适合数百到上千维的控制点优化。
  3. 硬约束保证：只要给出精确的约束表达式，求解器会严格在可行域内搜索，最后输出的轨迹必然满足“凸包无碰撞”、“速度/加速度/曲率硬性上限”这一类不可妥协的条件。
  4. 容易集成与切换：若后续需要改用 SNOPT、KNITRO 等商用解算器，只需改建 solver 接口，多数代码无需改动。

• 缺点

  1. 对分辨率敏感：当插值后控制点数量 $M$ 很大（几百或上千），每一步迭代都要处理大小为 $n\times n$ 的 Hessian 近似或 $m\times n$ 的雅可比矩阵，内存与计算开销显著上升。
  2. 求解时间不可控：虽然收敛性好，但若初始点处于边界很近的位置，或者障碍物约束非常紧，解算器可能需要多轮线性化与回溯搜索，单次优化可能花费数百毫秒到几秒。对极限实时规划场景可能不够快。
  3. 黑盒调试难：当优化失败或收敛到不合理解时，排查“到底是哪条约束搞死了”、“哪个 Hessian 条件数太差”比较困难，需要深入 Solver 日志甚至源码。

5. 综合对比与推荐

推荐策略

1. 若需要在工程系统里部署、对“绝对避障”和“固定翼最小回转半径”要求严格，强烈推荐使用成熟的非线性优化库（如 IPOPT、SNOPT、KNITRO）：

   • 理由：它们能自动维护 KKT 可行性，保证最终解满足所有硬性约束（最低速度、最大速度、最大加速度、最大曲率、凸包碰撞等）；同时对稀疏 Hessian/Jacobian 求解有良好优化，即使控制点多，也能在合理时间内收敛。

   • 实现建议：

     1. 手动推导并实现代价函数和各不等式约束的雅可比（Jacobian）与 Hessian。
     2. 充分利用自动微分库（如 CasADi）来生成符号梯度和 Hessian，使代码更简洁。
     3. 调整容忍度（Tolerance）和迭代次数上限，保证在线性化与罚函数切换时稳健。

2. 若对实时性要求非常高、障碍稀疏，且能接受“近似无碰撞”方案，可选SQP或交替最小二乘：

   • SQP：适合中等规模（数十到一两百维控制点）场景。每次迭代需解一个 QP，若使用高效稀疏 QP 解算器（OSQP、qpOASES），并把不等式约束中“碰撞”与“曲率”只保留最关键片段，就能在十几毫秒至几十毫秒内完成一次迭代。对在线二次优化并逐步精化很有效。
   • 交替最小二乘：当“避障碰撞”在整体轨迹里只占极小一部分时，可先经过一次全局平滑（只关注平滑+动力学惩罚），再对碰撞片段做局部插值与拉扯。如果大部分轨迹区域障碍稀疏且环境宽敞，就能在非常短时间内得到基本可行的轨迹。

3. 纯梯度下降 / 拟牛顿

   • 较少用于严格工程化场景，可作为研究验证或快速原型。若将所有硬性约束都做成高权重的软性惩罚，一旦初始解与可行域边界尚有足够余量，常规 L-BFGS 或 Adam 优化也能快速收敛到一个“足够安全”的解。
   • 但若初始插值后“有段凸包已经碰撞”，你必须在每次迭代时动态检测并“手动 projecting”（将控制点移动到可行域内），或不断以“小步长”优化，这会拖慢收敛速度。

6. 最终建议

• 若项目对“绝无碰撞、严格满足固定翼机动学极限”有硬性要求，使用成熟的非线性优化库（IPOPT、SNOPT、KNITRO 等）配合自动微分，让求解器直接处理所有硬性约束。只需：

  1. 定义好“目标函数 $J(\mathbf{q})$”与每个非线性约束 $c_i(\mathbf{q}) \le 0$。
  2. 将问题输入 IPOPT，设置合适的 Lagrange 容忍度、最大迭代次数、初始步长等；
  3. 等待迭代输出最终 $\mathbf{q}^*$。

  这样能保证——只要给出一个 初始可行解（前端插值阶段尽量让 $\{Q_j\}$ 满足最小速度和凸包安全），IPOPT 就能在严格可行域内收敛到一个高质量解。

• 若对实时性要求极端苛刻，但环境相对简单，允许一定近似，可以选 SQP + 稀疏 QP 解算器：

  1. 构造一个“速度+加速度+曲率+碰撞”约束的“线性化 QP”。
  2. 每次迭代用 OSQP/qpOASES 解 QP，更新控制点。
  3. 只做很少次迭代，甚至每隔若干帧才做一次完整优化，其余时刻使用上一帧轨迹插值执行。

• 若目前仅需“验证思路”或“跑一下样例”，可以先用 L-BFGS 类型的梯度下降：

  • 将所有约束做成“高权重罚项”，手动实现或用 PyTorch/Ceres 一类库做自动微分；
  • 先取得一个大致平滑可行的轨迹，再针对最严重的几处违反进行局部精修。

综合结论：

对于固定翼 B 样条避障，最推荐的方案是基于 成熟非线性优化库（IPOPT、SNOPT、KNITRO 等），因为它能自动处理所有约束（硬约束与软惩罚并存），具有较好收敛性与工程可靠性。若项目对实时性要求更高，可在此基础上引入稀疏 SQP 或分段交替最小二乘等策略，将“全局严格优化”与“在线快速修正”相结合。