基于矢量地圖的無(wú)人機(jī)三維航線(xiàn)校正仿真

劉 霞，馬 躍，周 倩

(1. 天津中德應(yīng)用技術(shù)大學(xué)，天津 300350；2. 天津中醫(yī)藥大學(xué)，天津 300193)

1 引言

無(wú)人機(jī)的航線(xiàn)校正是在一定的限制條件下，建立能夠?qū)崿F(xiàn)無(wú)人機(jī)自動(dòng)駕駛與執(zhí)行任務(wù)的最佳飛行路徑。無(wú)人機(jī)在執(zhí)行任務(wù)時(shí)容易受到氣流等環(huán)境因素影響，導(dǎo)致航線(xiàn)偏離，所以航線(xiàn)校正是當(dāng)前研究的核心問(wèn)題。

目前，用于航線(xiàn)校正的主要方法包括遺傳算法[1]、人工勢(shì)場(chǎng)法[2]和雙目測(cè)距模塊[3]等。遺傳算法存在計(jì)算復(fù)雜，不利于快速收斂從而易導(dǎo)致產(chǎn)生局部最優(yōu)；人工勢(shì)場(chǎng)法通常用在二維的航跡搜索與校正，不利于對(duì)三維行線(xiàn)的校正；雙目測(cè)距模塊會(huì)提前設(shè)定無(wú)人機(jī)飛行方位，在行駛過(guò)程中傾角容易變大，當(dāng)出現(xiàn)阻礙時(shí)會(huì)產(chǎn)生間歇性的目標(biāo)丟失，導(dǎo)致其存在準(zhǔn)確性較低的缺陷。

對(duì)此，本文引入適量地圖，提出了一種新的航線(xiàn)校正方法，矢量地圖主要作用于無(wú)人機(jī)運(yùn)行軌跡顯示設(shè)備當(dāng)中，它能夠展現(xiàn)出地理方位、設(shè)定運(yùn)行范圍，利于標(biāo)記航跡信息，通常情況下，地圖的使用策略都能夠較好的運(yùn)用在矢量地圖上。該方法首先利用LPA算法[4]和矢量地圖[5]的顯示數(shù)據(jù)部署三維航線(xiàn)，并且利用優(yōu)化后的估價(jià)函數(shù)有效搜索最優(yōu)路徑，結(jié)合了N.Nillson研究的一種對(duì)于航線(xiàn)搜索與部署傳統(tǒng)式A*算法，能夠快速尋找到最優(yōu)路徑。通過(guò)仿真實(shí)驗(yàn)驗(yàn)證該方法的時(shí)效性和準(zhǔn)確性。

2 矢量地圖的加載

地圖僅顯示地理方位不能滿(mǎn)足實(shí)際運(yùn)行要求，因此增添了城市、山脈、鐵路、河流等顯示信息，從而使地圖顯示變得更加完善。矢量地圖能夠根據(jù)數(shù)字高層地圖收集阻礙飛行軌跡的信息，經(jīng)過(guò)處理后構(gòu)成阻礙運(yùn)行模型，最后會(huì)展現(xiàn)在矢量地圖上?？s放矢量地圖的同時(shí)不會(huì)改變真實(shí)性，這也成為它的顯著特性[7]。矢量地圖可以通過(guò)構(gòu)建空間坐標(biāo)來(lái)描述地理方位，并且在實(shí)際運(yùn)用過(guò)程中，技術(shù)人員能夠通過(guò)無(wú)人機(jī)運(yùn)行情況，適當(dāng)?shù)年P(guān)閉與當(dāng)前所需要無(wú)關(guān)的信息。無(wú)人機(jī)矢量地圖加載過(guò)程分為以下幾個(gè)階段：

1)啟動(dòng)BIGEMAP加載軟件[8]。

2)把鼠標(biāo)移動(dòng)到加載軟件的左上方，單擊頁(yè)面中帶有“選取地圖”的標(biāo)志，接下來(lái)點(diǎn)擊地圖下載渠道。

3)單擊屏幕上的“行政范圍選取”，接下來(lái)在列框中點(diǎn)擊中國(guó)。

