基于4D毫米波雷达的位姿图SLAM计算方法及系统
知识产权简介
本发明提供了一种基于4D毫米波雷达的位姿图SLAM算法。该算法包括三个模块：雷达点云滤波、自车速度估计和位姿图优化。针对4D毫米波雷达（4D雷达）点云噪声较大的问题，设计了一个滤波步骤来减少原始4D雷达点云中的鬼影点和随机噪声；接着，从滤波后的点云的多普勒速度中估计出自车速度，这在接下来的位姿图优化中发挥了重要作用；在位姿图优化中，通过基于正态分布变换的点云配准实现雷达里程计，估计相对位姿变换；为了在点云稀疏的情况下达到更好的精度，将估计的自车速度进行预积分，获得额外的相对位姿估计；此外，引入了回环检测，识别重新到达过的地方，从全局的角度优化历史位姿，以减少累积漂移，实现精确、鲁棒的建图和定位。