机器人学—数学基础

1、王新庆 n机械设计与车辆工程系n 工科D414n工程软件演示与讲解教学法机器人运动学及其数学基础参考教材n美付京逊机器人学n中南大学蔡自兴机器人学n美理查德鲍尔机器人操作手数学编程与控制参考教材n中南大学蔡自兴n 中南大学教授，我国人工中南大学教授，我国人工智能和机器人领域著名专智能和机器人领域著名专家家n 中国人工智能学会智能机中国人工智能学会智能机器人专委会理事长器人专委会理事长n 曾与付京逊教授一起工作曾与付京逊教授一起工作过过一 机器人位置和姿态的描述Justin catch balln串联机器人可以用一个开环关节链来建模n由数个驱动器驱动的转动或移动关节串联而成n一端固定在基座上，另

2、一端是自由的，安装工具（末端执行器），用以操纵物体，或完成各种任务运动学问题：nB,H坐标系n位置：H的原点在B坐标系中的坐标表示n姿态：H坐标系相对于B坐标系的姿态inoa关节变量末端执行器位置和姿态运动学研究的两个问题Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!n轴线平行及相交n轴线异面n轴线平行时采用几何分析方法n1955年丹纳维特（Denavit）和哈顿伯格（Hartenberg）提出了一种采用矩阵代数方法解决机器人的运动

3、学问题D-H方法具有直观的几何意义能表达动力学、计算机视觉和比例变换问题n数学基础是齐次变换二 数学基础齐次坐标和齐次变换2.1 点和面的齐次坐标2.1.1 点的齐次坐标 n用n+1个变量表示n维空间的几何元素。n 引入齐次坐标的目的是为了表示几何变换的旋转、平移和缩放 显然，齐次坐标表达并不是唯一的，随w值的不同而不同。在计算机图学中，w 作为通用比例因子，它可取任意正值，但在机器人的运动分析中，总是取w=1 。kcj bi av 一个点矢：一个点矢：Tx y z Vw列矩阵列矩阵式中式中i, j, k为为x, y, z 轴上的单位矢量，轴上的单位矢量，a= , b= , c= ，w为比例系

4、数为比例系数 wxwywz例1：kjiV543可以表示为：可以表示为： V=3 4 5 1V=3 4 5 1T T 或或 V=6 8 10 2V=6 8 10 2T T 或或 V=-12 -16 -20 -4V=-12 -16 -20 -4T T n 齐次坐标与三维直角坐标的区别nV点在OXYZ坐标系中表示是唯一的（a、b、c）n而在齐次坐标中表示可以是多值的。不同的表示方法代表的V点在空间位置上不变。 n 几个特定意义的齐次坐标：n0 0 0 nT坐标原点矢量的齐次坐标，n为任意非零比例系数 n1 0 0 0T 指向无穷远处的OX轴n0 1 0 0T 指向无穷远处的OY轴 n0 0 1 0T

