《機(jī)器人技術(shù)-建模、仿真及應(yīng)用》課件 第五章 機(jī)器人機(jī)械臂的運(yùn)動(dòng)軌跡規(guī)劃_第1頁
《機(jī)器人技術(shù)-建模、仿真及應(yīng)用》課件 第五章 機(jī)器人機(jī)械臂的運(yùn)動(dòng)軌跡規(guī)劃_第2頁
《機(jī)器人技術(shù)-建模、仿真及應(yīng)用》課件 第五章 機(jī)器人機(jī)械臂的運(yùn)動(dòng)軌跡規(guī)劃_第3頁
《機(jī)器人技術(shù)-建模、仿真及應(yīng)用》課件 第五章 機(jī)器人機(jī)械臂的運(yùn)動(dòng)軌跡規(guī)劃_第4頁
《機(jī)器人技術(shù)-建模、仿真及應(yīng)用》課件 第五章 機(jī)器人機(jī)械臂的運(yùn)動(dòng)軌跡規(guī)劃_第5頁
已閱讀5頁,還剩46頁未讀, 繼續(xù)免費(fèi)閱讀

下載本文檔

版權(quán)說明:本文檔由用戶提供并上傳,收益歸屬內(nèi)容提供方,若內(nèi)容存在侵權(quán),請進(jìn)行舉報(bào)或認(rèn)領(lǐng)

文檔簡介

ROBOT機(jī)器人技術(shù)——建模、仿真及應(yīng)用軌跡規(guī)劃第五章

