CN113514017B - 一种并联驱动机构动平台位姿测量方法 - Google Patents

Abstract

本发明公开了一种并联驱动机构动平台位姿测量方法，首先在静平台安装4个位移传感器作为基点，组成静平台下的笛卡尔坐标系；计算出任意一动点在静坐标系下的坐标；然后测量基点和动点在测量设备为原点的坐标系下的坐标；得到误差函数，经过多次测量得到最优化估计的位移零点；再重复上述步骤得到三个静坐标系测量出的三个动点的坐标；最后根据三个动点坐标，计算动平台坐标系的平面法向量、平面内向量，并归一化动平台坐标系的平面法向量和平面内向量，通过向量外积计算出最后一个坐标轴在不动坐标系中的投影向量，归一化后的向量按列顺序组集而成的方阵即为动平台的姿态；本发明能够快速确定动平台位置。

Description

一种并联驱动机构动平台位姿测量方法

技术领域

本发明属于驱动平台位置姿态检测领域，具体涉及一种并联驱动机构动平台位姿测量方法。

背景技术

六并联驱动运动台的伺服驱动器可以根据电机码盘计数和电动缸丝杆导程推算每一个电动缸的行程，进一步根据各个电动缸的空间位置关系计算出上平台的位姿。这种求解过程属于运动学正解。对于并联驱动机构而言，求解过程较为复杂，需要联立多个距离方程和6个与坐标转换矩阵正交性相关的约束方程。特别是这几个方程的非线性较强，通常需要通过数值迭代的方式求解，不易保证运算的实时性和收敛性。

使用惯性测量单元可以精确的测量动平台的姿态，但是在运动台动平台位置测量方面的精度和数据稳定性较低。利用卫星定位信号可以确定运动台的地理位置，但是很难精确的分辨动平台微小的位置变化。光干涉方法可以敏感的感知动平台的位置变化，但是位移量程有限且对被测对象的位姿运动有较强的限制。激光测距法(比如全站仪)需要精细的瞄准被测结构上的指定点，不适合持续运动中物体的位置测量。

发明内容

本发明的目的在于提供一种并联驱动机构动平台位姿测量方法，通过线位移传感器对动平台的姿态和位置进行测量。

实现本发明目的的技术解决方案为：

一种并联驱动机构动平台位姿测量方法，包括以下步骤：

步骤1、建立静坐标并计算任意一动点坐标：在静平台安装4个位移传感器作为基点，其中一个传感器作为静坐标系原点，其他三个传感器和原点组成三个向量，三个向量两两正交，三个向量组成一个静平台下的笛卡尔坐标系；并根据位移传感器得出的任意一动点到4个基点距离，计算出任意一动点在静坐标系下的坐标；

步骤2、对位移传感器进行标定：使用测量设备测量基点在测量设备为原点的坐标系下的坐标；使用测量设备测量动点在测量设备为原点的坐标系下的坐标，并记录位移传感器的读数，得到误差函数，经过多次测量得到最优化估计的位移零点；

步骤3、测量三个动点位置坐标：重复两次步骤1-2，再得到两个标定后的静坐标系；选取动平台上三个不在同一条直线上的动点，分别得到三个静坐标系测量出的三个动点的坐标；

步骤4、动平台姿态计算：根据三个动点坐标，计算动平台坐标系的平面法向量、平面内向量，并归一化动平台坐标系的平面法向量和平面内向量，通过向量外积计算出最后一个坐标轴在不动坐标系中的投影向量，归一化后的向量按列顺序组集而成的方阵即为动平台的姿态。

本发明与现有技术相比，其显著优点是：

本发明使用新型方法测量动平台的位移和姿态，保证了位姿误差在有界范围内，降低了计算复杂度，丰富了动态物体位姿测量的方法，能够快速确定动平台位置。

附图说明

图1为并联驱动机构动平台位姿测量方法流程图。

图2为静坐标系和任意一动点坐标示意图。

