基于道路高程图前馈的机器人定位与建图方法及系统

本发明提供了一种基于道路高程图前馈的机器人定位与建图方法及系统。其中方法包括：步骤S1：接收外部传感器数据，利用径向基函数拟合前方道路情况，构建前方道路的高程图信息；步骤S2：接收内部传感器数据，结合高程图信息构建优化问题，计算机器人帧间的位姿变换；步骤S3：构建位姿因子图，在后台同时优化机器人的总体位姿信息和全局地图的匹配精度；步骤S4：建立回环检测进程，通过检测机器人移动闭环，优化机器人的总体位姿和全局地图。本发明在移动机器人的传统的建图与定位算法中融入了道路高度信息作为前馈，特别是在存在地形起伏的场景中，该方法提高了算法的精度。