定位与建图是自动驾驶的关键技术之一.激光传感器或视觉传感器具有局限性,通过多传感器融合可以发挥不同传感器各自的优点,提高定位与建图的精度和鲁棒性.通过优化Harris算法对图像进行角点提取,利用关键帧对特征点匹配算法进行优化,然后利用非线性最小二乘法进行后端优化.通过试验平台进行定位与建图试验,对算法进行验证,并用EVO工具对定位误差进行分析.结果表明,提出后端优化算法误差比单一传感器定位误差减少13%.