学习笔记搜索
学习笔记 / 05｜模型驱动抗扰实验室：推导、C++ 验证与八推进器仿真

05.3｜从 C++ 小实验走到 Gazebo／ArduSub SITL／ROS 2

先修：Euler 离散更新、状态空间与离散模型。

零基础入口：这些软件各自替代现实中的哪一块

“仿真”不是一个程序的名字，而是用计算代替真实系统的某些部分。先辨清谁在算力、谁在算控制、谁只负责送消息，才知道实验通过意味着什么。

工具／对象现实对应与职责它本身不证明什么
本章独立 C++ 数值实验用简化方程推进状态，检查推导与离散实现不证明传感器、网络、三维物理已接通
Gazebo／Stonefish 物理仿真虚拟艇、推进器、流体／接触与配置的传感器模型逼真不等于参数就是实艇真值
ArduSub SITLsoftware in the loop：让飞控软件在普通计算机运行不是独立水下物理世界，也不是真实硬件时序验证
Gazebo 的 ArduPilot 插件连接飞控输入输出与物理世界插件装好不等于单位和编号正确
ROS 2多个程序交换带类型消息的通信框架不自动完成坐标转换、估计或安全停机
ros_gz_bridgeROS 与 Gazebo 消息类型及传输的适配不自动将牛顿变成速度，也不实现控制器
rosbag／CSV保存时间序列以便回放和评分记录存在不表示记录字段定义正确

节点 node 是执行某项工作的程序单元；话题 topic 是一类消息的命名通道；发布／订阅是发送方发消息、接收方监听通道。ROS 的节点和 A* 的图节点只是同名，不是同一个对象。真值 truth 是物理引擎内部状态；测量 measurement 是传感器能提供的有噪声、延迟、频率受限的信息；估计 estimate 才是观测器输出。

最小闭环应画成有回程的图，而不是只有向推进器发命令：

目标 → 控制器 → 力请求 → 分配器 → 实际八路推力 → 虚拟艇物理
        ↑                                          ↓
        └──── 状态估计 ← 测量适配器 ← 配置的虚拟传感器

物理真值 ───────────────→ 评分器（不偷偷接入估计器）

若先做合成位置传感器，允许把真值送入独立的测量生成模块，该模块明确加入噪声、延迟和采样；必须把实验标注为合成位置测量，而不是声称现有 DVL 已测到绝对位置。评分器仍保留无噪声真值作为对照。

SITL 路线则把“控制器＋分配器”交给飞控：外部程序只给飞控所支持的运动参考，飞控输出执行信号，插件让虚拟艇运动，虚拟传感器再把反馈送回飞控。若外部控制器同时直接发布推力，相当于两个人同时握方向盘；必须明确唯一拥有者。

仿真时间按虚拟世界步进，可能比墙上时钟快或慢，也可能暂停。仿真暂停时，不能用墙钟经过的两秒去积分虚拟运动；重置时钟后，不能保留旧时间戳和旧积分。先检查时间、符号、单位、唯一发布者，再看曲线是否漂亮。

对应在线练习： 延迟缓存 练同一时刻的测量对齐，PID 练离散控制状态；这些只是单元练习，均不代表 ROS／SITL 联调通过。本页后面的环境状态和分层验收仍是完成判断依据。

先看实验边界

本章已经在当前环境编译运行了纯 C++ 实验、九组对照、分配矩阵和 Lyapunov 回归测试。 当前环境没有 Gazebo、ROS 2 或 Docker，因此下述完整仿真是源码核对后的实施路线，尚未完成本机联调。先把可独立验证的控制数学做实，再逐层接线，不把“模型能加载”说成“抗扰系统已验证”。

第一条路线选 Gazebo Harmonic：它既能让自写 C++ 控制器直接给八个推进器发牛顿推力，也能接官方 ArduPilot JSON 插件。ROS 2 使用 Ubuntu 24.04＋Jazzy＋Harmonic 的官方配套组合；这是为新工作区选择的环境，不是上游旧 Dockerfile 的复刻。Gazebo 官方配套说明

