基于云臺(tái)相機(jī)的旋翼無人機(jī)視覺伺服方法研究

楊明曄, 劉婷婷, 付 貴

(西南科技大學(xué),四川 綿陽 621000)

0 引言

四旋翼無人機(jī)有著體積小、運(yùn)動(dòng)靈活、生產(chǎn)成本低等優(yōu)點(diǎn),能夠在室內(nèi)、城市、森林等眾多復(fù)雜環(huán)境下進(jìn)行定點(diǎn)懸停、目標(biāo)跟蹤等多種方式的飛行,因此四旋翼無人機(jī)被廣泛用于航拍、搜救、巡檢等任務(wù)[1-3]。隨著計(jì)算機(jī)技術(shù)以及機(jī)器人技術(shù)的快速發(fā)展,更優(yōu)良的傳感器、新型材料與設(shè)備、嵌入式設(shè)備等技術(shù)的出現(xiàn),獲取廣闊且穩(wěn)定的相機(jī)視場成為當(dāng)前的主流研究方向。目前,針對相機(jī)視場的研究主要集中在對相機(jī)視場的邊界約束控制算法設(shè)計(jì)以及添加云臺(tái)(PTZ)裝置的無人機(jī)控制方法研究。

文獻(xiàn)[4]針對基于視覺伺服的無人機(jī)運(yùn)動(dòng)控制進(jìn)行了研究,采用輸出反饋和基于圖像的視覺伺服來控制無人機(jī)的位姿,使無人機(jī)的視覺伺服擺脫了GPS的限制;文獻(xiàn)[5]提出了一種基于云臺(tái)的視覺伺服(PBVS)的方法來獲取世界坐標(biāo)信息,通過組合多個(gè)傳感器實(shí)現(xiàn)了在GNSS信息拒止的情況下通過自行構(gòu)建的一個(gè)新的云臺(tái)視覺伺服控制系統(tǒng)完成了旋翼無人機(jī)的視覺伺服,使旋翼無人機(jī)能夠在GNSS系統(tǒng)干擾環(huán)境下實(shí)現(xiàn)快速定位和導(dǎo)航;文獻(xiàn)[6]在GNSS拒止環(huán)境下基于云臺(tái)視覺伺服的無人機(jī)自動(dòng)著陸策略中又提出了一種基于云臺(tái)視覺伺服(PBVS)系統(tǒng)的無人機(jī)快速自主著陸策略,使用了基于云臺(tái)相機(jī)的控制系統(tǒng)進(jìn)行無人機(jī)自主導(dǎo)航。但是文獻(xiàn)[5-6]都是在云臺(tái)相機(jī)的基礎(chǔ)上進(jìn)行基于位置的視覺伺服研究,相對于基于圖像的視覺伺服來說在降落精確度和計(jì)算復(fù)雜度方面都有所不足。文獻(xiàn)[7]在僅裝備單目相機(jī)和IMU傳感器條件下,基于虛擬圖像動(dòng)力學(xué)提出了抗干擾的IBVS方法,但該方法是基于單目相機(jī)的IBVS方法,可以獲取的視野十分有限,適用范圍較小。因此,在旋翼無人機(jī)的相機(jī)上引入云臺(tái)裝置,拓寬了旋翼無人機(jī)的相機(jī)視野,成為當(dāng)前提升旋翼無人機(jī)相機(jī)視場的一種解決思路。

本文提出了一種基于云臺(tái)相機(jī)的視覺伺服控制系統(tǒng),在單目相機(jī)的基礎(chǔ)上加入云臺(tái)裝置,使相機(jī)有獨(dú)立的旋轉(zhuǎn)角度,有效地拓寬了無人機(jī)的視野,并設(shè)計(jì)基于模糊PID的云臺(tái)控制方法,使旋翼無人機(jī)快速、精準(zhǔn)地完成定點(diǎn)懸停。據(jù)仿真測試數(shù)據(jù)可知,這種方法能夠讓無人機(jī)具有更廣的視野,提高了無人機(jī)視覺伺服的普適性。

