全向輪運(yùn)動(dòng)平臺(tái)分析

1、全輪轉(zhuǎn)向式小車一、坐標(biāo)系與位置表示圖1 地理坐標(biāo)系與體坐標(biāo)系定義如圖所示的坐標(biāo)系，地理坐標(biāo)系XI，YI,體坐標(biāo)系XR，YR，坐標(biāo)之間夾角為，P點(diǎn)位置描述為I=xy由地理坐標(biāo)轉(zhuǎn)為體坐標(biāo)的映射由正交旋轉(zhuǎn)矩陣完成R=RI=cossin0-sincos0001xy反方向變換矩陣如下R-1=cos-sin0sincos0001二、運(yùn)動(dòng)學(xué)模型與控制律2.1全向輪直角坐標(biāo)運(yùn)動(dòng)學(xué)方程圖2 軌跡跟蹤示意圖坐標(biāo)系參照?qǐng)D2，對(duì)于地理坐標(biāo)中的位置指令pI=(xr yr r)和速度指令qI=(vr r)將對(duì)應(yīng)的誤差在體坐標(biāo)系中表示出來pR=xeyee=cossin0-sincos0001xr-xyr-yr-對(duì)上式求導(dǎo)的

2、到1: xe=xr-xcos-xr-xsin+yr-ysin+(yr-y)cos =ye-xcos+ysin+vrcosrcos+vrsinrsin =ye-vx+vrcosr- =ye-vx+vrcose ye=-xr-xsin-xr-xcos+yr-ycos-(yr-y)sin=-xe+xsin-ycos-vrcosrsin+vrsinrcos=-xe-vy+vrsine將上式合并寫出得到位置誤差微分方程pR=xeyee=ye-vx+vrcose-xe-vy+vrsiner-2.1.1全向輪直角坐標(biāo)下控制律設(shè)計(jì)設(shè)李雅普諾夫函數(shù)為V1=12xe2+ye2+e2求其導(dǎo)數(shù)如下，當(dāng)漸進(jìn)穩(wěn)定時(shí)導(dǎo)數(shù)小

