CN111508282A - 低空无人机农田作业飞行障碍物冲突检测方法 - Google Patents

Abstract

本发明提供一种低空无人机农田作业飞行障碍物冲突检测方法，涉及无人机交通管理技术领域。该方法首先通过已知的无人机的飞行数据和已知障碍物体的位置信息，以障碍物为原点，建立坐标系；然后依据运动学原理，去除无人机飞行数据中的异常飞行数据，并判断无人机的当前飞行状态；并利用四个已知障碍物在新建立坐标系中的位置和改进的最小二乘法计算出未知障碍物的位置；同时将无人机与确定位置的障碍物的信息进行对齐；最后对无人机与确定位置的障碍物之间的飞行冲突情况进行检测。本发明方法对在农田作业的无人机与静态障碍物之间的冲突情况进行检测，保障无人机的作业安全飞行，为实现安全飞行农田作业提供技术支撑。

Description

低空无人机农田作业飞行障碍物冲突检测方法

技术领域

本发明涉及无人机交通管理技术领域，尤其涉及一种低空无人机农田作业飞行障碍物冲突检测方法。

背景技术

随着社会的发展，通用航空产业越来越成熟；受到复杂而多变的地形、建筑物以及气象条件等多种因素的影响，在有限的低空空域内，在农田作业的无人机与障碍物之间呈现出复杂的相互制约性，使得产生飞行冲突的概率增加，传统的航路航线冲突检测方法在特定农田的作业空间内并不适用。

目前，用于无人机在飞行过程中的飞行冲突的检测，主要有如下的检测方法，第一类采用加装硬件，如雷达测距、激光测距和超声波测距法，此类方法均使用距离传感器，其工作原理主要依靠自身发射电磁波或者激光、声波反射到前方障碍物，测量回波时间差来确定距离。被探测物体的材质不同所反射回来的回波强弱也有很大差异性。视觉避障工作原理是通过图像的差异计算每个点的距离，算法比较复杂，需要对图像处理。这类方法需要对无人机进行改装，且费用较高，不适用于农田作业的无人机。第二类主要是在飞行数据的基础上，采用智能算法与航迹处理方法，如支持向量机、贝叶斯估计等，多用于无人机之间。航迹预测方法如4D航迹预测、航迹滤波处理等方法，由于航迹点较多，计算量大，较为复杂。

发明内容

本发明要解决的技术问题是针对上述现有技术的不足，提供一种低空无人机农田作业飞行障碍物冲突检测方法，利用障碍物的位置信息，获取飞行冲突的概率。

为解决上述技术问题，本发明所采取的技术方案是：低空无人机农田作业飞行障碍物冲突检测方法，包括以下步骤：

步骤1、已知无人机的飞行数据和已知障碍物体的位置信息，以障碍物为原点，建立坐标系；

所述无人机的飞行数据包括无人机飞行时的位置信息，包括：经纬度、速度、iqi高度、航向角和时间；用北斗模块来获取到障碍物的位置信息，包括经纬度、高度和时间；

以障碍物A为原点，建立坐标系；已知无人机与障碍物所在点的经纬度，计算无人机与障碍物之间的距离s，如下公式所示：

s＝2×6387.137×t

其中，lat1、lat2分别为无人机与障碍物所在点的纬度，a＝lat1-lat2代表无人机与障碍物两点之间的纬度差，b代表无人机与障碍物两点之间的经度差，6378.137表示地球半径，单位为千米；

则无人机与障碍物之间的三维距离S如下公式所示：

其中，H为无人机与障碍物两点之间的高度差；

当无人机处于北半球的东半球时，确定坐标系的正方向(X、Y、Z轴的两两坐标轴之间的角度为90°)为：

当计算无人机在X轴上的位置时，b＝0，X轴的正方向是正南，当a＜0时，无人机在X轴上的数值为正；

当计算无人机在Y轴上的位置时，a＝0，且Y轴的正方向是正东，当b＞0时，无人机在Y轴上的数值为正；

当计算无人机在Z轴上的位置时，a＝b＝0，当H＞0时，无人机在Z轴上的数值为正；

当无人机不处于北半球的东半球时，依据当地的实际情况选择坐标系的正方向；

从而得到第i时刻的无人机的飞行数据信息集合Ω_i，如下式所示：

Ω_i＝(x_i，y_i，z_i，v_i，t_i，h_i，θ_i)_1×6(i＝0，1，2，...，n)

其中，x_i，y_i，z_i，v_i，t_i，h_i，θ_i分别表示第i时刻的无人机在X，Y，Z轴上的位置、速度、时间、高度和航向角，n为时间序列；

步骤2、依据运动学原理，去除无人机飞行数据中的异常飞行数据，并判断无人机的当前飞行状态；

步骤2.1、依据已知的无人机飞行数据手册对无人机飞行轨迹进行建模，去除飞行数据中的异常飞行数据；

(1)依据加速度去除异常速度数据：

求无人机重力加速度：将无人机重力分解在X、Y、Z轴方向上：

其中，G_x、G_y、G_z分别表示无人机在X、Y、Z轴方向上的重力，θ、φ分别表示无人机飞行速度的方向与X、Y轴正方向的夹角；

依据牛顿第二定律，得到无人机瞬时重力加速度，如下式所示：

其中，a_x、a_y、a_z分别表示无人机在X、Y、Z轴方向上的重力加速度，g为重力加速度；

依据无人机的飞行数据手册获取无人机理论上的最大加速度a_k，从而求得无人机加速度理论上的最大值在X、Y、Z轴方向上的分量：

其中，a_{max x}、a_{max y}、a_{max z}分别表示无人机在X、Y、Z轴方向上的加速度理论上的最大值；

