CN114111787B - 一种基于三维路标的视觉定位方法及系统 - Google Patents

Abstract

本发明公开一种基于三维路标的视觉定位方法，包括以下步骤：S1、根据移动机器人的任务环境设置三维路标定位模块；S2、实时采集移动机器人前方行驶道路上的深度图像，并通过分割处理获得若干个子区块；S3、对子区块进行预处理，获取匹配路标排序，并对匹配路标进行编码；S4、基于路标编码获取移动机器人的当前位置和前进方向；S5、基于当前深度图像获取障碍物信息，并更新移动机器人的行驶状态；S6、综合前进方向和行驶状态的结果，优先满足S5中的行驶状态，并向S4中的前进方向运动。本发明通过在移动机器人行驶路径上放置三维路标，并利用深度相机采集道路图像，具有深度信息，能够精准判断障碍物，实现精准定位。

Description

一种基于三维路标的视觉定位方法及系统

技术领域

本发明涉及智能物流技术领域，特别涉及一种基于三维路标的视觉定位方法及系统。

背景技术

随着机器人技术的快速发展，移动机器人或AGV在仓储物流、智能巡检、移动操作等领域有着广泛的应用需求。在移动机器人自主执行任务时，其在环境中所处的位置信息是运动规划的关键。

目前，按照采用传感器的不同，机器人定位技术主要有磁导定位、二维码视觉定位、激光定位等方法。磁导定位方法一般采用地下铺设金属线缆或地上铺设磁带的方式，前者铺设工程较为繁琐，行驶线路固定，缺乏灵活性，后者则容易受到机械损坏且易受到铁磁物质的影响，需要定期维护；如中国专利文献CN201811315946.1公开了一种叉车式AGV磁导轨跟踪控制系统及方法；中国专利文献CN201710078154.6公开了一种磁场检测电路、磁钉检测装置及磁钉定位方法等。采用二维码视觉定位的方法通过视觉传感器扫描地面或墙壁上的二维码来获取位姿坐标信息，但二维码标签容易受到环境污染，影响识别效果，而且基于光学传感器的方法也容易受到环境光的干扰；如中国专利文献CN201711144865.5提出一种基于二维码的车辆定位系统和定位方法；如中国专利文献CN202011519953.0公开了一种基于QR码和惯性导航融合的车辆定位方法。基于激光的定位方法如今多采用激光SLAM的方法，通过激光传感器不断扫描环境深度信息并构建二维平面地图，这种方法的缺点是其高昂的成本限制，很难用于机器人数量较多的仓储场景；此外，激光传感器也容易受到温度、湿度变化的影响；如中国专利文献CN209927144U公开了一种基于激光SLAM的室内导航车；如中国专利文献CN201910655946.4公开了一种基于十字激光和机器视觉的机器人定位装置及方法。

由于移动机器人往往工作环境多变，因而需要适应特殊的环境条件和多种工作模式，同时，不同的用户由于已经提前构建了生成现场，但在现有技术中还不能在非常少的改变现场环境的条件下，实现准确、稳定的定位与导航。

发明内容

针对上述问题，本发明提出一种基于三维路标的视觉定位方法及系统，以解决现有技术中存在的技术问题，通过在移动机器人行驶路径上放置三维路标，实现精确定位；利用深度相机采集到的道路图像，具有深度信息，能够精准地判断障碍物，不存在检测盲区，更加安全可靠。

为实现上述目的，本发明提供了如下方案：

一种基于三维路标的视觉定位方法，包括以下步骤：

S1、根据移动机器人的任务环境设置三维路标定位模块，获取路标；

S2、实时采集所述移动机器人前方行驶道路上的深度图像，并对所述深度图像进行分割处理，获得若干个子区块；

S3、对所述子区块进行预处理，获取匹配路标排序，并对所述匹配路标进行编码，获取路标编码；

S4、基于所述路标编码获取所述移动机器人的当前位置，并基于所述当前位置获取所述移动机器人的前进方向；

S5、基于当前所述深度图像获取障碍物信息，并根据所述障碍物信息更新所述移动机器人的行驶状态；

