郁 伉， 肖本賢， 李艷紅

(1.合肥工業(yè)大學(xué)電氣與自動化工程學(xué)院，安徽合肥 230009;2.咸陽師范學(xué)院物理系，陜西咸陽 712000)

0 引 言

由于存在非完整約束，移動機器人路徑跟蹤控制十分具有挑戰(zhàn)性。文獻(xiàn)[1]對系統(tǒng)在期望路徑附近Taylor線性化，對得到的線性時變系統(tǒng)設(shè)計控制律，從而實現(xiàn)原系統(tǒng)的局部跟蹤;文獻(xiàn)[2]采用輸入輸出反饋線性化的方法，將非線性系統(tǒng)分解為2個子系統(tǒng)，再根據(jù)性能指標(biāo)逐一設(shè)計控制律，以達(dá)到跟蹤目的;文獻(xiàn)[3]采用后退(back steping)方法的思想，將系統(tǒng)分解為低階系統(tǒng)，利用中間虛擬變量和部分Lyapunov函數(shù)，設(shè)計了具有全局漸進(jìn)穩(wěn)定的跟蹤控制器。

目前常見機器人模型有運動學(xué)模型、動力學(xué)模型以及位姿誤差模型。自文獻(xiàn)[4]提出位姿誤差模型以來，還很少見到新的模型。本文針對兩輪差動驅(qū)動移動機器人，在仔細(xì)分析其工作過程的基礎(chǔ)上，推導(dǎo)出新的模型，并給出2種算法。該模型在表達(dá)式上具有運動學(xué)模型簡潔的優(yōu)點，經(jīng)過簡單變換，得到1個運動周期內(nèi)機器人位姿誤差。2種算法針對機器人初始位置在期望路徑附近，相對于傳統(tǒng)的基于Lyapunov函數(shù)的控制律設(shè)計，不必計算大量偏導(dǎo)數(shù)，在保證跟蹤精度、跟蹤速度的前提下，具有計算量小的優(yōu)點;和反饋線性化算法相比，不存在奇異性問題[5]。

1 定 義

機器人在t1時刻給定一個輸入，緊接著在t2時刻給定另一個輸入，則t2-t1即為機器人的運動周期T。由以上定義可知，在1個運動周期里，兩輪差動驅(qū)動機器人的左輪線速度VL、右輪線速度V R保持不變。

若VL=VR，軌跡為直線段;若VL≠VR，軌跡為圓弧。假設(shè)機器人在平面上做純滾動無滑動的運動，質(zhì)心選取在兩輪連線中心點上，機器人前進(jìn)速度v、旋轉(zhuǎn)角速度ω分別為:

其中，ω逆時針為正;D為兩輪間距。

由(1)式解出VR、VL:

(2)式為兩輪差動驅(qū)動機器人實際控制用的表達(dá)式，關(guān)鍵在于v、ω如何確定。

后面討論的機器人，無特殊交代，都是指兩輪差動驅(qū)動移動機器人，且設(shè)V SET為期望運動速度。

由前面分析，1個運動周期內(nèi)，機器人軌跡只可能是直線段或圓弧，又因為直線段可以看成半徑為∞的圓弧，因此模型中機器人軌跡以圓弧為例。假設(shè)某個運動周期開始，機器人位姿向量為(x0，y0，φ0)，結(jié)束時刻位姿向量為(x1，y1，φ1)。φ0、φ1是機器人前進(jìn)方向與x軸正向夾角。

由圖1可計算出圓心坐標(biāo)(x0-R sinφ0，y0+R cosφ0)，R為圓弧半徑。進(jìn)而求得:

圖1 運動周期模型

2 控制律提出

令Δx=x1-x0，Δy=y1-y0，代入(3)式得:

令k=Δy/Δx，Δx≠0。用(5)式除以(4)式得:

整理(6)式得:

解得:

將(11)式代入(4)式得:

考慮到sin(2α-φ0)-sinφ0=2sin(α-φ0)cosα，則有:

(11)式和(12)式即為機器人路徑跟蹤控制律。關(guān)于如何選取Δx、Δy，將在后文重點介紹。

若期望路徑是斜率為k的直線，并且機器人方向角沿著直線方向，即k=tanφ0。sinα=得α=φ0，代入(11)式得ω=0。代入(12)式得v=Δx/(T cosα)，這顯然與實際情況相符合。

