[发明专利]面向斜坡区域的基于多线激光雷达的斜坡角度估计方法

权利要求书

1.一种面向斜坡区域的基于多线激光雷达的斜坡角度估计方法，其特征在于，包括以下步骤：

步骤S1，获取给定斜坡区域的原始激光雷达点云数据D_L；

步骤S2，结合多线激光雷达坐标系O_LX_LY_LZ_L到机器人坐标系O_RX_RY_RZ_R的坐标变换关系，对D_L进行坐标变换和滤波处理，生成新的激光雷达点云数据D_S；

步骤S3，基于D_S，利用PROSAC算法进行平面拟合，得到斜坡坡面方程；

步骤S4，从D_S中选取3个到平面X_RO_RY_R的距离均不超过给定阈值d_Z的数据点，并利用选取出的3个数据点计算得到地面方程；

步骤S5，依据所述斜坡坡面方程和所述地面方程计算得到斜坡角度的估计结果；

其中，

步骤S3利用PROSAC算法得到斜坡坡面方程的具体方法为：

步骤S31，从新的激光雷达点云数据D_S中任选3个数据点，计算出一个平面；

步骤S32，求取D_S中的其它数据点到步骤S31计算所得平面的距离，以对应的距离在d_H范围内的数据点作为内点构成内点集；其中d_H为给定阈值；

步骤S33，以最大化内点数量为指引进行迭代处理，其中每次迭代过程中用于确定新平面的3个数据点从上一次的内点集里面选取，直至内点占所有点的比例超过预设的内点比例阈值λ或达到预设迭代次数M_K；

步骤S34，以最后一次迭代得到的平面作为斜坡坡面，并输出斜坡坡面方程。

2.根据权利要求1所述的方法，其特征在于，步骤S5中斜坡角度的具体计算方法为：

其中，为斜坡角度的估计结果；u_p为斜坡坡面法向量，将斜坡坡面方程在机器人坐标系下表示为A_SL·x+B_SL·y+C_SL·z+D_SL＝0，其中A_SL、B_SL、C_SL、D_SL为斜坡坡面方程的四个系数，从而u_p＝(A_SL,B_SL,C_SL)；u_xoy为地面法向量，将地面方程在机器人坐标系下表示为A_GF·x+B_GF·y+C_GF·z+D_GF＝0，其中A_GF、B_GF、C_GF、D_GF为地面方程的四个系数，从而u_xoy＝(A_GF,B_GF,C_GF)。

3.根据权利要求1所述的方法，其特征在于，步骤S2中对D_L进行坐标变换和滤波处理，生成新的激光雷达点云数据D_S的方法为：

步骤S21，基于多线激光雷达坐标系到机器人坐标系的坐标变换关系，对步骤S1所获取的原始激光雷达点云数据D_L进行坐标变换，得到对应的机器人坐标系下点云数据D_R；

步骤S22，利用统计滤波对D_R进行滤波处理，生成新的激光雷达点云数据D_S。