目錄軌跡規(guī)劃應(yīng)考慮的問題PART.1關(guān)節(jié)空間的軌跡規(guī)劃PART.2直角坐標(biāo)空間的軌跡規(guī)劃PART.3引言一類是點(diǎn)到點(diǎn)的運(yùn)動(dòng),如將部件從一點(diǎn)搬運(yùn)到另外一點(diǎn)。另外一類是沿著一段連續(xù)的曲線運(yùn)動(dòng),如擬合一段焊縫的操作。機(jī)器人機(jī)械臂的運(yùn)動(dòng)規(guī)劃是指機(jī)械臂做運(yùn)動(dòng)的指令函數(shù),這種隨時(shí)間變化的指令函數(shù)主要包含兩類:軌跡規(guī)劃應(yīng)考慮的問題PART.1軌跡規(guī)劃應(yīng)考慮的問題軌跡規(guī)劃中不僅要規(guī)定機(jī)器人的起始點(diǎn)和終止點(diǎn),還要給出中間點(diǎn)(路徑點(diǎn))的位姿及路徑點(diǎn)間的時(shí)間分配,即給出兩個(gè)路徑點(diǎn)之間的運(yùn)動(dòng)時(shí)間,必須沿特定的路徑運(yùn)動(dòng)(路徑約束)。軌跡規(guī)劃既可在關(guān)節(jié)空間進(jìn)行,將所有的關(guān)節(jié)變量表示為時(shí)間的函數(shù),用其一階、二階導(dǎo)數(shù)描述機(jī)器人的預(yù)期動(dòng)作;也可在直角坐標(biāo)空間進(jìn)行,將手部位姿參數(shù)表示為時(shí)間的函數(shù),相應(yīng)的關(guān)節(jié)位置、速度和加速度由手部信息導(dǎo)出。在規(guī)劃機(jī)器人的運(yùn)動(dòng)軌跡時(shí),還需要弄清楚在其路徑上是否存在障礙物(障礙約束)。軌跡規(guī)劃應(yīng)考慮的問題項(xiàng)目障礙約束有無路徑約束有離線無碰撞路徑規(guī)劃+在線路徑跟蹤離線路徑規(guī)劃+在線路徑跟蹤無位置控制+在線障礙探測和避障位置控制表.操作臂控制方式軌跡規(guī)劃器框圖用戶根據(jù)作業(yè)給出各個(gè)路徑節(jié)點(diǎn)后,規(guī)劃器的任務(wù)包含解變換方程、進(jìn)行運(yùn)動(dòng)學(xué)反解和插值運(yùn)算等。在關(guān)節(jié)空間進(jìn)行規(guī)劃時(shí),大量工作是對關(guān)節(jié)變量的插值運(yùn)算。以二自由度平面關(guān)節(jié)機(jī)器人為例解釋軌跡規(guī)劃的基本原理。要求機(jī)器人從A點(diǎn)運(yùn)動(dòng)到B點(diǎn),在A點(diǎn)時(shí)形位角為α=20°、β=30°,到B點(diǎn)時(shí)的形位角是α=40°、β=80°,兩關(guān)節(jié)運(yùn)動(dòng)的最大速率均為10°/s。關(guān)節(jié)空間非歸一化運(yùn)動(dòng)例所有關(guān)節(jié)以最大角速度運(yùn)動(dòng),下方連桿用2s到達(dá),上方連桿還需再運(yùn)動(dòng)3s??梢娐窂讲灰?guī)則,手部掠過的距離點(diǎn)也不均勻。軌跡規(guī)劃應(yīng)考慮的問題關(guān)節(jié)空間歸一化運(yùn)動(dòng)例機(jī)器人手臂兩關(guān)節(jié)速率用有關(guān)公共因子做歸一化處理,成比例減慢手臂運(yùn)動(dòng)范圍較小的關(guān)節(jié)運(yùn)動(dòng),使兩個(gè)關(guān)節(jié)同步開始和結(jié)束運(yùn)動(dòng),即兩個(gè)關(guān)節(jié)以不同速度一起連續(xù)運(yùn)動(dòng)。運(yùn)動(dòng)軌跡的各部分更均衡,但是路徑仍然不規(guī)則。例1、例2只關(guān)注關(guān)節(jié)值,忽略機(jī)器人手臂末端位置,均在關(guān)節(jié)空間中進(jìn)行規(guī)劃,所需計(jì)算僅是運(yùn)動(dòng)終點(diǎn)的關(guān)節(jié)量。軌跡規(guī)劃應(yīng)考慮的問題直角坐標(biāo)空間運(yùn)動(dòng)例其路徑是直線,但形位角變化不均勻。若希望機(jī)器人手部可沿直線AB這條線運(yùn)動(dòng),可在兩點(diǎn)間的插值,即將直線等分為幾部分,計(jì)算出各點(diǎn)所需形位角α和β值。若路徑點(diǎn)過少,不能保證各小段內(nèi)的嚴(yán)格直線軌跡,為獲得良好的沿循精度,應(yīng)更細(xì)致分割路徑。軌跡規(guī)劃應(yīng)考慮的問題具有加速和減速段的軌跡規(guī)劃例開始時(shí)直線段AB基于方程x=(1/2)at2劃分,到達(dá)所需要的運(yùn)動(dòng)速度v=at時(shí)為止。對路徑進(jìn)行不同方法分段。手臂開始加速運(yùn)動(dòng)的路徑分段較小,隨后以恒定速度運(yùn)動(dòng);接近B點(diǎn)時(shí)在較小分段上減速。末端運(yùn)動(dòng)依據(jù)減速過程類似劃分。軌跡規(guī)劃應(yīng)考慮的問題路徑平滑過渡例機(jī)器人運(yùn)動(dòng)更平穩(wěn),降低其應(yīng)力水平,減少能量消耗。B點(diǎn)兩邊運(yùn)動(dòng)進(jìn)行平滑過渡。機(jī)器人先接近B點(diǎn)(必要時(shí)可減速),然后沿著平滑過渡路徑重新加速,最終抵達(dá)并停于C點(diǎn)。多段運(yùn)動(dòng)的中間段均可采用過渡方式平滑連接。機(jī)器人經(jīng)過的B’點(diǎn),若要求機(jī)器人精確經(jīng)過B點(diǎn),可預(yù)設(shè)不同的B“點(diǎn),使得平滑過渡曲線剛好經(jīng)過B點(diǎn)。軌跡規(guī)劃應(yīng)考慮的問題通過中間規(guī)定點(diǎn)規(guī)劃例保證機(jī)器人通過中間規(guī)定點(diǎn)的替代方案,B點(diǎn)前后各加過渡點(diǎn)D和E,使得B點(diǎn)落在DE連線上,確保機(jī)器人能夠經(jīng)過B點(diǎn)。軌跡規(guī)劃應(yīng)考慮的問題關(guān)節(jié)空間的軌跡規(guī)劃PART.2過路徑點(diǎn)的三次多項(xiàng)式插值規(guī)劃用拋物線過渡的線性插值規(guī)劃三次多項(xiàng)式插值規(guī)劃過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃高階多項(xiàng)式插值規(guī)劃關(guān)節(jié)空間的軌跡規(guī)劃三次多項(xiàng)式插值規(guī)劃假設(shè)機(jī)器人的初始位姿已知,通過求解逆運(yùn)動(dòng)學(xué)方程可求得機(jī)器人期望末端位姿對應(yīng)的關(guān)節(jié)角。在機(jī)械手運(yùn)動(dòng)過程中,由于相應(yīng)與起始點(diǎn)的關(guān)節(jié)角度

是已知的,而終止點(diǎn)的關(guān)節(jié)角

可以通過運(yùn)動(dòng)學(xué)逆解得到。因此,運(yùn)動(dòng)軌跡的描述可用起始點(diǎn)關(guān)節(jié)角度的一個(gè)平滑插值函數(shù)

來表示。

時(shí)刻的值是起始關(guān)節(jié)角度

,在終端時(shí)刻

的值是

。顯然,平滑插值函數(shù)可作為關(guān)節(jié)插值函數(shù),如圖所示。單個(gè)關(guān)節(jié)的不同軌跡曲線關(guān)節(jié)空間的軌跡規(guī)劃三次多項(xiàng)式插值規(guī)劃為了實(shí)現(xiàn)單個(gè)關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng),軌跡函數(shù)