若Δx=0，即直線沿著y軸方向，若仍采用(11)式、(12)式控制律，則控制器奇異?？芍匦略O(shè)k=Δx/Δy(Δy≠0)，具體推導(dǎo)公式同Δx≠0。因此，可根據(jù)運動方向定義k，避免奇異性問題。

該算法用到了移動機器人當(dāng)前點和期望跟蹤點2點之間的斜率，本文稱其為斜率算法。

3 選取期望跟蹤點

把跟蹤點選取在期望路徑上，是為了便于后續(xù)計算、分析、比較，同時也確保了機器人始終不會偏離期望路徑太遠(yuǎn)。

設(shè)期望路徑方程f(x)，機器人起始位姿向量(x0，y0，φ0)，運動周期結(jié)束時位姿向量即期望跟蹤點位姿向量(x0+Δx，y0+Δy，φ0+ωT)。機器人軌跡方程(x-x r)2+(y-y r)2=R2，x r=x0-R sinφ0，yr=y0+R cosφ0，R=Δx/(2sin(α-φ0)cosα)，α是機器人起始點與期望跟蹤點連線的傾斜角。由于跟蹤點選取在期望路徑上，Δy=f(x0+Δx)-f(x0)，關(guān)鍵在于Δx如何選取。

機器人在1個運動周期里前進(jìn)的距離是V SET T，T是恒定的。顯然，讓機器人在每個運動周期里前進(jìn)的距離一樣，就可以讓跟蹤速度保持恒定，確保跟蹤快速穩(wěn)定，由此可建立等式(13):

y(x)是機器人實際軌跡。由于軌跡方程是二次微分方程，所以有2個解，y具體求解如圖2所示。

圖2 機器人運動軌跡

(13)式中被積函數(shù)和積分上限中都含有Δx，計算十分復(fù)雜，考慮到1個運動周期中，機器人起點、終點都在期望路徑上，故Δy=f(x0+Δx)-f(x0)，有如下近似計算公式:

將f(x0+Δx)在x=x0處泰勒展開得:f(x0+Δx)=f(x0)+f′(x0)Δx+o(Δx)，并代入(14)式解得:

4 曲率半徑算法

下面提出的算法用到了曲率半徑的概念，稱之為曲率半徑算法。在1個運動周期里，移動機器人軌跡為圓弧，考慮到前進(jìn)距離很短，且期望路徑一般比較平滑，可認(rèn)為曲率1/ρ不變，ρ為曲率半徑。設(shè)移動機器人起點在期望路徑上，方向角沿著圓弧切線方向，起點曲率半徑為ρ。ρ即為運動周期模型中半徑R，代入(4)式得:

解出:

圖3 期望路徑為圓弧

令β=V SET T/R，代入(18)式:

繼續(xù)化簡:

將sin(β+φ0)=sinβcosφ0+cosβsinφ0，sinφ0=x0/R，cosφ0=-y0/R代入(21)式，有

當(dāng)期望路徑為圓弧時，Δx計算公式如(22)式。

由(17)式，欲求ω，先求出Δx/ρ+sinφ0:

將(23)式代入(17)式得:ω=β/T=V SET/R，由此v=VSET，與實際情況完全相符。

5 仿 真

首先定義跟蹤誤差e，如圖4所示。直線x=xt，與期望路徑相交于(xt，y f t)，與實際軌跡相交于(xt，yrt)。此時跟蹤誤差e為:

圖4 跟蹤誤差定義

為了驗證此模型和斜率算法的有效性，并且和現(xiàn)有的反饋線性化算法[5]做對比，在Matlab7.0上進(jìn)行仿真實驗。仿真參數(shù)見表1所列。

反饋線性化算法和斜率算法，可保證跟蹤速度都恒為VSET，跟蹤誤差仿真結(jié)果如圖5～圖7所示。

表1 仿真參數(shù)

圖6 斜率算法

圖7 跟蹤圓弧對比

在1個運動周期內(nèi)，若e處處為零，則實際軌跡與期望路徑完全重合。由于1個運動周期內(nèi)，跟蹤誤差e有大有小，本次仿真中誤差采樣10次，取其平均值。讓移動機器人跟蹤直線y=x、圓弧拋物線y=x2、圓弧y=初始位置如下:圖5a、圖6a為(-1，-1.01，π/4);圖5b、圖6b為(-1，0，-π/2);圖5c、圖6c(-1，1，-1.107 1);圖7a、圖7b為(-0.099，0，-π/2);圖7c為(-0.1，0，-π/2)。

