一种基于二维激光的机器人定位误差评估方法和装置与流程

1.本发明实施例涉及机器人定位技术，尤其涉及一种基于二维激光的机器人定位误差评估方法和装置。


背景技术：

2.随着自动化技术的发展，机器人越来越普遍的出现在了人们的生活和工作中，承担着各类生产生活任务。机器人在执行任务的过程中，其定位精度的高低在一定程度上决定了其执行任务的好坏，为了保证机器人执行任务的质量，需要评估机器人的定位误差。
3.现有技术中通常通过在机器人上设置额外的定位标识装置来进行机器人定位精度的评估，该方法成本高且评估结果受外接环境影响较大，评估结果的准确定不高。


技术实现要素：

4.本发明提供一种基于二维激光的机器人定位误差评估方法和装置，仅通过相邻帧的点云数据即可实现对机器人运行过程中定位能力的实时评估，从而验证机器人定位结果的准确性。
5.第一方面，本发明实施例提供了一种基于二维激光的机器人定位误差评估方法，其特征在于，包括：
6.s1、建立数据存储容器，所述数据存储容器中存储有连续两帧点云数据和连续两帧时刻下的机器人全局位姿；
7.s2、根据当前帧时刻下的机器人全局位姿以及与当前帧相邻的上一帧时刻下的机器人全局位姿，计算当前帧点云转换到上一帧点云的帧间转换矩阵；
8.s3、根据所述帧间转换矩阵对所述当前帧点云进行转换，以得到转换后点云；
9.s4、计算所述转换后点云与所述上一帧点云之间的重合程度，根据所述重合程度对机器人定位误差进行评估。
10.可选的，所述s4包括：
11.根据所述转换后点云与所述上一帧点云计算所述转换后点云中各激光点的定位平移误差和定位旋转误差；
12.对扫描数据进行多区域划分，根据设定的阈值在每个区域中选择固定数量的目标激光点，每个目标激光点权重相同，通过计算目标激光点定位平移误差和定位旋转误差的方差来评估机器人的定位误差。
13.可选的，计算所述转换后点云中各激光点的定位平移误差，包括：
14.以转换后点云中的每个激光点为目标，在所述上一帧点云中搜索满足阈值条件的两个最近点并进行拟合得到最近直线，并计算对应激光点到对应最近直线的目标距离；
15.计算所述转换后点云中每个激光点的法向量信息，通过转换后点云中能够与上一帧点云建立起点与拟合直线关联的有效激光点的方向信息对所述目标距离进行投影分解，以获得各激光点在x和y方向上的分量，并将各激光点在x和y方向上的分量作为转换后点云
中各激光点的定位平移误差。
16.可选的，计算所述转换后点云中每个激光点的法向量信息，包括：
17.搜索转换后点云中各激光点对应的至少两个最近邻点，根据平面内不共线三点确定唯一圆的方式拟合出每个激光点所处的圆；
18.根据空间几何学中圆上任意一点的法向量定义，以及拟合圆所涉及的三点坐标确定各激光点的法向量信息。
19.可选的，计算所述转换后点云中各激光点的定位旋转误差，包括：
20.根据各激光与对应最近直线之间的距离，基于三角几何原理以及微小量近似原则，计算所述转换后点云中各激光点的定位旋转误差。
21.第二方面，本发明实施例还提供一种基于二维激光的机器人定位误差评估装置，包括：
22.存储容器建立模块，用于建立数据存储容器，所述数据存储容器中存储有连续两帧点云数据和连续两帧时刻下的机器人全局位姿；
23.帧间转换矩阵计算模块，用于根据当前帧时刻下的机器人全局位姿以及与当前帧相邻的上一帧时刻下的机器人全局位姿，计算当前帧点云转换到上一帧点云的帧间转换矩阵；
24.点云转换模块，用于根据所述帧间转换矩阵对所述当前帧点云进行转换，以得到转换后点云；
25.定位误差评估模块，用于计算所述转换后点云与所述上一帧点云之间的重合程度，根据所述重合程度对机器人定位误差进行评估。
26.可选的，定位误差评估模块具体用于：
27.根据所述转换后点云与所述上一帧点云计算所述转换后点云中各激光点的定位平移误差和定位旋转误差；
28.对扫描数据进行多区域划分，根据设定的阈值在每个区域中选择固定数量的目标激光点，每个目标激光点权重相同，通过计算目标激光点定位平移误差和定位旋转误差的方差来评估机器人的定位误差。
29.本发明通过计算连续两帧点云之间的帧间转换矩阵，并根据所述帧间转换矩阵对所述当前帧点云进行转换，通过计算所述转换后点云与所述上一帧点云之间的重合程度对机器人定位误差进行实时评估，实现了对机器人运行过程中定位能力的实时评估。同时，本发明能够在多种因素干扰导致机器人定位不准确、定位丢失、位姿漂移等问题场景下及时输出多层级报警信息，如位置偏离、定位失败等。
附图说明
30.图1为本发明实施例提供的一种基于二维激光的机器人定位误差评估的流程图；
31.图2是本发明实施例中的数据容器及处理流程示意图；
32.图3是本发明实施例中的点云法向量计算示意图；
33.图4是本发明实施例中的一帧点云中法向量可视化示意图；
34.图5是本发明实施例中的不同时刻机器人坐标系变换示意图；
35.图6是本发明实施例中的前后帧点云关联示意图；
36.图7是本发明实施例中的点云平移维度上误差计算示意图；
37.图8是本发明实施例中的点云旋转维度上误差计算示意图；
38.图9是本发明实施例中的点云区域划分示意图。
具体实施方式
39.下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。
40.实施例
41.图1为本发明实施例提供的一种基于二维激光的机器人定位误差评估的流程图，具体包括如下步骤：
42.s1、建立数据存储容器，所述数据存储容器中存储有连续两帧点云数据和连续两帧时刻下的机器人全局位姿。
43.s11、数据容器及处理流程
44.本实施例中，首先建立大小为2的数据存储容器，如图2所示，并规定容器中的两个元素分别为front、back。订阅激光点云话题，第一帧点云数据来临时，仅做数据存储的处理，点云数据存入容器中的back位置，记为scanback，并保存第一帧点云时刻机器人位姿poseback。
45.第二帧点云数据来临时，对第一帧点云数据保存位置前移至容器中front位置，记为scanfront，第一帧时刻位姿转换为posefront。第二帧数据保存为scanback，并记录第二帧时刻机器人位姿poseback。
46.此时数据存储容器中数据包含连续两帧点云数据，并且保存有两帧时刻下的机器人全局位姿。对于后续不断订阅获得的激光数据，当前帧的数据总是存储在数据容器中back位置，上一帧数据转移至容器中front位置,不断循环更新容器中的数据。
47.s12、计算点云方向信息
48.本实施例中，计算所述转换后点云中每个激光点的法向量信息，包括：搜索转换后点云中各激光点对应的至少两个最近邻点，根据平面内不共线三点确定唯一圆的方式拟合出每个激光点所处的圆；根据空间几何学中圆上任意一点的法向量定义，以及拟合圆所涉及的三点坐标确定各激光点的法向量信息。
49.具体的，在已有的建图与定位算法中，对于单线激光雷达的数据，主要利用其原始数据中点的障碍物距离信息以及强度信息，对点云结构的利用方案较少，未能挖掘原始激光数据中更为丰富的信息。算法提出一种从单线激光雷达的每一帧扫描数据中提取点云法向量的方法，获得点云内每个点的法向量信息，也即为点的方向信息。
50.为充分利用点云的结构信息，对于点云中每一个激光点搜索至少两个最近邻点，并根据平面内不共线三点确定唯一圆的方式拟合出每个激光点所处的圆。此处所拟合得到的圆上经过三点的圆弧即为每个激光点局部点云结构的准确表达，因此根据空间几何学中圆上任意一点的法向量定义以及拟合圆涉及的三点坐标可以完成对一个激光点的法向量求解，如图3所示，激光点的法向量信息解过程如下：
51.(1)连接待求解点b与搜索得到的两个最近邻点c、d，得到向量并计算向
量的模长
52.(2)过b点做向量的垂线；
53.(3)以圆外方向作为垂线向量的方向，以对应向量模长作为垂线向量的模长比例，获得向量以及
54.(4)根据向量运算原理，将两个垂线向量相加获得b点处的法向量由于算法仅关注法向量的方向，故可直接进行单位化，完成对法向量的模长的统一。
55.上述过程为根据每个激光点以及周围两个最近邻进行拟合圆并求解该点法向量的过程，为保证结果的准确性并排除可能的异常值的干扰，算法增加了冗余计算机制，在最近邻搜索中若满足距离阈值条件的相邻点数量超过了最低要求的两个，同样对这些激光点进行最多数量为5的最近邻数组中，所有的最近邻点均可参与法向量计算的过程，从而提高了每个点的法向量表达的准确性。同样，由于部分杂点在点云中较为稀疏，无法找到最少数量的最近邻点，因此不进行法向量的计算，这也实现了点云的初步过滤。
56.最终，算法获得了每帧点云内激光点法向量信息如图4所示，也即为该点处的方向性信息，用于后续处理。相较于原始的点云仅有每个点的距离以及强度数据，进一步丰富了数据的有效信息。
57.s2、根据当前帧时刻下的机器人全局位姿以及与当前帧相邻的上一帧时刻下的机器人全局位姿，计算当前帧点云转换到上一帧点云的帧间转换矩阵。
58.通过s1中对数据的循环更新操作，每时每刻都能获得当前帧下的点云、机器人全局位姿以及上一帧时刻下的点云与全局位姿的信息。并且，能够在算法需要时获得每帧点云内部点的法向量信息。
59.进一步的，如图5所示，根据两帧时刻的机器人全局位姿求解帧间变换的计算公式如下：
60.t
back_to_front
＝[r
back_to_front
t
back_to_front
]
[0061][0062][0063]
其中r
back_to_front
为当前帧转换到前一时刻的旋转参数，t
back_to_front
为当前帧转换到前一时刻的旋转参数平移参数，所求解得到的变换矩阵是根据机器人定位程序的输出结果计算而来，将点云数据与对应的帧间变换进行关联是进行误差定量分析的基本前提。
[0064]
s3、根据所述帧间转换矩阵对所述当前帧点云进行转换，以得到转换后点云。
[0065]
根据获得的帧间变换矩阵对当前帧点云scanback进行处理，通过旋转、平移变换将其转移至上一帧时刻的机器人坐标系下，获得坐标转换后点云记为scan_transed：
[0066]
scan_transed＝t
back_to_front
*scan_back
ꢀꢀ
(2)
[0067]
对于新生成的点云scan_transed而言，点云内激光点的数量、强度以及各激光点间的相对位置等均与当前帧点云scan_back一致，但点云内各激光点坐标则完成了从当前帧机器人坐标系到上一帧时刻机器人坐标系的全局坐标转换。
[0068]
s4、计算所述转换后点云与所述上一帧点云之间的重合程度，根据所述重合程度对机器人定位误差进行评估。
[0069]
从机器人的实际运行过程来看，激光雷达的扫描频率较高，并且移动机器人往往考虑到安全性等因素而以低速运行，因此正常情况下机器人接收到的连续两帧点云变化率较小。两帧间仅有区别在于部分当前帧新检测到的环境特征点以及因机器人移动消失在检测范围中的特征点，但对于普通应用场景来说，存在区别部分在一帧完整点云中的占比极低。
[0070]
基于上述客观前提，在机器人定位功能正常、定位结果输出正确的前提下，通过帧间变换将当前帧点云转移至上一帧时刻的机器人坐标系下时，转换后的点云与上一帧点云的重合度较高。由于该推断建立在机器人定位正确的背景下，若机器人定位误差较大或位置信息丢失，可以直接表现出的现象即为转换后点云与上一帧重合度较低，进而可以利用点云间的关联特性建立定位误差评价模型，对转换后点云以及上一帧点云的重合程序进行深入分析。从2d激光slam算法中机器人位姿表达的三个维度(x，y，theta)出发，对每时刻机器人定位结果进行分析。
[0071]
s41、建立评估模型
[0072]
本实施例中，首先根据所述转换后点云与所述上一帧点云计算所述转换后点云中各激光点的定位平移误差和定位旋转误差。
[0073]
其中，计算所述转换后点云中各激光点的定位平移误差，包括：
[0074]
以转换后点云中的每个激光点为目标，在所述上一帧点云中搜索满足阈值条件的两个最近点并进行拟合得到最近直线，并计算对应激光点到对应最近直线的目标距离；
[0075]
计算所述转换后点云中每个激光点的法向量信息，通过转换后点云中能够与上一帧点云建立起点与拟合直线关联的有效激光点的方向信息对所述目标距离进行投影分解，以获得各激光点在x和y方向上的分量，并将各激光点在x和y方向上的分量作为转换后点云中各激光点的定位平移误差。
[0076]
具体的，对于前后两帧均观测到的环境中同一障碍物，在理想情况下将当前帧点云转换到上一帧机器人坐标系下后，对应区域的点云应完全重合。但转换过程中所依赖的变换矩阵来源于两个时刻下定位结果的位姿变换，由于机器人的定位程序无法保证误差为0，因此转换后点云与上一帧点云中同一障碍物特征的点集存在一定的距离，该距离的大小便是衡量定位误差更为具体的依据。如图6所示，该图用于形象化展示，非真实数据。运行时前后两帧点云内容差异较小，且在理想情况下转换后的当前点云能够与上一帧点云高度重合，如图6中左下的重合图；但实际存在误差时点云间总是存在一定的偏差，如图6中右下的重合图。因此对该偏差的量化评估便是对定位误差分析的有效方案。
[0077]
首先，在点云的平移方面涉及的x、y维度上，对转换后点云与上一帧点云间的相对距离进行定量分析。简单来说，如果能直接建立起转换后点云与上一帧点云中共同观测到的环境特征的每个激光点间的对应关系，并计算对应点间的欧式距离即可作为误差评价的数据来源。但是，从实际激光雷达的数据特性出发，点云密集且每个激光点不具备语义信息，因此无法直接建立起点与点之间的对应关系。算法采用最近直线拟合的方式来建立转换后点云中每个激光点与上一帧点云的对应关联，以转换后点云中的每个激光点为目标，在上一帧点云中搜索满足阈值条件的两个最近点并进行拟合得到最近直线，并计算出该点到拟合出的最近直线距离d
transed_to_front
，如图7所示。
[0078]
根据前述点云中法向量求取的工作可以知道，由于环境中存在各种类型的非结构
化障碍物，因此一帧点云中激光点的法向量方向各异，无法在单一的维度上对一帧点云中满足条件的所有点求解得到的d
transed_to_front
进行统一的量化评价。算法进一步利用转换后点云中点的法向量信息，通过转换后点云中能够与上一帧点云建立起点与拟合直线关联的有效激光点的方向信息对d
transed_to_front
进行投影分解，获得在x、y方向上的分量，根据x、y方向上的分量确定机器人的平移误差：
[0079][0080]
从而完成了在转换后点云与上一帧点云间由每个点独立分析到一帧内所有满足条件点的数据对齐、统一处理的转变。
[0081]
进一步的，计算所述转换后点云中各激光点的定位旋转误差，包括：根据各激光与对应最近直线之间的距离，基于三角几何原理以及微小量近似原则，计算所述转换后点云中各激光点的定位旋转误差。
[0082]
本实施例中，在点云的旋转所涉及的角度方面，对于移动机器人所搭载的单线激光雷来说，仅有航向角的变化。与平移维度上的偏差相同，转换后的点云scan_transed与scan_front间存在角度上的差异，该差异同样能够对机器人的定位能力进行准确评估。
[0083]
如图8所示，以一帧点云中单个激光点数据为例进行角度误差求解详细阐述。图中选取scan_transed中一点ptransed以及scan_front中对应于该点的两个最近点拟合的直线,根据前述中平移误差的求解可以快速确定ptransed到拟合直线的距离d
transed_to_front
。根据三角几何原理以及微小量近似原则，在车体坐标系下过ptransed做垂线垂直于optransed并与拟合直线交于一点ptransed’,则optransed与optransed’夹角θ则为角度误差的具体化表达，即scan_transed中点云经过一定的角度补偿后可与scan_front在方向上重合，则该补偿的角度可作为定位旋转误差的衡量指标。对角度误差的具体计算过程如下：
[0084]
1)根据坐标转换关系获得ptransed在地图坐标系中坐标，并计算得到optransed长为l以及与x轴夹角α；
[0085]
2)根据ptransed坐标以及拟合得到的最近直线获得过ptransed并垂直于拟合直线的垂线方程，从而得到图中垂线与x轴夹角β；
[0086]
3)根据空间几何原理计算得到图中γ＝180
°‑
β-(90
°‑
α)＝90
°‑
β+α；
[0087]
4)根据角度γ、ptransed到拟合直线距离d
transed_to_front
以及三角函数计算得到ptransed与ptransed’间距离
[0088]
5)至此，已经计算得到δop
transed
p
transed
'中两条直角边，因此可以根据三角函数计算得到待求解的角度值
[0089]
进一步的，在计算出转换后点云中每个激光点的定位平移误差和定位旋转误差之后了，对点云中的激光点进行分区统计，进而确定具体的，对扫描数据进行多区域划分，根据设定的阈值在每个区域中选择固定数量的目标激光点，每个目标激光点权重相同，通过计算目标激光点定位平移误差和定位旋转误差的方差来评估机器人的定位误差。
[0090]
s42、分区统计、权重均衡
[0091]
本实施例中，经过上述过程，算法能够在获取到每帧点云时获得帧内大量激光点在x、y、theta方向上的误差统计。对于scan_transed中满足条件的每个激光点都能够获得其在平移与旋转维度上的误差，但是仅有每个点的误差数据无法对该帧时刻的定位误差进行整体评估。如果仅仅是通过误差阈值设置来判断定位正确与否，则可能存在个别激光点误差较大而整体误差小导致错误判断的情况。同时，根据统计学原理可知，数据中方差以及多维度的协方差是衡量随机变量或一组数据时离散程度的度量，常用来评估随机变量和其数学期望(即均值)之间的偏离程度。
[0092]
区别于数理统计中对某一固定变量多次测量后根据所有数据计算方差的方法，本算法采用单次测量中的多点数据进行误差在多维度上的方差计算并形成协方差矩阵的方案，前述步骤中根据点云法向量进行误差投影的方案保证了数据的一致性，利用公式(4)仅通过单帧数据就能完成x、y、theta三个维度上的方差计算。
[0093][0094]
根据数理统计原理，对于每一维度先直接计算均值再计算方差的过程，其中每一个数据元素享有同样的权重被带入计算。在机器人的实际应用过程中，部分重点场景由于环境结构特殊，在某一维度上的误差体现不够明显，如长廊环境中大量点云落在了两侧墙体上，少量点落在了该场景下特征明显、区分度高的障碍物上。如此一来，落在两侧墙体的点云即使存在前后的误差也会因高度相似的点云结构而在从scan_front中寻找scan_transed最近直线的过程中起到干扰，无法建立起真实的关联，导致误差距离计算有误。虽然在该场景下少量的区分度高的障碍物点云在两帧的相对距离计算中依然保持着准确性，但是由于场景中此类点云数量在点云总量中占比很低，从而导致在每帧点云的数据统一计算方差时，真实的误差数据被大量受干扰的无效数据“稀释”，无法体现出该帧点云时刻的误差评估情况。
[0095]
为解决该问题，算法采用分区统计、权重均衡的方式来提高对不同场景的适应度。以360度扫描的单线激光雷达数据为例，对于扫描数据进行多区域划分，如图9所示，原始数据落在了多个同等面积的区域中，图中红色折线为点云数据，区域的划分可根据实际数据量灵活配置。算法根据设定的阈值在每个区域中选择固定数量的激光点，被选择的点成为了每个区域中的代表点，同时各区域中的代表点在进行后续统计学分析时享有同样的权重。举例来说，在长廊环境中一次扫描总共接收到1000个激光点数据，其中落在两侧墙体的点云数量为900，落在环境中部分体积较小但区分度高的环境特征上的点云为100个，按原有的方案墙体特征占比90％，即使在区分度高的特征上得到了较大的误差值但最终呈现在整体结果上并不明显。但通过分区统计后，墙体特征的点云数量下降明显，而区分度高的特征因其原本就较为集中而在数量上没有大幅波动，进而在利用采样后点云计算误差协方差的过程中提升了该类特征的权重比，计算结果更符合真实情况。基于该方法实现了最终整体误差计算时点云的权重均衡，经过分区采样后即使在诸如长廊环境这类高度结构化场景中也能够将原有大量的重复性特征进行权重归一，从而能够更准确的把握每帧点云中每一处具有区分度的特征，更为客观的对定位误差进行评估。
[0096]
本发明实施例还提供一种基于二维激光的机器人定位误差评估装置，包括：
[0097]
存储容器建立模块，用于建立数据存储容器，所述数据存储容器中存储有连续两帧点云数据和连续两帧时刻下的机器人全局位姿；
[0098]
帧间转换矩阵计算模块，用于根据当前帧时刻下的机器人全局位姿以及与当前帧相邻的上一帧时刻下的机器人全局位姿，计算当前帧点云转换到上一帧点云的帧间转换矩阵；
[0099]
点云转换模块，用于根据所述帧间转换矩阵对所述当前帧点云进行转换，以得到转换后点云；
[0100]
定位误差评估模块，用于计算所述转换后点云与所述上一帧点云之间的重合程度，根据所述重合程度对机器人定位误差进行评估。
[0101]
可选的，定位误差评估模块具体用于：
[0102]
根据所述转换后点云与所述上一帧点云计算所述转换后点云中各激光点的定位平移误差和定位旋转误差；
[0103]
对扫描数据进行多区域划分，根据设定的阈值在每个区域中选择固定数量的目标激光点，每个目标激光点权重相同，通过计算目标激光点定位平移误差和定位旋转误差的方差来评估机器人的定位误差。
[0104]
其中，计算所述转换后点云中各激光点的定位平移误差，包括：
[0105]
以转换后点云中的每个激光点为目标，在所述上一帧点云中搜索满足阈值条件的两个最近点并进行拟合得到最近直线，并计算对应激光点到对应最近直线的目标距离；
[0106]
计算所述转换后点云中每个激光点的法向量信息，通过转换后点云中能够与上一帧点云建立起点与拟合直线关联的有效激光点的方向信息对所述目标距离进行投影分解，以获得各激光点在x和y方向上的分量，并将各激光点在x和y方向上的分量作为转换后点云中各激光点的定位平移误差。
[0107]
其中，计算所述转换后点云中每个激光点的法向量信息，包括：
[0108]
搜索转换后点云中各激光点对应的至少两个最近邻点，根据平面内不共线三点确定唯一圆的方式拟合出每个激光点所处的圆；
[0109]
根据空间几何学中圆上任意一点的法向量定义，以及拟合圆所涉及的三点坐标确定各激光点的法向量信息。
[0110]
计算所述转换后点云中各激光点的定位旋转误差，包括：
[0111]
根据各激光与对应最近直线之间的距离，基于三角几何原理以及微小量近似原则，计算所述转换后点云中各激光点的定位旋转误差。
[0112]
本发明实施例提供的一种基于二维激光的机器人定位误差评估装置可以执行本发明任意实施例所提供的一种基于二维激光的机器人定位误差评估方法，具备执行方法相应的功能模块和有益效果。
[0113]
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。