4)雙擊下載軟件后，會(huì)彈出一個(gè)“下載對(duì)話(huà)框”，點(diǎn)擊“行政區(qū)域”，然后會(huì)跳到矢量地圖下載界面，同時(shí)選取矢量地圖的儲(chǔ)存方式“Shape File”，選擇地理坐標(biāo)，用鼠標(biāo)單擊“選擇”，選取相應(yīng)的坐標(biāo)投影，但經(jīng)緯度坐標(biāo)是固定的WGS84。“運(yùn)行名稱(chēng)”以及“保存方式”可以自行編輯。

(5)對(duì)加載區(qū)域級(jí)別進(jìn)行選取，若選取“市”區(qū)域級(jí)別，那么此時(shí)矢量地圖上的“基礎(chǔ)數(shù)據(jù)”會(huì)根據(jù)所選擇的區(qū)域進(jìn)行編制，編制完成后單擊“確定”，從而獲得市級(jí)行政區(qū)域的矢量地圖。

綜上所述，就能夠加載所需要的矢量地圖，若想要下載市級(jí)或鄉(xiāng)鎮(zhèn)行政區(qū)域，要在第(4)階段選取相應(yīng)的行政階級(jí)，但不會(huì)變更保留方式與方位坐標(biāo)。

3 無(wú)人機(jī)三維航線(xiàn)校正

3.1 基于LPA算法的飛行軌跡部署

經(jīng)典的A*算法通過(guò)啟發(fā)式方法很好的采用了地圖展現(xiàn)出來(lái)的信息，(作用在下面黃色部分)有效的減少了節(jié)點(diǎn)轉(zhuǎn)換次數(shù)。該算法利用估價(jià)函數(shù)[9]f(n)預(yù)算現(xiàn)有節(jié)點(diǎn)n與終點(diǎn)間的距離，同時(shí)確定了搜尋方位。代價(jià)函數(shù)如式(1)所示

f(n)=g(n)+h(n)

(1)

其中，g(n)表示初始點(diǎn)位距離目標(biāo)點(diǎn)位n的實(shí)際代價(jià)，h(n)表示啟發(fā)(發(fā)生)函數(shù)。根據(jù)A*算法能夠推導(dǎo)出許多啟發(fā)式搜索方法[10]，但絕大部分都是應(yīng)用在二維航線(xiàn)部署上，有極少部分使用在三維航線(xiàn)中，基于此，本文提出了LPA算法，該算法由A與SWSP-FP構(gòu)成，主要核心就是利用前搜索樹(shù)中的數(shù)據(jù)，來(lái)應(yīng)對(duì)權(quán)值發(fā)生變化時(shí)產(chǎn)生重復(fù)部署的情況。針對(duì)無(wú)人機(jī)的飛行要求，把LPA算法轉(zhuǎn)換到三維路徑部署當(dāng)中，然后利用矢量地圖的數(shù)據(jù)信息，很好的避免了地勢(shì)的阻礙，從而縮短了優(yōu)化搜索時(shí)間，提高了航線(xiàn)校正效率。

經(jīng)典的LPA算法通常被廣泛用在二維航線(xiàn)中，但結(jié)合本文的題目會(huì)把該算法轉(zhuǎn)換到三維航線(xiàn)中，保持和A*方法當(dāng)中的非負(fù)相同啟發(fā)值h(n)，由此可知三角不等式如下所示

(2)

其中，succ(n)表示n的下一個(gè)元素。LPA*算法的估價(jià)值分別為g(n)與rhs(n)，前者與A*用到的含義相同，后者表達(dá)式如式(3)所示

(3)

其中，rhs(n)表示n的父節(jié)點(diǎn)，c(s′，n)表示兩節(jié)點(diǎn)n與s之間的最短距離。當(dāng)g(s′)=rhs(n)時(shí)，說(shuō)明n節(jié)點(diǎn)達(dá)成局部一致性。

