CN106647810A - 一种基于负比例导引的无人机自动避撞方法 - Google Patents

Abstract

本发明提供了一种基于负比例导引的无人机自动避撞方法，属于无人机飞行控制技术领域。通过基于无人机与侵入机相对几何关系的碰撞检测，基于负比例导引的无人机避撞指令生成与飞行控制，无人机自动避撞完成时间估计，检测是否达到无人机避撞完成时间，无人机正常飞行等步骤完成无人机自动避撞，本发明利用负比例导引所需过载小的特点，采用负比例导引律引导无人机进行自动避撞飞行，并通过迭代法求解避撞模型方程，以估计避撞完成时间，适用于多种初始航向角条件和速度条件。

Description

一种基于负比例导引的无人机自动避撞方法

技术领域

本发明属于无人机飞行控制技术领域，具体涉及一种基于负比例导引的无人机自动避撞方法。

背景技术

随着空中无人机不断增多，如何防止无人机与有人机之间以及无人机相互之间发生碰撞，已到了刻不容缓的地步。在伊拉克和阿富汗战场，由于无人机环境感知与规避能力缺乏、安全性能较弱，无人机活动频繁，严重干扰了有人机的正常飞行，致使操作人员无法及时判明周围空域的情况，只能按照任务程序操作。因此，有人机飞行员执行任务时需要时常留意闯入自己航线的无人机，以避免相撞。自动避撞技术已成为当前无人机研究的热点。

在无人机避撞方面的研究，核心思路如下：首先通过避撞检测方法检测可能发生的碰撞，然后采取合适的避撞方法，在一定的性能指标和约束下进行避撞飞行。但是现有的基于比例导引的无人机自动避撞方法在两无人机初始航向角偏差值较小时，完成避撞需要较大的过载甚至超过无人机的过载约束导致无法完成避撞，另外现有的自动避撞方法难以准确估计自动避撞完成时间。

发明内容

针对现有的无人机自动避撞方法在两无人机初始航向角偏差值较小时，完成避撞需要较大的过载甚至超过无人机的过载约束导致无法完成避撞，以及现有的自动避撞方法难以准确估计自动避撞完成时间的问题，本发明提供了一种基于负比例导引的无人机自动避撞方法。

本发明采用以下的技术方案：

一种基于负比例导引的无人机自动避撞方法，包括以下步骤：

步骤1：基于无人机与侵入机相对几何关系的碰撞检测；初始状态下避撞机处于正常飞行模式，沿预定的正常飞行轨迹飞往目标点，利用机载传感器获得避撞机和侵入机的飞行状态信息，飞行状态信息包括避撞机初始位置(x₀,y₀)、飞行速度V和航向角ψ(t)，侵入机的位置(x_OB,y_OB)、速度V_OB和航向角ψ_OB，根据飞行状态信息得出两无人机的相对距离R_T(t)为：

两无人机相对速度V_rel(t)为：

V_rel(t)＝Vcos(ψ_rel-ψ(t))+V_OBcos(π+ψ_OB-ψ_rel(t)) (2)；

其中，ψ_rel(t)为两无人机相对速度方位角

视线角λ(t)为：

相对速度方位角和视线角的差值ε(t)为：

ε(t)＝|λ(t)-ψ_rel(t)| (5)；

给定安全距离R_S后可求取碰撞锥的半顶角θ(t)为：

碰撞锥下边界角μ(t)为：

μ(t)＝λ(t)-θ(t) (7)；

当相对速度V_rel(t)在碰撞锥内，即相对速度方位角与视线角的偏差绝对值小于碰撞锥的半顶角，公式为|λ(t)-ψ_rel(t)|＝|ε(t)|＜θ(t)，则两个无人机会发生碰撞，并执行步骤2，如果速度方位角与视线角的偏差绝对值大于或等于碰撞锥的半顶角，则执行步骤5；

步骤2：基于负比例导引的无人机避撞指令生成与飞行控制；检测到避撞机与侵入机发生碰撞，首先计算机生成负比例导引指令a(t)，负比例导引指令a(t)经过下式的变换之后作为无人机滚转角姿态控制回路的给定值φ_g，

