CN102692201B - 空间六自由度运动的测量装置及动态测量方法 - Google Patents

Abstract

本发明公开了一种空间六自由度运动的测量装置及动态测量方法，测量装置的定平台(1)上装有六个虎克铰(2)，在定平台(1)的上方设有动平台(10)，该动平台(10)上装有六个球铰(9)，六个球铰(9)均通过支链与对应的六个虎克铰(2)相连接，该支链的旋转副(6)上安装有旋转编码器(5)。动态测量方法包括建立坐标系、利用旋转编码器(5)测量旋转副(6)的上下支杆的相对运动的角度时间曲线、以及由旋转编码器(5)的角度时间曲线正解得出被测运动物体空间六自由度的运动规律等步骤。本发明不仅能实现六自由度运动的同时测量，而且能够确保测量结果的准确性，具有灵活性好，适应能力强，测量范围广，精度高等特点。

Description

空间六自由度运动的测量装置及动态测量方法

技术领域

本发明涉及一种空间六自由度运动的测量装置及动态测量方法，利用6-URS并联机构及旋转编码器动态测量运动物体空间六自由度运动。空间六自由度运动包括三自由度运动位移x(t)、y(t)、z(t)及三自由度转动角度θ_x(t)、θ_y(t)、θ_z(t)，运动位移及转动角度微分后可转换成运动速度、运动加速度、转动角速度、转动角加速度。

背景技术

在现代工业、国防及航空航天中，对空间六自由度运动动态测量的要求越来越高，如飞机或舰艇的大型工件焊接装配、并联机床、航海上的潜艇救援对接、航天星(船)箭对接、太空空间站对接、移动火箭(导弹)发射系统中装填车和发射车的对接、飞行或道路或导弹发射的模拟器、精密操作的微动器、飞机空中加油、火箭(导弹)发射装置初始扰度的测量与控制等。

空间六自由度运动动态测量的理论、方法、手段和工程应用研究一直是国内外学术界和工程界的热点问题，尽管进行了大量研究，但是仍然有许多急需解决而又没有完全解决好的工程实际问题和学术难点问题，集中体现在空间六自由度运动动态测量技术问题上。

并联机构具有承载能力强、运动精度高、刚度大、运动惯量小、自重负荷比小、动力性能好、控制容易等一系列优点，在国民生产的各个领域应用越来越广泛。并联机构复杂的机构学问题属于空间多自由度多环并联机构学理论研究领域，也包括随机器人高技术发展起来的多机器人协调、多足步行机、多指多关节高灵活手爪等构成的并联多环机构学问题。

目前，空间六自由度运动的测量多采用线性位移测量方式，线性位移测量方式存在较大的摩擦和磨损，存在精度低、寿命短等缺点，且没有实现六自由度运动的同时测量。

发明内容

本发明的目的之一在于提供一种空间六自由度运动的测量装置，能够对运动物体进行空间六自由度运动位姿高精度动态测量。

空间六自由度运动位姿包括：三自由度运动位移x(t)、y(t)、z(t)及三自由度运动角度θ_x(t)、θ_y(t)、θ_z(t)，运动位移及角度微分后可转换成运动速度、运动加速度、转动角速度、转动角加速度。

为实现上述目的，本发明的技术方案如下：一种空间六自由度运动的测量装置，在定平台(1)上装有六个虎克铰(2)，这六个虎克铰(2)围成一个六角形；在所述定平台(1)的上方设有动平台(10)，该动平台(10)上装有六个球铰(9)，六个球铰(9)也围成一个六角形，并且六个球铰(9)与六个虎克铰(2)一一对应；所述六个球铰(9)均通过支链与对应的六个虎克铰(2)相连接，该支链包括上支杆(8)、下支杆(3)和旋转副(6)，所述上支杆(8)的上端与对应的球铰(9)连接，上支杆(8)的下端通过旋转副(6)与下支杆(3)的上端铰接，在旋转副(6)上安装有旋转编码器(5)，所述下支杆(3)的下端与对应的虎克铰(2)连接，同一支链上的旋转副(6)位于该支链所对应的球铰(9)与虎克铰(2)中心连线的外侧，同一支链所对应的球铰(9)与虎克铰(2)中心连线同对应的旋转副(6)位于一个平面内，该支链只能在这个平面内运动。

