鄒 強(qiáng),付 超,莫申童

(1.天津大學(xué)微電子學(xué)院,天津 300072;2.天津市物聯(lián)網(wǎng)國(guó)際聯(lián)合研究中心,天津 300072)

由于慣性導(dǎo)航系統(tǒng)INS(Inertial Navigation System)陀螺儀、加速度計(jì)等傳感器的自身測(cè)量誤差,還會(huì)受到機(jī)體震動(dòng)、溫度等多方面影響,以致測(cè)量數(shù)據(jù)不精準(zhǔn)[1]。同時(shí),四旋翼飛行器在室外環(huán)境下,可以借助INS與全球定位系統(tǒng)GPS(Global Position System)實(shí)現(xiàn)自主導(dǎo)航[2],但在室內(nèi)環(huán)境下,無(wú)法利用GPS實(shí)現(xiàn)精準(zhǔn)定位,因此需要可行的導(dǎo)航技術(shù),實(shí)現(xiàn)室內(nèi)精準(zhǔn)導(dǎo)航。

采用更優(yōu)的算法或多傳感器融合是研究方向之一,即在原有慣性測(cè)量傳感器的基礎(chǔ)上融合更多姿態(tài)傳感器的姿態(tài)信息,選擇最優(yōu)的在線數(shù)據(jù)融合策略,從而得到最優(yōu)的組合導(dǎo)航[3-4]。文獻(xiàn)[5]利用二維激光雷達(dá)和慣性導(dǎo)航系統(tǒng)構(gòu)成微小型無(wú)人機(jī)MAV(Micro Aerial Vehicle)室內(nèi)組合導(dǎo)航方案,采用基于擴(kuò)展卡爾曼濾波(EKF)的D&C同步定位與構(gòu)圖技術(shù)(SLAM)實(shí)現(xiàn)定位和構(gòu)圖;文獻(xiàn)[6]How等使用單目視覺(jué)對(duì)室內(nèi)環(huán)境進(jìn)行角點(diǎn)特征提取,根據(jù)角點(diǎn)匹配執(zhí)行基于擴(kuò)展卡爾曼濾波EKF(Extended Kalman Filter)的SLAM算法(EKF SLAM),得到MAV的位置估計(jì),同時(shí)構(gòu)造一個(gè)低精度的3D地圖。雖然EKF執(zhí)行效率很高,但是圖像處理運(yùn)算量較大,無(wú)法滿足MAV對(duì)導(dǎo)航的實(shí)時(shí)性要求;文獻(xiàn)[7]Bachrach等提出了一種大范圍室內(nèi)導(dǎo)航方案。該方案以二維激光雷達(dá)為傳感器,采用基于粒子濾波器PF(Particle Filter)的GMapping算法進(jìn)行定位,具有較高的定位精度,但是PF-SLAM的耗時(shí)較長(zhǎng),也無(wú)法滿足MAV對(duì)導(dǎo)航的實(shí)時(shí)性要求;文獻(xiàn)[8]Grzonka等在MAV的室內(nèi)導(dǎo)航方案中使用慣性測(cè)量模塊直接輸出姿態(tài)角,但并未充分利用速度進(jìn)行組合濾波,無(wú)法保證長(zhǎng)時(shí)間的導(dǎo)航精度;文獻(xiàn)[9]Adam Bry等為了對(duì)攻擊性飛行提供精確的狀態(tài)估計(jì),將高斯粒子濾波器擴(kuò)展到允許將激光測(cè)距傳感器定位與卡爾曼濾波器相結(jié)合;文獻(xiàn)[10]針對(duì)室內(nèi)定位,文獻(xiàn)提出了一種融合可視光通信和基于粒子濾波器的慣性導(dǎo)航的融合定位系統(tǒng),與傳統(tǒng)慣性導(dǎo)航相比,此融合系統(tǒng)定位精度要提高2倍～4倍;文獻(xiàn)[11]提出了一種共軛梯度優(yōu)化方法與互補(bǔ)濾波相結(jié)合的數(shù)據(jù)融合策略,克服了梯度下降法的不足,彌補(bǔ)了牛頓法計(jì)算量大的缺點(diǎn);文獻(xiàn)[12]針對(duì)顯示互補(bǔ)濾波中PI參數(shù)只能試湊的問(wèn)題,提出了自適應(yīng)顯示互補(bǔ)濾波,實(shí)時(shí)調(diào)整PI參數(shù),提高了姿態(tài)估計(jì)的精度。

