基于UAV地面站避讓系統(tǒng)的地理圍欄算法設(shè)計與仿真

強(qiáng)明輝 ，把翠芳

（1.蘭州理工大學(xué) 電氣工程與信息工程學(xué)院，蘭州 730050；2.甘肅省工業(yè)過程先進(jìn)控制重點實驗室，蘭州 730050；3.蘭州理工大學(xué) 電氣與控制工程國家級實驗教學(xué)示范中心，蘭州 730050）

隨著功能越來越先進(jìn)的新式無人機(jī)不斷涌現(xiàn)，同時也帶來諸多安全隱患[1]，如自身設(shè)備安全問題、社會安全問題。無人機(jī)在機(jī)場附近飛行威脅民航客機(jī)的起降，在國家機(jī)關(guān)和軍隊駐地等敏感區(qū)域進(jìn)行航拍，竊取、危害國家安全，以及無人機(jī)操作不當(dāng)闖入人群密集區(qū)域，造成人身傷害等。因此，防止無人機(jī)進(jìn)入禁飛區(qū)域，實現(xiàn)無人機(jī)的安全應(yīng)用是無人機(jī)發(fā)展的一個關(guān)鍵問題，其中感知與規(guī)避系統(tǒng)[2-3]在目前的研究中扮演了至關(guān)重要的角色。

無論是陸基的還是機(jī)載的感知與規(guī)避系統(tǒng)，都旨在為無人機(jī)及其操作人員提供 “看見與避讓”以及飛行的保障技術(shù)，然而目前國內(nèi)外的感知與規(guī)避技術(shù)目前發(fā)展尚未成熟。其技術(shù)難點有以下兩方面原因：①由于低空慢速微型無人機(jī)的發(fā)展，尤其體型越來越小，對其的防御和打擊很難實現(xiàn)精方面、準(zhǔn)、快。②在無人機(jī)的感知探測技術(shù)方面，由于現(xiàn)代無人機(jī)的機(jī)體結(jié)構(gòu)廣泛選用復(fù)合材料等性能良好的材料制作，一些無人機(jī)本身具有雷達(dá)隱身功能或者動力裝置為電動機(jī)、活塞式發(fā)動機(jī)，其紅外信號的特征極弱，而且低空慢速微型無人機(jī)的體型越來越小，使得無人機(jī)很難被雷達(dá)、聲學(xué)、光學(xué)、紅外線的探測器發(fā)現(xiàn)。在使用時無人機(jī)還可以自主規(guī)避固定的雷達(dá)站和防空區(qū)域。因此實現(xiàn)對無人機(jī)的感知探測變得困難。

1 無人機(jī)感知與規(guī)避系統(tǒng)

目前，典型的無人機(jī)避讓禁飛區(qū)的體系是無人機(jī)感知與規(guī)避系統(tǒng)，系統(tǒng)模型如圖1所示。

圖1 無人機(jī)感知與規(guī)避系統(tǒng)模型Fig.1 UAV sense and avoid system model

該系統(tǒng)的難點是感知模塊中感知探測技術(shù)和防撞算法模塊中的防撞算法。具體如下：

1）感知探測有雷達(dá)、視覺、EO/IR（光電/紅外）等技術(shù)。但是，探測性能受到無人機(jī)姿態(tài)影響而存在盲區(qū)，如在坡度轉(zhuǎn)彎時視場將隨著側(cè)傾角在水平面上轉(zhuǎn)動從而導(dǎo)致很多入侵機(jī)無法被探測。探測范圍、掃描角速度、更新率和信號質(zhì)量等均需要進(jìn)一步改進(jìn)提高。

2）防撞算法可歸納為2種：一種是采用幾何算法，通過分析無人機(jī)和入侵機(jī)在幾何空間的相對運動關(guān)系，按照沖突探測與解脫實施被動式防撞。另一種是轉(zhuǎn)化為最小安全間隔約束條件下的航跡規(guī)劃問題，采用航跡規(guī)劃算法、根據(jù)感知探測的入侵機(jī)的狀態(tài)信息主動規(guī)劃從當(dāng)前位置到目標(biāo)位置的無碰撞安全航跡。這2種算法在實施性和效率上尚有待提高。

綜上，針對感知與規(guī)避系統(tǒng)的技術(shù)缺陷，文中提出了一種無人機(jī)基于地理圍欄的地面站避讓系統(tǒng)，將地理圍欄技術(shù)[4]——一種主動式的地理位置服務(wù)——應(yīng)用于無人機(jī)地面站，預(yù)先劃定禁飛區(qū)域（靜態(tài)地理圍欄），地面站接收無人機(jī)的定位信息，利用地理圍欄算法解算判斷無人機(jī)是否進(jìn)入禁飛區(qū)域，并設(shè)置進(jìn)入禁飛區(qū)立即會通知地面站操作人員，操控?zé)o人機(jī)避讓。其中地理圍欄算法的研究與優(yōu)化對于無人機(jī)在空域的融合以及安全飛行有很大的研究意義。

