全自動(dòng)植保無(wú)人機(jī)含障不規(guī)則區(qū)域的航跡規(guī)劃算法

金澤選， 吳開(kāi)華， 王　朔， 王文杰， 孫學(xué)超

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

近年來(lái)，國(guó)內(nèi)外對(duì)植保無(wú)人機(jī)(簡(jiǎn)稱(chēng)植保機(jī))已經(jīng)開(kāi)展廣泛研究，具有自主作業(yè)功能的全自動(dòng)植保機(jī)[1]越來(lái)越受到研究者的重視。我國(guó)部分農(nóng)田是比較零散、形狀不規(guī)則的，其中有些地塊中存在障礙物。為實(shí)現(xiàn)植保機(jī)的全自動(dòng)作業(yè)，需要研究覆蓋航跡規(guī)劃技術(shù)[2]，覆蓋航跡規(guī)劃可以避開(kāi)障礙物，規(guī)劃出一條能夠遍歷某一區(qū)域的最優(yōu)飛行路徑。目前對(duì)于存在障礙物的不規(guī)則農(nóng)田航跡規(guī)劃問(wèn)題的研究相對(duì)較少，因此迫切需要解決植保機(jī)對(duì)于含障不規(guī)則區(qū)域的航跡規(guī)劃問(wèn)題，以大幅度增加全自動(dòng)植保機(jī)可作業(yè)的地塊。

對(duì)于植保機(jī)區(qū)域覆蓋的航跡規(guī)劃問(wèn)題，目前的主要方法是采用“Z”字形掃描線方式，即植保機(jī)沿直線飛行，在區(qū)域邊界處轉(zhuǎn)向，然后朝反方向沿平行直線飛行，如此反復(fù)，逐行遍歷待掃描區(qū)域。這種方法較適用于規(guī)則的矩形作業(yè)區(qū)域，但對(duì)于不規(guī)則形狀的多邊形作業(yè)區(qū)域來(lái)說(shuō)，無(wú)論沿何種方向進(jìn)行作業(yè)，作業(yè)航線無(wú)法保證與所有的邊界都平行，因此均會(huì)出現(xiàn)多余覆蓋的現(xiàn)象，須對(duì)“Z”字形掃描算法進(jìn)行優(yōu)化，使其滿足不規(guī)則區(qū)域航跡規(guī)劃的要求。

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

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

1　含障不規(guī)則區(qū)域航跡規(guī)劃算法研究

1.1　不規(guī)則區(qū)域全局航跡規(guī)劃算法

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

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

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

(3)計(jì)算區(qū)域內(nèi)各條航線。將最遠(yuǎn)邊和起始邊之間的距離按噴幅W進(jìn)行分割，分得的每塊小區(qū)域?yàn)橹脖C(jī)作業(yè)的寬度。進(jìn)而可求出區(qū)域內(nèi)的所有航線。所有航線均平行，除了最后一條航線，每條航線的距離都是定值W。通過(guò)計(jì)算可求得分割的航線數(shù)，以及第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為分割的條數(shù)；kL(A1A2)為起始邊LA1A2的斜率；kLi為第i條航線的斜率；bL(A4-A1A2)為最遠(yuǎn)邊LA4-A1A2的截距；bL(A1A2)為起始邊LA1A2的截距；bLi為第i條航線的截距。

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

(4)計(jì)算每條航線與邊界線的交點(diǎn)。每條航線都會(huì)和邊界線產(chǎn)生2個(gè)交點(diǎn)如D1、D2。這些交點(diǎn)就是植保機(jī)航跡規(guī)劃出的航點(diǎn)。

(5)對(duì)航點(diǎn)進(jìn)行“Z”字形掃描。對(duì)已經(jīng)求出的全部航點(diǎn)，通過(guò)“Z”字形排序，確定航點(diǎn)先后順序。

通過(guò)以上過(guò)程可將區(qū)域內(nèi)全部航點(diǎn)求出，然后對(duì)航點(diǎn)排序得到上傳植保機(jī)的航跡。該算法實(shí)現(xiàn)了對(duì)不規(guī)則區(qū)域的全覆蓋航跡規(guī)劃，是研究含障區(qū)域航跡規(guī)劃的基礎(chǔ)。

1.2　含障區(qū)域局部航跡規(guī)劃算法

對(duì)于不含障礙物的區(qū)域，植保機(jī)根據(jù)全局航跡規(guī)劃算法，生成飛行航線，實(shí)現(xiàn)區(qū)域的全覆蓋。而對(duì)于含障礙物區(qū)域，障礙物僅可能影響其中某一條或幾條航線，其余航線不受障礙物影響，那些被障礙物影響的航線，可通過(guò)插入航點(diǎn)的方式進(jìn)行避障。

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

障礙點(diǎn)與避障線的距離分別如下：點(diǎn)Z到直線AB的距離為d1；點(diǎn)Z到直線BC的距離為d2；點(diǎn)Z到直線CD的距離為d3；長(zhǎng)度d1、d2、d3需要通過(guò)對(duì)飛機(jī)的飛行速度、減速所需時(shí)間及轉(zhuǎn)彎的機(jī)動(dòng)性等因素的綜合分析來(lái)確定。

根據(jù)向量運(yùn)算可求得4個(gè)避障點(diǎn)的坐標(biāo)如下：

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的長(zhǎng)度；dZZ′為線段ZZ′的長(zhǎng)度。

通過(guò)上面的算法可求出4個(gè)避障點(diǎn)的坐標(biāo)。將4個(gè)避障點(diǎn)按A、B、C、D的順序插入原航跡中，可實(shí)現(xiàn)含點(diǎn)狀障礙物區(qū)域的完整繞飛功能。

