无人机避障轨迹规划：SCP逐次凸化原理与Matlab实现

做无人机避障轨迹规划的时候最让人头疼的往往不是动力学方程本身而是那一堆把可行域切得乱七八糟的非凸约束。无人机要绕开楼、躲开树避障约束写成||p - p_obs|| r_safe的样子。这个式子看着直观但它在数学上恰好是个凹约束——可行域相当于整个空间挖掉一个球属于非凸问题的典型代表普通凸优化器看到它就罢工了。后来我在无人机轨迹优化项目里换用 SCPSuccessive 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 的流程可以概括成三个反复循环的步骤在参考轨迹处把非线性动力学线性化把凹的避障约束线性化构造一个带有信赖域约束的凸子问题用凸优化求解器解出当前最优轨迹把参考轨迹更新为解出的新轨迹再重复上面两步直到收敛。打个比方就明白了假设你站在山顶想下山又看不太清整条山路那就先朝目测方向走一小步到新的位置再看、再走一小步。每一步都只根据当前可见的局部信息决策走几步之后回头发现自己已经绕过了山腰的大石头到了山脚。SCP 就是把这个“走一步看一眼”的思路用数学形式执行出来。这里面最关键的细节是每次线性化只在参考轨迹附近可信所以不能一步跨太远。这就是信赖域约束存在的理由。2.2 三大支柱凸化、信赖域、松弛SCP 能稳定工作靠的是三个机制配合凸化是把非线性动力学和凹约束变成线性的。非线性动力学线性化后得到仿射等式约束凹约束线性化后得到半空间不等式约束。目标和剩下的大部分约束本来就是凸的于是整个子问题变成一个标准的凸优化问题二次规划。松弛变量解决约束不可行的问题。比如直线初始轨迹穿过了障碍物那么在第一次迭代里避障约束可能无解。这时允许引入非负的松弛变量 s让约束临时被轻微违反代价是在目标函数里加一个大的惩罚项。随着迭代推进参考轨迹被推离障碍物松弛量自然就趋于零这样既保住了求解的可行性又不会让最终轨迹真的穿障。2.3 与其它方法的关系SCP 不是万能的但很实用做轨迹优化时常被提到的还有 LQR、MPC、直接用 NLP 求解器等。它们之间的区别简单说方法适用范围特点LQR线性系统、小扰动速度快但处理不了大范围避障MPC在线滚动控制能在线执行但每一步还是要一个优化核心NLP 求解器一般非线性优化建模灵活但对初值敏感求解结果不稳定SCP非凸轨迹优化每步都是凸子问题求解有保证对初值要求相对宽松我在实际项目里常常把 SCP 当作离线轨迹规划的核心来用生成一条安全平滑的参考轨迹再交给底层的 MPC 或飞控去跟踪。两者分工明确整体效果很稳。3. 建模实操把无人机运动约束写成可求解的数学语言3.1 状态和控制变量怎么选我用的状态是六维的x [px, py, pz, vx, vy, vz] u [ax, ay, az]运动模型类似点质量为无人机模型的二阶积分器。选这个模型不是偷懒是因为轨迹规划层本来就不该管姿态角、扭矩这类底层细节那部分是飞控姿态环负责的。规划层输出加速度指令飞控去跟踪这种分层设计在工程上是成熟的主流做法。3.2 离散化时间栅格怎么取把一段总时间 T 等分成 N-1 段每段长度 dt。位置和速度用梯形法离散p_{k1} p_k 0.5 * dt * (v_k v_{k1}) v_{k1} 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)第一项让轨迹尽量平滑、不消耗过多能量第二项强迫新的解不能离上一轮参考轨迹太远和信赖域约束一起保证线性化模型的有效性。最终每个凸子问题的完整约束清单如下约束类型数学表达说明动力学等式梯形法离散方程线性等式约束起点终点x_1 x_start, x_N x_goal边界约束速度限幅||v_k|| v_max凸约束需近似或 SOCP加速度限幅||u_k|| u_max凸约束需近似或 SOCP避障约束n_k^T (p_k - p_obs) r_obs margin线性约束信赖域约束||x_k - x_ref,k|| delta线性约束后文主代码里用的是相对保守的切比雪夫近似把速度、加速度的范数约束拆成每个分量都限幅。如果装了二阶锥求解器可以直接换成 SOCP 版本效果更精确。4. Matlab 实现全过程与代码骨架4.1 变量组织一个大向量搞定所有决策量实现 SCP 的二次规划子问题时第一步是把所有状态量和控制量拼成一个大向量 zz [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 bH 矩阵由两部分组成控制能量项是标准二次型信赖域项是对角占优矩阵。线性项 f 主要来自信赖域惩罚里的-2 * rho * x_ref。注意 H 必须是对称半正定的。动力学等式约束通过 Aeq 写入。对第 k 段时间p_{k1} - p_k - 0.5*dt*v_k - 0.5*dt*v_{k1} 0 v_{k1} - 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说明线性化失真了必须缩小 δ。我用的是rho 范围操作rho 0.75delta delta * 1.5rho 0.25delta delta * 0.5其它保持不动收敛判断我习惯同时看三个条件目标函数相对变化足够小、最大约束违反量小于阈值、δ 小于阈值。三个条件满足任意一个可接受匹配的速度更快。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_{k1} p_k 0.5*dt*(v_k v_{k1}) 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_{k1} 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(end1, :) row; b(end1) 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(end1, :) eq_start; beq(end1) 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 最实用的地方在于它不挑场景障碍物不管是规则球体还是任意凸多面体都能通过线性化塞进同一个框架里。每完成一次迭代看到轨迹真的往安全侧“挤”过去整个过程还是很有成就感的。最后再分享一个小技巧每次迭代都把目标函数值和最大约束违反量打印出来形成一条单调下降的曲线如果某个时刻曲线突然翘头不用怀疑一定是某个参数越界了回头检查初值和信赖域半径基本就能解决问题。