CN108334114A - 基于运动学正解的三自由度并联机构的姿态角控制方法 - Google Patents

Abstract

本发明实施例提供的基于运动学正解的三自由度并联机构的姿态角控制方法，所述并联机构包括三组作动器、三组支撑杆和动平台，控制方法包括：以动平台上铰接点所在圆的中心处建立体坐标系O_b‑X_bY_bZ_b，并在大地上建立固定的参考坐标系O_p‑X_pY_pZ_p，并联机构处于工作初始位置时体坐标系与参考坐标系重合；应用体坐标系相对与参考坐标系的广义坐标来描述所述动平台的姿态，所述广义坐标包括三个欧拉角，三个所述欧拉角为横摇角、纵摇角和偏航角；获取体坐标系相对与参考坐标系的旋转矩阵；根据所述作动器或所述支撑杆的长度，依据旋转矩阵与运动学关系，应用运动学正解算法，获取所述动平台的姿态角。

Description

基于运动学正解的三自由度并联机构的姿态角控制方法

技术领域

本发明涉及三自由度摇摆试验台的领域，具体而言，涉及一种基于运动学正解的三自由度并联机构的姿态角控制方法。

背景技术

多自由度摇摆试验台包括并联机构和串联机构，并联机构具有刚度大、承载能力大、积累误差大、动态特性好、结构紧凑等特点，近年来广泛应用于模拟战机、机床、机器人等方面。并联机构各零件结构复杂，运动关系复杂，导致并联机构的姿态控制困难，难以满足预期的运动需求。

发明内容

有鉴于此，本发明实施例的目的在于提供一种基于运动学正解的三自由度并联机构的姿态角控制方法，以改善现有技术中并联机构的姿态控制实时性与精度低的问题。

本发明较佳实施例提供了：

基于运动学正解的三自由度并联机构的姿态角控制方法，所述并联机构包括三组作动器、三组支撑杆和动平台，三组所述作动器的一端分别与所述动平台铰接，三组所述作动器的另一端分别与地基或者所述支撑杆铰接；三组所述支撑杆的一端分别与地基铰接，三组所述作动器的另一端分别与所述动平台或者所述作动器铰接；

控制方法包括：

以动平台上铰接点所在圆的中心处建立体坐标系O_b-X_bY_bZ_b，并在大地上建立固定的参考坐标系O_p-X_pY_pZ_p，并联机构处于工作初始位置时体坐标系与参考坐标系重合；

应用体坐标系相对与参考坐标系的广义坐标来描述所述动平台的姿态，所述广义坐标包括三个欧拉角，三个所述欧拉角为横摇角、纵摇角和偏航角；获取体坐标系相对与参考坐标系的旋转矩阵；

根据所述作动器或所述支撑杆的长度，依据旋转矩阵与运动学关系，应用运动学正解算法，获取所述动平台的姿态角。

进一步的，所述并联机构处于工作初始位置时：

所述作动器、所述支撑杆与所述动平台的上铰接点在同一平面内，且位于一个圆上；

三个所述支撑杆的长度相同，且与所述地基的下铰接点位于一个圆上；所述下铰接点所在圆的圆心是动平台中心在地基上的垂直投影；

三个所述作动器之一与所述地基平行，其余所述作动器与所述地基垂直。

进一步的，采用ZYX型欧拉角来描述所述并联机构的姿态；

体坐标系相对于参考坐标系的广义坐标表示为：q＝[q₁,q₂,q₃,q₄,q₅, q₆]T；q₁、q₂和q₃依次表示为横摇角、纵摇角和偏航角，q₄、q₅和q₆为体坐标系原点O1在参考坐标系中的坐标分量。

进一步的，获取体坐标系相对与参考坐标系的旋转矩阵，具体包括：

首先，绕OZ轴旋转偏航角q₃，相应地OX转至OX′，OY转至OY′，此旋转对应的变换矩阵为R₃，其中R₃表示为：

其次，绕OY′轴旋转纵摇角q2，相应地OZ转至OZ′，OX′转至OX₁，对应的变换矩阵为R2，其中R2表示为：

