全向移動(dòng)機(jī)器人的自抗擾軌跡跟蹤控制

張相勝，黃將

（江南大學(xué)輕工過(guò)程先進(jìn)控制教育部重點(diǎn)實(shí)驗(yàn)室，江蘇無(wú)錫214122）

傳統(tǒng)輪式移動(dòng)機(jī)器人在平面運(yùn)動(dòng)中屬于非完整約束系統(tǒng)，一般僅適合于空間較大，轉(zhuǎn)向靈活性不高的場(chǎng)合[1]。而全向移動(dòng)機(jī)器人（Omni-directional mobile robot，ODMR）的優(yōu)勢(shì)在于運(yùn)動(dòng)過(guò)程中不需要改變自身位姿，僅依靠全向移動(dòng)機(jī)構(gòu)即可實(shí)現(xiàn)任意方向移動(dòng)，適合在狹窄的空間，精度要求較高的場(chǎng)合，如物流搬運(yùn)、救災(zāi)、偵查地形等[2]。常見(jiàn)的全向移動(dòng)機(jī)構(gòu)有：Mecanum 輪、球輪、連續(xù)切換輪、正交輪、偏心輪等[3]。其中，以Mecanum 輪作為行走機(jī)構(gòu)的ODMR 結(jié)構(gòu)簡(jiǎn)單實(shí)用，平移運(yùn)動(dòng)靈活自如，因此它的機(jī)械結(jié)構(gòu)、力學(xué)模型、運(yùn)動(dòng)控制等方面受到許多國(guó)內(nèi)外學(xué)者的關(guān)注，尤其是針對(duì)ODMR 軌跡跟蹤問(wèn)題成為近幾年的研究熱點(diǎn)。

目前，輪式移動(dòng)機(jī)器人軌跡跟蹤的控制方法主要有PID控制、自適應(yīng)滑?？刂?、模糊控制、神經(jīng)網(wǎng)絡(luò)控制、自抗擾控制[4-8]，以及這幾種控制方法的相互結(jié)合，如自適應(yīng)反步滑?？刂?、模糊自適應(yīng)滑模控制、自適應(yīng)神經(jīng)網(wǎng)絡(luò)滑?？刂频萚9-11]。PID控制雖然較簡(jiǎn)單，但對(duì)復(fù)雜的非線(xiàn)性耦合系統(tǒng)的控制效果并不理想；自適應(yīng)滑?？刂坪湍：刂凭哂幸欢ǖ聂敯粜院妥赃m應(yīng)性，但前者的輸出易出現(xiàn)抖振，后者模糊規(guī)則的選擇缺乏系統(tǒng)性，且無(wú)法在線(xiàn)調(diào)整；神經(jīng)網(wǎng)絡(luò)控制有較好的魯棒性，但控制算法復(fù)雜，在線(xiàn)學(xué)習(xí)時(shí)間長(zhǎng)；而自抗擾控制不需要依賴(lài)被控對(duì)象的精確模型，仍可對(duì)系統(tǒng)集總擾動(dòng)進(jìn)行精確的估計(jì)補(bǔ)償，且結(jié)構(gòu)簡(jiǎn)單，計(jì)算量小[12-13]，因此在移動(dòng)機(jī)器人領(lǐng)域得到了廣泛應(yīng)用。

本文主要研究了4-Mecanum 輪ODMR 在未知干擾下的軌跡跟蹤問(wèn)題。首先，建立了ODMR 的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型，然后設(shè)計(jì)了改進(jìn)的擴(kuò)張狀態(tài)觀測(cè)器和自抗擾動(dòng)力學(xué)控制器，最后通過(guò)仿真對(duì)比結(jié)果驗(yàn)證了本文控制器的優(yōu)越性。

1 動(dòng)力學(xué)建模

1.1 運(yùn)動(dòng)學(xué)分析

