CN115407357A - 基于大场景的低线束激光雷达-imu-rtk定位建图算法 - Google Patents

Abstract

本发明公开了一种基于大场景的低线束激光雷达‑IMU‑RTK定位建图算法，包括：初始化阶段使用RTK位置信息快速精准的完成IMU对准，并解算激光雷达‑IMU‑RTK***在全局坐标下的正确初始位姿；提取周围点云环境特征，根据曲率区分角点和面点；实时构建局部点云地图，利用当前帧点云信息与局部地图动态匹配，计算当前激光雷达位姿；图优化模型构建激光雷达里程计因子、IMU预积分因子、RTK绝对位置因子，优化求解定位结果并进行实时点云地图拼接。本发明通过图优化模型，能够解决低线束激光雷达在大场景特征稀疏环境下定位和建图问题，同时能够保障实时采集。

Description

基于大场景的低线束激光雷达-IMU-RTK定位建图算法

技术领域

本发明属于激光雷达SLAM(Simultaneous Localization And Mapping)领域，涉及多传感器融合技术，具体涉及基于大场景的低线束激光雷达-IMU-RTK定位建图算法。

背景技术

获得精准、可靠的定位是移动机器人、自动驾驶的基本需求。近年来，视觉、激光同步定位与建图技术取得长足的发展。视觉定位依靠相机可实现六自由度状态估计，但视觉定位受限环境较多，容易受到***初始化、光照、低纹理特征等因素影响。而激光传感器可直接获得深度信息,并且具备较高分辨率，可在夜间工作，能实现更为准确的位姿估计。

点云匹配方案以往是迭代ICP算法，但容易陷入局部最小值，且配准耗时长。2014年发布的LOAM首次采用点线、点面特征匹配位姿计算，在建图过程利用帧与帧(scan-to-scan) 里程计结果初值和帧与图(scan-to-map)匹配共同更新维护点云地图。2020年发布的LIO-SAM 融合了激光雷达-IMU-GNSS三种传感器信息，采用IMU里程计高频解算、激光雷达里程计特征匹配的方式，GNSS和回环信息在建图过程用于抑制定位结果漂移，获得全局一致性较好的点云地图。

根据上述原理，激光雷达SLAM技术主要采用提取角点和面点特征的前端处理方式，通过IMU里程计或者scan-to-scan匹配提供预测值。优化过程采用当前帧和局部地图匹配获得更为精准的定位结果。在后端处理普遍利用因子图框架，进行多源信息处理融合。但是现有的算法主要针对小场景运动状态。而对于场景复杂，载体环境变化大，易出现累计误差，稳健性差。为此，需在保证定位和建图的条件下，精度探索基于大场景的城市环境下可靠的激光SLAM技术。

发明内容

为解决上述问题，本发明公开了基于大场景的低线束激光雷达-IMU-RTK定位建图算法，根据如今城市环境定位的复杂性，利用低线束激光雷达、IMU与RTK实现信息互补利用，实时采集并获得可靠的定位建图结果。

为达到上述目的，本发明的技术方案如下：

基于大场景的低线束激光雷达-IMU-RTK定位建图算法，包含以下步骤：

步骤1、初始化利用RTK位置信息快速精准的完成IMU对准，并解算激光雷达 -IMU-RTK***在全局坐标下的正确初始位姿；

步骤2、提取周围点云环境特征，根据扫描点曲率区分角点和面点，采用IMU数据进行点云去畸变校正；

步骤3、实时构建局部点云地图，对地图与当前帧进行体素滤波提升后续计算效率，并利用当前帧点云信息与局部地图动态匹配，迭代计算当前激光雷达位姿；

步骤4、图优化模型构建激光雷达里程计因子、IMU预积分因子、RTK绝对位置因子，优化求解定位结果并进行实时点云地图拼接，获得一致性较好的全局地图。

具体步骤为：

步骤1.IMU快速精准的初始对准

式中，

表示导航坐标系到载体坐标系的转换矩阵；a^b、b^b、c^b分别表示载体坐标系下的位置坐标；aⁿ、bⁿ、cⁿ表示对应点在导航坐标系下的坐标值。初始对准确定惯性器件在导航坐标系下的姿态，获得激光雷达-IMU-RTK融合定位在全局坐标下的姿态初值。

步骤2.提取周围点云环境特征，根据扫描点曲率区分角点和面点

提取环境点云特征，根据扫描点曲率区分角点和面点，曲率计算公式如下式:

