六自由度运动平台控制中心的定位方法

【】本发明涉及六自由度运动平台的，特别是六自由度运动平台控制中心的定位方法的。

背景技术

0、
背景技术：

1、六自由度运动平台的运动控制精度，直接影响到其构建环境的模拟效果，大到科研实验、工程建设、技术开发，小到虚拟现实的浸入式效果，都会受到影响。因此，急需研究科学有效的方法测量校准六自由度运动平台的特性参数，以验证实际提供的模拟仿真环境是否达到生产研制的目标要求。目前对六自由度运动平台的测量校准方法大都是针对单一的姿态或者位移进行部分测试的技术规范，而对位姿的复合运动校准，尤其对复合运动的动态特性校准，还没有完整系统的方法。

2、现有的对六自由度运动平台的测量工作一般从单个坐标轴的位移或者单个方向的转角(以下总结为“单自由度”)独立测量出发，这实际上不符合六自由度运动平台的现实情况，对平台的六个自由度复合运动的测量才是符合测量逻辑的。单自由度独立的测量方式只能体现六自由度运动平台对单坐标轴位移或转角运动的准确性，这意味着当平台在进行单自由度运动时，无论测量靶标在平台上的哪个位置，都能获得准确的测量结果。然而，六自由度运动平台事实上是在进行六个自由度的复合运动，在这种运动模式下，因为六自由度平台发送的运动指令只针对控制中心，所以只有对六自由度运动平台的控制中心的位置姿态描述p6d控制中心(x,y,z,rx,ry,rz)才能够代表六自由度平台的运动位置和姿态信息，平台上其他测量点均无法代表。所以，原来的单自由度独立测量的方式显然无法准确反映六自由度运动平台在复合运动下的平台性能，是一种局部、片面的测量方式。由此，想要对六自由度运动平台的位置姿态进行准确测量，关键在于寻找六自由度运动平台的控制中心。

3、在实际工作过程中，利用传统方法也可以大致确定六自由度运动平台的控制中心。一般情况下，六自由度运动平台本身不会给出其世界坐标系的基准，因而需要通过测量六自由度运动平台的各个运动状态，寻找六自由度运动平台的世界坐标系和实际控制中心。传统方法可以将六自由度运动平台复位至零位，在六自由度运动平台任意非控制中心位置固定一个靶标，控制六自由度运动平台沿z轴运动确定z轴方向，沿x轴运动确定x轴方向，最后控制六自由度运动平台绕控制中心旋转，确定控制中心位置，即可确定六自由度运动平台的世界坐标系和控制中心位置。其中，六自由度运动平台的控制中心在初值位置状态下坐标系位置即为测试时的世界坐标系位置。

4、但是，寻找六自由度运动平台控制中心的传统方法在实际操作时会引入多个较大误差。通过传统方法建立的六自由度运动平台世界坐标系的重复性非常依赖六自由度运动平台本身的精度，而因为六自由度运动平台常常被应用与大型工程中，通常采用液压控制，其本身控制精度不会太高，再加上六自由度运动平台本身的设计结构因素，在某些自由度上旋转角度较小(如常规六自由度运动平台绕z轴可旋转角度范围很小，而小角度旋转量找圆心误差极大)，也会对其控制中心定位精度造成较大影响。因而这种传统方法存在理论上能找到六自由度运动平台世界坐标系和控制中心的可能性，但是实际操作时会引入多个较大误差。例如，控制六自由度运动平台绕某轴转时可以产生控制中心另外两轴的坐标值，分别绕三个坐标轴旋转时会产生6个坐标值，而其中三个坐标值是重复的，往往会因为被测设备精度较低的缘故造成这些重复的坐标值之间偏差很大。又或者，通过控制六自由度运动平台向x，y，z三轴移动时，三轴之间相互不垂直问题，也会引起构造坐标系的误差。最终这些误差会反应在两个误差上，即传感器绑定的控制中心和真实控制中心的位置误差，以及六自由度运动平台世界坐标系与激光跟踪仪世界坐标系误差。因此该传统方法只能作为粗定位六自由度世界坐标系和控制中心的方式。传统方法无法对六自由度运动平台控制中心进行更精确的定位，就无法指导厂家把控制中心调整至平台上更加直观准确的初始位置，导致六自由度运动平台实际的控制精度降低，达不到设计的精度指标要求。

5、六自由度运动平台目前的主要应用在环境模拟、稳态环境构建和大型工件拼接上，针对这些用途和六自由度运动平台本身特点，确定六自由度运动平台世界坐标系测量和实际控制中心十分重要，这不仅有利于平台的校准、提高平台的运动精度，也为平台在实际场景中的应用提供了保障和更好的应用扩展空间。

6、为更好地阐述技术方案，以下名词解释为：

