[发明专利]一种激光雷达场景三维姿态点法向量估计修正方法

摘要

本发明提出一种激光雷达场景三维姿态点法向量估计修正方法，所述方法包括步骤一：利用局部平面拟合的方法求得点云中各点的点法向量ni，计算出点法向量的质心并求得所有ni与的夹角θi；步骤二：根据点云中点法向量与质心向量之间夹角的分布，在夹角直方图中选出频次f大于阈值λ的夹角区间，选定区间的数量即为聚簇数量k；步骤三：初步选取代表法向量；步骤四：选取待拟合的点集对代表法向量进行修正；步骤五：采用随机抽样一致算法得到最优拟合平面，计算最终代表法向量。本发明针对真实场景距离像，通过随机抽样一致算法对聚类中的点法向量进行筛选，修正了代表法向量的偏差，获取最优代表法向量，三维姿态估计更精确。

主权项

1.一种激光雷达场景三维姿态点法向量估计修正方法，其特征在于：根据场景目标代表法向量估计得出场景坐标系SCS的X_s、Y_s和Z_s轴，将SCS转换为大地坐标系GCS的过程中，场景目标绕SCS的坐标轴X_s、Y_s和Z_s轴转换的角度即为三维姿态角偏航角Ψ、俯仰角θ和滚转角Φ；所述方法包括以下步骤：/n步骤一：利用局部平面拟合的方法求得点云中各点的点法向量n_i，计算出点法向量的质心并求得所有n_i与的夹角θ_i；/n步骤二：根据点云中点法向量与质心向量之间夹角的分布，在夹角直方图中选出频次f大于阈值λ的夹角区间，选定区间的数量即为聚簇数量k；/n步骤三：初步选取代表法向量；/n在分类后的每个聚类δ_i,i＝1,...,k中，对聚类δ_i中每个点法向量与聚类δ_i中的其他所有点法向量所成的夹角进行计算，并对所成的夹角进行求和，选取聚类δ_i中与其他点法向量夹角和最小的点法向量为代表法向量，记为n_pi；/n对每个聚簇中的点加以计数，记为count_i，选取count_i最大的聚簇的代表点法向量为目标坐标系的一个方向向量记为n，依次选取按count_i降序排列的聚簇代表点法向量计算其与n所成的角度，选取所成角度在[90°-δ，90°+δ]范围内的代表法向量为目标坐标系的另一个方向向量；其中δ是一个度数阈值，其大小根据实际情况进行相应调整；/n步骤四：选取待拟合的点集对代表法向量进行修正；/n计算聚类δ_i中所有点g_i∈δ_i与代表法向量n_pi对应点的欧氏距离ρ_i，求得距离平均值选取距离值小于的点，得到点集G₁：/n /n计算聚类δ_i中所有点法向量n_i与代表法向量n_pi的夹角值θ_pi，选取夹角值小于45°的点，得到点集G₂：/n /n其中，代表法向量n_pi对应点坐标表示为(x_p,y_p,z_p)，点法向量n_i对应点坐标表示为(x_i,y_i,z_i)，最后求得待拟合的点集为G_d＝G₁∩G₂；/n步骤五：采用随机抽样一致算法得到最优拟合平面，计算最终代表法向量。/n