陳博 李擎

1.北京信息科技大學(xué)自動(dòng)化學(xué)院智能控制研究所，北京 100192；2.北京信息科技大學(xué)高動(dòng)態(tài)導(dǎo)航實(shí)驗(yàn)室，北京 100192

0 前言

近年來，人們對(duì)基于室內(nèi)定位的無人機(jī)檢測探查技術(shù)有很大的需求，因此，如何提高室內(nèi)定位精度是一個(gè)熱門話題。

最開始，研究人員提出了一些局部定位系統(tǒng)（Local Positioning System，LPS），例如：文獻(xiàn)[1]將無線保真（Wireless Fidelity，WiFi）定位技術(shù)應(yīng)用到復(fù)雜的室內(nèi)環(huán)境中；文獻(xiàn)[2]采用射頻識(shí)別（Radio Frequency Identification，RFID）標(biāo)簽進(jìn)行室內(nèi)定位。盡管基于WiFi和RFID的LPS技術(shù)已經(jīng)在室內(nèi)環(huán)境中廣泛使用，但是它們只能提供米級(jí)的定位精度。

為了獲取更高的定位精度，研究人員把目光投向超寬帶（Ultra Wide Band，UWB）技術(shù)。文獻(xiàn)[3]闡述了超寬帶室內(nèi)定位技術(shù)的分析和最新進(jìn)展；文獻(xiàn)[4]分析比較了基于信號(hào)到達(dá)時(shí)間差的3種定位算法，最好的泰勒算法二維平面定位精度能到10 cm以內(nèi)；文獻(xiàn)[5]使用了粒子濾波和多路徑補(bǔ)償算法，將三維定位精度提高到了20 cm，但使用了16個(gè)基站；文獻(xiàn)[6]使用了8個(gè)基站，在三維空間中精度仍然大于20 cm。

需要指出的是，單獨(dú)使用超寬帶的定位精度并不能滿足室內(nèi)高精度運(yùn)行的要求。IMU是一種常用的機(jī)器人姿態(tài)估計(jì)傳感器，具有精度高、更新頻率快、體積小等優(yōu)點(diǎn)，然而，IMU是通過積分來估計(jì)其位置的，存在累積誤差，另外，長時(shí)間運(yùn)行后，位置估計(jì)會(huì)發(fā)生漂移。文獻(xiàn)[7]中，使用非線性濾波融合UWB和IMU，得到了厘米級(jí)的汽車二維位置估計(jì)；文獻(xiàn)[8]中，UWB和IMU通過改進(jìn)的EKF濾波算法融合得到了更準(zhǔn)確的行人姿態(tài)估計(jì)，在周長40 m的封閉軌跡內(nèi)實(shí)現(xiàn)小于4%的全局誤差；文獻(xiàn)[9]中，利用EKF和神經(jīng)網(wǎng)絡(luò)，將UWB和IMU融合后得到的非視距誤差0.43 m，視距下為0.12 m；文獻(xiàn)[10]中，利用粒子濾波融合IMU和UWB，得到精度10 cm的定位效果，其問題主要是粒子濾波計(jì)算量大，所需時(shí)間過長；文獻(xiàn)[11]中，在組合定位系統(tǒng)中建立了INS / UWB緊耦合模型，實(shí)現(xiàn)0.24 m以內(nèi)的定位精度。

常用的多傳感器融合算法包括：擴(kuò)展卡爾曼濾波（Extended Kalman Filtering，EKF）[12]、無跡卡爾曼濾波（Unscented Kalman Filtering，UKF）[13]、粒子濾波（Particle Filtering，PF）[14]等，以及文獻(xiàn)[15]提出的單基站和腳安裝的行人軌跡推算系統(tǒng)組合定位關(guān)鍵算法。超寬帶定位系統(tǒng)模型和IMU模型都是非線性高斯模型。擴(kuò)展卡爾曼濾波和無跡卡爾曼濾波雖然可以處理非線性高斯模型，但它們的估計(jì)精度不同。本文使用姿態(tài)角作為狀態(tài)量，更加減少了濾波過程中的計(jì)算量。

