点云强度辅助下的固态激光雷达与IMU鲁棒紧耦合系统

本发明提出了点云强度辅助下的固态激光雷达与IMU鲁棒紧耦合系统，首先，系统将对固态激光雷达点云进行预处理，剔除点云瑕疵点，对点云强度值进行校正并下采样。然后对预处理后的点云进行几何特征提取和强度特征提取，并进行激光雷达点云的强度校准。随后利用IMU预测的高频先验信息对点云的运动进行补偿。对于几何和强度特征点云，分别采用经典配准和快速平面拟合方法计算残差，并构建强度特征辅助的点云残差量测模型。并在滤波算法中利用残差估计值，设计了基于IGGⅢ鲁棒权函数的抗差策略。本发明在保持较低计算成本的前提下，提高了小视角激光雷达紧耦合系统在复杂环境中的定位和建图能力及其鲁棒性。