第7章 機器人軌跡規(guī)劃

1、機器人學基礎機器人學基礎國家級國家級智能科學基礎系列課程教學團隊智能科學基礎系列課程教學團隊“機器人學機器人學”課程配套教材課程配套教材蔡自興蔡自興 主編主編20092第七章 機器人軌跡規(guī)劃所謂軌跡，是指機械手在運動過程中的位移、速度和加速度。而軌跡規(guī)劃是根據(jù)作業(yè)任務的要求，計算出預期的運動軌跡。機器人學基礎7.1 軌跡規(guī)劃應考慮的問題路徑約束和障礙約束的組合把機器人的規(guī)劃與控制方式劃分為四類，如表7.1所示。表7.1 操作臂控制方式障礙約束有無路徑約束有 離線無碰撞路徑規(guī)劃在線路徑跟蹤 離線路徑規(guī)劃在線路徑跟蹤 無 位置控制在線障礙探測和避障 位置控制 第七章 機器人軌跡規(guī)劃37.1 軌跡規(guī)

2、劃應考慮的問題軌跡規(guī)劃器可形象地看成為一個黑箱（見圖7.1），其輸入包括路徑的設定和約束，輸出的是機械手末端手部的位姿序列，表示手部在各離散時刻的中間位形 。第七章 機器人軌跡規(guī)劃7.1圖 軌跡規(guī)劃器框圖47.1 軌跡規(guī)劃應考慮的問題機械手最常用的軌跡規(guī)劃方法有兩種：第一種方法要求用戶對于選定的轉(zhuǎn)變結(jié)點（插值點）上的位姿、速度和加速度給出一組顯式約束（例如連續(xù)性和光滑程度等），軌跡規(guī)劃器從一類函數(shù)（例如n次多項式）中選取參數(shù)化軌跡，對結(jié)點進行插值，并滿足約束條件。第二種方法要求用戶給出運動路徑的解析式；如為直角坐標空間中的直線路徑，軌跡規(guī)劃器在關節(jié)空間或直角坐標空間中確定一條軌跡來逼近預定的路

3、徑。第七章 機器人軌跡規(guī)劃57.1 軌跡規(guī)劃應考慮的問題在第一種方法中，約束的設定和軌跡規(guī)劃均在關節(jié)空間進行。因此可能會發(fā)生與障礙物相碰。第二種方法的路徑約束是在直角坐標空間中給定的，而關節(jié)驅(qū)動器是在關節(jié)空間中受控的。第七章 機器人軌跡規(guī)劃67.2 關節(jié)軌跡的插值計算對關節(jié)進行插值時，應滿足一系列的約束條件，例如抓取物體時，手部運動方向（初始點），提升物體離開的方向（提升點），放下物體（下放點）和停止點等結(jié)點上的位姿、速度和加速度的要求；與此相應的各個關節(jié)位移、速度、加速度在整個時間間隔內(nèi)連續(xù)性要求；其極值必須在各個關節(jié)變量的容許范圍之內(nèi)等。在滿足所要求的約束條件下，可以選取不同類型的關節(jié)插值

