机器人学第二章(数学基础)

1、机器人运动学机器人运动学第二章第二章 数学基础数学基础2.1 2.1 引言引言 机器人位置和姿态的描述机器人位置和姿态的描述 机器人可以用一个开环关节链来建模机器人可以用一个开环关节链来建模 由数个驱动器驱动的转动或移动关节串联而成由数个驱动器驱动的转动或移动关节串联而成 一端固定在基座上，另一端是自由的，安装工具，用以一端固定在基座上，另一端是自由的，安装工具，用以操纵物体操纵物体inoa 人们感兴趣的是操作机末端执行人们感兴趣的是操作机末端执行器相对于固定参考坐标数的空间器相对于固定参考坐标数的空间几何描述，也就是机器人的运动几何描述，也就是机器人的运动学问题学问题 机器人的运动学即是研究

2、机器人机器人的运动学即是研究机器人手臂末端执行器位置和姿态与关手臂末端执行器位置和姿态与关节变量空间之间的关系节变量空间之间的关系运动学研究的问题运动学研究的问题Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!a0vzyxzyxpcb0uEH图2.1 点向量的描述数学基础数学基础 2.2.1 点向量（Point vectors） 点向量描述空间的一个点在某个坐标系的空点向量描述空间的一个点在某个坐标系的空间位置。同一个点在不同坐标系的

3、描述及位置向间位置。同一个点在不同坐标系的描述及位置向量的值也不同。如图量的值也不同。如图2.1中，点中，点p在在E坐标系上表示坐标系上表示为为 Ev，在，在H坐标系上表示为坐标系上表示为 Hu，且，且v u。一个点。一个点向量可表示为向量可表示为 v = ai + bj + ck 通常用一个（通常用一个（n + 1）维列矩阵表示，即除）维列矩阵表示，即除 x、y、z 三个方向上的分量外，再加一个比例因子三个方向上的分量外，再加一个比例因子 w ，即即 v = x y z w T 其中其中 a = x/w, b = y/w, c = z/w。 已知两个向量已知两个向量 a = ax i + a

4、y j + az k b = bx i + by j + bz k （2.1） 向量的点积是标量。用向量的点积是标量。用“ ”来定义向量点积，即来定义向量点积，即 a b = ax bx + ay by + az bz （2.2 ） 向量的叉积是一个垂直于由叉积的两个向量构成的平面的向量。用向量的叉积是一个垂直于由叉积的两个向量构成的平面的向量。用“”表示叉积，即表示叉积，即 a b = ( ay bz az by ) i + ( az bx ax bz ) j + ( ax by ay by ) k （ 2.3） 可用行列式表示为可用行列式表示为 i j k a b = ax ay az （

5、2.4） bx by bz旋转矩阵旋转矩阵 设固定参考坐标系直角坐标为设固定参考坐标系直角坐标为Oxyz，动坐标系为，动坐标系为O uvw，研究旋转变换情况。研究旋转变换情况。xyzwvuPo(O)图2-3 初始位置时，动静坐标系重合，初始位置时，动静坐标系重合，O、O 重合，如图。各轴重合，如图。各轴对应重合，设对应重合，设P点是动坐标系点是动坐标系O uvw中的一点，且固定不变。中的一点，且固定不变。则则P点在点在O uvw中可表示为：中可表示为： wwuvuuuvwkPjPiPP 、 、 为坐标系为坐标系O uvw的单位矢的单位矢量，则量，则P点在点在oxyz中可表示为：中可表示为： u

