四旋翼无人机PD轨迹跟踪的MATLAB仿真与增益整定

简介：面向无人机控制领域研究者和工程师，这份基于MATLAB的四旋翼无人机PD轨迹跟踪算法资源包提供了完整的控制实现方案。包含比例微分控制、加速度控制及无人机动力学模型仿真等环节，可用于轨迹跟踪控制策略的设计与验证。压缩包共16个文件，以14个M脚本为主，另含1个Simulink模型及1个仿真程序文件，代码按控制器、被控对象、绘图与跟踪微分器等模块划分，结构清晰，便于二次开发与学习。已有61人学习下载。资源覆盖从PD控制器设计、加速度控制到模型仿真与性能曲线绘制的完整链路，适合用于本科毕业设计、课程实验或科研预研，能够帮助读者快速搭建四旋翼无人机控制仿真环境，理解PD控制在轨迹跟踪中的调节作用与实现细节。

1. 四旋翼PD轨迹跟踪：为什么“老算法”在MATLAB仿真里依然抗打

很多人翻到“基于MATLAB的四旋翼无人机PD轨迹跟踪算法”这类压缩包，第一反应是：PD？还轨迹跟踪？在MPC、LQR、ADRC满天飞的论文里，这种线性控制器早该进陈列室了。但真去MATLAB里跑一圈你会发现，PD在四旋翼的悬停附近依然有极强的工程价值：模型简单、整定直觉、采样周期低时性能完全够用。下面就把动力学建模、PD串级控制、圆形轨迹仿真和增益整定这条链一次性讲透，让你拿到类似“上传.zip”资源时，能自己改参数、跑通数据、看懂每一条曲线。

2. 四旋翼动力学建模：从欧拉角到MATLAB状态方程的参数表

2.1 12维状态量与坐标系选择

四旋翼轨迹跟踪很少直接对电机转速建模，而是把总推力T和三个方向的力矩τx、τy、τz作为虚拟控制输入。原因是电机动态比机体动态快一个数量级，PD控制器面对的是电调与螺旋桨组合后的响应，只要在悬停点附近能近似为线性推力系数，就能省去电机模型，把注意力集中在控制律本身。下载到的压缩包里如果只给你代码，大概率也采用了这种“控制优先”的建模思路，先别急着找电机参数，先把状态方程对齐。

状态量选为12维：x(1)~x(3)是惯性系下的位置，x(4)~x(6)是三个线速度，x(7)~x(9)是滚转、俯仰、偏航欧拉角，x(10)~x(12)是机体角速度p、q、r。这里使用Z轴向上的右手惯性坐标系，与无人机视觉库常用的ENU一致，方便把高度方向和重力方向分开。欧拉角顺序固定为“先Z偏航，再Y俯仰，再X滚转”，在普通轨迹跟踪中三个欧拉角都限制在±30°以内，这个顺序不会遇到万向节锁。如果要做连续翻滚，下面的姿态角微分方程会退化，那就要换成四元数状态。

2.2 运动学与动力学方程

位置部分直接写牛顿方程。总推力T沿机体Z轴方向，经过旋转矩阵投影到惯性系后，得到三个轴的加速度。为方便阅读，这里用文字公式表示：

x方向加速度 = (cosφ sinθ cosψ + sinφ sinψ) T/m
y方向加速度 = (cosφ sinθ sinψ − sinφ cosψ) T/m
z方向加速度 = −g + (cosφ cosθ) T/m

悬停时φ=θ=0，z轴加速度为−g+T/m，因此T=mg。PD控制器都是从这条线性关系上倒推控制量，所以这个符号约定必须统一，否则后面位置环求出的总推力会差一个符号。姿态角微分使用欧拉角运动学方程：

φ_dot = p + (q sinφ + r cosφ) tanθ
θ_dot = q cosφ − r sinφ
ψ_dot = (q sinφ + r cosφ) / cosθ

这四个方程和三个角速度微分一起组成12维一阶微分方程组，写入MATLAB状态方程函数。

2.3 quadrotor_dynamics函数的编写与参数表

下面是可直接运行的基础模型函数。状态输入x，控制输入u=[T,τφ,τθ,τψ]，输出状态导数xd。代码里明确注释了每一项的含义：

