h100-control/PROJECT_PLAN.md

H100 水下无人航行器 6-DOF 仿真与运动控制项目规划

项目代号：h100-control 目标：构建 H100 型 AUV/ROV 的六自由度（6-DOF）运动仿真平台，并在此之上开发闭环运动控制器（深度保持、定点/定速、航迹跟踪等）。

---

1. 技术选型

项	选择	说明
语言	Python ≥ 3.10	开发迭代快，数值生态完善
数值计算	NumPy / SciPy	状态向量运算、线性代数
时间积分	SciPy scipy.integrate.ode（rk45/rk853 自适应步长，C 实现）	不自研积分器；Python 循环管控制周期，C 积分管周期之间
旋转	SciPy scipy.spatial.transform.Rotation	欧拉角/旋转矩阵/四元数互转、增量旋转组合，零手推公式
QP 求解	cvxpy + OSQP	推力分配带约束 QP（热启动，单步 µs–ms）；欠驱动基线用 NumPy 伪逆
经典控制	python-control	control.lqr 直接解 LQR 增益，兼做阶跃/Bode 频率响应分析
滤波	scipy.signal.lfilter / filterpy（预留）	传感器一阶延迟；EKF 导航仅在可选项阶段使用
参数标定	lmfit / scipy.optimize	M6 用海试数据拟合阻尼 D、附加质量 M_A、推力效率 η_t
配置	PyYAML + pydantic	参数文件化 + 模型级校验，免改代码
数据记录	pandas → CSV（可选 HDF5）	便于离线分析与对比
可视化	Matplotlib + plotly（可选交互 3D）	2D 曲线 + 交互式 3D 轨迹/姿态
测试	pytest	每层模块可独立单测
依赖管理	requirements.txt + pyproject.toml

原则：不重复造轮子 —— 旋转、ODE 积分、QP、LQR、滤波、参数标定都有成熟库（多为 C/Cython 后端）；项目代码只保留 H100 特有的物理组装、场景与控制逻辑。

后续若需实时性（>1 kHz）或联合仿真，可将 dynamics/ 层单独翻译为 C++ 并通过 pybind11 暴露，Python 接口保持不变，目录结构无需改动。

---

2. 目录结构

