一种基于激光对射的定位方法以及机器人自主充电方法与流程

本发明属于智能机器人技术领域，具体涉及一种基于激光对射的定位方法以及机器人自主充电方法。

背景技术：

目前，变电站巡检机器人的自主充电多采用接触式自主充电技术。当巡检机器人需要充电时，会自动驶向指定充电区，机器人充电连接触点与充电基座的供电触点自动准确对接并实施充电。充电完成后巡检机器人自动与充电基座脱离并驶向工作区或待命区。

如何实现巡检机器人的定位导航是机器人实现自主充电的关键。目前常用的方案是基于磁轨道引导的导航定位方法。该方法实施的前提是铺设轨道，不仅带来了大量的基础设施建造工作，而且存在机器人行走路线不灵活的缺陷。鉴于此，激光导航定位技术被越来越多的应用到巡检机器人的导航定位作业过程中。激光导航定位技术摆脱了轨道限制，在节省成本的同时，机器人能够灵活地调整行走线路。

机器人在充电房场景中的定位方法多采用构建出充电房的集合信息，利用充电房的边缘特征或关键点的位置对机器人进行定位。例如，中国专利cn2016112609096公开了一种巡检机器人定位方法及自动充电方法，首先通过快速高效的二次点集分割算法对连续的激光数据集进行分割，获取鲁棒性较高的三个边缘特征，然后对充电房模型进行重构，利用重构充电房模型对应的关键点对机器人进行定位。在保证定位准确性的同时，不直接依赖于充电房墙角拐点，对充电房建造工艺依赖较低。

上述方法存在以下问题：

(1)如果充电房左右墙壁不平整，则会导致拟合直线出现偏差，影响充电房墙角拐点位置的准确性，使机器人的导航定位精度不理想，从而出现机器人充电连接触点与充电基座的供电触点对接失败的现象。采用上述方法进行机器人的导航定位及自主充电时，对充电房墙壁的平整性要求较高，机器人对环境的适应能力较差。

(2)采用二维传感器采集充电房的环境信息，检测范围小，数据量的信息有限，造成机器人导航定位精度不够理想难以满足实际场景的需求。

技术实现要素：

本发明目的是：针对现有技术的不足，提供一种基于激光对射的定位方法以及基于该定位方法的机器人自主充电方法。该方法适用于无轨式导航定位中的机器人自主充电，实现整个充电过程的自动化、智能化；不论充电房墙壁是否平整，都可以具有很高的导航定位精度。

一方面，本发明提供的基于激光对射的定位方法，用于机器人进入充电房进行充电时的定位，包括以下步骤：

判断机器人是否已进入充电房的步骤：

在充电房中机器人正前方墙面、左边墙面和右面墙面建立三维激光栅格；当根据机器人的三维激光传感器扫描所述墙面所得三维激光数据获取到标准特征栅格时，认为机器人已进入充电房；

对机器人进行粗定位的步骤：

在机器人向目标位姿移动的过程中将每一帧来自机器人的三维激光传感器的三维激光数据处理成激光点云数据后，将三维栅格空间中的激光点云数据投射到二维激光栅格平面上，根据所述二维平面上的连续两帧激光数据中位姿计算机器人位移速度和旋转速度，并通过前一帧位姿、位移速度和旋转速度计算下一帧的预测位姿初值；

根据最新接收的里程计数据和最新接收的惯性导航数据对预测位姿初值进行校正得到位姿的估计值；

计算定位扫描窗口大小，根据定位扫描窗口大小确定各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿；每得到一帧来自机器人的二维激光传感器的激光数据后，计算各扫描角度下各激光反射点在地图坐标系中的坐标；并结合地图数据计算各扫描角度下各激光反射点对应的地图栅格在地图坐标系中的坐标，作为各扫描角度的离散扫描数据；

计算各可能的候选位姿的置信度，选择置信度分值最高的位姿作为机器人最优位姿估计，即机器人当前位姿；

对机器人进行精定位的步骤：

