一种激光里程计算方法和地图构建方法与流程

本发明涉及的机器人运动的技术领域，具体涉及一种激光里程计算的方法，还涉及基于该方法的地图构建方法。

背景技术：

机器人在环境中移动时，里程信息是机器人运动状态的最基本信息。传统的机器人的里程计是利用致动器的移动数据来估算机器人位姿随时间改变量的方法。位姿变换需要由可编码器和惯性导航传感器来获得。这种传统的机器人的里程计需要依赖惯性导航传感器的累计信息，因此，里程计的误差也会不断累积，导致精度下降。

因此，针对传统的机器人的里程计所存在的精度问题，本发明实施例提出一种激光里程计算方法，该激光里程计算方法基于具有俯仰电机的二维激光雷达实现实时计算里程，从而有效地避免由于随着时间增长而误差累计的问题，进而提高了里程计的精度。基于所述的激光里程计算方法，本发明实施例还提出一种三维地图的构建方法，从而使得三维地图的构建不再依赖惯性测量单元。

技术实现要素：

针对传统的机器人的里程计所存在的精度问题，本发明实施例提出一种激光里程计算方法。该激光里程计算方法基于具有俯仰电机的二维激光雷达实现实时计算里程，通过三维激光雷达感知的点云来估计机器人平台自身的运动状态，从而有效地避免传统里程计由于随着时间增长而误差累计的问题，进而提高了里程计的精度。

该激光里程计算方法的具体方案如下：一种激光里程计算方法，包括以下步骤：通过激光雷达扫描多次而获得多个不均匀分布的点云；从所述每个点云中提取出具有共面几何关系的特征点；采用估算运动变换算法将不同时刻扫描所获取的点云投影至同一时刻，从而获得投影点云，再依据所述投影点云而获取时空对应的投影特征点；对不同时刻扫描所获取的点云进行线性插值姿态变换；采用非线性最小二乘法计算激光雷达的运动，从而获取激光里程。

优选地，所述激光雷达连接电机，所述电机具有-90度至90度的区间俯仰角。

优选地，所述激光雷达的运动模型以恒定的角速度和恒定的线性速度进行线性插值。

优选地，所述共面几何关系的特征点包括在尖锐边缘的特征点和平面表面的特征点。

优选地，用来评估每次所扫描获得的点云的局部表面平滑度的表达式如式1所示：

式1：

其中，表示第k次扫描期间感知的点云，点i为激光雷达坐标系{lk}中的点，被记作点j为激光雷达坐标系{lk}中的点，被记作是激光雷达在同一扫描中返回的点i的连续点云合，c为局部表面平滑度数值。

优选地，将所述c值对扫描中的点进行排序，以最大的c值作为边缘特征点，以最小的c值作为平面特征点。

优选地，所述非线性最小二乘法包括列文伯格马夸尔特法。

优选地，所述线性插值姿态变换的具体表达式如式4所示：

式4：

其中，是时刻[tk+1，t]之间的激光雷达姿态变换，表示激光雷达的在自由空间中具有6个自由度的刚性运动，其中tx，ty和tz分别是沿激光雷达坐标系{l)的x轴，y轴和z轴的平移，θx，θy和θz是右手规则下的旋转角度；i为给定点，其中ti为其时间戳，为时刻[tk+1，ti]之间的姿态变换。

优选地，对所述式4进行变换可得式5：

式5：

其中，是点云中边缘特征点或平面特征点的坐标，是的第a至第b个数值，r是由罗德里格公式定义的旋转矩阵。

本发明实施例还提供一种地图构建的方法，所述方法基于上述激光里程计算方法，具体包括步骤：采用上述任意一种所述的激光里程计算方法所获取的里程数据，去除相邻点云的失真形变；将去除失真形变的点云投影至同一时刻进行点云匹配，获得准确的变换矩阵，进行里程计的矫正；将所述矫正后的点云从激光雷达坐标系转换为世界坐标系，构建三维地图。

