CN113409410A - 一种基于3d激光雷达的多特征融合igv定位与建图方法 - Google Patents

Abstract

本发明公开了一种基于3D激光雷达的多特征融合IGV定位与建图方法。主要包括数据采集处理、扫描匹配及局部地图构建、后端优化、回环检测四个过程；数据采集处理是对3D激光雷达传感器的数据处理；扫描匹配及局部地图构建是对处理过的激光点云数据采用帧‑子图的匹配方式，本发明利用具有角度、距离、反光强度等多特征信息的三维地标解算初始位姿，通过占据栅格地图，构建局部最优子图；后端优化对于不断迭代的子图，采用图的优化策略，用高斯牛顿法解决优化问题，并利用三维地标加速求解过程，从而消除累计误差；回环检测中存储所有轨迹，采用多分辨率地图，通过分支定界法加速计算，完成闭环检测。最终实现AGV的高精度定位和建图。

Description

一种基于3D激光雷达的多特征融合IGV定位与建图方法

技术领域

本发明属于自动控制领域，具体是涉及一种基于3D激光雷达的多特征融合IGV定位与建图方法。

背景技术

随着仓储物流的发展,我们国家己经从依靠劳动力为主的低端制造业转向人工智能技术产业,人工供给不足等因素促使移动机器人行业的不断发展,未来中国将成为移动机器人制造和应用大国.而目前广泛应用于仓促物流的IGV,大多采用2D激光slam作为定位建图方式，在遇到环境对称，很难实现高精度定位和建图，而且只能获取平面地图信息，对于导航、避障等需要额外增添传感器，不仅增加经济成本，而且存在一定的安全隐患。对比相机，3D激光雷达是另一种可感知三维环境的传感器,它能直接获取三维点云数据,有测距精度高、受光线影响小、抗电磁干扰等特点。研究一种适用于仓储物流环境的高度精确定位和建图对于仓储机器人作业意义重大。

发明内容

本发明的目的在于使仓储IGV实现高精度定位和建图，同时克服2D激光雷达的平面局限，满足仓储IGV全天运转的需求，从而本申请提出一种基于3D激光雷达的多特征融合的IGV定位与建图方法，该方法能适用与各种复杂仓储环境，通过在环境中建立若干三维地标，通过标定在子图这一概念中加入这一多特征属性，对IGV运行过程中扫描匹配进行约束，实现局部最优子图的构建。由于建立好的子图存在误差，该方法采用图优化建立优化目标函数，通过高斯牛顿法求解，依靠多特征三维地标这一观测值减少计算量，快速找到最优子图。在最优子图构建后，存储所有轨迹，再次经过时，遍历所有轨迹，对IGV重新定位，完成闭环检测，从而实现IGV高精度定位和建图

为达到上述目的，本发明的技术方案如下：一种基于3D激光雷达的多特征融合IGV定位与建图方法，包括如下步骤：

S1：将整个地图场景划分为大小相同的若干立方体，赋予每个立方体一定概率值，其中三维地标区域概率值赋予1，构建三维占据栅格地图。在地图中加入具有撕裂点、角点、直线、圆弧多种特征的三维地标，用3D激光雷达传感器对三维地标进行标定，根据实际距离和角度信息定义IGV初始位姿。

S2：对3D激光雷达传感器获取的数据进行预处理，记3D激光雷达采集的激光点为C，其中包含时间戳、反光强度、距离和角度信息。步骤S1定义的IGV在世界坐标系下初始位姿为(x,y,θ)，那么扫描到的激光点在世界坐标系下的坐标(x_c，y_c)为：

其中，d为C中包含的距离信息，θ为C中包含的角度信息。

激光点C的点云数据结构由x_c、y_c、时间戳、反光强度构成，设在k时刻在雷达坐标系L下获得的一帧点云为

其中t_k＝k为获取这一帧点云时的时间戳，

为k时刻在雷达坐标系L下点云中第i个点的坐标。由此可以根据雷达扫描时间、扫描角度、角度分辨率实现对三维点云数据有序化。

最后，对有序化的点云采用范围滤波、体素滤波、点云去噪去除不需要的点云，输出预处理后的点云数据。其中，范围滤波去除不在3D激光雷达场景内的点云；体素滤波使用部分点云代替整体点云并保存原点云特征，降低点云数量；点云去噪是去除范围滤波和体素滤波产生的噪声。