由圖5a、圖5b、圖6a、圖6b可以看出，在跟蹤直線和半徑為1 m圓弧時，2種算法跟蹤精度都能滿足實際需求，誤差在0.01 m以內(nèi)，并且斜率算法不存在奇異性問題。由圖5c、圖6c可以看出，跟蹤拋物線時，反饋線性化算法跟蹤誤差大于0.01 m，而斜率算法跟蹤精度足以滿足實際需求。

圖5b、圖6b跟蹤圓弧半徑r=1 m，圖7a、7b跟蹤圓弧半徑r=0.1m。當(dāng)圓弧半徑減小時，反饋線性化算法跟蹤精度急劇下降，而斜率算法跟蹤精度提高2個數(shù)量級，并且誤差波動很小。觀察圖6c，跟蹤拋物線y=x2，在x=0附近，誤差較小而此處曲率較大。圓弧半徑越小，曲率越大。綜上所述，斜率算法尤其適用于跟蹤曲率較大的曲線。

對于期望路徑是圓弧的特例，曲率半徑算法仿真結(jié)果如圖7c所示。由圖7c可以看出，跟蹤精度達(dá)10-17，并且進(jìn)入穩(wěn)態(tài)后，跟蹤誤差幾乎為零。若初始條件和圖7a、圖7b一樣，跟蹤精度下降到10-3，但仍可以滿足實際需求。跟蹤精度下降，是因為推導(dǎo)曲率半徑算法的前提中要求機器人初始位置在圓弧上，方向角沿著切線方向。

6 結(jié)束語

本文推導(dǎo)了兩輪差動驅(qū)動移動機器人的一種實用模型，該模型關(guān)鍵在于找出一個運動周期內(nèi)機器人起點坐標(biāo)和終點坐標(biāo)之間的關(guān)系，對于別的類型機器人建模也具有借鑒意義。最后通過仿真，驗證模型、斜率算法和曲率半徑算法的有效性，并且通過對比反饋線性化算法和斜率算法，驗證了斜率算法的優(yōu)越性。

仿真中大多假定機器人初始位置在期望路徑上，若實際初始位置偏離期望路徑太遠(yuǎn)，可以采用生成臨時路徑的辦法[6]，并結(jié)合假設(shè)-預(yù)測-調(diào)整的迭代優(yōu)化思想[7]、最大、最小螞蟻系統(tǒng)的思想[8]，先讓機器人運動到期望路徑上，再采用本文的算法。減小算法對初始條件的依賴性，是下一步研究的方向。

[1] Walsh G，Tilbury D，Sastry S，etal.Stabilization of trajectories for systems w ith non-holonom ic constraints[C]//Proceedings of IEEE In ternational Con ference on Robotics and Automation.Nice，F(xiàn)rance:IEEE，1992:1999-2004.

[2] Coelho P，Nunes U.Lie algeb ra application tom obile robot control:a tutorial[M].Camb ridge University Press，2003:483-493.

[3] 吳衛(wèi)國，陳輝堂，王月娟.移動機器人全局軌跡跟蹤控制[J].自動化學(xué)報，2001，27(3):326-331.

[4] Kanayama Y，Kimura Y，M iyazaki F.A stable tracking controlmethod for an autonomousmobile robot[C]//Proceedings of IEEE In ternational Conference on Robotics and Autom ation.Piscataway，USA:IEEE，1990:384-389.

[5] 馬保離，霍 偉.移動小車的路徑跟蹤與鎮(zhèn)定[J].機器人，1995，17(6):358-362.

[6] Lee S，Park JH.Dynam ic path follow ing using temporary path generator for mobile robots with non-holonom ic constrain ts[C]//Proceedings of IEEE In ternational Conference on Systems，Man and Cybernetics.Tok yo，Japan:IEEE，1999:631-636.

[7] 武 星，樓佩煌，楊 雷.基于視野狀態(tài)分析的機器人路徑跟蹤智能預(yù)測控制[J].機器人，2009，31(4):357-364.

[8] 肖本賢，劉 剛，余 雷，等.基于MMAS的機器人路徑規(guī)劃[J].合肥工業(yè)大學(xué)學(xué)報:自然科學(xué)版，2008，31(1):63-67.