多旋翼无人机的动力学建模与控制算法仿真，包含LQR线性二次调节器和 LQ-MPC线性二次模型预测控制两种控制策略Matlab代码-优快云博客

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码及仿真咨询内容私信。

🔥 内容介绍

【无人机控制】多旋翼无人机的动力学建模与控制算法仿真，主要包含 LQR（线性二次调节器）和 LQ-MPC（线性二次模型预测控制）两种控制策略Matlab代码

这段 MATLAB 代码实现了多旋翼无人机的动力学建模与控制算法仿真，主要包含 LQR（线性二次调节器）和 LQ-MPC（线性二次模型预测控制）两种控制策略，结合状态估计与非线性动力学仿真，验证控制性能。以下是代码的核心功能与结构解析：

一、参数初始化与物理建模

基础参数定义设定无人机的物理参数，包括质量（M）、重力加速度（g）、尺寸（L1/L2/L3，电机到质心的距离）、转动惯量（Ixx/Iyy/Izz，来自 CAD 模型），以及电机特性参数（推力系数Kthrust、扭矩系数Ktau等），这些参数是动力学建模的基础。
平衡点输入定义悬停或稳定飞行时的基准输入（U_e为电机电压，W_e为对应转速），控制算法将在该平衡点附近进行小信号调整。

二、线性化状态空间模型

连续时间动力学模型构建 14 维状态空间模型（A/B/C/D矩阵），状态包括：
- 位置（x）、速度（x_dot）、角度（phi）、角速度（phi_dot）
- 电机转速（6 个电机）输入为 6 个电机的控制信号，输出为位置和角度。矩阵元素基于牛顿 - 欧拉方程推导，反映状态间的动态耦合关系。
离散化处理通过c2d函数将连续系统转换为离散时间系统（采样时间T=0.01s，100Hz），适配数字控制器的实现。

三、LQR 控制设计与仿真

状态反馈增益计算定义代价函数权重矩阵Q（状态惩罚，如位置、角度误差）和R（控制惩罚，抑制过大输入），使用dlqr函数求解最优反馈增益Kdt，实现全状态反馈控制。
仿真与结果可视化运行 LQR 控制仿真，记录无人机的高度（Alt）、姿态（phi/theta/psi）及电机 PWM 输入，绘制响应曲线，验证控制算法的稳定性和跟踪性能。

四、卡尔曼滤波状态估计

滤波器设计考虑传感器噪声和模型误差，构建卡尔曼滤波器，通过定义过程噪声（Rw）和测量噪声（Rv）协方差矩阵，计算滤波增益Ldt，实现对位置、速度、姿态等状态的最优估计。
状态估计流程结合非线性动力学预测（ode45求解Hex_Dynamics）和测量误差反馈，修正状态估计值，提升控制系统的抗噪声能力。

五、LQ-MPC 控制设计与仿真

模型预测控制框架定义预测 horizon（N=100），构建包含终端惩罚的成本函数（基于 LQR 的Q/R矩阵和终端矩阵P），通过二次规划（quadprog）求解带约束的最优控制序列。
仿真与性能分析对比 LQR，LQ-MPC 考虑输入约束（如电机 PWM 信号范围），在跟踪参考轨迹（高度、姿态阶跃指令）时，通过滚动优化实时调整控制输入，绘制实际输出、估计误差、控制输入及成本函数曲线，验证其在约束条件下的优越性。

六、核心功能与应用

控制算法验证
：对比 LQR（无约束）和 LQ-MPC（带约束）的控制效果，评估轨迹跟踪精度、控制输入平滑性及抗噪声能力。
状态估计验证
：通过卡尔曼滤波减少测量噪声影响，提升状态反馈的可靠性，模拟实际传感器环境。
非线性动力学耦合
：结合Hex_Dynamics非线性模型，使仿真更接近真实飞行场景，避免线性化模型的局限性。

⛳️ 运行结果

📣 部分代码

% Hessian

Sc = 2*Gxcl'*Qd*Gxcl + 2*Gucl'*Rd*Gucl;

% make sure the Hessian is symmetric

Sc = (Sc+Sc')/2;

% Linear term

Scx = 2*Gxcl'*Qd*Fxcl + 2*Gucl'*Rd*Fucl;

🔗 参考文献

[1] Tao J , Ma L , Zhu Y .Improved control using extended non-minimal state space MPC and modified LQR for a kind of nonlinear systems[J].Isa Transactions, 2016, 65.DOI:10.1016/j.isatra.2016.08.015.

[2] Tao J , Ma L , Zhu Y .Improved control using extended non-minimal state space MPC and modified LQR for a kind of nonlinear systems[J].ISA transactions, 2016, 65:319-326.DOI:10.1016/j.isatra.2016.08.015.