圖1為本文所設(shè)計(jì)的4-Mecanum ODMR，各輪分別由一臺(tái)直流電機(jī)獨(dú)立驅(qū)動(dòng)，結(jié)構(gòu)布局為縱向?qū)ΨQ(chēng)分布，由于各輪速矢量均可被分解至 XM和 Yu方向上，因此只需調(diào)節(jié)4個(gè)輪子的轉(zhuǎn)速方向，就可以在水平面上實(shí)現(xiàn)三自由度的平面運(yùn)動(dòng)。

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

假設(shè)：1）ODMR 在絕對(duì)平坦的地面上運(yùn)動(dòng)，各輪不存在打滑或空轉(zhuǎn)現(xiàn)象；2）ODMR 的重心與自身幾何中心重合。以大地作為廣義坐標(biāo)系O（X,Y），以O(shè)DMR 的幾何中心為原點(diǎn)的移動(dòng)坐標(biāo)系為OM（XM,YM），由此建立ODMR 逆運(yùn)動(dòng)學(xué)方程為[14]：

將式（1）寫(xiě)成正運(yùn)動(dòng)學(xué)矩陣方程為

式中J0為ODMR 的雅克比矩陣。

設(shè)移動(dòng)坐標(biāo)系到廣義坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣為E，則ODMR 在廣義坐標(biāo)系下的位姿為

聯(lián)立式（2）與式（4）得到ODMR 在廣義坐標(biāo)系下的正運(yùn)動(dòng)學(xué)方程為

1.2 動(dòng)力學(xué)分析

ODMR 屬于多剛體系統(tǒng)，可根據(jù)拉格朗日方程描述其動(dòng)力學(xué)模型為[15]

由于機(jī)器人只做平面運(yùn)動(dòng)，選取重心所在平面為零勢(shì)能面，即V =0，可得到拉氏函數(shù)為

式中：K 為總動(dòng)能； m為 機(jī)器人的總質(zhì)量； Jz為機(jī)器人繞 z 軸的轉(zhuǎn)動(dòng)慣量； θ˙z為機(jī)器人繞 z軸旋轉(zhuǎn)的角速度；Jω是各輪繞輪軸的轉(zhuǎn)動(dòng)慣量；速度分量vx=，vy=

聯(lián)立式（2）、式（6）與式（7）可得系統(tǒng)的動(dòng)力學(xué)方程為

2 反步法運(yùn)動(dòng)學(xué)控制器設(shè)計(jì)

設(shè)ODMR 的參考線(xiàn)速度和角速度分別為 vr和ωr，則在廣義坐標(biāo)系中滿(mǎn)足如下速度轉(zhuǎn)換方程：對(duì)式（10）求1階導(dǎo)后聯(lián)立式（11）得到誤差微分方程為：

基于反步法設(shè)計(jì)思想構(gòu)造李雅普諾夫函數(shù)[16]，設(shè)計(jì)如下運(yùn)動(dòng)學(xué)控制律：

式中： K1、K2、 K3為正常數(shù)。

3 自抗擾動(dòng)力學(xué)控制器設(shè)計(jì)

由于運(yùn)動(dòng)學(xué)控制律輸出的是虛擬速度，ODMR在運(yùn)動(dòng)過(guò)程中受動(dòng)力學(xué)參數(shù)影響更為復(fù)雜，因此基于虛擬速度為參考速度，設(shè)計(jì)動(dòng)力學(xué)控制器可以實(shí)現(xiàn)更好的軌跡跟蹤。首先設(shè)計(jì)擴(kuò)張狀態(tài)觀測(cè)器來(lái)估計(jì)擾動(dòng)值，然后設(shè)計(jì)動(dòng)力學(xué)控制器并實(shí)時(shí)補(bǔ)償擾動(dòng)。

3.1 擴(kuò)張狀態(tài)觀測(cè)器設(shè)計(jì)

在實(shí)際問(wèn)題中會(huì)存在重心偏移、振動(dòng)等不確定因素對(duì)運(yùn)動(dòng)性能產(chǎn)生影響，故觀測(cè)器能否準(zhǔn)確對(duì)未知擾動(dòng)進(jìn)行動(dòng)態(tài)估計(jì)與實(shí)時(shí)反饋是控制器設(shè)計(jì)的關(guān)鍵。由式（8）建立含有不確定因素的動(dòng)力學(xué)方程為

