1. 摘要 (Abstract)一个健壮的6-DOF仿真器不仅仅是公式的堆砌更是软件架构的艺术。本文将基于Python构建一个面向对象的SixDOFSimulator类。我们将定义严格的状态向量State Vector结构采用dataclasses提升代码可读性并利用scipy.integrate.solve_ivp替换手动编写的RK4循环以获得更好的数值稳定性和事件处理能力。本文最终实现了一个“无动力滑翔”的基准Demo用于验证系统集成是否正确。2. 架构设计从脚本到系统在编写代码前我们需要明确仿真器的核心组件及其职责。2.1 核心组件划分仿真器的UML类图。仿真器持有状态并调用飞机模型和大气模型来计算导数。2.2 数据流标准控制循环我们将采用标准的连续-离散混合架构连续域物理世界微分方程。离散域计算机仿真数值积分。接口在每个时间步积分器询问“在当前状态下导数x˙是多少”仿真器调用物理模型计算并返回。3. 代码实现构建仿真内核3.1 状态向量封装 (State Class)状态是仿真器的“灵魂”。我们将12个核心变量封装在一个类中并提供与NumPy数组互转的方法这是为了适配solve_ivp的输入输出格式。# sixdof/state.py import numpy as np from dataclasses import dataclass dataclass class State: 6-DOF 状态向量封装 位置/速度在NED惯性系角速度在体轴系 # Position (NED) [m] pn: float 0.0 pe: float 0.0 pd: float 0.0 # Velocity (Body) [m/s] u: float 0.0 v: float 0.0 w: float 0.0 # Attitude (Quaternion) [unitless] q0: float 1.0 q1: float 0.0 q2: float 0.0 q3: float 0.0 # Angular Rates (Body) [rad/s] p: float 0.0 q: float 0.0 r: float 0.0 # Acceleration (Body) [m/s^2] - Derived, for logging ax: float 0.0 ay: float 0.0 az: float 0.0 def to_array(self) - np.ndarray: 将状态转换为NumPy数组 (用于积分器) return np.array([ self.pn, self.pe, self.pd, self.u, self.v, self.w, self.q0, self.q1, self.q2, self.q3, self.p, self.q, self.r ]) staticmethod def from_array(arr: np.ndarray) - State: 从NumPy数组恢复状态 return State(*arr[:13]) def normalize_quaternion(self): 四元数归一化 norm np.sqrt(self.q0**2 self.q1**2 self.q2**2 self.q3**2) if norm 1e-16: self.q0 1.0 self.q1 self.q2 self.q3 0.0 else: self.q0 / norm self.q1 / norm self.q2 / norm self.q3 / norm def rotation_matrix(self) - np.ndarray: 返回 C_b^n (Body to NED) q0, q1, q2, q3 self.q0, self.q1, self.q2, self.q3 return np.array([ [q0**2q1**2-q2**2-q3**2, 2*(q1*q2 q0*q3), 2*(q1*q3 - q0*q2)], [2*(q1*q2 - q0*q3), q0**2-q1**2q2**2-q3**2, 2*(q2*q3 q0*q1)], [2*(q1*q3 q0*q2), 2*(q2*q3 - q0*q1), q0**2-q1**2-q2**2q3**2] ])3.2 飞机模型封装 (Aircraft Class)飞机模型负责所有物理参数的存储和力/力矩的计算。# sixdof/aircraft.py import numpy as np class Aircraft: def __init__(self, mass, inertia, S_ref): self.mass mass self.inertia np.array(inertia) # [Ixx, Iyy, Izz] self.S_ref S_ref # 气动导数 (简化模型) self.CLA 5.5 # Lift slope self.CD0 0.02 # Zero-lift drag self.CDA 0.3 # Induced drag factor self.Cm0 0.01 # Zero-lift pitching moment self.Cma -1.2 # Static stability self.Cmq -25.0 # Pitch damping self.Cm_de -1.0 # Elevator effectiveness def get_forces_and_moments(self, state: State, control: np.ndarray, rho: float) - tuple: 计算气动力和力矩 control: [de, da, dr, throttle] # --- Kinematics --- V np.sqrt(state.u**2 state.v**2 state.w**2) if V 0.1: return np.zeros(3), np.zeros(3) alpha np.arctan2(state.w, state.u) beta np.arcsin(state.v / V) q_bar 0.5 * rho * V**2 de control[0] # Elevator deflection # --- Aerodynamics --- # Lift Drag CL self.CLA * alpha CD self.CD0 self.CDA * CL**2 X_aero -CD * q_bar * self.S_ref Z_aero -CL * q_bar * self.S_ref # Pitching Moment mac 1.5 # Mean Aerodynamic Chord Cm (self.Cm0 self.Cma * alpha self.Cmq * state.q * mac / (2 * V) self.Cm_de * de) M Cm * q_bar * self.S_ref * mac forces_b np.array([X_aero, 0.0, Z_aero]) moments_b np.array([0.0, M, 0.0]) return forces_b, moments_b3.3 仿真器核心 (SixDOFSimulator Class)这是最重要的部分。我们将使用solve_ivp来管理时间推进并将导数计算逻辑集中在此。# sixdof/simulator.py import numpy as np from scipy.integrate import solve_ivp from typing import Callable, List from .state import State from .aircraft import Aircraft class SixDOFSimulator: def __init__(self, aircraft: Aircraft, initial_state: State): self.aircraft aircraft self.state initial_state self.time 0.0 # History for logging self.history: List[State] [initial_state] self.time_history: List[float] [0.0] # Control input function (can be set externally) self.control_func: Callable[[float], np.ndarray] lambda t: np.zeros(4) # Atmosphere model (simplified) self.rho0 1.225 self.H 8500.0 def density(self, altitude): Exponential atmosphere model return self.rho0 * np.exp(-altitude / self.H) def _derivatives(self, t: float, y: np.ndarray) - np.ndarray: 计算状态导数 dy/dt。这是solve_ivp调用的核心函数。 # 1. Update internal state from integrators array current_state State.from_array(y) current_state.normalize_quaternion() # 2. Get control inputs control self.control_func(t) # 3. Get environment properties rho self.density(-current_state.pd) # pd is negative altitude # 4. Calculate Forces and Moments forces_b, moments_b self.aircraft.get_forces_and_moments( current_state, control, rho ) # Add Gravity (in body frame) R_bn current_state.rotation_matrix().T # C_n^b gravity_force_n np.array([0, 0, self.aircraft.mass * 9.81]) gravity_force_b R_bn gravity_force_n forces_b gravity_force_b # 5. Kinematics (Position and Attitude) # Velocity in NED vel_n current_state.rotation_matrix() np.array([current_state.u, current_state.v, current_state.w]) pos_dot vel_n # Quaternion derivative p, q, r current_state.p, current_state.q, current_state.r Omega np.array([ [0, -p, -q, -r], [p, 0, r, -q], [q, -r, 0, p], [r, q, -p, 0] ]) quat_dot 0.5 * Omega np.array([current_state.q0, current_state.q1, current_state.q2, current_state.q3]) # 6. Dynamics (Velocity and Angular Rates) # Translational dynamics (Body frame) vel_dot_b forces_b / self.aircraft.mass - np.cross(np.array([p, q, r]), np.array([current_state.u, current_state.v, current_state.w])) # Rotational dynamics (Eulers equation) I np.diag(self.aircraft.inertia) inv_I np.linalg.inv(I) gyro_torque np.cross(np.array([p, q, r]), I np.array([p, q, r])) omega_dot_b inv_I (moments_b - gyro_torque) # 7. Assemble derivative vector return np.concatenate([pos_dot, vel_dot_b, quat_dot, omega_dot_b]) def step(self, dt: float): 执行一个仿真步长 (Wrapper for solve_ivp) t_span (self.time, self.time dt) y0 self.state.to_array() # Use RK45 (DOP853 is also excellent) sol solve_ivp(self._derivatives, t_span, y0, methodRK45, rtol1e-9, atol1e-12) # Update state self.state State.from_array(sol.y[:, -1]) self.state.normalize_quaternion() self.time sol.t[-1] # Log self.history.append(self.state) self.time_history.append(self.time) def run(self, t_final: float, dt: float 0.01): 运行仿真直到结束 while self.time t_final: step_dt min(dt, t_final - self.time) self.step(step_dt) def get_history_arrays(self) - dict: 提取历史数据为字典方便绘图 data { time: np.array(self.time_history), pn: [], pe: [], pd: [], u: [], v: [], w: [], alpha: [], beta: [], quat: [], omega: [] } for s in self.history: data[pn].append(s.pn); data[pe].append(s.pe); data[pd].append(s.pd) data[u].append(s.u); data[v].append(s.v); data[w].append(s.w) data[quat].append([s.q0, s.q1, s.q2, s.q3]) data[omega].append([s.p, s.q, s.r]) V np.sqrt(s.u**2 s.v**2 s.w**2) data[alpha].append(np.rad2deg(np.arctan2(s.w, s.u))) if V 0.1 else data[alpha].append(0) data[beta].append(np.rad2deg(np.arcsin(s.v / V))) if V 0.1 else data[beta].append(0) return {k: np.array(v) for k, v in data.items()}4. 仿真Demo无动力滑翔让我们测试这个仿真器。设定一个简单的场景飞机具有一定的初速度无动力无操纵面偏转观察其滑翔轨迹。# examples/steady_glide_test.py import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from sixdof.simulator import SixDOFSimulator from sixdof.aircraft import Aircraft from sixdof.state import State def main(): # 1. Initialize Aircraft aircraft Aircraft( mass10.0, inertia[0.2, 1.0, 1.0], # Ixx, Iyy, Izz S_ref0.5 ) # 2. Initial State init_state State( pn0.0, pe0.0, pd0.0, # Start at origin u30.0, v0.0, w-2.0, # Forward speed 30m/s, slight downward q01.0, # Level attitude p0.0, q0.0, r0.0 ) # 3. Initialize Simulator sim SixDOFSimulator(aircraft, init_state) # 4. Define Control (Zero control) sim.control_func lambda t: np.array([0.0, 0.0, 0.0, 0.0]) # [de, da, dr, throttle] # 5. Run Simulation print(Starting simulation...) sim.run(t_final20.0, dt0.02) # Run for 20 seconds print(Simulation finished.) # 6. Extract Data data sim.get_history_arrays() # 7. Visualization fig plt.figure(figsize(14, 6)) # Subplot 1: 3D Trajectory ax1 fig.add_subplot(121, projection3d) ax1.plot(data[pn], data[pe], -data[pd], labelGlide Path) ax1.set_xlabel(North (m)) ax1.set_ylabel(East (m)) ax1.set_zlabel(Altitude (m)) ax1.set_title(3D Glide Trajectory) ax1.legend() # Subplot 2: Flight Parameters ax2 fig.add_subplot(122) ax2.plot(data[time], data[alpha], labelAoA (deg)) ax2.plot(data[time], data[pd], labelAltitude (m)) ax2.axhline(0, colork, linestyle--, alpha0.3) ax2.set_xlabel(Time (s)) ax2.set_ylabel(Value) ax2.set_title(Flight Parameters Over Time) ax2.legend() ax2.grid(True) plt.tight_layout() plt.show() if __name__ __main__: main()4.1 结果分析运行上述代码你应该看到3D轨迹一条平滑向下的滑翔曲线。由于没有侧滑和转弯飞机沿直线前进。参数曲线高度持续下降符合重力作用。迎角AoA在初始阶段略有波动由于初始w的存在随后迅速收敛到一个稳定的正值。这证明我们的静稳定导数Cmα0生效了——飞机自动找到了升力平衡重力的迎角。5. 总结与展望 (Conclusion)本篇标志着我们的6-DOF仿真从“理论推导”正式迈入“工程实践”完成了架构搭建设计了State、Aircraft和SixDOFSimulator三个核心类实现了高内聚低耦合。标准化了积分接口利用solve_ivp接管时间推进大幅提升了数值鲁棒性并简化了主循环逻辑。验证了系统集成通过无动力滑翔Demo验证了动力学、运动学与气动模型的联合运作正常特别是静稳定性的体现。当前局限目前的仿真器虽然能飞但环境模型过于简单无风控制输入也是开环的。飞机无法按照预定航线飞行因为它还没有“大脑”控制器。
