CN105807712A - 一种六自由度并联机器人正向运动学的对偶四元数解法 - Google Patents

Abstract

本发明公开一种针对六自由度并联机器人正向运动学的对偶四元数解法。该方法中采用对偶四元数作为并联机器人系统的广义坐标，正向运动学方程组是关于对偶四元数的二次型代数方程，针对该方程组提出了高效率的数值算法，能够快速计算这类六自由度并联机器人运动平台位置和姿态的最小二乘解。对于不含冗余驱动的并联机器人，最小二乘解也是精确解。

Description

一种六自由度并联机器人正向运动学的对偶四元数解法

技术领域

本发明属于机器人系统的运动学、动力学与控制研究领域，尤其是一类六自由度并联机器人正向运动学求解方法，面向机器人系统实时控制的应用需求。

背景技术

并联机器人，有时称为并联机床，是一类闭环运动机构，相比串联机器人在很多方面具有优势，包括更大的刚度质量比，更高的基频，可以承受相对较大的负载；更强的动态性能和稳定性；以及更高的运动精度，能完成精密级任务，现已逐渐应用到运动模拟系统、微位移定位装置、可视化触觉装置、工业机器人和医用机器人、天文望远镜、多维隔振等方面。六自由度机器人可以通过六个或者更多的运动支链连接一个运动平台(也称末端执行器)构成，将独立的单向运动(转动或移动)转换成一个运动平台的三自由度转动和三自由度移动，这些支链可以是RRPS,RPRS,PRRS,RRRS等。通过交换关节的顺序，或者用转动关节(移动关节)代替移动关节(转动关节)，可以衍生出多种构型，例如6-UPS(Gough1947)，6-RUS(Hunt1983)，6-PUS(Merlet1991)等。这类六自由度并联机器人的研究和应用领域不断拓展，其运动学、奇异性、工作空间与灵巧性、动力学与控制、平台的设计与开发等方面均得到深入而广泛的研究。

虽然并联机构的多项优点使其成为高速运动、精密定位等应用场合下(例如加工中心、射电望远镜等)的理想解决方案，然而该机构耦合程度高，运动控制复杂，寻求高精度、低时耗的运动学稳定解是一个研究难点。其逆向运动学问题定义为根据运动平台确切的位姿(位置和姿态)求解若干个独立的输入运动。实际上，该问题并不复杂，多个输入运动的表达式独立，能并行计算，很快完成求解。运动学正解问题则是在这些输入运动已知的情况下，求解运动平台的位置和姿态，在一般情形下，不具备封闭形式和唯一解。而运动学正解在反馈控制、工作空间分析中具有极其重要的作用，因此解决正向运动学问题是并联机器人研究领域内亟待解决的挑战性任务之一。

解决正向运动学的方法有两类：解析法和数值法。在解析法方面，众多学者采用代数消元法、连续法、区间分析等。将运动学方程组转化为一个高阶多项式方程，致力于找到该方程的所有可能解，并取得了一些进展，这些解称作该机器人的装配模式。但是，至今无法表达出位姿变量的显式形式。况且，找到所有可能解也未完全解决正向运动学问题，仍需进一步在这些解中确定唯一的实际位姿，这是实际应用时必需的。在某些情况下，针对由解析法得到的一个单变量高次代数方程或者非线性方程组，可利用附加传感器获得唯一解，但在实际应用中有所限制，例如昂贵的价格和测量误差等。在数值法方面，牛顿-拉夫逊法被广泛使用，该方法是将非线性代数方程组线性化为线性方程组求解，其收敛域依赖于非线性方程组的性质，若迭代初值位于收敛域内，可获得精确解。也有学者采用神经网络算法获得牛顿-拉夫逊算法所需初值，保证算法的稳定性。直接采用遗传算法、神经网络算法等优化算法求解运动学方程也可获得唯一解，但遗传算法、神经网络算法等均耗时较长，不适合实时性的应用要求。

因此亟需解决六自由度并联机器人快速运动学正解问题，以用于闭环反馈实时控制等应用中。

发明内容

本发明的目的是针对现有技术存在的不足，提供一种具有高效计算速度的并联机器人正向运动学的解法。

为解决上述问题，本发明提供的六自由度并联机器人正向运动学的对偶四元数解法可采用如下技术方案：