图3为动平台姿态测量方法示意图。

具体实施方式

下面结合附图及具体实施例对本发明做进一步的介绍。

步骤1、建立静坐标系并计算任意一动点坐标：

1.1建立静坐标系

在静平台安装4个线位移传感器作为基点，其中一个传感器作为静坐标系原点，其他三个传感器和原点组三个向量，三个向量两两正交，三个向量组成一个静平台下的笛卡尔坐标系。

1.2计算任意一动点坐标

根据距离传感器得出任意一动点到基点的距离，算出任意一动点在静坐标系下的坐标，如图2所示。假设基点和任意一动点在坐标系中的坐标分别为(x_i,y_i,z_i)i＝0,1,2,3和(x_d,y_d,z_d)。定义L_ij为基点i到j的距离(其中i,j＝0,1,2,3i≠j)，传感器得到任意一动点到基点i距离为L_i(i＝0,1,2,3)，则任意一动点到基点i距离的平方

表示为(i＝0,1,2,3)：

由于(x₀,y₀,z₀)作为原点，因此x₀＝y₀＝z₀＝0，消去方程(1)中有关x_d、y_d和z_d的二次项，得到方程组，

于是任意一动点坐标为：

步骤2、对位移传感器进行标定：

拉线式位移传感器的原理比较简单，但是安装要求相对较高。为了充分发挥传感器性能，有必要通过标定确定线位移零点和修正基点坐标。

针对绝对式拉线位移传感器，标定的目的是消除安装偏差引起的初始位移量，即找出位移传感器的真实零点。为此，采用全站仪测量基点和动点的空间坐标，通过优化方法估计位移零点。

对于基点位置的标定，直接使用全站仪测量并换算出第i个基点在测量坐标系中的坐标([x_i]_m,[y_i]_m,[z_i]_m)i＝0,1,2,3。

为了标定线位移传感器，改变动点位置，并进行n次测量。其中第k次测量的空间坐标为([x_dk]_m,[y_dk]_m,[z_dk]_m)k＝1,…,n；同时记录线位移传感器的读数[L_ik]_m即为基点i到动点k的距离，其中i＝0,1,2,3k＝1,…n。

这时，根据公式(3)得到，

由于存在零点偏差，方程(4)不能平衡。定义ΔL_i为第i个位移传感器安装偏差引起的初始位移量(i＝0,1,2,3)，那么根据方程(4)得到函数，

位移传感器安装偏差引起的初始位移量ΔL_i应当使函数(5)等于0，但是F(ΔL_i)＝0只提供了3个等式却需要确定4个参数。这时，通过选定n个动点测量，使用二次寻优优化的方法决定参数ΔL_i。

选定优化目标函数，

J(ΔL_i)是一个标量函数，对于确定的测量数据([x_i]_m,[y_i]_m,[z_i]_m)，([x_dk]_m,[y_dk]_m,[z_dk]_m)和[L_ik]_m形成的数据集，使J(ΔL_i)最小时的ΔL_i是有关位移传感器零点的最佳估计。

步骤3、测量三个动点位置坐标：

重复两次步骤1至步骤2两次，再得到两个标定好的静坐标系；选取动平台上三个不在同一条直线上的动点，分别使用三个静坐标系测量三个动点的坐标；如图3所示：基点“0”(第一原点)，基点“1”、基点“2”和基点“3”组成第一组静坐标系，动点“12”在第一组静坐标系下的测量的坐标为[12]_f，f表示第一组坐标系。基点“4”(第二原点)，基点“5”、基点“6”和基点“7”组成第二组静坐标系，动点“13”在第二组静坐标系下的测量的坐标为[13]_f4，f₄表示第二组坐标系。基点“8”(第三原点)，基点“9”、基点“10”和基点“11”组成第三组静坐标系，动点“14”在第三组静坐标系下的测量的坐标为[14]_f8，f₈表示第三组坐标系。通过三组坐标系的坐标变换，使[13]_f4和[14]_f8统一到第一组静坐标系上，即为[13]_f和[14]_f。