function xd = quadrotor_dynamics(x, u, p) % x: 12维状态向量 % u: [总推力T, 滚转力矩, 俯仰力矩, 偏航力矩] % p: 参数结构体 m = p.m; g = p.g; Ixx = p.Ixx; Iyy = p.Iyy; Izz = p.Izz; phi = x(7); theta = x(8); psi = x(9); p_ = x(10); q_ = x(11); r_ = x(12); T = u(1); t_phi = u(2); t_theta = u(3); t_psi = u(4); % 位置与线速度 dx = x(4:6); dv = zeros(3,1); dv(1) = (cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) * T/m; dv(2) = (cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) * T/m; dv(3) = -g + (cos(phi)*cos(theta)) * T/m; % 姿态角微分 dphi = p_ + q_*sin(phi)*tan(theta) + r_*cos(phi)*tan(theta); dtheta = q_*cos(phi) - r_*sin(phi); dpsi = q_*sin(phi)/cos(theta) + r_*cos(phi)/cos(theta); % 角速度微分 dp = (Iyy - Izz)*q_*r_/Ixx + t_phi/Ixx; dq = (Izz - Ixx)*p_*r_/Iyy + t_theta/Iyy; dr = (Ixx - Iyy)*p_*q_/Izz + t_psi/Izz; xd = [dx; dv; dphi; dtheta; dpsi; dp; dq; dr]; end

这段代码里故意没有加姿态角限幅，因为限幅放在控制器输出比放在模型里更合适。如果仿真中发现θ接近±90°，tan(theta)会爆炸，这种情况说明控制器已经失效，检查整定参数而不是加限幅掩盖问题。这里使用小角度近似，但当俯仰角超过30°后，位置与姿态之间的线性关系会发生明显偏差，PD轨迹跟踪的误差会快速上升。

下面的参数表能稳定跑通圆轨迹仿真。质量m对应常见的350级机架，惯量数值来自粗略估计，也就是让动力学仿真有合理的惯性耦合，不必追求高精度。

把这些参数保存在p结构体里后，模型函数每次调用都读取p的次数更少，MATLAB的JIT加速也更好。另一个容易忽视的细节是min_thrust不能设为0，因为电机有最低油门，实际飞控在油门低于某个阈值时无法保持转速，仿真中设成5N代表悬停油门附近的工作点。

2.4 从连续模型到离散仿真的步长选择

后续闭环仿真需要把状态导数积分成状态。使用欧拉积分时步长dt一般取0.001到0.01秒，对应100Hz到1000Hz控制频率。四旋翼的角速度环带宽通常在10Hz到30Hz，控制频率必须高于20倍带宽，否则离散化引入的相位延迟会让PD控制器很容易振荡。如果你想验证更高带宽下的性能，dt取0.001s，但仿真速度会慢一些；作为轨迹跟踪验证，0.01s已经能正确反映PD的控制效果。这里不推荐直接用ode45，后面第4章会说明原因。

3. PD轨迹跟踪控制器：位置环与姿态环的增益映射与MATLAB代码

3.1 串级PD结构：为什么外环位置、内环姿态

PD控制器在四旋翼上很少单独用一组PD完成跟踪，因为执行器输出的是总推力和力矩，而位置误差需要通过姿态角来改变推力方向。常见做法是串级控制：外环位置PD输出期望加速度，期望加速度通过姿态解耦映射成期望滚转、俯仰角和总推力；内环姿态PD将机体角度稳定到期望角度并输出力矩。内环带宽一般设计成外环带宽的3到5倍，这样外环看到的是近似一阶的姿态响应，不会因为内环滞后引起相位不足。

从频率角度看，位置环的PD增益决定了跟踪带宽，姿态环的PD增益决定了对姿态指令的跟随速度。如果内环响应太慢，外环误差信号会被内环滞后打开一个不正的相位，最终导致整个系统在某个频率上形成正反馈。所以实际调参顺序一定是先调内环，再调外环，这和很多论文里直接给一组“最优增益”的做法不同，那组增益换个机型就跑飞。另外这里不加入积分项，一是PD轨迹跟踪本身是稳定的零型系统，二是积分项会引入更大的相位滞后，对四旋翼这种开环不稳定对象很容易激发出低频振荡。

3.2 位置环PD：期望加速度到姿态角映射的MATLAB实现

位置环的输入是轨迹规划器给出的期望位置pos_des、期望速度vel_des、期望加速度acc_des，输出是总推力T和期望姿态角phi_des、theta_des。PD项直接加在加速度上：

