[发明专利]空间六自由度运动的六位移传感器动态测量法

权利要求书

1.一种空间六自由度运动的六位移传感器动态测量法，其特征在于包括以下步骤：

1)搭建六位移传感器并联测量机构：

搭建的六位移传感器并联测量机构包括上平台、下平台和六个拉线式位移传感器，上平台和下平台上各有六个安装点用于安装拉线式位移传感器，同一平台上的六个安装点在一个圆上或者六角形的六个角处，上平台的六个安装点与下平台的六个安装点一一对应，上平台的安装点与对应的下平台的安装点之间按照最短距离原则连接拉线式位移传感器，六个拉线式位移传感器无触碰，所述下平台固定在基座上，上平台上固定被测物体，并随被测物体一起运动；

2)建立坐标系：

在上平台上建立动坐标系o′-x′y′z′，下平台上建立静坐标系o-xyz，A_i，B_i分别为六个拉线式位移传感器在上平台和下平台中的接触点，A_i，B_i在直角坐标系o-xyz中的向量表示分别为A(A_ix，A_iy，A_iz)、B(B_ix，B_iy，B_iz)，l_i表示A_iB_i之间的位移，求解过程中，运动物体的六自由度x(t)、y(t)、z(t)、θx(t)、θy(t)、θz(t)表示为x，y，z，θ_x，θ_y，θ_z，便于计算；

3)位置逆解：

当上平台改变时，根据平面与平面上点的关系求出此时新点的坐标值A，

A＝T*A_i′+P

T为上平台姿态的方向余弦矩阵，A_i′为A_i点在动坐标系的位置向量，P为上平台上的动坐标系相对于下平台静坐标系中的位移矢量；

设上下两个平台对应安装点之间的初始长度为l_i0，i＝1，2·6；上下两平台各对应安装点之间的连线作为六个分支，各分支的实时测量长度可以表示成被测运动物体位置参数的函数：

li=|Ai-Bi|=(Aix-Bix)2-(Aiy-Biy)2-(Aiz-Biz)2=gi(x,y,z,θx,θy,θz)---(1.2)]]>

式中，A_ix，A_iy，A_iz为拉线式位移传感器上端接触点在定坐标系中的位置，B_ix，B_iy，B_iz为拉线式位移传感器下端接触点在定坐标系中的位置，因为被测运动物体在上平台的位姿逆解即拉线式位移传感器测得的长度变化量：

Δl_i＝l_i-l_i0＝g_i(x，y，z，θ_x，θ_y，θ_z)-l_i0              (1.3)

将被测运动物体的六个位姿参数代入式(1.3)，可求出测量被测物体时拉线式位移传感器测得的长度变化量，然后据式l_i0＝l_i-Δl_i再调整上下平台之间的距离；

4)位置正解

正解方程由逆解方程变化而来

l²＝(A-B_i)(A-B_i)^T

B_i为拉线式位移传感器下端点在下平台静坐标系中的坐标，l_i为拉线式位移传感器测得的长度

令Σi=16fi(x,y,z,θx,θy,θz)=(Ti*A′i+Pi-Bi)(Ti*A′i+Pi-Bi)T-li2=0---(1.4)]]>

首先令初始点x，y，z，θ_x，θ_y，θ_z＝(0，0，0，0，0，0)；将f_i(A_i)(i＝1，2，…6)在A_i附近进行泰勒展开，并取其线性部分，得：

fi(Ai)+Σk=16(xk-xk)∂fi(Ai)∂xk=0]]>

式(1.4)可以看作是以A_i为未知数的线性方程组，其雅可比系数矩阵为：

J=∂f1∂x1∂f1∂x2·∂f1∂x6∂f2∂x1∂f2∂x2·∂f2∂x6·∂f6∂x1∂f6∂x2·∂f6∂x6]]>

通过初等行变换求雅可比矩阵的逆矩阵，这样完全转化为求矩阵的加减和乘法运算；其中(x₁，x₂，x₃，x₄，x₅，x₆)＝(x，y，z，θ_x，θ_y，θ_z)；解非线性方程组(1.4)就可得到被测物体的运动姿态x，y，z，θ_x，θ_y，θ_z。

2.根据权利要求1所述的空间六自由度运动的六位移传感器动态测量法，其特征在于：所述上平台和下平台均为正六边形或圆形，上平台位于下平台的正上方。