CN104933232A - 一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法 - Google Patents

Abstract

本发明针对一类由螺母可旋转(称之为杆端浮动)的滚珠丝杠副作为主动关节的六自由度并联机器人，公开一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法：首先，对于给定的机器人末端位姿(位置和姿态)向量，计算出各个丝杠与其螺母之间的相对旋转角度以及由该旋转角引起的附加杆长，给出带角度补偿的逆向运动学求解方法；其次，考虑丝杠与螺母的相对旋转，在逆向运动学求解方法的基础上，利用高效的数值迭代解法给出由给定杆长计算机器人末端位姿的运动学正向求解方法。本发明可以解决滚珠丝杠副螺母旋转对六自由度并联机器人运动学计算的影响，具有精度高收敛速度快的优点，利于实现该型并联机器人的高精度实时轨线跟踪控制。

Description

一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法

【技术领域】

本发明属于自动化领域，涉及一类杆端浮动型六自由度并联机器人的运动学求解方法。

【背景技术】

并联机器人是一类具有若干个并行连接的运动支链的智能化机械装备，与串联机器人相比，它的承载能力更高，结构刚性更好，累积误差也更小。六自由度并联机器人由于能够在三维空间中实现“3自由度平动+3自由度转动”的复合运动，因此在机械加工与制造、航空与航天、通信与自动控制等诸多领域中应用广泛。

从结构组成来看，六自由度并联机器人通常由一个固定平台，一个运动平台，以及以并行方式连接在两个平台之间的6根伸缩杆组成。伸缩杆通常由电机驱动的滚珠丝杠副构成，丝杠副的两端则通过球铰关节分别与固定平台和运动平台相连接。然而由于球铰关节的运动范围相对较小，极大地限制了并联机器人的工作空间，因此可以采用运动范围较大的虎克铰关节与旋转关节配合来代替杆端的球铰关节。为了避免额外增加旋转关节，还可以利用螺母可旋转的滚珠丝杠副(称之为杆端浮动)与虎克铰配合(如图1所示)，等效替代连接伸缩杆与运动平台的球铰关节。采用上述机构设计的杆端浮动型六自由度并联机器人，为了实现高精度的实时轨线跟踪控制的目的，其运动学求解需要特别关注由于螺母旋转对杆长变化量的影响。

并联机器人实时和高精度的运动学解算是实现机器人快速精确运动控制的前提和基础，具有重要的意义。运动学解算的基本任务就是要建立并联机器人末端位姿与主动关节变量之间的映射关系，它包括两个方面的内容：第一，在给定并联机器人运动平台的位姿条件下，计算出机器人各主动关节的输入变量，称之为逆向运动学求解；第二，对于给定的并联机器人主动关节输入变量，通过正向运动学求解计算出运动平台的位姿。从并联机器人研究所涉及的领域来看，运动学解算建立在机构分析的基础上，它又是开展动力学建模与分析、机构控制、误差分析与补偿等相关研究的基础。对于并联机器人而言，其运动学解算的特点是：一般来说，逆向运动学求解通常具有解析表达形式，而由于非线性和多值的影响，正向运动学求解相对复杂，很难构建解析形式的解。

目前的研究通常在分析并联机器人机构运动原理的基础上，利用空间几何分析方法建立从运动平台位姿到主动关节输入变量的解析形式的运动学逆解模型。而对于相对复杂的并联机器人正向运动学求解，采用的方法可以分为解析法和迭代法两大类。

解析法通过消去部分变量，试图将描述并联机器人逆向运动学关系的非线性方程组化成一元高次方程然后求得位姿正解。虽然对于少部分特殊类型的并联机器人可以采用这种方法获得解析解，但是对于大多数并联机器人而言，采用这种消元方式求解构造过程非常复杂，有时甚至无法进行消元，即使可以也需要较高的技巧，缺乏一种完备的方法，容易产生丢根和增根的问题。

