[發明專利]一種面向無人機的航跡規劃方法

有效

• 申請號: 202110372562.9
• 申請日: 2021-04-07
• 公開（公告）號: CN112947594B
• 公開（公告）日: 2023-08-04
• 發明（設計）人: 代福成;劉軍;牟瀚波;張勝楠;潘志軍
• 申請（專利權）人: 東北大學
• 主分類號: G05D1/12
• 分類號: G05D1/12
• 代理公司: 沈陽東大知識產權代理有限公司 21109
• 代理人: 梁焱

1.一種面向無人機的航跡規劃方法，其特征在于：包括以下步驟：

步驟1、搭建系統環境；

在長寬分別為L,W的矩形區域R內有若干處雷達監測點和威脅區域，所以該區域可以分為可拓展區域R_free，和法擴展區域R_obs，R_free與R_obs都是R的子集，且滿足R＝R_free∪R_obs與起始位置q_init∈R_free和目標位置q_goal∈R_free；

步驟2、在確定下一跳節點時，利用目標啟發式的思想生成多個候選點q_rand，在所有的候選點中選擇合適的點作為下一跳點q_near；具體過程如下：

步驟2-1、確定概率閾值p_gc，0＜p_gc＜1，在每迭代m次后產生的隨機數；

步驟2-2、在(0，1)之間生成一個隨機數，將值設置為概率p_g；

步驟2-3、比較p_g和p_gc；如果p_g＞p_gc，則轉到步驟2-4；如果不是，轉到步驟2-6；

步驟2-4、在樹上找到最近的q_goal＝q_rand的節點，并將其定義為q_near；根據步長，生成未確定的節點q_unrand；

步驟2-5、如果q_unrand位于障礙威脅區域中則返回步驟2-2；如果不在，則將q_unrand添加到搜索樹并轉到步驟2-2；

步驟2-6、在規劃區域中，隨機生成m個節點，并用表示；比較最近的點并用表示；

步驟2-7、對m個確定點最小值q_min；

步驟2-8、如果P_min在威脅區域內返回步驟2-2，否則增加P_min到樹中并返回步驟2-2；

步驟3、把起始點q_init和目標點q_goal分別當作對方的終點進行雙向擴展，直到它們連接起來終止擴展；

步驟4、交換兩顆樹的搜索路徑從而得到起始點與目標點的路徑規劃；

步驟5、檢查路由表，刪除路由表中的冗余路有點；具體步驟如下：

步驟5-1、為簡化路由航跡建立初始清單Γ點；最初Γ是空的；

步驟5-2、增加目標點p_n到Γ，讓j＝n，i＝1；

步驟5-3、如果存在威脅障礙點在p_j與p_i中返回第四步，否則，執行5-5；

步驟5-4、如果i＝j-1，則j＝j-1，i＝1返回5-3；否則i＝i+1，返回5-3；

步驟5-5、增加p_i到Γ并且讓j＝i，i＝1；如果j＝1，則刪除該點，否則返回5-3；

步驟6、對路徑進行滿足曲率約束的平滑處理；

假設無人機執行任務的飛行速度為v_t，無人機約束最大轉向角速率為ω，此時對應的最小轉向半徑為R，二維平面下則無人機相對位置(x,y)和航向θ的運動方程簡化為以下方程：

x(t)＝vcosθ(t)????????????????????????????????(1)

x(t)＝vsinθ(t)????????????????????????????????(2)

θ(t)＝Ω?Ω∈[-ω,ω]?????????????????????????????(3)

R＝v/ω??????????????????????????????????(4)

無人機進行航跡交接處對接看作在二維平面內具有特定方向向量的兩點，無人機飛行進入彎道約束區域切點稱之為起始點，無人機轉向結束離開自適應過度進入下一航段的交接點稱之為結束點，交接自適應過度軌跡為Dubins曲線軌跡，該軌跡曲線曲率滿足大于1/R；通過以下3個步驟實現平滑，具體步驟如下：

步驟6-1：生成轉彎圓弧，圓心為o；

根據進入航段ab建立直角坐標系，其中航跡點a、b、c的坐標分別為(x_a,y_a)，(x_b,y_b)，(x_c,y_c)，航段進入點d的坐標為(x_d,y_d)，航段離開點坐標為(x_n,y_n)；

無人機最小轉彎約束半徑為R，無人機寬度為D；

根據無人機的約束條件通過幾何知識求出轉彎Dubins曲線圓弧的圓心o位于銳角的角平分線上，同時求得圓心o的坐標(x_o,y_o)為：

步驟6-2：生成切線軌跡；

從航跡進入點d向轉彎軌跡做一條切線，切點為e，得到切線長de；由已知條件及幾何知識可知圓弧圓心o點與e點距離為半徑R，設圓心o點與d點距離為l_od，則點e與點d點距離為l_ed，切線航跡e點坐標(x_e,y_e)為：

步驟6-3：生成對準軌跡圓心；

對準軌跡圓也是滿足轉彎約束條件生成的Dubins曲線軌跡圓對準軌跡圓心記為m，轉彎軌跡圓心o與b點距離為l_ob，轉彎軌跡圓心o點與對準軌跡圓心m點距離為2R，通過求解得到圓心m坐標(x_m,y_m)為：

由上面三個步驟求解出滿足約束條件的基于Dubins曲線的銳角自適應過度軌跡；

步驟7、輸出一條從起始點到目標點的平滑路徑。