本发明涉及一种精准评判机器人定位精度的方法，属于智能机器人技术领域。
背景技术
变电站智能巡检机器人是一种室外轮式移动机器人，其可以携带可见光、红外等检测设备对室外变电站内设备进行全时段自主巡检。
导航和定位是巡检机器人完全自主运行的关键，巡检机器人定位导航系统的性能直接决定了其完成巡检任务的质量。在室外条件下，智能巡检机器人采用激光定位的方式，具有定位设备成本低、可全时段工作、不受电磁干扰、定位精度高、无累积误差、地面附属设施施工简单等特点，可以精准的实现机器人的实时可靠的定位。但是在实际运行过程中，真实环境的动态变化和行走路面的崎岖颠簸等因素，可能会对机器人的定位精度产生影响。在机器人运行过程中，若对机器人定位精度的评判不合理，造成出现定位精度很高但实际上已经定位失败的情况的话，会引起机器人的盲目行走，可能对变电站的安全正常运行造成严重的后果，并且也会增加用户的运维成本和安全事故出现的风险。
技术实现要素：
发明目的：为了克服现有技术中存在的不足，本发明提供一种精准评判机器人定位精度的方法，该方法在机器人运行过程中，能够实时检测机器人当前所处位姿的精准度，并且能够进行位姿的校正，保证了机器人可以安全可靠的运行。
技术方案：为实现上述目的，本发明采用的技术方案为：
一种精准评判机器人定位精度的方法，包括以下步骤：
步骤s1：对导航过程中实时采集的激光雷达点云数据进行预处理。
步骤s2：采用匹配度算法处理步骤s1得到的预处理数据，获取激光点云的轮廓与地图中的障碍物轮廓的相似度，根据该相似度计算机器人当前位姿的置信度。
步骤s3：根据步骤s1得到的预处理数据获取当前位姿，根据当前位姿寻找地图坐标系中当前时刻机器人的最优位姿。
步骤s4、根据步骤s3得到的机器人的当前位姿和最优位姿，实时判断机器人的定位精度。
步骤s4.1通过判断机器人当前位姿和最优位姿的距离和角度的差，得出归一化的位姿准确度，结合当前位姿处所获得的激光数据匹配的置信度，归一化得到当前机器人的定位精度。
s4.2根据得到的当前机器人的定位精度与设置的定位精度阈值进行比较，得到当前机器人的定位精度。
优选的：所述步骤s1中对激光雷达点云数据进行预处理具体步骤为：
步骤s1.1：机器人内的激光雷达扫描周围的动态环境，采集到的激光数据作为原始激光数据，通过噪声滤波算法和数据阈值对原始激光数据进行处理，从而得到有效的激光数据。
步骤s1.2：机器人内的惯性测量单元实时监测自身姿态，当发生姿态倾斜时，对经过步骤s1.1处理后的数据进行平面映射，保证机器人扫描平面与实际环境的一致性。
步骤s1.3，对经过步骤s1.2处理后的数据的合理性进行判断，剔除在合理区域以外的数据，合理区域作为经验阈值，一般根据实际场景需要进行设置。
优选的：所述步骤s1.1：中通过噪声滤波算法和数据阈值对原始激光数据进行处理的方法：
针对每帧原始激光数据中的超出激光测量范围的数据点进行过滤，超出最大测量范围的重新赋值为激光的最大量程值，保证每一个激光点都在正常的范围内。
将每一帧的原始激光数据获得的特征点依次计算其欧式距离，并与设置的数据阈值进行比较，若小于该数据阈值，则认为是特征点，反之，则是噪声点。对于噪声点的处理，将其距离值设置成前一点的距离。
优选的：所述步骤s1.2中对激光数据进行如下平面映射：
d’＝d*cos(θ)。
其中，d'是映射后的激光点的距离值，d是映射前的激光点的距离值，θ是倾斜的角度值。
优选的：所述步骤s2中计算机器人当前位置的置信度的方法包括以下步骤：
步骤s2.1数据转换：将步骤s1得到的预处理后数据中的机器人到周围环境中物体的距离和角度信息转换为实际的世界坐标系中的坐标，然后通过世界坐标系和地图坐标系之间的固定关系，将激光数据转换为地图坐标系中的坐标数据。
步骤s2.2设置激光点匹配窗口：对每一个参与匹配的激光数据点在地图坐标系中设定激光点匹配窗口。激光点匹配窗口通过距离和角度判断选择地图中的匹配点，其满足：
min(r_d*distance+r_a*angle)
其中，distance为激光点到候选匹配点的距离，angle为激光点和候选匹配点分别与机器人中心的连线所形成的角度差绝对值，r_d和r_a为分别是距离和角度所占的权重。
步骤s2.3激光点匹配：针对一帧激光数据里面的每一个激光点在匹配点选择窗口内通过距离和角度判断选择地图中匹配点，以使得总的激光点与地图中的障碍点的距离最小，通过归一化，得到单纯数据点匹配的得分。
步骤s2.4轮廓算法判定：通过对激光点的距离和角度信息进行整合设置轮廓检测条件，对连续的激光点进行判断，若判断该段激光轮廓与地图障碍轮廓吻合，则该段激光轮廓内的数据点轮廓得分有效，否则无效。
步骤s2.5地图中特征权重分类：将机器人构建的环境地图按实际环境中特征的重要程度进行分类，得到地图匹配点的权重值。
步骤s2.6计算置信度：
c＝∑(w*(r_match*score_match+r_outline*score_outline))/n
其中c代表置信度，n代表激光点个数，w表征地图匹配点的权重值，r_match和score_match为数据点距离匹配的系数和得分，r_outline和score_outline数据点轮廓匹配的系数和得分。
优选的：所述步骤s2.3中，若激光点在匹配点选择窗口内无法匹配到有效的特征点，则设定其匹配值为默认最大值，默认最大值设定可依据窗口大小和地图栅格尺寸大小设定。
优选的：所述步骤s3寻找地图坐标系中当前时刻机器人的最优位姿具体步骤为：
步骤s3.1确定搜索窗口：在机器人当前位姿周围设置搜索窗口，寻找最优位姿。当判断发生定位失败以后，则以机器人当前位姿为中心，在整个地图空间进行搜索，寻找最优位姿。
步骤s3.2确定候选位姿：在搜索窗口内通过调整机器人的平移和旋转，得到符合要求的机器人候选位姿点集。
步骤s3.3地图分层：将地图按分辨率进行分层，逐层比较，直至搜索到分辨率最高的层次中。
步骤s3.4采用高斯-牛顿算法或者最速下降法结合树形存储结构对候选位姿点集进行筛选，通过对候选位姿点集中的每一个位姿点在地图坐标系进行激光数据重投影，计算该位姿点处的置信度，置信度最大的机器人位姿即为最优位姿。
优选的：步骤s4.2中若比较得到的当前机器人的定位精度小于设置的定位精度阈值，则用求出的最优位姿对其进行微调校正。
本发明相比现有技术，具有以下有益效果：
本发明适应性较强，可在室内外不同的场景和路面条件下均能获得较为准确的机器人实时定位精度的信息，对发生的定位失败的情况及时做出反应，整个过程不需要人工干预，本发明可以实现在机器人正常导航过程中，实时精准地评判机器人的定位精度，从而保证机器人可以安全可靠的运行，降低了机器人在定位出错的情况下可能引发的安全问题的风险。
附图说明
图1是本发明实施的定位精度评判流程图
图2是激光点云预处理流程图
图3是机器人当前位姿置信度计算流程图
图4是机器人最优位姿求解流程图
具体实施方式
下面结合附图和具体实施例，进一步阐明本发明，应理解这些实例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
一种精准评判机器人定位精度的系统，包括：
用于获取有效准确的激光数据的激光雷达原始点云数据的预处理单元。
衡量当前位姿处激光点云与地图环境特征匹配相似度的置信度计算单元。
针对机器人里程位姿进行优化的最优位姿求取单元。
评判机器人导航过程中定位准确度的精度计算单元。
用于微调机器人位姿的位姿校正单元。
一种精准评判机器人定位精度的方法，如图1所示，包括以下步骤：
步骤s1：为了避免因为机械抖动或者其他干扰造成的激光数据不准确的情况，需要对导航过程中实时采集的激光雷达点云数据进行预处理。
如图2，所述步骤s1中对激光雷达点云数据进行预处理具体步骤为：
步骤s1.1：机器人内的激光雷达以较高的频率扫描周围的动态环境，采集到的数据中包含很多噪声点和无效点，为得到更加精确的激光数据，通过噪声滤波算法和设置合理的数据阈值对原始激光数据进行处理，从而得到有效的激光数据。
所述步骤s1.1：中通过噪声滤波算法和数据阈值对原始激光数据进行处理的方法：
针对每帧原始激光数据中的超出激光测量范围的数据点进行过滤，超出最大测量范围的重新赋值为激光的最大量程值，保证每一个激光点都在正常的范围内。
将每一帧的原始激光数据获得的特征点依次计算其欧式距离，并与设置的数据阈值进行比较，若小于该数据阈值，则认为是特征点，反之，则是噪声点。对于噪声点的处理，将其距离值设置成前一点的距离。
步骤s1.2：机器人内的惯性测量单元实时监测自身姿态，当发生姿态倾斜时，对经过步骤s1.1处理后的数据进行平面映射，保证机器人扫描平面与实际环境的一致性。
步骤s1.3，对经过步骤s1.2处理后的数据的合理性进行判断，剔除在合理区域以外的数据，合理区域作为经验阈值，一般根据实际场景需要进行设置。
如图2所示：(1)机器人启动时，自动加载激光雷达驱动，通过解析激光数据包，获取原始的点云数据，并封装成自定义的消息格式存入到激光数据队列中。
(2)针对每帧数据中的超出激光测量范围的数据点进行过滤，超出最大测量范围的重新赋值为激光的最大量程值，保证每一个激光点都在正常的范围内。
(3)为了有效地滤除噪声点，将每一帧的激光扫描数据获得的特征点依次计算其欧式距离，并与设置的阈值进行比较，若小于该阈值，则认为是特征点，反之，则是噪声点。对于噪声点的处理，将其距离值设置成前一点的距离。
(4)通过惯性测量单元得到自身姿态角度信息，当发生姿态倾斜时，对激光数据进行平面映射：
d’＝d*cos(θ)
其中，d’是映射后的激光点的距离值，d是映射前的激光点的距离值，θ是倾斜的角度值。
激光数据映射后，可以保证机器人扫描平面与实际环境平面的一致性，使得匹配结果更加准确。
(5)为了便于后续计算，保证计算的准确性，需要对数据的合理性进行判断，剔除在合理区域以外的数据，合理区域作为经验阈值，一般根据实际场景需要进行设置。
步骤s2：采用匹配度算法处理步骤s1得到的预处理数据，获取激光点云的轮廓与地图中的障碍物轮廓的相似度，根据该相似度计算机器人当前位姿的置信度。
如图3所示，所述步骤s2中计算机器人当前位置的置信度的方法包括以下步骤：
步骤s2.1数据转换：由于激光雷达安装的位置并不一定在机器人的中心上，故激光坐标系与机器人坐标系通常存在一个固定的平移偏量，同理，机器人坐标系统与里程计坐标系也存在固定的平移偏量。并且在机器人系统中，世界坐标系可以简单看作是里程计坐标系，也与具体场景所在的地图坐标系存在着旋转和平移偏量。在实际应用时，通常会先进行起始点标定，获取各个坐标系之间的转换关系，由此也可以将激光数据通过层层的坐标转换，最终转换为地图坐标。总的来说将步骤s1得到的预处理后数据中的机器人到周围环境中物体的距离和角度信息转换为实际的世界坐标系中的坐标，即将激光雷达获取的机器人到周围环境中物体的距离和角度信息转换为实际的物理世界坐标系中的x和y坐标，然后通过世界坐标系和地图坐标系之间的固定关系，将激光数据转换为地图坐标系中的坐标数据。
步骤s2.2设置激光点匹配窗口：对每一个参与匹配的激光数据点在地图坐标系中设定激光点匹配窗口。激光点匹配窗口的设置主要是为了保证匹配精度和加快匹配过程，防止错误匹配对结果产生影响。
激光点匹配窗口通过距离和角度判断选择地图中合适的匹配点，其满足：
min(r_d*distance+r_a*angle)
其中，distance为激光点到候选匹配点的距离，angle为激光点和候选匹配点分别与机器人中心的连线所形成的角度差绝对值，r_d和r_a为分别是距离和角度所占的权重，是经验值，依具体情况设定。
步骤s2.3激光点匹配：针对一帧激光数据里面的每一个激光点在匹配点选择窗口内通过距离和角度判断选择地图中匹配点，以使得总的激光点与地图中的障碍点的距离最小，通过归一化，得到单纯数据点匹配的得分。
所述步骤s2.3中，若激光点在匹配点选择窗口内无法匹配到有效的特征点，则设定其匹配值为默认最大值，默认最大值设定可依据窗口大小和地图栅格尺寸大小设定。
步骤s2.4轮廓算法判定：通过对激光点的距离和角度信息进行整合设置轮廓检测条件，对连续的激光点进行判断，若判断该段激光轮廓与地图障碍轮廓吻合，则该段激光轮廓内的数据点轮廓得分有效，否则无效。
轮廓点的判定条件是该点在连续的激光点集内，该点集中的所有激光点均能匹配到特征点，且它们的距离的角度的偏差均在合理的阈值范围内且趋势一致，点集的长度和阈值的大小是经验值，依具体情况而定。
步骤s2.5地图中特征权重分类：将机器人构建的环境地图按实际环境中特征的重要程度进行分类，按这些特征是否一直存在、会不会一直微动、规则形状是否明显、容不容易匹配等标准对这些物体特征的权重依次划分等级，得到地图匹配点的权重值。
地图特征权重的不同主要是依据在实际环境中，不同环境特征对于激光匹配精度的影响不同而定的，持续移动的物体(如行人、车辆等)，会造成激光点云的误匹配，应该将其匹配权重置零，处于微动状态的物体(如草木、粗绳等)，则会影响激光匹配的精度，其匹配权重应低一点，而轮廓清晰、静止不动的物体(如墙壁、围栏等)则可以将其匹配权重置高一点，权重的大小也是经验值，要依据具体的环境设定。
步骤s2.6计算置信度：
c＝∑(w*(r_match*score_match+r_outline*score_outline))/n
其中c代表置信度，n代表激光点个数，w表征地图匹配点的权重值，r_match和score_match为数据点距离匹配的系数和得分，r_outline和score_outline数据点轮廓匹配的系数和得分。
步骤s3：根据步骤s1得到的预处理数据获取当前位姿，根据当前位姿寻找地图坐标系中当前时刻机器人的最优位姿。
如图4所示，所述步骤s3寻找地图坐标系中当前时刻机器人的最优位姿具体步骤为：
步骤s3.1确定搜索窗口：机器人在正常导航过程中，位姿相对较精确，可以在机器人当前位姿周围设置搜索窗口，寻找最优位姿。当判断发生定位失败以后，则以机器人当前位姿为中心，在整个地图空间进行搜索，寻找最优位姿。
机器人在正常导航过程中，位姿不停地进行匹配校正，相对较精确，最优位姿一般在当前位姿附近。获取当前时刻机器人位姿，在该位姿周围设置固定的线性搜索窗口和旋转搜索窗口，保证机器人在整个位姿空间进行搜索。若在搜索窗口内无法找到最优位姿，则将搜索窗口加倍扩大，直到符合条件；
步骤s3.2确定候选位姿：在搜索窗口内通过调整机器人的平移和旋转，一般是将机器人按固定的步长进行旋转和平移，得到符合要求的机器人候选位姿点集。
步骤s3.3地图分层：为加快搜索过程，可以将地图按分辨率进行分层，逐层比较，直至搜索到分辨率最高的层次中。
步骤s3.4采用高斯-牛顿算法或者最速下降法等结合树形存储结构对候选位姿点集进行筛选，通过对候选位姿点集中的每一个位姿点在地图坐标系进行激光数据重投影，计算该位姿点处的置信度，置信度最大的机器人位姿即为最优位姿。
步骤s4、根据步骤s3得到的机器人的当前位姿和最优位姿，实时判断机器人的定位精度。
步骤s4.1通过判断机器人当前位姿和最优位姿的距离和角度的差，得出归一化的位姿准确度，结合当前位姿处所获得的激光数据匹配的置信度，归一化得到当前机器人的定位精度。
s4.2根据得到的当前机器人的定位精度与设置的定位精度阈值进行比较，得到当前机器人的定位精度。
步骤s4.2中若比较得到的当前机器人的定位精度小于设置的定位精度阈值，即精度较低，则用求出的最优位姿对其进行微调校正。
本发明可以实现在机器人正常导航过程中，实时精准地评判机器人的定位精度，从而保证机器人可以安全可靠的运行，降低了机器人在定位出错的情况下可能引发的安全问题的风险。
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。