CN110186661A - 含up副支链的并联机构的运动学正解求解方法 - Google Patents

Abstract

本发明公开一种含UP副支链的并联机构的运动学正解求解方法，其包括以下步骤：S1：在UP支链的U副的两个转轴上安装角度传感器；S2：建立分支坐标系；S3：建立定坐标系和动坐标系；S4：在初始状态各坐标系位姿关系已知的条件下，建立它们之间的位姿坐标关系；S5：构建并联机构各个分支杆长约束方程；S6：结合U副上的角度传感器测得的数据，分析UP支链，确定动平台上的分支坐标系原点在空间的位置矢量；S7：根据闭环矢量方程，构造分支杆长表达式；S8：结合S5构造并联机构的求解位置正解方程组；S9：求出位置正解求解式；S10：得到位置正解唯一解。本发明所提方法通过测量U副的转角值，可将高次方程简化，整个求解过程简单高效、具有普遍适用性。

Description

含UP副支链的并联机构的运动学正解求解方法

技术领域

本发明涉及并联机构技术领域，具体涉及一种含UP副支链的并联机构的运动学正解求解方法。

背景技术

并联机构具有承载能力大、结构稳定、定位精度高和系统动态响应快等优点，在车辆轮胎检测机构、飞行驾驶训练模拟器、新型机床、娱乐和医疗设备等领域获得了广泛应用。国内外很多学者都对并联机构运动学正解进行了大量研究。但是，不难发现这些研究对象大多为特殊构型的并联机构，一般构型的并联机构运动学正解仍存在一些问题。

对于并联机构运动学分析，目前主要有数值法、解析法、基于智能算法的搜索算法和附加传感器法，其中数值法计算速度较慢、不能求得所有解，且当初值范围选取不当时，甚至得不到确定的位置正解。通过上述方法得到的正解方程一般为一元20次代数方程，而且求解过程中会出现复数根和无效根。有的研究学者分别通过添加多个测量杆长传感器的辅助方法来快速得到位姿，但同时也大大增加了硬件成本；此外，还有学者通过在并联机构中心位置添加1个位移传感器，来求得并联机构的运动学唯一正解，但当并联机构中心空间位置有其他用途或并联机构内部安装空间受限时，该方法并不适用。

现有的求解并联机构运动学正解的方法，均通过使用现代数学工具等进行求解，其求解过程十分繁琐，甚至会出现高次方程组，使求解变得极其困难，更有甚者，无法求出计算结果，计算成本很大，随着并联机构在生产实际中的应用，现急需一种可以快速有效地求解并联机构正解的方法。

发明内容

针对以上情况，本发明提供一种含UP副支链的并联机构的运动学正解求解方法，对于含有UP支链的并联机构，将动平台相对定平台的位姿关系转换为UP支链上的分支坐标系间的关系，通过在与定平台连接的U副的两个转动轴处安装角度传感器，测量U副的转动角数值，从而将原来位置正解方程组的一元高次方程简化为一元二次方程，在求得中间变量后可逐步求得其他姿态参量，最终得到唯一的姿态矩阵，整个求解过程无高次运算、简单高效。

本发明所采用的技术方案是提供一种含UP副支链的并联机构的运动学正解求解方法，所述并联机构包括定平台、动平台和连接所述定平台和动平台的多个含UP支链的分支，UP支链的U副与定平台连接，具体包括以下步骤：

S1：在UP支链的U副的两个转轴上安装角度传感器；

S2：建立分支坐标系X₁Y₁Z₁和分支坐标系X₂Y₂Z₂，所述分支坐标系X₁Y₁Z₁的原点选于所述UP支链U副的中心，所述分支坐标系X₂Y₂Z₂的原点设置于与所述动平台连接的运动副的中心或轴线之上，且所述分支坐标系X₂Y₂Z₂与建立于动平台上的动坐标系X_bY_bZ_b的姿态关系保持不变；

S3：建立定坐标系X_BY_BZ_B和动坐标系X_bY_bZ_b，且所述定坐标系X_BY_BZ_B与含UP副支链的并联机构的定平台固连，所述动坐标系X_bY_bZ_b与含UP副支链的并联机构的动平台固连；

