基于非線性系統(tǒng)正則標(biāo)準(zhǔn)形的三維路徑跟蹤問題

劉 偉

(江蘇科技大學(xué) 自動(dòng)化學(xué)院，鎮(zhèn)江212100)

機(jī)器人的運(yùn)動(dòng)穩(wěn)定控制中問題有兩大類：軌跡跟蹤和路徑跟蹤，兩者最大的區(qū)別在于軌跡跟蹤要求在某個(gè)時(shí)間到達(dá)軌跡上的某點(diǎn)，而路徑跟蹤不需要考慮時(shí)間上的限制[1].通常在實(shí)際情況中更多的使用路徑跟蹤算法.

對于路徑跟蹤問題，即是由適當(dāng)?shù)目刂坡蓪⑹芸貙ο笾敢侥繕?biāo)路徑曲線，并保持對象沿該曲線移動(dòng).而目標(biāo)路徑曲線通常是空間中的任意一條滿足一定光滑條件的曲線.針對這一問題研究出許多有效的算法來解決路徑跟隨問題[2-3]，主要包括3類：① 利用人工向量場構(gòu)造制導(dǎo)律，經(jīng)典的方法有向量場法[4]；② 引入路徑虛擬點(diǎn)，通過追蹤虛擬點(diǎn)實(shí)現(xiàn)對給定路徑的靠近和跟蹤，例如純追蹤[5]和非線性導(dǎo)航法[6]；③ 將路徑追蹤問題轉(zhuǎn)換為相對偏差系統(tǒng)的穩(wěn)定問題，進(jìn)而利用一些非線性理論中的常見方法實(shí)現(xiàn)穩(wěn)定控制，例如滑模控制[7]，反步控制[8]，反饋線性化[9]等，實(shí)現(xiàn)路徑的穩(wěn)定追蹤.受控對象的類型不同，例如輪式機(jī)器人[9]，水面艦艇[10]和水下機(jī)器人[11]，從不同的角度描述，數(shù)學(xué)模型也有不同，但控制策略和方法具有一定的通用性和借鑒性.

上述文獻(xiàn)基本上都是研究平面上的路徑跟蹤問題，即二維路徑跟蹤.但實(shí)際上對于飛行和水下機(jī)器人更需要關(guān)注三維路徑跟蹤問題.文獻(xiàn)[12]研究了恒定速度的無人機(jī)在三維空間中的路徑跟蹤，其設(shè)計(jì)的控制律是為了使對象的偏差和速度方向與指定曲線的偏差趨于零.因此，將路徑跟隨問題重新轉(zhuǎn)換表述為一部分狀態(tài)變量在零處的穩(wěn)定問題[13]，然后，根據(jù)仿射系統(tǒng)的正則標(biāo)準(zhǔn)形，設(shè)計(jì)出反饋控制律，成功解決了這種狀態(tài)穩(wěn)定問題.

基于文獻(xiàn)[12]中控制律的設(shè)計(jì)思想，文中研究無人機(jī)飛行變速時(shí)的三維路徑跟蹤問題.通過目標(biāo)路徑上的移動(dòng)伴隨坐標(biāo)系，將慣性坐標(biāo)系中的無人機(jī)運(yùn)動(dòng)系統(tǒng)轉(zhuǎn)換為基于路徑的偏差運(yùn)動(dòng)系統(tǒng).再使用非線性系統(tǒng)的正則標(biāo)準(zhǔn)形推導(dǎo)出跟蹤控制律.最后在仿真中驗(yàn)證了該運(yùn)動(dòng)控制律的有效性，無人機(jī)可以在變速度的情況下精確追蹤曲線路徑.

1 非線性系統(tǒng)的正則形式

為了得到合適的運(yùn)動(dòng)控制律，采用非線性系統(tǒng)f(x)=Ax+Bu的正則形式[14]：

(1)

式中：狀態(tài)x=(x1,…xn)T∈Rn；輸入u=(u1,…um)T∈Rm；A(x)=(a1(x),…,an(x))T；B(x)=(bij(x))n×m為n×m的矩陣，ai(x),bij(x)∈C∞(Ω)，i=1,2,…,n，j=1,2,…,m，Ω?Rn；系統(tǒng)輸出y=h(x)=(h1(x),…,hm(x))T.

通過設(shè)計(jì)輸入為：

將式(1)轉(zhuǎn)換成如下關(guān)于狀態(tài)變量Z的線性形式為：

使用線性系統(tǒng)理論中的標(biāo)準(zhǔn)方法就可得到新的控制輸入ν1,ν2,…,νm，在平衡零點(diǎn)處鎮(zhèn)定狀態(tài)變量z.

2 無人機(jī)運(yùn)動(dòng)模型

文中將飛行空間中的無人機(jī)視為一個(gè)可控實(shí)質(zhì)點(diǎn)，在慣性坐標(biāo)系中使用以下6個(gè)變量來描述其運(yùn)動(dòng)的六維數(shù)學(xué)模型[15]：L為縱向距離，H為高度，Z為計(jì)算在右邊的橫向偏差，V為無人機(jī)速度的絕對大小，?為航跡傾角，ψ為航跡偏向角.

