全自動植保無人機含障不規則區域的航跡規劃算法

金澤選， 吳開華， 王　朔， 王文杰， 孫學超

(杭州電子科技大學生命信息與儀器工程學院，浙江杭州 310018)

近年來，國內外對植保無人機(簡稱植保機)已經開展廣泛研究，具有自主作業功能的全自動植保機[1]越來越受到研究者的重視。我國部分農田是比較零散、形狀不規則的，其中有些地塊中存在障礙物。為實現植保機的全自動作業，需要研究覆蓋航跡規劃技術[2]，覆蓋航跡規劃可以避開障礙物，規劃出一條能夠遍歷某一區域的最優飛行路徑。目前對于存在障礙物的不規則農田航跡規劃問題的研究相對較少，因此迫切需要解決植保機對于含障不規則區域的航跡規劃問題，以大幅度增加全自動植保機可作業的地塊。

對于植保機區域覆蓋的航跡規劃問題，目前的主要方法是采用“Z”字形掃描線方式，即植保機沿直線飛行，在區域邊界處轉向，然后朝反方向沿平行直線飛行，如此反復，逐行遍歷待掃描區域。這種方法較適用于規則的矩形作業區域，但對于不規則形狀的多邊形作業區域來說，無論沿何種方向進行作業，作業航線無法保證與所有的邊界都平行，因此均會出現多余覆蓋的現象，須對“Z”字形掃描算法進行優化，使其滿足不規則區域航跡規劃的要求。

針對植保機的避障問題，目前主要有2種解決方法，第一種方法是通過在無人機平臺增加避障傳感器，根據檢測到的障礙物信息進行實時避障，張躍東等研究了一種基于單目視覺的障礙物深度提取算法，可以有效發現并區分不同深度的障礙物[3]；程虹霞等通過對超聲波及紅外傳感器的數據融合，研究了多傳感器信息融合技術在無人機平臺自主避障行為中的應用[4]；王和平等通過分析紅外成像和毫米波雷達測距技術，研發了基于紅外與雷達一體化吊艙的無人機避障技術[5]。另一種方法是事先將障礙物信息標記出來，并在航跡規劃時將其規避。張遜遜等提出一種基于改進人工勢場的避障控制方法[6]，將障礙物劃分為低矮型和高桿型，并制定不同的避障策略。將無人機與障礙物的相對運動速度引入人工勢場中，給出基于改進人工勢場的避障控制算法，無論從避障路徑還是避障時間上看均優于傳統人工勢場避障。關震宇等研究了一種基于Dubins路徑的無人機避障規劃算法，通過采用遺傳算法，結合無人機的飛行性能和最小轉彎半徑，在已知障礙物空間位置前提下實現無人機避障，并將其推廣到多障礙環境下，使之能解決飛行器多障礙避障航跡規劃問題[7]。

在無人機平臺增加傳感器來進行避障的方法主要適用于不確定的或未知環境的區域。對于地理信息已知的農田區域可通過將邊界信息和障礙物信息事先標記出來的方式進行避障。文獻[6]將障礙物分為低矮型和高桿型，分別采用爬升和繞飛的方式進行避障，但是沒有涉及障礙物的大小。文獻[7]雖然是事先標記障礙物進行航跡規劃，但主要研究從起始點到目標點避開所有障礙物的最優飛行路線問題，沒有涉及區域的全覆蓋問題。本研究以植保機含障不規則區域覆蓋航跡規劃問題為研究背景，首先對“Z”字形掃描進行優化，在掃描線方式的基礎上，通過柵格法[8]對工作區域進行劃分，使該算法滿足對不規則區域的全覆蓋。然后根據障礙物的大小進行分類，將其分為點狀障礙物(如樹、電線桿等)和區域障礙物(如房屋、樹林等)，對不同的障礙物設計不同的避障算法，在優化全局航跡規劃的基礎上通過插入航點的方式進行局部規劃，從而達到繞過障礙物繼續飛行的目的。

1　含障不規則區域航跡規劃算法研究

1.1　不規則區域全局航跡規劃算法

