欠驅(qū)動(dòng)移動(dòng)機(jī)器人的路徑跟蹤控制

楊興明， 朱 建， 高銀平， 許東昌

（合肥工業(yè)大學(xué) 計(jì)算機(jī)與信息學(xué)院，安徽 合肥 230009）

0 引 言

輪式移動(dòng)機(jī)器人是一種典型的具有非完整約束的系統(tǒng)，因其具有廣泛的應(yīng)用前景和理論意義引起了國內(nèi)外學(xué)者的廣泛關(guān)注［1－6］。近年來，根據(jù)基于的模型不同，移動(dòng)機(jī)器人路徑跟蹤的研究可以分為基于運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型2種研究方式?；谶\(yùn)動(dòng)學(xué)模型的研究主要是以速度作為控制量使真實(shí)軌跡和參考軌跡之間的誤差收斂到0，研究的是速度和參考軌跡之間的關(guān)系。基于動(dòng)力學(xué)模型控制器反映的是控制電壓和速度之間的關(guān)系，能夠比較真實(shí)地反應(yīng)移動(dòng)機(jī)器人的控制規(guī)律。

文獻(xiàn)［7－8］基于動(dòng)力學(xué)模型，利用自適應(yīng)的方法實(shí)現(xiàn)了三輪移動(dòng)機(jī)器人的路徑跟蹤?？紤]到雙輪移動(dòng)機(jī)器人比三輪或四輪機(jī)器人具有更好的機(jī)動(dòng)性和靈活性，體積更小，尤其是在擁擠的空間，更能體現(xiàn)其靈活性，雙輪移動(dòng)機(jī)器人的路徑跟蹤也同樣具有很高的研究價(jià)值。

本文針對雙輪移動(dòng)機(jī)器人的研究首先建立系統(tǒng)的運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型，并針對運(yùn)動(dòng)學(xué)模型通過反演技術(shù)設(shè)計(jì)運(yùn)動(dòng)學(xué)控制器，進(jìn)而考慮到系統(tǒng)的不確定性和跟蹤速度與姿態(tài)平衡控制欠驅(qū)動(dòng)的特點(diǎn)，基于動(dòng)力學(xué)模型設(shè)計(jì)了分層滑?？刂破鳎詈笸ㄟ^Matlab仿真驗(yàn)證該控制方法的有效性。

1 雙輪移動(dòng)機(jī)器人模型的建立

（1）雙輪移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型。在機(jī)器人移動(dòng)的區(qū)域內(nèi)建立笛卡爾坐標(biāo)系（Oxy），選擇移動(dòng)機(jī)器人2個(gè)驅(qū)動(dòng)輪軸線的中點(diǎn)坐標(biāo)（xc，yc）作為參考點(diǎn)，則機(jī)器人的位姿可以描述為：p＝［xc，yc，θ］T，其中θ為機(jī)器人的方向角。假設(shè)機(jī)器人運(yùn)動(dòng)滿足純滾動(dòng)無滑動(dòng)的約束條件，則根據(jù)文獻(xiàn)［9］可以得到系統(tǒng)的運(yùn)動(dòng)學(xué)方程：

其中，vc、ωc分別為小車運(yùn)動(dòng)速度和轉(zhuǎn)彎角速度。

（2）雙輪移動(dòng)機(jī)器人的動(dòng)力學(xué)模型。圖1所示為雙輪平衡機(jī)器人的簡化模型。其中，R表示車輪半徑；l表示車體重心到電機(jī)軸線之間的距離；D表示兩輪之間的距離。

