基于空間兩點(diǎn)的視覺自主著陸導(dǎo)引算法設(shè)計(jì)

魏祥灰， 唐超穎， 王彪

(南京航空航天大學(xué) 自動(dòng)化學(xué)院， 南京 211106)

垂直起降無(wú)人機(jī)(UAV of the type Vertical Take Off and Landing,VTOL UAV)被越來(lái)越廣泛地運(yùn)用到動(dòng)目標(biāo)跟蹤、緊急著降以及危險(xiǎn)救援等任務(wù)[1-2]中，這對(duì)其導(dǎo)引效率與精度要求越來(lái)越高[3]。由于GPS信號(hào)的局限性，基于視覺傳感器的導(dǎo)引方式受到了越來(lái)越多的關(guān)注[4-5]。本文以無(wú)人機(jī)著陸為背景，采用視覺傳感器，設(shè)計(jì)和研究垂直起降無(wú)人機(jī)的高效率導(dǎo)引算法。

目前，從實(shí)現(xiàn)導(dǎo)引的方式上，國(guó)內(nèi)外的無(wú)人機(jī)視覺著陸研究可分為基于位置的導(dǎo)引方式和基于速度的導(dǎo)引方式，這2種方式都需要使用視覺解算當(dāng)前位置，容易引入計(jì)算誤差[6]。其中基于位置的導(dǎo)引方式[7-8]是指利用視覺信息解算無(wú)人機(jī)當(dāng)前位置，計(jì)算其與預(yù)設(shè)基準(zhǔn)軌跡的位置偏差，采用糾偏處理，使無(wú)人機(jī)按照既定的軌跡飛行。這種導(dǎo)引方式便于理解分析，被當(dāng)前大多數(shù)研究人員采用[9-10]。基于速度的導(dǎo)引方式[11-12]是指建立無(wú)人機(jī)當(dāng)前位置與速度指令之間的關(guān)系，不計(jì)較導(dǎo)引過程中的位置誤差，以一定著陸精度完成定點(diǎn)著陸。這種方式具備計(jì)算復(fù)雜度低、實(shí)時(shí)性好等特點(diǎn)，能夠快速高效地完成導(dǎo)引任務(wù)。Min等[13]采用基于速度的導(dǎo)引方式，設(shè)計(jì)以時(shí)間為基礎(chǔ)的縱向加速度指令生成算法，實(shí)現(xiàn)豎直方向上的著陸。

另外，為實(shí)現(xiàn)無(wú)人機(jī)精準(zhǔn)導(dǎo)引，目前使用的導(dǎo)引法主要有比例導(dǎo)引法、速度向量場(chǎng)法和線性反饋法等[14]。比例導(dǎo)引律是指飛行器在攻擊或者追蹤目標(biāo)的導(dǎo)引過程中，速度矢量的轉(zhuǎn)動(dòng)角速度與目標(biāo)瞄準(zhǔn)線(視線)的轉(zhuǎn)動(dòng)角速度成比例的一種導(dǎo)引律[15]，在導(dǎo)引的末端，運(yùn)動(dòng)軌跡近似為直線，不能滿足垂直起降無(wú)人機(jī)著陸瞬間速度方向豎直向下的要求，在無(wú)人機(jī)著陸領(lǐng)域，比例導(dǎo)引法使用很少。速度向量場(chǎng)法是一種常用的無(wú)人機(jī)導(dǎo)引方法。針對(duì)垂直起降無(wú)人機(jī)，國(guó)內(nèi)外學(xué)者幾乎都采用“直角型”的轉(zhuǎn)彎方式[16-17]。由此設(shè)計(jì)的速度向量場(chǎng)，需要無(wú)人機(jī)在“直角點(diǎn)”懸停一段時(shí)間再轉(zhuǎn)彎飛行，這嚴(yán)重影響了無(wú)人機(jī)著陸效率。假若不懸停、直接直角轉(zhuǎn)彎，對(duì)無(wú)人機(jī)的方向機(jī)動(dòng)性性能要求過高，難以工程實(shí)現(xiàn)。張炳亮[18]設(shè)計(jì)了基于圓的速度向量場(chǎng)，但該方案只適用于飛行高度與水平著陸距離相當(dāng)?shù)那闆r，不具有普適性。

