gz-sim 水下机器人控制实战：LRAUV 速度、偏航与俯仰的 PD/P 控制器解析

• 机器人

本文以 gz-sim（Gazebo Sim）仓库中的examples/standalone/lrauv_control示例为核心，深入讲解如何为 MBARI Tethys LRAUV（长航程自主水下航行器）搭建一个解耦的速度—偏航—俯仰控制器：从世界文件的系统插件配置，到基于 gz-transport 话题的控制器源码（C++ 与 Python 双版本），再到完整的构建、运行与输出判读流程。读完本文，你将掌握在 gz-sim 中通过cmd_thrust、cmd_pos等标准话题驱动水下航行器、并利用里程计话题闭环反馈的完整实战方案。

一、示例概览：一台有三个作动器的 LRAUV

本示例演示一个针对 LRAUV 的简单闭环控制器。示例车辆（模型名为tethys）配备三个作动器：

• 推进器（thruster）：控制前进速度；
• 垂直尾翼舵（vertical fins）：控制偏航角（yaw）；
• 水平尾翼舵（horizontal fins）：控制俯仰角（pitch）。

控制器的目标状态是三个量：速度（m/s）、偏航角（rad）、俯仰角（rad）。控制器需要实时调整舵面角度与推进器推力，使这三个状态逼近目标值，属于典型的多输入多输出（MIMO）水下航行器控制问题。

原文档明确指出一个重要的工程近似：在真实系统中这是 MIMO 系统，三个作动器相互耦合——改变推进器推力会影响俯仰，转动舵面也会影响阻力与速度。但当需要改变的偏航、俯仰角不太剧烈时，可以将系统近似为三个独立的单输入单输出（SISO）回路来分别设计控制器。

据此，示例采用如下控制律（README 原始描述）：

• 速度通道：PD 控制器（按 lrauv_control.cc 源码，实际实现为比例 + 积分，即 PI 形式，见下文第五节的源码对照）；
• 偏航角通道：P 控制器；
• 俯仰角通道：P 控制器。

而闭环所需的状态反馈由OdometryPublisher系统插件提供，控制器通过订阅里程计话题获取当前速度、偏航角与俯仰角。

二、运行前置条件与整体流程

本示例需要分两步运行：先启动仿真世界，再单独编译并运行控制器可执行程序（独立于仿真进程，通过 gz-transport 话题通信）。

前置条件：

1. 已安装并编译 gz-sim 及其依赖（gz-transport、gz-msgs、gz-math 等）；
2. 已配置 Gazebo 工作空间（示例 README 假定为~/gazebo_ws/，仓库源码位于~/gazebo_ws/src/gz-sim/）；
3. 每个新终端都需要先 source Gazebo 工作空间，使gz命令行与相关库可用。

第一步：启动仿真世界

世界文件位于 examples/worlds/lrauv_control_demo.sdf（注意：它位于examples/worlds/而不是examples/standalone/下）。打开一个终端，source 工作空间后执行：

cd ~/gazebo_ws/src/gz-sim/examples/worlds gz sim -r lrauv_control_demo.sdf

-r参数表示启动后立即运行（不暂停）。执行后应弹出一个 Gazebo 窗口，LRAUV 静止位于原点附近（世界文件将模型<pose>设为0 0 1 0 0 0，见 examples/worlds/lrauv_control_demo.sdf）。由于WorldControlGUI 插件中start_paused设为true，若希望仿真从暂停状态开始，可在界面中点击播放；使用-r时则直接运行。

第二步：编译并运行控制器

打开一个新终端，source 工作空间，进入示例目录并构建：

cd ~/gazebo_ws/src/gz-sim/examples/standalone/lrauv_control mkdir build; cd build cmake ..; make

构建成功后，按以下用法运行：

# 用法：./lrauv_control speed_in_metres_per_sec yaw_angle_in_rad pitch_angle_in_rad ./lrauv_control 0.5 0.174 0.174

