王明明, 朱瑩瑩, 張磊, 王璐, 衛(wèi)宣伯, 方靜

(1．西北工業(yè)大學(xué)航天學(xué)院，陜西西安710072; 2．西北工業(yè)大學(xué)深圳研究院，廣州深圳518057; 3．武警特警指揮學(xué)院，北京102202)

移動(dòng)機(jī)器人在復(fù)雜地形探測(cè)、戰(zhàn)場(chǎng)偵查、地質(zhì)勘探、倉(cāng)儲(chǔ)物流等方面具有重要的應(yīng)用前景，其控制器的設(shè)計(jì)水平是保證任務(wù)成功的關(guān)鍵[1]。機(jī)器人控制器的設(shè)計(jì)開(kāi)始向著應(yīng)用于復(fù)雜環(huán)境，且能抵抗外部干擾的方向發(fā)展。由于能全向移動(dòng)且轉(zhuǎn)彎半徑為零，基于全向輪的移動(dòng)機(jī)器人受到了廣泛關(guān)注，并且被應(yīng)用于多種場(chǎng)景，如倉(cāng)庫(kù)搬運(yùn)機(jī)器人、全向輪椅等。全向移動(dòng)機(jī)器人中，麥克納姆輪驅(qū)動(dòng)的全向移動(dòng)機(jī)器人受到越來(lái)越多的關(guān)注[2-3]。

由于麥克納姆輪驅(qū)動(dòng)的全向移動(dòng)機(jī)器人存在諸多優(yōu)勢(shì)，使其在工業(yè)界和學(xué)術(shù)界得到越來(lái)越多的關(guān)注，諸多學(xué)者對(duì)其運(yùn)動(dòng)學(xué)建模和動(dòng)力學(xué)建模展開(kāi)了研究[3]。Muir等[4]對(duì)此類(lèi)機(jī)器人進(jìn)行了運(yùn)動(dòng)學(xué)建模，從運(yùn)動(dòng)學(xué)方式上，給出了基于麥克納姆輪的全向機(jī)器人運(yùn)動(dòng)學(xué)公式。Tlale等[5-6]為此類(lèi)機(jī)器人建立了動(dòng)力學(xué)模型并從動(dòng)力學(xué)角度對(duì)基于麥克納姆輪的全向機(jī)器人展開(kāi)分析。盡管此類(lèi)機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型發(fā)展較為成熟，針對(duì)應(yīng)用于實(shí)際環(huán)境的移動(dòng)機(jī)器人軌跡跟蹤控制研究仍然方興未艾。

由于4個(gè)麥克納姆輪分別由4個(gè)直流電機(jī)驅(qū)動(dòng)，針對(duì)此類(lèi)機(jī)器人如何設(shè)計(jì)多驅(qū)動(dòng)電機(jī)的協(xié)調(diào)控制器成為至關(guān)重要的問(wèn)題。實(shí)際工程應(yīng)用中，電機(jī)選型多選用帶有速度伺服系統(tǒng)的電機(jī)，由于電機(jī)的速度伺服系統(tǒng)多采用PID控制，因此伺服控制系統(tǒng)會(huì)對(duì)整個(gè)系統(tǒng)帶來(lái)更多的不確定性；此外機(jī)器人所運(yùn)行的環(huán)境也具有諸多不確定性，因此控制器的魯棒性和自適應(yīng)性就顯得至關(guān)重要。Shimada等[7]提出了一種基于位置誤差的控制方法，并研制了實(shí)驗(yàn)裝置，對(duì)所提出的方法進(jìn)行了實(shí)驗(yàn)驗(yàn)證。Wang等[8]在不考慮擾動(dòng)的情況下，提出了一種用于軌跡規(guī)劃的滑動(dòng)模態(tài)控制器，Viet等[9]考慮了有擾動(dòng)的情況，在外加擾動(dòng)的情況下針對(duì)全向移動(dòng)機(jī)器人設(shè)計(jì)了滑?？刂破?，其研究結(jié)果表明滑模控制器能有效抑制外部干擾。Fierro等[10]針對(duì)全向移動(dòng)機(jī)器人的軌跡規(guī)劃設(shè)計(jì)了一種神經(jīng)網(wǎng)絡(luò)控制器。另外，Xu等[11]針對(duì)全向移動(dòng)機(jī)器人設(shè)計(jì)了一種基于神經(jīng)網(wǎng)絡(luò)的魯棒滑?？刂破鳌；谏窠?jīng)網(wǎng)絡(luò)的控制器具有控制精度高等優(yōu)點(diǎn)，但需要大量學(xué)習(xí)，因此具體實(shí)現(xiàn)上存在不便。以上控制器存在參數(shù)難以整定的問(wèn)題，如何設(shè)計(jì)一種高效的自適應(yīng)控制器是解決此問(wèn)題的關(guān)鍵。Wang等[12]針對(duì)移動(dòng)機(jī)器人的軌跡規(guī)劃設(shè)計(jì)了一種自適應(yīng)控制器，并證明了其穩(wěn)定性。Sira-Ramirez等[13]設(shè)計(jì)了一種能夠快速收斂的自適應(yīng)控制器，以應(yīng)對(duì)直流電機(jī)的不確定性。

