[發明專利]一種面向突發威脅的無人機航跡規劃算法

有效

• 申請號: 202110273500.2
• 申請日: 2021-03-06
• 公開（公告）號: CN113050684B
• 公開（公告）日: 2022-05-24
• 發明（設計）人: 雷磊;黃加紅;范兼睿;袁代數;王玉;王睿;蔡圣所;張莉涓
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種面向突發威脅的無人機航跡規劃算法，所采用的步驟是：

步驟1：首先將起始點當作當前路徑擴展點，生成到目標點的路徑；如果在此方向上不存在突發威脅區域，則將目標點P_f加入到CLOSE表中，構建成一條從起始點到目標點的路徑曲線，算法運行結束；如果在此方向上存在突發威脅區域，則生成當前路徑擴展點到威脅區域的多條Dubins曲線，具體方法：

(1)路徑擴展點環境搭建

無人機的最小轉彎半徑為ρ，即無人機最小以轉彎半徑為ρ的圓調整航向，起始點位置為p_i，航向為ψ_i，目標點位置為p_f，航向為ψ_f，在無人機起始位置和目標位置之間存在一個半徑為r_threat的威脅區域，并且在最小轉彎半徑和航向的限制條件下，Dubins路徑是從起始位置到達目標位置的最短路徑；另一方面，將威脅區域表示為(x_threat，y_threat，r_threat，G_threat)，其中(x_threat，y_threat)代表威脅區域的中心坐標，r_threat代表威脅區域半徑，G_threat代表威脅級別，威脅等級越大，表明對無人機可能造成的傷害程度越高，并且無人機距離威脅區域越近，遭遇威脅的可能性越大；

(2)路徑擴展點類型

當無人機飛行方向相較于威脅區域處于逆時針方向時，有兩條Dubins路徑可以繞過威脅區域到達目標點位置，分別是路徑p_ip_n1p_f和路徑p_ip_n2p_f，它們分別屬于LSR類型和LSL類型；當無人機飛行方向相較于威脅區域處于順時針方向時，有兩條Dubins路徑可以繞過威脅區域到達目標點位置，它們分別屬于RSL類型和RLR類型；

步驟2：通過多因素Dubins算法估價函數計算并選出代價值最小的點作為下一步的路徑擴展點，將此路徑擴展點加入到CLOSE表中；其中，路徑擴展點的代價計算與選取的具體方法為：

(1)估價函數的建立

多因素Dubins算法同時考慮路徑代價和威脅代價對無人機軌跡的影響，估價函數建立如下：

f(n)＝g(n)+h(n) (1)

g(n)＝w₁·Distance+w₂·Hazard (2)

其中，w₁表示距離代價的影響因子，w₂表示威脅代價的影響因子，同時滿足w₁+w₂＝1，Distance距離由兩端弧長和一段直線長度構成，下面分別計算LSR和RLS兩種類型的Dubins路徑距離；

(2)切點坐標計算

起始節點(x_i，y_i，ψ_i)，求解圓O_i和圓O_threat的切線Q_iP_n的坐標，

計算圓O_i的圓心坐標為

其中，ρ表示無人機的最小轉彎半徑；

威脅區域O_threat的圓心坐標已知，并在此基礎上生成直線航跡Q_iP_n，

因此，向量O_threatO_i逆時針旋轉β₁角度與向量O_threatP_n重合，向量O_threatO_i的模L已知，之后按照比例進行縮放可以得到向量O_threatP_n，從而計算出切點P_n坐標，同理可得到切點Q_i坐標；

(3)指定路徑代價值計算

當無人機在位置為p_i，航向為ψ_i準備繞過威脅區域O_threat時，選擇切點P_n或者p′_n作為A*算法的下一步路徑擴展節點，因此需要對P_n和p′_n節點的代價值通過估價函數進行計算；在這里計算P_n節點的代價值，同理可計算得到；首先計算g(n)為

其中節點Q_i、P_n的位置已由前述步驟計算可知，因此和O_iP_n的長度也可計算得到；接下來計算Hazard，距離威脅區域越近，所受到的威脅就越大，假設把不經過威脅區域的航跡路徑所受到的威脅設置為0，因此只有繞威脅區域飛行的圓弧段航跡才會受到威脅；從當前節點到下一個擴展節點的威脅代價值為

因此Hazard(Q_n-1P_n)＝0，其中，

因此，

(4)預估代價值計算

計算h(n)，同樣將距離代價和威脅代價的預評估影響因子分別用w₁、w₂表示；當無人機以P_n或p′_n點切入威脅區域，則無人機分別以P_nQ_nP_f路徑或P′_nQ′_nP_f路徑以指定位姿到達目標點，則和分別預示了無人機繞過威脅區域的飛行軌跡，所以h(n)表示為

h(P_n)＝w₁·distance′(P_nP_f)+w₂·Hazard′(P_nP_f) (12)

其中，

步驟3：將步驟2中選取的路徑擴展點作為當前的位置，然后重新執行步驟1的操作；之后重復上述過程，不斷產生新的路徑擴展點，直到到達目標位置，構建出一條完整的路徑。