# UWB_SLAM

**Repository Path**: abonaventure/uwb_slam

## Basic Information

- **Project Name**: UWB_SLAM
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-05-15
- **Last Updated**: 2026-08-12

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 基于UWB芯片化模组的多源融合定位建图技术

## 项目简介

本项目面向地下车库、复杂立交等弱GNSS场景，提出并实现了一套完整的**多源融合定位与建图（SLAM）** 解决方案。系统融合了视觉、惯性、激光雷达与UWB测距信息，支持**单车高精度自运动估计**与**多车协同定位建图**，旨在为自动驾驶、车联网等应用提供连续、可靠的位置与地图服务。

项目核心技术指标已达到：
- **单车定位精度 ≤ 1%**，定向精度 ≤ 0.01 deg/m
- **多车协同定位精度 ≤ 0.5%**，定向精度 ≤ 0.005 deg/m
- **可协同建图车辆数 ≥ 3辆**

---

## 代码结构

项目分为两个主要模块，分别对应单车与多车功能，代码组织如下：

```
.
├── conodes/                    # 多车协同SLAM定位与建图代码
│   ├── config/                 # 配置文件（节点参数、仿真参数）
│   │   ├── nodes_config.yaml
│   │   └── simul_config.yaml
│   ├── include/                # 头文件（工具函数、SIM3计算）
│   │   ├── compute_sim3.h
│   │   ├── tic_toc.h
│   │   └── utility.h
│   ├── launch/                 # ROS启动文件
│   │   ├── conodes.launch
│   │   └── couwbs.launch
│   ├── src/                    # 核心源码
│   │   ├── factor/             # 因子图优化相关因子
│   │   ├── bag2data.cpp
│   │   ├── conodes_node.cpp
│   │   ├── data_manager.cpp/h
│   │   ├── estimator.cpp/h
│   │   ├── node.cpp/h
│   │   └── parameters.cpp/h
│   ├── CMakeLists.txt
│   ├── package.xml
│   └── ThirdParty/             # 第三方依赖（如GeographicLib）
│
├── mononodes/                  # 单车多源融合SLAM定位与建图代码
│   ├── config/                 # 不同雷达型号配置文件
│   │   ├── avia.yaml
│   │   ├── horizon.yaml
│   │   ├── lark.yaml
│   │   ├── marsim.yaml
│   │   ├── mid360.yaml
│   │   ├── ouster64.yaml
│   │   └── velodyne.yaml
│   ├── include/                # 头文件（数学工具、IKFOM/MSCKF等）
│   │   ├── common_lib.h
│   │   ├── Exp_mat.h
│   │   ├── matplotlibcpp.h
│   │   ├── so3_math.h
│   │   ├── use-ikfom.hpp
│   │   └── use-msckf.hpp
│   ├── launch/                 # ROS启动文件（各雷达型号对应launch）
│   │   ├── mapping_avia.launch
│   │   ├── mapping_horizon.launch
│   │   ├── mapping_mid360.launch
│   │   ├── mapping_velodyne.launch
│   │   └── mapping_velodyne1.launch
│   ├── src/                    # 核心源码
│   │   ├── features/           # 特征提取与处理
│   │   ├── fail_old/           # 废弃旧代码
│   │   ├── myso3/              # SO3数学工具
│   │   ├── balmclass.hpp
│   │   ├── balm_front_back.cpp
│   │   ├── balm_only_back.cpp
│   │   ├── distortion_voxel.hpp
│   │   ├── DSmsckf.cpp
│   │   ├── IMU_Processing.hpp
│   │   ├── IMU_Processing_clone.hpp
│   │   ├── laserMapping.cpp
│   │   ├── loamscan2map.cpp
│   │   ├── msckf.cpp
│   │   ├── oc_lins.cpp
│   │   ├── preprocess.cpp/h
│   │   ├── Raw_voxel.cpp
│   │   ├── readpcd.cpp
│   │   └── voxel_extraction.cpp
│   ├── CMakeLists.txt
│   ├── package.xml
│   ├── README.en.md
│   └── README.md
│
└── README.md                     # 代码工程说明 
```

### 模块说明

