企业官网建设流程全解析

做无人机避障轨迹规划的时候，最让人头疼的往往不是动力学方程本身，而是那一堆把可行域切得乱七八糟的非凸约束。无人机要绕开楼、躲开树，避障约束写成||p - p_obs|| >= r_safe的样子。这个式子看着直观，但它在数学上恰好是个凹约束——可行域相当于整个空间挖掉一个球，属于非凸问题的典型代表，普通凸优化器看到它就罢工了。后来我在无人机轨迹优化项目里换用 SCP（Successive Convexification，逐次凸化）的思路，把非凸问题按迭代方式切成一个个凸子问题，丢给 Matlab 的 quadprog 去解，逐步逼近真实最优轨迹，整条链路顺畅了很多。

这篇笔记是当时从建模、约束处理、代码落地到调参踩坑的完整记录，适合正在做无人机路径规划、机器人轨迹优化、或者研究序列凸优化的同学参考。内容包括 SCP 的核心原理、凸化推导、Matlab 代码骨架，以及几个不亲自动手很难发现的坑。想直接看代码的话，可以跳到第 4 节。

1. 无人机轨迹优化难在哪：先看清这个问题

1.1 动力学层面只是门槛，不是真正的坎

无人机轨迹优化本质上要做的事是：给定起点、终点，在避开环境障碍、满足物理限制的前提下，找出一条满足动力学方程的最优运动轨迹。

多旋翼无人机在位置层面一般被近似成二阶积分器，我习惯用状态x = [px, py, pz, vx, vy, vz]^T表示位置和速度，用控制u = [ax, ay, az]^T表示加速度指令。这样的运动模型本身并不复杂，优化时把位置、速度按时间步离散成传动关系即可。

真正让问题变难的不是动力学方程，而是约束。约束一多，可行域就被切得越来越碎，加上动力学方程和非凸约束混在一起，整个问题在数学上属于非线性非凸优化，直接用通用非线性求解器上去解，往往解不动、收敛慢，还对初值非常敏感。

1.2 障碍物约束是非凸性的主要来源

标题里的“路径规划”不是几何找路，而是要在优化框架里把避障写成约束。避障约束写成：

||p_k - p_obs|| >= r_obs

意思是无人机到障碍物球心的距离必须大于安全半径。问题是，这个不等式定义的可行域是一个“全空间去掉一个球”的区域，它不满足凸性。类似的问题还有禁飞区、转弯半径限制、视线遮挡约束，全都可能把可行域变成非凸的。

对于这类约束，很多人会想到用 A*、RRT 之类的搜索算法先把路径找出来。这个方法确实能避开障碍，但搜出来的是离散路径点，既不满足动力学，也不会自动变平滑，还需要大量后处理。而直接做轨迹优化，又会被非凸约束卡住。SCP 的思路是把两者结合起来：先有一条粗略初值，然后通过“凸化 + 迭代”的方式做局部优化。

1.3 我们需要的是“稳定收敛”，不只是“能搜到”

做工程和做算法研究有一个很大的区别：算法领域喜欢提完备性、最优性的保证，但在无人机这种实际任务里，我更关心的是“在有限时间内稳定地得到一条能飞、够平滑、不碰障碍的轨迹”。

A* 能找到路径但不平滑，RRT* 理论上能满足动力学约束但计算成本高、路径也不够干净。非线性优化虽然能直接得到满足动力学、目标最优的轨迹，但非凸优化求解器在处理障碍物约束时，经常陷在“找不动”的状态里。

SCP 站在两者之间：先用直线插值或搜索算法给一条初始轨迹，再通过反复“线性化 + 求解凸子问题 + 更新参考轨迹”的方式把轨迹“挤”到可飞行区域里。它不保证全局最优，但能在可接受的时间给出满足约束的局部最优解。对无人机来说，这个性质比理论最优更实用。

2. SCP 算法核心流程拆解

2.1 核心思想：把难问题“软化”成一系列易问题

SCP 的流程可以概括成三个反复循环的步骤：

1. 在参考轨迹处，把非线性动力学线性化，把凹的避障约束线性化；
2. 构造一个带有信赖域约束的凸子问题，用凸优化求解器解出当前最优轨迹；
3. 把参考轨迹更新为解出的新轨迹，再重复上面两步，直到收敛。