根据所述机器人当前位姿和目标位姿判断机器人是否已到达距离目标位姿指定范围内；

搜索目标所在的激光发射器发出的激光引导信号；

当机器人上的激光接收器点阵中指定编号的激光接收器接收到激光引导信号时，机器人向着激光引导信号的方向移动，移动过程中保证激光接收器点阵正确地接收激光引导信号。

进一步而言，所述根据机器人的三维激光传感器扫描所述墙面所得三维激光数据获取标准特征栅格的方法包括：

使用三维激光传感器扫描所述墙面；

将三维栅格空间中的激光点云数据投射到二维激光栅格平面上；

在二维激光栅格平面上提取出水平方向和竖直方向相互垂直的连续灰色栅格；

校验提取的所述水平方向和竖直方向相互垂直的连续灰色栅格是否处于同一水平线上，满足条件的即为标准特征栅格；当不满足条件时，返回使用三维激光传感器扫描所述墙面的步骤，对下一帧激光数据进行上述处理。

进一步而言，将三维栅格空间中的激光点云数据投射到二维激光栅格平面上包括，

将三维栅格空间中的激光点云数据投射到激光中间线束所在的二维栅格平面上，得到机器人在全局坐标系x0y中坐标为p(x，y，z)，其中x，y，z的取值如下所示：

其中，点s为三维栅格空间中的某一激光点到点0的距离，α为该激光点相对于水平方向的角度，θ为该激光点相对于竖直方向的角度。

进一步而言，所述将三维栅格空间中的激光点云数据投射到二维激光栅格平面上之前，还包括：在每得到一帧激光数据后，先检查激光数据的帧率是否满足阈值的要求，如果小于阈值则表示不满足阈值的要求，此时不使用该激光数据并上报告警，等待接收到下一帧激光数据。

进一步而言，所述将三维栅格空间中的激光点云数据投射到二维激光栅格平面上之前，还包括：在每得到一帧激光数据后，如该激光数据的帧率满足阈值的要求，则先对该激光数据中的各激光反射点进行过滤，去掉各激光反射点中相距较近的点和较远的点，剩余的各激光反射点再用于后续的计算。

进一步而言，所述计算各可能的候选位姿的置信度的具体方法为：

根据每个候选位姿对应的扫描角度的离散扫描数据中各个地图栅格的置信度，计算每个候选位姿的置信度candidate_probability，公式如下：

其中，m为每个候选位姿对应的扫描角度的离散扫描数据中的地图栅格的总数，设第n个地图栅格的地图坐标为(xn，yn)，则该地图栅格置信度是

根据每个候选位姿与位姿的估计值的位姿差来计算每个候选位姿对应的置信度权重candidate_weight，公式如下：

其中，x_offset是每个候选位姿与位姿的估计值间沿x轴的位移，y_offset是每个候选位姿与位姿的估计值间沿y轴的位移，transweight是位移权重，candiate.rotation是候选位姿与位姿的估计值间旋转角度，rotweight是旋转角度权重；

将每个候选位姿的置信度candidate_probability与置信度权重candidate_weight的乘积作为当前位姿的置信度分值，公式如下：

score＝candidate_probability*candidate_weight

选择置信度分值score最高的位姿作为最优位姿估计。

进一步而言，所述移动过程中保证激光接收器点阵正确地接收激光引导信号的方法包括：

在机器人向目标位姿移动过程中，判断指定编号的激光接收器是否都接收到激光引导信号：若是，则机器人朝目标位姿运动；若否，则机器人调整位姿，使得指定编号的激光接收器接收到激光引导信号。

进一步而言，所述机器人调整位姿，使得指定编号的激光接收器接收到激光引导信号的步骤包括：

当位于指定编号的激光接收器一侧的激光接收器接收到激光引导信号时，机器人向一侧移动，使得指定编号的激光接收器都接收到激光引导信号；

