一种基于四元数的快速求解STEWART并联机构的运动学正解方法.pdf

上传人：a2

文档编号：4855013

上传时间：2018-11-18

格式：PDF

页数：13

大小：1.11MB

《一种基于四元数的快速求解STEWART并联机构的运动学正解方法.pdf》由会员分享，可在线阅读，更多相关《一种基于四元数的快速求解STEWART并联机构的运动学正解方法.pdf（13页完整版）》请在专利查询网上搜索。

1、(10)申请公布号 CN 103538067 A (43)申请公布日 2014.01.29 CN 103538067 A (21)申请号 201310465247.6 (22)申请日 2013.10.08 B25J 9/16(2006.01) (71)申请人南京航空航天大学地址 210016 江苏省南京市御道街 29 号 (72)发明人吴洪涛杨小龙陈柏申浩宇叶纯杰 (74)专利代理机构南京苏高专利商标事务所 ( 普通合伙 ) 32204 代理人张弛 (54) 发明名称一种基于四元数的快速求解 Stewart 并联机构的运动学正解方法 (57) 摘要本发明公开一种基于四元。

2、数的快速求解 Stewart 并联机构的运动学正解方法，该方法中采用四元数的运算法则，经过变量代换和公式化简，导出了形式上简洁对称的 Stewart 并联机构能够使用的运动学方程，能够有效提高 Stewart 并联机构运动学正解的计算效率。 (51)Int.Cl. 权利要求书 1 页说明书 7 页附图 4 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请权利要求书1页说明书7页附图4页 (10)申请公布号 CN 103538067 A CN 103538067 A 1/1 页 2 1. 一种基于四元数的快速求解 Stewart 并联机构的运动学正解方法，。

3、所述的 Stewart 并联机构包括下平台、上平台及连接上、下平台的若干并联的伸缩杆，其特征在于，该方法包括如下步骤：（1）、用四元数表示刚体转动：任意矢量 x 能按与单位矢量 n 平行和垂直的方向分解为和的形式： x (xn)n+(nx)n 矢量 x 绕轴 n 转动角度后为： R(,n) (xn)n+(nx)sin+(nx)ncos 用 (， 0) (1230) 表示一个单位四元数，令是一个单位四元数 (nsin/2,cos/2,)， n 是一个单位矢量， R 为转动矢量， R3为在三维坐标系中转动矢量集合，对于任意 x R3，乘积而且与刚体转动矢量 R。

4、(,n) 是等价的，通过以下计算证明该定理（2）、建立正向运动学方程： Stewart 并联机构的运动学正解方程为式中 x 是动平台的位姿坐标， Qi是描述并联机构结构的 88 常对称矩阵， Ci是与姿态坐标无关的数。（3）、构造迭代序列：采用下述迭代序列 xk为迭代计算 k 次后的位姿坐标， xk为迭代时第 k+1 次与第 k 次的位姿之差， Jk为第 k 次计算中的雅可比矩阵， C 为由 Ci构成并扩展后的八维矢量。 2. 如权利要求 1 所述的基于四元数的快速求解 Stewart 并联机构的运动学正解方法，其特征在于，还包括：步骤（4）、确保收敛。

5、性：避免奇异性与选择迭代初值该步骤中，当在第 k 次迭代计算中判定 Jk接近奇异时，将步骤（3）中的迭代序列公式变为另，对 Stewart 机构实时控制时，其动平台按给定要求连续运动，伸缩杆长度 L 是时间 t 的函数；当杆长从 L(t0) L0连续改变至 L(t) Lt时，将时间段 t t-t0分割为若干控制周期，每个周期中采用上述迭代序列公式计算动平台位姿，其初值选取上一周期内计算获得的位姿坐标。权利要求书 CN 103538067 A 2 1/7 页 3 一种基于四元数的快速求解 Stewart 并联机构的运动学正解方法技术领域 000。

6、1 本发明属于机械系统的运动学、动力学与控制研究领域，尤其是一种 0002 Stewart 并联机构的正向运动学正解方法。背景技术 0003 Stewart 并联机构 ( 也称 Stewart 平台 ) 由上下两个平台和六个并联的自由伸缩杆组成，每条伸缩杆通过两个球铰或者一个球铰和一个虎克铰与上下两平台相连。该机构本身的下平台（基座）静止不动，通过控制六个伸缩杆独立运动，可使上平台（动平台）获得需要的位置和姿态，即将六个移动自由度转变为三个位置和三个方向自由度。相比串联机构，它具有一些固有的优势，包括更大的刚度质量比，更高的基频，可以承受相对较大的负载。

7、；更强的动态性能和稳定性；以及更高的运动精度，能完成精密级任务。自1965年被提出以来，其运动学、奇异性、工作空间与灵巧性、动力学与控制、平台的设计与开发等方面均得到深入而广泛的研究，现已广泛应用于运动模拟系统、微位移定位装置、可视化触觉装置、工业机器人和医用机器人、天文望远镜等方面。 0004 虽然并联机构的多项优点使其成为高速运动、精密定位等应用场合下（例如加工中心、射电望远镜等）的理想解决方案，然而该机构耦合程度高，运动控制复杂，寻求高精度、低时耗的运动学稳定解是一个研究难点。其逆向运动学问题定义为根据动平台确切的位姿（位置和姿态。