其中0.174rad ≈ 9.97°，因此这条命令让 LRAUV 以 0.5 m/s 前进，同时转向偏航约 10°、俯仰约 10°。源码头部注释中给出的另一组示例参数为./lrauv_control 0.5 0.78 0.174（偏航约 44.7°），可见三个参数单位分别是 m/s、rad、rad。

运行后车辆应开始运动并沿路径行进，同时控制器控制台会周期性打印三个状态各自的（目标值、当前值、误差）。此外，世界文件中的Plot3DGUI 插件（标题为 "LRAUV path plot"，跟踪tethys::base_link）会在界面中实时绘制车辆轨迹，这正是 README 所说 "tracing its path along the way" 的可视化来源，见 examples/worlds/lrauv_control_demo.sdf。

三、控制器的典型输出与状态判读

运行后控制台每隔约 200 ms 打印一次状态（README 中的示例输出，循环打印相同的两行格式）：

----------------------- States ( target, current, error) : Speed (m/s) : 0.50000 0.54543 -0.04543 Yaw angle (deg) : 9.96947 11.31291 -1.34344 Pitch angle (deg) : 9.96947 6.39717 3.57229 ----------------------- States ( target, current, error) : Speed (m/s) : 0.50000 0.54543 -0.04543 Yaw angle (deg) : 9.96947 11.31291 -1.34344 Pitch angle (deg) : 9.96947 6.39717 3.57229

解读要点：

• 目标偏航角0.174 rad打印时被转换为角度显示：0.174 * 180 / π ≈ 9.96947°，俯仰角同理（源码见 lrauv_control.cc）；
• error = target - current：速度误差 -0.04543 表示当前速度略超目标（0.545 vs 0.500）；偏航误差 -1.34° 表示转向略过；俯仰误差 +3.57° 表示俯仰角尚未到位，控制器仍在收敛过程中；
• 输出使用std::fixed与 5 位小数精度格式化（std::setprecision(5)），方便观测收敛过程。

四、世界文件解析：支撑闭环仿真的系统插件栈

控制器能闭环工作，依赖 examples/worlds/lrauv_control_demo.sdf 中配置的整套仿真系统。理解这些插件是掌握示例的关键：

1. 基础仿真系统

<plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics"/> <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/> <plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"/> <plugin filename="gz-sim-contact-system" name="gz::sim::systems::Contact"/>

物理引擎为 ODE，<max_step_size>0.001</max_step_size>（1 ms 步长）、<real_time_factor>1.0</real_time_factor>。

2. 水环境：浮力与水动力

<plugin filename="gz-sim-buoyancy-system" name="gz::sim::systems::Buoyancy"> <uniform_fluid_density>1000</uniform_fluid_density> </plugin> <plugin filename="gz-sim-particle-emitter-system" name="gz::sim::systems::ParticleEmitter"/>

Buoyancy以 1000 kg/m³（淡水密度）提供均匀浮力；ParticleEmitter用于渲染水下粒子效果。

3. 模型内嵌插件（位于<include>块内）

世界通过<uri>https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV</uri>从 Fuel 拉取 Tethys 模型，并在 include 块内为模型附加以下插件：

