拓冰网站定制 
很多刚开始接触四旋翼无人机的朋友第一反应都是找个Matlab仿真跑一跑。搜一圈下来PID控制、串级控制、Simulink模型满天飞但真正能看懂、能自己改参数、能复现整个闭环过程的完整代码其实不多。更常见的情况是模型文件一大堆参数脚本上百行PID藏在某个S-function里点一下运行倒是能出图可是每条曲线代表什么、为什么会振荡、参数改了有没有用完全是一头雾水。这篇文章打算把整个流程拆开从最底层讲起。我会用纯Matlab脚本加一个动力学函数不依赖Simulink搭一个完整的四旋翼无人机PID控制仿真。代码会完整贴在正文里每一段都会解释在干什么、为什么这么写。物理模型用12维状态方程描述控制结构按悬停场景设计成串级PID外环跟踪姿态角内环跟踪角速度高度环独立闭环。仿真目标很简单——初始时刻给无人机一个姿态扰动让它通过PID控制自动恢复悬停并完成高度爬升到期望值。无论你是做毕业设计、准备电赛还是想为后面玩PX4、ArduPilot这类开源飞控打基础这套代码都可以作为你第一次亲手搭起来的完整闭环。它不花哨但每个环节都能讲出道理这比跑通一个别人写好的复杂工程有用得多。1. 仿真不是Makeup先想清楚控制回路里谁管谁1.1 为什么找个现成模型直接跑对新手最坑我见过太多这种场景从开源社区下载一个四旋翼仿真工程里面有十几二十个.m文件跑之前还要配置路径、加载参数脚本、初始化Simulink模型。好不容易跑通了改一个PID参数曲线完全没反应或者直接发散到NaN然后就没有然后了。问题不在于代码写得不好而在于你跳过了建模和控制器设计这两步最关键的学习过程。你没有亲手定义过状态量就不知道仿真结果里哪些量是可信的你没有亲手推过模型方程就不知道某些简化假设会带来什么后果。最后论文里能写的只是采用了PID控制方法进行仿真验证问细节就露馅。所以我强烈建议新手从最简版本开始搭。所谓最简不是功能少而是每一行代码你都看得懂、都能解释清楚。等这个基础闭环跑通再去研究更复杂的工程化模型也不迟。1.2 从外到内理一遍位置、姿态、角速度、电机四旋翼的控制回路本质上是一条误差逐级消除的链条。外环的期望值是位置和航向但四旋翼这个执行机构有个特点它不能直接产生水平方向的作用力只能靠倾斜机体来获得水平分量。所以要改变水平位置必须先改变姿态角。这条因果链是这样的期望位置 与 实际位置 的偏差换算成期望姿态角期望姿态角 与 实际姿态角 的偏差换算成期望角速度期望角速度 与 实际角速度 的偏差换算成期望力矩期望力矩 加上 期望升力通过混控器变成四个电机的拉力最终由电机转速变化实现。理解这条链是今天所有内容的地基。很多人调PID越调越乱就是因为把外环的误差当成内环的输入去用或者反过来把内环的误差直接作用到电机上回路层次一乱参数就永远调不好。1.3 本文仿真的边界悬停工况下的最小完整闭环这里先明确仿真边界避免期待错位。本文不做航迹跟踪不做复杂机动不做风场干扰。只做一件事四旋翼在某个初始姿态扰动和高度的偏差下通过PID闭环自动回到期望悬停状态。期望值设定为高度10米滚转角、俯仰角、偏航角全部为0。初始状态设为高度8米滚转角0.3弧度俯仰角-0.25弧度偏航角0.1弧度角速度和线速度都是0。这种情况下你会看到非常直观的控制过程无人机先偏着机身然后逐渐摆正同时缓缓爬升到10米高度最后稳定悬停。这个场景虽然简单但完整覆盖了四旋翼仿真的所有核心要素动力学模型、串级PID、控制分配、数值积分、调参分析。把这套逻辑跑通后面做轨迹跟踪只是在这个基础上再加一个位置外环而已。2. 动力学模型一个能看懂的12维状态方程2.1 状态向量位置、速度、姿态、角速度四旋翼在三维空间里的运动状态可以用12个变量完整描述。这套状态定义几乎是行业通用的后面你去看PX4的EKF源码、看任何一篇四旋翼动力学论文都能对上号。状态向量state [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]含义如下变量含义单位x, y, z机体质心在世界坐标系中的位置mvx, vy, vz三个方向的线速度m/sphi, theta, psi滚转角、俯仰角、偏航角欧拉角radp, q, r机体坐标系下的角速度分量rad/s注意姿态角用的是欧拉角这在悬停小角度工况下非常方便人能直观理解。但如果后面要做大机动欧拉角会遇到万向锁问题那就得换四元数这个我们在扩展部分再讲。2.2 加速度怎么算牛顿-欧拉方程的小角度近似动力学方程是整个仿真的发动机它告诉我们在给定控制输入下状态量会怎么变化。把四旋翼当成一个刚体控制输入为总推力 u1 和三轴力矩 u2、u3、u4。总推力方向沿机体Z轴所以把推力从机体坐标系映射到世界坐标系时要经过姿态角的旋转。这里我直接写出完整的坐标变换关系ax u1/m * (sin(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)) ay u1/m * (-sin(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)) az -g u1/m * (cos(phi)*cos(theta))三个角加速度方程在小角度假设下可以简化为p_dot u2 / Ixx q_dot u3 / Iyy r_dot u4 / Izz姿态角速率和角速度的关系我直接用了最简形式phi_dot p、theta_dot q、psi_dot r。严格来说欧拉角导数和角速度之间有一个矩阵变换关系中间还包含sin(phi)*tan(theta)这样的项但悬停附近角度很小这些高阶项可以忽略。这就是小角度近似的含义。它牺牲了一定的精确性换来了直观性和可读性。对新手来说先通过这个模型把控制逻辑吃透比一上来就怼四元数微分方程要友好得多。2.3 控制分配如何把油门和三轴力矩反解成四个电机推力PID控制器算出来的是总推力和三个轴的力矩但真正的执行机构是四个电机。从总推力力矩到四个电机拉力的变换在飞控领域叫混控器Mixer原理是把期望的合力与合力矩分配到各个电机上。假设一个X型四旋翼四个电机的布局为M1在前M2在右M3在后M4在左。定义电机编号沿机架顺时针排列则控制分配公式为F1 u1/4 - u3/(4*l) - u4/(4*km) F2 u1/4 u2/(4*l) u4/(4*km) F3 u1/4 u3/(4*l) - u4/(4*km) F4 u1/4 - u2/(4*l) u4/(4*km)其中 l 是机臂长度km 是偏航力矩系数。式子里的加减号反映了俯仰力矩主要由前后两个电机的推力差产生滚转力矩由左右两个电机的推力差产生偏航力矩由两组反向旋转电机的转速差产生。这里特别提醒一句混控器公式与电机编号、旋向定义强相关。不同开源飞控的规定不一样你在我这个仿真里看到的是这套定义换到PX4里可能是另一套符号。但只要逻辑一致控制效果等价。2.4 物理参数表与选择依据仿真里用到的物理参数多数来自小型四旋翼的典型值比如常用轴距450毫米左右、起飞重量1到1.5公斤的机架可参考如下取值参数数值说明m1.2 kg起飞总质量g9.8 m/s^2重力加速度Ixx0.03 kg*m^2绕机体X轴转动惯量Iyy0.03 kg*m^2绕机体Y轴转动惯量Izz0.05 kg*m^2绕机体Z轴转动惯量l0.25 m电机到质心的距离km0.012偏航力矩与推力差的换算系数注意一个关键约束悬停时总推力必须等于重力也就是 u1 约等于 11.76 N。对应四个电机每个电机需要输出约 2.94 N 的拉力。如果电机推力上限设得太小仿真一开始就会因为推力饱和而爬不上去。我在代码里把单电机的推力范围设为 0.1 到 15 N留出足够余量同时避免仿真中推力出现负值这种物理上不可能的情况。3. 串级PID外环给角速度、内环给力矩为什么不能只用一个PID3.1 单环PID为什么在四旋翼上行不通如果只用一个角度环PID把期望姿态角和实际姿态角的误差直接映射成电机力矩会面临一个尴尬的问题悬停在半空中的四旋翼当机体受到扰动开始转动时你测量到的角速度响应和角度响应是耦合在一起的。角度变化是角速度的积分也就是说角度环PID实际上在控制一个二阶积分对象比例增益稍微给大一点就容易振荡给小了又回正得很慢。更关键的是角速度这个中间量本身存在高频扰动。比如电机振动、突风引起的瞬间力矩变化都会首先反映在角速度上然后才慢慢积累成角度偏差。如果只用角度环等到角度误差大到能产生足够的控制量时扰动已经作用了很久了。3.2 内环快、外环慢时间尺度分离的本质串级PID的思路是把过程中的中间状态也闭环起来让控制回路变成一个梯队内环角速度环负责把角速度控制到期望值。它直接面对高频扰动响应快是系统稳定性的核心。外环角度环负责把姿态角控制到期望值。它的输出不是执行机构的指令而是给内环的期望角速度。因为内环比外环快得多我们可以认为当外环在计算时内环已经完全跟踪上了目标。时间尺度分离是串级控制能成立的前提。这个设计和开车非常像。你不会直接打死方向盘来修正行驶路线而是先控制车头的转动速度再根据车头方向不断微调方向盘。内环是手外环是眼睛手必须比眼睛反应快。3.3 三通道统一套路与积分限幅滚转、俯仰、偏航三个通道都可以套用同一个套路角度误差 - 角度环PID - 期望角速度 - 角速度误差 - 角速度环PID - 期望力矩高度通道也类似高度误差 - 高度环PID - 期望垂向加速度 - u1 m*(g az_cmd)代码里积分项都加了限幅。这是新手最容易忽略的地方。积分项的作用是消除稳态误差但如果误差一直存在积分项会无限累加导致控制量长期处于饱和状态等误差反向时系统却反应不过来这就是积分饱和。限幅的意义在于把积分项的贡献限制在一个可控范围内物理意义是我最多容忍积分项给你加这么多额外输出超过这个范围就封顶。4. 完整代码主脚本、动力学函数与控制分配逐段拆解4.1 不依赖Simulink的脚本组织方式这套代码由两个文件组成主脚本quad_sim_main.m和动力学函数quadRotorDynamics.m。把动力学单独写成函数是为了让主循环代码更清爽也方便你以后替换成更复杂的模型。运行环境要求Matlab R2016b以上不需要任何额外的工具箱纯基础函数就能跑。主脚本完成五件事定义参数、定义PID增益、初始化状态、运行仿真循环、绘制曲线。动力学函数只负责一件事输入当前状态和控制量输出状态导数。4.2 完整主脚本%% 四旋翼无人机 PID 控制仿真从零手写版 % 状态量: [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r] % 控制目标: 期望高度10m期望姿态角0从初始扰动恢复悬停 clear; clc; close all; %% 1. 物理参数与仿真参数 param.m 1.2; % 质量 kg param.g 9.8; % 重力加速度 m/s^2 param.Ixx 0.03; % X轴转动惯量 kg*m^2 param.Iyy 0.03; % Y轴转动惯量 kg*m^2 param.Izz 0.05; % Z轴转动惯量 kg*m^2 param.l 0.25; % 机臂长度 m param.km 0.012; % 偏航力矩系数 param.u1_min 4; % 总推力下限 N param.u1_max 18; % 总推力上限 N param.u2_max 2; % 横滚/俯仰力矩限幅 N*m param.u4_max 0.5; % 偏航力矩限幅 N*m dt 0.01; % 仿真步长 s T 15; % 仿真时间 s N round(T / dt); % 总步数 %% 2. 期望值 desired.z 10; desired.phi 0; desired.theta 0; desired.psi 0; %% 3. PID 参数 % 高度环外环产生期望垂向加速度 ctrl.Kp_z 8; ctrl.Ki_z 1; ctrl.Kd_z 5; % 滚转角环外环产生期望角速度 p_des ctrl.Kp_phi 5; ctrl.Ki_phi 0.5; ctrl.Kd_phi 0.3; % 滚转角速度环内环产生力矩 u2 ctrl.Kp_p 15; ctrl.Ki_p 3; ctrl.Kd_p 0.5; % 俯仰角环与角速度环 ctrl.Kp_theta 5; ctrl.Ki_theta 0.5; ctrl.Kd_theta 0.3; ctrl.Kp_q 15; ctrl.Ki_q 3; ctrl.Kd_q 0.5; % 偏航角环与角速度环 ctrl.Kp_psi 4; ctrl.Ki_psi 0.5; ctrl.Kd_psi 0.2; ctrl.Kp_r 10; ctrl.Ki_r 2; ctrl.Kd_r 0.4; %% 4. 初始状态 state [0; 0; 8; 0; 0; 0; 0.3; -0.25; 0.1; 0; 0; 0]; %% 5. 存储数据 t_arr zeros(1, N 1); state_arr zeros(12, N 1); u_arr zeros(4, N 1); F_arr zeros(4, N 1); state_arr(:, 1) state; t_arr(1) 0; % PID 积分与微分变量初始化 err_z_i 0; err_z_prev 0; err_phi_i 0; err_phi_prev 0; err_p_i 0; err_p_prev 0; err_theta_i 0; err_theta_prev 0; err_q_i 0; err_q_prev 0; err_psi_i 0; err_psi_prev 0; err_r_i 0; err_r_prev 0; %% 6. 主仿真循环 for k 1:N t k * dt; x state(1); y state(2); z state(3); vx state(4); vy state(5); vz state(6); phi state(7); theta state(8); psi state(9); p state(10); q state(11); r state(12); % ------------------- 高度控制 ------------------- err_z desired.z - z; err_z_i err_z_i err_z * dt; err_z_i max(-2, min(2, err_z_i)); % 积分限幅 err_z_d (err_z - err_z_prev) / dt; az_cmd ctrl.Kp_z * err_z ctrl.Ki_z * err_z_i ctrl.Kd_z * err_z_d; u1 param.m * (param.g az_cmd); u1 max(param.u1_min, min(param.u1_max, u1)); err_z_prev err_z; % ------------------- 滚转控制phi - p - 力矩 ------------------- err_phi desired.phi - phi; err_phi_i err_phi_i err_phi * dt; err_phi_i max(-0.5, min(0.5, err_phi_i)); err_phi_d (err_phi - err_phi_prev) / dt; p_des ctrl.Kp_phi * err_phi ctrl.Ki_phi * err_phi_i ... ctrl.Kd_phi * err_phi_d; p_des max(-1, min(1, p_des)); % 期望角速度限幅 err_p p_des - p; err_p_i err_p_i err_p * dt; err_p_i max(-2, min(2, err_p_i)); err_p_d (err_p - err_p_prev) / dt; u2 ctrl.Kp_p * err_p ctrl.Ki_p * err_p_i ctrl.Kd_p * err_p_d; u2 max(-param.u2_max, min(param.u2_max, u2)); err_phi_prev err_phi; err_p_prev err_p; % ------------------- 俯仰控制theta - q - 力矩 ------------------- err_theta desired.theta - theta; err_theta_i err_theta_i err_theta * dt; err_theta_i max(-0.5, min(0.5, err_theta_i)); err_theta_d (err_theta - err_theta_prev) / dt; q_des ctrl.Kp_theta * err_theta ctrl.Ki_theta * err_theta_i ... ctrl.Kd_theta * err_theta_d; q_des max(-1, min(1, q_des)); err_q q_des - q; err_q_i err_q_i err_q * dt; err_q_i max(-2, min(2, err_q_i)); err_q_d (err_q - err_q_prev) / dt; u3 ctrl.Kp_q * err_q ctrl.Ki_q * err_q_i ctrl.Kd_q * err_q_d; u3 max(-param.u2_max, min(param.u2_max, u3)); err_theta_prev err_theta; err_q_prev err_q; % ------------------- 偏航控制psi - r - 力矩 ------------------- err_psi desired.psi - psi; err_psi_i err_psi_i err_psi * dt; err_psi_i max(-0.5, min(0.5, err_psi_i)); err_psi_d (err_psi - err_psi_prev) / dt; r_des ctrl.Kp_psi * err_psi ctrl.Ki_psi * err_psi_i ... ctrl.Kd_psi * err_psi_d; r_des max(-1, min(1, r_des)); err_r r_des - r; err_r_i err_r_i err_r * dt; err_r_i max(-2, min(2, err_r_i)); err_r_d (err_r - err_r_prev) / dt; u4 ctrl.Kp_r * err_r ctrl.Ki_r * err_r_i ctrl.Kd_r * err_r_d; u4 max(-param.u4_max, min(param.u4_max, u4)); err_psi_prev err_psi; err_r_prev err_r; % ------------------- 控制分配反解四个电机推力 ------------------- F1 u1/4 - u3/(4*param.l) - u4/(4*param.km); F2 u1/4 u2/(4*param.l) u4/(4*param.km); F3 u1/4 u3/(4*param.l) - u4/(4*param.km); F4 u1/4 - u2/(4*param.l) u4/(4*param.km); F [F1; F2; F3; F4]; F max(0.1, min(15, F)); % 单电机推力饱和 % ------------------- 状态更新欧拉法 ------------------- dstate quadRotorDynamics(state, [u1; u2; u3; u4], param); state state dt * dstate; % ------------------- 存储 ------------------- state_arr(:, k 1) state; u_arr(:, k 1) [u1; u2; u3; u4]; F_arr(:, k 1) F; t_arr(k 1) t; end %% 7. 绘图 figure(Name, 四旋翼PID仿真结果, Color, w); subplot(2, 2, 1); plot(t_arr, state_arr(3, :), b-, LineWidth, 1.5); hold on; plot(t_arr, desired.z * ones(size(t_arr)), r--, LineWidth, 1); xlabel(时间/s); ylabel(高度/m); title(高度跟踪); legend(实际高度, 期望高度, Location, best); grid on; subplot(2, 2, 2); plot(t_arr, state_arr(7, :) * 180/pi, r-, LineWidth, 1.2); hold on; plot(t_arr, state_arr(8, :) * 180/pi, g-, LineWidth, 1.2); plot(t_arr, state_arr(9, :) * 180/pi, b-, LineWidth, 1.2); xlabel(时间/s); ylabel(角度/deg); title(姿态角); legend(滚转 phi, 俯仰 theta, 偏航 psi, Location, best); grid on; subplot(2, 2, 3); plot(t_arr, state_arr(10, :) * 180/pi, r-, LineWidth, 1.2); hold on; plot(t_arr, state_arr(11, :) * 180/pi, g-, LineWidth, 1.2); plot(t_arr, state_arr(12, :) * 180/pi, b-, LineWidth, 1.2); xlabel(时间/s); ylabel(角速度/deg/s); title(机体角速度); legend(p, q, r, Location, best); grid on; subplot(2, 2, 4); plot(t_arr, F_arr(1, :), LineWidth, 1.2); hold on; plot(t_arr, F_arr(2, :), LineWidth, 1.2); plot(t_arr, F_arr(3, :), LineWidth, 1.2); plot(t_arr, F_arr(4, :), LineWidth, 1.2); xlabel(时间/s); ylabel(推力/N); title(四个电机推力); legend(F1, F2, F3, F4, Location, best); grid on;4.3 动力学子函数新建一个文件命名为quadRotorDynamics.m放在和主脚本同一个目录下。function dstate quadRotorDynamics(state, u, param) % 四旋翼动力学小角度近似下的12维状态导数 % u [u1总推力, u2滚转力矩, u3俯仰力矩, u4偏航力矩] x state(1); y state(2); z state(3); vx state(4); vy state(5); vz state(6); phi state(7); theta state(8); psi state(9); p state(10); q state(11); r state(12); u1 u(1); u2 u(2); u3 u(3); u4 u(4); m param.m; g param.g; Ixx param.Ixx; Iyy param.Iyy; Izz param.Izz; % 线性加速度小角度近似 ax u1/m * (sin(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)); ay u1/m * (-sin(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)); az -g u1/m * (cos(phi)*cos(theta)); % 姿态运动学与角加速度 phi_dot p; theta_dot q; psi_dot r; p_dot u2 / Ixx; q_dot u3 / Iyy; r_dot u4 / Izz; dstate [vx; vy; vz; ax; ay; az; phi_dot; theta_dot; psi_dot; p_dot; q_dot; r_dot]; end4.4 为什么不用ode45而是自己写积分循环这里有一个很多人会问的问题Matlab明明有强大的求解器为什么非要用最原始的欧拉法原因有两层。第一PID控制器是离散的它需要在每一个控制周期内读取状态、计算误差、累加积分项然后输出控制量。我用固定步长循环天然就是一个控制周期的结构。如果换成ode45整个状态更新时间步由求解器自动决定PID的积分项积分项就不知道在哪里更新——你只能把控制器放在一个事件函数里或者极不优雅地强行固定步长这完全违背了使用变步长求解器的初衷。第二真实飞控从来不用变步长求解器。飞控是一个实时的、固定周期控制系统主频就是控制频率通常250到1000赫兹。我的仿真步长取0.01秒对应100赫兹这个频率对本文的悬停场景完全够用。用欧拉法虽然数值精度不如高阶求解器但只要步长足够小、系统不发散结果就是可信的。工程仿真不是越精确越好而是满足分析需求即可。4.5 中文乱码问题代码和绘图里的中文标签在Matlab老版本里可能会显示为乱码。如果出现这种情况检查文件编码是否为UTF-8或者把绘图标签改成英文不影响运行。高版本Matlab里如果注释乱码一般是因为操作系统区域设置和文件编码不一致改成UTF-8编码保存即可解决。5. 调参实战初始值、发散原因、从曲线反推问题5.1 一组能用的初始参数与运行结果预期上面代码里给的那组PID参数直接复制运行就能得到一个稳定的结果。运行后你会看到什么高度曲线大约在3到4秒内从8米爬升到10米基本无超调或者最多轻微超调0.1米然后稳定在10米。姿态角曲线会更快收敛滚转角和俯仰角在1到2秒内回到0度附近中间有一次小幅回摆之后彻底稳定。偏航角收敛得稍微慢一点但也在4秒内归零。四个电机推力曲线在悬停稳定后都趋近于2.94牛这个值正好是重力平均到四个电机上的结果验证了模型的正确性。这个结果说明PID参数的量级和系统的物理参数是匹配的。如果换成其他的转动惯量或质量这套参数不一定还能稳定这就是为什么后面要学会自己调参。5.2 调参顺序先内环后外环先比例后微分调PID参数最忌讳一上来就六个环路一起调。正确顺序是从最内层开始一层层往外调。第一步调角速度环。把外环角度环的增益先设为0也就是让期望角速度恒为0然后给一个初始角速度扰动观察角速度能不能快速归零。这个环节只用调内环的Kp_p和Kd_p。Kp_p决定角速度被拉回来的力度Kd_p决定对突变响应的抑制作用。只要角速度能在0.5秒内稳定归零内环就算合格。第二步调角度环。把外环增益恢复期望角度设为0给一个初始角度扰动观察角度回正过程。这时候如果角度曲线出现明显的超调和回摆说明外环的Kp_phi偏大如果回正很慢说明Kp_phi偏小适当加Kd_phi可以加快收敛并抑制超调。第三步调高度环调法和角度环类似。高度环属于最外层响应最慢给它足够的反应时间。调参的口诀是先比例、后积分、再加微分。先把P调到临界振荡再稍微减小一点然后加D抑制超调最后用I消除稳态误差。这套方法在仿真里可以直接用因为你在电脑上能看到完整曲线。5.3 从仿真曲线反向定位问题很多时候系统发散或者效果差不是参数调不好而是不知道问题出在哪个环。这里给你几个最常见的曲线特征以及对应的原因仿真现象可能原因检查优先级角速度曲线高频剧烈振荡内环Kp_p过大或dt过大最高角速度缓慢爬坡、长时间不回零内环Kp_p过小最高角度回正很慢、无超调外环Kp_phi不足中角度大幅反复超调外环Kp_phi过大或内环响应太慢中高度始终有稳态误差高度环Ki_z不足中仿真初期推力冲到上限然后NaN初始姿态角过大或u1限幅设置过宽低有一个非常容易忽略的问题微分项对噪声敏感。如果你的状态量是从传感器模型里读出来的带测量噪声那么直接对误差做差分会把噪声放大得非常厉害。真实飞控里常用角速度反馈代替角度误差的微分量因为角速度本身就是角度误差微分的最直接体现。本文的仿真没有加传感器噪声所以用差分法没问题但如果你要做更真实的仿真建议把err_phi_d直接换成-p参与运算效果会干净很多。5.4 新手最容易踩的三个坑第一个坑是单位错误。期望角度和初始角度必须用弧度很多人赋值时顺手写了30系统直接以为你让它转30弧度结果当然发散。建议在代码注释里反复标注单位养成习惯。第二个坑是忘记加饱和。控制量必须限幅不仅是为了符合物理约束更是为了让仿真数值稳定。没有饱和的PID在初期大误差下可能产生一个天大的控制量欧拉积分一步就把状态推到不可思议的值之后怎么调都救不回来。所以我的代码里对u1、u2、u4以及期望角速度都做了限幅这是仿真稳定性的第一道防线。第三个坑是积分项不设限幅。高度从8米爬到10米需要好几秒时间期间高度误差一直存在如果积分项无限累积到达10米时积分项已经攒了很多额外出力系统会直接冲过去形成很大的超调。我的代码里对所有积分项都加了max和min限幅你可以试着把限幅值调大观察超调明显增大的现象这样对积分饱和的理解会非常深刻。6. 从悬停到航线这套仿真还能往哪个方向扩展6.1 加上水平位置控制从姿态控制到位置控制的关键一步这篇文章的仿真只做了高度和姿态闭环水平位置完全没有控。这也意味着初始状态里vx和vy即使为0x和y的位置误差也不会被自动修正。真正做航迹飞行的四旋翼必须在姿态环外面再套一个位置环逻辑和高度环几乎一样x方向误差经过位置环PID产生一个期望滚转角注意方向y方向误差经过位置环PID产生一个期望俯仰角把这两个期望角度作为姿态环的输入姿态环再继续往下走。加上这个位置环之后整套控制结构就变成三层串级位置环 → 姿态环 → 角速度环。层次多了但每个环的职责依旧清晰。你可以在这份代码的基础上给x和y也加上PID观察无人机能否自动回到原点这比Simulink里拖模块更能加深理解。6.2 模型升级全量姿态运动学与四元数小角度近似终究有适用范围。当无人机做大角度机动时比如快速翻滚、大俯仰爬升phi_dot p这种近似就不准了需要换成完整的欧拉角运动学方程甚至直接用四元数避免万向锁。此外真实四旋翼还存在陀螺力矩、电机动态响应滞后、旋翼入流效应等这些都可以逐步加进quadRotorDynamics.m里。每加一项你都会对四旋翼的真实行为多一分理解。从脚本迁移到Simulink也是个自然的扩展方向。Simulink的优势在于可视化搭积木调试方便你可以在Simulink里保留我这里串级PID的结构用PID Controller模块代替手写循环。但建议至少先把脚本版本吃透再迁移否则只是从一个黑盒换到另一个黑盒。6.3 下一步从仿真到实物飞控等你把Matlab仿真完全调明白可以再去看PX4或ArduPilot这类开源飞控的代码。你会发现它们飞行模式的底层逻辑其实就是这套串级PID只是增加了更完整的姿态表示、更复杂的滤波和状态估计、以及庞大的参数配置系统。有了这个仿真基础至少看到MPC_XY_P、MC_ROLL_P、MC_PITCHRATE_P这些参数时你能立刻知道它们在哪个环路、主要影响什么。这就是仿真学习最大的回报。我自己在带新人的时候一直坚持一个观点仿真能帮你学会控制逻辑和调参手感但替代不了对真实系统的敬畏。真实飞行中还有电池电压下降、振动、风扰、GPS漂移、电机一致性等问题仿真给不了的就只能靠多炸机、多复盘去积累了。希望这套代码能成为你从零开始的第一块垫脚石。 四旋翼无人机PID控制仿真：从零手写Matlab闭环代码
很多刚开始接触四旋翼无人机的朋友第一反应都是找个Matlab仿真跑一跑。搜一圈下来PID控制、串级控制、Simulink模型满天飞但真正能看懂、能自己改参数、能复现整个闭环过程的完整代码其实不多。更常见的情况是模型文件一大堆参数脚本上百行PID藏在某个S-function里点一下运行倒是能出图可是每条曲线代表什么、为什么会振荡、参数改了有没有用完全是一头雾水。这篇文章打算把整个流程拆开从最底层讲起。我会用纯Matlab脚本加一个动力学函数不依赖Simulink搭一个完整的四旋翼无人机PID控制仿真。代码会完整贴在正文里每一段都会解释在干什么、为什么这么写。物理模型用12维状态方程描述控制结构按悬停场景设计成串级PID外环跟踪姿态角内环跟踪角速度高度环独立闭环。仿真目标很简单——初始时刻给无人机一个姿态扰动让它通过PID控制自动恢复悬停并完成高度爬升到期望值。无论你是做毕业设计、准备电赛还是想为后面玩PX4、ArduPilot这类开源飞控打基础这套代码都可以作为你第一次亲手搭起来的完整闭环。它不花哨但每个环节都能讲出道理这比跑通一个别人写好的复杂工程有用得多。1. 仿真不是Makeup先想清楚控制回路里谁管谁1.1 为什么找个现成模型直接跑对新手最坑我见过太多这种场景从开源社区下载一个四旋翼仿真工程里面有十几二十个.m文件跑之前还要配置路径、加载参数脚本、初始化Simulink模型。好不容易跑通了改一个PID参数曲线完全没反应或者直接发散到NaN然后就没有然后了。问题不在于代码写得不好而在于你跳过了建模和控制器设计这两步最关键的学习过程。你没有亲手定义过状态量就不知道仿真结果里哪些量是可信的你没有亲手推过模型方程就不知道某些简化假设会带来什么后果。最后论文里能写的只是采用了PID控制方法进行仿真验证问细节就露馅。所以我强烈建议新手从最简版本开始搭。所谓最简不是功能少而是每一行代码你都看得懂、都能解释清楚。等这个基础闭环跑通再去研究更复杂的工程化模型也不迟。1.2 从外到内理一遍位置、姿态、角速度、电机四旋翼的控制回路本质上是一条误差逐级消除的链条。外环的期望值是位置和航向但四旋翼这个执行机构有个特点它不能直接产生水平方向的作用力只能靠倾斜机体来获得水平分量。所以要改变水平位置必须先改变姿态角。这条因果链是这样的期望位置 与 实际位置 的偏差换算成期望姿态角期望姿态角 与 实际姿态角 的偏差换算成期望角速度期望角速度 与 实际角速度 的偏差换算成期望力矩期望力矩 加上 期望升力通过混控器变成四个电机的拉力最终由电机转速变化实现。理解这条链是今天所有内容的地基。很多人调PID越调越乱就是因为把外环的误差当成内环的输入去用或者反过来把内环的误差直接作用到电机上回路层次一乱参数就永远调不好。1.3 本文仿真的边界悬停工况下的最小完整闭环这里先明确仿真边界避免期待错位。本文不做航迹跟踪不做复杂机动不做风场干扰。只做一件事四旋翼在某个初始姿态扰动和高度的偏差下通过PID闭环自动回到期望悬停状态。期望值设定为高度10米滚转角、俯仰角、偏航角全部为0。初始状态设为高度8米滚转角0.3弧度俯仰角-0.25弧度偏航角0.1弧度角速度和线速度都是0。这种情况下你会看到非常直观的控制过程无人机先偏着机身然后逐渐摆正同时缓缓爬升到10米高度最后稳定悬停。这个场景虽然简单但完整覆盖了四旋翼仿真的所有核心要素动力学模型、串级PID、控制分配、数值积分、调参分析。把这套逻辑跑通后面做轨迹跟踪只是在这个基础上再加一个位置外环而已。2. 动力学模型一个能看懂的12维状态方程2.1 状态向量位置、速度、姿态、角速度四旋翼在三维空间里的运动状态可以用12个变量完整描述。这套状态定义几乎是行业通用的后面你去看PX4的EKF源码、看任何一篇四旋翼动力学论文都能对上号。状态向量state [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]含义如下变量含义单位x, y, z机体质心在世界坐标系中的位置mvx, vy, vz三个方向的线速度m/sphi, theta, psi滚转角、俯仰角、偏航角欧拉角radp, q, r机体坐标系下的角速度分量rad/s注意姿态角用的是欧拉角这在悬停小角度工况下非常方便人能直观理解。但如果后面要做大机动欧拉角会遇到万向锁问题那就得换四元数这个我们在扩展部分再讲。2.2 加速度怎么算牛顿-欧拉方程的小角度近似动力学方程是整个仿真的发动机它告诉我们在给定控制输入下状态量会怎么变化。把四旋翼当成一个刚体控制输入为总推力 u1 和三轴力矩 u2、u3、u4。总推力方向沿机体Z轴所以把推力从机体坐标系映射到世界坐标系时要经过姿态角的旋转。这里我直接写出完整的坐标变换关系ax u1/m * (sin(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)) ay u1/m * (-sin(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)) az -g u1/m * (cos(phi)*cos(theta))三个角加速度方程在小角度假设下可以简化为p_dot u2 / Ixx q_dot u3 / Iyy r_dot u4 / Izz姿态角速率和角速度的关系我直接用了最简形式phi_dot p、theta_dot q、psi_dot r。严格来说欧拉角导数和角速度之间有一个矩阵变换关系中间还包含sin(phi)*tan(theta)这样的项但悬停附近角度很小这些高阶项可以忽略。这就是小角度近似的含义。它牺牲了一定的精确性换来了直观性和可读性。对新手来说先通过这个模型把控制逻辑吃透比一上来就怼四元数微分方程要友好得多。2.3 控制分配如何把油门和三轴力矩反解成四个电机推力PID控制器算出来的是总推力和三个轴的力矩但真正的执行机构是四个电机。从总推力力矩到四个电机拉力的变换在飞控领域叫混控器Mixer原理是把期望的合力与合力矩分配到各个电机上。假设一个X型四旋翼四个电机的布局为M1在前M2在右M3在后M4在左。定义电机编号沿机架顺时针排列则控制分配公式为F1 u1/4 - u3/(4*l) - u4/(4*km) F2 u1/4 u2/(4*l) u4/(4*km) F3 u1/4 u3/(4*l) - u4/(4*km) F4 u1/4 - u2/(4*l) u4/(4*km)其中 l 是机臂长度km 是偏航力矩系数。式子里的加减号反映了俯仰力矩主要由前后两个电机的推力差产生滚转力矩由左右两个电机的推力差产生偏航力矩由两组反向旋转电机的转速差产生。这里特别提醒一句混控器公式与电机编号、旋向定义强相关。不同开源飞控的规定不一样你在我这个仿真里看到的是这套定义换到PX4里可能是另一套符号。但只要逻辑一致控制效果等价。2.4 物理参数表与选择依据仿真里用到的物理参数多数来自小型四旋翼的典型值比如常用轴距450毫米左右、起飞重量1到1.5公斤的机架可参考如下取值参数数值说明m1.2 kg起飞总质量g9.8 m/s^2重力加速度Ixx0.03 kg*m^2绕机体X轴转动惯量Iyy0.03 kg*m^2绕机体Y轴转动惯量Izz0.05 kg*m^2绕机体Z轴转动惯量l0.25 m电机到质心的距离km0.012偏航力矩与推力差的换算系数注意一个关键约束悬停时总推力必须等于重力也就是 u1 约等于 11.76 N。对应四个电机每个电机需要输出约 2.94 N 的拉力。如果电机推力上限设得太小仿真一开始就会因为推力饱和而爬不上去。我在代码里把单电机的推力范围设为 0.1 到 15 N留出足够余量同时避免仿真中推力出现负值这种物理上不可能的情况。3. 串级PID外环给角速度、内环给力矩为什么不能只用一个PID3.1 单环PID为什么在四旋翼上行不通如果只用一个角度环PID把期望姿态角和实际姿态角的误差直接映射成电机力矩会面临一个尴尬的问题悬停在半空中的四旋翼当机体受到扰动开始转动时你测量到的角速度响应和角度响应是耦合在一起的。角度变化是角速度的积分也就是说角度环PID实际上在控制一个二阶积分对象比例增益稍微给大一点就容易振荡给小了又回正得很慢。更关键的是角速度这个中间量本身存在高频扰动。比如电机振动、突风引起的瞬间力矩变化都会首先反映在角速度上然后才慢慢积累成角度偏差。如果只用角度环等到角度误差大到能产生足够的控制量时扰动已经作用了很久了。3.2 内环快、外环慢时间尺度分离的本质串级PID的思路是把过程中的中间状态也闭环起来让控制回路变成一个梯队内环角速度环负责把角速度控制到期望值。它直接面对高频扰动响应快是系统稳定性的核心。外环角度环负责把姿态角控制到期望值。它的输出不是执行机构的指令而是给内环的期望角速度。因为内环比外环快得多我们可以认为当外环在计算时内环已经完全跟踪上了目标。时间尺度分离是串级控制能成立的前提。这个设计和开车非常像。你不会直接打死方向盘来修正行驶路线而是先控制车头的转动速度再根据车头方向不断微调方向盘。内环是手外环是眼睛手必须比眼睛反应快。3.3 三通道统一套路与积分限幅滚转、俯仰、偏航三个通道都可以套用同一个套路角度误差 - 角度环PID - 期望角速度 - 角速度误差 - 角速度环PID - 期望力矩高度通道也类似高度误差 - 高度环PID - 期望垂向加速度 - u1 m*(g az_cmd)代码里积分项都加了限幅。这是新手最容易忽略的地方。积分项的作用是消除稳态误差但如果误差一直存在积分项会无限累加导致控制量长期处于饱和状态等误差反向时系统却反应不过来这就是积分饱和。限幅的意义在于把积分项的贡献限制在一个可控范围内物理意义是我最多容忍积分项给你加这么多额外输出超过这个范围就封顶。4. 完整代码主脚本、动力学函数与控制分配逐段拆解4.1 不依赖Simulink的脚本组织方式这套代码由两个文件组成主脚本quad_sim_main.m和动力学函数quadRotorDynamics.m。把动力学单独写成函数是为了让主循环代码更清爽也方便你以后替换成更复杂的模型。运行环境要求Matlab R2016b以上不需要任何额外的工具箱纯基础函数就能跑。主脚本完成五件事定义参数、定义PID增益、初始化状态、运行仿真循环、绘制曲线。动力学函数只负责一件事输入当前状态和控制量输出状态导数。4.2 完整主脚本%% 四旋翼无人机 PID 控制仿真从零手写版 % 状态量: [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r] % 控制目标: 期望高度10m期望姿态角0从初始扰动恢复悬停 clear; clc; close all; %% 1. 物理参数与仿真参数 param.m 1.2; % 质量 kg param.g 9.8; % 重力加速度 m/s^2 param.Ixx 0.03; % X轴转动惯量 kg*m^2 param.Iyy 0.03; % Y轴转动惯量 kg*m^2 param.Izz 0.05; % Z轴转动惯量 kg*m^2 param.l 0.25; % 机臂长度 m param.km 0.012; % 偏航力矩系数 param.u1_min 4; % 总推力下限 N param.u1_max 18; % 总推力上限 N param.u2_max 2; % 横滚/俯仰力矩限幅 N*m param.u4_max 0.5; % 偏航力矩限幅 N*m dt 0.01; % 仿真步长 s T 15; % 仿真时间 s N round(T / dt); % 总步数 %% 2. 期望值 desired.z 10; desired.phi 0; desired.theta 0; desired.psi 0; %% 3. PID 参数 % 高度环外环产生期望垂向加速度 ctrl.Kp_z 8; ctrl.Ki_z 1; ctrl.Kd_z 5; % 滚转角环外环产生期望角速度 p_des ctrl.Kp_phi 5; ctrl.Ki_phi 0.5; ctrl.Kd_phi 0.3; % 滚转角速度环内环产生力矩 u2 ctrl.Kp_p 15; ctrl.Ki_p 3; ctrl.Kd_p 0.5; % 俯仰角环与角速度环 ctrl.Kp_theta 5; ctrl.Ki_theta 0.5; ctrl.Kd_theta 0.3; ctrl.Kp_q 15; ctrl.Ki_q 3; ctrl.Kd_q 0.5; % 偏航角环与角速度环 ctrl.Kp_psi 4; ctrl.Ki_psi 0.5; ctrl.Kd_psi 0.2; ctrl.Kp_r 10; ctrl.Ki_r 2; ctrl.Kd_r 0.4; %% 4. 初始状态 state [0; 0; 8; 0; 0; 0; 0.3; -0.25; 0.1; 0; 0; 0]; %% 5. 存储数据 t_arr zeros(1, N 1); state_arr zeros(12, N 1); u_arr zeros(4, N 1); F_arr zeros(4, N 1); state_arr(:, 1) state; t_arr(1) 0; % PID 积分与微分变量初始化 err_z_i 0; err_z_prev 0; err_phi_i 0; err_phi_prev 0; err_p_i 0; err_p_prev 0; err_theta_i 0; err_theta_prev 0; err_q_i 0; err_q_prev 0; err_psi_i 0; err_psi_prev 0; err_r_i 0; err_r_prev 0; %% 6. 主仿真循环 for k 1:N t k * dt; x state(1); y state(2); z state(3); vx state(4); vy state(5); vz state(6); phi state(7); theta state(8); psi state(9); p state(10); q state(11); r state(12); % ------------------- 高度控制 ------------------- err_z desired.z - z; err_z_i err_z_i err_z * dt; err_z_i max(-2, min(2, err_z_i)); % 积分限幅 err_z_d (err_z - err_z_prev) / dt; az_cmd ctrl.Kp_z * err_z ctrl.Ki_z * err_z_i ctrl.Kd_z * err_z_d; u1 param.m * (param.g az_cmd); u1 max(param.u1_min, min(param.u1_max, u1)); err_z_prev err_z; % ------------------- 滚转控制phi - p - 力矩 ------------------- err_phi desired.phi - phi; err_phi_i err_phi_i err_phi * dt; err_phi_i max(-0.5, min(0.5, err_phi_i)); err_phi_d (err_phi - err_phi_prev) / dt; p_des ctrl.Kp_phi * err_phi ctrl.Ki_phi * err_phi_i ... ctrl.Kd_phi * err_phi_d; p_des max(-1, min(1, p_des)); % 期望角速度限幅 err_p p_des - p; err_p_i err_p_i err_p * dt; err_p_i max(-2, min(2, err_p_i)); err_p_d (err_p - err_p_prev) / dt; u2 ctrl.Kp_p * err_p ctrl.Ki_p * err_p_i ctrl.Kd_p * err_p_d; u2 max(-param.u2_max, min(param.u2_max, u2)); err_phi_prev err_phi; err_p_prev err_p; % ------------------- 俯仰控制theta - q - 力矩 ------------------- err_theta desired.theta - theta; err_theta_i err_theta_i err_theta * dt; err_theta_i max(-0.5, min(0.5, err_theta_i)); err_theta_d (err_theta - err_theta_prev) / dt; q_des ctrl.Kp_theta * err_theta ctrl.Ki_theta * err_theta_i ... ctrl.Kd_theta * err_theta_d; q_des max(-1, min(1, q_des)); err_q q_des - q; err_q_i err_q_i err_q * dt; err_q_i max(-2, min(2, err_q_i)); err_q_d (err_q - err_q_prev) / dt; u3 ctrl.Kp_q * err_q ctrl.Ki_q * err_q_i ctrl.Kd_q * err_q_d; u3 max(-param.u2_max, min(param.u2_max, u3)); err_theta_prev err_theta; err_q_prev err_q; % ------------------- 偏航控制psi - r - 力矩 ------------------- err_psi desired.psi - psi; err_psi_i err_psi_i err_psi * dt; err_psi_i max(-0.5, min(0.5, err_psi_i)); err_psi_d (err_psi - err_psi_prev) / dt; r_des ctrl.Kp_psi * err_psi ctrl.Ki_psi * err_psi_i ... ctrl.Kd_psi * err_psi_d; r_des max(-1, min(1, r_des)); err_r r_des - r; err_r_i err_r_i err_r * dt; err_r_i max(-2, min(2, err_r_i)); err_r_d (err_r - err_r_prev) / dt; u4 ctrl.Kp_r * err_r ctrl.Ki_r * err_r_i ctrl.Kd_r * err_r_d; u4 max(-param.u4_max, min(param.u4_max, u4)); err_psi_prev err_psi; err_r_prev err_r; % ------------------- 控制分配反解四个电机推力 ------------------- F1 u1/4 - u3/(4*param.l) - u4/(4*param.km); F2 u1/4 u2/(4*param.l) u4/(4*param.km); F3 u1/4 u3/(4*param.l) - u4/(4*param.km); F4 u1/4 - u2/(4*param.l) u4/(4*param.km); F [F1; F2; F3; F4]; F max(0.1, min(15, F)); % 单电机推力饱和 % ------------------- 状态更新欧拉法 ------------------- dstate quadRotorDynamics(state, [u1; u2; u3; u4], param); state state dt * dstate; % ------------------- 存储 ------------------- state_arr(:, k 1) state; u_arr(:, k 1) [u1; u2; u3; u4]; F_arr(:, k 1) F; t_arr(k 1) t; end %% 7. 绘图 figure(Name, 四旋翼PID仿真结果, Color, w); subplot(2, 2, 1); plot(t_arr, state_arr(3, :), b-, LineWidth, 1.5); hold on; plot(t_arr, desired.z * ones(size(t_arr)), r--, LineWidth, 1); xlabel(时间/s); ylabel(高度/m); title(高度跟踪); legend(实际高度, 期望高度, Location, best); grid on; subplot(2, 2, 2); plot(t_arr, state_arr(7, :) * 180/pi, r-, LineWidth, 1.2); hold on; plot(t_arr, state_arr(8, :) * 180/pi, g-, LineWidth, 1.2); plot(t_arr, state_arr(9, :) * 180/pi, b-, LineWidth, 1.2); xlabel(时间/s); ylabel(角度/deg); title(姿态角); legend(滚转 phi, 俯仰 theta, 偏航 psi, Location, best); grid on; subplot(2, 2, 3); plot(t_arr, state_arr(10, :) * 180/pi, r-, LineWidth, 1.2); hold on; plot(t_arr, state_arr(11, :) * 180/pi, g-, LineWidth, 1.2); plot(t_arr, state_arr(12, :) * 180/pi, b-, LineWidth, 1.2); xlabel(时间/s); ylabel(角速度/deg/s); title(机体角速度); legend(p, q, r, Location, best); grid on; subplot(2, 2, 4); plot(t_arr, F_arr(1, :), LineWidth, 1.2); hold on; plot(t_arr, F_arr(2, :), LineWidth, 1.2); plot(t_arr, F_arr(3, :), LineWidth, 1.2); plot(t_arr, F_arr(4, :), LineWidth, 1.2); xlabel(时间/s); ylabel(推力/N); title(四个电机推力); legend(F1, F2, F3, F4, Location, best); grid on;4.3 动力学子函数新建一个文件命名为quadRotorDynamics.m放在和主脚本同一个目录下。function dstate quadRotorDynamics(state, u, param) % 四旋翼动力学小角度近似下的12维状态导数 % u [u1总推力, u2滚转力矩, u3俯仰力矩, u4偏航力矩] x state(1); y state(2); z state(3); vx state(4); vy state(5); vz state(6); phi state(7); theta state(8); psi state(9); p state(10); q state(11); r state(12); u1 u(1); u2 u(2); u3 u(3); u4 u(4); m param.m; g param.g; Ixx param.Ixx; Iyy param.Iyy; Izz param.Izz; % 线性加速度小角度近似 ax u1/m * (sin(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)); ay u1/m * (-sin(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)); az -g u1/m * (cos(phi)*cos(theta)); % 姿态运动学与角加速度 phi_dot p; theta_dot q; psi_dot r; p_dot u2 / Ixx; q_dot u3 / Iyy; r_dot u4 / Izz; dstate [vx; vy; vz; ax; ay; az; phi_dot; theta_dot; psi_dot; p_dot; q_dot; r_dot]; end4.4 为什么不用ode45而是自己写积分循环这里有一个很多人会问的问题Matlab明明有强大的求解器为什么非要用最原始的欧拉法原因有两层。第一PID控制器是离散的它需要在每一个控制周期内读取状态、计算误差、累加积分项然后输出控制量。我用固定步长循环天然就是一个控制周期的结构。如果换成ode45整个状态更新时间步由求解器自动决定PID的积分项积分项就不知道在哪里更新——你只能把控制器放在一个事件函数里或者极不优雅地强行固定步长这完全违背了使用变步长求解器的初衷。第二真实飞控从来不用变步长求解器。飞控是一个实时的、固定周期控制系统主频就是控制频率通常250到1000赫兹。我的仿真步长取0.01秒对应100赫兹这个频率对本文的悬停场景完全够用。用欧拉法虽然数值精度不如高阶求解器但只要步长足够小、系统不发散结果就是可信的。工程仿真不是越精确越好而是满足分析需求即可。4.5 中文乱码问题代码和绘图里的中文标签在Matlab老版本里可能会显示为乱码。如果出现这种情况检查文件编码是否为UTF-8或者把绘图标签改成英文不影响运行。高版本Matlab里如果注释乱码一般是因为操作系统区域设置和文件编码不一致改成UTF-8编码保存即可解决。5. 调参实战初始值、发散原因、从曲线反推问题5.1 一组能用的初始参数与运行结果预期上面代码里给的那组PID参数直接复制运行就能得到一个稳定的结果。运行后你会看到什么高度曲线大约在3到4秒内从8米爬升到10米基本无超调或者最多轻微超调0.1米然后稳定在10米。姿态角曲线会更快收敛滚转角和俯仰角在1到2秒内回到0度附近中间有一次小幅回摆之后彻底稳定。偏航角收敛得稍微慢一点但也在4秒内归零。四个电机推力曲线在悬停稳定后都趋近于2.94牛这个值正好是重力平均到四个电机上的结果验证了模型的正确性。这个结果说明PID参数的量级和系统的物理参数是匹配的。如果换成其他的转动惯量或质量这套参数不一定还能稳定这就是为什么后面要学会自己调参。5.2 调参顺序先内环后外环先比例后微分调PID参数最忌讳一上来就六个环路一起调。正确顺序是从最内层开始一层层往外调。第一步调角速度环。把外环角度环的增益先设为0也就是让期望角速度恒为0然后给一个初始角速度扰动观察角速度能不能快速归零。这个环节只用调内环的Kp_p和Kd_p。Kp_p决定角速度被拉回来的力度Kd_p决定对突变响应的抑制作用。只要角速度能在0.5秒内稳定归零内环就算合格。第二步调角度环。把外环增益恢复期望角度设为0给一个初始角度扰动观察角度回正过程。这时候如果角度曲线出现明显的超调和回摆说明外环的Kp_phi偏大如果回正很慢说明Kp_phi偏小适当加Kd_phi可以加快收敛并抑制超调。第三步调高度环调法和角度环类似。高度环属于最外层响应最慢给它足够的反应时间。调参的口诀是先比例、后积分、再加微分。先把P调到临界振荡再稍微减小一点然后加D抑制超调最后用I消除稳态误差。这套方法在仿真里可以直接用因为你在电脑上能看到完整曲线。5.3 从仿真曲线反向定位问题很多时候系统发散或者效果差不是参数调不好而是不知道问题出在哪个环。这里给你几个最常见的曲线特征以及对应的原因仿真现象可能原因检查优先级角速度曲线高频剧烈振荡内环Kp_p过大或dt过大最高角速度缓慢爬坡、长时间不回零内环Kp_p过小最高角度回正很慢、无超调外环Kp_phi不足中角度大幅反复超调外环Kp_phi过大或内环响应太慢中高度始终有稳态误差高度环Ki_z不足中仿真初期推力冲到上限然后NaN初始姿态角过大或u1限幅设置过宽低有一个非常容易忽略的问题微分项对噪声敏感。如果你的状态量是从传感器模型里读出来的带测量噪声那么直接对误差做差分会把噪声放大得非常厉害。真实飞控里常用角速度反馈代替角度误差的微分量因为角速度本身就是角度误差微分的最直接体现。本文的仿真没有加传感器噪声所以用差分法没问题但如果你要做更真实的仿真建议把err_phi_d直接换成-p参与运算效果会干净很多。5.4 新手最容易踩的三个坑第一个坑是单位错误。期望角度和初始角度必须用弧度很多人赋值时顺手写了30系统直接以为你让它转30弧度结果当然发散。建议在代码注释里反复标注单位养成习惯。第二个坑是忘记加饱和。控制量必须限幅不仅是为了符合物理约束更是为了让仿真数值稳定。没有饱和的PID在初期大误差下可能产生一个天大的控制量欧拉积分一步就把状态推到不可思议的值之后怎么调都救不回来。所以我的代码里对u1、u2、u4以及期望角速度都做了限幅这是仿真稳定性的第一道防线。第三个坑是积分项不设限幅。高度从8米爬到10米需要好几秒时间期间高度误差一直存在如果积分项无限累积到达10米时积分项已经攒了很多额外出力系统会直接冲过去形成很大的超调。我的代码里对所有积分项都加了max和min限幅你可以试着把限幅值调大观察超调明显增大的现象这样对积分饱和的理解会非常深刻。6. 从悬停到航线这套仿真还能往哪个方向扩展6.1 加上水平位置控制从姿态控制到位置控制的关键一步这篇文章的仿真只做了高度和姿态闭环水平位置完全没有控。这也意味着初始状态里vx和vy即使为0x和y的位置误差也不会被自动修正。真正做航迹飞行的四旋翼必须在姿态环外面再套一个位置环逻辑和高度环几乎一样x方向误差经过位置环PID产生一个期望滚转角注意方向y方向误差经过位置环PID产生一个期望俯仰角把这两个期望角度作为姿态环的输入姿态环再继续往下走。加上这个位置环之后整套控制结构就变成三层串级位置环 → 姿态环 → 角速度环。层次多了但每个环的职责依旧清晰。你可以在这份代码的基础上给x和y也加上PID观察无人机能否自动回到原点这比Simulink里拖模块更能加深理解。6.2 模型升级全量姿态运动学与四元数小角度近似终究有适用范围。当无人机做大角度机动时比如快速翻滚、大俯仰爬升phi_dot p这种近似就不准了需要换成完整的欧拉角运动学方程甚至直接用四元数避免万向锁。此外真实四旋翼还存在陀螺力矩、电机动态响应滞后、旋翼入流效应等这些都可以逐步加进quadRotorDynamics.m里。每加一项你都会对四旋翼的真实行为多一分理解。从脚本迁移到Simulink也是个自然的扩展方向。Simulink的优势在于可视化搭积木调试方便你可以在Simulink里保留我这里串级PID的结构用PID Controller模块代替手写循环。但建议至少先把脚本版本吃透再迁移否则只是从一个黑盒换到另一个黑盒。6.3 下一步从仿真到实物飞控等你把Matlab仿真完全调明白可以再去看PX4或ArduPilot这类开源飞控的代码。你会发现它们飞行模式的底层逻辑其实就是这套串级PID只是增加了更完整的姿态表示、更复杂的滤波和状态估计、以及庞大的参数配置系统。有了这个仿真基础至少看到MPC_XY_P、MC_ROLL_P、MC_PITCHRATE_P这些参数时你能立刻知道它们在哪个环路、主要影响什么。这就是仿真学习最大的回报。我自己在带新人的时候一直坚持一个观点仿真能帮你学会控制逻辑和调参手感但替代不了对真实系统的敬畏。真实飞行中还有电池电压下降、振动、风扰、GPS漂移、电机一致性等问题仿真给不了的就只能靠多炸机、多复盘去积累了。希望这套代码能成为你从零开始的第一块垫脚石。