简介面向无人机控制与MATLAB仿真的入门者这份压缩包聚焦四阶无人机模型下的模型预测控制避障问题可作为理解MPC预测、滚动优化与约束处理的轻量参考。包内共1个文件为m格式的MATLAB脚本压缩包约1KB体量精简核心内容集中在控制算法主干便于直接阅读和二次修改。已有1631人学习说明该主题在课程设计与算法实践中关注度较高。读者可从中梳理无人机位置与姿态预测、避障路径规划、优化目标设定以及速度、机动和高度约束的处理思路并以此为基础在MATLAB中搭建自己的仿真验证流程对需要快速了解MPC避障代码骨架、再逐步补充传感器建模与优化求解细节的工程师和学生较为合适。1. 需求先立住MPC 无人机避障到底卡在哪一步MPC 无人机避障在 MATLAB 里跑出轨迹动画不难难的是让这条轨迹在约束边缘仍然可信障碍物稍微靠近一点、预测时域稍微拉长一点求解器就可能返回失败或者给出一个看起来平滑却贴着障碍物擦过去的解。这套方案围绕 matlab 无人机 模型预测控制 避障这条主线把质点动力学、滚动优化、半平面避障约束和求解失败回退串成一个能验收的闭环。它适合正在做无人机控制方向毕设、课设或者需要交付一套 MATLAB 仿真方案的人不适合只想调一个 PID 看动画的人。下面从总体设计开始把每个模块的职责、参数和验收口径拆开。2. 方案总体设计四层架构与三个坐标系的职责划分2.1 四层架构环境层、预测层、优化层、执行层这套仿真我通常拆成四层不是为了画架构图好看而是为了让每次改参数都能定位到具体层。环境层负责维护障碍物列表、目标点、地图边界和动态障碍物的速度预测层把连续的双积分模型离散化构造预测矩阵生成未来 N 步的参考状态优化层把跟踪误差、控制量惩罚和避障约束组装成二次规划调用求解器执行层只做三件事取优化解的第一拍、推进仿真状态、写日志。四层之间用结构体传数据避免全局变量到处飞。环境层最容易被写成一个“画图函数”这是后面很多问题的源头。障碍物不能只在绘图时存在它必须同时出现在约束构造、最小距离统计和失败回放里。我一般把障碍物定义成obs [x, y, vx, vy, r]的矩阵每一行一个障碍物动态障碍物按匀速模型预测未来位置。目标点则单独维护参考轨迹由目标点和当前状态生成不跟障碍物混在一起。这样换场景时只改obs和目标点优化层代码不动。预测层的关键是“预测模型可以比真实对象简单但约束必须覆盖真实边界”。避障层用质点双积分模型底层姿态环和速度环由飞控负责模型失配留给底层吸收。优化层不做花哨的智能算法核心就是一个带线性约束的二次规划。执行层只执行第一拍这是 MPC 和普通轨迹规划最不一样的地方规划器给出 N 步但只下发第一步下一拍重新求解。日志里要记录exitflag、最小安全距离、求解耗时和松弛变量最大值这几个字段后面验收时比轨迹图更有说服力。2.2 状态量与控制量定义为什么选双积分质点模型状态量取[x; y; vx; vy]控制量取[ax; ay]采样周期Ts 0.1 s。这个选择不是偷懒而是避障层关心的是几何可行性无人机能不能在 2 秒内从障碍物左侧绕过去速度够不够加速度会不会超限。完整的六自由度模型当然更真实但状态维数一高预测矩阵和约束构造的调试成本会迅速上升做毕设时很容易陷在模型参数里出不来。质点模型把问题压到四个状态、两个控制量QP 规模小扫参数快先把避障逻辑跑通再考虑加高度通道。三维扩展也不复杂状态加z和vz控制加az避障约束从圆变成球半平面约束仍然成立。真正需要小心的是坐标系。我一般统一用世界坐标系做约束和代价计算机体坐标系只在绘图和底层控制接口处出现。角度单位统一用弧度矩阵行列顺序在注释里写清楚别让x和y在某个函数里悄悄换了位置。参数表放在第 3 章统一列这里先记住一个原则速度上限、加速度上限和安全距离必须来自同一个场景假设不能速度按 5 m/s 调安全距离按 1 m/s 算。2.3 仿真主循环与数据流从状态更新到可视化主循环是整套方案最该先写对的部分。很多翻车不是优化器的问题而是主循环里把“预测的第一拍”和“仿真的第一拍”混了或者求解失败后继续用上一次控制量最后轨迹看着能跑日志里全是异常。下面这段框架代码只保留关键动作方便对照自己的工程。% 仿真主循环每个控制周期只执行第一拍控制量 x x0; log struct(x, [], u, [], exitflag, [], minDist, []); for t 1:round(T_sim/Ts) ref build_reference(t, x); % 生成 N 步参考状态 [U, exitflag] solve_mpc(x, ref, obs, Phi, Gamma, Q, R, N, d_safe, u_max); if exitflag 0 U zeros(size(U)); % 求解失败时退回零加速度先制动 end u U(1:2); % 只取第一拍 x A*x B*u; % 用同一离散模型推进仿真 log record(log, x, u, exitflag, obs); end这段代码里有两个设计点需要说明。第一U(1:2)只取第一拍不是把 N 步控制量依次执行MPC 的滚动优化靠的就是下一拍重新求解。第二exitflag 0时不要静默沿用上一次解至少退回零加速度并记录否则验收时你无法解释“为什么求解失败但轨迹还在走”。record函数要把当前状态、控制量、求解状态和当前最小障碍物距离都存下来后面画图只是日志的附属品。仿真推进最好用比控制周期更小的积分步长比如控制周期 0.1 s、积分步长 0.01 s这样状态更新更接近连续模型。示例里为了可读性用了同一个Ts实际交付时建议把积分步长单独拿出来。还有一点参考轨迹不要给阶跃给一条从当前点到目标点的平滑直线或多项式否则权重整定会让你怀疑人生。3. 预测模型从双积分方程到 Phi/Gamma 矩阵3.1 简化质点模型与离散状态转移矩阵连续方程写成\dot{x}vx、\dot{y}vy、\dot{vx}ax、\dot{vy}ay用前向欧拉离散后得到的状态转移矩阵很干净。Ts0.1时位置更新里会出现0.5*Ts^2的加速度项速度更新里是Ts的加速度项。这个模型没有姿态角也没有推力饱和但避障层用它足够。真正需要补的是速度上限和加速度上限把它们作为 QP 的箱式约束不要让优化器给出一个物理上飞不出来的解。% 二维双积分质点模型状态 [x; y; vx; vy]控制 [ax; ay] Ts 0.1; A [1, 0, Ts, 0; 0, 1, 0, Ts; 0, 0, 1, 0; 0, 0, 0, 1]; B [0.5*Ts^2, 0; 0, 0.5*Ts^2; Ts, 0; 0, Ts];这段代码的参数含义很直接A的前两行把速度积分成位置后两行保持速度B的前两行是加速度对位置的二次贡献后两行是加速度对速度的线性贡献。Ts改大离散误差会变大避障约束会更保守Ts改小预测步数要相应增加否则预测时域太短无人机看到障碍物时已经来不及绕。我一般从Ts0.1、N20起步预测时域 2 秒对应室内低速场景够用。3.2 预测时域内的状态堆叠预测时域内把所有未来状态堆成一个长向量X Phi*x0 Gamma*U。Phi的维数是4N x 4Gamma是4N x 2NU是2N x 1。这个形式的好处是跟踪误差代价和避障约束都可以写成U的线性函数或二次函数直接丢给quadprog。如果不用堆叠形式每个预测步单独写循环约束矩阵拼起来容易错行调试时很难定位是第几步、第几个障碍物出的问题。function [Phi, Gamma] build_prediction_matrices(A, B, N) % 构造 X Phi*x0 Gamma*U示例取控制时域等于预测时域 nx size(A, 1); nu size(B, 2); Phi zeros(nx*N, nx); Gamma zeros(nx*N, nu*N); for k 1:N rows (k-1)*nx (1:nx); Phi(rows, :) A^k; for j 1:k cols (j-1)*nu (1:nu); Gamma(rows, cols) A^(k-j) * B; end end end这个函数的逻辑是第k步状态由初始状态经过k次A作用加上每一步控制量经过相应次数的A作用得到。代码里控制时域等于预测时域所以Gamma的列数是2N工程上常把控制时域Nc降到 8 到 10减少决策变量但要把Nc之后的控制量锁在最后一拍矩阵构造会多一个保持项。为了先把避障逻辑跑通示例不引入这个分支等主循环稳定后再降Nc。3.3 代码实现构建 A/B 与预测矩阵的调用方式把上面的模型和矩阵构造串起来初始化部分通常长这样。注意Q和R要是块对角矩阵维数分别跟状态堆叠和控制堆叠对齐不要拿单步的4x4矩阵直接乘Gamma否则 MATLAB 会报维度错误或者更糟广播出一个你以为对但实际错的结果。% 初始化预测矩阵和权重 N 20; [Phi, Gamma] build_prediction_matrices(A, B, N); Q kron(eye(N), diag([10, 10, 1, 1])); % 位置权重大速度权重小 R kron(eye(N), diag([0.1, 0.1])); % 控制量惩罚Q的位置权重取 10、速度权重取 1是为了让跟踪误差主要压位置R取 0.1先让控制量有惩罚但不至于让无人机“懒得动”。这些值不是标准答案场景变大、速度变快时要重新扫。kron的用法是把单步权重复制成块对角eye(N)的顺序要和Phi的行块顺序一致。代码后面如果加控制增量惩罚Rdu要把U改成Delta U的形式或者单独构造差分矩阵不要直接把Rdu加到R上那样惩罚的物理含义就混了。3.4 参数表与初值建议参数含义初值调整方向Ts控制周期0.1 s室内低速可保持高速场景减小N预测时域步数20先增到 30 看求解时间不要盲目上 50Nc控制时域步数20熟练后降到 8~10 减少决策变量v_max速度上限5 m/s按场景缩放不要超过底层控制器能力a_max加速度上限3 m/s²太小绕不过去太大轨迹抖d_safe安全距离1.0 m由障碍物半径、无人机半径和余量相加Q_pos位置权重10跟踪慢就加抖动先查RR控制量权重0.1抖动加大绕障迟缓减小这张表我一般直接放在工程根目录的params.m里主脚本只调用不写死。参数表最大的价值不是初值多准而是让你在答辩时能解释“为什么是 20 不是 50”20 步对应 2 秒预测时域覆盖当前速度下 10 米左右的制动距离求解时间在 0.05 秒以内满足 0.1 秒控制周期。这个解释比“我试出来的”更站得住。4. 避障约束把“别撞”写进二次规划4.1 障碍物建模球形膨胀与动态障碍物预测障碍物用圆形表示圆心(ox, oy)半径r_obs。安全距离不是简单加一个障碍物半径而是d_safe r_obs r_uav v_max*T_delay margin。r_uav是无人机外接圆半径T_delay是控制延迟和通信延迟之和margin是给模型误差留的余量。室内小场景我一般取r_obs0.5、r_uav0.3、margin0.2d_safe落在 1.0 米左右。这个值太小仿真看着没事实物一擦就出问题太大狭窄通道直接不可行。动态障碍物按匀速模型预测obs_k obs0 v_obs * k * Ts。预测时域内每个障碍物在每个预测步都生成一条约束障碍物数量乘以预测步数就是约束行数。10 个障碍物、20 步预测就是 200 条半平面约束quadprog还能吃下如果障碍物到 30 个就要考虑只保留距离最近的几个或者把约束从每步都加改成每两步加一次。别一上来就上 50 个障碍物求解时间会把控制周期吃光。4.2 代价函数跟踪误差、控制量与平滑项代价函数写成J sum (X_k - ref_k) Q (X_k - ref_k) U R U Delta U Rdu Delta U。第一项拉跟踪第二项压控制量第三项压控制增量。第三项是防“画龙”的关键只有Q和R时无人机可能在两个障碍物之间左右切换轨迹像蛇一样摆。加上Delta U惩罚后控制量变化变缓轨迹更顺但绕障响应会慢半拍所以Rdu不要一开始就调大先看轨迹再决定。权重整定的顺序我一般这样走先把R设小、Rdu0只看跟踪轨迹能跟上后加R压抖动最后加Rdu压控制增量。每次只动一个参数跑三组场景对比。不要同时改Q、R、Rdu否则你无法判断是哪个参数起了作用。参考轨迹也要平滑阶跃参考会让任何权重都显得不对。4.3 用 quadprog 求解带约束的 MPC避障约束||p_k - p_obs|| d_safe是非凸的不能直接进quadprog。常用做法是在标称预测轨迹上做一次线性化把圆约束变成半平面约束n * p_k n * p_obs d_safe其中n是从障碍物指向标称预测位置的单位向量。这个半平面是圆在标称点处的切平面保守但可解。标称轨迹可以用上一拍解也可以先用零控制量预测一次。下面这段代码把跟踪代价、输入幅值约束和避障半平面约束组装起来。function [U, exitflag] solve_mpc(x0, ref, obs, Phi, Gamma, Q, R, N, d_safe, u_max) nx 4; nu 2; nU nu * N; % 二次代价J (Phi*x0 Gamma*U - ref)Q(...) URU H 2 * (Gamma * Q * Gamma R); f 2 * Gamma * Q * (Phi * x0 - ref); H (H H) / 2; % 消除数值不对称 lb -u_max * ones(nU, 1); ub u_max * ones(nU, 1); Aineq []; bineq []; % 用零控制量预测作为标称轨迹逐障碍物、逐预测步线性化 X0 Phi * x0; for k 1:N rows (k-1)*nx (1:2); % 取位置 x,y p0 X0(rows); for j 1:size(obs, 1) dp p0 - obs(j, 1:2); dist norm(dp); if dist 1e-6 dp [1; 0]; dist 1; end n dp / dist; % n*(Phi_k*x0 Gamma_k*U) n*obs d_safe Aineq [Aineq; -n * Gamma(rows, :)]; bineq [bineq; n * Phi(rows, :) * x0 - n * obs(j, 1:2) - d_safe]; end end options optimoptions(quadprog, Display, off); [U, ~, exitflag] quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options); end这段代码的输入里x0是当前状态ref是4N x 1的参考状态堆叠obs每行一个障碍物Phi和Gamma是预测矩阵。H和f的维数分别是2N x 2N和2N x 1Aineq的行数是N * 障碍物数每行对应一个预测步、一个障碍物的半平面约束。bineq的推导来自把p_k Phi_k*x0 Gamma_k*U代入半平面不等式移项后得到-n*Gamma_k*U n*Phi_k*x0 - n*obs - d_safe。代码里X0 Phi*x0是零控制量标称轨迹如果上一拍有解用上一拍解预测会更贴近当前可行域。提示线性化只做一次时如果标称轨迹离障碍物很近半平面会非常保守甚至把可行域切没。我一般会迭代两次先用零控制量解一次再用第一次解作为标称轨迹重新线性化第二次的解通常更合理。4.4 约束不可行时的松弛策略硬约束在障碍物突然靠近、初始状态已经在安全距离内、或者狭窄通道宽度小于2*d_safe时必然不可行。quadprog返回负exitflag如果代码不检查继续用零向量或上一次解轨迹就会看起来“正常”但实际已经失去避障保证。软约束的做法是在决策变量后追加N个松弛量s_k约束改成n*p_k s_k n*p_obs d_safe代价里加rho * sum(s_k)。rho取1e3到1e5太小会穿障碍太大等于硬约束。% 软约束思路决策变量扩成 [U; s]约束矩阵右侧加松弛项 % Aineq_soft [Aineq, -eye(N)]; bineq_soft bineq; % 目标函数追加 rho*sum(s)s 0这段伪代码只表达结构实际实现时要注意松弛量只加在避障约束上不要加到输入幅值约束上。输入幅值超限是物理不可接受的避障约束在极端场景下可以短暂松弛但日志里必须记录max(s)。验收时如果看到max(s)长期大于 0.1 米说明安全距离或场景设置有问题不是靠调大rho能盖过去的。注意松弛变量不是“允许撞”而是让求解器在极端场景下给出可解释的退化解。交付时要把松弛量曲线和最小距离曲线一起放别只放轨迹图。5. 避坑与排查五个让仿真结果不可信的高频问题5.1 预测时域太长导致求解超时现象是单步求解时间从 0.03 秒涨到 0.3 秒控制周期 0.1 秒轨迹明显滞后转弯时像“反应慢半拍”。原因通常不是 MATLAB 慢而是N从 20 加到 50 后决策变量变成 100 维避障约束行数按N * 障碍物数增长quadprog内点法迭代次数上升。解决顺序是先降Nc到 8 到 10再把障碍物按距离排序只保留最近 5 个最后才考虑稀疏矩阵或换求解器。不要一上来就上热启动先把问题规模压下来。我见过一个典型翻车A 同学把N调到 60说“预测越远越安全”结果单步求解 0.4 秒仿真步长还是 0.1 秒轨迹看起来绕过了障碍物但实际控制量是 0.4 秒前的解等效于延迟 4 拍。验收时一测最小距离比N20还差。预测时域不是越长越好它要覆盖当前速度下的制动距离不是覆盖整个地图。5.2 障碍物膨胀半径拍脑袋现象是仿真里无人机离障碍物还有一段距离实物或高保真模型里已经擦碰或者反过来路径过度保守明明能过的通道非要绕大圈。原因通常是d_safe只加了障碍物半径没算无人机外接圆、定位误差、控制延迟和离散时间内的位移。解决方法是把d_safe拆成四项障碍物半径、无人机半径、v_max * T_delay、固定余量。每一项都写进参数表答辩时能逐项解释。动态障碍物还要多一层预测位置用的是匀速模型如果障碍物会变速d_safe要再留余量。我一般会做一次敏感性测试把d_safe从 0.8 扫到 1.4看最小距离和路径长度怎么变。如果d_safe增加 0.1 米路径长度暴涨说明场景本身太窄不是算法问题。5.3 权重整定让无人机“画龙”现象是轨迹左右摆动控制量高频抖动看起来像在障碍物前面“犹豫”。原因多半是Q位置权重大、R太小、Rdu没加再加上参考轨迹是阶跃。解决顺序是先把参考轨迹改成平滑直线或多项式再加大R最后加Rdu。如果还抖检查控制周期是不是太小Ts0.02时噪声和离散化误差都会被放大避障层不需要这么高的控制频率。权重不是玄学但确实需要一点血泪经验每次只改一个参数跑静态单障碍、动态交叉、狭窄通道三组场景记录路径长度、最小距离、最大加速度。不要只看一张轨迹图就定参数那张图可能刚好是运气好。5.4 坐标系数值对不上现象是障碍物在图上画在无人机正前方约束却认为它在左边或者无人机明明往右绕日志里minDist却先减小后增大。原因通常是世界坐标系和机体坐标系混用角度单位弧度/度没统一或者矩阵行列顺序在某个函数里换了。解决方法是所有约束和代价统一在世界坐标系绘图函数只做显示转换不参与计算。再写一个单元测试障碍物放在(5,0)无人机在原点预测第一步的n应该接近[1;0]半平面约束应该把无人机往负x方向推。5.5 求解器失败被静默忽略现象是轨迹跑完了看起来没撞但日志里exitflag有一半是 0 或负值。原因是代码只取U不检查exitflagquadprog失败时返回的U可能是空、可能是上一次解、也可能是不可行点。解决方法是每次求解后检查exitflag失败时降级为零加速度或制动并记录失败原因和当前松弛量。验收时把失败率作为硬指标动态场景失败率超过 1% 就要回头查约束和初始状态。提示失败日志要能回放。只存最终轨迹没用至少存每拍的x、u、exitflag、minDist和max(s)否则你无法解释“为什么第 137 拍突然向右打了一把”。6. 验收与答辩三组场景、一张指标表和四个必答问题验收不要只看一张漂亮的轨迹图。我一般固定三组场景静态单障碍验证基本绕障动态交叉障碍验证预测时域和膨胀半径狭窄通道验证约束不可行时的松弛和降级。每组场景跑 10 次初始状态加小幅扰动记录最小安全距离、跟踪 RMSE、最大加速度、单步求解时间和求解失败率。指标表比轨迹图更能说明方案可交付。场景指标判定阈值记录方式静态单障碍最小距离≥ 1.0 m每拍计算动态交叉最小距离≥ 0.9 m每拍计算狭窄通道求解失败率≤ 1%exitflag统计全部场景跟踪 RMSE≤ 0.15 m稳态段统计全部场景最大加速度≤ 3 m/s²控制量日志全部场景单步求解时间≤ 0.08 stic/toc答辩时四个问题基本绕不开。第一“为什么用 MPC 不用 A* 或反应式规则”回答要落到滚动优化和约束显式处理A* 给路径不保证动力学可行反应式规则在动态障碍前容易抖MPC 把未来 N 步的避障和动力学约束放进同一个优化问题。第二“预测时域怎么选”回答 20 步对应 2 秒覆盖当前速度下制动距离不是拍脑袋。第三“约束不可行怎么办”回答软约束加松弛、失败降级、日志记录不要只说“调大安全距离”。第四“权重怎么整定”回答先跟踪、再压控制量、最后加平滑项每次只动一个参数用三组场景对比。从那以后我每次交付前都强制走一遍失败日志回放不再只看最终轨迹图。轨迹图会骗人日志不会。希望帮到你。这套 MATLAB 工程我按模块拆好了需要对着源码逐行核对的读者可以在资源页取这份压缩包先跑主循环再改参数表最后用失败日志回放验收。本文还有配套的精品资源点击获取