285 lines
17 KiB
Markdown
285 lines
17 KiB
Markdown
# 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 仿真主循环（伪代码）
|
||
|
||
```python
|
||
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 增加残差分析模块
|