基于引力搜索算法的植保無人機三維路徑規劃方法

王 宇 陳海濤 李海川

(東北農業大學工程學院， 哈爾濱 150030)

0 引言

將無人機作為載體，用于重復耗時的植保作業，可有效降低勞作壓力與健康危害，而且植保無人機還具有作業高度低、藥劑漂移少、對作物穿透性強等優勢[1-3]。由于植保無人機藥劑箱容量與電量的限制，在大田植保作業過程中，無人機往往需要多次返航進行藥劑補充或電池更換，所以為了提高效率，須為其規劃合理的返航點。目前植保無人機的作業方式主要有遙控作業和自主作業兩種[4-5]。遙控作業通過飛手操縱完成，飛行路徑與返航點安排也多是依賴飛手的經驗。自主作業為無人機依靠其航點飛行與斷點續噴等功能對田地自主進行噴施作業，其飛行路徑須人為設置航點或田地邊界。僅依靠飛手現場操縱與觀察，并不能及時規劃出較為理想的作業路徑，同時，目前植保無人機的斷點續噴功能并不支持對返航點的規劃，這就會導致時間與能源的浪費。因此，為了使無人機的植保作業更加高效與節能，其路徑規劃已成為該方面相關研究中一個亟待解決的問題。

植保無人機的路徑規劃應與實際的農藝要求相結合，綜合考慮作業過程中藥劑補充與電池更換的問題，并對田地實現全覆蓋[6-11]。針對植保無人機返航點的規劃，徐博等[12]提出了一種以工作總耗能最小為目標的航跡規劃算法，實現了田地的全覆蓋路徑規劃與返航點位置的尋優。王宇等[13]提出了基于Grid-GSA算法的路徑規劃方法，能夠針對具有不規則邊界的田地區域規劃出合理的返航點，使得非植保作業時間最短。但上述這些方法均未考慮三維地形的影響。

無人機在進行植保作業時，應以距離作物一定的高度飛行，以保證良好的噴施效果，目前大多數植保無人機通過加裝超聲波、雙目、光流等傳感器，也已經具備了定高飛行的功能；同時，由于有的田地存在高度起伏的情況，此時若將田地視為平面進行計算，則會導致計算得出的距離比實際飛行距離短，從而造成規劃結果失真。所以對植保無人機進行路徑規劃時，應充分考慮飛行高度變化造成的附加飛行距離的影響。針對上述問題，本文運用引力搜索算法與三維柵格法，對一種可用于三維地形的植保無人機路徑規劃方法進行研究。

1 植保無人機三維路徑規劃方法

1.1 建立三維環境模型與路徑仿真算法

根據實際的田地地理信息，運用柵格法(Grid method)將三維環境柵格化處理，建立環境模型，如圖1所示[14]。柵格化后的三維環境模型，柵格邊長為作業幅寬，另外每個柵格還包含柵格中心處的三維坐標(xi,yi,zi)與移動權值ωi等信息。其中，(xi,yi,zi)根據第i個柵格在田地中的實際位置與高度設定；初始柵格的移動權值ωi設為1或0，ωi=1的柵格為待作業區域柵格，ωi=0的柵格為不可進入柵格，如田地邊界與田間障礙物等。

圖1 柵格化三維環境模型Fig.1 3D grid environment model

在初始的柵格化環境中，設定無人機的初始時刻t0的位置(X(t0),Y(t0),Z(t0))位于柵格i，則無人機的初始位置為

(X(t0),Y(t0),Z(t0))=(xi,yi,zi+h)

(1)

式中h——無人機距離地面的高度

按照如下原則對無人機周圍柵格的移動權值進行修正：

(1)若無人機前方柵格的移動權值不為0，則為該柵格附加移動權值獎勵U1。

(2)若無人機左后方柵格的移動權值不為0，則為該柵格附加移動權值獎勵U2，U2>U1。

(3)同步驟(2)，為無人機右后方柵格的移動權值附加獎勵。

(4)將無人機后方柵格的移動權值置0。

