7.1概述7.2空間描述與坐標系7.3運動學建模與分析7.4速度傳遞矩陣7.5軌跡規(guī)劃7.6運動學建模與軌跡規(guī)劃項目實踐第七章運動學建模與軌跡規(guī)劃7.1概述

運動學包括兩類問題：正運動學和逆運動學。正運動學研究如何將機電系統(tǒng)各個坐標軸的運動映射為其末端工具（刀具）在直角坐標系的運動；逆運動學研究如何將機電系統(tǒng)末端工具在直角坐標系的運動分解為各個坐標軸的運動。本章主要以工業(yè)機器人為例介紹其運動學建模過程和分析方法。7.1概述

運動學模型是控制工業(yè)機器人按給定要求完成作業(yè)任務的基礎，在完成給定的作業(yè)任務之前，還應該先規(guī)定工業(yè)機器人的操作順序、路徑形狀、起點、中點、終點和操作時間等，然后控制系統(tǒng)根據(jù)這些給定條件，自動確定其每個坐標軸按時間歷程的位置、速度和加速度等，這就是軌跡規(guī)劃。1.空間描述7.2

空間描述與坐標系

齊次矩陣

工業(yè)機器人的各個運動組件都可以看做一個剛體，要描述剛體的運動，可以在剛體上固定一個坐標系，用該坐標系原點在參考坐標系的位置矢量和坐標系姿態(tài)來描述剛體的位置和姿態(tài)。在一個機械手抓取系統(tǒng)中，機械手爪的參考坐標系為{A}，與手爪固定的坐標系為{B}，則手爪的位置矢量用

表示。

左上標A表示矢量的參考坐標系，

表示矢量

在坐標系{A}的三個坐標軸的投影。1.空間描述7.2

空間描述與坐標系

齊次矩陣

手爪的姿態(tài)

可以由坐標系{B}的三個坐標軸矢量

在坐標系{A}中的投影按順序作為矩陣的列構(gòu)成的旋轉(zhuǎn)矩陣表示。1.空間描述7.2

空間描述與坐標系

齊次矩陣手爪的位姿由矢量

和

的三個矢量，共計四個矢量確定，記為當{B}和{A}平行時，

僅表示坐標系{B}的位置，

為單位矩陣，當{B}和{A}的原點重合時，

僅表示坐標系{B}的姿態(tài)，

為零矢量。1.空間描述7.2

空間描述與坐標系

齊次矩陣為了便于用矩陣進行運算，將其改成齊次矩陣的形式式中：

稱為齊次坐標，

稱為齊次變換矩陣，共有16個元素，除去4個常數(shù)，

含有3個獨立元素，

中含有9個元素，但

中只有3個元素獨立，因為

、

、

都是單位主向量，且兩兩正交，故有6個約束條件1.空間描述7.2

空間描述與坐標系

歐拉角所以，齊次矩陣只有6個獨立變量，3個表示位置，3個表示姿態(tài)。實際上，旋轉(zhuǎn)矩陣可以用三個參數(shù)的姿態(tài)表示，如X-Y-Z歐拉角。這種表示方法是：假設初始狀態(tài)時坐標系與參考坐標系重合，然后將繞著自身坐標系的軸旋轉(zhuǎn)角，再繞著旋轉(zhuǎn)后新坐標系的旋轉(zhuǎn)角，最后繞著第二次旋轉(zhuǎn)后新坐標系的旋轉(zhuǎn)角，進而形成最終的坐標系。1.空間描述7.2

空間描述與坐標系

歐拉角按照X-Y-Z歐拉角的變換方式得到坐標系相對于參考坐標系的旋轉(zhuǎn)矩陣為：為使矩陣簡潔起見，

和

縮寫為

和

?？梢愿鶕?jù)姿態(tài)矩陣求出

，

，

即可以用這三個參數(shù)表示兩個坐標系之間的姿態(tài)。2.坐標變換7.2

空間描述與坐標系

剛體的位置和姿態(tài)可以在不同的坐標系中描述，并且可以通過坐標變換公式將這些不同的描述聯(lián)系在一起。坐標變換包括坐標平移變換、坐標旋轉(zhuǎn)變換和復合變換。平移變換和旋轉(zhuǎn)變換

2.坐標變換7.2

空間描述與坐標系

復合變換

3.坐標系設置7.2

空間描述與坐標系

如前所述，剛體的位姿可以在坐標系中描述，因此坐標系的設定非常重要。這里的坐標系都是直線運動軸兩兩垂直的右手笛卡爾直角坐標系。3.坐標系設置7.2