針對(duì)以上無(wú)人機(jī)著陸效率較低的問題，一方面，從減少著陸路程和降低飛行機(jī)動(dòng)性能要求的角度，設(shè)計(jì)速度向量場(chǎng)，提高無(wú)人機(jī)的轉(zhuǎn)彎效率。另一方面，采用基于速度的著陸導(dǎo)引方式，選取合適的視覺合作特征，建立圖像信息與速度指令之間的直接關(guān)系。避免位置解算及其引入的計(jì)算耗時(shí)和誤差，提高著陸導(dǎo)引效率。

本文首先結(jié)合無(wú)人機(jī)著陸的速度方向以及轉(zhuǎn)彎能力等要求，設(shè)計(jì)速度向量場(chǎng)，同時(shí)討論相關(guān)參數(shù)與無(wú)人機(jī)性能的約束關(guān)系；然后，研究圖像特征與航跡傾斜角、航跡方位角和速度大小的關(guān)系，推導(dǎo)出合理的著陸導(dǎo)引規(guī)律；最后，利用Simulink仿真平臺(tái)驗(yàn)證本文設(shè)計(jì)導(dǎo)引律的正確性和準(zhǔn)確性。

1 速度向量場(chǎng)的設(shè)計(jì)與分析

本節(jié)以1/4橢圓弧為單位，設(shè)計(jì)著陸的速度向量場(chǎng)，并分析此時(shí)無(wú)人機(jī)的機(jī)動(dòng)性能要求。以弧上各點(diǎn)的切線方向確定航跡傾斜角的理想值(見圖1)，從而滿足著陸瞬間機(jī)體速度方向豎直向下的需求。定義航跡傾斜角γ為機(jī)體速度Vb與水平面的夾角。建立縱平面的坐標(biāo)系，其中C1點(diǎn)為目標(biāo)位置，橢圓表達(dá)式為

(1)

式中：x≥0，y≥0，a>0，b>0，橢圓弧與y軸的交點(diǎn)為(0,a)，與x軸交點(diǎn)為C1：(b,0)。設(shè)橢圓曲線上任意一點(diǎn)M的坐標(biāo)為(xM,yM)，則該點(diǎn)航跡傾斜角γM為

(2)

以x=0以及x=2b軸上任意點(diǎn)，與C1點(diǎn)作橢圓弧，建立目標(biāo)著陸點(diǎn)周圍的速度向量場(chǎng)(見圖2)。空間中任意一點(diǎn)N(xN,yN)的航跡傾斜角γN表達(dá)式為

圖1 橢圓弧示意圖Fig.1 Schematic diagram of elliptic arc

圖2 速度向量場(chǎng)示意圖Fig.2 Schematic diagram of velocity vector field

(3)

式中：aN為點(diǎn)N所在橢圓弧與y軸交點(diǎn)的縱坐標(biāo)。

以軌跡中各點(diǎn)的曲率半徑為衡量指標(biāo)，分析橢圓弧對(duì)機(jī)動(dòng)性能的要求，半徑越小表示對(duì)無(wú)人機(jī)轉(zhuǎn)彎?rùn)C(jī)動(dòng)能力要求越高。假設(shè)著陸的光滑曲線L在著陸平面上的表達(dá)式為yL=f(xL)，則點(diǎn)(xL,yL)的曲率半徑為

(4)

若點(diǎn)(xL,yL)不可求導(dǎo)，則

lim(R(xL,yL))=0

(5)

將式(1)代入式(4)計(jì)算軌跡的曲率半徑，得到最大曲率半徑Rmax與最小曲率半徑Rmin分別為