式中：ΔM 為 M的 不確定量； Δμ為μ的不確定量；ΔF 是 F的不確定量； τd為 τ 的未知輸入擾動(dòng)。ξ=(ΔM+Δμ+ΔF-τd)

令，聯(lián)立式（5）、式（14），并整理得到式中： ξ為機(jī)器人運(yùn)動(dòng)過(guò)程中所受不確定變化量和未知輸入擾動(dòng)及未建模干擾項(xiàng)的集總擾，且 ξ有界可導(dǎo)；

根據(jù)式（15），分別定義ODMR 的狀態(tài)變量為x1=q ，x2=， x3=σ(t)=-J(θa)M-1ξ，因此建立系統(tǒng)的狀態(tài)空間方程為：

為了提高觀測(cè)器的估計(jì)能力，采用滑模技術(shù)來(lái)控制觀測(cè)誤差，并根據(jù)誤差大小設(shè)計(jì)可相互切換的線(xiàn)性/非線(xiàn)性擴(kuò)張狀態(tài)觀測(cè)器。傳統(tǒng)的滑模面通常是比例-微分的形式，但易引起抖振，為了削弱這一缺點(diǎn)，引入比例-微分-積分形式的滑模面為[17]

式中： ε1為位姿觀測(cè)誤差，定義為ε1=q-z1；zi為狀態(tài)變量 xi的估計(jì)值。

同時(shí)采用冪次趨近率s˙=-β|s|αsgn s，并改寫(xiě)為

根據(jù)式（16）建立如下滑模線(xiàn)性/非線(xiàn)性切換擴(kuò)張狀態(tài)觀測(cè)器：

式中：ω0為觀測(cè)器寬帶，且ω0＞0；根據(jù)文獻(xiàn)[18]可知：

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

基于無(wú)源性思想的動(dòng)力學(xué)控制器是根據(jù)狀態(tài)反饋來(lái)重建系統(tǒng)能量，并通過(guò)觀測(cè)器與控制器組合來(lái)實(shí)現(xiàn)控制系統(tǒng)的誤差[19]。拉式方程導(dǎo)出的動(dòng)力學(xué)模型已直接定義了從實(shí)際控制輸入 τ到 q˙的被動(dòng)映射，因此改進(jìn)傳統(tǒng)無(wú)源性軌跡跟蹤控制器的設(shè)計(jì)如下

式中：Φ=J(θa)[(θa)++μM-1J(θa)+]；e 為系統(tǒng)控制誤差，e =q-qd；和均 為輔助變量，=-Λe，=z2-Λε2，其中 Λ為 定常矩陣， Λ=diag(ωc,ωc,ωc)，且ωc＞0。

集總擾反饋補(bǔ)償設(shè)計(jì)如下

將集總擾動(dòng)估計(jì)部分并入無(wú)源性控制器中作為整體來(lái)實(shí)現(xiàn)兩者組合，由此得到最終的無(wú)源性動(dòng)力學(xué)控制率為

式中 Kd=diag(ωc,ωc,ωc)。

4 穩(wěn)定性分析

聯(lián)立式（15）與式（23）得閉環(huán)動(dòng)態(tài)跟蹤誤差方程為

式中：Q1=；ν=H(KdQ2+ε3)，ε3為擾動(dòng)觀測(cè)誤差值；Q2=；H=MJ(θa)+；Γ= M(θa)++M-1μJ(θa)+)。

因M 是正定對(duì)稱(chēng)的慣性矩陣，雅克比矩陣是q 的變換矩陣。根據(jù)文獻(xiàn)[20]對(duì)于任意 q ，x ∈Rn滿(mǎn)足λminM||x||2＜xTMx ≤λmaxM||x||2，其中： λmin和λmax分別是最小最大特征值。

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

