专利交易市场

本发明公开了一种基于FMCW速度的高精度定位建图方法、系统及存储介质，包括：采集点云数据和惯性数据；S2、估计对应帧移动平台的FMCW速度；S3、进行预积分融合获得第k+1帧相对第k帧运动的FMCW‑IMU预积分模型；S4、获取第k帧和第k+1帧之间的残差项；S5、最小化第k帧和第k+1帧之间的残差项中各项平方的和，获得对应帧的最大后验估计；S6、根据第k帧的最大后验估计获得第k+1帧的位姿参数以形成第k帧到第k+1帧的转换矩阵，并将新获取的第k+1帧的点云数据通过转换矩阵转换后添加到地图中完成重建。该方法能够解决因惯导漂移带来的累积误差问题，提高建图的定位精度和鲁棒性。