面向斜坡区域的基于多线激光雷达的斜坡角度估计方法与流程

技术特征：

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的数据点，并利用该组数据点计算得到地面方程；

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

2.根据权利要求1所述的方法，其特征在于，步骤S3利用PROSAC算法得到斜坡坡面方程的具体方法为：

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

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

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

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

3.根据权利要求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)。

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

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

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

5.根据权利要求4所述的方法，其特征在于，利用统计滤波对D_R进行滤波处理，具体方法包括：

步骤S221，计算D_R中的每个点到其m个最近邻点间的平均距离；其中m为预设最近邻点的数量；

步骤S222，计算距离阈值d_F：

d_F＝μ+δ·σ

其中，μ是高斯分布Φ的均值，σ是高斯分布Φ的标准差，δ为给定系数；Φ为步骤S221中得到的平均距离所符合的高斯分布；

步骤S223，结合步骤S221中所计算的D_R中的每个点到其m个最近邻点间的平均距离，选择平均距离小于等于距离阈值d_F的点构成新的激光雷达点云数据D_S。

6.根据权利要求1～5中任一项所述的方法，其特征在于，所述多线激光雷达坐标系O_LX_LY_LZ_L到机器人坐标系O_RX_RY_RZ_R的坐标变换关系为：

$\left(\begin{array}{c}{x}_R\\ y_R\\ z_R\end{array}\right)=R_d{R}_r\left(\begin{array}{c}{x}_L\\ y_L\\ z_L\end{array}\right)+T$

其中，(x_R,y_R,z_R)和(x_L,y_L,z_L)分别是多线激光雷达点云数据中的点在机器人坐标系O_RX_RY_RZ_R和多线激光雷达坐标系O_LX_LY_LZ_L中的三维坐标；R_r为从多线激光雷达坐标系O_LX_LY_LZ_L到坐标系O_PX_PY_PZ_P的坐标变换的旋转矩阵；R_d为从坐标系O_PX_PY_PZ_P到坐标系O_DX_DY_DZ_D的坐标变换的旋转矩阵；T为多线激光雷达坐标系O_LX_LY_LZ_L与机器人坐标系O_RX_RY_RZ_R之间的平移矩阵；

多线激光雷达坐标系O_LX_LY_LZ_L和机器人坐标系O_RX_RY_RZ_R均为右手系，多线激光雷达坐标系O_LX_LY_LZ_L的原点O_L(x_LO,y_LO,z_LO)位于多线激光雷达的中心，Z_L轴垂直于多线激光雷达底平面且方向向上，X_L轴垂直于Z_L轴且与多线激光雷达正前方保持一致；机器人坐标系O_RX_RY_RZ_R的原点O_R(x_RO,y_RO,z_RO)选择机器人中心在地面上的投影点，Z_R轴垂直于机器人底平面且方向向上，X_R轴垂直于Z_R轴且与机器人运动方向保持一致；

坐标系O_PX_PY_PZ_P的原点O_P(x_PO,y_PO,z_PO)与多线激光雷达坐标系O_LX_LY_LZ_L的原点重合，Z_P轴在多线激光雷达坐标系O_LX_LY_LZ_L中的向量表示为：X_P轴和Y_P轴在多线激光雷达坐标系O_LX_LY_LZ_L中的向量表示分别为：和

坐标系O_DX_DY_DZ_D为机器人坐标系O_RX_RY_RZ_R经过-T的平移变换得到的坐标系。

7.根据权利要求6所述的方法，其特征在于，多线激光雷达坐标系O_LX_LY_LZ_L与机器人坐标系O_RX_RY_RZ_R之间的平移矩阵T的计算方法为：

$T=\left(\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right)$

其中，t_x、t_y、t_z分别为多线激光雷达坐标系O_LX_LY_LZ_L的原点与机器人坐标系O_RX_RY_RZ_R的原点在三个坐标分量上的偏差。

8.根据权利要求7所述的方法，其特征在于，从多线激光雷达坐标系O_LX_LY_LZ_L到坐标系O_PX_PY_PZ_P的坐标变换的旋转矩阵R_r具体为：

$R_r=\left[\begin{array}{ccc}cos\mathrm{\α}& -sin\mathrm{\α}& 0\\ sin\mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathrm{\β}& -sin\mathrm{\β}\\ 0& sin\mathrm{\β}& \cos \mathrm{\β}\end{array}\right]\left[\begin{array}{ccc}cos\mathrm{\γ}& -sin\mathrm{\γ}& 0\\ sin\mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right]$

在没有斜坡的平坦开阔空场地获取地面的多线激光雷达点云数据，从中任选3个数据点构成集合ξ_g，并进一步计算得到地面在多线激光雷达坐标系O_LX_LY_LZ_L下的方程：A_G·x+B_G·y+C_G·z+D_G＝0，其中A_G、B_G、C_G、D_G为地面在多线激光雷达坐标系O_LX_LY_LZ_L下的方程的四个系数；

在坐标系O_LX_LY_LZ_L中，平面X_PO_PY_P与平面X_LO_LY_L的交线的向量记为N轴，表示为

定义α的旋转方向为从X_P轴到N轴，β的旋转方向为从Z_P轴到Z_L轴，γ的旋转方向为从N轴到X_L轴，从Z_P轴正方向看过去，α的旋转方向为逆时针时，否则从N轴的正方向看过去，β的旋转方向为逆时针时，否则从Z_L轴的正方向看过去，γ的旋转方向为逆时针时，否则

9.根据权利要求8所述的方法，其特征在于，从坐标系O_PX_PY_PZ_P到坐标系O_DX_DY_DZ_D的坐标变换的旋转矩阵R_d具体为：

$R_d=\left[\begin{array}{ccc}cos\mathrm{\η}& -sin\mathrm{\η}& 0\\ sin\mathrm{\η}& \cos \mathrm{\η}& 0\\ 0& 0& 1\end{array}\right]$

其中η的获取方法为：

在所述没有斜坡的平坦开阔空场地立一根细长的直杆，移动直杆使得多线激光雷达至少有两条线同时测量到直杆，且探测到直杆的每条线所测量到的直杆上的点的个数为1个，多线激光雷达测量到的直杆上的点所对应的测量数据构成集合ξ_P；

利用从多线激光雷达坐标系O_LX_LY_LZ_L到坐标系O_PX_PY_PZ_P的坐标变换的旋转矩阵R_r，从集合ξ_P中任选2个数据点变换到坐标系O_PX_PY_PZ_P下，计算直杆在坐标系O_PX_PY_PZ_P下的直线方程L_P；

利用从多线激光雷达坐标系O_LX_LY_LZ_L到坐标系O_PX_PY_PZ_P的坐标变换的旋转矩阵R_r，将集合ξ_g中的3个数据点变换到坐标系O_PX_PY_PZ_P下，计算地面在坐标系O_PX_PY_PZ_P下的平面方程P_P；

根据L_P和P_P，计算直杆与地面的交点在坐标系O_PX_PY_PZ_P下的坐标点P_C(x_PC,y_PC,z_PC)；测量得到直杆与地面的交点在坐标系O_DX_DY_DZ_D下的坐标点Q_C(x_DC,y_DC,z_DC)，连接P_C与P_Z(0,0,z_PC)得到向量连接Q_C与Q_Z(0,0,z_DC)得到向量得到向量和的夹角η：

10.根据权利要求9所述的方法，其特征在于，多线激光雷达的线数大于等于4。