1 理論基礎(chǔ)

1.1 四旋翼無人機(jī)動(dòng)力學(xué)模型

由牛頓-歐拉法,可以將導(dǎo)航坐標(biāo)系四旋翼無人機(jī)的動(dòng)力學(xué)模型表示為[8]

(1)

1.2 相機(jī)成像模型

圖1是相機(jī)的成像模型。相機(jī)鏡頭看作是一個(gè)理想針孔,光線穿過針孔并與圖像平面相交。

圖1 針孔相機(jī)成像模型

假設(shè)導(dǎo)航坐標(biāo)系中的點(diǎn)P的坐標(biāo)為(xw,yw,zw),而在相機(jī)坐標(biāo)系中P的坐標(biāo)為(xc,yc,zc)。P在圖像平面上的投影點(diǎn)設(shè)為P′,其坐標(biāo)為(x,y),P′在像素平面上的坐標(biāo)為(u,v),點(diǎn)P′,P與相機(jī)坐標(biāo)系的原點(diǎn)OC在三維空間中位于同一直線上[9]。

點(diǎn)P在導(dǎo)航坐標(biāo)系下的坐標(biāo)與點(diǎn)P在相機(jī)坐標(biāo)系下的坐標(biāo)可以相互轉(zhuǎn)換,即

(2)

式中,R和T是相機(jī)的外參,R是導(dǎo)航坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣,T是導(dǎo)航坐標(biāo)系和相機(jī)坐標(biāo)系之間的平移矩陣[10]。

點(diǎn)P由相機(jī)坐標(biāo)系投影至圖像平面上的坐標(biāo)為

(3)

式中,f為相機(jī)焦距。

在像素坐標(biāo)系中,光心的像素坐標(biāo)表示為(u0,v0),Px表示單個(gè)像素水平方向上的長度,Py表示單個(gè)像素垂直方向上的長度,則投影點(diǎn)在像素平面上的坐標(biāo)為

(4)

由式(3)與式(4)可得導(dǎo)航坐標(biāo)系到像素坐標(biāo)系的轉(zhuǎn)換關(guān)系為

(5)

式中:α=f/Px;β=f/Py;參數(shù)α,β,cu,cv為相機(jī)內(nèi)參,不會(huì)隨相機(jī)的移動(dòng)而發(fā)生改變。選取相機(jī)內(nèi)參矩陣為

1.3 三維坐標(biāo)轉(zhuǎn)換

由于云臺(tái)相機(jī)的光軸垂直于相機(jī)成像平面,并且規(guī)定旋翼無人機(jī)隨時(shí)保持目標(biāo)處于相機(jī)光軸上,因此,像素視場中心的對象垂直于相機(jī)平面。當(dāng)無人機(jī)接近目標(biāo)物體時(shí),無人機(jī)攜帶的相機(jī)由穩(wěn)定PTZ控制,用以跟隨目標(biāo)點(diǎn),并可根據(jù)無人機(jī)的不同位置改變鏡頭方向。因此,無人機(jī)攝像頭和目標(biāo)對象之間的空間幾何關(guān)系如圖2所示[11]。

圖2 無人機(jī)和目標(biāo)物體之間的空間幾何關(guān)系示意圖

在無人機(jī)系統(tǒng)中,相機(jī)的姿態(tài)由偏航角和俯仰角兩個(gè)自由度的攝像機(jī)云臺(tái)實(shí)時(shí)控制,如圖3所示。

圖3 無人機(jī)云臺(tái)攝像機(jī)示意圖