綜上所述，本文的研究對(duì)象具有確定的運(yùn)動(dòng)學(xué)模型，適合采用基于模型的控制方法。常見(jiàn)的基于模型的控制方法有：LQR控制方法[14]以及結(jié)合LQR控制方法優(yōu)點(diǎn)與卡爾曼濾波器的控制方法LQG控制[15]，然而LQR控制雖然具有良好的魯棒性，但其計(jì)算復(fù)雜且對(duì)模型精確度要求過(guò)高。而LQG控制魯棒性較差，并且該類(lèi)方法多用于線(xiàn)性系統(tǒng)控制器設(shè)計(jì)。通過(guò)對(duì)移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)建模可知，基于麥克納姆輪的全向移動(dòng)機(jī)器人是一類(lèi)多輸入多輸出(MIMO)的非線(xiàn)性系統(tǒng)，而滑動(dòng)模態(tài)控制[16]是一種針對(duì)不確定條件且具有魯棒性的非線(xiàn)性控制器。因此本文提出基于滑模控制的自適應(yīng)魯棒軌跡跟蹤控制器?；？刂破鞯某Ｒ?jiàn)問(wèn)題為控制器輸出的抖振問(wèn)題，馬廣富等[17]提出了一種多冪次滑模趨近律,能夠顯著減小抖振，并且通過(guò)仿真進(jìn)行了驗(yàn)證，但其不具有自適應(yīng)性。為使系統(tǒng)具有更高的魯棒性，本文設(shè)計(jì)了一種比例-微分-積分形式的滑模面來(lái)提高系統(tǒng)的魯棒性，并且提出了一種多冪次趨近律來(lái)提高系統(tǒng)的動(dòng)態(tài)性能，最后為避免參數(shù)整定的麻煩操作，本文設(shè)計(jì)了自適應(yīng)律以提高系統(tǒng)的自適應(yīng)性，借此可以解決上述問(wèn)題。

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

麥克納姆輪是一種在輪表面存在一些滾子(如圖1所示)。

圖1 麥克納姆輪

這些滾子的轉(zhuǎn)軸方向與整個(gè)輪的轉(zhuǎn)軸方向呈45°夾角，每個(gè)麥克納姆輪由一個(gè)獨(dú)立的直流電機(jī)驅(qū)動(dòng)，通過(guò)調(diào)節(jié)4只電機(jī)的不同轉(zhuǎn)速，即可實(shí)現(xiàn)全向的移動(dòng)，比如橫向和縱向移動(dòng)，傾斜移動(dòng)和零轉(zhuǎn)彎角度轉(zhuǎn)動(dòng)(如圖2所示)。

圖2 帶麥克納姆輪的移動(dòng)機(jī)器人運(yùn)動(dòng)演示

對(duì)麥克納姆輪驅(qū)動(dòng)的全向移動(dòng)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模，首先要建立相關(guān)的坐標(biāo)系(如圖3所示)，Oixiyi為慣性坐標(biāo)系(inertial coordinate system),Obxbyb為連體坐標(biāo)系(body-fixed coordinate system),其原點(diǎn)Ob為車(chē)體的幾何中心,其y軸始終指向車(chē)體縱軸,x軸與y軸垂直并組成右手坐標(biāo)系,Owixwiywi為第i只輪子的輪系坐標(biāo)系,其原點(diǎn)位于麥克納姆輪的幾何中心,x軸與驅(qū)動(dòng)電機(jī)輸出軸平行,y軸與x軸垂直并組成右手坐標(biāo)系。規(guī)定車(chē)體右前方的輪子為1號(hào)輪,順序按逆時(shí)針排序。

圖3 移動(dòng)機(jī)器人坐標(biāo)系