至少需要滿足4個(gè)約束條件,其中兩個(gè)約束條件是起始點(diǎn)和終止點(diǎn)對應(yīng)的關(guān)節(jié)角度滿足以下方程:為了滿足關(guān)節(jié)運(yùn)動(dòng)速度的連續(xù)性要求,另外還有兩個(gè)約束條件,即在起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度要求。在當(dāng)前情況下,規(guī)定:(1)(2)式(1)和式(2)確定了一個(gè)三次多項(xiàng)式:(3)運(yùn)動(dòng)軌跡上的關(guān)節(jié)速度和加速度則為:(4)關(guān)節(jié)空間的軌跡規(guī)劃三次多項(xiàng)式插值規(guī)劃對式(3)和式(4)代人相應(yīng)的約束條件,得到有關(guān)系數(shù)

、

、

的四個(gè)線性方程:求解得這組解只用于關(guān)節(jié)起始速度和終止速度為零的運(yùn)動(dòng)情況。對于其他情況,后面另行討論。關(guān)節(jié)空間的軌跡規(guī)劃三次多項(xiàng)式插值規(guī)劃例如圖所示的三連桿機(jī)械臂,當(dāng)

時(shí),

當(dāng)

時(shí),機(jī)械臂各個(gè)連桿同時(shí)運(yùn)動(dòng)到

,

。在

時(shí),使轉(zhuǎn)角

,

。假設(shè)機(jī)械臂在起點(diǎn)和終點(diǎn)的速度和加速度都為0,對機(jī)械臂連桿轉(zhuǎn)角

進(jìn)行三次多項(xiàng)式插值規(guī)劃,得出位置、速度和加速度的變化曲線,仿真結(jié)果如下。三連桿機(jī)械臂已知條件:當(dāng)時(shí),當(dāng)時(shí),,,當(dāng)時(shí),,,關(guān)節(jié)空間的軌跡規(guī)劃三次多項(xiàng)式插值規(guī)劃已知條件:當(dāng),。當(dāng),,,。當(dāng),

,。例q_array=[0,-60,45];t_array=[0,2,5];v_array=[0,20,0];a_array=[0,-10,0];t=t_array(1);q=q_array(1);v=v_array(1);a=a_array(1);fori=1:1:length(q_array)-1T=t_array(i+1)-t_array(i);a0=q_array(i);a1=0;a2=(q_array(i+1)-q_array(i))*3/(T^2);a3=(q_array(i)-q_array(i+1))*2/(T^3);ti=t_array(i):0.02:t_array(i+1);qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3;vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2;ai=2*a2+6*a3*(ti-t_array(i));t=[t,ti(2:end)];q=[q,qi(2:end)];v=[v,vi(2:end)];a=[a,ai(2:end)];

endsubplot(3,1,1),plot(t,q,'r’),xlabel('t'),ylabel('position’);holdon;plot(t_array,q_array,'*','color','r’),gridon;subplot(3,1,2),plot(t,v,'b’),xlabel('t'),ylabel('velocity’);holdon;plot(t_array,v_array,'o','color','g’),gridon;subplot(3,1,3),plot(t,a,'g’),xlabel('t'),ylabel('accelerate’);holdon;plot(t_array,a_array,'^','color','b'),gridon;三次多項(xiàng)式插值規(guī)劃仿真圖代碼:關(guān)節(jié)空間的軌跡規(guī)劃三次多項(xiàng)式插值規(guī)劃要求一個(gè)6軸機(jī)器人的第一關(guān)節(jié)在5s內(nèi)從初始角30°運(yùn)動(dòng)到終端角75°,用三項(xiàng)式計(jì)算在1s、2s、3s和4s時(shí)關(guān)節(jié)的角度。例邊界值代入可得:解由此得位置、速度和加速度的多項(xiàng)式方程:關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)的三次多項(xiàng)式插值規(guī)劃一般而言,要求規(guī)劃過路徑點(diǎn)的軌跡。可以把所有路徑點(diǎn)也看作是“起始點(diǎn)”或“終止點(diǎn)”,求解逆運(yùn)動(dòng)學(xué),得到相應(yīng)的關(guān)節(jié)矢量值。然后確定所要求的三次多項(xiàng)式插值函數(shù),把路徑點(diǎn)平滑地連接起來。但是,在這些“起始點(diǎn)”或“終止點(diǎn)”的關(guān)節(jié)運(yùn)動(dòng)速度不再是零。路徑點(diǎn)上的關(guān)節(jié)速度可以根據(jù)需要設(shè)定,這樣一來,確定三次多項(xiàng)式的方法與前面所述的完全相同,只是速度約束條件式

