s0 + rA/ =O,該約束條件保證輪式移動(dòng)機(jī)器人兩輪軸線上的瞬時(shí)速度為 〇。其中[x，y，0]T是系統(tǒng)的狀態(tài)，（x，y)為質(zhì)心在世界坐標(biāo)系中的位置坐標(biāo)，0為機(jī)器 人姿態(tài)角，v、w分別是機(jī)器人的實(shí)際線速度和實(shí)際角速度，d是機(jī)器人的質(zhì)心與幾何中 心之間的距離。期望軌跡模型
，其中0 J是期望軌跡姿 態(tài)，\是期望線速度，是期望角速度。針對(duì)期望軌跡模型和運(yùn)動(dòng)學(xué)模型建立誤差模型
，其中，9 e為輪 式移動(dòng)機(jī)器人實(shí)際位姿與期望位姿的偏差。
[0054]步驟（2):設(shè)計(jì)虛擬反饋量6以及滑模面Sl、s2:
[0056]其中 Si= xe，=見，a !、|3 ! (i = 1，2)滿足 a!>〇、|3 々〇,pp qi (i = 1，2)是正 奇數(shù)且滿足Qi<Pi< 2q p \為期望線速度，x e、ye、0 e為輪式移動(dòng)機(jī)器人實(shí)際位姿與期望 位姿的偏差。
[0057] 步驟（3):設(shè)計(jì)有限時(shí)間虛擬速度控制器，如圖2所示，其中虛擬線速度V。和虛擬 角速度w。為：
[0059] 其中，ye、為輪式移動(dòng)機(jī)器人實(shí)際位姿與期望位姿的偏差，^為期望線速度， Wj?為期望角速度， a i、0 i (i = 1，2)彳兩足a }>〇、|3 }>〇，pp (i = 1，2)是正奇數(shù)且彳兩足qi < Pi< 2q i〇
[0060] 取李雅普諾夫方程為：
[0062] 對(duì)其求導(dǎo)，得
[0064] 這表明sf x 6可以在有限時(shí)間
內(nèi)到達(dá)零。
[0065] 選取李雅普諾夫方程為：
[0067] 對(duì)其求導(dǎo)，得

- I
[0069] 同理表明& =慫可以在有限時(shí)間 =內(nèi)到達(dá) 零。因?yàn)榉?之+arctan(vr凡），故可以得到0e=-arctan(v rye)。根據(jù)誤差模型有 丸二-\w+v,.sin慫，又因?yàn)樵趖sl以后x 6就到達(dá)零，所以允二vr sin必。這樣我們選取李雅普 諾夫方程為：
[0071] 對(duì)其求導(dǎo)，得
[0073]由此可知當(dāng)xe= 0,慫=0時(shí)，ye可以漸近到達(dá)ye= 0。又因?yàn)? e=-arctan(vrye)， 所以9e= 0。
[0074] 步驟（4):根據(jù)執(zhí)行機(jī)構(gòu)以及周邊的環(huán)境建立動(dòng)力學(xué)模型：
[0076] 對(duì)其進(jìn)行適當(dāng)?shù)淖儞Q可以得到：
[0077]
[0078] 其中

