livox-mid70激光雷达使用实录livox-mid70是由大疆发布的面阵激光雷达，是大疆为自动驾驶推出的一款激光雷达，同时也可用于建立三维地图，文物修复，与大疆无人机相结合使用。你可以在x86和arm架构上都可以使用(arm需要交叉编译)。 bilibili上Livox官方发布了大量的视频demo可以去围观一下，同时Livox也有一个论坛可以去看一下。官方文档https://livox-wiki-cn.readthedocs.io/zh_CN/latest/index.html 这是中文官方文档，请根据以上文档完成硬件上的连接以及Ip配置，一切以官方文档为主。更改IP：一般来说刚拿到手都是静态IP，这时候你只需要将网线与你自己笔记本相连，然后通过用户手册查看你自己雷达的IP，那需要设置你自己电脑的IP为手动，并将其分在同一网段不同的地址下： 如我的Mid70的IP地址为192.168.1.105， 那么我电脑设置时 IP：192.168.1.104， 子网掩码：255.255.255.0, 网关：192.168.0.1如果对IP配置有问题,或想改为动态IP详情参见用户手册.pdf)Livox ViewerLivox Viewer是一款图形化操作的显示软件，有助于快速让你知道你的硬件连接和Ip配置有没有问题。Livox-Viewer for WindowsLivox-Viewer for ubuntu16.04,下载后解压运行./livox viewer.sh即可打开如下界面。官方软件Livox Viewer仅支持Windows或ubuntu16.04/14.04,所以建议如果不是这两个版本有ros可以直接上。Livox-SDK刚刚Livox Viewer只是一个快速尝试，如果你要做开发就必须要安装Livox-SDK(包括后面要在ros上使用也必须先要安装这个)github上中文README先从github上clone下来git clone https://github.com/Livox-SDK/Livox-SDK.git cd Livox-SDK cd build && cmake .. make sudo make install简单运行demo，注意这里的sample是在build文件夹下面的cd sample/lidar && ./lidar_samplelivox-ros-driver不得不说ros是真的好用，所以建议在ros下使用，会使得一切都很清晰明了。装livox-ros-driver前必须要先装Livox-SDK。先从github上clone下来，这里一定要用下面这个命令把它clone到相应位置，否则会出问题。git clone https://github.com/Livox-SDK/livox_ros_driver.git ws_livox/src简单的catkin_make一下cd ws_livox catkin_make别忘了添加环境source ./devel/setup.sh之后跑个rviz的demo，让你的激光雷达点云数据在rviz里显示出来roslaunch livox_ros_driver livox_lidar_rviz.launch录制你的点云数据点云数据不像二维图像一样，简简单单录制一个视频就好，作为点云数据记录是比较特殊的。 livox一共有两种录制格式lvx格式lvx格式是livox的一种自定义格式，如果你想在Windows上快速录制，可以使用Livox Viewer进行录制，也可以通过Livox-SDK中的文件录制首先进入以下路径cd ./Livox_SDK/build/sample/lidar_lvx_file然后运行下面这个文件，这里不用-c指定也可以，这里指定了lidar并设定持续10s./lidar_lvx_sample -c "此处为已连接的Lidar的15位广播码" -t 10通过上面的操作会得到一个.lvx格式的文件，这个文件可以在Livox Viewer中直接播放，也可以转成rosbag(一个可以把ros中当前所有topic中message记录下来的东西)。roslaunch livox_ros_driver lvx_to_rosbag.launch lvx_file_path:="/home/livox/test.lvx"rosbag格式rosbag是ros中非常好用的一个东西，我在之前的ros文章中有提到过。当我们在运行livox-ros-driver中的rviz显示时，即roslaunch livox_ros_driver livox_lidar_rviz.launch #详情看上面livox-ros-driver我们可以把此时的所有消息用rosbag记录下来rosbag record -a -o myrosbag.bag #-a代表所有的消息,-c代表输出名称此时就会生成一个.bag文件，具体名称看终端的输出。 你可以通过以下命令进行播放rosbagrosbag play *.bag -l #-l代表loop循环播放此时再打开在livox-ros-driver下的的rviz,就可以得到rosbag中的点云图了cd ws_livox/src/livox_ros_driver/ rviz -d config/display_lidar_points.rviz #这种.rviz文件实际上是保存rviz相关显示的数据livox_detectionlivox_detection指的是激光点云检测，livox官方给的代码可以检测各种车和行人。官方说2080ti下需要50ms。在Livox官方的github仓库中有两个激光点云检测仓库分别为livox_detection和livox_dection_simu。 这两个仓库看上去都差不多，但实际上非常坑的是：livox_detection仓库中的预训练模型在可以识别人和车，但车的识别效果不是很好;livox_dection_simu仓库中的预训练模型只能识别车，而几乎无法识别到人(虽然代码看上去都差不多)。至于原因我也不知道，只是这坑了我一会儿，后来在issue发现了这个问题。由于两个仓库操作都非常相似，这里选livox_detection_simu为例。依赖python3.6+tensorflow1.13+ (tested on 1.13.0)pybind11(后面的过程中安装)ros准备首先clone仓库git clone https://github.com/Livox-SDK/livox_detection_simu然后配置pybind(pybind就是这样，每次都需要clone在那个路径下才行)。cd livox_detection_simu/utils/lib_cpp git clone https://github.com/pybind/pybind11.git编译(在刚刚的lib_cpp路径下)。mkdir build && cd build cmake -DCMAKE_BUILD_TYPE=Release .. make把c++得到的依赖文件放到项目根目录下cp lib_cpp.so ../../../下载预训练模型,会得到model文件夹，把它放在项目根目录下，这样你就不用改模型地址了,下载rosbag,这是官方提供的rosbag，你也可以自己录一个。运行打开一个终端运行roscoreroscore打开一个终端运行rvizrviz -d ./config/show.rviz #在livox_detection_simu仓库下打开一个终端播放rosbagrosbag play *.bag -r 0.1 #找到你存放rosbag的路径运行，发送消息间隔0.1s打开一个终端运行目标检测程序python livox_detection_simu.py #如果你的模型放在其他地方，你需要在config下将其模型路径改为你所放预训练模型的路径由于目标检测速度相当慢，实时性非常差，队列里可能还缓存了很多帧本文使用 Zhihu On VSCode 创作并发布
IMU标定[TOC]安装 Ceres首先安装依赖 libdw-dev，在小车系统终端执行以下命令：sudo apt-get install libdw-dev然后需要进行 Ceres 库的安装，首先安装 Ceres 库所需的依赖项。sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-devCeres 的编译安装https://github.com/ceres-solver/ceres-solver/tree/2.0.0 获取到 Ceres2.0.0 版mkdir ceres-bin cd ceres-bin cmake ../ceres-solver-2.0.0 make -j3 make test sudo make install bin/simple_bundle_adjuster ../ceres-solver-2.0.0/data/problem-16-22106-pre.txt安装 code_utils 和 imu_utils在安装完 Ceres 之后我们需要获取 code_utils 和 imu_utils 两个功能包，用于IMU 数据的误差标定分析。这两个功能包我们都有提供 opencv3 和 opencv4 环境下修改后的版本，由于原版在编译时会遇到一些报错，基于这些报错我们对功能包源码进行了一定修改，使得这两个功能包在我们的环境下能直接编译成功。在编译时需注意，imu_utils 包时依赖于 code_utils 的，所以我们需要先编译code_utils 功能包，之后再编译 imu_utils。这里我们可以采用单独编译的方式，具体编译指令如下。# 创建workspace，将功能包放入src目录内，然后编译 catkin_make -DCATKIN_WHITELIST_PACKAGES=code_utils catkin_make -DCATKIN_WHITELIST_PACKAGES=imu_utils#运行功能包 roslaunch fdilink_ahrs ahrs_data.launch # 录制一小时 timeout 1h rosbag record -O imu_biaoding.bag /imu录制bag按照录制时间和imu发布的话题进行修改，我这里是1h，imu话题是/imutimeout 1h rosbag record -O imu_biaoding.bag /imu修改标定参数打开imu_utils 功能包中的任意launch 文件（所有launch 文件本质是一样的），根据实际情况修改其中的话题名，IMU 名及标定时长三个参数roslaunch imu_utils A3.launch (找到刚才编译的code_utils和imu_utils) rosbag play -r 200 imu.bag (找到你录制bag的文件夹)编译code_utils 报错错误提示表明在构建ROS工程 code_utils 时，CMake找不到名为 Ceres 的包。这可能是由于你的系统中没有安装 Ceres 或者 Ceres 的CMake配置文件未在CMake的搜索路径中。所以，重新安装一下Ceres 包
WIN10安装VLFeat工具箱教程下载VLFeat下载地址下载后解压到文件夹，文件夹最好不要含有中文防止报错，win安装下载binary package即可添加路径我解压之后的路径是D:\software\vlfeat-0.9.21\toolbox， 在matlab命令行里输入addpath('D:\software\vlfeat-0.9.21\toolbox');% 添加您的VLFeat工具箱路径 vl_setup;测试安装使用下面的命令来进行测试：vl_version verbose永久添加路径（可选）如果您希望MATLAB在每次启动时都能够访问VLFeat工具箱，可以将上述添加路径的命令添加到您的MATLAB启动脚本（如startup.m文件）中。
win10安装PCL点云库（详细教程）下载PCL软件点这里跳转Github下载我这里选的是AllInOne版本安装PCL右键以管理员身份运行，然后运行更改，进入软件安装界面下一步 --> 我接受 ---> 勾选 Add PCL to the system PATH for all users --> 下一步 ---> 选择合适的位置安装（最好路径不要有中文）报错 path too long installer unable to modify path如下界面右键点击属性->高级系统设置->环境变量->系统变量修改Path和PCL_ROOTPCL_ROOT选中PCL_ROOT，点击编辑，将路径改为你安装的路径Path选中Path，点击编辑，右上角新建，添加如下路径%PCL_ROOT%\bin %PCL_ROOT%\3rdParty\FLANN\bin %PCL_ROOT%\3rdParty\VTK\bin %PCL_ROOT%\3rdParty\Qhull\bin %PCL_ROOT%\3rdParty\OpenNI2\Tools大功告成！
gazebo中添加gps模块首先，需下载一个功能包hector gazebo plugins。Hector gazebo plugins 提供了来自 Hector 团队的 gazebo 插件。目前，它包含一个6瓦差分驱动插件，一个 IMU 传感器插件，一个地球磁场传感器插件，一个 GPS 传感器插件和一个声纳测距插件。安装hector_gazebo_plugins：sudo apt-get install ros-melodic-hector-gazebo-plugins添加gps的xacro文件导入gps的物体描述xacro文件；即gps的物体描述代码与功能配置代码，需连接在一起。原因：如果是传感器的功能配置plugin标签，则必须在gazebo标签内的sensor标签下，否则不起作用。而该gps的配置gazebo标签内没有sensor标签，若想plugin标签起作用，就得紧接在物体描述代码后，而不能分成两个单独得文件。<robot name="gps" xmlns:xacro="http://wiki.ros.org/xacro"> <xacro:property name="gps_offset_x" value="0" /> <xacro:property name="gps_offset_y" value="0" /> <xacro:property name="gps_offset_z" value="0.0075" /> <xacro:property name="gps_size" value="0.005" /> <xacro:property name="gps_m" value="0.01" /> <visual> <origin rpy="0 0 0" xyz="0 0 0" /> <geometry> <box size="${gps_size} ${gps_size} ${gps_size}"/> </geometry> <material name= "red" > <color rgba="1.0 0.0 0.0 1.0" /> </material> </visual> <collision> <geometry> <box size="${gps_size} ${gps_size} ${gps_size}" /> </geometry> <origin xyz="0.0 0.0 0" rpy="0.0 0.0 0.0" /> </collision> <xacro:Box_inertial_matrix m = "${gps_m}" l = "${gps_size}" w = "${gps_size}" h = "${gps_size}"/> <joint name="gps2imu" type="fixed"> <origin xyz="${gps_offset_x} ${gps_offset_y} ${gps_offset_z}" rpy="0 0 0" /> <parent link="imu"/> <child link="gps"/> </joint> <gazebo reference="gps"> <material>Gazebo/Green</material> </gazebo> <gazebo> <plugin name="gazebo_ros_gps" filename="libhector_gazebo_ros_gps.so"> <updateRate>4.0</updateRate> <bodyName>gps</bodyName> <frameId>gps</frameId> <topicName>fix</topicName> <velocityTopicName>fix_velocity</velocityTopicName> <referenceLatitude>-30.06022459407145675</referenceLatitude> <referenceLongitude>-51.173913575780311191</referenceLongitude> <referenceHeading>90</referenceHeading> <referenceAltitude>10</referenceAltitude> <offset>0 0 0</offset> <drift>0.001 0.001 0.001</drift> <gaussianNoise>0.05 0.05 0.05</gaussianNoise> <velocityDrift>0.001 0.001 0.001</velocityDrift> <velocityGaussianNoise>0.5 0.5 0.5</velocityGaussianNoise> </plugin> </gazebo> </robot>集成xacro文件<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro"> <xacro:include filename="head.xacro"/> <xacro:include filename="gazebo_carbase.xacro"/> <xacro:include filename="gazebo_imu.xacro"/> <xacro:include filename="gazebo_rplidar.xacro"/> <xacro:include filename="gazebo_gps.xacro"/> <xacro:include filename="move.xacro"/> <xacro:include filename="sensor_rplidar.xacro"/> <xacro:include filename="sensor_imu.xacro"/> </robot>
将IMU（惯性测量单元）和GNSS（全球导航卫星系统）数据融合以用作里程计的过程可以分为以下步骤：数据采集：收集IMU数据：IMU提供加速度和角速度的测量值。收集GNSS数据：GNSS接收器获取卫星定位信息，包括位置、速度和时间戳。数据预处理：对IMU数据进行预处理：包括去除偏差、噪声滤波和积分加速度和角速度以估算速度和位移。对GNSS数据进行预处理：解决多路径效应、钟差和其他误差，得到高质量的位置和速度测量。坐标系对齐：确保IMU和GNSS数据的坐标系一致。这可能需要进行坐标系变换，以便将它们对齐。状态估计：使用扩展卡尔曼滤波器（EKF）或粒子滤波器等状态估计算法，将IMU和GNSS数据融合。这些算法将帮助你估算车辆或机器人的状态，包括位置、速度和姿态。更新和融合：周期性地将新的IMU和GNSS数据与当前状态估计融合，以更新估计的位置和速度。这通常涉及到预测步骤（使用IMU数据预测状态）和更新步骤（使用GNSS数据校正预测）。误差建模：考虑IMU和GNSS测量的不确定性和误差特性，将其合并到状态估计中，以更好地估算里程和位置。输出估计：输出融合后的位置和速度估计，这可以用作里程计数据。评估性能：定期评估融合后的里程计性能，以检测误差的累积并进行校正。这个过程的关键在于合理处理IMU和GNSS数据的不确定性，以及在融合步骤中正确处理误差。IMU通常提供高频率的数据，但随时间漂移，而GNSS提供全球性的位置信息，但可能受多种误差因素影响。融合算法的设计和参数调整也对最终性能至关重要。需要注意的是，这只是一个高层次的概述，实际的IMU和GNSS融合过程可能会更为复杂，具体取决于你的应用和硬件配置。你可能需要使用专门设计的融合库或框架来帮助实施这一过程。
State Estimation Nodesekf_localization_nodeekf_localization_node是扩展卡尔曼滤波器的实现。它使用全向运动模型及时预测状态，并使用感知的传感器数据校正该投影估计值。ukf_localization_nodeukf_localization_node是无迹卡尔曼滤波器的实现。它使用一组精心挑选的西格玛点通过 EKF 中使用的相同运动模型投影状态，然后使用这些投影西格玛点来恢复状态估计值和协方差。这消除了雅可比矩阵的使用，并使滤波器更加稳定。但是，它也比 更费力。ekf_localization_node参数ekf_localization_node并共享其绝大多数参数，因为大多数参数控制数据在与核心过滤器融合之前如何处理。ukf_localization_node状态估计节点可用的参数数量相对较多，这使得启动和配置文件成为启动其任何节点的首选方法。该包包含模板启动和配置文件，可帮助用户入门。ekf_localization_node和ukf_localization_node通用参数标准参数~频率滤波器生成状态估计值的实值频率（以 Hz 为单位）。注意：筛选器不会开始计算，直到它从其中一个输入接收至少一条消息。~sensor_timeout实数周期，以秒为单位，表示我们何时将任何传感器视为已超时。在此情况下，我们会在不进行校正的情况下对扩展卡尔曼滤波器（EKF）执行预测周期。可以将这个参数视为滤波器生成新输出的最低频率的倒数。~two_d_mode如果您的机器人在平面环境中运行，并且您愿意忽略地面的细微变化（如IMU报告的那样），则将其设置为true。它将为所有 0D 变量（Z、横滚、俯仰及其各自的速度和加速度）融合 3 值。这样可以防止这些值的协方差爆炸，同时确保机器人的状态估计值仍然固定在 X-Y 平面上。~[frame]具体参数：~map_frame~odom_frame~base_link_frame~base_link_output_frame~world_frame这些参数定义了robot_localization的操作“模式”。REP-105规定了三个主要的坐标框架：map、odom和base_link。base_link是与机器人固定的坐标框架。机器人在odom坐标框架中的位置会随时间漂移，但在短期内是准确的，应该是连续的。map坐标框架与odom坐标框架类似，是一个世界固定的坐标框架，虽然它包含了机器人最全球准确的位置估计，但它可能会发生离散跳跃，例如由于融合GPS数据。以下是如何使用这些参数的方式：将 map_frame odom_frame和 base_link_frame 参数设置为适合系统的帧名称。注意：如果您的系统没有map_frame，只需将其删除，并确保world_frame设置为 odom_frame 的值。注意:如果您正在运行多个 EKF 实例，并且希望“覆盖”输出转换和消息以使其child_frame_id此帧，则可以进行此设置。base_link_output_frame是可选的，将默认为base_link_frame。这有助于在运行多个 EKF 实例时启用断开连接的 TF 树。当计算最终状态时，我们“覆盖”输出转换和消息，使其具有此帧作为其child_frame_id。如果仅融合连续位置数据（如车轮编码器测程法、视觉里程计或 IMU 数据），请设置world_frame 为您的odom_frame值。这是robot_localization中状态估计节点的默认行为，也是最常见的用法。如果要融合受离散跳跃影响的全局绝对位置数据（例如，GPS 或地标观测值的位置更新），则：将您的world_frame 值设置为您的map_frame值确保其他内容正在生成 odom->base_link 转换。这甚至可以是robot_localization状态估计节点的另一个实例。但是，该实例不应融合全局数据。map_frame odom_frame base_link_frame 的默认值分别为map_frame、odom 和 base_link。base_link_output_frame 的默认值为base_link_frame 。world_frame 默认值为odom_frame。~transform_time_offset某些软件包要求您的变换需要在时间上稍微超前一小段时间。该参数的值将被添加到由机器人定位中的状态估计节点生成的 map->odom 或 odom->base_link 变换的时间戳中。~transform_timeoutrobot_localization 包使用tf2的lookupTransform 方法来请求转换。此参数指定如果转换尚不可用，我们希望等待多长时间。如果未设置，则默认为 0。值 0 表示我们只获取最新的可用（请参阅tf2实现）转换，因此我们不会阻塞过滤器。指定非零transform_timeout会影响筛选器的计时，因为它会等待最大transform_timeout以使转换变为可用。这直接意味着大多数情况下无法满足指定的所需输出速率，因为滤波器在更新时必须等待转换。~[sensor]对于每个传感器，用户需要根据消息类型定义此参数。例如，如果我们定义一个 Imu 消息源和两个里程计消息源，则配置将如下所示： 每个参数名称的索引都是从 0 开始的（例如，odom0 odom1 等），并且必须按顺序定义（例如，不要使用pose0和 pose2 如果您尚未定义pose1）。每个参数的值是该传感器的主题名称。~[sensor]_config具体参数：~odomN_config~twistN_config~imuN_config~poseN_config对于上面定义的每个传感器消息，用户必须指定这些消息的哪些变量应融合到最终状态估计中。测程配置示例可能如下所示：<rosparam param="odom0_config">[true, true, false, false, false, true, true, false, false, false, false, true, false, false, false]</rosparam>布尔值的顺序$(X, Y, Z, roll, pitch, yaw, \dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}, \ddot{X}, \ddot{Y}, \ddot{Z})$注意:规范是在传感器的frame_id中完成的，而不是在world_frame或base_link_frame中完成的。有关详细信息，请参阅配置教程。~[sensor]_queue_size具体参数：~odomN_queue_size~twistN_queue_size~imuN_queue_size~poseN_queue_size用户可以使用这些参数来调整每个传感器的回调队列大小。如果frequency参数值远低于传感器的频率，这将非常有用，因为它允许筛选器合并在更新周期之间到达的所有测量值。~[sensor]_differential具体参数：~odomN_differential~imuN_differential~poseN_differential对于上述定义了包含姿态信息的每个传感器消息，用户可以指定是否应对姿态变量进行差分积分。如果某个值设置为 true，那么对于来自所讨论传感器的时间 t 的测量，首先会减去时间 t−1 的测量值，然后将得到的值转换为速度。这个设置在以下情况下特别有用：如果您的机器人具有两个绝对姿态信息的来源，例如，里程计和惯性测量单元（IMU）提供的偏航角测量。在这种情况下，如果输入源的方差没有正确配置，这些测量可能会与彼此不同步，并导致滤波器中的振荡，但通过进行差分积分，我们可以避免出现这种情况。用户在方向数据使用此参数时应小心，因为转换为速度意味着方向状态变量的协方差将无限制地增长（除非正在融合另一个绝对方向数据源）。如果您只是希望所有姿势变量都从0，然后请使用_relative参数。注意:如果要通过navsat_transform_node或utm_transform_node融合 GPS 信息，则应确保_differential设置为 false。~[sensor]_relative具体参数：~odomN_relative~imuN_relative~poseN_relative如果将此参数设置为true，那么来自该传感器的任何测量将与首次接收到的该传感器的测量相对融合。这在某些情况下非常有用，例如，如果您希望状态估计始终从真实值 (0,0,0) 开始，并具有横滚、俯仰和偏航的值 (0,0,0)。这类似于参数_differential，但不同之处在于，不是在时间t- 1 处删除测量，而是始终在时间 0 处删除测量，并且测量不会被转换为速度。~imuN_remove_gravitational_acceleration如果融合来自 IMU 的加速度计数据，则此参数确定在融合加速度测量之前是否从加速度测量中去除重力加速度。注意:这假定提供加速度数据的 IMU 也生成绝对方向。需要方向数据才能正确消除重力加速度。~gravitational_acceleration如果imuN_remove_gravitational_acceleration 设置为 true，则此参数确定将从 IMU 的线性加速度数据中删除的重力引起的 Z 加速度。默认值为 9.80665 （m/s^2）。~initial_state使用指定的状态启动滤波器。状态以15维的浮点数向量的形式给出，与传感器配置的顺序相同。例如，要将您的机器人的起始位置设置为 (5.0, 4.0, 3.0)，偏航角为 1.57，并且线性速度为 (0.1, 0.2, 0.3)，您可以使用以下方式：<rosparam param="initial_state">[5.0, 4.0, 3.0, 0.0, 0.0, 1.57, 0.1, 0.2, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</rosparam>~publish_tf如果设置为 true，状态估计节点将发布从由world_frame参数指定的框架到其子框架的变换。如果world_frame与map_frame相同，它将发布从map_frame到odom_frame的变换；如果world_frame与odom_frame相同，它将发布从odom_frame到base_link_frame的变换。默认值为 true。~publish_acceleration如果为 true，则状态估计节点将发布线性加速度状态。默认为 false。~print_diagnostics如果为 true，则状态估计节点将向/diagnostics主题发布诊断消息。这对于调试配置和传感器数据非常有用。高级参数~use_control如果为 true，则状态估计节点将侦听 geometry_msgs/Twist 消息的cmd_vel主题，并使用该主题生成加速项。然后，该术语用于机器人的状态预测。这在给定状态变量的收敛中出现少量滞后会导致应用中出现问题（例如，旋转期间的 LIDAR 偏移）的情况下特别有用。默认为 false。注意:来自 IMU 的线性加速度数据的存在和包含当前将“覆盖”预测的线性加速度值。~stamped_control如果为 true 并且use_control也为 true，则查找 geometry_msgs/TwistStamped 消息，而不是 geometry_msgs/Twist 消息。~control_timeout如果use_control设置为 true，并且在此时间内未收到控制命令（以秒为单位），则不再应用基于控制的加速项。~control_config控制消息中的哪些变量用于状态预测。这些值的顺序是 cmd_vel、 X'、Y'、Z'、roll'、pitch'、yaw'。仅当 use_control]设置为 true 时才会使用。<rosparam param="control_config">[true, false, false, false, false, true]</rosparam>~acceleration_limits您的机器人在每个维度上的加速速度。匹配control_config 中的参数顺序。仅在use_control设置为 true 时使用。<rosparam param="acceleration_limits">[1.3, 0.0, 0.0, 0.0, 0.0, 3.2]</rosparam>~deceleration_limits您的机器人在每个维度上减速的速度。匹配 control_config 中的参数顺序。仅在use_control设置为 true 时使用。~acceleration_gains如果您的机器人无法立即达到其加速度极限，则可以通过这些增益来控制允许的变化。仅在use_control设置为 true 时使用。<rosparam param="acceleration_limits">[0.8, 0.0, 0.0, 0.0, 0.0, 0.9]</rosparam>~deceleration_gains如果您的机器人无法立即达到其减速极限，则可以通过这些增益来控制允许的变化。仅在use_control设置为 true 时使用。~smooth_lagged_data如果您的任何传感器生成的数据具有早于最近的滤波器更新的时间戳（更明显地说，如果您有一个来源于滞后传感器数据的数据源），将此参数设置为true将使滤波器在接收到滞后数据时，返回到滞后测量之前的最后状态，然后处理直到当前时间的所有测量。这对于来自需要大量CPU使用量来生成姿态估计的节点的测量特别有用（例如，激光扫描匹配器），因为它们经常滞后于当前时间。~history_length如果smooth_lagged_data设置为 true，则此参数指定筛选器将保留其状态和测量历史记录的秒数。此值应至少与滞后测量与当前时间之间的时间增量一样大。~[sensor]_nodelay具体参数：~odomN_nodelay~twistN_nodelay~imuN_nodelay~poseN_nodelay如果为 true，则设置 tcpNoDelay 传输提示。有证据表明，Nagle算法干扰了大型消息类型的及时接收，例如nav_msgs/里程计消息。对于输入，如果将其设置为 true，则会禁用该订阅者的 Nagle 算法。默认为 false。~[传感器]_threshold具体参数：~odomN_pose_rejection_thresholdodomN_twist_rejection_thresholdposeN_rejection_thresholdtwistN_rejection_thresholdimuN_pose_rejection_thresholdimuN_angular_velocity_rejection_thresholdimuN_linear_acceleration_rejection_threshold如果您的数据受到异常值的影响，请使用这些阈值设置（以马氏距离表示）来控制允许传感器测量值与当前车辆状态的距离。如果numeric_limits<double>::max()未指定，则每个默认为。~debug指定是否在调试模式下运行的布尔标志。警告：将其设置为 true 将生成大量数据。数据将写入debug_out_file参数的值。默认为 false。~debug_out_file如果debug为 true，则调试输出写入到的文件。~process_noise_covariance过程噪声协方差，通常表示为Q，用于模拟滤波算法预测阶段的不确定性。它可能很难进行调整，因此将其暴露为一个参数，以便更容易进行自定义设置。可以不必去调整此参数，但通过调整它，您可以获得更优越的结果。一般来说，对于输入消息中给定变量的方差而言，Q的值越大，滤波器越快地收敛到测量值。~dynamic_process_noise_covariance如果设置为true，将根据机器人的速度动态调整process_noise_covariance。这在机器人停止移动时希望机器人的估计误差协方差不再增加时非常有用。默认值为false。~initial_estimate_covariance估计协方差（通常表示为 P）定义了当前状态估计中的误差。该参数允许用户设置矩阵的初始值，这将影响滤波器收敛的速度。例如，如果用户将值设置为[0,0]到一个非常小的值，例如 1e-12，然后尝试将 X 位置的测量值与高方差值融合X，那么滤波器“信任”这些测量值的速度将非常慢，并且收敛所需的时间将增加。同样，用户应注意此参数。当仅融合速度数据（例如，没有绝对姿势信息）时，用户可能不希望将绝对姿势变量的初始协方差值设置为大数字。这是因为这些误差将无限制地增长（由于缺乏绝对姿势测量来减少误差），并且从大值开始它们将不利于状态估计。~reset_on_time_jump如果设置为 true且ros::Time::isSimTime()为 true，则在主题上检测到时间跳回时，筛选器将重置为其未初始化状态。这在处理包数据时很有用，因为包可以在不重新启动节点的情况下重新启动。~predict_to_current_time如果设置为 true，筛选器将预测并校正到最新测量的时间（默认情况下），但现在也会预测到当前时间步长。~disabled_at_startup如果设置为 true，则不会在启动时运行筛选器。Node-specific Parameters标准参数和高级参数对于robot_localization中的所有状态估计节点都是通用的。本节详细介绍了其各自状态估计节点所特有的参数。ukf_localization_nodeukf_localization_node的参数遵循原始论文和维基文章的命名法。~alpha - 控制西格玛点的扩散。除非您熟悉无味卡尔曼滤波器，否则此设置最好保持其默认值 （0.001）。~kappa - 也控制西格玛点的传播。除非您熟悉无味卡尔曼滤波器，否则此设置最好保持其默认值 （0）。~beta - 与状态向量的分布有关。默认值 2 表示分布为高斯分布。与其他参数一样，除非用户熟悉无味卡尔曼滤波器，否则这应该保持不变。已发布的主题odometry/filtered (nav_msgs/里程计)accel/filtered (geometry_msgs/加速与协方差标记）（如果启用）已发布的转换如果用户的world_frame参数设置为odom_frame的值，将会发布一个从odom_frame参数指定的框架到base_link_frame参数指定的框架的变换。如果用户的world_frame参数设置为map_frame的值，将会发布一个从map_frame参数指定的框架到odom_frame参数指定的框架的变换。注意:此模式假定另一个节点正在将转换从 odom_frame 参数给出的帧广播到 base_link_frame 参数给出的帧。这可以是robot_localization状态估计节点的另一个实例。服务业通过向set_pose主题发布geometry_msgs/PoseWithCovarianceStamped消息，用户可以手动设置滤波器的状态。这在测试期间重置滤波器并允许与rviz进行交互时非常有用。另外，状态估计节点提供了一个SetPose服务，其类型为robot_localization/SetPose，也可以用于设置滤波器的状态。navsat_transform_nodenavsat_transform_node将nav_msgs/里程计消息（通常是ekf_localization_node ukf_localization_node 的输出）、包含机器人航向准确估计的 sensor_msgs/Imu 和包含 GPS 数据的 sensor_msgs/NavSatFix 消息作为输入。它以与机器人世界坐标系一致的坐标生成里程计信息。此值可以直接融合到状态估计值中。注意:如果将此节点的输出与 robot_localization 中的任何状态估计节点融合在一起，则应确保该输入的odomN_differential设置为 false。参数~频率navsat_transform_node检查新的 sensor_msgs/NavSatFix消息并在publish_filtered_gps参数设置为true时发布经过滤波的 sensor_msgs/NavSatFix消息的实际频率，以赫兹（Hz）为单位。~延迟在计算从 GPS 坐标到机器人世界框架的转换之前等待的时间（以秒为单位）。~magnetic_declination_radians输入您所在位置的磁偏角。如果您不知道，请参阅 http://www.ngdc.noaa.gov/geomag-web（确保将值转换为弧度）。如果您的 IMU 提供相对于磁北的方向，则需要此参数。~yaw_offset面向东方时，您的 IMU 应读取 0 表示偏航。如果没有，请在此处输入偏移量 （desired_value = 偏移量 + sensor_raw_value）。例如，如果您的 IMU 在朝北时报告 0，就像大多数 IMU 一样，则此参数将为pi/2 （~1.5707963）。此参数在版本2.2.1 中已更改。以前，navsat_transform_node假设 IMU 在朝北时读数为 0，因此yaw_offset被一致使用。~zero_altitude如果为 true，则此节点生成的 nav_msgs/里程计消息的姿势 Z 值设置为 0。~publish_filtered_gps如果设置为true，navsat_transform_node还将把机器人的世界坐标系（例如，map坐标系）的位置转换回GPS坐标，并在/gps/filtered主题上发布一个 sensor_msgs/NavSatFix 消息。~broadcast_utm_transform如果这是真的，navsat_transform_node将广播UTM网格和输入里程计数据帧之间的转换。有关详细信息，请参阅下面的已发布转换。~use_odometry_yaw如果设置为true，navsat_transform_node将不会从IMU数据获取方向，而是从输入的里程消息获取方向。用户应该注意只有在您的里程消息中指定了以地球为参考的坐标系的方向数据时，才将其设置为true，例如，由磁力计生成的方向数据。此外，如果里程源是robot_localization中的一个状态估计节点之一，用户应该至少有一个输入到节点的绝对方向数据源，且_differential和_relative参数设置为false。~wait_for_datum如果为 true，navsat_transform_node则等待从以下任一位置获取数据：参数datum服务set_datum~broadcast_utm_transform_as_parent_frame如果为 true，navsat_transform_node则将发布 utm->world_frame 转换，而不是 world_frame->utm 转换。 请注意，对于要发布的转换，还必须broadcast_utm_transform设置为 true。~transform_timeout此参数指定在转换尚不可用时，我们希望等待多长时间。如果未设置，则默认为 0。值 0 表示我们只是获得最新的可用（参见tf2实现）转换。订阅主题imu/data带有方向数据的 sensor_msgs/Imu 消息odometry/filtered机器人当前位置的nav_msgs/里程计消息。如果您的机器人达到一些非零姿势后，您的第一个 GPS 读数是必需的。gps/fix包含机器人 GPS 坐标的 sensor_msgs/NavSatFix 消息已发布主题odometry/gps: 一个包含机器人的GPS坐标的nav_msgs/里程计消息，已经转换到机器人的世界坐标系。这个消息可以直接融合到robot_localization的状态估计节点中。gps/filtered (可选): 一个sensor_msgs/NavSatFix 消息，包含了机器人的世界坐标系位置，已经转换为GPS坐标。已发布的转换world_frame->utm（可选） - 如果broadcast_utm_transform参数设置为true，navsat_transform_node会计算从utm坐标系到输入里程数据的frame_id之间的变换。默认情况下，使用逆变换方式，utm坐标系被发布为里程坐标系的子坐标系。使用broadcast_utm_transform_as_parent_frame参数，utm坐标系将被发布为里程坐标系的父坐标系。这在一个TF树内有多个机器人时非常有用。Preparing Your Data for Use with robot_localization在开始使用robot_localization状态估计节点之前，用户必须确保其传感器数据格式正确。每类传感器数据都有各种注意事项，建议用户在尝试使用 robot_localization之前完整阅读本教程。如需更多信息，建议用户观看 ROSCon 2015 的此演示文稿。遵守ROS标准要考虑的两个最重要的 ROS REP 是：REP-103（标准计量单位和坐标约定）REP-105（坐标系约定）。我们鼓励不熟悉 ROS 或状态估计的用户仔细阅读这两个 REP，因为它几乎肯定会帮助您准备传感器数据。 尝试尽可能遵守这些标准。robot_localization此外，查看每种受支持的 ROS 消息类型的规范可能会使用户受益：nav_msgs/里程计geometry_msgs/PoseWithCovarianceStampedgeometry_msgs/TwistWithCovarianceStampedsensor_msgs/IMU坐标系和转换传感器数据REP-105规定了四个主要的坐标框架：base_link、odom、map和earth。base_link坐标框架与机器人固定连接。map和odom坐标框架是世界固定坐标系，它们的原点通常与机器人的起始位置对齐。earth坐标框架用于为多个map坐标框架提供一个共同的参考坐标系（例如，分布在大区域内的机器人）。在本教程中，earth坐标框架与无关。robot_localization的状态估计节点产生一个状态估计，其姿态在map或odom坐标框架中给出，速度在base_link坐标框架中给出。在融合到状态之前，所有传入的数据都会被转换成这些坐标框架之一。不同类型的消息数据的转换方式如下：nav_msgs/里程计：所有姿态数据（位置和方向）从消息头的frame_id转换为world_frame参数指定的坐标框架（通常是map或odom）。在消息本身中，这特别指的是pose属性内包含的所有内容。所有扭矩数据（线性和角速度）从消息的child_frame_id转换为base_link_frame参数指定的坐标框架（通常是base_link）。geometry_msgs/PoseWithCovarianceStamped ：处理方式与Odometry消息中的姿态数据相同。geometry_msgs/PoseWithCovarianceStamped ：处理方式与Odometry消息中的扭矩数据相同。sensor_msgs/Imu：目前IMU消息存在一些歧义，但ROS社区正在解决这个问题。大多数IMUs在本地坐标系中本来就报告方向数据，其X和Z轴分别由指向磁北极和地球中心的矢量定义，Y轴朝东（与磁北极矢量有90度的偏移）。这个坐标系通常被称为NED（North, East, Down）。然而，REP-103规定了一个用于户外导航的ENU（East, North, Up）坐标系。截止到本文撰写时，robot_localization假定所有IMU数据采用ENU坐标系，不支持NED坐标系的数据。这个情况可能会在未来发生变化，但目前用户应确保数据在与robot_localization中的任何节点一起使用之前被转换为ENU坐标系。IMU可能还会以不同于其“中性”位置的方向放置在机器人上。例如，用户可以侧向安装IMU，或将其旋转，使其面向机器人前方以外的方向。这个偏移通常由从base_link_frame参数到IMU消息的frame_id的静态变换来指定。robot_localization中的状态估计节点将自动校正传感器的方向，使其数据与base_link_frame参数指定的坐标系对齐。处理tf_prefix在ROS Indigo版本迁移到tf2后，robot_localization仍然允许使用tf_prefix参数，但与tf2一致，所有frame_id值都会去除任何前导的'/'字符。每种传感器消息类型的注意事项里程计对于具有提供瞬时平移和旋转速度的轮式编码器的许多机器人平台，它们通常也内部集成这些速度以生成位置估计。如果您负责处理这些数据或可以编辑它们，请记住以下事项：速度/姿态：robot_localization可以集成速度或绝对姿态信息。一般来说，最佳实践是：如果里程提供了位置和线性速度，请融合线性速度。如果里程提供了方向和角速度，请融合方向。注意：如果您有两个来源的方向数据，那么需要小心。如果两者都生成带有准确协方差矩阵的方向数据，那么可以安全地融合方向数据。然而，如果一个或两个方向数据的协方差矩阵不准确，那么应该只融合来自更准确传感器的方向数据。对于另一个传感器，可以使用角速度（如果提供），或者继续融合绝对方向数据，但需要为该传感器打开_differential模式。frame_id：参见上面有关坐标框架和变换的部分。协方差：协方差值对于robot_localization非常重要。robot_pose_ekf试图将里程消息中的所有姿态变量进行融合。一些机器人的驱动程序已经按照其要求编写。这意味着如果某个传感器不生成某个特定变量（例如，不报告Z位置的机器人），那么唯一的方法让robot_pose_ekf忽略它就是将其方差膨胀到一个非常大的值（大致为1e3次方），以便有效地忽略相关变量。这种做法既是不必要的，甚至对robot_localization的性能有害。唯一的例外情况是，如果您有第二个测量相关变量的输入源，那么膨胀的协方差将有效。注意：有关详细信息，请参阅配置robot_localization和从 robot_pose_ekf 迁移。符号：遵守 REP-103意味着您需要确保数据的符号是正确的。例如，如果您有一台地面机器人并将其逆时针旋转，那么它的偏航角应增加，其偏航角速度应为正。如果您将其向前驱动，其X坐标位置应增加，其X坐标速度应为正。变换：odom->base_link变换的广播。当world_frame参数设置为配置文件中的odom_frame参数的值时，robot_localization的状态估计节点会输出一个包含位置估计的nav_msgs/里程计消息，并从由其odom_frame参数指定的框架到其base_link_frame参数指定的框架的变换。然而，一些机器人驱动程序也会在里程消息一起广播此变换。如果用户希望robot_localization负责这个变换，那么他们需要通过机器人驱动程序禁用此变换的广播。这通常是作为一个参数公开的。IMU除以下内容外，请务必阅读上述有关 IMU 数据坐标系和变换的部分。遵守规范：与里程计一样，请确保您的数据符合 REP-103 和 sensor_msgs/Imu 规范。仔细检查数据的迹象，并确保值正确。frame_id协方差：呼应里程计的建议，确保您的协方差有意义。不要使用较大的值来使筛选器忽略给定变量。将要忽略的变量的配置设置为 false。加速度：小心加速度数据。中的状态估计节点假定放置在平面上中性右侧朝上位置的 IMU 将：robot_localization测量值为Z轴方向上的加速度，为+9.81米每秒平方。如果传感器被翻转+90度（左侧朝上），则Y轴的加速度应为+9.81米每秒平方。如果传感器被俯仰+90度（前端朝下），X轴的加速度应为-9.81米每秒平方。PoseWithCovarianceStamped请参阅里程计部分。TwistWithCovarianceStamped请参阅里程计部分。常见错误输入数据不符合 REP-103。确保所有值（尤其是方向角）在正确的方向上增加和减少。值不正确。不正确的数值。速度数据应该报告在参数给定的框架内，或者速度数据和base_link_frame之间应存在一个变换。膨胀的协方差。忽略测量中的变量的首选方法是通过参数 odomN_config。缺少协方差。如果您已经配置了某个传感器将某个变量融合到状态估计节点中，那么该值的方差（即方差矩阵中位置(i, i)处的值，其中i是该变量的索引）不应为0。如果遇到正在融合的变量的方差值为0，状态估计节点将向该值添加一个小的epsilon值（1e-6）。更好的解决方案是用户适当设置协方差值。配置robot_localization当将传感器数据合并到任何状态估计节点的位置估计中时，提取尽可能多的信息是很重要的。本教程详细介绍了传感器集成的最佳实践。robot_localization如需更多信息，建议用户观看 ROSCon 2015 的此演示文稿。传感器配置配置向量的格式对于所有传感器都是相同的，即使涉及的消息类型不包含配置向量中的某些变量（例如，一个geometry_msgs/TwistWithCovarianceStamped消息可能不包含任何姿态数据，但配置向量仍然具有姿态变量的值）。未使用的变量将被简单地忽略。请注意，配置向量是根据输入消息的frame_id给定的。例如，考虑一个产生geometry_msgs/TwistWithCovarianceStamped消息的速度传感器，其frame_id为velocity_sensor_frame。在这个示例中，我们假设存在一个从velocity_sensor_frame到您的机器人的base_link_frame（例如，base_link）的变换，并且该变换会将velocity_sensor_frame中的X轴速度转换为base_link_frame中的Z轴速度。要将传感器中的X轴速度数据包含到滤波器中，配置向量应将X轴速度值设置为true，而不是$\dot{Z}$速度值：<rosparam param="twist0_config">[false, false, false, false, false, false, true, false, false, false, false, false, false, false, false]</rosparam>注意:布尔值的顺序为$(X, Y, Z, roll, pitch, yaw, \dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}, \ddot{X}, \ddot{Y}, \ddot{Z})$以 2D 方式操作配置传感器时要做出的第一个决定是机器人是否在平面环境中运行，并且您可以忽略 IMU 可能报告的接地平面变化的细微影响。如果是这样，请将参数two_d_mode设置为 true。这有效地将每次测量中的 3D 位姿变量归零，并强制将它们融合到状态估计中。融合未测量的变量让我们从一个例子开始。假设您有一个在平面环境中工作的轮式非完整机器人。您的机器人有一些车轮编码器，用于估计瞬时 X 速度以及绝对姿态信息。此信息在nav_msgs/里程计消息中报告。此外，您的机器人还有一个 IMU，可以测量旋转速度、车辆姿态和线性加速度。其数据在 sensor_msgs/Imu 消息中报告。由于我们在平面环境中操作，因此我们将参数two_d_mode设置为 true。这将自动将所有 3D 变量归零，例如Z,roll,pitch，它们各自的速度，以及Z加速度。我们从以下配置开始：<rosparam param="odom0_config">[true, true, false, false, false, true, true, false, false, false, false, true, false, false, false]</rosparam> <rosparam param="imu0_config">[false, false, false, false, false, true, false, false, false, false, false, true, true, false, false]</rosparam>首先，这是有道理的，因为平面机器人只需要关注自己$X,Y,X,\dot{X},\dot{Y},\ddot{X},\ddot{Y},yaw$和$\dot{yaw}$.但是，这里有几点需要注意。对于odom0 ，我们包括X和Y（在世界坐标系中报告），$yaw,\dot{X}$（在body frame中报告），以及$\dot{yaw}$。然而，除非您的机器人内部使用IMU，否则它很可能只是使用轮式编码器数据来生成其测量中的值。因此，其速度、航向和位置数据都是从相同的源生成的。在这种情况下，我们不希望使用所有的值，因为这会将重复的信息输入到滤波器中。相反，最好只使用速度数据：<rosparam param="odom0_config">[false, false, false, false, false, false, true, false, false, false, false, true, false, false, false]</rosparam> <rosparam param="imu0_config">[false, false, false, false, false, true, false, false, false, false, false, true, true, false, false]</rosparam>接下来，我们注意到我们没有融合$\dot{Y}$。乍一看，这是正确的选择，因为我们的机器人不能瞬间侧向移动。但是，如果 nav_msgs/里程计消息报告$\dot{Y}$的值为0（且$\dot{Y}$协方差不会膨胀到一个大值），最好将该值提供给过滤器。在这种情况下，0测量表示机器人根本不会沿着那个方向移动，因此它作为一个完全有效的测量值：<rosparam param="odom0_config">[false, false, false, false, false, false, true, true, false, false, false, true, false, false, false]</rosparam> <rosparam param="imu0_config">[false, false, false, false, false, true, false, false, false, false, false, true, true, false, false]</rosparam>您可能想知道为什么我们没有出于同样的原因融合$\dot{Z}$速度。答案是，当我们设置two_d_mode为 false 时，我们实际上融合了$\dot{Z}$速度。如果我们没有这样做，事实上，我们可以将$\dot{Z}$速度的0测量值融入滤波器中。最后，我们来到IMU。您可能会注意到，我们已经将$\ddot{Y}$更改为 false。这是因为许多系统，包括我们在这里讨论的假设系统，都不会发生瞬时Y加速度。但是，IMU 可能会报告 Y 加速度的非零噪声值，这可能会导致您的估计值快速漂移。微分参数和相对参数“robot_localization”中的状态估计节点允许用户融合任意数量的传感器。这允许用户使用多个源来测量某些状态向量变量，特别是姿态变量。例如，您的机器人可能从多个 IMU 获取绝对方向信息，或者它可能有多个数据源提供对其绝对位置的估计。在这种情况下，用户有两个选择：按原样融合所有绝对位置/方向数据，例如，<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam> <rosparam param="imu1_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam> 在这种情况下，用户应非常小心，确保每个测量的方向变量的协方差设置正确。例如，如果每个IMU都宣传了偏航方差为0.1，但是IMUs的偏航测量之间的差异大于0.1，那么滤波器的输出将在每个传感器提供的值之间来回振荡。用户应确保每个测量周围的噪声分布重叠。另外，用户可以使用_differential参数。通过为给定的传感器将其设置为true，所有姿态（位置和方向）数据将通过计算两个连续时间步骤之间的测量值变化来转换为速度。然后，数据将作为速度进行融合。然而，用户应该谨慎使用：当测量值被绝对融合时（尤其是IMUs），如果测量在给定变量上具有静态或非递增的方差，那么估计的协方差矩阵中的方差将受到限制。如果该信息被转换为速度，那么在每个时间步骤，估计值将获得一些小误差，并且相关变量的方差将无限增长。对于位置（X，Y，Z）信息，这不是问题，但对于方向数据，这是一个问题。例如，对于机器人来说，经过一段时间后，其在X方向上累积了1.5米的误差是可以接受的。但是，如果同一台机器人在偏航方向上累积了1.5弧度的误差，那么当机器人下次前进时，其位置误差将急剧增加。关于_differential参数的一般经验法则是，如果一个给定的机器人只有一个方向数据来源，那么_differential参数应设置为false。如果有N个方向数据来源，用户可以为其中N-1个设置_differential参数为true，或者简单地确保协方差值足够大，以消除振荡。从 robot_pose_ekf 迁移迁移robot_pose_ekf过程相当简单。本页旨在突出显示软件包之间的相关差异，以促进快速转换。源消息中的协方差对于robot_pose_ekf，让滤波器忽略测量的常见方法是将其协方差大幅膨胀，通常在10^3数量级。然而，robot_localization中的状态估计节点允许用户指定应与当前状态融合的测量中的哪些变量。如果您的传感器针对特定变量报告为零，而您不希望将该值与滤波器融合，或者如果已知该传感器对该字段产生较差的数据，那么只需将其xxxx_config参数值设置为false以针对相关变量（请参考主页面以获取有关此参数的描述）。然而，用户应该小心：有时平台约束会隐式导致变量的0测量。例如，一个不能瞬间向Y方向移动的差动驱动机器人可以安全地将Y˙的0测量与小协方差值一起融合。''differential'' 参数默认情况下，robot_pose_ekf将在时间t接收到一个姿态测量，确定它与时间t−1接收到的测量之间的差异，将该差异转换为当前帧，然后对该测量进行积分。这在两个传感器测量相同的姿态变量的情况下非常有用：随着时间的推移，每个传感器报告的值将开始发散。如果至少一个这些测量的协方差没有适当增长，滤波器最终将在测量值之间开始振荡。通过进行微分积分，可以避免这种情况，并且测量始终与当前状态一致。这种情况可以通过使用robot_localization状态估计节点的三种不同方法来避免：如果要融合两个不同源的相同姿态数据（例如，两个不同的传感器测量Z位置），确保这些源准确报告它们的协方差。如果这两个源开始发散，那么它们的协方差应反映至少一个源中必定发生的增长误差。如果有可用的话，融合速度数据而不是姿态数据。如果有两个单独的数据源测量相同的变量，将最准确的一个作为姿态数据融合，将另一个作为速度融合。作为第二种方法的替代，如果某个姿态测量中无法获得速度数据，可以为其中一个传感器启用_differential参数。这将导致其被微分并作为速度融合。集成 GPS 数据GPS数据的集成是用户的常见需求。robot_localization包含一个节点，navsat_transform_node，用于将GPS数据转换为与机器人在其世界坐标系中的起始姿态（位置和方向）一致的坐标系。这极大地简化了GPS数据的融合。本教程将解释如何使用navsat_transform_node，并深入探讨其背后的一些数学原理。如需更多信息，建议用户观看 ROSCon 2015 的此演示文稿。融合GPS数据的注意事项在开始本教程之前，用户应该确保熟悉REP-105。用户应该认识到，使用包含GPS数据的位置估计可能不适合供导航模块使用，因为GPS数据容易出现离散性的不连续性（"跳跃"）。如果您想将GPS数据融入您的位置估计中，一种潜在的解决方案是执行以下操作：运行一个robot_localization状态估计节点的实例，仅融合连续数据，如里程计和IMU数据。将此实例的world_frame参数设置为与odom_frame参数相同的值。在此坐标系中执行本地路径规划和运动。运行另一个robot_localization状态估计节点的实例，融合所有数据源，包括GPS。将此实例的world_frame参数设置为与map_frame参数相同的值。这仅仅是一个建议，用户可以选择将GPS数据融入单个robot_localization状态估计节点中。使用 navsat_transform_node下图说明了一个示例设置：必需的输入navsat_transform_node需要三种信息来源：机器人在其世界坐标系中的当前姿态估计、地球参考的朝向以及以纬度/经度对（可选高度）表示的地理坐标。这些数据可以通过三种不同的方式获得：（默认行为）数据可以完全来自机器人的传感器和姿态估计软件。要启用此模式，请确保wait_for_datum参数设置为false（其默认值）。所需的消息包括：包含原始 GPS 坐标的 sensor_msgs/NavSatFix 消息。具有绝对（地球参考）标题的 sensor_msgs/Imu 消息。一个包含机器人当前位置估计的nav_msgs/Odometry消息，该估计是在其起始位置指定的坐标系中的（通常是robot_localization状态估计节点的输出）。基准（全局坐标系原点）可以通过参数datum指定。注意：若要使用此模式，必须将 wait_for_datum 参数设置为 true。datum参数采用以下形式：<rosparam param="datum">[55.944904, -3.186693, 0.0, map, base_link]</rosparam>在这种模式下，参数的顺序是纬度（以十进制度数表示）、经度（以十进制度数表示）、朝向（以弧度表示）、机器人的世界坐标系（即robot_localization状态估计节点中world_frame参数的值）和机器人的机体坐标系（即robot_localization状态估计节点中base_link_frame参数的值）。当使用这种模式时，机器人假定您的机器人的世界坐标系原点位于指定的纬度和经度，并且其朝向为0（朝东）。通过使用set_datum服务和 robot_localization/SetDatum 服务消息，可以手动设置基准点。GPS数据请注意，所有navsat_transform_node开发都是使用 Garmin 18x GPS 装置完成的，因此可能需要处理其他装置生成的数据错综复杂。出色的nmea_navsat_driver包提供所需的 sensor_msgs/NavSatFix 消息。下面是我们将在本教程中使用的nmea_navsat_driver启动文件：<node pkg="nmea_navsat_driver" type="nmea_serial_driver" name="navsat" respawn="true"> </node>这些信息仅在用户未通过datum参数或set_datum服务手动指定原点时才相关。IMU数据注意:从版本2.2.1开始，navsat_transform_node已经采用了一个标准，其中假定所有的朝向数据都以零点朝东开始。如果您的IMU不符合这个标准，而是在朝北时报告零点，您仍然可以使用yaw_offset参数来进行校正。在这种情况下，yaw_offset的值将为π/2（约1.5707963）。用户应确保他们的 IMU 符合 REP-105。特别是，检查方向角的迹象是否在正确的方向上增加。此外，用户应查找机器人工作区域的磁偏角，将其转换为弧度，然后将该值用于magnetic_declination_radians参数。这些信息仅在用户未通过datum参数或set_datum服务手动指定原点时才相关。里程计数据这应该只是您用于融合GPS数据的robot_localization状态估计节点实例的输出。配置navsat_transform_node下面是我们将用于本教程的navsat_transform_node启动文件：<launch> <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true"> <remap from="/imu/data" to="/your/imu/topic" /> <remap from="/gps/fix" to="/your/gps/fix/topic" /> <remap from="/odometry/filtered" to="/your/robot_localization/output/topic" /> </node> </launch>这些参数在主页上讨论。配置robot_localization在这一点上，与robot_localization的集成非常简单。只需将以下块添加到您的状态估计节点启动文件中： <rosparam param="odomN_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam> 确保将odomN更改为您的里程输入值（例如，odom1、odom2等）。另外，如果您希望包括高度数据，请将odomN_config的第三个值设置为true。注意，如果您在二维环境中操作，并且没有任何传感器测量Z或Z速度，您可以选择以下之一：将navsat_transform_node的zero_altitude参数设置为true，然后将odomN_config的第三个值设置为true。在robot_localization状态估计节点中将two_d_mode设置为true。您不应该需要修改状态估计节点内的_differential设置。GPS是一个绝对位置传感器，启用差分积分会破坏使用它的目的。Details我们将从一张图片开始。考虑一个机器人，它从某个纬度和经度开始，并带有一些航向。在本教程中，我们假设航向来自一个 IMU，该 IMU 在朝东时读数为 0，并根据 ROS 规范（即逆时针方向）增加。本教程的其余部分将参考图 2：REP-105提出了四个坐标系：base_link、odom、map和earth。base_link坐标系是与车辆刚性连接的坐标系。odom和map坐标系是世界固定坐标系，通常将它们的原点设置在车辆的起始位置和方向。earth坐标系用作多个map坐标系的共同参考坐标系，目前navsat_transform_node不支持earth坐标系。请注意，在图2中，机器人刚开始（t = 0），因此它的base_link、odom和map坐标系是对齐的。我们还可以为UTM网格定义一个坐标系，我们将其称为utm。在本教程中，我们将UTM网格坐标系称为utm。因此，我们要做的是创建一个utm->map的坐标变换。参考图2，这些概念（希望）是清晰的。UTM原点是与机器人的GPS位置相关联的UTM区域的$（0_{UTM},0_{UTM}）$点。机器人在UTM区域内的某个位置开始，位置为$（x_{UTM},y_{UTM}）$。机器人的初始方向是相对于UTM网格的X轴的某个角度$θ$。因此，我们的变换将需要知道$x_{UTM}$、$y_{UTM}$和$θ$。现在，我们需要将纬度和经度转换为UTM坐标。UTM网格假定X轴面向东，Y轴面向（真正的）北，Z轴朝上伸出地面。这符合REP-105规定的右手坐标系。REP还规定，偏航角为0表示我们正朝下X轴，偏航增加是逆时针方向。navsat_transform_node假定您的航向数据符合这个标准。但需要考虑两个因素：：IMU 驱动程序可能不允许用户应用磁偏角校正因子IMU驱动程序可能在面朝北时不正确地报告0，而不是在面朝东时（尽管其航向正确增减）。幸运的是，navsat_transform_node公开了两个参数来解决IMU数据可能的这些问题：magnetic_declination_radians和yaw_offset。参考图2，对于当前测量yaw值为imu_yaw的IMU，$ yaw_{imu} =-\omega-offset_{yaw}+θ $$θ = yaw_{imu}+\omega+offset_{yaw} $现在我们有一个平移$（x_{UTM},y_{UTM}）$和旋转$θ$，可以用来创建所需的utm -> map变换。我们使用这个变换将所有未来的GPS位置转换为机器人的本地坐标系。如果设置broadcast_utm_transform参数为true，navsat_transform_node还会广播这个变换。如果您对本教程有任何疑问，请随时在 answers.ros.org 上提问。
雷达点云数据保存为mat格式创建功能包在终端中执行以下命令以创建一个新的ROS功能包：catkin_create_pkg laser_data_collector_pkg rospy sensor_msgs scipy这将创建一个名为laser_data_collector_pkg的ROS功能包，并添加了所需的依赖项。创建src目录并将脚本添加到其中在功能包的根目录下创建src目录，并将你的Python脚本（比如laser_data_collector.py）复制到该目录。修改CMakeLists.txt在功能包的根目录下，找到CMakeLists.txt文件，修改下图163-166行的代码（按照你自己设定）构建功能包在你的Catkin工作空间中执行以下命令以构建你的ROS功能包：catkin_make运行脚本赋予py文件运行权限进入python文件所在目录chmod +x laser_data_collector.py运行python在一个终端中运行roscore，然后在另一个终端中执行以下命令启动你的节点：source devel/setup.bash rosrun laser_data_collector_pkg laser_data_collector.py这将启动你的ROS节点，开始订阅/scan主题，并在收集足够的数据后保存到MAT文件中。请注意，上述步骤是一种创建ROS节点的简单方式。如果你的功能包需要更复杂的结构或功能，请根据需要进行修改。保存数据代码3S数据采集#!/usr/bin/env python # -*- coding: utf-8 -*- import rospy import scipy.io as sio from sensor_msgs.msg import LaserScan from datetime import datetime from collections import deque class LaserDataCollector: def __init__(self): rospy.init_node('laser_data_collector') self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback) self.angle_increment = None self.angles = None self.ranges = None self.data_buffer = deque() # 使用队列来缓存数据 self.start_time = rospy.get_time() def scan_callback(self, data): self.angle_increment = data.angle_increment self.angles = [data.angle_min + data.angle_increment * i for i in range(len(data.ranges))] self.ranges = data.ranges self.data_buffer.append((self.angles, self.ranges)) current_time = rospy.get_time() if current_time - self.start_time >= 3: # 收集3秒的数据 self.save_to_mat() self.start_time = rospy.get_time() # 重置起始时间 self.data_buffer.clear() # 清空数据缓存 def save_to_mat(self): # 生成一个时间戳以用于MAT文件的文件名 timestamp = datetime.now().strftime("%Y%m%d%H%M%S") file_name = 'laser_scan_data_{}.mat'.format(timestamp) # 将数据从队列中收集起来 all_angles, all_ranges = zip(*self.data_buffer) # 保存数据为MAT文件 scan_data = { 'angleIncrement': self.angle_increment, 'angles': all_angles, 'ranges': all_ranges } sio.savemat(file_name, scan_data) rospy.loginfo('Laser scan data saved to {}'.format(file_name)) if __name__ == '__main__': try: data_collector = LaserDataCollector() rospy.spin() # 这里会一直运行直到数据采集完成 except rospy.ROSInterruptException: pass
实操全过程确保你已经在ROS环境中，可以通过运行以下命令来检查：roscore如果roscore成功启动，说明你已经在ROS环境中。创建一个ROS包（如果尚未创建）：在终端中导航到你的ROS工作空间（通常是catkin_ws/src），然后使用catkin_create_pkg命令创建一个ROS包，例如：cd ~/catkin_ws/src catkin_create_pkg my_icp_save rospy sensor_msgs这将创建一个名为my_icp_save的ROS包，并将rospy和sensor_msgs添加为依赖项。编写你的Python节点：在my_ros_package包的目录下，创建一个Python脚本，例如my_node.py，并将你的Python节点代码放入其中。#!/usr/bin/env python import rospy import scipy.io as sio from sensor_msgs.msg import LaserScan class LaserDataCollector: def __init__(self): rospy.init_node('laser_data_collector') self.front_scan_sub = rospy.Subscriber('/front_laser_scan_topic', LaserScan, self.front_scan_callback) self.rear_scan_sub = rospy.Subscriber('/rear_laser_scan_topic', LaserScan, self.rear_scan_callback) self.front_scan_data = None self.rear_scan_data = None def front_scan_callback(self, data): self.front_scan_data = { 'angles': data.angle_min + data.angle_increment * range(len(data.ranges)), 'ranges': data.ranges, 'angle_increment': data.angle_increment } self.save_to_mat() def rear_scan_callback(self, data): self.rear_scan_data = { 'angles': data.angle_min + data.angle_increment * range(len(data.ranges)), 'ranges': data.ranges, 'angle_increment': data.angle_increment } self.save_to_mat() def save_to_mat(self): if self.front_scan_data is not None and self.rear_scan_data is not None: # Combine and save the data as a MAT file combined_data = { 'front_scan_data': self.front_scan_data, 'rear_scan_data': self.rear_scan_data } sio.savemat('laser_scan_data.mat', combined_data) rospy.loginfo('Laser scan data saved to laser_scan_data.mat') if __name__ == '__main__': try: data_collector = LaserDataCollector() rospy.spin() except rospy.ROSInterruptException: pass构建ROS包：在终端中，返回到ROS工作空间的根目录，并运行catkin_make来构建你的ROS包：cd ~/catkin_ws catkin_make文件位置和权限：确保你的my_save.py文件位于ROS包的src目录下，并且具有可执行权限。你可以使用以下命令来添加可执行权限：chmod +x my_save.py安装scipy库：在终端中运行以下命令来安装scipy库：bashCopy code pip install scipy这将安装scipy库到你的Python环境中。运行ROS节点：运行你的ROS节点，你可以使用rosrun命令，例如：rosrun my_ros_package my_save.py这将在ROS中运行你的Python节点，使其能够订阅和发布ROS话题，包括激光扫描数据。