专利名称：一种六自由度串联机械臂的位置逆解控制方法
技术领域：
本发明涉及一种六自由度串联机械臂的位置逆解控制方法。
背景技术：
六自由度机械臂控制系统，包括依次连接的上位机、下位机、电机驱动器、电机和六自由度机械臂。在整个系统中六自由度机械臂的位置逆解问题至关重要。位置逆解问题是机械手机构学乃至机械学中的最基础也是最重要的研究问题之一，它直接关系到机械手运动分析、离线编程、轨迹规划和实时等工作。机械臂的位置逆算法，就是机械手臂的手部在固定的直角坐标空间中某一点和某一姿态，求解机械臂六个自由度相应的关节角(或称关节坐标)，因此逆算法是机器人控制的基本组成部分。传统的机械手的结构一般比较特殊，如轴线相交或平行，轴线长度为零等等，这样它的姿态和位置之间就没有耦合，其逆解很容易用分离变量的办法实现。然而对于一类结构尺寸比较一般的复杂机械手，由于姿态和位置高度耦合，一般无法进行变量分离，这时必须借助于数值算法，可分为三类(I)数值-解析法、牛顿-拉弗森法等，这些算法可满足实时性要求，较难得到全部逆解，且必须给出适当的初值。(2)优化算法、区间迭代法、遗传算法等，这类算法收敛范围大，可求出全部逆解，但一般实时性差。(3)位置和姿态分别迭代法，这类算法能较迅速地求得全部解，但当机械手位置和姿态高度耦合时，迭代过程会发散。
发明内容
为了克服已有位置逆解控制方法的实时性较差、准确性不高的不足，本发明提供一种实时性良好、准确性高的六自由度串联机械臂的位置逆解控制方法。本发明解决其技术问题所采用的技术方案是一种六自由度串联机械臂的位置逆解控制方法，其特征在于利用对偶四元数能表示三维物体旋转和平移的性质，六自由度机械臂的各个转动关节经过转换由对偶四元数表示出来，在三维空间中，绕单位矢量U = (Ux, Uy, Uz)旋转0角的旋转可以用单位四元数表示为cos ( 0 /2) +sin ( 0/2) (uxi+uyj+uzk)即q= [cos( 0/2), sin( 0/2)<ux, uy, uz>] (I)单位四元数如式(2)所示能够描述出刚体的旋转，三维空间内的一个位移可以由旋转加平移合成，以单位四元数q表示旋转，P = (Px，Py, Pz)表示平移矢量，则用对偶四元数表示为Q(q, p) = ([cos( 0/2), sin( 0/2)〈ux, uy, uz>], <px, py, pz>) (2)则对偶四元数求逆表示为Q-1 = (q_1, -q_1*p*q) (14)
其中-q-1*p*q =-p+[_2s (vX (_p))+2vX (vX (_p)) ] (15)机械臂的转动关节与对偶四元数的转换关系定义成[Rw, Tj = ([w, <a, b, c>, <px, py, pz>]) (16)其中(Rw，Tw)代表机械臂各个转动关节相对于底座的转动方向和在三维空间里的位移向量；设Qi, I < i < 6为各个机械臂转动关节相对于机械臂底座空间上的转换公式，利用这个转换公式把各个转动关节联系在一起如Qjq1, P1) , Qjq1, p2) ...Q6 (q6, p6) (17)且设Si = QiQwQ6 (18)Lj+l = Q-1Lj(19)其中分别取I彡i彡6，I彡j彡5且L1 = [Rw, Tw],为了能提取机械臂转动关节的共有变量如关于s，V, px, py, pz的函数、固定的连接参数，认定Si和Lj+1对应相等，即S1 = L1, S2 = Lf S6 = L6。本发明的技术构思为把对偶四元数既能在三维空间中描述出刚体的位移又能描述其旋转的性质应用到六自由度机械臂上，确定六自由度机械手臂的转动关节与对偶四元数的转换关系，再用各个转动关节之间的联系确立方程求出逆解。要解决的技术问题是如何将对偶四元数应用到六自由度机械臂上，及确立转动关节之间的联系确立方程求逆解。本发明的有益效果主要表现在1)发明将对偶四元数运用到求解六自由度机械臂逆解中，对求解机械臂逆解方法进行了改进与创新，并扩展了新的研究领域；2)本发明所提出的求解机械臂逆解的新方法能很好的满足机械臂逆运动学算法的实时性和准确性； 3)本发明所采用的方法非常实用，计算量小，易于实现，很好地体现了新理论的工程化与实用化。
图I是RS型机械臂简易图。
具体实施例方式下面参照附图对本发明作进一步描述。参照图I，一种六自由度串联机械臂的位置逆解控制方法，其特征在于利用对偶四元数能表示三维物体旋转和平移的性质，六自由度机械臂的各个转动关节经过转换由对偶四元数表示出来，在三维空间中，绕单位矢量U = (Ux, Uy, Uz)旋转0角的旋转可以用单位四元数表示为cos ( 0 /2) +sin ( 0/2) (uxi+uyj+uzk)即q= [cos( 0/2), sin( 0/2)<ux, uy, uz>] (I)单位四元数如式(2)所示能够描述出刚体的旋转，三维空间内的一个位移可以由旋转加平移合成，以单位四元数q表示旋转，P = (Px，Py, Pz)表示平移矢量，则用对偶四元数表示为 Q(q, p) = ([cos( 0/2), sin( 0/2)〈ux, uy, uz>], <px, py, pz>) (2)则对偶四元数求逆表示为Q-1 = (q' -q-1*p*q) (14)其中-q-1*p*q = -p+[_2s (vX (_p))+2vX (vX (_p)) ] (15)机械臂的转动关节与对偶四元数的转换关系定义成[Rw, Tj = ([w, <a, b, c>, <px, py, pz>]) (16)其中(Rw，Tw)代表机械臂各个转动关节相对于底座的转动方向和在三维空间里的位移向量；设Qi, I < i < 6为各个机械臂转动关节相对于机械臂底座空间上的转换公式，利用这个转换公式把各个转动关节联系在一起如
Ql ( ，Pi)，Q2 ( ，P2)…( , P6)(17)且设Si = QiQw-Q6 (18)Lj+1= Q-lLi(19)其中分别取I≤i≤6，I≤j≤5且L1 = [Rw, Tw],为了能提取机械臂转动关节的共有变量如关于s，V, px, py, pz的函数、固定的连接参数，认定Si和Li+1对应相等，即S1 = L1, S2 = L2... S6 = L6O本实施例中，四元数的定义如下若设q = a+bi+cj+dk (a, b, c, d G R), i、j、k满足
(
权利要求
1.一种六自由度串联机械臂的位置逆解控制方法，其特征在于将对偶四元数运用到求解机械臂逆解中，对偶四元数能表示三维物体旋转和平移，在三维空间中，绕单位矢量u =(Ux, Uy, Uz)旋转0角的旋转可以用单位四元数表示为cos ( 0 /2) +sin ( 0/2) (uxi+uyj+uzk)即q = [cos ( 0 /2), sin (0/2) <ux, Uy, uz>] (I)单位四元数如式(2)所示能够描述出刚体的旋转，三维空间内的一个位移可以由旋转加平移合成，以单位四元数q表示旋转，P = (Px，Py, Pz)表示平移矢量，则用对偶四元数表示为Q(q, p) = ([cos( 0 /2), sin( 0 /2)<ux, uy, uz>], <px, py, pz (2)则对偶四元数求逆表示式为(14)CT1 = (q' _q-1*p*q) (14)其中-q-1*p*q = -p+[-2s (vX (_p))+2vX (vX (-p)) ] (15)将机械臂的转动关节与对偶四元数的转换关系定义成[Rw，Tw] = ([w,〈a, b, c>, <px, py, pz>]) (16)其中(Rw，Tw)代表机械臂各个转动关节相对于底座的转动方向和在三维空间里的位移向量;设Qi(l ^ i ^6)为各个机械臂转动关节相对于机械臂底座空间上的转换公式，利用这个转换公式把各个转动关节联系在一起如Ql (Ql Pi)，Q2 ( ，P2)…( , P6) (17)且设Si = QiQw-Q6 (18)AU(19)其中分别取I彡i彡6，I彡j彡5且L1 = [Rw, Tw],为了能提取机械臂转动关节的共有变量如关于s，v，px，py，pz的函数、固定的连接参数， 认定Si和Lj+1对应相等，即S1 = L1, S2 = Lf S6 = L6。
2.如权利要求I所述的一种六自由度串联机械臂的位置逆解控制方法，其特征在于 四元数的定义如下若设q = a+bi+cj+dk(a, b, c, d G R)，i、j、k满足
全文摘要
一种六自由度串联机械臂的位置逆解控制方法，利用对偶四元数能表示三维物体旋转和平移的性质，六自由度机械臂的各个转动关节经过转换由对偶四元数表示出来，确定六自由度机械手臂的转动关节与对偶四元数的转换关系，再用各个转动关节之间的联系确立方程求出逆解。要解决的技术问题是如何将对偶四元数应用到六自由度机械臂上，及确立转动关节之间的联系确立方程求逆解。本发明提供一种实时性良好、准确性高的六自由度串联机械臂的位置逆解控制方法。
文档编号G05D3/00GK102609002SQ20121005545
公开日2012年7月25日 申请日期2012年3月5日 优先权日2012年3月5日
发明者南余荣, 吴攀峰 申请人:浙江工业大学