采用常规PID控制律设计无人机的横侧向滚转角姿态控制回路和侧滑角姿态控制回路，用给定指令φ_g作为滚转角姿态控制回路的输入，侧滑角控制回路的输入为0，实现无侧滑飞行；避撞机通过滚转机动，去追踪避撞点，经过一定的时间，两无人机的相对速度V_rel与碰撞锥下边界角μ(t)重合，这时相对速度V_rel(t)开始脱离碰撞锥；负比例导引指令变为0，避撞机保持此时的航向角作匀速直线飞行，直至与侵入机航迹相交后避撞完成；

步骤3：无人机自动避撞完成时间估计；在执行步骤2时，同时对无人机完成避撞的时间进行估计；

步骤4：检测避撞是否完成；检测飞行时间是否达到估计的无人机避撞完成时间T，如果达到了就执行步骤5。如果未达到继续执行步骤2；

步骤5：避撞机进入正常飞行模式；避撞机继续飞往目标点。

优选地，步骤2中的负比例导引指令a(t)为：

其中，N为负的导引系数，取N＝-3；相避撞机航向角ψ(t)可通过机载传感器测量得到；为碰撞锥的下边界角速度，

其中，为相对速度的变化率，

优选地，无人机自动避撞完成时间的估计过程为：

步骤1：避撞机滚转机动时间估计；首先计算负比例导引指令a(t)初值a₀，当避撞机机载传感器检测避撞机与侵入机发生碰撞时，以此时的无人机状态作为初始状态，此时飞行时间t＝0；

避撞机初始航向角ψ(0)可由机载传感器测量得到，由式(2)可得两无人机初始相对速度V_rel(0)。通过式(10)可计算碰撞锥的下边界角速度初值最后将V_rel(0)和代入式(9)可以计算出负比例导引指令初值a₀；

避撞机航向角为：

其中，g为重力加速度，V为避撞机的速度；

根据式(9)至(12)可计算当前时刻的负比例导引指令a(t)；

持续计算负比例导引指令，直到相对速度方位角ψ_rel(t)与碰撞锥的下边界角μ(t)重合为止，此时的时刻T₁即为避撞机滚转机动的时间；

步骤2：T₁时刻之后，避撞机保持此时的航向作匀速直线飞行至两无人机航迹交点，避撞机进行直线飞行的时间T₂求解过程为：

首先求解航迹交点坐标(x_T,y_T)：

式中和是T₁时刻避撞机的位置坐标，ψ(T₁)可通过式(12)求得，通过解方程组(13)可以求出两无人机航迹的交点坐标(x_T,y_T)，则避撞机进行直线飞行的时间为：

无人机自动避撞完成时间T：

T＝T₁+T₂ (15)。

本发明具有的有益效果是：

本发明提出的基于负比例导引的无人机自动避撞方法所需过载较小，避免无人机过大的机动。该无人机自动避撞方法适用于多种初始航向角和速度条件，且自动避撞完成时间估计方法准确且求解过程简单。

附图说明

图1为基于负比例导引的无人机自动避撞方法的流程图。

图2为无人机避撞轨迹示意图。

图3为避撞机与侵入机的几何关系图。

图4为避撞机避撞过程示意图。

图5为无人机自动避撞轨迹仿真图。

图6为无人机自动避撞距离仿真图。

具体实施方式

下面结合附图对本发明进行具体的说明：

结合图1至图6，本发明提出了一种基于负比例导引的无人机自动避撞方法。在初始状态避撞机处于正常飞行模式。避撞机沿预定的正常轨迹飞往目标点。根据侵入机和避撞机的相对几何关系检测两无人机是否会发生碰撞。如果不发生碰撞，无人机继续正常飞行飞往目标点。如果检测到两无人机会发生碰撞，采用负比例导引指令引导避撞机进行自动避撞飞行，同时通过迭代法求解避撞模型方程，以估计避撞完成时间。当两无人机相对速度向量脱离碰撞锥时，导引指令变为0，避撞机保持此时的航向角作匀速直线飞行。当达到估计的避撞完成时间后避撞完成，避撞机进入正常飞行模式飞往目标点。

一种基于负比例导引的无人机自动避撞方法，流程图如图1所示，包括以下步骤：

步骤1：基于无人机与侵入机相对几何关系的碰撞检测。

根据图2，初始状态下避撞机处于正常飞行模式，沿预定的正常飞行轨迹飞往目标点。利用机载传感器获得避撞机和侵入机的飞行状态信息，飞行状态信息包括避撞机初始位置(x₀,y₀)、飞行速度V和航向角ψ(t)，侵入机的位置(x_OB,y_OB)、速度V_OB和航向角ψ_OB。根据图3的避撞机和侵入机的相对几何关系，得出两无人机的相对距离R_T(t)为：