按照該原則對無人機周圍柵格的移動權值進行修正后，使無人機選擇移動權值最大的柵格進行移動，且限制無人機僅能夠向前、左、右3個方向移動，即可實現無人機往復全覆蓋路徑的仿真計算[8]。修正原則中，步驟(1)使無人機能夠直行；步驟(2)、(3)使無人機能夠在地頭處回轉；步驟(4)使無人機避免重復進入已作業區域。

如圖1所示，為無人機在地頭處回轉飛行路徑仿真。在t1時刻無人機由柵格5飛至柵格1，其前、后、左、右分別為柵格2、5、3、7。其前方柵格2為邊界柵格，則ω2=0；柵格5、6、7為已作業區域柵格，由步驟(4)可知，無人機駛過的已作業區域柵格移動權值均為0，則ω5=ω6=ω7=0；由步驟(1)～(3)可知，移動權值為0的柵格不再接受獎勵，故僅對無人機左后方柵格4進行移動權值修正，附加獎勵U2，則ω4=1+U2；對于柵格3，沒有相應的修正原則，所以其移動權值保持初始值不變，ω3=1。此時無人機可選擇的3個移動方向中，左側的柵格3移動權值最大，故在下一時刻t2無人機左轉飛至柵格3。在t2時刻，按照前述原則對無人機周圍柵格的移動權值進行修正，得出柵格8的移動權值ω8=1+U1<ω4，所以無人機下一時刻t3將左轉飛至柵格4，從而實現在地頭處回轉。

1.2 建立基于引力搜索算法的返航點尋優模型

在1.1節路徑仿真算法運算過程中，可設置無人機每次飛行距離的閾值D=(d1,d2, …,dN)，監測每次飛行的距離，當達到閾值時，記錄無人機當前的位置Pn=(xn,yn,zn)，n=1, 2, …,N。由此可知，可根據載藥量或電量設置飛行距離閾值D，從而得出返航點Pn，所以本文尋優模型以每次植保作業的飛行距離作為變量。

在同一柵格化的環境中，無人機植保作業時的總飛行距離不變，但由于返航點位置與數量的不同，會使得保障次數、返航點與補給點之間的總往返飛行距離也不同，所以要縮短總作業時間，則應減小總的往返飛行距離、藥劑補充與電池更換次數等，若無人機飛行方向不受作物行方向的限制，則還須使得地頭回轉次數最小化，綜上所述，建立目標函數為

(2)

其中

P0=(x0,y0,z0)

式中E——總作業時間，s

Du——地頭回轉總距離,m

vu——地頭回轉飛行速度,m/s

Ten——第n次保障作業時更換電池時間,s

Tpn——第n次保障作業時補充藥劑時間,s

P0——保障點的位置坐標

Hn——升降距離,m

voff——非植保作業狀態下的飛行速度,m/s

本文運用引力搜索算法(Gravitational search algorithm，GSA)對尋優模型進行求解，其屬于啟發式算法，該類算法已經被廣泛應用于諸多領域的路徑規劃問題并表現出較好的性能[15-20]。引力搜索算法的迭代規則為

(3)

(4)

(i=1,2,…,I;n=1,2,…,N)

式中ri——區間[0, 1]內的隨機數

由于尋優模型中變量D=(d1,d2, …,dN)，其維數N在一次尋優中不能變化，但維數N對應返航點的數量，所以還須對不同維數分別進行尋優計算。

2 試驗

試驗中，植保無人機固有參數與田地地理信息為輸入，返航點位置與數量為輸出，設植保無人機電池容量可支持的續航時間為25 min，載藥量所允許的最長植保作業時間為15 min，非植保作業狀態下的直線飛行速度為6 m/s，地頭的回轉飛行速度為3 m/s，植保作業飛行速度為3 m/s，作業幅寬為5 m，電池更換時間Ten為1 min，最大藥劑補充時間Tpmax為2 min，第n次藥劑補充時間為

(5)

式中tb——基礎藥劑補充時間，設tb=20 s

Dn+1——第n+1次植保作業距離

Dmax——載藥量所允許的最大植保作業距離

2.1 二維與三維區域返航點情況對比

