CN110260867B - 一种机器人导航中位姿确定、纠正的方法、设备及装置 - Google Patents

一种机器人导航中位姿确定、纠正的方法、设备及装置 Download PDF

Info

Publication number
CN110260867B
CN110260867B CN201910690768.9A CN201910690768A CN110260867B CN 110260867 B CN110260867 B CN 110260867B CN 201910690768 A CN201910690768 A CN 201910690768A CN 110260867 B CN110260867 B CN 110260867B
Authority
CN
China
Prior art keywords
pose
robot
point cloud
determining
positioning
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.)
Active
Application number
CN201910690768.9A
Other languages
English (en)
Other versions
CN110260867A (zh
Inventor
林辉
卢维
殷俊
穆方波
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Huaray Technology Co Ltd
Original Assignee
Zhejiang Huaray Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang Huaray Technology Co Ltd filed Critical Zhejiang Huaray Technology Co Ltd
Priority to CN201910690768.9A priority Critical patent/CN110260867B/zh
Publication of CN110260867A publication Critical patent/CN110260867A/zh
Application granted granted Critical
Publication of CN110260867B publication Critical patent/CN110260867B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles

Abstract

本发明公开了一种机器人导航中位姿确定、纠正的方法、设备及装置，包括：根据机器人的第一定位，确定所述机器人的真实点云；根据机器人第一定位下的位姿，确定该定位下的虚拟点云；将虚拟点云与真实点云进行匹配，根据所述匹配结果确定机器人的第二位姿。在确定当前位姿需要纠正后，采用局部栅格化搜索的方式选取评分最高的位姿对应的位置和角度组合；将选取的所述位置和角度组合确定为纠正后的位姿。采用本发明，通过将虚拟点云与真实点云进行匹配的方式计算定位的可信度，从而能够及时判断机器人定位异常的情况。当机器人定位异常时，采用局部栅格化搜索的方式纠正位姿，保证了导航的可靠性。

Description

一种机器人导航中位姿确定、纠正的方法、设备及装置
技术领域
本发明涉及导航技术领域，特别涉及一种机器人导航中位姿确定、纠正的方法、设备及装置。
背景技术
目前随着机器智能化的不断发展，越来越多的领域应用智能机器人进行巡检，即通过路径规划指导机器人自主移动。其中，现有针对智能巡检机器人路径规划的方案主要为通过SLAM算法获取巡检机器人的坐标和朝向，以及综合利用分层地图及工作任务信息确定全局最优路径，采用DWA(Dynamic Window Approach，动态窗口法)进行局部路径规划。
上述巡检机器人路径规划的方法存在的主要问题是：在采用动态窗口法计算局部路径时，在机器人前方遇到障碍物或全局路径有转折点时会产生一条与全局路径有偏离的局部路径，意味着机器人需要频繁转弯调整方向。而根据轮式移动机器人的运动特性，直线匀速行驶的导航精度远高于曲线行驶，因此上述方法虽然能够动态躲避障碍物但也使得导航精度降低；与此同时，在通过SLAM算法进行定位获取巡检机器人的坐标和朝向时，SLAM算法并不是完全稳定可靠的，当定位异常时机器人导航将遇到不可控风险，然而上述方案未给出对SLAM定位结果出现异常的情况进行处理的方案。
发明内容
本发明提供了一种机器人导航中位姿确定、纠正的方法、设备及装置，用以提供一种可靠地导航方案。
第一方面，本发明实施例中提供了一种机器人导航中位姿的确定方法，包括：
根据机器人的第一定位，确定所述机器人的真实点云；
根据机器人第一定位下的位姿，确定该定位下的虚拟点云；
将虚拟点云与真实点云进行匹配，根据所述匹配结果确定机器人的第二位姿。
一种可实现的方式中，所述根据机器人的第一定位，确定所述机器人的真实点云，包括：根据所述机器人的第一定位，将通过机器人的2D激光雷达采集得到的点云确定为真实点云。
一种可实现的方式中，所述根据机器人第一定位下的位姿，确定该定位下的虚拟点云，包括：根据所述机器人的第一定位估算第一位姿；以所述机器人的第一位姿为基础，将从占据栅格地图中截取周围的地图环境轮廓所形成的点云确定为该定位下的虚拟点云。
一种可实现的方式中，所述将虚拟点云与真实点云进行匹配，根据所述匹配结果确定机器人的第二位姿，包括：通过将虚拟点云与真实点云进行匹配，得到旋转和平移量；根据所述旋转和平移量对所述第一位姿进行修正，并将所述修正后获得的位姿结果确定为所述第二位姿。
一种可实现的方式中，根据机器人的第一定位，确定所述机器人的真实点云之前，还包括：根据所述机器人的轮式里程计和/或激光雷达数据进行AMCL自适应蒙特卡洛定位算法定位；通过PLICP基于点到线距离的迭代最近邻点搜索算法对所述定位结果进行优化，并将优化后的定位结果确定为所述第一定位。
一种可实现的方式中，虚拟点云与真实点云是通过PLICP算法进行匹配的。
一种可实现的方式中，根据所述旋转和平移量对第一位姿进行修正后获得的位姿结果，是通过将所述旋转和平移量施加给AMCL的定位输出后得到的。
一种可实现的方式中，在确定机器人的定位时，机器人所处的位置位于全局路径规划模块规划的路径之上。
一种可实现的方式中，机器人所处的位置位于分段后的全局路径规划模块规划的路径之上，其中，全局路径规划模块规划的路径在遍历所有路径点后，按如下条件之一或者其组合分段：如果当前路径点是任务点，则将上一个拆分位置到当前路径点作为一段道路段；如果从当前路径点开始道路方向发生了改变，则将上一个拆分位置到当前路径点作为一段道路段；如果从当前路径点开始行进模式发生了改变，则将上一个拆分位置到当前路径点作为一段道路段；如果当前路径点机器人需要旋转，则将该路径点当成单独的道路段。
一种可实现的方式中，预设配对点的数目占真实点云中点的数目的百分比的阈值；将所述匹配结果与所述阈值进行比较，若所述匹配结果不小于所述阈值，则确定所述第二位姿正确，并通过所述第二位姿进行导航；若所述匹配结果小于所述阈值，则确定所述第二位姿异常，并对所述第二位姿进行纠正。
一种可实现的方式中，通过下列方式对所述第二位姿进行纠正：采用局部栅格化搜索的方式选取所述匹配结果最高且超过阈值的对应位置和角度组合做为纠正后的位姿。
一种可实现的方式中，在遇障碍物时，若在设定时间内障碍物未消失，则进入DWA动态窗口法局部路径规划后，循迹行驶；在遇障碍物时，若在设定时间内障碍物消失，则进入DWA规划一条沿全局路径行驶的局部路径后，循迹行驶。
第二方面，本发明实施例提供的一种机器人导航中位姿的纠正方法，包括：
在确定当前位姿需要纠正后，采用局部栅格化搜索的方式选取评分最高的位姿对应的位置和角度组合；
将选取的所述位置和角度组合确定为纠正后的位姿。
一种可实现的方式中，通过下列方式确定位姿的评分：确定需要计算评分的位姿对应的虚拟点云以及真实点云；将所述真实点云与所述虚拟点云通过PLICP匹配，计算配对点的数目占真实点云中点的数目的百分比，并将所述百分比确定为当前位姿的评分。
一种可实现的方式中，所述采用局部栅格化搜索的方式选取评分最高的位姿对应的位置和角度组合，包括：所述采用局部栅格化搜索的方式选取评分最高且超过阈值的位姿对应的位置和角度组合；其中，所述阈值为预设的配对点的数目占真实点云中点的数目的百分比。
第三方面，本发明实施例提供一种计算机设备，包括：处理器以及存储器：
所述处理器，用于根据机器人的第一定位，确定所述机器人的真实点云；根据机器人第一定位下的位姿，确定该定位下的虚拟点云；将虚拟点云与真实点云进行匹配，根据所述匹配结果确定机器人的第二位姿。
第四方面，本发明实施例提供一种计算机设备，包括：处理器以及存储器：
所述处理器，用于在确定当前位姿需要纠正后，采用局部栅格化搜索的方式选取评分最高的位姿对应的位置和角度组合；将选取的所述位置和角度组合确定为纠正后的位姿。
第五方面，本发明实施例还提供了一种机器人导航中位姿的确定装置，该设备包括：
至少一个处理单元以及至少一个存储单元，其中，所述存储单元存储有程序代码，当所述程序代码被所述处理单元执行时，使得所述处理单元执行上述第一方面中任一方面的各实施例的功能。
第六方面，本发明实施例还提供了一种机器人导航中位姿的确定装置，该设备包括：
至少一个处理单元以及至少一个存储单元，其中，所述存储单元存储有程序代码，当所述程序代码被所述处理单元执行时，使得所述处理单元执行上述第二方面中任一方面的各实施例的功能。
第七方面，本申请还提供一种计算机存储介质，其上存储有计算机程序，该程序被处理器执行时实现第一方面中任一方面所述方法的步骤。
第八方面，本申请还提供一种计算机存储介质，其上存储有计算机程序，该程序被处理器执行时实现第二方面中任一方面所述方法的步骤。
本发明有益效果如下：
在本发明实施例提供的技术方案中，在进行定位后，在进行位姿纠正的过程中，将真实点云与虚拟点云进行PLICP并计算匹配点对数目占点云数目的比例作为定位的评分，当评分不满足阈值时，通过栅格化搜索的方式找出当前位置附近评分最高的位姿作为正确的定位结果。通过将虚拟点云与真实点云进行匹配的方式计算定位的可信度，从而能够及时判断机器人定位异常的情况。当机器人定位异常时，采用局部栅格化搜索的方式纠正位姿，保证了导航的可靠性。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为本发明实施例中机器人导航中位姿的确定方法实施流程示意图；
图2为本发明实施例中导航方案实施流程示意图；
图3为本发明实施例中路径点序列拆分示意图；
图4为本发明实施例中路径插值示意图；
图5为本发明实施例中计算定位评分与位姿纠正前示意图；
图6为本发明实施例中计算定位评分与位姿纠正后示意图；
图7为本发明实施例中避障逻辑示意图；
图8为本发明实施例第一种机器人导航中位姿确定的设备结构示意图；
图9为本发明实施例第二种机器人导航中位姿确定的设备结构示意图；
图10为本发明实施例第一种机器人导航中位姿纠正的设备结构示意图；
图11为本发明实施例第二种机器人导航中位姿纠正的设备结构示意图。
具体实施方式
下面结合附图对本发明的具体实施方式进行说明。
图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)遍历Ri中所有相邻点对，如Pm和Pm+1，求其欧氏距离
Figure BDA0002147805970000111
Figure BDA0002147805970000112
并确定位姿点数目
Figure BDA0002147805970000113
然后将位姿点集合Path与当前插值得到的位姿点集合
Figure BDA0002147805970000114
Figure BDA0002147805970000115
合并，其中所有位姿点的角度θ均等于Pm和Pm+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并计算匹配点对数目占点云数目的比例作为定位的评分。当评分不满足阈值时，通过栅格化搜索的方式找出当前位置附近评分最高的位姿作为正确的定位结果。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (29)