本文提出一種基于激光測(cè)距傳感器校正四旋翼飛行器姿態(tài)的室內(nèi)組合導(dǎo)航,在短時(shí)間內(nèi)依靠INS系統(tǒng),長(zhǎng)時(shí)間內(nèi)通過(guò)激光測(cè)距傳感器校正姿態(tài),通過(guò)激光測(cè)距傳感器所測(cè)數(shù)據(jù)進(jìn)行數(shù)學(xué)變換來(lái)估算飛行器的姿態(tài)參數(shù),將所估算的姿態(tài)參數(shù)與經(jīng)過(guò)互補(bǔ)濾波后的姿態(tài)數(shù)據(jù)通過(guò)擴(kuò)展卡爾曼濾波融合,該室內(nèi)組合導(dǎo)航結(jié)合了互補(bǔ)濾波與擴(kuò)展卡爾曼濾波的優(yōu)點(diǎn),在靜態(tài)時(shí)能夠抑制姿態(tài)角漂移和濾出噪聲,動(dòng)態(tài)時(shí)能夠快速跟蹤姿態(tài)的變化,提高了姿態(tài)角的解算精度。本文詳細(xì)給出了上述方法的實(shí)現(xiàn)過(guò)程,并通過(guò)實(shí)驗(yàn)驗(yàn)證了方法的有效性。

1 姿態(tài)角的描述

四旋翼飛行器的坐標(biāo)系由機(jī)體坐標(biāo)系和地理坐標(biāo)系組成,兩者關(guān)系如圖1所示。

圖1 地理坐標(biāo)系與機(jī)體坐標(biāo)系的相對(duì)關(guān)系

機(jī)體坐標(biāo)系與地理坐標(biāo)系的原點(diǎn)重合,由歐拉旋轉(zhuǎn)定理,通過(guò)三次依次圍繞機(jī)體坐標(biāo)系x、y、z軸的旋轉(zhuǎn)可以使機(jī)體坐標(biāo)系與地理坐標(biāo)系重合,其中繞機(jī)體坐標(biāo)系x、y、z軸旋轉(zhuǎn)的角度分別為橫滾角φ、俯仰角θ、偏航角ψ。

圖2 三次坐標(biāo)旋轉(zhuǎn)得到歐拉角

按照z-y-x的旋轉(zhuǎn)順序轉(zhuǎn)換坐標(biāo)系,得到如下的姿態(tài)旋轉(zhuǎn)矩陣:

R=

(1)

由公式可以看出,由歐拉角方法表示的姿態(tài)矩陣直觀,計(jì)算簡(jiǎn)單,但會(huì)出現(xiàn)奇異性問(wèn)題,不能夠全姿態(tài)運(yùn)行,因此還需要其他的表示方法。

2 四元數(shù)姿態(tài)解算

描述姿態(tài)旋轉(zhuǎn),除了歐拉角方法,還有方向余弦矩陣、四元數(shù)等,方向余弦法的姿態(tài)微分矩陣方程中包含了9個(gè)未知參數(shù),計(jì)算量大,實(shí)時(shí)性困難;而四元數(shù)描述方法包含了四軸飛行器所有的姿態(tài)信息,有效的避免了奇異性問(wèn)題,而且只有4個(gè)未知數(shù),方程解算簡(jiǎn)單,是目前四軸飛行器采用最廣泛的方式。

