一种井下隧道多传感器融合算法的定位建图方法与流程

本发明属于无人驾驶，具体地说是一种井下隧道多传感器融合算法的定位建图方法。

背景技术：

1、近些年，随着无人车的不断发展，无人驾驶领域逐步渗透到各行各业，其中研究井下车辆的无人驾驶正在逐步兴起。作为无人驾驶领域使用最为频繁的激光雷达，在一些结构特征不够丰富的井下场景时，基于点云信息的激光slam将会出现退化现象，导致机器人定位误差大以及建图失效，进而将会导致后续导航任务的失败；imu可以获取加速度和角速度信息，而且输出频率较高，但是imu获取的测量由于是通过积分运算得到的，不可避免的会存在误差累积，长时间就会存在较大误差；uwb超宽带定位技术常用于无人车在室内的定位，且uwb定位精度高，有较强的抗干扰能力。

技术实现思路

1、本发明提供一种井下隧道多传感器融合算法的定位建图方法，利用多种传感器的优势互补，用以解决现有技术中的缺陷。

2、本发明通过以下技术方案予以实现：

3、一种井下隧道多传感器融合算法的定位建图方法，包括如下步骤：

4、步骤一：根据先验的高精度点云地图，通过去除隧道底面和顶部的方法，获取隧道的轮廓投影，得到隧道的轮廓投影图，构建全局地图，结合每个路口的静态扫描模型，以及在两个路口的中间位置放置反光强度较强的反光柱或反光膜，作为激光雷达两帧点云的配准依据；为了进一步获得较好的车辆位姿，根据先验的高精度点云地图，首先确定反光强度较强的点云，然后通过精密测量确定隧道设施在高精地图中所对应的点云，将这两类点云保留，存储为地标地图，然后将当前扫描点云中的隧道设施点云与对应的局部地标地图进行icp配准，得到全局位置，对全局位置约束；

5、步骤二：定位系统初始化，uwb定位标签位于车身后轴重心，uwb定位基站位于隧道路口处，imu传感器与激光雷达传感器都固定安装在无人车辆平台上，车辆从隧道起始路口出发，接收到位于路口处的uwb信号，获得车辆在隧道路口处的位置估计，此时使用基于路口特征的点云配准算法以及提取的隧道设施点云与局部地标地图的icp配准算法实现车辆位置矫正；

6、步骤三：最后利用激光雷达(lidar)、imu和uwb的融合算法做车辆的实时位姿估计。

7、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的步骤二中在每一个路口处放置一个uwb定位锚点，通过uwb锚点获取完整的3d定位，根据提取的特征点，确定关键帧，并实时更新一帧位置。

8、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的方法还包括基于随机一致性采样对雷达采集的原始数据进行地面和顶部的分割，得到激光里程计，得到激光里程计因子作为局部因子图优化的一个参数。

9、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的imu通过先对imu进行预积分，imu预积分因子作为局部因子图优化的一个参数。

10、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的步骤二中基于三维正态分布(3d-ndt)的点云配准方法来实现车辆在隧道路口处的位姿矫正。

11、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的步骤二中通过提取的隧道点云与局部地标地图的icp配准实现车辆的全局位姿约束，得到全局位置因子作为全局因子图优化的一个参数。

12、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的步骤三中采用基于全局因子图的多传感器融合定位算法获取车辆的实时位姿估计。

13、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的步骤三中全局地图点云通过全局因子图优化得到车辆的实时位姿。

14、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的局部因子图融合的lidar-imu里程计因子作为全局因子图优化的一个参数。

15、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的步骤三中uwb先进行uwb数据处理，得到的uwb测距因子作为全局因子图优化的一个参数。

16、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的lidar-imu预积分因子通过激光里程计因子和imu预积分因子通过局部因子图优化构建而来。

17、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的车辆的实时位姿与全局地图点云相结合，更新全局地图点云，然后将submap点云发送给前端激光里程计，矫正激光里程计；

18、如上所述的一种井下隧道多传感器融合算法的定位建图方法，所述的车辆的实时位姿与lidar隧道轮廓相结合，得到更新后的lidar隧道轮廓。

19、本发明的优点是：本发明的方法采用了lidar、imu和uwb的多传感器，利用其优势互补，解决了无人车辆在井下隧道等的激光雷达退化的环境下定位困难的问题，以及采用因子图的多传感器融合定位算法，获取到无人车辆的实时位姿估计，提高了车辆的定位精度，实现无人驾驶车辆的精确定位。

技术特征：

1.一种井下隧道多传感器融合算法的定位建图方法，其特征在于：包括如下步骤：

2.根据权利要求书1所述的一种井下隧道多传感器融合算法的定位建图方法，其特征在于：所述的步骤二中在每一个路口处放置一个uwb定位锚点，通过uwb锚点获取完整的3d定位，根据提取的特征点，确定关键帧，并实时更新一帧位置。

3.根据权利要求书1所述的种井下隧道多传感器融合算法的定位建图方法，其特征在于：所述的方法还包括基于随机一致性采样对雷达采集的原始数据进行地面和顶部的分割，得到激光里程计，得到激光里程计因子作为局部因子图优化的一个参数。

4.根据权利要求书1所述的一种井下隧道多传感器融合算法的定位建图方法，其特征在于：所述的imu通过先对imu进行预积分，imu预积分因子作为局部因子图优化的一个参数。

5.根据权利要求书1所述的一种井下隧道多传感器融合算法的定位建图方法，其特征在于：所述的步骤二中基于三维正态分布的点云配准方法来实现车辆在隧道路口处的位姿矫正。

6.根据权利要求书1所述的一种井下隧道多传感器融合算法的定位建图方法，其特征在于：所述的步骤三中采用基于全局因子图的多传感器融合定位算法获取车辆的实时位姿估计。

7.根据权利要求书1所述的一种井下隧道多传感器融合算法的定位建图方法，其特征在于：所述的步骤三中全局地图点云通过全局因子图优化得到车辆的实时位姿。

8.根据权利要求书6所述的一种井下隧道多传感器融合算法的定位建图方法，其特征在于：所述的局部因子图融合的lidar-imu里程计因子作为全局因子图优化的一个参数；

9.根据权利要求书1所述的一种井下隧道多传感器融合算法的定位建图方法，其特征在于：所述的车辆的实时位姿与全局地图点云相结合，更新全局地图点云，然后将submap点云发送给前端激光里程计，矫正激光里程计。

10.根据权利要求书1所述的一种井下隧道多传感器融合算法的定位建图方法，其特征在于：所述的车辆的实时位姿与lidar隧道轮廓相结合，得到更新后的lidar隧道轮廓。

技术总结
一种井下隧道多传感器融合算法的定位建图方法，包括如下步骤：步骤一：根据先验的高精度点云地图，获取隧道的轮廓投影，得到隧道的轮廓投影图,构建全局地图；步骤二：定位系统初始化，车辆从隧道起始路口出发，接收到位于路口处的UWB信号，获得车辆在隧道路口处的位置估计；步骤三：最后利用激光雷达、IMU和UWB的融合算法做车辆的实时位姿估计。本发明的方法采用了LiDAR、IMU和UWB的多传感器，利用其优势互补，解决了无人车辆在井下隧道等的激光雷达退化的环境下定位困难的问题，以及采用因子图的多传感器融合定位算法，获取到无人车辆的实时位姿估计，提高了车辆的定位精度。

技术研发人员：宋昊,吴宏涛,李朝霞,张林梁
受保护的技术使用者：山西省智慧交通研究院有限公司
技术研发日：
技术公布日：2024/2/6