当位于指定编号的激光接收器另一侧的激光接收器接收到激光引导信号时，机器人向另一侧移动，使得指定编号的激光接收器都接收到激光引导信号；

当接收到激光引导信号的两个激光接收器的编号间隔大于指定数值时，机器人调整自己的偏转角度，使得接收到激光引导信号的两个激光接收器的编号间隔为指定数值，之后搜索激光引导信号，重新进入判断指定编号的激光接收器是否都接收到激光引导信号的步骤。

进一步而言，所述激光发射器和激光接收器距离地面的高度相同，使得激光接收其能够接收激光发射器发出的水平方向的激光引导信号。

进一步而言，所述指定编号的激光发射器为2个，水平分布在目标两侧。

进一步而言，所述激光接收器点阵包含若干个水平均匀分布的激光接收器。

另一方面，本发明还提供一种基于三维激光栅格进行机器人自主充电的方法，利用上述基于激光对射的定位方法，使得机器人移动到充电桩的预定充电位置，开始充电。

本发明的有益效果如下：本发明的基于激光对射的定位方法以及机器人自主充电方法，利用三维激光传感器采集充电房的环境特征数据，利用激光栅格几何特征判断机器人是否进入充电房，确定机器人进入充电房后，通过来自三维激光传感器的连续两帧激光数据，计算机器人位移速度和旋转速度，并通过前一帧位姿、位移速度和旋转速度计算下一帧的预测位姿初值，根据最新接收的里程计数据和最新接收的惯性导航数据对预测位姿初值进行校正得到位姿的估计值；根据定位扫描窗口大小确定各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿；计算各可能的候选位姿的置信度，选择置信度分值最高的位姿作为机器人最优位姿估计，根据机器人最优位姿估计和预想设置的目标位姿，机器人不断调整的位置和偏转角度，当到达距离目标位置正前方指定范围内时，机器人的定位策略调整到精定位策略，利用激光对射开关对机器人进行精定位。机器人根据精定位的数据信息不断调整自己的位姿，直到到达指定位置，充电连接触点与充电基座的供电触点对接进行充电。经测试，机器人的定位精度大大提高，机器人充电连接触点与充电基座的供电触点对接成功率达到99％以上。

附图说明

图1是本发明实施例的定位算法流程图。

图2是本发明实施例的判断是否进入充电房流程图。

图3是本发明实施例的特征激光栅格示意图。

图4是本发明实施例的映射到平面后的栅格示意图。

图5是本发明实施例的三维激光点云数据映射示意图。

图6是本发明实施例的机器人粗定位流程图。

图7是本发明实施例的机器人精定位流程图。

图8是本发明实施例的激光发射器的示意图。

图9是本发明实施例的激光接收器的示意图。

图10是本发明实施例的激光接收器点阵正确接收引导信号示意图。

图中的标号：1-充电房门，2-充电房左墙面，3-充电房右墙面，4-充电房左角落，5-充电房右角落，6-机器人，7-激光发射器1，8-激光发射器2，9-激光接收器。

具体实施方式

下面结合实施例并参照附图对本发明作进一步详细描述。

实施例1：

本发明的一个实施例，介绍了一种基于三维激光栅格定位的变电站巡检机器人自主充电方法。

与本实施例相关的技术术语的定义如下：

栅格：大小均匀紧密相邻的网格阵列。

激光栅格：有激光照射的栅格。

灰色栅格：满足置信度要求的激光栅格。

栅格平面：在充电房正前方墙面、左边墙面和右面墙面的某个相同高度上建立栅格，形成的一个栅格平面。

连续灰色栅格：紧密相连的灰色栅格。

特征栅格：组成l形灰色栅格序列的栅格。

标准特征栅格：位于特征栅格序列的直角上的特征栅格，且在栅格平面上处于水平方向的同一条直线上。

