1、第第5 5章章 軌跡規劃軌跡規劃（4學時）學時）學習目的：學習目的：1 理解軌跡規劃原理理解軌跡規劃原理2 學會用軌跡規劃處理實際問題學會用軌跡規劃處理實際問題學習內容：學習內容：1 軌跡規劃原理軌跡規劃原理 2 關節空間的軌跡規劃關節空間的軌跡規劃3 直角坐標空間的軌跡規劃直角坐標空間的軌跡規劃4 連續軌跡紀錄連續軌跡紀錄定義：定義：如果規定一個機器人從如果規定一個機器人從A點經過點經過B點運動到點運動到C點而不點而不強調時間的概念，那么這一過程中的位形序列就構強調時間的概念，那么這一過程中的位形序列就構成了一條成了一條路徑路徑。如果我們強調到達其中任意一點的。如果我們強調到達其中任意一點的

2、時間，那么這就是一條時間，那么這就是一條軌跡軌跡。我們可以看出軌跡和。我們可以看出軌跡和路徑的區別就在于路徑的區別就在于軌跡依賴速度和加速度軌跡依賴速度和加速度。5.1 路徑與軌跡路徑與軌跡5.2 關節空間描述與直角空間描述關節空間描述與直角空間描述1 關節空間描述關節空間描述如果給定機器人運動的起點和終點，就可以利用逆如果給定機器人運動的起點和終點，就可以利用逆運動學方程計算出每個關節的矢量角度值；然后機運動學方程計算出每個關節的矢量角度值；然后機器人控制器驅動關節電機運動使機器人到達相應的器人控制器驅動關節電機運動使機器人到達相應的位置。這種以關節角度的函數來描述機器人軌跡的位置。這種以關

3、節角度的函數來描述機器人軌跡的方法稱為關節空間法。方法稱為關節空間法。特點：特點：在機器人運動的過程中，中間狀態是不可知在機器人運動的過程中，中間狀態是不可知的，但計算量較小，不會出現奇異點的，但計算量較小，不會出現奇異點 。2 直角坐標空間描述直角坐標空間描述將軌跡分成若干段，使機器人的運動經過這些中間將軌跡分成若干段，使機器人的運動經過這些中間點，在每一點都求解機器人的關節變量，直到到達點，在每一點都求解機器人的關節變量，直到到達終點，如下圖所示：終點，如下圖所示：特點：特點：路徑可控且可預知，直觀、容易看到機器人路徑可控且可預知，直觀、容易看到機器人末端軌跡；但計算量大，容易出現奇異點末

4、端軌跡；但計算量大，容易出現奇異點，如下圖，如下圖所示：所示：直角空間描述直角空間描述軌跡穿過軌跡穿過機器人自機器人自身身關節值突變關節值突變5.3 軌跡規劃的基本原理軌跡規劃的基本原理1 關節空間的軌跡規劃關節空間的軌跡規劃 1. 計算起點和終點的關節變量，各關節都以最大角計算起點和終點的關節變量，各關節都以最大角 速度運動速度運動 特點：特點：軌跡不規則，末端走過的距離不均勻，且各軌跡不規則，末端走過的距離不均勻，且各關節不是同時到達。關節不是同時到達。AB2. 在在1的基礎上對關節速率做歸一化處理，使各關的基礎上對關節速率做歸一化處理，使各關節同時到達終點。節同時到達終點。特點：特點：各

5、關節同時到達終點，軌跡各部分比較均各關節同時到達終點，軌跡各部分比較均 衡，但所得路徑仍然是不規則的。衡，但所得路徑仍然是不規則的。AB二二 直角坐標空間軌跡規劃直角坐標空間軌跡規劃1. 首先畫出路徑，接著將路徑首先畫出路徑，接著將路徑n等分等分(為了獲得較好為了獲得較好的沿循精度，的沿循精度，n越大越好越大越好) ，分別計算到達各點所需，分別計算到達各點所需的關節變量。的關節變量。特點：特點：關節角非均勻變化，末端沿已知路徑行走。關節角非均勻變化，末端沿已知路徑行走。2. 在在1的基礎上，考慮各關節的加速減速時間，為的基礎上，考慮各關節的加速減速時間，為防止在加速期間軌跡落后于設想的軌跡，在

