做飞行控制仿真的，基本都绕不开一类特殊场景：无人机拖着载荷在空中飞行，飞到某个航点后忽然把载荷释放掉。这个动作看起来简单，实际上是最考验控制器鲁棒性的工况之一。机体质量瞬间变小，重心位置突变，转动惯量也跟着变，原本调好的控制参数在那一刻全部失效，轨迹曲线立马给你脸色看。
我最近在Matlab里完整做了一遍“无人机载荷投放轨迹跟踪控制”仿真，从动力学建模、控制器设计到数值仿真全链路走通。这篇文章不铺背景，直接把这套项目的思路、计算公式、Matlab代码核心片段和踩过的坑完整记录下来，给做飞行器方向课题、想复现变质量无人机控制的同学一份可以直接抄作业的参考。
1. 项目解构：载荷投放到底改变了什么
1.1 核心矛盾是“模型断裂”而不是“扰动”
很多人一听到载荷投放，第一反应是“外界的力扰动”。但在仿真里你会发现问题没那么简单。投放前，你的控制系统是围绕一个包含机体和载荷的完整模型设计的，所有控制增益都在这个模型下被调到了稳定状态。投放瞬间，模型本身变了——质量从 m0+m载荷 跳到 m0，质心沿着机体轴移动了一段距离，转动惯量矩阵对角线和非对角线分量全部改变。
这相当于你正在骑一辆后座驮着重物的自行车，骑得好好的，突然重物没了，重心后移，手把立刻变轻。这不是一阵风把你吹歪了，而是整个车架动力学变了。控制器如果还按旧模型计算期望推力，会瞬间输出一个偏大的油门，再加上重心偏移引入的额外力矩，姿态就和参考轨迹脱开了。
所以我的第一个设计判断是：载荷投放的建模核心不是“加一个瞬时扰动”，而是把系统视为一个“分段切换的混杂系统”。投放前一个模型，投放后一个模型，切换瞬间是跳跃条件。控制器要做的是让切换后的状态平滑地回到参考轨迹上。
1.2 控制方案选型：为什么是内外环串级结构
轨迹跟踪控制主流的方案有几类：直接对非线性模型做反步法、反馈线性化、滑模控制，或者更工程化的内外环串级PID。我在这个项目里选的是后者，原因有三：
第一，载荷投放的突变主要体现在外环的加速度需求和内环的姿态响应之间。位置环输出的期望加速度需要转化为期望姿态角，姿态环再跟踪这个角度。投放瞬间的质量变化会直接反映在位置环的计算里，而姿态环只需要处理重心偏移带来的力矩扰动，两件事被天然解耦。
第二，内外环串级的调参逻辑清晰。先调内环姿态，再调外环位置，哪一环出问题很容易定位。反步法虽然理论上可以一次设计出全局稳定的控制器，但模型切换后需要重新推李雅普诺夫函数，计算量很大，调起来很不直观。
第三，对于仿真验证来说，串级结构容易加入前馈补偿。比如投放瞬间额外补偿一个油门增量，这个在纯反馈控制器里很难单独加进去。
1.3 轨迹跟踪的评价指标先定下来
动工写代码之前，我把仿真结果的评价方式先定好了，避免后面调参时“好坏全凭感觉”。我关注的指标有四个：
- 投放后轨迹的最大位置偏差，这是最直观的指标，反映了控制器抗模型切换的能力
- 恢复到参考轨迹半径1米以内所需的时间，体现的是回复动态
- 姿态角是否超过35度，超过这个值意味着飞行状态比较危险
- 控制量是否在饱和范围内，如果推力或力矩指令长时间顶到饱和，控制器已经失效了
这套指标在后面的所有试验里都保持不变，调参和对比方案时才有参照系。
2. 动力学建模与载荷投放的关键细节
2.1 四旋翼刚体模型用哪套方程
先用大家最熟悉的牛顿-欧拉方程描述四旋翼。定义地面惯性系下的位置向量 p=[x,y,z]^T，机体速度用 v 表示，姿态用四元数 q（后面我会说为什么不直接用欧拉角），角速度用 ω 表示。系统方程是：
m \dot{v} = -m g e3 + R(q) T e3 + f_{ext}
J \dot{\omega} = -\omega \times J \omega + \tau + \tau_{ext}
其中 R(q) 是四元数对应的旋转矩阵，T 是四个旋翼产生的总推力沿机体z轴方向，τ 是机体坐标系下的控制力矩，f_ext 和 τ_ext 是外部力和力矩。位置和姿态的运动学方程是：
\dot{p} = v
\dot{q} = 0.5 q \otimes [0, \omega]^T
公式本身不复杂，但这个项目里每一个变量都可能随载荷投放而变化：m 是切变量，J 是切变量，质心位置变化后，推力力臂也变了。所以我在代码里把质量、惯量矩阵、质心偏移这三个量全部设计成“可以随时切换”的参数，而不是写死在常量区。
2.2 载荷投放的三种建模方式
载荷投放的建模方法我试过三种，各有利弊。
第一种是“切换模型法”。设定一个投放时刻 t_release，t<t_release 时用包含载荷的质量和惯量，t>=t_release 时用机体的质量和惯量。这种方法最简单，代码里一个 if 语句就能实现，但缺点是没有描述投放瞬间的力学过程，相当于把载荷凭空消失。
第二种是“质点耦合法”。把载荷建模为一个通过绳索或硬固定在机体某个位置的质量点，投放前它通过约束力与机体相连，投放瞬间删除约束力。这个方法最接近物理真实，能模拟出投放瞬间因为约束突然消失而产生的冲击，但实现复杂度高，需要处理约束方程，数值仿真时还容易因为约束力突变而收敛困难。
第三种是“分段切换+扰动脉冲”的混合法。质量、惯量用切换模型处理，同时在投放时刻叠加一个衰减的扰动力矩脉冲，模拟重心突变和约束释放引起的横滚/俯仰冲击。这样既保留了切换模型的高效率，又能体现投放动态。
我在仿真里最终采用了混合法。投放瞬间的质量变化是阶跃的，但由此引起的力矩突变我把它建模成一个短时衰减的扰动项：
\tau_{drop}(t) = M_{drop} e^{-(t - t_release)/τ_d}，施加时间约0.1秒
M_{drop} 的大小由重心偏移量和当前推力估算。这个处理很关键，后面会详细说。
2.3 重力、推力与重心偏移的力矩计算
投放瞬间最容易被忽视的是重心偏移。载荷通常挂在机体下方或侧面，释放前系统总质心在机体传感器参考点附近，释放后质心位置会沿着载荷连接点的方向偏移一段距离 δr_p。
这里要算清楚的是：推力 T 在机体z轴方向，作用点通常在机体几何中心，而重力作用点在质心。质心偏移后，推力相对于质心产生一个力矩。机体重力本身也会产生扰动力矩吗？不会，重力作用在质心，力臂为零。但推力的作用线不过质心了，所以：
\tau_{offset} = δr_{CG} \times (R^T T e3)
这个力矩的大小和当前推力成正比，所以大油门状态下投放载荷，姿态冲击会特别明显。我的仿真里把 δr_CG 设为一个可配置向量，比如 [0.05, 0, -0.1]^T 米，意思是质心在x轴正向偏移5厘米、z轴负向偏移10厘米。这个配置直接决定投放瞬间姿态扰动的大小，值得仔细调。
3. Matlab实现：从零搭建轨迹跟踪仿真
3.1 整体程序架构设计
我把整个项目拆成了四个文件：
config_params.m：定义无人机参数、载荷参数、控制器参数、轨迹参数dynamics.m：四旋翼动力学方程，支持运行中切换质量/惯量/质心偏移controller.m：内外环串级控制器，输出推力与三轴力矩main_run.m：主循环，负责积分、数据记录和画图
没有用Simulink，而是用纯M脚本加固定步长四阶龙格库塔（RK4）求解。原因是载荷投放这个时刻有强烈的模型切换，Simulink的变步长求解器在切换点时经常因为Jacobian突变而步长收缩到非常小，仿真速度慢得离谱。固定步长RK4配合小步长（我用了0.005秒），反而稳定且速度快。
3.2 动力学函数实现
动力学函数接收当前状态和控制量，输出状态导数。核心代码如下：
function dx = dynamics(t, x, param, T, M_tau) % x = [px, py, pz, vx, vy, vz, qw, qx, qy, qz, wx, wy, wz] p = x(1:3); v = x(4:6); q = x(7:10); % [w, i, j, k] 顺序 w = x(11:13); m = param.mass_total; J = param.inertia_total; % 姿态旋转矩阵 R = quat2rotm(q'); e3 = [0; 0; 1]; % 主动力：重力和推力 f_gravity = [0; 0; -m * param.g]; f_thrust = R * (T * e3); % 外部扰动力（风等，默认0） f_ext = param.f_ext; % 加速度 a = (f_gravity + f_thrust + f_ext) / m; % 姿态动力学 omega_skew = skew(w); tau_ext = param.tau_ext; % 含投放冲击力矩 dw = J \ (-cross(w, J * w) + M_tau + tau_ext); % 运动学 dp = v; dq = 0.5 * quatmultiply(q', [0, w'])'; % 四元数微分 dx = [dp; a; dq; dw]; end注意这里的quatmultiply是Matlab自带函数，要求输入四元数为行向量且顺序为 [w, x, y, z]，和状态向量里的顺序要严格对齐。我在这个项目里被顺序坑过一次，后面会在排查章节里专门说。
3.3 控制器实现：位置环与姿态环
控制器是整篇项目的核心。我采用的思路是：位置外环根据位置和速度误差计算期望加速度，再把期望加速度转成期望姿态角和期望总推力；姿态内环根据当前姿态角和期望姿态角的误差计算控制力矩。
位置环的期望加速度公式是：
a_des = a_ref + Kp_p * (p_ref - p) + Kd_p * (v_ref - v)
这里没有用积分项，原因是投放瞬间位置误差会突然变大，积分项会在短时间内大幅累积，等误差恢复后积分导致的过冲反而更麻烦。位置环只靠PD就能回到参考轨迹，当然前提是内环姿态跟得上。
期望总推力是期望加速度沿机体z轴的投影加重力抵消：
T_des = m * (a_des + g * e3) \cdot (R * e3)
期望姿态角则需要从期望加速度反解。这里我用了简化的方式：假设小角度姿态，直接从期望加速度在机体轴的投影反算期望俯仰和横滚角：
phi_des = (a_des_x * sin(psi_ref) - a_des_y * cos(psi_ref)) / g;
theta_des = (a_des_x * cos(psi_ref) + a_des_y * sin(psi_ref)) / g;
psi_ref 是期望偏航角，通常给0或缓慢变化的航向角。
姿态内环我用PD控制，对期望姿态角做一阶滤波得到期望角速度，再计算力矩：
% 姿态角误差 err_phi = phi_des - x(8); % 实际phi从四元数解算 err_theta = theta_des - x(9); err_psi = psi_ref - x(10); % 角速度误差 dphi_ref = params.Kp_theta * err_phi; % 简化的角速度期望 ... % 力矩指令 M_x = params.Kp_phi * err_phi + params.Kd_phi * (dphi_ref - x(11)); M_y = params.Kp_theta * err_theta + params.Kd_theta * (dtheta_ref - x(12)); M_z = params.Kp_psi * err_psi + params.Kd_psi * (dpsi_ref - x(13));姿态角的PD参数是调参的重点。投放瞬间姿态误差可能出现短时增大，如果Kd太小，姿态回正会很慢，整个轨迹都会受影响；Kd太大则姿态响应会抖，容易振荡。
3.4 载荷投放时刻的模型切换与补偿
在main_run.m的主循环里，我每步积分前都检查当前时间是否到达t_release。一旦到达，就执行三件事：
- 更新总质量为机体质量
- 更新转动惯量矩阵
- 在
tau_ext里注入持续0.15秒的衰减扰动力矩
为了抵消模型切换导致的掉高和控制迟滞，我在投放时刻同步给一个额外的推力前馈增量。这个增量的经验值我后面会给出表格。
% 主循环核心片段 for k = 1:floor(t_end / dt) t = (k-1) * dt; if t >= t_release && ~updated_flag param.mass_total = param.m_body; param.inertia_total = param.I_body; param.tau_ext = disturbance_amp * exp(-(0:dt:0.15)/tau_decay); % 推力前馈补偿 param.thrust_feedforward = m_body * 0.4; % 经验值 updated_flag = true; end T = position_controller(...) + param.thrust_feedforward; M_tau = attitude_controller(...); [~, dx] = rk4_step(@dynamics, t, x, param, T, M_tau, dt); x = x + dx * dt; end推力前馈补偿的量不能太大，我的经验是补偿机体重量对应的推力的30%~50%，补偿时间控制在0.2秒内，否则飞机会先冲高再回落，轨迹波动反而更大。
3.5 参考轨迹生成
我用的参考轨迹是一圈水平圆周加投放后的直线返回。水平圆的好处是轨迹连续，能稳定考察跟踪精度；投放点选在圆轨迹的三点钟位置。参考轨迹生成方式：
% 参考轨迹参数 R_circle = 5; % 半径 omega_circle = 0.2; % 角速度 rad/s z_ref = 3; % 高度 % 位置参考 x_ref = R_circle * cos(omega_circle * t); y_ref = R_circle * sin(omega_circle * t); z_ref = z_ref; % 速度参考（解析求导） vx_ref = -R_circle * omega_circle * sin(omega_circle * t); vy_ref = R_circle * omega_circle * cos(omega_circle * t); vz_ref = 0;如果用带平滑过渡的轨迹更好，比如用梯形速度曲线代替纯正弦，但圆形轨迹用于验证控制器带宽足够了。高度参考一直保持3米，投放发生在 t=8s，投放后继续沿圆飞完一整圈，观察位置偏差的恢复过程。
3.6 仿真结果可视化
我输出的图形有三类：三维轨迹图、位置误差曲线图、姿态角响应曲线图。三维轨迹图能把投放点前后的轨迹偏离看得很直观，位置误差曲线能定量评估恢复时间，姿态响应则用来检查是否有超出安全范围的姿态角。
画图代码没什么特别的，关键是记录好时间序列和误差变量。我建议在main_run.m里用一个 struct 数组把所有状态量存下来，最后统一绘图，避免在循环里反复打开figure，那样速度会非常慢。
4. 常见问题与排查技巧实录
4.1 投放瞬间掉高幅度大，怎么救
我第一版仿真投放瞬间垂直高度掉了快0.8米，参考圆轨迹完全不是圆了，而是一个明显的“凹陷”。排查下来原因有两个：一是控制器的响应速度不够，位置环带宽太窄，掉高发生后一两秒才反应过来；二是没有给前馈补偿，控制器完全靠反馈慢慢把高度拉回来。
解决方法是双管齐下：提高位置环高度通道的Kp，从0.6提高到1.2；同时增加投放时刻的推力前馈，补偿时间约0.1秒。调完之后最大掉高从0.8米降到了0.15米以内。
这里有一个可复用的经验：变质量工况下，依靠纯反馈去扛质量突变会非常吃力，因为误差在这一刻急剧放大，反馈增益又不能无限提高，提高增益会导致姿态振荡。所以前馈补偿不是可选项，是必须项。
4.2 四元数与欧拉角的坑
最初我把姿态状态用欧拉角表示，仿真到投放后姿态角变化超过40度时，某些通道的角度解算出现奇异，俯仰角的导数变得巨大，数值直接爆炸。后来全部改成四元数表达，状态向量里存放四元数，只在控制器反馈计算时把四元数转成欧拉角用于误差计算。
要特别注意Matlab的quatmultiply和quat2eul是航天领域习惯的四元数顺序 [w,x,y,z]，而很多教材里用 [x,y,z,w]。我在动力学函数和控制器里都统一用 [w,x,y,z] 顺序，并在文件头部写了注释，否则一两个星期后再看代码一定会踩这个坑。
4.3 投放瞬间扰动太大导致数值积分发散
有一组仿真参数下，投放瞬间的扰动矩阵施加后，状态在几个积分步内就飞掉了。检查发现是我把扰动力矩幅值设为了静平衡点稳态推力的一半，这个值在实际中可能等价于瞬间给飞机施加了一个巨大的横滚力矩，加上固定步长RK4在强非线性和模型切换叠加时精度不足。
解决办法是把扰动衰减时间常数从0.005秒加大到0.02秒，同时把步长从0.005秒降到0.002秒。如果你发现仿真发散，优先检查步长是否匹配系统最高频率，其次再检查扰动量级。
4.4 控制器积分项导致投放后长时间过冲
控制器如果用了位置环积分项，投放瞬间的大误差会让积分项飞快累加，即使高度已经恢复到参考值，积分器里还残留大量“记忆”，导致无人机继续冲高，然后反向修正，来回振荡好几轮才稳定。我在调试中把位置环积分项完全去掉了，只保留PD控制。这样稳态误差略微增大，但换来了更好的动态响应。
这个取舍在变质量系统里很重要：积分项是为恒定扰动设计的，而模型切换属于瞬时结构性变化，积分器在这个场景下帮不上忙，只会坏事。
4.5 调参顺序与经验值速查
我分享一组跑通的参数作为参考，注意这些参数需要结合你自身的无人机模型调整，不能直接套用：
| 参数 | 数值 | 说明 |
|---|---|---|
| 机体质量 | 1.2 kg | 不含载荷 |
| 载荷质量 | 0.5 kg | 占总质量约30% |
| 旋翼臂长 | 0.25 m | 用于惯量计算 |
| 位置环Kp | 1.2 | 投放后偏高一些 |
| 位置环Kd | 1.8 | 阻尼适当 |
| 姿态环Kp | 8.0 | 内环要快 |
| 姿态环Kd | 1.2 | 防止振荡 |
| 仿真步长 | 0.002 s | RK4固定步长 |
| 投放前馈推力 | 0.35m_bodyg | 经验值 |
调参时先断开投放逻辑，把圆周轨迹跟踪调到位置误差小于0.05米，再开启投放功能调扰动和补偿。如果投放后轨迹恢复慢，优先加位置环Kp；如果姿态振荡，优先减小姿态环Kp或增大Kd；如果数值发散，优先减小步长而不是盲目调增益。
4.6 一个容易被忽略的细节：参考轨迹速度同步
投放时刻参考轨迹的期望速度如果是突变的，也会造成额外误差。我的参考轨迹用正弦，速度和加速度都是连续变化的，投放点在圆轨上速度连续，所以控制器看到的参考信号是平滑的，这部分误差不会干扰投放过程的评估。如果你用航点插值生成的轨迹，建议用S型速度规划或者五次多项式平滑过渡，避免参考轨迹本身给控制器引入高频分量。
5. 项目扩展与我的个人体会
这套仿真跑通之后，我最大的体会是：做载荷投放控制，真正的难点不在控制算法本身，而在于对“模型切换”这个物理过程的理解是否到位。很多人一开始会花很多时间研究高级控制算法，结果发现投放瞬间连基础PID都救不回来，问题出在模型切换被建模得过于理想化。把质量变化、重心偏移、推力补偿这些工程细节做扎实，比换一个复杂的控制器有效得多。
后续这个项目如果想要继续深化，有几个值得尝试的方向：把载荷建模成摆模型，研究投放过程中绳索张力的影响；或者在仿真中加入风扰动，测试控制器对外界干扰和模型切换双重作用下的鲁棒性；还可以把整个控制器换成ADRC，对比一下自抗扰控制在变工况下的表现。
如果做一个无人机海上救援物资投放的项目，投放瞬间的动力学特性基本就是这套框架，只是要把风浪扰动和着陆误差叠加进去。控制框架可以复用，重点在于调整补偿策略和评价指标。这套代码改一改，很快就能用在类似的变质量飞行器仿真场景里。