最后，绕OX₁轴旋转横摇角q1，OY′转至OY1，OZ′转至OZ1，获得体坐标系O-X1Y1Z1，相应的变换矩阵为R1，其中R1表示为：

式中，sq_i表示为sin(qi)，i＝1,2,3；cq_i表示为cos(qi)，i＝1,2,3；

则体坐标系相对于参考坐标系的旋转矩阵为：

进一步的，将体坐标系相对参考坐标系的位移矢量表示为[q₄ q₅ q₆]^T，且欧拉角表示为[q₁ q₂ q₃]^T；将所述动平台的上铰接点在体坐标系中的坐标表示为a_i＝[a_xi a_yi a_zi]^T，并将其下铰接点在参考坐标系中的坐标表示为b_i＝[b_xi b_yi b_zi]^T，其中i＝1～6，则上铰接点在参考坐标系中的矢量坐标表示为：

g_i＝Ra_i+c

式中，g_i表示为上铰接点在参考坐标系中的矢量坐标，g_i＝[g_xi g_yi g_zi]^T， i＝1,2,…,6；

c表示为体坐标系原点在参考坐标系中的矢量，c＝[q₄ q₅ q₆]^T；

根据动平台的姿态角和平动位移，得到所述作动器的矢量表示为：

l_i＝Ra_i-b_i+c(i＝1,2,3)

得到支撑杆的矢量表示为：

l_i＝Ra_i-b_i+c(i＝4,5,6)。

进一步的，采用基于Stewart机构的运动学正解，应用运动学正解算法，获取所述动平台的姿态角，具体包括：

已知三组所述作动器的长度，且已知或者测量三组所述支撑杆的长度，求解非线性方程组：

||Ra_i-b_i+c||＝||g_i-b_i||＝||l_i||(i＝1,2…,6) (1)

应用牛顿-泰勒展开发求解式(1)，表示为:

式中,l_i0表示为作动器或支撑杆上铰和下铰间的初始长度(m)；Δl_i表示为作动器或支撑杆的伸缩量(m)；g_ki表示为上铰接点在参考坐标系中的坐标分量，k＝1,2,3；b_ki表示为下铰接点在参考坐标系中的坐标分量，k＝1,2,3；

令

解式(2)，得到所述动平台的当前姿态q，

将f_i(q)在初始位置q₀附近进行泰勒级数展开，并取其线性部分得

进一步令Δq＝q-q₀和Δq_j＝q_j-q_0j(j＝1,2…,6)，则式(3)表示为：

将方程组(4)看成以Δq_i(i＝1,2,…,6)未知数的线性方程组，其系数矩阵 J₁为：

若J₁非奇异，则方程组(5)有唯一解Δq，

若Δq满足求解精度要求，即||Δq||≤ε(ε为求解精度)，则q＝q₀+Δq为所求得的正解；否则令q₀＝q，并依据新q₀重新计算作动器或支撑杆上铰和下铰间的长度l_i0(i＝1,2,…,6)和系数矩阵J₁，然后依据式(4)再次求解Δq，直到Δq满足求解精度为止。

此外，采用基于支撑杆约束的运动学正解，应用运动学正解算法，获取所述动平台的姿态角，具体包括：

基于运动学关系，获得作动器或者支撑杆的伸缩速度表示为：

式中，表示为作动器和支撑杆的伸缩速度(m/s)；l_ni表示为作动器和支撑杆的单位矢量；表示为上铰接点的速度；表示为动平台的平动速度；ω表示为动平台在参考坐标系中的角速度；ai＝[axi ayi azi]T表示为动平台的上铰接点在体坐标系中的坐标；

依据三组所述支撑杆的长度固定，则其伸缩速度为零，因此：

将式(6)以矩阵形式表示：

式中，

D₂＝[l_n4 l_n5 l_n6]^T，

B₂＝[(Ra₄×l_n4) (Ra₅×l_n5) (Ra₆×l_n6)]^T；

则由式(7)可得：

同样的，将作动器的伸缩速度表示为：

式(8)以矩阵形式表示：

式中，

D₁＝[l_n1 l_n2 l_n3]^T，

