CN116626700A - 一种机器人定位方法、装置、电子设备和存储介质 - Google Patents
一种机器人定位方法、装置、电子设备和存储介质 Download PDFInfo
- Publication number
- CN116626700A CN116626700A CN202310576360.5A CN202310576360A CN116626700A CN 116626700 A CN116626700 A CN 116626700A CN 202310576360 A CN202310576360 A CN 202310576360A CN 116626700 A CN116626700 A CN 116626700A
- Authority
- CN
- China
- Prior art keywords
- pose
- current
- target
- determining
- robot
- 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
- 238000000034 method Methods 0.000 title claims abstract description 94
- 230000004927 fusion Effects 0.000 claims abstract description 181
- 230000008859 change Effects 0.000 claims description 26
- 238000013507 mapping Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 9
- 238000010586 diagram Methods 0.000 description 13
- 230000008569 process Effects 0.000 description 10
- 238000005516 engineering process Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 230000033001 locomotion Effects 0.000 description 5
- 230000007774 longterm Effects 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 4
- 238000011161 development Methods 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 239000013307 optical fiber Substances 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 239000002699 waste material Substances 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000033228 biological regulation Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
Classifications
-
- 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
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- 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/003—Bistatic lidar systems; Multistatic lidar systems
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请公开了一种机器人定位方法、装置、电子设备和存储介质,用于提升机器人定位的精度,提高激光数据的利用率。本申请实施例中,从至少两个激光雷达中确定出目标激光雷达;采用目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿;以及获取目标激光雷达在当前时刻采集到的局部子地图,基于局部子地图得到第二当前位姿;以及采用里程计确定机器人当前时刻的第三当前位姿;基于第一当前位姿、第二当前位姿以及第三当前位姿得到机器人的当前融合位姿。充分利用每个激光雷达获取到的激光数据,在确定第二当前位姿时基于局部子地图进行,确定第一当前位姿时基于目标激光雷达获取到的激光数据进行,保证了得到的当前融合位姿的准确性。
Description
技术领域
本申请涉及激光雷达技术领域,尤其涉及一种机器人定位方法、装置、电子设备和存储介质。
背景技术
随着激光雷达技术的发展,机器人的应用越来越广泛,在相关技术中采用的机器人定位方法中,当环境发生变化时,比如新增或者减少环境中的物体,或者环境中存在移动物体时,当前的环境信息与先验地图发生较大的变化,若直接使用当前激光数据与先验地图进行匹配,则必然导致匹配的误差比较大,定位精度不高。且在多个激光雷达工作时,相关技术中只是简单地选择测距近的激光雷达进行工作,并不是同时选择两个激光的观测数据,没有充分利用两个激光的观测信息。
发明内容
本申请的目的是提供一种机器人定位方法、装置、电子设备和存储介质,用于提升机器人定位的精度,提高激光数据的利用率。
第一方面,本申请实施例提供了一种机器人定位方法,所述机器人包括里程计和至少两个激光雷达,所述方法包括:
基于所述至少两个激光雷达本次获取激光数据的时刻与所述至少两个激光雷达对应的目标时刻之间的时间差,从所述至少两个激光雷达中确定出目标激光雷达;其中所述目标时刻为最近一次得到的所述机器人的融合位姿的时刻;
采用所述目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿;以及获取所述目标激光雷达在所述当前时刻采集到的局部子地图,基于所述局部子地图得到所述第二当前位姿;以及采用里程计确定所述机器人当前时刻的第三当前位姿;
基于所述第一当前位姿、所述第二当前位姿以及所述第三当前位姿,得到当前融合位姿的约束条件;
基于所述当前融合位姿的约束条件得到所述机器人的当前融合位姿。
在本申请中在机器人运动过程中,基于时间差从多个激光雷达中确定目标激光雷达,可以充分利用每个激光雷达获取到的激光数据,在确定第二当前位姿时基于局部子地图进行,确定第一当前位姿时基于目标激光雷达获取到的激光数据进行,保证了得到的当前融合位姿的准确性。
在一些可能的实施例中,所述采用所述目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿,包括:
根据所述当前时刻对应的激光数据确定目标环境中是否存在新增物体;
若确定所述目标环境中存在新增物体,则基于所述激光数据、先验地图以及当前时刻对应的局部子地图,得到所述机器人的第一当前位姿;其中,所述先验地图是基于所述目标环境采集到的地图;
若确定所述目标环境中不存在新增物体,则基于所述激光数据与所述先验地图,得到所述机器人的第一当前位姿。
在本申请中,基于激光数据确定目标环境中是否存在新增物体,并根据是否存在新增物体来确定第一当前位姿,保证了得到的第一当前位姿的准确性。
在一些可能的实施例中,所述根据所述当前时刻对应的激光数据确定目标环境中是否存在新增物体,包括:
针对所述激光数据中的任意一束点云执行:确定所述点云中的每个点在所述机器人的机器人坐标系中的第一坐标信息;并获取里程计确定的所述机器人在世界坐标系中的角度;其中,所述机器人坐标系为以所述机器人的中心为坐标系原点建立的坐标系;
根据所述每个点的第一坐标信息以及所述角度,确定所述每个点在世界坐标系下的第二坐标信息;
基于所述点云中每个点的第二坐标信息与先验地图中栅格点云的坐标信息,确定所述点云与最近栅格点云之间的目标距离;其中,所述栅格点云为所述先验地图中的障碍物构成的点云;所述最近栅格点云为与所述点云之间距离最短的栅格点云;
若所述目标距离大于预设距离,则确定所述目标环境中存在新增物体;
若所述目标距离小于等于所述预设距离,则确定所述目标环境中不存在新增物体。
在本申请中,基于先验地图中的栅格点云与激光数据来确定目标环境中是否存在新增物体,可以实时了解目标环境的情况。
在一些可能的实施例中,所述获取所述目标激光雷达在所述当前时刻采集到的局部子地图,基于所述局部子地图得到所述第二当前位姿,包括:
获取在所述当前时刻之前得到融合位姿的时刻对应的全部或部分局部子地图,并获取所述当前时刻对应的局部子地图;
对所述在所述当前时刻之前计算得到融合位姿的时刻对应的全部或部分局部子地图、所述当前时刻对应的局部子地图进行拼接处理,得到当前帧地图;
基于所述当前帧地图,得到第二当前位姿。
在本申请中,通过对局部子地图进行拼接处理,进一步保证了得到的第二当前位姿的准确性。
在一些可能的实施例中,所述从所述至少两个激光雷达中确定出目标激光雷达,包括:
针对每个激光雷达,确定所述激光雷达本次获取激光数据的时间戳与所述目标时刻之间的时间差;
若所述时间差大于预设时间差,则将所述激光雷达本次获取激光数据的时间戳作为当前时刻,将所述激光雷达作为目标激光雷达;
若所有时间差均小于等于所述预设时间差,则返回执行确定所述激光雷达本次获取激光数据的时间戳与所述目标时刻之间的时间差。
在本申请中,为了在保证对机器人定位的准确性的同时,减低对资源的浪费,因此本申请中通过合理的设定预设时间差,减少了对计算资源的浪费。
在一些可能的实施例中,所述基于所述第一当前位姿、所述第二当前位姿以及所述第三当前位姿,得到当前融合位姿的约束条件,包括:
基于获取到的第三前位姿与所述第三当前位姿,得到所述当前融合位姿与前融合位姿之间的第一相对变化量;
基于获取到的第二前位姿与所述第二当前位姿,得到所述当前融合位姿与所述前融合位姿之间的第二相对变化量;
针对所述目标时刻与所述当前时刻中间的每个获取激光数据的时间戳,确定所述时间戳与所述目标时刻之间的第一对应关系,以及与所述当前时刻之间的第二对应关系;基于所述时间戳对应的第一位姿与所述第一当前位姿,得到第三对应关系;
其中,所述第一前位姿为所述目标时刻对应的第一当前位姿、所述第二前位姿为所述目标时刻对应的第二当前位姿、所述第三前位姿为所述目标时刻对应的第三当前位姿,所述前融合位姿为所述目标时刻对应的当前融合位姿;
基于所述第一相对变化量、第二相对变化量、所述第一对应关系、所述第二对应关系、所述第三对应关系确定所述当前融合位姿的约束条件。
在本申请中基于目标时刻对应的位姿以及当前时刻对应的位姿来确定约束条件,保证了得到的当前融合位姿的准确性。
在一些可能的实施例中,所述确定所述时间戳与所述目标时刻之间的第一对应关系,以及与所述当前时刻之间的第二对应关系,包括:
确定所述时间戳在时序上前后相邻的第三中间位姿;所述第三中间位姿为采用里程计获取到的所述机器人的位姿;
基于所述前后相邻的第三中间位姿,得到所述时间戳对应的第三中间位姿;
基于目标时刻在时序上前后相邻的第三中间位姿,得到所述目标时刻对应的中间位姿;
基于当前时刻在时序上相邻的第三中间位姿,得到所述当前时刻对应的中间位姿;
基于所述时间戳对应的第三中间位姿与所述目标时刻对应的中间位姿,得到所述时间戳与所述目标时刻之间的第一对应关系;
基于所述时间戳对应的第三中间位姿与所述当前时刻对应的中间位姿得到所述时间戳与所述当前时刻之间的第二对应关系。
在一些可能的实施例中,所述基于所述第一相对变化量、第二相对变化量、所述第一对应关系、所述第二对应关系、所述第三对应关系确定所述当前融合位姿的约束条件,包括:
基于第一权重、第一当前位姿确定所述当前融合位姿的第一约束关系;其中,所述第一权重是根据第一当前位姿得到的;
基于第二权重、第一相对变化量确定所述当前融合位姿的第二约束关系;其中,所述第二权重是预先设置的;
基于第三权重、第二相对变化量确定所述当前融合位姿的第三约束关系;其中,所述第三权重是预先设置的;
基于第四权重、所述第一对应关系、所述第二对应关系、所述第三对应关系,确定所述当前融合位姿的第四约束关系;其中,所述第四权重是预先设置的。
在本申请中通过设定合理的权重,进一步保证了得到的当前融合位姿的准确性。
在一些可能的实施例中,所述第一权重是根据以下方法确定的:
基于所述第一当前位姿与所述先验地图,得到所述第一当前位姿对应的第一分值;并,获取所述机器人前融合位姿对应的第二分值;
基于所述第一分值、第二分值确定所述第一位姿对应的权重。
在本申请中,基于第一当前位姿的得分来确定第一权重,保证了第一权重的准确性。
在一些可能的实施例中,所述基于所述第一分值、第二分值确定所述第一位姿对应的权重,包括:
若所述第一分值大于第一阈值或所述第二分值小于第二阈值,则所述第一权重为第一固定值;其中,第一阈值大于第二阈值;
若所述第一分值小于所述第一阈值,且所述第二分值大于所述第一阈值,则确定所述第一固定值与所述第一分值的积值,将所述积值与所述第二分值的比值作为所述第一权重。
在一些可能的实施例中,若存在多个目标激光雷达,则所述基于所述前融合位姿与当前融合位姿之间的关系得到所述机器人的当前融合位姿之后,所述方法还包括:
对每个目标激光雷达对应的当前融合位姿进行评分,得到每个目标激光雷达对应的分值;
基于每个目标激光雷达对应的分值,得到每个激光雷达对应的权重;
基于每个激光雷达对应的权重以及每个激光雷达对应的当前融合位姿,得到所述机器人对应的最终位姿。
在本申请中,在同一时刻可能存在多个目标激光雷达,在存在多个目标激光雷达的情况下,通过对应的权重来确定机器人的最终位姿,保证了得到的最终位姿的准确性。
在一些可能的实施例中,若所述目标激光雷达的目标时刻为初始时刻,则所述初始时刻对应的初始位姿是根据以下方法确定的:
以所述机器人中心机器人坐标系,确定所述目标激光雷达在所述机器人坐标系下的坐标信息;
分别确定每个激光雷达获取到的激光数据在激光坐标系下的点云信息;
将所述目标激光雷达对应的点云信息映射至所述机器人坐标系中;
针对其他激光雷达对应的点云信息中的每个点,若所述点映射至所述机器人坐标系中的位置已存在点,则丢弃所述点;若所述点映射至所述机器人坐标系中的位置不存在点,则将该点映射至所述机器人坐标系中,得到所述机器人在机器人坐标系下的激光点云;
将所述机器人在机器人坐标系下的激光点云与先验地图进行匹配,得到所述机器人的初始位姿。
在本申请中,通过上述方法得到机器人的初始位姿,保证了后续对机器人定位的准确性。
第二方面,本申请还提供了一种机器人定位装置,所述机器人包括里程计和至少两个激光雷达,所述装置包括:
目标激光雷达确定模块,用于基于所述至少两个激光雷达本次获取激光数据的时刻与所述至少两个激光雷达对应的目标时刻之间的时间差,从所述至少两个激光雷达中确定出目标激光雷达;其中所述目标时刻为最近一次得到的所述机器人的融合位姿的时刻;
位姿确定模块,用于采用所述目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿;以及获取所述目标激光雷达在所述当前时刻采集到的局部子地图,基于所述局部子地图得到所述第二当前位姿;以及采用里程计确定所述机器人当前时刻的第三当前位姿;
条件确定模块,用于基于所述第一当前位姿、所述第二当前位姿以及所述第三当前位姿,得到当前融合位姿的约束条件;
融合模块,用于基于所述当前融合位姿的约束条件得到所述机器人的当前融合位姿。
在一些可能的实施例中,所述位姿确定模块执行采用所述目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿时,具体用于:
根据所述当前时刻对应的激光数据确定目标环境中是否存在新增物体;
若确定所述目标环境中存在新增物体,则基于所述激光数据、先验地图以及当前时刻对应的局部子地图,得到所述机器人的第一当前位姿;其中,所述先验地图是基于所述目标环境采集到的地图;
若确定所述目标环境中不存在新增物体,则基于所述激光数据与所述先验地图,得到所述机器人的第一当前位姿。
在一些可能的实施例中,所述位姿确定模块执行根据所述当前时刻对应的激光数据确定目标环境中是否存在新增物体时,具体用于:
针对所述激光数据中的任意一束点云执行:确定所述点云中的每个点在所述机器人的机器人坐标系中的第一坐标信息;并获取里程计确定的所述机器人在世界坐标系中的角度;其中,所述机器人坐标系为以所述机器人的中心为坐标系原点建立的坐标系;
根据所述每个点的第一坐标信息以及所述角度,确定所述每个点在世界坐标系下的第二坐标信息;
基于所述点云中每个点的第二坐标信息与先验地图中栅格点云的坐标信息,确定所述点云与最近栅格点云之间的目标距离;其中,所述栅格点云为所述先验地图中的障碍物构成的点云;所述最近栅格点云为与所述点云之间距离最短的栅格点云;
若所述目标距离大于预设距离,则确定所述目标环境中存在新增物体;
若所述目标距离小于等于所述预设距离,则确定所述目标环境中不存在新增物体。
在一些可能的实施例中,所述位姿确定模块执行获取所述目标激光雷达在所述当前时刻采集到的局部子地图,基于所述局部子地图得到所述第二当前位姿时,具体用于:
获取在所述当前时刻之前得到融合位姿的时刻对应的全部或部分局部子地图,并获取所述当前时刻对应的局部子地图;
对所述在所述当前时刻之前计算得到融合位姿的时刻对应的全部或部分局部子地图、所述当前时刻对应的局部子地图进行拼接处理,得到当前帧地图;
基于所述当前帧地图,得到第二当前位姿。
在一些可能的实施例中,所述目标激光雷达确定模块执行从所述至少两个激光雷达中确定出目标激光雷达时,具体用于:
针对每个激光雷达,确定所述激光雷达本次获取激光数据的时间戳与所述目标时刻之间的时间差;
若所述时间差大于预设时间差,则将所述激光雷达本次获取激光数据的时间戳作为当前时刻,将所述激光雷达作为目标激光雷达;
若所有时间差均小于等于所述预设时间差,则返回执行确定所述激光雷达本次获取激光数据的时间戳与所述目标时刻之间的时间差。
在一些可能的实施例中,所述条件确定模块执行基于所述第一当前位姿、所述第二当前位姿以及所述第三当前位姿,得到当前融合位姿的约束条件时,具体用于:
基于获取到的第三前位姿与所述第三当前位姿,得到所述当前融合位姿与前融合位姿之间的第一相对变化量;
基于获取到的第二前位姿与所述第二当前位姿,得到所述当前融合位姿与所述前融合位姿之间的第二相对变化量;
针对所述目标时刻与所述当前时刻中间的每个获取激光数据的时间戳,确定所述时间戳与所述目标时刻之间的第一对应关系,以及与所述当前时刻之间的第二对应关系;基于所述时间戳对应的第一位姿与所述第一当前位姿,得到第三对应关系;
其中,所述第一前位姿为所述目标时刻对应的第一当前位姿、所述第二前位姿为所述目标时刻对应的第二当前位姿、所述第三前位姿为所述目标时刻对应的第三当前位姿,所述前融合位姿为所述目标时刻对应的当前融合位姿;
基于所述第一相对变化量、第二相对变化量、所述第一对应关系、所述第二对应关系、所述第三对应关系确定所述当前融合位姿的约束条件。
在一些可能的实施例中,所述条件确定模块执行确定所述时间戳与所述目标时刻之间的第一对应关系,以及与所述当前时刻之间的第二对应关系时,具体用于:
确定所述时间戳在时序上前后相邻的第三中间位姿;所述第三中间位姿为采用里程计获取到的所述机器人的位姿;
基于所述前后相邻的第三中间位姿,得到所述时间戳对应的第三中间位姿;
基于目标时刻在时序上前后相邻的第三中间位姿,得到所述目标时刻对应的中间位姿;
基于当前时刻在时序上相邻的第三中间位姿,得到所述当前时刻对应的中间位姿;
基于所述时间戳对应的第三中间位姿与所述目标时刻对应的中间位姿,得到所述时间戳与所述目标时刻之间的第一对应关系;
基于所述时间戳对应的第三中间位姿与所述当前时刻对应的中间位姿得到所述时间戳与所述当前时刻之间的第二对应关系。
在一些可能的实施例中,所述条件确定模块执行基于所述第一相对变化量、第二相对变化量、所述第一对应关系、所述第二对应关系、所述第三对应关系确定所述当前融合位姿的约束条件时,具体用于:
基于第一权重、第一当前位姿确定所述当前融合位姿的第一约束关系;其中,所述第一权重是根据第一当前位姿得到的;
基于第二权重、第一相对变化量确定所述当前融合位姿的第二约束关系;其中,所述第二权重是预先设置的;
基于第三权重、第二相对变化量确定所述当前融合位姿的第三约束关系;其中,所述第三权重是预先设置的;
基于第四权重、所述第一对应关系、所述第二对应关系、所述第三对应关系,确定所述当前融合位姿的第四约束关系;其中,所述第四权重是预先设置的。
在一些可能的实施例中,所述第一权重是根据以下方法确定的:
基于所述第一当前位姿与所述先验地图,得到所述第一当前位姿对应的第一分值;并,获取所述机器人前融合位姿对应的第二分值;
基于所述第一分值、第二分值确定所述第一位姿对应的权重。
在一些可能的实施例中,所述条件确定模块执行基于所述第一分值、第二分值确定所述第一位姿对应的权重时,具体用于:
若所述第一分值大于第一阈值或所述第二分值小于第二阈值,则所述第一权重为第一固定值;其中,第一阈值大于第二阈值;
若所述第一分值小于所述第一阈值,且所述第二分值大于所述第一阈值,则确定所述第一固定值与所述第一分值的积值,将所述积值与所述第二分值的比值作为所述第一权重。
在一些可能的实施例中,若存在多个目标激光雷达,则所述融合模块执行基于所述前融合位姿与当前融合位姿之间的关系得到所述机器人的当前融合位姿之后,还用于:
对每个目标激光雷达对应的当前融合位姿进行评分,得到每个目标激光雷达对应的分值;
基于每个目标激光雷达对应的分值,得到每个激光雷达对应的权重;
基于每个激光雷达对应的权重以及每个激光雷达对应的当前融合位姿,得到所述机器人对应的最终位姿。
在一些可能的实施例中,若所述目标激光雷达的目标时刻为初始时刻,则所述初始时刻对应的初始位姿是根据以下方法确定的:
以所述机器人中心机器人坐标系,确定所述目标激光雷达在所述机器人坐标系下的坐标信息;
分别确定每个激光雷达获取到的激光数据在激光坐标系下的点云信息;
将所述目标激光雷达对应的点云信息映射至所述机器人坐标系中;
针对其他激光雷达对应的点云信息中的每个点,若所述点映射至所述机器人坐标系中的位置已存在点,则丢弃所述点;若所述点映射至所述机器人坐标系中的位置不存在点,则将该点映射至所述机器人坐标系中,得到所述机器人在机器人坐标系下的激光点云;
将所述机器人在机器人坐标系下的激光点云与先验地图进行匹配,得到所述机器人的初始位姿。
第三方面,本申请另一实施例还提供了一种电子设备,包括至少一个处理器;以及与所述至少一个处理器通信连接的存储器;其中,所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行本申请第一方面实施例提供的任一方法。
第四方面,本申请另一实施例还提供了一种计算机可读存储介质,其中,所述计算机可读存储介质存储有计算机程序,所述计算机程序用于使计算机执行本申请第一方面实施例提供的任一方法。
本申请的其它特征和优点将在随后的说明书中阐述,并且,部分地从说明书中变得显而易见,或者通过实施本申请而了解。本申请的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
附图说明
为了更清楚地说明本申请实施例的技术方案,下面将对本申请实施例中所需要使用的附图作简单地介绍,显而易见地,下面所介绍的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的一种机器人定位方法的应用场景示意图;
图2为本申请实施例提供的一种机器人定位方法的机器人示意图;
图3为本申请实施例提供的一种机器人定位方法的机器人的工作环境示意图;
图4为本申请实施例提供的一种机器人定位方法的整体流程示意图;
图5为本申请实施例提供的一种机器人定位方法的确定第一当前位姿的流程示意图;
图6为本申请实施例提供的一种机器人定位方法的确定新增物体的流程示意图;
图7为本申请实施例提供的一种机器人定位方法的确定第二当前位姿的流程示意图;
图8为本申请实施例提供的一种机器人定位方法的确定约束条件的流程示意图;
图9为本申请实施例提供的一种机器人定位方法的确定第二对应关系的流程示意图;
图10为本申请实施例提供的一种机器人定位方法的时序示意图;
图11为本申请实施例提供的一种机器人定位方法的确定约束条件的另一流程示意图;
图12为本申请实施例提供的一种机器人定位方法的确定第一权重的流程示意图;
图13为本申请实施例提供的一种机器人定位方法的确定最终位姿的流程示意图;
图14为本申请实施例提供的一种机器人定位方法的确定目标激光雷达的流程示意图;
图15为本申请实施例提供的一种机器人定位方法的确定初始位姿的流程示意图;
图16为本申请实施例提供的一种机器人定位方法的装置示意图;
图17为本申请实施例提供的一种机器人定位方法的电子设备示意图。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚明白,下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。在不冲突的情况下,本申请中的实施例及实施例中的特征可以相互任意组合。并且,虽然在流程图中示出了逻辑顺序,但是在某些情况下,可以按不同于此处的顺序执行所示出或描述的步骤。
本申请的说明书和权利要求书及上述附图中的术语“第一”和“第二”是用于区别不同对象,而非用于描述特定顺序。此外,术语“包括”以及它们任何变形,意图在于覆盖不排他的保护。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元,而是可选地还包括没有列出的步骤或单元,或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。本申请中的“多个”可以表示至少两个,例如可以是两个、三个或者更多,本申请实施例不做限制。
本申请技术方案中,对数据的采集、传播、使用等,均符合国家相关法律法规要求。
发明人研究发现,随着激光雷达技术的发展,机器人的应用越来越广泛,在相关技术中采用的机器人定位方法中,当环境发生变化时,比如新增或者减少环境中的物体,或者环境中存在移动物体时,当前的环境信息与先验地图发生较大的变化,若直接使用当前激光数据与先验地图进行匹配,则必然导致匹配的误差比较大,定位精度不高。且在多个激光雷达工作时,相关技术中只是简单地选择测距近的激光雷达进行工作,并不是同时选择两个激光的观测数据,没有充分利用两个激光的观测信息。
有鉴于此,本申请提出了一种机器人定位方法、装置、电子设备和存储介质,用于解决上述问题。本申请的发明构思可概括为:针对包括里程计和至少两个激光雷达的机器人,基于至少两个激光雷达本次获取激光数据的时刻与至少两个激光雷达对应的目标时刻之间的时间差,从至少两个激光雷达中确定出目标激光雷达;其中目标时刻为最近一次得到的机器人的融合位姿的时刻;采用目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿;以及获取目标激光雷达在当前时刻采集到的局部子地图,基于局部子地图得到第二当前位姿;以及采用里程计确定机器人当前时刻的第三当前位姿;基于第一当前位姿、第二当前位姿以及第三当前位姿,得到当前融合位姿的约束条件;基于当前融合位姿的约束条件得到机器人的当前融合位姿。
为了便于理解,下面结合附图对本申请实施例提供的一种机器人定位方法进行说明:
如图1所示,为本申请实施例中的一种机器人定位方法的应用场景图。图中包括:服务器10、里程计20、激光雷达30包括:激光雷达301、激光雷达302,……,目标激光雷达30k,……,激光雷达30n;其中:
服务器10基于激光雷达30本次获取激光数据的时刻与激光雷达30对应的目标时刻之间的时间差,从激光雷达30中确定出目标激光雷达30k;其中目标时刻为最近一次得到的机器人的融合位姿的时刻;采用目标激光雷达30k获取到的激光数据确定当前时刻对应的第一当前位姿;以及获取目标激光雷达30k在当前时刻采集到的局部子地图,基于局部子地图得到第二当前位姿;以及采用里程计20确定机器人当前时刻的第三当前位姿;基于第一当前位姿、第二当前位姿以及第三当前位姿,得到当前融合位姿的约束条件;基于当前融合位姿的约束条件得到机器人的当前融合位姿。
需要知道的是,服务器除了如图1所示的设置方式外,还可设置在机器人内部,即上述由服务器执行的步骤均由机器人本体执行。本申请对具体的设置方式不做限定,技术人员可根据需求自行设定。
本申请中的描述中仅就单个服务器10、里程计20加以详述,但是本领域技术人员应当理解的是,示出的服务器10、里程计20、激光雷达30旨在表示本申请的技术方案涉及的服务器10、里程计20、激光雷达30的操作。而非暗示对服务器10、里程计20、激光雷达30的数量、类型或是位置等具有限制。应当注意,如果向图示环境中添加附加模块或从其中去除个别模块,不会改变本申请的示例实施例的底层概念。另外,本领域技术人员可以理解的是,上述数据的收发也是需要通过网络实现的。
此外,本申请提出的一种机器人定位方法不仅适用于图1所示的应用场景,还适用于任何有机器人定位需求的装置。
为了便于理解,下面对本申请实施例提供的机器人进行说明,如图2所示,机器人包括:移动底盘和激光雷达,其中移动底盘包括:运动控制器、电机、电池、嵌入式计算机、里程计;如图3所示,为机器人的工作环境示意图;该机器人在基于工作环境下生成的二维栅格地图中进行高精度定位。该地图中包括货架、工作台、房屋支撑架以及需要搬运的货物等部件。本申请提供的移动机器人利用里程计、激光雷达和先验地图进行自主定位,利用激光雷达快速重定位确认初始位姿,当环境发生较大变化时,多个激光雷达交替融合与环境匹配度高的激光观测结果,并实时构建和更新子地图,利用子地图对当前的位姿进行纠正,从而达到高精度定位的目的。
如图4所示,为本申请实施例提供的一种机器人定位方法的整体流程示意图,其中:
步骤401中:基于至少两个激光雷达本次获取激光数据的时刻与至少两个激光雷达对应的目标时刻之间的时间差,从至少两个激光雷达中确定出目标激光雷达;其中目标时刻为最近一次得到的机器人的融合位姿的时刻;
步骤402中:采用目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿;以及获取目标激光雷达在当前时刻采集到的局部子地图,基于局部子地图得到第二当前位姿;以及采用里程计确定机器人当前时刻的第三当前位姿;
步骤403中:基于第一当前位姿、第二当前位姿以及第三当前位姿,得到当前融合位姿的约束条件;
步骤404中:基于当前融合位姿的约束条件得到机器人的当前融合位姿。
在本申请中在运动过程中,基于时间差从多个激光雷达中确定目标激光雷达,可以充分利用每个激光雷达获取到的激光数据,在确定第二当前位姿时基于局部子地图进行,确定第一当前位姿时基于目标激光雷达获取到的激光数据进行,保证了得到的当前融合位姿的准确性。
为了便于进一步了解本申请实施例提供的一种机器人定位方法,下面对图4中的步骤进行详细说明。
在本申请中,为了保证确定的机器人的当前融合位资的准确性,因此,基于第一当前位资、第二当前位资、第三当前位资以及当前融合位姿的约束条件确定当前融合位资,下面对下面几部分分别进行说明:
1、获取第一当前位资
在一些可能的实施例中,采用目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿时,具体可实施为如图5所示的步骤,其中:
步骤501中:根据当前时刻对应的激光数据确定目标环境中是否存在新增物体;
由于目标环境中的可能存在新增物体,仅采用先验地图来对机器人进行定位,会导得到的机器人的位资不准确,因此,在本申请中会通过激光数据来确定目标环境中是否存在新增物体,具体可实施为如图6所示的步骤,其中:
步骤601中:针对激光数据中的任意一束点云执行:确定点云中的每个点在机器人的机器人坐标系中的第一坐标信息;并获取里程计确定的机器人在世界坐标系中的角度;
其中,机器人坐标系为以机器人的中心为坐标系原点建立的坐标系;
步骤602中:根据每个点的第一坐标信息以及角度,确定每个点在世界坐标系下的第二坐标信息;
针对激光数据中的第k束激光数据确定第k束激光数据/>在机器人坐标系的第一坐标信息为(xk,sens,yk,sens,zk,sens),里程计确定的机器人在世界坐标系中的角度为θ,则第k束激光数据/>在世界坐标系下的第二坐标信息为:
其中,为第k束激光在世界坐标系下的坐标,里程计确定的机器人在世界坐标系中的角度为θ。
计算第二坐标信息离最近栅格坐标的距离,当该距离小于预设距离,则表示该激光点云是由目标环境中原有的物体产生的,说明该激光点云具有长期激光点云特征;如果该距离大于预设距离,则表示该激光点云是由目标环境中新增的物体产生的,称为短期激光点云特征。因此,在得到坐标信息后,可实施步骤603,在步骤603中:基于点云中每个点的第二坐标信息与先验地图中栅格点云的坐标信息,确定点云与最近栅格点云之间的目标距离;
其中,栅格点云为先验地图中的障碍物构成的点云;最近栅格点云为与点云之间距离最短的栅格点云;
步骤604中:若目标距离大于预设距离,则确定目标环境中存在新增物体;
步骤605中:若目标距离小于等于预设距离,则确定目标环境中不存在新增物体。
例如:针对目标环境,确定该目标环境中存在栅格点云1,栅格点云2,栅格点云3,栅格点云4,确定第k束激光数据与栅格点云1之间的距离为0.2米,第k束激光数据与栅格点云2之间的距离为0.4米,第k束激光数据与栅格点云3之间的距离为0.5米,第k束激光数据与栅格点云4之间的距离为0.1米,则确定栅格点云4为最近栅格点云;若预设距离为0.2米,则确定栅格点云4为目标环境中的新增物体对应的栅格点云。
步骤502中:若确定目标环境中存在新增物体,则基于激光数据、先验地图以及当前时刻对应的局部子地图,得到机器人的第一当前位姿;
其中,先验地图是基于目标环境采集到的地图;
例如:将具有长期激光数据特征的激光数据与先验地图进行匹配,并将具有短期激光特征的激光数据和具有长期激光特征的激光数据一起与当前时刻对应的局部子地图进行匹配,使用激光地图(scan to map)的方法,得到第一当前位资。
步骤503中:若确定目标环境中不存在新增物体,则基于激光数据与先验地图,得到机器人的第一当前位姿。
例如:将具有长期激光数据特征的激光数据与先验地图进行匹配,使用scan tomap的方法,得到第一当前位资。
在本申请中,基于先验地图中的栅格点云与激光数据来确定目标环境中是否存在新增物体,可以实时了解目标环境的情况。
2、获取第二位资
在仓储环境中,比如行人、设备和货物的移动以及货物的增加或者减少都会环境经常发生比较大的变化,如果此时只是根据先验地图进行机器人定位,定位精度差且容易出现定位异常的情况。如果根据情况实时对先验地图进行更新,由于此时设备本身的定位精度差,基于该位姿对先验地图更新会影响原先建立得地图,从而影响后期先验地图的使用。因此,在本申请中通过局部子地图来确定第二当前位资,在本申请实施例中在获取目标激光雷达在当前时刻采集到的局部子地图,基于局部子地图得到第二当前位姿时,具体可实施为如图7所示的步骤,其中:
步骤701中:获取在当前时刻之前得到融合位姿的时刻对应的全部或部分局部子地图,并获取当前时刻对应的局部子地图;
步骤702中:对在当前时刻之前计算得到融合位姿的时刻对应的全部或部分局部子地图、当前时刻对应的局部子地图进行拼接处理,得到当前帧地图;
步骤703中:基于当前帧地图,得到第二当前位姿。
例如:当前时刻为第三次计算融合位资,得到的局部子地图为子地图3,该时刻之前计算过两次融合位资,得到的融合子地图分别为子地图1,子地图2;则可以对子地图3和子地图1进行拼接处理,或对子地图3和子地图2进行拼接处理,或对子地图3、子地图2和子地图1进行拼接处理,得到当前帧地图。
3、获取第三位资
在机器人运行过程中,利用里程计进行位姿估算。当机器人移动时,利用里程计确定机器人的第三当前位资为(x,y,θ),其中x,y分别为世界坐标系下横纵坐标,表示机器人在x轴、y轴所在二维平面的位置,θ表示机器人在x轴、y轴所在二维平面的方向。
4、确定约束关系
在一些可能的实施例中,在得到机器人的第一当前位姿、第二当前位姿以及第三当前位姿后,需要根据当前融合位姿的约束条件来确定当前融合位资,其中,确定当前融合位资的约束条件时,具体可实施为如图8所示的步骤:
步骤801中:基于获取到的第三前位姿与第三当前位姿,得到当前融合位姿与前融合位姿之间的第一相对变化量;
在本申请实施例中,可根据公式2来确定第一相对变化量:
/>
其中:ΔT0为第一相对变化量,O1为第三前位资,O2为第三当前位资,为第三前位资对应的横坐标,/>为第三前位资对应的纵坐标,/>为第三前位资对应的在世界坐标的角度,/>为第三前位资对应的横坐标,/>为第三前位资对应的纵坐标,/>为第三前位资对应的在世界坐标的角度。
步骤802中:基于获取到的第二前位姿与第二当前位姿,得到当前融合位姿与前融合位姿之间的第二相对变化量;
在本申请实施例中,可根据公式3来确定第一相对变化量:
其中,ΔTl为第二相对变化量,T1为第二前位资,T2为第二当前位资,为第二前位资对应的横坐标,/>为第二前位资对应的纵坐标,/>为第二前位资对应的在世界坐标的角度,/>为第二前位资对应的横坐标,/>为第二前位资对应的纵坐标,/>为第二前位资对应的在世界坐标的角度。
步骤803中:针对目标时刻与当前时刻中间的每个获取激光数据的时间戳,确定时间戳与目标时刻之间的第一对应关系,以及与当前时刻之间的第二对应关系;基于时间戳对应的第一位姿与第一当前位姿,得到第三对应关系;
其中,第一前位姿为目标时刻对应的第一当前位姿、第二前位姿为目标时刻对应的第二当前位姿、第三前位姿为目标时刻对应的第三当前位姿,前融合位姿为目标时刻对应的当前融合位姿。
例如:如图10所示,时序上的A点为目标时刻,B点为当前时刻,在A与B之间获取激光数据的时间戳有:时间戳1、时间戳2、时间戳3;则基于时间戳1、时间戳2、时间戳3与A、B时刻分别得到第一对应关系、第二对应关系以及第三对应关系。
在本申请实施例中,由于激光雷达采集激光数据的频率较快,在本申请中为了避免对计算资源的浪费,因此设定预设时间差,基于预设时间差确定参与计算的目标激光雷达,由此可见,并不是每次采集的激光数据后都会计算一次机器人的当前融合位资,在本申请中,为了充分利用激光数据,因此设定了第一对应关系和第二对应关系。在确定时间戳与目标时刻之间的第一对应关系,以及与当前时刻之间的第二对应关系时,具体可实施为如图9所示的步骤,其中:
步骤901中:确定时间戳在时序上前后相邻的第三中间位姿;第三中间位姿为采用里程计获取到的机器人的位姿;
继续以图10为例,A、B之间存在时间戳1、时间戳2、时间戳3,将时间戳1、时间戳2、时间戳3中的任意一个记为时间戳i,则针对时间戳i,该时间戳在时序上前后相邻的第三位资分别记为
步骤902中:基于前后相邻的第三中间位姿,得到时间戳对应的第三中间位姿;
例如:针对采用线性插值的方法,得到时间戳对应的第三中间位姿/>/>
步骤903中:基于目标时刻在时序上前后相邻的第三中间位姿,得到目标时刻对应的中间位姿;
例如:如图10所示,目标时刻在时序上相邻的两个时间戳分别为时间戳4和时间戳1,则基于时间戳4和时间戳1分别对应的第三中间位姿,并利用线性插值的方法得到目标时刻A对应的中间位姿,记为ON1。
步骤904中:基于当前时刻在时序上相邻的第三中间位姿,得到当前时刻对应的中间位姿;
例如:如图10所示,当前时刻在时序上相邻的两个时间戳分别为时间戳3和时间戳5,则基于时间戳3和时间戳5分别对应的第三中间位姿,并利用线性差值的方法得到当前时刻B对应的中间位姿,记为ON2。
步骤905中:基于时间戳对应的第三中间位姿与目标时刻对应的中间位姿,得到时间戳与目标时刻之间的第一对应关系;
在本申请中,可基于公式4,得到第一对应关系:
其中:为第一对应关系,/>为时间戳对应的第三中间位姿,ON1为目标时刻对应的中间位姿。
步骤906中:基于时间戳对应的第三中间位姿与当前时刻对应的中间位姿得到时间戳与当前时刻之间的第二对应关系。
在本申请中,可基于公式5,得到第二对应关系:
其中:为第一对应关系,/>为时间戳对应的第三中间位姿,ON2为当前时刻对应的中间位姿。
步骤804中:基于第一相对变化量、第二相对变化量、第一对应关系、第二对应关系、第三对应关系确定当前融合位姿的约束条件。
在本申请中,在基于第一相对变化量、第二相对变化量、第一对应关系、第二对应关系、第三对应关系确定当前融合位姿的约束条件时,具体可实施为如图11所示的步骤,其中:
步骤1101中:基于第一权重、第一当前位姿确定当前融合位姿的第一约束关系;
其中:ei1为第一约束关系,p1为第一权重,G2为第一当前位资,N2为当前融合位资,x为第一当前位资的横坐标,y为第一当前位资的纵坐标,θ为第一当前位资的角度。
其中,第一权重是根据第一当前位姿得到的,具体可采用如图12所示的步骤,其中:
步骤1201:基于所述第一当前位姿与所述先验地图,得到所述第一当前位姿对应的第一分值;并,获取所述机器人前融合位姿对应的第二分值;
步骤1202:若所述第一分值大于第一阈值或所述第二分值小于第二阈值,则所述第一权重为第一固定值;其中,第一阈值大于第二阈值;
步骤1203:若所述第一分值小于所述第一阈值,且所述第二分值大于所述第一阈值,则确定所述第一固定值与所述第一分值的积值,将所述积值与所述第二分值的比值作为所述第一权重。
例如:第一分值为sl,第二分值为st1,第一固定值为pG,则第一权重可表示为公式7:
步骤1102中:基于第二权重、第一相对变化量确定当前融合位姿的第二约束关系;其中,第二权重是预先设置的;
其中:ei2为第二约束关系,p2为第二权重,N1为本次计算得到的目标时刻对应的融合位资的计算值,N2为当前融合位资,θ为第一当前位资的角度,为,t为
步骤1103中:基于第三权重、第二相对变化量确定当前融合位姿的第三约束关系;其中,第三权重是预先设置的;
其中,ei3为第三约束关系,p3为第三权重,ΔTl为第二相对变化量,L1为第二前位资,L2为第二当前位资。
步骤1104中:基于第四权重、第一对应关系、第二对应关系、第三对应关系,确定当前融合位姿的第四约束关系;其中,第四权重是预先设置的。
其中,ei4为第四约束关系,p4为第四权重,为第一对应关系,/>为第二对应关系,/>为第三对应关系,N2为当前融合位姿,Li为时间戳i对应的第二当前位资,L1为第一前位资。
综上所述,在得到约束关系后,联立公式6、8、9、10即可得到当前融合位资。
在一些可能的实施例中,若存在多个目标激光雷达,则在得到机器人的当前融合位姿之后,还需实施如图13所示的步骤,其中:
步骤1301中:对每个目标激光雷达对应的当前融合位姿进行评分,得到每个目标激光雷达对应的分值;
步骤1302中:基于每个目标激光雷达对应的分值,得到每个激光雷达对应的权重;
步骤1303中:基于每个激光雷达对应的权重以及每个激光雷达对应的当前融合位姿,得到机器人对应的最终位姿。
例如:针对机器人1,确定该机器人1对应的目标激光雷达有:雷达1、雷达2、雷达3,且确定雷达1对应的得分为a,雷达2对应的得分为b,雷达3对应的得分为c,且雷达1对应的权重为p1,雷达2对应的权重为p2,雷达3对应的权重为p3,则机器人对应的最终位资为:p1*a/(a+b+c)+p2*b/(a+b+c)+p3*c/(a+b+c)。
5、确定目标激光雷达
在本申请实施例中,针对包括至少两个激光雷达的机器人,从该机器人中确定出目标激光雷达的方法如图14所示,其中:
步骤1401中:针对每个激光雷达,确定激光雷达本次获取激光数据的时间戳与目标时刻之间的时间差;
步骤1402中:若时间差大于预设时间差,则将激光雷达本次获取激光数据的时间戳作为当前时刻,将激光雷达作为目标激光雷达;
步骤1403中:若所有时间差均小于等于预设时间差,则返回执行确定激光雷达本次获取激光数据的时间戳与目标时刻之间的时间差。
例如:针对机器人1,该机器人1对应的激光雷达有:雷达1、雷达2、雷达3,确定预设时间差为0.2秒,确定激光雷达1本次获取激光数据的时间戳为12:01:12,确定激光雷达1对应的目标时刻为12:01:11,则确定激光雷达1不是目标激光雷达;确定激光雷达2本次获取激光数据的时间戳为12:01:13,确定激光雷达2对应的目标时刻为12:01:11,则确定激光雷达2是目标激光雷达;确定激光雷达3本次获取激光数据的时间戳为12:01:12,确定激光雷达3对应的目标时刻为12:01:10,则确定激光雷达3是目标激光雷达。
6、确定初始位资
在一些可能的实施例中,当机器人开机时或者在运动过程中因为某种原因定位丢失时,需要快速知道设备自己在当前环境中的位置,后续基于初始位置,进行位姿的递推。在本申请中机器人的初始位姿是根据如图15所示的方法确定的:
步骤1501中:以机器人中心机器人坐标系,确定目标激光雷达在机器人坐标系下的坐标信息;
步骤1502中:分别确定每个激光雷达获取到的激光数据在激光坐标系下的点云信息;
步骤1503中:将目标激光雷达对应的点云信息映射至机器人坐标系中;
步骤1504中:针对其他激光雷达对应的点云信息中的每个点,若点映射至机器人坐标系中的位置已存在点,则丢弃点;若点映射至机器人坐标系中的位置不存在点,则将该点映射至机器人坐标系中,得到机器人在机器人坐标系下的激光点云;
步骤1505中:将机器人在机器人坐标系下的激光点云与先验地图进行匹配,得到机器人的初始位姿。
例如:针对机器人1,该机器人1对应的激光雷达有:雷达1、雷达2、雷达3,目标激光雷达为雷达1,则将雷达1中点云中包括的点的坐标分别为(x1,y1),(x11,y11),(x12,y12);雷达2中的点云中包括的点的坐标分别为(x1,y1),(x2,y2),(x22,y22);雷达2中的点云中包括的点的坐标分别为(x3,y3),(x32,y32),(x31,y31),则将雷达1中的点均映射至机器人坐标系下,将雷达2中的点(x2,y2),(x22,y22)映射至机器人坐标系下,将雷达3中的点(x3,y3),(x32,y32),(x31,y31)映射至机器人坐标系下,得到的机器人在机器人坐标系下的激光点云包括(x1,y1),(x11,y11),(x12,y12),(x2,y2),(x22,y22),(x3,y3),(x32,y32),(x31,y31)。
综上,在本申请中在运动过程中,基于时间差从多个激光雷达中确定目标激光雷达,可以充分利用每个激光雷达获取到的激光数据,在确定第二当前位姿时基于局部子地图进行,确定第一当前位姿时基于目标激光雷达获取到的激光数据进行,保证了得到的当前融合位姿的准确性。
如图16所示,基于相同的发明构思,提出一种机器人定位装置1600,所述机器人包括里程计和至少两个激光雷达,所述装置包括:
目标激光雷达确定模块16001,用于基于所述至少两个激光雷达本次获取激光数据的时刻与所述至少两个激光雷达对应的目标时刻之间的时间差,从所述至少两个激光雷达中确定出目标激光雷达;其中所述目标时刻为最近一次得到的所述机器人的融合位姿的时刻;
位姿确定模块16002,用于采用所述目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿;以及获取所述目标激光雷达在所述当前时刻采集到的局部子地图,基于所述局部子地图得到所述第二当前位姿;以及采用里程计确定所述机器人当前时刻的第三当前位姿;
条件确定模块16003,用于基于所述第一当前位姿、所述第二当前位姿以及所述第三当前位姿,得到当前融合位姿的约束条件;
融合模块16004,用于基于所述当前融合位姿的约束条件得到所述机器人的当前融合位姿。
在一些可能的实施例中,所述位姿确定模块16002执行采用所述目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿时,具体用于:
根据所述当前时刻对应的激光数据确定目标环境中是否存在新增物体;
若确定所述目标环境中存在新增物体,则基于所述激光数据、先验地图以及当前时刻对应的局部子地图,得到所述机器人的第一当前位姿;其中,所述先验地图是基于所述目标环境采集到的地图;
若确定所述目标环境中不存在新增物体,则基于所述激光数据与所述先验地图,得到所述机器人的第一当前位姿。
在一些可能的实施例中,所述位姿确定模块16002执行根据所述当前时刻对应的激光数据确定目标环境中是否存在新增物体时,具体用于:
针对所述激光数据中的任意一束点云执行:确定所述点云中的每个点在所述机器人的机器人坐标系中的第一坐标信息;并获取里程计确定的所述机器人在世界坐标系中的角度;其中,所述机器人坐标系为以所述机器人的中心为坐标系原点建立的坐标系;
根据所述每个点的第一坐标信息以及所述角度,确定所述每个点在世界坐标系下的第二坐标信息;
基于所述点云中每个点的第二坐标信息与先验地图中栅格点云的坐标信息,确定所述点云与最近栅格点云之间的目标距离;其中,所述栅格点云为所述先验地图中的障碍物构成的点云;所述最近栅格点云为与所述点云之间距离最短的栅格点云;
若所述目标距离大于预设距离,则确定所述目标环境中存在新增物体;
若所述目标距离小于等于所述预设距离,则确定所述目标环境中不存在新增物体。
在一些可能的实施例中,所述位姿确定模块16002执行获取所述目标激光雷达在所述当前时刻采集到的局部子地图,基于所述局部子地图得到所述第二当前位姿时,具体用于:
获取在所述当前时刻之前得到融合位姿的时刻对应的全部或部分局部子地图,并获取所述当前时刻对应的局部子地图;
对所述在所述当前时刻之前计算得到融合位姿的时刻对应的全部或部分局部子地图、所述当前时刻对应的局部子地图进行拼接处理,得到当前帧地图;
基于所述当前帧地图,得到第二当前位姿。
在一些可能的实施例中,所述目标激光雷达确定模块16001执行从所述至少两个激光雷达中确定出目标激光雷达时,具体用于:
针对每个激光雷达,确定所述激光雷达本次获取激光数据的时间戳与所述目标时刻之间的时间差;
若所述时间差大于预设时间差,则将所述激光雷达本次获取激光数据的时间戳作为当前时刻,将所述激光雷达作为目标激光雷达;
若所有时间差均小于等于所述预设时间差,则返回执行确定所述激光雷达本次获取激光数据的时间戳与所述目标时刻之间的时间差。
在一些可能的实施例中,所述条件确定模块16003执行基于所述第一当前位姿、所述第二当前位姿以及所述第三当前位姿,得到当前融合位姿的约束条件时,具体用于:
基于获取到的第三前位姿与所述第三当前位姿,得到所述当前融合位姿与前融合位姿之间的第一相对变化量;
基于获取到的第二前位姿与所述第二当前位姿,得到所述当前融合位姿与所述前融合位姿之间的第二相对变化量;
针对所述目标时刻与所述当前时刻中间的每个获取激光数据的时间戳,确定所述时间戳与所述目标时刻之间的第一对应关系,以及与所述当前时刻之间的第二对应关系;基于所述时间戳对应的第一位姿与所述第一当前位姿,得到第三对应关系;
其中,所述第一前位姿为所述目标时刻对应的第一当前位姿、所述第二前位姿为所述目标时刻对应的第二当前位姿、所述第三前位姿为所述目标时刻对应的第三当前位姿,所述前融合位姿为所述目标时刻对应的当前融合位姿;
基于所述第一相对变化量、第二相对变化量、所述第一对应关系、所述第二对应关系、所述第三对应关系确定所述当前融合位姿的约束条件。
在一些可能的实施例中,所述条件确定模块16003执行确定所述时间戳与所述目标时刻之间的第一对应关系,以及与所述当前时刻之间的第二对应关系时,具体用于:
确定所述时间戳在时序上前后相邻的第三中间位姿;所述第三中间位姿为采用里程计获取到的所述机器人的位姿;
基于所述前后相邻的第三中间位姿,得到所述时间戳对应的第三中间位姿;
基于目标时刻在时序上前后相邻的第三中间位姿,得到所述目标时刻对应的中间位姿;
基于当前时刻在时序上相邻的第三中间位姿,得到所述当前时刻对应的中间位姿;
基于所述时间戳对应的第三中间位姿与所述目标时刻对应的中间位姿,得到所述时间戳与所述目标时刻之间的第一对应关系;
基于所述时间戳对应的第三中间位姿与所述当前时刻对应的中间位姿得到所述时间戳与所述当前时刻之间的第二对应关系。
在一些可能的实施例中,所述条件确定模块16003执行基于所述第一相对变化量、第二相对变化量、所述第一对应关系、所述第二对应关系、所述第三对应关系确定所述当前融合位姿的约束条件时,具体用于:
基于第一权重、第一当前位姿确定所述当前融合位姿的第一约束关系;其中,所述第一权重是根据第一当前位姿得到的;
基于第二权重、第一相对变化量确定所述当前融合位姿的第二约束关系;其中,所述第二权重是预先设置的;
基于第三权重、第二相对变化量确定所述当前融合位姿的第三约束关系;其中,所述第三权重是预先设置的;
基于第四权重、所述第一对应关系、所述第二对应关系、所述第三对应关系,确定所述当前融合位姿的第四约束关系;其中,所述第四权重是预先设置的。
在一些可能的实施例中,所述第一权重是根据以下方法确定的:
基于所述第一当前位姿与所述先验地图,得到所述第一当前位姿对应的第一分值;并,获取所述机器人前融合位姿对应的第二分值;
基于所述第一分值、第二分值确定所述第一位姿对应的权重。
在一些可能的实施例中,所述条件确定模块执行基于所述第一分值、第二分值确定所述第一位姿对应的权重时,具体用于:
若所述第一分值大于第一阈值或所述第二分值小于第二阈值,则所述第一权重为第一固定值;其中,第一阈值大于第二阈值;
若所述第一分值小于所述第一阈值,且所述第二分值大于所述第一阈值,则确定所述第一固定值与所述第一分值的积值,将所述积值与所述第二分值的比值作为所述第一权重。
在一些可能的实施例中,若存在多个目标激光雷达,则所述融合模块16004执行基于所述前融合位姿与当前融合位姿之间的关系得到所述机器人的当前融合位姿之后,还用于:
对每个目标激光雷达对应的当前融合位姿进行评分,得到每个目标激光雷达对应的分值;
基于每个目标激光雷达对应的分值,得到每个激光雷达对应的权重;
基于每个激光雷达对应的权重以及每个激光雷达对应的当前融合位姿,得到所述机器人对应的最终位姿。
在一些可能的实施例中,若所述目标激光雷达的目标时刻为初始时刻,则所述初始时刻对应的初始位姿是根据以下方法确定的:
以所述机器人中心机器人坐标系,确定所述目标激光雷达在所述机器人坐标系下的坐标信息;
分别确定每个激光雷达获取到的激光数据在激光坐标系下的点云信息;
将所述目标激光雷达对应的点云信息映射至所述机器人坐标系中;
针对其他激光雷达对应的点云信息中的每个点,若所述点映射至所述机器人坐标系中的位置已存在点,则丢弃所述点;若所述点映射至所述机器人坐标系中的位置不存在点,则将该点映射至所述机器人坐标系中,得到所述机器人在机器人坐标系下的激光点云;
将所述机器人在机器人坐标系下的激光点云与先验地图进行匹配,得到所述机器人的初始位姿。
在介绍了本申请示例性实施方式的机器人定位方法和装置之后,接下来,介绍根据本申请的另一示例性实施方式的电子设备。
所属技术领域的技术人员能够理解,本申请的各个方面可以实现为系统、方法或程序产品。因此,本申请的各个方面可以具体实现为以下形式,即:完全的硬件实施方式、完全的软件实施方式(包括固件、微代码等),或硬件和软件方面结合的实施方式,这里可以统称为“电路”、“模块”或“系统”。
在一些可能的实施方式中,根据本申请的电子设备可以至少包括至少一个处理器、以及至少一个存储器。其中,存储器存储有程序代码,当程序代码被处理器执行时,使得处理器执行本说明书上述描述的根据本申请各种示例性实施方式的机器人定位方法中的步骤。
下面参照图17来描述根据本申请的这种实施方式的电子设备130。图17显示的电子设备130仅仅是一个示例,不应对本申请实施例的功能和使用范围带来任何限制。
如图17所示,电子设备130以通用电子设备的形式表现。电子设备130的组件可以包括但不限于:上述至少一个处理器131、上述至少一个存储器132、连接不同系统组件(包括存储器132和处理器131)的总线133。
总线133表示几类总线结构中的一种或多种,包括存储器总线或者存储器控制器、外围总线、处理器或者使用多种总线结构中的任意总线结构的局域总线。
存储器132可以包括易失性存储器形式的可读介质,例如随机存取存储器(RAM)1321和/或高速缓存存储器1322,还可以进一步包括只读存储器(ROM)1323。
存储器132还可以包括具有一组(至少一个)程序模块1324的程序/实用工具1325,这样的程序模块1324包括但不限于:操作系统、一个或者多个应用程序、其它程序模块以及程序数据,这些示例中的每一个或某种组合中可能包括网络环境的实现。
电子设备130也可以与一个或多个外部设备134(例如键盘、指向设备等)通信,还可与一个或者多个使得用户能与电子设备130交互的设备通信,和/或与使得该电子设备130能与一个或多个其它电子设备进行通信的任何设备(例如路由器、调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口135进行。并且,电子设备130还可以通过网络适配器136与一个或者多个网络(例如局域网(LAN),广域网(WAN)和/或公共网络,例如因特网)通信。如图所示,网络适配器136通过总线133与用于电子设备130的其它模块通信。应当理解,尽管图17中未示出,可以结合电子设备130使用其它硬件和/或软件模块,包括但不限于:微代码、设备驱动器、冗余处理器、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。
在一些可能的实施方式中,本申请提供的一种机器人定位方法的各个方面还可以实现为一种程序产品的形式,其包括程序代码,当程序产品在计算机设备上运行时,程序代码用于使计算机设备执行本说明书上述描述的根据本申请各种示例性实施方式的一种机器人定位方法中的步骤。
程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件,或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
本申请的实施方式的用于机器人定位的程序产品可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码,并可以在电子设备上运行。然而,本申请的程序产品不限于此,在本文件中,可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了可读程序代码。这种传播的数据信号可以采用多种形式,包括——但不限于——电磁信号、光信号或上述的任意合适的组合。可读信号介质还可以是可读存储介质以外的任何可读介质,该可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。
可读介质上包含的程序代码可以用任何适当的介质传输,包括——但不限于——无线、有线、光缆、RF等等,或者上述的任意合适的组合。
可以以一种或多种程序设计语言的任意组合来编写用于执行本申请操作的程序代码,程序设计语言包括面向对象的程序设计语言—诸如Java、C++等,还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户电子设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户电子设备上部分在远程电子设备上执行、或者完全在远程电子设备或服务端上执行。在涉及远程电子设备的情形中,远程电子设备可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户电子设备,或者,可以连接到外部电子设备(例如利用因特网服务提供商来通过因特网连接)。
应当注意,尽管在上文详细描述中提及了装置的若干单元或子单元,但是这种划分仅仅是示例性的并非强制性的。实际上,根据本申请的实施方式,上文描述的两个或更多单元的特征和功能可以在一个单元中具体化。反之,上文描述的一个单元的特征和功能可以进一步划分为由多个单元来具体化。
此外,尽管在附图中以特定顺序描述了本申请方法的操作,但是,这并非要求或者暗示必须按照该特定顺序来执行这些操作,或是必须执行全部所示的操作才能实现期望的结果。附加地或备选地,可以省略某些步骤,将多个步骤合并为一个步骤执行,和/或将一个步骤分解为多个步骤执行。
本领域内的技术人员应明白,本申请的实施例可提供为方法、系统、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然,本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样,倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内,则本申请也意图包含这些改动和变型在内。
Claims (26)
1.一种机器人定位方法,其特征在于,所述机器人包括里程计和至少两个激光雷达,所述方法包括:
基于所述至少两个激光雷达本次获取激光数据的时刻与所述至少两个激光雷达对应的目标时刻之间的时间差,从所述至少两个激光雷达中确定出目标激光雷达;其中所述目标时刻为最近一次得到的所述机器人的融合位姿的时刻;
采用所述目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿;以及获取所述目标激光雷达在所述当前时刻采集到的局部子地图,基于所述局部子地图得到所述第二当前位姿;以及采用里程计确定所述机器人当前时刻的第三当前位姿;
基于所述第一当前位姿、所述第二当前位姿以及所述第三当前位姿,得到当前融合位姿的约束条件;
基于所述当前融合位姿的约束条件得到所述机器人的当前融合位姿。
2.根据权利要求1所述的方法,其特征在于,所述采用所述目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿,包括:
根据所述当前时刻对应的激光数据确定目标环境中是否存在新增物体;
若确定所述目标环境中存在新增物体,则基于所述激光数据、先验地图以及当前时刻对应的局部子地图,得到所述机器人的第一当前位姿;其中,所述先验地图是基于所述目标环境采集到的地图;
若确定所述目标环境中不存在新增物体,则基于所述激光数据与所述先验地图,得到所述机器人的第一当前位姿。
3.根据权利要求2所述的方法,其特征在于,所述根据所述当前时刻对应的激光数据确定目标环境中是否存在新增物体,包括:
针对所述激光数据中的任意一束点云执行:确定所述点云中的每个点在所述机器人的机器人坐标系中的第一坐标信息;并获取里程计确定的所述机器人在世界坐标系中的角度;其中,所述机器人坐标系为以所述机器人的中心为坐标系原点建立的坐标系;
根据所述每个点的第一坐标信息以及所述角度,确定所述每个点在世界坐标系下的第二坐标信息;
基于所述点云中每个点的第二坐标信息与先验地图中栅格点云的坐标信息,确定所述点云与最近栅格点云之间的目标距离;其中,所述栅格点云为所述先验地图中的障碍物构成的点云;所述最近栅格点云为与所述点云之间距离最短的栅格点云;
若所述目标距离大于预设距离,则确定所述目标环境中存在新增物体;
若所述目标距离小于等于所述预设距离,则确定所述目标环境中不存在新增物体。
4.根据权利要求1所述的方法,其特征在于,所述获取所述目标激光雷达在所述当前时刻采集到的局部子地图,基于所述局部子地图得到所述第二当前位姿,包括:
获取在所述当前时刻之前得到融合位姿的时刻对应的全部或部分局部子地图,并获取所述当前时刻对应的局部子地图;
对所述在所述当前时刻之前计算得到融合位姿的时刻对应的全部或部分局部子地图、所述当前时刻对应的局部子地图进行拼接处理,得到当前帧地图;
基于所述当前帧地图,得到第二当前位姿。
5.根据权利要求1所述的方法,其特征在于,所述从所述至少两个激光雷达中确定出目标激光雷达,包括:
针对每个激光雷达,确定所述激光雷达本次获取激光数据的时间戳与所述目标时刻之间的时间差;
若所述时间差大于预设时间差,则将所述激光雷达本次获取激光数据的时间戳作为当前时刻,将所述激光雷达作为目标激光雷达;
若所有时间差均小于等于所述预设时间差,则返回执行确定所述激光雷达本次获取激光数据的时间戳与所述目标时刻之间的时间差。
6.根据权利要求1所述的方法,其特征在于,所述基于所述第一当前位姿、所述第二当前位姿以及所述第三当前位姿,得到当前融合位姿的约束条件,包括:
基于获取到的第三前位姿与所述第三当前位姿,得到所述当前融合位姿与前融合位姿之间的第一相对变化量;
基于获取到的第二前位姿与所述第二当前位姿,得到所述当前融合位姿与所述前融合位姿之间的第二相对变化量;
针对所述目标时刻与所述当前时刻中间的每个获取激光数据的时间戳,确定所述时间戳与所述目标时刻之间的第一对应关系,以及与所述当前时刻之间的第二对应关系;基于所述时间戳对应的第一位姿与所述第一当前位姿,得到第三对应关系;
其中,所述第一前位姿为所述目标时刻对应的第一当前位姿、所述第二前位姿为所述目标时刻对应的第二当前位姿、所述第三前位姿为所述目标时刻对应的第三当前位姿,所述前融合位姿为所述目标时刻对应的当前融合位姿;
基于所述第一相对变化量、第二相对变化量、所述第一对应关系、所述第二对应关系、所述第三对应关系确定所述当前融合位姿的约束条件。
7.根据权利要求6所述的方法,其特征在于,所述确定所述时间戳与所述目标时刻之间的第一对应关系,以及与所述当前时刻之间的第二对应关系,包括:
确定所述时间戳在时序上前后相邻的第三中间位姿;所述第三中间位姿为采用里程计获取到的所述机器人的位姿;
基于所述前后相邻的第三中间位姿,得到所述时间戳对应的第三中间位姿;
基于目标时刻在时序上前后相邻的第三中间位姿,得到所述目标时刻对应的中间位姿;
基于当前时刻在时序上相邻的第三中间位姿,得到所述当前时刻对应的中间位姿;
基于所述时间戳对应的第三中间位姿与所述目标时刻对应的中间位姿,得到所述时间戳与所述目标时刻之间的第一对应关系;
基于所述时间戳对应的第三中间位姿与所述当前时刻对应的中间位姿得到所述时间戳与所述当前时刻之间的第二对应关系。
8.根据权利要求6所述的方法,其特征在于,所述基于所述第一相对变化量、第二相对变化量、所述第一对应关系、所述第二对应关系、所述第三对应关系确定所述当前融合位姿的约束条件,包括:
基于第一权重、第一当前位姿确定所述当前融合位姿的第一约束关系;其中,所述第一权重是根据第一当前位姿得到的;
基于第二权重、第一相对变化量确定所述当前融合位姿的第二约束关系;其中,所述第二权重是预先设置的;
基于第三权重、第二相对变化量确定所述当前融合位姿的第三约束关系;其中,所述第三权重是预先设置的;
基于第四权重、所述第一对应关系、所述第二对应关系、所述第三对应关系,确定所述当前融合位姿的第四约束关系;其中,所述第四权重是预先设置的。
9.根据权利要求8所述的方法,其特征在于,所述第一权重是根据以下方法确定的:
基于所述第一当前位姿与所述先验地图,得到所述第一当前位姿对应的第一分值;并,获取所述机器人前融合位姿对应的第二分值;
基于所述第一分值、第二分值确定所述第一位姿对应的权重。
10.根据权利要求9所述的方法,其特征在于,所述基于所述第一分值、第二分值确定所述第一位姿对应的权重,包括:
若所述第一分值大于第一阈值或所述第二分值小于第二阈值,则所述第一权重为第一固定值;其中,第一阈值大于第二阈值;
若所述第一分值小于所述第一阈值,且所述第二分值大于所述第一阈值,则确定所述第一固定值与所述第一分值的积值,将所述积值与所述第二分值的比值作为所述第一权重。
11.根据权利要求1~10任一所述的方法,其特征在于,若存在多个目标激光雷达,则所述基于所述前融合位姿与当前融合位姿之间的关系得到所述机器人的当前融合位姿之后,所述方法还包括:
对每个目标激光雷达对应的当前融合位姿进行评分,得到每个目标激光雷达对应的分值;
基于每个目标激光雷达对应的分值,得到每个激光雷达对应的权重;
基于每个激光雷达对应的权重以及每个激光雷达对应的当前融合位姿,得到所述机器人对应的最终位姿。
12.根据权利要求1~10任一所述的方法,其特征在于,若所述目标激光雷达的目标时刻为初始时刻,则所述初始时刻对应的初始位姿是根据以下方法确定的:
以所述机器人中心机器人坐标系,确定所述目标激光雷达在所述机器人坐标系下的坐标信息;
分别确定每个激光雷达获取到的激光数据在激光坐标系下的点云信息;
将所述目标激光雷达对应的点云信息映射至所述机器人坐标系中;
针对其他激光雷达对应的点云信息中的每个点,若所述点映射至所述机器人坐标系中的位置已存在点,则丢弃所述点;若所述点映射至所述机器人坐标系中的位置不存在点,则将该点映射至所述机器人坐标系中,得到所述机器人在机器人坐标系下的激光点云;
将所述机器人在机器人坐标系下的激光点云与先验地图进行匹配,得到所述机器人的初始位姿。
13.一种机器人定位装置,其特征在于,所述机器人包括里程计和至少两个激光雷达,所述装置包括:
目标激光雷达确定模块,用于基于所述至少两个激光雷达本次获取激光数据的时刻与所述至少两个激光雷达对应的目标时刻之间的时间差,从所述至少两个激光雷达中确定出目标激光雷达;其中所述目标时刻为最近一次得到的所述机器人的融合位姿的时刻;
位姿确定模块,用于采用所述目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿;以及获取所述目标激光雷达在所述当前时刻采集到的局部子地图,基于所述局部子地图得到所述第二当前位姿;以及采用里程计确定所述机器人当前时刻的第三当前位姿;
条件确定模块,用于基于所述第一当前位姿、所述第二当前位姿以及所述第三当前位姿,得到当前融合位姿的约束条件;
融合模块,用于基于所述当前融合位姿的约束条件得到所述机器人的当前融合位姿。
14.根据权利要求13所述的装置,其特征在于,所述位姿确定模块执行采用所述目标激光雷达获取到的激光数据确定当前时刻对应的第一当前位姿时,具体用于:
根据所述当前时刻对应的激光数据确定目标环境中是否存在新增物体;
若确定所述目标环境中存在新增物体,则基于所述激光数据、先验地图以及当前时刻对应的局部子地图,得到所述机器人的第一当前位姿;其中,所述先验地图是基于所述目标环境采集到的地图;
若确定所述目标环境中不存在新增物体,则基于所述激光数据与所述先验地图,得到所述机器人的第一当前位姿。
15.根据权利要求14所述的装置,其特征在于,所述位姿确定模块执行根据所述当前时刻对应的激光数据确定目标环境中是否存在新增物体时,具体用于:
针对所述激光数据中的任意一束点云执行:确定所述点云中的每个点在所述机器人的机器人坐标系中的第一坐标信息;并获取里程计确定的所述机器人在世界坐标系中的角度;其中,所述机器人坐标系为以所述机器人的中心为坐标系原点建立的坐标系;
根据所述每个点的第一坐标信息以及所述角度,确定所述每个点在世界坐标系下的第二坐标信息;
基于所述点云中每个点的第二坐标信息与先验地图中栅格点云的坐标信息,确定所述点云与最近栅格点云之间的目标距离;其中,所述栅格点云为所述先验地图中的障碍物构成的点云;所述最近栅格点云为与所述点云之间距离最短的栅格点云;
若所述目标距离大于预设距离,则确定所述目标环境中存在新增物体;
若所述目标距离小于等于所述预设距离,则确定所述目标环境中不存在新增物体。
16.根据权利要求13所述的装置,其特征在于,所述位姿确定模块执行获取所述目标激光雷达在所述当前时刻采集到的局部子地图,基于所述局部子地图得到所述第二当前位姿时,具体用于:
获取在所述当前时刻之前得到融合位姿的时刻对应的全部或部分局部子地图,并获取所述当前时刻对应的局部子地图;
对所述在所述当前时刻之前计算得到融合位姿的时刻对应的全部或部分局部子地图、所述当前时刻对应的局部子地图进行拼接处理,得到当前帧地图;
基于所述当前帧地图,得到第二当前位姿。
17.根据权利要求13所述的装置,其特征在于,所述目标激光雷达确定模块执行从所述至少两个激光雷达中确定出目标激光雷达时,具体用于:
针对每个激光雷达,确定所述激光雷达本次获取激光数据的时间戳与所述目标时刻之间的时间差;
若所述时间差大于预设时间差,则将所述激光雷达本次获取激光数据的时间戳作为当前时刻,将所述激光雷达作为目标激光雷达;
若所有时间差均小于等于所述预设时间差,则返回执行确定所述激光雷达本次获取激光数据的时间戳与所述目标时刻之间的时间差。
18.根据权利要求13所述的装置,其特征在于,所述条件确定模块执行基于所述第一当前位姿、所述第二当前位姿以及所述第三当前位姿,得到当前融合位姿的约束条件时,具体用于:
基于获取到的第三前位姿与所述第三当前位姿,得到所述当前融合位姿与前融合位姿之间的第一相对变化量;
基于获取到的第二前位姿与所述第二当前位姿,得到所述当前融合位姿与所述前融合位姿之间的第二相对变化量;
针对所述目标时刻与所述当前时刻中间的每个获取激光数据的时间戳,确定所述时间戳与所述目标时刻之间的第一对应关系,以及与所述当前时刻之间的第二对应关系;基于所述时间戳对应的第一位姿与所述第一当前位姿,得到第三对应关系;
其中,所述第一前位姿为所述目标时刻对应的第一当前位姿、所述第二前位姿为所述目标时刻对应的第二当前位姿、所述第三前位姿为所述目标时刻对应的第三当前位姿,所述前融合位姿为所述目标时刻对应的当前融合位姿;
基于所述第一相对变化量、第二相对变化量、所述第一对应关系、所述第二对应关系、所述第三对应关系确定所述当前融合位姿的约束条件。
19.根据权利要求18所述的装置,其特征在于,所述条件确定模块执行确定所述时间戳与所述目标时刻之间的第一对应关系,以及与所述当前时刻之间的第二对应关系时,具体用于:
确定所述时间戳在时序上前后相邻的第三中间位姿;所述第三中间位姿为采用里程计获取到的所述机器人的位姿;
基于所述前后相邻的第三中间位姿,得到所述时间戳对应的第三中间位姿;
基于目标时刻在时序上前后相邻的第三中间位姿,得到所述目标时刻对应的中间位姿;
基于当前时刻在时序上相邻的第三中间位姿,得到所述当前时刻对应的中间位姿;
基于所述时间戳对应的第三中间位姿与所述目标时刻对应的中间位姿,得到所述时间戳与所述目标时刻之间的第一对应关系;
基于所述时间戳对应的第三中间位姿与所述当前时刻对应的中间位姿得到所述时间戳与所述当前时刻之间的第二对应关系。
20.根据权利要求18所述的装置,其特征在于,所述条件确定模块执行基于所述第一相对变化量、第二相对变化量、所述第一对应关系、所述第二对应关系、所述第三对应关系确定所述当前融合位姿的约束条件时,具体用于:
基于第一权重、第一当前位姿确定所述当前融合位姿的第一约束关系;其中,所述第一权重是根据第一当前位姿得到的;
基于第二权重、第一相对变化量确定所述当前融合位姿的第二约束关系;其中,所述第二权重是预先设置的;
基于第三权重、第二相对变化量确定所述当前融合位姿的第三约束关系;其中,所述第三权重是预先设置的;
基于第四权重、所述第一对应关系、所述第二对应关系、所述第三对应关系,确定所述当前融合位姿的第四约束关系;其中,所述第四权重是预先设置的。
21.根据权利要求20所述的装置,其特征在于,所述第一权重是根据以下方法确定的:
基于所述第一当前位姿与所述先验地图,得到所述第一当前位姿对应的第一分值;并,获取所述机器人前融合位姿对应的第二分值;
基于所述第一分值、第二分值确定所述第一位姿对应的权重。
22.根据权利要求21所述的装置,其特征在于,所述条件确定模块执行基于所述第一分值、第二分值确定所述第一位姿对应的权重时,具体用于:
若所述第一分值大于第一阈值或所述第二分值小于第二阈值,则所述第一权重为第一固定值;其中,第一阈值大于第二阈值;
若所述第一分值小于所述第一阈值,且所述第二分值大于所述第一阈值,则确定所述第一固定值与所述第一分值的积值,将所述积值与所述第二分值的比值作为所述第一权重。
23.根据权利要求13~22任一所述的装置,其特征在于,若存在多个目标激光雷达,则所述融合模块执行基于所述前融合位姿与当前融合位姿之间的关系得到所述机器人的当前融合位姿之后,还用于:
对每个目标激光雷达对应的当前融合位姿进行评分,得到每个目标激光雷达对应的分值;
基于每个目标激光雷达对应的分值,得到每个激光雷达对应的权重;
基于每个激光雷达对应的权重以及每个激光雷达对应的当前融合位姿,得到所述机器人对应的最终位姿。
24.根据权利要求13~22任一所述的装置,其特征在于,若所述目标激光雷达的目标时刻为初始时刻,则所述初始时刻对应的初始位姿是根据以下方法确定的:
以所述机器人中心机器人坐标系,确定所述目标激光雷达在所述机器人坐标系下的坐标信息;
分别确定每个激光雷达获取到的激光数据在激光坐标系下的点云信息;
将所述目标激光雷达对应的点云信息映射至所述机器人坐标系中;
针对其他激光雷达对应的点云信息中的每个点,若所述点映射至所述机器人坐标系中的位置已存在点,则丢弃所述点;若所述点映射至所述机器人坐标系中的位置不存在点,则将该点映射至所述机器人坐标系中,得到所述机器人在机器人坐标系下的激光点云;
将所述机器人在机器人坐标系下的激光点云与先验地图进行匹配,得到所述机器人的初始位姿。
25.一种电子设备,其特征在于,包括至少一个处理器;以及与所述至少一个处理器通信连接的存储器;其中,所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以实现如权利要求1~12中任一项所述的方法。
26.一种计算机存储介质,其特征在于,所述计算机存储介质存储有计算机程序,所述计算机程序用于使计算机能够执行如权利要求1~12中任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310576360.5A CN116626700A (zh) | 2023-05-19 | 2023-05-19 | 一种机器人定位方法、装置、电子设备和存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310576360.5A CN116626700A (zh) | 2023-05-19 | 2023-05-19 | 一种机器人定位方法、装置、电子设备和存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116626700A true CN116626700A (zh) | 2023-08-22 |
Family
ID=87609300
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310576360.5A Pending CN116626700A (zh) | 2023-05-19 | 2023-05-19 | 一种机器人定位方法、装置、电子设备和存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116626700A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117315175A (zh) * | 2023-09-28 | 2023-12-29 | 广东拓普视科技有限公司 | 一种基于机器人的构图定位装置及其方法 |
-
2023
- 2023-05-19 CN CN202310576360.5A patent/CN116626700A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117315175A (zh) * | 2023-09-28 | 2023-12-29 | 广东拓普视科技有限公司 | 一种基于机器人的构图定位装置及其方法 |
CN117315175B (zh) * | 2023-09-28 | 2024-05-14 | 广东拓普视科技有限公司 | 一种基于机器人的构图定位装置及其方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
EP3505869B1 (en) | Method, apparatus, and computer readable storage medium for updating electronic map | |
EP3798974A1 (en) | Method and apparatus for detecting ground point cloud points | |
CN109540142B (zh) | 一种机器人定位导航的方法、装置、计算设备 | |
JP6952165B2 (ja) | 障害物検出方法および装置 | |
US10659925B2 (en) | Positioning method, terminal and server | |
US20180306922A1 (en) | Method and apparatus for positioning vehicle | |
CN111192331B (zh) | 一种激光雷达和相机的外参标定方法及装置 | |
CN109407073B (zh) | 反射值地图构建方法和装置 | |
CN111209978B (zh) | 三维视觉重定位方法、装置及计算设备、存储介质 | |
CN111596298B (zh) | 目标对象的定位方法、装置、设备及存储介质 | |
EP3505868A1 (en) | Method and apparatus for adjusting point cloud data acquisition trajectory, and computer readable medium | |
WO2019126950A1 (zh) | 一种定位方法、云端服务器、终端、系统、电子设备及计算机程序产品 | |
CN116626700A (zh) | 一种机器人定位方法、装置、电子设备和存储介质 | |
CN114140759A (zh) | 高精地图车道线位置确定方法、装置及自动驾驶车辆 | |
CN113920217A (zh) | 用于生成高精地图车道线的方法、装置、设备和产品 | |
CN113093128A (zh) | 用于标定毫米波雷达的方法、装置、电子设备及路侧设备 | |
CN113177980B (zh) | 用于自动驾驶的目标对象速度确定方法、装置及电子设备 | |
CN113050660B (zh) | 误差补偿方法、装置、计算机设备及存储介质 | |
CN112652062A (zh) | 一种点云地图构建方法、装置、设备和存储介质 | |
CN113034603B (zh) | 用于确定标定参数的方法和装置 | |
CN116974291A (zh) | 主从协同导航农业机械的控制误差确定方法及装置 | |
CN110853098A (zh) | 机器人定位方法、装置、设备及存储介质 | |
CN115147561A (zh) | 位姿图生成方法、高精地图生成方法和装置 | |
CN112988932A (zh) | 高精地图标注方法、装置、设备、可读存储介质及产品 | |
CN112414444A (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 |