如圖1所示,定義麥克納姆輪滾子的轉(zhuǎn)軸與Owixwiywi的x軸所呈夾角為ψ,定義第i只輪Owixwiywi位置矢量pwi(i=1,2,3,4)=[xwi,ywi,zwi]T,ωix為第i只輪的角速度,ωiy為第i只輪上的滾子角速度,ωiz為麥克納姆輪繞過(guò)Owi且垂直于Owixwiywi平面轉(zhuǎn)軸的角速度。Ri(i=1,2,3,4)為第i只輪子的半徑,ri為滾子半徑,輪系坐標(biāo)系中輪子的速度矢量可以表示為:

(1)

根據(jù)旋轉(zhuǎn)矩陣,可以獲得如(2)式所示的速度計(jì)算公式:

(2)

對(duì)雅可比矩陣分析可以得出如下結(jié)論:

1) 當(dāng)且僅當(dāng)ψi=0時(shí),|Ji|=0。所以對(duì)于麥克納姆輪來(lái)說(shuō),此時(shí)雅可比矩陣是奇異的。

2) 值得注意的是,rank(Ji)=3,所以每個(gè)輪子具有3個(gè)自由度。

(3)

通過(guò)(2)式和(3)式,移動(dòng)機(jī)器人的微分運(yùn)動(dòng)學(xué)公式可以描述如下:

(4)

式中,ωi(i=1,2,3,4)是第i只輪的轉(zhuǎn)速,單位為rad/s。通過(guò)運(yùn)動(dòng)學(xué)公式不難發(fā)現(xiàn),基于麥克納姆輪的移動(dòng)機(jī)器人可以實(shí)現(xiàn)真正的全向移動(dòng)。

閉環(huán)控制的反饋為靜止坐標(biāo)系下的位姿信息。進(jìn)一步地,根據(jù)旋轉(zhuǎn)矩陣,可得到連體坐標(biāo)系下的速度Obxbyb和慣性坐標(biāo)系下的速度Oixiyi的轉(zhuǎn)換關(guān)系為:

(5)

2 自適應(yīng)滑?？刂破髟O(shè)計(jì)

2.1 滑模面設(shè)計(jì)

滑模控制器設(shè)計(jì)的第一個(gè)任務(wù)就是設(shè)計(jì)滑模面,在控制系統(tǒng)工作的過(guò)程中,保證系統(tǒng)始終工作在滑模面s(t)=0上。

(6)

(7)

2.2 趨近律設(shè)計(jì)

在滑?？刂破髦?常用的趨近律有:等速趨近律、指數(shù)趨近律、冪次趨近律、一般趨近律[12,18]。其中冪次趨近律具有較好的收斂速度,更進(jìn)一步地,為了保證較高的收斂速度,并且盡量減小控制器的抖振問(wèn)題,本文設(shè)計(jì)一種多冪次滑模趨近律以達(dá)到控制目標(biāo),并通過(guò)仿真驗(yàn)證其在系統(tǒng)動(dòng)態(tài)響應(yīng)過(guò)程中具有快速的收斂速度。本文選用的多冪次滑模趨近律如下:

(8)

式中

(9)

符號(hào)函數(shù)sgn( )的引入會(huì)使系統(tǒng)的工作過(guò)程出現(xiàn)振蕩。因此,為了減小符號(hào)函數(shù)對(duì)系統(tǒng)的影響,本文對(duì)趨近律進(jìn)行如下改進(jìn),引入飽和函數(shù),可有效削弱趨近律在滑模面附近的跳變,以改善抖振問(wèn)題:

(10)

式中，δi為正小量:

(11)

穩(wěn)定性證明如下:首先選取李雅普諾夫函數(shù)

當(dāng)且僅當(dāng)si(t)=0,(i=1,2,3)時(shí)等號(hào)成立,故該系統(tǒng)是李雅普諾夫穩(wěn)定的。

2.3 自適應(yīng)律設(shè)計(jì)

σi|si(t)|βsi(t),(i=1,2,3)

(14)

自適應(yīng)律設(shè)計(jì)如下:

(15)

穩(wěn)定性證明如下:如(13)式所示已經(jīng)證明了李雅普諾夫函數(shù)V1的穩(wěn)定性,現(xiàn)取李雅普諾夫函數(shù)如下:

(16)

(17)

將公式(15)代入上式,可得:

(18)

穩(wěn)定性得證。

