全輪轉(zhuǎn)向式小車、坐標(biāo)系與位置表示I>A X)圖1地理坐標(biāo)系與體坐標(biāo)系定義如圖所示的坐標(biāo)系，地理坐標(biāo)系&, hh體坐標(biāo)系Xy Yr,坐標(biāo)之間 夾角為0, P點(diǎn)位置描述為rX-i山地理坐標(biāo)轉(zhuǎn)為體坐標(biāo)的映射山正交旋轉(zhuǎn)矩陣完成ER = R 5 =反方向變換矩陣如下R7 = cosO-sine .0sinOcosO0001.cosO-sineO'sinOcosO0001.知二yx-y3二、運(yùn)動學(xué)模型與控制律2.1 全向輪直角坐標(biāo)運(yùn)動學(xué)方程實(shí)際位姿V圖2軌跡跟蹤示意圖坐標(biāo)系參照圖2,對于地理坐標(biāo)中的位置指令廠 的誤差在體坐標(biāo)系中表不出來二(Xryr0r)和速度指令卬=(Vr將對應(yīng)/Xe屬=%A/對上式求導(dǎo)的到：(cosO sinO 0 /xr x-sinO cosO 0 7r - y 00 1/ 0r - OJXe = (Xr x)cosO - (%r x)sinOO + (y- yAsinO + (y r - y)cosOO =y eO) - (xcosO + ysinO) +VrCosOrCosO + vsinOsinO =y eO) -Vx + v r cos( e 廠 - 0) =y ea) -Vx + v r cos Oeye = -(x r - x)sinO - (xr - x)cos0O + (yr - y)cosO 一( % - y)sin60 = -x &a) + (xsinO -ycosO) - VrcosOrsinO + VrsinOrcosO =-xe o)-Vy + Vr sin Oe將上式合并寫出得到位置誤差微分方程yeo) - Vx + Vr cos0e-xea)-Vy + Vr sin Oe0)L(JL)2.1.1 全向輪宜角坐標(biāo)下控制律設(shè)計(jì)設(shè)雅普諾夫函數(shù)為% = *( 龍 + % + %)求其導(dǎo)數(shù)如下，當(dāng)漸進(jìn)穩(wěn)定時導(dǎo)數(shù)小于0；% = I + y eye + oe6e乂 e =一眼 1j7= -kyy、£ = 0e上式系數(shù)為正時，雅普諾夫函數(shù)的導(dǎo)數(shù)小于零，系統(tǒng)漸進(jìn)穩(wěn)定代入微分方程得到控制律如下：vx = yeo) + vr cos 0e + kxxevy = -x eo) + vr sin 0e + kyyeo) = o) r + k e0e2.2 差動輪直角坐標(biāo)運(yùn)動學(xué)方程差動輪與全向輪的區(qū)別是，全向輪小車速度方向與四個輪子的共同朝向相同為任意方向，而差動輪小車的切向速度方向與x軸重合，故方程中s=o,微分方程如下：y/n?e e g g e ?%yea) -v + vr cos 0e-xea) + 壞 sin 0eo) r 0)2.2.1 差動輪直角坐標(biāo)下控制律設(shè)計(jì)選擇 Lyapunov 函數(shù)如下：P 2 =才顯 + 廿)+ 士(1 一 COS8J對上式沿求導(dǎo)：1 .V2=x exe+y eye + -0 esn0e=尢 - v + vr cos 0g) + ye(-xe(jo + v r sin 0e)+ s)sin&eK11=-x ev + xevr cos 0e + yevr sin 0e + -b ) r sin 0e - -to sin 0eKK11=-XeV + XeVr COS 0e + y eVr sin 0e + -0 ) r sin 0e -a) sin 0e 選擇如下速度控制輸入：v = vr Cos 0e + kxxeo) = o)r + vr(kye + kesinOe)將上式代入Lyapunov 函數(shù)導(dǎo)數(shù)得到：V2 = -kxXA A-VrSin2 0e當(dāng)上式系數(shù)為正時，P 2 S 0,故以上Lyapunov函數(shù)選擇正確。山此得到基于運(yùn)動 學(xué)模型的軌跡跟蹤速度控制律為習(xí):Vr COS 0e + k xXeo) r + Vr(kye + keSinOey其中，k,際，褊為控制器參數(shù)。2.2.2控制器參數(shù)選取將控制律代入微分方程得下式：yeAr + V r(kyePR =+ 島 sine- kxXe-Xe(a) r + V r(kye + k eSinOe) + V r Sin Qe-Vr(kye + k eSinOe)上式在零點(diǎn)附近線性化，忽略高次項(xiàng)得PR= APRkx310a)r0vrk系數(shù)值與角速度和速度指令值共同決定系統(tǒng)根,0 % I-vrke J當(dāng)系數(shù)為正是所有根為負(fù)數(shù)。2.3對比仿真與結(jié)果仿真系統(tǒng)結(jié)果圖如下:圖3軌跡跟蹤結(jié)構(gòu)圖圖中q=(v 3" v. 3分別為移動機(jī)器人的線速度和角速度,丹=(x y 0)T,對于差動機(jī)器人 運(yùn)動學(xué)方程可表示為：cosO =sinO.0O'o =JQC1.cosO 圖中J二sinOO'0 爐旌1.Qc = q;對于全向輪機(jī)器人運(yùn)動學(xué)方程可表示為X- /cosO y =I sinO 6' 0對角速度為0. 2和線速度為5的圓形軌跡進(jìn)行跟蹤，仿真結(jié)果如下圖-sinOcosO1/LQ71=R (6)-1Lg403S30252015105 0-5.20-1001020圖4圓形軌跡跟蹤仿真圖圖中X點(diǎn)線為差動輪跟蹤軌跡，。點(diǎn)線為全向輪跟蹤軌跡。:、全向輪平臺的設(shè)計(jì)對全向輪采用如下圖所示的結(jié)構(gòu)時，進(jìn)行系統(tǒng)分析與設(shè)計(jì)圖5互補(bǔ)型全向輪(onuii wheels)3.1 運(yùn)動學(xué)模型義冒2圖6全向輪式移動機(jī)器人運(yùn)動學(xué)模型移動坐AXe-Ye固定在機(jī)器人重心上，而質(zhì)心正好位于兒何中心上。機(jī)器人 P點(diǎn)在 全局坐標(biāo)系的位置坐標(biāo)為：（xyO）,三個全向輪以3號輪中心轉(zhuǎn)動軸反 方向所為機(jī)器人的X軸。假設(shè)三個全向輪完全相同，三個全向輪中心到車體中心位置的距離L。在移動坐標(biāo) XQ YQ的速度用以。表示。山文獻(xiàn)3可得三個全向輪的速度與其在移動坐標(biāo)和全局坐標(biāo)系下的速度分 之間的關(guān)系分別為以下二式：/14/ sin(60)cos(60)I A2 ) = I -sin(60) cos(60) W3 o -1cos(60 - &) L /x=Ta 3X3Q/V( sin(60 - 0)I A2 I = ( -sin(60 + 0) vj cos(60 + &) L y sine-cose u o'3.2 動力學(xué)模型在移動坐標(biāo)屁-匕中，設(shè)機(jī)器人在沿軸屁和匕方向上收到的力分別為Fxe和FYe第1、2、3號驅(qū)動輪提供給機(jī)器人的驅(qū)動力分別為fl、刈、f3,機(jī)器人慣性轉(zhuǎn) 矩為M,根據(jù)牛頓第二定律可得到如下的動力學(xué)方程：mxem必=10 -LM Jcos(30)sin(30)L-cos(30)sin(30)LA1滄-AJV3V3- 2 1 - 2/ - 1 -2_L 手)/ 2 - Tr/xlELAJ=Tt>3X3( 2E在地理坐標(biāo)系X-Y下的方程如下mxIO-ycos(30 + &) sin(30 + 0)-cos(30 - 0) sin(30 sinO-&)-cosOLL33基于動力學(xué)模型的控制器設(shè)計(jì)如上式所示，基于機(jī)器人動力學(xué)模型的控制方案， 宜接根據(jù)機(jī)器人的動力學(xué) 模型 設(shè)il?運(yùn)動控制器，控制器的輸出為機(jī)器人上驅(qū)動電機(jī)的驅(qū)動電壓?；趧恿W(xué)模型的控制方案，不需對驅(qū)動電機(jī)進(jìn)行底層的速度控制，消除了底層速度控制帶來的延時。山動力學(xué)方程：V3mxe m必10 -可知在體坐標(biāo)系中各個方向上的控制輸入輸出是獨(dú)立的并且相互之間無耦合；于是可在體坐標(biāo)中對各個控制M分別進(jìn)行控制。當(dāng)以各個電機(jī)電壓作為控制M U時，對控制M U進(jìn)行矩陣變換Tb3X3 /后可 得 到各個方向控制M Fi尸Tgy"。先對系統(tǒng)參數(shù)m刀進(jìn)行辨識，得到山控制 i Fu到 速度V的傳遞函數(shù)；然后設(shè)計(jì) Fu的控制器，進(jìn)過變 ma3/3Fu后得至I各個電機(jī)的 控制M U =Ta3X3Fu o速度控制指令唄3由第2節(jié)控制律求得。3.4 基于編碼器的位姿推算圓弧模型在文獻(xiàn)4中介紹機(jī)器人里程訃圓弧模型是把移動機(jī)器人在運(yùn)動過程中的實(shí)際軌跡通過圓弧去逼近。圖7平臺樣品示意圖! W X僦/)X»I%I圖8采樣期間的圓弧運(yùn)動軌跡圖中A(Xn, yn9 0n) NB(X n+p yn+p On+l)分別為在采樣時間間隔起始點(diǎn)與終點(diǎn)的位姿坐標(biāo),L(ASA + ASl)%N+1 = & +2(AS R - ASJY，N+1 yn 2(asa-as l)sr as ；021 =O n + / AS© AS； 叫四 十y_- sinCSr - ASl、 cos On +L- J -COSOnAB為采樣期間的圓弧軌跡，利用圖中兒何關(guān)系可以得到運(yùn)動軌跡 為圓弧時的推算公式如下：當(dāng)ASr -AS A較小時可采用宜線模型?An+1 =+2"嚴(yán)+陰+啦.i n ,、SR AS L '2、Sr AS/ 0+1 = 0 n +-隨著移動距離的增加，誤差逐漸加大，其誤差的來源主要包括系統(tǒng)誤差和非 系統(tǒng) 誤差。系統(tǒng)誤差跟實(shí)際采用的器件的精度和測M上的誤差等方面產(chǎn)生的：非 系統(tǒng)誤 差是在移動過程中隨機(jī)發(fā)生的誤差，主要包括 ：測位輪子的打滑、路況等。山于非系統(tǒng)誤差不容易消除，因此，這里將通過實(shí)驗(yàn)的方法來校準(zhǔn)機(jī)構(gòu)的安 裝精度，減小因系統(tǒng)誤差對定位精度產(chǎn)生較大影響。影響測M誤差的主要參數(shù)是編碼器輸出一個脈沖對應(yīng)輪子運(yùn)動的距離廠和兩個定位輪之間的距離L, r和L精度校正的具體方法和實(shí)現(xiàn)步驟如下：編碼器一個脈沖代表定位輪運(yùn)行的距離廠校正方法：使兩個定位輪在室平面上沿著一條5米長度的宜線運(yùn)行，編寫軟件程序，對與定位輪同軸相連的兩個 自山編碼 器的輸出脈沖進(jìn)行計(jì)數(shù)，將該數(shù)值記錄左右自山編碼器輸出脈沖個數(shù)NL和NR,根據(jù)公式5000r = (Nl+Nr) /2mm求出每次測M計(jì)算得到r的值，再取平均值即可。經(jīng)過多次測試實(shí)驗(yàn)結(jié)果列表。定位輪之間的距離L校正方法：在平地上，使測位裝置從莫一起始位置出發(fā)，順時針或逆時針旋轉(zhuǎn)n周后再回到該出發(fā)位置，記下在該過程與左右定位輪相連的編碼器輸出的脈沖數(shù)分別為 NR,根據(jù)公式(M 必)來rL =27m求出每次測M計(jì)算得到 L的值，再取平均值即可。經(jīng)過多次測試兩定位輪之間距離L校正實(shí)驗(yàn)列表。四、Mecanum輪平臺的設(shè)計(jì)Mecanum輪采用滾輪與軸線成45?夾角的結(jié)構(gòu)，如下圖所示圖9麥克納姆輪(Mecanum wheels)假設(shè)圖中小翟子可沿徑向自山滾動，而沿軸向與地面無滑動。4.1 運(yùn)動學(xué)模型圖10麥克納姆輪移動機(jī)器人運(yùn)動學(xué)原理圖山文獻(xiàn)6可得四個輪的速度與其在移動坐標(biāo)系下的速度分M之間的關(guān)系為 下式，其中W為車半寬，L為車半長：(%,=Ta4x3(Vye b)其逆變換如下Tb34XTa4 <3=1股4/ |4.2 動力學(xué)模型在移動坐標(biāo)X。-匕中，設(shè)機(jī)器人在沿軸/和匕萬向上收到的力分別為Fxe和FYe第1.2、3和4號驅(qū)動輪提供給機(jī)器人的驅(qū)動力分別為fl、fl. f3和f4,機(jī) 器人慣性轉(zhuǎn)矩為M,根據(jù)牛頓第二定律可得到如下的動力學(xué)方程：事 mx，嘰-10 -Fxe FYeM .111L + W11-11f2-1-1L + W L + WA-=Tb 3X4上式驅(qū)動力逆變換為/11RL4JL + W -L- W -L-W L + W 'LM J=Ta4X3% 尸丫LM J模學(xué)模 帶來的 mye 10lTw1-11L + W1 1-1il4.3 基于動力學(xué)模型的控制器設(shè)計(jì)如上式所示，基于機(jī)器人動力學(xué)模型的控制方案，宜接根據(jù)機(jī)器人的動力學(xué) 型設(shè)計(jì)運(yùn)動控制器，控制器的輸出為機(jī)器人上驅(qū)動電機(jī)的驅(qū)動電壓。基于動力消除了底層速度控制型的控制方案，不需對驅(qū)動電機(jī)進(jìn)行底層的速度控制, 延時。由動力學(xué)方程：合;可知在體坐標(biāo)系中各個方向上的控制輸入輸出是獨(dú)立的并且相互之間無耦 于是可在體坐標(biāo)中對各個控制量分別進(jìn)行控制。當(dāng)以各個電機(jī)電壓作為控制M U時，對控制M U進(jìn)行矩陣變換Tb3X4(7后可得到在體坐標(biāo)系中的各個方向控制MFu=Tb3X4(/o先對系統(tǒng)參數(shù)m進(jìn)行辨識，得到111控制M Fu到體坐標(biāo)速度V的傳遞函數(shù)：然后設(shè)訃 Fu的控制器，進(jìn)過 變換Ta4X3Fu后得到各個電機(jī)的控制MU =Ta4x?Fu。速度控制指令VxeVye由第2節(jié)控制律求得。4.4 基于三全向輪編碼器的位姿推算圓弧模型在文獻(xiàn)5中介紹機(jī)器人里程計(jì)圓弧模型是把移動機(jī)器人在運(yùn)動過程中的實(shí) 際 軌 跡 通 過 圓 弧 去 逼 近。圖7平臺樣品示意圖Ow圖8采樣期間的圓弧運(yùn)動軌跡由于三輪全向機(jī)器人具有完整約束動力學(xué)特性23-25,因此采用三個編碼器才可以得到機(jī)器人平面運(yùn)動的三自山度信息，即機(jī)器人的坐標(biāo)信息PcOccycA) o圖中A( “n何)和何+1)分別為在采樣時間間隔起始點(diǎn)與終點(diǎn)的位姿坐標(biāo)，AB為采樣期間的圓弧軌跡，利用圖中兒何關(guān)系可以得到運(yùn)動軌跡 為圓弧時的推算公式如下：sin 億+魚冷-s 呃(* 4 - SlXN+1=%N2(ASR -ASJ . ,=,L(AS R + ASJ YN+1% 2(ASA-cos-cosOnAS l).ASn ASr On + 1 = 0 n +-當(dāng)ASR -ASe較小時可采用宜線模型=foj + 2cos 0 n +-SR + ASJ . ( 'SR - SL'次田=% + -2-sm 嚴(yán) + L Sr SlVn+1 = %+隨著移動距離的增加，誤差逐漸加大，其誤差的來源主要包括系統(tǒng)誤差和非 系統(tǒng) 誤差。系統(tǒng)誤差跟實(shí)際采用的器件的精度和測M上的誤差等方面產(chǎn)生的：非 系統(tǒng)誤 差是在移動過程中隨機(jī)發(fā)生的誤差，主要包括 ：測位輪子的打滑、路況等。山于非系統(tǒng)誤差不容易消除， 因此，這里將通過實(shí)驗(yàn)的方法來校準(zhǔn)機(jī)構(gòu)的安裝精度，減小因系統(tǒng)誤差對定位精度產(chǎn)生較大影響。影響測M誤差的主要參數(shù)是編碼器輸出一個脈沖對應(yīng)輪子運(yùn)動的距離廠和兩個定位輪之間的距離L, r和L精度校正的具體方法和實(shí)現(xiàn)步驟如下：編碼器一個脈沖代表定位輪運(yùn)行的距離廠校正方法：使兩個定位輪在室平面上沿著一條5米長度的宜線運(yùn)行，編寫軟件程序，對與定位輪同軸相連的兩個 自山編碼 器的輸出脈沖進(jìn)行計(jì)數(shù)，將該數(shù)值記錄左右自山編碼器輸出脈沖個數(shù)NL和NR,根據(jù)公式50007'=(皿+必)/2吶求出每次測M計(jì)算得到廠的值，再取平均值即可。經(jīng)過多次測試實(shí)驗(yàn)結(jié)果列 表。定位輪之間的距離L校正方法：在平地上，使測位裝置從莫一起始位置出發(fā)， 順時針或逆時針旋轉(zhuǎn)n周后再回到該出發(fā)位置，記下在該過程與左右定位輪相連的編碼器輸出的脈沖數(shù)分別為 NR,根據(jù)公式(M -Sr2m求出每次測量計(jì)算得到L 的值，再取平均值即可。經(jīng)過多次測試兩定位輪之間距離 L 校正實(shí)驗(yàn)列表。參考文獻(xiàn)：1 金琨?滑模變結(jié)構(gòu)控制MATLAB 仿真 M. ： 清華大學(xué)，2005.2 Y. Kanayama? Y. Kimura, F. Miyazaki. T. Noguchi. A stable tracking control method for an autonomous mobile robotC. Proceedings 1990 IEEE International Conference on Robotics and Automation, Cincinnati, OH. 1990 、 384-389.3 志誠 . 三輪全向移動機(jī)器人運(yùn)動控制研究D.: 理工大學(xué),2010.4 于金霞，王璐，蔡自興. 未知環(huán)境中移動機(jī)器人自定位技術(shù)M.: 電子工業(yè),2011.5 向譽(yù) . 基于碼盤定位的全向移動機(jī)器人運(yùn)動控制系統(tǒng)研究與設(shè)計(jì)D. 市：理工大學(xué) ,2014.6 文軍 .Mecanum 輪移動機(jī)器人循跡控制及其調(diào)度研究D. 市 : 東南大學(xué),2014.