做四旋翼轨迹跟踪控制绕不开一个尴尬的阶段PID调参调到怀疑人生室内匀速巡航没问题一旦加上风扰或者让轨迹蛇形走位置误差立刻开始抖。我当时转向滑模控制是把“非线性、不确定性”交给控制器本身去扛的思路但随之而来的又是高频抖振——控制量在仿真里看上去像锯齿实际飞控上这种输出能把执行器和传感器都磨掉一层皮。最后我是靠自适应滑模控制把问题收拾干净的仿真平台用的是Matlab效果稳定性和工程可解释性都是我试过的控制器里最好的。这篇内容写给准备做四旋翼轨迹跟踪仿真、或者已经写完普通滑模想往下走的同学和工程师。我会把模型、控制器设计、Matlab落地、调参顺序和防坑经验全部过一遍结合我自己在仿真里踩过的问题尽量让这篇文章可以当作一套可以复现的闭环方案来参考。1. 先把被控对象吃透四旋翼位置-姿态级联模型1.1 为什么要拆成两个控制环四旋翼本质上是一个欠驱动系统四个旋翼能提供总共四个独立控制量也就是总推力加三个方向力矩但系统有六个自由度。所以想控制三维位置只能走“位置环给姿态环下达期望姿态指令”这种方式。飞行器不能像汽车那样直接横向推进先要倾斜机身让总推力向量在水平方向产生分量位置才能横向移动。这个特性决定了仿真模型和控制器必须采用级联结构。外环是位置环负责根据当前位置误差计算出期望的虚拟加速度内环是姿态环负责快速跟踪外环给出的期望欧拉角。姿态环响应速度必须明显快于位置环否则级联关系会失效整个闭环会出现振荡。我见过不少仿真初稿的问题是只建了位置环就当作“轨迹跟踪”姿态直接用理想状态代替这种模型只能用来验证轨迹规划不能用来验证控制器鲁棒性。因为滑模控制处理的主要对象就是模型不确定性和未建模干扰姿态动力学本身的非线性和耦合必须要留在仿真里。1.2 简化但足够用的动力学方程常用的仿真模型是牛顿-欧拉方程。位置方程建立在惯性系下姿态方程建立在机体坐标系下m * ddot_p R * [0; 0; U1] - m * g * e3 d_wind J * dot_omega M - cross(omega, J * omega) d_tau其中 p 是机体重心在地坐标系中的位置R 是体坐标系到地坐标系的旋转矩阵U1 是总推力M 是滚转、俯仰、偏航三个力矩分量omega 是机体角速度d_wind和d_tau是外扰和力矩不确定性。为了仿真代码方便姿态方程我通常展开成以下形式Jxx * ddot_phi U2 (Jyy - Jzz) * dot_theta * dot_psi Jyy * ddot_theta U3 (Jzz - Jxx) * dot_phi * dot_psi Jzz * ddot_psi U4 (Jxx - Jyy) * dot_phi * dot_theta对于实验室级别的仿真这样处理已经能保留核心非线性耦合。转子陀螺效应、电机响应延迟都先不考虑后面可以把它们作为不确定项丢给控制器去弥补。仿真阶段就追求“全真”反而会让问题定位变得困难建议一层一层加复杂度。1.3 控制量与旋翼转速的映射位置环算出来的期望加速度最终要转换成四个电机的转速。中间经过两组映射第一组是从期望位置加速度算出总推力和期望姿态角度第二组是从总推力和期望姿态力矩算出单个旋翼的转速。对于“X”型四旋翼如果四个螺旋桨转速分别是 w1、w2、w3、w4那么U1 b * (w1^2 w2^2 w3^2 w4^2) U2 b * L * (w1^2 - w2^2 w3^2 - w4^2) U3 b * L * (w1^2 w2^2 - w3^2 - w4^2) U4 d * (w1^2 - w2^2 w3^2 - w4^2) // 注意可根据旋翼方向调整符号仿真里我建议保留这个映射关系不要直接用“U1直接作用于M”。原因是执行器饱和问题只有通过转速映射才看得出来。给一组常用的仿真参数作为起点单位都是国际单位制参数数值说明m1.2 kg总质量L0.25 m机臂长度Jxx0.012 kg·m²滚转惯量Jyy0.012 kg·m²俯仰惯量Jzz0.02 kg·m²偏航惯量b1.5×10⁻⁵推力系数d3.0×10⁻⁷阻力矩系数这组参数本身没有特殊含义关键是要量级合理。转动惯量必须与质量、臂长匹配否则姿态环控制器的设计会失真。比如质量1.2kg但转动惯量取成0.1这个量级反应就会显得极其迟钝会让仿真看起来“更像飞机”实际上飞机会更快地倾斜控制器参数也会完全不同。2. 滑模控制为什么需要“自适应”普通SMC的抖振困局2.1 从PID到滑模的转变PID的问题在于它建立在平衡点线性化的基础上干扰超过一定范围后控制性能下降很快。滑模控制比较讨巧它不追求“在所有时刻精确抵消非线性”而是强迫状态误差先到达一个设计的滑模面然后沿着这个面滑向原点。到达阶段靠切换项滑动阶段靠等效控制。可以用一个非常粗糙但直观的表达来理解滑模的定位普通反馈控制是“根据误差大小决定推力方向”滑模控制是“根据带误差变化趋势的滑模面决定切换动作”所以它更不纠结于模型的精确性尤其适合处理飞行器的强耦合和参数摄动。2.2 抖振的数学本质你如果只用等效控制和常系数切换增益通常写成这样s dot_e lambda * e U U_eq - K * sign(s)其中 s 是滑模面lambda是滑模面系数K是切换增益。理论上只要K大于系统不确定性的上界就可以保证到达条件成立最终误差收敛。但现实中模型不确定性上界往往不知道或者被估计得过于保守K取值过大。当系统状态在一个很小的区域内反复穿越滑模面时sign(s)会在1和-1之间来回跳变这就是抖振。仿真里看它是控制输入的高频毛刺硬件上更麻烦不只会损坏执行器还会激励起未建模的高频动态。很多人误以为抖振只是“K太大”造成的参数选择问题想办法把K调小就行。但K是抗不确定性的关键参数K太小系统对外扰和参数摄动的鲁棒性就没有了。这里面的张力很明显K大鲁棒性够但抖振伤身K小抖振减轻但跟踪误差可能变差甚至失稳。单纯固定增益的滑模本质上就是个两难选择。2.3 自适应机制在设计上解决了什么自适应滑模的思路是把“不确定性上界”作为在线估计的变量而不是事先猜测一个固定K。切换增益不再是一个常数而是随着误差和滑模面的变化实时调整的动态参数K(t) K0 integral(gamma * abs(s) dt)逻辑很直接系统趋向滑模面、误差快速收敛时K增长的速率放缓状态偏离滑模面、s增大时K自动变大提供更强的纠正力。相当于一个“需要多少就给多少”的保险阀。实际设计里我会用“自适应项叠加到等效控制”的方案。先基于标称模型计算等效控制U_eq再用自适应切换项补偿模型偏差、外部扰动和未建模动态。利用李雅普诺夫函数 V 0.5·s² 0.5·ρ·k˜² 可以证明构造合适的自适应律后闭环系统是渐近稳定或一致最终有界的。仿真时不用纠结每一步公式但这一条稳定性准则很重要最后看K曲线是不是逐渐收敛到一个平稳区间如果是说明不确定性上界估计得当如果K一路疯涨说明切换增益自适应律设计有bug或者扰动边界本身在发散。我再补一个组装的细节自适应律中为什么要用abs(s)而不是s从李雅普诺夫函数导数推导会得到k_hat_dot gamma * abs(s)。它保证K只能单调增长除非加入了遗忘因子让它缓慢衰减。实际系统扰动通常是时变的K只增不减会造成过度保守所以工程中会加边界层处理或死区。3. 自适应滑模律设计与Matlab实现3.1 控制律整体架构整个控制器分成三层写代码前先把逻辑想清楚比直接堆脚本省很多时间位置滑模计算期望位置加速度a_des输入是当前位置误差、速度误差、参考轨迹的加速度前馈项姿态解算从a_des反解期望滚转角和期望俯仰角偏航角直接按任务设定姿态滑模根据当前欧拉角和角速度与期望值的差计算力矩U2、U3、U4。因此自适应律至少有两套位置环的自适应增益K_pos姿态环的自适应增益K_att。两者要分开估计因为位置环的不确定性来源是阻力、风、质量摄动姿态环的不确定性来源是转动惯量误差、力矩系数偏差、电机延时。把它们合在一起自适应控制器会互相干扰调试时会非常狼狈。3.2 一个可运行的MATLAB状态方程骨架我用的是脚本式仿真按固定步长写RK4积分器因为固定步长下观测控制器之间的数值耦合更直观。状态向量设置成x [px; py; pz; vx; vy; vz; phi; theta; psi; p; q; r]其中 p、q、r是机体角速度在机体坐标系的三分量phi、theta、psi是滚转、俯仰、偏航。注意姿态角解算时的欧拉角顺序必须是Z-Y-X实际代码里要统一约定。核心状态方程可以写成一个MATLAB函数参数用struct打包避免到处传参function dx quadrotor_state(t, x, u, params) g params.g; m params.m; % 位置状态 ddx [0; 0; -g] (params.R * [0; 0; u(1)]) / m; % 姿态角与角速度耦合 J params.J; domega [ (u(2) (J(3)-J(2))*x(10)*x(11)) / J(1); (u(3) (J(1)-J(3))*x(9)*x(11)) / J(2); (u(4) (J(2)-J(1))*x(9)*x(10)) / J(3) ]; dx [x(4); x(5); x(6); ddx(1); ddx(2); ddx(3); x(10); x(11); x(12); domega(1); domega(2); domega(3)]; end这里R矩阵我直接从姿态角更新或者使用四元数。小角度机动时欧拉角没问题但是轨迹跟踪涉及大倾角时建议用四元数或者至少加防奇异保护否则算着算着角度越过90度旋转矩阵会跳变。这个坑很容易让新手误以为控制器发散。3.3 控制器函数的实现细节控制器的核心是滑模面和自适应增益。以位置环Z方向为例期望轨迹pd、期望速度vd、期望加速度ad已知位置误差和速度误差分别是e pz - pz_d ed vz - vz_d sz ed lambda_pos * e由于参考轨迹通常本身加速度不为零等效控制里必须包含参考加速度这一项a_z_des ad(3) - lambda_pos * ed ...位置滑模的期望加速度建议写成a_des a_ref - lambda_pos * e_dot_series - K_hat .* tanh(s ./ phi)其中符号函数换成tanh或者用饱和函数。为什么不用sign我们在仿真里逐步缩小步长到1e-4时sign也能跑通只要步长再大一点控制量就会出现伪抖振。这种抖振不是物理意义上的纯粹是数值步长跟不上符号切换速度容易误导你对控制效果的判断。换成饱和函数可以给滑模切换加一个边界层代价是理论上的强鲁棒性略微降级但工程上值得。自适应律直接用欧拉法更新K_hat(1) K_hat(1) dt * gamma_pos * abs(sx);引入边界层后的自适应律我会按半饱和形式处理超调的增长率降下来。K_hat初值不要给零否则系统对外扰的初始响应会很弱。我用的是1到5之间的初始切换增益。3.4 期望姿态角分配计算位置环给出的期望加速度a_des是三维向量。总推力沿机体Z轴建立期望推力方向与a_des加上重力补偿之后的方向一致。如果偏航角是ψ_d期望滚转角和俯仰角可以由下面关系解出先把总推力大小定义为U1 m * sqrt(a_dx^2 a_dy^2 (a_dz g)^2)期望滚转角和俯仰角按小角度假设简化phi_d (a_dx * sin(psi_d) - a_dy * cos(psi_d)) / g theta_d (a_dx * cos(psi_d) a_dy * sin(psi_d)) / g小角度假设在大多数缓变轨迹里够用。但如果目标轨迹是高速小半径盘旋期望倾斜角可能到30度以上此时小角度假设误差会带来明显的耦合。我建议求解全量的解算方式或者至少让位置环自适应增益留出余量来吃这部分误差。姿态环的参考角加速度可以用期望角加上二阶低通滤波器或者前馈获取。姿态环滑模面用角速度误差构建e_atti [phi; theta; psi] - [phi_d; theta_d; psi_d] omega_des 对期望角求导得到的角速度常用数值微分或解析式 s_atti e_omega lambda_atti * e_atti如果仿真中没有期望角速度的解析表达式可以用前一时刻的期望角差分但差分噪声会混进控制器最好在差分后加低通滤波。这一点很容易被忽略一旦姿态响应出现周期性小波动先检查期望角信号是否够平滑。3.5 完整闭环的仿真循环结构我习惯把整个仿真循环拆成四个明确阶段注释写清楚排错时才不会乱dt 0.001; tFinal 15; ts 0:dt:tFinal; x [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0]; cntState.K_hat_pos ones(3,1) * 2; cntState.K_hat_att ones(3,1) * 0.1; for i 1:length(ts)-1 t ts(i); [pos_ref, vel_ref, acc_ref] reference_trajectory(t); [U, cntState] adaptive_smc(x, pos_ref, vel_ref, acc_ref, params, cntState, dt); % 把U映射到电机转速并设置饱和 rpm control_alloc(U, params, rpm_max); U rpm2force(rpm, params); % RK4积分 x_next rk4step(quadrotor_state, x, U, params, dt); % 记录数据 end注意一点控制分配之后一定要做转速饱和之后再用饱和后的转速重新计算实际力和力矩。如果不做这一步位置环要求的高加速度在物理上完全不可实现控制器也不会报错但仿真的曲线“好看”得没有工程意义。我见到过很多次误差看起来很小映射到电机转速却发现早超出最大转速的情况。4. 仿真参数、调参顺序和常见坑4.1 初版参数怎么给参数从零开始调必须有个递推路径不要一次性把位置环、姿态环、自适应速率全都调到接近理想值。给一组我验证过比较稳的启动参数参数建议初值作用方向位置环滑模系数 λ_pos5越大越激进误差收敛快但容易振荡姿态环滑模系数 λ_att12姿态内环需要比位置环快2倍以上位置环自适应速率 γ_pos1.0越高收敛越快过高则K超调姿态环自适应速率 γ_att0.6姿态不确定相对小不用太快饱和函数边界层 φ_pos0.05越厚抖动越小但误差偏大饱和函数边界层 φ_att0.1姿态环可以厚一点K_hat初始值位置2/姿态0.1不要从零起步拉格朗日意义上的“2倍以上”只是最低要求工程上更希望姿态环的时间常数是位置环的三分之一。一个具体可行的办法先固定姿态环参数跑姿态阶跃仿真单独验证姿态跟踪没问题再接位置环。4.2 调参顺序与观察方法第一步只调姿态内环。给一个期望姿态角阶跃比如从零倾斜10度记录滚转角响应的上升时间和超调。姿态环滑模系数λ_att影响响应带宽自适应速率影响切换增益爬升速度。出现超调明显偏大就减小λ出现稳态误差长期不消就检查K_hat是否增长过慢。第二步接位置环先让期望轨迹是悬停即参考位置固定为某个点。悬停场景看起来简单但能暴露所有耦合问题偏航指令不为零时滚转和俯仰通道之间是否会交叉耦合位置误差收敛到多少控制输入是否平稳。我用悬停测试绕开了很多在追踪轨迹时根本没法区分的错误。第三步再做平滑圆形和螺旋轨迹。先慢速最高速度大约1m/s逐步加难度。如果挂在轨迹跟踪一半出现发散多半不是控制器设计错误而是期望姿态解算在大角度时出了严重耦合或者是轨迹加速度前馈没有给够。4.3 我踩过的一些仿真坑第一个坑是参考轨迹初始时刻不连续。这个问题很隐蔽很多参考函数用的是位置表达式而飞行器初始位置是0如果轨迹函数在t0时位置不为0控制器会立刻输出一个非常大的修正加速度幅度可超出电机转速几倍仿真看起来像是“炸机”。解决办法是让参考轨迹的位置、速度、加速度初始值都与仿真初始状态一致或者做一个平滑过渡段。第二个坑是欧拉角奇异。我在做螺旋上升时想加入大幅偏航导致滚转角偶尔接近90度旋转矩阵计算出现NaN。仿真崩溃不算难找难的是某些情况下矩阵仍然能计算但位置动力学却产生了一个向下的错误分力于是飞行器一边跟踪一边缓慢掉高让人以为是滑模增益不足。这类问题必须靠计算期望姿态角时对倾角做软限制来避免。第三个坑是自适应增益不加边界地增大。自适应律中如果只写K_hat gamma * abs(s)一旦某段时间出现高频测量噪声或参考信号突变K_hat会飞速上升之后就算误差正常了K_hat也降不下来。建议给自适应增益设置上下限或者让自适应律只在|s|超过一定阈值时工作。对于干扰快速变化的场景我甚至会把自适应律改成K_hat_dot gamma * (abs(s) - rho * K_hat)rho取0.05到0.2之间的小值让增益在干扰减弱后缓慢回落同时维持足够的鲁棒性。第四个坑涉及仿真步长和求解器。普通滑模用ODE45跑得很好自适应滑模对符号切换更敏感所以最好先固定步长仿真用RK4和dt1e-3。如果换成ODE45建议设相对误差1e-6、绝对误差1e-8否则观测到的“抖振”结果可能与真实控制器行为相差很大。先把仿真环境稳定下来再去讨论控制器抖动问题。5. 结果曲线解读与再进一步5.1 一个典型仿真结果长什么样经过上述调参后一个典型仿真结果是这样的无人机从初始位置0,0,0出发参考轨迹是半径1米的圆高度2米角速度0.5rad/s。前0.5秒会出现一个明显的收敛峰值位置误差从初始偏移快速收敛到厘米级。1秒以后位置误差围绕零作小幅周期性波动幅度大概在2-5厘米。姿态角响应则出现与轨迹周期同步的正弦状变化滚转角峰值大约10到15度。自适应增益K_hat这条曲线是最值得看的一条。如果K_hat在初始阶段快速上升之后停留在一个恒定或窄区间内说明自适应律对不确定性进行了有效估计如果K_hat始终在很小范围内不见明显增长说明系统干扰可能并不大控制器的切换项基本没起什么作用你可以适当加大扰动幅度去看自适应机制能否跟上。控制输入U1曲线大约在mg附近波动波动幅度不超过20%。如果U1曲线出现每秒几十次的高频往返先看边界层φ是否太小再检查是不是符号函数使用不当最后检查电机转速是否频繁进入饱和边界。排除顺序很重要因为三个原因产生的曲线形状高度相似。5.2 干扰和参数不确定性的注入方式仿真里注入干扰要遵循物理逻辑。最粗放的方式是在速度状态上加一个常值比如vx2这会造成一种很假的“瞬移”效应控制器看到的是位置几乎没动但速度突变。更合理的方式是在加速度层面加干扰比如给位置方程加一个d_wind项同时在姿态方程加入力矩扰动d_tau。我会这样设置随机风d_wind 0.5 * sin(2*pi*0.3*t) 0.2 * randn 参数摄动m_actual m_nominal * 1.1或者J_actual J_nominal * 1.2控制器里使用的还是标称参数m和J这样能真实反映参数失配。特别提醒randn每次运行产生的噪声序列都不同同一套仿真两次结果会有差异。想要结果可复现在每个仿真脚本开头加上rng(0)固定随机种子否则对比调参效果时会把随机因素误判成控制器改进。参数摄动也可以直接体现在控制分配上。比如真实推力系数是标称值的0.9倍那么给定转速实际产生的推力就偏小控制器必须在不知情的情况下靠自适应切换项补偿。这种低层干扰对轨迹跟踪的破坏力往往比直接加在位置方程上的常数干扰更真实也是在实物平台上最常遇到的情况。5.3 后面可以怎么玩自适应滑模控制跑通之后可以往几个方向继续扩展。一是把自适应机制从“切换增益自适应”改为“系统参数在线辨识”。比如设计自适应律直接估计质量和阻力系数控制律变成参数自适应滑模鲁棒项的组合跟踪精度通常更高。代价是参数估计的收敛速度需要有持续激励条件轨迹如果只有一条直线质量和阻力的可辨识性都不会很好。二是和扩张状态观测器结合。用扩张状态观测器估计总扰动再把扰动估计值前馈进等效控制这样切换增益可以进一步降低抖振吸到不明显的程度。与纯自适应滑模相比这个方案在回路噪声大的场景下更好使但调试工作量也更大。三是考虑执行器动态。四旋翼电机并不是瞬间响应转速指令仿真中加入一阶惯性环节带宽设20到50Hz会让控制器设计更贴近实机。加入之后你会发现原来普通滑模在“瞬时执行器”假设下表现很好但一旦执行器有延迟控制效果就明显变差而自适应滑模因为增益能自动调整受影响程度小很多。这也是我在实际仿真中最终坚定使用自适应方案的原因。做仿真控制就是这样曲线跑得再漂亮最后还是要回到物理可实现性。把执行器饱和、欧拉角奇异、数值步长这些“底层杂事”处理干净自适应滑模才能真正算落地。如果你也正在为抖振或者轨迹偏离大而调整普通滑模参数不妨先建一个带参数摄动和风扰的固定步长仿真环境然后把固定切换增益换成自适应律。你会发现改动量不大但控制器的“脾气”完全不一样了。