對于不含障礙物的不規則區域，需要研究一種航跡規劃算法，使其可以根據任意的作業方向，或者在未指定作業方向的情況下，給出某一推薦的作業方向和航線，盡可能地減小多余覆蓋面積和總飛行距離，解決不規則區域的全覆蓋問題。由于在植保機噴藥作業過程中，各條作業航線是相互平行的，相鄰平行航線的距離均為植保機的噴藥寬度W，采用不規則區域航跡規劃算法，可快速得到植保機的作業航線。圖1為植保機作業航跡規劃示意。

如圖1所示，植保機對不規則區域全局航跡規劃算法如下：(1)計算航跡規劃的起始點和起始邊。定義實際作業中飛機起飛時所在的位置為起飛點，根據邊界多邊形各頂點距飛機起飛點的距離計算起始點。選擇距飛機起飛點最近的頂點作為起始點。在飛機的飛行過程中，為盡量減少轉彎次數，從而提高作業效率，應使飛機沿地塊較長的邊飛行，因此本研究選擇經過起始點2條邊界線中較長的邊界線作為起始邊。假設起始點為A1(x1，y1)，起始邊為LA1A2，可求得起始邊的表達式為(y-y2)(x1-x2)=(y1-y2)(x-x2)。

(2)計算距起始邊最遠的點。計算邊界多邊形除頂點A1、A2外其余各頂點到起始邊的距離，假設其余各頂點到起始邊的距離分別為d1，d2，…，dm。獲得距離的最大值dmax=max[d1，d2，…，dm]。圖1中A4(x4，y4)到起始邊A1A2的距離最大。過點A4作起始邊的平行線即為最遠邊LA4-A1A2，最遠邊的表達式為(y-y4)(x2-x1)=(x-x4)(y2-y1)。

(3)計算區域內各條航線。將最遠邊和起始邊之間的距離按噴幅W進行分割，分得的每塊小區域為植保機作業的寬度。進而可求出區域內的所有航線。所有航線均平行，除了最后一條航線，每條航線的距離都是定值W。通過計算可求得分割的航線數，以及第i條航線Li方程的斜率和截距：

dis_cut=(int)dmax/W；

(1)

kLi=kL(A1A2)；

(2)

bLi=(bL(A4-A1A2)-bL(A1A2))×i/dis_cut+bL(A1A2)。

(3)

式中：dis_cut為分割的條數；kL(A1A2)為起始邊LA1A2的斜率；kLi為第i條航線的斜率；bL(A4-A1A2)為最遠邊LA4-A1A2的截距；bL(A1A2)為起始邊LA1A2的截距；bLi為第i條航線的截距。

由方程(2)(3)得航線Li的表達式為yi=kLi×xi+bLi。

(4)計算每條航線與邊界線的交點。每條航線都會和邊界線產生2個交點如D1、D2。這些交點就是植保機航跡規劃出的航點。

(5)對航點進行“Z”字形掃描。對已經求出的全部航點，通過“Z”字形排序，確定航點先后順序。

通過以上過程可將區域內全部航點求出，然后對航點排序得到上傳植保機的航跡。該算法實現了對不規則區域的全覆蓋航跡規劃，是研究含障區域航跡規劃的基礎。

1.2　含障區域局部航跡規劃算法

對于不含障礙物的區域，植保機根據全局航跡規劃算法，生成飛行航線，實現區域的全覆蓋。而對于含障礙物區域，障礙物僅可能影響其中某一條或幾條航線，其余航線不受障礙物影響，那些被障礙物影響的航線，可通過插入航點的方式進行避障。

1.2.1點狀障礙物局部航跡規劃算法點狀障礙物的特點是面積較小，實際影響的航線條數有限，僅能影響其中一條或幾條航線，對點狀障礙物影響的航線可以通過插入4個航點的方式避過障礙物。如圖2所示，設障礙物Z點坐標為(xZ，yZ)，受影響的航線為LMN，過Z點向航線LMN作垂線，交點為Z′(xZ′，yZ′)，其中航線上2個航點M、N的坐標都是已知的，分別為M(xM，yM)、N(xN，yN)。現需要插入避障點A、B、C、D，使植保機沿航線LMN飛行時，當飛到障礙物Z點附近時，沿A—B—C—D路徑飛行，避過障礙物后，再回到航線LMN上。

障礙點與避障線的距離分別如下：點Z到直線AB的距離為d1；點Z到直線BC的距離為d2；點Z到直線CD的距離為d3；長度d1、d2、d3需要通過對飛機的飛行速度、減速所需時間及轉彎的機動性等因素的綜合分析來確定。