令人不解的是，那支騎兵，還有騎兵后面的馬車(chē)，此刻卻沒(méi)有動(dòng)靜。老太醫(yī)滿腹狐疑，正要與秦鐵崖談及此事，馬蹄聲碎，煙塵騰起，騎兵已分為兩支縱隊(duì)，護(hù)衛(wèi)著馬車(chē)走來(lái)。馬車(chē)停下，轎簾掀起，下來(lái)一位官員。

1.2.2區(qū)域障礙物局部航跡規(guī)劃算法區(qū)域障礙物的特點(diǎn)是面積比較大，實(shí)際影響的航線比較多，不能像點(diǎn)狀障礙物那樣通過(guò)插入4個(gè)航點(diǎn)的方式進(jìn)行避障。為解決含區(qū)域障礙物的避障問(wèn)題，需要改變插入航點(diǎn)的策略，通過(guò)將區(qū)域障礙物邊界向外平移的方式進(jìn)行避障。

圖3為區(qū)域障礙物避障示意圖。設(shè)區(qū)域障礙物為多邊形H1I1G1K1L1(簡(jiǎn)稱(chēng)H1)，將該多邊形每條邊均向外平移距離d得到多邊形H2I2G2K2L2(簡(jiǎn)稱(chēng)H2)，要求距離d必須保證飛機(jī)沿多邊形H2邊界飛行時(shí)不會(huì)碰到障礙物。

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

現(xiàn)以航線LMN為例，多邊形H2航線LMN存在2個(gè)交點(diǎn)，分別為P、Q。通過(guò)幾何知識(shí)可求得直線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)

聯(lián)立方程(8)(9)(10)可求出交點(diǎn)P、Q的坐標(biāo)分別為(xp，yp)、(xQ，yQ)。

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

(5)重復(fù)(3)和(4)的操作，將被區(qū)域障礙物影響的航線全部找到，并通過(guò)插入航點(diǎn)的方式進(jìn)行避障。

以上過(guò)程可求出穿過(guò)區(qū)域障礙物的所有航線，對(duì)穿過(guò)區(qū)域障礙物的航線，使其沿平移后的多邊形邊界飛行，可解決存在區(qū)域障礙物地塊的全覆蓋航跡規(guī)劃問(wèn)題。

2　仿真和田間試驗(yàn)

2.1　仿真

通過(guò)APM SITL仿真模擬器可以在沒(méi)有任何硬件配合的情況下在室內(nèi)方便地搭建出一個(gè)運(yùn)行在電腦上的完全模擬真實(shí)飛行的仿真環(huán)境。因此可以利用APM SITL仿真模擬器對(duì)含障不規(guī)則區(qū)域航跡規(guī)劃算法進(jìn)行驗(yàn)證。

2.2　田間試驗(yàn)

在實(shí)際作業(yè)中，為滿足植保機(jī)的全自動(dòng)飛行，要求飛行精度偏差控制在0.5 m以?xún)?nèi)，而且在避障過(guò)程中不能撞上障礙物。為了驗(yàn)證含障不規(guī)則區(qū)域航跡規(guī)劃算法的實(shí)際應(yīng)用效果，需要進(jìn)行田間試驗(yàn)計(jì)算實(shí)際飛行航跡和理論規(guī)劃航跡的最大偏差。試驗(yàn)在1塊包含點(diǎn)狀障礙物和區(qū)域障礙物的田地進(jìn)行，通過(guò)手持GPS測(cè)得田地邊界點(diǎn)的坐標(biāo)、點(diǎn)狀障礙物坐標(biāo)以及區(qū)域障礙物邊界坐標(biāo)。根據(jù)上文所述航跡規(guī)劃算法求出規(guī)劃好的航跡點(diǎn)，上傳飛控，記錄植保機(jī)的飛行情況。

進(jìn)行10組飛行試驗(yàn)，每組飛行10次，通過(guò)機(jī)載GPS實(shí)時(shí)記錄植保機(jī)的飛行航跡，并實(shí)時(shí)下傳到地面站。圖6為作業(yè)地塊示意，其中矩形區(qū)域?yàn)閷?shí)際作業(yè)的區(qū)域，圖7為實(shí)際飛行航跡圖，其中直線為上傳飛控的規(guī)劃航跡，曲線為植保機(jī)飛行的實(shí)際航跡。

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

表1　規(guī)劃航跡和實(shí)際飛行航跡的最大偏差

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

3　結(jié)論

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

參考文獻(xiàn)：

[1]蔣智超，劉朝宇. 淺談植保無(wú)人機(jī)發(fā)展現(xiàn)狀及趨勢(shì)[J]. 新疆農(nóng)機(jī)化，2016(2)：30-31，42.

[2]陳海，王新民，焦裕松，等. 一種凸多邊形區(qū)域的無(wú)人機(jī)覆蓋航跡規(guī)劃算法[J]. 航空學(xué)報(bào)，2010(9)：1802-1808.

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

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

[5]王和平，汪俊，申?duì)幑? 基于紅外與雷達(dá)一體化吊艙的直升機(jī)安全避障技術(shù)[J]. 科技創(chuàng)新導(dǎo)報(bào)，2014(29)：56-59.

[6]張遜遜，許宏科，朱旭. 低空低速植保無(wú)人直升機(jī)避障控制系統(tǒng)設(shè)計(jì)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2016，32(2)：43-50.

[7]關(guān)震宇，楊東曉，李杰，等. 基于Dubins路徑的無(wú)人機(jī)避障規(guī)劃算法[J]. 北京理工大學(xué)學(xué)報(bào)，2014，34(6)：570-575.

[8]徐博，陳立平，譚彧，等. 基于無(wú)人機(jī)航向的不規(guī)則區(qū)域作業(yè)航線規(guī)劃算法與驗(yàn)證[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2015，31(23)：173-178.