引言學習成果達成要求：1、掌握剛體位置和姿態(tài)的概念；2、掌握坐標變換和基變換的方法；3、掌握齊次坐標和剛體位姿矩陣含義；4、掌握基于齊次坐標描述剛體運動的方法；5、掌握剛體變換矩陣左乘和右乘的物理意義。引言機器人末端的運動機器人每個構件（連桿）的運動點的運動機器人的運動質點的運動描述剛體運動描述2.12.2目錄2.1質點的運動描述

在三維空間中，一個自由質點P的位置可以用三個獨立參數(shù)來唯一地確定，也稱該質點具有三個自由度圖（2-1）質點的運動三個獨立參數(shù)在直角坐標系{A}可以用于表示質點P的位置向量記為：（T表示矩陣轉置，即行變成列）質點P從位置P1到P2的位移為：質點P的位置向量、速度和加速度分別為：在中，Px、Py、Pz分別表示向量在坐標系{A}中的三個坐標軸X、y、z上的投影。2.1質點的運動描述（2-2）2.2剛體運動描述2.2.1剛體的一般運動分析

機器人是由若干個連桿通過關節(jié)連接而成的，研究機器人的運動時，一般可以把每個連桿看成是剛體，繼而研究組成機器人的一系列剛體的運動?？臻g中自由運動剛體具有6個自由度。圖2-2飛機的飛行運動2.2剛體運動描述2.2.2剛體的位置和姿態(tài)描述為了確定剛體的位置和姿態(tài)，需要在剛體上建立一個與剛體“固定”連接的坐標系{B}：如圖所示剛體的運動，包括平動和轉動，可由位置和姿態(tài)的變化來體現(xiàn)，平動和轉動可以由坐標系{B}與坐標系{A}之間的關系變化來確定。圖2-3剛體的位置、姿態(tài)與坐標系的關系剛體在某一時刻的位置：坐標系{B}的原點OB在歐氏空間?3中的位置向量1由3X1矩陣表示2.2剛體運動描述2.2.2剛體的位置和姿態(tài)描述

剛體的姿態(tài)反映的是剛體與坐標系{A}的三個坐標軸XA、YA、ZA之間的方向，由坐標系{B}的三個坐標軸XB、YB、ZB與坐標系{A}的三個坐標軸向量XA、YA、ZA之間的夾角確定。2(2-4)2.2剛體運動描述2.2.2剛體的位置和姿態(tài)描述

圖2-5右手正交坐標系三個坐標軸之間的關系2.2剛體運動描述2.2.2剛體的位置和姿態(tài)描述

2.2剛體運動描述2.2.3向量的坐標變換和基變換

由線性代數(shù)可知，一個自由向量v的坐標與選取的“基向量”相關，一旦“一組基向量”確定，則向量v在該基下的坐標便唯一確定，如圖2-6所示。圖2-6向量在不同基下的坐標

2.2剛體運動描述2.2.3向量的坐標變換和基變換圖2-6向量在不同基下的坐標設是向量空間

3的一組正交規(guī)范基；是

3的另外一組正交規(guī)范基。向量v可以分別表示成基和基的線性組合即：分別稱作向量v在基

和下的坐標其中和(2-6)(2-7)2.2剛體運動描述2.2.3向量的坐標變換和基變換圖2-6向量在不同基下的坐標

(2-8)2.2剛體運動描述2.2.3向量的坐標變換和基變換由于向量是三維空間的

3中的向量，故也可以由基的線性組合來表示，即：(2-9)過渡矩陣P即為2.2.2節(jié)中的姿態(tài)矩陣R，即：(2-10)2.2剛體運動描述2.2.3向量的坐標變換和基變換式（2-11）稱向量v由基

下的坐標[l1,l2,l3]到基下的坐標[m1,m2,m3]之間的坐標變換矩陣(2-11)過度矩陣的物理意義：由式（2-10）可知，在自然基下的某一時刻，由坐標系{A}到坐標系{B}的過渡矩陣P，即為坐標系{B}相對于坐標系{A}的姿態(tài)矩陣