5、 指向无穷远处的OZ轴 n0 0 0 0T 没有意义n 2个常用的公式：个常用的公式：zzyyxxbabababakbabajbabaibababbbaaakjibaxyyxzxxzyzzyzyxzyx)()()(点乘点乘:叉乘叉乘:2.1.2 平面的齐次坐标n平面齐次坐标由行矩阵P=a b c d 来表示n当点v=x y z wT处于平面P内时，矩阵乘积PV=0，或记为0dwczbyaxwzyxdcbaPV如果定义一个常数 m= ，则有： 222cba() ()xyzabcx ay bz cdijkijkwwwmmmw mw mw mm 可以把矢量 解释为某个平面的外法线，此平面沿着法线方向

6、与坐标原点的距离为(-d/m)。 )(kmcjmbiman 点和平面间的位置关系设一个平行于设一个平行于x、y轴，且在轴，且在z轴上的坐标为单位距离的平轴上的坐标为单位距离的平面面P可以表示为：可以表示为： 或或 有：有： PV= 1100P2200P v0 v0 v0 点在平面下方点在平面上点在平面上方与点矢与点矢 相仿，平面相仿，平面 也没有意义也没有意义 T00000000例如：点例如：点 V=10 20 1 1T 必定处于此平面内，而点必定处于此平面内，而点 V=0 0 2 1T处于平处于平 P 的上方，点的上方，点V=0 0 0 1T处于处于P平面下方，因为：平面下方，因为：1020

7、001010011 0 1120011000 -110001-100n2.1.3 平移变换平移变换n1、二维坐标平移变换、二维坐标平移变换 1111byaxyxabyxy1x1P(x1,y1)oo1沿坐标轴平移（沿坐标轴平移（a, b）。）。P位于位于O1坐标系中，坐标系中，O为绝对坐标系为绝对坐标系人（人（P）坐在汽车里运动）坐在汽车里运动11111001110011xxaxyTyby n2、三维坐标平移变换、三维坐标平移变换1100010001000111111111zyxcbazyxTzyx沿坐标轴方向平移（沿坐标轴方向平移（a,b,ca,b,c）车绕盘山公路行驶车绕盘山公路行驶n 平移

8、齐次变换矩阵平移齐次变换矩阵100010TTrans (a b c)0010001abcn 对任意向量对任意向量u=u=（x,y,z,wx,y,z,w）进行）进行T T变换后为：变换后为：100/010/001/00011axxawx wabyybwy wbVTuczzcwz wcww n 对已知任意向量对已知任意向量u=u=（x,y,z,wx,y,z,w）进行）进行T T变换变换实质实质是将是将u u与平移向量与平移向量(a,b,c,1)(a,b,c,1)相加相加2.2 旋转矩阵及旋转齐次变换旋转矩阵及旋转齐次变换2.2.1 2.2.1 旋转矩阵旋转矩阵 设固定参考坐标系直角坐标为设固定参考

9、坐标系直角坐标为Oxyz，动坐标系为，动坐标系为O uvw，研究旋转变换情况。研究旋转变换情况。xyzwvuPo(O)图2-3 初始位置时，动静坐标系重合，初始位置时，动静坐标系重合，O、O 重合，如图。各轴重合，如图。各轴对应重合，设对应重合，设P点是动坐标系点是动坐标系O uvw中的一点，且固定不变。中的一点，且固定不变。则则P点在点在O uvw中可表示为：中可表示为： wwvvuuuvwkPjPiPP 、 、 为坐标系为坐标系O uvw的单位矢的单位矢量，则量，则P点在点在oxyz中可表示为：中可表示为： uivjwkzzyyxxxyzkPjPiPPxyzuvwPPn例：绕坐标轴Z旋转c

10、ossinABBpppxxysincosABBpppyxyABppzz2.2.2 三个基本旋转矩阵和合成旋转矩阵 n写成矩阵形式00001ABppABppABppxcsxyscyzz()A BZn图中动坐标系换成uvw00001zwcskksc方向余弦阵方向余弦阵由图由图2-52-5可知，可知， 在在x x轴上的投影为轴上的投影为 ， 在在y y轴上的投影轴上的投影为为 , , 在在x x轴上的投影为轴上的投影为 ， 在在y y轴上的投影为轴上的投影为 uicosuisinvjvjuisinuivjcosvjn绕Z轴的基本旋转矩阵对对x轴投影轴投影对对y轴投影轴投影对对z轴投影轴投影同理：同理

11、： cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （ssin0sincos0001)R(x,co三个基本旋转矩阵三个基本旋转矩阵: : xyzouvwUWO总结总结三个基本旋转矩阵三个基本旋转矩阵 ( , )R z即动坐标系即动坐标系 求求 的旋转矩阵，也就是的旋转矩阵，也就是求出坐标系求出坐标系 中各轴单位矢量中各轴单位矢量 在固定坐标系在固定坐标系中各轴的投影分量，很容易得到在两个坐标系重合时，有：中各轴的投影分量，很容易得到在两个坐标系重合时，有：OvwOZ，绕轴转动 角，vwOwvkji,Oxyz( , )R z100010001R 当动坐标

12、系当动坐标系O uvw绕绕O点回转时，求点回转时，求P点在固定坐标系点在固定坐标系oxyz中的位置中的位置 yzxo(O)uvwPPwPvPu图2-4已知：已知：P点在点在O uvw中是不变的仍然中是不变的仍然成立，由于成立，由于O uvw回转，则：回转，则： wwvvuuuvwkPjPiPPxwwvvuuxuvwxikPjPiPiP)(PywwvvuuyuvwyjkPjPiPjP)(PzwwvvuuzuvwzkkPjPiPkP)(P用矩阵表示为用矩阵表示为: wvwzvzzwvyywxvxxzyxPPPkkjkikkjjjijkijiiiPPPy（2-7） uvwxyzwzvzzwvyywx

13、vxxPRpkkjkikkjjjijkijiii:R y则旋转矩阵为：定义反过来：反过来： xyzuvwPRP1RRRdet*1T1RRRdet因此是正交矩阵，的行列式，由于为的伴随矩阵，为RRRR2.2.2 旋转齐次变换 用齐次坐标变换来表示式（用齐次坐标变换来表示式（2-7） 110000001wvuzyxPPPRPPP1100000011zyxwvuPPPRPPP00001zwcskkscn以绕Z轴的基本旋转矩阵为例验证yxxvxwyyvwzzvzwi ii ji kRj ijjj kk ik jk k n 合成旋转矩阵合成旋转矩阵: :例例1：在动坐标中有一固定点：在动坐标中有一固定点

14、 ，相对固定参，相对固定参考坐标系考坐标系 做如下运动：做如下运动： R（x, 90）；）； R(z, 90)； R(y,90)。求运动后点。求运动后点 在固定参考坐标系在固定参考坐标系 下的位置。下的位置。 TuvwPo1321OxyzuvwPoOxyz解解1：用画图的简单方法：用画图的简单方法 n塑料块演示塑料块演示解解2：用分步计算的方法：用分步计算的方法 R（x, 90） R（z, 90） R（y, 90） 123113211000001001-000001P12131231100001000001001-0 P1312121310000001-00100100 P（2-14） （2-

15、15） （2-16） 上述计算方法非常繁琐，可以通过一系列计算得到上述上述计算方法非常繁琐，可以通过一系列计算得到上述结果。将式（结果。将式（2-14）（）（2-15）（）（2-16）联写为如下形式：）联写为如下形式： 3 3000100011xuyvzwPPPRPPPR4x4为二者之间的关系矩阵，我们令：为二者之间的关系矩阵，我们令： ),(),(),RR33xRzRy（定义定义1： 当动坐标系当动坐标系 绕固定坐标系绕固定坐标系 各坐标轴顺序有限次各坐标轴顺序有限次转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘。转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘。注意：旋转矩阵间不

