本技术涉及一种位姿优化栅格地图更新技术,包括处理测距传感器捕获的载具视野场景点云数据和IMU传感器提供的载具位姿数据,实现时间同步以获得当前帧载具的精确信息。
背景技术
随着自动驾驶和自主移动机器人技术的发展、新的应用场景的开发,对原有技术提出了新的要求。
其中,随着无人驾驶车辆活动范围的增大,行驶速度的大幅提高,对SLAM(Simultaneous Localization and Mapping)技术来讲,汽车等载具必须具备实时定位及大规模建图的能力,例如以下现有的技术方案:
现有技术1:《DLO:Direct LiDAR Odometry for 2.5D Outdoor Environment》发表了一种基于2.5D栅格地图的帧间匹配技术,该技术从视觉的光流直接法中得到启示,假定同一位置的地面点在不同的时刻具有相同的高程值,并借鉴概率栅格地图的思想对场景进行建模,但不同的是,栅格中不再存储是否被占有的概率值,而是存储与其位置对应的地面点的高程值,并以当前帧的高程值与历史帧中相同位置处高程值的差值为代价构建最小二乘模型,对载具的六维姿态进行优化。但该技术并未意识到载具的六个自由度对栅格中高程值具有不同的影响方式,六个自由度并未做拆分处理,优化变量具有较高的维度,会增加优化算法的迭代次数,影响程序运行效率;其次,该技术中并未消去动态障碍物的影响;最后,该技术对于大规模场景中可能会出现的栅格地图存储效率问题并未解决。
现有技术2:《A Universal Grid Map Library:Implementation and Use Casefor Rough Terrain Navigation》发表了一种构建2.5D栅格地图的技术,地图移动时,该技术可以实现内部数据的快速更新,同时该技术可以提供多个数据层,可以在地图中存储不同类型的数据,最后,该技术提供了与现有的多种数据类型的兼容接口,可实现数据类型的快速转换。但是该技术也未解决大规模场景中栅格地图存储效率问题。
当前3D激光SLAM中前端的帧间匹配算法可以分为两类:1.基于点集,例如ICP、NDT等经典算法及其变种;2.基于特征点,例如LOAM中的线、面特征等。学术上已经对各类算法进行了相当深入的研究,对算法的特点及优势产生了相当成熟的认识,但是在工程应用中,每种方案或多或少都会暴露出一定的问题。
例如ICP算法,ICP配准过程是以相邻两帧点云对应点对间的距离为代价,迭代地找到两帧点云之间最优的转换矩阵。首先,为确定两帧点云中各点的对应关系,需循环处理每个点,这将导致较高的计算成本;其次,算法受噪声及异常值的影响较大,数据处理不好,很容易导致算法无法收敛或者陷入局部最优;最后,算法强烈依赖于位姿初值,如果初值给的不够好,也很容易导致优化陷入局部最优。
为了避免ICP中大量点间的匹配,降低计算成本,基于特征点的配准方法(通过在当前点云中提取具有代表性的点,然后建模两帧点云中特征点间的对应关系),越来越受欢迎。然而,特征点本身的提取就会耗费一定的时间,另外,在场景比较单一的地方,很难提取到合适的特征点,从而导致场景退化等问题;最后,由于视点变化带来的特征点间的误匹配,也会对位姿优化带来较大的影响。
实现思路