1.一种机器人导航中位姿的确定方法，其特征在于，包括：
根据机器人的第一定位，确定所述机器人的真实点云；在确定机器人的所述第一定位时，所述机器人所处的位置位于分段后的全局路径规划模块规划的路径之上；其中，所述全局路径规划模块规划的路径在遍历所有路径点后，按如下条件之一或者其组合分段：
如果当前路径点是任务点，则将上一个拆分位置到当前路径点作为一段道路段；
如果从当前路径点开始道路方向发生了改变，则将上一个拆分位置到当前路径点作为一段道路段；
如果从当前路径点开始行进模式发生了改变，则将上一个拆分位置到当前路径点作为一段道路段；
如果当前路径点机器人需要旋转，则将该路径点当成单独的道路段；
根据机器人第一定位下的位姿，确定该定位下的虚拟点云；
将虚拟点云与真实点云进行匹配，根据匹配结果确定机器人的第二位姿。
2.如权利要求1所述的方法，其特征在于，所述根据机器人的第一定位，确定所述机器人的真实点云，包括：
根据所述机器人的第一定位，将通过机器人的2D激光雷达采集得到的点云确定为真实点云。
3.如权利要求2所述的方法，其特征在于，所述根据机器人第一定位下的位姿，确定该定位下的虚拟点云，包括：
根据所述机器人的第一定位估算第一位姿；
以所述机器人的第一位姿为基础，将从占据栅格地图中截取周围的地图环境轮廓所形成的点云确定为该定位下的虚拟点云。
4.如权利要求1所述的方法，其特征在于，所述将虚拟点云与真实点云进行匹配，根据匹配结果确定机器人的第二位姿，包括：
通过将虚拟点云与真实点云进行匹配，得到旋转和平移量；
根据所述旋转和平移量对所述第一定位下的位姿进行修正，并将所述修正后获得的位姿结果确定为所述第二位姿。
5.如权利要求1所述的方法，其特征在于，根据机器人的第一定位，确定所述机器人的真实点云之前，还包括：
根据所述机器人的轮式里程计和/或激光雷达数据进行AMCL自适应蒙特卡洛定位算法定位，得到定位结果；
通过PLICP基于点到线距离的迭代最近邻点搜索算法对所述定位结果进行优化，并将优化后的定位结果确定为所述第一定位。
6.如权利要求1所述的方法，其特征在于，虚拟点云与真实点云是通过PLICP算法进行匹配的。
7.如权利要求4所述的方法，其特征在于，根据所述旋转和平移量对第一位姿进行修正后获得的位姿结果，是通过将所述旋转和平移量施加给AMCL的定位输出后得到的。
8.如权利要求1～7任一所述的方法，其特征在于，所述方法还包括：
预设配对点的数目占真实点云中点的数目的百分比的阈值；
将匹配结果与所述阈值进行比较，若所述匹配结果不小于所述阈值，则确定所述第二位姿正确，并通过所述第二位姿进行导航；若所述匹配结果小于所述阈值，则确定所述第二位姿异常，并对所述第二位姿进行纠正。
9.如权利要求8所述的方法，其特征在于，通过下列方式对所述第二位姿进行纠正：
采用局部栅格化搜索的方式选取匹配结果最高且超过阈值的对应位置和角度组合做为纠正后的位姿。
10.如权利要求1所述的方法，其特征在于，进一步包括：
在遇障碍物时，若在设定时间内障碍物未消失，则进入DWA动态窗口法局部路径规划后，循迹行驶；
在遇障碍物时，若在设定时间内障碍物消失，则进入DWA规划一条沿全局路径行驶的局部路径后，循迹行驶。
11.一种机器人导航中位姿的纠正方法，其特征在于，包括：
在确定当前位姿需要纠正后，采用局部栅格化搜索的方式选取评分最高的位姿对应的位置和角度组合；
将选取的所述位置和角度组合确定为纠正后的位姿。
12.如权利要求11所述的方法，其特征在于，通过下列方式确定位姿的评分：
确定需要计算评分的位姿对应的虚拟点云以及真实点云；
将所述真实点云与所述虚拟点云通过PLICP匹配，计算配对点的数目占真实点云中点的数目的百分比，并将所述百分比确定为当前位姿的评分。
13.如权利要求11或12所述的方法，其特征在于，所述采用局部栅格化搜索的方式选取评分最高的位姿对应的位置和角度组合，包括：
所述采用局部栅格化搜索的方式选取评分最高且超过阈值的位姿对应的位置和角度组合；
其中，所述阈值为预设的配对点的数目占真实点云中点的数目的百分比。
14.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如下方法：
根据机器人的第一定位，确定所述机器人的真实点云；在确定机器人的所述第一定位时，所述机器人所处的位置位于分段后的全局路径规划模块规划的路径之上；其中，所述全局路径规划模块规划的路径在遍历所有路径点后，按如下条件之一或者其组合分段：
如果当前路径点是任务点，则将上一个拆分位置到当前路径点作为一段道路段；
如果从当前路径点开始道路方向发生了改变，则将上一个拆分位置到当前路径点作为一段道路段；
如果从当前路径点开始行进模式发生了改变，则将上一个拆分位置到当前路径点作为一段道路段；
如果当前路径点机器人需要旋转，则将该路径点当成单独的道路段；
根据机器人第一定位下的位姿，确定该定位下的虚拟点云；
将虚拟点云与真实点云进行匹配，根据匹配结果确定机器人的第二位姿。
15.如权利要求14所述的设备，其特征在于，所述处理器具体用于：
根据所述机器人的第一定位，将通过机器人的2D激光雷达采集得到的点云确定为真实点云。
16.如权利要求15所述的设备，其特征在于，所述处理器具体用于：
根据所述机器人的第一定位估算第一位姿；
以所述机器人的第一位姿为基础，将从占据栅格地图中截取周围的地图环境轮廓所形成的点云确定为该定位下的虚拟点云。
17.如权利要求14所述的设备，其特征在于，所述处理器具体用于：
通过将虚拟点云与真实点云进行匹配，得到旋转和平移量；
根据所述旋转和平移量对所述第一定位下的位姿进行修正，并将所述修正后获得的位姿结果确定为所述第二位姿。
18.如权利要求14所述的设备，其特征在于，所述处理器还用于：
根据所述机器人的轮式里程计和/或激光雷达数据进行AMCL自适应蒙特卡洛定位算法定位，得到定位结果；
通过PLICP基于点到线距离的迭代最近邻点搜索算法对所述定位结果进行优化，并将优化后的定位结果确定为所述第一定位。
19.如权利要求14所述的设备，其特征在于，虚拟点云与真实点云是通过PLICP算法进行匹配的。
20.如权利要求17所述的设备，其特征在于，根据所述旋转和平移量对第一位姿进行修正后获得的位姿结果，是通过将所述旋转和平移量施加给AMCL的定位输出后得到的。
21.如权利要求14～20任一所述的设备，其特征在于，所述处理器还用于：
预设配对点的数目占真实点云中点的数目的百分比的阈值；
将匹配结果与所述阈值进行比较，若所述匹配结果不小于所述阈值，则确定所述第二位姿正确，并通过所述第二位姿进行导航；若所述匹配结果小于所述阈值，则确定所述第二位姿异常，并对所述第二位姿进行纠正。
22.如权利要求21所述的设备，其特征在于，通过下列方式对所述第二位姿进行纠正：
采用局部栅格化搜索的方式选取匹配结果最高且超过阈值的对应位置和角度组合做为纠正后的位姿。
23.如权利要求14所述的设备，其特征在于，所述处理器还用于：
在遇障碍物时，若在设定时间内障碍物未消失，则进入DWA动态窗口法局部路径规划后，循迹行驶；
在遇障碍物时，若在设定时间内障碍物消失，则进入DWA规划一条沿全局路径行驶的局部路径后，循迹行驶。
24.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如下方法：
在确定当前位姿需要纠正后，采用局部栅格化搜索的方式选取评分最高的位姿对应的位置和角度组合；
将选取的所述位置和角度组合确定为纠正后的位姿。
25.如权利要求24所述的设备，其特征在于，通过下列方式确定位姿的评分：
确定需要计算评分的位姿对应的虚拟点云以及真实点云；
将所述真实点云与所述虚拟点云通过PLICP匹配，计算配对点的数目占真实点云中点的数目的百分比，并将所述百分比确定为当前位姿的评分。
26.如权利要求24或25所述的设备，其特征在于，所述处理器具体用于：
所述采用局部栅格化搜索的方式选取评分最高且超过阈值的位姿对应的位置和角度组合；
其中，所述阈值为预设的配对点的数目占真实点云中点的数目的百分比。
27.一种机器人导航中位姿的确定装置，其特征在于，包括：
确定模块，用于根据机器人的第一定位，确定所述机器人的真实点云；在确定机器人的所述第一定位时，所述机器人所处的位置位于分段后的全局路径规划模块规划的路径之上；其中，所述全局路径规划模块规划的路径在遍历所有路径点后，按如下条件之一或者其组合分段：
如果当前路径点是任务点，则将上一个拆分位置到当前路径点作为一段道路段；
如果从当前路径点开始道路方向发生了改变，则将上一个拆分位置到当前路径点作为一段道路段；
如果从当前路径点开始行进模式发生了改变，则将上一个拆分位置到当前路径点作为一段道路段；
如果当前路径点机器人需要旋转，则将该路径点当成单独的道路段；
根据机器人第一定位下的位姿，确定该定位下的虚拟点云；
处理模块，用于将虚拟点云与真实点云进行匹配，根据匹配结果确定机器人的第二位姿。
28.一种机器人导航中位姿的纠正装置，其特征在于，包括：
选取模块：用于在确定当前位姿需要纠正后，采用局部栅格化搜索的方式选取评分最高的位姿对应的位置和角度组合；
处理模块：用于将选取的所述位置和角度组合确定为纠正后的位姿。
29.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有执行权利要求1至10或权利要求11-13任一所述方法的计算机程序。
CN201910690768.9A 2019-07-29 2019-07-29 一种机器人导航中位姿确定、纠正的方法、设备及装置 Active CN110260867B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910690768.9A CN110260867B (zh) 2019-07-29 2019-07-29 一种机器人导航中位姿确定、纠正的方法、设备及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910690768.9A CN110260867B (zh) 2019-07-29 2019-07-29 一种机器人导航中位姿确定、纠正的方法、设备及装置

