CN109341697B - 一种无人机航路规划安全性评估方法 - Google Patents

Abstract

本发明一种无人机航路规划安全性评估方法，步骤如下：1)根据OBB包围盒构造算法计算包围盒中心点坐标和主方向半径；2)采用雅克比迭代算法计算得到OBB包围盒协方差矩阵特征值和特征向量；3)分别构造禁飞区和无人机包围盒，采用包围盒碰撞检测算法对无人机航路进行安全性评估，如果无人机航路经过禁飞区，则该航路不满足安全性要求。本发明采用包围盒碰撞检测原理，可以精确判断无人机航路是否经过禁飞区，既安全可靠又不浪费资源，为无人机航路规划安全性分析提供了解决方案。

Description

一种无人机航路规划安全性评估方法

技术领域

本发明涉及一种无人机航路规划安全性评估方法，属于无人机航路规划领域。

背景技术

随着科学技术的发展和人民生活水平的提高，无人机逐渐走进了人们的生活中，无人机广泛参与影像航拍、摄影测量、遥感数据采集、农业植保、国土测绘等近距离、短途应用，随着无人机续航能力的增加，逐步发展为中大型无人机，应用也更加广泛，涉及电力巡查、环境监测、快递运输等。而随着无人机航程的增加，要对无人机开展航路规划，在无人机航路规划中，要避免进入重要会议区、重要建筑等禁飞区上空，现阶段主要考虑无人机飞行直接远离禁飞区上空，这种方法虽然可以保证无人机远离禁飞区，而且规划简单，但是增加了无人机航程，浪费资源成本，冗余过大。因此急需一种合理、科学的无人机航路规划安全性评估方法，可以精确检测无人机航路是否经过禁飞区上空，既安全可靠又不浪费资源。

发明内容

本发明所要解决的技术问题是：克服现有技术的不足，提供本发明一种无人机航路规划安全性评估方法，采用包围盒碰撞检测原理，可以精确判断无人机航路是否经过禁飞区，既安全可靠又不浪费资源，为无人机航路规划安全性分析提供了解决方案。

本发明包括如下技术方案：一种无人机航路规划安全性评估方法，步骤如下：

1)根据OBB包围盒构造算法计算包围盒中心点坐标和主方向半径；

2)采用雅克比迭代算法计算得到OBB包围盒协方差矩阵特征值和特征向量；

3)分别构造禁飞区和无人机包围盒，采用包围盒碰撞检测算法对无人机航路进行安全性评估，如果无人机航路经过禁飞区，则该航路不满足安全性要求。

所述步骤1)OBB包围盒构造算法计算包围盒中心点坐标和主方向半径具体方法为：

11)计算得到空间多面体协方差矩阵A；

12)计算得到包围盒中心点坐标(x′ y′ z′)；

13)计算得到包围盒主方向半径r_x，r_y，r_z。

所述步骤11)计算得到空间多面体协方差矩阵的具体过程为：

设空间多面体顶点包括n个坐标点位，第i个点位坐标为(x_i y_i z_i)；

顶点分布的均值(μ_x μ_y μ_z)：

协方差矩阵A为：

a₁₁＝cov(x,x)；a₁₂＝cov(x,y)；a₁₃＝cov(x,z)；

a₂₁＝cov(y,x)；a₂₂＝cov(y,y)；a₂₃＝cov(y,z)；

a₃₁＝cov(y,x)；a₃₂＝cov(y,y)；a₃₃＝cov(y,z)；

所述步骤12)计算包围盒中心点坐标的具体方法为：

计算得到协方差矩阵A的三个特征向量，三个特征向量确定了OBB包围盒的三个主方向，利用三个主方向的坐标系与空间多面体点位坐标(x_i y_i z_i)的夹角，将(x_i y_i z_i)变换到OBB包围盒的三个主方向上：

(x_i y_i z_i)→(x′_i y′_i z′_i)；

分别计算坐标变换后的每个元素的最大值和最小值：

x_max＝max(x′_i)

y_max＝max(y′_i)

z_max＝max(z′_i)

x_min＝min(x′_i)

y_min＝min(y′_i)

z_min＝min(z′_i)

i＝1～n

则包围盒中心点位坐标(x′ y′ z′)为：

所述步骤13)中包围盒在三个主方向的半径大小为：

所述的采用雅克比迭代算法计算OBB包围盒协方差矩阵特征值和特征向量具体方法为：

21)初始化特征向量为三维对角阵V，即主对角线的元素都是1，其他元素为0；

22)在包围盒协方差矩阵A的非主对角线元素中，找到绝对值最大元素a_pq；