| 目录 | 功能描述 |
| :--- | :--- |
| **mononodes** | **单车多源融合SLAM**：融合双目视觉、IMU、激光雷达，实现高精度、鲁棒的单车自运动估计与地图构建。实现了视觉-惯性-雷达紧耦合的里程计与后端优化。 |
| **conodes**   | **多车协同SLAM**：在单车里程计基础上，利用UWB测距与节点间里程计信息交换，通过分布式因子图优化，实现统一坐标系下的多车相对位姿估计与协同建图。集成了初始化与NLOS处理模块。 |

---

## 系统框架与核心算法

### 单车多源融合SLAM

系统以激光雷达-惯性里程计为核心，融合视觉惯性里程计结果，通过异常检测机制应对单一传感器退化。主要流程包括：

- **前端**：点云特征提取（边缘/平面）、双目视觉ORB特征、IMU预积分与状态预测。
- **后端**：基于滑动窗口的局部集束调整（BA）与因子图优化，估计IMU偏置并优化位姿。
- **融合策略**：当视觉或雷达特征退化时，系统可切换至单一可靠子系统，保证连续运行。

### 多车协同SLAM

每个节点独立运行单车里程计，并通过UWB标签进行节点间测距。系统核心包括：

- **测量模型**：距离测量与里程计测量均建模为因子，纳入滑动窗口优化。
- **初始化方法**：基于三角化不确定度与加权统计，实现不同间距下的快速相对位姿初始化。
- **NLOS处理**：通过一致性检测与测距筛选，有效抑制非视距测量野值。
- **分布式估计**：各节点在板载计算平台（Jetson AGX）上独立运行，仅交换里程计结果（带宽<500字节/秒），优化耗时<10毫秒。

---

## 编译与运行

### 依赖环境

- **操作系统**：Ubuntu 18.04 / 20.04
- **ROS**：Melodic / Noetic
- **主要依赖**：C++11及以上、Eigen、OpenCV、Ceres Solver、PCL、GTSAM（可选）

### 编译步骤

1. 克隆仓库并进入工作空间。
2. 安装依赖（以ROS Melodic为例）：

```bash
sudo apt install ros-melodic-pcl-ros ros-melodic-cv-bridge ros-melodic-tf2-geometry-msgs
sudo apt install libeigen3-dev libceres-dev libpcl-dev
```

3. 编译项目：

```bash
cd /path/to/your/catkin_workspace/src
git clone https://gitee.com/abonaventure/uwb_slam.git
cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release
source devel/setup.bash
```

### 运行示例

#### 单车SLAM（以Velodyne雷达为例）

```bash
roslaunch mononodes mapping_velodyne.launch
# 播放数据包或接入实时传感器数据
```

#### 多车协同（以3节点为例）

```bash
# 分别在三个终端（或三台车）启动各节点
roslaunch conodes couwbs.launch node_id:=1
roslaunch conodes couwbs.launch node_id:=2
roslaunch conodes couwbs.launch node_id:=3
```

> **注意**：具体启动文件与参数需根据实际传感器配置调整，详见各目录下的 `config` 与 `launch` 文件。

---

## 实验结果概览

### 单车里程计精度

| 实验序列 | 轨迹长度 | APE (LVIO) | RPE (LVIO) |
| :--- | :--- | :--- | :--- |
| 序列1 | 50.16m | **0.125m** / 3.465° | **0.121m** / 0.723° |
| 序列2 | 58.22m | **0.154m** / 3.888° | **0.128m** / 0.907° |
| 序列3 | 87.49m | **0.238m** / 4.144° | **0.171m** / 2.172° |

融合里程计（LVIO）在全局一致性与鲁棒性上均优于单一VIO或LIO。

### 多车协同定位精度（三节点测试）

| 相对关系 | 仅测距结果 | 协同估计结果 |
| :--- | :--- | :--- |
| 节点2→1 | 0.366m / 5.357° | **0.113m** / **3.234°** |
| 节点3→1 | 0.321m / 5.297° | **0.086m** / **2.489°** |
| 节点3→2 | 0.594m / 6.750° | **0.144m** / **3.750°** |

协同方法显著改善了相对定位精度与轨迹全局一致性。

---

## 许可证
商业专有许可证，详见 LICENSE 文件。

## 作者
Yongji Gao
Email: bonaventure@163.com