《機器人》第5章-軌跡規(guī)劃

第5章軌跡規(guī)劃在前面的機器人運動學(xué)分析中：引入齊次坐標，將機器人位置和姿態(tài)有效表達；并將機器人桿件與運動坐標系相固連—將機器人運動轉(zhuǎn)化引入相對參考坐標系的絕對運動和相對運動坐標系的相對運動—機器人空間位姿和關(guān)節(jié)電機控制關(guān)節(jié)空間已知機器人關(guān)節(jié)變量求得機器人位姿；給定機器人位姿求得各關(guān)節(jié)變量進而控制機器人到達給定位姿機器人求解問題：給定一個位姿到達新的位姿—中間怎么辦？控制電機轉(zhuǎn)動方式—啟動、停車、通過中間點等電機轉(zhuǎn)動角度、速度等與位姿及微分變化關(guān)系？？本章學(xué)習(xí)目的：1理解軌跡規(guī)劃原理2學(xué)會用軌跡規(guī)劃處理實際問題本章學(xué)習(xí)內(nèi)容：1軌跡規(guī)劃原理2關(guān)節(jié)空間的軌跡規(guī)劃3直角坐標空間的軌跡規(guī)劃4連續(xù)軌跡紀錄定義：如果規(guī)定一個機器人從A點經(jīng)過B點運動到C點而不強調(diào)時間的概念，那么這一過程中的位形序列就構(gòu)成了一條路徑。如果我們強調(diào)到達其中任意一點的時間，那么這就是一條軌跡。我們可以看出軌跡和路徑的區(qū)別就在于軌跡依賴速度和加速度。5.1路徑與軌跡5.2關(guān)節(jié)空間描述與直角空間描述

如果給定機器人運動起點和終點，就可以利用逆運動學(xué)方程計算出每個關(guān)節(jié)對應(yīng)的角度值；然后機器人控制器驅(qū)動關(guān)節(jié)電機運動使機器人到達相應(yīng)的關(guān)節(jié)值—對應(yīng)位置。

以關(guān)節(jié)參數(shù)描述機器人運動的方法稱為關(guān)節(jié)空間法。

如果只給定初始和終止位姿狀態(tài)？

特點：給定機器人位姿狀態(tài)是在直角坐標空間，計算轉(zhuǎn)化為關(guān)節(jié)空間—控制電機運動參數(shù)

。機器人具體實現(xiàn)兩點運動方法如下圖所示：1將軌跡分成若干段，使機器人的運動經(jīng)過這些中間點；2每一點都求解機器人的運動方程得到系列關(guān)節(jié)變量；3控制器驅(qū)動機器人關(guān)節(jié)到達下一位姿直到到達終點。

特點：

1機器人位姿系列狀態(tài)已知；2該狀態(tài)在直角坐標空間；3轉(zhuǎn)化到關(guān)節(jié)空間計算并控制4直角坐標空間計算大于關(guān)節(jié)空間。解決辦法：

1指定機器人必須通過避開障礙物或奇異點的中間點2每一步都走一微步，中間不會出現(xiàn)奇異點，或可加閾值判斷軌跡穿過機器人自身關(guān)節(jié)值突變5.3軌跡規(guī)劃的基本原理一關(guān)節(jié)空間軌跡規(guī)劃1.已知起點和終點的關(guān)節(jié)變量，各關(guān)節(jié)運動的最大角速度。特點：軌跡不規(guī)則，末端走過的距離不均勻，且各關(guān)節(jié)不是同時到達。AB2.在1的基礎(chǔ)上對關(guān)節(jié)速率做歸一化處理，兩個關(guān)節(jié)以不同速度運動，使各關(guān)節(jié)同時到達終點。特點：各關(guān)節(jié)同時到達終點，軌跡各部分比較均衡，但所得路徑仍然是不規(guī)則的。AB二直角坐標空間軌跡規(guī)劃1.首先畫出路徑，接著將路徑n等分(為了獲得較好的沿循精度，n越大越好)，分別計算到達各點所需的關(guān)節(jié)變量。特點：關(guān)節(jié)角非均勻變化，末端沿已知路徑行走。前面幾種情況的角加速度？（無窮大—需動力同樣）2.考慮各關(guān)節(jié)的加速減速時間，為防止動力原因在加速期間軌跡落后于設(shè)想的軌跡，在劃分路徑段時：（1）加速階段路徑分段較小，由于加速度使路徑分段逐漸變大直到最大；（2）以最大路徑分段恒速運動到減速停車階段；