|S|代表此扫描点的同线束周围点集合，r_i表示此点的深度值。通过此式可依次计算每帧激光雷达扫描到的所有点的曲率。同时，利用每个点的深度信息剔除平行点和遮挡点。为了提升遍历效率，每帧点云坐标信息投影成二维图像，并分为六个子图，对子图分别提取角点和面点。根据阈值c_th进行判定，c＜c_th为角点，c＞c_th为面点。由于载体运动，每帧扫描过程会出现点云畸变，采用高频IMU数据短时间积分可实现运动补偿。

步骤3.构建局部点云地图，点云配准优化位姿

采用滑窗的方式合并一定范围内的关键帧信息。为了提升点云匹配速率，局部地图的点云信息都存储在kd-tree的数据结构中，方便后续搜索。运动估计模块将当前帧与局部地图匹配求解运动状态。

对于角点，搜索其在局部地图最近的五个点，计算五点均值和协方差矩阵。当有满足条件的线特征，可计算当前角点到线的距离。距离计算公式如下：

其中·和×分别表示点积和叉乘运算，p_n是单位向量，p_ε代表当前角点，

代表特征值对应的特征向量。同理，面点p_s在局部地图中搜索出五点构成平面。此时五点的协方差矩阵的最小特征值对应的特征向量

为这个平面所对应法向量。点面距离计算如下：

式子

和

分别表示局部地图提取角点和面点的几何中心。通过联合最小化点线残差 f(p_ε)和点面残差f(p_s)，得到当前点在局部地图的最佳位姿估计T_k。求解过程如下式：

min{∑f(p_ε)+f(p_s)} (6)

步骤4.图优化模型求解定位结果并进行实时点云地图拼接

对于一定窗口内的关键帧，数量为n，优化状态量

最小化求解方程如下式：

式中R_p(x)代表边缘化残差，边缘化的目的是减少优化方程规模，移出滑窗的关键帧信息保存为当前优化的先验信息。

代表滑窗内激光雷达里程计的帧间约束，残差计算如下式：

式中i和j分别表示上一帧和当前帧，T是位姿的欧式矩阵表示，ΔT即为位姿变化量。

代表滑窗内IMU预积分约束，残差按下式解得：

式中

是旋转矩阵表示的姿态，W是世界坐标系。δα、δβ和δθ分别表示位置、速度和角度的变化量，δb^a和δb^w分别表示加速度计和陀螺仪的偏置变化。g^W表示世界坐标系下的重力值，p^W、v^W和q^W是世界系下的位置、速度和四元数表示的姿态。[×]_xyz指四元数虚部，

分别代表IMU预积分计算的相邻两帧之间的位置、速度和角度。同时每帧解算估计的加速度计和陀螺仪的偏置为b^a和b^w。

R_g(x)表示RTK绝对位置残差约束，计算公式如下：

式中

表示RTK测量的位置结果。RTK因子在优化问题的权重根据其协方差变化。

本步骤根据(7)式利用Ceres优化库求解滑窗内每帧的状态量，并实时更新采集到的点云姿态，将载体运动状态下的激光雷达扫描点转到世界坐标系下，依次拼接合成全局一致的点云地图。

本发明的有益效果包括：

本发明提出的基于大场景的低线束激光雷达-IMU-RTK定位建图算法，利用图优化模型，合理利用三种传感器的数据信息，克服低线束激光雷达在大场景易定位发散，RTK在城市环境信号遮挡，IMU长时间出现累积漂移的缺点，实现高精度定位和建图的SLAM***。所提方法在城市环境中车载测试，定位精度和鲁棒性可明显提升，对于三维实景测绘、自动驾驶等行业具有重要借鉴意义。

附图说明

图1是本发明所述的基于低线束激光雷达-IMU-RTK定位建图算法流程图；

图2是应用图优化模型在后端解算定位结果的示意图；

图3是激光雷达-IMU-RTK实验设备，用于采集数据并实时定位与建图；

图4是实验***在车载测试中大场景定位和建图的示意图，可实时输出高精度结果且稳定运行。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如附图1所示，本实施例公开一种基于低线束激光雷达-IMU-RTK定位建图算法，具体步骤如下：

步骤1.IMU快速精准的初始对准

式中，

表示导航坐标系到载体坐标系的转换矩阵；a^b、b^b、c^b分别表示载体坐标系下的位置坐标；aⁿ、bⁿ、cⁿ表示对应点在导航坐标系下的坐标值。初始对准确定惯性器件在导航坐标系下的姿态，获得激光雷达-IMU-RTK融合定位在全局坐标下的姿态初值。

步骤2.提取周围点云环境特征，根据扫描点曲率区分角点和面点

