基于IMU与激光雷达并结合因子图的四足机器人大场景定位与建图方法

本发明公开了一种基于IMU与激光雷达并结合因子图的四足机器人大场景定位与建图方法，包括：数据采集步骤，利用IMU采集四足机器人惯性测量数据，同时利用激光雷达采集点云数据；数据预处理步骤，对所述IMU采集的四足机器人惯性测量数据进行预积分处理，以获取机器人的位姿和速度初步估计；之后对所述激光雷达采集的点云数据进行畸变校正、最近邻匹配和点云配准；因子图构建步骤，基于处理后的IMU数据和激光雷达数据构建因子图，通过高斯‑牛顿法对构建的因子图进行优化求解，以最小化因子图中所有因子联合概率的负对数为目标，得到机器人位姿最优估计。本发明可以有效地用于实现足式机器人大场景下的定位与建图。