迭代法是目前相对应用广泛的求解并联机器人运动学正解的方法，它也称为数值方法。迭代法的形式有多种，包括牛顿法、拟牛顿法、共轭梯度法、不动点法、区间分析法、同伦法等。以牛顿法为代表的基于梯度信息的方法，在初值选取适当的条件下具有较高的收敛速度和精度，可以满足位置正解的实时计算需求。以同伦法为代表的大范围收敛算法，对初值的选取要求并不严格，但计算过程相对复杂，已经应用于并联机构的位置分析与综合、机构特殊位形分析、柔性机构力逆解等问题的研究。此外，也有一些智能优化方法被应用于迭代求解并联机构的位置正解，如遗传算法、粒子群算法等，但其计算的实时性和精确性仍待加强。

【发明内容】

本发明的目的在于针对一类杆端浮动型六自由度并联机器人的机构特点(如图1所示)，分析了由于螺母相对丝杠旋转对滚珠丝杠副的杆长变化量造成的影响(如图2所示)，针对该问题提出了一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法。该方法可以精确计算由于螺母旋转引起的滚珠丝杠副的附加杆长，并且在滚珠丝杠副的驱动电机指令杆长中进行补偿，从而有效的提高了杆端浮动型六自由度并联机器人的运动学求解精度，同时具有计算实时性好和易于编程实现的优点。

为了实现上述目的，本发明采用如下技术方案：

一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法，包括杆端浮动型六自由度并联机器人的逆向运动学求解方法：

给定杆端浮动型六自由度并联机器人的末端位姿向量基于空间矢量链的传递关系，分别计算出并联机器人各个滚珠丝杠副的有效杆长l_i,i＝1,2,...6，然后通过空间解析几何分析计算出各滚珠丝杠副中螺母与丝杠的相对旋转角θ_i，以及由该角度导致的滚珠丝杠副的伸长变化量，即附加杆长l_ai，将各个有效杆长与其对应的附加杆长相叠加，得到并联机器人的驱动电机指令杆长l_ci＝l_i+l_ai。

本发明进一步的改进在于：所述杆端浮动型六自由度并联机器人，包括固定平台和运动平台，固定平台上安装有六个固定平台虎克铰关节，运动平台上安装有六个运动平台虎克铰关节；固定平台虎克铰关节与对应的运动平台虎克铰关节通过杆端浮动型滚珠丝杠副连接；杆端浮动型滚珠丝杠副均通过电机驱动。

本发明进一步的改进在于：滚珠丝杠副的螺母相对于丝杠的旋转角θ_i和由此产生的附加杆长l_ai的计算步骤如下：

Step1.1、第i个滚珠丝杠副的有效杆长为式中

$\left[\begin{array}{c}{x}_{M_i}\\ y_{M_i}\\ z_{M_i}\end{array}\right]=\left[\begin{array}{ccc}\mathrm{c\γc\β}& \mathrm{c\γs\βs\α}-\mathrm{s\γc\α}& \mathrm{c\γs\βc\α}+\mathrm{s\γs\α}\\ \mathrm{s\γc\β}& \mathrm{s\γs\βs\α}+\mathrm{c\γc\α}& \mathrm{s\γs\βc\α}-\mathrm{c\γs\α}\\ -\mathrm{s\β}& \mathrm{c\βs\α}& \mathrm{c\βc\α}\end{array}\right]\left[\begin{array}{c}{x}_{M_i}^{\′}\\ y_{M_i}^{\′}\\ z_{M_i}^{\′}\end{array}\right]+\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(1\right)$

其中， ${\left[\begin{array}{ccc}{x}_{M_i}^{\′}& y_{M_i}^{\′}& z_{M_i}^{\′}\end{array}\right]}^T$ 是运动平台上各个虎克铰关节在运动平台坐标系下的位置坐标， ${\left[\begin{array}{ccc}{x}_{B_i}& y_{B_i}& z_{B_i}\end{array}\right]}^T$ 是固定平台上各个虎克铰关节在固定平台坐标系下的位置坐标，[x y z α β γ]^T＝p为并联机器人的运动平台在固定平台坐标系下的位姿向量；