4、函數(shù)，生成不同的軌跡。第七章 機器人軌跡規(guī)劃77.2.1 三次多項式插值運動軌跡的描述可用起始點關節(jié)角度與終止點關節(jié)角度的一個平滑插值函數(shù)來表示，在t00時刻的值是起始關節(jié)角度，在終端時刻tf的值是終止關節(jié)角度。顯然，有許多平滑函數(shù)可作為關節(jié)插值函數(shù)，如圖7.2所示。7.2 關節(jié)軌跡的插值計算7.2圖 單個關節(jié)的不同軌跡曲線87.2.1 三次多項式插值為了實現(xiàn)單個關節(jié)的平穩(wěn)運動，軌跡函數(shù)至少需要滿足四個約束條件。(7.1)(7.2)fft)()0(00)(0)0(ft97.2 關節(jié)軌跡的插值計算(7.3)(7.4)332210)(tatataattaattataat32232162)(32)(

5、 求解約束條件可得：(7.6)(2)(30033022100fffftataaa107.2.1 三次多項式插值7.2 關節(jié)軌跡的插值計算7.2.2 過路徑點的三次多項式插值可以把所有路徑點也看作是“起始點”或“終止點”，求解逆運動學，得到相應的關節(jié)矢量值。然后確定所要求的三次多項式插值函數(shù)，把路徑點平滑地連接起來。但是，在這些“起始點”和“終止點”的關節(jié)運動速度不再是零。117.2 關節(jié)軌跡的插值計算路徑點上的關節(jié)速度可以根據(jù)需要設定，這樣一來，確定三次多項式的方法與前面所述的完全相同，只是速度約束條件(7.2)變?yōu)榍蟮萌味囗検降南禂?shù)：(7.7)fft)()0(0127.2.2 過路徑點的三

6、次多項式插值7.2 關節(jié)軌跡的插值計算(7.9)(1)(212)(3003300220100fffffffffttatttaaa如何確定路徑點上的關節(jié)速度可由以下三種方法規(guī)定：在直角坐標空間或關節(jié)根據(jù)工具坐標系在直角坐標空間中的瞬時線速度和角速度來確定每個路徑點的關節(jié)速度?？臻g中采用適當?shù)膯l(fā)式方法，由控制系統(tǒng)自動地選擇路徑點的速度。為了保證每個路徑點上的加速度連續(xù)，由控制系統(tǒng)按此要求自動地選擇路徑點的速度。137.2.2 過路徑點的三次多項式插值7.2 關節(jié)軌跡的插值計算7.2.3 高階多項式插值如果對于運動軌跡的要求更為嚴格，約束條件增多，則必須用更高階的多項式對運動軌跡的路徑段進行插值。

7、例如，對某段路徑的起始點和終止點都規(guī)定了關節(jié)的位置、速度和加速度，則要用一個五次多項式進行插值，即(7.10)5544332210)(tatatatataat147.2 關節(jié)軌跡的插值計算多項式的系數(shù)a0, a1, , a5必須滿足6個約束條件：(7.11)35243220453423211055443322100020126225432ffffffffffffffftatataaatatatataaatatatatataaa 157.2.3 高階多項式插值7.2 關節(jié)軌跡的插值計算線性方程組的解為：(7.12)320004320043200030201002)()66(12122)23()1