16、可以交换注意：旋转矩阵间不可以交换 ，平移矩阵间可以交换平移矩阵间可以交换uvwOOxyz2.2.4 相对变换 举例说明：举例说明：例例1：动坐标系：动坐标系0起始位置与固定参考坐标系起始位置与固定参考坐标系0重合重合,动坐标系动坐标系0做如下运动：做如下运动：R(Z,90) R（y,90） Trans(4，-3, 7)，求合成矩阵，求合成矩阵 解解1：用画图的方法：用画图的方法： ozyx74-3owuvvuwzyxoo(o)xyzuvwzyxuwo(o) v解解2：用计算的方法：用计算的方法 根据定义根据定义1，我们有：，我们有：TTrans(4, -3, 7) R(Y, 90 ) R(Z

17、,90 )(start)00141003 01070001 以上均以固定坐标系各轴为变换基准，因此矩阵左乘。以上均以固定坐标系各轴为变换基准，因此矩阵左乘。如果我们做如下变换，也可以得到相同的结果：如果我们做如下变换，也可以得到相同的结果： 例例2：先平移先平移Trans (4,-3,7)；绕当前绕当前 轴转动轴转动90； 绕当前绕当前 轴转动轴转动90；求合成旋转矩阵。；求合成旋转矩阵。 vw （2-202-20）解解1：用画图的方法：用画图的方法 zyxo(o)vwuzyxoowuvozyxowvuxyzoowuv解解2：用计算的方法：用计算的方法 o00141003T()Trans(4,

18、 -3, 7) R(v , 90 ) R(w ,90 )01070001ostart（2-212-21）00141003TTrans(4, -3, 7) R(y, 90 ) R(Z,90 )(start)(220)01070001 o00141003T()Trans(4, -3, 7) R(v , 90 ) R(w ,90 )01070001(221)ostart式（式（2-202-20）和式（）和式（2-212-21）无论在形式上，还是在结果上都是）无论在形式上，还是在结果上都是一致的。因此我们有如下的结论：一致的。因此我们有如下的结论：动坐标系在固定坐标系中的齐次变换有动坐标系在固定坐标系

19、中的齐次变换有2 2种情况：种情况：定义定义1 1：如果所有的变换都是相对于：如果所有的变换都是相对于固定坐标系固定坐标系中各坐标轴旋中各坐标轴旋转或平移，则依次转或平移，则依次左乘，称为绝对变换左乘，称为绝对变换。结果均为动坐标系在固定坐标中的位姿（位置结果均为动坐标系在固定坐标中的位姿（位置+ +姿态）。相姿态）。相对于固定坐标系，对于固定坐标系，轴。轴相当于轴，轴相对于轴，轴相当于ZYXwv 也就是说，动坐标系绕自身坐标轴做齐次变换，要达到绕固也就是说，动坐标系绕自身坐标轴做齐次变换，要达到绕固定坐标系相等的结果，就应该用相反的顺序。定坐标系相等的结果，就应该用相反的顺序。 定义定义2

