AprilTag检测辅助的激光雷达建图与定位方法

本发明公开了一种AprilTag检测辅助的激光雷达建图与定位方法。该方法首先通过IMU数据估计重力方向，并结合IMU和里程计数据估计机器人初始位姿；根据重力方向将激光雷达点云数据投影到2D世界，并根据初始位姿在当前栅格子地图下进行局部扫描匹配；对相机数据进行去畸变，并在机器人位姿接近预先设定的固定点位时进行AprilTag检测，以获取机器人的精确位姿；同时，使用投影后的点云数据，对所有栅格子地图定期执行闭环检测以获取机器人精确位姿；根据AprilTag检测或闭环检测结果，每当有新的精确位姿时进行位姿优化；根据局部扫描匹配结果或位姿优化结果更新栅格子地图，当子地图的数据容量达到上限时生成新的子地图，并利用各子地图坐标系间的位姿关系构建全局地图。本发明通过相机AprilTag检测和闭环检测，修正了激光雷达建图与定位过程中的累积误差，显著提高定位系统的精度和鲁棒性，实时性高，适用于各种机器人和自动化系统的导航与定位。