科研自动驾驶物理引擎【免费下载链接】webotsWebots Robot Simulator项目地址https://gitcode.com/gh_mirrors/web/webots点击查看免费下载Webots 的GPS节点Global Positioning Sensor用于在控制器程序中获取设备在世界坐标系中的绝对位置与运动速度是移动机器人定位、自主导航与地理仿真场景中最基础的传感器之一。本文以官方参考文档为核心结合仓库源码、PROTO 定义与自动化测试用例系统讲解 GPS 节点的字段配置、噪声模型高斯噪声与 Gauss-Markov 相关噪声、速度测量机制、多语言 API 用法以及通过WorldInfo的gpsCoordinateSystem/gpsReference字段实现的本地坐标与 WGS84 经纬度坐标切换。读完本文你将能够为自己的机器人仿真添加带真实噪声特性的 GPS 传感器并在控制器中正确处理位置、速度与经纬度坐标数据。GPS 节点概览GPS节点继承自 Device 与 Solid因此它既是一个可被控制器读取的传感器设备也是一个可以挂载到机器人任何部位的刚体。节点在 PROTO 层面由 resources/nodes/GPS.wrl 定义其完整字段如下GPS { SFString type satellite # {satellite, laser} SFFloat accuracy 0 # [0, inf) SFFloat noiseCorrelation 0 # [0, 1] SFFloat resolution -1 # {-1, [0, inf)} SFFloat speedNoise 0 # [0, inf) SFFloat speedResolution -1 # {-1, [0, inf)} }从 GPS.wrl 可以看到除上述 6 个专属字段外GPS 还继承了translation、rotation、children、name、physics、boundingObject等 Solid 节点字段默认name为gps。把 GPS 放在机器人模型的哪个部位测量得到的就是哪个部位在世界坐标中的位置。字段速查表字段类型默认值取值范围含义typeSFStringsatellite{satellite, laser}GPS 技术类型目前被实现忽略accuracySFFloat0[0, inf)位置测量高斯噪声的标准差米noiseCorrelationSFFloat0[0, 1]噪声相关性等级Gauss-Markov 过程resolutionSFFloat-1{-1, [0, inf)}位置测量的最小可分辨变化米speedNoiseSFFloat0[0, inf)速度测量高斯噪声的标准差米/秒speedResolutionSFFloat-1{-1, [0, inf)}速度测量的最小可分辨变化米/秒字段语义详解typeGPS 技术类型type字段定义所使用的 GPS 技术类型可选satellite卫星或laser激光目前该字段在仿真实现中被忽略——从 WbGps.cpp 的init()可以看到虽然节点解析并保存了mType但传感器刷新逻辑refreshSensorIfNeeded()并不读取它两种类型的行为完全一致。accuracy位置测量精度accuracy定义 GPS 位置测量的精度即添加到位置上的高斯噪声的标准差单位为米。当accuracy 0默认值时GPS 返回的是没有任何噪声干扰的理想位置当accuracy 0时每次刷新都会在真实位置上叠加高斯噪声标准差越大位置抖动越明显。noiseCorrelation噪声相关性Gauss-Markov 过程真实 GPS 的误差并非纯白噪声而是存在时间相关性——例如信号漂移现象某一时刻的误差与前一秒的误差有关联。noiseCorrelation字段正是用来模拟这种特性取值在[0, 1]之间含义如下0当前噪声与 1 秒前的噪声无任何关联纯高斯白噪声1当前噪声与 1 秒前的噪声完全相关噪声保持恒定表现为持续的漂移偏移。该字段内部会依据传感器时间步长计算对应的相关性因子并使用Gauss-Markov 过程估计当前噪声。在 WbGps.cpp 中噪声更新遵循如下递推公式noise[i] ratio * noise[i] sqrt(1 - pow(ratio, 2)) * WbRandom::nextGaussian(); mMeasuredPosition[i] accuracy * noise[i];其中相关性因子ratio pow(correlation, elapsedTime / 1000.0)elapsedTime为距上次刷新的毫秒数。可见noiseCorrelation越接近 1ratio越接近 1当前噪声越依赖历史噪声从而产生缓慢漂移的误差轨迹反之越接近 0噪声越接近独立的高斯白噪声。仓库对noiseCorrelation越界值做了钳制处理超出[0, 1]时会被重置为0见 WbGps.cpp 的updateCorrelation()。resolution位置分辨率resolution定义传感器能够测量的最小位置变化米。设置为-1默认表示传感器具有无限分辨率即能测量任意微小的变化设置为任意正数(0.0, inf)时测量值会被量化到该分辨率的整数倍。实现上量化通过 WbMathsUtilities::discretize 完成。非正且非-1的非法取值会被重置为-1见 updateResolution。speedNoise与speedResolution速度测量的噪声与分辨率与位置类似speedNoise定义速度测量高斯噪声的标准差米/秒speedResolution定义速度测量的最小可分辨变化米/秒。在 WbGps.cpp 中速度噪声以乘性方式施加mMeasuredSpeed * 1.0 mSpeedNoise-value() * WbRandom::nextGaussian(); if (mSpeedResolution-value() ! -1.0) mMeasuredSpeed WbMathsUtilities::discretize(mMeasuredSpeed, mSpeedResolution-value());注意这与位置噪声加性不同速度噪声按比例缩放即真实速度越大速度测量的绝对误差也越大。GPS 的工作原理从位置到噪声的完整数据流WbGps::refreshSensorIfNeeded()WbGps.cpp是仿真侧的核心逻辑完整描述了 GPS 每个采样周期的数据流获取真实位置读取 GPS 节点在其 Solid 父级坐标系中的平移量matrix().translation()叠加参考点将WorldInfo.gpsReference参考向量加到测量位置上本地坐标模式或先经 UTM 投影转换后加入WGS84 模式叠加噪声按accuracy与noiseCorrelation生成相关高斯噪声并加到各分量上应用分辨率若resolution ! -1对位置各分量做量化处理WGS84 反算若使用 WGS84 坐标系将 UTM 的 North-East 坐标反投影回经纬度计算速度向量若父级 Solid 存在 Physics 节点通过 ODE 物理引擎的dBodyGetPointVel获取该点瞬时速度否则用相邻两次采样位置差分估算计算标量速度取速度向量的模长再施加speedNoise与speedResolution。当父级节点没有Physics 节点时速度由位置差分计算WbGps.cppmSpeedVector (t - mPreviousPosition) * 1000.0 / mSensor-elapsedTime();而第一个采样周期的速度无法通过差分得到因此无 Physics 时首次getSpeed/getSpeedVector的返回值为NaN。这一行为在测试 tests/api/controllers/gps_speed/gps_speed.c 中被精确验证带 Physics 的 GPS 第一步即可测得5.0 m/s而无 Physics 的 GPS 第一步返回NaN第二步起才测得正确速度。坐标系本地坐标与 WGS84 经纬度GPS 返回的位置表达方式由 WorldInfo 节点的两个字段控制SFString gpsCoordinateSystem local # {WGS84, local} SFVec3f gpsReference 0 0 0 # any vectorgpsCoordinateSystem坐标表达方式local默认直接返回 GPS 设备在 Webots 笛卡尔坐标系中的坐标。gpsReference向量被简单地加到设备位置上。WGS84使用 WGS84 世界大地坐标系与通用横轴墨卡托投影UTM将设备的笛卡尔坐标转换为纬度、经度、高度三元组。该字段只接受local与WGS84两个值其他取值会在 WbWorldInfo::updateGpsCoordinateSystem 中被重置为local并发出解析警告。gpsReferenceGPS 参考点gpsReference指定 GPS 的参考点gpsCoordinateSystem local时三个分量均为米参考向量直接加到设备位置上gpsCoordinateSystem WGS84时三个分量依次为参考点的纬度十进制度、经度十进制度与高度米并应指向 Webots 笛卡尔坐标系原点对应的地理坐标。UTM 投影实现WGS84 模式的经纬度换算由 WbGps.cpp 中的WbUTMConverter类完成。该类以 WGS84 椭球参数长半轴a 6378137.0 m、扁率f 1/298.2572236、中央经线比例因子k0 0.9996实现经纬度 ↔ North/East 的相互投影并依据参考经度自动计算 UTM 分区mZone 1 floor((longitude 180) / 6)。实现注释特别说明该投影与 Webots 的 OpenStreetMap 导入器默认投影保持一致因此基于 OSM 数据构建的仿真环境中经纬度数据能够自洽。gpsCoordinateSystem或gpsReference在仿真运行期间被修改时postFinalize 中建立的信号连接会触发updateCoordinateSystem/updateReferences动态重建 UTM 转换器。GPS API 函数详解GPS 的全部 API 在多语言中一一对应C 语言声明见 include/controller/c/webots/gps.h实现见 src/controller/c/gps.cPython 封装见 lib/controller/python/controller/gps.py。控制器中通过wb_robot_get_device(gps)获取设备 tag 后即可使用。使能与读取enable/disable/get_sampling_period/get_values/get_speed/get_speed_vector#include webots/gps.h void wb_gps_enable(WbDeviceTag tag, int sampling_period); // 单位毫秒 void wb_gps_disable(WbDeviceTag tag); int wb_gps_get_sampling_period(WbDeviceTag tag); const double *wb_gps_get_values(WbDeviceTag tag); // 返回 3D 位置向量 double wb_gps_get_speed(WbDeviceTag tag); // 标量速度单位 m/s const double *wb_gps_get_speed_vector(WbDeviceTag tag); // 速度向量单位 m/s关键行为与注意事项wb_gps_enable的sampling_period单位为毫秒且必须是正数——负数会在 gps.c 中打印错误并忽略调用首次测量值只有等第一个采样周期结束后才可用因此在wb_robot_step之前读取会得到NaN。wb_gps_disable关闭传感器以节省计算时间等价于以0为周期调用enable。wb_gps_get_sampling_period返回enable时设置的周期设备被禁用时返回0。wb_gps_get_values返回的 3D 向量表示 GPS 设备的绝对位置仅索引 0、1、2 有效具体是笛卡尔坐标还是经纬度坐标取决于WorldInfo.gpsCoordinateSystem。wb_gps_get_speed返回标量速度m/swb_gps_get_speed_vector返回三维速度向量m/s。若父级 Solid 无 Physics 节点首次返回值是NaN。指针有效性C/C返回的向量是指向 GPS 节点内部数据的指针严禁自行 free且指向的值仅在下一次wb_robot_step/Robot::step调用前有效如需长期保存必须拷贝。PythongetValues()返回包含三个浮点数的 list。测试 tests/api/controllers/gps/gps.c 展示了标准的使能-读取流程设备在使能前与第一次wb_robot_step前均返回NaN此后每步返回固定位置(0.19996, 0.05, -0.35)同时验证了默认local模式下wb_gps_get_coordinate_system返回WB_GPS_LOCAL_COORDINATE。查询坐标系wb_gps_get_coordinate_systemtypedef enum { WB_GPS_LOCAL_COORDINATE, // 0 WB_GPS_WGS84_COORDINATE // 1 } WbGpsCoordinateSystem; WbGpsCoordinateSystem wb_gps_get_coordinate_system(WbDeviceTag tag);该函数返回WorldInfo.gpsCoordinateSystem字段对应的枚举值字段为local时返回WB_GPS_LOCAL_COORDINATE否则返回WB_GPS_WGS84_COORDINATE。在仿真侧坐标系统类型通过配置消息C_CONFIGURE下发给控制器见 WbGps.cpp控制器端在 gps.c 中解析存储。当运行期间gpsCoordinateSystem改变时GPS 节点会重新发送配置消息通知控制器更新。各语言对应写法C 为GPS::LOCAL/GPS::WGS84与getCoordinateSystem()Python 为GPS.LOCAL/GPS.WGS84与getCoordinateSystem()Java 为GPS.LOCAL/GPS.WGS84与getCoordinateSystem()。坐标格式转换wb_gps_convert_to_degrees_minutes_secondsconst char *wb_gps_convert_to_degrees_minutes_seconds(double decimal_degrees);该函数将十进制度坐标转换为度分秒Degrees Minutes Seconds, DMS格式的字符串输出形如47° 22′ 0″。实现见 gps.c返回字符串在 C 语言中由malloc分配需要调用者手动释放。注意DMS 格式使用了°、′、″等 Unicode 符号系统需支持 UTF-8 编码否则可能出现乱码。多语言 API 速览各语言接口完全等价以下是完整签名对照// C #include webots/GPS.hpp namespace webots { class GPS : public Device { typedef enum {LOCAL, WGS84} CoordinateSystem; virtual void enable(int samplingPeriod); virtual void disable(); int getSamplingPeriod() const; const double *getValues() const; double getSpeed() const; const double *getSpeedVector() const; const CoordinateSystem getCoordinateSystem() const; static std::string convertToDegreesMinutesSeconds(double decimalDegree); }; }# Python from controller import GPS gps GPS(gps) gps.enable(timestep) # 采样周期毫秒 values gps.getValues() # [x, y, z] 三个浮点数 speed gps.getSpeed() # 标量速度 m/s speed_vector gps.getSpeedVector() # 速度向量 m/s cs gps.getCoordinateSystem() # GPS.LOCAL 或 GPS.WGS84 dms GPS.convertToDegreesMinutesSeconds(47.3667) # 静态方法// Java import com.cyberbotics.webots.controller.GPS; GPS gps new GPS(gps); gps.enable(timestep); double[] values gps.getValues(); double speed gps.getSpeed(); double[] speedVector gps.getSpeedVector(); int cs gps.getCoordinateSystem(); // GPS.LOCAL / GPS.WGS84 String dms GPS.convertToDegreesMinutesSeconds(47.3667);% MATLAB wb_gps_enable(tag, sampling_period) wb_gps_disable(tag) period wb_gps_get_sampling_period(tag) x_y_z_array wb_gps_get_values(tag) speed wb_gps_get_speed(tag) coordinate_system wb_gps_get_coordinate_system(tag) % WB_GPS_LOCAL_COORDINATE / WB_GPS_WGS84_COORDINATE coordinate wb_gps_convert_to_degrees_minutes_seconds(decimal_degrees)实战示例带噪声的 GPS 定位控制器综合以上内容一个完整的 C 控制器示例启用 GPS、读取位置与速度、并输出 DMS 坐标如下#include webots/robot.h #include webots/gps.h #include stdio.h #define TIME_STEP 32 // 与基本时间步长一致或为其整数倍 int main(int argc, char **argv) { wb_robot_init(); WbDeviceTag gps wb_robot_get_device(gps); wb_gps_enable(gps, TIME_STEP); while (wb_robot_step(TIME_STEP) ! -1) { const double *position wb_gps_get_values(gps); // 单位米local或 经纬度/米WGS84 const double *velocity wb_gps_get_speed_vector(gps); // 单位m/s printf(GPS position: %f %f %f, speed: %f m/s, speed vector: %f %f %f\n, position[0], position[1], position[2], wb_gps_get_speed(gps), velocity[0], velocity[1], velocity[2]); if (wb_gps_get_coordinate_system(gps) WB_GPS_WGS84_COORDINATE) { char *lat wb_gps_convert_to_degrees_minutes_seconds(position[0]); char *lon wb_gps_convert_to_degrees_minutes_seconds(position[1]); printf(Latitude: %s, Longitude: %s\n, lat, lon); free(lat); // C 语言中必须释放返回的字符串 free(lon); } } wb_robot_cleanup(); return 0; }若要在世界文件中配置一个带有轻微漂移的 GPS例如模拟信号缓慢漂移的定位误差可在机器人模型的children中添加GPS { accuracy 0.5 noiseCorrelation 0.9 speedNoise 0.1 }总结与测试验证GPS节点以极简的 6 个字段提供了可配置的仿真级定位传感器accuracy与noiseCorrelation联手刻画从白噪声到漂移型误差的多种噪声形态resolution与speedResolution模拟真实传感器的量化限制而WorldInfo.gpsCoordinateSystem/gpsReference让 GPS 既可以输出 Webots 本地笛卡尔坐标也可以通过 UTM 投影输出 WGS84 经纬度坐标——后者尤其适合与 OpenStreetMap 导入器结合的真实地图自动驾驶场景。仓库测试对本文提到的关键行为均有覆盖tests/api/controllers/gps/gps.c验证使能前/首次采样前返回NaN、local坐标系下的位置精度与坐标系枚举tests/api/controllers/gps_speed/gps_speed.c验证带/不带 Physics 节点时速度与速度向量的行为差异首帧NaN规则另有gps_coordinates与gps_coordinates_north_direction等测试用例见 tests/api/controllers覆盖 WGS84 坐标转换与北向语义。通过对照这些测试用例与 WbGps.cpp 的实现可以确认本文描述的字段语义、噪声公式与首帧NaN行为均与当前仓库的实际实现一致。赞分享科研自动驾驶物理引擎【免费下载链接】webotsWebots Robot Simulator项目地址https://gitcode.com/gh_mirrors/web/webots点击查看免费下载相关推荐Webots InertialUnit 节点完全指南姿态测量、坐标系约定与多语言 API 实战Webots InertialUnit 节点完全指南姿态测量、坐标系约定与多语言 API 实战 Webots 的 InertialUnit 是一个模拟惯性测量科研自动驾驶物理引擎Webots 纹理坐标变换指南TextureTransform 节点原理与实战Webots 纹理坐标变换指南TextureTransform 节点原理与实战 导读 TextureTransform 是 WebotsWebots Rob科研自动驾驶物理引擎Webots Accelerometer 节点完全指南加速度与重力感测的原理、字段配置与多语言 API 实战Webots Accelerometer 节点完全指南加速度与重力感测的原理、字段配置与多语言 API 实战 本文围绕 Webots 参考文档中的 Accel科研自动驾驶物理引擎创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考