CN113340304A - 坡度提取方法及装置 - Google Patents

Abstract

本申请提供的一种坡度提取方法，包括：获取激光雷达打在斜坡上的点云；根据激光雷达坐标系和INS坐标系的位置关系，以及所述INS坐标系和INS水平坐标系的变换关系，将所述点云变换到所述INS水平坐标系中；将所述INS水平坐标系中的点云投影到二维栅格地图中，以及将所述点云的高度值作为栅格的值，生成二维高度栅格地图；根据预设条件，在所述二维高度栅格地图中选择感兴趣矩形区域，根据所述感兴趣矩形区域中所述栅格的值计算所述感兴趣矩形区域的坡度。通过高精度的激光雷达和惯性导航系统，准确提取激光斜坡数据，在根据斜坡数据建立二维栅格高度地图后，通过迭代和优化后提取出准确的坡度。本申请同时还提供一种坡度提取装置。

Description

坡度提取方法及装置

技术领域

本发明涉及一种环境提取方法，尤其涉及一种坡度提取方法，本申请还涉及一种坡度提取装置。

背景技术

随着社会的不断发展，安全、高效、健康、绿色的矿山作业逐渐受到越来越多的关注，智慧矿山的概念被提出。智慧矿山已经成为矿山发展的重要方向，在露天矿山中，矿车行驶的道路往往是修建在高低坡上，坡度变化范围大，无人驾驶矿车在这种矿道上行驶，非常具有挑战性。

矿车拉出的矿石，通常来说具有很大的质量和惯性，在一个下坡的矿道上行驶，如果不能准确识别坡度的大小，就无法合理规划矿车的行驶速度，容易造成因速度过大而产生威胁，或者在无人驾驶矿车超速过程中进行急刹车，导致矿石物料等外撒。无人驾驶矿车在一个上坡的矿道上行驶，如果对坡度的提取有误，则容易导致矿车在坡度的影响下产生溜车的后果，可能造成身命财产的损失。

在现有的技术中，解决上述技术问题通常有四类方法：

第一类方法是，基于惯性导航系统(INS)，直接从车载的高精度惯性导航系统读取车辆的俯仰角度。或者，基于全球定位系统(GPS)的单天线计算水平和竖直方向的速度比，亦或基于全球定位系统的双天线提取信号的低频部分作为道路的坡度。这类方法是基于车辆本身状态对道路坡度的推断，因此设备的安装误差会导致检查精度的降低，并且由于矿山道路坡度大且不平整，矿车在行驶过程中的俯仰和弹跳运动也会导致检测进度降低，且这类方法无法检测未知环境的道路坡度。

第二类方法是，基于SLAM(Simultaneous Localization and Mapping)算法，提取周围环境的线、面等特征，然后利用帧间匹配算法或地图匹配算法等构建精确的三维环境地图，提取精确的道路坡度信息。这类方法需要检测环境中具有突出的物体提供明显的点、线、面特征、而露天矿山中难以提供，因此该算法在露天矿山中的测试效果不理想，难以准确提取坡度。

第三类方法是，基于卡尔曼滤波器或者隆伯格观测器等，通过分析车辆的受外力情况建立车辆模型，进而利用动力学的方法估计道路的坡度提取坡度，但是这类方法需要知道自身位置，无法提取未知环境中的道路坡度。

第四类方法是，通过激光雷达提取打在斜坡上的点云，然后使用PROSAC(Progressive Sample Consensus)、RANSAC(Random Sample Consensus)等平面拟合的方法得到斜坡坡面方程，进而获得斜坡坡度。但是这种方法依靠安装在车辆上的激光雷达，因此提取的坡度是基于车辆的坐标系的坡度，不能提取道路基于水平面的真实坡度。

发明内容

为解决上述技术问题，本申请公开一种坡度提取方法，规避车辆俯仰运动产生的误差，并且提高坡度的提取精度，本申请还提供一种坡度提取装置。