建立如圖2所示的三維環境模型，平面尺寸為700 m×100 m，邊界形狀不規則，作物行方向固定且已知，其二維平面圖如圖3所示，由矩形區域去掉左上角邊長為100 m的等腰直角三角形，從而形成不規則邊界，圖2、3中坐標系均為非等比坐標系，保障點P0=(0, 0, 0)。

圖2 三維地形返航點情況Fig.2 Return points in field with 3D terrain

圖3 二維地形返航點情況Fig.3 Return points in field with 2D terrain

未規劃的返航點在三維與二維地形中的情況分別如圖2與圖3中的圓圈所示，具體數據如表1所示。

表1 二維與三維地形返航點情況對比Tab.1 Comparison of return points in fields with 2D and 3D terrains m

2.2 三維區域路徑規劃

建立與2.1節相同的三維環境模型，對返航點進行簡單規劃，即將返航點置于保障點所處的田地邊界上，其具體位置的選取應在載藥量所允許的范圍內，運用仿真方法得出的簡單規劃結果如圖4所示。

圖4 三維地形簡單規劃情況Fig.4 Simple path planning for field with 3D terrain

運用本文提出的基于GSA算法的三維路徑規劃方法得出的變量結果為

D=(2 402, 2 484, 2 562, 2 669)

最終輸出的實際植保作業距離為

D=(2 400.95,2 480.81,2 561.10,2 665.62,2 695.96)

具體的返航點數量與位置如圖5所示。

圖5 基于GSA算法的三維路徑規劃結果Fig.5 Path planning results based on GSA

2種返航點情況及對應的相關數據信息如表2所示。

2.3 實際應用測試

使用作者自行設計制造的無人機作為試驗機，無人機采用pixhawk開源飛行控制系統，具有圖傳與數傳模塊，采用GPS定位，使用氣壓計與超聲波進行定高，具備航點飛行與地形跟蹤功能，通過編程可模擬斷點續噴功能，試驗環境與試驗機如圖6所示。

表2 不同規劃方法計算的返航點情況對比Tab.2 Comparison of return points with different methods m

圖6 試驗機與試驗環境Fig.6 UAV and environment for experiment

選取田地地形如圖7所示(位于黑龍江省哈爾濱市賓縣宣陽村，2017年5月11日)，田地具有不規則的邊界，高度起伏2.9 m，無人機飛行高度1 m，等高線間距為0.5 m。

圖7 試驗田地Fig.7 Field for experiment

通過輸入田地的實際地形數據信息，即可得出規劃后的返航點，如圖8所示。

圖8 試驗田地規劃后的返航點Fig.8 Optimal return points for experimental field

將計算得出的返航點輸入地面站，使無人機按照設定航點飛行，根據數據傳輸模塊傳回的數據，得到規劃前后的往返距離總和分別為16 387 m與14 578 m，非植保作業時間分別為85 min與81 min，規劃后的各項數據情況如表3所示。

3 結果分析與討論

對2.1節算例中的二維與三維田地進行路徑規劃時，生成的對應返航點之間存在偏差距離，這主要是由三維地形中額外的高度起伏對距離的影響造成的，表1中相對應的返航點之間的偏差距離為5 m，該距離的精度也與柵格劃分的精細程度有關，本算例中由于柵格邊長5 m，所以偏差距離的范圍為0～10 m；在實際田地規劃中，偏差距離范圍為15～25 m。由于高度的起伏使得無人機的飛行距離增加，所以若不考慮三維地形的影響，就會造成藥劑已經耗盡，而無人機仍繼續作業的現象，從而導致藥劑漏失，說明了本文研究的必要性。

表3 試驗田地路徑規劃數據Tab.3 Data of path planning result for experimental field m

在2.2節算例中，分別運用本文提出的基于GSA算法的路徑規劃方法與簡單規劃方法對返航點進行了規劃，對比情況如表4所示。

表4 路徑規劃結果數據對比情況Tab.4 Data comparison of path planning results

運用本文提出的基于GSA算法的路徑規劃方法規劃后的結果中，往返飛行距離總和與非植保作業時間分別為440.2 m與9 min，相比于未規劃情況分別減少了90%與54%，相比于簡單規劃結果分別減少了23%與7%。