8、）求解对应杆长。实际上，该问题并不复杂，六个杆长的表达式独立，能并行计算，很快完成求解。正向运动学问题则是在六个杆长已知的情况下，求解动平台相对基座的位置和姿态，在一般情形下，不具备封闭形式和唯一解。而快速运动学正解在反馈控制、机构奇异性和工作空间分析中具有及其重要的作用，因此解决正向运动学问题是并联机构研究领域内亟待解决的挑战性任务之一。 0005 解决正向运动学的方法有两类：解析法和数值法。在解析法方面，众多学者采用代数消元法、连续法、区间分析等将运动学方程组转化为一个高阶多项式方程，致力于找到该方程的所有可能解，并取得了一些进展，这些解称。

9、作 Stewart 机构的装配模式。但是，至今无法表达出位姿变量的显式形式。况且，找到所有可能解也未完全解决正向运动学问题，仍需进一步在这些解中确定唯一的实际位姿，这是实际应用时必需的。在某些情况下，针对由解析法得到的一个单变量高次代数方程或者非线性方程组，可利用附加传感器获得唯一解，但在实际应用中有所限制，例如昂贵的价格和测量误差等。在数值法方面，牛顿 - 拉夫逊法被广泛使用，该方法是将非线性代数方程组线性化为线性方程组求解，其收敛域依赖于非线性方程组的性质，若迭代初值位于收敛域内，可获得精确解。也有学者采用神经网络算法获得牛顿 - 拉夫逊算法所需。

10、初值，保证算法的稳定性。直接采用遗传算法、神经网络算法等优化算法求解运动学方程也可获得唯一解，但遗传算法、神经网络算法等均耗时较长，不适合实时性的应用要求。 0006 上述的正向运动学的复杂性在很大程度上取决于该机构的构型、几何尺寸和传感说明书 CN 103538067 A 3 2/7 页 4 器布局。尽管针对某一简化的构型（例如采用复合球铰、动平台顶点平行布置等）已有很可观的研究成果，但研究一般形式的 Stewart 平台的正向运动学算法更具有普遍意义。而且，目前已有的若干种数值算法难以使 Stewart 平台满足高速、实时的工程应用。 0007 故，需要。

11、一种新的技术方案以解决上述问题。发明内容 0008 本发明的目的是针对现有技术存在的不足，提供一种提高计算效率的基于四元数的快速求解 Stewart 并联机构的运动学正解方法。 0009 为解决上述问题，本发明基于四元数的快速求解 Stewart 并联机构的运动学正解方法可采用如下技术方案： 0010 一种基于四元数的快速求解 Stewart 并联机构的运动学正解方法，所述的 Stewart 并联机构包括下平台、上平台及连接上、下平台的若干并联的伸缩杆，该方法包括如下步骤： 0011 （1）、用四元数表示刚体转动： 0012 任意矢量 x 能按与单位矢量 n 平行。

12、和垂直的方向分解为和的形式： 0013 x (xn)n+(nx)n 0014 矢量 x 绕轴 n 转动角度后为： 0015 R(,n) (xn)n+(nx)sin+(nx)ncos 0016 用 (， 0) (1230) 表示一个单位四元数，令是一个单位四元数 (nsin/2,cos/2,)， n 是一个单位矢量， R 为转动矢量， R3为在三维坐标系中转动矢量集合，对于任意 x R3，乘积而且与刚体转动矢量 R(,n) 是等价的，通过以下计算证明该定理 0017 0018 （2）、建立正向运动学方程： 0019 Stewart 并联机构的运动学正解方程为 0020。

13、式中 x 是动平台的位姿坐标， Qi是描述并联机构结构的 88 常对称矩阵， Ci是与姿态坐标无关的数。 0021 （3）、构造迭代序列： 0022 采用下述迭代序列 0023 0024 xk为迭代计算 k 次后的位姿坐标， xk为迭代时第 k+1 次与第 k 次的位姿之差， Jk 为第 k 次计算中的雅可比矩阵， C 为由 Ci构成并扩展后的八维矢量。 0025 本发明的有益效果：利用四元数运算处理既有移动又有转动的机构运动学问题时，能使运动学方程转化成仅有二次的多项式方程组，大大地降低了求解的难度。特别地，应用牛顿法数值求解时，迭代函数能够进一步巧妙地简化，有效地。

14、降低了计算耗时。该方法说明书 CN 103538067 A 4 3/7 页 5 具有计算精度高、收敛速度快的优点。附图说明 0026 图 1 是本发明中一个矢量的转动示意图。 0027 图 2 是本发明中实施例中的 Stewart 机构简图。 0028 图 3 是本发明中 Stewart 机构动平台位置的绝对偏差测试结果图。 0029 图 4 是本发明中 Stewart 机构动平台姿态的绝对偏差测试结果图。 0030 图 5 是本发明中迭代次数对计算误差的影响测试结果图。具体实施方式 0031 下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不。

15、用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。 0032 1. 请参阅图 1 所示，用四元数表示刚体转动 0033 任意矢量 x 能按与 n 平行和垂直的方向分解为和的形式： 0034 x (xn)n+(nx)n (1) 由于矢量 n， nx 和 (nx)n 是相互正交的，那么矢量 x 的转动为： 0035 R(x) (xn)n+R(nx)n (xn)n+(nx)sin+(nx)ncos (2) 使用矢量径向和横向分解的方法 (1)，上述方程也可写成广为人知的 Euler-Rodrigues 形式：。

16、 0036 R(x) x+(nx)sin+n(nx)(1-cos) (3) 0037 考虑集合 R3R，其元素时由一个矢量 q 和一个标量 q0组成的对 q,q0，表示为： (q,q0) (q1q2q3q0) 或 q=q1i+q2j+q3k+q0，其中 i2 j2 k2 -1，且 ij -ji k， jk -kj i， ki -ik j。那么，对于任意 q qq0 和 p pp0，其合成运算 0038 (q,p) qp q0p+p0q+qp,q0p0-qp (4) 0039 是 q,p 的双线性型，显然，该运算是可结合的，由于包含叉积而不可交换，这使得 R3R 是一个结合。

