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

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

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

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

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

1 攝像機(jī)模型與標(biāo)定

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

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

(1)

令

(2)

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

圖1 攝像機(jī)模型

圖2 氧化鋁標(biāo)定板

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

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

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

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

圖4 MATLAB GUI中攝像機(jī)標(biāo)定工具箱中的棋盤圖案

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

(3)

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

(4)

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

(5)

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

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

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

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

(6)

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

(7)

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

2 著陸標(biāo)志設(shè)計(jì)

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

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

圖7 四個(gè)圓環(huán)著陸標(biāo)志

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

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

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

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

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

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

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

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

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

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

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

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

(8)

計(jì)算出任意兩個(gè)特征點(diǎn)P1,P2的坐標(biāo)以及它們映射點(diǎn)的像素坐標(biāo),就可以測量出|P1P2|的物理距離,并得出|I1I2|的像素距離,從而得到相對(duì)高度h。

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

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

圖11 光流估計(jì)相對(duì)平移速度

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

圓周光流平均值可以表示成如下運(yùn)動(dòng)過程:

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

(9)

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

(10)

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

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

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

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

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

(11)

觀測更新向量定義為

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

(12)

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

根據(jù)式

(13)

計(jì)算C′矩陣。

根據(jù)式

(14)

(15)

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

根據(jù)式

(16)

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

根據(jù)式

(17)

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

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

4.1 實(shí)驗(yàn)平臺(tái)概述

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

圖14 成像、慣性與高度組合導(dǎo)航實(shí)驗(yàn)平臺(tái)

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

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

4.2.1 高度估計(jì)實(shí)驗(yàn)

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

圖15 實(shí)時(shí)高度估計(jì)曲線圖

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

4.2.2 平移速度與相對(duì)位置估計(jì)實(shí)驗(yàn)

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

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

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

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

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

5 總結(jié)

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

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