B₁＝[(Ra₁×l_n1) (Ra₂×l_n2) (Ra₃×l_n3)]^T；

将式(8)代入(10)式，得到：

因此角速度到作动器的伸缩速度的系数矩阵为并联机构正解的数值解法表示为：

β_k+1＝β_k+J₂ ^-1(L₁-L_1k)

式中，k表示为迭代次数；β_k表示为姿态角的第k次迭代值，β_k＝[q_1k q_2k q_3k]^T；J₂表示为角速度到作动器伸缩速度的系数矩阵，L₁表示为作动器的长度，L₁＝[l1 l2 l3]T；L_1k表示为作动器的长度第k次迭代值，L_1k＝[l_1k l_2k l_3k]^T。

本发明的有益效果是：通过建立体坐标系与参考坐标，并采用欧拉角来描述姿态，进而获得动平台的平动位移；最后依据动平台的平动位移以及作动器、支撑杆的长度，运用运动学正解的方法，获得动平台的姿态角，具有精度高、实时性好的优点。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本发明建立的体坐标系与参考坐标系处于工作初始位置时的结构示意图。

图2为本发明中并联机构的姿态角定义示意图。

具体实施方式

实施例中，并联机构包括三组作动器、三组支撑杆和动平台，三组作动器的一端分别与动平台铰接，三组作动器的另一端分别与地基或者支撑杆铰接；三组支撑杆的一端分别与地基铰接，三组作动器的另一端分别与动平台或者作动器铰接；

并联机构处于工作初始位置时：作动器、支撑杆与动平台的上铰接点在同一平面内，且位于一个圆上；三个支撑杆的长度相同，且与地基的下铰接点位于一个圆上；下铰接点所在圆的圆心是动平台中心在地基上的垂直投影；三个作动器之一与地基平行，其余作动器与地基垂直。

实施例中，三自由度并联机构的结构参数如表1

表1 三自由度并联机构的结构参数

参见图1所示，实施例中，以动平台上铰接点所在圆的中心处建立体坐标系O_b-X_bY_bZ_b，并在大地上建立固定的参考坐标系O_p-X_pY_pZ_p，并联机构处于工作初始位置时体坐标系与参考坐标系重合；

应用体坐标系相对与参考坐标系的广义坐标来描述动平台的姿态，广义坐标包括三个欧拉角，三个欧拉角为横摇角、纵摇角和偏航角；获取体坐标系相对与参考坐标系的旋转矩阵；并获取动平台在体坐标系中角速度、角加速度与欧拉角导数之间的关系；

根据期望的姿态角，获得动平台的平动位移；

依据动平台的姿态角和平动位移，运用运动学反解方法，获得并联结构中各个作动器的运动规律；

三个作动器依据获得的运动规律进行运动。

通过建立体坐标系与参考坐标，并采用欧拉角来描述姿态，进而获得动平台的平动位移；最后依据动平台与平动位移，运用运动学反解的方法，获得作动器的运动规律。能够根据期望的姿态角，准确的控制作动器的运动，具有精度高、实时性好的优点。

实施例中，并联机构是作动器、支撑杆和动平台等一系列构件用运动副连接而成的空间闭环结构。可以利用Kutzbach-Glübler公式求解自由度：

式中，F表示机构的自由度数目；n表示为机构构件的总数；m表示为机构中运动副的总数；f_i第i个运动副的自由度数；λ表示为机构的局部自由度数目。

实施例中，并联机构具有三个独立自由度，是典型的少自由度并联机构。

实施例中，采用ZYX型欧拉角来描述并联机构的姿态；

体坐标系相对于参考坐标系的广义坐标表示为：q＝[q₁,q₂,q₃,q₄,q₅, q₆]T；q₁、q₂和q₃依次表示为横摇角、纵摇角和偏航角，q₄、q₅和q₆为体坐标系原点O₁在参考坐标系中的坐标分量。

实施例中，参见图2所示，获取体坐标系相对与参考坐标系的旋转矩阵，具体包括：

首先，绕OZ轴旋转偏航角q₃，相应地OX转至OX′，OY转至OY′，此旋转对应的变换矩阵为R₃，其中R₃表示为：

