第三章：仿真环境搭建——Python + NumPy + Matplotlib 搭建简易六自由度仿真器
做飞控调参，最怕什么？
怕火箭还没飞起来就炸了。怕调试一次成本几百万。怕改个参数要等三天才能试一次。
所以我个人的习惯是——先仿真，再实飞。这不是偷懒，这是对自己和项目负责。
这一章，我们就来搭一个简易的六自由度仿真器。说白了，就是用 Python 模拟火箭在空中的姿态变化。你不需要买任何硬件，只需要一台电脑，装上 Python 和几个库，就能开始调 PID 了。
3.1 为什么选 Python？
你可能想问：为什么不用 MATLAB？
嗯，MATLAB 确实强大，但它的授权费不便宜。而且我个人觉得，Python 的生态更适合快速迭代。NumPy 做矩阵运算，Matplotlib 画图，配合起来非常顺手。
我在项目中遇到过好几次这样的情况：现场调试时发现某个参数不对劲，掏出笔记本，打开 Python 脚本，改两行代码，跑一下仿真，结果就出来了。整个过程不超过五分钟。这种效率，用 MATLAB 很难做到。
3.2 六自由度仿真器的核心逻辑
六自由度，听起来很唬人。其实说白了，就是描述火箭在三维空间里的运动——三个平动（前后、左右、上下）和三个转动（俯仰、偏航、滚转）。
我们不需要把每个细节都算得特别精确。初期仿真，重点是姿态控制，也就是转动部分。平动可以先简化。
下面这张图，是我自己画的仿真器核心逻辑框架：
你看，这个闭环其实很简单：期望姿态 → PID 控制器 → 执行器 → 刚体动力学 → 传感器 → 反馈给 PID。你调参，就是在 PID 控制器那个框里改三个数。
3.3 代码实现：从零开始搭框架
好，我们直接上代码。我会把每一步拆开讲，你跟着敲一遍就能跑起来。
3.3.1 导入库和初始化参数
import numpy as np
import matplotlib.pyplot as plt
# 仿真参数
dt = 0.01 # 时间步长，单位秒
total_time = 10.0 # 仿真总时长
steps = int(total_time / dt)
# 火箭物理参数（简化版）
Ixx = 1.0 # 绕X轴转动惯量
Iyy = 1.5 # 绕Y轴转动惯量
Izz = 0.8 # 绕Z轴转动惯量
# 初始姿态（欧拉角：滚转、俯仰、偏航）
phi0 = 0.0 # 滚转
theta0 = 0.2 # 俯仰（初始有个小偏差）
psi0 = 0.0 # 偏航
# 期望姿态
phi_des = 0.0
theta_des = 0.0
psi_des = 0.0这里我故意给俯仰角设了个 0.2 弧度的初始偏差。为什么？因为真实火箭发射时不可能绝对垂直，总有点小扰动。你想想看，如果仿真从完美状态开始，那 PID 调出来也没用——实战中一有扰动就露馅了。
3.3.2 PID 控制器类
class PIDController:
def __init__(self, kp, ki, kd, dt):
self.kp = kp
self.ki = ki
self.kd = kd
self.dt = dt
self.integral = 0.0
self.prev_error = 0.0
def update(self, error):
self.integral += error * self.dt
derivative = (error - self.prev_error) / self.dt
output = self.kp * error + self.ki * self.integral + self.kd * derivative
self.prev_error = error
return output这个类很简单，但够用。我刚开始做飞控时，也想过用更复杂的 PID 变体，比如积分分离、抗饱和之类的。后来发现，先把基础 PID 调明白，比什么都强。那些高级技巧，等基础调通了再锦上添花。
3.3.3 刚体动力学更新
def update_attitude(phi, theta, psi, p, q, r, tau_x, tau_y, tau_z, dt):
"""
简化版刚体姿态动力学
输入：当前姿态角、角速度、控制力矩
输出：更新后的姿态角和角速度
"""
# 角加速度（欧拉方程简化）
p_dot = (tau_x - (Izz - Iyy) * q * r) / Ixx
q_dot = (tau_y - (Ixx - Izz) * p * r) / Iyy
r_dot = (tau_z - (Iyy - Ixx) * p * q) / Izz
# 更新角速度
p += p_dot * dt
q += q_dot * dt
r += r_dot * dt
# 运动学方程（欧拉角速率）
phi_dot = p + (q * np.sin(phi) + r * np.cos(phi)) * np.tan(theta)
theta_dot = q * np.cos(phi) - r * np.sin(phi)
psi_dot = (q * np.sin(phi) + r * np.cos(phi)) / np.cos(theta)
# 更新姿态角
phi += phi_dot * dt
theta += theta_dot * dt
psi += psi_dot * dt
return phi, theta, psi, p, q, r3.3.4 主仿真循环
# 初始化
phi, theta, psi = phi0, theta0, psi0
p, q, r = 0.0, 0.0, 0.0
# 创建三个轴的 PID 控制器
pid_roll = PIDController(kp=2.0, ki=0.5, kd=0.1, dt=dt)
pid_pitch = PIDController(kp=2.0, ki=0.5, kd=0.1, dt=dt)
pid_yaw = PIDController(kp=1.5, ki=0.3, kd=0.05, dt=dt)
# 记录数据
time_hist = np.arange(0, total_time, dt)
phi_hist = np.zeros(steps)
theta_hist = np.zeros(steps)
psi_hist = np.zeros(steps)
# 仿真循环
for i in range(steps):
# 计算误差
error_roll = phi_des - phi
error_pitch = theta_des - theta
error_yaw = psi_des - psi
# PID 输出（控制力矩）
tau_x = pid_roll.update(error_roll)
tau_y = pid_pitch.update(error_pitch)
tau_z = pid_yaw.update(error_yaw)
# 更新姿态
phi, theta, psi, p, q, r = update_attitude(
phi, theta, psi, p, q, r, tau_x, tau_y, tau_z, dt
)
# 记录
phi_hist[i] = phi
theta_hist[i] = theta
psi_hist[i] = psi3.4 结果可视化
仿真跑完了，光看数字没用。得画出来，一眼就能看出 PID 调得好不好。
plt.figure(figsize=(12, 8))
plt.subplot(3, 1, 1)
plt.plot(time_hist, phi_hist * 180 / np.pi, label='滚转')
plt.ylabel('角度 (deg)')
plt.legend()
plt.grid(True)
plt.subplot(3, 1, 2)
plt.plot(time_hist, theta_hist * 180 / np.pi, label='俯仰', color='orange')
plt.ylabel('角度 (deg)')
plt.legend()
plt.grid(True)
plt.subplot(3, 1, 3)
plt.plot(time_hist, psi_hist * 180 / np.pi, label='偏航', color='green')
plt.xlabel('时间 (s)')
plt.ylabel('角度 (deg)')
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()运行这段代码，你会看到三条曲线。如果 PID 参数合适，它们应该在大约 2-3 秒内收敛到 0。如果出现震荡，说明 P 太大；如果收敛太慢，说明 P 太小；如果有稳态误差，说明 I 不够。
3.5 仿真器的局限性
说实话，这个仿真器很简陋。它没有考虑空气动力学、弹性振动、执行器饱和、传感器噪声等等。但它的价值在于——让你在 10 分钟内跑通一个闭环，快速验证 PID 参数是否合理。
我在实际项目中，经常用这种简易仿真器做初步筛选。把明显不行的参数排除掉，再用高精度仿真器做精细调优。这样效率最高。
3.6 本章小结
我们搭了一个能跑的六自由度仿真器。虽然简单，但五脏俱全——有 PID 控制器、有刚体动力学、有反馈回路。你可以用它来测试不同的 PID 参数，观察姿态响应。
下一章，我们会在这个仿真器的基础上，系统地讲解 PID 调参方法。到时候，你会看到同样的代码，不同的参数，结果天差地别。
嗯，先把这个仿真器跑通再说。有问题随时找我。