S6、综合所述S4中的前进方向和所述S5中的行驶状态的结果，优先满足所述S5中的行驶状态，并向所述S4中的前进方向运动，完成所述移动机器人的视觉导航定位。

优选地，所述S1中的任务环境包括所述移动机器人的行驶环境和工厂安全行驶场景。

优选地，所述S1中，三维路标定位模块的主体为类三角柱体，所述柱体上下底面为等腰直角三角形，所述柱体上下底面固定有易识别标志物，侧面板上通过不同颜色或不同反射率材质及其组合进行编码。

优选地，所述S2中的深度图像采用3D深度视觉传感器获取。

优选地，所述S3中预处理包括以下步骤：

S3.1、通过描述处理获取描述点，并设置参考坐标系；

S3.2、以所述描述点的轴对齐边界盒为边界，将空间划分为若干个网格单元，并统计每个所述网格单元中所述描述点的个数，并将所述描述点的个数进行归一化和统计处理，获得特征直方图。

S3.3、计算所述特征直方图与数据库中的模板直方图之间的曼哈顿距离，基于所述曼哈顿距离获得若干个候选模板；

S3.4、设置场景路标，并将所述场景路标与所述描述点进行配准，获得内联点数量，并基于所述曼哈顿距离、内联点数量、若干个候选模板进行加权排序处理，获得匹配路标排序。

优选地，所述描述处理采用GASD描述符。

优选地，所述S3.3中的曼哈顿距离如下所示：

D_L1＝∑_i|H₁(i)-H₂(i)|

式中，D_L1表示曼哈顿距离；H₁表示特征直方图；H₂表示模板直方图；i表示直方图规模。

优选地，所述S4中当前位置的获取过程为：将所述路标编码与实际地图进行对应，获取所述移动机器人的当前位置。

一种基于三维路标的视觉定位系统，包括：三维路标定位模块、图像分割模块、匹配编码模块、方向定位模块、状态更新模块、定位导航模块；

所述三维路标定位模块用于根据移动机器人的任务环境设置三维路标定位模块，获取路标；

所述图像分割模块用于实时采集所述移动机器人前方行驶道路上的深度图像，并对所述深度图像进行分割处理，获得若干个子区块；

所述匹配编码模块用于对所述子区块进行预处理，获取匹配路标排序，并对所述匹配路标进行编码，获取路标编码；

所述方向定位模块用于基于所述路标编码获取所述移动机器人的当前位置，并基于所述当前位置获取所述移动机器人的前进方向；

所述状态更新模块用于基于当前所述深度图像获取障碍物信息，并根据所述障碍物信息更新所述移动机器人的行驶状态；

所述定位导航模块用于综合所述前进方向和行驶状态的结果，优先满足所述行驶状态，并向所述前进方向运动，完成所述移动机器人的视觉导航定位。

本发明公开了以下技术效果：

本发明解决了现有技术方法中安装复杂、难以全面覆盖、易受环境光干扰等技术难点，通过在移动机器人行驶路径上放置三维路标，实现精确定位；利用深度相机采集到的道路图像，具有深度信息，能够精准地判断障碍物，不存在检测盲区，更加安全可靠。

本发明结合具体实例概述了本发明的算法流程和实施方法，通过本发明所描述的方法，移动机器人不仅能够准确地识别前进方向的三维路标，精准定位当前位置，检测障碍物，实现安全可靠的自主导航；而且能够实现在复杂环境下的高可靠性，高精度运行，且运行方式灵活，抗干扰能力强，也适用于强磁、强辐射等特殊环境。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的方法流程图；

图2为本发明实施例中识别三维路标的应用示意图；

图3为本发明实施例中三维路标的设计示意图；

图4为本发明实施例中三维路标部署在不同路径上的示意图；

图5为本发明实施例的系统示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

参照图1-4所示，本实施例提供一种基于三维路标的视觉定位方法，包括以下步骤：

S1、根据移动机器人行驶环境以及工厂安全行驶的要求(场景)，在不同条件的场景中预置不同预编码的三维路标。在后续匹配算法中将对这些路标三维特征编码与数据库中的路标编码对比，用以获取并计算机器人的实时位置。

