自主飛行無(wú)人機(jī)地理圍欄算法設(shè)計(jì)與實(shí)現(xiàn)

付其喜,梁曉龍,張佳強(qiáng),何呂龍,周萬(wàn)銀

(1.空軍工程大學(xué)國(guó)家空管防相撞技術(shù)重點(diǎn)實(shí)驗(yàn)室,710051,西安;2.空軍工程大學(xué)陜西省電子信息系統(tǒng)綜合集成重點(diǎn)實(shí)驗(yàn)室,710051,西安;3.空軍工程大學(xué)空管領(lǐng)航學(xué)院,710051,西安)

隨著無(wú)人機(jī)(unmanned aircraft system,UAS)產(chǎn)業(yè)的飛速發(fā)展,無(wú)人機(jī)數(shù)量激增,這對(duì)地面人員的人身財(cái)產(chǎn)安全和空中的航空器飛行安全構(gòu)成了嚴(yán)重的威脅[1],因此人們迫切需要發(fā)展無(wú)人機(jī)交通管理系統(tǒng)(UAS traffic management system,UTM)來(lái)保證無(wú)人機(jī)在空域中的安全有序飛行。無(wú)人機(jī)的機(jī)載地理圍欄是UTM的重要組成部分[2]。在獲得由權(quán)威來(lái)源發(fā)布和共享的地理圍欄數(shù)據(jù)后,地理圍欄將空域分為可用空域(禁出地理圍欄)與禁飛區(qū)(禁入地理圍欄),由垂直方向上的高度限制與水平方向上的邊界組成[3]。無(wú)人機(jī)在水平方向上的邊界可以視為由若干頂點(diǎn)構(gòu)成的多邊形。飛行過(guò)程中,無(wú)人機(jī)的飛行空域可以視為由一個(gè)禁出地理圍欄與任意數(shù)量的禁入地理圍欄組成。為了避免無(wú)人機(jī)地理圍欄越界,需要在無(wú)人機(jī)飛行時(shí)實(shí)時(shí)監(jiān)控?zé)o人機(jī)與地理圍欄邊界的位置關(guān)系,判斷無(wú)人機(jī)是否越界或存在越界的危險(xiǎn),在必要時(shí)根據(jù)無(wú)人機(jī)的越界狀態(tài)來(lái)觸發(fā)邊界保持控制。

近年來(lái),在無(wú)人機(jī)地理圍欄的應(yīng)用與研究方面,國(guó)內(nèi)外已取得了一定的進(jìn)展。在一些現(xiàn)有無(wú)人機(jī)商業(yè)產(chǎn)品中,大疆[4]、Ardupilot[5]公司已經(jīng)應(yīng)用了地理圍欄系統(tǒng),但這些地理圍欄系統(tǒng)不能完全可靠地防止無(wú)人機(jī)地理圍欄越界。文獻(xiàn)[6]提出,地理圍欄系統(tǒng)可以作為感知避讓系統(tǒng)的安全替代系統(tǒng),認(rèn)為安裝了機(jī)載地理圍欄系統(tǒng)的低空無(wú)人機(jī)對(duì)地面的人、財(cái)、物幾乎不會(huì)再構(gòu)成任何威脅。當(dāng)該系統(tǒng)被動(dòng)監(jiān)視到無(wú)人機(jī)可能的地理圍欄越界后,通過(guò)奪取無(wú)人機(jī)的導(dǎo)航控制權(quán)以避免越界。不同于直接奪取無(wú)人機(jī)的導(dǎo)航控制權(quán),文獻(xiàn)[7]利用優(yōu)化的方法來(lái)生成在避免地理圍欄越界的前提下的最小幅度機(jī)動(dòng),實(shí)現(xiàn)了以盡可能微小的飛行指令變動(dòng)生成平滑的躲避軌跡,但該方法僅將地理圍欄簡(jiǎn)化為圓形,難以適應(yīng)無(wú)人機(jī)復(fù)雜的飛行環(huán)境。文獻(xiàn)[8]提出了一種在UTM環(huán)境下基于無(wú)人機(jī)性能和風(fēng)的影響的代數(shù)幾何地理圍欄算法,將地理圍欄垂直和水平方向上的最低尺寸由30 m降低到5 m以下。文獻(xiàn)[9-10]中提出了一種無(wú)人機(jī)“安全網(wǎng)”技術(shù)——安全衛(wèi)士,并介紹了對(duì)既定的規(guī)則(如地理圍欄)進(jìn)行驗(yàn)證測(cè)試的結(jié)果。安全衛(wèi)士中地理圍欄邊界分為三層,即警告邊界、飛行終止邊界及硬邊界,但并沒有提出地理圍欄邊界的生成算法。文獻(xiàn)[11]通過(guò)對(duì)植保無(wú)人機(jī)的凸禁出地理圍欄進(jìn)行內(nèi)縮處理和越界檢測(cè),能夠在出現(xiàn)越界危險(xiǎn)時(shí)為植保無(wú)人機(jī)操作人員發(fā)出越界預(yù)警。