• OdometryPublisher（反馈来源）：为tethys模型发布里程计。从其实现 src/systems/odometry_publisher/OdometryPublisher.cc 可看到，默认话题构造规则为/model/<model_name>/odometry，对应本示例订阅的/model/tethys/odometry；同时还会发布/model/<name>/odometry_with_covariance与/model/<name>/pose（TF）。发布频率由<odom_publish_frequency>控制，默认 50 Hz（见 OdometryPublisher.cc）。控制器即从该话题的msgs::Odometry中读取位姿四元数与线速度。
• JointPositionController（舵面执行器）：两个实例分别控制horizontal_fins_joint与vertical_fins_joint，<p_gain>0.1</p_gain>。由 src/systems/joint_position_controller/JointPositionController.cc 可知，默认命令话题为/model/<model>/joint/<joint_name>/<joint_index>/cmd_pos，恰好对应控制器发布的/model/tethys/joint/vertical_fins_joint/0/cmd_pos与/model/tethys/joint/horizontal_fins_joint/0/cmd_pos（索引0表示该关节组中的第 0 号关节）。
• Thruster（推进器执行器）：<namespace>tethys</namespace>、<joint_name>propeller_joint</joint_name>、<thrust_coefficient>0.004422</thrust_coefficient>、<fluid_density>1000</fluid_density>、<propeller_diameter>0.2</propeller_diameter>、<use_angvel_cmd>0</use_angvel_cmd>（即采用推力指令模式而非角速度指令模式）。从 src/systems/thruster/Thruster.cc 可确认，ForceCmd 模式下默认订阅话题为/model/<ns>/joint/<joint_name>/cmd_thrust——正是控制器发布推力的/model/tethys/joint/propeller_joint/cmd_thrust。tutorials 中关于推力—转速关系的说明（见 tutorials/underwater_vehicles.md）提到，该插件接收牛顿单位的力指令，并依据 Fossen 教材中的公式换算转速。
• LiftDrag（舵面水动力）：两组，分别为垂直尾翼（<upward>0 1 0</upward>）与水平尾翼（<upward>0 0 1</upward>），配置升力系数cla 4.13、阻力系数cda 0.2、失速角alpha_stall 0.17、面积area 0.0244等，用于产生舵面转角的升/阻力。
• Hydrodynamics（水动力阻尼）：作用于base_link，配置了附加质量（xDotU、yDotV、zDotW、mDotQ、nDotR）与非线性阻尼（xUabsU、yVabsV、zWabsW、mQabsQ、nRabsR）系数，使航行器获得贴近真实的水中运动特性。

五、控制器源码剖析：话题、状态更新与控制律

核心实现是 examples/standalone/lrauv_control/lrauv_control.cc（约 225 行），逻辑清晰，可拆解为四部分。

1. 目标状态设置与合法性检查

Controller::SetTargets(speed, yaw, pitch)接收命令行三个参数。值得注意的是其中有一个物理约束检查（lrauv_control.cc）：