3種方法得出的結果中，電池更換次數均為4次，藥劑補充次數分別為4、5、4次。未規劃情況下的保障作業時間最短，但由于往返距離較遠，使得非植保作業時間最長?；贕SA算法的規劃結果與簡單規劃結果相比較，往返飛行時間與保障作業時間均較短，2種方法的結果中，藥劑補充平均時間分別為116 s與97 s，雖然后者優于前者，但由于藥劑補充次數較多，所以增長了總的藥劑補充時間。

根據續航時間與實際飛行時間計算并輸出每組電池的電量消耗，如表5所示。3種方法的結果中，電池用量分別為5、6、5組。

表5 電池使用情況Tab.5 Consumption of battery power %

在2.3節方法的實際運用過程中，規劃前后情況對比，往返距離減少了11%，非植保作業時間減少了5%。由于目標函數中計算了垂直方向上的飛行距離，使得無人機往返路徑盡量少的跨越較高的田地區域，具體規劃前后的返航點分布與地形情況分別如圖9、10所示。圖9未規劃的返航點中，有8個返航點位置，使得無人機在往返途中需要跨越較高區域，從而增加了垂直移動距離；而圖10規劃后的結果中，僅有5個返航點存在該情況。

圖9 試驗田地未規劃的返航點Fig.9 Unplanned return points in experimental field

圖10 試驗田地規劃后的返航點Fig.10 Optimal return points for experimental field

通過對算例與實際田地進行實際應用，對結果進行分析發現，方法能夠適用于復雜情況的地理地形要求，搭建的模型能夠按照目標函數進行尋優，考慮因素包含了保障作業時間、往返飛行的平面距離與垂直距離。

4 結論

(1)針對植保無人機的植保作業過程，提出了基于GSA算法的三維路徑規劃方法。通過對柵格化的環境模型附加高度信息，使得仿真算法中無人機的距離計算包含高度起伏變化引起的距離增加；建立的目標函數中也考慮了返航點與保障點之間地形的高度起伏引起的無人機起降距離的變化，從而增強了計算結果的可靠性。

(2)在實例檢驗中，運用本文提出方法得出的結果，相比于二維空間內的路徑規劃情況，返航點的位置存在偏差距離，說明了路徑規劃過程中考慮高度起伏的必要性。運用基于GSA算法的路徑規劃方法規劃后，往返飛行距離總和與非植保作業時間分別為440.2 m與9 min，相比于未規劃情況分別減少了90%與54%，相比于簡單規劃結果分別減少了23%與7%。在實際田地的路徑規劃應用中，田地形狀與高度起伏情況更為復雜，本文提出方法不僅能夠計算三維空間中返航點與保障點之間的距離，而且還綜合考慮了返航途中由于高度起伏增加的距離，從而驗證了該方法的合理性與可行性。

1 HUANG L, QU H, JI P, et al. A novel coordinated path planning method using k-degree smoothing for multi-UAVs[J]. Applied Soft Computing, 2016, 48: 182-192.

2 TISDALE J, KIM Z W, HEDRICK J K. Autonomous UAV path planning and estimation[J]. IEEE Robotics & Automation Magazine, 2009, 16(2): 35-42.

3 張東彥, 蘭玉彬, 陳立平, 等. 中國農業航空施藥技術研究進展與展望[J/OL]. 農業機械學報, 2014, 45(10): 53-59.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20141009&flag=1&journal_id=jcsam.DOI:10.6041/j.issn.1000-1298.2014.10.009.

