diff --git a/docs/img/water/rov_thruster_allocation_ros1/achieved_vs_desired.png b/docs/img/water/rov_thruster_allocation_ros1/achieved_vs_desired.png
new file mode 100644
index 0000000000..c602ec0dae
Binary files /dev/null and b/docs/img/water/rov_thruster_allocation_ros1/achieved_vs_desired.png differ
diff --git a/docs/img/water/rov_thruster_allocation_ros1/saturation_fault.png b/docs/img/water/rov_thruster_allocation_ros1/saturation_fault.png
new file mode 100644
index 0000000000..25af2c7dcd
Binary files /dev/null and b/docs/img/water/rov_thruster_allocation_ros1/saturation_fault.png differ
diff --git a/docs/img/water/rov_thruster_allocation_ros1/thruster_cmds.png b/docs/img/water/rov_thruster_allocation_ros1/thruster_cmds.png
new file mode 100644
index 0000000000..372e9c5f95
Binary files /dev/null and b/docs/img/water/rov_thruster_allocation_ros1/thruster_cmds.png differ
diff --git a/docs/index.md b/docs/index.md
index c6516c6404..892845a340 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -657,6 +657,7 @@ ROS 2 Action 斐波那契数列计算示例:[chap2/fibonacci_action](./chap2/f
* [ROV 浮力-压载配平与初稳性核算](./water/rov_ballast_trim.md) —— 全潜状态按 GM = z_B - z_G(不含水面船 BM = I/V 项)算中性浮力、重心/浮心偏心与横摇周期;默认样机缺 38.5 kg 压载,配平后 GM 42 mm、2° 倾角只对应 1.5 mm 横向偏心
* [ROV 深度保持 PID 控制与扰动恢复仿真](./water/rov_depth_hold.md) —— 负载上浮扰动与波浪扰动下的定深精度、扰动恢复时间,附实现动图
* [ROV 推进器配置矩阵与饱和感知推力分配](./water/rov_thruster_allocation.md) —— 8 推进器 6×8 配置矩阵(秩 6、条件数 10.9),箱约束 QP 分配;超包络时力旋量残差比先伪逆再限幅低 36%(20.00 vs 31.41)
+* [ROV 六推进器推力分配(ROS 1/catkin)](./water/rov_thruster_allocation_ros1.md) —— 用推进器布置矩阵的伪逆把六维力/力矩分到 4 个水平推进器和 2 个垂直推进器,对比饱和时 clamp/scale 两种策略,并支持单推进器故障重构
* [动态洋流 ROV 键盘遥操与 MLP 巡航控制](./water/mjc_rov_current_teleop.md)
* [水下 ROV 声呐图像 CNN 目标分类(Gazebo 训练链路)](./water/rov_gazebo_sonar_cnn.md) —— Gazebo 采集图像、3 类 CNN 训练与 2 Hz 定时推理,权重与类别表随包安装
diff --git a/docs/water/rov_thruster_allocation_ros1.md b/docs/water/rov_thruster_allocation_ros1.md
new file mode 100644
index 0000000000..246ad1da13
--- /dev/null
+++ b/docs/water/rov_thruster_allocation_ros1.md
@@ -0,0 +1,187 @@
+# ROV 六推进器推力分配实验
+
+上层控制器通常输出一个六维力/力矩,而 ROV 只能通过各个推进器的推力来合成这个 wrench
+(力与力矩的合称)。本实验实现「wrench → 各推进器推力」的分配节点:用推进器布置矩阵
+的伪逆做最小二乘分配,并处理推进器饱和与单推进器故障。
+
+## 一、实验目的与环境说明
+
+### 1.1 实验目的
+
+1. 理解推进器布置矩阵 $B$ 的构造:每一列是一个推进器对六维 wrench 的贡献;
+2. 掌握用伪逆 $B^{+}$ 把期望 wrench 分配到各推进器的方法;
+3. 理解**饱和**对分配结果的影响,对比 `clamp`(直接限幅)与 `scale`(按比例缩小)两种策略;
+4. 掌握**推进器故障重构**:移除故障列后重新分配,并评估剩余控制能力;
+5. 学会在 ROS 中把「期望 wrench」和「推进器指令」组织成话题接口。
+
+### 1.2 实验环境
+
+| 项目 | 配置 |
+| ---- | ---- |
+| 操作系统 | Ubuntu 20.04(ROS 端);Windows/Linux(离线验证) |
+| ROS 发行版 | ROS Noetic(Python 3) |
+| 依赖 | rospy、geometry_msgs、std_msgs、numpy |
+| 离线验证 | Python 3 + numpy + matplotlib(不需要 ROS) |
+| 被控对象抽象 | 六推进器 ROV(4 个 45° 水平推进器 + 2 个垂直推进器) |
+
+功能包目录结构:
+
+```text
+rov_thruster_allocation_ros1/
+├── main.py # ROS 节点入口:订阅 wrench、发布推进器指令
+├── allocation.py # 纯 Python 分配算法(可单独测试)
+├── offline_sim.py # 预设任务、饱和与故障场景、出图
+├── launch/main.launch # roslaunch 入口
+├── config/params.yaml
+├── test/test_rov_thruster_allocation_ros1_local.py # 本地测试
+├── package.xml
+├── CMakeLists.txt
+└── README.md
+```
+
+## 二、推进器布置与分配矩阵
+
+### 2.1 推进器布置
+
+| 编号 | 名称 | 位置 (x, y) m | 推力方向 |
+| ---- | ---- | ---- | ---- |
+| T1 | 前左水平 | (0.22, 0.16) | 45° |
+| T2 | 前右水平 | (0.22, −0.16) | −45° |
+| T3 | 后左水平 | (−0.22, 0.16) | 135° |
+| T4 | 后右水平 | (−0.22, −0.16) | −135° |
+| T5 | 前垂直 | (0.18, 0.0) | +Z |
+| T6 | 后垂直 | (−0.18, 0.0) | +Z |
+
+### 2.2 分配矩阵
+
+设第 $i$ 个推进器的推力为 $T_i$、安装位置为 $\mathbf{r}_i$、推力方向单位向量为
+$\mathbf{d}_i$,则它产生的力与力矩为
+
+$$
+\mathbf{F}_i = T_i \mathbf{d}_i, \qquad \mathbf{M}_i = \mathbf{r}_i \times \mathbf{F}_i
+$$
+
+把所有推进器按列拼起来就得到分配矩阵 $B$:
+
+$$
+\underbrace{\begin{bmatrix} F_x \\ F_y \\ F_z \\ M_x \\ M_y \\ M_z \end{bmatrix}}_{\text{wrench}}
+= B \underbrace{\begin{bmatrix} T_1 \\ \vdots \\ T_6 \end{bmatrix}}_{T}
+$$
+
+本布置无法产生横滚力矩(所有推进器的力都通过或平行于 x 轴所在平面),
+因此 $B$ 的秩为 5,横滚是**不可控自由度**。
+
+### 2.3 伪逆分配
+
+期望 wrench 已知时,最小范数解为
+
+$$
+T = B^{+}\,\text{wrench}
+$$
+
+伪逆解会自动让各推进器“平均分担”载荷,不会出现一组很大的正推力和另一组很大的负推力。
+
+### 2.4 饱和与故障
+
+- **饱和**:当某个 $|T_i|$ 超过单推进器上限 $T_{\max}$ 时:
+ - `clamp`:直接限幅 $T_i \leftarrow \mathrm{clip}(T_i, \pm T_{\max})$。各推进器被独立裁剪,
+ 合力方向会改变;
+ - `scale`:把整个解乘以 $s = T_{\max} / \max_i |T_i|$,所有推进器等比缩小,
+ 合力方向不变、幅度降为 $s$ 倍。
+- **故障**:把故障推进器的列从 $B$ 中删掉,对剩余列求伪逆,故障推进器输出置 0。
+
+## 三、节点结构与接口
+
+```mermaid
+graph LR
+ C["上层控制器 / 手柄"] -->|/rov/wrench| A["推力分配节点"]
+ A -->|/rov/thruster_cmd| T["推进器驱动"]
+ A -->|/rov/wrench_achieved| M["监控与评估"]
+```
+
+| 方向 | 话题 | 类型 | 说明 |
+| ---- | ---- | ---- | ---- |
+| 订阅 | `/rov/wrench` | `geometry_msgs/Wrench` | 期望力/力矩(机体坐标系) |
+| 发布 | `/rov/thruster_cmd` | `std_msgs/Float64MultiArray` | 6 个推进器推力(N) |
+| 发布 | `/rov/wrench_achieved` | `geometry_msgs/Wrench` | 限幅后实际可达的 wrench |
+
+安全处理:超过 `wrench_timeout`(0.5 s)没有收到新的期望 wrench 时,推进器指令归零;
+节点退出时同样归零。
+
+## 四、运行步骤
+
+### 4.1 ROS 节点
+
+```bash
+mkdir -p ~/catkin_ws/src
+cp -r <本仓库>/src/water/rov_thruster_allocation_ros1 ~/catkin_ws/src/
+chmod +x ~/catkin_ws/src/rov_thruster_allocation_ros1/main.py
+cd ~/catkin_ws && catkin_make
+source devel/setup.bash
+
+roslaunch rov_thruster_allocation_ros1 main.launch
+
+# 另开终端发送期望 wrench
+rostopic pub -1 /rov/wrench geometry_msgs/Wrench \
+ "{force: {x: 30.0, y: 0.0, z: 0.0}, torque: {x: 0.0, y: 0.0, z: 0.0}}"
+rostopic echo /rov/thruster_cmd
+```
+
+故障重构示例:`roslaunch rov_thruster_allocation_ros1 main.launch failed_thruster:=1`。
+
+### 4.2 离线验证
+
+```bash
+cd <本仓库>
+python -m pip install numpy matplotlib
+python src/water/rov_thruster_allocation_ros1/offline_sim.py
+```
+
+脚本发送一段 12 s 的预设任务(前进 → 转向 → 下潜抬头 → 组合运动),
+再单独测试饱和和故障场景,输出三张图与指标。
+
+## 五、实验结果
+
+### 5.1 正常任务下的推进器指令
+
+
+
+- 0~3 s 前进:四个水平推进器成对出力(T1/T2 为正、T3/T4 为负),合力沿 +x;
+- 3~6 s 转向:对角推进器反向出力,产生 +Mz 力矩;
+- 6~9 s 下潜抬头:T6 明显大于 T5,既提供垂向力又产生抬头力矩;
+- 9~12 s 组合运动:水平与垂直推进器同时工作。
+
+### 5.2 期望与可达 wrench
+
+
+
+正常工况下(最大推力 40 N),实际可达 wrench 与期望值完全重合,
+任务段最大分配残差约 $10^{-13}\%$(数值精度),说明伪逆分配是精确的。
+
+### 5.3 饱和与故障对比
+
+
+
+| 场景 | 相对残差 | 说明 |
+| ---- | ---- | ---- |
+| 正常 | ≈ 0% | 指令在推进器能力范围内 |
+| 饱和 + clamp | 82.56% | 各推进器被独立裁剪,水平力方向偏差 13.57° |
+| 饱和 + scale | 55.92% | 等比缩小 0.441 倍,方向不变,幅度不足 |
+| T2 故障 | 15.15% | 剩余推进器重新分配,前进力降到约 25.5 N,转向力矩部分损失 |
+
+结论:饱和时 `clamp` 的残差反而比 `scale` 更大,因为独立限幅破坏了推进器之间的比例关系;
+`scale` 虽然也达不到期望,但至少保持了合力方向,更适合作为默认策略。
+
+## 六、讨论与限制
+
+1. 伪逆分配只考虑几何关系,没有考虑推进器推力与电压/转速的非线性、死区和效率差异;
+2. 本布置横滚不可控,若需要横滚自由度需要增加推进器或改变布置;
+3. 故障重构假设已通过检测知道哪个推进器失效,故障诊断本身不在本实验范围内;
+4. 离线验证只检查分配矩阵与限幅逻辑,不包含 ROV 动力学响应;
+5. 本机为 Windows,没有 ROS 与 ROV 仿真环境,**ROS 端实际运行未验证**,
+ 需要在 ROS Noetic 与 ROV 仿真器/实机上按 4.1 节复验。
+
+## 说明
+
+本文档与配套代码在整理过程中使用了 AI 助手(DeepSeek)辅助,实验参数、运行结果
+和图表均由作者实际运行核对;作者对本文内容负全部责任。
diff --git a/mkdocs.yml b/mkdocs.yml
index b0722465d0..d7dbc60e0a 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -569,6 +569,7 @@ nav:
- ROV 浮力-压载配平与初稳性核算: './water/rov_ballast_trim.md'
- ROV 深度保持 PID 控制与扰动恢复仿真: './water/rov_depth_hold.md'
- ROV 推进器配置矩阵与饱和感知推力分配: './water/rov_thruster_allocation.md'
+ - ROV 六推进器推力分配(ROS 1/catkin): './water/rov_thruster_allocation_ros1.md'
- 水下多传感器感知与 3D 轨迹跟踪控制: './water/rov_trajectory_tracking.md'
- 水下声呐 SLAM 占据栅格建图与自主避障导航: './water/rov_sonar_slam_navigation.md'
- MuJoCo 水下 ROV 串级定深控制: './water/mujoco_rov_depth_control.md'
diff --git a/src/water/rov_thruster_allocation_ros1/CMakeLists.txt b/src/water/rov_thruster_allocation_ros1/CMakeLists.txt
new file mode 100644
index 0000000000..0cd3890a27
--- /dev/null
+++ b/src/water/rov_thruster_allocation_ros1/CMakeLists.txt
@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(rov_thruster_allocation_ros1)
+
+# 分配算法使用 numpy,ROS Noetic 的 python3-numpy 由系统提供
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ geometry_msgs
+ std_msgs
+)
+
+catkin_package()
+
+catkin_install_python(PROGRAMS
+ main.py
+ allocation.py
+ offline_sim.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch config
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS
+)
diff --git a/src/water/rov_thruster_allocation_ros1/README.md b/src/water/rov_thruster_allocation_ros1/README.md
new file mode 100644
index 0000000000..7e4d6c217b
--- /dev/null
+++ b/src/water/rov_thruster_allocation_ros1/README.md
@@ -0,0 +1,87 @@
+# rov_thruster_allocation_ros1 — ROV 六推进器推力分配
+
+把上层控制器给出的期望力/力矩 `/rov/wrench`(`geometry_msgs/Wrench`,机体坐标系)分配到
+4 个 45° 布置的水平推进器和 2 个垂直推进器,发布 `/rov/thruster_cmd`
+(`std_msgs/Float64MultiArray`,单位 N),同时发布实际可达的 `/rov/wrench_achieved`
+(`Wrench`)。支持推进器饱和处理和单推进器故障重构。配套教程见
+[docs/water/rov_thruster_allocation_ros1.md](../../docs/water/rov_thruster_allocation_ros1.md)。
+
+## 运行环境
+
+| 项目 | 配置 |
+| ---- | ---- |
+| 操作系统 | Ubuntu 20.04(ROS 节点) |
+| ROS | ROS Noetic(Python 3) |
+| 依赖 | rospy、geometry_msgs、std_msgs、numpy |
+| 离线验证 | Windows / Linux,Python 3 + numpy + matplotlib(不需要 ROS) |
+
+## 运行步骤
+
+### 方式一:ROS 节点
+
+```bash
+# 1. 拷贝到工作空间并编译
+mkdir -p ~/catkin_ws/src
+cp -r <本仓库>/src/water/rov_thruster_allocation_ros1 ~/catkin_ws/src/
+chmod +x ~/catkin_ws/src/rov_thruster_allocation_ros1/main.py
+cd ~/catkin_ws && catkin_make
+source devel/setup.bash
+
+# 2. 启动分配节点
+roslaunch rov_thruster_allocation_ros1 main.launch
+
+# 3. 另开终端手动发一个期望 wrench(向前的 20 N 推力)
+rostopic pub -1 /rov/wrench geometry_msgs/Wrench \
+ "{force: {x: 20.0, y: 0.0, z: 0.0}, torque: {x: 0.0, y: 0.0, z: 0.0}}"
+
+# 4. 查看分配结果
+rostopic echo /rov/thruster_cmd
+rostopic echo /rov/wrench_achieved
+```
+
+### 方式二:离线仿真(不需要 ROS)
+
+```bash
+cd <本仓库>
+python -m pip install numpy matplotlib
+python src/water/rov_thruster_allocation_ros1/offline_sim.py
+```
+
+脚本按一段预设任务发送多组 wrench,输出推进器指令、可达 wrench 和饱和/故障对比图。
+
+## 参数
+
+| 参数 | 默认值 | 说明 |
+| ---- | ---- | ---- |
+| `~max_thrust` | 40.0 | 单个推进器最大推力(N) |
+| `~saturation_mode` | `scale` | 饱和策略:`scale` 按比例缩小,`clamp` 直接限幅 |
+| `~failed_thruster` | -1 | 故障推进器下标(0~5),-1 表示正常 |
+| `~control_rate` | 20.0 | 分配频率(Hz) |
+| `~wrench_timeout` | 0.5 | 期望 wrench 超时(s),超时后推进器归零 |
+
+## 话题接口
+
+| 方向 | 话题 | 类型 | 说明 |
+| ---- | ---- | ---- | ---- |
+| 订阅 | `/rov/wrench` | `geometry_msgs/Wrench` | 期望力/力矩(机体坐标系,Z 向上) |
+| 发布 | `/rov/thruster_cmd` | `std_msgs/Float64MultiArray` | 6 个推进器推力(N) |
+| 发布 | `/rov/wrench_achieved` | `geometry_msgs/Wrench` | 限幅后实际可达的 wrench |
+
+## 推进器布置
+
+| 编号 | 名称 | 位置 (x, y) m | 推力方向 |
+| ---- | ---- | ---- | ---- |
+| T1 | 前左水平 | (0.22, 0.16) | 45° |
+| T2 | 前右水平 | (0.22, -0.16) | −45° |
+| T3 | 后左水平 | (−0.22, 0.16) | 135° |
+| T4 | 后右水平 | (−0.22, −0.16) | −135° |
+| T5 | 前垂直 | (0.18, 0.0) | +Z |
+| T6 | 后垂直 | (−0.18, 0.0) | +Z |
+
+该布置无法产生横滚力矩(Mx),横滚是不可控自由度,分配矩阵秩为 5。
+
+## 预期效果
+
+正常工况下,分配后的实际 wrench 与期望值误差在数值精度范围内(< 0.01%);
+期望推力超过推进器能力时,`scale` 模式按比例缩小并保持方向,`clamp` 模式会改变合力方向;
+某个推进器故障后,其余推进器重新分配,仍能保持大部分自由度的控制能力。
diff --git a/src/water/rov_thruster_allocation_ros1/allocation.py b/src/water/rov_thruster_allocation_ros1/allocation.py
new file mode 100644
index 0000000000..a1dc6a6e27
--- /dev/null
+++ b/src/water/rov_thruster_allocation_ros1/allocation.py
@@ -0,0 +1,111 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""ROV 六推进器推力分配(纯 Python + numpy,不依赖 ROS)。
+
+给定期望的六维力/力矩(机体坐标系,单位 N 与 N·m):
+
+ wrench = [Fx, Fy, Fz, Mx, My, Mz]
+
+按照推进器布置矩阵 B(6 x N),用最小二乘伪逆求每个推进器的推力:
+
+ T = B^+ * wrench
+
+并处理两类工程问题:
+
+1. **饱和**:任一推进器超过最大推力时,
+ - `clamp` 模式直接限幅(会改变合力的方向);
+ - `scale` 模式把整个期望力/力矩按比例缩小(方向不变,但幅度变小);
+2. **推进器故障**:把故障推进器对应的列从 B 中移除,用剩余推进器重新分配,
+ 故障推进器输出固定为 0。
+
+推进器布置参考 BlueROV2 一类的六推进器 ROV:
+4 个 45° 布置的水平推进器 + 2 个垂直推进器。该布置无法产生横滚力矩(Mx),
+因此 B 的秩为 5,横滚是不可控自由度。
+"""
+from __future__ import print_function
+
+import numpy as np
+
+# 推进器布置:名称、安装位置 (x, y) m、推力方向(单位向量,机体坐标系)
+THRUSTERS = [
+ {'name': 'T1_front_left', 'pos': (0.22, 0.16), 'dir': (0.7071, 0.7071, 0.0)},
+ {'name': 'T2_front_right', 'pos': (0.22, -0.16), 'dir': (0.7071, -0.7071, 0.0)},
+ {'name': 'T3_rear_left', 'pos': (-0.22, 0.16), 'dir': (-0.7071, 0.7071, 0.0)},
+ {'name': 'T4_rear_right', 'pos': (-0.22, -0.16), 'dir': (-0.7071, -0.7071, 0.0)},
+ {'name': 'T5_vertical_front', 'pos': (0.18, 0.0), 'dir': (0.0, 0.0, 1.0)},
+ {'name': 'T6_vertical_rear', 'pos': (-0.18, 0.0), 'dir': (0.0, 0.0, 1.0)},
+]
+
+WRENCH_NAMES = ('Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz')
+
+
+def build_allocation_matrix(thrusters=None):
+ """由推进器布置构造 6 x N 的分配矩阵 B:wrench = B * T。"""
+ if thrusters is None:
+ thrusters = THRUSTERS
+ B = np.zeros((6, len(thrusters)))
+ for i, thruster in enumerate(thrusters):
+ x, y = thruster['pos']
+ dx, dy, dz = thruster['dir']
+ force = np.array([dx, dy, dz], dtype=float)
+ moment = np.cross(np.array([x, y, 0.0]), force)
+ B[:, i] = np.concatenate([force, moment])
+ return B
+
+
+def allocate(wrench, max_thrust=20.0, failed=None, mode='scale', thrusters=None):
+ """把期望 wrench 分配到各推进器。
+
+ 参数
+ ----
+ wrench : 长度 6 的序列 [Fx, Fy, Fz, Mx, My, Mz]
+ max_thrust : 单个推进器最大推力(N)
+ failed : 故障推进器下标(从 0 开始),None 表示全部正常
+ mode : 'scale'(按比例缩小,保方向)或 'clamp'(直接限幅)
+ thrusters : 可选的自定义推进器布置
+
+ 返回字典:commands、achieved、residual、saturated、scale、active
+ """
+ if thrusters is None:
+ thrusters = THRUSTERS
+ B = build_allocation_matrix(thrusters)
+ wrench = np.asarray(wrench, dtype=float)
+ n = B.shape[1]
+
+ active = [i for i in range(n) if i != failed]
+ B_active = B[:, active]
+ raw = np.linalg.pinv(B_active).dot(wrench)
+
+ scale = 1.0
+ saturated = False
+ if len(raw) and np.max(np.abs(raw)) > max_thrust:
+ saturated = True
+ if mode == 'clamp':
+ raw = np.clip(raw, -max_thrust, max_thrust)
+ else:
+ scale = max_thrust / np.max(np.abs(raw))
+ raw = raw * scale
+
+ commands = np.zeros(n)
+ for k, index in enumerate(active):
+ commands[index] = raw[k]
+
+ achieved = B.dot(commands)
+ residual = wrench - achieved
+ return {
+ 'commands': commands,
+ 'achieved': achieved,
+ 'residual': residual,
+ 'saturated': saturated,
+ 'scale': scale,
+ 'active': active,
+ }
+
+
+def residual_ratio(result, wrench):
+ """残差相对期望 wrench 的比例(%),用于横向对比不同分配策略。"""
+ wrench = np.asarray(wrench, dtype=float)
+ norm = np.linalg.norm(wrench)
+ if norm < 1e-12:
+ return 0.0
+ return float(np.linalg.norm(result['residual']) / norm * 100.0)
diff --git a/src/water/rov_thruster_allocation_ros1/config/params.yaml b/src/water/rov_thruster_allocation_ros1/config/params.yaml
new file mode 100644
index 0000000000..29039416b9
--- /dev/null
+++ b/src/water/rov_thruster_allocation_ros1/config/params.yaml
@@ -0,0 +1,6 @@
+# 推力分配参数(launch 中作为节点私有参数加载)
+max_thrust: 40.0 # 单个推进器最大推力(N)
+saturation_mode: scale # 饱和策略:scale 按比例缩小 / clamp 直接限幅
+failed_thruster: -1 # 故障推进器下标(0~5),-1 表示全部正常
+control_rate: 20.0 # 分配频率(Hz)
+wrench_timeout: 0.5 # 期望 wrench 超时(s),超时后推进器归零
diff --git a/src/water/rov_thruster_allocation_ros1/launch/main.launch b/src/water/rov_thruster_allocation_ros1/launch/main.launch
new file mode 100644
index 0000000000..951e57fabd
--- /dev/null
+++ b/src/water/rov_thruster_allocation_ros1/launch/main.launch
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/water/rov_thruster_allocation_ros1/main.py b/src/water/rov_thruster_allocation_ros1/main.py
new file mode 100644
index 0000000000..ab0233d50f
--- /dev/null
+++ b/src/water/rov_thruster_allocation_ros1/main.py
@@ -0,0 +1,127 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""ROV 推力分配 ROS 节点(模块入口 main.py)。
+
+订阅期望力/力矩 `/rov/wrench`(geometry_msgs/Wrench,机体坐标系),
+用 allocation.py 的伪逆分配算法把六维 wrench 分配到各推进器,
+发布:
+
+ /rov/thruster_cmd std_msgs/Float64MultiArray 各推进器推力(N)
+ /rov/wrench_achieved geometry_msgs/Wrench 实际可达的 wrench
+
+支持推进器故障(参数 ~failed_thruster)和两种饱和策略(~saturation_mode):
+`scale` 按比例缩小、保持合力方向;`clamp` 直接限幅。
+
+运行:
+ rosrun rov_thruster_allocation_ros1 main.py
+ roslaunch rov_thruster_allocation_ros1 main.launch
+"""
+from __future__ import print_function
+
+import rospy
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray
+
+from allocation import THRUSTERS, allocate
+
+
+class ThrusterAllocationNode(object):
+
+ def __init__(self):
+ self.wrench_topic = rospy.get_param('~wrench_topic', '/rov/wrench')
+ self.cmd_topic = rospy.get_param('~cmd_topic', '/rov/thruster_cmd')
+ self.achieved_topic = rospy.get_param('~achieved_topic', '/rov/wrench_achieved')
+ self.max_thrust = rospy.get_param('~max_thrust', 40.0)
+ self.saturation_mode = rospy.get_param('~saturation_mode', 'scale')
+ failed = int(rospy.get_param('~failed_thruster', -1))
+ self.failed = None if failed < 0 else failed
+ self.wrench_timeout = rospy.get_param('~wrench_timeout', 0.5)
+
+ if self.failed is not None and not (0 <= self.failed < len(THRUSTERS)):
+ rospy.logwarn('failed_thruster=%d 超出范围,按无故障处理', self.failed)
+ self.failed = None
+
+ self.wrench = None
+ self.last_wrench_time = None
+ self.was_saturated = False
+
+ self.pub_cmd = rospy.Publisher(self.cmd_topic, Float64MultiArray, queue_size=10)
+ self.pub_achieved = rospy.Publisher(self.achieved_topic, Wrench, queue_size=10)
+ rospy.Subscriber(self.wrench_topic, Wrench, self.on_wrench)
+
+ rospy.loginfo('推力分配节点已启动:wrench=%s -> cmd=%s,max_thrust=%.1f N,模式=%s',
+ self.wrench_topic, self.cmd_topic, self.max_thrust,
+ self.saturation_mode)
+ if self.failed is not None:
+ rospy.logwarn('推进器 %s 已标记为故障,其余推进器重新分配',
+ THRUSTERS[self.failed]['name'])
+
+ def on_wrench(self, msg):
+ self.wrench = [msg.force.x, msg.force.y, msg.force.z,
+ msg.torque.x, msg.torque.y, msg.torque.z]
+ self.last_wrench_time = rospy.Time.now()
+
+ def publish_cmd(self, commands):
+ msg = Float64MultiArray()
+ msg.data = [float(v) for v in commands]
+ self.pub_cmd.publish(msg)
+
+ def publish_achieved(self, achieved):
+ msg = Wrench()
+ msg.force.x, msg.force.y, msg.force.z = achieved[0], achieved[1], achieved[2]
+ msg.torque.x, msg.torque.y, msg.torque.z = achieved[3], achieved[4], achieved[5]
+ self.pub_achieved.publish(msg)
+
+ def publish_idle(self):
+ self.publish_cmd([0.0] * len(THRUSTERS))
+ self.publish_achieved([0.0] * 6)
+
+ def run(self):
+ rate = rospy.Rate(rospy.get_param('~control_rate', 20.0))
+ while not rospy.is_shutdown():
+ now = rospy.Time.now()
+ if self.wrench is None or self.last_wrench_time is None:
+ self.publish_idle()
+ rate.sleep()
+ continue
+
+ if (now - self.last_wrench_time).to_sec() > self.wrench_timeout:
+ self.publish_idle()
+ rospy.logwarn_throttle(2.0, '超过 %.1f 秒没有收到期望 wrench,推进器已归零',
+ self.wrench_timeout)
+ rate.sleep()
+ continue
+
+ result = allocate(self.wrench, max_thrust=self.max_thrust,
+ failed=self.failed, mode=self.saturation_mode)
+ self.publish_cmd(result['commands'])
+ self.publish_achieved(result['achieved'])
+
+ if result['saturated'] != self.was_saturated:
+ if result['saturated']:
+ if self.saturation_mode == 'scale':
+ rospy.logwarn('推进器饱和,期望 wrench 按 %.2f 比例缩小',
+ result['scale'])
+ else:
+ rospy.logwarn('推进器饱和,已按最大推力限幅')
+ self.was_saturated = result['saturated']
+
+ rate.sleep()
+
+ self.publish_idle()
+
+
+def main():
+ rospy.init_node('rov_thruster_allocation_ros1')
+ node = ThrusterAllocationNode()
+ try:
+ node.run()
+ except rospy.ROSInterruptException:
+ pass
+ finally:
+ node.publish_idle()
+ rospy.loginfo('节点退出,推进器指令已归零')
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/water/rov_thruster_allocation_ros1/offline_sim.py b/src/water/rov_thruster_allocation_ros1/offline_sim.py
new file mode 100644
index 0000000000..e63e425a7e
--- /dev/null
+++ b/src/water/rov_thruster_allocation_ros1/offline_sim.py
@@ -0,0 +1,200 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""ROV 推力分配的离线验证与出图(不需要 ROS / 仿真器)。
+
+用法(在仓库根目录执行):
+ python src/water/rov_thruster_allocation_ros1/offline_sim.py
+
+内容:
+1. 一段 12 s 的预设任务(前进、转向、下潜、组合运动),验证正常分配精度;
+2. 饱和对比:期望推力超过单推进器上限时,clamp 与 scale 两种策略的差异;
+3. 故障对比:T2 推进器失效后,用剩余推进器重新分配的效果。
+
+输出:
+ docs/img/water/rov_thruster_allocation_ros1/thruster_cmds.png
+ docs/img/water/rov_thruster_allocation_ros1/achieved_vs_desired.png
+ docs/img/water/rov_thruster_allocation_ros1/saturation_fault.png
+"""
+from __future__ import print_function
+
+import json
+import math
+import os
+import sys
+
+import numpy as np
+
+HERE = os.path.dirname(os.path.abspath(__file__))
+REPO_ROOT = os.path.abspath(os.path.join(HERE, '..', '..', '..'))
+sys.path.insert(0, HERE)
+
+from allocation import ( # noqa: E402
+ THRUSTERS, WRENCH_NAMES, allocate, build_allocation_matrix, residual_ratio,
+)
+
+OUTPUT_DIR = os.path.join(REPO_ROOT, 'docs', 'img', 'water', 'rov_thruster_allocation_ros1')
+
+RATE = 20.0
+MAX_THRUST = 40.0
+DURATION = 12.0
+
+
+def mission_wrench(t):
+ """12 s 预设任务:前进 -> 转向 -> 下潜抬头 -> 组合运动。"""
+ if t < 3.0:
+ return [30.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+ if t < 6.0:
+ return [0.0, 0.0, 0.0, 0.0, 0.0, 5.0]
+ if t < 9.0:
+ return [0.0, 0.0, 20.0, 0.0, 3.0, 0.0]
+ return [20.0, 10.0, 12.0, 0.0, 0.0, 3.0]
+
+
+def run_mission():
+ steps = int(DURATION * RATE)
+ times = np.arange(steps) / RATE
+ commands = np.zeros((steps, len(THRUSTERS)))
+ achieved = np.zeros((steps, 6))
+ desired = np.zeros((steps, 6))
+ ratios = np.zeros(steps)
+
+ for k, t in enumerate(times):
+ wrench = mission_wrench(t)
+ result = allocate(wrench, max_thrust=MAX_THRUST, mode='scale')
+ commands[k] = result['commands']
+ achieved[k] = result['achieved']
+ desired[k] = wrench
+ ratios[k] = residual_ratio(result, wrench)
+
+ return times, desired, achieved, commands, ratios
+
+
+def wrench_angle_error_deg(wrench, result):
+ """水平推力方向误差(度),用 Fx/Fy 两个分量计算。"""
+ w = np.asarray(wrench[:2], dtype=float)
+ a = np.asarray(result['achieved'][:2], dtype=float)
+ nw, na = np.linalg.norm(w), np.linalg.norm(a)
+ if nw < 1e-9 or na < 1e-9:
+ return float('nan')
+ cos_value = float(np.dot(w, a) / (nw * na))
+ cos_value = max(-1.0, min(1.0, cos_value))
+ return math.degrees(math.acos(cos_value))
+
+
+def scenarios():
+ """返回各场景的指标,用于柱状图。"""
+ normal = allocate([20.0, 0.0, 0.0, 0.0, 0.0, 0.0], max_thrust=MAX_THRUST)
+ over = [80.0, 10.0, 0.0, 0.0, 0.0, 10.0] # 超过推进器能力
+ clamp = allocate(over, max_thrust=MAX_THRUST, mode='clamp')
+ scale = allocate(over, max_thrust=MAX_THRUST, mode='scale')
+ fault_wrench = [30.0, 0.0, 0.0, 0.0, 0.0, 4.0]
+ fault = allocate(fault_wrench, max_thrust=MAX_THRUST, failed=1)
+
+ data = {
+ 'normal': residual_ratio(normal, [20.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
+ 'clamp': residual_ratio(clamp, over),
+ 'scale': residual_ratio(scale, over),
+ 'fault': residual_ratio(fault, fault_wrench),
+ }
+ extra = {
+ 'normal_max_residual_pct': data['normal'],
+ 'saturation_clamp_residual_pct': data['clamp'],
+ 'saturation_scale_residual_pct': data['scale'],
+ 'saturation_scale_factor': scale['scale'],
+ 'saturation_clamp_angle_error_deg': wrench_angle_error_deg(over, clamp),
+ 'saturation_scale_angle_error_deg': wrench_angle_error_deg(over, scale),
+ 'fault_residual_pct': data['fault'],
+ 'fault_achieved': fault['achieved'].tolist(),
+ 'fault_commands': fault['commands'].tolist(),
+ 'matrix_rank': int(np.linalg.matrix_rank(build_allocation_matrix())),
+ }
+ return data, extra
+
+
+def plot(times, desired, achieved, commands, ratios, bar_data):
+ import matplotlib
+ matplotlib.use('Agg')
+ import matplotlib.pyplot as plt
+
+ os.makedirs(OUTPUT_DIR, exist_ok=True)
+
+ # 1) 推进器指令
+ styles = ['-', '--', '-.', ':']
+ fig, axes = plt.subplots(2, 1, figsize=(9, 6), sharex=True)
+ for i in range(4):
+ axes[0].plot(times, commands[:, i], label=THRUSTERS[i]['name'],
+ linewidth=1.6, linestyle=styles[i])
+ axes[0].set_ylabel('thrust (N)')
+ axes[0].set_title('Horizontal thruster commands during the mission')
+ axes[0].grid(True, alpha=0.3)
+ axes[0].legend(loc='upper right', fontsize=7, ncol=2)
+
+ for i in (4, 5):
+ axes[1].plot(times, commands[:, i], label=THRUSTERS[i]['name'],
+ linewidth=1.6, linestyle=styles[i - 4])
+ axes[1].set_xlabel('time (s)')
+ axes[1].set_ylabel('thrust (N)')
+ axes[1].set_title('Vertical thruster commands')
+ axes[1].grid(True, alpha=0.3)
+ axes[1].legend(loc='upper right', fontsize=8)
+ fig.tight_layout()
+ fig.savefig(os.path.join(OUTPUT_DIR, 'thruster_cmds.png'), dpi=150)
+ plt.close(fig)
+
+ # 2) 期望 vs 实际 wrench(四个可控分量)
+ components = [0, 1, 2, 5] # Fx, Fy, Fz, Mz
+ fig, axes = plt.subplots(2, 2, figsize=(9, 6), sharex=True)
+ for ax, index in zip(axes.flat, components):
+ ax.plot(times, desired[:, index], 'k--', linewidth=1.5, label='desired')
+ ax.plot(times, achieved[:, index], color='#1f77b4', linewidth=1.5, label='achieved')
+ ax.set_title(WRENCH_NAMES[index])
+ ax.grid(True, alpha=0.3)
+ ax.legend(loc='upper right', fontsize=8)
+ for ax in axes[1]:
+ ax.set_xlabel('time (s)')
+ fig.suptitle('Desired vs achieved wrench (normal operation)')
+ fig.tight_layout()
+ fig.savefig(os.path.join(OUTPUT_DIR, 'achieved_vs_desired.png'), dpi=150)
+ plt.close(fig)
+
+ # 3) 饱和与故障残差对比
+ labels = ['normal', 'clamp\n(saturated)', 'scale\n(saturated)', 'fault T2']
+ values = [bar_data['normal'], bar_data['clamp'], bar_data['scale'], bar_data['fault']]
+ colors = ['#2ca02c', '#ff7f0e', '#1f77b4', '#d62728']
+ fig, ax = plt.subplots(figsize=(7, 4.2))
+ bars = ax.bar(labels, values, color=colors)
+ for bar, value in zip(bars, values):
+ ax.text(bar.get_x() + bar.get_width() / 2.0, value + 0.3,
+ '%.2f%%' % value, ha='center', va='bottom', fontsize=9)
+ ax.set_ylabel('relative wrench residual (%)')
+ ax.set_title('Allocation residual: normal, saturation and fault')
+ ax.grid(True, axis='y', alpha=0.3)
+ fig.tight_layout()
+ fig.savefig(os.path.join(OUTPUT_DIR, 'saturation_fault.png'), dpi=150)
+ plt.close(fig)
+
+
+def main():
+ times, desired, achieved, commands, ratios = run_mission()
+ bar_data, extra = scenarios()
+ plot(times, desired, achieved, commands, ratios, bar_data)
+
+ result = {
+ 'mission_max_residual_pct': float(ratios.max()),
+ 'mission_mean_residual_pct': float(ratios.mean()),
+ 'max_thruster_command_N': float(np.abs(commands).max()),
+ 'saturation_scale_factor': extra['saturation_scale_factor'],
+ 'saturation_clamp_residual_pct': extra['saturation_clamp_residual_pct'],
+ 'saturation_scale_residual_pct': extra['saturation_scale_residual_pct'],
+ 'saturation_clamp_angle_error_deg': extra['saturation_clamp_angle_error_deg'],
+ 'fault_T2_residual_pct': extra['fault_residual_pct'],
+ 'matrix_rank': extra['matrix_rank'],
+ }
+ print(json.dumps(result, ensure_ascii=False, indent=2))
+ for name in ('thruster_cmds.png', 'achieved_vs_desired.png', 'saturation_fault.png'):
+ print('figure:', os.path.join(OUTPUT_DIR, name))
+ return 0
+
+
+if __name__ == '__main__':
+ sys.exit(main())
diff --git a/src/water/rov_thruster_allocation_ros1/package.xml b/src/water/rov_thruster_allocation_ros1/package.xml
new file mode 100644
index 0000000000..4bf7760f7d
--- /dev/null
+++ b/src/water/rov_thruster_allocation_ros1/package.xml
@@ -0,0 +1,20 @@
+
+
+ rov_thruster_allocation_ros1
+ 0.1.0
+
+ ROV 六推进器推力分配模块:订阅期望力/力矩 /rov/wrench,用分配矩阵
+ 的伪逆把六维 wrench 分配到 4 个水平推进器和 2 个垂直推进器,
+ 发布 /rov/thruster_cmd 与 /rov/wrench_achieved;支持饱和处理和
+ 单推进器故障重构。配套文档见 docs/water/rov_thruster_allocation_ros1.md。
+
+ xinyangtang91
+ MIT
+
+ catkin
+
+ rospy
+ geometry_msgs
+ std_msgs
+ python3-numpy
+
diff --git a/src/water/rov_thruster_allocation_ros1/test/test_rov_thruster_allocation_ros1_local.py b/src/water/rov_thruster_allocation_ros1/test/test_rov_thruster_allocation_ros1_local.py
new file mode 100644
index 0000000000..66b6a2d7fa
--- /dev/null
+++ b/src/water/rov_thruster_allocation_ros1/test/test_rov_thruster_allocation_ros1_local.py
@@ -0,0 +1,145 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""rov_thruster_allocation_ros1 本地测试(不需要 ROS / 仿真器).
+
+A. 分配矩阵:形状、秩、横滚不可控
+B. 正常分配:合力与期望一致
+C. 饱和:clamp / scale 两种策略与限幅
+D. 故障重构:故障推进器输出为 0,其余推进器重新分配
+E. 工程文件
+
+用法: python3 tests/test_rov_thruster_allocation_ros1_local.py
+"""
+from __future__ import annotations
+
+import os
+import sys
+import xml.etree.ElementTree as ET
+
+import numpy as np
+
+PASS, FAIL = [], []
+
+
+def check(cond, label):
+ (PASS if cond else FAIL).append(label)
+ print(" [%s] %s" % ("PASS" if cond else "FAIL", label))
+
+
+HERE = os.path.dirname(os.path.abspath(__file__))
+PKG = os.path.abspath(os.path.join(HERE, ".."))
+ROOT = os.path.abspath(os.path.join(PKG, "..", "..", ".."))
+sys.path.insert(0, PKG)
+
+from allocation import ( # noqa: E402
+ THRUSTERS, allocate, build_allocation_matrix, residual_ratio,
+)
+
+
+# ================================================================= A. 分配矩阵
+def test_matrix():
+ print("== A. 分配矩阵 ==")
+ B = build_allocation_matrix()
+ check(B.shape == (6, len(THRUSTERS)), "B 形状为 6 x %d" % len(THRUSTERS))
+ check(np.linalg.matrix_rank(B) == 5, "B 的秩为 5(横滚不可控)")
+ check(np.allclose(B[3, :], 0.0), "横滚行全为 0")
+ check(np.allclose(B[2, 4:], [1.0, 1.0]), "两个垂直推进器提供垂向推力")
+
+
+# ================================================================= B. 正常分配
+def test_normal_allocation():
+ print("== B. 正常分配 ==")
+ for wrench in ([30.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 20.0, 0.0, 3.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 5.0]):
+ result = allocate(wrench, max_thrust=40.0)
+ check(not result["saturated"], "wrench=%s 未饱和" % wrench[:2])
+ check(np.linalg.norm(result["residual"]) < 1e-9,
+ "wrench=%s 分配残差接近 0" % wrench[:2])
+ check(np.all(np.abs(result["commands"]) <= 40.0 + 1e-9),
+ "各推进器推力不超过 40 N")
+
+ result = allocate([0.0, 0.0, 0.0, 2.0, 0.0, 0.0], max_thrust=40.0)
+ check(abs(result["achieved"][3]) < 1e-9, "横滚指令无法执行,Mx 实际为 0")
+
+
+# ================================================================= C. 饱和
+def test_saturation():
+ print("== C. 饱和处理 ==")
+ wrench = [80.0, 10.0, 0.0, 0.0, 0.0, 10.0]
+ clamp = allocate(wrench, max_thrust=40.0, mode="clamp")
+ scale = allocate(wrench, max_thrust=40.0, mode="scale")
+
+ check(clamp["saturated"] and scale["saturated"], "两种模式都检测到饱和")
+ check(np.all(np.abs(clamp["commands"]) <= 40.0 + 1e-9), "clamp 后推力不超过上限")
+ check(np.all(np.abs(scale["commands"]) <= 40.0 + 1e-9), "scale 后推力不超过上限")
+ check(scale["scale"] < 1.0, "scale 比例小于 1(%.2f)" % scale["scale"])
+
+ check(residual_ratio(clamp, wrench) > residual_ratio(scale, wrench),
+ "clamp 残差大于 scale(方向被破坏)")
+ check(np.allclose(scale["achieved"], np.array(wrench) * scale["scale"], atol=1e-9),
+ "scale 模式下实际 wrench 与期望成正比(保持方向)")
+
+
+# ================================================================= D. 故障重构
+def test_fault():
+ print("== D. 故障重构 ==")
+ result = allocate([30.0, 0.0, 0.0, 0.0, 0.0, 4.0], max_thrust=40.0, failed=1)
+ check(result["commands"][1] == 0.0, "故障推进器 T2 指令为 0")
+ check(1 not in result["active"] and len(result["active"]) == len(THRUSTERS) - 1,
+ "活动推进器数量少 1")
+ check(abs(result["achieved"][0]) > 20.0, "剩余推进器仍能提供主要前进推力")
+ check(result["residual"][0] != 0.0, "故障后存在不可完全补偿的残差")
+
+
+# ================================================================= E. 工程文件
+def test_project_files():
+ print("== E. 工程文件 ==")
+ pkg = ET.parse(os.path.join(PKG, "package.xml")).getroot()
+ check(pkg.findtext("name").strip() == "rov_thruster_allocation_ros1", "package.xml 包名正确")
+ check(pkg.get("format") == "3", "package.xml 使用 format=\"3\"")
+ deps = [d.text.strip() for d in pkg.findall("depend")]
+ for dep in ("rospy", "geometry_msgs", "std_msgs"):
+ check(dep in deps, "package.xml 声明依赖 %s" % dep)
+ exec_deps = [d.text.strip() for d in pkg.findall("exec_depend")]
+ check("python3-numpy" in exec_deps, "package.xml 声明 python3-numpy 运行依赖")
+
+ cmake = open(os.path.join(PKG, "CMakeLists.txt"), encoding="utf-8").read()
+ for name in ("main.py", "allocation.py", "offline_sim.py"):
+ check(name in cmake, "CMakeLists.txt 安装 %s" % name)
+
+ launch = ET.parse(os.path.join(PKG, "launch", "main.launch")).getroot()
+ nodes = [n.get("type") for n in launch.findall("node")]
+ check("main.py" in nodes, "main.launch 启动 main.py")
+ args = [a.get("name") for a in launch.findall("arg")]
+ for name in ("wrench_topic", "cmd_topic", "failed_thruster"):
+ check(name in args, "main.launch 暴露参数 %s" % name)
+
+ check(os.path.exists(os.path.join(PKG, "README.md")), "README.md 存在")
+ for rel in (
+ "docs/water/rov_thruster_allocation_ros1.md",
+ "docs/img/water/rov_thruster_allocation_ros1/thruster_cmds.png",
+ "docs/img/water/rov_thruster_allocation_ros1/achieved_vs_desired.png",
+ "docs/img/water/rov_thruster_allocation_ros1/saturation_fault.png",
+ ):
+ check(os.path.exists(os.path.join(ROOT, rel)), "文件存在:%s" % rel)
+
+
+def main():
+ test_matrix()
+ test_normal_allocation()
+ test_saturation()
+ test_fault()
+ test_project_files()
+ print("\n==================== 结果 ====================")
+ print("通过 %d 项,失败 %d 项" % (len(PASS), len(FAIL)))
+ if FAIL:
+ for f in FAIL:
+ print(" FAIL: %s" % f)
+ return 1
+ print("全部通过")
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())