因此，本文考慮了超寬帶和IMU的信息融合概念設(shè)計(jì)。利用IMU對(duì)目標(biāo)進(jìn)行預(yù)測和超寬帶定位系統(tǒng)抑制IMU積分產(chǎn)生的累積誤差，以獲得高精度的位置估計(jì)。此外，提出了一種基于無跡卡爾曼濾波的超寬帶數(shù)據(jù)的設(shè)計(jì)方法來獲取四旋翼飛行器的位置信息。實(shí)驗(yàn)結(jié)果表明，本文提出的融合設(shè)計(jì)方法在四旋翼無人機(jī)路徑規(guī)劃中有較高的定位精度，且計(jì)算速度快很多。

1 系統(tǒng)結(jié)構(gòu)

本文所使用的室內(nèi)無人機(jī)定位系統(tǒng)由UWB定位系統(tǒng)、上位PC機(jī)、樹莓派和無人機(jī)組成，通信方式主要使用串口和網(wǎng)絡(luò)通信，基本結(jié)構(gòu)如圖1所示。機(jī)載飛控Pixhawk與機(jī)載樹莓派之間采用有線串口通信，Pixhawk將基本的姿態(tài)和傳感器數(shù)據(jù)發(fā)送給樹莓派，樹莓派可以通過串口與UWB系統(tǒng)標(biāo)簽連接，獲取無人機(jī)定位數(shù)據(jù)，完成控制計(jì)算后將電機(jī)控制指令發(fā)送給Pixhawk，實(shí)現(xiàn)四旋翼的基本控制。本文主要研究其中的控制、定位問題。

在只有4個(gè)定位基站的前提下，三維定位的精度（主要是高度上的精度）較差，且UWB本身定位效果受到非視距誤差的影響非常大，因此，本文所用無人機(jī)上還安裝了光流傳感器和慣性測量單元IMU。系統(tǒng)控制流程框圖如圖2所示。首先，利用光流傳感器輸出的高度值修正UWB測得的高度值，得到較好的三維位置信息，在視距場景時(shí)，使用三維位姿信息修正IMU的積累誤差，在非視距場景時(shí)，使用IMU慣性測量單元短時(shí)間內(nèi)所得精確位姿信息彌補(bǔ)UWB測得的位置信息，通過多傳感器數(shù)據(jù)的融合互補(bǔ)，得到任何場景下精確的三維位置信息，提供給無人機(jī)進(jìn)行后續(xù)一系列任務(wù)。

2 無人機(jī)數(shù)學(xué)模型

具體建模方法可參考文獻(xiàn)[16]，本章只簡單介紹建模過程中使用的公式和實(shí)驗(yàn)系統(tǒng)的實(shí)際參數(shù)。

本實(shí)驗(yàn)中，無人機(jī)機(jī)體坐標(biāo)系定義X為飛機(jī)中心水平向前，Y為垂直X軸水平向右，Z軸為垂直XY平面豎直向下。電機(jī)定義與旋轉(zhuǎn)方向如圖3所示，1號(hào)和2號(hào)電機(jī)逆時(shí)針旋轉(zhuǎn)，3號(hào)和4號(hào)電機(jī)順時(shí)針旋轉(zhuǎn)。

四旋翼無人機(jī)的動(dòng)力學(xué)系統(tǒng)模型為：

其中，——電機(jī)的轉(zhuǎn)速；

σ——油門信號(hào)（0＜σ＜1）；

Tm、CR、——常參數(shù)（具體數(shù)值見表1）。

電機(jī)控制效率模型為：

其中，f——4個(gè)螺旋槳拉力的合力；

cT——拉力系數(shù)，為常參數(shù)（具體數(shù)值見表1）。

其中，τx、τy——X和Y軸的力矩；

τz——Z軸的反扭矩；

d——無人機(jī)中心到任一電機(jī)旋轉(zhuǎn)軸的距離，為常參數(shù)；

cM——扭矩系數(shù)，為常參數(shù)（具體數(shù)值見表1）。

機(jī)體姿態(tài)與位置的運(yùn)動(dòng)和動(dòng)力學(xué)模型為：