1. 固定哪些版本，为什么不盲目“全部 latest”

部件本章基线／检查项
BlueROV Heavy 模型4451a5d1b15d204581d43ef7d93df4a0508eddbb，包版本 0.7.0
GazeboHarmonic，库主版本 gz-sim8；安装后记录具体 Debian 包版本
ArduPilot Gazebo 插件上游模型 Dockerfile 所用 4b30a3d8ac2276fb590a28f562304cf49fee4ca3，先作为历史匹配基线
ROS 2Jazzy；ros_gz_bridge 使用对应发行版包，不混用 Rolling
ArduSub独立 SITL 源码工作区；上游 Dockerfile 未锁 ArduPilot SHA，所以必须在本机首次构建前记录所选 SHA，联调后保留，不宣称已有整栈复现锁
实艇原有 ArduSub 4.1.1、参数和配置不改动

上游 Dockerfile 使用 Ubuntu 22.04 和 Harmonic，不含完整固定版本的 ROS 2 Jazzy 环境。这里不直接执行其会修改系统的大型安装脚本。先按官方安装说明准备独立虚拟机或学习环境；源代码构建与系统包安装分开审核。插件固定提交若与新环境 API 不兼容，应根据编译错误选择另一个固定提交并写入清单，不能静默切到 main 后仍说版本相同。

模型 package.xml 声明 MIT；本课程没有重新分发第三方网格和纹理。学习工程只带原创 C++、数值参数说明与上游引用，第三方模型从原仓库获取并保留原始许可信息。

建立独立源码目录

以下命令仅操作学习工作区；依赖准备好后再运行。clone 不是安装，不执行仓库的脚本。

mkdir -p ~/rov-model-lab/src
cd ~/rov-model-lab/src
git clone https://github.com/clydemcqueen/bluerov2_gz.git
git -C bluerov2_gz checkout 4451a5d1b15d204581d43ef7d93df4a0508eddbb
git clone https://github.com/ArduPilot/ardupilot_gazebo.git
git -C ardupilot_gazebo checkout 4b30a3d8ac2276fb590a28f562304cf49fee4ca3
export GZ_VERSION=harmonic
cmake -S ardupilot_gazebo -B ardupilot_gazebo/build \
  -DCMAKE_BUILD_TYPE=RelWithDebInfo
cmake --build ardupilot_gazebo/build -j2

插件的依赖与构建方式见 官方 ArduPilot Gazebo 仓库。不要把另一款 Gazebo Classic 的插件混进 Harmonic 的路径。

cd ~/rov-model-lab
{
  date -u
  git -C src/bluerov2_gz rev-parse HEAD
  git -C src/ardupilot_gazebo rev-parse HEAD
  gz sim --versions
  printenv ROS_DISTRO
  dpkg-query -W 'ros-jazzy-ros-gz*' 'libgz-sim8*'
} > versions.txt

2. 两条控制链只能选一条占用推进器

路线 A：验证自己设计的控制器

Gazebo plant → 独立传感器仿真/时间戳 → 估计器 → 状态反馈
                  ↑                                  ↓
             真值只供评分                    六维力/力矩 → 有界分配
                                                      ↓
                                             八路 cmd_thrust（N）

这个实验不让 ArduSub 再控制同一组推进器。使用学习副本移除模型内的 ArduPilotPlugin，保留浮力、流体动力、八个 Thruster 插件；不要只是不启动 SITL，却保留锁步插件等它发消息。最稳妥的起步是复制整个模型／world 到单独资源目录，并让副本 world 的模型 URI 指向副本；这样不会污染版本锁定的原模型。

路线 B：验证飞控与仿真接口

ROS/地面站参考 → ArduSub SITL（内部控制+分配）
                         ↓ JSON/servo
                 ArduPilot Gazebo 插件 → 八路推进器 → Gazebo