3、于0；V1=xexe+yeye+eexe=-kxxe,ye=-kyye,e=-ke上式系數(shù)為正時(shí)，李雅普諾夫函數(shù)的導(dǎo)數(shù)小于零，系統(tǒng)漸進(jìn)穩(wěn)定代入微分方程得到控制律如下：vx=ye+vrcose+kxxevy=-xe+vrsine+kyye=r+ke2.2差動(dòng)輪直角坐標(biāo)運(yùn)動(dòng)學(xué)方程差動(dòng)輪與全向輪的區(qū)別是，全向輪小車速度方向與四個(gè)輪子的共同朝向相同可為任意方向，而差動(dòng)輪小車的切向速度方向與X軸重合，故方程中vy=0,微分方程如下：pR=xeyee=ye-v+vrcose-xe+vrsiner-2.2.1差動(dòng)輪直角坐標(biāo)下控制律設(shè)計(jì)選擇Lyapunov函數(shù)如下：V2=12xe2+ye2+1k(1-cose

4、)對(duì)上式沿求導(dǎo)： V2=xexe+yeye+1kesine=xeye-v+vrcose+ye-xe+vrsine+1kr-sine =-xev+xevrcose+yevrsine+1krsine-1ksine=-xev+xevrcose+yevrsine+1krsine-1ksine選擇如下速度控制輸入：v=vrcose+kxxe =r+vr(kye+ksine)將上式代入Lyapunov函數(shù)導(dǎo)數(shù)得到：V2=-kxxe2-kkvrsin2e當(dāng)上式系數(shù)為正時(shí)，V20，故以上Lyapunov函數(shù)選擇正確。由此得到基于運(yùn)動(dòng)學(xué)模型的軌跡跟蹤速度控制律為2：v=vrcose+kxxer+vr(kye+k

5、sine)其中，k，kx，k為控制器參數(shù)。2.2.2控制器參數(shù)選取將控制律代入微分方程得下式：pR=xeyee=ye(r+vr(kye+ksine)-kxxe-xe(r+vr(kye+ksine)+vrsine-vr(kye+ksine)上式在零點(diǎn)附近線性化，忽略高次項(xiàng)得pR=ApRA=-kxr0-r0vr0-vrky-vrk系數(shù)值與角速度和速度指令值共同決定系統(tǒng)根，當(dāng)系數(shù)為正是所有根為負(fù)數(shù)。2.3對(duì)比仿真與結(jié)果仿真系統(tǒng)結(jié)果圖如下：圖3 軌跡跟蹤結(jié)構(gòu)圖圖中q=(v )T，v、分別為移動(dòng)機(jī)器人的線速度和角速度，I=(x y ) T,對(duì)于差動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)方程可表示為：I=xy=cos0sin001

6、v=Jqc圖中J=cos0sin001；pR=xeyee；qc=q；對(duì)于全向輪機(jī)器人運(yùn)動(dòng)學(xué)方程可表示為：xy=cos-sin0sincos0001vxvy=R-1vxvy對(duì)角速度為0.2和線速度為5的圓形軌跡進(jìn)行跟蹤，仿真結(jié)果如下圖：圖4 圓形軌跡跟蹤仿真圖圖中×點(diǎn)線為差動(dòng)輪跟蹤軌跡，點(diǎn)線為全向輪跟蹤軌跡。三、全向輪平臺(tái)的設(shè)計(jì)對(duì)全向輪采用如下圖所示的結(jié)構(gòu)時(shí)，進(jìn)行系統(tǒng)分析與設(shè)計(jì) 圖5 互補(bǔ)型全向輪(omni wheels)3.1運(yùn)動(dòng)學(xué)模型圖6 全向輪式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型移動(dòng)坐標(biāo)Xe-Ye固定在機(jī)器人重心上，而質(zhì)心正好位于幾何中心上。機(jī)器人 P 點(diǎn)在全局坐標(biāo)系的位置坐標(biāo)為：(x ,y,

7、)，三個(gè)全向輪以 3號(hào)輪中心轉(zhuǎn)動(dòng)軸反方向所為機(jī)器人的X軸。假設(shè)三個(gè)全向輪完全相同，三個(gè)全向輪中心到車體中心位置的距離L。在移動(dòng)坐標(biāo)Xe-Ye的速度用vxevye表示。由文獻(xiàn)3可得三個(gè)全向輪的速度與其在移動(dòng)坐標(biāo)和全局坐標(biāo)系下的速度分量之間的關(guān)系分別為以下二式：V1V2V3=sin(60)cos(60)L-sin(60)cos(60)L0-1Lvxevye=3212L-3212L0-1Lvxevye=Ta33vxevyeV1V2V3=sin(60-)cos(60-)L-sin(60+)cos(60+)Lsin-cosLxy3.2動(dòng)力學(xué)模型在移動(dòng)坐標(biāo)Xe-Ye中，設(shè)機(jī)器人在沿軸Xe和Ye方向上收到的

8、力分別為FXe和FYe第1、2、3號(hào)驅(qū)動(dòng)輪提供給機(jī)器人的驅(qū)動(dòng)力分別為f1、f2、f3，機(jī)器人慣性轉(zhuǎn)矩為M，根據(jù)牛頓第二定律可得到如下的動(dòng)力學(xué)方程：mxemyeI=FXeFYeM=cos30-cos300sin30sin30-1LLLf1f2f3=32-3201212-1LLLf1f2f3=Tb33f1f2f3在地理坐標(biāo)系XY下的方程如下：mxmyI=FXFYM=cos(30+)-cos(30-)sinsin(30+)sin(30-)-cosLLLf1f2f33.3基于動(dòng)力學(xué)模型的控制器設(shè)計(jì)如上式所示，基于機(jī)器人動(dòng)力學(xué)模型的控制方案，直接根據(jù)機(jī)器人的動(dòng)力學(xué)模型設(shè)計(jì)運(yùn)動(dòng)控制器，控制器的輸出為機(jī)器人

9、上驅(qū)動(dòng)電機(jī)的驅(qū)動(dòng)電壓。基于動(dòng)力學(xué)模型的控制方案，不需對(duì)驅(qū)動(dòng)電機(jī)進(jìn)行底層的速度控制，消除了底層速度控制帶來的延時(shí)。由動(dòng)力學(xué)方程：mxemyeI=FXFYM=32-3201212-1LLLf1f2f3可知在體坐標(biāo)系中各個(gè)方向上的控制輸入輸出是獨(dú)立的并且相互之間無耦合；于是可在體坐標(biāo)中對(duì)各個(gè)控制量分別進(jìn)行控制。當(dāng)以各個(gè)電機(jī)電壓作為控制量U時(shí)，對(duì)控制量U進(jìn)行矩陣變換Tb33U后可得到各個(gè)方向控制量Fu=Tb33U。先對(duì)系統(tǒng)參數(shù)mI進(jìn)行辨識(shí)，得到由控制量Fu到速度V的傳遞函數(shù)；然后設(shè)計(jì)Fu的控制器，進(jìn)過變換Ta33Fu后得到各個(gè)電機(jī)的控制量U=Ta33Fu 。速度控制指令vxevye 由第2節(jié)控制律求得

10、。3.4基于編碼器的位姿推算圓弧模型在文獻(xiàn)4中介紹機(jī)器人里程計(jì)圓弧模型是把移動(dòng)機(jī)器人在運(yùn)動(dòng)過程中的實(shí)際軌跡通過圓弧去逼近。圖7平臺(tái)樣品示意圖5圖8 采樣期間的圓弧運(yùn)動(dòng)軌跡圖中Axn,yn,n和Bxn+1,yn+1,n+1分別為在采樣時(shí)間間隔內(nèi)起始點(diǎn)與終點(diǎn)的位姿坐標(biāo)，AB為采樣期間的圓弧軌跡，利用圖中幾何關(guān)系可以得到運(yùn)動(dòng)軌跡為圓弧時(shí)的推算公式如下：xn+1=xn+LSR+SL2SR-SLsinn+SR-SLL-sinnyn+1=yn-LSR+SL2SR-SLcosn+SR-SLL-cosnn+1=n+SR-SLL 當(dāng)SR-SL較小時(shí)可采用直線模型xn+1=xn+SR+SL2cosn+SR-SLL

11、yn+1=yn+SR+SL2sinn+SR-SLLn+1=n+SR-SLL 隨著移動(dòng)距離的增加，誤差逐漸加大，其誤差的來源主要包括系統(tǒng)誤差和非系統(tǒng)誤差。系統(tǒng)誤差跟實(shí)際采用的器件的精度和測量上的誤差等方面產(chǎn)生的；非系統(tǒng)誤差是在移動(dòng)過程中隨機(jī)發(fā)生的誤差，主要包括：測位輪子的打滑、路況等。由于非系統(tǒng)誤差不容易消除，因此，這里將通過實(shí)驗(yàn)的方法來校準(zhǔn)機(jī)構(gòu)的安裝精度，減小因系統(tǒng)誤差對(duì)定位精度產(chǎn)生較大影響。影響測量誤差的主要參數(shù)是編碼器輸出一個(gè)脈沖對(duì)應(yīng)輪子運(yùn)動(dòng)的距離r和兩個(gè)定位輪之間的距離L，r和L精度校正的具體方法和實(shí)現(xiàn)步驟如下：編碼器一個(gè)脈沖代表定位輪運(yùn)行的距離r校正方法：使兩個(gè)定位輪在室內(nèi)平面上沿著一

12、條 5 米長度的直線運(yùn)行，編寫軟件程序，對(duì)與定位輪同軸相連的兩個(gè)自由編碼器的輸出脈沖進(jìn)行計(jì)數(shù)，將該數(shù)值記錄左右自由編碼器輸出脈沖個(gè)數(shù)NL和NR，根據(jù)公式r=5000(NL+NR)/2mm求出每次測量計(jì)算得到r的值，再取平均值即可。經(jīng)過多次測試實(shí)驗(yàn)結(jié)果列表。定位輪之間的距離L校正方法：在平地上，使測位裝置從某一起始位置出發(fā)，順時(shí)針或逆時(shí)針旋轉(zhuǎn)n周后再回到該出發(fā)位置，記下在該過程與左右定位輪相連的編碼器輸出的脈沖數(shù)分別為NL，NR，根據(jù)公式L=NL-NR*r2n求出每次測量計(jì)算得到 L的值，再取平均值即可。經(jīng)過多次測試兩定位輪之間距離 L校正實(shí)驗(yàn)列表。四、Mecanum輪平臺(tái)的設(shè)計(jì)Mecanum輪

13、采用滾輪與軸線成45。夾角的結(jié)構(gòu)，如下圖所示： 圖9麥克納姆輪(Mecanum wheels)假設(shè)圖中小輥?zhàn)涌裳貜较蜃杂蓾L動(dòng)，而沿軸向與地面無滑動(dòng)。4.1運(yùn)動(dòng)學(xué)模型圖10 麥克納姆輪移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)原理圖由文獻(xiàn)6可得四個(gè)輪的速度與其在移動(dòng)坐標(biāo)系下的速度分量之間的關(guān)系為以下式，其中W為車半寬，L為車半長：V1V2V3V4=11L+W1-1-L-W111-1-L-WL+Wvxevye=Ta43vxevye其逆變換如下：vxevye=1411111-11-11L+W-1L+W-1L+W1L+WV1V2V3V4= Tb34V1V2V3V4Tb34Ta43=I4.2動(dòng)力學(xué)模型在移動(dòng)坐標(biāo)Xe-Ye中，設(shè)機(jī)