6、ivjwkzzyyxxxyziPiPiPPuvwPxyzP 当动坐标系当动坐标系O uvw绕绕O点回转时，求点回转时，求P点在固定坐标系点在固定坐标系oxyz中的位置中的位置 yzxo(O)uvwPPwPvPu图2-4已知：已知：P点在点在O uvw中是不变的仍然中是不变的仍然成立，由于成立，由于O uvw回转，则：回转，则： wwuvuuuvwkPjPiPP)(PwwvvuuxxuvwxkPjPiPiiP)(PwwvvuuyyuvwykPjPiPjjP)(PwwvvuuzzuvwzkPjPiPjjP用矩阵表示为用矩阵表示为: wvwzvzzwvyywxvxxzyxPPPkkjkikkjjji

7、jkijiiiPPPy（2-7） uvwxyzwzvzzwvyywxvxxPRpkkjkikkjjjijkijiii:R y则旋转矩阵为：定义反过来：反过来： xyzuvwPRP1RRRdet*1T1RRRdet因此是正交矩阵，的行列式，由于为的伴随矩阵，为RRRR由刚体的等距变换可知：uvwTuvwxyzTxyzpppp将上式代入，可得：IRRTR为正交矩阵为正交矩阵。 由图可知，由图可知， 在在y y轴上的投影为轴上的投影为 ， 在在z z轴上的投影轴上的投影为为 , , 在在y y轴上的投影为轴上的投影为 ， 在在z z轴上的投影为轴上的投影为 ，所以有：，所以有： vjcosyjsin

8、zksinyjwkcoszkvjwkwzvzzwvyywxvxxkkjkikkjjjijkijiiiy)R(x,xyzouvwUVWO图2-5cossin0sincos0001uxii方向余弦阵方向余弦阵同理：同理： cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （cossin0sincos0001)R(x,三个基本旋转矩阵三个基本旋转矩阵: : xyzouvwUWOxyzouvwUWOv 丹纳维特（丹纳维特（DenavitDenavit）和哈顿贝格（）和哈顿贝格（HartenbergHartenberg） 于于19551955年提出了一种矩阵代数方

9、法解决机器人年提出了一种矩阵代数方法解决机器人的运动学问题的运动学问题D-HD-H方法方法 具有直观的几何意义具有直观的几何意义 能表达动力学、计算机视觉和比例变换问题能表达动力学、计算机视觉和比例变换问题 其数学基础即是齐次变换其数学基础即是齐次变换2.2 2.2 点齐次坐标点齐次坐标2.2.1 2.2.1 点的齐次坐标点的齐次坐标 一般来说，一般来说，n维空间的齐次坐标表示是一个（维空间的齐次坐标表示是一个（n+1）维空间）维空间实体。有一个特定的投影附加于实体。有一个特定的投影附加于n维空间，也可以把它看作维空间，也可以把它看作一个附加于每个矢量的特定坐标一个附加于每个矢量的特定坐标比例

10、系数。比例系数。 kcj bi av zy x TwwzyxV式中式中i, j, k为为x, y, z 轴上的单位矢量，轴上的单位矢量，a= , b= , c= ，w为比例系数为比例系数 wxwywz 显然，齐次坐标表达并不是唯一的，随显然，齐次坐标表达并不是唯一的，随w值的不同而不同。在计算机图学中，值的不同而不同。在计算机图学中，w 作为通用比例因子，它可取任意正值，但作为通用比例因子，它可取任意正值，但在机器人的运动分析中，总是取在机器人的运动分析中，总是取w=1 。列矩阵列矩阵 例例 ：kjiV543可以表示为：可以表示为： V=3 4 5 1V=3 4 5 1T T 或或 V=6 8

11、 10 2V=6 8 10 2T T 或或 V=-12 -16 -20 -4V=-12 -16 -20 -4T T 齐次坐标与三维直角坐标的区别齐次坐标与三维直角坐标的区别 V点在点在OXYZ坐标系中表坐标系中表示是唯一的（示是唯一的（x、y、z） 而在齐次坐标中表示可而在齐次坐标中表示可以是多值的。不同的表以是多值的。不同的表示方法代表的示方法代表的V点在空间点在空间位置上不变。位置上不变。 xyzzzxV图2-2o2.2 2.2 旋转齐次变换旋转齐次变换 用齐次坐标变换来表示式旋转变换：用齐次坐标变换来表示式旋转变换： 110000001wvuzyxPPPRPPP1100000011zyx

12、wvuPPPRPPP 2.2.3 2.2.3 合成旋转矩阵合成旋转矩阵: :例例1：在动坐标中有一固定点：在动坐标中有一固定点 ，相对固定参，相对固定参考坐标系考坐标系 做如下运动：做如下运动： R（x, 90）；）； R(z, 90)； R(y,90)。求点。求点 在固定参考坐标系在固定参考坐标系 下下的位置。的位置。 TuvwPo1321OxyzuvwPoOxyz解解1：用画图的简单方法：用画图的简单方法 解解2：用分步计算的方法：用分步计算的方法 R（x, 90） R（z, 90） R（y, 90） 123113211000001001-000001P121312311000010000

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

14、动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘左乘。注意：注意：旋转矩阵间不可以交换旋转矩阵间不可以交换 uvwOOxyz 平移齐次变换矩阵平移齐次变换矩阵1000100010001c) b (a TransHcba注意：注意：平移矩阵间可以交换，平移矩阵间可以交换， 平移和旋转矩阵间不可以交换平移和旋转矩阵间不可以交换 zyxoo w u v abc因此对向量 u = x y z w T，经H变换为向量v可表示为 x + aw x / w + a y + bw y / w + b z + cw z / w + c w 12.2.4 2.2.4 相对变换相对变换 举例说明：举例说明：例例1

15、：动坐标系动坐标系0起始位置与固定参考坐标系起始位置与固定参考坐标系0重合重合,动坐标系动坐标系0做如下运动：做如下运动：R(Z,90) R（y,90） Trans(4，-3, 7)，求合成矩阵，求合成矩阵 解解1：用画图的方法：用画图的方法： ozyx74-3owuvvuwzyxoo(o)xyzuvwzyxuwo(o) v解解2：用计算的方法：用计算的方法 根据定义根据定义1，我们有：，我们有：1000701030014100 )R(Z,90 )90 R(y, 7) , 3 ,Trans(4T 以上均以固定坐标系多轴为变换基准，因此矩阵左乘。以上均以固定坐标系多轴为变换基准，因此矩阵左乘。如

16、果我们做如下变换，也可以得到相同的结果：如果我们做如下变换，也可以得到相同的结果： 例例2：先平移先平移Trans (4,-3,7)；绕当前绕当前 轴转动轴转动90； 绕当前绕当前 轴转动轴转动90；求合成旋转矩阵。；求合成旋转矩阵。 vw （2-202-20）解解1：用画图的方法：用画图的方法 zyxo(o)vwuzyxoowuvozyxowvuxyzoowuv解解2：用计算的方法：用计算的方法 1000701030014100)R(Z,90 )90 R(y, 7) , 3 ,Trans(4Too式（式（2-202-20）和式（）和式（2-212-21）无论在形式上，还是在结果上都是）无论在

17、形式上，还是在结果上都是一致的。因此我们有如下的结论：一致的。因此我们有如下的结论：动坐标系在固定坐标系中的齐次变换有动坐标系在固定坐标系中的齐次变换有2 2种情况：种情况：定义定义1 1：如果所有的变换都是相对于固定坐标系中各坐标轴旋如果所有的变换都是相对于固定坐标系中各坐标轴旋转或平移，则依次左乘，称为绝对变换。转或平移，则依次左乘，称为绝对变换。定义定义2 2：如果动坐标系相对于自身坐标系的当前坐标轴旋转或如果动坐标系相对于自身坐标系的当前坐标轴旋转或平移，则齐次变换为依次右乘，称为相对变换。平移，则齐次变换为依次右乘，称为相对变换。 结果均为动坐标系在固定坐标中的位姿（位置结果均为动坐

18、标系在固定坐标中的位姿（位置+ +姿态）。相姿态）。相对于固定坐标系，对于固定坐标系，轴。轴相当于轴，轴相对于轴，轴相当于ZYXwv 也就是说，动坐标系绕自身坐标轴做齐次变换，也就是说，动坐标系绕自身坐标轴做齐次变换，要达到绕固要达到绕固定坐标系相等的结果，就应该用相反的顺序。定坐标系相等的结果，就应该用相反的顺序。 机器人用到相对变换的机器人用到相对变换的时候比较多时候比较多 例如机械手抓一个杯子，例如机械手抓一个杯子，如右图所示，手爪需要如右图所示，手爪需要转动一个角度才抓的牢，转动一个角度才抓的牢，相对于固定坐标系表达相对于固定坐标系表达太麻烦，可以直接根据太麻烦，可以直接根据手爪的坐标

19、系表示手爪的坐标系表示 但也要知道在但也要知道在O中的位中的位姿，就用右乘的概念。姿，就用右乘的概念。 xyzoH2.2.5 2.2.5 绕通过原点的任意轴旋转的齐次变换绕通过原点的任意轴旋转的齐次变换 有时动坐标系有时动坐标系OO 可能绕过原点可能绕过原点O O的而分量分别为的而分量分别为rx、ry、rz的任意单位矢量的任意单位矢量r 转动转动角。角。研究这种转动的好处是可用研究这种转动的好处是可用OO 绕某轴绕某轴r 的一次转动代的一次转动代替绕替绕OO各坐标轴的数次转动各坐标轴的数次转动为推导此旋转矩阵，可作下述变换：为推导此旋转矩阵，可作下述变换：a.绕绕X 轴转轴转角，角， 使使r

20、轴处于轴处于XZ平面内平面内a.绕绕Y 轴转轴转-角，使角，使r 轴与轴与OZ轴重合轴重合a.绕绕OZ轴转动轴转动角角b.绕绕Y 轴转轴转角角c.绕绕X 轴转轴转-角角XYZrxryrzABCDBO51243rA由上图容易求出：由上图容易求出：2z2yyrrrsin2z2yzrrrcosxxrrrrOCsin2z2y2z2yrrrrrOBCBcos由定义由定义1和定义和定义2，上述，上述5次旋转的合成旋转矩阵为：次旋转的合成旋转矩阵为：cossin0sin-cos0001cos0sin010sin-0cos1000cossin0sin-coscos0sin-010sin0coscossin0s

21、incos0001RRRRRR,x,y, z,y,x, r（2-252-25）带入式带入式（2-252-25），得），得cos)cos(1rsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrcos)cos(1rsinr)cos(1rrsinr)cos(1rrcos)cos(1rR2zxzyyzxxzyyzx2yzyxzyx2x, r2.2.6 2.2.6 齐次交换矩阵的几何意义齐次交换矩阵的几何意义 设设T= T= ，有一个手爪，已知其在，有一个手爪，已知其在OO的位置，设一个的位置，设一个该坐标系该坐标系OO ，已知，已知， ，那么，那么OO 在

22、在OO中的齐次坐中的齐次坐标变换为标变换为 ，如果手爪转了一个角度，如果手爪转了一个角度， 则：则：1000321twzzztwyyytwxxx111cbao1000100010001T 1111cba1000pppTzyyyxxxzzzyxwwwT T反映了反映了O O 在在O O中的位置和姿态，即表示了该坐标系原点中的位置和姿态，即表示了该坐标系原点和各坐标轴单位矢量在固定坐标系中的位置和姿态。和各坐标轴单位矢量在固定坐标系中的位置和姿态。该矩阵可以由该矩阵可以由4 4个子矩阵组成，写成如下形式：个子矩阵组成，写成如下形式：比例系数透视矩阵位置矢量旋转矩阵11311333wfPRTzzzy

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

24、时的齐次变换矩阵为阵为 ，即求逆矩阵：，即求逆矩阵： 1T1000)(RT13T331PRT kpjpippzyx其中：其中：这些式子以后经常遇到，这些式子以后经常遇到，在机器人计算中，所要在机器人计算中，所要求的就是齐次变换矩阵求的就是齐次变换矩阵习题习题1 1：O O 与与O O初始重合，初始重合，O O 作如下运动：作如下运动：绕绕Z Z轴转动轴转动3030 ；绕绕X X轴转动轴转动6060 ；绕绕Y Y轴转动轴转动9090 。求。求T T。 100001000030cos30sin0030sin30cosR11000060cos60sin0060sin60cos000012R10000

25、90cos090sin0010090sin090cos3R1000002/12/302/34/34/102/14/34/3123RRRT习题习题2 2：OO 与与OO初始重合，初始重合，OO 作如下运动：作如下运动：绕绕X X轴转动轴转动9090 ；绕绕w w轴转动轴转动9090 ；绕绕Y Y轴转动轴转动9090 。求。求 T T；改变旋转顺序，如改变旋转顺序，如何旋转才能获得相同的结果。何旋转才能获得相同的结果。 1000090cos90sin0090sin-09cos00001R1100001000090cos90sin0090sin90cos2R1000090cos090sin00100

26、90sin090cos3R1000001001000001213RRRT解解： 解解： 绕绕Z Z（w w）轴转动）轴转动9090 ； 绕绕X X轴转动轴转动9090 ； 绕绕Y Y轴转动轴转动9090 。 变换方程变换方程（Transform equations） 研究一下图描述的研究一下图描述的一个物体与机械手一个物体与机械手情情况，机械手用变换况，机械手用变换 Z 相对于基坐标系被定位。相对于基坐标系被定位。机械手的端点用变换机械手的端点用变换 ZT6 来描述，而末端执行器来描述，而末端执行器用变换用变换 T6E 来描述。物体用变换来描述。物体用变换 B 相对于基坐相对于基坐标系被定位。

27、最后，机械手末端抓手用变换标系被定位。最后，机械手末端抓手用变换 BG相对于物体被定位。末端抓手位置的描述有两种相对于物体被定位。末端抓手位置的描述有两种方式，一种是相对于物体的描述，一种是相对于方式，一种是相对于物体的描述，一种是相对于机械手的描述。由于两种方式描述的是同一个机械手的描述。由于两种方式描述的是同一个点，我们可以把这个描述等同起来，得到点，我们可以把这个描述等同起来，得到 Z ZT6 T6E = B BG 这个方程可以用有向变换图来表示。图的每这个方程可以用有向变换图来表示。图的每一段弧表示一个变换。从它的定义的坐标系一段弧表示一个变换。从它的定义的坐标系向外指向。向外指向。 用用 Z- -1左乘和用左乘和用E- -1右乘方程，得到右乘方程，得到 T6 = Z- -1 B G E- -10EGBZT6zyx一个物体与机械手有向变换图GBET6Z0例题：例题：试求立方体中心在机座坐标系试求立方体中心在机座坐标系0中的位置中的