第二章机器人运动学

null《工业机器人技术基础》 第二章 机器人运动学《工业机器人技术基础》 第二章 机器人运动学 机器人运动学要解决的问 题 快递公司问题件快递公司问题件货款处理关于圆的周长面积重点题型关于解方程组的题及答案关于南海问题 机器人运动学要解决的问题机器人运动学通过研究机器人的关节变量和末端执行器的位姿关系，建立机器人本体运动的 数学 数学高考答题卡模板高考数学答题卡模板三年级数学混合运算测试卷数学作业设计案例新人教版八年级上数学教学计划 模型，为机器人的运动控制和机构 设计 领导形象设计圆作业设计ao工艺污水处理厂设计附属工程施工组织设计清扫机器人结构设计 提供依据。 正向运动学问题（用于机构设计） 已知各关节变量，求取机械手末端位姿； 逆向运动学问题（用于运动控制） 已知机械手末端位姿，求取各关节变量；第2章主要内容：第2章主要内容： 2.1 齐次坐标与动系位姿矩阵 2.2 齐次变换及运算 2.3 机器人位姿分析 2.4 机器人正向运动学 2.5 机器人逆向运动学 2.1齐次坐标与动系位姿矩阵 2.1齐次坐标与动系位姿矩阵一、空间任意点的坐标表示 在直角坐标系A 中,空间任一点 P 的位置可用3×1的位置矢量表示。 其左上标A代表选定的参考坐标系 式中: 是点P在坐标系A中的三个位置坐标分量。2.1.1齐次坐标二、齐次坐标二、齐次坐标 Px P= Py Pz 1 将一个n维空间的点用n + 1维坐标表示，则该n + 1维坐标即为n维坐标的齐次坐标。 一般情况下w称为该齐次坐标中的比例因子，当取w = 1时，其表示 方法 快递客服问题件处理详细方法山木方法pdf计算方法pdf华与华方法下载八字理论方法下载 称为齐次坐标的规格化形式，即二、齐次坐标二、齐次坐标 当w不为1时，则相当于将该列阵中各元素同时乘以一个非零的比例因子w. 仍表示同一点P。 P = [a b c w]T 式中：a = wPX；b = wPY；c = wPZ。三、坐标轴方向的描述三、坐标轴方向的描述 如图所示,i、j、k分别是直角坐标系中X、Y、Z坐标轴的单位向量。若用齐次坐标来描述X 、Y 、Z 轴的方向,则 X=[1 0 0 0] T Y=[0 1 0 0] T Z=[0 0 1 0] T 从上可知,我们 规定 关于下班后关闭电源的规定党章中关于入党时间的规定公务员考核规定下载规定办法文件下载宁波关于闷顶的规定 : (4×1)列阵[ a b c 0] T中第四个元素为零,且 ,则表示某轴的方向;三、坐标轴方向的描述三、坐标轴方向的描述规定: (4×1)列阵[ a b c w ] T 中第四个元素不为零,则表示空间某点的位置。 图中矢量 v 的方向用(4×1)列阵可表达为: v=[a b c 0] T a=cosα, b=cosβ, c=cosγ null例2-1 用齐次坐标写出图中矢量 u 、v、w 的方向列阵。解 矢量 u: cosα =0, cosβ =0.707, cosγ=0.707 u=[0 0.707 0.707 0] T 矢量 v: cosα =0.707, cosβ =0, cosγ =0.707 v=[0.707 0 0.707 0] T 矢量 w: cosα =0.5, cosβ =0.5, cosγ =0.707 w=[0.5 0.5 0.707 0] T null 2.1.2 动系的位姿表示 在机器人坐标系中，运动时相对于连杆不动的坐标系称为静坐标系；跟随连杆运动的坐标系称为动坐标系。 动系位置与姿态的描述称为动系的位姿表示。 动系位姿的描述就是对动坐标系原点位置的描述以及对动坐标系各坐标轴方向的描述。 一、连杆的位姿表示 设有一个机器人的连杆，若给定了连杆PQ上某点的位置和该连杆在空间的姿态，则称该连杆在空间是完全确定的。 连杆PQ在固定坐标系OXYZ中的位置可用一齐次坐标表示为：                      null 连杆的姿态可由动系的坐标轴方向来表示。 令n、o、a分别为X、Y、Z坐标轴的单位矢量，各单位方向矢量在静系上的分量为动系各坐标轴的方向余弦，以齐次坐标形式分别表示为：null连杆的位姿可用下述齐次矩阵表示： 连杆的位姿表示就是对固连于连杆上的动系位姿表示。null 例2.2 图2.5表示固连于连杆的坐标系{B}位于OB点，XB = 2，YB = 1， ZB = 0。在XOY平面内，坐标系{B}相对固定坐标系{A}有一个30°的偏转，试写出表示连杆位姿的坐标系{B}的4  4矩阵表达式。图2.5 动坐标系{B}的位姿表示null解 XB的方向列阵：YB 的方向列阵：ZB的方向列阵：坐标系{B}的位置阵列null则动坐标系{B}的4  4矩阵表达式为 null二.手部位置和姿态的表示 机器人手部的位置和姿态也可以用固连于手部的坐标系{B}的位姿来表示,如图所示。 坐标系{B}可以这样来确定： 1）取手部的中心点为原点OB；关节轴为ZB轴，ZB轴的单位方向矢量a称为接近矢量，指向朝外； 2）两手指的连线为YB轴，YB轴的单位方向矢量o称为姿态矢量，指向可任意选定； 3）XB轴与YB轴及ZB轴垂直，XB轴的单位方向矢量n称为法向矢量，且n = o  a，指向符合右手法则。 null 手部的位置矢量为固定参考系原点指向手部坐标系{B}原点的矢量P，手部的方向矢量为n、o、a。于是手部的位姿可用4  4矩阵表示为：null例2-3 图表示手部抓握物体 Q ,物体为边长2个单位的正立方体,写出表达该手部位姿的矩阵式。null例2-3 图表示手部抓握物体 Q ,物体为边长2个单位的正立方体,写出表达该手部位姿的矩阵式。 解： 因为物体 Q 形心与手部坐标系 O ′X ′Y′ Z′的坐标原点 O ′相重合,所以手部位置的(4×1)列阵为 p = [ 1 1 1 1 ] T手部坐标系X′轴的方向可用单位矢量n来表示: α=90°,β=180°,γ=90° n: nx=cosα=0 ny=cosβ=-1 nz=cosγ=0 同理,手部坐标系 Y′轴与 Z ′轴的方向可分别用单位矢量 o 和 a 来表示。手部位姿可用矩阵表达为：null三、目标物齐次矩阵表示 如图2.8所示，楔块Q在图2.8(a)所示位置，其位置和姿态可用8个点描述，矩阵表达式为null 若让楔块绕Z轴旋转–90°，再沿X轴方向平移4，则楔块成为图2.8(b)所示的情况。此时楔块用新的8个点来描述它的位置和姿态，其矩阵表达式为null 2.2齐次变换 连杆的运动是由转动和平移组成的。为了能用同一矩阵表示转动和平移,有必要引入齐次坐标变换矩阵。2.2.1 旋转的齐次变换 一、点在空间直角坐标系中绕坐标轴的旋转变换 如图，空间某一点A，坐标为(XA，YA，ZA)，当它绕Z轴旋转角后至A点，坐标为(XA，YA，ZA)。A点和A点的坐标关系为或用矩阵表示为：null也可简写为 式中：Rot(Z，)表示齐次坐标变换时绕Z轴的转动齐次变换矩阵，又称旋转算子，旋转算子左乘表示相对于固定坐标系进行变换，旋转算子的内容为 式中：c=cos；s=sin。null同理，可写出绕X轴转动的旋转算子和绕Y轴转动的旋转算子： null二、点在空间直角坐标系中绕过原点任意轴的一般旋转变换 图所示为点A绕任意过原点的单位矢量k旋转角的情况。kX、kY、kZ分别为k矢量在固定参考系坐标轴X、Y、Z上的三个分量，且可以证得，绕任意过原点的单位矢量k转角的旋转算子为式中：null反之，若给出某个旋转算子求出其等效转轴矢量k及等效转角为null 式中：当取0°到180°之间的值时，式中的符号取“+”号。null三、算子左、右乘规则 若相对固定坐标系进行变换，则算子左乘；若相对动坐标系进行变换，则算子右乘。例2.4 已知坐标系中点U的位置矢量U=[7 3 2 1]T，将此点绕Z轴旋转90°，再绕Y轴旋转90°，如图2.11所示，求旋转变换后所得的点W。nullnull例2.5 图2.12所示单臂操作手的手腕也具有一个自由度。已知手部起始位姿矩阵为 若手臂绕Z0轴旋转+90°，则手部到达G2；若手臂不动，仅手部绕手腕Z1轴旋转+90°，则手部到达G30写出手部坐标系{G2}及{G3}的矩阵表达式。null解 手臂绕定轴转动是相对固定坐标系作旋转变换，故有 手部绕手腕轴旋转是相对动坐标系作旋转变换，所以null2.2.2 平移的齐次变换 一、点在空间直角坐标系中的平移变换 如图2.13所示，空间某一点A，坐标为(XA，YA，ZA)，当它平移至A点后，坐标为(XA，YA，ZA)。其中 null2.2.2 平移的齐次变换 一、点在空间直角坐标系中的平移变换或写成如下形式：也可以简写为 式中：第四列元素X、Y、Z分别表示沿坐标轴X、Y、Z的移动量。null 二、坐标系与物体的平移变换 点的平移的齐次变换公式同样适用于坐标系、物体等的变换，算子左、右乘规则同样适于平移的齐次变换。 例2.6 动坐标系{A}相对于固定坐标系的X0、Y0、Z0轴作(-1，2，2)平移后到{A}；动坐标系{A}相对于自身坐标系的X、Y、Z轴分别作(-1，2，2)平移后到{A}；物体Q相对于固定坐标系作(2，6，0)平移后到Q。已知： 写出坐标系{A}、{A}以及物体 Q的矩阵表达式。null解 动坐标系{A}的两个齐次坐标变换平移算子均为{A}坐标系是动系{A}沿固定坐标系作平移变换得来的，故算子左乘，{A}的矩阵表达式为 null{A}坐标系是动系{A}沿自身坐标系作平移变换得来的，故算子右乘，{A}的矩阵表达式为 null物体Q的齐次坐标变换平移算子为:故有经过平移坐标变换后，物体Q的实际情况如图所示。null 2.2.3复合变换 平移变换和旋转变换可以组合在一个齐次变换中,称为复合变换。 已知坐标系中点U的位置矢量U=[7 3 2 1]T，将此点绕Z轴旋转90°，再绕Y轴旋转90°，还要作4i-3j+7k 的平移如图所示，求旋转变换后所得的点E。 nullE=Trans(4,-3,7)Rot(y,90°)Rot(z,90°) U ③ ② ① 1 0 0 4 0 0 1 0 7 = 0 1 0 -3 1 0 0 0 3 0 0 1 7 0 1 0 0 2 0 0 0 1 0 0 0 1 1 0 0 1 4 7 6 = 1 0 0 -3 3 = 4 0 1 0 7 2 10 0 0 0 1 1 1 0 0 1 4 1 0 0 -3 0 1 0 7 0 0 0 1其中, 为平移加旋转的一般齐次变换矩阵。null 课堂练习： 如图所示的楔块Q，在图所示位置下描述它的齐次矩阵为 试求楔块经过绕固定坐标系OXYZ的Z轴旋转–90°，再沿X轴方向平移4后[如图(b)所示]的齐次矩阵表达式及其复合变换矩阵H。null解 楔块从图(a)至图(b)的所有变换都是相对于固定坐标系OXYZ进行的，故各坐标变换算子应该依次左乘，即复合变换矩阵：= = nullnull2.3 机器人的位姿分析2.3.1 杆件坐标系的建立 一、坐标系号的分配方法 机器人的各连杆通过关节连接在一起，关节有移动副与转动副两种。 按从机座到末端执行器的顺序，由低到高依次为各关节和各连杆编号，如图所示。机座的编号为杆件0，与机座相连的连杆编号为杆件1，依此类推。机座与连杆1的关节编号为关节1，连杆1与连杆2 的连接关节编号为2， 依此类推。各连杆的 坐标系Z轴方向与关 节轴线重合。nullnull 1.连杆描述 连杆的功能在于保持其两端的关节轴线具有固定的几何关系，连杆的特征也是由这两条轴线规定的。连杆i是由关节轴线i和i+1的公法线长度 和夹角 所规定的。 和 分别称为连杆i的长度和扭角。 的指向规定为从关节i绕公垂线转至关节i+1。 两轴线平行时 ；两轴线相交时 ，这时 的指向不定。 null 相邻两连杆i和i一1由关节i相连，因此关节轴线i有两条公法线与它垂直，每条公法线代表一条连杆，两条公法线之间的距离di称为这两条连杆之间的偏置，两条公法线之间的转角度称为两条连杆之间的关节角 。null2. 连杆坐标系的建立 连杆i坐标系(简称i系)的坐标原点设在关节i的轴线与关节i+1的轴线的公垂线与关节i+l的轴线相交之处，i系的Z轴与关节i+1的轴线重合．X轴与上述公垂线重合，且方向从关节i指向关节i+1，Y轴则按右手确之。null二、连杆坐标系间变换矩阵的确定 如图所示，一旦对全部连杆规定坐标系后，就能按照下列的步骤实现两连杆i-1系到i系的变换： (1) i-1坐标系绕Zi–1轴旋转i角，使Xi–1轴转到与Xi同一平面内。 (2) 沿Zi–1轴平移一距离di，把Xi–1移到与Xi同一直线上。 (3) 沿Xi轴平移一距离ai，把连杆i–1的坐标系移动到使其原点与连杆i坐标系原点重合的地方。 null二、连杆坐标系间变换矩阵的确定 (4) 绕Xi旋转αi角，使Zi–1转到与Zi同一直线上。 连杆i–1的坐标系经过上述变换与连杆i的坐标系重合。如果把表示相邻连杆相对空间关系的矩阵称为A矩阵，那么根据上述变换步骤，从连杆i-1到连杆i的坐标变换矩阵Ai为null2.4 机器人正向运动学 研究机器人运动学首先应建立机器人各杆件的构件坐标系，从而得出齐次坐标变换矩阵Ai。Ai能描述连杆坐标系之间相对平移和旋转的齐次变换。 A1描述第一个连杆对于机身的位姿，A2描述第二个连杆坐标系相对于第一个连杆坐标系的位姿。 如果已知一点在最末一个坐标系(如n坐标系)的坐标，要把它表示成前一个坐标系(如n–1)的坐标，那么齐次坐标变换矩阵为An。依此类推，可知此点到基础坐标系的齐次坐标变换矩阵为：A1A2A3…An–1Annull 若有一个六连杆机器人，机器人末端执行器坐标系(即连杆坐标系6)的坐标相对于连杆i–1坐标系的齐次变换矩阵，用 表示，即 机器人末端执行器相对于机身坐标系的齐次变换矩阵为 常写成T6。null2.4.1 平面关节型机器人的运动学方程例2.8 平面关节型机器人的结构示意图如图所示。求Ai(i = 1,2,3)及T3的表达式。null2.4.1 平面关节型机器人的运动学方程该平面关节型机器人的运动学方程为：null2.4.1 平面关节型机器人的运动学方程该平面关节型机器人的运动学方程为：null2.4.1 平面关节型机器人的运动学方程可以写出平面关节型机器人的运动学方程：null2.4.2 斯坦福机器人运动方程例2.9 斯坦福机器人的结构示意图如图所示。求Ai(i = 1,2,3,4,5,6)及T6的表达式。null2.4.2 斯坦福机器人运动方程例2.9 斯坦福机器人的结构示意图如图所示。求Ai(i = 1,2,3,4,5,6)及T6的表达式。解 1) D-H坐标系的建立 建立各连杆坐标系，如图所示。图中Z0轴沿关节1的轴，Zi轴沿关节i + 1的轴，令所有Xi轴与机座坐标系X0轴平行，Yi轴按右手坐标系确定。null2) 各连杆的D-H参数和关节变量 表给出了各连杆的D-H参数和关节变量。 根据各连杆坐标系的关系可以写出齐次变换矩阵。null2.4.2 斯坦福机器人运动方程3) 求两杆之间的位姿矩阵Ainull2.4.2 斯坦福机器人运动方程3) 求两杆之间的位姿矩阵Ainull2.4.2 斯坦福机器人运动方程3) 求两杆之间的位姿矩阵Ainull2.4.2 斯坦福机器人运动方程3) 求两杆之间的位姿矩阵Ainull2.4.2 斯坦福机器人运动方程3) 求两杆之间的位姿矩阵Ainull2.4.2 斯坦福机器人运动方程3) 求两杆之间的位姿矩阵Ainull2.4.2 斯坦福机器人运动方程3) 求两杆之间的位姿矩阵Ainull3) 求两杆之间的位姿矩阵Ai 求得Ai如下：null4) 求机器人的运动方程T6 = A1 A2 A3 A4 A5 A6 =  式中：nX = c1[c 2(c 4c 5c 6 – s 4s 6) – s 2s 6c 6] – s 1(s 4c 5c 6 + c 4s 6)； nY = s 1[c 2(c 4c 5c 6 – s 4s 6) – s 2s 5c 6] – c 1(s 4c 5c 6 + c 4s 4)； nZ = – s 2(c 4c 5c 6 – s 4s 6) – c 2s 5c 6； oX = c 1[ – c 2(c 4c 5s 6 + s 4c 6) + s 2s 5s 6] – s 1( – s 4c 5s 6 + c 4c 5)； oY = s 1[ – c 2(c 4c 5c 6 – s 4c 6) + s 2s 5s 6] + c 1( – s 4c 5s 6 + c 4c 5)； oZ = s 2(c 4c 5s 6 + s 4c 6) + c 2s 5c 6； aX = c 1(c 2c 4s 5 + s 2c 5) – s 1s 4s 5； aY = s 1(c 2c 4s 5 + s 2c 5) + c 1s 4s 5； aZ = – s 2c 4s 5 + c 2c 5； pX = c 1s 2d3 – s 1d2； pY = s 1s 2d3 + c 1d2； pZ = c 2d3。 null2.4.3 PUMA 560机器人运动学方程例2.10 求如图所示PUMA 560机器人的运动学方程。null解 1) D-H坐标系的建立 按D-H方法建立各连杆坐标系，如图所示。 2) 确定各连杆的D-H参数和关节变量 表中给出了各连杆的D-H参数和关节变量。null3) 求两杆之间的位姿矩阵Ai 根据表所示的D-H参数和齐次变换矩阵公式可求得Ai为 null4) 求机器人的运动方程 T6 = A1 A2 A3 A4 A5 A6 = 式中： nX = c 1[c23(c 4c 5c 6 – s 4s 6) – s23s 6c 6] + s1(s 4c 5c 6 + c 4s 6)； nY = s 1[c23(c 4c 5c 6 – s 4s 6) – s23s 5c 6] – c 1(s 4c 5c 6 + c 4s 4)； nZ = – s23(c 4c 5c 6 – s 4s 6) – c 2s 5c 6； oX = c 1[c23( – c 4c 5s 6 – s 4c 6) + s23s 5s 6] + s 1( – s 4c 5s 6 + c 4c 6)； oY = s 1[c23( – c 4c 5s 6 – s 4c 6) + s23s 5s 6] – c 1( – s 4c 5c 6 + c 4c 6)； oZ = – s23( – c 4c 5s 6 – s4c6) + c23s 5s 6； aX = – c 1(c23c 4s 5 + s23c 5) – c 1s 4s 5； aY = – s 1(c23c 4s 5 + s23c 5) + c 1s 4s 5； aZ = s23c 4s 5 – c23c 5；PX = c 1[a2c 2 + a3c23 – s23d4] – s 1d2； PY = s 1[a2c 2 + a3c23 – s23d4] + c 1d2； PZ = – a3s23 – a2s 2 – c23d4； 式中：c23 = cos(2 + 3) = c 2c3 – s 2s 3；s23 = sin(2 + 3) = c 2s 3 – s 2c 3。null2.5 机器人逆向运动学对于具有6个自由度的操作臂，其运动学方程可以写成 给定末端连杆的位姿计算相应关节变量的过程叫做运动学逆解。null例2.11 已知斯坦福机器人末端执行器的位姿，求其逆向运动学解。即斯坦福机器人的运动学方程为： 现在给出上述矩阵及各杆的参数 ，求关节变量 ，其中 。 null1) 求 1用A 左乘上式，得 A T6= A2A3A4A5A6 = 1T6 式中： null式中： f11(i) = c 1iX + s 1iY； f12(i) = – iZ； f13(i) = – s 1iX + c 1iY； 1T6 = A2A3A4A5A6 null第3行、第4列的元素为常数，把对应的元素等同起来，可得 采用三角代换 式中：进行三角代换后可解得 式中：正、负号对应的两个解对应于1的两个可能解。null2) 求2 根据前述原则，用A 左乘方程式，得 查找右边的元素，这些元素是各关节的函数。计算矩阵后可知，第1行、第4列和第2行、第4列是s 2d3的函数。因此可得 由于d3大于0(棱形导轨的伸展大于0)，所以2有惟一解 AA T6 = A3A4A5A6nullnullnullnull6) 求解6 采用下列方程： 根据等式两边第3行、第1列及第1行、第1列的元素分别对应相等，得 解得null 至此，六个变量全部求出。从以上解的过程看出、这种方法就是将一个未知数由矩阵方程的右边移向左边，使其与其它未知数分开，解出这个未知数，再把下一个未知数移到左边，重复进行，直至解出所有未知数，所以这种方法也叫分离变量法。 这是代数法的一种，它的特点是首先利用运动方程的不同形式，找出矩阵中简单表达某个未知数的元素，力求得到未知数较少的方程，然后求解。null机器人运动学逆解问题的求解存在如下三个问题： (1)解可能不存在。机器人具有一定的工作域，假如给定手部位置在工作域之外，则解不存在。null机器人运动学逆解问题的求解存在如下三个问题： (2)解的多重性。机器人的逆运动学问题可能出现多解。 图表示一个二自由度平面关节机械手出现两个逆解的情况。对于给定的在机器人工作域内的手部位置A可以得到两个逆解。 但是手部是不能以任意方向到达目标点A的，可以增加一个手腕关节自由度。null机器人运动学逆解问题的求解存在如下三个问题： (2)解的多重性。机器人的逆运动学问题可能出现多解。 在多解情况下，—定有一个最接近解，即最接近起始点解。 图(a)表示3R机械手的手部从起始点A运动到目标点B，完成实线所表示的解为最接近解，是一个“最短行程”的优化解。 但是，如图(b)所示，在有障碍存在的情况下．上述的最接近解会引起碰撞，只能采用另一解如图(b)中实线所示。null(1) 根据关节运动空间来选择合适的解。 (2) 选择一个最接近的解。 (3) 根据避障要求选择合适的解。 (4) 逐级剔除多余解。通常采用剔除多余解的方法：null机器人运动学逆解问题的求解存在如下三个问题： (3)求解方法的多样性。机器人逆运动学求解有多种方法，一般分为两类：封闭解和数值解。 不同学者对同一机器人的运动学逆解也提出不同的解法，应该从计算方法的计算效率、计算精度等要求出发，选择较好的解法。