鄒 強,付 超,莫申童

(1.天津大學微電子學院,天津 300072;2.天津市物聯(lián)網(wǎng)國際聯(lián)合研究中心,天津 300072;3.天津市成像與感知微電子技術(shù)重點實驗室,天津 300072;4.天津大學青島海洋技術(shù)研究院,山東 青島 300072)

由于慣性導航系統(tǒng)INS(Inertial Navigation System)陀螺儀、加速度計等傳感器的自身測量誤差,還會受到機體震動、溫度等多方面影響,以致測量數(shù)據(jù)不精準[1]。同時,當四軸飛行器在城市環(huán)境或室內(nèi)環(huán)境下,GPS信號受到干擾甚至丟失,無法利用GPS實現(xiàn)精準定位,因此需要一種全新的室內(nèi)導航技術(shù),實現(xiàn)室內(nèi)精準導航[2]。

現(xiàn)有的研究已經(jīng)表明,將一組互補傳感器提供的數(shù)據(jù)進行融合是一種能夠完全估計無人機狀態(tài)參數(shù)以及感知周圍環(huán)境的適用方法[3-4]。Kelly J等提出了組合立體視覺測程法和慣性測量的無人機導航系統(tǒng)。無人機的位置和姿態(tài)由兩個傳感器給出的運動估計經(jīng)擴展卡爾曼濾波器融合得到,由導航系統(tǒng)提供的信息經(jīng)離線分析,以評估點對點導航任務(wù)性能[5]。Croke P等給出了應(yīng)用于飛行器的視覺和慣性傳感器系統(tǒng)相結(jié)合的實例。在該實例中,視覺和慣性傳感器系統(tǒng)用于對小型直升機進行實時控制。地面高度信息和自運動引起的光流由一對下視攝像機提供?；パa濾波器用于融合從慣性和視覺傳感器得到的速度信息[6]。文獻[7]中發(fā)現(xiàn)關(guān)于無人機視覺定位的有趣結(jié)果。Blosch等利用單目視覺系統(tǒng)實現(xiàn)實時定位與地圖建模SLAM(Simultaneous Localization And Mapping)算法。試驗結(jié)果表明,該算法能對室內(nèi)飛行的四旋翼無人機進行精準的定位。文獻[8]提出了一種視覺伺服系統(tǒng),通過四個圓形標志,對四軸飛行器進行位置控制。Ahrens S等通過一組跟蹤特征對四軸飛行器自身進行估計,能夠?qū)崿F(xiàn)環(huán)境感知和躲避障礙物[9]?？滋旌愕壤枚S激光雷達和慣性導航系統(tǒng)構(gòu)成微小型無人機MAV(Micro Aerial Vehicle)室內(nèi)組合導航方案,采用基于擴展卡爾曼濾波EKF(Extended Kalman Filter)的D&C同步定位與構(gòu)圖技術(shù)(SLAM)實現(xiàn)定位和構(gòu)圖[10]。

本文提出一種基于卡爾曼濾波的四軸飛行器成像、慣性與高度組合導航,使四軸飛行器實現(xiàn)自主起飛、相對定位、導航和著陸。根據(jù)人造地面標志,通過相對姿態(tài)估計算法來確定飛行器的位置,利用光流算法來調(diào)節(jié)飛行器的運動速度。實驗結(jié)果表明,基于互補濾波的四軸飛行器成像、慣性與高度組合導航能夠?qū)︼w行器平移運動狀態(tài)進行精確估計,對飛行器的三維軌跡進行有效重建。

1 攝像機模型與標定

機載視覺傳感器的配備,使四軸飛行器獲取周圍環(huán)境信息以及自身信息的原始方法得以發(fā)展。然而,在使用視覺傳感器之前,必須建立數(shù)學模型,用于描述三維空間點與二維圖像點之間的對應(yīng)關(guān)系。

攝像機模型如圖1所示,其中OXcYcZc為攝像機坐標系,(xcam,ycam)為圖像坐標系,(x,y)為圖像像素坐標系,物體空間點X在攝像機坐標系下被映射到圖像坐標系x的轉(zhuǎn)換關(guān)系為:

(1)

令

(2)

式中:(px,py)為圖像坐標系中的主點坐標,K為攝像機標定矩陣,由攝像機的投影矩陣P的RQ分解導出,而P由給定足夠多的三維點X與二維圖像x的對應(yīng)點確定。