根據向量運算可求得4個避障點的坐標如下：

A[(xM×d1-xZ′×d1+xZ′×dZ′M)/dZ′M，(yM×d1-yZ′×d1+yZ′×dZ′M)/dZ′M]；

(4)

B{[(xZ′-xZ)×(d2-dZZ′)+xA×dZZ′]/dZZ′，[(yZ′-yZ)×(d2-dZZ′)+yA×dZZ′]/dZZ′}；

(5)

C{[(xZ′-xZ)×(d2-dZZ′)+xD×dZZ′]/dZZ′，[(yZ′-yZ)×(d2-dZZ′)+yD×dZZ′]/dZZ′}；

(6)

D{[(xZ′-xA)×d3+xZ′×d1]/d1，[(yZ′-yA)×d3+yZ′×d1]/d1}。

(7)

式中：dZ′M為線段Z′M的長度；dZZ′為線段ZZ′的長度。

通過上面的算法可求出4個避障點的坐標。將4個避障點按A、B、C、D的順序插入原航跡中，可實現含點狀障礙物區域的完整繞飛功能。

令人不解的是，那支騎兵，還有騎兵后面的馬車，此刻卻沒有動靜。老太醫滿腹狐疑，正要與秦鐵崖談及此事，馬蹄聲碎，煙塵騰起，騎兵已分為兩支縱隊，護衛著馬車走來。馬車停下，轎簾掀起，下來一位官員。

1.2.2區域障礙物局部航跡規劃算法區域障礙物的特點是面積比較大，實際影響的航線比較多，不能像點狀障礙物那樣通過插入4個航點的方式進行避障。為解決含區域障礙物的避障問題，需要改變插入航點的策略，通過將區域障礙物邊界向外平移的方式進行避障。

圖3為區域障礙物避障示意圖。設區域障礙物為多邊形H1I1G1K1L1(簡稱H1)，將該多邊形每條邊均向外平移距離d得到多邊形H2I2G2K2L2(簡稱H2)，要求距離d必須保證飛機沿多邊形H2邊界飛行時不會碰到障礙物。

植保機對區域障礙物局部航跡規劃算法如下：(1)采集區域障礙物多邊形H1的坐標。得到多邊形H1中各點的坐標分別為H1(xH1，yH1)、I1(xI1，yI1)、G1(xG1，yG1)、K1(xK1，yK1)、L1(xL1，yL1)。(2)計算平移后多邊形H2的坐標。得到平移后的多邊形H2中各點的坐標分別為H2(xH2，yH2)、I2(xI2，yI2)、G2(xG2，yG2)、K2(xK2，yK2)、L2(xL2，yL2) 。(3)確定多邊形H2影響原有航線的條數。通過對不規則區域的全局航跡規劃可計算所有的飛行航線，計算每條航線與多邊形H2存在交點的數量。若航線與多邊形H2沒有交點，說明這條航線不受障礙物的影響；若航線與多邊形H2有1個交點，說明這條航線恰好通過多邊形H2的某個頂點，由于飛行沿多邊形H2飛行時不會碰到障礙物，所以這條航線也不受障礙物的影響；若航線與多邊形A2B2C2D2E2有2個交點，說明植保機沿該航線飛行時，需要穿過障礙物區域，必須采集避障動作，使植保機可以繞過障礙物繼續飛行。

現以航線LMN為例，多邊形H2航線LMN存在2個交點，分別為P、Q。通過幾何知識可求得直線LMN、直線LL2H2、直線LL2K2的方程分別如下：

(y-yN)(xM-xN)=(yM-yN)(x-xN)；

(8)

(y-yH2)(xL2-xH2)=(yL2-yH2)(x-xH2)；

(9)

(y-yL2)(xK2-xL2)=(yK2-yL2)(x-xL2)。

(10)

聯立方程(8)(9)(10)可求出交點P、Q的坐標分別為(xp，yp)、(xQ，yQ)。

(4)確定插入原有航跡的航點，使植保機沿多邊形H2邊界飛行以達到避障的效果。植保機要在航線PQ段進行避障，有2條避障路線，分別為M—P—L2—Q—N和M—P—H2—I2—G2—K2—Q—N。選擇插入航點數量較少的路線作為實際避障路線，在2條路線插入航點數量一致的情況下，選擇避障路線距離較短的路線作為避障路線。因此在航線LMN中需要插入的航點為點P、L2、Q。

