简介这是一份基于Python与ROS Noetic的石鱼生态仿真项目资源包面向研究海洋生物行为、机器人仿真及多智能体系统的开发者与学生。包内共223个文件主体为205个obj三维模型覆盖石鱼、锚链、岩石等场景物件并包含4个py行为控制脚本、3个scn场景文件、launch启动配置及yaml参数文件可支撑从环境搭建到仿真运行的全流程。资源压缩包仅1.7MB组件紧凑适合快速部署实验。项目模拟了石鱼的伪装、捕食、繁殖与防御等生物习性并融入物理碰撞、环境交互等多学科知识obj模型可自由替换场景物件scn文件预设布局launch与yaml简化ROS节点启动可作为学习ROS通信机制、行为建模或虚拟生态研究的参考实现。目前已有179人学习下载对希望结合机器人框架开展生物模拟研究的入门者尤为实用。1. 石鱼的世界模拟它是水下机器人的虚拟海试场水下机器人的真机海试是典型的高成本环节调一次船时、吊放、布放遇到推进器故障可能就是几万块的损失而且同一条航线很难原样复跑第二遍。石鱼的世界模拟smarc_stonefish_sims把这套成本压缩到一台工作站上它基于Stonefish物理引擎把水下机器人、海床高度图、流体阻力、推进器推力曲线以及声呐、DVL、IMU、压力计等传感器全部装进同一个仿真世界并保留了完整的Python控制接口。它能解决三层问题一是让控制算法在虚拟海床上先跑通不用把没验证过的代码直接带上船二是用脚本批量注入不同水流和地形把定位、定深算法的边界试出来三是给传感器融合提供一套参数可调的虚拟数据源方便调滤波器。适合做AUV/ROV控制、导航或传感器融合的开发者也适合需要给水下算法做回归测试的一线工程师。下文按场景怎么跑起来 → 传感器怎么接 → 控制怎么写 → 坑在哪 → 怎么当回归工具用的顺序拆开讲。2. 场景与物理模型先搞清楚石鱼在模拟什么这套资源的底座是Stonefish模拟器。一说水下机器人仿真很多人先想到Gazebo搭配uuv_simulator但水下环境有很多通用物理引擎不仔细处理的量浮力随深度和盐度变化、流体对刚体的阻尼、推进器正反转推力差异、声线在水层里的弯曲。Stonefish的设计目标就是把这些量当成一等公民来建模。smarc_stonefish_sims选择它一是因为传感器链路保真度高二是因为它有现成的推进器求解和流场描述不需要自己在插件里硬算流体。2.1 选型理由为什么是Stonefish而不是GazeboGazebo不是不能做水下而是要做的事情太多。浮力要靠插件给每个link附加向上力阻力要按速度方向反推系数声呐基本是自写raycast或者接第三方包。这种做法的本质是把物理上耦合在一起的量拆成独立插件参数之间互相牵扯。某团队用Gazebo调好的AUV定深控制拿到水池里开环漂了半米查到最后是水阻系数给得过于象征性。Stonefish把流体静力学和流体动力学做进刚体求解器每个具有体积的link自动获得阿基米德浮力和与速度相关的阻尼推进器使用四象限推力曲线正转和反转分别标定声呐用GPU raycast模拟声波反射。代价是场景配置比Gazebo更讲究你得理解排水体积浮心流场在场景文件里怎么描述。smarc_stonefish_sims正好把这些配置做成了开箱即用的场景省掉了从零建模那一大段工作。2.2 场景配置XML拆解高度图、水体与波浪在Stonefish里一个世界的骨架写在scene XML里。smarc_stonefish_sims的场景描述通常包含三类配置地形高度图、水体物理参数、光照和波浪。我打开资源包后最先找的就是这三段。下面是从包里整理出的场景骨架字段名在不同版本间略有差异但结构是一致的!-- 场景配置节选以资源包内 bay.scene.xml 为骨架整理 -- cpp:scenario scene !-- 海床灰度高度图驱动source 指向 assets/terrain/ -- terrain typeheightfield sourceassets/terrain/bay_depth.png size300 300 origin0 0 -35/ !-- 水体与波浪 -- ocean water density1024 viscosity1.0e-3/ waves amplitude0.1 direction120 wavelength4/ !-- 定深实验的干扰流从 current 修改 -- current velocity0.2 0.05 0/ /ocean !-- 光照影响下水下光学相机的成像 -- light typesun intensity1.2 skyclear turbidity3/ /scene /cpp:scenario这段配置的逻辑是terrain用一张灰度图生成实际海底source指向包内的高度图资源size控制地图覆盖范围ocean里的water决定浮力计算基准waves影响水面光学成像current是干扰流入口定深抗流实验就从这里改。光照参数对激光类传感器影响不大但如果你是做视觉SLAM的turbidity和intensity直接影响水下相机图片的对比度。参数常见量级改了影响什么water density10001030 kg/m³浮力与排水体积计算current velocity01.0 m/s抗流实验的扰动条件waves amplitude00.5 m水面成像与近水面控制terrain source / size数百米范围地形覆盖面积与分辨率改动某个参数后一般只需要重新roslaunch就会生效。但要注意如果你在运行时已经加载了场景修改XML后在同一个进程里不会热更新必须重启仿真进程。2.3 机器人模型挂载推进器曲线与浮力配置场景里光有地形和水体还不够真正的核心是把机器人模型挂进去。smarc_stonefish_sims里机器人本体是用URDF或者SDF描述的Stonefish通过插件读取link的体积信息把它换算成浮力和附加质量。常见做法是在URDF里定义每个link的质量和惯量然后在插件配置里单独给出排水体积和浮心位置两者加在一起就是完整的水下刚体。推进器是这套资源配置里最值得读的部分。Stonefish的推进器模型用四象限曲线描述正转和反转的推力-转速关系不是简单乘一个系数。资源包里每个推进器节点会带有最大推力、正反转曲线参数和安装坐标系。我第一次跑的时候没注意坐标系垂推装在机架坐标系Z轴朝下我的控制代码却按朝上发指令结果机器人直接往海底钻这是很容易踩的坑。传感器插槽也在模型文件里定义。声呐、DVL、IMU、压力计各自挂在不同的link上仿真器按安装位置生成数据。你不需要在Python侧做任何坐标系变换话题里发布出来的就是以该传感器安装点为原点的量。后续做传感器融合时只需要看包内的tf树就能把各个传感器的安装偏差对齐。2.4 用launch文件把最小场景跑起来这一节的目标是让场景窗口出现在你屏幕上。以ROS1为例整个流程是编译工作区、启动launch、确认节点# 假设已经按资源包说明把依赖装好放在 ~/smarc_ws 下 cd ~/smarc_ws catkin_make source devel/setup.bash # 启动bay场景headless:false 表示显示仿真窗口 roslaunch smarc_stonefish_sims bay.launch headless:false看到水下场景窗口之后新开终端确认节点和话题都起来了source ~/smarc_ws/devel/setup.bash rosnode list rostopic list | grep -E sonar|dvl|imu|pressure如果rostopic列表里能看到带这些关键字的主题说明仿真世界和机器人模型都加载成功。headless参数建议第一次别设成true因为你需要用眼睛确认机器人没有沉进地形、没有乱翻。从第二次起批量跑实验就可以headless:true了这个我放到最后一章讲。Python控制入口在下一章开始接入仿真跑起来之后传感器数据已经在话题上流动了。3. 传感器数据链路从仿真器到Python回调仿真器一半的价值在传感器。smarc_stonefish_sims里给你预备好了多种水下传感器数据通过ROS话题发布Python节点可以直接订阅。这些数据不是看起来像就完了关键是消息类型、字段含义和噪声参数要跟真实硬件对得上否则你调完的融合算法依然不敢上真机。3.1 传感器话题一览声呐、DVL、IMU与压力计资源包里常见的话题和消息类型大致如下具体话题名前缀以你拿到的启动文件为准话题消息类型关键字段典型仿真频率/imu/datasensor_msgs/Imuorientation, angular_velocity, linear_acceleration100400 Hz/pressuresensor_msgs/FluidPressurefluid_pressure50 Hz/dvl/velocitygeometry_msgs/TwistWithCovariance对地速度与协方差10 Hz/sonar/pointssensor_msgs/PointCloud2海底反射点云530 Hz如果包里的声呐用的是自定义消息类型先跑一句rosmsg show 包名/消息名看清楚了再写订阅代码。这块不建议蒙。我见过有人按PointCloud2的接口写完跑起来发现声呐话题根本不对后来查消息定义才发现是自定义类型白白耽误一晚上。传感器频率决定你的控制周期能取多少。IMU是高频基础量DVL低频但提供速度观测声呐点云数据量大不能所有东西都在同一个回调里做重处理。做定位融合时通常把IMU当预测源DVL和压力计当修正源声呐点云单独做特征提取。先把这些话题跑通看数据再谈融合。3.2 噪声与标定参数别让虚拟数据太干净默认参数下仿真器的IMU、DVL数据往往比真机干净得多。真机IMU有温漂和零偏DVL在近底或浑浊水里有随机丢包压力计有量化噪声。如果直接用理想数据调滤波器到了真机上协方差矩阵大概率要重新调。所以资源里几乎每个传感器都有噪声参数常见的位置在传感器插件的xml或yaml配置里。我一般会把这些参数设到与手册标称值同量级IMU角度随机游走按10 μg/√Hz量级给DVL的速度噪声给0.01 m/s标准差压力计给0.1 mbar量级的量化误差。声呐的噪声要小心给得太大点云会很散给得太小融合算法会过度信任声呐。做法是先跑一段静水数据记录下来看看话题输出值的抖动幅度是否符合预期。如果IMU静止数据的标准差接近0说明噪声被关掉了这时候就要去检查传感器配置里的noise相关参数。记住一个原则仿真数据是拿来验证算法鲁棒性的不是拿来证明传感器很准的。3.3 用Python订阅并多路录制数据接下来用一个Python脚本把IMU、DVL、压力计和声呐同时订阅下来存成ROS bag和numpy文件方便后面做离线分析。下面是完整脚本骨架#!/usr/bin/env python3 import rospy import rosbag import numpy as np from sensor_msgs.msg import Imu, PointCloud2 from geometry_msgs.msg import TwistWithCovariance from sensor_msgs.msg import FluidPressure bag rosbag.Bag(sea_trial.bag, w) depth_history [] imu_history [] def imu_cb(msg): # 高频话题攒批写入避免回调内频繁写盘 bag.write(/imu/data, msg, trospy.Time.now()) imu_history.append((msg.header.stamp.to_sec(), msg.linear_acceleration.z)) def dvl_cb(msg): bag.write(/dvl/velocity, msg, trospy.Time.now()) def pressure_cb(msg): bag.write(/pressure, msg, trospy.Time.now()) # 由压力换算深度p rho * g * h depth_history.append((msg.header.stamp.to_sec(), msg.fluid_pressure / 9806.65)) def sonar_cb(msg): # 声呐点云量太大只按帧抽帧记录 if msg.width * msg.height % 128 0: bag.write(/sonar/points, msg, trospy.Time.now()) rospy.init_node(data_logger) rospy.Subscriber(/imu/data, Imu, imu_cb) rospy.Subscriber(/dvl/velocity, TwistWithCovariance, dvl_cb) rospy.Subscriber(/pressure, FluidPressure, pressure_cb) rospy.Subscriber(/sonar/points, PointCloud2, sonar_cb) rospy.spin() bag.close() np.savez(pressure_profile.npz, t[x[0] for x in depth_history], depth[x[1] for x in depth_history])这个脚本的逻辑分三块高频量IMU和低频量DVL、压力计直接写入bag声呐这种大流量话题做抽帧避免回调内卡死最终把压力换算的深度单独存一份npz后面做深度控制效果评估直接读它。bag写入在回调里做虽然不优雅但对短时间实验足够用。深度换算用9806.65是默认海水密度如果你改了场景里的water density这里要同步改。跑完一段任务用rosbag info sea_trial.bag检查话题完整度。如果发现声呐话题只有几百帧大概率是抽帧条件写错了把msg.width * msg.height % 128 0改成% 256或者直接每N帧记一次都行。数据链路通了下一步就是在仿真里动推进器。4. 推进器控制与深度保持用Python搭一个定深闭环水下机器人最常用的基础功能就是定深。把场景跑起来、传感器数据能拿到之后下一步是用Python写一个完整的深度控制闭环。这个章节从推力分配开始到PID参数整定最后给出一个能直接复现的验证流程。4.1 推力分配从Twist期望速度到四推进器指令大多数紧凑型水下机器人用四个或六个推进器控制器通常输出六自由度期望力/力矩具体转成每个推进器的转速由推力分配模块完成。在smarc_stonefish_sims里推进器话题接收的是推力或转速指令常见做法是先做垂向和航向解耦。下面的代码给一个四推进器布局做推力分配两个水平推进器负责前进和转向两个垂推负责定深import numpy as np # 推进器布局矩阵每行是 [fx, fy, fz, mz] 上一个推进器的贡献系数 # 这里简化为平面布局h1/h2水平v1/v2垂直 A np.array([ [1.0, 0.0, 0.0, 0.6], # h1: 前进 偏航 [1.0, 0.0, 0.0, -0.6], # h2: 前进 - 偏航 [0.0, 0.0, 1.0, 0.0], # v1: 垂向 [0.0, 0.0, 1.0, 0.0] # v2: 垂向 ]) # 期望力/力矩 [fx, fy, fz, mz] tau_desired np.array([5.0, 0.0, 8.0, 0.0]) # 伪逆分配再截到推进器最大推力 thrust np.linalg.pinv(A) tau_desired thrust np.clip(thrust, -10.0, 10.0) print(四个推进器推力指令, thrust)分配矩阵A的含义是每个推进器对机体力/力矩的贡献方向伪逆是让总推力误差最小的常见解法。注意垂向两个推进器在矩阵里同列说明它们一起承担升力不会各自产生偏航。实际模型中推进器位置会造成额外力矩你需要按资源包里机器人模型的实际坐标重填这四行。推力饱和值10.0也要跟推进器最大推力对齐我给的是模拟值工程上以模型文件里的max_thrust为准。分配完成后把结果发布到推进器话题机器人就会有对应动作。4.2 深度PID垂推闭环的常规写法深度控制器最常规的就是PID。为什么不用更复杂的LQR或ADRC因为在这个阶段你要验证的是仿真链路通不通PID最容易观察问题。等PID在仿真里稳定了再上更高级的控制器也不迟。#!/usr/bin/env python3 import rospy import numpy as np from std_msgs.msg import Float64 from sensor_msgs.msg import FluidPressure class DepthPID: def __init__(self, kp3.0, ki0.2, kd1.5, limit10.0): self.kp kp self.ki ki self.kd kd self.limit limit self.err_sum 0.0 self.last_err None def compute(self, depth_ref, depth_meas, dt): err depth_ref - depth_meas self.err_sum err * dt # 积分抗饱和超过限幅就截断 self.err_sum np.clip(self.err_sum, -self.limit / self.ki, self.limit / self.ki) derr (err - self.last_err) / dt if self.last_err is not None else 0.0 self.last_err err out self.kp * err self.ki * self.err_sum self.kd * derr return np.clip(out, -self.limit, self.limit) rospy.init_node(depth_controller) pid DepthPID() pub rospy.Publisher(/thruster/v_vertical_cmd, Float64, queue_size1) rate rospy.Rate(50) pressure_msg rospy.wait_for_message(/pressure, FluidPressure) current_depth pressure_msg.fluid_pressure / 9806.65 depth_ref 20.0 # 目标深度 while not rospy.is_shutdown(): pm rospy.wait_for_message(/pressure, FluidPressure) current_depth pm.fluid_pressure / 9806.65 cmd pid.compute(depth_ref, current_depth, 1.0 / 50.0) pub.publish(Float64(datacmd)) rate.sleep()这个控制循环在50Hz运行深度由压力计换算得到和上一章日志脚本用的换算保持一致。PI两个参数决定了你跟踪深度的速度和静态误差kp太大容易在海底附近震荡ki负责消掉静差kd用来抑制超调。初次给参数时可以先kp3.0、ki0.2、kd1.5然后逐步加大kp观察是否出现周期震荡。调PID像玄学但仿真里有个好处你可以在几分钟内跑完真机需要反复布放十几次的流程。每次改参数后记录深度曲线看超调量和稳态误差比在水池里靠感觉调快两个数量级。积分抗饱和的代码必须保留否则深度误差大时积分项会堆到一个很大的值等机器人到深度了还会继续冲过头。4.3 加流场跑验证什么样的结果算合格闭环能跑之后要加上流场验证抗流性能。在场景XML里把current从0改成0.3 m/s重新启动仿真然后用脚本跑一条20米定深任务# 终端1启动带横流的场景 roslaunch smarc_stonefish_sims bay.launch headless:true current:0.3 # 终端2运行深度控制与数据采集 python3 run_depth_profile.py --depth 20 --duration 180 --save profile.csv判断结果是否合格我用三个标准一是稳态误差在无流下小于0.1米二是加水流后稳态误差小于0.2米三是整个过程中深度不允许超调超过0.5米。超过这个范围就要回头调PID的kd和积分限幅。注意current:0.3是launch参数的覆盖方式前提是launch文件里已经定义了这个参数否则命令行不生效得回XML里改。跑完把profile.csv画出来如果深度曲线是单调逼近没有震荡这套参数就可以作为基线。下一步就是去第五章节里的坑里看看你会遇到什么。5. 避坑指南把仿真跑出真实感的五个常见问题这个资源我拆过不止一次每次在换地形、换机器人配置时都会撞到几个相同的问题。下面五条是我按现象→原因→解决整理的踩坑记录每一条都花过不少时间才定位到根因。5.1 沉底弹跳不止甚至穿透地形现象机器人一接触海床就开始弹跳幅度越来越大严重时直接陷进地形网格里。这在仿真里比真机还难受真机至少不会反复穿地。原因Stonefish的接触求解对地形的刚度和仿真步长敏感。默认步长下接触点刚度太大一个步长内产生的接触力就把机器人弹起来。高度图地形本身也是一格一格的机器人接触在网格棱线上时更容易跳。解决先把仿真步长固定到400Hz附近接触刚度从默认值降到包内示例场景使用的推荐值然后检查高度图是否过于粗糙换一张更大尺寸的灰度图让海床起伏不要出现接近网格分辨率的尖角。如果你只是做定深实验就把初始位姿设在海床以上五米别让机器人在着底状态下启动。5.2 推进器指令发出去了机器人纹丝不动现象推进器话题有数据节点运行正常但机器人就是不动或者只动一个方向。原因最常见的是话题没对上。Stonefish的推进器话题不是简单的/cmd_vel而是一组这样的命名空间每个推进器单独一个话题而且subscribe的topic名字必须和模型文件里的一致。另一个原因是推进器安装方向反了发射正推力反而向反方向走。解决先用rostopic echo确认推进器话题有数据再用rqt_graph看节点连接。装了rviz的话打开TF和RobotModel检查推进器坐标系跟机体系是否一致。反了的话不要改代码里的符号去模型文件里把推进器安装方向改回来这样后续标定才不混乱。5.3 声呐点云漂移噪点严重现象声呐点云在每次扫描之间位置乱跳或者出现大量离群点看起来像雪花。同一块海底特征两次扫描对不上。原因声呐的raycast是在GPU上做的如果场景的网格没有标记物理材质或者材质属性缺失声波反射计算会异常。另一个原因是噪声参数给得过大某些仿真版本里声呐有两种噪声模型混用会叠加出离谱的随机漂移。解决先检查声呐传感器插件的mesh是否存在且有没有挂碰撞材质再把noise标准差降到一个低量级比如0.01米确认点云稳定后再逐步加大。点云稳定后最后才谈降噪滤波。如果项目里有其他激光雷达插件确认它们没有共享同一个GPU资源否则也会造成显存冲突。5.4 仿真时间比真实时间快三倍控制参数失真现象控制周期写的是50Hz但机器人动作比预期快很多PID参数在仿真里能收敛换成真机就发散。原因Stonefish运行时默认可能不锁定时钟如果仿真步长小于实时约束它会跑得比真实时间快。你的PID计算用了dt1/50但实际仿真的时间推进速度不是按真实时钟来的导致积分项和微分项失真。解决在launch里找到时间同步参数把仿真器固定到与实时等速或者用仿真时钟作为所有节点的时钟源。检查方法是订阅/clock话题对比ROS时间和wall clock。从那以后我每次跑实验前都先确认时间同步状态再谈参数。5.5 水流参数改了机器人没有被推动现象场景XML里current已经改成0.3 m/s但机器人像钉在坐标里一样一点偏移都没有。原因流场不是自动作用于所有物体的。如果流场定义在某个局部区域而机器人的位置不在区域内就感受不到水流或者机器人模型用了某个锚定选项仿真器默认把它固定在初始位置。也有低配版本只支持全局均匀流局部流场会被忽略。解决先确认机器人是free-floating状态没有勾选固定基座选项然后看流场定义是局部的还是全局的。做抗流实验时用全局均匀流最省事直接把current的velocity改大重启仿真把机器人放到场景中央观察它是否在慢慢漂移。如果还不漂就在机器人上方加一个浮标可视化看看仿真器里水流方向向量是不是真的指向机体。6. 进阶技巧把仿真器当自动化回归测试台到这一步场景、传感器、推进器和避坑经验都已经齐了。这个资源的更大价值在于用它做自动化回归每次改完算法把一批指定工况跑一遍用脚本判断通过不通过。这一章给出无头模式批量跑和参数扫描的常见做法。6.1 无头模式一批任务跑完再看结果无头模式就是不开渲染窗口仿真照常运行CPU和内存占用更低适合一条接一条地跑任务。启动时把headless设为true然后让控制脚本自动记录结果。roslaunch smarc_stonefish_sims bay.launch headless:true current:0.3 depth:20 python3 run_profile.py --duration 180 --save result_03.csv注意headless模式没有画面出问题时肉眼看不到建议在每个阶段结束时用日志打印关键状态量比如深度、姿态、推进器输出。无头模式下GPU开销小可以同时开好几个场景进程但每个进程要保证不同的日志文件名别互相覆盖。6.2 用Python脚本自动跑多组实验一个简单的回归脚本思路用subprocess启动launch等仿真稳定后跑控制脚本结束后杀进程换下一组参数。下面是一个极简调度器#!/usr/bin/env python3 import subprocess import time import csv cases [ {name: calm, current: 0.0, depth: 20}, {name: flow_gentle, current: 0.2, depth: 20}, {name: flow_strong, current: 0.5, depth: 20}, ] results [] for case in cases: # 启动仿真当前参数覆盖launch里的默认值 sim subprocess.Popen([ roslaunch, smarc_stonefish_sims, bay.launch, headless:true, current: case[current] ]) time.sleep(15) # 等场景加载 # 运行深度控制与采集 subprocess.run([ python3, run_depth_profile.py, --depth, case[depth], --duration, 120, --save, fresult_{case[name]}.csv ]) # 停掉当前仿真进程 sim.terminate() sim.wait() results.append((case[name], finished)) with open(regression_summary.csv, w) as f: writer csv.writer(f) writer.writerow([case, status]) writer.writerows(results)这个脚本的逻辑是先让每组实验跑两分钟结束后记录状态再进入下一组。sleep 15秒是为了等Stonefish把地形和模型加载完时间太短会连话题都没起来。每组之间用terminate杀掉仿真进程保证下一组的流场参数从干净状态开始加载。脚本跑完直接看result_*.csv里的深度曲线把稳态误差、超调量汇总成一张表。我从这之后养成了一个习惯每次调完控制参数强制走一遍无流、0.2m/s、0.5m/s三档回归三档都通过才敢把参数标到实验记录里。这个流程帮我挡掉过好几次在某一组流场下恰好收敛的假象也让我对仿真结果的信任度高了不少。希望这套拆解能帮到正在跟水下仿真较劲的你。资源包拿下来之后优先跑通2.4节的最小场景再一节一节往后加你就知道石鱼的世界模拟到底能做到哪一步了。本文还有配套的精品资源点击获取