h100-control/
├── PROJECT_PLAN.md              # 本规划文档
├── README.md                    # 快速上手：安装、运行、看结果
├── requirements.txt
├── pyproject.toml
│
├── config/                      # ── 全部参数配置（YAML，改参数不改代码）
│   ├── vehicle.yaml             # 车体：质量、惯性张量、附加质量、重心/浮心坐标、浮力
│   ├── thrusters.yaml           # 推进器：推力型/泵喷、安装位置、安装方向、效率、推力上限
│   ├── controller.yaml          # 控制器：级联 PID 增益、采样/更新周期
│   ├── sensors.yaml             # 传感器：噪声标准差、一阶延迟、量程
│   └── scenarios/               # 仿真场景预设（每个文件定义一个工况）
│       ├── nominal.yaml         #   静水、无扰动，基准工况
│       ├── current.yaml         #   均匀洋流 / 分层洋流
│       ├── maneuver.yaml        #   S 形机动 / 正弦横航
│       └── disturbance.yaml     #   波浪/外推力扰动
│
├── src/
│   └── h100/                    # Python 包
│       ├── __init__.py
│       ├── dynamics/            # ── 动力学层（物理模型核心）
│       │   ├── kinematics.py    #   姿态运动学：Rotation.from_rotvec(ω·dt) 增量旋转，不手推四元数代数
│       │   ├── equations.py     #   6-DOF 刚体动力学方程（Newton-Euler 组装）
│       │   ├── hydrodynamics.py #   水动力：附加质量 C(ν)ν、线性+二次阻尼 D(ν)ν
│       │   ├── gravity.py       #   重力/浮力 g(η)：由重心/浮心几何计算
│       │   └── thruster.py      #   推进器数学模型 + 推力分配（基线 NumPy 伪逆；带限幅约束 QP → cvxpy/OSQP）
│       │
│       ├── control/             # ── 控制层
│       │   ├── base.py          #   ControllerBase 抽象接口：update(dt, state) -> v_des
│       │   ├── cascade_pid.py   #   级联 PID：外层位置/姿态 → 内层速度/角速度（基线）
│       │   ├── lqr.py           #   LQR（扩展：python-control 的 control.lqr + 增益调度）
│       │   └── backstepping.py  #   反步法（扩展：非线性鲁棒）
│       │
│       ├── sensors/             # ── 传感器层（含噪声与延迟，闭环真实性关键）
│       │   ├── base.py          #   SensorBase：numpy 噪声 + scipy.signal.lfilter 一阶延迟
│       │   ├── imu.py           #   陀螺/加速度计
│       │   ├── dvl.py           #   多普勒测速仪（体坐标速度）
│       │   ├── depth.py         #   压力深度计
│       │   └── ekf.py           #   EKF 导航（可选扩展：filterpy）
│       │
│       ├── environment/         # ── 环境层
│       │   ├── current.py       #   洋流：均匀流 / 随深度分层流
│       │   └── disturbance.py   #   外力/力矩扰动：正弦、阶跃、随机（海况模拟）
│       │
│       ├── sim/                 # ── 仿真引擎（总装与主循环，时间积分直接用 SciPy）
│       │   ├── world.py         #   World：车体 + 环境 + 传感器 + 控制器 装配，输出 ODE 右端函数
│       │   ├── recorder.py      #   时序记录：状态、控制量、误差 → CSV
│       │   └── sim.py           #   Simulation 主类：load_scenario() → 控制周期循环 + f.integrate() → run()
│       │
│       └── utils/               # ── 公共工具
│           ├── rotation.py      #   薄封装：scipy.spatial.transform.Rotation + NED 约定映射
│           └── config.py        #   pydantic 模型：YAML 加载、字段校验、默认值合并
│
├── scripts/                     # ── 各实验运行入口（薄封装，调用 sim 包）
│   ├── run_free_motion.py       #   自由运动/漂移：无控制，验证动力学正确性
│   ├── run_depth_hold.py        #   深度保持（单回路经典基准）
│   ├── run_velocity_hold.py     #   定速/定点悬停
│   ├── run_waypoint_nav.py      #   航点/三维轨迹跟踪任务
│   └── run_maneuver.py          #   机动性能：S 形、正弦横航
│
├── tests/                       # pytest
│   ├── test_rotation.py         #   旋转/四元数变换、正交性、对偶性
│   ├── test_dynamics.py         #   平衡点验证、能量耗散、零输入自由运动衰减
│   ├── test_thruster.py         #   推力分配：满推可行域、欠驱动约束
│   └── test_pid.py              #   单轴阶跃响应稳定性
│
├── results/                     # 仿真输出（.gitignore，自动生成）
│   └── <exp_name>/trajectory.csv, control.csv, summary.json
├── data/                        # 外部数据：参考轨迹、实船海试数据（对比用）
└── .gitignore

---

3. 总体架构

3.1 分层架构与数据流（每个仿真步 dt）

┌─────────────────────────────────────────────────────────────┐
│  Scenario（场景）                                            │
│  参考指令 r(t)：位置/速度/姿态   +   洋流/扰动参数            │
└──────────────┬──────────────────────────────────────────────┘
               │ 参考指令
               ▼
┌──────────────────────────┐
│  Controller 控制层        │  输入：传感器状态（含噪声/延迟）
│  级联 PID（基线）          │  输出：期望线速度/角速度 ν_d（体坐标）
└──────────────┬───────────┘
               │ ν_d
               ▼
┌──────────────────────────┐
│  Thruster 推力分配        │  ν_d → 期望合力/力矩 τ = B·u
│  伪逆 / QP（含推力限幅）   │  输出：各推进器推力 u
└──────────────┬───────────┘
               │ τ
               ▼
┌──────────────────────────┐
│  Dynamics 动力学层        │  M(ν̇) + C(ν)ν + D(ν)ν + g(η) = τ
│  SciPy rk45 积分          │  环境洋流/扰动在此叠加
└──────────────┬───────────┘
               │ 新状态 (η, ν)
        ┌──────┴────────────────┐
        ▼                       ▼
┌───────────────┐      ┌───────────────┐
│  Sensors      │      │  Recorder      │
│  噪声+延迟     │      │  CSV 记录      │
└───────┬───────┘      └───────────────┘
        └──→ 反馈回 Controller（闭环）

3.2 仿真主循环（伪代码）

from scipy.integrate import ode

