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

摘要

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

用ε＝(ε，ε₀)＝(ε₁ε₂ε₃ε₀)表示一个单位四元数，令ε是一个单位四元数 (nsinω/2,cosω/2,)，n是一个单位矢量，R为转动矢量，R³为在三维坐标系中转 动矢量集合，对于任意x∈R³，乘积而且与刚体转动矢 量R(ω,n)是等价的，通过以下计算证明该定理

$\left(\begin{array}{c}\mathrm{ϵx}=(x\cos \frac{\omega }{2}+(n\times x)\sin \frac{\omega }{2},(-n·x)\sin \frac{\omega }{2})\\ \mathrm{ϵx}\widetilde{ϵ}=((x·n)n+(n\times x)\sin \omega +[(n\times x)\times n]\cos \omega ,0)\end{array},;\right)$

（2）、建立正向运动学方程：

Stewart并联机构的运动学正解方程为

式中x是动平台的位姿坐标，Q_i是描述并联机构结构的8×8常对称矩阵，C_i是与姿态坐标无关的数。

（3）、构造迭代序列：

采用下述迭代序列

$\left(\begin{array}{c}{x}_{k+1}=\frac{1}{2}{x}_k+{\mathrm{\Delta x}}_k\\ J_k\Delta x_k=C\end{array}\right)(k=\mathrm{0,1,2}...)$

x_k为迭代计算k次后的位姿坐标，Δx_k为迭代时第k+1次与第k次的位姿之 差，J_k为第k次计算中的雅可比矩阵，C为由C_i构成并扩展后的八维矢量。

本发明的有益效果：利用四元数运算处理既有移动又有转动的机构运动学问 题时，能使运动学方程转化成仅有二次的多项式方程组，大大地降低了求解的难 度。特别地，应用牛顿法数值求解时，迭代函数能够进一步巧妙地简化，有效地 降低了计算耗时。该方法具有计算精度高、收敛速度快的优点。

附图说明

图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)

考虑集合R³×R，其元素时由一个矢量q和一个标量q₀组成的对{q,q₀}，表示 为：ε＝(q,q₀)＝(q₁q₂q₃q₀)或q=q₁i+q₂j+q₃k+q₀，其中i²＝j²＝k²＝-1，且ij＝-ji＝k， jk＝-kj＝i，ki＝-ik＝j。那么，对于任意q＝{qq₀}和p＝{pp₀}，其合成运算

(q,p)→qp＝{q₀p+p₀q+q×p,q₀p₀-q·p}   (4)

是q,p的双线性型，显然，该运算是可结合的，由于包含叉积而不可交换，这使 得R³×R是一个结合代数。具有这种结构的集合R³×R命名为Q，其元素称为四 元数。qp称为q右乘p，或p左乘q。一个四元数q＝(q,q₀)的组成部分q和q₀分 别看成是虚数部分和实数部分

与复数（z=x+i y虚部是一个实数）不同的是，q的虚部则是R³上的矢量。

四元数q＝(-q,q₀)称为是q＝(q,q₀)的共轭，用表示。映射是矢量空间 Q的自同构，由于则是代数结构的反自同构。由于

是两个正数的和，那么定义一个四元数的范数为标量是合理的。显然， 当且仅当||q||=0时，q=0。而且，对任意p,q∈Q，

${\left|\right|\mathrm{pq}\left|\right|}^2=\left(\mathrm{pq}\right)\widetilde{\left(\mathrm{pq}\right)}=\left(\mathrm{pq}\right)\left(\tilde{q}\tilde{p}\right)=p\left(q\tilde{q}\right)\tilde{p}=\left(p\tilde{p}\right){\left|\right|q\left|\right|}^2={\left|\right|p\left|\right|}^2{\left|\right|q\left|\right|}^2$

这意味着范数q→||q||:Q→R⁺的存在使Q为一个赋范代数。

对于一个四元数q，如果||q||＝1，那么称该四元数为单位四元数。令表 示所有单位四元数的集合：

显然，它不是空集，因为而且是ε的逆元。对任意的 其乘积总之，四元数的乘积运算赋予一个群的结 构。必须说明的是，群是不可交换的。