S1.1、三维路标主体1为类三角柱设计，如图2所示，柱体面板可选浅色金属或白色PVC等红外线反射率较好的材料，两侧面板夹角为90度。两个高反射率的球体2组成，上下底面三角形为等腰直角三角形，两个球体位于三角柱体上部和底部，位置固定。三角柱体高1m，地面等腰直角三角形腰长20cm，球体半径10cm。

S1.2、用于编码的条纹可以根据具体需要设定。条纹可以选用光谱吸收性较强的纯黑色碳板或黑色氯丁橡胶材质，这种设计的条纹特征可以在算法中处理为二进制的编码，或根据路标表面红外光反射强度的不同，三角锥体侧面可由多条不同反射率条纹组成，最上方条纹3反射率最低，最下方条纹4反射率最高，中间的条纹从上到下以3进制形式编码位置信息，对定位模块进行编号。

S1.3、移动机器人或AGV前方固定ToF深度相机，根据行驶环境的要求以及工厂安全行驶的要求，根据生成环境的具体条件灵活安置，如图3所示。在多叉路口，可将两个三维路标进行拼接组合起来，使得路标在各个路口皆可被有效探测到；在狭窄通道场景，单个路标可挂于墙体、立柱上；在开阔场地上，可以在合适距离布置多个包含位置信息的三维路标于地面。

S2、实时采集移动机器人前方行驶道路上的深度图像，并对深度图像进行分割处理，获得若干个子区块。其中，深度图像采用3D深度视觉传感器获取。

S2.1、3D深度视觉传感器安装在机器人前方，采集前方行驶道路上的深度图像。传感器发出特定频率的红外线，通过计算相位差来获得深度信息。

S2.2、采用一种自顶向下的分割算法，将点云场景中疑似3D路标的点云区块分割出来，分割的思路如下：

设点云与/>属于两个不同的区块，则满足：

min||p_i-p_j||₂≥d_th

其中，d_th为最大距离阈值，该式表示，如果一组点和另一组点/>之间的最小距离大于给定阈值，则/>被归为点云O_i，/>中的点被归为点云O_j。

S2.3、为了提高算法的鲁棒性，基于三维路标的先验知识，增加了区域分割的约束条件：

其中，minSize和MaxSize为子区块划分的点数阈值上下限，Amp(p_i)表示该点对应的红外反射光强，为p_i与y轴法向量的夹角。

加入先验知识后，分割的子区块数量减少，有利于提高系统后续的计算效率。

S3、对子区块进行预处理，获取匹配路标排序，并对匹配路标进行解码，获取路标的编码号。

S3.1、全局一致空间分布(GASD)基于目标点云的全局参考坐标系估计，将其与规范的坐标系对齐，使不同的点云具有姿态不变性。

参考坐标系的估计基于主成分分析法，对于一组目标点云计算其质心：

其中i∈{1,...,n}代表点云全部n个点的索引。

根据p_i和计算协方差矩阵C：

求出其特征值λ_j和相应的特征向量v_j，其中j∈{1,2,3}，即Cv_j＝λ_jv_j。将与最小特征值相关的特征向量v₁用于参考坐标系的z轴。坐标系的x轴是与最大特征值相关的特征向量v₃，y轴由v₂＝v₁×v₃得出。根据上述参考坐标系的估计，可以得出点云对齐到该坐标系下的刚体位姿变换[R|t]：

S3.2、以描述点的轴对齐边界盒(axis-aligned bounding box)为边界，将空间划分为m_s×m_s×m_s个规则的网格单元，并统计每个网格单元中描述点的个数，并将描述点的个数进行归一化和统计处理，获得m_s×m_s×m_s规模的特征直方图。

S3.3、计算特征直方图与数据库中的模板直方图之间的曼哈顿距离；将获取的直方图与数据库中的模板直方图进行相似度比较，相似度与曼哈顿距离成反比。在满足距离阈值条件的情况下，得到若干个候选基准点云。

曼哈顿距离如下所示：

式中，D_L1表示曼哈顿距离；H₁表示特征直方图；H₂表示模板直方图m直方图规模，即m_s×m_s×m_s。