（3）在減速停車階段路徑分段在以加速度相同值的減速度下逐漸變小直至為零。S=0.5×at2多點的情況（ABC）1從A向B先加速，再勻速，接近B時再減速，從B到C再重復(fù)。為避免這一過程中不必要的停止動作，可將B點兩邊的動作進行平滑過渡。機器人先抵達B點，然后沿著平滑過渡的路徑重新加速，最終抵達并停止在C點。特點：由于采用了平滑過渡，機器人運動經(jīng)過的不是原所要求的B點，而是圖中的Bˊ點。（如何解決？）2考慮到由于采用了平滑過渡曲線，機器人經(jīng)過的可能不是原來的B點，可事先設(shè)定一個不同的B〞點，使平滑過渡曲線正好經(jīng)過B點。(難度大)3在B點前后各加過渡點D、E，使得B點落在DE連線上。

如果機器人路徑非常復(fù)雜，無法用方程表示，無法進行逆運動學(xué)求解，則可用手動示教方式，將示教記錄下的每個關(guān)節(jié)狀態(tài)值代替逆運動學(xué)求解值用于驅(qū)動機器人的運動。四軌跡規(guī)劃的分類1對于點位作業(yè)機器人，需要描述它的起始狀態(tài)和目標狀態(tài)。如果用表示工具坐標系的起始值，表示目標值，就是表示這兩個值的相對關(guān)系。這種運動稱為點到點運動(PTP)2對于弧焊、研磨、拋光等曲面作業(yè)，不僅要規(guī)定起始點和終止點，還要規(guī)定中間整個運動過程。對于一段連續(xù)運動過程，理論上無法精確實現(xiàn)，實際上是選取一定數(shù)量(滿足軌跡插補精度)的點作為中間點，從而近似實現(xiàn)沿給定的路徑運動。這種運動稱為連續(xù)路徑運動或輪廓運動(CP)3障礙約束軌跡規(guī)劃5.4關(guān)節(jié)空間的軌跡規(guī)劃一、三次多項式的軌跡規(guī)劃假設(shè)機器人某一關(guān)節(jié)的運動方程是三次的注意：這里均為關(guān)節(jié)量而不是笛卡爾坐標空間量

從上例可以看出，若我們已知開始和終止時刻的角度以及角速度，那么就可以求得，進而求得關(guān)節(jié)的運動方程。三次多項式軌跡規(guī)劃特點：

1盡管每一個關(guān)節(jié)都是分別計算的，但是在實際控制中，所有關(guān)節(jié)自始至終都是同步運動；

2如果機器人初始和末端速度不為零，可以通過給定數(shù)據(jù)得到未知數(shù)值；

3如果要求機器末端人依次通過兩個以上的點，則每一段求解出的邊界速度和位置均可作為下一段的初始條件，其余相同；

4位置、速度連續(xù)，但是加速度不連續(xù)。例5.1：已知一個關(guān)節(jié)在5秒之內(nèi)從初始角30度運動到終端角75度，使用三次多項式計算在第1，2，3，4秒時關(guān)節(jié)的角度。（假設(shè)在開始和終止的瞬間關(guān)節(jié)的速度是0)解：由題意可得到由此得到位置，速度和加速度的多項式方程：我們可以進一步畫出關(guān)節(jié)的位置，速度和加速度曲線可以看出，本例中需要的初始加速度為10.8度/秒2運動末端的角加速度為-10.8度/秒2。例題：在例5.1的基礎(chǔ)上繼續(xù)運動，要求在其后的3秒內(nèi)關(guān)節(jié)角到達。畫出該運動的位置，速度和加速度曲線。思路點撥：可將第一運動段末端的關(guān)節(jié)位置和速度作為下一運動段的初始條件。解：進而可以畫出以下曲線為保證機器人的加速度不超過其自身能力，應(yīng)考慮加速度的限制。根據(jù)此式可計算出達到目標所需要的時間二、五次多項式軌跡規(guī)劃

