CN114777770A - 机器人定位方法、装置、控制终端以及可读存储介质 - Google Patents
机器人定位方法、装置、控制终端以及可读存储介质 Download PDFInfo
- Publication number
- CN114777770A CN114777770A CN202210320073.3A CN202210320073A CN114777770A CN 114777770 A CN114777770 A CN 114777770A CN 202210320073 A CN202210320073 A CN 202210320073A CN 114777770 A CN114777770 A CN 114777770A
- Authority
- CN
- China
- Prior art keywords
- pose
- robot
- matching
- positioning
- final
- 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.)
- Pending
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种机器人定位方法、装置、控制终端以及可读存储介质,其中,所述方法包括:根据惯性导航系统获取所述机器人的预测位姿;基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿;若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿;基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿;若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿。本发明提高了定位算法的计算效率,提高了机器人定位的精确度。
Description
技术领域
本发明涉及激光定位技术领域,尤其涉及一种机器人定位方法、装置、控制终端以及可读存储介质。
背景技术
目前,室内商用机器人定位方案主要是依靠激光、IMU(Inertial MeasurementUnit,惯性传感器)和odom(Odometry,轮式里程计)等传感器,在已知先验地图的情况下,通过IMU和odom获取预测值,并通过激光数据获取观测值,再根据预测值和观测值得出一个较为准确的定位结果。这种方案在传感器数据无异常、环境信息不发生明显变化的情况下可以正常使用,但是遇到环境变化较大的场景比如电梯,货物堆积的仓库,有大量可活动桌椅的餐厅等则无法准确定位。
因此,针对环境信息变化较大的场景,常见的方案为采用激光里程计的定位方案来解决机器人在动态场景下的定位问题,通过在先验全局地图中定位的同时,构建一张当前环境的局部地图(由若干个关键帧组成),即动态场景改变后的环境,在通过当前激光帧和局部地图匹配后得到定位结果。
然而,传统的采用激光里程计的定位方案由于其局部地图始终维持一个较小的窗口,缺乏有效的回环信息,导致前端的匹配误差会随着时间的推移不断累积,无法长时间为后端提供一个稳定而准确的定位输出。
上述内容仅用于辅助理解本发明的技术方案,并不代表承认上述内容是现有技术。
发明内容
本发明的主要目的在于提供一种机器人定位方法、装置、控制终端以及可读存储介质,旨在解决激光定位误差的问题。
为实现上述目的,本发明提供的一种机器人定位方法,所述机器人定位方法包括:
根据惯性导航系统获取所述机器人的预测位姿;
基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿;
若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿;
基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿;
若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿。
可选地,所述根据惯性导航系统获取所述机器人的预测位姿的步骤包括:
获取所述惯性导航系统中所述机器人对应的里程信息、加速度信息和角度变化信息;
根据所述里程信息、所述加速度信息和所述角度变化信息,确定所述机器人的所述预测位姿。
可选地,所述基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿的步骤包括:
确定所述当前时刻激光观测数据与所述预测位姿在所述局部地图中的局部匹配结果;
根据所述局部匹配结果确定所述机器人在所述局部地图的所述第一匹配分值;
根据所述第一匹配分值确定所述预测位姿在局部地图中的所述第一位姿。
可选地,所述若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿的步骤包括:
检测所述第一匹配分值;
当所述第一匹配分值大于所述第一阈值时,根据所述第一位姿和所述预测位姿通过预设算法确定所述机器人在所述局部地图中的第二位姿;
当所述第一匹配分值小于或等于所述第一阈值时,将所述预测位姿作为所述第二位姿。
可选地,所述基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿的步骤包括:
根据激光雷达的扫描数据建立全局地图,确定所述当前时刻激光观测数据与所述预测位姿在所述全局地图中的全局匹配结果;
根据所述全局匹配结果确定所述机器人在全局地图的第二匹配分值;
根据所述第二匹配分值确定所述机器人在所述全局地图的所述第三位姿。
可选地,所述若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿的步骤包括:
检测所述第二匹配分值;
当所述第二匹配分值大于所述第二阈值时,确定所述第三位姿对应的预设融合权重值;
根据所述融合权重值将所述第二位姿和所述第三位姿融合,得到所述最终定位结果的最终位姿;
当所述第二匹配分值小于或等于所述第二阈值时,将所述第二位姿作为所述最终定位结果的最终位姿。
可选地,所述若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿的步骤之后,还包括:
将所述最终定位结果的所述最终位姿输出至决策层,以使所述决策层根据所述最终位姿规划所述机器人行驶路线。
此外,为实现上述目的,本发明还提供一种机器人定位装置,所述机器人定位装置包括:
位姿预测模块,用于根据惯性导航系统获取所述机器人的预测位姿;
局部定位模块,用于基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿;
局部定位融合模块,用于若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿;
全局定位模块,用于基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿;
全局定位融合模块,用于若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿。
此外,为实现上述目的,本发明还提供一种控制终端,所述控制终端包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的机器人定位程序,所述机器人定位程序被所述处理器执行时实现如上任一项所述的机器人定位方法的步骤。
此外,为实现上述目的,本发明还提供一种计算机可读存储介质,所述计算机可读存储介质上存储有机器人定位程序,所述机器人定位程序被处理器执行时实现如上任一项所述的机器人定位方法的步骤。
本发明实施例提供一种机器人定位方法、装置以及计算机可读存储介质,根据惯性导航系统获取所述机器人的预测位姿;基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿;若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿;基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿;若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿。通过对激光里程计的定位结果进行融合,无需引入额外的传感器,在不明显增加算力的基础上消除了机器人在动态场景下的定位误差。本发明提高了定位算法的计算效率,提高了机器人定位的精确度。
附图说明
图1为本发明实施例涉及的控制终端的硬件架构示意图;
图2为本发明机器人定位方法的第一实施例的流程示意图;
图3为本发明机器人定位方法的第二实施例中步骤S10细化流程示意图;
图4为本发明机器人定位方法中第三实施例的流程示意图;
图5为本发明机器人定位方法中第四实施例中步骤S40的细化流程示意图;
图6为本发明机器人定位方法中第五实施例中步骤S50细化流程示意图;
图7为本发明机器人定位方法中第六实施例的流程示意图;
图8为本发明机器人定位装置的架构示意图。
本发明目的的实现、功能特点及优点将结合实施例,参照附图做进一步说明。
具体实施方式
应当理解,本发明的附图中显示了本发明的示例性实施例,可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反,提供这些实施例是为了能够更透彻地理解本发明,并且能够将本发明的范围完整的传达给本领域的技术人员。
需要强调的是,在本发明中引入的激光里程计融合定位方案,能够有效解决环境变化较大的特殊场景定位。由于本发明本质上是一种融合方案,由此本发明不局限于动态变化场景,类似轮式里程计故障等单一传感器失效的情况也可正常输出定位结果。
作为一种实现方案,基于控制终端的硬件架构可以如图2所示。
本发明实施例方案涉及的是控制终端,所述控制终端包括:处理器101,例如CPU,存储器102,通信总线103。其中,通信总线103用于实现这些组件之间的连接通信。
存储器102可以是高速RAM存储器,也可以是稳定的存储器(non-volatilememory),例如磁盘存储器。如图1所示,作为一种计算机可读存储介质的存储器102中可以包括机器人定位程序;而处理器101可以用于调用存储器102中存储的机器人定位程序,并执行以下操作:
根据惯性导航系统获取所述机器人的预测位姿;
基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿;
若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿;
基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿;
若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿。
在一实施例中,处理器101可以用于调用存储器102中存储的机器人定位程序,并执行以下操作:
获取所述惯性导航系统中所述机器人对应的里程信息、加速度信息和角度变化信息;
根据所述里程信息、所述加速度信息和所述角度变化信息,确定所述机器人的所述预测位姿。
在一实施例中,处理器101可以用于调用存储器102中存储的机器人定位程序,并执行以下操作:
确定所述当前时刻激光观测数据与所述预测位姿在所述局部地图中的局部匹配结果;
根据所述局部匹配结果确定所述机器人在所述局部地图的所述第一匹配分值;
根据所述第一匹配分值确定所述预测位姿在局部地图中的所述第一位姿。
在一实施例中,处理器101可以用于调用存储器102中存储的机器人定位程序,并执行以下操作:
检测所述第一匹配分值;
当所述第一匹配分值大于所述第一阈值时,根据所述第一位姿和所述预测位姿通过预设算法确定所述机器人在所述局部地图中的第二位姿;
当所述第一匹配分值小于或等于所述第一阈值时,将所述预测位姿作为所述第二位姿。
在一实施例中,处理器101可以用于调用存储器102中存储的机器人定位程序,并执行以下操作:
根据激光雷达的扫描数据建立全局地图,确定所述当前时刻激光观测数据与所述预测位姿在所述全局地图中的全局匹配结果;
根据所述全局匹配结果确定所述机器人在全局地图的第二匹配分值;
根据所述第二匹配分值确定所述机器人在所述全局地图的所述第三位姿。
在一实施例中,处理器101可以用于调用存储器102中存储的机器人定位程序,并执行以下操作:
检测所述第二匹配分值;
当所述第二匹配分值大于所述第二阈值时,确定所述第三位姿对应的预设融合权重值;
根据所述融合权重值将所述第二位姿和所述第三位姿融合,得到所述最终定位结果的最终位姿;
当所述第二匹配分值小于或等于所述第二阈值时,将所述第二位姿作为所述最终定位结果的最终位姿。
在一实施例中,处理器101可以用于调用存储器102中存储的机器人定位程序,并执行以下操作:
将所述最终定位结果的所述最终位姿输出至决策层,以使所述决策层根据所述最终位姿规划所述机器人行驶路线。
基于上述基于激光定位技术的基于控制终端的硬件架构,提出本发明机器人定位方法的实施例。
本发明提高了定位算法的计算效率,提高了机器人定位的精确度。
参照图2,在第一实施例中,所述机器人定位方法包括以下步骤:
步骤S10,根据惯性导航系统获取所述机器人的预测位姿;
在本实施例中,在机器人开启实时定位的初始时刻,据惯性导航系统获取所述机器人的预测位姿,并根据该预测位姿确定机器人的激光在局部子图中的定位结果,需要强调的是,由于激光匹配的解算是一个求解非线性方程的过程,初值是特别重要的,好的数值会使方程快速准确的迭代到正确位置,这也是一种融合的手段。
步骤S20,基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿;
在本实施例中,通过将当前时刻激光观测数据与预测位姿的局部地图进行匹配,迅速得到一个融合了激光观测数据和机器人在局部地图中定位结果的第一位姿。
步骤S30,若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿;
在本实施例中,在获得第一匹配分值之后,当判断所述第一匹配分值大于第一阈值时,将所述第一位姿和所述预测位姿在所述局部地图中融合,得到所述第二位姿,其中,所述第二位姿是消除了所述第一位姿中由于前端缺乏有效的回环信息而产生匹配误差的位姿。
步骤S40,基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿;
在本实施例中,在获得第二位姿之后,基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿,其中,所述第三位姿是将第二位姿放在全局地图中进行全局定位所得的,需要强调的是,本实施例中的全局定位是在机器人的局部定位基础上通过加入一个融合手段来将局部定位放到全局地图中进行。
步骤S50,若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿。
在本实施例中,当所述第二匹配分值大于预设第二阈值时,判断为所述第二位姿和所述第三位姿满足融合条件,则根据所述第二位姿和所述第三位姿确定包含最终定位结果的最终位姿
在本实施例提供的技术方案中,通过将机器人在局部地图和全局地图中的定位结果融合得到最终定位结果,在不明显增加算力的基础上消除了机器人在动态场景下的定位误差,解决了激光定位误差的问题,提高了机器人定位的精确度。
参照图3,在第二实施例中,基于第一实施例,所述步骤S10,根据惯性导航系统获取所述机器人的预测位姿,包括:
步骤S11,获取所述惯性导航系统中所述机器人对应的里程信息、加速度信息和角度变化信息;
步骤S12,根据所述里程信息、所述加速度信息和所述角度变化信息,确定所述机器人的所述预测位姿。
可选地,上位机获取所述惯性导航系统中所述机器人对应的里程信息、加速度信息和角度变化信息,根据平移量和旋转角度,并根据所述里程信息、所述加速度信息和所述角度变化信息,确定所述机器人的所述预测位姿。里程信息从惯性导航系统中的里程计获取,加速度信息从惯性导航系统中的加速度计获取,角度变化信息从惯性导航系统中的陀螺仪获取。具体地来说,是根据里程信息、加速度信息和角度变化信息,通过自适应蒙特卡洛白定位算法获取用干预测移动机器人位姿的粒子分布信息。由于在检测到移动机器人的运动状态异常时,移动机器人的位姿可能就不准确了,需要对移动机器人的当前位姿态进行局部自定位,重新确定准确的位姿。这里移动机器人的位姿包括移动机器人的位置和姿态,用于指明移动机器人所处的位置和朝向。
在本实施例提供的技术方案中,通过确定所述机器人的所述预测位姿实现了将扫描到的数据与地图中的坐标匹配,提高了机器人定位的精确度。
参照图4,在第三实施例中,基于任一实施例,所述步骤S20,基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿的包括:
步骤S21,确定所述当前时刻激光观测数据与所述预测位姿在所述局部地图中的局部匹配结果;
步骤S22,根据所述局部匹配结果确定所述机器人在所述局部地图的所述第一匹配分值;
步骤S23,根据所述第一匹配分值确定所述预测位姿在局部地图中的所述第一位姿;
所述步骤S30,若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿,包括:
步骤S31,检测所述第一匹配分值;
步骤S32,当所述第一匹配分值大于所述第一阈值时,根据所述第一位姿和所述预测位姿通过预设算法确定所述机器人在所述局部地图中的第二位姿;
步骤S33,当所述第一匹配分值小于或等于所述第一阈值时,将所述预测位姿作为所述第二位姿;
由于通过粒子聚类得到的移动机器人预测位姿是比较粗糙的位姿,本申请的一些实施例中,再将当前时刻的激光观测数据与预测位姿对应的地图通过迭代最近点算法进行点云匹配,得到观测匹配度和移动机器人的位姿,该匹配是比较精细的匹配,可得到更加精确的位姿。
可选地,在本实施例中,将当前时刻激光观测数据与所述预测位姿在所述局部地图中进行匹配,所得到的局部匹配结果在局部地图中进行一个误差判断,即对所述局部匹配结果的进行一个打分,当所述第一匹配分值满足预设分数阈值时,则判断所述局部匹配结果是准确的,将其作为第一位姿。需要强调的是,本实施例中的预设阈值为一个自适应变量,而非固定常量,该预设阈值能够根据局部定位结果的匹配分值进行自适应的变换。示例性地,假定预设阈值为0.4,获取到当前机器人的局部定位结果的匹配分值为0.39,虽然此时匹配分值小于或等于预设阈值,当由于其较为接近,机器人的人工智能模块能够根据当前场景自适应的将阈值改变为一个小于或等于0.39的分值。
进一步的,当所述第一匹配分值大于所述第一阈值时,则将第一位姿和预测位姿融合,得到所述机器人在所述局部地图中的第二位姿;当所述第一匹配分值小于或等于所述第一阈值时,则直接将预测位姿作为第二位姿。
在一些实施例中,可以通过迭代最近点算法将当前时刻激光观测数据与所述预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿。该匹配是比较精细的匹配,可得到更加精确的位姿。迭代最近点(Iterative Closest Point,ICP)算法是一种迭代计算方法,能够使不同的坐标下的点云数据合并到同一个坐标系统中。ICP算法的目的是要找到待配准点云数据与参考云数据之间的旋转参数和平移参数,使得两点数据之间满足某种度量准则下的最优匹配。ICP算法本质上是基于最小二乘法的最优配准方法,算法重复进行选择对应关系点对,计算最优刚体变换,直到满足正确配准的收敛精度要求。
示例性地,融合权重为factor,计算第二位姿,第二位姿=第一位姿*factor+预测位姿*(1-factor)。
在本实施例提供的技术方案中,通过将局部定位结果进行打分,并判断所述局部定位结果是否足够精确,来得到一个作为全局定位和局部定位之间的过渡值的方式,在将局部定位与全局定位进行融合转化的同时,减小了局部定位的匹配误差。通过将当前时刻激光观测数据与预测位姿在所述局部地图中的局部匹配结果进行匹配打分,当匹配得分大于预设阈值时将当前时刻激光观测数据与预测位姿融合得到第二位姿,小于或等于时则直接将预测位姿作为第二位姿。提高了机器人定位的精确度。
参照图5,在第四实施例中,基于任一实施例,所述步骤S40,基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿,包括:
步骤S41,根据激光雷达的扫描数据建立全局地图,确定所述当前时刻激光观测数据与所述预测位姿在所述全局地图中的全局匹配结果;
步骤S42,根据所述全局匹配结果确定所述机器人在全局地图的第二匹配分值;
步骤S43,根据所述第二匹配分值确定所述机器人在所述全局地图的所述第三位姿。
可选地,在本实施例中,在获取完所述第二位姿之后,加载全局地图,并获取所述机器人在所述全局地图中关联的当前时间时刻的轮式里程计数据和惯性传感器数据,根据所述轮式里程计数据和惯性传感器数据确定所述机器人在所述全局地图中的全局定位结果。
在本实施例提供的技术方案中通过轮式里程计数据和惯性传感器数据确定所述机器人在所述全局地图中的全局定位结果,并将所述全局定位结果进行匹配打分,得到所述机器人在所述全局地图的所述第三位姿的方式,将机器人在局部地图中的定位结果融合至全局地图中。
参照图6,在第五实施例中,基于任一实施例,所述步骤S50,若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿,包括:
步骤S51,检测所述第二匹配分值;
步骤S52,当所述第二匹配分值大于所述第二阈值时,确定所述第三位姿对应的预设融合权重值;
步骤S53,根据所述融合权重值将所述第二位姿和所述第三位姿融合,得到所述最终定位结果的最终位姿;
步骤S54,当所述第二匹配分值小于或等于所述第二阈值时,将所述第二位姿作为所述最终定位结果的最终位姿。
可选地,通过迭代最近点算法将当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿。
示例性地,预设融合权重为Factor,计算最终位姿Final pose,Final pose=第二位姿*Factor+第三位姿*(1-Factor)。
在本实施例提供的技术方案中,整个融合过程只用到了简单的四则运算,方便快捷,能够满足实时性的要求,同时又能保证一个较为准确的定位结果输出。
参照图7,在第六实施例中,基于任一实施例,所述步骤S50之后,还包括:
步骤S90:将所述最终定位结果的所述最终位姿输出至决策层,以使所述决策层根据所述最终位姿规划所述机器人行驶路线。
可选地,本实施例提供一种目标定位结果的用途,在得到最终定位结果之后,将最终定位结果发送至决策层模块,决策层模块根据最终定位结果规划出机器人所行驶的路线。
在本实施例提供的技术方案中,通过将最终得到的目标定位结果输出至决策层,将上位机根据融合激光里程后得到的定位发送至机器人的决策层,决策层根据定位结果规划出机器人的行驶路线,实现了定位、决策这两个不同模块之间的串连。
此外,参照图8,本发明还提出一种机器人定位装置,所述机器人定位装置包括:
位姿预测模块100,用于根据惯性导航系统获取所述机器人的预测位姿;
局部定位模块200,用于基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿;
局部定位融合模块300,用于若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿;
全局定位模块400,用于基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿;
全局定位融合模块500,用于若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿。
可选地,所述机器人定位装置还可以执行以下步骤:
获取所述惯性导航系统中所述机器人对应的里程信息、加速度信息和角度变化信息;
根据所述里程信息、所述加速度信息和所述角度变化信息,确定所述机器人的所述预测位姿。
可选地,所述机器人定位装置还可以执行以下步骤:
确定所述当前时刻激光观测数据与所述预测位姿在所述局部地图中的局部匹配结果;
根据所述局部匹配结果确定所述机器人在所述局部地图的所述第一匹配分值;
根据所述第一匹配分值确定所述预测位姿在局部地图中的所述第一位姿。
可选地,所述机器人定位装置还可以执行以下步骤:
检测所述第一匹配分值;
当所述第一匹配分值大于所述第一阈值时,根据所述第一位姿和所述预测位姿通过预设算法确定所述机器人在所述局部地图中的第二位姿;
当所述第一匹配分值小于或等于所述第一阈值时,将所述预测位姿作为所述第二位姿。
可选地,所述机器人定位装置还可以执行以下步骤:
根据激光雷达的扫描数据建立全局地图,确定所述当前时刻激光观测数据与所述预测位姿在所述全局地图中的全局匹配结果;
根据所述全局匹配结果确定所述机器人在全局地图的第二匹配分值;
根据所述第二匹配分值确定所述机器人在所述全局地图的所述第三位姿。
可选地,所述机器人定位装置还可以执行以下步骤:
检测所述第二匹配分值;
当所述第二匹配分值大于所述第二阈值时,确定所述第三位姿对应的预设融合权重值;
根据所述融合权重值将所述第二位姿和所述第三位姿融合,得到所述最终定位结果的最终位姿;
当所述第二匹配分值小于或等于所述第二阈值时,将所述第二位姿作为所述最终定位结果的最终位姿。
可选地,所述机器人定位装置还可以执行以下步骤:
将所述最终定位结果的所述最终位姿输出至决策层,以使所述决策层根据所述最终位姿规划所述机器人行驶路线。
此外,本发明还提供一种控制终端,所述控制终端包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的机器人定位程序,所述机器人定位程序被所述处理器执行时实现如上实施例所述的机器人定位方法的各个步骤。
此外,本发明还提供一种计算机可读存储介质,所述计算机可读存储介质上存储有机器人定位程序,所述机器人定位程序被处理器执行时实现如上任一项所述的机器人定位方法的各个步骤。
需要说明的是,在本文中,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
通过以上的实施方式的描述,本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现,当然也可以通过硬件,但很多情况下前者是更佳的实施方式。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来,该计算机软件产品存储在如上所述的一个计算机可读存储介质(如ROM/RAM、磁碟、光盘)中,包括若干指令用以使得一台终端设备(可以是手机,计算机,服务器,或者网络设备等)执行本发明各个实施例所述的方法。
以上仅为本发明的优选实施例,并非因此限制本发明的专利范围,凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换,或直接或间接运用在其他相关的技术领域,均同理包括在本发明的专利保护范围内。
Claims (10)
1.一种机器人定位方法,其特征在于,所述机器人包括惯性导航系统和激光雷达,所述机器人定位方法的步骤包括:
根据惯性导航系统获取所述机器人的预测位姿;
基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿;
若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿;
基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿;
若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿。
2.如权利要求1所述的机器人定位方法,其特征在于,所述根据惯性导航系统获取所述机器人的预测位姿的步骤包括:
获取所述惯性导航系统中所述机器人对应的里程信息、加速度信息和角度变化信息;
根据所述里程信息、所述加速度信息和所述角度变化信息,确定所述机器人的所述预测位姿。
3.如权利要求1所述的机器人定位方法,其特征在于,所述基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿的步骤包括:
确定所述当前时刻激光观测数据与所述预测位姿在所述局部地图中的局部匹配结果;
根据所述局部匹配结果确定所述机器人在所述局部地图的所述第一匹配分值;
根据所述第一匹配分值确定所述预测位姿在局部地图中的所述第一位姿。
4.如权利要求1所述的机器人定位方法,其特征在于,所述若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿的步骤包括:
检测所述第一匹配分值;
当所述第一匹配分值大于所述第一阈值时,根据所述第一位姿和所述预测位姿通过预设算法确定所述机器人在所述局部地图中的第二位姿;
当所述第一匹配分值小于或等于所述第一阈值时,将所述预测位姿作为所述第二位姿。
5.如权利要求1所述的机器人定位方法,其特征在于,所述基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿的步骤包括:
根据激光雷达的扫描数据建立全局地图,确定所述当前时刻激光观测数据与所述预测位姿在所述全局地图中的全局匹配结果;
根据所述全局匹配结果确定所述机器人在全局地图的第二匹配分值;
根据所述第二匹配分值确定所述机器人在所述全局地图的所述第三位姿。
6.如权利要求1所述的机器人定位方法,其特征在于,所述若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿的步骤包括:
检测所述第二匹配分值;
当所述第二匹配分值大于所述第二阈值时,确定所述第三位姿对应的预设融合权重值;
根据所述融合权重值将所述第二位姿和所述第三位姿融合,得到所述最终定位结果的最终位姿;
当所述第二匹配分值小于或等于所述第二阈值时,将所述第二位姿作为所述最终定位结果的最终位姿。
7.如权利要求1所述的机器人定位方法,其特征在于,所述若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿的步骤之后,还包括:
将所述最终定位结果的所述最终位姿输出至决策层,以使所述决策层根据所述最终位姿规划所述机器人行驶路线。
8.一种机器人定位装置,其特征在于,所述机器人定位装置包括:
位姿预测模块,用于根据惯性导航系统获取所述机器人的预测位姿;
局部定位模块,用于基于激光雷达获取的当前时刻激光观测数据与预测位姿的局部地图进行匹配,以获得第一匹配分值和机器人在局部地图的第一位姿;
局部定位融合模块,用于若所述第一匹配分值大于第一阈值,则将所述第一位姿和所述预测位姿融合,以获得第二位姿;
全局定位模块,用于基于激光雷达获取的当前时刻激光观测数据与全局地图进行匹配,以获得第二匹配分值和机器人在全局地图的第三位姿;
全局定位融合模块,用于若所述第二匹配分值大于第二阈值,则将所述第二位姿和所述第三位姿融合,以获得最终定位结果的最终位姿。
9.一种控制终端,其特征在于,所述控制终端包括:存储器、处理器及存储在所述存储器上并可在所述处理器上运行的机器人定位程序,所述机器人定位程序被所述处理器执行时实现如权利要求1-7中任一项所述的机器人定位方法的步骤。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质上存储有机器人定位程序,所述机器人定位程序被处理器执行时实现如权利要求1-7中任一项所述的机器人定位方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210320073.3A CN114777770A (zh) | 2022-03-29 | 2022-03-29 | 机器人定位方法、装置、控制终端以及可读存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210320073.3A CN114777770A (zh) | 2022-03-29 | 2022-03-29 | 机器人定位方法、装置、控制终端以及可读存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114777770A true CN114777770A (zh) | 2022-07-22 |
Family
ID=82426211
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210320073.3A Pending CN114777770A (zh) | 2022-03-29 | 2022-03-29 | 机器人定位方法、装置、控制终端以及可读存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114777770A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115326051A (zh) * | 2022-08-03 | 2022-11-11 | 广州高新兴机器人有限公司 | 一种基于动态场景的定位方法、装置、机器人及介质 |
-
2022
- 2022-03-29 CN CN202210320073.3A patent/CN114777770A/zh active Pending
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115326051A (zh) * | 2022-08-03 | 2022-11-11 | 广州高新兴机器人有限公司 | 一种基于动态场景的定位方法、装置、机器人及介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110260867B (zh) | 一种机器人导航中位姿确定、纠正的方法、设备及装置 | |
CN109211251B (zh) | 一种基于激光和二维码融合的即时定位与地图构建方法 | |
US10921461B2 (en) | Method and apparatus for determining unmanned vehicle positioning accuracy | |
EP4170282A1 (en) | Method for calibrating mounting deviation angle between sensors, combined positioning system, and vehicle | |
KR102628778B1 (ko) | 위치결정을 위한 방법, 컴퓨팅 기기, 컴퓨터 판독가능 저장 매체 및 컴퓨터 프로그램 | |
JP2024050990A (ja) | 判定装置 | |
CN108351216B (zh) | 估计装置、控制方法、程序以及存储介质 | |
CN110749901B (zh) | 自主移动机器人及其地图拼接方法、装置和可读存储介质 | |
CN109325979B (zh) | 基于深度学习的机器人回环检测方法 | |
WO2019188745A1 (ja) | 情報処理装置、制御方法、プログラム及び記憶媒体 | |
CN110889808A (zh) | 一种定位的方法、装置、设备及存储介质 | |
WO2020189079A1 (ja) | 自己位置推定装置、それを備えた自動運転システム、および、自己生成地図共有装置 | |
US11158065B2 (en) | Localization of a mobile unit by means of a multi hypothesis kalman filter method | |
CN110866927A (zh) | 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法 | |
CN108507579B (zh) | 一种基于局部粒子滤波的重定位方法 | |
CN112750161B (zh) | 用于移动机器人的地图更新方法 | |
CN110789529A (zh) | 车辆的控制方法、装置及计算机可读存储介质 | |
CN114488183A (zh) | 障碍物点云的处理方法、装置、设备以及可读存储介质 | |
CN114777770A (zh) | 机器人定位方法、装置、控制终端以及可读存储介质 | |
CN111856499A (zh) | 基于激光雷达的地图构建方法和装置 | |
JP2020098196A (ja) | 推定装置、制御方法、プログラム及び記憶媒体 | |
Zeng et al. | An Indoor 2D LiDAR SLAM and Localization Method Based on Artificial Landmark Assistance | |
JPH07146121A (ja) | 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置 | |
US10914840B2 (en) | Self position estimation apparatus | |
Ozaki et al. | 6-DoF EKF SLAM with global planar features in artificial environments |
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 |