提取环境点云特征，根据扫描点曲率区分角点和面点，曲率计算公式如下式:

|S|代表此扫描点的同线束周围点集合，r_i表示此点的深度值。通过此式可依次计算每帧激光雷达扫描到的所有点的曲率。同时，利用每个点的深度信息剔除平行点和遮挡点。为了提升遍历效率，每帧点云坐标信息投影成二维图像，并分为六个子图，对子图分别提取角点和面点。根据阈值c_th进行判定，c＜c_th为角点，c＞c_th为面点。由于载体运动，每帧扫描过程会出现点云畸变，采用高频IMU数据短时间积分可实现运动补偿。

步骤3.构建局部点云地图，点云配准优化位姿

采用滑窗的方式合并一定范围内的关键帧信息。为了提升点云匹配速率，局部地图的点云信息都存储在kd-tree的数据结构中，方便后续搜索。运动估计模块将当前帧与局部地图匹配求解运动状态。

对于角点，搜索其在局部地图最近的五个点，计算五点均值和协方差矩阵。当有满足条件的线特征，可计算当前角点到线的距离。距离计算公式如下：

其中·和×分别表示点积和叉乘运算，p_n是单位向量，p_ε代表当前角点，

代表特征值对应的特征向量。同理，面点p_s在局部地图中搜索出五点构成平面。此时五点的协方差矩阵的最小特征值对应的特征向量

为这个平面所对应法向量。点面距离计算如下：

式子

和

分别表示局部地图提取角点和面点的几何中心。通过联合最小化点线残差f(p_ε)和点面残差f(p_s)，得到当前点在局部地图的最佳位姿估计T_k。求解过程如下式：

min{∑f(p_ε)+f(p_s)} (6)

步骤4.图优化模型求解定位结果并进行实时点云地图拼接

对于一定窗口内的关键帧，数量为n，优化状态量

最小化求解方程如下式：

式中R_p(x)代表边缘化残差，边缘化的目的是减少优化方程规模，移出滑窗的关键帧信息保存为当前优化的先验信息。

代表滑窗内激光雷达里程计的帧间约束，残差计算如下式：

式中i和j分别表示上一帧和当前帧，T是位姿的欧式矩阵表示，ΔT即为位姿变化量。

代表滑窗内IMU预积分约束，残差按下式解得：

式中

是旋转矩阵表示的姿态，W是世界坐标系。δα、δβ和δθ分别表示位置、速度和角度的变化量，δb^a和δb^w分别表示加速度计和陀螺仪的偏置变化。g^W表示世界坐标系下的重力值，p^W、v^W和q^W是世界系下的位置、速度和四元数表示的姿态。[×]_xyz指四元数虚部，

分别代表IMU预积分计算的相邻两帧之间的位置、速度和角度。同时每帧解算估计的加速度计和陀螺仪的偏置为b^a和b^w。

R_g(x)表示RTK绝对位置残差约束，计算公式如下：

式中

表示RTK测量的位置结果。RTK因子在优化问题的权重根据其协方差变化。

本步骤根据(7)式利用Ceres优化库求解滑窗内每帧的状态量，并实时更新采集到的点云姿态，将载体运动状态下的激光雷达扫描点转到世界坐标系下，依次拼接合成全局一致的点云地图。

附图3是在城市环境下定位与建图的实验设备，采用Velodyne-VLP16激光雷达、ADIS16488惯导、天宝接收机。传感器之间安装参数已知，硬件方式已实现时间同步。附图 4是车载平台在大场景下的实验定位与建图结果，在城市环境23.6公里车载测试(包含RTK遮挡的高架、林荫场景，点云稀疏退化的大场景环境等)全程定位精度(均方根误差RMSE)指标为0.24m。同时本***可利用点云实时构建周围场景，实现自动驾驶建图与感知。

需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims (5)

1.基于大场景的低线束激光雷达-IMU-RTK定位建图算法，其特征在于，具体方法如下：

步骤1、初始化利用RTK位置信息完成IMU对准，并解算激光雷达-IMU-RTK***在全局坐标下的正确初始位姿；

步骤2、提取周围点云环境特征，根据扫描点曲率区分角点和面点，采用IMU数据进行点云去畸变校正；

步骤3、实时构建局部点云地图，对地图与当前帧进行体素滤波提升后续计算效率，并利用当前帧点云信息与局部地图动态匹配，迭代计算当前激光雷达位姿；

步骤4、图优化模型构建激光雷达里程计因子、IMU预积分因子、RTK绝对位置因子，优化求解定位结果并进行实时点云地图拼接，获得一致性较好的全局地图。