两无人机相对速度V_rel(t)为：

V_rel(t)＝Vcos(ψ_rel-ψ(t))+V_OBcos(π+ψ_OB-ψ_rel(t)) (2)；

其中，ψ_rel(t)为两无人机相对速度方位角

视线角λ(t)为：

相对速度方位角和视线角的差值ε(t)为：

ε(t)＝|λ(t)-ψ_rel(t)| (5)；

给定安全距离R_S后可求取碰撞锥的半顶角θ(t)为：

碰撞锥下边界角μ(t)为：

μ(t)＝λ(t)-θ(t) (7)；

当相对速度V_rel(t)在碰撞锥内，即相对速度方位角与视线角的偏差绝对值小于碰撞锥的半顶角，公式为|λ(t)-ψ_rel(t)|＝|ε(t)|＜θ(t)，则两个无人机会发生碰撞，并执行步骤2；如果速度方位角与视线角的偏差绝对值大于或等于碰撞锥的半顶角，则执行步骤5。

步骤2：基于负比例导引的无人机避撞指令生成与飞行控制。

检测到避撞机与侵入机发生碰撞，首先计算机生成负比例导引指令a(t)，

式中，N为负的导引系数，取N＝-3。相对速度V_rel(t)可根据式(2)获得。相对速度方位角ψ_rel(t)可通过式(3)计算。避撞机航向角ψ(t)可通过机载传感器测量得到。为碰撞锥的下边界角速度，

其中，为相对速度的变化率，

负比例导引指令a(t)经过下式的变换之后作为无人机滚转角姿态控制回路的给定值φ_g，

采用常规PID控制律设计无人机的横侧向滚转角姿态控制回路和侧滑角姿态控制回路，用给定指令φ_g作为滚转角姿态控制回路的输入，侧滑角控制回路的输入为0，实现无侧滑飞行。根据图4，D点为避撞点，虚线AD为避撞机在负比例导引律下的飞行航迹。避撞机通过滚转机动，去追踪避撞点D，经过一定的时间，两无人机的相对速度V_rel与碰撞锥下边界角μ(t)重合，这时相对速度V_rel(t)开始脱离碰撞锥。负比例导引指令变为0，避撞机保持此时的航向角作匀速直线飞行，直至与侵入机航迹相交后避撞完成。

步骤3：无人机自动避撞完成时间估计。

在执行步骤2时，同时对无人机完成避撞的时间进行估计；无人机自动避撞完成时间的估计过程为：

步骤1：避撞机滚转机动时间估计；首先计算负比例导引指令a(t)初值a₀，当避撞机机载传感器检测避撞机与侵入机发生碰撞时，以此时的无人机状态作为初始状态，此时飞行时间t＝0；

避撞机初始航向角ψ(0)可由机载传感器测量得到，由式(2)可得两无人机初始相对速度V_rel(0)。通过式(9)可计算碰撞锥的下边界角速度初值最后将V_rel(0)和代入式(8)可以计算出负比例导引指令初值a₀。

避撞机航向角为：

其中，g为重力加速度，V为避撞机的速度；

根据式(9)至(12)可计算当前时刻的负比例导引指令a(t)；

持续计算负比例导引指令，直到相对速度方位角ψ_rel(t)与碰撞锥的下边界角μ(t)重合为止，此时的时刻T₁即为避撞机滚转机动的时间。

步骤2：T₁时刻之后，避撞机保持此时的航向作匀速直线飞行至两无人机航迹交点，避撞机进行直线飞行的时间T₂求解过程为：

首先求解航迹交点坐标(x_T,y_T)，根据图2几何关系可知：

式中和是T₁时刻避撞机的位置坐标，ψ(T₁)可通过式(12)求得，通过解方程组(13)可以求出两无人机航迹的交点坐标(x_T,y_T)，则避撞机进行直线飞行的时间为：

无人机自动避撞完成时间T：

T＝T₁+T₂ (15)。

步骤4：检测避撞是否完成。

检测飞行时间是否达到估计的无人机避撞完成时间T，如果达到了就执行步骤5。如果未达到继续执行步骤2；

步骤5：避撞机进入正常飞行模式。

避撞机继续飞往目标点。图5和图6为基于负比例导引的无人机自动避撞仿真图。

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (3)