四元數(shù)(Quaternions)是由愛(ài)爾蘭數(shù)學(xué)家哈密頓(William Rowan Hamilton,1805-1865)在1843年發(fā)明的數(shù)學(xué)概念。四元數(shù)由4個(gè)元組成,其復(fù)數(shù)形式為:

Q=q0+q1i+q2j+q3k=[q0q1q2q3]T

(2)

通過(guò)四元數(shù)推導(dǎo)得到由機(jī)體坐標(biāo)系(b系)到地理坐標(biāo)系(n系)的旋轉(zhuǎn)矩陣為:

(3)

對(duì)比姿態(tài)描述矩陣式(2)與式(3),可以得到四元數(shù)與歐拉角的對(duì)應(yīng)關(guān)系:

(4)

圖3 標(biāo)準(zhǔn)飛行下激光測(cè)距傳感器獲取姿態(tài)角原理圖

3 激光測(cè)距傳感器校正姿態(tài)參數(shù)的混合濾波算法

3.1 激光測(cè)距傳感器混合濾波算法的基本思路

激光測(cè)距傳感器的抗干擾性強(qiáng)于陀螺儀、加速度計(jì)以及磁力計(jì)等慣性傳感器,光信號(hào)不含或含有少量的噪音,不需要設(shè)計(jì)專用的數(shù)字濾波器。陀螺儀具有良好的動(dòng)態(tài)特性,產(chǎn)生的數(shù)據(jù)含有低頻噪音;而加速度計(jì)和磁力計(jì)具有良好的靜態(tài)特性,產(chǎn)生的數(shù)據(jù)含有高頻噪音。因此通過(guò)互補(bǔ)濾波分別設(shè)置高通濾波器和低通濾波器濾除低頻和高頻干擾,方便融合3個(gè)傳感器,提高測(cè)量精度和動(dòng)態(tài)特性。激光測(cè)距傳感器獲取四軸飛行器相對(duì)地面的感知,經(jīng)過(guò)解算之后得到飛行器的姿態(tài)參數(shù),再通過(guò)擴(kuò)展卡爾曼濾波算法,將激光測(cè)距傳感器與慣性傳感器的數(shù)據(jù)融合,得到更為精確的導(dǎo)航信息。

3.2 激光測(cè)距傳感器測(cè)量飛行模型

3.2.1 四旋翼飛行器水平標(biāo)準(zhǔn)飛行

激光測(cè)距傳感器測(cè)量得到距離參數(shù)l1、l2,而兩個(gè)激光測(cè)距傳感器之間的距離l已知,得姿態(tài)角計(jì)算公式:

(5)

3.2.2 四旋翼飛行器傾斜橫動(dòng)/縱動(dòng)飛行

(6)

圖4 傾斜橫動(dòng)/縱動(dòng)飛行下激光測(cè)距傳感器獲取姿態(tài)角原理圖

3.2.3 誤差校準(zhǔn)

圖5為圖4的右視圖,如下所示。

圖5 俯仰角θ大小與l2′-l1′關(guān)系l2′-l1′

由于飛行模型復(fù)雜,本文采用

(7)

式中激光測(cè)距參數(shù)與俯仰角的關(guān)系來(lái)粗略校準(zhǔn)。

圖6為四旋翼飛行器飛行模型全姿態(tài)示意圖,激光測(cè)距傳感器在具有穩(wěn)態(tài)誤差的基礎(chǔ)上進(jìn)行測(cè)量,測(cè)量后的數(shù)據(jù)需要再校準(zhǔn)后才能得到真實(shí)的姿態(tài)角。

激光測(cè)距傳感器得到修正后的橫滾角速度、俯仰角、偏航角數(shù)據(jù),對(duì)其微分,得到角速度wl。

3.3 基于擴(kuò)展卡爾曼濾波算法融合INS與激光測(cè)距傳感器數(shù)據(jù)

