CN112561998A - 一种基于三维点云配准的机器人定位和自主充电方法 - Google Patents

Abstract

本发明公开了一种基于三维点云配准的机器人定位和自主充电方法，属于智能机器人技术领域。所述方法包括：通过三维激光雷达实时获取当前位置的三维点云；在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准，并获取最优配准结果；利用所述最优配准结果计算机器人当前位姿；根据机器人当前位姿，引导机器人到达充电点进行自动充电。本发明适用于基于三维激光的无轨式移动机器人自主充电，能够实现机器人充电过程的自主化和智能化。

Description

一种基于三维点云配准的机器人定位和自主充电方法

技术领域

本发明属于智能机器人技术领域，具体涉及一种基于三维点云配准的机器人定位和自主充电方法。

背景技术

国内变电站多为无人值守，电力巡检机器人代替人工巡检电力设备保障了电力系统持续稳定的运行，因此保证电力巡检机器人的持久续航尤为重要，目前电力巡检机器人多采用接触式充电，即机器人自主行移动至预定的充电位置，与充电桩完成对接，实现自主充电。而实现这一点的前提是机器人在充电房区域有着稳定且准确的定位信息。三维激光导航定位技术在室外有着较高的定位精度，但是机器人进入充电房后其定位会有产生较大的误差，且不稳定，无法满足自主充电的定位精度要求。

现有技术中，有的采用二维激光雷达水平扫描，获取充电桩及充电桩周围墙面的线状点云数据；将点云数据转换为二维坐标系下的点云坐标，构建点云数组；从点云数组中筛选出预选点云坐标；根据充电桩的宽度、充电桩的厚度和预选点云坐标，计算充电桩中点坐标，并计算充电桩中点坐标相对于机器人的角度；机器人根据充电桩中点坐标和充电桩中点坐标相对于机器人的角度，移动到充电桩进行充电。但是，使用二维激光采集点云信息是稀疏的，且在机器人靠近充电桩或者墙壁时，是采集不到正前方的点云信息的，这样在机器人靠近充电桩时就无法定位充电桩，同时通过线状的点云分析获得充电桩的边界点，存在一定的误差。

有的通过粗定位和精定位两步实现机器人在充电房的定位，粗定位中根据前后两帧三维激光投影到二维栅格平面上的二维点云数据，计算机器人的位姿，并根据里程计和惯性导航数据对计算出的位姿进行优化，获得机器人位姿，用于判断机器人是否到达预定位置；精定位中通过采集指定高度的有效三维激光数据，将此高度的激光数据投影到二维栅格平面，在栅格平面上利用角度大小寻找到充电房墙角点，利用墙角点将指定高度上的点云分成三类，然后分别对这三类点云拟合直线求取交点作为最终的墙角点，然后根据墙角点坐标计算机器人的坐标，实现机器人定位。但该方法对充电房的内部形状和布局有何特殊的规定，同时对地图也有精确的要求；当充电房墙体不平整，通过拟合直线求取交点作为墙角点就存在一定的误差，很可能就会导致机器人无法与充电触头对接，导致机器人停机。

发明内容

本发明的目的在于针对现有技术存在的不足或缺陷之一，提供一种基于三维点云配准的机器人定位和自主充电方法。

为实现上述目的，本发明采用如下技术方案：

一种基于三维点云配准的机器人定位和自主充电方法，包括以下步骤：

通过三维激光雷达实时获取当前位置的三维点云；

在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准，并获取最优配准结果；

利用所述最优配准结果计算机器人当前位姿；

根据机器人当前位姿，引导机器人到达充电点进行自动充电。

进一步地，所述三维模板点云的构建方法包括：

控制机器人从充电房外匀速进入充电房并到达充电点附近，在机器人行进的过程中利用三维激光雷达等时间间隔采集三维点云数据；

对采集的三维点云进行预处理；

根据预处理后的三维点云构建机器人位姿图，使用点云配准的方法获得机器人不同位姿之间的变换关系；

根据不同位姿间变换关系的约束对机器人位姿图进行优化，获得各个点云对应的最终位姿信息，从而完成三维模板点云的构建。