2 基于地理圍欄的地面站避讓系統(tǒng)模型

基于地理圍欄的地面站避讓系統(tǒng)實現(xiàn)的具體過程是，通過在無人機(jī)地面站系統(tǒng)中，引入地理圍欄、定義圍欄事件，在地理信息系統(tǒng)GIS（geographic information system）平臺的支持下，首先在地面站電子地圖中劃分禁飛區(qū)域，然后由無人機(jī)機(jī)載GPS定位模塊實時向地面站傳送無人機(jī)位置信息，利用地理圍欄算法解算無人機(jī)位置與預(yù)劃分禁飛區(qū)域的位置關(guān)系，從而判斷無人機(jī)是否進(jìn)入禁飛區(qū)域，若未進(jìn)入且沒有進(jìn)入禁飛區(qū)域的趨勢，則無人機(jī)正常飛行；若進(jìn)入禁飛區(qū)域，則通知地面操作人員，向飛行控制系統(tǒng)傳達(dá)返航或避讓指令，進(jìn)而實現(xiàn)無人機(jī)對禁飛區(qū)域的避讓。基于地理圍欄的地面站的避讓系統(tǒng)原理如圖2所示。

圖2 地面站的避讓系統(tǒng)模型Fig.2 Avoid system model of ground station

所研究的新型無人機(jī)避讓禁飛區(qū)域的系統(tǒng)中，地理圍欄算法的設(shè)計是關(guān)鍵，具體研究設(shè)計適合該系統(tǒng)的地理圍欄算法過程分為2步：①研究常用的典型多邊形地理圍欄；②優(yōu)化多邊形地理圍欄，使得系統(tǒng)的避讓準(zhǔn)確度更高。

3 地理圍欄算法

3.1 常用多邊形圍欄算法

實際應(yīng)用中，禁飛區(qū)域的不規(guī)則性，而且地理圍欄側(cè)重于對區(qū)域邊界的界定，可以準(zhǔn)確勾勒出小區(qū)、寫字樓等特定坐標(biāo)的實際形狀、區(qū)域，因此地理圍欄一般為不規(guī)則的多邊形區(qū)域。具體分為2個階段：①使用R樹索引[5]快速檢測無人機(jī)測試點是否在最小邊界矩形內(nèi)，并由最小邊界矩形將多邊形分割嵌套；②根據(jù)X值將交叉點進(jìn)行排序。整理出所有交叉點后。判斷交叉點與測試點的關(guān)系，如果測試點的值將在2個排序點之間，那么它將位于多邊形內(nèi)部[6]，否則位于多邊形外部。

多邊形[6]由一條閉合的路徑所界定，由有限的直線段組成，用點P1，…，Pn（其中n=1，2，…）來表示。多邊形的頂點逆時針排序如圖3所示。圖中，P1P2，…，Pn-1Pn，PnP1為多邊形的邊；Pn為多邊形的頂點；IP為多邊形與直線的交點。默認(rèn)規(guī)則是逆時針順序為正，順時針順序為負(fù)。

圖3 多邊形的頂點逆時針排序Fig.3 Vertex reverse clockwise ordering graph of a polygon

由圖可見，逆時針排列多邊形頂點，其坐標(biāo)分別為P1（X1，Y1），P2（X2，Y2），…，Pn（Xn，Yn）。 假設(shè)無人機(jī)測試點為P（x，y），通過點P畫1條垂直于y軸的直線；多邊形的每個點是具有X坐標(biāo)和Y坐標(biāo)的位置，將其應(yīng)用于地理圍欄，可以將X坐標(biāo)視為經(jīng)度，將Y坐標(biāo)視為緯度。算法描述如下：

①劃定多邊形區(qū)域，找出所選多邊形的左、右、最上面和最下面的頂點。

②獲取多邊形的最小邊界矩形并將其添加到R樹[5]。

③多邊形的邊數(shù)為polyn，具體由第1個點P1依次連接到最后1個點Pn再與點P1連接構(gòu)成。

④將無人機(jī)測試點P的y坐標(biāo)與多邊形每組兩兩相交邊的2個頂點的Y坐標(biāo)進(jìn)行比較，檢查是否介于這兩邊的坐標(biāo)之間，即

式中：Ymin為2個頂點中較小的；Ymax為2個頂點中較大的。