依据无人机的瞬时飞行速度，得到第i时刻无人机实际的飞行加速度分量，如下式所示：

其中，a_ix、a_iy、a_iz表示第i时刻无人机实际的飞行加速度在X、Y、Z轴方向上的分量，且三个方向上的加速度均小于加速度理论上的最大值；Δt是记录的第i+1时刻与第i时刻之间的时间差；v_i、v_i+1分别表示第i时刻和第i+1时刻的无人机的瞬时飞行速度；

依据飞行数据得到i时刻无人机在X、Y、Z轴方向上的实际加速度，从而得到i时刻加速度的约束条件：

当a_ix、a_iy、a_iz不满足上述约束条件时，则无人机在第i时刻的瞬时速度值为异常数据；

(2)去除异常位置数据

利用无人机的飞行数据手册以及运动学原理计算无人机的理论位置，如下式所示：

其中，X_i、Y_i、Z_i表示在第i时刻无人机在X、Y、Z轴上的理论的位置，v_ix、v_iy、v_iz表示第i时刻无人机的瞬时飞行速度在X，Y，Z轴上的分量，如下式所示：

v_ix＝v_isinθ

v_iy＝v_icosθsinφ

v_iz＝v_icosθcosφ

X_i+1、Y_i+1、Z_i+1分别为在i+1时刻无人机在X、Y、Z轴上的理论的位置，且满足以下条件：

其中，x_i+1、y_i+1、z_i+1分别为i+1时刻无人机在X、Y、Z轴上的实际位置，V_xi、V_yi、V_zi表示无人机在第i时刻依据飞行数据手册得到的理论速度在X、Y、Z轴上的分量；

若无人机的实际位置不满足以上约束条件，则无人机在X、Y、Z轴上的位置为异常数据；

(3)去除飞行速度与飞行高度异常数据

依据无人机的飞行数据手册，得到无人机的最大飞行速度V_max与最大飞行高度H_max，依据实际的飞行数据得到第i时刻无人机的瞬时速度为v_i和飞行高度为h_i；

若无人机实际飞行数据中的飞行速度与飞行高度不满足如下条件：

则无人机的飞行速度数据和飞行高度数据是异常数据；

步骤2.2、利用已知的无人机飞行数据判断无人机运动状态；

匀速运动：当a_ix＝a_iy＝a_iz＝0时，无人机处于匀速运动；

匀加速运动：当a_(i+1)x-a_ix＝a_(i+1)y-a_iy＝a_(i+1)z-a_iz＝η时，无人机处于匀加速运动，其中，η为一个常数，a_(i+1)x、a_(i+1)y、a_(i+1)z分别为在第i+1时刻无人机的加速度在x、y、z上的分量；

转弯运动：已知航向角θ：当θ_i+1-θ_i≠0时，无人机为转弯运动，其中，θ_i+1、θ_i分别为无人机在第i+1时刻和第i时刻的航向角；

步骤3、利用四个已知障碍物在新建立坐标系中的位置和改进的最小二乘法计算出未知障碍物的位置；

依据卫星信息获得坐标系中第λ个已知障碍物的位置S_λ为：

S_λ＝(x_λ，y_λ，，z_λ)(λ＝1，2，3，4)

其中，x_λ，y_λ，，z_λ分别为第λ个已知障碍物在X，Y，Z三个方向上的坐标值；

除去这四个已知障碍物之外的第l个未知障碍物的位置F_l为：

F_l＝(x_l，y_l，z_l)

其中，x_l，y_l，z_l表示第l个未知障碍物在X，Y，Z三个方向上的坐标值；

利用仪器测得任意一个未知障碍物与已知四个障碍物的距离集合R为：

R＝(r₁，r₂，r₃，r₄)

则通过以下改进的最小二乘法方法计算求得F_l：

(1)设置F_l的初始位置(x′₀，y′₀，z′₀)，为了简化计算过程，此处将其设为(0，0，0)；

(2)设定迭代误差阈值，迭代求解F_l的坐标值x_l，y_l，z_l；

由于在飞行数据手册中，描述无人机形状参数的数据为毫米级别，故设定迭代误差阈值为：1×10^-4，即误差在10^-4m以内；

未知障碍物F_l到已知障碍物S_λ的欧式距离r^λ如下公式所示：

其中，(x_l，y_l，z_l)为待求障碍物点的位置坐标，x_λ，y_λ，z_λ为参与解算的第λ个已知障碍物的位置坐标值；

1)计算单位观测矢量：

其中，

分别为第λ个已知障碍物经过第k次迭代后单位观测矢量在X，Y，Z三个方向上的分量，x′_k、y′_k、z′_k分别为第k次迭代后未知障碍物在X，Y，Z轴上的位置坐标值；

2)定义第k-1，(k＞1)次迭代时的几何矩阵G_k-1、b_k-1，如下公式所示：

其中，

为第λ个已知障碍物到未知障碍物的实际距离，

表示进行k-1次迭代后第λ个已知障碍物到未知障碍物的欧式距离；

3)求解线性方程组：

其中，Δ_x′、Δ_y′、Δ_z′为相邻两次迭代未知障碍物位置坐标的差值；

4)更新未知障碍物点的位置信息：

其中，x′_k、y′_k、z′_k为第k次迭代后未知障碍物点的位置坐标，x′_k-1、y′_k-1、z′_k-1为第k-1次迭代后未知障碍物点的位置坐标；

5)将迭代误差与设定的迭代误差阈值比较：

其中，Δd′_χ为迭代的误差值，如果Δd′_χ大于设定的迭代误差阈值，则重新执行步骤1)-5)，直至Δd′_χ小于等于设定的迭代误差阈值，从而获得未知障碍物F_l的准确位置；