打个比方就明白了：假设你站在山顶想下山，又看不太清整条山路，那就先朝目测方向走一小步，到新的位置再看、再走一小步。每一步都只根据当前可见的局部信息决策，走几步之后回头发现自己已经绕过了山腰的大石头，到了山脚。SCP 就是把这个“走一步看一眼”的思路用数学形式执行出来。

这里面最关键的细节是：每次线性化只在参考轨迹附近可信，所以不能一步跨太远。这就是信赖域约束存在的理由。

2.2 三大支柱：凸化、信赖域、松弛

SCP 能稳定工作，靠的是三个机制配合：

凸化是把非线性动力学和凹约束变成线性的。非线性动力学线性化后得到仿射等式约束，凹约束线性化后得到半空间不等式约束。目标和剩下的大部分约束本来就是凸的，于是整个子问题变成一个标准的凸优化问题（二次规划）。

松弛变量解决约束不可行的问题。比如直线初始轨迹穿过了障碍物，那么在第一次迭代里避障约束可能无解。这时允许引入非负的松弛变量 s，让约束临时被轻微违反，代价是在目标函数里加一个大的惩罚项。随着迭代推进，参考轨迹被推离障碍物，松弛量自然就趋于零，这样既保住了求解的可行性，又不会让最终轨迹真的穿障。

2.3 与其它方法的关系：SCP 不是万能的，但很实用

做轨迹优化时，常被提到的还有 LQR、MPC、直接用 NLP 求解器等。它们之间的区别简单说：

我在实际项目里常常把 SCP 当作离线轨迹规划的核心来用，生成一条安全平滑的参考轨迹，再交给底层的 MPC 或飞控去跟踪。两者分工明确，整体效果很稳。

3. 建模实操：把无人机运动约束写成可求解的数学语言

3.1 状态和控制变量怎么选

我用的状态是六维的：

x = [px, py, pz, vx, vy, vz] u = [ax, ay, az]

运动模型类似点质量为无人机模型的二阶积分器。选这个模型不是偷懒，是因为轨迹规划层本来就不该管姿态角、扭矩这类底层细节，那部分是飞控姿态环负责的。规划层输出加速度指令，飞控去跟踪，这种分层设计在工程上是成熟的主流做法。

3.2 离散化：时间栅格怎么取

把一段总时间 T 等分成 N-1 段，每段长度 dt。位置和速度用梯形法离散：

p_{k+1} = p_k + 0.5 * dt * (v_k + v_{k+1}) v_{k+1} = v_k + u_k * dt

欧拉法是一阶精度，同样的时间步长会产生更大的模型误差；梯形法用区间两端的速度做平均，精度明显更高。更重要的是，梯形法对状态变量仍然完全是线性的，不会给优化问题增加额外负担。

时间步长 dt 的选择有个常用经验：对几十米的飞行范围，dt 取 0.2~0.5 秒、N 取 30~60 个点，通常就能兼顾约束分辨率与计算速度。dt 太大，障碍物约束在两段时间之间产生非常大的跳变，细长障碍可能被“跳过去”；dt 太小，维度膨胀，quadprog 求解变慢。

3.3 避障约束的凸化推导

避障约束是整个 SCP 里最值得手工推导一遍的地方。原始约束：

||p_k - p_obs|| >= r_obs

它在第 k 个时间步的梯度信息依赖参考点 p_ref,k。定义：

d_k = p_ref,k - p_obs n_k = d_k / ||d_k||

然后对凹约束做线性化。经过整理，约束可以化简成：

n_k^T (p_k - p_obs) >= r_obs

这个形式的含义很直观：在参考点附近，避障约束变成一个半平面约束，无人机必须待在一个以障碍物球心为中心、朝向参考方向的平面外侧。每次迭代参考轨迹变了，n_k 也跟着变，半平面绕障碍物“旋转”，最终把轨迹推到真正安全的一侧。

具体推导过程其实不复杂。考虑函数g(p) = r_obs - ||p - p_obs||，原始约束就是g(p) <= 0。在 p_ref 处做一阶泰勒展开：

g(p) ≈ g(p_ref) + ∇g(p_ref)^T (p - p_ref) <= 0

其中∇g(p_ref) = -n_k。代入整理，最后得到的正是n_k^T (p_k - p_obs) >= r_obs。这个结果非常干净，写代码的时候直接用这个线性约束就好，不需要在约束矩阵里展开复杂的非线性函数。