# world 把车体/环境/传感器装配好，暴露 ODE 右端 f(t, y)（y=[p, q, v]），
# 本控制周期内的推进器力 τ 以全局量形式被 f 引用
f = ode(world.dynamics_rhs, t0, y0).set_integrator("rk45", rtol=1e-8, atol=1e-10)

T_c = 0.02                                   # 控制周期 100 Hz
t = t0
while t < t_end:
    sd   = world.sensors.read(t)              # 1. 传感器读数（噪声+延迟）
    v_d  = controller.update(T_c, sd, ref(t)) # 2. 控制器 → 期望速度 ν_d
    u    = world.thrusters.allocate(v_d, sd)  # 3. 推力分配（限幅）
    world.set_forces(u)                       #    本周期内 τ 恒定
    y    = f.integrate(t + T_c)               # 4. SciPy C 积分器自适应步长推进
    t   += T_c
    world.recorder.log(t, sd, v_d, u)         # 5. 记录

为什么这样用库：solve_ivp 是自适应步长的黑盒，内部步长不受控，没法在固定控制周期处插入控制律； scipy.integrate.ode(...).integrate(t_next) 则允许 Python 循环按 T_c 精确调度控制律， 周期之间的积分由 C 实现的自适应 RK45 完成——零自研积分器代码，且比纯 Python 逐步 RK4 快一个量级以上。 无控制的开环验证（M2 自由运动）直接用 solve_ivp(t_eval=...) 一次调用即可，更快。

3.3 模块职责边界

层	输入	输出	不做什么
dynamics	τ、环境力、状态	新状态 (η, ν)	不感知参考指令、不含控制逻辑
control	传感器状态、参考指令	期望速度 ν_d	不直接输出推进器量、不含物理参数
thruster	ν_d、当前速度	推进器推力 u	不含姿态换算
sensors	真实状态	带噪声/延迟的读数	不含滤波算法（可加 EKF 扩展目录）
sim	场景 + 配置	实验结果 CSV	只负责装配与循环，无算法

原则：每层只依赖下层接口，上层通过 YAML 配置切换（换控制器=换 controller: 一行，换工况=换场景文件）。

---

4. 数学模型定义（实现口径，写代码前对齐）

4.1 坐标系与符号

• 惯性系 N：右手系，x 指向北，y 指向东，z 指向地心（z 向下为正，水下常用约定）
• 体坐标系 B：x 指向艏，y 指向右，z 指向下
• 位姿 η = [pᵀ, φ, θ, ψ]ᵀ（位置 + 欧拉角：横滚 φ、俯仰 θ、航向 ψ，3-2-1 ZYX）
• 速度 ν = [u, v, w, p, q, r]ᵀ（体坐标系线速度 + 角速度）
• 姿态用四元数在仿真内部存储（避免万向锁），欧拉角仅用于日志与展示；utils/rotation.py 提供全部互转

4.2 6-DOF 刚体方程

M ν̇ + C(ν) ν + D(ν) ν + g(η) = τ_prop + τ_current + τ_dist

• M = M_RB + M_A：刚体惯性（含浮力减重效应）+ 附加质量张量（9×9，由 vehicle.yaml 配置）
• C(ν) = C_RB(ν) + C_A(ν)：科氏力与向心力项（由 M 反对称性质推导生成，不手填）
• D(ν) = D_L + D_Q·|ν|：线性阻尼 + 二次阻尼（由海试/经验公式标定）
• g(η)：重力/浮力，由重心 z_g、浮心 z_b、质量 m、排水体积 Δ 计算，在 N 系中 (0,0,ρgΔ−mg) 经 R(η)ᵀ 变换进 B 系
• 运动学：ν̂_body → ν̂_inertial 映射、四元数 q̇ = ½ q ⊗ [0, ω]

4.3 推进器与分配

• 单推进器：F = η_t · sgn(ω) · k · ω²（电机/泵喷通用，thrusters.yaml 给位置 p_i 与单位方向 d_i）
• 广义力：τ_prop = Σ (F_i·d_i ⊕ p_i×(F_i·d_i)) = B u，B ∈ R⁶ˣⁿ
• 分配律：
  • 六自由度全驱动（n≥6 且 B 满秩）：QP 最小化 ‖u‖²，约束 u_min ≤ u ≤ u_max
  • 欠驱动（无横推/前泵喷双推 + 尾桨等）：伪逆 + 死区映射，文档中显式标注可达速度子集
• H100 具体推进器数量/布局待确认，先按 thrusters.yaml 参数化，文档示例以 6 推进器（4 斜置 + 2 纵向）为基准