前提：现场工作人员已将充电房的尺寸数据、充电桩在充电房中的位置(机器人的目标位姿)录入机器人的工控机中。在机器人上安装激光接收器，充电桩上安装激光发射器。机器人内部有电量检测模块，当总电量低于一定值(例如10％)时，判断机器人需要充电。当机器人需要补充电力时，通过定位导航技术自动驶向充电房，机器人上位机发送指令给充电房的电气柜，电气柜收到指令后将充电房门打开，机器人倒退着进入充电房中。进入充电房后，利用以下步骤所述的基于激光对射的定位方法，得到机器人的坐标信息和偏转角度信息。

基于激光对射的定位方法的整体流程如图1所示，具体实施步骤如下：

1，判断对机器人是否进入充电房。如图2所示，具体步骤如下：

1-1)利用三维激光传感器扫描充电房中机器人正前方墙面、左边墙面和右面墙面的信息。在充电房机器人正前方墙面、左边墙面和右面墙面分别建立边长为a的栅格，a∈[1cm，2cm]。如图3所示，每个栅格用s＝0或s＝1表示该格点是空闲还是占据状态。对于每一个能观察到的栅格，激光点云所落在的栅格标识为灰色，灰色栅格表示检测到存在障碍物，该栅格为被占据的状态；无激光点云的栅格标识为白色，白色栅格表示未检测到障碍物(例如充电房打开的房门)，该栅格为空闲状态。

1-2)将三维栅格空间中的激光点云数据投射到激光中间线束所在的平面上二维激光栅格平面上，在机器人坐标系中即x’o′y’平面上。如图4所示，灰色部分是激光点云映射到二维激光栅格平面后的栅格区域。如图5所示，用点s来表示三维空间中的某一激光点p到到原点0的距离，α、θ为该激光的水平方向角度值和竖直方向角度值。映射到平面z0后为p(x’，y’，z’)，其中x’，y’，z’的的取值如下公式所示。

对三维激光点云进行映射处理可以得到以下有益效果：

·利用三维激光点云数据，可以增加稀疏环境下激光数据的丰富性，从而减少定位的误差；

·对于有遮挡部分的物体，映射后依旧能够检测到该物体的反射数据，可以增加定位算法的健壮性和稳定性；

·通过将三维激光数据映射到二维平面，可以提高定位算法的运行效率。

1-3)在二维激光栅格平面上提取出水平方向和竖直方向相互垂直的连续灰色栅格，即成直角形状的栅格。满足要求的特征栅格如图4中左上角和右上角灰色栅格所示。

1-4)激光栅格特征校验：校验提取的两个特征栅格是否处于同一水平线上，满足条件的即为标准特征栅格，即表示机器人已经进入充电房。当不满足条件时，等待下一帧激光数据并返回步骤1-1)。

2，利用三维激光数据对机器人进行粗定位。

整个粗定位流程分为估算初值、定位扫描窗口、计算置信度三个阶段，结合图6对机器人粗定位的详细步骤进行描述。

2-1)估算初值

2-1-1)在三维空间中机器人6自由度的位姿可以表示为p(x，y，z，α，β，γ)，其中，x，y，z分别为机器人在全局坐标系xoy中x轴、y轴、z轴上的坐标，欧拉角α，β，γ分别对应绕三个坐标轴向的旋转角度。由于充电房内的地面光滑平坦，机器人的移动处于一个平面内。因此，可以限定机器人z轴坐标、欧拉角α，β三个自由度，使z＝z0，α＝0，β＝0，其中z0为三维激光传感器的安装高度。此时机器人的俯仰角度和倾斜角度为0，相当于机器人只能够在z＝z0的高度上运动，只具有3个自由度。限定自由度后机器人位姿可表示为p(xt，yt，γt)。