本申请提供的一种坡度提取方法，包括：

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

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

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

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

可选的，所述变换关系包括：INS坐标系和INS水平坐标系原点重合，INS坐标系在INS水平坐标系的基础上偏转角度N；

所述角度N指俯仰角度，其中N＝λ₁+λ₂+λ₃，所述λ₁是安装误差，所述λ₂是指车底和路面角度，所述λ₃是指坡度。

可选的，所述点云在INS坐标系和INS水平坐标系中的关系如下：

所述

代表点云在INS坐标系中的坐标，所述

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

可选的，所述将所述INS水平坐标系中的所述点云按照如下关系投影到二维栅格地图中：

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

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

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

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

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

根据公式：

计算所述感兴趣区域的初始坡度。

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

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

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

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

可选的，其特征在于，还包括：

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

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

可选的，所述激光雷达坐标系和所述INS坐标系位置关系相对固定。

可选的，所述激光雷达包括：机械旋转雷达和固态雷达；

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

本申请还提供一种坡度提取装置，包括：

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

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

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

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

本申请相对与现有技术的优点如下：

本申请提供的一种坡度提取方法，包括：获取激光雷达打在斜坡上的点云；根据激光雷达坐标系和INS坐标系的位置关系，以及所述INS坐标系和INS水平坐标系的变换关系，将所述点云变换到所述INS水平坐标系中；将所述INS水平坐标系中的点云投影到二维栅格地图中，以及将所述点云的高度值作为栅格的值，生成二维高度栅格地图；根据预设条件，在所述二维高度栅格地图中选择感兴趣矩形区域，根据所述感兴趣矩形区域中所述栅格的值计算所述感兴趣矩形区域的坡度。通过高精度的激光雷达和惯性导航系统，准确提取激光雷达打在坡度上的点云，并通过坐标系的转化，提取到基于水平面的坡度数据，建立二维栅格地图后，通过迭代和优化后提取出准确的坡度。

附图说明

图1是本申请中坡度提取步骤的流程图。

图2是本申请中点云和坐标关系的示意图。

图3是本申请中二维栅格地图的示意图。

图4是本申请中坡度提取装置的示意图。

具体实施方式

在下面的描述中阐述了很多具体细节以便于充分理解本申请。但是本申请能够以很多不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本申请内涵的情况下做类似推广，因此本申请不受下面公开的具体实施的限制。

本申请提一种坡度提取方法(GKSE)，包括：获取激光雷达打在斜坡上的点云；根据激光雷达坐标系和INS坐标系的位置关系，以及所述INS坐标系和INS水平坐标系的变换关系，将所述点云变换到所述INS水平坐标系中；将所述INS水平坐标系中的点云投影到二维栅格地图中，以及将所述点云的高度值作为栅格的值，生成二维高度栅格地图；根据预设条件，在所述二维高度栅格地图中选择感兴趣矩形区域，根据所述感兴趣矩形区域中所述栅格的值计算所述感兴趣矩形区域的坡度。通过激光雷达和INS系统的联合标定，给出激光雷达打在斜坡上点云在INS水平坐标系104中的坐标，建立二维高度栅格地图，并依据所述二维栅格地图中的感兴趣矩形区域计算坡度。

图1是本申请中坡度提取步骤的流程图。

请参照图1所示，本申请获取坡度的方法主要由四步骤组成，并且所述每一步骤中还可能包括其他小步骤，下面将进行详细的描述。

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

当一束激光照射到物体表面时，所反射的激光会携带方位、距离等信息。若将激光束按照某种轨迹进行扫描，会边扫描边记录到反射的激光点信息，由于扫描极为精细，则能够得到大量的激光点，因而就可形成激光点云。本申请中，所述点云就是激光雷达打在斜坡上后得到的激光点云。

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