采用以上技术方案，虎克铰用U表示，旋转副用R表示，球铰用S表示，六组球铰、六组虎克铰和六组支链构成测量空间六自由度运动的装置，因此，本发明命名为6-URS并联机构。上面选用球铰，下面选用虎克铰，能够防止发生干涉，使各支链只在所处平面作内外方向的运动。当被测运动物体空间六自由度运动的幅值、频率较高时，6-URS并联机构需承受大的冲击和震动。支链中的上支杆和下支杆刚度高、质量小，转动灵活，抗冲击能力强，从而降低了6-URS并联机构对被测运动物体的附加力及附加质量，提高了测量精度。定平台和下支杆通过虎克铰相连，动平台和上支杆通过球铰连接，上支杆和下支杆之间通过转动副连接以组成一个有机的整体，通过旋转编码器记录下上支杆和下支杆作相对旋转运动的角度时间曲线，以此来计算运动物体的空间六自由度运动。旋转编码器是集光机电技术于一体的角度位移传感器，具有体积小、重量轻、品种多、功能全、频响高、分辨能力高、力矩小、耗能低、性能稳定、可靠使用寿命长等特点。利用6-URS并联机构与旋转编码器相结合，不仅能实现六自由度运动的同时测量，而且能够确保测量结果的准确性。

本发明结构简单、性能可靠、应用范围广，并拥有较大的测量空间，能够高精度地动态测量被测物体的空间六自由度运动。

所述上支杆(8)和下支杆(3)的结构相同，均由螺纹套筒、螺杆和锁定螺钉组成，螺杆的一端伸入螺纹套筒内，两者之间螺纹配合，并通过径向穿设的锁定螺钉顶紧；所述上支杆(8)的螺杆与球铰(9)连接，上支杆(8)的螺纹套筒通过旋转副(6)与下支杆(3)的螺纹套筒铰接，下支杆(3)的螺杆与虎克铰(2)连接。以上结构各支杆的长度可以根据需要进行调节，以扩大测量的范围。调节支杆长度时，只需将锁定螺钉松开，拧动螺杆调整其伸入螺纹套筒的长度，再重新拧紧锁定螺钉即可，既方便又快捷。支杆长度调整好以后，在螺纹配合的紧固作用以及锁定螺钉的顶紧作用下，能有效防止支杆的长度在测量过程中发生变化，从而进一步保障了测量结果的准确性。

所述定平台(1)为正六边形，虎克铰(2)安装在定平台(1)的六个角处；所述动平台(10)也为正六边形，并位于定平台(1)的正上方，所述球铰(9)安装在动平台(10)的六个角处。以上结构加工制作容易、装配简单方便，不仅能够简化算法，而且进一步确保了在测量六自由度运动的过程中6-URS并联机构不产生干涉。

本发明的目的之二在于提供一种空间六自由度运动的动态测量方法，能够对运动物体进行空间六自由度运动位姿高精度动态测量。

为实现上述目的，本发明的技术方案如下：一种空间六自由度运动的动态测量方法，其特征在于包括以下步骤：

1)将被测运动物体刚性固定在动平台(10)上，定平台(1)刚性固定在相对静止不动的物体上，六个球铰(9)分别编号为S₁、S₂、S₃、S₄、S₅、S₆，六个虎克铰(2)分别编号为U₁、U₂、U₃、U₄、U₅、U₆；在被测运动物体上设定空间直角正交体坐标系o-xyz，在动平台上球铰(9)所围成六角形的几何中心设定空间直角正交体坐标系动坐标系S-X'Y'Z'，S-X'Y'Z'与动平台固连，其原点S与动平台的形心重合，Z′轴垂直于动平台向上，X′轴与S₁S₆垂直，Y′轴平行于S₁S₆，在定平台上虎克铰(2)所围成六角形的几何中心设定空间直角正交体坐标系O-XYZ，其原点O与定平台的形心重合，Z轴垂直向上，Y轴与U₁U₆垂直，X轴平行于U₁U₆；

2)被测运动物体空间六自由度的运动带动动平台(10)运动及旋转编码器(5)转动，旋转编码器(5)测量旋转副(6)的上支杆(8)与下支杆(3)的相对运动的角度时间曲线θ_i(t)，i=1，2，3，4，5，6；

3)由旋转编码器(5)的角度时间曲线正解，得出被测运动物体空间六自由度的运动规律。

5、根据权利要求4所述空间六自由度运动的动态测量方法，其特征在于：

步骤3)包含如下过程：

a、运动分解：

动平台的运动可分解为随S-X'Y'Z'坐标原点S沿O-XYZ三个坐标轴方向上的平移(X(t)，Y(t)，Z(t))^T，以及绕坐标轴的旋转(α(t)，β(t)，γ(t))^T；被测运动物体可分解为随o-xyz坐标原点o沿S-X'Y'Z'三个坐标轴方向上的平移(x(t)，y(t)，z(t))^T，以及绕坐标轴的旋转(θ_x(t)，θ_y(t)，θ_z(t))^T；

b、坐标变换：