變?yōu)椋捍_定三次多項(xiàng)式的四個(gè)方程為:求解得關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)的三次多項(xiàng)式插值規(guī)劃實(shí)際上,由上式確定的三次多項(xiàng)式描述了起始點(diǎn)和終止點(diǎn)具有任意輸定位置和速度的運(yùn)動(dòng)軌跡。剩下的問題就是如何確定路徑點(diǎn)上的關(guān)節(jié)速度,確定中間點(diǎn)處的期望關(guān)節(jié)速度可以使用以下幾種方法:根據(jù)坐標(biāo)系在直角坐標(biāo)空間中的瞬時(shí)線速度和角速度來確定每個(gè)路徑點(diǎn)的關(guān)節(jié)速度。在直角坐標(biāo)空間或關(guān)節(jié)空間中采用適當(dāng)?shù)膯l(fā)式方法,由控制系統(tǒng)自動(dòng)地選擇路徑點(diǎn)的速度。為了保證每個(gè)路徑點(diǎn)上的加速度連續(xù),由控制系統(tǒng)按此要求自動(dòng)地選擇路徑點(diǎn)的速度。關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)的三次多項(xiàng)式插值規(guī)劃對轉(zhuǎn)角

進(jìn)行過路徑點(diǎn)的三次多項(xiàng)式插值規(guī)劃。根據(jù)過路徑點(diǎn)的規(guī)劃要求,機(jī)械臂在起點(diǎn)的速度為

,終點(diǎn)的速度為

。假設(shè)機(jī)械臂在起點(diǎn)的加速度為

,終點(diǎn)的加速度為

。例q_array=[0,-60,45];t_array=[0,2,5];v_array=[-15,20,10];a_array=[-20,-10,15];t=t_array(1);q=q_array(1);v=v_array(1);a=a_array(1);fori=1:1:length(q_array)-1T=t_array(i+1)-t_array(i);a0=q_array(i);a1=v_array(i);a2=(q_array(i+1)-q_array(i))*3/(T^2)-(2*v_array(i)+v_array(i+1))/T;a3=(q_array(i)-q_array(i+1))*2/(T^3)+(v_array(i)+v_array(i+1))/(T^2);ti=t_array(i):0.02:t_array(i+1);qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3;vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2;ai=2*a2+6*a3*(ti-t_array(i));t=[t,ti(2:end)];q=[q,qi(2:end)];v=[v,vi(2:end)];a=[a,ai(2:end)];