function [T, phi_des, theta_des] = pos_controller(pos_des, vel_des, acc_des, pos, vel, yaw, p) e_pos = pos_des - pos; e_vel = vel_des - vel; % 期望加速度 = 前馈加速度 + 位置误差比例 + 速度误差微分 a_des = acc_des + p.kp_pos .* e_pos + p.kd_pos .* e_vel; % Z轴向上的总推力，悬停时T = m*g T = p.m * (p.g + a_des(3)); T = max(min(T, p.max_thrust), p.min_thrust); % 小角度映射：从期望水平加速度到期望欧拉角 phi_des = (a_des(1) * sin(yaw) - a_des(2) * cos(yaw)) / p.g; theta_des = (a_des(1) * cos(yaw) + a_des(2) * sin(yaw)) / p.g; end

代码里a_des(3)是期望高度加速度，悬停时为零。如果轨迹规划器给出的目标点有突变，前馈加速度为零而位置误差很大，PD项会先给出很大的加速度期望，映射出很大的phi_des和theta_des，这时必须对phi_des和theta_des限幅在±30度以内，否则就会触发第2.3节说的欧拉角奇异问题。常见做法是：

phi_des = max(min(phi_des, deg2rad(30)), -deg2rad(30)); theta_des = max(min(theta_des, deg2rad(30)), -deg2rad(30));

有人问为什么不用atan2映射精确角度。在PD轨迹跟踪里小角度映射足够，而且线性映射让增益整定有直观意义，把p.g换成重力加速度就是比例系数的一部分，角度饱和后相当于位置误差限幅，这是PD的饱和特性，不是缺陷。如果要做大角度机动，这时候位置环应该输出期望四元数而不是欧拉角，PD增益也要重新设计，已经超出这套代码的适用边界。

3.3 姿态环PD：角度外环与角速度内环的写法

姿态环设计为两层：角度环输出期望角速度，角速度环输出力矩。角度环只用P控制，因为几何本质上是姿态跟踪，比例项就能给出与误差成正比的角速度指令；角速度环用PD抑制测量噪声和外部扰动。

function tau = att_controller(phi_des, theta_des, psi_des, x, p) phi = x(7); theta = x(8); psi = x(9); p_ = x(10); q_ = x(11); r_ = x(12); e_phi = phi_des - phi; e_theta = theta_des - theta; e_psi = wrapToPi(psi_des - psi); % 角度外环：P控制器得到期望角速度 w_des(1) = p.kp_att(1) * e_phi; w_des(2) = p.kp_att(2) * e_theta; w_des(3) = p.kp_att(3) * e_psi; % 角速度内环：PD控制器得到力矩 tau(1) = p.kp_rate(1) * (w_des(1) - p_) + p.kd_rate(1) * (0 - p_); tau(2) = p.kp_rate(2) * (w_des(2) - q_) + p.kd_rate(2) * (0 - q_); tau(3) = p.kp_rate(3) * (w_des(3) - r_) + p.kd_rate(3) * (0 - r_); % 力矩限幅，避免仿真发散 tau = max(min(tau, p.max_tau), -p.max_tau); end

这段代码里(0 - p_)是对角速度的阻尼项，增加kd_rate会大幅提高系统阻尼，让姿态响应更平滑，但也会降低抑制高频扰动能力。偏航误差用wrapToPi处理，因为偏航是周期量，直接用psi_des - psi会产生±2π的跳变，导致角速度指令错误。滚转和俯仰角在正常飞行中不会跨±π，不需要wrap操作。

3.4 PD增益初值表与频带分离经验值

下面给出一组能跑通圆轨迹仿真的增益初始值。位置环分别对x、y、z取不同比例，z轴因为重力补偿单独大一些；姿态环的kp_att决定姿态角速度响应的快速性，kp_rate和kd_rate决定角速度环阻尼。

这个表的顺序就是整定顺序：先固定姿态环（kp_att、kp_rate、kd_rate）让角度阶跃响应快速收敛，再调位置环。如果位置环调好后发现轨迹有高频抖动，多半是内环阻尼不足或测量噪声通过kd_pos放大，不是位置增益太高。PD两个环的参数相互影响，改一个环的参数必须重新看另一个环的响应曲线。

