CN103471545A - 六自由度位姿测量系统及测量方法 - Google Patents

Abstract

本发明公开了一种六自由度位姿测量系统及测量方法，旨在克服现有技术不能在实际环境下进行测量，且结构复杂、测量精度低与成本高的问题。该测量系统包括六自由度位姿测量仪、数据存储装置、工控机及显示器。六自由度位姿测量仪包括六自由度位姿测量仪上固定板、六自由度位姿测量仪下固定板与6个拉线式位移传感器总成。6个拉线式位移传感器总成皆由拉线式位移传感器固定板、拉线式位移传感器与万向节连接头组成。6个拉线式位移传感器总成中的万向节连接头与六自由度位姿测量仪上固定板连接，6个拉线式位移传感器总成中的拉线式位移传感器固定板与六自由度位姿测量仪下固定板连接。本发明还提供了一种采用六自由度位姿测量系统测量位姿的方法。

Description

六自由度位姿测量系统及测量方法

技术领域

本发明涉及一种轨道车辆转向架参数检测装置，更确切地说，本发明涉及一种六自由度位姿测量系统及测量方法。

背景技术

目前国内外对于物体六自由度位姿已将进行了较为深入的研究并开发出了相应的测量系统。对物体的六自由度位姿测量主要通过GPS技术、磁场感应技术、图像视觉处理技术及直接接触式测量几种方式实现。在应用GPS技术方面，美国的Adroit公司推出的Adroit ADS姿态测量系统，Trimble公司推出的MS860定位与姿态测量系统；在应用磁场感应技术方面，美国的William R.Patterson及Harvey F.Silverman等人开发出用于测量远距离物体位置与角度的磁性追踪系统，HIVISONG、Honeywell等公司均推出了电子罗盘，北京理工大学基于LabVIEW平台开发出了虚拟电子罗盘产品，清华大学开发出了利用MEMS技术的微型姿态测量装置；在应用图像视觉处理技术方面，美国的Perceptron、Faro公司，英国的3D Scanners公司均开发出了相应的视觉测量系统。北京航空航天大学的张广军、周福强等人提出一种摄像机空间三维位姿测量方法，中国专利公开号为CN1804541A，公开日为2006年7月19日；北京理工大学的王全玉等人提出了利用光束确定位置姿态的系统与方法，中国专利公开号为CN101308014A，公开日为2008年11月19日；在通过直接测量获得相应的六自由度位姿参数主要在国内得到研究与应用，北京邮电大学的魏世民、廖启征等提出了一种用于空间全位置和姿态的测量方法，中国专利公开号为CN1346964A，公开日为2002年5月1日；山东理工大学的宫金良、张彦斐等人提出六维位置姿态传感器，中国专利公开号为CN101178306A，公开日为2002年5月1日；吉林大学的苏建、陈熔、宋健等人提出的六杆六铰点六自由度测量装置，中国专利公开号为CN102878963A，公开日为：2013年1月16日。

由于转向架参数测定试验台加载平台的六自由度位姿测量系统需要在室内安装，安装空间较小，且周围有较强的磁场干扰，因此各六自由度测量方式与系统在实际的应用过程中均存在一定的使用限制。GPS技术更适用于在视野开阔，障碍物较少的地区进行定位测量，尤其不适用于进行室内位姿测量；磁场感应原理由于其检测原理的限制，易受来自于周围环境磁场的影响，因此并不适用于有着复杂电磁环境情况下的物体位置姿态检测；图像视觉处理技术为了对被测物体进行实时姿态检测，需要实时获得物体位置变化的图像，并进行实时图像处理与解算，所需系统体积较大，且价格昂贵，解算过程也较为复杂。

现有的直接接触式六自由度位姿测量系统具有结构复杂、测量精度低、成本高的缺陷。吉林大学的苏建等人提出的六杆六铰点六自由度测量装置虽较好解决了一般检测系统结构复杂、制造成本高的缺陷，但并未提出对应的解算系统，不能直接测量得出被测物体的位姿参数，且工作空间小不能满足实际要求。

发明内容

本发明所要解决的技术问题是克服了现有的六自由度位姿测量系统不能满足轨道车辆转向架参数测定试验台加载平台实际环境下进行测量，且结构复杂、测量精度低、成本高的问题，提出一种满足实际测试环境要求，并能够实时获得位姿参数的六自由度位姿测量系统及测量方法。

为解决上述技术问题，本发明是采用如下技术方案实现的：所述的六自由度位姿测量系统包括六自由度位姿测量仪、数据存储装置、工控机及显示器。

所述的六自由度位姿测量仪包括六自由度位姿测量仪上固定板、1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、六自由度位姿测量仪下固定板、4号拉线式位移传感器总成、5号拉线式位移传感器总成与6号拉线式位移传感器总成。

所述的1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、4号拉线式位移传感器总成、5号拉线式位移传感器总成与6号拉线式位移传感器总成的结构相同，皆由拉线式位移传感器固定板、拉线式位移传感器与万向节连接头组成。

拉线式位移传感器采用螺栓固定于拉线式位移传感器固定板的右侧固定板上，拉线式位移传感器中的拉线伸出端与万向节连接头为球头连接。

1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、4号拉线式位移传感器总成、5号拉线式位移传感器总成与6号拉线式位移传感器总成中的万向节连接头分别采用螺栓与六自由度位姿测量仪上固定板固定连接，1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、4号拉线式位移传感器总成、5号拉线式位移传感器总成与6号拉线式位移传感器总成中的拉线式位移传感器固定板的左侧固定板分别采用螺栓与六自由度位姿测量仪下固定板固定连接。

技术方案中所述的1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、4号拉线式位移传感器总成、5号拉线式位移传感器总成与6号拉线式位移传感器总成中的万向节连接头分别采用螺栓与六自由度位姿测量仪上固定板固定连接是指：1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、4号拉线式位移传感器总成、5号拉线式位移传感器总成与6号拉线式位移传感器总成中的万向节连接头依次与六自由度位姿测量仪上固定板上的1号上连接头通孔、2号上连接头通孔、3号上连接头通孔、4号上连接头通孔、5号上连接头通孔及6号上连接头通孔采用螺栓连接。1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、4号拉线式位移传感器总成、5号拉线式位移传感器总成与6号拉线式位移传感器总成中的拉线式位移传感器固定板与六自由度位姿测量仪下固定板固定连接是指：1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、4号拉线式位移传感器总成、5号拉线式位移传感器总成与6号拉线式位移传感器总成中的拉线式位移传感器固定板依次与1号下固定板通孔、2号下固定板通孔、3号下固定板通孔、4号下固定板通孔、5号下固定板通孔及6号下固定板通孔采用螺栓连接。

技术方案中所述的六自由度位姿测量仪安装的初始状态为六自由度位姿测量仪上固定板与六自由度位姿测量仪下固定板相互平行，六自由度位姿测量仪上固定板的上中心通孔与六自由度位姿测量仪下固定板的下中心通孔的回转轴线共线，同时六自由度位姿测量仪上固定板与六自由度位姿测量仪下固定板的三个顶角对正后六自由度位姿测量仪上固定板的上中心通孔绕六自由度位姿测量仪下固定板的下中心孔旋转60°。

技术方案中所述的六自由度位姿测量仪上固定板为等边三角形的板类结构件，六自由度位姿测量仪上固定板的中心处设置有上中心通孔，上中心通孔周围的圆周上均匀分布有安装螺栓的上螺栓通孔，六自由度位姿测量仪上固定板的三个顶角处设置有用于安装1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、4号拉线式位移传感器总成、5号拉线式位移传感器总成与6号拉线式位移传感器总成中的万向节连接头的上连接头通孔，即1号上连接头通孔、2号上连接头通孔、3号上连接头通孔、4号上连接头通孔、5号上连接头通孔及6号上连接头通孔。

技术方案中所述的六自由度位姿测量仪下固定板与六自由度位姿测量仪上固定板形状相同，皆为等边三角形的板类结构件，六自由度位姿测量仪下固定板的中心处设置有下中心通孔，下中心通孔的周围的圆周上也均匀分布有安装螺栓的下螺栓通孔，并在下螺栓通孔的周围设置有呈等边三角形分布的2号螺栓通孔，六自由度位姿测量仪下固定板三个顶角处的下方对称地设置有用于安装1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、4号拉线式位移传感器总成、5号拉线式位移传感器总成与6号拉线式位移传感器总成中的拉线式位移传感器固定板的下固定板通孔，即1号下固定板通孔、2号下固定板通孔、3号下固定板通孔、4号下固定板通孔、5号下固定板通孔及6号下固定板通孔。