endsubplot(3,1,1),plot(t,q,'r'),xlabel('t’),ylabel('position');holdon;plot(t_array,q_array,'*','color','r'),gridon;subplot(3,1,2),plot(t,v,'b'),xlabel('t’),ylabel('velocity');holdon;plot(t_array,v_array,'o','color','g'),gridon;subplot(3,1,3),plot(t,a,'g'),xlabel('t’),ylabel('accelerate');holdon;plot(t_array,a_array,'^','color','b'),gridon;代碼:三次多項(xiàng)式插值規(guī)劃仿真圖三連桿機(jī)械臂關(guān)節(jié)空間的軌跡規(guī)劃用拋物線過渡的線性插值規(guī)劃對于給定的起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)角度,可選擇直線插值函數(shù)來表示路徑形狀,對于運(yùn)動(dòng)軌跡不為直線的。單純的線性插值將導(dǎo)致在節(jié)點(diǎn)處關(guān)節(jié)運(yùn)動(dòng)速度不連續(xù),加速度無限大。為了生成一條位置和速度都連續(xù)的平滑運(yùn)動(dòng)軌跡,在使用線性插值時(shí),在每個(gè)節(jié)點(diǎn)增加一段拋物線擬合區(qū)域。由于拋物線對于時(shí)間的二階導(dǎo)數(shù)為常數(shù),即相應(yīng)區(qū)段內(nèi)的加速度恒定不變,這樣使得平滑過渡,不至于在節(jié)點(diǎn)處產(chǎn)生跳躍。線性函數(shù)與兩段拋物線函數(shù)平滑銜接在一起形成的軌跡,稱為帶有拋物線過渡域線性軌跡,如圖1所示。為了構(gòu)造這段運(yùn)動(dòng)軌跡,假設(shè)這兩段的過渡域(拋物線)具有相同的持續(xù)時(shí)間,因而再這兩個(gè)域中采用相同的恒加速度值,只是符號相反。正如圖2所示,存在多個(gè)解,得到軌跡不是唯一的。圖1含有一個(gè)解圖2含有多個(gè)解關(guān)節(jié)空間的軌跡規(guī)劃用拋物線過渡的線性插值規(guī)劃但是每個(gè)結(jié)果都對稱于時(shí)間中點(diǎn)

和位置中點(diǎn)

。由于過渡域

終點(diǎn)的速度必須等于線性域的速度,所以:式中,

為過渡域終點(diǎn)處

的關(guān)節(jié)角度。用

表示過渡域內(nèi)的加速度,

的值可按解得:令

,根據(jù)上兩式可得:關(guān)節(jié)空間的軌跡規(guī)劃用拋物線過渡的線性插值規(guī)劃這樣對于任意給定的

、

,可以按式

選擇相應(yīng)的

,得到路徑曲線。通常的做法是先選擇加速度

的值,然后算出相應(yīng)的

即:由上式可知:為保證

有解,過渡域加速度值

必須選得足夠大,即:當(dāng)上式中的等號成立時(shí),線性域的長度縮減為零,整個(gè)路徑段由兩個(gè)過渡域組成,這兩個(gè)過渡域在銜接處的斜率相等。當(dāng)加速度的取值越來越大,過渡域的長度會(huì)越來越短。如果加速度選為無限大,路徑又恢復(fù)到簡單的線性插值情況。選取,求得,可得關(guān)節(jié)的軌跡函數(shù)為:解:首先選取過渡域的加速度,其應(yīng)滿足關(guān)節(jié)空間的軌跡規(guī)劃用拋物線過渡的線性插值規(guī)劃例一個(gè)旋轉(zhuǎn)關(guān)節(jié)在3s內(nèi)從起始點(diǎn)

運(yùn)動(dòng)到終止點(diǎn)

,對該關(guān)節(jié)進(jìn)行不過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃,仿真計(jì)算。關(guān)節(jié)空間的軌跡規(guī)劃用拋物線過渡的線性插值規(guī)劃3s內(nèi)關(guān)節(jié)的角度變化如圖一所示。可以看出關(guān)節(jié)的角度變化是光滑的。關(guān)節(jié)的角速度和角加速度的變化分別如圖二和圖三所示。關(guān)節(jié)的角速度曲線呈梯形,由加速段、勻速段和減速段組成,屬于等加速等減速運(yùn)動(dòng)。關(guān)節(jié)的角加速度曲線由多個(gè)線段組成,變化不光滑,存在加速度瞬時(shí)要達(dá)到某個(gè)值及瞬時(shí)又要變?yōu)榱愕耐蛔儯@會(huì)造成關(guān)節(jié)的振動(dòng)沖擊。關(guān)節(jié)角度仿真代碼及結(jié)果如下。t=0:0.0001:3;theta=zeros(size(t));fori=1:length(t)ift(i)<0.7362theta(i)=10+15*(t(i).^2);elseif(t(i)>=0.7362)&&(t(i)<=2.2638)theta(i)=1.8702+22.0860*t(i);elsetheta(i)=-15*(t(i).^2)+90*t(i)-75;endendplot(t,theta)圖一關(guān)節(jié)角度曲線圖關(guān)節(jié)角速度仿真代碼及結(jié)果如下。t=0:0.0001:3w=zeros(size(t))fori=1:length(t)ift(i)<0.7362w(i)=30*t(i);elseif(t(i)>=0.7362)&&(t(i)<=2.2638)w(i)=22.0860;elsew(i)=-30*t(i)+90;endendplot(t,w)關(guān)節(jié)力矩仿真代碼及結(jié)果如下。t=0:0.0001:3a=zeros(size(t))fori=1:length(t)ift(i)<0.7362a(i)=30;elseif(t(i)>=0.7362)&&(t(i)<=2.2638)a(i)=0;elsea(i)=-30;endendplot(t,a)圖2

關(guān)節(jié)角速度曲線圖圖3

關(guān)節(jié)角加速度曲線圖關(guān)節(jié)空間的軌跡規(guī)劃用拋物線過渡的線性插值規(guī)劃關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃如圖所示,某個(gè)關(guān)節(jié)在運(yùn)動(dòng)中設(shè)有

個(gè)路徑點(diǎn),其中三個(gè)相鄰的路徑點(diǎn)表示為

,每兩個(gè)相鄰的路徑點(diǎn)之間都以線性函數(shù)相連,而所有路徑點(diǎn)附近由拋物線過渡。圖中,在點(diǎn)

的過渡域的持續(xù)時(shí)間為

;點(diǎn)

和點(diǎn)

之間線性域的持續(xù)時(shí)間為

;連接

與點(diǎn)

的路徑段全部持續(xù)時(shí)間為

。另外,

與點(diǎn)

之間線性域速度為

,

點(diǎn)過渡域的加速度為

?,F(xiàn)在的問題是在含有路徑點(diǎn)的情況下,如何確定帶有拋物線過渡域的線性軌跡。多段帶有拋物線過渡的線性插值軌跡關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃與上述用拋物線過渡的線性插值相同,這個(gè)問題有許多解,每一解對應(yīng)于一個(gè)選取的速度值。給定任意路徑點(diǎn)的位置

、持續(xù)時(shí)間

及加速度的絕對值

,可以計(jì)算出過渡域的持續(xù)時(shí)間

。對于那些內(nèi)部路徑段

,2;

