融合UWB进行SLAM建图

REF：基于多传感器融合的自动驾驶煤矿梭车同时定位与建图方法研究

1. 硬件

• 相机：按照结构不同可分单目相机、双目相机和 RGB-D 相机

3c7572623807f0cca7cc7c1485879106.png

• 激光雷达：按照结构不同可以分为单线激光雷达、多线激光雷达和固态激光雷达

efe0f2fd0aeb3c356c46e8fee25881b5.png

eef2ec758a0963b5b8e41bd0e68131c1.png

• 惯性测量单元（IMU）： 用于测量和报告物体的比力、角速度，
  • 加速度计：一种基于牛顿力学第二定律的传感器，即加速度与作用力成正比，与质量成反比，可以测量加速度确定物体的运动状态。
  • 陀螺仪：用于测量物体绕轴线旋转的角速度，感知物体姿态，原理基于旋转惯性、科里奥利效应，遵循角动量守恒，有对应物体横滚、俯仰、偏航方向的三个轴

  

  3e11480e6e7346fbd4c5f1abcf96c481.png

  

  b045189f8389f03b30a868001b68a7fd.png

  

  2277cf278925a9f25f3c0d35a47d5e66.png

• 超宽带技术（UWB）：基于带宽大于 500 MHz 或频谱占比超 20%的无线通信与定位技术，利用信号传播时间（对称双边双向测距）计算距离

34f1f76a5be2b183c1ce8890f9f56a4c.png

a527fdfb8e496071d72f9c0193a3d67b.png

• UWB定位方法

bdc789b57455cec867dd55d2e5a80d78.png

• 指纹定位
• 信号强度定位
• 到达时间（Time of Arrival，TOA）：测量信号从发射源传播至多个接收器所耗费的时间，依信号传播速度将时间换算为距离，再借几何关系算出发射源位置

9db0207f73ca22e5a88e08e7a33ba94c.png

• 到达时间差（Time Difference Of Arrival, TDOA）：通过对信号从同一个信号源传至多个接收器过程中的时间差进行测量来实现定位

70280b94c1531d98e678e0a345aeb2c6.png

• 到达角度（Angle of Arrival, AOA）：利用测量信号到达 UWB 标签的角度来确定基站的位置，标签发送超宽带信号，基站接收信号，测量信号到各天线相位差算出到达角度，综合多个基站角度信息确定信号源位置

5469e5805dc33896ca9d182255222cd2.png

2. 多传感器融合

• 因子图优化模型：状态节点是优化目标变量，路标为可观测自身状态的节点搭建观测方程，节点间有相邻状态节点的运动约束因子、路标与状态节点的观测约束因子两类，整合这两类约束，能够把复杂的最大后验估计难题，转变为相对简单的非线性优化问题，以此达成对所有状态量的联合估计

243eeb2b7373ad3786b860cd0d7068cf.png

• 时间同步：传感器帧率成整数倍可直接对齐帧，不成整数倍则运用内插外推法，激光雷达、IMU 和 UWB 这三类传感器的数据采样频率不同，将时间戳设定为横坐标，绘制出一幅时间序列图，用以展示各传感器的采样时刻

64e8272719aab06949a1dd1bbf457a6b.png

• IEKF融合激光雷达-IMU（紧耦合lio）：
  1. 状态向量定义：包含系统状态的关键参数，描述三维空间中的运动状态

  

  6af5dcb89609e0bcd0809c5f746d4242.png

  1. 系统建模：分为状态转移方程（描述系统状态随时间的变化）和观测方程（体现测量值与系统状态之间的关系）

  

  8fbbc3c6aa457b3a5b8005bd46ab673b.png

  

  1b50217e20f968da0af3e7377d30c1e6.png

  1. 预测阶段：利用前一时刻的状态估计和 IMU 的测量数据，预测当前时刻的系统状态

  

  fdf1381aadfc49c16ce923afafe8870e.png

  1. 更新阶段：利用激光雷达的测量数据对预测阶段得到的状态估计进行修正

  

  a74a2949152f47579eef73441872d1c3.png

  1. 迭代优化：由于系统模型和观测方程通常是非线性的，一次更新可能无法得到最优的状态估计，因此需要多次迭代，通过最大迭代次数或收敛阈值终止迭代
  2. IMU内参标定：辨识内部参数，包括陀螺仪和加速度计的零偏、标度因数和安装误差

  

  90c70d447901830b5c44ba5237908b49.png

  

  c1e660646660f790a9cc1bc7f21ebca5.png

  1. 激光雷达与 IMU 外参标定：旋转矩阵描述从 IMU 坐标系到激光雷达坐标系的旋转关系，平移向量表示 IMU 坐标系原点在激光雷达坐标系下的位置。
  2. IMU 预积分校正误差：在相邻关键帧间对 IMU 测量值预积分，得出相对旋转、速度和位置的预积分量

  

  dd6bd7ba85ee188fde4d4f0f863fc080.png

  1. 体素滤波：将三维空间划分为等大体素，计算各体素点云质心，实现点云降采样与滤波，进而保留特征的同时削减数据量

  

  55314196e6402a29fa9fb30c6bd5c76d.png

  1. 里程计去除点云畸变：通过IMU获取平台的运动信息，利用IMU的高频特性，在激光雷达扫描周期内精确的计算平台的位姿变化，将畸变的激光雷达点云恢复到统一的坐标系下，实现激光雷达点云去畸变

  

  00aecc5ae6fdd96906e18c93065e0372.png

  1. IEKF算法实现紧融合：通过对系统状态的预测与更新，能够充分利用激光雷达与 IMU 两种传感器的优势，提高系统的整体性能

34ed5845ad5360e407e53ec24e906d30.png

• 因子图进一步融合UWB
  • 激光惯性lio因子：前面用 IEKF 实现了激光雷达与 IMU数据的紧耦合为前端里程计，定义因子为：

  

  be5e5cf6d8c74e4a4f276bf9871096d9.png

  • UWB测距因子：

  

  e514de7c2cef256a86ff3b0678632a01.png

  • 全局点云优化： 预处理后的新点云与全局地图点云匹配后，将新点云融入全局地图，重合区域按新点云精度和可靠性更新，新区域添加至地图以扩大范围

  

  83749048a5a053fe2c4e01e160352a95.png

  1. 定义窗口大小：包含足够的数据点来反映数据的统计特征的同时，能适应数据的时效性

  

  4bb0170819d9b97d86025003cc13fb92.png

  1. 去除极值：识别并去除滑动窗口内偏离正常范围的测距异常值，设定相对于均值距离作为阈值

  

  6e9a16fcd647b593421022f05e6b2366.png

  1. 计算代表值：使用剩余数据点的均值作为该滑动窗口的代表值

  

  78d13909b834d229e0214a7acec6beae.png

  1. UWB测距：使用滑动窗口去极值取均值算法，减缓非视距及多径效应的干扰
  2. UWB基站布局：经过实验测试，最终选择4基站的矩形结构布置，并尽可能保证在同一竖直高度上，这样可以让定位标准差相对更小。

  

  1c82a97f894b696388adc8714db14bb8.png

  1. 使用因子图融合：全局因子图优化框架由激光惯性里程计因子与 UWB 测距因子联合构成。

  

  43e89db82df0e693b69c5bf4220170ee.png