1. 四旋翼轨迹跟踪的痛点与PSOPID双闭环的破局思路四旋翼无人机从实验室里的玩具级飞控到工业巡检、农业植保、物流配送这些真实场景中间隔着一道很深的鸿沟这道鸿沟的名字就叫轨迹跟踪精度。你让它悬停它勉强能稳住你让它沿着一条螺旋线爬升它就开始画龙你让它做高速S形机动它直接翻给你看。很多刚入行的朋友拿到一套开源飞控调了几天PID发现定点还行一跑航线就飘得没边最后归结为“这飞机就这样”。其实问题往往不在硬件而在控制器的参数整定和环路结构上。我这些年做过不少四旋翼的仿真和实机项目从最朴素的单环PID到后来的LQR、MPC都试过。说实话对于大多数中小型四旋翼平台双闭环PID仍然是性价比最高、最容易落地、最经得起工程考验的方案。所谓双闭环就是位置环套速度环或者姿态环套角速度环外环负责“我要去哪”内环负责“我怎么快速且稳定地到达”。这个结构的好处是分工明确外环可以慢一点、稳一点内环必须快、必须硬这样才能抵抗风扰和模型不确定性。但双闭环PID有个老毛病——参数太多耦合太强。位置环三个轴速度环三个轴姿态环三个轴角速度环三个轴每个轴至少Kp、Ki、Kd三个参数加起来就是几十个参数。你手动调调完X轴Y轴又乱了调完Y轴Z轴又超调了。这时候粒子群算法PSO就派上用场了。PSO是一种群体智能优化算法说白了就是让一群“粒子”在参数空间里飞来飞去每个粒子代表一组PID参数通过迭代找到让轨迹跟踪误差最小的那组参数。它不需要梯度信息对目标函数的凸性没要求特别适合这种多参数、非线性、强耦合的优化问题。这个专题要做的就是在Simulink里搭一套完整的四旋翼动力学模型设计位置-速度-姿态-角速度的双闭环PID控制器然后用PSO自动整定所有环路参数最后跑三维轨迹跟踪仿真看看到底能跟得多准。适合谁看如果你正在做毕设、写论文、或者公司里要快速验证一个飞控算法这套流程可以直接抄作业。如果你只是好奇PSO怎么和PID结合我也会把原理拆开讲清楚保证你看完能自己改代码、改模型。注意本文所有仿真均基于MATLAB/Simulink环境不涉及任何实际飞行操作。实机飞行有风险参数迁移需谨慎。2. 四旋翼模型与双闭环控制架构拆解2.1 为什么选双闭环而不是单闭环先说说单闭环PID为什么不够用。单闭环通常指位置环直接输出电机转速或者姿态角中间没有速度反馈。这种结构在低速、小扰动下能凑合但一旦遇到阵风或者负载变化外环输出的姿态指令会剧烈抖动内环根本跟不上结果就是飞机像喝醉酒一样晃。双闭环的核心思想是把“位置误差”转化为“速度期望”再把“速度期望”转化为“姿态期望”最后把“姿态期望”转化为“角速度期望”。每一层都有独立的PID控制器外环的输出是内环的输入层层递进每一层只关心自己那一小段动态。具体到四旋翼我采用的结构是位置环输入期望位置(xd, yd, zd)和实际位置(x, y, z)输出期望速度(vxd, vyd, vzd)。速度环输入期望速度和实际速度输出期望姿态角(φd, θd)和期望总推力T。姿态环输入期望姿态角和实际姿态角输出期望角速度(pd, qd, rd)。角速度环输入期望角速度和实际角速度输出四个电机的PWM或转速指令。这个结构里位置环和速度环是慢回路姿态环和角速度环是快回路。慢回路的带宽通常设在1~2 Hz快回路设在10~20 Hz这样内外环在频域上拉开足够距离避免相互干扰。如果你把内外环带宽设得太近系统就会产生振荡这是很多新手调参时最容易踩的坑。2.2 四旋翼非线性模型的简化与假设四旋翼的完整动力学模型很复杂包含气动阻尼、桨叶挥舞、电机动态、电池电压衰减等等。但在仿真阶段我们通常做以下合理简化刚体假设机体是质量均匀的刚体忽略弹性变形。对称假设四旋翼关于机体轴系对称惯性矩阵是对角阵。小角度假设姿态角在±30度以内时欧拉角变化率近似等于机体角速度。电机一阶惯性电机响应用一阶传递函数近似时间常数取0.02~0.05秒。忽略地面效应和桨间干扰在高度大于1米时这些影响较小。基于这些假设四旋翼的平动方程可以写成m * a R * [0, 0, T]^T - [0, 0, m*g]^T - D * v其中m是质量a是加速度R是旋转矩阵T是总推力g是重力加速度D是气动阻尼矩阵v是速度。转动方程写成I * ω_dot τ - ω × (I * ω)其中I是惯性矩阵ω是角速度τ是电机产生的力矩。这两个方程就是Simulink里要搭的核心。2.3 双闭环PID控制器的设计要点每个环路的PID控制器形式我统一采用位置式PID因为位置式PID在Simulink里实现简单而且积分项可以单独限幅防止积分饱和。公式如下u(t) Kp * e(t) Ki * ∫e(t)dt Kd * de(t)/dt对于位置环和速度环我通常只加P和I不加D因为位置和速度信号本身噪声小加D容易引入高频抖动。对于姿态环和角速度环P、I、D都要加尤其是角速度环的D项对抑制超调和振荡非常关键。这里有个经验角速度环的Kd不要太大否则电机指令会抖得厉害仿真里可能看不出来实机上会烧电机。我一般把角速度环的Kd限制在Kp的1/10到1/5之间。另外积分限幅很重要。位置环的积分限幅可以设得大一点比如±2米速度环的积分限幅设±1米/秒姿态环的积分限幅设±0.1弧度角速度环的积分限幅设±0.5弧度/秒。这些数值不是绝对的但可以作为起点。3. PSO优化PID参数的完整实现流程3.1 PSO算法原理与参数选择PSO的核心思想来自鸟群觅食。每只鸟就是一个粒子它在搜索空间里的位置代表一组候选解速度代表它下一步往哪飞。每个粒子记住自己历史最优位置pbest整个群体记住全局最优位置gbest。每次迭代粒子根据这两个最优位置调整自己的速度v_i(t1) w * v_i(t) c1 * r1 * (pbest_i - x_i(t)) c2 * r2 * (gbest - x_i(t)) x_i(t1) x_i(t) v_i(t1)其中w是惯性权重c1和c2是学习因子r1和r2是0到1之间的随机数。w控制全局搜索和局部搜索的平衡c1控制粒子向自身历史最优学习c2控制粒子向群体最优学习。我常用的参数组合是种群规模N50最大迭代次数T100w从0.9线性递减到0.4c1c22.0。为什么w要递减因为前期需要大范围探索后期需要精细收敛。如果w一直很大最后会在最优解附近震荡如果w一直很小容易陷入局部最优。还有一个细节速度限幅。粒子的速度不能无限大否则会飞出搜索空间。我一般把速度限制在搜索空间范围的10%~20%。比如某个参数范围是[0, 10]速度限幅就设[-1, 1]或[-2, 2]。3.2 目标函数的设计与权重分配PSO需要一个标量目标函数来评价每组PID参数的好坏。对于轨迹跟踪最直接的目标是累积位置误差。但只用位置误差不够因为如果只优化位置误差PSO可能会找到一组让飞机剧烈振荡但平均误差很小的参数。所以目标函数里还要加入控制量变化率和超调量的惩罚。我用的目标函数形式如下J ∫(w1 * |e_pos| w2 * |e_vel| w3 * |e_att| w4 * |u_dot|) dt其中e_pos是位置误差e_vel是速度误差e_att是姿态误差u_dot是控制量变化率。权重w1到w4需要根据实际需求调整。如果最看重跟踪精度w1取大一点比如10如果担心控制量抖动w4取大一点比如1。我一般先用w110, w21, w31, w40.1跑一遍看看结果再微调。还有一个技巧在目标函数里加入惩罚项如果仿真过程中飞机发散或者姿态角超过限制直接给一个很大的J值。这样可以避免PSO搜索到不稳定的参数区域。3.3 Simulink模型与PSO的联合仿真接口PSO是跑在MATLAB脚本里的Simulink模型是图形化的两者怎么联合有两种方法方法一用sim命令直接调用Simulink模型。在PSO的适应度函数里把粒子位置赋值给MATLAB工作区的PID参数变量然后调用sim(model_name)仿真结束后从logsout或yout里提取轨迹数据计算目标函数。这种方法简单直接适合模型不太复杂的情况。方法二用MATLAB Function块把PSO算法嵌入Simulink。这种方法适合需要在线优化的场景但实现起来复杂而且仿真速度慢。对于离线整定我推荐方法一。具体操作步骤在Simulink模型里所有PID参数不要写死而是引用MATLAB工作区的变量比如Kp_pos_x、Ki_pos_x等。在PSO脚本里定义粒子位置向量的维度等于所有待优化参数的总数。比如位置环3个轴每个轴Kp、Ki两个参数就是6个速度环6个姿态环9个角速度环9个总共30个参数。设置每个参数的搜索范围。Kp一般取[0, 20]Ki取[0, 5]Kd取[0, 2]。这些范围可以根据经验调整。在适应度函数里用assignin把粒子位置赋给工作区变量调用sim然后计算J。运行PSO主脚本等待迭代完成输出最优参数。注意Simulink仿真时间不要设得太长否则PSO迭代一次要等很久。我一般设仿真时间10秒步长0.001秒这样一次仿真大概几秒钟100次迭代乘以50个粒子就是5000次仿真大概几个小时能跑完。如果嫌慢可以开并行计算用parfor加速。4. Simulink仿真搭建与参数整定实操4.1 从零搭建四旋翼Simulink模型打开MATLAB新建一个Simulink模型命名为quadrotor_pso_pid。模型分为四个主要部分动力学模块、控制器模块、轨迹生成模块、可视化模块。动力学模块我用MATLAB Function块实现输入是四个电机转速输出是位置、速度、姿态角、角速度。代码大概长这样function [pos, vel, att, omega] quad_dynamics(motor_speeds, pos0, vel0, att0, omega0) % 参数定义 m 1.0; % 质量 kg g 9.81; Ixx 0.01; Iyy 0.01; Izz 0.02; kf 1e-5; % 推力系数 km 1e-6; % 力矩系数 L 0.2; % 臂长 dt 0.001; % 步长 % 计算总推力和力矩 T kf * sum(motor_speeds.^2); tau_phi L * kf * (motor_speeds(4)^2 - motor_speeds(2)^2); tau_theta L * kf * (motor_speeds(3)^2 - motor_speeds(1)^2); tau_psi km * (motor_speeds(1)^2 - motor_speeds(2)^2 motor_speeds(3)^2 - motor_speeds(4)^2); % 旋转矩阵 phi att0(1); theta att0(2); psi att0(3); R [cos(theta)*cos(psi), sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi), cos(phi)*sin(theta)*cos(psi)sin(phi)*sin(psi); cos(theta)*sin(psi), sin(phi)*sin(theta)*sin(psi)cos(phi)*cos(psi), cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi); -sin(theta), sin(phi)*cos(theta), cos(phi)*cos(theta)]; % 平动加速度 acc R * [0; 0; T/m] - [0; 0; g]; % 转动角加速度 omega_dot [tau_phi/Ixx; tau_theta/Iyy; tau_psi/Izz]; % 积分更新状态 vel vel0 acc * dt; pos pos0 vel * dt; omega omega0 omega_dot * dt; att att0 omega * dt; end这个函数块用连续积分或者离散积分都可以我习惯用离散积分步长0.001秒和Simulink的固定步长一致。控制器模块用四个子系统实现分别对应位置环、速度环、姿态环、角速度环。每个子系统里放三个PID Controller块参数引用工作区变量。轨迹生成模块我用MATLAB Function块生成三维螺旋线或者S形轨迹输出期望位置和期望速度。可视化模块用Scope或者XY Graph显示三维轨迹我更喜欢用MATLAB Function块把数据输出到工作区然后用plot3画图这样更灵活。4.2 PSO整定PID参数的代码实现PSO主脚本我一般写成这样% PSO参数 N 50; % 种群规模 T 100; % 迭代次数 w_max 0.9; w_min 0.4; c1 2.0; c2 2.0; % 待优化参数维度 dim 30; % 位置环6 速度环6 姿态环9 角速度环9 % 搜索范围 lb zeros(1, dim); % 下界 ub [20*ones(1,6), 5*ones(1,6), 20*ones(1,9), 2*ones(1,9)]; % 上界 % 初始化粒子 x repmat(lb, N, 1) rand(N, dim) .* repmat(ub-lb, N, 1); v zeros(N, dim); pbest x; pbest_fit inf(N, 1); gbest x(1, :); gbest_fit inf; % 主循环 for iter 1:T w w_max - (w_max - w_min) * iter / T; for i 1:N % 计算适应度 fit fitness_function(x(i, :)); if fit pbest_fit(i) pbest_fit(i) fit; pbest(i, :) x(i, :); end if fit gbest_fit gbest_fit fit; gbest x(i, :); end end % 更新速度和位置 for i 1:N v(i, :) w * v(i, :) c1 * rand * (pbest(i, :) - x(i, :)) c2 * rand * (gbest - x(i, :)); % 速度限幅 v(i, :) max(v(i, :), -0.2*(ub-lb)); v(i, :) min(v(i, :), 0.2*(ub-lb)); x(i, :) x(i, :) v(i, :); % 位置限幅 x(i, :) max(x(i, :), lb); x(i, :) min(x(i, :), ub); end fprintf(Iteration %d: Best fitness %.4f\n, iter, gbest_fit); end % 输出最优参数 disp(Optimal PID parameters:); disp(gbest);适应度函数fitness_function.m大概长这样function J fitness_function(params) % 解析参数 Kp_pos params(1:3); Ki_pos params(4:6); Kp_vel params(7:9); Ki_vel params(10:12); Kp_att params(13:15); Ki_att params(16:18); Kd_att params(19:21); Kp_omega params(22:24); Ki_omega params(25:27); Kd_omega params(28:30); % 赋值到工作区 assignin(base, Kp_pos, Kp_pos); assignin(base, Ki_pos, Ki_pos); % ... 其他参数类似 % 运行仿真 try sim(quadrotor_pso_pid, 10); % 提取轨迹数据 pos_actual logsout.get(pos).Values.Data; pos_desired logsout.get(pos_des).Values.Data; error pos_actual - pos_desired; J sum(sum(abs(error))) * 0.001; % 积分 catch J 1e6; % 仿真失败给大惩罚 end end这里有个细节assignin和sim的配合。如果你在Simulink模型里用的是基础工作区变量assignin(base, ...)就能生效。如果你用的是模型工作区那就得用set_param或者Simulink.SimulationInput对象。我推荐用基础工作区简单直接。4.3 仿真结果分析与参数验证跑完PSO之后你会得到一组最优参数。但别急着高兴PSO找到的不一定是最优的只是它搜索到的最好的。你需要做几件事来验证第一把最优参数代回Simulink跑一条和优化时不同的轨迹。比如优化时用的是螺旋线验证时用S形或者圆形。如果跟踪效果依然很好说明参数泛化能力不错如果变差了说明过拟合了需要重新设计目标函数或者增加轨迹多样性。第二看控制量的变化率。如果电机转速指令在0.1秒内从0跳到10000转那实机上肯定做不到电调会饱和。这时候需要在目标函数里加大u_dot的权重或者直接在Simulink里加一个速率限幅器。第三看姿态角的响应。如果姿态角在跟踪过程中频繁超过30度那说明速度环输出的姿态期望太大了需要限制速度环的输出幅值或者降低位置环的Kp。我做过一组对比手动调参的双闭环PID位置跟踪误差RMS大概是0.15米PSO优化后的RMS降到0.05米左右。提升还是很明显的尤其是Z轴的高度跟踪手动调参时高度总是有0.2米左右的稳态误差PSO优化后基本能压到0.05米以内。5. 常见问题与排查技巧实录5.1 PSO不收敛或者收敛到局部最优怎么办这是最常见的问题。表现是迭代曲线早早平了但适应度值还是很大。原因通常有三个一是种群多样性不足。如果初始粒子都挤在一起或者速度限幅太小粒子飞不开。解决办法是增大初始分布范围或者把速度限幅从10%提高到20%。我还会在迭代中期随机重置几个粒子的位置强制增加多样性。二是目标函数设计不合理。如果目标函数对某些参数不敏感PSO就找不到优化方向。比如位置环的Ki如果轨迹跟踪时间很短积分项还没起作用仿真就结束了那PSO就不知道Ki该取多少。解决办法是延长仿真时间或者把积分项单独拿出来做阶跃响应测试。三是搜索范围设得太窄。如果你把Kp限制在[0, 5]但实际最优值在8那PSO永远找不到。我一般先用手动调参大致确定一个范围然后把这个范围扩大2~3倍作为PSO的搜索空间。5.2 Simulink仿真速度太慢怎么加速PSO优化需要跑几千次仿真如果每次仿真要几十秒那整个优化过程要几天。加速方法有用加速模式Simulink的Accelerator模式比Normal模式快很多尤其是模型复杂的时候。在Simulation菜单里选Accelerator或者用set_param(model,SimulationMode,accelerator)。用并行计算如果你有多个CPU核心用parfor替换for循环每个粒子在一个核心上跑。我试过8核并行速度提升大概6倍。简化模型把不必要的Scope和Display去掉把数据记录改成只记录需要的信号。Scope在仿真时很耗资源。增大步长如果模型里没有高频动态步长可以从0.001秒改成0.002秒甚至0.005秒。但要注意步长太大可能导致数值不稳定。5.3 优化后的参数实机迁移需要注意什么仿真归仿真实机归实机。仿真里忽略的东西实机上都会冒出来。迁移时注意电机动态仿真里电机是一阶惯性实机上还有电调的死区、电池电压衰减、桨叶不平衡。建议在仿真里把电机时间常数从0.02秒改成0.05秒留点余量。传感器噪声仿真里位置和速度是完美的实机上GPS有噪声IMU有漂移。建议在仿真里给位置和速度加高斯噪声幅度根据实际传感器手册设定。风扰仿真里没有风实机上户外飞行肯定有风。建议在仿真里加一个常值风扰和阵风模型看看参数抗不抗扰。安全第一实机首飞一定要在开阔场地高度不要超过2米随时准备切手动。参数先按仿真值的70%用慢慢往上加。5.4 常见问题速查表问题现象可能原因排查方法解决措施仿真发散位置无限增大PID参数过大或符号错误检查Kp是否为正减小Kp把Kp降到原来的1/10逐步增加跟踪有稳态误差积分项太弱或没有积分限幅检查Ki是否为零检查积分限幅增大Ki但不要超过Kp的1/5姿态角高频振荡角速度环Kd太大观察角速度指令波形减小Kd或加低通滤波器PSO迭代曲线不下降目标函数计算错误手动代入一组参数验证J值检查误差提取和积分逻辑Simulink报代数环错误控制器有直接馈通检查PID块是否用了微分项在微分项后加一个一阶滤波仿真速度极慢步长太小或模型太复杂用tic/toc测单次仿真时间改用Accelerator模式或并行计算提示如果你在Simulink里用Bus Selector发现没有可选信号先检查总线是否正确定义。在MATLAB里用Simulink.Bus对象定义总线然后在Bus Creator里指定总线类型Bus Selector就能看到信号了。6. 从仿真到落地的经验总结这套PSO优化双闭环PID的流程我从头到尾跑过不下十遍有成功的也有翻车的。最大的体会是PSO不是万能的它只是一个工具关键还是你对控制对象的理解。如果你连四旋翼的动力学都没搞明白PSO给你一组参数你也解释不了为什么好。反过来如果你对手动调参有感觉PSO能帮你省掉大量试错时间而且能找到一些你手动调不出来的参数组合。另一个体会是目标函数的设计比PSO算法本身更重要。我见过很多人用PSO优化PID结果优化出来的参数在仿真里很好实机上直接炸机。原因就是目标函数只考虑了跟踪误差没考虑控制量平滑度和鲁棒性。后来我在目标函数里加了控制量变化率惩罚和风扰测试优化出来的参数就稳多了。最后分享一个小技巧在PSO迭代后期把惯性权重w降到0.2以下同时把学习因子c1和c2都调到1.5左右这样粒子会在全局最优附近做精细搜索收敛精度能提高不少。这个技巧是我在一次调参中偶然发现的后来查文献发现有人专门研究过叫“自适应PSO”原理差不多。如果你正在做四旋翼轨迹跟踪的课题我建议你先用手动调参跑通整个Simulink模型确保动力学和控制器逻辑没问题然后再上PSO。否则PSO跑了几小时最后发现是模型里有个符号写反了那才叫崩溃。模型验证的方法很简单给一个阶跃位置指令看看飞机能不能稳定飞到目标点并停住。如果能说明模型基本正确如果不能先别碰PSO回去查模型。