《機器人技術-建模、仿真及應用》課件 第五章 機器人機械臂的運動軌跡規(guī)劃

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

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

是已知的，而終止點的關節(jié)角

可以通過運動學逆解得到。因此，運動軌跡的描述可用起始點關節(jié)角度的一個平滑插值函數

來表示。

在

時刻的值是起始關節(jié)角度

，在終端時刻

的值是

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

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

、

、

和

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

時，

；

當

時，機械臂各個連桿同時運動到

，

，

。在

時，使轉角

，

，

。假設機械臂在起點和終點的速度和加速度都為0，對機械臂連桿轉角

進行三次多項式插值規(guī)劃，得出位置、速度和加速度的變化曲線，仿真結果如下。三連桿機械臂已知條件：當時，當時，，，當時，，，關節(jié)空間的軌跡規(guī)劃三次多項式插值規(guī)劃已知條件：當，。當，，，。當，

，。例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;三次多項式插值規(guī)劃仿真圖代碼：關節(jié)空間的軌跡規(guī)劃三次多項式插值規(guī)劃要求一個6軸機器人的第一關節(jié)在5s內從初始角30°運動到終端角75°，用三項式計算在1s、2s、3s和4s時關節(jié)的角度。例邊界值代入可得：解由此得位置、速度和加速度的多項式方程：關節(jié)空間的軌跡規(guī)劃過路徑點的三次多項式插值規(guī)劃一般而言，要求規(guī)劃過路徑點的軌跡?？梢园阉新窂近c也看作是“起始點”或“終止點”，求解逆運動學，得到相應的關節(jié)矢量值。然后確定所要求的三次多項式插值函數，把路徑點平滑地連接起來。但是，在這些“起始點”或“終止點”的關節(jié)運動速度不再是零。路徑點上的關節(jié)速度可以根據需要設定，這樣一來，確定三次多項式的方法與前面所述的完全相同，只是速度約束條件式

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

進行過路徑點的三次多項式插值規(guī)劃。根據過路徑點的規(guī)劃要求，機械臂在起點的速度為

，終點的速度為

。假設機械臂在起點的加速度為

，終點的加速度為

。例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;代碼：三次多項式插值規(guī)劃仿真圖三連桿機械臂關節(jié)空間的軌跡規(guī)劃用拋物線過渡的線性插值規(guī)劃對于給定的起始點和終止點的關節(jié)角度，可選擇直線插值函數來表示路徑形狀，對于運動軌跡不為直線的。單純的線性插值將導致在節(jié)點處關節(jié)運動速度不連續(xù)，加速度無限大。為了生成一條位置和速度都連續(xù)的平滑運動軌跡，在使用線性插值時，在每個節(jié)點增加一段拋物線擬合區(qū)域。由于拋物線對于時間的二階導數為常數，即相應區(qū)段內的加速度恒定不變，這樣使得平滑過渡，不至于在節(jié)點處產生跳躍。線性函數與兩段拋物線函數平滑銜接在一起形成的軌跡，稱為帶有拋物線過渡域線性軌跡，如圖1所示。為了構造這段運動軌跡，假設這兩段的過渡域（拋物線）具有相同的持續(xù)時間，因而再這兩個域中采用相同的恒加速度值，只是符號相反。正如圖2所示，存在多個解，得到軌跡不是唯一的。圖1含有一個解圖2含有多個解關節(jié)空間的軌跡規(guī)劃用拋物線過渡的線性插值規(guī)劃但是每個結果都對稱于時間中點

和位置中點

。由于過渡域

終點的速度必須等于線性域的速度，所以：式中，

為過渡域終點處

的關節(jié)角度。用

表示過渡域內的加速度，

的值可按解得：令

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

、

和

，可以按式

選擇相應的

和

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

的值，然后算出相應的

即：由上式可知：為保證

有解，過渡域加速度值

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

運動到終止點

