CN117974926A - 一种基于机械旋转式三维激光的定位与建图方法及*** - Google Patents

Abstract

本申请涉及一种基于机械旋转式三维激光的定位与建图方法及***，其属于定位与建图技术领域，其中方法包括：基于运动畸变校正模型，对激光点云数据执行畸变校正处理；对畸变校正后的激光点云数据执行预处理操作，所述预处理操作至少包括地面点云分割、角点分割、平面点分割，实时滤除动态点；将预处理后的激光点云数据分别合并形成关键帧，并与之前的关键帧进行回环检测，在检测出回环后，对所有的关键帧位姿进行优化处理；基于关键帧点云和优化后的关键帧位姿，构建并输出整体点云地图。本申请提出的方法能使用较少运算量并提高三维激光雷达定位和建图精度。

Description

一种基于机械旋转式三维激光的定位与建图方法及***

技术领域

本申请涉及定位与建图技术领域，尤其是涉及一种基于机械旋转式三维激光的定位与建图方法。

背景技术

移动机器人目前已运用到各类场所中，例如工业物流中的仓储机器人，服务业中的扫地机器人，医疗行业中的消杀机器人等。随着人们对智能化生活方式的向往，人们对于移动机器人的需求变得多元化，希望其能够完美实现特定的任务。同时定位与建图技术（SLAM）目前普遍运用于移动机器人领域，其被认为是实现避障、路劲规划、导航等应用的核心环节。

SLAM的任务是定位与建图；当机器人处于未知环境下，通过传感器感知环境信息并构建地图，同时估计自身位姿。而激光雷达因能提供精准可靠的测距，且不易受光照影响，被广泛应用于SLAM测距。

由于大多数激光雷达是旋转结构，所以其会在旋转过程中逐步扫描周围环境，并采集生成激光点云数据，相应的，激光点云数据即为外部环境在三维空间的离散取样数据；但是基于机械旋转扫描的激光雷达在运动过程中，其坐标系不断发生变化，会导致同一帧点云中的点坐标不在同一坐标系下，从而产生运动畸变，而运动畸变易降低SLAM***的鲁棒性和精准性。

发明内容

为了改善激光雷达的运动畸变对SLAM***的影响，优化SLAM***的鲁棒性和精准性，本申请提供一种基于机械旋转式三维激光的定位与建图方法。

第一方面，本申请提供了一种基于机械旋转式三维激光的定位与建图方法，包括：

基于运动畸变校正模型，对激光点云数据执行畸变校正处理；

对畸变校正后的激光点云数据执行预处理操作，所述预处理操作至少包括地面点云分割、角点分割、平面点分割，实时滤除动态点；

将预处理后的激光点云数据分别合并形成关键帧，并与之前的关键帧进行回环检测，在检测出回环后，对所有的关键帧位姿进行优化处理；

基于关键帧点云和优化后的关键帧，构建并输出整体点云地图。

通过采用上述技术方案，对获取到的激光点云数据进行去运动畸变处理，以重新确定点云数据中的每一点的实际位置，优化定位精准性，再对点云数据进行预处理、合并关键帧、回环检测及回环优化，最终得到整体点云地图，实现定位与构图，提高定位与建图技术的精准性和鲁棒性。

可选的，所述激光点云数据由预设的激光雷达按照预设频率扫描获得，且每一次激光雷达扫描获得的激光点云数据为一帧点云数据；

所述基于运动畸变校正模型，对激光点云数据执行畸变校正处理，包括：

每当获取到一帧点云数据时，基于预构建的轮式里程计插值模型，计算当前帧点云数据的位姿，以及前一帧点云数据的位姿/>；其中，所述前一帧点云数据是指：从对应的激光雷达扫描时间维度，在当前帧点云数据之前的一帧帧点云数据；

计算得出所述当前帧点云数据相对所述前一帧点云数据的相对位姿；

遍历所述当前帧点云数据的每一点，根据每一点所在Z方向投影坐标，计算得出每一点所对应的水平方向角度，将所述当前帧点云数据中的最后一个点的水平角度定义为；计算得出每一点所对应生成时间/>，以及插值比例/>；其中，第/>个点所在Z方向投影坐标可以表示为/>，第/>个点所对应的水平方向角度/>；/>；；

根据所述当前帧点云数据中的每一点所对应的生成时间，以及插值比例/>，通过预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿，根据位姿计算得到当前帧每一点的坐标数据，完成畸变校正。

通过采用上述技术方案，本申请主要利用传感器辅助方法来补偿运动畸变，具体使用轮式里程计方法，通过求解激光雷达每一帧点云数据所对应的里程计位姿，再将求得的位姿转换到同一坐标系下，最后封装得到对应帧的点云数据，实现畸变纠正。

可选的，所述方法还包括：

获取轮式里程计检测得出的里程计数据，并按检测时间先后顺序，将里程计数据顺次从预设队列的尾部***所述预设队列中；其中，每一所述里程计数据均包括数据时间戳和位姿信息；

当接收到插值时间戳时，所述预构建的轮式里程计差值模型将所述预设队列中的里程计数据从队首逐一删除；且每删除一个里程计数据时，暂存被删除的里程计数据的数据时间戳/>和位姿信息/>，并判定是否满足预设的插值条件，若是，则停止删除，并执行插值操作；其中，所述预设的差值条件为：预设队列中仅剩1个里程计数据或预设队列队首的里程计数据所对应的数据时间戳/>大于所述插值时间戳/>；

其中，所述执行插值操作，包括：

若所述插值时间戳满足第一预设条件，则确定并输出所述插值时间戳/>所对应的位姿信息/>；其中，所述第一预设条件为：所述插值时间戳/>与所述/>的时间差值小于第一预设差值；

若所述插值时间戳满足第二预设条件，则确定并输出/>；其中，所述第二预设条件为：所述插值时间戳/>与所述/>的时间差值小于第二预设差值；

若所述插值时间戳不满足第一预设条件和第二预设条件，且在/>与/>之间，则确定并输出/>，其中，/>为所述/>与/>之间的位姿速度；

若所述插值时间戳大于/>，且/>与/>的时间间隔小于预设时间阈值，则确定并输出；若/>与/>的时间间隔不小于预设时间阈值，则确定并输出所述插值时间戳/>所对应的位姿信息/>；

所述根据所述当前帧点云数据中的每一点所对应的生成时间，以及插值比例/>，通过预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿，包括：

根据所述当前帧点云数据中的每一点所对应的生成时间，以及插值比例/>确定每一点所对应的插值时间戳/>，将插值时间戳/>输入预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿。

可选的，所述预处理操作还包括降采样处理；所述将预处理后的激光点云数据分别合并形成关键帧，包括：

S1041，每当对激光点云数据完成预处理操作之后，判定当前关键帧是否为空；