1.一种基于负比例导引的无人机自动避撞方法，其特征在于，包括以下步骤：

步骤1：基于无人机与侵入机相对几何关系的碰撞检测；初始状态下避撞机处于正常飞行模式，沿预定的正常飞行轨迹飞往目标点，利用机载传感器获得避撞机和侵入机的飞行状态信息，飞行状态信息包括避撞机初始位置(x₀,y₀)、飞行速度V和航向角ψ(t)，侵入机的位置(x_OB,y_OB)、速度V_OB和航向角ψ_OB，根据飞行状态信息得出两无人机的相对距离R_T(t)为：

$R_T\left(t\right)=\sqrt{{(x_0+V{\∫}_0^{t}cos\mathrm{\ψ}(t)dt-x_{OB}-V_{OB}{\mathrm{cos\ψ}}_{OB}t)}^2+{(y_0+V{\∫}_0^{t}sin\mathrm{\ψ}(t)dt-y_{OB}-V_{OB}{\mathrm{sin\ψ}}_{OB}t)}^2}---\left(1\right);$

两无人机相对速度V_rel(t)为：

V_rel(t)＝V cos(ψ_rel-ψ(t))+V_OBcos(π+ψ_OB-ψ_rel(t)) (2)；

其中，ψ_rel(t)为两无人机相对速度方位角

${\mathrm{\ψ}}_{rel}\left(t\right)=\mathrm{\π}+{\tan }^{-1}\left(\frac{Vsin\mathrm{\ψ}\left(t\right)+V_{OB}sin(\mathrm{\π}+{\mathrm{\ψ}}_{OB})}{Vcos\mathrm{\ψ}\left(t\right)+V_{OB}cos(\mathrm{\π}+{\mathrm{\ψ}}_{OB})}\right)---\left(3\right);$

视线角λ(t)为：

$\mathrm{\λ}\left(t\right)=\mathrm{\π}+{\tan }^{-1}\left(\frac{y_0+V{\∫}_0^{t}sin\mathrm{\ψ}\left(t\right)dt-y_{OB}-V_{OB}{\mathrm{sin\ψ}}_{OB}t}{x_0+V{\∫}_0^{t}cos\mathrm{\ψ}\left(t\right)dt-x_{OB}-V_{OB}{\mathrm{cos\ψ}}_{OB}t}\right)---\left(4\right);$

相对速度方位角和视线角的差值ε(t)为：

ε(t)＝|λ(t)-ψ_rel(t)| (5)；

给定安全距离R_S后可求取碰撞锥的半顶角θ(t)为：

$\mathrm{\θ}\left(t\right)={\tan }^{-1}\left(\frac{R_S}{\sqrt{\left(R_T^2\right(t)-R_S^2)}}\right)---\left(6\right);$

碰撞锥下边界角μ(t)为：

μ(t)＝λ(t)-θ(t) (7)；

当相对速度V_rel(t)在碰撞锥内，即相对速度方位角与视线角的偏差绝对值小于碰撞锥的半顶角，公式为|λ(t)-ψ_rel(t)|＝|ε(t)|＜θ(t)，则两个无人机会发生碰撞，并执行步骤2，如果速度方位角与视线角的偏差绝对值大于或等于碰撞锥的半顶角，则执行步骤5；

步骤2：基于负比例导引的无人机避撞指令生成与飞行控制；检测到避撞机与侵入机发生碰撞，首先计算机生成负比例导引指令a(t)，负比例导引指令a(t)经过下式的变换之后作为无人机滚转角姿态控制回路的给定值φ_g，

${\mathrm{\φ}}_g=a\left(t\right)\×\frac{180}{\mathrm{\π}}---\left(8\right);$

采用常规PID控制律设计无人机的横侧向滚转角姿态控制回路和侧滑角姿态控制回路，用给定指令φ_g作为滚转角姿态控制回路的输入，侧滑角控制回路的输入为0，实现无侧滑飞行；避撞机通过滚转机动，去追踪避撞点，经过一定的时间，两无人机的相对速度V_rel与碰撞锥下边界角μ(t)重合，这时相对速度V_rel(t)开始脱离碰撞锥；负比例导引指令变为0，避撞机保持此时的航向角作匀速直线飞行，直至与侵入机航迹相交后避撞完成；

步骤3：无人机自动避撞完成时间估计；在执行步骤2时，同时对无人机完成避撞的时间进行估计；