，對該關節(jié)進行不過路徑點用拋物線過渡的線性插值規(guī)劃，仿真計算。關節(jié)空間的軌跡規(guī)劃用拋物線過渡的線性插值規(guī)劃3s內關節(jié)的角度變化如圖一所示?？梢钥闯鲫P節(jié)的角度變化是光滑的。關節(jié)的角速度和角加速度的變化分別如圖二和圖三所示。關節(jié)的角速度曲線呈梯形，由加速段、勻速段和減速段組成，屬于等加速等減速運動。關節(jié)的角加速度曲線由多個線段組成，變化不光滑，存在加速度瞬時要達到某個值及瞬時又要變?yōu)榱愕耐蛔儯@會造成關節(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)圖一關節(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)關節(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

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

關節(jié)角加速度曲線圖關節(jié)空間的軌跡規(guī)劃用拋物線過渡的線性插值規(guī)劃關節(jié)空間的軌跡規(guī)劃過路徑點用拋物線過渡的線性插值規(guī)劃如圖所示，某個關節(jié)在運動中設有

個路徑點，其中三個相鄰的路徑點表示為

和

，每兩個相鄰的路徑點之間都以線性函數相連，而所有路徑點附近由拋物線過渡。圖中，在點

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

；點

和點

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

；連接

與點

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

。另外，

與點

之間線性域速度為

，

點過渡域的加速度為

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

、持續(xù)時間

及加速度的絕對值

，可以計算出過渡域的持續(xù)時間

。對于那些內部路徑段

（

、

，2；

、

），根據下列方程求解：第一個路徑段和最后一個路徑段的處理與上式略有不同，因為軌跡端部的整個過渡域的持續(xù)時間都必須計入這一路徑段內。對于第一個路徑段，令線性域速度的兩個表達式相等，就可求出

，計算式為：關節(jié)空間的軌跡規(guī)劃過路徑點用拋物線過渡的線性插值規(guī)劃根據前式算出起始點過渡域的持續(xù)時間

之后，進而求出

和

，即：對于最后一個路徑段，路徑點

與終止點

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

進行過路徑點用拋物線過渡的線性插值規(guī)劃。按線性插值規(guī)劃要求，當

時，機械臂各個連桿同時運動到

，

，

。當

時，使轉角滿足

，

，

。設定機械臂在起點和終點的速度和加速度都為0，并且

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

和

可以計算出4個點

和

分別為:關節(jié)空間的軌跡規(guī)劃過路徑點用拋物線過渡的線性插值規(guī)劃根據規(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ū)段

和

的兩直線方程邊界條件表關節(jié)空間的軌跡規(guī)劃過路徑點用拋物線過渡的線性插值規(guī)劃根據表的邊界條件計算各個區(qū)段的系數，得出規(guī)劃方程為：關節(jié)空間的軌跡規(guī)劃確定過渡點的仿真代碼如下：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]);過渡點的選擇過路徑點用拋物線過渡的線性插值規(guī)劃關節(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)通過

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

、

、

、

、

和

必須滿足以下6個約束條件：求解得關節(jié)空間的軌跡規(guī)劃高階多項式插值規(guī)劃例基于前邊的例題，對轉角

進行五次多項式的插值規(guī)劃。按五次多項式插值規(guī)劃要求，當

時，機械臂各個連桿同時運動到

，

，

；當

時，使轉角滿足

，，

。機械臂在起點的速度為

，終點的速度為

；在起點的加速度為

終點的加速度為

按上述已知條件進行仿真計算。解：仿真計算過程如下。q_array=[0,-30,60];%指定起止位置t_array=[0,2,5];%指定起止時間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;關節(jié)空間的軌跡規(guī)劃高階多項式插值規(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;高階多項式插值規(guī)劃仿真圖直角坐標空間的軌跡規(guī)劃PART.3直角坐標空間軌跡規(guī)劃直角坐標空間軌跡與機器人相對于直角坐標系的運動有關。除了簡單的直線軌跡以外，也可用許多其他的方法來控制機器人在不同點之間沿一定軌跡運動。實際上所有用于關節(jié)空間軌跡規(guī)劃的方法都可用于直角坐標空間的軌跡規(guī)劃。最根本的差別就是，直角坐標空間軌跡規(guī)劃必須反復求解逆運動方程來計算關節(jié)角。也就是說，對于關節(jié)空間軌跡規(guī)劃，規(guī)劃函數生成的值就是關節(jié)值，而直角坐標空間軌跡規(guī)劃函數生成的值是機器人末端手的位姿，它們需要通過求解逆運動方程才能化為關節(jié)量。這個過程可以簡化為一個計算循環(huán):直角坐標空間軌跡規(guī)劃所有用于關節(jié)空間的軌跡規(guī)劃方法均可用于直角坐標空間軌跡規(guī)劃。直角坐標軌跡規(guī)劃必須不斷進行逆運動學運算，及時獲得關節(jié)角，計算循環(huán)為：將時間增加一個增量1利用所選擇的軌跡函數計算出手的位姿；2利用逆運動學方程計算相應的關節(jié)變量；3將關節(jié)變量信息送給控制器；4返回到循環(huán)的開始。5直角坐標空間軌跡規(guī)劃為實現(xiàn)一條直線軌跡，必須計算起點和終點位姿之間的變換，并將該變換劃分為許多小段。起點構型Ti

和終點構型Tf間的總變換R

可通過以下方程計算：至少有三種不同方法可將總變換轉化為許多小段變換。2）在起點和終點之間的變換

分解為一個平移和兩個旋轉。平移是將坐標原點從起點移動到終點，第一個旋轉是將末端機械手坐標系與期望姿態(tài)對準，而第二個旋轉是手坐標系繞其自身軸轉到最終的姿態(tài)。所有這3個變換應同時進行。3）在起點和終點之間的變換

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