姚其昌，盧國軒，藍(lán) 偉

(中國北方車輛研究所，北京 100072)

路徑跟蹤控制是指移動機(jī)器人沿著給定的與時間無關(guān)的幾何路徑進(jìn)行移動控制的過程.路徑跟蹤控制的基本思想是先求出軌跡目標(biāo)值和車體位姿的偏差，隨后根據(jù)獲得的偏差量，產(chǎn)生一個相應(yīng)的控制命令，并發(fā)送給移動機(jī)器人的控制系統(tǒng)，使其偏差逐漸減小.路徑跟蹤問題有不同的數(shù)學(xué)描述方法，比較普遍并得到廣泛認(rèn)同的提法為:對于任意給定的幾何路徑 f(x，y)=0，尋找反饋控制律u=u(x，y，φ)，使受控移動機(jī)器人最終能沿著期望的幾何路徑運動，即對于任意給定的ε＞0，有f(x，y，φ)＜ ε[1].

早期移動機(jī)器人路徑跟蹤控制是使動力學(xué)模型在目標(biāo)路徑附近線性化，這種方法雖然計算簡單，但由于局部線性化不能保證系統(tǒng)的全局穩(wěn)定，而采用反饋線性化方法可完全補(bǔ)償機(jī)器人的非線性.Kanayam YJ在文獻(xiàn)[2]和[3]中將利用移動機(jī)器人的路徑曲率的微分表示曲率、位置誤差、姿態(tài)誤差的線性組合形式，稱為“操縱函數(shù) (Steering Function)”，并取位置誤差、位置誤差的一次微分、位置誤差的二次微分作為變量，使控制系統(tǒng)線性化[2-3];Zhong-Ping Jiang等利用鏈?zhǔn)较到y(tǒng)采用反推技術(shù)實現(xiàn)跟蹤[4];變結(jié)構(gòu)控制不需要知道系統(tǒng)精確的數(shù)學(xué)模型，當(dāng)系統(tǒng)進(jìn)入滑動模態(tài)后，對系統(tǒng)干擾和參數(shù)變化具有完全的自適應(yīng)性;文獻(xiàn)[5]采用滑?？刂?，使路徑跟蹤控制具有很好魯棒性的控制律[5].

本文研究的移動機(jī)器人為武裝機(jī)器人車輛(Armed Robot Vehicle即ARV)，其路徑跟蹤控制屬于非完整控制系統(tǒng)的范疇.為了達(dá)到良好的路徑跟蹤控制效果，首先利用ARV參考路徑的曲率、車體相對于參考路徑的偏航角以及位置偏移量等變量參數(shù)建立運動學(xué)模型，然后利用鏈?zhǔn)较到y(tǒng)把非線性運動學(xué)模型線性化，從而實現(xiàn)與速度無關(guān)的路徑跟蹤控制.

1 ARV運動學(xué)模型

1.1 模型假設(shè)

為了推導(dǎo)可用于控制器設(shè)計的ARV運動學(xué)模型，需要做出如下的平衡:

1)模型必須能夠很好地反映ARV的行為，但是其復(fù)雜程度要受到控制器設(shè)計要求的限制;

2)從控制器設(shè)計的角度建立的簡單運動學(xué)模型能夠較好地反映ARV的行為.

因此，模型在建立的過程中遵循以下假設(shè):

ARV及其車載部件為單一的剛體，因此把其描述為雙輪車模型 (即Ackermann’s模型):前中軸都用虛擬的單輪所代替，后軸作為車體剛體的一部分，其轉(zhuǎn)向輪轉(zhuǎn)角根據(jù)前軸轉(zhuǎn)向輪的轉(zhuǎn)角由幾何關(guān)系換算獲得，如圖1所示，虛擬輪位于實際輪子的中間位置.在不考慮滑移的情況下，這個假設(shè)在移動機(jī)器人領(lǐng)域中普遍應(yīng)用[6].

圖1 車體簡化模型

本研究的控制目標(biāo)為路徑跟蹤，因此結(jié)合相對參考路徑C而不是絕對參考路徑描述車體構(gòu)型.如圖1所示.各參數(shù)的意義是:

C為所跟隨的路徑，定義在絕對坐標(biāo)系XAAYA下;

O為車體虛擬中輪的中心 (即車體中軸的中心);

M為C上距離O點最近的點，假設(shè)點M是唯一的.實際上，一方面由于ARV總是接近C，另一方面路徑C的曲率很小，因此這個假設(shè)是合理的;

