四旋翼无人机PD控制器设计原理与Matlab仿真实践指南

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倍。用人话说就是姿态的响应速度要比位置的响应速度快，这样外环给出的姿态指令才能被内环"及时"实现。如果内环响应慢，外环看到位置误差加大就会输出更激进的角度指令，内环又跟不上，系统就变成两个滞后环节串联，非常容易振荡。

我建议的整定流程是：

1. 先固定悬停油门，把四个电机转速设为基础值
2. 只整定横滚和俯仰的PD参数，偏航参数先给保守值
3. 手动给横滚一个期望角度阶跃，观察响应曲线
4. 逐步增大kp_phi，直到出现等幅振荡，记录临界增益
5. 取临界增益的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_crit
• kd = kp * T_crit / 8

然后微调kd。注意四旋翼姿态环和典型二阶系统的区别在于：姿态环有一个由重力力矩和转速耦合构成的等效阻尼，所以临界增益附近的行为不完全和理论一致，经验公式只能作为起点，还需要手动优化一到两轮。

我在一个典型仿真环境里整出的参数大概是这样的（机型参数如上面struct所示）：

这组参数放在同型号的仿真里基本都能飞稳，换了机型参数后，内环参数需要按转动惯量的比例缩放，外环参数缩放幅度不大。

4.3 我常用的整定经验表

参数整定过程中，如果某个现象出现，基本可以直接定位到问题：

还有一个很多人会忽略的点：PD参数必须和采样步长配合。当你把仿真步长从1ms改成5ms，同样的参数就可能从稳定变成发散，因为数字控制器的离散化引入了额外相位滞后。所以说，参数整定前先固定好仿真步长，不要中途换。

5. 仿真里最容易翻车的地方与排查方法

这一节把我实际调试中见过的高频问题列出来，全都对应可复现的现象，方便你对照排查。

5.1 数值发散：步长、模型刚性与初始状态

最典型的翻车现场是：仿真跑到第几秒，状态变量突然变成NaN或者inf，plot图里曲线直接冲出屏幕。

原因通常有三个。第一是仿真步长太大，四旋翼角速度环在参数较大时已经进入刚性区间，欧拉法无法稳定积分，解决办法是改用RK4或减小步长。第二是电机转速出现负值，混控矩阵在某个极端姿态下计算出负的转速指令，而推力方程里转速是平方项，负转速平方和正转速平方效果相同，但状态却在错误的方向上累积，解决办法是对电机转速指令做上下限饱和，禁用负值。第三是初始状态给得太离谱，比如初始俯仰角90度，旋转矩阵奇异，解决方法是把初始姿态和初始角速度都设为0，让控制器先自平衡。

我在代码里会加一段状态合法性检查，每100步检查一次状态是否包含NaN，一旦检测到立即终止仿真并输出断点信息，省得跑完10秒才发现数据无效。

5.2 姿态误差的符号与旋转顺序

这个坑非常隐蔽。前面提到状态向量里姿态角顺序是phi、theta、psi，但在控制器的期望姿态角计算里，位置环输出的phi_des和theta_des的符号必须和动力学方程的旋转矩阵严格一致。我在实际中见过两种错误：

第一种是把theta_des的负号丢了，导致飞机对y方向的位置误差做出的俯仰响应是反的，位置越偏越远。第二种是欧拉角转旋转矩阵时用了不同的旋转顺序，导致仿真里姿态角的含义和控制器理解的不一样，表现为某个轴的控制量串到了另一个轴。

排查方法很简单：给一个纯x方向的位置阶跃(比如期望x=5米，其他不变)，画出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控制器输出、期望姿态角、实际姿态角三个信号画在同一张图上，盯着它们之间的跟随关系调参数，效率比只看位置误差高得多。