为了说明单位四元数与R³上的转动群SO(3)的联系，用 ε＝(ε，ε₀)＝(ε₁ε₂ε₃ε₀)表示一个单位四元数。令ε是一个单位四元数 (nsinω/2,cosω/2,)，n是一个单位矢量。对于任意x∈R³，乘积而且 与转动R(ω,n)是等价的。通过以下计算即可证明该定理

$\left(\begin{array}{c}\mathrm{ϵx}=(x\cos \frac{\omega }{2}+(n\times x)\sin \frac{\omega }{2},(-n·x)\sin \frac{\omega }{2})\\ \mathrm{ϵx}\widetilde{ϵ}=((x·n)n+(n\times x)\sin \omega +[(n\times x)\times n]\cos \omega ,0)\end{array},-,-,-,\left(5\right)\right)$5)

方程(5)和方程(2)是一致的。值得注意的是，ε和–ε只与半角ω/2的形式有 关，描述的则是相同的转动，计算结果表明，用单位四元数对转动建模是可行的。

2.建立正向运动学方程

对于一般的六自由度并联机构，假定两个参考系分别与动平台和基座固连， 称为动坐标系和静坐标系。每条伸缩杆与动平台的连接点在动坐标系中的矢径记 为：a₁，…，a₆，与基座的连接点在静坐标系中的矢径记为：b₁，…，b₆。动坐 标系相对静坐标系的姿态用选择矩阵R表示，其原点在静坐标系中矢径记为：P。 在三维矢量空间中建立方程：

L_ie_i＝P+Ra_i-b_i(i＝1,…,6)   (6) 式(6)中，L_i为第i个伸缩杆的长度，e_i为单位向量，表示第i个驱动关节在静坐 标系中的方向。考虑式(5)，将方程(6)扩展到空间表达如下：

$L_i(e_i,0)=(P,0)+ϵ(a_i,0)\widetilde{ϵ}-(b_i,0)(i=1,...,6)---\left(7\right)$

将式(7)右乘ε，得

L_i(e_i,0)ε＝(P,0)ε+ε(a_i，0)-(b_i，0)ε(i＝1,…,6)

简写为

L_if_i＝h+ε(a_i，0)-(b_i，0)ε(i＝1,…,6)   (8) 式(8)中，h＝(P，0)ε＝(ε₀P-ε×P，-ε·P)∈Q。式(8)自乘其共轭四 元数得：

定义A_i＝a_i-b_i，B_i＝a_i+b_i，其坐标阵形式：(A_ixA_iyA_iz)^T和(B_ixB_iyB_iz)^T，h表示为： (h^T,h₀)＝(h₁h₂h₃h₀),h∈R³，L＝(L₁L₂…L₆)^T，化简并整理式(9)得到六个方程：

$\left(\begin{array}{c}{\left|\right|h\left|\right|}^2+2(h\times ϵ)·B_i+2A_i·({ϵ}_{0}h-h_0ϵ)+\frac{1}{2}(A_{\mathrm{xi}}^2-A_{\mathrm{yi}}^2-B_{\mathrm{xi}}^2+B_{\mathrm{yi}}^2)({ϵ}_1^2-{ϵ}_2^2)\\ +\frac{1}{2}(A_{\mathrm{zi}}^2-B_{\mathrm{zi}}^2)({2ϵ}_3^2-{ϵ}_1^2-{ϵ}_2^2)+{2ϵ}_0ϵ·(B_i\times A_i)+{2ϵ}_1{ϵ}_2(A_{\mathrm{xi}}{A}_{\mathrm{yi}}-B_{\mathrm{xi}}{B}_{\mathrm{yi}})\\ +{2ϵ}_2{ϵ}_2(A_{\mathrm{yi}}{A}_{\mathrm{zi}}-B_{\mathrm{yi}}{B}_{\mathrm{zi}})+{2ϵ}_1{ϵ}_3(A_{\mathrm{xi}}{A}_{\mathrm{zi}}-B_{\mathrm{xi}}{B}_{\mathrm{zi}})+\frac{1}{2}(B_i^2-A_i^2)({ϵ}_3^2-{ϵ}_0^2)\\ +\frac{1}{2}(A_i^2+B_i^2)-L_i^2=0(i=1,...,6)\end{array}\right)---\left(10\right)$

上述方程左边可整理成：