LPA算法不僅對(duì)受阻礙的節(jié)點(diǎn)實(shí)施部署，并且要對(duì)所有部署后的飛行節(jié)點(diǎn)進(jìn)行局部一致性核實(shí)，對(duì)于非局部一致性的節(jié)點(diǎn)會(huì)將其轉(zhuǎn)化為初始點(diǎn)，因此當(dāng)進(jìn)行最后一次部署飛行軌跡時(shí)采用的仍是最佳路徑。且通過(guò)rhs(n)對(duì)權(quán)值轉(zhuǎn)變的應(yīng)急能力來(lái)更換增加的節(jié)點(diǎn)。若矢量地圖重新刷新時(shí)，則需檢查節(jié)點(diǎn)的局部是否存在一致性，若不是應(yīng)采用啟發(fā)函數(shù)來(lái)增加節(jié)點(diǎn)，節(jié)點(diǎn)為一致性那么不需要進(jìn)行改變，由此可知當(dāng)權(quán)值發(fā)生變換時(shí)，不必對(duì)全局節(jié)點(diǎn)進(jìn)行更改，從而提高了航線(xiàn)部署和校正效率，算法過(guò)程如下所示：

步驟1：首先讓估價(jià)函數(shù)滿(mǎn)足f(n)=g(n)+h(n)，h(n)選擇此時(shí)位置與終點(diǎn)位置的最短路徑，由給定限制條件可知，要合理選擇參變量S0、α、β、Smax、Hmin、P、與S。更新U表，每個(gè)節(jié)點(diǎn)都要滿(mǎn)足n∈Svertex，制定g(n)=rhs(n)=∞，rhs(Sstart)=0，將Sstart儲(chǔ)存在U表里，從而僅有Sstart不存在一致性，然后不斷運(yùn)轉(zhuǎn)以下步驟。

步驟2：不斷推進(jìn)步驟2到步驟8的運(yùn)行，讓rhs(Sstart)=g(Sstart)，也就是搜索到目標(biāo)節(jié)點(diǎn)，然后從結(jié)束計(jì)算的地方重新向上推算，最終回到初始點(diǎn)，從而能夠獲得從初始點(diǎn)距離終點(diǎn)的極小代價(jià)飛行軌跡。如果U表內(nèi)無(wú)儲(chǔ)存內(nèi)容，那么代表運(yùn)行失敗。

步驟3：在U表中選擇含有極小f函數(shù)的節(jié)點(diǎn)，然后把它轉(zhuǎn)換到Q表中。

步驟4：如果BestVode是最后搜索到的最優(yōu)節(jié)點(diǎn)，那么對(duì)其求解。

步驟5：如果BestVode非最優(yōu)節(jié)點(diǎn)，那么求出g(BestVode)與rhs(BestVode)，如果前者大于后者，那么直接執(zhí)行步驟6，如果前者小于后者，那么跳轉(zhuǎn)到步驟7。

步驟6：讓g(BestVode)=rhs(BestVode)，擴(kuò)大目標(biāo)點(diǎn)位的范圍。橫截面的面積相當(dāng)于轉(zhuǎn)彎夾角的雙倍，同時(shí)根據(jù)運(yùn)行得到目標(biāo)節(jié)點(diǎn)處的位置，采用橫截面投影作為中心線(xiàn)?？v截面的面積相當(dāng)于最長(zhǎng)升降值的雙倍，并垂直于橫截面。擴(kuò)大范圍處的半徑長(zhǎng)短取決于極小步長(zhǎng)S0。BestVode形成P×M個(gè)后繼節(jié)點(diǎn)Successor，最后將所有Successor節(jié)點(diǎn)u按照步驟8運(yùn)行。

步驟7：g(BestVode)=∞，將所有Successor節(jié)點(diǎn)u按照步驟8運(yùn)行。

步驟8：構(gòu)建u重新回到BestVode的方法：若u不能夠滿(mǎn)足Sstart，那么讓rhs(u)=mins′∈pred(u)(g(s′)+c(s′，u))；若u∈U，則把此節(jié)點(diǎn)叫做OLD，通過(guò)對(duì)比新舊航跡的代價(jià)，來(lái)決定OLD節(jié)點(diǎn)的父節(jié)點(diǎn)；若g(u)≠rhs(u)，那么求出u的f大小，看能否實(shí)現(xiàn)極大距離限制與運(yùn)行高度限制；若達(dá)成D(x)+SL(x)≤Smax與矢量中所有點(diǎn)距地面長(zhǎng)度h≥Hmin，那么把u儲(chǔ)存在U表中。