(2)

式中：nx為縱向過載，沿著飛行速度方向；ny為橫向或法向過載，垂直與飛行速度矢量；γ為過載向量側(cè)傾角(滾轉(zhuǎn)角)；g為重力加速度.由于過載能夠表征可操縱力的大小和方向，而駕駛員一般就是通過改變該力的大小和方向來實(shí)現(xiàn)各種飛行器的機(jī)動(dòng)動(dòng)作，因此可以利用過載的概念來研究無人機(jī)的機(jī)動(dòng)性[16].文中將nx，ny，γ視為控制量.對飛行器性能的約束通常由飛行器的飛行包線和過載極曲線得到.不同種類和型號的飛行器飛行性能差別很大.文獻(xiàn)[16]中設(shè)定，-1≤nx≤1，-5≤ny≤5，-85°≤γ≤85°.

在式(2)中引入3個(gè)虛擬控制ν1=nx，ν2=nycosγ和ν3=nysinγ，則系統(tǒng)變?yōu)榭刂频姆律湎到y(tǒng)：

(3)

由系統(tǒng)(3)可知：ν1控制速度大小的變化，ν2和ν3控制速度矢量方向.

由于設(shè)計(jì)出的控制算法是要使得無人機(jī)能夠沿著目標(biāo)曲線移動(dòng)，換個(gè)角度來說，控制的目標(biāo)是要盡可能使得無人機(jī)與目標(biāo)曲線間的偏差為零，因此可以不在慣性坐標(biāo)系中，而是在基于路徑的移動(dòng)伴隨坐標(biāo)系中來描述無人機(jī)運(yùn)動(dòng)，即無人機(jī)相對目標(biāo)曲線的偏差變化，基于追蹤路徑，提出了三維路徑坐標(biāo).

3 變速度情況下的控制律設(shè)計(jì)

3.1 Bishop坐標(biāo)系

路徑伴隨坐標(biāo)系：設(shè)p(s)為給定路徑；s為自然參數(shù)，文中s選用曲線弧長；q為無人機(jī)的當(dāng)前位置.選擇在參考路徑上最接近q的點(diǎn)p0.并假定自然參數(shù)值s*對應(yīng)于該點(diǎn)，即p0=p(s*).通常，最近點(diǎn)p0為唯一點(diǎn)，可以通過數(shù)值方法解出.所以點(diǎn)q位于與向量τ=dp/ds正交的平面π中，向量τ是最近點(diǎn)p(s*)在路徑上的切向量.在π平面上選擇一個(gè)平面坐標(biāo)系n1和n2，則點(diǎn)q在n1和n2向量上坐標(biāo)分別為d1和d2.

位于點(diǎn)p0處的3個(gè)單位向量τ，n1和n2就組成R3空間的坐標(biāo)系，即為路徑伴隨坐標(biāo)系.相對于慣性坐標(biāo)系的轉(zhuǎn)換矩陣為U(s)=(τ，n1，n2)，它的確定則取決于參數(shù)s.在以前的路徑跟蹤文獻(xiàn)中，路徑伴隨坐標(biāo)系較多地使用了Frenet坐標(biāo)系，其公式為：

(4)

式中：τ,n和b分別是切線向量，主法向量和次法向量；k和κ分別曲線曲率和撓率.

但是Frenet坐標(biāo)系的使用有一些限制，例如，曲率必需存在，不能為零；對于曲線路徑，在彎曲部分主法向的方向通常會劇烈變化[17].考慮無人機(jī)飛行特性，文中采用Bishop frame坐標(biāo)系[18].

其運(yùn)動(dòng)特征為：

(5)

根據(jù)Frenet和Bishop坐標(biāo)系的定義，兩者之間的關(guān)系描述如下：

n=cosθFn1+sinθFn2

b=-sinθFn2+cosθFn2

式中：cosθF=k1/k，sinθF=k2/k.因此Bishop坐標(biāo)系可以由Frenet坐標(biāo)系通過繞向量τ旋轉(zhuǎn)角度θF得到.但是通過θF角旋轉(zhuǎn)，計(jì)算Bishop坐標(biāo)系時(shí)，還是需要先計(jì)算出Frenet坐標(biāo)系.這就仍然要面對Frenet坐標(biāo)系的缺點(diǎn).

Bishop坐標(biāo)系沿曲線的變化可以看作是一個(gè)固連在該坐標(biāo)系上的剛體的旋轉(zhuǎn)，于是向量n1的動(dòng)態(tài)變化[12]為：

(6)

使用數(shù)值積分方法，通過已知的向量n1的初值n1(s0)，就能計(jì)算得出任意n1(s).再由n2=τ×n1得到向量n2(s)，從而得到Bishop坐標(biāo)系{τ,n1,n2}.

