6-DOF模型全景概览——从刚体动力学到仿真系统

1. 摘要 (Abstract)

作为6-DOF仿真技术讲解的入门。我们将首先定义什么是6自由度模型，介绍其在航空航天领域的核心地位。同时我们也将建立仿真系统的宏观架构，明确各模块（动力学、运动学、环境、控制）之间的数据流关系。最后，将实现一个“最小可用”的自由落体与自旋仿真Demo，作为后续复杂模型的验证基线（Baseline）。

---

2. 应用场景与重要性 (Applications)

6-DOF模型描述了刚体在三维空间中同时具有3个平移自由度（沿X/Y/Z轴的移动）和3个旋转自由度（绕X/Y/Z轴的转动）。

在飞机与导弹场景中，6-DOF仿真是：

1. 飞控律设计（Control Law Design）：验证自动驾驶仪（Autopilot）在不同飞行包线内的稳定性。

2. 制导算法验证（Guidance Law）：测试比例导引（PN）或其他先进制导律对机动目标的拦截能力。

3. 硬件在环仿真（HIL）：在实际控制器接入前，通过软件模拟物理世界。

4. 数字孪生：构建高保真的虚拟样机，降低试飞成本与风险。

相比于开源飞控仿真（如PX4 SITL或AirSim，它们通常封装了底层动力学），手写6-DOF模型能让我们完全掌控气动参数、质量特性及环境模型，这对于导弹这类非标准气动布局的仿真至关重要。

---

3. 理论框架：6-DOF系统构成

3.1 状态变量定义

一个完整的6-DOF刚体状态由12个变量组成，分为两组：

1. 位置与速度（惯性系）：

2. 姿态与角速度（体轴系）：

3.2 动力学方程组

6-DOF的核心微分方程组如下：

平移动力学（牛顿第二定律）：

此处采用体轴系下的速度导数形式，便于引入科里奥利力项。

旋转动力学（欧拉方程）：

其中 I为惯性张量。

运动学方程：

4. 仿真系统架构设计

为了将理论转化为代码，我们需要设计一个清晰的软件架构。下图展示了本系列将实现的仿真系统数据流：

架构解读：

1. 闭环迭代：仿真内核是一个典型的闭环系统。RK4积分器根据当前的力和力矩更新状态向量。

2. 模块化：动力学模型（计算力/力矩）与运动学模型（更新位置/姿态）解耦，便于后期替换不同的气动模型（如从飞机切换到导弹）。

3. 接口清晰：控制输入和环境参数作为独立模块输入，方便后续接入PID控制器或大气紊流模型。

---

5. 最小Demo：自由落体与自旋

为了验证我们的仿真框架，我们先不考虑复杂的气动力，仅考虑重力和初始角速度的影响。

5.1 物理设定

5.2 Python 实现（核心代码）

我们将使用dataclasses管理状态，使用numpy进行矩阵运算。