3.4 目标函数与约束汇总

目标函数我一般取控制能量最小，并加上信赖域惩罚：

min J = sum(0.5 * dt * ||u_k||^2) + rho * sum(||x_k - x_ref,k||^2)

第一项让轨迹尽量平滑、不消耗过多能量；第二项强迫新的解不能离上一轮参考轨迹太远，和信赖域约束一起保证线性化模型的有效性。

最终每个凸子问题的完整约束清单如下：

后文主代码里用的是相对保守的切比雪夫近似，把速度、加速度的范数约束拆成每个分量都限幅。如果装了二阶锥求解器，可以直接换成 SOCP 版本，效果更精确。

4. Matlab 实现全过程与代码骨架

4.1 变量组织：一个大向量搞定所有决策量

实现 SCP 的二次规划子问题时，第一步是把所有状态量和控制量拼成一个大向量 z：

z = [x_1; x_2; ...; x_N; u_1; u_2; ...; u_{N-1}]

每个 x_k 占 6 个连续变量，每个 u_k 占 3 个连续变量。总变量数为：

dim = 6*N + 3*(N-1)

写代码时用索引函数来定位，避免后续组装矩阵时算错位置。

4.2 目标矩阵和约束矩阵的组装

二次规划的标准形式是：

min 0.5 * z^T * H * z + f^T * z subject to Aeq * z = beq A * z <= b

H 矩阵由两部分组成：控制能量项是标准二次型，信赖域项是对角占优矩阵。线性项 f 主要来自信赖域惩罚里的-2 * rho * x_ref。注意 H 必须是对称半正定的。

动力学等式约束通过 Aeq 写入。对第 k 段时间：

p_{k+1} - p_k - 0.5*dt*v_k - 0.5*dt*v_{k+1} = 0 v_{k+1} - v_k - dt*u_k = 0

避障不等式通过 A b 写入，每条约束对应一个半平面不等式。每个时刻、每个障碍物各占一行矩阵。

4.3 SCP 主循环与信赖域更新

SCP 迭代的核心逻辑可以写成下面这个流程：

% 伪代码：SCP 主循环 for iter = 1:maxIter % 1. 根据当前参考轨迹 Xref, Uref 组装 H, f, Aeq, beq, A, b % 2. 调用 quadprog 求解凸子问题 % 3. 得到新解 Xnew, Unew % 4. 计算真实非线性约束违反量 % 5. 根据实际下降量与预测下降量的比值，调整信赖域半径 delta % 6. 判断是否收敛：若满足则终止 % 7. 更新 Xref = Xnew, Uref = Unew end

信赖域半径 δ 的更新是控制收敛行为的关键。经典策略是计算：

rho = (实际目标函数下降量) / (预测目标函数下降量)

rho 接近 1，说明线性模型很准确，可以放心扩大 δ；rho 如果明显小于 1，说明线性化失真了，必须缩小 δ。我用的是：

收敛判断我习惯同时看三个条件：目标函数相对变化足够小、最大约束违反量小于阈值、δ 小于阈值。三个条件满足任意一个可接受，匹配的速度更快。

4.4 一个可以直接跑的避障规划示例

下面按我的经验给出一份能跑的 Matlab 示例骨架，场景是三维空间里有两个球形障碍物，无人机要从起点飞到终点。