Step1.2、对于给定的位姿向量p，令表示在当前位姿下由并联机器人运动平台坐标系的原点O_m指向运动平台虎克铰关节M_i的向量，表示由并联机器人固定平台坐标系的原点O_b指向固定平台虎克铰关节B_i的向量，表示由点B_i指向M_i的向量，则由与构成的平面为Γ_i，计算其法向量由与构成的平面为Κ_i，计算其法向量对于预先设置的初始位姿向量p₀＝[x₀ y₀ z₀ α₀ β₀ γ₀]^T，采用同样的方法计算出和则在当前位姿p下螺母相对于丝杠的旋转角为

${\mathrm{\θ}}_i=\mathrm{\φ}\left(p\right)-\mathrm{\φ}\left(p_0\right)=\arccos \frac{t_i\·n_i}{\left|t_i\right|\·\left|n_i\right|}-\arccos \frac{t_i^{\′}\·n_i^{\′}}{\left|t_i^{\′}\right|\·\left|n_i^{\′}\right|}---\left(2\right)$

Step1.3、判断各滚珠丝杠副的螺母是正转还是反转：

若

Step1.4、根据螺母旋转方向，计算由螺母旋转引起的各滚珠丝杠副的附加杆长l_ai：

其中，z为丝杠的头数，d为螺距；

Step1.5、计算各个滚珠丝杠副的电机指令杆长l_ci：

l_ci＝l_i+l_ai   (5)。

本发明进一步的改进在于：所述逆向运动学求解方法还包括以下控制步骤：

Step1.6、根据计算的电机指令杆长l_ci控制各电机转动，使各滚珠丝杠副达到对应的杆长，从而实现并联机器人精确的轨线跟踪控制。

本发明进一步的改进在于：还包括杆端浮动型六自由度并联机器人的正向运动学计算方法：

给定并联机器人的主动关节输入变量即各滚珠丝杠副的驱动电机指令杆长l_ci,i＝1,2,...6，基于所述逆向运动学求解方法，通过矩阵微分建立杆长向量的微小变化量δl_c＝[δl_c1 ... δl_c6]^T与位姿向量的微小变化量δp的线性映射关系δp＝Jδl_c，根据给定杆长与当前计算杆长之间的偏差映射得到位姿的补偿向量Δp，将其与当前的计算位姿相叠加，如此不断的迭代计算，直至迭代k次之后杆长偏差小于预先设定的误差限，之后停止，此时计算得到的位姿向量p_k即为与给定杆长相对应的位姿向量。

本发明进一步的改进在于：所述正向运动学计算方法中线性映射关系δp＝Jδl的计算步骤如下：

(1)令表示并联机器人的位姿向量，由逆向运动型模型可知并联机器人各主动关节的输入变量是位姿向量p的函数，即l_c(p)＝l(p)+l_a(p)，其中l_c＝[l_c1 ... l_c6]^T，l＝[l₁ ... l₆]^T，l_a＝[l_a1 ... l_a6]^T；

(2)对式l_c(p)＝l(p)+l_a(p)两端同时微分，得δl_c＝(J₁+J₂)δp，其中

(3)位姿微小变化量δp＝Jδl_c＝(J₁+J₂)^-1δl_c。

本发明进一步的改进在于：所述正向运动学计算方法具体包括以下步骤：

Step2.1、初始化，令给定的并联机器人电机的指令杆长向量为l_cg＝[l_cg1 ... l_cg6]^T，迭代索引k＝0，最大迭代次数为k_max，初始位姿向量为p₀；

Step2.2、对于当前位姿p_k，根据所述的逆向运动学求解方法，计算出对应的电机指令杆长向量如果其中ε为给定的误差限，则停止，输出p_k；

Step2.3、否则，令i＝1,2,...6，计算

同时，利用向前差商的数值求导方法计算J₂(p_k)；

Step2.4、计算位姿向量的改变量令p_k＝p_k+Δp，k＝k+1；

Step2.5、如果k≥k_max，算法停止，否则，返回Step2.2。

相对于现有技术，本发明具有以下有益效果：