為驗(yàn)證上述自抗擾軌跡跟蹤控制器（SADRC）的有效性與優(yōu)越性，實(shí)驗(yàn)采用直線(xiàn)、圓形及雙扭線(xiàn)作為參考軌跡并計(jì)算累積絕對(duì)誤差T(exy)、T(eθ)和最大絕對(duì)誤差 exymax、 eθmax來(lái)量化控制器的性能，并與傳統(tǒng)自抗擾控制器（ADRC）作對(duì)比，仿真采樣時(shí)間均設(shè)置為0.001s，模型參數(shù)與控制器參數(shù)如表1所示，最終仿真實(shí)驗(yàn)結(jié)果采用的量化指標(biāo)為：

表1 ODMR 模型參數(shù)與控制器參數(shù)

實(shí)驗(yàn)1采用直線(xiàn)作為參考軌跡：Xd=0．05t，Yd=0．05t ，θd=初始位姿參考位姿將系統(tǒng)集總擾動(dòng)同時(shí)施加在ODMR 的4個(gè)輪子上，即在10 ～20 s間在施加的總擾動(dòng)同時(shí)在20～25 s施加幅值為0.1 m，頻率為5 rad/s的正弦信號(hào)，實(shí)驗(yàn)結(jié)果如圖2～ 圖6所示。

圖2 直線(xiàn)軌跡跟蹤

圖6 SADRC中各輪輸出力矩曲線(xiàn)

圖2 表示兩控制器在直線(xiàn)參考軌跡中的跟蹤效果，其中ADRC的跟蹤曲線(xiàn)有明顯波動(dòng)現(xiàn)象。對(duì)比圖3與圖4的直線(xiàn)跟蹤誤差曲線(xiàn)，SADRC的誤差曲線(xiàn)在起始時(shí)刻無(wú)劇變現(xiàn)象，擾動(dòng)期間誤差曲線(xiàn)平滑且誤差值較小，可見(jiàn)對(duì)擾動(dòng)有抑制作用，且收斂快。而ADRC在受到干擾的起始時(shí)刻，因集總擾動(dòng)的幅值或其導(dǎo)數(shù)變化較大，導(dǎo)致其系統(tǒng)性能急劇下降而出現(xiàn)較大的誤差峰值，且控制欠平穩(wěn)。圖5中可見(jiàn)改進(jìn)后的觀測(cè)器對(duì)集總擾估計(jì)響應(yīng)速度快，其估計(jì)曲線(xiàn)基本與實(shí)際輸入擾動(dòng)相擬合，觀測(cè)值始終沒(méi)有明顯的誤差，而ADRC于擾動(dòng)起始時(shí)刻均存在明顯誤差，尤其在20 s 時(shí)刻因固定增益而導(dǎo)致出現(xiàn)峰值現(xiàn)象。圖6中力矩曲線(xiàn)符合了機(jī)器人沿直線(xiàn)以不變角度移動(dòng)時(shí)的Mecanum 輪運(yùn)動(dòng)特性，且受擾過(guò)程可實(shí)現(xiàn)實(shí)時(shí)補(bǔ)償力矩。表2中的評(píng)價(jià)指標(biāo)中，兩控制器的eθmax相差并不大，嚴(yán)格來(lái)說(shuō)SADRC 稍遜于ADRC，但SADRC在整個(gè)跟蹤過(guò)程的T(exy)和 T(eθ)以及exymax均優(yōu)于ADRC。綜上，SADRC在直線(xiàn)跟蹤過(guò)程 中的抗擾能力較強(qiáng)，魯棒性更好。

圖3 ADRC直線(xiàn)跟蹤誤差曲線(xiàn)

圖4 SADRC 直線(xiàn)跟蹤誤差曲線(xiàn)

圖5 集總擾估計(jì)曲線(xiàn)

表2 直線(xiàn)軌跡評(píng)價(jià)指標(biāo)

實(shí)驗(yàn)2采用圓形和雙扭線(xiàn)作為參考軌跡，并選取雙扭線(xiàn)實(shí)驗(yàn)結(jié)果進(jìn)行分析。其中采用的圓形參考軌跡為雙?扭線(xiàn)參考軌跡為：θd=0。初始位姿均參考位姿將系統(tǒng)集總擾動(dòng)施加在ODMR 的第一個(gè)輪子上，即在10～ 20 s間第一個(gè)輪子施加的總擾動(dòng)同時(shí)在20～25 s施加幅值為0.1 m，頻率為5 rad/s的正弦信號(hào)，得出的實(shí)驗(yàn)結(jié)果如圖7～ 圖12所示。

