[發(fā)明專利]一種多旋翼無人機自動返航路徑規(guī)劃算法

有效

• 申請?zhí)枺?/td>: 201610017606.5
• 申請日: 2016-01-12
• 公開（公告）號: CN105487554B
• 公開（公告）日: 2018-01-23
• 發(fā)明（設計）人: 王謙;金衛(wèi)民;胡鎮(zhèn);胡陽;張耀;鄧海;李頂根
• 申請（專利權）人: 武漢頂翔智控科技有限公司;江西頂翔智控科技有限公司
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 武漢宇晨專利事務所42001
• 代理人: 李鵬,王敏鋒

1.一種多旋翼無人機自動返航路徑規(guī)劃算法，其特征在于，包括以下步驟：

步驟1，以原點在起飛點，X,Y,Z軸分別指向北、東、地的當地鉛錘坐標系為導航坐標系，在無人機正常飛行過程中在固定的時間間隔ΔT的記錄點記錄無人機在導航坐標系中的飛行參數，飛行參數包括無人機在各個記錄點的實時位置、速度矢量，

步驟2，自動返航飛行觸發(fā)后，根據記錄點的飛行參數搜索飛行軌跡交叉點，

將無人機起飛點、飛行軌跡交叉點以及返航起始點統(tǒng)稱為興趣點，從起飛點開始沿著飛行軌跡計算任意兩個興趣點之間的沿飛行軌跡的長度，非直接連通的興趣點之間的距離則記為∞，作鄰接矩陣：

其中，鄰接矩陣的第i行與興趣點P_i對應，第j列與興趣點P_j對應，其中i∈1,…,n，j∈1,…,n，n為興趣點總的個數，鄰接矩陣中第i行的各個元素為對應的興趣點P_i依次與各列對應的興趣點P₁～P_n之間的飛行軌跡長度；

步驟3，從返航起始點開始，將返航起始點作為多叉樹的根節(jié)點，其他的興趣點作為多叉樹的子節(jié)點構建多叉樹，剔除多叉樹中葉子節(jié)點不是起飛點的分支，在多叉樹中運用深度優(yōu)先方法搜索最短的返航路徑；

步驟4，將最短的返航路徑對應的記錄點組成返航路徑點集，通過依次將返航路徑點集中的點作為無人機的目標航路點進行跟蹤，完成返航軌跡的跟蹤。

2.根據權利要求1所述的一種多旋翼無人機自動返航路徑規(guī)劃算法，其特征在于，所述的飛行軌跡交叉點的判別需要同時滿足以下三個標準：

(1)兩個記錄點的無人機的實時位置的距離小于設定的距離Δl；

(2)兩個記錄點的時間差大于設定的時間差Δt；

(3)兩個記錄點的無人機的速度矢量夾角大于設定的速度矢量夾角Δα。

3.根據權利要求1所述的一種多旋翼無人機自動返航路徑規(guī)劃算法，其特征在于，所述的步驟3包括以下步驟：

步驟3.1、選取返航起始點作為多叉樹的根節(jié)點，記為當前級節(jié)點；

步驟3.2、若所有興趣點均已納入多叉樹，執(zhí)行步驟3.5；否則，執(zhí)行步驟3.3；

步驟3.3、對當前級的各個節(jié)點依次進行如下處理：

在鄰接矩陣對應節(jié)點的行中搜索大于0且小于∞的元素作為直通點元素，直通點元素所在列則對應了與節(jié)點直接連通的興趣點，查找直通點元素中所在列對應的興趣點不屬于節(jié)點的所有父節(jié)點的元素作為子節(jié)點元素，將子節(jié)點元素的所在列對應的興趣點作為節(jié)點的子節(jié)點并入多叉樹；

步驟3.4、將當前節(jié)點級別遞增，返回步驟3.2；

步驟3.5、將多叉樹中葉子節(jié)點不是起飛點的分支去除；

步驟3.6、在多叉樹中運用深度優(yōu)先方法搜索最短的返航路徑。