CN106055519A - 一种Stewart并联机构的奇异性分析方法 - Google Patents

Abstract

本发明公开一种Stewart并联机构的奇异性分析方法，该方法采用四元数描述旋转矩阵，通过研究位置变量与姿态变量之间的耦合关系以及四元数的性质，得到7个等价的方程式；进而导出一新的雅可比矩阵，并对其取行列式最终获得该类型Stewart并联机构奇异轨迹的一般表达式。该表达式完全符号表达时的多项式只有258项；相较于传统方法是一种较为简洁的结果。

Description

一种Stewart并联机构的奇异性分析方法

技术领域

本发明属于机械***的运动学、动力学与控制研究领域，尤其是一种Stewart并联机构的工作空间中奇异性位形以及奇异轨迹的分析方法。

背景技术

众所周知，Stewart并联机构(也称Stewart-Gough平台或Gough平台)作为并联机构的典型代表，是由动、静两个平台和六根长度可伸缩的驱动杆组成的六自由度空间机构；每根驱动杆两端通过两个球铰或者一个球铰和一个虎克铰分别与动、静两平台相连接。在工作过程中静平台静止不动，通过控制六根驱动杆的长度变化，可使动平台获得三个平动自由度和三个转动自由度。相较于传统的串联机构，具有运动部分重量轻、运动精度高、各向同性较好、刚度大、承受负载的能力相对较大、动态性能和稳定性更强等优点。现已广泛应用于运动模拟器、并联运动机床、并联机器人、微位移定位装置、医疗、娱乐等领域。

自上世纪中期并联机构成为机构学领域的热点研究问题以来，在Stewart并联机构的运动学问题、奇异性分析、工作空间与灵巧性、动力学与控制等方面，国内外众多学者进行了深入而广泛的研究。然而，尽管Stewart并联机构无论是在理论研究还是工程应用方面都取得了长足的进步，但是时至今日仍然存在很多问题没有得到很好的解决，尤其是被J.-P.Merlet称为并联机构三大基本问题的运动学正解、奇异性和工作空间三方面的问题。当机构发生奇异时，动平台在某个方向上获得多余的自由度或失去某些自由度时就会发生奇异。获得多余的自由度会导致动平台的位姿是失去控制；失去某些自由度则用于平衡动平台上外载荷的关节驱动力将趋于无穷大，影响并联机构正常工作，严重时还会毁坏机构。所以，在Stewart并联机构理论研究以及工程应用中奇异性都是无法回避的研究内容。

目前，研究Stewart并联机构奇异的方法有雅可比代数法、线几何法、奇异的运动学法等。雅可比代数法主要是通过建立驱动杆的伸缩速度(输入量)与动平台的线速度和角速度(输出量)之间的映射矩阵即雅可比矩阵，并令其行列式为零来研究机构的奇异性的。然而，这种传统的雅可比矩阵每个元素都是复杂的多项式，计算效率相对较低且难以应用于机构的实时控制之中。线几何法是基于Grassman线几何原理，通过研究线矢量的相关性来研究机构的奇异性，对机构的位形已经确定的情况下，运用此方法判别奇异性较为容易。现有的研究成果难以满足奇异轨迹表达简洁、计算效率较高而且易于判别已知位形的奇异性的要求。

故，需要一种新的技术方案以解决上述问题。

发明内容

本发明的目的是针对现有技术手段存在的不足，提供一种获取Stewart并联机构奇异位形与奇异轨迹解析表达式的方法。该表达式适用于所有动、静平台铰接点对称布置在圆周上的Stewart并联机构的奇异性分析。

为解决上述问题，本发明一种基于四元数与Rodriguez参数的Stewart并联机构的奇异性分析方法可采用如下技术方案：

一种基于四元数与Rodriguez参数的Stewart并联机构的奇异性分析方法，所述的Stewart并联机构包括动平台、静平台及连接动、静平台的6根并联的长度可伸缩的驱动杆，该奇异性分析方法包括如下步骤：

(1)、建立杆长约束的等价方程式：

旋转矩阵采用四元数描述，将位置变量通过姿态变量表达出来，建立杆长约束的等价方程式，具体形式如下：