$f_i\left(x\right)=\frac{1}{2}{x}^T{Q}_{i}x-C_i(i=1,...,6)---\left(11\right)$其中是动平台的位姿坐标为

x＝(ε₁ε₂ε₃ε₀h₁h₂h₃h₀)^T

同时

$C_i=L_i^2-\frac{1}{2}(A_i^2+B_i^2)$

$Q_i=\left(\begin{array}{cccccccc}{A}_i^2-{2A}_{\mathrm{ix}}^2-B_i^2+{2B}_{\mathrm{iy}}^2& -{2A}_{\mathrm{ix}}{A}_{\mathrm{iy}}+{2B}_{\mathrm{ix}}{B}_{\mathrm{iy}}& -2A_{\mathrm{ix}}{A}_{\mathrm{iz}}+{2B}_{\mathrm{ix}}{B}_{\mathrm{iz}}& {2A}_{\mathrm{iz}}{B}_{\mathrm{iy}}-{2A}_{\mathrm{iy}}{B}_{\mathrm{iz}}& 0& {2B}_{\mathrm{iz}}& {-2B}_{\mathrm{iy}}& {2A}_{\mathrm{ix}}\\ -2A_{\mathrm{ix}}{A}_{\mathrm{iy}}+{2B}_{\mathrm{ix}}{B}_{\mathrm{iy}}& A_i^2-2A_{\mathrm{iy}}^2-B_i^2+{2B}_{\mathrm{iz}}^2& -{2A}_{\mathrm{iy}}{A}_{\mathrm{iz}}+{2B}_{\mathrm{iy}}{B}_{\mathrm{iz}}& -{2A}_{\mathrm{iz}}{B}_{\mathrm{ix}}+{2A}_{\mathrm{ix}}{B}_{\mathrm{iz}}& {-2B}_{\mathrm{iz}}& 0& {2B}_{\mathrm{ix}}& {2A}_{\mathrm{iy}}\\ -{2A}_{\mathrm{ix}}{A}_{\mathrm{iz}}+{2B}_{\mathrm{ix}}{B}_{\mathrm{iz}}& -{2A}_{\mathrm{iy}}{A}_{\mathrm{iz}}+{2B}_{\mathrm{iy}}{B}_{\mathrm{iz}}& A_i^2-2A_{\mathrm{iz}}^2-B_i^2+{2B}_{\mathrm{iz}}^2& {2A}_{\mathrm{iy}}{B}_{\mathrm{ix}}-{2A}_{\mathrm{ix}}{B}_{\mathrm{iy}}& 2B_{\mathrm{iy}}& {-2B}_{\mathrm{ix}}& 0& {2A}_{\mathrm{iz}}\\ {2A}_{\mathrm{iz}}{B}_{\mathrm{iy}}-{2A}_{\mathrm{iy}}{B}_{\mathrm{iz}}& -{2A}_{\mathrm{iz}}{B}_{\mathrm{ix}}+{2A}_{\mathrm{ix}}{B}_{\mathrm{iz}}& {2A}_{\mathrm{iy}}{B}_{\mathrm{ix}}-{2A}_{\mathrm{ix}}{B}_{\mathrm{iy}}& -A_i^2+B_i^2& {-2A}_{\mathrm{ix}}& -2A_{\mathrm{iy}}& -2A_{\mathrm{iz}}& 0\\ 0& -{2B}_{\mathrm{iz}}& {2B}_{\mathrm{iy}}& {-2A}_{\mathrm{ix}}& -2& 0& 0& 0\\ {2B}_{\mathrm{iz}}& 0& {-2B}_{\mathrm{ix}}& -{2A}_{\mathrm{iy}}& 0& -2& 0& 0\\ -{2B}_{\mathrm{iy}}& {2B}_{\mathrm{ix}}& 0& {-2A}_{\mathrm{iz}}& 0& 0& -2& 0\\ 2A_{\mathrm{ix}}& {2A}_{\mathrm{iy}}& {2A}_{\mathrm{iz}}& 0& 0& 0& 0& -2\end{array}\right)$

Q_i是只由并联平台结构参数决定的常对称矩阵。

另外，根据单位四元数的性质有方程：