进一步地，所述根据预处理后的三维点云构建机器人位姿图，使用点云配准的方法获得机器人不同位姿之间的变换关系，包括：

对预处理后的三维点云帧，先提取奇数帧的点云，按照升序排列，再提取偶数帧的点云，按照降序排列，将两组点云按照奇数帧序列在前、偶数帧序列在后的顺序组合到一起，形成新的点云序列；

在新的点云序列中，使用点云配准的方法分别计算第i帧点云与第j帧点云间的变换关系T_i,j，其中，i＜j≤min(i+3,n)，n表示点云的帧数；

根据第i帧点云与第j帧点云间的变换关系T_i,j，计算各个点云对应的机器人位姿，计算公式如下：

其中V_i和V_j分别表示机器人第i帧和第j帧在世界坐标系中的位姿；

是T_i,j的逆，表示第i帧机器人位姿到第j帧机器人位姿的变换关系，它的值由T_i,j转换得到：

根据各个点云对应的机器人位姿和第i帧机器人位姿到第j帧机器人位姿的变换关系，构建机器人位姿图。

进一步地，所述点云配准的方法，包括：

先采用正态分布变换的点云配准算法进行点云的粗配准，得到两匹配点云间的变换关系，再使用迭代最近点的点云配准方法实现点云的精配准，在精配准中使用粗配准得到的变换关系作为迭代最近点点云配准方法的初始值，获得最终的点云变换关系。

进一步地，所述机器人位姿图用(V,E)表示，V表示图的顶点，E表示图中连接节点的边，图中两顶点V_i和V_j的位姿间满足如下关系：

V_j＝V_i·E_i,j

i＜j≤min(i+3,n)

其中V_i和V_j分别表示机器人第i个和第j个位姿,n表示位姿图中顶点的个数，E_i,j表示位姿V_i和V_j间的变换关系，它的值由第i帧点云与第j帧点云间的变换关系T_i,j按照如下关系转换得到：

E_i,j＝(T_i,j)^-1；

所述根据不同位姿间变换关系的约束对机器人位姿图进行优化，获得各个点云对应的最终位姿信息，包括：

根据不同的边E_i,j计算顶点V_j的位姿值，顶点V_j的最终位姿值V_j ^*与根据不同边约束计算出的位姿值之间的误差e_i,j表示如下：

将所有边约束产生的总误差作为优化的目标函数，表示如下：

其中目标函数

表示所有边产生误差二范数的和，ε表示所有边的集合；

通过使目标函数

最小化，获得所有顶点的最终位姿。

进一步地，所述在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准，并获取最优配准结果，包括：

使用K近邻算法在三维模板点云位姿中搜索距离机器人上一时刻位姿最近的三个点；

将当前位置点云分别与这三个点对应的模板点云进行配准，分别获得当前点云到这三个模板点云的变换矩阵；

分别计算当前点云与这三个模板点云的相似度，取相似度最高的模板点云的配准结果作为最终的变换矩阵。

进一步地，所述利用所述最优配准结果计算机器人当前位姿，包括：

通过以下公式计算当前点云对应的位姿矩阵：

X_now＝X_t·T_now,t

其中，X_now表示当前点云对应的位姿矩阵，X_t表示最优配准结果对应的模板点云对应的位姿矩阵，T_now,t表示当前位置点云到最优配准结果对应的模板点云的变换矩阵；

根据当前点云对应的位姿矩阵，计算出当前机器人在地图中的位姿坐标。

进一步地，所述预处理包括点云裁剪、滤波。

进一步地，所述的一种基于三维点云配准的机器人定位和自主充电方法，还包括：在实时获取三维点云前，判断机器人是否开启三维点云配准定位方法。

进一步地，所述判断机器人是否开启三维点云配准定位方法，包括：在充电房正前方设置一起始点，并规定机器人进入或者离开充电房区域都必须停靠所述起始点，当机器人到达所述起始点，且机器人下一目标为充电房区域的点，则开启三维点云配准定位方法。