S3：对预处理后的3D激光雷达传感器数据进行扫描匹配，对于处理后的3D激光雷达传感器数据采用帧-子图的插入方式，其中一帧由若干点云组成，子图由若干帧组成。当前帧插入子图之前，需要对扫描帧位姿和当前的子图进行优化，让帧中所有激光点落在当前子图中的概率和最大，则问题可转化为求解以下最小二乘问题：

其中，ξ为IGV位姿，函数M()为占据栅格地图中某个坐标为障碍物的概率，函数S_i(ξ)为一帧中i号激光点照射到的障碍物在占据栅格地图中的坐标。

最小二乘问题为局部最优问题，需要一个初始位姿估计，若此时IGV静止，初始位姿可由步骤S1解算得出，若IGV运行中，则由编码器和IMU预测提供初始位姿。由此求解可得得到对当前帧和子图的位姿变换关系矩阵T_ξ如下：

其中，R_ξ为旋转矩阵，t_ξ为平移矩阵，子图使用概率网格描述。

S4：局部地图构建，对于3D激光雷达传感器持续扫描匹配，采用子图集策略，首先构建子图集合，每个子图集合由若干子图构成，子图集的最后两个子图，分别记为P1和P2，当前帧与P1匹配，并插入P1和P2中，由于子图由若干帧组成，当P2中的帧数量达到阈值数目后，标记P1已经完成，并建新的子图，将原来的P2记为新的P1，新的子图则记为P2，继续进行当前帧的插入过程。通过子图集策略，增加点云占据栅格代价函数、平移代价函数、旋转代价函数三种约束，构建局部最优子图。

S5：后端优化，由于步骤S3中激光雷达传感器扫描帧仅与当前子图进行匹配，环境地图是由一系列子图构成的，因此会存在累积误差。对此，3D激光雷达传感器获取的扫描帧在插入子图时的位姿会被缓存到内存，当子图不再变化时，所有的扫描帧和子图都会被用来进行闭环检测。采用图的优化方式，以多特征三维地标和IGV位姿为顶点，以步骤S3得到的当前帧和子图的位姿变换关系矩阵和此时IGV观测的位姿变换关系的误差函数为边，总体优化问题变为若干条边加和的形式，构建目标优化函数如下：

上式中，函数E()为误差函数,

通过分别表示在一定约束条件下，子图的位姿和扫描帧的位姿，m为子图数量，n为扫描帧数量。相对位姿ξ_ij表示扫描帧j在子图i中的匹配位置，与其相关的协方差矩阵Ω_ij共同构成优化约束。结合高斯牛顿法进行优化问题的快速求解，得到帧和子图优化后的转换关系。

S6：轨迹位姿获取，在步骤S4构建局部地图的同时，结合运动控制量的输入，多特征三维地标作为观测量，使用IMU和编码器预测IGV位姿，获得IGV运行轨迹。

S7：闭环检测，对于步骤S6获得的轨迹位姿进行存储，再次经过轨迹上某一点时，采用分支定界法快速遍历所有轨迹，得到历史帧。若当前帧和历史帧匹配成功，则在步骤S5中图优化问题中增加一条边，形成约束完成闭环检测。

进一步地，步骤S3具体包括如下步骤：

S31：静态时，通过IGV相对于三维地标物理信息解算初始位姿，动态时，由编码器和IMU预测提供初始位姿。记初始位姿T＝{x,y,θ}，在其附近设置一个窗口并设定窗口的大小，通过下式计算窗口的迭代步长和迭代次数。

其中，h_k表示扫描帧中的激光点，d_max表示最远的激光点，r表示位置迭代步长，通常为一个栅格；δ_θ表示角度迭代步长；W_x、W_y、W_z分别表示x、y、θ的窗口大小，ω_x、ω_y、ω_θ分别表示x、y、z方向上的迭代次数。

S32：根据S31得到搜索窗口所有候选位姿变换组合w如下：

w＝{-ω_x,...,ω_x}×{-ω_y,...,ω_y}×{-ω_θ,...,ωθ}

