用Python和Simulink从零搭建四旋翼动力学模型（附完整代码与避坑指南）

用Python和Simulink从零搭建四旋翼动力学模型（附完整代码与避坑指南）

四旋翼飞行器的建模与仿真一直是机器人学和无人机控制领域的热门课题。对于刚接触这个领域的学生和工程师来说，最大的挑战往往不是理解理论公式，而是如何将这些数学表达式转化为可运行的代码和仿真模型。本文将带你从零开始，用Python和Simulink两种工具实现四旋翼的完整动力学模型，并分享在实际编码过程中容易踩的坑和解决方案。

1. 四旋翼建模基础与环境准备

在开始编码之前，我们需要明确几个关键概念。四旋翼的动力学模型主要包含两个部分：平动动力学（描述飞行器在空间中的位置变化）和转动动力学（描述飞行器的姿态变化）。这两种运动通过欧拉角和旋转矩阵相互关联。

1.1 必备工具与库

要完成这个项目，你需要准备以下工具：

• Python环境（推荐Anaconda）：

  conda create -n quadrotor python=3.8 conda activate quadrotor pip install numpy scipy matplotlib control

• MATLAB/Simulink（2020b或更高版本）

• 可选工具：

  • Jupyter Notebook（用于交互式开发）
  • Git（版本控制）

1.2 四旋翼基本参数设定

我们先定义一个标准的四旋翼参数结构体，这些参数将贯穿整个建模过程：

class QuadrotorParams: def __init__(self): # 物理参数 self.mass = 1.0 # 质量 (kg) self.g = 9.81 # 重力加速度 (m/s^2) # 几何参数 self.arm_length = 0.25 # 旋翼到中心的距离 (m) # 惯性参数 self.Ixx = 0.01 # x轴转动惯量 (kg·m^2) self.Iyy = 0.01 # y轴转动惯量 self.Izz = 0.02 # z轴转动惯量 # 空气动力学参数 self.k_drag = 0.1 # 空气阻力系数 self.k_thrust = 1.0 # 升力系数 self.k_torque = 0.05 # 扭矩系数

2. 坐标系转换与姿态表示

四旋翼建模中最容易出错的部分就是坐标系转换。我们需要在三个坐标系之间进行转换：惯性坐标系（NED）、机体坐标系和欧拉角表示。

2.1 旋转矩阵实现

旋转矩阵是连接不同坐标系的关键。以下是Python实现：

import numpy as np def rotation_matrix(phi, theta, psi): """计算从机体坐标系到惯性坐标系的旋转矩阵""" R_x = np.array([[1, 0, 0], [0, np.cos(phi), np.sin(phi)], [0, -np.sin(phi), np.cos(phi)]]) R_y = np.array([[np.cos(theta), 0, -np.sin(theta)], [0, 1, 0], [np.sin(theta), 0, np.cos(theta)]]) R_z = np.array([[np.cos(psi), np.sin(psi), 0], [-np.sin(psi), np.cos(psi), 0], [0, 0, 1]]) return R_z @ R_y @ R_x # 注意矩阵乘法顺序

常见错误：

1. 旋转顺序错误（应该是Z-Y-X）
2. 角度单位混淆（确保使用弧度制）
3. 矩阵乘法顺序错误（从右向左应用旋转）

2.2 欧拉角与角速度转换