在机器人向目标位姿移动的过程中，每得到一帧来自机器人的三维激光传感器的激光数据后，对激光数据进行处理得到激光点云数据，该激光点云数据是每帧激光数据所包含的激光数据点信息的统称，反映了各激光反射点在激光传感器坐标系中的坐标。针对稀疏环境的特点，可以对激光点云数据进行过滤，即去掉激光点云数据中的各激光反射点中相应的噪点(相距较近的点和较远的点)，剩余作为有效点，可以提高定位估计的置信度。对于激光数据，要进行帧率的检测，即在每得到一帧激光数据后，检查激光数据的帧率是否小于阈值，如果小于阈值则上报告警并等待接收到下一帧激光数据后重新进行相应处理。例如，如果定义的帧率是25hz，而得到相邻两帧激光数据的时间大于40ms，则激光数据不符合要求，可能激光传感器出现过热等故障，需要上报告警并等待下一帧激光数据。

2-1-2)对于符合要求的激光点云数据进行平面映射，将三维栅格空间中的激光点云数据投射到激光中间线束所在的平面上二维栅格平面上，在机器人坐标系中即为x’o’y’平面。

2-1-3)用p(t-1)、p(t)、p(t+1)分别表示机器人在t-1、t、t+1时刻的位姿，通过p(t)和p(t-1)之间x轴、y轴方向的位移分别计算机器人沿x轴的速度velocity_x、沿y轴的速度velocity_y，通过p(t)和p(t-1)之间机器人绕z轴旋转角度的差值来计算机器人的旋转速度velocityγ。通过p(t)、velocity_x、velocity_y和velocity_γ计算p(t+1)，得到机器人在时间点t+1的预测位姿p(t+1)，称为预测位姿初值pose_prediction。

2-1-4)当获得里程计数据(包括位移和旋转角度)后，根据两帧里程计数据之间的差异，计算出里程计数据的校正值，用于优化p(t+1)。

2-1-5)根据惯性导航单元获得的线性加速度信息加速度校正当前的位移速度velocity_x和velocity_y，根据角加速度校正当前的旋转速度velocity_γ，将预测位姿初值pose_prediction更新为位姿的估计值pose_estimated。

2-2)定位扫描窗口

2-2-1)根据机器人位姿的估计值pose_estimated设置定位扫描窗口。定位窗口扫描时使用位移扫描参数linear_search_window和角度扫描参数，其中角度扫描参数包括扫描角度大小angular_search_window和扫描角度步长angular_step。位移扫描参数用于限定定位扫描窗口的位移范围为以位姿的估计值pose_estimated为中心，linear_search_window大小的正方形。角度扫描参数用于限定定位扫描窗口的角度范围为以位姿的估计值pose_estimated为中心角度，上下各偏离angular_search_window大小的角度。定位扫描窗口大小确定定位扫描窗口内各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿possible_candidates。

2-2-2)根据机器人的定位扫描窗口(通过位姿的估计值pose_estimated、角度扫描参数和位移扫描参数确定)和激光点云数据计算各扫描角度的激光点云数据的坐标。根据位姿的估计值pose_estimated和各扫描角度的激光点云数据坐标计算各扫描角度下各激光反射点对应的地图栅格位置(即计算每个地图栅格在地图坐标系中的坐标)，作为各扫描角度的离散扫描数据。对某个扫描角度的离散扫描数据而言，若其中有重复落在同一地图栅格位置的多个激光反射点，仅取其中一个激光反射点对应的地图栅格在地图坐标系中的坐标，用于后续步骤的置信度计算。

2-3)计算置信度。

根据每个候选位姿possible_candidate对应的各个地图栅格的置信度(地图栅格的置信度值与地图构建过程相关，在定位过程中，为已经确定的值)，计算每个候选位姿的置信度candidate_probability，公式如下：

其中，m为某个候选位姿对应的扫描角度的离散扫描数据中的地图栅格的总数，由于z＝z0计算位姿置信度的过程中不考虑z坐标的值，设其中第n个栅格的地图坐标为(xn，yn)，该栅格置信度是取值范围为0.1～0.9。

根据每个候选位姿possible_candidate与位姿的估计值pose_estimated的位姿差来计算每个候选位姿对应的置信度权重candidate_weight，公式如下：

