[发明专利]坡度提取方法及装置

权利要求书

1.一种坡度提取方法，其特征在于，包括：

获取激光雷达打在斜坡上的点云；

根据激光雷达坐标系和INS坐标系的位置关系，以及所述INS坐标系和INS水平坐标系的变换关系，将所述点云变换到所述INS水平坐标系中；

将所述INS水平坐标系中的点云投影到二维栅格地图中，以及将所述点云的高度值作为栅格的值，生成二维高度栅格地图；

根据预设条件，在所述二维高度栅格地图中选择感兴趣矩形区域，根据所述感兴趣矩形区域中所述栅格的值计算所述感兴趣矩形区域的坡度；

所述感兴趣矩形区域包括，第一栅格区和第二栅格区；

所述第一栅格区和第二栅格区大小相等，位置相邻，以及在二维栅格地图中的位置固定；

所述根据所述感兴趣矩形区域中栅格的高度值计算所述感兴趣矩形区域的坡度还包括：

计算所述第一栅格区和第二栅格区中心位置高度；

根据公式：计算所述感兴趣区域的初始坡度；

所述计算所述第一栅格区和第二栅格区中心位置高度包括：

输入迭代数据执行感兴趣矩形区域迭代优化算法；

所述迭代数据包括：二维高度栅格地图、矩形区域边界、初始迭代次数、迭代次数阈值、初始偏移量阈值、偏移量步长以及标准差阈值；

其中，所述初始偏移量阈值、偏移量步长与标准差阈值用于配合筛选每次迭代时偏离均值较远的栅格。

2.根据权利要求1所述坡度提取方法，其特征在于，所述变换关系包括：INS坐标系和INS水平坐标系原点重合，INS坐标系在INS水平坐标系的基础上偏转角度N；

所述角度N指俯仰角度，其中N=，所述是安装误差，所述是指车底和路面角度，所述是指坡度。

3.根据权利要求2所述坡度提取方法，其特征在于，所述点云在INS坐标系和INS水平坐标系中的关系如下：

所述代表点云在INS坐标系中的坐标，所述代表点云在INS水平坐标系中的坐标，所述R为坐标系转换矩阵。

4.根据权利要求1所述坡度提取方法，其特征在于，所述将所述INS水平坐标系中的所述点云按照如下关系投影到二维栅格地图中：

所述Z_index表示为索引index的栅格的高度，所述index和i符合如下关系：

5.根据权利要求1~4中任意一项所述坡度提取方法，其特征在于，还包括：

获取所述坡度的状态估计向量和误差协方差矩阵；

根据所述状态估计向量、误差协方差矩阵和坡度和道路坡度测量结果提取道路坡度的更新测量结果。

6.根据权利要求1所述坡度提取方法，其特征在于，所述激光雷达坐标系和所述INS坐标系位置关系相对固定。

7.根据权利要求6所述坡度提取方法，其特征在于，所述激光雷达包括：机械旋转雷达和固态雷达；

所述机械旋转雷达提取点云后，基于匀速运动模型对点云进行运动畸变矫正。

8.一种坡度提取装置，其特征在于，包括：

获取模块，用于获取激光雷达打在斜坡上的点云；

变换模块，用于根据激光雷达坐标系和INS坐标系的位置关系，以及所述INS坐标系和INS水平坐标系的变换关系，将所述点云变换到所述INS水平坐标系中；

投影模块，用于将所述INS水平坐标系中的点云投影到二维栅格地图中，以及将所述点云的高度值作为栅格的值，生成二维高度栅格地图；

计算模块，用于根据预设条件，在所述二维高度栅格地图中选择感兴趣矩形区域，根据所述感兴趣矩形区域中所述栅格的值计算所述感兴趣矩形区域的坡度。