1、山东大学机械工程学院机电工程研究所2010/09/02前置模式：前置模式：i-1坐标系i。仅涉及i杆件的参数，1、杆长:沿xi轴从zi-1到zi的距离。2、扭角：绕xi从zi-1转到zi的角度。3、平移量：沿zi-1轴从xi-1轴量至xi轴的距离。4、转角：绕zi-1轴从xi-1轴到xi的转角。山东大学机械工程学院机电工程研究所2010/09/02第第3 3章章 机器人运动学机器人运动学3.1 3.1 机器人的位姿描述机器人的位姿描述3.2 3.2 齐次变换及运算齐次变换及运算3.3 3.3 机器人运动学方程机器人运动学方程3.4 3.4 机器人微分运动机器人微分运动山东大学机械工程学院机电工
2、程研究所2010/09/023.3.2小节运 动 学 方 程 的 逆解3.3 机器人运动学方程机器人运动学方程山东大学机械工程学院机电工程研究所2010/09/023.3 机器人运动学方程 机器人运动学方程的逆解，也称机器人的逆运动学问题，或间接位置求解。逆运动学问题：对某个机器人，当给出机器人手部在基座标系中所处的位置和姿态时（即M0h中各元素给定），求出其对应的关节变量值qi。山东大学机械工程学院机电工程研究所2010/09/022、运动学方程的逆解65236465423z646541652364654231646541652364654231)()()()()(cscsscccsnscc
3、csccssssccccsnscccsscsssscccccnyx逆运动学问题的可解性：下面以六自由度机器人PUMA为例，研究其可解性。65236465423z646541652364654231646541652364654231)()()()()(ssccscccsoccscscssscsscccsoccscssssscssccccoyx其中：山东大学机械工程学院机电工程研究所2010/09/022、运动学方程的逆解5235423z5415235423154152354231)()(ccccsassccscccsassscsccccayx)sin(cossinsincos)cos(sins
4、incoscosjijijiijjijijiijsc其中：2223454235236z25461222345235423612546122234523542361)()()()()(slcdccsccdpdssdcclsdcssccdspdssdsclsdcssccdcpyx山东大学机械工程学院机电工程研究所2010/09/022、运动学方程的逆解 可见，我们有12个方程及6个未知数。上述12个方程关系如何？我们先看看转动部分，它是3X3子矩阵，共有9个元素；我们知道，转动矩阵的每列都是单位矢量，并且每列之间都两两正交；因此，9个元素中仅三个是独立的，或则说，12个方程中仅有6个是独立，对应6
5、个未知数。因此，一般情况下，单从数学的角度看，方程组应该是有解的。山东大学机械工程学院机电工程研究所2010/09/022、运动学方程的逆解 上述方程组是由一些非线性的、超越、难解的方程组成。为了降低求解难度，机器人的杆件参数应仅可能地取为0，如常见的PUMA机器人那样。对于任何非线性方程组，必须关心其解的存在性、多解性和求解方法。山东大学机械工程学院机电工程研究所2010/09/022、运动学方程的逆解解得存在性：解是否存在与机器人的工作空间密切相关，工作空间又取决于机器人的结构、杆件参数，或手部（工具）的位姿。一般情况下，如果手部坐标系的位置和姿态都位于工作空间内，则至少存在一个解；相反，
6、若手部坐标系的位置和姿态都位于工作空间外，则无解。山东大学机械工程学院机电工程研究所2010/09/022、运动学方程的逆解多解性问题：解得数量不仅与机器人的关节数有关，还与它的杆件参数、关节活动范围等相关。一般说，连杆的非零参数越多，解的数量就越多，即到达某个位置的路经就越多。多个解的存在使我们面临选择。如何选择？如：路径最短、最近原则。多解的应用：躲避障碍物等。山东大学机械工程学院机电工程研究所2010/09/023.3 机器人运动学方程运动学逆解的求解方法 不像线性方程，不存在求解非线性方程组的通用算法。非线性方程组的算法应能求出它的所有解；因此，某些数值递推方法不适用。逆解的形式：1)
7、闭式解(Close-form solution):用解析函数式表示解。特点：求解速度快。存在闭式解是机器人设计的目标，仅仅在一些特殊情况下，机器人存在解析的闭式解，如：相邻的多个关节轴交与一点，杆件扭角等于0或90度等。山东大学机械工程学院机电工程研究所2010/09/023.3 机器人运动学方程 2)数值解(Numerical solution):特点：递推求解。求解方法分类：代数法、几何法以及数值法，前两种用于求闭式解，后一种用于数值解。下面我们结合几个实例，介绍机器人闭式解析解的求解方法。山东大学机械工程学院机电工程研究所2010/09/023.3 机器人运动学方程例例1 1：已知四轴平
8、面关节已知四轴平面关节SCARASCARA机器机器人如图所示，试计算：人如图所示，试计算：（1 1）机器人的运动学方程；）机器人的运动学方程；（2 2）当关节变量取）当关节变量取qi=30=30，-60-60，120120，9090 T T时，机器人手部的位置和姿态；时，机器人手部的位置和姿态；（3 3）机器人运动学逆解的数学）机器人运动学逆解的数学表达式。表达式。800400300200山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解：解：1 1）运动学方程）运动学方程a a、建立坐标系（前置模式）、建立坐标系（前置模式）机座坐标系机座
9、坐标系0 0 杆件坐标系杆件坐标系i i 手部坐标系手部坐标系h hx0z0 x1z1x4hz4h800400300200 x2z2x3z31023山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解解：（1 1）运动学方程）运动学方程b b、确定参数、确定参数 i dii lii qi 1 800 1400 01 2 02300 02 3 d3 0 0 0 d3 4-200 4 0 04800400300200 x0z0 x1z1x2z2x3z3x4hz4h0123山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人
10、运动学方程机器人运动学方程解解：（1 1）运动学方程）运动学方程c c、相邻杆件位姿矩阵、相邻杆件位姿矩阵 10008001004000400010000100001040000110008001000000)0,0,400(),()800,0,0(1111111111101 scscsccsscTranszRotTransM800400300200 x0z0 x1z1x2z2x3z3x4hz4h山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解解：（1 1）运动学方程）运动学方程c c、相邻杆件位姿矩阵、相邻杆件位姿矩阵 10000100
11、30003000100001000010300001100001000000)0,0,300(),(2222222222212 scscsccsscTranszRotM800400300200 x0z0 x1z1x2z2x3z3x4hz4h山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解解：（1 1）运动学方程）运动学方程c c、相邻杆件位姿矩阵、相邻杆件位姿矩阵 100010000100001),0,0(3323ddTransM800400300200 x0z0 x1z1x2z2x3z3x4hz4h山东大学机械工程学院机电工程研究所20
12、10/09/023.3 3.3 机器人运动学方程机器人运动学方程解解：（1 1）运动学方程）运动学方程c c、相邻杆件位姿矩阵、相邻杆件位姿矩阵 10002001000000),()200,0,0(44444)(34 cssczRotTransMh800400300200 x0z0 x1z1x2z2x3z3x4hz4h山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解解：（1 1）运动学方程）运动学方程d d、建立方程、建立方程 1000600100300400030040003121124124121124124)(342312010ds
13、scsccscMMMMMhh )sin(),cos(421124421124 sc式中：式中：)sin(),cos(21122112 sc山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解解：（2 2）已知）已知qi=30，-60，120，90T，则：，则：100048010050021233350023210hM山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解解：（3 3）逆解数学表达式）逆解数学表达式已知运动学方程，用通式表示为：已知运动学方程，用通式表示为：10001000010
14、004311221112412412211124124dddslslcsclclscpaonpaonpaonzzzzyyyyxxxx 已知关系分析：上述矩阵方程有4个未知量，由于第一行第一列元素与第二行第二列元素相等，第一行第二列元素与第二行第一列元素大小相等、符号相反；因此，仅4个元素相互独立，与变量数相同。山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解解：（3 3）逆解数学表达式）逆解数学表达式联立方程：联立方程：)()()()()(4311221112211124124epddddpslslcpclclbonsaonczyxxyy
15、x 其中，其中，n nx x、n ny y、p px x、p py y和和p pz z是已知的手的位姿，是已知的手的位姿，1 1、2 2、4 4及及d d3 3是待求的未知量。是待求的未知量。山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解解：（3 3）逆解数学表达式）逆解数学表达式由上面（由上面（a a）、（）、（b b）两式可得）两式可得 ：xynn1421tan yxnsnc)sin()cos(421124421124山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程2212222
16、11211221221222211211221sin)sin(sin2sincos)cos(cos2cosyxpll llpll ll）（）（yxpllpll）（）（2121121211sinsincoscos由上面（由上面（c c）、（）、（d d）两式：）两式：2122212222cosl lllppyx21222122122cosllllppyx则：两边平方可得两边平方可得 ：将两式相加得：将两式相加得：211212211212ssccsssccc山东大学机械工程学院机电工程研究所2010/09/02这时 已经求出。3.3 3.3 机器人运动学方程机器人运动学方程yxpccslslpss
17、cclcl）（）（21212112121211s2解解：（3 3）逆解数学表达式）逆解数学表达式 为了求为了求1 1，由上面（，由上面（c c）、（）、（d d）两式展开）两式展开可得可得 ：yxpscllcslpsslccll12211221221221)()()()(化简，得：化简，得：)()()()()(4311221112211124124epddddpslslcpclclbonsaonczyxxyyx 山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解解：（3 3）逆解数学表达式）逆解数学表达式由上面两式可得由上面两式可得 ：22
18、21222222211)()()(cllslpslpcllcyx 2222221222211)()()(slcllpslpcllsxy yxxypslpcllpslpcll222212222111111)()(tancossintan ）（可得可得 ：山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程 xynn1421tan 解解：（3 3）逆解数学表达式）逆解数学表达式已知已知1，2后，由后，由2114tan xynn可得：可得：最后由（最后由（e)e)式可得式可得 ：413ddpdz 山东大学机械工程学院机电工程研究所2010/09/02
19、3.3 3.3 机器人运动学方程机器人运动学方程解解：（3）逆解数学表达式逆解数学表达式为：413ddpdz yxxypslpcllpslpcll222212222111)()(tan 21222122122cosllllppyx 2114tan xynn 可见，四轴平面关节SCARA机器存在封闭式逆解表达式。山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程 结合实例介绍另一种求逆解的代数法。例2：已知PUMA机器人，如图所示，试用递推逆变换法计算其运动学逆解（后置模式）。与教材有些不同 D2=0,d3不等0.山东大学机械工程学院机电工程研
20、究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解：结构参数和关节变量表31山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程 接下来写出一些杆件间齐次变换阵，并注意其中的一些元素。山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程其中：山东大学机械工程学院机电工程研究所2010/09/02手部相对基座坐标系的位姿矩阵：2、运动学方程的逆解1000r3332312322211312116zyxoprrrprrrprrT已知山东大学机械工程学院机电工程研究所2010/0
21、9/023.3 3.3 机器人运动学方程机器人运动学方程、由：1606101561120106TTTTTTTTii311dpcpsyx1633323123222113121111111000100001000000Tprrrprrrprrrcssczyx 注意到，T16的（2,4）元素为d3。让上式中等号两边的（2,4）元素相等，得：（1）（2）已知一个未知量1越靠近基座越简单山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程令：代入（2）式有：根据和差公式，得：最后：求出了1山东大学机械工程学院机电工程研究所2010/09/023.3 3.
22、3 机器人运动学方程机器人运动学方程 、求出1后，(1)式的左边矩阵就已知，如果我们再令(1)式等号两边(1,4)和(3,4)元素相等，可得：（3）将以上两式平方后相加，可得：其中：（4）两个未知量2，,3山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程（4）式与（2）式有相同的形式，可得：、我们注意到，T36中的（1,4）和（2,4）元素为常数，由：3606103TTT（5）令(5)式等号两边(1,4)和(2,4)元素相等，可得：（6）1、2已求出山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人
23、运动学方程 （6）式中，仅c23和s23两个未知数，联立可解得：山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程 这样，式（5）左边矩阵中的所有元素都已知了。、为了求4和5，令(5)式等号两边(1,3)和(3,3)元素相等，可得：设s5不等于零，得：5等于零对应4轴与6轴共线的奇异结构，这时 的转动效果相同，可任取 。464与山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程、为了求5和6，我们应用T46：4606104TTT其中：令(7)式等号两边(1,3)和(3,3)元素相等，可得：（
24、7）山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程解得：同样，令(7)式等号两边(3,1)和(1,1)元素相等，可得：其中：山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程总结：山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程 从所得的各关节变量表达式可看出：只有1-3的式中有 ,故他们确定了末杆坐标系原点的位置。而4-6 三式中有 ，它们确定了末杆坐标系的姿态。这是后三个关节交与一点这种结构的重要特点之一。zyxppp,xzyxo
25、nnn山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程递推逆变换法小结：递推逆变换法小结：1、原则：等号两端的矩阵中对应元素相等，列出相关方程进行求解。2、步骤：1）、从含变量少的左边开始，如T01，向右递推，直到求出所有变量或无法继续。2）、选择等号左边或右边矩阵中等于常数或仅含有一个变量的元素，列出相应元素对应的方程或方程组。60610iiTTT山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程 3、技巧：求角时，尽量采用反正切，并依据x和y的符号，判定它所在的象限。4、问题：求解过程中可能会出现多解的情况，这时要根据操作机的机构特点判定位姿的可能性，选用适合的最终公式。山东大学机械工程学院机电工程研究所2010/09/02多解情况多解情况山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程 例3：用几何法求出下述机器人的运动学逆解。山东大学机械工程学院机电工程研究所2010/09/023.3 3.3 机器人运动学方程机器人运动学方程 解：由余弦定理得：为了求1，定义和角，如图。有：
