一种巡检机器人定位方法及自动充电方法与流程

本发明涉及一种巡检机器人定位方法及自动充电方法。

背景技术：

市场上现有的自动充电方式主要有两种：(1)扫地机器人的自动充电是通过充电基座不断发出信号，然后机器人顶部的接收器接收到信号，最终找到“回家”的路，但是在某些情况下会出现无法导航，无法回到基座而搁浅在“半路上”的情况发生。(2)自动接触式充电技术，连接触点位于机器人本体上方，当机器人到达充电座时，与地面连接触点自动完成对接，充电完毕后自动脱离。这种方法脱离定位，对对接误差要求较高。

技术实现要素：

为解决现有技术存在的问题，本发明提供一种巡检机器人定位方法，实现巡检机器人在充电房中的精确定位，提高与充电桩对接的成功率，方便巡检机器人在充电房内充电。

本发明提供的巡检机器人定位方法，包括以下步骤：

(1)利用激光传感器采集巡检机器人在充电房中的正前方和两侧墙壁的激光数据；

(2)对激光数据进行二次点集分割及二次直线拟合；其中，第一次点集分割得到对应充电房左边墙面的左激光数据集、对应充电房门和充电房门左、右角落的中间激光数据集、对应充电房右边墙面的右激光数据集；第一次直线拟合采用最小二乘法对左激光数据集和右激光数据集进行拟合，得到充电房左边墙面对应直线L₃和右边墙面对应直线L₂；第二次点集分割从中间激光数据集中剔除左、右角落对应的激光数据，得到充电房门直线L₁对应激光数据集；第二次直线拟合通过最小二乘法对充电房门直线L₁对应的激光数据集进行拟合，得到充电房门对应直线L₁；

(3)将直线L₁,L₂,L₃按照充电房特征进行匹配，即判断直线L₁是否同时垂直于直线L₂和直线L₃，如果同时垂直则匹配成功，否则返回步骤(1)；

(4)获取充电房边缘特征，即，将步骤(3)中匹配成功的直线L₁,L₂,L₃作为充电房的边缘特征；

(5)通过步骤(4)得到的充电房边缘特征L₁,L₂,L₃，通过直线相交获取充电房的两个关键点，即左边墙面对应直线L₃与充电房左角落、右边墙面对应直线L₂与充电房右角落形成的拐点；

(6)利用步骤(5)获得两个关键点与机器人之间的相对位置，确定机器人位置。

第一次点集分割具体包括：选取连续的2m+1个激光数据，将前m个激光数据坐标取平均值得到前参考点A的坐标，第m+1个激光点坐标作为当前点B的坐标，后m个激光数据坐标取平均值得到后参考点C的坐标，然后计算后参考点C、前参考点A和当前点B三个点形成的角度∠CAB，∠CAB大小记为θ，如果则判定当前点A已经接近充电房右角落，将当前点作为右分割点P_r，其中，m为自然数，为设定阈值；以同样方法得到左分割点P_l；利用左、右分割点对连续的激光数据进行第一次分割，得到对应充电房左边墙面的左激光数据集、对应充电房门和充电房门左、右角落的中间激光数据集、对应充电房右边墙面的右激光数据集。m的取值范围为[30,80]，的取值范围为

第二次点集分割具体包括：从右分割点P_r开始遍历中间激光数据集，当中间激光数据集中的点到右边墙面对应直线L₂的距离大于或等于设定阈值threshold时，则认为当前激光数据点位于充电房门对应直线L₁，并将当前激光数据点作为充电房门直线L₁对应激光数据集的右端点P_re；以同样的方法，得到充电房门直线L₁对应激光数据集的左端点P_le，利用左、右端点对中间激光数据集进行第二次分割，得到充电房门直线L₁对应激光数据集。

在获取右端点P_re、左端点P_le时，设定阈值threshold分别略大于充电房右角落的宽度、充电房左角落的宽度。

为降低计算复杂度，同时提高算法的精确性，在步骤(2)之前，先对激光数据进行去噪声处理，去除噪声数据。

本发明还提供一种巡检机器人自动充电方法，巡检机器人通过定位导航自动驶入充电房；利用本发明提供的巡检机器人定位方法对巡检机器人进行定位；根据定位调整巡检机器人位置和朝向，直到巡检机器人定位在充电桩正前方处；巡检机器人充电部件与充电桩接触进行充电。

如果充电过程出现充电状态中断，判断巡检机器人位姿是否发生变化，若是，退出充电桩并调整位姿后重新与充电桩接触充电，否则直接进行充电。

本发明为了实现巡检机器人安全准确、快速高效地自动充电，通过激光设备和充电房的几何特性进行机器人局部定位。为了实现定位的精确性，使用二次点集分割算法对连续激光数据进行分割，通过最小二乘法对分割数据集进行拟合，利用拟合得到的边缘特征对充电房模型进行重构，最后通过重构得到的充电房模型关键点对机器人进行定位。

