基于機(jī)器視覺的多旋翼無人機(jī)飛行偏航控制

張 歡,李林俊,厲 偉,李宏林,王亞萍,姚繼鋒

(1.國能鐵路裝備有限責(zé)任公司生產(chǎn)技術(shù)部,北京 100048) (2.南京斯?fàn)柲姎庥邢薰局悄苄畔⑹聵I(yè)部,江蘇 南京 211161) (3.中國科學(xué)院軟件研究所,北京 101408)

由于多旋翼無人機(jī)具備質(zhì)量輕、結(jié)構(gòu)簡單、操作便利等優(yōu)勢,因此在軍事與民用領(lǐng)域均有應(yīng)用[1]。多旋翼無人機(jī)的反扭力矩顯著低于升力力矩,因此多旋翼無人機(jī)容易出現(xiàn)偏航問題[2]。當(dāng)多旋翼無人機(jī)出現(xiàn)偏航現(xiàn)象時(shí),會(huì)直接影響其應(yīng)用效果,因此必須及時(shí)糾正偏航問題,以確保多旋翼無人機(jī)能按照設(shè)定的航線飛行[3-4]。例如,王思孝等[5]設(shè)計(jì)的串級TD-PID無人機(jī)飛行偏航控制方法,以PID算法為外環(huán)控制器、微分跟蹤器為內(nèi)環(huán)控制器,提升無人機(jī)飛行偏航控制效果。該方法可有效實(shí)現(xiàn)無人機(jī)飛行的偏航控制,且偏航控制的穩(wěn)定性較優(yōu)。朱文杰等[6]通過在PID控制器內(nèi)引入tansig誤差反饋函數(shù),修正控制器的參數(shù),解決了風(fēng)力擾動(dòng)問題。該方法可有效實(shí)現(xiàn)無人機(jī)偏航控制,具備較優(yōu)的適用性。上述方法均易于調(diào)節(jié)、容易實(shí)現(xiàn),但在無人機(jī)執(zhí)行器出現(xiàn)飽和現(xiàn)象時(shí),上述方法受飽和現(xiàn)象影響,容易出現(xiàn)超調(diào)量大以及振蕩等問題,影響無人機(jī)偏航控制性能。

機(jī)器視覺具備精度高、成本低、抗干擾能力強(qiáng)等優(yōu)勢[7],在無人機(jī)飛行控制領(lǐng)域,應(yīng)用機(jī)器視覺,能夠較好地提升無人機(jī)飛行偏航控制性能。因此,迫切需要研究基于機(jī)器視覺的多旋翼無人機(jī)飛行偏航控制方法,通過基于機(jī)器視覺的方法,準(zhǔn)確獲取無人機(jī)相對位置信息,并有效抵御外界干擾,從而提升無人機(jī)的飛行性能。

1 多旋翼無人機(jī)飛行偏航控制

1.1 基于機(jī)器視覺的多旋翼無人機(jī)飛行位置解算

針孔模型是計(jì)算機(jī)視覺中常用的相機(jī)模型之一,其特點(diǎn)包括簡化和線性化。針孔模型假設(shè)相機(jī)通過一個(gè)小孔投射光線到圖像平面上,因此可以忽略復(fù)雜的光學(xué)元件。它的核心思想是基于投影原理,將三維空間中的點(diǎn)通過直線投影到二維圖像平面上,保持了尺度不變性[8-10]。這種模型簡單而有效,常用于相機(jī)定位和三維重建等計(jì)算機(jī)視覺任務(wù)中。利用基于針孔模型的相對位置解算算法,依據(jù)CCD攝像機(jī)采集的參考目標(biāo)圖像,計(jì)算無人機(jī)的相對位置[11]。

圖像坐標(biāo)系包含xy直角坐標(biāo)系與uv像素坐標(biāo)系,兩種坐標(biāo)系間的轉(zhuǎn)換關(guān)系為:

(1)

式中:(x,y)為直角坐標(biāo)系坐標(biāo),(u,v)為像素坐標(biāo)系坐標(biāo),(u0,v0)為xy坐標(biāo)系原點(diǎn)在uv坐標(biāo)系內(nèi)的坐標(biāo)。

