简介这份面向无人机控制初学者与算法实践者的MATLAB资料围绕四阶无人机模型预测控制避障展开重点解决三维空间中位置姿态预测、飞行路径规划与安全避障问题。压缩包内仅1个m文件约1KB主体为MPC.m脚本可直接在MATLAB中阅读和运行便于查看预测模型、优化函数及约束条件的组织方式。已有1631人学习下载说明该主题具有一定参考需求。读者可据此理解四阶模型涵盖前后、左右、升降及偏航四个主要自由度的运动建模梳理滚动优化、控制输入求解和障碍物规避的基本流程并观察速度、高度与机动限制如何写入优化问题理解优化目标、预测时域与约束边界之间的权衡也可替换代价函数或障碍参数作为课程设计、仿真练习和算法二次开发的轻量模板。1. 一段“贴着墙飞”的仿真MPC 无人机避障这套 MATLAB 资源能替你省掉哪两周无人机三维避障很多人第一反应是“先规划一条折线再在拐角贴一段圆弧”真机上稍微有点风、有点通信延迟就蹭上去了。这套 MATLAB 无人机模型预测控制避障仿真资源走的是另一条路把障碍写进滚动优化的不等式约束里每一拍重新算一次未来 N 步的可行轨迹只执行第一步下一拍拿新状态再算。它适合两类人——正在做课程设计或毕设、需要一份能跑出三维轨迹图和代价曲线对照的基线以及已经在飞控上跑过 PID、想搞清楚时域、权重、约束这三件事到底怎么改变避障行为的一线工程师。给的是脚本级实现不是黑盒工具箱模型、障碍几何、求解器都能自己改。2. 拆开看骨架四层结构决定这套 MPC 避障能不能改成你的模型拿到一份 MPC 避障的 MATLAB 工程最忌讳的是先改 main 里的数字。真正决定它能不改得动的是四层状态怎么选、障碍怎么写进约束、用哪个求解器、模块之间怎么传数据。这四层里任何一层选错后面调参调到天亮也没用。2.1 状态量与预测模型为什么是六维双积分而不是十二维全状态最省事也最常用的写法是把无人机当成三维空间里的质点加三轴速度状态 6 维、控制 3 维符号维度含义说明p3位置 x/y/z直接和障碍约束耦合v3速度由加速度积分得到u3三轴加速度指令上层给下层的期望不是电机转速采样周期 Ts 取 0.050.2 s用零阶保持离散化得到一个标准的双积分模型。它的好处是 A、B 都是常数矩阵预测可以写成状态的线性组合代价函数是二次型约束是线性的天然就是一个凸二次规划。十二维全状态再加姿态角、角速度听起来更“真实”但姿态动力学含三角函数和旋转矩阵一进来 QP 就不成立了得换 SQP 或者干脆上非线性 MPC单次求解从毫秒级跳到几十毫秒实时性直接崩。业界常见的分层做法是外环 MPC 只算加速度指令内环用几何控制器或姿态 PID 去跟踪这个加速度。这套资源走的是外环那条线你如果要在后面接自己的姿态环接口就是这三个加速度分量。还有一点容易被忽略模型精度不需要多高但必须和你的执行机构匹配。如果你的机型响应有明显一阶滞后把 B 直接换成带滞后补偿的形式比在权重上硬拗有效得多。2.2 障碍怎么进优化问题三种几何写法的取舍障碍约束的写法直接决定问题是不是凸的也决定你后面会遇到哪类坑。三种常见写法的对比如下几何体数学表达凸性实现难度典型场景球体‖p−c‖₂ ≥ R非凸球外区域中需要逐点线性化圆形障碍、其他飞行器竖直圆柱水平距离 ≥ R 且 z∈[z0,z1]水平部分非凸中建筑、塔架、树干长方体/半空间逐面法向不等式单面凸整体非凸低到中楼房、走廊、室内球约束 ‖p−c‖ ≥ R 的可行域是球外面是个非凸集不能直接丢进 QP。工程上最常用的处理是逐点线性化在当前预测点 p̂ 处沿 (p̂−c) 方向取单位外法向 n把球约束替换成一张切平面 nᵀ(p−c) ≥ R。这张切平面是球在 p̂ 处的支撑平面满足它就一定满足球约束属于保守近似安全但不最优。长方体更麻烦一点。“在盒子外面”是四个甚至六个半空间的并集不是交集直接写约束会得到一个空集。常见做法是先用几何判断找出离预测点最近的那个面只对这个面施加半空间约束下一拍再重新判断——这本质上也是一种逐拍线性化。注意不管用哪种几何安全半径一定要留裕度。机身半径、定位误差、线性化的保守损失三项加起来通常要把 R 放大约 30%50%否则你会看到轨迹“擦着表皮”过去。2.3 求解器选择quadprog、fmincon 与第三方求解器的分工同一个避障问题不同求解器走的是不同的路求解器问题类型依赖单次耗时量级在本资源里的角色quadprog凸 QPOptimization Toolbox15 ms主循环实时求解fminconsqp 算法非线性非凸Optimization Toolbox1050 ms离线对照、真值复核第三方 QP 求解器凸 QP需额外安装0.53 ms大规模或多机扩展我的建议是两条腿走路主循环用 quadprog 跑线性化后的凸 QP保证每拍都有解另外写一份 fmincon 版本保留真实的球约束做逐拍对照看线性化到底吃掉了多少最优性。这两份结果的平均偏差如果超过安全裕度说明你的线性化方向取错了或者初始点离障碍太近。调用 quadprog 的标准形式是quadprog(H,f,A,b,Aeq,beq,lb,ub,x0,options)前四项就够用了。options 里把 Display 关掉把 MaxIterations 提到 200 左右其余用默认别一上来就换算法——interior-point-convex 在这个规模下够稳。2.4 模块职责与数据流改哪一块不会把别的块带崩这类仿真工程通常按职责切成五块理解了这个切分你才知道该动哪里模型定义给出 A、B、状态维数、采样周期是所有预测矩阵的唯一来源预测矩阵构造一次性算出 Phi 和 Theta主循环里不再重算约束拼装读入障碍列表和当前状态输出不等式矩阵 Ai、bi求解与执行调 quadprog取第一步控制量推进状态可视化与统计画三维轨迹、障碍球、速度曲线统计侵入深度和求解耗时。数据流是单向的模型 → 预测矩阵 → 约束 → 求解 → 状态推进 → 回到约束。这个单向性很重要只要你不把参数塞进约束函数内部去改状态任何一块替换都不会污染其他块。很多人改崩工程就是把障碍半径写死在约束函数里同时又想在主脚本里扫参数结果扫到一半发现全是一样的结果。3. 从零跑通一次三维避障预测矩阵、半空间约束和滚动优化主循环这一章是可抄作业的部分。目标很明确给定起点、终点和两个球形障碍让无人机在 20 步预测时域下飞出一条不侵入安全半径的轨迹同时统计求解耗时。3.1 离散化与预测矩阵Phi 和 Theta 一次算好预测模型的推导不复杂麻烦的是别在循环里反复算矩阵幂。下面是构造批量预测矩阵的写法跑一次存起来主循环直接用% 三维双积分模型离散化Ts 0.1 s Ts 0.1; nx 6; nu 3; N 20; % N: 预测时域步数 I3 eye(3); A [I3, Ts*I3; zeros(3), I3]; % 状态转移 B [0.5*Ts^2*I3; Ts*I3]; % 控制输入矩阵 % 批量预测X Phi*x0 Theta*U Phi zeros(nx*N, nx); Theta zeros(nx*N, nu*N); Ak eye(nx); for k 1:N Ak A*Ak; % A^k避免重复 power Phi((k-1)*nx1 : k*nx, :) Ak; for j 1:k Theta((k-1)*nx1 : k*nx, (j-1)*nu1 : j*nu) A^(k-j)*B; end end % 代价权重与常量矩阵只和 Phi/Theta 有关可移出主循环 Q diag([10 10 10 1 1 1]); % 位置权重大于速度权重 R diag([0.5 0.5 0.5]); Qbar kron(eye(N), Q); Rbar kron(eye(N), R); H 2*(Theta*Qbar*Theta Rbar); % 二次项常量这里的关键是 Phi 和 Theta 的形状。Phi 是 (6N)×6把初始状态映射到整段预测Theta 是 (6N)×(3N)是一个下三角块状矩阵第 k 步只依赖前 k 个控制量。有了这两个矩阵约束和代价都能写成控制序列 U 的线性/二次函数问题就完全落进 QP 的框架。参数上唯一需要斟酌的是 N。它不是越大越好真正有意义的是 N×Ts 这个乘积也就是预测视野覆盖的时间长度。避障场景里 1.53 秒足够太短来不及绕太长则远端预测误差大、约束数量翻倍。3.2 把球形障碍拆成半空间逐点线性化与安全裕度约束拼装函数是整份代码里最容易写错的地方符号一错轨迹会朝着障碍里面钻。下面这版按“上一拍预测轨迹”做线性化输出 Ai、bi 使得 Ai*U ≤ bifunction [Ai, bi] obstacleHalfSpaces(Phi, Theta, x0, obs, rSafe) % obs: M×4每行 [cx cy cz R] % rSafe: 额外安全裕度米 nx 6; nu 3; N size(Theta,1)/nx; Pfree (Phi*x0)(1:3:end); % 各步无控预测位置3 分量抽取 Ai []; bi []; for m 1:size(obs,1) c obs(m,1:3); % 球心 R obs(m,4) rSafe; % 含裕度的安全半径 for k 1:N p Pfree(3*k-2:3*k); d p - c; if norm(d) 1e-6, d [1;0;0]; end nv d / norm(d); % 由预测点指向球外的单位法向 rows 3*k-2:3*k; % nv*(p_k - c) R -nv*Theta_k*U nv*(Phi_k*x0 - c) - R Ai [Ai; -nv * Theta(rows, :)]; bi [bi; nv * (Phi(rows,:)*x0 - c) - R]; end end end逐行解释一下先用无控预测作为展开点也有人用上一拍的解收敛更快但实现稍复杂求出这一步的预测位置 p。法向 nv 取 p 到球心的方向取反即从球心指向预测点的外侧。约束的原始形式是 nvᵀ(p_k−c) ≥ R把 p_k Phi_k·x0 Theta_k·U 代进去移项并整体乘 −1 得到标准的不等式形式。bi 里带着 −R这就是安全裕度起作用的地方。提示只线性化一次在靠得比较近时误差不小。稳妥做法是在同一拍里迭代 23 次用刚解出的 U 重算预测位置再重建约束重解 QP。实测侵入深度能降一个数量级代价是耗时翻倍。3.3 主循环quadprog 求解 只取第一步滚动优化的精髓就一句话算 N 步只用第一步。下面这段是主循环把参考轨迹、约束、上下界、失败回退串在一起x x0; goal [8; 6; 3]; uMax 3; % uMax: 三轴加速度限幅 obs [4 3 2 1.0; 6 2 2.5 0.8]; % 两个球形障碍 rSafe 0.5; U0 zeros(nu*N,1); % 上一拍解做初值 logx zeros(nx, 0); logt []; opts optimoptions(quadprog,Display,off,MaxIterations,200); for step 1:200 % 1) 参考轨迹当前点到目标点的直线末端保持在目标点 Xref refLine(x, goal, N, Ts); % 2) 线性项H 已在循环外算好 f 2*Theta*Qbar*(Phi*x - Xref); % 3) 障碍约束 [Ai, bi] obstacleHalfSpaces(Phi, Theta, x, obs, rSafe); % 4) 控制量上下界 lb -uMax*ones(nu*N,1); ub uMax*ones(nu*N,1); % 5) 求解 tic; [U, ~, flag] quadprog(H, f, Ai, bi, [], [], lb, ub, U0, opts); logt(end1) toc; if flag 0 % 不可行或迭代失败 U fallbackU(x, goal, uMax, N); % 回退到简单的刹车/悬停指令 end u U(1:nu); % 只执行第一步 x A*x B*u; % 推进仿真模型 U0 [U(nu1:end); U(end-nu1:end)]; % 控制序列左移做下一拍初值 logx(:, end1) x; if norm(x(1:3) - goal) 0.2, break; end end几个参数值得单独说。uMax 是三轴加速度的硬限幅它既是物理约束也是让 QP 保持有界的保险很多人忘了设界结果第一拍解出一个 50 m/s² 的加速度画出来的轨迹像瞬移。U0 用上一拍解左移一位填充虽然在 interior-point-convex 算法下不保证加速但作为初值总比全零稳。flag 一定要判quadprog 返回负值意味着要么不可行、要么迭代次数耗尽这时候还硬用 U 会让仿真直接发散。refLine 这个辅助函数很简单把目标点按等分插值成 N 个参考位置速度参考置零。用视线法或者多项式插值都可以但对避障来说参考轨迹只是个“想去的方向”真正的绕行动作由约束驱动不需要设计得很精细。3.4 结果自检约束违反量、求解耗时和三条对照曲线跑完不能只看轨迹图漂亮得量化。侵入深度是最硬的指标P logx(1:3,:); viol 0; for k 1:size(P,2) for m 1:size(obs,1) g norm(P(:,k) - obs(m,1:3)) - (obs(m,4) rSafe); viol max(viol, -g); % 取负值里的最大值即最大侵入 end end fprintf(最大侵入 %.3f m | 平均耗时 %.1f ms | 最大耗时 %.1f ms\n, ... viol, 1e3*mean(logt), 1e3*max(logt));三个数字各有各的判断标准。最大侵入深度应该接近 0 但允许小幅负值因为线性化是保守的一般不会真正穿进去如果出现 0.1 m 以上的侵入多半是安全裕度不够或者线性化方向取反了。平均耗时要和 Ts 对比必须显著小于采样周期否则这套东西只能离线跑。最大耗时看的是最坏情况第一次调用 quadprog 会有函数加载开销把第一拍剔除再看。对照曲线建议画三条实际轨迹到障碍的距离随时间的变化、三轴加速度指令、以及求解耗时。距离曲线最能暴露问题它一掉到 rSafe 以下你立刻知道是哪一拍出的问题回去查那一拍的约束矩阵就行。4. 参数整定不是玄学N、Q/R、松弛因子与终端代价的取值边界调参之所以看起来像玄学是因为多数人一次动三个参数。可行的做法是把参数按影响面排序先定时域再定权重最后放松弛。每一步只动一类跑一次看一个指标。4.1 预测时域 N 与采样周期 Ts 的乘积才是真参数参数常用范围调大后的现象调小后的现象Ts0.050.2 s单步跨度大线性化误差放大求解频率上升实时压力大N1030约束数量↑绕行更早耗时↑来不及绕转弯半径变大N×Ts1.53.0 s视野过远远端参考意义弱视野不足急转、易不可行真正需要盯住的是 N×Ts。它决定了无人机在多远之外就开始“看见”障碍并做出反应。如果它小于从当前速度到停下所需的时间你就等着看轨迹擦着障碍过去吧。N 本身只是离散化的密度Ts 和 N 一起决定视野所以改 Ts 之后一定要同步改 N 把乘积拉回区间。4.2 权重整定顺序先 R 后 Q位置权重大于速度权重Q 和 R 是典型的跷跷板一起调永远调不明白。我的顺序是先定 R 的绝对值再调 Q 的相对比例。R 决定控制量的“贵”它直接对应加速度的大小。把 R 调大轨迹变得平缓、转弯半径变大、避障更晚调小则激进、抖动。从一个能让最大加速度贴近 uMax 的 70% 左右的 R 起步是合理的。Q 内部也要分主次位置权重通常比速度权重大一个数量级因为避障关心的是位置三个轴之间如果高度通道执行能力弱可以把 z 的位置权重调低避免解算器为了压高度而牺牲水平绕行。用dlqr求一次最优反馈把 Riccati 解出来的 P 拿来当终端权重比你手拍一个 Q 靠谱得多[K, Pinf] dlqr(A, B, Q, R); % Pinf 作为终端代价矩阵这一步不改变问题类型只让短时域下的 MPC 具有更好的稳定性保证属于低成本高收益的改动。4.3 软约束松弛因子什么时候必须加、加多少硬约束的致命弱点是只要初始状态已经在安全半径内问题立刻不可行整个循环停摆。软约束就是给约束装一个可伸缩的弹簧代价是允许轻微侵入% z [U; s]s 为每个约束对应的非负松弛量 ns size(Ai,1); Ai [Ai, -eye(ns)]; % nTheta_k*U s_k R 侧移项 lb [-uMax*ones(nu*N,1); zeros(ns,1)]; % s 0 ub [ uMax*ones(nu*N,1); 10*ones(ns,1)];% 松弛上限防止无限退让 H blkdiag(H, 2*1e3*eye(ns)); % 松弛惩罚系数 M 1e3M 的取法有个经验边界它要比 Q 里位置权重至少大两个数量级才能在正常工况下把松弛压到接近 0但又不能大到让 Hessian 条件数失控。1e21e4 之间一般够用。加上软约束后flag 基本不会再返回不可行但代价是侵入深度不再由约束保证必须靠 3.4 那段统计去盯。4.4 终端代价与终端约束短时域下的可行性保险N 只有 1520 步的时候MPC 只看得到 1.52 秒的未来末端状态完全是“放飞”的这会导致两个后果一是靠近末端时控制量突然变大二是循环多次后可能进入一个递归不可行的死循环。终端代价用 Pinf解决第一个问题让末端的偏离也被惩罚。终端约束要求在最后一步到达某个不变集内理论更完整但实现成本高工程上更常见的替代是把最后 35 步的参考速度强制置零相当于要求“预测视野末尾必须是悬停态”。这个改动很小但对抑制末端抖动非常有效尤其在与目标点距离小于 1 m 的时候。5. 避坑五条把“仿真能飞”变成“跑不下去”的现场记录下面这五条都是我自己踩过的按现象、原因、解决三段写出现频率从高到低。5.1 换个初始点就报 infeasible现象默认初始点跑得好好的把起点挪到离障碍 2.5 m 的位置quadprog 返回 −2循环直接卡死。原因硬约束加上初始点已经非常接近安全边界线性化后的切平面把当前状态本身排除在外——约束集合非空但不包含你现在的解。另外初始速度如果朝障碍方向前几步的预测位置会落在球内同样无解。解决三件事一起做。第一加 4.3 的软约束松弛让不可行变成“有代价的可行”第二初始化时检查起点到各障碍的距离小于 1.5 倍安全半径就先把 rSafe 临时缩到 0飞出去再恢复第三逐拍线性化改成迭代 2 次。这三步下去基本不会再出现初始点敏感的报错。5.2 轨迹贴着障碍表面划过去现象侵入统计显示最大侵入 0.02 m看起来合格但三维图上轨迹像贴在球面上滑过去的稍微改一下速度就穿了。原因切平面约束只保证在展开点处满足球约束圆球在切平面和球面之间有凸出的部分线性化的保守性在切点附近最强、在切点两侧反而会高估可行域再加上球约束被 N 份复制每份用不同的展开点各切平面之间的交集未必覆盖真实球体。解决把 rSafe 从“机身半径的 20%”提到 40%50%同时在每拍里把线性化迭代 2 次用解出来的 U 重算预测位置再重建约束。代价是耗时翻倍但轨迹不会再出现贴面现象。这个裕度不是保守是必要的误差补偿。5.3 模型对、约束对飞机就是抖现象轨迹大致正确但加速度曲线高频振荡速度曲线呈锯齿状。原因代价函数只惩罚 u没惩罚 Δu。相邻两拍解出的控制量可以差很大而两拍之间没有任何耦合约束于是形成抖振。N 太长、Ts 太小的时候这个现象更明显因为约束数量多可行域边界变化剧烈。解决在代价函数里加 Δu 项或者更省事的做法是在执行端加一阶滤波。加 Δu 要改 H 和 f 的结构引入 U 的差分矩阵但对平滑性的改善最直接。如果时间紧把 u 指令限幅在一个采样周期内变化不超过 uMax/3也能压下去大半。5.4 换个障碍尺寸就发散现象障碍半径从 0.8 改到 2.0约束矩阵规模 4 倍增长跑几十拍以后出现数值问题或者明显的卡顿。原因约束是Ai [Ai; row]这样拼起来的每次都在扩容MATLAB 的数组重分配开销随规模非线性上升同时 N×M 条约束里离得远的那些对解毫无贡献纯属浪费。解决先按 M×N 预分配 Ai、bi用计数器写入再加一层距离裁剪只对离当前预测位置最近的关键障碍建约束比如按距离排序取前 3 个。这两步做完N20、M5 的规模下单拍耗时可降一半以上。5.5 循环越跑越慢内存一直涨现象短循环没事跑 500 拍以后风扇狂转memory 曲线一路往上。原因日志用log.x(:, end1) x增长绘图写在循环里面每拍都 drawnow还有 figure 反复创建没有 hold on。这几个习惯单独看都无害叠在一起就是性能杀手。解决日志全部预分配先zeros(nx, T_end/Ts)再按下标写入绘图移到循环外或者只在最后画一次必须实时看动画的话用drawnow limitrate并把绘图频率降到每 10 拍一次。改完这些同样的循环耗时会稳定在一条水平线上。6. 让结果敢用非线性复核、可行性回退与加速的一个组合拳仿真跑通不等于能拿去支撑结论。我一般做三件事来给结果上保险。第一件是线性化复核。主循环用 quadprog另外写一个小函数用 fmincon 保留真实球约束求同一拍的最优解逐拍比较两者的代价和末端位置。偏差在 5% 以内说明线性化可以接受偏差大就必须加迭代重线性化。复核不需要每拍都做抽样 20 拍就足够看出趋势。% fmincon 版本直接对控制序列 U 优化球约束用非线性不等式 nonlcon (U) deal( sphereViolation(U, Phi, Theta, x0, obs, rSafe), []); [U_ref, cost_ref] fmincon((U) U*H*U/2 f*U, U0, ... [],[],[],[], lb, ub, nonlcon, ... optimoptions(fmincon,Display,off,Algorithm,sqp));第二件是可行性回退。flag≤0 的时候绝不能继续用那个解。fallbackU 我通常写成“保持当前速度、加速度取反的一半”也就是温和刹车如果连速度都很大就直接输出零指令悬停一拍等下一拍状态更新后再算。这个兜底逻辑在真实系统里比任何参数都重要——它保证了失败的时候是可控的失败。第三件是加速。三步走把 H、Qbar、Rbar 这些常量全部移到循环外用profile on定位实际热点八成在约束拼装而不是 quadprog 本身最后再考虑用 MATLAB Coder 把求解函数编译成 MEXquadprog 支持代码生成配置好之后单拍耗时通常能再降 30%50%。加速之前一定先把功能跑对否则你会在一个错误的实现上优化出错误的速度。复核项达标线不达标时的处置与非线性解代价偏差 5%增加线性化迭代次数最大侵入深度 0.05 m上调 rSafe 或改用软约束平均耗时 / Ts 30%裁剪障碍数量、预分配、编译 MEX不可行帧占比 1%加软约束松弛、检查初始点从那以后我每次改完约束形式或权重矩阵都会先跑一遍随机初始点的两百次抽样只盯着“不可行帧占比”和“最大侵入深度”这两个数其他图一概不看。这两个数稳了再谈轨迹好不好看。希望帮到你。本文还有配套的精品资源点击获取