其次，绕OY′轴旋转纵摇角q2，相应地OZ转至OZ′，OX′转至OX₁，对应的变换矩阵为R2，其中R2表示为：

最后，绕OX₁轴旋转横摇角q1，OY′转至OY1，OZ′转至OZ1，获得体坐标系O-X1Y1Z1，相应的变换矩阵为R1，其中R1表示为：

式中，sq_i表示为sin(qi)，i＝1,2,3；cq_i表示为cos(qi)，i＝1,2,3；

则体坐标系相对于参考坐标系的旋转矩阵为：

实施例中，将体坐标系相对参考坐标系的位移矢量表示为[q₄ q₅ q₆]^T，且欧拉角表示为[q₁ q₂ q₃]T；将动平台的上铰接点在体坐标系中的坐标表示为a_i＝[a_xi a_yi a_zi]^T，并将其下铰接点在参考坐标系中的坐标表示为b_i＝[b_xi b_yi b_zi]^T，其中i＝1～6，则上铰接点在参考坐标系中的矢量坐标表示为：

g_i＝Ra_i+c

式中，g_i表示为上铰接点在参考坐标系中的矢量坐标，g_i＝[g_xi g_yi g_zi]^T， i＝1,2,…,6；

c表示为体坐标系原点在参考坐标系中的矢量，c＝[q₄ q₅ q₆]^T；

进而，支撑杆的长度约束方程为：

式中，l₄、l₅、l₆分别表示三个支撑杆的长度；

实施例中，基于支撑杆的长度约束方程，求解q₄、q₅、q₆，具体如下：

令s_i＝R_ai-b_i＝[s_xi s_yi s_zi]^T，那么支撑杆的长度约束方程进一步表示为：

基于此方程获得如下方程：

式中，

将方程(2)中q₆假定为待定未知数，那么：

将q₄和q₅用q₆表示出来：

再将q₄和q₅代入方程(1)中，则该式可变形为标准的一元二次方程的形式，表示为：

c₁q₆ ²+c₂q₆+c₃＝0 (4)。

求解方程(4)，得到q₆的两个根，两个根中的一个根表示动平台处在地基之上，另一个根表示动平台处在地基之下。根据三自由度并联机构的实际装配情况可以舍掉其中的一个，再将q₆代入方程(3)即可得到q4和q5，这样就解出了动平台的平动位移。

实施例中，根据动平台的姿态角和平动位移，获得作动器的矢量为：

l_i＝Ra_i-b_i+c(i＝1,2,3)；

进一步，支撑杆的矢量为：

l_i＝Ra_i-b_i+c(i＝4,5,6)。

实施例中，运动学的正解包括基于Stewart机构的运动学正解和基于支撑杆约束的运动学正解，以下分别针对不同的正解方法进行介绍：

实施例中，采用基于Stewart机构的运动学正解，应用运动学正解算法，获取所述动平台的姿态角，具体包括：

已知三组所述作动器的长度，且已知或者测量三组所述支撑杆的长度，求解非线性方程组：

||Ra_i-b_i+c||＝||g_i-b_i||＝||l_i||(i＝1,2…,6) (1)

应用牛顿-泰勒展开发求解式(1)，表示为:

式中,l_i0表示为作动器或支撑杆上铰和下铰间的初始长度(m)；Δl_i表示为作动器或支撑杆的伸缩量(m)；g_ki表示为上铰接点在参考坐标系中的坐标分量，k＝1,2,3；b_ki表示为下铰接点在参考坐标系中的坐标分量，k＝1,2,3；

令

解式(2)，得到所述动平台的当前姿态q，

将f_i(q)在初始位置q₀附近进行泰勒级数展开，并取其线性部分得

进一步令Δq＝q-q₀和Δq_j＝q_j-q_0j(j＝1,2…,6)，则式(3)表示为：

将方程组(4)看成以Δq_i(i＝1,2,…,6)未知数的线性方程组，其系数矩阵 J₁为：

若J₁非奇异，则方程组(5)有唯一解Δq，

