[发明专利]一种基于LeGO-LOAM的分步式帧间位姿估计算法

权利要求书

1.一种基于LeGO-LOAM的分步式帧间位姿估计算法，其特征在于，具体方法如下：

(1)读入激光点云数据；

系统通过激光雷达获取周围环境的点云信息，并将点云信息进行处理，将激光束编号，从仰角最小的激光束开始编号，作为点云的纵坐标，激光的水平编号，从旋角为0的点开始编号，作为其横坐标，对点云进行编号，最后记录点云的深度信息、水平旋角和仰角；

(2)对点云进行分类；

点云分类通过两步完成，第一步通过地面点计算以及公式(1)的原理完成地面点的标记；

θ＝atan2(ΔZ-ΔX)

ΔX＝R_r-1cosα-R_rcosβ

ΔZ＝R_r-1sinα-R_rsinβ (1)

公式(1)中R_r-1表示仰角为α的激光束的长度，R_r表示仰角为β的激光束的长度，当θ大于阈值时，则该点标记为非地面点，否则标记为地面点，计入地面点集合

完成地面点标记后，进行点云集群的划分，其原理点云集群划分和公式(2)所示；

公式(2)中β表示点云中最临近的两个点到激光雷达中心连线的夹角，θ为两个点中深度更大的点到激光雷达的连线和两点之间连线的夹角，d₁为两点中深度更大的点的深度，d₂为另一个点的深度，若θ大于阈值这说明两个点不为一个集群，否则两个点为一个集群，在完成所有点的集群划分后，对点的集群进行标记并剔除小集群点；

(3)提取地面特征点和边缘特征点；

首先计算每个点的曲率值，公式如下式(3)所示；

式中|S|表示点集中点的个数，||r_i||表示点i的深度，通过式(3)得到点云中每个点的曲率值，在非地面点中，点的曲率越大则表明此处点云深度变化幅度越大，并计入边缘特征点集合

若特征点集中于一个区域，可能会出现某一时刻该区域不可观测导致的位姿跟踪丢失，造成位姿估计出现较大的误差，所以对点云进行分散采样，将每束点云平均划分为六个区域，在六个区域中各选取2个曲率最大的边缘特征点计入集合F_e，或四个曲率值最小的地面特征点计入集合F_p；

(4)分步帧间位姿计算：

针对无人车平台，将帧间位姿分为六个部分，三个旋转角度：rx俯仰角，ry偏航角和rz翻滚角，三个平移量：tx左右方向上的位移，ty竖直方向上的位移和tz前后方向上的位移；

在求取帧间位姿时，设相邻帧间，由后一帧到前一帧的旋转矩阵为R，平移向量为-t，并且将后一帧中的边缘特征点F_e和地面特征点F_p按照下述公式(4)投影至前一帧中，式中代表k时刻雷达坐标系下的点i的坐标，而表示k+1时刻雷达坐标系下点i的坐标；

式(4)中R和t可用(r_x，r_y，r_z，t_x，t_y，t_z)表示，如下式(5)可见：

相邻帧间仅匹配具有相同的集群标志的点，并采用LOAM算法中提出的“点到线”和“点到面”的匹配方案，该匹配方案的误差由下式(6)给出；

式中表示边缘特征点，和表示k时刻中与匹配的两个边缘特征点的坐标，表示地面特征点，和表示k时刻中与匹配的地面点的坐标，d_ξ表示边缘点匹配中的“点到线”的距离，d_φ表示地面点匹配中的“点到面”的距离；

针对无人车平台位姿变化集中于水平面上的运动，将帧间位姿分为两步求解，第一步为通过地面特征点求解得到(rx，rz，ty)，其最小二乘原理如下式(7)所示；

通过式(7)求解得到(rx，rz，ty)之后，将(rx，rz，ty)代入第二步，相对于LeGO-LOAM算法的仅通过边缘特征点求解(ry，tx，tz)，通过边缘特征点求解(rx，ry，rz，tx，ty，tz)，其原理如下式(8)；

通过上述步骤完成分布式帧间六自由度位姿估计。