[发明专利]一种基于负比例导引的无人机自动避撞方法

权利要求书

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

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

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

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

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

步骤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)。