S4：在初始状态所述分支坐标系X₁Y₁Z₁、分支坐标系X₂Y₂Z₂、定坐标系X_BY_BZ_B和动坐标系X_bY_bZ_b之间的位姿关系已知的条件下，建立所有坐标系间的位姿坐标关系；

S5：旋转矩阵¹R₂为3x3的方向余弦矩阵，其各列分别为l、m、n，且满足约束条件：l·l＝m·m＝n·n＝1，l·m＝m·n＝n·l＝0，l×m＝n，m×n＝l，n×l＝m，以此，作为求解含UP支链的支链杆长方程组的约束条件；

S6：建立所述含UP支链的分支相对于分支坐标系X₁Y₁Z₁的姿态变换矩阵，结合所述U副上的角度传感器测得的转动角α和β的数据，确定分支坐标系X₂Y₂Z₂的原点在分支坐标系X₁Y₁Z₁下的位置矢量¹P₂＝[x,y,z]^T；

S7：根据各个支链满足的闭环矢量方程，得出各个支链的杆长表达式；

S8：结合S5建立并联机构的求解位置正解方程组；

S9：化简所述位置正解方程组，得到位置正解求解式，并求出旋转矩阵¹R₂中的所有元素；

S10：得到含UP副支链的并联机构的动平台的位置正解唯一解，包括姿态矩阵^BR_b和位置矢量^BP_b，且^BR_b＝¹R₂，^BP_b＝¹P₂。

优选地，所述分支坐标系X₁Y₁Z₁的原点选在U副的中心，X₁轴沿所述U副的第一转轴的轴线方向，Y₁轴沿所述U副的第二转轴的轴线方向，Z₁轴方向根据右手法则确定，且所述第一转轴与定平台相连，所述第二转轴与UP支链相连，定坐标系X_BY_BZ_B的原点选于定平台的几何中心，且定坐标系X_BY_BZ_B的姿态与分支坐标系X₁Y₁Z₁的初始姿态相同，所述动坐标系X_bY_bZ_b的原点选于动平台的几何中心，且动坐标系X_bY_bZ_b的姿态与分支坐标系X₂Y₂Z₂的姿态始终相同。

优选地，若所述UP支链与动平台通过球副S连接，分支坐标系X₂Y₂Z₂的原点选于球副S的中心，若所述UP支链与动平台通过转动副R连接，分支坐标系X₂Y₂Z₂的原点选于转动副的轴线之上。

优选地，所述步骤S4中所有坐标系间的位姿坐标关系为：^BR_b ^BP_b＝^BP₁ ¹R₂ ¹P₂ ²P_b；

式中，^BR_b为动坐标系X_bY_bZ_b相对于定坐标系X_BY_BZ_B的姿态矩阵，^BP_b为动坐标系X_bY_bZ_b相对于定坐标系X_BY_BZ_B的位置矢量，^BP₁为分支坐标系X₁Y₁Z₁原点相对于定坐标系X_BY_BZ_B原点的位置矢量，¹R₂为分支坐标系X₂Y₂Z₂相对于分支坐标系X₁Y₁Z₁的旋转矩阵，¹P₂为分支坐标系X₂Y₂Z₂相对于分支坐标系X₁Y₁Z₁的位置矢量，²P_b为动坐标系X_bY_bZ_b相对于分支坐标系X₂Y₂Z₂的位置矢量，其中，

式中，存在关系l＝[l_x,l_y,l_z]^T，m＝[m_x,m_y,m_z]^T，n＝[n_x,n_y,n_z]^T，l_x、m_x、l_y、n_x、n_y、n_z、m_y、l_z和m_z是旋转矩阵¹R₂中的元素，x、y和z是分支坐标系X₂Y₂Z₂相对于分支坐标系X₁Y₁Z₁的位置矢量元素。

优选地，所述步骤S6中的姿态变换矩阵具体如下：

所述步骤S6中的位置矢量具体如下：

式中，sα＝sinα，cα＝cosα，sβ＝sinβ，cβ＝cosβ，l₁为含UP副的支链的长度，为已知量，α为支链绕X₁轴转动的角度，转动后得到新的坐标系为X'Y'Z'，β为绕Y′轴转动的角度，且α和β通过所述角度传感器测得。

优选地，所述步骤S7中的各个分支满足的闭环矢量方程为：l_is_i＝Ra_i+P-b_i；