17、代数。具有这种结构的集合 R3R 命名为 Q，其元素称为四元数。qp 称为 q 右乘 p，或 p 左乘 q。一个四元数 q (q,q0) 的组成部分 q 和 q0分别看成是虚数部分和实数部分 0040 与复数（z=x+i y 虚部是一个实数）不同的是， q 的虚部则是 R3上的矢量。 0041 四元数 q (-q,q0) 称为是 q (q,q0) 的共轭，用表示。映射是矢量空间 Q 的自同构，由于则是代数结构的反自同构。由于 0042 0043 是两个正数的和，那么定义一个四元数的范数为标量是合理的。显然，当且仅当 |q|=0 时， q=0。而且，对任意 p,q Q，。

18、 0044 说明书 CN 103538067 A 5 4/7 页 6 0045 这意味着范数 q |q|:Q R+的存在使 Q 为一个赋范代数。 0046 对于一个四元数 q，如果 |q| 1，那么称该四元数为单位四元数。令表示所有单位四元数的集合： 0047 0048 显然，它不是空集，因为而且是的逆元。对任意的其乘积总之，四元数的乘积运算赋予一个群的结构。必须说明的是，群是不可交换的。 0049 为了说明单位四元数与 R3上的转动群 SO(3) 的联系，用 (， 0) (1230) 表示一个单位四元数。令是一个单位四元数 (nsin/2,cos/2,)， n 。

19、是一个单位矢量。对于任意 x R3，乘积而且与转动 R(,n) 是等价的。通过以下计算即可证明该定理 0050 5) 0051 方程 (5) 和方程 (2) 是一致的。值得注意的是，和只与半角 /2 的形式有关，描述的则是相同的转动，计算结果表明，用单位四元数对转动建模是可行的。 0052 2. 建立正向运动学方程 0053 对于一般的六自由度并联机构，假定两个参考系分别与动平台和基座固连，称为动坐标系和静坐标系。每条伸缩杆与动平台的连接点在动坐标系中的矢径记为： a1，， a6，与基座的连接点在静坐标系中的矢径记为： b1， b6。动坐标系相对静坐标系的姿态用。

20、选择矩阵 R 表示，其原点在静坐标系中矢径记为： P。在三维矢量空间中建立方程： 0054 Liei P+Rai-bi(i 1,6) (6) 式 (6) 中， Li为第 i 个伸缩杆的长度， ei为单位向量，表示第 i 个驱动关节在静坐标系中的方向。考虑式 (5)，将方程 (6) 扩展到空间表达如下： 0055 0056 将式 (7) 右乘，得 0057 Li(ei,0) (P,0)+(ai， 0)-(bi， 0)(i 1,6) 0058 简写为 0059 Lifi h+(ai， 0)-(bi， 0)(i 1,6) (8) 式 (8) 中， h (P， 0) (0P-P，。

21、 -P) Q。式 (8) 自乘其共轭四元数得： 0060 0061 定义 Ai ai-bi， Bi ai+bi，其坐标阵形式： (AixAiyAiz)T和 (BixBiyBiz)T， h 表示为： (hT,h0) (h1h2h3h0),h R3， L (L1L2L6)T，化简并整理式 (9) 得到六个方程：说明书 CN 103538067 A 6 5/7 页 7 0062 0063 上述方程左边可整理成： 0064 其中是动平台的位姿坐标为 0065 x (1230h1h2h3h0)T 0066 同时 0067 0068 0069 Qi是只由并联平台结构参数决定的常对称矩阵。

22、。 0070 另外，根据单位四元数的性质有方程： 0071 0072 f7(x)、 f8(x) 可写成表达式 (11) 的形式： 0073 0074 方程 (11)、 (12) 构成八个二次非线性代数方程组，通过对该方程组的计算可获得一般并联机构的运动学正解。 0075 3. 构造迭代序列 0076 对于非线性代数方程组 (11)、 (12) 中的任一方程，若 a,b R8，则： 0077 0078 设 x* R8为 fi(x) xTQix/2-Ci(i 1,8) 的一个实数解， xk R8为 x*的近似。令式 (14) 中 a x*， b xk， x x*-x，在 xk处。