圖1 攝像機模型

圖2 氧化鋁標定板

原則上,任意帶有適當特征的物體都可以作為標定物,但最符合實際的選擇是規(guī)則圖案,如棋盤。

棋盤特征點的測量誤差和圖像中相對應(yīng)角點的提取誤差與相機標定的誤差成正相關(guān)性[11],且棋盤格尺寸較小的靶標,相機的標定誤差較小[12]。本文建立了精度為0.01 mm的180 mm×180 mm棋盤格,其中包含相互交替且尺寸為15 mm×15 mm的黑白方格,如圖2所示。

通過MATLAB攝像機標定工具箱可以精準的計算出攝像機的內(nèi)外參數(shù),為了獲得較高的精度,拍攝了20幅不同方位和位置的棋盤圖像,如圖3所示。圖4展示了標定工具的圖像序列。

圖3 攝像機與靶標之間關(guān)系

圖4 MATLAB GUI中攝像機標定工具箱中的棋盤圖案

利用工具箱計算出的OV7275攝像機的參數(shù)矩陣K為

(3)

若要根據(jù)圖像信息得出飛行器在世界坐標系下的位置和姿態(tài),還需要建立機體坐標系到世界坐標系的關(guān)系,世界坐標系與攝像機坐標系之間可通過一次旋轉(zhuǎn)和一次平移聯(lián)系起來。

(4)

世界坐標系與機體坐標系之間的轉(zhuǎn)換矩陣R為:

(5)

式中:φ為橫滾角、θ為俯仰角、ψ為偏航角。

圖5 序號2圖像外參數(shù):平移矩陣和轉(zhuǎn)換矩陣的計算

以圖像5為實例,測得平移矩陣T為:

T=[-53.569 795 -46.582 296 244.833 651]

(6)

轉(zhuǎn)換矩陣R為:

(7)

通過上述原理和方法,求出攝像機相對于標志物的位置和姿態(tài)后,四軸飛行器在空中相對于著陸標志的位置和姿態(tài)也就唯一確定了。

2 著陸標志設(shè)計

實現(xiàn)四軸飛行器三維位置控制需要著陸平臺或地面人工標志,本文設(shè)計了一種著陸標志如圖7所示。

圖6 基于視覺的位置穩(wěn)定原理圖

圖7 四個圓環(huán)著陸標志

對于四軸飛行器在空中懸停的每一個時刻,利用已知四個圓圓心位置的先驗知識,可以計算出單應(yīng)性矩陣H。利用該估計轉(zhuǎn)換矩陣和基于文獻[13]方法離線辨識校準的攝像機內(nèi)參數(shù)矩陣,可以計算出攝像機的外參數(shù),得到飛行器相對于著陸平臺的位置(x,y,z)。識別流程如下:

Step 1 Hough圓形檢測 利用OpenCV Hough函數(shù)可以檢測到圖像中的每一個圓,根據(jù)圓的半徑大小進行分類,可以實現(xiàn)對著陸平臺坐標的正確辨識。第一個圓對應(yīng)右下圓,將左下圓作為列表中的第二個圓,然后依據(jù)半徑大小確定為右上圓,最終左上圓被辨識為半徑較大的圓。圖8顯示了從攝像機中觀測到的著陸平臺圖像,四個圓環(huán)根據(jù)其半徑大小用紫色圓環(huán)凸顯出來。

圖8 單應(yīng)性圖像估計:四個圓環(huán)的檢測和顏色識別

Step 2 圓環(huán)排序 必須忽略著陸平臺圓環(huán)的錯誤檢測,否則將提供一個錯誤的位置估計。因此,總平臺檢測到的四個圓環(huán)圓心的顏色必須被確認。圖8顯示著陸平臺四個顏色不同的圓環(huán)。四個圓環(huán)按半徑從大到小依次為深藍色、淺藍色、紅色、黃色。

Step 3 顏色確認 圖9為四個圓環(huán)圓心顏色識別流程,通過攝像頭采集到著陸標志的圖像,利用Hough變換檢測到圓,并確定圓心坐標,將彩色圖像由RGB模型轉(zhuǎn)換為HSV模型,設(shè)定上述顏色的閾值,在此設(shè)定閾值內(nèi),將圖像二值化,再次檢測圓心坐標的顏色是否為白色,識別出顏色。