步骤4、将无人机与确定位置的障碍物的信息进行对齐，使无人机与确定障碍物的信息数据维度相同；

确定位置的障碍物的信息f_α与无人机的飞行数据Ω_i分别为：

f_α＝(x_α，y_α，z_α，v_α，t_α，θ_α)_1×6(α＝1，2，...，c)

Ω_i＝(x_i，y_i，z_i，v_i，t_i，θ_i)_1×6(i＝0，1，2，...，n)

其中，Ω_i表示第i时刻无人机的飞行数据信息，x_i，y_i，z_i，v_i，t_i，θ_i分别为第i时刻无人机在X，Y，Z轴上的位置、速度、时间和航向角，航向角θ_i即为无人机飞行速度方向与X轴的夹角，t_i为时间；f_α表示第α个障碍物的信息，其中，x_α，y_α，z_α，v_α，t_α，θ_α分别为第i时刻障碍物在X，Y，Z轴上的位置、速度、时间和航向角，且v_α＝0，t_α＝t_i，θ_α＝0，c为确定位置的障碍物的总数；

步骤5、对无人机与确定位置的障碍物之间的飞行冲突情况进行检测；

步骤5.1、对无人机保护区与飞行冲突保护区进行建模，从而判断无人机与确定位置的障碍物是否存在冲突；

以无人机的中心为原点，即以无人机在X、Y、Z轴上的实际的位置为球心，以无人机结构上机翼离放置定位模块的点的距离为球半径L，建立球体无人机保护区模型：

(x_β-x_i)²+(y_β-y_i)²+(z_β-z_i)²＝L²

其中，x_β、y_β、z_β为球内任意一点；

建立无人机飞行冲突保护区：

对于不同飞行状态时，采用不同的无人机飞行冲突保护区：

无人机处于匀速飞行时，无人机的飞行冲突保护区为球体，表达式为：

(x_γ-x_β)²+(y_γ-y_β)²+(z_γ-z_β)²＝(L+l_max)²

其中，l_max是无人机保护区模型到飞行冲突保护区模型的距离；x_γ、y_γ、z_γ为球体保护区内除无人机保护区外任意一点；无人机处于匀加速飞行时，无人机的飞行冲突保护区为椭体保护区I，表达式为：

其中，x_τ、y_τ、z_τ为椭体保护区I内除无人机保护区外的任意一点，s_x、s_y、s_z表示无人机在X、Y、Z方向上的位置偏移量，表达式为：

无人机处于转弯飞行时，无人机的飞行冲突保护区为椭体保护区II，表达式为：

其中，(x_δ、y_δ、z_δ)为椭体保护区II内除无人机保护区外的任意一点；

当已经确定位置的障碍物在飞行冲突保护区外时，则无人机与障碍物不存在飞行冲突，当已经确定位置的障碍物在飞行冲突保护区内时，则无人机与障碍物存在飞行冲突；

步骤5.2、若无人机与已确定位置的障碍物之间存在飞行冲突，计算无人机与障碍之间的冲突概率；

计算已确定位置的障碍物到无人机的欧氏距离d′，以及无人机与飞行冲突保护区边缘的距离d″，d″＞d′，且d″与d′均服从

化为标准正态分布为：

其中，Δ为无人机与已确定位置的障碍物之间的位置差，

分别为无人机的位置误差的方差与障碍物的位置误差的方差；

则无人机与障碍物之间冲突概率为：

其中，

为无人机的位置的误差与障碍物的位置误差，且为高斯分布：

其中，μ_b为数学期望，μ_b与

通过历史飞行数据与障碍物的数据计算得出，

为无人机与已确定位置的障碍物之间的距离差与最小安全距离之差。

采用上述技术方案所产生的有益效果在于：本发明提供的低空无人机农田作业飞行障碍物冲突检测方法，不需要测绘来新建地图，只需要在卫星导航定位基础上，对在农田作业的无人机与静态障碍物之间的冲突情况进行检测，保障无人机的作业安全飞行，为实现安全飞行农田作业提供技术支撑。同时无需对无人机的摄像头进行优化，不需要图像处理，考虑了无人机的本身的形状，故安全度更高，使用灵活，可用于各种农田作业的场景。

附图说明

图1为本发明实施例提供的低空无人机农田作业飞行障碍物冲突检测方法的流程图；

图2为本发明实施例提供的根据飞行数据得到的无人机飞行轨迹图；

图3为本发明实施例提供的无人机在建立的坐标系上的飞行轨迹图；

图4为本发明实施例提供的各障碍物在坐标系中的位置示意图；

图5为本发明实施例提供的当无人机处于匀速运动时无人机保护区和冲突保护区是示意图；

图6为本发明实施例提供的在匀加速运动和转弯运动时无人机保护区和冲突保护区的示意图；

图7为本发明实施例提供的障碍物在无人机的冲突保护区内的示意图；

图8为本发明实施例提供的障碍物在无人机的冲突保护区内时的冲突概率图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

本实施例以表1所示的某无人机的飞行数据为例，使用本发明的低空无人机农田作业飞行障碍物冲突检测方法对该无人机与障碍物之间的冲突情况进行检测。

本实施例中，低空无人机农田作业飞行障碍物冲突检测方法，如图1所示，包括以下步骤：

步骤1、已知无人机的飞行数据和已知障碍物体的位置信息，以障碍物为原点，建立坐标系；

所述无人机的飞行数据包括无人机飞行时的位置信息，包括：经纬度、速度、高度、航向角和时间；用北斗模块来获取到障碍物的位置信息，包括经纬度、高度和时间；