23、以超切平面代替二次曲面，即略去二阶微量 (xTQix)/2，得到： 0079 -fi(xk) xkTQi(x*-xk)(i 1,8) (15) 那么迭代序列可以写成说明书 CN 103538067 A 7 6/7 页 8 0080 式 (16) 中， Jk (xTQ1xTQ2xTQ8)T， F(xk) (f1(xk)f2(xk)f8(xk)T，式 (16) 是牛顿 - 拉夫逊法应用于非线性方程组时的一般形式。但针对由四元数导出的二次方程组，迭代函数能够被进一步化简，考虑到多项式函数 F(xk) 和雅可比矩阵 Jk有如下关系： 0081 0082 将上式代入式 (16)。

24、中消去迭代函数可简化为如此中每次迭代计算中省略计算 F(xk)，减少了计算时耗，有利于实时性应用。为了避免计算矩阵的逆在实际计算时以数值计算线性方程组代替，采取下列迭代序列： 0083 0084 4. 收敛性：奇异性与迭代初值 0085 当雅可比矩阵 Jk非奇异并且迭代初值接近真实解时，迭代序列 (17) 具有二次收敛特性，能快速计算出局部真实解。若雅可比矩阵奇异或者接近奇异，迭代结果将跃出局部解的邻域，收敛于另一个解或者发散。为了避免这种情况的发生，我们再对 (17) 进行细微修改。我们已经知道如果将(17)中雅可比矩阵Jk替换成常矩阵J0，那么迭代序。

25、列将线性收敛，此时，每次迭代计算中将不会出现雅可比矩阵奇异。这种迭代法可以称作简化牛顿法。 0086 现在我们将迭代序列 (17) 与简化牛顿法结合，如果预测出雅可比矩阵 Jk接近奇异，那么在这次迭代计算中使用简化牛顿法： 0087 0088 我们可以采用很多方式判定雅可比矩阵是否接近奇异，例如较大的矩阵条件数、较小的行列式以及 xk较大的改变。由于计算 xk的变化量相比另外两种方法消耗更少的计算时间，这里，我们将通过这种方式判定雅可比矩阵临近奇异。而在下一次循环中，我们将返回使用式 (17)。这样我们以少量的时间消耗避免了奇异性的发生。 0089 如何选取初值也至。

26、关重要。根据局部收敛性原理知，存在 x*的邻域 S x R8|x*-x ，对于迭代序列 (17) 收敛于 x*，且至少是平方收敛。该方法具有收敛速度快，能够自我校正，不会放大舍入误差等优点，难点是需确保初始值 x0 S。 0090 另一方面，从式 (10)、 (11) 可知杆长 L 是姿态坐标 x 的连续函数，即存在实际杆长 L* 的邻域 T L R6|L*-L ，当 L T时， x S，故可通过控制杆长的变化量，使位姿的变化在允许的范围内。考虑式 (17) 收敛性，对 Stewart 机构实时控制时，其动平台按给定要求连续运动，伸缩杆长度 L 是时间 t 的函。

27、数。当杆长从 L(t0) L0连续改变至 L(t) Lt时，将时间段 t t-t0分割为若干控制周期，每个周期中采用迭代式 (17) 计算动平台位姿，其初值选取上一周期内计算获得的位姿坐标。若将运动学正解应用到工作空间分析，亦将 L1至 L2的一次变化逐步分割为多次变化，对生成的一系列杆长分别采用迭代式 (17) 计算姿态坐标，上一次计算获得的姿态坐标作为下一次迭代计算的初值，以确保说明书 CN 103538067 A 8 7/7 页 9 收敛性。 0091 5. 应用示范 0092 导出的迭代序列适用于任意连接形式的 Stewart 机构，不失一般性，以常用构型。

28、为例验证算法的正确性，六个虎克铰、球铰平行布置，按圆周对称分布。 0093 图 2 描述动平台、静平台与六个伸缩杆连接位置的矢量分别在动坐标系和静坐标系中的坐标表示如下： 0094 0095 0096 在动态仿真时， Stewart 机构的初始装配模型已知，可作为算法 (17) 的初值。在采样周期 T 内，需利用算法找到新解，该解作为下一周期的初值。采用动平台位置和姿态关于时间的函数描述 Stewart 机构的运动。根据式 (5) 和 (7)，动平台位姿由矢量 P 和四元数决定，定义 0097 0098 式 (18) 中， 0099 0100 那么动平台的初始位姿。

29、为： P(t 0) (001)T， (t 0) (0001)。 0101 动态仿真时，采样周期设为 1ms，相当于 1000Hz 的采样频率，计算机的机器精度设为 16，进行了 1000ms 的仿真计算。显然，迭代序列 (17) 的计算次数不同，动平台位置和姿态的计算精度也不同。 0102 图 3 和图 4 中误差曲线是迭代次数为 10 次时计算结果与动平台真实位姿的绝对偏差值。由于仿真循环的次数较多，这里将计算误差定义为最后 100ms 内的最大绝对偏差值。可以看出，迭代次数为10次时，计算误差为3.1110-15，此时已达到计算机的最高计算精度。图 5 显。

30、示了迭代计算次数和计算误差间的变化关系，若每个循环内进行 2 次迭代计算，则计算误差为1.4610-7，当迭代次数大于4时，计算误差下降到10-15量级，在实时应用时我们可以根据实际精度要求控制迭代计算次数。说明书 CN 103538067 A 9 1/4 页 10 图 1 说明书附图 CN 103538067 A 10 2/4 页 11 图 2 说明书附图 CN 103538067 A 11 3/4 页 12 图 3 图 4 说明书附图 CN 103538067 A 12 4/4 页 13 图 5 说明书附图 CN 103538067 A 13 。

摘要
申请专利号：	CN201310465247.6	申请日：	2013.10.08
公开号：	CN103538067A	公开日：	2014.01.29
当前法律状态：	授权	有效性：	有权
法律详情：	授权\|\|\|实质审查的生效IPC(主分类):B25J 9/16申请日:20131008\|\|\|公开
IPC分类号：	B25J9/16	主分类号：	B25J9/16
申请人：	南京航空航天大学
发明人：	吴洪涛; 杨小龙; 陈柏; 申浩宇; 叶纯杰
地址：	210016 江苏省南京市御道街29号
优先权：
专利代理机构：	南京苏高专利商标事务所(普通合伙) 32204	代理人：	张弛
PDF完整版下载：	PDF下载

内容摘要

本发明公开一种基于四元数的快速求解Stewart并联机构的运动学正解方法，该方法中采用四元数的运算法则，经过变量代换和公式化简，导出了形式上简洁对称的Stewart并联机构能够使用的运动学方程，能够有效提高Stewart并联机构运动学正解的计算效率。

权利要求书

权利要求书
1. 一种基于四元数的快速求解Stewart并联机构的运动学正解方法，所述的Stewart并联机构包括下平台、上平台及连接上、下平台的若干并联的伸缩杆，其特征在于，该方法包括如下步骤：
（1）、用四元数表示刚体转动：
任意矢量x能按与单位矢量n平行和垂直的方向分解为和的形式：
x＝(x·n)n+(n×x)×n
矢量x绕轴n转动ω角度后为：
R(ω,n)＝(x·n)n+(n×x)sinω+[(n×x)×n]cosω
用ε＝(ε，ε0)＝(ε1ε2ε3ε0)表示一个单位四元数，令ε是一个单位四元数(nsinω/2,cosω/2,)，n是一个单位矢量，R为转动矢量，R3为在三维坐标系中转动矢量集合，对于任意x∈R3，乘积而且与刚体转动矢量R(ω,n)是等价的，通过以下计算证明该定理
ϵx=(xcosω2+(n×x)sinω2,(-n·x)sinω2)ϵxϵ~=((x·n)n+(n×x)sinω+[(n×x)×n]cosω,0);]]>
（2）、建立正向运动学方程：
Stewart并联机构的运动学正解方程为
式中x是动平台的位姿坐标，Qi是描述并联机构结构的8×8常对称矩阵，Ci是与姿态坐标无关的数。
（3）、构造迭代序列：
采用下述迭代序列
xk+1=12xk+ΔxkJkΔxk=C(k=0,1,2...)]]>
xk为迭代计算k次后的位姿坐标，Δxk为迭代时第k+1次与第k次的位姿之差，Jk为第k次计算中的雅可比矩阵，C为由Ci构成并扩展后的八维矢量。

