[發明專利]一種基于蟻群算法的四旋翼無人機的航跡規劃優化方法

在審

• 申請號: 201710940108.2
• 申請日: 2017-10-11
• 公開（公告）號: CN107806877A
• 公開（公告）日: 2018-03-16
• 發明（設計）人: 王粟;江鑫;朱飛;李庚;邱春輝
• 申請（專利權）人: 湖北工業大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 武漢科皓知識產權代理事務所(特殊普通合伙)42222
• 代理人: 魏波

1.一種基于蟻群算法的四旋翼無人機的航跡規劃優化方法，其特征在于，包括以下步驟：

步驟1：利用三維柵格圖法劃分規劃空間；

步驟2：繪制基于三維柵格圖的三維環境地圖；

步驟3：進行四旋翼無人機路徑規劃。

2.根據權利要求1所述的基于蟻群算法的四旋翼無人機的航跡規劃優化方法，其特征在于：步驟1中，以緯度值作為橫坐標，經度值作為縱坐標，以地面障礙物的高度作為豎坐標，垂直于海平面，劃分三維柵格單元；單元格內若有障礙物，則判定該單元格是不能通過的單元格。

3.根據權利要求1所述的基于蟻群算法的四旋翼無人機的航跡規劃優化方法，其特征在于，步驟3的具體實現包括以下子步驟：

步驟3.1：參數初始化設置，包括起始點的確定、主方向的選取、種群數的確定、迭代次數的選擇、航跡搜尋范圍選取、信息素初始化設置；

所述起始點的確定，假設四旋翼無人機S起始點所在的柵格的坐標值(X_start，Y_start，Z_start)，終止點所在柵格的坐標值(X_end，Y_end，Z_end)，三維柵格地圖原點坐標值為(X_Gridstart，Y_Gridstart，Z_Gridstart)，則四旋翼無人機S的放置位置(S_lat，S_lon，S_h)和其所在柵格的柵格坐標位置(X_start，Y_start，Z_start)的關系為：

Xstart=ceil(Slat-XGridstartXGrid)×XGrid+XGridstartYstart=ceil(Slon-YGridstartYGrid)×YGrid+YGridstartZstart=ceil(Sh-ZGridstartZGrid)×ZGrid+ZGridstart;]]>

其中：X_Grid表示X軸劃分的單位大小，Y_Grid表示Y軸劃分單位大小，Z_Grid表示Z軸劃分的單位大小，S_lat表示當前無人機放置的X軸的坐標，S_lon表示當前無人機放置的Y軸的坐標，S_h表示當前無人機放置的Z軸的坐標，ceil表示向正無窮方向取整；

所述主方向的選取，是選擇緯度方向和經度方向中柵格變化數最多的方向作為四旋翼無人機航跡規劃主方向；

所述種群數的確定，根據實際情況進行人為確定；

所述迭代次數的選擇，根據實際情況進行人為確定；

所述航跡搜尋范圍選取，假設選定X方向為主方向，沿X軸方向從X_start到X_end劃分成n＝|X_start－X_end|+1個平面，編號為Π₁，Π₂，Π₃，...，Π_n，則四旋翼無人機航跡就分成(n-1)段；假設四旋翼無人機運行至第i個平面Π_i上的一點(X_i，Y_i，Z_i)處，那么下一個運行的柵格就在Π_i+1上；下一個柵格坐標選擇的具體過程為：對于主方向X上直接以平面Π_i+1的橫坐標作為下一個節點的橫坐標，即新的X坐標值為X_i+1；對于Y方向和Z方向坐標值的選擇不是直接選擇的，而是在平面Π_i+1選擇沒有障礙物的柵格放入數組Allowed中，否則被舍棄；然后從中選擇一個柵格點作為下一個運行柵格；對部分柵格進行搜索以找到最優航跡；對于非主方向Y，從平面Π_i到平面Π_i+1，對于Y方向上以Y_i為中心從Y_i－bc_max到Y_i+bc_max范圍內的點都是可選擇作為Y_i+1點；同樣，對于Z方向上來說以Z_i為中心從Z_i－hc_max到Z_i+hc_max范圍內的點都是可選擇作為Z_i+1點；其中，bc_max表示在Y軸上搜索的半徑范圍，hc_max表示在Z軸上搜索的半徑范圍；

所述信息素初始化設置，把三維柵格圖的三維環境地圖中的所有柵格信息素值設置為固定值；

步驟3.2：航跡搜索；

在航跡搜索過程中，假設PopNum只螞蟻中的第k只螞蟻已運行至平面Π_i上的點(X_i，Y_i，Z_i)處，搜索在平面Π_i+1上以(X_i+1，Y_i，Z_i)為中心count＝(2×bc_max+1)×(2×hc_max+1)個點；將count個柵格中所有的可行柵格放入數組Allowed中，可行柵格是沒有障礙物的柵格；如果數組Allowed為空，那么將當前點(X_i，Y_i，Z_i)的Z_i坐標值加1，當前點坐標變為(X_i，Y_i，Z_i+1)，重新搜索平面上的可行柵格，直到數組Allowed不為空；從數組Allowed中按照輪盤賭法選出一個可行的柵格作為平面Π_i+1上的航跡節點；

步驟3.3：對平面Π_i上的節點進行局部信息素更新；

步驟3.4：重復執行步驟3.2和步驟3.3，直到到達平面Π_n－1，然后使用變主方向搜索策略和簡化航跡策略；

步驟3.5：按照適應度值函數計算每條航跡的適應度值，比較找出最小適應度值，而最小適應度對應的航跡即為當前最優航跡；

適應度值函數為：

W＝k₁c₁L+k₂c₂/d_min+(1-k₁-k₂)c₃H；

其中，W為適應度值，k₁、k₂為權值系數，c₁、c₂、c₃為比例系數，L為路程，d_min為路徑上的每一點離最危險點的距離，H為每一點離海平面的高度；

步驟3.6：進行全局信息素更新；

從現有的搜索到的路徑中選擇出最短的航跡，更新適應度值最小的航跡所經過的所有柵格的信息素值；

步驟3.7：將上述步驟3.2-步驟3.6過程迭代N次，找到迭代N次的最優航跡。