且Tdl彡E^td2< E 2;EdE2是未知的正常數(shù)，r是輪子的半徑，b是機(jī) 器人輪軸長(zhǎng)度的一半，d是質(zhì)心與幾何中心之間的距離。
<p= [(/?,, +ml2)/bc(mn -mr)/b]' ,m = mc+2mw, m12= 0.25b _2r2 (mb2-J) , J = mcd2+2mwb2+J c+2Jm，c = 0? Sb-Vnicd，mn= 0? 25b _2r2 (mb2+J) +JW，爪^是機(jī)器人的質(zhì)量（不包括 驅(qū)動(dòng)輪和它的電機(jī)轉(zhuǎn)子的質(zhì)量），mw是一個(gè)驅(qū)動(dòng)輪和它的電機(jī)轉(zhuǎn)子的質(zhì)量，J。表不機(jī)器人 (不包括驅(qū)動(dòng)輪和它的電機(jī)轉(zhuǎn)子）繞z軸（過質(zhì)心的垂直于XY平面）的轉(zhuǎn)動(dòng)慣量，J m表示 每個(gè)輪子及它的電機(jī)轉(zhuǎn)子關(guān)于直徑的轉(zhuǎn)動(dòng)慣量，Jw表示每個(gè)輪子及它的電機(jī)轉(zhuǎn)子關(guān)于輪軸 的轉(zhuǎn)動(dòng)慣量，T為左右輪力矩控制器，V。為虛擬線速度，W。為虛擬角速度，W為實(shí)際角速度， =「K = [V - V,.，H' _ U., r，q =卜' W]r 〇
[0079] 如圖2所示，設(shè)計(jì)了一個(gè)有限時(shí)間力矩控制器：
[0080]
[0081] 其中T = [ T p T 2]T，T挪T 2分別為驅(qū)動(dòng)左輪和右輪的控制力矩， 々 = [K=[v_vc，w_wcf , BKG R2X2是可逆的矩陣，Y e R2X3是關(guān)于實(shí)際速度與虛 擬速度的矩陣，分e及3x1是對(duì)所述未知參數(shù)的估計(jì)向量，a3>0、03>0,p3、93是正奇數(shù) 并且滿足q3< P 3< 2q3，rpr 2是可設(shè)定的增益，// = (\，色，Xe、ye為輪式 移動(dòng)機(jī)器人實(shí)際位姿與期望位姿的偏差，左=[右，表]"是對(duì)外部擾動(dòng)上限值E的估計(jì)，
[0082] 選取李雅普諾夫方程為
，對(duì)其求導(dǎo)并代入控制器 最終可得：
[0083] v4 <-rfp/fr <〇 ( 1^)
[0084] 由此可知$也可以在有限時(shí)間內(nèi)到達(dá)零，即實(shí)際線速度v和實(shí)際角速度w可以在 有限時(shí)間內(nèi)跟蹤虛擬線速度V。和虛擬角速度w。。
[0085] 綜上所述，本發(fā)明在存在未知參數(shù)和外部擾動(dòng)等干擾的復(fù)雜工況下，能夠在有限 時(shí)間內(nèi)跟蹤期望軌跡，跟蹤效果良好，并且對(duì)未知參數(shù)和外部擾動(dòng)的魯棒性強(qiáng)。
[0086] 在本發(fā)明的實(shí)施例中所采用的擾動(dòng)模型為
，（>〇,跟蹤軌跡 采用如下兩種模型：
[0087] (I)勾速圓周運(yùn)動(dòng)，其中vr= l，wr= 0. 5。此時(shí)所選取的參數(shù)為|3 Q= |3 != |3 2 = 0.8，aQ= a i= a 2=0.5。圖3和圖4是（14)式中的左右輪控制力矩t 1和t ^的 曲線圖，圖5是輪式移動(dòng)機(jī)器人實(shí)際的運(yùn)動(dòng)軌跡與期望軌跡的曲線圖，圖6是軌跡跟蹤的位 姿誤差圖，圖7是期望線速度虛擬線速度V。和實(shí)際線速度v的曲線圖，圖8是期望角速 度Wp虛擬角速度W。和實(shí)際角速度W的曲線圖。
[0088] (II)變速余弦運(yùn)動(dòng)，其中
。此時(shí)所選取的參數(shù)為 0 〇= 0 i= 1. 9，0 2= 1. 2，a 〇= a 2. 8，a 2= 〇? 5。圖 9 和圖 10 是（14)式中的左 右輪控制力矩、和t 2的曲線圖，圖11是輪式移動(dòng)機(jī)器人實(shí)際的運(yùn)動(dòng)軌跡與期望軌跡的 曲線圖，圖12是軌跡跟蹤的位姿誤差圖，圖13是期望線速度虛擬線速度v。和實(shí)際線速 度v的曲線圖，圖14是期望角速度w,、虛擬角速度w。和實(shí)際角速度w的曲線圖。
【主權(quán)項(xiàng)】
1. 基于快速終端滑模的輪式移動(dòng)機(jī)器人軌跡跟蹤方法，其特征在于：該方法采用如下 步驟： 步驟（1);建立輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型和期望軌跡模型，并根據(jù)運(yùn)動(dòng)學(xué)模型和 期望軌跡模型建立誤差模型； 步驟（2);引入合適的滑模面Si、S2,根據(jù)步驟（1)中的誤差模型設(shè)計(jì)虛擬反饋量巧； 步驟（3);結(jié)合步驟（1)中的誤差模型W及步驟（2)中的虛擬反饋量巧和滑模面Si、S2， 設(shè)計(jì)有限時(shí)間虛擬線速度V。和虛擬角速度W根據(jù)輪式移動(dòng)機(jī)器人的實(shí)際線速度V和實(shí)際 角速度W，得到線速度偏差信號(hào)巧和角速度偏差信號(hào)W; 步驟（4);建立輪式移動(dòng)機(jī)器人的動(dòng)力學(xué)模型，將線速度偏差信號(hào)C和角速度偏差信號(hào) 巧代入動(dòng)力學(xué)模型，設(shè)計(jì)輪式移動(dòng)機(jī)器人的左右輪力矩控制器T、未知參數(shù)估計(jì)器參和外部 擾動(dòng)估計(jì)器后。2. 根據(jù)權(quán)利要求1所述的基于快速終端滑模的輪式移動(dòng)機(jī)器人軌跡跟蹤方法，其特征 在于；所述步驟（2)中設(shè)計(jì)的虛擬反饋量巧和滑模面Si、S2滿足：其中Si=Xe，S2=是，ai、0i(i= 1，。滿足ai〉〇、0i〉〇，Pi、qi(i= 1，。是正奇數(shù)且 滿足qi<Pi< 2q1，Vr為期望線速度，Xe、y。、0e為輪式移動(dòng)機(jī)器人實(shí)際位姿與期望位姿的 偏差。3. 根據(jù)權(quán)利要求1所述的基于快速終端滑模的輪式移動(dòng)機(jī)器人軌跡跟蹤方法，其特征 在于；所述步驟（3)中設(shè)計(jì)的虛擬線速度V。和虛擬角速度W。為：其中，y。、0。為輪式移動(dòng)機(jī)器人實(shí)際位姿與期望位姿的偏差，Vt為期望線速度，Wt為期 望角速度，a1、Pi(i= 1, 2)滿足a1〉〇、P1〉〇,Pi、Qi(i= 1, 2)是正奇數(shù)且滿足屯<Pi < 2Qi〇4. 根據(jù)權(quán)利要求1所述的基于快速終端滑模的輪式移動(dòng)機(jī)器人軌跡跟蹤方法，其特征 在于；所述步驟（4)中的未知參數(shù)包括輪式移動(dòng)機(jī)器人的質(zhì)量m、轉(zhuǎn)動(dòng)慣量I或者質(zhì)屯、與幾 何中屯、的距離d。5. 根據(jù)權(quán)利要求4所述的基于快速終端滑模的輪式移動(dòng)機(jī)器人軌跡跟蹤方法，其特征 在于；所述步驟（4)中設(shè)計(jì)的左右輪力矩控制器T、未知參數(shù)估計(jì)器參和外部擾動(dòng)估計(jì)器怎 分別為：其中T= [T。T2]T，T1和T2分別為驅(qū)動(dòng)左輪和右輪的控制力矩， 巧=化例r=[V_Vr，W_Wcf，BrER2X2是可逆的矩陣，YER2X3是關(guān)于實(shí)際速度與虛 擬速度的矩陣，^^￡於^1是對(duì)所述未知參數(shù)的估計(jì)向量，〇3〉0、0 3〉〇,P3、93是正奇數(shù) 并且滿足Q3<P3< 2q3，r1、r2是可設(shè)定的增益，// =(-、;.，巧-y,.)'，Xe、ye為輪式 移動(dòng)機(jī)器人實(shí)際位姿與期望位姿的偏差，怎=[與，4]''是對(duì)外部擾動(dòng)上限值E的估計(jì)，，I巧 |=(|v-Vr|，|w-wj)r。
【專利摘要】本發(fā)明公開基于快速終端滑模的輪式移動(dòng)機(jī)器人軌跡跟蹤方法。該方法包括四個(gè)步驟：(1)建立輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型和期望軌跡模型，并根據(jù)運(yùn)動(dòng)學(xué)模型和期望軌跡模型建立誤差模型；(2)引入合適的滑模面s1、s2，根據(jù)誤差模型設(shè)計(jì)虛擬反饋量(3)得到線速度偏差信號(hào)和角速度偏差信號(hào)(4)建立輪式移動(dòng)機(jī)器人的動(dòng)力學(xué)模型，將線速度偏差信號(hào)和角速度偏差信號(hào)代入動(dòng)力學(xué)模型，設(shè)計(jì)輪式移動(dòng)機(jī)器人的左右輪力矩控制器τ、未知參數(shù)估計(jì)器和外部擾動(dòng)估計(jì)器本發(fā)明對(duì)存在未知參數(shù)和外部擾動(dòng)等干擾的復(fù)雜工況下，能夠在有限時(shí)間內(nèi)跟蹤期望軌跡，跟蹤效果良好，并且對(duì)未知參數(shù)和外部擾動(dòng)的魯棒性強(qiáng)。
【IPC分類】G05D1/02
【公開號(hào)】CN104932506
【申請(qǐng)?zhí)枴緾N201510312675
【發(fā)明人】翟軍勇, 黃大偉, 王智慧
【申請(qǐng)人】東南大學(xué)
【公開日】2015年9月23日
【申請(qǐng)日】2015年6月9日