康 雪 ，馬 瑞 ，曹 杰 ，董亮亮 ，徐金欣

（江蘇理工學(xué)院機(jī)械工程學(xué)院，江蘇 常州 213001）

0 引言

無人駕駛飛機(jī)（UAV）是利用無線電遙控設(shè)備和自備的程序控制裝置操控的不載人飛機(jī)，分為軍用和民用兩種?，F(xiàn)在，無人機(jī)技術(shù)已然成熟，被大量運(yùn)用于生活中，在消防救災(zāi)應(yīng)急通信、植物保護(hù)、電力巡檢、快遞運(yùn)輸、警用巡邏等方面均有廣泛應(yīng)用[1-6]。軍用無人機(jī)[7]規(guī)格較大，續(xù)航時(shí)間在60 h以上，對(duì)于目前的無人機(jī)來說，這樣長(zhǎng)的續(xù)航時(shí)間已足夠用于大型的抗災(zāi)、戰(zhàn)爭(zhēng)等方面。而對(duì)于民用無人機(jī)，因?yàn)橐?guī)模較小，所以普遍續(xù)航時(shí)間為10 min~20 min，遠(yuǎn)遠(yuǎn)不能滿足人們?nèi)粘Ｉ畹男枨?。續(xù)航時(shí)間是由無人機(jī)電機(jī)軸數(shù)、電機(jī)轉(zhuǎn)速、負(fù)重、電池電壓、信號(hào)等決定的，其中最重要的是，每次無人機(jī)飛行前都需要把電池充滿，同時(shí)需要在電量見底之前自主降落至指定平臺(tái)，否則無人機(jī)會(huì)因?yàn)殡娏坎蛔銓?dǎo)致墜落損毀?，F(xiàn)在多數(shù)的無人機(jī)精準(zhǔn)降落[8]主要是以視覺定位技術(shù)主導(dǎo)。李海琳等[9]提出一種搭載電池快換裝置的無人機(jī)續(xù)航移動(dòng)基站，這一搭載電池快換裝置的無人機(jī)續(xù)航移動(dòng)基站的定位系統(tǒng)具備機(jī)器視覺和衛(wèi)星-慣性導(dǎo)航組合定位系統(tǒng)雙重定位模塊。陳國棟[10]提出UAV/UGV異構(gòu)系統(tǒng)中的四旋翼自主起降方法。陳至坤等[11]利用機(jī)器視覺系統(tǒng)定點(diǎn)識(shí)別的方法自主精準(zhǔn)降落。于翔[12]驗(yàn)證了以無人機(jī)為中繼進(jìn)行UWB定位的可行性。

基于上述無人機(jī)的研究，續(xù)航時(shí)間問題影響了各項(xiàng)工作的開展。對(duì)此，本文設(shè)計(jì)了一種跟隨降落系統(tǒng)，考慮到救援等任務(wù)在室外進(jìn)行，本文將在GPS定位的基礎(chǔ)上利用新型UWB定位與視覺識(shí)別標(biāo)簽的方法，跟隨移動(dòng)平臺(tái)，實(shí)現(xiàn)準(zhǔn)確跟隨降落。

1 四旋翼降落系統(tǒng)設(shè)計(jì)

1.1 總體設(shè)計(jì)

四旋翼無人機(jī)跟隨移動(dòng)平臺(tái)由四旋翼飛行器與無人駕駛小車構(gòu)成，包含了GPS、UWB、OpenMV等傳感器的定位技術(shù)。無人機(jī)以自身坐標(biāo)系和地面坐標(biāo)系控制方向，以世界坐標(biāo)系{A}為飛行姿態(tài)的參考坐標(biāo)系。其中，無人車作為無人機(jī)的降落平臺(tái)，它的運(yùn)動(dòng)相對(duì)于世界坐標(biāo)系{A}，復(fù)雜的全向運(yùn)動(dòng)可分為平移與轉(zhuǎn)動(dòng)。構(gòu)建無人機(jī)—無人車總體設(shè)計(jì)示意圖，設(shè)四旋翼坐標(biāo)系為{B}，無人車的坐標(biāo)系為{C}，兩者坐標(biāo)系關(guān)系及OpenMV、4個(gè)基站T1、T2、T3、T4與標(biāo)簽T0的位置，如圖1所示。

圖1 無人機(jī)—無人車總體設(shè)計(jì)示意圖

1.2 UWB定位系統(tǒng)設(shè)計(jì)