若Δq满足求解精度要求，即||Δq||≤ε(ε为求解精度)，则q＝q₀+Δq为所求得的正解；否则令q₀＝q，并依据新q₀重新计算作动器或支撑杆上铰和下铰间的长度l_i0(i＝1,2,…,6)和系数矩阵J₁，然后依据式(4)再次求解Δq，直到Δq满足求解精度为止。

实施例中，依据动平台的姿态角和平动位移，运动学关系分析，具体包括：

根据空间矢量关系，依据作动器的矢量与支撑杆的矢量，获得各个上铰接点的速度；并将上铰接点速度在作动器与支撑杆的方向上投影，获得作动器与支撑杆的伸缩速度；

参见图1和图2所示，作动器包括活塞杆和缸筒，依据作动器或支撑杆中上铰接点的角速度、矢量以及加速度的关系，获得作动器的缸筒质心的加速度与活塞杆质心的加速度。

具体的，根据作动器与支撑杆的矢量，则上铰接点的速度表示为：

式中，ω表示为动平台在参考坐标系中的角速度；表示为动平台的平动速度。

式(5)以矩阵型式表示为：

式中，表示为上铰接点a_i的反对称矩阵，～为反对称矩阵算子；表示为动平台的广义速度；J_ai,x表示为动平台广义速度到上铰接点速度的雅克比矩阵。

具体的，将上铰接点速度在作动器与支撑杆的方向上投影，获得作动器与支撑杆的伸缩速度，表示为：

式中，表示为作动器和支撑杆的伸缩速度(m/s)；l_ni表示为作动器和支撑杆的单位矢量，

将式(4)代入式(7)，获得：

实施例中，采用基于支撑杆约束的运动学正解，应用运动学正解算法，获取所述动平台的姿态角，具体包括：

基于运动学关系，获得作动器或者支撑杆的伸缩速度表示为：

式中，表示为作动器和支撑杆的伸缩速度(m/s)；l_ni表示为作动器和支撑杆的单位矢量；表示为上铰接点的速度；表示为动平台的平动速度；ω表示为动平台在参考坐标系中的角速度；ai＝[axi ayi azi]T表示为动平台的上铰接点在体坐标系中的坐标；

依据三组所述支撑杆的长度固定，则其伸缩速度为零，因此：

将式(6)以矩阵形式表示：

式中，

D₂＝[l_n4 l_n5 l_n6]^T，

B₂＝[(Ra₄×l_n4) (Ra₅×l_n5) (Ra₆×l_n6)]^T；

则由式(7)可得：

同样的，将作动器的伸缩速度表示为：

式(8)以矩阵形式表示：

式中，

D₁＝[l_n1 l_n2 l_n3]^T，

B₁＝[(Ra₁×l_n1) (Ra₂×l_n2) (Ra₃×l_n3)]^T；

将式(8)代入(10)式，得到：

因此角速度到作动器的伸缩速度的系数矩阵为并联机构正解的数值解法表示为：

β_k+1＝β_k+J₂ ^-1(L₁-L_1k)

式中，k表示为迭代次数；β_k表示为姿态角的第k次迭代值，β_k＝[q_1k q_2k q_3k]^T；J₂表示为角速度到作动器伸缩速度的系数矩阵，L₁表示为作动器的长度，L₁＝[l1 l2 l3]T；L_1k表示为作动器的长度第k次迭代值，L_1k＝[l_1k l_2k l_3k]^T。

实施例中，对上述两种三自由度并联机构的运动学正解算法进行收敛性分析：应用康托洛维奇定理推导出收敛域，根据作动器的最大伸缩速度进一步得到两次正解间允许的最大时间间隔，通过仿真研究表明实施例中的运动学正解算法在整个工作空间内都是收敛的。

实施例中，三自由度并联机构运动学正解实时性的测试结果，如表1 所示：

表1 全工作空间运动学正解的实时性

由表可知，当迭代终止条件为迭代次数不大于3次时，最大迭代时间为0.257ms，最小迭代时间为0.210ms；当迭代终止条件为迭代次数不大于 4次时，最大迭代时间为0.324ms，最小迭代时间为0.210ms。可见三自由度并联机构运动学正解的实时性非常好。