技术方案中所述的六自由度位姿测量仪中的六自由度位姿测量仪下固定板与六自由度位姿测量仪支架上端采用螺栓固定连接，六自由度位姿测量仪支架下端与转向架参数测定试验台基础固定连接，或者六自由度位姿测量仪中的六自由度位姿测量仪下固定板与六自由度位姿测量仪安装法兰立柱的一端固定连接，六自由度位姿测量仪安装法兰立柱的另一端与转向架参数测定试验台框架固定连接。

技术方案中所述的六自由度位姿测量仪支架由六自由度位姿测量仪支架上立柱与六自由度位姿测量仪支架下立柱组成，六自由度位姿测量仪支架上立柱与六自由度位姿测量仪支架下立柱皆为等横截面的U字形的冲压结构件，六自由度位姿测量仪支架上立柱的中心处设置有上立柱中心通孔，上立柱中心通孔的周围设置有呈等边三角形分布的上立柱螺栓通孔，上立柱螺栓通孔结构、孔距和六自由度位姿测量仪下固定板上的呈等边三角形分布的2号螺栓通孔的结构、孔距相等。

六自由度位姿测量仪支架下立柱由立柱主体与安装座组成，立柱主体的底端设置有2个底端孔，立柱主体的下端与安装座的中心位置垂直并采用焊接方式连接成一体，安装座的周围设置有安装螺栓的安装座通孔，六自由度位姿测量仪支架上立柱与六自由度位姿测量仪支架下立柱之间采用连接板与螺栓紧固连接，连接板的一端与六自由度位姿测量仪支架上立柱的下端采用焊接连接，连接板的另一端与六自由度位姿测量仪支架下立柱上端螺栓连接。

技术方案中所述的六自由度位姿测量仪安装法兰立柱为圆筒类结构件，六自由度位姿测量仪安装法兰立柱由圆筒主体、左端法兰盘与右端法兰盘组成。左端法兰盘与右端法兰盘上分别均匀分布有安装螺栓的法兰通孔，左端法兰盘与右端法兰盘上的法兰通孔距左端法兰盘与右端法兰盘回转中心的回转半径和六自由度位姿测量仪下固定板的下中心通孔外侧圆周上的螺栓通孔距下中心通孔回转中心的回转半径相等，左端法兰盘与右端法兰盘上相邻两法兰通孔的孔距与六自由度位姿测量仪下固定板上相邻两螺栓通孔的孔距也相等，左端法兰盘与右端法兰盘依次焊接固定在圆筒主体的左端与右端，左端法兰盘、圆筒主体与右端法兰盘的回转轴线共线。

一种采用六自由度位姿测量系统测量物体实时六自由度位姿方法的步骤如下：

1.对六自由度位姿测量仪（A）进行建模并建立牛顿迭代公式：