圖7 圓形軌跡跟蹤

圖12 SADRC中各輪輸出力矩

由圖7和圖8看出，兩控制器在受到相同擾動(dòng)過(guò)程中，SADRC均無(wú)明顯偏離參考軌跡的現(xiàn)象，可見(jiàn)其總體動(dòng)態(tài)跟蹤效果較好。圖9和圖10對(duì)比，ADRC的響應(yīng)速度略慢，穩(wěn)態(tài)誤差較大，受擾動(dòng)期間的橫縱位姿誤差曲線(xiàn)峰值較大且波動(dòng)較為明顯。相反，SADRC 收斂速度快，擾動(dòng)過(guò)程的橫縱誤差曲線(xiàn)無(wú)明顯震蕩現(xiàn)象，誤差峰值較小，克服了ADRC受擾過(guò)程因增益不變而引起性能急劇下降的缺點(diǎn)。圖11中觀測(cè)誤差曲線(xiàn)說(shuō)明了改進(jìn)后的觀測(cè)器與所設(shè)計(jì)的滑模函數(shù)結(jié)合后響應(yīng)速度靈敏，估計(jì)精度高，且觀測(cè)器在帶寬值較小的情況下十分穩(wěn)定，傳統(tǒng)滑?？刂频亩墩瘳F(xiàn)象不明顯。對(duì)比表3中的評(píng)價(jià)指標(biāo)，兩控制器的eθmax仍然相差不大，SADRC在擾動(dòng)跟蹤過(guò)程中的T(exy)和exymax均優(yōu)于ADRC。但在整個(gè)跟蹤過(guò)程中，SADRC在T(eθ)上稍差于ADRC。圖12中力矩曲線(xiàn)體現(xiàn)了機(jī)器人沿雙扭線(xiàn)軌跡移動(dòng)時(shí)的各輪輸入虛擬力矩，因其以恒定角度進(jìn)行平移而需要不斷調(diào)整四輪轉(zhuǎn)速，故在無(wú)擾動(dòng)期間呈波形曲線(xiàn)，符合其運(yùn)動(dòng)學(xué)特性。綜上，在雙扭線(xiàn)跟蹤控制中，改進(jìn)后的控制器僅在角度位姿控制上欠佳，其余指標(biāo)均優(yōu)于傳統(tǒng)控制器，在一定程度上抑制了誤差峰值，體現(xiàn)了SADRC整體跟蹤能力有更好的抗干擾性和魯棒性。

圖8 雙扭線(xiàn)軌跡跟蹤

圖9 ADRC雙扭線(xiàn)跟蹤誤差曲線(xiàn)

圖10 SADRC雙扭線(xiàn)跟蹤誤差曲線(xiàn)

圖11 SADRC雙扭線(xiàn)位姿觀測(cè)誤差曲線(xiàn)

表3 雙扭線(xiàn)軌跡評(píng)價(jià)指標(biāo)

6 結(jié)論

本文針對(duì)4-Mecanum 輪全向移動(dòng)機(jī)器人在實(shí)際應(yīng)用中，存在負(fù)載變化導(dǎo)致的重心偏移，復(fù)雜路況引起的滑移、振動(dòng)及其他未知輸入干擾等不確定因素，基于反步法與無(wú)源性思想分別設(shè)計(jì)了運(yùn)動(dòng)學(xué)控制器和自抗擾動(dòng)力學(xué)控制器，并通過(guò)改進(jìn)擴(kuò)張狀態(tài)觀測(cè)器對(duì)不同形式的集總擾動(dòng)進(jìn)行更準(zhǔn)確的估計(jì)與補(bǔ)償，仿真結(jié)果證明了本文設(shè)計(jì)的控制器在總體控制效果中有較好的魯棒性和抗擾性。下一步工作將在全向移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)上對(duì)本文設(shè)計(jì)的控制器進(jìn)行驗(yàn)證和完善。