動態(tài)環(huán)境下固定翼無人機的路徑跟蹤控制

動態(tài)環(huán)境下固定翼無人機的路徑跟蹤控制

1無人機模型預(yù)測控制方法設(shè)計無人機在軍事和民用領(lǐng)域都有廣泛的應(yīng)用前景。提高無人機的獨立性已成為研究的熱點。無人機的獨立性決定了飛機在動態(tài)環(huán)境中能夠?qū)崟r避免未知物體。問題的難點在于，動態(tài)環(huán)境中未知物體的不確定性以及在線避免障礙的現(xiàn)時性要求。目前已有很多學(xué)者通過在線路徑規(guī)劃算法解決這一問題.文采用A*算法,在3維空間內(nèi)在線生成無人機的飛行航路.由于生成的航路由一系列直線段構(gòu)成,因此要使航路滿足固定翼無人機的運動學(xué)約束還需進(jìn)行光順處理,但這需要耗費更長的運算時間.文分別采用改進(jìn)的Grossberg神經(jīng)網(wǎng)絡(luò)(GNN)法、遺傳算法和人工勢場法在線生成無人機的避障航路,但僅考慮了環(huán)境中的靜止未知障礙物而沒有考慮移動障礙物.其它的用于無人機路徑規(guī)劃的算法,如線性規(guī)劃法、非線性規(guī)劃法以及混合整數(shù)規(guī)劃法,能在給定約束下對目標(biāo)函數(shù)進(jìn)行優(yōu)化并產(chǎn)生滿足約束條件的可飛行路徑.但由于計算量過大,這些方法并不適用于動態(tài)環(huán)境的在線路徑規(guī)劃.另一方面,當(dāng)無人機遇到未知障礙物時,大多數(shù)在線路徑規(guī)劃算法首先是在線重新生成一條可飛行的避障路徑,然后再由無人機跟蹤該避障路徑從而避開環(huán)境中的未知障礙物.綜上,這些算法仍不能滿足實時性要求較高的情況(如存在移動障礙物).模型預(yù)測控制方法通過滾動優(yōu)化自定義的性能指標(biāo),能實時得到控制對象的控制輸入.文指出滾動優(yōu)化控制法(即模型預(yù)測控制)在解決不確定環(huán)境下無人機避障問題時具有很好的實時性.文給出了模型預(yù)測控制方法的詳細(xì)綜述.文在2維平面內(nèi)采用非線性模型預(yù)測控制方法使無人機在線跟蹤直線參考路徑的同時能實時避開環(huán)境中的未知障礙物,但該方法無法使無人機跟蹤任意的曲線路徑且僅能避開環(huán)境中的小型靜止障礙物.根據(jù)以上分析,本文研究了一種能使固定翼無人機及時避開環(huán)境中未知障礙物的路徑跟蹤控制算法.當(dāng)機載傳感器未探測到未知障礙物時,采用非線性模型預(yù)測控制器(NMPC)實時優(yōu)化得到跟蹤參考路徑的控制輸入,當(dāng)探測到未知障礙物時,通過NMPC控制器實時優(yōu)化得到避障控制輸入,使無人機產(chǎn)生避障動作及時避開環(huán)境中的未知障礙物.為了使參考路徑上的每一點都能滿足固定翼無人機的運動學(xué)約束(如曲率連續(xù)、最大曲率約束和最大撓率約束等),直接將曲線用于路徑規(guī)劃得到適于無人機飛行的參考路徑顯得更加合理.因此,這里利用畢達(dá)哥拉斯速端曲線的有理性、曲率連續(xù)以及曲線平滑等特性,首先通過離線路徑規(guī)劃器生成基于PH曲線的參考路徑,使參考路徑上的每一點都是無人機可飛行的.然后采用非線性模型預(yù)測控制器跟蹤PH參考路徑,同時避開動態(tài)環(huán)境中的未知障礙物,并確保無人機在避障后繼續(xù)沿參考路徑朝目標(biāo)點飛行.這里,未知障礙物包括了靜止障礙物和移動障礙物.算法借鑒了文中的目標(biāo)函數(shù)形式,但相比文做出了如下改進(jìn):(1)算法能穩(wěn)定跟蹤任意形式的參數(shù)化曲線參考路徑;(2)能使無人機避開環(huán)境中的大型靜止障礙物和移動障礙物;(3)在3維空間內(nèi)驗證了算法的有效性.2問題描述procedural2.1跟蹤參考ph路徑無人機的飛行任務(wù)是以勻速V從指定起點沿參考軌跡飛往目標(biāo)點.其中,參考軌跡是由離線路徑規(guī)劃器根據(jù)已知的作戰(zhàn)環(huán)境(如已知的靜止障礙物/威脅)和飛行任務(wù)規(guī)劃出的一條可飛行的PH曲線路徑.為了能完成既定的飛行任務(wù),要求無人機能盡量沿離線規(guī)劃的PH參考路徑飛行,但當(dāng)遭遇未知障礙物/威脅時,則要求無人機能及時避開障礙物,并在避障后繼續(xù)沿參考路徑飛行.這里假定無人機上的機載傳感器能及時探測到未知障礙物的位置和大小,并用球體近似表示障礙物.圖1給出了無人機跟蹤參考PH路徑的俯視示意圖.圖中實線表示PH參考路徑,虛線表示無人機跟蹤參考路徑時的實際飛行軌跡.2.2無人機飛行過程中ph曲線路徑的選擇文中選用PH曲線作為無人機的可飛行路徑.PH曲線由Farouki首次提出,是一種具有有理特性的參數(shù)化曲線.有關(guān)PH曲線的詳細(xì)介紹可參見文[10-12],這里僅簡要介紹PH曲線的定義和它的數(shù)學(xué)表達(dá)式.設(shè)r(q)=[x(q),y(q),z(q)]為以q為參數(shù)的任意曲線,若r(q)各分量的導(dǎo)數(shù)能滿足以下條件:其中σ(q)為多項式,則稱r(q)為PH曲線.PH曲線的貝塞爾形式及其1階導(dǎo)數(shù)如下所示:其中pk為曲線的控制頂點,△pk=pk+1-pk,n為曲線多項式中最高次項的次數(shù).文中采用五次PH曲線作為參考曲線路徑(即n=5),因為它是具有足夠靈活性且次數(shù)最低的PH曲線.根據(jù)無人機在起點和目標(biāo)點位置和飛行姿態(tài)的約束、環(huán)境中已知的障礙物、無人機的最小轉(zhuǎn)彎半徑以及油耗等約束可以通過離線優(yōu)化算法(如遺傳算法)得到滿足約束條件的五次PH曲線路徑.該PH曲線路徑可作為無人機實際飛行的參考路徑.由于本文篇幅有限,不介紹離線獲取PH曲線路徑的過程,而認(rèn)為PH參考路徑為已知,也即控制點pk(k=0,···,5)為已知.2.3無人機運動學(xué)模型的建立這里將無人機視為一個質(zhì)點以勻速V飛行,并假設(shè)內(nèi)環(huán)控制器能很好地保持無人機的飛行姿態(tài)(即偏航角、俯仰角和滾轉(zhuǎn)角)且通過控制油門保持無人機勻速飛行.因此,這里僅考慮無人機的外環(huán)軌跡跟蹤控制.那么,3維空間內(nèi)無人機的運動學(xué)模型如下:式中狀態(tài)向量X=(χ,γ,x,y,z)T中各分量分別表示航跡坐標(biāo)系下的航向角、爬升角以及無人機在地球坐標(biāo)系下的位置坐標(biāo),控制輸入U=(ny,nz)T的各分量分別為過載在航跡坐標(biāo)系的y軸和z軸的投影,并限定了控制輸入的取值范圍.過載控制輸入ny和nz取值的限定相當(dāng)于給出了無人機最小轉(zhuǎn)彎半徑的約束.在實際應(yīng)用中采用四級四階龍格–庫塔法近似得到式(4)的離散化模型:式中△T為采樣時間.通過非線性模型預(yù)測控制器(NMPC)在線實時優(yōu)化目標(biāo)函數(shù)可以得到式(4)中的控制輸入U=(ny,nz)T.3非線性模型預(yù)測控制器非線性模型預(yù)測控制(NMPC)又稱為滾動時域控制,是一種基于模型的有限時域開環(huán)最優(yōu)控制算法,其本質(zhì)是在每個采樣時刻以當(dāng)前時刻的系統(tǒng)狀態(tài)(即無人機在當(dāng)前時刻的航向角、爬升角和在地球坐標(biāo)系下的位置坐標(biāo))為初始狀態(tài)使用系統(tǒng)模型(即式(4)給出的無人機運動學(xué)模型)對未來N步內(nèi)的系統(tǒng)狀態(tài)或輸出進(jìn)行預(yù)測,通過優(yōu)化算法生成使目標(biāo)函數(shù)(例如未來N步內(nèi)的輸出與參考軌跡間的累積誤差)達(dá)到最小的最優(yōu)控制序列{U0,U1,···,UN-1},其中將第一個分量U0作為無人機運動學(xué)方程(如式(4)所示)的控制輸入,而其它分量被存儲,即以{U1,···,UN-1,UN-1}作為下一步優(yōu)化中的控制輸入的猜測值.若設(shè)非線性模型預(yù)測控制的有限時間域為t,其離散時間步長就為N:文中非線性模型預(yù)測控制的目標(biāo)函數(shù)形式如下:Jnav為未來N步內(nèi)無人機的預(yù)測狀態(tài)量與參考軌跡的累積誤差,Jobs是當(dāng)無人機接近障礙物時產(chǎn)生的懲罰項,Jadd為將無人機引向目標(biāo)點的函數(shù),該函數(shù)的作用是防止無人機在避開障礙物后背離目標(biāo)點飛行,R為正定對角矩陣.在每一個采樣時間內(nèi),采用非線性優(yōu)化算法(如牛頓法)最小化目標(biāo)函數(shù)J,可得到最優(yōu)控制輸入序列{U0,U1,···,UN-1},其中U0則作為當(dāng)前時刻下無人機的實際控制輸入,其值也被限定在給定范圍內(nèi)(如式(4)所示).而其余的控制輸入{U1,···,UN-1,UN-1}則作為下一個采樣時刻中式(5)的控制輸入預(yù)測未來N步內(nèi)無人機的狀態(tài)向量{X(1),···,X(N)},本文中均假定當(dāng)前時刻為0時刻.非線性模型預(yù)測控制器的計算流程如圖2所示.式(7)中Jnav用于跟蹤PH參考路徑,Jobs用于避開障礙物,Jadd則用于引導(dǎo)無人機避障后繼續(xù)沿參考路徑朝目標(biāo)點飛行.其中,Jobs和Jadd僅在無人機遭遇未知障礙物時起作用.下面重點介紹目標(biāo)函數(shù)J中Jnav、Jobs以及Jadd的計算方法.3.1穩(wěn)定跟蹤參考路徑這里介紹穩(wěn)定跟蹤PH參考路徑的方法,也即式(7)中Jnav的計算方法.首先可以將上一采樣時刻優(yōu)化得到的控制序列{U1,···,UN-1,UN-1}作為當(dāng)前時刻下未來N步內(nèi)控制輸入的初始猜測值,通過式(5)估計無人機在未來N步內(nèi)的狀態(tài)向量,即S(k)=(χ(k),γ(k),x(k),y(k),z(k))T,k=1,2,···,N.若設(shè)Sr(k)=(χr(k),γr(k),xr(k),yr(k),zr(k))T為未來第k時刻的參考狀態(tài)向量,那么Jnav的表達(dá)式為其中N為預(yù)測時間長度,Q為五階正定對角矩陣.正確選取未來N步內(nèi)的參考狀態(tài)向量是穩(wěn)定跟蹤PH參考路徑的關(guān)鍵.在介紹參考狀態(tài)向量Sr(k)的選取方法前,先討論連續(xù)時間內(nèi)穩(wěn)定跟蹤參考路徑的條件.這里設(shè)無人機的飛行速度為V,P=(x,y,z)T為無人機的位置向量,γ、χ為無人機的當(dāng)前爬升角和航向角.Pd=(xd,yd,zd)T為參考曲線路徑r(q)上的目標(biāo)點位置向量,γd、χd為該點切向量的爬升角和航向角.位置向量P和Pd均隨時間t連續(xù)變化.若設(shè)Oxgygzg為地球坐標(biāo)系,Oxkykzk為由目標(biāo)點Pd上的切向量、法向量以及副法向量構(gòu)成的動坐標(biāo)系(如從圖3可以看出,若要使無人機穩(wěn)定跟蹤點Pd,應(yīng)保證當(dāng)時間t→∞時,∥ε∥→0.這里通過李亞普諾夫穩(wěn)定判據(jù)求解穩(wěn)定跟蹤參考路徑r(q)的條件.首先定義李亞普諾夫函數(shù):圖3所示),那么由地球坐標(biāo)系到動坐標(biāo)系的轉(zhuǎn)換矩陣Rkg為無人機與目標(biāo)點間的位置誤差在動坐標(biāo)系Oxkykzk上的投影為其導(dǎo)數(shù)為無人機位置向量P和目標(biāo)位置向量Pd的導(dǎo)數(shù)分別為其中Vd為目標(biāo)點Pd移動的速度.將式(12)、(13)代入式(11)可以得到:對式(15)求導(dǎo)并代入式(14)可以得到:將式(9)代入式(16)可以得到使˙V<0,也即穩(wěn)定跟蹤參考曲線路徑r(q)的充分條件:又因˙,所以可以得到參考路徑r(q)的參數(shù)q的變化率:式中η1和η2為自定義的正常數(shù),(x′d,y′d,z′d)為目標(biāo)點上的切向量.式(20)決定了穩(wěn)定跟蹤參考路徑時目標(biāo)點Pd的位置隨時間的變化,式(18)和式(19)確定了穩(wěn)定跟蹤參考路徑時無人機的飛行方向.將式(18)~(20)離散化,可以確定參考狀態(tài)向量Sr(k)中的各分量:由式(21)可以看出Sr(k)中的(xr(k),yr(k),zr(k))實際就是參考路徑上目標(biāo)點Pd的位置坐標(biāo),χr(k)和γr(k)通過Pd上切向量的航向角χd(k)和爬升角γd(k)計算得到.第k時刻的參考狀態(tài)向量Sr(k)由前一時刻無人機的狀態(tài)向量S(k-1)和參考狀態(tài)向量Sr(k-1)遞推得到.另外,值得說明的是,以上給出的穩(wěn)定跟蹤參考路徑的方法適用于任意參數(shù)化曲線路徑.3.2懲罰函數(shù)的改進(jìn)這里未知障礙物包括了靜止障礙物和移動障礙物,并假設(shè)障礙物的位置、大小信息可以由機載傳感器(如視頻、光電或紅外等設(shè)備)探測得到.同時,用半徑為robs、完全包含了障礙物的球體近似表示環(huán)境中的障礙物.假定移動障礙物的運動學(xué)模型如下:其中Vobs為障礙物的速度,這里認(rèn)為是常數(shù).γobs、χobs分別為障礙物的爬升角和航向角,γobs0為爬升角初值,是常值,u1為航向角的控制輸入.(xobs,yobs,zobs)T為障礙物中心在地球坐標(biāo)系下的位置坐標(biāo).在每一個采樣時刻移動障礙物都被視為一個靜止障礙物.當(dāng)未知障礙物(包括靜止障礙物和移動障礙物)與無人機間的最小距離小于給定閾值ξd時可在目標(biāo)函數(shù)中加入懲罰項Jobs以避開障礙物.文提出了一種避障懲罰函數(shù)形式:式中N為預(yù)測時間長度,Nobs為當(dāng)前時刻下機載傳感器探測到的障礙物的個數(shù),(x(i),y(i),z(i))表示未來第i時刻無人機的位置坐標(biāo),其值由式(5)預(yù)測得到,(xobs(j),yobs(j),zobs(j))為當(dāng)前時刻下第j個障礙物圓心的位置坐標(biāo),kj、σj均為大于0的常數(shù).從式(23)可以看出,無人機離障礙物圓心越近其懲罰值越大,但懲罰值的大小僅與無人機到障礙物圓心的距離相關(guān),與障礙物的大小無關(guān).因此當(dāng)障礙物較大時,式(23)不能產(chǎn)生足夠大的懲罰值使無人機及時避開障礙物.其中,robs(j)表示探測到的第j個障礙物半徑,(x(i),y(i),z(i))表示未來第i時刻下無人機的位置坐標(biāo),(xobs(j),yobs(j),zobs(j))表示當(dāng)前采樣時刻下傳感器探測到的第j個障礙物(包括靜止障礙物和移動障礙物)的位置坐標(biāo).從式(24)可以看出,改進(jìn)后的懲罰函數(shù)考慮了障礙物的大小,當(dāng)無人機與障礙物圓心距離D大于障礙物半徑robs時,懲罰值隨無人機離障礙物邊緣距離的減小而增大;當(dāng)D小于障礙物半徑robs時其懲罰值繼續(xù)隨無人機與障礙物圓心距離的減小而增大.因此,改進(jìn)后的避障懲罰函數(shù)能使無人機及時避開環(huán)境中的大型障礙物.此外,值得注意的是式(8),即目標(biāo)函數(shù)中的Jnav可以使無人機穩(wěn)定跟蹤PH參考路徑,但并不能保證無人機是朝著終點(即圖1中“?”所標(biāo)記的終點位置)的方向沿PH參考路徑飛行的.換句話說,當(dāng)無人機遭遇大型障礙物時,目標(biāo)函數(shù)中避障懲罰函數(shù)Jobs的作用使無人機能及時避開大型障礙物,但是為了能避開障礙物,無人機可能朝著背離終點的方向飛行.因此,為了使無人機在避開障礙物后仍能朝著終點方向飛行,當(dāng)無人機遭遇大型障礙物時還需在目標(biāo)函數(shù)中添加一個額外懲罰值:N為預(yù)測時間,λ為正常數(shù),(x(i),y(i),z(i))為通過式(5)預(yù)測得到的未來第i時刻下無人機的位置坐標(biāo),(xgoal,ygoal,zgoal)為終點位置.額外懲罰值Jadd相當(dāng)于在目標(biāo)函數(shù)中加入了終點對無人機的吸引作用.從式(25)中可以看出,無人機距離終點位置越遠(yuǎn)這種吸引作用越大,越接近終點則吸引作用越小.3.3無人機在n步內(nèi)的狀態(tài)向量及狀態(tài)向量的預(yù)測綜上所述,為了確保無人機能穩(wěn)定跟蹤PH參考路徑,并在遭遇障礙物時能及時避開,非線性模型預(yù)測控制器在每一個采樣時刻按以下步驟得到當(dāng)前時刻下無人機的控制輸入:步驟1:以上一時刻存儲下來的控制輸入序列{U1,···,UN-1,UN-1}作為初始猜測值,根據(jù)式(5)預(yù)測N步內(nèi)無人機的狀態(tài)向量S(k)(k=1,···,N).步驟2:按照式(21)計算無人機在N步內(nèi)的參考狀態(tài)向量Sr(k)(k=1,···,N),并按照式(8)計算Jnav.步驟3:計算無人機在當(dāng)前時刻下與環(huán)境中障礙物間的最短距離,若均大于給定閾值ξd,則令式(7)中的Jobs和Jadd為0;若有障礙物與無人機間的最短距離小于ξd,則按照式(24)和式(25)分別計算Jobs和Jadd.步驟4:最小化式(7)所示的目標(biāo)函數(shù),得到最優(yōu)控制序列.步驟5:輸出最優(yōu)控制序列{U0,U1,···,UN-1}中的第1個分量U0作為無人機運動學(xué)方程(如式(4)所示)的控制輸入,而其它分量被存儲作為下一個采樣時刻控制輸入的初始猜測值.4初始控制設(shè)置仿真實驗在IntelCoreTM2、3GHz、2G內(nèi)存的計算機上進(jìn)行,編程環(huán)境為Matlab7.0.4.非線性模型預(yù)測控制器在每一采樣時刻按照3.2節(jié)所示步驟計算當(dāng)前時刻下無人機的控制輸入,其中在步驟4中采用Matlab自帶的函數(shù)fmincon最小化式(7)所示的目標(biāo)函數(shù)得到最優(yōu)控制輸入序列.仿真中一些初始條件的設(shè)置如下:無人機的飛行速度V=50m/s;采樣時間△T和有限時間域長度N分別為0.1s和10;式(22)中移動障礙物的控制輸入u1為常值0.02,飛行速度Vobs=70m/s,它的初始爬升角γobs0為0,初始航向角χobs0為-90°;靜止障礙物和移動障礙物的半徑均為500m;PH參考路徑的6個控制點分別為其中p0是參考路徑的起點,p5是參考路徑的終點;無人機的控制輸入ny和nz分別限定在[-0.5,0.5]和[-1,3]范圍內(nèi);由于預(yù)測時間長度N=10,在t=0時刻控制輸入ny和nz的初始猜測值分別為[0,0,0,0,0,0,0,0,0,0],[1,1,1,1,1,1,1,1,1,1].基于上述仿真環(huán)境和初始條件的設(shè)置,開展相關(guān)的算法仿真實驗驗證,主要內(nèi)容包括:跟蹤PH參考路徑的穩(wěn)定性驗證改進(jìn)避障懲罰函數(shù)Jobs的有效性驗證額外懲罰值Jadd的有效性驗證4.1無人機初始位置與參考路徑的關(guān)系首先在環(huán)境中沒有障礙物的情況下給出了無人機跟蹤PH參考路徑的仿真結(jié)果(如圖4、5所示).此時目標(biāo)函數(shù)中的Jobs和Jadd始終為0,僅Jnav起作用.無人機的初始狀態(tài)向量為(χ0,γ0,x0,y0,z0)T=(0,0,-4050,100,2050)T.由于PH參考路徑的起點位置為(-4000,0,2000)T,因此無人機的初始位置與參考路徑的起點位置有偏差.圖4給出了無人機跟蹤PH參考路徑的軌跡,其中實線表示PH參考路徑,小三角形表示無人機的實際飛行軌跡,“”表示參考路徑的起點,“?”表示參考路徑的終點,“?”表示無人機的初始位置.從圖中可以看出雖然在初始狀態(tài)下無人機的位置與參考路徑的起點位置有偏差,但無人機很快能回到參考路徑上來并繼續(xù)沿參考路徑飛行.圖5給出了由式(10)定義的無人機與目標(biāo)點Pd間的位置誤差在動坐標(biāo)系Oxkykzk上的投影εx、εy和εz隨時間的變化,它們分別由實線、短虛線和長虛線表示.從圖中可以看出,無人機與目標(biāo)點Pd間的位置誤差最終都收斂到0附近.雖然控制輸入ny和nz分別限定在[-0.5,0.5]和[-1,3]范圍內(nèi),但無人機仍能緊密跟蹤PH參考路徑,這說明PH參考路徑能滿足無人機的運動學(xué)約束,適于無人機飛行.4.2飛行軌跡仿真為了驗證改進(jìn)懲罰函數(shù)Jobs的有效性,分別采用了文提出的避障懲罰函數(shù)形式(如式(23))以及改進(jìn)后的避障懲罰函數(shù)形式(如式(24))進(jìn)行仿真,且式(23)和式(24)中kj和σj的設(shè)置相同分別為kj=2800,σj=40.當(dāng)無人機與障礙物間的最短距離小于1000m時,目標(biāo)函數(shù)中的避障懲罰值和額外懲罰值開始起作用.此時,無人機的初始狀態(tài)向量為(χ0,γ0,x0,y0,z0)T=(0,0,-4000,-1000,2000)T,與參考路徑的起點位置重合.仿真結(jié)果分別如圖6和圖7所示.圖中實心球表示靜止障礙物,空心球表示移動障礙物并用點劃線表示移動障礙物的運動軌跡.從圖6可以看出文提出的Jobs并不能使無人機很好地避開較大的障礙物,因此無人機的飛行軌跡穿過了障礙物,這說明在實際飛行中此時已發(fā)生碰撞.而當(dāng)采用了本文提出的改進(jìn)的Jobs后無人機的飛行軌跡能成功地繞開環(huán)境中的障礙物,并在避障后繼續(xù)沿參考路徑飛行(見圖7).此時的控制輸入ny和nz以及無人機的路徑跟蹤航跡角和位置隨時間的變化如圖8和圖9所示.從圖中可看出過載控制輸入ny和nz均被限定在指定范圍以內(nèi)(見圖8).圖9中的目標(biāo)值指的是無人機在當(dāng)前時刻的目標(biāo)點,即PH參考軌跡上目標(biāo)點Pd的位置坐標(biāo)以及該點切向量的航向角和爬升角.當(dāng)遭遇障礙物時,為了避開大型障礙物,無人機的航向角和爬升角較大,偏離目標(biāo)值較多,且由于nz限定的范圍大于ny,使得無人機的爬升角變化更快.但在避開障礙物后無人機能及時回