S33：计算所有候选位姿变换得分，记其中一个候选值为T_i＝{x_i,y_i,θ_i}，将当前帧点云C变换到已建栅格地图中，对变化后各点坐标对应栅格概率值求和并除以点数，获得概率均值记为S，根据下式计算分数。

其中，α和β表示平移和旋转所占权重；

S34：从候选值中选出分数最高的位姿变换，以此作为最优匹配；

进一步地，所述步骤S5具体包括如下步骤：

S51：记IGV在A和B点扫描匹配中最高得分的位姿变换矩阵为T_a和T_b，两点间位姿变换关系为T:

其中，在图的优化理论中T_a和T_b为图的顶点；

S52：由于步骤S3中激光雷达扫描帧仅与当前子图进行匹配，环境地图是由一系列子图构成的，因此会存在累积误差。记此时3D激光雷达观测到的位姿变换矩阵为Z_ab,则误差函数e_ab(A,B,Z_ab)表示为：

其中，t2v()表示将变换矩阵化为向量，即平移和姿态；

S53：定义图优化的边为：

e_ab＝e(A,B,Z_AB)^TΩ_ABe(A,B,Z_AB)

其中，信息矩阵Ω_AB表示对误差各分量重视程度的不一样，也就是顶点A和B权重；

S54：根据图中顶点与边的关系，优化目标函数为求解所有边之和的最小值

其中，x为顶点集，C为两个顶点排列组合的集合，

表示顶点i和j之间的边，Ω_ij为顶点i和j权重；

S55：采用高斯牛顿法对目标函数求解，利用三维地标相对于IGV的位姿变换作为顶点，在IGV在运行过程中，只有在观测到三维地标时，才构建边的关系，从而减小目标函数求解计算量，优化位姿转换关系。

本发明的有益效果在于：本发明采用多特征三维地标作为路标，无需考虑平面环境存在地图对称情况；利用3D激光雷达捕获空间中三维地标距离、反光强度、角度等多特征信息，为前端扫描匹配提供一个好的初始位姿，同时对于后端优化中，采用图优化方式构建的优化目标函数能够进行快速求解。通过IGV对自身所有轨迹数据的存储，利用分支定界法快速实现闭环检测。使该方法使IGV具有对环境进行高精度定位和建图的能力，从而实现仓促物流的稳定和快速的运转。

附图说明

图1为本发明方法实现流程图。

图2为本发明三维地标示意图。

具体实施方式

下面结合附图说明进一步详细描述本方法的执行步骤，但本发明的保护范围不局限于以下所述。

如图1所示，本发明提供的一种基于3D激光雷达的多特征融合IGV定位与建图方法，该方法使得IGV能在复杂环境实现高精度定位和建图。具体包括如下步骤：

S1：将整个地图场景划分为大小相当的若干立方体，赋予每个立方体一定概率值，其中三维地标区域概率值赋予1，构建三维占据栅格地图。在栅格地图中，记激光击中为hit，未击中为miss，用p(s＝1)表示它miss的概率，用p(s＝0)来表示它hit的概率，且两者之和1。

将两者比值作为点的状态：

用P来表示p(s＝1):

对于3D激光传感器，新来一个测量值，需要更新点的状态，设测量之前概率为odd(p),更新后被占据的概率为：

M_new(x)＝clamp(odds^-1(odds(M_old(x))·odd(P_hit)))

其中，M_old(x)为更新前占据的概率，P_hit为P。

在地图中放置若干多特征三维地标，包括具有撕裂点、角点、直线、圆弧等多种特征的三维地标，对于三维地标，在几何特征上，具有直线、圆弧多种特征信息。在材质上，具有强反光特性。用3D激光雷达传感器对三维地标进行标定，采用多特征三维地标做为栅格地图中的标记物，无需额外标记物，根据实际距离和角度信息定义IGV初始位姿。

S2：对3D激光雷达的水平扫描范围、俯仰扫描范围、扫描最大距离以及频率的确认。

对3D激光雷达传感器获取的传感器数据进行预处理，具体包括如下步骤：

S21：记3D激光雷达采集的激光点为C，由S1可知IGV在世界坐标系下初始位姿为(x,y,θ)，将所有扫描到的激光点转换到在世界坐标系下的坐标，坐标(x_c，y_c)为：