以障碍物A为原点，建立坐标系；已知无人机与障碍物所在点的经纬度，计算无人机与障碍物之间的距离s，如下公式所示：

s＝2×6387.137×t

其中，lat1、lat2分别为无人机与障碍物所在点的纬度，a＝lat1-lat2代表无人机与障碍物两点之间的纬度差，b代表无人机与障碍物两点之间的经度差，6378.137表示地球半径，单位为千米；

则无人机与障碍物之间的三维距离S如下公式所示：

其中，H为无人机与障碍物两点之间的高度差；

当无人机处于北半球的东半球时，确定坐标系的正方向(X、Y、Z轴的两两坐标轴之间的角度为90°)为：

当计算无人机在X轴上的位置时，b＝0，X轴的正方向是正南，当a＜0时，无人机在X轴上的数值为正；

当计算无人机在Y轴上的位置时，a＝0，且Y轴的正方向是正东，当b＞0时，无人机在Y轴上的数值为正；

当计算无人机在Z轴上的位置时，a＝b＝0，当H＞0时，无人机在Z轴上的数值为正；

当无人机不处于北半球的东半球时，依据当地的实际情况选择坐标系的正方向；

从而得到第i时刻的无人机的飞行数据信息集合Ω_i，如下公式所示：

Ω_i＝(x_i，y_i，z_i，v_i，t_i，h_i，θ_i)_1×6(i＝0，1，2，...，n)

其中，x_i，y_i，z_i，v_i，t_i，h_i，θ_i分别表示第i时刻的无人机在X，Y，Z轴上的位置、速度、时间、高度和航向角，n为时间序列；

本实施例中，无人机在在X，Y，Z轴上的位置坐标单位均为m。

步骤2、依据运动学原理，去除无人机飞行数据中的异常飞行数据，并判断无人机的当前飞行状态；

步骤2.1、依据已知的无人机飞行数据手册对无人机飞行轨迹进行建模，去除飞行数据中的异常飞行数据；

(1)依据加速度去除异常速度数据：

相较与通航飞行器与民用航空飞行器，在农田作业的无人机处于低空低速的飞行状态且无人机的重量较轻。

求无人机重力加速度：将无人机重力分解在X、Y、Z轴方向上：

其中，G_x、G_y、G_z分别表示无人机在X、Y、Z轴方向上的重力，θ、φ分别表示无人机飞行速度的方向与X、Y轴正方向的夹角；

依据牛顿第二定律，得到无人机瞬时重力加速度，如下公式所示：

其中，a_x、a_y、a_z分别表示无人机在X、Y、Z轴方向上的重力加速度，g为重力加速度；

依据无人机的飞行数据手册获取无人机理论上的最大加速度a_k，从而求得无人机加速度理论上的最大值在X、Y、Z轴方向上的分量：

其中，a_{max x}、a_{max y}、a_{max z}分别表示无人机在X、Y、Z轴方向上的加速度理论上的最大值；

依据无人机的瞬时飞行速度，得到第i时刻无人机实际的飞行加速度分量，如下公式所示：

其中，a_ix、a_iy、a_iz表示第i时刻无人机实际的飞行加速度在X、Y、Z轴方向上的分量，且三个方向上的加速度均小于加速度理论上的最大值；Δt是记录的第i+1时刻与第i时刻之间的时间差；v_i、v_i+1分别表示第i时刻和第i+1时刻的无人机的瞬时飞行速度；

依据飞行数据得到i时刻无人机在X、Y、Z轴方向上的实际加速度，从而得到i时刻加速度的约束条件：

当a_ix、a_iy、a_iz不满足上述约束条件时，则无人机在第i时刻的瞬时速度值为异常数据；

(2)去除异常位置数据

利用加速度的约束条件去除飞行数据中的异常值外，还可能存在连续异常的飞行数据不能被检测，为排除此类数据中的异常值，利用无人机的飞行数据手册以及运动学原理计算无人机的理论位置，如下公式所示：

其中，X_i、Y_i、Z_i表示在第i时刻无人机在X、Y、Z轴上的理论的位置，v_ix、v_iy、v_iz表示第i时刻无人机的瞬时飞行速度在X，Y，Z轴上的分量，如下公式所示：

v_ix＝v_isinθ

v_iy＝v_icosθsinφ

v_iz＝v_icosθcosφ

X_i+1、Y_i+1、Z_i+1分别为在i+1时刻无人机在X、Y、Z轴上的理论的位置，且满足以下条件：

其中，x_i+1、y_i+1、z_i+1分别为i+1时刻无人机在X、Y、Z轴上的实际位置，V_xi、V_yi、V_zi表示无人机在第i时刻依据飞行数据手册得到的理论速度在X、Y、Z轴上的分量；

若无人机的实际位置不满足以上约束条件，则无人机在X、Y、Z轴上的位置为异常数据；

(3)去除飞行速度与飞行高度异常数据

依据无人机的飞行数据手册，得到无人机的最大飞行速度V_max与最大飞行高度H_max，依据实际的飞行数据得到第i时刻无人机的瞬时速度为v_i和飞行高度为h_i；

若无人机实际飞行数据中的飞行速度与飞行高度不满足如下条件：

则无人机的飞行速度数据和飞行高度数据是异常数据；

步骤2.2、利用已知的无人机飞行数据判断无人机运动状态；

匀速运动：当a_ix＝a_iy＝a_iz＝0时，无人机处于匀速运动；

匀加速运动：当a_(i+1)x-a_ix＝a_(i+1)y-a_iy＝a_(i+1)z-a_iz＝η时，无人机处于匀加速运动，其中，η为一个常数，a_(i+1)x、a_(i+1)y、a_(i+1)z分别为在第i+1时刻无人机的加速度在x、y、z上的分量；