激光雷达可以精确地提供坡度的三维信息，但是激光雷达提供的三维信息是在雷达本身的坐标系或者在矿车105的坐标系中，不能提供基于水平坐标系的信息，而INS则可以在矿车俯仰或者跳跃运动中提供基于水平面的坐标系，本申请中基于激光雷达和INS联合标定点云的位置信息。

图2是本申请中点云和坐标关系的示意图。

请参照图2所示，本申请中所述点云是激光雷达扫描获取的，并通过激光雷达坐标系101、INS坐标系102和INS水平坐标系104之间的位置关系变换到INS水平坐标系104当中。

激光雷达和INS系统是安装在矿车105的不同位置上的，基于扫描坡度的需要，激光雷达通常安装在矿车105的车头顶端，这样可以更容易的进行坡度点云的提取。INS系统和激光雷达的位置相对固定，且激光雷达和INS系统都有一个相对于矿车105固定的坐标系，即激光雷达坐标系101和INS坐标系102。这两个坐标系随着矿车105的运动而运动，两者之间只是原点位置不同，所述激光雷达坐标系101和所述INS坐标系102位置关系相对矿车105固定。因此，通过坐标变换可以容易的将点云从激光雷达坐标系101中变换到INS坐标系102中。

如图2所示，点云在激光雷达坐标系101中的坐标确定，则在INS坐标系102中也是确定的。激光雷达和INS系统联合标定点云的位置信息，将点云在INS坐标系中102标出，这样，INS及获取到了点云的数据信息，点云在INS坐标系102中可以表示为：

P^I表示在INS坐标系中的点云，i表示点云中点的编号，其既可以表示点云第0个点

也可以表示点云第n个点

在激光雷达提取点云的步骤中，本申请采用激光雷达的类型包括：所述激光雷达包括：机械旋转雷达和固态雷达。但是在实际的使用过程中，所述机械旋转雷达输出的点云会产生运动畸变，因此需要消除所述运动畸变，本申请提供一种消除运动畸变的方法是，所述机械旋转雷达提取点云后，基于匀速运动模型对点云进行运动畸变矫正：即假设矿车在矿道上匀速运动，并将所述矿车的运动和速度考虑进获取点云的过程中。

上述内容已经描述根据激光雷达和INS系统联合标定点云在INS坐标系102中坐标的过程，接下来，则需要确定所述点云在INS水平坐标系104中的坐标。

如图2所示，INS水平坐标系104是INS系统提供的另一个坐标系，所述INS水平坐标系104平行于水平面，并且和INS坐标系102具有实时的变换关系，所述变换关系包括：INS坐标系102和INS水平坐标系104原点重合，INS坐标系102在INS水平坐标系104的基础上偏转角度N。实际上，INS坐标系102是跟随矿车105的运动中不仅仅有起伏的变化，还有侧倾的变化，但是本申请中只考虑起伏角度的影响。

请继续参考图2所示，INS坐标系102和INS水平坐标系104之间的角度是由三个部分组成的，包括INS系统的安装误差角度λ₁，所述λ₁也可以表述为INS坐标系102和矿车坐标系103之间的夹角；矿车105和地面的夹角λ₂；所述矿车所在地面的坡度角度λ₃。因此所述偏转角度N＝λ₁+λ₂+λ₃。

根据上述关系可知，点云在INS坐标系102和INS水平坐标系104之间转换的公式一如下：

其中，所述

代表点云在INS坐标系102中的坐标，所述

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

所述R由公式二获取，如下：

根据所述公式一和公式二，获取到点云在INS水平坐标系104中的坐标，在此基础上即可进行下一步的计算。

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

栅格地图，即栅格图像，也称光栅图像，是指在空问和亮度上都已经离散化了的图像。把一幅栅格图像考虑为一个矩阵，矩阵中的任一元素对应于图像中的一个点，而相应的值对应于该点的灰度级，本申请中栅格上相应的值是点云的高度值。

