7.1概述7.2空間描述與坐標(biāo)系7.3運(yùn)動學(xué)建模與分析7.4速度傳遞矩陣7.5軌跡規(guī)劃7.6運(yùn)動學(xué)建模與軌跡規(guī)劃項(xiàng)目實(shí)踐第七章運(yùn)動學(xué)建模與軌跡規(guī)劃7.1概述

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

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

空間描述與坐標(biāo)系

齊次矩陣

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

表示。

左上標(biāo)A表示矢量的參考坐標(biāo)系，

表示矢量

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

空間描述與坐標(biāo)系

齊次矩陣

手爪的姿態(tài)

可以由坐標(biāo)系{B}的三個坐標(biāo)軸矢量

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

空間描述與坐標(biāo)系

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

和

的三個矢量，共計(jì)四個矢量確定，記為當(dāng){B}和{A}平行時，

僅表示坐標(biāo)系{B}的位置，

為單位矩陣，當(dāng){B}和{A}的原點(diǎn)重合時，

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

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

空間描述與坐標(biāo)系

齊次矩陣為了便于用矩陣進(jìn)行運(yùn)算，將其改成齊次矩陣的形式式中：

稱為齊次坐標(biāo)，

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

含有3個獨(dú)立元素，

中含有9個元素，但

中只有3個元素獨(dú)立，因?yàn)?/p>

、

、

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

空間描述與坐標(biāo)系

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

空間描述與坐標(biāo)系

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

和

縮寫為

和

。可以根據(jù)姿態(tài)矩陣求出

，

，

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

空間描述與坐標(biāo)系

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

2.坐標(biāo)變換7.2

空間描述與坐標(biāo)系

復(fù)合變換

3.坐標(biāo)系設(shè)置7.2

空間描述與坐標(biāo)系

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

空間描述與坐標(biāo)系

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

運(yùn)動學(xué)建模與分析

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

運(yùn)動學(xué)建模與分析

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

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

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

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

運(yùn)動學(xué)建模與分析

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

。3.在連桿上建立坐標(biāo)系7.3

運(yùn)動學(xué)建模與分析

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

運(yùn)動學(xué)建模與分析

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

運(yùn)動學(xué)建模與分析

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

運(yùn)動學(xué)建模與分析

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

運(yùn)動學(xué)建模與分析

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

運(yùn)動學(xué)建模與分析

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

運(yùn)動學(xué)建模與分析

PUMA機(jī)器人的逆運(yùn)動學(xué)建模過程，就是已知之前求得的

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

6.逆運(yùn)動學(xué)建模與分析7.3

運(yùn)動學(xué)建模與分析

2）求θ3：

其中，

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

運(yùn)動學(xué)建模與分析

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

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

運(yùn)動學(xué)建模與分析

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

速度傳遞矩陣

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

，得到參照坐標(biāo)系{i+1}的角速度進(jìn)一步，可得：其中，2.線速度傳遞7.4

速度傳遞矩陣

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

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

表示坐標(biāo)系{i+1}的原點(diǎn)在{i}的位移矢量，可以由

的平移矢量獲得。

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

軌跡規(guī)劃

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

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

軌跡規(guī)劃

僅考慮起止點(diǎn)的三次多項(xiàng)式

三次多項(xiàng)式的關(guān)節(jié)函數(shù)θ(t)形式如下：

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

軌跡規(guī)劃

具有中間點(diǎn)的三次多項(xiàng)式

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

軌跡規(guī)劃

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

軌跡規(guī)劃

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

軌跡規(guī)劃

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

軌跡規(guī)劃

1.機(jī)器人運(yùn)動學(xué)求解項(xiàng)目實(shí)踐7.6

運(yùn)動學(xué)建模與軌跡規(guī)劃項(xiàng)目實(shí)踐

本實(shí)驗(yàn)擬采用matlab中Robotics

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

運(yùn)動學(xué)建模與軌跡規(guī)劃項(xiàng)目實(shí)踐

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

運(yùn)動學(xué)建模與軌跡規(guī)劃項(xiàng)目實(shí)踐

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

運(yùn)動學(xué)建模與軌跡規(guī)劃項(xiàng)目實(shí)踐

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

運(yùn)動學(xué)建模與軌跡規(guī)劃項(xiàng)目實(shí)踐

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

運(yùn)動學(xué)建模與軌跡規(guī)劃項(xiàng)目實(shí)踐

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

Toolbox機(jī)器人工具箱進(jìn)行機(jī)器人笛卡爾空間