2.如权利要求1所述的基于大场景的低线束激光雷达-IMU-RTK定位建图算法，其特征在于，步骤1所述IMU初始对准按下式解得：

式中，

表示导航坐标系到载体坐标系的转换矩阵；a^b、b^b、c^b分别表示载体坐标系下的位置坐标；aⁿ、bⁿ、cⁿ表示对应点在导航坐标系下的坐标值，初始对准确定惯性器件在导航坐标系下的姿态，获得激光雷达-IMU-RTK融合定位在全局坐标下的姿态初值。

3.如权利要求1所述的基于大场景的低线束激光雷达-IMU-RTK定位建图算法，其特征在于，步骤2所述提取环境点云特征，根据扫描点曲率区分角点和面点，曲率c计算公式如下式：

|S|代表此扫描点的同线束周围点集合，r_i表示此点的深度值，r_j表示此点附近同线束点深度值，通过此式依次计算每帧激光雷达扫描到的所有点的曲率，同时，利用每个点的深度信息剔除平行点和遮挡点，为了提升遍历效率，每帧点云坐标信息投影成二维图像，并分为六个子图，对子图分别提取角点和面点，根据阈值c_th进行判定，c＜c_th为角点，c＞c_th为面点，由于载体运动，每帧扫描过程会出现点云畸变，采用高频IMU数据短时间积分运算实现运动补偿。

4.如权利要求1所述的基于大场景的低线束激光雷达-IMU-RTK定位建图算法，其特征在于，步骤3所述的实时构建局部点云地图，对地图与当前帧进行体素滤波提升后续计算效率，利用当前帧点云信息与局部地图动态匹配，迭代计算当前激光雷达位姿，采用滑窗的方式合并一定范围内的关键帧信息，为了提升点云匹配速率，局部地图的点云信息都存储在kd-tree的数据结构中，方便后续搜索，

运动估计模块将当前帧与局部地图匹配求解运动状态，对于角点，搜索其在局部地图最近的五个点，计算五点均值和协方差矩阵，当有满足条件的线特征，计算当前角点到线的距离，距离计算公式如下：

其中·和×分别表示点积和叉乘运算，p_n是单位向量，p_ε代表当前角点，

代表特征值对应的特征向量，同理，面点p_s在局部地图中搜索出五点构成平面，此时五点的协方差矩阵的最小特征值对应的特征向量

为这个平面所对应法向量，点面距离计算如式(5)：

式子

和

分别表示局部地图提取角点和面点的几何中心，通过联合最小化点线残差f(p_ε)和点面残差f(p_s)，得到当前点在局部地图的最佳位姿估计T_k，求解过程如下式：

min{∑f(p_ε)+f(p_s)} (6)。

5.如权利要求1所述的基于大场景的低线束激光雷达-IMU-RTK定位建图算法，其特征在于，步骤4所述的图优化模型构建激光雷达里程计因子、IMU预积分因子、RTK绝对位置因子，优化求解定位结果并进行实时点云地图拼接，对于一定窗口内的关键帧，数量为n，优化状态量

最小化求解方程如下式：

式中R_p(x)代表边缘化残差，边缘化的目的是减少优化方程规模，移出滑窗的关键帧信息保存为当前优化的先验信息，

代表滑窗内激光雷达里程计的帧间约束，残差计算如下式：

式中i和j分别表示上一帧和当前帧，T是位姿的欧式矩阵表示，ΔT即为位姿变化量，

代表滑窗内IMU预积分约束，残差按下式解得：

式中

是旋转矩阵表示的姿态，W是世界坐标系，b_i和b_j指第i帧与第j帧所在载体系，δα、δβ和δθ分别表示位置、速度和角度的变化量，δb^a和δb^w分别表示加速度计和陀螺仪的偏置变化,Δt指两帧之间的时间间隔，g^W表示世界坐标系下的重力值，p^W、v^W和q^W是世界系下的位置、速度和四元数表示的姿态，[×]_xyz指四元数虚部，

分别代表IMU预积分计算的相邻两帧之间的位置、速度和角度，同时每帧解算估计的加速度计和陀螺仪的偏置为b^a和b^w，

R_g(x)表示RTK绝对位置残差约束，计算公式如下：

式中

表示RTK测量的位置结果，RTK因子在优化问题的权重根据其协方差变化，

本步骤根据(7)式利用Ceres优化库求解滑窗内每帧的状态量，并实时更新采集到的点云姿态，将载体运动状态下的激光雷达扫描点转到世界坐标系下，依次拼接合成全局一致的点云地图。

Images