简介面向无人机控制领域研究者和工程师这份基于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/my方向加速度 (cosφ sinθ sinψ − sinφ cosψ) T/mz方向加速度 −g (cosφ cosθ) T/m悬停时φθ0z轴加速度为−gT/m因此Tmg。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级机架惯量数值来自粗略估计也就是让动力学仿真有合理的惯性耦合不必追求高精度。参数值说明m1.5 kg起飞质量g9.81 m/s²重力加速度Ixx / Iyy0.02 kg·m²滚转、俯仰惯量Izz0.04 kg·m²偏航惯量max_thrust25 N总推力限幅min_thrust5 N总推力下限max_tau2 N·m力矩限幅把这些参数保存在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决定角速度环阻尼。控制参数x / y取值z取值整定方向kp_pos5.08.0增大加快位置响应过大会振荡kd_pos3.04.0增大增加阻尼过大会放大噪声kp_att2020增大姿态响应快过大引入弹性振荡kp_rate3.03.0增大角速度比例提高刚度kd_rate0.50.5增大增加角速度阻尼过大会噪声敏感这个表的顺序就是整定顺序先固定姿态环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零型系统的固有特性加前馈能显著降低这个滞后。圆形轨迹参数如下方便后续调整测试轨迹参数值说明半径2 m圆周半径高度1 m定高飞行周期10 s沿圆一圈时间向心加速度0.395 m/s²R*w²用于前馈输入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(:,i1) x(:,i) dt * xd; end欧拉积分在控制频率100Hz、仿真时间10s时稳定性足够误差与ode45相比不超过几个百分点但代码直接可调增益、可加噪声更适合教学。numel(t)在dt0.01时是1001个时间点循环跑1000步MATLAB现代版本几秒钟就能跑完。如果换成dt0.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%留出模型不确定性裕量。扫掠现象原因调整RMSE随kp增大先降后升比例增益增大减小稳态误差过大会引入振荡取最低点左侧20%即使kp很小也有恒定误差轨迹曲率带来的常值滞后检查前馈加速度是否正确传入误差曲线周期性波动内环带宽不足优先提高kp_att和kp_rate5.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控制器在机载处理器上的真实表现而不是在干净仿真里刷出漂亮曲线。本文还有配套的精品资源点击获取