[發明專利]一種多目標約束下的無人機分級式路徑規劃方法

在審

• 申請號: 202111326666.2
• 申請日: 2021-11-10
• 公開（公告）號: CN113985922A
• 公開（公告）日: 2022-01-28
• 發明（設計）人: 劉洋;王志;呂人力
• 申請（專利權）人: 浙江建德通用航空研究院;中國民航管理干部學院
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 濟南龍瑞知識產權代理有限公司 37272
• 代理人: 韓園園

1.一種多目標約束下的無人機分級式路徑規劃方法，其特征在于，包括以下步驟：

S1、根據給定無人機起始點S以及目的地點D進行坐標系轉換，將轉換后的坐標系定義為新坐標系，將轉換前的坐標系定義為原始坐標系，轉換規則為：無人機起始點S作為新坐標系的原點，無人機起始點S以及目的地點D的連線作為新坐標系的X軸，表示由起始點S指向目的地點D的矢量，在原始坐標系所在二維平面內過起始點S做的垂線作為新坐標系的Y軸，過起始點S做原始坐標系所在二維平面的垂線作為新坐標系的Z軸；

S2、在S1所述新坐標系下，無人機二維路徑規劃問題，以無人機飛行路徑最短、距離危險區域邊界直線距離最遠為優化目標，將無人機運行環境內的障礙物定義為危險區域，二維路徑規劃問題中危險區域是圓形區域，采用天牛須搜索算法進行初次路徑規劃，形成無人機一級路徑；無人機三維路徑規劃問題，以無人機飛行路徑最短、距離危險區域邊界直線距離最遠、無人機飛行高度最低為優化目標，將無人機運行環境內的障礙物定義為危險區域，三維路徑規劃問題中危險區域是圓柱體區域，采用天牛須搜索算法進行初次路徑規劃，形成無人機一級路徑；

S3、在S1所述新坐標系下，以S2形成的無人機一級路徑為中心線，無人機二維路徑規劃問題，以垂直距離d向中心線兩側平面擴展，形成不進入危險區域的無人機二維飛行走廊；無人機三維路徑規劃問題，以垂直距離d為半徑向中心線周圍空間擴展，形成不進入危險區域的無人機三維飛行走廊；

S4、在S3形成的無人機飛行走廊內，二維路徑規劃問題，以無人機飛行路徑最短、距離危險區域邊界直線距離最遠、轉彎角度最小為優化目標，采用天牛須搜索算法、粒子群優化算法進行二次聯合路徑規劃，形成無人機二級路徑；三維路徑規劃問題，以無人機飛行路徑最短、距離危險區域邊界直線距離最遠、轉彎角度最小、升降角度最小、無人機飛行高度最低為優化目標，采用天牛須搜索算法、粒子群優化算法進行二次聯合路徑規劃，形成無人機二級路徑；

S5、將S4形成的無人機二級路徑作為最優路徑并進行坐標系轉換，轉換規則為：將新坐標系下得到的無人機二級路徑點坐標轉換為原始坐標系下的路徑點坐標，經平滑處理后，上傳至無人機機載飛控模塊后供無人機使用。

2.根據權利要求1所述的多目標約束下的無人機分級式路徑規劃方法，其特征在于，所述步驟S2包括以下分步驟：

S21、設定所要規劃無人機運行路徑上的路徑點數量為N_dim，其中N_dim也是天牛須搜索算法中搜索空間的維度，隨機生成無人機每個路徑點的初始坐標并將所有初始路徑點表示為路徑點集合WP＝{WP_m}，其中m＝1,2,…,N_dim，路徑點集合WP即為天牛的位置坐標；分別計算以下優化目標：無人機飛行路徑初始長度J₁、所有路徑點距離每個危險區域邊界初始最短直線距離總和的倒數J₂、無人機初始路徑點飛行高度總和J₃；二維路徑規劃問題中J₃＝0；得到目標函數其中，J為路徑代價值，并將min(J)作為最終優化目標；存儲路徑代價值J及路徑點集合WP；

S22、開始迭代，根據公式A_L＝WP+Dir·d₀計算天牛左須在當前代數對應的位置坐標、根據公式A_R＝WP-Dir·d₀計算天牛右須在當前代數對應的位置坐標，公式A_L＝WP+Dir·d₀和A_R＝WP-Dir·d₀中，WP表示路徑點集合，Dir表示天牛右須指向左須的隨機單位向量，d₀表示天牛兩須之間的距離；根據公式計算天牛左須對應的左須代價值、根據公式計算天牛右須對應的右須代價值；采用公式WP＝WP-Step·Dir·Sign(VA_L-VA_R)，更新WP后，將更新后的路徑點坐標重新存儲在WP中，從而更新天牛的位置坐標，其中Step表示天牛每次運動的可變步長，Dir表示天牛右須指向左須的隨機單位向量，Sign()表示符號函數，d₀表示天牛兩須之間的距離；

