1、刚体位姿描述和齐次变换预备知识旋转矩阵坐标变换齐次坐标,欧拉角与 RPY 角齐次变换和齐次变换矩阵的运算例子目录第二章 位姿描述和齐次变换要求：熟练掌握描述刚体位姿描述的齐次变换方法cossin0sincos0001),(xRazayaxozoyoxnznynxRBBBBBBBBBAB如图，绕X轴旋转900010100001BABABAABzyxRRAB900绕X轴旋转nxBRotation Matrices in 3D1000cos sin 0sin cos),(zRcos0sin010sin0cos),(yRcos sin 0sin cos 0001),(xR绕Z轴旋转绕Y轴旋转绕X轴旋转
2、注意：,BAx,BAyBAz1BABABABABABAzzyyxx0BABABABABABAxzzyyx为单位矢量33旋转矩阵有9个元素，6个约束条件，3个独立变量.1 ;1RRRR ABTABABAB是正交矩阵，且满足 称为旋转矩阵，上标A代表参考坐标系A，下标B代表被描述的坐标系B。RAB旋转矩阵的逆等于其转置矩阵BABABAABzyxR 旋转矩阵的性质旋转矩阵的性质 为了完全描述刚体B在空间的位姿(位置和姿态)、通常将物体B与某一坐标系B相固接。B的坐标原点一般选在物体B的特征点上，如质心、或对称中心等。相对参考系A，由位置矢量 和旋转矩阵 分别描述坐标系B的原点位置和坐标轴的方位。因此
3、，刚体B的位姿可由坐标系B来描述，即三、位姿的描述（位置姿态）三、位姿的描述（位置姿态）RAB0BAp 0BAABpRB A BoBp四、手爪坐标系四、手爪坐标系 A BBz aBx nBy opn=o aaonR,paonT,TAB求AB1.坐标平移0BABAppp2.2 坐标变换坐标变换在机器人学的许多问题中，涉及到以不同坐标系表示同一量。下面讨论从一个坐标系的描述到另个坐标系的描述之间的变换关系。pB AByAxAzBxBzAyAoBo BpAoBAppB AByAxAzBxBzAyo BpRpRpRpBTBABBABABA12.坐标旋转同一点p在两个坐标系A和B中的描述具有以下变换关系
4、：.1 ;1RRRR ABTABABAB刚体位姿描述刚体位姿描述(Location Representing)机器人的操作，就其本义来说，意味着由某种机构在空间移动零件和工具。这自然由必要表示零件、工具以及机构本身的位置和方位。为了规定和运算表示位置和方位的数学量，我们必需规定坐标系并提出它们的表达式的习惯形式。我们采取这样的思想，即某处存在一通用的坐标系统，我们讨论的每一个物体均可参考此参考坐标系。刚体的位置、姿势可由其上的任一点(称作基准点，通常可选作物体的质心)和过该点的坐标系相对于参考坐标系的相对关系来确定。我们在物体上附一坐标系，然后再给出这一坐标系相对于参考系的描述。5.刚体位置、
5、姿态的描述刚体位置、姿态的描述或BABABAABzyxR 333231232221131211rrrrrrrrrRABBA表示刚体 B 相对于 A的方位B与物体固结，A 为参考系。用坐标系B的三个单位主矢量相对于坐标系A的方向余弦组成的33矩阵 A B 为了完全描述刚体B在空间的位姿(位置和姿态)、通常将物体B与某一坐标系B相固接。B的坐标原点一般选在物体B的特征点上，如质心、或对称中心等。相对参考系A，由位置矢量 和旋转矩阵 分别描述坐标系B的原点位置和坐标轴的方位。因此，刚体B的位姿可由坐标系B来描述，即RAB0BAp 0BAABpRB A BoBp AAxAzAyAopAoBApBypB
6、 BBzBxBo CoBABABCACAppRppp03.一般变换 齐次变换矩阵也代表坐标平移与坐标旋转的复合齐次变换矩阵也代表坐标平移与坐标旋转的复合将其分解成两个矩阵相乘的形式之后就可以看出这一点。将其分解成两个矩阵相乘的形式之后就可以看出这一点。其中，其中，I33是是33阶单位矩阵，等式右端第一个矩阵称为阶单位矩阵，等式右端第一个矩阵称为平移变换矩阵，常用平移变换矩阵，常用Trans(ApBo)来表示；第二个矩阵标为来表示；第二个矩阵标为旋转变换矩阵旋转变换矩阵,常用常用Rot(k,)来表示来表示.齐次变换矩阵齐次变换矩阵1310pRToBAABAB100001000103331RpIp
7、RTABBABAABABo0,0kRotpTransTBAAB0BABAppp1 移动变换移动变换pB AByAxAzBxBzAyAoBo BpAoBAp 11000100010001zyxcbappBABAT2.转动变换转动变换100001000000),(cossinsincoszR100000001000),(cossin sin cosyR100000000001),(cossinsincosxR绕Z轴旋转绕Y轴旋转绕X轴旋转3.对坐标系的解释对坐标系的解释RFM作为坐标系解释变换RFMRFM齐次坐标和齐次变换齐次坐标和齐次变换111 31p0pRpppRpBBAABABABABAoo
8、pTpBABAOrientation matrixVector of coordinate origin相对运动坐标系，变换式“从左向右”写：Rot(y,90)Rot(z,90)相对固定坐标系，变换式“从右向左”写：Rot(z,90)Rot(x,90)4.相对变换相对变换变换矩阵的左乘和右乘的运动解释是不同的：变换顺序“从右向左”，指明运动是相对固定坐标系而言的；变换顺序“从左向右”，指明运动是相对运动坐标系而言的。从左向右从右向左Example:Displacement in an Absolute FrameDisplace(7,3,2)through a sequence of:1.Ro
9、t(z,90)2.Rot(y,90)3.Trans(4,-3,7)Trans(4,-3,7)Rot(y,90)Rot(z,90)齐次变换矩阵T具有以下不同的物理解释：1.坐标系的描述 描述B相对于参考系A的位姿2.坐标映射 表示同一点P在两个坐标系A和B中描述之间的映射关系 3.运动算子 T表示在同一坐标系中，点P运动前后的算子关系。Ap2=T Ap1pTpBABATTTTABBCBCAB变换矩阵相乘不满足交换率变换矩阵相乘不满足交换率1 31AB0pRToBAAB1ABBA TT1 31BA0pRToABBA6.逆变换逆变换11 31310pRR0pRTooBATABTABABBABA1 3
10、1AB0pRToBAAB1 31BA0pRToABBA1ABBA TT1 31BA0pRToABBA0A BABpR，?,oABBApR6.逆变换逆变换TABABBARRR1 ooooooBATABBABAABABBABABABpRpRp0ppRp 11 31310pRR0pRTooBATABTABABBABA坐标系B原点 在B中的描述：oBAp0oBBp框A下的点 映射到B中描述：oBAp AAxAzAyAooBApBy BBzBxBooABpooBABABABpp T oABp6.逆变换逆变换1000zzzzyyyyxxxxpaonpaonpaonT10001apaaaopooonpnnn
11、Tzyxzyxzyx已知 表示B相对于A绕其z轴转30度，再沿x轴移动4,沿y轴移动3。求 .(例2.5,P.21)034 ,100030cos30sin030sin30cosoBAABpR10000100598.00866.05.0964.405.0866.01 310pRRToBATABTABBA BATTAB7.变换矩阵相乘变换矩阵相乘111,and 3131310ppRRR0pR0pRTTTTTooooBACBABBCABCBBCBAABBCABACBCAB ，分别表示C相对于A和B的描述 表示坐标系C从 映射为 的变换TTTBCABACTACTBCTABTBCTACTABTBCTAC
12、8.手爪坐标系手爪坐标系 A BBz aBx nBy opaonaonR,paonT,9.变换方程变换方程 and ,TTTTSGBSWTBW?TGT TTTTTGTSGBSWTBW TGT 11TTTTTWTBWBSSGGT可以写出测头中心位置的测量运动方程:利用内外传感器数据，采用参数辨识方法，如最小二乘方法可求得:测量方程GGSBTTSBTGTpT T T(p)可测量待求量可控量BTT欧拉角与 RPY 角Yaw(Roll)PitchShip,),(),(,AAAxyzABxyzRRRR回转俯仰偏转一、RPY角：依次绕绕固定轴x-y-z旋转zstation about rotation a
13、ystation about rotation astation xabout ofrotation accscssccssccssscssscsccsssccccssccssccsscxyzAB000010010010000,R,xyzABR,二、欧拉角：依次绕绕动坐标系z-y-z轴旋转axis x newabout rotation aaxisy newabout rotation aaxis zabout ofrotation a,xyzABR二、欧拉角：依次绕绕动坐标系z-y-z轴旋转ZAXAYAZBXBYBKABoABA1B1),(kRRAB三、绕任意轴三、绕任意轴/角的转动角的转动
14、前面讨论了旋转矩阵的三种特殊情况，即绕x,y和z轴的旋转矩阵，现在讨论绕过原点的任意轴k旋转角的变换矩阵。表示坐标系B相对参考系A方位TBBAABBAABBABAAABRzRRRzRRRRRkRR),(),(),(1RkAB,ABA1B1ZAXAYAZBXBYBKABozzzyyyxxxBBAAaonaonaonRR运用旋转矩阵的正交性质：化简整理后得到：其中，k轴即a轴。12.等效转轴和等效转角,kRABMatlab编程作业编程作业2.8(p30)2.9(p31)，参考P35图3-6 TAB求机器人技术基础第三章 操作臂运动学课程的基本要求:熟练掌握机器人运动学正解的D-H矩阵方法，掌握运动
15、学反解的基本原理。理解机器人运动的二个描述空间。n背景知识n机器人运动学n机器人逆运动学n关节空间与操作空间3.1 连杆参数和连杆坐标系Denavit-Hartenberg Parameters第三章 操作臂运动学连杆的描述n自由度机械臂-n个单自由度关节与n-1个零长度连杆组成的模型。只考虑具有单自由度关节的操作器。连杆编号由固定基座开始：固定基座连杆0第一个运动体连杆1通常为了能在三维空间定位末端执行器，最少要求有6个关节。连杆坐标系 关节 1 是垂直于肩,关节 2 经过肩水平线,关节 3 是在肘部。关节 4,5&6 是在手腕上，初始位置关节4 和关节6 共同沿着前臂,关节5 垂直于关节4
16、 和关节6。连杆坐标系Z(i-1)X(i-1)Y(i-1)(i-1)a(i-1)Z i Y i X i a i d i i 111111 joint to1-joint from:direction and 1-axesbetween normalcommon arbitrary:direction 1,-axisjoint iiiiiixzyyiiiixizSpecification of Base&Final link frames 0 n首、末连杆参数/变量:,a,d,基本思想：每个关节分配一个坐标系。用D-H参数，描述框i相对于前一个框i-1的位姿需要4个参数D-H参数Z(i-1)X(
17、i-1)Y(i-1)(i-1)a(i-1)Z i Y i X i a i d i i 1）ai-1 定义:ai-1 两个关节轴线公垂线的长度.关节轴是围绕它发生旋转的有向空间直线，在图中是 Zi-1和 Zi 轴。Zi-1Xi-1Yi-1 i-1ai-1Z i Y i X i a i d i i 可视化方法：想象一个圆柱面围绕轴Z(i-1)扩展 当圆柱面刚刚触及轴 i 时，圆柱的半径等于a(i-1)。图示方法：若已经定义了坐标系,公垂线通常是X(i-1)轴.因此 a(i-1)恰是沿着X(i-1)从框i-1 到框i 的位移如果连杆是移动关节,那么 a(i-1)是变量，而不是参数Z(i-1)X(i-
18、1)Y(i-1)(i-1)a(i-1)Z i Y i X i a i d i i 连杆参数连杆参数a(i-1)的的识别方法：识别方法：2)(i-1)定义:使关节轴平行时，绕公垂线旋转的角度.按右手规则确定正向旋转。绕X(i-1)轴旋转使 Z(i-1)指向Zi 轴的方向Z(i-1)X(i-1)Y(i-1)(i-1)a(i-1)Z i Y i X i a i d i i 3)di定义:为了使公垂线a(i-1)和公垂线ai与Zi的交点对起，沿Zi 轴所需的位移。即，沿Zi 对准X(i-1)和 Xi 轴.Z(i-1)X(i-1)Y(i-1)(i-1)a(i-1)Z i Y i X i a i d i
19、i 4)i 为了对准X(i-1)轴和Xi 轴，绕Zi 轴所需转动的角度Z(i-1)X(i-1)Y(i-1)(i-1)a(i-1)Z i Y i X i a i d i i 连杆的描述参数为了运动学建模的目的，一个连杆由两个数字来确定，这两个数字规定了空间这两个轴线的相对位置。1ia1in连杆连接参数的描述中间连杆 两条连杆之间的偏置 两条连杆之间的关节角idi0,06060aa首、末连杆d1和d6以及1和6的确定方法如下。若关节1是转动关节，则1是可变的，称为关节变量，规定1 0为连杆1的零位。习惯约定d10若关节1是移动关节，则d1是可变的，称为关节变量，规定d1=0为连杆1的零位。习惯约定
20、10。上面的约定对于关节6同样适用。连杆参数和关节变量每个连杆由四个参数来描述，描述连杆i-1本身的特征，描述连杆i-1与连杆i之间的联系。对于旋转关节i仅 是关节变量，其他三个参数固定不变；对于移动关节i，仅 是关节变量，其他三个参数因定不变。这种描述机构运动的方法首先是Denavit和Hartenberg提出来的，称为D-H方法。1ia1iidiidi1ia1iiid一个6关节的机器人，用18个参数可以完全表示它的运动学中固定部分，而用6个关节变量描述运动学变动部分。移动关节转动关节1ia1iidi连杆连杆i-1几何特征几何特征i-1从zi-1到zi沿xi-1旋转的角度ai-1 从zi-1
21、到zi沿xi-1测量的距离di从xi-1到xi沿zi测量的距离i从xi-1到xi沿zi旋转的角度3.1连杆变换和运动学方程连杆变换.axis along on translati)(;axisabout rotation )(;axis along on translati)(;axisabout rotation )(1111-iiiiiiiizddzcxabxaT1ii连杆变换可以看成是坐标系i经以下四个子变换得到的：1ia1iidi用4个参数对准两个关节的轴线Z(i-1)X(i-1)Y(i-1)(i-1)a(i-1)Z i Y i X i a i d i i),(),(),(),(111
22、iiiiiidzTranszRotaxTransxRotT因为这些子变换都是相对于动坐标系描述的，按照“从左向右”的原则得到连杆变换矩阵),(),(),(),(),(),(11111iiiiiiiiiidzScrewaxScrewdzTranszRotaxTransxRotTTii1（The Denavit-Hartenberg Matrix）连杆变换矩阵10000111111111iiiiiiiiiiiiiiiiicdcscsssdscccsascD-H参数矩阵参数矩阵1000coscossincossinsinsinsincoscoscossin0sincosi1)(i1)(i1)(ii1
23、)(iii1)(i1)(i1)(ii1)(ii1)(iiidda与齐次变换矩阵一样，D-H参数矩阵是从一个坐标系到下一个坐标系的变换。用一系列D-H参数矩阵相乘，最终的结果是从某个坐标系到初始坐标系的变换。Z(i-1)X(i-1)Y(i-1)(i-1)a(i-1)Z i Y i X i a i d i i 连杆变换依赖于四个参数，其中只有一个是变化的。以下用qi表示第i个关节变量手臂变换运动学方程手臂变换运动学方程TTTTnnn112010)()()(),(1212101210nnnnnqTqTqTqqqT10000pRTnnn)()()(1212101nnnqTqTqT手臂变换矩阵Z0X0Y
24、0Z1X2Y1Z2X1Y2d2a0a1Denavit-Hartenberg Link Parameter Table表的用途:1)描述机器人的变量和参数2)通过变量的数值描述机器人的状态i-1从zi-1到zi沿xi-1旋转的角度ai-1从zi-1到zi沿xi-1测量的距离di从xi-1到xi沿zi测量的距离i从xi-1到xi沿zi旋转的角度Z0X0Y0Z1X2Y1Z2X1Y2d2a0a1T)T)(T12011VVVTV222000ZYXZYX100000000cossina0sincosT11011011100000cossind100a0sincosT22212212This is a tr
25、anslation by a0 followed by a rotation around the Z1 axisThis is a translation by a1 and then d2 followed by a rotation around the X1 and Z2 axisTTT1201Z0X0Y0Z1X2Y1Z2X1Y2d2a0a1i100029003001ia1iidi11l22l3100001000000111101csscTy3x3100000010002212212cslscT100001000003323323cslscTTTTT23120103The Situat
26、ion:You have a robotic arm that starts out aligned with the xo-axis.You tell the first link to move by 1 and the second link to move by 2.The Quest:What is the position of the end of the robotic arm?12两关节机器人X2X3Y2Y3123123 Example Problem:You are have a three link arm that starts out aligned in the x
27、-axis.Each link has lengths l1,l2,l3,respectively.You tell the first one to move by 1,and so on as the diagram suggests.Find the Homogeneous matrix to get the position of the yellow dot in the X0Y0 frame.X1Y1X0Y0The position of the yellow dot relative to the X3Y3 frame is(l1,0).Multiplying H by that
28、 position vector will give you the coordinates of the yellow point relative the the X0Y0 frame.X2X3Y2Y3123123X1Y1X0Y0H=Rz(1)*Tx1(l1)*Rz(2)*Tx2(l2)*Rz(3)i.e.Rotating by 1 will put you in the X1Y1 frame.Translate in the along the X1 axis by l1.Rotating by 2 will put you in the X2Y2 frame.and so on unt
29、il you are in the X3Y3 frame.Slight variation on the last solution:Make the yellow dot the origin of a new coordinate X4Y4 frame X2X3Y2Y3123123X1Y1X0Y0X4Y4H=Rz(1)*Tx1(l1)*Rz(2)*Tx2(l2)*Rz(3)*Tx3(l3)This takes you from the X0Y0 frame to the X4Y4 frame.The position of the yellow dot relative to the X4
30、Y4 frame is(0,0).We are interested in two kinematics topicsForward Kinematics(angles to position)What you are given:The length of each link The angle of each jointWhat you can find:The position of any point (i.e.its (x,y,z)coordinates)Inverse Kinematics(position to angles)What you are given:The leng
31、th of each linkThe position of some point on the robotWhat you can find:The angles of each joint needed to obtain that position3.4 PUMA560机器人运动学PUMA560机器人关节空间运动PUMA560连杆坐标系则工具相对于工作站的位姿为I n v e r s e K i n e m a t i c sFrom Position to Angles A Simple Example 1XYSRevolute and Prismatic Joints Combine
32、d(x,y)Finding 1:)xyarctan(More Specifically:)xy(2arctan arctan2()specifies that its in the first quadrantFinding S:)y(xS22 2 1(x,y)l2l1Inverse Kinematics of a Two Link ManipulatorGiven:l1,l2,x,yFind:1,2Redundancy:A unique solution to this problem does not exist.Notice,that using the“givens”two solut
33、ions are possible.Sometimes no solution is possible.(x,y)l2l1l2l1The Geometric Solutionl1l2 2 1(x,y)Using the Law of Cosines:2122212221222122212221222222arccos2)cos()cos()180cos()180cos(2)(cos2l lllyxl lllyxl lllyxCabbac22222Using the Law of Cosines:xy2arctanyx)sin(yx)sin(180sinsinsin1122222221lcCbB
34、xy2arctanyx)sin(arcsin22221lRedundant since 2 could be in the first or fourth quadrant.Redundancy caused since 2 has two possible values 2122212222212221211211212221211212212221212112122122212122222yxarccosc2)(sins)(cc2)(sins2)(sins)(cc2)(ccyx)2(1)l llll llll llll llll lllThe Algebraic Solutionl1l2
35、2 1(x,y)212121121211122111(3)sinsy(2)ccx(1)cos(ccos cllllOnly Unknown)(sin(cos)(sin(cos)sin()(sin(sin)(cos(cos)cos(:abbababababaNote记：有)(sin(cos)(sin(cos)sin()(sin(sin)(cos(cos)cos(:abbababababaNote)c(s)s(c cscss sinsy)()c(c ccc ccx2211221122212112121122122112122121121211llllllllslsllsslllllWe know
36、what 2 is from the previous slide.We need to solve for 1.Now we have two equations and two unknowns(sin 1 and cos 1)2222221122122211222212211222212212212211yxx)c(ys)c2(sx)c(1 )c(s)s()c()(xy )c()(xcslllllllsllllllllslsllslsSubstituting for c1 and simplifying many timesNotice this is the law of cosine
37、s and can be replaced by x2+y222222211yxx)c(yarcsinslll例如，PUMA 560存在8种运动反解3.6 腕部三轴相交时的封闭解腕部三轴相交时的封闭解 对于6个自由度的机器人而言运动学反解非常复杂，一般没有封闭解。6个自由度的机器人具有封闭反解的两个充分条件(Pieper准则)(1)三个相邻关节轴交于一点；(PUMA、Stanford),或(2)三个相邻关节轴相互平行；(ASEA，MINIMOVER)对于如PUMA560机器人，满足条件(1)，运动学方程可分解为：(1)腕部位置的反解(2)手腕方位的反解TTT3603063.7运动学反解的有关问
38、题运动学反解的有关问题运动学方程的一般形式：n6,6个未知数，12个方程，其中6个为独立方程，存在以下问题：解是否存在？是否唯一？是否可以写成封闭解形式？如何求解？理论上，可达工作空间为一个圆环，其内外半径分别为l1l2和l1l2；灵活工作空间：若l1l2，原点；若l1l2，空集。实际上，还需要考虑关节角的限制，以及结构参数等。例如，平面2R机械手工作空间(Workspace)：不同关节转角所达到的末端执行器的所有形位的集合。是反解存在的区域（操作空间中）。灵活(工作)空间(Dextrous Workspace)：机器人手爪能以任意方位到达的目标集合。可达(工作)空间(Reachable Wo
39、rkspace)：机器人手爪至少能以一个方位到达的目标集合。工作空间讨论（1）关节角取值范围对工作空间的影响；（2）操作臂的自由度对工作空间的影响；（3）末端执行器或工具坐标系对工作空间的影响；反解的唯一性和最优解机器人操作臂运动学反解的数目决定于关节数目、连杆参数和关节变量的活动范围。一般而言，非零连杆参数愈多，运动学反解的数目愈多，例如PUMA 560。最优解：如何从多重解中选择一个最优解？最优准则？寻求方法？在避免碰撞的前提下，通常按“最短行程”准则。使每个关节的移动量为最小。对于典型工业机器人应遵循“多移动小关节、少移动大关节”的原则。例如，PUMA 560存在8种运动反解几何解解析解
40、(analytical solution,closure solution)封闭解法计算速度快，效率高数值求解(numerical solution)在多重解情况下，难以算出所有的解关节空间n个自由度的操作臂的末端位姿由n个关节变量所决定，这n个关节变量统称为n维关节矢量，记为q，所有的关节矢量q构成的空间称为关节空间。操作空间：末端抓手的位置和方位在直角坐标空间中的描述；操作空间 末端手爪的位姿x是在直角坐标空间中描述的，即用操作空间来表示。其中位置用直角坐标表示，而方位用齐次坐标或者欧拉角、RPY角方法表示。运动学方程 可以看成是由关节空间向操作空间的映射；而运动学反解是由其映象求其关节空
41、间中的原象。关节空间操作空间运动学正解运动学正解运动学反解运动学反解二种描述空间)(qxx 单个地看，不同的关节非常简单。它们的运动容易理解和可视化。在左边的例子中，一个棱柱关节和旋转关节用来移动简单的机械手末端操纵装置。在同一时刻，只有一个关节运动，以便你能容易地看见是由用棱柱型关节(黄色元件沿着红色元件的线性运动)和旋转关节(红色元件相对基座回转运动)提供的独立的运动。当它们共同地工作的时候,这二个简单的关节能产生更复杂的运动,如例子所示在操作空间的运动。关节空间运动操作空间运动作业：作业：3.9各驱动器的位置统称为驱动矢量S驱动空间：驱动矢量S所构成的空间x0z0z1x1z2x2y2y0
42、y1z3x3z4x4z5x5z6x6机器人技术基础第四章 机器人雅可比矩阵 (Manipulator Jacobian)课程的基本要求:掌握运动和力雅可比矩阵的物理含义及基本的求解方法4.1 雅可比矩阵的定义雅可比矩阵的定义回顾：基本概念刚体位姿描述和齐次变换齐次坐标,欧拉角与 RPY 角齐次变换和齐次变换矩阵的运算操作臂运动学连杆参数、连杆坐标系连杆变换和运动学方程机器人关节空间与操作空间关节角位置和操作臂末端的直角坐标位置关节空间操作空间运动学正解运动学正解运动学反解运动学反解)(qxx)(qxx q关节角速度和操作臂末端的直角坐标速度关节空间操作空间运动学正解运动学正解运动学反解运动学反
43、解4.1 雅可比矩阵的定义(Jacobian matrix)操作空间速度与关节空间速度之间的线性变换。jiijqqxqJqqJx)(操作臂的雅可比矩阵 ，建立了从关节速度向操作速度的映射关系。进行机器人操作臂的速度分析。)(qJ式中，称为末端在操作空间的广义速度，简称为操作速度，为关节速度；是6n的偏导数矩阵，称为操作臂的雅可比矩阵。它的第i行第j列元素为 ，i=1,2,6;j=1,2,n。x q)(qJ操作臂的运动学方程，描述机器人操作臂的位移关系，建立了操作空间与关节空间的映射关系。)(qxx pTpBABA刚体的齐次变换矩阵，描述刚体之间的空间位姿关系。假设矢量yRm为uRn的函数y=y
44、(u),(),(),()(y)()(21212211m21nmnnuuuyuuuyuuuyyyuuuy相对于u的偏导数定义为 uuuuuuuuuuy)J(yR)J()(y)()(212221212111m21nmnmmmnnuyuyuyuyuyuyuyuyuyyynuyuyuyy12111u对于m=1,（标量对矢量的导数）根据上述一般数学定义，对于6关节机器人：设有6个各含6个独立变量的函数，简写为x=f(q)。求微分，注意，如果函数 f1(q)到 f6(q)是非线性的，则 是q的函数，写成 ，式子两边同除以时间的微分，上式中，66的偏导数矩阵J(q)叫做雅可比矩阵。其中qqfxqf qqJx
45、)(qqJx)(111262212666126(,)(,)()(,)xf q qqxfq qqxf qxfq qqjiijqqxqJ机器人关节数*雅可比矩阵的行数取决于机器人的类型雅可比矩阵在机器人中的应用可以把雅可比矩阵看作是关节的速度 变换到操作速度V的变换矩阵在任何特定时刻，q具有某一特定值，J(q)就是一个线性变换。在每一新的时刻，q已改变，线性变换也因之改变，所以雅可比矩阵是一个时变的线性变换矩阵。在机器人学领域内，通常谈到的雅可比矩阵是把关节角速度和操作臂末端的直角坐标速度联系在一起的。必须注意到，对于任何给定的操作臂的结构和外形，关节速度是和操作臂末端的直角坐标速度成线性关系，但
46、这只是一个瞬间关系。q(x,y)21xyl1l2例4.11221112211slslyclclx将平面2R机械手的运动学方程两端分别对时间t求导,则得其雅可比矩阵为平面2R机械手的运动学方程为对于关节空间的某些形位q，操作臂的雅可比矩阵的秩减少、这些形位称为操作臂的奇异形位：操作臂的雅可比矩阵的秩减少的形位（数学上）操作臂在操作空间的自由度将减少（物理上）(singular configuration)(x,y)21xyl1l2例4.1可利用雅可比矩阵的行列式判别奇异形位当290或2 0时,机械手的雅可比行列式为0矩阵的秩为1，因而处于奇异状态。从几何上看机械手完全伸直(2 0)或完全缩回(2
47、 180)时，机械手末端丧失了径向自由度仅能沿切向运动，在奇异形位时，机械手在操作空间的自由度将减少。例4.2 如图所示为了实现平面2R机械手末端沿x0轴以l m/s的速度运动，求相应的关节速度 解：由 可以看出，只要机械手的雅可比J(q)是满秩的方阵,相应的关节速度即可解出对于平面2R机械手，运动学方程为xqJq)(1T21 q平面2R机械手的速度反解例4.2 如图所示为了实现平面2R机械手末端沿x0轴以l m/s的速度运动，求相应的关节速度 解：雅可比J(q)为于是得到与末端速度 相应的关节速度反解为1,0Tx T21 q逆雅可比可为122111221221112222111)(slsls
48、lclclclsl lqJ2112221221121;slcslcslc讨论：机械手接近奇异形位时，关节速度将趋于无穷大。当20；2180时，机械手在水平位置，2112221221121;slcslcslcxqJq)(1例：物理仿真中的雅可比矩阵约束函数C(x),单位圆上的质点位置约束为一般情况下，采用位姿矢量q聚合表达n个粒子的位置。在3D空间，矢量长度为3n。考虑位置约束C是一个关于位姿矢量q的未知函数，则速度约束 矩阵 被称作C的雅可比矩阵，记作J。为了进行物理仿真，求微分 ，根据力学关系，建立微分约束方程，基于物理仿真。1)(xxxCqqCCqC/qqC JJ例子2：立体视觉雅可比矩阵
49、 两只CCD摄像机任意的安装在机器人手腕上，形成手眼机器人立体视觉系统。Xc,Yc,Zc为摄像机坐标系，x,y为图像坐标系，CO为摄像机焦距 f Xw,Yw,Zw为世界坐标系，则 根据上述透视投影关系,得到以 世界坐标系表示的P点坐标与其 投影点p的坐标(x,y)的关系：yxXcYcCOpP(Xc,Yc,Zc)ZcXwYwZwW摄像机成像模型101000000001ccccZYXffyxZwwcwwwwcwcTZYXTffyxZMXXM1010000000011 对上式两边求导，得：为世界坐标系到图像坐标系的雅可比映射矩阵，它是摄像机内外参数的函数。进一步，经过立体视觉摄像机定标，得到：其中，
50、=，k代表摄像机1，2。上式为手眼机器人跟踪系统的视觉伺服控制方程。如果物体在世界坐标系下的速度 已知，根据采样时间步长t，前一帧图像位置x(k)，根据上式可以估计下一帧图像位置x(k+1)，则可通过控制摄像机位姿，可以实现对目标的跟踪。wVJ XXVJ)()()()()1(kukJttkxkxV)(kuwX)()()()()1(kukJttkxkxVwX4.2 微分运动与广义速度微分运动与广义速度4.2 微分运动与广义速度 刚体或坐标系的微分运动包含微分移动矢量d和微分转动矢量。前者由沿三个坐标轴的微分移动组成；后者又绕三个坐标轴的微分转动组成，即 将两者合并为6维列矢量D，称为刚体或坐标系