空間描述與坐標系

為了規(guī)范工業(yè)機器人的運動，需要為工業(yè)機器人和操作對象確定專門的“標準”坐標系。為了便于描述機器人的運動控制，設置了5個直角坐標系：基坐標系{B}、固定坐標系{S}、腕部坐標系{W}、工具坐標系{T}、目標坐標系{G}。7.3

運動學建模與分析

工業(yè)機器人是由連桿通過關節(jié)連接在一起的開式運動鏈。為了便于描述工業(yè)機器人的運動關系，需要在其每個連桿上分別固接一個連桿坐標系，然后再研究當各個連桿通過關節(jié)連接起來后，連桿坐標系之間的相對關系，最后建立以關節(jié)變量為自變量的工業(yè)機器人末端執(zhí)行器位姿函數(shù)。1.連桿描述7.3

運動學建模與分析

工業(yè)機器人的連桿是連接兩個相鄰關節(jié)軸的剛體，在分析運動關系時，可以將連桿抽象為兩個相鄰關節(jié)軸之間的公垂線，這樣連桿可以用兩個參數(shù)描述。

連桿i-1長度：連桿i-1前端關節(jié)軸i-1和后端關節(jié)軸i之間公垂線的長度，記為ai-1

連桿i-1扭角：關節(jié)軸i-1按右手法則繞ai-1

旋轉(zhuǎn)到與關節(jié)軸i平行的角度，記為αi-12.連桿連接的描述7.3

運動學建模與分析

連桿偏距：從公垂線ai-1與關節(jié)軸i的交點到公垂線ai與關節(jié)軸i交點的有向距離，為關節(jié)軸i上的連桿偏距，記為di。關節(jié)角：從公垂線ai-1繞關節(jié)軸i按右手法則旋轉(zhuǎn)到公垂線ai的角度，為繞關節(jié)軸i的關節(jié)角，記為θi

。3.在連桿上建立坐標系7.3

運動學建模與分析

對中間連桿的坐標系做如下規(guī)定:（1）坐標系{i-1}的原點位于公垂線ai-1與關節(jié)軸i-1的交點處；（2）Zi-1軸與關節(jié)軸i-1重合；（3）Xi-1軸沿ai-1方向，由關節(jié)i-1指向關節(jié)i；（4）Yi-1軸由右手定則確定，即Yi-1=Zi-1×Xi-1。4.連桿間坐標系變換7.3

運動學建模與分析

按上述規(guī)定將坐標系固連在工業(yè)機器人的全部連桿后，可以建立任意相鄰連桿坐標系{i-1}和{i}之間的變換關系，這種變換通過幾個中間坐標系來實現(xiàn)，按照之前介紹的坐標復合變換方法，可得：考慮每一個變換矩陣，將這些變換矩陣分別帶入各自相對X和Z軸的旋轉(zhuǎn)和平移公式，最終可得連桿間坐標變換的通式：5.正運動學建模與分析7.3

運動學建模與分析

綜上所述，對工業(yè)機器人進行正運動學建模的大致步驟如下:1)找出各個關節(jié)軸線；2)確定各連桿坐標系；3)確定各連桿D-H參數(shù)；4)求出相鄰兩桿間的位姿矩陣；5)求末端工具相對于基座的位姿矩陣。5.正運動學建模與分析7.3

運動學建模與分析

下面以PUMA560六軸機器人為例，介紹其運動學建模過程。首先確定機器人各連桿D-H參數(shù)：5.正運動學建模與分析7.3

運動學建模與分析

按連桿變換通式及連桿參數(shù)表，可求得各個連桿間的變換矩陣如下：5.正運動學建模與分析7.3

運動學建模與分析

將上述連桿變換矩陣依次相乘便得到PUMA560的末端工具相對于基座的位姿描述矩陣，也叫運動學方程其中，6.逆運動學建模與分析7.3

運動學建模與分析

PUMA機器人的逆運動學建模過程，就是已知之前求得的

齊次變換矩陣中的n、o、a、p四個矢量，求解對應的關節(jié)角度θ1~θ6。1）求θ1：

6.逆運動學建模與分析7.3

運動學建模與分析

2）求θ3：

其中，

由推導可知，θ3也有兩個解。3）求θ2：接著上述獲得的條件繼續(xù)推導，可以得到θ1和θ3各有兩個解，所以θ2應該有四種解。6.逆運動學建模與分析7.3

運動學建模與分析