對(duì)公式(6)兩端同時(shí)求導(dǎo),可以得到:

(19)

結(jié)合(14)式可以得到:

(20)

進(jìn)一步可以得到控制信號(hào):

(21)

結(jié)合公式(4)和公式(5),可以得到各個(gè)輪的轉(zhuǎn)速:

(22)

3 仿真驗(yàn)證與分析

3.1 設(shè)計(jì)流程

圖4展示了本文所提控制器的設(shè)計(jì)流程。首先對(duì)機(jī)器人的運(yùn)動(dòng)學(xué)進(jìn)行建模,此后為保證對(duì)此類(lèi)非線(xiàn)性耦合系統(tǒng)能夠提供優(yōu)越的控制性能,本文設(shè)計(jì)以滑?？刂破鳛榛A(chǔ);為了提高控制器的魯棒性,選取了比例-積分-微分形式的滑模面;為保證系統(tǒng)的動(dòng)態(tài)性能,選取了適當(dāng)?shù)内吔?為了避免整定參數(shù),同時(shí)使系統(tǒng)具有更好的自適應(yīng)性,設(shè)計(jì)了一種自適應(yīng)律。然后針對(duì)所設(shè)計(jì)的控制器在仿真中加入擾動(dòng)并與PID控制的普通滑模控制(SMC)的效果進(jìn)行了對(duì)比,最后通過(guò)實(shí)驗(yàn)驗(yàn)證的方式展示了本文所設(shè)計(jì)控制器的實(shí)用性。

圖4 設(shè)計(jì)流程圖

3.2 仿真驗(yàn)證

為驗(yàn)證本文所提的自適應(yīng)滑動(dòng)模態(tài)控制器的魯棒性和動(dòng)態(tài)性能,首先設(shè)計(jì)了一條軌跡,通過(guò)對(duì)標(biāo)稱(chēng)軌跡的跟蹤控制,并且在其中加入了脈沖擾動(dòng),與傳統(tǒng)PID控制器和無(wú)自適應(yīng)性的SMC控制器進(jìn)行對(duì)比驗(yàn)證,分析本文所設(shè)計(jì)控制器的性能。本文測(cè)試使用的標(biāo)稱(chēng)軌跡可表示如下:

?t≥0

[xiyiφi]T=[0 m -0.2 m 0 m]T為系統(tǒng)的初始位置。同時(shí),為了驗(yàn)證系統(tǒng)的動(dòng)態(tài)性能和魯棒性,實(shí)驗(yàn)中加入了擾動(dòng),在x軸和y軸方向分別加入幅值為0.1 m,頻率為0.4 rad/s的正弦信號(hào),同時(shí)在10 s和30 s分別加入幅值為0.1 m和-0.2 m的階躍信號(hào)。仿真測(cè)試的系數(shù)設(shè)置如下:

對(duì)于SMC控制器和ASMC控制器:

表1 控制消耗時(shí)間

為了更加直觀地分析3種控制器的控制效果,對(duì)x軸和y軸方向的偏差進(jìn)行統(tǒng)計(jì)分析,如圖7和圖8所示。

通過(guò)圖7和圖8可以明顯地看出,ASMC控制器雖然在控制的開(kāi)始階段,系統(tǒng)的誤差要大于其他兩種控制器,但是隨著時(shí)間的推移,ASMC控制器的控制效果逐漸改良,并取得了優(yōu)于其他2種控制器的控制效果?；？刂破鞯淖畲髥?wèn)題在于控制信號(hào)的抖振,前文中已經(jīng)提到,多冪次趨近律的引入可以明顯的改善系統(tǒng)抖振,為驗(yàn)證其效果,參見(jiàn)圖9和圖10。

圖5 軌跡跟蹤 圖6 局部放大圖7 x軸偏差

圖8 y軸偏差 圖9 x軸速度圖10 y軸速度

為準(zhǔn)確分析各控制器性能,本文取x軸坐標(biāo)誤差和y軸坐標(biāo)誤差的均值作為控制器的控制效果,取x軸速度和y軸速度的輸出反映控制器的抖振情況統(tǒng)計(jì)結(jié)果見(jiàn)表格2和表格3。

表2 偏差平均值

表3 控制器輸出方差

