Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
1 change: 1 addition & 0 deletions docs/index.md
Original file line number Diff line number Diff line change
Expand Up @@ -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 定时推理,权重与类别表随包安装
Expand Down
187 changes: 187 additions & 0 deletions docs/water/rov_thruster_allocation_ros1.md
Original file line number Diff line number Diff line change
@@ -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 正常任务下的推进器指令

![推进器指令](../img/water/rov_thruster_allocation_ros1/thruster_cmds.png)

- 0~3 s 前进:四个水平推进器成对出力(T1/T2 为正、T3/T4 为负),合力沿 +x;
- 3~6 s 转向:对角推进器反向出力,产生 +Mz 力矩;
- 6~9 s 下潜抬头:T6 明显大于 T5,既提供垂向力又产生抬头力矩;
- 9~12 s 组合运动:水平与垂直推进器同时工作。

### 5.2 期望与可达 wrench

![期望与实际 wrench](../img/water/rov_thruster_allocation_ros1/achieved_vs_desired.png)

正常工况下(最大推力 40 N),实际可达 wrench 与期望值完全重合,
任务段最大分配残差约 $10^{-13}\%$(数值精度),说明伪逆分配是精确的。

### 5.3 饱和与故障对比

![饱和与故障残差](../img/water/rov_thruster_allocation_ros1/saturation_fault.png)

| 场景 | 相对残差 | 说明 |
| ---- | ---- | ---- |
| 正常 | ≈ 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)辅助,实验参数、运行结果
和图表均由作者实际运行核对;作者对本文内容负全部责任。
1 change: 1 addition & 0 deletions mkdocs.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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'
Expand Down
23 changes: 23 additions & 0 deletions src/water/rov_thruster_allocation_ros1/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
)
87 changes: 87 additions & 0 deletions src/water/rov_thruster_allocation_ros1/README.md
Original file line number Diff line number Diff line change
@@ -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` 模式会改变合力方向;
某个推进器故障后,其余推进器重新分配,仍能保持大部分自由度的控制能力。
Loading
Loading