从以上技术方案可以看出，本发明实施例具有以下优点：

本发明实施例中所提供的激光里程计算方法，基于具有俯仰电机的二维激光雷达实现实时计算里程，通过三维激光雷达感知的点云来估计机器人平台自身的运动状态，从而有效地避免传统里程计误差累计的问题，进而提高了里程计的精度。本发明实施例所提供的地图构建方法基于上述所述激光里程计算方法，而不再像传统的地图构建方法需要依赖惯性测量单元。

附图说明

图1为本发明实施例中提供的一种具有俯仰电机的激光雷达装置的示意图；

图2为本发明实施例中提供的一种激光里程计软件系统的示意图；

图3为本发明实施例中提供的一种激光里程计算方法的流程示意图；

图4为本发明实施例中提供的一种时空对应特征点对应的过程示意图；

图5为本发明实施例中提供的一种相邻两组点云的对应边缘特征点和平面特征点投影在同一时刻的位移示意图；

图6为本发明实施例中提供的一种地图构建方法的流程示意图；

图7(a1)为传统里程计算法所构建地图的深度示意图；

图7(a2)为传统里程计算法所构建地图的俯视示意图；

图7(b1)为图6实施例所构建地图的深度示意图；

图7(b2)为图6实施例所构建地图的俯视示意图。

附图标记说明：

100、激光雷达装置10、电机

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

为了更好地阐述本发明实施例，此处先对本发明中相关的变量、使用的平台及相关的软件系统进行介绍。

本发明实施例中提供的里程计算方法是通过三维激光雷达所感知的点云来估计机器人平台自身的运动状态，并基于所得到里程数据为遍历环境构建地图。在本发明实施例中，定义激光雷达的角速度和线速度随着时间的推移是平滑和连续的，不存在突然的变化。在本发明实施例中，坐标系采用右侧上角标的大写字母来表示。扫描定义为激光雷达完成一次扫描，采用右侧下角标k，来表示扫描次数，表示第k次扫描期间感知的点云。在本发明实施例中，定义了如下两个坐标系：(a)激光雷达坐标系{l}，激光雷达坐标系{l}是一个三维坐标系，起源于激光雷达的几何中心。根据右手原则，这x轴指向左侧，y轴指向前方，z轴指向上方。坐标系{lk}中的点i，被记作(b)世界坐标系{w}，世界坐标系{w}是在初始位置与{l}重合的三维坐标系。坐标系{wk}中的点i，被记作经过上面的假设和标记方法，本发明实施例将三维激光雷达的测量问题定义为：给定激光雷达点云的序列，计算每次扫描k中激光雷达的自身运动。

如图1所示，本发明实施例中提供的一种具有俯仰电机的激光雷达装置的示意图。在该实施例中，激光雷达装置100的具体参数如下：具有190°的视野，分辨率为0.667°，且扫描速率为50线/秒。激光雷达装置100连接电机10，电机10能够在-90°至90°之间以50°/s的角速度旋转，其中，定义激光雷达装置100的水平方向为零。激光雷达装置100的扫描是从-90°到90°的旋转或者以相反的方向进行旋转。当机器人平台相对地面静止的时候，激光雷达装置100的扫描平面是半球形。在一具体实施例中，车载编码器以1°的分辨率测量电机旋转角度，激光点投射到激光雷达坐标{l}中。激光雷达坐标{l}的具体坐标系表示如图1所示。在一具体实施例中，激光雷达装置100可基于思科激光测距系统511进行定制成三维激光雷达。当然，激光雷达装置100并不局限于所述的硬件平台。