其中，d为激光点的距离信息，θ为激光点的角度信息。

S22：由x_c、y_c、时间戳、反光强度等构成激光点C的点云数据结构，设在k时刻在雷达坐标系L下获得的一帧点云为

其中t_k＝k为获取这一帧点云时的时间戳，

为k时刻在雷达坐标系L下点云中第i个点的坐标。根据雷达扫描时间、扫描角度、角度分辨率等，实现对三维点云数据有序化。

S23：对排序后的点云，结合范围滤波、体素滤波、点云去噪减少点云，具体流程如下：

1.首先去除不在3D激光雷达场景内的点云数据；

2.然后统计每个栅格中包含的点云和通过该栅格的激光线条数；

3.遍历所有点云，得到各个点所在栅格的点云数目n和通过该栅格的激光线条数m。设置阈值L，若n>Lm，则认为该点为外点，进行删除，反之进行保留。

4.最后进行下采样，用栅格内的重心代替整个栅格内的点；

S3：对预处理后的点云数据，进行扫描匹配，对于处理后的3D激光雷达传感器数据采用帧-子图的插入方式，其中一帧由若干点云组成，子图由若干帧组成。当前帧插入子图之前，需要对扫描帧位姿和当前的子图进行优化，让帧中所有激光点落在当前子图中的概率和最大，则问题可转化为求解以下最小二乘问题：

其中，ξ为IGV位姿，函数M()为占据栅格地图中某个坐标为障碍物的概率，函数S_i(ξ)为一帧中i号激光点照射到的障碍物在占据栅格地图中的坐标。

最小二乘问题为局部最优问题，需要一个初始位姿估计，若此时IGV静止，初始位姿可由步骤S1解算得出，若IGV运行中，则由编码器和IMU预测提供初始位姿。由此求解可得得到对当前帧和子图的位姿变换关系矩阵T_ξ如下：

其中，R_ξ为旋转矩阵，t_ξ为平移矩阵，子图使用概率网格描述。步骤S3具体包括如下子步骤：

S31：静态时，通过通过IGV相对于三维地标物理解算初始位姿，动态时，由编码器和IMU预测提供初始位姿。通过三维地标物理解算初始位姿时，需要用3D激光雷达扫描三维地标，通过对角度、距离、反光强度和角点等多特征特征信息的提取，定义IGV初始位姿，该过程具体如下：

基于平面曲率，只通过激光点坐标间的加减运算来区分角点和平面点，记点i处曲率c的计算公式为：

其中，S为点i周围的n个连续点的集合，包括分布在点i左右的各n/2个点。

在点云密度较大且均匀的环境中，对特征点提取算法考虑点邻域的局部曲率特征，再根据曲率特征的在局部的相对大小排序区分点的类型，对步骤S31中计算公式简化计算有：

完成对角点的提取，选取曲率最大的Mc个点作为角点，其中点云排序最大的Nc个点作为主角点，Mc-Nc个作为次角点。选取其中最小的Np个点作为主平面点，其它剩余的点则全部归为次平面点。特征点的选取逐线进行，直至一帧点云的全部点都被归类，完成对多特征三维地标的特征提取。

S32:记步骤S31提供初始位姿T＝{x,y,θ}，在当前帧点云附近生成一系列候选值，对应也在待求值附近生成了一系列候选值，形成搜索窗口并设定窗口的大小，通过下式计算窗口的迭代步长和迭代次数。

其中，h_k表示扫描帧中的激光点，d_max表示最远的激光点，r表示位置迭代步长，通常为一个栅格；δ_θ表示角度迭代步长；W_x、W_y、W_z分别表示x、y、θ的窗口大小，ω_x、ω_y、ω_θ分别表示x、y、z方向上的迭代次数。

S32：根据S32得到搜索窗口所有候选位姿变换组合：

w＝{-ω_x，...,ω_x}×{-ω_y，...,ω_y}×{-ω_θ,...,ωθ}

S33：计算所有候选位姿变换得分，记其中一个候选值为T_i＝{x_i，y_i，θ_i}，将当前帧点云C变换到已建栅格地图中，对变化后个点坐标对应栅格概率值求和并除以点数，获得概率均值记为S，根据下式计算分数。

其中，α和β表示平移和旋转所占权重；