23)计算得到矩阵U_pq；

24)计算得到矩阵A₁,V₁：

V₁＝V·U_pq

25)更新包围盒协方差矩阵A和特征向量矩阵V：

A＝A₁，V＝V₁

26)若当前迭代前的包围盒协方差矩阵A的非主对角线元素中最大值小于给定的阈值epsl时，停止计算；否则，重复执行步骤22)～26)；停止计算时，得到特征值以及特征向量矩阵。

所述步骤23)计算得到矩阵U_pq的具体方法为：

其中，左上

为第p行第p列，

为第p行第q列，

为第q行第p列，右下

为第q行第q列；

所述步骤3)中分别构造禁飞区和无人机航路包围盒，采用包围盒碰撞检测算法对无人机航路进行安全性评估的具体方法为：

31)设禁飞区包围盒中心点为O_A，根据步骤11)～13)构造禁飞区包围盒，得到主方向单位向量为

主方向单位向量分别对应的半径为a₁,a₂,a₃；设无人机航路包围盒中心点为O_B，根据步骤11)～13)构造无人机航路包围盒，主方向单位向量为

主方向单位向量分别对应的半径为b₁,b₂,b₃；

32)依次计算15条投影轴单位向量

每个OBB包围盒的中心在投影轴线

上的投影就是其投影的中点，计算得到禁飞区包围盒和无人机航路包围盒的对应的投影区段半径r_Ak和r_Bk；

33)定义平移向量

平移向量在投影轴

的长度为

当且仅当满足下式时，证明投影轴

为禁飞区包围盒和无人机航路包围盒的分离轴：

即两个包围盒投影到

方向上不会发生碰撞；依次对15条投影轴

进行分离轴计算，如果检测到有一条分离轴存在，则确定两个包围盒是不相交的，立即停止在其余分离轴上的测试；

34)无人机航路任意连续的两个轨迹点及其各个经过点的不确定性大小构成一个个空间圆柱体，n个轨迹点构成n-1个包围盒，根据步骤31)～33)逐个测试无人机航路包围盒是否与禁飞区包围盒相交，如果检测到其中一个无人机航路包围盒与禁飞区包围盒相交，则立即停止测试，证明该航路经过禁飞区，不满足安全性要求；如果检测完毕所有无人机航路包围盒与禁飞区包围盒均不相交，则证明该航路不经过禁飞区，满足安全性要求。

所述步骤32)依次计算15条投影轴单位向量

的具体过程为：

所述步骤32)中计算得到禁飞区包围盒和无人机航路包围盒的对应的投影区段半径r_Ak和r_Bk的具体方法为：

本发明的有益效果是：

本发明一种无人机航路规划安全性评估方法，针对禁飞区和无人机航路的三维特征，建立禁飞区包围盒和无人机航路包围盒，通过包围盒碰撞检测原理判断两个包围盒之间是否存在分离轴，以此确定无人机航路是否经过禁飞区。本方法可以精确、快速判断无人机航路是否经过禁飞区并且易于实现，相对现有技术手段，本方法对无人机航路规划更加合理、缩短了无人机航程使得无人机航路规划既安全可靠，又经济节约。

附图说明

图1为禁飞区包围盒示意图；

图2为无人机航路包围盒示意图；

图3为碰撞检测算法原理示意图；

图4为无人机航路规划安全性分析流程示意图。

具体实施方式

1)禁飞区OBB包围盒(Oriented Bounding Box，方向包围盒)构造

(1)禁飞区包围盒建模

禁飞区形状如图1所示，区域边界点由经纬高表示，因此需要将2n个边界点通过坐标变换转换到地心坐标系，底面上的边界点(图1中黑色圆圈)通过以下公式转换：

顶面上的边界点(图1中黑色三角)通过以下公式转换：

其中，B_i为禁飞区底面第i个点的地心纬度，L_i为禁飞区底面第i个点的经度，R为地球半径，h为禁飞区高程。

上述2n个坐标点的坐标均值如下：

协方差矩阵A为：

a₁₁＝cov(x,x)；a₁₂＝cov(x,y)；a₁₃＝cov(x,z)；

a₂₁＝cov(y,x)；a₂₂＝cov(y,y)；a₂₃＝cov(y,z)；

a₃₁＝cov(y,x)；a₃₂＝cov(y,y)；a₃₃＝cov(y,z)；

(2)求协方差矩阵A特征值和特征向量

a)初始化特征向量矩阵V

b)求协方差矩阵A非主对角线元素最大值：首先设置一个最大值为maxa＝0，

if(a_pq≥maxa)