下文中用到的符號含義如下：x、v表示車底盤中心的位移和速度；δ表示擺桿與豎直方向的角度；θ、ω表示車體繞豎直方向旋轉(zhuǎn)的角度和角速度；Mp、mr分別表示車體質(zhì)量和小輪的質(zhì)量；Jr、Jp、Jθ分別表示小輪繞軸的轉(zhuǎn)動(dòng)慣量、車體繞電機(jī)軸的轉(zhuǎn)動(dòng)慣量和車體繞擺桿的轉(zhuǎn)動(dòng)慣量；φ表示斜坡角度，在這里取0；；Km、Ke表示電機(jī)的力矩系數(shù)和電機(jī)的反電動(dòng)勢系數(shù)；ul、ur表示左右輪的控制電壓；cl、cr表示左右電機(jī)的輸出轉(zhuǎn)矩。

圖1 雙輪移動(dòng)機(jī)器人的簡化模型

［10］，利用牛頓力學(xué)的建模方法對系統(tǒng)建模，可以得到與系統(tǒng)狀態(tài)相關(guān)的方程組，即

定義X＝［δ，x，，v，θ，ω］T為狀態(tài)變量，對（2）式進(jìn)行線性化后，利用電機(jī)的輸出轉(zhuǎn)矩和控制電壓的關(guān)系c＝Km（u－K），把狀態(tài)方程中控制量cl、cr轉(zhuǎn)化為由電機(jī)的輸入電壓ul、ur作為控制量。然后定義ul＝0.5uv＋0.5uω，ur＝0.5uv－0.5uω，可以得到系統(tǒng)的狀態(tài)方程為：

其中

其中，uv（t）、uω（t）為控制輸入；fv（X，t）、fδ（X，t）、fω（X，t）不確 定 部 分 分 別 為 Δfv（X，t）、Δfδ（X，t）、Δfω（X，t），假設(shè)系統(tǒng)不確定部分上界已知，且滿足｜Δfv（X，t）｜≤ΔFv，｜Δfδ（X，t）｜≤ΔFδ，｜Δfω（X，t）｜≤ΔFω；dv（t）、dδ（t）、dω（t）為界干擾且滿足條件｜dv（t）｜≤Dv，｜dδ（t）｜≤Dδ，｜dω（t）｜≤Dω。

由于系統(tǒng)的平衡角度難以大范圍可控，因此必須對系統(tǒng)平衡角度的控制范圍進(jìn)行分析，系統(tǒng)平衡角度的臨界值可以定義為δ0，當(dāng)δ的變化超過δ0時(shí)，即使電機(jī)對系統(tǒng)施加最大的力矩，也不能使系統(tǒng)回到平衡點(diǎn)附近。當(dāng)｜δ｜＝δ0時(shí)，機(jī)器人平衡角度的變化速率＝0，假設(shè)機(jī)器人兩輪的運(yùn)動(dòng)速度是相同的，則可以得到系統(tǒng)的綜合邊界條件為：

把（4）式代入（2）式中前2個(gè)式中的任意一個(gè)，例如代入第1個(gè)式中并假設(shè)兩電機(jī)的最大扭轉(zhuǎn)力矩相同為Cm，可得：

將各參數(shù)值代入（5）式并由電機(jī)的最大扭轉(zhuǎn)力矩Cm＝6.8N·m，可以求得系統(tǒng)平衡角度的控制范圍為：－27.6°≤δ≤27.6°。

2 雙輪移動(dòng)機(jī)器人控制器的設(shè)計(jì)

圖2所示為雙輪機(jī)器人的系統(tǒng)結(jié)構(gòu)圖，其中，pd＝［xd，yd，θd］T表示參考位姿；ep＝［ex，ey，eθ］T表示跟蹤誤差；Vc＝［vc，ωc］T為運(yùn)動(dòng)學(xué)控制器輸出的控制速度和角速度；e＝［ev，eω］T表示動(dòng)力學(xué)控制器對Vc的跟蹤誤差；eδ為機(jī)器人平衡角度δ的跟蹤誤差，在機(jī)器人的運(yùn)動(dòng)過程中希望其能夠保持豎直，所以取δd＝0；u＝［uv，uω］T表示動(dòng)力學(xué)控制器的輸出電壓，由于機(jī)器人的欠驅(qū)動(dòng)特點(diǎn)uv控制機(jī)器人的速度v和平衡角度的變化速率，uω控制機(jī)器人的轉(zhuǎn)彎角速度ω；V＝［v，ω］T表示機(jī)器人的實(shí)際速度和轉(zhuǎn)動(dòng)角速度；p＝［xc，yc，θ］T表示機(jī)器人的實(shí)際位姿。