UWB定位中，飛行時(shí)間根據(jù)雙向測(cè)距飛行時(shí)間法計(jì)算[10]：

基站與標(biāo)簽之間的信號(hào)將作為球體散播，利用球面定位目標(biāo)標(biāo)簽位置，節(jié)點(diǎn)T0與參考節(jié)點(diǎn)T1、T2、T3、T4之間的距離RANGTi(T0)決定以Dij=RANGTI(T0)為半徑、T0為中心的球，位置(Xi,Yi,Zi)=POS(Ti)由中心位于 (T1,···,Tk)，半徑為 (Di1,···,Dik)的k個(gè)球的交點(diǎn)決定。設(shè)4個(gè)位置已知的節(jié)點(diǎn)坐標(biāo)分別為(x1,y1,z1)、(x2,y2,z2)、(x3,y3,z3)、(x4,y4,z4)，目標(biāo)標(biāo)簽的坐標(biāo)p為(x,y,z)。則有以下關(guān)系成立：

求解式（2）方程組可算出目標(biāo)節(jié)點(diǎn)的坐標(biāo)參數(shù)，但由于會(huì)產(chǎn)生系統(tǒng)與隨機(jī)誤差，造成位置不精準(zhǔn)。因此，本文先計(jì)算飛行時(shí)間差TDOA，再利用三維幾何關(guān)系、Taylor級(jí)數(shù)算法確定未知節(jié)點(diǎn)(T0)的位置坐標(biāo)。

三維Taylor級(jí)數(shù)算法從假設(shè)初值(x(0),y(0),z(0))開始計(jì)算，設(shè)未知節(jié)點(diǎn)坐標(biāo)(x,y,z)，其他錨節(jié)點(diǎn)的位置為上述4個(gè)已知節(jié)點(diǎn)坐標(biāo)。則未知節(jié)點(diǎn)到第i個(gè)錨節(jié)點(diǎn)的位置Di為[13]：

式中，c代表光速。

未知節(jié)點(diǎn)位置坐標(biāo)誤差(Δx,Δy,Δz)為：

式中，Q是到達(dá)時(shí)間差TDOA的協(xié)方差矩陣。

令x=x(0)、y=y(0)、z=z(0)代入矩陣（5）中，算出D1、D2、D3、D4的值，用x(0)+Δx代替x的值，y(0)+Δy代替y，z(0)+Δz代替z，直到步驟循環(huán)到Δx、?y、?z足夠小，即求得目標(biāo)標(biāo)簽(x,y,z)。

1.3 機(jī)器視覺處理

為確保識(shí)別到完整的標(biāo)簽內(nèi)容，精簡(jiǎn)實(shí)驗(yàn)，達(dá)到識(shí)別降落的效果，本文采用OpenMV識(shí)別H字符的形式進(jìn)行識(shí)別降落。與AprilTag二維碼識(shí)別功能類似，H字符識(shí)別通過Sensor模塊拍攝，如圖2所示。

圖2 OpenMV識(shí)別H字符降落

識(shí)別成功后發(fā)送至無人機(jī)處理端，為確定無人機(jī)與無人車之間的位置關(guān)系，以無人機(jī)上的OpenMV為傳感器計(jì)算兩者坐標(biāo)差。當(dāng)相機(jī)光心投影于圖像坐標(biāo)系時(shí)，相機(jī)的位置在圖像上以像素點(diǎn)形式存在。利用OpenMV識(shí)別生成的矩形框，可得到中心點(diǎn)像素坐標(biāo)p(u1,v1)，此時(shí)坐標(biāo)為相機(jī)于圖像上的坐標(biāo)。設(shè)無人車上H字符的圖像坐標(biāo)為q(u2,v2)，未識(shí)別到H字符時(shí)，無人機(jī)通過UWB定位，識(shí)別到字符時(shí)改用OpenMV識(shí)別，此時(shí)利用p(u1,v1)-q(u2,v2)計(jì)算得到相機(jī)與字符的坐標(biāo)差，即可獲得無人機(jī)與無人車的坐標(biāo)差。

1.4 數(shù)據(jù)融合處理