Publications (2)

Publication Number Publication Date
CN110260867A CN110260867A (zh) 2019-09-20
CN110260867B true CN110260867B (zh) 2021-06-15

Family

ID=67912228

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910690768.9A Active CN110260867B (zh) 2019-07-29 2019-07-29 一种机器人导航中位姿确定、纠正的方法、设备及装置

Country Status (1)

Country Link
CN (1) CN110260867B (zh)

Families Citing this family (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021056283A1 (en) * 2019-09-25 2021-04-01 Beijing Didi Infinity Technology And Development Co., Ltd. Systems and methods for adjusting a vehicle pose
CN111060131B (zh) * 2019-11-27 2021-08-10 四川阿泰因机器人智能装备有限公司 一种基于激光雷达的机器人精确姿态矫正方法及装置
CN111136655B (zh) * 2019-12-10 2021-06-29 深圳市优必选科技股份有限公司 一种获取巡检点的方法及装置
CN110954100A (zh) * 2019-12-30 2020-04-03 广东省智能制造研究所 一种基于激光与惯导融合的足式机器人本体状态估计方法
EP3882649B1 (en) * 2020-03-20 2023-10-25 ABB Schweiz AG Position estimation for vehicles based on virtual sensor response
CN116106927A (zh) * 2020-03-27 2023-05-12 深圳市镭神智能系统有限公司 一种基于激光雷达的二维栅格地图构建方法、介质和系统
CN113748693B (zh) * 2020-03-27 2023-09-15 深圳市速腾聚创科技有限公司 路基传感器的位姿校正方法、装置和路基传感器
CN111627114A (zh) * 2020-04-14 2020-09-04 北京迈格威科技有限公司 室内视觉导航方法、装置、系统及电子设备
CN111678521B (zh) * 2020-06-18 2021-12-28 上海大学 一种移动机器人定位精度的评估方法和系统
CN114102577B (zh) * 2020-08-31 2023-05-30 北京极智嘉科技股份有限公司 一种机器人及应用于机器人的定位方法
CN111966109B (zh) * 2020-09-07 2021-08-17 中国南方电网有限责任公司超高压输电公司天生桥局 基于柔性直流换流站阀厅的巡检机器人定位方法及装置
CN113156971A (zh) * 2021-05-08 2021-07-23 珠海市一微半导体有限公司 一种纠正行走路径的方法、芯片和机器人
CN113219488A (zh) * 2021-05-08 2021-08-06 珠海市一微半导体有限公司 一种机器人的建图的方法
CN113108774B (zh) * 2021-05-21 2023-11-10 深圳市人工智能与机器人研究院 水下机器人及水下机器人导航定位方法
CN113390417A (zh) * 2021-06-25 2021-09-14 深圳市优必选科技股份有限公司 机器人及其导航方法、装置和计算机可读存储介质
CN113485366B (zh) * 2021-08-05 2022-03-04 泰瑞数创科技(北京)有限公司 用于机器人的导航路径生成方法及装置
CN113359151B (zh) * 2021-08-09 2021-11-23 浙江华睿科技股份有限公司 机器人任务点定位方法及装置
CN113854892B (zh) * 2021-10-21 2022-08-02 唐山学院 一种自动规划路径的清洁装置
CN114415664A (zh) * 2021-12-16 2022-04-29 北京航天测控技术有限公司 一种机器人导航方法及装置

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104536445B (zh) * 2014-12-19 2018-07-03 深圳先进技术研究院 移动导航方法和系统
CN104808671B (zh) * 2015-05-19 2017-03-15 东南大学 一种家居环境下的机器人路径规划方法
WO2019018315A1 (en) * 2017-07-17 2019-01-24 Kaarta, Inc. ALIGNMENT OF MEASURED SIGNAL DATA WITH SLAM LOCATION DATA AND ASSOCIATED USES
CN107167148A (zh) * 2017-05-24 2017-09-15 安科机器人有限公司 同步定位与地图构建方法和设备
US10572854B2 (en) * 2017-11-09 2020-02-25 Locus Robotics Corporation Order grouping in warehouse order fulfillment operations
CN107917712B (zh) * 2017-11-16 2020-07-28 苏州艾吉威机器人有限公司 一种同步定位与地图构建方法及设备
CN109959377A (zh) * 2017-12-25 2019-07-02 北京东方兴华科技发展有限责任公司 一种机器人导航定位系统及方法
CN108645413A (zh) * 2018-06-06 2018-10-12 江苏海事职业技术学院 一种移动机器人的同时定位与地图创建的动态纠正方法
CN109579849B (zh) * 2019-01-14 2020-09-29 浙江大华技术股份有限公司 机器人定位方法、装置和机器人及计算机存储介质
CN109916393B (zh) * 2019-03-29 2023-03-31 电子科技大学 一种基于机器人位姿的多重栅格值导航方法及其应用

Also Published As

Publication number Publication date
CN110260867A (zh) 2019-09-20

Similar Documents

Publication Publication Date Title
CN110260867B (zh) 一种机器人导航中位姿确定、纠正的方法、设备及装置
CN110927740B (zh) 一种移动机器人定位方法
EP3660618B1 (en) Map building and positioning of robot
CN107436148B (zh) 一种基于多地图的机器人导航方法及装置
WO2020237890A1 (zh) 一种速度规划方法、装置、电子设备及存储介质
Goto et al. Mobile robot navigation: The CMU system
CN110162029B (zh) 一种基于规划路径的运动控制方法及装置、机器人
US10611028B1 (en) Map building and positioning of robot
WO2022056770A1 (zh) 一种路径规划方法和路径规划装置
CN106933223B (zh) 一种机器人自主导航方法及系统
Chen et al. An enhanced dynamic Delaunay triangulation-based path planning algorithm for autonomous mobile robot navigation
US20210164800A1 (en) Method for Determining the Position of a Vehicle
CN111857114A (zh) 一种机器人编队移动方法、系统、设备和存储介质
Chen et al. Path planning for autonomous vehicle based on a two-layered planning model in complex environment
CN111045433B (zh) 一种机器人的避障方法、机器人及计算机可读存储介质
JP7032062B2 (ja) 点群データ処理装置、移動ロボット、移動ロボットシステム、および点群データ処理方法
Signifredi et al. A general purpose approach for global and local path planning combination
Spanogiannopoulos et al. Sampling-based non-holonomic path generation for self-driving cars
CN112925302A (zh) 机器人位姿控制方法和装置
JP7160110B2 (ja) 経路計画装置、経路計画方法、及びプログラム
JP7235060B2 (ja) 経路計画装置、経路計画方法、及びプログラム
CN116215517A (zh) 碰撞检测方法、装置、设备、存储介质和自动驾驶车辆
CN113296099B (zh) 一种根据障碍物自动生成规避路径的方法及终端
Piccinelli et al. Hybrid motion planner integrating global voronoi diagrams and local velocity obstacle method
CN115583254A (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
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20201225

Address after: C10, No. 1199 Bin'an Road, Binjiang District, Hangzhou City, Zhejiang Province

Applicant after: ZHEJIANG HUARAY TECHNOLOGY Co.,Ltd.

Address before: Hangzhou City, Zhejiang province Binjiang District 310053 shore road 1187

Applicant before: ZHEJIANG DAHUA TECHNOLOGY Co.,Ltd.

GR01 Patent grant
GR01 Patent grant
CP01 Change in the name or title of a patent holder
CP01 Change in the name or title of a patent holder

Address after: C10, No. 1199 Bin'an Road, Binjiang District, Hangzhou City, Zhejiang Province

Patentee after: Zhejiang Huarui Technology Co.,Ltd.

Address before: C10, No. 1199 Bin'an Road, Binjiang District, Hangzhou City, Zhejiang Province

Patentee before: ZHEJIANG HUARAY TECHNOLOGY Co.,Ltd.