2. 如权利要求1所述的基于四元数的快速求解Stewart并联机构的运动学正解方法，其特征在于，还包括：
步骤（4）、确保收敛性：避免奇异性与选择迭代初值
该步骤中，当在第k次迭代计算中判定Jk接近奇异时，将步骤（3）中的迭代序列公式变为xk+1=12xk+ΔxkJk-1Δxk=C;]]>
另，对Stewart机构实时控制时，其动平台按给定要求连续运动，伸缩杆长度L是时间t的函数；当杆长从L(t0)＝L0连续改变至L(t)＝Lt时，将时间段Δt＝t-t0分割为若干控制周期，每个周期中采用上述迭代序列公式计算动平台位姿，其初值选取上一周期内计算获得的位姿坐标。

说明书

说明书一种基于四元数的快速求解Stewart并联机构的运动学正解方法
技术领域
本发明属于机械系统的运动学、动力学与控制研究领域，尤其是一种
Stewart并联机构的正向运动学正解方法。
背景技术
Stewart并联机构(也称Stewart平台)由上下两个平台和六个并联的自由伸缩杆组成，每条伸缩杆通过两个球铰或者一个球铰和一个虎克铰与上下两平台相连。该机构本身的下平台（基座）静止不动，通过控制六个伸缩杆独立运动，可使上平台（动平台）获得需要的位置和姿态，即将六个移动自由度转变为三个位置和三个方向自由度。相比串联机构，它具有一些固有的优势，包括更大的刚度质量比，更高的基频，可以承受相对较大的负载；更强的动态性能和稳定性；以及更高的运动精度，能完成精密级任务。自1965年被提出以来，其运动学、奇异性、工作空间与灵巧性、动力学与控制、平台的设计与开发等方面均得到深入而广泛的研究，现已广泛应用于运动模拟系统、微位移定位装置、可视化触觉装置、工业机器人和医用机器人、天文望远镜等方面。
虽然并联机构的多项优点使其成为高速运动、精密定位等应用场合下（例如加工中心、射电望远镜等）的理想解决方案，然而该机构耦合程度高，运动控制复杂，寻求高精度、低时耗的运动学稳定解是一个研究难点。其逆向运动学问题定义为根据动平台确切的位姿（位置和姿态）求解对应杆长。实际上，该问题并不复杂，六个杆长的表达式独立，能并行计算，很快完成求解。正向运动学问题则是在六个杆长已知的情况下，求解动平台相对基座的位置和姿态，在一般情形下，不具备封闭形式和唯一解。而快速运动学正解在反馈控制、机构奇异性和工作空间分析中具有及其重要的作用，因此解决正向运动学问题是并联机构研究领域内亟待解决的挑战性任务之一。
解决正向运动学的方法有两类：解析法和数值法。在解析法方面，众多学者采用代数消元法、连续法、区间分析等将运动学方程组转化为一个高阶多项式方程，致力于找到该方程的所有可能解，并取得了一些进展，这些解称作Stewart机构的装配模式。但是，至今无法表达出位姿变量的显式形式。况且，找到所有可能解也未完全解决正向运动学问题，仍需进一步在这些解中确定唯一的实际位姿，这是实际应用时必需的。在某些情况下，针对由解析法得到的一个单变量高次代数方程或者非线性方程组，可利用附加传感器获得唯一解，但在实际应用中有所限制，例如昂贵的价格和测量误差等。在数值法方面，牛顿-拉夫逊法被广泛使用，该方法是将非线性代数方程组线性化为线性方程组求解，其收敛域依赖于非线性方程组的性质，若迭代初值位于收敛域内，可获得精确解。也有学者采用神经网络算法获得牛顿-拉夫逊算法所需初值，保证算法的稳定性。直接采用遗传算法、神经网络算法等优化算法求解运动学方程也可获得唯一解，但遗传算法、神经网络算法等均耗时较长，不适合实时性的应用要求。
上述的正向运动学的复杂性在很大程度上取决于该机构的构型、几何尺寸和传感器布局。尽管针对某一简化的构型（例如采用复合球铰、动平台顶点平行布置等）已有很可观的研究成果，但研究一般形式的Stewart平台的正向运动学算法更具有普遍意义。而且，目前已有的若干种数值算法难以使Stewart平台满足高速、实时的工程应用。
故，需要一种新的技术方案以解决上述问题。
发明内容
本发明的目的是针对现有技术存在的不足，提供一种提高计算效率的基于四元数的快速求解Stewart并联机构的运动学正解方法。
为解决上述问题，本发明基于四元数的快速求解Stewart并联机构的运动学正解方法可采用如下技术方案：
一种基于四元数的快速求解Stewart并联机构的运动学正解方法，所述的Stewart并联机构包括下平台、上平台及连接上、下平台的若干并联的伸缩杆，该方法包括如下步骤：
（1）、用四元数表示刚体转动：
任意矢量x能按与单位矢量n平行和垂直的方向分解为和的形式：
x＝(x·n)n+(n×x)×n
矢量x绕轴n转动ω角度后为：
R(ω,n)＝(x·n)n+(n×x)sinω+[(n×x)×n]cosω
用ε＝(ε，ε0)＝(ε1ε2ε3ε0)表示一个单位四元数，令ε是一个单位四元数(nsinω/2,cosω/2,)，n是一个单位矢量，R为转动矢量，R3为在三维坐标系中转动矢量集合，对于任意x∈R3，乘积而且与刚体转动矢量R(ω,n)是等价的，通过以下计算证明该定理
ϵx=(xcosω2+(n×x)sinω2,(-n·x)sinω2)ϵxϵ~=((x·n)n+(n×x)sinω+[(n×x)×n]cosω,0);]]>
（2）、建立正向运动学方程：
Stewart并联机构的运动学正解方程为
式中x是动平台的位姿坐标，Qi是描述并联机构结构的8×8常对称矩阵，Ci是与姿态坐标无关的数。
（3）、构造迭代序列：
采用下述迭代序列
xk+1=12xk+ΔxkJkΔxk=C(k=0,1,2...)]]>
xk为迭代计算k次后的位姿坐标，Δxk为迭代时第k+1次与第k次的位姿之差，Jk为第k次计算中的雅可比矩阵，C为由Ci构成并扩展后的八维矢量。
本发明的有益效果：利用四元数运算处理既有移动又有转动的机构运动学问题时，能使运动学方程转化成仅有二次的多项式方程组，大大地降低了求解的难度。特别地，应用牛顿法数值求解时，迭代函数能够进一步巧妙地简化，有效地降低了计算耗时。该方法具有计算精度高、收敛速度快的优点。
附图说明
图1是本发明中一个矢量的转动示意图。
图2是本发明中实施例中的Stewart机构简图。
图3是本发明中Stewart机构动平台位置的绝对偏差测试结果图。
图4是本发明中Stewart机构动平台姿态的绝对偏差测试结果图。
图5是本发明中迭代次数对计算误差的影响测试结果图。
具体实施方式
下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
1.请参阅图1所示，用四元数表示刚体转动
任意矢量x能按与n平行和垂直的方向分解为和的形式：
x＝(x·n)n+(n×x)×n   (1)由于矢量n，n×x和(n×x)×n是相互正交的，那么矢量x的转动为：
R(x)＝(x·n)n+R[(n×x)×n]＝(x·n)n+(n×x)sinω+[(n×x)×n]cosω   (2)使用矢量径向和横向分解的方法(1)，上述方程也可写成广为人知的Euler-Rodrigues形式：
R(x)＝x+(n×x)sinω+[n×(n×x)](1-cosω)   (3)
考虑集合R3×R，其元素时由一个矢量q和一个标量q0组成的对{q,q0}，表示为：ε＝(q,q0)＝(q1q2q3q0)或q=q1i+q2j+q3k+q0，其中i2＝j2＝k2＝-1，且ij＝-ji＝k，jk＝-kj＝i，ki＝-ik＝j。那么，对于任意q＝{qq0}和p＝{pp0}，其合成运算
(q,p)→qp＝{q0p+p0q+q×p,q0p0-q·p}   (4)
是q,p的双线性型，显然，该运算是可结合的，由于包含叉积而不可交换，这使得R3×R是一个结合代数。具有这种结构的集合R3×R命名为Q，其元素称为四元数。qp称为q右乘p，或p左乘q。一个四元数q＝(q,q0)的组成部分q和q0分别看成是虚数部分和实数部分
与复数（z=x+i y虚部是一个实数）不同的是，q的虚部则是R3上的矢量。
四元数q＝(-q,q0)称为是q＝(q,q0)的共轭，用表示。映射是矢量空间Q的自同构，由于则是代数结构的反自同构。由于

