基于矢量場法的無人機三維路徑跟蹤控制

顏宗淵，王道波，王博航，周晨昶，姜 燕

(南京航空航天大學(xué)自動化學(xué)院，江蘇 南京 211106)

0 引言

無人機路徑跟蹤問題可以由3個任務(wù)來描述：跟蹤移動目標、跟蹤航點路徑以及在固定航點處運動[1]。路徑跟蹤最新發(fā)展主要可以分為3類：基于動力學(xué)和運動學(xué)模型的算法[2-3]、基于虛擬目標點的算法[4]和基于矢量場的算法[5]。盡管基于動力學(xué)和運動學(xué)模型的路徑跟蹤算法可以保證足夠的跟蹤性能和穩(wěn)定性，但是其制導(dǎo)命令依賴于模型并且復(fù)雜，從而增加了計算復(fù)雜度[6]。虛擬目標點制導(dǎo)算法源自導(dǎo)彈制導(dǎo)系統(tǒng)中使用的視線制導(dǎo)方法，現(xiàn)已發(fā)展為非線性制導(dǎo)律[4,7-8]，但是，與其他制導(dǎo)律方法相比，該方法引入了更多的簡便性和更少的控制工作，因為它沒有考慮到無人機的運動學(xué)和動力學(xué)約束，這些約束在實際應(yīng)用中會降低制導(dǎo)律的性能?；谑噶繄龅穆窂礁櫵惴ǖ幕舅枷胧且獦?gòu)造一個理想的航向場將無人機移動到路徑上，因此，制導(dǎo)律的計算取決于路徑的類型[9]。直線和圓路徑是最常見的路徑，直線和圓也可以組合構(gòu)造更復(fù)雜的路徑。研究發(fā)現(xiàn)基于矢量場的路徑跟蹤算法具有較大優(yōu)勢，可以不依賴模型，且計算復(fù)雜度較小，可以加入無人機的運動學(xué)和動力學(xué)約束，跟蹤性能好[10]。本文將二維矢量場法推廣到三維空間，實現(xiàn)固定翼無人機的三維路徑跟蹤控制。

針對航段切換問題，采用基于圓角幾何的切換準則，根據(jù)無人機是否通過半平面作為切換策略，保證無人機可以平滑過渡2條相鄰航段。仿真結(jié)果表明，本文提出的三維路徑跟蹤算法具有極佳的跟蹤性能。

1 無人機運動學(xué)模型

本文主要研究外回路產(chǎn)生的制導(dǎo)指令使無人機跟隨空間三維路徑。建立無人機的運動學(xué)模型為

(1)

由于風(fēng)對無人機影響的效應(yīng)還未研究清楚，一般將風(fēng)的影響作為擾動輸入，由無人機的飛行控制系統(tǒng)實時處理。因此在進行路徑跟蹤時，通常不會將風(fēng)的影響考慮進去，認為UAV的空速等于地速，航向角ψ認為等于航線角(假設(shè)側(cè)滑角β等于0)[9]。圖1展示了無人機空速V、航向角ψ和航跡角γ。

圖1 無人機運動模型圖示

2 矢量場法

制導(dǎo)律將采用在文獻[11]提出的矢量場法。需要跟蹤的在三維空間中的路徑是2個二維流形的交線，其中二維流形分別由α1(r)=0和α2(r)=0定義，α1和α2為有界的二階偏微分，r∈R3。一個基本的假設(shè)是交線給出的路徑是連通的，并且是一維的。定義函數(shù)為

(2)

給出V(r)的一階導(dǎo)數(shù)為

(3)

式(3)存在的一個問題是計算得到的u′或許不會等于無人機的空速V，因此,u′需要標準化為

(4)

當(dāng)u′被標準化為式(4)的值時，由文獻[11]證明得到的穩(wěn)定性依然是有效的。

將無人機在慣性坐標系的速度分量與式(4)得到的速度u=(u1,u2,u3)T進行等價得到

(5)

對式(5)進行求解可以得到期望的航跡γc和期望的航向角ψd，表達式為

(6)

飽和函數(shù)被定義為

(7)

假設(shè)通過協(xié)調(diào)轉(zhuǎn)彎方程對內(nèi)環(huán)橫向動力學(xué)方程進行了精確建模，則可以從式(8)獲得產(chǎn)生理想轉(zhuǎn)彎性能的滾轉(zhuǎn)角命令值，即

(8)

kφ為一個正常數(shù)。

2.1 直線路徑

直線可以由直線的方向和直線上的一個點描述。讓cL=(cn,ce,cd)T表示直線上任意的一個點，直線的方向可以由在東北坐標系下的相對正北方向的航向角ψL和相對水平面的航跡角γL表示。因此，直線的方向可以表示為

(9)

qL為一個指向期望直線的方向單位向量。這條直線可以表示為