。2.2剛體運動描述2.2.3向量的坐標變換和基變換圖2-7向量v在兩個坐標系之間的坐標變換如圖2-7所示，若已知任意向量v在坐標系{B}中坐標為

以及坐標系{B}相對于坐標系{A}的姿態(tài)矩陣為

，則向量v在坐標系{A}中坐標如何確定？2.2剛體運動描述2.2.3向量的坐標變換和基變換根據(jù)向量合成與分解法則，

3空間中的任何一個向量都可以由該空間中的任何三個正交的分向量之和表示：若已知向量在坐標系{B}中的坐標以及坐標系{B}相對于坐標系{A}的姿態(tài)矩陣為，則向量在坐標系{A}中的坐標為姿態(tài)矩陣“左乘”。該結論具有一般性，可以推廣到任意一個向量在兩個坐標系{A}和{B}之間的坐標變換。（2-12）2.2剛體運動描述2.2.3向量的坐標變換和基變換2.2剛體運動描述2.2.4點的坐標變換、齊次坐標和剛體位姿矩陣圖2-8點的坐標在兩個坐標系之間的坐標變換

如圖2-8所示，剛體上一點Q，設它在坐標系{B}（與剛體固定連接）中的坐標為

，如果已知該時刻剛體的位置向量

和姿態(tài)矩陣

此時，點Q坐標系{A}中的坐標

如何確定？2.2剛體運動描述2.2.4點的坐標變換、齊次坐標和剛體位姿矩陣由空間解析幾何中的向量代數(shù)可知，只有同一個坐標系中的向量才能合成。如圖2-8所示，在坐標系{A}中，根據(jù)向量合成的三角形法則可得：（2-14）其中

表示圖2-8中的向量在坐標系{A}中的描述。向量在坐標系{B}中的描述為

，為已知向量，現(xiàn)在需要確定向量與向量的關系。

2.2.4點的坐標變換、齊次坐標和剛體位姿矩陣

由2.2.2節(jié)分析以及式（2-4）可知，沿著XB軸、YB軸和ZB軸三個正方向的單位向量在坐標系{A}中分別為0n、o、a，因而由式（2-12）可得向量

在坐標系{A}中描述為：（2-15）在坐標系{B}中，點Q的坐標為，將式（2-15）代入式（2-14），可得（2-16）2.2剛體運動描述2.2.4點的坐標變換、齊次坐標和剛體位姿矩陣引入“齊次”坐標后，根據(jù)矩陣運算規(guī)則可得：（2-17）（2-18）其中

4X4矩陣包括坐標系{B}(剛體)相對于坐標系{A}的姿態(tài)矩陣，也包含坐標系{B}(剛體)相對于坐標系{A}的位置向量，稱為坐標系{B}(剛體)相對于坐標系{A}的“位姿”矩陣或“位形”矩陣。2.2剛體運動描述2.2.4點的坐標變換、齊次坐標和剛體位姿矩陣引入齊次坐標后，向量的齊次坐標變換表達式為：（2-19）位姿矩陣

的物理意義和作用是：（1）

表示坐標系{B}相對于坐標系{A}的位置和姿態(tài)；（2）

可以把坐標系{B}中的向量和點的坐標變換到坐標系{A}中。2.2剛體運動描述2.2.4點的坐標變換、齊次坐標和剛體位姿矩陣2.2剛體運動描述

根據(jù)位姿矩陣的含義，可以確定三個坐標系{A}、{B}和{C}之間的位姿矩陣的關系。

如圖2-9所示，已知坐標系{C}相對于坐標系{B}的位姿矩陣

和坐標系{B}相對于坐標系{A}的位姿矩陣

，如何確定坐標系{C}相對于坐標系{A}的位姿？

由于位姿矩陣

的作用是把坐標系{C}中的向量和點的坐標變換到坐標系{B}中；而位姿矩陣

的作用同是；圖2-9剛體在不同坐標系之間的位姿變換2.2.4點的坐標變換、齊次坐標和剛體位姿矩陣（2-22）2.2剛體運動描述點Q在坐標系{C}和坐標系{A}之間的坐標變換為：故坐標系{C}相對于坐標系{A}的位姿矩陣