本步骤中就是将点云投影到栅格地图中。上述步骤S101、S102已近获取到点云在INS水平坐标系104中的坐标，所述INS水平坐标系104包括水平面上的X₂(图2中未示出)轴和Y₂轴以及垂直的Z₂轴，其中点云在Z₂轴上的数值是点云的高度值。

本申请中，投影点云到栅格地图上是按照一定规则投影的，本申请中的一个优选实施例是投影按照公式三进行。

公式三：

所述Z_index表示为索引index的栅格的高度。其中被选择像素index和i的关系符合公式四：

公式四中，

为向下取整符号，Δx、Δy分别为INS水平坐标系中心相对于二维栅格地图坐标系中心竖直和水平方向的偏移量，单位为m，r为二维栅格地图的分辨率，单位为m，w为二维栅格地图的宽，即宽度方向的栅格个数。

在将点云投影到栅格地图上之后，将栅格的值设置为点云的高度值，形成二维栅格高度地图。

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

本申请中，所述感兴趣矩形区域的选择应当是有利于的坡度计算的区域，一个优选的方案是，根据点云密度值，选择所述密度大的区域作为感兴趣矩形区域。这样可以更精确地计算坡度。

在选择感兴趣矩形区域的时候应当清楚的是，所述感兴趣矩形区域的宽度大于车辆宽度。

在选择出感兴趣矩形区域后，可以根据二维高度栅格地图上的高度值以及栅格距离计算出坡度的大小。

图3是本申请中二维栅格地图的示意图。

请参考图3所示，为了更为精确地计算坡度，本申请提供一种优选的坡度计算方法。二维栅格高度地图中选出感兴趣矩形区域后，将所述感兴趣矩形区域分割成两个部分，包括，第一栅格区A和第二栅格区B；所述第一栅格区A和第二栅格区B的大小相等，位置相邻，以及在二维栅格地图中的位置固定。通过分割感兴趣矩形区域，则可以将坡度计算等价于计算第一栅格区和第二栅格区中心位置的连线和水平面之间的角度。

本申请中，采用二维栅格地图矩形区域迭代优化算法分别计算A和B的几何中心位置处的高度，然后基于直角三角形模型计算感兴趣区域A+B的坡度。输入迭代数据执行感兴趣矩形区域迭代优化算法；所述迭代数据包括：二维高度栅格地图、矩形区域边界、初始迭代次数、迭代次数阈值、初始偏移量阈值、偏移量步长以及标准差阈值；其中，所述初始偏移量阈值、偏移量步长与标准差阈值用于配合筛选每次迭代时偏离均值较远的栅格。

所述二维栅格地图矩形区域优化算法如下：

输入：ogm二维高度栅格地图，minrow矩形区域下边界，marrow矩形区域上边界，mincol矩形区域右边界，marcol矩形区域左边界，count初始迭代次数，counther迭代次数值阈值，offsethre初始偏移量阈值，s偏移量步长，stdevthre标准差阈值

输出：mean矩形区域几何中心位置高度

根据二维栅格地图矩形区域迭代优化算法可以分别计算第一栅格区A与第二栅格区B的几何中心位置处的高度。然后基于直角三角形模型，由公式五可确定感兴趣区域的坡度。

公式五如下：

上述步骤算出的坡度，是根据一帧点云数据计算的，为了使得出的坡度更为准确，需要对所述坡度进行进一步处理，结合相邻两帧点云得到更精确地数据。本申请基于卡尔曼滤波器的坡度优化算法进行坡度优化。

根据实际情况，基于相邻帧道路坡度均匀变化模型，融合卡尔曼滤波器，进一步优化当前帧感兴趣矩形区域几何中心位置的道路坡度，优化的结果即为最终的道路坡度实时提取结果。

本申请中，所述相邻帧道路坡度均匀变化模型是指相邻帧的感兴趣矩形区域几何中心位置的道路坡度均匀变化。

所述基于卡尔曼滤波器的坡度优化算法如下：

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