实施例中，通过仿真验证运动学正解的收敛精度。首先采用Adams软件对三自由度并联机构进行建模，然后选取多组作动器杆长组合并用 Adams模型和运动学正解算法分别解出并联机构的姿态，最后以Adams模型的计算结果为标准分析运动学正解算法的精度，分析结果见表2。

表2 运动学正解的精度

当迭代次数设置为3次时，三自由度并联机构运动学正解的收敛精度约为0.0001度，可见，运动学正解的收敛精度都非常好。

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.基于运动学正解的三自由度并联机构的姿态角控制方法，其特征在于，所述并联机构包括三组作动器、三组支撑杆和动平台，三组所述作动器的一端分别与所述动平台铰接，三组所述作动器的另一端分别与地基或者所述支撑杆铰接；三组所述支撑杆的一端分别与地基铰接，三组所述作动器的另一端分别与所述动平台或者所述作动器铰接；

控制方法包括：

以动平台上铰接点所在圆的中心处建立体坐标系O_b-X_bY_bZ_b，并在大地上建立固定的参考坐标系O_p-X_pY_pZ_p，并联机构处于工作初始位置时体坐标系与参考坐标系重合；

应用体坐标系相对与参考坐标系的广义坐标来描述所述动平台的姿态，所述广义坐标包括三个欧拉角，三个所述欧拉角为横摇角、纵摇角和偏航角；获取体坐标系相对与参考坐标系的旋转矩阵；

根据所述作动器或所述支撑杆的长度，依据旋转矩阵与运动学关系，应用运动学正解算法，获取所述动平台的姿态角。

2.根据权利要求1所述的基于运动学正解的三自由度并联机构的姿态角控制方法，其特征在于，所述并联机构处于工作初始位置时：

所述作动器、所述支撑杆与所述动平台的上铰接点在同一平面内，且位于一个圆上；

三个所述支撑杆的长度相同，且与所述地基的下铰接点位于一个圆上；所述下铰接点所在圆的圆心是动平台中心在地基上的垂直投影；

三个所述作动器之一与所述地基平行，其余所述作动器与所述地基垂直。

3.根据权利要求2所述的基于运动学正解的三自由度并联机构的姿态角控制方法，其特征在于，采用ZYX型欧拉角来描述所述并联机构的姿态；

体坐标系相对于参考坐标系的广义坐标表示为：q＝[q₁,q₂,q₃,q₄,q₅,q₆]T；q₁、q₂和q₃依次表示为横摇角、纵摇角和偏航角，q₄、q₅和q₆为体坐标系原点O1在参考坐标系中的坐标分量。

4.根据权利要求3所述的基于运动学正解的三自由度并联机构的姿态角控制方法，其特征在于，获取体坐标系相对与参考坐标系的旋转矩阵，具体包括：

首先，绕OZ轴旋转偏航角q₃，相应地OX转至OX′，OY转至OY′，此旋转对应的变换矩阵为R₃，其中R₃表示为：

其次，绕OY′轴旋转纵摇角q2，相应地OZ转至OZ′，OX′转至OX₁，对应的变换矩阵为R2，其中R2表示为：

最后，绕OX₁轴旋转横摇角q1，OY′转至OY1，OZ′转至OZ1，获得体坐标系O-X1Y1Z1，相应的变换矩阵为R1，其中R1表示为：

式中，sq_i表示为sin(q_i)，i＝1,2,3；cq_i表示为cos(q_i)，i＝1,2,3；

则体坐标系相对于参考坐标系的旋转矩阵为：

5.根据权利要求4所述的基于运动学正解的三自由度并联机构的姿态角控制方法，其特征在于，

将体坐标系相对参考坐标系的位移矢量表示为[q₄ q₅ q₆]^T，且欧拉角表示为[q₁ q₂ q₃]^T；将所述动平台的上铰接点在体坐标系中的坐标表示为a_i＝[a_xi a_yi a_zi]^T，并将其下铰接点在参考坐标系中的坐标表示为b_i＝[b_xi b_yi b_zi]^T，其中i＝1～6，则上铰接点在参考坐标系中的矢量坐标表示为：

g_i＝Ra_i+c