同例5.1，若采用五次多項式，若再已知初始加速度和末端減速度均為5度/秒2

，其他條件不變，試畫出三條相應(yīng)曲線。（邊界條件變?yōu)?個）

根據(jù)這些方程，可以通過位置、速度和加速度邊界條件計算出五次多項式的系數(shù)。關(guān)節(jié)位置、速度和加速度圖形三、拋物線過渡的線性運動軌跡

如果機器人關(guān)節(jié)以恒定速度運動，那么軌跡方程就相當(dāng)于一次多項式，其速度是常數(shù)，加速度為0，這說明在起點和終點，加速度為無窮大，只有這樣才可以瞬間達到勻速狀態(tài)。但很顯然這是不可能的，因此在起點和終點處，可以用拋物線來進行過渡。如圖所示顯然，這個拋物線運動段的加速度是一常數(shù)，并在公共點A和B上產(chǎn)生連續(xù)的速度。將邊界條件代入拋物線段的方程，得到：從而給出拋物線段的方程為：假設(shè)ti和

tf時刻對應(yīng)的起點和終點位置為和，拋物線與直線部分的過渡段在時間tb和tf-tb處是對稱的，因此可得：顯然，對于直線段，速度將保持為常值，它可以根據(jù)驅(qū)動器的物理性能來加以選擇。將零點速度、線性段常值速度以及零末端速度代入和中，可以得到A、B點以及終點的關(guān)節(jié)位置和速度如下：

顯然，不能大于總時間的一半，否則在整個過程中將沒有直線運動段而只有拋物線加速和拋物線減速段。由上式可以計算出對應(yīng)的最大速度。應(yīng)該說明，如果運動段的初始時間不是0而是，則可采用平移時間軸的辦法使初始時間為0。終點的拋物線段是對稱的，只是其加速度為負。由上式可以求解過渡時間：進而由上式可以解得過渡時間：因此可表示為：例5.4在例5.1中，機器人關(guān)節(jié)1以速度=10度/秒在5秒內(nèi)從初始角運動到目的角。求解所需的過渡時間并繪制位置、速度和加速度曲線。解：代入相應(yīng)公式可得到（tf=5秒）曲線如下圖所示：四、具有中間點及用拋物線過渡的線性運動規(guī)劃

實際應(yīng)用的機器人工作經(jīng)常是多個運動段組成，第一個運動段末端點后，還將繼續(xù)向下一點運動，因此這一點可能是起點、中間點、終止點，必須采用各運動段間過渡方法解決時停時走運動問題。

實際上，把所有中間路徑點既看作“下一段起始點”也看做“上一段終止點”，相對應(yīng)可以通過運動規(guī)劃函數(shù)求出該點的直角坐標空間的“位置”“速度”插值分量以及該點的關(guān)節(jié)坐標空間的“位置”“速度”插值分量，將所有這些插值分量連接起來就得到直角坐標空間的機器人路徑和關(guān)節(jié)坐標空間的關(guān)節(jié)變化。但在這些“起始點”“終止點”關(guān)節(jié)運動速度不是零。在各段之間過渡時，使用每一點的邊界條件計算拋物線系數(shù)。

例如：已知機器人開始運動關(guān)節(jié)位置、速度，并且在第一運動段末端點位置、速度必須連續(xù)，則將它們作為中間點的邊界條件，進而對新的運動段進行求解計算，重復(fù)這一過程直至計算出所有運動段并到達終點。