為：（2-21）2.2.5剛體變換——剛體的平動、轉動和一般運動描述2.2剛體運動描述

剛體的位移，即剛體在空間的運動狀態(tài)發(fā)生變化，是通過剛體的初始位姿和末了位姿的變化來描述的，為此，需要確定初始位姿和末了位姿之間的關系T，稱為剛體變換，其表達式為：

（2-23）其中：，

。因為位姿矩陣可逆，故式（2-23）的兩端右乘的逆陣可得：（2-24）其中：R是旋轉矩陣2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述

描述剛體位姿和剛體變換目前常用的方法包括位姿矩陣法、旋量法和四元素法等。這些方法各有優(yōu)缺點，本書采用位姿矩陣法。

目前的商用工業(yè)機器人，如ABB等主要采用位姿矩陣的方法；而KUKA機器人則采用旋量方法。

為了便于深入研究剛體運動，特別是多個剛體運動的“疊加”結果，需要理解剛體變換矩陣的物理意義，這就是Chasles定理。2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（1）剛體一般運動（轉動和平動的組合）的Chasles定理圖2-10剛體變換矩陣的物理意義

由理論力學中的Chasles定理可知，剛體的位移可以通過繞某一軸線的“轉動”加上繞該軸線的“平移”來實現(xiàn)。剛體變換T和式（2-18）表示的位姿矩陣雖然具有相同的形式，但其物理意義不同。2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（2）位姿矩陣和剛體變換矩陣的關系圖2-11剛體位姿矩陣和剛體變換的關系

當取

（四階單位陣）時，

，此時位的姿矩陣

等于剛體變換矩陣

，即坐標系{B}相對于坐標系{A}的位姿

，

也就是把坐標系{A}由初始位置變換到與坐標系{B}的當前位置“重合”時對應的剛體變換T，它是通過繞某一軸線k旋轉一定角度

，到達位置（XmYmZm），再沿該軸線平移距離d來實現(xiàn)的，如圖2-11所示。2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（3）向量經(jīng)過剛體運動后，僅僅與轉動運動有關，而與剛體的平移運動無關。設剛體運動前的向量為v0，經(jīng)過剛體運動后變?yōu)関t，由式（2-16）可知：（2-26）即，因此上述結論成立。2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（4）剛體變換矩陣中向量p的物理意義：坐標系{A}的原點OA0的齊次坐標為，隨上述剛體運動后，該原點位置變?yōu)镺At，且，即，因此，向量p即為坐標系{A}的原點OA0隨剛體運動后的位置向量OAt。2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述

剛體在空間中運動分為平動、繞定軸的轉動以及一般運動（既有平動，又有轉動），下面將分別分析與這三種類型的運動相對應的剛體變換T。（1）剛體平動（2）剛體轉動：1）、剛體繞坐標軸旋轉旋轉的剛體變換2）、繞過坐標系原點的一般軸線

旋轉的剛體變換3）、繞空間一般軸線旋轉的剛體變換（3）剛體的一般運動變換2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（1）剛體平動圖2-12剛體平動

指在運動過程中剛體的姿態(tài)一直保持不變，即任意時刻t剛體的姿態(tài)矩陣Rt與初時刻t=0時的位姿矩陣R0關系為：RtR0。

設剛體運動的初始位置向量為p0,任意時刻t的位置向量為pt，則剛體在t=0和t時刻的位姿矩陣

和

分別為：2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（1）剛體平動對于剛體做平動，對應的剛體變換為：（2-30）由式（2-30）可知，剛體平移時，其位移只取決于“首末”位置，該運動“等效于”剛體沿著向量d的平移了距離l=||d||。2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（2）剛體轉動