圖2 雙輪移動(dòng)機(jī)器人系統(tǒng)結(jié)構(gòu)圖

2.1 運(yùn)動(dòng)學(xué)控制器的設(shè)計(jì)

在固定的坐標(biāo)系內(nèi)系統(tǒng)的位姿誤差為：

通過反演法設(shè)計(jì)控制器，步驟如下。

（1）引入虛擬量α。根據(jù)（1）式有：

構(gòu)造Lyapunov函數(shù)：

對V1求導(dǎo)，并把（7）式代入可得：

則需設(shè)計(jì)虛擬量α滿足如下條件：

其中，C1＞0；C2＞0。則由（10）式可得線速度vc和虛擬控制量α為：

（2）定義z＝α－θ，實(shí)現(xiàn)θ跟蹤α。構(gòu)造Lyapunov函數(shù)為：

對（12）式求導(dǎo)：

取角速度ωc的控制率為：

則有：

2.2 動(dòng)力學(xué)控制器的設(shè)計(jì)

雙輪移動(dòng)機(jī)器人的動(dòng)力學(xué)系統(tǒng)可以分為平衡控制系統(tǒng)、速度控制系統(tǒng)和轉(zhuǎn)彎角速度控制系統(tǒng)3個(gè)子系統(tǒng)，由于其平衡控制子系統(tǒng)和速度控制子系統(tǒng)由同一個(gè)控制電壓uv來控制，所以具有欠驅(qū)動(dòng)的特點(diǎn)，為了避免復(fù)雜的解耦過程，本文采用分層滑?？刂破骺刂茩C(jī)器人的平衡角度δ和運(yùn)動(dòng)速度v，采用傳統(tǒng)的滑?？刂破骺刂茩C(jī)器人的轉(zhuǎn)彎角速度。

定義ev＝vc－v，eω＝ω－ωc，eδ＝－δ，設(shè)計(jì)第1層滑模面為：

其中，λδ、λv、λω為待設(shè)計(jì)正的滑模面常系數(shù)。

對（16）式求導(dǎo)，并結(jié)合（3）式可以得到：

為了使得平衡控制子系統(tǒng)和速度控制子系統(tǒng)趨近各自的滑模面，令δ＝0，v＝0，先不考慮dδ、dv對系統(tǒng)的影響，則每個(gè)子系統(tǒng)的等效控制律可以取為：

為了使得平衡角度δ和運(yùn)動(dòng)速度v跟蹤誤差都為0，則根據(jù)第1層滑模面（16）式定義第2層滑模面：

其中，λ為正常數(shù)。取趨近率：

結(jié)合（18）式、（19）式可設(shè)計(jì)系統(tǒng)的控制律為：

其中，kv、kω為切換項(xiàng)增益，為正常數(shù)。增加系統(tǒng)的切換項(xiàng)主要是為了提高控制器的抗干擾能力。

為了證明系統(tǒng)的穩(wěn)定性，在系統(tǒng)的可控范圍內(nèi)，構(gòu)造Lyapunov函數(shù)：

則對V3求導(dǎo)，然后結(jié)合（17）～（19）式和（22）式可得：

由（23）式可知：

所以＜0，則系統(tǒng)的滑動(dòng)模態(tài)存在且可達(dá)，系統(tǒng)穩(wěn)定。

為了抑制傳統(tǒng)滑?？刂破饔捎诳刂坡实牟贿B續(xù)性導(dǎo)致的抖振問題，在滑模面附近引入邊界層，用脈沖函數(shù)取代不連續(xù)的符號函數(shù)。此時(shí)控制律為：

脈沖函數(shù)sat（·）被定義為：