激光測(cè)距校正姿態(tài)角的互補(bǔ)濾波與卡爾曼濾波混合算法示意圖如圖7所示。

圖6 四旋翼飛行器飛行模型全姿態(tài)示意圖

圖7 激光測(cè)距校正互補(bǔ)濾波與擴(kuò)展卡爾曼濾波混合算法流程圖

將姿態(tài)向量誤差e輸入到PI控制器與陀螺儀測(cè)量的角速度數(shù)據(jù)相融合,得到經(jīng)過(guò)加速度計(jì)和磁力計(jì)修正過(guò)的角速度ω′為:

(8)

姿態(tài)四元數(shù)為狀態(tài)量的卡爾曼濾波狀態(tài)方程是非線性的,因此在無(wú)人機(jī)飛控系統(tǒng)的數(shù)字處理器中主要采用擴(kuò)展卡爾曼濾波算法。

圖8 擴(kuò)展卡爾曼濾波算法預(yù)測(cè)更新流程圖

通過(guò)擴(kuò)展卡爾曼濾波將激光測(cè)距傳感器得到的角速度wl與INS的角速度w′融合。

EKF的預(yù)測(cè)更新流程圖如圖8所示。

用INS的輸出計(jì)算四元數(shù)關(guān)于時(shí)間的離散狀態(tài)方程:

(9)

則時(shí)間更新方程為:

(10)

進(jìn)行下一時(shí)刻的數(shù)據(jù)測(cè)量,讀入激光測(cè)距傳感器觀測(cè)值,即觀測(cè)更新方程Z(k):

以z軸為參考系,則激光測(cè)距傳感器以地理坐標(biāo)系為參考的角速度矢量:

wl=(wnxwny0)T

(11)

在機(jī)體坐標(biāo)系中,激光測(cè)距傳感器的測(cè)量值為:

wb=(wbxwbywbz)T

(12)

因?yàn)闃?biāo)準(zhǔn)角速度向量無(wú)法測(cè)量,將式(12)轉(zhuǎn)換到地理坐標(biāo)系n系,得到預(yù)測(cè)角速度向量:

hn=(hxhyhz)T

(13)

式(11)與(13)有以下關(guān)系:

(14)

當(dāng)機(jī)體處于靜止或勻速直線運(yùn)動(dòng)時(shí),以上空間向量滿足如下關(guān)系:

(15)

以式(12)為測(cè)量模型,測(cè)量值為6個(gè)激光測(cè)距傳感器組成的3軸角速度數(shù)據(jù),展開(kāi)如下:

(16)

(3)讀入四元數(shù)的數(shù)據(jù),根據(jù)式

(17)

計(jì)算C′矩陣。

根據(jù)式

(18)

(19)

得到此時(shí)的濾波增益矩陣H(k)。

根據(jù)式

(20)

根據(jù)式

(21)

計(jì)算下一時(shí)刻的協(xié)方差P(k),當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí),P(k)就是式(18)的P(k-1),這樣卡爾曼濾波算法就可以自回歸的運(yùn)算下去。

經(jīng)過(guò)擴(kuò)展卡爾曼濾波后四元數(shù)旋轉(zhuǎn)矩陣得到修正,代入到式(4)中,求解出橫滾角φ、俯仰角θ、偏航角ψ。

4 實(shí)驗(yàn)與結(jié)果

4.1 姿態(tài)解算實(shí)驗(yàn)測(cè)試平臺(tái)

本節(jié)簡(jiǎn)單介紹姿態(tài)解算實(shí)驗(yàn)平臺(tái),在實(shí)驗(yàn)平臺(tái)上檢測(cè)帶有激光測(cè)距傳感器及慣性系統(tǒng)的組合導(dǎo)航的靜態(tài)特性、水平滑動(dòng)和動(dòng)態(tài)特性。

圖9 姿態(tài)解算實(shí)驗(yàn)測(cè)試平臺(tái)與顯示界面