这里推进器由飞控控制；外部算法首先做参考生成或观测研究。若要把本课六维力控制器真正换进飞控内环，需要明确修改飞控的控制接入点、饱和和分配逻辑，并重新验证，不能把牛顿值当 RC 通道值发送。MANUAL_CONTROL、RC override、速度目标、姿态目标、PWM、N 是不同层的接口。

3. 先验证单推进器方向与单位

设置资源路径，启动路线 B 的原始 world；路线 A 则将路径改为移除 ArduPilot 插件后的学习副本。

export ROVLAB="$HOME/rov-model-lab"
export GZ_SIM_RESOURCE_PATH="$ROVLAB/src/bluerov2_gz/models:$ROVLAB/src/bluerov2_gz/worlds"
export GZ_SIM_SYSTEM_PLUGIN_PATH="$ROVLAB/src/ardupilot_gazebo/build"
gz sim -v 3 -r bluerov2_heavy_underwater.world

下面的单推测试只在路线 A 中运行，不与 SITL 同时争用。

gz topic -l
gz topic -t /model/bluerov2_heavy/joint/thruster1_joint/cmd_thrust \
  -m gz.msgs.Double -p 'data: 1.0'
# 观察短时运动，然后归零；停止发布不等于自动归零。
gz topic -t /model/bluerov2_heavy/joint/thruster1_joint/cmd_thrust \
  -m gz.msgs.Double -p 'data: 0.0'

SDF 的 use_angvel_cmd=False 表示命令是有符号推力 N；velocity_control=true 控制的是桨叶关节转速实现方式，不把外部命令改成艇速度。该语义既见 Thruster API，也可在 Thruster.cc 的 OnCmdThrust 与 PreUpdate 查到：接收标量推力，再沿 joint 世界方向施力。

逐个给八个推进器相同的短小正脉冲，记录初始线加速度和角加速度方向，与上一课 BfB_f 的各列比较。只在符号、编号、单位一致以后才跑闭环。不同模型的推进器编号不能靠视觉位置猜。

4. 接 ROS 2：先桥接，不马上上复杂节点

在已安装 Jazzy 与对应 ros_gz_bridge 的环境，先桥接时钟与单个推力话题：

source /opt/ros/jazzy/setup.bash
ros2 run ros_gz_bridge parameter_bridge \
  '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock' \
  '/model/bluerov2_heavy/joint/thruster1_joint/cmd_thrust@std_msgs/msg/Float64]gz.msgs.Double'

[ 是 Gazebo→ROS，] 是 ROS→Gazebo；用单引号保护 shell 特殊字符。时钟只由仿真发布。若实际只有 /world/<worldname>/clock，先用 gz topic -l 确认，再改桥接配置映射到 ROS /clock。八路输出的 YAML 配置已在配套 sim-lab/bridge.yaml 中列出。下面的桥接命令须在下载课程项目的根目录执行（这里有 sim-lab 子目录），而不是上面的第三方源码目录。

ros2 run ros_gz_bridge parameter_bridge --ros-args \
  -p config_file:="$PWD/sim-lab/bridge.yaml"
ros2 topic pub --once \
  /model/bluerov2_heavy/joint/thruster1_joint/cmd_thrust \
  std_msgs/msg/Float64 '{data: 1.0}'
# 完成短脉冲后必须同样发布 data: 0.0。

std_msgs/msg/Float64 ↔ gz.msgs.Double、单向桥接和 YAML 配置来自 Jazzy 分支 bridge 文档。这只是消息转换，不负责 ENU/NED、单位转换、状态估计或命令超时保护。

C++ 节点应拆成哪些边界