4）求θ2：接著上述獲得的條件繼續(xù)推導，可以得到因為s5有兩個解，所以θ5也有兩個解。5）求θ4：只要s5≠0，便可求出θ4

當s5=0時，機械手處于奇異形位，此時，關節(jié)軸4和6重合，只能解出θ4與θ6的和或差。在奇形位時，可任意選取θ4的值（一般取當前值），再計算相應的θ6。6.逆運動學建模與分析7.3

運動學建模與分析

6）求θ6：計算方法和過程跟計算θ4一樣。當s5≠0時當s5=0時，機械手處于奇異形位，θ4與θ6與的計算過程與上一步一樣。綜上，由于θ1、θ3、θ5各有兩個解，所以PUMA機器人的逆解共有8個。由于機器人結(jié)構(gòu)的限制，有些解不能實現(xiàn)，需要根據(jù)實際工況進行優(yōu)化選擇。1.角速度傳遞7.4

速度傳遞矩陣

連桿i+1的角速度用坐標系{i+1}的角速度表示，等于坐標系{i}的角速度加上由于關節(jié)i+1的轉(zhuǎn)動引起的角速度分量。參照坐標系{i}，連桿i+1的角速度可寫成將上式兩邊同時左乘

，得到參照坐標系{i+1}的角速度進一步，可得：其中，2.線速度傳遞7.4

速度傳遞矩陣

連桿i+1的線速度用坐標系{i+1}原點的線速度表示，等于坐標系{i}原點的線速度加上由于連桿i+1的角速度引起的新的分量。參照坐標系{i}，連桿i+1的線速度可寫成：將上式兩邊同時左乘

，得到參照坐標系{i+1}的角速度：這里，

表示坐標系{i+1}的原點在{i}的位移矢量，可以由

的平移矢量獲得。

上式是轉(zhuǎn)動關節(jié)的速度傳遞公式，對于移動關節(jié)，相應的關系為：1.不同空間軌跡規(guī)劃對比7.5

軌跡規(guī)劃

在關節(jié)空間進行規(guī)劃時，是將關節(jié)變量表示成時間的函數(shù)，并規(guī)劃它的一階和二階或高階時間導數(shù)，主要規(guī)劃任務是對關節(jié)變量的插值運算，常規(guī)的關節(jié)空間軌跡規(guī)劃函數(shù)一般有多項式型、線性型、拋物線過渡型等。

2.關節(jié)空間軌跡規(guī)劃7.5

軌跡規(guī)劃

僅考慮起止點的三次多項式

三次多項式的關節(jié)函數(shù)θ(t)形式如下：

解方程，可得：三次多項式的關節(jié)函數(shù)位置、速度、加速度的時間函數(shù)如圖所示：2.關節(jié)空間軌跡規(guī)劃7.5

軌跡規(guī)劃

具有中間點的三次多項式

假設工業(yè)機器人在由位置A到位置B的過程中經(jīng)過了C、D等中間點，如果機器人在中間點做了停留，則仍可以用上述的軌跡規(guī)劃方法，求出每兩個點之間的關節(jié)規(guī)劃函數(shù)。確定中間點期望速度的方法有以下幾種：1）利用速度雅可比逆矩陣，將工具坐標系在直角坐標空間中的瞬時線速度和角速度轉(zhuǎn)換為每個路徑點的關節(jié)速度。2）在操作空間或關節(jié)空間中采用適當?shù)膯l(fā)式方法，由控制系統(tǒng)自動選擇中間點的速度。3）采用使中間點處的加速度連續(xù)的方法，系統(tǒng)自動選取中間點速度。3.操作空間軌跡規(guī)劃7.5

軌跡規(guī)劃

操作空間規(guī)劃的軌跡是機器人末端工具沿直角坐標空間運動的軌跡，除了前面介紹的直線軌跡以外，也可控制機器人在不同點之間沿其他類型的軌跡運動。實際上所有用于關節(jié)空間軌跡規(guī)劃的方法都可用于操作空間的軌跡規(guī)劃。對于關節(jié)空間軌跡規(guī)劃，規(guī)劃函數(shù)生成的值就是關節(jié)值，而直角坐標空間軌跡規(guī)劃函數(shù)生成的值是機器人末端工具的位姿，它們需要通過求解逆運動方程才能化為關節(jié)量。3.操作空間軌跡規(guī)劃7.5

軌跡規(guī)劃

