13 工业机器人机构学工业机器人机构学13.1 概 述 概 述13.2 工业机器人的组成 工业机器人的组成13.3 工业机器人的分类与性能 工业机器人的分类与性能13.4 工业机器人的运动学基础 工业机器人的运动学基础 13.5 工业机器人的正向运动学 工业机器人的正向运动学 13.6 工业机器人的逆向运动学 工业机器人的逆向运动学 提提 要 要 研究了研究了坐标变换与空间物体坐标变换与空间物体的位姿与位移的齐次坐标表达；的位姿与位移的齐次坐标表达；研究了研究了已知各个关节的相对运动已知各个关节的相对运动时，如何确定工业机器人末端操时，如何确定工业机器人末端操作器的位姿；作器的位姿；研究了研究了已知目标对已知目标对象的位姿时，如何确定工业机器象的位姿时，如何确定工业机器人各个关节的相对运动量人各个关节的相对运动量 介绍了工业机器人的组成原理、分类与工作性能特点介绍了工业机器人的组成原理、分类与工作性能特点Chapter 13 Kinematics of Industrial Robots13 工业机器人机构学工业机器人机构学 13.1 概 述 概 述 工业机器人是用来搬运材料、零件与工具，进行焊接与喷涂 工业机器人是用来搬运材料、零件与工具，进行焊接与喷涂的可再编程的多功能机械手，通过调用不同的程序来完成预设的的可再编程的多功能机械手，通过调用不同的程序来完成预设的多种工作任务。
多种工作任务 图图13－－1 工业机器人 工业机器人(a) (b) 工业机器人由三大部分六个子系统组成 工业机器人由三大部分六个子系统组成三大部分三大部分是机械是机械部分、传感部分和控制部分部分、传感部分和控制部分六个系统六个系统是驱动系统、机构与结是驱动系统、机构与结构系统、感觉系统、机器人与环境交互系统、人机交互系统和构系统、感觉系统、机器人与环境交互系统、人机交互系统和控制系统控制系统 图图13－－2 汽车生产线上的工业机器人 汽车生产线上的工业机器人 1. 机器人的机构与结构系统机器人的机构与结构系统 工业机器人的机械部分由三部分组成，即机身、手臂和末 工业机器人的机械部分由三部分组成，即机身、手臂和末端操作器机身可以是固定的，也可以是移动的手臂进一步端操作器机身可以是固定的，也可以是移动的手臂进一步划分为上臂和下臂，上臂与机身形成肩关节，上臂与下臂形成划分为上臂和下臂，上臂与机身形成肩关节，上臂与下臂形成肘关节，下臂与末端操作器形成碗关节，如图肘关节，下臂与末端操作器形成碗关节，如图13-3所示。
所示 图图13－－3 机器人的机构与结构机器人的机构与结构(a)(b) 图图13F01 喷涂机器人喷涂机器人(1) 喷涂机器的一种类型喷涂机器的一种类型 2. 机器人手部的机构与结构系统机器人手部的机构与结构系统图图13-4 单自由度操作器单自由度操作器图图13-5 多自由度操作器多自由度操作器(2) 具有多个自由具有多个自由 度的末端操作器度的末端操作器(1) 具有一个相对自具有一个相对自 由度的末端操作器由度的末端操作器 13.3 工业机器人的分类与性能 工业机器人的分类与性能 直直角角坐坐标标型型操操作作机机如如图图13－－6所所示示，，它它有有三三个个移移动动关关节节(PPP)，可使末端操作器作三个方向的独立位移可使末端操作器作三个方向的独立位移1. 直角坐标型直角坐标型yxzPxPzPy末端操作器123PPP图图13－－6 直角坐标型操作机 直角坐标型操作机 该种型式的工业机器人， 该种型式的工业机器人，定位精度较高，空间轨迹规定位精度较高，空间轨迹规划与求解相对较容易，计算划与求解相对较容易，计算机控制相对较简单它的不机控制相对较简单它的不足是空间尺寸较大，运动的足是空间尺寸较大，运动的灵活性相对较差，运动的速灵活性相对较差，运动的速度相对较低。
度相对较低 2. 圆柱坐标型圆柱坐标型 圆圆柱柱坐坐标标型型操操作作机机如如图图13－－7所所示示，，它它有有两两个个移移动动关关节节和和一一个个转转动动关关节节(PPR)，，末末端端操操作作器器的的安安装装轴轴线线之之位位姿姿由由(z，，r，，θ)坐坐标标予予以以表表示示该该种种型型式式的的工工业业机机器器人人，，空空间间尺尺寸寸较较小小，，工工作作范范围围较较大大，，末末端端操操作作器器可可获获得得较较高高的的运运动动速速度度它它的的缺缺点点是是末末端端操操作作器器离离z轴轴愈愈远远，，其其切切向向线线位位移移的的分辨精度就愈低分辨精度就愈低θrZ图图13－－7 圆柱坐标型操作机 圆柱坐标型操作机 3. 球坐标型球坐标型 球球坐坐标标型型操操作作机机如如图图13－－8所所示示，，它它有有两两个个转转动动关关节节和和一一个个移移动动关关节节(RRP)，，末末端端操操作作器器的的安安装装轴轴线线之之位位姿姿由由(θ，，φ，， r)坐坐标标予予以以表表示示该该种种型型式式的的工工业业机机器器人人，，空空间间尺尺寸较小，工作范围较大寸较小，工作范围较大zθrxyRRPφ图图13－－8 球坐标型操作机 球坐标型操作机 4. 关节型关节型 腕腕关关节节的的转转动动θZ3属属于于末末端端操操作作器器的的自自由由度度。
该该种种结结构构的的工工业业机机器器人人，，空空间间尺尺寸寸相相对对较较小小，，工工作作范范围围相相对对较较大大，，还还可可以以绕绕过过机机座座周周围围的的障障碍碍物物，，是是目目前前应应用用较较多多的的一种机型一种机型控制控制系统系统Z1Y0谐波减速器谐波减速器光电编码盘光电编码盘末端执行器末端执行器X1伺服伺服电机电机θY0θZ1θZ2θZ3上臂上臂下臂下臂RRRY1X0Z0图图13－－9 关节型操作机 关节型操作机 关节型操作机如图 关节型操作机如图13－－9所示，它有三个转动关节所示，它有三个转动关节(RRR)，，即机身上部相对于下部的转动即机身上部相对于下部的转动θY0，肩关节的转动，肩关节的转动θZ1和肘关节的和肘关节的转动转动θZ2 13.4 工业机器人的运动学基础 工业机器人的运动学基础 工业机器人是由若干个 工业机器人是由若干个关节所联系起来的一种开链，关节所联系起来的一种开链，其一端固结在机座上，另一其一端固结在机座上，另一端安装有末端操作器端安装有末端操作器确定确定工业机器人末端操作器安装工业机器人末端操作器安装轴线的方位，轴线的方位，确定确定末端操作末端操作器的位姿与位移，器的位姿与位移，确定确定工业工业机器人的操作对象，即目标机器人的操作对象，即目标物体的位姿与位移，构成了物体的位姿与位移，构成了工业机器人运动学基础应该工业机器人运动学基础应该研究的一部分工作。
研究的一部分工作图图13－－3 机器人的机构与结构机器人的机构与结构末端操作器末端操作器(a) 13.4.1 目标物体的空间转动矩阵 目标物体的空间转动矩阵 一一个个通通过过坐坐标标原原点点的的矢矢量量V1绕绕通通过过坐坐标标原原点点的的单单位位矢矢量量u转转动动φ角角到到达达V2，，要要求求确确定定V2的的位位姿姿为为了了确确定定矢矢量量V1绕绕通通过过坐标原点的单位矢量坐标原点的单位矢量u转动转动φ角到达角到达V2的位姿，将它作如下转动的位姿，将它作如下转动图图13－－10 目标物体的空间转动 目标物体的空间转动OV1uzyxuxuz②②γ-β①①④④-γ③③φφuyV2⑤⑤+β 1. 平面内平面内单位矢量单位矢量绕绕坐标轴坐标轴的转动矩阵的转动矩阵UU'θxyUxUyU'yU'xθUxUyθ图图13.4F01 平面内单平面内单位矢量绕坐标轴的转动位矢量绕坐标轴的转动 2. 空间内空间内单位矢量绕单位矢量绕坐标轴坐标轴的转动矩阵的转动矩阵Ouzyxuxuzφuyγγβ图图13－－10(a) 空间内单位 空间内单位矢量绕矢量绕坐标轴线的坐标轴线的转动转动 矢量矢量V1绕通过坐标原点的单位矢量绕通过坐标原点的单位矢量u转动转动φ角的矩阵角的矩阵 矢量矢量V1绕通过坐标原点的单位矢量绕通过坐标原点的单位矢量u转动转动φ角的矩阵角的矩阵 当当式式(13－－7)中中的的每每一一个个元元素素为为已已知知时时，，利利用用式式(13－－5)中中的的元元素素与与式式(13－－7)中中的的前前3行行3列列元元素素对对应应相相等等，，即即可可求求出出矢矢量量V1绕矢量绕矢量u转动的转转动的转φ角和矢量角和矢量u的姿态。
的姿态 矢量矢量V1绕矢量绕矢量u转动的转动的φ角和矢量角和矢量u的姿态为的姿态为 [例例13-1] 图图13－－11为为单单臂臂操操作作机机械械手手，，手手臂臂相相对对于于机机身身拥拥有有一一个个转转动动自自由由度度，，手手腕腕相相对对于于手手臂臂拥拥有有一一个个转转动动自自由由度度已已知知手手腕上的坐标系腕上的坐标系oxyz相对于机身坐标系相对于机身坐标系OXYZ的位姿矩阵的位姿矩阵SW为为XYyxzOOz1y1x2z2y2O2O1Z图图13－－11 单臂操作机械手单臂操作机械身机身手臂手臂手腕手腕x1 (2) 若手臂相对于机身不动，若手臂相对于机身不动，手腕上的坐标系手腕上的坐标系Oxyz相对相对于手臂上的于手臂上的z轴转动＋轴转动＋90º，，则坐标系则坐标系oxyz转到坐标系转到坐标系O2x2y2z2试写出以上两种试写出以上两种转动的矩阵转动的矩阵SW1、、SW2 SW中前三行前三列的元素表示手腕坐标系的姿态，中前三行前三列的元素表示手腕坐标系的姿态，[2,6,2]T表示手腕坐标系原点的位置表示手腕坐标系原点的位置1)若手臂相对于机身坐标系若手臂相对于机身坐标系OXYZ的的Z轴转动＋轴转动＋90º，则坐标系，则坐标系oxyz转到坐标系转到坐标系o1x1y1z1。
XYyxzOOz1y1x2z2y2O2O1Z图图13－－11 单臂操作机械手单臂操作机械身机身手臂手臂手腕手腕x1 坐标系坐标系O1x1y1z1在固定坐标系在固定坐标系OXYZ的位姿矩阵的位姿矩阵SW1为为O2x2y2z2坐标系在固定坐标系坐标系在固定坐标系OXYZ的位姿矩阵的位姿矩阵SW2为为 13.4.2 坐标系之间的空间变换矩阵 坐标系之间的空间变换矩阵 XYZxyzOO′vvzvxvyVxVyVzP图图13－－12点在坐点在坐标系之间的变换标系之间的变换 设设单单位位矢矢量量v在在坐坐标标系系O'xyz中中的的投投影影分分别别为为vx、、vy和和vz；；矢矢量量P在在坐坐标标系系OXYZ中中的的投投影影分分别别为为PX，，PY和和PZ；；x轴轴在在坐坐标标系系OXYZ中中X、、Y和和Z上上的的投投影影分分别别为为txX、、txY和和txZ；；y轴轴在在坐坐标标系系OXYZ中中X、、Y和和Z上上的的投投影影分分别别为为tyX、、tyY和和tyZ；；z轴轴在在坐坐标标系系OXYZ中中X、、Y和和Z上上的的投投影影分分别别为为tzX、、tzY和和tzZ 为此，连杆坐标系 为此，连杆坐标系O׳xyz相对于固相对于固定坐标系定坐标系OXYZ的位姿为的位姿为 XYZxyzOO′vvzvxvyVxVyVzP图图13－－12点在坐点在坐标系之间的变换标系之间的变换txX、、txY和和txZ的表达式分别为的表达式分别为txX＝＝cos(x，，X)，，txY＝＝cos(x，，Y)，，txZ＝＝cos(x，，Z)，其余的关系式类推。
其余的关系式类推 为了计算机求解方便，将上式 为了计算机求解方便，将上式改写为齐次坐标形式改写为齐次坐标形式 13.4.3 目标物体的齐次坐标表示目标物体的齐次坐标表示 在如图 在如图13－－13a所示的坐标系所示的坐标系OXYZ中放置一个楔块，在中放置一个楔块，在楔块上设置坐标系楔块上设置坐标系oxyz，其上的特征点为，其上的特征点为A1，，A2，，A3，，A4，，A5和和A6这些特征点在自身坐标系这些特征点在自身坐标系oxyz中的坐标分别为中的坐标分别为A1（（1,0,0），），A2（－（－1,0,0），），A3（－（－1,0,2），），A4（（1,0,2），），A5（（1,4,0），），A6（－（－1,4,0）XZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4图图13－－13 目标物体的齐次坐标变换目标物体的齐次坐标变换(a)(b)OoA1 用齐次坐标用齐次坐标Wxyz(4×6)表示这些点在自身坐标系表示这些点在自身坐标系oxyz中的位置为中的位置为XZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4图图13－－13 目标物体的齐次坐标变换目标物体的齐次坐标变换(a)(b)OoA1 若让楔块绕若让楔块绕Z轴转过轴转过90º，再绕，再绕Y轴转过轴转过90º，最后沿，最后沿X轴方轴方向平移向平移4，则楔块到达图，则楔块到达图12-13b所示的位置。
以上的变换所示的位置以上的变换T[xyz→XYZ] 为为XZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4图图13－－13 目标物体的齐次坐标变换目标物体的齐次坐标变换(a)(b)OoA1 此时，楔块上的特征点在 此时，楔块上的特征点在OXYZ坐标系中的齐次坐标坐标系中的齐次坐标WXYZ(4×6)为为XZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4图图13－－13 目标物体的齐次坐标变换目标物体的齐次坐标变换(a)(b)OoA1 由图 由图13－－13b也可以得到坐标系也可以得到坐标系OXYZ在坐标系在坐标系oxyz中中的齐次坐标的齐次坐标 已知 已知X轴轴的方位为的方位为[0,0,1,0]T，， Y轴轴的方位为的方位为[1,0,0,0]T，， Z轴轴的方位为的方位为[0,1,0,0]T，坐标系，坐标系OXYZ的原点的原点O在坐标系在坐标系oxyz中的位中的位置为置为[0,0,－－4,1] TXZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4图图13－－13 目标物体的齐次坐标变换目标物体的齐次坐标变换(a)(b)OoA1 为此，坐标系为此，坐标系OXYZ在坐标系在坐标系oxyz中的位姿矩阵中的位姿矩阵T[XYZ→xyz]为为XZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4图图13－－13 目标物体的齐次坐标变换目标物体的齐次坐标变换(a)(b)OoA1 可以证明，可以证明，T[XY Z→xyz]与与T[xyz→XYZ]的乘积为单位矩阵，即的乘积为单位矩阵，即T[XY Z→xyz]＝＝T －１－１[xyz→XYZ]。
若若T[xyz→XYZ]的一般形式为的一般形式为则则T[xyz→XYZ]的逆变换矩阵的逆变换矩阵T[XYZ→ xyz]为为 13.4.4 刚体的空间位移矩阵 刚体的空间位移矩阵 设设已已知知p1=[p1X p1Y p1Z]T，，q1=[q1X q1Y q1Z]T，，则则q=[qX qY qZ]T 的的矢矢量量表表达达式式与与矩阵表达式分别为矩阵表达式分别为 在如图 在如图13－－14所示的坐标系所示的坐标系OXYZ中有一个连杆，连杆的初中有一个连杆，连杆的初始位置用始位置用p1q1表示，终止位置用表示，终止位置用pq表示，表示，p1点的位置矢量用点的位置矢量用R表表示，连杆上的示，连杆上的p1点沿一单位矢量点沿一单位矢量u位移位移s，同时连杆绕矢量，同时连杆绕矢量u转动转动φ角，现在确定角，现在确定q点相对于点相对于q1点的位置点的位置p1ZYXp=p1+suq1sqO图图13－－14 刚体的空间位移刚体的空间位移Ruq' 式式(13－－19)中的中的 同式同式(13－－5)式式(13－－19)右右端端左左侧侧的的矩矩阵阵称称为为刚刚体的有限螺旋位移矩阵。
体的有限螺旋位移矩阵p1ZYXp=p1+suq1sqO图图13－－14 刚体的空间位移刚体的空间位移Ruq' 13.4.5 欧拉角表示的变换矩阵 欧拉角表示的变换矩阵 在图 在图13－－15a所示的固定坐标系所示的固定坐标系OXYZ中放置一个矢量中放置一个矢量U，，其初始位置为其初始位置为U1，坐标系，坐标系OX'Y' Z'是由是由OXYZ绕绕Z轴转轴转ψ角度而角度而得到的位置，此时，矢量得到的位置，此时，矢量U1转到转到U2的位置；坐标系的位置；坐标系OX"Y"Z"是由是由OX'Y'Z'绕绕X'轴转轴转θ角度而得到的位置，此时，矢量角度而得到的位置，此时，矢量U2转到转到U3的位置；矢量的位置；矢量U3再绕再绕Z"转动转动φ角而到达角而到达U4的位置XY″X′Y′X″Z′ZYθZ″ψψθO123U1U2U3U4ψθ4XZYψθO1U1U2U3U4423(a)(b)图图13－－15 欧拉角表示的变换欧拉角表示的变换φφφ 在以上的相对转动中，每次都是相对于动坐标系进行的， 在以上的相对转动中，每次都是相对于动坐标系进行的，而不是相对于固定坐标系进行的而不是相对于固定坐标系进行的。
ψ、、θ和和φ 称为欧拉角称为欧拉角 若让所有的转动都是相对于固定坐标系 若让所有的转动都是相对于固定坐标系OXYZ进行的，如图进行的，如图13-15b所示，且转动顺序为，先绕所示，且转动顺序为，先绕Z轴转轴转φ角度，再绕角度，再绕X轴转轴转θ角度，角度，最后绕最后绕Z轴转轴转ψ角度转动变换矩阵为角度转动变换矩阵为 XY″X′Y′X″Z′ZYθZ″ψψθO123U1U2U3U4ψθ4XZYψθO1U1U2U3U4423(a)(b)图图13－－15 欧拉角表示的变换欧拉角表示的变换φφφ 以上两种变换的展开式均为以上两种变换的展开式均为XY″X′Y′X″Z′ZYθZ″ψψθO123U1U2U3U4ψθ4XZYψθO1U1U2U3U4423(a)(b)图图13－－15 欧拉角表示的变换欧拉角表示的变换φφφ 13.4.6 转动关节之间的位移矩阵 转动关节之间的位移矩阵 连杆连杆n右端的坐标系右端的坐标系OnXnYnZn在左端的坐标系在左端的坐标系On-1Xn-1Yn-1Zn-1中的齐次变换矩阵中的齐次变换矩阵Tn为为OnZnaXn连杆连杆nθZ＋＋1XnYn关节关节n-1关节关节n关节关节n＋＋1θZθZ-1θXn图图13－－16 转动关节转动关节 连杆连杆n-1连杆连杆n＋＋1Xn-1θZn-1OZ(n－－1)OZ(n-2)Yn-2Zn-1Zn-2Yn-1 化简后得转动关节之间的位移矩阵为化简后得转动关节之间的位移矩阵为OnZnaXn连杆连杆nθZ＋＋1XnYn关节关节n-1关节关节n关节关节n＋＋1θZθZ-1θXn图图13－－16 转动关节转动关节 连杆连杆n-1连杆连杆n＋＋1Xn-1θZn-1OZ(n－－1)OZ(n-2)Yn-2Zn-1Zn-2Yn-1 13.5 工业机器人的正向运动学 工业机器人的正向运动学 工业机器人的 工业机器人的正向运动正向运动学学是指已知各关节的类型、是指已知各关节的类型、相邻关节之间的尺寸和相邻相邻关节之间的尺寸和相邻关节相对运动量的大小时，关节相对运动量的大小时，如何确定工业机器人末端操如何确定工业机器人末端操作器在固定坐标系中的位姿。
作器在固定坐标系中的位姿a)图图13－－1 工业机器人 工业机器人 设设工工业业机机器器人人中中的的一一个个连连杆杆一一端端关关节节上上的的坐坐标标系系相相对对于于另另一一端端关关节节上上的的坐坐标标系系的的位位姿姿由由齐齐次次变变换换矩矩阵阵Ti表表示示，，设设T1表表示示第第一一个个连连杆杆一一端端动动关关节节上上的的坐坐标标系系相相对对于于另另一一端端固固定定关关节节上上的的坐坐标标系系的的位位姿姿；；设设第第二二个个连连杆杆的的一一端端与与第第一一个个连连杆杆形形成成动动关关节节，，另另一一端端与与下下一一个个连连杆杆形形成成动动关关节节，，齐齐次次变变换换矩矩阵阵用用T2表表示示，，则则第第二二个个连连杆杆相相对对于于固固定定关关节节上上的的坐坐标标系系的的位位姿姿W2为为W2＝＝T1 T2依依次次类类推推，，若若有有六六个个连连杆杆，，则则第第六六个个连连杆杆相相对对于于固固定定关关节节上上的的坐标系的位姿坐标系的位姿W6为为W6＝＝T1 T2 T3 T4 T5 T6 (12－－22) W6＝＝T1 T2 T3 T4 T5 T6 (13－－22) W6的表现形式可以用以下的的表现形式可以用以下的 (4×4)矩阵予以表示矩阵予以表示 式式(13－－23)右右端端的的前前三三列列前前三三行行表表示示末末端端操操作作器器的的姿姿态态，，第四列前三行表示末端操作器的位置。
第四列前三行表示末端操作器的位置13.5.1 平面关节型机器人的正向运动方程 平面关节型机器人的正向运动方程 图图13－－17a所所示示为为由由一一个个肩肩关关节节、、一一个个肘肘关关节节和和一一个个腕腕关关节节组组成成的的平平面面关关节节型型的的机机器器人人简简图图，，它它的的三三个个关关节节的的轴轴线线Z0、、Z1、、Z2是平行的，它的结构参数如表是平行的，它的结构参数如表13-1所示 表表13－－1 平面关节型的机器人的结构参数 平面关节型的机器人的结构参数 连杆序连杆序号号n关于关于Zn轴轴的转角的转角两连杆之间两连杆之间的距离的距离dZn-1连杆的长度连杆的长度aXn连杆的连杆的扭角扭角θXn1θZ1=θ1（逆时（逆时针为正）针为正）0aX1=a1=(100mm)θX1=02θZ2=θ2（逆时（逆时针为正）针为正）0aX2=a2=(100mm)θX2=03θZ3=θ3（逆时（逆时针为正）针为正）0aX3=a3=(100mm)θX3=0 图 图13－－17a所示的平面关节型机器人的运动分析简图所示的平面关节型机器人的运动分析简图如图如图13－－17b所示该平面关节型机器人的运动学方程为所示。
该平面关节型机器人的运动学方程为W3=T1 T2 T3 图图13－－17 平面关节型机器人平面关节型机器人X0Y0O0O1Y1X1Y2X2X3Y3O2O3θ3θ2θ1(b)O0Y0Y1Y2Y3X0X1X2X3O1O2O3θ1θ2θ3(c)(a)Z0Z1Z2Z3O0O1O2O3Y0Y1Y2Y3X0X1X2X3123aX1aX2aX3θ3θ2θ10 W3＝＝T1 T2 T3中每一项的矩阵表达式为中每一项的矩阵表达式为 W3＝＝T1 T2 T3矩阵表达式为矩阵表达式为图图13－－17 平面关节型机器人平面关节型机器人X0Y0O0O1Y1X1Y2X2X3Y3O2O3θ3θ2θ1(b)O0Y0Y1Y2Y3X0X1X2X3O1O2O3θ1θ2θ3(c)(a)Z0Z1Z2Z3O0O1O2O3Y0Y1Y2Y3X0X1X2X3123aX1aX2aX3θ3θ2θ1 若转角 若转角θ1=30 º ，，θ2=－－60º和和θ3=－－30 º ，，如图如图13－－17c所示，则所示，则该平面关节型机器人的该平面关节型机器人的手部坐标系手部坐标系O3X3Y3Z3在在固定坐标系固定坐标系O0X0Y0Z0中中的位姿的位姿W3为为图图13－－17 平面关节型机器人平面关节型机器人X0Y0O0O1Y1X1Y2X2X3Y3O2O3θ3θ2θ1(b)O0Y0Y1Y2Y3X0X1X2X3O1O2O3θ1θ2θ3(c)(a)计计 算算 演演 示示Z0Z1Z2Z3O0O1O2O3Y0Y1Y2Y3X0X1X2X3123aX1aX2aX3θ3θ2θ10 13.5.2 斯坦福机器人的正向运动方程 斯坦福机器人的正向运动方程 X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H1. 坐坐标标系系X1Y1Z1相相对对于于固固定坐标系定坐标系X0Y0Z0的位姿的位姿 图 图13－－18所示为斯坦福机器人的结构简图，针对图示的坐所示为斯坦福机器人的结构简图，针对图示的坐标系，其参数关系如表标系，其参数关系如表12－－2所示。
所示 首先，将坐标系 首先，将坐标系X0Y0Z0绕绕X0轴转动轴转动θX1＝－＝－90°得坐标系得坐标系X1Y1Z1，然后，绕，然后，绕Z0轴转动轴转动θZ0该变换矩阵为该变换矩阵为下面求末端操作器的位姿下面求末端操作器的位姿 表表13－－2 斯坦福机器人的结构参数 斯坦福机器人的结构参数杆件编号杆件编号关节关于关节关于Zi轴的转角轴的转角关节关于关节关于Xi轴的扭转角轴的扭转角θXn杆件长度杆件长度aXn1θZ0＝＝θ1＝＝(90°)－－90°02θZ1＝＝θ2＝＝(90°)90°d2=100mm3θZ2＝＝θ3＝＝0°0°d3=300mm4θZ3＝＝θ4＝＝(90°)－－90°05θZ4＝＝θ5＝＝(90°)90°06θZ5＝＝θ6＝＝(90°)0°H=50mm 1) 坐标系坐标系X1Y1Z1相对于固定坐标系相对于固定坐标系X0Y0Z0的位姿的位姿X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H 2) 坐标系坐标系X2Y2Z2相对于相对于X1Y1Z1的位姿的位姿 X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H 3) 坐标系坐标系X3Y3Z3相对于相对于X2Y2Z2的位姿的位姿 X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H 4) 手腕坐标系相对于 手腕坐标系相对于X3Y3Z3的位姿的位姿 (1) 坐标系坐标系X4Y4Z4相对于相对于X3Y3Z3的变换矩阵的变换矩阵 X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H (2) 坐标系坐标系X5Y5Z5相对于相对于X4Y4Z4的变换矩阵的变换矩阵X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H (3) 坐标系坐标系X6Y6Z6相对于相对于X5Y5Z5的变换矩阵的变换矩阵 X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H 一旦知道了 一旦知道了T1～～T6，则任意杆件之间的变换矩阵可以使用以，则任意杆件之间的变换矩阵可以使用以上公式求解出来。
上公式求解出来 下下面面依依次次给给出出[5T6]、、非非相相邻邻杆杆件件之之间间的的变变换换矩矩阵阵[4T6]、、[3T6]、、[2T6]和和[1T6]的的矩矩阵阵乘乘积积形形式式，，即即 [5T6] =T6 ，，[4T6] = T5T6 ，，[3T6]＝＝T4T5T6，，[2T6]＝＝T3T4T5T6和和[1T6]＝＝T2T3T4T5T6的的表表达达式式首首先先，，给给出杆件出杆件6相对于相对于4的位姿矩阵为的位姿矩阵为 其次，给出杆件其次，给出杆件6相对于相对于3的位姿矩阵的位姿矩阵[3T6]为为 再次，给出杆件再次，给出杆件6相对于相对于2的位姿矩阵的位姿矩阵[2T6] 为为 最后，给出杆件最后，给出杆件6相对于相对于1的位姿矩阵的位姿矩阵[1T6]为为 于是，手部坐标系于是，手部坐标系X6Y6Z6相对于固定坐标系相对于固定坐标系X0Y0Z0的变换的变换矩阵矩阵[0T6]为为[0T6]＝＝T1T2T3T4T5T6 (13－－39) 若若给给定定各各个个关关节节关关于于Zi轴轴的的转转角角θi，，d2、、d3和和H的的大大小小，，如表如表13－－2所示，设所示，设[0T6]的矩阵元素如下的矩阵元素如下 式式(13－－37)中各个元素的表达式分别为中各个元素的表达式分别为NX＝＝C1[C2(C4C5C6－－S4S6)－－C6S2S5]－－ (C5C6 S4＋＋C4S6) S1NY＝＝[C2(C4C5C6－－S4S6)－－C6S2S5] S1＋＋ C1 (C5C6 S4＋＋C4S6) NZ＝－＝－(C4C5C6－－S4S6) S2－－C2 C6S5MX＝＝C1[－－C2(C4C5S6＋＋C6S4)＋＋S2S5S6]－－ (－－C5 S4S6＋＋C4C6) S1MY＝＝S1[－－C2(C4C5S6＋＋C6S4)＋＋S2S5S6]＋＋ C1 (－－C5 S4S6＋＋C4C6) MZ＝＝(C4C5S6＋＋C6S4) S2＋＋C2S5 S6QX＝＝C1 (C2C4S5＋＋C5S2)－－S1S4 S5QY＝＝(C2C4S5＋＋C5S2) S1＋＋C1S4 S5QZ＝－＝－C4S2 S5＋＋C2C5PX＝＝C1 d3S2－－d2S1＋＋H(C1C2C4S5－－C1C5S2－－S1S4S5)PY＝＝S1 d3S2＋＋C1d2＋＋H(S1C2C4S5－－C5S1S2＋＋C1S4S5)PZ＝＝C2d3－－H(C4S2S5＋＋C2C5) 以上诸式中， 以上诸式中，Ci＝＝cosθi，，Si＝＝sinθi，，i＝＝1～～6。
则则[0T6]的运算结果为的运算结果为 X1X0Y10图图12－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H 13.6 工业机器人的逆向运动学 工业机器人的逆向运动学 工业机器人的逆向运动学是 工业机器人的逆向运动学是指已知被作对象的初始位姿与终指已知被作对象的初始位姿与终止位姿时，止位姿时，如何确定工业机器人如何确定工业机器人各关节的相对运动量的大小以及各关节的相对运动量的大小以及末端操作器的相对位姿末端操作器的相对位姿根据被作对象的初始位姿与终止位姿，作对象的初始位姿与终止位姿，确定工业机器人各关节的相对运确定工业机器人各关节的相对运动量的大小是对工业机器人进行动量的大小是对工业机器人进行运动控制的基础运动控制的基础 X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H 下面以图 下面以图13－－18所示的斯坦福机器人为例，说明工所示的斯坦福机器人为例，说明工业机器人的逆向运动学的求解方法。
业机器人的逆向运动学的求解方法 斯斯坦坦福福机机器器人人手手部部坐坐标标系系X6Y6Z6相相 对对 于于 固固 定定 坐坐 标标 系系X0Y0Z0的的变变换换矩矩阵阵[0T6]如如式式(12-39)所所 示示 ，， 即即 [0T6]＝＝T1T2T3T4T5T6，，[0T6]的的矩矩阵阵形形式式如如式式(13-40) 所所示示设设给给定定了了所所有有的的结结构构参参数数并并已已知知手手部部坐坐标标系系X6Y6Z6相相对对于于固固定定坐坐标标系系X0Y0Z0的的位位姿姿(式式13－－40)，，令令H＝＝００下下面面求求各各个个关关节节的的相对运动量的大小相对运动量的大小X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H (1) 求坐标系求坐标系X1Y1Z1相对于相对于X0Y0Z0的转角的转角θ1 用用T1－－1左左乘乘式式(13－－40)，，得得T1－－1 [ 0T6]＝＝T1－－1T1T2T3T4T5T6＝＝T2T3T4T5T6，，X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H其中其中T1－－1由式由式(13－－29)以及式以及式(13－－15) 与与(13－－16)的变换关系得的变换关系得到，于是到，于是T1－－1和和T1－－1 [ 0T6]分别分别为为 求坐标系求坐标系X1Y1Z1相对于相对于X0Y0Z0的转角的转角θ1 式式(13－－42)右右端端T2T3T4T5T6的的展展开开矩矩阵阵如如式式(13－－38)所所示示，，只要令式只要令式(13－－38)中的中的H＝＝0即可。
即可 下面展开式下面展开式(13－－42)中间两个矩阵的乘积，得中间两个矩阵的乘积，得T1－－1 [ 0T6]为为 令式 令式(13－－43)两个矩阵的第三行第四列的对应元素相等，两个矩阵的第三行第四列的对应元素相等，得含有得含有θ1的三角方程以及的三角方程以及θ1的解分别为的解分别为 －－PXsinθ1＋＋PYcosθ1＝＝d2 (13－－44) (2) 求坐标系求坐标系X2Y2Z2相对于相对于X1Y1Z1的转角的转角θ2 PXcosθ1＋＋PYsinθ1＝＝d3sinθ2 (13－－46)－－PZ＝－＝－d3cosθ2 (13－－47) 式式(13－－46) 除以式除以式(13－－47)得得θ2为为 令式 令式(13－－43) 两个矩阵的第一行第四列的对应元素相等，两个矩阵的第一行第四列的对应元素相等，第二行第四列的对应元素相等，得含有第二行第四列的对应元素相等，得含有θ2的三角方程为的三角方程为 (3) 求坐标系求坐标系X3Y3Z3相对于相对于X2Y2Z2的位移的位移d3 将将式式(13－－46)两两端端乘乘以以sinθ2 ，，式式(13－－47)两两端端乘乘以以cosθ2 ，，然后相加得然后相加得d3为为X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H (4) 求坐标系求坐标系X4Y4Z4相对于相对于X3Y3Z3的转角的转角θ4 由于 由于[3T6]＝＝T4T5T6，，T4 －－1[3T6]＝＝T4 －－1T4T5T6＝＝T5T6＝＝[4T6]＝＝T4 －－1 T3 －－1 T2 －－1 T1 －－1[0T6]。
X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H 所以，由式 所以，由式(13－－32)、、 (13－－31)、、(13－－30)以及式以及式(13－－15) 与与(13－－16)的变换关系，首先求出的变换关系，首先求出T4 、、T3和和T2的逆矩阵的逆矩阵T4－－1 、、T3－－1和和 T2－－1分别为分别为 (4) 求坐标系求坐标系X4Y4Z4相对于相对于X3Y3Z3的转角的转角θ4X1X0Y10图图13－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H 为此，为此，[4T6]＝＝T4 －－1 T3 －－1 T2 －－1 T1 －－1[0T6]的表达式为的表达式为 将将[4T6]＝＝T4 －－1 T3 －－1 T2 －－1 T1 －－1[0T6]展开展开[4T6]＝＝T4 －－1 T3 －－1 T2 －－1 T1 －－1[0T6]＝＝(13-53) 令式 令式(13－－53) 与式与式(13－－35)的第三行第三列的矩阵元素的第三行第三列的矩阵元素对应相等，得关于转角对应相等，得关于转角θ4的三角方程为的三角方程为由式由式(13－－54)得得θ4为为 (5) 求坐标系 求坐标系X5Y5Z5相对于相对于X4Y4Z4的转角的转角θ5 令式 令式(13－－53) 与与(13－－35)的第一行第三列的矩阵元素对应相的第一行第三列的矩阵元素对应相等，第二行第三列的矩阵元素对应相等，得关于转角等，第二行第三列的矩阵元素对应相等，得关于转角θ5的三角方的三角方程为程为由式由式(13－－56)、式、式(13－－57)得得θ5为为 (6) 求坐标系求坐标系X6Y6Z6相对于相对于X5Y5Z5的转角的转角θ6 由式 由式(13－－33) 以及式以及式(13－－15) 与与(13－－16)的变换关系得的变换关系得T5的逆矩阵的逆矩阵T5－－1为为 X1X0Y10图图12－－18 斯坦福机器人斯坦福机器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H 由于 由于T5T6＝＝[4T6]＝＝T4 －－1 T3 －－1 T2 －－1 T1 －－1[0T6]，，T5 －－1T5T6＝＝T6＝＝ [5T6]＝＝T5 －－1T4 －－1 T3 －－1 T2 －－1 T1 －－1[0T6]，所以，，所以，T5 －－1T4 －－1 T3 －－1 T2 －－1 T1 －－1[0T6]的表达式为的表达式为 (6) 求坐标系 求坐标系X6Y6Z6相对于相对于X5Y5Z5的转角的转角θ6(13－－60) 式 式(13－－60)中的中的Aij与与Bi4组成的矩阵就是组成的矩阵就是T4 －－1 T3 －－1 T2 －－1 T1 －－1[0T6]的连乘积矩阵。
的连乘积矩阵 令式 令式(13－－60) 与与(13－－34)的第一行第二列的矩阵元素对应的第一行第二列的矩阵元素对应相等，第二行第二列的矩阵元素对应相等，得关于转角相等，第二行第二列的矩阵元素对应相等，得关于转角θ6的三的三角方程为角方程为 由式由式(13－－61) 与与(13－－62)得得θ6为为 至此，各个关节的运动参数 至此，各个关节的运动参数θ1、、θ2、、d3、、θ4、、θ5、、θ6已全部已全部求出 从从以以上上的的求求解解过过程程可可以以看看出出，，这这种种求求解解方方法法就就是是将将一一个个未未知知数数从从方方程程的的右右端端移移到到左左端端，，使使其其与与其其它它未未知知数数分分离离开开来来，，从从而而解解出出这这个个未未知知数数重重复复这这一一过过程程，，解解出出所所有有的的未未知知数数值值得得注注意意的的是是，，若若目目标标物物体体不不在在机机器器人人的的操操作作范范围围之之内内，，则则解解可可能能不不存存在在由由以以上上求求解解公公式式可可以以看看出出，，关关节节的的相相对对位位移移不不一一定定唯唯一一，，可可能能出出现现多多解解在在多多解解的的情情况况下下，，一一定定有有一一个个解解是是相相对对较较优优的的，，或或者者是是路路径径最最短短，，或或者者可可以以避避开开其其它它障障碍碍物物。
当当然然，，还还有有其其它它的的相相对对位位移移求求解解方方法法，，本本书书不不再再介介绍绍，，请阅读相关的文献请阅读相关的文献。