8、614(30302)3()128(20202ffffffffffffffffffftttatttatttaaaa 167.2.3 高階多項式插值7.2 關節(jié)軌跡的插值計算7.2.4 用拋物線過渡的線性插值線性函數(shù)與兩段拋物線函數(shù)平滑地銜接在一起形成的軌跡稱為帶有拋物線過渡域的線性軌跡。如圖7.3(a)所示。7.3圖 帶拋物線過渡的線性插值(a) 含有一個解177.2 關節(jié)軌跡的插值計算為了構(gòu)這段運動軌跡，假設兩端的過渡域（拋物線）具有相同的持續(xù)時間，因而在這兩個域中采用相同的恒加速度值，只是符號相反。正如圖7.3(b)所示，存在有多個解，得到的軌跡不是唯一的。(b) 含有多個解7.3圖 帶拋物

9、線過渡的線性插值187.2.4 用拋物線過渡的線性插值7.2 關節(jié)軌跡的插值計算令t=2th，據(jù)式(7.13)和(7.14)可得bhbhtbtt 2021bbt (7.13)(7.14)0)(02fbbttt (7.15)197.2.4 用拋物線過渡的線性插值7.2 關節(jié)軌跡的插值計算按式(7.15)算出相應的tb由上式可知，為保證tb有解，過渡域加速度值 必須選得足夠大，即 2)(42022fbttt(7.16)20)(4tf (7.17)207.2 關節(jié)軌跡的插值計算7.2.4 用拋物線過渡的線性插值7.2.5 過路徑點的用拋物線過渡的線性插值如圖7.4所示，某個關節(jié)在運動中設有n個路徑點

10、，其中三個相鄰的路徑點表示為j, k和l，每兩個相鄰的路徑點之間都以線性函數(shù)相連，而所有路徑點附近則由拋物線過渡。7.4圖 多段帶有拋物線過渡的線性插值軌跡217.2 關節(jié)軌跡的插值計算7.2.5 過路徑點的用拋物線過渡的線性插值22kjdjkjkkjkklkkjkklkdjkjkjktttttt2121)sgn( (7.18)7.2 關節(jié)軌跡的插值計算7.2.5 過路徑點的用拋物線過渡的線性插值第一個路徑段和最后一個路徑段的處理與上式(7.18)略有不同，因為軌跡端部的整個過渡域的持續(xù)時間都必須計入這一路徑段內(nèi)。對于一個路徑段，令線性域速度的兩個表達式相等，就可求出t1。23(7.19)11

11、1121221tttd 7.2 關節(jié)軌跡的插值計算7.2.5 過路徑點的用拋物線過渡的線性插值算出起始點過渡域的持續(xù)時間t1之后，進而求出 和t12： 12211212112121211221212111212121)(2)sgn(tttttttttdddd (7.20)247.2 關節(jié)軌跡的插值計算7.2.5 過路徑點的用拋物線過渡的線性插值對于最后一個路徑段，路徑點n-1與終止點n之間的參數(shù)與第一個路徑段相似，即 根據(jù)式(7.21)便可求出：nnnnndnnttt 21)1(1(7.21)257.2 關節(jié)軌跡的插值計算(7.22)1)1()1()1(1)1(112)1()1(12121)(

12、2)sgn(nnnndnnnnndnnnnnnnnndnndnnnnnttttttttt 7.3 笛卡兒路徑軌跡規(guī)劃物體對象的描述物體空間的描述方法，任一剛體相對參考系的位姿是用與它固接的坐標系來描述的。相對于固接坐標系，物體上任一點用相應的位置矢量p表示；任一方向用方向余弦表示。給出物體的幾何圖形及固接坐標系后，只要規(guī)定固接坐標系的位姿，便可重構(gòu)該物體。如圖7.5所示。7.5圖 對象的描述第七章 機器人軌跡規(guī)劃267.3 笛卡兒路徑軌跡規(guī)劃作業(yè)的描述作業(yè)和機械手的運動可用手部位姿結(jié)點序列來規(guī)定，每個結(jié)點是由工具坐標系相對于作業(yè)坐標系的齊次變換來描述。相應的關節(jié)變量可用運動學反解程序計算。如圖

13、7.6。第七章 機器人軌跡規(guī)劃27作業(yè)的描述287.6圖 作業(yè)的描述7.3 笛卡兒路徑軌跡規(guī)劃7.3 笛卡兒路徑軌跡規(guī)劃兩個結(jié)點之間的“直線”運動機械手在完成作業(yè)時，夾手的位姿可用一系列結(jié)點Pi來表示。因此，在直角坐標空間中進行軌跡規(guī)劃的首要問題是由兩結(jié)點Pi和Pi+1所定義的路徑起點和終點之間，如何生成一系列中間點。兩結(jié)點之間最簡單的路徑是在空間的一個直線移動和繞某定軸的轉(zhuǎn)動。若運動時間給定之后，則可以產(chǎn)生一個使線速度和角速度受控的運動。第七章 機器人軌跡規(guī)劃29兩段路徑之間的過渡為了避免兩段路徑銜接點處速度不連續(xù)，由一段軌跡過渡到下一段軌跡時，需要加速度或減速度。在機械手手部到達結(jié)點前的時

14、刻 開始改變速度，然后保持加速度不變，直至到達結(jié)點之后 （單位時間）為止，如圖7.7所示。7.3 笛卡兒路徑軌跡規(guī)劃第七章 機器人軌跡規(guī)劃7.7圖 兩段軌跡間的過渡307.4 規(guī)劃軌跡的實時生成關節(jié)空間軌跡的生成對于帶拋物線過渡的直線樣條插值，每次更新軌跡時，應首先檢測時間t的值以判斷當前處于路徑段的是線性域還是過渡域。當處于線性域時，各關節(jié)的軌跡按下式計算：第七章 機器人軌跡規(guī)劃0 jkjkjt(7.41)31關節(jié)空間軌跡的生成令 則： jkjinbtttt21kinbkjkinbkinbjkjtttt 221)(7.42)327.4 規(guī)劃軌跡的實時生成笛卡兒空間軌跡的生成笛卡兒空間軌跡實時生成方法與關節(jié)空間相似，在線性域中，的每一自由度按下式計算：0 xxxtxxxjkjkj (7.44)337.4 規(guī)劃軌跡的實時生成笛卡兒空間軌跡的生成在線性域中，每個自由度的軌跡按下式計算：kinbkjkinbkinbjkjjkjinbxxtxxxtxttxxxtttt 22121(7.45)347.4 規(guī)劃軌跡的實時生成笛卡兒空間軌跡的生成將笛卡兒空間軌跡轉(zhuǎn)換成等價的關節(jié)空間的量，算法如下：根據(jù)計算結(jié)果：q， 由控制系統(tǒng)