import numpy as np from dataclasses import dataclass from typing import Tuple import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # ---------------------------------------------------- # 1. 数据结构定义 # ---------------------------------------------------- @dataclass class State: """6-DOF 状态向量""" pos: np.ndarray # [x, y, z] 位置 (NED) vel: np.ndarray # [u, v, w] 速度 (Body) quat: np.ndarray # [q0, q1, q2, q3] 四元数 omega: np.ndarray# [p, q, r] 角速度 (Body) def normalize_quaternion(q: np.ndarray) -> np.ndarray: """四元数归一化""" return q / np.linalg.norm(q) # ---------------------------------------------------- # 2. 动力学与运动学方程 # ---------------------------------------------------- def derivatives(state: State, mass: float, gravity: float) -> Tuple: """ 计算状态导数 忽略气动力，仅考虑重力 """ # 提取状态 pos, vel, quat, omega = state.pos, state.vel, state.quat, state.omega # 1. 位置导数 (NED坐标系) # 四元数旋转矩阵 C_b^n (Body to NED) # 简化版：这里我们只关心重力导致的下落，速度转换稍后完善 # 实际上 dp/dt = C_b^n @ vel # 为了Demo简单，我们暂时直接用速度积分（忽略姿态对速度的复杂影响，仅用于演示架构） # 2. 速度导数 (体轴系) # F = m*a => a = F/m # 重力在体轴系的投影 (简化：假设重力始终沿NED的-z方向) # 此处为最小Demo，设加速度为 [0,0,g] 在NED系，转换到体轴系需要旋转矩阵 # 极度简化：假设物体初始水平，重力只影响垂直速度 vel_dot = np.array([0, 0, gravity]) # 3. 四元数导数 # q_dot = 0.5 * Omega * q # Omega matrix from omega vector p, q, r = omega Omega = np.array([ [0, -p, -q, -r], [p, 0, r, -q], [q, -r, 0, p], [r, q, -p, 0] ]) quat_dot = 0.5 * Omega @ quat # 4. 角速度导数 (忽略惯性积和力矩) omega_dot = np.zeros(3) return vel, vel_dot, quat_dot, omega_dot # ---------------------------------------------------- # 3. 数值积分 (RK4) # ---------------------------------------------------- def rk4_step(state: State, dt: float, mass: float, gravity: float) -> State: """四阶龙格库塔积分一步""" k1_vel, k1_vel_d, k1_q, k1_w = derivatives(state, mass, gravity) s2 = State(state.pos + 0.5*dt*k1_vel, state.vel + 0.5*dt*k1_vel_d, normalize_quaternion(state.quat + 0.5*dt*k1_q), state.omega + 0.5*dt*k1_w) k2_vel, k2_vel_d, k2_q, k2_w = derivatives(s2, mass, gravity) s3 = State(state.pos + 0.5*dt*k2_vel, state.vel + 0.5*dt*k2_vel_d, normalize_quaternion(state.quat + 0.5*dt*k2_q), state.omega + 0.5*dt*k2_w) k3_vel, k3_vel_d, k3_q, k3_w = derivatives(s3, mass, gravity) s4 = State(state.pos + dt*k3_vel, state.vel + dt*k3_vel_d, normalize_quaternion(state.quat + dt*k3_q), state.omega + dt*k3_w) k4_vel, k4_vel_d, k4_q, k4_w = derivatives(s4, mass, gravity) # 更新状态 new_pos = state.pos + (dt/6.0)*(k1_vel + 2*k2_vel + 2*k3_vel + k4_vel) new_vel = state.vel + (dt/6.0)*(k1_vel_d + 2*k2_vel_d + 2*k3_vel_d + k4_vel_d) new_quat = normalize_quaternion(state.quat + (dt/6.0)*(k1_q + 2*k2_q + 2*k3_q + k4_q)) new_omega = state.omega + (dt/6.0)*(k1_w + 2*k2_w + 2*k3_w + k4_w) return State(new_pos, new_vel, new_quat, new_omega) # ---------------------------------------------------- # 4. 仿真主循环 # ---------------------------------------------------- def run_simulation(): # 初始化 state = State( pos=np.array([0.0, 0.0, 0.0]), vel=np.array([0.0, 0.0, 0.0]), quat=np.array([1.0, 0.0, 0.0, 0.0]), # w, x, y, z omega=np.array([0.0, 1.0, 0.0]) # 绕Y轴旋转 ) dt = 0.01 steps = 1000 history = {'pos': [], 'quat': []} for _ in range(steps): history['pos'].append(state.pos.copy()) history['quat'].append(state.quat.copy()) state = rk4_step(state, dt, mass=1.0, gravity=9.81) return history if __name__ == "__main__": hist = run_simulation() # ---------------------------------------------------- # 5. 可视化 # ---------------------------------------------------- fig = plt.figure(figsize=(12, 5)) # 子图1: 轨迹 (X-Z平面) ax1 = fig.add_subplot(121, projection='3d') positions = np.array(hist['pos']) ax1.plot(positions[:, 0], positions[:, 1], -positions[:, 2], label='Trajectory') # Z轴取反因为NED向下为正 ax1.set_xlabel('North (X)') ax1.set_ylabel('East (Y)') ax1.set_zlabel('Down (Z)') ax1.set_title('3D Trajectory (Free Fall)') ax1.legend() # 子图2: 姿态变化 (四元数) ax2 = fig.add_subplot(122) quats = np.array(hist['quat']) ax2.plot(quats[:, 0], label='q0 (w)') ax2.plot(quats[:, 1], label='q1 (x)') ax2.plot(quats[:, 2], label='q2 (y)') ax2.plot(quats[:, 3], label='q3 (z)') ax2.set_title('Quaternion Evolution (Spinning)') ax2.set_xlabel('Time Step') ax2.set_ylabel('Value') ax2.legend() ax2.grid(True) plt.tight_layout() plt.show()

5.3 结果分析

运行上述代码，我们将得到如下现象：

1. 轨迹图（左）：物体沿垂直方向（Z轴，图中显示为Down）加速下落，同时由于初始位置在原点，水平位移几乎为零。这验证了重力模型的正确性。

2. 四元数图（右）：四元数的分量随时间呈周期性变化。由于我们设置了绕体轴Y轴的角速度 (q=1.0)，物体处于稳态自旋状态。四元数的模长始终保持为1（得益于归一化处理），证明了数值积分的稳定性。

---

6. 总结与展望 (Conclusion)

本文构建了6-DOF仿真系统的顶层认知。我们定义了核心状态变量，梳理了动力学/运动学方程，并设计了一个模块化的仿真架构。通过“自由落体+自旋”的最小Demo，我们验证了数值积分器（RK4）和四元数更新的基本逻辑。

然而，目前的模型还非常粗糙：

1. 缺少气动力：真实的飞机/导弹受到升力、阻力和力矩的显著影响。

2. 坐标变换不完整：我们在速度更新中做了过度简化，未严格处理体轴系与惯性系之间的转换。

3. 无控制输入：无法模拟舵面偏转带来的动态响应。

在下一篇《坐标系与运动学》中，我们将重点攻克坐标变换这一难点，深入讲解旋转矩阵与四元数的物理意义，并修正本篇Demo中的运动学缺陷，为引入气动模型打下坚实基础。