當(dāng)無人機(jī)移動(dòng)到達(dá)目標(biāo)點(diǎn)上方一定的水平距離時(shí),調(diào)整相機(jī)角度,使光軸垂直向下并確保無人機(jī)拍攝圖像中有完整的目標(biāo)點(diǎn)圖像,能夠正常識(shí)別,根據(jù)相機(jī)的成像原理,可以根據(jù)圖像上目標(biāo)點(diǎn)與中心點(diǎn)的偏移量判斷無人機(jī)與目標(biāo)點(diǎn)的相對位置關(guān)系,然后調(diào)整無人機(jī)的位置,采用像素級(jí)別方法進(jìn)行降落,進(jìn)一步提高降落精度。

1.4 云臺(tái)圖像特征動(dòng)力學(xué)建模

將無人機(jī)在導(dǎo)航坐標(biāo)系中的位置設(shè)置為P1=[X1Y1Z1]T,目標(biāo)在導(dǎo)航坐標(biāo)系中的位置設(shè)置為P2=[X2Y2Z2]T,相機(jī)在無人機(jī)機(jī)體坐標(biāo)系中的安裝位置設(shè)置為bP2=[bx2by2bz2]T,目標(biāo)在相機(jī)坐標(biāo)系中的位置為CP3=[Cx3Cy3Cz3]T,每個(gè)向量均滿足

(6)

目標(biāo)在圖像平面中的成像位置I=[uv]T可以近似為

(7)

式中,M為相機(jī)的參數(shù)矩陣,表示為

(8)

ax和ay是相機(jī)的焦距,[u0v0]T是圖像的中心像素值。

將式(7)代入式(6),可得

(9)

(10)

(11)

云臺(tái)運(yùn)動(dòng)補(bǔ)償控制信號(hào)可表示為

(12)

式中:

(13)

(14)

1.5 云臺(tái)模糊PID控制

為獲得更快的響應(yīng)速度和更好的控制效果,PTZ控制系統(tǒng)加入了模糊PID控制。傳統(tǒng)PID控制的輸出表達(dá)式為

(15)

式中:KP是比例系數(shù);TI是積分時(shí)間常數(shù);TD是微分時(shí)間常數(shù);e(t)為目標(biāo)點(diǎn)坐標(biāo)與圖像中心點(diǎn)之間的偏移量,即

(16)

式中,u(t),v(t)表示t時(shí)刻目標(biāo)點(diǎn)在相機(jī)圖像平面中的坐標(biāo)值。

模糊PID控制算法是在經(jīng)典PID的基礎(chǔ)上,通過模糊推理,反模糊化來對PID控制器的KP,TI,TD參數(shù)進(jìn)行實(shí)時(shí)整定[14]。

PID控制器參數(shù)自整定為

(17)

式中:KP(0),TI(0),TD(0)是PID控制器的參數(shù)初值;ΔKP,ΔTI,ΔTD分別是KP,TI,TD的實(shí)時(shí)的調(diào)整值。

將云臺(tái)裝置的偏轉(zhuǎn)角誤差e(θ)和誤差變化率Δe(θ)作為控制器的輸入,對應(yīng)的模糊語言變量為E和Ec,ΔKP,ΔTI,ΔTD作為PID控制器輸出,對應(yīng)的模糊語言變量為KP,TI,TD。m(θ)為云臺(tái)的期望偏轉(zhuǎn)角,n(θ)為角度傳感器采集到的云臺(tái)偏轉(zhuǎn)角度值,則e(θ)=m(θ)-n(θ),Δe(θ)=de(θ)/dt。

定義模糊集如下:模糊語言變量的模糊集合均為{負(fù)大(NB),負(fù)中(NM),負(fù)小(NS),零(ZO),正小(PS),正中(PM),正大(PB)}。輸入變量e(θ),Δe(θ)和輸出變量ΔKP,ΔTI,ΔTD的基礎(chǔ)論域分別為{-50,50},{-30,30},{-20,20},{-6,6},{-0.7,0.7}。將輸入的模糊語言變量E和EC映射到{-3,3},將輸出的模糊語言變量KP,TI,TD映射到{-1,1}。e(θ)和Δe(θ)的量化因子為Ke=Kec=3/50,輸出變量ΔKP,ΔTI,ΔTD的比例因子為KP=20/1=20,KI=6/1=6,KD=0.7/1=0.7。輸入、輸出變量均使用三角形隸屬度函數(shù)表示,隸屬度函數(shù)如圖4所示。