S34：从候选值中选出分数最高的位姿变换，以此作为最优位姿态变换关系；

S4：局部地图构建。所述步骤S3具体包括如下步骤：

S41:对于3D激光雷达持续扫描匹配，局部地图的构建采用子图集策略。

S42：首先，增加点云占据栅格代价函数、平移代价函数、旋转代价函数三种约束。

S43：然后，构建子图集合，每个子图集合由一定数目的子图构成，子图集的最后两个子图，分别记为P1和P2。

S44：当前帧与P1匹配，并插入P1和P2中，由于子图由若干帧组成，当P2中的帧数量达到阈值数目后，标记P1已经完成，正式称为子图集的一员。

S45：建立新的子图，将原来的P2记为新的P1，新的子图则记为P2，继续进行当前帧的插入，不断迭代得出局部最优地图。

S5：由于步骤S3中激光雷达传感器扫描帧仅与当前子图进行匹配，环境地图是由一系列子图构成的，因此会存在累积误差。对此，3D激光雷达传感器获取的扫描帧在插入子图时的位姿会被缓存到内存，当子图不再变化时，所有的扫描帧和子图都会被用来进行闭环检测。对于扫描匹配和局部建图中计算的累计误差，采取图的优化方式，进行后端优化，以多特征三维地标和IGV位姿为顶点，以步骤S3得到的当前帧和子图的位姿变换关系矩阵和此时IGV观测的位姿变换关系的误差函数为边，总体优化问题变为若干条边加和的形式，构建目标优化函数如下：

上式中，函数E为误差函数，

通过分别表示在一定约束条件下，子图的位姿和扫描帧的位姿，m为子图数量，n为扫描帧数量。相对位姿ξ_ij表示扫描帧j在子图i中的匹配位置，与其相关的协方差矩阵Ω_ij共同构成优化约束。结合高斯牛顿法进行优化问题的快速求解，得到帧和子图优化后的转换关系，对前端扫描产生的累计误差进行修正。

通过建立的三维地标，结合高斯牛顿法进行优化问题的快速求解，得到优化后的位姿转换关系，所述步骤S5具体包括如下步骤：

S51：根据步骤S4，记IGV在A和B点扫描匹配中最高得分的位姿变换矩阵为T_a和T_b，两点间位姿变换关系为T：

其中，在图的优化理论中T_a和T_b为图的顶点；

S52：由于步骤S3中激光雷达扫描帧仅与当前子图进行匹配，环境地图是由一系列子图构成的，因此会存在累积误差。记此时3D激光雷达传感器观测到的位姿变换矩阵为Z_ab，则误差函数表示为：

其中，t2v()表示将变换矩阵化为向量，即平移和姿态；

S53：定义图优化的边为：

e_ab＝e(A，B，Z_AB)^TΩ_ABe(A,B，Z_AB)

其中，信息矩阵Ω_AB表示对误差各分量重视程度的不一样，也就是顶点A和B权重；

S54：根据图中顶点与边的关系，优化目标函数为求解所有边之和的最小值

其中，x为顶点集，C为两个顶点排列组合的集合，

表示顶点i和j之间的边，Ω_ij为顶点i和j权重；

S55：采用高斯牛顿法对目标函数求解，利用三维地标相对于IGV的位姿变换作为顶点，在迭代初值x₀处泰勒一阶展开，则有：

其中，C为两个顶点排列组合的集合，c为与Δx无关的常数项，b^T为一次项系数，H是Hessian矩阵，为二次项系数。

S56：计算雅克比矩阵，对步骤S55中式子展开如下：

其中，R_z，t_z分别表示观测的变换矩阵中的旋转矩阵和平移矩阵，

表示误差平移量，

表示误差旋转矩阵。

S57：对i和j分别求偏导，先对平移向量求导，在对姿态求导，平移向量求导结果记为M_ij，姿态求导结果记为N_ij。

S58：三维地标为IGV路径上的路标，在运行过程中，只有部分点与三维地标顶点构建边的关系，所以雅克比矩阵形式为：

J_ij＝(0，0，0，M_ij,N_ij,0，...，0)

S59：计算一次项系数，有步骤S55可得：

S510:计算Hessian矩阵，由步骤S55可得：

