wheatfox/go2_semantic_nav

基于大模型的机器狗室内语义导航方法研究

本项目是面向室内语义导航的研究性实现，融合 视觉-语言大模型 (VLM) 与 大语言模型 (LLM) 的感知与推理能力，使 Unitree GO2 四足机器人能够理解自然语言指令并自主导航至目标物体。系统集成 3D激光雷达定位、ROS导航栈与语音交互，实现「说出目标，自主抵达」的智能导航体验。

✨ 功能特点

模块	功能	技术栈
全局定位	离线点云地图重定位	FAST-LIO2 + Open3D ICP
路径规划	2D栅格地图避障导航	ROS move_base
语音控制	自然语言→导航目标	Whisper STT + LLM 推理
语义理解	场景物体识别与定位	VLM 标注 + 语义地图

🏗️ 系统架构

┌─────────────────────────────────────────────────────────────────┐
│                         应用层 (Application)                     │
│  ┌──────────────────────┐    ┌────────────────────────────────┐ │
│  │     Web Client       │    │      Goal Processor            │ │
│  │   (FastAPI + LLM)    │───▶│  (坐标转换 + 安全调度)          │ │
│  └──────────────────────┘    └────────────────────────────────┘ │
└─────────────────────────────────────────────────────────────────┘
                                      │
                                      ▼
┌─────────────────────────────────────────────────────────────────┐
│                         算法层 (Algorithm)                       │
│  ┌────────────────────┐  ┌──────────────┐  ┌────────────────┐   │
│  │   Localization     │  │   Map Pub    │  │   Move Base    │   │
│  │ (FAST-LIO+Open3D)  │  │  (2D Grid)   │  │  (Navigation)  │   │
│  └────────────────────┘  └──────────────┘  └────────────────┘   │
└─────────────────────────────────────────────────────────────────┘
                                      │
                                      ▼
┌─────────────────────────────────────────────────────────────────┐
│                         驱动层 (Drivers)                         │
│  ┌────────────────────┐  ┌────────────────┐  ┌───────────────┐  │
│  │  Livox MID360      │  │  LiDAR Filter  │  │    Bridge     │  │
│  │  (点云 + IMU)       │  │  (滤波降噪)    │  │  (运动控制)    │  │
│  └────────────────────┘  └────────────────┘  └───────────────┘  │
└─────────────────────────────────────────────────────────────────┘

📁 项目结构

go2_semantic_nav_system/
├── robot_ws/                    # ROS工作空间
│   └── src/
│       ├── livox_ros_driver2/   # Livox MID360 雷达驱动 (已修改)
│       ├── uni_localization/    # 定位模块
│       │   ├── FAST_LIO/        # 里程计 (LiDAR-Inertial Odometry)
│       │   └── open3d_loc/      # 全局定位 (ICP配准)
│       ├── uni_lidar_filter/    # 点云滤波
│       ├── uni_laserscan/       # 3D点云→2D激光扫描转换
│       ├── uni_map/             # 2D栅格地图发布
│       ├── uni_nav/             # move_base 导航配置
│       ├── uni_goal_processor/  # 目标点处理与安全调度
│       └── uni_bridge/          # 机器狗运动控制桥接
│
├── client_app/                  # 语音控制Web应用
│   ├── main.py                  # FastAPI 服务器
│   ├── static/                  # 前端页面
│   └── requirements.txt         # Python依赖
│
├── assets/                      # 资源文件
│   ├── maps/                    # 地图文件
│   │   ├── map.ply              # 3D点云地图
│   │   ├── map.pgm              # 2D栅格地图
│   │   ├── map.yaml             # 地图配置
│   │   └── semantic_map.json    # 语义标注
│   ├── prompts/                 # LLM提示词
│   │   ├── llm_prompt.txt       # 导航指令解析
│   │   └── vlm_prompt.txt       # 视觉场景分析
│   ├── config/                  # 配置文件
│   │   └── nav.rviz             # RViz 可视化配置
│   └── navigation/              # 导航参数
│
├── offline_tools/               # 离线工具
│   ├── pcd_to_ply.py            # 点云过滤与格式转换
│   ├── pcd_to_pgm.py            # 点云→2D栅格地图转换
│   └── alignment_gui.py         # 地图对齐标定GUI
│
├── docs/images/                 # 文档图片
│
└── scripts/                     # 启动脚本
    └── startup_robot.sh         # 一键启动 (tmux)

🚀 快速开始

1. 环境依赖