Rmax=(max{a,b})2/min{a,b}

(6)

Rmin=(min{a,b})2/max{a,b}

(7)

結(jié)合式(7)，若無(wú)人機(jī)固有的轉(zhuǎn)彎?rùn)C(jī)動(dòng)性能對(duì)應(yīng)的轉(zhuǎn)彎半徑為Rplane，則滿足約束條件Rplane

2 導(dǎo)引律的設(shè)計(jì)

2.1 圖像特征與速度方向的關(guān)系

無(wú)人機(jī)在三維空間中的速度方向由航跡傾斜角γ和航跡方位角χ兩個(gè)角度確定，其中航跡方位角表示速度在水平面的投影與地面固定軸的夾角。

機(jī)體坐標(biāo)系XbYbZb、相機(jī)坐標(biāo)系XcYcZc以及像素坐標(biāo)系uv之間的關(guān)系如圖4所示，其中特征點(diǎn)C1的像點(diǎn)c1在相機(jī)光軸上，這是由云臺(tái)的跟瞄功能保證的，在無(wú)人機(jī)的飛行過程中，相機(jī)能完成快速對(duì)準(zhǔn)，使得光軸OcZc一直保持指向點(diǎn)C1。機(jī)體坐標(biāo)系一點(diǎn)P(xb,yb,zb)在圖像中的像點(diǎn)坐標(biāo)為(u,v)，由相機(jī)成像原理可知兩坐標(biāo)關(guān)系為

(8)

式中：Du=-zcdu/f，Dv=-zcdv/f，f為相機(jī)焦距，du與dv分別為像素坐標(biāo)系u軸、v軸方向上單位像素對(duì)應(yīng)的物理長(zhǎng)度，一般地，du=dv；zc為點(diǎn)P在相機(jī)坐標(biāo)系下的Z軸坐標(biāo)；u0與v0為相機(jī)主點(diǎn)位置；Rbc與Tbc分別為機(jī)體坐標(biāo)系與相機(jī)坐標(biāo)系之間的旋轉(zhuǎn)矩陣和平移矩陣。

(9)

圖3 本文導(dǎo)引律的結(jié)構(gòu)框圖Fig.3 Structure diagram of proposed guidance law

圖4 機(jī)體坐標(biāo)系與像素坐標(biāo)系的關(guān)系示意圖Fig.4 Schematic diagram of relationship between body coordinate system and image coordinate system

(10)

(11)

則Rbc表達(dá)式為

(12)

由式(8)可得圖像中某一向量(Δu,Δv)對(duì)應(yīng)機(jī)體坐標(biāo)系下的Xb-Yb二維單位矢量(Δxb,Δyb)的關(guān)系如下：

(13)

式中：k為尺度因子；R的表達(dá)式為

(14)

(15)

代入式(13)，可得航跡方位角指令χcmd表達(dá)式為

(16)

由式(16)，得到圖像信息與航跡方位角指令的關(guān)系，接下來(lái)探討圖像信息與航跡傾斜角的關(guān)系，用圖像信息表示公式(3)。圖5展示了無(wú)人機(jī)與合作特征的空間關(guān)系，其中α為相機(jī)光軸與世界坐標(biāo)系-Zw軸的夾角，β為視線MC1與MC2構(gòu)成的夾角，α與β反映了機(jī)體與目標(biāo)之間的相對(duì)位置關(guān)系，并且滿足α≥β。

其中由ΔMC1C2的正弦定理可得

(17)

點(diǎn)M(xM,yM)坐標(biāo)表達(dá)式為

(18)

(19)

將式(17)代入式(18)與式(19)，得到α、β與點(diǎn)M坐標(biāo)的關(guān)系為

(20)

(21)

將式(20)、式(21)代入式(3)，得到點(diǎn)M處航跡傾斜角指令γcmd的表達(dá)式為

(22)

