【无人机控制】Simulink四旋翼无人机在SE(3)上的几何控制仿真【含Matlab源码 7230期】

✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信或扫描文章底部QQ二维码。

🍎个人主页：Matlab凤凰涅槃
🏆代码获取方式：扫描文章底部QQ二维码

⛳️座右铭：行百里者，半于九十。
更多Matlab路径规划仿真内容点击👇
①Matlab路径规划（凤凰涅槃版）

⛳️关注微信公众号Matlab王者助手或Matlab海神之光，更多资源等你来！！

⛄一、Simulink四旋翼无人机在SE(3)上的几何控制仿真

Simulink是一款由MATLAB公司提供的工具箱，广泛用于系统建模和仿真。在四旋翼无人机的SE(3)几何控制仿真中，原理主要包括对无人机的姿态（旋转和平移）进行数学建模，以及在Simulink环境中构建相应的控制系统模型。以下是基本的原理和流程：

1 原理：
SE(3)表示3维欧几里得空间加上所有刚体变换（旋转和平移），是描述机器人在空间自由度运动的理想框架。
对于四旋翼无人机，需要建立一个包含姿态角（yaw, pitch, roll）、位置(x, y, z)和速度（vx, vy, vz）等状态的模型。控制目标可能是保持稳定的轨迹、精确的定位等。
几何控制利用了四元数或其他数学工具来有效地处理姿态变化，同时考虑到全局定位信息。

2 流程：
（1）模型定义：使用Simulink创建一个模块化的模型，包括传感器模块（接收IMU数据），动力学模块（基于电机动力学计算转矩），以及姿态和位置更新模块（使用卡尔曼滤波或其他算法整合来自各种传感器的数据）。
（2）SE(3)转换：将姿态数据转换成四旋翼相对于世界坐标的矩阵表示（如旋转矩阵和平移向量）。
（3）控制律设计：设计几何控制律，比如基于四旋翼动力学的PID控制或更复杂的控制策略，比如差分进化、模型预测控制等，来生成对应每个状态的控制输入。
（4）模型仿真：在Simulink中连接各个模块并模拟环境交互，观察和调整控制效果。
迭代改进：根据仿真结果调整控制参数，分析系统动态响应，直至满足设计需求。

⛄二、部分源代码和运行步骤

1 部分代码
function out = untitled(u,P)
% inputs
f = u(1);
M = reshape(u(2:4),3,1);
% states
p_m = reshape(u(5:7),3,1);
v_m = reshape(u(8:10),3,1);
R_m = reshape(u(11:19),3,3);
q_m = reshape(rotm2quat(R_m),4,1);
Omega_m = reshape(u(20:22),3,1);
t = u(23);

persistent ekf;
if isempty(ekf)
% 初始状态和协方差
initialState = ones(17,1)*1e-1;
initialState(7:10)=[1;0;0;0];
initialCovariance = ones(17)*1e-1+eye(17)*1;
processNoise=eye(17)*1e-2;
measurementNoise=eye(13)*1e-2;

% 创建trackingEKF对象
ekf = trackingEKF('StateTransitionFcn',@stateTrans