[发明专利]GNSS-SLAM初始化方法、系统、移动机器人及存储介质

权利要求书

1.一种GNSS-SLAM初始化方法，其特征在于，所述方法包括：

以IMU坐标系为本体坐标系，对GNSS、轮式里程计、激光雷达进行标定，并获取标定后的相对位姿关系；

获取IMU数据、GNSS数据、轮式里程计数据、激光雷达数据，并根据所述相对位姿关系将所述轮式里程计数据、所述激光雷达数据转换到所述IMU坐标系下；

在所述IMU坐标系下，根据所述IMU数据、所述轮式里程计数据、所述激光雷达数据得到局部激光雷达里程计轨迹数据；

对所述GNSS数据进行预处理，并根据预处理后的GNSS数据、所述局部激光雷达里程计轨迹数据构建目标函数；

根据所述目标函数调整参数，直到完成初始化。

2.根据权利要求1所述的GNSS-SLAM初始化方法，其特征在于，以IMU坐标系为本体坐标系，对GNSS、轮式里程计、激光雷达进行标定，包括：

以IMU坐标系为本体坐标系，标定所述轮式里程计、IMU的内参数据；

以IMU坐标系为本体坐标系，标定所述轮式里程计、所述IMU、所述GNSS、所述激光雷达的外参数据。

3.根据权利要求1所述的GNSS-SLAM初始化方法，其特征在于，在所述IMU坐标系下，根据所述IMU数据、所述轮式里程计数据、所述激光雷达数据得到局部激光雷达里程计轨迹数据，包括：

在所述IMU坐标系下，对所述IMU数据、所述轮式里程计数据进行融合，得到相对运动；

根据所述相对运动，对所述激光雷达数据进行畸变矫正，得到矫正后的激光雷达数据；

根据所述IMU数据、所述轮式里程计数据、所述矫正后的激光雷达数据得到局部激光雷达里程计轨迹数据。

4.根据权利要求3所述的GNSS-SLAM初始化方法，其特征在于，根据所述IMU数据、所述轮式里程计数据、所述矫正后的激光雷达数据得到局部激光雷达里程计轨迹数据，包括：

根据所述IMU数据构建帧间IMU预积分残差∑||imu||²；

根据所述轮式里程计数据构建帧间物理里程计域积分残差∑||odom||²；

根据所述矫正后的激光雷达数据构建特征点距离残差∑||lidar||²；

根据所述IMU预积分残差∑||imu||²、所述物理里程计域积分残差∑||odom||²、所述特征点距离残差∑||lidar||²构建残差函数min_(x){PP+∑||imu||²+∑||odom||²+∑||lidar||²}，其中，PP为边缘化后的先验信息。

5.根据权利要求1所述的GNSS-SLAM初始化方法，其特征在于，对所述GNSS数据进行预处理，包括：

利用多普勒频移测量移除所述GNSS数据中的噪声数据；

根据所述GNSS获取经纬高数据，并将所述经纬高数据转换为ECEF坐标系下三维数据；

构建所述GNSS的ENU坐标系，并计算所述ECEF坐标系下三维数据转换到所述ENU坐标系的变换矩阵；

计算移除所述噪声数据后的GNSS数据在所述ENU坐标系的坐标。

6.根据权利要求1所述的GNSS-SLAM初始化方法，其特征在于，根据预处理后的GNSS数据、所述局部激光雷达里程计轨迹数据构建目标函数，包括：

根据时间戳，查找所述局部激光雷达里程计轨迹数据与预处理后的GNSS数据之间的对应关系；

根据所述对应关系构建目标函数。

7.根据权利要求1所述的GNSS-SLAM初始化方法，其特征在于，根据所述目标函数调整参数，直到完成初始化，包括：

根据所述目标函数对预处理后的GNSS数据、所述局部激光雷达里程计轨迹数据进行验证；

若验证通过，则初始化成功；若验证未通过，则剔除部分预处理后的GNSS数据后再次进行验证，直到完成初始化。