具体实施方式
下面结合附图对本发明的具体实施方式进行说明。
图1为机器人导航中位姿的确定方法实施流程示意图,如图所示,可以包括:
步骤101、根据机器人的第一定位,确定所述机器人的真实点云;
步骤102、根据机器人第一定位下的位姿,确定该定位下的虚拟点云;
步骤103、将虚拟点云与真实点云进行匹配,根据所述匹配结果确定机器人的第二位姿。
进一步的,本发明实施例中,在根据所述机器人的第一定位,确定所述机器人的真实点云之前,还需要确定所述机器人的第一定位。
具体的,本申请实施例主要根据所述机器人的轮式里程计和/或激光雷达数据进行AMCL自适应蒙特卡洛定位算法定位;然后通过PLICP基于点到线距离的迭代最近邻点搜索算法对所述定位结果进行优化,最后将优化后的定位结果确定为所述第一定位。
实施中,本发明实施例中主要通过下列方式确定所述第一定位下的虚拟点云:
根据所述机器人的第一定位估算第一位姿;以所述机器人的第一位姿为基础,将从占据栅格地图中截取周围的地图环境轮廓所形成的点云确定为该定位下的虚拟点云。
实施中,真实点云是通过机器人的2D激光雷达采集得到的。
需要说明的是,确定所述第一定位下的真实点云并不一定在确定所述第一定位下的位姿之前实施,而是可以在需要时进行确定,换言之,在匹配前获取即可。位姿是指工业机器人未端操作器在指定坐标系中的位置和姿态。
具体的,本发明实施例中,在确定所述第二位姿时,主要通过将虚拟点云与真实点云进行匹配,得到旋转和平移量;然后根据所述旋转和平移量对所述第一位姿进行修正,最后将所述修正后获得的位姿结果确定为所述第二位姿,其中,所述修正后获得的位姿结果,是通过将所述旋转和平移量施加给AMCL的定位输出后得到的。
进一步的,虚拟点云与真实点云是通过PLICP算法进行匹配的。
在实施中选用AMCL,是因为AMCL是移动机器人激光定位的经典算法,其具有计算代价低、稳定性强、抗干扰能力强的优点,所以这里选用AMCL。但也存在定位精度不高的缺点,因此实施中选用PLICP去修正AMCL的定位结果。但是,从理论上来说,用其它的定位算法也是可以的,AMCL仅用于教导本领域技术人员具体如何实施本发明,但不意味仅能使用AMCL一种方式,实施过程中可以结合实践需要来确定相应的算法。
在实施中选用PLICP,是因为PLICP计算复杂度低,且精度高。从理论上来说,用其它的算法也是可以的,例如普通的ICP算法、Point-to-SurfaceICP算法也是可以选用的,只是它们在2D激光定位的精度都不如PLCIP高;PLICP仅用于教导本领域技术人员具体如何实施本发明,但不意味仅能使用PLICP一种方式,实施过程中可以结合实践需要来确定相应的算法。
实施中,在确定机器人的定位时,机器人所处的位置位于全局路径规划模块规划的路径之上。
具体实施中,机器人所处的位置位于分段后的全局路径规划模块规划的路径之上,其中,全局路径规划模块规划的路径在遍历所有路径点后,按如下条件之一或者其组合分段:
如果当前路径点是任务点,则将上一个拆分位置到当前路径点作为一段道路段;
如果从当前路径点开始道路方向发生了改变,则将上一个拆分位置到当前路径点作为一段道路段;
如果从当前路径点开始行进模式发生了改变,则将上一个拆分位置到当前路径点作为一段道路段;
如果当前路径点机器人需要旋转,则将该路径点当成单独的道路段。
进一步的,本发明实施例中,在完成所述机器人导航中第二位姿的确定后,为了更好的保障所述第二位姿的准确性,还可以对所述第二位姿的准确性进行判断,并在所述第二位姿准确性不足时,对所述第二位姿进行纠正。
具体的,本发明实施例中可以先预设配对点的数目占真实点云中点的数目的百分比的阈值;然后将所述匹配结果与所述阈值进行比较,若所述匹配结果不小于所述阈值,则确定所述第二位姿正确,并通过所述第二位姿进行导航;若所述匹配结果小于所述阈值,则确定所述第二位姿异常,并对所述第二位姿进行纠正。
进一步的,在确定所述第二位姿异常时,可以通过下列方式对所述第二位姿进行纠正。
具体的,采用局部栅格化搜索的方式选取所述匹配结果最高且超过阈值的对应位置和角度组合做为纠正后的位姿。
通过将虚拟点云与真实点云进行匹配的方式计算定位的可信度,从而能够及时判断机器人定位异常的情况。当机器人定位异常时,采用局部栅格化搜索的方式纠正位姿,保证了导航的可靠性。
实施中,还可以进一步包括:
在遇障碍物时,若在设定时间内障碍物未消失,则进入DWA动态窗口法局部路径规划后,循迹行驶;
在遇障碍物时,若在设定时间内障碍物消失,则进入DWA规划一条沿全局路径行驶的局部路径后,循迹行驶。
下面结合实例进行说明,以便更好地理解本发明的实施。
例中提出了可以用于巡检机器人的高精度混合导航方案,方案中通过将循迹导航与自由导航相结合的混合导航模式实现导航精度和导航自由度的平衡;通过AMCL+PLICP后计算定位评分的方法得到高精度定位结果及其可信度;通过2D激光与地图进行匹配的方式重新纠正自身定位结果以提高导航的可靠性。
例中的“导航”实际上包含三部分概念:定位、全局路径规划、局部路径规划。本例在全局路径规划中进行合理的路径拆分、结合高精度定位算法实现高精度循迹导航,且局部路径规划采用等待超时机制保留了自由导航的灵活度,是一种兼顾导航精度和自由度的方案。具体的,在例中:
在局部路径规划执行路径中,将说明通过将循迹导航与自由导航相结合的混合导航模式实现导航精度和导航自由度的平衡的实施;
在AMCL+PLICP定位中,将说明通过AMCL+PLICP后计算定位评分的方法得到高精度定位结果及其可信度的实施;
在纠正位姿中,将说明通过2D激光与地图进行匹配的方式重新纠正自身定位结果以提高导航的可靠性的实施;
在路径点序列拆分中,将说明将导航路径点序列根据路径点的属性、行进模式和方向进行拆分得到若干段道路段以保证每一道路段内机器人均是匀速直线行驶或原地旋转,从而保证导航精度的实施。
例中将以2D激光雷达为主要传感器,以轮式移动机器人为载体来实现用于室内外巡检任务的高精度混合导航方案。
图2为导航方案实施流程示意图,如图所示,可以包括:
步骤201、下发巡检任务。
步骤202、全局路径规划产生拓扑路径点序列。
实施中,使用占据栅格地图和拓扑地图两类地图描述环境信息,其中占据栅格地图用于局部路径规划,拓扑地图用于全局路径规划。
全局路径规划模块根据巡检任务找到一条能够覆盖所有巡检任务点的最合适的路径,比如最短路径,该路径包含一系列拓扑路径点{P1,P2,…,PN},其中每个路径点都包含以下信息:Pi={xi,yi,θi,Ti,Hi},其中x,y表示该路径点的坐标,θ表示期望机器人运动到该点时的朝向,通常仅需要巡检的任务点具有朝向;T=0表示该点不是任务点,T=1表示该点是任务点;H表示机器人从该点出发的运动模式,H=0表示机器人从该点开始正向前进,H=1表示机器人从该点开始倒退前进,H=2表示机器人维持原来的运动模式不变。例中的表示仅用以示意,也可以用其他形式来表述路径点。
步骤203、路径点序列拆分。
将步骤202中得到的路径点序列进行拆分后得到一系列道路段。路径点序列拆分的方式可以是按照顺序遍历所有路径点,当满足以下四个条件中的任意一个或者其组合时即进行拆分:
(1)如果当前路径点是任务点,则将上一个拆分位置到当前路径点作为一段道路段;
(2)如果从当前路径点开始道路方向发生了改变,则将上一个拆分位置到当前路径点作为一段道路段;
(3)如果从当前路径点开始行进模式发生了改变,则将上一个拆分位置到当前路径点作为一段道路段;
(4)如果当前路径点机器人需要旋转,则将该路径点当成单独的道路段。
图3为路径点序列拆分示意图,如图3所示,本例中假设路径点序列包含6个路径点:
P1是起始点。
P2开始道路方向改变,机器人需要再此处原地旋转;
P3是任务点且要求机器人转到0o;
P4开始要求机器人以倒退模式行进;
P5维持行进模式不变;
P6也是任务点且要求机器人转到改回正向前进模式。
则,可按上述条件将路径序列拆分成7段道路段{R1,R2,R3,R4,R5,R6,R7},其中,R1={P1,P2},R2={P2},R3={P2,P3},R4={P3,P4},R5={P4},R6={P4,P5,P6},R7={P6}。
由该实施可以看出,按上述方案根据任务点的属性、行进模式、方向等要素将全局路径点序列拆分成若干段道路段后,可以使每一段道路段内机器人均为匀速直线行驶或原地旋转,保证了循迹导航的高精度。
步骤204、路径点插值。
例中,将针对步骤203拆分得到的某一道路段Ri={Pm,Pm+1,…,Pn},将其插值成为一段离散的位姿点序列,具体步骤可以为:
(1)先设定一个空的位姿点集合Path,同时取占据栅格地图的分辨率作为位姿点序列的分辨率,设为s;
(2)遍历R
i中所有相邻点对,如P
m和P
m+1,求其欧氏距离
并确定位姿点数目
然后将位姿点集合Path与当前插值得到的位姿点集合
合并,其中所有位姿点的角度θ均等于P
m和P
m+1所形成向量的方向;
(3)对于只有一个路径点的道路段,如{Pj},则将{(xj,yj,θj)}单独构成一个位姿集合,其中θj的取值为:
若Pj是任务点,则θj等于任务点的方向,否则θj等于Pj和Pj+1所形成向量的方向;
图4为路径插值示意图,如图4所示,将图3拆分出来的道路段插值后得到7个位姿点集合,其中在道路点P2、P4、P6分别生成仅包含一个位姿的集合,表示机器人在此处旋转。
步骤205、根据步骤204插值得到的位姿点集合,规划并执行局部路径。
具体流程可以如下:
步骤2051、AMCL+PLICP定位。
实施中,可以根据机器人的轮式里程计、激光雷达数据进行AMCL(Adaptive MonteCarlo Localization,自适应蒙特卡洛定位)算法定位,并用PLICP(Point-to-LineIterative Closest Point,基于点到线距离的迭代最近邻点搜索)算法优化定位结果:
具体的,以机器人当前估算的定位位姿为基础,从占据栅格地图中截取周围的地图环境轮廓,形成虚拟点云,同时机器人的2D激光雷达采集得到真实点云,将虚拟点云与真实点云通过PLICP算法进行匹配,得到旋转和平移量RΔ、TΔ,将该旋转和平移量施加给AMCL的定位输出,得到更加精确的定位结果。
步骤2052、判断定位评分是否大于阈值,是则转入步骤2054,否则转入步骤2053。
实施中,计算当前定位的评分的方式可以如下:
用步骤2051中的方式生成虚拟点云,将其与真实点云通过PLICP匹配,计算配对点的数目占点云中2D点的数目的百分比,作为当前定位的评分,图5为计算定位评分与位姿纠正前示意图,如图5所示;
如果步骤2052中计算的定位评分未达到阈值,说明当前定位异常,需要纠正位姿,进入步骤2053。
步骤2053、纠正位姿。
具体的,纠正位姿的方式可用栅格化搜索:
以当前的定位位置为中心,将周围width×height范围按照分辨率rxy划分成栅格,遍历这些栅格,在每个栅格内按照ra遍历0o~360o,对每个位置和角度的组合按照步骤2052的方式计算定位评分。
依此遍历结束后,取评分最高且超过阈值的对应位置和角度组合当做正确的位姿,图6为计算定位评分与位姿纠正后示意图,如图6所示;
例中,由于采用AMCL+PLICP算法进行定位,并通过将虚拟点云与真实点云进行匹配的方式计算定位的可信度,从而能够及时判断机器人定位异常的情况。当机器人定位异常时,再采用局部栅格化搜索的方式纠正位姿,保证了导航的可靠性。
步骤2054、判断前方是否遇到障碍物,是则转入步骤2055,否则转入步骤2057。
步骤2055、等待障碍解除。
步骤2056、判断是否超时,是则转入步骤2057,否则转入步骤2054。
步骤2057、DWA规划局部路径。
具体如下,图7为避障逻辑示意图,判断前方是否遇到障碍物时的处理如图7所示,若通过激光雷达在前进方向上发现前方一定的预设距离内发现有障碍物,则立即停车等待障碍物消除。若在等待时间阈值内障碍物消除,则直接进入DWA(Dynamic WindowApproach,动态窗口法)局部路径规划,此时由于没有障碍物因此DWA将让机器人继续循迹行驶;
若等待障碍物时间超过阈值,则直接进行DWA局部路径规划。DWA局部路径规划会根据当前机器人前进或倒退的行进模式生成一条绕开障碍物的局部路径。
也即,若通过激光雷达在前进方向上发现前方一定的预设距离内发现有障碍物,则立即停车并持续判断障碍物是否消除直到超过等待时间。若等待时间阈值内检测到障碍物消除,则由DWA规划一条沿着全局路径行驶的局部路径;若在等待时间阈值内障碍物未消除,则由DWA规划一条绕开障碍物行驶的局部路径,此时局部路径并不贴合着全局路径。
步骤2058、判断是否到达目标点,是则结束步骤205转入步骤206,否则转入步骤2051。
具体的,若机器人到达了当前道路段的目标点则结束本步骤205,否则从2051开始执行;
例中,由于在遇到障碍物的情况下先停车等待,若一定时间内障碍物解除则继续循迹导航,若超时则进入临时自由导航绕开障碍物,因而保证了导航的自由度。
步骤206、判断路径点序列是否执行完毕,是则结束,否则转入步骤203。
若所有路径点序列被执行完毕,则结束导航任务,否则重复步骤203至步骤205;
在一些可能的实施方式中,本发明实施例提供的网络重组的各个方面还可以实现为一种程序产品的形式,其包括程序代码,当所述程序代码在计算机设备上运行时,所述程序代码用于使所述计算机设备执行本说明书中描述的根据本发明各种示例性实施方式的数据传输的方法中的步骤。
所述程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的***、装置或器件,或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
根据本发明的实施方式的用于数据转发控制的程序产品,其可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码,并可以在服务器设备上运行。然而,本发明的程序产品不限于此,在本文件中,可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被信息传输、装置或者器件使用或者与其结合使用。
可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了可读程序代码。这种传播的数据信号可以采用多种形式,包括——但不限于——电磁信号、光信号或上述的任意合适的组合。可读信号介质还可以是可读存储介质以外的任何可读介质,该可读介质可以发送、传播或者传输用于由周期网络动作***、装置或者器件使用或者与其结合使用的程序。
可读介质上包含的程序代码可以用任何适当的介质传输,包括——但不限于——无线、有线、光缆、RF等等,或者上述的任意合适的组合。
可以以一种或多种程序设计语言的任意组合来编写用于执行本发明操作的程序代码,所述程序设计语言包括面向对象的程序设计语言—诸如Java、C++等,还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中,远程计算设备可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算设备,或者,可以连接到外部计算设备。
基于同一发明构思,本发明实施例中还提供了一种计算机设备、计算机可读存储介质、机器人导航中位姿的确定装置,由于这些设备解决问题的原理与机器人导航中位姿的确定方法相似,因此这些设备的实施可以参见方法的实施,重复之处不再赘述。
如图8所示,本申请一种机器人导航中位姿确定的设备,该网络设备包括处理器800、存储器801和通信接口802。
处理器800负责管理总线架构和通常的处理,存储器801可以存储处理器800在执行操作时所使用的数据。通信接口802用于在处理器800的控制下接收和发送数据与存储器801进行数据通信。
所述处理器800可以是中央处理器(central processing unit,CPU),网络处理器(network processor,NP)或者CPU和NP的组合。所述处理器800还可以进一步包括硬件芯片。上述硬件芯片可以是专用集成电路(application-specific integrated circuit,ASIC),可编程逻辑器件(programmable logic device,PLD)或其组合。上述PLD可以是复杂可编程逻辑器件(complex programmable logic device,CPLD),现场可编程逻辑门阵列(field-programmable gate array,FPGA),通用阵列逻辑(generic array logic,GAL)或其任意组合。存储器801可以包括:U盘、移动硬盘、只读存储器(read-only memory,ROM)、随机存取存储器(random access memory,RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
所述处理器800、所述存储器801以及所述通信接口802之间相互连接。可选的,所述处理器800、所述存储器801以及所述通信接口802可以通过总线803相互连接;所述总线803可以是外设部件互连标准(peripheral component interconnect,PCI)总线或扩展工业标准结构(extended industry standard architecture,EISA)总线等。所述总线可以分为地址总线、数据总线、控制总线等。为便于表示,图8中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
具体地,所述处理器800,用于读取存储器801中的程序并执行:
根据机器人的第一定位,确定所述机器人的真实点云;
根据机器人第一定位下的位姿,确定该定位下的虚拟点云;
将虚拟点云与真实点云进行匹配,根据所述匹配结果确定机器人的第二位姿。
实施中,所述处理器800具体用于:
根据所述机器人的第一定位,将通过机器人的2D激光雷达采集得到的点云确定为真实点云。
实施中,所述处理器800具体用于:
根据所述机器人的第一定位估算第一位姿;
以所述机器人的第一位姿为基础,将从占据栅格地图中截取周围的地图环境轮廓所形成的点云确定为该定位下的虚拟点云。
实施中,所述处理器800具体用于:
通过将虚拟点云与真实点云进行匹配,得到旋转和平移量;
根据所述旋转和平移量对所述第一位姿进行修正,并将所述修正后获得的位姿结果确定为所述第二位姿。
实施中,所述处理器800还用于:
根据所述机器人的轮式里程计和/或激光雷达数据进行AMCL自适应蒙特卡洛定位算法定位;
通过PLICP基于点到线距离的迭代最近邻点搜索算法对所述定位结果进行优化,并将优化后的定位结果确定为所述第一定位。
实施中,虚拟点云与真实点云是通过PLICP算法进行匹配的。
实施中,根据所述旋转和平移量对第一位姿进行修正后获得的位姿结果,是通过将所述旋转和平移量施加给AMCL的定位输出后得到的。
实施中,在确定机器人的定位时,机器人所处的位置位于全局路径规划模块规划的路径之上。
实施中,机器人所处的位置位于分段后的全局路径规划模块规划的路径之上,其中,全局路径规划模块规划的路径在遍历所有路径点后,按如下条件之一或者其组合分段:
如果当前路径点是任务点,则将上一个拆分位置到当前路径点作为一段道路段;
如果从当前路径点开始道路方向发生了改变,则将上一个拆分位置到当前路径点作为一段道路段;
如果从当前路径点开始行进模式发生了改变,则将上一个拆分位置到当前路径点作为一段道路段;
如果当前路径点机器人需要旋转,则将该路径点当成单独的道路段。
实施中,所述处理器800还用于:
预设配对点的数目占真实点云中点的数目的百分比的阈值;
将所述匹配结果与所述阈值进行比较,若所述匹配结果不小于所述阈值,则确定所述第二位姿正确,并通过所述第二位姿进行导航;若所述匹配结果小于所述阈值,则确定所述第二位姿异常,并对所述第二位姿进行纠正。
实施中,通过下列方式对所述第二位姿进行纠正:
采用局部栅格化搜索的方式选取所述匹配结果最高且超过阈值的对应位置和角度组合做为纠正后的位姿。
实施中,所述处理器800还用于:
在遇障碍物时,若在设定时间内障碍物未消失,则进入DWA动态窗口法局部路径规划后,循迹行驶;
在遇障碍物时,若在设定时间内障碍物消失,则进入DWA规划一条沿全局路径行驶的局部路径后,循迹行驶。
如图9所示,本发明实施例提供一种机器人导航中位姿确定的设备,包括确定模块900和处理模块901:
确定模块900:用于根据机器人的第一定位,确定所述机器人的真实点云;根据机器人第一定位下的位姿,确定该定位下的虚拟点云;
处理模块901:用于将虚拟点云与真实点云进行匹配,根据所述匹配结果确定机器人的第二位姿。
可选的,所述确定模块900具体用于:
根据所述机器人的第一定位,将通过机器人的2D激光雷达采集得到的点云确定为真实点云。
可选的,所述确定模块900具体用于:
根据所述机器人的第一定位估算第一位姿;
以所述机器人的第一位姿为基础,将从占据栅格地图中截取周围的地图环境轮廓所形成的点云确定为该定位下的虚拟点云。
可选的,所述处理模块901具体用于:
通过将虚拟点云与真实点云进行匹配,得到旋转和平移量;
根据所述旋转和平移量对所述第一位姿进行修正,并将所述修正后获得的位姿结果确定为所述第二位姿。
可选的,所述处理模块901还用于:
根据所述机器人的轮式里程计和/或激光雷达数据进行AMCL自适应蒙特卡洛定位算法定位;
通过PLICP基于点到线距离的迭代最近邻点搜索算法对所述定位结果进行优化,并将优化后的定位结果确定为所述第一定位。
可选的,虚拟点云与真实点云是通过PLICP算法进行匹配的。
可选的,根据所述旋转和平移量对第一位姿进行修正后获得的位姿结果,是通过将所述旋转和平移量施加给AMCL的定位输出后得到的。
可选的,在确定机器人的定位时,机器人所处的位置位于全局路径规划模块规划的路径之上。
可选的,机器人所处的位置位于分段后的全局路径规划模块规划的路径之上,其中,全局路径规划模块规划的路径在遍历所有路径点后,按如下条件之一或者其组合分段:
如果当前路径点是任务点,则将上一个拆分位置到当前路径点作为一段道路段;
如果从当前路径点开始道路方向发生了改变,则将上一个拆分位置到当前路径点作为一段道路段;
如果从当前路径点开始行进模式发生了改变,则将上一个拆分位置到当前路径点作为一段道路段;
如果当前路径点机器人需要旋转,则将该路径点当成单独的道路段。
可选的,所述处理模块901还用于:
预设配对点的数目占真实点云中点的数目的百分比的阈值;
将所述匹配结果与所述阈值进行比较,若所述匹配结果不小于所述阈值,则确定所述第二位姿正确,并通过所述第二位姿进行导航;若所述匹配结果小于所述阈值,则确定所述第二位姿异常,并对所述第二位姿进行纠正。
可选的,通过下列方式对所述第二位姿进行纠正:
采用局部栅格化搜索的方式选取所述匹配结果最高且超过阈值的对应位置和角度组合做为纠正后的位姿。
可选的,所述处理模块901还用于:
在遇障碍物时,若在设定时间内障碍物未消失,则进入DWA动态窗口法局部路径规划后,循迹行驶;
在遇障碍物时,若在设定时间内障碍物消失,则进入DWA规划一条沿全局路径行驶的局部路径后,循迹行驶。
为了描述的方便,以上所述装置的各部分以功能分为各种模块或单元分别描述。当然,在实施本发明时可以把各模块或单元的功能在同一个或多个软件或硬件中实现。
本发明实施例还提供一种计算机可读非易失性存储介质,包括程序代码,当所述程序代码在计算设备上运行时,所述程序代码用于使所述计算设备执行上述本发明实施例机器人导航中位姿的确定方法的步骤。
如图10所示,本申请一种机器人导航中位姿纠正的设备,该网络设备包括处理器1000、存储器1001和通信接口1002。
处理器1000负责管理总线架构和通常的处理,存储器1001可以存储处理器1000在执行操作时所使用的数据。通信接口1002用于在处理器1000的控制下接收和发送数据与存储器1001进行数据通信。
所述处理器1000可以是中央处理器(central processing unit,CPU),网络处理器(network processor,NP)或者CPU和NP的组合。所述处理器1000还可以进一步包括硬件芯片。上述硬件芯片可以是专用集成电路(application-specific integrated circuit,ASIC),可编程逻辑器件(programmable logic device,PLD)或其组合。上述PLD可以是复杂可编程逻辑器件(complex programmable logic device,CPLD),现场可编程逻辑门阵列(field-programmable gate array,FPGA),通用阵列逻辑(generic array logic,GAL)或其任意组合。存储器801可以包括:U盘、移动硬盘、只读存储器(read-only memory,ROM)、随机存取存储器(random access memory,RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
所述处理器1000、所述存储器1001以及所述通信接口1002之间相互连接。可选的,所述处理器1000、所述存储器1001以及所述通信接口1002可以通过总线1003相互连接;所述总线1003可以是外设部件互连标准(peripheral component interconnect,PCI)总线或扩展工业标准结构(extended industry standard architecture,EISA)总线等。所述总线可以分为地址总线、数据总线、控制总线等。为便于表示,图10中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
具体地,所述处理器1000,用于读取存储器1001中的程序并执行:
在确定当前位姿需要纠正后,采用局部栅格化搜索的方式选取评分最高的位姿对应的位置和角度组合;
将选取的所述位置和角度组合确定为纠正后的位姿。
实施中,通过下列方式确定位姿的评分:
确定需要计算评分的位姿对应的虚拟点云以及真实点云;
将所述真实点云与所述虚拟点云通过PLICP匹配,计算配对点的数目占真实点云中点的数目的百分比,并将所述百分比确定为当前位姿的评分。
实施中,所述处理器1000具体用于:
所述采用局部栅格化搜索的方式选取评分最高且超过阈值的位姿对应的位置和角度组合;
其中,所述阈值为预设的配对点的数目占真实点云中点的数目的百分比。
如图11所示,本发明实施例提供一种机器人导航中位姿纠正的设备,包括选取模块1100和处理模块1101:
选取模块1100:用于在确定当前位姿需要纠正后,采用局部栅格化搜索的方式选取评分最高的位姿对应的位置和角度组合;
处理模块1101:用于将选取的所述位置和角度组合确定为纠正后的位姿。
为了描述的方便,以上所述装置的各部分以功能分为各种模块或单元分别描述。当然,在实施本发明时可以把各模块或单元的功能在同一个或多个软件或硬件中实现。
本发明实施例还提供一种计算机可读非易失性存储介质,包括程序代码,当所述程序代码在计算设备上运行时,所述程序代码用于使所述计算设备执行上述本发明实施例机器人导航中位姿的纠正方法的步骤。
综上所述,在本发明实施例提供的技术方案中,针对巡检任务重复性强、导航精度要求高的特点,以及轮式移动机器人在匀速直线行驶状态下导航精度更高的特性,提出将导航路径点序列根据路径点的属性、行进模式和方向进行拆分得到若干段道路段以保证每一道路段内机器人均是匀速直线行驶或原地旋转,从而保证导航精度;融合循迹导航和自由导航的混合导航方案,在通过遇障等待时间阈值切换循迹导航和自由导航两种模式,兼顾了导航精度和导航自由度;提供定位评分和位姿纠正方案,将真实点云与虚拟点云进行PLICP并计算匹配点对数目占点云数目的比例作为定位的评分。当评分不满足阈值时,通过栅格化搜索的方式找出当前位置附近评分最高的位姿作为正确的定位结果。
本领域内的技术人员应明白,本发明的实施例可提供为方法、***、或计算机程序产品。因此,本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然,本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样,倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内,则本发明也意图包含这些改动和变型在内。