(10)

垂直于由qL定義的縱向平面的單位向量定義為

(11)

相似地，垂直于由qL定義的橫向平面的單位向量為

(12)

因此，pline由以下定義的平面的交線給定

(13)

(14)

因此，在標準化前，期望的速度矢量可以表示為

(15)

圖2表示了qL、cL以及由αlon(r)=0和αlat(r)=0定義的平面。

圖2 由2個平面定義的空間直線

2.2 螺旋線路徑

時間參數(shù)化的螺旋路徑為

(16)

(17)

Rh為半徑；λ=+1為螺旋線順時針旋轉(zhuǎn)；λh=-1為螺旋線逆時針旋轉(zhuǎn)；γh為沿著螺旋線的航跡角。

為了找到2個面可以定義螺旋線路徑，式(16)中的時間參數(shù)需要消除，式(16)可以表示為

(18)

除此之外，將式(16)的東向分量除以北向分量可以得到

(19)

求解t，并將其代入到式(16)中的第3個表達式中得到

(20)

除以Rh將這些等式標準化可以得到

(21)

(22)

用除以Rh去進行歸一化可以使得控制策略的增益不會隨著軌道的大小變化而變化。αcyl和αpl的梯度可以表示為

(23)

(24)

在標準化前，期望的速度矢量可以表示為

(25)

3 三維路徑跟蹤算法

3.1 基于圓角幾何的切換策略

圖3 圓角幾何策略

三維空間里2條線段的切點的計算如下所述：

a.將空間中相鄰的3個航點Wi-1、Wi和Wi+1投影到二維平面，得到wi-1、wi和wi+1。

b.通過下式計算出二維平面內(nèi)表示相鄰航段方向的向量：

(26)

(27)

c.通過圖3右側(cè)的表達式計算得到a、b、c點。

d.a、b、c是A、B、C在二維平面的投影點，且A、B在航段上，由此可以計算得出A、B切點;C是螺旋線的圓心，起始高度為A切點高度，由此得到C的坐標。

3.2 切換策略的算法步驟

在切換策略算法中，pathstate是一個切換標志，如果pathstate=1，則代表無人機將沿著直線路徑運動；pathstate=2，則代表無人機將沿著螺旋線運動；ψpath代表直線路徑的航向，對于螺旋線來說，代表其起始點的航向；γpath是所走路徑相對水平面的夾角；對于直線路徑，λpath=0、λpath=1代表螺旋線是順時針方向，λpath=-1代表螺旋線是逆時針方向；ρ是螺旋線的半徑；pathflag是一個路徑標志，輸出用于下一層路徑跟蹤算法，pathflag=1代表無人機將跟隨直線路徑，pathflag=2代表無人機將跟隨螺旋線路徑；C是螺旋線路徑的圓心位置坐標，對于直線，是起始航點的坐標。

輸入是航點數(shù)據(jù)pathwaypoit={w1,w2,…,wn}、無人機的位置p=(pn,pe,pd)和最大航跡角γmax，根據(jù)無人機的位置實時更新輸出值，算法流程如圖4所示。

圖4 基于圓角幾何的切換策略算法流程

切換策略算法初始化設(shè)置pathstate=1，計算出由3個航點構(gòu)成的2條相鄰航段方向，采用螺旋線擬合相鄰航段，因此，形成了2條直線路徑和1條螺旋線路徑。無人機先沿著直線路徑運動，計算得到直線路徑參數(shù)，用于輸出給路徑跟蹤模塊，當(dāng)?shù)竭_半平面H1，無人機開始處于航段過渡階段，pathstate=2，將沿著螺旋線路徑運動，計算得到螺旋線路徑參數(shù)。當(dāng)無人機到達半平面H2，說明航段過渡階段已經(jīng)運動完，pathstate=1，無人機將開始沿著直線路徑運動，重復(fù)以上步驟，計算路徑跟隨所需參數(shù)。

3.3 路徑跟蹤算法步驟

圖5 基于矢量場法的路徑跟蹤算法流程

路徑跟蹤算法根據(jù)pathflag參數(shù)確定將要跟隨路徑的參數(shù)。當(dāng)pathflag=1時，無人機將要跟隨直線路徑，采用式(15)計算出跟隨直線路徑所需的速度矢量u′=(u1,u2,u3)T；當(dāng)pathflag=2時，無人機將要跟隨螺旋線路徑，采用式(25)計算出跟隨螺旋線路徑所需的速度矢量u′=(u1,u2,u3)T。由得到的速度矢量u′，計算得到期望的空速Vc、期望的航跡角γc、期望的航向角ψc和期望的滾轉(zhuǎn)角φc。將這些期望參數(shù)輸入給無人機控制回路中，從而實現(xiàn)無人機的路徑跟蹤。

4 仿真實驗