步骤4、动平台姿态计算：

由于动点“12”，动点“13”和动点“14”不在一条直线上，通过[12]_f，[13]_f和[14]_f可以构建出一个坐标系。又由于动点“12”，动点“13”和动点“14”是与动平台固联的，所以通过动点“12”，动点“13”和动点“14”构建的坐标系可以作为动平台坐标系，动平台坐标系的平面法向量[n]_f表示为：

[n]_f＝([13]_f-[12]_f)×([14]_f-[12]_f) (7)

归一化动平台坐标系的平面法向量[n]_f得到

即为静坐标系中动平台平面法线在不动坐标系中的投影坐标。通过动点“13”和动点“14”的连线定义动平台平面内的一个坐标轴。于是动平台坐标系的有平面内向量[t]_f：

[t]_f＝[13]_f-[14]_f (8)

归一化动平台坐标系的平面内向量[t]_f得到

为动平台在平面内选定坐标轴方向在不动坐标系中的投影坐标。平面法向量[n]_f与平面内向量

必然是正交的。通过向量外积计算出坐标系的最后一个坐标轴在不动坐标系中的投影

归一化后的向量

和

两两正交，按列顺序组集而成的方阵即为从动平台旋转到不动平台的传递矩阵。于是动平台相对不动平台的姿态得解。

位置误差分析：

公式(3)中，基点坐标和基点到原点的距离是常数，测量误差来自L_i。假定测量偏差是一致的，对公式(3)两端取变分后得到，

由于L₀，L₁和L₁₀构成三角形，所以L₁₀≥L₀-L₁。同理，L₂₀≥L₀-L₂，L₃₀≥L₀-L₃。所以根据(10)得到，