分為剛體繞坐標軸旋轉、剛體繞通過坐標原點的一般軸線的旋轉，以及剛體繞空間任意軸的旋轉三種情況。1）剛體繞坐標軸旋轉的剛體變換以繞X軸轉動為例，如圖2-13(a)所示，坐標系{B}和坐標系{A}和在初始時刻t=0時重合。圖2-13繞坐標軸旋轉(a)當坐標系{B}繞XA軸逆時針旋轉角度θ后，X0B軸與X0A軸仍然重合，因此2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述將這些參數(shù)代入代入式（2-4）可得姿態(tài)矩陣（旋轉矩陣）為：（2）剛體轉動(b)(c)圖2-13繞坐標軸旋轉（2-31）同理，如圖2-13所示，剛體繞YA軸轉動

角度后的位姿矩陣（旋轉矩陣）為：同理繞ZA軸轉動

角度后的位姿矩陣（旋轉矩陣）為：（2-32）（2-33）1）剛體繞坐標軸旋轉的剛體變換2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（2）剛體轉動分析式（2-31）-（2-33）可以發(fā)現(xiàn)，繞坐標軸旋轉矩陣的特點可以用圖2-14描述：圖2-14繞坐標軸旋轉的變換矩陣特點示意圖①繞某一軸旋轉角度

，設與該軸對應的數(shù)字為i（i=1,2,3）則旋轉矩陣的第“I”行“I”列對應的元素為1，其余兩個主對角元素均為轉角

的余弦值。②元素1所在的行和列的其余元素為0；矩陣的其余的副對角元素為轉角

的正弦值；③“負號”的位置，由旋轉軸以外的另外兩個數(shù)字按逆時針順序決定，如繞X軸旋轉，則“1”以外的另外兩個數(shù)字為“2”和“3”，則“負號”出現(xiàn)在“2

3”，即第2行第3列；同理，繞Z軸旋轉，則“負號”出現(xiàn)在“1

2”，即第1行第2列。1）剛體繞坐標軸旋轉的剛體變換2.2.5剛體變換——剛體的平動、轉動和一般運動描述2）繞過坐標系原點的一般軸線旋轉的剛體變換（2）剛體轉動圖2-16繞過坐標系{A}的原點的k軸旋轉變換如圖2-16所示，剛體處于空間一般位置，剛體運動為繞過坐標系{A}原點的直線l做旋轉運動，旋轉角度為

；設為直線l的單位向量，該剛體的初始位姿為，旋轉后的位姿為，由位姿相對應的剛體變換為

（2-37）2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（2）剛體轉動2）繞過坐標系原點的一般軸線旋轉的剛體變換對于（2-37）式：p為坐標系{A}的原點由初始位置

隨剛體運動后的位置向量；因為原點O在kA軸上，故原點繞kA軸旋轉后位置不變，因而

；

稱為繞過原點的KA軸的旋轉矩陣，其表達式為：

（2-38）2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（2）剛體轉動式（2-38）的證明參見習題2。當分別取

時，式（2-38）分別與式（2-31）、（2-32）、（2-33）一致，即繞坐標系{A}的X軸、Y軸和Z軸轉動，是繞過原點的K軸的旋轉矩陣的特例。當旋轉矩陣或姿態(tài)矩陣

已知時，如何確定與相對應的旋轉軸K和旋轉角度

？2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述3）繞空間一般軸線旋轉的剛體變換（2）剛體轉動圖2-17空間一般軸線k的旋轉變換如圖2-17所示，剛體處于空間一般位置，剛體運動為繞過坐標系Q點的直線l（不一定過原點）做旋轉運動，旋轉角度為θ；為直線l的單位向量，設該剛體的初始位姿為，旋轉后的位姿為。由式（2-38）可得，剛體繞kA軸旋轉的變換矩陣為，故剛體由位姿相對應的剛體變換為為：

其中為坐標系{A}的原點O隨剛體運動后的坐標（2-42）2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（3）剛體的一般運動變換剛體的一般運動是指剛體運動中包含有平動和轉動。由理論力學中的Chasles定理可知，任意剛體運動都可以通過繞某一軸線的平移運動加上繞該軸線的轉動來實現(xiàn)。圖2-18剛體的一般運動如圖2-18所示，若已知初始位姿為，運動后的位姿為。根據(jù)上述Chasles定理，位姿

