用于机器人的自动充电系统及充电桩定位导航方法与流程

1.本发明属于自动导航充电领域，特别涉及一种用于机器人的自动充电系统及充电桩定位导航方法。


背景技术：

2.机器人的自动充电技术主要解决机器人在电量接近耗尽的状态下室内自动导航和特定场景的定位问题，该技术属于室内精准定位导航相关研究课题的一部分，也即所谓“最后10米”的精准定位和导航问题。这个问题的解决需要配合一系列特殊的技术手段来分解进行，该方案的核心难点在于机器人对室内场景的感知，如针对充电桩的形状进行识别，针对室内的特征进行识别(室内的墙壁形状、障碍物等等)。根据场景识别方法不同，主流方案有：基于建图的定位导航方案，基于红外的定位导航方案，基于点云的定位导航方案等。
3.基于建图的定位导航方案属于slam定位导航的一部分，基于二维雷达点云信息进行标定后，机器人建立代价地图，再利用代价地图反映的几何信息进行导航和路线规划，其优点在于精准可靠，slam建图和导航算法多种多样，开源社区代码提交活跃。其软件包navigation本身作为ros机器人导航的核心功能，在全世界的使用实践中久经考验。虽然该软件包在导航和定位精度方面不存在短板，但不足之处是该方法对室内场景缺乏动态感知，例如：机器人本身并不存在对于充电桩形状的识别和位置的定位，如果充电桩被拿开，机器人不能自主识别充电桩的移动，就会继续执行后续的充电动作，这会导致系统并不能正确充电，执行上有一定机械性。
4.基于红外的定位导航方案需要在机器人和充电装置上分别搭建一套红外通讯系统，其主要硬件为三个红外传感器件，这个装置可以完整区分出远左、远中、远右，近左、近中、近右共计6个区域，定位时通过充电桩和机器人之间轮训进行红外通讯，来确定车身位于哪个区域中，该方法是kobuki协议官方推荐的自动充电方式。优势在于对于使用了kobuki协议作为单片机和工控机硬件通讯的系统而言，这种方案完全没有开发成本，但是该方案依靠三个红外传感器件区分出来6个区域子场，如果出现硬件故障，则会出现相应的充电失败风险，另外该红外通讯系统也增加了整体的项目成本。
5.基于点云的定位导航方案是基于雷达的点云数据实现对特定场景或物品的特征识别，并完成自身的路径规划。其优势在于基于雷达的点云数据可以完全基于算法实现，对于导航机器人来说，没有增加识别充电场景的额外硬件成本，缺点是对点云识别算法提出了较高的要求。比如点云配准坐标变换矩阵算法由点云图像获取到的变换位姿坐标可以经过转换用于定位，其配准算法计算开销较大，点云的结构性缺失对计算结果有影响，部分点云的随机噪声滤波困难，雷达在线数较低的情况下计算结果很容易过拟合。最终导致定位错误。


技术实现要素：