S3.4、将场景路标点云进行配准，通过设定一个距离阈值，统计两幅配准点云之间的内联点数量。内联点数量占点云总数量的比重与GASD描述符的D_L1距离进行加权并作为排序指标对得到的若干个候选基准点云进行排序，并选出最优的识别目标作为当前找到的路标。

S4、基于路标编码获取移动机器人的当前位置，并基于当前位置获取移动机器人的前进方向。

S4.1、不同ID的路标在导航地图中的位置信息是已知的，如图3所示，ID为i的路标放置在行驶路线上，并且在地图中的坐标齐次形式为：m₀₀(x,y,z,1)。

考虑到移动机器人仅在二维地图中行驶，目标的姿态可以仅保留旋转姿态(即，世界坐标系下绕z轴旋转)，则有：

t_m＝(0,0,0)

θ_B表示模板路标点云采集时，ToF传感器与路标模板正前方向的夹角。路标正前方的视角经过变换T_m＝[R_m|t_m]得到基准数据库中的视角姿态，再经过T_f ^-1＝[R_f|t_f]^-1变换到实际场景拍摄的旋转姿态。此时，机器人与路标正前方向的夹角为θ。

机器人到路标质心的空间欧几里得距离可以用ToF传感器光心到路标表面的深度值d近似。于是可以估算出路标坐标到机器人坐标的新的几何变换Τ_R＝[R_R|t_R]，其中：

t_R＝(d·sin(θ),d·cos(θ),0)

即机器人在地图中的坐标位置为a(x',y',0,1)＝T_Rm₀₀。

S4.2、计算出当前位置到目标位置需要经过的定位模块，结合历史经过定位模块编号，计算将要前进的方向。

S5、基于当前深度图像获取障碍物信息，并根据障碍物信息更新移动机器人的行驶状态。

S6、综合S4中的前进方向和S5中的行驶状态的结果，在优先满足步骤S5避障的情况下，向满足步骤S4下一个定位模块位置的方向运动，通过以太网、串口等方式与机器人控制器进行通信，控制其运动方向与速度，完成移动机器人的视觉导航定位。

参照图5所示，本实施例提供一种基于三维路标的视觉定位系统，包括：三维路标定位编号模块、图像分割模块、匹配编码模块、方向定位模块、状态更新模块、定位导航模块；

三维路标定位编号模块用于根据移动机器人的任务环境设置三维路标定位模块，获取路标；

图像分割模块用于实时采集移动机器人前方行驶道路上的深度图像，并对深度图像进行分割处理，获得若干个子区块；

匹配编码模块用于对子区块进行预处理，获取匹配路标排序，并对匹配路标进行编码，获取路标编码；

方向定位模块用于基于路标编码获取移动机器人的当前位置，并基于当前位置获取移动机器人的前进方向；

状态更新模块用于基于当前深度图像获取障碍物信息，并根据障碍物信息更新移动机器人的行驶状态；

定位导航模块用于综合前进方向和行驶状态的结果，优先满足行驶状态，并向前进方向运动，完成移动机器人的视觉导航定位。

本发明公开了以下技术效果：

本发明解决了现有技术方法中安装复杂、难以全面覆盖、易受环境光干扰等技术难点，通过在移动机器人行驶路径上放置三维路标，能够精确定位；利用深度相机采集到的道路图像，具有深度信息，能够精准地判断障碍物，不存在检测盲区，更加安全可靠。

本发明结合具体实例概述了本发明的算法流程和实施方法，通过本发明所描述的方法，移动机器人不仅能够准确地识别前进方向的三维路标，精准定位当前位置，检测障碍物，实现安全可靠的自主导航；而且能够实现在复杂环境下的高可靠性，高精度运行，且运行方式灵活，抗干扰能力强，也适用于强磁、强辐射等特殊环境。

最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (8)

1.一种基于三维路标的视觉定位方法，其特征在于，包括以下步骤：

S1、根据移动机器人的任务环境设置三维路标定位模块，获取路标；

S2、实时采集所述移动机器人前方行驶道路上的深度图像，并对所述深度图像进行分割处理，获得若干个子区块；

S3、对所述子区块进行预处理，获取匹配路标排序，并对所述匹配路标进行编码，获取路标编码；

预处理包括以下步骤：

