車洪磊

（中國安全生產(chǎn)科學(xué)研究院地鐵火災(zāi)與客流疏運(yùn)安全北京市重點(diǎn)實(shí)驗(yàn)室 北京100012）

0 引言

全方位移動(dòng)機(jī)器人具有較好的機(jī)動(dòng)性、任意方向零半徑轉(zhuǎn)彎能力以及受限制空間中的移動(dòng)能力,在家庭生活及工業(yè)環(huán)境中得到了廣泛的應(yīng)用,如全方位移動(dòng)輪椅、叉車、機(jī)械手等[1-3]。其中,在各種類型的全方位移動(dòng)機(jī)器人中,麥克納姆輪的移動(dòng)機(jī)器人應(yīng)用尤為廣泛[4-6]。然而,由于麥克納姆輪的移動(dòng)機(jī)器人使用4 個(gè)獨(dú)立的電機(jī),在面臨不確定性及外界擾動(dòng)的情況下,其運(yùn)動(dòng)控制極具挑戰(zhàn)。

針對(duì)麥克納姆輪全方位移動(dòng)機(jī)器人的運(yùn)動(dòng)控制,國內(nèi)外開展了許多相關(guān)研究工作。文獻(xiàn)[7]提出了模糊比例積分微分（proportion integral differential,PID）控制算法,實(shí)現(xiàn)了對(duì)移動(dòng)機(jī)器人4 個(gè)麥克納姆輪轉(zhuǎn)速的精確控制,但該算法并未對(duì)移動(dòng)機(jī)器人的具體方位進(jìn)行跟蹤控制。文獻(xiàn)[8]基于麥克納姆輪的移動(dòng)平臺(tái)特性設(shè)計(jì)了路徑跟蹤解耦控制方案,采用增量式PID 控制算法分別對(duì)移動(dòng)機(jī)器人的位置和角度進(jìn)行跟蹤控制。文獻(xiàn)[9]提出了自適應(yīng)滑?？刂破?設(shè)計(jì)了PID 形式的滑模面,用于麥克納姆輪驅(qū)動(dòng)的移動(dòng)機(jī)器人軌跡跟蹤以獲得跟蹤性能及魯棒性能。文獻(xiàn)[10]設(shè)計(jì)了基于反饋線性化方法的運(yùn)動(dòng)控制器,通過李雅普諾夫方法證明了其漸近穩(wěn)定性,然而該算法的控制精度不夠。文獻(xiàn)[11,12]分別提出了四麥克納姆輪全方位移動(dòng)機(jī)器人的自適應(yīng)滑?？刂坪妥赃m應(yīng)二階滑?？刂?在一定程度上實(shí)現(xiàn)了較高的軌跡跟蹤精度,但是控制結(jié)構(gòu)較為復(fù)雜。

本文針對(duì)目前麥克納姆輪移動(dòng)機(jī)器人軌跡跟蹤控制存在的問題,首先推導(dǎo)出麥克納姆輪移動(dòng)機(jī)器人的動(dòng)力學(xué)模型,然后利用雙環(huán)滑?？刂品椒ㄔO(shè)計(jì)了基于移動(dòng)機(jī)器人動(dòng)力學(xué)模型的軌跡跟蹤控制器,最后通過Lyapunov 函數(shù)證明該機(jī)器人系統(tǒng)的穩(wěn)定性。

1 全方位移動(dòng)機(jī)器人運(yùn)動(dòng)模型

1.1 運(yùn)動(dòng)學(xué)模型

圖1 全方位移動(dòng)機(jī)器人簡圖

移動(dòng)機(jī)器人的逆運(yùn)動(dòng)學(xué)公式如式（2）所示。

1.2 動(dòng)力學(xué)模型

假設(shè)全方位移動(dòng)機(jī)器人在平坦的平面內(nèi)移動(dòng),采用拉格朗日方程建立機(jī)器人系統(tǒng)的動(dòng)力學(xué)模型,系統(tǒng)的動(dòng)能為

其中,vx、vy和ωz分別表示機(jī)器人在平面內(nèi)沿坐標(biāo)軸的線速度和角速度;m為系統(tǒng)的總質(zhì)量;Jz為車輛繞z軸的轉(zhuǎn)動(dòng)慣量,Jω為車輪繞其轉(zhuǎn)軸的轉(zhuǎn)動(dòng)慣量;υ為粘性摩擦系數(shù)。