S_i(i=1，2，3，4，5，6)表示动平台上的各球铰点，U_i(i＝1，2，3，4，5，6)表示定动平台上的各虎克铰点；S_i(i＝1，2，3，4，5，6)在固定空间直角坐标系O-XYZ和动空间直角坐标系S-X'Y'Z'中的坐标向量表示分别为S(S_iX，S_iY，S_iZ)^T、S(S_iX′，S_iY′，S_iZ′)^T；U_i(i＝1，2，3，4，5，6)在固定空间直角坐标系O-XYZ中的坐标向量表示分别为U(U_iX，U_iY，U_iZ)^T，l_i表示杆S_iU_i的长度(i＝1，2，3，4，5，6)；静坐标向量S(S_iX，S_iY，S_iZ)^T和坐标向量S(S_iX′，S_iY′，S_iZ′)^T有如下变换公式：

$\left[\begin{array}{c}{S}_{\mathrm{iX}}\\ S_{\mathrm{iY}}\\ S_{\mathrm{iZ}}\end{array}\right]=\left[T\right]\×\left[\begin{array}{c}{S}_{{\mathrm{iX}}^{\′}}\\ S_{{\mathrm{iX}}^{\′}}\\ S_{{\mathrm{iX}}^{\′}}\end{array}\right]+\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right],$ i＝1，2，3，4，5，6

其中T为坐标变换公式，这里有：

$\left[T\right]=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& -\cos \mathrm{\α}\sin \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}\\ \cos \mathrm{\β}\sin \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}& -\sin \mathrm{\α}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}\\ -\sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]$