14、器人在沿軸Xe和Ye方向上收到的力分別為FXe和FYe第1、2、3和4號(hào)驅(qū)動(dòng)輪提供給機(jī)器人的驅(qū)動(dòng)力分別為f1、f2、f3和f4，機(jī)器人慣性轉(zhuǎn)矩為M，根據(jù)牛頓第二定律可得到如下的動(dòng)力學(xué)方程：mxemyeI=FXeFYeM=1411111-11-11L+W-1L+W-1L+W1L+Wf1f2f3f4=Tb34f1f2f3f4上式驅(qū)動(dòng)力逆變換為f1f2f3f4=11L+W1-1-L-W111-1-L-WL+WFXeFYeM=Ta43FXeFYeM4.3基于動(dòng)力學(xué)模型的控制器設(shè)計(jì)如上式所示，基于機(jī)器人動(dòng)力學(xué)模型的控制方案，直接根據(jù)機(jī)器人的動(dòng)力學(xué)模型設(shè)計(jì)運(yùn)動(dòng)控制器，控制器的輸出為機(jī)器人上驅(qū)動(dòng)電機(jī)的驅(qū)動(dòng)電

15、壓?；趧?dòng)力學(xué)模型的控制方案，不需對(duì)驅(qū)動(dòng)電機(jī)進(jìn)行底層的速度控制，消除了底層速度控制帶來的延時(shí)。由動(dòng)力學(xué)方程：mxemyeI=FXeFYeM=1411111-11-11L+W-1L+W-1L+W1L+Wf1f2f3f4可知在體坐標(biāo)系中各個(gè)方向上的控制輸入輸出是獨(dú)立的并且相互之間無耦合；于是可在體坐標(biāo)中對(duì)各個(gè)控制量分別進(jìn)行控制。當(dāng)以各個(gè)電機(jī)電壓作為控制量U時(shí)，對(duì)控制量U進(jìn)行矩陣變換Tb34U后可得到在體坐標(biāo)系中的各個(gè)方向控制量Fu=Tb34U。先對(duì)系統(tǒng)參數(shù)mI進(jìn)行辨識(shí)，得到由控制量Fu到體坐標(biāo)速度V的傳遞函數(shù)；然后設(shè)計(jì)Fu的控制器，進(jìn)過變換Ta43Fu后得到各個(gè)電機(jī)的控制量U=Ta43Fu 。速度