按照相機(jī)針孔模型的近似思想[12],根據(jù)直角坐標(biāo)系與像素坐標(biāo)系,構(gòu)造圖像坐標(biāo)系和相機(jī)坐標(biāo)系。通過相機(jī)的內(nèi)參矩陣,可以將直角坐標(biāo)系中的點(diǎn)坐標(biāo)轉(zhuǎn)換為歸一化的圖像坐標(biāo);相機(jī)坐標(biāo)系則考慮了相機(jī)與物體的3D空間關(guān)系,通常需要進(jìn)行相機(jī)的外參矩陣轉(zhuǎn)換,外參矩陣包含了相機(jī)的位置和姿態(tài)信息,通過給定物體在相機(jī)坐標(biāo)系中的坐標(biāo),可以使用外參矩陣將其轉(zhuǎn)換為圖像坐標(biāo)系中的坐標(biāo)。圖像坐標(biāo)的轉(zhuǎn)換方程為:

(2)

式中:(xb,yb,zb)為參考目標(biāo)b在相機(jī)坐標(biāo)系內(nèi)的坐標(biāo),M為相機(jī)內(nèi)參矩陣。通過公式(2)便可初步建立相機(jī)針孔模型。

多旋翼無人機(jī)飛行位置解算的具體步驟如下:

步驟1,按照參考目標(biāo)在圖像內(nèi)的坐標(biāo)值,結(jié)合式(1)的坐標(biāo)轉(zhuǎn)換關(guān)系,可確定相機(jī)的歐拉旋轉(zhuǎn)角φ、θ,其中,φ為滾轉(zhuǎn)角,θ為俯仰角。φ與θ的旋轉(zhuǎn)矩陣如下:

(3)

(4)

步驟2,令多旋翼無人機(jī)的飛行高度為h1,對應(yīng)的相機(jī)歐拉旋轉(zhuǎn)角為φ1、θ1。通過式(3)與式(4)可獲取相機(jī)旋轉(zhuǎn)后的光軸和地面交點(diǎn)坐標(biāo)間的關(guān)系:

Qφ1·Qθ1·[0,0,h1]=[h1sinθ1cosφ1-

h1cosθ1sinφ1-h1cosθ1cosφ1]

(5)

式中:Qφ1、Qθ1分別為相機(jī)歐拉旋轉(zhuǎn)角φ1、θ1的旋轉(zhuǎn)矩陣。

步驟3,通過推導(dǎo)式(5),同時(shí)將歐拉角改變成通用形式,獲取參考目標(biāo)在圖像內(nèi)的坐標(biāo),公式如下:

(6)

式中:z為三維坐標(biāo)系中參考目標(biāo)與平面垂直的高度。

步驟4,依據(jù)相機(jī)旋轉(zhuǎn)關(guān)系可知z=h1,已知φ1與θ1,可得:

(7)

式中:(x1,y1)為三維坐標(biāo)系中參考目標(biāo)的坐標(biāo)。

(8)

Q′=Qφ1×Qθ1×Qφ2×Qθ2

(9)

式中:Qφ2、Qθ2分別為相機(jī)歐拉旋轉(zhuǎn)角φ2、θ2的旋轉(zhuǎn)矩陣。

步驟7,聯(lián)立式(9)、式(3)與式(4),可獲取多旋翼無人機(jī)的相對位置,公式如下:

(10)

式中:X、Y為無人機(jī)的橫縱坐標(biāo)相對位置。

通過式(10)可確定多旋翼無人機(jī)的位置,當(dāng)計(jì)算獲取的多旋翼無人機(jī)位置與預(yù)先設(shè)定的位置間差距較大時(shí),說明多旋翼無人機(jī)已偏航[13],需進(jìn)行偏航控制。

1.2 基于機(jī)器視覺的多旋翼無人機(jī)偏航角計(jì)算

當(dāng)多旋翼無人機(jī)偏航時(shí),通常采用三角測量法檢測參考目標(biāo)圖像的角點(diǎn),通過角點(diǎn)匹配獲取多旋翼無人機(jī)的旋轉(zhuǎn)矩陣與平移向量[14],完成多旋翼無人機(jī)偏航角的計(jì)算。

令參考目標(biāo)圖像連通域輪廓H內(nèi)待檢測的像素種子點(diǎn)為S。在H內(nèi),按照步長L,在S左右兩邊分別選擇兩個(gè)鄰近點(diǎn)P1、P2,通過包含P1與P2的直線方程,便可得到S至直線的距離dS。

依據(jù)dS可獲取全部參考目標(biāo)圖像的候選角點(diǎn),令距離閾值為ε。當(dāng)dS≥ε時(shí),則對應(yīng)的種子點(diǎn)為可能的候選角點(diǎn);反之為非角點(diǎn)。

參考目標(biāo)圖像角點(diǎn)檢測的具體步驟如下:

候選角點(diǎn)判定條件為:

(11)

通過上述步驟能夠獲取m個(gè)角點(diǎn),作為參考目標(biāo)的角點(diǎn)位置。

