SW1AG7/360

Fork 0

Go to file

SW1AG7 994d314f09 上传文件至 /

2025-07-16 18:41:26 +08:00

autostart_core.sh

上传文件至 /

2025-07-16 18:37:38 +08:00

distance_calculator.py

上传文件至 /

2025-07-16 18:37:38 +08:00

laserMapping.cpp

上传文件至 /

2025-07-16 18:37:38 +08:00

mid360.yaml

上传文件至 /

2025-07-16 18:41:26 +08:00

preprocess.cpp

上传文件至 /

2025-07-16 18:37:38 +08:00

README.md

上传文件至 /

2025-07-16 18:37:38 +08:00

README.md

FAST-LIO 雷达定位系统

高精度实时激光雷达惯性里程计定位系统

基于 FAST-LIO 算法和 Livox MID360 雷达的实时 SLAM 定位解决方案

🎯 项目概述

FAST-LIO 雷达定位系统是一个基于 FAST-LIO 算法和 Livox MID360 雷达的高精度实时定位解决方案。该系统融合激光雷达和IMU数据，提供厘米级的位置精度和完整的轨迹跟踪功能。

核心技术

FAST-LIO: 快速激光雷达惯性里程计算法
Livox MID360: 中远距离激光雷达传感器
ROS2 Humble: 机器人操作系统框架
ikd-Tree: 增量式KD树数据结构

应用场景

🏗️ 室内定位导航
🚗 自动驾驶车辆
🤖 移动机器人SLAM
📐 精密测量与建图
🔬 科研与教育

✨ 功能特性

🎯 核心功能

实时定位: 提供 X、Y、Z 三维坐标位置
姿态估计: 完整的 Roll、Pitch、Yaw 角度信息
轨迹跟踪: 完整的运动路径记录
距离计算: 自动累计移动距离统计
地图构建: 实时3D点云地图生成

📊 性能指标

定位精度: ±2cm
更新频率: 10Hz
延迟: <100ms
检测范围: 100m
角度分辨率: 0.1°

🛠️ 系统特性

即插即用: 简单的配置和启动
实时可视化: RViz2 3D显示界面
数据记录: 轨迹数据自动保存
模块化设计: 易于集成和扩展
跨平台支持: Linux/Ubuntu/Jetson

🔧 系统要求

硬件要求

处理器: ARM64 或 x86_64 (推荐 4核以上)
内存: 4GB RAM (推荐 8GB)
存储: 10GB 可用空间
网络: 千兆以太网接口
雷达: Livox MID360 或兼容型号

软件要求

操作系统: Ubuntu 22.04 LTS
ROS版本: ROS2 Humble
编译器: GCC 9.0+
CMake: 3.16+
Python: 3.8+

依赖库

# 核心依赖
ros-humble-desktop-full
ros-humble-pcl-ros
ros-humble-livox-ros-driver2

# 第三方库
libeigen3-dev
libpcl-dev
libopencv-dev

🚀 安装指南

1. 环境准备

# 更新系统
sudo apt update && sudo apt upgrade -y

# 安装ROS2 Humble
wget -qO - https://raw.githubusercontent.com/ros/rosdistro/master/ros.key | sudo apt-key add -
sudo sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu jammy main" > /etc/apt/sources.list.d/ros2-latest.list'
sudo apt update
sudo apt install ros-humble-desktop-full -y

# 安装依赖
sudo apt install -y \
    python3-colcon-common-extensions \
    python3-rosdep \
    libeigen3-dev \
    libpcl-dev \
    libopencv-dev \
    ros-humble-pcl-ros \
    ros-humble-rviz2

2. 工作区创建

# 创建工作区
mkdir -p ~/mid360test/src
cd ~/mid360test

# 克隆源码
cd src
git clone https://github.com/Livox-SDK/livox_ros_driver2.git
git clone https://github.com/hku-mars/FAST_LIO.git
git clone https://github.com/Livox-SDK/Livox-SDK2.git

3. 编译安装

# 初始化rosdep
sudo rosdep init
rosdep update

# 安装依赖
cd ~/mid360test
rosdep install --from-paths src --ignore-src -r -y

# 编译
colcon build

# 设置环境
echo "source ~/mid360test/install/setup.bash" >> ~/.bashrc
source ~/.bashrc

4. 硬件连接

# 配置网络 (修改为实际网卡名称)
sudo ip addr add 192.168.1.5/24 dev eth0
sudo ip link set eth0 up

# 测试连接
ping 192.168.1.194

⚡ 快速开始

基本启动流程

