[发明专利]一种全局地图构建方法与系统

权利要求书

1.一种全局地图构建方法，其特征在于，包括以下步骤：

步骤1、采用激光雷达采集地表数据得到激光点云，对所述激光点云进行滤波得到滤波后的激光点云；

步骤2、采用全球导航系统/惯性导航系统采集所述地表数据得到速度位置信息；

步骤3、对所述滤波后的激光点云和所述速度位置信息进行预处理得到得到预积分因子和运动估计因子；

步骤4、根据所述速度位置信息对所述滤波后的激光点云进行畸变补偿得到消除点云畸变的激光点云；

步骤5、对所述消除点云畸变的激光点云进行点云到地图的配准得到配准因子；

步骤6、根据所述预积分因子、运动估计因子和配准因子构成因子图，并利用第一优化方程对所述因子图进行优化生成局部地图；

步骤7、根据约束因子利用第二优化方程优化所述局部地图并构建全局因子图；

步骤8、根据所述全局因子图构建全局地图。

2.根据权利要求1所述的一种全局地图构建方法，其特征在于，所述步骤1包括：

步骤1.1：获取所述激光点云上的三维坐标量测量值；

步骤1.2：根据所述三维坐标量测量值利用曲面方程进行曲面拟合得到曲面模型，其中所述曲面模型为：

其中，X表示三维坐标量测量值x方向的值，Y表示三维坐标量测量值y方向的值，Z表示三维坐标量测量值z方向的值，E_A表示偶然误差矩阵，e表示观测向量矩阵，e_A表示列矢量化E_A，vec表示矩阵列向量化算子，I_n表示n×n阶的单位矩阵，I_m表示m×m阶的单位矩阵，表示矩阵之间的直积，a₁表示第一多项系数，a₂表示第二多项系数，a₃表示第三多项系数，a₄表示第四多项系数，a₅表示第五多项系数，a₆表示第六多项系数；

步骤1.3：根据所述曲面模型建立目标函数；其中，所述目标函数为：

e^Te+(e_A)^T(e_A)＝min；

步骤1.4：利用最小二乘法对所述目标函数求解得到滤波模型；

步骤1.5：利用所述滤波模型对所述激光点云进行滤波得到所述滤波后的激光点云。

3.根据权利要求1所述的一种全局地图构建方法，其特征在于，所述步骤3包括：

步骤3.1、对惯性导航系统测量值进行预积分得到所述预积分因子，所述预积分因子为：

其中，表示第i+1次点云相比于第i次时载体坐标系的旋转的观测变化量，表示第i+1次点云相比于第i次载体坐标系的速度的观测变化量，为表示第i+1次点云相比于第i次载体坐标系的位置的观测变化量，

残差项E_pre记为：

其中，表示第i次扫描的相对于载体坐标系的姿态预测量，表示第i+1次扫描的载体坐标系的速度预测量，表示第i+1次扫描相比于第i次载体坐标系的平移变化量；

步骤3.2、对所述速度位置信息滤波融合得到所述运动估计因子，其中所述运动估计因子为：

其中，i表示激光雷达扫描时刻，上标(～)表示含有高斯噪声的观测量，N表示第N个，为当前时刻N_i局部视图相对于相对参考时刻N_ref的θ观测量的变化量，为当前时刻N_i相对于参考时刻N_ref的ρ观测量的变化量，

误差项为：

其中，表示两个李代数的和，上标(-)表示预测量，E_odom为残差项，为当前时刻相对于参考时刻N_ref-1的θ的预测量，为参考时刻N_ref的θ的预测量，为当前时刻N_i的θ的观测量，为当前时刻N_i的ρ的观测量，为参考时刻N_ref的ρ的观测量，为当前时刻相对于参考时刻N_ref的ρ预测量。