基于单个角度传感器的6-UPS并联机构位置正解解析算法

张 贺，李开明，陈 语，周文全

(南京理工大学机械工程学院，南京 210094)

0 引言

并联机构因其具有较强的承载能力和较高的刚度和精度并且控制简单等优点，已被广泛应用于工业，医疗和娱乐设备等领域[1-2]。并联机构的运动学及其正解是实现其闭环控制的基础，多年来一直是并联机器人领域的研究重点。然而，并联机构的运动学正解求解复杂，且存在多解，直到现在，并联机构的运动学正解问题一直没有完全解决[3]。

目前人们主要通过解析法，数值法和添加传感器等方法求解并联机构的位置正解[4]。解析法可以求出并联机构所有可能的解，但其计算过程相当复杂，不利于机构的实时反馈控制[5-6]。常见的数值解法包括神经网络算法等智能算法、基于几何结构法和数值迭代法三类。智能算法往往算法复杂且精度较低。数值迭代法能够求解出一组最优解。但此类方法不能得到所有正解且最终的结果与初值选取有直接关系，易造成结果不收敛[7-9]。文献[10]通过额外添加不同数量的位移传感器和旋转传感器来辅助计算运动学正解。但多个数量的传感器不仅增加了硬件成本，更会使得测量误差增大。

文献[11]提出在6-UPS并联机构中心处通过额外添加一个位移传感器，测出此中心杆的长度，用代数消元等方法求出6-UPS机构的解析正解。但此方法增加的第7个杆占用了机构的中心位置。如果并联机构内部空间有其它用途，则此方法不再适合。

本文在以上研究的基础上，在6-UPS并联机构的其中一个虎克铰处只添加一个旋转传感器，通过测得其中一个支链的特定方向旋转角度，将位置正解的求解过程由12个未知数减少为11个未知数。利用机构位姿耦合关系得到相容方程组，由此结合机构实际情况求解得到唯一一组姿态参数。最后通过数值算例，进行验证。

1 构建矢量及逆解方程

建立如图1所示的6-UPS并联机构的简化模型。首先在机构的上、下平台上各建立一坐标系。以虎克铰B1的中心为坐标原点建立固定坐标系B1-X0Y0Z0(或称坐标系S0)，其X0轴通过虎克铰B2的中心，Z0轴垂直于机构定平台方向向上。以球铰链E1的中心为坐标原点建立动坐标系E1-XNYNZN(或称坐标系SN)。动坐标系固联于动平台，其XN轴指向球铰E2的中心，ZN轴垂直于动平台方向向上。动、静平台的6个顶点分别循环对称布置在一个平面圆周上。

图1 6-UPS并联机构结构简图

本文在传统的6-UPS并联机构的基础上，在B1-E1支链的虎克铰链处引入旋转传感器从而测得该支链绕虎克铰十字支架其中一旋转轴的旋转角度φ。支链具体结构如图2所示。

图2 B1-E1支链结构简图

如图中虚线位置所示，支链在初始状态时Y0轴方向分量为零(见图中B1-E1′)。角度φ为支链由初始状态运动到当前状态时支链绕B1G轴旋转的角度。规定顺时针方向为正。由图可知，当角度φ一定时，无论支链绕定平台铰链怎样运动，其末端铰点E1始终在以D点为圆心，r为半径的圆上。点D为铰点E1在Y0轴上的投影。通过在十字支架的旋转铰链G处引入一旋转传感器可测得支链由初始位置运动到当前状态时支链绕B1G轴的旋转角度φ，设l1为支链B1-E1的杆长，则:

(1)

(2)

矢量P={X,Y,Z}T为动坐标系原点E1在固定坐标系B1-X0Y0Z0下的位置矢量。R为动平台的旋转变换矩阵。

(3)

当6-UPS并联机构的结构参数已定后，根据矢量封闭关系可得各支链矢量表达式为：

Li=P+Rei-Bi(i=1,2,…6)

(4)

式中，ei为动平台上的各铰链点Ei在动坐标系SN下的坐标，由图1可知，有{eix,eiy,eiz}T={pi,qi,0}T，pi、qi为数值常量且p1=q1=q2=0。Bi为定平台上的各铰链点Bi在固定坐标系S0下的坐标，由图1可知，有{Bix,Biy,Biz}T={ai,bi,0}T，ai、bi为数值常量且a1=b1=b2=0。

根据式(4)可得各驱动杆长度li为:

(pilz+qimz+Z)2i=1,2,…,6

(5)

2 构造位置正解方程并求解

2.1 位姿约束方程

求解6-UPS并联机构的位置正解，就是当6个支链杆长给定后，求解上述矩阵R和矢量P中的12个元素。

在支链B1-E1的虎克铰处引入旋转传感器测得支链角度信号。设其虎克铰链绕B1G轴旋转角度为φ(如图2所示)。则位置矢量P中的元素Y可通过角度φ直接求得：

(6)

引入角度φ求得元素Y后，则未知数的个数由12个变为11个。由于矩阵R是动平台在定平台下的方向余弦矩阵，因此具有归一性和正交性等性质。只要知道其中三个独立元素的值，其余元素便可通过以下性质求得：

(7)

(8)

lxmx+lymy+lzmz=0

(9)

nx=lymz-lzmy

(10)

ny=lzmx-lxmz

(11)

nz=lxmy-lymx

(12)

令W为P矢量在动坐标系SN中的矢量表达式，W={Wx,Wy,Wz}T。即W、P为同一矢量在不同坐标系下的位置参数。由坐标变换关系可知，存在P=RW，结合R的正交性，同时成立W=RTP。

由式(5)可得各支链杆长表达式。当i=1时，由a1=b1=p1=q1=0可知，式(5)可简化为：