其中，x_offset是每个候选位姿与位姿的估计值pose_estimated间沿x轴的位移，y_offset是每个候选位姿与位姿的估计值pose_estimated间沿y轴的位移，transweight是位移权重，candiate.rotation是候选位姿与位姿的估计值pose_estimated间旋转角度，rotweight是旋转角度权重；

将每个候选位姿的置信度candidate_probability与置信度权重candidate_weight的乘积作为当前位姿的置信度分值，公式如下，

score＝candidate_probability*candidate_weight(3)

选择置信度分值score最高的位姿更新位姿的估计值pose_estimated，作为最优位姿估计，即作为p(t+1)，粗定位结束。

3，对机器人进行精定位。如图7所示，具体流程如下：

3-1)根据预先设定的目标位姿，机器人不断调整的位置和偏转角度，确认是否到达距离目标位姿指定范围内，例如0.5m。

3-2)如图8所示，在充电桩上安装激光发射器，激光发射器1和激光发射器2水平分布在充电电极两侧。如图9和图10所示，在机器人背后安装激光对射式光电传感器的接收器点阵，从左到右的编号为1～10，激光接收器阵列水平均匀分布在充电卡槽上方，且发射器和接收器距离地面的高度相等。

机器人按照导航算法规定的路径行走，搜索激光引导信号。如图10所示，当机器人上的激光接收器点阵中编号为4和7的激光接收器均接收到激光引导信号时，机器人向着激光引导信号的方向后退，后退过程中保持激光接收器点阵正确的接收激光引导信号，机器人后退过程中按照如下步骤调整位姿。

3-2-1)移动机器人判断4号和7号激光接收器是否都接收到激光引导信号时，若是，则说明找到准确的位置，此时机器人后退，朝充电桩方向运动，否则机器人按照以下策略不断调整位姿，最终达到激光接收器4、激光接收器7均接收到激光引导信号。

·当编号为(1，4)或(2，5)或(3，6)的接收器接收到激光引导信号时，说明机器人偏在充电桩右方。机器人左移，使得4号和7号激光接收器都接收到激光引导信号。

·当编号为(5，8)或(6，9)或(7，10)的接收器接收到激光引导信号时，说明机器人偏在充电桩左方。机器人右移，使得4号和7号激光接收器都接收到激光引导信号。

·当接收到激光引导信号的两个接收器的编号间隔大于2时，说明机器人与两个激光发射器的连线间存在偏转角。机器人左右调整自己的偏转角度，使得接收到激光引导信号的两个接收器的编号间隔为2，之后按照步骤3-2-1)调整自己的位姿。

4，机器人朝着充电桩运动直到碰撞到充电电极后停止，机器人伸出充电臂，插入到充电桩中。机器人检测到充电臂与充电桩连接良好时，开始充电；否则，机器人上位机发送指令给机器人，收回充电臂，重新按照前述步骤调整机器人的位姿。

本发明的基于激光对射的自主充电方法，利用三维激光传感器采集充电房的环境特征数据，利用激光栅格几何特征判断机器人是否进入充电房，确定机器人进入充电房后，对机器人的位姿进行预测并结合里程计和惯性导航单元的数据对预测位姿进行校正，得到机器人位姿额估计值，根据机器人位姿估计值设定定位扫描窗口，计算定位扫描窗口内候选位姿的置信度分值，选取分值最高的候选位姿作为机器人的粗定位最优位姿。当机器人接近预先设定的目标位姿后，利用激光对射开关对机器人进行精定位。机器人根据精定位的数据信息不断调整自己的位姿，直到到达指定位置，充电连接触点与充电基座的供电触点对接进行充电。经测试，机器人的定位精度大大提高，机器人充电连接触点与充电基座的供电触点对接成功率达到99％以上。

虽然本发明已以较佳实施例公开如上，但实施例并不是用来限定本发明的。在不脱离本发明之精神和范围内，所做的任何等效变化或润饰，同样属于本发明之保护范围。因此本发明的保护范围应当以本申请的权利要求所界定的内容为标准。