保持上一课数学核心为无 ROS 依赖的 C++ 类，再在外层封装节点：

  1. MeasurementAdapter：从真实存在的话题构造带仿真时间戳的测量，做坐标转换、噪声、延迟、降采样与掉线。不要虚构一个“默认已有 DVL”的 topic。
  2. Estimator：仅接收测量和已执行输入，输出状态、扰动估计、有效标志。开始阶段单轴，后续才扩到 12／18 状态。
  3. Controller：根据目标与估计计算期望力／力矩；加速度输出先乘有效质量，不能直接送入 N 接口。
  4. Allocator：把六维请求分配到八路推力，实施幅值／变化率限制，并把实际合力反馈给估计器。
  5. Recorder：分别记录 truth、measurement、estimate、request、applied；真值通道不得连回正常控制路径。

每个节点 use_sim_time=true。控制周期来自仿真时间，不用墙钟睡眠假装同步。时间回退或 world reset 时，清空积分、观测器、滤波缓存与上一条命令；未收到新测量、输入非有限值、测量超龄时，进入明确的仿真安全输出状态。实际项目应由推进器接收端实现超时归零，不能指望崩溃的控制节点自己发停止命令。

对第一轮教学位置测量，可从仿真位姿真值经独立噪声／延迟模块生成，再用于本课位置观测器；实验名必须标注“合成位置传感器”。第二轮替换成 DVL＋IMU＋深度时，重新选择可观测任务：速度保持、深度保持或增加外部位置约束后的定位，不能继续把仿真真值冒充传感器融合结果。

5. 接通 ArduSub SITL 的最小检查

ArduPilot 的源码与依赖按 官方 SITL 开发文档 在独立目录准备。首次启动前把该目录的 git rev-parse HEAD 与参数备份写入版本清单。下面是本模型 README 对应的 Heavy 帧命令，将输出地址收敛为本机回环：

cd ~/rov-model-lab/src/ardupilot
Tools/autotest/sim_vehicle.py -L RATBeach -v ArduSub \
  -f vectored_6dof --model=JSON \
  --out=udp:127.0.0.1:14550 --console

原模型 ArduPilot 插件 fdm_addr=127.0.0.1、输入端口 9002、锁步开启；推进器 channel 0–7 映射到 thruster 1–8。只有本地仿真已连接且未接真实 MAVLink 设备时，才使用 MAVProxy 的解锁／小命令做仿真验证。不要把 -w 当普通启动参数：它会清除该 SITL 工作目录的参数。

验收先看消息和时钟：SITL 是否收到传感器、仿真是否推进、servo 是否八路、neutral 是否零推力。然后才在仿真里检查上浮、前进、偏航及姿态方向。发生异常时优先查编号、坐标、neutral 和单位，而不是先调 PID。

6. 一个完整实验回合的定义

每回合先重置 world、SITL 参数状态与控制器内部状态，使用相同初始深度／姿态／速度和相同扰动序列。按下面阶段逐步增加难度：

阶段只改变的因素必须通过的判断
几何单推小脉冲八列合力方向与 BfB_f 一致
模型无扰动、小范围运动模型预测残差量级可解释，符号无误
感知合成位置测量、固定噪声不使用真值闭环；时间戳与坐标一致
控制PD、PID、DOB 同任务同限幅常扰下理论静差与实验吻合；PID 允许公平调参
鲁棒性阻尼／质量失配、慢变外扰跟踪误差与输入代价同时报告
限制饱和、延迟、掉线不可行任务能被识别，不以积分爆炸“坚持”
SITL飞控内环接管，外部只给参考无第二个推进器发布者，模式／参数有记录

在 Gazebo 中施加扰动时，优先实现一个明确的仿真外力插件或使用该版本 world 已配置的 wrench 系统；确认作用对象、坐标和作用点。不要把扰动直接加到目标值上，那测试的是参考跟踪，不是外力抗扰。海流通过相对流速改变阻力，是另一种模型，不等同于恒定外力。

存档至少包括模型与插件 SHA、固件 SHA、参数、随机种子或确定性信号、仿真步长、控制周期、测量延迟、饱和上限，以及 rosbag／CSV。指标定义为