S3.1、通过描述处理获取描述点，并设置参考坐标系；

S3.2、以所述描述点的轴对齐边界盒为边界，将空间划分为若干个网格单元，并统计每个所述网格单元中所述描述点的个数，并将所述描述点的个数进行归一化和统计处理，获得特征直方图；

S3.3、计算所述特征直方图与数据库中的模板直方图之间的曼哈顿距离，基于所述曼哈顿距离获得若干个候选模板；

S3.4、设置场景路标，并将所述场景路标与所述描述点进行配准，获得内联点数量，并基于所述曼哈顿距离、内联点数量、若干个候选模板进行加权排序处理，获得匹配路标排序；

S4、基于所述路标编码获取所述移动机器人的当前位置，并基于所述当前位置获取所述移动机器人的前进方向；

S5、基于当前所述深度图像获取障碍物信息，并根据所述障碍物信息更新所述移动机器人的行驶状态；

S6、综合所述S4中的前进方向和所述S5中的行驶状态的结果，优先满足所述S5中的行驶状态，并向所述S4中的前进方向运动，完成所述移动机器人的视觉导航定位。

2.根据权利要求1所述的基于三维路标的视觉定位方法，其特征在于，所述S1中的任务环境包括所述移动机器人的行驶环境和工厂安全行驶场景。

3.根据权利要求1所述的基于三维路标的视觉定位方法，其特征在于，所述S1中，三维路标定位模块的主体为类三角柱体，所述柱体上下底面为等腰直角三角形，所述柱体上下底面固定有易识别标志物，侧面板通过不同颜色或不同反射率材质及其组合进行编码。

4.根据权利要求1所述的基于三维路标的视觉定位方法，其特征在于，所述S2中的深度图像采用3D深度视觉传感器获取。

5.根据权利要求1所述的基于三维路标的视觉定位方法，其特征在于，所述描述处理采用GASD描述符。

6.根据权利要求4所述的基于三维路标的视觉定位方法，其特征在于，所述S3.3中的曼哈顿距离如下所示：

D_L1＝Σ_i|H₁(i)-H₂(i)|

式中，D_L1表示曼哈顿距离；H₁表示特征直方图；H₂表示模板直方图；i表示直方图规模。

7.根据权利要求1所述的基于三维路标的视觉定位方法，其特征在于，所述S4中当前位置的获取过程为：将所述路标编码与实际地图进行对应，获取所述移动机器人的当前位置。

8.一种基于三维路标的视觉定位系统，其特征在于，包括：三维路标定位模块、图像分割模块、匹配编码模块、方向定位模块、状态更新模块、定位导航模块；

所述三维路标定位模块用于根据移动机器人的任务环境设置三维路标定位模块，获取路标；

所述图像分割模块用于实时采集所述移动机器人前方行驶道路上的深度图像，并对所述深度图像进行分割处理，获得若干个子区块；

所述匹配编码模块用于对所述子区块进行预处理，获取匹配路标排序，并对所述匹配路标进行编码，获取路标编码；

预处理包括以下步骤：

S3.1、通过描述处理获取描述点，并设置参考坐标系；

S3.2、以所述描述点的轴对齐边界盒为边界，将空间划分为若干个网格单元，并统计每个所述网格单元中所述描述点的个数，并将所述描述点的个数进行归一化和统计处理，获得特征直方图；

S3.3、计算所述特征直方图与数据库中的模板直方图之间的曼哈顿距离，基于所述曼哈顿距离获得若干个候选模板；

S3.4、设置场景路标，并将所述场景路标与所述描述点进行配准，获得内联点数量，并基于所述曼哈顿距离、内联点数量、若干个候选模板进行加权排序处理，获得匹配路标排序；

所述方向定位模块用于基于所述路标编码获取所述移动机器人的当前位置，并基于所述当前位置获取所述移动机器人的前进方向；

所述状态更新模块用于基于当前所述深度图像获取障碍物信息，并根据所述障碍物信息更新所述移动机器人的行驶状态；

所述定位导航模块用于综合所述前进方向和行驶状态的结果，优先满足所述行驶状态，并向所述前进方向运动，完成所述移动机器人的视觉导航定位。