、

),根據(jù)下列方程求解:第一個(gè)路徑段和最后一個(gè)路徑段的處理與上式略有不同,因?yàn)檐壽E端部的整個(gè)過渡域的持續(xù)時(shí)間都必須計(jì)入這一路徑段內(nèi)。對于第一個(gè)路徑段,令線性域速度的兩個(gè)表達(dá)式相等,就可求出

,計(jì)算式為:關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃根據(jù)前式算出起始點(diǎn)過渡域的持續(xù)時(shí)間

之后,進(jìn)而求出

,即:對于最后一個(gè)路徑段,路徑點(diǎn)

與終止點(diǎn)

之間的參數(shù)與第一個(gè)路徑段相似,即:關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃根據(jù)前式便可求出:上述公式可用來求出多段軌跡中各個(gè)過渡域的時(shí)間和速度。值得注意的是:多段用拋物線過渡的直線樣條函數(shù)一般并不經(jīng)過哪些路徑點(diǎn),除非在這些路徑點(diǎn)處停止。若選的加速度充分大,則實(shí)際路徑將與理想路徑點(diǎn)十分靠近。如果要求機(jī)器人途經(jīng)某個(gè)節(jié)點(diǎn),那么將軌跡分成兩段,把此點(diǎn)作為前一段的終止點(diǎn)和后一段的起始點(diǎn)即可。關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃例基于例5.1,對轉(zhuǎn)角

進(jìn)行過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃。按線性插值規(guī)劃要求,當(dāng)

時(shí),機(jī)械臂各個(gè)連桿同時(shí)運(yùn)動(dòng)到

,

。當(dāng)

時(shí),使轉(zhuǎn)角滿足

,

,

。設(shè)定機(jī)械臂在起點(diǎn)和終點(diǎn)的速度和加速度都為0,并且

,進(jìn)行仿真計(jì)算:解:規(guī)劃曲線穿過點(diǎn)的規(guī)劃步驟如下:1)確定區(qū)段和的兩直線方程。2)通過區(qū)段和的直線方程找到點(diǎn)11和。3)確定區(qū)段和的兩直線方程。4)取時(shí)間段,找到4個(gè)點(diǎn)和。5)將邊界條件分別帶入系數(shù)計(jì)算式,計(jì)算并導(dǎo)出在和內(nèi)為直線,而在,,11111和內(nèi)為曲線的規(guī)劃方程。關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃首先,區(qū)段和的兩直線方程為:由和兩直線方程可以計(jì)算出和為:再由和可以確定區(qū)段和的兩直線方程為:進(jìn)一步由直線方程

可以計(jì)算出4個(gè)點(diǎn)

分別為:關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃根據(jù)規(guī)劃曲線的速度和加速度的連續(xù)性,列出邊界條件見表邊界條件0.00.00.027.555.00.055.055.00.090.010.0-3085.0-25.00.072.5-25.00.060.00.00.0區(qū)段

的兩直線方程邊界條件表關(guān)節(jié)空間的軌跡規(guī)劃過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃根據(jù)表的邊界條件計(jì)算各個(gè)區(qū)段的系數(shù),得出規(guī)劃方程為:關(guān)節(jié)空間的軌跡規(guī)劃確定過渡點(diǎn)的仿真代碼如下:t1=0.5:0.1:3;t2=2:0.1:4.5;t3=0.5:0.1:2;t4=3:0.1:4.5;theta1=-22.5+45*t1;theta2=127.5-15*t2;theta3=-27.5+55*t3;theta4=172.5-25*t4;plot(t1,theta1,'b');holdon;plot(t2,theta2,'b');holdon;plot(t3,theta3,'b');holdon;plot(t4,theta4,'b');axis([050100]);過渡點(diǎn)的選擇過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃關(guān)節(jié)空間的軌跡規(guī)劃t=0:0.1:5;theta=zeros(size(t));fori=1:length(t)ift(i)<1theta(i)=55*(t(i).^3)-27.5*(t(i).^4);elseif(t(i)>=1)&&(t(i)<1.5)theta(i)=-27.5+55*t(i);elseif(t(i)>=1.5)&&(t(i)<2.5)theta(i)=55+55*(t(i)-1.5)-35*((t(i)-1.5).^3)+15*((t(i)-1.5).^4);else

if(t(i)>=2.5)&&(t(i)<3.5)theta(i)=90+10*(t(i)-2.5)-15*((t(i)-2.5).^2)+35*((t(i)-2.5).^3)-65*((t(i)-2.5).^4)+30*((t(i)-2.5).^5);elseif(t(i)>=3.5)&&(t(i)<4)theta(i)=172.5-25*t(i);elsetheta(i)=72.5-25*(t(i)-4)+25*((t(i)-4).^3)-12.5*((t(i)-4).^4);endendplot(t,theta)通過