為使無人機(jī)定位更精準(zhǔn)，本系統(tǒng)采用基于經(jīng)典卡爾曼濾波的多傳感器信息融合處理。離散時(shí)間線性卡爾曼濾波模型主要根據(jù)系統(tǒng)上時(shí)刻的狀態(tài)估計(jì)值與當(dāng)前時(shí)刻的狀態(tài)測(cè)量值估計(jì)當(dāng)前狀態(tài)，由此得到更為準(zhǔn)確的位置估計(jì)值。因此，可用最小方差無偏原則估計(jì)得到當(dāng)前的狀態(tài)估計(jì)值，卡爾曼濾波的計(jì)算方程組為：

式中，XK|K∈Rn為K時(shí)刻的狀態(tài)向量；XK|K-1∈Rn為K時(shí)刻的觀測(cè)向量；UK-1∈Rn為K-1時(shí)刻的控制向量；ΦK-1∈Rn×n為K-1時(shí)刻到K時(shí)刻的系統(tǒng)轉(zhuǎn)移矩陣，ΦK-1是K-1時(shí)刻不變的常數(shù)矩陣；HK-1∈Rm×n為K-1時(shí)刻的觀測(cè)矩陣。

卡爾曼數(shù)據(jù)融合處理步驟如圖3所示。

圖3 卡爾曼濾波融合處理步驟

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

測(cè)量無人車與無人機(jī)兩者尺寸，無人車長(zhǎng)100 cm、寬55 cm、高28 cm，降落平臺(tái)面積為5 500 cm2，無人機(jī)長(zhǎng)28.7 cm、寬28.3 cm、高10.5 cm。對(duì)飛行數(shù)據(jù)進(jìn)行卡爾曼濾波計(jì)算前，先將數(shù)據(jù)分組處理，無人機(jī)在三維中軌跡運(yùn)動(dòng)分為x軸、y軸、z軸，分別得到三軸數(shù)據(jù)后對(duì)其進(jìn)行卡爾曼濾波，濾波結(jié)果如圖4所示。

圖4 分成三軸方向?qū)?shù)據(jù)卡爾曼濾波處理結(jié)果

對(duì)三軸分別卡爾曼濾波處理，對(duì)比處理前后的飛行軌跡，經(jīng)過卡爾曼濾波處理后的無人機(jī)飛行與預(yù)期位置的偏移更少，系統(tǒng)卡爾曼濾波前后誤差對(duì)比如圖5所示。

圖5 系統(tǒng)卡爾曼濾波前后誤差對(duì)比

在分別對(duì)無人機(jī)卡爾曼濾波后，無人機(jī)的偏移誤差降低。配合移動(dòng)平臺(tái)，卡爾曼濾波前的飛行軌跡與卡爾曼濾波后的飛行軌跡對(duì)比如圖6所示。

圖6 無人機(jī)—無人車系統(tǒng)的卡爾曼濾波處理前后軌跡對(duì)比

可見，卡爾曼濾波后的飛行軌跡與小車軌跡接近，實(shí)現(xiàn)了無人機(jī)跟隨移動(dòng)平臺(tái)，提高了跟隨移動(dòng)平臺(tái)飛行的精確度，減少了無人機(jī)在飛行過程中因誤差與自然因素導(dǎo)致飛行出現(xiàn)偏移的狀況，為后續(xù)無人機(jī)降落于無人車上提供了更高的精準(zhǔn)度，使其安全性更高。

將無人車放置于地面上，啟動(dòng)程序，讓無人車在固定范圍內(nèi)自主移動(dòng)，接著釋放無人機(jī)，兩者距離相差較遠(yuǎn)時(shí)，無人機(jī)根據(jù)GPS快速導(dǎo)引，定位無人機(jī)的大致位置，中段切換UWB導(dǎo)引，對(duì)無人車采用球面定位，近距離時(shí)利用OpenMV識(shí)別H標(biāo)簽算法定位，如圖7所示。

圖7 多段制導(dǎo)示意圖

3 結(jié)語

四旋翼無人機(jī)相對(duì)于其他型號(hào)無人機(jī)，姿態(tài)與飛行模式轉(zhuǎn)換的可操控性更強(qiáng)。本文介于航空母艦的應(yīng)用場(chǎng)景下，通過多段定位制導(dǎo)結(jié)合，建設(shè)無人機(jī)能自主跟隨移動(dòng)小車的平臺(tái)。與傳統(tǒng)定位系統(tǒng)GPS相比，本文增加了UWB定位與OpenMV導(dǎo)航降落，通過串口通信，將不同階段的位置信息傳給飛控，利用卡爾曼濾波分階段融合數(shù)據(jù)，達(dá)到多段制導(dǎo)的目的，實(shí)現(xiàn)更高精度的降落。