转弯运动：已知航向角θ：当θ_i+1-θ_i≠0时，无人机为转弯运动，其中，θ_i+1、θ_i分别为无人机在第i+1时刻和第i时刻的航向角；

本实施例中，某时间序列内无人机的飞行数据及其异常情况如表1所示：

表1 无人机某时间序列内的飞行数据

本实施例中，根据表1的飞行数据画出如图2所示的无人机飞行轨迹图，已知障碍物A的位置信息为(123.371223°，42.391031°，92m)，并以此为原点建立坐标系，则无人机在该坐标系上的飞行轨迹如图3所示；

本实施例中，该无人机的飞行数据手册如表2所示：

表2 无人机飞行数据手册

最大平飞速度	60km/h	规格(长x宽X高)	800mmx800mmx450mm
				巡航速度	40-50km/h	飞行持续时间	＜＝60min
最大过载	0.5g	相对飞行高度	0-500m
				空载质量	2kg	最大起飞重量	＜10kg
活动半径	＜20km	最长飞行距离	40km

步骤3、利用四个已知障碍物在新建立坐标系中的位置和改进的最小二乘法计算出未知障碍物的位置；

依据卫星信息获得坐标系中第λ个已知障碍物的位置S_λ为：

S_λ＝(x_λ，y_λ，，z_λ)(λ＝1，2，3，4)

其中，x_λ，y_λ，，z_λ分别为第λ个已知障碍物在X，Y，Z三个方向上的坐标值；

除去这四个已知障碍物之外的第l个未知障碍物的位置F_l为：

F_l＝(x_l，y_l，z_l)

其中，x_l，y_l，z_l表示第l个未知障碍物在X，Y，Z三个方向上的坐标值；

利用仪器测得任意一个未知障碍物与已知四个障碍物的距离集合R为：

R＝(r₁，r₂，r₃，r₄)

则通过以下改进的最小二乘法方法计算求得F_l：

(1)设置F_l的初始位置(x′₀，y′₀，z′₀)，为了简化计算过程，此处将其设为(0，0，0)；

(2)设定迭代误差阈值，迭代求解F_l的坐标值x_l，y_l，z_l；

由于在飞行数据手册中，描述无人机形状参数的数据为毫米级别，故设定迭代误差阈值为：1×10^-4，即误差在10^-4m以内；

未知障碍物F_l到已知障碍物S_λ的欧式距离r^λ如下公式所示：

其中，(x_l，y_l，z_l)为待求障碍物点的位置坐标，x_λ，y_λ，z_λ为参与解算的第λ个已知障碍物的位置坐标值；

本实施例以参与计算的第1个已知障碍物为例，首次迭代欧式距离r¹ ₀表示为如下公式：

1)计算单位观测矢量：

其中，

分别为第λ个已知障碍物经过第k次迭代后单位观测矢量在X，Y，Z三个方向上的分量，x′_k、y′_k、z′_k分别为第k次迭代后未知障碍物在X，Y，Z轴上的位置坐标值；

本实施例中，单位观测矢量首次迭代可表示为如下公式：

2)定义第k-1，(k＞1)次迭代时的几何矩阵G_k-1、b_k-1，如下公式所示：

其中，

为第λ个已知障碍物到未知障碍物的实际距离，

表示进行k-1次迭代后第λ个已知障碍物到未知障碍物的欧式距离；

3)求解线性方程组：

其中，Δ_x′、Δ_y′、Δ_z′为相邻两次迭代未知障碍物位置坐标的差值；

4)更新未知障碍物点的位置信息：

其中，x′_k、y′_k、z′_k为第k次迭代后未知障碍物点的位置坐标，x′_k-1、y′_k-1、z′_k-1为第k-1次迭代后未知障碍物点的位置坐标；

本实施例以首次迭代为例，未知障碍物点的位置信息可表示为：

5)将迭代误差与设定的迭代误差阈值比较：

其中，Δd′_χ为迭代的误差值，如果Δd_χ大于设定的迭代误差阈值，则重新执行步骤1)-5)，直至Δd_χ小于等于设定的迭代误差阈值，从而获得未知障碍物F_l的准确位置；

本实施例中，已知四个障碍物在建立的坐标系中的位置坐标分别为A(0，0，0)，B(2003，11825，0)，C(1858，15020，15)，D(158，156184，46)，通过仪器测得这四个障碍物点到未知障碍物点E的距离分别为(78.15961，11964.59629，15106.98411，15161.14228)(单位为m)，则依据改进的最小二乘法，求得未知障碍物E点的位置为E(36.12561，23.35982，65.25482)，各障碍物在坐标系中的位置如图4所示。

步骤4、将无人机与确定位置的障碍物的信息进行对齐，使无人机与确定障碍物的信息数据维度相同；

确定位置的障碍物的信息f_α与无人机的飞行数据Ω_i分别为：

f_α＝(x_α，y_α，z_α，v_α，t_α，θ_α)_1×6(α＝1，2，...，c)

Ω_i＝(x_i，y_i，z_i，v_i，t_i，θ_i)_1×6(i＝0，1，2，...，n)

其中，Ω_i表示第i时刻无人机的飞行数据信息，x_i，y_i，z_i，v_i，t_i，θ_i分别为第i时刻无人机在X，Y，Z轴上的位置、速度、时间和航向角，航向角θ_i即为无人机飞行速度方向与X轴的夹角，t_i为时间；f_α表示第α个障碍物的信息，其中，x_α，y_α，z_α，v_α，t_α，θ_α分别为第i时刻障碍物在X，Y，Z轴上的位置、速度、时间和航向角，且v_α＝0，t_α＝t_i，θ_α＝0，c为确定位置的障碍物的总数；