每次四個圓環(huán)的檢測,首先要經(jīng)過以上兩項約束確認,保證一個好的平面單應(yīng)性被估計,目的是獲得較好的攝像機外參數(shù)計算。

圖9 四個圓環(huán)圓心顏色識別

3 視覺、慣性與高度數(shù)據(jù)融合

3.1 成像系統(tǒng)相對高度估計

利用文獻[14]提出的方法,攝像機固定在四軸飛行器上,因此相對高度的估計就是計算著陸標志在攝像機坐標系下的位置坐標。圖10為相對高度估計的原理圖,由相似三角形定理求出高度h的值。

圖10 高度、焦距和標志特征點位置的幾何對應(yīng)關(guān)系

圖中f/d為攝像機焦距的長度,由攝像機標定得出。P1,P2是世界坐標系下的兩個特征點,I1,I2是其在圖像坐標系下的映射點,由相似三角形定理,可以得出下式:

(8)

計算出任意兩個特征點P1,P2的坐標以及它們映射點的像素坐標,就可以測量出|P1P2|的物理距離,并得出|I1I2|的像素距離,從而得到相對高度h。

3.2 Lucas-Kanade金字塔光流法估計位移速度

假設(shè)攝像機相對于著陸標志相對運動,在慣性坐標系下的速度和自轉(zhuǎn)速度分別表示為(VxVyVz)和(wxwywz)。為了準確估計四軸飛行器的偽速度,在著陸平臺定義一個跟蹤區(qū)域,該區(qū)域中心與著陸平臺中心重合。通過OpenCV函數(shù)來檢測著陸標志上最具代表性的特征。本文選擇4個圓環(huán)的圓周為跟蹤特征。在圖11整個圖像上執(zhí)行基于OpenCV光流算法的跟蹤過程,并測量被跟蹤特征的位移。

圖11 光流估計相對平移速度

稠密光流對圖像中每個像素點都進行計算,運算時間較長,考慮到四軸飛行器的快速運動要求導航系統(tǒng)具有實時性,因此本文采用稀疏光流對圖像中的模式運動進行運算。

圓周光流平均值可以表示成如下運動過程:

OFx=VOFx+PxVOFz+IOFx
OFy=VOFx+PyVOFz+IOFy

(9)

利用文獻[15]對轉(zhuǎn)動光流進行補償,并推導偽速度(VOFxVOFyVOFz)。由于攝像機安裝并固定在四軸飛行器上沒有自由度,因此,推導的偽速度取決于四軸飛行器的運動?？梢詫懗?

(10)

當四軸飛行器做等高飛行時,通過式(10),本文提出的光流視覺系統(tǒng)就可以按照一定的比例因子實現(xiàn)對四軸飛行器的速度估計,從而來控制飛行器的平移速度。

3.3 卡爾曼濾波器融合數(shù)據(jù)

為了能夠精確估計四軸飛行器的位置和速度,本文通過卡爾曼濾波算法將視覺、慣性和高度數(shù)據(jù)進行融合。

圖12 視覺、慣性與高度數(shù)據(jù)融合過程

狀態(tài)更新向量定義為

(11)

觀測更新向量定義為

Z[k]=[xVOxa,imuyVOya,imuzVOza,imuzas]T

(12)

式中:(xVOyVOzVO)為由視覺傳感器得到的飛行器位置測量值;(xa,imuya,imuza,imu)為慣性測量單元測量的加速度值;zas為超聲波傳感器提供的測量值。濾波器融合了zVO與zas來得到更準確的飛行器高度估計值。

根據(jù)式

(13)

計算C′矩陣。

根據(jù)式

(14)

(15)

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

根據(jù)式

(16)

圖13 卡爾曼濾波算法預測更新流程圖

根據(jù)式

(17)

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

4 實驗與結(jié)果

4.1 實驗平臺概述

本文組合導航實驗平臺如圖14所示,該平臺基于SANYE LIGHT開源飛控構(gòu)成,搭載了慣性測量單元、OpenMV機器視覺模塊、超聲波傳感器等設(shè)備。OpenMV以STM32F427為核心,集成了OV7725攝像頭芯片,是一個開源,低成本、功能強大的機器視覺模塊,其中OV7725攝像頭為下視安裝,分辨率為320×240像素,視場角110°,提供硬件二值化的黑白圖像。組合導航系統(tǒng)以25 Hz的頻率融合傳感器的數(shù)據(jù)。