# 1. 启动雷达驱动
cd ~/mid360test
source install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360_launch.py

# 2. 启动FAST-LIO定位 (新终端)
cd ~/mid360test
source install/setup.bash
ros2 launch fast_lio mapping.launch.py

# 3. 启动距离计算器 (新终端)
cd ~/mid360test
source install/setup.bash
python3 distance_calculator.py

验证系统运行

# 检查话题
ros2 topic list

# 查看定位数据
ros2 topic echo /Odometry --once

# 检查数据频率
ros2 topic hz /Odometry

⚙️ 详细配置

雷达配置文件

编辑 src/livox_ros_driver2/config/MID360_config.json:

{
  "lidar_summary_info": {
    "lidar_type": 8
  },
  "MID360": {
    "lidar_net_info": {
      "cmd_data_port": 56100,
      "push_msg_port": 56200,
      "point_data_port": 56300,
      "imu_data_port": 56400,
      "log_data_port": 56500
    },
    "host_net_info": {
      "cmd_data_ip": "192.168.1.5",     // 主机IP
      "cmd_data_port": 56101,
      "push_msg_ip": "192.168.1.5",
      "push_msg_port": 56201,
      "point_data_ip": "192.168.1.5",
      "point_data_port": 56301,
      "imu_data_ip": "192.168.1.5",
      "imu_data_port": 56401,
      "log_data_ip": "",
      "log_data_port": 56501
    }
  },
  "lidar_configs": [
    {
      "ip": "192.168.1.194",           // 雷达IP
      "pcl_data_type": 1,
      "pattern_mode": 0,
      "extrinsic_parameter": {
        "roll": 0.0,
        "pitch": 0.0,
        "yaw": 0.0,
        "x": 0,
        "y": 0,
        "z": 0
      }
    }
  ]
}

FAST-LIO参数配置

编辑 src/FAST_LIO/config/mid360.yaml:

/**:
  ros__parameters:
    # 基础参数
    feature_extract_enable: false
    point_filter_num: 3
    max_iteration: 3
    filter_size_surf: 0.5
    filter_size_map: 0.5
    cube_side_length: 1000.0
    
    common:
      lid_topic: "/livox/lidar"
      imu_topic: "/livox/imu"
      time_sync_en: false
      time_offset_lidar_to_imu: 0.0
    
    preprocess:
      lidar_type: 1                    # 1: Livox雷达
      scan_line: 4
      blind: 0.5                       # 盲区距离
      timestamp_unit: 3
      scan_rate: 10
    
    mapping:
      acc_cov: 0.1                     # 加速度协方差
      gyr_cov: 0.1                     # 陀螺仪协方差
      b_acc_cov: 0.0001
      b_gyr_cov: 0.0001
      fov_degree: 360.0                # 视场角
      det_range: 100.0                 # 检测范围
      extrinsic_est_en: true           # 外参自动估计
      
    publish:
      path_en: true                    # 发布路径
      scan_publish_en: true            # 发布点云
      dense_publish_en: false
      scan_bodyframe_pub_en: true
      
    pcd_save:
      pcd_save_en: true                # 保存PCD文件
      interval: -1                     # 保存间隔

📖 使用说明

基本操作

1. 启动系统

# 方式1: 分步启动 (推荐)
ros2 launch livox_ros_driver2 msg_MID360_launch.py    # 终端1
ros2 launch fast_lio mapping.launch.py               # 终端2
python3 distance_calculator.py                       # 终端3

# 方式2: 一键启动
./start_system.sh

2. 系统初始化

启动后保持雷达静止 5-10秒
等待系统完成初始化
看到 "Node init finished" 提示

3. 开始定位

缓慢移动雷达设备
观察RViz2中的实时轨迹
监控距离计算器输出

4. 停止系统

# 停止各个进程
Ctrl+C  # 在各个终端中

# 查看最终统计报告
# 距离计算器会自动显示总结

数据获取

位置信息获取

# 实时位置 (单次)
ros2 topic echo /Odometry --once

# 持续监控
ros2 topic echo /Odometry

# 位置数据格式
header:
  stamp: {sec: xxx, nanosec: xxx}
  frame_id: camera_init
pose:
  pose:
    position: {x: 1.23, y: -0.45, z: 0.67}    # 位置(米)
    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}  # 姿态(四元数)

轨迹数据获取

# 轨迹路径
ros2 topic echo /path --once

# 保存轨迹到文件
ros2 bag record /Odometry /path

距离计算

距离计算器会自动生成轨迹文件：

trajectory_log_YYYYMMDD_HHMMSS.txt

文件格式：

时间戳,运行时间(s),X坐标(m),Y坐标(m),Z坐标(m),累计距离(m),瞬时速度(m/s)
2025-07-01 20:56:28.170,0.142,0.068884,0.051447,0.012272,0.000000,0.000000

可视化界面

RViz2界面说明

绿色点云: 实时构建的环境地图
红色轨迹线: 雷达运动路径
坐标轴: 当前位置和姿态
彩色点: 当前扫描数据

界面操作

鼠标左键拖拽: 旋转视角
鼠标右键拖拽: 平移视角
滚轮: 缩放视角
中键拖拽: 平移视角

高级功能

地图保存

# 地图自动保存在
ls src/FAST_LIO/PCD/

# 手动保存当前地图
ros2 service call /save_map std_srvs/srv/Trigger

参数调优

# 运行时调整参数
ros2 param set /laser_mapping filter_size_surf 0.3
ros2 param set /laser_mapping cube_side_length 500.0

📡 API文档

ROS话题

发布话题

话题名称	消息类型	频率	描述
`/Odometry`	nav_msgs/Odometry	10Hz	位置和姿态信息
`/path`	nav_msgs/Path	1Hz	运动轨迹路径
`/cloud_registered`	sensor_msgs/PointCloud2	10Hz	配准后点云
`/Laser_map`	sensor_msgs/PointCloud2	1Hz	全局地图

订阅话题

话题名称	消息类型	描述
`/livox/lidar`	livox_ros_driver2/CustomMsg	雷达点云数据
`/livox/imu`	sensor_msgs/Imu	IMU惯性数据

服务接口

# 保存地图
ros2 service call /save_map std_srvs/srv/Trigger

# 重置定位
ros2 service call /reset_localization std_srvs/srv/Trigger

参数列表

参数名称	类型	默认值	描述
`filter_size_surf`	double	0.5	表面点滤波大小
`filter_size_map`	double	0.5	地图点滤波大小
`cube_side_length`	double	1000.0	局部地图边长
`det_range`	double	100.0	检测范围

🐛 故障排除

常见问题

1. 雷达连接失败

症状: 无法ping通雷达IP

# 检查网络配置
ip addr show
ping 192.168.1.194

# 解决方案
sudo ip addr add 192.168.1.5/24 dev eth0
sudo ip link set eth0 up

2. 无定位数据输出

症状: /Odometry话题无数据

# 检查数据流
ros2 topic hz /livox/lidar
ros2 topic hz /livox/imu

# 可能原因和解决
# - 系统未初始化: 等待5-10秒
# - 需要运动激励: 缓慢移动雷达
# - 进程冲突: 重启所有进程

3. 定位精度差

症状: 位置跳跃或漂移

# 检查环境
# - 确保有足够几何特征
# - 避免玻璃、镜面等反射面
# - 检查雷达安装稳定性

# 参数调优
ros2 param set /laser_mapping filter_size_surf 0.3

4. RViz2显示异常

症状: 无点云或轨迹显示

# 检查Fixed Frame设置
# 确保设置为 "camera_init"

# 重启RViz2
pkill rviz2
ros2 run rviz2 rviz2

5. 编译错误

# 常见编译问题
# ikd-Tree未找到
cd src/FAST_LIO/include
git clone https://github.com/hku-mars/ikd-Tree.git

# 依赖缺失
rosdep install --from-paths src --ignore-src -r -y

# 权限问题
sudo chown -R $USER:$USER ~/mid360test

错误代码

错误代码	描述	解决方案
`E001`	雷达连接超时	检查网络配置
`E002`	IMU数据异常	检查雷达连接
`E003`	定位初始化失败	提供运动激励
`E004`	内存不足	增加swap空间

诊断工具

# 系统状态检查脚本
./scripts/system_check.sh

# 网络诊断
./scripts/network_check.sh

# 性能监控
./scripts/performance_monitor.sh

⚡ 性能优化

系统级优化

# CPU性能优化
echo performance | sudo tee /sys/devices/system/cpu/cpu*/cpufreq/scaling_governor

# 内存优化
echo 1 | sudo tee /proc/sys/vm/swappiness

# 网络优化
sudo sysctl -w net.core.rmem_max=134217728
sudo sysctl -w net.core.wmem_max=134217728

算法参数优化

# 高性能模式 (牺牲精度)
filter_size_surf: 0.8
filter_size_map: 0.8
cube_side_length: 500.0

# 高精度模式 (牺牲性能)
filter_size_surf: 0.2
filter_size_map: 0.2
cube_side_length: 2000.0

