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

本发明专利技术公开了一种基于3D激光雷达的多特征融合IGV定位与建图方法。主要包括数据采集处理、扫描匹配及局部地图构建、后端优化、回环检测四个过程；数据采集处理是对3D激光雷达传感器的数据处理；扫描匹配及局部地图构建是对处理过的激光点云数据采用帧

【技术实现步骤摘要】
一种基于3D激光雷达的多特征融合IGV定位与建图方法


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

技术介绍

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

技术实现思路

[0003]本专利技术的目的在于使仓储IGV实现高精度定位和建图，同时克服2D激光雷达的平面局限，满足仓储IGV全天运转的需求，从而本申请提出一种基于3D激光雷达的多特征融合的IGV定位与建图方法，该方法能适用与各种复杂仓储环境，通过在环境中建立若干三维地标，通过标定在子图这一概念中加入这一多特征属性，对IGV运行过程中扫描匹配进行约束，实现局部最优子图的构建。由于建立好的子图存在误差，该方法采用图优化建立优化目标函数，通过高斯牛顿法求解，依靠多特征三维地标这一观测值减少计算量，快速找到最优子图。在最优子图构建后，存储所有轨迹，再次经过时，遍历所有轨迹，对IGV重新定位，完成闭环检测，从而实现IGV高精度定位和建图
[0004]为达到上述目的，本专利技术的技术方案如下：一种基于3D激光雷达的多特征融合IGV定位与建图方法，包括如下步骤：
[0005]S1：将整个地图场景划分为大小相同的若干立方体，赋予每个立方体一定概率值，其中三维地标区域概率值赋予1，构建三维占据栅格地图。在地图中加入具有撕裂点、角点、直线、圆弧多种特征的三维地标，用3D激光雷达传感器对三维地标进行标定，根据实际距离和角度信息定义IGV初始位姿。
[0006]S2：对3D激光雷达传感器获取的数据进行预处理，记3D激光雷达采集的激光点为C，其中包含时间戳、反光强度、距离和角度信息。步骤S1定义的IGV在世界坐标系下初始位姿为(x,y,θ)，那么扫描到的激光点在世界坐标系下的坐标(x
c
，y
c
)为：
[0007][0008]其中，d为C中包含的距离信息，θ为C中包含的角度信息。
[0009]激光点C的点云数据结构由x
c
、y
c
、时间戳、反光强度构成，设在k时刻在雷达坐标系
L下获得的一帧点云为其中t
k
＝k为获取这一帧点云时的时间戳，为k时刻在雷达坐标系L下点云中第i个点的坐标。由此可以根据雷达扫描时间、扫描角度、角度分辨率实现对三维点云数据有序化。
[0010]最后，对有序化的点云采用范围滤波、体素滤波、点云去噪去除不需要的点云，输出预处理后的点云数据。其中，范围滤波去除不在3D激光雷达场景内的点云；体素滤波使用部分点云代替整体点云并保存原点云特征，降低点云数量；点云去噪是去除范围滤波和体素滤波产生的噪声。
[0011]S3：对预处理后的3D激光雷达传感器数据进行扫描匹配，对于处理后的3D激光雷达传感器数据采用帧
‑
子图的插入方式，其中一帧由若干点云组成，子图由若干帧组成。当前帧插入子图之前，需要对扫描帧位姿和当前的子图进行优化，让帧中所有激光点落在当前子图中的概率和最大，则问题可转化为求解以下最小二乘问题：
[0012][0013]其中，ξ为IGV位姿，函数M()为占据栅格地图中某个坐标为障碍物的概率，函数S
i
(ξ)为一帧中i号激光点照射到的障碍物在占据栅格地图中的坐标。
[0014]最小二乘问题为局部最优问题，需要一个初始位姿估计，若此时IGV静止，初始位姿可由步骤S1解算得出，若IGV运行中，则由编码器和IMU预测提供初始位姿。由此求解可得得到对当前帧和子图的位姿变换关系矩阵T
ξ
如下：
[0015][0016]其中，R
ξ
为旋转矩阵，t
ξ
为平移矩阵，子图使用概率网格描述。
[0017]S4：局部地图构建，对于3D激光雷达传感器持续扫描匹配，采用子图集策略，首先构建子图集合，每个子图集合由若干子图构成，子图集的最后两个子图，分别记为P1和P2，当前帧与P1匹配，并插入P1和P2中，由于子图由若干帧组成，当P2中的帧数量达到阈值数目后，标记P1已经完成，并建新的子图，将原来的P2记为新的P1，新的子图则记为P2，继续进行当前帧的插入过程。通过子图集策略，增加点云占据栅格代价函数、平移代价函数、旋转代价函数三种约束，构建局部最优子图。
[0018]S5：后端优化，由于步骤S3中激光雷达传感器扫描帧仅与当前子图进行匹配，环境地图是由一系列子图构成的，因此会存在累积误差。对此，3D激光雷达传感器获取的扫描帧在插入子图时的位姿会被缓存到内存，当子图不再变化时，所有的扫描帧和子图都会被用来进行闭环检测。采用图的优化方式，以多特征三维地标和IGV位姿为顶点，以步骤S3得到的当前帧和子图的位姿变换关系矩阵和此时IGV观测的位姿变换关系的误差函数为边，总体优化问题变为若干条边加和的形式，构建目标优化函数如下：
[0019][0020]上式中，函数E()为误差函数,通过分别表示在一定约束条件下，子图的位姿和扫描帧的位姿，m为子图数量，n为扫描帧数量。相对位姿ξ
ij
表示扫描帧j在子图i中的匹配位置，与其相关的协方差矩阵Ω
ij
共同构成优化约束。结合高斯牛顿法进行优化问题的快速求解，得到帧和子图优化后的转换关系。
[0021]S6：轨迹位姿获取，在步骤S4构建局部地图的同时，结合运动控制量的输入，多特征三维地标作为观测量，使用IMU和编码器预测IGV位姿，获得IGV运行轨迹。
[0022]S7：闭环检测，对于步骤S6获得的轨迹位姿进行存储，再次经过轨迹上某一点时，采用分支定界法快速遍历所有轨迹，得到历史帧。若当前帧和历史帧匹配成功，则在步骤S5中图优化问题中增加一条边，形成约束完成闭环检测。
[0023]进一步地，步骤S3具体包括如下步骤：
[0024]S31：静态时，通过IGV相对于三维地标物理信息解算初始位姿，动态时，由编码器和IMU预测提供初始位姿。记初始位姿T＝{x,y,θ}，在其附近设置一个窗口并设定窗口的大小，通过下式计算窗口的迭代步长和迭代次数。
[0025][0026][0027][0028]其中，h
k
表示扫描帧中的激光点，d
max
表示最远的激光点，r表示位置迭代步长，通常为一个栅格；δ
本文档来自技高网...

【技术保护点】

【技术特征摘要】
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构建局部地图的同时，...

【专利技术属性】
技术研发人员：马啸，林志赟，韩志敏，王博，
申请(专利权)人：杭州电子科技大学，
类型：发明
国别省市：