7、1.六自由度运动——六自由度运动分为三个平移的位置运动和三个旋转的姿态运动，尤其在姿态旋转中，其旋转方式、旋转顺序都不是唯一的，本文所有描述的位置姿态运动都是先绕世界坐标系的x轴、y轴、z轴的顺序，进行(δrx,δry,δrz)的角度旋转，再经过(dx,dy,dz)的位移；

8、2.六自由度点——它是一个绑定在六自由度运动平台上的被测坐标系，表示六自由度运动平台的一种位置姿态的状态，它表示物体从原点出发，依次经过(δrx,δry,δrz)的顺序进行旋转，再经过(dx,dy,dz)的位移，最终所处的位姿可以用p6d(x,y,z,rx,ry,rz)表示，称为六自由度点；

9、3.控制中心六自由度点p6d控制中心(x,y,z,rx,ry,rz)——六自由度运动平台上被绑定了一个被测坐标系，其绑定被测坐标系的原点位置即为控制中心，p6d控制中心(x,y,z,rx,ry,rz)的前三个参量(x,y,z)位置坐标，描述的实际上是控制中心位置，p6d控制中心(x,y,z,rx,ry,rz)的后三个参量(rx,ry,rz)是通过描述控制中心当前姿态来反映六自由度运动平台的姿态；

10、4.六自由度运动和六自由度点的矩阵公式——为了方便计算的统一，将六自由度运动和六自由度点用矩阵形式表示；

11、5.原点p6d(0,0,0,0,0,0)的矩阵形式——原点的矩阵为单位矩阵

12、6.六自由度平移矩阵——

13、7.六自由度旋转矩阵——

14、绕z轴旋转δrz

15、

16、绕y轴旋转δry

17、

18、绕x轴旋转δrx

19、

技术实现思路

0、
技术实现要素：

1、本发明的目的就是解决现有技术中的问题，提出六自由度运动平台控制中心的定位方法，能够提高六自由度运动平台世界坐标系测量和控制中心寻找的精度，实现六自由度运动平台动态校准。

2、为实现上述目的，本发明提出了六自由度运动平台控制中心的定位方法，包括以下步骤：

3、s1、使用传统方法粗定位六自由度运动平台世界坐标系和控制中心的初始位置；

4、s2、使用激光跟踪仪和t-mac测头对均匀分布于六自由度运动平台行程范围内的大量运动位姿进行测量；

5、s3、依据六自由度运动平台的测量模型，以步骤s2中运动位姿作为参数，通过最佳拟合得到六自由度运动平台控制中心位置偏差和世界坐标系位置偏差；

6、s4、利用步骤s3拟合得到的两个偏差，分别对六自由度运动平台的控制中心位置和激光跟踪仪测量软件中的世界坐标系位置进行调整；

7、s5、通过t-mac测头直接测得六自由度运动平台的位置姿态。

8、作为优选，所述步骤s1中，通过传统方法对六自由度运动平台的控制中心及世界坐标系进行一轮粗定位：采用六自由度运动平台表面中心位置的销孔作为六自由度运动平台的控制中心，将六自由度运动平台处于零点状态时的控制中心位置作为测量坐标系的原点，将六自由度运动平台的三个坐标轴方向作为测量坐标系的三个轴的方向。

9、作为优选，所述步骤s2中：在步骤s1粗定位的基础上，对六自由度运动平台发送一组i个点的运动控制指令p6d平台i(x平台i,y平台i,z平台i,rx平台i,ry平台i,rz平台i)，测量得到一组一一对应的i个点的跟踪仪测得值p6d跟踪仪i(x跟踪仪i,y跟踪仪i,z跟踪仪i,rx跟踪仪i,ry跟踪仪i,rz跟踪仪i)，将这组测得值在步骤s3中用于作为最佳拟合的参数。

10、作为优选，所述步骤s3中，p6d(x,y,z,rx,ry,rz)六自由度点的矩阵形式：

11、6dmartix＝trans(x,y,z)×rotz(rz)×roty(ry)×rotx(rx)×i (1)

12、将步骤s2获得的i组参数带入公式(1)得以下目标函数公式(2)

13、

14、式中，6dmartix六自由度平台控制值i和6dmartix激光跟踪仪测量值i是分别为将p6d平台i(x平台i,y平台i,z平台i,rx平台i,ry平台i,rz平台i)和p6d跟踪仪i(x跟踪仪i,y跟踪仪i,z跟踪仪i,rx跟踪仪i,ry跟踪仪i,rz跟踪仪i)带入公式(1)计算得到的矩阵形式；

15、同时，由公式(1)的矩阵部分展开项罗列，其中6dmartix[i j]为6dmartix矩阵的第i行第j列：

16、

17、由公式(3)可以继续推导得到公式(4)

18、rx＝arctan(6dmartix[3 2]/6dmartix[3 3])

19、

20、rz＝arctan(6dmartix[21]/6dmartix[11])

21、x＝6dmartix[14]

22、y＝6dmartix[24]

23、z＝6dmartix[34] (4)