式中：b為無(wú)人機(jī)在初始位置時(shí)與目標(biāo)點(diǎn)的水平距離：

(23)

式中：下標(biāo)0表示相應(yīng)變量的初始值；α的大小由云臺(tái)系統(tǒng)給出的2個(gè)轉(zhuǎn)角(傾角θY和滾轉(zhuǎn)角φY)求出:

(24)

目標(biāo)視線夾角β與圖像信息之間的關(guān)系如圖6所示，其中Occ1⊥c1c2，得出β的表達(dá)式為

(25)

聯(lián)列式(22)～式(25)可得點(diǎn)M處，航跡傾斜角指令γcmd與圖像信息之間的關(guān)系表達(dá)式為

在實(shí)際應(yīng)用中，角度測(cè)量具有精度限制，同時(shí)α、β的測(cè)量精度取決于特征矢量的高度C。接下來(lái)討論角度精度與特征高度之間的關(guān)系。假設(shè)無(wú)人機(jī)從距離著陸點(diǎn)豎直高度為h，水平距離為l的位置著陸，則得到本文設(shè)計(jì)的橢圓著陸軌跡的參數(shù)表達(dá)式為

(26)

式中：0<θ<π/2。假設(shè)無(wú)人機(jī)著陸末端允許的豎直高度偏差為hT，即當(dāng)高度小于等于hT時(shí)，無(wú)人機(jī)可以將速度降為0。則由y=hT得到著陸末端允許的最大位置偏差(Δx,Δy)為

(27)

如圖5所示，設(shè)無(wú)人機(jī)某一點(diǎn)的位置為(x,y)，特征點(diǎn)C1、C2的空間位置分別為(b,0)，(b,-C)。由ΔPC1C2中的余弦定理，可得α、β與(x,y)的關(guān)系為

(28)

由式(28)可知，在確定無(wú)人機(jī)和目標(biāo)點(diǎn)的位置時(shí)，合作矢量的高度C與β成正相關(guān)。將式(1)代入式(28)得到β與y之間的關(guān)系：

β=arccos(g(y))

(29)

經(jīng)計(jì)算g(y)在x≥0、y≥0的范圍內(nèi)為關(guān)于y的單調(diào)遞減函數(shù)，故β的大小與y為正相關(guān)。假設(shè)合作特征高度矢量的高度值為C，利用式(27)計(jì)算得到的偏差(Δx,Δy)代入式(29)，可得

Δβ=

(30)

若(Δx,Δy)表示無(wú)人機(jī)能夠允許的位置偏差，則相應(yīng)的Δβ表示著陸系統(tǒng)允許的β值的測(cè)量偏差(即目標(biāo)視線夾角的精度，用dβ表示)，由此可得精度dβ與C的關(guān)系：

dβ=

(31)

同理可得α的精度dα：

(32)

由此可知，在針對(duì)不同對(duì)象和場(chǎng)景應(yīng)用時(shí)，可以根據(jù)應(yīng)用背景的參數(shù)h、l，以及無(wú)人機(jī)和相機(jī)傳感器的性能參數(shù)hT、dβ求出對(duì)應(yīng)的合作矢量的高度C。

2.2 圖像特征與速度大小的關(guān)系

由于α≥β，故本文建立α與速度大小指令|Vcmd|的關(guān)系。將式(1)代入式(28)，xP為無(wú)人機(jī)與目標(biāo)的水平距離，則α與xP之間的關(guān)系表示為

α=arctan(f(xP))

(33)

經(jīng)計(jì)算f(xP)在xP≥0，yP≥0的范圍內(nèi)為關(guān)于xP的單調(diào)遞減函數(shù)，故α與xP呈負(fù)相關(guān)。由于|Vcmd|與xP也呈負(fù)相關(guān)。故本文以參數(shù)α1、α2構(gòu)成速度大小指令|Vcmd|的分段函數(shù)如式(34)所示，其中V0為無(wú)人機(jī)初始速度的大小，當(dāng)α2>α>α1時(shí)，|Vcmd|的變化與α的變化成比例關(guān)系。