其中，ep——慣性系下的位置；

eυ——慣性系下的速度。

其中，qeb——四元數(shù)；

bω——機(jī)體角速度。

其中，bv——無人機(jī)在機(jī)體系下的速度；

bω——無人機(jī)在機(jī)體系下的角速度；

fb——無人機(jī)在機(jī)體系下的合力；

m——無人機(jī)質(zhì)量，為常參數(shù)；

g——重力加速度，為常參數(shù)；

R——從慣性系到機(jī)體系的旋轉(zhuǎn)矩陣；

e3——慣性系的z軸矢量[0 0 1]T；

f——4個(gè)電機(jī)的拉力；

Cd——無人機(jī)的阻力系數(shù)，為常參數(shù)（具體數(shù)值見表1）。

其中，J——無人機(jī)的轉(zhuǎn)動(dòng)慣量矩陣，為常參數(shù)；

Ga——陀螺力矩；

τ——螺旋槳在機(jī)體軸上的力矩；

JRP——螺旋槳的轉(zhuǎn)動(dòng)慣量，為常參數(shù)；

Ki——螺旋槳的旋轉(zhuǎn)在機(jī)體系下的方向，與電機(jī)旋轉(zhuǎn)方向和機(jī)體坐標(biāo)系定義有關(guān)，在本系統(tǒng)中，K1=K2=-1，K3=K4=1。

其余參數(shù)數(shù)值見表1。

表1 常參數(shù)值

3 UWB定位模型

UWB定位系統(tǒng)本質(zhì)上是一種通信過程，由基站向帶有標(biāo)簽的目標(biāo)物體發(fā)送信號(hào)，記錄下信號(hào)飛行時(shí)間，根據(jù)ToF測距原理，測得基站到目標(biāo)（標(biāo)簽）的距離，再根據(jù)空間中已知的基站坐標(biāo)和測得的各個(gè)基站到目標(biāo)的距離，通過最小二乘法計(jì)算出目標(biāo)相對(duì)于定位系統(tǒng)的坐標(biāo)信息，從而實(shí)現(xiàn)定位效果。

在實(shí)驗(yàn)室環(huán)境下建立如圖4所示的UWB定位系統(tǒng)。其中，A0、A1、A2、A3位于同一水平高度的矩形平面上，且分別代表定位基站，T代表標(biāo)簽位置，標(biāo)簽放置在無人機(jī)上。d0、d1、d2、d3分別代表標(biāo)簽距各個(gè)基站的距離。

因此可以建立坐標(biāo)系，并以A0為原點(diǎn)，為x軸，為y軸，過A0且垂直xoy平面為z軸，且向上為正方向。為方便與IMU數(shù)據(jù)融合，根據(jù)右手定則，將坐標(biāo)系排列成東-北-天（E-N-U）。x軸正方向?yàn)闁|方向，y軸正方向?yàn)楸狈较颍瑉軸正方向?yàn)樘炜辗较颉?/p>

設(shè)T的坐標(biāo)為（x,y,z），A0坐標(biāo)為（x0,y0,z0），A1坐標(biāo)為（x1,y1,z1），A2坐標(biāo)為（x2,y2,z2），A3坐標(biāo)為(x3,y3,z3)，A0、A1、A2、A3坐標(biāo)已知，則根據(jù)圖4的定位模型可以得到：

將（8）展開可得：

用第一行分別減去后面三行可得：

其中：

此時(shí)，可以把問題轉(zhuǎn)化為矩陣相乘，將式（10）寫成矩陣形式，得：

根據(jù)矩陣除法，求得：

4 IMU姿態(tài)解算和誤差分析

本文選取東-北-天（E-N-U）為導(dǎo)航坐標(biāo)系，記為n系；X-Y-Z為載體坐標(biāo)系，記為b系。