實(shí)驗(yàn)平臺(tái)采用ALIENTEK開(kāi)發(fā)板,主控制器采用STM32F103進(jìn)行數(shù)據(jù)處理,姿態(tài)傳感器由MPU6050(三軸陀螺儀和三軸加速度計(jì))和三軸電子羅盤(pán)HMC5883L組成,用于采集機(jī)體角速度、加速度以及磁場(chǎng)強(qiáng)度數(shù)據(jù),姿態(tài)校正傳感器由6個(gè)激光測(cè)距傳感器組成,激光測(cè)距采用的是ST公司的VL53L0X模塊,用于測(cè)量機(jī)體的姿態(tài)角,在高速模式下,采樣頻率為50 Hz。為保證水平,在采樣前需要對(duì)激光測(cè)距傳感器進(jìn)行校準(zhǔn)。顯示界面采用匿名四軸上位機(jī)。

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

姿態(tài)解算時(shí),姿態(tài)傳感器模塊MPU6050、HMC5883L以及激光測(cè)距模塊通過(guò)I2C協(xié)議實(shí)現(xiàn)與主控制器之間的通信,主控制器獲取實(shí)時(shí)的加速度、角速度以及磁場(chǎng)強(qiáng)度數(shù)據(jù)進(jìn)行姿態(tài)解算得到相對(duì)準(zhǔn)確的角速度值,再由激光測(cè)距模塊對(duì)該角速度值進(jìn)行校正,最終獲得姿態(tài)角數(shù)據(jù),通過(guò)串口調(diào)試助手發(fā)送到上位機(jī)中,可顯示激光測(cè)距傳感器、陀螺儀等的測(cè)量數(shù)據(jù)和姿態(tài)角波形,并可將測(cè)量數(shù)據(jù)導(dǎo)出到EXCEL中進(jìn)一步分析。

4.2.1 靜態(tài)測(cè)試

將姿態(tài)解算實(shí)驗(yàn)平臺(tái)置于水平臺(tái)面上,理想的橫滾角應(yīng)該為0°。當(dāng)姿態(tài)解算平臺(tái)處于靜止時(shí),單獨(dú)使用加速度計(jì)時(shí)輸出的橫滾角如圖10(a)所示,在5 s內(nèi)最大誤差在0.35以內(nèi)。使用陀螺儀時(shí)(修正后)輸出的橫滾角如圖10(b)所示,在5 s內(nèi)最大誤差在0.18以內(nèi)。而單獨(dú)使用激光測(cè)距傳感器時(shí),實(shí)驗(yàn)平臺(tái)輸出的橫滾角在5 s內(nèi)靜態(tài)誤差基本控制在0.05附近,成一條水平的直線。由此可以得出結(jié)論,激光測(cè)距傳感器在靜態(tài)測(cè)試中具有較好的濾除作用。

圖10 靜態(tài)測(cè)試橫滾角估計(jì)

4.2.2 水平滑動(dòng)

針對(duì)加速度無(wú)法區(qū)分運(yùn)動(dòng)加速度和重力加速度,激光測(cè)距傳感器工作原理能夠區(qū)分運(yùn)動(dòng)加速度和重力加速度,因此本文設(shè)計(jì)水平滑動(dòng)實(shí)驗(yàn),將姿態(tài)解算平臺(tái)放置在水平桌面上做快速滑動(dòng)處理,理論上俯仰角和橫滾角輸出角度為0°,但I(xiàn)NS系統(tǒng)下,橫滾角誤差達(dá)到了22°左右。經(jīng)過(guò)本文設(shè)計(jì)的激光測(cè)距傳感器校正姿態(tài)后,誤差控制在7°。實(shí)驗(yàn)驗(yàn)證了水平滑動(dòng)狀態(tài)下,激光測(cè)距校正加速度數(shù)據(jù)缺陷的有效性。