16、控制指令vxevye 由第2節(jié)控制律求得。4.4基于三全向輪編碼器的位姿推算圓弧模型在文獻(xiàn)5中介紹機(jī)器人里程計(jì)圓弧模型是把移動(dòng)機(jī)器人在運(yùn)動(dòng)過程中的實(shí)際軌跡通過圓弧去逼近。圖7平臺(tái)樣品示意圖6圖8 采樣期間的圓弧運(yùn)動(dòng)軌跡由于三輪全向機(jī)器人具有完整約束動(dòng)力學(xué)特性23-25，因此采用三個(gè)編碼器才可以得到機(jī)器人平面運(yùn)動(dòng)的三自由度信息，即機(jī)器人的坐標(biāo)信息PcOccycA)。圖中Axn,yn,n和Bxn+1,yn+1,n+1分別為在采樣時(shí)間間隔內(nèi)起始點(diǎn)與終點(diǎn)的位姿坐標(biāo)，AB為采樣期間的圓弧軌跡，利用圖中幾何關(guān)系可以得到運(yùn)動(dòng)軌跡為圓弧時(shí)的推算公式如下：xn+1=xn+LSR+SL2SR-SLsinn+SR-

17、SLL-sinnyn+1=yn-LSR+SL2SR-SLcosn+SR-SLL-cosnn+1=n+SR-SLL 當(dāng)SR-SL較小時(shí)可采用直線模型xn+1=xn+SR+SL2cosn+SR-SLLyn+1=yn+SR+SL2sinn+SR-SLLn+1=n+SR-SLL 隨著移動(dòng)距離的增加，誤差逐漸加大，其誤差的來源主要包括系統(tǒng)誤差和非系統(tǒng)誤差。系統(tǒng)誤差跟實(shí)際采用的器件的精度和測量上的誤差等方面產(chǎn)生的；非系統(tǒng)誤差是在移動(dòng)過程中隨機(jī)發(fā)生的誤差，主要包括：測位輪子的打滑、路況等。由于非系統(tǒng)誤差不容易消除，因此，這里將通過實(shí)驗(yàn)的方法來校準(zhǔn)機(jī)構(gòu)的安裝精度，減小因系統(tǒng)誤差對(duì)定位精度產(chǎn)生較大影響。影響測量