式中，a_i为定坐标系X_BY_BZ_B中分支坐标系X₁Y₁Z₁原点的坐标，l_i为第i个支链的杆长，s_i为第i个支链的方向单位矢量，P为动坐标系X_bY_bZ_b在定坐标系X_BY_BZ_B的位置矢量，b_i为分支坐标系X₂Y₂Z₂的原点在动坐标系X_bY_bZ_b中的坐标，R为动坐标系X_bY_bZ_b在定坐标系X_BY_BZ_B的姿态变换矩阵。

优选地，将分支满足的闭环矢量方程展开并整理，便可得到所述步骤S7中的各个分支杆长方程表达式：

式中，i＝1,2…n，n为并联机构的支链数目，l_i为并联机构中第i个分支的支链长度，a_i和b_i分别为U副在分支坐标系X₁Y₁Z₁的X₁轴和Y₁轴的坐标，p_i和q_i分别为连接动平台的运动副的中心在分支坐标系X₂Y₂Z₂的X₂轴和Y₂轴的坐标，l_x、m_x、l_y、m_y、l_z和m_z是旋转矩阵¹R₂中的元素，x、y和z是分支坐标系X₂Y₂Z₂相对于分支坐标系X₁Y₁Z₁的位置矢量，即所述位置矢量¹P₂中的元素。

优选地，所述步骤S8中的位置正解方程组如下：

式中，A_i、B_i、C_i、D_i、E_i均为中间变量，i＝2,3…6，且为常数，各中间变量具体为：

式中，l_x、l_y、l_z、m_x、m_y、m_z、w₁、w₂为未知参量。

优选地，所述步骤S9中的位置正解求解式如下：

本发明与现有技术相比具有以下优点：

1、本发明所提方法对于含有UP副支链的并联机构的运动学正解求解方法具有普遍适用性，无论是少自由度并联机构、冗余自由度并联机构还是全自由度并联机构均适用；

2、本发明所提方法通过测量U副的转动角数值，可将原来位置正解方程组的一元高次方程简化为一元二次方程，在求得中间变量后可逐步求得其他姿态参量，最终得到唯一的姿态矩阵，整个求解过程无高次运算、简单高效；

3、本发明所提方法在分支虎克铰处添加角度传感器，不仅避免了并联机构平台中心空间位置另有用途及安装空间有限等问题，而且可推广到所有分支通过U副与定平台连接的并联机构中；

4、本发明所提方法可将求解动坐标系X_bY_bZ_b相对于定坐标系X_BY_BZ_B的位置和姿态转化为求解分支坐标系X₂Y₂Z₂相对于坐标系X₁Y₁Z₁的位置和姿态。

附图说明

图1为本发明的正解求解方法流程图；

图2为本发明的实施例机构简图；

图3为本发明的实施例的坐标系间变换关系示意图；

图4为本发明的实施例的分支坐标系原理图；

图5为本发明的实施例的仿真位姿示意图；以及

图6为本发明的实施例的一元二次方程组的公共解曲线图。

具体实施方式

为详尽本发明之技术内容、结构特征、所达成目的及功效，以下将结合说明书附图进行详细说明。

本发明提供一种含UP副支链的并联机构的运动学正解求解方法，如图1所示，包括以下步骤：

S1：在UP支链的U副的两个转轴上安装角度传感器；

S2：建立分支坐标系X₁Y₁Z₁和分支坐标系X₂Y₂Z₂，分支坐标系X₁Y₁Z₁的原点选于UP支链U副的中心，且U副与定平台连接，分支坐标系X₂Y₂Z₂的原点设置于与动平台连接的运动副的中心或轴线之上，且分支坐标系X₂Y₂Z₂与建立于动平台上的动坐标系的姿态关系保持不变；

S3：建立定坐标系X_BY_BZ_B和动坐标系X_bY_bZ_b，且定坐标系X_BY_BZ_B与含UP副支链的并联机构的定平台固连，动坐标系X_bY_bZ_b与含UP副支链的并联机构的动平台固连；

S4：在初始状态分支坐标系X₁Y₁Z₁、分支坐标系X₂Y₂Z₂、定坐标系X_BY_BZ_B和动坐标系X_bY_bZ_b之间的位姿关系已知的条件下，建立所有坐标系间的位姿坐标关系；