本文在國(guó)內(nèi)外無(wú)人機(jī)地理圍欄相關(guān)研究的基礎(chǔ)上,針對(duì)水平方向上的地理圍欄邊界問(wèn)題,首先提出了對(duì)凹凸地理圍欄普遍適用的地理圍欄預(yù)控制層生成算法,包括地理圍欄縮放子算法、頂角平滑子算法及自相交檢測(cè)處理子算法,然后利用改進(jìn)的射線法進(jìn)行地理圍欄越界探測(cè)。對(duì)于地理圍欄的邊界保持控制問(wèn)題,設(shè)計(jì)了針對(duì)自主飛行無(wú)人機(jī)的越界航點(diǎn)重規(guī)劃方法及邊界保持控制控制律,并進(jìn)行了仿真及實(shí)驗(yàn)驗(yàn)證。

1 地理圍欄預(yù)控制層生成

無(wú)人機(jī)在飛行過(guò)程中一旦出現(xiàn)越界危險(xiǎn),無(wú)人機(jī)上的地理圍欄系統(tǒng)將會(huì)利用相應(yīng)的反應(yīng)機(jī)制來(lái)避免地理圍欄的越界,例如強(qiáng)制終止無(wú)人機(jī)飛行或者返回起降點(diǎn)等控制機(jī)制[12]。但是,如果在無(wú)人機(jī)越界之后再進(jìn)行警告或者對(duì)無(wú)人機(jī)的控制進(jìn)行干預(yù),無(wú)人機(jī)將不能被地理圍欄嚴(yán)格包含而只是停留在地理圍欄邊界附近,如圖1a所示,其中xoy為平面直角坐標(biāo)。這種看似微小的地理圍欄越界在復(fù)雜危險(xiǎn)的環(huán)境下(如空中禁飛區(qū)、空中危險(xiǎn)區(qū)等)可能會(huì)造成毀滅性的后果。針對(duì)以上問(wèn)題,本文提出了一種地理圍欄預(yù)控制層自主生成算法,生成了分層地理圍欄,以確保無(wú)人機(jī)足夠早地觸發(fā)地理圍欄的邊界保持控制來(lái)避免越界,從而保證無(wú)人機(jī)的飛行安全,如圖1b所示。地理圍欄預(yù)控制層生成算法包括地理圍欄縮放子算法、地理圍欄頂角平滑子算法以及地理圍欄自相交檢測(cè)處理子算法。

(a)未處理地理圍欄 (b)處理后地理圍欄圖1 地理圍欄對(duì)比

1.1 地理圍欄縮放子算法

地理圍欄縮放指在原始地理圍欄的基礎(chǔ)上產(chǎn)生一個(gè)新的地理圍欄,新生成的地理圍欄邊與原始地理圍欄的對(duì)應(yīng)邊間隔為δb。δb的取值與無(wú)人機(jī)的強(qiáng)制停車飛行距離、轉(zhuǎn)彎半徑、風(fēng)的影響及狀態(tài)估計(jì)誤差等因素有關(guān)。當(dāng)δb<0時(shí),地理圍欄向內(nèi)縮放,即地理圍欄為禁出地理圍欄;當(dāng)δb>0時(shí),地理圍欄向外縮放,即地理圍欄為禁入地理圍欄。

地理圍欄可以通過(guò)簡(jiǎn)單縮放法來(lái)實(shí)現(xiàn),公式為

g′=gsb

(1)

式中:g′為縮放地理圍欄;g為原始地理圍欄;sb為地理圍欄縮放系數(shù)且0

(a)簡(jiǎn)單縮放法 (b)等距離縮放法圖2 地理圍欄縮放方法對(duì)比