$\begin{array}{c}{P}_P=P_{P0}+k_0({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_3^2)\\ P_X=P_{x0}+2k_1{\mathrm{\δ}}_1{\mathrm{\ϵ}}_2\\ P_Y=P_{y0}+k_1({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)\\ W_X=W_{x0}+2k_2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2\\ W_Y=W_{y0}+k_2({\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_2^2)\\ 2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3=C_0\end{array}---\left(1\right)$

式中，为矢量P模的平方；W_x、与W_y分别为矢量P向静坐标系基矢量x与y的投影，具体表达式为，

$W_x=P_x({\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2)+P_y(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3)+P_z(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_2),$

参数{k₀,k₁,k₂}是由动、静平台铰链点参数{r₁,r₂,θ₁,θ₂}决定的常数；{P_P0,P_x0,P_y0,W_x0,W_y0,C₀}在具***姿下是由动、静平台铰链点参数{r₁,r₂,θ₁,θ₂}和杆长l₁～l₆共同决定的常数，

(2)、构建新的雅可比矩阵

基于式(1)以及单位四元数的性质，有如下方程式成立：

$P_{P0}=P_x^2+P_y^2+P_z^2-k_0({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_3^2)$

P_x0＝P_X-2k₁ε₁ε₂

$P_{y0}=P_Y-k_1({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)$

$W_{x0}=P_x({\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2)+P_y(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3)+P_z(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_2)-2k_2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2$ (2)

$W_{y0}=P_x(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3)+P_y({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2)+P_z(2{\mathrm{\ϵ}}_2{\mathrm{\ϵ}}_3+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_1)-k_2({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)$

C₀＝2ε₀ε₃

$1={\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2+{\mathrm{\ϵ}}_3^2$

式(2)为关于七个变量(P_X P_Y P_Z ε₀ ε₁ ε₂ ε₃)的七个方程式，这七个等价方程对时间求导就可以得到输入与输出之间的映射关系式：

$\[M(P,\mathrm{\ϵ})\]\·\left(\begin{array}{c}{\stackrel{\·}{P}}_X\\ {\stackrel{\·}{P}}_Y\\ {\stackrel{\·}{P}}_Z\\ {\stackrel{\·}{\mathrm{\ϵ}}}_0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_1\\ {\stackrel{\·}{\mathrm{\ϵ}}}_2\\ {\stackrel{\·}{\mathrm{\ϵ}}}_3\end{array}\right)=\left(\begin{array}{c}{M}_l\\ \stackrel{\→}{0}\end{array}\right)\left(\begin{array}{c}{\stackrel{\·}{l}}_1\\ {\stackrel{\·}{l}}_2\\ {\stackrel{\·}{l}}_3\\ {\stackrel{\·}{l}}_4\\ {\stackrel{\·}{l}}_5\\ {\stackrel{\·}{l}}_6\end{array}\right)---\left(3\right)$

其中M_l∈R^6×6，在杆长给定的情况下M_l(i,j)＝f(l_j)的为一常数；

$M(P,\mathrm{\ϵ})=\left(\begin{array}{ccccccc}{P}_X& P_Y& P_Z& -k_0{\mathrm{\ϵ}}_0& 0& 0& k_0{\mathrm{\ϵ}}_3\\ 1& 0& 0& 0& -2k_1{\mathrm{\ϵ}}_2& -2k_1{\mathrm{\ϵ}}_1& 0\\ 0& 1& 0& 0& -2k_1{\mathrm{\ϵ}}_1& 2k_1{\mathrm{\ϵ}}_2& 0\\ M(4,1)& M(4,2)& M(4,3)& M(4,4)& M(4,5)& M(4,6)& M(4,7)\\ M(5,1)& M(5,2)& M(5,3)& M(5,4)& M(5,5)& M(5,6)& M(5,7)\\ 0& 0& 0& {\mathrm{\ϵ}}_3& 0& 0& {\mathrm{\ϵ}}_0\\ 0& 0& 0& {\mathrm{\ϵ}}_0& {\mathrm{\ϵ}}_1& {\mathrm{\ϵ}}_2& {\mathrm{\ϵ}}_3\end{array}\right)---\left(4\right)$

M(4,2)＝2ε₁ε₂+2ε₀ε₃

M(4,3)＝-2ε₀ε₂+2ε₁ε₃

M(4,4)＝2P_Xε₀-2P_Zε₂+2P_Yε₃

M(4,5)＝2P_Xε₁+2P_Yε₂-2k₂ε₂+2P_Zε₃

M(4,6)＝-2P_Zε₀+2P_Yε₁-2k₂ε₁-2P_Xε₂

M(4,7)＝2P_Yε₀+2P_Zε₁-2P_Xε₃

M(5,1)＝2ε₁ε₂-2ε₀ε₃

M(5,3)＝2ε₀ε₁+2ε₂ε₃

M(5,4)＝2P_Yε₀+2P_Zε₁-2P_Xε₃

M(5,5)＝2P_Zε₀-2P_Yε₁-2k₂ε₁+2P_Xε₂

M(5,6)＝2P_Xε₁+2P_Yε₂+2k₂ε₂+2P_Zε₃

M(5,7)＝-2PXε₀+2P_Zε₂-2P_Yε₃

式(4)中M(P,ε)为驱动杆的杆长变化率，也就是驱动速率与动平台位置姿态变化率之间的映射变换矩阵，即雅可比矩阵。

(3)、建立奇异轨迹方程

当机构处于奇异位姿时，雅可比矩阵的行列式应该为零，即为奇异轨迹方程；

f＝Det[M(P,ε)]＝0 (5)

由于四元数中有四个分量且在实际使用中根据对称性，规定ε₀>0，所以有下式成立：

${\mathrm{\ϵ}}_0=\sqrt{1-{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2}---\left(6\right)$

将式(6)代入式(5)就可以得到一个新的只含有6个变量的奇异轨迹方程，如下式所示：

f(ε₁,ε₂,ε₃,P_X,P_Y,P_Z)＝0 (7)

此外，四元数与Rodriguez参数之间有如下转换关系：

$\begin{array}{c}{\mathrm{\ϵ}}_0=\frac{1}{\sqrt{1+U^2+V^2+W^2}}\\ {\mathrm{\ϵ}}_1=\frac{U}{\sqrt{1+U^2+V^2+W^2}}\\ {\mathrm{\ϵ}}_2=\frac{V}{\sqrt{1+U^2+V^2+W^2}}\\ {\mathrm{\ϵ}}_3=\frac{W}{\sqrt{1+U^2+V^2+W^2}}\end{array}---\left(8\right)$

将式(8)带入式(5)就可以得到Rodriguez参数表达的奇异轨迹方程，如下式所示：

f(U,V,W,P_X,P_Y,P_Z)＝0 (9)

式(7)与式(9)两个奇异轨迹方程在本质上是等价的，除位姿变量以外只含有{k₀,k₁,k₂}三个参数，这三个参数在机构确定以后为常量。奇异轨迹方程的计算速度得到了提高，尤其是式(9)完全展开之后符号表达时只有258项；如果在进行因式分解的情况下进行奇异性判断，计算速度将会进一步提高。

(4)、求解奇异轨迹以及判断奇异性：

奇异轨迹方程无论是采用式(7)还是式(9)，均可以在给定任意三个位姿变量研究奇异轨迹随其余三个变量的变化关系；或者已知姿态变量时，求解位置奇异轨迹，反之亦然；亦或对一组确定的位姿进行判断该位姿是否处于奇异位形。

本发明的有益效果：利用通过新雅可比获得的Stewart并联机构的奇异轨迹表达式研究机构的奇异性问题时，奇异轨迹表达式完全展开之后的符号表达形式只有258项，且在进行因式分解的情况下可以进一步提高计算效率。无论是求解奇异轨迹还是针对已知确定位形进行奇异性判断，均可以有效地降低计算耗时。

附图说明

图1是本发明中涉及研究的Stewart并联机构简图。

图2是与Rodriguez参数(U＝0.7、V＝0.3、W＝0.4)对应的位置奇异轨迹分布图。

图3是与四元数(ε₀＝0.758098、ε₁＝0.530669、ε₂＝0.227429、ε₃＝0.303239)对应的位置奇异轨迹分布图。

图4是位置矢量为(2，2，4)时，奇异轨迹随Rodriguez参数变化情况分布图。

图5是位置矢量为(2，2，4)时，奇异轨迹随四元数变化情况分布图。

具体实施方式

下面结合附图进一步阐明本发明，应理解这些仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

(1)、动、静平台铰链点坐标参数及旋转矩阵

如图1所示的Stewart并联机构简图，其动、静平台的铰链点是分别对称的布置在两个圆上。此时由于对称性，动、静平台的铰链点坐标可以用四个参数，r₁、r₂、θ₁、θ₂表示，如下表所示。

表1Stewart并联机构动、静平台铰链点坐标参数 单位/mm

因此，动平台的铰链点坐标向量在动坐标系中就可以表示为：

a_k＝(a_x,k a_y,k 0)^T (10)

静平台的铰链点坐标向量在静坐标系中就可以表示为：

b_k＝(b_x,k b_y,k 0)^T (10)

采用四元数描述的旋转矩阵，如下式所示：

$R=\left[\begin{array}{ccc}{\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2& 2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3& 2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_2\\ 2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3& {\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2& 2{\mathrm{\ϵ}}_2{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_1\\ 2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_2& 2{\mathrm{\ϵ}}_2{\mathrm{\ϵ}}_3+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_1& {\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2+{\mathrm{\ϵ}}_3^2\end{array}\right]---\left(11\right)$

式中，ε₁、ε₂、ε₃、ε₀∈R，i²＝j²＝k²＝-1，且ij＝-ji＝k，jk＝-kj＝i，ki＝-ik＝j。与单位向量类似，ε^Tε＝1。

采用Rodriguez参数描述的旋转矩阵，具体形式如下式所示：

$R=\frac{1}{\mathrm{\Δ}}\left[\begin{array}{ccc}{U}^2-V^2-W^2+1& 2UV-2W& 2UW+2V\\ 2UV+2W& -U^2+V^2-W^2+1& 2VW-2U\\ 2UW-2V& 2VW+2U& -U^2-V^2+W^2+1\end{array}\right]---\left(12\right)$

其中，Δ＝U²+V²+W²+1，U、V、W为Rodriguez参数。可以证明，四元数ε₁、ε₂、ε₃、ε₀与Rodriguez参数U、V、W之间有如下转换关系：

$\begin{array}{c}{\mathrm{\ϵ}}_0=\frac{1}{\sqrt{1+U^2+V^2+W^2}}\\ {\mathrm{\ϵ}}_1=\frac{U}{\sqrt{1+U^2+V^2+W^2}}\\ {\mathrm{\ϵ}}_2=\frac{V}{\sqrt{1+U^2+V^2+W^2}}\\ {\mathrm{\ϵ}}_3=\frac{W}{\sqrt{1+U^2+V^2+W^2}}\end{array}---\left(13\right)$

用矢量P＝[P_x P_y P_z]^T描述动平台参考点的位置向量，则一对铰链点之间的连杆矢量为：

l_ke_k＝P+R·a_k-b_k(k＝1～6) (14)

其中：

l_k是第k驱动杆的长度；

e_k是沿驱动杆k轴线方向的单位矢量；

a_k是动平台铰链点在动坐标系中的矢量；

b_k是静平台铰链点在静坐标系中的矢量；

P动平台参考点的位置在静坐标系中的位置矢量；

R动平台姿态正交矩阵，即旋转矩阵。

(2)、构造等价方程式

为了使雅可比矩阵的元素具有简洁的形式，旋转矩阵采用如式(11)所示的四元数描述形式。将铰链点坐标代入式(14)，并取矢量的模；将其平方展开之后，由于a_k，b_k的Z分量为零，则得到杆长平方的方程式为(为表达简洁，略去下标k)：

$\begin{array}{c}{l}^2-r_1^2-r_2^2=-2({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)(a_x{b}_x-a_y{b}_y)-2({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_3^2)(a_x{b}_x+a_y{b}_y)\\ +2\left(2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3\right)(a_y{b}_x-a_x{b}_y)-2\left(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2\right)(a_y{b}_x+a_x{b}_y)-\\ 2b_x{P}_x-2b_y{P}_y+2a_x{W}_x+2a_y{W}_y+P_P\end{array}---\left(15\right)$

式中，

$W_x=P_x({\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2)+P_y(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3)+P_z(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_2)$

$W_y=P_x(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3)+P_y({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2)+P_z(2{\mathrm{\ϵ}}_2{\mathrm{\ϵ}}_3+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_1)$

由式(15)可以得知，P_P、P_x、P_y、W_x、W_y、2ε₀ε₃、2ε₁ε₂均为只与位姿变量有关的量，可以视为新的未知变量。将这九个新的未知变量划分为主变量与次变量两组。令η₁＝[P_P P_x P_y W_x W_y 2ε₀ε₃]^T为主变量；令为次变量，则式(15)就等价于下式：

$\begin{array}{c}{P}_P=P_{P0}+k_0({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_3^2)\\ P_X=P_{x0}+2k_1{\mathrm{\δ}}_1{\mathrm{\ϵ}}_2\\ P_Y=P_{y0}+k_1({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)\\ W_X=W_{x0}+2k_2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2\\ W_Y=W_{y0}+k_2({\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_2^2)\\ 2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3=C_0\end{array}---\left(16\right)$

其中，参数{k₀,k₁,k₂}是由动、静平台铰链点参数所{r₁,r₂,θ₁,θ₂}决定的常数，一旦机构的结构参数确定，它们就随之确定；{P_P0,P_x0,P_y0,W_x0,W_y0,C₀}是由动、静平台铰链点参数{r₁,r₂,θ₁,θ₂}和杆长l₁～l₆共同决定的常数，它们的具体形式如下式所示。

$\begin{array}{c}{P}_{P0}=\frac{-6r_1^2-6r_2^2+l_1^2+l_2^2+l_3^2+l_4^2+l_5^2+l_6^2}{6}\\ P_{x0}=\frac{Csc\[{\mathrm{\θ}}_1-{\mathrm{\θ}}_2\]\left(\right(l_1^2-l_2^2-2l_3^2+2l_4^2+l_5^2-l_6^2)Cos\[{\mathrm{\θ}}_2\]+\sqrt{3}(l_1^2+l_2^2-l_5^2-l_6^2)Sin\[{\mathrm{\θ}}_2\])}{12r_1}\\ P_{y0}=\frac{Csc\[{\mathrm{\θ}}_1-{\mathrm{\θ}}_2\]\left(\sqrt{3}\right(l_1^2-l_2^2-l_5^2+l_6^2)Cos\[{\mathrm{\θ}}_2\]-(l_1^2+l_2^2-2l_3^2-2l_4^2+l_5^2+l_6^2)Sin\[{\mathrm{\θ}}_2\])}{12r_1}\\ W_{x0}=\frac{Csc\[{\mathrm{\θ}}_1-{\mathrm{\θ}}_2\]\left(\right(l_1^2-l_2^2-2l_3^2+2l_4^2+l_5^2-l_6^2)Cos\[{\mathrm{\θ}}_1\]+\sqrt{3}(l_1^2+l_2^2-l_5^2-l_6^2)Sin\[{\mathrm{\θ}}_1\])}{12r_2}\\ W_{y0}=\frac{Csc\[{\mathrm{\θ}}_1-{\mathrm{\θ}}_2\]\left(\sqrt{3}\right(l_1^2-l_2^2-l_5^2+l_6^2)Cos\[{\mathrm{\θ}}_1\]-(l_1^2+l_2^2-2l_3^2-2l_4^2+l_5^2+l_6^2)Sin\[{\mathrm{\θ}}_1\])}{12r_2}\\ C_0=\frac{(l_1^2-l_2^2+l_3^2-l_4^2+l_5^2-l_6^2)Csc\[{\mathrm{\θ}}_1-{\mathrm{\θ}}_2\]}{12r_1{r}_2}\end{array}---\left(17\right)$

(3)、建立奇异轨迹方程：

在Stewart并联机构工作时，是通过控制6根驱动杆的伸缩速度来使动平台获得相应的速度和角速度；二者之间是通过雅可比矩阵相联系的。通过研究雅可比矩阵可以判断机构的奇异性。动平台的位置和姿态是分别采用位置矢量P和四元数来描述的，因此，并联机构动平台的输出量又可以表达为位姿变量的导数即在输入变量与输出变量之间建立一种映射矩阵，即一种新的雅可比矩阵。

对于一个具体机构而言{P_P0,P_x0,P_y0,W_x0,W_y0,C₀}只含有l_i(i＝1～6)共六个变量，其余均为常数；因此式(16)具有如下等价形式：

$\begin{array}{c}{P}_{P0}=P_x^2+P_y^2+P_z^2-k_0({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_3^2)\\ P_{x0}=P_X-2k_1{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2\\ P_{y0}=P_Y-k_1({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)\\ W_{x0}=P_x({\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_2^2)+P_y(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3)+P_z(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_2)-2k_2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2\\ W_{y0}=P_x(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2-{2\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3)+P_y({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2)+P_z(2{\mathrm{\ϵ}}_2{\mathrm{\ϵ}}_3+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_1)-k_2({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)\\ C_0=2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3\end{array}---\left(18\right)$

又根据四元数的性质，为与式(18)保持形式上的统一有：

$1={\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2+{\mathrm{\ϵ}}_3^2---\left(19\right)$

式(18)与式(19)共七个关于变量(P_X P_Y P_Z ε₀ ε₁ ε₂ ε₃)等价方程式，这七个位姿变量与驱动杆长度l_i(i＝1～6)一样都是随时间变化的量；因而这七个等价方程对时间求导就可以得到输入与输出之间的映射关系式：

$\[M(P,\mathrm{\ϵ})\]\·\left(\begin{array}{c}{\stackrel{\·}{P}}_X\\ {\stackrel{\·}{P}}_Y\\ {\stackrel{\·}{P}}_Z\\ {\stackrel{\·}{\mathrm{\ϵ}}}_0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_1\\ {\stackrel{\·}{\mathrm{\ϵ}}}_2\\ {\stackrel{\·}{\mathrm{\ϵ}}}_3\end{array}\right)=\left(\begin{array}{c}{M}_l\\ \stackrel{\→}{0}\end{array}\right)\left(\begin{array}{c}{\stackrel{\·}{l}}_1\\ {\stackrel{\·}{l}}_2\\ {\stackrel{\·}{l}}_3\\ {\stackrel{\·}{l}}_4\\ {\stackrel{\·}{l}}_5\\ {\stackrel{\·}{l}}_6\end{array}\right)---\left(20\right)$

其中M_l∈R^6×6，在杆长给定的情况下M_l(i,j)＝f(l_j)的为一常数；

$M(P,\mathrm{\ϵ})=\left(\begin{array}{ccccccc}{P}_X& P_Y& P_Z& -k_0{\mathrm{\ϵ}}_0& 0& 0& k_0{\mathrm{\ϵ}}_3\\ 1& 0& 0& 0& -2k_1{\mathrm{\ϵ}}_2& -2k_1{\mathrm{\ϵ}}_1& 0\\ 0& 1& 0& 0& -2k_1{\mathrm{\ϵ}}_1& 2k_1{\mathrm{\ϵ}}_2& 0\\ M(4,1)& M(4,2)& M(4,3)& M(4,4)& M(4,5)& M(4,6)& M(4,7)\\ M(5,1)& M(5,2)& M(5,3)& M(5,4)& M(5,5)& M(5,6)& M(5,7)\\ 0& 0& 0& {\mathrm{\ϵ}}_3& 0& 0& {\mathrm{\ϵ}}_0\\ 0& 0& 0& {\mathrm{\ϵ}}_0& {\mathrm{\ϵ}}_1& {\mathrm{\ϵ}}_2& {\mathrm{\ϵ}}_3\end{array}\right)---\left(21\right)$

$M(4,1)={\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2$

M(4,2)＝2ε₁ε₂+2ε₀ε₃

M(4,3)＝-2ε₀ε₂+2ε₁ε₃

M(4,4)＝2P_Xε₀-2P_Zε₂+2P_Yε₃

M(4,5)＝2P_Xε₁+2P_Yε₂-2k₂ε₂+2P_Zε₃

M(4,6)＝-2P_Zε₀+2P_Yε₁-2k₂ε₁-2P_Xε₂

M(4,7)＝2P_Yε₀+2P_Zε₁-2P_Xε₃

M(5,1)＝2ε₁ε₂-2ε₀ε₃

$M(5,2)={\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2$

M(5,3)＝2ε₀ε₁+2ε₂ε₃

M(5,4)＝2P_Yε₀+2P_Zε₁-2P_Xε₃

M(5,5)＝2P_Zε₀-2P_Yε₁-2k₂ε₁+2P_Xε₂

M(5,6)＝2P_Xε₁+2P_Yε₂+2k₂ε₂+2P_Zε₃

M(5,7)＝-2PXε₀+2P_Zε₂-2P_Yε₃

式(20)中M(P,ε)为驱动杆杆长变化率，即驱动速率与动平台位置姿态变化率之间的映射变换矩阵，即雅可比矩阵。当机构处于奇异位姿时，雅可比矩阵的行列式应该为零；

f＝Det[M(P,ε)]＝0 (22)

由式(21)可以看出该雅可比矩阵的各个元素除第四、第五两行各个元素较为复杂外，其他各行的各个元素均非常简单，为单项式。雅可比矩阵的这种特点使得在计算行列式的时候，计算速度相较于传统的雅可比矩阵要快得多，而且结果简洁完全展开之后多项式的总项数要少的多。

由于四元数中有四个分量且在实际使用中根据对称性，规定ε₀>0，所以有下式成立：

${\mathrm{\ϵ}}_0=\sqrt{1-{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2}---\left(23\right)$

将式(23)代入式(22)就可以得到一个新的奇异轨迹方程，如下式所示：

f(ε₁,ε₂,ε₃,P_X,P_Y,P_Z)＝0 (24)

此外，根据式(13)四元数与Rodriguez参数之间的转换关系，将式(13)带入式(22)就可以得到Rodriguez参数表达的奇异轨迹方程，如下式所示：

f(U,V,W,P_X,P_Y,P_Z)＝0 (25)

无论是式(24)还是式(25)，这两个奇异轨迹方程均可以在给定任意三个位姿变量研究奇异轨迹随其余三个变量的变化关系；或者已知姿态变量时，求解位置奇异轨迹，反之亦然；亦或对一组确定的位姿进行判断该位姿是否处于奇异位形。这两个奇异轨迹方程虽然在本质上是等价的，除位姿变量以外只含有在机构的机构参数完全确定以后为常量的{k₀,k₁,k₂}三个参数，但是式(25)完全展开之后符号表达时只有258项。如果在进行因式分解的情况下进行奇异性判断，计算速度将会进一步提高。

(4)、求解奇异轨迹以及判断奇异性应用示例：

如前所述，本发明所涉及的Stewart并联机构的结构参数可以用θ₁、θ₂、r₁、r₂来描述，现分别取值为π/5、π/9、1、0.618。经过计算可以得出k₀＝1.18812,k₁＝-2.17548,k₂＝-3.62575。

①在已知动平台的姿态变量时，可以通过式(24)与式(25)研究在该姿态条件下，奇异轨迹随位置变量的变化情况。例如：姿态变量采用Rodriguez参数表达且分别取值U＝0.7、V＝0.3、W＝0.4时，奇异轨迹随位置变化的情况如图2所示；通过式(13)求得与Rodriguez参数(U＝0.7、V＝0.3、W＝0.4)对应的四元数为ε₀＝0.758098、ε₁＝0.530669、ε₂＝0.227429、ε₃＝0.303239，其奇异轨迹分布状况如图3所示。奇异轨迹方程均为位置变量的3次多项式。通过对比发现两图是一致的，也说明了方法的正确性。

②在已知动平台的位置变量时，可以通过式(24)与式(25)研究奇异轨迹随姿态变量的变化情况。例如：在位置矢量P＝(2，2，4)时，用Rodriguez参数表达时，姿态奇异轨迹方程是Rodriguez参数U、V、W的6次多项式，奇异轨迹如图4所示；用四元数表达时，奇异轨迹方程是四元数ε₀、ε₁、ε₂、ε₃的8次多项式，奇异轨迹(其中ε₀已经通过进行代换表达)如图5所示。

③如果针对机构的某一具体的位形，则可以将位姿参数带入式(24)或者式(25)进行奇异性判断，若方程成立则机构处于奇异位形。如位置矢量P＝(0，0，5)、U、V、W分别为(0，0，1)时，奇异轨迹方程成立，机构处于奇异位形。

Claims (2)

1.一种Stewart并联机构的奇异性分析方法，所述的Stewart并联机构包括动平台、静平台以及连接动、静平台的6根并联的长度可伸缩的驱动杆，其特征在于，该方法包括如下步骤：

(1)、建立杆长约束的等价方程式：

旋转矩阵采用四元数ε＝(ε₁ ε₂ ε₃ ε₀)^T描述，具体形式如下：

$R=\left[\begin{array}{ccc}{\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2& 2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3& 2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_2\\ 2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3& {\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2& 2{\mathrm{\ϵ}}_2{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_1\\ 2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_2& 2{\mathrm{\ϵ}}_2{\mathrm{\ϵ}}_3+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_1& {\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2+{\mathrm{\ϵ}}_3^2\end{array}\right]$

动平台参考点的位置在静坐标系中的位置矢量P＝[P_x P_y P_z]^T表示；将位置变量(P_x，P_y，P_z)通过姿态变量(ε₁，ε₂，ε₃，ε₀)表达出来，建立杆长约束的等价方程式，具体形式如下：

$P_P=P_{P0}+k_0({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_3^2)$

P_X＝P_x0+2k₁ε₁ε₂

$P_Y=P_{y0}+k_1({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)$

W_X＝W_x0+2k₂ε₁ε₂

$W_Y=W_{y0}+k_2({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)$

2ε₀ε₃＝C₀

式中，为矢量P模的平方；W_x、与W_y分别为矢量P向静坐标系基矢量x与y的投影，具体表达式为，

$W_x=P_x({\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2)+P_y(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3)+P_z(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_2),$

参数{k₀,k₁,k₂}是由动、静平台铰链点参数{r₁,r₂,θ₁,θ₂}决定的常数；{P_P0,P_x0,P_y0,W_x0,W_y0,C₀}在具***姿下是由动、静平台铰链点参数{r₁,r₂,θ₁,θ₂}和杆长l₁～l₆共同决定的常数，具体形式如下：

$P_{P0}=\frac{-6r_1^2-6r_2^2+l_1^2+l_2^2+l_3^2+l_4^2+l_5^2+l_6^2}{6}$

$P_{x0}=\frac{Csc\[{\mathrm{\θ}}_1-{\mathrm{\θ}}_2\]\left(\right(l_1^2-l_2^2-2l_3^2+2l_4^2+l_5^2-l_6^2)Cos\[{\mathrm{\θ}}_2\]+\sqrt{3}(l_1^2+l_2^2-l_5^2-l_6^2)Sin\[{\mathrm{\θ}}_2\])}{12r_1}$

$P_{y0}=\frac{Csc\[{\mathrm{\θ}}_1-{\mathrm{\θ}}_2\]\left(\sqrt{3}\right(l_1^2-l_2^2-l_5^2+l_6^2)Cos\[{\mathrm{\θ}}_2\]-(l_1^2+l_2^2-2l_3^2-2l_4^2+l_5^2+l_6^2)Sin\[{\mathrm{\θ}}_2\])}{12r_1}$

$W_{x0}=\frac{Csc\[{\mathrm{\θ}}_1-{\mathrm{\θ}}_2\]\left(\right(l_1^2-l_2^2-2l_3^2+2l_4^2+l_5^2-l_6^2)Cos\[{\mathrm{\θ}}_1\]+\sqrt{3}(l_1^2+l_2^2-l_5^2-l_6^2)Sin\[{\mathrm{\θ}}_1\])}{12r_2}$

$W_{y0}=\frac{Csc\[{\mathrm{\θ}}_1-{\mathrm{\θ}}_2\]\left(\sqrt{3}\right(l_1^2-l_2^2-l_5^2+l_6^2)Cos\[{\mathrm{\θ}}_1\]-(l_1^2+l_2^2-2l_3^2-2l_4^2+l_5^2+l_6^2)Sin\[{\mathrm{\θ}}_1\])}{12r_2}$

$C_0=\frac{(l_1^2-l_2^2+l_3^2-l_4^2+l_5^2-l_6^2)Csc\[{\mathrm{\θ}}_1-{\mathrm{\θ}}_2\]}{12r_1{r}_2}$

(2)、构建新的雅可比矩阵

基于步骤(1)中6个等价等式以及单位四元数的性质，有如下方程式成立：

$P_{P0}=P_x^2+P_y^2+P_z^2-k_0({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_3^2)$

P_x0＝P_X-2k₁ε₁ε₂

$P_{y0}=P_Y-k_1({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)$

$W_{x0}=P_x({\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2)+P_y(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3)+P_z(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_3-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_2)-2k_2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2$

$W_{y0}=P_x(2{\mathrm{\ϵ}}_1{\mathrm{\ϵ}}_2-2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_3)+P_y({\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2)+P_z(2{\mathrm{\ϵ}}_2{\mathrm{\ϵ}}_3+2{\mathrm{\ϵ}}_0{\mathrm{\ϵ}}_1)-k_2({\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2)$

C₀＝2ε₀ε₃

$1={\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2+{\mathrm{\ϵ}}_3^2$

上式为关于七个变量(P_X P_Y P_Z ε₀ ε₁ ε₂ ε₃)的七个方程式，这七个等价方程对时间求导得到输入与输出之间的映射关系式：

$\[M(P,\mathrm{\ϵ})\]\·\left(\begin{array}{c}{\stackrel{\·}{P}}_X\\ {\stackrel{\·}{P}}_Y\\ {\stackrel{\·}{P}}_Z\\ {\stackrel{\·}{\mathrm{\ϵ}}}_0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_1\\ {\stackrel{\·}{\mathrm{\ϵ}}}_2\\ {\stackrel{\·}{\mathrm{\ϵ}}}_3\end{array}\right)=\left(\begin{array}{c}{M}_l\\ \stackrel{\→}{0}\end{array}\right)\left(\begin{array}{c}{\stackrel{\·}{l}}_1\\ {\stackrel{\·}{l}}_2\\ {\stackrel{\·}{l}}_3\\ {\stackrel{\·}{l}}_4\\ {\stackrel{\·}{l}}_5\\ {\stackrel{\·}{l}}_6\end{array}\right)$

其中M_l∈R^6×6，在杆长给定的情况下M_l(i,j)＝f(l_j)的为一常数；

$M(P,\mathrm{\ϵ})=\left(\begin{array}{ccccccc}{P}_X& P_Y& P_Z& -k_0{\mathrm{\ϵ}}_0& 0& 0& k_0{\mathrm{\ϵ}}_3\\ 1& 0& 0& 0& -2k_1{\mathrm{\ϵ}}_2& -2k_1{\mathrm{\ϵ}}_1& 0\\ 0& 1& 0& 0& -2k_1{\mathrm{\ϵ}}_1& 2k_1{\mathrm{\ϵ}}_2& 0\\ M(4,1)& M(4,2)& M(4,3)& M(4,4)& M(4,5)& M(4,6)& M(4,7)\\ M(5,1)& M(5,2)& M(5,3)& M(5,4)& M(5,5)& M(5,6)& M(5,7)\\ 0& 0& 0& {\mathrm{\ϵ}}_3& 0& 0& {\mathrm{\ϵ}}_0\\ 0& 0& 0& {\mathrm{\ϵ}}_0& {\mathrm{\ϵ}}_1& {\mathrm{\ϵ}}_2& {\mathrm{\ϵ}}_3\end{array}\right)$

$M(4,1)={\mathrm{\ϵ}}_0^2+{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2$

M(4,2)＝2ε₁ε₂+2ε₀ε₃

M(4,3)＝-2ε₀ε₂+2ε₁ε₃

M(4,4)＝2P_Xε₀-2P_Zε₂+2P_Yε₃

M(4,5)＝2P_Xε₁+2P_Yε₂-2k₂ε₂+2P_Zε₃

M(4,6)＝-2P_Zε₀+2P_Yε₁-2k₂ε₁-2P_Xε₂

M(4,7)＝2P_Yε₀+2P_Zε₁-2P_Xε₃

M(5,1)＝2ε₁ε₂-2ε₀ε₃

$M(5,2)={\mathrm{\ϵ}}_0^2-{\mathrm{\ϵ}}_1^2+{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2$

M(5,3)＝2ε₀ε₁+2ε₂ε₃

M(5,4)＝2P_Yε₀+2P_Zε₁-2P_Xε₃

M(5,5)＝2P_Zε₀-2P_Yε₁-2k₂ε₁+2P_Xε₂

M(5,6)＝2P_Xε₁+2P_Yε₂+2k₂ε₂+2P_Zε₃

M(5,7)＝-2PXε₀+2P_Zε₂-2P_Yε₃

M(P,ε)为驱动杆的杆长变化率，也就是驱动速率与动平台位置姿态变化率之间的映射变换矩阵，即雅可比矩阵；

(3)、建立奇异轨迹方程

当机构处于奇异位形时，雅可比矩阵的行列式应该为零，即可建立奇异轨迹方程；

f＝Det[M(P,ε)]＝0

由于四元数中有四个分量且在实际使用中根据对称性，规定ε₀>0，所以有下式成立：

${\mathrm{\ϵ}}_0=\sqrt{1-{\mathrm{\ϵ}}_1^2-{\mathrm{\ϵ}}_2^2-{\mathrm{\ϵ}}_3^2}$

将上式代入奇异轨迹方程就可以得到一个新的只含有6个变量的奇异轨迹方程，其中姿态参数是用四元数表示的，如下式所示：

f(ε₁,ε₂,ε₃,P_X,P_Y,P_Z)＝0

此外，可以通过转换将姿态参数用Rodriguez参数表示，就可以得到Rodriguez参数表达的奇异轨迹方程，如下式所示：

f(U,V,W,P_X,P_Y,P_Z)＝0

(4)、求解奇异轨迹以及判断奇异性：

依据(3)中得到的奇异轨迹方程，采用姿态参数用四元数表示或者采用Rodriguez参数表示，而在给定任意三个位姿变量研究奇异轨迹随其余三个变量的变化关系；或者已知姿态变量时，求解位置奇异轨迹；或者已知位置奇异轨迹时，求解姿态变量；或对一组确定的位姿进行判断该位姿是否处于奇异位形。

2.根据权利要求1所述的Stewart并联机构的奇异性分析方法，其特征在于：在建立杆长约束的等价方程式时，旋转矩阵直接采用Rodriguez参数描述。