前述過程可以簡化為如下的計算循環(huán):(1)為機器人末端工具各個自由度選擇合適的軌跡函數(shù)；(2)利用所選擇的軌跡函數(shù)，按時間順序計算出末端工具的位姿；(3)利用機器人逆運動方程計算與末端工具位姿對應的關節(jié)信息；(4)將關節(jié)信息傳遞給控制器；(5)重復步驟（2）到（4），直至軌跡執(zhí)行完畢。在工業(yè)應用中，最實用的操作空間軌跡是點到點之間的直線運動，但也經(jīng)常遇到多目標點(例如有中間點)間需要平滑過渡的情況。4.工業(yè)機器人仿真7.5

軌跡規(guī)劃

現(xiàn)代仿真技術不僅能夠?qū)崿F(xiàn)單臺機器人在特定應用場景下的幾何路徑規(guī)劃、運動過程仿真、碰撞干涉檢查、離線編程和動力學分析等，甚至能夠模擬復雜的物理環(huán)境和多機器人協(xié)作。4.工業(yè)機器人仿真7.5

軌跡規(guī)劃

1.機器人運動學求解項目實踐7.6

運動學建模與軌跡規(guī)劃項目實踐

本實驗擬采用matlab中Robotics

Toolbox機器人工具箱進行機器人逆運動學求解，這是一套功能強大的MATLAB工具箱，專門用于機器人學研究。本章的項目實踐中，主要使用了工具箱中的Link()類函數(shù)和SerialLink()類函數(shù)，關于這兩個類函數(shù)的使用方法，請參考官網(wǎng)的在線說明。1.機器人運動學求解項目實踐7.6

運動學建模與軌跡規(guī)劃項目實踐

a.正運動學求解具體步驟(1)建立DH參數(shù)，確定關節(jié)移動范圍；(2)在Link()函數(shù)中輸入DH參數(shù)建立連桿，設置offset參數(shù)指定連桿初始偏置，每個連桿DH參數(shù)的輸入順序為：關節(jié)角，關節(jié)距離d，連桿長度a，連桿轉(zhuǎn)角，連桿初始偏置offset，DH參數(shù)類型）；(3)根據(jù)機器人關節(jié)角移動范圍，在Link()函數(shù)的qlim參數(shù)指定關節(jié)的轉(zhuǎn)動極值；(4)通過SerialLink()函數(shù)連接各連桿建立機器人并指定其標題名稱(5)通過SerialLink.fkine()函數(shù)進行機器人正運動學計算，得出機器人末端執(zhí)行器位姿。1.機器人運動學求解項目實踐7.6

運動學建模與軌跡規(guī)劃項目實踐

b.逆運動學求解具體步驟(1)前4個步驟參考正運動學求解步驟進行；(2)通過SerialLink.ikine()函數(shù)進行機器人逆運動學計算，得出機器人六個關節(jié)角。%%逆運動學matlab代碼init_ang=robot0.ikine(T_1); %計算第一個位姿對應的關節(jié)角targ_ang=robot0.ikine(T_2); %計算第二個位姿對應的關節(jié)角1.機器人運動學求解項目實踐7.6

運動學建模與軌跡規(guī)劃項目實踐

c.雅可比矩陣求解具體步驟1(1)前4個步驟參考正運動學求解步驟進行；(2)通過SerialLink.jacob0()函數(shù)求出某個位姿下關節(jié)速度到基坐標系運動速度的雅可比矩陣；(3)通過SerialLink.jacobn()函數(shù)求出某個位姿下關節(jié)速度到工具坐標系運動速度的雅可比矩陣。1.機器人運動學求解項目實踐7.6

運動學建模與軌跡規(guī)劃項目實踐

c.雅可比矩陣求解具體步驟2%%雅可比矩陣求解matlab代碼J_1=robot0.jacob0(T_1); %計算第一個位姿下相對于基坐標系的雅可比矩陣J_2=robot0.jacob0(T_2); %計算第二個位姿下相對于基坐標系的雅可比矩陣J_3=robot0.jacobn(T_1); %計算第一個位姿下相對于工具坐標系的雅可比矩陣J_4=robot0.jacobn(T_2); %計算第二個位姿下相對于工具坐標系的雅可比矩陣2.笛卡爾空間軌跡規(guī)劃項目實踐7.6

運動學建模與軌跡規(guī)劃項目實踐

笛卡爾空間軌跡規(guī)劃是通過規(guī)劃末端執(zhí)行器的位姿來控制機器人運動，即保證機器人末端以確定的姿態(tài)在規(guī)定軌跡上運動。采用matlab中Robotics

Toolbox機器人工具箱進行機器人笛卡爾空間