4. MATLAB轨迹跟踪仿真：圆形轨迹、主循环与误差统计

4.1 圆形轨迹的生成与加速度前馈

要验证PD轨迹跟踪，最直观的参考轨迹是匀速圆周运动。圆轨迹同时包含x/y通道的耦合和恒定向心加速度，比直线阶跃更能暴露增益不足。常见的验证轨迹是半径2m、高度1m、周期10s的圆，按时间参数方程生成位置、速度、加速度：

dt = 0.01; t = 0:dt:10; R = 2.0; w = 2*pi/10; x_des = R * cos(w*t); y_des = R * sin(w*t); z_des = ones(size(t)) * 1.0; vx_des = -R*w*sin(w*t); vy_des = R*w*cos(w*t); vz_des = zeros(size(t)); ax_des = -R*w^2*cos(w*t); ay_des = -R*w^2*sin(w*t); az_des = zeros(size(t));

速度是位置的一阶导数，加速度是二阶导数。ax_des和ay_des对应的向心加速度大小是R*w²≈0.395m/s²，这个值远小于重力加速度g，所以在位置环中它作为前馈项不是主项，位置误差PD才是主导。但加上前馈可以减小圆轨迹的稳态误差，尤其是半径大、转速快的场景。没有前馈，PD位置环会在曲率方向上产生常值滞后，这是纯PD零型系统的固有特性，加前馈能显著降低这个滞后。圆形轨迹参数如下，方便后续调整测试：

4.2 闭环仿真主循环：欧拉积分与控制器调用

完整仿真不需要Simulink，纯脚本在MATLAB里就能跑通。把第2、3章的函数放到同一路径下，主循环如下：

p = load_params(); % 加载第2.3节的参数表和3.4节的增益 x = zeros(12, numel(t)); x(:,1) = [R; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 0]; for i = 1:numel(t)-1 % 当前期望信号 pos_des = [x_des(i); y_des(i); z_des(i)]; vel_des = [vx_des(i); vy_des(i); vz_des(i)]; acc_des = [ax_des(i); ay_des(i); az_des(i)]; % 位置环PD [T, phi_des, theta_des] = pos_controller(... pos_des, vel_des, acc_des, ... x(1:3,i), x(4:6,i), 0, p); % 姿态环PD psi_des = 0; tau = att_controller(phi_des, theta_des, psi_des, x(:,i), p); % 组合控制输入并运行模型 u = [T, tau]; xd = quadrotor_dynamics(x(:,i), u, p); x(:,i+1) = x(:,i) + dt * xd; end

欧拉积分在控制频率100Hz、仿真时间10s时稳定性足够，误差与ode45相比不超过几个百分点，但代码直接可调增益、可加噪声，更适合教学。numel(t)在dt=0.01时是1001个时间点，循环跑1000步，MATLAB现代版本几秒钟就能跑完。如果换成dt=0.001，循环变10000步，依然很快。

有一个常见错误是忘记把pos_controller输出的phi_des限幅，导致大偏差时姿态角瞬间饱和，接着tan(theta)在状态方程里爆炸，仿真在几十步内发散。如果你在跑上面代码时发现状态变成NaN，八成是这个原因。另一点是att_controller里的tau限幅值p.max_tau不要设得太大，2N·m已经足够让350级机架产生很强的角加速度，太大会让内环看起来很快，实际上电机早就饱和。

4.3 误差统计与轨迹对比

仿真结束后需要量化跟踪性能，不能只看动画。计算每个时刻的位置误差欧几里得范数，并输出均方根误差：

e_pos = sqrt((x(1,:)-x_des).^2 + ... (x(2,:)-y_des).^2 + ... (x(3,:)-z_des).^2); rms_err = sqrt(mean(e_pos.^2)); fprintf('RMS position error = %.3f m\n', rms_err); figure; subplot(2,1,1); plot(t, x(1,:), 'b-', 'LineWidth', 1.5); hold on; plot(t, x_des, 'r--'); ylabel('x (m)'); legend('实际','期望'); subplot(2,1,2); plot(t, x(2,:), 'b-', 'LineWidth', 1.5); hold on; plot(t, y_des, 'r--'); ylabel('y (m)'); legend('实际','期望');