采用本发明在给定的位姿下计算螺母相对于丝杠的旋转角以及由此引起的附加杆长的方法，可以针对一类由螺母可旋转的滚珠丝杠副(称之为杆端浮动)作为主动关节的六自由度并联机器人的驱动电机指令杆长进行精确的计算，从而极大的提高该类并联机器人的轨线跟踪控制精度。而在此基础上进行的并联机器人运动学正解计算，则可以获得与驱动电机指令杆长相对应的更加准确的位姿向量，从而为实现该型并联机器人的完整动力学分析与控制、工作空间分析以及运动安全防护提供坚实依据。本发明可以解决滚珠丝杠副螺母旋转对六自由度并联机器人运动学计算的影响，具有精度高收敛速度快的优点，利于实现该型并联机器人的高精度实时轨线跟踪控制。

【附图说明】

为了更加清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。

图1是本发明所涉及的杆端浮动型六自由度并联机器人的主动关节，即由电机驱动的杆端浮动型滚珠丝杠副的示意图；其中1为运动平台虎克铰；2为固定平台虎克铰；3为驱动电机；4为杆端浮动型滚珠丝杠副；

图2a是并联机器人的位姿变化前的示意图，图2b为并联机器人的位姿变化前圆圈所示区域的滚珠丝杠副的螺母的位置示意图；图2c为并联机器人的位姿变化后的示意图；图2d为并联机器人的位姿变化后圆圈所示区域的滚珠丝杠副的螺母发生旋转后的位置示意图；

图3是并联机器人的坐标系及运动支链的空间矢量关系示意图；

图4是计算平面Γ_i与平面Κ_i的夹角的示意图。

【具体实施方式】

请参阅图1所示，本发明所涉及的杆端浮动型六自由度并联机器人，包括固定平台和运动平台，固定平台上安装有六个固定平台虎克铰关节2，运动平台上安装有六个运动平台虎克铰关节1；固定平台虎克铰关节2与对应的运动平台虎克铰关节1通过杆端浮动型滚珠丝杠副4连接；杆端浮动型滚珠丝杠副4均通过电机3驱动。

下面结合附图对本发明的具体实施方式做详细的说明。

首先明确坐标系的建立方式，如图3所示，其中：

固定平台坐标系O_b-x_by_bz_b——令B_i，i＝1,2,...6表示固定平台上安装的六个虎克铰关节的中心点，由B_i组成的六边形的几何中心为O_b,以O_b为坐标系原点，x_b轴垂直平分线段B₁B₂，y_b轴在O_b、B₁、B₂确定的平面内，按右手法则建立固定平台坐标系O_b-x_by_bz_b，令 ${\left[\begin{array}{ccc}{x}_{B_i}& y_{B_i}& z_{B_i}\end{array}\right]}^T$ 为B_i在固定平台坐标系下的位置坐标；

运动平台坐标系O_m-x_my_mz_m——令M_i，i＝1,2,...6表示运动平台上安装的六个虎克铰关节的中心点，由M_i组成的六边形的几何中心为O_m,以O_m为坐标系原点，x_m轴垂直平分线段M₁M₂，y_m轴在O_m、M₁、M₂确定的平面内，按照右手法则建立运动平台坐标系O_m-x_my_mz_m，令 ${\left[\begin{array}{ccc}{x}_{M_i}^{\′}& y_{M_i}^{\′}& z_{M_i}^{\′}\end{array}\right]}^T$ 为M_i在运动平台坐标系下的位置坐标。

令p＝[x y z α β γ]^T表示并联机器人的运动平台在固定平台坐标系下的位姿向量，其中x、y、z是点O_m在系O_b-x_by_bz_b中的坐标，α、β、γ分别为系O_m-x_my_mz_m相对于系O_b-x_by_bz_b的x_b、y_b和z_b轴的旋转角；表示在当前位姿p下由点O_m指向点M_i的向量，表示由点O_b指向点B_i的向量。

本发明一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法，包括杆端浮动型六自由度并联机器人的逆向运动学求解方法和杆端浮动型六自由度并联机器人的正向运动学计算方法：

第一步、杆端浮动型六自由度并联机器人的逆向运动学求解方法：