是两个正数的和，那么定义一个四元数的范数为标量是合理的。显然，当且仅当||q||=0时，q=0。而且，对任意p,q∈Q，
||pq||2=(pq)(pq)~=(pq)(q~p~)=p(qq~)p~=(pp~)||q||2=||p||2||q||2]]>
这意味着范数q→||q||:Q→R+的存在使Q为一个赋范代数。
对于一个四元数q，如果||q||＝1，那么称该四元数为单位四元数。令表示所有单位四元数的集合：

显然，它不是空集，因为而且是ε的逆元。对任意的其乘积总之，四元数的乘积运算赋予一个群的结构。必须说明的是，群是不可交换的。
为了说明单位四元数与R3上的转动群SO(3)的联系，用ε＝(ε，ε0)＝(ε1ε2ε3ε0)表示一个单位四元数。令ε是一个单位四元数(nsinω/2,cosω/2,)，n是一个单位矢量。对于任意x∈R3，乘积而且与转动R(ω,n)是等价的。通过以下计算即可证明该定理
ϵx=(xcosω2+(n×x)sinω2,(-n·x)sinω2)ϵxϵ~=((x·n)n+(n×x)sinω+[(n×x)×n]cosω,0)---(5)]]>5)
方程(5)和方程(2)是一致的。值得注意的是，ε和–ε只与半角ω/2的形式有关，描述的则是相同的转动，计算结果表明，用单位四元数对转动建模是可行的。
2.建立正向运动学方程
对于一般的六自由度并联机构，假定两个参考系分别与动平台和基座固连，称为动坐标系和静坐标系。每条伸缩杆与动平台的连接点在动坐标系中的矢径记为：a1，…，a6，与基座的连接点在静坐标系中的矢径记为：b1，…，b6。动坐标系相对静坐标系的姿态用选择矩阵R表示，其原点在静坐标系中矢径记为：P。在三维矢量空间中建立方程：
Liei＝P+Rai-bi(i＝1,…,6)   (6) 式(6)中，Li为第i个伸缩杆的长度，ei为单位向量，表示第i个驱动关节在静坐标系中的方向。考虑式(5)，将方程(6)扩展到空间表达如下：
Li(ei,0)=(P,0)+ϵ(ai,0)ϵ~-(bi,0)(i=1,...,6)---(7)]]>
将式(7)右乘ε，得
Li(ei,0)ε＝(P,0)ε+ε(ai，0)-(bi，0)ε(i＝1,…,6)
简写为
Lifi＝h+ε(ai，0)-(bi，0)ε(i＝1,…,6)   (8)式(8)中，h＝(P，0)ε＝(ε0P-ε×P，-ε·P)∈Q。式(8)自乘其共轭四元数得：