6、劃分分防止在加速期間軌跡落后于設想的軌跡，在劃分分界點時，如果是直線軌跡，就按照方程劃分。曲線界點時，如果是直線軌跡，就按照方程劃分。曲線軌跡就相對復雜一些。軌跡就相對復雜一些。 3. 多點的情況多點的情況（1）從）從A向向B先加速，再勻速，接近先加速，再勻速，接近B時再減速，時再減速，從從B到到C再重復。為避免這一過程中不必要的停再重復。為避免這一過程中不必要的停止動作，可將止動作，可將B點兩邊的動作進行平滑過渡。機點兩邊的動作進行平滑過渡。機器人先抵達器人先抵達B點，然后沿著平滑過渡的路徑重新點，然后沿著平滑過渡的路徑重新加速，最終抵達并停止在加速，最終抵達并停止在C點。點。（2）考慮到由

7、于采用了平滑過渡曲線，機器人經）考慮到由于采用了平滑過渡曲線，機器人經過的可能不是原來的過的可能不是原來的B點，可事先設定一個不同點，可事先設定一個不同的的B點，使曲線正好經過點，使曲線正好經過B點。點。(3) 在在B點前后各加過渡點點前后各加過渡點D,E，使得，使得B點落在點落在DE上。上。三三 軌跡規劃的分類軌跡規劃的分類v1) 對于點位作業機器人，需要描述它的起始狀態和對于點位作業機器人，需要描述它的起始狀態和目標狀態。如果用目標狀態。如果用 表示工具坐標系的起始值，表示工具坐標系的起始值， 表示目標值，就是表示這兩個值的相對關系。表示目標值，就是表示這兩個值的相對關系。 這種運動稱為點

8、到點運動這種運動稱為點到點運動(PTP)v2) 對于弧焊、研磨、拋光等曲面作業，不僅要規定對于弧焊、研磨、拋光等曲面作業，不僅要規定起始點和終止點，還要規定中間整個運動過程。對起始點和終止點，還要規定中間整個運動過程。對于一段連續運動過程，理論上無法精確實現，實際于一段連續運動過程，理論上無法精確實現，實際上是選取一定數量上是選取一定數量(滿足軌跡插補精度滿足軌跡插補精度)的點作為中的點作為中間點，從而近似實現沿給定的路徑運動。間點，從而近似實現沿給定的路徑運動。 這種運動稱為連續路徑運動或輪廓運動這種運動稱為連續路徑運動或輪廓運動(CP)v3) 障礙約束軌跡規劃障礙約束軌跡規劃0TfT5.4

