CN109828280A - 一种基于三维激光栅格的定位方法以及机器人自主充电方法 - Google Patents

Abstract

本发明公开了一种基于三维激光栅格的定位方法以及机器人自主充电方法，属于智能机器人技术领域。本发明的定位方法用于机器人进入充电房进行充电时的定位，包括以下步骤：判断机器人是否已进入充电房的步骤；对机器人进行粗定位的步骤；对机器人进行精定位的步骤；确定机器人的坐标的步骤。采用本发明的方法，适用于无轨式导航定位中的机器人自主充电，实现整个充电过程的自动化、智能化；不论充电房墙壁是否平整，都可以具有很高的导航定位精度。

Description

一种基于三维激光栅格的定位方法以及机器人自主充电方法

技术领域

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

背景技术

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

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

中国专利CN201610324865公开了一种基于激光导航的变电站巡检机器人自主充电方法，先对激光数据进行直线拟合，然后选取四条符合要求的直线对激光数据进行分割，最后利用这四条直线获取两个墙角对应拐点，对机器人进行定位。

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

上述两种方法均存在以下问题：

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

(2)利用二维激光传感器采集的激光数据具有稀疏性的特点，影响直线的拟合精度，进而影响机器人的定位精度。

发明内容

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

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

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

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

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

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

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

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

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

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

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

由三维激光数据获得充电房左墙面、右墙面和充电房门在指定高度对应的直线；

确定机器人的坐标的步骤：

根据所述直线的交点获得关键点；

根据所述关键点与机器人间的相对位置确定机器人的坐标。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

score＝candidate_probability*candidate_weight

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

进一步而言，由三维激光数据获得充电房左、右墙面和充电房门在指定高度对应的直线的方法包括：

根据连续的三维激光数据计算分别位于充电房左墙角和右墙角的两个分割点；

根据所述两个分割点将连续区域内激光数据分割成三个激光数据集；

对所述三个激光数据集进行拟合，分别获得充电房左、右墙面和充电房门在指定高度对应的直线。

进一步而言，所述根据连续的三维激光数据计算分别位于充电房左墙角和右墙角的两个分割点的方法包括：

针对每个墙角，取连续的2m+1个激光数据，其中m为整数，取值范围为[30，80]；

设A为前参考点，B为当前点，C为后参考点，计算所述2m+1个激光数据中第1至m个激光数据相应的坐标平均值作为前参考点A的坐标，计算第m+2至2m+1个激光数据相应的坐标平均值作为后参考点C的坐标，将第m个激光数据的坐标作为当前点B的坐标；

计算角∠CAB，当角∠CAB大于指定值时，当前点B作为相应墙角的分割点。

进一步而言，对所述三个激光数据集进行拟合，获得充电房左、右墙面和充电房门在指定高度对应的直线的方法包括：

分别对所述三个激光数据集中连续的数据采用最小二乘法进行拟合。

进一步而言，所述机器人的坐标为机器人在全局坐标系中的坐标，为

其中，l为充电房的长度，w为充电房的宽度，设M为充电房左墙面和充电房门在指定高度对应的直线的交点，x_m为M在机器人坐标系中X轴坐标，y_m为M在机器人坐标系中Y轴坐标，θ为机器人的偏转角度，即机器人坐标系中Y轴相对于全局坐标系中Y轴的偏转角度。

另一方面，本发明还提供一种基于三维激光栅格进行机器人自主充电的方法，包括以下步骤：

1)利用上述基于三维激光栅格的定位方法，得到机器人的坐标和偏转角度；

2)机器人调整自己的偏转角度，使机器人两侧与左右两个关键点的中线保持平行；

3)机器人上位机发送指令给机器人，计算机器人自己到充电桩的距离并移动到预定充电位置，开始充电。

本发明的有益效果如下：本发明的基于三维激光栅格的定位方法以及机器人自主充电方法，采用基于三维激光栅格几何和数量特征的导航定位方法，在充电房正前方墙面、左边墙面和右面墙面分别建立栅格，激光投射到栅格上以后，通过识别激光栅格的几何特征，寻找充电房两个墙角关键点的位置信息，利用两个关键点实现机器人的准确定位，并引入置信度的概念，提高了机器人的定位精度，对充电房墙壁的平整性要求较低。利用三维激光点云数据，可以增加稀疏环境下激光数据的丰富性，从而减少定位的误差；对于有遮挡部分的物体，映射后依旧能够检测到该物体的反射数据，可以增加定位算法的健壮性和稳定性；通过将三维激光数据映射到二维平面，可以提高定位算法的运行效率。经测试，机器人的定位精度大大提高，误差在±0.5cm范围内，机器人充电连接触点与充电基座的供电触点对接成功率达到99.5％以上。

