基于滤波与图优化的定位与建图系统

针对室外大场景环境建图精度不高,地图出现重影和漂移等问题,提出一种融合滤波与图优化理论实时定位与建图系统.该系统由点云数据预处理、基于滤波紧耦合惯性里程计和后端位姿图优化等3部分构成.首先,点云数据预处理采用随机采样一致性算法分割地面,并提取地面模型参数构建后端优化中的地面约束因子.然后,前端紧耦合惯性里程计采用迭代误差状态卡尔曼滤波,以激光里程计作为观测值,IMU预积分结果作为预测值,通过构建联合函数,滤波融合得到较为精准的激光惯导里程计.最后,后端结合图优化理论引入闭环因子、地面约束因子以及帧与图匹配的里程计因子作为约束条件,构建因子图并优化地图位姿.其中闭环因子采用改进的扫描文本的闭环检测算法进行位置识别,可以降低环境误识别率.所提出的算法在室外厂区楼栋,停车场以及室内车间等多个场景完成场景建图,在距离、水平和高程3个方向的累积偏差均控制10 cm左右,能够有效解决地图的重影和漂移问题,具有高鲁棒性和高精度.