S511:采用高斯牛顿法对目标函数求解，优化位姿转换关系；在IGV在运行过程中，只有在观测到三维地标时，才构建边的关系，从而减小目标函数求解计算量，优化位姿转换关系。

S6：轨迹位姿获取，在步骤S4构建局部地图的同时，通过给定的控制量和地图中多特征三维地标位姿态信息，其中多特征三维地标作为观测量，结合IMU、编码器、3D激光雷达传感器解算IGV轨迹。

S7：闭环检测，每一次建图都记录一次轨迹，IGV存储所有的轨迹，再次经过轨迹上某一点时，采用分支定界法快速遍历所有轨迹，得到历史帧。通过当前帧和历史帧之间检测成功，在步骤S5中图优化问题中增加一条边，形成约束完成闭环检测。如果IGV在经过的轨迹上，IGV进行快速重新定位并上线。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。

Claims (3)

1.一种基于3D激光雷达的多特征融合IGV定位与建图方法，其特征在于，包括如下步骤：

S1：将整个地图场景划分为大小相同的若干立方体，赋予每个立方体一定概率值，其中三维地标区域概率值赋予1，构建三维占据栅格地图。在地图中加入具有撕裂点、角点、直线、圆弧多种特征的三维地标，用3D激光雷达传感器对三维地标进行标定，根据实际距离和角度信息定义IGV初始位姿。

S2：对3D激光雷达传感器获取的数据进行预处理，记3D激光雷达采集的激光点为C，其中包含时间戳、反光强度、距离和角度信息。步骤S1定义的IGV在世界坐标系下初始位姿为(x，y，θ)，那么扫描到的激光点在世界坐标系下的坐标(x_c，y_c)为：

其中，d为C中包含的距离信息，θ为C中包含的角度信息。

激光点C的点云数据结构由x_c、y_c、时间戳、反光强度构成，设在k时刻在雷达坐标系L下获得的一帧点云为

其中t_k＝k为获取这一帧点云时的时间戳，

为k时刻在雷达坐标系L下点云中第i个点的坐标。由此可以根据雷达扫描时间、扫描角度、角度分辨率实现对三维点云数据有序化。

最后，对有序化的点云采用范围滤波、体素滤波、点云去噪去除不需要的点云，输出预处理后的点云数据。其中，范围滤波去除不在3D激光雷达场景内的点云；体素滤波使用部分点云代替整体点云并保存原点云特征，降低点云数量；点云去噪是去除范围滤波和体素滤波产生的噪声。

S3：对预处理后的3D激光雷达传感器数据进行扫描匹配，对于处理后的3D激光雷达传感器数据采用帧-子图的插入方式，其中一帧由若干点云组成，子图由若干帧组成。当前帧插入子图之前，需要对扫描帧位姿和当前的子图进行优化，让帧中所有激光点落在当前子图中的概率和最大，则问题可转化为求解以下最小二乘问题：

其中，ξ为IGV位姿，函数M()为占据栅格地图中某个坐标为障碍物的概率，函数S_i(ξ)为一帧中i号激光点照射到的障碍物在占据栅格地图中的坐标。

最小二乘问题为局部最优问题，需要一个初始位姿估计，若此时IGV静止，初始位姿可由步骤S1解算得出，若IGV运行中，则由编码器和IMU预测提供初始位姿。由此求解可得得到对当前帧和子图的位姿变换关系矩阵T_ξ如下：

其中，R_ξ为旋转矩阵，t_ξ为平移矩阵，子图使用概率网格描述。

S4：局部地图构建，对于3D激光雷达传感器持续扫描匹配，采用子图集策略，首先构建子图集合，每个子图集合由若干子图构成，子图集的最后两个子图，分别记为P1和P2，当前帧与P1匹配，并插入P1和P2中，由于子图由若干帧组成，当P2中的帧数量达到阈值数目后，标记P1已经完成，并建新的子图，将原来的P2记为新的P1，新的子图则记为P2，继续进行当前帧的插入过程。通过子图集策略，增加点云占据栅格代价函数、平移代价函数、旋转代价函数三种约束，构建局部最优子图。