(34)

綜上，無(wú)人機(jī)的機(jī)體坐標(biāo)系下速度指令：

(35)

3 仿真驗(yàn)證與分析

3.1 與傳統(tǒng)方法比較分析

傳統(tǒng)著陸軌跡與本文著陸軌跡如圖7所示。其中傳統(tǒng)著陸方式下的軌跡表達(dá)式為

(36)

對(duì)比本文提出的軌跡表達(dá)式(式(1))，得到著陸距離L1、L2分別為

(37)

由此可見，在本文提出的著陸距離更短，并且水平方向距離目標(biāo)越遠(yuǎn)(b值越大)，本文算法在著陸距離上的優(yōu)勢(shì)越明顯。

另外，分析比較2種軌跡的最小曲率半徑，以此評(píng)價(jià)2種著陸方式對(duì)無(wú)人機(jī)的轉(zhuǎn)彎?rùn)C(jī)動(dòng)性能的需求。本文設(shè)計(jì)軌跡的最小曲率半徑由式(7)給出，當(dāng)參數(shù)a或b固定，滿足a=b時(shí)，理想軌跡的最小曲率半徑Rmin最大。由式(4)、式(5)的曲率半徑計(jì)算原理，得到傳統(tǒng)軌跡的曲率半徑RT為

(38)

由此可見，與傳統(tǒng)著陸方式相比，本文的著陸軌跡對(duì)機(jī)動(dòng)性能要求更低。傳統(tǒng)方式要求轉(zhuǎn)彎?rùn)C(jī)動(dòng)性能無(wú)限高，而這在實(shí)際應(yīng)用中無(wú)法達(dá)到，所以工程中通過在轉(zhuǎn)彎處懸停的方式，通過消耗時(shí)間，降低對(duì)機(jī)動(dòng)性能的高要求。

綜上，與傳統(tǒng)著陸方式相比，本文的導(dǎo)引方法實(shí)現(xiàn)了著陸距離更短，機(jī)動(dòng)性要求更低，效率更高。

圖7 著陸軌跡的比較Fig.7 Comparison of landing trajectory

3.2 系統(tǒng)仿真驗(yàn)證

基于Simulink搭建了視覺著陸導(dǎo)引系統(tǒng)仿真平臺(tái)，將整個(gè)系統(tǒng)分為X450四旋翼模型、飛行控制器、著陸導(dǎo)引算法、云臺(tái)系統(tǒng)、視覺探測(cè)算法、小孔成像、目標(biāo)七大模塊，各個(gè)模塊之間通過各自的輸入輸出信號(hào)建立關(guān)聯(lián)，系統(tǒng)仿真結(jié)構(gòu)框圖如圖8所示。

圖8中，X450四旋翼模型為筆者課題組已有的X450型四旋翼非線性模型，飛行控制器包括機(jī)體的速度、姿態(tài)、角速率的閉環(huán)控制，云臺(tái)系統(tǒng)由云臺(tái)模型與云臺(tái)跟瞄控制器組成，小孔成像模塊將世界坐標(biāo)系中的目標(biāo)投影到像素坐標(biāo)系中，視覺探測(cè)算法獲取目標(biāo)信息后，將視覺解算信息提供給云臺(tái)系統(tǒng)。為便于描述，本文假設(shè)相機(jī)、云臺(tái)質(zhì)心與機(jī)體質(zhì)心重合。

(39)

圖9(a)為著陸過程中某時(shí)刻的狀態(tài)，其中曲線表示已飛經(jīng)過的軌跡，左端為無(wú)人機(jī)的初始位置，右端為當(dāng)前位置，其中的藍(lán)色圖形表示相機(jī)的空間姿態(tài)，下面的藍(lán)色圖形為合作目標(biāo)，該圖可動(dòng)態(tài)地演示視覺導(dǎo)引無(wú)人機(jī)著陸的全過程。圖9(b)為對(duì)應(yīng)的著陸豎直平面內(nèi)的二維著陸軌跡，其中平行于時(shí)間軸的紅線表示著陸目標(biāo)點(diǎn)的高度1 m。