等距離縮放法具體步驟如下,設(shè)計(jì)過(guò)程示意圖為圖3。

步驟1 設(shè)置縮放距離δb,獲取地理圍欄頂點(diǎn)集合g=(p1,…,pn)=[(x1,y1),(x2,y2),…,(xn,yn)],其中pi為地理圍欄的第i個(gè)頂點(diǎn)?？s放地理圍欄進(jìn)行初始化,g′=g。

步驟2 計(jì)算地理圍欄各邊的斜率為

(2)

各邊的截距為

bi=yi-ximi

(3)

每個(gè)內(nèi)角的半角為

(4)

步驟3 計(jì)算使得縮放后新舊邊間隔為δb的各角平分線上的距離為

(5)

將θi轉(zhuǎn)換到0～2π之間得到

(6)

步驟4 若δb<0(向內(nèi)縮放),則有

(7)

(8)

若δb>0(向外縮放),則有

(9)

(10)

圖3 等距離縮放法的設(shè)計(jì)過(guò)程示意圖

雖然等距離縮放算法已經(jīng)能夠初步達(dá)到地理圍欄預(yù)控制層生成的目的,但當(dāng)?shù)乩韲鷻跒榘级噙呅螘r(shí),簡(jiǎn)單縮放法無(wú)法解決地理圍欄縮放問(wèn)題,等距離縮放法會(huì)出現(xiàn)過(guò)度縮放導(dǎo)致地理圍欄中可用空域浪費(fèi)的現(xiàn)象,如圖4所示。為此,需要用頂角平滑子算法或自相交處理子算法來(lái)輔助進(jìn)行處理。

(a)簡(jiǎn)單縮放法 (b)等距離縮放法圖4 凹地理圍欄縮放方法對(duì)比

1.2 地理圍欄頂角平滑子算法

步驟2 利用式(2)(3)計(jì)算地理圍欄各邊的斜率mi和截距bi。

(11)

(12)

將β轉(zhuǎn)換到0～2π之間得到

(13)

(14)

(15)

否則得到

(16)

(17)

(18)

(19)

更新縮放地理圍欄的坐標(biāo)。

圖5 頂角平滑算法的設(shè)計(jì)過(guò)程示意圖

1.3 地理圍欄自相交檢測(cè)處理子算法

由于環(huán)境的限制,比如飛行空域中存在隧道、橋梁等,劃設(shè)的地理圍欄兩端部分較大、中間部分較窄,地理圍欄經(jīng)過(guò)縮放后會(huì)出現(xiàn)地理圍欄邊的自相交現(xiàn)象,如圖6所示。針對(duì)這種異常情況,提出了地理圍欄的自相交檢測(cè)處理子算法。地理圍欄縮放子算法與自相交檢測(cè)處理子算法能夠快速提供與原始邊界保持一定安全間隔的可飛空域范圍,并有助于判斷無(wú)人機(jī)是否可穿過(guò)該狹窄區(qū)域。

圖6 縮放地理圍欄自相交現(xiàn)象

自相交檢測(cè)處理算法的主要步驟如下。

步驟2 對(duì)縮放地理圍欄交叉邊與交叉點(diǎn)進(jìn)行處理,將縮放地理圍欄分割為若干條邊界。

步驟3 按原縮放地理圍欄的頂點(diǎn)順序?qū)⒎指詈蟮倪吔缃M合成若干多邊形,如圖6所示。

步驟4 檢測(cè)組合后的若干多邊形的頂點(diǎn)排列順序,刪除與原縮放地理圍欄頂點(diǎn)順序不一致的多邊形,保留下來(lái)的部分即為沒有沖突的縮放地理圍欄g′,如圖7所示。

圖6所示的縮放地理圍欄經(jīng)過(guò)自相交檢測(cè)處理子算法修正后生成了圖7所示的縮放地理圍欄,避免了自相矛盾的情況,確保了生成的縮放地理圍欄的有效性,且在緊急態(tài)勢(shì)下無(wú)人機(jī)可以忽略越界警告而通過(guò)手動(dòng)控制穿過(guò)該狹窄區(qū)域。

2 地理圍欄越界探測(cè)