由于機(jī)器人系統(tǒng)的勢(shì)能等于0,因此其拉格朗日函數(shù)可以表示為

移動(dòng)機(jī)器人的拉格朗日方程為

聯(lián)立式（3）～式（5）得到機(jī)器人系統(tǒng)動(dòng)力學(xué)方程為

由于機(jī)器人系統(tǒng)面臨擾動(dòng)和不確定性,結(jié)合式（6）移動(dòng)平臺(tái)的動(dòng)力學(xué)模型表示為

其中,τd∈R4為干擾項(xiàng)。

2 雙環(huán)滑?？刂坡稍O(shè)計(jì)

采用雙環(huán)滑模變結(jié)構(gòu)控制設(shè)計(jì)移動(dòng)機(jī)器人的控制律,使用積分滑模來實(shí)現(xiàn)切換函數(shù)的設(shè)計(jì)。雙環(huán)滑?？刂葡到y(tǒng)結(jié)構(gòu)如圖2 所示,該系統(tǒng)由速度環(huán)和位置環(huán)構(gòu)成,內(nèi)環(huán)為速度環(huán),外環(huán)為位置環(huán)。外環(huán)滑?？刂坡蓪?shí)現(xiàn)位置的跟蹤,外環(huán)控制器產(chǎn)生速度指令,并傳遞給內(nèi)環(huán)系統(tǒng),內(nèi)環(huán)則通過內(nèi)環(huán)滑?？刂坡蓪?shí)現(xiàn)對(duì)速度指令的跟蹤。

圖2 移動(dòng)機(jī)器人軌跡跟蹤控制原理圖

2.1 外環(huán)滑模控制

外環(huán)為位置環(huán),設(shè)外環(huán)控制指令為θd,則外環(huán)誤差為θe=θd-θ。定義外環(huán)滑模函數(shù)為

2.2 內(nèi)環(huán)滑模控制

內(nèi)環(huán)為速度環(huán),將外環(huán)控制律的輸出作為內(nèi)環(huán)的速度指令,則內(nèi)環(huán)指令設(shè)為ωd,內(nèi)環(huán)控制誤差為

由Lyapunov 穩(wěn)定性理論可知,系統(tǒng)在擾動(dòng)和不確定部分存在的情況下是漸進(jìn)穩(wěn)定的。

3 仿真實(shí)驗(yàn)

為了驗(yàn)證全方位移動(dòng)機(jī)器人雙環(huán)滑模控制軌跡跟蹤算法的有效性,分別以直線與圓弧作為參考軌跡進(jìn)行跟蹤控制。設(shè)計(jì)外環(huán)控制參數(shù)為ρ1=5,K1=diag（0.3 0.3 0.3 0.3）,在外環(huán)控制律中,采用飽和函數(shù)代替切換函數(shù),飽和函數(shù)的參數(shù)取Δi=0.1,i=1,2,3,4。設(shè)計(jì)內(nèi)環(huán)控制律參數(shù)為ρ2=1.5,μ=10,K2=diag（1 1 1 1）。設(shè)干擾及不確定部分為τd=[sintcostsin2tcos2t]T。仿真機(jī)器人系統(tǒng)的物理參數(shù)如表1 所示。

表1 移動(dòng)機(jī)器人參數(shù)

3.1 直線軌跡跟蹤

仿真部分控制輸入為4 個(gè)獨(dú)立驅(qū)動(dòng)的麥克納姆旋轉(zhuǎn)角度,設(shè)置其4 個(gè)角度跟蹤指令為θd=[10cos0.1t20cos0.1t10cos0.1t20cos0.1t]T,初始角θ0=[15 30 15 30]T,初始角速度ω為0。由運(yùn)動(dòng)學(xué)模型可知,移動(dòng)平臺(tái)在x -y平面內(nèi)的參考軌跡方程為,t（t≥0） 為仿真時(shí)間。圖3 為麥克納姆輪全方位移動(dòng)機(jī)器人的直線軌跡跟蹤仿真結(jié)果。

圖3（a）顯示了移動(dòng)機(jī)器人每個(gè)車輪旋轉(zhuǎn)角度能夠很快地跟蹤到目標(biāo)軌跡。圖3（b）顯示了移動(dòng)機(jī)器人跟蹤目標(biāo)軌跡時(shí)每臺(tái)電機(jī)的控制輸入力矩。發(fā)生在電機(jī)控制力矩的突變主要是由外部擾動(dòng)產(chǎn)生的,從圖3（b）可以看出擾動(dòng)產(chǎn)生的影響在很短的時(shí)間內(nèi)就能夠穩(wěn)定下來。從圖3（c）～（e）可以看出,移動(dòng)平臺(tái)從任意初始位置開始運(yùn)動(dòng)都能夠較快地跟蹤到參考軌跡,且位置誤差ex、ey均能較快地收斂至0。

