第六讲机器人运动学逆解课件.ppt

山东大学机械工程学院机电工程研究所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前置模,山东大学机械工程学院机电工程研究所2010/09/02,第3章 机器人运动学,3.1 机器人的位姿描述3.2 齐次变换及运算3.3 机器人运动学方程3.4 机器人微分运动,山东大学机械工程学院机电工程研究所2010/09/02第3章,山东大学机械工程学院机电工程研究所2010/09/02,3.3.2小节运 动 学 方 程 的 逆解,3.3 机器人运动学方程,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,机器人运动学方程的逆解，也称机器人的逆运动学问题，或间接位置求解。逆运动学问题：对某个机器人，当给出机器人手部在基座标系中所处的位置和姿态时（即M0h中各元素给定），求出其对应的关节变量值qi。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,2、运动学方程的逆解,逆运动学问题的可解性：下面以六自由度机器人PUMA为例，研究其可解性。,其中：,山东大学机械工程学院机电工程研究所2010/09/022、运,山东大学机械工程学院机电工程研究所2010/09/02,2、运动学方程的逆解,其中：,山东大学机械工程学院机电工程研究所2010/09/022、运,山东大学机械工程学院机电工程研究所2010/09/02,2、运动学方程的逆解,可见，我们有12个方程及6个未知数。上述12个方程关系如何？我们先看看转动部分，它是3X3子矩阵，共有9个元素；我们知道，转动矩阵的每列都是单位矢量，并且每列之间都两两正交；因此，9个元素中仅三个是独立的，或则说，12个方程中仅有6个是独立，对应6个未知数。因此，一般情况下，单从数学的角度看，方程组应该是有解的。,山东大学机械工程学院机电工程研究所2010/09/022、运,山东大学机械工程学院机电工程研究所2010/09/02,2、运动学方程的逆解,上述方程组是由一些非线性的、超越、难解的方程组成。为了降低求解难度，机器人的杆件参数应仅可能地取为0，如常见的PUMA机器人那样。对于任何非线性方程组，必须关心其解的存在性、多解性和求解方法。,山东大学机械工程学院机电工程研究所2010/09/022、运,山东大学机械工程学院机电工程研究所2010/09/02,2、运动学方程的逆解,解得存在性：解是否存在与机器人的工作空间密切相关，工作空间又取决于机器人的结构、杆件参数，或手部（工具）的位姿。一般情况下，如果手部坐标系的位置和姿态都位于工作空间内，则至少存在一个解；相反，若手部坐标系的位置和姿态都位于工作空间外，则无解。,山东大学机械工程学院机电工程研究所2010/09/022、运,山东大学机械工程学院机电工程研究所2010/09/02,2、运动学方程的逆解,多解性问题：解得数量不仅与机器人的关节数有关，还与它的杆件参数、关节活动范围等相关。一般说，连杆的非零参数越多，解的数量就越多，即到达某个位置的路经就越多。多个解的存在使我们面临选择。如何选择？如：路径最短、最近原则。多解的应用：躲避障碍物等。,山东大学机械工程学院机电工程研究所2010/09/022、运,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,运动学逆解的求解方法 不像线性方程，不存在求解非线性方程组的通用算法。非线性方程组的算法应能求出它的所有解；因此，某些数值递推方法不适用。逆解的形式：1)闭式解(Close-form solution):用解析函数式表示解。特点：求解速度快。存在闭式解是机器人设计的目标，仅仅在一些特殊情况下，机器人存在解析的闭式解，如：相邻的多个关节轴交与一点，杆件扭角等于0或90度等。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,2)数值解(Numerical solution):特点：递推求解。求解方法分类：代数法、几何法以及数值法，前两种用于求闭式解，后一种用于数值解。下面我们结合几个实例，介绍机器人闭式解析解的求解方法。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,例1：已知四轴平面关节SCARA机器人如图所示，试计算：（1）机器人的运动学方程；（2）当关节变量取qi=30，-60，120，90T时，机器人手部的位置和姿态；（3）机器人运动学逆解的数学表达式。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：1）运动学方程a、建立坐标系（前置模式）机座坐标系0 杆件坐标系i 手部坐标系h,1,0,2,3,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（1）运动学方程b、确定参数,0,1,2,3,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（1）运动学方程c、相邻杆件位姿矩阵,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（1）运动学方程c、相邻杆件位姿矩阵,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（1）运动学方程c、相邻杆件位姿矩阵,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（1）运动学方程c、相邻杆件位姿矩阵,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（1）运动学方程d、建立方程,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（2）已知qi=30，-60，120，90T，则：,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（3）逆解数学表达式已知运动学方程，用通式表示为：,分析：上述矩阵方程有4个未知量，由于第一行第一列元素与第二行第二列元素相等，第一行第二列元素与第二行第一列元素大小相等、符号相反；因此，仅4个元素相互独立，与变量数相同。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（3）逆解数学表达式联立方程：,其中，nx、ny、px、py和pz是已知的手的位姿，1、2、4及d3是待求的未知量。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（3）逆解数学表达式由上面（a）、（b）两式可得：,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,由上面（c）、（d）两式：,两边平方可得：,将两式相加得：,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,这时 已经求出。,3.3 机器人运动学方程,解：（3）逆解数学表达式 为了求1，由上面（c）、（d）两式展开可得：,化简，得：,山东大学机械工程学院机电工程研究所2010/09/02这时,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（3）逆解数学表达式由上面两式可得：,可得：,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（3）逆解数学表达式已知1，2后，由,可得：,最后由（e)式可得：,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：（3）逆解数学表达式逆解数学表达式为：,可见，四轴平面关节SCARA机器存在封闭式逆解表达式。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,结合实例介绍另一种求逆解的代数法。例2：已知PUMA机器人，如图所示，试用递推逆变换法计算其运动学逆解（后置模式）。,与教材有些不同 D2=0,d3不等0.,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：结构参数和关节变量表,31,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,接下来写出一些杆件间齐次变换阵，并注意其中的一些元素。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,其中：,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,手部相对基座坐标系的位姿矩阵：,2、运动学方程的逆解,山东大学机械工程学院机电工程研究所2010/09/02手部相,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,、由：,注意到，T16的（2,4）元素为d3。让上式中等号两边的（2,4）元素相等，得：,（1）,（2）,已知,一个未知量1,越靠近基座越简单,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,令：代入（2）式有：根据和差公式，得：最后：,求出了1,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,、求出1后，(1)式的左边矩阵就已知，如果我们再令(1)式等号两边(1,4)和(3,4)元素相等，可得：,（3）,将以上两式平方后相加，可得：,其中：,（4）,两个未知量2，,3,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,（4）式与（2）式有相同的形式，可得：,、我们注意到，T36中的（1,4）和（2,4）元素为常数，由：,（5）,令(5)式等号两边(1,4)和(2,4)元素相等，可得：,（6）,1、2已求出,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,（6）式中，仅c23和s23两个未知数，联立可解得：,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,这样，式（5）左边矩阵中的所有元素都已知了。、为了求4和5，令(5)式等号两边(1,3)和(3,3)元素相等，可得：,设s5不等于零，得：,5等于零对应4轴与6轴共线的奇异结构，这时 的转动效果相同，可任取。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,、为了求5和6，我们应用T46：,其中：,令(7)式等号两边(1,3)和(3,3)元素相等，可得：,（7）,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解得：,同样，令(7)式等号两边(3,1)和(1,1)元素相等，可得：,其中：,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,总结：,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,从所得的各关节变量表达式可看出：只有1-3的式中有,故他们确定了末杆坐标系原点的位置。而4-6 三式中有，它们确定了末杆坐标系的姿态。这是后三个关节交与一点这种结构的重要特点之一。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,递推逆变换法小结：1、原则：等号两端的矩阵中对应元素相等，列出相关方程进行求解。2、步骤：1）、从含变量少的左边开始，如T01，向右递推，直到求出所有变量或无法继续。2）、选择等号左边或右边矩阵中等于常数或仅含有一个变量的元素，列出相应元素对应的方程或方程组。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,3、技巧：求角时，尽量采用反正切，并依据x和y的符号，判定它所在的象限。4、问题：求解过程中可能会出现多解的情况，这时要根据操作机的机构特点判定位姿的可能性，选用适合的最终公式。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,多解情况,山东大学机械工程学院机电工程研究所2010/09/02多解情,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,例3：用几何法求出下述机器人的运动学逆解。,山东大学机械工程学院机电工程研究所2010/09/023.3,山东大学机械工程学院机电工程研究所2010/09/02,3.3 机器人运动学方程,解：由余弦定理得：,为了求1，定义和角，如图。有：,山东大学机械工程学院机电工程研究所2010/09/023.3,