無(wú)人機(jī)飛行過(guò)程中需要實(shí)時(shí)探測(cè)無(wú)人機(jī)位置與地理圍欄的關(guān)系,以判斷是否存在越界危險(xiǎn)[13]。水平方向上地理圍欄的越界探測(cè)可以視為一種點(diǎn)包容性檢測(cè)問(wèn)題[14]。在點(diǎn)包容性檢測(cè)問(wèn)題中廣泛使用的射線法能夠很好地解決水平地理圍欄探測(cè)問(wèn)題[15]。射線法通過(guò)以r點(diǎn)(坐標(biāo)為(rx,ry))為端點(diǎn)引出的一條射線s來(lái)判斷r點(diǎn)是否在地理圍欄g′內(nèi),如圖8所示。

圖8 射線法示意圖

在原始的射線法中,射線由端點(diǎn)開始向y軸的正半軸方向投射,如果射線與地理圍欄的交點(diǎn)個(gè)數(shù)為奇數(shù),則判定點(diǎn)在地理圍欄內(nèi),否則點(diǎn)在地理圍欄外。但射線與地理圍欄的頂點(diǎn)重合時(shí),則會(huì)錯(cuò)誤地將兩條邊記為一條,從而錯(cuò)誤判斷交點(diǎn)個(gè)數(shù)。為了避免上述問(wèn)題導(dǎo)致的探測(cè)錯(cuò)誤,在原始的射線法的基礎(chǔ)上定義了緩沖距離b,從而提出改進(jìn)的射線法。若地理圍欄某個(gè)頂點(diǎn)的橫坐標(biāo)在rx的緩沖距離b以內(nèi),則給該頂點(diǎn)的橫坐標(biāo)施加一個(gè)擾動(dòng)-2b,以避免射線與地理圍欄頂點(diǎn)重合。地理圍欄越界探測(cè)的具體流程如圖9所示,其中,pi點(diǎn)的橫坐標(biāo)為pix,pi點(diǎn)經(jīng)過(guò)緩沖距離b處理后的橫坐標(biāo)為pix,b。

圖9 地理圍欄越界探測(cè)算法流程圖

3 地理圍欄邊界保持自主控制

地理圍欄預(yù)控制層生成之后,無(wú)人機(jī)在飛行中會(huì)利用地理圍欄越界探測(cè)算法實(shí)時(shí)判斷自己與地理圍欄預(yù)控制層的位置關(guān)系。當(dāng)沒有越界危險(xiǎn)時(shí),無(wú)人機(jī)會(huì)根據(jù)預(yù)先規(guī)劃的路徑飛行完成相應(yīng)的任務(wù)。一旦地理圍欄探測(cè)到無(wú)人機(jī)存在越界的危險(xiǎn),機(jī)載地理圍欄系統(tǒng)將會(huì)立即奪取無(wú)人機(jī)的導(dǎo)航控制權(quán),并按照算法設(shè)定的導(dǎo)航控制方法進(jìn)行反應(yīng)以避免無(wú)人機(jī)越界,從而保證無(wú)人機(jī)的飛行安全[16]。由此可見,地理圍欄邊界保持自主控制的導(dǎo)航控制方法尤為重要。

3.1 越界航點(diǎn)重規(guī)劃

無(wú)人機(jī)飛行前會(huì)進(jìn)行航點(diǎn)規(guī)劃,在實(shí)際飛行過(guò)程中根據(jù)規(guī)劃的航點(diǎn)飛行以完成任務(wù)。無(wú)人機(jī)在航點(diǎn)飛行過(guò)程中可能會(huì)出現(xiàn)地理圍欄越界的情況,需要進(jìn)行無(wú)人機(jī)的航點(diǎn)重規(guī)劃。

地理圍欄邊界自主保持控制過(guò)程中為了避免出現(xiàn)轉(zhuǎn)彎?rùn)C(jī)動(dòng)過(guò)于劇烈導(dǎo)致航跡偏移過(guò)大的情況,無(wú)人機(jī)在邊界保持自主控制中采用直線段勻速、轉(zhuǎn)彎段先減速再轉(zhuǎn)彎的策略。航點(diǎn)重規(guī)劃算法采用在原始航點(diǎn)間插入新航點(diǎn)的方式來(lái)盡可能增加無(wú)人機(jī)地理圍欄邊界保持自主控制中的勻速飛行的距離,以減少飛行時(shí)間。