RMSEp=1N∑k=1N∥pk−pref,k∥2,Jf=h∑k=1N∥fk∥2.\mathrm{RMSE}_p=\sqrt{\frac1N\sum_{k=1}^{N}\|p_k-p_{ref,k}\|^2},\qquad J_f=h\sum_{k=1}^{N}\|f_k\|^2.

NN 是评分样本数，hh 是采样间隔，fkf_k 是实际推进器推力而非请求推力。JfJ_f 只是推力平方代价，不是电能；要报告能耗需要电机／功率模型或电流电压数据。位置误差不能与角度误差直接无权相加。

7. Stonefish 是可行备选，不是同时开两套工程

若目标更偏水下传感器与环境，备选是 bvibhav/stonefish_bluerov2，本次检查提交 6448383af6b7ef6083b0eac2c08102660591e318。实际 scenarios/bluerov2.scn 有八个 thruster actuator；不能被文件后部仍残留的“6 thrusters”注释误导。文件声明 DVL、IMU、压力、相机等传感器和 /$(arg vehicle_name)/setpoint/pwm 订阅。这个名称不证明消息数值就是微秒 PWM，需沿适配器与 wrapper 回调继续核对单位。

该仓库 README 指定 Ubuntu 24.04／Jazzy、Stonefish 与 ROS 2 wrapper 都取 v1.3；launch/bluerov2_sim.py 加载 tank 场景，设 100 Hz 仿真，并启动 ardusim_patch.py。这意味着其现成 SITL 接桥含 Python；自己的估计器／控制器仍可全部 C++，但不能把整条现成工具链宣传为纯 C++。

选择这条路线时，先分别固定库、wrapper 和场景提交，并核对 Stonefish ROS 2 官方 wrapper 的同版本要求。最小构建／启动形态是：

# 前提：匹配版本的 Stonefish 库及 wrapper 已按官方文档构建。
cd ~/stonefish_ws
colcon build --symlink-install --packages-select stonefish_bluerov2
source install/setup.bash
ros2 launch stonefish_bluerov2 bluerov2_sim.py

不要混用 Stonefish 的几何、Gazebo 的推进器编号与另一份论文的质量矩阵。它们都是 BlueROV2 系列，不代表数值模型相同。先完成一条路线的验收，再用第二个仿真器做跨模型验证，这才有研究价值。

8. 论文怎么读才能真的指导设计

推荐 Cao 等人的 A Nonlinear Model Predictive Controller for Remotely Operated Underwater Vehicles With Disturbance Rejection（IEEE Access，2020）；可读 作者稿全文。它使用六自由度动力学描述，但文中艇为六推进器、四个主动控制自由度，不是本章 Heavy 的八推六轴基线。其 NMPC 接口讨论已测扰动；本章三状态观测器是独立教学推导，不能冒称论文已提供同一 DOB。

阅读顺序：先看动力学和推进器几何，再看归一化输入怎样变成推力，然后看控制目标、约束与评估条件。每读一部分，回到自己的工程回答一个问题：

  1. 哪些参数由结构给定，哪些经过辨识？换艇后哪些矩阵必须重算？
  2. 输入是归一化命令、力还是力矩？是否包含推进器非线性？
  3. 算法每周期实际获得什么状态和扰动信息？你的传感器能否提供？
  4. 比较对象是否使用同一输入约束、测量信息、扰动和初值？
  5. 仿真、水池实验、硬件在环各验证了什么，尚未验证什么？

tsaoyu/ORCA_control 可以辅助查看旧 ROS 控制工程，但其 actuators.xacro 实际是六个推进器，README 也将高级控制指向另一项目；不要把它当作本章 NMPC、八推、ROS 2 的一键复现包。

你现在应能清楚回答：“我的控制器稳定”究竟指哪个模型、哪个测量假设、哪个输入范围、哪组参数；也能先在小实验中反驳错误，再把同一套逻辑逐层接到真实仿真接口上。这比从一开始就堆一个无法解释的复杂控制器更接近可交付工程。

本章编程练习

查看全部练习

仅保存在当前浏览器，可在学习记录页导出。