2、仿真环境搭建：基于Python的简易四旋翼模型与可视化

说实话，很多初学者一上来就怼着PX4或者ArduPilot的源码啃，结果被一堆宏定义和回调函数搞得晕头转向。我个人习惯是，先搭一个自己能完全掌控的仿真环境，把核心逻辑跑通了再说。这一章，我们就用Python手撸一个简易的四旋翼模型，再配上实时可视化，让你亲眼看到PID调参的效果。

2.1 为什么选Python？

你想想看，做飞控仿真最怕什么？最怕改个参数要编译半小时，然后一运行就炸。Python的好处就是快——改完就能跑，跑完就能看。我在项目里做原型验证时，经常先用Python把算法跑通，再移植到C++里。说白了，Python就是你的“草稿纸”。

2.2 四旋翼的简易数学模型

嗯，这里要注意，我们不需要搞太复杂的空气动力学。对于PID调参来说，一个简化的刚体模型就够用了。核心就两个部分：动力学方程和运动学方程。

2.2.1 状态变量定义

我习惯用12维状态向量来描述四旋翼：

状态向量 x = [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]

其中：

• 位置：x, y, z（世界坐标系）
• 速度：vx, vy, vz（世界坐标系）
• 姿态角：phi（横滚）, theta（俯仰）, psi（偏航）
• 角速度：p, q, r（机体坐标系）

2.2.2 控制输入

四旋翼有4个电机，但控制输入通常抽象为4个通道：

通道	符号	说明
油门	u1	总升力，控制高度
横滚	u2	左右电机差速，控制横滚角
俯仰	u3	前后电机差速，控制俯仰角
偏航	u4	对角电机差速，控制偏航角

2.3 代码实现：核心仿真类

下面是我写的一个简化版四旋翼仿真类。代码不长，但五脏俱全。

import numpy as np

class QuadrotorSim:
    def __init__(self, dt=0.01):
        self.dt = dt  # 仿真步长
        self.g = 9.81  # 重力加速度
        self.m = 1.0   # 质量
        self.I = np.diag([0.01, 0.01, 0.02])  # 转动惯量
        
        # 状态初始化
        self.x = np.zeros(12)
        self.x[2] = 1.0  # 初始高度1米
        
    def dynamics(self, x, u):
        """四旋翼动力学方程"""
        # 解包状态
        pos = x[0:3]
        vel = x[3:6]
        euler = x[6:9]
        omega = x[9:12]
        
        # 旋转矩阵（从机体到世界）
        phi, theta, psi = euler
        R = self._euler_to_rot(phi, theta, psi)
        
        # 推力（世界坐标系）
        thrust_body = np.array([0, 0, u[0]])
        thrust_world = R @ thrust_body
        
        # 加速度
        acc = thrust_world / self.m - np.array([0, 0, self.g])
        
        # 姿态角速度转换
        euler_dot = self._omega_to_euler_dot(omega, euler)
        
        # 角加速度（简化版）
        omega_dot = np.linalg.inv(self.I) @ (u[1:4] - np.cross(omega, self.I @ omega))
        
        return np.concatenate([vel, acc, euler_dot, omega_dot])
    
    def step(self, u):
        """单步仿真（RK4积分）"""
        k1 = self.dynamics(self.x, u)
        k2 = self.dynamics(self.x + 0.5*self.dt*k1, u)
        k3 = self.dynamics(self.x + 0.5*self.dt*k2, u)
        k4 = self.dynamics(self.x + self.dt*k3, u)
        self.x += (self.dt/6) * (k1 + 2*k2 + 2*k3 + k4)
        return self.x
    
    def _euler_to_rot(self, phi, theta, psi):
        """欧拉角转旋转矩阵"""
        # 标准ZYX顺序
        cphi, sphi = np.cos(phi), np.sin(phi)
        ctheta, stheta = np.cos(theta), np.sin(theta)
        cpsi, spsi = np.cos(psi), np.sin(psi)
        
        R = np.array([
            [cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi],
            [spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi],
            [-stheta, ctheta*sphi, ctheta*cphi]
        ])
        return R
    
    def _omega_to_euler_dot(self, omega, euler):
        """角速度转欧拉角速率"""
        phi, theta = euler[0], euler[1]
        W = 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 W @ omega

2.4 可视化：让数据“活”起来

光有数据不够，你得看到飞机在飞。我习惯用Matplotlib做实时3D可视化，再加一个姿态指示器。

2.4.1 3D轨迹显示

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

class Visualizer:
    def __init__(self):
        self.fig = plt.figure(figsize=(12, 5))
        
        # 3D轨迹图
        self.ax3d = self.fig.add_subplot(121, projection='3d')
        self.ax3d.set_xlim([-2, 2])
        self.ax3d.set_ylim([-2, 2])
        self.ax3d.set_zlim([0, 3])
        self.ax3d.set_xlabel('X (m)')
        self.ax3d.set_ylabel('Y (m)')
        self.ax3d.set_zlabel('Z (m)')
        
        # 姿态指示器（2D）
        self.ax2d = self.fig.add_subplot(122)
        self.ax2d.set_xlim([-1, 1])
        self.ax2d.set_ylim([-1, 1])
        self.ax2d.set_aspect('equal')
        self.ax2d.set_title('姿态指示器')
        
        # 初始化绘图元素
        self.traj_line, = self.ax3d.plot([], [], [], 'b-', lw=1)
        self.quad_body, = self.ax3d.plot([], [], [], 'ro-', lw=2)
        
    def update(self, state, trajectory):
        """更新可视化"""
        # 更新轨迹
        if len(trajectory) > 1:
            traj = np.array(trajectory)
            self.traj_line.set_data(traj[:, 0], traj[:, 1])
            self.traj_line.set_3d_properties(traj[:, 2])
        
        # 更新四旋翼机体（简化：画一个十字）
        pos = state[0:3]
        self.quad_body.set_data([pos[0]], [pos[1]])
        self.quad_body.set_3d_properties([pos[2]])
        
        plt.pause(0.01)

2.4.2 运行主循环

# 主循环示例
sim = QuadrotorSim(dt=0.01)
vis = Visualizer()
trajectory = []

# 简单的悬停控制（后面章节会讲PID）
for t in range(1000):
    # 悬停：油门补偿重力
    u = [sim.m * sim.g, 0, 0, 0]
    state = sim.step(u)
    trajectory.append(state[0:3].copy())
    
    if t % 10 == 0:  # 每10步刷新一次
        vis.update(state, trajectory)

plt.show()

2.5 知识体系总览

下面这张图概括了本章的核心逻辑。你看，从数学模型到代码实现，再到可视化，是一条完整的链路。

2.6 一些实用建议

代码写完了，但有几个点我想特别提醒你：

• 仿真步长别太大：0.01秒是底线，再大数值就不稳了。我试过0.05秒，结果飞机直接飞出了银河系。
• 初始状态要合理：别让飞机从0高度开始，否则重力一拉就穿地了。给个1米初始高度比较安全。
• 可视化别每步都刷：每10步刷新一次就够了，否则Matplotlib会卡到你怀疑人生。

好了，仿真环境搭好了。下一章我们就要往里面塞PID控制器了。你想想看，当你的代码让这架虚拟飞机稳稳悬停在空中时，那种成就感——嗯，我到现在还记得第一次调通时的心情。