硬件优化建议

CPU: 优先选择高主频多核处理器
内存: 推荐8GB以上DDR4
存储: 使用SSD提升IO性能
网络: 千兆以太网，低延迟网卡

🛠️ 开发指南

项目结构

mid360test/
├── src/
│   ├── livox_ros_driver2/      # 雷达驱动
│   ├── FAST_LIO/               # FAST-LIO算法
│   └── Livox-SDK2/             # Livox SDK
├── install/                    # 安装文件
├── build/                      # 编译文件
├── log/                        # 日志文件
├── distance_calculator.py      # 距离计算器
├── README.md                   # 项目文档
└── scripts/                    # 辅助脚本
    ├── start_system.sh
    ├── system_check.sh
    └── performance_monitor.sh

自定义开发

添加新的传感器

// 在FAST_LIO中添加新传感器支持
// 1. 修改preprocess.h
// 2. 实现数据处理函数
// 3. 更新CMakeLists.txt

扩展距离计算器

# distance_calculator.py
class ExtendedCalculator(DistanceCalculator):
    def __init__(self):
        super().__init__()
        # 添加新功能
        
    def custom_analysis(self):
        # 自定义分析功能
        pass

集成其他算法

# 添加新的SLAM算法
cd src
git clone https://github.com/your-slam-algorithm.git

# 修改launch文件
# 更新依赖关系

测试框架

# 单元测试
colcon test --packages-select fast_lio

# 集成测试
./scripts/integration_test.sh

# 性能测试
./scripts/benchmark.sh

🤝 贡献说明

我们欢迎社区贡献！请遵循以下指南：

贡献流程

Fork 项目到您的账户
创建功能分支 (git checkout -b feature/AmazingFeature)
提交更改 (git commit -m 'Add some AmazingFeature')
推送分支 (git push origin feature/AmazingFeature)
提交 Pull Request

代码规范

遵循 Google C++ Style Guide
使用 4空格缩进
添加适当的 注释和文档
确保 代码通过测试

Bug报告

提交Bug时请包含：

详细的问题描述
复现步骤
系统环境信息
相关日志文件

功能请求

提交功能请求时请说明：

功能的详细描述
使用场景和必要性
可能的实现方案

📞 技术支持

获取帮助

GitHub Issues: 提交问题
讨论区: GitHub Discussions
技术文档: Wiki页面

社区资源

FAST-LIO论文: Fast Direct LiDAR-Inertial Odometry
Livox官网: https://www.livoxtech.com/
ROS2文档: https://docs.ros.org/en/humble/

📜 许可证

本项目采用 MIT License 许可证 - 详见 LICENSE 文件。

第三方许可证

FAST-LIO: GPLv2 License
Livox SDK: MIT License
ROS2: Apache 2.0 License

🙏 致谢

感谢以下项目和贡献者：

FAST-LIO - HKU-Mars实验室
Livox-SDK - Livox Technology
ROS2 - Open Source Robotics Foundation

⭐ 如果这个项目对您有帮助，请给我们一个星标！

Made with ❤️ by the FAST-LIO Community

README.md Unescape Escape

FAST-LIO 雷达定位系统

📋 目录

🎯 项目概述

核心技术

应用场景

✨ 功能特性

🎯 核心功能

📊 性能指标

🛠️ 系统特性

🔧 系统要求

硬件要求

软件要求

依赖库

🚀 安装指南

1. 环境准备

2. 工作区创建

3. 编译安装

4. 硬件连接

⚡ 快速开始

基本启动流程

验证系统运行

⚙️ 详细配置

雷达配置文件

FAST-LIO参数配置

📖 使用说明

基本操作

1. 启动系统

2. 系统初始化

3. 开始定位

4. 停止系统

数据获取

位置信息获取

轨迹数据获取

距离计算

可视化界面

RViz2界面说明

界面操作

高级功能

地图保存

参数调优

📡 API文档

ROS话题

发布话题

订阅话题

服务接口

参数列表

🐛 故障排除

常见问题

1. 雷达连接失败

2. 无定位数据输出

3. 定位精度差

4. RViz2显示异常

5. 编译错误

错误代码

诊断工具

⚡ 性能优化

系统级优化

算法参数优化

硬件优化建议

🛠️ 开发指南

项目结构

自定义开发

添加新的传感器

扩展距离计算器

集成其他算法

测试框架

🤝 贡献说明

贡献流程

代码规范

Bug报告

功能请求

📞 技术支持

获取帮助

社区资源

📜 许可证

第三方许可证

🙏 致谢

README.md