做无人机轨迹优化最头大的不是写代码而是问题本身挂满了“非凸”标签。避障要求离障碍够远动力学要求加速度有上下限飞行时间又要尽可能短这些条件凑在一起直接丢给非线性求解器往往要么不收敛要么陷在局部最优里出不来。我折腾过好几条路线最终长期使用的是SCP算法Successive Convexification逐次凸化——把原始非凸问题拆成一连串凸子问题每轮用Matlab求解一次凸优化迭代逼近原问题的解。这篇就把原理、推导、代码和踩坑记录一起放出来标题里那套“无人机路径规划轨迹优化Matlab”的组合在这篇文章里会一步步落地成能跑的代码。这套方法不只玩无人机适用喷漆路径规划、泊车路径规划、AGV调度这类带避障与非凸约束的轨迹优化都可以直接套同一套框架。我会从数学建模讲到可运行的Matlab代码再分享几个我实际调试时踩过的坑第一轮迭代就不可行、数值病态、在线重规划太慢等等。适合正在做无人机路径规划课题的硕博学生也适合想快速搭建轨迹优化原型的工程师。看完你能明白SCP为什么能收敛也能知道崩溃时该往哪个方向查。1. 思路拆解为什么SCP能啃下无人机轨迹优化1.1 轨迹优化到底在解什么三大非凸来源先说清楚一个概念路径规划和轨迹优化不是一回事。路径规划通常只关心几何上有没有一条线能避开障碍比如A*搜出一条折线而轨迹优化要给这条线加上时间戳让它满足无人机动力学也就是说每个时刻的位置、速度、加速度都得合法。用数学语言描述就是找一个控制序列 (u(t))让状态 (x(t)) 在满足动力学方程、避障不等式、控制量上下限的前提下最小化某个目标函数飞行时间、能量消耗等。这个最优控制问题之所以难是因为里面几乎到处都是非凸约束。第一避障约束是典型的非凸约束要求无人机与球形障碍的距离大于安全半径它的可行域是整个空间“挖掉”一个球球内和球外之间的连线必然穿过禁区所以这是一个凸集的补集而不是凸集。第二时间自由带来的非凸性如果终点时间T是可优化的离散化的动力学方程里会出现“时间步长乘以状态”的双线性项这类项没法直接塞进凸优化框架。第三复杂地形、禁飞区组合在一起可行状态空间本身就被撕得七零八落。理解这一点很重要因为很多新手一上来就想用现成的非线性优化器硬解结果要么无解要么解出来一团糟。SCP的思路很朴素既然整个问题是非凸的那我就把每一个非凸的局部在当前参考点附近用凸函数去近似然后只在一个小范围内求一个凸子问题求完以后更新参考点再求下一个子问题。这就像在迷宫里拽一根有最小转弯半径的管子每走几步就重新估算一下周围墙壁的方向只往前探一小段。1.2 为什么不直接上NLP求解器主流方案横向对比先看一组我常给学生用的对比表格三种主流方案各有各的脾气方案优点缺点适用场景图搜索/采样A*、RRT、PRM实现简单、全局搜索能力强能快速找到可行几何路径轨迹不平滑几乎不考虑动力学约束速度和加速度剖面无法保证二维/三维静态地图的粗略路径搜索直接非线性规划IPOPT、SNOPT等建模直观非线性约束都能直接写进去对初值极其敏感容易陷入局部最优大规模问题收敛缓慢且不稳定离线计算、有良好初值的场景SCP逐次凸化每轮子问题都是凸规划有成熟的内点法兜底收敛行为相对可控线性化会带来截断误差需要好的初值和信任域管理动力学约束避障最优性同时要求的中小规模问题直接非线性规划看起来最省事把约束全都丢给求解器但实际用过的人都知道初值稍微差一点IPOPT能把轨迹优化到障碍内部然后你根本不知道是该调初值还是改权重。SCP相当于把“一次性硬啃非凸问题”换成“一轮轮逼近非凸问题”每一轮的子问题是一个凸规划凸规划有全局最优解所以单轮行为是可预测的调试体验好很多。SCP也有缺点它本质上像带约束的牛顿法参考轨迹如果太离谱线性化方向就是错的所以需要配合信任域管理。信任域的灵魂作用后面专门讲。总的来说对无人机这类状态维数不高、约束种类不复杂的问题SCP是性价比极高的选择。1.3 SCP的迭代骨架线性化、信任域、求解子问题SCP的完整流程可以拆成五步我每次写代码都照着这个骨架走第一步生成一条初始参考轨迹包括状态序列 (x_{ref}) 和控制序列 (u_{ref})。初值好后面迭代就快。第二步在参考轨迹附近把非线性的动力学、避障约束做线性化或凸化处理构造一个凸近似子问题。第三步给子问题加上信任域约束也就是限制新轨迹和参考轨迹不能差太远保证线性化近似还在有效范围内。第四步调用内点法求解这个凸子问题得到新轨迹。第五步判断收敛如果相邻两次目标函数值变化足够小并且原始约束误差在容差内就退出否则更新参考轨迹调整信任域半径回到第二步。信任域是SCP的灵魂。没有它线性化模型只在参考点附近一小块区域成立而优化器才不管你这些它会拼命把解往远处推结果就是一次迭代后轨迹飞到十万八千里外算法直接爆炸。加上信任域之后每一轮只在小范围内信任线性化结果步子稳了收敛性才有保障。我习惯把信任域初始值设为节点间距的2到3倍后面在3.4节详细讲解怎么调。2. 数学建模与凸化细节先把约束写成SCP能接受的样子2.1 无人机运动模型选型双积分模型最常用在SCP框架里模型并不是越精细越好而是“够用且好凸化”最好。对于低空小型无人机做轨迹规划我默认使用三维点质量双积分模型[ \dot{p} v,\quad \dot{v} a ]其中 (p) 是位置向量(v) 是速度向量(a) 是加速度向量也就是控制输入 (u)。离散化以后用欧拉法或梯形法写成等式约束[ p_{k1} p_k dt \cdot v_k,\quad v_{k1} v_k dt \cdot a_k ]这两个等式是线性等式凸优化框架里非常友好。使用这个模型的原因很简单轨迹优化层关心的核心是“几何路径是否避障、速度剖面是否合理”至于无人机到底怎么倾斜机身、电机转速怎么变化那是底层飞控的事。把姿态动力学塞进规划层会让状态维度从6涨到十几甚至二十几非线性程度暴涨但带来的实际收益在多数场景里微乎其微。什么时候需要升级模型呢做倾转旋翼、特技飞行、或对末端姿态有硬性要求时就要在规划层加入姿态动力学。这种情况下SCP依然可用只是动力学约束本身也是非线性的需要额外线性化复杂度上一个台阶。对绝大多数“从A点到B点绕开楼宇”的任务双积分模型已经能跑出很好的效果。2.2 避障约束凸化球状障碍的线性化推导球形障碍是最常用也最好处理的模型。假设障碍中心是 (p_o)安全半径是 (r)那么避障约束写成[ |p - p_o|_2 \geq r ]这个约束为什么非凸因为它要求解落在球外。想象在球外取两个点连线必然穿过球体所以“球外的点构成的集合”不是凸集。SCP的做法是在当前参考点 (p_{ref}) 处对这个非凸约束做一阶泰勒展开把它近似成一个半空间约束。具体推导如下令 (f(p) |p - p_o|)在 (p_{ref}) 处展开得到 (f(p) \approx |p_{ref} - p_o| \frac{(p_{ref}-p_o)^T}{|p_{ref}-p_o|}(p - p_{ref}))。定义单位向量 (n (p_{ref} - p_o)/|p_{ref}-p_o|)它从障碍中心指向参考点方向也就是“远离障碍的外侧方向”。要求线性化后的距离不小于 (r)最终得到一个线性不等式[ n^T(p - p_{ref}) \geq r - |p_{ref} - p_o| ]这个不等式在几何上就是在参考点附近用一张垂直于 (n) 的平面把可行域“切”成一侧在这个半空间内约束是凸的。直观理解就是每一轮迭代都用一把尺子紧贴当前轨迹所在的位置把轨迹往障碍外推随着参考轨迹更新尺子的方向不断旋转最终逼近真实的圆形边界。我在调试时喜欢把“半平面约束”可视化出来能清楚看到每一轮切平面像剥洋葱一样转那画面比任何公式都直观。如果参考点本身就落在障碍内部线性化给出的方向依然是“往外推”所以数学上仍然能写但此时线性化误差巨大子问题很可能不可行这就需要引入松弛变量兜底我在3.2节的代码里直接体现了这个设计。2.3 时间自由难题最短时间问题怎么离散化轨迹优化里最麻烦的变体是“最短时间问题”因为终点时刻 (T) 本身是未知量。如果直接把 (T) 当作优化变量离散化的动力学里就会出现“时间步长 (h) 乘以状态/控制”这样的双线性项比如 (p_{k1} p_k h \cdot v_k)这个等式中 (h) 和 (v_k) 都是变量乘积是非凸的没法直接放进凸子问题。我推荐新手先从“两步走”开始外层固定 (T)内层用SCP求固定时间下的最优控制然后外层用二分法或一维扫描不断缩短 (T)直到轨迹还满足约束且控制量没饱和。这个方案实现简单而且每一步的子问题都是标准凸问题调试效率非常高。想一步到位的可以把时间步长 (h) 当作额外的优化变量并把双线性等式在参考点处做一阶线性化就像避障约束一样。足球场大小的仿真里两种方法都能收敛但后者对信任域和初值更敏感我建议先把“固定T”的方案跑通再去优化时间维度。2.4 目标函数设计能量、时间与平滑性的权衡SCP对目标函数的形式没有强要求但为了保持凸性目标函数本身必须是凸函数。我的默认目标是三项组合[ J \sum_{k1}^{N} u_k^T u_k \cdot dt w_t \cdot T w_s \sum_{k2}^{N} |u_k - u_{k-1}|^2 ]第一项是控制能量它压制过大的加速度让轨迹更省电第二项是时间项权重 (w_t) 调大时轨迹会更倾向于“拉直跑快”第三项是平滑项抑制加速度的跳变避免给飞控发送带毛刺的控制指令。如果还有避障松弛变量就要在目标函数里加上 ( \rho \cdot \sum slack )把“穿障”变成代价而不是硬约束。权重的量级需要根据物理单位来配。比如加速度单位是 m/s²平方以后数值在几十到几百之间dt 是 0.5 秒能量项每步的贡献大约几十这时 (w_t) 如果也取几十时间项才和能量项在同一个数量级否则目标会被某一项主导。我调试时的习惯是从全零权重开始先只跑能量最小看轨迹形态再逐渐把时间权重和平滑权重加进去避免一开始就陷入多目标调参的泥潭。3. Matlab代码实现从零搭一个SCP轨迹规划器3.1 工具链选型YALMIP还是CVX在Matlab里做SCP最常用的建模工具是CVX和YALMIP。CVX语法简洁适合快速验证标准问题我自己经常先用CVX搭原型确认算法没问题后再换到YALMIP做性能优化因为YALMIP可以把模型编译成optimizer对象之后重复调用时省去每次解析模型的巨大开销这对在线重规划非常关键。求解器方面SCP的核心子问题通常是二次规划或二阶锥规划所以需要选择支持SOCP的求解器。MOSEK和Gurobi是最稳的商业求解器学生可以申请学术license如果不想折腾ECOS和SDPT3也能用CVX自带ECOS。我实测下来几十个节点、几百个变量的SCP子问题ECOS完全能扛住只是鲁棒性略逊于MOSEK。安装方面没什么玄学下载安装包、在Matlab路径里添加、运行cvx_setup或者yalmip setup即可。注意一点SCP里避障约束线性化以后是线性不等式但信任域约束 ( |p - p_{ref}| \leq \Delta ) 是二阶锥约束目标函数又是二次的所以整个子问题是一个SOCP。选求解器时一定要确认它支持二阶锥约束否则运行到一半会报“不支持的模型类型”。3.2 核心主循环代码与逐行解释下面给出一段可以直接放进Matlab跑通的核心循环虽然去掉了部分包装代码但主干已经很完整。这段代码使用CVX编写便于初学者阅读。N 60; % 离散节点数 dt 0.5; % 时间步长 u_max 8; % 最大加速度 m/s^2 rho 1e3; % 松弛变量惩罚系数 p_start [0; 0; 0]; p_goal [80; 60; 20]; v_start zeros(3,1); v_goal zeros(3,1); obs_pos [25 20 10; 55 35 15; 40 60 5]; % 障碍中心3x3 obs_r [8; 7; 6]; % 障碍半径 % 初始参考轨迹逐维线性插值 p_ref zeros(3, N); for dim 1:3 p_ref(dim, :) linspace(p_start(dim), p_goal(dim), N); end v_ref zeros(3, N); u_ref zeros(3, N); trust_region 8.0; % 信任域初始半径 J_prev 1e10; for iter 1:20 cvx_begin quiet variable p(3, N) variable v(3, N) variable u(3, N) variable slack(1, size(obs_pos,2)) 0 minimize( sum(sum(u.^2)) * dt rho * sum(slack) ) subject to p(:,1) p_start; v(:,1) v_start; p(:,N) p_goal; v(:,N) v_goal; for k 1:N-1 p(:,k1) p(:,k) dt * v(:,k); v(:,k1) v(:,k) dt * u(:,k); norm(u(:,k), 2) u_max; end norm(u(:,N), 2) u_max; for k 1:N for j 1:size(obs_pos,2) n (p_ref(:,k) - obs_pos(:,j)) / norm(p_ref(:,k) - obs_pos(:,j)); n * (p(:,k) - p_ref(:,k)) obs_r(j) - norm(p_ref(:,k) - obs_pos(:,j)) - slack(j); end norm(p(:,k) - p_ref(:,k), 2) trust_region; end cvx_end if ~strcmp(cvx_status, Solved) trust_region trust_region * 0.6; continue; end J_new cvx_optval; if abs(J_prev - J_new) / max(1, abs(J_prev)) 1e-3 break; end J_prev J_new; % 更新参考轨迹 p_ref p; v_ref v; u_ref u; end几处细节需要特别说明。第一给每个障碍的避障约束加了非负松弛变量slack并且目标函数里用rho * sum(slack)惩罚它。这一步非常关键当参考轨迹还穿障时线性化约束的右边会是正数如果没有松弛变量子问题直接不可行程序当场崩溃。有了松弛变量求解器会优先让轨迹远离障碍但如果真的被几何关系卡死也只会把惩罚值拉大而不会报错。第二信任域约束norm(p(:,k) - p_ref(:,k), 2) trust_region加在每一个节点上保证整个新轨迹不会偏离参考轨迹太远。第三收敛判定我直接在代码里用cvx_optval比较实际项目里还要额外检查原始约束误差我在3.4节细说。如果想跑更大的地图把for k 1:N这种逐点约束写成矩阵运算CVX仍然支持但可读性会下降。我建议先用上面的结构把逻辑跑通再考虑向量化优化。3.3 初始轨迹生成与关键参数初始化初始参考轨迹怎么来直接决定SCP要迭代多少轮。最简单的做法是逐维线性插值起点和终点之间拉一条直线。但这条直线很可能穿过障碍导致第一轮避障约束的线性化点在障碍内部。更稳妥的做法有三种一是先用A*或RRT在栅格地图上搜索出一条避障路径再沿路径重采样成等间距节点作为SCP的初值二是在同一个SCP框架里先关掉避障约束跑两轮能量最优拿那条“穿障但动力学可行”的轨迹当参考轨迹再打开避障约束三是干脆让前几轮的松弛惩罚系数从小往大调逐步把轨迹挤出障碍。我个人的经验是离线场景首选“A*粗路径 SCP精优化”在线重规划用“上一帧热启动”最省事。如果真的只做简单demo那就先跑两轮无避障迭代几乎不需要额外写代码只需要临时在约束循环里加一个if use_obs开关。实测下来一个三维绕三个球形障碍、节点数60、初始线性插值穿过第一个障碍的例子前两轮无避障迭代后轨迹被拉直开启避障约束后第7轮目标函数下降量已经小于0.1%第8轮正常退出。这个收敛速度在工程上是完全可以接受的。参数初始化方面信任域半径我通常取相邻节点距离的2到3倍也就是2 * dt * v_expected上代码里取8.0松弛惩罚系数rho取1e3到1e5太小会穿障太大虽然更安全但会让目标函数的量级失衡影响数值稳定性。3.4 收敛判定、信任域更新与结果可视化不能只看CVX返回的目标函数值就认为收敛了因为目标函数里带着松弛惩罚项很可能松弛变量还很大也就是说轨迹还在穿障。我的习惯是优化结束后单独跑一遍几何检查逐点计算新轨迹到每个障碍的实际距离如果距离小于安全半径就认为这轮结果无效需要继续迭代或调整信任域。这个检查很便宜几行Matlab就能写完但能避免大量“目标函数降了但轨迹穿障”的迷惑时刻。信任域更新我常用的启发式规则是如果本轮求解成功并且原始约束误差在容差内下一轮信任域乘1.2允许更大胆地探索如果本轮求解失败或者约束误差超限信任域乘0.6让下一步走得更保守信任域还需要限制上下界比如最小0.5米最大20米。这套规则没有严格理论保证但工程上非常实用我在很多项目里都靠它稳定收敛。结果可视化是三件事第一用plot3画三维轨迹用sphere加surf画障碍球的表面第二用子图画速度模长 ( |v| ) 和加速度模长 ( |u| ) 沿时间的变化曲线检查速度是否平滑、加速度是否频繁碰到上限第三把每一轮迭代的轨迹画成不同颜色直观看到SCP如何把一条穿障直线“推”成光滑曲线。如果加速度曲线出现剧烈跳变说明平滑权重太低或者离散时间步长太大需要回头调参。最后SCP输出的是离散的点列直接发给飞控会非常生硬。落地时要用B样条或多项式拟合把离散点变成连续轨迹再按时间戳生成速度前馈和加速度前馈。这一步虽然不算SCP算法本身但少了它仿真和实测效果会差一大截。4. 常见问题与排查技巧实录4.1 第一轮迭代就不可行怎么救现象很典型运行到第一轮CVX返回Infeasible problem程序直接停住。绝大部分原因有三个初始参考轨迹穿障导致线性化约束方向错位信任域半径设得太小和边界条件冲突起点或终点本身就在障碍内部这种情况几何上无解加再多松弛也没用。对应的解法也很直接。首先确保每个避障约束都带了非负松弛变量slack这是最廉价的一层保护。其次可以先关掉避障约束跑两轮无避障迭代拿到一条动力学可行轨迹后再打开避障约束。我甚至会把这种“先无约束、后有约束”的启动方式写进框架的默认流程省得每次都手工处理。最后写一个简单的几何检查计算起点和终点到所有障碍中心的距离如果任何一个小于障碍半径直接提示用户修改边界条件。4.2 目标函数来回震荡不收敛SCP最常见的问题就是目标函数值波浪一样起伏这一轮下降下一轮又涨回去轨迹形态也来回变。我排查时首先怀疑信任域太大导致线性化失真严重于是把信任域半径缩小到当前节点间距的60%重新跑几轮。如果还是震荡再看松弛惩罚系数rho是否太小轨迹局部穿障但没有受到足够的惩罚导致求解器在“穿障省能量”和“远离障碍多耗能”之间反复横跳。还有一个我很推荐的阻尼技巧在目标函数里临时加一小项 ( \alpha \cdot |p - p_{ref}| ) 或 ( \alpha \cdot |u - u_{ref}| )也就是“参考轨迹跟踪项”让每一轮新解不会偏离上一轮太多。跑几轮之后再逐步把 ( \alpha ) 减小到零。这个技巧我在很多难收敛的场景里用过效果立竿见影比盲目调信任域更可控。4.3 数值病态量级差太远导致求解器摆烂如果你看到求解器警告Numerical difficulties或者解出来的轨迹看着还行但拿放大镜看速度剖面全是锯齿那多半是数值病态。无人机位置动辄几百米速度单位是米每秒加速度单位是米每二次方秒时间步长又是零点几秒这些变量同时出现在约束里各项的数量级从0.1到几百相差悬殊KKT矩阵条件数会变得非常差。治本的办法是统一缩放。我习惯做一组无量纲化位置除以参考长度 (L)速度除以 (L/T_{ref})加速度除以 (L/T_{ref}^2)代入原始动力学之后数学形式完全一样但所有变量都落在0到10这个舒服的范围。治标的办法是把CVX精度调高也就是cvx_precision high但你很快会发现这只能救一时换个地图又崩了。所以我在写模型之前一定会先打印所有变量和约束项的典型量级一眼就能看出有没有需要归一化的地方。4.4 求解太慢在线重规划的性能调优离线仿真慢一点问题不大但如果要做在线重规划40秒一次肯定不行。我优化速度的顺序是先减少节点数N从100降到40到60时间步长适当增大轨迹精度靠后处理B样条插值补再把CVX换成YALMIP把模型编译成optimizer对象模型只解析一次之后每一轮只是矩阵运算和求解器调用省掉的建模时间非常可观最后是设置求解器容差MOSEK和ECOS都有reltol、abstol、feastol这类选项把容差从默认高精度放宽到1e-4级别单轮求解时间能压缩一半。热启动是另一个关键手段。把上一轮最优解直接扔给下一轮作初始点让凸优化求解器从一个很好的起点开始。CVX没有标准的“设置初值”接口但YALMIP支持assign和initial所以一旦开始做在线版本我会果断切到YALMIP。实测优化良好的 YALMIP MOSEK 在线SCP60个节点单轮求解时间通常在0.05到0.2秒之间每一控制周期跑两三轮完全够用。如果单轮超过一秒先怀疑模型解析的重复开销再怀疑求解器配置。5. 从离线到在线SCP还能怎么玩5.1 与A*/RRT粗规划结合先找路再优化SCP不是用来替代全局路径搜索的它更适合做“精优化”。工程里我习惯使用两段式第一段用A*或RRT在静态地图上找一条避开所有障碍的粗略几何路径第二段把这个几何路径离散成节点序列作为SCP的初始参考轨迹。SCP所做的是把一条锯齿状、不满足动力学的几何路径变成一条平滑、有速度剖面、控制量不越界的可行轨迹。这套组合的好处是彻底解决了SCP初值敏感的问题。A*给出一条全局合理但局部不平滑的路径SCP只需要在这个路径周围做局部调整迭代次数通常可以控制在5轮以内。很多实际项目比如山区或城市环境的无人机运输与通信协同任务用的都是这个模式先搜禁飞区外的一条粗路径再用SCP平滑并做速度分配。比起单独用采样算法或者单独用SCP稳定性高出不少。5.2 动态避障与在线重规划SCP天然适合在线重规划因为它本身就是迭代算法不需要一次求到最优。处理动态障碍时把静态避障约束改成时变约束也就是每一轮都用障碍物当前时刻的预测位置来线性化。具体改法是把前面公式里的 (p_o) 换成 (p_o(t_k))然后对每一个节点分别计算法向量和常数项其他结构完全不动。在线运行时的做法是维护上一次的解作为参考轨迹每个控制周期只做一到两轮SCP把新生成的轨迹前几秒控制指令发送给飞控然后滚动更新。因为每一轮都是凸优化计算时间相对可预测只要单轮在0.1秒级别整个系统就能跑得过来。经验不足的时候记得把动态障碍的半径外扩10%到20%用保守锥约束弥补预测偏差这一招很土但很管用。5.3 与学习方法的互补我的个人看法有些课题组喜欢用强化学习端到端生成无人机控制策略优势是反应快、能处理高维感知但安全约束和最优性很难给出硬保障试错成本也高。SCP则刚好相反每一轮都基于凸优化理论能明确检查约束是否满足收敛性有保证。两者其实不矛盾。我比较看好的组合是用一个小型神经网络生成好的初始轨迹分布或者预测重要障碍区域然后交给SCP做精修和兜底。这样既降低了SCP的迭代压力又保留了凸优化的安全可解释性。不过这条路线调试量并不小如果课题时间紧张还是老老实实把SCP本身吃透更实用。最后再分享一点实际体会。SCP不是那种“调好一组参数就能一劳永逸”的算法它跟着场景走换地图、换约束都必须重新检查初值和信任域。我刚入门时总想把代码写得足够通用结果一换环境就崩后来固定成一套简单流程线性插值启动、先空跑两轮无避障、再开避障约束、收敛后校验原始约束这套组合一直用到现在。如果你也想快速验证和理解这个算法强烈建议先在二维地图上把障碍和每一轮线性化出来的半平面约束画出来看着它旋转逼近真实边界那一幕理解了SCP的核心就通了。