S5：旋转矩阵¹R₂为3x3的方向余弦矩阵，其各列分别为l、m、n，且满足约束条件：l·l＝m·m＝n·n＝1，l·m＝m·n＝n·l＝0，l×m＝n，m×n＝l，n×l＝m，以此，作为求解含UP支链的支链杆长方程组的约束条件；

S6：确定含UP支链的分支相对于分支坐标系X₁Y₁Z₁的姿态变换矩阵，结合U副上的角度传感器测得的转动角α和β的数据，确定分支坐标系X₂Y₂Z₂的原点在分支坐标系X₁Y₁Z₁下的位置矢量¹P₂＝[x,y,z]^T；

S7：根据各个分支满足的闭环矢量方程，得出各个支链的杆长表达式；

S8：结合S5构造并联机构的求解位置正解方程组；

S9：化简位置正解方程组，得到位置正解求解式，并求出旋转矩阵¹R₂中的所有元素；

S10：得到含UP副支链的并联机构的动平台的位置正解唯一解，包括姿态矩阵^BR_b和位置矢量^BP_b，且^BR_b＝¹R₂，^BP_b＝¹P₂。

实施例1：

以结构对称的6-UPS并联机构为例，对本发明所提方法的具体实施过程进行说明：

如图2和图3所示，对称结构的6-UPS并联机构由定平台1、动平台2和六个完全相同的UP支链3构成，定平台1和动平台2的连接点中心B_i和b_i(i＝1,2,…6)分别位于定平台1和动平台2所在的平面上。

S1：在UP支链7的U副4的第一转轴41上安装第一角度传感器51，第二转轴42上安装第二角度传感器52。

S2：建立分支坐标系X₁Y₁Z₁和分支坐标系X₂Y₂Z₂。分支坐标系按照如下规则建立：每个U副4均包含两个相互垂直的转动副，规定与定平台1相连的转动副转轴为第一转轴41，与UP支链7相连的转动副转轴为第二转轴42。分支坐标系X₁Y₁Z₁的原点选在第一分支的U副4中心，U副4的安装方式及坐标系建立规则为，X₁轴沿第一转轴41的轴线方向，且与B₁B₂平行，Y₁轴沿第二转轴42的轴线方向，Z₁轴方向根据右手法则确定，分支坐标系X₂Y₂Z₂以动平台2球铰S的中心为坐标原点，其各坐标轴建立与分支坐标系X₁Y₁Z₁类似。

S3：建立定坐标系X_BY_BZ_B和动坐标系X_bY_bZ_b，且定坐标系X_BY_BZ_B与定平台1固连，与分支坐标系X₁Y₁Z₁的姿态相同，动坐标系X_bY_bZ_b与动平台2固连，与分支坐标系X₂Y₂Z₂的姿态相同。

S4：在初始状态分支坐标系X₁Y₁Z₁、分支坐标系X₂Y₂Z₂、定坐标系X_BY_BZ_B和动坐标系X_bY_bZ_b之间的位姿关系已知的条件下，建立它们之间的位姿坐标关系。

采用变换矩阵来表示姿态矩阵，由于定坐标系X_BY_BZ_B和分支坐标系X₁Y₁Z₁固连在定平台1上，动坐标系X_bY_bZ_b和分支坐标系X₂Y₂Z₂固连在动平台2上，且初始条件下各坐标系姿态相同，运动过程中各坐标系之间存在如下变换关系：

1)定坐标系X_BY_BZ_B与动坐标系X_bY_bZ_b间存在变换：r_B＝^BR_b ^BP_br_b；

2)定坐标系X_BY_BZ_B与分支坐标系X₁Y₁Z₁间存在变换：r_B＝^BP₁r₁，其中，^BP₁＝[^BP_1,x ^BP_1,y ^BP_1,z]^T；

3)分支坐标系X₁Y₁Z₁与分支坐标系X₂Y₂Z₂间存在变换：r₁＝¹R₂ ¹P₂r₂，其中，

4)动坐标系X_bY_bZ_b与分支坐标系X₂Y₂Z₂间存在变换：r₂＝²P_br_b，其中，²P_b＝[²P_b,x ²P_b,y ²P_b,z]^T。

由于定坐标系X_BY_BZ_B既可以直接通过^BR_b ^BP_b变换到动坐标系X_bY_bZ_b中，也可通过中间分支坐标系X₁Y₁Z₁和分支坐标系X₂Y₂Z₂变换得到，故存在关系：