附图说明

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

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

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

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

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

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

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

图8是本发明实施例的点集分割原理示意图。

图9是本发明实施例的点集分割流程图。

图10是本发明实施例的机器人定位原理示意图。

图11是本发明实施例的机器人自主充电流程图。

图中的标号：1-充电房门，2-充电房左墙面，3-充电房右墙面，4-充电房左角落，5-充电房右角落，6-机器人。

具体实施方式

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

实施例1：

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

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

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

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

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

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

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

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

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

标准特征栅格的拐点：位于标准特征栅格的直角上的点，参见图4。

关键点：充电房内正前方墙面上位于栅格平面的两个墙角，在算法中用标准特征栅格的拐点来表示。

关键点坐标：关键点在充电房模型对应的全局坐标系中的坐标。

前提：现场工作人员已将充电房的尺寸数据、充电桩在充电房中的位置(机器人的目标位姿)录入机器人的工控机中。机器人内部有电量检测模块，当总电量低于一定值(例如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的距离，α、θ为该激光的水平方向角度值和竖直方向角度值。映射到平面z₀后为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＝z₀，α＝0，β＝0，其中z₀为三维激光传感器的安装高度。此时机器人的俯仰角度和倾斜角度为0，相当于机器人只能够在z＝z₀的高度上运动，只具有3个自由度。限定自由度后机器人位姿可表示为p(x_t，y_t，γ_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个栅格的地图坐标为(x_n，y_n)，该栅格置信度是取值范围为0.1～0.9。

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

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

将每个候选位姿的置信度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所示。L₁表示充电房左边墙面在二维激光栅格平面上对应的直线，L₂表示表示右边墙面在二维激光栅格平面上对应的直线，L₃表示激光充电房门在二维激光栅格平面上对应的直线，4表示充电房左角落，5表示充电房右角落，A为前参考点， B为当前点，C为后参考点，θ表示∠CAB。

结合图8、图9详细阐述点集分割的程序流程图，N表示激光数据的数目，选取连续的 2m+1个激光数据(对应第i个到第2m+i个激光数据，m取值范围为[30，80])，将前m个激光数据坐标取平均值得到前参考点A的坐标，第m+1个激光点坐标作为当前点B的坐标，后 m个激光数据坐标取平均值得到后参考点C的坐标，然后计算后参考点、前参考点和当前点三个点形成的角度θ，如果(的取值范围为)，判定当前点B已经接近充电房的右角落(5)，将当前点B作为右分割点P_r。同样的方法，可以得到左分割点P_L。利用左右分割点对连续的激光数据进行分割，即可得到三个激光数据集：左激光数据集(对应充电房左边墙面)、中间激光数据集(对应充电房门和两个角落)和右激光数据集(对应充电房右边墙面)。

3-3)直线拟合。

采用最小二乘法对左激光数据集、右激光数据集和中间激光数据集进行拟合，得到充电房左边墙面对应直线L₁、右边墙面对应直线L₂，充电房门对应直线L₃。直线L₁、L₂、L₃的表达式分别为：

L₁：α₁x+b₁y+c₁＝0 (4)

L₂：α₂x+b₂y+c₂＝0 (5)

L₃：α₃x+b₃y+c₃＝0 (6)

上述表达式为直线的一般表达式，以直线L₁为例，各参数的几何意义如下：

b₁≠0时，则(斜率)，(y轴上的截距)；

当b₁＝0，则(x轴上的截距)，此时不存在斜率。

4，确定机器人在全局坐标系XOY中的坐标。

机器人的定位原理如图10所示，其中，坐标系XOY为充电房模型所采用的全局坐标系；坐标系X’O’Y’为机器人坐标系，O’对应机器人的位置。M对应L₁与L₃的交点，N对应L₂与L₃的交点。

4-1)在坐标系XOY中，获取M、N两个关键点的坐标。L₁与L₃的交点即点M，L₂与L₃的交点即点N。因此，在坐标系XOY中，M的坐标为N的坐标为其中，1、 w分别对应充电房的长和宽。根据L₁、L₂与L₃的直线表达式，求出M、N在坐标系X’O’Y’中的坐标，分别为M(x_m，y_m)，N(x_n，y_n)。

4-2)确定机器人在全局坐标系XOY中的坐标。过O’点做直线MN的垂线，垂足为P；过M点做Y’轴的垂线，垂足为Q；直线MN与Y，轴的交点为R。那么，只需计算出|MP|和|O’P|即可算出机器人在全局坐标系XOY中的坐标。以下为计算|MP|和|O’P|的具体过程：

设|MQ|＝-x_m，|O’Q|＝y_m。过点N作X’轴的平行线，与Y’相交于点L，设∠MNL＝θ，