$\left(\begin{array}{c}{f}_7\left(x\right)={ϵ}_1^2+{ϵ}_2^2+{ϵ}_3^2+{ϵ}_0^2-1=0\\ f_8\left(x\right)={ϵ}_1{h}_1+{ϵ}_2{h}_2+{ϵ}_3{h}_3+{ϵ}_0{h}_0=0\end{array}\right)---\left(12\right)$

f₇(x)、f₈(x)可写成表达式(11)的形式：

$\left(\begin{array}{c}{f}_7\left(x\right)=\frac{1}{2}{x}^{T\left(\begin{array}{cc}{2I}_{4\times 4}& 0\\ 0& 0_{4\times 4}\end{array}\right)x-1}\\ f_8\left(x\right)=\frac{1}{2}{x}^T\left(\begin{array}{cc}{0}_{4\times 4}& I\\ I& 0_{4\times 4}\end{array}\right)x\end{array}\right)---\left(13\right)$

方程(11)、(12)构成八个二次非线性代数方程组，通过对该方程组的计算可 获得一般并联机构的运动学正解。

3.构造迭代序列

对于非线性代数方程组(11)、(12)中的任一方程，若a,b∈R⁸，则：

$f_i\left(a\right)-f_i\left(b\right)b^T{Q}_i(a-b)+\frac{1}{2}{(a-b)}^T{Q}_i(a-b)(i=1,...,8)---\left(14\right)$

设x^*∈R⁸为f_i(x)＝x^TQ_ix/2-C_i(i＝1,…,8)的一个实数解，x_k∈R⁸为x^*的近 似。令式(14)中a＝x^*，b＝x_k，Δx＝x^*-x，在x_k处以超切平面代替二次曲面，即 略去二阶微量(Δx^TQ_iΔx)/2，得到：

-f_i(x_k)≈x_k^TQ_i(x^*-x_k)(i＝1,…,8)   (15) 那么迭代序列可以写成

$x_{k+1}=\phi \left(x_k\right)=x_k-J_k^{-1}F\left(x_k\right)(k=\mathrm{0,1,2}...)---\left(16\right)$式(16)中，J_k＝(x^TQ₁x^TQ₂…x^TQ₈)^T，F(x_k)＝(f₁(x_k)f₂(x_k)…f₈(x_k))^T，式(16)是 牛顿-拉夫逊法应用于非线性方程组时的一般形式。但针对由四元数导出的二次 方程组，迭代函数能够被进一步化简，考虑到多项式函数F(x_k)和雅可比矩阵J_k有如下关系：

$\left(x_k\right)=\frac{1}{2}{J}_k{x}_k-C,C={(C_1{C}_2...C_{6}10)}^T$

将上式代入式(16)中消去迭代函数可简化为如此 中每次迭代计算中省略计算F(x_k)，减少了计算时耗，有利于实时性应用。为了 避免计算矩阵的逆在实际计算时以数值计算线性方程组代替，采取下列迭 代序列：

$\left(\begin{array}{c}{x}_{k+1}=\frac{1}{2}{x}_k+\Delta x_k\\ J_k{\mathrm{\Delta x}}_k=C\end{array}\right)(k=\mathrm{0,1,2}...)---\left(17\right)$

4.收敛性：奇异性与迭代初值

当雅可比矩阵J_k非奇异并且迭代初值接近真实解时，迭代序列(17)具有二次 收敛特性，能快速计算出局部真实解。若雅可比矩阵奇异或者接近奇异，迭代结 果将跃出局部解的邻域，收敛于另一个解或者发散。为了避免这种情况的发生， 我们再对(17)进行细微修改。我们已经知道如果将(17)中雅可比矩阵J_k替换成常 矩阵J₀，那么迭代序列将线性收敛，此时，每次迭代计算中将不会出现雅可比矩 阵奇异。这种迭代法可以称作简化牛顿法。

现在我们将迭代序列(17)与简化牛顿法结合，如果预测出雅可比矩阵J_k接近 奇异，那么在这次迭代计算中使用简化牛顿法：

$\left(\begin{array}{c}{x}_{k+1}=\frac{1}{2}{x}_k+\Delta X_k\\ J_{k-1}{\mathrm{\Delta x}}_k=C\end{array}\right)$

我们可以采用很多方式判定雅可比矩阵是否接近奇异，例如较大的矩阵条件 数、较小的行列式以及Δx_k较大的改变。由于计算Δx_k的变化量相比另外两种方 法消耗更少的计算时间，这里，我们将通过这种方式判定雅可比矩阵临近奇异。 而在下一次循环中，我们将返回使用式(17)。这样我们以少量的时间消耗避免了 奇异性的发生。