s為點M沿路徑C曲線的橫坐標(biāo)，c(s)為路徑C在該點的曲率，θc(s)為C上該點的切線在坐標(biāo)系XAAYA下的方位角;

θt為ARV的中心線在坐標(biāo)系XAAYA下的方位角.因此θ=θt-θc(s)為ARV相對于路徑C的偏轉(zhuǎn)角;

y為ARV相對于路徑C的橫向偏移距離;

υ為ARV在點O的線性速度;

δ為前輪相對于ARV中心線的方位角;

l為中前輪輪距.

由于把ARV及其車載部件作為一個剛體，根據(jù)車體中心線的方位角以及車體上任意一點的位置(比如點O)便可以得出車體的總體構(gòu)型.而這2個參數(shù)信息可以由(s，y)這一對參數(shù)和變量θ來代替，因此車輛的狀態(tài)變量可以寫成:

作為路徑跟蹤控制的運動學(xué)模型，ARV車輛控制狀態(tài)變量為:

由式(1)、式(2)便得出了考慮兩輪與地面持續(xù)相接觸的車輛狀態(tài)空間模型.

1.2 車輛運動學(xué)模型

假設(shè)ARV在運動過程中車輪只發(fā)生純滾動而沒有滑轉(zhuǎn)，這說明車輪中心點線速度矢量在輪面上.把此假設(shè)應(yīng)用于圖1所示的雙輪車模型，便可以得到雙輪車模型中前兩輪的速度以及前輪轉(zhuǎn)向角度.

考慮到把整個車體作為一個剛體，在任意時刻，車體的運動分為兩種:純粹的平移運動和繞某一瞬時轉(zhuǎn)向中心點IRC的轉(zhuǎn)動.實際上，這兩種情況可以歸為一種情況，平移運動其實就是瞬時轉(zhuǎn)向中心點在無窮遠(yuǎn)處的特殊轉(zhuǎn)動.當(dāng)車體做純滾動時，IRC可以由車體任意兩點線速度矢量垂線的交點確定;當(dāng)車體做平移運動時，任意兩點線速度矢量相互平行，IRC在無窮遠(yuǎn)處.如圖2所示.

圖2 車體角速度的示意圖

圖2中，IRC由點O和前輪中心點速度矢量垂線的交點確定.此處定義d為IRC和點O的距離.車體的絕對角速度˙θt便可以得出，即

d的值可以通過公式 (4)得出.

因此由式(3)和式(4)可以得出:

圖3 車體偏移角速度的示意圖

其中，y在圖3中為負(fù)值，因此在式(6)中為減號.從式(6)中可以得出:

從圖3中還可以得到:

注意到圖3中θ為負(fù)值，式(9)中的符號與此相一致.當(dāng)輪地接觸符合純滾動無滑轉(zhuǎn)的假設(shè)時，式(5)、式(7)～式(9)為我們提供了最終的車輛運動學(xué)模型，即

2 ARV控制算法

控制算法開發(fā)的目的是保證ARV能夠較好地跟隨路徑C，即要求狀態(tài)變量y=0，θ=0.y=0意味著ARV車輛在規(guī)劃路徑C上，θ=0意味著車輛方位角變化與路徑C切線方向的變化相一致.在不考慮滑轉(zhuǎn)的情況下，車輛運動模型可以由式(10)來描述，這個模型很明顯為非線性模型.運用現(xiàn)代控制理論已經(jīng)可以把移動機(jī)器人模型轉(zhuǎn)換為線性模型，即鏈?zhǔn)侥Ｐ蚚7]，具體細(xì)節(jié)見文獻(xiàn)[7].這種方法可以把實際非線性車輛模型使用線性系統(tǒng)理論方法求解.

轉(zhuǎn)換為兩輸入鏈?zhǔn)椒匠痰姆蔷€性模型一般寫成如下的形式[7]:

設(shè) A=(a1，a2，…an)T和 M=(m1，m2)T分別為狀態(tài)向量和控制向量.

為了保證鏈?zhǔn)椒匠袒旧蠟榫€性系統(tǒng)，用各狀態(tài)變量對a1的導(dǎo)數(shù)代替其對時間的導(dǎo)數(shù)，即:

則鏈?zhǔn)椒匠淌?11)可以寫成:

式(13)中前n-1個等式很明顯為線性系統(tǒng).

把ARV車輛模型式(10)轉(zhuǎn)換為鏈?zhǔn)椒匠?當(dāng)維數(shù)為3時，通用的鏈?zhǔn)较到y(tǒng)式(11)和式(13)可以寫成如下形式:

對時間的導(dǎo)數(shù):

對a1的導(dǎo)數(shù):

為了使所設(shè)計的控制器性能不受車速的影響，狀態(tài)變量a1可以設(shè)為:

式(16)作為起始設(shè)定值，把其代入式(14)或式(15)即可把非線性車輛模型轉(zhuǎn)換為鏈?zhǔn)椒匠?

為了符合式(14)的形式，新的控制量m1可以定義為:

為了簡化模型，可以嘗試a2=y，同時設(shè)定˙a2=vsin θ?a3m1.

因而最后一個狀態(tài)變量a3只能為

最終，可以得出最后一個控制變量m2為:

通過上面的變換，非線性ARV運動學(xué)模型式(10)轉(zhuǎn)換為鏈?zhǔn)?14)或式(15)的形式，其狀態(tài)變換為:

式中:Ψ(X)=(s，y，(1-yc(s))tan θ)T.

控制變量轉(zhuǎn)換為:

3 ARV路徑跟蹤控制的實現(xiàn)

有了上面的鏈?zhǔn)侥Ｐ?，控制器便可以很容易地設(shè)計出來.由于鏈?zhǔn)较到y(tǒng)式(15)為線性系統(tǒng)，虛擬控制律可以很自然地描述為:

實際上，把式(21)代入式(15)可以得到:

上式說明a2能夠收斂于零，根據(jù)式(19)，y收斂于0就保證路徑跟蹤的目標(biāo)得以實現(xiàn).同時，a2收斂說明a3也收斂(可參見鏈?zhǔn)椒匠淌?15)).狀態(tài)變換式(19)也保證了角度偏差θ也收斂于0.

由于誤差動態(tài)方程式(22)基于a1=s(Kd，Kp)為距離的增益量而不是時間的增益量，因而，對于給定的初始誤差，不管車體的速度是多少或者車體速度是否隨時間發(fā)生變化，車體的軌跡都是一樣的.即從控制器設(shè)計的角度來看，路徑跟蹤的效果與速度無關(guān)，控制律增益與車速無關(guān).實際上，理論結(jié)果或許有稍微的變化，這是由于θt(因而影響到θ)的測量值很明顯受到車速的影響 (參見式(5));另一方面，車載傳感器并不是完全線性的.但是，考慮到車體的速度范圍為0～15 km/h，并限制控制增益(Kd，Kp)的值不至于高到使執(zhí)行器飽和的程度.

通過變換得到的式(20)就是實際的控制律 (把式(12)、式(17)和式(18)代入式(21)):

可以看到，在控制器的設(shè)計過程中沒有引入近似值，這就保證了非線性控制律式(23)能夠獲得精確的曲線路徑跟隨效果:非線性模型式(10)和參考路徑曲率c(s)都在控制律中明確地表達(dá)出來.此外，非線性控制律式(23)的性能調(diào)節(jié)也非常容易，因為它僅僅依賴于線性誤差方程式(22)，通過調(diào)節(jié)增益量(Kd，Kp)可以很直觀地獲得所期望的距離值.

在實際應(yīng)用中，參考路徑C通常為直線，即c(s)=0，因此控制律式(23)就變得非常簡單.

最后，討論一下執(zhí)行器飽和的問題.在式(23)或者式(24)中，反正切函數(shù)的參數(shù)是無界的，而轉(zhuǎn)向輪轉(zhuǎn)向角度被限制在±45°的范圍，因此可能發(fā)生執(zhí)行器飽和的問題.處理這個問題的基本方法是調(diào)節(jié)控制器的性能 (如調(diào)節(jié)增益量Kd，Kp)，從而使預(yù)定的控制操作不會遇到飽和.需要指出的是，執(zhí)行器飽和不會阻止車體收斂于參考路徑C，即使從理論的角度上也可以看出:由于鏈?zhǔn)侥Ｐ褪?15)包含一個二重積分器，即使式(21)中的m3為任意數(shù)，都可以保證其漸進(jìn)穩(wěn)定性，參見文獻(xiàn)[8].控制性能的唯一缺點就是當(dāng)執(zhí)行器飽和的時候，控制律就不再與速度無關(guān).

實際上，轉(zhuǎn)向執(zhí)行器的最主要問題不是飽和，而是導(dǎo)航閉環(huán)控制中所引入的延遲問題.這種延遲所帶來的影響在出現(xiàn)滑移的情況下可以很明顯地感覺出來.

4 仿真分析

采用Matlab仿真環(huán)境分別仿真在不同車速下階躍響應(yīng)的控制效果、跟蹤正弦曲線的控制效果.