clear; clc; close all; % 场景参数 P0 = [0; 0; 5]; PT = [30; 20; 8]; obs = [10, 10, 5, 3; % x, y, z, r 20, 8, 5, 2.5]; % 时间栅格 dt = 0.2; N = 41; T = dt * (N - 1); % 初始参考轨迹：直线插值 Xref = zeros(6, N); Uref = zeros(3, N-1); for k = 1:N tau = (k-1) / (N-1); Xref(1:3, k) = P0 + (PT - P0) * tau; end % SCP 参数 rho = 0.5; delta = 5.0; maxIter = 30; margin = 0.5; tol = 1e-3; vmax = 10; % 速度限幅 umax = 5; % 加速度限幅 % 变量总维度 dim = 6*N + 3*(N-1); % 主循环 for iter = 1:maxIter % 组装 H, f H = sparse(dim, dim); f = zeros(dim, 1); for k = 1:N-1 u_idx = 6*N + (k-1)*3 + (1:3); H(u_idx, u_idx) = dt * eye(3); end % 信赖域惩罚项 for k = 1:N x_idx = (k-1)*6 + (1:6); H(x_idx, x_idx) = H(x_idx, x_idx) + 2*rho*eye(6); f(x_idx) = -2*rho * Xref(:, k); end % 动力学等式约束 Aeq Aeq = sparse(6*(N-1), dim); beq = zeros(6*(N-1), 1); for k = 1:N-1 rows = (k-1)*6 + (1:6); xk_idx = (k-1)*6 + (1:6); xk1_idx = k*6 + (1:6); uk_idx = 6*N + (k-1)*3 + (1:3); % p_{k+1} = p_k + 0.5*dt*(v_k + v_{k+1}) Aeq(rows(1:3), xk_idx(1:3)) = -eye(3); Aeq(rows(1:3), xk_idx(4:6)) = -0.5*dt*eye(3); Aeq(rows(1:3), xk1_idx(1:3)) = eye(3); Aeq(rows(1:3), xk1_idx(4:6)) = -0.5*dt*eye(3); % v_{k+1} = v_k + dt*u_k Aeq(rows(4:6), xk_idx(4:6)) = -eye(3); Aeq(rows(4:6), xk1_idx(4:6)) = eye(3); Aeq(rows(4:6), uk_idx) = -dt*eye(3); end % 避障约束 A, b A = []; b = []; for k = 1:N xk_idx = (k-1)*6 + (1:3); p_ref = Xref(1:3, k); for oi = 1:size(obs, 1) p_obs = obs(oi, 1:3)'; r_obs = obs(oi, 4); d_k = p_ref - p_obs; n_k = d_k / norm(d_k); row = zeros(1, dim); row(xk_idx) = n_k'; A(end+1, :) = row; b(end+1) = r_obs + margin; end end % 上下界 lb = -inf(dim, 1); ub = inf(dim, 1); for k = 1:N xk_idx = (k-1)*6 + (1:6); lb(xk_idx) = reshape(Xref(:, k) - delta, [], 1); ub(xk_idx) = reshape(Xref(:, k) + delta, [], 1); end % 速度和加速度分量限幅 for k = 1:N v_idx = (k-1)*6 + (4:6); lb(v_idx) = max(lb(v_idx), -vmax); ub(v_idx) = min(ub(v_idx), vmax); end for k = 1:N-1 u_idx = 6*N + (k-1)*3 + (1:3); lb(u_idx) = -umax; ub(u_idx) = umax; end % 起点终点约束 eq_start = zeros(1, dim); eq_start(1:6) = 1; Aeq(end+1, :) = eq_start; beq(end+1) = norm(P0); % 注：以上写法示意，实际应按分量约束 % 调用 quadprog opts = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'interior-point-convex'); [z, ~] = quadprog(H, f, A, b, Aeq, beq, lb, ub, [], opts); % 解码 Xnew = zeros(6, N); Unew = zeros(3, N-1); for k = 1:N Xnew(:, k) = z((k-1)*6 + (1:6)); end for k = 1:N-1 Unew(:, k) = z(6*N + (k-1)*3 + (1:3)); end % 收敛判断 delta_x = norm(Xnew(:) - Xref(:), Inf); if delta_x < tol Xref = Xnew; Uref = Unew; break; end % 更新参考轨迹 Xref = Xnew; Uref = Unew; end

这块代码重点是展示变量索引和矩阵组装的骨架。起点终点约束在示例里只写了示意，实际应该按分量分别约束（位置分量、速度分量逐条写入），否则等式约束不够严谨。障碍物、信赖域更新等逻辑都是可以直接用的。

在实际项目里，为了跟实验室后续的飞控对接，我通常会在 SCP 结束后再加一层 B 样条平滑，把离散轨迹拟合成连续的轨迹曲线，再采样下发。这个后处理很简单，但能把轨迹质量提升一大截，建议你也加上。

5. 参数调节与收敛性经验

5.1 信赖域初值和更新倍率怎么定

δ 的初始值对收敛影响很大。我一般先根据飞行距离估算：几十米范围的场景，δ 从 5 开始比较保险；如果场景里障碍物密集、通道窄，δ 初始给 2 更稳。δ 太大，第一次迭代就会产生明显偏离参考轨迹的解，线性化失真，然后就开始振荡；δ 太小，每次只能挪一小步，要迭代几十轮才能收敛。

