[发明专利]安全飞行包线的估算方法

权利要求书

1.一种安全飞行包线的估算方法，其特征在于：包括以下步骤，

S1，获取周围地形环境模型的三维数据坐标，形成点云数据，获取飞行器位置坐标、运动方向、速度以及安全飞行高度；

S2，估算点云数据的表面法矢量；

S3，根据步骤S2得到的点云数据的表面法矢量、飞行器位置坐标、运动方向、速度以及安全飞行高度计算安全飞行包线。

2.如权利要求1所述的安全飞行包线的估算方法，其特征在于：所述步骤S2包括，

S2-1，读入点云数据，并获取点云个数n；

S2-2，从点云数据集中任取一点P_i；

S2-3，建立P_i的k-邻域；

S2-4，将这k+1个点用最小二乘法进行曲面拟合，得到拟合曲面的切平面；

S2-5，将得到的曲面切平面的法矢量当做点P_i的法线，然后检查法矢量方向的一致性，若指向视点，则是一致的，将得到的法矢量存储起来；若不是，则对法矢量方向进行翻转，再将得到的法矢量存储起来。

S2-6，遍历点云数据集中的所有点，重复上述步骤。

3.如权利要求2所述的安全飞行包线的估算方法，其特征在于：所述步骤S2-4包括以下步骤，

对得到的n个点云数据，P_i是测量数据中的某一点，为求得该点处的法线，先设平面方程为：

ax+by+cz+d＝0(1)

式(1)中，a²+b²+c²＝1，可以得到平面参数a，b，c，d，使得k个邻近点到该平面的距离的平方和最小，获得的拟合平面是最佳的，即满足式(2)：

e=Σidi2→min---(2)]]>

其中，d_i是点云数据中的任一点Pi(x_i,y_i,z_i)到这个平面的距离d_i＝|ax_i+by_i+cz_i-d|

采用拉格朗日乘子法求解极值，得到式(3)

f=e-λ(a2+b2+c2-1)=Σidi2-λ(a2+b2+c2-1)---(3)]]>

将式(3)两边对d求偏导，并且令偏导数为零，得到：

d=1n(aΣixi+bΣiyi+cΣizi)---(4)]]>

令质心为

d_i＝|aΔx_i+bΔy_i+cΔz_i|(5)

再对式(3)两边求a，b，c的偏导数，得：

2Σi(aΔxi+bΔyi+cΔzi)Δxi-2λa=02Σi(aΔxi+bΔyi+cΔzi)Δyi-2λb=02Σi(aΔxi+bΔyi+cΔzi)Δzi-2λc=0---(6)]]>

将上述方程组构成特征值方程得：

Ax＝λx(7)

式(7)中，

那么，求解平面参数a，b，c，就是求解矩阵的特征值和特征向量，又因为A是3阶实对称矩阵，根据矩阵的知识可知，对于实对称矩阵，特征值可利用式(7)求解，得：

λ=(Ax,x)(x,x),(x≠0)---(8)]]>

在约束条件a²+b²+c²＝1下，可得所以，e的最小值就是矩阵A的最小特征值，对应的特征向量为平面参数a，b，c，利用质心可求得d。