禁出地理圍欄越界航點(diǎn)重規(guī)劃如圖10所示,若地理圍欄越界探測(cè)算法探測(cè)到航點(diǎn)出現(xiàn)了越界,即進(jìn)行航點(diǎn)重規(guī)劃。首先,在越界航線的兩原始航點(diǎn)間以等距離D插入新的航點(diǎn),生成航點(diǎn)集Winsert。利用地理圍欄越界探測(cè)算法判斷航點(diǎn)集Winsert中航點(diǎn)是否越界。若越界,則將該航點(diǎn)映射到地理圍欄預(yù)控制層最近邊界上,得到修正航點(diǎn)并替換越界航點(diǎn),最終得到修正航點(diǎn)集Wcorrect。

圖10 禁出地理圍欄越界航點(diǎn)重規(guī)劃

圖11 穿越禁入地理圍欄越界航點(diǎn)重規(guī)劃

當(dāng)無(wú)人機(jī)規(guī)劃的航點(diǎn)沒有穿越禁入地理圍欄時(shí),越界航點(diǎn)重規(guī)劃與圖10類似。當(dāng)無(wú)人機(jī)預(yù)先規(guī)劃的航線穿越禁入地理圍欄時(shí),越界航點(diǎn)重規(guī)劃與未穿越時(shí)有很大不同,此時(shí)的越界航點(diǎn)重規(guī)劃如圖11所示。為避免無(wú)人機(jī)相向飛行時(shí)進(jìn)行穿越禁入地理圍欄航點(diǎn)重規(guī)劃時(shí)可能的沖突情況,在穿越禁入地理圍欄航點(diǎn)重規(guī)劃時(shí)加入飛行規(guī)則,即沿禁入地理圍欄預(yù)控制層邊界逆時(shí)針插入修正航點(diǎn),也就是右轉(zhuǎn)。

穿越禁入地理圍欄航點(diǎn)重規(guī)劃主要步驟如下。

步驟1 判斷無(wú)人機(jī)原始航點(diǎn)生成的航線(以下簡(jiǎn)稱穿越航線,其可能包含多個(gè)原始航點(diǎn))是否穿越禁入地理圍欄預(yù)控制層g′,若穿越,則進(jìn)行穿越禁入地理圍欄的航點(diǎn)重規(guī)劃。

步驟2 在穿越航線的兩端的原始航點(diǎn)之間以等距離D插入航點(diǎn),生成插入航點(diǎn)集Winsert。

步驟3 利用地理圍欄越界探測(cè)算法判斷插入航點(diǎn)集Winsert中航點(diǎn)是否越界。若越界,則刪除越界航點(diǎn)。

步驟4 沿禁入地理圍欄預(yù)控制層邊界逆時(shí)針以等距離D插入修正航點(diǎn)后,得到修正航點(diǎn)集Wcorrect,其中禁入地理圍欄預(yù)控制層頂點(diǎn)固定為修正航點(diǎn)。

航點(diǎn)重規(guī)劃過(guò)程中轉(zhuǎn)彎前的兩航點(diǎn)之間需要保留一定的安全距離Ds,其公式為

(20)

式中:us為無(wú)人機(jī)地理圍欄觸發(fā)邊界保持控制時(shí)的標(biāo)準(zhǔn)速度,方向?yàn)楫?dāng)前位置指向下一航點(diǎn);amax為無(wú)人機(jī)的最大加速度。

圖12是穿越禁入地理圍欄越界航點(diǎn)的重規(guī)劃航點(diǎn)設(shè)計(jì)過(guò)程,為了保證如圖中w4航點(diǎn)等始端修正航點(diǎn)之間的安全距離,需要對(duì)不同情況進(jìn)行討論。對(duì)航點(diǎn)重規(guī)劃的始端修正航點(diǎn)討論步驟如下。

步驟1 計(jì)算以航點(diǎn)w3為圓心,以D為半徑的圓與禁入地理圍欄預(yù)控制層邊界所在直線的交點(diǎn),得兩交點(diǎn)A與B,由飛行規(guī)則得下一個(gè)修正航點(diǎn)方向?yàn)榻稽c(diǎn)B的方向。

步驟2 若禁入地理圍欄預(yù)控制層邊界所在線段與航點(diǎn)w3和交點(diǎn)B連接構(gòu)成的線段相交,如圖12a所示,則轉(zhuǎn)向Step3,若不相交,如圖12b所示,則轉(zhuǎn)向Step4。

步驟3 若點(diǎn)B與頂點(diǎn)的距離大于Ds,則點(diǎn)B即為修正航點(diǎn)w4,否則頂點(diǎn)為w4。

