基于MATLAB/Simulink的四旋翼着陆控制仿真与触地状态机实践

1. 着陆这十几秒为什么值得单独拿出来做仿真四旋翼从三米高度降到地面全程可能不到二十秒但这一段控制代码的密度往往比整个巡航段加起来还高。我在做实机调试的时候有个很直观的体会悬停的时候飞机稳得像钉在空中一旦开始降高各种平时被气压高度和速度前馈掩盖的问题全冒出来了——地面效应让推力突然“变富余”触地瞬间高度传感器跳变电机卸载后积分项还在死命累积结果就是落地后原地抽搐甚至侧翻。这些问题在实机上复现代价太高一次桨叶打地就是几百块的教训所以我把整套着陆逻辑搬进了MATLAB做仿真验证。这篇内容聊的是基于MATLAB/Simulink的四旋翼着陆控制仿真核心不在于把动力学方程抄一遍而在于怎么构建一个“足够真实、又能跑得动”的验证环境让着陆阶段的状态机、轨迹规划、控制器参数在接触到真实硬件之前就被反复蹂躏一遍。适合已经能写出基本四旋翼模型、但在着陆逻辑上反复翻车的朋友如果你是刚接触飞行器建模的新手前三节里有大量的坐标系和分配矩阵推导跟着走也能看懂。需要先摆正一个预期仿真不是用来证明“我的控制器是对的”而是用来快速证伪。它最擅长告诉你哪种参数组合一定会发散、哪种状态机切换顺序会死锁、哪个环节的地面效应没补偿会撞地。凡是涉及真实气动、电机响应非线性、传感器噪声谱的东西仿真只能给你一个近似方向别指望它替你调好最终参数。把这条边界想清楚后面的建模取舍会容易很多。我在前期搭建时踩过一个典型的坑一开始为了让“曲线好看”把地面效应、传感器延迟、电机一阶惯性全部简化掉结果仿真里完美着陆实机上从三十厘米高度直接砸下来。后来把这三项加回去仿真才开始复现真实问题。这件事让我后面所有的模型都在追求一个原则——宁可模型粗糙但包含关键非线性也不要精确却过于干净。2. 把四旋翼写成十二状态方程建模阶段的三次返工2.1 坐标系约定先定死别中途改建模返工最频繁的地方不是方程本身而是坐标系约定。机体前右下、世界北东地还是东北天旋转矩阵是R乘还是转置角速度在机体系还是世界系——这些约定如果中途改一次之前所有调好的参数全部作废。我的建议是在打开MATLAB之前先在一张纸上把符号约定写清楚作为后续所有代码的注释头。我采用的是世界坐标系为北东地NEDz轴向下为正机体坐标系为前右下旋转矩阵用ZYX顺序从机体系到世界系% 欧拉角转旋转矩阵phi滚转, theta俯仰, psi偏航 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)];这里有个细节值得说着陆阶段俯仰和滚转都很小一般不超过10度但偏航角可能任意。如果我为了省事用小角度近似落地前的偏航对齐逻辑就会在仿真里“看起来很准”实际却因为忽略了大偏航下的耦合项产生横向漂移。所以我的做法是姿态解算用全旋转矩阵状态方程里用四元数积分只在需要显示和判断时才转回欧拉角。这样既避免了万向节奇异又不会让代码里到处是三角函数。2.2 力和力矩怎么写进分配矩阵四旋翼的动力学本质上就是牛顿-欧拉方程平动m * v_dot R * [0; 0; T] - m * g * [0; 0; 1] F_dist转动J * omega_dot -cross(omega, J * omega) tau其中T是四个旋翼产生的总推力tau是三个轴向力矩。这些都是教科书内容真正容易出错的是分配矩阵的符号和布局。我用的是X型布局前左、前右、后右、后左四个电机编号1到4那么电机编号位置滚转力矩符号俯仰力矩符号偏航力矩符号1前左-2前右---3后右-4后左-表格看着简单但我在第一次写混控时把偏航符号搞反了仿真里表现为“想左转偏要往右拧”排查花了整整一个下午。后来我写了一段自检代码给四个电机分别注入单位转速差检查产生的力矩方向是否符合物理直觉这段自检现在成了我所有新模型的标配。推力模型我用的是简化的二次方关系T_i k_f * w_i^2力矩用tau_i k_m * w_i^2反扭矩系数k_m和推力系数k_f的比值决定了偏航控制效率。这两个参数不需要很准但比值要接近你所用的桨叶实际数据否则偏航环的增益会离谱到没法调。2.3 地面效应必须进模型哪怕只是个近似前面说过把地面效应砍掉是我最初最大的错误。当旋翼离地高度低于大约一个旋翼直径时桨盘下方的气流受到地面阻挡压力升高同样的转速能产生更大的推力。工程上常用Cheeseman-Bennett近似% z为旋翼平面到地面的距离, R为旋翼半径 ratio 1 / (1 - (R/(4*z))^2); % 推力放大系数 thrust_ige thrust_oge * ratio;这个公式在z R附近会急剧发散所以实际代码里要加一个下限保护比如限制z不小于0.3R同时在z 2R时直接取1。这段保护不做着陆末段会因为系数爆炸导致数值积分崩掉仿真直接报错退出。地面效应带来的控制影响很具体在同样的推力指令下飞机越接近地面下降越慢甚至出现“悬停”假象。如果你的控制器没意识到这一点积分项会继续累积推力等到触地后转速还在高位于是弹跳。仿真里看到“落地前速度突然变得很平缓”八成就是这个原因别以为是自己参数调得好。3. 串级控制器的分工外环管“到哪”内环管“怎么转”3.1 位置环的参考信号怎么给四旋翼的位置控制标准做法是串级外环根据位置误差输出期望的姿态角和总推力内环跟踪姿态角输出三轴力矩最底层通过混控矩阵转成四个电机的转速指令。这个结构本身没什么争议着陆阶段的特殊性在于参考信号不是阶跃而是一条精心设计的下降剖面。我的外环是位置-速度-加速度三环并联的形式输出期望加速度% 期望加速度 位置误差项 速度误差项 前馈加速度 a_des Kp_pos .* (p_ref - p) Kd_pos .* (v_ref - v) a_ref; % 叠加前馈重力补偿后反解姿态与总推力 f_des a_des [0; 0; g]; T_des norm(f_des); phi_des asin( (f_des(1)*sin(psi_ref) - f_des(2)*cos(psi_ref)) / T_des ); theta_des atan2( f_des(1)*cos(psi_ref) f_des(2)*sin(psi_ref), f_des(3) );这里有个很实际的取舍位置环的带宽通常只能做到1 Hz左右因为再快就会被内环的姿态响应拖住产生振荡。所以外环不负责“落地时刻的精确跟踪”它只保证飞机大致沿着剖面下降真正决定触地质量的是内环姿态精度和轨迹剖面的斜率设计。很多人调不好着陆问题不在位置环增益而在于把参考轨迹给得太陡。3.2 姿态环的PD为什么必须加前馈姿态环用PD是最常见的但着陆阶段有个特殊需求姿态不能有稳态误差否则在恒定侧风下飞机会以一个小倾角持续漂移。纯PD在常值扰动下确实会留误差加积分能消掉但积分在触地瞬间又会因为推力卸载的突变产生过冲。我的折中方案是姿态环用PD加角加速度前馈前馈项由期望姿态的微分滤波得到把大部分跟踪压力交给前馈PD只负责消除残差和抑制扰动。前馈的代价是对噪声很敏感期望姿态一旦有高频抖动微分之后放大成剧烈的力矩指令。所以前馈通道必须串联一个二阶低通滤波器截止频率设在姿态环带宽的1.5倍左右既能保留前馈的前瞻性又不至于把高频噪声引进电机。这个滤波器参数我第一次调的时候设成了10 Hz结果仿真里电机指令像心电图一样跳动把截止频率降到3 Hz之后曲线立刻变得干净。3.3 参数整定的顺序不能乱串级控制器的参数必须自内向外调这个顺序乱一步就白费功夫先调姿态环只给姿态阶跃位置环断开目标是阶跃响应上升时间小于0.15 s超调小于10%。姿态环稳定后闭合速度环观察速度跟踪的相位滞后。最后闭合位置环此时外环增益可以压得比较低1 Hz带宽足够。全环闭合后单独做一次着陆剖面仿真看垂直速度跟踪误差。我习惯用频域自检辅助时域把每个环的开环传递函数画出来要求相位裕度大于45度幅值裕度大于6 dB。着陆过程本质上是低动压、强扰动的工况裕度给足一点比追求响应快更重要。有一次我把姿态环增益提高到相位裕度只剩30度仿真里常规飞行没问题一进着陆末段遇到地面效应引起的推力扰动就直接振荡起来——这种问题在频域曲线上其实早有预警。4. 着陆轨迹规划为什么不干脆直接往下飞4.1 一条直线下降会带来什么最朴素的想法是让参考高度从3米线性降到0速度恒定到点触地。这个方案在仿真里跑起来漂亮但几乎必然在真实飞行中出问题原因有三接近地面时下降速度没有缓冲触地瞬间垂直速度仍是设定值冲击载荷直接作用在起落架上。地面效应的推力增强让实际下降速度低于指令值控制器积分累积触地后突然卸载导致弹跳。水平位置漂移没有纠正窗口如果下降过程中有侧风飞机可能落在目标点半米之外。所以着陆轨迹必须分段设计每段的边界条件位置、速度、加速度都要显式给出来让控制器在每一段都有明确的“任务定义”。4.2 分段剖面的具体设计我目前用的剖面分四段每段都用五次多项式生成因为五次多项式能同时满足位置、速度、加速度三个边界条件生成的曲线是C2连续的不会给姿态环造成加速度突变。阶段起始高度结束高度垂直速度设计意图快速下降3.0 m1.2 m-0.8 m/s迅速脱离高空气流扰动区减速缓冲1.2 m0.6 m-0.4 m/s提前降速避开地面效应最强区精降段0.6 m0.15 m-0.25 m/s慢速贴近给触地判定留时间窗触地段0.15 m0 m-0.12 m/s极低速接地减小冲击五次多项式的实现要注意归一化把时间映射到[0,1]再求解系数能避免高次项数值爆炸function [p, v, a] quinticTraj(t, T, p0, pf, v0, vf, a0, af) s t / T; % 归一化时间 A [1 0 0 0 0 0; % 系数矩阵求解 0 1 0 0 0 0; 0 0 2 0 0 0; 1 1 1 1 1 1; 0 1 2 3 4 5; 0 0 2 6 12 20]; b [p0; v0*T; a0*T^2; pf; vf*T; af*T^2]; c A \ b; p c(1) c(2)*s c(3)*s^2 c(4)*s^3 c(5)*s^4 c(6)*s^5; v (c(2) 2*c(3)*s 3*c(4)*s^2 4*c(5)*s^3 5*c(6)*s^4) / T; a (2*c(3) 6*c(4)*s 12*c(5)*s^2 20*c(6)*s^3) / T^2; end每段的时间T不是随便给的要从起止速度差除以允许加速度反推。比如快速下降段从0加速到-0.8 m/s允许加速度0.5 m/s²那时间至少1.6秒实际取2.5秒留余量。这个计算过程不能省我见过直接把T设成1秒的结果加速度峰值到了2 m/s²姿态环直接被拉出10度倾角飞机横着飘走了。4.3 末段速度和触地阈值的联动最后一段的垂直速度设定直接决定了触地冲击和触地判定的可靠性。速度太快判定窗口窄速度太慢地面效应导致的“悬停假象”会让飞机迟迟不下降飞到电池耗尽。我实测下来-0.12 m/s到-0.15 m/s这个区间比较合适配合高度阈值0.08 m能在0.6秒左右完成触地判定留出的时间窗足够状态机完成切换。阈值不能拍脑袋定它和你的高度传感器精度直接挂钩。气压计在近地面的分辨率大概在0.1 m量级超声波能到0.01 m但容易受地面材质影响。仿真时我用的是“理想高度高斯噪声”标准差按实际传感器标定值设定这样触地判定逻辑在仿真里被噪声考验过一遍实机上才不会因为一次读数抖动就误判。5. 触地检测与电机卸载仿真里最容易被忽略的状态机5.1 三条件联合判定比单看高度可靠得多只用高度判断触地是最危险的做法。地面不平、传感器零漂、桨叶下洗流打在平台上造成的压力跳变都可能让高度读数瞬间掉到零控制器以为落地了实际还在空中于是切断推力飞机直接自由落体。我在仿真里专门构造了这个场景在0.3米高度注入一次持续50 ms的负向高度跳变纯高度判定立刻失效。我的判定逻辑用三个条件同时满足高度低于阈值 h_land0.08 m垂直速度绝对值小于 v_land0.2 m/s且连续N个周期成立期望推力小于悬停推力的0.9倍说明控制器已经在主动卸载第三个条件是关键它本质上是问“控制器是不是已经认为不需要那么多推力了”。三个条件联合误判概率大幅下降。仿真里我把这三个条件做成可配参数跑蒙特卡洛时随机扰动传感器噪声和初始高度统计误判率这套流程比在实机上反复试错高效太多。5.2 落地后的“抽搐”是积分饱和在作怪触地后电机不能立刻全停需要有个渐降过程否则飞机会因为残余升力弹起来。但渐降过程中如果位置环的积分项还在累积“我还差一点高度”的误差推力指令会维持在高位等真正切断时又猛地掉下来形成上下抖动。解决办法是在状态机进入LANDED状态时同时做三件事冻结位置环和速度环的积分器、把参考高度锁定为当前高度、将推力指令按斜坡降到零。这三步的顺序很重要先冻积分再锁参考否则锁定瞬间参考值跳变又会激发一次积分累积。% 状态机触地分支的简化处理 if state LANDED pos_int pos_int; % 冻结不再累加 vel_int vel_int; h_ref h_current; % 参考锁定 T_cmd max(0, T_cmd - dT_rate * dt); % 斜坡卸载 if T_cmd 0.02 * T_hover motors_off(); end end积分抗饱和还有一层在正常下降段就要用clamping当推力指令饱和在上下限时停止积分。我最初的版本没做这个下降段推力接近下限饱和积分项一路累积到负值触地后积分反向释放导致飞机在地面上“点头”。这个现象在仿真曲线上表现得很清楚也是我后来养成习惯——每次看着陆曲线先看积分项那条线。6. 仿真工程的搭建细节求解器、步长与噪声注入6.1 变步长求解器在着陆仿真里会坑你Simulink默认的ode45是变步长对纯连续系统很好用但着陆模型里有大量离散事件控制器以100 Hz采样、触地判定是离散判断、电机指令有零阶保持器。变步长求解器遇到这些离散跳变时会在跳变点附近疯狂缩小步长仿真速度慢到无法接受而且不同版本的求解器对跳变的处理不一致导致同一份模型在不同机器上跑出不同结果。我现在的配置是固定步长ode4RK4步长1 ms。这个选择有明确的理由控制器周期10 ms1 ms步长意味着每个控制周期内积分10次精度足够电机一阶惯性时间常数通常在20到50 ms1 ms的分辨率能准确捕捉。如果把步长放到5 ms快速姿态响应会失真放到0.1 ms仿真时间翻十倍但精度提升有限不划算。模型里离散模块和连续模块混用时要保证采样周期是仿真步长的整数倍否则会出现“控制器在两次采样之间执行了半次”的诡异现象。我一般在模型初始化脚本里统一定义Ts_ctrl 0.01和Ts_sim 0.001所有模块引用变量而不是写死数字改步长时只改一处。6.2 噪声和延迟怎么注入才接近真实干净的仿真只会给你虚假的信心。我在模型里固定注入三类“不干净”的东西传感器噪声高度用高斯白噪声叠加慢漂移IMU角速度叠加高频噪声和常值零偏。噪声幅值按实际器件手册的量级设定不用精确谱密度但量级要对。执行器延迟电机指令到实际转速之间串一个一阶惯性环节加纯延迟。纯延迟用Simulink的Transport Delay模块设置在15 ms左右。采样与保持控制器输出经过零阶保持器再进被控对象这一步很多人省掉但它决定了实际系统的相位滞后省掉之后内环增益会调得偏高。三类扰动全加上之后仿真曲线会变得“脏”但这才是调参应该面对的画面。我有个判断标准如果控制参数是在干净模型上调出来的拿到带扰动的模型里增益至少要降30%才能稳定。与其在实机上被迫降增益不如一开始就在脏模型里调。6.3 蒙特卡洛批量跑别只信一条曲线单次仿真通过不代表逻辑可靠。我写了一个批处理脚本随机化质量±10%、转动惯量±15%、气动系数±20%、初始水平位置±0.5 m、常值风速0到3 m/s随机方向跑200次着陆统计成功率、最大触地垂直速度、最大水平偏移三个指标。指标合格线我第一版的实测值着陆成功率大于95%78%触地垂直速度小于0.3 m/s0.52 m/s最差样本水平偏移小于0.3 m0.61 m最差样本第一版78%的成功率很有说服力地指出了问题失败样本里超过一半是“风速超过2 m/s时水平偏移超标”说明位置环的抗风能力不足而不是着陆逻辑本身有错。这种定位效率是单次仿真给不了的。批处理跑一次大概二十分钟比在实机上试二十次划算太多。7. 从仿真曲线到真机那几条仿真不会告诉你的差距仿真跑通之后最容易产生的误判是“我的算法没问题了”。实际上一上真机至少还有四类差距需要现场处理。第一类是电机响应非线性。仿真里我用的是一阶惯性加二次方推力模型真机上低油门段的死区和响应延迟明显不同导致小推力调整时飞机反应迟钝着陆末段的高度微调变得很困难。应对办法是在仿真里给推力模型加一个死区让控制器提前适应。第二类是下洗流与地面相互作用。仿真里的地面效应是个静态公式真机上桨叶下洗流打到地面会反射回来形成周期性的压力波动表现为高度读数的小幅抖动同时让飞机有轻微的上浮感。这个现象在硬质地面比草地明显得多我一般会在仿真里给地面效应系数加一个低频正弦扰动来近似。第三类是电池电压下降导致的推力衰减。仿真里推力系数是常数真机上电池从满电放到低压同样的转速指令产生的推力会下降5%到10%。着陆通常发生在飞行末段正好是电压最低的时候这个衰减必须靠前馈补偿或者转速闭环来处理。仿真里我把k_f设成随“剩余电量”线性下降逼着控制器去面对这个问题。第四类是姿态估计的滞后。仿真里姿态是真值直接可用真机上要经过滤波和融合相位滞后可能到20 ms以上。这个滞后对内环稳定性影响很大。我在仿真里给姿态反馈串了一个二阶低通来模拟截止频率设在15 Hz虽然不完全准但至少让增益整定时考虑到这个约束。8. 踩坑实录三个卡了很久的具体问题第一个坑是仿真发散但找不到原因。现象是着陆到0.2米左右状态量突然变成NaN。排查了两天才发现是地面效应公式在z趋近于0时除零虽然我加了保护但保护写在了计算之后。这种数值问题一定要在公式入口处做钳位而不是算完再修正。第二个坑是触地后飞机反向上升。原因是积分抗饱和只做了上限没做下限下降段推力指令长期处在下限饱和积分累积到很大的负值触地卸载后积分慢慢回正过程中推力被抬起来。修法是把积分钳位做成双向的并且在饱和期间完全不累加。第三个坑是同一份模型两次运行结果不同。查到最后是随机噪声模块的种子没固定蒙特卡洛脚本里每次初始化都用了不同的随机流。后来所有随机模块统一用固定的种子序列保证结果可复现——这点在做参数对比时特别重要不然你根本分不清性能差异是参数改的还是噪声随机的。现在我的习惯是每改一次控制器参数先跑一遍固定种子的基准用例再跑200次蒙特卡洛两组结果都通过才认为改动有效。这套流程看起来很笨但比在实机上试错省下来的时间和成本完全值得。