CN115164906A - 定位方法、机器人和计算机可读存储介质 - Google Patents
定位方法、机器人和计算机可读存储介质 Download PDFInfo
- Publication number
- CN115164906A CN115164906A CN202211069393.2A CN202211069393A CN115164906A CN 115164906 A CN115164906 A CN 115164906A CN 202211069393 A CN202211069393 A CN 202211069393A CN 115164906 A CN115164906 A CN 115164906A
- Authority
- CN
- China
- Prior art keywords
- node
- topological graph
- global
- current
- nodes
- 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 55
- 238000011144 upstream manufacturing Methods 0.000 claims description 22
- 238000005295 random walk Methods 0.000 claims description 20
- 238000004590 computer program Methods 0.000 claims description 13
- 238000010276 construction Methods 0.000 claims description 13
- 230000000875 corresponding effect Effects 0.000 description 86
- 238000004891 communication Methods 0.000 description 11
- 238000007781 pre-processing Methods 0.000 description 10
- 230000000007 visual effect Effects 0.000 description 10
- 230000006870 function Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 238000001514 detection method Methods 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 4
- 230000005021 gait Effects 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000013500 data storage Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000005286 illumination Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000001276 controlling effect Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000007599 discharging Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000002372 labelling Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 230000005236 sound signal Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- 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/20—Control system inputs
- G05D1/24—Arrangements for determining position or orientation
- G05D1/246—Arrangements for determining position or orientation using environment maps, e.g. simultaneous localisation and mapping [SLAM]
- G05D1/2469—Arrangements for determining position or orientation using environment maps, e.g. simultaneous localisation and mapping [SLAM] using a topologic or simplified map
-
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
-
- 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/20—Control system inputs
- G05D1/24—Arrangements for determining position or orientation
- G05D1/242—Means based on the reflection of waves generated by the vehicle
-
- 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/20—Control system inputs
- G05D1/24—Arrangements for determining position or orientation
- G05D1/243—Means capturing signals occurring naturally from the environment, e.g. ambient optical, acoustic, gravitational or magnetic signals
-
- 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/20—Control system inputs
- G05D1/24—Arrangements for determining position or orientation
- G05D1/246—Arrangements for determining position or orientation using environment maps, e.g. simultaneous localisation and mapping [SLAM]
- G05D1/2467—Arrangements for determining position or orientation using environment maps, e.g. simultaneous localisation and mapping [SLAM] using a semantic description of the environment
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D2109/00—Types of controlled vehicles
- G05D2109/10—Land vehicles
- G05D2109/12—Land vehicles with legs
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
- G05D2111/10—Optical signals
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
- G05D2111/10—Optical signals
- G05D2111/17—Coherent light, e.g. laser signals
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种定位方法、机器人和存储介质。定位方法包括:获取根据机器人当前观测到的环境中的物体建立的当前拓扑图,获取根据预设区域内的全局环境中的物体预先建立的全局拓扑图,当前拓扑图和全局拓扑图包括表示物体的节点;对当前拓扑图中的两节点与全局拓扑图中的两节点构成的待关联节点对进行匹配;若关联程度大于阈值,则确定待关联节点对为关联节点对;从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支;根据匹配度最高的搜索分支确定机器人的当前位姿。上述定位方法,可以根据匹配度最高的搜索分支确定机器人的当前位姿。
Description
技术领域
本发明涉及机器人定位技术领域,特别涉及一种定位方法、机器人和计算机可读存储介质。
背景技术
机器人在室内的初始定位是比较难的,机器人在没有先验定位的情况下,需要识别自己在全局地图中的位姿。在相关技术中,机器人定位通常需要遍历数据库中所有数据,导致定位效率下降。
发明内容
本发明提供了一种定位方法、机器人和计算机可读存储介质。
本发明实施方式提供的定位方法,可以用于机器人,所述定位方法包括:获取当前拓扑图和全局拓扑图,所述当前拓扑图为根据所述机器人当前观测到的环境中的物体建立的,所述全局拓扑图为根据预设区域内的全局环境中的物体预先建立的,所述当前拓扑图和所述全局拓扑图包括表示物体的节点;对所述当前拓扑图和所述全局拓扑图中的节点进行匹配,所述匹配包括判断所述当前拓扑图中的节点与所述全局拓扑图中的节点构成的待关联节点对的关联程度;若所述关联程度大于阈值,则确定所述待关联节点对为关联节点对;从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支;根据所述匹配度最高的搜索分支确定所述机器人的当前位姿。
上述定位方法,通过拓扑图匹配可以快速、准确地确定关联节点对(即关联物体对),从而可以通过当前拓扑图中和全局拓扑图中关联节点对数量最多的若干个关联节点能够快速地确定匹配度最高的搜索分支,进而可以根据匹配度最高的搜索分支确定机器人的当前位姿。
本申请实施方式的机器人,包括控制模块,所述控制模块用于:获取当前拓扑图和全局拓扑图,所述当前拓扑图为根据所述机器人当前观测到的环境中的物体建立的,所述全局拓扑图为根据预设区域内的全局环境中的物体预先建立的,所述当前拓扑图和所述全局拓扑图包括表示物体的节点;对所述当前拓扑图和所述全局拓扑图中的节点进行匹配,所述匹配包括判断所述当前拓扑图中的节点与所述全局拓扑图中的节点构成的待关联节点对的关联程度;若所述关联程度大于阈值,则确定所述待关联节点对为关联节点对;从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支;根据所述匹配度最高的搜索分支确定所述机器人的当前位姿。
上述机器人,通过拓扑图匹配可以快速、准确地确定关联节点对(即关联物体对),从而可以通过当前拓扑图中和全局拓扑图中关联节点对数量最多的若干个关联节点能够快速地确定匹配度最高的搜索分支,进而可以根据匹配度最高的搜索分支确定机器人的当前位姿。
本申请实施方式的机器人,包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时,实现上述任一个实施方式所述的定位方法。
上述机器人,通过拓扑图匹配可以快速、准确地确定关联节点对(即关联物体对),从而可以通过当前拓扑图中和全局拓扑图中关联节点对数量最多的若干个关联节点能够快速地确定匹配度最高的搜索分支,进而可以根据匹配度最高的搜索分支确定机器人的当前位姿。
本申请实施方式的计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时,实现上述任一个实施方式所述的定位方法。
上述计算机可读存储介质,通过拓扑图匹配可以快速、准确地确定关联节点对(即关联物体对),从而可以通过当前拓扑图中和全局拓扑图中关联节点对数量最多的若干个关联节点能够快速地确定匹配度最高的搜索分支,进而可以根据匹配度最高的搜索分支确定机器人的当前位姿。
本发明的附加方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施方式的描述中将变得明显和容易理解,其中:
图1是本发明实施方式的机器人在某些实施方式中的硬件结构示意图;
图2是本发明实施方式的机器人在某些实施方式中的结构示意图;
图3是本发明实施方式的定位方法的流程示意图;
图4是本发明实施方式的生成全局拓扑图的流程示意图;
图5是本发明实施方式的当前拓扑图和全局拓扑图的示意图;
图6是本发明实施方式的确定机器人的当前位姿的流程示意图;
图7是本发明实施方式的搜索解释树的示意图;
图8是本发明实施方式的获得新的全局拓扑图的示意图。
主要元件符号说明:
机器人100、机械单元101、驱动板1011、电动机1012、机械结构1013、机身主体1014、腿部1015、足部1016、通讯单元102、摄像单元1021、传感单元103、接口单元104、存储单元105、显示单元106、输入单元107、触控面板1071、输入设备1072、触摸检测装置1073、触摸控制器1074、音频输出单元108、控制模块110、电源111。
具体实施方式
应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
在后续的描述中,使用用于表示部件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明,其本身没有特定的意义。因此,“模块”、“部件”或“单元”可以混合地使用。
请参考图1,图1为本发明其中一个实施方式的机器人100的硬件结构示意图。在图1所示的实施方式中,机器人100可以为足式机器人,机器人100可以包括机械单元101、通讯单元102、传感单元103、接口单元104、存储单元105、控制模块110、电源111。机器人100的各种部件可以以任何方式连接,包括有线或无线连接等。本领域技术人员可以理解,图1中示出的机器人100的具体结构并不构成对机器人100的限定,机器人100可以包括比图示更多或更少的部件,某些部件也并不属于机器人100的必须构成,完全可以根据需要在不改变发明的本质的范围内而省略,或者组合某些部件。
下面结合图1对机器人100的各个部件进行具体地介绍:
机械单元101为机器人100的硬件。如图1所示,机械单元101可包括驱动板1011、电动机1012、机械结构1013,如图2所示,机械结构1013可包括机身主体1014、可伸展的腿部1015、足部1016,在其他实施方式中,机械结构1013还可包括可伸展的机械臂(图未示)、可转动的头部结构1017、可摇动的尾巴结构1018、载物结构1019、鞍座结构1020、摄像单元1021等。机器人100可以通过摄像单元1021进行摄像,从而能够获取到视觉信息。需要说明的是,机械单元101的各个部件模块可以为一个也可以为多个,可根据具体情况设置,比如腿部1015可为4个,每个腿部1015可配置3个电动机1012,对应的电动机1012为12个。
通讯单元102可用于信号的接收和发送,还可以通过与网络和其他设备通信,比如,接收遥控器或其他机器人100发送的按照特定步态以特定速度值向特定方向移动的指令信息后,传输给控制模块110处理。通讯单元102包括如WiFi模块、4G模块、5G模块、蓝牙模块、红外模块等。
传感单元103用于获取机器人100周围环境的信息数据以及监控机器人100内部各部件的参数数据,并发送给控制模块110。传感单元103包括多种传感器,如获取周围环境信息的传感器:激光雷达(用于远程物体检测、距离确定和/或速度值确定)、毫米波雷达(用于短程物体检测、距离确定和/或速度值确定)、摄像头、红外摄像头、全球导航卫星系统(GNSS,Global Navigation Satellite System)等。如监控机器人100内部各部件的传感器:惯性测量单元(IMU,Inertial Measurement Unit)(用于测量速度值、加速度值和角速度值的值),足底传感器(用于监测足底着力点位置、足底姿态、触地力大小和方向)、温度传感器(用于检测部件温度)。至于机器人100还可配置的载荷传感器、触摸传感器、电机角度传感器、扭矩传感器等其他传感器,在此不再赘述。
接口单元104可以用于接收来自外部装置的输入(例如,数据信息、电力等)并且将接收到的输入传输到机器人100内的一个或多个部件,或者可以用于向外部装置输出(例如,数据信息、电力等)。接口单元104可包括电源端口、数据端口(如USB端口)、存储卡端口、用于连接具有识别模块的装置的端口、音频输入/输出(I/O)端口、视频I/O端口等。
存储单元105用于存储软件程序以及各种数据。存储单元105可主要包括程序存储区和数据存储区,其中,程序存储区可存储操作系统程序、运动控制程序、应用程序(比如文本编辑器)等;数据存储区可存储机器人100在使用中所生成的数据(比如传感单元103获取的各种传感数据,日志文件数据)等。此外,存储单元105可以包括高速随机存取存储器,还可以包括非易失性存储器,例如磁盘存储器、闪存器、或其他易失性固态存储器。
显示单元106用于显示由用户输入的信息或提供给用户的信息。显示单元106可包括显示面板1061,可以采用液晶显示器(Liquid Crystal Display,LCD)、有机发光二极管(Organic Light-Emitting Diode, OLED)等形式来配置显示面板1061。
输入单元107可用于接收输入的数字或字符信息。具体地,输入单元107可包括触控面板1071以及其他输入设备1072。触控面板1071,也称为触摸屏,可收集用户的触摸操作(比如用户使用手掌、手指或适合的附件在触控面板1071上或在触控面板1071附近的操作),并根据预先设定的程式驱动相应的连接装置。触控面板1071可包括触摸检测装置1073和触摸控制器1074两个部分。其中,触摸检测装置1073检测用户的触摸方位,并检测触摸操作带来的信号,将信号传送给触摸控制器1074;触摸控制器1074从触摸检测装置1073上接收触摸信息,并将它转换成触点坐标,再送给控制模块110,并能接收控制模块110发来的命令并加以执行。除了触控面板1071,输入单元107还可以包括其他输入设备1072。具体地,其他输入设备1072可以包括但不限于遥控操作手柄等中的一种或多种,具体此处不做限定。
进一步的,触控面板1071可覆盖显示面板1061,当触控面板1071检测到在其上或附近的触摸操作后,传送给控制模块110以确定触摸事件的类型,随后控制模块110根据触摸事件的类型在显示面板1061上提供相应的视觉输出。虽然在图1中,触控面板1071与显示面板1061是作为两个独立的部件来分别实现输入和输出功能,但是在某些实施例中,可以将触控面板1071与显示面板1061集成而实现输入和输出功能,具体此处不做限定。
音频输出单元108可以将通讯单元102接收的,或者在存储单元105中存储的音频数据转换成音频信号并且输出为声音。音频输出单元108可以包括扬声器、蜂鸣器等等。
控制模块110是机器人100的控制中心,利用各种接口和线路连接整个机器人100的各个部件,通过运行或执行存储在存储单元105内的软件程序,以及调用存储在存储单元105内的数据,从而对机器人100进行整体控制。
电源111用于给各个部件供电,电源111可包括电池和电源控制板,电源控制板用于控制电池充电、放电、以及功耗管理等功能。在图1所示的实施方式中,电源111电连接控制模块110,在其它的实施方式中,电源111还可以分别与传感单元103(比如摄像头、雷达、音箱等)、电动机1012电性连接。需要说明的是,各个部件可以各自连接到不同的电源111,或者由相同的电源111供电。
在上述实施方式的基础上,具体地,在一些实施方式中,可以通过终端设备来与机器人100进行通信连接,在终端设备与机器人100进行通信时,可以通过终端设备来向机器人100发送指令信息,机器人100可通过通讯单元102来接收指令信息,并可在接收到指令信息的情况下,将指令信息传输至控制模块110,使得控制模块110可根据指令信息来处理得到目标速度值。终端设备包括但不限于:具备图像拍摄功能的手机、平板电脑、服务器、个人计算机、可穿戴智能设备、其它电器设备。
指令信息可以根据预设条件来确定。在一个实施方式中,机器人100可以包括传感单元103,传感单元103可根据机器人100所在的当前环境可生成指令信息。控制模块110可根据指令信息来判断机器人100的当前速度值是否满足对应的预设条件。若满足,则会保持机器人100的当前速度值和当前步态移动;若不满足,则会根据对应的预设条件来确定目标速度值和相应的目标步态,从而可控制机器人100以目标速度值和相应的目标步态移动。环境传感器可以包括温度传感器、气压传感器、视觉传感器、声音传感器。指令信息可以包括温度信息、气压信息、图像信息、声音信息。环境传感器与控制模块110之间的通信方式可以为有线通信,也可以为无线通信。无线通信的方式包括但不限于:无线网络、移动通信网络(3G、4G、5G等)、蓝牙、红外。
机器人在室内的初始定位通常是一个比较难的挑战,机器人在没有先验定位的情况下,需通过当前时刻的自身传感器信息,识别到自己所在全局地图中的精准位姿。视觉词袋模型是一种当前比较流行的解决机器人初始定位的方法之一,它利用图像信息提取和聚类ORB特征描述子构造视觉字典,并用视觉词汇的相似度来匹配查询图片与数据库中最接近的图片,从而找到机器人所对应的全局位姿。此技术方案存在明显的缺点,比如特征点为低阶局部图像描述子,容易受到光照,旋转角度,移动的物体的影响;受到相机视场角的限制,对于同一个地点不同的视角容易匹配不上;效率低下,需要遍历数据库中所有数据,随着地图数据变大效率也下降。
请参考图1至图3,本发明实施方式的定位方法可以用于机器人100。定位方法包括:
01:获取当前拓扑图和全局拓扑图,当前拓扑图为根据机器人100当前观测到的环境中的物体建立的,全局拓扑图为根据预设区域内的全局环境中的物体预先建立的,当前拓扑图和全局拓扑图包括表示物体的节点;
02:对当前拓扑图和全局拓扑图中的节点进行匹配,匹配包括判断当前拓扑图中的节点与全局拓扑图中的节点构成的待关联节点对的关联程度;
03:若关联程度大于阈值,则确定待关联节点对为关联节点对;
04:从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支;
05:根据匹配度最高的搜索分支确定机器人100的当前位姿。
本发明实施方式的定位方法可以通过本发明实施方式的机器人100来实现。具体地,请结合图1,控制模块110用于:获取当前拓扑图和全局拓扑图,当前拓扑图为根据机器人100当前观测到的环境中的物体建立的,全局拓扑图为根据预设区域内的全局环境中的物体预先建立的,当前拓扑图和全局拓扑图包括表示物体的节点;对当前拓扑图和全局拓扑图中的节点进行匹配,匹配包括判断当前拓扑图中的节点与全局拓扑图中的节点构成的待关联节点对的关联程度;若关联程度大于阈值,则确定待关联节点对为关联节点对;从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支;根据匹配度最高的搜索分支确定机器人100的当前位姿。
上述定位方法和机器人100,通过拓扑图匹配可以快速、准确地确定关联节点对(即关联物体对),从而可以通过当前拓扑图中和全局拓扑图中关联节点对数量最多的若干个关联节点能够快速地确定匹配度最高的搜索分支,进而可以根据匹配度最高的搜索分支确定机器人100的当前位姿。
当前拓扑图为根据机器人100当前检测到的环境中的物体建立的,其中,机器人100当前检测到的环境中的物体,是指机器人100处于当前位姿时所检测到的环境中的物体。
全局拓扑图为根据预设区域内的全局环境中的物体预先建立的,其中,预设区域可以是任意设定区域,例如预设区域可以是室内区域,具体可以是整栋楼的所有区域、或者其中一层楼、或者其中一个房间等,在此不做具体限定。
其中,对当前拓扑图和全局拓扑图中的节点进行匹配,根据匹配的结果可以确定待关联节点对的关联程度。若待关联节点对的关联程度大于阈值,说明待关联节点对的两节点是相关的,因此,可以将待关联节点对确定为关联节点对。其中,阈值可以预先设定,在此不做具体限定。
当前拓扑图和全局拓扑图可以形成多个搜索分支,确定每一个搜索分支中关联节点对的数量,确定关联节点对数量最多的搜索分支,从而可以根据匹配度最高的搜索分支确定机器人100的当前位姿。本申请将机器人100的定位问题转化为拓扑图匹配问题,能够通过拓扑图匹配快速、准确地确定匹配度最高的搜索分支,从而快速、准确地确定机器人100的当前位姿。其中,机器人100的当前位姿可以是指当前时刻的初始位姿,初始位姿是指机器人100没有先验定位(没有早于当前时刻的定位信息),位姿可以是指位置和姿态(朝向)。
在某些实施方式中,步骤01中的获取全局拓扑图,包括:
011:获取全局环境图像和全局深度图像;
012:根据全局环境图像和全局深度图像建立全局语义地图,全局语义地图包括全局点云信息和全局点云信息对应的第一物体种类标签;
013:根据第一物体种类标签对全局语义地图中的全局点云信息进行聚类,识别出各个物体,并获取各物体的第一包围盒,第一包围盒是将具有同一第一物体种类标签的点云聚类后的点云对应的独立空间;
014:以第一包围盒为节点,以相邻的两个第一包围盒的连线作为边,生成全局拓扑图,相邻的两个第一包围盒之间的距离小于预设距离。
本发明实施方式的定位方法可以通过本发明实施方式的机器人100来实现。具体地,请结合图1,控制模块110用于:获取摄像单元1021拍摄的全局环境图像和全局深度图像;根据全局环境图像和全局深度图像建立全局语义地图,全局语义地图包括全局点云信息和全局点云信息对应的第一物体种类标签;根据第一物体种类标签对全局语义地图中的全局点云信息进行聚类,识别出各个物体,并获取各物体的第一包围盒,第一包围盒是将具有同一第一物体种类标签的点云聚类后的点云对应的独立空间;以第一包围盒为节点,以相邻的两个第一包围盒的连线作为边,生成全局拓扑图,相邻的两个第一包围盒之间的距离小于预设距离。
如此,可以基于物体语义生成全局拓扑图。
请参阅图4,全局拓扑图可以是在离线阶段(不是确定机器人100的当前位姿的阶段)预先建立。具体地,在视觉里程计的作用下,机器人100可以绕预设区域内的全局环境走一圈,通过摄像单元1021拍摄获得全局环境图像(可以是普通的RGB三通道环境图像,摄像单元1021可以为可见光摄像头)和全局深度图像(Depth图像,类似于灰度图像,只是Depth图像的每个像素值是传感器距离物体的实际距离,摄像单元1021可以为结构光摄像头、TOF摄像头等),其中,全局环境图像和全局深度图像可以是配准的,两者的像素点之间具有一对一的对应关系。通过全局环境图像和全局深度图像构建全局语义地图,全局语义地图包括全局点云信息和全局点云信息对应的第一物体种类标签。其中,全局环境图像通过被训练过的网络进行物体语义分割,结合全局深度图像,语义和点云被三维重构,从而构成3D的全局语义地图。分割物体可以为墙、墙角线等建筑结构、沙发、空调、冰箱等不容易移动的物体。根据第一物体种类标签对全局语义地图中的全局点云信息进行聚类,即将带有同一第一物体种类标签的点云进行聚类。其中,可以对同一第一物体种类标签且相近的点云进行聚类,从而识别出各个物体。例如,预设区域内的全局环境中存在两张凳子,两张凳子的标签都是凳子,包围盒能够根据点云的距离进行区分,将对应同一张凳子的点云进行聚类。计算出各物体的质心,并通过训练的模型获取各物体的第一包围盒和六自由度坐标,第一包围盒是将具有同一第一物体种类标签的点云聚类后的点云对应的独立空间(独立空间具体可以是球体、长方体等,本申请实施例以独立空间为长方体为例进行说明,例如,包围盒是空间中将同一物体标签的点云包围在内的最小长方体),其中,第一包围盒是有向包围盒(即第一包围盒可以根据物体的一阶矩任意旋转)。物体的六自由度坐标的坐标原点可以为包围盒的中心,坐标方向为标注时所定义的空间正交的三维向量。
以第一包围盒作为节点(以墙、墙角线等建筑结构、沙发、空调、冰箱等不容易移动的物体作为节点),以相邻的两个第一包围盒的连线作为边(可以是无向边,其中,无向边是指没有方向的边,无向边没有箭头,即不存在一个包围盒指向另一个包围盒),从而生成全局拓扑图,其中,若两个第一包围盒之间的距离小于预设距离则可以认为两者是相邻的,从而形成两者的连线以作为边;若两个第一包围盒之间的距离大于预设距离则可以认为两者是不相邻的,两个第一包围盒不连线,即不生成边。请参阅图5,例如节点4对应的包围盒和节点8对应的包围盒的距离大于预设距离,则可以认为两者是不相邻的,节点4和节点8不连线,即不生成边。以语义分割的物体作为特征,具有高阶特征的属性,相对于视觉词袋等低阶特征描述子而言,不易受光照,角度的影响;通过物体的邻近关系构建拓扑图,不受视角的限制。以建筑结构和不易移动的物体作为特征来构建拓扑图,不受易移动物体的影响。
在某些实施方式中,步骤01中的获取当前拓扑图,包括:
015:获取当前环境图像和当前深度图像;
016:根据当前环境图像和当前深度图像建立当前语义地图,当前语义地图包括当前点云信息和当前点云信息对应的第二物体种类标签;
017:根据第二物体种类标签对当前语义地图中的当前点云信息进行聚类,识别出各物体,并获取各物体的第二包围盒,第二包围盒是将具有同一第二物体种类标签的点云聚类后的点云对应的独立空间;
018:以第二包围盒为节点,以相邻的两个第二包围盒的连线作为边,生成当前拓扑图,相邻的两个第二包围盒之间的距离小于预设距离。
本发明实施方式的定位方法可以通过本发明实施方式的机器人100来实现。具体地,请结合图1,控制模块110用于:获取摄像单元1021拍摄的当前环境图像和当前深度图像;根据当前环境图像和当前深度图像建立当前语义地图,当前语义地图包括当前点云信息和当前点云信息对应的第二物体种类标签;根据第二物体种类标签对当前语义地图中的当前点云信息进行聚类,识别出各物体,并获取各物体的第二包围盒,第二包围盒是将具有同一第二物体种类标签的点云聚类后的点云对应的独立空间;以第二包围盒为节点,以相邻的两个第二包围盒的连线作为边,生成当前拓扑图,相邻的两个第二包围盒之间的距离小于预设距离。
如此,可以基于物体语义生成当前拓扑图。
具体地,在机器人100置身于未知的位置时,可以生成当前拓扑图。请参阅图6,当前拓扑图可以是在在线阶段(确定机器人100的当前位姿的阶段)生成。生成当前拓扑图的方式与生成全局拓扑图的方式类似,在此不再赘述。与生成全局拓扑图相比,生成当前拓扑图时,机器人100是在当前位姿下拍摄获得当前环境图像和当前深度图像的。
在某些实施方式中,步骤04(从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支),包括:
0412:将当前拓扑图和全局拓扑图结合构成搜索解释树,搜索解释树包括多个搜索分支,遍历每个搜索分支;
0414:当前拓扑图的每一个节点构成第二层节点;
0416:以第二层节点为父节点,第二层节点的子节点为全局拓扑图中的每一个节点并构成第三层节点,第二层节点和第三层节点构成搜索解释树的第一节点对;
0418:确定第一节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签是否相同;
0422:若第一节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签不同,则结束对应的搜索分支的构建;
0424:若第一节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签相同,则以第三层节点作为父节点,第三层节点的子节点为当前拓扑图除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第四层节点;以第四层节点为父节点,第四层节点的子节点为全局拓扑图中除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第五层节点,第四层节点和第五层节点构成搜索解释树的第二节点对;
0426:确定第二节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签是否相同;
0428:根据同一搜索分支中,第一节点对和第二节点对中的两个当前拓扑图节点之间的距离和夹角以及两个全局拓扑图节点之间的距离和夹角,获得第一节点对和第二节点对构成的待关联节点对的关联程度;
0432:若第二节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签不同,或关联程度小于阈值,则结束对应的搜索分支的构建;
0434:若在第二节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签相同,且关联程度大于阈值,则继续遍历当前拓扑图与全局拓扑图的节点;
0436:统计每个搜索分支的关联节点对的数量,关联节点对的数量最多的搜索分支作为匹配度最高的搜索分支。
本发明实施方式的定位方法可以通过本发明实施方式的机器人100来实现。具体地,请结合图1,控制模块110用于:将当前拓扑图和全局拓扑图结合构成搜索解释树,搜索解释树包括多个搜索分支,遍历每个搜索分支;当前拓扑图的每一个节点构成第二层节点;以第二层节点为父节点,第二层节点的子节点为全局拓扑图中的每一个节点并构成第三层节点,第二层节点和第三层节点构成搜索解释树的第一节点对;确定第一节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签是否相同;若第一节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签不同,则结束对应的搜索分支的构建;若第一节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签相同,则以第三层节点作为父节点,第三层节点的子节点为当前拓扑图除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第四层节点;以第四层节点为父节点,第四层节点的子节点为全局拓扑图中除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第五层节点,第四层节点和第五层节点构成搜索解释树的第二节点对;确定第二节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签是否相同;根据同一搜索分支中,第一节点对和第二节点对中的两个当前拓扑图节点之间的距离和夹角以及两个全局拓扑图节点之间的距离和夹角,获得第一节点对和第二节点对构成的待关联节点对的关联程度;若第二节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签不同,或关联程度小于阈值,则结束对应的搜索分支的构建;若第二节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签相同,且关联程度大于阈值,则继续遍历当前拓扑图与全局拓扑图的节点;统计每个搜索分支的关联节点对的数量,关联节点对的数量最多的搜索分支作为匹配度最高的搜索分支。
如此,可以边构建搜索解释树边进行搜索,搜索不到符合要求的节点就不再继续构建对应的搜索分支。
具体地,将当前拓扑图和全局拓扑图结合构成搜索解释树,搜索解释树包括多个搜索分支,遍历每个搜索分支以搜索符合条件的搜索分支,从多个搜索分支中,选出关联节点对数量最多的搜索分支作为匹配度最高的搜索分支。更具体的,请参考图5,图5的上半部分为当前拓扑图,例如总共包括3个节点:节点1、节点2、节点3。图5的下半部分为全局拓扑图,例如总共包括13个节点:节点1、……、节点13。其中,同样的填充图案可视作物体种类标签是相同的。请参阅图7,当前拓扑图的每一个节点构成第二层节点,以第二层节点为父节点,第二层节点的子节点为全局拓扑图中的每一个节点并构成第三层节点,第二层节点和第三层节点构成搜索解释树的第一节点对(例如图7中的1-1,1-3,1-6)。对比第一节点对的两个物体之间的一元约束,即,判断单个节点的相似性约束:对比第一节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签是否相同,若第一节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签不同,则说明第一节点对互不关联,因此,可以结束对应的搜索分支的构建,不再继续构建该搜索分支(例如图7中的1-1,两者的物体种类标签不同,可以结束对应的搜索分支的构建)。若第一节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签相同,则说明第一节点对可能存在关联,因此,可以继续构建该搜索分支(例如图7中的1-3,1-6,两者的物体种类标签相同,可以继续构建搜索分支),即,以第三层节点作为父节点,第三层的子节点为当前拓扑图除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第四层节点,在一个实施例中,若第二层出现过的节点为节点1,则节点2和节点3构成第四层节点。以第四层节点作为父节点,第四层的子节点为全局拓扑图除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第五层节点。在一个实施例中,若第三层出现过的节点为节点6,则节点1-5、7-13构成第五层节点。第四层节点和第五层节点构成搜索解释树的第二节点对(例如图7中的2-1,2-10),第一节点对和第二节点对构成待关联节点对。其中,需要说明的是,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。例如,本申请的第二层节点可以为第二层,也可以为初始层,并不限定第二层节点前面必须具有第一层节点。具体地,在一个实施例中,请参阅图7,第二层节点前面具有第一层节点,第一层节点为空节点,空节点可以作为根节点。在另一个实施例中,第二层节点前面不具有第一层节点,第二层节点直接作为根节点。
对比第二节点对的两个物体之间的一元约束,即,判断单个节点的相似性约束:对比第二节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签是否相同;对比待关联节点对之间的二元约束,即,判断同一搜索分支中,第一节点对和第二节点对中的两个当前拓扑图节点和两个全局拓扑图节点的相似性约束:判断当前拓扑图的两个节点是否相连(拓扑图是否有相应的边)、全局拓扑图的两个节点是否相连,根据当前拓扑图中两节点之间的距离和夹角以及全局拓扑图中两节点之间的距离和夹角获得待关联节点对的关联程度。其中,当前拓扑图或全局拓扑图中的两节点之间的距离和夹角,可以根据两节点对应的包围盒和空间方向计算获得,距离计算例如为:d=|p1-p2|,其中,p1和p2分别为两个包围盒的中心点,夹角计算例如为:ΔR=|R1 -1·R2|,其中,R1和R2分别为两个包围盒相对于同一坐标系的旋转矩阵。包围盒在某个坐标系的旋转矩阵R可以用罗德里格斯公式求出,具体方法如下。设p为包围盒的中心点,px为包围盒x轴与包围盒表面的交点,向量a=p-px,向量b为单位向量(1,0,0)。
令v=a×b,s=||v||,c=a·b。旋转矩阵R为:
v1、v2和v3为三维向量v里的三个值。待关联节点对的关联程度,与当前拓扑图的两个节点之间的距离与全局拓扑图的两个节点之间的距离的差值,呈负相关关系。在一个实施例中,若距离的差值大于预设距离差,则可以认为待关联节点对是互不关联的。待关联节点对的关联程度,与当前拓扑图的两个节点之间的夹角与全局拓扑图的两个节点之间的夹角的差值,呈负相关关系。
若第二节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签不同(例如图7中的2-1),则说明第二节点对互不关联,可以结束对应的搜索分支的构建,不再继续构建该搜索分支。若当前拓扑图的两个节点不相连、全局拓扑图的两个节点不相连(例如图7中的1-3、2-10的搜索分支中,全局拓扑图的节点3和节点10是互不相连的)、或待关联节点对的关联程度小于阈值,则说明待关联节点对的关联程度较低,待关联节点对不是关联节点对,可以结束对应的搜索分支的构建,不再继续构建该搜索分支。若第二节点对中全局拓扑图中的节点所对应的第一物体种类标签和当前拓扑图中的节点所对应的第二物体种类标签相同、当前拓扑图的两个节点相连、全局拓扑图的两个节点相连、且待关联节点对的关联程度大于阈值,则说明待关联节点对的关联程度较高,待关联节点对是关联节点对(例如图7中的1-6和2-10构成关联节点对),可以继续遍历当前拓扑图和全局拓扑图的其他节点,以继续构建该搜索分支:即,以第五层节点作为父节点,第五层的子节点为当前拓扑图除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第六层节点。以第六层节点作为父节点,第六层的子节点为全局拓扑图除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第七层节点。第六层节点和第七层节点构成搜索解释树的第三个节点对,第二节点对和第三节点对构成待关联节点对,以此类推。其中,第三节点对可以是在继续遍历当前拓扑图和全局拓扑图的节点时产生的,此处只是为了方便对方案做进一步描述,并不能理解为对权利要求的限定。
统计每个搜索分支的关联节点对的数量,关联节点对的数量最多的搜索分支作为匹配度最高的搜索分支。
在某些实施方式中,步骤04(从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支)之前,定位方法包括:
061:将当前拓扑图和全局拓扑图结合构成搜索解释树;
062:当前拓扑图的每一个节点构成第二层节点;
063:以第二层节点为父节点,第二层节点的子节点为全局拓扑图中的每一个节点并构成第三层节点;
064:以第三层节点作为父节点,第三层节点的子节点为当前拓扑图除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第四层节点;
065:以第四层节点为父节点,第四层节点的子节点为全局拓扑图中除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第五层节点;
066:以此类推继续遍历当前拓扑图与全局拓扑图的节点以构成搜索解释树。
本发明实施方式的定位方法可以通过本发明实施方式的机器人100来实现。具体地,请结合图1,控制模块110用于:将当前拓扑图和全局拓扑图结合构成搜索解释树;当前拓扑图的每一个节点构成第二层节点;以第二层节点为父节点,第二层节点的子节点为全局拓扑图中的每一个节点并构成第三层节点;以第三层节点作为父节点,第三层节点的子节点为当前拓扑图除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第四层节点;以第四层节点为父节点,第四层节点的子节点为全局拓扑图中除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第五层节点;以此类推继续遍历当前拓扑图与全局拓扑图的节点以构成搜索解释树。
如此,可以根据当前拓扑图和全局拓扑图先构建完整的搜索解释树,后续可以基于完整的搜索解释树搜索关联节点对。
具体地,根据当前拓扑图和全局拓扑图构建完整的搜索解释树,当前拓扑图的每一个节点构成第二层节点,以第二层节点为父节点,第二层节点的子节点为全局拓扑图中的每一个节点并构成第三层节点。以第三层节点作为父节点,第三层的子节点为当前拓扑图除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第四层节点。以第四层节点作为父节点,第四层的子节点为全局拓扑图除去搜索分支的上游分支出现过的节点后剩下的所有节点并构成第五层节点。以此类推继续遍历当前拓扑图与全局拓扑图的其他节点以构成搜索解释树。每一个搜索分支即为一条完整的节点匹配对,所有搜索分支构成待关联节点对的所有可能情况。其中,需要说明的是,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。例如,本申请的第二层节点可以为第二层,也可以为初始层,并不限定第二层节点前面必须具有第一层节点。具体地,在一个实施例中,第二层节点前面具有第一层节点,第一层节点为空节点,空节点可以作为根节点。在另一个实施例中,第二层节点前面不具有第一层节点,第二层节点直接作为根节点。
在某些实施方式中,步骤02(对当前拓扑图和全局拓扑图中的节点进行匹配,匹配包括判断当前拓扑图中的节点与全局拓扑图中的节点构成的待关联节点对的关联程度),包括:
022:判断当前拓扑图的第一节点和全局拓扑图的第一节点是否属于相同的物体种类标签,当前拓扑图的第一节点和全局拓扑图的第一节点构成第一节点对;
024:若当前拓扑图的第一节点和全局拓扑图的第一节点属于相同的物体种类标签,则判断当前拓扑图的第二节点和全局拓扑图的第二节点是否属于相同的物体种类标签,当前拓扑图的第二节点和全局拓扑图的第二节点构成第二节点对;
026:若当前拓扑图的第二节点和全局拓扑图的第二节点属于相同的物体种类标签,则将当前拓扑图的第一节点和当前拓扑图的第二节点之间的距离和夹角,与全局拓扑图的第一节点和全局拓扑图的第二节点之间的距离和夹角进行比对,获取第一节点对和第二节点对所构成的待关联节点对的关联程度,若关联程度大于阈值,则确定待关联节点对为关联节点对。
本发明实施方式的定位方法可以通过本发明实施方式的机器人100来实现。具体地,请结合图1,控制模块110用于:判断当前拓扑图的第一节点和全局拓扑图的第一节点是否属于相同的物体种类标签,当前拓扑图的第一节点和全局拓扑图的第一节点构成第一节点对;若当前拓扑图的第一节点和全局拓扑图的第一节点属于相同的物体种类标签,则判断当前拓扑图的第二节点和全局拓扑图的第二节点是否属于相同的物体种类标签,当前拓扑图的第二节点和全局拓扑图的第二节点构成第二节点对;若当前拓扑图的第二节点和全局拓扑图的第二节点属于相同的物体种类标签,则将当前拓扑图的第一节点和当前拓扑图的第二节点之间的距离和夹角,与全局拓扑图的第一节点和全局拓扑图的第二节点之间的距离和夹角进行比对,获取第一节点对和第二节点对所构成的待关联节点对的关联程度,若关联程度大于阈值,则确定待关联节点对为关联节点对。
如此,可以根据物体种类标签、节点之间的距离和夹角获得待关联节点对的关联程度。
具体地,待关联节点对的匹配,包括对比两个物体之间的一元约束和对比待关联节点对之间的二元约束。对比两个物体之间的一元约束,即,判断单个节点的相似性约束:对比当前拓扑图的第一节点的第二物体种类标签与全局拓扑图的第一节点的第一物体种类标签是否相同,若当前拓扑图的第一节点的第二物体种类标签与全局拓扑图的第一节点的第一物体种类标签不同,则说明第一节点对互不关联;若当前拓扑图的第一节点的第二物体种类标签与全局拓扑图的第一节点的第一物体种类标签相同,则说明第一节点对可能存在关联。若当前拓扑图的第一节点的第二物体种类标签与全局拓扑图的第一节点的第一物体种类标签相同,则对比当前拓扑图的第二节点的第二物体种类标签与全局拓扑图的第二节点的第一物体种类标签是否相同,若当前拓扑图的第二节点的第二物体种类标签与全局拓扑图的第二节点的第一物体种类标签不同,则说明第二节点对互不关联;若当前拓扑图的第二节点的第二物体种类标签与全局拓扑图的第二节点的第一物体种类标签相同,则说明第二节点对可能存在关联,关联程度由节点之间的距离和夹角确定。对比待关联节点对之间的二元约束,即,判断当前拓扑图的两个节点和全局拓扑图的两个节点的相似性约束:判断当前拓扑图的两个节点是否相连(拓扑图是否有相应的边)、全局拓扑图的两个节点是否相连,根据当前拓扑图中两节点之间的距离和夹角以及全局拓扑图中两节点之间的距离和夹角获得待关联节点对的关联程度。其中,当前拓扑图或全局拓扑图中的两节点之间的距离和夹角,可以根据两节点对应的包围盒和空间方向计算获得,距离计算例如为:d=|p1-p2|,其中,p1和p2分别为两个包围盒的中心点,夹角计算例如为:ΔR=|R1 -1·R2|,其中,R1和R2分别为两个包围盒相对于同一坐标系的旋转矩阵。
通过拓扑图匹配,可以对比物体之间的联系(物体种类标签、节点是否相连、节点之间的距离和夹角),从而得到关联节点对(关联的物体)。
在某些实施方式中,搜索解释树包括多个搜索分支,步骤04(从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支),包括:
0442:遍历每个搜索分支;
0444:若搜索解释树的搜索分支包括关联节点对,则将搜索分支确定为匹配分支(即符合条件的搜索分支),否则,中断对应的搜索分支的搜索;
0446:统计每个匹配分支的关联节点对的数量,关联节点对的数量最多的匹配分支作为匹配对最高的搜索分支。
本发明实施方式的定位方法可以通过本发明实施方式的机器人100来实现。具体地,请结合图1,控制模块110用于:遍历每个搜索分支;若搜索解释树的搜索分支包括关联节点对,则将搜索分支确定为匹配分支,否则,中断对应的搜索分支的搜索;统计每个匹配分支的关联节点对的数量,关联节点对的数量最多的匹配分支作为匹配对最高的搜索分支。
如此,可以通过遍历搜索解释树来确定匹配度最高的搜索分支。
具体地,例如可以采用深度优先遍历方法来遍历搜索解释树的每个搜索分支。若当前拓扑图的单个节点的第二物体种类标签与全局拓扑图的单个节点的第一物体种类标签不同,则可以中断对应的搜索分支的搜索;若当前拓扑图的两个节点不相连、全局拓扑图的两个节点不相连、或待关联节点对的关联程度小于阈值,则说明待关联节点对的关联程度较低,待关联节点对不是关联节点对,可以中断对应的搜索分支的搜索;若当前拓扑图的单个节点的第二物体种类标签与全局拓扑图的单个节点的第一物体种类标签相同、当前拓扑图的两个节点相连、全局拓扑图的两个节点相连、且待关联节点对的关联程度大于阈值,则说明待关联节点对的关联程度较高,待关联节点对是关联节点对,可以将对应的搜索分支确定为匹配分支并继续遍历该搜索分支以确定所有的关联节点对。统计每个匹配分支的关联节点对的数量,关联节点对的数量最多的匹配分支作为匹配对最高的搜索分支。
在某些实施方式中,在步骤0412或步骤061(将当前拓扑图和全局拓扑图结合构成搜索解释树)之前,定位方法还包括:
071:计算全局拓扑图中各物体的第一包围盒的大小和当前拓扑图中各物体的第二包围盒的大小;
072:统计全局拓扑图中第一物体种类标签对应的物体数量以确定第一稀有率和当前拓扑图中第二物体种类标签对应的物体数量以确定第二稀有率;
073:计算全局拓扑图中每个物体的第一随机游走特征和当前拓扑图中每个物体的第二随机游走特征;
074:根据第一包围盒的大小、第二包围盒的大小、第一稀有率、第二稀有率、第一随机游走特征、第二随机游走特征计算当前拓扑图和全局拓扑图中的拓扑节点集的相似度,拓扑节点集包括全局拓扑图的第一节点和当前拓扑图的第二节点,第一节点对应的第一物体种类标签与第二节点对应的第二物体种类标签相同;
075:确定拓扑节点集中相似度高于预设相似度的节点对,将节点对在全局拓扑图中的节点作为中心节点,将中心节点以及在全局拓扑图中中心节点为中心周围预设范围内的节点作为新的全局拓扑图,新的全局拓扑图用于与当前拓扑图结合构成搜索解释树。
本发明实施方式的定位方法可以通过本发明实施方式的机器人100来实现。具体地,请结合图1,控制模块110用于:计算全局拓扑图中各物体的第一包围盒的大小和当前拓扑图中各物体的第二包围盒的大小;统计全局拓扑图中第一物体种类标签对应的物体数量以确定第一稀有率和当前拓扑图中第二物体种类标签对应的物体数量以确定第二稀有率;计算全局拓扑图中每个物体的第一随机游走特征和当前拓扑图中每个物体的第二随机游走特征;根据第一包围盒的大小、第二包围盒的大小、第一稀有率、第二稀有率、第一随机游走特征、第二随机游走特征计算当前拓扑图和全局拓扑图中的拓扑节点集的相似度,拓扑节点集包括全局拓扑图的第一节点和当前拓扑图的第二节点,第一节点对应的第一物体种类标签与第二节点对应的第二物体种类标签相同;确定拓扑节点集中相似度高于预设相似度的节点对,将节点对在全局拓扑图中的节点作为中心节点,将中心节点以及在全局拓扑图中中心节点为中心周围预设范围内的节点作为新的全局拓扑图,新的全局拓扑图用于与当前拓扑图结合构成搜索解释树。
如此,能够根据包围盒大小、物体的稀有率和随机游走特征对全局拓扑图进行剪枝以获得新的全局拓扑图,新的全局拓扑图相对原先的拓扑图的节点数量更少,从而可以以新的全局拓扑图作为入口进行有限范围的拓扑图匹配,提高匹配的效率。
具体地,可以根据物体的六自由度坐标计算包围盒的中心点到构成包围盒的长方体的任意一个顶点的距离,将该距离作为包围盒的大小。统计各种物体的数量以计算物体的稀有率,例如,拓扑图中有100个物体A,则物体A的稀有率为1/100;拓扑图只有1个物体B,则物体B的稀有率为1。计算物体的随机游走特征,物体的随机游走特征可以为b组长度为c的游走序列。从当前物体为起始点,随机向连接的物体走去为游走1步,接着再随机往下一个连接的物体走去,直到走了c步,即为游走一次,记录经过的物体顺序即为1组长度为c的游走,再回到起始点如此进行b次。例如,物体游走2次,每次随机向连接的物体游走4步。
通过包围盒的大小、稀有率、随机游走特征计算当前拓扑图和全局拓扑图中的拓扑节点集的相似度,从而可以找出拓扑节点集中相似度高于预设相似度的节点对,例如可以找出k个相似度高于预设相似度的节点对。其中,拓扑节点集的第一节点和第二节点的物体种类标签相同。将物体种类标签相同且相似度高于预设相似度的节点对在全局拓扑图中的节点作为中心节点,即,将节点对的第一节点作为中心节点,将中心节点以及在全局拓扑图中中心节点为中心周围预设范围内的节点作为新的全局拓扑图,新的全局拓扑图相对原先的拓扑图的节点数量更少,从而可以以新的全局拓扑图作为入口进行有限范围的拓扑图匹配,提高匹配的效率,进而可以快速确定关联节点对、根据匹配度最高的搜索分支确定机器人100的当前位姿。其中,周围预设范围内,可以是指与中心节点相连预设步数以内的所有节点,其中,一条边(一次连接)可以认为是一步,预设步数例如为1、2、3、4、5等,在此不做具体限定。请参考图8,在一个实施例中,当前拓扑图的节点1和全局拓扑图的节点6作为相似度高于预设相似度的节点对,以全局拓扑图的节点6作为中心节点,将节点6以及在全局拓扑图中节点6为中心周围预设范围内的节点作为新的全局拓扑图。
在某些实施方式中,匹配度最高的搜索分支的关联节点对为匹配度最高的关联节点对,步骤05(根据匹配度最高的搜索分支确定机器人100的当前位姿),包括:
052:根据匹配度最高的关联节点对确定机器人100的预处理初始位置;
054:基于预处理初始位置,对匹配度最高的关联节点对进行点云匹配,从而确定机器人100的当前位姿。
本发明实施方式的定位方法可以通过本发明实施方式的机器人100来实现。具体地,请结合图1,控制模块110用于:根据匹配度最高的关联节点对确定机器人100的预处理初始位置;基于预处理初始位置,对匹配度最高的关联节点对进行点云匹配,从而确定机器人100的当前位姿。
如此,能够通过匹配度最高的关联节点对确定机器人100的当前位姿。
具体地,关联节点对通过结合最小二乘法的三边定位法(也可以是结合ransac等其他方法)可以得到一个精确度不高的预处理初始位置,其中,预处理初始位置只有位置信息而没有姿态信息。在预处理初始位置作为初始估算的基础上,对匹配度最高的关联节点对进行点云匹配,从而得到精确的六自由度的当前位姿。将关联节点对对应当前拓扑图的节点的点云作为一组点云,将关联节点对对应全局拓扑图的节点的点云作为另一组点云,求一个最优的旋转矩阵R与平移t,使得其中一组点云转换后跟另一组点云最大重合。其中,根据匹配度最高的关联节点对确定机器人100的预处理初始位置,可以视作点云粗匹配,点云粗匹配可以采用SAC-IA(Sample Consensus Initial Aligment,采样一致性初始配准算法),SAC-IA不需要初始值,匹配误差较大。基于预处理初始位置,对匹配度最高的关联节点对进行点云匹配,从而确定机器人100的当前位姿,可以视作点云精匹配,点云精匹配可以采用ICP(Iterative Closest Point,迭代最近点算法),ICP对初始值敏感,匹配精确。
请再次参考图6,在一个实施例中,在在线阶段,可以生成当前拓扑图,根据当前拓扑图和离线阶段预先建立的全局拓扑图对全局拓扑图进行裁剪以获得新的全局拓扑图,根据新的全局拓扑图和当前拓扑图可以获得匹配度最高的关联节点对,根据匹配度最高的关联节点对,可以确定机器人100的预处理初始位置,再根据机器人100的预处理初始位置确定机器人100的当前位姿。
本申请实施方式的一种机器人100,包括存储器和处理器。存储器存储有计算机程序。处理器执行程序时,实现上述任一个实施方式的定位方法。请结合图1,在一个实施方式中,存储器可以为存储单元105,处理器可以为控制模块110。
例如,程序被处理器执行的情况下,实现以下定位方法的步骤:
01:获取当前拓扑图和全局拓扑图,当前拓扑图为根据机器人100当前观测到的环境中的物体建立的,全局拓扑图为根据预设区域内的全局环境中的物体预先建立的,当前拓扑图和全局拓扑图包括表示物体的节点;
02:对当前拓扑图和全局拓扑图中的节点进行匹配,匹配包括判断当前拓扑图中的节点与全局拓扑图中的节点构成的待关联节点对的关联程度;
03:若待关联节点对的关联程度大于阈值,则确定待关联节点对为关联节点对;
04:从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支;
05:根据匹配度最高的搜索分支确定机器人100的当前位姿。
上述机器人100,通过拓扑图匹配可以快速、准确地确定关联节点对(即关联物体对),从而可以通过当前拓扑图中和全局拓扑图中关联节点对数量最多的若干个关联节点能够快速地确定匹配度最高的关联节点对,进而可以根据匹配度最高的关联节点对确定机器人100的当前位姿。
本申请实施方式的一种计算机可读存储介质,其上存储有计算机程序。程序被处理器执行时,实现上述任一个实施方式的定位方法。
例如,程序被处理器执行的情况下,实现以下定位方法的步骤:
01:获取当前拓扑图和全局拓扑图,当前拓扑图为根据机器人100当前观测到的环境中的物体建立的,全局拓扑图为根据预设区域内的全局环境中的物体预先建立的,当前拓扑图和全局拓扑图包括表示物体的节点;
02:对当前拓扑图和全局拓扑图中的节点进行匹配,匹配包括判断当前拓扑图中的节点与全局拓扑图中的节点构成的待关联节点对的关联程度;
03:若待关联节点对的关联程度大于阈值,则确定待关联节点对为关联节点对;
04:从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支;
05:根据匹配度最高的搜索分支确定机器人100的当前位姿。
上述计算机可读存储介质,通过拓扑图匹配可以快速、准确地确定关联节点对(即关联物体对),从而可以通过当前拓扑图中和全局拓扑图中关联节点对数量最多的若干个关联节点能够快速地确定匹配度最高的关联节点对,进而可以根据匹配度最高的关联节点对确定机器人100的当前位姿。
计算机可读存储介质可设置在机器人100,也可设置在其它的终端设备,机器人100能够与其它的终端设备进行通信来获取到相应的程序。
可以理解,计算机可读存储介质可以包括:能够携带计算机程序的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM,Read-OnlyMemory)、随机存取存储器(RAM,Random Access Memory)、以及软件分发介质等。计算机程序包括计算机程序代码。计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读存储介质可以包括:能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM,Read-OnlyMemory)、随机存取存储器(RAM,Random Access Memory)、以及软件分发介质等。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为,表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分,并且本发明的优选实施方式的范围包括另外的实现,其中可以不按所示出或讨论的顺序,包括根据所涉及的功能按基本同时的方式或按相反的顺序,来执行功能,这应被本发明的实施例所属技术领域的技术人员所理解。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤,例如,可以被认为是用于实现逻辑功能的可执行指令的定序列表,可以具体实现在任何计算机可读介质中,以供指令执行系统、装置或设备(如基于计算机的系统、包括处理模块的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用,或结合这些指令执行系统、装置或设备而使用。
在本说明书的描述中,参考术语“一个实施方式”、“一些实施方式”、“示意性实施方式”、“示例”、“具体示例”、或“一些示例”等的描述意指结合实施方式或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施方式或示例中。在本说明书中,对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且,描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。
尽管已经示出和描述了本发明的实施方式,本领域的普通技术人员可以理解:在不脱离本发明的原理和宗旨的情况下可以对这些实施方式进行多种变化、修改、替换和变型,本发明的范围由权利要求及其等同物限定。
Claims (18)
1.一种定位方法,用于机器人,其特征在于,所述定位方法包括:
获取当前拓扑图和全局拓扑图,所述当前拓扑图为根据所述机器人当前观测到的环境中的物体建立的,所述全局拓扑图为根据预设区域内的全局环境中的物体预先建立的,所述当前拓扑图和所述全局拓扑图包括表示物体的节点;
对所述当前拓扑图和所述全局拓扑图中的节点进行匹配,所述匹配包括判断所述当前拓扑图中的节点与所述全局拓扑图中的节点构成的待关联节点对的关联程度;
若所述关联程度大于阈值,则确定所述待关联节点对为关联节点对;
从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支;
根据所述匹配度最高的搜索分支确定所述机器人的当前位姿。
2.根据权利要求1所述的定位方法,其特征在于,获取全局拓扑图,包括:
获取全局环境图像和全局深度图像;
根据所述全局环境图像和所述全局深度图像建立全局语义地图,所述全局语义地图包括全局点云信息和全局点云信息对应的第一物体种类标签;
根据所述第一物体种类标签对所述全局语义地图中的所述全局点云信息进行聚类,识别出各个物体,并获取各物体的第一包围盒,所述第一包围盒是将具有同一所述第一物体种类标签的点云聚类后的点云对应的独立空间;
以所述第一包围盒为节点,以相邻的两个所述第一包围盒的连线作为边,生成所述全局拓扑图,相邻的两个所述第一包围盒之间的距离小于预设距离。
3.根据权利要求2所述的定位方法,其特征在于,所述获取当前拓扑图,包括:
获取当前环境图像和当前深度图像;
根据所述当前环境图像和所述当前深度图像建立当前语义地图,所述当前语义地图包括当前点云信息和当前点云信息对应的第二物体种类标签;
根据所述第二物体种类标签对所述当前语义地图中的所述当前点云信息进行聚类,识别出各物体,并获取各物体的第二包围盒,所述第二包围盒是将具有同一所述第二物体种类标签的点云聚类后的点云对应的独立空间;
以所述第二包围盒为节点,以相邻的两个所述第二包围盒的连线作为边,生成所述当前拓扑图,相邻的两个所述第二包围盒之间的距离小于预设距离。
4.根据权利要求3所述的定位方法,其特征在于,所述从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支,包括:
将所述当前拓扑图和所述全局拓扑图结合构成搜索解释树,所述搜索解释树包括多个搜索分支,遍历每个搜索分支;
所述当前拓扑图的每一个节点构成第二层节点;
以所述第二层节点为父节点,所述第二层节点的子节点为所述全局拓扑图中的每一个节点并构成第三层节点,所述第二层节点和所述第三层节点构成所述搜索解释树的第一节点对;
确定所述第一节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签是否相同;
若第一节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签不同,则结束对应的搜索分支的构建;
若第一节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和所当前拓扑图中的节点所对应的述第二物体种类标签相同,则以所述第三层节点作为父节点,所述第三层节点的子节点为所述当前拓扑图除去所述搜索分支的上游分支出现过的节点后剩下的所有节点并构成第四层节点;以所述第四层节点为父节点,所述第四层节点的子节点为所述全局拓扑图中除去所述搜索分支的上游分支出现过的节点后剩下的所有节点并构成第五层节点,所述第四层节点和所述第五层节点构成所述搜索解释树的第二节点对;
确定所述第二节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签是否相同;
根据同一搜索分支中,所述第一节点对和所述第二节点对中的两个当前拓扑图节点之间的距离和夹角以及两个全局拓扑图节点之间的距离和夹角,获得所述第一节点对和所述第二节点对构成的所述待关联节点对的所述关联程度;
若第二节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签不同,或所述关联程度小于所述阈值,则结束对应的搜索分支的构建;
若第二节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签相同,且所述关联程度大于所述阈值,则继续遍历所述当前拓扑图与所述全局拓扑图的节点;
统计每个搜索分支的所述关联节点对的数量,所述关联节点对的数量最多的搜索分支作为所述匹配度最高的搜索分支。
5.根据权利要求3所述的定位方法,其特征在于,所述从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支之前,所述定位方法包括:
将所述当前拓扑图和所述全局拓扑图结合构成搜索解释树;
所述当前拓扑图的每一个节点构成第二层节点;
以所述第二层节点为父节点,所述第二层节点的子节点为所述全局拓扑图中的每一个节点并构成第三层节点;
以所述第三层节点作为父节点,所述第三层节点的子节点为所述当前拓扑图除去所述搜索分支的上游分支出现过的节点后剩下的所有节点并构成第四层节点;
以所述第四层节点为父节点,所述第四层节点的子节点为所述全局拓扑图中除去所述搜索分支的上游分支出现过的节点后剩下的所有节点并构成第五层节点;
以此类推继续遍历所述当前拓扑图与所述全局拓扑图的节点以构成所述搜索解释树。
6.根据权利要求5所述的定位方法,其特征在于,所述搜索解释树包括多个搜索分支,所述从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支,包括:
遍历每个搜索分支;
若所述搜索解释树的搜索分支包括所述关联节点对,则将所述搜索分支确定为匹配分支,否则,中断对应的搜索分支的搜索;
统计每个所述匹配分支的所述关联节点对的数量,所述关联节点对的数量最多的所述匹配分支作为所述匹配度最高的搜索分支。
7.根据权利要求6所述的定位方法,其特征在于,所述对所述当前拓扑图和所述全局拓扑图中的节点进行匹配,所述匹配包括判断所述当前拓扑图中的节点与所述全局拓扑图中的节点构成的待关联节点对的关联程度,包括:
判断所述当前拓扑图的第一节点和所述全局拓扑图的第一节点是否属于相同的物体种类标签,所述当前拓扑图的第一节点和所述全局拓扑图的第一节点构成第一节点对;
若所述当前拓扑图的第一节点和所述全局拓扑图的第一节点属于相同的物体种类标签,则判断所述当前拓扑图的第二节点和所述全局拓扑图的第二节点是否属于相同的物体种类标签,所述当前拓扑图的第二节点和所述全局拓扑图的第二节点构成第二节点对;
若所述当前拓扑图的第二节点和所述全局拓扑图的第二节点属于相同的物体种类标签,则将所述当前拓扑图的第一节点和所述当前拓扑图的第二节点之间的距离和夹角,与所述全局拓扑图的第一节点和所述全局拓扑图的第二节点之间的距离和夹角进行比对,获取所述第一节点对和所述第二节点对所构成的所述待关联节点对的关联程度,若所述关联程度大于阈值,则确定所述待关联节点对为关联节点对。
8.根据权利要求4或5所述的定位方法,其特征在于,在将所述当前拓扑图和所述全局拓扑图结合构成搜索解释树之前,所述定位方法还包括:
计算所述全局拓扑图中所述各物体的第一包围盒的大小和所述当前拓扑图中所述各物体的第二包围盒的大小;
统计所述全局拓扑图中第一物体种类标签对应的物体数量以确定第一稀有率和所述当前拓扑图中第二物体种类标签对应的物体数量以确定第二稀有率;
计算所述全局拓扑图中每个物体的第一随机游走特征和所述当前拓扑图中每个物体的第二随机游走特征;
根据所述第一包围盒的大小、所述第二包围盒的大小、所述第一稀有率、所述第二稀有率、所述第一随机游走特征、所述第二随机游走特征计算所述当前拓扑图和所述全局拓扑图中的拓扑节点集的相似度,所述拓扑节点集包括所述全局拓扑图的第一节点和所述当前拓扑图的第二节点,所述第一节点对应的第一物体种类标签与所述第二节点对应的第二物体种类标签相同;
确定所述拓扑节点集中所述相似度高于预设相似度的节点对,将所述节点对在所述全局拓扑图中的节点作为中心节点,将所述中心节点以及在所述全局拓扑图中所述中心节点为中心周围预设范围内的节点作为新的全局拓扑图,所述新的全局拓扑图用于与所述当前拓扑图结合构成一个所述搜索解释树。
9.一种机器人,其特征在于,所述机器人包括控制模块,所述控制模块用于:
获取当前拓扑图和全局拓扑图,所述当前拓扑图为根据所述机器人当前观测到的环境中的物体建立的,所述全局拓扑图为根据预设区域内的全局环境中的物体预先建立的,所述当前拓扑图和所述全局拓扑图包括表示物体的节点;
对所述当前拓扑图和所述全局拓扑图中的节点进行匹配,所述匹配包括判断所述当前拓扑图中的节点与所述全局拓扑图中的节点构成的待关联节点对的关联程度;
若所述关联程度大于阈值,则确定所述待关联节点对为关联节点对;
从多个搜索分支中,选出关联节点对数量最多的搜索分支,作为匹配度最高的搜索分支;
根据所述匹配度最高的搜索分支确定所述机器人的当前位姿。
10.根据权利要求9所述的机器人,其特征在于,所述控制模块用于:
获取摄像单元拍摄的全局环境图像和全局深度图像;
根据所述全局环境图像和所述全局深度图像建立全局语义地图,所述全局语义地图包括全局点云信息和全局点云信息对应的第一物体种类标签;
根据所述第一物体种类标签对所述全局语义地图中的所述全局点云信息进行聚类,识别出各个物体,并获取各物体的第一包围盒,所述第一包围盒是将具有同一所述第一物体种类标签的点云聚类后的点云对应的独立空间;
以所述第一包围盒为节点,以相邻的两个所述第一包围盒的连线作为边,生成所述全局拓扑图,相邻的两个所述第一包围盒之间的距离小于预设距离。
11.根据权利要求10所述的机器人,其特征在于,所述控制模块用于:
获取摄像单元拍摄的当前环境图像和当前深度图像;
根据所述当前环境图像和所述当前深度图像建立当前语义地图,所述当前语义地图包括当前点云信息和当前点云信息对应的第二物体种类标签;
根据所述第二物体种类标签对所述当前语义地图中的所述当前点云信息进行聚类,识别出各物体,并获取各物体的第二包围盒,所述第二包围盒是将具有同一所述第二物体种类标签的点云聚类后的点云对应的独立空间;
以所述第二包围盒为节点,以相邻的两个所述第二包围盒的连线作为边,生成所述当前拓扑图,相邻的两个所述第二包围盒之间的距离小于预设距离。
12.根据权利要求11所述的机器人,其特征在于,所述控制模块用于:
将所述当前拓扑图和所述全局拓扑图结合构成搜索解释树,所述搜索解释树包括多个搜索分支,遍历每个搜索分支;
所述当前拓扑图的每一个节点构成第二层节点;
以所述第二层节点为父节点,所述第二层节点的子节点为所述全局拓扑图中的每一个节点并构成第三层节点,所述第二层节点和所述第三层节点构成所述搜索解释树的第一节点对;
确定所述第一节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签是否相同;
若第一节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签不同,则结束对应的搜索分支的构建;
若第一节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签相同,则以所述第三层节点作为父节点,所述第三层节点的子节点为所述当前拓扑图除去所述搜索分支的上游分支出现过的节点后剩下的所有节点并构成第四层节点;以所述第四层节点为父节点,所述第四层节点的子节点为所述全局拓扑图中除去所述搜索分支的上游分支出现过的节点后剩下的所有节点并构成第五层节点,所述第四层节点和所述第五层节点构成所述搜索解释树的第二节点对;
确定所述第二节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签是否相同;
根据同一搜索分支中,所述第一节点对和所述第二节点对中的两个当前拓扑图节点之间的距离和夹角以及两个全局拓扑图节点之间的距离和夹角,获得所述第一节点对和所述第二节点对构成的所述待关联节点对的所述关联程度;
若第二节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签不同,或所述关联程度小于所述阈值,则结束对应的搜索分支的构建;
若第二节点对中全局拓扑图中的节点所对应的所述第一物体种类标签和当前拓扑图中的节点所对应的所述第二物体种类标签相同,且所述关联程度大于所述阈值,则继续遍历所述当前拓扑图与所述全局拓扑图的节点;
统计每个搜索分支的所述关联节点对的数量,所述关联节点对的数量最多的搜索分支作为所述匹配度最高的搜索分支。
13.根据权利要求11所述的机器人,其特征在于,所述控制模块用于:
将所述当前拓扑图和所述全局拓扑图结合构成搜索解释树;
所述当前拓扑图的每一个节点构成第二层节点;
以所述第二层节点为父节点,所述第二层节点的子节点为所述全局拓扑图中的每一个节点并构成第三层节点;
以所述第三层节点作为父节点,所述第三层节点的子节点为所述当前拓扑图除去所述搜索分支的上游分支出现过的节点后剩下的所有节点并构成第四层节点;
以所述第四层节点为父节点,所述第四层节点的子节点为所述全局拓扑图中除去所述搜索分支的上游分支出现过的节点后剩下的所有节点并构成第五层节点;
以此类推继续遍历所述当前拓扑图与所述全局拓扑图的节点以构成所述搜索解释树。
14.根据权利要求13所述的机器人,其特征在于,所述搜索解释树包括多个搜索分支,所述控制模块用于:
遍历每个搜索分支;
若所述搜索解释树的搜索分支包括所述关联节点对,则将所述搜索分支确定为匹配分支,否则,中断对应的搜索分支的搜索;
统计每个所述匹配分支的所述关联节点对的数量,所述关联节点对的数量最多的所述匹配分支作为所述匹配度最高的搜索分支。
15.根据权利要求14所述的机器人,其特征在于,所述控制模块用于:
判断所述当前拓扑图的第一节点和所述全局拓扑图的第一节点是否属于相同的物体种类标签,所述当前拓扑图的第一节点和所述全局拓扑图的第一节点构成第一节点对;
若所述当前拓扑图的第一节点和所述全局拓扑图的第一节点属于相同的物体种类标签,则判断所述当前拓扑图的第二节点和所述全局拓扑图的第二节点是否属于相同的物体种类标签,所述当前拓扑图的第二节点和所述全局拓扑图的第二节点构成第二节点对;
若所述当前拓扑图的第二节点和所述全局拓扑图的第二节点属于相同的物体种类标签,则将所述当前拓扑图的第一节点和所述当前拓扑图的第二节点之间的距离和夹角,与所述全局拓扑图的第一节点和所述全局拓扑图的第二节点之间的距离和夹角进行比对,获取所述第一节点对和所述第二节点对所构成的所述待关联节点对的关联程度,若所述关联程度大于阈值,则确定所述待关联节点对为关联节点对。
16.根据权利要求12或13所述的机器人,其特征在于,所述控制模块用于:
计算所述全局拓扑图中所述各物体的第一包围盒的大小和所述当前拓扑图中所述各物体的第二包围盒的大小;
统计所述全局拓扑图中第一物体种类标签对应的物体数量以确定第一稀有率和所述当前拓扑图中第二物体种类标签对应的物体数量以确定第二稀有率;
计算所述全局拓扑图中每个物体的第一随机游走特征和所述当前拓扑图中每个物体的第二随机游走特征;
根据所述第一包围盒的大小、所述第二包围盒的大小、所述第一稀有率、所述第二稀有率、所述第一随机游走特征、所述第二随机游走特征计算所述当前拓扑图和所述全局拓扑图中的拓扑节点集的相似度,所述拓扑节点集包括所述全局拓扑图的第一节点和所述当前拓扑图的第二节点,所述第一节点对应的第一物体种类标签与所述第二节点对应的第二物体种类标签相同;
确定所述拓扑节点集中所述相似度高于预设相似度的节点对,将所述节点对在所述全局拓扑图中的节点作为中心节点,将所述中心节点以及在所述全局拓扑图中所述中心节点为中心周围预设范围内的节点作为新的全局拓扑图,所述新的全局拓扑图用于与所述当前拓扑图结合构成一个所述搜索解释树。
17.一种机器人,其特征在于,所述机器人包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时,实现权利要求1-8任一项所述的定位方法。
18.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1-8任一项所述的定位方法。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211069393.2A CN115164906B (zh) | 2022-09-02 | 2022-09-02 | 定位方法、机器人和计算机可读存储介质 |
US18/238,118 US20240077875A1 (en) | 2022-09-02 | 2023-08-25 | Robot and method for robot positioning |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211069393.2A CN115164906B (zh) | 2022-09-02 | 2022-09-02 | 定位方法、机器人和计算机可读存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115164906A true CN115164906A (zh) | 2022-10-11 |
CN115164906B CN115164906B (zh) | 2022-11-15 |
Family
ID=83481676
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211069393.2A Active CN115164906B (zh) | 2022-09-02 | 2022-09-02 | 定位方法、机器人和计算机可读存储介质 |
Country Status (2)
Country | Link |
---|---|
US (1) | US20240077875A1 (zh) |
CN (1) | CN115164906B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117213500A (zh) * | 2023-11-08 | 2023-12-12 | 北京理工大学前沿技术研究院 | 基于动态点云与拓扑路网的机器人全局定位方法及系统 |
CN117572454A (zh) * | 2023-11-15 | 2024-02-20 | 武汉万曦智能科技有限公司 | 场车蓄电池安全间隙测量方法及系统 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104123279A (zh) * | 2013-04-24 | 2014-10-29 | 腾讯科技(深圳)有限公司 | 关键词的聚类方法和装置 |
CN107063258A (zh) * | 2017-03-07 | 2017-08-18 | 重庆邮电大学 | 一种基于语义信息的移动机器人室内导航方法 |
KR20190064311A (ko) * | 2017-11-30 | 2019-06-10 | 주식회사 모빌테크 | 라이다를 이용한 지도 생성 방법 및 장치 |
CN110376594A (zh) * | 2018-08-17 | 2019-10-25 | 北京京东尚科信息技术有限公司 | 一种基于拓扑图的智能导航的方法和系统 |
WO2020253842A1 (zh) * | 2019-06-20 | 2020-12-24 | 杭州海康威视数字技术股份有限公司 | 一种车辆位姿确定方法、装置及电子设备 |
CN114683290A (zh) * | 2022-05-31 | 2022-07-01 | 深圳鹏行智能研究有限公司 | 一种足式机器人位姿优化的方法,装置以及存储介质 |
-
2022
- 2022-09-02 CN CN202211069393.2A patent/CN115164906B/zh active Active
-
2023
- 2023-08-25 US US18/238,118 patent/US20240077875A1/en active Pending
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104123279A (zh) * | 2013-04-24 | 2014-10-29 | 腾讯科技(深圳)有限公司 | 关键词的聚类方法和装置 |
CN107063258A (zh) * | 2017-03-07 | 2017-08-18 | 重庆邮电大学 | 一种基于语义信息的移动机器人室内导航方法 |
KR20190064311A (ko) * | 2017-11-30 | 2019-06-10 | 주식회사 모빌테크 | 라이다를 이용한 지도 생성 방법 및 장치 |
CN110376594A (zh) * | 2018-08-17 | 2019-10-25 | 北京京东尚科信息技术有限公司 | 一种基于拓扑图的智能导航的方法和系统 |
WO2020253842A1 (zh) * | 2019-06-20 | 2020-12-24 | 杭州海康威视数字技术股份有限公司 | 一种车辆位姿确定方法、装置及电子设备 |
CN114683290A (zh) * | 2022-05-31 | 2022-07-01 | 深圳鹏行智能研究有限公司 | 一种足式机器人位姿优化的方法,装置以及存储介质 |
Non-Patent Citations (1)
Title |
---|
金泰宇等: ""一种基于语义地图的激光雷达定位方法"", 《测绘地理信息》 * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117213500A (zh) * | 2023-11-08 | 2023-12-12 | 北京理工大学前沿技术研究院 | 基于动态点云与拓扑路网的机器人全局定位方法及系统 |
CN117213500B (zh) * | 2023-11-08 | 2024-02-13 | 北京理工大学前沿技术研究院 | 基于动态点云与拓扑路网的机器人全局定位方法及系统 |
CN117572454A (zh) * | 2023-11-15 | 2024-02-20 | 武汉万曦智能科技有限公司 | 场车蓄电池安全间隙测量方法及系统 |
CN117572454B (zh) * | 2023-11-15 | 2024-05-10 | 武汉万曦智能科技有限公司 | 场车蓄电池安全间隙测量方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
US20240077875A1 (en) | 2024-03-07 |
CN115164906B (zh) | 2022-11-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN115164906B (zh) | 定位方法、机器人和计算机可读存储介质 | |
US11127203B2 (en) | Leveraging crowdsourced data for localization and mapping within an environment | |
CN106092104B (zh) | 一种室内机器人的重定位方法及装置 | |
CN108801268B (zh) | 目标对象的定位方法、装置及机器人 | |
TWI467494B (zh) | 使用深度圖進行移動式攝影機定位 | |
WO2020107772A1 (zh) | 机器人地图构建和定位 | |
Erol et al. | Improved deep neural network object tracking system for applications in home robotics | |
WO2022052660A1 (zh) | 仓储机器人定位与地图构建方法、机器人及存储介质 | |
JP6853156B2 (ja) | 姿勢推定システム、姿勢推定装置、及び距離画像カメラ | |
US10078333B1 (en) | Efficient mapping of robot environment | |
WO2020063878A1 (zh) | 一种处理数据的方法和装置 | |
CN114683290B (zh) | 一种足式机器人位姿优化的方法,装置以及存储介质 | |
WO2023125363A1 (zh) | 电子围栏自动生成方法、实时检测方法及装置 | |
US20230245476A1 (en) | Location discovery | |
WO2023031620A1 (en) | Incremental dense 3-d mapping with semantics | |
KR20210083198A (ko) | 증강 현실 장치 및 위치 설정 방법 | |
Ishihara et al. | Deep radio-visual localization | |
Al-Forati et al. | Design and implementation an indoor robot localization system using minimum bounded circle algorithm | |
Zingaretti et al. | Appearance based robotics | |
Davis et al. | Reflective fiducials for localization with 3D light detection and ranging scanners | |
Lee et al. | Development of indoor localization system using a mobile data acquisition platform and BoW image matching | |
Hernández et al. | Visual SLAM with oriented landmarks and partial odometry | |
Zhang et al. | ARCargo: Multi-Device Integrated Cargo Loading Management System with Augmented Reality | |
Guan et al. | Recognition-driven 3D navigation in large-scale virtual environments | |
Wu | A Low-cost SLAM Fusion Algorithm for Robot Localization |
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 |