Step1.1、根据图3所示的空间矢量的链接关系，有则第i个滚珠丝杠副的有效杆长为 $l_i=\sqrt{{(x_{B_i}-x_{M_i})}^2+{(y_{B_i}-y_{M_i})}^2+{(z_{M_i}-z_{B_i})}^2},$ 式中

$\left[\begin{array}{c}{x}_{M_i}\\ y_{M_i}\\ z_{M_i}\end{array}\right]=\left[\begin{array}{ccc}\mathrm{c\γc\β}& \mathrm{c\γs\βs\α}-\mathrm{s\γc\α}& \mathrm{c\γs\βc\α}+\mathrm{s\γs\α}\\ \mathrm{s\γc\β}& \mathrm{s\γs\βs\α}+\mathrm{c\γc\α}& \mathrm{s\γs\βc\α}-\mathrm{c\γs\α}\\ -\mathrm{s\β}& \mathrm{c\βs\α}& \mathrm{c\βc\α}\end{array}\right]\left[\begin{array}{c}{x}_{M_i}^{\′}\\ y_{M_i}^{\′}\\ z_{M_i}^{\′}\end{array}\right]+\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(1\right)$

Step1.2、对于给定的位姿向量p，由与构成的平面为Γ_i，计算其法向量由与构成的平面为Κ_i，计算其法向量如图4所示。对于预先设置的初始位姿向量p₀＝[x₀ y₀ z₀ α₀ β₀ γ₀]^T，类似的计算出和则在位姿向量p下，螺母相对于丝杠的旋转角为

${\mathrm{\θ}}_i=\mathrm{\φ}\left(p\right)-\mathrm{\φ}\left(p_0\right)=\arccos \frac{t_i\·n_i}{\left|t_i\right|\·\left|n_i\right|}-\arccos \frac{t_i^{\′}\·n_i^{\′}}{\left|t_i^{\′}\right|\·\left|n_i^{\′}\right|}---\left(2\right)$

并联机器人运动平台的位姿向量为p时，平面Γ_i与平面Κ_i的夹角为并联机器人运动平台的位姿向量为p₀时，平面Γ_i与平面Κ_i的夹角为

Step1.3、判断各滚珠丝杠副的螺母是正转还是反转：

若

Step1.4、根据螺母旋转方向，计算由螺母旋转引起的各滚珠丝杠副的附加杆长l_ai

对于并联机器人的初始位姿向量p₀和给定的位姿向量p，各个滚珠丝杠副的螺母与丝杠之间的旋转角为θ_i＝φ(p)-φ(p₀)；设滚珠丝杠为右旋，头数为z，螺距为d；

Step1.5、计算各个滚珠丝杠副的电机指令杆长l_ci

l_ci＝l_i+l_ai   (5)

Step1.6、根据计算的电机指令杆长l_ci控制各电机转动，使各滚珠丝杠副达到对应的杆长，从而实现并联机器人精确的轨线跟踪控制。

第二步、杆端浮动型六自由度并联机器人的正向运动学计算方法：

Step2.1、初始化，令给定的并联机器人电机3的指令杆长向量为l_cg＝[l_cg1 ... l_cg6]^T，迭代索引k＝0，最大迭代次数为k_max，初始位姿向量为p₀；

Step2.2、对于当前位姿p_k，根据之前所述的逆向运动学求解方法，计算出对应的电机指令杆长向量 $l_c^k={\left[\begin{array}{ccc}{l}_{c1}^k& ...& l_{c6}^k\end{array}\right]}^T,$ 如果其中ε为给定的误差限，则停止，输出p_k；

Step2.3、否则，令i＝1,2,...6，计算

同时，利用向前差商的数值求导方法计算J₂(p_k)；

Step2.4、计算位姿向量的改变量令p_k＝p_k+Δp，k＝k+1；

Step2.5、如果k≥k_max，算法停止，否则，返回Step2.2。

下面以数值算例说明本发明方法的效果。选取并联机器人的机构参数如下：运动平台上6个铰点的外接圆半径0.275m，六边形短边对应的中心角为18.296°^.，固定平台上6个铰点的外接圆半径0.65m，六边形短边对应的中心角为11.748°^.。假设并联机器人运动平台的位姿如表1所示，不考虑螺母相对丝杠旋转时，计算得到的驱动电机指令杆长如表2所示，而利用本发明提出的带角度补偿的逆向运动学方法得到的驱动电机指令杆长如表3所示。

