CN112561998A - 一种基于三维点云配准的机器人定位和自主充电方法 - Google Patents
一种基于三维点云配准的机器人定位和自主充电方法 Download PDFInfo
- Publication number
- CN112561998A CN112561998A CN202011487267.XA CN202011487267A CN112561998A CN 112561998 A CN112561998 A CN 112561998A CN 202011487267 A CN202011487267 A CN 202011487267A CN 112561998 A CN112561998 A CN 112561998A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- robot
- pose
- point
- registration
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 83
- 230000009466 transformation Effects 0.000 claims description 53
- 239000011159 matrix material Substances 0.000 claims description 20
- 238000007781 pre-processing Methods 0.000 claims description 6
- 238000001914 filtration Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 4
- 238000009826 distribution Methods 0.000 claims description 4
- 230000001174 ascending effect Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000007689 inspection Methods 0.000 description 4
- 210000001503 joint Anatomy 0.000 description 4
- 238000005520 cutting process Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000002045 lasting effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
- G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/12—Target-seeking control
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
- G06T7/337—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving reference images or patches
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20016—Hierarchical, coarse-to-fine, multiscale or multiresolution image processing; Pyramid transform
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于三维点云配准的机器人定位和自主充电方法,属于智能机器人技术领域。所述方法包括:通过三维激光雷达实时获取当前位置的三维点云;在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准,并获取最优配准结果;利用所述最优配准结果计算机器人当前位姿;根据机器人当前位姿,引导机器人到达充电点进行自动充电。本发明适用于基于三维激光的无轨式移动机器人自主充电,能够实现机器人充电过程的自主化和智能化。
Description
技术领域
本发明属于智能机器人技术领域,具体涉及一种基于三维点云配准的机器人定位和自主充电方法。
背景技术
国内变电站多为无人值守,电力巡检机器人代替人工巡检电力设备保障了电力系统持续稳定的运行,因此保证电力巡检机器人的持久续航尤为重要,目前电力巡检机器人多采用接触式充电,即机器人自主行移动至预定的充电位置,与充电桩完成对接,实现自主充电。而实现这一点的前提是机器人在充电房区域有着稳定且准确的定位信息。三维激光导航定位技术在室外有着较高的定位精度,但是机器人进入充电房后其定位会有产生较大的误差,且不稳定,无法满足自主充电的定位精度要求。
现有技术中,有的采用二维激光雷达水平扫描,获取充电桩及充电桩周围墙面的线状点云数据;将点云数据转换为二维坐标系下的点云坐标,构建点云数组;从点云数组中筛选出预选点云坐标;根据充电桩的宽度、充电桩的厚度和预选点云坐标,计算充电桩中点坐标,并计算充电桩中点坐标相对于机器人的角度;机器人根据充电桩中点坐标和充电桩中点坐标相对于机器人的角度,移动到充电桩进行充电。但是,使用二维激光采集点云信息是稀疏的,且在机器人靠近充电桩或者墙壁时,是采集不到正前方的点云信息的,这样在机器人靠近充电桩时就无法定位充电桩,同时通过线状的点云分析获得充电桩的边界点,存在一定的误差。
有的通过粗定位和精定位两步实现机器人在充电房的定位,粗定位中根据前后两帧三维激光投影到二维栅格平面上的二维点云数据,计算机器人的位姿,并根据里程计和惯性导航数据对计算出的位姿进行优化,获得机器人位姿,用于判断机器人是否到达预定位置;精定位中通过采集指定高度的有效三维激光数据,将此高度的激光数据投影到二维栅格平面,在栅格平面上利用角度大小寻找到充电房墙角点,利用墙角点将指定高度上的点云分成三类,然后分别对这三类点云拟合直线求取交点作为最终的墙角点,然后根据墙角点坐标计算机器人的坐标,实现机器人定位。但该方法对充电房的内部形状和布局有何特殊的规定,同时对地图也有精确的要求;当充电房墙体不平整,通过拟合直线求取交点作为墙角点就存在一定的误差,很可能就会导致机器人无法与充电触头对接,导致机器人停机。
发明内容
本发明的目的在于针对现有技术存在的不足或缺陷之一,提供一种基于三维点云配准的机器人定位和自主充电方法。
为实现上述目的,本发明采用如下技术方案:
一种基于三维点云配准的机器人定位和自主充电方法,包括以下步骤:
通过三维激光雷达实时获取当前位置的三维点云;
在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准,并获取最优配准结果;
利用所述最优配准结果计算机器人当前位姿;
根据机器人当前位姿,引导机器人到达充电点进行自动充电。
进一步地,所述三维模板点云的构建方法包括:
控制机器人从充电房外匀速进入充电房并到达充电点附近,在机器人行进的过程中利用三维激光雷达等时间间隔采集三维点云数据;
对采集的三维点云进行预处理;
根据预处理后的三维点云构建机器人位姿图,使用点云配准的方法获得机器人不同位姿之间的变换关系;
根据不同位姿间变换关系的约束对机器人位姿图进行优化,获得各个点云对应的最终位姿信息,从而完成三维模板点云的构建。
进一步地,所述根据预处理后的三维点云构建机器人位姿图,使用点云配准的方法获得机器人不同位姿之间的变换关系,包括:
对预处理后的三维点云帧,先提取奇数帧的点云,按照升序排列,再提取偶数帧的点云,按照降序排列,将两组点云按照奇数帧序列在前、偶数帧序列在后的顺序组合到一起,形成新的点云序列;
在新的点云序列中,使用点云配准的方法分别计算第i帧点云与第j帧点云间的变换关系Ti,j,其中,i<j≤min(i+3,n),n表示点云的帧数;
根据第i帧点云与第j帧点云间的变换关系Ti,j,计算各个点云对应的机器人位姿,计算公式如下:
根据各个点云对应的机器人位姿和第i帧机器人位姿到第j帧机器人位姿的变换关系,构建机器人位姿图。
进一步地,所述点云配准的方法,包括:
先采用正态分布变换的点云配准算法进行点云的粗配准,得到两匹配点云间的变换关系,再使用迭代最近点的点云配准方法实现点云的精配准,在精配准中使用粗配准得到的变换关系作为迭代最近点点云配准方法的初始值,获得最终的点云变换关系。
进一步地,所述机器人位姿图用(V,E)表示,V表示图的顶点,E表示图中连接节点的边,图中两顶点Vi和Vj的位姿间满足如下关系:
Vj=Vi·Ei,j
i<j≤min(i+3,n)
其中Vi和Vj分别表示机器人第i个和第j个位姿,n表示位姿图中顶点的个数,Ei,j表示位姿Vi和Vj间的变换关系,它的值由第i帧点云与第j帧点云间的变换关系Ti,j按照如下关系转换得到:
Ei,j=(Ti,j)-1;
所述根据不同位姿间变换关系的约束对机器人位姿图进行优化,获得各个点云对应的最终位姿信息,包括:
根据不同的边Ei,j计算顶点Vj的位姿值,顶点Vj的最终位姿值Vj *与根据不同边约束计算出的位姿值之间的误差ei,j表示如下:
将所有边约束产生的总误差作为优化的目标函数,表示如下:
进一步地,所述在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准,并获取最优配准结果,包括:
使用K近邻算法在三维模板点云位姿中搜索距离机器人上一时刻位姿最近的三个点;
将当前位置点云分别与这三个点对应的模板点云进行配准,分别获得当前点云到这三个模板点云的变换矩阵;
分别计算当前点云与这三个模板点云的相似度,取相似度最高的模板点云的配准结果作为最终的变换矩阵。
进一步地,所述利用所述最优配准结果计算机器人当前位姿,包括:
通过以下公式计算当前点云对应的位姿矩阵:
Xnow=Xt·Tnow,t
其中,Xnow表示当前点云对应的位姿矩阵,Xt表示最优配准结果对应的模板点云对应的位姿矩阵,Tnow,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帧点云间的变换关系Ti,j,其中,i<j≤min(i+3,n),n表示点云的帧数。由此可知,若当前帧为i,当i<=n-3时,分别计算当前帧i与其后连续三帧点云(i+1,i+2,i+3)的变换关系Ti,i+1,Ti,i+2和Ti,i+3,当i>n-3时,分别计算当前帧与其后续所有帧点云的变换关系;
根据第i帧点云与第j帧点云间的变换关系Ti,j,计算各个点云对应的机器人位姿,计算公式如下:
构建机器人位姿图(V,E),V表示图的顶点,E表示图中连接节点的边,如图3所示。顶点Vi和Vj分别表示机器人第i个和第j个位姿,以位姿Vi和Vj间的变换关系作为位姿图的边Ei,j,两顶点位姿间满足如下关系:
Vj=Vi·Ei,j (2)
i<j≤min(i+3,n) (3)
其中n表示位姿图中顶点的个数,Ei,j表示机器人不同位姿之间的变换关系,它的值由不同点云间的变换关系Ti,j按照如下关系转换:
Ei,j=(Ti,j)-1 (4)
(Ti,j)-1表示Ti,j的逆,Ti,j的值是通过两点云的配准获得。
在一具体实施方式中,第i帧点云与第j帧点云间的变换关系Ti,j使用点位配准的方法计算得到,具体为:
先采用正态分布变换(NDT)的点云配准算法进行点云的粗配准,得到两匹配点云间的变换关系T′i,j,再使用迭代最近点(ICP)的点云配准方法实现点云的精配准,在精配准中使用粗配准得到的变换关系作为ICP配准方法的初始值,获得最终的点云变换关系Ti,j。
1-4)根据不同位姿间变换关系的约束对机器人位姿图进行优化,获得各个点云对应的最终位姿信息,从而完成三维模板点云的构建。
在位姿图中,初始点和终点的位姿是已知的且不变的,图中其他顶点位姿的估计值由上一帧位姿根据上述点云变换关系Ti,j计算得到。同一顶点不同边约束计算出的位姿存在的误差ei,j如下所示:
步骤2、基于构建的三维点云模板对机器人进行定位。
定位方法包括:
判断机器人是否开启三维点云配准定位方法;
通过三维激光雷达获取当前位置的点云;
在构建的三维模板点云中搜索与当前点云距离最近的多个点云进行配准,并获取最优的配准结果;
利用配准结果来计算和发布机器人当前的位姿。
其中,判断机器人是否开启三维点云配准定位方法,包括:在充电房正前方设置一起始点,并规定机器人进入或者离开充电房区域都必须停靠所述起始点,当机器人到达所述起始点,且机器人下一目标为充电房区域的点,则开启三维点云配准定位方法。
在一具体实施方式中,如图4所示,机器人定位的具体步骤包括:
2-1)模板点云信息读入;
将制作好的三维点云模板读入工控机中,根据三维点云模板的位姿信息建立kd树,供后续搜索点云使用。
2-2)判断是否需要切换定位方式;
在自动充电过程中,机器人定位切换均在起始点完成,当机器人到达起始点,开始判断下一目标点是否在充电房区域,若是,则切换至点云配准定位方式,否则切换至其他定位方式。
2-3)实时点云采集与处理;
利用三维激光传感器获得实时的三维点云信息,并对点云进行裁剪和滤波。
2-4)点云配准。
机器人上一时刻的位姿为(xl,yl,θl),使用近邻算法在三维模板点云位姿中搜索距离(xl,yl,θl)最近的三个点(xi,yi,θi)、(xj,yj,θj)和(xk,yk,θk),将位姿坐标转换成矩阵形式Xi、Xj和Xk,转换方式如下所示:
将实时点云分别与这三个点对应的点云进行配准,配准方法:先使用NDT点云配准方法,获得当前位置点云到最近的三个模板点云的初始变换矩阵T′i、T′j和T′k,分别利用获得的初始变换矩阵作为ICP点云配准方法的初始值,对当前位置点云与模板点云进行配准,分别获得变换矩阵Ti、Tj和Tk;
统计当前位置点云与各个模板点云的对应点的对数,分别为ci、cj和ck,当前点云中点的个数为Cnow,则相似度ρ根据如下公式计算:
取相似度最高的点云的配准结果作为最终的变换矩阵Tnow,t,模板点云对应的位姿矩阵为Xt,则当前点云对应的位姿矩阵Xnow表示如下:
Xnow=Xt·Tnow,t (8)
根据机器人当前的位姿矩阵Xnow,计算出当前机器人在地图中的位姿坐标(xnow,ynow,θnow)。
步骤3、机器人自主充电。
根据机器人定位结果,引导机器人到达充电点进行自动充电。
具体的,如图5所示,机器人根据当前的定位信息,不断的规划自身的速度和方向,使机器人朝着充电点运动。当机器人到达充电点附近,且位姿与设定值相差在允许范围内,机器人开始缓慢向后倒退,准备与充电桩充电触头对接。当检测到充电电压后,机器人停止运动并开启充电,当倒退一定距离后没有检测到充电电压,机器人朝前运动一段距离后,重新开始向充电点运动,直到成功充电。
本发明的一种基于三维点云配准的机器人定位和自主充电方法,首先在充电房区域建立多个位置的三维点云模板,通过帧间匹配的方式获得每个模板点云的位姿,构造位姿图,使用图优化的方式对模板点云位姿进行处理,降低了帧间匹配的配准误差,获得最优的点云位姿。对模板点云位姿建立kd树,在机器人进入充电房到达充电位置的过程中,利用近邻算法搜索距离上一时刻机器位姿最近的三个模板点云,将实时点云分别与这三个模板点云进行粗配准和精配准,利用配准结果和模板点云的位姿计算当前机器人的位姿,实现机器人的定位。机器人根据定位信息进行路径和速度规划,直到到达充电位置,与充电桩对接完成充电任务。经测试,机器人的重复定位精度在3cm以内,定位信息发布频率40HZ,机器人与充电桩触片对接成功率在99.9%以上。
由以上实施例可以看出,本发明方法适用于基于三维激光的无轨式移动机器人自主充电,能够实现机器人充电过程的自主化和智能化。
以上已以较佳实施例公布了本发明,然其并非用以限制本发明,凡采取等同替换或等效变换的方案所获得的技术方案,均落在本发明的保护范围内。
Claims (10)
1.一种基于三维点云配准的机器人定位和自主充电方法,其特征在于,包括以下步骤:
通过三维激光雷达实时获取当前位置的三维点云;
在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准,并获取最优配准结果;
利用所述最优配准结果计算机器人当前位姿;
根据机器人当前位姿,引导机器人到达充电点进行自动充电。
2.根据权利要求1所述的方法,其特征在于,所述三维模板点云的构建方法包括:
控制机器人从充电房外匀速进入充电房并到达充电点附近,在机器人行进的过程中利用三维激光雷达等时间间隔采集三维点云数据;
对采集的三维点云进行预处理;
根据预处理后的三维点云构建机器人位姿图,使用点云配准的方法获得机器人不同位姿之间的变换关系;
根据不同位姿间变换关系的约束对机器人位姿图进行优化,获得各个点云对应的最终位姿信息,从而完成三维模板点云的构建。
3.根据权利要求2所述的方法,其特征在于,所述根据预处理后的三维点云构建机器人位姿图,使用点云配准的方法获得机器人不同位姿之间的变换关系,包括:
对预处理后的三维点云帧,先提取奇数帧的点云,按照升序排列,再提取偶数帧的点云,按照降序排列,将两组点云按照奇数帧序列在前、偶数帧序列在后的顺序组合到一起,形成新的点云序列;
在新的点云序列中,使用点云配准的方法分别计算第i帧点云与第j帧点云间的变换关系Ti,j,其中,i<j≤min(i+3,n),n表示点云的帧数;
根据第i帧点云与第j帧点云间的变换关系Ti,j,计算各个点云对应的机器人位姿,计算公式如下:
根据各个点云对应的机器人位姿和第i帧机器人位姿到第j帧机器人位姿的变换关系,构建机器人位姿图。
4.根据权利要求3所述的方法,其特征在于,所述点云配准的方法,包括:
先采用正态分布变换的点云配准算法进行点云的粗配准,得到两匹配点云间的变换关系,再使用迭代最近点的点云配准方法实现点云的精配准,在精配准中使用粗配准得到的变换关系作为迭代最近点点云配准方法的初始值,获得最终的点云变换关系。
5.根据权利要求3所述的方法,其特征在于,所述机器人位姿图用(V,E)表示,V表示图的顶点,E表示图中连接节点的边,图中两顶点Vi和Vj的位姿间满足如下关系:
Vj=Vi·Ei,j
i<j≤min(i+3,n)
其中Vi和Vj分别表示机器人第i个和第j个位姿,n表示位姿图中顶点的个数,Ei,j表示位姿Vi和Vj间的变换关系,它的值由第i帧点云与第j帧点云间的变换关系Ti,j按照如下关系转换得到:
Ei,j=(Ti,j)-1;
所述根据不同位姿间变换关系的约束对机器人位姿图进行优化,获得各个点云对应的最终位姿信息,包括:
根据不同的边Ei,j计算顶点Vj的位姿值,顶点Vj的最终位姿值Vj *与根据不同边约束计算出的位姿值之间的误差ei,j表示如下:
ei,j=Vj *-Vi·Ei,j
将所有边约束产生的总误差作为优化的目标函数,表示如下:
6.根据权利要求1所述的方法,其特征在于,所述在预先构建的充电房区域三维模板点云中搜索与当前点云距离最近的多个点云进行配准,并获取最优配准结果,包括:
使用K近邻算法在三维模板点云位姿中搜索距离机器人上一时刻位姿最近的三个点;
将当前位置点云分别与这三个点对应的模板点云进行配准,分别获得当前点云到这三个模板点云的变换矩阵;
分别计算当前点云与这三个模板点云的相似度,取相似度最高的模板点云的配准结果作为最终的变换矩阵。
7.根据权利要求1所述的方法,其特征在于,所述利用所述最优配准结果计算机器人当前位姿,包括:
通过以下公式计算当前点云对应的位姿矩阵:
Xnow=Xt·Tnow,t
其中,Xnow表示当前点云对应的位姿矩阵,Xt表示最优配准结果对应的模板点云对应的位姿矩阵,Tnow,t表示当前位置点云到最优配准结果对应的模板点云的变换矩阵;
根据当前点云对应的位姿矩阵,计算出当前机器人在地图中的位姿坐标。
8.根据权利要求1所述的方法,其特征在于,所述预处理包括点云裁剪、滤波。
9.根据权利要求1所述的方法,其特征在于,还包括:在实时获取三维点云前,判断机器人是否开启三维点云配准定位方法。
10.根据权利要求9所述的方法,其特征在于,所述判断机器人是否开启三维点云配准定位方法,包括:在充电房正前方设置一起始点,并规定机器人进入或者离开充电房区域都必须停靠所述起始点,当机器人到达所述起始点,且机器人下一目标为充电房区域的点,则开启三维点云配准定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011487267.XA CN112561998B (zh) | 2020-12-16 | 2020-12-16 | 一种基于三维点云配准的机器人定位和自主充电方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011487267.XA CN112561998B (zh) | 2020-12-16 | 2020-12-16 | 一种基于三维点云配准的机器人定位和自主充电方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112561998A true CN112561998A (zh) | 2021-03-26 |
CN112561998B CN112561998B (zh) | 2024-02-20 |
Family
ID=75064068
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011487267.XA Active CN112561998B (zh) | 2020-12-16 | 2020-12-16 | 一种基于三维点云配准的机器人定位和自主充电方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112561998B (zh) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113138596A (zh) * | 2021-03-31 | 2021-07-20 | 深圳市优必选科技股份有限公司 | 机器人自动充电方法、系统、终端设备及存储介质 |
CN113341396A (zh) * | 2021-05-28 | 2021-09-03 | 深圳市优必选科技股份有限公司 | 机器人及其充电桩识别方法和装置 |
CN113447949A (zh) * | 2021-06-11 | 2021-09-28 | 天津大学 | 一种基于激光雷达和先验地图的实时定位系统及方法 |
CN113504543A (zh) * | 2021-06-16 | 2021-10-15 | 国网山西省电力公司电力科学研究院 | 无人机LiDAR系统定位定姿系统及方法 |
CN114895686A (zh) * | 2022-05-27 | 2022-08-12 | 广州高新兴机器人有限公司 | 机器人对桩充电方法及系统 |
CN115525048A (zh) * | 2022-06-20 | 2022-12-27 | 达闼机器人股份有限公司 | 一种机器人自动对桩方法、装置、机器人及存储介质 |
Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106296693A (zh) * | 2016-08-12 | 2017-01-04 | 浙江工业大学 | 基于3d点云fpfh特征实时三维空间定位方法 |
US20170361468A1 (en) * | 2016-06-15 | 2017-12-21 | Irobot Corporation | Systems and methods to control an autonomous mobile robot |
CN108152831A (zh) * | 2017-12-06 | 2018-06-12 | 中国农业大学 | 一种激光雷达障碍物识别方法及系统 |
CN108830902A (zh) * | 2018-04-19 | 2018-11-16 | 江南大学 | 一种基于点云处理的散乱工件识别与定位方法 |
CN109345578A (zh) * | 2018-10-15 | 2019-02-15 | 深圳步智造科技有限公司 | 基于贝叶斯优化的点云配准方法、系统及可读存储介质 |
CN109623816A (zh) * | 2018-12-19 | 2019-04-16 | 中新智擎科技有限公司 | 一种机器人回充方法、装置、存储介质及机器人 |
CN110176071A (zh) * | 2019-05-06 | 2019-08-27 | 南京航空航天大学 | 一种基于特征模板的三维点云重构方法 |
CN110793437A (zh) * | 2019-10-23 | 2020-02-14 | 珠海格力智能装备有限公司 | 手操器的定位方法、装置、存储介质及电子设备 |
CN110927740A (zh) * | 2019-12-06 | 2020-03-27 | 合肥科大智能机器人技术有限公司 | 一种移动机器人定位方法 |
US20200276713A1 (en) * | 2019-02-28 | 2020-09-03 | Intelligrated Headquarters, Llc | Vision calibration system for robotic carton unloading |
CN111786465A (zh) * | 2020-06-23 | 2020-10-16 | 国网智能科技股份有限公司 | 一种变电站巡检机器人无线充电系统及方法 |
CN111915677A (zh) * | 2020-07-08 | 2020-11-10 | 哈尔滨工程大学 | 一种基于三维点云特征的船舶位姿估计方法 |
CN111932628A (zh) * | 2020-09-22 | 2020-11-13 | 深圳市商汤科技有限公司 | 一种位姿确定方法及装置、电子设备、存储介质 |
-
2020
- 2020-12-16 CN CN202011487267.XA patent/CN112561998B/zh active Active
Patent Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20170361468A1 (en) * | 2016-06-15 | 2017-12-21 | Irobot Corporation | Systems and methods to control an autonomous mobile robot |
CN106296693A (zh) * | 2016-08-12 | 2017-01-04 | 浙江工业大学 | 基于3d点云fpfh特征实时三维空间定位方法 |
CN108152831A (zh) * | 2017-12-06 | 2018-06-12 | 中国农业大学 | 一种激光雷达障碍物识别方法及系统 |
CN108830902A (zh) * | 2018-04-19 | 2018-11-16 | 江南大学 | 一种基于点云处理的散乱工件识别与定位方法 |
CN109345578A (zh) * | 2018-10-15 | 2019-02-15 | 深圳步智造科技有限公司 | 基于贝叶斯优化的点云配准方法、系统及可读存储介质 |
CN109623816A (zh) * | 2018-12-19 | 2019-04-16 | 中新智擎科技有限公司 | 一种机器人回充方法、装置、存储介质及机器人 |
US20200276713A1 (en) * | 2019-02-28 | 2020-09-03 | Intelligrated Headquarters, Llc | Vision calibration system for robotic carton unloading |
CN110176071A (zh) * | 2019-05-06 | 2019-08-27 | 南京航空航天大学 | 一种基于特征模板的三维点云重构方法 |
CN110793437A (zh) * | 2019-10-23 | 2020-02-14 | 珠海格力智能装备有限公司 | 手操器的定位方法、装置、存储介质及电子设备 |
CN110927740A (zh) * | 2019-12-06 | 2020-03-27 | 合肥科大智能机器人技术有限公司 | 一种移动机器人定位方法 |
CN111786465A (zh) * | 2020-06-23 | 2020-10-16 | 国网智能科技股份有限公司 | 一种变电站巡检机器人无线充电系统及方法 |
CN111915677A (zh) * | 2020-07-08 | 2020-11-10 | 哈尔滨工程大学 | 一种基于三维点云特征的船舶位姿估计方法 |
CN111932628A (zh) * | 2020-09-22 | 2020-11-13 | 深圳市商汤科技有限公司 | 一种位姿确定方法及装置、电子设备、存储介质 |
Non-Patent Citations (2)
Title |
---|
RONI PERMANA SAPUTRA: ""Casualty Detection from 3D Point Cloud Data for Autonomous Ground Mobile Rescue Robots"", 《2018 IEEE INTERNATIONAL SYMPOSIUM ON SAFETY, SECURITY, AND RESCUE ROBOTICS (SSRR)》 * |
孔令升: ""面向非规则目标的3D视觉引导抓取方法及系统研究"", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113138596A (zh) * | 2021-03-31 | 2021-07-20 | 深圳市优必选科技股份有限公司 | 机器人自动充电方法、系统、终端设备及存储介质 |
CN113341396A (zh) * | 2021-05-28 | 2021-09-03 | 深圳市优必选科技股份有限公司 | 机器人及其充电桩识别方法和装置 |
WO2022247137A1 (zh) * | 2021-05-28 | 2022-12-01 | 深圳市优必选科技股份有限公司 | 机器人及其充电桩识别方法和装置 |
CN113341396B (zh) * | 2021-05-28 | 2023-12-15 | 深圳市优必选科技股份有限公司 | 机器人及其充电桩识别方法和装置 |
CN113447949A (zh) * | 2021-06-11 | 2021-09-28 | 天津大学 | 一种基于激光雷达和先验地图的实时定位系统及方法 |
CN113447949B (zh) * | 2021-06-11 | 2022-12-09 | 天津大学 | 一种基于激光雷达和先验地图的实时定位系统及方法 |
CN113504543A (zh) * | 2021-06-16 | 2021-10-15 | 国网山西省电力公司电力科学研究院 | 无人机LiDAR系统定位定姿系统及方法 |
CN113504543B (zh) * | 2021-06-16 | 2022-11-01 | 国网山西省电力公司电力科学研究院 | 无人机LiDAR系统定位定姿系统及方法 |
CN114895686A (zh) * | 2022-05-27 | 2022-08-12 | 广州高新兴机器人有限公司 | 机器人对桩充电方法及系统 |
CN115525048A (zh) * | 2022-06-20 | 2022-12-27 | 达闼机器人股份有限公司 | 一种机器人自动对桩方法、装置、机器人及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN112561998B (zh) | 2024-02-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112561998A (zh) | 一种基于三维点云配准的机器人定位和自主充电方法 | |
CN109541997B (zh) | 一种面向平面/近似平面工件的喷涂机器人快速智能编程方法 | |
CN109410321B (zh) | 基于卷积神经网络的三维重建方法 | |
CN111598916A (zh) | 一种基于rgb-d信息的室内占据栅格地图的制备方法 | |
CN107514993A (zh) | 基于无人机的面向单体建筑建模的数据采集方法及系统 | |
CN115290097B (zh) | 基于bim的实时精确地图构建方法、终端及存储介质 | |
CN110487286B (zh) | 基于点特征投影与激光点云融合的机器人位姿判断方法 | |
CN110243380A (zh) | 一种基于多传感器数据与角度特征识别的地图匹配方法 | |
CN110908374B (zh) | 一种基于ros平台的山地果园避障系统及方法 | |
WO2021082380A1 (zh) | 一种基于激光雷达的托盘识别方法、系统和电子设备 | |
CN114061486A (zh) | 面向飞机大型蒙皮曲面的自动化测量装置及其测量方法 | |
CN110705385B (zh) | 一种障碍物角度的检测方法、装置、设备及介质 | |
CN110806585B (zh) | 一种基于树干聚类跟踪的机器人定位方法及系统 | |
CN113724387A (zh) | 一种激光与相机融合的地图构建方法 | |
CN106292656A (zh) | 一种环境建模方法及装置 | |
CN113406658A (zh) | 一种基于点线特征扫描匹配的移动机器人定位方法 | |
CN115294287A (zh) | 一种用于温室巡检机器人的激光slam建图方法 | |
CN116604212A (zh) | 一种基于面阵结构光的机器人焊缝识别方法和系统 | |
CN115032648A (zh) | 一种基于激光雷达密集点云的三维目标识别与定位方法 | |
CN117840995A (zh) | 基于两阶段视觉伺服的自动砌墙方法及系统 | |
CN115201849A (zh) | 一种基于矢量地图的室内建图方法 | |
CN109636897B (zh) | 一种基于改进RGB-D SLAM的Octomap优化方法 | |
CN112182122A (zh) | 一种移动机器人工作环境导航地图的获取方法及装置 | |
CN115469659A (zh) | 基于v型板识别和定位的自主移动机器人自动充电系统 | |
CN112348950A (zh) | 一种基于激光点云分布特性的拓扑地图节点生成方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |