Files
360/README.md
2025-07-16 18:37:38 +08:00

716 lines
16 KiB
Markdown
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
# FAST-LIO 雷达定位系统
<div align="center">
![FAST-LIO](https://img.shields.io/badge/FAST--LIO-v2.0-blue.svg)
![ROS2](https://img.shields.io/badge/ROS2-Humble-green.svg)
![Livox](https://img.shields.io/badge/Livox-MID360-orange.svg)
![Platform](https://img.shields.io/badge/Platform-Jetson-red.svg)
**高精度实时激光雷达惯性里程计定位系统**
基于 FAST-LIO 算法和 Livox MID360 雷达的实时 SLAM 定位解决方案
</div>
## 📋 目录
- [项目概述](#项目概述)
- [功能特性](#功能特性)
- [系统要求](#系统要求)
- [安装指南](#安装指南)
- [快速开始](#快速开始)
- [详细配置](#详细配置)
- [使用说明](#使用说明)
- [API文档](#api文档)
- [故障排除](#故障排除)
- [性能优化](#性能优化)
- [开发指南](#开发指南)
- [贡献说明](#贡献说明)
- [许可证](#许可证)
## 🎯 项目概述
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+
### 依赖库
```bash
# 核心依赖
ros-humble-desktop-full
ros-humble-pcl-ros
ros-humble-livox-ros-driver2
# 第三方库
libeigen3-dev
libpcl-dev
libopencv-dev
```
## 🚀 安装指南
### 1. 环境准备
```bash
# 更新系统
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. 工作区创建
```bash
# 创建工作区
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. 编译安装
```bash
# 初始化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. 硬件连接
```bash
# 配置网络 (修改为实际网卡名称)
sudo ip addr add 192.168.1.5/24 dev eth0
sudo ip link set eth0 up
# 测试连接
ping 192.168.1.194
```
## ⚡ 快速开始
### 基本启动流程
```bash
# 1. 启动雷达驱动
cd ~/mid360test
source install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360_launch.py
```
```bash
# 2. 启动FAST-LIO定位 (新终端)
cd ~/mid360test
source install/setup.bash
ros2 launch fast_lio mapping.launch.py
```
```bash
# 3. 启动距离计算器 (新终端)
cd ~/mid360test
source install/setup.bash
python3 distance_calculator.py
```
### 验证系统运行
```bash
# 检查话题
ros2 topic list
# 查看定位数据
ros2 topic echo /Odometry --once
# 检查数据频率
ros2 topic hz /Odometry
```
## ⚙️ 详细配置
### 雷达配置文件
编辑 `src/livox_ros_driver2/config/MID360_config.json`:
```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`:
```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. 启动系统
```bash
# 方式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. 停止系统
```bash
# 停止各个进程
Ctrl+C # 在各个终端中
# 查看最终统计报告
# 距离计算器会自动显示总结
```
### 数据获取
#### 位置信息获取
```bash
# 实时位置 (单次)
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} # 姿态(四元数)
```
#### 轨迹数据获取
```bash
# 轨迹路径
ros2 topic echo /path --once
# 保存轨迹到文件
ros2 bag record /Odometry /path
```
#### 距离计算
距离计算器会自动生成轨迹文件:
```
trajectory_log_YYYYMMDD_HHMMSS.txt
```
文件格式:
```csv
时间戳,运行时间(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界面说明
- **绿色点云**: 实时构建的环境地图
- **红色轨迹线**: 雷达运动路径
- **坐标轴**: 当前位置和姿态
- **彩色点**: 当前扫描数据
#### 界面操作
- **鼠标左键拖拽**: 旋转视角
- **鼠标右键拖拽**: 平移视角
- **滚轮**: 缩放视角
- **中键拖拽**: 平移视角
### 高级功能
#### 地图保存
```bash
# 地图自动保存在
ls src/FAST_LIO/PCD/
# 手动保存当前地图
ros2 service call /save_map std_srvs/srv/Trigger
```
#### 参数调优
```bash
# 运行时调整参数
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惯性数据 |
### 服务接口
```bash
# 保存地图
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
```bash
# 检查网络配置
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`话题无数据
```bash
# 检查数据流
ros2 topic hz /livox/lidar
ros2 topic hz /livox/imu
# 可能原因和解决
# - 系统未初始化: 等待5-10秒
# - 需要运动激励: 缓慢移动雷达
# - 进程冲突: 重启所有进程
```
#### 3. 定位精度差
**症状**: 位置跳跃或漂移
```bash
# 检查环境
# - 确保有足够几何特征
# - 避免玻璃、镜面等反射面
# - 检查雷达安装稳定性
# 参数调优
ros2 param set /laser_mapping filter_size_surf 0.3
```
#### 4. RViz2显示异常
**症状**: 无点云或轨迹显示
```bash
# 检查Fixed Frame设置
# 确保设置为 "camera_init"
# 重启RViz2
pkill rviz2
ros2 run rviz2 rviz2
```
#### 5. 编译错误
```bash
# 常见编译问题
# 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空间 |
### 诊断工具
```bash
# 系统状态检查脚本
./scripts/system_check.sh
# 网络诊断
./scripts/network_check.sh
# 性能监控
./scripts/performance_monitor.sh
```
## ⚡ 性能优化
### 系统级优化
```bash
# 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
```
### 算法参数优化
```yaml
# 高性能模式 (牺牲精度)
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
```
### 自定义开发
#### 添加新的传感器
```cpp
// 在FAST_LIO中添加新传感器支持
// 1. 修改preprocess.h
// 2. 实现数据处理函数
// 3. 更新CMakeLists.txt
```
#### 扩展距离计算器
```python
# distance_calculator.py
class ExtendedCalculator(DistanceCalculator):
def __init__(self):
super().__init__()
# 添加新功能
def custom_analysis(self):
# 自定义分析功能
pass
```
#### 集成其他算法
```bash
# 添加新的SLAM算法
cd src
git clone https://github.com/your-slam-algorithm.git
# 修改launch文件
# 更新依赖关系
```
### 测试框架
```bash
# 单元测试
colcon test --packages-select fast_lio
# 集成测试
./scripts/integration_test.sh
# 性能测试
./scripts/benchmark.sh
```
## 🤝 贡献说明
我们欢迎社区贡献!请遵循以下指南:
### 贡献流程
1. **Fork** 项目到您的账户
2. **创建** 功能分支 (`git checkout -b feature/AmazingFeature`)
3. **提交** 更改 (`git commit -m 'Add some AmazingFeature'`)
4. **推送** 分支 (`git push origin feature/AmazingFeature`)
5. **提交** Pull Request
### 代码规范
- 遵循 **Google C++ Style Guide**
- 使用 **4空格缩进**
- 添加适当的 **注释和文档**
- 确保 **代码通过测试**
### Bug报告
提交Bug时请包含
- 详细的问题描述
- 复现步骤
- 系统环境信息
- 相关日志文件
### 功能请求
提交功能请求时请说明:
- 功能的详细描述
- 使用场景和必要性
- 可能的实现方案
## 📞 技术支持
### 获取帮助
- **GitHub Issues**: [提交问题](https://github.com/your-repo/issues)
- **讨论区**: [GitHub Discussions](https://github.com/your-repo/discussions)
- **技术文档**: [Wiki页面](https://github.com/your-repo/wiki)
### 社区资源
- **FAST-LIO论文**: [Fast Direct LiDAR-Inertial Odometry](https://arxiv.org/abs/2010.08196)
- **Livox官网**: [https://www.livoxtech.com/](https://www.livoxtech.com/)
- **ROS2文档**: [https://docs.ros.org/en/humble/](https://docs.ros.org/en/humble/)
## 📜 许可证
本项目采用 **MIT License** 许可证 - 详见 [LICENSE](LICENSE) 文件。
### 第三方许可证
- **FAST-LIO**: GPLv2 License
- **Livox SDK**: MIT License
- **ROS2**: Apache 2.0 License
## 🙏 致谢
感谢以下项目和贡献者:
- [FAST-LIO](https://github.com/hku-mars/FAST_LIO) - HKU-Mars实验室
- [Livox-SDK](https://github.com/Livox-SDK) - Livox Technology
- [ROS2](https://github.com/ros2) - Open Source Robotics Foundation
---
<div align="center">
**⭐ 如果这个项目对您有帮助,请给我们一个星标!**
Made with ❤️ by the FAST-LIO Community
</div>