定义Ai＝ai-bi，Bi＝ai+bi，其坐标阵形式：(AixAiyAiz)T和(BixBiyBiz)T，h表示为：(hT,h0)＝(h1h2h3h0),h∈R3，L＝(L1L2…L6)T，化简并整理式(9)得到六个方程：
||h||2+2(h×ϵ)·Bi+2Ai·(ϵ0h-h0ϵ)+12(Axi2-Ayi2-Bxi2+Byi2)(ϵ12-ϵ22)+12(Azi2-Bzi2)(2ϵ32-ϵ12-ϵ22)+2ϵ0ϵ·(Bi×Ai)+2ϵ1ϵ2(AxiAyi-BxiByi)+2ϵ2ϵ2(AyiAzi-ByiBzi)+2ϵ1ϵ3(AxiAzi-BxiBzi)+12(Bi2-Ai2)(ϵ32-ϵ02)+12(Ai2+Bi2)-Li2=0(i=1,...,6)---(10)]]>
上述方程左边可整理成：
fi(x)=12xTQix-Ci(i=1,...,6)---(11)]]>其中是动平台的位姿坐标为
x＝(ε1ε2ε3ε0h1h2h3h0)T
同时
Ci=Li2-12(Ai2+Bi2)]]>
Qi=Ai2-2Aix2-Bi2+2Biy2-2AixAiy+2BixBiy-2AixAiz+2BixBiz2AizBiy-2AiyBiz02Biz-2Biy2Aix-2AixAiy+2BixBiyAi2-2Aiy2-Bi2+2Biz2-2AiyAiz+2BiyBiz-2AizBix+2AixBiz-2Biz02Bix2Aiy-2AixAiz+2BixBiz-2AiyAiz+2BiyBizAi2-2Aiz2-Bi2+2Biz22AiyBix-2AixBiy2Biy-2Bix02Aiz2AizBiy-2AiyBiz-2AizBix+2AixBiz2AiyBix-2AixBiy-Ai2+Bi2-2Aix-2Aiy-2Aiz00-2Biz2Biy-2Aix-20002Biz0-2Bix-2Aiy0-200-2Biy2Bix0-2Aiz00-202Aix2Aiy2Aiz0000-2]]>
Qi是只由并联平台结构参数决定的常对称矩阵。
另外，根据单位四元数的性质有方程：
f7(x)=ϵ12+ϵ22+ϵ32+ϵ02-1=0f8(x)=ϵ1h1+ϵ2h2+ϵ3h3+ϵ0h0=0---(12)]]>
f7(x)、f8(x)可写成表达式(11)的形式：
f7(x)=12xT2I4×40004×4x-1f8(x)=12xT04×4II04×4x---(13)]]>
方程(11)、(12)构成八个二次非线性代数方程组，通过对该方程组的计算可获得一般并联机构的运动学正解。
3.构造迭代序列
对于非线性代数方程组(11)、(12)中的任一方程，若a,b∈R8，则：
fi(a)-fi(b)bTQi(a-b)+12(a-b)TQi(a-b)(i=1,...,8)---(14)]]>
设x*∈R8为fi(x)＝xTQix/2-Ci(i＝1,…,8)的一个实数解，xk∈R8为x*的近似。令式(14)中a＝x*，b＝xk，Δx＝x*-x，在xk处以超切平面代替二次曲面，即略去二阶微量(ΔxTQiΔx)/2，得到：
-fi(xk)≈xkTQi(x*-xk)(i＝1,…,8)   (15)那么迭代序列可以写成
xk+1=φ(xk)=xk-Jk-1F(xk)(k=0,1,2...)---(16)]]>式(16)中，Jk＝(xTQ1xTQ2…xTQ8)T，F(xk)＝(f1(xk)f2(xk)…f8(xk))T，式(16)是牛顿-拉夫逊法应用于非线性方程组时的一般形式。但针对由四元数导出的二次方程组，迭代函数能够被进一步化简，考虑到多项式函数F(xk)和雅可比矩阵Jk有如下关系：
(xk)=12Jkxk-C,C=(C1C2...C610)T]]>
将上式代入式(16)中消去迭代函数可简化为如此中每次迭代计算中省略计算F(xk)，减少了计算时耗，有利于实时性应用。为了避免计算矩阵的逆在实际计算时以数值计算线性方程组代替，采取下列迭代序列：
xk+1=12xk+ΔxkJkΔxk=C(k=0,1,2...)---(17)]]>
4.收敛性：奇异性与迭代初值
当雅可比矩阵Jk非奇异并且迭代初值接近真实解时，迭代序列(17)具有二次收敛特性，能快速计算出局部真实解。若雅可比矩阵奇异或者接近奇异，迭代结果将跃出局部解的邻域，收敛于另一个解或者发散。为了避免这种情况的发生，我们再对(17)进行细微修改。我们已经知道如果将(17)中雅可比矩阵Jk替换成常矩阵J0，那么迭代序列将线性收敛，此时，每次迭代计算中将不会出现雅可比矩阵奇异。这种迭代法可以称作简化牛顿法。
现在我们将迭代序列(17)与简化牛顿法结合，如果预测出雅可比矩阵Jk接近奇异，那么在这次迭代计算中使用简化牛顿法：
xk+1=12xk+ΔXkJk-1Δxk=C]]>
我们可以采用很多方式判定雅可比矩阵是否接近奇异，例如较大的矩阵条件数、较小的行列式以及Δxk较大的改变。由于计算Δxk的变化量相比另外两种方法消耗更少的计算时间，这里，我们将通过这种方式判定雅可比矩阵临近奇异。而在下一次循环中，我们将返回使用式(17)。这样我们以少量的时间消耗避免了奇异性的发生。
如何选取初值也至关重要。根据局部收敛性原理知，存在x*的邻域Sδ＝{x∈R8|x*-x＜δ}，对于迭代序列(17)收敛于x*，且至少是平方收敛。该方法具有收敛速度快，能够自我校正，不会放大舍入误差等优点，难点是需确保初始值x0∈Sδ。
另一方面，从式(10)、(11)可知杆长L是姿态坐标x的连续函数，即存在实际杆长L*的邻域Tτ＝{L∈R6|L*-L＜τ}，当L∈Tτ时，x∈Sδ，故可通过控制杆长的变化量，使位姿的变化在允许的范围内。考虑式(17)收敛性，对Stewart机构实时控制时，其动平台按给定要求连续运动，伸缩杆长度L是时间t的函数。当杆长从L(t0)＝L0连续改变至L(t)＝Lt时，将时间段Δt＝t-t0分割为若干控制周期，每个周期中采用迭代式(17)计算动平台位姿，其初值选取上一周期内计算获得的位姿坐标。若将运动学正解应用到工作空间分析，亦将L1至L2的一次变化逐步分割为多次变化，对生成的一系列杆长分别采用迭代式(17)计算姿态坐标，上一次计算获得的姿态坐标作为下一次迭代计算的初值，以确保收敛性。
5.应用示范
导出的迭代序列适用于任意连接形式的Stewart机构，不失一般性，以常用构型为例验证算法的正确性，六个虎克铰、球铰平行布置，按圆周对称分布。
图2描述动平台、静平台与六个伸缩杆连接位置的矢量分别在动坐标系和静坐标系中的坐标表示如下：
a2i-1=cos(2π3(i-1)+π12)sin(2π3(i-1)+π12)0,a2i=cos(2π3i-π12)sin(2π3i-π12)0(i=1,2,3)]]>
b2i-1=cos(2π3(i-1)+π6)sin(2π3(i-1)+π6)0,b2i=2·cos(2π3i-π6)sin(2π3i-π6)0(i=1,2,3)]]>
在动态仿真时，Stewart机构的初始装配模型已知，可作为算法(17)的初值。在采样周期T内，需利用算法找到新解，该解作为下一周期的初值。采用动平台位置和姿态关于时间的函数描述Stewart机构的运动。根据式(5)和(7)，动平台位姿由矢量P和四元数ε决定，定义
P(t)=(0.1sin(π2t)0.12sin(π2t)1+0.15sint)Tϵ(t)=(nsinω2cosω2)(0≤t≤2)---(18)]]>
式(18)中，
n=(sinγ·cosαsinγ·sinαcosγ),γ=π12sin(2πt+π2)+5π12,α=2πsin(2πt),ω=π4sin(2πt),]]>
那么动平台的初始位姿为：P(t＝0)＝(001)T，ε(t＝0)＝(0001)。
动态仿真时，采样周期设为1ms，相当于1000Hz的采样频率，计算机的机器精度设为16，进行了1000ms的仿真计算。显然，迭代序列(17)的计算次数不同，动平台位置和姿态的计算精度也不同。
图3和图4中误差曲线是迭代次数为10次时计算结果与动平台真实位姿的绝对偏差值。由于仿真循环的次数较多，这里将计算误差定义为最后100ms内的最大绝对偏差值。可以看出，迭代次数为10次时，计算误差为3.11×10-15，此时已达到计算机的最高计算精度。图5显示了迭代计算次数和计算误差间的变化关系，若每个循环内进行2次迭代计算，则计算误差为1.46×10-7，当迭代次数大于4时，计算误差下降到10-15量级，在实时应用时我们可以根据实际精度要求控制迭代计算次数。