当前帧的状态估计向量

和误差协方差矩阵P_k根据第六公式：

和第七公式：

计算得出。

上述公式中，其中

为前一帧的感兴趣矩形区域几何中心位置的道路坡度测量更新结果，只包含道路坡度一个状态变量，F_k为状态转移矩阵，

是F_k的转置矩阵，P_k-1初始化为一行一列的单位矩阵。基于相邻帧道路坡度均匀变化模型，令F_k＝1，Q_k为过程噪声协方差，本申请中忽略过程噪声协方差，令Q_k＝0。

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

融合当前帧感兴趣矩形区域几何中心位置的道路坡度测量结果

根据公式八：

公式九：

和公式十：P′_k＝P_k-K′H_kP_k，获得当前帧感兴趣矩形区域几何中心位置的道路坡度测量更新结果

即为最终的道路坡度实时提取结果。

上述各个公式中，H_k为尺度变换矩阵，

是H_k的转置矩阵，令H_k＝1，R_k为测量噪声协方差矩阵，本模型中忽略测量噪声协方差，令R_k＝0。

上述内容详细描述了本申请中的坡度提取方法，本申请还提供一种坡度提取装置。

所述坡度提取装置，包括：获取模块201，用于获取激光雷达打在斜坡上的点云；变换模块202，用于根据激光雷达坐标系和INS坐标系的位置关系，以及所述INS坐标系和INS水平坐标系的变换关系，将所述点云变换到所述INS水平坐标系中；投影模块203，用于将所述INS水平坐标系中的点云投影到二维栅格地图中，以及将所述点云的高度值作为栅格的值，生成二维高度栅格地图；计算模块204，用于根据预设条件，在所述二维高度栅格地图中选择感兴趣矩形区域，根据所述感兴趣矩形区域中所述栅格的值计算所述感兴趣矩形区域的坡度。。通过激光雷达和INS系统的联合标定，给出激光雷达打在斜坡上点云在INS水平坐标系104中的坐标，建立二维高度栅格地图，并依据所述二维栅格地图中的感兴趣矩形区域计算坡度。

图4是本申请中坡度提取装置的结构示意图。

请参照图4所述，本申请获取坡度的装置主要由四个模块组成，并且所述每一模块中还包括其他单元，下面将进行详细的描述。

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

当激光照射到物体表面时，所反射的激光会携带方位、距离等信息。若将激光束按照某种轨迹进行扫描，便会边扫描边记录到反射的激光点信息，由于扫描极为精细，则能够得到大量的激光点，因而就可形成激光点云。本申请中，所述点云就是激光雷达打在斜坡上后得到的激光点云。

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

激光雷达可以精确地提供坡度的三维信息，但是激光雷达提供的三维信息是在雷达本身的坐标系或者在矿车105的坐标系中，不能提供基于水平坐标系的信息，而INS系统则可以在矿车俯仰或者跳跃运动中提供基于水平面的坐标系，本申请中基于激光雷达和INS联合标定点云的位置信息。

本申请中所述点云是激光雷达扫描获取的，并通过激光雷达坐标系101、INS坐标系102和INS水平坐标系104之间的位置关系变换到INS水平坐标系104当中。

激光雷达和INS系统是安装在矿车105的不同位置上的，基于扫描坡度的需要，激光雷达通常安装在矿车105的车头顶端，这样可以更容易的进行坡度点云的提取。INS系统和激光雷达的位置相对固定，且激光雷达和INS系统都有一个相对于矿车105固定的坐标系，即激光雷达坐标系101和INS坐标系102。这两个坐标系随着矿车105的运动而运动，两者之间只是原点位置不同，所述激光雷达坐标系101和所述INS坐标系102位置关系相对矿车105固定。因此，通过坐标变换可以容易的将点云从激光雷达坐标系101中变换到INS坐标系102中。