如图2所示，本发明实施例中提供的一种激光里程计软件系统的流程示意图。激光里程计软件系统流程主要包括：激光雷达及机器人的运动，进行点云采集，采用激光里程计获取里程，分别对里程进行无畸变从而进行点云匹配，对里程进行位姿变换，并结合点云匹配结果进行周期内位姿变换再变换合成，以对激光里程计的误差进行校正。结合具体的点云数据的流程如下：将记作激光扫描中接收到的点。在每次扫描期间，记录在激光雷达坐标系{l}中，并且定义第k次扫描期间形成点云对采用两种算法进行处理：(a)激光雷达里程计使用点云来计算激光雷达在两次连续扫描之间的运动，估计所得的运动用于校正中的失真。该算法以每周期大约10次的频率运行。(b)所得到的输出通过激光雷达地图匹配进一步处理，将以每个周期1次频率跟未失真点云进行匹配。最后，通过两种算法发布的姿态变换被集中处理，每个周期输出约10次的激光雷达的位姿数据。

如图3所示，本发明实施例中提供的一种激光里程计算方法的流程示意图。本发明实施例所提供的一种激光里程计算方法包括五大步骤，具体如下所示。

步骤s1：通过激光雷达扫描多次而获得多个不均匀分布的点云。继续参照图1，采用激光雷达装置100在扫描过程中产生不均匀分布的点云激光雷达装置100的返回值分辨率为0.667°，相当于一次扫描将产生287个点，同时这些点位于扫描平面上。

步骤s2：从所述每个点云中提取出具有共面几何关系的特征点。基于步骤s1中激光雷达装置100的参数，当激光雷达装置100以50°/s的角速度旋转并且以50hz产生扫描时，此时与扫描平面垂直的方向上的分辨率为50/50＝1°。因此，在该实施例中，仅使用每次独立扫描的信息从点云中提取特征点，且所提取的特征点具有共面的几何关系。在该实施例中，共面几何关系的特征点包括在尖锐边缘的特征点和平面表面的特征点。将i记作中的一个点，是激光扫描仪在同一扫描中返回的点i的连续点集合。在该实施例中，用来评估每次所扫描获得的点云的局部表面平滑度的表达式如公式1所示：

其中，表示第k次扫描期间感知的点云，点i为激光雷达坐标系{lk}中的点，被记作点j为激光雷达坐标系{lk}中的点，被记作是激光雷达在同一扫描中返回的点i的连续点云合，c为局部表面平滑度数值。将所述c值对扫描中的点进行排序，以最大的c值作为边缘特征点，以最小的c值作为平面特征点。

步骤s3：采用估算运动变换算法将不同时刻扫描所获取的点云投影至同一时刻，从而获得投影点云，再依据所述投影点云而获取时空对应的投影特征点。

在该实施例中，采用测距算法估计激光雷达装置100在一个扫描周期内的运动。令tk为扫描k的开始时间。在每次扫描结束时，在扫描期间感知到的点云二被重新投射到时间戳tk+1。在该实施例中，将重投影点云表示为在下一次扫描k+1期间，与新接收的点云一起用于估计激光雷达的运动。如图4所示，本发明实施例中提供的一种时空对应特征点对应的过程示意图。其中，虚线线段表示在第k次扫描中获得的点云在第k次扫描结束时，被重新投射到时间戳tk+1以获得(实线线段所示)。然后，在扫描k+1期间，和新感知点云(点划线线段所示)一起用来估计激光雷达运动。

在该实施例中，在k+1扫描期间，定义将新感知的点云投影到扫描开始时刻tk+1记作然后对于中坐标为的一个边缘特征点i，将在中找到距离点i最近的点j坐标，记作并且将点云存储在三维kd树中以便快速索引，令点l为在j点扫描平面相邻的两个连续扫描中距离点i最近的点。三点和形成三角形，距离dε是边缘线的高度。

同理，由上述三点形成三角形推导到四点形成四面体。四个点和形成四面体。点j仍然是中最接近中i的点。与三点的区别在于点l在j所在扫描平面内，而点m在其相邻的连续扫描中平面里，同时都是各自集合中最接近的点i的点，以便确保和三点不共线。距离是由四面体的和所形成平面上的高度。如图5所示，本发明实施例中提供的一种相邻两组点云的对应边缘特征点和平面特征点投影在同一时刻的位移示意图。相应的位移，具体可以使用公式2和公式3进行计算。