本发明通过激光设备进行局部定位，精度高，可以达到±0.5cm的误差范围内。利用高精度的定位，进行充电桩对接成功率大大提高，经测试，成功率达到99.8％。通过本发明可实现巡检机器人在变电站长期值守、完全自治，在无人干预情况下，安全准确、快速高效地自动充电，能实现与充电桩的成功对接，对外界干扰具有较好的鲁棒性。

附图说明

图1为巡检机器人充电房局部定位算法流程图；

图2为二次点集分割算法原理图；

图3为第一次点集分割程序流程图；

图4为第二次点集分割程序流程图；

图5为机器人定位原理图。

图中：1-充电房，2-机器人，3-激光，4-充电房右角落，5-充电房左角落，6-充电房门对应直线L₁，7-前参考点A，8-当前点B，9-后参考点C，10-∠CAB，大小为θ，11-右分割点P_r(表示第r个激光数据点)，12-充电房门直线L₁对应激光数据集的右端点P_re，13-右边墙面对应直线L₂，14-左边墙面对应直线L₃，15-前参考点D，16-当前点E，17-后参考点F，18-∠FDE，大小为α，19-左分割点P_l(表示第1个激光数据点)，20-充电房门直线L₁对应激光数据集的左端点P_le。

具体实施方式

如图1所示，本发明提供的巡检机器人定位方法主要步骤有：

1、噪声剔除：对激光传感器采集巡检机器人2在充电房1中的正前方和两侧墙壁的激光数据进行去噪处理，去除噪声数据，降低计算复杂度的同时，提高精确性；

2、二次点集分割：将激光采集数据按连续性分割成多个连续区域。二次点集分割算法的原理如图2所示。

①第一次点集分割：通过平滑拟合，将累计误差平摊，提高分割的准确性。

第一次点集分割的程序流程图如图3所示，N表示激光数据的数目，选取连续的2m+1个激光数据(对应第i个到第2m+i个激光数据，m取值范围为[30,80]，m值过小会导致遍历次数增加，影响算法实时性，m值过大，会影响分割准确性)，将前m个激光数据坐标取平均值得到前参考点A7的坐标，第m+1个激光点坐标作为当前点B 8的坐标，后m个激光数据坐标取平均值得到后参考点C 9的坐标，然后计算后参考点C、前参考点A和当前点B三个点形成的角度∠CAB 10(记为θ)，如果(的取值范围为适当的取值可以保证分割的准确性)，则判定当前点B已经接近充电房右角落4，将当前点B作为右分割点P_r 11。

同样的方法，可以得到左分割点P_l，

选取连续的2m+1个激光数据(对应第i个到第2m+i个激光数据，m取值范围为[30,80])，将前m个激光数据坐标取平均值得到前参考点D 15的坐标，第m+1个激光点坐标作为当前点E 16的坐标，后m个激光数据坐标取平均值得到后参考点F 17的坐标，然后计算后参考点F、前参考点D和当前点E三个点形成的角度∠FDE 18(记为α)，如果(的取值范围为)，则判定当前点E已经接近充电房左角落5，将当前点E作为左分割点P_l 19。

利用左右分割点对连续的激光数据进行第一次分割，即可得到三个激光数据：左激光数据集(对应充电房左边墙面)、中间激光数据集(对应充电房门和两个角落)和右激光数据集(对应充电房右边墙面)。

②第一次直线拟合：采用最小二乘法对左激光数据集和右激光数据集进行拟合，得到充电房左边墙面对应直线L₃(14)和右边墙面对应直线L₂(13)，对应直线表达式分别为：

L₂:a₂x'+b₂y'+c₂＝0

L₃:a₃x'+b₃y'+c₃＝0

其中，a₂、b₂、c₂为对应L₂的参数，通过对右激光数据集进行最小二乘直线拟合得到，a₃、b₃、c₃为对应L₃的参数，通过对左激光数据集进行拟合得到；L₂、L₃对应的直线表达式位于机器人坐标系X’O’Y’中。

③第二次点集分割：利用充电房几何特征，从中间激光数据集中剔除两个角落对应的激光数据。

第二次点集分割的程序流程图如图4所示，从右分割点P_r 11开始遍历中间激光数据集，利用中间激光数据集中的点到右边墙面对应直线L₂的距离

当d≥threshold(threshold为设置的距离阈值，大小略大于充电房右角落4的宽度，该宽度即充电房右角落在充电房门直线方向上的长度)时，认为当前激光数据点位于充电房门对应直线L₁，并将当前激光数据点作为充电房门直线L₁对应激光数据集的右端点P_re12；同样的方法，可以得到充电房门直线L₁对应激光数据集的左端点P_le 20，即，从左分割点P_l 19开始遍历中间激光数据集，利用中间激光数据集中的点到左边墙面对应直线L₃的距离