點(diǎn)的規(guī)劃穿過中間點(diǎn)的運(yùn)動(dòng)規(guī)劃規(guī)劃方程的仿真代碼如下:過路徑點(diǎn)用拋物線過渡的線性插值規(guī)劃關(guān)節(jié)空間的軌跡規(guī)劃高階多項(xiàng)式插值規(guī)劃如果對于運(yùn)動(dòng)軌跡的要求更為嚴(yán)格,約束條件增多,那么三次多項(xiàng)式就不能滿足需要,必須用更高階的多項(xiàng)式對運(yùn)動(dòng)軌跡的路徑段進(jìn)行插值。例如,對某段路徑的起始點(diǎn)和終止點(diǎn)都規(guī)定了關(guān)節(jié)的位置、速度和加速度,則要用一個(gè)五次多項(xiàng)式進(jìn)行插值,即:多項(xiàng)式的系數(shù)

、

、

、

、

必須滿足以下6個(gè)約束條件:求解得關(guān)節(jié)空間的軌跡規(guī)劃高階多項(xiàng)式插值規(guī)劃例基于前邊的例題,對轉(zhuǎn)角

進(jìn)行五次多項(xiàng)式的插值規(guī)劃。按五次多項(xiàng)式插值規(guī)劃要求,當(dāng)

時(shí),機(jī)械臂各個(gè)連桿同時(shí)運(yùn)動(dòng)到

,

,

;當(dāng)

時(shí),使轉(zhuǎn)角滿足

,,

。機(jī)械臂在起點(diǎn)的速度為

,終點(diǎn)的速度為

;在起點(diǎn)的加速度為

終點(diǎn)的加速度為

按上述已知條件進(jìn)行仿真計(jì)算。解:仿真計(jì)算過程如下。q_array=[0,-30,60];%指定起止位置t_array=[0,2,5];%指定起止時(shí)間v_array=[10,-20,15];%指定起止速度a_array=[-30,15,10];%指定起止加速度t=t_array(1);q=q_array(1);v=v_array(1);a=a_array(1);fori=1:1:length(q_array)-1T=t_array(i+1)-t_array(i);a0=q_array(i);a1=v_array(i);a2=a_array(i)/2;關(guān)節(jié)空間的軌跡規(guī)劃高階多項(xiàng)式插值規(guī)劃代碼后續(xù):a3=(20*q_array(i+1)-20*q_array(i)-(8*v_array(i+1)+12*v_array(i))*T-(3*a_array(i)-a_array(i+1))*T^2)/(2*T^3);a4=(30*q_array(i)-30*q_array(i+1)+(14*v_array(i+1)+16*v_array(i))*T+(3*a_array(i)-2*a_array(i+1))*T^2)/(2*T^4);a5=(12*q_array(i+1)-12*q_array(i)-(6*v_array(i+1)+6*v_array(i))*T-(a_array(i)-a_array(i+1))*T^2)/(2*T^5);ti=t_array(i):0.02:t_array(i+1);qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3+a4*(ti-t_array(i)).^4+a5*(ti-t_array(i)).^5;vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2+4*a4*(ti-t_array(i)).^3+5*a5*(ti-t_array(i)).^4;ai=2*a2+6*a3*(ti-t_array(i))+12*a4*(ti-t_array(i)).^2+20*a5*(ti-t_array(i)).^3;t=[t,ti(2:end)];q=[q,qi(2:end)];v=[v,vi(2:end)];a=[a,ai(2:end)];endsubplot(3,1,1),plot(t,q,'r'),xlabel('t'),ylabel('position');holdon;plot(t_array,q_array,'o','color','g'),gridon;subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');holdon;plot(t_array,v_array,'*','color','y'),gridon;subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');holdon;plot(t_array,a_array,'^','color','r'),gridon;高階多項(xiàng)式插值規(guī)劃仿真圖直角坐標(biāo)空間的軌跡規(guī)劃PART.3直角坐標(biāo)空間軌跡規(guī)劃直角坐標(biāo)空間軌跡與機(jī)器人相對于直角坐標(biāo)系的運(yùn)動(dòng)有關(guān)。除了簡單的直線軌跡以外,也可用許多其他的方法來控制機(jī)器人在不同點(diǎn)之間沿一定軌跡運(yùn)動(dòng)。實(shí)際上所有用于關(guān)節(jié)空間軌跡規(guī)劃的方法都可用于直角坐標(biāo)空間的軌跡規(guī)劃。最根本的差別就是,直角坐標(biāo)空間軌跡規(guī)劃必須反復(fù)求解逆運(yùn)動(dòng)方程來計(jì)算關(guān)節(jié)角。也就是說,對于關(guān)節(jié)空間軌跡規(guī)劃,規(guī)劃函數(shù)生成的值就是關(guān)節(jié)值,而直角坐標(biāo)空間軌跡規(guī)劃函數(shù)生成的值是機(jī)器人末端手的位姿,它們需要通過求解逆運(yùn)動(dòng)方程才能化為關(guān)節(jié)量。這個(gè)過程可以簡化為一個(gè)計(jì)算循環(huán):直角坐標(biāo)空間軌跡規(guī)劃所有用于關(guān)節(jié)空間的軌跡規(guī)劃方法均可用于直角坐標(biāo)空間軌跡規(guī)劃。直角坐標(biāo)軌跡規(guī)劃必須不斷進(jìn)行逆運(yùn)動(dòng)學(xué)運(yùn)算,及時(shí)獲得關(guān)節(jié)角,計(jì)算循環(huán)為:將時(shí)間增加一個(gè)增量1利用所選擇的軌跡函數(shù)計(jì)算出手的位姿;2利用逆運(yùn)動(dòng)學(xué)方程計(jì)算相應(yīng)的關(guān)節(jié)變量;3將關(guān)節(jié)變量信息送給控制器;4返回到循環(huán)的開始。5直角坐標(biāo)空間軌跡規(guī)劃為實(shí)現(xiàn)一條直線軌跡,必須計(jì)算起點(diǎn)和終點(diǎn)位姿之間的變換,并將該變換劃分為許多小段。起點(diǎn)構(gòu)型Ti