18、誤差的主要參數(shù)是編碼器輸出一個(gè)脈沖對(duì)應(yīng)輪子運(yùn)動(dòng)的距離r和兩個(gè)定位輪之間的距離L，r和L精度校正的具體方法和實(shí)現(xiàn)步驟如下：編碼器一個(gè)脈沖代表定位輪運(yùn)行的距離r校正方法：使兩個(gè)定位輪在室內(nèi)平面上沿著一條 5 米長度的直線運(yùn)行，編寫軟件程序，對(duì)與定位輪同軸相連的兩個(gè)自由編碼器的輸出脈沖進(jìn)行計(jì)數(shù)，將該數(shù)值記錄左右自由編碼器輸出脈沖個(gè)數(shù)NL和NR，根據(jù)公式r=5000(NL+NR)/2mm求出每次測量計(jì)算得到r的值，再取平均值即可。經(jīng)過多次測試實(shí)驗(yàn)結(jié)果列表。定位輪之間的距離L校正方法：在平地上，使測位裝置從某一起始位置出發(fā)，順時(shí)針或逆時(shí)針旋轉(zhuǎn)n周后再回到該出發(fā)位置，記下在該過程與左右定位輪相連的編碼器輸出的脈沖數(shù)分別為NL，NR，根據(jù)公式L=NL-NR*r2n求出每次測量計(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 anautonomous