王雙雙,張?jiān)ツ希鹾驮?,?鵬,李瀚飛
(1.裝甲兵工程學(xué)院,北京100072;2.浙江信陽(yáng)實(shí)業(yè)有限公司,浙江金華321025;3.66222部隊(duì),北京102202)
全方位移動(dòng)機(jī)構(gòu)是機(jī)器人領(lǐng)域的研究方向之一[1]。相比于傳統(tǒng)的輪式移動(dòng)機(jī)構(gòu),全方位移動(dòng)機(jī)構(gòu)具有平面內(nèi)完全的三個(gè)運(yùn)動(dòng)自由度,即沿X、Y方向的平動(dòng)和繞Z軸的轉(zhuǎn)動(dòng)??梢詫?shí)現(xiàn)平面上任意方向的平動(dòng)或繞任意點(diǎn)進(jìn)行任意半徑的轉(zhuǎn)向運(yùn)動(dòng),具有非常靈活的機(jī)動(dòng)性能,非常適用于狹窄空間和高精度軌跡跟蹤的場(chǎng)合,在航空航天、軍事安全[2]、倉(cāng)儲(chǔ)運(yùn)輸和社會(huì)服務(wù)等領(lǐng)域具有廣泛的用途。
全方位移動(dòng)技術(shù)在輪組結(jié)構(gòu)、運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)、運(yùn)動(dòng)規(guī)劃、運(yùn)動(dòng)控制等方面一直受到國(guó)內(nèi)外眾多學(xué)者的關(guān)注,并進(jìn)行了相關(guān)內(nèi)容的研究。文獻(xiàn)[3]分析給出了一般結(jié)構(gòu)形式的Mecanum四輪系統(tǒng)的運(yùn)動(dòng)學(xué)模型,優(yōu)選出四輪全方位運(yùn)動(dòng)系統(tǒng)的最佳結(jié)構(gòu)布局形式;文獻(xiàn)[4]根據(jù)工作環(huán)境和任務(wù)要求的不同,研究了全方位移動(dòng)機(jī)器人的自主、尋跡和遙控等3種控制方式;文獻(xiàn)[5]利用模型參考模糊自適應(yīng)控制方法構(gòu)建了運(yùn)動(dòng)學(xué)位姿環(huán)控制器,結(jié)合動(dòng)力學(xué)PID控制,形成全方位機(jī)器人的位姿閉環(huán)控制系統(tǒng);文獻(xiàn)[6]采用多傳感器融合技術(shù),組成智能式模糊邏輯邏輯控制系統(tǒng),使全方位移動(dòng)機(jī)器人具有自主導(dǎo)航和運(yùn)動(dòng)控制的功能;文獻(xiàn)[7]通過(guò)外置CCD攝像頭,對(duì)全方位機(jī)器人的位姿進(jìn)行實(shí)時(shí)監(jiān)測(cè),實(shí)現(xiàn)了運(yùn)動(dòng)過(guò)程中位姿的校正控制;文獻(xiàn)[8]使用采用模糊方位角估計(jì)器,對(duì)全方位機(jī)器人進(jìn)行跟蹤控制;文獻(xiàn)[9]將摩擦影響和動(dòng)力學(xué)考慮在內(nèi),描述了時(shí)間最優(yōu)軌跡算法,實(shí)現(xiàn)了全方位足球機(jī)器人的路徑生成和運(yùn)動(dòng)控制。
滑模變結(jié)構(gòu)控制本質(zhì)上是一類特殊的非線性控制方法,具有對(duì)系統(tǒng)的非線性、不確定因素和外界干擾的不變性,在工程實(shí)踐中得到了廣泛的應(yīng)用。但滑模控制的缺點(diǎn)是控制律固有的抖振現(xiàn)象,會(huì)造成系統(tǒng)的不穩(wěn)定[10]。
本文通過(guò)建立Mecanum輪型全方位移動(dòng)平臺(tái)的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型,針對(duì)全方位輪的滑移和平臺(tái)的重心偏移等非線性和不確定因素,利用多體動(dòng)力學(xué)軟件RecurDyn建立了平臺(tái)的虛擬樣機(jī)模型;設(shè)計(jì)了基于等效控制的滑??刂破?,通過(guò)模糊切換增益,降低了輸出抖振;采用虛擬樣機(jī)模擬重心不斷變化的情況,通過(guò)聯(lián)合仿真實(shí)現(xiàn)了良好的軌跡跟蹤效果,為實(shí)際全方位移動(dòng)平臺(tái)的運(yùn)動(dòng)控制研究提供了有效的方法。
本文研究的Mecanum輪型全方位移動(dòng)平臺(tái)如圖1所示,該平臺(tái)采用四輪縱向?qū)ΨQ式布局結(jié)構(gòu),斜向的兩個(gè)輪子是相同的,而兩側(cè)的輪子是鏡像對(duì)稱的,輥?zhàn)拥钠媒菫椤?5°。
對(duì)全方位移動(dòng)平臺(tái)的定義如圖2所示,為了便于建立運(yùn)動(dòng)學(xué)模型,設(shè)定假設(shè)條件如下:
(1)全方位移動(dòng)平臺(tái)在理想的平面上運(yùn)動(dòng),車體、輪子和路面均為剛體。
(2)輪子與路面之間具有足夠大的摩擦力,運(yùn)動(dòng)過(guò)程中不會(huì)發(fā)生輪子的打滑現(xiàn)象。
(3)平臺(tái)的重心與平面幾何中心重合。
在此輪組布局結(jié)構(gòu)下,通過(guò)對(duì)輪子的運(yùn)動(dòng)解析,得到平臺(tái)的逆向運(yùn)動(dòng)學(xué)方程[3]:
對(duì)于系統(tǒng)的動(dòng)力學(xué)建模,通常采用動(dòng)力學(xué)基本理論,即牛頓-歐拉方程。這是一種力的動(dòng)態(tài)平衡法,對(duì)于較為復(fù)雜的系統(tǒng),此種方法十分復(fù)雜和麻煩。而拉格朗日力學(xué)不必求出內(nèi)作用力,是一種功能平衡法,適用于復(fù)雜系統(tǒng)的動(dòng)力學(xué)建模[11]。
拉格朗日方程可表示如下:
式中:qj為系統(tǒng)的廣義坐標(biāo);L為拉格朗日函數(shù),等于系統(tǒng)的動(dòng)能減去勢(shì)能;Φ為系統(tǒng)的耗散函數(shù);Qj為系統(tǒng)的廣義力。
由于在平面上運(yùn)動(dòng),全方位移動(dòng)平臺(tái)的勢(shì)能Ep=0,忽略輥?zhàn)拥淖杂尚D(zhuǎn),因此其拉格朗日函數(shù)和耗散函數(shù)分別:
式中:m為平臺(tái)的質(zhì)量;Ek為平臺(tái)的動(dòng)能;Dw為輪子的黏性摩擦系數(shù);Iz和Iw分別為平臺(tái)和輪子的轉(zhuǎn)動(dòng)慣量。
對(duì)于全方位移動(dòng)平臺(tái),定義廣義力為輪子的驅(qū)動(dòng)轉(zhuǎn)矩Ti,廣義坐標(biāo)為輪子的角位移 θi,將式(1)、式(3)、式(4)代入式(2)中,得到拉格朗日動(dòng)力學(xué)方程如下:
其 中: θ = [θ1θ2θ3θ4]T∈ R4,T =[T1T2T3T4]T∈R4,Dw=diag(Dw1,Dw2,Dw3,Dw4);為輪子的角加速度;M為運(yùn)動(dòng)慣量陣,其中的元素是由平臺(tái)的參數(shù)決定的。
因此,全方位移動(dòng)平臺(tái)的動(dòng)力學(xué)狀態(tài)空間模型可表示為:
對(duì)于實(shí)際的全方位移動(dòng)平臺(tái)及其工程應(yīng)用(如全方位叉車和運(yùn)載平臺(tái)),在運(yùn)動(dòng)過(guò)程中會(huì)受到輪子的滑移和平臺(tái)的重心偏移等非線性和不確定因素的影響,而式(6)作為動(dòng)力學(xué)名義模型,不能完全包含上述影響因素。
本文建立了基于多體動(dòng)力學(xué)軟件RecurDyn的全方位移動(dòng)平臺(tái)虛擬樣機(jī)模型。通過(guò)添加輥?zhàn)优c地面之間的面面接觸以及輥?zhàn)拥男D(zhuǎn)約束,模擬平臺(tái)運(yùn)動(dòng)過(guò)程中輪子的滑移問(wèn)題。設(shè)置平臺(tái)上的負(fù)載物塊緩慢運(yùn)動(dòng),使得平臺(tái)重心不斷變化,進(jìn)而模擬平臺(tái)轉(zhuǎn)動(dòng)慣量不確定性和輪子負(fù)載擾動(dòng)的情況,如圖3所示。將虛擬樣機(jī)模型和控制系統(tǒng)模型結(jié)合起來(lái),進(jìn)行聯(lián)合仿真分析,可以為運(yùn)動(dòng)控制策略的研究提供更為有效的驗(yàn)證方法。
圖3 全方位移動(dòng)平臺(tái)虛擬樣機(jī)側(cè)視圖
本文將滑模控制與模糊控制結(jié)合起來(lái),設(shè)計(jì)基于等效控制的模糊滑模控制器,總體結(jié)構(gòu)如圖4所示。由期望位姿和實(shí)際位姿得到位姿誤差及其變化率,利用模糊規(guī)則對(duì)切換控制中的切換增益進(jìn)行有效估計(jì)。在相點(diǎn)接近滑模面時(shí),應(yīng)減小切換增益,以減小相點(diǎn)穿越滑模面時(shí)的速度;在遠(yuǎn)離滑模面的地方,應(yīng)該增大切換增益,以保證系統(tǒng)的快速性。通過(guò)模糊切換增益,減小滑??刂坡傻亩墩瘢玫礁鱾€(gè)輪子的驅(qū)動(dòng)轉(zhuǎn)矩。在運(yùn)動(dòng)過(guò)程中抑制非線性因素、不確定量和外界干擾的影響,進(jìn)而實(shí)現(xiàn)全方位移動(dòng)平臺(tái)精確的軌跡跟蹤控制。
圖4 模糊滑模軌跡跟蹤控制總體結(jié)構(gòu)
以全方位移動(dòng)平臺(tái)的位姿為廣義坐標(biāo),由式(1)和式(6)得到動(dòng)力學(xué)模型:
式中:X=[x y θz]為平臺(tái)的位姿;為 X、Y 方向的線加速度和繞 Z軸的角加速度;dt=[dt1dt2dt3]T為輪子受到的外界干擾。
系統(tǒng)輸入為平臺(tái)的期望位姿 Xr=[xryrθzr],則位姿誤差定義:
設(shè)計(jì)切換函數(shù):
對(duì)于等效控制,不考慮平臺(tái)的外界干擾,可以得到:
定義S=[S1S2S3]T,通過(guò)取=0,得到等效控制律:
式中:C=diag(c11c21c31)。
式中:切換增益 η =diag(η1η2η3),ηi≥(J+M-1dt)i。
結(jié)合式(11)和式(12),得到滑??刂坡蔀?
因此系統(tǒng)是全局漸進(jìn)穩(wěn)定的。
其中,μ(s)=diag(μ1μ2μ3),當(dāng) μi=1 時(shí),此時(shí)控制律為傳統(tǒng)的等效滑??刂?當(dāng)μi≠1時(shí),通過(guò)μi的變化實(shí)現(xiàn)降低抖振的目的。
圖5 模糊控制器結(jié)構(gòu)圖
本文的模糊控制器屬于 Mamdani型二維模糊控制器,結(jié)構(gòu)如圖5所示,輸入變量為和,輸 出變 量 為μi,是切換控制增益 ηi的模糊系數(shù)值,K1、K2是輸入變量的量化因子,K3是輸出變量的比例因子[12]。
采用重心法將模糊輸出清晰化,可以得到模糊控制查詢表。軌跡跟蹤控制時(shí),根據(jù)si和的實(shí)際值,通過(guò)查詢模糊控制表即可得到切換增益模糊系數(shù)值μi,實(shí)現(xiàn)對(duì)切換增益的模糊估計(jì),進(jìn)而可以降低滑??刂频妮敵龆墩瘛?/p>
全方位移平臺(tái)仿真模型的參數(shù):平臺(tái)質(zhì)量m=1 703.5 kg,幾何中心點(diǎn)轉(zhuǎn)動(dòng)慣量 Iz=692.1 kg·m2,單個(gè)輪子轉(zhuǎn)動(dòng)慣量 Iw=1.749 5 kg·m2,輪子半徑 R=0.23 m,l=0.675 m,w=0.525 m。
采用PD控制和模糊滑??刂七M(jìn)行對(duì)比,其中,PD控制參數(shù)Kpi=500,Kdi=400;滑??刂茀?shù)Ci=[10 1]。
對(duì)全方位移動(dòng)平臺(tái)進(jìn)行平動(dòng)的圓軌跡跟蹤仿真分析,圓軌跡公式:
式中:r為圓軌跡的半徑;ωr為圓軌跡的角速度,設(shè)置 r=1,ωr=0.3。
采用全方位平臺(tái)的狀態(tài)空間模型作為被控對(duì)象,考慮系統(tǒng)干擾為高斯函數(shù)的形式,即:
取 Di=100,bi=0.5,ci=10,比較 PD 控制和模糊滑模控制的圓軌跡跟蹤效果,部分仿真結(jié)果如圖6~圖9所示。
由仿真結(jié)果可以得到,采用模糊滑模控制可以基本消除控制量的抖振,在受到擾動(dòng)時(shí),模糊滑??刂频乃俣群臀灰普`差均小于PD控制的誤差,因此具有更高的軌跡跟蹤精度。
采用全方位平臺(tái)的虛擬樣機(jī)模型,結(jié)合上述控制方法,建立Simulink/RecurDyn聯(lián)合仿真模型。在虛擬樣機(jī)模型中,設(shè)置負(fù)載物塊以vL=0.125×sin(0.25t)的速度沿車體縱向中心線做緩慢往復(fù)運(yùn)動(dòng),模擬平臺(tái)重心不斷變化的情況,驗(yàn)證控制方法的效果,結(jié)果如圖10、圖11所示。
由聯(lián)合仿真結(jié)果可知,采用模糊滑模控制的位移誤差小于PD控制位移誤差,且基本趨近于零。在運(yùn)動(dòng)過(guò)程中,消除了重心不斷變化對(duì)姿態(tài)的影響,使平臺(tái)始終保持原姿態(tài),實(shí)現(xiàn)了平動(dòng)的圓軌跡跟蹤。
本文以Mecanum輪型全方位移動(dòng)平臺(tái)為研究對(duì)象,建立了運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型;結(jié)合全方位輪的滑移和平臺(tái)的重心偏移等非線性因素,采用多體動(dòng)力學(xué)分析軟件RecurDyn構(gòu)建了虛擬樣機(jī)模型;設(shè)計(jì)了基于等效控制的模糊滑??刂破?,通過(guò)模糊估計(jì)切換增益,降低了輸出抖振;分別使用解析模型和聯(lián)合仿真模型對(duì)圓軌跡進(jìn)行跟蹤,通過(guò)與PD控制相比較,表明模糊滑??刂凭哂懈玫能壽E跟蹤效果。
[1] 趙冬斌,易建強(qiáng).全方位移動(dòng)機(jī)器人導(dǎo)論[M].北京:科學(xué)出版社,2010.
[2] McGowen H.Navy omni-directional vehicle(ODV)development:where the rubber meets the deck[J].Naval Engineers Journal,2000,112(4):217-228.
[3] 王一治,常德功.Mecanum四輪全方位系統(tǒng)的運(yùn)動(dòng)性能分析及結(jié)構(gòu)形式優(yōu)選[J].機(jī)械工程學(xué)報(bào),2009,45(5):307-310.
[4] 賈茜,王興松,周婧.基于多種控制方式的全方位移動(dòng)機(jī)器人研制[J].電子機(jī)械工程,2010,26(4):61-64.
[5] 吳定會(huì),黃旭東.全方位移動(dòng)機(jī)器人速度的 MRFAC控制[C]//中國(guó)控制與決策學(xué)術(shù)年會(huì)議.2007:401-406.
[6] Kumile C M,Tlale N S.Intelligent distributed fuzzy logic control system(IDFLCS)of a mecanum wheeled autonomous guided vehicle[C]//IEEE International Conference of Mechatronics and Automation.2005,1:131-137.
[7] Viboonchaicheep P,Shimada A,Kosaka Y.Position rectification control for mecanum wheeled omni-directional vehicle[C]//The 29th Annual Conference of the IEEE on Industrial Electronics Society.2003:854-859.
[8] Kim Sangdae,Hyun Changho,Cho Youngwan,et al.Tracking control of 3-wheels omni-directional mobile robot using fuzzy azimuth estimator[C]//Proceedings of the 10th WSEAS International Conference on Robotics,Control And Manufacturing Technology.2010:47-51.
[9] Purwin O,D'Andrea R.Trajectory generation and control for four wheeled omnidirectional vehicles[J].Robotics and Autonomous Systems,2006,54:13-22.
[10] 劉金琨.滑模變結(jié)構(gòu)控制MATLAB仿真[M].北京:清華大學(xué)出版社,2005:1-7.
[11] 蔡自興.機(jī)器人學(xué)[M].北京:清華大學(xué)出版社,2000:89-90.
[12] 石新民,郝整清.模糊控制及其MATLAB仿真[M].北京:清華大學(xué)出版社&北京交通大學(xué)出版社,2008:93-102.