x'_k+1=x'_k+J_i,sx ^-1(x'_k)(l_f-l_k)           （9）

式中：l_f表示当前需要解算状态下各拉线式位移传感器总成伸缩量的向量，x'表示所需求的位移及欧拉角度值的向量；

2.进行坐标变换上方六自由度加载平台：

根据六自由度位姿测量系统测量点并不在六自由度位姿测量仪理论模型六自由度位姿测量仪上固定板的中心位置的情况，设计模块实现测量点与六自由度位姿测量仪上固定板中心点的位姿参数转换即进行坐标变换。

设六自由度位姿测量仪中的六自由度位姿测量仪上固定板中心点坐标点为原点，下方六自由度加载平台中心点相对于坐标原点的空间坐标点为p[x,y,z]，由于刚体在转动时，各点旋转的欧拉角度值均相同，则使用欧拉角的旋转矩阵R乘以P即可得到经过旋转之后的下方六自由度加载平台中心点空间坐标值，其值与原坐标值差值的大小即为由于刚体的旋转而产生的空间坐标值的位移的变化，将此数值与平动位移量相加即得转化后的下方六自由度加载平台中心点位移的变化，由于欧拉角度值相同，可以直接得出需要测量的下方六自由度加载平台Ⅰ中心点六个位姿参数；

3.输出2个结构相同的下方六自由度加载平台与上方六自由度加载平台中心点六个位姿参数。

技术方案中所述的对六自由度位姿测量仪进行建模并建立牛顿迭代公式步骤如下：

1）以六自由度位姿测量仪上固定板中心点作为坐标原点建立坐标系，根据六自由度位姿测量仪上固定板与各拉线式位移传感器总成连接点相对于六自由度位姿测量仪上固定板的连接点半径、六自由度位姿测量仪下固定板与各拉线式位移传感器总成连接点相对于六自由度位姿测量仪下固定板中心点的连接点半径、六自由度位姿测量仪上固定板中心平面相对于六自由度位姿测量仪下固定板中心平面初始高度、六自由度位姿测量仪上固定板与各拉线式位移传感器总成相邻连接点连线相对于六自由度位姿测量仪上固定板中心点的圆心角、六自由度位姿测量仪下固定板与各拉线式位移传感器总成相邻连接点连线相对于六自由度位姿测量仪下固定板中心点的圆心角，得出1号拉线式位移传感器总成、2号拉线式位移传感器总成、3号拉线式位移传感器总成、4号拉线式位移传感器总成、5号拉线式位移传感器总成、6号拉线式位移传感器总成与六自由度位姿测量仪上固定板及六自由度位姿测量仪下固定板连接点在所建坐标系中初始坐标值，共12个。

2）通过同一个拉线式位移传感器总成两端坐标初值相减可得出相应拉线式位移传感器总成在坐标系中的空间坐标矢量初始值并求出初始杆长，以l_k表示。

3）由于物体在空间中的位姿变化由旋转及平移组成，旋转角度用欧拉角描述，各坐标点的任意时刻坐标可以利用欧拉角旋转矩阵乘以坐标点初始坐标后与空间平移量相加得出，利用各拉线式位移传感器总成两端分别与六自由度位姿测量仪上固定板及六自由度位姿测量仪下固定板相连连接点的任意时刻坐标值相减即可得到对应拉线式位移传感器总成任意时刻空间坐标矢量值：

l_i=P+RB_i-C_i               （1）

式中：P为六自由度位姿测量仪上固定板a位置变化的空间矢量，即为六自由度位姿测量仪上固定板中心的移动矢量，此处由于连接点固定于六自由度位姿测量仪上固定板上，因此移动距离相同；R为欧拉角旋转矩阵；B_i为六自由度位姿测量仪上固定板与第i号拉线式位移传感器总成连接点初始坐标值；C_i为六自由度位姿测量仪下固定板与第i号拉线式位移传感器总成连接点初始坐标值；l_i为坐标对应拉线式位移传感器总成的空间矢量。根据空间矢量的定义，各拉线式位移传感器总成的长度可以由下式得出：

|l_i|²=l_i ^Tl_i                （2）

式中：|l_i|为各拉线式位移传感器总成长度绝对值；l_i为拉线式位移传感器总成任意时刻空间坐标矢量值。

4）对六自由度位姿测量仪进行运动学分析，将六自由度位姿测量仪上固定板（a）与各拉线式位移传感器总成连接点速度同各拉线式位移传感器总成的长度变化速度联系，可知各拉线式位移传感器总成长度变化速度即为各连接点的空间速度矢量在各拉线式位移传感器总成空间矢量上的投影，对各拉线式位移传感器总成长度进行微分得出下式，并根据矢量投影原理得出下式：

$\frac{d\left|l_i\right|}{\mathrm{dt}}=\frac{{l_i}^T{V}_{\mathrm{Bi}}}{\left|l_i\right|}={l_{\mathrm{ui}}}^T{V}_{\mathrm{Bi}}---\left(3\right)$

式中：l_ui表示各拉线式位移传感器总成方向的空间单位矢量，V_Bi表示对应连接点的速度矢量，根据沙勒定律，对于刚体而言，其广义的位移等价于一个平移与一个旋转的结合，因此连接点速度矢量形式可以由以下公式得出：

V_Bi=V+ω×RB_i

式中：V_Bi表示六自由度位姿测量仪上固定板与第i个拉线式位移传感器总成连接点的速度矢量；V表示六自由度位姿测量仪上固定板中心点的速度矢量，ω表示六自由度位姿测量仪上固定板的角速度矢量；R为欧拉角旋转矩阵；B_i为六自由度位姿测量仪上固定板与第i号拉线式位移传感器总成相连连接点初始坐标值，RB_i即为任意时刻六自由度位姿测量仪上固定板a与第i号拉线式位移传感器总成相连连接点相对于坐标中心点的空间矢量值。

将上式进行整理，并与公式（3）合并改写为矩阵的形式，得到公式：

${\stackrel{\·}{l}}_i={l_{\mathrm{ui}}}^T{V}_{\mathrm{Bi}}={l_{\mathrm{ui}}}^{T}V+{l_{\mathrm{ui}}}^T(\mathrm{\ω}\×{\mathrm{RB}}_i)---\left(4\right)$

将上式进行整理，并改写为矩阵的形式，得到公式：

$\stackrel{\·}{l}={l_u}^{T}V+{(\mathrm{RB}\×l_u)}^T\mathrm{\ω}=...\·\·\·\left[\begin{array}{cc}{l_u}^T& {(\mathrm{RB}\×l_u)}^T\end{array}\right]\left[\begin{array}{c}V\\ \mathrm{\ω}\end{array}\right]=J_{l,x}\stackrel{\·}{x}---\left(5\right)$

由上式可以建立六自由度位姿测量仪上固定板速度与各拉线式位移传感器总·成速度之间的关系，实现牛顿迭代格式的初步搭建，式中均为矩阵形式，其中为各拉线式位移传感器总成的速度矩阵，l_u为各拉线式位移传感器总成的单位方向矢量矩阵，B为六自由度位姿测量仪上固定板各连接点初始坐标矩阵，J_i,x则表示六自由度位姿测量仪上固定板速度及角速度对应到各拉线式位移传感器总成与六自由度位姿测量仪上固定板相连连接点速度及角速度的雅可比矩阵。

5）由于在进行牛顿迭代时，未知量中所包含的角度应为六自由度位姿测量仪上固定板拉线式的欧拉角参数，在上式中的雅可比矩阵所对应的是六自由度位姿测量仪上固定板速度及角速度，而不是针对欧拉角的角速度建立的，因此需要进行一定的转化。对雅可比矩阵进行转化，有：

$J_{i,\mathrm{sx}}=J_{l,x}\left[\begin{array}{cc}I& \\ & \mathrm{RJ}\end{array}\right]=\left[{l_u}^T{(\mathrm{RB}\×l_u)}^T\mathrm{RJ}\right]---\left(6\right)$

式中：I为单位矩阵，J为将六自由度位姿测量仪上固定板a角速度转化为对应欧拉角速度的雅可比矩阵。

在J_i,x中上半部分表示各拉线式位移传感器总成速度关于位移的雅可比矩阵，而下半部分为各拉线式位移传感器总成速度关于六自由度位姿测量仪上固定板角速度的雅可比矩阵，因此只需对下半部分矩阵乘以转换雅可比矩阵，与旋转矩阵性质相同，不同顺规的欧拉角也对应不同的转换雅可比矩阵，相应转换雅可比矩阵为：

$J=\left[\begin{array}{ccc}1& 0& -\sin \mathrm{\β}\\ 0& \cos \mathrm{\α}& \sin \mathrm{\α}\cos \mathrm{\β}\\ 0& -\sin \mathrm{\α}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]---\left(7\right)$

根据上式可以推算得出：

$\stackrel{\·}{l}=J_{l,x}\stackrel{\·}{x}=J_{i,\mathrm{sx}}{\stackrel{\·}{x}}^{\′}---\left(8\right)$

即可得出：

x'_k+1=x'_k+J_i,sx ^-1(x'_k)(l_f-l_k)              （9）

式中：l_f表示当前需要解算状态下各拉线式位移传感器总成伸缩量的向量，由l_i组成，x'表示所需求的位移及欧拉角度值的向量；上式（9）即为求解六自由度位姿测量仪A位姿参数的牛顿迭代法迭代格式。

与现有技术相比本发明的有益效果是：

1.本发明所述的六自由度位姿测量系统及测量方法在提出硬件结构的同时提出对应的软件系统，能够实现对于相应环境下加载平台位姿的快速准确测量。

2.本发明所述的六自由度位姿测量系统及测量方法中采用的硬件系统具有结构简单、测量精度高、成本低与工作空间大的优点。

3.本发明所述的六自由度位姿测量系统及测量方法提出的六自由度位姿测量系统具有广泛的适用范围，能够在多种环境与条件下实现物体的六自由度位姿测量。

附图说明

下面结合附图对本发明作进一步的说明：

图1为本发明所述的六自由度位姿测量系统结构组成的示意框图；

图2为本发明所述的六自由度位姿测量系统所采用的六自由度位姿测量仪结构组成的轴测投影图；

图3为本发明所采用的六自由度位姿测量仪在转向架参数试验台中安装位置的轴测投影示意图；

图4为本发明采用的六自由度位姿测量仪所安装的转向架参数试验台中的下方六自由度加载平台的轴测投影图；

图5为本发明采用的六自由度位姿测量仪所安装的转向架参数试验台中的上加载平台的轴测投影图；

图6为图3中M处的局部放大轴测投影图；

图7为图3中N处的局部放大轴测投影图；

图8为本发明所采用的六自由度位姿测量仪与六自由度位姿测量仪支架连接关系的轴测投影图；

图9为本发明所采用的六自由度位姿测量仪与六自由度位姿测量仪安装法兰立柱连接关系的轴测投影图；

图10为本发明所采用的六自由度位姿测量仪中上固定板结构组成的轴测投影图，图中的1号上、2号上、3号上、4号上、5号上及6号上代表1号上连接头通孔、2号上连接头通孔、3号上连接头通孔、4号上连接头通孔、5号上连接头通孔及6号上连接头通孔；

图11为本发明所采用的六自由度位姿测量仪下固定板结构组成的轴测投影图，图中的1号下、2号下、3号下、4号下、5号下及6号下代表1号下固定板通孔、2号下固定板通孔、3号下固定板通孔、4号下固定板通孔、5号下固定板通孔及6号下固定板通孔；

图12为本发明所采用的六自由度位姿测量仪中的拉线式位移传感器总成结构组成的轴测投影图；

图13为本发明所采用的六自由度位姿测量仪中的万向节连接头结构组成的轴测投影图；

图14为本发明所采用的六自由度位姿测量仪支架结构组成的轴测投影图；

图15为本发明所采用的六自由度位姿测量仪安装法兰立柱结构组成的轴测投影图；

图16为采用本发明所述的六自由度位姿测量系统测量物体实时六自由度位姿方法的功能流程框图；

图17为采用本发明所述的基于物体实时六自由度位姿方法的Simulink程序整体图；

图中：A.六自由度位姿测量仪，B.数据存储装置，C.工控机，D.显示器，Ⅰ.下方六自由度加载平台，Ⅱ.上方六自由度加载平台，Ⅲ.转向架参数测定试验台框架，Ⅳ.转向架参数测定试验台基础，a.六自由度位姿测量仪上固定板，b.1号拉线式位移传感器总成，c.2号拉线式位移传感器总成，d.3号拉线式位移传感器总成，e.六自由度位姿测量仪下固定板，f.4号拉线式位移传感器总成，g.5号拉线式位移传感器总成，h.6号拉线式位移传感器总成，i.六自由度位姿测量仪支架，j.六自由度位姿测量仪安装法兰立柱，1.拉线式位移传感器固定板，2.拉线式位移传感器，3.万向节连接头，4.六自由度位姿测量仪支架上立柱，5.六自由度位姿测量仪支架下立柱。

具体实施方式

下面结合附图对本发明作详细的描述：

参阅图1，本发明涉及一种用于轨道车辆转向架参数测定试验台的六自由度加载平台的位姿测量系统，简曰六自由度位姿测量系统。该六自由度位姿测量系统由四个部分组成：用于采集信息的六自由度位姿测量仪A、用于存储所采集信息的数据存储装置B、用于实时处理所采集数据并可以得出对应状态下2个结构相同的下方六自由度加载平台Ⅰ与上方六自由度加载平台Ⅱ的位姿参数的工控机C及实时显示2个结构相同的下方六自由度加载平台Ⅰ与上方六自由度加载平台Ⅱ的位姿参数的显示器D，其中六自由度位姿测量仪A中的六个结构相同的拉线式位移传感器2通过串行SSI接口与数据存储装置B连接并进行信号输送，工控机C通过PCI接口与数据存储装置B相连。

参阅图2，所述的六自由度位姿测量仪A包括六自由度位姿测量仪上固定板a、1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、六自由度位姿测量仪下固定板e、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g、6号拉线式位移传感器总成h。

参阅图10，所述的六自由度位姿测量仪上固定板a为等边三角形（确切地说为9边形）的板类结构件，六自由度位姿测量仪上固定板a的中心处设置有上中心通孔，上中心通孔周围的圆周上均匀分布有（实施例中采用8个）安装螺栓的上螺栓通孔，六自由度位姿测量仪上固定板a的三个顶角处设置有用于安装1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g与6号拉线式位移传感器总成h中的万向节连接头3的3组结构相同的（每组为2×1个）上连接头通孔，即1号上连接头通孔、2号上连接头通孔、3号上连接头通孔、4号上连接头通孔、5号上连接头通孔及6号上连接头通孔，且1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g及6号拉线式位移传感器总成h中的万向节连接头3依次与六自由度位姿测量仪上固定板a上的1号上连接头通孔、2号上连接头通孔、3号上连接头通孔、4号上连接头通孔、5号上连接头通孔及6号上连接头通孔采用螺栓连接。

参阅图11，所述的六自由度位姿测量仪下固定板e与六自由度位姿测量仪上固定板a形状相同，皆为等边三角形（确切地说为9边形）的板类结构件，所述的六自由度位姿测量仪下固定板e的中心处设置有下中心通孔，下中心通孔的周围的圆周上也均匀分布有（实施例中采用8个）安装螺栓的下螺栓通孔，该下螺栓通孔用于六自由度位姿测量仪下固定板e与六自由度位姿测量仪安装法兰立柱j的连接。该下螺栓通孔的周围另外设置有呈等边三角形分布的（实施例中采用9个）2号螺栓通孔，该螺栓通孔用于六自由度位姿测量仪下固定板e与六自由度位姿测量仪支架i的连接。六自由度位姿测量仪下固定板e三个顶角处的下方对称地设置有3组用于安装1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g与6号拉线式位移传感器总成h中的拉线式位移传感器固定板1的3组结构相同的（每组为4×2个）下固定板通孔，且1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g及6号拉线式位移传感器总成h中的拉线式位移传感器固定板1依次与1号下固定板通孔、2号下固定板通孔、3号下固定板通孔、4号下固定板通孔、5号下固定板通孔及6号下固定板通孔采用螺栓连接。

参阅图12，所述的1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g与6号拉线式位移传感器总成h的结构相同，皆由拉线式位移传感器固定板1、拉线式位移传感器2与万向节连接头3组成。

所述的1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g与6号拉线式位移传感器总成h中的拉线式位移传感器2的壳体采用螺栓固定于拉线式位移传感器固定板1的右侧固定板上，拉线式位移传感器2中拉线的伸出端与万向节连接头3为球头连接。拉线式位移传感器固定板1的左侧固定板通过（4个）螺栓与六自由度位姿测量仪下固定板e的一角处固定连接。万向节连接头3通过（1个）螺栓与六自由度位姿测量仪上固定板a的一角处固定连接。拉线式位移传感器2的出线端为球头结构并与万向节连接头3共同作用降低对于拉线式位移传感器2拉线的磨损并提高六自由度测量装置A的工作空间。

参阅图13，所述的1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g与6号拉线式位移传感器总成h中的万向节连接头3的一端具有球头凹槽，万向节连接头3的另一端具有螺纹孔，万向节连接头3的球头凹槽端与拉线式位移传感器2的拉线端球头相连，万向节连接头3的螺纹孔端与六自由度位姿测量仪上固定板a上对应的上连接头通孔通过螺栓固定连接。

所述的1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g与6号拉线式位移传感器总成h中的拉线式位移传感器2采用开思KS15系列拉线式位移传感器。万向节连接头3采用住野SC10型万向节。

所述的六自由度位姿测量仪A安装时的初始状态下为六自由度位姿测量仪上固定板a与六自由度位姿测量仪下固定板e相互平行，六自由度位姿测量仪上固定板a的上中心通孔与六自由度位姿测量仪下固定板e的下中心通孔的回转轴线共线，同时（六自由度位姿测量仪上固定板a与六自由度位姿测量仪下固定板e的三个顶角对正后）六自由度位姿测量仪上固定板a的上中心通孔绕六自由度位姿测量仪下固定板e的下中心孔相对旋转60°。1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g与6号拉线式位移传感器总成h中的万向节连接头3分别采用螺栓与六自由度位姿测量仪上固定板a上对应的1号上连接头通孔、2号上连接头通孔、3号上连接头通孔、4号上连接头通孔、5号上连接头通孔及6号上连接头通孔固定连接；1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g与6号拉线式位移传感器总成h中的拉线式位移传感器固定板1分别采用螺栓与在六自由度位姿测量仪下固定板e上对应1号下固定板通孔、2号下固定板通孔、3号下固定板通孔、4号下固定板通孔、5号下固定板通孔及6号下固定板通孔固定连接。

参阅图6与图14，所述的六自由度位姿测量仪支架i由六自由度位姿测量仪支架上立柱4与六自由度位姿测量仪支架下立柱5组成，为了便于拆装，六自由度位姿测量仪支架i设置成分体式结构，六自由度位姿测量仪支架上立柱4与六自由度位姿测量仪支架下立柱5皆为U字形的冲压结构件，六自由度位姿测量仪支架上立柱4的中心处设置有上立柱中心通孔，上立柱中心通孔的周围设置有呈等边三角形分布的（实施例中采用9个）上立柱螺栓通孔，上立柱螺栓通孔结构、孔距和六自由度位姿测量仪下固定板e上的呈等边三角形分布的（实施例中采用9个）2号螺栓通孔的结构、孔距相等。六自由度位姿测量仪支架i为保证六自由度位姿测量仪A初始安装位置的正确，其中的六自由度位姿测量仪支架上立柱4上设置的上立柱中心孔、六自由度位姿测量仪上固定板a的上中心孔与六自由度位姿测量仪下固定板e的下中心孔同轴心共同起到定位作用。六自由度位姿测量仪支架下立柱5由立柱主体与安装座组成，立柱主体的底端设置有（2个）底端孔，利用底端孔便于将六自由度位姿测量仪支架i安装在转向架参数测定试验台基础Ⅳ上。立柱主体的下端与安装座的中心位置垂直并采用焊接连接，安装座的周围设置有安装螺栓的安装座通孔。六自由度位姿测量仪支架上立柱4与六自由度位姿测量仪支架下立柱5之间采用连接板与螺栓紧固连接，连接板的一端与六自由度位姿测量仪支架上立柱4下端采用焊接连接，连接板的另一端与六自由度位姿测量仪支架下立柱5上端螺栓连接。

所述的转向架参数测定试验台基础Ⅳ即为铸铁平台，其通过地锚器与水泥地基固定。转向架参数测定试验台基础Ⅳ上设计有纵向分布的T型槽，便于六自由度位姿测量仪支架i中的安装座通过螺栓安装固定。

参阅图4，所述的用于轨道车辆转向架参数测定试验台中的2个结构相同的下方六自由度加载平台Ⅰ侧面均匀地设置有螺纹孔，螺纹孔的数量与分布半径和六自由度位姿测量仪上固定板a的上中心通孔外侧圆周上的螺栓通孔数量与相对于上中心通孔回转中心的回转半径相等，六自由度位姿测量仪A上的六自由度位姿测量仪上固定板a通过螺栓固定下方六自由度加载平台Ⅰ的侧面上。

参阅图7与图15，所述的六自由度位姿测量仪安装法兰立柱j为圆筒类结构件，六自由度位姿测量仪安装法兰立柱j由圆筒主体、左端法兰盘与右端法兰盘组成，左端法兰盘与右端法兰盘上分别均匀分布有安装螺栓的法兰通孔，左端法兰盘与右端法兰盘上的法兰通孔距左端法兰盘与右端法兰盘回转中心的回转半径和六自由度位姿测量仪下固定板e的下中心通孔外侧圆周上的螺栓通孔相对于下中心通孔回转中心的回转半径相等，相邻两法兰通孔的弧心角与相邻两螺栓通孔的弧心角也相等。左端法兰盘与右端法兰盘依次焊接固定在圆筒主体的左端与右端，左端法兰盘、圆筒主体与右端法兰盘的回转轴线共线。六自由度位姿测量仪安装法兰立柱j一端与六自由度位姿测量仪A中的六自由度位姿测量仪下固定板e螺栓固定相连，另一端与转向架参数测定试验台框架Ⅲ采用螺栓紧固连接。

所述的转向架参数测定试验台框架Ⅲ为龙门结构，其上安装有作动器，并通过作动器与上方六自由度加载平台Ⅱ连接。转向架参数测定试验台框架Ⅲ顶部框架的下表面具有螺纹孔其分布半径与右端法兰盘上的法兰通孔距右端法兰盘回转中心的回转半径相等，六自由度位姿测量仪安装法兰立柱j上的右端法兰盘通过螺栓固定在转向架参数测定试验台框架Ⅲ顶部框架的下表面上。

参阅图5，所述的用于轨道车辆转向架参数测定试验台中的上方六自由度加载平台Ⅱ的上平面均匀地设置有螺纹孔，螺纹孔的数量与分布半径和六自由度位姿测量仪上固定板a的上中心通孔外侧圆周上的螺栓通孔数量与相对于上中心通孔回转中心的回转半径相等，六自由度位姿测量仪A上的六自由度位姿测量仪上固定板a通过螺栓固定在上方六自由度加载平台Ⅱ的上平面。

参阅图3与图6，所述的六自由度位姿测量仪A中的六自由度位姿测量仪上固定板a固定在2个结构相同的下方六自由度加载平台Ⅰ与上方六自由度加载平台Ⅱ上并跟随2个结构相同的下方六自由度加载平台Ⅰ与上方六自由度加载平台Ⅱ一起运动。

参阅图6与图7，所述的六自由度位姿测量仪下固定板e通过六自由度位姿测量仪支架i或六自由度位姿测量仪安装法兰立柱j固定在转向架参数测定试验台基础Ⅳ或转向架参数测定试验台框架Ⅲ上，从而带动拉线式位移传感器2中的拉线伸缩产生信号。

所述的六自由度位姿测量仪A中的六个结构相同的拉线式位移传感器2的输出端分别与数据存储装置B通过串行SSI接口连接，从而将采集的信号输入数据存储装置B。

所述的数据存储装置B可以实现具有高精度采样、实时传送数据、高度集成及各通道独立的实时信号处理。数据存储装置B是采用型号为东华DH5922的动态信号测试分析系统，动态信号测试分析系统能够实时采集并存储由六个结构相同的拉线式位移传感器2所测量的信号，进行简单处理后传输给工控机C进行位姿解算。

所述的工控机C通过PCI接口与数据存储装置B相连，接收数据存储装置B传输的数据信号并利用安装于其上的实时位姿解算软件进行位姿参数的计算。

所述的工控机C可对数据采集装置B进行参数设置（量程、传感器灵敏度、采样速率等）、清零、采样、停止等操作，并可实时传送采样数据。

参阅图16与图17，所述的工控机C上安装的实时位姿解算软件利用Simulink平台构建，读取由数据存储装置B输入的数据信号后进行解算并实时得出2个结构相同的下方六自由度加载平台Ⅰ与上方六自由度加载平台Ⅱ的位姿参数。

所述的工控机C上安装的实时位姿解算软件利用牛顿迭代法进行位姿解算，将1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g与6号拉线式位移传感器总成h采集信号作为输入，经过运动学正解与运动学反解模块后得出计算数据与输入数据进行比较作为迭代解算的终止条件。

所述的安装于工控机C中的位姿解算软件将输入的六个拉线式位移传感器2信号作为输入，即为杆长变化量实测值，首次迭代时将实测值经QR模块解算得出位姿参数修正量，位姿参数修正量经累次相加后得出位姿参数计算值，此值经运动学反解模块计算后得出杆长变化量计算值并将此值与杆长变化量实测值比较得出杆长差值。杆长差值与误差判定模块中的设定值进行比较，如果小于设定值则输出位姿参数计算值及迭代次数，如果大于设定值则继续进入QR模块进行下一次迭代解算。此处设定误差判定模块输入值为0.0001。位姿参数计算值须经坐标变换模块作用后才能得出2个结构相同的下方六自由度加载平台Ⅰ与上方六自由度加载平台Ⅱ中心点六个位姿参数。

采用六自由度位姿测量系统测量物体实时六自由度位姿方法的步骤如下：

1.对六自由度位姿测量仪A进行建模并建立牛顿迭代公式

1）以六自由度位姿测量仪上固定板a中心点作为坐标原点建立坐标系，根据六自由度位姿测量仪上固定板a与各拉线式位移传感器总成连接点相对于六自由度位姿测量仪上固定板a的连接点半径、六自由度位姿测量仪下固定板e与各拉线式位移传感器总成连接点相对于六自由度位姿测量仪下固定板e中心点的连接点半径、六自由度位姿测量仪上固定板a中心平面相对于六自由度位姿测量仪下固定板e中心平面初始高度、六自由度位姿测量仪上固定板a与各拉线式位移传感器总成相邻连接点连线相对于六自由度位姿测量仪上固定板a中心点的圆心角、六自由度位姿测量仪下固定板e与各拉线式位移传感器总成相邻连接点连线相对于六自由度位姿测量仪下固定板e中心点的圆心角，得出1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g、6号拉线式位移传感器总成h与六自由度位姿测量仪上固定板a及六自由度位姿测量仪下固定板e连接点在所建坐标系中初始坐标值，共12个。

2）通过同一个拉线式位移传感器总成两端坐标初值相减可得出相应拉线式位移传感器总成在坐标系中的空间坐标矢量初始值并求出初始杆长，以l_k表示。

3）由于物体在空间中的位姿变化由旋转及平移组成，旋转角度用欧拉角描述，各坐标点的任意时刻坐标可以利用欧拉角旋转矩阵乘以坐标点初始坐标后与空间平移量相加得出，利用各拉线式位移传感器总成两端分别与六自由度位姿测量仪上固定板a及六自由度位姿测量仪下固定板e相连连接点的任意时刻坐标值相减即可得到对应拉线式位移传感器总成任意时刻空间坐标矢量值：

l_i=P+RB_i-C_i                （1）

式中：P为六自由度位姿测量仪上固定板a位置变化的空间矢量，即为六自由度位姿测量仪上固定板a中心的移动矢量，此处由于连接点固定于六自由度位姿测量仪上固定板a上，因此移动距离相同；R为欧拉角旋转矩阵；B_i为六自由度位姿测量仪上固定板a与第i号拉线式位移传感器总成连接点初始坐标值；C_i为六自由度位姿测量仪下固定板e与第i号拉线式位移传感器总成连接点初始坐标值；l_i为坐标对应拉线式位移传感器总成的空间矢量。根据空间矢量的定义，各拉线式位移传感器总成的长度可以由下式得出：

|l_i|²=l_i ^Tl_i                 （2）

式中：|l_i|为各拉线式位移传感器总成长度绝对值；l_i为拉线式位移传感器总成任意时刻空间坐标矢量值。

4）对六自由度位姿测量仪A进行运动学分析，将六自由度位姿测量仪上固定板a与各拉线式位移传感器总成连接点速度同各拉线式位移传感器总成的长度变化速度联系，可知各拉线式位移传感器总成长度变化速度即为各连接点的空间速度矢量在各拉线式位移传感器总成空间矢量上的投影，对各拉线式位移传感器总成长度进行微分得出下式，并根据矢量投影原理得出下式：

$\frac{d\left|l_i\right|}{\mathrm{dt}}=\frac{{l_i}^T{V}_{\mathrm{Bi}}}{\left|l_i\right|}={l_{\mathrm{ui}}}^T{V}_{\mathrm{Bi}}---\left(3\right)$

式中：l_ui表示各拉线式位移传感器总成方向的空间单位矢量，V_Bi表示对应连接点的速度矢量，根据沙勒定律，对于刚体而言，其广义的位移等价于一个平移与一个旋转的结合，因此连接点速度矢量形式可以由以下公式得出：

V_Bi=V+ω×RB_i

式中：V_Bi表示六自由度位姿测量仪上固定板a与第i个拉线式位移传感器总成连接点的速度矢量；V表示六自由度位姿测量仪上固定板a中心点的速度矢量，ω表示六自由度位姿测量仪上固定板a的角速度矢量；R为欧拉角旋转矩阵；B_i为六自由度位姿测量仪上固定板a与第i号拉线式位移传感器总成相连连接点初始坐标值，RB_i即为任意时刻六自由度位姿测量仪上固定板a与第i号拉线式位移传感器总成相连连接点相对于坐标中心点的空间矢量值。

将上式进行整理，并与公式（3）合并改写为矩阵的形式，得到公式：

${\stackrel{\·}{l}}_i={l_{\mathrm{ui}}}^T{V}_{\mathrm{Bi}}={l_{\mathrm{ui}}}^{T}V+{l_{\mathrm{ui}}}^T(\mathrm{\ω}\×{\mathrm{RB}}_i)---\left(4\right)$

将上式进行整理，并改写为矩阵的形式，得到公式：

$\stackrel{\·}{l}={l_u}^{T}V+{(\mathrm{RB}\×l_u)}^T\mathrm{\ω}=...\·\·\·\left[\begin{array}{cc}{l_u}^T& {(\mathrm{RB}\×l_u)}^T\end{array}\right]\left[\begin{array}{c}V\\ \mathrm{\ω}\end{array}\right]=J_{l,x}\stackrel{\·}{x}---\left(5\right)$

由上式可以建立六自由度位姿测量仪上固定板a速度与各拉线式位移传感器总·成速度之间的关系，实现牛顿迭代格式的初步搭建，式中均为矩阵形式，其中

为各拉线式位移传感器总成的速度矩阵，l_u为各拉线式位移传感器总成的单位方向矢量矩阵，B为六自由度位姿测量仪上固定板a各连接点初始坐标矩阵，J_i,x则表示六自由度位姿测量仪上固定板a速度及角速度对应到各拉线式位移传感器总成与六自由度位姿测量仪上固定板a相连连接点速度及角速度的雅可比矩阵。

5）由于在进行牛顿迭代时，未知量中所包含的角度应为六自由度位姿测量仪上固定板a拉线式的欧拉角参数，在上式中的雅可比矩阵所对应的是六自由度位姿测量仪上固定板a速度及角速度，而不是针对欧拉角的角速度建立的，因此需要进行一定的转化。对雅可比矩阵进行转化，有：

$J_{i,\mathrm{sx}}=J_{l,x}\left[\begin{array}{cc}I& \\ & \mathrm{RJ}\end{array}\right]=\left[{l_u}^T{(\mathrm{RB}\×l_u)}^T\mathrm{RJ}\right]---\left(6\right)$

式中：I为单位矩阵，J为将六自由度位姿测量仪上固定板a角速度转化为对应欧拉角速度的雅可比矩阵。

在J_i,x中上半部分表示各拉线式位移传感器总成速度关于位移的雅可比矩阵，而下半部分为各拉线式位移传感器总成速度关于六自由度位姿测量仪上固定板a角速度的雅可比矩阵，因此只需对下半部分矩阵乘以转换雅可比矩阵。与旋转矩阵性质相同，不同顺规的欧拉角也对应不同的转换雅可比矩阵，相应转换雅可比矩阵为：

$J=\left[\begin{array}{ccc}1& 0& -\sin \mathrm{\β}\\ 0& \cos \mathrm{\α}& \sin \mathrm{\α}\cos \mathrm{\β}\\ 0& -\sin \mathrm{\α}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]---\left(7\right)$

根据上式可以推算得出：

$\stackrel{\·}{l}=J_{l,x}\stackrel{\·}{x}=J_{i,\mathrm{sx}}{\stackrel{\·}{x}}^{\′}---\left(8\right)$

即可得出：

x'_k+1=x'_k+J_i,sx ^-1(x'_k)(l_f-l_k)             （9）

式中：l_f表示当前需要解算状态下各拉线式位移传感器总成伸缩量的向量，由l_i组成，x'表示所需求的位移及欧拉角度值的向量；上式（9）即为求解六自由度位姿测量仪A位姿参数的牛顿迭代法迭代格式。

参阅图16,工控机C首次读取六自由度测量仪A输入的信号（即为杆长变化量实测值）后与杆长变化量计算值相减取得杆长差值，即为公式（10）中的(l_f-l_k)，此处杆长变化量计算值初始值设置为零向量。杆长差值数值与误差判定模块中设定的允许误差限值进行比较，如小于允许误差限值则输出迭代次数及位姿参数计算值，如大于允许误差限值则将杆长差值代入QR算法模块，并将初始雅可比矩阵代入QR算法模块，利用公式（10）中的J_i,sx ^-1(x'_k)(l_f-l_k)进行求解，此处允许误差限值设定为0.0001，位姿参数计算值的初始值设定为0，雅可比矩阵初始设定为零矩阵。杆长差值及雅可比矩阵经QR算法模块解算后得出位姿参数修正量，通过各单次循环该修正量的累加可以得到位姿参数计算值。将位姿参数计算值代入运动学反解模型，通过在已知12个连接点空间坐标值及位姿参数计算值的情况下进行运动学反解，得到杆长变化量计算值、各连接杆空间向量、运动平台连接点旋转矩阵、角速度雅可比矩阵。将各连接杆空间向量、运动平台连接点旋转矩阵、角速度雅可比矩阵以公式（7）形式进行搭建得到下次循环的雅可比矩阵。杆长变化量计算值继续与工控机C读取的杆长变化量实测值进行比较，得到新的杆长差值，并将差值与误差判定模块中的允许误差限值比较，进行下一次循环，如此循环往复直到误差判定模块中的允许误差限值为止，输出迭代次数及位姿参数计算值。

2.进行坐标变换上方六自由度加载平台Ⅱ

六自由度位姿测量仪A必须与用于轨道车辆转向架参数测定试验台中的2个结构相同的下方六自由度加载平台Ⅰ与上方六自由度加载平台Ⅱ进行固定连接从而实现接触式测量，但是安装的实际情况可能会导致六自由度测量系统测量点并不在六自由度位姿测量仪A理论模型六自由度位姿测量仪上固定板a的中心位置，在这种情况下，必须设计模块能够实现测量点与六自由度位姿测量仪上固定板a中心点的位姿参数转换，即进行坐标变换。

设六自由度位姿测量仪A中的六自由度位姿测量仪上固定板a中心点坐标点为原点，下方六自由度加载平台Ⅰ中心点相对于坐标原点的空间坐标点为p[x,y,z]，由于刚体在转动时，各点旋转的欧拉角度值均相同，则使用欧拉角的旋转矩阵R乘以P即可得到经过旋转之后的下方六自由度加载平台Ⅰ中心点空间坐标值，其值与原坐标值差值的大小即为由于刚体的旋转而产生的空间坐标值的位移的变化，将此数值与平动位移量相加即得转化后的下方六自由度加载平台Ⅰ中心点位移的变化，由于欧拉角度值相同，可以直接得出需要测量的下方六自由度加载平台Ⅰ中心点六个位姿参数。

3.输出2个结构相同的下方六自由度加载平台Ⅰ与上方六自由度加载平台Ⅱ中心点六个位姿参数

参阅图17，即为所述的基于以上物体实时六自由度位姿方法利用Simulink编制的物体六自由度位姿参数程序图。（图17即为最终完成的程序，利用Simulink编完的程序最后是以框图表示的，每个框图有基本的作用，将有基本作用的框图连接就能完成特定的，复杂的解算）

所述的显示器D实时接收经工控机C解算得出的位姿参数并进行显示。

Claims (10)

1.一种六自由度位姿测量系统，包括六自由度位姿测量仪（A）、数据存储装置（B）、工控机（C）及显示器（D）,其特征在于，所述的六自由度位姿测量仪（A）包括六自由度位姿测量仪上固定板（a）、1号拉线式位移传感器总成（b）、2号拉线式位移传感器总成（c）、3号拉线式位移传感器总成（d）、六自由度位姿测量仪下固定板（e）、4号拉线式位移传感器总成（f）、5号拉线式位移传感器总成（g）与6号拉线式位移传感器总成（h）；

所述的1号拉线式位移传感器总成（b）、2号拉线式位移传感器总成（c）、3号拉线式位移传感器总成（d）、4号拉线式位移传感器总成（f）、5号拉线式位移传感器总成（g）与6号拉线式位移传感器总成（h）的结构相同，皆由拉线式位移传感器固定板（1）、拉线式位移传感器（2）与万向节连接头（3）组成；

拉线式位移传感器（2）采用螺栓固定于拉线式位移传感器固定板（1）的右侧固定板上，拉线式位移传感器（2）中的拉线伸出端与万向节连接头（3）为球头连接；

1号拉线式位移传感器总成（b）、2号拉线式位移传感器总成（c）、3号拉线式位移传感器总成（d）、4号拉线式位移传感器总成（f）、5号拉线式位移传感器总成（g）与6号拉线式位移传感器总成（h）中的万向节连接头（3）分别采用螺栓与六自由度位姿测量仪上固定板（a）固定连接，1号拉线式位移传感器总成（b）、2号拉线式位移传感器总成（c）、3号拉线式位移传感器总成（d）、4号拉线式位移传感器总成（f）、5号拉线式位移传感器总成（g）与6号拉线式位移传感器总成（h）中的拉线式位移传感器固定板（1）的左侧固定板分别采用螺栓与六自由度位姿测量仪下固定板（e）固定连接。

2.按照权利要求1所述的六自由度位姿测量系统，其特征在于，所述的1号拉线式位移传感器总成（b）、2号拉线式位移传感器总成（c）、3号拉线式位移传感器总成（d）、4号拉线式位移传感器总成（f）、5号拉线式位移传感器总成（g）与6号拉线式位移传感器总成（h）中的万向节连接头（3）分别采用螺栓与六自由度位姿测量仪上固定板（a）固定连接是指：

1号拉线式位移传感器总成（b）、2号拉线式位移传感器总成（c）、3号拉线式位移传感器总成（d）、4号拉线式位移传感器总成（f）、5号拉线式位移传感器总成（g）与6号拉线式位移传感器总成（h）中的万向节连接头（3）依次与六自由度位姿测量仪上固定板（a）上的1号上连接头通孔、2号上连接头通孔、3号上连接头通孔、4号上连接头通孔、5号上连接头通孔及6号上连接头通孔采用螺栓连接；

1号拉线式位移传感器总成（b）、2号拉线式位移传感器总成（c）、3号拉线式位移传感器总成（d）、4号拉线式位移传感器总成（f）、5号拉线式位移传感器总成（g）与6号拉线式位移传感器总成（h）中的拉线式位移传感器固定板（1）与六自由度位姿测量仪下固定板（e）固定连接是指：

1号拉线式位移传感器总成（b）、2号拉线式位移传感器总成（c）、3号拉线式位移传感器总成（d）、4号拉线式位移传感器总成（f）、5号拉线式位移传感器总成（g）与6号拉线式位移传感器总成（h）中的拉线式位移传感器固定板（1）依次与1号下固定板通孔、2号下固定板通孔、3号下固定板通孔、4号下固定板通孔、5号下固定板通孔及6号下固定板通孔采用螺栓连接。

3.按照权利要求1所述的六自由度位姿测量系统，其特征在于，所述的六自由度位姿测量仪（A）安装的初始状态为六自由度位姿测量仪上固定板（a）与六自由度位姿测量仪下固定板（e）相互平行，六自由度位姿测量仪上固定板（a）的上中心通孔与六自由度位姿测量仪下固定板（e）的下中心通孔的回转轴线共线，同时六自由度位姿测量仪上固定板（a）与六自由度位姿测量仪下固定板（e）的三个顶角对正后六自由度位姿测量仪上固定板（a）的上中心通孔绕六自由度位姿测量仪下固定板（e）的下中心孔旋转60°。

4.按照权利要求1所述的六自由度位姿测量系统，其特征在于，所述的六自由度位姿测量仪上固定板（a）为等边三角形的板类结构件，六自由度位姿测量仪上固定板（a）的中心处设置有上中心通孔，上中心通孔周围的圆周上均匀分布有安装螺栓的上螺栓通孔，六自由度位姿测量仪上固定板（a）的三个顶角处设置有用于安装1号拉线式位移传感器总成（b）、2号拉线式位移传感器总成（c）、3号拉线式位移传感器总成（d）、4号拉线式位移传感器总成（f）、5号拉线式位移传感器总成（g）与6号拉线式位移传感器总成（h）中的万向节连接头（3）的上连接头通孔，即1号上连接头通孔、2号上连接头通孔、3号上连接头通孔、4号上连接头通孔、5号上连接头通孔及6号上连接头通孔。

5.按照权利要求1所述的六自由度位姿测量系统，其特征在于，所述的六自由度位姿测量仪下固定板（e）与六自由度位姿测量仪上固定板（a）形状相同，皆为等边三角形的板类结构件，六自由度位姿测量仪下固定板（e）的中心处设置有下中心通孔，下中心通孔的周围的圆周上也均匀分布有安装螺栓的下螺栓通孔，并在下螺栓通孔的周围设置有呈等边三角形分布的2号螺栓通孔，六自由度位姿测量仪下固定板（e）三个顶角处的下方对称地设置有用于安装1号拉线式位移传感器总成（b）、2号拉线式位移传感器总成（c）、3号拉线式位移传感器总成（d）、4号拉线式位移传感器总成（f）、5号拉线式位移传感器总成（g）与6号拉线式位移传感器总成（h）中的拉线式位移传感器固定板（1）的下固定板通孔，即1号下固定板通孔、2号下固定板通孔、3号下固定板通孔、4号下固定板通孔、5号下固定板通孔及6号下固定板通孔。

6.按照权利要求1所述的六自由度位姿测量系统，其特征在于，所述的六自由度位姿测量仪（A）中的六自由度位姿测量仪下固定板（e）与六自由度位姿测量仪支架（i）上端采用螺栓固定连接，六自由度位姿测量仪支架（i）下端与转向架参数测定试验台基础（Ⅳ）固定连接，或者六自由度位姿测量仪（A）中的六自由度位姿测量仪下固定板（e）与六自由度位姿测量仪安装法兰立柱（j）的一端固定连接，六自由度位姿测量仪安装法兰立柱（j）的另一端与转向架参数测定试验台框架（Ⅲ）固定连接。

7.按照权利要求6所述的六自由度位姿测量系统，其特征在于，所述的六自由度位姿测量仪支架（i）由六自由度位姿测量仪支架上立柱（4）与六自由度位姿测量仪支架下立柱（5）组成，六自由度位姿测量仪支架上立柱（4）与六自由度位姿测量仪支架下立柱（5）皆为等横截面的U字形的冲压结构件，六自由度位姿测量仪支架上立柱（4）的中心处设置有上立柱中心通孔，上立柱中心通孔的周围设置有呈等边三角形分布的上立柱螺栓通孔，上立柱螺栓通孔结构、孔距和六自由度位姿测量仪下固定板（e）上的呈等边三角形分布的2号螺栓通孔的结构、孔距相等；

六自由度位姿测量仪支架下立柱（5）由立柱主体与安装座组成，立柱主体的底端设置有2个底端孔，立柱主体的下端与安装座的中心位置垂直并采用焊接方式连接成一体，安装座的周围设置有安装螺栓的安装座通孔，六自由度位姿测量仪支架上立柱（4）与六自由度位姿测量仪支架下立柱（5）之间采用连接板与螺栓紧固连接，连接板的一端与六自由度位姿测量仪支架上立柱（4）的下端采用焊接连接，连接板的另一端与六自由度位姿测量仪支架下立柱（5）上端螺栓连接。

8.按照权利要求1所述的六自由度位姿测量系统，其特征在于，所述的六自由度位姿测量仪安装法兰立柱（j）为圆筒类结构件，六自由度位姿测量仪安装法兰立柱（j）由圆筒主体、左端法兰盘与右端法兰盘组成；左端法兰盘与右端法兰盘上分别均匀分布有安装螺栓的法兰通孔，左端法兰盘与右端法兰盘上的法兰通孔距左端法兰盘与右端法兰盘回转中心的回转半径和六自由度位姿测量仪下固定板（e）的下中心通孔外侧圆周上的螺栓通孔距下中心通孔回转中心的回转半径相等，左端法兰盘与右端法兰盘上相邻两法兰通孔的孔距与六自由度位姿测量仪下固定板（e）上相邻两螺栓通孔的孔距也相等，左端法兰盘与右端法兰盘依次焊接固定在圆筒主体的左端与右端，左端法兰盘、圆筒主体与右端法兰盘的回转轴线共线。

9.一种采用六自由度位姿测量系统测量物体实时六自由度位姿的方法，其特征在于，所述的采用六自由度位姿测量系统测量物体实时六自由度位姿的方法的步骤如下：

1）对六自由度位姿测量仪（A）进行建模并建立牛顿迭代公式：

x'_k+1=x'_k+J_i,sx ^-1(x'_k)(l_f-l_k)              （9）

式中：l_f表示当前需要解算状态下各拉线式位移传感器总成伸缩量的向量，x'表示所需求的位移及欧拉角度值的向量；

2）进行坐标变换上方六自由度加载平台（Ⅱ）：

根据六自由度位姿测量系统测量点并不在六自由度位姿测量仪（A）理论模型六自由度位姿测量仪上固定板（a）的中心位置的情况，设计模块实现测量点与六自由度位姿测量仪上固定板（a）中心点的位姿参数转换即进行坐标变换。

设六自由度位姿测量仪（A）中的六自由度位姿测量仪上固定板（a）中心点坐标点为原点，下方六自由度加载平台（Ⅰ）中心点相对于坐标原点的空间坐标点为p[x,y,z]，由于刚体在转动时，各点旋转的欧拉角度值均相同，则使用欧拉角的旋转矩阵R乘以P即可得到经过旋转之后的下方六自由度加载平台（Ⅰ）中心点空间坐标值，其值与原坐标值差值的大小即为由于刚体的旋转而产生的空间坐标值的位移的变化，将此数值与平动位移量相加即得转化后的下方六自由度加载平台（Ⅰ）中心点位移的变化，由于欧拉角度值相同，可以直接得出需要测量的下方六自由度加载平台Ⅰ中心点六个位姿参数；

3）输出2个结构相同的下方六自由度加载平台（Ⅰ）与上方六自由度加载平台（Ⅱ）中心点六个位姿参数。

10.按照权利要求9所述的采用六自由度位姿测量系统测量物体实时六自由度位姿的方法，其特征在于，所述的对六自由度位姿测量仪（A）进行建模并建立牛顿迭代公式步骤如下：

1）以六自由度位姿测量仪上固定板（a）中心点作为坐标原点建立坐标系，根据六自由度位姿测量仪上固定板（a）及六自由度位姿测量仪下固定板（e）与各拉线式位移传感器总成连接点分别相对于六自由度位姿测量仪上固定板（a）及六自由度位姿测量仪下固定板（e）中心点的连接点半径、六自由度位姿测量仪上固定板（a）中心平面及六自由度位姿测量仪下固定板（e）中心平面相对初始高度、六自由度位姿测量仪上固定板（a）及六自由度位姿测量仪下固定板（e）与各拉线式位移传感器总成相邻连接点连接线相对于六自由度位姿测量仪上固定板（a）及六自由度位姿测量仪下固定板（e）中心点的圆心角得出1号拉线式位移传感器总成b、2号拉线式位移传感器总成c、3号拉线式位移传感器总成d、4号拉线式位移传感器总成f、5号拉线式位移传感器总成g、6号拉线式位移传感器总成h与六自由度位姿测量仪上固定板（a）及六自由度位姿测量仪下固定板e连接点在所建坐标系中初始坐标值，共12个；

2）通过同一个拉线式位移传感器总成两端坐标初值相减可得出相应拉线式位移传感器总成在坐标系中的空间坐标矢量初始值并求出初始杆长，以l_k表示；

3）由于物体在空间中的位姿变化由旋转及平移组成，旋转角度用欧拉角描述，各坐标点的任意时刻坐标可以利用欧拉角旋转矩阵乘以坐标点初始坐标后与空间平移量相加得出，利用各拉线式位移传感器总成分别与六自由度位姿测量仪上固定板（a）及六自由度位姿测量仪下固定板（e）连接点的任意时刻坐标值相减即得到对应拉线式位移传感器总成任意时刻空间坐标矢量值：

l_i=P+RB_i-C_i（1）

式中：P为六自由度位姿测量仪上固定板（a）位置变化的空间矢量，即为六自由度位姿测量仪上固定板（a）中心点的移动矢量，此处由于连接点固定于六自由度位姿测量仪上固定板（a）上，因此移动距离相同；R为欧拉角旋转矩阵；B_i为六自由度位姿测量仪上固定板（a）与第i号拉线式位移传感器总成连接点初始坐标值；C_i为六自由度位姿测量仪下固定板（e）与第i号拉线式位移传感器总成连接点初始坐标值；l_i为坐标对应拉线式位移传感器总成的空间矢量。根据空间矢量的定义，各拉线式位移传感器总成的长度可以由下式得出：

|l_i|²=l_i ^Tl_i             （2）

式中：|l_i|为各拉线式位移传感器总成长度绝对值；l_i为拉线式位移传感器总成任意时刻空间坐标矢量值；

4）对六自由度位姿测量仪A进行运动学分析，将六自由度位姿测量仪上固定板（a）与各拉线式位移传感器总成连接点速度同各拉线式位移传感器总成的长度变化速度联系，可知各拉线式位移传感器总成长度变化速度即为各连接点的空间速度矢量在各拉线式位移传感器总成空间矢量上的投影，对各拉线式位移传感器总成长度进行微分得出下式，并根据矢量投影原理得出下式：

$\frac{d\left|l_i\right|}{\mathrm{dt}}=\frac{{l_i}^T{V}_{\mathrm{Bi}}}{\left|l_i\right|}={l_{\mathrm{ui}}}^T{V}_{\mathrm{Bi}}---\left(3\right)$

式中：l_ui表示各拉线式位移传感器总成方向的空间单位矢量，V_Bi表示对应连接点的速度矢量，根据沙勒定律，对于刚体而言，其广义的位移等价于一个平移与一个旋转的结合，因此连接点速度矢量形式可以由以下公式得出：

V_Bi=V+ω×RB_i

式中：V_Bi表示六自由度位姿测量仪上固定板（a）与第i个拉线式位移传感器总成连接点的速度矢量；V表示六自由度位姿测量仪上固定板（a）中心点的速度矢量，ω表示六自由度位姿测量仪上固定板（a）的角速度矢量；R为欧拉角旋转矩阵；B_i为六自由度位姿测量仪上固定板（a）与第i号拉线式位移传感器总成连接点初始坐标值，RB_i即为任意时刻六自由度位姿测量仪上固定板（a）与第i号拉线式位移传感器总成相连连接点相对于坐标中心点的空间矢量值；

将上式进行整理，并与公式（3）合并改写为矩阵的形式，得到公式：

${\stackrel{\·}{l}}_i={l_{\mathrm{ui}}}^T{V}_{\mathrm{Bi}}={l_{\mathrm{ui}}}^{T}V+{l_{\mathrm{ui}}}^T(\mathrm{\ω}\×{\mathrm{RB}}_i)---\left(4\right)$

将上式进行整理，并改写为矩阵的形式，得到公式：

$\stackrel{\·}{l}={l_u}^{T}V+{(\mathrm{RB}\×l_u)}^T\mathrm{\ω}=...\·\·\·\left[\begin{array}{cc}{l_u}^T& {(\mathrm{RB}\×l_u)}^T\end{array}\right]\left[\begin{array}{c}V\\ \mathrm{\ω}\end{array}\right]=J_{l,x}\stackrel{\·}{x}---\left(5\right)$

由上式可以建立六自由度位姿测量仪上固定板a速度与各拉线式位移传感器总成速度之间的关系，实现牛顿迭代格式的初步搭建，式中均为矩阵形式，其中

为各拉线式位移传感器总成的速度矩阵，l_u为各拉线式位移传感器总成的单位方向矢量矩阵，B为六自由度位姿测量仪上固定板（a）各连接点初始坐标矩阵，J_i,x则表示六自由度位姿测量仪上固定板（a）速度及角速度对应到各拉线式位移传感器总成与六自由度位姿测量仪上固定板（a）连接点速度及角速度的雅可比矩阵；

5）由于在进行牛顿迭代时，未知量中所包含的角度应为六自由度位姿测量仪上固定板拉线式的欧拉角参数，在上式中的雅可比矩阵所对应的是六自由度位姿测量仪上固定板（a）速度及角速度，而不是针对欧拉角的角速度建立的，因此需要进行一定的转化。对雅可比矩阵进行转化，有：

$J_{i,\mathrm{sx}}=J_{l,x}\left[\begin{array}{cc}I& \\ & \mathrm{RJ}\end{array}\right]=\left[{l_u}^T{(\mathrm{RB}\×l_u)}^T\mathrm{RJ}\right]---\left(6\right)$

式中：I为单位矩阵，J为将六自由度位姿测量仪上固定板（a）角速度转化为对应欧拉角速度的雅可比矩阵；

在J_i,x中上半部分表示各拉线式位移传感器总成速度关于位移的雅可比矩阵，而下半部分为各拉线式位移传感器总成速度关于六自由度位姿测量仪上固定板（a）角速度的雅可比矩阵，因此只需对下半部分矩阵乘以转换雅可比矩阵。与旋转矩阵性质相同，不同顺规的欧拉角也对应不同的转换雅可比矩阵，相应转换雅可比矩阵为：

$J=\left[\begin{array}{ccc}1& 0& -\sin \mathrm{\β}\\ 0& \cos \mathrm{\α}& \sin \mathrm{\α}\cos \mathrm{\β}\\ 0& -\sin \mathrm{\α}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]---\left(7\right)$

根据上式可以推算得出：

$\stackrel{\·}{l}=J_{l,x}\stackrel{\·}{x}=J_{i,\mathrm{sx}}{\stackrel{\·}{x}}^{\′}---\left(8\right)$

即可得出：

x'_k+1=x'_k+J_i,sx ^-1(x'_k)(l_f-l_k)            （9）

式中：l_f表示当前需要解算状态下各拉线式位移传感器总成伸缩量的向量，由l_i组成，x'表示所需求的位移及欧拉角度值的向量；上式（9）即为求解六自由度位姿测量仪A位姿参数的牛顿迭代法迭代格式。

Images