计算实例表明，对于杆端浮动型六自由度并联机器人，利用通常意义下的运动学求解方法和利用本发明所提出的带角度补偿的运动学求解方法，所得的结果存在偏差，而这些偏差在实现该类型并联机器人高精度的轨线跟踪控制时绝不可忽略，将直接影响到控制的精度。同时本发明所述方法的运算时间均小于0.02ms，完全能够满足机器人控制的实时性要求。

表1运动平台的实际位姿

表2不考虑杆端浮动效应的驱动电机指令杆长(单位：m)

表3利用带角度补偿的运动学方法得到的驱动电机指令杆长(单位：m)

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施方式仅限于此，对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单的推演或替换，都应当视为属于本发明由所提交的权利要求书确定的专利保护范围。

Claims (8)

1.一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法，其特征在于，包括杆端浮动型六自由度并联机器人的逆向运动学求解方法：

给定杆端浮动型六自由度并联机器人的末端位姿向量基于空间矢量链的传递关系，分别计算出并联机器人各个滚珠丝杠副的有效杆长l_i,i＝1,2,...6，然后通过空间解析几何分析计算出各滚珠丝杠副中螺母与丝杠的相对旋转角θ_i，以及由该角度导致的滚珠丝杠副的伸长变化量，即附加杆长l_ai，将各个有效杆长与其对应的附加杆长相叠加，得到并联机器人的驱动电机指令杆长l_ci＝l_i+l_ai。

2.根据权利要求1所述的一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法，其特征在于，所述杆端浮动型六自由度并联机器人，包括固定平台和运动平台，固定平台上安装有六个固定平台虎克铰关节，运动平台上安装有六个运动平台虎克铰关节；固定平台虎克铰关节与对应的运动平台虎克铰关节通过杆端浮动型滚珠丝杠副连接；杆端浮动型滚珠丝杠副均通过电机驱动。

3.根据权利要求1所述的一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法，其特征在于，滚珠丝杠副的螺母相对于丝杠的旋转角θ_i以及由此产生的附加杆长l_ai的计算步骤如下：

Step1.1、第i个滚珠丝杠副的有效杆长为式中

$\left[\begin{array}{c}{x}_{M_i}\\ y_{M_i}\\ z_{M_i}\end{array}\right]=\left[\begin{array}{ccc}\mathrm{c\γc\β}& \mathrm{c\γs\βs\α}-\mathrm{s\γc\α}& \mathrm{c\γs\βc\α}+\mathrm{s\γs\α}\\ \mathrm{s\γc\β}& \mathrm{s\γs\βs\α}+\mathrm{c\γc\α}& \mathrm{s\γs\βc\α}-\mathrm{c\γs\α}\\ -\mathrm{s\β}& \mathrm{c\βs\α}& \mathrm{c\βc\α}\end{array}\right]\left[\begin{array}{c}{x}_{M_i}^{\′}\\ y_{M_i}^{\′}\\ z_{M_i}^{\′}\end{array}\right]+\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(1\right)$

其中，[x′_Mi y′_Mi z′_Mi]^T是运动平台上各个虎克铰关节在运动平台坐标系下的位置坐标，[x_Bi y_Bi z_Bi]^T是固定平台上各个虎克铰关节在固定平台坐标系下的位置坐标，[x y z α β γ]^T＝p为并联机器人的运动平台在固定平台坐标系下的位姿向量，x、y、z是运动平台的位置坐标，α、β、γ为运动平台坐标系分别绕固定平台坐标系的三个坐标轴的旋转角；

Step1.2、对于给定的位姿向量p，令表示在当前位姿下由并联机器人运动平台坐标系的原点O_m指向运动平台虎克铰关节M_i的向量，表示由并联机器人固定平台坐标系的原点O_b指向固定平台虎克铰关节B_i的向量，表示由点B_i指向M_i的向量，则由与构成的平面为Γ_i，计算其法向量由向量构成的平面为Κ_i，计算其法向量对于预先设置的初始位姿向量p₀＝[x₀ y₀ z₀ α₀ β₀ γ₀]^T，采用同样的方法计算出和则在当前位姿p下螺母相对于丝杠的旋转角为