3.2 基于Bishop坐標(biāo)系的路徑坐標(biāo)系統(tǒng)

基于慣性坐標(biāo)系和路徑伴隨坐標(biāo)系，無人機(jī)的位置q描述：

q=p+U(s)d

式中：d=(0,d1,d2)T表示無人機(jī)在Bishop坐標(biāo)系中的位置，即無人機(jī)相對給定路徑的偏差.

該方程對時(shí)間微分，得：

代入轉(zhuǎn)換矩陣U(s)，展開合并后得：

寫成矩陣相乘形式，可得：

通過標(biāo)量積，將上述方程拆分整理為：

在此基礎(chǔ)上加上速度V，狀態(tài)?和ψ的變化方程，由式(3)變換成描述無人機(jī)運(yùn)動(dòng)的新方程為：

(7)

該系統(tǒng)也是仿射的.

(8)

與此同時(shí)，已知q=(L,H,Z)，并且

通過變換自變量，將式(8)寫成另一種形式：

(9)

并且無人機(jī)位置向量經(jīng)過變換，可得：

因此，q′再次微分得到：

為了得到反饋控制律，對d′1和d′2再次微分，得：

(10)

式中：

式(10)中的子系統(tǒng)z″1=P+Qν可以使用非線性穩(wěn)定方法穩(wěn)定.選擇控制ν使得方程z″1=P+Qν轉(zhuǎn)換為方程z″1+K1z′1+K2z1=0.為了確保系統(tǒng)的漸近穩(wěn)定性，參數(shù)矩陣K1=λ1E和K2=λ2E，其中E為單位矩陣，參數(shù)λ1和λ2滿足條件：二次方程σ2+λ1σ+λ2=0具有實(shí)數(shù)正根.于是得到了虛擬控制ν來穩(wěn)定變量d1,d2,d′1,d′2:

ν=-Q-1(P+K1z′1+K2z1)

(11)

式(10)的另一個(gè)變量s.當(dāng)‖z(t)‖→0時(shí)，變量s的狀態(tài)由零動(dòng)態(tài)方程決定.而

綜上，在一定條件下可以實(shí)現(xiàn)無人機(jī)變速度情況下的路徑追蹤，即沿著目標(biāo)曲線以速度V(t)運(yùn)動(dòng).

4 仿真結(jié)果

基于文中控制律式(11)，檢驗(yàn)無人機(jī)的路徑跟蹤效果.Matlab仿真實(shí)驗(yàn)中給定路徑為直線：起點(diǎn)(0,0,0)，終點(diǎn)(100 m,100 m,100 m).無人機(jī)的初始狀態(tài)(L0，H0，Z0，?0，ψ0)=(10 m,20 m,10 m,0,0).速度值V(t)=15+3sin(t)(m/s).文獻(xiàn)[21]中將前視法推廣到了三維空間的路徑跟蹤，前視方法中有4個(gè)參數(shù)k?,kψ,δy,δz需要調(diào)節(jié).而文中只用到兩個(gè)參數(shù)λ1和λ2就可以調(diào)節(jié)跟蹤效果.本節(jié)中，選擇兩種方法各自跟蹤效果都較優(yōu)的參數(shù).前視方法參數(shù)[21]：k?=10，kψ=10，δy=10，δz=10；基于Bishop坐標(biāo)系的反饋控制參數(shù)λ1=0.75，λ2=0.135.兩種方法的路徑跟蹤如圖1，2.

圖1 基于前視法的無人機(jī)路徑跟蹤

從圖1和圖2中可以發(fā)現(xiàn)，基于前視法和基于Bishop坐標(biāo)系的控制方法，無人機(jī)都能較好跟蹤給定直線，無人機(jī)從初始點(diǎn)出發(fā)直接靠近并追蹤給定路徑，如圖2.

圖2 基于Bishop的無人機(jī)路徑跟蹤

圖3 基于前視法的無人機(jī)偏差距離d的變化

圖4 基于Bishop的無人機(jī)偏差距離d的變化

5 結(jié)論

(1) 基于移動(dòng)路徑坐標(biāo)系，通過路徑坐標(biāo)將三維路徑追蹤問題轉(zhuǎn)換為關(guān)于相對偏差動(dòng)力系統(tǒng)的穩(wěn)定問題.再將該系統(tǒng)化為非線性系統(tǒng)的正則標(biāo)準(zhǔn)形，從而較容易地得到相應(yīng)的系統(tǒng)穩(wěn)定控制律.

(2) 針對無人機(jī)飛行速度大小變化且沒有提前已知的情況，采用了自變量變換的方法，從而得到新的偏差系統(tǒng)方程.

(3) 經(jīng)過仿真實(shí)驗(yàn)，驗(yàn)證了該控制律的正確與良好性，能夠?qū)崿F(xiàn)在變速度的情況下控制無人機(jī)路徑追蹤.下一步就是將該方法在半實(shí)物仿真平臺中及實(shí)物平臺中實(shí)現(xiàn)，并同其他方法進(jìn)行比較.