通過匹配檢測到的角點(diǎn)與參考圖像角點(diǎn),估計(jì)偏航角。其角點(diǎn)匹配步驟如下:

步驟1,按照參考目標(biāo)圖像輪廓順序,存儲(chǔ)檢測獲取的m個(gè)角點(diǎn),并依次求解鄰近兩個(gè)角點(diǎn)間的距離,獲取m個(gè)距離值。

步驟2,對比分析m個(gè)距離值,以最大距離值相應(yīng)的兩個(gè)角點(diǎn)輸出的直線斜率[15],為參考目標(biāo)圖像的主方向,將參考目標(biāo)圖像分割成4個(gè)象限。

步驟3,按照角點(diǎn)至質(zhì)心歐氏距離的3種情況,分類各象限內(nèi)的角點(diǎn),得到外點(diǎn)、中點(diǎn)與內(nèi)點(diǎn)3種類型,按照參考圖像內(nèi)定義的角度,完成角點(diǎn)匹配,得到m個(gè)匹配角點(diǎn)對。

按照m個(gè)匹配角點(diǎn)對,估計(jì)多旋翼無人機(jī)的偏航角,具體步驟如下:

步驟1,標(biāo)定CCD攝像機(jī),得到CCD攝像機(jī)的內(nèi)參矩陣,二維像素坐標(biāo)系和三維世界坐標(biāo)系的相應(yīng)關(guān)系為:

(12)

其中:

式中:ξ為尺度因子,t為平移矩陣,Q為相機(jī)旋轉(zhuǎn)矩陣,[Q|t]為3×3的相機(jī)外參矩陣,R為世界坐標(biāo)系矩陣,qij為旋轉(zhuǎn)向量,ti為平移向量。

步驟2,采用m個(gè)匹配角點(diǎn),按照直接線性變換可獲取對應(yīng)的單應(yīng)矩陣,并對其進(jìn)行分解,獲取Q與t,按照式(12)解算獲取多旋翼無人機(jī)的偏航角ψ,公式如下:

ψ=arctan(q21,q11)

(13)

1.3 多旋翼無人機(jī)飛行偏航控制的實(shí)現(xiàn)

在抗飽和控制器的外環(huán)中,將根據(jù)上述計(jì)算得到的偏航角與期望偏航角之間的誤差作為輸入,輸出多旋翼無人機(jī)所期望的偏航角速度,并將其輸入到抗飽和控制器的內(nèi)環(huán)中,生成多旋翼無人機(jī)的偏航控制量,從而完成對多旋翼無人機(jī)的偏航控制。

外環(huán)控制器采用PD控制多旋翼無人機(jī)的偏航角度,輸出αε為:

(14)

式中:eψ為多旋翼無人機(jī)偏航角誤差,eψ=|ψε-ψ|,其中ψε為期望偏航角;KP、KD分別為比例、微分系數(shù);αε為期望偏航角速度。

內(nèi)環(huán)采用抗飽和PID控制多旋翼無人機(jī)偏航角度,輸出偏航控制量c,當(dāng)電機(jī)轉(zhuǎn)速存在積分飽和問題時(shí),c為飽和狀態(tài),c?[cmin,cmax],cmax、cmin為最大、最小偏航控制量;令多旋翼無人機(jī)偏航飽和控制量為c′,計(jì)算公式如下:

c′=max(cmin,min(c,cmax))

(15)

c的計(jì)算公式如下:

c=KPeα+KIξ

(16)

式中:eα為偏航角速度誤差,eα=|αε-α|,其中α為實(shí)際偏航角速度;KI為積分系數(shù);ξ為內(nèi)環(huán)積分值。令虛擬積分控制量cξ=ξ,則:

(17)

式中:β為抗飽和系數(shù),ρ為退飽和閾值。

考慮飽和補(bǔ)償、飽和未補(bǔ)償與未飽和3種模式下的控制需求,構(gòu)建抗飽和控制器,獲得最終的偏航控制量,計(jì)算公式如下:

(18)

通過式(18)便可以獲取不同模式下的多旋翼無人機(jī)偏航控制量,提升偏航控制算法的魯棒性與抗干擾能力。

2 實(shí)驗(yàn)分析

以某四旋翼無人機(jī)為實(shí)驗(yàn)對象,該四旋翼無人機(jī)型號為HMJ-00D4000P,最大載質(zhì)量為8.6 kg,最大續(xù)航時(shí)間為38 min,機(jī)身質(zhì)量為5.4 kg,軸徑為900 mm,最大飛行速度為12 m/s,水平懸停精度≤±1.5 m ,垂直懸停精度≤±0.3 m,最大抗風(fēng)等級為六級,視頻傳輸距離為10～25 km。

