机器人技术—数学基础

1、第三章第三章 机器人运动学的数学基础机器人运动学的数学基础讲授预备知识讲授预备知识目的：运动学目的：运动学 动力学动力学研究对象：研究对象： 工业机器人工业机器人第第2.52.5章章 机器人学关键技术机器人学关键技术1. 1. 机器人学机器人学 机器人学机器人学机械电子工程的典型案例机械电子工程的典型案例 机械电子的由来？机械电子的由来？ Mechatronics Mechanics + Electronics Tetsuro Mori（森哲郎）（森哲郎） 日本安川电气公司工程师日本安川电气公司工程师 1969年提出、年提出、1971年注册年注册第第2.52.5章章 机器人学关键技术机器人学关

2、键技术2. 2. 机器人学特点机器人学特点 机械电子工程机械电子工程 交叉学科特性交叉学科特性 Dr. Kevin Craig Marquette University第第2.52.5章章 机器人学关键技术机器人学关键技术3. 3. 机器人学研究内容机器人学研究内容 IEEE International Conference on Robotics and Automation (ICRA) 2013 Karlsruhe (德国德国) 文章：文章：873/2265 (38.5%) 分会场：分会场：84 国家：国家：63 IEEE/RSJ International Conference on

3、Intelligent Robots and Systems (IROS) 2013 Tokyo (日本日本) 文章：文章：904/2094 (43.2%) 分会场：分会场：153 国家：国家：-I. 机器人学内容机器人学内容第第2.52.5章章 机器人学关键技术机器人学关键技术3. 3. 机器人学研究内容机器人学研究内容 Technical Session的主要内容的主要内容Human robot interactionMedical roboticsSensor fusionLegged robotsUnderwater robotsManipulator motion planningC

4、amera calibrationIntelligent transportation systemsSLAM: Features and landmarksHumanoid robot body motionMicrorobotsBiologically-inspired robotic devicesRehabilitation roboticsField roboticsGraspingNanorobotic manipulationFish-like robot第三章第三章 机器人运动学的数学基础机器人运动学的数学基础参考教材参考教材 美美付京逊付京逊机器人学机器人学 中南大学中南大学

5、蔡自兴蔡自兴机器人学机器人学 美美理查德理查德鲍尔鲍尔机器人操作手机器人操作手数学数学编编程与控制程与控制参考教材参考教材 美美付京逊付京逊机器人学机器人学n 美籍华人美籍华人n 普渡大学（普渡大学（Purdue University）电机工程专业）电机工程专业著名教授著名教授n 4部著作、部著作、400多篇论文多篇论文n 第一任国际模式识别学会会长，被誉为自动模第一任国际模式识别学会会长，被誉为自动模式识别之父式识别之父n 1985年去世年去世参考教材参考教材 中南大学中南大学蔡自兴蔡自兴n 中南大学教授，我国人工中南大学教授，我国人工智能和机器人领域著名专智能和机器人领域著名专家家n 中国

6、人工智能学会智能机中国人工智能学会智能机器人专委会理事长器人专委会理事长n 曾在普渡大学工作过曾在普渡大学工作过第一节第一节 引言引言 串联机器人可以用一个开环关节链来建模串联机器人可以用一个开环关节链来建模 由数个驱动器驱动的由数个驱动器驱动的转动转动或或移动移动关节串联而成关节串联而成 一端固定在基座上，另一端是自由的，安装工具（末端一端固定在基座上，另一端是自由的，安装工具（末端执行器），用以操纵物体，或完成各种任务执行器），用以操纵物体，或完成各种任务inoa 关节的关节的相对运动相对运动导致杆件的运动，导致杆件的运动，使末端执行器定位于所需要的方使末端执行器定位于所需要的方位上位上

7、在一般机器人应用问题中，人们在一般机器人应用问题中，人们感兴趣的是：末端执行器相对于感兴趣的是：末端执行器相对于固定参考坐标数的固定参考坐标数的空间几何描述空间几何描述，也就是机器人的运动学问题也就是机器人的运动学问题 机器人的运动学即是研究机器人机器人的运动学即是研究机器人手臂手臂末端执行器位置和姿态末端执行器位置和姿态与与关关节变量空间节变量空间之间的关系之间的关系运动学研究的问题运动学研究的问题Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these