用默认增益，RMS误差一般在0.05~0.15m之间。如果RMS超过0.3m，先看第一个周期的启动阶段有没有大的超调。初始位置设在圆的起点且速度为零，而期望速度在初始时刻为−Rwsin(0)=0，所以起步没有速度跳变；但期望加速度为−Rw²cos(0)=−0.395，即初始有x方向负的向心加速度。如果PD增益太软，第一个周期会产生明显的向内偏移，这个瞬态误差会在后续周期中逐步被PD拉回。

4.4 把圆轨迹换成8字轨迹的两种方式

圆轨迹只考察常值向心加速度，8字轨迹则考察时变的曲率和符号反转，能更严格地测试PD跟踪。常见做法是利萨如曲线，将x轴频率设为y轴的两倍，产生8字形状：

x_des = R * cos(w*t); y_des = R * sin(2*w*t)/2; z_des = ones(size(t)) * 1.0;

对应速度和加速度用gradient函数数值差分即可，但数值差分在首尾会有误差，建议让时间向量长度足够大。8字轨迹中y方向频率变成原来的两倍，向心加速度不再是常数，PD增益不足时会在两个顶端出现明显滞后，这是对位置环带宽的直接压力测试。如果你在资源包里看到8字轨迹的代码，多半是为了补足圆轨迹覆盖不到的变曲率场景。

5. PD增益整定的3个可复现技巧与仿真验证

5.1 让偏航参考轨迹参与整定

前面主循环里偏航始终为0，这隐藏了偏航通道的问题。实际轨迹跟踪中，四旋翼期望偏航角往往与速度方向绑定，比如让机头始终指向飞行方向。把偏航期望设为速度方向角psi_des = atan2(vy_des, vx_des)，再观察yaw误差曲线，你会发现带wrapToPi的姿态环能跟踪跳变，但偏航通道的kp_att如果太小，机头会明显落后于速度方向。一个简单验证方式是在主循环后增加：

psi_des = atan2(vy_des, vx_des); psi_err = wrapToPi(psi_des - x(9,:)); rms_yaw = sqrt(mean(psi_err.^2));

如果RMS偏航误差超过5度，说明偏航增益不够，单独增加kp_att(3)和kp_rate(3)，不要动x/y通道增益。偏航通道惯性Izz通常比Ixx大，所以偏航kp_rate初值比滚转低，参数表中为2对应这一点。

5.2 用脚本扫掠kp_pos定位“下临界增益”

快速整定PD位置环时，不用拉普拉斯图，直接在脚本里扫kp_pos，从2开始以0.5步长递增到12，每一组重跑仿真并记录RMS位置误差。然后画出误差曲线，最低点往往在出现持续振荡之前的位置。

kp_list = 2:0.5:12; rms_hist = zeros(size(kp_list)); for i = 1:numel(kp_list) p.kp_pos = [kp_list(i), kp_list(i), kp_list(i)*1.5]; % 调用封装好的仿真函数run_sim(p) [~, rms_hist(i)] = run_sim(p); end [~, idx] = min(rms_hist); fprintf('最佳kp_pos = %.1f, RMSE = %.3f\n', kp_list(idx), rms_hist(idx));

这里的run_sim可以把第4章主循环封装成函数。注意每次扫掠前重新初始化状态矩阵，否则上一次的末端状态会污染下一轮。该方法不会给出绝对最优，因为离散步长和姿态环增益固定，PD增益不是完全独立；但它能给你一个“下临界增益”，低于它响应慢，高于它振荡，整定目标通常取略低于临界值的20%，留出模型不确定性裕量。

5.3 加传感器噪声与滤波器验证鲁棒性

PD的微分项决定了它对噪声的放大程度。真实传感器里GPS位置噪声标准差通常0.1~0.3m，速度由位置差分时噪声更明显。在仿真里给状态测量值加噪声：

meas_noise = 0.02 * randn(size(x)); x_meas = x + meas_noise; % 控制器内部用x_meas代替x

你会发现加了噪声后RMS误差明显上升，且kd_pos越大上升越明显。这时需要给速度信号加一阶低通滤波，而不是单纯减小kd_pos。一阶低通滤波器在MATLAB里用filter函数或者状态变量实现，截止频率取30Hz左右，不增加Kd也能压制噪声。这个验证过程能提前暴露PD控制器在机载处理器上的真实表现，而不是在干净仿真里刷出漂亮曲线。

本文还有配套的精品资源，点击获取