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 正常任务下的推进器指令 + +![推进器指令](../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)辅助,实验参数、运行结果 +和图表均由作者实际运行核对;作者对本文内容负全部责任。 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())