4.4 传感器模型

• 读数 = 真实值（延迟 τ） + 零偏 + 白噪声（σ，可配）
• IMU/DVL/深度计各自独立参数，控制器只能读传感器输出，禁止直接读真值（防仿真失真）

---

5. 控制器设计路线

1. M3 基线：级联 PID
   • 外层（导航系）：位置误差 → 期望速度 ν_d^N；姿态外环：欧拉误差 → 期望角速度
   • 内层（体坐标）：速度跟踪 PID（可带前馈项 C(ν)ν + D(ν)ν）
   • 每个被控通道独立增益组，全部 YAML 可调，含积分限幅（防 windup）
2. M4 扩展：LQR —— 线性化均衡点，python-control 的 control.lqr(A,B,Q,R) 解增益，随深度调度；用于对比 PID 性能
3. M5 扩展：反步法/滑模 —— 显式处理 C(ν)、D(ν) 非线性与扰动，鲁棒性对比
4. 所有控制器实现 ControllerBase 接口（update(dt, state, ref) -> v_des），实验脚本中一键切换

性能指标（验收口径）

任务	指标
深度保持	稳态误差 < 0.1 m，超调 < 5%，恢复时间 < 10 s（±1 m 阶跃）
定点悬停	三维位置 RMS < 0.2 m（静水）、< 0.5 m（含 0.2 m/s 洋流）
航迹跟踪	直线/waypoint 平均横向偏差、最大航向偏差、稳态跟踪误差
机动	S 形机动最大偏航延迟、侧向漂移量

---

6. 开发里程碑

阶段	内容	交付物 / 验收
M1 骨架	工程脚手架、配置加载、旋转工具 + 单测、SciPy 积分管线跑通	pytest 全绿
M2 动力学	6-DOF 方程、水动力、重力浮力、自由运动验证	静水自由释放：偏离平衡后稳定衰减回平衡点（能量单调减）；零输入不动
M3 执行与单回路	推进器模型 + 推力分配、级联 PID 深度保持	深度阶跃响应达标（§5 指标）；run_free_motion.py、run_depth_hold.py 出图
M4 全通道控制	三位置 + 三姿态全级联、定点/定速/航点任务	三维跟踪演示脚本 + 性能报告（CSV + 图）
M5 鲁棒性与对比	洋流/扰动场景、LQR/反步法实现、多控制器对比	同一工况下 PID vs LQR vs 反步对比表与轨迹图
M6 工程化	3D 可视化（plotly）、lmfit 海试数据参数标定（D、M_A、η_t）、实船数据对比（如有）	README 完整使用文档

---

7. 测试策略

• 单元测试（tests/，pytest）
  • 旋转：正交性 R·Rᵀ=I、欧拉-四元数往返一致（容差 1e-12）
  • 动力学：零输入在平衡点不动；释放扰动后总机械能单调下降；M 阵对称正定
  • 推力分配：给定 ν_d 重构 τ 误差 < 1e-9；满推时不越限
• 数值实验（脚本即测试）：每个 scripts/run_*.py 内置断言（收敛/误差阈值），失败时非零退出，可挂 CI
• 基准回归：results/baseline/ 保存 M3 完成的基准 CSV，后续改动跑 diff 防止物理模型被无意破坏

---

8. 编码约定

• 状态向量恒定使用体坐标速度 ν（方程内部），导航系位置 p ∈ R³ 独立存储
• 所有物理参数只出现在 config/*.yaml，代码中禁止硬编码数值（测试数据除外）
• 时间单位 s、力 N、长度 m、质量 kg、角速度 rad/s，全程 SI
• 类型注解 + docstring（含公式来源编号）；ruff + mypy 常规检查
• 仿真种子：随机项（传感器噪声、扰动）由场景文件提供 seed，结果可复现

---

9. 待确认事项（开工前对齐）

1. H100 推进器配置：数量、类型（电机桨 / 泵喷）、安装位置角度、最大推力 —— 决定 B 阵与欠驱动程度
2. 车体参数来源：CAD 质量属性 + 拖曳水池/拖曳试验水动力系数，还是先用文献典型值起步
3. 控制器需求：是否需要自主避碰/路径规划（如需，后续新增 planning/ 层，接口预留于 control/base.py 之上）
4. 是否需要实船日志比对：如有海试数据，M6 增加残差分析模块