技术领域
[0001] 本
发明涉及高
精度定位地图领域，尤其涉及一种基于激光点云的无动态物体地图分段建立方法。
背景技术
[0002] 随着无人驾驶技术的深入发展，人们对车辆自主定位的精度及
稳定性的要求越来越高。车辆的自主定位是车辆自主导航的前提，只有当车辆能够实时地获得自身准确的
位置及
姿态，车辆才有可能进行自主导航及运动，而精度及准确性是无人车辆安全的保证。
[0003] 基于环境特征地图的定位技术，一般是通过当前扫描的激光点云或者图像与已经建立好的地图进行匹配，得到车辆位于地图上的位置。在地图准确度较高的前提下，这种定位技术往往能实现厘米级的定位精度。目前，这种类型的地图大部分是通过SLAM(Simultaneous Localization And Mapping)方法建立。SLAM技术通过将
传感器得到的环境信息与地图信息进行匹配，计算车辆当前在地图上的位置，同时，基于计算得到的车辆位置，将当前的环境信息加载到地图上进行地图的更新。定位与地图更新交替进行，这也是SLAM技术名字的由来。SLAM技术基于传感器类型，将使用摄像头的部分称为视觉SLAM，将使用
激光雷达的部分称为激光SLAM。激光SLAM具有激光雷达具有的优点：稳定性高，精度高以及不受光照影响；同时也有以下缺点：受极端天气影响，成本高，无法得到图案信息。而基于摄像头的视觉SLAM方法则具有：成本低、探测得到的环境信息丰富，尤其是图案纹理信息；也同样有摄像头的缺点：受光照影响大，计算量大，计算精度及稳定性往往不能达到自动驾驶要求。在各有利弊的情况下，两种SLAM 技术在近些年都取得了一定的发展。
[0004] 激光SLAM技术可以分布两个模
块：前端的
帧间信息匹配和后端的优化。前端的配准
算法主要分为两种类型：基于点与点之间的ICP类配准算法；基于势场类的配准算法，如NDT配准。ICP类配准算法主要通过寻找目标点云中与输入点云的匹配点，形成匹配点对，通过一定方法，尽可能减小匹配点对之间的距离。而势场类配准算法则是通过在目标点云中生成一个势场，然后将输入点云基于转换矩阵投射在目标点云空间中，每一个投射结果都会获得一个得分，表示匹配程度，之后再通过一定的
优化算法，最优化投射得分，从而得到最优的转换矩阵。由于NDT方法使用了一种基于统计的概率
密度函数生成，该方法对于三维环境和非结构环境的鲁棒性、容错性都有较好的表现，比较适合校园内室外环境。
[0005] 前端匹配起到了
里程计的作用，但是与其他里程计一样，这种基于位移的地图更新方法会有累积误差。因此，在地图闭环时会不可避免地出现回环误差。回环检测和回环误差的消除则是后端优化的重要任务。
[0007] 1、在SLAM建图过程中，需要对每一帧激光点云信息进行配准操作，从而得到当前车辆的位置。NDT配准算法中概率密度函数的生成的复杂度正比于目标点云的点数(其他配准算法的复杂度也与目标点云点数成正相关)，即O(n)，n 为目标点云的点数。而随着地图的不断生长，NDT配准消耗的时间也会逐渐变大。因此当建立大环境的地图时，建图效率相对较低；
[0008] 2、由于激光点云是一种稀疏的点云结构，基于激光雷达的SLAM技术在回环检测的实现比较困难，从而无法进行回环误差的消除。而在室外开阔环境中，回环误差的消除又是十分必要的；
[0009] 3、在激光雷达扫描过程中，行人和其他车辆等动态物体会被激光雷达扫描到从而形成点云，这类点云信息在建图过程中会被持续放入地图中。而由于在不同的点云扫描帧中，动态物体所在的位置是不同的，因此在最终的地图中会出现类似于动态物体的运动轨迹的信息，这些是地图中不需要的冗余信息。这种错误信息的累积会对最终地图的精度和后面车辆的定位精度造成影响。
[0010] 因此，本领域的技术人员致
力于开发一种新的地图建立方法，能够克服现有技术中存在的上述
缺陷。
发明内容
[0011] 有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是如何使用单一激光雷达，应用激光SLAM技术构建可以实现车辆的高精度(厘米级)及高
频率(10Hz) 定位的环境地图，以及如何通过一系列优化，防止激光SLAM过程中建图效率过度下降同时进行回环检测和回环误差消除。同时，本发明所要解决的技术问题还包括如何解决动态物体在建图过程中在不同位置多次出现，导致地图上出现其运动轨迹的问题。
[0012] 相关术语的具体解释如下：
[0013] 点云：一种数据结构。一系列点的集合，描述了这些点的位置信息。点云的相加指两个点集的并集。
[0014] NDT(见参考文献：Biber P.The Normal Distributions Transform:A New Approach to Laser Scan Matching[J].Proc.of IROS2003,2003.)：一种基于网格划分、概率密度函数生成和最大化似然的配准算法；
[0015] 回环检测：又称闭环检测，指车辆识别在整个建图过程中曾经到达某个场景，进而使地图进行闭环的检测工作；回环检测
中子地图间出现的位置误差又称为回环误差。
[0016] kdTree查询方法：一种利用树结构进行高效查询的方法。可以得到点云中距离某一坐标P距离小于一定
阈值的点的数量，这些被找到的点称为P点的r邻域点；
[0017] FPFH特征(见参考文献：Rusu R B.Semantic 3D Object Maps for Everyday Manipulation in Human Living Environments[J].KI-Kunstliche Intelligenz,2010, 24(4):345-348.P57-P61.)：快速点特征直方图，用于描述点云在一个局部空间的几何特征；
[0018] SAC-IA算法(见参考文献：Rusu R B.Semantic 3D Object Maps for Everyday Manipulation in Human Living Environments[J].KI-Kunstliche Intelligenz,2010, 24(4):345-348.P57-P61.)：
采样一致性初始配准算法，通
过采样的方式，得到两幅点云之间基于特征的矩阵变换关系。这种方法出来的结果往往精确度比较低，还需要进行进一步的精准计算，但是可以给出离正确结果较近的一个值。该方法中，得分越低表示结果越优。
[0019] 为实现上述目的，本发明提供了一种基于激光点云的无动态物体地图分段建立方法，所述方法包括以下步骤：
[0020] 步骤1、用激光雷达进行
数据采集，获得点云数据；所述点云数据为多帧数据；
[0021] 步骤2、使用工控机播放采集好的所述点云数据，离线创建地图；对所述点云数据的每一帧进行
体素网格降采样后传递给所述地图创建方法；
[0022] 其中，预设的参数有：
[0023] nframe为所述点云数据中某一帧数据的序号；
[0024] ninit为初始化帧数；
[0025] shiftadded为地图更新距离；
[0026] r为kdTree的查询半径；
[0027] nstable为“新点”的稳定次数阈值；
[0028] ndelete为所述“新点”的消失次数阈值；
[0029] nbuffer为缓存
种子地图的帧数；
[0030] nsubmap为子地图的帧数；
[0031] nstartBuffer为缓存种子地图开始帧序号，nstartBuffer＝nsubmap-nbuffer；
[0032] 步骤3、对所述地图进行无动态物体初始化；
[0033] 步骤4、对所述子地图进行无动态物体创建及更新；
[0034] 步骤5、对所述地图进行回环检测，并计算所述地图的误差；
[0035] 步骤6、优化所述地图的所述误差；
[0036] 步骤7、将经过误差优化的所述地图输出。
[0037] 进一步地，所述激光雷达位于车辆的顶端正中间处。
[0038] 进一步地，所述步骤3包括：
[0039] 步骤3.1、记录所述点云数据的第一帧信息为frameFirst，以扫描所述第一帧信息时的所述激光雷达的
坐标系作为所述地图的坐标系，扫描所述第一帧信息时的所述激光雷达的位置为初始位置；
[0040] 步骤3.2、使用NDT配准方法，以所述frameFirst为目标点云，以所述激光雷达的每一帧扫描信息为输入点云，得到所述车辆相对所述初始位置的位移shift；
[0041] 步骤3.3若nframe≥ninit或shift≥shiftadded，使用kdTree在当前帧的配准结果 scanAligned与frameFirst间进行互相查询；去除掉属于scanAligned却无法在frameFirst 中查询到r邻域点的点；去除掉属于frameFirst却无法在scanAligned中查询到r邻域点的点，得到frameFirstFiltered与scanAlignedFiltered；
[0042] 步骤3.4将frameFirstFiltered与scanAlignedFiltered进行相加，得到最终的初始地图，即mapinit，完成所述地图的初始化。
[0043] 进一步地，所述步骤4包括：
[0044] 步骤4.1、令submap＝mapinit，其中submap为当前子地图；
[0045] 步骤4.2、设置缓存种子地图mapbuffer；
[0046] 步骤4.3、设置变量nadded，表示在所述当前子地图下的地图更新次数；
[0047] 步骤4.4、设置数据结构“新点”pointNew，包含成员：坐标信息：x，y，z；所述“新点”的出现时间：texist，初始化为0；所述“新点”被看到次数：nsee，初始化为0。
[0048] 同时设置一个容器PointCloudNew，用于存放所述“新点”；
[0049] 步骤4.5、使用所述NDT配准方法，以所述submap为目标点云，以所述激光雷达的扫描信息scan为输入点云，获取所述车辆的位置，进而计算所述车辆相对上一次地图更新时刻的位移shift；配准结果的点云为scanAligned；
[0050] 步骤4.6、若shift≥shiftadded，则需要进行地图信息添加，所述nadded的值增加1；
[0051] 步骤4.7、对所述容器PointCloudNew中的所述pointNew的状态进行更新；若所述PointCloudNew不为空，则在所述PointCloudNew上查询所述scanAligned中的点，被查询到的所述pointNew，其nsee值增加1；若nsee>nstable，则认为所述pointNew稳定，将所述pointNew添加到所述submap中，同时将该所述pointNew从所述容器 PointCloudNew中删除；将剩下的所述pointNew的texist增加1，若texist>ndelete，则认为该所述pointNew为临时出现的非稳定点，将该所述pointNew从所述容器 PointCloudNew中删除；
[0052] 步骤4.8、在所述scanAligned中寻找“新点”；在所述submap中查询所述 scanAligned中的点，对于查询不到r邻域点的所述scanAligned中的点，则认为该点为新出现的点，使用该点坐标创建所述pointNew，添加到所述容器PointCloudNew中，同时将该点从所述scanAligned中删除；
[0053] 步骤4.9、将所述scanAligned中的数据点加入所述submap中，即 submap＝submap+scanAligned；
[0054] 步骤4.10、若nadded≥nstartBuffer，则将所述scanAligned加入所述缓存种子地图 mapbuffer中，即mapbuffer＝mapbuffer+scanAligned；
[0055] 步骤4.11、若nadded＝nsubmap，则当前子地图的建立完毕，将该子地图保存到本地；同时重置多个参数，重置地图更新次数nadded＝0；用缓存种子地图作为当前子地图 submap＝mapbuffer；清空缓存种子地图；
[0056] 步骤4.12、传入下一帧点云信息，重复所述步骤4.5到所述步骤4.11的操作直到结束所述点云数据的所有帧。
[0057] 进一步地，所述步骤5包括：
[0058] 步骤5.1、每建立一幅子地图submapcurrent，对其他已建立的子地图进行一个判定，若该子地图与submapcurrent距离小于一定阈值，且二者建立的序号差大于一定阈值时，将该子地图加入回环检测子地图集合中。
[0059] 步骤5.2、进行FPFH粗配准的回环检测；
[0060] 步骤5.3、进行NDT精配准的误差计算。
[0061] 进一步地，所述步骤5.2包括：
[0062] 步骤5.2.1、分别计算所述子地图submapcurrent与所述子地图submaptest的法向特征；
[0063] 步骤5.2.2、基于所述法向特征分别计算所述子地图submapcurrent与所述子地图submaptest的FPFH特征；
[0064] 步骤5.2.3、基于所述FPFH特征，使用SAC-IA算法进行回环粗配准；
[0065] 步骤5.2.4取回环检测得分最小的为最优粗匹配，若最优粗配准得分小于一定阈值，准得分小于一定阈值，则认为回环检测成功，将被检测的子地图和当前子地图的序号分别赋值给nstart和nend。
[0066] 进一步地，所述步骤5.3包括：
[0067] 步骤5.3.1、基于所述FPFH粗配准的回环检测成功的最优结果使用NDT配准方法，对两幅点云的粗配准结果进行精确配准；
[0068] 步骤5.3.2、得到最终的配准结果转化矩阵Terror；所述转化矩阵即为用所述地图创建方法所产生的回环误差。
[0069] 进一步地，所述步骤6包括：
[0070] 步骤6.1、将所述回环误差扁平化；计算序号处于nstart和nend间的每两幅相邻子地图间的平均误差：TerrorSubmap＝Terror/(nend-nstart)；
[0071] 步骤6.2、以子地图为单位进行位置修正。
[0072] 进一步地，在所述步骤6.2中，对于序号为i(nstart≤i≤nend)的子地图，进行Ti＝(i- nstart)×TerrorSubmap的点云变换；对于每幅子地图，有：submapcorrect_i＝submapi×Ti。
[0073] 进一步地，所述步骤7包括：
[0075] 步骤7.2、地图降采样，对map进行体素网格降采样；
[0076] 步骤7.3、输出并保存完整地图。
[0077] 在本发明的实施方式中，还可以使用其他的多线激光雷达。
[0078] 在本发明的另一实施方式中，点云的帧间配准方法不一定要使用NDT方法，也可以使用ICP方法、PL-ICP方法等其他帧间配准算法。
[0079] 在本发明的另一实施方式中，回环检测也不一定要使用FPFH+SAC-IA的粗配准方法，也可以使用其他基于点云特征的配准方法。
[0080] 本发明提供的一种基于激光点云的无动态物体地图分段建立方法有以下技术效果：
[0081] 1、通过逐步保存地图到本地，避免了地图过度增长给工控机带来的巨大的计算压力；通过这种优化，使得算法具有建立大环境激光点云地图的能力。若全程使用保持着完整的地图进行更新，则配准算法中概率密度函数生成的复杂度是：O(n)，其中n 为地图点数；本发明通过逐步保存地图，限定了概率密度函数生成的复杂度最大值： O(K)，其中，K为与预设值相关的常数；
[0082] 2、针对回环误差出现的问题，又通过FPFH特征计算及基于FPFH特征的初始配准算法，检测并消除了回环误差，保证了该方法的正常使用。
[0083] 3、子地图方法的带来的另一点优化是，地图过大会给计算机内存带来影响，甚至造成工控机的崩溃。我们可以把一幅最终大小为100M级别的地图分成2～5M的若干幅子地图分别建立，这种方法十分适用于大环境、长距离的地图建立。
[0084] 4、通过设置缓存容器，将要加入地图的点先置于容器中，等到这些点出现足够多次数时再认为是在地图中稳定的点，该操作有效地滤除环境中的动态物体(如车辆、行人等)，使得建立的地图更贴近真实环境原有的场景，有利于最后定位的进行。
[0085] 以下将结合
附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。
附图说明
[0087] 图2是地图初始化示意图；
[0088] 图3是地图更新示意图；
[0089] 图4是回环检测及误差优化示意图；
[0090] 图5是动态物体运动轨迹滤除效果图。
具体实施方式
[0091] 以下参考
说明书附图介绍本发明的多个优选
实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。
[0092] 如图2所示，为本发明所公开的一种基于激光点云的无动态物体地图分段建立方法，包括以下步骤：
[0093] 步骤1、激光雷达数据采集；
[0094] 步骤2、数据降采样；
[0095] 步骤3、地图无动态物体初始化；
[0096] 步骤4、子地图无动态物体创建及更新；
[0097] 步骤5、回环检测及误差计算；
[0098] 步骤6、误差优化；
[0099] 步骤7、地图输出。
[0100] 其中，预设的参数有：
[0101] nframe为点云数据中某一帧数据的序号；
[0102] ninit为初始化帧数；
[0103] shiftadded为地图更新距离；
[0104] r为kdTree的查询半径；
[0105] nstable为“新点”的稳定次数阈值；
[0106] ndelete为“新点”的消失次数阈值；
[0107] nbuffer为缓存种子地图的帧数；
[0108] nsubmap为子地图的帧数；
[0109] nstartBuffer为缓存种子地图开始帧序号，nstartBuffer＝nsubmap-nbuffer；
[0110] 其中，步骤1、用激光雷达进行数据采集，获得点云数据；点云数据为多帧数据。
[0111] 其中，步骤2、使用工控机播放采集好的点云数据，离线创建地图；对点云数据的每一帧进行体素网格降采样后传递给地图创建方法。
[0112] 其中，激光雷达位于车辆的顶端正中间处。
[0113] 如图3所示，步骤3的地图初始化包括以下步骤：
[0114] 步骤3.1、记录点云数据的第一帧信息为frameFirst，以扫描第一帧信息时的激光雷达的坐标系作为地图的坐标系，扫描第一帧信息时的激光雷达的位置为初始位置；
[0115] 步骤3.2、使用NDT配准方法，以frameFirst为目标点云，以激光雷达的扫描信息scan为输入点云，得到车辆相对初始位置的位移shift；
[0116] 步骤3.3若nframe≥ninit或shift≥shiftadded，使用kdTree在当前帧的配准结果 scanAligned与frameFirst间进行互相查询；去除掉属于scanAligned却无法在frameFirst 中查询到r邻域点的点；去除掉属于frameFirst却无法在scanAligned中查询到r邻域点的点，得到frameFirstFiltered与scanAlignedFiltered；
[0117] 步骤3.4将frameFirstFiltered与scanAlignedFiltered进行相加，得到最终的初始地图，即mapinit＝frameFirstFiltered+scanAlignedFiltered，完成地图的初始化。
[0118] 如图4所示，步骤4的地图更新包括以下步骤：
[0119] 步骤4.1、令submap＝mapinit，其中submap为当前子地图；
[0120] 步骤4.2、设置缓存种子地图mapbuffer；
[0121] 步骤4.3、设置变量nadded，表示在当前子地图下的地图更新次数；
[0122] 步骤4.4、设置数据结构“新点”pointNew，包含成员：坐标信息：x，y，z；“新点”的出现时间：texist，初始化为0；“新点”被看到次数：nsee，初始化为0；同时设置一个容器PointCloudNew，用于存放“新点”；
[0123] 步骤4.5、使用NDT配准方法，以submap为目标点云，以激光雷达的扫描信息 scan为输入点云，获取车辆的位置，进而计算车辆相对上一次地图更新时刻的位移 shift；配准结果的点云为scanAligned；
[0124] 步骤4.6、若shift≥shiftadded，则需要进行地图信息添加，nadded的值增加1；
[0125] 步骤4.7、对所述容器PointCloudNew中的所述pointNew的状态进行更新；若所述PointCloudNew不为空，则在所述PointCloudNew上查询所述scanAligned中的点，被查询到的所述pointNew，其nsee值增加1；若nsee>nstable，则认为所述pointNew稳定，将所述pointNew添加到所述submap中，同时将该所述pointNew从所述容器 PointCloudNew中删除；将剩下的所述pointNew的texist增加1，若texist>ndelete，则认为该所述pointNew为临时出现的非稳定点，将该所述pointNew从所述容器 PointCloudNew中删除；
[0126] 步骤4.8、在scanAligned中寻找“新点”；在submap中查询scanAligned中的点，对于查询不到r邻域点的scanAligned中的点，则认为该点为新出现的点，使用该点坐标创建pointNew，添加到容器PointCloudNew中，同时将该点从scanAligned中删除；
[0127] 步骤4.9、将scanAligned中的数据点加入submap中，即 submap＝submap+scanAligned；
[0128] 步骤4.10、若nadded≥nstartBuffer，则将scanAligned加入缓存种子地图mapbuffer中，即mapbuffer＝mapbuffer+scanAligned；
[0129] 步骤4.11、若nadded＝nsubmap，则当前子地图的建立完毕，将该子地图保存到本地；同时重置多个参数，重置地图更新次数nadded＝0；用缓存种子地图作为当前子地图 submap＝mapbuffer；清空缓存种子地图；
[0130] 步骤4.12、传入下一帧点云信息，重复步骤4.5到步骤4.11的操作直到结束点云数据的所有帧。
[0131] 如图5所示，步骤5的回环检测及误差计算包括以下步骤：
[0132] 步骤5.1、每建立一幅子地图submapcurrent，对其他已建立的子地图进行一个判定，若该子地图与submapcurrent距离小于一定阈值，且二者建立的序号差大于一定阈值时，将该子地图加入回环检测子地图集合中。
[0133] 步骤5.2、进行FPFH粗配准的回环检测；
[0134] 步骤5.3、进行NDT精配准的误差计算。
[0135] 其中，步骤5.2包括：
[0136] 步骤5.2.1、分别计算子地图submapcurrent与子地图submaptest的法向特征；
[0137] 步骤5.2.2、基于法向特征分别计算子地图submapcurrent与子地图submaptest的FPFH 特征；
[0138] 步骤5.2.3、基于FPFH特征，使用SAC-IA算法进行粗配准；
[0139] 步骤5.2.4取回环检测得分最小的为最优粗匹配，若最优粗配准得分小于一定阈值，则认为回环检测成功，将被检测的子地图和当前子地图的序号分别赋值给nstart和 nend。
[0140] 其中，步骤5.3包括：
[0141] 步骤5.3.1、基于FPFH粗配准的回环检测成功的结果使用NDT配准方法，以nstart为目标点云，以nend为输入点云，对两幅点云进行精确配准；
[0142] 步骤5.3.2、得到最终的配准结果转化矩阵Terror；转化矩阵即为地图建立所产生的回环误差。
[0143] 步骤6的误差优化包括以下步骤：
[0144] 步骤6.1、将回环误差扁平化；计算序号处于nstart和nend间的每两幅相邻子地图间的平均误差：TerrorSubmap＝Terror/(nend-nstart)；
[0145] 步骤6.2、以子地图为单位进行位置修正。
[0146] 其中，在步骤6.2中，对于序号为i(nstart≤i≤nend)的子地图，进行Ti＝(i- nstart)×TerrorSubmap的点云变换；对于每幅子地图，有：submapcorrect_i＝submapi×Ti。
[0147] 步骤7的地图输出包括以下步骤：
[0148] 步骤7.1、子地图叠加，即
[0149] 步骤7.2、地图降采样，对map体素网格降采样；
[0150] 步骤7.3、输出并保存完整地图。
[0151] 以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多
修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的
基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由
权利要求书所确定的保护范围内。
