[发明专利]一种基于体素化精配准的LiDAR-IMU-GNSS融合定位方法

权利要求书

1.一种基于体素化精配准的LiDAR-IMU-GNSS融合定位方法，包括以下步骤：

(1)采用一种基于曲率分割的体素化降采样方法：给定一组激光雷达采集的原始点云序列，遍历原始点云序列中的所有点，利用广度优先方法进行粗聚类；

具体来说，利用基于欧氏距离的几何角度阈值对深度相近的点云簇进行细分割；设激光雷达扫描中心为O，点云簇中两个相近边缘点p_a、p_b，深度分别为d_a、d_b(d_ad_b)，设点p_i所在点云簇中点云数为M，则p_i点云粗糙度为：

设粗糙度阈值为遍历M，将的点归为边缘特征点集，将的点归为平面特征点集，分别进行降采样操作，然后利用哈希函数映射值代替随机取样对曲率粗分类后的点云簇进行快速降采样；设一组点云序列中某特征点在体素空间中的坐标为p(x,y,z)；设体素网格尺寸为r，则体素网格在x方向的维数为D_x＝(x_max-x_min)/r，p在体素网格内的x方向索引为h_x＝(x-x_min)/r，y、z方向同理；

得到特征点在体素空间中的三维索引后，采用哈希函数对特征点索引快速排序，将其映射到N个容器中(N＝80)，哈希映射函数为：

hash(h_x,h_y,h_z)＝(h_x+h_y·D_x+h_z·D_x·D_y)％N (2)

为避免哈希冲突，设冲突检测条件为：

hash(h_x,h_y,h_z)＝hash(h'_x,h'_y,h'_z)(h_x≠h'_x|h_y≠h'_y|h_z≠h'_z) (3)

一旦检测到哈希冲突，则将当前容器中的索引值输出并清空容器，将新索引值置入容器中；

(2)基于平滑体素化处理的点云配准

采用基于体素内特征点分布求解T的方法，将利用树状图构建单点对最近邻模型问题转化为构建点与邻域点集的最近邻模型；首先将两组点云序列近似为高斯分布，即其中i∈(1,n)，和分别为两组点云序列协方差阵；设a_i的邻域点集为其中，λ为邻域判断阈值，由此计算刚体变换误差为：

该步骤实现了对a_i邻域内所有邻近点云的平滑处理；由此可推估帧间位姿变换矩阵T的最大似然估计如下，其中N_i为a_i邻域内点云数量；

除了对a_i邻域内所有邻近点云的平滑处理外，为避免多次迭代后陷入局部最优的盲区，设立迭代终止阈值ε如下：

|RMSE_k+1-RMSE_k|＞ε (6)

其中，RMSE_k+1和RMSE_k分别是前k+1次迭代和前k次迭代的均方根误差，当均方根误差的变化量的绝对值|RMSE_k+1-RMSE_k|≤ε或达到最大迭代次数时，迭代完成；

(3)采用适用于激光雷达-IMU-GNSS多传感器位姿空间统一的方法；

设为IMU到GNSS的外参转换矩阵，为激光雷达到IMU的外参转换矩阵；由于硬件固连于移动载体，故二者标定后均为定值；引入空间转换参数关联两位姿空间；包括平移参数和旋转参数t时刻的多源位姿空间统一问题表示为：

式中，初值设为单位阵，每次添加GNSS因子求解全局最优解后将更新下一时刻的值，修正局部坐标系与全局坐标系间的累计偏移；

(4)全局地图构建

利用图优化方法，将构建全局地图视为一个最大后验估计问题，对滑动窗口内的状态向量进行非线性优化；构建全局优化函数如下：

其中，ρ为GNSS置信度；为当前全局点云P_k与帧间局部匹配后派生的局部点云S_k之间的位姿转换矩阵；公式中各传感器代价函数的具体含义如下所述；

①激光雷达-惯性里程计因子：根据激光雷达-惯性里程计局部位姿估计结果可得t时刻局部坐标系下的载体位置和旋转由此可构建局部残差因子如下：

②GNSS因子：设两帧GNSS观测值时间间隔为Δt，与LIO位姿估计值以插值的方式实现时间对齐；现给定ENU坐标系中的GNSS测量值LIO在全局坐标系中的位姿观测值则GNSS残差因子表示如下：

当载体运动至GNSS信号良好区域时，为充分且可靠地利用GNSS观测值，以GNSS置信度作为权重添加GNSS因子，GNSS置信度由GNSS可见有效卫星数决定；

③体素化回环因子：考虑到移动载体行驶区域可能存在的重合性，需添加回环检测环节建立非相邻帧间存在的回环约束；根据(5)式可构建回环因子如下：