与现有技术相比，本发明所达到的有益技术效果：本发明的一种基于三维点云配准的机器人定位和自主充电方法，采用基于三维点云配准的方式定位机器人，在充电房区域建立多个位置的三维点云模板，通过帧间匹配的方式获得每个模板点云的位姿，构造位姿图，并使用图优化的方式对点云位姿进行处理，降低了帧间匹配的配准误差，提高了模板点云位姿的精度。在机器人进入充电房到达充电位置的过程中，通过不断地将实时三维点云与模板点云配准，获得机器人的实时位姿。配准过程中，采用了粗配准和精配准的策略，提高了配准的精度，同时对模板点云构建了kd(K-dimension)树，加速了近邻算法搜索最优模板点云的速度，提高了配准的速度，保证了机器人定位的实时性。本发明方法适用于基于三维激光的无轨式移动机器人自主充电，能够实现机器人充电过程的自主化和智能化。

附图说明

图1是本发明实施例的一种基于三维点云配准的机器人定位和自主充电方法的整体流程图；

图2是三维模板点云制作流程图；

图3是机器人位姿图；

图4是机器人定位流程图；

图5是机器人自主充电流程图。

具体实施方式

下面结合具体实施例对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

首先，给出与本发明相关的技术术语的定义：

点云：是在同一空间参考系下表达目标空间分布和目标表面特性的众多点的集合；

点云配准：通过平移、旋转使得两个点云尽可能重合的过程。

本发明方法实施的前提：现场工作人员已经完成充机器人服务区域的建图工作，在充电房周围划定区域作为充电房区域，在充电房区域外机器人能够实现定位。机器人内部具有运行逻辑，当机器人无巡检任务或者电量低于设定值(例如30％)时，机器人通过定位导航技术自行移动至充电房前的起始点，利用本发明的基于三维点云配准的机器人定位和自主充电方法实现机器人定位和自主充电。

在本发明一实施例中，一种基于三维点云配准的机器人定位和自主充电方法，包括以下步骤：

通过三维激光雷达实时获取当前位置的三维点云；

在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准，并获取最优配准结果；

利用所述最优配准结果计算机器人当前位姿；

根据机器人当前位姿，引导机器人到达充电点进行自动充电。

在本发明具体实施例中，如图1所示，一种基于三维点云配准的机器人定位和自主充电方法，具体实施步骤如下：

步骤1、制作充电房区域三维点云模板。

如图2所示，具体步骤包括：

1-1)控制机器人从充电房外匀速进入充电房并到达充电点附近，在机器人行进的过程中利用三维激光雷达等时间间隔采集三维点云数据；

具体的，手动控制机器人从充电房前的起始点倒退进入充电房并到达充电点附近，在行进过程中尽量保持匀速直线运动，同时利用三维激光传感器等时间间隔获取行进过程中充电房区域的三维点云数据，共计n帧点云数据。

1-2)对采集的三维点云进行预处理；

预处理包括点云裁剪、滤波等。

点云裁剪：将起始点设定在充电房中线附近且离充电房的距离约为充电房长度。保证所有采集的点云有效包含充电房，则点云宽度不小于充电房的宽度，机器人后方的点云大于充电房长度，机器人前方点云长度大于充电房2倍长度，假设充电房长度为l，宽度为w，则按如下规则对点云坐标进行约束，不满足要求的直接删除。

点云滤波：使用边长为5cm的正方体将每个点云空间划分成多个小方格，然后计算落在每个小正方体中点的重心，这个重心作为新的点，代替正方体中其他的点。

1-3)根据预处理后的三维点云构建机器人位姿图，使用点云配准的方法获得机器人不同位姿之间的变换关系构建位姿图；

该步骤具体包括：

将预处理后的点云进行排序，首先提取奇数帧的点云，按照升序排列，再提取偶数帧的点云，按照降序排列。将两组点云按照奇数帧序列在前，偶数帧序列在后的顺序组合到一起,，形成一个新的点云序列；