本实施例中，将某时刻障碍物P与无人机的信息进行对齐如表3所示：

表3 障碍物与无人机的信息进行对齐

步骤5、对无人机与确定位置的障碍物之间的飞行冲突情况进行检测；

步骤5.1、对无人机保护区与飞行冲突保护区进行建模，从而判断无人机与确定位置的障碍物是否存在冲突；

由于农田作业的无人机的飞行速度较慢，无人机的大小相较于飞行距离不能忽略不计，故无人机不能看作质心来计算。为保障飞行过程中的安全性与计算的实时性，故以无人机的中心为原点，即以无人机在X、Y、Z轴上的实际的位置为球心，以无人机结构上机翼离放置定位模块的点的距离为球半径L，建立球体无人机保护区模型：

(x_β-x_i)²+(y_β-y_i)²+(z_β-z_i)²＝L²

其中，x_β、y_β、z_β为球内任意一点；

建立无人机飞行冲突保护区：

对于不同飞行状态时，采用不同的无人机飞行冲突保护区：

无人机处于匀速飞行时，无人机的飞行冲突保护区为球体，表达式为：

(x_γ-x_β)²+(y_γ-y_β)²+(z_γ-z_β)²＝(L+l_max)²

其中，l_max是无人机保护区模型到飞行冲突保护区模型的距离；x_γ、y_γ、z_γ为球体形保护区内除无人机保护区外任意一点；

无人机处于匀加速飞行时，无人机的飞行冲突保护区为椭体保护区I，表达式为：

其中，x_τ、y_τ、z_τ为椭体保护区I内除无人机保护区外的任意一点，s_x、s_y、s_z表示无人机在X、Y、Z方向上的位置偏移量，表达式为：

无人机处于转弯飞行时，无人机的飞行冲突保护区为椭体保护区II，表达式为：

其中，(x_δ、_yδ、z_δ)为椭体保护区II内除无人机保护区外的任意一点；

当已经确定位置的障碍物在飞行冲突保护区外时，则无人机与障碍物不存在飞行冲突，当已经确定位置的障碍物在飞行冲突保护区内时，则无人机与障碍物存在飞行冲突；

步骤5.2、若无人机与已确定位置的障碍物之间存在飞行冲突，计算无人机与障碍之间的冲突概率；

计算已确定位置的障碍物到无人机的欧氏距离d′，以及无人机与飞行冲突保护区边缘的距离d″，d″＞d′，且d″与d′均服从

化为标准正态分布为：

其中，Δ为无人机与已确定位置的障碍物之间的位置差，

分别为无人机的位置误差的方差与障碍物的位置误差的方差；

则无人机与障碍物之间冲突概率为：

其中，

为无人机的位置的误差与障碍物的位置误差，且为高斯分布：

其中，μ_b为数学期望，μ_b与

通过历史飞行数据与障碍物的数据计算得出，

为无人机与已确定位置的障碍物之间的距离差与最小安全距离之差。

本实施例中，当无人机处于匀速运动时，无人机保护区和冲突保护区均为球体，如图5所示；在匀加速运动和转弯运动时，冲突保护区模型都为椭球体模型，只是椭球体的参数不一样，故在匀加速运动和转弯运动时，无人机保护区和冲突保护区的示意图如图6所示；

本实施例以无人机在匀速运动时为例判断该无人机与障碍物P的冲突情况；障碍物在无人机的冲突保护区内，则该无人机与障碍物P存在冲突，如图7所示，冲突概率如图8所示。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。

Claims (7)

1.一种低空无人机农田作业飞行障碍物冲突检测方法，其特征在于：包括以下步骤：

步骤1、已知无人机的飞行数据和已知障碍物体的位置信息，以障碍物为原点，建立坐标系；

步骤2、依据运动学原理，去除无人机飞行数据中的异常飞行数据，并判断无人机的当前飞行状态；

步骤3、利用四个已知障碍物在新建立坐标系中的位置和改进的最小二乘法计算出未知障碍物的位置；

步骤4、将无人机与确定位置的障碍物的信息进行对齐；使无人机与确定障碍物的信息数据维度相同。

步骤5、对无人机与确定位置的障碍物之间的飞行冲突情况进行检测；

步骤5.1、对无人机保护区与飞行冲突保护区进行建模，从而判断无人机与确定位置的障碍物是否存在冲突；

当已经确定位置的障碍物在飞行冲突保护区外时，则无人机与障碍物不存在飞行冲突，当已经确定位置的障碍物在飞行冲突保护区内时，则无人机与障碍物存在飞行冲突；

步骤5.2、若无人机与已确定位置的障碍物之间存在飞行冲突，计算无人机与障碍之间的冲突概率。

2.根据权利要求1所述的低空无人机农田作业飞行障碍物冲突检测方法，其特征在于：所述步骤1的具体方法为：

所述无人机的飞行数据包括无人机飞行时的位置信息，包括：经纬度、速度、高度、航向角和时间；用北斗模块来获取到障碍物的位置信息，包括经纬度、高度和时间；

以障碍物A为原点，建立坐标系；已知无人机与障碍物所在点的经纬度，计算无人机与障碍物之间的距离s，如下公式所示：

s＝2×6387.137×t

其中，lat1、lat2分别为无人机与障碍物所在点的纬度，a＝lat1-lat2代表无人机与障碍物两点之间的纬度差，b代表无人机与障碍物两点之间的经度差，6378.137表示地球半径，单位为千米；

则无人机与障碍物之间的三维距离S如下式所示：

其中，H为无人机与障碍物两点之间的高度差；

当无人机处于北半球的东半球时，确定坐标系的正方向(X、Y、Z轴的两两坐标轴之间的角度为90°)为：

