买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：南京航空航天大学

摘要：本发明公开了一种基于激光雷达三维点云的固定翼飞机位姿测量方法及装置，所述方法包括：使用激光雷达扫描目标区域生成三维点云；采用体素滤波和球半径滤波剔除三维点云中的噪声点；设计快速欧氏聚类，分割出三维点云中的潜在固定翼飞机点云簇；通过主成分分析计算固定翼飞机的姿态角并计算出几何中心作为位置；设计十字拟合算法和卡尔曼滤波器，对姿态角和位置计算结果进行优化。本发明采用不受光照条件影响的激光雷达，能够全天候扫描生成三维点云，实时动态对扫描结果进行去噪和分割，测量固定翼飞机的位置姿态，并建立卡尔曼滤波模型进行优化，提高固定翼飞机位姿测量准确度。

主权项：1.一种基于激光雷达三维点云的固定翼飞机位姿测量方法，其特征在于，包括以下步骤：1点云采集，使用激光雷达扫描目标区域，生成目标区域的三维点云数据集；2噪声抑制，使用体素滤波，对场景点云进行降采样，提高点云质量；根据球半径滤波规则，对场景点云中的离群点进行剔除；3点云聚类，基于快速欧氏聚类，分割出场景点云中潜在的固定翼飞机点云簇；4位姿测量，对步骤3中分割出的固定翼飞机点云簇进行主成分分析，计算出固定翼飞机的机体坐标系，进而计算出固定翼飞机的姿态角，并计算出该点云簇的几何中心，作为固定翼飞机的位置；5结果校正，通过十字拟合方法，对固定翼飞机的姿态角中的航向角进行二次计算，替换步骤4中计算的姿态角中的航向角；结合上一帧的估计位姿与当前帧的观测位姿，建立卡尔曼滤波模型，对位姿测量结果进行最优估计，修正步骤4中计算出的固定翼飞机位姿；所述步骤3实现过程如下：31以点云集为输入，随机挑选一个初始点a0，查找其附近距离阈值d之内的临近点，将临近点以及初始点a0标记为标签0；32在剩下的点云中，随机挑选一个初始点a1，查找其附近距离阈值d之内的临近点；若临近点中没有被标记过的点，将临近点以及初始点a1标记为标签；若临近点有被标记过的点，则将临近点以及初始点a1标记为与被标记点相同的标签；若附近有多个被标记过的点，则将将临近点以及初始点a1标记为与被标记点中标签较小的标记点相同的标签；33循环执行31和32，直到点云集中所有点都有自己的标签，标签相同的即为一个点云簇；所述步骤4实现过程如下：主成分分析首先计算固定翼飞机点云数据的质心，然后将固定翼飞机点云移动到以质心为原点的坐标系中，消除数据的偏移，将所有固定翼飞机点云围绕原点对齐；计算中心化后固定翼飞机点云中的任意一点p，计算其邻域k个点构成的协方差矩阵C： 其中，为邻域点集的三维质心；对协方差矩阵C进行特征值分解，得到特征值λ和特征向量ν：Cνm＝λνm根据特征值的大小进行排序，其对应的特征向量ν1、ν2和ν3分别对应固定翼飞机机体坐标系的x轴、y轴和z轴；点云簇的几何中心计算，是点云簇所占据的三维空间的中间点，即x轴、y轴和z轴方向跨度的中点；步骤5所述的通过十字拟合方法对固定翼飞机的姿态角中的航向角进行二次计算实现过程如下：将固定翼飞机点云簇投影至固定翼飞机机体坐标系的x轴、y轴组成的平面上，通过最小二乘算法的原理，正交直线lx和ly以点云簇几何中心处为交点，求解旋转矩阵R使平面上的所有点qi到lx、ly中较近的一条直线的距离之和最小，即： 其中，函数disq,l表示点q,到直线l的距离，R×l表示通过旋转矩阵R对直线l进行旋转；步骤5所述建立卡尔曼滤波模型对位姿测量结果进行最优估计实现过程如下：以固定翼飞机的位置p＝[px,py,pz]、姿态速度v＝[vx,vy,vz]、角速度为状态值x＝[p,v,α,ω]T，确定协方差矩阵P以及过程噪声协方差矩阵Q、测量噪声协方差矩阵R；建立固定翼飞机的状态转移方程结合过程噪声协方差矩阵，预测当前固定翼飞机的状态和协方差矩阵其中，和分别表示k的预测值和k-1时刻的估计值，Ak是状态转移矩阵；wk是过程噪声，服从高斯分布wk～N0,Q，Pk和分别表示k的预测协方差矩阵和k-1时刻的估计协方差矩阵；建立激光雷达三维点云测量固定翼飞机的测量方程zk＝Hkxk+υk，结合测量噪声协方差矩阵，计算卡尔曼增益： 其中，Hk是测量矩阵，υk是测量噪声，服从高斯分布υk～N0,R；根据测量值、预测值和卡尔曼增益更新固定翼飞机状态估计值：和协方差矩阵其中估计值即为固定翼飞机位姿测量结果的最优估计。

全文数据：

权利要求：

百度查询： 南京航空航天大学 基于激光雷达三维点云的固定翼飞机位姿测量方法及装置