步骤s4：对不同时刻扫描所获取的点云进行线性插值姿态变换。在该实施例中，由于采用恒定的角速度和线性速度构建扫描期间激光雷达的运动模型，因此，可以对不同时间内接收的扫描点进行线性插值姿态变换。具体的线性插值姿态变换的计算过程如下所述：

令t为当前时间戳，tk+1是第k+1次扫描开始的时间。令是时刻[tk+1，t]之间的激光雷达姿态变换。描述了自由空间中具有6个自由度的激光雷达的刚性运动，其中tx，ty和tz分别是沿激光雷达坐标系{l)的x轴，y轴和z轴的平移，θx，θy和θz是右手规则下的旋转角度。给定点i，令ti为其时间戳，并且令为时刻[tk+1，ti]之间的姿态变换。通过对的线性插值来计算具体计算表达式如公式4所示：

通过使用公式4中的变换，可以得到：

其中，是中边缘特征点的坐标或平面特征点的坐标。是的第a至第b个数值，r是由罗德里格公式定义的旋转矩阵，具体表达式如公式6所示：

其中，θ是旋转量的大小，具体的表达式如公式7所示：

其中，ω表示旋转方向的单位向量，是ω的偏斜对称矩阵，具体的表达式如公式8所示：

对于点云的每个边缘特征点和平面特征点，可综合公式2至公式8推导得出最终的计算表达式，具体如公式9和公式10所示：

在该实施例中，通过使用从前一部分获得的粗略测距数值进行点云重构，在时间戳tk+1处匹配两个相邻点云，以便获得准确的变换矩阵。

步骤s5：采用非线性最小二乘法计算激光雷达的运动，从而获取激光里程。在具体实施例中，非线性最小二乘法采用列文伯格马夸尔特法(levenberg-marquardt)。将边缘特征点和平面特征点的公式9和公式10进行累加，得到一个非线性函数，具体表达式如公式11所示：

其中，每行f对应于一个特征点，d为该特征点的距离。计算f函数相对于的雅可比矩阵，记作j，其中公式11可以通过非线性迭代求解，

从而将d最小化为零，具体的求解过程如公式12所示：

其中，λ是由列文伯格马夸尔特法(levenberg-marquardt)方法确定的因子。

本发明实施例中所提供的激光里程计算方法，基于具有俯仰电机的二维激光雷达实现实时计算里程，通过三维激光雷达感知的点云来估计机器人平台自身的运动状态，从而有效地避免传统里程计由于随着时间增长而误差累计的问题，进而提高了里程计的精度。

本发明实施例还提供一种基于上述激光里程计算方法的地图构建方法。如图6所示，本发明实施例中提供的一种地图构建方法的流程示意图。该地图构建方法包括三大步骤，具体为：

步骤s11：采用上述的激光里程计算方法所获取的里程数据，去除相邻点云的失真形变。

步骤s22：将去除失真形变的点云投影至同一时刻进行点云匹配，获得准确的变换矩阵，进行里程计的矫正。在一具体实施例中，点云匹配可采用正态分布变换法(normaldistributedtransform)进行匹配，从而获得准确的变换矩阵，完成最终的里程计矫正。

步骤s33：将所述矫正后的点云从激光雷达坐标系转换为世界坐标系，构建三维地图。如图7所示，7(a1)为传统里程计算法所构建地图的深度示意图；7(a2)为传统里程计算法所构建地图的俯视示意图；7(b1)为本发明实施例所构建地图的深度示意图；7(b2)为本发明实施例所构建地图的俯视示意图。通过7(a)和7(b)比对可发现，本发明实施例所构建的地图进行了误差校正，且三维地图的构建不再依赖惯性测量单元。

本发明实施例所提供的地图构建方法基于上述所述激光里程计算方法，而不再像传统的地图构建方法需要依赖惯性测量单元。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。