1、一类六自由度并联机器人包含一个运动平台，通过n(n≥6)个运动支链连接基座，如6-UPS、6-RUS、9-SPS并联机器人等。采用单位对偶四元数建立正向运动学方程，该方法包括如下步骤:

(1)、建立并联机器人运动学闭环方程

E₁(θ)＝p+Ra_i(i＝1,2,…,n)，

其中，主动关节变量用θ，p是运动平台的绝对位置矢量，R是旋转矩阵，表示运动平台的姿态，a_i是运动平台与每个支链连接点相对运动平台的位置矢量，n≥6，表示方程的个数，对于无冗余输入的并联机器人n＝6，具有冗余输入的并联机器人n>6。

(2)、引入单位对偶四元数描述运动平台的位置和姿态

E₁(θ)ε＝λ+εa_i(i＝1,2,…,n)，

其中，ε和λ分别是单位对偶四元数的实部和对偶部，共同构成并联机器人系统的广义坐标x＝(ελ)^T。

ε和λ按照以下方式构造：

其中，c₁和c₂是任意非零常数。

(3)、建立基于对偶四元数的正向运动学方程

||E₂(θ)||²＝||λ||²+||a_i||²+||c_i||²+2(ε^*λ)·a_i+2(λε^*)·c_i+2a_i·c_i(i＝1,2,…,n)，

其中，c_i是从E₁(θ)中分离的常矢量。

(4)、正向运动学方程与对偶四元数的内在约束方程的变形

$f_i\left(x\right)=\frac{1}{2}{x}^T{Q}_{i}x-C_i=0,(i=1,2,...,n+2),$

其中，C_i是常数，由给定的主动关节输入值和机器人几何参数确定，Q_i是仅由机器人几何参数确定的常数对称矩阵。

本发明的有益效果：利用对偶四元数能够建立六自由度并联机器人正向运动学方程，并转化为对偶四元数的二次型。在此基础构造的数值解法能够快速计算出并联机器人运动平台的位置和姿态。

进一步的，上述基于单位对偶四元数的正向运动学方程的基础上建立快速解法包括：

(1)、六自由度并联机器人正向运动学的快速求解方法

$x_{k+1}=\frac{1}{2}{x}_k+{\left(J_k^T{J}_k\right)}^{-1}{J}_k^{T}C,(k=0,1,2...),$

其中，雅可比矩阵J_k＝(x^TQ₁x^TQ₂…x^TQ_n+2)^T，

F(x_k)＝(f₁(x_k)f₂(x_k)…f_n+2(x_k))^T。

(2)、采用数值求解线性方程组的方法代替求解矩阵的广义逆

