CN118049985A - 基于激光点云的关键帧判定方法及地图更新方法 - Google Patents
基于激光点云的关键帧判定方法及地图更新方法 Download PDFInfo
- Publication number
- CN118049985A CN118049985A CN202211472949.2A CN202211472949A CN118049985A CN 118049985 A CN118049985 A CN 118049985A CN 202211472949 A CN202211472949 A CN 202211472949A CN 118049985 A CN118049985 A CN 118049985A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- grid
- map
- frame
- current
- 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 78
- 238000013507 mapping Methods 0.000 claims description 45
- 230000008569 process Effects 0.000 claims description 19
- 230000001131 transforming effect Effects 0.000 claims description 4
- 230000008859 change Effects 0.000 description 27
- 230000009466 transformation Effects 0.000 description 16
- 238000006243 chemical reaction Methods 0.000 description 10
- 238000013519 translation Methods 0.000 description 10
- 230000007613 environmental effect Effects 0.000 description 9
- 230000000694 effects Effects 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- 238000004590 computer program Methods 0.000 description 5
- 238000001514 detection method Methods 0.000 description 5
- 238000010408 sweeping Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000005070 sampling Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 239000000523 sample Substances 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000001502 supplementing effect Effects 0.000 description 1
Landscapes
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明公开基于激光点云的关键帧判定方法及地图更新方法,所述关键帧判定方法包括每当采集到一个当前点云帧,则判断当前点云帧匹配到的栅格与预设设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值,是则将当前点云帧判定为关键帧,以更新所述预先设置的关键帧匹配到的栅格对应的索引信息。所述地图更新方法包括每当采集到一个当前点云帧,则判断当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值,是则将当前点云帧更新到地图,否则不将当前点云帧更新到地图中。
Description
技术领域
本发明涉及激光点云数据转换的技术领域,特别是基于激光点云的关键帧判定方法及地图更新方法。
背景技术
SLAM(simultaneous localization and mapping)即时定位与地图构建,顾名思义,是机器人或其他设备在自身位置不确定的条件下,在完全未知环境中创建地图(Mapping),同时利用地图进行自主定位和导航(Localization),实现路径规划(Navigation)的技术。SLAM技术主要用于移动机器人的实时位姿计算上,该技术可一边采集周围环境信息并根据环境信息建图,同时获得当前采集点在地图上的坐标和方向角。SLAM问题可以描述为:机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位,同时在自身定位的基础上建造增量式地图,实现机器人的自主定位和导航。SLAM技术的核心步骤大体上而言,SLAM包含了:感知、定位、建图这三个过程。感知是指机器人能够通过传感器获取周围的环境信息。定位是指通过传感器获取的当前和历史信息,推测出自身的位置和姿态。建图是指根据自身的位姿以及传感器获取的信息,描绘出自身所处环境的样貌。
现有的激光SLAM导航方法中,一般会直接基于机器人的单方向上移动的距离和同一时针方向上旋转的角度来对采集到的点云数据进行过滤,则机器人行走在宽度较窄且容易发生碰撞的窄道区域的过程中,机器人在狭窄的通道内碰撞中前进,则机器人前进的方向会频繁变化,而机器人采集到的点云数据在单一方向上产生的距离变化和角度变化都不大,所以机器人在穿行窄道区域的过程中没有将实时采集到的点云数据更新到地图中;直至机器人从窄道口离开窄道区域并进入更为宽阔的空间区域后,机器人采集的点云数据相对于其在窄道区域内采集的在单一方向上产生较大的距离变化或角度变化,此时机器人才将点云数据更新到地图中,然而机器人所处的环境特征与其在窄道区域内的存在很大区别,因此,更新到地图中的点云数据反映的位姿信息的连续性较差,表征的环境特征不完整,造成前后采集的点云帧之间的联系较弱,导致落入地图中的点云所反映的环境信息精度降低,可用性不足,影响机器人的定位精准度。
发明内容
本申请公开基于激光点云的关键帧判定方法及地图更新方法,具体的技术方案如下:
基于激光点云的关键帧判定方法,所述关键帧判定方法包括:每当采集到一个当前点云帧,则判断当前点云帧匹配到的栅格与预设设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值,是则将当前点云帧判定为关键帧,以更新所述预先设置的关键帧匹配到的栅格对应的索引信息。
进一步地,所述判断当前点云帧匹配到的栅格与预设设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值的方法包括:步骤S1、控制当前点云帧与地图作匹配,获取当前栅格索引集合,再将当前栅格索引集合内的元素数量标记为当前栅格索引数量,然后执行步骤S2;当前栅格索引集合,用于表示与当前点云帧存在映射关系的栅格对应的索引信息组成的集合;栅格是设置在地图中;步骤S2、在当前栅格索引集合内,统计与预先设置的参考栅格索引集合内的元素不相同的元素的数量,再将当前统计的数量标记为差异数量;然后执行步骤S3;其中,所述预先设置的参考栅格索引集合内的元素的数量是参考栅格索引数量;步骤S3、若差异数量与参考栅格索引数量的比值大于预设差异阈值,则将当前点云帧判定为关键帧,其中,差异数量与参考栅格索引数量的比值用于表示所述当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异。
进一步地,在第一次执行步骤S1之前,还包括:将采集到的第一个点云帧设置为所述预设设置的关键帧,并将采集到的第一个点云帧加入地图中以使对应栅格被投射入第一个点云帧;然后将与第一个点云帧存在映射关系的栅格对应的索引信息所组成的集合标记为所述预先设置的参考栅格索引集合,并将与第一个点云帧存在映射关系的栅格的数量标记为所述参考栅格索引数量;其中,所述当前点云帧不是第一个点云帧。
进一步地,在执行步骤S3之后,将当前栅格索引集合更新为所述预先设置的参考栅格索引集合,并将当前栅格索引数量更新为所述参考栅格索引数量,则确定对所述预先设置的关键帧匹配到的栅格对应的索引信息完成更新;然后执行步骤S1。
进一步地,步骤S3还包括:若差异数量与参考栅格索引数量的比值小于或等于所述预设差异阈值,则不将所述当前点云帧配置为关键帧,以使所述预设设置的关键帧匹配到的栅格对应的索引信息没有发生变化;然后执行步骤S1。
进一步地,控制当前点云帧与地图作匹配的方法包括:在采集到一个当前点云帧时,将当前点云帧中的点云坐标信息转换为地图坐标系中的坐标信息;根据转换过来的地图坐标系中坐标信息,将当前点云帧所包括的点云索引到地图坐标系的相应栅格中以建立起当前点云帧与地图中的栅格建立起映射关系,再将当前点云帧索引到的所有栅格对应的索引信息组成所述当前栅格索引集合,但不在地图上改变所述当前点云帧索引到的栅格,并将当前点云帧索引到的所有栅格对应的索引信息都标记为所述当前点云帧匹配到的栅格对应的索引信息;其中,至少一个点云帧索引到同一个栅格,或同一个点云帧所包括的至少一个点云点索引到同一个栅格;与当前点云帧作匹配的地图是栅格地图。
进一步地,所述参考栅格索引集合内的元素是所述预先设置的关键帧匹配到的栅格对应的索引信息;所述当前栅格索引集合内的元素是所述当前点云帧匹配到的栅格对应的索引信息;其中,参考栅格索引集合内的一个元素用于表示一个栅格,一个栅格携带一个索引信息,不同栅格之间对应的索引信息是不同;在当前栅格索引集合内,与所述预先设置的参考栅格索引集合内的元素不相同的元素用于表示:当前栅格索引集合内,与所述预先设置的参考栅格索引集合内的元素的位姿信息不相同的栅格,其中,栅格对应的索引信息包括栅格中标记的位姿信息或栅格在地图中的索引号。
基于点云帧的地图更新方法,所述地图更新方法包括:每当采集到一个当前点云帧,则判断当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值,是则将当前点云帧更新到地图,否则不将当前点云帧更新到地图中。
进一步地,所述地图更新方法具体包括:步骤S1、每当采集到一个当前点云帧,则控制当前点云帧与地图作匹配,获取当前栅格索引集合,再将当前栅格索引集合内的元素数量标记为当前栅格索引数量;当前栅格索引集合,用于表示与当前点云帧存在映射关系的栅格对应的索引信息组成的集合;栅格是设置在所述地图中;步骤S2、在当前栅格索引集合内,统计与预先设置的参考栅格索引集合内的元素不相同的元素的数量,再将当前统计的数量标记为差异数量;其中,参考栅格索引集合内的元素的数量是参考栅格索引数量;步骤S3、若差异数量与参考栅格索引数量的比值大于预设差异阈值,则将当前点云帧判定为新的关键帧,再将新的关键帧加入地图中,生成新地图,则确定将当前点云帧更新到地图中;步骤S4、若差异数量与参考栅格索引数量的比值小于或等于预设差异阈值,则不将所述当前点云帧更新到地图中;其中,差异数量与参考栅格索引数量的比值用于表示所述当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异。
进一步地,在第一次执行步骤S1之前,还包括:将采集到的第一个点云帧设置为所述预设设置的关键帧,并将采集到的第一个点云帧加入地图坐标系中以使对应栅格被投射入第一个点云帧,生成原始地图;再将与第一个点云帧存在映射关系的栅格对应的索引信息所组成的集合标记为所述预先设置的参考栅格索引集合,并将与第一个点云帧存在映射关系的栅格的数量标记为所述参考栅格索引数量;其中,所述当前点云帧不是第一个点云帧。
进一步地,所述步骤S1控制所述当前点云帧与地图作匹配的过程中,所述当前点云帧与地图中的栅格建立起映射关系,并提取出与所述当前点云帧存在映射关系的栅格对应的索引信息以生成当前栅格索引集合,但所述当前点云帧没有加入地图中以使得地图没有被更新;所述步骤S3将所述新的关键帧加入地图后,生成新地图,并将新地图更新为步骤S1中的地图,其中,新地图中已经存在栅格被投射入所述当前点云帧,以将对应栅格中的位姿信息被更新为所述当前点云帧中的位姿信息。
进一步地,在执行步骤S3之后,将当前栅格索引集合更新为所述预先设置的参考栅格索引集合,并将当前栅格索引数量更新为所述参考栅格索引数量,则确定对所述预先设置的关键帧匹配到的栅格对应的索引信息完成更新;然后执行步骤S1以控制后续采集的点云帧与上一次执行的步骤S3中更新过的地图作匹配,其中,步骤S3生成的新地图被更新为后续执行的步骤S1中的地图。
进一步地,控制当前点云帧与地图作匹配的方法包括:在采集到一个当前点云帧时,将当前点云帧中的点云坐标信息转换为地图坐标系中的坐标信息,以建立起当前点云帧与地图中的栅格之间的映射关系;根据转换后的坐标信息,将当前点云帧所包括的点云索引到地图坐标系的相应栅格中,再将当前点云帧索引到的所有栅格对应的索引信息组成所述当前栅格索引集合,但不控制所述当前点云帧加入地图中;其中,至少一个点云帧索引到同一个栅格,或同一个点云帧所包括的至少一个点云点索引到同一个栅格;与当前点云帧作匹配的地图是栅格地图。
进一步地,将关键帧加入地图中的方法包括:将关键帧所包括的点云变换到地图坐标系中,获得栅格索引坐标,以实现将当前点云帧中的点云坐标信息对齐至地图坐标系下;其中,关键帧所包括的点云中的每个位置点都对应到一个栅格索引坐标,形成所述索引信息;然后将关键帧所包括的点云投射入到对应的栅格索引坐标所表示的栅格中,并将点云所携带的激光信息更新为对应投射到的栅格内原来填充的激光信息,获得更新后的栅格;然后在同一地图坐标系中将更新后的栅格与未更新的栅格组成新地图,则确定将关键帧加入地图中并更新地图。
进一步地,在步骤S2中,所述参考栅格索引集合内的元素是预先设置的关键帧匹配到的栅格对应的索引信息;所述当前栅格索引集合内的元素是当前点云帧匹配到的栅格对应的索引信息;其中,参考栅格索引集合内的一个元素用于表示一个栅格,一个栅格携带一个索引信息,不同栅格之间对应的索引信息是不同;在当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素不相同的元素用于表示:当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素的位姿信息不相同的栅格,其中,栅格对应的索引信息包括栅格中标记的位姿信息或栅格在地图中的索引号。
本申请的技术效果在于:机器人在连续采集的点云帧的过程中,若判断到当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异大于预设差异阈值,则将当前点云帧判定为关键帧,并将当前点云帧更新到地图中并将当前判定出的关键帧更新为预先设置的关键帧,以控制后续采集的每个点云帧都与上一次更新过的地图作匹配;从而克服仅通过距离变化(位移变化)和角度变化来确定关键帧会存在方向干扰的问题,即机器人的行走方向频繁变化但发生的行走距离或偏转的角度不大的情况下,将当前点云帧与最新设置的关键帧匹配的栅格(或栅格对应的索引信息)进行数量维度上的对比,准确地确定出机器人所扫描的环境中匹配出的新的栅格的数量是否出现明显变化,从而将不同方向上采集的点云帧依次设置为关键帧,以便更新到地图中,不会因为行走距离或角度变化过小而将采集的点云帧视为相似的点云数据以致于没有及时更新到地图中。
基于前述技术方案的步骤S1至步骤S3当中设置关键帧和生成新地图的方法,机器人在连续采集的点云帧的过程中,若判断到当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异大于预设差异阈值,则将当前点云帧判定为关键帧,并将当前点云帧更新到地图中并将当前判定出的关键帧匹配到的栅格对应的索引信息更新为预先设置的关键帧匹配到的栅格对应的索引信息,以控制后续采集的每个点云帧都与上一次更新过的地图作匹配;从而克服仅通过距离变化(位移变化)和角度变化来确定关键帧会存在方向干扰的问题,即机器人的行走方向频繁变化但发生的行走距离或偏转的角度不大的情况下,将当前点云帧与最新设置的关键帧匹配的栅格对应的索引信息进行数量维度上的对比,准确地确定出机器人所扫描的环境中匹配出的新的栅格的数量是否出现明显变化,从而将不同方向上采集的点云帧依次设置为关键帧,以便更新到地图中,不会因为行走距离或角度变化过小而将采集的点云帧视为相似的点云数据以致于没有及时更新到地图中。
其中,机器人控制采集的第一个点云帧不与地图作匹配,而是控制采集的第一个点云帧进行直接转换,则在较小计算量的基础上建立初始定位能力,形成后续帧进行匹配的基准地图;除了采集到的第一个点云帧之外,在对采集到的每个点云帧与地图作匹配的过程中,每个点云帧都通过坐标转换的方式来在地图中匹配到可索引的栅格的信息,提高每个点云帧与所匹配(所映射)的地图的重合度,从而确定点云帧能够对应到有效的定位栅格并能够提取出这些定位栅格的数量信息,以便于比较先后采集的两个点云帧映射到地图栅格位置之间的差异,再确定是否将点云帧更新到地图,从而通过帧间对应索引的栅格的精细化匹配计算,来在机器人的多种行走距离跨度和各种偏转方向跨度的情况下进行地图更新,减少行走距离的变化幅度和行走方向偏转幅度的影响。
附图说明
图1是本发明的一实施例公开基于激光点云的关键帧判定方法的流程示意图。
图2是本发明的另一实施例公开基于点云帧的地图更新方法的流程示意图。
具体实施方式
以下结合附图对本申请的示范性实施例做出说明,其中包括本申请实施例的各种细节以助于理解,应当将它们认为仅仅是示范性的。因此,本领域普通技术人员应当认识到,可以对这里描述的实施例做出各种改变和修改,而不会背离本申请的范围和精神。同样,为了清楚和简明,以下的描述中省略了对公知功能和结构的描述。
在介绍本申请的实施例之前,首先对本申请中涉及到的术语进行解释:
点云:点云是在同一空间参考系下表达目标空间分布和目标表面特性的海量点集合,在获取物体表面每个采样点的空间坐标后,得到的是点的集合,称之为“点云”(PointCloud)。
点云帧:点云帧,也可以称为一帧的点云数据,是每一次通过单线或多线激光雷达等激光传感器扫描获取到的具有三维位置的点组成的集合,类似于每次获取到一帧的包括很多点的图像。具体实现中,根据激光测量原理得到的点云,包括三维坐标(XYZ)和激光反射强度(Intensity),强度信息与目标的表面材质、粗糙度、入射角方向、以及仪器的发射能量,激光波长有关。根据摄影测量原理得到的点云,包括三维坐标(XYZ)和颜色信息(RGB)。
本申请中,术语“帧”是现有技术,其由点云构成,称为点云帧。点云帧的坐标系是地图坐标系,包括横纵坐标和角度;帧中点云或者其中的点的坐标系是雷达坐标系,包括横纵坐标和角度;位姿变化包括横纵坐标的变化和旋转角度或这三者的速度。
需要说明的是,对于一个固定的被扫描的障碍物,即静止障碍物,可以通过移动激光传感器,在尽可能的情况下获取尽量多的实体表面点信息;当一束激光照射到被扫描物体的表面时,所反射的激光信息会携带方位、距离等信息。结合激光测量和摄影测量原理得到点云,包括坐标(XY)、激光反射强度(Intensity)和颜色信息(RGB);具体地,激光传感器在获取被扫描物体的表面的每个采样点的空间坐标后,得到的是点的集合,称之为点云(Point Cloud),使得点云也是目标表面特性的海量点集合。
需要说明的是,激光传感器采集到的反射自被扫描物体的激光信息转换至栅格地图的坐标点,则激光点是激光扫描点或者激光采样点,反映位置信息(包括到被扫描物体表面的探测距离和探测角度)和激光反射强度;具体地,激光传感器若将激光束按照某种轨迹对被扫描物体进行扫描,便会边扫描边记录到反射回的激光点信息,由于扫描极为精细,则能够得到大量的激光点,因而就可形成激光点云。其中,激光传感器一般是支持360度旋转扫描的激光雷达,带有激光发射探头和接收探头;具体地,激光传感器采集到的反射自被扫描物体的激光信息是包括激光雷达数据包,激光雷达数据包包括若干帧激光点云,每帧激光点云包括若干个激光点,每个激光点包含角度(逆时针为正方向)和距离,因此,所述激光点所携带的位姿信息用于更新其所处的栅格的位置信息;具体地,激光传感器就是使用激光束对障碍物进行按角度扫描,每一个角度对应一个激光点,所用角度的激光点加在一起是一帧点云,例如,每帧激光点云是可以设置为由激光雷达扫描一圈而采集到的激光点的集合。
本申请的执行主体可以是需要导航定位的移动机器人,也可以为能够与移动机器人进行信息传输,并对移动机器人进行控制的另一控制设备执行,控制设备可以为服务器或终端设备,当控制设备为服务器时,本说明书实施例并不限制所述控制设备为分布式服务器或集群式服务区,当所述控制设备为终端设备时,可以为任一现有的终端设备,例如,笔记本电脑、手机、服务器等,本说明书对此不不作限制。仅示例性的,以移动机器人自身作为执行主体为例对本说明书实施例以下部分进行说明。
执行主体扫地机器人为例进行说明,扫地机器人的主体包括前向部分和后向部分,具有近似圆形形状(前后都为圆形),也可具有其他形状,包括但不限于前方后圆的近似D形形状或前方后方的矩形或正方形。扫地机器人的机体内部安装一个控制器,扫地机器人的左右两侧各安装一个驱动轮;扫地机器人的机体顶部至少安装激光传感器,该激光传感器可以是360度旋转的激光雷达或固定式激光雷达,也可以是三角测距的激光传感器或TOF激光传感器,作为移动机器人构建地图和定位的点云采样装置;控制器控制360度旋转激光雷达扫描一圈或机器人带动固定激光雷达旋转,采集到点云帧,点云帧所包括的点云生成反映障碍物占据区域的轮廓的点云,可以用于测量机器人与障碍物的距离;控制器读取点云帧,通过SLAM算法生成室内地图,一般为构建起栅格地图。
作为一种实施例,公开基于激光点云的关键帧判定方法,包括:每当采集到一个当前点云帧,则判断当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值,是则将当前点云帧判定为关键帧,即获得新的关键帧,并将当前判定出的关键帧匹配到的栅格对应的索引信息更新为预先设置的关键帧匹配到的栅格对应的索引信息,以便从后续采集的点云帧中筛选出新的关键帧;但还没有对预先设置的关键帧匹配到的栅格进行更新,也不一定替换掉原来设置出的关键帧,原因在于,相对于新的关键帧,原来设置出的关键帧所包括的部分或全部点云是具有定位意义。
在本实施例中,当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异表现为不同点云帧在地图中匹配的栅格数量之间的差异时,能够基于预设差异阈值的设置来对小范围区域内采集的点云进行筛选,获得具有定位意义的关键帧,减少点云帧当中携带的位姿变化因素(行走距离变化或旋转角度变化)的影响。
在本实施例中,对于世界坐标中的每个栅格,用于标记机器人所在环境中的位置信息;当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异可以是当前点云帧匹配到的栅格相对于预先设置的关键帧匹配到的栅格产生的新的栅格的占比率,本实施例会采用当前点云帧匹配到的栅格对应的索引信息相对于预先设置的关键帧匹配到的栅格对应的索引信息产生的新的索引信息的数量占比判断前述栅格之间的差异特征,不是直接表示为包括点的点云帧的帧数或点云中的点的数量在所述栅格中所占的比例。需要说明的是,所述预设差异阈值可以根据经验去设定,也可以根据实验数据确定,可以是一定的百分比,具体的值本申请不做限制。
在一些实施例中,机器人将陆续获取的多个点云在世界坐标中进行映射后,世界坐标中的每个栅格中可以都包括多个点云帧,基于点云帧中没有障碍物的位置,可能没有出现点,或者密集的点,也有可能由于空中的颗粒物等导致在某次的点云帧上存在点,然而,对于有一定体积的静态障碍来说,在连续的一段时间内获取的多个点云帧中存在点。基于此,在判断当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值的过程中,将预设差异阈值考虑在较小的栅格数量(使用对应的索引信息的数量表示,比如对应位置坐标的索引号的数量)或其占比范围内,可以将机器人周围环境中更多轮廓细节反馈到地图中;如果比较多的点云帧中都有点,则能确定该栅格所在的位置有障碍物,特别是连续的帧上出现较多的点。
作为一种实施例,所述判断当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值的方法,如图1所示,具体包括:
步骤S101、每当采集到一个当前点云帧,则控制当前点云帧与地图作匹配,获取当前栅格索引集合,再将当前栅格索引集合内的元素数量标记为当前栅格索引数量,然后执行步骤S102;当前栅格索引集合用于表示地图中与当前点云帧存在映射关系的栅格对应的索引信息组成的集合,标识为当前点云帧与地图之间的匹配结果;点云与地图的匹配在本实施例中是指将机器人周围的点云通过旋转平移,变换对齐至与地图坐标系下,从而根据所确定出的旋转平移矩阵来提取出机器人及其周围的障碍物对应映射到的栅格,优选地,将对应映射到的各个栅格在地图中的索引号组成的集合记为所述当前栅格索引集合,一个索引信息是一个索引号并对应一个栅格的坐标,不同栅格的索引号是不同,不同栅格的坐标也是不同,以使一个索引号对应一个栅格的坐标;因此,所述差异数量表示一个点云帧中所包括的点云在地图中映射的栅格相对于另一个点云帧中所包括的点云在地图中映射的栅格产生的新栅格数量。
步骤S102、在当前栅格索引集合内,统计与预先设置的参考栅格索引集合内的元素不相同的元素的数量,再将当前统计的数量标记为差异数量,然后执行步骤S103;其中,参考栅格索引集合内的元素的数量是参考栅格索引数量;在一些实施方式中,所述参考栅格索引集合内的元素是所述预先设置的关键帧匹配到的栅格对应的索引信息;所述当前栅格索引集合内的元素是所述当前点云帧匹配到的栅格对应的索引信息。在所述当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素不相同的元素用于表示:当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素的位姿信息不相同的栅格,其中,栅格对应的索引信息包括栅格中标记的位姿信息或栅格在地图中的索引号。
在一些实施例中,在所述当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素不相同的元素可以直接表示为当前栅格索引集合中与预先设置的参考栅格索引集合内的元素的位姿信息不相同的栅格。当然,在当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素不相同的元素也可以包括当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素的环境标记信息(标识为障碍物的类型、障碍物的轮廓、反射的信号强度等)不同的栅格。
步骤S103、若差异数量与参考栅格索引数量的比值大于预设差异阈值,则将当前点云帧判定为关键帧,即获得新的关键帧,但不将当前点云帧更新为所述预先设置的关键帧,而是将当前判定出的关键帧匹配到的栅格对应的索引信息更新为所述预先设置的关键帧匹配到的栅格对应的索引信息,以便于下一次执行步骤S103时使用新的参考栅格索引集合对所述当前点云帧进行判定。因此,为了实现持续获得关键帧以获得连续的定位信息,选择执行步骤S104。
在本实施例中,差异数量与参考栅格索引数量的比值用于表示所述当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异。需要说明的是,所述当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格都可以视为转换在同一地图坐标系下的栅格,若二者的栅格的坐标信息相同或二者的栅格对应的索引信息相同,则确定所述当前栅格索引集合内存在元素与所述预先设置的参考栅格索引集合内的元素重合,则可以将所述当前栅格索引集合内与所述预先设置的参考栅格索引集合重合的元素占所述预先设置的参考栅格索引集合内所有元素的数量的比例作为两个点云帧之间的重叠度;也可以将所述当前栅格索引集合内与所述预先设置的参考栅格索引集合不重合的元素占所述预先设置的参考栅格索引集合内所有元素的数量的比例作为两个点云帧之间的不重叠度,即所述差异数量与所述参考栅格索引数量的比值。针对采集到的每个当前点云帧,可以根据当前点云帧所包括的点云匹配的栅格与关键帧所包括的点云匹配的栅格之间的不重叠度,确定当前点云帧是否需要被配置为新的关键帧,若对应的不重叠度越高,则所采集到的当前点云帧越需要被配置为关键帧,获得具有定位意义的点云帧。
在一些实施例中,执行步骤S103的过程中,在将当前点云帧判定为关键帧后,将当前判定的关键帧更新到地图中,即将关键帧所包括的点云所携带的位姿信息都转换到地图坐标系下,并将转换后的位姿信息都更新到地图对应栅格中,让对应匹配的栅格都填充上新加入的位姿信息,在此基础上步骤S103生成的新地图被更新为后续执行的步骤S101中的地图,此时确定步骤S103使用当前点云帧完成地图更新;其中,该新地图中已经存在栅格被投射入所述当前点云帧,被投射入所述当前点云帧的栅格中的位姿信息被更新为所述当前点云帧中的位姿信息,当然,所述当前点云帧匹配的栅格对应的索引信息也更新为所述预先设置的关键帧匹配到的栅格对应的索引信息,使得预先设置的参考栅格索引集合内的元素都被更新。
步骤S104、将当前栅格索引集合更新为所述预先设置的参考栅格索引集合,即将当前栅格索引集合内的每个元素依次更新为所述预先设置的参考栅格索引集合内对应排序的元素;并将当前栅格索引数量更新为步骤S102所述的参考栅格索引数量;然后执行步骤S101,以控制后续采集的点云帧与上一次执行的步骤S103中更新过的地图作匹配,其中,除了第一个点云帧之外,每次采集的点云帧依次被更新为所述当前点云帧,即步骤S101中公开的当前点云帧不是机器人第一次采集到的点云帧。
在前述地图中,需要获取世界坐标,对所述世界坐标进行空间栅格化处理,处理后的世界坐标中包括多个栅格,则前述地图也可以描述为很多的栅格组成的空间,这里的栅格指的是世界坐标系中划分出来的单元格,作为在空间上划分出的最小单元。
在上述实施例的基础上,在第一次执行步骤S101之前,还包括:在机器人的激光传感器采集到第一个点云帧后,将采集到的第一个点云帧设置为所述预设设置的关键帧,作为初始关键帧,并将采集到的第一个点云帧加入地图中以使对应栅格被投射入第一个点云帧;然后将与第一个点云帧存在映射关系的栅格对应的索引信息所组成的集合标记为所述预先设置的参考栅格索引集合,并将与第一个点云帧存在映射关系的栅格的数量标记为所述参考栅格索引数量,即所述的预先设置的参考栅格索引集合包括的元素的数量,其中,索引信息的数量等于栅格的数量,一个索引信息对应一个栅格;然后执行步骤S101,在执行步骤S101中,所述当前点云帧不是第一个点云帧。
在本实施例中,第一个点云帧未加入地图之前,机器人构建的地图当中没有点云匹配的栅格信息,即没有标记出点云反馈的任意障碍物的位置信息,地图中只是配置坐标系但没有填充上具体的坐标信息,在将采集到的第一个点云帧转换到地图坐标系才形成地图中的第一批具有定位意义的坐标信息,从而将与第一个点云帧存在映射关系的栅格反馈为地图中的首个障碍物的位置信息。因此,本实施例控制采集的第一个点云帧不与地图作匹配,而是控制采集的第一个点云帧直接转换到地图坐标系上,则在较小计算量的基础上建立初始定位能力,形成后续帧进行匹配和判定新关键帧的基准地图。
在上述实施例的基础上,所述步骤S103还包括:若差异数量与参考栅格索引数量的比值小于或等于预设差异阈值,则不将所述当前点云帧配置为关键帧;在步骤S103中,所述差异数量与参考栅格索引数量的比值具体是通过栅格对应的索引信息的数量差异来表现各个集合占据的位置区域的差异;所述当前点云帧所能匹配的地图栅格区域相对于参考栅格索引集合内所包括的栅格的位置差异不大,至少在新匹配的栅格的数量变化不大,不足以产生定位效果,则不将所述当前点云帧加入地图中,且保留上一次判定的关键帧不变,且不将当前栅格索引集合更新为所述预先设置的参考栅格索引集合,即停止对所述预先设置的关键帧匹配到的栅格对应的索引信息进行更新;也不将当前栅格索引数量更新为所述参考栅格索引数量;然后执行步骤S101(图1中没有表示出)。则在下一次执行步骤S101至步骤S103的过程中继续使用所述预先设置的参考栅格索引集合和所述参考栅格索引数量去判定的新的关键帧,基于预设差异阈值在较窄的通道区域试验的结果,能够保证在短距离和小角度变化的范围内保证地图中反馈的点云数据的连续性。
作为一种实施例,在前述步骤S101中,控制当前点云帧与地图作匹配的方法包括:在采集到一个当前点云帧时,将当前点云帧中的点云坐标信息转换为地图坐标系中的坐标信息,即根据点云所在的坐标系与地图坐标系之间的变换关系,将当前点云帧中的点云坐标信息换算为地图坐标系(世界坐标系)中的坐标信息,以便于投射到世界坐标系中,且尽量让当前点云帧所包括的点云与地图重叠在一起,将当前点云帧中所包括的更多点索引到地图的对应栅格。然后,根据转换过来的地图坐标系中坐标信息,将当前点云帧所包括的点云索引到地图坐标系的相应栅格中以建立起当前点云帧与地图中的栅格建立起映射关系,并将当前点云帧索引到的所有栅格对应的索引信息组成所述当前栅格索引集合,所述当前栅格索引集合内每个元素是对应一个栅格的索引信息;但不在地图上改变所述当前点云帧索引到的栅格,还没开始控制所述当前点云帧对地图进行更新;因此,在本实施例中,尽管所述当前点云帧与地图中的栅格建立起映射关系以生成所述当前栅格索引集合,但是所述当前点云帧没有加入地图中,则保持地图没有被更新;只是将当前点云帧索引到的所有栅格对应的索引信息依次标记为所述当前点云帧匹配到的栅格对应的索引信息,但没有在地图上将当前点云帧索引到的栅格更新为所述当前点云帧匹配到的栅格。从而本实施例在改变原来的地图的前提下提取出所述当前栅格索引集合,以在执行步骤S102时统计所述差异数量。
需要说明的是,在控制当前点云帧与地图作匹配的过程中,至少一个点云帧索引到同一个栅格,或同一个点云帧所包括的至少一个点云点索引到同一个栅格;与当前点云帧作匹配的地图是栅格地图,以点云匹配的栅格所描述出的定位区域中的场景。空间中包括多个栅格,这些栅格可以是三维空间上的栅格,也可以是不考虑高度维度的二维空间上的栅格。一般来说,在每个栅格中,都包括不断获取到的多个点云帧的部分或全部,这多个点云帧可以是连续的,也可以是具有一定间隙的点云帧,对此本方案不做限制。
综合上述步骤S101至步骤S104,机器人利用当前栅格索引集合中的与预设设置的参考栅格索引集合的元素不相同(包括坐标位置不重合)的元素的数量占据预先设置的参考栅格索引集合的总元素数量的比值大小,来确定后续帧相对于上一帧的匹配的栅格区域是否明显的差异;若存在则将当前点云帧设置为关键帧,既可以将其更新到地图中,又可以作为下一次进行栅格或其对应的索引信息数量对比的参考点云帧;因此,本申请充分利用前后连续采集到的点云数据索引到地图中的相匹配的栅格确定关键帧,特别是从机器人短距离行走和小角度偏转的行程(例如穿行于窄道区域的行程)中采集的点云帧当中挑选出用于加入地图的关键帧,能够反馈出的环境信息是相对连续,不受到机器人行走方向的影响,能够达到点云帧连续匹配地图的效果,实现使用位姿连续性较强的多个点云帧对原来的地图进行更新,提高更新后的地图的定位精度,提高激光导航的环境适应性和稳定性,从而能够较好解决机器人经过窄道所采集的激光点云不连续问题。保证点云帧与地图匹配的连续性,不容易出现漏检,提高地图定位精度。
作为另一种实施例,公开基于点云帧的地图更新方法,所述地图更新方法包括:每当采集到一个当前点云帧,则判断当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值,是则将当前点云帧更新到地图中,并将当前点云帧配置为新的关键帧,因此将当前配置出的关键帧匹配到的栅格对应的索引信息更新为所述预先设置的关键帧匹配到的栅格对应的索引信息,以实现从后续采集的当前点云帧中继续筛选出新的关键帧,进而保持对地图进行连续更新的趋势;在一些实施例中,为了保留预先设置的关键帧所包括的点云及其映射到地图中的位置信息,不对预先设置的关键帧所包括的点云进行更新,即不直接将当前点云帧更新为所述预先设置的关键帧本身。
所述地图更新方法还包括:若当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异小于或等于所述预设差异阈值时,不将所述当前点云帧更新到地图中,即暂停对地图进行更新,同时,不将所述当前点云帧配置为关键帧;所述当前点云帧所能匹配的栅格区域相对于参考栅格索引集合内所包括的栅格的位置差异不大,至少在新匹配的栅格的数量变化不大,不足以产生定位效果,则不将所述当前点云帧加入地图中,且保留上一次判定的关键帧和所述预先设置的关键帧不变,且不将当前栅格索引集合更新为所述预先设置的参考栅格索引集合,即停止对所述预先设置的关键帧匹配到的栅格对应的索引信息进行更新;也不将当前栅格索引数量更新为所述参考栅格索引数量。因此,在本实施例中,所述当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异表现为不同点云帧在地图中匹配的栅格数量之间的差异时,能够基于预设差异阈值的设置来对小范围区域内采集的点云依次更新到地图中,减少点云帧当中携带的位姿变化因素对地图定位精度的影响。
作为一种实施例,参阅图2可知,所述地图更新方法具体包括:
步骤S201、每当采集到一个当前点云帧,则控制当前点云帧与地图作匹配,获取当前栅格索引集合,再将当前栅格索引集合内的元素数量标记为当前栅格索引数量,然后执行步骤S202;其中,当前栅格索引集合用于表示地图中与当前点云帧存在映射关系的栅格对应的索引信息组成的集合,标识为当前点云帧与地图之间的栅格匹配结果。在本实施例中,点云帧与地图的匹配是指将机器人周围的点云通过旋转平移,变换对齐至与地图坐标系下,从而根据所确定出的旋转平移矩阵来提取出机器人及其周围的障碍物对应映射到的栅格,再将对应映射到的各个栅格在地图中的索引号组成的集合记为所述当前栅格索引集合;优选地,一个索引信息是一个索引号并对应一个栅格的坐标,不同栅格的索引号是不同;不同栅格的坐标也是不同,以使一个索引号对应一个栅格的坐标;因此,所述差异数量表示一个点云帧中所包括的点云在地图中映射的栅格相对于另一个点云帧中所包括的点云在地图中映射的栅格产生的新栅格数量。
步骤S202、在当前栅格索引集合内,统计与预先设置的参考栅格索引集合内的元素不重合的元素的数量,再将当前统计的数量标记为差异数量,并确定该差异数量是所述当前点云帧所包括的点云与预先设置的关键帧所包括的点云之间的差异;然后执行步骤S203;其中,参考栅格索引集合内的元素的数量是参考栅格索引数量;所述参考栅格索引集合内的元素是所述预先设置的关键帧匹配到的栅格对应的索引信息;所述当前栅格索引集合内的元素是所述当前点云帧匹配到的栅格对应的索引信息,其中,参考栅格索引集合内的一个元素用于表示一个栅格,一个栅格携带一个索引信息,不同栅格之间对应的索引信息是不同。
在所述当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素不相同的元素用于表示:当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素的位姿信息不相同的栅格,其中,栅格对应的索引信息包括栅格中标记的位姿信息或栅格在地图中的索引号。在一些实施例中,在当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素不相同的元素也可以包括当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素的环境标记信息(标识为障碍物的类型、障碍物的轮廓、反射的信号强度等)不同的栅格。
步骤S203、判断差异数量与参考栅格索引数量之间的比值是否大于预设差异阈值,是则执行步骤S204,否则执行步骤S206。在本实施例中,差异数量与参考栅格索引数量的比值用于表示所述当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异。需要说明的是,所述当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格都可以视为转换在同一地图坐标系下的栅格,若二者的栅格的坐标信息相同或二者的栅格对应的索引信息相同,则确定所述当前栅格索引集合内存在元素与所述预先设置的参考栅格索引集合内的元素重合,则可以将所述当前栅格索引集合内与所述预先设置的参考栅格索引集合重合的元素占所述预先设置的参考栅格索引集合内所有元素的数量的比例作为两个点云帧之间的重叠度;也可以将所述当前栅格索引集合内与所述预先设置的参考栅格索引集合不重合的元素占所述预先设置的参考栅格索引集合内所有元素的数量的比例作为两个点云帧之间的不重叠度,即所述差异数量与所述参考栅格索引数量的比值。针对采集到的每个当前点云帧,可以根据当前点云帧所包括的点云匹配的栅格与关键帧所包括的点云匹配的栅格之间的不重叠度,确定当前点云帧是否需要被配置为新的关键帧并进一步更新到原来构建的地图中以获得最新扫描的环境特征,若对应的不重叠度越高,则所采集到的当前点云帧越需要被配置为关键帧并及时更新到地图中以形成连续性较强的环境特征信息组合。
步骤S204、将当前点云帧判定为关键帧,则确定将当前点云帧判定为新的关键帧,再将新的关键帧加入地图中,生成新地图,则确定将当前点云帧更新到地图中,即实现更新步骤S201中的地图;然后执行步骤S205,以将当前判定出的关键帧匹配到的栅格对应的索引信息更新为所述预先设置的关键帧匹配到的栅格对应的索引信息,便于下一次执行步骤S203时使用新的参考栅格索引集合对所述当前点云帧进行判定。
对于步骤S204中将新的关键帧加入地图的实施方式,可以将当前判定出的关键帧匹配到的栅格更新为所述预先设置的关键帧匹配到的栅格;或当前判定出的关键帧匹配到的栅格相对于上一次判定的关键帧匹配到的栅格存在新的栅格时,为地图中匹配到的新的栅格添加点云帧反馈的位姿信息等,以实现对地图的更新。
对于步骤S204中将新的关键帧加入地图的实施方式,还可以将当前点云帧更新为所述预先设置的关键帧;或在所述预先设置的关键帧的基础上补充所述当前点云帧所包括的新的点云,拼成新的关键帧,则将该新的关键帧更新为所述预先设置的关键帧,在新的关键帧更新到地图后,确定实现对地图的更新。
步骤S205、将当前栅格索引集合更新为所述预先设置的参考栅格索引集合,即将当前栅格索引集合内的每个元素依次更新为所述预先设置的参考栅格索引集合内对应排序的元素,则确定将当前判定出的关键帧匹配到的栅格对应的索引信息更新为所述预先设置的关键帧匹配到的栅格对应的索引信息;并将当前栅格索引数量更新为步骤S201所述的参考栅格索引数量;然后执行步骤S201,以控制后续采集的点云帧与在先执行的步骤S204最新更新过的地图作匹配,其中,在先执行的步骤S204更新过的地图会更新为后续执行的步骤S201中的地图(即与最新采集的一个点云帧作匹配的地图);除了第一个点云帧之外,每次采集的点云帧依次被更新为步骤S201所述当前点云帧,即步骤S201中公开的当前点云帧不是机器人第一次采集到的点云帧。从而实现对当前点云帧在地图上进行闭环检测,以减少机器人在移动过程中增加的定位误差。
步骤S206、不将所述当前点云帧更新到地图中,然后执行步骤S201。在步骤S206中,可以确定所述当前点云帧所能匹配的地图栅格区域相对于参考栅格索引集合内所包括的栅格的位置差异不大,至少在新匹配的栅格的数量变化不大,不足以产生定位效果,则不将所述当前点云帧加入地图中,且保留上一次判定的关键帧不变,且不将当前栅格索引集合更新为所述预先设置的参考栅格索引集合,即停止对所述预先设置的关键帧匹配到的栅格对应的索引信息进行更新;也不将当前栅格索引数量更新为所述参考栅格索引数量;然后执行步骤S201。则在下一次执行步骤S201至步骤S206的过程中继续使用所述预先设置的参考栅格索引集合和所述参考栅格索引数量去判定的新的关键帧,并及时使用新的关键帧去更新在先执行的步骤S204最新更新过的地图,从而基于预设差异阈值在较窄的通道区域试验的结果能够保证在短距离和小角度变化的范围内保证地图中反馈的点云数据的连续性。
综合前述步骤S201至步骤S206,机器人在连续采集的点云帧的过程中,若判断到当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异大于预设差异阈值,则将当前点云帧判定为关键帧,并将当前点云帧更新到地图中并将当前判定出的关键帧匹配到的栅格对应的索引信息更新为预先设置的关键帧匹配到的栅格对应的索引信息,以控制后续采集的每个点云帧都与上一次更新过的地图作匹配;从而克服仅通过距离变化(位移变化)和角度变化来确定关键帧会存在方向干扰的问题,即机器人的行走方向频繁变化但发生的行走距离或偏转的角度不大的情况下,将当前点云帧与最新设置的关键帧匹配的栅格对应的索引信息进行数量维度上的对比,准确地确定出机器人所扫描的环境中匹配出的新的栅格的数量是否出现明显变化,从而将不同方向上采集的点云帧依次设置为关键帧,以便更新到地图中,不会因为行走距离或角度变化过小而将采集的点云帧视为相似的点云数据以致于没有及时更新到地图中。从而能够反馈出的环境信息是相对连续,不受到机器人行走方向的影响,能够达到点云帧连续匹配地图的效果,实现使用位姿连续性较强的多个点云帧对原来的地图进行更新,提高更新后的地图的定位精度,提高激光导航的环境适应性和稳定性,从而能够较好解决机器人经过窄道所采集的激光点云不连续问题。保证点云帧与地图匹配的连续性,不容易出现漏检,提高地图定位精度。
其中,机器人控制采集的第一个点云帧不与地图作匹配,而是控制采集的第一个点云帧进行直接转换,则在较小计算量的基础上建立初始定位能力,形成后续帧进行匹配的基准地图;除了采集到的第一个点云帧之外,在对采集到的每个点云帧与地图作匹配的过程中,每个点云帧都通过坐标转换的方式来在地图中匹配到可索引的栅格的信息,提高每个点云帧与所匹配(所映射)的地图的重合度,从而确定点云帧能够对应到有效的定位栅格并能够提取出这些定位栅格的数量信息,以便于比较先后采集的两个点云帧映射到地图栅格位置之间的差异,再确定是否将点云帧更新到地图,从而通过帧间对应索引的栅格的精细化匹配计算,来在机器人的多种行走距离跨度和各种偏转方向跨度的情况下进行地图更新,减少行走距离的变化幅度和行走方向偏转幅度的影响。
在上述实施例的基础上,在第一次执行步骤S201之前,还包括:在机器人的激光传感器第一次采集到点云帧后,记为采集到第一个点云帧,并将采集到的第一个点云帧设置为所述预设设置的关键帧,并将采集到的第一个点云帧加入地图坐标系中以使对应栅格被投射入第一个点云帧,生成原始地图;再将与第一个点云帧存在映射关系的栅格所组成的集合标记为所述预先设置的参考栅格索引集合,并将与第一个点云帧存在映射关系的栅格的数量标记为所述参考栅格索引数量;其中,索引信息的数量等于栅格的数量,一个索引信息对应一个栅格;然后执行步骤S201,在执行步骤S201中,所述当前点云帧不是第一个点云帧。在本实施例中,第一个点云帧未加入地图之前,机器人构建的地图当中没有点云匹配的栅格信息,即没有标记出点云反馈的任意障碍物的位置信息,地图中只是配置坐标系但没有填充上具体的坐标信息,在将采集到的第一个点云帧转换到地图坐标系才形成地图中的第一批具有定位意义的坐标信息,从而将与第一个点云帧存在映射关系的栅格反馈为地图中的首个障碍物的位置信息。因此,本实施例控制采集的第一个点云帧不与地图作匹配,而是控制采集的第一个点云帧直接转换到地图坐标系上,则在较小计算量的基础上建立初始定位能力,形成后续帧进行匹配和判定新关键帧的基准地图。
需要说明的是,所述步骤S201控制所述当前点云帧与地图作匹配的过程中,所述当前点云帧与地图中的栅格建立起映射关系以生成所述当前栅格索引集合,并提取出与所述当前点云帧存在映射关系的栅格对应的索引信息以生成当前栅格索引集合,但所述当前点云帧没有加入地图中以使得地图没有被更新,直至判定到所述当前点云帧能够成为所述关键帧。所述步骤S204将所述新的关键帧加入地图后,生成新地图,并将新地图更新为步骤S201中的地图,其中,新地图中已经存在栅格被投射入所述当前点云帧,被投射入所述当前点云帧的栅格中的位姿信息被更新为所述当前点云帧中的位姿信息,以将对应栅格中的位姿信息被更新为所述当前点云帧中的位姿信息;具体地,当前判定出的关键帧匹配到的栅格相对于上一次判定的关键帧匹配到的栅格存在新的栅格时,所述当前点云帧投射入地图后,为地图中匹配到的新的栅格添加点云帧反馈的位姿信息等,以实现对地图的更新。
作为一种实施例,控制当前点云帧与地图作匹配的方法包括:
在步骤S201中,机器人采集到一个当前点云帧时,将当前点云帧中的点云坐标信息转换为地图坐标系中的坐标信息,以建立起当前点云帧与地图中的栅格建立起映射关系;即根据点云所在的坐标系与地图坐标系之间的变换关系,将当前点云帧中的点云坐标信息换算为地图坐标系(世界坐标系)中的坐标信息,以便于投射到世界坐标系中,且尽量让当前点云帧所包括的点云与地图重叠在一起,将当前点云帧中所包括的更多点索引到地图的对应栅格,在一些实施例中,当前点云帧中的点云坐标信息转换为地图坐标系中的坐标信息是属于坐标系变换的过程,并获得离散点云,实现将当前点云帧所包括的点云对齐到预设设置的栅格地图的坐标系内,其中,坐标系变换包括旋转变换和/或平移变换,所需的旋转参数和/或平移参数构成点云帧与地图之间的匹配所需的参数,需要强调的是,当前点云帧与地图之间的每一次匹配包括一次坐标系变换,且是基于最新获得的栅格地图执行,具体的变换计算方法是现有技术,在此不再赘述。
其中,当前点云帧所包括的点云中的位置点坐标可以对应转换为栅格对应的索引值,则栅格对应的索引值是前述转换出的地图坐标系中的坐标信息,便于从该索引值中读取到栅格内部包括的位姿信息以及其它环境信息。这里的索引值可以是栅格的存储地址值,也可以是栅格的位置坐标值。
然后,根据转换后的坐标信息,将当前点云帧所包括的点云索引到地图坐标系的相应栅格中,并将当前点云帧索引到的所有栅格对应的索引信息组成所述当前栅格索引集合,所述当前栅格索引集合内每个元素是对应一个栅格的索引信息;但不在地图上改变所述当前点云帧索引到的栅格,还没开始控制所述当前点云帧对地图进行更新;因此,在本实施例中,尽管所述当前点云帧与地图中的栅格建立起映射关系以生成所述当前栅格索引集合,但是所述当前点云帧没有加入地图中,则保持地图没有被更新;只是将当前点云帧索引到的所有栅格对应的索引信息依次标记为所述当前点云帧匹配到的栅格对应的索引信息,但没有在地图上将当前点云帧索引到的栅格更新为所述当前点云帧匹配到的栅格。在本实施例中,当前点云帧与地图中的栅格之间的映射关系是由旋转变换和/或平移变换构成,便于对所述当前点云帧与原始地图的栅格区域之间的点云配准,以控制各个点云帧与原始地图的栅格区域的坐标系对齐。
在一些实施例中,当前点云帧与地图中的栅格建立起映射关系中所包括的旋转变换可以是步进旋转,保证每次旋转的角度都是在上一次旋转得到的角度的基础上再旋转一次预设的角度搜索步长得到,使得旋转变换不是每次都从初始位置开始旋转;然后将旋转变换的结果更新为地图中现存有的栅格位置信息。
需要说明的是,在控制当前点云帧与地图作匹配的过程中,至少一个点云帧索引到同一个栅格,或同一个点云帧所包括的至少一个点云点索引到同一个栅格;与当前点云帧作匹配的地图是栅格地图,以点云匹配的栅格所描述出的定位区域中的场景。空间中包括多个栅格,这些栅格可以是三维空间上的栅格,也可以是不考虑高度维度的二维空间上的栅格。一般来说,在每个栅格中,都包括不断获取到的多个点云帧的部分或全部,这多个点云帧可以是连续的,也可以是具有一定间隙的点云帧,对此本方案不做限制。
作为一种实施例,将关键帧加入地图中的方法包括:
机器人将关键帧所包括的点云变换到地图坐标系中,即机器人将所述步骤S204确定的关键帧或第一次执行步骤S201之前所确定的关键帧所包括的点云变换到地图坐标系中,获得栅格索引坐标以将当前点云帧中的点云坐标信息对齐至地图坐标系下;其中,当前点云帧所包括的点云中的位置点坐标可以对应转换为栅格对应的索引值,则栅格对应的索引值可以表示为前述栅格索引坐标,便于外部点云携带的激光信息写入该索引值对应的栅格内以更新其位姿信息以及其它环境信息,这里的索引值可以是栅格的存储地址值,也可以是栅格的位置坐标值。
在本实施例中,关键帧所包括的点云中的每个位置点都对应到一个栅格索引坐标,点云中的每个位置点可以通过旋转变换和平移变换来转换到地图坐标系下获得对应的栅格索引坐标,具体是点云中的点所索引到的栅格的坐标,被配置为与当前点云帧存在映射关系的栅格对应的索引信息。其中,步骤S204将所述关键帧加入地图中所需的坐标系变换关系可以是来源于在先执行的步骤S201中确定的关键帧(即步骤S201所述的当前点云帧)与地图中的栅格之间形成的映射关系。
在一些实施例中,每个栅格内的值代表该栅格被占用的概率;点云与栅格地图对齐理解为:激光雷达扫描到的用于表征障碍物的点云与栅格地图内障碍物栅格对齐的过程,优选地执行旋转和平移,以实现使用固定的坐标系转换关系来将点云映射为和障碍物栅格重合。
优选地,每次执行步骤S204更新出的地图会与后续采集的新的点云帧形成新的坐标系转换关系,即点云帧与地图之间的转换关系会被更新,转换关系最初是预设设定,后面,这一转换关系所需的旋转平移参数会随着地图的和点云帧的变化而变化。
然后基于所述栅格索引坐标,将关键帧所包括的点云投射入到对应的栅格索引坐标所表示的栅格中,即将点云中的点逐一索引到标记为栅格索引坐标的栅格中,并将点云所携带的激光信息更新为对应投射到的栅格内原来填充的激光信息,获得更新后的栅格,则确定使用当前点云帧对地图进行更新,且当前点云帧所包括的点云都已经投射入对应的栅格内;这里的激光信息包括位姿信息、反射光强度信息等。例如,当前判定出的关键帧匹配到的栅格相对于上一次判定的关键帧匹配到的栅格存在新的栅格时,基于栅格索引坐标为地图中匹配到的新的栅格添加对应点云反馈的位姿信息(在地图中反映障碍物占据的位置特征)和反射光强度信息(在同一地图中反映障碍物表面的材质特征)等,更新了栅格索引坐标对应表示的栅格,以实现将当前点云帧加入地图。
然后在同一地图坐标系中将更新后的栅格与未更新的栅格组成新地图,获得更新后的地图;从而在步骤S204中更新为步骤S201所述的地图,则确定将关键帧加入地图中并更新地图,即确定关键帧所包括的点云都已经落入匹配到栅格中。
本领域内的技术人员应明白,本发明实施例的实施例可提供为方法、装置、或计算机程序产品。因此,本发明实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明实施例是参照根据本发明实施例的方法、终端设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
最后,还需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。
Claims (15)
1.基于激光点云的关键帧判定方法,其特征在于,所述关键帧判定方法包括:
每当采集到一个当前点云帧,则判断当前点云帧匹配到的栅格与预设设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值,是则将当前点云帧判定为关键帧,以更新所述预先设置的关键帧匹配到的栅格对应的索引信息。
2.根据权利要求1所述关键帧判定方法,其特征在于,所述判断当前点云帧匹配到的栅格与预设设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值的方法包括:
步骤S1、控制当前点云帧与地图作匹配,获取当前栅格索引集合,再将当前栅格索引集合内的元素数量标记为当前栅格索引数量,然后执行步骤S2;当前栅格索引集合,用于表示与当前点云帧存在映射关系的栅格对应的索引信息组成的集合;栅格是设置在地图中;
步骤S2、在当前栅格索引集合内,统计与预先设置的参考栅格索引集合内的元素不相同的元素的数量,再将当前统计的数量标记为差异数量;然后执行步骤S3;其中,所述预先设置的参考栅格索引集合内的元素的数量是参考栅格索引数量;
步骤S3、若差异数量与参考栅格索引数量的比值大于预设差异阈值,则将当前点云帧判定为关键帧,其中,差异数量与参考栅格索引数量的比值用于表示所述当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异。
3.根据权利要求2所述关键帧判定方法,其特征在于,在第一次执行步骤S1之前,还包括:
将采集到的第一个点云帧设置为所述预设设置的关键帧,并将采集到的第一个点云帧加入地图中以使对应栅格被投射入第一个点云帧;然后将与第一个点云帧存在映射关系的栅格对应的索引信息所组成的集合标记为所述预先设置的参考栅格索引集合,并将与第一个点云帧存在映射关系的栅格的数量标记为所述参考栅格索引数量;
其中,所述当前点云帧不是第一个点云帧。
4.根据权利要求2所述关键帧判定方法,其特征在于,在执行步骤S3之后,将当前栅格索引集合更新为所述预先设置的参考栅格索引集合,并将当前栅格索引数量更新为所述参考栅格索引数量,则确定对所述预先设置的关键帧匹配到的栅格对应的索引信息完成更新;然后执行步骤S1。
5.根据权利要求4所述关键帧判定方法,其特征在于,步骤S3还包括:
若差异数量与参考栅格索引数量的比值小于或等于所述预设差异阈值,则不将所述当前点云帧配置为关键帧,以使所述预设设置的关键帧匹配到的栅格对应的索引信息没有发生变化;然后执行步骤S1。
6.根据权利要求5所述关键帧判定方法,其特征在于,控制当前点云帧与地图作匹配的方法包括:
在采集到一个当前点云帧时,将当前点云帧中的点云坐标信息转换为地图坐标系中的坐标信息;
根据转换过来的地图坐标系中坐标信息,将当前点云帧所包括的点云索引到地图坐标系的相应栅格中以建立起当前点云帧与地图中的栅格建立起映射关系,再将当前点云帧索引到的所有栅格对应的索引信息组成所述当前栅格索引集合,但不在地图上改变所述当前点云帧索引到的栅格,并将当前点云帧索引到的所有栅格对应的索引信息都标记为所述当前点云帧匹配到的栅格对应的索引信息;
其中,至少一个点云帧索引到同一个栅格,或同一个点云帧所包括的至少一个点云点索引到同一个栅格;与当前点云帧作匹配的地图是栅格地图。
7.根据权利要求2所述关键帧判定方法,其特征在于,所述参考栅格索引集合内的元素是所述预先设置的关键帧匹配到的栅格对应的索引信息;所述当前栅格索引集合内的元素是所述当前点云帧匹配到的栅格对应的索引信息;其中,参考栅格索引集合内的一个元素用于表示一个栅格,一个栅格携带一个索引信息,不同栅格之间对应的索引信息是不同;
在当前栅格索引集合内,与所述预先设置的参考栅格索引集合内的元素不相同的元素用于表示:当前栅格索引集合内,与所述预先设置的参考栅格索引集合内的元素的位姿信息不相同的栅格,其中,栅格对应的索引信息包括栅格中标记的位姿信息或栅格在地图中的索引号。
8.基于点云帧的地图更新方法,其特征在于,所述地图更新方法包括:
每当采集到一个当前点云帧,则判断当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异是否大于预设差异阈值,是则将当前点云帧更新到地图,否则不将当前点云帧更新到地图中。
9.根据权利要求8所述地图更新方法,其特征在于,所述地图更新方法具体包括:
步骤S1、每当采集到一个当前点云帧,则控制当前点云帧与地图作匹配,获取当前栅格索引集合,再将当前栅格索引集合内的元素数量标记为当前栅格索引数量;当前栅格索引集合,用于表示与当前点云帧存在映射关系的栅格对应的索引信息组成的集合;栅格是设置在所述地图中;
步骤S2、在当前栅格索引集合内,统计与预先设置的参考栅格索引集合内的元素不相同的元素的数量,再将当前统计的数量标记为差异数量;其中,参考栅格索引集合内的元素的数量是参考栅格索引数量;
步骤S3、若差异数量与参考栅格索引数量的比值大于预设差异阈值,则将当前点云帧判定为新的关键帧,再将新的关键帧加入地图中,生成新地图,则确定将当前点云帧更新到地图中;
步骤S4、若差异数量与参考栅格索引数量的比值小于或等于预设差异阈值,则不将所述当前点云帧更新到地图中;
其中,差异数量与参考栅格索引数量的比值用于表示所述当前点云帧匹配到的栅格与预先设置的关键帧匹配到的栅格之间的差异。
10.根据权利要求9所述地图更新方法,其特征在于,在第一次执行步骤S1之前,还包括:
将采集到的第一个点云帧设置为所述预设设置的关键帧,并将采集到的第一个点云帧加入地图坐标系中以使对应栅格被投射入第一个点云帧,生成原始地图;再将与第一个点云帧存在映射关系的栅格对应的索引信息所组成的集合标记为所述预先设置的参考栅格索引集合,并将与第一个点云帧存在映射关系的栅格的数量标记为所述参考栅格索引数量;
其中,所述当前点云帧不是第一个点云帧。
11.根据权利要求9所述地图更新方法,其特征在于,所述步骤S1控制所述当前点云帧与地图作匹配的过程中,所述当前点云帧与地图中的栅格建立起映射关系,并提取出与所述当前点云帧存在映射关系的栅格对应的索引信息以生成当前栅格索引集合,但所述当前点云帧没有加入地图中以使得地图没有被更新;
所述步骤S3将所述新的关键帧加入地图后,生成新地图,并将新地图更新为步骤S1中的地图,其中,新地图中已经存在栅格被投射入所述当前点云帧,以将对应栅格中的位姿信息被更新为所述当前点云帧中的位姿信息。
12.根据权利要求11所述地图更新方法,其特征在于,在执行步骤S3之后,将当前栅格索引集合更新为所述预先设置的参考栅格索引集合,并将当前栅格索引数量更新为所述参考栅格索引数量,则确定对所述预先设置的关键帧匹配到的栅格对应的索引信息完成更新;然后执行步骤S1以控制后续采集的点云帧与上一次执行的步骤S3中更新过的地图作匹配,其中,步骤S3生成的新地图被更新为后续执行的步骤S1中的地图。
13.根据权利要求12所述地图更新方法,其特征在于,控制当前点云帧与地图作匹配的方法包括:
在采集到一个当前点云帧时,将当前点云帧中的点云坐标信息转换为地图坐标系中的坐标信息,以建立起当前点云帧与地图中的栅格之间的映射关系;
根据转换后的坐标信息,将当前点云帧所包括的点云索引到地图坐标系的相应栅格中,再将当前点云帧索引到的所有栅格对应的索引信息组成所述当前栅格索引集合,但不控制所述当前点云帧加入地图中;
其中,至少一个点云帧索引到同一个栅格,或同一个点云帧所包括的至少一个点云点索引到同一个栅格;与当前点云帧作匹配的地图是栅格地图。
14.根据权利要求10或13所述地图更新方法,其特征在于,将关键帧加入地图中的方法包括:
将关键帧所包括的点云变换到地图坐标系中,获得栅格索引坐标,以实现将当前点云帧中的点云坐标信息对齐至地图坐标系下;其中,关键帧所包括的点云中的每个位置点都对应到一个栅格索引坐标,形成所述索引信息;
然后将关键帧所包括的点云投射入到对应的栅格索引坐标所表示的栅格中,并将点云所携带的激光信息更新为对应投射到的栅格内原来填充的激光信息,获得更新后的栅格;然后在同一地图坐标系中将更新后的栅格与未更新的栅格组成新地图,则确定将关键帧加入地图中并更新地图。
15.根据权利要求9所述地图更新方法,其特征在于,在步骤S2中,所述参考栅格索引集合内的元素是预先设置的关键帧匹配到的栅格对应的索引信息;所述当前栅格索引集合内的元素是当前点云帧匹配到的栅格对应的索引信息;其中,参考栅格索引集合内的一个元素用于表示一个栅格,一个栅格携带一个索引信息,不同栅格之间对应的索引信息是不同;
在当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素不相同的元素用于表示:当前栅格索引集合内,与预先设置的参考栅格索引集合内的元素的位姿信息不相同的栅格,其中,栅格对应的索引信息包括栅格中标记的位姿信息或栅格在地图中的索引号。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211472949.2A CN118049985A (zh) | 2022-11-17 | 2022-11-17 | 基于激光点云的关键帧判定方法及地图更新方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211472949.2A CN118049985A (zh) | 2022-11-17 | 2022-11-17 | 基于激光点云的关键帧判定方法及地图更新方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN118049985A true CN118049985A (zh) | 2024-05-17 |
Family
ID=91047130
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211472949.2A Pending CN118049985A (zh) | 2022-11-17 | 2022-11-17 | 基于激光点云的关键帧判定方法及地图更新方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN118049985A (zh) |
-
2022
- 2022-11-17 CN CN202211472949.2A patent/CN118049985A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111492265B (zh) | 基于3d激光雷达测量的多分辨率、同时定位和地图绘制 | |
Kriegel et al. | Efficient next-best-scan planning for autonomous 3D surface reconstruction of unknown objects | |
Rottensteiner et al. | The ISPRS benchmark on urban object classification and 3D building reconstruction | |
CN111429574A (zh) | 基于三维点云和视觉融合的移动机器人定位方法和系统 | |
CN111563450B (zh) | 数据处理方法、装置、设备及存储介质 | |
CN115451977A (zh) | 车道标注数据的获取方法、计算机设备及存储介质 | |
CN112346463B (zh) | 一种基于速度采样的无人车路径规划方法 | |
CN112464812B (zh) | 一种基于车辆的凹陷类障碍物检测方法 | |
CN112904358B (zh) | 基于几何信息的激光定位方法 | |
KR20230137395A (ko) | 3차원 지도 구축 방법 및 장치 | |
CN112329846A (zh) | 激光点云数据高精度标注方法及系统、服务器及介质 | |
CN111856499B (zh) | 基于激光雷达的地图构建方法和装置 | |
CN115407364A (zh) | 点云地图处理方法、车道标注数据获取方法、设备及介质 | |
CN113778096B (zh) | 室内机器人的定位与模型构建方法及系统 | |
CN114565726A (zh) | 一种非结构化动态环境下的同时定位与建图方法 | |
CN111780744B (zh) | 移动机器人混合导航方法、设备及存储装置 | |
CN116817891A (zh) | 一种实时多模态感知的高精地图构建方法 | |
CN111781606A (zh) | 一种新型激光雷达和超声波雷达融合的小型化实现方法 | |
Dehbi et al. | Improving gps trajectories using 3d city models and kinematic point clouds | |
CN118049985A (zh) | 基于激光点云的关键帧判定方法及地图更新方法 | |
CN115390088A (zh) | 点云地图建立方法、车道标注数据获取方法、设备及介质 | |
CN112505723B (zh) | 一种基于导航点选择的三维地图重建方法 | |
CN113433568B (zh) | 一种激光雷达观测模拟方法及装置 | |
CN114488026A (zh) | 基于4d毫米波雷达的地下停车库可通行空间检测方法 | |
CN115019167B (zh) | 基于移动终端的融合定位方法、系统、设备及存储介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination |