一种基于误差状态卡尔曼滤波算法的点云地图构建方法

本发明提供了一种基于误差状态卡尔曼滤波算法的点云地图构建方法，包括获取激光里程计采集的第一数据，以及视觉里程计采集的第二数据。对第一数据和第二数据进行数据同步关联，得到关联后第一数据和关联后第二数据。采用误差状态卡尔曼滤波对关联后第一数据和关联后第二数据进行融合，得到融合里程计。采用GTSAM库的因子图对融合里程计进行非线性优化，得到优化后里程计。将优化后里程计与多种传感器数据结合，得到多源点云地图。非线性优化能更好的在农业颠簸环境中减少前端带来的误差，提高融合里程计的精度和鲁棒性。采用因子图进行非线性优化，可以解决大规模非线性优化问题，多源点云地图为自动化作业、环境监测以及精准农业管理提供了支持。