[发明专利]一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统

权利要求书

1.一种顾及动态障碍物的激光雷达惯导里程计与建图方法，包括数据预处理，其特征在于，还包括以下步骤：

步骤1：顾及动态障碍物的特征提取；

步骤2：进行激光点云帧间匹配和ESKF数据融合；

步骤3：利用帧与局部地图进行精细点云匹配，优化激光里程计的位姿，建立全局的3D点云地图；

所述数据预处理是指通过高频的MEMS惯导辅助激光雷达，对每一帧内的所有激光点云进行畸变补偿校正，所述畸变补偿校正的方法包括以下子步骤：

步骤01：首先找到与当前激光点的时间戳t_curr最接近的时间戳t_k和t_k+1的连续惯导测量值；

步骤02：通过惯导积分过程，分别得到惯导在时间k和时间k+1的世界坐标系W中的状态，公式为：

其中，p表示位置，v表示速度，θ表示角向量，x表示x轴朝向，y表示y轴朝向，z表示z轴朝向；

步骤03：将世界坐标系W定义为起始惯导坐标系，可利用时间因子线性插值来确定惯导在时刻t_curr在世界坐标系中的状态公式为

其中，ratio₁和ratio₂为时间插值因子，t_k表示k时刻的时间戳，t_k+1表示k+1时刻的时间戳；

步骤04：利用和能够得到当前激光点和起始激光点所对应惯导的位置和惯导的速度在起始激光点坐标中得到每个激光点的运动畸变量，公式为

其中，表示激光雷达当前帧起始点坐标系与世界坐标系之间的变换矩阵，i表示一帧点云数据中每个点占据一帧扫描周期的时间，表示当前激光点所对应的惯导在世界坐标系下的状态，表示起始激光点所对应的惯导在世界坐标系下的状态。

2.如权利要求1所述的顾及动态障碍物的激光雷达惯导里程计与建图方法，其特征在于，所述步骤1包括剔除道路环境中的动态障碍物，并从静态点云中提取特征点，所述特征点包括地面特征点、非地面线特征点和非地面表面特征点。

3.如权利要求2所述的顾及动态障碍物的激光雷达惯导里程计与建图方法，其特征在于，所述动态障碍物的剔除方法为提取地面点云，对剩余的非地面点云进行目标分割并聚类，将小于设定尺寸的目标剔除。

4.如权利要求3所述的顾及动态障碍物的激光雷达惯导里程计与建图方法，其特征在于，所述地面点云的提取方法为采用基于深度图像的角度分割方式提取地面点，将每一帧内的激光点云投影为深度图像，将无序杂乱的非结构化点云数据转化成有组织的点云数据；深度图像中的行数r由激光雷达垂直方向的激光线束数量决定，列数c由激光雷达的水平分辨率决定；

点A和点B是激光雷达在同一水平分辨率下发射，在竖直方向的相邻激光点，角度和对应在深度图像中r-1和r行激光束的垂直角度；定义一个竖直角度α，将深度图像R的每列c转换成角度的形式，已知两个相邻垂直激光束的深度值，角度α的计算公式为：

其中，R_r-1,c表示第r-1行第c列激光点的深度值，R_r,c表示第r行第c列激光点的深度值，θ₁和θ₂表示不同激光束在竖直方向上的角度，每一个角度α计算需要两个距离读数，将这些角度的所有集合视为矩阵M_α，得到夹角图像，将Savitsky-Golay平滑算法应用在矩阵M_α的每一列，在夹角图像上进行地面标记，从期望属于地面的行开始，使用广度优先搜索将相似的组件标记在一起。