6.本发明针对现有技术中存在的问题，提出了一种能准确、快速完成充电桩定位，且
计算量小的用于机器人的自动充电系统。
7.为实现上述目的，本发明提供一种用于机器人的自动充电系统，包括充电桩识别模块和导航模块；
8.其中充电桩识别模块采用随机采样一致性的方法对设置在机器人上的单线雷达反馈的空间内的点云数据进行聚类，筛选出斜率相同的线段，并计算所有斜率相同的线段的端点之间的欧式距离，并保留欧式距离符合设定长度的两个端点，根据两个端点的坐标获得充电桩的位置；并实时将得到的充电桩的位置发送给导航模块；
9.导航模块根据充电桩的位置获得准备充电位置坐标，机器人沿着由机器人本身与准备充电位置形成的直线移动到准备充电位置，再沿着准备充电位置与充电桩位置形成的直线移动到充电桩进行充电。
10.进一步，还包括定位校正模块，其中，充电桩识别模块将获得的充电桩的位置发送给定位校正模块，定位校正模块将接收到的最新的充电桩的位置与历史值进行比较，如果最新的充电桩的位置坐标与历史值坐标之间的距离是在第一阈值范围内，则用最新的充电桩的位置更新历史坐标，并将计数器值加1，如果最新的充电桩的位置坐标与历史坐标值之间的距离是不在第一阈值范围内，则舍弃最新的充电桩的位置坐标，且计数器清零；直到计数器的值达到第二阈值，则输出充电桩的坐标给导航模块；初始状态下，历史值坐标为(0,0)，第一次获取的充电桩的位置更新历史值坐标。这样增加了充电桩识别的准确性。
11.进一步，定位校正模块根据验证充电桩位置与第一次获取的充电桩位置进行比较，如果两者的距离在验证范围内，则用第一次获得充电桩位置更新历史坐标值；如果两者的距离不在验证范围内，则移动机器人重新进行充电桩位置识别后再进行比较，直到两者的距离在验证范围内；其中验证充电桩位置通过设置在机器人上的具有gps的rtk模块、室内超宽频定位模块或基于slam的室内定位系统获得。这样可以更有效的提高充电桩算法的识别准确率。
12.其中，欧式距离符合设定长度的两个端点为两个端点之间的欧式距离等于充电桩的长度。
13.进一步，欧式距离符合设定长度的两个端点为两个端点之间的欧式距离最短。这样可以优化判断条件，有效的降低了计算量，提高了识别速度。
14.进一步，所述准备充电位置在充电桩直线的中垂线上，且与充电桩直线中点之间的距离为预留距离distance。这样可以方便优化机器人到充电桩的路径。
15.进一步，导航模块将准备充电位置坐标与机器人本身组成线段，组成的线段与充电桩直线的中垂线之间的夹角为第一夹角，根据第一夹角的正切值判断机器人是否需要旋转，以及旋转的方向；如果正切值为正，则机器人右转第一夹角；如果正切值为无穷，则机器人不旋转，如果正切值为负，则机器人左转第一夹角；旋转后，机器人沿着由机器人本身与准备充电位置形成的直线移动到准备充电位置。机器人能在较少的动作下完成自动充电。
16.本发明还提供了一种基于用于机器人的自动充电系统的充电桩定位导航方法，包括以下步骤：
17.步骤1：采用随机采样一致性的方法对设置在机器人上的单线雷达反馈的空间内的点云数据进行聚类，得到多条线段；
18.步骤2：根据步骤1得到的线段筛选并保存斜率为实数的线段；
19.步骤3：从步骤2保存的线段冲筛选出斜率相同的线段，且舍弃斜率为无穷的线段；
20.步骤4：分别计算步骤3得到所有斜率相同的线段的端点之间的欧式距离，并保留欧式距离符合设定长度的两个端点；
21.步骤5：步骤4得到的两个端点构成的线段表示充电桩的线段；获取充电桩的线段的中点坐标，电桩的线段的中点表示充电桩的位置；
22.步骤6：根据充电桩直线的中点坐标获取准备充电位置坐标，准备充电位置在充电桩直线的中垂线上，且与充电桩直线中点之间的距离为预留距离distance；
23.步骤7：机器人沿着由机器人本身与准备充电位置形成的直线移动到准备充电位置，再沿着准备充电位置与充电桩位置形成的直线移动到充电桩进行充电。
24.进一步，还包括定位校正，将步骤5得到的充电桩直线的中点坐标与历史值进行比较；如果最新的充电桩的位置坐标与历史值坐标之间的距离是在第一阈值范围内，则用最新的充电桩的位置更新历史坐标，并将计数器值加1，如果最新的充电桩的位置坐标与历史坐标值之间的距离是不在第一阈值范围内，则舍弃最新的充电桩的位置坐标，且计数器清零；直到计数器的值达到第二阈值，则输出充电桩的位置；初始状态下，历史值坐标为(0,0)，第一次获取的充电桩的位置更新历史值坐标。这样能够有效的提高充电桩位置识别的准确性和稳定性。
25.进一步，将准备充电位置坐标与机器人本身组成线段，组成的线段与充电桩直线的中垂线之间的夹角为第一夹角，根据第一夹角的正切值判断机器人是否需要旋转，以及旋转的方向；如果正切值为正，则机器人右转第一夹角；如果正切值为无穷，则机器人不旋转，如果正切值为负，则机器人左转第一夹角；旋转后，机器人沿着由机器人本身与准备充电位置形成的直线移动到准备充电位置。机器人能在较少的动作下完成自动充电。
26.工作原理：本发明通过对单线雷达反馈的点云数据进行聚类和筛选出充电桩位置，并对筛选出的充电桩位置进行打分验证，从而确定充电桩位置；本发明在充电桩线段的中垂线上设置了准备充电位置，根据准备充电位置坐标与机器人本身组成线段与充电桩线段的中垂线之间的夹角来判断机器人是否需要旋转和旋转的角度与方向，从而简化了机器人到充电桩的行径路线。
27.与现有技术相比，本发明不仅能够准确、稳定、快速的识别出充电桩，同时能够使机器人在最少动作的情况下移动到充电桩完成充电；本发明提供的系统成本低，易于实现，通用性强；同时计算量小。
附图说明
28.图1是本发明提供的用于机器人的自动充电系统结构示意图；
29.图2是本发明提供的用于机器人的充电桩定位导航方法工作流程示意图；
30.图3是定位校正模块工作流程示意图；
31.图4是导航模块中路径导航工作流程示意图；
32.图5是机器人充电导航方位示意图。
具体实施方式
33.下面将结合本发明实例中的附图，对本发明实施例中的技术方案进行清楚、完整
地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
34.如图1所示，本实施例提供了一种用于机器人的自动充电系统，主要包括充电桩识别模块、定位校正模块和导航模块；其中充电桩识别模块采用随机采样一致性的方法对设置在机器人上的单线雷达反馈的空间内的点云数据进行聚类，从而快速获取到小车周围场景的空间信息，推算出充电桩的位置；并实时将得到的充电桩的位置发送给定位校正模块；定位校正模块将接收到的最新的充电桩的位置与历史值进行比较，如果两者的差值在设定的第一阈值范围内,即判断最新的充电桩的位置坐标与历史坐标值之间的距离是否在第一阈值范围内，在第一阈值范围内的话，则用该点坐标更新历史坐标，并将计数器值加1，如果两者的差值不在设定的阈值范围，则舍弃该坐标，直到计数器的值达到第二阈值，则输出充电桩的坐标给导航模块；这样能够保证得到的充电桩的坐标更加的稳定和准确；导航模块根据充电桩的位置获得准备充电位置坐标，机器人沿着由机器人本身与准备充电位置形成的直线移动到准备充电位置，再沿着准备充电位置与充电桩位置形成的直线移动到充电桩进行充电。从而完成自动充电，这样能够使机器人沿着最简单的路径到达充电桩。
35.本实施例提供了基于机器人的自动充电系统的充电桩定位导航方法，如图2所示，主要包括以下步骤：
36.步骤1：采用随机采样一致性的方法对设置在机器人上的单线雷达反馈的空间内的点云数据进行聚类：由单线雷达扫描得出的点云数据进行数据提取，满足一定离散程度的点云数据，比如扫描在同一面墙面的离散点云数据，经过随机采样一致性算法可被抽象为高层数据结构
‑‑‑
线段，线段内部封装了斜率，起点，终点等进行平面几何运算必要的数据信息，所有线段会被放入第一线段容器extractionangleline中；
37.步骤2：遍历第一线段容器extractionline，获取所有可能的线段；存在实数斜率的线段则为有可能的线段；
38.步骤3：筛选获取容器内所有斜率相同的线段，放入第二线段容器sameangleline中；且舍弃斜率为无穷的线段。由于单线雷达信号点分布是远密近疏，虽然理论上充电桩正面斜率和墙面的斜率相同，但是充电桩正面上获得的雷达信号点更稀疏，距离雷达越近，波动比较大，所以获得的雷达信号点形成的线段的斜率在剧烈的波动，从而不如墙面上的雷达信号点形成的线段的斜率稳定，筛选斜率相同的线段可以有效的过滤不稳定，且波动比较大的雷达信号点。
39.步骤4：筛选提取第二线段容器sameangleline中斜率相同的线段，根据筛选出的线段的端点，计算每两个端点之间的欧氏距离，将计算的所有的欧氏距离与设定的第三阈值进行比较，存储欧式距离等于第三阈值的两个端点的坐标值；第三阈值一般等于充电桩的长度。一般情况下，采用的充电桩很小，也可以不设置第三阈值进行比较，直接存储欧式距离最短的两个端点即可，因为一般情况下采用的充电桩的尺寸比较小，根据雷达扫描出的点进行筛选和提取后，一般最短的线段即为充电桩的长度；
40.步骤5：步骤4得到的两个端点构成的线段表示充电桩的直线；根据步骤4得到的两个端点获得充电桩直线的中点坐标(vertical.x，vertical.y)，其中，vertical.x表示充电桩直线的中点的x轴坐标，vertical.y表示充电桩直线的中点的y轴坐标；
41.步骤6：如图3所示，将步骤5得到的充电桩直线的中点坐标(vertical.x，vertical.y)与历史值进行比较；如果当前充电桩直线的中点坐标(vertical.x，vertical.y)与历史值之间距离不超过第一阈值，则当前充电桩直线的中点坐标(vertical.x，vertical.y)更新历史值，并且计数器加一，判断计数器的值是否超过第二阈值，如果超过第二阈值，则直接输出当前的充电桩直线的中点坐标(vertical.x，vertical.y)，如果没有超过第二阈值，则重复步骤1～6；如果当前充电桩直线的中点坐标(vertical.x，vertical.y)与历史值之间距离超过第一阈值；则舍弃当前电桩直线的中点坐标(vertical.x，vertical.y)，计数器归零，重复步骤1～6；在初始情况下，历史值的坐标为(0,0)，则第一次获取的充电桩直线的中点坐标即为历史值；第一阈值的优选范围2cm
‑
5cm；第二阈值优选为50，这样可以在保证准确性和可靠性的同时，有效的提高输出的速度，从而能够更快更准确的输出充电桩直线的中点坐标(vertical.x，vertical.y)。
42.本实施例中还可以在机器人上设置具有gps的rtk模块、室内超宽频定位模块或基于slam的室内定位系统，给出一个用于验证的充电桩中点位置数据；在初始状态下，第一次通过单线雷达获得的充电桩直线的中点坐标与gps的rtk模块、室内超宽频定位模块或基于slam的室内定位系统给出的充电桩直线的中点坐标之间的距离超出了验证范围，则可以驱动机器人重新运动一下，左右微动若干厘米，在调整后的位置姿态上再识别一次充电桩直线的中点坐标，直至两种方式给出的坐标之间的距离在验证范围内，则将此次单线雷达获得的充电桩直线的中点坐标作为历史值，这样可以有效的提高充电桩算法的识别准确率。
43.步骤7：获取准备充电位置坐标(target.x，target.y)：根据步骤4的筛选结果可以知道充电桩直线起点坐标(start.x，start.y)，充电桩直线终点坐标(end.x，end.y)，其中，target.x和target.y分别表示准备充电位置的x轴坐标值和y轴坐标值，start.x和start.y分别表示充电桩直线起点的x轴坐标值和y轴坐标值，end.x和end.y分别表示充电桩直线终点的x轴坐标值和y轴坐标值：准备充电位置在充电桩直线的中垂线上，且与充电桩直线中点之间的距离为预留距离distance，则根据两点间距离公式获取准备充电位置坐标(target.x，target.y)：
44.(target.x
‑
vertical.x)2 (target.y
‑
vertical.y)2＝distance2；
ꢀꢀꢀ
(1)
45.根据充电桩直线和中垂线之间的斜率垂直关系可知：
[0046][0047]
根据公式(1)和(2)求解得到准备充电位置坐标(target.x，target.y)，舍弃含无穷的解，所得的实数根即为准备充电位置的实际坐标(target.x，target.y)。其中，预留距离distance优选为80cm。
[0048]
步骤8：如图4所示，根据步骤7得到的准备充电位置坐标与机器人本身组成线段，结合该线段与充电桩直线的中垂线之间夹角的正切值判断机器人是否需要旋转，以及旋转的方向；该夹角为第一夹角，如果正切值为正，则机器人右转第一夹角；如果正切值为无穷，则机器人不旋转，如果正切值为负，则机器人左转第一夹角。如图5所示，如果机器人在1号位置时，tan(th1)为正，则机器人右转th1度；如果机器人在2号位置时，正切值无穷大，则机器人不旋转；如果机器人在2号位置时，tan(th2)为负，则机器人左转th2度。
[0049]
步骤9：根据步骤7得到的准备充电位置与机器人本身坐标计算出两点之间的距
离，则机器人沿着准备充电位置与机器人本身组成线段运动到准备充电位置停止；
[0050]
步骤10：机器人按照第一次旋转的方向，再旋转第一夹角的补角，从而使得机器人尾部的充电接口对准充电桩。本实施例中如果机器人在1号位置开始向准备充电位置移动的，则到准备充电位置时，再向右旋转th1的补角，从而使机器人尾部的充电接口对准充电桩；如果机器人在2号位置开始向准备充电位置移动的，则到准备充电位置时，再向右旋转180度，从而使机器人尾部的充电接口对准充电桩；如果机器人在3号位置开始向准备充电位置移动的，则到准备充电位置时，再向左旋转th2的补角，从而使机器人尾部的充电接口对准充电桩。
[0051]
步骤11：机器人从准备充电位置后退到充电桩，实现自动充电。
[0052]
这样把复杂的路径规划和避障问题转化为单纯的小车速度驱动问题，且只需旋转、前进两个动作的组合，一条折线规划路径，就可以快速驱动小车到达目标点。
[0053]
当机器人到达预定充电位置时，系统会开始自动充电流程，位于车身前端的雷达会扫描机器人前端的点云图像，若判断出前方存在给定的充电桩形状，机器人会根据充电桩的相对自身位置的角度和距离，先执行一定的自转角度，然后前进相应的直线距离，到达充电桩正前方之后，车身自转，使车尾充电接口正对充电桩，后退相应的距离之后，完成充电，车灯变为蓝色提示正在充电，但充电完成后，车体会从充电桩自动弹出，车灯变成绿色。
[0054]
虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，当可作各种的更动与润饰。因此，本发明的保护范围当视权利要求书所界定者为准。