根据上下支杆旋转角度的变化，利用机构运动学算法解算出动平台的六自由度运动变化，得出动平台的空间位姿V＝(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T，通过坐标变换得到被测运动物体的六自由度运动变化，确定被测物体的空间位姿W＝(x(t)，y(t)，z(t)，θ_x(t)，θ_y(t)，θ_z(t))^T；通过旋转编码器电机上的编码记录的旋转角度，能够精确地的得出上球铰与下虎克铰距离的变化Δl_i(t)(i＝1，2，3，4，5，6)；则上球铰与下虎克铰的距离可表示为：

(i＝1，2，3，4，5，6)，其中l_i(t)表示状态角度为θ时上球铰与下虎克铰的距离，l_下为转动副中心到虎克铰中心的距离，l_上为转动副中心到球铰中心的距离，则动平台位姿(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T和六组球铰及胡可铰的距离l_i(t)的非线性约束方程式为：

F_i(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))=l_i ²-[(S_ix-U_ix)²+(S_iy-U_iy)²+(S_iz-U_iz)²]＝0，(i＝1，2，3，4，5，6)

c、迭代解法：

令V＝(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T，F(V)=(f₁(V)，f₂(V)，f₃(V)，f₄(V)，f₅(V)，f₆(V))^T，选择初始点V₀，F_i(V₀)(i＝1，2，3，4，5，6)在V₀附近进行泰勒展开，取得其线性部分作为动平台姿态的牛顿迭带公式：

V_k+1=V_k-[J(V_k)]^-1F(V_k)(k＝0，1，2，...)

其中

经过多次迭代后，当满足|V_k+1-V_k|<E时终止迭代，此时的V_k+1便是满足精度要求的动平台空间位姿V=(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T的值，再经坐标转换矩阵的转换、平移，可得被测运动物体的空间位姿W＝(x(t)、y(t)、z(t)，θ_x(t)、θ_y(t)、θ_z(t))^T；被测运动物体上的空间直角坐标系相对动平台上坐标系的坐标转换矩阵T′：

$T^{\&prime;}=\left[\begin{array}{ccc}{\cos \mathrm{\&theta;}}_y{\cos \mathrm{\&theta;}}_z& -{\cos \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_z+{\sin \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_y{\cos \mathrm{\&theta;}}_z& {\sin \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_z+{\cos \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_y{\cos \mathrm{\&theta;}}_z\\ {\cos \mathrm{\&theta;}}_y{\sin \mathrm{\&theta;}}_z& {\cos \mathrm{\&theta;}}_x{\cos \mathrm{\&theta;}}_z+{\sin \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_y{\sin \mathrm{\&theta;}}_z& -{\sin \mathrm{\&theta;}}_x{\cos \mathrm{\&theta;}}_z+{\cos \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_y{\sin \mathrm{\&theta;}}_z\\ -{\sin \mathrm{\&theta;}}_y& {\sin \mathrm{\&theta;}}_x{\cos \mathrm{\&theta;}}_y& {\cos \mathrm{\&theta;}}_x{\cos \mathrm{\&theta;}}_y\end{array}\right].$

本发明的有益效果是：

1)采用旋转编码器，将被测运动物体空间六自由度运动的测量转换为六个角度的测量，性能稳定、可靠，不仅能实现六自由度运动的同时测量，而且能够确保测量结果的准确性。

2)采用6-URS并联机构，刚度好、精度高、结构简单。

3)6-URS并联机构结构紧凑，承载力强，负载/自重比大，稳定性好。

4)灵活性好，适应能力强，测量范围广，可根据环境和用户需要改变6-URS并联机构的安装结构、调整上支杆和下支杆长度。

5)应用范围广泛，在机器人、并联机床、精密仪器测试设备等领域中较为普及，同时在飞行模拟器、风洞试验模型装置、空间对接设备等国防重点领域等领域中也有相当重要的作用。

附图说明

图1为本发明的立体图。

图2为本发明中的上、下支杆连接示意图。

图3为本发明的原理图之一。

图4为本发明的局部原理图。

图5为本发明的原理图之二。

具体实施方式

下面结合附图和实施例对本发明作进一步说明：

如图1所示，空间六自由度运动的测量装置，由定平台1、虎克铰2、支链、旋转编码器5、球铰9和动平台10等构成。其中，在定平台1上装有六个虎克铰2，编号分别为U₁、U₂、U₃、U₄、U₅、U₆，这六个虎克铰2围成一个六角形，该六角形可以是正六角形也可以是非正六角形，六个角可不在同一平面。作为优选，本实施例中定平台1为正六边形平板结构，虎克铰2安装在定平台1的六个角处，六个虎克铰2围成正六角形。在所述定平台1的上方设有动平台10，该动平台10上装有六个球铰9，编号分别为S₁、S₂、S₃、S₄、S₅、S₆，六个球铰9也围成一个六角形，该六角形可以是正六角形也可以是非正六角形，六个角可不在同一平面。作为优选，本实施例中动平台10为正六边形平板结构，并位于定平台1的正上方，动平台10与定平台1相互平行；所述球铰9安装在动平台10的六个角处，六个球铰9围成正六角形。

如图1、图2、图3所示，六个球铰9与六个虎克铰2一一对应，各球铰9均通过支链与对应的虎克铰2相连接，支链相应地具有六组。所述支链包括上支杆8、下支杆3和旋转副6，上支杆8和下支杆3的结构相同，均由螺纹套筒、螺杆和锁定螺钉7组成，螺杆的一端伸入螺纹套筒内，两者之间螺纹配合，并通过沿螺纹套筒径向穿设的锁定螺钉7顶紧。所述上支杆8的螺杆向上伸出，上支杆8的螺杆的上端与相应的球铰9连接，上支杆8的螺纹套筒的下端通过旋转副6与下支杆3的螺纹套筒的上端铰接，下支杆3的螺杆向下伸出，下支杆3的螺杆的下端与相应的虎克铰2连接。所述上支杆8的编号分别为

六个上支杆8长度可不相同，调整上支杆8中的螺旋副可改变上支杆8长度，长度调整后由锁定螺钉7重新锁定。下支杆3的编号分别为

六个下支杆3长度可不相同，调整下支杆3中的螺旋副可改变下支杆3长度，长度调整后由锁定螺钉4重新锁定。

如图1、图2所示，在每组支链的旋转副6上均安装有旋转编码器5，同一支链上的旋转副6位于该支链所对应的球铰9与虎克铰2中心连线的外侧，同一支链所对应的球铰9与虎克铰2中心连线同对应的旋转副6位于一个平面内，该支链只能在这个平面内运动。所述旋转副优选为销子，也可以是其它能够使上下支杆相对旋转的部件。

利用上述结构进行空间六自由度运动的动态测量方法，包括以下步骤：

1)将被测运动物体刚性固定在动平台10上，定平台1刚性固定在相对静止不动的物体上(定平台1和动平台10可以平行安装或不平行安装；定平台1和动平台10的几何中心连线可以垂直于定平台也可以不垂直于定平台。为简化算法，本实施例中定平台1和动平台10相平行，定平台1和动平台10的几何中心连线垂直于定平台)。如图3所示，上述装置有18个运动副，其中自由度为1的运动副数目为6个，与动平台连接的自由度为3的球铰运动副数目为6个，与定平台连接的自由度为2的虎克铰运动副数目为6个，独立闭环数为5，故此机构的自由度为F＝6×(1+3+2)-6×5＝6。

在被测运动物体上设定空间直角正交体坐标系o-xyz，在动平台上球铰9所围成六角形的几何中心设定空间直角正交体坐标系动坐标系S-X'Y'Z'，S-X'Y'Z′与动平台固连，其原点S与动平台的形心重合，Z′轴垂直于动平台向上，X′轴与S₁S₆垂直，Y′轴平行于S₁S₆，在定平台上虎克铰2所围成六角形的几何中心设定空间直角正交体坐标系O-XYZ，其原点O与定平台的形心重合，Z轴垂直向上，Y轴与U₁U₆垂直，X轴平行于U₁U₆。

2)被测运动物体空间六自由度的运动带动动平台10运动及旋转编码器5转动，旋转编码器5测量旋转副6的上支杆8与下支杆3的相对运动的角度时间曲线θ_i(t)，i=1，2，3，4，5，6。

3)根据测量空间六自由度运动的6-URS并联机构特点，由旋转编码器5的角度时间曲线正解，得出被测运动物体空间六自由度的运动规律。其中，本步骤如下过程：

a、运动分解：

动平台的运动可分解为随S-X'Y'Z'坐标原点S沿O-XYZ三个坐标轴方向上的平移(X(t)，Y(t)，Z(t))^T，以及绕坐标轴的旋转(α(t)，β(t)，γ(t))^T。为了避免发生角度间的“耦合”，一般采用欧拉角来描述刚体的旋转状态，而欧拉角的定义又随旋转次序的不同而不同。本发明中将欧拉角定义为依次绕Z轴旋转γ，绕Y轴旋转β，绕X轴旋转α。同理，被测运动物体可分解为随o-xyz坐标原点o沿S-X'Y'Z'三个坐标轴方向上的平移(x(t)，y(t)，z(t))^T，以及绕坐标轴的旋转(θ_x(t)，θ_y(t)，θ_z(t))^T；

b、坐标变换：

S_i(i=1，2，3，4，5，6)表示动平台上的各球铰点，U_i(i＝1，2，3，4，5，6)表示定动平台上的各虎克铰点；S_i(i＝1，2，3，4，5，6)在固定空间直角坐标系O-XYZ和动空间直角坐标系S-X'Y'Z'中的坐标向量表示分别为S(S_iX，S_iY，S_iZ)^T、S(S_iX′，S_iY′，S_iZ′)^T；U_i(i＝1，2，3，4，5，6)在固定空间直角坐标系O-XYZ中的坐标向量表示分别为U(U_iX，U_iY，U_iZ)^T，l_i表示杆S_iU_i的长度(i＝1，2，3，4，5，6)；静坐标向量S(S_iX，S_iY，S_iZ)^T和坐标向量S(S_iX′，S_iY′，S_iZ′)^T有如下变换公式：

$\left[\begin{array}{c}{S}_{\mathrm{iX}}\\ S_{\mathrm{iY}}\\ S_{\mathrm{iZ}}\end{array}\right]=\left[T\right]\&times;\left[\begin{array}{c}{S}_{{\mathrm{iX}}^{\&prime;}}\\ S_{{\mathrm{iX}}^{\&prime;}}\\ S_{{\mathrm{iX}}^{\&prime;}}\end{array}\right]+\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right],$ i＝1，2，3，4，5，6