圖3 移動(dòng)平臺(tái)的圓弧軌跡跟蹤仿真結(jié)果

3.2 圓弧軌跡跟蹤

設(shè)置角度跟蹤指令為θd=[1 0 sin 0. 1t20cos0.1t10sin0.1t20cos0.1t]T,選取初始輸入θ0=[12 32 12 32]T,初始角速度ω=0。由運(yùn)動(dòng)學(xué)模型可知,移動(dòng)平臺(tái)在x -y平面內(nèi)的參考軌跡方程為,t（t≥0） 為仿真時(shí)間。圖4 為四麥克納姆輪全方位移動(dòng)平臺(tái)的圓弧軌跡跟蹤仿真結(jié)果。

由圖4 可以看出,基于本文中所述的雙環(huán)滑?？刂品椒?四輪全向移動(dòng)機(jī)器人對(duì)圓軌跡的跟蹤效果較好,可以在存在較大初始誤差的情況下,確保機(jī)器人在較短的時(shí)間內(nèi)實(shí)現(xiàn)對(duì)期望軌跡的完全跟蹤。

圖4 移動(dòng)平臺(tái)的圓弧軌跡跟蹤仿真結(jié)果

4 實(shí)驗(yàn)驗(yàn)證

為了進(jìn)一步驗(yàn)證上述算法的實(shí)際應(yīng)用效果,在自主研發(fā)的麥克納姆輪全方位移動(dòng)機(jī)器人平臺(tái)上進(jìn)行了軌跡跟蹤實(shí)驗(yàn),移動(dòng)平臺(tái)實(shí)物圖如圖5 所示。該實(shí)驗(yàn)平臺(tái)以固高嵌入式運(yùn)動(dòng)控制器為核心控制器,采用編碼器、慣性測(cè)量單元（inertial measurement unit,IMU）和StarGazer 定位傳感器實(shí)現(xiàn)機(jī)器人的定位和導(dǎo)航。

圖5 麥克納姆輪移動(dòng)平臺(tái)實(shí)物圖

設(shè)定移動(dòng)機(jī)器人的線速度為v=0.2 m/s,采樣周期為0.01s,移動(dòng)機(jī)器人直線和圓弧軌跡跟蹤實(shí)驗(yàn)的初始位置分別為（330 cm,600 cm）和（310 cm,40 cm）,其中圓形軌跡的半徑r=100 cm。為了驗(yàn)證本文所提算法的抗干擾能力,實(shí)驗(yàn)過程中,分別在直線軌跡（290 cm,210 cm）處和圓形軌跡的（135 cm,-15 cm）處施以外力干擾,改變機(jī)器人的運(yùn)動(dòng)狀態(tài)。實(shí)驗(yàn)結(jié)果如圖6 所示。

圖6 的實(shí)驗(yàn)結(jié)果表明,移動(dòng)平臺(tái)從任意位置出發(fā)都能夠較快地跟蹤到期望軌跡,并且當(dāng)機(jī)器人在外力作用下偏離期望軌跡時(shí),系統(tǒng)能夠較快地做出調(diào)整,說明其具有較強(qiáng)的抗干擾能力。

圖6 移動(dòng)平臺(tái)的實(shí)際跟蹤結(jié)果

5 結(jié)論

針對(duì)受擾動(dòng)及參數(shù)不確定的麥克納姆輪全方位移動(dòng)機(jī)器人的軌跡跟蹤問題,將雙環(huán)滑?？刂破饕胧芸叵到y(tǒng)中,應(yīng)用雙環(huán)滑模變結(jié)構(gòu)跟蹤期望輸出,在Matlab/Simulink 仿真環(huán)境驗(yàn)證了該控制算法的可行性。仿真和實(shí)驗(yàn)結(jié)果表明,利用雙環(huán)滑模切換函數(shù)設(shè)計(jì)的移動(dòng)機(jī)器人運(yùn)動(dòng)控制器在擾動(dòng)和不確定性存在的情況下能夠很好地跟蹤到期望軌跡。