對本文提出的制導(dǎo)方案進行仿真驗證，以評估算法的性能和魯棒性，無人機的詳細模型詳見文獻[9]，使用硬件是個人計算機Intel Core i7-8750H CPU @2.20 GHz 16 GB RAM。將無人機的起始點和初始航向分別設(shè)置為不同的初始值，對直線、螺旋線路徑和由此組合的復(fù)雜路徑進行跟蹤，進行多次仿真實驗，驗證算法的跟蹤性能。

4.1 直線路徑

參數(shù)kφ會影響無人機在橫向平面的收斂性能和跟蹤表現(xiàn)，kφ的取值范圍需在0.2～0.6之間，如果kφ大于0.7，無人機跟隨路徑可能會出現(xiàn)振蕩現(xiàn)象，如果小于0.2，跟蹤效果比較差，本文算法將其取為0.3。矩陣K1=diag([k11k22k33])是一個對角矩陣；k11和k22會影響無人機在橫向平面上對路徑的收斂性能，k33會對其在縱向平面的收斂性能。k11和k22的取值范圍在5～20之間，本文取9；k33的取值范圍在5～35之間，過大會使得無人機的俯仰角出現(xiàn)振蕩現(xiàn)象，本文取15；K2會影響無人機對路徑的跟蹤性能，取值范圍在140～180之間，本文取150。改變無人機的起始點和初始航向，對2個航點直線軌跡進行跟蹤，仿真結(jié)果如圖6所示，可以看到無人機能夠快速跟蹤設(shè)定的直線，而且跟蹤誤差很小。

圖6 直線路徑跟蹤結(jié)果

4.2 螺旋線路徑

kφ、K1、K2這些參數(shù)無人機對螺旋線路徑跟蹤的影響和直線路徑類似，將kφ取值為1.5，k11和k22都取值為20，k33取值為25，K2取值為150。改變無人機的起始點和初始航向，對螺旋線路徑進行跟蹤，仿真結(jié)果如圖7所示。初始時刻無人機對于螺旋線跟蹤誤差相對較大，這是因為無人機的初始航向與預(yù)設(shè)路徑的航向偏差較大，且距離相對較遠，但無人機能夠迅速消除誤差，跟蹤螺旋線路徑，而且之后跟蹤誤差很小。

圖7 螺旋線路徑跟蹤結(jié)果

4.3 組合復(fù)雜路徑

在本節(jié)中，無人機要跟蹤根據(jù)航點設(shè)定的一系列軌跡，在2個航段的交接處，無人機將會對飛行螺旋線進行平滑過渡，仿真結(jié)果如圖8所示。從圖8可以看到，無人機很好地跟蹤所有路徑，且在切換點處沒有比較大的超調(diào)，跟蹤誤差極小。期望俯仰角、滾轉(zhuǎn)角、空速與實際對比的結(jié)果和誤差值的結(jié)果如圖9和圖10所示。在各過渡位置處，無人機的俯仰角和期望滾轉(zhuǎn)角會出現(xiàn)波動，因為無人機需要調(diào)整姿態(tài)來跟蹤下一段路徑；期望值與實際值在過渡位置存在一定偏差，這是因為無人機模型存在動力學(xué)和運動學(xué)約束，導(dǎo)致在過渡點實際值無法等于期望值，不過誤差很快就會趨于0，這也驗證了本文提出算法的可行性。

圖8 組合路徑跟蹤三維結(jié)果

圖9 期望俯仰角、滾轉(zhuǎn)角、空速與實際對比

圖10 期望與實際俯仰角、滾轉(zhuǎn)角、空速誤差值

5 結(jié)束語

本文提出了一種基于矢量場法的三維航點制導(dǎo)算法，只需要任意給定合理的空間航點，算法將依據(jù)空間航點順序?qū)⒑近c連接成各航段，使用直線和螺旋線對航段進行近似擬合得到適合無人機飛行的路徑。根據(jù)路徑類型，通過矢量場法計算得到無人機運動的期望空速、期望航向角和期望航跡角，由期望的航向角和航跡角信息可以得到期望滾轉(zhuǎn)角和俯仰角信息，用于無人機的縱向和橫向通道的控制，將無人機引導(dǎo)到預(yù)定的軌跡上。此外，采用一種基于圓角幾何的切換策略，實現(xiàn)各航段的平滑過渡，使用螺旋線擬合相鄰航段，螺旋線僅在切點處與2條線段相交，該切點定義了切換半平面的位置，當(dāng)無人機通過了切換半平面，無人機會跟蹤下一航段，故此方法還可以減少路徑長度實現(xiàn)路徑優(yōu)化。采用本文提出的算法進行仿真實驗，仿真結(jié)果表明無人機可以很好地跟蹤直線、螺旋線及組合的復(fù)雜路徑，驗證了算法的有效性。