三輪全向機(jī)器人原理及matlab仿真

三輪全向機(jī)器人原理及matlab仿真計(jì)算機(jī)輔助設(shè)計(jì)報(bào)告三輪全向移動(dòng)機(jī)器人運(yùn)動(dòng)控制仿真201103009004余楊廣201103009019沈陽(yáng)201103009031陳斌人員分工：余楊廣：總體負(fù)責(zé)，系統(tǒng)理解及控制器設(shè)計(jì)，PPT制作，后期報(bào)告審查及修改陳斌：PPT制作，報(bào)告撰寫沈陽(yáng)：資料收集，輔助其余兩人完成任務(wù)實(shí)驗(yàn)?zāi)康慕⑷喨驒C(jī)器人系統(tǒng)的數(shù)學(xué)模型，然后基于simulink建立該系統(tǒng)的仿真模型并設(shè)計(jì)控制器，最終滿足控制要求；控制的最終目的是使該機(jī)器人能夠良好跟蹤預(yù)期的運(yùn)動(dòng)軌跡；通過(guò)對(duì)復(fù)雜系統(tǒng)的分析、建模、仿真、驗(yàn)證，全面提高利用計(jì)算機(jī)對(duì)復(fù)雜系統(tǒng)進(jìn)行輔助設(shè)計(jì)的能力；通過(guò)集體作業(yè)、分工完成任務(wù)的方式培養(yǎng)團(tuán)隊(duì)意識(shí)，提高團(tuán)隊(duì)集體攻關(guān)能力實(shí)驗(yàn)原理控制對(duì)象——三輪全向機(jī)器人三輪全向移動(dòng)機(jī)器人其驅(qū)動(dòng)輪由三個(gè)全向輪組成，徑向?qū)ΨQ安裝，各輪互成120°角，滾柱垂直于各主輪。三個(gè)全向輪的大小和質(zhì)量完全相同，而且由性能相同的電機(jī)驅(qū)動(dòng)。圖1三輪全向移動(dòng)機(jī)器人2.2控制系統(tǒng)結(jié)構(gòu)圖2基于運(yùn)動(dòng)學(xué)模型的分層控制框圖實(shí)驗(yàn)內(nèi)容3.1電機(jī)模型3.1.1物理建模瑞士的MAXON公司的無(wú)刷直流電機(jī)建模如下:無(wú)刷直流電機(jī)的數(shù)學(xué)模型，其等效電路如下圖所示：根據(jù)上圖，建立電機(jī)數(shù)學(xué)方程如下：瞬態(tài)電壓方程電壓方程轉(zhuǎn)矩方程運(yùn)動(dòng)方程3.1.2Simulink模塊搭建根據(jù)以上數(shù)學(xué)模型，我們搭建電機(jī)的Simulink模塊如下：電壓方程模塊轉(zhuǎn)矩方程模塊運(yùn)動(dòng)方程模塊其他必要模塊設(shè)計(jì)反電動(dòng)勢(shì)模塊邏輯換向模塊模塊組裝——電機(jī)仿真模型驅(qū)動(dòng)電機(jī)模塊封裝3.1.3無(wú)刷直流電機(jī)仿真模型的驗(yàn)證到此電機(jī)的建模就算完成了，但其正確性還需要結(jié)果來(lái)驗(yàn)證，以下是仿真結(jié)果：繞組端電壓波形：反電動(dòng)勢(shì)波形電流波形轉(zhuǎn)速波形根據(jù)圖像可知，仿真結(jié)果跟實(shí)際是相吻合的。3.2運(yùn)動(dòng)學(xué)模型為了實(shí)現(xiàn)現(xiàn)實(shí)世界速度與機(jī)器人三個(gè)電機(jī)轉(zhuǎn)速之間的轉(zhuǎn)換，我們建立起運(yùn)動(dòng)學(xué)模型如下：3.2.1物理建模1.建立如圖所示的世界坐標(biāo)系xoy和機(jī)器人坐標(biāo)系XOY。圖3三輪全向輪式機(jī)器人示意圖圖中，θ為機(jī)器人坐標(biāo)系與世界坐標(biāo)系之間的夾角；φ為驅(qū)動(dòng)輪間的夾角，φ=120；L為機(jī)器人中心到輪子中心的水平距離。2.設(shè)v1，v2，v3為全向輪線速度，vx，vy分別為機(jī)器人在XOY坐標(biāo)系X軸和Y軸的速度分量；ω為機(jī)器人自轉(zhuǎn)的角速度。那么，機(jī)器人在世界坐標(biāo)系中的速度與驅(qū)動(dòng)輪速度之間的關(guān)系為：3.2.2Simulink模塊搭建根據(jù)2.1中的公式，我們搭建出Simulink模塊如下：圖中三個(gè)函數(shù)模塊實(shí)現(xiàn)2.1中的矩陣轉(zhuǎn)換。最終，模塊封裝如下：3.3路徑規(guī)劃路徑規(guī)劃方面，我們利用B樣條曲線規(guī)劃方法，取了幾個(gè)點(diǎn)作為控制點(diǎn)，生成了一條類似S的曲線，來(lái)檢驗(yàn)小車的跟隨情況。Simulink模塊如下：其中，MATLABFCN為核心算法，因代碼較長(zhǎng)，放至附錄部分，在此不一一贅述。模塊中，Vd為待輸入的期望速度值，xs為期望的x軸位置，ys為期望的y軸位置,ths為期望的小車前進(jìn)偏角。最后，曲線生成模塊封裝圖如下圖所示：3.4.傳感器設(shè)計(jì)為了實(shí)現(xiàn)對(duì)小車自身狀態(tài)（包括自身位置信息和朝向信息）的感知，我們?cè)O(shè)計(jì)了一個(gè)傳感器模塊，以便引入反饋。搭建的Simulink模塊如下：其中，v1,v2,v3為三個(gè)電機(jī)的線速度，中間的函數(shù)實(shí)現(xiàn)電機(jī)線速度到真實(shí)世界中Vx,Vy和w的轉(zhuǎn)化。最后，我們將傳感器模塊封裝，封裝圖如下：3.5.控制器設(shè)計(jì)對(duì)于小車的控制，我們認(rèn)為無(wú)非要解決兩個(gè)問(wèn)題，一個(gè)是小車是否受控聽(tīng)話即電機(jī)的控制問(wèn)題；第二個(gè)問(wèn)題是小車是否足夠聰明知道該怎樣走，即運(yùn)動(dòng)學(xué)控制問(wèn)題。下面我們將就這兩個(gè)方面展開(kāi)論述。3.5.1電機(jī)控制器設(shè)計(jì)從電機(jī)模型一節(jié)中可以看出，電機(jī)模型的數(shù)學(xué)公式非常復(fù)雜，推算電壓V和轉(zhuǎn)速w之間的關(guān)系非常的困難，我們推算了很久也沒(méi)有成功。后來(lái)我們想到，不管是交流電動(dòng)機(jī)或者是直流電動(dòng)機(jī)，他們的本質(zhì)都是電動(dòng)機(jī)。而電動(dòng)機(jī)的傳遞函數(shù)都是一階的，對(duì)于一階模型我們利用P控制就能夠?qū)崿F(xiàn)很好的控制，而且也非常好調(diào)試，比例環(huán)節(jié)K的值越大越好，總能夠?qū)崿F(xiàn)較好的動(dòng)態(tài)性能。最終我們出于消除穩(wěn)態(tài)誤差的考慮，采用了PI控制加上前置濾波的控制方式。經(jīng)過(guò)調(diào)試，最終我們的PI控制器為2（S+100）S，前置濾波器為為了檢驗(yàn)我們控制器的效果，我們特意與學(xué)長(zhǎng)的控制器進(jìn)行了對(duì)比，對(duì)比方式為控制下的電機(jī)階躍響應(yīng)性能。對(duì)比圖如下圖所示：學(xué)長(zhǎng)電機(jī)控制的階躍響應(yīng)圖我們的電機(jī)控制階躍響應(yīng)圖從圖中我們可以看出，不管是從調(diào)節(jié)時(shí)間還是從穩(wěn)定性、超調(diào)量來(lái)看，我們的控制器都有著絕對(duì)的優(yōu)越性。3.5.2運(yùn)動(dòng)控制器設(shè)計(jì)對(duì)于運(yùn)動(dòng)控制即位置控制，我們認(rèn)為控制框圖應(yīng)該如下圖所示：為了運(yùn)動(dòng)控制有更深層次的理解，我們建立如下數(shù)學(xué)模型：一個(gè)在真實(shí)坐標(biāo)戲中運(yùn)動(dòng)的小車模型如下圖所示：設(shè)Vu為軸向前進(jìn)速度；θ為小車中軸線與水平線夾角；[x，y]為小車中心位置,L為車中心到小車前端的距離，Vs為速度誤差，d為位置誤差，則根據(jù)上圖可建立以下公式：xy小車在前進(jìn)過(guò)程中，軌跡與預(yù)期的軌跡之間的誤差如下圖所示：其中，要達(dá)到良好的控制效果，設(shè)計(jì)上必須要確定一個(gè)控制目標(biāo)。在設(shè)計(jì)中，我們要達(dá)到的目的有3個(gè)：1.p02.Vs期望速度Vd3.d0因?yàn)槲覀兡軌蚩刂齐姍C(jī)的轉(zhuǎn)速，所以速度控制暫不考慮，我們只需考慮小車的期望向角?d。而這三個(gè)目標(biāo)只要能夠保證根據(jù)以上公式，可以推導(dǎo)出?d到d根據(jù)以上函數(shù)傳遞圖，可以推導(dǎo)出以下關(guān)系圖：我們要得到的是預(yù)期輸入d為0，輸出也為0；我們采用內(nèi)外環(huán)控制方式，引入反饋d和θp,因?yàn)閮?nèi)外環(huán)均是一階環(huán)節(jié)，采用比例控制就能獲得很好的控制效果。最終我們的內(nèi)環(huán)的比例控制是100，外環(huán)的比例控制是30，因此?d=-3000d-30θp控制器的階躍響應(yīng)圖如下圖所示：最后，得到真實(shí)世界坐標(biāo)系中的預(yù)期期望速度：VV最后，經(jīng)過(guò)世界坐標(biāo)系到機(jī)器坐標(biāo)系的轉(zhuǎn)換，得到預(yù)期的電機(jī)線速度。至此，整個(gè)運(yùn)動(dòng)學(xué)控制結(jié)束。根據(jù)以上推導(dǎo)，我們搭建的Simulink模塊如下圖所示：整個(gè)封裝與路徑規(guī)劃模塊、坐標(biāo)轉(zhuǎn)換模塊的連接如下圖所示：3.6觀測(cè)器為了方便觀察預(yù)期與實(shí)際的誤差，我們?cè)O(shè)計(jì)了一個(gè)觀測(cè)器模塊如下：觀測(cè)器封裝如下：四、結(jié)果驗(yàn)收為了對(duì)我們的設(shè)計(jì)效果進(jìn)行評(píng)估，我們?cè)俅卫脤W(xué)長(zhǎng)做對(duì)照，比較了x軸方向的誤差，y軸方向的誤差，前進(jìn)方向偏角θ，和速度誤差，結(jié)果如下：4.1x軸方向的誤差我們的設(shè)計(jì)學(xué)長(zhǎng)的設(shè)計(jì)從兩圖對(duì)比可以看出，我們的性能比學(xué)長(zhǎng)的提高了300%。4.2y軸方向的誤差我們的設(shè)計(jì)學(xué)長(zhǎng)的設(shè)計(jì)從兩圖對(duì)比可以看出，我們的性能比學(xué)長(zhǎng)的提高了800%。4.3前進(jìn)方向偏角θ我們的設(shè)計(jì)學(xué)長(zhǎng)的設(shè)計(jì)從兩圖對(duì)比可以看出，我們的性能比學(xué)長(zhǎng)的提高了10000倍。4.4速度誤差我們的設(shè)計(jì)學(xué)長(zhǎng)的設(shè)計(jì)從兩圖對(duì)比可以看出，我們的設(shè)計(jì)速度保持的較好而學(xué)長(zhǎng)的速度波動(dòng)較大。五、致謝首先，非常感謝徐明老師給了我們這個(gè)機(jī)會(huì)，讓我們了解了一些三輪全向機(jī)器人的基本原理，以及提高了對(duì)MATLAB運(yùn)用的熟練程度。其次，要向我們素未謀面的為我們提供了一份樣板的師兄們。他們的工作為我們節(jié)約了大量的時(shí)間。雖然在文中我們用他們的結(jié)果作對(duì)比，來(lái)證明我們控制的優(yōu)越性，但并不意味著我們比他們高明，因?yàn)樗麄儾攀情_(kāi)拓者，是他們完成了從無(wú)到有的過(guò)程，在此向他們致敬！六、附錄（路徑規(guī)劃函數(shù)）functionP=cal_P(t)%P=cal_P(t)%ê?è?t￡oB?ùì??ú??2?êyt%ê?3?P:[x(t);y(t);V(t);thelta(t)]%±?3ìDòó?óúsimulink·???%?-ê?????μ?C=[0123450000.545];V1=[1;0];V2=[1;0];L=0.254;NC=length(C);%???ó????μ?￡?±￡?¤1y?eμ?oí??μ?RC=[C(:,1)-V1*L,C(:,1),C(:,1)+V1*L,C(:,2:NC-1),C(:,NC)-V2*L,C(:,NC),C(:,NC)+V2*L];N=length(RC);OUT=0;if(t>=N-3)I=N-4;OUT=1;dt=t-(N-3);t=1;elseI=fix(t);t=mod(t,1);endf1=(1-t).^3/6;f2=(3*t.^3-6*t.^2+4)/6;f3=(-3*t.^3+3*t.^2+3*t+1)/6;f4=t.^3/6;d1=-1/2*(1-t)^2;d2=3/2*t^2-2*t;d3=-3/2*t^2+t+1/2;d4=1/2*t^2;a1=1-t;a2=3*t-2;a3=-3*t+1;a4=t;P(1,1)=f1*RC(1,I+1)+f2*RC(1,I+2)+f3*RC(1,I+3)+f4*RC(1,I+4);P(2,1)=f1*RC(2,I+1)+f2*RC(2,I+2)+f3*RC(2,I+3)+f4*RC(2,I+4);V(1,1)=d1*RC(1,I+1)+d2*RC(1,I+2)+d3*RC(1,I+3)+d4*RC(1,I+4);V(2,1)=d1*RC(2,I+1)+d2*RC(2,I+2)+d3*RC(2,I+3)+d4*RC(2,I+4);A(1,1)=a