commit cfc9485ce15985dbd60031d3c0e33439417fa60d Author: zjk <1553836110@qq.com> Date: Thu Aug 27 22:27:40 2026 +0800 chore: 初始提交，项目规划文档 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..025d5ca --- /dev/null +++ b/.gitignore @@ -0,0 +1,20 @@ +# Python +__pycache__/ +*.py[cod] +*.egg-info/ +.venv/ +venv/ +.pytest_cache/ +.mypy_cache/ +.ruff_cache/ + +# 仿真输出（生成物） +results/ + +# 外部数据（大体量，不进仓库） +data/ +!data/.gitkeep + +# IDE +.vscode/ +.idea/ diff --git a/PROJECT_PLAN.md b/PROJECT_PLAN.md new file mode 100644 index 0000000..29ac7b7 --- /dev/null +++ b/PROJECT_PLAN.md @@ -0,0 +1,284 @@ +# 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，自动生成） +│ └── /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 增加残差分析模块