圖11 水平滑動(dòng)測(cè)試橫滾角估計(jì)

4.2.3 動(dòng)態(tài)測(cè)試

動(dòng)態(tài)測(cè)試分為姿態(tài)變化較慢與姿態(tài)變化較快兩種情況,5 s內(nèi)采用激光測(cè)距傳感器校正四旋翼飛行器姿態(tài)的混合濾波算法(以下簡(jiǎn)稱激光測(cè)距混合濾波算法)后的橫滾角曲線如圖12所示。

從圖12可以看出,姿態(tài)變化較慢時(shí)采用激光測(cè)距混合濾波算法后的姿態(tài)角曲線更加平穩(wěn),產(chǎn)生的波動(dòng)較小,橫滾角偏差最大值不超過(guò)1°。實(shí)驗(yàn)結(jié)果表明,激光測(cè)距混合濾波算法能夠有效的修正陀螺儀的漂移特性而產(chǎn)生的積分累積誤差,提高姿態(tài)解算的精度。

圖12 姿態(tài)變化較慢時(shí)橫滾角估計(jì)

但當(dāng)運(yùn)動(dòng)角度過(guò)大時(shí),如圖13所示,激光測(cè)距組合導(dǎo)航系統(tǒng)產(chǎn)生的誤差逐漸增大,這是由于激光測(cè)距自身性能限制,角度過(guò)大時(shí),激光測(cè)距接收器不能接收所發(fā)射的光子,采用性能好的激光測(cè)距傳感器會(huì)避免這一點(diǎn)。同時(shí),由于本文激光測(cè)距測(cè)量數(shù)據(jù)修正方法較為粗略,對(duì)誤差有一定的影響。

圖13 姿態(tài)變化較慢時(shí)橫滾角誤差估計(jì)

為進(jìn)一步驗(yàn)證基于激光測(cè)距混合濾波算法的有效性,本文模擬了姿態(tài)變化劇烈的情況。如圖14所示,在平穩(wěn)階段,激光測(cè)距混合濾波算法幾乎沒(méi)有延遲,能夠很好地實(shí)現(xiàn)姿態(tài)的跟蹤。在姿態(tài)變化劇烈的時(shí)候,采用激光測(cè)距混合濾波算法有一定的延遲,與采用擴(kuò)展卡爾曼濾波方式有關(guān)。

圖14 姿態(tài)變化劇烈時(shí)橫滾角估計(jì)對(duì)比圖

5 總結(jié)

本文針對(duì)室內(nèi)環(huán)境下無(wú)法使用GPS實(shí)現(xiàn)精準(zhǔn)導(dǎo)航的問(wèn)題,提出一種基于激光測(cè)距傳感器校正四旋翼飛行器姿態(tài)的組合導(dǎo)航,并結(jié)合姿態(tài)解算算法建立了姿態(tài)解算實(shí)驗(yàn)測(cè)試平臺(tái),對(duì)該組合方式進(jìn)行實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)結(jié)果表明,基于激光測(cè)距傳感器校正姿態(tài)組合導(dǎo)航算法,能夠使姿態(tài)解算平臺(tái)具有較好的靜態(tài)特性、水平滑動(dòng)和動(dòng)態(tài)特性,有效的修正陀螺儀的漂移特性而產(chǎn)生的積分累積誤差,提高了導(dǎo)航的精度,而且滿足姿態(tài)解算的實(shí)時(shí)性,但由于激光測(cè)距傳感器測(cè)量姿態(tài)原理自身缺陷造成角度過(guò)大時(shí)誤差增大,還需要進(jìn)一步研究?jī)?yōu)化,修正誤差,而且僅限用于室內(nèi)環(huán)境下。激光測(cè)距傳感器組合導(dǎo)航對(duì)未來(lái)將激光測(cè)距傳感器單獨(dú)作為姿態(tài)解算傳感器做了基礎(chǔ)性研究,有較好的工程應(yīng)用前景。