S5：后端优化，由于步骤S3中激光雷达传感器扫描帧仅与当前子图进行匹配，环境地图是由一系列子图构成的，因此会存在累积误差。对此，3D激光雷达传感器获取的扫描帧在插入子图时的位姿会被缓存到内存，当子图不再变化时，所有的扫描帧和子图都会被用来进行闭环检测。采用图的优化方式，以多特征三维地标和IGV位姿为顶点，以步骤S3得到的当前帧和子图的位姿变换关系矩阵和此时IGV观测的位姿变换关系的误差函数为边，总体优化问题变为若干条边加和的形式，构建目标优化函数如下：

上式中，函数E()为误差函数，

通过分别表示在一定约束条件下，子图的位姿和扫描帧的位姿，m为子图数量，n为扫描帧数量。相对位姿ξ_ij表示扫描帧j在子图i中的匹配位置，与其相关的协方差矩阵Ω_ij共同构成优化约束。结合高斯牛顿法进行优化问题的快速求解，得到帧和子图优化后的转换关系。

S6：轨迹位姿获取，在步骤S4构建局部地图的同时，结合运动控制量的输入，多特征三维地标作为观测量，使用IMU和编码器预测IGV位姿，获得IGV运行轨迹。

S7：闭环检测，对于步骤S6获得的轨迹位姿进行存储，再次经过轨迹上某一点时，采用分支定界法快速遍历所有轨迹，得到历史帧。若当前帧和历史帧匹配成功，则在步骤S5中图优化问题中增加一条边，形成约束完成闭环检测。

2.根据权利要求1所述的一种基于3D激光雷达的多特征融合IGV定位与建图方法，其特征在于，步骤S3具体包括如下步骤：

S31：静态时，通过IGV相对于三维地标物理信息解算初始位姿，动态时，由编码器和IMU预测提供初始位姿。记初始位姿T＝{x，y，θ}，在其附近设置一个窗口并设定窗口的大小，通过下式计算窗口的迭代步长和迭代次数。

其中，h_k表示扫描帧中的激光点，d_max表示最远的激光点，r表示位置迭代步长，通常为一个栅格；δ_θ表示角度迭代步长；W_x、W_y、W_z分别表示x、y、θ的窗口大小，ω_k、ω_y、ω_θ分别表示x、y、z方向上的迭代次数。

S32：根据S31得到搜索窗口所有候选位姿变换组合w如下：

w＝{-ω_x，...，ω_x}×{-ω_y，...，ω_y}×{-ω_θ，...，ωθ}

S33：计算所有候选位姿变换得分，记其中一个候选值为T_i＝{x_i，y_i，θ_i}，将当前帧点云C变换到已建栅格地图中，对变化后各点坐标对应栅格概率值求和并除以点数，获得概率均值记为S，根据下式计算分数。

其中，α和β表示平移和旋转所占权重；

S34：从候选值中选出分数最高的位姿变换，以此作为最优匹配。

3.根据权利要求2所述的一种基于3D激光雷达的多特征融合IGV定位与建图方法，其特征在于，所述步骤S5具体包括如下步骤：

S51：记IGV在A和B点扫描匹配中最高得分的位姿变换矩阵为T_a和T_b，两点间位姿变换关系为T：

其中，在图的优化理论中T_a和T_b为图的顶点；

S52：由于步骤S3中激光雷达扫描帧仅与当前子图进行匹配，环境地图是由一系列子图构成的，因此会存在累积误差。记此时3D激光雷达观测到的位姿变换矩阵为Z_ab，则误差函数e_ab(A，B，Z_ab)表示为：

其中，t2v()表示将变换矩阵化为向量，即平移和姿态；

S53：定义图优化的边为：

e_ab＝e(A，B，Z_AB)^TΩ_ABe(A，B，Z_AB)

其中，信息矩阵Ω_AB表示对误差各分量重视程度的不一样，也就是顶点A和B权重；

S54：根据图中顶点与边的关系，优化目标函数为求解所有边之和的最小值

其中，x为顶点集，C为两个顶点排列组合的集合，

表示顶点i和j之间的边，Ω_ij为顶点i和j权重；

S55：采用高斯牛顿法对目标函数求解，利用三维地标相对于IGV的位姿变换作为顶点，在IGV在运行过程中，只有在观测到三维地标时，才构建边的关系，从而减小目标函数求解计算量，优化位姿转换关系。

Images