S1042，若是，则将当前完成预处理操作的激光点云数据作为第一帧点云数据***关键帧，并将所述所述第一帧点云数据作为当前关键帧的参考帧；计算所述所述第一帧点云数据中的角点点云的点云重心和平面点点云的点云重心/>；其中，，/>，/>为所述第一帧点云数据中的角点数量，为所述第一帧点云数据中的平面点数量，/>为所述第一帧点云数据所对应的角点点云中的第/>个角点的坐标，/>为所述第一帧点云数据所对应的平面点点云中的第/>个平面点的坐标；

S1043，若否，则计算并判定当前完成预处理操作的当前帧点云数据是否满足第一判定条件或第二判定条件，再根据判定结果执行预设处理操作；

其中，所述根据判定结果执行预设处理操作，包括：

若满足第一判定条件，则将当前帧点云数据所对应的位姿变换到参考帧坐标系，将所述当前帧点云数据与参考帧进行点云叠加并降采样形成新的参考帧，更新新的参考帧的、/>；

若满足第二判定条件，则将此时的关键帧打包并存入预设的关键帧数据库中，清空关键帧，将所述当前帧点云数据***清空后的关键帧，并用所述当前帧点云数据替换当前参考帧；其中，所述关键帧数据库用于存储每次打包形成的关键帧；

其中，所述第一判定条件为：第n帧点云数据的相对参考帧的/>、第n帧点云数据的/>相对参考帧的/>的距离值之和小于第一距离阈值；第二判定条件为：第n帧点云数据的/>相对参考帧的/>、第n帧点云数据的/>相对参考帧的/>的距离值之和不小于第一距离阈值，或第n帧点云数据的位姿坐标参考帧位姿坐标的距离大于第二距离阈值，或第n帧激光点云数据的旋转角度大于指定角度阈值。

可选的，所述将预处理后的激光点云数据分别合并形成关键帧，还包括：

每当对激光点云数据完成预处理操作后，将完成预处理操作后的当前帧点云数据***副关键帧，并对主关键帧执行所述S1041、S1042、S1043的步骤内容；

当主关键帧满足第二判定条件并执行所述预设处理操作中的打包操作时，即主关键帧被打包并存入预设的关键帧数据库中时，若此次打包为首次打包，则记录当前主关键帧被打包时的位姿坐标M、确定当前主关键帧位姿坐标M与参考帧位姿坐标的相对距离；若此次打包非首次打包，则确定当前主关键帧位姿、主关键帧位姿与前一次打包记录的主关键帧位姿坐标M的相对距离，用当前主关键帧位姿更新M，用当前确定的相对距离更新/>；

当副关键帧的位姿坐标距离坐标M的相对距离达到时，将副关键帧打包并保存至关键帧数据库中；

在每次确定得出相对距离之后，若主关键帧在副关键帧的位姿坐标与M的相对距离达到/>之前，执行预处理操作中的打包操作，此时则将副关键帧和主关键帧一同打包并保存到关键帧数据库中；

每当主关键帧被打包后，清空主关键帧；每当副关键帧被打包后，清空副关键帧。

可选的，所述与之前的关键帧进行回环检测，在检测出回环后，对所有的关键帧位姿进行优化处理，包括：

每当所述关键帧数据库中新增关键帧时，计算当前新增关键帧的特征向量，将当前新增关键帧的特征向量存储于预设的回环检测模块中；

基于预建立的k-d树，确定与所述当前新增关键帧的特征向量最相似的关键帧编号所对应的关键帧，将其确定为潜在关键帧，若潜在关键帧的特征向量与当前新增特征向量的汉明距离小于第三距离阈值，则认为所述潜在关键帧为所述当前新增关键帧所对应的候选关键帧；

若存在候选关键帧，则生成大关键帧，其中，大关键帧为所述候选关键帧，以及在检测时间上与候选关键帧相邻的若干帧点云数据组成的关键帧；

计算所述当前新增关键帧与大关键帧之间的相对位姿，变换当前新增关键帧的坐标，以使得所述当前新增关键帧与所述大关键帧的坐标坐标原点相一致；遍历所述当前新增关键帧的每一点，计算每一点距离其最近点的欧氏距离，若欧式距离小于第四距离阈值，则认为匹配有效；

当完成遍历时，将所有认定为匹配有效的点所对应的欧氏距离求平均值，若求得的平均值小于第四距离阈值，则认为检测出回环，认为所述当前新增关键帧为回环帧，所述回环帧所对应的候选关键帧组内的所有候选关键帧为相邻帧；

当检测出回环后，使用图优化的方式连接所述当前新增关键帧及其对应的候选关键帧，进行非线性位姿优化，使用优化后的位姿更新所述关键帧数据库中的每一关键帧的位姿。

可选的，所述若潜在关键帧的特征向量与当前新增特征向量的汉明距离小于第三距离阈值，则认为所述潜在关键帧为所述当前新增关键帧所对应的候选关键帧，包括：

若潜在关键帧的特征向量与当前新增特征向量的汉明距离小于第三距离阈值，且所述潜在关键帧的关键帧编号与当前新增关键帧的关键帧编号的差值不小于指定差值，且候选关键帧与当前关键帧的相对位置间距不大于指定位置阈值；则认为所述潜在关键帧为所述当前新增关键帧所对应的候选关键帧；其中，所述关键帧编号用于表征对应的关键帧的合成时间，任意关键帧所对应的关键帧编号的差值可用于表征关键帧的合成时间的邻近程度，且差值越小，关键帧越邻近。

第二方面，本申请提供了一种基于机械旋转式三维激光的定位与建图***，包括：

运动畸变校正模块，用于基于运动畸变校正模型，对激光点云数据执行畸变校正处理；

点云数据预处理模块，用于对畸变校正后的激光点云数据执行预处理操作，所述预处理操作至少包括地面点云分割、角点分割、平面点分割，实时滤除动态点；

关键帧合并维护模块，用于将预处理后的激光点云数据分别合并形成关键帧，并与之前的关键帧进行回环检测，在检测出回环后，对所有的关键帧位姿进行优化处理；

点云地图生成模块，基于关键帧点云和优化后的关键帧位姿，构建并输出整体点云地图。

第三方面，本申请提供了一种基于机械旋转式三维激光的定位与建图装置，包括存储器和处理器，所述存储器上存储有能够被处理器加载并执行如第一方面中任一种方法的计算机程序。

第四方面，本申请提供了一种计算机可读存储介质，存储有能够被处理器加载并执行如第一方面中任一种方法的计算机程序。

综上所述，本申请包括以下有益技术效果：

本申请利用三维激光雷达的机械旋转扫描功能，辅之以轮式里程计的位姿等信息的探测，纠正激光雷达扫描过程中所产生运动失真，有效补偿运动畸变，提高激光雷达运动估计精度，生成全局一致的地图。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本申请实施例的公开的一种基于机械旋转式三维激光的定位与建图方法的流程示意图。

图2是本申请实施例的公开的一种基于机械旋转式三维激光的定位与建图的结构框图。

附图标记说明：1、运动畸变校正模块；2、点云数据预处理模块；3、关键帧合并维护模块；4、点云地图生成模块。

