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

本发明涉及机器人环境理解领域，特别是一种面向斜坡区域的基于多线激光雷达的斜坡角度估计方法。

背景技术：

随着移动机器人和传感器技术的不断发展，其应用领域也在不断拓展。对于机器人所面向的室外环境来说，斜坡是普遍存在的。斜坡检测的质量将直接影响到机器人任务执行的性能。国内外研究人员在斜坡角度估计方面做出了一定的研究。在斜坡区域的信息采集方面，多利用立体相机或深度相机进行感知，但通过立体相机或深度相机获得的距离信息往往不够准确，这降低了斜坡角度估计的质量。在方法上多通过寻找斜坡纵向截面的方式来计算斜坡的角度，该方法比较直观，但由于斜坡位置和坡面朝向等因素的影响，纵向截面的确定比较困难，这对斜坡角度的估计准确度造成较大的影响。与纵向截面特征相比，斜坡坡面法向量这一全局特征能够更有效的反映斜坡。考虑到多线激光雷达能够获取更为准确的距离信息，有必要利用多线激光雷达获取斜坡区域的距离信息并基于斜坡坡面法向量实现斜坡角度的估计。

技术实现要素：

为了解决现有技术中的上述问题，即为了解决现有技术对斜坡角度的估计准确度难以满足需求的问题，本发明提出了一种面向斜坡区域的基于多线激光雷达的斜坡角度估计方法，提高了斜坡角度估计的质量。

本发明提出的一种面向斜坡区域的基于多线激光雷达的斜坡角度估计方法，包括以下步骤：

步骤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，依据所述斜坡坡面方程和所述地面方程计算得到斜坡角度的估计结果。

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

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

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

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

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

优选地，步骤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)。

优选地，步骤S2中对D_L进行坐标变换和滤波处理，生成新的激光雷达点云数据D_S的方法为：

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

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

优选地，利用统计滤波对D_R进行滤波处理，具体方法包括：

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

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

d_F＝μ+δ·σ

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

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

优选地，所述多线激光雷达坐标系O_LX_LY_LZ_L到机器人坐标系O_RX_RY_RZ_R的坐标变换关系为：

其中，(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的原点O_L(x_LO,y_LO,z_LO)重合，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的平移变换得到的坐标系。

优选地，多线激光雷达坐标系O_LX_LY_LZ_L与机器人坐标系O_RX_RY_RZ_R之间的平移矩阵T的计算方法为：

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

优选地，从多线激光雷达坐标系O_LX_LY_LZ_L到坐标系O_PX_PY_PZ_P的坐标变换的旋转矩阵R_r具体为：

在没有斜坡的平坦开阔空场地获取地面的多线激光雷达点云数据，从中任选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轴的正方向看过去，γ的旋转方向为逆时针时，否则

优选地，从坐标系O_PX_PY_PZ_P到坐标系O_DX_DY_DZ_D的坐标变换的旋转矩阵R_d具体为：

其中η的获取方法为：

在所述没有斜坡的平坦开阔空场地立一根细长的直杆，移动直杆使得多线激光雷达至少有两条线同时测量到直杆，且探测到直杆的每条线所测量到的直杆上的点的个数为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)得到向量得到向量和的夹角η：

优选地，多线激光雷达的线数大于等于4。

本发明利用多线激光雷达获取斜坡区域的距离信息并基于斜坡坡面法向量实现斜坡角度的估计，提高斜坡角度估计的质量，为机器人环境理解等方面的应用提供技术支持。

附图说明

图1是本发明面向斜坡区域的基于多线激光雷达的斜坡角度估计方法的流程示意图。

具体实施方式

下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是，这些实施方式仅仅用于解释本发明的技术原理，并非旨在限制本发明的保护范围。

本发明的一种面向斜坡区域的基于多线激光雷达的斜坡角度估计方法，包括以下步骤：

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

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

该步骤中对D_L进行坐标变换和滤波处理，生成新的激光雷达点云数据D_S的方法为：

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

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

该步骤中利用统计滤波对D_R进行滤波处理，具体方法包括：

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

步骤S222，计算距离阈值d_F，如公式(1)所示：

d_F＝μ+δ·σ (1)

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

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

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

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

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

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

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

步骤S34，以最后一次迭代得到的平面作为斜坡坡面，并输出斜坡坡面方程；斜坡坡面方程在机器人坐标系下表示为A_SL·x+B_SL·y+C_SL·z+D_SL＝0，其中A_SL、B_SL、C_SL、D_SL为斜坡坡面方程的四个系数。

步骤S4，从D_S中选取3个到平面X_RO_RY_R的距离均不超过给定阈值d_Z的数据点，并利用该组数据点计算并得到地面方程；地面方程在机器人坐标系下表示为A_GF·x+B_GF·y+C_GF·z+D_GF＝0，其中A_GF、B_GF、C_GF、D_GF为地面方程的四个系数。

步骤S5，依据所述斜坡坡面方程和所述地面方程计算得到斜坡角度的估计结果。斜坡角度的具体计算方法如公式(2)所示：

其中，为斜坡角度的估计结果，u_p＝(A_SL,B_SL,C_SL)为斜坡坡面法向量，u_xoy＝(A_GF,B_GF,C_GF)为地面法向量。

本实施例中，多线激光雷达坐标系O_LX_LY_LZ_L到机器人坐标系O_RX_RY_RZ_R的坐标变换关系如公式(3)所示：

其中，(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的平移变换得到的坐标系。

下面对多线激光雷达坐标系到机器人坐标系的坐标变换关系的具体获取方法进行详细描述：

记t_x、t_y、t_z分别为多线激光雷达坐标系O_LX_LY_LZ_L的原点O_L(x_LO,y_LO,z_LO)与机器人坐标系O_RX_RY_RZ_R的原点O_R(x_RO,y_RO,z_RO)在三个坐标分量上的偏差，t_x、t_y、t_z可通过测量得到，从而得到多线激光雷达坐标系O_LX_LY_LZ_L与机器人坐标系O_RX_RY_RZ_R之间的平移矩阵T如公式(4)所示：

选择一块平坦开阔的空场地(没有斜坡的空场地)，获取地面的多线激光雷达点云数据，从中任选3个数据点构成集合ξ_g。

基于集合ξ_g中的3个数据点计算得到地面在多线激光雷达坐标系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轴，表示为得到从多线激光雷达坐标系O_LX_LY_LZ_L到坐标系O_PX_PY_PZ_P的坐标变换的旋转矩阵R_r如公式(5)所示：

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

在前述的平坦开阔空场地(没有斜坡的空场地)立一根细长的直杆，移动直杆使得多线激光雷达至少有两条线同时测量到直杆，且探测到直杆的每条线所测量到的直杆上的点的个数为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)得到向量得到向量和的夹角η如公式(6)所示：

从坐标系O_PX_PY_PZ_P到坐标系O_DX_DY_DZ_D的坐标变换的旋转矩阵R_d如公式(7)所示：

于是，得到从多线激光雷达坐标系O_LX_LY_LZ_L到机器人坐标系O_RX_RY_RZ_R的坐标变换如公式(3)所示。

本实施例中，为保证对给定斜坡区域的斜坡角度的估计效果，多线激光雷达的线数大于等于4。

本实施例中，可做如下参数取值：d_H＝0.03米，M_K＝50，λ＝0.9，d_z＝0.03米，m＝30，δ＝2.0。

本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。