圖4 隸屬度函數(shù)

模糊PID控制目標(biāo)是找出輸出參數(shù)KP,TI,TD與輸入?yún)?shù)e(θ),Δe(θ)存在的模糊關(guān)系,在運(yùn)行中通過無人機(jī)攜帶的角度傳感器實(shí)時(shí)檢測e(θ)和Δe(θ),之后,根據(jù)模糊控制原理對3個(gè)控制參數(shù)進(jìn)行實(shí)時(shí)在線調(diào)整,使控制器達(dá)到較為理想的性能。模糊控制器采取輸入為2個(gè)參數(shù)和輸出為3個(gè)參數(shù)的形式,再通過PID控制參數(shù)整定原則,得到模糊規(guī)則如表1所示。

表1 模糊控制規(guī)則

通過上述的輸入、輸出變量隸屬度函數(shù)及模糊規(guī)則,根據(jù)制定的模糊決策,可以獲得不同的e(θ)和Δe(θ)情況下對應(yīng)的輸出量ΔKP,ΔTI,ΔTD,再與PID參數(shù)初值相加得到模糊PID控制的輸出值。圖5為本文所提控制方法與運(yùn)動(dòng)補(bǔ)償控制方法的輸出對比。

圖5 兩種控制方法輸出控制量對比

從圖5中可以看出,加入了模糊PID控制之后的系統(tǒng)輸出比運(yùn)動(dòng)補(bǔ)償控制的輸出更快,并能更快速穩(wěn)定于期望控制量且誤差波動(dòng)更小,使控制系統(tǒng)具有更好的性能。

2 基于云臺(tái)的視覺伺服控制系統(tǒng)設(shè)計(jì)

2.1 系統(tǒng)控制方案

