[发明专利]一种六自由度串联机械臂的位置逆解控制方法

权利要求书

1.一种六自由度串联机械臂的位置逆解控制方法，其特征在于：将对偶四元数运用到求解机械臂逆解中，对偶四元数能表示三维物体旋转和平移，在三维空间中，绕单位矢量u＝(u_x，u_y，u_z)旋转θ角的旋转可以用单位四元数表示为：

cos(θ/2)+sin(θ/2)(u_xi+u_yj+u_zk)

即

q＝[cos(θ/2)，sin(θ/2)<u_x，u_y，u_z>]    (1)

单位四元数如式(2)所示能够描述出刚体的旋转，三维空间内的一个位移可以由旋转加平移合成，以单位四元数q表示旋转，p＝(p_x，p_y，p_z)表示平移矢量，则用对偶四元数表示为：

Q(q，p)＝([cos(θ/2)，sin(θ/2)<u_x，u_y，u_z>]，<p_x，p_y，p_z>)    (2)

则对偶四元数求逆表示式为(14)：

Q^-1＝(q^-1，-q^-1*p*q)    (14)

其中

-q^-1*p*q＝-p+[-2s(v×(-p))+2v×(v×(-p))]    (15)

将机械臂的转动关节与对偶四元数的转换关系定义成：

[R_w，T_w]＝([w，<a，b，c>，<p_x，p_y，p_z>])    (16)

其中(R_w，T_w)代表机械臂各个转动关节相对于底座的转动方向和在三维空间里的位移向量；

设Q_i(1≤i≤6)为各个机械臂转动关节相对于机械臂底座空间上的转换公式，利用这个转换公式把各个转动关节联系在一起如：

Q₁(q₁，p₁)，Q₂(q₁，p₂)…Q₆(q₆，p₆)    (17)

且设

S_i＝Q_iQ_i+1…Q₆    (18)

Lj+1=Qj-1Lj---(19)]]>

其中分别取1≤i≤6，1≤j≤5且L₁＝[R_w，T_w]，

为了能提取机械臂转动关节的共有变量如关于s，v，p_x，p_y，p_z的函数、固定的连接参数，认定S_i和L_j+1对应相等，即S₁＝L₁，S₂＝L₂…S₆＝L₆。

2.如权利要求1所述的一种六自由度串联机械臂的位置逆解控制方法，其特征在于：四元数的定义如下：若设q＝a+bi+cj+dk(a，b，c，d∈R)，i、j、k满足

i2=j2=k2=-1ij=-ji=kjk=-kj=iki=-ik=j---(3)]]>

则称q为四元数，其中i，j，k为虚数单元，各虚数单元间的关系如式(3)所示模a²+b²+c²+d²＝1的四元数称为单位四元数；

是q的共轭四元数为：

q‾=a-bi-cj-dk---(4)]]>

单位四元数满足公式：

qq‾=1---(5)]]>

将四元数表示为：

q＝(s，v)    (6)

其中s表示纯量a，v表示向量bi+cj+dk，且两个单位四元数q₁和q₂相乘表示为：

q₁*q₂＝(s₁+v₁)*(s₂+v₂₂)＝    (7)

s₁*s₂-v₁·v₂+v₁×v₂+s₁*v₂+s₂*v₁

即

q₁*q₂＝[s₁s₂-v₁·v₂，s₁v₂+s₂v₁+v₂×v₁]    (8)

在三维空间中，绕单位矢量u＝(u_x，u_y，u_z)旋转θ角的旋转用单位四元数表示为

q＝[cos(θ/2)，sin(θ/2)<u_x，u_y，u_z>]    (9)

单位四元数如式(9)所示能够描述出机械臂的旋转，但没有描述出在三维空间中的机械臂位移关系；三维空间内的机械臂的一个位移由旋转加平移合成，以单位四元数q表示旋转，p＝(p_x，p_y，p_z)表示平移矢量，则用对偶四元数表示为：

Q(q，p)＝([cos(θ/2)，sin(θ/2)<u_x，u_y，u_z>]，<p_x，p_y，p_z>)    (10)

两个对偶四元数Q₁(q₁，p₁)和Q₂(q₂，p₂)相乘可表示为：

Q₁Q₂＝(q₁，p₁)*(q₂，p₂)＝(q₁*q₂，q₁*p₂*q₁^-1+p₁)    (11)

其中q₁*q₂由式(7)可得，而

q1*p2*q1-1+p1=p2+2s1(v1×p2)+2v1×(v1×p2)---(12)]]>

求单位四元数的逆表示成：

q^-1＝[s，-v]    (13)

由所述公式(13)得到对偶四元数求逆表达式(14)。