1、第二章位姿几何基础第二章位姿几何基础yTxTzTOT描述工业机器人的刚体（构件）相对于基础坐标系的位姿途径描述工业机器人的刚体（构件）相对于基础坐标系的位姿途径是描述与刚体固联的坐标系相对于基础坐标系的位姿。是描述与刚体固联的坐标系相对于基础坐标系的位姿。z0 x0y0o02-1 刚体位姿的确定刚体位姿的确定一、确定刚体位姿的矩阵方法一、确定刚体位姿的矩阵方法 z0 x0y0O0A 0A0A0A000AzyxAOP1 1、点、点/向量的矩阵描述向量的矩阵描述 pA0Ay0Az“0”表示参考系表示参考系的编号的编号“A”表示被描述表示被描述系的编号系的编号0AxO02 2、单位向量方向的矩阵描述
2、、单位向量方向的矩阵描述 x0z0y0iA cos cos cos coscoscos0Acos0Acos0Acosikijiii0A设当前向量设当前向量iA是单位向量，与参考系轴是单位向量，与参考系轴x0的单位向量的单位向量i0的夹角为的夹角为；与轴与轴y0的单位向量的单位向量j0的夹角为的夹角为；与轴与轴z0的单位向量的单位向量k0的夹角为的夹角为：表示轴表示轴iA与轴与轴 i0的夹角的夹角O0 x0z0y0iAkAjA3 3、坐标系方向的矩阵描述、坐标系方向的矩阵描述 现以现以iA为基础建立编号为为基础建立编号为A的坐标系，的坐标系，3个轴分别为个轴分别为 iA、jA、kA：0Acos0
3、Acos0Acosikijiii0A 0Acos0Acos0Acosjkjjjij0A 0Acos0Acos0Acoskkkjkik0AO0 x0z0y0iAkAjA 0Acos0Acos0Acos0Acos0Acos0Acos0Acos0Acos0Acoskkkjkijkjjjiikijii0AR RO0 x0z0y0iAkAjA 1|000P|R0A0A0AT TAAA0O00O0Ocoscoscosx000coscoscosyT00coscoscosz0000001iAAAAAAAAAAij ik ii jk jj ji kj kk k 0 0 x0z0y0O0iAkAjAOApA 0O
4、0O0O000AAAAzyxAOP4 4、坐标系位姿的奇次矩阵描述、坐标系位姿的奇次矩阵描述 10PR0A0A0AT T 1|0P|R1000ponponponTijijzzzzyyyyxxxxij 1000z0Acos0Acos0Acosy0Acos0Acos0Acosx0Acos0Acos0AcosT0O0O0O0AAAAkkkjkijkjjjiikijii系系Sj 的的z轴在系轴在系Si中的方向余旋中的方向余旋系系Sj 的的y轴在系轴在系Si中的方向余旋中的方向余旋系系Sj 的的x轴在系轴在系Si中的方向余旋中的方向余旋二、位姿矩阵的几何意义二、位姿矩阵的几何意义 1姿态矩阵姿态矩阵 的
5、的几何意义几何意义 ijR（1 1）表示）表示Sj 坐标系坐标系在在Si 坐标系中的姿态；坐标系中的姿态；zzzyyyxxxijaonaonaonRxiziyiOi=Ojxjyjzj zzzyyyxxxijaonaonaonR cos sin0 sin cos0001 ijR 1000cs0scRij 简写成：简写成：c cos表示表示 sin s表示表示例如例如（2 2）是坐标系之间的旋转变换矩阵）是坐标系之间的旋转变换矩阵；xiziyiOiyjxjzjOjpA jAjAjAijiAiAiAzyxRzyx设向量设向量p pA A 在坐标系在坐标系Si 和和Sj 的的 描述分别为：描述分别为：
6、iAiAiAiAzyxP那么存在下列变换关系：那么存在下列变换关系：jAjAjAjAzyxP（3）代表代表运动运动还可看作是一新的坐标系还可看作是一新的坐标系Sj，该坐标系是该坐标系是Si 经旋转（运动）而得。经旋转（运动）而得。xiziyiOi=Ojxjyjzj图示坐标系的运动方法：坐标系图示坐标系的运动方法：坐标系Sj先与坐标系先与坐标系Si完全重合，完全重合，然后绕轴然后绕轴zi转动角转动角，得到具有新的姿态的系得到具有新的姿态的系SjxiziyiOiyjOjxjzj2位姿矩阵位姿矩阵 的的几何意义几何意义 ijT 1|0P|R1000paonpaonpaonTijijzzzzyyyyx
7、xxxij是齐次矩阵是齐次矩阵齐次坐标齐次坐标（1）齐次矩阵的相关术语：）齐次矩阵的相关术语：1）齐次坐标）齐次坐标用用4 4个数表示空间点的坐标：个数表示空间点的坐标：A A（x x1 1 x x2 2 x x3 3 x x4 4)齐次坐标的几何含义：齐次坐标的几何含义：x x4 400时表示唯一点，点的坐标分别为时表示唯一点，点的坐标分别为 x xx x1 1/x/x4 4 yx2/x4 zx3/x4 x x4 40 0时表示从坐标原点到点（时表示从坐标原点到点（x y zx y z）的方向；）的方向；x xx x1 1 yx2 zx3齐次坐标性质：齐次坐标性质：唯一的点可用不同的齐次坐标
8、表示。唯一的点可用不同的齐次坐标表示。X4为比例因子，为比例因子，在在机器人学机器人学里取里取1。例如齐次坐标（例如齐次坐标（1 2 3 1）、）、（2 4 6 2）、）、（3 6 9 3）均表示笛卡尔坐标下的空间点（均表示笛卡尔坐标下的空间点（1 2 3）齐次矩阵齐次矩阵 T 432143210Axxxxxxxxp 2）齐次矩阵：矩阵形式表示的齐次坐标）齐次矩阵：矩阵形式表示的齐次坐标 j4j3j2j1zzzzyyyyxxxxi4i3i2i1xxxx1000paonpaonpaonxxxx3）齐次变换）齐次变换点在坐标系点在坐标系Si的齐次矩阵表示的齐次矩阵表示点在坐标系点在坐标系Sj的齐次
9、矩阵表示的齐次矩阵表示 1zyx1000ponponpon1zyxjAjAjAzzzzyyyyxxxxiAiAiA 例如点例如点A的齐次变换在的齐次变换在机器人学机器人学里通常写成里通常写成点点A在坐标系在坐标系Si的齐次矩阵表示的齐次矩阵表示点点A在坐标系在坐标系Sj的齐次矩阵表示的齐次矩阵表示表示表示Sj的坐标系原点在的坐标系原点在Si下位置下位置表示表示Sj在在Si下的姿态下的姿态 1|0P|R1000paonpaonpaonTijijzzzzyyyyxxxxij1 1）表示）表示Sj 坐标系坐标系在在Si 坐标系中的位姿；坐标系中的位姿；（2）位姿矩阵）位姿矩阵 的几何意义的几何意义i
10、jT 1zyxT1zyx1000paonpaonpaon1zyxjAjAjAijjAjAjAzzzzyyyyxxxxiAiAiA点点A在坐标系在坐标系Si的齐次矩阵表示的齐次矩阵表示点点A在坐标系在坐标系Sj的齐次矩阵表示的齐次矩阵表示2 2）坐标系之间的齐次变换）坐标系之间的齐次变换；3）代表运动）代表运动xiziyiOiOjyjxjzj第第1步：步：Sj 先与先与Si完全重合；完全重合；第第2步：作旋转运动，具有新的姿态；步：作旋转运动，具有新的姿态；第第3步：再作平移运动，具有新的位姿。步：再作平移运动，具有新的位姿。新的坐标系新的坐标系Sj是经过下列运动达到新的位姿的是经过下列运动达到
11、新的位姿的 10000010010020001T01 10000100000110010T2从坐标系运动的角度叙述：从坐标系运动的角度叙述：01T表示表示S1先与先与S0完全重合，再绕完全重合，再绕x0旋转旋转90再沿再沿x0移动移动20y0z0 x0O0z1x1y1O120z0 x0y0O0y0z0 x0O0z1x1y1O1O1z1x1y1三、多次数的变换三、多次数的变换z1x1y1O120z0 x0y0O0z1x1y1O120z2x2y2O2 10000100000110010T2（1）T2表示表示S2先与先与S1完全重合，完全重合，再绕再绕z1旋转旋转90再沿再沿x1移动移动10z0 x
12、0y0O0z1x1y1O120z2x2y2O210z0 x0y0O0 x2O2z2y2T2分两种情况讨论分两种情况讨论z0 x0y0O002201T00000001010030010TT 02012T100000102000110100TT z1x1y1O120z2x2y2O210 00000001010030010T02 10000010010020001T01 10000100000110010T2结论结论1：当：当S2是沿是沿S1运动时用运动时用T2右乘右乘01Tx2y2z2O210z1x1y1O120z0 x0y0O0 x2O2z2y2x2y2z2O2z0 x0y0O0z1x1y1O1
13、20 10000100000110010T2（2）T2表示表示S2先与先与S1完全重合，完全重合，再绕再绕z0旋转旋转90再沿再沿x0移动移动10z0 x0y0O0z1x1y1O120 x2y2z2O2z0 x0y0O0z1x1y1O120 100000102000110100T021002201T00000001010030010TT 02012T100000102000110100TT 结论结论2:当当S2是沿是沿S0运动时用运动时用T2左乘左乘01T左乘和右乘法则：左乘和右乘法则：第二次及其以后的变换，如果是相对于基础坐标系的变换，第二次及其以后的变换，如果是相对于基础坐标系的变换，用左
14、乘；用左乘；如果是相对于流动坐标系的变换，用右乘。如果是相对于流动坐标系的变换，用右乘。设从设从Si到到Sj是经过了是经过了2次运动，第一次运动后形成了次运动，第一次运动后形成了S1，将将Si称为基础坐标系称为基础坐标系,S1称为流动坐标系，称为流动坐标系，Sj称为当前坐称为当前坐标系（目标坐标系），那么求解当前坐标系相对于基础标系（目标坐标系），那么求解当前坐标系相对于基础坐标系的位姿依据下列法则：坐标系的位姿依据下列法则：xiyiOiOjxjyj213IT01 1000010000cossin00sincosT2 1000010000102001T3 1000010040100001Tj
15、1000010040cossin20sincos1000010000cossin00sincos10000100401020011000010000cossin00sincos10000100001020011000010040100001TTTT23jij 【教师例题教师例题】如图，给出运动变换，解释从如图，给出运动变换，解释从S Si i到到S Sj j经过的运动次序。经过的运动次序。全部是相对于基系，先旋转，后移动，左乘。全部是相对于基系，先旋转，后移动，左乘。到达到达正确正确位置，具有正确姿态。位置，具有正确姿态。yixiOiOjxjyj123Ojxjyjj2j3T=T T Tcoss
16、in0010001002sincos0001040100001000100010000100010001cossin001002cossin02cos4sinsincos0001040010001000010001i sincos02sin4sin00100001 全部是相对于基系，先移动，后旋转，左乘。全部是相对于基系，先移动，后旋转，左乘。到达错误位置。到达错误位置。yixiOiOjxjyj123Ojxjyj 10000100sin4ins20cossinsin4cos20sincos10000100401020011000010000cossin00sincos1000010000102
17、00110000100401000011000010000cossin00sincosTTTT23jij 先相对于基系移动，左乘；先相对于基系移动，左乘；后后相对于流系相对于流系旋转，右乘。旋转，右乘。到达正确位置、姿态。到达正确位置、姿态。【课堂练习课堂练习】【练习练习1】绕基系绕基系S Si i的的z zi i轴转动轴转动角形成系角形成系S S1 1，再绕再绕z z1 1轴转动轴转动角形成当前坐标系角形成当前坐标系S Sj j。画出各。画出各坐标系并求坐标系并求Sj相对于相对于Si的位姿矩阵。的位姿矩阵。【练习练习2】沿基系沿基系S Si i的的x xi i轴移动轴移动a a形成系形成系S
18、 S1 1，再沿，再沿y y1 1轴移动轴移动b b形成当前坐标系形成当前坐标系S Sj j。画出各坐标系并画出各坐标系并求求Sj相对于相对于Si的位姿矩阵。的位姿矩阵。【练习练习3】沿沿xi轴移动轴移动20形成形成S1，再，再绕绕z zi i轴转动轴转动9090形成系形成系S S2 2，再沿再沿z2移动移动10形成当前坐标系形成当前坐标系Sj。画出各坐标系并画出各坐标系并求求Sj相对于相对于Si的位姿矩阵。的位姿矩阵。【练习练习1】绕基系绕基系S Si i的的z zi i轴转动轴转动角形成系角形成系S S1 1，再绕，再绕z z1 1轴转动轴转动角形成当前坐标系角形成当前坐标系S Sj j。
19、画出各坐标系并求。画出各坐标系并求Sj相对于相对于Si的位姿矩阵。的位姿矩阵。yiOi=O1=Ojxixjyjx1y1100001000000T1cssc 1000010000cs00scT2 【解解】结论：当基系与流动坐标系同名轴重合时，转转变换，左乘右乘结论：当基系与流动坐标系同名轴重合时，转转变换，左乘右乘yiOi=O1=Ojxixjyjx1y1 1000010000)(c)(s00)(s)(cTTT12ij 1000010000)(c)(s00)(s)(cTTT21ij 【练习练习2】沿基系沿基系S Si i的的x xi i轴移动轴移动a a形成系形成系S S1 1，再沿，再沿y y1
20、 1轴移动轴移动b b形成形成当前坐标系当前坐标系S Sj j。画出各坐标系并画出各坐标系并求求Sj相对于相对于Si的位姿矩阵。的位姿矩阵。结论：当基系与流动坐标系同名轴平行时，平移变换，左乘右乘结论：当基系与流动坐标系同名轴平行时，平移变换，左乘右乘【解解】yiOixix1y1xjyjOjO1 100001000010a001T1 10000100b0100001T2 10000100b010a001TTTTT1221ij【练习练习3】沿沿xi轴移动轴移动20形成形成S1，再，再绕绕z zi i轴转动轴转动9090形成系形成系S S2 2，再沿再沿z2移动移动10形成当前坐标系形成当前坐标系
21、Sj，画出各坐标系并，画出各坐标系并求求Sj相对于相对于Si的位姿矩阵。的位姿矩阵。结论：结论：T3右乘表达式右乘表达式T2T1形成形成T2T1T3，而不是只左乘而不是只左乘T2形成形成T2T3T1 10000100001020001T1 1000010000010010T2 10001010000100001T3 000010010200010100TTTT312ij【解解】xjzjyjOjzixiyiOiy1x1z1O120 x2y2z2O210【例例2-1】已知坐标系已知坐标系S1与与S2之间的变换为：之间的变换为：10000010010020001T12求点求点P在两坐标系中的坐标之间
22、关系。在两坐标系中的坐标之间关系。【解解】1yz20 x1zyx100000100100200011zyx2P2P2P2P2P2P1P1P1Pz1x1y1O1x2z2y2 1yz20 x1zyx100000100100200011zyx2P2P2P2P2P2P1P1P1PO220四、位姿矩阵的逆阵四、位姿矩阵的逆阵1姿态矩阵的逆阵姿态矩阵的逆阵 jiT ij1ijRRR 2位姿矩阵的逆阵位姿矩阵的逆阵 1PR0RTTiOTijTjiji1ijj补充知识：旋转矩阵的两个正交性质补充知识：旋转矩阵的两个正交性质（1）R矩阵矩阵9个元素，个元素，3个是独立的，个是独立的，6个约束条件（正交条件）个约
23、束条件（正交条件）0naaoon 1aaoonn ，（2）R矩阵的逆阵等于转置矩阵矩阵的逆阵等于转置矩阵T1RR 1nnnnn2z2y2x 0ononononzzyyxx 10000100y0csx0scTiOiOijjj 【教师例题教师例题】下式为下式为坐标系坐标系Sj与坐标系与坐标系Si的变换，的变换，（1 1）画图标注变换矩阵的转角和移动量；）画图标注变换矩阵的转角和移动量；（2 2）画图标注变换矩阵画图标注变换矩阵逆阵逆阵的转角和移动量；的转角和移动量；（3）写出逆阵的）写出逆阵的R阵和阵和P阵。阵。yixiOiOjyjxjiOjxiOjy【解解】（1）画图标注变换矩阵的转角和移动量）
24、画图标注变换矩阵的转角和移动量yixiOiOjyjxjiOjxiOjyjOiyjOix-（2）画图标注变换矩阵逆阵的转角和移动量）画图标注变换矩阵逆阵的转角和移动量yixiOiOjyjxjiOjxiOjyjOiyjOix 1PR0RTTiOTijTjiji1ijj-10000100y0csx0scTiOiOijjj 1000cs0scRT T ij 1000cs0sc （3）写出逆阵的）写出逆阵的R阵和阵和P阵阵yixiOiOjyjxjiOjxiOjyjOiyjOix 1PR0RTTiOTijTjiji1ijj-10000100y0csx0scTiOiOijjj 0yx1000cs0scPR
25、iOiOiOT ijjjj 0cysxsycxiOiOiOiOjjjj cxiOj syiOj sxiOj cy-iOj ABTBAT【例例2-2】坐标系坐标系SB相对于相对于SA绕绕zA转（右旋）转（右旋）30，并沿并沿xA，yA正向平移正向平移3，4单位，求单位，求和和【解解】zAxAyAO1zB34xByBO23030 100001004030c30s3030s30cTAB由题意构造出坐标系由题意构造出坐标系B B 10000100402321302123 100001003223023212233021231PR0RTTAOTABTAB1ABBAB 0yx1000cs0scPR iOi
26、OiOT ijjjj 0cysxsycxiOiOiOiOjjjj 04c3030s330s430c3 （1）绕）绕x x 轴旋转轴旋转角的旋转变换矩阵：角的旋转变换矩阵：10000cs00sc00001),x(Rot （2）绕）绕y y轴旋转轴旋转角的旋转变换矩阵：角的旋转变换矩阵：10000c0s00100s0c),y(Rot 2-3 两种重要的旋转矩阵两种重要的旋转矩阵一、绕坐标轴旋转的旋转变换矩阵一、绕坐标轴旋转的旋转变换矩阵（3）绕）绕z z轴旋转轴旋转角的旋转变换矩阵：角的旋转变换矩阵：1000010000cs00sc),z(Rot 教师附加：平移教师附加：平移变换矩阵：变换矩阵：1
27、000z100y010 x001)z,y,x(Tran cverssverssverssverscverssverssverssverscvers,otRzzxzyyzxxyzyyzyxyxzzxyxx二、绕空间任意轴二、绕空间任意轴旋转旋转角的旋转变换矩阵：角的旋转变换矩阵：x、y、z轴轴的的3个方向余旋；个方向余旋；c cos；s s；vers 1-cos；其中其中1旋转变换矩阵旋转变换矩阵 2等效旋转角和等效旋转轴等效旋转角和等效旋转轴 zzzyyyxxxaonaonaonR R R RR R ，ot c21c3c1c3cccaon2z2y2x2z2y2xzyx versversvers
28、vers设已知设已知 cverssverssverssverscverssverssverssverscvers,otRzzxzyyzxxyzyyzyxyxzzxyxx相加相加相加相加求转轴求转轴和转角和转角1）求）求 1on21czyx 求求 和和的方法是令矩阵相等的方法是令矩阵相等不准用反余不准用反余旋求角旋求角 s2aoxyz zzzyyyxxxaonaonaonR R cverssverssverssverscverssverssverssverscvers,otRzzxzyyzxxyzyyzyxyxzzxyxx相减相减相减相减 s2nayzx s2onzxy 222z2y2x2xy2
29、zx2yzs4s)(4onnaao ）（）（）（2xy2zx2yzonnaao21s 同理同理 1ononnotanzyx2xy2zx2yz 2xy2zx2yzonno21s 1on21czyx 规定：绕规定：绕正向旋转，即正向旋转，即00 180，sin就只取正号，就只取正号，对应负号部分是将对应负号部分是将反向。反向。只能用反正只能用反正切求角切求角 sin2oyzx sin2nzxy sin2onxyz s2aoxyz s2nayzx s2onzxy 2）求）求一、一、动轴欧拉角动轴欧拉角表示法表示法zxz序列的含义：首先绕序列的含义：首先绕zi右旋右旋 角，得到角，得到S1；再以；再以
30、S1的的x1轴轴为轴，右旋为轴，右旋 角得到角得到S2；最后以；最后以z2为轴，右旋为轴，右旋 角得到角得到Sj 2-4 姿态矩阵的欧拉角的表示法姿态矩阵的欧拉角的表示法用绕流动坐标轴转用绕流动坐标轴转动的动的欧拉角表示法欧拉角表示法（旋转序列为流动坐标（旋转序列为流动坐标系的系的zxz序列，又称序列，又称313序列，也可用序列，也可用zyz即即323序列）序列）ccssssccccccscccsssccsccscscc1000cs0sccs0sc00011000cs0sc,Euler，1、欧拉矩阵、欧拉矩阵 ccssssccccccscccsssccsccscscc,Euler，2、欧拉角的
31、求解、欧拉角的求解 zzzyyyxxxaonaonaonR R设已知设已知求欧拉求欧拉角角、。yxaatan zzontan z2y2xaaatan z1z2z4z3o2o1o3o4x1x2x4x3z0 x0o0y4o4z1z2z3o2o1o3x1x2x3z0 x0o0 x4y4 z4o4z1z2z3o2o1o3x1x2x3z0 x0o0 x4y4 z4二、定轴欧拉角表示法二、定轴欧拉角表示法xyz滚转角滚转角 Roll俯仰角俯仰角 Pitch偏转角偏转角 Yaw ccscssccscccssccssscsccsssccccs0sc0001c0s010s0c1000cs0sc),RPY(1、RPY矩阵矩阵旋转序列：旋转序列：1st:Yaw(x轴轴)；2nd:Pitch(y轴轴)；3rd:Roll(z轴轴)。2、RPY角度求解角度求解2y2xznnntan xynntan zzaotan ccscssccscccssccssscsccsssccc),RPY(zzzyyyxxxaonaonaonR R设已知设已知求求RPY角角
