[发明专利]一种Stewart并联机构运动学正解方法

摘要

本发明公开一种针对Stewart并联机构的运动学正解方法。该方法中在平面平台型Stewart并联机构中心添加一个测试杆及用以测量该测试杆杆长的附加传感器，通过测出测试杆杆长，对于该新Stewart并联机构的姿态矩阵，结合代数消元等方法对11个相容方程进行降次、降次、升次处理，最终求出面向闭环实时反馈控制所需要的Stewart并联机构解析正解，解决了奇异性局限，且位置和姿态可获得唯一解。

主权项

一种Stewart并联机构运动学正解方法，其中的Stewart并联机构包含下平台、上平台，上平台通过至少6个并联的伸缩杆连接下平台，其特征在于，在所述Stewart并联机构中增加一个测试杆，该测试杆同样为可伸缩杆，该测试杆的上端连接上平台外接圆圆心O2，下端连接下平台外接圆圆心O1，且该测试杆上设有用以测量该测试杆杆长L0的附加传感器，该方法包括如下步骤：(1)、建立Stewart并联机构的姿态矩阵：四元数ε表示为：ε＝ε1i+ε2j+ε3k+ε0式中：ε1、ε2、ε3、ε0∈R；i2＝j2＝k2＝‑1；ij＝‑ji＝k；jk＝‑kj＝i；ki＝‑ik＝j；εTε＝1；采用单位四元数表示的3阶姿态矩阵R的具体形式如下：R=ϵ02+ϵ12-ϵ22-ϵ322ϵ1ϵ2-2ϵ0ϵ32ϵ0ϵ2+2ϵ1ϵ32ϵ1ϵ2+2ϵ0ϵ3ϵ02-ϵ12+ϵ22-ϵ32-2ϵ0ϵ1+2ϵ2ϵ3-2ϵ0ϵ2+2ϵ1ϵ32ϵ0ϵ1+2ϵ2ϵ3ϵ02-ϵ12-ϵ22+ϵ32∈R3×3]]>为了算法求解正解方便和最终得到唯一一组位置姿态，将R中的一些元素采用新符号进行等价代换处理，并将剩余元素也用新符号表示，令姿态矩阵R等价表示为如下形式：R=A+BD-Cγ1D+CA-Bγ2α1α2γ3]]>其中，C＝2ε0ε3，D＝2ε1ε2，γ1＝2ε0ε2+2ε1ε3，α1＝‑2ε0ε2+2ε1ε3，α2＝2ε0ε1+2ε2ε3，(2)、主、次变量分离：Stewart型并联机构的矢量表示形式如下：Lkek＝P+Rak‑bk k＝0,1,..其中：Lk为O1O2和第k个连杆杆长；ek为第k个连杆单位向量；P为动平台位置在静坐标系中的位置矢量，P＝{Px,Py,Pz}T；R为姿态矩阵；ak为动平台各个顶点和中心坐标，ak＝{axk,ayk,azk}T；bk为静平台各个顶点和中心坐标，bk＝{bxk,byk,bzk}T；由此矢量形式得到标量方程：L2-r12-r22=-2A(axbx+ayby)-2B(axbx-ayby)+2C(aybx-axby)-2D(aybx+axby)-2bxPx-2byPy+2axWx+2ayWy+Pp]]>其中：L为O1O2和第k个连杆杆长；r1为静平台外接圆半径；r2为动平台外接圆半径；ax、ay、az为动平台第k个顶点ak的各个分量；bx、by、bz为动平台第k个顶点bk的各个分量；Px、Py为动平台的位置矢量P的分量；Wx、Wy为静平台的位置矢量W的分量；Pp为位置矢量P模长的平方，且Pp可以通过传感器测量出的数值L0间接得到，Pp为已知；对标量方程进行主、次变量分离，得到主、次变量分离的线性方程组如下：Pp=L02Px=Px0+k1DPy=Py0+k1BWx=Wx0+k2DWy=Wy0+k2BC=C5A=A5]]>其中：L0为通过传感器测量出的测试杆杆长；Px0=112r1Csc[θ1-θ2][Cos[θ2](L12-L22-2L32+2L42+L52-L62)+3Sin[θ2](L12+L22-L52-L62)];]]>Py0=112r1Csc[θ1-θ2][3Cos[θ2](L12-L22-L52+L62)-Sin[θ2](L12+L22-2L32-2L42+L52+L62)];]]>Wx0=112r2Csc[θ1-θ2][Cos[θ1](L12-L22-2L32+2L42+L52-L62)+3Sin[θ1](L12+L22-L52-L62)];]]>Wy0=112r2Csc[θ1-θ2][3Cos[θ1](L12-L22-L52+L62)-Sin[θ1](L12+L22-2L32-2L42+L52+L62)];]]>C5=112r1r2Csc[θ1-θ2][(L12-L22+L32-L42+L52-L62)];]]>A5=112r1r2Sec[θ1-θ2][(6L02-L12-L22-L32-L42-L52-L62+6r12+6r22)];]]>k1＝‑r2Sin[θ1+2θ2]Csc[θ1‑θ2]；k2＝r1Sin[2θ1+θ2][‑Csc[θ1‑θ2]]；因此，根据Stewart并联机构的结构参数r1、r2、θ1、θ2和杆长参数Lk(k＝0,1,...,6)可以确定姿态矩阵R中变量C和A；(3)、构造变量：根据R的正交性、归一性，R中9个元素存在如下约束条件：(A+B)2+(D-C)2+γ12=1]]>(A-B)2+(D+C)2+γ22=1]]>(A+B)(C+D)+(D‑C)(A‑B)+γ1γ2＝0(A+B)2+(C+D)2+α12=1]]>(A-B)2+(D-C)2+α22=1]]>(A+B)(D‑C)+(C+D)(A‑B)+α1α2＝0γ1＝(D+C)α2‑(A‑B)α1γ2＝(D‑C)α1‑(A+B)α2γ3＝(A+B)(A‑B)‑(D‑C)(D+C)动平台位置在静坐标系O1x1y1z1中的位置矢量P与其在动坐标系O2x2y2z2中的位置矢量W之间有如下关系：P＝R·W由此，可以得到如下关系式：Px＝(A+B)Wx+(D‑C)Wy+γ1Wz，Py＝(C+D)Wx+(A‑B)Wy+γ2Wz；其中，Px、Py为动平台的位置矢量P的分量；Wx、Wy、Wz为静平台的位置矢量W的分量；由以上两式解得：γ1Wz＝Px‑(A+B)Wx‑(D‑C)Wyγ2Wz＝Py‑(C+D)Wx‑(A‑B)Wy同时根据姿态矩阵的正交性，又可以得到：W＝RT·P由此可以得到如下关系式：Wx＝(A+B)Px+(C+D)Py+α1PzWy＝(D‑C)Px+(A‑B)Py+α2Pz由以上两式解得α1Pz、α2Pz的表达式：α1Pz＝Wx‑(A+B)Px‑(C+D)Pyα2Pz＝Wy‑(D‑C)Px‑(A‑B)Py(4)构造相容方程对Stewart类并联机构可以得到包含B、D的以下12个相容方程：eq1:(γ1Wz)2-γ12Wz2=0]]>eq2:(γ2Wz)2-γ22Wz2=0]]>eq3:γ1γ2Wz2-(γ1Wz)(γ2Wz)=0]]>eq4:(γ1γ2)(γ1Wz)-γ12γ2Wz=0]]>eq5:(γ1γ2)(γ2Wz)-γ22γ1Wz=0]]>eq6:(γ1γ2)2-(γ12)(γ22)=0]]>eq7:(α1Pz)2-α12Pz2=0]]>eq8:(α2Pz)2-α22Pz2=0]]>eq9:α1α2Pz2-(α1Pz)(α2Pz)=0]]>eq10:(α1α2)(α1Pz)-α12α2Pz=0]]>eq11:(α1α2)(α2Pz)-α22α1Pz=0]]>eq12:(α1α2)2-(α12)(α22)=0]]>其中，eq6和eq12是完全等价的；(5)构造运动学方程对Stewart类并联机构，将步骤(3)中的变量代入步骤(4)中11个相容方程，得到11个含有B和D的相关方程，将11个相容方程统一写为如下形式：f1,jB4+f2,jB3D+f3,jB2D2+f4,jBD3+f5,jD4+f6,jB3+f7,jB2D+f8,jBD2+f9,jD3+f10,jB2+f11,jBD+f12,jD2+f13,jB+f14,jD+f15,j＝0 j＝1,…11fi,j皆是由结构参数r1、r2、θ1、θ2和杆长参数Lk决定的常数；(6)求解方程对步骤(5)中方程按照降次、降次、升次思路，并利用第三次升次的结果和第二次降次的结果相等，将11个相容方程化简为6个常系数且关于B和D的二次线性方程，方程组形式如下：t1,jB2+t2,jBD+t3,jD2+t4,jB+t5,jD+t6,j＝0；j＝1～6对此方程组求解，即可求得B和D唯一解。