在新的点云序列中，分别计算第i帧点云与第j帧点云间的变换关系T_i,j，其中，i＜j≤min(i+3,n)，n表示点云的帧数。由此可知，若当前帧为i，当i<＝n-3时，分别计算当前帧i与其后连续三帧点云(i+1,i+2,i+3)的变换关系T_i,i+1,T_i,i+2和T_i,i+3，当i>n-3时，分别计算当前帧与其后续所有帧点云的变换关系；

根据第i帧点云与第j帧点云间的变换关系T_i,j，计算各个点云对应的机器人位姿，计算公式如下：

其中V_i和V_j分别表示机器人第i帧和第j帧在世界坐标系中的位姿，

是T_i,j的逆，表示第i帧机器人位姿到第j帧机器人位姿的变换关系；

构建机器人位姿图(V,E)，V表示图的顶点，E表示图中连接节点的边，如图3所示。顶点V_i和V_j分别表示机器人第i个和第j个位姿,以位姿V_i和V_j间的变换关系作为位姿图的边E_i,j，两顶点位姿间满足如下关系：

V_j＝V_i·E_i,j (2)

i＜j≤min(i+3,n) (3)

其中n表示位姿图中顶点的个数，E_i,j表示机器人不同位姿之间的变换关系，它的值由不同点云间的变换关系T_i,j按照如下关系转换：

E_i,j＝(T_i,j)^-1 (4)

(T_i,j)^-1表示T_i,j的逆，T_i,j的值是通过两点云的配准获得。

在一具体实施方式中，第i帧点云与第j帧点云间的变换关系T_i,j使用点位配准的方法计算得到，具体为：

先采用正态分布变换(NDT)的点云配准算法进行点云的粗配准，得到两匹配点云间的变换关系T′_i,j，再使用迭代最近点(ICP)的点云配准方法实现点云的精配准，在精配准中使用粗配准得到的变换关系作为ICP配准方法的初始值，获得最终的点云变换关系T_i,j。

1-4)根据不同位姿间变换关系的约束对机器人位姿图进行优化，获得各个点云对应的最终位姿信息，从而完成三维模板点云的构建。

在位姿图中，初始点和终点的位姿是已知的且不变的，图中其他顶点位姿的估计值由上一帧位姿根据上述点云变换关系T_i,j计算得到。同一顶点不同边约束计算出的位姿存在的误差e_i,j如下所示：

在整个位姿优化过程中，所有边的约束产生的总误差，即优化的目标函数

如下所示：

其中目标函数

表示所有边产生误差二范数的和，ε表示所有边的集合。

通过使用列文伯格-马夸尔特或者高斯牛顿法使目标函数

最小化，即可得出所有顶点的位姿，完成模板点云的构建。

步骤2、基于构建的三维点云模板对机器人进行定位。

定位方法包括：

判断机器人是否开启三维点云配准定位方法；

通过三维激光雷达获取当前位置的点云；

在构建的三维模板点云中搜索与当前点云距离最近的多个点云进行配准，并获取最优的配准结果；

利用配准结果来计算和发布机器人当前的位姿。

其中，判断机器人是否开启三维点云配准定位方法，包括：在充电房正前方设置一起始点，并规定机器人进入或者离开充电房区域都必须停靠所述起始点，当机器人到达所述起始点，且机器人下一目标为充电房区域的点，则开启三维点云配准定位方法。

在一具体实施方式中，如图4所示，机器人定位的具体步骤包括：

2-1)模板点云信息读入；

将制作好的三维点云模板读入工控机中，根据三维点云模板的位姿信息建立kd树，供后续搜索点云使用。

2-2)判断是否需要切换定位方式；

在自动充电过程中，机器人定位切换均在起始点完成，当机器人到达起始点，开始判断下一目标点是否在充电房区域，若是，则切换至点云配准定位方式，否则切换至其他定位方式。

2-3)实时点云采集与处理；

利用三维激光传感器获得实时的三维点云信息，并对点云进行裁剪和滤波。

2-4)点云配准。