步驟9：觀(guān)察是否有新阻礙產(chǎn)生。如果產(chǎn)生新的阻礙，使用ModifyCost()參數(shù)來(lái)調(diào)整阻礙大小，并用Update-Vertex()參數(shù)來(lái)檢驗(yàn)局部非一致性節(jié)點(diǎn)，如果g(u)≠rhs(u)，那么對(duì)遭到阻礙的節(jié)點(diǎn)采用insert(u)參數(shù)，儲(chǔ)存到U表中。

3.2 基于偏離部署航線(xiàn)的校正算法

無(wú)人機(jī)在完成任務(wù)時(shí)，可能會(huì)受到天氣等因素的影響，導(dǎo)致航線(xiàn)偏離。因此，在已部署好的航線(xiàn)基礎(chǔ)上提出新的三維航線(xiàn)校正方法。在進(jìn)行航線(xiàn)校正前，對(duì)無(wú)人機(jī)進(jìn)行準(zhǔn)確定位是必要條件，定位的主要核心就是通過(guò)路面監(jiān)控站監(jiān)測(cè)無(wú)人機(jī)所在方位，接下來(lái)把方位數(shù)據(jù)利用監(jiān)控站的通訊設(shè)備傳輸給無(wú)人機(jī)，無(wú)人機(jī)會(huì)采用方位數(shù)據(jù)與參變量，通過(guò)卡爾曼濾波方法估價(jià)無(wú)人機(jī)所在方位。

設(shè)該算法在tk時(shí)間的無(wú)人機(jī)三維空間坐標(biāo)為(X(tk)，Y(tk)，Z(tk))，則可推導(dǎo)出以下公式

(4)

路面監(jiān)控站的方位坐標(biāo)用(Xi，Yi，0)來(lái)表示，在飛行過(guò)程中，若監(jiān)控站可以監(jiān)測(cè)出無(wú)人機(jī)此時(shí)的對(duì)應(yīng)方向夾角與高度夾角，而且給出監(jiān)控站的高度，則能夠估價(jià)出無(wú)人機(jī)的空間方位。所以，如果監(jiān)控站的高度等于零，那么可以忽略不計(jì)無(wú)人機(jī)所在高度夾角，由此可以判定無(wú)人機(jī)的飛行高度不會(huì)根據(jù)時(shí)間而發(fā)生改變。φ(tk)表示無(wú)人機(jī)的中心到監(jiān)控站之間的直線(xiàn)與X軸夾角的角度，角度信息是經(jīng)過(guò)監(jiān)控站監(jiān)測(cè)后傳輸?shù)綗o(wú)人機(jī)中，由此可以推導(dǎo)公式為

(5)

式中，εk描述了監(jiān)控站監(jiān)測(cè)無(wú)人機(jī)方位中產(chǎn)生的所有偏差，滿(mǎn)足平均值等于零，差值是σ2的高斯分布。代表根據(jù)時(shí)間發(fā)生改變的元素，但它只在無(wú)人機(jī)不在監(jiān)測(cè)區(qū)域內(nèi)同時(shí)接收到其它監(jiān)控站信息時(shí)產(chǎn)生變化。若無(wú)人機(jī)僅接收此時(shí)監(jiān)控站的信息，那么Ii(tk)=1。

以上公式能夠根據(jù)坐標(biāo)信息與參變量準(zhǔn)確估價(jià)出無(wú)人機(jī)所在方位，成為航線(xiàn)校正的一種主要保障。

根據(jù)3.1的航線(xiàn)部署公式可以推算出，當(dāng)實(shí)際航線(xiàn)受到干擾時(shí)可以表示為

(6)

(7)

本文校正算法在基于無(wú)人機(jī)偏離部署航線(xiàn)上，引入了一種校正因子θj(tk)，用于校正無(wú)人機(jī)的三維航線(xiàn)，其表達(dá)式為

(8)

(9)

航線(xiàn)經(jīng)過(guò)校正后，根據(jù)式(9)可以達(dá)到與部署航線(xiàn)之間的最小方差值。

(10)

(11)

(12)

從而得到：

(13)

通過(guò)綜上所述內(nèi)容，能夠有效校正當(dāng)無(wú)人機(jī)偏離部署航線(xiàn)時(shí)的飛行角度。

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

4.1 實(shí)驗(yàn)參數(shù)設(shè)置