9、 關節空間的軌跡規劃關節空間的軌跡規劃 0)(0)()()(322103fiffiitttttctctcct：這里初始和末端條件是一、一、 三次多項式的軌跡規劃三次多項式的軌跡規劃我們假設機器人某一關節的運動方程是三次的我們假設機器人某一關節的運動方程是三次的 從上例可以看出，若我們已知開始和終止時刻從上例可以看出，若我們已知開始和終止時刻的角度以及角速度，那么就可以求得的角度以及角速度，那么就可以求得 ，進而求，進而求得關節的運動方程。得關節的運動方程。ic 0320322211322100221322103333fffifffffiitCtCCtCttCtCtCCtCttctccttctc

10、tcct得到：將初始和末端條件代入求一階導數得到：對 盡管每一個關節都是分別計算的，但是在實際盡管每一個關節都是分別計算的，但是在實際控制中，所有關節自始至終都是同步運動；控制中，所有關節自始至終都是同步運動； 如果機器人初始和末端速度不為零，可以通過如果機器人初始和末端速度不為零，可以通過給定數據得到未知數值；給定數據得到未知數值； 如果要求機器末端人依次通過兩個以上的點，如果要求機器末端人依次通過兩個以上的點，則每一段求解出的邊界速度和位置均可作為下一段則每一段求解出的邊界速度和位置均可作為下一段的初始條件，其余相同；的初始條件，其余相同； 位置、速度連續，但是加速度不連續。位置、速度連續

11、，但是加速度不連續。例例5.1：已知一個關節在已知一個關節在5秒之內從初始角秒之內從初始角30度運動度運動到終端角到終端角75度，使用三次多項式計算在第度，使用三次多項式計算在第1，2，3，4秒時關節的角度。（我們假設在開始和終止的瞬秒時關節的角度。（我們假設在開始和終止的瞬間關節的速度是間關節的速度是0)解：由題意可得到解：由題意可得到0)5(3)5(2)(0)(75)5()5()5()(30)(232113322100ccctctcccctctfiofoi72. 04 . 50303210cccc由此得到位置，速度和加速度的多項式方程如下：由此得到位置，速度和加速度的多項式方程如下： tt

12、tttttt32. 48 .1016. 28 .1072. 04 . 530232 32.70)4(16.59) 3(84.45)2(68.34) 1 (我們可以進一步畫出關節的位置，速度和加速度曲線我們可以進一步畫出關節的位置，速度和加速度曲線可以看出，本例中需要的初始加速度為可以看出，本例中需要的初始加速度為10.8度度/秒秒2運動末端的角加速度為運動末端的角加速度為-10.8度度/秒秒2。例題：例題：在例在例5.1的基礎上繼續運動，要求在其后的的基礎上繼續運動，要求在其后的3秒內關節秒內關節角到達角到達 。畫出該運動的位置，速度和加速度曲線。畫出該運動的位置，速度和加速度曲線。105思路

13、點撥：思路點撥：可將第一運動段末端的關節位置和速度可將第一運動段末端的關節位置和速度作為下一運動段的初始條件。作為下一運動段的初始條件。解：解：tttttttt332.1320)(666. 620)(222. 21075)(232 進而可以畫出以下曲線進而可以畫出以下曲線 可以求得其中3062)(32322213221033fitttCCttCtCCttCtCtCCt 10575fi00fi為保證為保證機器人機器人的加速的加速度不超度不超過其自過其自身能力，身能力，應考慮應考慮加速度加速度的限制。的限制。2max)()(4ififtt 根據此式可計算出達到目標所需根據此式可計算出達到目標所需要

14、的時間要的時間二、二、 五次多項式軌跡規劃五次多項式軌跡規劃 同例同例5.1，若采用，若采用五五次多項式，若再已知初始次多項式，若再已知初始加速度和末端減速度均為加速度和末端減速度均為5 度度/秒秒2 ，其他條件不變，其他條件不變，試畫出三條相應曲線。（邊界條件變為試畫出三條相應曲線。（邊界條件變為6個）個）352432453423215544332210201262)(5432)()(tctctccttctctctccttctctctctcct 根據這些方程，可以通過位置、速度和加速度根據這些方程，可以通過位置、速度和加速度邊界條件計算出五次多項式的系數。邊界條件計算出五次多項式的系數。35

15、2432453423215544332210201262)(5432)()(tctctccttctctctccttctctctctcct 32432543230928. 096. 66 . 95)(232. 032. 28 . 45)(0464. 058. 06 . 15 . 230)(6 . 1307530ttttttttttttttccfoi ：進而得到如下運動方程中，得出：將初始和末端條件代入秒度秒度/0/0fi22/5/5秒度秒度fi 58. 0041cc0464. 05 . 252cc關節位置、速度和加速度圖形關節位置、速度和加速度圖形三、拋物線過渡的線性運動軌跡三、拋物線過渡的線性

16、運動軌跡 如果機器人關節以恒定速度運動，那么軌跡方程就相當于如果機器人關節以恒定速度運動，那么軌跡方程就相當于一次多項式，其速度是常數，加速度為一次多項式，其速度是常數，加速度為0，這說明在起點和終，這說明在起點和終點，加速度為無窮大，只有這樣才可以瞬間達到勻速狀態。但點，加速度為無窮大，只有這樣才可以瞬間達到勻速狀態。但很顯然這是不可能的，因此在起點和終點處，可以用拋物線來很顯然這是不可能的，因此在起點和終點處，可以用拋物線來進行過渡。如圖所示進行過渡。如圖所示22222102102212210)()(21)(0)(0)0()0()()(21)(cttcttctcccctctctcttcct

17、tctcctiii 顯然，這個拋物線運動段的加速度是一常數，并在公共點顯然，這個拋物線運動段的加速度是一常數，并在公共點A和和B上產生連續的速度。將邊界條件代入拋物線段的方程，上產生連續的速度。將邊界條件代入拋物線段的方程，得到：得到：從而給出拋物線段的方程為：從而給出拋物線段的方程為：假設假設ti和和 tf時刻對應的起點和終點時刻對應的起點和終點位置為位置為 和和 ，拋物線與直線，拋物線與直線部分的過渡段在時間部分的過渡段在時間tb和和tf-tb處是處是對稱的，因此可得：對稱的，因此可得：if0221222fiABfABbfAbbfABbAbiAttttttctc顯然，對于直線段，速度將保持

18、為常值，它可以根據顯然，對于直線段，速度將保持為常值，它可以根據驅動器的物理性能來加以選擇。將零出速度、線性段驅動器的物理性能來加以選擇。將零出速度、線性段常值速度常值速度 以及零末端速度代入以及零末端速度代入 和和 中，中，可以得到可以得到A、B點以及終點的關節位置和速度如下：點以及終點的關節位置和速度如下： 2221)(tctitct2)(顯然，顯然， 不能大于總時間不能大于總時間 的一半，否則在整個過程中的一半，否則在整個過程中將沒有直線運動段而只有拋物線加速和拋物線減速段。將沒有直線運動段而只有拋物線加速和拋物線減速段。由上式可以計算出對應的最大速度由上式可以計算出對應的最大速度 。應

19、該。應該說明，如果運動段的初始時間不是說明，如果運動段的初始時間不是0而是而是 ，則可采用，則可采用平移時間軸的辦法使初始時間為平移時間軸的辦法使初始時間為0。終點的拋物線段是。終點的拋物線段是對稱的，只是其加速度為負。因此可表示為：對稱的，只是其加速度為負。因此可表示為：bfbbifbfbifbtttttttctc222222fift / )(2max由上式可以求解過渡時間：由上式可以求解過渡時間：進而由上式可以解得過渡時間：進而由上式可以解得過渡時間：ffibttbtftat bfftcttct22221其中bfbfbftttttttttt)()()()(2)(2 例題：例題：若已知某關節

20、以速度若已知某關節以速度 =10度度/秒在秒在5秒內從秒內從初始角初始角 運動到目的角運動到目的角 。求解所需的過渡。求解所需的過渡時間并繪制位置、速度和加速度曲線。時間并繪制位置、速度和加速度曲線。130i70f解：代入相應公式可得到解：代入相應公式可得到1010530,12 ttsttfBBAAiffib的方程如下所示到由到由到由01010 tA10)5(10)5(5702 tt曲線如下圖所示：曲線如下圖所示：5.5 直角坐標空間的軌跡規劃直角坐標空間的軌跡規劃 實際上，所有用于關節空間軌跡規劃的方法實際上，所有用于關節空間軌跡規劃的方法都可用于直角坐標空間的軌跡規劃。最根本的差都可用于直

21、角坐標空間的軌跡規劃。最根本的差別在于，直角坐標空間軌跡規劃必須反復求解逆別在于，直角坐標空間軌跡規劃必須反復求解逆運動學方程來計算關節角，也就是說，對于關節運動學方程來計算關節角，也就是說，對于關節空間軌跡規劃，規劃函數生成的值就是關節值，空間軌跡規劃，規劃函數生成的值就是關節值，而直角坐標空間軌跡規劃函數生成的值是機器人而直角坐標空間軌跡規劃函數生成的值是機器人末端手的位姿，他們需要通過求解逆運動學方程末端手的位姿，他們需要通過求解逆運動學方程化為關節量。化為關節量。 以上過程可以簡化為如下的計算循環：以上過程可以簡化為如下的計算循環： 在工業應用中，最實用的軌跡是點到點之間的直在工業應用

22、中，最實用的軌跡是點到點之間的直線運動，但也經常碰到多目標點線運動，但也經常碰到多目標點(例如有中間點例如有中間點)間需間需要平滑過渡的情況。要平滑過渡的情況。 為實現一條直線軌跡，必須計算起點和終點位姿為實現一條直線軌跡，必須計算起點和終點位姿之間的變換，并將該變換劃分許多小段。起點構型之間的變換，并將該變換劃分許多小段。起點構型(1) 將時間增加一個增量將時間增加一個增量t=t+ (2) 利用所選擇的軌跡函數計算出手的位姿利用所選擇的軌跡函數計算出手的位姿(3) 利用機器人逆運動學方程計算出對應手位姿利用機器人逆運動學方程計算出對應手位姿的關節量的關節量(4) 將關節信息送給控制器將關節信

23、息送給控制器t和終點構型和終點構型 之間的總變換之間的總變換R可通過下面的方程進行可通過下面的方程進行計算：計算：iTfiiifiifTTRRTTTTRTT111 至少有以下三種不同方法可用來將該總變換化為至少有以下三種不同方法可用來將該總變換化為許多的小段變換。許多的小段變換。 (1) 希望在起點和終點之間有平滑的線性變換，因希望在起點和終點之間有平滑的線性變換，因此需要大量很小的分段，從而產生了大量的微分運動。此需要大量很小的分段，從而產生了大量的微分運動。利用上一章導出的微分運動方程，可將末端手坐標系利用上一章導出的微分運動方程，可將末端手坐標系在每個新段的位姿與微分運動、雅可比矩陣及關

24、節速在每個新段的位姿與微分運動、雅可比矩陣及關節速度通過下列方程聯系在一起。度通過下列方程聯系在一起。dTTTTdTJDDoldnewDJD1這一方法需要進行大量的計算，并且僅當雅可比矩這一方法需要進行大量的計算，并且僅當雅可比矩陣逆存在時才有效。陣逆存在時才有效。(2) 在起點和終點之間的變換分解為一個平移和兩在起點和終點之間的變換分解為一個平移和兩個旋轉。平移是將坐標原點從起點移動到終點，第個旋轉。平移是將坐標原點從起點移動到終點，第一個旋轉是將末端手坐標系與期望姿態對準，而第一個旋轉是將末端手坐標系與期望姿態對準，而第二個旋轉是手坐標系繞其自身周轉到最終的姿態。二個旋轉是手坐標系繞其自身

25、周轉到最終的姿態。所有這所有這3個變換同時進行。個變換同時進行。(3) 在起點和終點之間的變換在起點和終點之間的變換R分解為一個平移和一個分解為一個平移和一個K軸的旋轉。平移仍是將坐標原點從起點移動到終點，軸的旋轉。平移仍是將坐標原點從起點移動到終點，而旋轉則是將手臂坐標系與最終的期望姿態對準。兩而旋轉則是將手臂坐標系與最終的期望姿態對準。兩個變換同時進行。個變換同時進行。6 . 78 . 08 . 0381014814xyxy例例5.6 一個兩自由度平面機器人要求從起點一個兩自由度平面機器人要求從起點(3,10)沿直沿直線運動到終點線運動到終點(8,14)。假設路徑分為。假設路徑分為10段，

26、求出機器段，求出機器人的關節變量。每一根連桿的長度為人的關節變量。每一根連桿的長度為9英寸。英寸。解：直角坐標空間中起點和終點間的直線可描述為：解：直角坐標空間中起點和終點間的直線可描述為： 中間點的坐標可以通過將起點和終點的中間點的坐標可以通過將起點和終點的x,y坐標坐標之差簡單地加以分割得到，然后通過求解逆運動學方之差簡單地加以分割得到，然后通過求解逆運動學方程得到對應每個中間點的兩個關節角。程得到對應每個中間點的兩個關節角。或者或者#XY131018.810923.510.419104.03410.819.5100.444.511.220.295.85511.621.390.965.51

27、222.585.77612.424.180.186.512.82674.29713.228.267.8107.513.630.860.71181433.952.812 133312221311181arccos162162arccosarctanCCSPCCPPCPPPyZzyyx例：例：一個一個3自由度機器人有兩根連桿，每根連桿長自由度機器人有兩根連桿，每根連桿長9英寸。假設定義坐標系使得當所有關節角均為英寸。假設定義坐標系使得當所有關節角均為0時手時手臂處于垂直向上狀態。要求機器人沿直線從點（臂處于垂直向上狀態。要求機器人沿直線從點（9，6，10）移動到點（）移動到點（3，5，8）。求）。求3各關節在每個