劉 偉

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

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

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

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

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

1 非線性系統(tǒ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è)計輸入為：

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

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

2 無人機運動模型

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

(2)

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

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

(3)

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

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

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

3.1 Bishop坐標(biāo)系

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

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

(4)

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

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

其運動特征為：

(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)，計算Bishop坐標(biāo)系時，還是需要先計算出Frenet坐標(biāo)系.這就仍然要面對Frenet坐標(biāo)系的缺點.

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

(6)

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

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

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

q=p+U(s)d

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

該方程對時間微分，得：

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

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

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

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

(7)

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

(8)

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

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

(9)

并且無人機位置向量經(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ù)正根.于是得到了虛擬控制ν來穩(wěn)定變量d1,d2,d′1,d′2:

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

(11)

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

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

4 仿真結(jié)果

基于文中控制律式(11)，檢驗無人機的路徑跟蹤效果.Matlab仿真實驗中給定路徑為直線：起點(0,0,0)，終點(100 m,100 m,100 m).無人機的初始狀態(tài)(L0，H0，Z0，?0，ψ0)=(10 m,20 m,10 m,0,0).速度值V(t)=15+3sin(t)(m/s).文獻[21]中將前視法推廣到了三維空間的路徑跟蹤，前視方法中有4個參數(shù)k?,kψ,δy,δz需要調(diào)節(jié).而文中只用到兩個參數(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 基于前視法的無人機路徑跟蹤

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

圖2 基于Bishop的無人機路徑跟蹤

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

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

5 結(jié)論

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

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

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