步驟4 若航點(diǎn)w3與頂點(diǎn)的距離大于Ds,則頂點(diǎn)為修正航點(diǎn)w4,否則,刪除航點(diǎn)w3,且頂點(diǎn)替代航點(diǎn)w3。

(a)相交

(b)不相交圖12 穿越禁入地理圍欄越界航點(diǎn)的重規(guī)劃航點(diǎn)設(shè)計(jì)過(guò)程

對(duì)航點(diǎn)重規(guī)劃的末端修正航點(diǎn)的討論與始端類似。地理圍欄航點(diǎn)修正以后,無(wú)人機(jī)在合理設(shè)置參數(shù)δb的基礎(chǔ)上按照修正航點(diǎn)飛行就能夠有效避免地理圍欄越界。

3.2 邊界保持控制律設(shè)計(jì)

地理圍欄在探測(cè)到越界危險(xiǎn)并對(duì)航點(diǎn)進(jìn)行重規(guī)劃之后,無(wú)人機(jī)就可以按照修正航點(diǎn)飛行以避免地理圍欄越界。然而,不同的飛行控制器的控制律并不盡相同,無(wú)人機(jī)仍然會(huì)有越界的危險(xiǎn)。所以,地理圍欄需要設(shè)計(jì)獨(dú)立于飛行控制器的控制律。

若將無(wú)人機(jī)視為質(zhì)點(diǎn),其采用一階積分模型的動(dòng)力學(xué)模型為

(21)

式中x(t)、u(t)分別為無(wú)人機(jī)的位置與速度,同時(shí)u(t)為對(duì)應(yīng)的控制輸入。

當(dāng)無(wú)人機(jī)進(jìn)行直線飛行時(shí),設(shè)計(jì)無(wú)人機(jī)的控制律為

(22)

式中Δt為無(wú)人機(jī)狀態(tài)更新間隔。

當(dāng)無(wú)人機(jī)進(jìn)行轉(zhuǎn)彎飛行時(shí),設(shè)計(jì)無(wú)人機(jī)的控制律為

(23)

式中xw(i)為無(wú)人機(jī)當(dāng)前正在飛向的第i個(gè)航點(diǎn)。

4 仿真及實(shí)驗(yàn)結(jié)果分析

針對(duì)提出的地理圍欄算法在Matlab R2016a環(huán)境下對(duì)地理圍欄算法進(jìn)行仿真,并利用搭建的實(shí)驗(yàn)驗(yàn)證系統(tǒng)進(jìn)行實(shí)驗(yàn),最后進(jìn)行仿真及實(shí)驗(yàn)數(shù)據(jù)分析。

4.1 實(shí)驗(yàn)系統(tǒng)介紹

實(shí)驗(yàn)驗(yàn)證系統(tǒng)由四旋翼無(wú)人機(jī)平臺(tái)、數(shù)據(jù)鏈與地面站共同組成,如圖13所示。

圖13 實(shí)驗(yàn)驗(yàn)證系統(tǒng)

四旋翼無(wú)人機(jī)采用PIX自駕儀作為飛行控制系統(tǒng),自駕儀由用于估計(jì)無(wú)人機(jī)運(yùn)動(dòng)狀態(tài)的3組陀螺儀、3組加速度計(jì)、2組電子羅盤和用來(lái)記錄實(shí)驗(yàn)數(shù)據(jù)的8 GB反式(trans flash,TF)存儲(chǔ)卡組成。系統(tǒng)的定位模塊為全球定位系統(tǒng)(global position system,GPS),頻率為10 Hz,采用串口通信進(jìn)行數(shù)據(jù)傳輸。實(shí)驗(yàn)系統(tǒng)數(shù)傳部分采用基于Mesh組網(wǎng)技術(shù)的Microhard P400-840模塊,通信頻率為840 MHz。由于無(wú)人機(jī)進(jìn)行地理圍欄邊界保持控制時(shí)需要一定的計(jì)算量和較高的實(shí)時(shí)性,選用STM32F4系列微處理器進(jìn)行實(shí)時(shí)運(yùn)算。系統(tǒng)硬件結(jié)構(gòu)如圖14所示。

圖14 系統(tǒng)硬件結(jié)構(gòu)圖

4.2 仿真及實(shí)驗(yàn)數(shù)據(jù)分析