步骤4：检测避撞是否完成；检测飞行时间是否达到估计的无人机避撞完成时间T，如果达到了就执行步骤5。如果未达到继续执行步骤2；

步骤5：避撞机进入正常飞行模式；避撞机继续飞往目标点。

2.根据权利要求1所述的一种基于负比例导引的无人机自动避撞方法，其特征在于，步骤2中的负比例导引指令a(t)为：

$a\left(t\right)={\mathrm{NV}}_{rel}\left(t\right)\stackrel{\·}{\mathrm{\μ}}\left(t\right)sin\left(\mathrm{\ψ}\right(t)-{\mathrm{\ψ}}_{rel}(t\left)\right)---\left(9\right);$

其中，N为负的导引系数，取N＝-3；相避撞机航向角ψ(t)可通过机载传感器测量得到；为碰撞锥的下边界角速度，

$\stackrel{\·}{\mathrm{\μ}}\left(t\right)=\frac{{\stackrel{\·}{R}}_T}{R_T}\left(tan\mathrm{\λ}\right(t)-tan\mathrm{\θ}(t\left)\right)-\frac{V_{rel}\left(t\right){\mathrm{sin\ψ}}_{rel}\left(t\right)}{R_T\left(t\right)cos\mathrm{\λ}\left(t\right)}---\left(10\right);$

其中，为相对速度的变化率，

${\stackrel{\·}{R}}_T\left(t\right)=-Vcos\left(\mathrm{\λ}\right(t)-\mathrm{\ψ}(t\left)\right)+V_{OB}cos\left(\mathrm{\λ}\right(t)-{\mathrm{\ψ}}_{OB})---\left(11\right).$

3.根据权利要求2所述的一种基于负比例导引的无人机自动避撞方法，其特征在于，无人机自动避撞完成时间的估计过程为：

步骤1：避撞机滚转机动时间估计；首先计算负比例导引指令a(t)初值a₀，当避撞机机载传感器检测避撞机与侵入机发生碰撞时，以此时的无人机状态作为初始状态，此时飞行时间t＝0；

$a_0={\mathrm{NV}}_{rel}\left(0\right)\stackrel{\·}{\mathrm{\μ}}\left(0\right)\sin \left(\mathrm{\ψ}\right(0)-{\mathrm{\ψ}}_{rel}(0\left)\right);$

避撞机初始航向角ψ(0)可由机载传感器测量得到，由式(2)可得两无人机初始相对速度V_rel(0)。通过式(10)可计算碰撞锥的下边界角速度初值最后将V_rel(0)和代入式(9)可以计算出负比例导引指令初值a₀；

避撞机航向角为：

$\mathrm{\ψ}\left(t\right)={\mathrm{\ψ}}_0-\frac{g}{V}a(t-1)t---\left(12\right);$

其中，g为重力加速度，V为避撞机的速度；

根据式(9)至(12)可计算当前时刻的负比例导引指令a(t)；

持续计算负比例导引指令，直到相对速度方位角ψ_rel(t)与碰撞锥的下边界角μ(t)重合为止，此时的时刻T₁即为避撞机滚转机动的时间；

步骤2：T₁时刻之后，避撞机保持此时的航向作匀速直线飞行至两无人机航迹交点，避撞机进行直线飞行的时间T₂求解过程为：

首先求解航迹交点坐标(x_T,y_T)：

$\left\{\begin{array}{c}y-y_{OB}={\mathrm{tan\ψ}}_{OB}(x-x_{OB})\\ y-y_{T_1}=\tan \mathrm{\ψ}\left(T_1\right)(x-x_{T_1})\\ x_{T_1}=x_0+V{\∫}_0^{T_1}\cos \mathrm{\ψ}\left(t\right)dt\\ y_{T_1}=y_0+V{\∫}_0^{T_1}\sin \mathrm{\ψ}\left(t\right)dt\end{array}\right.---\left(13\right);$

式中和是T₁时刻避撞机的位置坐标，ψ(T₁)可通过式(12)求得，通过解方程组(13)可以求出两无人机航迹的交点坐标(x_T,y_T)，则避撞机进行直线飞行的时间为：

$T_2=\frac{\sqrt{{(x_T-x_{T_1})}^2+{(y_T-y_{T_1})}^2}}{V}---\left(14\right)$

无人机自动避撞完成时间T：

T＝T₁+T₂ (15)。