8、angles! 运动学运动学 滚动接触滚动接触 非完整控制非完整控制 数学基础数学基础-刚体运动刚体运动 参考文献：参考文献：机器人操作的数学导论机器人操作的数学导论 作者：理查德作者：理查德摩雷摩雷 李泽湘李泽湘 夏卡恩夏卡恩萨斯特里萨斯特里 翻译：徐卫良翻译：徐卫良 钱瑞明（东南大学）钱瑞明（东南大学）n 1955年丹纳维特（年丹纳维特（Denavit）和哈顿伯格（）和哈顿伯格（Hartenberg）提出）提出了一种了一种采用矩阵代数方法采用矩阵代数方法解决机器人的运动学问题解决机器人的运动学问题D-H方方法，其数学基础即是法，其数学基础即是齐次变换齐次变换 具有直观的几何意义具有直观的几

9、何意义 能表达动力学、计算机视觉和能表达动力学、计算机视觉和 比例变换问题比例变换问题 为以后的比例变换、透视变换为以后的比例变换、透视变换 等打下基础等打下基础1000pppTzyyyxxxzzzyxwww第二节第二节 数学基础数学基础齐次坐标和齐次变换齐次坐标和齐次变换2.1 2.1 点和面的齐次坐标点和面的齐次坐标2.1.1 2.1.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 作为通用比例因子，它可取任意正值，但作为通用比例因子，它可取任意正值，但在机器人的运动分析中，总是取在机器人的运动分析

11、中，总是取w=1 。列矩阵列矩阵一个点矢：一个点矢： 例例11：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 齐次坐标与三维直角坐标的区别齐次坐标与三维直角坐标的区别 V点在点在OXYZ坐标系中表坐标系中表示是示是唯一唯一的（的（a、b、c） 而在齐次坐标中表示可而在齐次坐标中表示可以是多值的。以是多值的。不同的表不同的表示方法代表的示方法代表的V点在空间点在空间位置上不变。位置上不变。 xyzzzxV图2-2on 几个特