(5)重復(3)和(4)的操作，將被區域障礙物影響的航線全部找到，并通過插入航點的方式進行避障。

以上過程可求出穿過區域障礙物的所有航線，對穿過區域障礙物的航線，使其沿平移后的多邊形邊界飛行，可解決存在區域障礙物地塊的全覆蓋航跡規劃問題。

2　仿真和田間試驗

2.1　仿真

通過APM SITL仿真模擬器可以在沒有任何硬件配合的情況下在室內方便地搭建出一個運行在電腦上的完全模擬真實飛行的仿真環境。因此可以利用APM SITL仿真模擬器對含障不規則區域航跡規劃算法進行驗證。

2.2　田間試驗

在實際作業中，為滿足植保機的全自動飛行，要求飛行精度偏差控制在0.5 m以內，而且在避障過程中不能撞上障礙物。為了驗證含障不規則區域航跡規劃算法的實際應用效果，需要進行田間試驗計算實際飛行航跡和理論規劃航跡的最大偏差。試驗在1塊包含點狀障礙物和區域障礙物的田地進行，通過手持GPS測得田地邊界點的坐標、點狀障礙物坐標以及區域障礙物邊界坐標。根據上文所述航跡規劃算法求出規劃好的航跡點，上傳飛控，記錄植保機的飛行情況。

進行10組飛行試驗，每組飛行10次，通過機載GPS實時記錄植保機的飛行航跡，并實時下傳到地面站。圖6為作業地塊示意，其中矩形區域為實際作業的區域，圖7為實際飛行航跡圖，其中直線為上傳飛控的規劃航跡，曲線為植保機飛行的實際航跡。

從圖7可以看出， 植保機在飛行時的實際航跡與上傳的規劃航跡是有一定偏差的，且拐彎處和避障處的偏差要明顯大于沿航線直線飛行的偏差。記錄第1組每次飛行試驗的規劃航跡和實際飛行航跡的最大偏差，結果見表1。

表1　規劃航跡和實際飛行航跡的最大偏差

從表1可以看出，在共計10次飛行中，雖然規劃航跡和實際飛行航跡均存在偏差，但每次飛行的最大偏差均小于 0.4 m，而且很好地規避了障礙物，滿足了植保機全自動作業的要求。

3　結論

本研究對植保機在實際作業中對區域的覆蓋航跡規劃問題進行了研究，重點研究并完成了含障不規則區域的航跡規劃算法設計，該算法將全局規劃和局部規劃相結合，通過APM SITL仿真試驗，從理論上證明了算法的可行性。在所進行的田間試驗中，理論航跡和實際飛行航跡最大偏差均小于0.4 m，而且很好地規避了障礙物，滿足實際作業要求，可為無人機全自動飛行提供參考。

參考文獻：

[1]蔣智超，劉朝宇. 淺談植保無人機發展現狀及趨勢[J]. 新疆農機化，2016(2)：30-31，42.

[2]陳海，王新民，焦裕松，等. 一種凸多邊形區域的無人機覆蓋航跡規劃算法[J]. 航空學報，2010(9)：1802-1808.

[3]張躍東，李麗，劉曉波，等. 基于單目視覺的無人機障礙探測算法研究[J]. 激光與紅外，2009，39(6)：673-676.

[4]程虹霞，駱云志，朱松柏，等. 多傳感器信息融合技術在無人機平臺避障中的應用[J]. 兵工自動化，2010，29(6)：89-91.

[5]王和平，汪俊，申爭光. 基于紅外與雷達一體化吊艙的直升機安全避障技術[J]. 科技創新導報，2014(29)：56-59.

[6]張遜遜，許宏科，朱旭. 低空低速植保無人直升機避障控制系統設計[J]. 農業工程學報，2016，32(2)：43-50.

[7]關震宇，楊東曉，李杰，等. 基于Dubins路徑的無人機避障規劃算法[J]. 北京理工大學學報，2014，34(6)：570-575.

[8]徐博，陳立平，譚彧，等. 基于無人機航向的不規則區域作業航線規劃算法與驗證[J]. 農業工程學報，2015，31(23)：173-178.