一种基于点云语义地图的定位方法、装置及机器人与流程

1.本发明涉及机器人技术领域，具体来说，涉及一种基于点云语义地图的定位方法、装置及机器人。


背景技术：

2.自主移动机器人要求机器人能实现自主寻径行走能力，实现这一能力前提是机器人知道自己所在位置。因此自主移动机器人的定位技术是近年来研究热点技术之一。目前广泛应用在自主导航移动机器人的定位方式主要为基于点云地图的激光定位方式或者融合gps提高室外的定位鲁棒性。
3.但单独基于点云地图的激光定位，在动态物体(比如车多)的情况下，容易出现定位精度下降甚至位置丢失；此外，在空旷处可以融合gps定位方式，但同时带来成本的上升，也不能在园区类似有遮挡的场景内使用。
4.本文提供的背景描述用于总体上呈现本公开的上下文的目的。除非本文另外指示，在该章节中描述的资料不是该申请的权利要求的现有技术并且不要通过包括在该章节内来承认其成为现有技术。


技术实现要素：

5.针对相关技术中的上述技术问题，本发明提出一种基于点云语义地图的定位方法，其包括如下步骤：
6.s1，获取机器人实时获取的实时图像；
7.s2，使用目标检测算法识别所述实时图像中的目标物；
8.s3，从语义地图中语义信息表，获取与所述目标物对应的语义；
9.s4，将所述目标物对应的语义的全部点云根据定位结果进行重投影处理获取重投影后的图像；
10.s5，由所述重投影后的图像与实时获取的目标物构建定位结果优化代价函数；
11.s6，获取轮式里程计给出定位结果[r|t]的粗值，利用牛顿-高斯法不断调整定位结果[r|t]值，直至定位结果优化代价函数err([r|t])达到最小，得到最优的定位结果[r|t]。
[0012]
具体的，所述步骤s2中的目标检测算法是yolo。
[0013]
具体的，所述语义信息表中记录了语义的类别，位置，大小。
[0014]
具体的，所述定位结果优化代价函数为：具体的，所述定位结果优化代价函数为：
[0015]
其中pixel(u
ai
,v
ai
)表示获取语义a中第i个点云的像素值，因为点云语义每个点都已经带有颜色；pixel(u
ai
,v
ai
)表示获取图像目标物a中第i个点云的像素值。
[0016]
第二方面，本发明的另一个实施例公开了一种基于点云语义地图的定位装置，其
包括如下单元：
[0017]
实时图像获取单元，用于获取机器人实时获取的实时图像；
[0018]
目标物检测单元，用于使用目标检测算法识别所述实时图像中的目标物；
[0019]
语义获取单元，用于从语义地图中语义信息表，获取与所述目标物对应的语义；
[0020]
重投影单元，用于将所述目标物对应的语义的全部点云根据定位结果进行重投影处理获取重投影后的图像；
[0021]
定位结果优化代价函数构建单元，用于使用所述重投影后的图像与实时获取的目标物构建定位结果优化代价函数；
[0022]
迭代优化单元，用于获取轮式里程计给出定位结果[r|t]的粗值，利用牛顿-高斯法不断调整定位结果[r|t]值，直至定位结果优化代价函数err([r|t])达到最小，得到最优的定位结果[r|t]。
[0023]
具体的，所述目标物检测单元中的目标检测算法是yolo。
[0024]
具体的，所述语义信息表中记录了语义的类别，位置，大小。
[0025]
具体的，所述定位结果优化代价函数为：具体的，所述定位结果优化代价函数为：
[0026]
其中pixel(u
ai
,v
ai
)表示获取语义a中第i个点云的像素值，因为点云语义每个点都已经带有颜色；pixel(u
ai
,v
ai
)表示获取图像目标物a中第i个点云的像素值。
[0027]
第三方面，本发明的另一个实施例公开了一种非易失性存储器，所述存储器上存储有指令，所述指令被处理器执行时，用于实现如上所述的一种基于点云语义地图的定位方法。
[0028]
第四方面，本发明的另一个实施例公开了一种机器人，其包括一视觉传感器，一底盘，还包括如上所述的基于点云语义地图的定位装置。
[0029]
本发明使用图像神经网络算法来识别实时图像中目标物，能鲁棒地检测目标物；此外，本发明利用点云语义进行定位(点云语义一般为一些固定物体)，避免受到动态物体的干扰。
附图说明
[0030]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0031]
图1是本发明实施例提供的一种基于点云语义地图的定位方法流程图；
[0032]
图2是本发明实施例提供的点云地图与点云语义地图示意图；
[0033]
图3是本发明实施例提供的一种基于点云语义地图的定位装置示意图；
[0034]
图4是本发明实施例提供的一种基于点云语义地图的定位设备示意图。
具体实施方式
[0035]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完
整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。
[0036]
实施例一
[0037]
参考图1，本实施例公开了一种基于点云语义地图的定位方法，其包括如下步骤：
[0038]
参考图2，图2(a)是一点云地图，其中在图2(a)左上是一个门，图2(b)是点云语义地图，在图2(b)左上角对应的门使用绿色来表示。点云语义地图是在点云地图上进行改造，一是对每个点云进行赋予实际颜色，使点云地图更真实反映实际场景，二是人工在点云地图进行了特定目标物(即点云语义)的标记，记为语义信息表，其中语义信息表包括语义是属于什么，语义的大小，语义包括的全部点。
[0039]
本实施例的语义一般是固定的物体，例如门，建筑物等可以避免动态物体的干扰。
[0040]
本实施例的机器人搭载激光雷达或毫米波雷达，一视觉传感器，其中视觉传感器用于实时获取图像，激光雷达或毫米波雷达用于获取点云数据，更为具体的本实施例的机器人还包括一处理单元，一底盘，其中处理单元可以接收指令或者根据激光雷达或毫米波雷达的数据，控制机器人的底盘进行自主行走。本领域技术人员知晓，本实施例的机器人还包括电源单元，用于为机器人提供电力，具体的电源单元可以是锂离子电池等。更为一般的情况，本实施例还包括其他用于实现机器人功能的部件，本实施例不在赘述。
[0041]
一般的，本实施例的机器人实现对待行走的区域建立了语义地图，其语义地图可以是使用slam算法进行点云地图，并使用人工的方式在点云语义地图上对特定物体进行颜色的标注和语义信息表的标注以成点云语义地图。
[0042]
具体的，对点云地图的特定物体的识别可以完全由人工来完成，例如将点云地图显示出来，由人工对点云地图中的特定目标物进行语义的标注。在另一个实施方式可以使用基于深度学习的目标检测算法从点云地图中识别出特定目标物，并在点云地图中标注出特定目标物的位置，从而辅助人工生成语义信息表。
[0043]
yolo是一种图像目标检测算法，在2016年被提出，发表在计算机视觉顶会cvpr(computer vision and pattern recognition)上，yolo的全称是you only look once，指只需要浏览一次就可以识别出图中的物体的类别和位置。本实施例可以使用yolo对点云地图实行目标识别。
[0044]
s1，获取机器人实时获取的实时图像；
[0045]
本实施例的实时图像由视觉传感器获得。
[0046]
s2，使用目标检测算法识别所述实时图像中的目标物；
[0047]
本实施例使用yolo对点云地图进行目标检测，yolo可以输出检测到的目标物的类别以及在图像中的位置。将所述实时图像输入到目标检测算法(yolo)中，检测出目标物a、b、c。本实施例以检测出三个目标物为例进行说明。
[0048]
s3，从语义地图中语义信息表，获取与所述目标物对应的语义；
[0049]
具体的，本实施例在机器人启动时，会自动加载事先建立好的点云语义地图。
[0050]
具体的，根据识别出的目标物a、b、c的类别，从所述语义地图的语义信息表中查找与所述目标物a、b、c的类别对应的语义信息a、b、c。
[0051]
具体的语义信息表中记录了语义的类别，位置，大小，颜色等。
[0052]
s4，将所述目标物对应的语义的全部点云根据定位结果进行重投影处理获取重投影后的图像；
[0053]
定位结果用[r|t]3x4的矩阵表示，其中包含了旋转信息r和平移信息t。
[0054]
式(1)是[r|t]表达式，是待求的量。
[0055][0056]
所述目标物对应的语义a、b、c全部点云进行重投影处理，以语义a为例；假设语义a里面包含n个点云p＝[x y z]
t
[0057][0058][0059][0060]
其中下标ai表述语义a里面第i个点云；上标1、2表示中间变量用于区分；其中f
x
，fy，u0，v0是相机的内参，出厂固有参数，fx，fy为焦距，u0，v0为主点坐标(相对于成像平面)，u
ai
，v
ai
为语义a里面第i个点云在图像像素坐标系的大小值。
[0061]
s5，由所述重投影后的图像与实时获取的目标物构建定位结果优化代价函数；
[0062]
由点云语义地图(ab c)与实时图像关联好的语义目标(a b c)，构建变量优化代价函数：
[0063][0064]
其中pixel(u
ai
,v
ai
)表示获取语义a中第i个点云的像素值，因为点云语义每个点都已经带有颜色；pixel(u
ai
,v
ai
)表示获取图像目标物a中第i个点云的像素值；
[0065]
s6、获取轮式里程计给出定位结果[r|t]的粗值，利用牛顿-高斯法不断调整定位结果[r|t]值，直至定位结果优化代价函数err([r|t])达到最小，得到最优的定位结果[r|t]；
[0066]
通过轮式里程计给出[r|t]的粗值，利用牛顿-高斯法不断调整[r|t]值，直至err([r|t])达到最小，得到最优的[r|t]；
[0067]
本实施例使用图像神经网络算法来识别实时图像中目标物，能鲁棒地检测目标
物；此外，本实施例利用点云语义进行定位(点云语义一般为一些固定物体)，避免受到动态物体的干扰。
[0068]
实施例二
[0069]
参考图2，本实施例公开了一种基于点云语义地图的定位装置，其包括如下单元：
[0070]
参考图2，图2(a)是一点云地图，其中在图2(a)左上是一个门，图2(b)是点云语义地图，在图2(b)左上角对应的门使用绿色来表示。点云语义地图是在点云地图上进行改造，一是对每个点云进行赋予实际颜色，使点云地图更真实反映实际场景，二是人工在点云地图进行了特定目标物(即点云语义)的标记，记为语义信息表，其中语义信息表包括语义是属于什么，语义的大小，语义包括的全部点。
[0071]
本实施例的语义一般是固定的物体，例如门，建筑物等可以避免动态物体的干扰。
[0072]
本实施例的机器人搭载激光雷达或毫米波雷达，一视觉传感器，其中视觉传感器用于实时获取图像，激光雷达或毫米波雷达用于获取点云数据，更为具体的本实施例的机器人还包括一处理单元，一底盘，其中处理单元可以接收指令或者根据激光雷达或毫米波雷达的数据，控制机器人的底盘进行自主行走。本领域技术人员知晓，本实施例的机器人还包括电源单元，用于为机器人提供电力，具体的电源单元可以是锂离子电池等。更为一般的情况，本实施例还包括其他用于实现机器人功能的部件，本实施例不在赘述。
[0073]
一般的，本实施例的机器人实现对待行走的区域建立了语义地图，其语义地图可以是使用slam算法进行点云地图，并使用人工的方式在点云语义地图上对特定物体进行颜色的标注和语义信息表的标注以成点云语义地图。
[0074]
具体的，对点云地图的特定物体的识别可以完全由人工来完成，例如将点云地图显示出来，由人工对点云地图中的特定目标物进行语义的标注。在另一个实施方式可以使用基于深度学习的目标检测算法从点云地图中识别出特定目标物，并在点云地图中标注出特定目标物的位置，从而辅助人工生成语义信息表。
[0075]
yolo是一种图像目标检测算法，在2016年被提出，发表在计算机视觉顶会cvpr(computer vision and pattern recognition)上，yolo的全称是you only look once，指只需要浏览一次就可以识别出图中的物体的类别和位置。本实施例可以使用yolo对点云地图实行目标识别。
[0076]
实时图像获取单元，用于获取机器人实时获取的实时图像；
[0077]
本实施例的实时图像由视觉传感器获得。
[0078]
目标物检测单元，用于使用目标检测算法识别所述实时图像中的目标物；
[0079]
本实施例使用yolo对点云地图进行目标检测，yolo可以输出检测到的目标物的类别以及在图像中的位置。将所述实时图像输入到目标检测算法(yolo)中，检测出目标物a、b、c。本实施例以检测出三个目标物为例进行说明。
[0080]
语义获取单元，用于从语义地图中语义信息表，获取与所述目标物对应的语义；
[0081]
具体的，本实施例在机器人启动时，会自动加载事先建立好的点云语义地图。
[0082]
具体的，根据识别出的目标物a、b、c的类别，从所述语义地图的语义信息表中查找与所述目标物a、b、c的类别对应的语义信息a、b、c。
[0083]
具体的语义信息表中记录了语义的类别，位置，大小，颜色等。
[0084]
重投影单元，用于将所述目标物对应的语义的全部点云根据定位结果进行重投影
处理获取重投影后的图像；
[0085]
定位结果用[r|t]3x4的矩阵表示，其中包含了旋转信息r和平移信息t。
[0086]
式(1)是[r|t]表达式，是待求的量。
[0087][0088]
所述目标物对应的语义a、b、c全部点云进行重投影处理，以语义a为例；假设语义a里面包含n个点云p＝[x y z]
t
[0089][0090][0091][0092]
其中下标ai表述语义a里面第i个点云；上标1、2表示中间变量用于区分；其中f
x
，fy，u0，v0是相机的内参，出厂固有参数，fx，fy为焦距，u0，v0为主点坐标(相对于成像平面)，u
ai
，v
ai
为语义a里面第i个点云在图像像素坐标系的大小值。
[0093]
定位结果优化代价函数构建单元，用于使用所述重投影后的图像与实时获取的目标物构建定位结果优化代价函数；
[0094]
由点云语义地图(ab c)与实时图像关联好的语义目标(a b c)，构建变量优化代价函数：
[0095][0096]
其中pixel(u
ai
,v
ai
)表示获取语义a中第i个点云的像素值，因为点云语义每个点都已经带有颜色；pixel(u
ai
,v
ai
)表示获取图像目标物a中第i个点云的像素值；
[0097]
迭代优化单元、用于获取轮式里程计给出定位结果[r|t]的粗值，利用牛顿-高斯法不断调整定位结果[r|t]值，直至定位结果优化代价函数err([r|t])达到最小，得到最优的定位结果[r|t]；
[0098]
通过轮式里程计给出[r|t]的粗值，利用牛顿-高斯法不断调整[r|t]值，直至err([r|t])达到最小，得到最优的[r|t]；
[0099]
本实施例使用图像神经网络算法来识别实时图像中目标物，能鲁棒地检测目标
物；此外，本实施例利用点云语义进行定位(点云语义一般为一些固定物体)，避免受到动态物体的干扰。
[0100]
实施例三
[0101]
本实施例公开了一种机器人，其包括一视觉传感器，一处理单元，一底盘，一存储单元，所述存储单元存储有指令，在所述指令被执行时，用于实现如实施例一所述的基于语义点云地图的定位方法。
[0102]
具体，本实施例的机器人还包括如实施例二所述的于语义点云地图的定位装置。
[0103]
实施例四
[0104]
参考图4，图4是本实施例的一种基于点云语义地图的定位设备的结构示意图。该实施例的基于点云语义地图的定位设备20包括处理器21、存储器22以及存储在所述存储器22中并可在所述处理器21上运行的计算机程序。所述处理器21执行所述计算机程序时实现上述方法实施例中的步骤。或者，所述处理器21执行所述计算机程序时实现上述各装置实施例中各模块/单元的功能。
[0105]
示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器22中，并由所述处理器21执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述基于点云语义地图的定位设备20中的执行过程。例如，所述计算机程序可以被分割成实施例二中的各个模块,各模块具体功能请参考上述实施例所述的装置的工作过程，在此不再赘述。
[0106]
所述基于点云语义地图的定位设备20可包括，但不仅限于，处理器21、存储器22。本领域技术人员可以理解，所述示意图仅仅是基于点云语义地图的定位设备20的示例，并不构成对基于点云语义地图的定位设备20的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述基于点云语义地图的定位设备20还可以包括输入输出设备、网络接入设备、总线等。
[0107]
所述处理器21可以是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器21是所述基于点云语义地图的定位设备20的控制中心，利用各种接口和线路连接整个基于点云语义地图的定位设备20的各个部分。
[0108]
所述存储器22可用于存储所述计算机程序和/或模块，所述处理器21通过运行或执行存储在所述存储器22内的计算机程序和/或模块，以及调用存储在存储器22内的数据，实现所述基于点云语义地图的定位设备20的各种功能。所述存储器22可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器22可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(smart media card,smc)，安全数字(secure digital,sd)卡，闪存卡(flash card)、至少一个磁盘存储器件、闪存器件、或其他
易失性固态存储器件。
[0109]
其中，所述基于点云语义地图的定位设备20集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器21执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
[0110]
需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
[0111]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。