[发明专利]旋转式三维激光雷达的点云匹配方法及系统

权利要求书

1.一种旋转式三维激光雷达的点云匹配方法，所述旋转式三维激光雷达(10)设置在载体上，所述旋转式三维激光雷达(10)具有在高度方向上间隔设置的J个雷达线，其特征在于，载体行进时，旋转式三维激光雷达(10)旋转且扫描四周环境，定义第i个采样周期得到的旋转式三维激光雷达(10)的点云数据为第i帧点云数据，第i帧点云数据中由第j个雷达线扫描的各个环境位置点的数据构成第i帧点云数据的第j个雷达环，定义环境位置点M_i,j,k为第i帧点云数据中由第j个雷达线扫描的第k个环境位置点，所述点云数据为带有时间戳信息和雷达线序号的点云数据；

步骤(A)：根据第i帧点云数据判断环境位置点M_i,j,k对应的水平方向上的环境角度变化值θ₁(i,j,k)是否在预设角度范围内，若判断结果为是，则判定环境位置点M_i,j,k为第一特征点，否则，判定环境位置点M_i,j,k为第二特征点，j＝1,2,…,J，k＝1,2,…,K，K为每个雷达线在每个采样周期中扫描的环境位置点的个数；

步骤(B)：构建误差方程求解误差方程，得到i+1帧到i帧平移参数tr_i+1＝(dx_i+1,dy_i+1,dz_i+1)^T、偏航角ψ'_i+1、翻滚角俯仰角θ'_i+1；

其中：

ψ′_i+1、θ'_i+1分别为第i帧点云数据到第i+1帧点云数据变换的偏航角、翻滚角、俯仰角，偏航角ψ'_i+1根据第i帧点云数据与第i+1帧点云数据中具有对应关系的环境位置点在雷达环中所在位置序号之差、旋转式三维激光雷达(10)的转动参数计算得到；

EA_i+1,u1为第u1个第一直线与坐标(xa'_i+1,u1,ya'_i+1,u1,za'_i+1,u1)之间的距离，EB_i+1,v1为第v1个第一平面与坐标(xb'_i+1,v1,yb'_i+1,v1,zb'_i+1,v1)之间的距离；

第u1个第一直线为经过第i+1帧中第u1个第一特征点在第i帧点云数据中的对应位置点、第i+1帧中第u1个第一特征点在第i帧点云数据中的对应位置点的同位位置点的直线；u1＝1,2,…,U(i+1)，U(i+1)为与第i帧点云数据的雷达环具有对应关系的第i+1帧点云数据的雷达环中第一特征点的总数；

第v1个第一平面由第i+1帧中第v1个第二特征点在第i帧点云数据中的对应位置点和次对应位置点、第i+1帧中第v1个第二特征点在第i帧点云数据中的对应位置点的同位位置点形成；v1＝1,2,…,V(i+1)，V(i+1)为与第i帧点云数据的雷达环具有对应关系的第i+1帧点云数据的雷达环中第二特征点的总数；

坐标(xa_i+1,u1,ya_i+1,u1,za_i+1,u1)为U(i+1)个第一特征点中第u1个第一特征点的坐标；

坐标(xb_i+1,v1,yb_i+1,v1,zb_i+1,v1)为V(i+1)个第二特征点中第v1个第二特征点的坐标；

旋转矩阵

其中，四元数q_i+1的表达式如下

步骤(C)：对于第i+1帧点云数据中的各个环境位置点的坐标(x_i+1,j,k,y_i+1,j,k,z_i+1,j,k)，利用下式计算变换后坐标(x'_i+1,j,k,y'_i+1,j,k,z'_i,j,k)

(x'_i+1,j,k,y'_i+1,j,k,z'_i,j,k)^T＝R_i+1×(x_i+1,j,k,y_i+1,j,k,z_i+1,j,k)^T+tr_i+1。