仿真及實(shí)驗(yàn)相關(guān)參數(shù)設(shè)置為:縮放距離δb=±3m,|u0|=0m/s,|us|=3m/s,D=12m。仿真結(jié)果如圖15所示:從圖15a可以看出,無(wú)人機(jī)能夠避免原始地理圍欄越界;從圖15b可以看出,無(wú)人機(jī)能夠按照設(shè)計(jì)的地理圍欄邊界保持自主控制控制律飛行,即直線段勻速飛行,轉(zhuǎn)彎點(diǎn)先減速再加速;從圖15c可以看出,無(wú)人機(jī)能夠始終與地理圍欄邊界保持至少δb的間隔,說(shuō)明了本文設(shè)計(jì)的地理圍欄算法的有效性。

實(shí)驗(yàn)結(jié)果如圖16所示:從圖16a可以看出,無(wú)人機(jī)出現(xiàn)了地理圍欄預(yù)控制層越界的情況,但卻并未出現(xiàn)原始地理圍欄越界;從圖16b可以看出,無(wú)人機(jī)的飛行速度與仿真結(jié)果存在一定的誤差,這是由于風(fēng)的影響、無(wú)人機(jī)狀態(tài)估計(jì)誤差等共同的作用;從圖16c可以看出,無(wú)人機(jī)出現(xiàn)了與原始地理圍欄的邊界間隔小于δb的情況,這主要是由于無(wú)人機(jī)慣性、控制響應(yīng)時(shí)延以及定位誤差等引起的,但并不會(huì)造成原始地理圍欄越界。實(shí)驗(yàn)視頻見文獻(xiàn)[17]。

(a)飛行航跡 (b)速度變化 (c)無(wú)人機(jī)與地理圍欄邊界的間隔圖15 仿真結(jié)果

(a)飛行航跡 (b)速度變化 (c)無(wú)人機(jī)與地理圍欄邊界的間隔圖16 實(shí)驗(yàn)結(jié)果

圖17 實(shí)驗(yàn)無(wú)人機(jī)飛行航跡

圖17是實(shí)驗(yàn)的無(wú)人機(jī)飛行航跡,實(shí)驗(yàn)選取田徑場(chǎng)作為禁出地理圍欄,黑色矩形為禁出地理圍欄范圍。禁出地理圍欄中包含一個(gè)禁入地理圍欄,即禁出地理圍欄中的平行四邊形區(qū)域。無(wú)人機(jī)飛行中設(shè)定了三個(gè)原始航點(diǎn)w1～w3,由飛行航跡可知,無(wú)人機(jī)在原始航點(diǎn)之間飛行時(shí)根據(jù)地理圍欄算法進(jìn)行了修正,避免了地理圍欄越界。

由仿真及實(shí)驗(yàn)結(jié)果可以得出,設(shè)計(jì)并實(shí)現(xiàn)的地理圍欄算法能夠有效地進(jìn)行地理圍欄預(yù)控制層生成、探測(cè),并在無(wú)人機(jī)有越界危險(xiǎn)時(shí)進(jìn)行實(shí)時(shí)邊界保持控制,避免地理圍欄越界,保證無(wú)人機(jī)的飛行安全。

5 結(jié) 論

針對(duì)無(wú)人機(jī)地理圍欄問(wèn)題,設(shè)計(jì)并實(shí)現(xiàn)了地理圍欄算法,確保了無(wú)人機(jī)飛行過(guò)程中能夠有效避免地理圍欄越界。

(1)提出了一種地理圍欄預(yù)控制層生成算法,并采用頂角平滑與自相交處理等子算法保證了對(duì)于凹凸地理圍欄以及包含狹窄區(qū)域的地理圍欄的普遍適用性。

(2)通過(guò)改進(jìn)的射線法進(jìn)行地理圍欄越界探測(cè),并通過(guò)越界航點(diǎn)重規(guī)劃與設(shè)計(jì)的邊界保持自主控制控制律來(lái)實(shí)現(xiàn)對(duì)地理圍欄越界的反應(yīng),使自主飛行的無(wú)人機(jī)能夠避免地理圍欄越界。

(3)利用搭建的四旋翼無(wú)人機(jī)實(shí)驗(yàn)系統(tǒng)對(duì)提出的地理圍欄算法進(jìn)行實(shí)驗(yàn)驗(yàn)證,仿真及實(shí)驗(yàn)結(jié)果均證明了本文算法的有效性。