$\left\{\begin{array}{cc}\begin{array}{c}{x}_{k+1}=\frac{1}{2}{x}_k+{\mathrm{\Δx}}_k\\ \left(J_k^T{J}_k\right){\mathrm{\Δx}}_k=J_k^{T}C\end{array}& (k=0,1,\mathrm{2...}).\end{array}\right.$

附图说明

图1是本发明中实施例中的6-PUS并联机器人几何图。

具体实施方式

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

1.六自由度并联机器人运动学的对偶四元数表示

本发明公开的这类六自由度并联机器人由一个运动平台(也成为末端执行器)通过若干个支链连接到基座而成，各部件通过低副连接。通过各支链的输入运动使运动平台实现空间的任意转动和平移。由于并联机器人的运动学反解容易，各支链的运动状态都可以表示成运动平台参数的函数。在动力学建模与分析中，也常常将运动平台的位姿变量定义为系统的广义坐标，令矢量x表示运动平台的位置和姿态，也是并联机器人系统的广义坐标，由一组描述能描述其六自由度运动的参数组成。机器人内部运动支链的几何关系可以用所有关节的变量表示，其中所有主动关节的变量组成一个矢量，用θ表示。建立运动学闭环方程

E(θ,x)＝0(1)

运动学正解是在θ已知的情形下根据方程(1)求解x。当x用运动平台的绝对位置矢量p和旋转矩阵进行参数化时，方程(1)可以写成

E₁(θ)＝p+Ra_i(i＝1,2,…,n)(2)

上式中a_i表示运动平台与每个支链连接点相对运动平台的位置矢量，n≥6，表示方程的个数，对于经典Stewart类型的并联机器人n＝6，具有冗余输入的并联机器人n>6。本文首先将x用一个单位对偶四元数进行参数化。对偶四元数的集合是一个Clifford代数，是两个四元数的有序对，实数部分的四元数表示转动，对偶部分的四元数包含了移动，这需要将p和R变换至两个四元数。

根据旋转矩阵与单位四元数的转化关系，令ε是单位对偶四元数的实部，方程(2)可以写成

E₁(θ)＝p+εa_iε^*(i＝1,2,…,n)(3)

由于ε与-ε对应相同的转动，本发明在计算时选择ε₀≥0的四元数。为了建立刚体移动与单位四元数的联系，根据位移矢量p和表示单位转动四元数ε构造对偶四元数的对偶部λ。这两个四元数要满足单位对偶四元数的两个内在约束方程：

$\{\begin{array}{c}\left|\right|\mathrm{\ϵ}\left|\right|=1\\ \mathrm{Re}\left({\mathrm{\ϵ\λ}}^{*}\right)=0\end{array}---\left(4\right)$

由矢量p和四元数ε可以这样构造λ：

λ＝c₁pε,orλ＝c₂εp(5)

式(5)中c₁和c₂是任意非零常数。

用广义坐标x＝(ελ)^T对运动平台的转动和移动进行参数化。按照式(5)中给出的方法，简单地令λ＝pε，根据式(5)对方程(3)进行变形

E₁(θ)ε＝λ+εa_i(i＝1,2,…,n)(6)

对于正向运动学问题，方程(6)中仅有ε和λ是未知的，有时为了消元，可以将E₁(θ)ε中含有未知几何参数(如杆长的矢量方向等)的量分离出来

E₂(θ)ε＝λ+εa_i+c_iε(i＝1,2,…,n)(7)

上式中c_i是从E₁(θ)中分离的常矢量，计算方程(7)与自身的四元数内积

(E₂(θ)ε)·(E₂(θ)ε)＝(λ+εa_i+c_iε)·(λ+εa_i+c_iε)(i＝1,2,…,n)(8)

方程(8)可以展开为

||E₂(θ)||²＝||λ||²+||a_i||²+||c_i||²+2(ε^*λ)·a_i+2(λε^*)·c_i+2a_i·c_i(i＝1,2,…,n)(9)

由于方程(9)中仅含有ε、λ的二次项和常数项，因此(9)可以改写成x的二次型形式

$f_i\left(x\right)=\frac{1}{2}{x}^T{Q}_{i}x-C_i=0,(i=1,2,...,n)---\left(10\right)$

方程(10)中，C_i是常数，由给定的主动关节输入值和机器人几何参数确定，Q_i是仅由机器人几何参数确定的常数对称矩阵。另外，方程(4)也可以写成x的二次型形式

$\{\begin{array}{c}{f}_{n+1}\left(x\right)=\frac{1}{2}{x}^T{Q}_{n+1}x-1=0\\ f_{n+2}\left(x\right)=\frac{1}{2}{x}^T{Q}_{n+2}x=0\end{array},Q_{n+1}=\left(\begin{array}{cc}2I_{4\×4}& 0\\ 0& 0_{4\×4}\end{array}\right),Q_{n+2}=\left(\begin{array}{cc}{0}_{4\×4}& I\\ I& 0_{4\×4}\end{array}\right)---\left(11\right)$

方程(10)和(11)共同构成了六自由度并联机器人的正向运动学方程组。对于经典的Stewart型并联机器人，总共有八个方程，对于具有冗余驱动的机器人，方程数目超过八个。

2.正向运动学方程的快速数值算法

并联机器人的正向运动学问题转化成求解一组非线性方程组数值解的问题，目前有很多求解的方法。但是，基于对偶四元数的正向运动学方程普遍地具有二次的形式，方程可以写成对偶四元数的二次型，这种特性可以被充分地加以利用。

任何非线性代数方程组F(x)＝0，均有牛顿迭代形式

$x_{k+1}=\mathrm{\Φ}\left(x_k\right)=x_k-J_k^{-1}F\left(x_k\right),(k=\mathrm{0,1,2}\·\·\·)$

上式中是方程组的雅可比矩阵。

对于本发明中的一类六自由度并联机器人的正向运动学方程，采用雅可比矩阵的广义逆代替逆，得到如下方式计算出方程组的最小二乘解

$x_{k+1}=\mathrm{\Φ}\left(x_k\right)=x_k-{\left(J_k^T{J}_k\right)}^{-1}{J}_k^{T}F\left(x_k\right),(k=0,1,2...)---\left(12\right)$

式(12)中，雅可比矩阵J_k＝(x^TQ₁x^TQ₂…x^TQ_n+2)^T，

F(x_k)＝(f₁(x_k)f₂(x_k)…f_n+2(x_k))^T。

为了进一步提高算法的效率，对其简化和改进。考虑到多项式函数F(x_k)和雅克比矩阵J_k有如下关系

$F\left(x_k\right)=\frac{1}{2}{J}_k{x}_k-C,C={\left(\begin{array}{cccccc}{C}_1& C_2& ...& C_n& 1& 0\end{array}\right)}^T---\left(13\right)$

于是，在式(12)中，我们可以消去F(x_k)，迭代函数可简化为如下形式

$\mathrm{\Φ}\left(x_k\right)=\frac{1}{2}{x}_k+{\left(J_k^T{J}_k\right)}^{-1}{J}_k^{T}C---\left(14\right)$

现在，每次迭代中不必计算F(x_k)，降低了计算时耗。另一方面，考虑到计算矩阵的逆耗时较长，我们可以采用数值求解线性方程组的方法代替，得到如下迭代序列

$\left\{\begin{array}{cc}\begin{array}{c}{x}_{k+1}=\frac{1}{2}{x}_k+{\mathrm{\Δx}}_k\\ \left(J_k^T{J}_k\right){\mathrm{\Δx}}_k=J_k^{T}C\end{array}& (k=0,1,\mathrm{2...})---\left(15\right)\end{array}\right.$

通过迭代式(14)和(15)计算得到的是并联机器人正向运动学方程组的最小二乘解。当并联机器人不含有冗余驱动时，n＝6，此时雅可比矩阵J_k是8×8方阵，最小二乘解也是精确解。

3.算法的奇异性分析与应用条件

并联机器人对于同一组主动关节变量θ，运动平台的位置和姿态x具有多个解，当θ连续变化时，每个解集构成了各自的空间，这些解空间通常是不连续的。数值算法(14)和(15)的目的是为了在实时控制时找到运动平台的真实解，但是，当算法的雅可比矩阵J_k奇异或者接近奇异时，迭代序列将很难收敛或者从一个解空间跳进另一个解空间中，这将使实时应用失败。注意到算法的奇异性与并联机器人运动学奇异性存在着内在联系。

计算闭环方程(1)的时间导数

$\frac{\∂E}{\∂\mathrm{\θ}}\stackrel{\·}{\mathrm{\θ}}+\frac{\∂E}{\∂x}\stackrel{\·}{x}=J_{\mathrm{\θ}}\stackrel{\·}{\mathrm{\θ}}+J_x\stackrel{\·}{x}=0---\left(16\right)$

Gosselin和Angeles定义与讨论了并联机器人的奇异性，J_θ奇异定义为串联奇异，J_x奇异则定义为并联奇异。J_θ被称为逆向运动学雅可比矩阵，J_x被称为正向运动学雅可比矩阵。其中，J_x恰好与算法的雅可比矩阵J_k对应，这可以通过计算正向运动学方程组(10)和(11)的时间导数看出，J_x和J_k都是系统广义速度(广义坐标x的时间导数)前的系数矩阵，它们的关系如下

$J_k=\left(\begin{array}{c}{J}_x\\ \begin{array}{cc}2\mathrm{\ϵ}& 0_4\end{array}\\ \begin{array}{cc}\mathrm{\λ}& \mathrm{\ϵ}\end{array}\end{array}\right)---\left(17\right)$

因此，只要并联机器人不处于并联奇异状态时，正向运动学算法始终有效。有时会采用运动平台的运动旋量w表示系统的广义速度，w由运动平台的绝对速度v和角速度ω组成，方程(16)具有如下形式

$\frac{\∂E}{\∂\mathrm{\θ}}\stackrel{\·}{\mathrm{\θ}}+\frac{\∂E}{\∂x}\stackrel{\·}{x}=J_{\mathrm{\θ}}\stackrel{\·}{\mathrm{\θ}}+J_{w}w=0---\left(18\right)$

此时的正向运动学雅可比矩阵是J_w，对比式(16)和式(18)，容易看出为了导出J_k与J_w的关系，我们需要找到与w间的变换矩阵H，使得J_x＝J_wH。因此

$J_k=\left(\begin{array}{c}{J}_{w}H\\ \begin{array}{cc}2\mathrm{\ϵ}& 0_4\end{array}\\ \begin{array}{cc}\mathrm{\λ}& \mathrm{\ϵ}\end{array}\end{array}\right)---\left(19\right)$

为了获得变换矩阵H的形式，需要理清v、ω与关系。对Ry＝εyε^*对时间求导，

$\stackrel{\·}{R}y=\stackrel{\·}{\mathrm{\ϵ}}{\mathrm{y\ϵ}}^{*}+\mathrm{\ϵ}y{\stackrel{\·}{\mathrm{\ϵ}}}^{*}=\stackrel{\·}{\mathrm{\ϵ}}{\mathrm{y\ϵ}}^{*}-{\left(\stackrel{\·}{\mathrm{\ϵ}}{\mathrm{y\ϵ}}^{*}\right)}^{*}---\left(20\right)$

考虑到ε是一个单位四元数：εε^*＝1，而且式(20)变换为

$\mathrm{\ω}\×\left({\mathrm{\ϵy\ϵ}}^{*}\right)=\stackrel{\·}{\mathrm{\ϵ}}{\mathrm{\ϵ}}^{*}\left({\mathrm{\ϵy\ϵ}}^{*}\right)-{\left(\stackrel{\·}{\mathrm{\ϵ}}{\mathrm{\ϵ}}^{*}\right({\mathrm{\ϵy\ϵ}}^{*}\left)\right)}^{*}---\left(21\right)$

对时间求导得因此Re(εε^*)＝0，即εε^*是一个矢量。另一方面，对于任意两个四元数r和s，有这样的计算结果：

rs-(rs)^*＝2r₀s+2s₀r+2r×s。特别地，这两个四元数均为矢量时，则

rs-(rs)^*＝2r×s。因此，式(21)化简为

$\mathrm{\ω}\×\left({\mathrm{\ϵy\ϵ}}^{*}\right)=2\left(\stackrel{\·}{\mathrm{\ϵ}}{\mathrm{\ϵ}}^{*}\right)\×\left({\mathrm{\ϵy\ϵ}}^{*}\right)---\left(22\right)$

从(22)中容易看出ω与的关系

$\mathrm{\ω}=2\stackrel{\·}{\mathrm{\ϵ}}{\mathrm{\ϵ}}^{*}---\left(23\right)$

根据λ与p的关系λ＝pε，可知p＝λε^*。对其计算相对于时间的导数

$v=\stackrel{\·}{p}=\stackrel{\·}{\mathrm{\λ}}{\mathrm{\ϵ}}^{*}+\mathrm{\λ}{\stackrel{\·}{\mathrm{\ϵ}}}^{*}---\left(24\right)$

由于两个四元数的积是它们的双线性型，因此等式(23)和(24)可以写成矩阵形式，可以得到矩阵H的形式

$H=\left(\begin{array}{cc}\stackrel{+}{\mathrm{\λ}}G& \stackrel{\‾}{\mathrm{\ϵ}}\\ 2\stackrel{\‾}{\mathrm{\ϵ}}& 0_{3\×4}\end{array}\right)---\left(25\right)$

上式中，

$\stackrel{+}{\mathrm{\λ}}=\left(\begin{array}{cccc}{\mathrm{\λ}}_0& -{\mathrm{\λ}}_3& {\mathrm{\λ}}_2& {\mathrm{\λ}}_1\\ {\mathrm{\λ}}_3& {\mathrm{\λ}}_0& -{\mathrm{\λ}}_1& {\mathrm{\λ}}_2\\ -{\mathrm{\λ}}_2& {\mathrm{\λ}}_1& {\mathrm{\λ}}_0& {\mathrm{\λ}}_3\end{array}\right),\stackrel{\‾}{\mathrm{\ϵ}}=\left(\begin{array}{cccc}{\mathrm{\ϵ}}_0& {\mathrm{\ϵ}}_3& -{\mathrm{\ϵ}}_2& {\mathrm{\ϵ}}_1\\ -{\mathrm{\ϵ}}_3& {\mathrm{\ϵ}}_0& {\mathrm{\ϵ}}_1& {\mathrm{\ϵ}}_2\\ {\mathrm{\ϵ}}_2& -{\mathrm{\ϵ}}_1& {\mathrm{\ϵ}}_0& {\mathrm{\ϵ}}_3\end{array}\right)---\left(26\right)$

和的各行正交，G＝diag(-1-1-10)，是一个正交矩阵。H是一个行满秩矩阵，直接计算HH^T就可以看出，这保证了rank(J_x)＝rank(J_wH)，于是有同样的结论，使正向运动学算法有效应用的条件是并联机器人不处于并联奇异状态。因此采用该正向运动学算法控制并联机器人前提是对并联机器人的奇异性进行分析，得出无奇异工作空间，并在该空间中规划一个无奇异运动轨迹。

5.应用示范

正向运动学算法适用于任意六自由度并联机器人，不失一般性，以6-PUS并联机器人为例验证算法的正确性和高效性。图1是6-PUS并联机器人的几何描述，O-x_ay_az_a是动平台坐标系，O-x_by_bz_b是绝对坐标系。a_i(i＝1,2…6)是运动平台关节中心相对于O-x_ay_az_a的位置矢量，b_i(i＝1,2…6)基座关节中心相对于O-x_by_bz_b的位置矢量。未标明单位的物理量均采用国际单位制SI.

6-PUS并联机器人的几何参数包含参数a_i(i＝1,2…6)、b_i(i＝1,2…6)和杆长||l_i||(i＝1,2…6)＝1.3，并假设主动移动关节的初始位移||z_i||＝0(i＝1,2…6)，球铰和万向节分别分布在两个圆上，有a_i＝r_a(cosα_isinα_i0)^T和b_i＝r_b(cosβ_isinβ_i0)^T，其中

$\left\{\begin{array}{cc}\begin{array}{c}{r}_a=r_b=0.84\\ {\mathrm{\α}}_{2i-1}=2\mathrm{\π}i/3-2\mathrm{\π}/3\\ {\mathrm{\α}}_{2i}=2\mathrm{\π}i/3\\ {\mathrm{\β}}_{2i-1}={\mathrm{\β}}_{2i}=2\mathrm{\π}i/3-\mathrm{\π}/3\end{array}& (i=1,2,3)\end{array}\right.---\left(27\right)$

当6-PUS并联机器人运动平台从初始状态

$\begin{array}{c}{p}_0={\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T\\ R_0=I_{3\×3}\end{array}---\left(28\right)$

运动至最终状态

p_f＝(0.10.11.1)^T

$\begin{array}{c}{R}_f=R(\mathrm{\π}/12,x_a).R(\mathrm{\π}/12,y_a).R(\mathrm{\π}/12,z_a)\\ =\left(\begin{array}{ccc}0.933012701892& -0.25& 0.258819045102\\ 0.314704761275& 0.915675113362& -0.25\\ -0.174494158464& 0.314704761275& 0.933012701892\end{array}\right)\end{array}---\left(29\right)$

初始状态和最终状态对应的广义坐标x₀＝(ε₀λ₀)^T和x_f＝(ε_fλ_f)^T分别为

ε₀＝(0001)

λ₀＝(0010)

ε_f＝(0.1451937383610.1114110739300.1451937383610.972329743084)

λ_f＝(-0.01079983317910.2424267126701.06618445095-0.185373593427)

(30)

并且采用式(28)与(29)的运动设定，在动态模拟实时控制时，将整个运动分割为5个控制周期，进行1秒的计算。我们首先对6-PUS并联机器人运动平台的运动x(t),t∈[0,1]进行规划，为了获得恒定的速度和角速度，对于平台移动采用线性插补，平台转动采用球面线性插补

$\mathrm{\ϵ}\left(t\right)={\mathrm{\ϵ}}_0\frac{sin\left(\right(1-t\left)\mathrm{\φ}\right)}{sin\mathrm{\φ}}+{\mathrm{\ϵ}}_f\frac{sin\left(t\mathrm{\φ}\right)}{sin\mathrm{\φ}}$

λ(t)＝((1-t)p₀+tp_f)ε(t)(31)

φ＝arccos(ε₀·ε_f)andt∈[0,1]

然后根据6-PUS并联机器人闭环方程

l_i+b_i+z_i＝p+Ra_i(i＝1,2…6)(32)

解出主动关节的驱动位移

$\left|\right|z_i\left|\right|=d_{iz}^2-\sqrt{l_i^2-d_{ix}^2-d_{iy}^2},(i=1,2...6)---\left(33\right)$

d_i＝λε^*+εa_iε^*-b_i

根据采样频率对||z_i(t)||进行采样，得到一组离散的采样值，在每一个采样周期内，运用快速正向运动学算法计算6-PUS并联机器人运动平台的位置和姿态，迭代序列的初值为上一周期的计算值，迭代终止的条件为||Δx||<10^-8。计算过程与结果列于表1中，在Win10/intelCorei7/16GBRAM/Mathematica9环境下，借助Mathematica的Timing命令计算出给该算法的平均每个周期的耗时为0.00021875秒。

表16-PUS的动态模拟计算

如果没有采用对偶四元数对运动学方程进行处理，仅通过方程(32)求解运动平台位置和姿态时，产生的计算时耗会大大提高，例如采用MathematicaFindRoot命令求解时，平均计算时间为0.00323438秒，是新算法的15倍。可见，本发明中的新方法使得六自由度并联机器人正向运动学的求解效率得到极大的提高。

Claims (4)

1.一种六自由度并联机器人正向运动学的对偶四元数解法，其中的六自由度并联机器人包含基座、运动平台，运动平台通过至少6个运动支链连接基座，其特征在于，采用单位对偶四元数建立正向运动学方程，包括如下步骤：

(1)、建立并联机器人运动学闭环方程：

E₁(θ)＝p+Ra_i(i＝1,2,…,n)；

其中，主动关节变量用θ，p是运动平台的绝对位置矢量，R是旋转矩阵，表示运动平台的姿态，a_i是运动平台与每个支链连接点相对运动平台的位置矢量，n≥6，表示方程的个数，对于无冗余输入的并联机器人n＝6，具有冗余输入的并联机器人n＞6；

(2)、引入单位对偶四元数描述运动平台的位置和姿态：

E₁(θ)ε＝λ+εa_i(i＝1,2,…,n)；

其中，ε是单位对偶四元数的实部，λ是单位对偶四元数的对偶部，ε和λ共同构成并联机器人系统的广义坐标x＝(ελ)^T；

ε和λ按照以下方式构造：

c₁和c₂是任意非零常数；

(3)、建立基于对偶四元数的正向运动学方程

||E₂(θ)||²＝||λ||²+||a_i||²+||c_i||²+2(ε^*λ)·a_i+2(λε^*)·c_i+2a_i·c_i(i＝1,2,…,n)

其中，c_i是从E₁(θ)中分离的常矢量；

(4)、正向运动学方程与对偶四元数的内在约束方程的变形

$f_i\left(x\right)=\frac{1}{2}{x}^T{Q}_{i}x-C_i=0,(i=1,2,...,n+2).$

2.根据权利要求1所述的六自由度并联机器人正向运动学的对偶四元数解法，其特征在于，基于单位对偶四元数的正向运动学方程的基础上建立快速解法：

(1)、六自由度并联机器人正向运动学的快速求解方法

$x_{k+1}=\frac{1}{2}{x}_k+{\left(J_k^T{J}_k\right)}^{-1}{J}_k^{T}C,(k=0,1,2...);$

其中，J_k是方程组的雅可比矩阵；C＝(C₁C₂……C_n10)^T；

(2)、采用数值求解线性方程组的方法代替求解矩阵的广义逆

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

3.根据权利要求2所述的六自由度并联机器人正向运动学的对偶四元数解法，其特征在于，在计算时选择ε₀≥0的四元数。

4.根据权利要求3所述的六自由度并联机器人正向运动学的对偶四元数解法，其特征在于，并联机器人不处于并联奇异状态。