4.1 不同速度下的階躍響應(yīng)

參考路徑為2 m的階躍響應(yīng)，速度從2 km/h加速到14 km/h，每個檔次提升2 km/h.在進(jìn)行仿真的時候需要首先調(diào)節(jié)(Kd，Kp)的值以獲得較好的性能，同時使轉(zhuǎn)向電機(jī)不至于飽和 (實際仿真時，Kd=0.6，Kp=0.09).仿真結(jié)果如圖4所示，可以看出所有不同速度下的階躍響應(yīng)曲線幾乎全重合在一起，因此可以得出路徑跟蹤算法和車速無關(guān).

同時還可以看出，除了兩端的速度，即速度為2 km/h或14 km/h時路徑跟蹤的偏差稍微大一些之外，其它速度的路徑跟蹤效果非常好 (車體軌跡和參考路徑之間的偏差小于2.7 cm).

圖4 不同車速下的階躍響應(yīng)曲線

4.2 曲線路徑跟蹤仿真

此處選用正弦曲線作為路徑跟蹤的參考路徑，雖然在ARV實際應(yīng)用場合很少遇到正弦曲線路線，但由于正弦曲線明顯不同于直線，因此用于驗證路徑跟蹤控制律式(23)的性能具有很好的代表性.

圖5 參考路徑為正弦曲線時的路徑跟蹤結(jié)果

參考正弦曲線路徑的振幅為3 m，周期為30 m，初始時刻，車體距離參考曲線的距離為0.5 m，速度為6 km/h，路徑跟蹤效果如圖5所示.圖 (b)給出了側(cè)向偏移y值的變化情況:從總體上來說，側(cè)向偏移被控制在25 cm之內(nèi)，較好地完成了控制器設(shè)計的目標(biāo);同時可以看到側(cè)向偏移周期性地出現(xiàn)一個最大值 (大于20 cm).結(jié)合圖 (a)可以看出，出現(xiàn)最大值的地方為正弦曲線的波峰處，這是由于正弦曲線波峰處，ARV運動學(xué)模型 (式(10))的非線性化更加厲害，而控制律 (式(23))從這個模型中演化過來，微小的模型變化或者標(biāo)定錯誤都能降低控制器的性能.

5 結(jié)束語

推導(dǎo)出了適用于ARV無人車路徑跟蹤控制的運動學(xué)模型，并應(yīng)用“鏈?zhǔn)较到y(tǒng)”控制理論設(shè)計出與速度無關(guān)的控制律，通過仿真分析表明該控制律具有良好的控制效果.

在推導(dǎo)運動學(xué)模型時，采用了兩輪車模型，而實際車輛的瞬時轉(zhuǎn)向中心不一定落在中間車輪的法線上，考慮到滑移的影響，可以通過控制后輪的轉(zhuǎn)向角度以減小滑轉(zhuǎn)，同時本控制算法僅僅考慮了0～15 km/h的速度范圍，如果車速進(jìn)一步提高，就需要考慮側(cè)滑及滑轉(zhuǎn)的影響，甚至需要考慮動力學(xué)因素.

[1]王學(xué)先，霍 偉.移動機(jī)器人路徑跟蹤的控制研究[D].北京:北京航空航天大學(xué)，2002.

[2]Kannayama YJ，F(xiàn)ahroo f.A new line tracking method for nonholonomic vehicles[C].//IEEE International Conference on Rbotics and Autonation. 1997:2908-2913.

[3]Kannayama YJ，F(xiàn)ahroo f.A circle tracking method for nonholonomic vehicles [J].Robot Control，1997，(2)，531-538.

[4]Zhong-ping Jiang， Henk Nijmeijer. A recursive technique for tracking control of nonholonomic systems in chained form [J].IEEE Transactions on Automatic Control，1999，44(2):265-279.

[5]Shim H S，Kim J H，Koh K.Variable structure control of nonholonomic wheeled mobile robot[C].//IEEE International Conference on Robotics and Automation.1995:1694-1699.

[6]C.Canudas de Wit，B.Siciliano，G.Bastin editors.Theory of robot control [M]. Springer-Verlag，Berlin，1996.

[7]C.Samson.Control of chained systems application to path following and time varying point stabilization of mobile robots[J].IEEE Transactions on Automatic Control，1995，40(1):64-77.

[8]H.Sussmann，E.Sontag，and Y.Yang.A general result on the stabilization oflinearsystemsusing bounded controls[J].IEEE Transactions on Automatic Control，1994，39(12):2411-2425.