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

说明书

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

技术领域

本发明属于激光雷达三维目标姿态估计技术领域，特别是涉及一种激光雷达场景三维姿态点法向量估计修正方法。

背景技术

现有的姿态估计方法主要有主成分分析法(PCA)、矩形包围盒法、模型匹配法和基于深度学习的方法。PCA方法应用较为广泛，但其对目标点云分布的完备性要求较高，由于激光雷达得到的是单个视角下的距离像数据，因此PCA方法并不适用。矩形包围盒法是基于汽车、坦克等目标具有类似矩形结构的特征，这种方法对遮挡较为敏感，会直接影响到姿态估计的结果。模型匹配法要求目标已知且具有完整的姿态模型库，目标点云特征和模型点云特征匹配度最高的姿态模型为目标点云姿态，在实际应用中较难实现。基于深度学习的姿态估计方法，一般针对二维图像，通过卷积神经网络获取姿态角数据。该方法需要大量的样本数据供训练使用，且对图像分辨率的要求较高。已提出的基于点法向量的姿态估计算法(PDVA)，这种方法对激光雷达仿真系统得到的无噪距离像具有较好的姿态估计效果，但对于真实激光雷达场景距离像，求得的代表法向量会产生较大的偏移，导致姿态估计与真实值存在一定的偏差。

发明内容

本发明目的是为了解决现有技术中的问题，提出了一种激光雷达场景三维姿态点法向量估计修正方法。本发明应用于获取的真实场景距离像，通过RANSAC的平面模型，得到最优代表法向量，减小三维姿态估计误差。本发明针对真实场景距离像，通过随机抽样一致算法对聚类中的点法向量进行筛选，修正了代表法向量的偏差，获取最优代表法向量，三维姿态估计更精确。

本发明是通过以下技术方案实现的，本发明提出一种激光雷达场景三维姿态点法向量估计修正方法，根据场景目标代表法向量估计得出场景坐标系SCS的X_s、Y_s和Z_s轴，将SCS转换为大地坐标系GCS的过程中，场景目标绕SCS的坐标轴X_s、Y_s和Z_s轴转换的角度即为三维姿态角偏航角Ψ、俯仰角θ和滚转角Φ；所述方法包括以下步骤：

步骤一：利用局部平面拟合的方法求得点云中各点的点法向量n_i，计算出点法向量的质心并求得所有n_i与的夹角θ_i；

步骤二：根据点云中点法向量与质心向量之间夹角的分布，在夹角直方图中选出频次f大于阈值λ的夹角区间，选定区间的数量即为聚簇数量k；

步骤三：初步选取代表法向量；

在分类后的每个聚类δ_i,i＝1,...,k中，对聚类δ_i中每个点法向量与聚类δ_i中的其他所有点法向量所成的夹角进行计算，并对所成的夹角进行求和，选取聚类δ_i中与其他点法向量夹角和最小的点法向量为代表法向量，记为n_pi；