机器人上一时刻的位姿为(x_l,y_l,θ_l),使用近邻算法在三维模板点云位姿中搜索距离(x_l,y_l,θ_l)最近的三个点(x_i,y_i,θ_i)、(x_j,y_j,θ_j)和(x_k,y_k,θ_k)，将位姿坐标转换成矩阵形式X_i、X_j和X_k，转换方式如下所示：

将实时点云分别与这三个点对应的点云进行配准，配准方法：先使用NDT点云配准方法，获得当前位置点云到最近的三个模板点云的初始变换矩阵T′_i、T′_j和T′_k，分别利用获得的初始变换矩阵作为ICP点云配准方法的初始值，对当前位置点云与模板点云进行配准，分别获得变换矩阵T_i、T_j和T_k；

统计当前位置点云与各个模板点云的对应点的对数，分别为c_i、c_j和c_k,当前点云中点的个数为C_now，则相似度ρ根据如下公式计算：

取相似度最高的点云的配准结果作为最终的变换矩阵T_now,t，模板点云对应的位姿矩阵为X_t，则当前点云对应的位姿矩阵X_now表示如下：

X_now＝X_t·T_now,t (8)

根据机器人当前的位姿矩阵X_now，计算出当前机器人在地图中的位姿坐标(x_now,y_now,θ_now)。

步骤3、机器人自主充电。

根据机器人定位结果，引导机器人到达充电点进行自动充电。

具体的，如图5所示，机器人根据当前的定位信息，不断的规划自身的速度和方向，使机器人朝着充电点运动。当机器人到达充电点附近，且位姿与设定值相差在允许范围内，机器人开始缓慢向后倒退，准备与充电桩充电触头对接。当检测到充电电压后，机器人停止运动并开启充电，当倒退一定距离后没有检测到充电电压，机器人朝前运动一段距离后，重新开始向充电点运动，直到成功充电。

本发明的一种基于三维点云配准的机器人定位和自主充电方法，首先在充电房区域建立多个位置的三维点云模板，通过帧间匹配的方式获得每个模板点云的位姿，构造位姿图，使用图优化的方式对模板点云位姿进行处理，降低了帧间匹配的配准误差，获得最优的点云位姿。对模板点云位姿建立kd树，在机器人进入充电房到达充电位置的过程中，利用近邻算法搜索距离上一时刻机器位姿最近的三个模板点云，将实时点云分别与这三个模板点云进行粗配准和精配准，利用配准结果和模板点云的位姿计算当前机器人的位姿，实现机器人的定位。机器人根据定位信息进行路径和速度规划，直到到达充电位置，与充电桩对接完成充电任务。经测试，机器人的重复定位精度在3cm以内，定位信息发布频率40HZ，机器人与充电桩触片对接成功率在99.9％以上。

由以上实施例可以看出，本发明方法适用于基于三维激光的无轨式移动机器人自主充电，能够实现机器人充电过程的自主化和智能化。

以上已以较佳实施例公布了本发明，然其并非用以限制本发明，凡采取等同替换或等效变换的方案所获得的技术方案，均落在本发明的保护范围内。

Claims (10)

1.一种基于三维点云配准的机器人定位和自主充电方法，其特征在于，包括以下步骤：

通过三维激光雷达实时获取当前位置的三维点云；

在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准，并获取最优配准结果；

利用所述最优配准结果计算机器人当前位姿；

根据机器人当前位姿，引导机器人到达充电点进行自动充电。

2.根据权利要求1所述的方法，其特征在于，所述三维模板点云的构建方法包括：

控制机器人从充电房外匀速进入充电房并到达充电点附近，在机器人行进的过程中利用三维激光雷达等时间间隔采集三维点云数据；

对采集的三维点云进行预处理；

根据预处理后的三维点云构建机器人位姿图，使用点云配准的方法获得机器人不同位姿之间的变换关系；

根据不同位姿间变换关系的约束对机器人位姿图进行优化，获得各个点云对应的最终位姿信息，从而完成三维模板点云的构建。

3.根据权利要求2所述的方法，其特征在于，所述根据预处理后的三维点云构建机器人位姿图，使用点云配准的方法获得机器人不同位姿之间的变换关系，包括：