式中，g_i表示为上铰接点在参考坐标系中的矢量坐标，g_i＝[g_xi g_yi g_zi]^T，i＝1,2,…,6；

c表示为体坐标系原点在参考坐标系中的矢量，c＝[q₄ q₅ q₆]^T；

根据动平台的姿态角和平动位移，得到所述作动器的矢量表示为：

l_i＝Ra_i-b_i+c(i＝1,2,3)

得到支撑杆的矢量表示为：

l_i＝Ra_i-b_i+c(i＝4,5,6)。

6.根据权利要求5所述的基于运动学正解的三自由度并联机构的姿态角控制方法，其特征在于，采用基于Stewart机构的运动学正解，应用运动学正解算法，获取所述动平台的姿态角，具体包括：

已知三组所述作动器的长度，且已知或者测量三组所述支撑杆的长度，求解非线性方程组：

||Ra_i-b_i+c||＝||g_i-b_i||＝||l_i||(i＝1,2…,6) (1)

应用牛顿-泰勒展开发求解式(1)，表示为:

式中,l_i0表示为作动器或支撑杆上铰和下铰间的初始长度(m)；Δl_i表示为作动器或支撑杆的伸缩量(m)；g_ki表示为上铰接点在参考坐标系中的坐标分量，k＝1,2,3；b_ki表示为下铰接点在参考坐标系中的坐标分量，k＝1,2,3；

令

解式(2)，得到所述动平台的当前姿态q，

将f_i(q)在初始位置q₀附近进行泰勒级数展开，并取其线性部分得

进一步令Δq＝q-q₀和Δq_j＝q_j-q_0j(j＝1,2…,6)，则式(3)表示为：

将方程组(4)看成以Δq_i(i＝1,2,…,6)未知数的线性方程组，其系数矩阵J₁为：

若J₁非奇异，则方程组(5)有唯一解Δq，

若Δq满足求解精度要求，即||Δq||≤ε(ε为求解精度)，则q＝q₀+Δq为所求得的正解；否则令q₀＝q，并依据新q₀重新计算作动器或支撑杆上铰和下铰间的长度l_i0(i＝1,2,…,6)和系数矩阵J₁，然后依据式(4)再次求解Δq，直到Δq满足求解精度为止。

7.根据权利要求5所述的基于运动学正解的三自由度并联机构的姿态角控制方法，其特征在于，采用基于支撑杆约束的运动学正解，应用运动学正解算法，获取所述动平台的姿态角，具体包括：

基于运动学关系，获得作动器或者支撑杆的伸缩速度表示为：

式中，表示为作动器和支撑杆的伸缩速度(m/s)；l_ni表示为作动器和支撑杆的单位矢量；表示为上铰接点的速度；表示为动平台的平动速度；ω表示为动平台在参考坐标系中的角速度；ai＝[axi ayi azi]T表示为动平台的上铰接点在体坐标系中的坐标；

依据三组所述支撑杆的长度固定，则其伸缩速度为零，因此：

将式(6)以矩阵形式表示：

式中，

D₂＝[l_n4 l_n5 l_n6]^T，

B₂＝[(Ra₄×l_n4) (Ra₅×l_n5) (Ra₆×l_n6)]^T；

则由式(7)可得：

同样的，将作动器的伸缩速度表示为：

式(8)以矩阵形式表示：

式中，

D₁＝[l_n1 l_n2 l_n3]^T，

B₁＝[(Ra₁×l_n1) (Ra₂×l_n2) (Ra₃×l_n3)]^T；

将式(8)代入(10)式，得到：

因此角速度到作动器的伸缩速度的系数矩阵为并联机构正解的数值解法表示为：

β_k+1＝β_k+J₂ ^-1(L₁-L_1k)

式中，k表示为迭代次数；β_k表示为姿态角的第k次迭代值，β_k＝[q_1k q_2k q_3k]^T；J₂表示为角速度到作动器伸缩速度的系数矩阵，L₁表示为作动器的长度，L₁＝[l1l2l3]T；L_1k表示为作动器的长度第k次迭代值，L_1k＝[l_1k l_2k l_3k]^T。