具体实施方式

以下结合附图1-2对本申请作进一步详细说明。

本申请实施例公开一种基于机械旋转式三维激光的定位与建图方法（下文简称为定位与建图方法），其应用于移动机器人的运动定位和地图构建，具体利用搭载于移动机器人上的激光雷达的机械旋转扫描功能，来构建并呈现移动机器人所处环境的地图信息，以及移动机器人的位置信息，且在前述功能的基础上，本申请还能够采用轮式里程计等外部传感器辅助纠正激光雷达扫描过程中所产生的信息误差，提高定位地图构建过程的实现精准性。具体的，本申请公开的定位与建图方法的执行主体为于机械旋转式三维激光的定位与建图***（下文简称为SLAM***），SLAM***通信连接于激光雷达和轮式里程计，从而得以获取激光雷达所检测得到的激光点云数据，以及轮式里程计所检测得到的位姿信息。

相应的，下文将结合图1具体阐述SLAM***执行该定位与建图方法的详细步骤。

S101，基于运动畸变校正模型，对激光点云数据执行畸变校正处理。

具体的，S101包括如下子步骤：

S1011，每当获取到一帧点云数据时，基于预构建的轮式里程计插值模型，计算当前帧点云数据的位姿，以及前一帧点云数据的位姿/>；其中，前一帧点云数据是指：从对应的激光雷达扫描时间维度，在当前帧点云数据之前的一帧帧点云数据；

S1012，计算得出当前帧点云数据相对前一帧点云数据的相对位姿；

S1013，遍历当前帧点云数据的每一点，根据每一点所在Z方向投影坐标，计算得出每一点所对应的水平方向角度，将当前帧点云数据中的最后一个点的水平角度定义为/>；计算得出每一点所对应生成时间/>，以及插值比例/>；其中，第/>个点所在Z方向投影坐标可以表示为/>，第/>个点所对应的水平方向角度/>；/>；；

S1014，根据当前帧点云数据中的每一点所对应的生成时间，以及插值比例/>，通过预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿，根据位姿计算得到当前帧每一点的坐标数据，完成畸变校正。

在实施中，首先获取激光点云数据，激光点云数据由激光雷达按照预设频率，并做机械旋转动作时扫描周围环境得到的、以点云形式存在的数据，且按照扫描时间的不同，且每当激光雷达完成一次旋转扫描操作时，则获得一帧点云数据，且每一帧点云数据包含多个点，每一点即为激光雷达在旋转过程中，每发射一束激光束所对应的位置；而点云数据的具体定义及其所包含的具体内容均为现有技术，在此不做赘述。

本申请实施例限定激光雷达是水平于地面安装，当前帧点云数据的位姿为当前帧点云的坐标原点处的位姿，同理，前一帧点云数据的位姿/>为前一帧点云的坐标原点处的位置，前述两个位姿信息将可通过预构建的运动畸变校正模型计算得出，运动畸变校正模型具体为轮式里程计插值器（其原理将在下文具体阐述）。

使用轮式里程计插值器计算处前一帧时的轮式里程计位姿和当前帧的轮式里程计位姿/>，此时可以得到两帧之间的相对位姿/>。遍历当前帧点云的每一个点，根据每一点在Z方向投影坐标算的每一点对应的水平方向角度/>，其中最后一个点的水平角度记为/>；根据/>和/>的比值可以计算出点所属帧的起始点到该点所用的时间；如点/>是第一帧的第/>个点，且本申请默认当激光雷达初次扫描时，扫描得出的点云的第一个点的时间已知，那么即可结合/>计算得出激光雷达在扫描第/>个点时的时间，且由于后续第二帧、第m帧的第一个点的时间均可基于/>，以及前一帧的一个点的时间测算出来，因此，可以此计算处每一帧每一点的时间。然后SLAM***将相对位姿/>的旋转矩阵转换为欧拉角，按照所用时间所占比例/>进行插值，位移大小也按照同样的比例/>进行插值，且保持Z方向的坐标不便，就可以计算出每一个点实际所在的坐标，实现校正。

可选的，前文提及的运动畸变校正模型的工作原理如下：

获取轮式里程计检测得出的里程计数据，并按检测时间先后顺序，将里程计数据顺次从预设队列的尾部***预设队列中；其中，每一里程计数据均包括数据时间戳和位姿信息；

当接收到插值时间戳时，预构建的轮式里程计差值模型将预设队列中的里程计数据从队首逐一删除；且每删除一个里程计数据时，暂存被删除的里程计数据的数据时间戳/>和位姿信息/>，并判定是否满足预设的插值条件，若是，则停止删除，并执行插值操作；其中，预设的差值条件为：预设队列中仅剩1个里程计数据或预设队列队首的里程计数据所对应的数据时间戳/>大于插值时间戳/>；

其中，步骤“执行插值操作”，包括：

若插值时间戳满足第一预设条件，则确定并输出插值时间戳/>所对应的位姿信息；其中，第一预设条件为：插值时间戳/>与/>的时间差值小于第一预设差值；

若插值时间戳满足第二预设条件，则确定并输出/>；其中，第二预设条件为：插值时间戳/>与/>的时间差值小于第二预设差值；

若插值时间戳不满足第一预设条件和第二预设条件，且在/>与/>之间，则确定并输出/>，其中，/>为/>与/>之间的位姿速度；

若插值时间戳大于/>，且/>与/>的时间间隔小于预设时间阈值，则确定并输出；若/>与/>的时间间隔不小于预设时间阈值，则确定并输出插值时间戳/>所对应的位姿信息/>。

相应的，S1014中的“根据当前帧点云数据中的每一点所对应的生成时间，以及插值比例/>，通过预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿”，具体体现为如下步骤：

根据当前帧点云数据中的每一点所对应生成时间，以及插值比例/>确定每一点所对应的插值时间戳/>，将插值时间戳/>输入预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿。

在实施中，轮式里程计将在激光雷达扫描的同时，同步检测移动机器人的里程计数据，其中，里程计数据至少包括检测时间（即数据时间戳）和对应的位姿信息；SLAM***将实时接收里程计数据，并将其***预设队列中，且***规则为：按照检测时间的先后顺序，顺次从队尾***。当接收到插值时间戳时，运动畸变校正模型将使用如下方法来计算得出插值时间戳/>的位姿，具体的：将预设队列中的里程计数据按照***队列时的***顺序，顺次从队首删除，且每删除一个里程计数据，均会暂存该删除的里程计数据，同时判定是否满足插值条件，若满足，则暂停删除，并执行上文所述的插值操作，来计算得出插值时间戳/>所对应的位姿/>；

其中，若插值时间戳大于/>，且/>与/>的时间间隔小于预设时间阈值（如0.2秒），则确定并输出/>；若/>与/>的时间间隔不小于预设时间阈值（如0.2秒），则认为插值失败，输出插值时间戳/>所对应的位姿信息/>。