^BR_b ^BP_b＝^BP₁ ¹R₂ ¹P₂ ²P_b (2)

将上式右侧展开，由于两种表示方式表征的是同一位姿，故表达式两侧对应项相等，因此

由于在给定坐标系后,^BP₁和²P_b是仅与机构构型参数相关的常量，因此，求解动坐标系X_bY_bZ_b在定坐标系X_BY_BZ_B下的位置^BP_b和姿态^BR_b，可转化为求解分支坐标系X₂Y₂Z₂在分支坐标系X₁Y₁Z₁下位置¹P₂和姿态¹R₂。

S5：旋转矩阵¹R₂是一个3×3的方向余弦矩阵，其各列分别为l、m、n，由旋转矩阵的单位正交性可知，各列满足以下单位正交条件：

l·l＝m·m＝n·n＝1 (7)；

l·m＝m·n＝n·l＝0 (8)；

l×m＝n，m×n＝l，n×l＝m (9)；

以此，作为求解含UP支链的支链杆长方程约束条件。

S6：建立含UP支链的分支相对于分支坐标系X₁Y₁Z₁的姿态变换矩阵，结合U副4上的第一角度传感器51和第二角度传感器52测得的数据，确定分支坐标系X₂Y₂Z₂的原点在分支坐标系X₁Y₁Z₁下的位置矢量¹P₂＝[x,y,z]^T。如图4所示，分支L₁的姿态可看作是由分支坐标系系X₁Y₁Z₁通过两个欧拉角变换得到，具体表述为：首先通过绕X轴转动α到达O'-X'Y'Z'，然后绕Y'轴转动β到达O”-X”Y”Z”，从而得到分支L_i相对于分支坐标系X₁Y₁Z₁的姿态变换矩阵¹R_i的表达式：

其中，sα＝sinα，cα＝cosα。

该矩阵的最后一列即为分支L_i的单位方向矢量，当各分支长度给定后，即分支坐标系X₂Y₂Z₂原点(S副中心)在分支坐标系X₁Y₁Z₁下的位置矢量为：

S7：根据各个分支满足的闭环矢量方程，得出各个分支的杆长表达式。

由图2可知，每个运动支链均满足闭环矢量方程：

l_is_i＝Ra_i+P-b_i (1)

式中，a_i为定坐标系X_BY_BZ_B中各B_i点的坐标；l_i为第i个支链的杆长；s_i为第i个支链的方向单位矢量；P为动坐标系X_bY_bZ_b在定坐标系X_BY_BZ_B的位置矢量；b_i为动坐标系X_bY_bZ_b中各点b_i的坐标；R为动坐标系X_bY_bZ_b在定坐标系X_BY_BZ_B的位姿变换矩阵。

定平台1的U副4的B_i在分支坐标系X₁Y₁Z₁下的坐标为：

动平台2的球面副S_i在分支坐标系X₂Y₂Z₂中的坐标为：

其中，a₁＝b₁＝b₂＝p₁＝q₁＝q₂＝0。

通过坐标变换得到球面副S_i在分支坐标系X₁Y₁Z₁下的坐标为：

则各分支杆长的表达式为：

S8：结合S5构造并联机构的求解位置正解方程组。6-UPS并联机构位置正解是已知杆长求解P和R中的12个元素，因此在现有结构基础上，在B₁处的U副4处附加两个角度传感器测得分支L₁的两个转动角α、β，通过增加约束条件来确定唯一位置正解。当给定杆长l₁及测量出转动角α、β后，可唯一确定位置矢量¹P₂，通过在分支坐标系X₁Y₁Z₁下构造方程组来求解¹R₂。

现分别讨论各分支杆长表达式：

1)当i＝1时，由于a₁＝b₁＝p₁＝q₁＝0，因此，式(13)可表达为：

附加传感器情况下，给定位置正解输入参量杆长l₁后，通过式(6)可唯一确定位置矢量x、y和z等3个参量。

2)当i＝2,3,…6时，将杆长表达式展开，结合式(7)～(9)通过引入中间变量A_i、B_i、C_i、D_i、E_i，可将式(13)简化为以下形式：

p_iw₁+q_iw₂-C_im_x＝A_il_x+B_il_y+D_im_y+E_i (15)