按照矩阵逆的定义，公式(11)中的逆矩阵可以改写为伴随矩阵除以矩阵的行列式。其中矩阵行列式即为以坐标原点为起点，基点为终点的3个空间向量所决定的四棱柱体积。而伴随矩阵与右端的[L₁₀ L₂₀ L₃₀]^T相乘后得到的3×1维矩阵的元素是以坐标原点为起点，(L₁₀,L₂₀,L₃₀(为终点的空间向量与其它基点确定的空间向量所决定的四棱柱体积。如果定义基点向量分别是

和

(L₁₀,L₂₀,L₃₀)与坐标原点确定的空间向量为

那么不等式(11)可以改写为，

由于

因此δx_d，δy_d和δz_d的上限分别由

与

确定的空间体积与

确定的空间体积的比值进行增益。又由于

是由

的长度构成的，所以当

两两正交时，增益系数获得最小值1。

因此，通过合理选择基点的空间分布，使之与坐标原点构成的向量两两正交时，对动平台平动位移的测量偏差不高于基点至动点距离的测量偏差。

姿态误差分析：

用符号δ表示对应测量值的偏差，将测量值表示为真值和偏差的和，根据方程(7)，[n]_f的偏差δ[n]_f表示为，

按照叉乘运算法则，

根据位置误差的分析，通过合理的布置测量基点，被测点位置的测量偏差与线位移传感器的精度一致。因此δ[12]_f、δ[13]_f和δ[14]_f中各个元素的上限均为δL，即线位移传感器精度。于是δ[12]_f、δ[13]_f和δ[14]_f两两求差时，差向量中各个元素极限偏差的绝对值为2δL，而差向量长度的最大值为

所以δ[n]_f有上限，

定义平面法线测量误差偏角为θ，则

点“12”，点“13”和点“14”的空间位置是可以设计调整的。对于呈等边三角形的空间布局，|[13]_f-[12]_f|＝|[14]_f-[12]_f|，且

所以，

根据公式(8)，向量[t]_f的偏差为，

δ[t]_f＝δ[13]_f-δ[14]_f (18)

所以，

定义[t]_f测量误差偏角为φ，则，

根据公式(9)，[s]_f的偏差为，

δ[s]_f＝δ[n]_f×[t]_f+[n]_f×δ[t]_f+δ[n]_f×δ[t]_f (21)

所以，

|δ[s]_f|≤|δ[n]_f||[t]_f|+|[n]_f||δ[t]_f|+|δ[n]_f||δ[t]_f| (22)

将方程(7)，方程(8)，方程(15)和方程(19)代入不等式(22)，得到，

对于呈等边三角形的空间布局，不等式(23)化简为，

定义[s]_f测量误差偏角为ψ，则，

从公式(17)，(20)和(25)可以看出，如果忽略高阶小量，姿态角偏差正比于测量误差与基线(等边三角形边长)的比值。对于有界的测量误差，增大基线长度可以减小姿态角测量偏差。

Claims (5)

1.一种并联驱动机构动平台位姿测量方法，其特征在于，包括以下步骤：

步骤1、建立静坐标并计算任意一动点坐标：在静平台安装4个位移传感器作为基点，其中一个传感器作为静坐标系原点，其他三个传感器和原点组成三个向量，三个向量两两正交，三个向量组成一个静平台下的笛卡尔坐标系；并根据位移传感器得出的任意一动点到4个基点距离，计算出任意一动点在静坐标系下的坐标；

步骤2、对位移传感器进行标定：使用测量设备测量基点在测量设备为原点的坐标系下的坐标；使用测量设备测量动点在测量设备为原点的坐标系下的坐标，并记录位移传感器的读数，得到误差函数，经过多次测量得到最优化估计的位移零点；

步骤3、测量三个动点位置坐标：重复两次步骤1-2，再得到两个标定后的静坐标系；选取动平台上三个不在同一条直线上的动点，分别得到三个静坐标系测量出的三个动点的坐标；

步骤4、动平台姿态计算：根据三个动点坐标，计算动平台坐标系的平面法向量、平面内向量，并归一化动平台坐标系的平面法向量和平面内向量，通过向量外积计算出最后一个坐标轴在不动坐标系中的投影向量，归一化后的向量按列顺序组集而成的方阵即为动平台的姿态。

2.根据权利要求1所述的并联驱动机构动平台位姿测量方法，其特征在于，步骤1得到的动点坐标为：

其中(x_i,y_i,z_i)i＝0,1,2,3表示基点的坐标，L_i为动点到基点i距离；L_ij为基点i到j的距离。

3.根据权利要求1所述的并联驱动机构动平台位姿测量方法，其特征在于，步骤2得到的误差函数为：

其中ΔL_i为第i个位移传感器安装偏差引起的初始位移量，i＝0,1,2,3；[L_ik]_m为基点i到动点k的距离，k＝1,…,n，n为动点个数；([x_i]_m,[y_i]_m,[z_i]_m)为基点i在测量坐标系中的坐标；([x_dk]_m,[y_dk]_m,[z_dk]_m)为第k次测量的空间坐标；

最优估计的优化目标函数为：

使J(ΔL_i)最小时的ΔL_i是有关位移传感器零点的最佳估计。

4.根据权利要求1所述的并联驱动机构动平台位姿测量方法，其特征在于，

平面法向量[n]_f为：

[n]_f＝([13]_f-[12]_f)×([14]_f-[12]_f)

其中[12]_f、[13]_f、[14]_f分别为三个动点在相应静坐标系下的测量的坐标；

平面内向量[t]_f为：

[t]_f＝[13]_f-[14]_f

最后一个坐标轴在不动坐标系中的投影向量

为：

其中

分别为静坐标系中动平台平面法线在不动坐标系中的投影坐标和动平台在平面内选定坐标轴方向在不动坐标系中的投影坐标。

5.根据权利要求1所述的并联驱动机构动平台位姿测量方法，其特征在于，所述测量设备为全站仪。

Images