24、将6dmartix激光跟踪仪测量值i代入公式(2)、公式(4)，得到bi关于x的函数，如公式(5)所示：

25、

26、其中x＝[δrz1δry1δrx1 dx1 dy1 dz1 dx2 dy2 dz2δrz2δry2δrx2]t。那么，当有n个测量点时，可联立方程组得到公式(6)：

27、

28、其中，y＝[y1y2…yn]t，是对六自由度运动平台发送的n次运动控制指令得到的运动控制位置，yi＝[x平台i y平台i z平台i rz平台i ry平台i rx平台i]t。f(x)即bi的联立方程组；

29、以零矩阵作为x＝[δrz1δry1δrx1 dx1 dy1 dz1 dx2 dy2 dz2δrz2δry2δrx2]t的初值，根据公式(9)进行迭代，

30、xk+1＝xk-(jtj)-1[jt(f(xk)-y)] (9)

31、其中j为y＝f(xk)的雅克比矩阵，采用牛顿下山法迭代使公式(9)等式两边误差的平方和最小，得到x即(dx1,dy1,dz1,δrx1,δry1,δrz1)和(dx2,dy2,dz2,δrx2,δry2,δrz2)的最佳拟合值。

32、作为优选，所述步骤s4中，将步骤s3得到的(dx1,dy1,dz1,δrx1,δry1,δrz1)用于调整跟踪仪测量软件中世界坐标系的位姿；(dx2,dy2,dz2,δrx2,δry2,δrz2)用于六自由度运动平台的控制软件调整六自由度运动平台控制中心的位姿。

33、本发明的有益效果：本发明通过对传感器绑定的控制中心和真实控制中心的位置误差、六自由度运动平台世界坐标系与激光跟踪仪世界坐标系误差两大误差的修正，能够提高寻找世界坐标系和实际控制中心的精度，结合激光跟踪仪和t-mac等硬件设备，构建测量模型，能够较好地完成六自由度运动平台的校准。在测量误差被修正消除后，平台在运动状态下，提供了实时的平台控制值，通过相对固定的t-mac硬件设备实时获取测量值，两者相互比较即可完成实时的动态校准。该方法精度高、效率高，能够广泛应用于六自由度运动平台。

34、本发明的特征及优点将通过实施例结合附图进行详细说明。

技术特征：

1.六自由度运动平台控制中心的定位方法，其特征在于：包括以下步骤：

2.如权利要求1所述的六自由度运动平台控制中心的定位方法，其特征在于：所述步骤s1中，通过传统方法对六自由度运动平台的控制中心及世界坐标系进行一轮粗定位：采用六自由度运动平台表面中心位置的销孔作为六自由度运动平台的控制中心，将六自由度运动平台处于零点状态时的控制中心位置作为测量坐标系的原点，将六自由度运动平台的三个坐标轴方向作为测量坐标系的三个轴的方向。

3.如权利要求1所述的六自由度运动平台控制中心的定位方法，其特征在于：所述步骤s2中：在步骤s1粗定位的基础上，对六自由度运动平台发送一组i个点的运动控制指令p6d平台i(x平台i,y平台i,z平台i,rx平台i,ry平台i,rz平台i)，测量得到一组一一对应的i个点的跟踪仪测得值p6d跟踪仪i(x跟踪仪i,y跟踪仪i,z跟踪仪i,rx跟踪仪i,ry跟踪仪i,rz跟踪仪i)，将这组测得值在步骤s3中用于作为最佳拟合的参数。

4.如权利要求1所述的六自由度运动平台控制中心的定位方法，其特征在于：所述步骤s3中，p6d(x,y,z,rx,ry,rz)六自由度点的矩阵公式：

5.如权利要求1所述的六自由度运动平台控制中心的定位方法，其特征在于：所述步骤s4中，将步骤s3得到的(dx1,dy1,dz1,δrx1,δry1,δrz1)用于调整跟踪仪测量软件中世界坐标系的位姿；(dx2,dy2,dz2,δrx2,δry2,δrz2)用于六自由度运动平台的控制软件调整六自由度运动平台控制中心的位姿。

技术总结
本发明公开了六自由度运动平台控制中心的定位方法，通过对传感器绑定的控制中心和真实控制中心的位置误差、六自由度运动平台世界坐标系与激光跟踪仪世界坐标系误差两大误差的修正，能够提高寻找世界坐标系和实际控制中心的精度，结合激光跟踪仪和T‑MAC等硬件设备，构建测量模型，能够较好地完成六自由度运动平台的校准。在测量误差被修正消除后，平台在运动状态下，提供了实时的平台控制值，通过相对固定的T‑MAC硬件设备实时获取测量值，两者相互比较即可完成实时的动态校准。该方法精度高、效率高，能够广泛应用于六自由度运动平台。

技术研发人员：申睿,叶欣
受保护的技术使用者：浙江金融职业学院
技术研发日：
技术公布日：2024/12/5