[发明专利]SLAM建图过程中的平面特征融合方法

权利要求书

1.SLAM建图过程中的平面特征融合方法，其特征在于：该方法包括以下步骤：

步骤1：以激光雷达中心位置为坐标原点建立激光雷达坐标系；在连续不同时刻，根据激光雷达的三维点云数据、分割出激光雷达坐标系下的平面特征；

步骤2：在世界坐标系下，利用车载IMU和GPS经纬度解算出车辆当前位姿相对于初始位置的欧拉变换；

步骤3：将所有激光雷达坐标系下的平面特征经过欧拉变换转换到世界坐标系下；

步骤4：针对任意两个不同时刻内的任意平面特征与x-o-y平面的交线进行角度大小的计算、特征圆重合度的计算和相对距离的计算，并将满足条件的特征归为同一类；

步骤5：删除类中特征的个数少于阈值的类，并对剩余类的特征求对应的线性回归方程，之后构建出世界坐标系下的全局地图；

所述步骤3包括：利用在k时刻的欧拉变换T_k，将所有激光雷达坐标系下的平面特征转换到世界坐标系下；

所述步骤4包括：

4-1：在世界坐标系下，任意两个时刻k和k-i(i＝1,2,3…k-1)的平面特征与x-o-y平面相交而形成的任意的两个端点分别为和

4-2：端点Γ_k和Γ_k-i对应的直线方程分别是A_kx+B_ky+C_k＝0和A_k-ix+B_k-iy+C_k-i＝0；

4-3：计算平面特征夹角大小为

4-4：第一个判断条件：根据实际环境设定阈值δ，如果β小于阈值δ且大于0，则认为满足第一个判断条件；

4-5：针对端点Γ_k和Γ_k-i，以线段的中点为圆心，以线段的长度为直径定义特征圆，与其对应的特征圆圆心半径大小分别为和两特征圆圆心分别记为O_k和O_k-i；

4-6：针对端点Γ_k和Γ_k-i对应特征圆的交点和两个特征圆圆心相连形成四边形，其中两个以圆心为顶点的内角的大小的一半分别记为α_k和α_k-i，则α_k＝cos^-1((R_k²+d²-R_k-i²)/2R_kd)、α_k-i＝cos^-1((R_k-i²+d²-R_k²)/2R_k-id)，其中，d为两特征圆圆心之间的距离，

4-7：计算两特征圆重合的面积

4-7：计算两特征圆的重合度ε＝S_overlap/S_min，其中

4-8：第二个判断条件：根据实际环境设定阈值γ，如果ε大于阈值γ且小于1，则认为满足第二个判断条件；

4-9：针对两直线方程A_kx+B_ky+C_k＝0和A_k-ix+B_k-iy+C_k-i＝0、特征圆圆心O_k和O_k-i，分别计算圆心到直线之间的相对距离d_k→k-i和d_k-i→k；

4-10：相对距离的最大值d_max＝max(d_k→k-i,d_k-i→k)；

4-11：第三个判断条件：根据实际环境设定阈值η，如果d_max小于η且大于0，则认为满足第三个判断条件；

4-12：针对所有帧内的平面特征进行判断，将满足三个判断条件的特征归为同一类；

所述步骤5包括：

5-1：删除类中特征个数少于设定阈值υ的类；

5-2：针对剩余类中的平面特征，投影到地平面上，根据线段端点求解线性回归方程；

5-3：根据线段端点在回归方程上的投影点，求解欧式距离最远的两个点作为融合后新的特征端点；

5-4：针对建筑物拐角处的端点坐标不重合，采用两个坐标的平均值代替；

5-5：根据新的特征端点构建在世界坐标系下基于平面特征的全局地图。