机体角速度(p,q,r)与欧拉角变化率(φ',θ',ψ')之间的转换关系：

def euler_derivatives(phi, theta, omega_body): """将机体角速度转换为欧拉角变化率""" p, q, r = omega_body transform = np.array([ [1, np.sin(phi)*np.tan(theta), np.cos(phi)*np.tan(theta)], [0, np.cos(phi), -np.sin(phi)], [0, np.sin(phi)/np.cos(theta), np.cos(phi)/np.cos(theta)] ]) return transform @ omega_body

注意：当θ接近±90°时会出现万向节锁问题，此时tan(θ)会趋向无穷大。在实际飞行控制中，通常会使用四元数来避免这个问题。

3. 动力学方程实现

3.1 平动动力学实现

根据牛顿第二定律，我们可以实现平动动力学方程：

def translational_dynamics(params, state, inputs): """计算平动加速度""" phi, theta, psi = state[6:9] # 欧拉角 T = inputs[0] # 总升力 # 旋转矩阵转置（从机体到惯性系） R = rotation_matrix(phi, theta, psi).T # 重力向量 gravity = np.array([0, 0, params.mass * params.g]) # 升力向量（在惯性系中） thrust = R @ np.array([0, 0, T]) # 空气阻力 velocity = state[3:6] drag = -params.k_drag * velocity # 总加速度 acceleration = (thrust + gravity + drag) / params.mass return acceleration

3.2 转动动力学实现

转动动力学更为复杂，需要考虑陀螺效应和交叉耦合：

def rotational_dynamics(params, state, inputs): """计算角加速度""" omega = state[9:12] # 机体角速度[p,q,r] tau = inputs[1:4] # 控制力矩[τ_x, τ_y, τ_z] # 惯性矩阵 I = np.diag([params.Ixx, params.Iyy, params.Izz]) # 陀螺力矩 gyro = np.cross(omega, I @ omega) # 角加速度 omega_dot = np.linalg.inv(I) @ (tau - gyro) return omega_dot

关键点：

1. 注意惯性矩阵是对角矩阵的假设
2. 陀螺力矩项不能遗漏
3. 力矩输入需要考虑电机配置（X型或+型）

4. 完整系统仿真

4.1 Python实现

我们可以使用SciPy的ODE求解器来进行系统仿真：

from scipy.integrate import solve_ivp def quadrotor_dynamics(t, state, params, controller): """完整的四旋翼动力学方程""" # 解析状态变量 position = state[0:3] velocity = state[3:6] euler = state[6:9] omega = state[9:12] # 获取控制输入（来自控制器） inputs = controller(t, state) # 计算导数 pos_dot = velocity vel_dot = translational_dynamics(params, state, inputs) euler_dot = euler_derivatives(euler[0], euler[1], omega) omega_dot = rotational_dynamics(params, state, inputs) return np.concatenate([pos_dot, vel_dot, euler_dot, omega_dot]) # 仿真参数 t_span = (0, 10) initial_state = np.zeros(12) # 初始状态 initial_state[2] = 5.0 # 初始高度5米 # 运行仿真 sol = solve_ivp(quadrotor_dynamics, t_span, initial_state, args=(params, simple_controller), dense_output=True)

4.2 Simulink实现

对于Simulink用户，可以按照以下步骤搭建模型：

1. 创建子系统实现旋转矩阵
2. 使用MATLAB Function块实现动力学方程
3. 配置ODE求解器为ode45（与Python一致）
4. 添加Scope模块可视化结果

Simulink建模技巧：

• 使用Bus Signal组织复杂信号
• 添加Saturation块限制电机输入
• 使用Memory块避免代数环

5. 常见问题与调试技巧

在实际实现过程中，你可能会遇到以下问题：

5.1 数值不稳定

症状：仿真很快发散，数值变为NaN或无穷大

解决方案：

1. 减小仿真步长
2. 检查动力学方程中的符号是否正确
3. 添加小的阻尼项

5.2 姿态表示奇异

症状：当俯仰角接近90°时，仿真崩溃

解决方案：

1. 改用四元数表示姿态
2. 限制控制器输出的期望姿态角

5.3 单位不一致

症状：仿真结果与预期不符，但方程看起来正确

解决方案：

1. 统一使用国际单位制（米、千克、秒）
2. 检查所有参数的物理单位
3. 特别留意角度单位（弧度vs度数）

6. 模型验证与PID控制器设计

为了验证我们的模型是否正确，可以设计一个简单的PID控制器进行测试：

class SimplePIDController: def __init__(self, params): self.params = params # PID参数 self.kp_z = 10.0 self.kd_z = 5.0 self.target_z = 2.0 # 目标高度 def __call__(self, t, state): # 高度控制 z = state[2] z_dot = state[5] error_z = self.target_z - z T = self.params.mass * self.params.g + self.kp_z * error_z - self.kd_z * z_dot # 简单姿态稳定 tau_x = -0.1 * state[6] - 0.05 * state[9] # 滚转角 tau_y = -0.1 * state[7] - 0.05 * state[10] # 俯仰角 tau_z = -0.1 * state[8] - 0.05 * state[11] # 偏航角 return np.array([T, tau_x, tau_y, tau_z])

控制器调试步骤：

1. 先调高度控制器（仅Kp）
2. 加入微分项（Kd）抑制振荡
3. 最后调姿态控制器
4. 使用MATLAB的pidTuner工具辅助参数整定

7. 进阶扩展方向

完成基础模型后，你可以考虑以下扩展：

1. 加入电机动力学：用一阶或二阶系统模拟电机响应
2. 环境扰动：添加风扰模型
3. 参数辨识：通过实验数据辨识实际无人机参数
4. 状态估计：实现基于IMU数据的卡尔曼滤波器
5. 非线性控制：尝试滑模控制或反馈线性化

四旋翼建模是一个复杂但极具教育意义的项目，通过这个练习，你不仅能够深入理解无人机动力学，还能掌握将数学方程转化为实际代码的关键技能。在实际项目中，建议从简单模型开始，逐步增加复杂度，并始终保持模型验证的习惯。