ZHANG Dongyan, LAN Yubin, CHEN Liping, et al. Current status and future trends of agricultural aerial spraying technology in China[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(10): 53-39.(in Chinese)

4 丁團結, 方威, 王鋒. 無人機遙控駕駛關鍵技術研究與飛行品質分析[J]. 飛行力學, 2011, 29(2): 17-24.

DING Tuanjie, FANG Wei, WANG Feng. Development of UAV remote-piloted key technology and flight qualities[J]. Flight Dynamics, 2011, 29(2): 17-24.(in Chinese)

5 彭孝東, 張鐵民, 李繼宇, 等. 基于目視遙控的無人機直線飛行與航線作業試驗[J/OL]. 農業機械學報, 2014, 45(11): 258-263. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20141140&flag=1&journal_id=jcsam. DOI: 10.6041/j.issn.1000-1298.2014.11.040.

PENG Xiaodong, ZHANG Tiemin, LI Jiyu, et al. Experiment of straight and airline flight operation for farmland based on UAV in visual remote mode[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(11): 258-263.(in Chinese)

6 YAO P, WANG H, SU Z. Cooperative path planning with applications to target tracking and obstacle avoidance for multi-UAVs[J]. Aerospace Science & Technology, 2016, 54: 10-22.

7 RAMANA M V, VARMA S A, KOTHARI M. Motion planning for a fixed-wing UAV in urban environments[J]. IFAC-Papers Online, 2016, 49(1): 419-424.

8 POPESCU D, STOICAN F, ICHIM L. Control and optimization of UAV trajectory for aerial coverage in photogrammetry applications[J]. Advances in Electrical and Computer Engineering, 2016, 16(3):99-106.

9 TORRES M, PELTA D A, VERDEGAY J L, et al. Coverage path planning with unmanned aerial vehicles for 3D terrain reconstruction[J]. Expert Systems with Applications, 2016, 55:441-451.

11 HAMEED I A, COUR-HARBO A L, OSEN O L. Side-to-side 3D coverage path planning approach for agricultural robots to minimize skip/overlap areas between swaths[J]. Robotics & Autonomous Systems, 2016, 76:36-45.

12 徐博, 陳立平, 譚彧, 等. 多架次作業植保無人機最小能耗航跡規劃算法研究[J/OL]. 農業機械學報, 2015, 46(11): 36-42.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20151106&flag=1&journal_id=jcsam.DOI: 10.6041/j.issn.1000-1298.2015.11.006.

XU Bo, CHEN Liping, TAN Yu, et al. Path planning based on minimum energy consumption for plant protection UAVs in sorties[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2015, 46(11): 36-42.(in Chinese)

13 王宇, 陳海濤, 李煜, 等. 基于Grid-GSA算法的植保無人機路徑規劃方法[J/OL]. 農業機械學報, 2017, 48(7):29-37.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20170704&flag=1.DOI: 10.6041/j.issn.1000-1298.2017.07.004.

WANG Yu, CHEN Haitao, LI Yu, et al. A path planning method based on Grid-GSA for plant protection UAV[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2017, 48(7):29-37.(in Chinese)

14 GU J, CAO Q. Path planning for mobile robot in a 2.5 dimensional grid based map[J]. Industrial Robot, 2011, 38(3): 315-321.

15 王宇, 黃勝, 廖全蜜, 等. 基于引力搜索算法的船舶艙室布置方法[J]. 上海交通大學學報, 2016, 50(1): 131-139.

WANG Yu, HUANG Sheng, LIAO Quanmi, et al. A method for ship compartment layout design based on gravitational search algorithm[J]. Journal of Shanghai Jiao Tong University, 2016, 50(1): 131-139.(in Chinese)

16 王宇, 黃勝, 廖全蜜, 等. 基于引力搜索算法的艙室分布方案設計方法[J]. 哈爾濱工程大學學報,2016, 37(1): 48-52.

WANG Yu, HUANG Sheng, LIAO Quanmi, et al. Method for the layout design of ship cabins based on gravitational search algorithm[J]. Journal of Harbin Engineering University, 2016, 37(1): 48-52.(in Chinese)

17 MAC T T, COPOT C, TRAN D T, et al. Heuristic approaches in robot path planning: a survey[J]. Robotics & Autonomous Systems, 2016, 86: 13-28.

18 DAS P K, BEHERA H S, DAS S, et al. A hybrid improved PSO-DV algorithm for multi-robot path planning in a clutter environment[J]. Neurocomputing, 2016, 207: 735-753.

19 BEHNCK L P, DOERING D, PEREIRA C E, et al. A modified simulated annealing algorithm for SUAVs path planning[J]. IFAC-Papers Online, 2015, 48(10): 63-68.

20 RASHEDI E, NEZAMABADI-POUR H, SARYAZDI S. GSA: a gravitational search algorithm[J]. Information Sciences, 2009, 179(13): 2232-2248.