四旋翼仿真模型：高精度非线性建模，支持ADRC与PID控制器灵活切换及纯姿态角控制模式

四旋翼仿真simulink模型，支持ADRC和pid控制器切换，支持断开位置环的纯姿态角控制模式，非线性建模，精度高。

最近在搞四旋翼仿真，发现一个挺有意思的Simulink模型。这玩意儿支持ADRC和PID控制器热切换，还能单独玩纯姿态角控制，建模精度比那些线性化模型靠谱多了。今天直接上干货，带大家拆解几个关键模块。

先看非线性动力学模型的核心代码块：

% 四旋翼刚体动力学微分方程 function dx = nonlinear_model(t, x, u) J = diag([0.016, 0.016, 0.0028]); % 惯量矩阵 g = 9.81; phi = x(4); theta = x(5); psi = x(6); R = rotation_matrix(phi, theta, psi); % 旋转矩阵 % 角加速度计算 omega = x(10:12); tau = u(2:4); omega_dot = J \ (tau - cross(omega, J*omega)); % 线加速度计算 F_total = R * [0; 0; u(1)] - [0; 0; g]; acc = F_total; % 质量归一化为1 dx = [x(7:9); x(10:12); acc; omega_dot]; end

这段代码把四元数换成欧拉角直接硬算，虽然计算量大了点，但胜在物理意义明确。注意看旋转矩阵的耦合项保留了所有非线性项，比小角度假设的模型更能反映真实飞行状态。有个坑要注意：当俯仰角接近90度时得加条件判断，不过模型里用了个符号运算技巧自动规避了奇点。

四旋翼仿真simulink模型，支持ADRC和pid控制器切换，支持断开位置环的纯姿态角控制模式，非线性建模，精度高。

控制器切换这块设计得很骚气，来看ADRC和PID的切换逻辑：

Switch_Controller |---[u] = ADRC_Controller(position_ref, states) |---[u] = PID_Controller(position_ref, states)

切换信号直接走外部输入，做对比实验时不用重新编译模型。重点说下ADRC的实现：

function u = ADRC_core(y, ref) persistent ESO; if isempty(ESO) beta1 = 100; beta2 = 300; beta3 = 1000; % 观测器带宽 ESO = extended_state_observer(beta1, beta2, beta3); end z = ESO.update(y); v0 = ref - z(1:3); u = nonlinear_feedback(v0, z); end

这个ESO（扩张状态观测器）参数调得比较激进，能吃到250Hz以上的扰动。不过要注意离散化方法得用Tustin变换，不然数值稳定性会出问题。对比PID参数：

Kp = diag([2.5, 2.5, 8]); % 位置环 Ki = diag([0.1, 0.1, 0.5]); Kd = diag([1.2, 1.2, 3]);

PID参数明显保守，但胜在调试直观。实测切换时过渡还算平滑，没有出现控制量跳变，关键是在积分项初始化时做了个状态同步：

if switch_flag PID.reset(ADRC.get_internal_state()); % 无缝衔接控制器状态 end

纯姿态模式更简单粗暴，直接在位置环输出后加了个Switch模块。当断开位置环时，控制器只接收姿态角指令。这个模式特别适合测试底层控制器性能，比如试飞前校准电机推力。

最后说下仿真精度，用变步长ode45求解器时，相对误差控制在1e-6级别。不过建议实时性测试改用ode23tb，虽然计算量大了30%，但能更好处理控制量突变的情况。模型里还埋了个彩蛋：在姿态角超过安全阈值时会触发保护逻辑，这个用Stateflow写的条件监测，比用S函数快得多。

跑个对比测试：ADRC在突加风扰时恢复时间比PID快40%，但CPU占用高了15%。要是想魔改模型，记得先把那个用mask封装起来的电机动力学模块解开，里面有惊喜——作者居然用实测数据拟合了电机延迟曲线。