圖5給出了導(dǎo)航系與載體系的關(guān)系。設(shè)OXYZ為導(dǎo)航坐標(biāo)系，ObXbYbZb代表載體坐標(biāo)系，其中Ob為載體質(zhì)心。假設(shè)，IMU和UWB標(biāo)簽分別安裝在載體上的P點(diǎn)和Q點(diǎn)，這里R表示IMU與飛機(jī)質(zhì)心之間的距離，r為IMU與UWB標(biāo)簽之間的偏移量，因此，R被認(rèn)為是一個(gè)不確定的杠桿臂，而r是一個(gè)確定的杠桿臂（一個(gè)偏移量）。

假設(shè)在點(diǎn)P處輸出的比力寫為：

其中，f pb——在點(diǎn)P處輸出的比力；

——在飛機(jī)質(zhì)心處輸出的比力；

——機(jī)身框架相對(duì)于慣性框架的轉(zhuǎn)角速率；

下標(biāo)i——相對(duì)于慣性系的導(dǎo)數(shù)；

下標(biāo)b——相對(duì)于身體架的導(dǎo)數(shù)。

從方程（14）可以得到：

其中：

則式（16）可以寫為：

公式（17）右側(cè)的第一項(xiàng)是切線加速度，右側(cè)的第二項(xiàng)是向心加速度。根據(jù)公式（17），δfb向北東地坐標(biāo)系變換如下：

其中，——從載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的變換矩陣。

其中，j、θ、V——方位角、俯仰角和滾動(dòng)角。

輸出速度可表示為：

其中，f b——加速度計(jì)輸出的比力；

ωie——地球自轉(zhuǎn)角速度；

ωen——位置角速度；

g——地球重力加速度。

經(jīng)度、緯度和高度的變化率分別為

其中，j、w、h——經(jīng)度、緯度和高度；

vE、vN、vU——東-北-天（E-N-U）方向上的速度；

Rn、Rm——地球橫向、縱向曲率半徑。

慣性測量單元的誤差模型為：

其中，fn——慣性導(dǎo)航系統(tǒng)承受的比力；

——地球自轉(zhuǎn)角速度在導(dǎo)航坐標(biāo)系的投影；

——系統(tǒng)坐標(biāo)系相對(duì)地球坐標(biāo)系的轉(zhuǎn)動(dòng)角速度在導(dǎo)航坐標(biāo)系下的投影；

——陀螺儀輸出的角速度誤差。

上述誤差模型忽略哥氏項(xiàng)誤差和重力矢量誤差，可以簡化為：

其中，δvn——速度誤差；

δpn——位置誤差；

Ψ——姿態(tài)誤差。

具體的IMU解算過程可參考文獻(xiàn)[17]。

5 無跡卡爾曼濾波

與擴(kuò)展卡爾曼濾波（EKF）不同，無跡卡爾曼濾波（UKF）并不對(duì)非線性方程f和h在估計(jì)點(diǎn)處做線性化逼近，而是利用無跡變換在估計(jì)點(diǎn)附近確定采樣，用這些樣本點(diǎn)表示高斯密度近似狀態(tài)的概率密度函數(shù)。這種近似實(shí)質(zhì)是一種統(tǒng)計(jì)近似而且求解，從而不會(huì)引入線性化帶來的誤差，濾波效果更好。因此，本文選用的融合方法為無跡卡爾曼濾波。

5.1 系統(tǒng)建模

設(shè)系統(tǒng)的狀態(tài)方程和觀測方程為：

其中，X(k) ——系統(tǒng)k時(shí)刻的狀態(tài)量；

fk-1(X(k-1)) ——上一時(shí)刻系統(tǒng)狀態(tài)到這一時(shí)刻系統(tǒng)狀態(tài)的非線性關(guān)系；

w(k-1) ——k-1時(shí)刻的系統(tǒng)噪聲，對(duì)應(yīng)的協(xié)方差矩陣為Qk；

z(k) ——k時(shí)刻系統(tǒng)的觀測量；

hk(X(k)) ——k時(shí)刻狀態(tài)量被觀測的非線性關(guān)系；

v(k) ——k時(shí)刻的觀測噪聲，對(duì)應(yīng)的協(xié)方差矩陣為Rk。