如何选取初值也至关重要。根据局部收敛性原理知，存在x^*的邻域 S_δ＝{x∈R⁸|x^*-x＜δ}，对于迭代序列(17)收敛于x^*，且至少是平方 收敛。该方法具有收敛速度快，能够自我校正，不会放大舍入误差等优点，难点 是需确保初始值x₀∈S_δ。

另一方面，从式(10)、(11)可知杆长L是姿态坐标x的连续函数，即存在实 际杆长L*的邻域T_τ＝{L∈R⁶|L^*-L＜τ}，当L∈T_τ时，x∈S_δ，故可通过控制杆长 的变化量，使位姿的变化在允许的范围内。考虑式(17)收敛性，对Stewart机构 实时控制时，其动平台按给定要求连续运动，伸缩杆长度L是时间t的函数。当 杆长从L(t₀)＝L₀连续改变至L(t)＝L_t时，将时间段Δt＝t-t₀分割为若干控制周期， 每个周期中采用迭代式(17)计算动平台位姿，其初值选取上一周期内计算获得的 位姿坐标。若将运动学正解应用到工作空间分析，亦将L₁至L₂的一次变化逐步分 割为多次变化，对生成的一系列杆长分别采用迭代式(17)计算姿态坐标，上一次 计算获得的姿态坐标作为下一次迭代计算的初值，以确保收敛性。

5.应用示范

导出的迭代序列适用于任意连接形式的Stewart机构，不失一般性，以常用 构型为例验证算法的正确性，六个虎克铰、球铰平行布置，按圆周对称分布。

图2描述动平台、静平台与六个伸缩杆连接位置的矢量分别在动坐标系和静 坐标系中的坐标表示如下：

$a_{2i-1}=\left(\begin{array}{c}\cos (\frac{2\pi }{3}(i-1)+\frac{\pi }{12})\\ \sin (\frac{2\pi }{3}(i-1)+\frac{\pi }{12})\\ 0\end{array}\right),a_{2i}=\left(\begin{array}{c}\cos (\frac{2\pi }{3}i-\frac{\pi }{12})\\ \sin (\frac{2\pi }{3}i-\frac{\pi }{12})\\ 0\end{array}\right)(i=\mathrm{1,2,3})$

$b_{2i-1}=\left(\begin{array}{c}\cos (\frac{2\pi }{3}(i-1)+\frac{\pi }{6})\\ \sin (\frac{2\pi }{3}(i-1)+\frac{\pi }{6})\\ 0\end{array}\right),b_{2i}=2·\left(\begin{array}{c}\cos (\frac{2\pi }{3}i-\frac{\pi }{6})\\ \sin (\frac{2\pi }{3}i-\frac{\pi }{6})\\ 0\end{array}\right)(i=\mathrm{1,2,3})$

在动态仿真时，Stewart机构的初始装配模型已知，可作为算法(17)的初值。 在采样周期T内，需利用算法找到新解，该解作为下一周期的初值。采用动平台 位置和姿态关于时间的函数描述Stewart机构的运动。根据式(5)和(7)，动平台位 姿由矢量P和四元数ε决定，定义

$\left(\begin{array}{c}P\left(t\right)={(0.1\sin \left(\frac{\pi }{2}t\right)0.12\sin \left(\frac{\pi }{2}t\right)1+0.15\sin t)}^T\\ ϵ\left(t\right)=\left(n\sin \frac{\omega }{2}\cos \frac{\omega }{2}\right)\end{array}\right)(0\le t\le 2)---\left(18\right)$

式(18)中，

$n=(\sin \gamma ·\cos \alpha \sin \gamma ·\sin \alpha \cos \gamma ),\gamma =\frac{\pi }{12}\sin (2\mathrm{\pi t}+\frac{\pi }{2})+\frac{5\pi }{12},\alpha =2\pi \sin \left(2\mathrm{\pi t}\right),\omega =\frac{\pi }{4}\sin \left(2\mathrm{\pi t}\right),$

那么动平台的初始位姿为：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量级，在实时应用时我们可以根据实际精度要求控 制迭代计算次数。