S102，对畸变校正后的激光点云数据执行预处理操作，预处理操作至少包括地面点云分割、角点分割、平面点分割，实时滤除动态点。

在实施中，本申请示例性地给出了如下方案来实现地面点云分割、焦点分割、平面点分割和滤除动态点的操作，但是需要说明的是，实现上述预处理操作的方式不仅仅局限于本申请实施例所公开的方式，其他可实现该预处理操作的技术均在本申请的保护范围内。

对于地面点云分割处理，本申请通过机械旋转式3维激光雷达水平方向角度相同的纵向点云曲率初步筛选地面点云，并降采样为稀疏点云，通过随机样本一致性算法提取更加准确的地面平面点云。

对于角点分割和平面点分割：通过对3维激光雷达每根激光线束所在的点云，分别计算曲率，曲率大于预设经验值的为角点，不大于预设经验值的为平面点。

对于滤除动态点：在非狭小室内环境下（如园区、宽阔车间等环境），认为只要激光雷达能照射到地面平面，则之后在该处上方出现的点云均为动态点，具体的判定方式为：使用上文提取得出的地面平面点云维护整体地面栅格，并将输入的激光点云数据投影到整体地面栅格，若整体地面栅格被占用则认为该点为动态点，否则为静态点。

在分割得出角点和平面点，并对其进行动态点剔除之后，此处需要注意的是，分割出的地面点需要作为平面点加入剔除动态点之后的平面点云中。

此外，本申请将构建激光里程来用于计算位姿；具体的，对于激光雷达第一帧点云，设置位姿为，然后第一帧时所处位置为中心画立方体，保留立方体中的点云，形成局部点云地图，局部点云地图包括角点局部点云地图和平面点局部点云地图，两者分别维护，需要注意的是由于角点数量较少且角点在定位中贡献相较于平面点更大，通常角点体素降采样分辨率要小于平面点体素降采样分辨率。然后将后面每一帧的剔除动态点角点与角点局部点云地图进行匹配，剔除动态点的平面点与平面点局部点云地图进行匹配，再联合计算出激光里程计的位姿结果，并更新局部点云地图，从而得以获知每一帧时的局部点云地图和帧内每一点在局部点云地图中的位姿信息。

S103，将预处理后的激光点云数据分别合并形成关键帧，并与之前的关键帧进行回环检测，在检测出回环后，对所有的关键帧位姿进行优化处理。

其中，S103中有关形成关键帧的步骤的具体实现内容如下：

S1041，每当对激光点云数据完成预处理操作之后，判定当前关键帧是否为空；

S1042，若是，则将当前完成预处理操作的激光点云数据作为第一帧点云数据***关键帧，并将第一帧点云数据作为当前关键帧的参考帧；计算第一帧点云数据中的角点点云的点云重心和平面点点云的点云重心/>；其中，/>，，/>为第一帧点云数据中的角点数量，/>为第一帧点云数据中的平面点数量，/>为第一帧点云数据所对应的角点点云中的第/>个角点的坐标，/>为第一帧点云数据所对应的平面点点云中的第/>个平面点的坐标；

S1043，若否，则计算并判定当前完成预处理操作的当前帧点云数据是否满足第一判定条件或第二判定条件，再根据判定结果执行预设处理操作；

其中，根据判定结果执行预设处理操作，包括：

若满足第一判定条件，则将当前帧点云数据所对应的位姿变换到参考帧坐标系，将当前帧点云数据与参考帧进行点云叠加并降采样形成新的参考帧，更新新的参考帧的、/>；

若满足第二判定条件，则将此时的关键帧打包并存入预设的关键帧数据库中，清空关键帧，将当前帧点云数据***清空后的关键帧，并用当前帧点云数据替换当前参考帧；其中，关键帧数据库用于存储每次打包形成的关键帧；

其中，第一判定条件为：第n帧点云数据的相对参考帧的/>、第n帧点云数据的/>相对参考帧的/>的距离值之和小于第一距离阈值；第二判定条件为：第n帧点云数据的/>相对参考帧的/>、第n帧点云数据的/>相对参考帧的/>的距离值之和不小于第一距离阈值，或第n帧点云数据的位姿坐标参考帧位姿坐标的距离大于第二距离阈值，或第n帧激光点云数据的旋转角度大于指定角度阈值。

在实施中，对于完成预处理操作的点云数据来说，将利用上述操作来合并形成关键帧。具体的，在程序刚启动时，关键帧为空；由于点云数据是由激光雷达按照指定频率生成的以帧为单位的离散数据，因此， SLAM***每接收到一帧点云数据，就会对该点云数据执行一次去运动畸变和预处理操作，每当对一帧点云数据完成预处理操作时，则先判定此时的关键帧是否为空，若为空，则将当前完成预处理操作的点云数据***关键帧，也就是说，此时的点云数据为***关键帧的第一帧点云数据，其将被作为参考帧，而参考帧主要用于作为与后续其他帧数据进行比对来判定是否符合第一判定条件或第二判定条件的基准。相应的，SLAM***将计算角点点云的点云重心和平面点点云的点云重心/>。

此时关键帧非空，那么当接收到第二个完成预处理的帧点云数据时，由于此时关键帧非空，SLAM将对该帧点云数据执行S1043步骤，即，计算将该帧点云数据的和/>，并判定是否满足第一判定条件或第二判定条件，若满足第一判定条件，那么SLAM***将该帧点云数据的位姿变换到参考帧坐标系，将该帧点云与当前参考帧的点云进行点云叠加并降采样处理，且此时由于点云叠加，因此，需要根据点云叠加后的参考帧，重新计算并更新参考帧的/>和/>。当接收到第三帧点云数据时，将再次执行S1041、S1042、S1043的步骤。

而若上述第二帧点云数据满足的是第二判定条件，则将此时关键帧打包至预设的关键帧数据库内，即，将当前关键帧内***的所有点云数据合并形成的关键帧保存至预设的关键帧数据库内，再清空当前关键帧，将第二帧点云数据***清空后的关键帧，同时用第二帧点云数据替换参考帧，实现参考帧的更新，相应的参考帧的和/>也更新为该第二帧点云数据的/>和/>。由此可知，参考帧是每次关键帧由空转换为非空时，***关键帧的第一帧点云数据。当完成上述操作之后，后续再接收到新的完成预处理的帧点云数据时，将再次执行S1041- S1043的步骤，即每当接收到完成预处理的帧点云数据时，均会执行一次S1041- S1043的步骤。

优选的，本申请还提出了同时维护两个并行的关键帧的具体维护方式，相应步骤如下：

S1044，每当对激光点云数据完成预处理操作后，将完成预处理操作后的当前帧点云数据***副关键帧，并对主关键帧执行S1041、S1042、S1043的步骤内容；

S1045，实时判定主关键帧是否满足第一判定条件和第二判定条件，再根据判定结果执行预设处理操作；

