1、机器人运动学机器人运动学20052005年年3 3月月2424日日运动学正问题运动学正问题 杆件参数的意义杆件参数的意义 坐标系的建立原则坐标系的建立原则 杆件坐标系间的变换过程杆件坐标系间的变换过程- -相邻关节坐标相邻关节坐标系的齐次变换系的齐次变换 机器人的运动学方程机器人的运动学方程 杆件参数的意义杆件参数的意义- - 和和 li 关节关节Ai轴和轴和Ai+1轴轴线公法线的长度线公法线的长度 关节关节i轴线与轴线与i+1轴线在垂直于轴线在垂直于li平面平面内的夹角内的夹角 串联关节，每个杆件最多与串联关节，每个杆件最多与2个杆件相连，如个杆件相连，如Ai与与Ai-1和和 Ai+1相连。
2、由运动学的观点来看，杆件的作用仅在于它能保相连。由运动学的观点来看，杆件的作用仅在于它能保持其两端关节间的形态不变。这种形态由两个参数决定，一持其两端关节间的形态不变。这种形态由两个参数决定，一是杆件的长度是杆件的长度 li（ ），一个是杆件的扭转角，一个是杆件的扭转角 iiaAi iAi+i+1 1iiliili 杆件参数的意义杆件参数的意义- - 和和 是从第是从第i-1坐标系坐标系的原点到的原点到Zi轴和轴和i轴的交点沿轴的交点沿i-1轴测量的距离轴测量的距离 绕绕 Zi-1轴由轴由i-1 轴转向轴转向i轴的关节轴的关节角角 确定杆件相对位置关系，由另外确定杆件相对位置关系，由另外2个参
3、数决定，一个是杆个参数决定，一个是杆件的距离：件的距离： ，一个是杆件的回转角：，一个是杆件的回转角： iidiidiAi iAi+i+1 1iilid1iliAi-i-1 1id 坐标系的建立原则坐标系的建立原则Ai iAi+i+1 1iilid1iliAi-i-1 11iz1ix1iy1ioizixiyio 为右手坐标系为右手坐标系 原点原点Oi：设在：设在Li与与Ai+1轴线的交点上轴线的交点上 Zi轴：与轴：与Ai+1关节轴关节轴重合，指向任意重合，指向任意 Xi轴：与公法线轴：与公法线Li重合，指向沿重合，指向沿Li由由Ai轴线指向轴线指向Ai+1轴线轴线 Yi轴：按右手定则轴：按右
4、手定则 Li 沿沿 xi 轴，轴， zi-1 轴与轴与 xi 轴交点到轴交点到 0i 的距离的距离i 绕绕 xi 轴，由轴，由 zi-1 转向转向zidi 沿沿 zi-1 轴，轴，zi-1 轴和轴和 xi 交点至交点至0i 1 坐标系原点的距离坐标系原点的距离i 绕绕 zi-1 轴，由轴，由 xi-1转向转向 xi 杆件坐标系间的变换过程杆件坐标系间的变换过程 - -相邻关节坐标系的齐次变换相邻关节坐标系的齐次变换 将将xi-1轴绕轴绕zi-1轴转轴转 i 角度，将其与角度，将其与xi轴平行；轴平行； 沿沿zi-1轴平移距离轴平移距离di ，使，使zi-1轴与轴与zi轴重合；轴重合； 沿沿xi
5、轴平移距离轴平移距离Li，使两坐标系原点及，使两坐标系原点及x轴重轴重合；合； 绕绕xi 轴转轴转 i角度，两坐标系完全重合角度，两坐标系完全重合 机器人的运动学方程机器人的运动学方程1sincos0cossincossinsin0sincoscoscossin00sincos1iiiiiiiiiiiiiiiiiiidaaA001112iiiTAAA D-H变换矩阵变换矩阵运动学逆问题运动学逆问题 多解性，剔除多余解原则多解性，剔除多余解原则v根据关节运动空间合适的解根据关节运动空间合适的解v选择一个与前一采样时间最接近的解选择一个与前一采样时间最接近的解v根据避障要求得选择合适的解根据避障要
6、求得选择合适的解v逐级剔除多余解逐级剔除多余解 可解性可解性v所有具有转动和移动关节的系统，在一个单一串联中所有具有转动和移动关节的系统，在一个单一串联中总共有总共有6 6个（或小于个（或小于6 6个）自由度时，是可解的，一般个）自由度时，是可解的，一般是数值解，它不是解析表达式，而是利用数值迭代原是数值解，它不是解析表达式，而是利用数值迭代原理求解，它的计算量要比解析解大理求解，它的计算量要比解析解大v如若干个关节轴线相交和或多个关节轴线等于如若干个关节轴线相交和或多个关节轴线等于0 0或或9090的情况下，具有的情况下，具有6 6个自由度的机器人可得到解析解个自由度的机器人可得到解析解例题
7、：例题：试求立方体中心在机座坐标系试求立方体中心在机座坐标系00中的位置中的位置该手爪从上方把物体抓起，同时手爪的开合方向与物体的该手爪从上方把物体抓起，同时手爪的开合方向与物体的Y Y轴同向，轴同向，那么，求手爪相对于那么，求手爪相对于00的姿态是什么？的姿态是什么？ 在机器人工作台上加装一电视摄像机，摄像机可见到固联在机器人工作台上加装一电视摄像机，摄像机可见到固联着着6DOF关节机器人的机座坐标系原点，它也可以见到被操作关节机器人的机座坐标系原点，它也可以见到被操作物体（立方体）的中心，如果在物体中心建一局部坐标系，则物体（立方体）的中心，如果在物体中心建一局部坐标系，则摄像机所见到的这
8、个物体可由齐次变换矩阵摄像机所见到的这个物体可由齐次变换矩阵T1来表示，如果摄来表示，如果摄像机所见到的机座坐标系为矩阵像机所见到的机座坐标系为矩阵T2表示。表示。1000101-002001-010-001T100091-00100011010T21xyz解解1 1：xyzz机y机z物y物x物oO机O物 T T 21物机机摄物摄求，已知TTT TT 11 -2）（有：物摄摄机物机TTT 100091-00100011010 1000101-002001-0100011000110010001-11010 O物根据T1画出O机根据T2画出因此物体位于机座坐标系的（因此物体位于机座坐标系的（11
9、，10，1）T处，它的处，它的X，Y，Z轴分别与机座坐标系的轴分别与机座坐标系的-Y，X，Z轴平行。轴平行。 解解2 2：xyzz机y机z物y物x物oO机O物手爪机实际要求Tpzazsznzpyaysynypxaxsxnx1000 向重合手爪开合方向与物体ya:Ts001有方向相反方向物体的从上向下抓，指出手爪zab:Ta 100则有Tkjikjiasnc01000100001:1-00001010因此：姿态矩阵为重合时与物体中心当手爪中心100011-001000111010 T物机xyzz机y机z物y物x物oO机O物手爪机实际要求Tpzazsznzpyaysynypxaxsxnx1000
10、向重合手爪开合方向与物体ya:Ts001有方向相反方向物体的从上向下抓，指出手爪zab:Ta100则有Osnayzx特殊情况坐标系的建立原则特殊情况坐标系的建立原则 Oi Ai与与Ai+1关节轴线的交点关节轴线的交点 Zi Ai+1轴线轴线 Xi Zi和和Zi-1构成的面的法线构成的面的法线 Yi 右手定则右手定则 AiAi+1o oi iz zi i- -1 1z zi ixiyi两个关节轴相交两个关节轴相交两个关节轴线平行两个关节轴线平行 先建立先建立 00i-1i-1 然后建立然后建立00i+1i+1 最后建立最后建立 00i i Ai-1AiAi+1Ai+2li-1oi-1xi-1yi
11、-1zi-1ABCDoi（xi）（yi）zixiyioi+1xi+1yi+1zi+1di+1li+1di举例：举例：StanfordStanford机器人机器人A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z3y3x3O3y4z4x4O4z5y5x5O534545,0o o odd重重合合d3z6x6y6O6d6z0y0 x0O0 为右手坐标系为右手坐标系 原点原点Oi： Ai与与Ai+1关节轴线的交点关节轴线的交点 Zi轴：与轴：与Ai+1关节轴关节轴重合，指向任意重合，指向任意 Xi轴：轴： Zi和和Zi-1构构成的面的法线成的面的法线 Yi轴：按右手定则轴：按右手定则
12、Li 沿沿 xi 轴，轴， zi-1 轴与轴与 xi 轴交点到轴交点到 0i 的距离的距离i 绕绕 xi 轴，由轴，由 zi-1 转向转向zidi 沿沿 zi-1 轴，轴，zi-1 轴和轴和 xi 交点至交点至0i 1 坐标系原坐标系原 点的距离点的距离i 绕绕 zi-1 轴，由轴，由 xi-1转向转向 xi解：解： 用未知的逆变换逐次左乘，由乘得的矩阵用未知的逆变换逐次左乘，由乘得的矩阵方程的元素决定未知数，即用逆变换把一方程的元素决定未知数，即用逆变换把一个未知数由矩阵方程的右边移到左边个未知数由矩阵方程的右边移到左边 求解这个未知数求解这个未知数 把下一个未知数移到左边把下一个未知数移到
13、左边 重复上述过程，直到解出所有解重复上述过程，直到解出所有解 运动学逆问题解法运动学逆问题解法Paul 等人提出的方法等人提出的方法: : Paul 等人提出的方法等人提出的方法65544332211060TTTTTTT TTTTTT T 6554433221601 -10）（1 q65544332601 -101 -21TTTTTTT）（）（2q65601 -101 -21132143154TTTT)T()T()T(）（）（5 qE601 -101 -65TT) T( ）（6 q100060pzazsznzpyaysynypxaxsxnxT机器人末端操作器位姿的其它描述方法机器人末端操作器
14、位姿的其它描述方法 用矩阵表示刚性体的转动简化了许多运算，用矩阵表示刚性体的转动简化了许多运算，但它需要但它需要9 9个元素来完全描述旋转刚体的姿个元素来完全描述旋转刚体的姿态，因此矩阵并不直接得出一组完备的广义态，因此矩阵并不直接得出一组完备的广义坐标。坐标。 一组广义坐标应能描述转动刚体相对于参考一组广义坐标应能描述转动刚体相对于参考坐标的方向，被称为欧拉角的三个角度，坐标的方向，被称为欧拉角的三个角度，、就是这种广义坐标。就是这种广义坐标。 有几种不同的欧拉角表示方法，它们均可描有几种不同的欧拉角表示方法，它们均可描述刚体相对于固定参考系的姿态。三种最常述刚体相对于固定参考系的姿态。三种
15、最常见的欧拉角类型列在表中见的欧拉角类型列在表中 3 3种最常见的欧拉角类型种最常见的欧拉角类型步步1步步2步步3类型类型1绕绕OZ轴转轴转角角绕当前绕当前OU 轴转轴转角角绕当前绕当前OW轴转轴转角角类型类型2绕绕OZ轴转轴转角角绕当前绕当前OV 轴转轴转角角绕当前绕当前OW轴转轴转角角类型类型3绕绕OX轴转轴转角角绕绕OY轴转轴转角角绕绕OZ轴转轴转角角uvwx(u)y (v)z (w)ouvwu?v?W?),(ZR),(R),(wR N0T100000000110000cssccssccsscccssssccccssscccsssccsscscscc类型类型1：表示法通常用于陀螺运动：表
16、示法通常用于陀螺运动 类型类型2：所得的转动矩阵为右乘所得的转动矩阵为右乘 10000c0s-010s0c 10000),(),v(),(RcssccsscwRRZR1000pzpyRpxTccsssssccscsscccssccssccssccc类型类型3： 一般称此转动的欧拉角一般称此转动的欧拉角为横滚、俯仰和偏航角，这种形为横滚、俯仰和偏航角，这种形 式主要用于航空工程中分析飞行式主要用于航空工程中分析飞行器的运动，其旋转矩阵为（这种器的运动，其旋转矩阵为（这种方法也叫做横滚、俯仰和偏航角方法也叫做横滚、俯仰和偏航角表示方法）表示方法） ccscssccssccssscssscsccss
17、sccccssccssccsscxRyRz000010010010000),(),(),RR（ZYX偏航偏航俯仰俯仰横滚横滚斯坦福机器人运动学逆问题解斯坦福机器人运动学逆问题解6533211060AAAAT61T653321AAA式中：式中： yxyxpCpSpfzpfpSpCpf1113121111)()()(211dpcpsyx由两端矩阵对应元素相等可得：由两端矩阵对应元素相等可得： 作三角变换：作三角变换： 式中：式中： 得到：得到： 即有：即有： （ ）由由1, 4和和2, 4元素对应相等，得：元素对应相等，得： 6261121TTA式中第四列：式中第四列： 6362132TTA式中第
18、三列式中第三列： 微动矩阵和微动齐次变换微动矩阵和微动齐次变换 对象对象: 微动矩阵主要是描述机器人在微动微动矩阵主要是描述机器人在微动范围内各关节的位移运动关系范围内各关节的位移运动关系 定义定义: 各关节当角度移小于各关节当角度移小于5时，平移时，平移在在0.1mm左右时，微动矩阵大致可用左右时，微动矩阵大致可用 设：设：有一机器人如图，末端执行器在机座坐标系中的有一机器人如图，末端执行器在机座坐标系中的齐次变换为齐次变换为oTN，做微动，做微动，绕任意轴绕任意轴w轴转轴转 ；绕绕各坐标轴平移各坐标轴平移dx,dy,dzdx,dy,dz求：求： 在在 中的位置和姿态中的位置和姿态. . 定
19、义定义 为微动齐次变换矩阵为微动齐次变换矩阵 N000O0nasX0Y0Z0OnNdTNransTdwRdzdydxTT0N0),(),( 变化后）（NransTdwRdzdydxTdTT0NN0),(),( 记为TdwRdzdydxTdTTrans),(),(TEdwRdzdydxTdTrans),(),()T( . ),(),(的左边在注意，称为微动率，令TdTEdwRdzdydxTrans在忽略高次项在忽略高次项的情况下：微的情况下：微动齐次变换与动齐次变换与次序无关次序无关 d 微动平移和微动旋转的齐次变换：微动平移和微动旋转的齐次变换：平移：平移： 1000100010001dzdy
20、dxTr旋转旋转R R ，绕任意轴，绕任意轴 旋转旋转 角角: :dw,wddddddddddddddddddddwcos)cos(1rsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrcos)cos(1rsinr)cos(1rrsinr)cos(1rrcos)cos(1rR2zxzyyzxxzyyzx2yzyxzyx2x,1000010101sin, 1cosdrdrdrdrdrdrdddxyxzyz在微动范围内绕经意轴转动在微动范围内绕经意轴转动 角角, ,可以看作绕可以看作绕x,y,zx,y,z轴的微转轴的微转动的合成。因此：动的合成。因此：
21、 ddrzddryddrxdzyx,1000010101,xdydxdzdydzddwR因此：因此：因此微动率因此微动率= = EdwRdzdydxTrans,0000000dzxdyddyxdzddxydzd微动的齐次变换：微动的齐次变换：dT= T 己知变换矩阵己知变换矩阵 1000000131007010T,001 . 0kjidkjidp6 . 003 . 0转动：转动： 平移平移： 求求d T 解：解： 00006 . 001 . 0001 . 0003 . 000010000100001000011000011 . 0001 . 010000110006 . 010000103 .
22、 000100009 . 01 . 0000001 . 03 . 0000100000013100701000006 . 001 . 0001 . 0003 . 0000TdT10009 . 01 . 0013101 . 03 . 7010dTT反过来：如果我们要求反过来：如果我们要求 在在 中的齐次交换矩阵为中的齐次交换矩阵为 nooo1000000131007010T实际测得的为实际测得的为 10009 . 01 . 0013101 . 03 . 7010那么末端执行器坐标系要如何运动才能到达期望值？那么末端执行器坐标系要如何运动才能到达期望值？ 1000011 . 0001 . 0100
23、001转动：转动： 10006 . 010000103 . 0001平移平移： 等效微动位移的求解等效微动位移的求解 前面研究的是动坐标系前面研究的是动坐标系On在在Oo中的中的b变换为变换为T，相对于基准坐标系作微平移和，相对于基准坐标系作微平移和微转动，来求微动齐次交换。微转动，来求微动齐次交换。 现在我们研究动坐标系现在我们研究动坐标系 On相对于自身相对于自身坐标系做了微位移或微转动，达到绕基坐标系做了微位移或微转动，达到绕基准坐标同样的效果则如何求解。准坐标同样的效果则如何求解。 dT=T （绕基准坐标系）（绕基准坐标系） =TT （绕动坐标系）（绕动坐标系）左乘左乘,绕基准绕基准右
24、乘右乘, 绕动坐标轴绕动坐标轴强调等效强调等效TTTTTT11TT1000zzzzyyyyxxxxpasnpasnpasnT 10001PaaaaPsssspnnnnTTzyxTzyxTzyx设：设： 有：有： 0000000pdpdaasdandpdpdssadsndpdpdnnadnsdT0000000pdpdandsdpdpdsndadpdpdnsdad绕自身轴的微动率绕自身轴的微动率和绕固定坐标系坐标轴的微动和绕固定坐标系坐标轴的微动率率之间的什么关系之间的什么关系，举例说明：举例说明： 例：一动坐标系相对于固定坐标系的齐例：一动坐标系相对于固定坐标系的齐 次交换为次交换为 10000
25、00131007010Tnsap己知相对固定坐标系的微己知相对固定坐标系的微动平移和转动动平移和转动 kjidkjidp001 . 06 . 003 . 0求：求： 与与 求求dT 求与之等效的绕动坐标系的微平移和微转动求与之等效的绕动坐标系的微平移和微转动 解：解： =0000000dzxdyddyxdzddxydyd00006 . 001 . 0001 . 0003 . 00000100001 . 01 . 0001001 . 00010001 . 0kjikjindkjikjisdkjikjiad kjikjikjipdpd6 . 003 . 0037001 . 0kjikji6 . 0
26、03 . 0037001 . 0 kjikjikji9 . 003 . 06 . 003 . 03 . 00009 . 003 . 00103 . 09 . 003 . 00019 . 09 . 003 . 0100kjikjipdpdakjikjipdpdskjikjipdpdn0000000pdpdandsdpdpdsndadpdpdnsdadT00000001 . 03 . 00009 . 01 . 00000009 . 01 . 0000001 . 03 . 0000100000013100701000006 . 001 . 0001 . 0003 . 0000TdT00009 . 0
27、1 . 0000001 . 03 . 000000000001 . 03 . 00009 . 01 . 0001000000131007010TTdT解解 ：解解： 绕自身平移和转动绕自身平移和转动TTTTTTTTkjidkjidp01 . 0003 . 09 . 0其结果等于绕固定坐标系转其结果等于绕固定坐标系转动和旋转动和旋转 kjidkjidp001 . 06 . 003 . 0等效等效说明：说明：如果我们发现末端操作器相对于基准坐标系有了如果我们发现末端操作器相对于基准坐标系有了微位移（平移或转动）微位移（平移或转动）, 我们可以认为末端操作器相对我们可以认为末端操作器相对于自己的坐标
28、系发生了微位移。只是微动率于自己的坐标系发生了微位移。只是微动率和和不不同而己。其结果是等效的。同而己。其结果是等效的。 这些在进行误差补偿和微动时有用这些在进行误差补偿和微动时有用, 如产生误差如产生误差 如何补偿？可以反向运动末端关节来补偿如何补偿？可以反向运动末端关节来补偿ddp微动齐次变换的意义微动齐次变换的意义误差及误差补偿误差及误差补偿 制造和检测误差制造和检测误差 运算过程中圆整、插补、拟合造成的误差运算过程中圆整、插补、拟合造成的误差原理性误差原理性误差 构件承受的负载、加速度、重力的变形误差构件承受的负载、加速度、重力的变形误差 传动误差传动误差 环境影响误差环境影响误差误差
29、来源：误差来源： 单关节补偿单关节补偿 多关节补偿多关节补偿误差补偿：误差补偿：单关节补偿：单关节补偿： 1116561111iiiiiiAATdAAid 11111656111211121iiiiiiiiAATTdAAA666565656565656522212121212121211110101010101010AAAAAdAAAAAAdAAAAAAdA6565212110106060AdAAdAAdATdT忽略高次项：忽略高次项：66651065322221106521111060AAAAAAAAATd绕自身绕自身656521106521211065100060AAAAAAAATd绕绕 i-1 多关节补偿：多关节补偿：并联机器人运动学并联机器人运动学 燕山大学燕山大学 黄真黄真 并联机器人机构学理论及其控制并联机器人机构学理论及其控制110001.,0.0,.0),()()()(,1000T 00222262,100zibyibxibpzazsznzpyaysynypxaxsxnxbizbiybixzibyibxibbibizbiybixBizBiyBixBizbizBiybiyBixbixllllpzazsznzpyaysynypxaxsxnxi）是已知的中，（在中的位置在也就是）现在求（中是已知的在易有：求：中的位置和姿态在即：已知即为所求解222iziyixillll