之間的剛體運動可以通過繞某一過點QA的軸線l旋轉角度θ，再沿軸線l平移距離h來實現(xiàn)。與該運動相對應的剛體變換為。可以分兩步求出該變換。2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（3）剛體的一般運動的變換第一步：求剛體繞點Q的軸線l旋轉角度θ后的位姿為，即：

由式（2-42）可得：第二步：求剛體由位置XmYmZm經(jīng)過平移后的位姿，即：由式（2-30）可得平移對應的剛體變換為：故由上兩式可得（2-43）2.2剛體運動描述2.2.5剛體變換——剛體的平動、轉動和一般運動描述（3）剛體的一般運動的變換繞同一個軸連續(xù)進行的兩個剛體運動，其對應的剛體變換矩陣可以交換。證明見習題4。由式（2-43）可得：（2-45）若已知剛體變換矩陣，如何確定如何確定其旋轉軸kA、旋轉角度θ、平移距離d以及軸線的軸線kA的位置？2.2剛體運動描述2.2.6剛體變換矩陣蘊含的物理意義圖2-19剛體變換蘊含的物理意義坐標系{i}中描述的剛體變換，它描述的運動包括繞坐標系{i}中某一軸線的旋轉運動以及沿的平移運動；剛體變換即為坐標系本身{i}“經(jīng)歷”了該剛體運動后，它相對于其初始位置的位姿。2.2剛體運動描述2.2.7剛體運動在不同坐標系中的描述——變換矩陣的左乘和右乘2.2剛體運動描述如圖2-18所示，剛體由初始位姿為，繞過Q點的軸線k旋轉角度θ，再沿軸線k平移距離h后的位姿為。上述剛體運動既可以在坐標系{A}中描述，也可以在坐標系{B}中描述。當剛體運動在坐標系{A}中描述時，Q點和軸線k在坐標系{A}中表示為QA和kA，即，其中為向量k在坐標系{A}的XA軸、YA軸和ZA軸上的投影。其相應的剛體變換為。由式（2-43）可知：（2-48）剛體變換“左乘”剛體初始位姿矩陣得到剛體運動后的位姿矩陣。2.2.7剛體運動在不同坐標系中的描述——變換矩陣的左乘和右乘2.2剛體運動描述當剛體運動在坐標系{B}中描述。設Q點和軸線k在坐標系{B}中表示為QB和kB，即，其中為向量k在坐標系{B}的XB軸、YB軸和ZB軸上的投影。在上述剛體運動在坐標系{B}中描述的剛體變換為。從“運動效果”上看，剛體在坐標系{B}中，從初始位姿繞軸線旋轉角度θ，再沿軸移動距離h，同樣達到了相同的“位置”，即位姿。參照式（2-44），繞軸轉動θ角，再沿軸平移的距離h的剛體變換為：（2-49）根據(jù)式（2-21），坐標系{Bt}相對于坐標系{A}的位姿為：式（2-51）表明，坐標系{B}中描述的剛體變換矩陣“右乘”剛體的初始位姿得到剛體運動后相對于坐標系{A}的位姿。（2-51）2.2.7剛體運動在不同坐標系中的描述——變換矩陣的左乘和右乘2.2剛體運動描述坐標系{A}中描述的剛體變換和坐標系{B}中描述的剛體變換之間的關系為：

（2-53）其中向量k和點Q在坐標系{A}中的坐標與它們在坐標系{B}中的坐標之間的關系可以由式（2-17）和（2-19）確定：（2-54）式（2-53）表明：對于同一個剛體運動，坐標系{A}中描述的剛體變換矩陣與坐標系{B}中描述的剛體變換矩陣是“相似”矩陣。2.2.7剛體運動在不同坐標系中的描述——變換矩陣的左乘和右乘2.2剛體運動描述式（2-53）的物理解釋為：圖2-20在不同坐標系中描述的“同一剛體運動”的兩個剛體變換的關系如圖2-20所示，坐標系{B}和坐標系{A}“同時”由各自初始位置{B0}、{A0}繞k軸旋轉

、再沿著k平移