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

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

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

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

1 多旋翼無人機飛行偏航控制

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

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

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

(2)

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

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

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

(3)

(4)

步驟2,令多旋翼無人機的飛行高度為h1,對應(yīng)的相機歐拉旋轉(zhuǎn)角為φ1、θ1。通過式(3)與式(4)可獲取相機旋轉(zhuǎ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分別為相機歐拉旋轉(zhuǎn)角φ1、θ1的旋轉(zhuǎn)矩陣。

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

(6)

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

步驟4,依據(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分別為相機歐拉旋轉(zhuǎn)角φ2、θ2的旋轉(zhuǎn)矩陣。

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

(10)

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

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

1.2 基于機器視覺的多旋翼無人機偏航角計算

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

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

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

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

候選角點判定條件為:

(11)

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

通過匹配檢測到的角點與參考圖像角點,估計偏航角。其角點匹配步驟如下:

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

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

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

按照m個匹配角點對,估計多旋翼無人機的偏航角,具體步驟如下:

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

(12)

其中:

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

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

ψ=arctan(q21,q11)

(13)

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

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

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

(14)

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

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

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

(15)

c的計算公式如下:

c=KPeα+KIξ

(16)

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

(17)

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

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

(18)

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

2 實驗分析

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

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

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

圖中,白色矩形框為四旋翼無人機的地面工作站,即參考目標(biāo)。由圖1可知,本文方法可有效采集參考目標(biāo)圖像,且采集的參考目標(biāo)圖像清晰度較高。

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

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

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

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

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

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

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

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

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

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

3 結(jié)束語

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