当计算无人机在X轴上的位置时，b＝0，X轴的正方向是正南，当a＜0时，无人机在X轴上的数值为正；

当计算无人机在Y轴上的位置时，a＝0，且Y轴的正方向是正东，当b＞0时，无人机在Y轴上的数值为正；

当计算无人机在Z轴上的位置时，a＝b＝0，当H＞0时，无人机在Z轴上的数值为正；

当无人机不处于北半球的东半球时，依据当地的实际情况选择坐标系的正方向；

从而得到第i时刻的无人机的飞行数据信息集合Ω_i，如下公式所示：

Ω_i＝(x_i，y_i，z_i，v_i，t_i，h_i，θ_i)_1×6(i＝0，1，2，...，n)

其中，x_i，y_i，z_i，v_i，t_i，h_i，θ_i分别表示第i时刻的无人机在X，Y，Z轴上的位置、速度、时间、高度和航向角，n为时间序列。

3.根据权利要求2所述的低空无人机农田作业飞行障碍物冲突检测方法，其特征在于：所述步骤2的具体方法为：

步骤2.1、依据已知的无人机飞行数据手册对无人机飞行轨迹进行建模，去除飞行数据中的异常飞行数据；

(1)依据加速度去除异常速度数据：

求无人机重力加速度：将无人机重力分解在X、Y、Z轴方向上：

其中，G_x、G_y、G_z分别表示无人机在X、Y、Z轴方向上的重力，θ、φ分别表示无人机飞行速度的方向与X、Y轴正方向的夹角；

依据牛顿第二定律，得到无人机瞬时重力加速度，如下式所示：

其中，a_x、a_y、a_z分别表示无人机在X、Y、Z轴方向上的重力加速度，g为重力加速度；

依据无人机的飞行数据手册获取无人机理论上的最大加速度a_k，从而求得无人机加速度理论上的最大值在X、Y、Z轴方向上的分量：

其中，a_{max x}、a_{max y}、a_{max z}分别表示无人机在X、Y、Z轴方向上的加速度理论上的最大值；

依据无人机的瞬时飞行速度，得到第i时刻实际的飞行加速度分量，如下式所示：

其中，a_ix、a_iy、a_iz表示第i时刻无人机实际的飞行加速度在X、Y、Z轴方向上的分量，且三个方向上的加速度均小于加速度理论上的最大值；Δt是记录的第i+1时刻与第i时刻之间的时间差；v_i、v_i+1分别表示第i时刻和第i+1时刻的无人机的瞬时飞行速度；

依据飞行数据得到i时刻无人机在X、Y、Z轴方向上的实际加速度，从而得到i时刻加速度的约束条件：

当a_ix、a_iy、a_iz不满足上述约束条件时，则无人机在第i时刻的瞬时速度值为异常数据；

(2)去除异常位置数据

利用无人机的飞行数据手册以及运动学原理计算无人机的理论位置，如下式所示：

其中，X_i、Y_i、Z_i表示在第i时刻无人机在X、Y、Z轴上的理论的位置，v_ix、v_iy、v_iz表示第i时刻无人机的瞬时飞行速度在X，Y，Z轴上的分量，如下式所示：

v_ix＝v_isinθ

v_iy＝v_icosθsinφ

v_iz＝v_icosθcosφ

X_i+1、Y_i+1、Z_i+1分别为在i+1时刻无人机在X、Y、Z轴上的理论的位置，且满足以下条件：

其中，x_i+1、y_i+1、z_i+1分别为i+1时刻无人机在X、Y、Z轴上的实际位置，V_xi、V_yi、V_zi表示无人机在第i时刻依据飞行数据手册得到的理论速度在X、Y、Z轴上的分量；

若无人机的实际位置不满足以上约束条件，则无人机在X、Y、Z轴上的位置为异常数据；

(3)去除飞行速度与飞行高度异常数据

依据无人机的飞行数据手册，得到无人机的最大飞行速度V_max与最大飞行高度H_max，依据实际的飞行数据得到第i时刻无人机的瞬时速度为v_i和飞行高度为h_i；

若无人机实际飞行数据中的飞行速度与飞行高度不满足如下条件：

则无人机的飞行速度数据和飞行高度数据是异常数据；

步骤2.2、利用已知的无人机飞行数据判断无人机运动状态；

匀速运动：当a_ix＝a_iy＝a_iz＝0时，无人机处于匀速运动；

匀加速运动：当a_(i+1)x-a_ix＝a_(i+1)y-a_iy＝a_(i+1)z-a_iz＝η时，无人机处于匀加速运动，其中，η为一个常数，a_(i+1)x、a_(i+1)y、a_(i+1)z分别为在第i+1时刻无人机的加速度在x、y、z上的分量；

转弯运动：已知航向角夕：当θ_i+1-θ_i≠0时，无人机为转弯运动，其中，θ_i+1、θ_i分别为无人机在第i+1时刻和第i时刻的航向角。

4.根据权利要求3所述的低空无人机农田作业飞行障碍物冲突检测方法，其特征在于：所述步骤3的具体方法为：

依据卫星信息获得坐标系中第λ个已知障碍物的位置S_λ为：

S_λ＝(x_λ，y_λ，，z_λ)(λ＝1，2，3，4)

其中，x_λ，y_λ，z_λ分别为第λ个已知障碍物在X，Y，Z三个方向上的坐标值；

除去这四个已知障碍物之外的第l个未知障碍物的位置F_l为：

F_l＝(x_l，y_l，z_l)

其中，x_l，y_l，z_l表示第l个未知障碍物在X，Y，Z三个方向上的坐标值；

