一种用于大型场景的实时栅格地图生成方法
技术领域
本发明涉及自动驾驶技术领域,尤其涉及一种用于大型场景的实时栅格地图生成方法。
背景技术
为清扫、搬运等任务设计的低速自动驾驶车辆逐步成熟。此类车辆可以完成一些特定的高强度、高重复性任务,取代这些任务当中的人力占比。同时,低速自动驾驶车辆具有易管理、高效率、高安全性等优势,在未来几年将打开市场。
对于低速自动驾驶车辆来说,即时定位与地图构建技术是它的核心基础技术之一。为了实现在各种环境内的自动驾驶,需要提供能够简单、快速地构建的场景地图的方法,进而基于地图实现区域划分、路径规划、示教清扫等上层任务。
即时定位与地图构建技术着重于构建车辆的场景地图。由于目标环境限制,现有的即时定位与地图构建技术仅适用于一、二百平方米的室内环境,且通常把整个地图表示成一张栅格地图。
但构建大型场景时,单一的地图很容易出现扭曲、歪斜等与现实场景不一致的情况,而且这种情况很难被修正。
发明内容
本发明的目的是针对现有技术的缺陷,提供一种用于大型场景的实时栅格地图生成方法,能够对大型场景进行地图构建,并且在地图出现偏差时能够快速修正。
为实现上述目的,本发明提供了一种用于大型场景的实时栅格地图生成方法,所述实时栅格地图生成方法包括:
实时获取机器的线速度数据、角速度数据、加速度数据和三维点云数据;
根据所述线速度数据、角速度数据和加速度数据进行航迹推算处理,得到当前所述机器的位姿信息;
根据所述位姿信息查找历史子地图;
当存在所述历史子地图时,根据所述位姿信息确定当前所述机器所在的第一子地图,并根据所述第一子地图建立似然场模型;
根据所述似然场模型和三维点云数据确定所述机器在所述第一子地图中的子地图位姿信息;
根据所述子地图位姿信息和预设距离数据确定关键帧位姿信息,和/或根据所述子地图位姿信息和预设转角数据确定关键帧位姿信息;
根据所述关键帧位姿信息对各个子地图进行拼接,得到栅格地图。
优选的,所述根据所述位姿信息确定当前所述机器所在的第一子地图具体包括:
统计所述历史子地图的数量;
当所述历史子地图的数量为1时,确定所述历史子地图为当前所述机器所在的第一子地图;
当所述历史子地图的数量大于1时,获取各个历史子地图的中心点信息;
根据所述中心点信息和位姿信息计算得到所述机器与各个历史子地图之间的距离数据,并确定最小距离数据对应的历史子地图为第一子地图。
优选的,当不存在所述历史子地图时,所述实时栅格地图生成方法还包括:
根据预设子地图尺寸信息和三维点云数据生成第一子地图,并确定所述机器在所述第一子地图中的子地图位姿信息;
根据所述子地图位姿信息和预设距离数据确定关键帧位姿信息,和/或根据所述子地图位姿信息和预设转角数据确定关键帧位姿信息;
根据所述关键帧位姿信息对各个子地图进行拼接,得到栅格地图。
优选的,所述根据所述似然场模型和三维点云数据确定所述机器在所述第一子地图中的子地图位姿信息具体包括:
将所述三维点云数据投影至所述似然场模型,得到二维点云数据;
根据所述二维点云数据和似然场模型构建误差函数,并利用最小二乘法对所述误差函数进行最小化处理,得到所述机器在所述第一子地图中的子地图位姿信息。
优选的,在所述根据所述子地图位姿信息和预设距离数据确定关键帧位姿信息,和/或根据所述子地图位姿信息和预设转角数据确定关键帧位姿信息之后,所述实时栅格地图生成方法还包括:
根据所述第一子地图的信息对所述关键帧位姿信息进行存储,得到第一子地图的历史关键帧位姿信息;
根据第一子地图历史关键帧位姿信息利用粒子滤波算法修正所述第一子地图;
根据所述关键帧位姿信息对各个修正后的子地图进行拼接,得到栅格地图。
进一步优选的,实时判断所述关键帧位姿信息是否是历史关键帧位姿信息;
当所述关键帧位姿信息是任一历史关键帧位姿信息时,确定所述历史关键帧位姿信息为第一历史关键帧位姿信息;
判断所述关键帧位姿信息与第一历史关键帧位姿信息是否匹配;
当所述关键帧位姿信息与第一历史关键帧位姿信息不匹配时,根据所述关键帧位姿信息和第一历史关键帧位姿信息确定累计偏差数据;
根据所述累计偏差数据进行位姿图优化,得到修正关键帧位姿信息;
根据所述修正关键帧位姿信息利用粒子滤波算法修正所述第一子地图。
进一步优选的,所述根据所述累计偏差数据进行位姿图优化,得到修正关键帧位姿信息具体包括:
提取所述第一历史关键帧位姿信息和所述关键帧信息之间的关键帧位姿信息,得到偏差关键帧位姿信息;
根据所述偏差参数和偏差关键帧位姿信息确定每个偏差关键帧位姿信息的偏差参数;
根据各所述偏差参数调整所述偏差关键帧位姿信息,得到修正关键帧位姿信息。
进一步优选的,根据所述修正关键帧位姿信息对各个子地图进行拼接,修正所述栅格地图。
本发明实施例提供的用于大型场景的实时栅格地图生成方法,根据采集数据生成子地图,再由多个子地图拼接生成栅格地图,同时利用粒子滤波算法消除大型场景下的累计偏差,通过消除偏差后关键帧位姿信息灵活调整各子地图之间的相对位置关系,从而修正栅格地图。本发明在生成栅格地图的过程中,不需要不断扩展一张单一地图,只需要拼接子地图即可,发生误差时也不需要重复刷新整张地图,调整各个子地图的相对位置关系即可,具备更好的灵活性,对一些局部震动、动态物体具备一定的抗干扰能力。
附图说明
图1为本发明实施例提供的用于大型场景的实时栅格地图生成方法的流程图;
图2为本发明实施例提供的二维点云数据构成的子地图;
图3为本发明实施例提供的似然场构成的子地图;
图4为本发明实施例提供的修正后的栅格地图的示意图。
具体实施方式
下面通过附图和实施例,对本发明的技术方案做进一步的详细描述。
本发明提供的用于大型场景的实时栅格地图生成方法,根据采集数据生成子地图,再由多个子地图拼接生成栅格地图,同时利用粒子滤波算法消除大型场景下的累计偏差,通过消除偏差后关键帧位姿信息灵活调整各子地图之间的相对位置关系,从而修正栅格地图。本发明在生成栅格地图的过程中,不需要不断扩展一张单一地图,只需要拼接子地图即可,发生误差时也不需要重复刷新整张地图,调整各个子地图的相对位置关系即可,具备更好的灵活性,对一些局部震动、动态物体具备一定的抗干扰能力。
图1为本发明实施例提供的用于大型场景的实时栅格地图生成方法的流程图,以下结合图1对本发明技术方案进行详述。
步骤110,实时获取机器的线速度数据、角速度数据、加速度数据和三维点云数据;
具体的,机器的轮速计实时输出车轮的线速度数据,惯性测量单元实时输出角速度数据和加速度数据,激光雷达实时输出三维点云数据。其中,线速度数据、角速度数据、加速度数据和三维点云数据具有时间属性。机器可以是一个,也可以是多个。最终栅格地图是对机器生成的多个局部的子地图的拼接。
步骤120,根据线速度数据、角速度数据和加速度数据进行航迹推算处理,得到当前机器的位姿信息;
具体的,航迹推算是利用已知的机器初始位置,根据运动载体在该点的航向、航速和航行时间,实时推算下一时刻的坐标位置的一种导航定位方法。它是一种自主式定位,其定位精度不会受到如电磁干扰、遮挡等外界因素的影响。但是,航迹推算系统不具有长期的稳定性,必须每隔一段时间进行误差校正。
本发明根据当前之前时刻线速度数据、角速度数据和加速度数据进行航迹推算处理,得到当前机器在物理坐标系下的位姿信息。
步骤130,根据位姿信息查找历史子地图;
具体的,查找范围包括该位姿信息的历史子地图。
步骤140,当存在历史子地图时,根据位姿信息确定当前机器所在的第一子地图,并根据第一子地图建立似然场模型;
具体的,统计历史子地图的数量。当历史子地图的数量为1时,确定历史子地图为当前机器所在的第一子地图。为了便于对子地图的拼接,本发明中的各子地图之间具有重叠区域。当当前机器的位姿信息处于几个子地图的重叠区域时,即当历史子地图的数量大于1时,获取各个历史子地图的中心点信息。根据中心点信息和位姿信息计算得到机器与各个历史子地图之间的距离数据,并确定最小距离数据对应的历史子地图为第一子地图。
也就是说,当当前机器的位姿信息处于几个子地图的重叠区域时,选取距离当前机器的位姿信息的最近的历史子地图为第一子地图。
图3为本发明实施例提供的似然场构成的子地图,如图3所示,似然场模型利用已知的样本结果,反推最有可能导致这样结果的参数值,表达激光末端点的概率。
步骤141,根据似然场模型和三维点云数据确定机器在第一子地图中的子地图位姿信息;
具体的,图2为本发明实施例提供的二维点云数据构成的子地图,如图2所示,将三维点云数据投影至似然场模型,得到二维点云数据。根据二维点云数据和似然场模型构建误差函数,并利用最小二乘法对误差函数进行最小化处理,得到机器在第一子地图中的子地图位姿信息。
机器在第一子地图中的子地图位姿信息就是根据二维点云数据输入似然场模型后,使得似然值最大的参数。
步骤150,当不存在历史子地图时,根据预设子地图尺寸信息和三维点云数据生成第一子地图,并确定机器在第一子地图中的子地图位姿信息;
具体的,当不存在历史子地图时,将三维点云数据投影处理,得到二维点云数据,根据预设子地图尺寸信息筛选二维点云数据,根据筛选结果生成第一子地图。
步骤160,根据子地图位姿信息和预设距离数据确定关键帧位姿信息,和/或根据子地图位姿信息和预设转角数据确定关键帧位姿信息;
具体的,关键帧位姿信息是均匀分布在机器运动轨迹上的点。
在一个具体的例子中,下一个关键帧位姿信息是相对于上一个关键帧位姿信息,位移变化1米和/或旋转角度变化15°确定的。
步骤170,根据关键帧位姿信息对各个子地图进行拼接,得到栅格地图。
具体的,根据关键帧位姿信息对各个子地图的部分区域进行重叠,从而实现各子地图的拼接,得到栅格地图。
为保证第一子地图的准确度,在根据子地图位姿信息和预设距离数据确定关键帧位姿信息,和/或根据子地图位姿信息和预设转角数据确定关键帧位姿信息之后,根据第一子地图的信息对关键帧位姿信息进行存储,得到第一子地图的历史关键帧位姿信息。根据第一子地图历史关键帧位姿信息利用粒子滤波算法修正第一子地图。根据关键帧位姿信息对各个修正后的子地图进行拼接,得到栅格地图。
为了消除栅格地图的累计误差,本发明采用回环检测,实时判断关键帧位姿信息是否是历史关键帧位姿信息。当关键帧位姿信息是任一历史关键帧位姿信息时,确定历史关键帧位姿信息为第一历史关键帧位姿信息。判断关键帧位姿信息与第一历史关键帧位姿信息是否匹配。这里的匹配主要指的是位姿信息中的位置信息是否匹配,判断依据是航迹推算结果。
当关键帧位姿信息与第一历史关键帧位姿信息不匹配时,根据关键帧位姿信息和第一历史关键帧位姿信息确定累计偏差数据。根据累计偏差数据进行位姿图优化,得到修正关键帧位姿信息。根据修正关键帧位姿信息利用粒子滤波算法修正第一子地图。
其中,根据累计偏差数据进行位姿图优化,得到修正关键帧位姿信息具体包括:提取第一历史关键帧位姿信息和关键帧信息之间的关键帧位姿信息,得到偏差关键帧位姿信息。根据偏差参数和偏差关键帧位姿信息确定每个偏差关键帧位姿信息的偏差参数。根据各偏差参数调整偏差关键帧位姿信息,得到修正关键帧位姿信息。根据修正关键帧位姿信息对各个子地图进行拼接,修正栅格地图。
也就是说,当机器在行驶过程中再次到达之前所经过的点时,理论上机器两次经过该点的位置信息应该相同,但是由于测量或者算法的原因,导致先后经过该点的位置信息不同,则表明在这段时间内的轨迹出现偏差。此时应当对两次经过该点之间的关键帧位置信息进行修正。
在一个具体的例子中,机器第一次经过A点的位置信息为(10,10),第二次经过A点的位置信息为(10,13),两次经过A点的位置信息不同,表明从A点到A点的这段过程中出现累计偏差,向上偏移3。已知这段过程之间存在100个关键帧位姿信息,则可以将累积偏差3均摊至各个关键帧位姿信息,也就是每个关键帧位置出现向上偏移0.03的偏差,再根据该偏差分别对每个关键帧位姿信息进行修正。
图4为本发明实施例提供的修正后的栅格地图的示意图,图4是由多个图3的子地图拼接构成的,图4中连续点指的是关键帧位姿信息,多个关键帧位姿信息构成机器的运行轨迹。
在优选的方案中,也可以采用分值界定法消除累计误差。
本发明的用于大型场景的实时栅格地图生成方法,根据采集数据生成子地图,再由多个子地图拼接生成栅格地图,同时利用粒子滤波算法消除大型场景下的累计偏差,通过消除偏差后关键帧位姿信息灵活调整各子地图之间的相对位置关系,从而修正栅格地图。本发明在生成栅格地图的过程中,不需要不断扩展一张单一地图,只需要拼接子地图即可,发生误差时也不需要重复刷新整张地图,调整各个子地图的相对位置关系即可,具备更好的灵活性,对一些局部震动、动态物体具备一定的抗干扰能力。
专业人员应该还可以进一步意识到,结合本文中所公开的实施例描述的各示例的单元及算法步骤,能够以电子硬件、计算机软件或者二者的结合来实现,为了清楚地说明硬件和软件的可互换性,在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块,或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上的具体实施方式,对本发明的目的、技术方案和有益效果进行了进一步详细说明,所应理解的是,以上仅为本发明的具体实施方式而已,并不用于限定本发明的保护范围,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。