控制系統(tǒng)分為無人機(jī)視覺伺服控制系統(tǒng)以及云臺(tái)相機(jī)的控制系統(tǒng)兩部分。在任務(wù)開始時(shí),云臺(tái)相機(jī)豎直向下尋找目標(biāo)點(diǎn),此時(shí)俯仰角θ達(dá)到最大值90°,如果沒有找到目標(biāo),云臺(tái)控制俯仰角減小10°并控制偏航角單方向旋轉(zhuǎn)360°,如果仍未找到目標(biāo)點(diǎn)就繼續(xù)減小俯仰角并旋轉(zhuǎn)偏航角,直至找到目標(biāo)或俯仰角減小到30°仍未找到目標(biāo)系統(tǒng)判定任務(wù)失敗為止。若順利捕捉到目標(biāo),云臺(tái)的旋轉(zhuǎn)隨即停止并記錄此時(shí)的姿態(tài)信息,首先,由高度計(jì)獲取的高度信息和云臺(tái)相機(jī)獲取的實(shí)時(shí)圖像通過特征點(diǎn)提取和坐標(biāo)系轉(zhuǎn)換估算出無人機(jī)與目標(biāo)點(diǎn)在三維空間中的相對位置,系統(tǒng)根據(jù)兩者相對距離L確定一個(gè)較大的水平速度V1使無人機(jī)快速到達(dá)目標(biāo)點(diǎn)上空,之后,進(jìn)入快速降落階段,計(jì)算相機(jī)成像平面中特征點(diǎn)位置與圖像中心點(diǎn)誤差,將像素誤差和無人機(jī)線速度、角速度以及云臺(tái)相機(jī)的角速度作為輸入,通過雅可比矩陣和圖像運(yùn)動(dòng)學(xué)得到輸出為無人機(jī)的期望線速度和角速度以及云臺(tái)的期望角速度。此外,在云臺(tái)控制系統(tǒng)中同時(shí)加入了模糊PID控制以獲得更好的控制效果。因此,在任務(wù)初始階段,期望控制量較大以達(dá)到快速到達(dá)著陸點(diǎn)附近的目的,在到達(dá)著陸點(diǎn)上空時(shí),云臺(tái)相機(jī)轉(zhuǎn)角達(dá)到閾值,鏡頭垂直向下并鎖定,控制系統(tǒng)由基于位置的視覺伺服轉(zhuǎn)變?yōu)榛趫D像的視覺伺服,根據(jù)像素平面誤差得到輸出期望控制量,此時(shí)期望控制量由之前距離目標(biāo)較遠(yuǎn)時(shí)給出的較大的控制量變?yōu)橛上袼仄矫嬲`差計(jì)算得到的較小的控制量,以得到較小的收斂誤差,無人機(jī)在收斂過程中與目標(biāo)在水平面上的位置誤差變化如圖6所示。

圖6 無人機(jī)收斂過程中誤差曲線

無人機(jī)在飛行過程中誤差不斷減小,最終到達(dá)期望位置完成著陸,無人機(jī)定點(diǎn)懸?？刂品椒ㄈ鐖D7所示。

圖7 基于云臺(tái)的無人機(jī)定點(diǎn)懸?？刂品椒?/p>

2.2 云臺(tái)控制系統(tǒng)設(shè)計(jì)

在云臺(tái)控制系統(tǒng)中,輸出的控制量由運(yùn)動(dòng)補(bǔ)償控制和模糊PID控制相結(jié)合生成,具體的控制流程如圖8所示。

圖8 云臺(tái)控制系統(tǒng)原理圖

由圖8可知,云臺(tái)的運(yùn)動(dòng)補(bǔ)償控制信號(hào)wc為

(18)

PID控制的輸出為

(19)

式中:

(20)

因此,整個(gè)云臺(tái)控制系統(tǒng)的輸出為

w=wc+wPID。

(21)

3 仿真與實(shí)驗(yàn)分析

為了驗(yàn)證所提出方法的有效性,建立了無人機(jī)自主著陸的仿真模型,仿真中,假設(shè)無人機(jī)在飛行過程中不受風(fēng)、磁場、光和其他外部環(huán)境的干擾。

3.1 實(shí)驗(yàn)1(經(jīng)典模式)

設(shè)置了一個(gè)期望的4點(diǎn)圖像作為無人機(jī)定點(diǎn)懸停的期望位置參考,將該期望位置中心點(diǎn)設(shè)置為(2.5 m,2.5 m,7 m),無人機(jī)的初始位置為(6 m,5 m,12 m)。仿真后,無人機(jī)的著陸軌跡如圖9所示。

圖9 無人機(jī)著陸三維軌跡圖(經(jīng)典模式)

著陸過程中無人機(jī)的位置變化如圖10所示。

經(jīng)多次實(shí)驗(yàn),當(dāng)無人機(jī)與目標(biāo)相對高度為5 m時(shí),若要使目標(biāo)能夠出現(xiàn)在相機(jī)視場進(jìn)行視覺伺服任務(wù),無人機(jī)與目標(biāo)的相對水平距離最大為5 m,即使用固定相機(jī)時(shí)的無人機(jī)任務(wù)半徑ra=5 m,因此在無人機(jī)與目標(biāo)高度相差5 m時(shí),X方向和Y方向上最遠(yuǎn)伺服半徑為5 m。

在實(shí)驗(yàn)中,基于普通相機(jī)的視覺伺服任務(wù)可以實(shí)現(xiàn),前提是無人機(jī)開始任務(wù)時(shí)目標(biāo)必須在相機(jī)視場中被檢測到,當(dāng)目標(biāo)一開始并未出現(xiàn)在相機(jī)視場中時(shí),系統(tǒng)會(huì)因?yàn)閬G失目標(biāo)導(dǎo)致任務(wù)失敗。

3.2 實(shí)驗(yàn)2(云臺(tái)模式)

在實(shí)驗(yàn)中,將期望的位置設(shè)置在相機(jī)固定時(shí)的任務(wù)半徑之外,即經(jīng)典模式的任務(wù)半徑之外,期望圖像特征表明無人機(jī)的期望位置中心點(diǎn)為(2.5 m,2.5 m,7 m),無人機(jī)的初始位置為(15 m,10 m,12 m),因?yàn)闊o人機(jī)初始水平位置距離期望水平位置較遠(yuǎn),一開始目標(biāo)并不會(huì)出現(xiàn)在相機(jī)視場中,如果繼續(xù)使用經(jīng)典模式,系統(tǒng)會(huì)因檢測不到目標(biāo)點(diǎn)終止并提示任務(wù)失敗,改為云臺(tái)模式后,系統(tǒng)通過云臺(tái)裝置的旋轉(zhuǎn)檢測到了目標(biāo)點(diǎn)位置并繼續(xù)懸停任務(wù),無人機(jī)的著陸軌跡如圖11所示,著陸過程中無人機(jī)的位置變化如圖12所示。

圖12 云臺(tái)模式下無人機(jī)位置變化圖

無人機(jī)期望控制量變化如圖13所示。

無人機(jī)和云臺(tái)的期望牛頓-歐拉角變化如圖14所示。

圖14 任務(wù)過程中無人機(jī)和云臺(tái)的期望牛頓-歐拉角

由仿真結(jié)果(圖11～圖14)可以看出,當(dāng)使用云臺(tái)模式執(zhí)行任務(wù)時(shí),期望圖像可以設(shè)置得比經(jīng)典模式更遠(yuǎn),并且無人機(jī)在任務(wù)過程中相機(jī)成像點(diǎn)與期望圖像點(diǎn)的偏移量比普通相機(jī)模式小。經(jīng)過多次測試,當(dāng)無人機(jī)與目標(biāo)的高度差同樣保持在5 m時(shí),X方向和Y方向最遠(yuǎn)可與目標(biāo)相差14 m,即使用云臺(tái)相機(jī)的無人機(jī)任務(wù)半徑rb=14 m,這表示在同樣高度的情況下,使用云臺(tái)相機(jī)時(shí)的任務(wù)半徑是不使用云臺(tái)相機(jī)時(shí)的任務(wù)半徑的2～3倍,并且圖像平面中圖像特征的軌跡和三維空間中無人機(jī)的軌跡依然很平滑,著陸時(shí)間和收斂精度均能達(dá)到較高水平。

經(jīng)過兩組不同的無人機(jī)自主著陸仿真實(shí)驗(yàn)可知,同樣高度情況下采用了云臺(tái)相機(jī)的無人機(jī)的任務(wù)半徑是采用固定相機(jī)的無人機(jī)的任務(wù)半徑的2～3倍,并且實(shí)驗(yàn)中基于云臺(tái)相機(jī)的無人機(jī)定點(diǎn)懸停的控制系統(tǒng)具有較高的懸停精度和較快的任務(wù)速度,其表現(xiàn)優(yōu)于搭載固定相機(jī)的無人機(jī)懸停方案。

4 結(jié)束語

本文提出了一種無人機(jī)快速定點(diǎn)懸停策略。為保證無人機(jī)能夠有更大的任務(wù)半徑,為相機(jī)加入了云臺(tái)裝置,使相機(jī)能夠有獨(dú)立旋轉(zhuǎn)角度以獲取更廣的視野,在控制方案方面,在云臺(tái)的控制中引入模糊PID控制,使無人機(jī)在距離目標(biāo)較遠(yuǎn)時(shí)有較快的收斂速度,在接近目標(biāo)點(diǎn)之后減小控制參數(shù),減小超調(diào)帶來的誤差,縮短了任務(wù)時(shí)間的同時(shí)減小了懸停誤差。仿真實(shí)驗(yàn)結(jié)果表明,與普通的視覺伺服無人機(jī)相比,提出的方案具有更廣闊的相機(jī)視野以及更好的普適性。