利用仪器测得任意一个未知障碍物与已知四个障碍物的距离集合R为：

R＝(r₁，r₂，r₃，r₄)

则通过以下改进的最小二乘法方法计算求得F_l：

(1)设置F_l的初始位置(x′₀，y′₀，z′₀)，为了简化计算过程，此处将其设为(0，0，0)；

(2)设定迭代误差阈值，迭代求解F_l的坐标值x_l，y_l，z_l；

由于在飞行数据手册中，描述无人机形状参数的数据为毫米级别，故设定迭代误差阈值为：1×10^-4，即误差在10^-4m以内；

未知障碍物F_l到已知障碍物S_λ的欧式距离r^λ如下公式所示：

其中，(x_l，y_l，z_l)为待求未知障碍物点的位置坐标，x_λ，y_λ，z_λ为参与解算的第λ个已知障碍物的位置坐标值；

1)计算单位观测矢量：

其中，

分别为第λ个已知障碍物经过第k次迭代后单位观测矢量在X，Y，Z三个方向上的分量，x′_k、y′_k、z′_k分别为第k次迭代后未知障碍物在X，Y，Z轴上的位置坐标值；

2)定义第k-1，(k＞1)次迭代时的几何矩阵G_k-1、b_k-1，如下公式所示：

其中，

为第λ个已知障碍物到未知障碍物的实际距离，

表示进行k-1次迭代后第λ个已知障碍物到未知障碍物的欧式距离；

3)求解线性方程组：

其中，Δx′、Δy′、Δz′为相邻两次迭代未知障碍物位置坐标的差值；

4)更新未知障碍物点的位置信息：

其中，x′_k、y′_k、z′_k为第k次迭代后未知障碍物点的位置坐标，x′_k-1、y′_k-1、z′_k-1为第k-1次迭代后未知障碍物点的位置坐标；

5)将迭代误差与设定的迭代误差阈值比较：

其中，Δd′_γ为迭代的误差值，如果Δd′_x大于设定的迭代误差阈值，则重新执行步骤1)-5)，直至Δd′_x小于等于设定的迭代误差阈值，从而获得未知障碍物F_l的准确位置。

5.根据权利要求4所述的低空无人机农田作业飞行障碍物冲突检测方法，其特征在于：步骤4所述将无人机与确定位置的障碍物的信息进行对齐，使无人机与确定障碍物的信息数据维度相同，具体方法为：

确定位置的障碍物的信息f_α与无人机的飞行数据Ω_i分别为：

f_α＝(x_α，y_α，z_α，v_α，t_α，θ_α)_1×6(α＝1，2，...，c)

Ω_i＝(x_i，y_i，z_i，v_i，t_i，θ_i)_1×6(i＝0，1，2，...，n)

其中，Ω_i表示第i时刻无人机的飞行数据信息，x_i，y_i，z_i，v_i，t_i，θ_i分别为第i时刻无人机在X，Y，Z轴上的位置、速度、时间和航向角，航向角θ_i即为无人机飞行速度方向与X轴的夹角，t_i为时间；f_α表示第α个障碍物的信息，其中，x_α，y_α，z_α，v_α，t_α，θ_α分别为第i时刻障碍物在X，Y，Z轴上的位置、速度、时间和航向角，且v_α＝0，t_α＝t_i，θ_α＝0，c为确定位置的障碍物的总数。

6.根据权利要求5所述的低空无人机农田作业飞行障碍物冲突检测方法，其特征在于：步骤5.1所述对无人机保护区和飞行冲突保护区进行建模的具体方法为：

以无人机的中心为原点，即以无人机在X、Y、Z轴上的实际的位置为球心，以无人机结构上机翼离放置定位模块的点的距离为球半径L，建立球体无人机保护区模型：

(x_β-x_i)²+(y_β-y_i)²+(z_β-z_i)²＝L²

其中，x_β、y_β、z_β为球内任意一点；

建立无人机飞行冲突保护区：

对于不同飞行状态时，采用不同的无人机飞行冲突保护区：

无人机处于匀速飞行时，无人机的飞行冲突保护区为球体，表达式为：

(x_γ-x_β)²+(y_γ-y_β)²+(z_γ-z_β)²＝(L+l_max)²

其中，l_max是无人机保护区模型到飞行冲突保护区模型的距离；x_γ、y_γ、z_γ为球体飞行冲突保护区内除无人机保护区外的任意一点；

无人机处于匀加速飞行时，无人机的飞行冲突保护区为椭体保护区I，表达式为：

其中，x_τ、y_τ、z_τ为椭体保护区I内除无人机保护区外的任意一点，s_x、s_y、s_z表示无人机在X、Y、Z方向上的位置偏移量，表达式为：

无人机处于转弯飞行时，无人机的飞行冲突保护区为椭体保护区II，表达式为：

其中，(x_δ、y_δ、z_δ)为椭体保护区II内除无人机保护区外的任意一点。

7.根据权利要求6所述的低空无人机农田作业飞行障碍物冲突检测方法，其特征在于：所述步骤5.2的具体方法为：

计算已确定位置的障碍物到无人机的欧氏距离d′，以及无人机与飞行冲突保护区边缘的距离d″，d″＞d′，且d″与d′均服从

化为标准正态分布为：

其中，Δ为无人机与已确定位置的障碍物之间的位置差，

分别为无人机的位置误差的方差与障碍物的位置误差的方差；

则无人机与障碍物之间冲突概率为：

其中，

为无人机的位置的误差与障碍物的位置误差，且为高斯分布：

其中，μ_b为数学期望，μ_b与

通过历史飞行数据与障碍物的数据计算得出，

为无人机与已确定位置的障碍物之间的距离差与最小安全距离之差。

Images