设备	操作系统	ROS 版本	备注
机器人 (GO2)	Ubuntu 20.04	Noetic	运行 `robot_ws`
PC 控制端	Ubuntu 20.04	Noetic	运行 RViz 与 `client_app`

⚠️ 重要：机器人与 PC 需在同一 ROS 网络中，确保 ROS_MASTER_URI 配置正确。

硬件: Unitree GO2 + Livox MID360

# 安装基础依赖
sudo apt install libc++-dev libc++abi-dev libeigen3-dev

# 安装 Open3D (推荐使用预编译版本)
# 下载: https://pan.baidu.com/s/1vTLXVYJ6JBlbhNpDf87Cdg?pwd=spdg
# 并修改 open3d_loc/CMakeLists.txt 中的 Open3D_DIR 路径

# 安装 Livox-SDK2
# 参考: https://github.com/Livox-SDK/Livox-SDK2

2. 编译

cd robot_ws
source /opt/ros/noetic/setup.bash
catkin_make -DROS_EDITION=ROS1
source devel/setup.bash

3. 配置

雷达IP: 修改 livox_ros_driver2/config/MID360_config.json
点云地图: 将 .ply 文件放入 assets/maps/，修改 launch 文件路径
API密钥: 复制 client_app/.env.example → .env，填入:
- OR_API_KEY: OpenRouter
- GROQ_API_KEY: Groq

4. 启动流程

系统需按以下顺序在不同设备上启动：

Step 1: 机器人端 (GO2)

# SSH 登录机器人后执行
cd ~/go2_semantic_nav_system
./scripts/startup_robot.sh

脚本启动的节点列表

roscore
Livox MID360 驱动
FAST-LIO 里程计
Open3D 全局定位
点云滤波 & LaserScan 转换
2D 地图发布
move_base 导航
目标处理 & 运动控制桥接

Step 2: PC 端 - RViz 初始化

# 设置 ROS 主节点 (替换为机器人 IP)
export ROS_MASTER_URI=http://<ROBOT_IP>:11311

# 启动 RViz 并加载配置
rviz -d assets/config/nav.rviz

在 RViz 中使用 2D Pose Estimate 工具点击地图设置机器人初始位姿。

Step 3: PC 端 - 语音控制客户端

启动 Web 客户端:

cd client_app
pip install -r requirements.txt
python main.py

访问 http://localhost:8000 开始语音导航。

工作流程

用户语音 ──▶ Whisper STT ──▶ LLM推理 ──▶ 语义地图匹配 ──▶ 坐标计算 ──▶ ROS导航
                               │
                               ▼
                         semantic_map.json
                         (物体bbox + 描述)

🗺️ 地图制作

1. 建图

有两种建图方式可选：

方式 A: 机器人激光雷达建图 (精度高)

使用 FAST-LIO2 驱动机器人遍历环境进行点云建图。

方式 B: 手机 LiDAR 建图 (便捷快速)

使用搭载激光雷达的手机 (iPhone Pro / iPad Pro) 配合扫描 App 进行建图：

推荐 App: 3D Scanner App
操作方式: 手持手机在场景中缓慢移动扫描，导出 .ply 格式点云

2. 后处理

使用 offline_tools/ 中的脚本进行点云处理：

cd offline_tools

# 点云过滤：离群点剔除 + 高度截取，生成定位用点云
python pcd_to_ply.py <z_min> <z_max> --input scans.pcd --output map.ply

# 生成 2D 栅格地图：用于导航
python pcd_to_pgm.py

💡 z_min 和 z_max 用于过滤指定高度范围内的点云，例如 0.1 0.6 表示保留 0.1m 到 0.6m 高度的点。

3. 语义标注

使用 VLM (如 Qwen3-VL) 对俯视图进行物体检测，生成 semantic_map.json。

4. 坐标对齐

使用 offline_tools/alignment_gui.py 对齐彩色俯视图与栅格地图坐标系。

地图对齐工具界面

📊 输出话题(部分)

Topic	类型	说明
`/localization_3d`	PoseStamped	定位结果 (6DoF)
`/localization_3d_confidence`	Float32	ICP配准置信度
`/localization_3d_delay_ms`	Float32	定位延迟 (ms)
`/cmd_vel`	Twist	速度指令
`/relative_goal_raw`	Point	LLM输出的图像坐标

🙏 致谢

FAST-LIO2 - 里程计
FAST_LIO_LOCALIZATION - 定位参考
Open3D - 点云处理
Livox SDK - 雷达驱动

📝 协议

本项目基于 LICENSE 发布。

GO2 Semantic Navigation System