更新倍率按上面表格的操作来就有不错的效果。值得提醒的是，缩小 δ 的倍率可以设得比扩大更激进，比如缩小一半、扩大 1.2 倍，防止震荡比加快收敛更重要。

5.2 初始轨迹直接影响成败

直线插值是最简单的初始化方式，但它穿过障碍物时，第一次迭代的避障约束可能不可行，需要松弛变量兜底。如果不想引入松弛变量，可以先用 RRT 之类的算法快速找一条无碰路径，再把离散路径点插值成轨迹作为初值。

我测试下来的经验：障碍物多的时候，用 RRT 初值的 SCP 收敛速度明显快于直线初值，而且不容易出现“轨迹绕远路”的局部最优。障碍物少、场景空旷时，直线初值完全够用，省去搜索的麻烦。

5.3 N 和 dt 的折中

N 越大、dt 越小，凸子问题的规模越大，quadprog 求解耗时也越长。无人机轨迹规划一般要求在几百毫秒到几秒内算完，所以 N 取 30~60 是合理范围。

有一个容易被忽略的点：障碍物约束只加在离散时刻点上，轨迹在离散点之间是有可能穿过细长障碍物的。如果场景里存在很薄的墙或者细杆，要么把 dt 调小，要么把安全 margin 加大，否则检查最终轨迹时会被“钻孔”问题吓一跳。

5.4 约束违反量的处理

即使 SCP 迭代收敛，也要在最后单独检查一次所有非凸约束的真实违反量，而不是只看凸子问题里的线性化约束。因为线性化约束只是近似，真实轨迹可能仍有微小穿障。我的做法是在收敛后固化轨迹，重新用原始障碍物方程逐点检查，如果 violation 超过容差，就加大 margin 再跑一次。这一步在工程交付时非常关键，别偷懒。

6. 常见问题与调试技巧实录

6.1 quadprog 报 Infeasible

排查思路是：检查变量维度是否匹配，Aeq 和 A 矩阵里有没有 NaN、Inf；检查边界约束、信赖域上下界与动力学约束、起始点终点约束是否自相矛盾；如果初值穿越障碍物，确认是否引入了松弛变量。

我踩过的坑是信赖域 lb/ub 的维数写错，导致 quadprog 内部变量数和约束矩阵列数不一致。这类问题通常靠size(Aeq,2) == dim就能提前挡住。

6.2 轨迹贴着障碍物飞，甚至微穿越

解决思路分两层。一是加大安全 margin。障碍物建模的半径本来就带着测量误差，margin 加 0.5 米相对常见。二是调整线性化的偏置方向。避障约束线性化后是一个半平面，把半平面的位置向安全侧多推一点点，轨迹就不会那么紧贴障碍物表面。

6.3 目标值下降很慢，迭代好多轮都收敛不了

先降低 rho，让信赖域项的影响力弱一点，给目标函数更多“自由”。再检查 δ 是否被设得太小，导致每步挪动太少。最后看初值质量，如果初值离可行域太远，SSCP 前几轮都在做“把轨迹拉回来”的工作，下降慢是正常的，可以多给几轮迭代。

6.4 数值病态与量纲问题

无人机轨迹里位置动辄几十米、速度几十米每秒、加速度几个 g，这些量级差在优化矩阵里会被放大成病态矩阵，quadprog 偶尔会报数值警告。处理办法是把位置、速度、加速度统一到米制单位后，再按场景范围做一次缩放。比如把坐标轴除以 50，让位置变量落在 0~1 范围内，求解稳定性会好很多。

另外我习惯全程用稀疏矩阵，quadprog 对大规模稀疏问题支持很好，能省下不少内存和计算时间。如果用普通稠密矩阵，N 一旦过百，求解速度会明显吃不消。

按我个人实际操作的感觉，SCP 最实用的地方在于它不挑场景，障碍物不管是规则球体还是任意凸多面体，都能通过线性化塞进同一个框架里。每完成一次迭代，看到轨迹真的往安全侧“挤”过去，整个过程还是很有成就感的。最后再分享一个小技巧：每次迭代都把目标函数值和最大约束违反量打印出来，形成一条单调下降的曲线；如果某个时刻曲线突然翘头，不用怀疑，一定是某个参数越界了，回头检查初值和信赖域半径基本就能解决问题。