1. 先说结论为什么四旋翼入门都绕不开PD控制器最近在研究基于PD控制器的四旋翼无人机控制顺便把整套Matlab仿真代码重新梳理了一遍。说实话四旋翼这个题材在控制类项目里快被写烂了但每次带新人跑完一遍还是能发现不少问题——有人把控制器搭在天花板上姿态角就是压不下去有人参数一调大就发散还有人Simulink模型跑得飞起一换M脚本就不知道怎么写微分方程。这篇内容就把这条线完整串一遍从四旋翼的动力学模型出发到PD控制器的设计原理再到Matlab代码怎么落地最后聊聊我从仿真里踩出来的坑。适合正在做课程设计、毕业设计或者刚接触无人机控制、想搞明白PD到底在控制什么的读者。先说一个最核心的结论PD控制器解决的是让无人机从一个姿态到另一个姿态的问题它不需要知道模型有多精确只需要把误差和误差变化率喂给控制器输出对应的力矩指令即可。四旋翼这样一个强耦合、非线性、欠驱动的系统之所以大家习惯先用PD做入门就是因为它把控制律设计和被控对象建模解耦了——你不需要先推导出一套精确的逆动力学模型也能把飞机稳定在空中。1.1 飞控系统里PD控制器到底干了什么四旋翼的控制链路通常分成两层内环是姿态控制外环是位置控制。内环PD控制器接收期望姿态角比如横滚角30度对比当前姿态角计算出需要的横滚力矩外环PD控制器接收期望位置对比当前位置计算出需要的期望姿态角再送给内环。换句话讲PD控制器在这套系统里不是一个孤立的模块而是串联在整个导航-制导-控制链条的末端它只做一件事调节误差。比例项P决定当前误差有多大我要给多大力度微分项D决定误差变化有多快我要不要踩刹车。这个过程用人话说就是你开车看到前面偏了P就是你打方向盘的幅度D就是你感觉到车在往回摆的时候提前回正方向盘的预判。所以PD控制器不需要知道飞机重多少、电机多大、风有多大它只需要持续盯着误差。这也解释了为什么四旋翼的PD控制研究这么多年还是经典——简单、鲁棒、参数意义清晰。1.2 为什么是PD而不是PID也不是更复杂的LQR很多初学者会问既然PD只是PID去掉积分项为什么不直接上PID原因很简单四旋翼姿态控制在悬停和低速飞行工况下重力、气动阻力等扰动相对固定比例项已经能消除大部分稳态误差积分项的引入会带来积分饱和尤其在电机输出有限幅的情况下容易造成姿态的超调和低频振荡。我在实际仿真中做过对比同样一组PD参数加上积分项后在持续风扰下稳态误差确实可以压到零但当期望姿态角突然变化时积分项累积的修正量会滞后释放导致明显的 overshoot。对四旋翼这种本身就好动的对象PID调参难度比PD高一个量级所以第一版研究普遍先用PD把框架跑通后续再考虑抗扰补偿。至于LQR、滑模、自适应这类更高级的控制器问题不在控制效果而在它们对模型精度的要求太高。四旋翼的转动惯量、电机时间常数、螺旋桨力效系数这类参数在仿真里可以随便给真机上一测一个不准。PD的好处恰恰是模型误差的容忍度很高把参数整定好就能得到一个可接受的飞行品质。2. 四旋翼动力学建模仿真代码的地基PD控制器本身不依赖精确模型但仿真必须要有模型——控制器是在模型这个假想世界里被验证的。所以写Matlab代码的第一步不是写控制器而是把四旋翼的数学模型搭对。2.1 坐标系约定与状态向量定义四旋翼仿真里有两套坐标系地球坐标系惯性系和机体坐标系。惯性系用于描述位置和速度机体系用于描述姿态角速度和电机推力。仿真的状态向量通常取12维x [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]其中前三个是位置接着三个是线速度然后是横滚角、俯仰角、偏航角三个欧拉角最后是三个机体角速度。这个顺序在Matlab代码里必须前后统一因为控制器输出的期望力矩要作用到角速度微分方程上状态向量的索引一旦错位整个仿真就会呈现一种说不清哪里不对但就是飞不稳的状态。欧拉角的旋转顺序也有讲究。我习惯用Z-Y-X偏航-俯仰-横滚的内旋约定这也是飞控领域最常见的表述方式。在代码里由欧拉角到机体角速度的转换矩阵是p phi_dot - psi_dot * sin(theta) q theta_dot * cos(phi) psi_dot * sin(phi) * cos(theta) r theta_dot * (-sin(phi)) psi_dot * cos(phi) * cos(theta)注意这个矩阵在俯仰角接近90度时会出现奇异仿真中通常会限制俯仰角在正负80度以内这既符合真实飞行器的物理约束也避免了数值问题。2.2 刚体运动方程的简化推导四旋翼动力学方程可以分成平动和转动两部分。平动部分基于牛顿第二定律在惯性系下写m * v_dot R * T_total [0, 0, -m*g]其中R是机体坐标系到惯性系的旋转矩阵T_total是四个电机推力在机体轴上的合力。转动部分基于欧拉方程在机体坐标系下写I * omega_dot M - cross(omega, I * omega)这里的M就是PD控制器输出的横滚、俯仰、偏航力矩omega是机体角速度。如果把转动惯量I近似看成对角阵展开后就是三个独立的角加速度方程p_dot (M_x (I_yy - I_zz) * q * r) / I_xx q_dot (M_y (I_zz - I_xx) * p * r) / I_yy r_dot (M_z (I_xx - I_yy) * p * q) / I_zz很多初学者在这里会忽略交叉项直接用M/I在小角度和低速飞行时问题不大但一旦做大机动仿真角速度耦合会造成姿态振荡发散。我的建议是代码里保留完整的交叉项反正只是几行乘法的成本。2.3 电机推力与扭矩模型电机模型在Matlab仿真里最常用的是一阶惯性环节加力效系数thrust_i K * omega_motor_i^2 omega_motor_dot (omega_cmd - omega_motor) / tau_motor这里的tau_motor是电机时间常数一般取0.02到0.05秒之间。力效系数K取决于螺旋桨尺寸、空气密度等因素仿真里可以先给一个典型值比如1.5e-6 N/(rad/s)^2。四个电机的转速指令由控制器输出的总推力和三个力矩通过混控矩阵得到omega_cmd_1 hover_cmd u_phi u_theta u_psi omega_cmd_2 hover_cmd - u_phi u_theta - u_psi omega_cmd_3 hover_cmd - u_phi - u_theta u_psi omega_cmd_4 hover_cmd u_phi - u_theta - u_psi混控矩阵里的正负号不是随便拍的它取决于电机旋转方向和机架布局。你可以在代码里用注释标注好每个电机的位置否则调试的时候很容易出现横滚控制越控越偏的诡异现象。这个模型的精度对仿真结果影响很大但对PD参数整定的趋势性结论影响不大——也就是说就算你的电机参数和真机差30%PD参数的相对变化趋势还是可以参考的这也再次体现了PD控制器对模型误差的宽容度。3. Matlab仿真框架的搭建思路Simulink做四旋翼仿真其实更直观但我个人更推荐用M脚本搭最小框架原因是代码可读性强、调试方便、迁移到C代码时思路清晰。下面是我常用的仿真结构。3.1 最小可用结构主脚本被控对象函数控制器函数整个仿真由三个部分组成主脚本负责设置参数、循环递推、绘图被控对象函数接收当前状态和控制量计算状态导数控制器函数接收期望值和当前状态输出控制量。主脚本的核心循环长这样dt 0.001; % 仿真步长 1ms T_total 10; % 仿真时长 t 0:dt:T_total; state [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % 初始状态 % 期望指令 desired [5; 5; -3; 0]; % x, y, z, yaw for i 1:length(t)-1 % 控制量计算 [u1, u2, u3, u4] pd_controller(state, desired, param); % 状态更新使用四阶Runge-Kutta积分 k1 quadrotor_dynamics(state, u, param); k2 quadrotor_dynamics(state 0.5*dt*k1, u, param); k3 quadrotor_dynamics(state 0.5*dt*k2, u, param); k4 quadrotor_dynamics(state dt*k3, u, param); state state dt/6 * (k1 2*k2 2*k3 k4); end这里我用的是四阶Runge-Kutta积分而非欧拉法因为四旋翼动力学本身是刚性的欧拉法在步长稍大时就会出现数值能量增长飞机莫名其妙越飞越高。1ms步长加RK4在普通笔记本上跑10秒仿真也就几秒钟完全够用。3.2 控制器代码的核心实现PD控制器内部按外环位置、内环姿态两层实现。位置环先计算期望加速度% 位置环PD thrust_cmd m * (g kp_z * (zd - z) kd_z * (vzd - vz)) / (cos(phi)*cos(theta)); phi_des kp_x * (xd - x) kd_x * (vxd - vx); theta_des -kp_y * (yd - y) - kd_y * (vyd - vy);注意这里的符号约定横滚角正方向对应y轴加速度为正俯仰角正方向对应x轴加速度为正具体正负要跟动力学模型里的旋转矩阵匹配。我第一次写的时候把theta_des的负号漏了结果飞机往期望位置的反方向飞花了一整天才定位到问题。姿态环再根据期望姿态角计算三个力矩phi_err phi_des - state(7); theta_err theta_des - state(8); psi_err yaw_des - state(9); d_phi -state(10); d_theta -state(11); d_psi -state(12); u_phi kp_phi * phi_err kd_phi * d_phi; u_theta kp_theta * theta_err kd_theta * d_theta; u_psi kp_psi * psi_err kd_psi * d_psi;微分项直接用角速度反馈而不是用误差的差分这是工程上标准的做法。因为真实传感器直接输出角速度用角度误差做差分在高频下噪声会被放大仿真里虽然没噪声但养成好习惯对后续迁移到真机有好处。3.3 仿真参数配置与采样步长的选择仿真步长是第一个要确定的参数。四旋翼的角速度动态范围通常在每秒几弧度到十几弧度电机时间常数在0.02到0.05秒控制器执行频率在真机上一般是250Hz到1kHz。仿真步长取1ms即1kHz可以覆盖这些动态特性再小就纯粹是浪费时间了。参数结构体建议用Matlab的struct集中管理别散落在各段代码里param.m 1.2; % 飞行器总质量 kg param.g 9.8; param.Ixx 0.015; % 转动惯量 kg*m^2 param.Iyy 0.015; param.Izz 0.028; param.K 1.8e-6; % 电机力效系数 param.l 0.25; % 机臂长度 m param.tau_motor 0.03; % 电机时间常数 s这样改参数只需要动struct不用在代码里手工查找替换。4. PD参数整定从拍脑袋到有章法PD参数整定是这门手艺里最像玄学的部分但实际上是有套路可循的。四旋翼的控制器参数整定顺序只有一个正确方向先内环后外环。4.1 分层整定内环姿态优先外环位置跟进内环姿态环的带宽必须比外环位置环大3到5倍。用人话说就是姿态的响应速度要比位置的响应速度快这样外环给出的姿态指令才能被内环及时实现。如果内环响应慢外环看到位置误差加大就会输出更激进的角度指令内环又跟不上系统就变成两个滞后环节串联非常容易振荡。我建议的整定流程是先固定悬停油门把四个电机转速设为基础值只整定横滚和俯仰的PD参数偏航参数先给保守值手动给横滚一个期望角度阶跃观察响应曲线逐步增大kp_phi直到出现等幅振荡记录临界增益取临界增益的60%作为kp_phi再增大kd_phi直到超调明显减小这个流程每步都可以在Matlab里用plot直接看状态曲线比真机调试安全得多。4.2 衰减振荡法在四旋翼上的具体操作衰减振荡法Ziegler-Nichols的一种变体是我在四旋翼仿真里最常用的参数整定方法。具体操作是先让kd_phi 0只保留kp_phi。从一个小值比如0.5开始每次增加0.2给一个10度的横滚阶跃指令观察响应。刚开始系统会缓慢趋近目标随着kp增大响应变快超调出现。继续增大kp直到系统产生持续等幅振荡此时记录振荡周期T_crit和临界增益K_crit。按经验公式取kp 0.6 * K_critkd kp * T_crit / 8然后微调kd。注意四旋翼姿态环和典型二阶系统的区别在于姿态环有一个由重力力矩和转速耦合构成的等效阻尼所以临界增益附近的行为不完全和理论一致经验公式只能作为起点还需要手动优化一到两轮。我在一个典型仿真环境里整出的参数大概是这样的机型参数如上面struct所示环路kpkd响应特征横滚/俯仰姿态环5.50.3510度阶跃约1.2s收敛超调小于5度偏航环3.00.2530度阶跃约2s收敛无明显振荡位置环0.80.253m位置误差约4s收敛这组参数放在同型号的仿真里基本都能飞稳换了机型参数后内环参数需要按转动惯量的比例缩放外环参数缩放幅度不大。4.3 我常用的整定经验表参数整定过程中如果某个现象出现基本可以直接定位到问题现象原因调整方式等幅振荡频率很高kp过大降低kp到60%响应慢且误差长期存在kp过小增大kp阶跃响应超调大但振荡平缓kd不足增大kd高频抖动甚至发散kd过大或步长不足先降kd再降kp检查仿真步长位置环到位后来回穿梭位置环kd过大降外环kd还有一个很多人会忽略的点PD参数必须和采样步长配合。当你把仿真步长从1ms改成5ms同样的参数就可能从稳定变成发散因为数字控制器的离散化引入了额外相位滞后。所以说参数整定前先固定好仿真步长不要中途换。5. 仿真里最容易翻车的地方与排查方法这一节把我实际调试中见过的高频问题列出来全都对应可复现的现象方便你对照排查。5.1 数值发散步长、模型刚性与初始状态最典型的翻车现场是仿真跑到第几秒状态变量突然变成NaN或者infplot图里曲线直接冲出屏幕。原因通常有三个。第一是仿真步长太大四旋翼角速度环在参数较大时已经进入刚性区间欧拉法无法稳定积分解决办法是改用RK4或减小步长。第二是电机转速出现负值混控矩阵在某个极端姿态下计算出负的转速指令而推力方程里转速是平方项负转速平方和正转速平方效果相同但状态却在错误的方向上累积解决办法是对电机转速指令做上下限饱和禁用负值。第三是初始状态给得太离谱比如初始俯仰角90度旋转矩阵奇异解决方法是把初始姿态和初始角速度都设为0让控制器先自平衡。我在代码里会加一段状态合法性检查每100步检查一次状态是否包含NaN一旦检测到立即终止仿真并输出断点信息省得跑完10秒才发现数据无效。5.2 姿态误差的符号与旋转顺序这个坑非常隐蔽。前面提到状态向量里姿态角顺序是phi、theta、psi但在控制器的期望姿态角计算里位置环输出的phi_des和theta_des的符号必须和动力学方程的旋转矩阵严格一致。我在实际中见过两种错误第一种是把theta_des的负号丢了导致飞机对y方向的位置误差做出的俯仰响应是反的位置越偏越远。第二种是欧拉角转旋转矩阵时用了不同的旋转顺序导致仿真里姿态角的含义和控制器理解的不一样表现为某个轴的控制量串到了另一个轴。排查方法很简单给一个纯x方向的位置阶跃(比如期望x5米其他不变)画出phi和theta曲线正常情况theta应该有响应、phi保持接近0。如果phi也跟着大幅变化八成是旋转矩阵或混控矩阵的轴序写错了。5.3 期望轨迹跳变引发的超调仿真中如果直接给位置期望一个阶跃比如从(0,0,0)跳到(5,5,-3)PD控制器会产生很大的初始误差导致期望姿态角度瞬间饱和到几十度飞机猛烈俯仰横滚。这不是控制器坏了而是指令规划的问题。实际飞行里位置指令来自上层路径规划不会这样跳变。仿真里想让结果好看一点我一般会给参考轨迹加一个斜坡或S曲线过渡把期望值平滑过去desired desired_prev (desired_target - desired_prev) * min(1, t / 2);这样PD控制器面临的就是一个有准备的战场响应曲线更接近真实飞行器参数判断也更准确。6. 从Matlab仿真走向实际飞行器的落差感如果你只是做课题研究、写论文到上一节就足够了。但如果你想把这套代码搬到真实飞行器上有几个落差必须心里有数。6.1 仿真里被白嫖的环节仿真里最容易忽视的是传感器模型。真机上的陀螺仪有偏置和噪声加速度计有振动耦合气压计有漂移GPS有跳变。Matlab仿真里状态是直接可得的理想值PD控制器看到的误差永远是干净的。到了真机上帮你算角速度微分项的传感器信号本身就带噪声同样的kd值在仿真里表现良好在真机上可能高频颤抖。还有一个白嫖的环节是执行器饱和。仿真的混控矩阵输出如果超过电机最大转速你可能没做饱和处理控制器继续积分姿态发散。我见过不少人在仿真里把期望力矩调到远超实际执行能力结果纸面曲线完美真机一飞就炸。6.2 把Matlab代码迁移到嵌入式平台要改什么迁移到嵌入式平台常见的是STM32这类MCU时第一个要改的是控制器频率。Matlab仿真可以跑1kHz真机上很多飞控内环跑了500Hz就不错了外环甚至只有100Hz。控制频率降下来之后你需要重新整定PD参数一般比例项要适当降低因为数字控制的相位滞后更大。第二个要改的是把连续积分改成离散积分把四阶RK4换成更简单的梯形积分或直接一阶前向欧拉因为嵌入式平台的算力有限。这个改动对仿真结论没有影响但会让你重新认识仿真和现实之间的那条鸿沟到底有多宽。我个人在实际中的体会是PD控制器研究最宝贵的产出不是那组终于调好的参数而是调试过程中建立的误差-响应-修正直觉。把这套直觉沉淀下来后续接扩展卡尔曼滤波、串级PID、甚至LQR都会顺畅很多。最后再分享一个小技巧——在Matlab里把PD控制器输出、期望姿态角、实际姿态角三个信号画在同一张图上盯着它们之间的跟随关系调参数效率比只看位置误差高得多。