当d≥threshold(threshold为设置的距离阈值，大小略大于充电房左角落5的宽度，该宽度即充电房左角落在充电房门直线方向上的长度)时，认为当前激光数据点位于充电房门对应直线L₁，并将当前激光数据点作为充电房门直线L₁对应激光数据集的左端点P_le20。

利用左、右端点对中间激光数据集进行第二次分割，即可得到充电房门直线对应的激光数据集。

④第二次直线拟合：通过最小二乘法对充电房门直线对应的激光数据集进行拟合，得到充电房门对应直线L₁ 6，对应直线表达式为：

L₁:a₁x'+b₁y'+c₁＝0，

其中，，a₁、b₁、c₁为对应L₁的参数，通过对充电房门直线对应的激光数据集进行拟合得到；L₁对应的直线表达式位于机器人坐标系X’O’Y’中。

这样，通过二次点集分割，实现对激光数据的分割。

3、线特征匹配：将分割后的直线L₁,L₂,L₃按照充电房特征进行匹配，即判断直线L₁是否同时垂直于直线L₂和直线L₃，验证提取直线特征的准确性，如果同时垂直则匹配成功，如果匹配不成功，则重新获取激光数据，重新执行步骤1、2。

4、边缘特征获取：获取充电房边缘特征，即将步骤3中匹配成功的直线L₁,L₂,L₃作为充电房的边缘特征。

5、关键点获取：利用直线L₂、L₃以及同时垂直于直线L₂和直线L₃的直线L₁重构充电房模型，即通过步骤4得到的充电房边缘特征L₁,L₂,L₃，通过直线相交获取重构充电房模型的两个关键点(拐点，即，左边墙面对应直线L₃与充电房左角落的交点、右边墙面对应直线L₂与充电房右角落的交点)。

6、机器人定位：利用步骤5重构充电房模型的两个关键点与机器人之间相对位置，确定机器人位置。

机器人定位原理图如图5所示，坐标系XOY为充电房模型对应的全局坐标系，坐标系X’O’Y’为机器人坐标系，其中O’对应机器人位置，M、N对应步骤5获得的关键点，坐标系XOY中M、N的坐标分别为其中w，h对应重构充电房模型的宽和高，坐标系X’O’Y’中M、N的坐标分别为M(x_m,y_m),N(x_n,y_n)，过O’做直线MN的垂线，垂足为P，过M做Y’轴的垂线，垂足为Q，MN与Y’轴交点为R，确定机器人位置即求|MP|、|O′P|。

由M点坐标为(x_m,y_m)有，|MQ|＝-x_m,|O′Q|＝y_m，由直线L₁的斜率角可得∠QMN＝θ₁，|MR|＝-x_m/cosθ₁，|QR|＝-x_m tanθ₁

所以|MP|＝|MR|+|PR|＝y_m sinθ₁-x_m cosθ₁，

|O′P|＝|O′R|cosθ₁＝(|O′Q|-|QR|)cosθ₁＝y_m cosθ₁+x_m sinθ₁

所以机器人位置为：

偏转角度为θ₁，机器人需要转动角度θ₁进行位置调整，通常单次转动角度为θ₁的六分之一，通过多次调整来完成对偏差角度θ₁的校正。

这样，通过两次线性分割重构出充电房模型。然后利用重构充电房模型的关键点，实现机器人的精准定位。该方法首先通过快速高效的二次点集分割算法对连续的激光数据集进行分割，获取鲁棒性较高的三个边缘特征，然后对充电房模型进行重构，利用重构充电房模型对应的关键点对机器人进行定位，在保证定位准确性的同时，不直接依赖于充电房墙角拐点，对充电房建造工艺依赖较低。

同时，在保证机器人自主返回充电高成功率的基础上，添加机器人充电过程中干扰自动恢复的功能。通过对机器人充电状态进行监测，对于外界环境造成的充电中断进行自动恢复，提高机器人充电过程的效率和抗干扰能力。

自动充电方法的具体方案描述如下：

1.当机器人需要补充电力时，通过定位导航技术自动驶向充电房，通过发送指令打开房门，机器人进入充电房中；

2.进入充电房后，利用激光设备采集的激光数据，进行充电房内几何特征的检测和提取，进行直线拟合、分割以及关键点的提取，根据提取的直线和关键点，通过几何运算进行机器人局部定位，必要时结合视觉信息；

3.持续根据定位调整机器人位置和朝向，直到机器人定位在充电桩正前方处；

4.确定机器人位于正确位置后，伸出充电臂，插入到充电桩中；

5.检查机器人是否处于充电状态，若是自动充电完成，收回充电臂，否则，重新进行调整并检测；通过对机器人充电状态进行监测，对于外界环境造成的充电中断进行自动恢复，若充电过程中出现充电状态打断，判断机器人位姿是否发生变化，若是，退出充电桩进行重新调整，否则再次进行充电桩和机器人端的上电操作，有效提高机器人充电过程的效率和抗干扰能力。

6.充电完成后机器人自动退出充电桩，继续进行工作。