if (_speed == 0 && (_yaw != 0 || _pitch != 0)) { std::cout << "Speed needs to be non zero for non zero" " pitch and yaw angles" << std::endl; return; // 拒绝设置 }

即速度为零时不允许设定非零姿态角——静止航行器没有前进速度时，舵面无法产生有效的操纵力矩，这一检查体现了对水动力原理的正确建模。

2. 话题发布与订阅（gz-transport 通信层）

控制器创建一个gz::transport::Node，共建立 1 个订阅、3 个发布：

话题字符串均通过gz::transport::TopicUtils::AsValidTopic规范化（见 lrauv_control.cc）。

里程计回调将Odometry中的四元数转为gz::math::Quaterniond后取欧拉角（q.Euler()，其中rpy[2]为偏航、rpy[1]为俯仰），同时用线速度三分量的模长（velocity.Length()）作为当前速度标量（lrauv_control.cc）。

3. 状态更新与误差计算

UpdateState(speed, yaw, pitch)在互斥锁保护下刷新当前状态并计算三个误差通道（lrauv_control.cc）：

errorSpeed = targetSpeed - speed; errorSpeedIntegral = std::min(errorSpeedIntegral + errorSpeed, errorSpeedIntegralMax); errorYawAngle = targetYawAngle - yawAngle; errorPitchAngle = targetPitchAngle - pitchAngle;

其中速度误差的积分项被限幅到errorSpeedIntegralMax = 10，防止积分饱和（wind-up）。

4. 控制律与增益参数

控制输出由三个独立函数生成（lrauv_control.cc）：

// 速度通道 double SpeedControl() { return errorSpeed * kSpeed + errorSpeedIntegral * kISpeed; } // 偏航通道 double YawControl() { return errorYawAngle * kYawAngle; } // 俯仰通道 double PitchControl() { return errorPitchAngle * kPitchAngle; }

对应增益定义（lrauv_control.cc）：

一处值得注意的文档与实现差异：README 将速度控制器描述为 "PD controller for speed"，但源码实际采用P + I（比例-积分）形式——缺少微分项，且积分带限幅。读者应以源码实现为准，并可将速度通道理解为 PI 控制器。这不妨碍整体控制策略（P 型角度通道 + PI 型速度通道）的理解。

5. 控制循环与输出

主循环每 200 ms 执行一次（std::this_thread::sleep_for(std::chrono::milliseconds(200))，见 lrauv_control.cc）：依次发布推力、偏航舵、俯仰舵三条指令，随后打印状态表。三个发布器始终处于发布状态，即使目标未变也会持续输出，保证仿真端执行器不断收到最新指令。

6. CMake 构建配置

构建文件 examples/standalone/lrauv_control/CMakeLists.txt 展示了独立控制程序所需的依赖链：

find_package(gz-transport QUIET REQUIRED OPTIONAL_COMPONENTS log) find_package(gz-sim REQUIRED) add_executable(lrauv_control lrauv_control.cc) target_link_libraries(lrauv_control gz-transport::core gz-sim::gz-sim)

即该独立程序依赖gz-transport::core（话题通信）与gz-sim::gz-sim（提供 gz::math 等接口）。

六、Python 版本：同一控制器的跨语言实现

同目录还提供了一份功能对等的 Python 实现 examples/standalone/lrauv_control/lrauv_control.py，适合快速原型验证。两者在结构上完全对应：Controller类、增益值、话题名、200 ms 周期均一致。

运行方式（源码头部注释）：

export PYTHONPATH=$PYTHONPATH:<path_to_gazebo_ws>/install/lib/python python3 lrauv_control.py 0.5 0.78 0.174

Python 版通过gz.msgs.double_pb2.Double、gz.msgs.odometry_pb2.Odometry、gz.math、gz.transport等 Python 绑定实现同样的话题收发（见 lrauv_control.py），姿态角换算使用q.yaw()/q.pitch()，控制循环同样休眠 0.2 秒。两个版本可任选其一，与仿真端完全兼容。

七、适用边界与进一步探索

原文档在结尾明确给出适用性警告（Note）：本示例仅用于教学演示，控制器在高速或目标角度突变时可能表现不佳。原因正是第一节提到的 SISO 解耦近似——当偏航/俯仰变化剧烈时，三通道间的耦合（推力—俯仰力矩耦合、舵面阻力变化等）不再可忽略，P/PI 控制器无法补偿。若要在该场景下获得更好的性能，需要考虑 MIMO 控制器或引入前馈补偿。

如果你想继续深入这一主题，仓库中还有直接相关的资源：

• examples/standalone/multi_lrauv_race/README.md：多台 LRAUV 竞速示例，展示通过命名空间区分多台同型航行器，用伪随机推力/舵指令驱动多机运动，可对比单机控制与多机控制的话题组织方式；
• tutorials/underwater_vehicles.md：系统讲解水下航行器仿真的完整链路——Thruster 插件（含推力—转速公式）、<fluid_added_mass>附加质量、Hydrodynamics 阻尼系数标定、LiftDrag 舵面建模，是理解lrauv_control_demo.sdf中每个数值来源的最佳参考资料；
• src/systems/odometry_publisher/OdometryPublisher.cc、src/systems/thruster/Thruster.cc、src/systems/joint_position_controller/JointPositionController.cc：三个核心系统插件的实现，可帮你精确推导任意模型名/关节名下的话题路径规律，从而将本示例的控制框架迁移到你自己的水下航行器模型上。

• 机器人