其中，A_i、B_i、C_i、D_i、E_i均为常数，各中间参量的具体表达式为：

此时，存在未知参量l_x、l_y、l_z、m_x、m_y、m_z、w₁、w₂，由于未知参量均为旋转矩阵元素，满足式(8)～(9)给出的正交性关系，因此，式(15)可简化为一个由6个未知数和5个方程组成的线性方程组，选择m_y作为基本变量，则其余5个未知参量可用其表示为：

可将上式化简为以下的线性方程形式：

w₁＝F₁m_y+G₁ (18)

w₂＝F₂m_y+G₂ (19)

m_x＝F₃m_y+G₃ (20)

l_x＝F₄m_y+G₄ (21)

l_y＝F₅m_y+G₅ (22)

其中，F_i、G_i(i＝1,2,…5)均为常数，其值可结合式(16)～(17)通过矩阵运算求得。另外两个未知数l_z、m_z可通过式(7)表示为：

由式(7)～(9)及(16)可知，还可以表示为：

l_zm_z＝-(l_xm_x+l_ym_y) (25)

l_zz＝w₁-l_xx-l_yy (26)

m_zz＝w₂-m_xx-m_yy (27)

S9：求解旋转矩阵中各位姿参数，利用式(23)～(27)构造以下3个恒等式：

(l_zm_z)(z²)-(l_zz)(m_zz)＝0 (30)

将式(18)～(27)代入式(28)～(30)，计算展开后可以发现，式(28)～(30)均为基本变量m_y的一元二次函数，因此，从上述3个恒等式中任取2个构成一元二次方程组进行求解，即可求出m_y的唯一解。利用式(20)～(24)可分别求得l_x、l_y、m_x、l_z和m_z的值，从而，再由式(9)依次求得n_x、n_y、n_z，至此，旋转矩阵中9个参量全部求出。结合式(4)和(6)可唯一确定表征运动平台位置正解的12个参量。

S10：得到含UP支链7的并联机构动平台2的位置正解唯一解,即表征运动平台位置正解的12个参量全部求出。

针对实施例1进行误差分析：

采用具体的仿真模型验证上述方法的正确性，构型参数如下：定平台1的U副4的B_i(i＝1,2,…6)在分支坐标系X₁Y₁Z₁下的坐标为：

B₁＝[0 0 0]^Tmm

B₂＝[46.59 0 0]^Tmm

B₃＝[110.23 110.23 0]^Tmm

B₄＝[86.93 150.57 0]^Tmm

B₅＝[-40.35 150.57 0]^Tmm

B₆＝[-63.64 110.28 0]^Tmm

动平台2的球面副S_i在分支坐标系X₂Y₂Z₂中的表示为：

b₁＝[0 0 0]^Tmm

b₂＝[33.65 0 0]^Tmm

b₃＝[79.61 79.61 0]^Tmm

b₄＝[62.79 108.75 0]^Tmm

b₅＝[-29.14 108.75 0]^Tmm

b₆＝[-45.97 79.61 0]^Tmm

分支坐标系X₂Y₂Z₂在分支坐标系X₁Y₁Z₁下的初始位置向量为[52.82 7.86 152]^Tmm。

在分支坐标系X₁Y₁Z₁下，将平台中心相对于定平台中心x向移动6mm、y向移动7mm和绕y轴旋转20°时的位姿作为验证位姿，将此时仿真模型的分支杆长作为正解的输入参量，求得分支坐标系X₂Y₂Z₂原点(S副中心)在分支坐标系X₁Y₁Z₁下的位置矢量为：

上述位姿下，分析模型的姿态在分支坐标系X₁Y₁Z₁下的理论姿态矩阵为：

分析模型的验证位姿如图5所示。

将上述l_i、x、y、z代入正解求解方程中，求出方程组各中间参数的数值，其值如表1所示。

在式(28)～(30)所示的3个姿态矩阵元素关系恒等式中，可任取两个一元二次方程构成方程组进行m_y值的求解，选取式(29)和(30)构成如下求解方程组：

表1：

通过联立式(29)和(30)，代入表1中各中间参数数值，利用数值计算软件编程，进行方程组的求解计算，解得满足条件的m_y唯一解。