{

maxa＝|a_pq|(p≠q)

}

c)计算

及矩阵U_pq

根据三角公式求

(这里取较小的值)

根据三角公式求

其中，左上

为第p行第p列，

为第p行第q列，

为第q行第p列，右下

为第q行第q列。

d)求

V₁＝V·U_pq

e)更新协方差矩阵A和特征向量矩阵V

A＝A₁

V＝V₁

f)检验矩阵A目前非主对角线元素是否小于收敛标准epsl，不小于则返回重复(b)～(f)的计算；如果小于则可认为A非主对角线元素均为0，这时特征向量矩阵V中的每一列都可以看做协方差阵的特征向量，即禁飞区包围盒的三个轴向向量。

(3)求禁飞区包围盒中心和轴向半径

禁飞区包围盒三个轴向向量确定了禁飞区包围盒的三个主方向，将禁飞区点坐标(x_i y_i z_i)变换到禁飞区包围盒的3个主方向上去：

(x_i y_i z_i)→(x′_i y′_i z′_i) (i＝1～2n)

分别计算各禁飞区各个顶点在禁飞区包围盒主方向的最大值和最小值：

x_max＝max(x′_i)

y_max＝max(y′_i)

z_max＝max(z′_i)

x_min＝min(x′_i)

y_min＝min(y′_i)

z_min＝min(z′_i)

i＝1～2n

包围盒中心点位坐标(x′ y′ z′)为：

包围盒在三个主方向的大小为：

2)无人机航路包围盒构造

(1)无人机航路包围盒建模

无人机飞行轨迹是由规划的无人机航路离散的经过点组成的，加上其各个经过点的不确定性大小构成一个个空间圆柱体。因此任意连续的两个轨迹点便形成了一个包围盒(如图2所示)。n个轨迹点可以构成n-1个包围盒。

假设无人机经过B_pt1与B_pt2两点，不确定性大小为R，则可形成以下圆柱体：圆柱体中心为O，是B_pt1与B_pt2连线的中点，圆柱体底面是以B_pt1为圆心，不确定性大小R为半径的圆，圆柱体顶面是以B_pt2为圆心，不确定性大小R为半径的圆。

(2)包围盒中心及轴向半径计算

无人机轨迹包围盒中心记为O，三个轴向向量分别记为

和

在垂直轴向的平面内，其方向是不确定的，只要两者相互垂直即可，为简化运算，规定

Z方向坐标为0。

记轨迹包围盒三个轴向单位向量的坐标分别为(x_b1 y_b1 z_b1)，(x_b2 y_b2 z_b2)和(x_b3y_b3 z_b3)。

因为B_pt1与B_pt2坐标已知，

再对其进行单位化：

因为

与

垂直，故有

即：

x_b1·x_b3+y_b1·x_b3+z_b1·z_b3＝0

又已知z_b1＝0，且轴向向量均为单位向量，故可得以下方程组：

求得

又

可求得

包围盒中心点O为B_pt1与B_pt2连线的中点：

轴向半径为：

依次选取无人机航路相邻两个点的坐标B_pt_n与B_pt_n+1(0<n<轨迹点数)，将B_pt_n与B_pt_n+1坐标数据依次赋给B_pt1与B_pt2，即可按照以上算法求取B_pt1与B_pt2构成的包围盒的中心、轴向单位向量和轴向半径。

3)包围盒碰撞检测

包围盒碰撞检测原理如图3所示，设禁飞区包围盒A，包围盒中心A.pt，包围盒三个轴向向量

对应的三个轴向半径分别为：a₁ a₂ a₃。

无人机航路包围盒B，包围盒中心B.pt，包围盒三个轴向向量

对应的三个轴向半径分别为：b₁ b₂ b₃。

(1)计算由包围盒A中心到包围盒B中心的位移向量：

(2)计算15条投影轴

将A和B的三条轴向量分别赋给

剩下9条：

(3)包围盒相交判定

依次判断投影轴

上是否满足

k＝1～15

若有一条投影轴满足以上不等式，则两个包围盒不会发生碰撞，可立即停止在其余投影轴上的测试。

依次更新无人机航路包围盒的坐标，直到遍历无人机航路的所有坐标点，如果禁飞区包围盒与一个无人机航路包围盒相交，则遍历停止，认为无人机航路进入禁飞区，该航路不满足安全性要求；如果遍历结束所有坐标点，禁飞区包围盒与无人机航路包围盒都不发生碰撞，则无人机航路不进入禁飞区，该航路满足安全性要求。

无人机航路规划安全性分析流程如图4所示。

(4)试验验证