為了檢驗(yàn)本文算法的有效性，該實(shí)驗(yàn)環(huán)境采用編程規(guī)格為VisualC++6.0，矢量地圖尺寸為128×128，同時(shí)加入了阻礙信息。實(shí)驗(yàn)使用的無(wú)人機(jī)為自行組裝的六旋翼無(wú)人機(jī)，GPS選用M8N，相機(jī)采用經(jīng)校驗(yàn)的SonyILCE-6000，水平視場(chǎng)角為36°，垂直視場(chǎng)角為27°。實(shí)驗(yàn)主要技術(shù)參數(shù)如表1所示。

表1 實(shí)驗(yàn)主要技術(shù)參數(shù)

使用無(wú)人機(jī)對(duì)某地拍攝了共 74幅圖像。為說(shuō)明本文方法可行，特選取了九幅正攝圖像，驗(yàn)證本文方法。拍攝圖像如圖1所示。

圖1 航線(xiàn)拍攝圖像

4.2 時(shí)效性分析

在實(shí)驗(yàn)環(huán)境與參變量相同的情況下，根據(jù)無(wú)人機(jī)的GPS 與姿態(tài)信息，軟件處理后給出了相機(jī)位置并計(jì)算得到稀疏點(diǎn)云，如圖2所示。航線(xiàn)上拍攝的圖像相對(duì)位置與所給的航線(xiàn)方案一致。

圖2 稀疏點(diǎn)云

采用SAS算法與本文算法對(duì)比整個(gè)場(chǎng)景建模時(shí)間，結(jié)果如表2所示。能夠看出本文算法在航線(xiàn)校正中所用時(shí)間相對(duì)較短，而且受到的阻礙節(jié)點(diǎn)也較少，從而提高了航線(xiàn)部署和校正效率。

表2 LPA和SAS算法的時(shí)效性對(duì)比分析

4.3 準(zhǔn)確性分析

圖3 無(wú)人機(jī)定位航線(xiàn)示意圖

(X(t1)，Y(t1))=(0，0)

(14)

(15)

由上圖可以看出，此時(shí)無(wú)人機(jī)路徑為平滑可飛，處于最優(yōu)狀態(tài)。圖4對(duì)無(wú)人機(jī)定位航線(xiàn)校正算法進(jìn)行了準(zhǔn)確性檢驗(yàn)。在監(jiān)控站監(jiān)測(cè)范圍內(nèi)，無(wú)人機(jī)能夠按照部署航線(xiàn)飛行，但不在監(jiān)控范圍內(nèi)時(shí)，無(wú)人機(jī)開(kāi)始慢慢偏離部署航線(xiàn)，校正開(kāi)始失效，從而可以看出本文校正方法的準(zhǔn)確性。

圖4 無(wú)人機(jī)航線(xiàn)校正準(zhǔn)確性

綜上所述，基于矢量地圖方法具有較高的時(shí)效性和準(zhǔn)確性，滿(mǎn)足了實(shí)際應(yīng)用需求。主要原因在于引入適量地圖加載，利用LPA航線(xiàn)部署方法分布三維航線(xiàn)點(diǎn)，并通過(guò)檢驗(yàn)局部一致性來(lái)防止重新對(duì)全局進(jìn)行搜索，減少了干擾因素的阻礙與搜索空間范圍，從而提高了校正實(shí)時(shí)性與準(zhǔn)確性。

5 結(jié)論

無(wú)人機(jī)在飛行過(guò)程中容易受到環(huán)境等因素影響，導(dǎo)致偏離航線(xiàn)，所以本文首先提出了算法，它能夠根據(jù)前期搜索樹(shù)上的數(shù)據(jù)提高后期的搜索效率，循環(huán)使用信息以免發(fā)生重復(fù)部署，同時(shí)克服一部分節(jié)點(diǎn)受到阻礙的影響，增強(qiáng)了后續(xù)的航線(xiàn)校正效率?？柭鼮V波算法計(jì)算出無(wú)人機(jī)當(dāng)前位置，最后通過(guò)路面監(jiān)控站傳輸?shù)?/p>

方位數(shù)據(jù)計(jì)算出無(wú)人機(jī)偏離角度，并提出校正因子對(duì)飛行航線(xiàn)進(jìn)行有效校正，實(shí)驗(yàn)結(jié)果證明了該方法的可行性。