对预处理后的三维点云帧，先提取奇数帧的点云，按照升序排列，再提取偶数帧的点云，按照降序排列，将两组点云按照奇数帧序列在前、偶数帧序列在后的顺序组合到一起，形成新的点云序列；

在新的点云序列中，使用点云配准的方法分别计算第i帧点云与第j帧点云间的变换关系T_i,j，其中，i＜j≤min(i+3,n)，n表示点云的帧数；

根据第i帧点云与第j帧点云间的变换关系T_i,j，计算各个点云对应的机器人位姿，计算公式如下：

其中V_i和V_j分别表示机器人第i帧和第j帧在世界坐标系中的位姿；

是T_i,j的逆，表示第i帧机器人位姿到第j帧机器人位姿的变换关系，它的值由T_i,j转换得到：

根据各个点云对应的机器人位姿和第i帧机器人位姿到第j帧机器人位姿的变换关系，构建机器人位姿图。

4.根据权利要求3所述的方法，其特征在于，所述点云配准的方法，包括：

先采用正态分布变换的点云配准算法进行点云的粗配准，得到两匹配点云间的变换关系，再使用迭代最近点的点云配准方法实现点云的精配准，在精配准中使用粗配准得到的变换关系作为迭代最近点点云配准方法的初始值，获得最终的点云变换关系。

5.根据权利要求3所述的方法，其特征在于，所述机器人位姿图用(V,E)表示，V表示图的顶点，E表示图中连接节点的边，图中两顶点V_i和V_j的位姿间满足如下关系：

V_j＝V_i·E_i,j

i＜j≤min(i+3,n)

其中V_i和V_j分别表示机器人第i个和第j个位姿,n表示位姿图中顶点的个数，E_i,j表示位姿V_i和V_j间的变换关系，它的值由第i帧点云与第j帧点云间的变换关系T_i,j按照如下关系转换得到：

E_i,j＝(T_i,j)^-1；

所述根据不同位姿间变换关系的约束对机器人位姿图进行优化，获得各个点云对应的最终位姿信息，包括：

根据不同的边E_i,j计算顶点V_j的位姿值，顶点V_j的最终位姿值V_j ^*与根据不同边约束计算出的位姿值之间的误差e_i,j表示如下：

e_i,j＝V_j ^*-V_i·E_i,j

将所有边约束产生的总误差作为优化的目标函数，表示如下：

其中目标函数

表示所有边产生误差二范数的和，ε表示所有边的集合；

通过使目标函数

最小化，获得所有顶点的最终位姿。

6.根据权利要求1所述的方法，其特征在于，所述在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准，并获取最优配准结果，包括：

使用K近邻算法在三维模板点云位姿中搜索距离机器人上一时刻位姿最近的三个点；

将当前位置点云分别与这三个点对应的模板点云进行配准，分别获得当前点云到这三个模板点云的变换矩阵；

分别计算当前点云与这三个模板点云的相似度，取相似度最高的模板点云的配准结果作为最终的变换矩阵。

7.根据权利要求1所述的方法，其特征在于，所述利用所述最优配准结果计算机器人当前位姿，包括：

通过以下公式计算当前点云对应的位姿矩阵：

X_now＝X_t·T_now,t

其中，X_now表示当前点云对应的位姿矩阵，X_t表示最优配准结果对应的模板点云对应的位姿矩阵，T_now,t表示当前位置点云到最优配准结果对应的模板点云的变换矩阵；

根据当前点云对应的位姿矩阵，计算出当前机器人在地图中的位姿坐标。

8.根据权利要求1所述的方法，其特征在于，所述预处理包括点云裁剪、滤波。

9.根据权利要求1所述的方法，其特征在于，还包括：在实时获取三维点云前，判断机器人是否开启三维点云配准定位方法。

10.根据权利要求9所述的方法，其特征在于，所述判断机器人是否开启三维点云配准定位方法，包括：在充电房正前方设置一起始点，并规定机器人进入或者离开充电房区域都必须停靠所述起始点，当机器人到达所述起始点，且机器人下一目标为充电房区域的点，则开启三维点云配准定位方法。

Images