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

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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