则

因此，∠QMN＝θ。

|MR|＝-x_m/cosθ，|QR|＝-x_mtanθ

所以，|MP|＝|MR|+|PR|＝y_msinθ-x_m cosθ，|O'P|＝|O'R|cosθ＝y_m cosθ+x_msinθ。因此，机器人在全局坐标系中的坐标为偏转角度为θ。

5，机器人根据定位信息进行自主充电。参见图11，前机器人根据实时的定位信息(坐标和偏转角度)，不断调整自己的偏转角度，使机器人两侧与左右两个关键点的中线保持平行，即将机器人的偏转角度θ调整到0°。然后，机器人上位机发送指令给机器人，计算机器人自已到充电桩的距离。若未到达预定位置(充电状态下的位置)，机器人缓慢后退，直至到达预定位置。充电桩的位置是已知的。机器人上位机确定机器人位于正确位置后，发送指令给机器人，机器人伸出充电臂，插入到充电桩中。机器人检测到充电臂与充电桩连接良好时，开始充电；否则，机器人上位机发送指令给机器人，收回充电臂，重新按照前述步骤调整机器人的位置和角度信息。

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

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

Claims (11)

1.一种基于三维激光栅格的定位方法，用于机器人进入充电房进行充电时的定位，其特征在于，包括以下步骤：

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

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

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

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

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

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

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

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

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

由三维激光数据获得充电房左墙面、右墙面和充电房门在指定高度对应的直线；

确定机器人的坐标的步骤：

根据所述直线的交点获得关键点；

根据所述关键点与机器人间的相对位置确定机器人的坐标。

2.根据权利要求1所述的基于三维激光栅格的定位方法，其特征在于，所述根据机器人的三维激光传感器扫描所述墙面所得三维激光数据获取标准特征栅格的方法包括：

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

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

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

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

3.根据权利要求2所述的基于三维激光栅格的定位方法，其特征在于，将三维栅格空间中的激光点云数据投射到二维激光栅格平面上包括，

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

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

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

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

6.根据权利要求1所述的基于激光对射的定位方法，其特征在于，所述计算各可能的候选位姿的置信度的具体方法为：

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

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

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

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

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

score＝candidate_probability*candidate_weight

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

7.根据权利要求1所述的基于三维激光栅格的定位方法，其特征在于，由三维激光数据获得充电房左、右墙面和充电房门在指定高度对应的直线的方法包括：

根据连续的三维激光数据计算分别位于充电房左墙角和右墙角的两个分割点；

根据所述两个分割点将连续区域内激光数据分割成三个激光数据集；

对所述三个激光数据集进行拟合，分别获得充电房左、右墙面和充电房门在指定高度对应的直线。

8.根据权利要求7所述的基于三维激光栅格的定位方法，其特征在于，所述根据连续的三维激光数据计算分别位于充电房左墙角和右墙角的两个分割点的方法包括：

针对每个墙角，取连续的2m+1个激光数据，其中m为整数，取值范围为[30，80]；

设A为前参考点，B为当前点，C为后参考点，计算所述2m+1个激光数据中第1至m个激光数据相应的坐标平均值作为前参考点A的坐标，计算第m+2至2m+1个激光数据相应的坐标平均值作为后参考点C的坐标，将第m个激光数据的坐标作为当前点B的坐标；

计算角∠CAB，当角∠CAB大于指定值时，当前点B作为相应墙角的分割点。

9.根据权利要求7所述的基于三维激光栅格的定位方法，其特征在于，对所述三个激光数据集进行拟合，获得充电房左、右墙面和充电房门在指定高度对应的直线的方法包括：

分别对所述三个激光数据集中连续的数据采用最小二乘法进行拟合。

10.根据权利要求1所述的基于三维激光栅格的定位方法，其特征在于，所述机器人的坐标为机器人在全局坐标系中的坐标，为

其中，l为充电房的长度，w为充电房的宽度，设M为充电房左墙面和充电房门在指定高度对应的直线的交点，x_m为M在机器人坐标系中X轴坐标，y_m为M在机器人坐标系中Y轴坐标，θ为机器人的偏转角度，即机器人坐标系中Y轴相对于全局坐标系中Y轴的偏转角度。

11.一种基于三维激光栅格的机器人自主充电的方法，其特征在于，包括以下步骤：

1)利用权利要求1～10任一所述的基于三维激光栅格的定位方法，得到机器人的坐标和偏转角度；

2)机器人调整自己的偏转角度，使机器人两侧与左右两个关键点的中线保持平行；

3)机器人上位机发送指令给机器人，计算机器人自己到充电桩的距离并移动到预定充电位置，开始充电。