全向輪運動平臺分析

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