其中T为坐标变换公式，这里有：

$\left[T\right]=\left[\begin{array}{ccc}\cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}& -\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}& \sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}\\ \cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}\\ -\sin \mathrm{\&beta;}& \sin \mathrm{\&alpha;}\cos \mathrm{\&beta;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}\end{array}\right]$

根据上下支杆旋转角度的变化，利用机构运动学算法解算出动平台的六自由度运动变化，得出动平台的空间位姿V＝(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T，通过坐标变换得到被测运动物体的六自由度运动变化，确定被测物体的空间位姿W＝(x(t)，y(t)，z(t)，θ_x(t)，θ_y(t)，θ_z(t))^T；通过旋转编码器电机上的编码记录的旋转角度，能够精确地的得出上球铰与下虎克铰距离的变化Δl_i(t)(i＝1，2，3，4，5，6)；则上球铰与下虎克铰的距离可表示为：

(i＝1，2，3，4，5，6)，其中l_i(t)表示状态角度为θ时上球铰与下虎克铰的距离，l_下为转动副中心到虎克铰中心的距离，l_上为转动副中心到球铰中心的距离，则动平台位姿(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T和六组球铰及胡可铰的距离l_i(t)的非线性约束方程式为：

F_i(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))=l_i ²-[(S_ix-U_ix)²+(S_iy-U_iy)²+(S_iz-U_iz)²]＝0，(i＝1，2，3，4，5，6)

c、迭代解法：

令V＝(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T，F(V)=(f₁(V)，f₂(V)，f₃(V)，f₄(V)，f₅(V)，f₆(V))^T，选择适当的初始点V₀(如：V₀=(0，0，1，0，0，0))。F_i(V₀)(i＝1，2，3，4，5，6)在V₀附近进行泰勒展开，取得其线性部分作为动平台姿态的牛顿迭带公式：

V_k+1=V_k-[J(V_k)]^-1F(V_k)(k＝0,1，2，...)

其中

经过多次迭代后，当满足|V_k+1-V_k|<E(在这里E为求解后所要达到的精度要求)时终止迭代，此时的V_k+1便是满足精度要求的动平台空间位姿V=(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T的值。再经坐标转换矩阵的转换、平移，可得被测运动物体的空间位姿W＝(x(t)、y(t)、z(t)，θ_x(t)、θ_y(t)、θ_z(t))^T；被测运动物体上的空间直角坐标系相对动平台上坐标系的坐标转换矩阵T′：

$T^{\&prime;}=\left[\begin{array}{ccc}{\cos \mathrm{\&theta;}}_y{\cos \mathrm{\&theta;}}_z& -{\cos \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_z+{\sin \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_y{\cos \mathrm{\&theta;}}_z& {\sin \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_z+{\cos \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_y{\cos \mathrm{\&theta;}}_z\\ {\cos \mathrm{\&theta;}}_y{\sin \mathrm{\&theta;}}_z& {\cos \mathrm{\&theta;}}_x{\cos \mathrm{\&theta;}}_z+{\sin \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_y{\sin \mathrm{\&theta;}}_z& -{\sin \mathrm{\&theta;}}_x{\cos \mathrm{\&theta;}}_z+{\cos \mathrm{\&theta;}}_x{\sin \mathrm{\&theta;}}_y{\sin \mathrm{\&theta;}}_z\\ -{\sin \mathrm{\&theta;}}_y& {\sin \mathrm{\&theta;}}_x{\cos \mathrm{\&theta;}}_y& {\cos \mathrm{\&theta;}}_x{\cos \mathrm{\&theta;}}_y\end{array}\right].$

误差分析

6-URS并联机构的实际位姿与理论位姿的偏差称为并联机构的位姿误差，这是个直接关系到并联机构自身质量的关键指标。

末端位姿矩阵T是机构参数P和运动输入参数θ的函数，即：

T=f(p，θ)

其中p=[p₁，p₂，..，p_n]^T是模型参数，n为参数个数，θ=[θ₁，θ₂，..，θ_m]^T是关节变量，m指有m个关节变量。

显然上式是通式，无论是串联机构还是并联机构都是适用的，而在并联机构的运动学中，往往是用下式来表达的：

T=f(R_m，R_b，L)

其中R_m=[R_m1，R_m2，..，R_mi]^T是动平台中心到分支上铰链点中心矢量。R_b=[R_b1，R_b2，..，R_bi]^T是定平台中心到分支下铰接点的中心矢量，如图5所示。在各自的坐标系中认为是个不变量，L=[L₁，L₂，...，L_i]^T是分支矢量，是个变量，其中i代表分支数。因此，测量空间六自由度运动的6-URS并联机构的工作空间位姿完全可以由这三个矢量来确定。

如图5所示，测量空间六自由度运动的6-URS并联机构运动学中等效广义坐标Q=[Q₁，Q₂，...，Q₆]^T可以用于表示动平台位姿。因此，考查动平台位姿误差[dT]可由[dQ]等效表示。之所以这样，原因是位姿T需要表达位姿的矢量P和表达姿态的[n，o，a]共4个矢量来表示，而等效广义坐标Q的前三个元素表达了末端的位姿，后三个元素表达了末端的姿态，因此仅用一个Q矢量来表示末端位姿在形式上要简洁得多。为了表征各参数误差对末端位姿误差影响大小，引入并联机构特征参数误差灵敏矢量定义：

$S_i=\frac{\mathrm{dQ}}{{\mathrm{dp}}_i}(i=\mathrm{1,2},...,36)$

依据上式，对六分支并联机构而言，i的最大值是36，则有误差灵敏矢量矩阵：

E=[E₁，E₂，...，E₃₆，]_6X36

上式为一6x36矩阵，将上式展开

$E={\left[\begin{array}{cccc}\frac{{\mathrm{dQ}}_1}{{\mathrm{dp}}_1}& \frac{{\mathrm{dQ}}_1}{{\mathrm{dp}}_2}& \&CenterDot;\&CenterDot;\&CenterDot;& \frac{{\mathrm{dQ}}_1}{{\mathrm{dp}}_{36}}\\ \frac{{\mathrm{dQ}}_2}{{\mathrm{dp}}_1}& \frac{{\mathrm{dQ}}_2}{{\mathrm{dp}}_2}& & \frac{{\mathrm{dQ}}_2}{{\mathrm{dp}}_{36}}\\ \&CenterDot;& & & \\ \&CenterDot;& & & \\ \&CenterDot;& & & \\ \frac{{\mathrm{dQ}}_6}{{\mathrm{dp}}_1}& \frac{{\mathrm{dQ}}_6}{{\mathrm{dp}}_2}& \&CenterDot;\&CenterDot;\&CenterDot;& \frac{{\mathrm{dQ}}_6}{{\mathrm{dp}}_{36}}\end{array}\right]}_{6x36}$

上式就是误差传递矩阵。为了充分反映工作空间内末端误差的情况，可以进一步写出上式在工作空间内的统计特性。为了描述方便，E矩阵中的r行s列用e_rs表示，则上式中，各元素的统计特性可以描述为：

$e_{\mathrm{rs}}=\frac{1}{N}\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{e}_{\mathrm{rs}}\left(k\right)$

${\mathrm{\&sigma;}}_{\mathrm{rs}}={\left\{\frac{\left[\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}(e_{\mathrm{rs}}\left(k\right)-{e_{\mathrm{rs}}}^2\right]}{N-1}\right\}}^2$

其中r＝1，2，...，6；s＝1，2，...，36。N为机构位姿[T]的采样数。其中e_rs称为并联机构位姿误差放大因子；σ_rs是考查该位姿误差放大因子的离散程度。因此，采用上述两式就可以定量的分析并联机构特征参数误差对末端位姿误差的影响。

Claims (3)

1.一种利用测量装置进行空间六自由度运动的动态测量方法，在测量装置的定平台（1）的上方设有动平台（10），其特征在于：在所述定平台（1）上装有六个虎克铰（2），这六个虎克铰（2）围成一个六角形；所述动平台（10）上装有六个球铰（9），六个球铰（9）也围成一个六角形，并且六个球铰（9）与六个虎克铰（2）一一对应；所述六个球铰（9）均通过支链与对应的六个虎克铰（2）相连接，该支链包括上支杆（8）、下支杆（3）和旋转副（6），所述上支杆（8）的上端与对应的球铰（9）连接，上支杆（8）的下端通过旋转副（6）与下支杆（3）的上端铰接，在旋转副（6）上安装有旋转编码器（5），所述下支杆（3）的下端与对应的虎克铰（2）连接，同一支链上的旋转副（6）位于该支链所对应的球铰（9）与虎克铰（2）中心连线的外侧，同一支链所对应的球铰（9）与虎克铰（2）中心连线同对应的旋转副（6）位于一个平面内，该支链只能在这个平面内运动；

所述测量方法包括以下步骤：

1）将被测运动物体刚性固定在动平台（10）上，定平台（1）刚性固定在相对静止不动的物体上，六个球铰（9）分别编号为S₁、S₂、S₃、S₄、S₅、S₆，六个虎克铰（2）分别编号为U₁、U₂、U₃、U₄、U₅、U₆；在被测运动物体上设定空间直角正交体坐标系o-xyz，在动平台上球铰（9）所围成六角形的几何中心设定空间直角正交体坐标系动坐标系S-X'Y'Z'，S-X'Y'Z'与动平台固连，其原点S与动平台的形心重合，Z'轴垂直于动平台向上，X'轴与S₁S₆垂直，Y'轴平行于S₁S₆，在定平台上虎克铰（2）所围成六角形的几何中心设定空间直角正交体坐标系O-XYZ，其原点O与定平台的形心重合，Z轴垂直向上，Y轴与U₁U₆垂直，X轴平行于U₁U₆；

2）被测运动物体空间六自由度的运动带动动平台（10）运动及旋转编码器（5）转动，旋转编码器（5）测量旋转副（6）的上支杆（8）与下支杆（3）的相对运动的角度时间曲线θ_i（t），i=1，2，3，4，5，6；

3）由旋转编码器（5）的角度时间曲线正解，得出被测运动物体空间六自由度的运动规律；所述步骤3）包含如下过程：

a、运动分解：

动平台的运动可分解为随S-X'Y'Z'坐标原点S沿O-XYZ三个坐标轴方向上的平移（X（t），Y（t），Z（t））^T，以及绕坐标轴的旋转（α（t），β（t），γ（t））^T；被测运动物体可分解为随o-xyz坐标原点o沿S-X'Y'Z'三个坐标轴方向上的平移（x(t)，y(t)，z(t)）^T，以及绕坐标轴的旋转（θ_x（t），θ_y（t），θ_z（t））^T；

b、坐标变换：

S_i(i=1,2,3,4,5,6)表示动平台上的各球铰点，U_i(i=1,2,3,4,5,6)表示定动平台上的各虎克铰点；S_i(i=1,2,3,4,5,6)在固定空间直角坐标系O-XYZ和动空间直角坐标系S-X'Y'Z'中的坐标向量表示分别为S(S_iX，S_iY，S_iZ)^T、S(S_iX'，S_iY'，S_iZ')^T；U_i(i=1,2,3,4,5,6)在固定空间直角坐标系O-XYZ中的坐标向量表示分别为U(U_iX，U_iY，U_iZ)^T，l_i表示杆S_iU_i的长度(i=1,2,3,4,5,6)；静坐标向量S(S_iX，S_iY，S_iZ)^T和坐标向量S(S_iX'，S_iY'，S_iZ')^T有如下变换公式：

$\left[\begin{array}{c}{S}_{\mathrm{iX}}\\ S_{\mathrm{iY}}\\ S_{\mathrm{iZ}}\end{array}\right]=\left[T\right]\&times;\left[\begin{array}{c}{S}_{{\mathrm{iX}}^{\&prime;}}\\ S_{{\mathrm{iX}}^{\&prime;}}\\ S_{i{X}^{\&prime;}}\end{array}\right]+\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right],i=\mathrm{1,2,3,4,5,6}$

其中T为坐标变换公式，这里有：

$\left[T\right]=\left[\begin{array}{ccc}\cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}& -\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}& \sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}\\ \cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}+\mathrm{cso\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}\\ -\sin \mathrm{\&beta;}& \sin \mathrm{\&alpha;}\cos \mathrm{\&beta;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}\end{array}\right]$

根据上下支杆旋转角度的变化，利用机构运动学算法解算出动平台的六自由度运动变化，得出动平台的空间位姿V=(X（t），Y（t），Z（t），α（t），β（t），γ（t）)^T，通过坐标变换得到被测运动物体的六自由度运动变化，确定被测物体的空间位姿W=（x(t)，y(t)，z(t)，θ_x（t），θ_y（t），θ_z（t））^T；通过旋转编码器电机上的编码记录的旋转角度，能够精确地的得出上球铰与下虎克铰距离的变化Δl_i（t）（i=1，2，3，4，5，6）；则上球铰与下虎克铰的距离可表示为：

（i=1，2，3，4，5，6），其中l_i(t)表示状态角度为θ时上球铰与下虎克铰的距离，l_下为转动副中心到虎克铰中心的距离，l_上为转动副中心到球铰中心的距离，则动平台位姿（X（t），Y（t），Z（t），α（t），β（t），γ（t））^T和六组球铰及胡可铰的距离l_i（t）的非线性约束方程式为：

F_i(X（t），Y（t），Z（t），α（t），β（t），γ（t）)=l_i ²–[(S_ix–U_ix)²+(S_iy–U_iy)²+(S_iz–U_iz)²]=0，(i=1,2,3,4,5,6)

c、迭代解法：

令V=(X（t），Y（t），Z（t），α（t），β（t），γ（t）)^T，F(V)=(f₁(V)，f₂(V)，f₃(V)，f₄(V)，f₅(V)，f₆(V))^T，选择初始点V₀，F_i(V₀)(i=1,2,3,4,5,6)在V₀附近进行泰勒展开，取得其线性部分作为动平台姿态的牛顿迭带公式：

V_k+1=V_k–[J(V_k)]^-1F(V_k)  (k=0,1,2,...)

其中

经过多次迭代后，当满足|V_k+1-V_k|<E时终止迭代，此时的V_k+1便是满足精度要求的动平台空间位姿V=(X（t），Y（t），Z（t），α（t），β（t），γ（t）)^T的值，再经坐标转换矩阵的转换、平移，可得被测运动物体的空间位姿W=（x(t)、y(t)、z(t)，θ_x（t）、θ_y（t）、θ_z（t））^T；被测运动物体上的空间直角坐标系相对动平台上坐标系的坐标转换矩阵T'：

$T^{\&prime;}=\left[\begin{array}{ccc}\cos {\mathrm{\&theta;}}_y\cos {\mathrm{\&theta;}}_z& -\cos {\mathrm{\&theta;}}_x\sin {\mathrm{\&theta;}}_z+\sin {\mathrm{\&theta;}}_x\sin {\mathrm{\&theta;}}_y\cos {\mathrm{\&theta;}}_z& \sin {\mathrm{\&theta;}}_x\sin {\mathrm{\&theta;}}_z+\cos {\mathrm{\&theta;}}_x\sin {\mathrm{\&theta;}}_y\cos {\mathrm{\&theta;}}_z\\ \cos {\mathrm{\&theta;}}_y\sin {\mathrm{\&theta;}}_z& \cos {\mathrm{\&theta;}}_x\cos {\mathrm{\&theta;}}_z+\sin {\mathrm{\&theta;}}_x\sin {\mathrm{\&theta;}}_y\sin {\mathrm{\&theta;}}_z& -\sin {\mathrm{\&theta;}}_x\cos {\mathrm{\&theta;}}_z+\cos {\mathrm{\&theta;}}_x\sin {\mathrm{\&theta;}}_y\sin {\mathrm{\&theta;}}_z\\ -\sin {\mathrm{\&theta;}}_y& \sin {\mathrm{\&theta;}}_x\cos {\mathrm{\&theta;}}_y& \cos {\mathrm{\&theta;}}_x\cos {\mathrm{\&theta;}}_y\end{array}\right].$

2.根据权利要求1所述的利用测量装置进行空间六自由度运动的动态测量方法，其特征在于：所述上支杆（8）和下支杆（3）的结构相同，均由螺纹套筒、螺杆和锁定螺钉组成，螺杆的一端伸入螺纹套筒内，两者之间螺纹配合，并通过径向穿设的锁定螺钉顶紧；所述上支杆（8）的螺杆与球铰（9）连接，上支杆（8）的螺纹套筒通过旋转副（6）与下支杆（3）的螺纹套筒铰接，下支杆（3）的螺杆与虎克铰（2）连接。

3.根据权利要求2所述的利用测量装置进行空间六自由度运动的动态测量方法，其特征在于：所述定平台（1）为正六边形，虎克铰（2）安装在定平台（1）的六个角处；所述动平台（10）也为正六边形，并位于定平台（1）的正上方，所述球铰（9）安装在动平台（10）的六个角处。

Images