通过编程求得的该一元二次方程组的公共解曲线图，如图6所示。分析可知，在m_y的合理取值范围内，方程存在唯一解，其值为0.99863，即在给定验证位姿下，所求得的m_y有且只有唯一解。根据式(28)～(30)可唯一确定其他姿态矩阵元素参数，从而求得位置正解唯一解。

由理论姿态矩阵可知，验证位姿下，理论位姿矩阵中m_y元素的理论值为1，利用本发明所提方法解得m_y的值为0.99863，与理论值之间的相对误差为0.137％，在误差允许范围之内。

通过上述计算分析后，发现：本发明所提方法简单高效，能够有效地将高次方程进行降次，很大程度上提高了正解的求解速度，对含UP副支链的并联机构的实际应用具有极大地推动作用。

以上所述是本申请的优选实施方式，不以此限定本发明的保护范围，应当指出，对于该技术领域的普通技术人员来说，在不脱离本技术原理前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本申请的保护范围。

Claims (9)

1.一种含UP副支链的并联机构的运动学正解求解方法，所述并联机构包括定平台、动平台和连接所述定平台和动平台的多个含UP支链的分支，UP支链的U副与定平台连接，其特征在于，其包括以下步骤：

S1：在UP支链的U副的两个转轴上安装角度传感器；

S2：建立分支坐标系X₁Y₁Z₁和分支坐标系X₂Y₂Z₂，所述分支坐标系X₁Y₁Z₁的原点选于所述UP支链U副的中心，所述分支坐标系X₂Y₂Z₂的原点设置于与所述动平台连接的运动副的中心或轴线之上，且所述分支坐标系X₂Y₂Z₂与建立于动平台上的动坐标系X_bY_bZ_b的姿态关系保持不变；

S3：建立定坐标系X_BY_BZ_B和动坐标系X_bY_bZ_b，且所述定坐标系X_BY_BZ_B与含UP副支链的并联机构的定平台固连，所述动坐标系X_bY_bZ_b与含UP副支链的并联机构的动平台固连；

S4：在初始状态所述分支坐标系X₁Y₁Z₁、分支坐标系X₂Y₂Z₂、定坐标系X_BY_BZ_B和动坐标系X_bY_bZ_b之间的位姿关系已知的条件下，建立所有坐标系间的位姿坐标关系；

S5：旋转矩阵¹R₂为3x3的方向余弦矩阵，其各列分别为l、m、n，且满足约束条件：l·l＝m·m＝n·n＝1，l·m＝m·n＝n·l＝0，l×m＝n，m×n＝l，n×l＝m，以此，作为求解含UP支链的支链杆长方程组的约束条件；

S6：建立所述含UP支链的分支相对于分支坐标系X₁Y₁Z₁的姿态变换矩阵，结合所述U副上的角度传感器测得的转动角α和β的数据，确定分支坐标系X₂Y₂Z₂的原点在分支坐标系X₁Y₁Z₁下的位置矢量¹P₂＝[x,y,z]^T；

S7：根据各个支链满足的闭环矢量方程，得出各个支链的杆长表达式；

S8：结合S5建立并联机构的求解位置正解方程组；

S9：化简所述位置正解方程组，得到位置正解求解式，并求出旋转矩阵¹R₂中的所有元素；以及

S10：得到含UP副支链的并联机构的动平台的位置正解唯一解，包括姿态矩阵^BR_b和位置矢量^BP_b，且^BR_b＝¹R₂，^BP_b＝¹P₂。

2.根据权利要求1所述的含UP副支链的并联机构的运动学正解求解方法，其特征在于，所述分支坐标系X₁Y₁Z₁的原点选在U副的中心，X₁轴沿所述U副的第一转轴的轴线方向，Y₁轴沿所述U副的第二转轴的轴线方向，Z₁轴方向根据右手法则确定，且所述第一转轴与定平台相连，所述第二转轴与UP支链相连，定坐标系X_BY_BZ_B的原点选于定平台的几何中心，且定坐标系X_BY_BZ_B的姿态与分支坐标系X₁Y₁Z₁的初始姿态相同，所述动坐标系X_bY_bZ_b的原点选于动平台的几何中心，且动坐标系X_bY_bZ_b的姿态与分支坐标系X₂Y₂Z₂的姿态始终相同。

3.根据权利要求1所述的含UP副支链的并联机构的运动学正解求解方法，其特征在于，若所述UP支链与动平台通过球副S连接，分支坐标系X₂Y₂Z₂的原点选于球副S的中心，若所述UP支链与动平台通过转动副R连接，分支坐标系X₂Y₂Z₂的原点选于转动副的轴线之上。