對(duì)表1進(jìn)行分析,ASMC控制器的x軸偏差的平均值僅為PID控制器的1.64%,為SMC控制器的1.87%,ASMC的y軸控制偏差為PID的31.87%,為SMC控制器的30.46%,證明本文設(shè)計(jì)的ASMC具有更好的控制效果。分析表2,ASMC控制器的x軸輸出方差為PID控制器的10.42%,ASMC的y軸輸出方差為PID的57.03%,證明本文ASMC具有更好的控制效果。此外,本文設(shè)計(jì)的ASMC控制器的抖振情況優(yōu)于PID控制器。

4 實(shí)驗(yàn)驗(yàn)證與分析

本文的實(shí)驗(yàn)環(huán)境如圖11所示,實(shí)驗(yàn)樣機(jī)(輪式機(jī)器人)和實(shí)驗(yàn)所用軟件如圖12所示。

圖11 實(shí)驗(yàn)環(huán)境

圖12 實(shí)驗(yàn)樣機(jī)與軟件

本文實(shí)驗(yàn)驗(yàn)證設(shè)備電控流程圖如圖13,由Kinect深度相機(jī)連接PC端,通過(guò)USB通信經(jīng)過(guò)機(jī)器視覺(jué)處理獲得機(jī)器人的x軸和y軸的坐標(biāo)信息,進(jìn)而通過(guò)串口將信息傳輸給STM32VET6單片機(jī),VET6單片機(jī)利用SPI通信借助NRF24L01通過(guò)2.4GHz通信,將x軸和y軸的坐標(biāo)信息傳輸?shù)絊TM32ZET6單片機(jī),本文定義其為一號(hào)下位機(jī),而與一號(hào)下位機(jī)連接的還有IMU(MPU6050)用于提供機(jī)器人的φ角度信息,一號(hào)下位機(jī)獲得這些信息后,通過(guò)與函數(shù)發(fā)生器對(duì)比產(chǎn)生偏差,進(jìn)而通過(guò)ASMC控制器產(chǎn)生速度控制信號(hào),并通過(guò)UART將信號(hào)傳出給另外一片STM32ZET6,本文定義其為二號(hào)下位機(jī),二號(hào)下位機(jī)接收到速度控制信號(hào)后經(jīng)過(guò)逆運(yùn)動(dòng)學(xué)解算獲得4只直流電機(jī)的期望速度,通過(guò)PID控制器對(duì)電機(jī)速度進(jìn)行控制,達(dá)到控制目的。

圖13 實(shí)驗(yàn)流程

實(shí)驗(yàn)驗(yàn)證過(guò)程中,跟蹤函數(shù)使用同3.2節(jié)中仿真所用類(lèi)似的標(biāo)稱(chēng)軌跡:

?t≥0

圖14 軌跡跟蹤

圖15 距離誤差

圖16 實(shí)驗(yàn)結(jié)果

5 結(jié) 論

本文針對(duì)麥克納姆輪驅(qū)動(dòng)的全向移動(dòng)機(jī)器人的軌跡跟蹤問(wèn)題,在運(yùn)動(dòng)學(xué)建模的基礎(chǔ)上設(shè)計(jì)了自適應(yīng)滑?？刂破鳌？刂破髟O(shè)計(jì)分為3個(gè)步驟:滑模面設(shè)計(jì)、趨近律設(shè)計(jì)以及自適應(yīng)律設(shè)計(jì)。通過(guò)上述3個(gè)步驟完成了自適應(yīng)滑模控制器的設(shè)計(jì)并利用李雅普諾夫函數(shù)證明了其穩(wěn)定性。存在脈沖擾動(dòng)以及正弦信號(hào)擾動(dòng)條件下對(duì)控制器進(jìn)行了仿真驗(yàn)證,并將結(jié)果與傳統(tǒng)PID控制器和普通滑?？刂破骼媒y(tǒng)計(jì)學(xué)的方法進(jìn)行了對(duì)比,統(tǒng)計(jì)結(jié)果表明本文設(shè)計(jì)的控制器具有響應(yīng)快速,魯棒性強(qiáng)等優(yōu)點(diǎn)。最后為驗(yàn)證控制器的實(shí)用性,構(gòu)建了專(zhuān)用的麥克納姆輪全向移動(dòng)機(jī)器人,并利用Kinect深度攝像頭和IMU搭建了實(shí)驗(yàn)平臺(tái)對(duì)機(jī)器人進(jìn)行了實(shí)際的軌跡跟蹤控制實(shí)驗(yàn),結(jié)果表明該控制器能夠高精度的控制機(jī)器人完成軌跡跟蹤,證明了文中所設(shè)計(jì)控制器的可行性與有效性。