注意：1對于每個運動段，必須基于給定的關(guān)節(jié)速度求出新的過渡時間tb;2檢驗加速度值是否滿足限定要求。五、具有多個中間點的運動規(guī)劃

除了指定起點和終點外，當(dāng)要求通過其它若干中間點時，理論上可以通過匹配這些點的位置、速度和加速度規(guī)劃出一條連續(xù)的軌跡（三次可解決兩點速度層次、五次可解決兩點加速度層次）。但是：1過多點的高次多項式方程求解需要大量的計算；2機器人工作經(jīng)常由于路徑要求而無法連續(xù)運動。

實際上，是把軌跡不同的運動段采用不同的低次多項式，然后將它們按滿足各點的邊界條件有機地連接起來（或平滑連接；或分成不同的運動段）。5.5直角坐標空間的軌跡規(guī)劃

所有用于關(guān)節(jié)空間軌跡規(guī)劃的方法都可用于直角坐標空間的軌跡規(guī)劃。最根本的差別在于，直角坐標空間軌跡規(guī)劃必須反復(fù)求解逆運動學(xué)方程來計算關(guān)節(jié)角，也就是說，對于關(guān)節(jié)空間軌跡規(guī)劃，規(guī)劃函數(shù)生成的值就是關(guān)節(jié)值，而直角坐標空間軌跡規(guī)劃函數(shù)生成的值是機器人末端手的位姿，他們需要通過求解逆運動學(xué)方程化為關(guān)節(jié)量。以上過程可以簡化為如下的計算循環(huán)：1將時間增加一個增量t=t+2利用所選擇的軌跡函數(shù)計算出手的位姿3利用機器人逆運動學(xué)方程計算出對應(yīng)手位姿的關(guān)節(jié)量4將關(guān)節(jié)信息送給控制器最實用的軌跡是點到點之間的直線運動，但也經(jīng)常碰到多目標點(例如有中間點)間需要平滑過渡的情況。為實現(xiàn)一條直線軌跡，必須計算起點和終點位姿之間的變換，并將該變換劃分許多小段。起點構(gòu)型和終點構(gòu)型之間的總變換R可通過下面的方程進行計算：

至少有以下三種不同方法可用來將該總變換化為許多的小段變換。

(1)希望在起點和終點之間有平滑的線性變換，因此需要大量很小的分段，從而產(chǎn)生了大量的微分運動。利用上一章導(dǎo)出的微分運動方程，可將末端手坐標系在每個新段的位姿與微分運動、雅可比矩陣及關(guān)節(jié)速度通過下列方程聯(lián)系在一起。這一方法需要進行大量的計算，并且僅當(dāng)雅可比矩陣逆存在時才有效。(2)在起點和終點之間的變換分解為一個平移和兩個旋轉(zhuǎn)。平移是將坐標原點從起點移動到終點，第一個旋轉(zhuǎn)是將末端手坐標系與期望姿態(tài)對準，而第二個旋轉(zhuǎn)是手坐標系繞其自身周轉(zhuǎn)到最終的姿態(tài)。所有這3個變換同時進行。(3)在起點和終點之間的變換R分解為一個平移和一個K軸的旋轉(zhuǎn)。平移仍是將坐標原點從起點移動到終點，而旋轉(zhuǎn)則是將手臂坐標系與最終的期望姿態(tài)對準。兩個變換同時進行。例5.6

一個兩自由度平面機器人要求從起點(3,10)沿直線運動到終點(8,14)。假設(shè)路徑分為10段，求出機器人的關(guān)節(jié)變量。每一根連桿的長度為9英寸。解：直角坐標空間中起點和終點間的直線可描述為：

中間點的坐標可以通過將起點和終點的x,y坐標之差簡單地加以分割得到，然后通過求解逆運動學(xué)方程得到對應(yīng)每個中間點的兩個關(guān)節(jié)角。或者#XY131018.810923.510.419104.03410.819.5100.444.511.220.295.85511.621.390.965.51222.585.77612.424.180.186.512.82674.29713.228.267.8107.513.