20、2：如果动坐标系相对于：如果动坐标系相对于自身坐标系自身坐标系的当前坐标轴旋转或的当前坐标轴旋转或平移，则齐次变换为依次平移，则齐次变换为依次右乘，称为相对变换右乘，称为相对变换。 右乘的意义：n机器人用到相对变换的时候比较多n例如机械手抓一个杯子，如右图所示，手爪需要转动一个角度才抓的牢，相对于固定坐标系表达太麻烦，可以直接根据手爪的坐标系表示xyzoH2.2.5 齐次变换矩阵的几何意义 设，有一个手爪，即动坐标系设，有一个手爪，即动坐标系OO ，已知，已知， 初始位置初始位置重合，那么重合，那么OO 在在OO中的齐次坐标变换为：中的齐次坐标变换为： ，如果手爪转了一个角度，如果手爪转了一个

21、角度， 则：则：111cbao1000100010001T 1111cba1000pppTzyyyxxxzzzyxwwwT反映了反映了O O 在在O O中的位置和姿态，即表示了该坐标系原中的位置和姿态，即表示了该坐标系原点和各坐标轴单位矢量在固定坐标系中的位置和姿态。点和各坐标轴单位矢量在固定坐标系中的位置和姿态。该矩阵可以由该矩阵可以由4 4个子矩阵组成，写成如下形式：个子矩阵组成，写成如下形式：比例系数透视矩阵位置矢量旋转矩阵11311333wfPRTzzzyyyxxxwww33R为姿态矩阵（旋转矩阵），表示动坐标系为姿态矩阵（旋转矩阵），表示动坐标系OO 在固定参考坐标系在固定参考坐标系

22、OO中的姿态，即表示中的姿态，即表示OO 各坐标轴单位矢量在各坐标轴单位矢量在OO各轴上的投影各轴上的投影 为位置矢量矩阵，代表动坐标系为位置矢量矩阵，代表动坐标系OO 坐标坐标原点在固定参考坐标系原点在固定参考坐标系OO中的位置中的位置 TzyxpppP13为透视变换矩阵，在视觉中进行图像计算，为透视变换矩阵，在视觉中进行图像计算，一般置为一般置为0 0 000f31为比例系数为比例系数 1 11w如果需要求解如果需要求解OO在在OO 中的位置和姿态，此时的齐次变换矩中的位置和姿态，此时的齐次变换矩阵为阵为 ，即求逆矩阵：，即求逆矩阵： 1T1000-R-TTT1 -33T1pwpvp）（）

23、（）（ kpjpippzyxkjizyxkvjvivvzyxkwjwiwwzyx其中：其中：这些式子以后经常遇到，这些式子以后经常遇到，在机器人计算中，所要在机器人计算中，所要求的就是齐次变换矩阵求的就是齐次变换矩阵 知识点： 1.点和面的齐次坐标和齐次变换2.三个基本旋转矩阵3.绝对变换：如果所有的变换都是相对于固定坐标系中各坐标轴旋转或平移，则依次左乘，称为绝对变换。4.相对变换：如果动坐标系相对于自身坐标系的当前坐标轴旋转或平移，则齐次变换为依次右乘，称为相对变换。知识点： 三个基本旋转矩阵cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （ssi

24、n0sincos0001)R(x,co例题例题1 1：O O 与与O O初始重合，初始重合，O O 作如下运动：作如下运动：绕绕Z Z轴转动轴转动3030 ；绕绕X X轴转动轴转动6060 ；绕绕Y Y轴转动轴转动9090 。求。求T T。 zcos30sin3000sin30cos3000R0010000110000cos60sin6000sin60cos6000001xRcos900sin9000100sin900cos9000001yR3 / 43/ 41/ 201/ 43 / 43 / 203 / 21/ 2000001yxzTR R R例题例题2 2：OO 与与OO初始重合，初始重合

25、，OO 作如下运动：作如下运动：绕绕X X轴转动轴转动9090 ；绕绕w w轴转动轴转动9090 ；绕绕Y Y轴转动轴转动9090 。求。求 T T；改变旋转顺序，如改变旋转顺序，如何旋转才能获得相同的结果。何旋转才能获得相同的结果。 x10000cos90-sin900R0sin90cos9000001cos90sin9000sin90cos900000100001wRcos900sin9000100sin900cos9000001yR1000001001000001yxwRTR R解解： 解解： 绕绕Z（w）?轴转动轴转动90； 绕绕X轴转动轴转动90； 绕绕Y轴转动轴转动90。 例题例题3 3： 矢量矢量 在在OO 中表示为中表示为 ，OO 相对于相对于OO的的齐次变换为：齐次变换为： Pkjip2230