点云在激光雷达坐标系101中的坐标确定，则在INS坐标系102中也是确定的。激光雷达和INS系统联合标定点云的位置信息，将点云在INS坐标系中102标出，这样，INS及获取到了点云的数据信息，点云在INS坐标系102中可以表示为：

P^I表示在INS坐标系中的点云，i表示点云中点的编号，其既可以表示点云第0个点

也可以表示点云第n个点

在变换模块202中，采用激光雷达的类型包括：所述激光雷达包括：机械旋转雷达和固态雷达。但是在实际的使用过程中，所述机械旋转雷达输出的点云会产生运动畸变，因此需要消除所述运动畸变。因此，本申请所述获取模块还包括：矫正单元。

矫正单元，用于所述机械旋转雷达提取点云后，基于匀速运动模型对点云进行运动畸变矫正，即假设矿车在矿道上匀速运动，并将所述矿车的运动和速度考虑进获取点云的过程中。

上述内容已经描述根据激光雷达和INS系统联合标定点云在INS坐标系102中坐标的过程，接下来，则需要确定所述点云在INS水平坐标系104中的坐标。

如图2所示，INS水平坐标系104是INS系统提供的另一个坐标系，所示INS水平坐标系104平行于水平面，并且和INS坐标系102具有实时的变换关系，所述变换关系包括：INS坐标系102和INS水平坐标系104原点重合，INS坐标系102在INS水平坐标系104的基础上偏转角度N。实际上，INS坐标系102是跟随矿车105的运动中不仅仅有起伏的变化，还有侧倾的变化，但是本申请中只考虑起伏角度的影响。

INS坐标系102和INS水平坐标系104之间的角度是由三个部分组成的，包括INS系统的安装误差角度λ₁，所述λ₁也可以表述为INS坐标系102和矿车坐标系103之间的夹角；矿车105和地面的夹角λ₂；所述矿车所在地面的坡度角度λ₃。因此所述偏转角度N＝λ₁+λ₂+λ₃。

根据上述关系可知，点云在INS坐标系102和INS水平坐标系104之间转换的公式一如下：

其中，所述

代表点云在INS坐标系102中的坐标，所述

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

所述R由公式二获取，如下：

根据所述公式一和公式二，获取到点云在INS水平坐标系104中的坐标，在此基础上即可进行下一步的计算。

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

栅格地图是指在空间和亮度上都已经离散化了的图像。把一幅栅格图像考虑为一个矩阵，矩阵中的任一元素对应于图像中的一个点，而相应的值对应于该点的灰度级，本申请中门锁上相应的值是点云的高度值。

投影模块203就是将点云投影到栅格地图中。上述已经获取到点云在INS水平坐标系104中的坐标，所述INS水平坐标系104包括水平面上的X₂轴和Y₂轴以及垂直的Z₂轴，其中点云在Z₂轴上的数值是点云的高度值。

本申请中，投影点云到栅格地图上是按照一定规则投影的，本申请中的一个优选实施例是投影按照公式三进行。

公式三：

所述Z_index表示为索引index的栅格的高度。其中被选择像素index和i的关系符合公式四：

公式四中，

为向下取整符号，Δx、Δy分别为INS水平坐标系中心相对于二维栅格地图坐标系中心竖直和水平方向的偏移量，单位为m，r为二维栅格地图的分辨率，单位为m，w为二维栅格地图的宽，即宽度方向的栅格个数。

在将点云投影到栅格地图上之后，将栅格的值设置为点云的高度值，形成二维栅格高度地图。

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

本申请中，所述感兴趣矩形区域的选择应当是有利于的坡度计算的区域，一个优选的方案是，根据点云密度值，选择所述密度大的区域作为感兴趣矩形区域。这样可以更精确地计算坡度。

在选择感兴趣矩形区域的时候应当清楚的是，所述感兴趣矩形区域的宽度大于车辆宽度。

在选择出感兴趣矩形区域后，可以根据二维高度栅格地图上的高度值以及栅格距离计算出坡度的大小。