S1046，当主关键帧满足第二判定条件并执行预设处理操作中的打包操作时，即主关键帧被打包并存入预设的关键帧数据库中时，若此次打包为首次打包，则记录当前主关键帧被打包时的位姿坐标M、确定当前主关键帧位姿坐标M与参考帧位姿坐标的相对距离；若此次打包非首次打包，则确定当前主关键帧位姿、主关键帧位姿与前一次打包记录的主关键帧位姿坐标M的相对距离，用当前主关键帧位姿更新M，用当前确定的相对距离更新/>；

S1047，当副关键帧的位姿坐标距离坐标M的相对距离达到时，将副关键帧打包并保存至关键帧数据库中；

S1048，在每次确定得出相对距离之后，若主关键帧在副关键帧的位姿坐标与M的相对距离达到/>之前，执行预处理操作中的打包操作，此时则将副关键帧和主关键帧一同打包并保存到关键帧数据库中；

S1049，每当主关键帧被打包后，清空主关键帧；每当副关键帧被打包后，清空副关键帧。

在实施中，该方案和上述方案的不同点在于新增了一个副关键帧，以使得关键帧由一个变为两个；而此处的主关键帧可被认为是上文S1041- S1043步骤中提及的关键帧，对于主关键帧来说，主关键帧将执行S1041- S1043的步骤来判定主关键帧是否满足打包条件。对于副关键帧来说，始终执行S1044来将完成预处理操作的激光点云数据***副关键帧内；

具体的，在程序启动初始，主关键帧和副关键帧均为空置状态，此时结合S1041和S1044可知，对于完成预处理操作的帧点云数据，将被同时***主关键帧和副关键帧中。而对于主关键帧来说，每当SLAM***获取到完成预处理操作的帧点云数据时，将执行上文S1041- S1043相同的步骤，以判定主关键帧是否符合打包条件（即第二判定条件），并在符合时执行打包操作，在此过程中，每次获取到的完成预处理操作的帧点云数据，将持续被***副关键帧内。

对于S1046，若在程序首次启动之后，主关键帧第一次满足打包条件并被打包至关键帧数据库时，SLAM***将记录当前主关键帧打包时当前帧（当前帧即为S1043中提及的满足第二判定条件的当前完成预处理操作的当前帧点云数据）的位姿坐标，记为M，以及当前帧与当前参考帧的在坐标上的相对距离，M和/>可由前文所述的激光里程计计算得出，

若主关键帧第二次或第（/>）次满足打包条件并被打包至关键帧数据库时，SLAM***将记录当前主关键帧打包时当前帧的位姿坐标，以及该位姿坐标与主关键帧上一次打包时的位姿坐标（即M）的相对距离，此次记录的位姿坐标更新M，用此次确定的相对激励更新/>。

在主关键帧被打包时，副关键帧始终保持被***完成预处理操作的帧点云数据的状态。而S1047和S1048说明了副关键帧被打包的条件：对于持续被***帧点云数据的副关键帧，在主关键帧每次完成打包之后，副关键帧相对M的距离达到，那么SLAM将副关键帧打包并保存到关键帧数据库中；

但若主关键帧每次打包之后，在副关键帧相对M的距离尚且还未达到时，主关键帧又实现了第二次打包，那么此时副关键帧与主关键帧将同时被打包并存储到关键帧数据库中，即强制对副关键帧打包一次，此时结合S1046可知，SLAM***将记录此次主关键帧打包时的位姿坐标，记录该位姿坐标与主关键帧上一次打包时所记录的位姿坐标的相对距离，用该位姿坐标更新M，用该相对距离更新/>，再重复执行S1047和S1048，用更新后的M和/>，再次实时判定副关键帧相对M的距离是否达到/>；直至程序结束。

由上可知，主关键帧打包的条件为：在执行S1041- S1043步骤时，满足第二判定条件。而副关键帧打包的条件为二：其一为主关键帧每次打包之后，副关键帧相对M的距离达到的时候；其二是：主关键帧连续两次打包期间（即每次打包之后，至下一次打包期间），若该期间内，副关键帧相对M的距离始终未达到/>，则强制打包；此处需要说明的是，每次主关键帧被打包时，在执行S1046之后，将清空主关键帧，再根据S1041- S1043重新***后续完成预处理的激光点云数据、重新确定参考帧；每次副关键帧被打包之后，将清空副关键帧，并重新按照S1044***后续完成预处理的激光点云数据。

其中，S103中有关回环检测的具体实施步骤如下：

S1141，每当关键帧数据库中新增关键帧时，计算当前新增关键帧的特征向量，将当前新增关键帧的特征向量存储于预设的回环检测模块中；

S1142，基于预建立的k-d树，确定与当前新增关键帧的特征向量最相似的关键帧编号所对应的关键帧，将其确定为潜在关键帧，若潜在关键帧的特征向量与当前新增特征向量的汉明距离小于第三距离阈值，且潜在关键帧的关键帧编号与当前新增关键帧的关键帧编号的差值不小于指定差值，且候选关键帧与当前关键帧的相对位置间距不大于指定位置阈值；则认为潜在关键帧为当前新增关键帧所对应的候选关键帧；其中，关键帧编号用于表征对应的关键帧的合成时间，任意关键帧所对应的关键帧编号的差值可用于表征关键帧的合成时间的邻近程度，且差值越小，关键帧越邻近；

S1143，若存在候选关键帧，则生成大关键帧，其中，大关键帧为候选关键帧，以及在检测时间上与候选关键帧相邻的若干帧点云数据组成的关键帧；

S1144，计算当前新增关键帧与大关键帧之间的相对位姿，变换当前新增关键帧的坐标，以使得当前新增关键帧与大关键帧的坐标坐标原点相一致；遍历当前新增关键帧的每一点，计算每一点距离其最近点的欧氏距离，若欧式距离小于第四距离阈值，则认为匹配有效；

S1145，当完成遍历时，将所有认定为匹配有效的点所对应的欧氏距离求平均值，若求得的平均值小于第四距离阈值，则认为检测出回环，认为当前新增关键帧为回环帧，回环帧所对应的候选关键帧组内的所有候选关键帧为相邻帧；

S1146，当检测出回环后，使用图优化的方式连接当前新增关键帧及其对应的候选关键帧，进行非线性位姿优化，使用优化后的位姿更新关键帧数据库中的每一关键帧的位姿。

在实施中，关键帧数据库则用于存储每次打包的关键帧，且关键帧数据库中的关键帧至少包括用于区分其他关键帧的关键帧编号，关键帧编号可用于表征对应的关键帧的合成时间，或关键帧被***关键帧数据库的时间，任意关键帧所对应的关键帧编号的差值可用于表征关键帧的合成时间的邻近程度，且差值越小，关键帧越邻近。

