当前位置：首页 > news >正文

mid360实现路径规划

news 2025/11/6 10:43:03

使用 Livox Mid360 激光雷达实现 路径规划 并支持 地图重定位 的完整流程如下。该流程基于 ROS1 (Noetic) + FAST-LIO 建图 + move_base 导航框架，适用于 Ubuntu 20.04 系统

✅ 一、系统环境与依赖安装

参考之前的文章：安装mid360驱动和跑fastlio2算法：https://blog.csdn.net/2301_79618994/article/details/146067579?spm=1001.2014.3001.5502

重定位模块：

https://blog.csdn.net/2301_79618994/article/details/153934600?spm=1001.2014.3001.5502

1. 系统要求

Ubuntu 20.04
ROS Noetic（已安装并配置好环境）

2. 安装依赖

# 安装 Livox SDK2 和驱动
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd Livox-SDK2
mkdir build && cd build
cmake .. && make -j
sudo make install# 安装 livox_ros_driver2
cd ~/catkin_ws/src
git clone https://github.com/Livox-SDK/livox_ros_driver2.git
cd ..
catkin_make

3. 安装 FAST-LIO 和导航相关包

# FAST-LIO
cd ~/catkin_ws/src
git clone https://github.com/hku-mars/FAST_LIO.git
cd ..
catkin_make# 安装 move_base, map_server, amcl 等
sudo apt install ros-noetic-move-base ros-noetic-map-server ros-noetic-amcl

✅ 二、建图（生成点云地图）

1. 配置雷达 IP

编辑文件：

~/catkin_ws/src/livox_ros_driver2/config/MID360_config.json

确保雷达 IP 正确（通常为 192.168.1.1xx）。

2. 启动雷达与建图

# 启动雷达
roslaunch livox_ros_driver2 msg_MID360.launch# 启动 FAST-LIO 建图
roslaunch fast_lio mapping_mid360.launch

建图时手动控制机器人缓慢移动，覆盖整个区域。

3. 保存点云地图（PCD）

# 保存点云地图
rosrun pcl_ros pointcloud_to_pcd input:=/laser_cloud_surround

或在 FAST_LIO/config/mid360.yaml 中设置：

pcd_save_en: true
map_file_path: "./test.pcd"

运行结束后会在指定路径生成 test.pcd。

✅ 三、地图转换（PCD → 2D 栅格地图）

自己写一个python3代码，可以实现：

安装包

python3 -m pip install wind-pypcd      # 兼容 Py3，功能同 pypcd[^26^]

#!/usr/bin/env python3
import numpy as np
import cv2
import yaml
from wind_pypcd import pypcd# 1. 读点云
pcd = pypcd.PointCloud.from_path('scans.pcd')
pts = np.stack([pcd.pc_data['x'], pcd.pc_data['y']], axis=1)# 2. 参数
res   = 0.05          # m/px
margin= 10            # px 留边# 3. 网格化（向下取整）
xy_min  = pts.min(0)
xy_max  = pts.max(0)
grid_min= np.floor(xy_min / res).astype(int) - margin
grid_max= np.ceil (xy_max / res).astype(int) + margin
w = int(grid_max[0] - grid_min[0] + 1)   # 列数
h = int(grid_max[1] - grid_min[1] + 1)   # 行数# 4. 坐标平移 → 从 0 开始
grid = np.floor((pts - xy_min) / res).astype(int)# 5. 裁剪到画布内（防越界）
grid[:,0] = np.clip(grid[:,0], 0, w-1)
grid[:,1] = np.clip(grid[:,1], 0, h-1)# 6. 画
img = np.full((h, w), 255, dtype=np.uint8)
img[grid[:,1], grid[:,0]] = 0# 7. 保存
cv2.imwrite('my_map.pgm', img)
origin = [float(xy_min[0]), float(xy_min[1]), 0.0]
yaml.dump({'image':'my_map.pgm','resolution':res,'origin':origin,'negate':0,'occupied_thresh':0.65,'free_thresh':0.25},open('my_map.yaml','w'))
print('my_map.pgm + my_map.yaml 生成完成')

复制粘贴放到PCD文件夹下面，运行即可；

✅ 四、重定位（Localization）

1. 启动雷达与定位节点

# 启动雷达
roslaunch livox_ros_driver2 msg_MID360.launch# 启动重定位节点
roslaunch fast_lio_localization sentry_localize.launch

2. 发布初始位姿

使用 RViz 发布初始位姿，或运行脚本：

rosrun fast_lio_localization publish_initial_pose.py 0 0 0 0 0 0

确保初始位姿与地图对齐，否则重定位失败。

✅ 五、路径规划与导航

1. 启动 move_base

roslaunch sentry_nav sentry_movebase.launch

2. 设置目标点

在 RViz 中使用 “2D Nav Goal” 工具点击目标点，机器人将自动规划路径并移动。

✅ 六、坐标系说明（重要）

map：二维栅格地图坐标系
camera_init：FAST-LIO 建图起始坐标系
body：机器人当前坐标系

你需要确保 map → camera_init → body 的 TF 树完整，否则导航会失败。

✅ 七、常见问题排查

问题	解决方案
重定位失败	检查初始位姿是否正确，TF 是否发布
地图不匹配	确保 PCD 地图与二维地图原点一致
导航不移动	检查 `/cmd_vel` 是否有输出，是否有障碍物