二维栅格高度地图中选出感兴趣矩形区域后，将所述感兴趣矩形区域分割成两个部分，包括，第一栅格区A和第二栅格区B；所述第一栅格区A和第二栅格区B的大小相等，位置相邻，以及在二维栅格地图中的位置固定。通过分割感兴趣矩形区域，则可以将坡度计算等价于计算第一栅格区和第二栅格区中心位置的连线和水平面之间的角度。

本申请中所述计算模块还包括：

高度计算单元，用于计算所述第一栅格区和第二栅格区中心位置高度；

坡度计算单元，用于根据公式：

计算所述感兴趣区域的坡度。

采用二维栅格高度地图矩形区域迭代优化算法分别计算A和B的几何中心位置处的高度，然后基于直角三角形模型计算感兴趣区域A+B的坡度，具体的，所述高度计算单元还包括：

迭代子单元，用于输入迭代数据执行感兴趣矩形区域迭代优化算法；所述迭代数据包括：二维高度栅格地图、矩形区域边界、初始迭代次数、迭代次数阈值、初始偏移量阈值、偏移量步长以及标准差阈值；其中，所述初始偏移量阈值、偏移量步长与标准差阈值用于配合筛选每次迭代时偏离均值较远的栅格。

根据二维栅格地图矩形区域迭代优化算法可以分别计算第一栅格区A与第二栅格区B的几何中心位置处的高度。然后基于直角三角形模型，由公式五可确定感兴趣区域的坡度。

公式五如下：

上述步骤算出的坡度，是根据一帧点云数据计算的，为了使得出的坡度更为准确，需要对所述坡度进行进一步处理，结合相邻两帧点云得到更精确地数据。

本申请基于卡尔曼滤波器的坡度优化算法进行坡度优化。因此，所述坡度提取装置还包括：

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

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

根据具体的实际情况，基于相邻帧道路坡度均匀变化模型，融合卡尔曼滤波器，进一步优化当前帧感兴趣矩形区域几何中心位置的道路坡度，优化的结果即为最终的道路坡度实时提取结果。

本申请中，所述相邻帧道路坡度均匀变化模型是指相邻帧的感兴趣矩形区域几何中心位置的道路坡度均匀变化。

首先，获取所述坡度的状态估计向量和误差协方差矩阵，当前帧的状态估计向量

和误差协方差矩阵P_k根据第六公式：

和第七公式：

计算得出。

上述公式中，其中

为前一帧的感兴趣矩形区域几何中心位置的道路坡度测量更新结果，只包含道路坡度一个状态变量，F_k为状态转移矩阵，

是F_k的转置矩阵，P_k-1初始化为一行一列的单位矩阵。基于相邻帧道路坡度均匀变化模型，令F_k＝1，Q_k为过程噪声协方差，本申请中忽略过程噪声协方差，令Q_k＝0。

然后，用于根据所述状态估计向量、误差协方差矩阵和坡度和道路坡度测量结果提取道路坡度的更新测量结果。融合当前帧感兴趣矩形区域几何中心位置的道路坡度测量结果

根据公式八：

公式九：

和公式十：P′_k＝P_k-K′H_kP_k，获得当前帧感兴趣矩形区域几何中心位置的道路坡度测量更新结果

即为最终的道路坡度实时提取结果。

上述各个公式中，H_k为尺度变换矩阵，

是H_k的转置矩阵，令H_k＝1，R_k为测量噪声协方差矩阵，本模型中忽略测量噪声协方差，令R_k＝0。

Claims (10)

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所述坡度提取方法，其特征在于，所述感兴趣矩形区域包括，第一栅格区和第二栅格区；

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

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

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

根据公式：

计算所述感兴趣区域的初始坡度。

6.根据权利要求5所述坡度提取方法，其特征在于，所述计算所述第一栅格区和第二栅格区中心位置高度包括：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Images