多傳感器融合在無人機室內三維定位中的應用

陳博 李擎

1.北京信息科技大學自動化學院智能控制研究所，北京 100192；2.北京信息科技大學高動態導航實驗室，北京 100192

0 前言

近年來，人們對基于室內定位的無人機檢測探查技術有很大的需求，因此，如何提高室內定位精度是一個熱門話題。

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

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

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

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

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

1 系統結構

本文所使用的室內無人機定位系統由UWB定位系統、上位PC機、樹莓派和無人機組成，通信方式主要使用串口和網絡通信，基本結構如圖1所示。機載飛控Pixhawk與機載樹莓派之間采用有線串口通信，Pixhawk將基本的姿態和傳感器數據發送給樹莓派，樹莓派可以通過串口與UWB系統標簽連接，獲取無人機定位數據，完成控制計算后將電機控制指令發送給Pixhawk，實現四旋翼的基本控制。本文主要研究其中的控制、定位問題。

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

2 無人機數學模型

具體建模方法可參考文獻[16]，本章只簡單介紹建模過程中使用的公式和實驗系統的實際參數。

本實驗中，無人機機體坐標系定義X為飛機中心水平向前，Y為垂直X軸水平向右，Z軸為垂直XY平面豎直向下。電機定義與旋轉方向如圖3所示，1號和2號電機逆時針旋轉，3號和4號電機順時針旋轉。

四旋翼無人機的動力學系統模型為：

其中，——電機的轉速；

σ——油門信號（0＜σ＜1）；

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

電機控制效率模型為：

其中，f——4個螺旋槳拉力的合力；

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

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

τz——Z軸的反扭矩；

d——無人機中心到任一電機旋轉軸的距離，為常參數；

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

機體姿態與位置的運動和動力學模型為：

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

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

其中，qeb——四元數；

bω——機體角速度。

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

bω——無人機在機體系下的角速度；

fb——無人機在機體系下的合力；

m——無人機質量，為常參數；

g——重力加速度，為常參數；

R——從慣性系到機體系的旋轉矩陣；

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

f——4個電機的拉力；

Cd——無人機的阻力系數，為常參數（具體數值見表1）。

其中，J——無人機的轉動慣量矩陣，為常參數；

Ga——陀螺力矩；

τ——螺旋槳在機體軸上的力矩；

JRP——螺旋槳的轉動慣量，為常參數；

Ki——螺旋槳的旋轉在機體系下的方向，與電機旋轉方向和機體坐標系定義有關，在本系統中，K1=K2=-1，K3=K4=1。

其余參數數值見表1。

表1 常參數值

3 UWB定位模型

UWB定位系統本質上是一種通信過程，由基站向帶有標簽的目標物體發送信號，記錄下信號飛行時間，根據ToF測距原理，測得基站到目標（標簽）的距離，再根據空間中已知的基站坐標和測得的各個基站到目標的距離，通過最小二乘法計算出目標相對于定位系統的坐標信息，從而實現定位效果。

在實驗室環境下建立如圖4所示的UWB定位系統。其中，A0、A1、A2、A3位于同一水平高度的矩形平面上，且分別代表定位基站，T代表標簽位置，標簽放置在無人機上。d0、d1、d2、d3分別代表標簽距各個基站的距離。

因此可以建立坐標系，并以A0為原點，為x軸，為y軸，過A0且垂直xoy平面為z軸，且向上為正方向。為方便與IMU數據融合，根據右手定則，將坐標系排列成東-北-天（E-N-U）。x軸正方向為東方向，y軸正方向為北方向，z軸正方向為天空方向。

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

將（8）展開可得：

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

其中：

此時，可以把問題轉化為矩陣相乘，將式（10）寫成矩陣形式，得：

根據矩陣除法，求得：

4 IMU姿態解算和誤差分析

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

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

假設在點P處輸出的比力寫為：

其中，f pb——在點P處輸出的比力；

——在飛機質心處輸出的比力；

——機身框架相對于慣性框架的轉角速率；

下標i——相對于慣性系的導數；

下標b——相對于身體架的導數。

從方程（14）可以得到：

其中：

則式（16）可以寫為：

公式（17）右側的第一項是切線加速度，右側的第二項是向心加速度。根據公式（17），δfb向北東地坐標系變換如下：

其中，——從載體坐標系到導航坐標系的變換矩陣。

其中，j、θ、V——方位角、俯仰角和滾動角。

輸出速度可表示為：

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

ωie——地球自轉角速度；

ωen——位置角速度；

g——地球重力加速度。

經度、緯度和高度的變化率分別為

其中，j、w、h——經度、緯度和高度；

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

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

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

其中，fn——慣性導航系統承受的比力；

——地球自轉角速度在導航坐標系的投影；

——系統坐標系相對地球坐標系的轉動角速度在導航坐標系下的投影；

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

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

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

δpn——位置誤差；

Ψ——姿態誤差。

具體的IMU解算過程可參考文獻[17]。

5 無跡卡爾曼濾波

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

5.1 系統建模

設系統的狀態方程和觀測方程為：

其中，X(k) ——系統k時刻的狀態量；

fk-1(X(k-1)) ——上一時刻系統狀態到這一時刻系統狀態的非線性關系；

w(k-1) ——k-1時刻的系統噪聲，對應的協方差矩陣為Qk；

z(k) ——k時刻系統的觀測量；

hk(X(k)) ——k時刻狀態量被觀測的非線性關系；

v(k) ——k時刻的觀測噪聲，對應的協方差矩陣為Rk。

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

根據慣導原理，可得狀態轉換關系為：

對于觀測模型，根據圖4所示的UWB定位系統的幾何關系可以很容易地得到：

其中，(xk,yk,zk) ——標簽在k時刻所對應系統坐標系的坐標；

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

5.2 數據融合

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

第一步，初始化狀態量。對于位置狀態量ζP，在靜止狀態下，取100個UWB輸出的標簽到基站的距離，取均值算出坐標后作為ζP的初值；靜止狀態時，ζv為0；靜止狀態取100組加速度計數據，根據公式計算出航向角α、姿態角β、滾轉角λ作為狀態量初值：

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

Hx、Hy、Hz——磁力計3個軸的測量值。

初始協方差矩陣可以設為單位陣。

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

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

第三步，根據公式計算σ點的：

第四步，根據公式計算σ點和Pk｜k-1通過量測方程對xk的傳播：

第五步，根據公式計算輸出的一步提前預測：

第六步，獲得新的量測值后，進行濾波更新：

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

6 實驗與分析

6.1 環境搭建

為驗證本文所提出的室內三維定位方法在實際中的定位效果與精確性，設計了實驗進行驗證，實驗地點為學校內高動態導航實驗室（5 m×4 m×3 m），如圖6所示。實驗所選設備如圖7所示。

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

6.2 實驗設計

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

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

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

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

可以看出，定位的精度和準確性有明顯提高，具有更高的可靠性。圖12為UKF后在x、y軸上的定位圖。圖13為UKF數據融合后在x、y、z軸上的定位效果圖。可以看出，在與IMU融合定位后，在角落原本UWB測得數據偏差大的位置處，已經可以達到與設定位置十分精確的位置，總體精度達到了10 cm，導航誤差為0.7%。在該定位方法下，室內定位精度得到了顯著提升，且具有較強的魯棒性。

7 結束語

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