S23、根據更新后的天牛位置坐標重新計算無人機路徑代價值J，與已保存的路徑代價值J比對，存儲最小的路徑代價值J及最小的路徑代價值J對應的路徑點集合WP；判斷是否達到指定的迭代次數，結果為否，返回步驟S22；結果為是，執行步驟S24；

S24、迭代完成，將最后存儲的路徑點集合WP作為無人機一級路徑的路徑點集合。

3.根據權利要求1所述的多目標約束下的無人機分級式路徑規劃方法，其特征在于，所述步驟S4包括以下分步驟：

S41、在S3形成的無人機飛行走廊內，采用粒子群優化算法，隨機生成粒子維度為N_dim、粒子數量為N_P的初始粒子群，第i個粒子的位置代表飛行走廊內可行的無人機路徑點集合WP′_i，表示為路徑點集合WP′_i的維度為N_dim，i＝1,2,…,N_P；對于每一個粒子，分別計算以下優化目標：無人機飛行路徑初始長度J′₁、所有路徑點距離每個危險區域邊界初始最短直線距離總和的倒數J′₂、無人機初始路徑點飛行高度總和J′₃、無人機相鄰路徑點間的轉彎角度總和J′₄、無人機相鄰路徑點間的升降角度總和J′₅；二維路徑規劃問題中J′₃＝0、J′₅＝0；得到目標函數其中，J'為粒子的路徑代價值，并將min(J')作為最終優化目標；存儲每個粒子的路徑代價值J'及路徑點集合WP′_i；以上述N_P個路徑代價值中的最小值為最小路徑代價值J′_min、以最小路徑代價值J′_min對應的路徑點集合為WP'，存儲最小路徑代價值J′_min及路徑點集合WP′；

S42、開始迭代，隨機選取N_P/2個粒子并將其中任意粒子視為一個天牛個體，并對每個天牛個體采用天牛須搜索算法，根據公式A′_L＝WP′_i+Dir'·d′₀計算天牛左須在當前代數對應的位置坐標、根據公式A′_R＝WP′_i-Dir'·d′₀計算天牛右須在當前代數對應的位置坐標，公式A′_L＝WP′_i+Dir'·d′₀和A′_R＝WP′_i-Dir'·d′₀中，WP′_i表示第i個粒子的路徑點集合，Dir'表示第i個粒子的天牛右須指向左須的隨機單位向量，d′₀表示第i個粒子的天牛兩須之間的距離；根據公式計算天牛左須對應的左須代價值、根據公式計算天牛右須對應的右須代價值；采用公式WP′_i＝WP′_i-Step'·Dir'·Sign(VA′_L-VA′_R)，更新WP′_i后，將更新后的路徑點坐標重新存儲在WP′_i中，從而更新每個天牛的位置坐標，即更新每個粒子的位置坐標，其中Step'表示第i個粒子的天牛每次運動的可變步長，Dir'表示第i個粒子的天牛右須指向左須的隨機單位向量，Sign()表示符號函數；對于剩余N_P/2個粒子，采用粒子群優化算法，對于剩余N_P/2個粒子中的任意粒子，采用公式更新第i個粒子在t+1代的速度，采用公式更新第i個粒子在t+1代的位置，即更新第i個粒子在t+1代可行的無人機路徑點集合，公式和中，α和β分別表示第i個粒子運動速度的慣性系數以及第i個粒子所處位置的慣性系數，c₁和c₂分別表示一個隨機數，和分別表示當前代數t內第i個粒子的局部最優路徑代價值與全局最優路徑代價值，與分別表示第i個粒子在當前第t代和第t+1代的位置坐標，即第i個粒子在當前第t代和第t+1代可行的無人機路徑點集合；

S43、根據更新后的全部N_P個粒子位置坐標重新計算每個粒子對應的無人機路徑代價值J'，與已保存的最小路徑代價值J′_min比對，以該N_P+1個路徑代價值中的最小值更新最小路徑代價值J′_min，一并存儲更新后的最小路徑代價值J′_min對應的路徑點集合WP'；判斷是否達到指定的迭代次數，結果為否，返回步驟S42；結果為是，執行步驟S44；

S44、迭代完成，將最后存儲的路徑點集合WP'作為無人機二級路徑的路徑點集合。