其中，μ為邊界層寬度。

3 仿真結(jié)果

3.1 仿真參數(shù)的選取

通過簡化模型轉(zhuǎn)換為狀態(tài)方程：

外界的干擾主要由路面的不平整引起，當(dāng)?shù)孛娴钠露葹?°，造成的干擾為dθ＝1.377 1，dx＝－0.903 8。

（2）控制器中的參數(shù)取值。λv、λδ、λω、λ的選擇決定了滑模面的性能特征，本文參考文獻(xiàn)［11］采用零極點(diǎn)配置的方法選取λω＝1.520，λδ＝3.975 3，λv＝0.862 3，λ＝1.744。其他參數(shù)：kω＝6，μ＝10，kv＝15，C1＝0.5，C2＝0.5，C3＝2。

3.2 仿真結(jié)果及分析

為了驗(yàn)證本文所提出算法的有效性，在加入系統(tǒng)不確定性干擾的條件下，利用本文設(shè)計(jì)的控制器對雙輪機(jī)器人進(jìn)行軌跡跟蹤控制仿真。

（1）直線軌跡的跟蹤。假設(shè)給定的參考軌跡是一條起始點(diǎn)為（0，0）、斜率為π／4的直線。機(jī)器人的初始位姿為（0.2，0，π／3），初始姿態(tài)角度為8.6°，在t＝20s時(shí)引入5°坡造成的脈沖干擾dθ、dx。在0～30s的時(shí)間內(nèi)對參考曲線以0.1m／s的速度進(jìn)行跟蹤仿真，結(jié)果如圖3～圖6所示。

圖3 0～30s內(nèi)移動(dòng)機(jī)器人平面運(yùn)動(dòng)曲線

圖4 0～30s移動(dòng)機(jī)器人的位姿誤差

從圖4的位姿誤差曲線可以看出，移動(dòng)機(jī)器人在6s內(nèi)跟蹤誤差收斂到0，對直線具有良好的跟蹤性能，在20s時(shí)引入外界干擾，系統(tǒng)仍可以穩(wěn)定跟蹤參考曲線。

2008年后，隨著中國地質(zhì)調(diào)查局新一輪區(qū)域化探項(xiàng)目的實(shí)施[1]，青海省第五地質(zhì)礦產(chǎn)勘查院承擔(dān)并完成了青海柴達(dá)木盆地周緣的區(qū)域化探掃面工作，并在該區(qū)新圈定了一大批面積大、強(qiáng)度高、具有找礦前景的化探異常[2]，通過對少數(shù)地球化學(xué)異常檢查，新發(fā)現(xiàn)夏日哈木等礦(床)點(diǎn)數(shù)十處[3]，極大的推動(dòng)了該區(qū)的地質(zhì)找礦工作。但該景觀區(qū)的化探方法技術(shù)研究工作較少[4-6]，這使得人們對該區(qū)的化探采樣工作更加關(guān)注。

從圖5的仿真曲線可以看出，移動(dòng)機(jī)器人自身在4s內(nèi)達(dá)到平衡，在5s內(nèi)速度達(dá)到并保持在0.1m／s。

綜上所述，本文設(shè)計(jì)的控制方法可以在保持雙輪移動(dòng)機(jī)器人自身平衡的同時(shí)穩(wěn)定地跟蹤期望的直線軌跡。

（2）圓軌跡的跟蹤。假設(shè)給定的參考軌跡的圓方程為：x2＋y2＝1，其起始點(diǎn)為（1，0），機(jī)器人的初始位姿為（0.8，0，0），初始姿態(tài)角度δ為8.6°，在t＝40s時(shí)引入5°坡造成的脈沖干擾dθ、dx。在0～70s的時(shí)間內(nèi)對參考曲線以0.1m／s的速度進(jìn)行跟蹤仿真，結(jié)果如圖7～圖10所示。