每当关键帧数据库内新增关键帧时，将执行上文S1141- S1146的回环检测步骤，首先，该新增关键帧在被保存至关键帧数据库中的同时，将使用点云的分布特征计算该新增关键帧的特征向量，并将特征向量保存到预设的回环检测模块中，并将所有的特征向量根据向量内容***到预建立的k-d树中。通过查询k-d树可以从当前关键帧数据库中快速找到和当前特征向量最相似（即特征向量中元素重合度最高）的关键帧编号，及其该最相似的关键帧编号所对应的关键帧（即S1142所提及的潜在关键帧）；

接着，将判断潜在关键帧是否为新增关键帧所对应的候选关键帧，经过上述筛选后，若存在候选关键帧，则先确定大关键帧，大关键帧是指由候选关键帧和在检测时间上与候选关键帧相邻的若干帧点云数据（如候选关键帧前两帧和后两帧）组成的关键帧；再使用点到平面的ICP匹配算法来计算新增关键帧和大关键帧之间的相对位姿。将新增关键帧坐标变换大关键帧之后通过遍历当前新增关键帧的每一个点，通过KNN算法计算每个点到最近点之间的欧式距离，若欧氏距离小于第四距离阈值（如1米）则认为匹配有效。再将所有匹配有效的点之间的距离求平均值，若平均距离小于第四距离阈值阈值，则认为匹配度较高，是真回环，认为发生回环；否则认为是假回环，不认为发生回环。

当认为发生回环后，使用图优化的方式连接当前关键帧和候选关键帧，进行非线性位姿优化。最后使用图优化的位姿结果更新关键帧数据库中每个关键帧的位姿。

S104，基于关键帧点云和优化后的关键帧，构建并输出整体点云地图。

在实施中，本申请实施例示例性地给出了如下用于构建整体点云地图的方法，但实现该功能的方法不限于本申请的示例。具体的，SLAM***遍历整个关键帧数据库，通过关键帧的位姿将关键帧点云位姿变换到全局坐标系，然后按照关键帧编号对每个点云进行叠加和降采样，最终得到整体点云地图。在其他实施例中，可以先将所有点云叠加，在使用八叉树的方式对整体点云分割成小块区域，然后对每隔分割后的小块区域进行降采样，由此可以进一步提高计算效率。

本申请实施例还公开一种基于机械旋转式三维激光的定位与建图***。参照图2，包括：

运动畸变校正模块1，用于基于运动畸变校正模型，对激光点云数据执行畸变校正处理；

点云数据预处理模块2，用于对畸变校正后的激光点云数据执行预处理操作，预处理操作至少包括地面点云分割、角点分割、平面点分割，实时滤除动态点；

关键帧合并维护模块3，用于将预处理后的激光点云数据分别合并形成关键帧，并与之前的关键帧进行回环检测，在检测出回环后，对所有的关键帧位姿进行优化处理；

点云地图生成模块4，基于关键帧点云和优化后的关键帧位姿，构建并输出整体点云地图。

可选的，运动畸变校正模块1用于每当获取到一帧点云数据时，基于预构建的轮式里程计插值模型，计算当前帧点云数据的位姿，以及前一帧点云数据的位姿/>；其中，前一帧点云数据是指：从对应的激光雷达扫描时间维度，在当前帧点云数据之前的一帧帧点云数据；计算得出当前帧点云数据相对前一帧点云数据的相对位姿/>；遍历当前帧点云数据的每一点，根据每一点所在Z方向投影坐标，计算得出每一点所对应的水平方向角度/>，将当前帧点云数据中的最后一个点的水平角度定义为/>；计算得出每一点所对应生成时间/>，以及插值比例/>；根据当前帧点云数据中的每一点所对应生成时间/>，以及插值比例/>，通过预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿，根据位姿计算得到当前帧每一点的坐标数据，完成畸变校正。

可选的，还包括里程计插值模块5，用于获取轮式里程计检测得出的里程计数据，并按检测时间先后顺序，将里程计数据顺次从预设队列的尾部***预设队列中；当接收到插值时间戳时，预构建的轮式里程计差值模型将预设队列中的里程计数据从队首逐一删除；且每删除一个里程计数据时，暂存被删除的里程计数据的数据时间戳/>和位姿信息/>，并判定是否满足预设的插值条件，若是，则停止删除，并执行插值操作；

还用于若插值时间戳满足第一预设条件，则确定并输出插值时间戳/>所对应的位姿信息/>；其中，第一预设条件为：插值时间戳/>与/>的时间差值小于第一预设差值；若插值时间戳/>满足第二预设条件，则确定并输出/>；其中，第二预设条件为：插值时间戳与/>的时间差值小于第二预设差值；若插值时间戳/>不满足第一预设条件和第二预设条件，且在/>与/>之间，则确定并输出/>；若插值时间戳/>大于/>，且/>与/>的时间间隔小于预设时间阈值，则确定并输出/>；若/>与/>的时间间隔不小于预设时间阈值，则确定并输出插值时间戳/>所对应的位姿信息/>；

运动畸变校正模块1还用于根据当前帧点云数据中的每一点所对应的生成时间，以及插值比例/>，通过预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿，根据位姿计算得到当前帧每一点的坐标数据，完成畸变校正。

可选的，关键帧合并维护模块3用于每当对激光点云数据完成预处理操作之后，判定当前关键帧是否为空；若是，则将当前完成预处理操作的激光点云数据作为第一帧点云数据***关键帧，并将第一帧点云数据作为当前关键帧的参考帧；计算第一帧点云数据中的角点点云的点云重心和平面点点云的点云重心/>；

还用于若满足第一判定条件，则将当前帧点云数据所对应的位姿变换到参考帧坐标系，将当前帧点云数据与参考帧进行点云叠加并降采样形成新的参考帧，更新新的参考帧的、/>；若满足第二判定条件，则将此时的关键帧打包并存入预设的关键帧数据库中，清空关键帧，将当前帧点云数据***清空后的关键帧，并用当前帧点云数据替换当前参考帧；其中，关键帧数据库用于存储每次打包形成的关键帧。

可选的，关键帧合并维护模块3还用于每当对激光点云数据完成预处理操作后，将完成预处理操作后的当前帧点云数据***副关键帧，并对主关键帧执行S1041、S1042、S1043的步骤内容；还用于当主关键帧满足第二判定条件并执行预设处理操作中的打包操作时，即主关键帧被打包并存入预设的关键帧数据库中时，若此次打包为首次打包，则记录当前主关键帧被打包时的位姿坐标M、确定当前主关键帧位姿坐标M与参考帧位姿坐标的相对距离；若此次打包非首次打包，则确定当前主关键帧位姿、主关键帧位姿与前一次打包记录的主关键帧位姿坐标M的相对距离，用当前主关键帧位姿更新M，用当前确定的相对距离更新/>；还用于当副关键帧的位姿坐标距离坐标M的相对距离达到/>时，将副关键帧打包并保存至关键帧数据库中；在每次确定得出相对距离/>之后，若主关键帧在副关键帧的位姿坐标与M的相对距离达到/>之前，执行预处理操作中的打包操作，此时则将副关键帧和主关键帧一同打包并保存到关键帧数据库中；每当主关键帧被打包后，清空主关键帧；每当副关键帧被打包后，清空副关键帧。