(13)

当i=2，3…，6时，将W=RTP代入式(5)并化简后可得：

piWx+qiWy-Ailx-Bily-Cimx=aiX+Dimy+Ei
i=2,3,…,6

(14)

其中，Ai，Bi，Ci，Di及Ei为常数。

与以往研究成果含8个未知数不同，由于元素Y已通过所测角度φ求得，式(14)为只含有Wx，Wy，lx，ly，mx，my及X共7个未知数的5个方程组成的线性方程组。若选其中的X，my作为基本变量，则其余5个未知数Wx，Wy，lx，ly，mx可以表达成关于X，my的函数。写成矩阵形式为：

(15)

将式(15)写成线性方程组的形式为：

Wx=F1X+G1my+H1

(16)

Wy=F2X+G2my+H2

(17)

lx=F3X+G3my+H3

(18)

ly=F4X+G4my+H4

(19)

mx=F5X+G5my+H5

(20)

式中，Fi，Gi，Hi只与机构的结构参数和各驱动杆长以及所测角度φ有关。通过式(13)及式(7)和式(8)可知，Z，lz，mz可表达为：

(21)

(22)

(23)

由式(9)及W=RTP可知，这3个未知数还可以表达为：

lzmz=-(lxmx+lymy)

(24)

lzZ=Wx-lxX-lyY

(25)

mzZ=Wy-mxX-myY

(26)

由R的性质可得：

(27)

(28)

lxly+mxmy+nxny=0

(29)

由P=RW可得：

X=lxWx+mxWy+nxWz

(30)

Y=lyWx+myWy+nyWz

(31)

所以有：

nxWz=X-lxWx-mxWy

(32)

nyWz=Y-lyWx-myWy

(33)

由于同一矢量在不同坐标系下的模长不变，所以有以下关系成立：

所有老年网站都提供了图片、文本课程资源，多数网站也提供了视频资源。由于视频资源制作成本高，数量有限，更新速度慢，且与多种终端兼容能力弱，共享方式有限。老年人群的年龄跨度大、文化层次不一、兴趣爱好广泛，老年人群体是一个复杂的群体，现有课程资源无法满足老年人群的复杂多样的需求。一些网站只是一些资源的组合，忽视了老年用户服务体验，公共服务功能弱，缺乏老年特色。

(34)

2.2 位置正解方程

根据以上关系，通过对式(16)～式(34)的不同组合可得到只包含X，my的以下11个相容方程：

(35)

(36)

(37)

(38)

(39)

(40)

(41)

(42)

(lzmz)(Z2)-(lzZ)(mzZ)=0

(43)

(44)

(45)

进一步将这11个方程化简处理后写为如下形式：

(46)

与以前的研究成果fi,j仍含有未知数不同，由于元素Y已求得，此处的fi,j已全部为常数。这为接下来获得动平台位置唯一正解提供了基础。

(47)

把式(47)得到的结果代入剩余的6个方程中，可获得6个关于X和my的三次方组合方程，记为：

(48)

(49)

将式(49)所得结果代入剩余的2个方程，可得关于X和my的二次方组合方程，记为：

(50)

(51)

(52)

2.3 求解位置矢量P和姿态矩阵R

将求得的X和my的值代入式(16)～式(20)可得lx，ly，mx，Wx，Wy的值，结合式(21)及式(25)和式(26)可得Z，lz，mz的值。

(53)

lz=(Wx-lxX-lyY)/Z

(54)

mz=(Wy-mxX-myY)/Z

(55)

结合机构实际，可获得唯一Z值。最后根据式(10)～式(12)可得nx，ny，nz的值。至此，位置矢量P和姿态矩阵R中的所有元素已求得，且为唯一值。

3 数值算例分析

有一6-UPS并联机构其结构参数为θ1=12.5°，θ2=15°，动平台铰点圆半径为350，定平台铰点圆半径为550。由此可确定结构参数ai，bi，pi，qi。设初始状态下位置矢量P={-20,5,800}T，姿态矩阵R为：

(1)反解：通过式(5)和式(6)位置反解可得到杆长和角度φ。

l1=800.266、l2=1 001.013、l3=1 075.68、
l4=973.076、l5=1 235.146、l6=1 032.376、
φ=0.358°。

根据式(16)～式(20)可计算得lx=0.904 508，ly=0.293 894，mx=-0.203 074，Wx=-263.834，Wy=243.846。结合式(53)可得Z=±800.000。结合机构实际情况，由于动平台在静平台的上方，取Z=800。再结合式(54)和式(55)求得lz=-0.309 017，mz=0.293 893。将结果代入式(10)～式(12)可得nx=0.375 000，ny=-0.203 075，nz=0.904 508。

整理得P={-20.000,5.000,800.000}T。

所得结果与给出的数据一致，位置正解求解过程完成，验证了所提方法的正确性。与传统解析算法得出40组位置正解且计算过程极为复杂不同，所述算法整个过程求解简单、求解速度快并能得到唯一解。

4 结论

提出了一种在6-UPS并联机构中只添加一个旋转传感器便测得机构运动学正解的方法。在建立合适的坐标系后，通过在其中一个支链的虎克铰链处添加旋转传感器，获得该支链特定方向的旋转角度信息，从而直接求得动平台位置矢量Y方向的值。位置正解的求解由原来的12个未知数减少为11个未知数。利用机构位姿耦合关系获得11个只包含元素X和my的相容方程。通过对方程组消元降次，得到符合实际情况的唯一正解，有利于此类并联机构的实时反馈控制。只添加一个传感器，不仅降低了硬件成本，而且减少了多个传感器带来的累积误差。所提方法不占用并联机构的内部空间，且传感器安装简单，在实际应用中更具有普适性。