⑤利用光線投射法，繪制垂直光線通過測試點，與多邊形的兩邊會有交點。將測試點左右兩側(cè)的任意交點分別記為點I和點E，根據(jù)斜率的方程

求出斜率KIE，帶入方程

求出測試點P的x坐標(biāo)值。

⑥根據(jù)X值將多邊形的交點進(jìn)行排序。

⑦判斷測試點與多邊形的位置關(guān)系。如果測試點的x值在2個排序點的X值之間，那么測試點位于多邊形內(nèi)部，否則位于多邊形外部。

其中最小邊界矩形框的解法如圖4所示。定義P，S，T的坐標(biāo)分別為P（XP，YP），S（XS，YS），T（XT，YT）。當(dāng)P在點S的右邊和點T的左邊時，判斷點P和線ST的位置關(guān)系的方法為

式中：KST為線ST的斜率。

圖4 最小矩形邊框Fig.4 Minimum rectangular border

從R樹索引中獲得多邊形的最小矩形邊界框后，再檢查判斷無人機(jī)測試點與最小矩形邊界框的位置關(guān)系。如果無人機(jī)測試點在最小矩形邊界框的內(nèi)部，則計算公式為

式中：yC為垂直光線與線ST交點C的坐標(biāo)。

如果yC>YP且垂直光線與多邊形的邊交叉，那么與多邊形的交點數(shù)+1，并且繼續(xù)檢測多邊形的下一條邊；如果yC>YP且垂直光線與多邊形的邊沒有交叉，則直接檢測多邊形的下一條邊。當(dāng)檢測結(jié)束，若交點總數(shù)是奇數(shù)，即可判斷無人機(jī)測試點P在多邊形外部。

3.2 優(yōu)化地理圍欄算法

目前常用的地理圍欄算法多用于應(yīng)用程序等場景，二維空間的平面圍欄即可滿足需求。無人機(jī)的飛行域為三維空間，以此針對無人機(jī)有限的飛行空域中禁飛區(qū)域的劃定，除了經(jīng)緯度，還需要有高度的規(guī)定，使得地理圍欄的功能更加精準(zhǔn)，性能更加優(yōu)越。

在常用地理圍欄的算法基礎(chǔ)上，添加高度參數(shù)進(jìn)行算法優(yōu)化。具體的實現(xiàn)過程有2部分：①利用八叉樹算法[7]，快速檢測無人機(jī)測試點是否在最小邊界長方體內(nèi)，并由八叉樹算法將多面體分割嵌套；②如果多面體在最小邊界長方體內(nèi)部，則繼續(xù)判斷。

無人機(jī)測試點在多面體地理圍欄內(nèi)部的充分必要條件是：多面體每個表面上任意一點到無人機(jī)測試點P的向量和該平面法向量的數(shù)量積≤0。其中，全部等于0表明P在多面體地理圍欄的邊界線上；全部小于0表明P在多面體地理圍欄的內(nèi)部。

長方體地理圍欄是典型的多面體地理圍欄。假設(shè)，無人機(jī)定位的 GPS 位置信息為P（X0，Y0，Z0），經(jīng)過坐標(biāo)轉(zhuǎn)換算法，無人機(jī)在電子地圖上的位置信息為P（X0′，Y0′，Z0′）。設(shè)，長方體地理圍欄由A（XA，YA，ZA），A′（XA′，YA′，ZA′），B（XB，YB，ZB），B′（XB′，YB′，ZB′），C（XC，YC，ZC），C′（XC′，YC′，ZC′），D（XD，YD，ZD），D′（XD′，YD′，ZD′）依次連接構(gòu)成，如圖 5 所示。 （規(guī)定長方體每個面的法向量朝外）那么長方體地理圍欄算法規(guī)則如下：

圖5 空間測試點與長方體位置關(guān)系Fig.5 Positional relation between point and hexahedron in space

規(guī)則1 分別求出長方體頂點D到點P的向量和長方體以點D為頂點的3個表面法向量的數(shù)量積，這3個表面分別是面DCC′D′，面DABC和面DAA′D′。在循環(huán)求數(shù)量積的過程中，如果數(shù)量積大于0，循環(huán)中斷，直接可以判斷無人機(jī)測試點在長方體地理圍欄外部，如圖5中P1點所示。循環(huán)結(jié)束，若3個數(shù)量積中存在等于0的值，則測試點在長方體地理圍欄內(nèi)部；否則，轉(zhuǎn)到規(guī)則2繼續(xù)進(jìn)行判斷。