12、定意义的齐次坐标：几个特定意义的齐次坐标： 0 0 0 nT坐标原点矢量的齐次坐标，坐标原点矢量的齐次坐标，n为任意为任意非零比例系数非零比例系数 1 0 0 0T 指向无穷远处的指向无穷远处的OX轴轴 0 1 0 0T 指向无穷远处的指向无穷远处的OY轴轴 0 0 1 0T 指向无穷远处的指向无穷远处的OZ轴轴 0 0 0 0T 没有意义没有意义n 2个常用的公式：个常用的公式：zzyyxxbabababakbabajbabaibababbbaaakjibaxyyxzxxzyzzyzyxzyx)()()(点乘点乘:叉乘叉乘:2.1.2 2.1.2 平面的齐次坐标平面的齐次坐标 平面齐次坐标由

13、平面齐次坐标由行矩阵行矩阵P=a b c d 来表示来表示 当点当点v=x y z wT处于平面处于平面P内时，矩阵乘积内时，矩阵乘积PV=0，或记为，或记为 0dwczbyaxwzyxdcbaPV与点矢与点矢 相仿，平面相仿，平面 也没有意义也没有意义 T00000000n 点和平面间的位置关系点和平面间的位置关系设一个平行于设一个平行于x、y轴，且在轴，且在z轴上的坐标为单位距离的平轴上的坐标为单位距离的平面面P可以表示为：可以表示为： 或或 有：有： PV= 1100P2200P v0 v0 v0 点在平面下方点在平面上点在平面上方例如：点例如：点 V=10 20 1 1T 必定处于此平

14、面内，而点必定处于此平面内，而点 V=0 0 2 1T处于平处于平 P 的上方，点的上方，点V=0 0 0 1T处于处于P平面下方，因为：平面下方，因为：01120101100 0 1120011000 -110001-1002.2 2.2 旋转矩阵及旋转齐次变换旋转矩阵及旋转齐次变换2.2.1 2.2.1 旋转矩阵旋转矩阵 设固定参考坐标系直角坐标为设固定参考坐标系直角坐标为Oxyz，动坐标系为，动坐标系为O uvw，研究旋转变换情况。研究旋转变换情况。xyzwvuPo(O)图2-3 初始位置时，动静坐标系重合，初始位置时，动静坐标系重合，O、O 重合，如图。各轴重合，如图。各轴对应重合，设

15、对应重合，设P点是动坐标系点是动坐标系O uvw中的一点，且固定不变。中的一点，且固定不变。则则P点在点在O uvw中可表示为：中可表示为： wwvvuuuvwkPjPiPP 、 、 为坐标系为坐标系O uvw的单位矢的单位矢量，则量，则P点在点在oxyz中可表示为：中可表示为： uivjwkzzyyxxxyzkPjPiPPxyzuvwPP 当动坐标系当动坐标系O uvw绕绕O点回转时，求点回转时，求P点在固定坐标系点在固定坐标系oxyz中的位置中的位置 yzxo(O)uvwPPwPvPu图2-4已知：已知：P点在点在O uvw中是不变的仍然中是不变的仍然成立，由于成立，由于O uvw回转，则

16、：回转，则： wwvvuuuvwkPjPiPPxwwvvuuxuvwxikPjPiPiP)(PywwvvuuyuvwyjkPjPiPjP)(PzwwvvuuzuvwzkkPjPiPkP)(P用矩阵表示为用矩阵表示为: wvwzvzzwvyywxvxxzyxPPPkkjkikkjjjijkijiiiPPPy（2-7） uvwxyzwzvzzwvyywxvxxPRpkkjkikkjjjijkijiii:R y则旋转矩阵为：定义反过来：反过来： xyzuvwPRP1RRRdet*1T1RRRdet是正交矩阵，的行列式，为的伴随矩阵，为RRRR2.2.2 2.2.2 旋转齐次变换旋转齐次变换 用齐次坐

17、标变换来表示式（用齐次坐标变换来表示式（2-7） 110000001wvuzyxPPPRPPP1100000011zyxwvuPPPRPPP2.2.3 2.2.3 三个基本旋转矩阵和合成旋转矩阵三个基本旋转矩阵和合成旋转矩阵 三个基本旋转矩阵三个基本旋转矩阵 ),(xR即动坐标系即动坐标系 求求 的旋转矩阵，也就是的旋转矩阵，也就是求出坐标系求出坐标系 中各轴单位矢量中各轴单位矢量 在固定坐标系在固定坐标系中各轴的投影分量，很容易得到在两个坐标系重合时，有：中各轴的投影分量，很容易得到在两个坐标系重合时，有：角，轴转动绕，XOvwOvwOwvkji,Oxyz),(xR100010001Rwzv

18、zzwvyywxvxxkkjkikkjjjijkijiiiy)R(x,xyzouvwUVWO图2-5ssin0sincos0001coiiux方向余弦阵方向余弦阵同理：同理： cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （ssin0sincos0001)R(x,co三个基本旋转矩阵三个基本旋转矩阵: : xyzouvwUWOxyzouvwUWOvn 合成旋转矩阵合成旋转矩阵: :例例1：在动坐标中有一固定点：在动坐标中有一固定点 ，相对固定参，相对固定参考坐标系考坐标系 做如下运动：做如下运动： R（x, 90）；）； R(z, 90)； R(y,

19、90)。求运动后点。求运动后点 在固定参考坐标系在固定参考坐标系 下的位置。下的位置。 TuvwPo1321OxyzuvwPoOxyz解解1：用画图的简单方法：用画图的简单方法 解解2：用分步计算的方法：用分步计算的方法 R（x, 90） R（z, 90） R（y, 90） 123113211000001001-000001P12131231100001000001001-0 P1312121310000001-00100100 P（2-14） （2-15） （2-16） 上述计算方法非常繁琐，可以通过一系列计算得到上述上述计算方法非常繁琐，可以通过一系列计算得到上述结果。将式（结果。将式（2

20、-14）（）（2-15）（）（2-16）联写为如下形式：）联写为如下形式： 11000133wvuzyxPPPRPPPR3x3为二者之间的关系矩阵，我们令：为二者之间的关系矩阵，我们令： ),(),(),RR33xRzRy（定义定义1： 当动坐标系当动坐标系 绕固定坐标系绕固定坐标系 各坐标轴顺序有限次各坐标轴顺序有限次转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘左乘。注意：注意：旋转矩阵间旋转矩阵间不可以交换不可以交换 uvwOOxyzn 平移齐次变换矩阵平移齐次变换矩阵1000100010001c) b (a TransHcba注意：

21、注意：平移矩阵间可以交换，平移矩阵间可以交换， 平移和旋转矩阵间不可以交换平移和旋转矩阵间不可以交换 zyxoowuvabc2.2.4 2.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，我们有：，我们有：1000

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

23、1000701030014100)R(Z,90 )90 R(y, 7) , 3- ,Trans(4Too（2-212-21）式（式（2-202-20）和式（）和式（2-212-21）无论在形式上，还是在结果上都是）无论在形式上，还是在结果上都是一致的。因此我们有如下的结论：一致的。因此我们有如下的结论：动坐标系在固定坐标系中的齐次变换有动坐标系在固定坐标系中的齐次变换有2 2种情况：种情况：定义定义1 1：如果所有的变换都是如果所有的变换都是相对于固定坐标系相对于固定坐标系中各坐标轴旋中各坐标轴旋转或平移，则依次转或平移，则依次左乘左乘，称为，称为绝对变换绝对变换。定义定义2 2：如果动坐标系

24、如果动坐标系相对于自身坐标系的当前坐标轴相对于自身坐标系的当前坐标轴旋转或旋转或平移，则齐次变换为依次平移，则齐次变换为依次右乘右乘，称为，称为相对变换相对变换。 结果均为动坐标系在固定坐标中的位姿结果均为动坐标系在固定坐标中的位姿（位置（位置+ +姿态）姿态）。相。相对于固定坐标系，对于固定坐标系，轴。轴相当于轴，轴相对于轴，轴相当于ZYXwv 也就是说，动坐标系绕自身坐标轴做齐次变换，也就是说，动坐标系绕自身坐标轴做齐次变换，要达到绕固要达到绕固定坐标系相等的结果，就应该用相反的顺序。定坐标系相等的结果，就应该用相反的顺序。 右乘的意义：右乘的意义： 机器人用到相对变换的机器人用到相对变换

25、的时候比较多时候比较多 例如机械手抓一个杯子，例如机械手抓一个杯子，如右图所示，手爪需要如右图所示，手爪需要转动一个角度才抓的牢，转动一个角度才抓的牢，相对于固定坐标系表达相对于固定坐标系表达太麻烦，可以直接根据太麻烦，可以直接根据手爪的坐标系表示手爪的坐标系表示 但也要知道在但也要知道在O中的位中的位姿，就用右乘的概念。姿，就用右乘的概念。 xyzoH2.2.5 2.2.5 绕通过原点的任意轴旋转的齐次变换绕通过原点的任意轴旋转的齐次变换 有时动坐标系有时动坐标系O O 可能绕过原点可能绕过原点O O的分量分别为的分量分别为rx、ry、rz的任的任意单位矢量意单位矢量r 转动转动角。角。研究

26、这种转动的好处是可用研究这种转动的好处是可用O O 绕某轴绕某轴r 的一次转动代替绕的一次转动代替绕O O各坐标轴的数次转动各坐标轴的数次转动为推导此旋转矩阵，可作下述为推导此旋转矩阵，可作下述5 5步变换：步变换：1.绕绕X 轴转轴转角，角， 使使r 轴处于轴处于XZ平面内平面内2.绕绕Y 轴转轴转-角，使角，使r 轴与轴与OZ轴重合轴重合3.绕绕OZ轴转动轴转动角角4.绕绕Y 轴转轴转角角5.绕绕X 轴转轴转-角角XYZrxryrzABCDBO51243rA由上图容易求出：由上图容易求出：2z2yyrrrsin2z2yzrrrcosxxrrrrOCsin2z2y2z2yrrrrrOBCBc

27、os由定义由定义1和定义和定义2，上述，上述5次旋转的合成旋转矩阵为：次旋转的合成旋转矩阵为：cossin0sin-cos0001cos0sin010sin-0cos1000cossin0sin-coscos0sin-010sin0coscossin0sincos0001RRRRRR,x,y, z,y,x, r（2-252-25）XYZrxryrzABCDBO51243rA带入式带入式（2-252-25），得），得cos)cos(1rsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrcos)cos(1rsinr)cos(1rrsinr)cos(1r

28、rcos)cos(1rR2zxzyyzxxzyyzx2yzyxzyx2x, r由该式可以推出由该式可以推出3个基本旋转矩阵个基本旋转矩阵2.2.6 2.2.6 齐次变换矩阵的几何意义齐次变换矩阵的几何意义 设，有一个手爪，即动坐标系设，有一个手爪，即动坐标系O O ，已知，已知， 初始位置初始位置重合，那么重合，那么O O 在在O O中的齐次坐标变换为：中的齐次坐标变换为： ，如果手爪转了一个角度，如果手爪转了一个角度， 则：则：111cbao1000100010001T 1111cba1000pppTzyyyxxxzzzyxwwwT反映了反映了O O 在在O O中的位置和姿态，即表示了该坐标

29、系原中的位置和姿态，即表示了该坐标系原点和各坐标轴单位矢量在固定坐标系中的位置和姿态。点和各坐标轴单位矢量在固定坐标系中的位置和姿态。该矩阵可以由该矩阵可以由4 4个子矩阵组成，写成如下形式：个子矩阵组成，写成如下形式：比例系数透视矩阵位置矢量旋转矩阵11311333wfPRTzzzyyyxxxwww33R为为姿态矩阵（旋转矩阵）姿态矩阵（旋转矩阵），表示动坐标系，表示动坐标系O O 在固定参考坐标系在固定参考坐标系O O中的姿态，即表示中的姿态，即表示O O 各坐标轴单位矢量在各坐标轴单位矢量在O O各轴上的投影各轴上的投影 为为位置矢量矩阵位置矢量矩阵，代表动坐标系，代表动坐标系O O 坐

30、标原坐标原点在固定参考坐标系点在固定参考坐标系O O中的位置中的位置 TzyxpppP13为为透视变换矩阵透视变换矩阵，在视觉中进行图像计算，在视觉中进行图像计算，一般置为一般置为0 0 00031f为为比例系数比例系数 1 11w如果需要求解如果需要求解O O在在O O 中的位置和姿态，此时的齐次变换矩中的位置和姿态，此时的齐次变换矩阵为阵为 ，即求逆矩阵：，即求逆矩阵： 1T1000-R-TTT1 -33T1pwpvp）（）（）（ kpjpippzyxkjizyxkvjvivvzyxkwjwiwwzyx其中：其中：这些式子以后经常遇到，这些式子以后经常遇到，在机器人计算中，所要在机器人计算

31、中，所要求的就是齐次变换矩阵求的就是齐次变换矩阵 2.2.7 2.2.7 透镜成像的齐次变换透镜成像的齐次变换 pp: Px1PP x1 1()1ppppTTy zy zypzpzpfypzpzpypfypfzpxpypfzpxpypypfypfypfypypypfypypffxpxpy 以光心为原点O，光轴与y轴重合，P为物点，用齐次坐标表示求 的齐次坐标，即求根据三角形相似原理：注意是负值， 是正值，所以实际上为相减关系又有设111000010000101110001ypzpypzppypzpfffxpxpxpypypypTfzpzpzpypf用矩阵表示：zyPypfozpfpzPpypp

32、: Px1PP x1 1()1ppppTTy zy zypzpzpfypzpzpypfypfzpxpypfzpxpypypfypfypfypypypfypypffxpxpy 以光心为原点O，光轴与y轴重合，P为物点，用齐次坐标表示求 的齐次坐标，即求根据三角形相似原理：注意是负值， 是正值，所以实际上为相减关系又有设111000010000101110001ypzpypzppypzpfffxpxpxpypypypTfzpzpzpypf用矩阵表示： 因此，进行机器人运动学计算时，不能省略透视矩阵，有因此，进行机器人运动学计算时，不能省略透视矩阵，有摄像头时，透视矩阵为摄像头时，透视矩阵为 0 -

33、 00 - 0，没有摄像头时为，没有摄像头时为0 0 0 0 0 0 。f11010010000100001T1T11pfzyxfyzyxzyxfpppfpppppp用矩阵表示：知识点：知识点： 1.1. 点和面的齐次坐标和齐次变换点和面的齐次坐标和齐次变换2.2. 三个基本旋转矩阵三个基本旋转矩阵3.3. 绝对变换：如果所有的变换都是相对于固定坐标系中各坐绝对变换：如果所有的变换都是相对于固定坐标系中各坐标轴旋转或平移，则依次左乘，称为绝对变换。标轴旋转或平移，则依次左乘，称为绝对变换。4.4. 相对变换：如果动坐标系相对于自身坐标系的当前坐标轴相对变换：如果动坐标系相对于自身坐标系的当前坐

34、标轴旋转或平移，则齐次变换为依次右乘，称为相对变换。旋转或平移，则齐次变换为依次右乘，称为相对变换。5.5. 绕任意轴旋转：绕任意轴旋转：5 5步顺序步顺序6.6. 透视变换透视变换知识点：知识点： 三 个 基 本 旋三 个 基 本 旋转矩阵转矩阵cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （ssin0sincos0001)R(x,co例题例题1 1：O O 与与O O初始重合，初始重合，O O 作如下运动：绕作如下运动：绕Z Z轴转动轴转动3030 ；绕绕X X轴转动轴转动6060 ；绕；绕Y Y轴转动轴转动9090 。求。求T T。 100001000030cos30sin0030sin30cosR11000060cos60sin0060sin60cos000012R1000090cos090sin0010090sin090cos3R1000002/12/302/34/34/102/14/34/3123RRRT例题例题2 2：O O