無(wú)人機(jī)的導(dǎo)引指令在著陸過程中的變化如圖10所示，航跡傾斜角指令γcmd由初始值逐漸增加到末端期望值(圖10(a))；航跡方位角指令χcmd與期望值曲線走勢(shì)一致，并在期望值上下波動(dòng)，這主要受成像誤差的影響(圖10(b))；隨著無(wú)人機(jī)越來(lái)越接近目標(biāo)，速度越來(lái)越小(圖10(c))，到達(dá)目標(biāo)位置時(shí)，速度方向指令接近豎直向下，大小為零。

圖8 系統(tǒng)仿真平臺(tái)結(jié)構(gòu)框圖Fig.8 Structure diagram of system simulation platform

圖9 無(wú)人機(jī)著陸過程仿真示意圖Fig.9 Schematic diagram of UAV landing process simulation

圖10 導(dǎo)引指令隨時(shí)間的變化Fig.10 Variation of guidance command with time

無(wú)人機(jī)速度響應(yīng)、位置響應(yīng)以及航跡傾斜角響應(yīng)值在著陸過程中的變化如圖11所示，可見，在本文算法約束下，無(wú)人機(jī)在接近目標(biāo)點(diǎn)時(shí)，速度緩慢降低，到達(dá)目標(biāo)位置時(shí)，速度大小為零，方向豎直向下，實(shí)現(xiàn)了垂直軟著陸。

航跡傾斜角指令是由圖像信息解算的目標(biāo)視線夾角β以及云臺(tái)偏角α計(jì)算所得，接下來(lái)驗(yàn)證角度的測(cè)量準(zhǔn)確性。圖12和圖13分別表示著陸過程中，兩夾角的真實(shí)值與測(cè)量值的關(guān)系。其中真實(shí)值與測(cè)量值的絕對(duì)誤差值Δβ與Δα的表達(dá)式為

Δβ=|β-βr|

(40)

Δα=|α-αr|

(41)

由式(40)得到誤差Δβ一直保持在0.14°內(nèi)，α的測(cè)量值在著陸始端誤差不超過0.9°，并且隨著無(wú)人機(jī)距離目標(biāo)越近，很快誤差保持在0.1°以內(nèi)。由此可知本文算法能準(zhǔn)確地解算出角度值，并且距離目標(biāo)越近則準(zhǔn)確度越高，滿足實(shí)際應(yīng)用的要求。

圖11 響應(yīng)隨時(shí)間的變化Fig.11 Variation of response with time

圖12 β角分析Fig.12 β angle analysis

圖13 α角分析Fig.13 α angle analysis

4 結(jié) 論

在無(wú)人機(jī)著陸中的導(dǎo)引研究中，基本上使用基于位置的方式，為提高導(dǎo)引效率，本文設(shè)計(jì)了基于空間兩點(diǎn)的視覺自主著陸導(dǎo)引算法，為今后實(shí)現(xiàn)自主、智能的無(wú)人機(jī)提供理論研究基礎(chǔ)。本文的創(chuàng)新性主要概括為以下2個(gè)方面：

1) 突破傳統(tǒng)直角型的轉(zhuǎn)彎方式，設(shè)計(jì)曲線型速度向量場(chǎng)，導(dǎo)引垂直起降無(wú)人機(jī)曲線著陸，保證末端約束的前提下，提高無(wú)人機(jī)的著陸效率。

2) 避免傳統(tǒng)導(dǎo)引算法中的位置解算，直接建立圖像信息與速度指令之間的關(guān)系，避免了位置解算引入的計(jì)算量和誤差，提高計(jì)算效率。