本文選取的狀態(tài)量為11維，如下公式所示，其中，ζP、ζv、ζΨ為位置誤差、速度誤差、姿態(tài)誤差在導(dǎo)航坐標(biāo)系 （E-N-U坐標(biāo)系）上的三維分量（其中，姿態(tài)Ψ包括航向角α、姿態(tài)角β、滾轉(zhuǎn)角λ）；ba為加速度計(jì)的偏置矢量；bg為陀螺儀的偏置矢量。

根據(jù)慣導(dǎo)原理，可得狀態(tài)轉(zhuǎn)換關(guān)系為：

對(duì)于觀測模型，根據(jù)圖4所示的UWB定位系統(tǒng)的幾何關(guān)系可以很容易地得到：

其中，(xk,yk,zk) ——標(biāo)簽在k時(shí)刻所對(duì)應(yīng)系統(tǒng)坐標(biāo)系的坐標(biāo)；

n0(k)、n1(k)、n2(k)、n3(k) ——標(biāo)簽到4個(gè)基站距離的測量噪聲。

5.2 數(shù)據(jù)融合

無跡卡爾曼濾波算法如下：

第一步，初始化狀態(tài)量。對(duì)于位置狀態(tài)量ζP，在靜止?fàn)顟B(tài)下，取100個(gè)UWB輸出的標(biāo)簽到基站的距離，取均值算出坐標(biāo)后作為ζP的初值；靜止?fàn)顟B(tài)時(shí)，ζv為0；靜止?fàn)顟B(tài)取100組加速度計(jì)數(shù)據(jù)，根據(jù)公式計(jì)算出航向角α、姿態(tài)角β、滾轉(zhuǎn)角λ作為狀態(tài)量初值：

其中，Ax、Ay、Az——加速度計(jì)的x軸、y軸、z軸的加速度值；

Hx、Hy、Hz——磁力計(jì)3個(gè)軸的測量值。

初始協(xié)方差矩陣可以設(shè)為單位陣。

第二步，計(jì)算σ點(diǎn)的。σ為采樣點(diǎn)。依據(jù)xk-1｜k-1和pk-1｜k-1生成 2n+1 個(gè)σ點(diǎn)，i=0,1,…,2n，n為狀態(tài)量的維數(shù)，本文n為11。無跡變換時(shí)，取尺度參數(shù)α=0.01，κ=0，β=2。根據(jù)公式計(jì)算采樣點(diǎn)的權(quán)重：

其中，λ=α2(n+κ)-n。

第三步，根據(jù)公式計(jì)算σ點(diǎn)的：

第四步，根據(jù)公式計(jì)算σ點(diǎn)和Pk｜k-1通過量測方程對(duì)xk的傳播：

第五步，根據(jù)公式計(jì)算輸出的一步提前預(yù)測：

第六步，獲得新的量測值后，進(jìn)行濾波更新：

更新完成后的狀態(tài)量與協(xié)方差矩陣可以直接帶入到下一次的迭代過程中，完成無跡卡爾曼濾波過程。

6 實(shí)驗(yàn)與分析

6.1 環(huán)境搭建

為驗(yàn)證本文所提出的室內(nèi)三維定位方法在實(shí)際中的定位效果與精確性，設(shè)計(jì)了實(shí)驗(yàn)進(jìn)行驗(yàn)證，實(shí)驗(yàn)地點(diǎn)為學(xué)校內(nèi)高動(dòng)態(tài)導(dǎo)航實(shí)驗(yàn)室（5 m×4 m×3 m），如圖6所示。實(shí)驗(yàn)所選設(shè)備如圖7所示。

其中，UWB定位系統(tǒng)采用Nooploop公司研發(fā)的LinkTrack系列產(chǎn)品，基站選用的是LinkTrack S型號(hào)，標(biāo)簽選用的是LinkTrack SS型號(hào)，廠家手冊(cè)所標(biāo)二維精度為20 cm，三維精度在30 cm左右。4個(gè)基站固定好后，分別用USB連接到一起，接入PC端即可。無人機(jī)選用的是北京靈思創(chuàng)奇科技有限公司的成品無人機(jī)，飛控使用的是Pixhawk和樹莓派，修改了部分控制器程序，改用內(nèi)外環(huán)控制方法，使其能在實(shí)驗(yàn)室場地內(nèi)平穩(wěn)飛行。光流傳感器選用的是匿名科技下的產(chǎn)品，可輸出根據(jù)攝像頭計(jì)算后的高度信息。還有機(jī)載飛控中的IMU慣性測量單元，可輸出無人機(jī)的位置、姿態(tài)信息。