可选的，关键帧合并维护模块3还用于每当关键帧数据库中新增关键帧时，计算当前新增关键帧的特征向量，将当前新增关键帧的特征向量存储于预设的回环检测模块中；还用于基于预建立的k-d树，确定与当前新增关键帧的特征向量最相似的关键帧编号所对应的关键帧，将其确定为潜在关键帧，若潜在关键帧的特征向量与当前新增特征向量的汉明距离小于第三距离阈值，则认为潜在关键帧为当前新增关键帧所对应的候选关键帧；

关键帧合并维护模块3还用于若存在候选关键帧，则生成大关键帧，其中，大关键帧为候选关键帧，以及在检测时间上与候选关键帧相邻的若干帧点云数据组成的关键帧；还用于计算当前新增关键帧与大关键帧之间的相对位姿，变换当前新增关键帧的坐标，以使得当前新增关键帧与大关键帧的坐标坐标原点相一致；遍历当前新增关键帧的每一点，计算每一点距离其最近点的欧氏距离，若欧式距离小于第四距离阈值，则认为匹配有效；

关键帧合并维护模块3还用于当完成遍历时，将所有认定为匹配有效的点所对应的欧氏距离求平均值，若求得的平均值小于第四距离阈值，则认为检测出回环，认为当前新增关键帧为回环帧，回环帧所对应的候选关键帧组内的所有候选关键帧为相邻帧；还用于当检测出回环时，使用图优化的方式连接当前新增关键帧和及其对应的候选关键帧，进行非线性位姿优化，使用优化后的位姿更新关键帧数据库中的每一关键帧的位姿。

可选的，关键帧合并维护模块3还用于若潜在关键帧的特征向量与当前新增特征向量的汉明距离小于第三距离阈值，且潜在关键帧的关键帧编号与当前新增关键帧的关键帧编号的差值不小于指定差值，且候选关键帧与当前关键帧的相对位置间距不大于指定位置阈值；则认为潜在关键帧为当前新增关键帧所对应的候选关键帧。

本申请实施例还公开一种基于机械旋转式三维激光的定位与建图装置，基于机械旋转式三维激光的定位与建图装置包括存储器和处理器，存储器上存储有能够被处理器加载并执行如上述的基于机械旋转式三维激光的定位与建图方法的计算机程序。

本申请实施例还公开一种计算机可读存储介质，其存储有能够被处理器加载并执行如上述的基于机械旋转式三维激光的定位与建图方法的计算机程序，该计算机可读存储介质例如包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。

以上实施例仅用以说明本申请的技术方案，而非对申请的保护范围进行限制。显然，所描述的实施例仅仅是本申请部分实施例，而不是全部实施例。基于这些实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请所要保护的范围。

Claims (10)

1.一种基于机械旋转式三维激光的定位与建图方法，其特征在于，包括：

基于运动畸变校正模型，对激光点云数据执行畸变校正处理；

对畸变校正后的激光点云数据执行预处理操作，所述预处理操作至少包括地面点云分割、角点分割、平面点分割，实时滤除动态点；

将预处理后的激光点云数据分别合并形成关键帧，并与之前的关键帧进行回环检测，在检测出回环后，对所有的关键帧位姿进行优化处理；

基于关键帧点云和优化后的关键帧，构建并输出整体点云地图。

2.根据权利要求1所述的基于机械旋转式三维激光的定位与建图方法，其特征在于，所述激光点云数据由预设的激光雷达按照预设频率扫描获得，且每一次激光雷达扫描获得的激光点云数据为一帧点云数据；

所述基于运动畸变校正模型，对激光点云数据执行畸变校正处理，包括：

每当获取到一帧点云数据时，基于预构建的轮式里程计插值模型，计算当前帧点云数据的位姿，以及前一帧点云数据的位姿/>；其中，所述前一帧点云数据是指：从对应的激光雷达扫描时间维度，在当前帧点云数据之前的一帧的帧点云数据；

计算得出所述当前帧点云数据相对所述前一帧点云数据的相对位姿；

遍历所述当前帧点云数据的每一点，根据每一点所在Z方向投影坐标，计算得出每一点所对应的水平方向角度，将所述当前帧点云数据中的最后一个点的水平角度定义为/>；计算得出每一点所对应生成时间/>，以及插值比例/>；其中，第/>个点所在Z方向投影坐标可以表示为/>，第/>个点所对应的水平方向角度/>；/>；；

根据所述当前帧点云数据中的每一点所对应的生成时间，以及插值比例/>，通过预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿，根据位姿计算得到当前帧每一点的坐标数据，完成畸变校正。

3.根据权利要求2所述的基于机械旋转式三维激光的定位与建图方法，其特征在于，所述方法还包括：

获取轮式里程计检测得出的里程计数据，并按检测时间先后顺序，将里程计数据顺次从预设队列的尾部***所述预设队列中；其中，每一所述里程计数据均包括数据时间戳和位姿信息；

当接收到插值时间戳时，所述预构建的轮式里程计差值模型将所述预设队列中的里程计数据从队首逐一删除；且每删除一个里程计数据时，暂存被删除的里程计数据的数据时间戳/>和位姿信息/>，并判定是否满足预设的插值条件，若是，则停止删除，并执行插值操作；其中，所述预设的差值条件为：预设队列中仅剩1个里程计数据或预设队列队首的里程计数据所对应的数据时间戳/>大于所述插值时间戳/>；

其中，所述执行插值操作，包括：

若所述插值时间戳满足第一预设条件，则确定并输出所述插值时间戳/>所对应的位姿信息/>；其中，所述第一预设条件为：所述插值时间戳/>与所述/>的时间差值小于第一预设差值；

若所述插值时间戳满足第二预设条件，则确定并输出/>；其中，所述第二预设条件为：所述插值时间戳/>与所述/>的时间差值小于第二预设差值；

若所述插值时间戳不满足第一预设条件和第二预设条件，且在/>与/>之间，则确定并输出/>，其中，/>为所述/>与/>之间的位姿速度；

若所述插值时间戳大于/>，且/>与/>的时间间隔小于预设时间阈值，则确定并输出；若/>与/>的时间间隔不小于预设时间阈值，则确定并输出所述插值时间戳/>所对应的位姿信息/>；

所述根据所述当前帧点云数据中的每一点所对应的生成时间，以及插值比例/>，通过预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿，包括：

根据所述当前帧点云数据中的每一点所对应的生成时间，以及插值比例/>确定每一点所对应的插值时间戳/>，将插值时间戳/>输入预构建的运动畸变校正模型，输出当前帧点云数据中的每一点所对应的位姿。

4.根据权利要求2所述的基于机械旋转式三维激光的定位与建图方法，其特征在于，所述预处理操作还包括降采样处理；所述将预处理后的激光点云数据分别合并形成关键帧，包括：