利用本文方法采集參考目標(biāo)圖像,以四旋翼無人機(jī)的地面工作站為參考目標(biāo),采集結(jié)果如圖1所示。

圖1 參考目標(biāo)圖像采集結(jié)果 圖2 四旋翼無人機(jī)飛行位置計(jì)算結(jié)果 圖3 角點(diǎn)檢測結(jié)果

圖中,白色矩形框?yàn)樗男頍o人機(jī)的地面工作站,即參考目標(biāo)。由圖1可知,本文方法可有效采集參考目標(biāo)圖像,且采集的參考目標(biāo)圖像清晰度較高。

將用本文方法解算獲取的無人機(jī)位置繪制成圖像,以圖像的形式分析采用本文方法時(shí)無人機(jī)位置的解算精度,并判定該四旋翼無人機(jī)是否存在偏航問題,分析結(jié)果如圖2所示。

由圖可知,本文方法可有效計(jì)算四旋翼無人機(jī)的飛行位置,且計(jì)算獲取的位置曲線與實(shí)際的位置曲線非常接近,說明本文方法的計(jì)算精度較高;當(dāng)四旋翼無人機(jī)飛行至15 s左右時(shí),通過計(jì)算獲取的位置曲線,與預(yù)設(shè)位置曲線間的差距較大,說明此時(shí)無人機(jī)已偏航。

利用本文方法對采集的參考目標(biāo)進(jìn)行角點(diǎn)檢測,角點(diǎn)檢測結(jié)果如圖3所示。

由圖可知,本文方法可有效檢測參考模板圖像的角點(diǎn),共檢測到8個(gè)角點(diǎn)。依據(jù)這8個(gè)角點(diǎn),利用本文方法進(jìn)行四旋翼無人機(jī)偏航角計(jì)算。在偏航角控制過程中,每次控制偏航角旋轉(zhuǎn)5°。對比本文方法控制下的偏航角與實(shí)際偏航角之間的誤差,以驗(yàn)證本文方法的控制性能。偏航角計(jì)算結(jié)果見表1。

分析表1可知,在不同無人機(jī)偏航角下,本文方法均可有效獲取無人機(jī)的偏航角,且用本文方法獲取的偏航角與實(shí)際偏航角差距較小,最大誤差僅有0.725 1°,可以忽略不計(jì)。實(shí)驗(yàn)證明本文方法可精準(zhǔn)計(jì)算四旋翼無人機(jī)偏航角。

當(dāng)四旋翼無人機(jī)飛行過程中出現(xiàn)瞬間風(fēng)擾,且風(fēng)擾時(shí)間在10 s左右,利用本文方法對四旋翼無人機(jī)進(jìn)行偏航控制,設(shè)定偏航角為8°,本文方法的偏航控制結(jié)果如圖4所示。

圖4 四旋翼無人機(jī)偏航控制結(jié)果

分析圖4(a)可知,當(dāng)無人機(jī)飛行至15 s左右時(shí),遭遇風(fēng)擾,出現(xiàn)偏航問題,本文方法可快速對其進(jìn)行偏航控制,通過10 s左右的時(shí)間,將偏航角控制在設(shè)定值附近,且超調(diào)量較小。

根據(jù)圖4(b)可知,當(dāng)無人機(jī)飛行至15 s前,無人機(jī)無偏航問題,因此偏航控制量為0 N·m;當(dāng)飛行至15 s時(shí),出現(xiàn)偏航問題,本文方法可快速解決偏航問題,最大偏航控制量在0.07 N·m左右,經(jīng)過10 s左右控制后,恢復(fù)正常。

綜合分析可知,本文方法可有效實(shí)現(xiàn)無人機(jī)偏航控制,且偏航控制速度較快,超調(diào)量較小,且無振蕩現(xiàn)象,具備較優(yōu)的偏航控制抗干擾性能。

3 結(jié)束語

多旋翼無人機(jī)的偏航運(yùn)動(dòng)能力較差,這主要是因?yàn)槠竭\(yùn)動(dòng)需要同時(shí)調(diào)整多個(gè)旋翼的轉(zhuǎn)速以實(shí)現(xiàn)轉(zhuǎn)向,這就增加了偏航控制的復(fù)雜性。為此,研發(fā)了基于機(jī)器視覺的多旋翼無人機(jī)偏航控制方法,提升偏航控制精度,確保多旋翼無人機(jī)穩(wěn)定飛行。