6.2 實(shí)驗(yàn)設(shè)計(jì)

實(shí)驗(yàn)方案設(shè)計(jì)：對(duì)無人機(jī)設(shè)定傾斜向上的封閉圓形軌跡，并讓無人機(jī)沿著設(shè)定軌跡飛行3圈再落地。設(shè)計(jì)了2組實(shí)驗(yàn)，分別采用純UWB定位和普通濾波函數(shù)處理后得到的飛行路線，以及采用EKF融合多傳感器得到飛行路線，用不同定位方式比較定位效果。

第一組：利用純UWB進(jìn)行定位飛行，結(jié)果如圖8所示。圖中黑色軌跡為設(shè)定路線，藍(lán)色軌跡為UWB測定的路徑，紅色軌跡為將UWB測得數(shù)據(jù)進(jìn)行普通卡爾曼濾波平滑處理后的軌跡。

從圖8中可以明顯看出，純UWB定位的誤差較大，且波動(dòng)較大，不穩(wěn)定，經(jīng)過普通濾波處理后，軌跡雖然平滑了許多，但還是與設(shè)定軌跡有一定程度上的偏差，這主要是由于UWB信號(hào)在角落處發(fā)生非視距傳播導(dǎo)致的。圖9為純UWB在x、y平面定位效果圖。圖10為純UWB在x、y、z軸上的定位效果圖?？梢钥闯?，在UWB單獨(dú)定位時(shí)，誤差在25 cm左右，與商家提供的產(chǎn)品手冊(cè)中精度一致。

第二組：利用無跡卡爾曼濾波融合UWB和IMU的測量值，在無遮擋、信號(hào)好時(shí)，利用UWB的精確位置修正IMU由于時(shí)間長帶來的累積誤差，在角落UWB信號(hào)不佳時(shí)，用IMU的短期準(zhǔn)確導(dǎo)航信息來彌補(bǔ)UWB由于非視距帶來的誤差，并結(jié)合光流傳感器進(jìn)行高度修正，結(jié)果如圖11所示，藍(lán)色為設(shè)定路線，紅色為實(shí)際路線。

可以看出，定位的精度和準(zhǔn)確性有明顯提高，具有更高的可靠性。圖12為UKF后在x、y軸上的定位圖。圖13為UKF數(shù)據(jù)融合后在x、y、z軸上的定位效果圖?？梢钥闯?，在與IMU融合定位后，在角落原本UWB測得數(shù)據(jù)偏差大的位置處，已經(jīng)可以達(dá)到與設(shè)定位置十分精確的位置，總體精度達(dá)到了10 cm，導(dǎo)航誤差為0.7%。在該定位方法下，室內(nèi)定位精度得到了顯著提升，且具有較強(qiáng)的魯棒性。

7 結(jié)束語

通過實(shí)物仿真實(shí)驗(yàn)驗(yàn)證，本文提出的基于無跡卡爾曼濾波的多傳感器數(shù)據(jù)融合內(nèi)外環(huán)控制室內(nèi)定位方法具有較好的定位精度，能使無人機(jī)系統(tǒng)快速達(dá)到穩(wěn)定且擁有較強(qiáng)的魯棒性，定位精度從原先的25 cm提高到了10 cm以內(nèi)，導(dǎo)航誤差為總路程的0.7%，相比純UWB定位誤差12.4%，誤差減小了11.7%；相比UWB和普通卡爾曼濾波數(shù)據(jù)平滑處理定位誤差7.1%，誤差減小了6.4%。該方法具有良好的導(dǎo)航精度，極大改善了原有設(shè)備的定位精度，使其能夠滿足四旋翼無人機(jī)在室內(nèi)進(jìn)行相關(guān)任務(wù)時(shí)的定位需求。