圖14 成像、慣性與高度組合導航實驗平臺

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

本文設(shè)計了兩個實驗,在室內(nèi)實時飛行條件下,分別對高度進行估計以及平移速度估計實驗。實驗中,通過卡爾曼濾波器將視覺傳感器、慣性測量單元的加速度數(shù)據(jù)和高度傳感器信號進行融合,所用的卡爾曼濾波器函數(shù)由OpenCV庫提供,它可以執(zhí)行所有與狀態(tài)估計和數(shù)據(jù)融合相關(guān)的任務(wù)。

4.2.1 高度估計實驗

本實驗的目的在于:驗證成像、慣性與高度傳感系統(tǒng)通過卡爾曼濾波融合后估計四軸飛行器高度的可靠性。在實驗中,成像信息由視覺傳感器獲取,慣性傳感器如陀螺儀、加速度計能夠精確的測量角速度的快速變化和線加速度,而高度測量值由超聲波傳感器提供,以著陸標志為中心,通過人工控制將四軸飛行器的飛行高度從60 cm升至180 cm。

圖15 實時高度估計曲線圖

如圖15所示,通過數(shù)據(jù)融合成像、慣性與高度組合系統(tǒng)估計的高度值與超聲波傳感器測量出來的高度具有一致性,證明成像、慣性與高度傳感系統(tǒng)能夠很好的描述四軸飛行器的飛行高度估計。然而它們之間存在一些差異,根據(jù)推測,視覺算法在獲取高度值的過程中選取了一些異常值,造成融合后的部分曲線與超聲波估計曲線存在差異,通過優(yōu)化后的濾波算法過濾掉超出正常范圍的測量值是可行的方案之一。

4.2.2 平移速度與相對位置估計實驗

本實驗視覺部分提供無人機相對固定參考系的三維位置,慣性傳感器部分提供無人機質(zhì)心的加速度,研究的目的在于:通過卡爾曼濾波器融合成像與慣性傳感器所提供的信息對四軸飛行器平移速度進行準確的估計。

以著陸標志為中心點,四軸飛行器在到達1.5 m實驗高度時通過人工進行控制,到達臨界點時,由視覺系統(tǒng)計算當前狀態(tài)下四軸飛行器的三維位置(0,0,1.5),作為實驗參考位置。

實驗中,過程和觀測噪聲分別為w=0.150和v=0.100。圖16為實驗中四軸飛行器平移速度估計曲線圖,可以看到卡爾曼濾波器得到的速度估計值很平滑,系統(tǒng)響應(yīng)更理想。圖17為將卡爾曼濾波器融合后的速度作為PID反饋,控制四軸飛行器保持在原點附近飛行。結(jié)果顯示,飛行器的位置接近期望值:PID控制器成功地將四軸飛行器的x方向位置保持在原點附近,將y方向位置保持在-1.0 m附近,高度位置的最大誤差在0.3 m之內(nèi)。

圖16 四軸飛行器x,y,z方向平移速度變化過程

圖17 四軸飛行器x,y,z方向誤差變化過程

5 總結(jié)

本文針對在非結(jié)構(gòu)化、室內(nèi)和GPS失效的情況下,實現(xiàn)四軸飛行器自主起飛、相對定位和著陸于“無人機自動充電平臺”的目的,提出了一種基于卡爾曼濾波的四軸飛行器成像、慣性與高度組合導航系統(tǒng)。視覺系統(tǒng)用于估計四軸飛行器在固定慣性坐標系下的三維位置(x,y,z),慣性測量系統(tǒng)用于提高飛行器線性加速度的估計性能,PID控制器能夠利用估計的速度數(shù)據(jù)穩(wěn)定飛行器的三維位置。

實驗結(jié)果表明,成像、慣性與高度傳感系統(tǒng)能夠很好的描述四軸飛行器的飛行高度估計以及通過卡爾曼濾波器融合成像與慣性傳感器所提供的信息能夠?qū)λ妮S飛行器平移速度進行準確的估計。但組合導航與超聲波傳感器出現(xiàn)了一些差異,需要進一步解決。同時,當進行PID控制時,調(diào)整控制器參數(shù)很復雜,目前控制器的增益只能通過以實驗經(jīng)驗為主進行調(diào)整,而系統(tǒng)辨識算法可以估計飛行器的參數(shù),看起來是一個很好的途徑。