[發明專利]一種基于負比例導引的無人機自動避撞方法

有效

• 申請號: 201710018062.9
• 申請日: 2017-01-10
• 公開（公告）號: CN106647810B
• 公開（公告）日: 2019-06-18
• 發明（設計）人: 蓋文東;張寧;張婧;張桂林;黃梁松
• 申請（專利權）人: 山東科技大學
• 主分類號: G05D1/10
• 分類號: G05D1/10;G01C21/20;G08G5/04
• 代理公司: 青島智地領創專利代理有限公司 37252
• 代理人: 肖峰

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