4.根据权利要求1所述的含UP副支链的并联机构的运动学正解求解方法，其特征在于，所述步骤S4中所有坐标系间的位姿坐标关系为：^BR_b ^BP_b＝^BP₁ ¹R₂ ¹P₂ ²P_b；

式中，^BR_b为动坐标系X_bY_bZ_b相对于定坐标系X_BY_BZ_B的姿态矩阵，^BP_b为动坐标系X_bY_bZ_b相对于定坐标系X_BY_BZ_B的位置矢量，^BP₁为分支坐标系X₁Y₁Z₁原点相对于定坐标系X_BY_BZ_B原点的位置矢量，¹R₂为分支坐标系X₂Y₂Z₂相对于分支坐标系X₁Y₁Z₁的旋转矩阵，¹P₂为分支坐标系X₂Y₂Z₂相对于分支坐标系X₁Y₁Z₁的位置矢量，²P_b为动坐标系X_bY_bZ_b相对于分支坐标系X₂Y₂Z₂的位置矢量，其中，

式中，存在关系l＝[l_x,l_y,l_z]^T，m＝[m_x,m_y,m_z]^T，n＝[n_x,n_y,n_z]^T，l_x、m_x、l_y、m_y、l_z、m_z、n_x、n_y和n_z是旋转矩阵¹R₂中的元素，x、y和z是分支坐标系X₂Y₂Z₂相对于分支坐标系X₁Y₁Z₁的位置矢量元素。

5.根据权利要求4所述的含UP副支链的并联机构的运动学正解求解方法，其特征在于，所述步骤S6中的姿态变换矩阵具体如下：

所述步骤S6中的位置矢量具体如下：

式中，sα＝sinα，cα＝cosα，sβ＝sinβ，cβ＝cosβ，l₁为含UP副的支链的长度，为已知量，α为支链绕X₁轴转动的角度，转动后得到新的坐标系为X'Y'Z'，β为绕Y′轴转动的角度，且α和β通过所述角度传感器测得。

6.根据权利要求5所述的含UP副支链的并联机构的运动学正解求解方法，其特征在于，所述步骤S7中的各个分支满足的闭环矢量方程为：l_is_i＝Ra_i+P-b_i；

式中，a_i为定坐标系X_BY_BZ_B中分支坐标系X₁Y₁Z₁原点的坐标，l_i为第i个支链的杆长，s_i为第i个支链的方向单位矢量，P为动坐标系X_bY_bZ_b在定坐标系X_BY_BZ_B的位置矢量，b_i为分支坐标系X₂Y₂Z₂的原点在动坐标系X_bY_bZ_b中的坐标，R为动坐标系X_bY_bZ_b在定坐标系X_BY_BZ_B的姿态变换矩阵。

7.根据权利要求6所述的含UP副支链的并联机构的运动学正解求解方法，其特征在于，将分支满足的闭环矢量方程展开并整理，便可得到所述步骤S7中的各个分支杆长方程表达式：

式中，i＝1,2...n，n为并联机构的支链数目，l_i为并联机构中第i个分支的支链长度，a_i和b_i分别为U副在分支坐标系X₁Y₁Z₁的X₁轴和Y₁轴的坐标，p_i和q_i分别为连接动平台的运动副的中心在分支坐标系X₂Y₂Z₂的X₂轴和Y₂轴的坐标，l_x、m_x、l_y、m_y、l_z和m_z是旋转矩阵¹R₂中的元素，x、y和z是分支坐标系X₂Y₂Z₂相对于分支坐标系X₁Y₁Z₁的位置矢量，即所述位置矢量¹P₂中的元素。

8.根据权利要求7所述的含UP副支链的并联机构的运动学正解求解方法，其特征在于，所述步骤S8中的位置正解方程组如下：

式中，A_i、B_i、C_i、D_i、E_i均为中间变量，i＝2,3...6，且为常数，各中间变量具体为：

式中，l_x、l_y、l_z、m_x、m_y、m_z、w₁、w₂为未知参量。

9.根据权利要求8所述的含UP副支链的并联机构的运动学正解求解方法，其特征在于，所述步骤S9中的位置正解求解式如下：