S1041，每当对激光点云数据完成预处理操作之后，判定当前关键帧是否为空；

S1042，若是，则将当前完成预处理操作的激光点云数据作为第一帧点云数据***关键帧，并将所述所述第一帧点云数据作为当前关键帧的参考帧；计算所述第一帧点云数据中的角点点云的点云重心和平面点点云的点云重心/>；其中，/>，，/>为所述第一帧点云数据中的角点数量，/>为所述第一帧点云数据中的平面点数量，/>为所述第一帧点云数据所对应的角点点云中的第/>个角点的坐标，/>为所述第一帧点云数据所对应的平面点点云中的第/>个平面点的坐标；

S1043，若否，则计算并判定当前完成预处理操作的当前帧点云数据是否满足第一判定条件或第二判定条件，再根据判定结果执行预设处理操作；

其中，所述根据判定结果执行预设处理操作，包括：

若满足第一判定条件，则将当前帧点云数据所对应的位姿变换到参考帧坐标系，将所述当前帧点云数据与参考帧进行点云叠加并降采样形成新的参考帧，更新新的参考帧的、/>；

若满足第二判定条件，则将此时的关键帧打包并存入预设的关键帧数据库中，清空关键帧，将所述当前帧点云数据***清空后的关键帧，并用所述当前帧点云数据替换当前参考帧；其中，所述关键帧数据库用于存储每次打包形成的关键帧；

其中，所述第一判定条件为：第n帧点云数据的相对参考帧的/>、第n帧点云数据的/>相对参考帧的/>的距离值之和小于第一距离阈值；第二判定条件为：第n帧点云数据的/>相对参考帧的/>、第n帧点云数据的/>相对参考帧的/>的距离值之和不小于第一距离阈值，或第n帧点云数据的位姿坐标参考帧位姿坐标的距离大于第二距离阈值，或第n帧激光点云数据的旋转角度大于指定角度阈值。

5.根据权利要求4所述的基于机械旋转式三维激光的定位与建图方法，其特征在于，所述将预处理后的激光点云数据分别合并形成关键帧，还包括：

每当对激光点云数据完成预处理操作后，将完成预处理操作后的当前帧点云数据***副关键帧，并对主关键帧执行所述S1041、S1042、S1043的步骤内容；

当主关键帧满足第二判定条件并执行所述预设处理操作中的打包操作时，即主关键帧被打包并存入预设的关键帧数据库中时，若此次打包为首次打包，则记录当前主关键帧被打包时的位姿坐标M、确定当前主关键帧位姿坐标M与参考帧位姿坐标的相对距离；若此次打包非首次打包，则确定当前主关键帧位姿、主关键帧位姿与前一次打包记录的主关键帧位姿坐标M的相对距离，用当前主关键帧位姿更新M，用当前确定的相对距离更新/>；

当副关键帧的位姿坐标距离坐标M的相对距离达到时，将副关键帧打包并保存至关键帧数据库中；

在每次确定得出相对距离之后，若主关键帧在副关键帧的位姿坐标与M的相对距离达到/>之前，执行预处理操作中的打包操作，此时则将副关键帧和主关键帧一同打包并保存到关键帧数据库中；

每当主关键帧被打包后，清空主关键帧；每当副关键帧被打包后，清空副关键帧。

6.根据权利要求4所述的基于机械旋转式三维激光的定位与建图方法，其特征在于，所述与之前的关键帧进行回环检测，在检测出回环后，对所有的关键帧位姿进行优化处理，包括：

每当所述关键帧数据库中新增关键帧时，计算当前新增关键帧的特征向量，将当前新增关键帧的特征向量存储于预设的回环检测模块中；

基于预建立的k-d树，确定与所述当前新增关键帧的特征向量最相似的关键帧编号所对应的关键帧，将其确定为潜在关键帧，若潜在关键帧的特征向量与当前新增特征向量的汉明距离小于第三距离阈值，则认为所述潜在关键帧为所述当前新增关键帧所对应的候选关键帧；

若存在候选关键帧，则生成大关键帧，其中，大关键帧为所述候选关键帧，以及在检测时间上与候选关键帧相邻的若干帧点云数据组成的关键帧；

计算所述当前新增关键帧与大关键帧之间的相对位姿，变换当前新增关键帧的坐标，以使得所述当前新增关键帧与所述大关键帧的坐标原点相一致；遍历所述当前新增关键帧的每一点，计算每一点距离其最近点的欧氏距离，若欧式距离小于第四距离阈值，则认为匹配有效；

当完成遍历时，将所有认定为匹配有效的点所对应的欧氏距离求平均值，若求得的平均值小于第四距离阈值，则认为检测出回环，认为所述当前新增关键帧为回环帧，所述回环帧所对应的候选关键帧组内的所有候选关键帧为相邻帧；

当检测出回环后，使用图优化的方式连接所述当前新增关键帧及其对应的候选关键帧，进行非线性位姿优化，使用优化后的位姿更新所述关键帧数据库中的每一关键帧的位姿。

7.根据权利要求6所述的基于机械旋转式三维激光的定位与建图方法，其特征在于，所述若潜在关键帧的特征向量与当前新增特征向量的汉明距离小于第三距离阈值，则认为所述潜在关键帧为所述当前新增关键帧所对应的候选关键帧，包括：

若潜在关键帧的特征向量与当前新增特征向量的汉明距离小于第三距离阈值，且所述潜在关键帧的关键帧编号与当前新增关键帧的关键帧编号的差值不小于指定差值，且候选关键帧与当前关键帧的相对位置间距不大于指定位置阈值；则认为所述潜在关键帧为所述当前新增关键帧所对应的候选关键帧；其中，所述关键帧编号用于表征对应的关键帧的合成时间，任意关键帧所对应的关键帧编号的差值可用于表征关键帧的合成时间的邻近程度，且差值越小，关键帧越邻近。

8.一种基于机械旋转式三维激光的定位与建图***，其特征在于，包括，

运动畸变校正模块（1），用于基于运动畸变校正模型，对激光点云数据执行畸变校正处理；

点云数据预处理模块（2），用于对畸变校正后的激光点云数据执行预处理操作，所述预处理操作至少包括地面点云分割、角点分割、平面点分割，实时滤除动态点；

关键帧合并维护模块（3），用于将预处理后的激光点云数据分别合并形成关键帧，并与之前的关键帧进行回环检测，在检测出回环后，对所有的关键帧位姿进行优化处理；

点云地图生成模块（4），基于关键帧点云和优化后的关键帧位姿，构建并输出整体点云地图。

9.一种基于机械旋转式三维激光的定位与建图装置，其特征在于，包括存储器和处理器，所述存储器上存储有能够被处理器加载并执行如权利要求1至7所述的任一种基于机械旋转式三维激光的定位与建图方法的计算机程序。

10.一种计算机可读存储介质，其特征在于，存储有能够被处理器加载并执行如权利要求1至7所述的任一种基于机械旋转式三维激光的定位与建图方法的计算机程序。