# 速腾雷达定位导航案例 **Repository Path**: Cube004/Nav_robosense16p ## Basic Information - **Project Name**: 速腾雷达定位导航案例 - **Description**: No description available - **Primary Language**: Unknown - **License**: Not specified - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 0 - **Forks**: 0 - **Created**: 2024-11-03 - **Last Updated**: 2024-11-28 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # 速腾雷达定位导航案例 ## 1. 项目背景 本案例的背景是在户外环境下的菜园中使用速腾雷达进行定位和导航。该系统的目标是帮助机器人在菜园中自动进行作业，使用速腾雷达能够在复杂的户外环境中提供准确的定位，保证机器人在菜地中的导航稳定性。 ## 2. 系统架构 - **硬件部分**： 雷达: robosense Helios 16p IMU: CH110 地盘: Bunker NUC: Jetson AGX Xavier (ubuntu 18.04) - **算法部分**： `bunker_ros` 控制地盘 `fusion_pointclouds-master` 融合点云，后续加装多雷达传感器可以使用点云融合 `imu` CH110数据输出，获取IMU数据 `livox_ros_driver-master` Point-LIO的依赖 `localization_bringup` 启动定位和导航的功能包 `Point-LIO-point-lio-with-grid-map` 鲁棒高带宽激光惯性里程计,得到精准的里程计数据 `pointcloud_to_laserscan` 三维点云转二维雷达信息，用于局部代价地图生成 `rslidar_sdk` 启动雷达和发布点云话题 `rs_to_velodyne` 将雷达消息格式转换成能够被Point-LIO处理的velodyne雷达数据 `ugv_sdk` 地盘控制节点的依赖包 ## 3. 整体框架 ### 节点关系 ![](assets/rosgraph-17306084303873.png) 我们需要了解具体的实现思路，小车进行导航需要**地图**和**自身位置**。通过这两个信息我们可以为小车规划一个大致的路线，我们称之为**全局路径规划**。它主要是利用了全局地图(/map)形成的**全局代价地图**进行规划。 [详细说明链接]: http://www.autolabor.com.cn/book/ROSTutorials/di-7-zhang-ji-qi-ren-dao-822a28-fang-771f29/72-dao-hang-shi-xian/724-dao-hang-shi-xian-04-lu-jing-gui-hua.html "(路径规划和代价地图)" 我们可以在rviz的path插件中可视化看到规划的路线。但是全局的地图不一定精准，可能会存在动态的障碍物，需要小车去规避，以及在狭窄路段，小车要通过**局部代价**去规划局部路径，而小车主要是通过局部路径进行速度控制。 **局部代价地图需要二维雷达数据来构成 rostopic: /scan** 这个话题直接通过`pointcloud_to_laserscan`功能包将指定高度范围的点云转换成二维雷达扫描到的障碍物信息 **全局代价地图需要地图数据来构成 rostopic: /map** **自身位置是小车到map参考的变换，一般是指base_link到map参考系的变换** 这两种都是通过Cartographer节点获取 通过Cartographer或是Gmapping算法我们可以获得map地图以及小车在地图上的位置 这里我们使用鲁棒性更强一些的Cartographer算法 **Cartographer需要 IMU数据 雷达数据 里程计数据** **IMU数据**:数据可以直接通过HI226节点读取CH110的数据后直接发布 **雷达数据**:我们需要将原始雷达数据的部分点云进行剔除，避免自身被识别成障碍物，这里我们主要是通过/point_cloud_filter_node来过滤点云 ```c++ box_filter_.setMin(Eigen::Vector4f(-0.65, -0.3, -std::numeric_limits::max(), 1.0)); box_filter_.setMax(Eigen::Vector4f(-0.05, 0.3, std::numeric_limits::max(), 1.0)); ``` 以上是过滤指定范围内的点云，**注意在修改雷达位置后可能需要修改这些参数** 相比与Gmapping不同的是Cartographer可以直接通过点云数据进行二维地图栅格地图的构建，而且更加稳定一些 **里程计数据**:我们可以通过Point-LIO定位算法来获取也可以通过bunker_base提供的里程计数据，后者主要是通过轮式里程计获得，鲁棒性较差，不推荐 还有很多其他用于三维雷达的里程计估算算法，比如FAST_LIO,LIO_SLAM。由于位姿估算的延迟较大，这两种主要用于3d建图而不推荐作为实时里程计 ### TF树 ![](assets/frames-17306095402376.png) ## 4. 一些重要的参数配置 ### Point-LIO Point-LIO是一种新的激光雷达惯性里程计系统，它解决了帧间运动畸变问题，实现了4-8kHz的高里程计输出和超过IMU测量范围的高带宽。通过逐点状态更新和随机过程增强的运动学模型，即使在极端运动条件下也能提供准确估计。该系统在微型飞行器上实时运行，适用于快速运动场景的导航和建图。 参考文献 https://github.com/hku-mars/Point-LIO/files/10989136/Point-LIO_preprint.pdf 项目链接 https://github.com/hku-mars/Point-LIO ![](assets/point_lio.png) 这里主要说明如何运行和调整参数 laserMapping是Point-LIO的主节点。它需要两份数据的输入，雷达数据和IMU数据 官方源码的launch文件夹下提供了多种雷达的启用方式 ```bash Point-LIO-point-lio-with-grid-map/ ├── config │ ├── avia.yaml │ ├── horizon.yaml │ ├── ouster64.yaml │ └── velody16.yaml ├── launch │ ├── gdb_debug_example.launch │ ├── mapping_avia.launch │ ├── mapping_horizon.launch │ ├── mapping_ouster64.launch │ ├── mapping_velody16.launch ``` 这里我们只使用mapping_velody16.launch，我们需要将速腾雷达节点输出的雷达数据转换成velodyne雷达数据，因此我们需要使用到`rs_to_velodyne`功能包 接下来我们看到Point-LIO功能包下config/velody16.yaml,以下是我们可能需要改动的地方 ```yaml common: lid_topic: "/velodyne_points" #这里是我们雷达输入的话题 imu_topic: "/imu/data" #这里是我们IMU输入的话题 mapping: imu_time_inte: 0.005 # = 1 / frequency of IMU lidar_time_inte: 0.1 #这个是雷达的扫描间隔 #以下这一部分参数要根据IMU进行调整达到更好的效果请参考imu_utils标定方法得到imu的参数 #为此在file附件文件夹下给出了修改好了部分代码的imu_utils和code_utils，具体标定教程参考请参考其他教程 #-------------------------------------------------- acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration lidar_meas_cov: 0.01 # 0.001 acc_cov_output: 500 gyr_cov_output: 1000 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 imu_meas_acc_cov: 0.00052 #0.1 # 2 imu_meas_omg_cov: 1.06e-6 #0.1 # 2 gyr_cov_input: 1.15e-11 # for IMU as input model acc_cov_input: 1.20e-7 # for IMU as input model gravity: [0.0, 0.0, -9.810] # [-0.30, 0.880, -9.76] # liosam [0.0, 9.810, 0.0] # # preknown gravity, use when imu_en is false or start from a non-stationary state gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU #--------------------------------------------------- #以下是imu到雷达的位移的旋转矩阵，建议将imu安装在雷达中心正下方减少误差 extrinsic_T: [ 0.025, 0, 0.156] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ] ``` ### Cartographer ```xml ``` ```lua include "map_builder.lua" include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "imu_link", --这里填写imu话题的坐标系，必须一致 published_frame = "base_link", --这里主要填写小车的关节，且他没有父关节连接。该节点将被map连接作为map的子关节。 odom_frame = "odom", --这里填写如果根据里程计来发布map到里程计坐标系变换的父坐标系名称 provide_odom_frame = false, --，若确认发布则发布 odom->map的变换 publish_frame_projected_to_2d = true, use_odometry = true, ...... } TRAJECTORY_BUILDER = TRAJECTORY_BUILDER_2D MAP_BUILDER.use_trajectory_builder_2d = true MAP_BUILDER.use_trajectory_builder_3d = false MAP_BUILDER.num_background_threads = 4 --以下是有关点云建图的参数，请根据雷达的高度进行修改，以下参数是仅将雷达为中心一定范围内的点云数据作为参考来建图 TRAJECTORY_BUILDER_2D.min_range = 0.1 TRAJECTORY_BUILDER_2D.max_range = 10.0 TRAJECTORY_BUILDER_2D.min_z = -0.2 TRAJECTORY_BUILDER_2D.max_z = 0.2 ``` ## 5. 导航与路径规划 描述导航系统中路径规划模块的实现： - **全局路径规划**：使用默认 - **局部路径规划**： 以下为部分参数内容，以下内容对于小车行走影响非常大 基本参数基本不用变动 机器人速度限制的最大线速度可以适当大一些避免机器人小步慢走的情况 目标容忍度非特殊要求可以大一些避免小车在终点处不断尝试挪动 优化器参数权重尽量不要再修改，数值为多次修改得出的合理数值，也可以按注释的提示慢慢尝试修改得出适合目标地形的数值 优化迭代次数最好小一些避免机器人不断计算尝试最优路径而导致规划频率下降控制频率下降，反而导致机器人不怎么移动和移动卡顿 ```yaml TebLocalPlannerROS: # 基本参数 odom_topic: odom # 里程计的话题，用于获取机器人的位置信息 map_frame: map # 地图坐标系的名称，通常为 "map" 或 "odom" teb_autosize: true # 自动调整轨迹优化器的时间分辨率，以适应机器人速度 dt_ref: 0.3 # 轨迹优化器的参考时间间隔（单位：秒） dt_hysteresis: 0.1 # 参考时间间隔的迟滞（单位：秒），用于避免频繁调整时间分辨率 global_plan_overwrite_orientation: true # 在局部规划中覆盖全局路径的方向信息 max_global_plan_lookahead_dist: 3.0 # 在局部规划中从全局路径中考虑的最大距离（单位：米） feasibility_check_no_poses: 5 # 检查轨迹可行性时要考虑的位姿数量 # 机器人速度限制 max_vel_x: 2.0 # 机器人在 x 方向的最大速度（单位：米/秒），降低该值可提高控制精度 max_vel_x_backwards: 0.4 # 机器人向后移动的最大速度（单位：米/秒） max_vel_theta: 0.8 # 机器人旋转的最大速度（单位：弧度/秒） acc_lim_x: 0.8 # 机器人在 x 方向的最大加速度（单位：米/秒^2） acc_lim_theta: 0.9 # 机器人旋转的最大加速度（单位：弧度/秒^2） # 目标容忍度 xy_goal_tolerance: 0.3 # 目标位置的容忍度（单位：米），机器人到达目标点的允许误差 yaw_goal_tolerance: 2 # 目标方向的容忍度（单位：弧度） free_goal_vel: false # 到达目标时是否允许机器人自由移动 # 障碍物参数 min_obstacle_dist: 0.08 # 机器人与障碍物之间的最小安全距离（单位：米） inflation_dist: 0.10 # 膨胀距离，用于计算障碍物影响范围，应与 min_obstacle_dist 一致或略大 include_costmap_obstacles: true # 是否将代价地图中的障碍物纳入考虑 costmap_obstacles_behind_robot_dist: 0.5 # 考虑机器人后方障碍物的最大距离（单位：米） obstacle_poses_affected: 30 # 在轨迹优化中受障碍物影响的位姿数量 # 优化器权重参数 weight_max_vel_x: 1.0 # 对 x 方向最大速度的权重 weight_max_vel_theta: 3.5 # 对角速度最大值的权重 weight_acc_lim_x: 1.0 # 对 x 方向加速度限制的权重 weight_acc_lim_theta: 1.0 # 对角加速度限制的权重 weight_kinematics_nh: 3000.0 # 非完整性约束的权重，增大以严格约束机器人运动学 weight_kinematics_forward_drive: 0.5 # 降低强制前进驱动的权重 weight_kinematics_turning_radius: 3.0 weight_obstacle: 50.0 # 避障的权重，增大以提高避障优先级 weight_viapoint: 1.0 # 途径点的权重，用于引导轨迹经过指定的中间点 weight_dynamic_obstacle: 10.0 # 动态障碍物的权重 weight_dynamic_obstacle_inflation: 0.5 # 动态障碍物膨胀的权重 # 优化器迭代次数 no_inner_iterations: 3 # 内部优化循环的次数 no_outer_iterations: 2 # 外部优化循环的次数 # Homotopy 类规划 enable_homotopy_class_planning: false # 禁用多路径规划，避免不必要的计算 ``` ## 6. 使用说明 开机后首先设置权限 ```bash sudo ip link set can0 up type can bitrate 500000 ``` 一键启动详见`src/localization_bringup/launch/open_all.launch` ```xml 是否开启远程调试服务器，用于转发一些从机控制导航相关的话题 如果remote参数为否则直接打开rviz进行控制 打开雷达和IMU 打开point_lio计算里程计，进行建图或是重定位 打开路径规划节点 打开地盘控制节点 ``` 特别提醒：若要编译文件，出现时间报错请校准时间。建议每次开机后都进行校时。 ```bash sudo ntpdate -u time.cloudflare.com 在线校时 ``` ```bash sudo date -s "2024-01-01 00:00" 本地校时 ``` #### 附录：远程控制 远程控制画面 ![](assets/show-17327871183541.png) 先配置多机通信 ```bash ##小车设置 export ROS_MASTER_URI=http://192.168.XXX.XXX:11311 ##设置roscore服务通信的ip，一般设置为小车本机ip export ROS_HOSTNAME=192.168.XXX.XXX ##直接设置为小车ip ``` ```bash ##控制端 export ROS_MASTER_URI=http://192.168.XXX.XXX:11311 ##设置roscore服务通信的ip，一般设置为小车本机ip export ROS_HOSTNAME=192.168.XXX.XXX ##直接设置为控制端ip ``` 这里使用rviz的自定义插件来实现。 使用的是ros-noetic版本 再将相关插件进行编译，相关文件在file文件夹下 ```bash ├── CMakeLists.txt ├── bunker_model 小车的模型 │ ├── CMakeLists.txt │ ├── meshes │ │ └── bunker_mini_c1158_v3.stl │ ├── package.xml │ └── urdf │ └── bunker_mini.urdf ├── listen_rviz_plugin 指针位置监听器，用于放置目标点 │ ├── CMakeLists.txt │ ├── package.xml │ ├── plugin_description.xml │ └── src │ └── GoalCaptureTool.cpp ├── navi_multi_goals_pub_rviz_plugin 多点导航面板 │ ├── CMakeLists.txt │ ├── package.xml │ ├── plugin_description.xml │ └── src │ ├── multi_navi_goal_panel.cpp │ └── multi_navi_goal_panel.h └── service_mark 控制数据发送器 ├── CMakeLists.txt ├── package.xml ├── src │ ├── main.cpp │ └── tf_odom.cpp └── srv └── setGoal.srv ``` 编译后先刷新环境变量 ```bash source path/setup.bash ``` 然后可以直接打开rviz。 ```bash rviz ##直接打开rviz LIBGL_ALWAYS_SOFTWARE=1 rviz ##可选: 若是没有显示模型可以尝试使用软件直接渲染画面 ``` 然后加载参数文件,点击左上角file->open 找到文件`AGILEXNAV.rviz` **然后先设置多点导航面板的最大导航点数量** **接下来使用上方工具栏的goal capture tool放置目标点，进行标记** 特别的，若是多点导航面板出现乱码请安装字体 ```bash sudo apt-get install fonts-wqy-microhei ``` 若控制无效请检查该节点是否正在工作 ```xml ```