基于激光測距傳感器校正四旋翼飛行器姿態(tài)的室內(nèi)組合導(dǎo)航*

鄒 強,付 超,莫申童

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

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

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

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

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

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

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

機體坐標(biāo)系與地理坐標(biāo)系的原點重合,由歐拉旋轉(zhuǎn)定理,通過三次依次圍繞機體坐標(biāo)系x、y、z軸的旋轉(zhuǎn)可以使機體坐標(biāo)系與地理坐標(biāo)系重合,其中繞機體坐標(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)矩陣直觀,計算簡單,但會出現(xiàn)奇異性問題,不能夠全姿態(tài)運行,因此還需要其他的表示方法。

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

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

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

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

(2)

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

(3)

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

(4)

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

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

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

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

3.2 激光測距傳感器測量飛行模型

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

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

(5)

3.2.2 四旋翼飛行器傾斜橫動/縱動飛行

(6)

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

3.2.3 誤差校準(zhǔn)

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

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

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

(7)

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

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

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

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

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

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

圖7 激光測距校正互補濾波與擴展卡爾曼濾波混合算法流程圖

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

(8)

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

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

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

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

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

(9)

則時間更新方程為:

(10)

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

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

wl=(wnxwny0)T

(11)

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

wb=(wbxwbywbz)T

(12)

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

hn=(hxhyhz)T

(13)

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

(14)

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

(15)

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

(16)

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

(17)

計算C′矩陣。

根據(jù)式

(18)

(19)

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

根據(jù)式

(20)

根據(jù)式

(21)

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

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

4 實驗與結(jié)果

4.1 姿態(tài)解算實驗測試平臺

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

圖9 姿態(tài)解算實驗測試平臺與顯示界面

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

4.2 實驗結(jié)果與分析

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

4.2.1 靜態(tài)測試

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

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

4.2.2 水平滑動

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

圖11 水平滑動測試橫滾角估計

4.2.3 動態(tài)測試

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

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

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

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

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

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

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

5 總結(jié)

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