規(guī)則2 分別求出長方體頂點A′到點P的向量和長方體以點A′為頂點的3個面（即面A′B′BA，面A′D′DA和面A′D′C′B′）法向量的數(shù)量積。 在循環(huán)求數(shù)量積的過程中，如果數(shù)量積大于0，則循環(huán)中斷，直接可判斷無人機(jī)測試點在長方體地理圍欄外部，如圖5中P3點所示。若數(shù)量積存在等于0的值，無人機(jī)測試點在長方體地理圍欄外部，如圖5中P2點。

4 地理圍欄算法仿真與分析

為了驗證提出的地理圍欄算法及其優(yōu)化算法的有效性，通過已經(jīng)建立的無人機(jī)地面站的避讓系統(tǒng)模型，在地面站半實物平臺[8]中，對2種地理圍欄算法進(jìn)行MatLab仿真驗證。地面站系統(tǒng)半物理仿真平臺結(jié)構(gòu)如圖6所示。

圖6 地面站系統(tǒng)半物理仿真平臺結(jié)構(gòu)Fig.6 Semi physical simulation platform structure of ground station system

小型無人機(jī)飛行使用GPS實現(xiàn)導(dǎo)航。GPS目前采用WGS-84坐標(biāo)系，而地理圍欄建立檢測過程需要在電子地圖上進(jìn)行操作。由于電子地圖往往采用BJ-54坐標(biāo)系，因此需要進(jìn)行坐標(biāo)轉(zhuǎn)換[9]。無人機(jī)GPS坐標(biāo)信息經(jīng)過坐標(biāo)轉(zhuǎn)換之后，再通過MatLab軟件進(jìn)行算法仿真[10]驗證。地理圍欄算法工作流程如圖7所示，具體如下：

1）矩形地理圍欄算法仿真。矩形地理圍欄參數(shù)設(shè)置為

P0（1000，500），P1（3000，500）

P2（1000，3000），P3（1000，1000）

圖7 地理圍欄算法流程Fig.7 Flow chart of geo-fencing algorithm

無人機(jī)的 GPS 位置信息為P（3000，2500，1500）。經(jīng)過地理圍欄算法解算，進(jìn)行判斷，其仿真結(jié)果如圖8所示。

圖8 矩形方法仿真Fig.8 Rectangular method simulation

2）長方體地理圍欄算法仿真。設(shè)置長方體圍欄參數(shù)為

A（3500，500，2000），A′（3500，500，500）

B（3500，3000，2000），B′（3500，3000，500）

C（1000，3000，2000），C′（1000，3000，500）

D（1000，1000，2000），D′（1000，1000，500）

無人機(jī)的GPS位置信息經(jīng)過坐標(biāo)轉(zhuǎn)換為P（3000，2500，1500），經(jīng)過地理圍欄算法解算，進(jìn)行判斷。仿真結(jié)果如圖9所示。

圖9 多面體算法仿真Fig.9 Polyhedron algorithm simulation

仿真結(jié)果表明，這2種算法均具有可行性，而優(yōu)化算法可以更加精細(xì)地劃分地理圍欄網(wǎng)格，符合無人機(jī)的空間需求，尤其對于城市復(fù)雜的地理結(jié)構(gòu)，可以在空間上提供更加準(zhǔn)確的圍欄邊界，更符合實際應(yīng)用，從而提升設(shè)計研究的無人機(jī)地面站避讓系統(tǒng)的避讓精確度與安全水平。

5 結(jié)語

針對無人機(jī)帶來社會安全問題，以及無人機(jī)感知與規(guī)避系統(tǒng)在技術(shù)上的難點，研究設(shè)計了一種無人機(jī)飛行區(qū)域地理圍欄管理的新方法。地理圍欄作為一種接近式觸發(fā)服務(wù)，能根據(jù)無人機(jī)系統(tǒng)的需求自定義觸發(fā)規(guī)則如觸發(fā)范圍、形狀、無人機(jī)行為等。該方法解決了無人機(jī)感知和避讓系統(tǒng)的若干缺陷，基于地理圍欄的地面站避讓系統(tǒng)，創(chuàng)新地利用地理圍欄算法與無人機(jī)的機(jī)載定位導(dǎo)航系統(tǒng)的結(jié)合，無需雷達(dá)等其它探測機(jī)載設(shè)備，解決了感知探測技術(shù)的難點，更加具有實際應(yīng)用性；該系統(tǒng)相比無人機(jī)感知與避讓系統(tǒng)，操作實現(xiàn)更容易，利用地理圍欄優(yōu)化算法還可以實現(xiàn)禁飛區(qū)域在空間中的精確劃分，使得無人機(jī)準(zhǔn)確避讓；該系統(tǒng)具有擴(kuò)展性，可以將地理圍欄算法嵌入無人機(jī)飛行控制系統(tǒng)，進(jìn)而完善系統(tǒng)的自主性，提升智能化水平。