一种基于FMCW速度的高精度定位建图方法、系统及存储介质技术方案

本发明专利技术公开了一种基于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帧的点云数据通过转换矩阵转换后添加到地图中完成重建。该方法能够解决因惯导漂移带来的累积误差问题，提高建图的定位精度和鲁棒性。

【技术实现步骤摘要】

本专利技术属于同时定位与建图，具体涉及一种基于fmcw速度的高精度定位建图方法、系统及存储介质。

技术介绍

1、同时定位与建图（simultaneous localization and mapping，slam）是机器人技术和测量领域一个核心挑战，也是无人系统实现智能感知的重要基础。目前，定位与导航技术主要依赖全球定位系统(global navigation satellite system，gnss)，但gnss性能在信号受限的环境（如室内、隧道和丛林）中表现不足，难以满足复杂场景的需求。相比之下，slam技术能够在无外部信号的依赖情况下，通过传感器自主感知环境信息，实现高精度的位姿估计和地图构建。这使得slam技术成为机器人导航和路径规划的关键。

2、slam技术的核心在于实时估计传感器的位姿，同时构建环境的地图模型。位姿估计包括位置和方向，其精确性直接决定了系统的定位能力。随着传感器技术的不断进步，slam正朝着多传感器融合的方向发展。如当前slam系统通过融合多种传感器（如imu、激光雷达、陀螺仪、加速度计、相机和轮速计），助力提升系本文档来自技高网...

【技术保护点】

1.一种基于FMCW速度的高精度定位建图方法，应用于移动平台，其特征在于：所述移动平台包括FMCW激光雷达和惯性传感器，所述基于FMCW速度的高精度定位建图方法包括如下步骤：

2.如权利要求1所述的基于FMCW速度的高精度定位建图方法，其特征在于：所述根据第k帧点云数据和目标函数估计对应帧移动平台的FMCW速度，具体如下：

3.如权利要求2所述的基于FMCW速度的高精度定位建图方法，其特征在于：所述绝对残差公式如下：

4.如权利要求2所述的基于FMCW速度的高精度定位建图方法，其特征在于：所述计算二维矩阵中各元素点的理论多普勒速度和点云数据中的实测多普...

【技术特征摘要】

1.一种基于fmcw速度的高精度定位建图方法，应用于移动平台，其特征在于：所述移动平台包括fmcw激光雷达和惯性传感器，所述基于fmcw速度的高精度定位建图方法包括如下步骤：

2.如权利要求1所述的基于fmcw速度的高精度定位建图方法，其特征在于：所述根据第k帧点云数据和目标函数估计对应帧移动平台的fmcw速度，具体如下：

3.如权利要求2所述的基于fmcw速度的高精度定位建图方法，其特征在于：所述绝对残差公式如下：

4.如权利要求2所述的基于fmcw速度的高精度定位建图方法，其特征在于：所述计算二维矩阵中各元素点的理论多普勒速度和点云数据中的实测多普勒速度之间的绝对残差前，还对二维矩阵中的元素点进行筛除，具体如下：

5.如权利要求1所述的基于fmcw速度的高精度定位建图方法，其特征在于：所述最小化第k帧和第k+1帧之间的残差项中各项平方的和，获得对应帧的最大后验估计，则第k帧的最大后验估计公式如下：

6.如权利要求1所述的基于fmcw速度的高精度定位建图方法，其特征在于：所述第k+1帧相对第k帧运动的...

【专利技术属性】
技术研发人员：杨哲，蒋晨豪，吴昊泽，李燕君，陈楠，
申请(专利权)人：浙江工业大学，
类型：发明
国别省市：