和終點(diǎn)構(gòu)型Tf間的總變換R

可通過以下方程計(jì)算:至少有三種不同方法可將總變換轉(zhuǎn)化為許多小段變換。2)在起點(diǎn)和終點(diǎn)之間的變換

分解為一個(gè)平移和兩個(gè)旋轉(zhuǎn)。平移是將坐標(biāo)原點(diǎn)從起點(diǎn)移動(dòng)到終點(diǎn),第一個(gè)旋轉(zhuǎn)是將末端機(jī)械手坐標(biāo)系與期望姿態(tài)對準(zhǔn),而第二個(gè)旋轉(zhuǎn)是手坐標(biāo)系繞其自身軸轉(zhuǎn)到最終的姿態(tài)。所有這3個(gè)變換應(yīng)同時(shí)進(jìn)行。3)在起點(diǎn)和終點(diǎn)之間的變換

分解為一個(gè)平移和一個(gè)旋轉(zhuǎn)。平移仍是將坐標(biāo)原點(diǎn)從起點(diǎn)移動(dòng)到終點(diǎn),而旋轉(zhuǎn)則是將手臂坐標(biāo)系與最終的期望姿態(tài)對準(zhǔn)。兩個(gè)變換應(yīng)同時(shí)進(jìn)行。直角坐標(biāo)空間軌跡規(guī)劃1)希望在起點(diǎn)和終點(diǎn)之間有平滑的線性變換,因此需要大量很小的分段,從而產(chǎn)生了大量的微分運(yùn)動(dòng)。利用微分運(yùn)動(dòng)方程,可將末端手坐標(biāo)系在每個(gè)新段得到位姿與微分運(yùn)動(dòng)、雅可比矩陣及關(guān)節(jié)速度通過下列方程聯(lián)系在一起:這一方法需要進(jìn)行大量的計(jì)算,并且僅當(dāng)雅可比矩陣的逆矩陣存在時(shí)才有效。例直角坐標(biāo)空間軌跡規(guī)劃一個(gè)三自由度機(jī)械臂有兩根連桿,每根連桿長9cm,如圖5.13所示。假設(shè)定義坐標(biāo)系使得當(dāng)前所有關(guān)節(jié)角均為0時(shí)手臂處于垂直向上狀態(tài)。要求機(jī)械臂沿直線從點(diǎn)(9,6,10)移動(dòng)到點(diǎn)(

溫馨提示

  • 1. 本站所有資源如無特殊說明,都需要本地電腦安裝OFFICE2007和PDF閱讀器。圖紙軟件為CAD,CAXA,PROE,UG,SolidWorks等.壓縮文件請下載最新的WinRAR軟件解壓。
  • 2. 本站的文檔不包含任何第三方提供的附件圖紙等,如果需要附件,請聯(lián)系上傳者。文件的所有權(quán)益歸上傳用戶所有。
  • 3. 本站RAR壓縮包中若帶圖紙,網(wǎng)頁內(nèi)容里面會(huì)有圖紙預(yù)覽,若沒有圖紙預(yù)覽就沒有圖紙。
  • 4. 未經(jīng)權(quán)益所有人同意不得將文件中的內(nèi)容挪作商業(yè)或盈利用途。
  • 5. 人人文庫網(wǎng)僅提供信息存儲(chǔ)空間,僅對用戶上傳內(nèi)容的表現(xiàn)方式做保護(hù)處理,對用戶上傳分享的文檔內(nèi)容本身不做任何修改或編輯,并不能對任何下載內(nèi)容負(fù)責(zé)。
  • 6. 下載文件中如有侵權(quán)或不適當(dāng)內(nèi)容,請與我們聯(lián)系,我們立即糾正。
  • 7. 本站不保證下載資源的準(zhǔn)確性、安全性和完整性, 同時(shí)也不承擔(dān)用戶因使用這些下載資源對自己和他人造成任何形式的傷害或損失。

評論

0/150

提交評論