從圖8的位姿誤差曲線可以看出，移動(dòng)機(jī)器人在8s內(nèi)跟蹤誤差收斂到0，在20s時(shí)引入外界干擾，系統(tǒng)仍可以穩(wěn)定跟蹤參考曲線，說明系統(tǒng)對圓具有良好的跟蹤性能。

圖5 0～30s機(jī)器人的平衡角度和速度

圖6 0～30s控制電壓

圖7 0～70s移動(dòng)機(jī)器人平面運(yùn)動(dòng)曲線

圖8 0～70s移動(dòng)機(jī)器人的位姿誤差

圖9 0～70s機(jī)器人的平衡角度和速度

圖10 0～70s控制電壓

從圖9的仿真曲線可以看出，移動(dòng)機(jī)器人在5.5s內(nèi)達(dá)到姿態(tài)平衡，在7.5s內(nèi)速度達(dá)到0.1m／s。

綜上所述，本文設(shè)計(jì)的控制方法可以在保持雙輪移動(dòng)機(jī)器人自身平衡的同時(shí)穩(wěn)定地跟蹤期望的圓軌跡。

4 結(jié)束語

本文針對雙輪機(jī)器人的路徑跟蹤問題，把系統(tǒng)的控制分為運(yùn)動(dòng)學(xué)控制和動(dòng)力學(xué)控制2個(gè)部分。在動(dòng)力學(xué)控制中，由于系統(tǒng)的姿態(tài)平衡和跟蹤速度的欠驅(qū)動(dòng)特點(diǎn)，本文采用分層滑?？刂破鲗?shí)現(xiàn)對跟蹤速度和姿態(tài)平衡的控制。在加入系統(tǒng)不確定性干擾的條件下，對直線軌跡和圓軌跡的跟蹤控制進(jìn)行仿真，仿真結(jié)果表明該控制方法可以控制雙輪移動(dòng)機(jī)器人在保持姿態(tài)平衡的同時(shí)實(shí)現(xiàn)軌跡的精確跟蹤。

［參考文獻(xiàn)］

［1］ Morin P，Samson C.Control of nonholonomic mobile robots based on the transverse function approach［J］.Robot，2009，25：1058－1073.

［2］ Fukao T，Nakagawa H，Adachi N.Adaptive tracking control of a nonholonomic mobile robot［J］.Robot，2000，16：609－615.

［3］ 郁 伉，肖本賢，李艷紅.受非完整性約束的移動(dòng)機(jī)器人路徑跟蹤算法［J］.合肥工業(yè)大學(xué)學(xué)報(bào)：自然科學(xué)版，2010，33（9）：1315－1319.

［4］ 楊 芳，王朝立.不確定非完整移動(dòng)機(jī)器人的軌跡跟蹤控制［J］.控制工程，2012，19（2）：258－261.

［5］ Buccieri D，Perritaz D，Mullhaupt P，et al.Velocity－scheduling control for a unicycle mobile robot：theory and experiments［J］.Robot，2009，25：451－458.

［6］ Coelho P，Nunes U.Path－following control of mobile robots in presence of uncertainties［J］.Robot，2005，21：252－261.

［7］ Fukao T，Nakagawa H，Adachi N.Adaptive tracking control of a nonholonomic mobile robot［J］.Robotics and Automation，2000，16（5）：609－615.

［8］ Pourboghrat F，Karlsson M P.Adaptive control of dynamic mobile robot with nonholonomic constraints［J］.Computers and Electrical Engineering，2002，28：241－253.

［9］ Jiang Z P，Nijmeijer H.Tracking control of mobile robots：a case study in backstepping［J］.Automatica，1997，33（7）：1393－1399.

［10］ 陳 星，魏衡華，張玉斌.兩輪行走倒立擺系統(tǒng)的建模與魯棒方差控制［J］.計(jì)算機(jī)仿真，2006，23（3）：263－266.

［11］ Ackermann J，Utkin V.Sliding mode control design based on Ackermann's formula［J］.IEEE Transactions on Automatic Control，1998，43（2）：234－237.