为了验证无人机航路安全性评估方法的准确性，选取了一组无人机航路和3组禁飞区进行了测试，经过测试验证，该方法可以精确判断无人机航路是否经过禁飞区。

测试结果表明，本发明提出的一种无人机航路规划安全性评估方法可以精确判断无人机航路是否经过禁飞区，方法合理，可行。

尽管上文对本发明的具体实施方式进行了详细的描述和说明，但是应该指明的是，对上述实施方式进行各种改变和修改，都不脱离本发明的精神和所附的权利要求记载的范围。

Claims (1)

1.一种无人机航路规划安全性评估方法，其特征在于步骤如下：

1)根据OBB包围盒构造算法计算包围盒中心点坐标和主方向半径；

2)采用雅克比迭代算法计算得到OBB包围盒协方差矩阵特征值和特征向量；

3)分别构造禁飞区和无人机包围盒，采用包围盒碰撞检测算法对无人机航路进行安全性评估，如果无人机航路经过禁飞区，则该航路不满足安全性要求；

所述步骤1)OBB包围盒构造算法计算包围盒中心点坐标和主方向半径具体方法为：

11)计算得到空间多面体协方差矩阵A；

12)计算得到包围盒中心点坐标(x′ y′ z′)；

13)计算得到包围盒主方向半径r_x，r_y，r_z；

所述步骤11)计算得到空间多面体协方差矩阵的具体过程为：

设空间多面体顶点包括n个坐标点位，第i个点位坐标为(x_i y_i z_i)；

顶点分布的均值(μ_x μ_y μ_z)：

协方差矩阵A为：

a₁₁＝cov(x,x)；a₁₂＝cov(x,y)；a₁₃＝cov(x,z)；

a₂₁＝cov(y,x)；a₂₂＝cov(y,y)；a₂₃＝cov(y,z)；

a₃₁＝cov(z,x)；a₃₂＝cov(z,y)；a₃₃＝cov(z,z)；

所述步骤12)计算包围盒中心点坐标的具体方法为：

计算得到协方差矩阵A的三个特征向量，三个特征向量确定了OBB包围盒的三个主方向，利用三个主方向的坐标系与空间多面体点位坐标(x_i y_i z_i)的夹角，将(x_i y_i z_i)变换到OBB包围盒的三个主方向上：

(x_i y_i z_i)→(x′_i y′_i z′_i)；

分别计算坐标变换后的每个元素的最大值和最小值：

x_max＝max(x′_i)

y_max＝max(y′_i)

z_max＝max(z′_i)

x_min＝min(x′_i)

y_min＝min(y′_i)

z_min＝min(z′_i)

i＝1～n

则包围盒中心点位坐标(x′ y′ z′)为：

所述步骤13)中包围盒在三个主方向的半径大小为：

所述步骤3)中分别构造禁飞区和无人机航路包围盒，采用包围盒碰撞检测算法对无人机航路进行安全性评估的具体方法为：

31)设禁飞区包围盒中心点为O_A，根据步骤11)～13)构造禁飞区包围盒，得到主方向单位向量为

主方向单位向量分别对应的半径为a₁,a₂,a₃；设无人机航路包围盒中心点为O_B，根据步骤11)～13)构造无人机航路包围盒，主方向单位向量为

主方向单位向量分别对应的半径为b₁,b₂,b₃；

32)依次计算15条投影轴单位向量

每个OBB包围盒的中心在投影轴单位向量

上的投影就是其投影的中点，计算得到禁飞区包围盒和无人机航路包围盒的对应的投影区段半径r_Ak和r_Bk；

33)定义平移向量

平移向量在投影轴单位向量

的长度为

当且仅当满足下式时，证明投影轴单位向量

为禁飞区包围盒和无人机航路包围盒的分离轴：

即两个包围盒投影到投影轴单位向量

方向上不会发生碰撞；依次对15条投影轴单位向量

进行分离轴计算，如果检测到有一条分离轴存在，则确定两个包围盒是不相交的，立即停止在其余分离轴上的测试；

34)无人机航路任意连续的两个轨迹点及其各个经过点的不确定性大小构成一个个空间圆柱体，n个轨迹点构成n-1个包围盒，根据步骤31)～33)逐个测试无人机航路包围盒是否与禁飞区包围盒相交，如果检测到其中一个无人机航路包围盒与禁飞区包围盒相交，则立即停止测试，证明该航路经过禁飞区，不满足安全性要求；如果检测完毕所有无人机航路包围盒与禁飞区包围盒均不相交，则证明该航路不经过禁飞区，满足安全性要求。

Images