${\mathrm{\θ}}_i=\mathrm{\φ}\left(p\right)-\mathrm{\φ}\left(p_0\right)=\arccos \frac{t_i\·n_i}{\left|t_i\right|\·\left|n_i\right|}-\arccos \frac{t_i^{\′}\·n_i^{\′}}{\left|t_i^{\′}\right|\·\left|n_i^{\′}\right|}---\left(2\right)$

Step1.3、判断各滚珠丝杠副的螺母是正转还是反转：

若

Step1.4、根据螺母旋转方向，计算由螺母旋转引起的各滚珠丝杠副的附加杆长l_ai：

其中，z为丝杠的头数，d为螺距；

Step1.5、计算各个滚珠丝杠副的电机指令杆长l_ci：

l_ci＝l_i+l_ai    (5)。

4.根据权利要求3所述的一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法，其特征在于，所述逆向运动学求解方法还包括以下控制步骤：

Step1.6、根据计算的电机指令杆长l_ci控制各个电机转动，使得各滚珠丝杠副达到对应的杆长，从而实现并联机器人精确的轨线跟踪控制。

5.根据权利要求1所述的一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法，其特征在于，还包括杆端浮动型六自由度并联机器人的正向运动学计算方法：

给定并联机器人的主动关节输入变量即各滚珠丝杠副的驱动电机指令杆长l_ci,i＝1,2,...6，基于所述逆向运动学求解方法，通过矩阵微分建立杆长向量的微小变化量δl_c＝[δl_c1 ... δl_c6]^T与位姿向量的微小变化量δp的线性映射关系δp＝Jδl_c，根据给定杆长与当前计算杆长之间的偏差映射得到位姿的补偿向量Δp，将其与当前的计算位姿相叠加，如此不断的迭代计算，直至迭代k次之后杆长偏差小于预先设定的误差限，之后停止，此时计算得到的位姿向量p_k即为与给定杆长相对应的位姿向量。

6.根据权利要求5所述的一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法，其特征在于，利用位姿向量p_k进行并联机器人的运动安全防护计算与工作空间分析。

7.根据权利要求5所述的一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法，其特征在于，所述正向运动学计算方法中线性映射关系δp＝Jδl的计算步骤如下：

(1)令表示并联机器人的位姿向量，由逆向运动学模型可知并联机器人各主动关节的输入变量是位姿向量p的函数，即l_c(p)＝l(p)+l_a(p)，其中l_c＝[l_c1 ... l_c6]^T，l＝[l₁ ... l₆]^T，l_a＝[l_a1 ... l_a6]^T；

(2)对式l_c(p)＝l(p)+l_a(p)两端同时微分，得δl_c＝(J₁+J₂)δp，其中

(3)位姿微小变化量δp＝Jδl_c＝(J₁+J₂)^-1δl_c。

8.根据权利要求5所述的一类杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法，其特征在于，所述正向运动学计算方法具体包括以下步骤：

Step2.1、初始化，令给定的并联机器人电机的指令杆长向量为l_cg＝[l_cg1 ... l_cg6]^T，迭代索引k＝0，最大迭代次数为k_max，初始位姿向量为p₀；

Step2.2、对于当前位姿p_k，根据所述的逆向运动学求解方法，计算出对应的电机指令杆长向量如果其中ε为给定的误差限，则停止，输出p_k；

Step2.3、否则，令i＝1,2,...6，计算

同时，利用向前差商的数值求导方法计算J₂(p_k)；

Step2.4、计算位姿向量的改变量 $\mathrm{\Δp}=\mathrm{J\Δ}{l}_c={(J_1\left(p_k\right)+J_2\left(p_k\right))}^{-1}(l_{\mathrm{cg}}-l_c^k),$ 令p_k＝p_k+Δp，k＝k+1；

Step2.5、如果k≥k_max，算法停止，否则，返回Step2.2。