CN114413893A - 一种融合惯性测量信息的动态位置、姿态同步的测量方法 - Google Patents

Abstract

本发明公开了一种融合惯性测量信息的动态位置、姿态同步的测量方法，包括：将IMU与wMPS光电接收器构成的位姿测靶刚性连接，利用三坐标测量机分别测量光电接收器球心坐标并拟合IMU自身坐标系，实现wMPS与IMU观测对象的统一；将刚性连接后的位姿测靶放置在被测目标上并随目标运动，利用激光发射站测量各接收器扫描角，构建光平面几何约束关系；将初始位置处的wMPS位姿观测作为IMU状态更新初值，利用IMU输出的角速度和加速度逐次更新IMU状态；若相邻两次IMU状态更新过程中有新的wMPS测量结果，则进行wMPS与IMU位姿测量数据融合，得到融合后的系统状态量，并结合IMU自身更新输出最终的位姿。

Description

一种融合惯性测量信息的动态位置、姿态同步的测量方法

技术领域

本发明涉及大空间动态测量领域，尤其涉及一种融合惯性测量信息的动态位置、姿态同步的测量方法。

背景技术

随着以航天器、飞机为代表的航空、航天领域高端装备制造技术的发展，智能制造成为重要趋势和研究热点，是促进装备制造向高端迈进、建设制造强国的重要举措。测量作为获取几何量信息的关键途径逐步融入装备制造的各个环节，是实现智能制造的必要前提。传统的单目标、静态测量模式难以满足效率和实时性等诸多要求，亟待研究与先进制造技术相匹配的高精度、多目标、多自由度动态测量技术，推动装备智能制造进程。

激光跟踪测量和视觉测量是当前装备制造中普遍采用的测量方式。作为激光跟踪测量系统的代表，激光跟踪仪利用相互垂直的两个回转机构驱动激光测头跟踪目标靶镜实现跟踪测量，单次只能测量单个目标，测量效率低。此外，由于靶镜接收角度有限，当其随目标运动，尤其是进行角运动时容易出现断光问题，影响动态测量连续性。视觉测量发展成熟，系统成本低，但是受视场角限制，单个相机的测量范围有限，视场边缘存在畸变误差。测量大型目标时需要利用多相机组网，增加了系统标定和精度控制的难度。此外，视觉测量系统对复杂现场环境下的杂光干扰敏感，拍摄动态目标时会产生图像拖影，影响成像质量，引入动态误差。

分布式光电扫描测量作为一种较为新兴的测量方式在近些年得到了越来越多的关注，并诞生了以iGPS(室内GPS)和wMPS(工作空间测量定位系统)为代表的测量系统，具有静态测量精度高、多目标并行测量、测量范围可扩展等显著优势，在飞机水平测量、AGV(自动导引车)导航等领域得到了成功应用，具有较强的动态测量潜能。受限于光电扫描测量原理，测量运动目标时的多台测站观测信息在时间和空间上均存在非同步问题，产生多角度交会误差，致使其动态测量精度远低于静态测量精度，目前仅可实现静态或准静态测量，限制了应用场景。研究分析光电扫描测量系统动态误差特性并对其进行针对性补偿是提高其动态测量应用价值的有效手段，能够进一步完善光电扫描动态测量的理论体系，具有重要的理论研究和工程应用价值。

由动态误差产生机理可知，光电扫描测量系统动态误差的关键在于获取其测量间隔内的目标角运动和线运动信息。以陀螺仪和加速度计为代表的惯性测量系统能够全天候、自主地感知目标运动角速度和加速度，具有测量频率高、抗干扰能力强等特点，其独立进行位姿测量时的漂移误差随时间快速发散，无法满足精度要求。受限于发射站转速，wMPS测量频率较低，但其单次测量误差间互不相关，具有长期稳定性，需要外部辅助系统敏感其测量间隔内的运动信息进而实现误差补偿。因此，wMPS与IMU(惯性测量单元)二者具有优良的测量互补性。IMU辅助wMPS动态测量成为提高wMPS动态性能的重要方法，其中的关键问题在于两个测量系统被测对象的统一问题和观测数据的有效融合问题。

发明内容

本发明提供了一种融合惯性测量信息的动态位置、姿态同步的测量方法，本发明首先介绍wMPS位姿测靶与IMU间坐标系标定，随后提出一种wMPS观测数据与IMU观测数据融合，实现测靶的动态位姿实时测量，提高wMPS动态测量精度和实时性，进一步满足高端装备智能制造背景下的高性能动态测量需求，详见下文描述：

一种融合惯性测量信息的动态位置、姿态同步的测量方法，所述方法包括：

将IMU与wMPS光电接收器构成的位姿测靶刚性连接，利用三坐标测量机分别测量光电接收器球心坐标并拟合IMU自身坐标系，实现wMPS与IMU观测对象的统一；

将刚性连接后的位姿测靶放置在被测目标上并随目标运动，利用激光发射站测量各接收器扫描角，构建光平面几何约束关系；将初始位置处的wMPS位姿观测作为IMU状态更新初值，利用IMU输出的角速度和加速度逐次更新IMU状态；

若相邻两次IMU状态更新过程中有新的wMPS测量结果，则进行wMPS与IMU位姿测量数据融合，得到融合后的系统状态量，并结合IMU自身更新输出最终的位姿。

其中，所述利用三坐标测量机分别测量光电接收器球心坐标并拟合IMU自身坐标系，实现wMPS与IMU观测对象的统一具体为：

利用三坐标测量机的接触式测头分别测量相互正交的三个平面上的多个特征点在三坐标测量机坐标系下的空间坐标，利用不同平面上的特征点分别拟合空间平面，进而确定坐标轴方向；三个平面的交点即为IMU坐标系原点；利用N个特征点拟合方程转化为使第一目标函数最小，通过最优化得到平面方程和法向量；

利用接触式测头测量接收器球面上的多个特征点坐标；利用M个特征点拟合球面转化为使第二目标函数最小：通过最优化得到球心和半径。

其中，所述若相邻两次IMU状态更新过程中有新的wMPS测量结果，则进行wMPS与IMU位姿测量数据融合，得到融合后的系统状态量，具体为：

构建系统方程和观测方程，根据IMU机械编排得到矩阵F和G；基于矩阵F和G将系统方程和观测方程离散化，得到其离散形式；

将wMPS的观测噪声v_k分解为白噪声v_{k_w}和时间相关噪声v_{k_c}，利用一阶自回归模型对时间相关噪声v_{k_c}建模得到观测方程；

对系统状态量进行扩维，以[δx_k v_{k-1_c}]^T作为新的系统状态量得到新的系统状态方程和观测方程；

对新的系统方程和观测方程进行变形，并进一步分析统计学特性，最终得到递推方程，实现对系统状态量的连续估计。

本发明提供的技术方案的有益效果是：

1、本发明提出了一种wMPS与IMU位姿观测数据融合模型，可以在被测目标自由运动的状态下实现IMU对wMPS动态误差的实时补偿，同时wMPS周期性校正IMU累计误差，最终实现wMPS与IMU的优势互补；

2.通过精度验证实验可知，在不同的目标运动速度下，利用上述方法均能显著减小wMPS动态误差，同时在现有wMPS硬件基础上显著提高其位姿测量数据率；

3、本发明能够从测量精度和实时性两方面提升wMPS动态性能，有助于实现wMPS静态、动态全工况下的精确、跟踪测量，能够为多基站、多相机等分布式测量系统动态误差分析与补偿提供依据，具有一定的理论和工程价值。

附图说明

图1为位姿测靶示意图；

图2为测靶IMU坐标系拟合示意图；

图3为测靶wMPS接收器球心拟合示意图；

图4为数据融合原理框图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面对本发明实施方式作进一步地详细描述。

实施例1

本发明实施例提供了一种融合惯性测量信息的动态位置、姿态同步的测量方法，参见图1-图4，该方法包括以下步骤：

101：将IMU与wMPS光电接收器构成的位姿测靶刚性连接，利用三坐标测量机分别测量光电接收器球心坐标并拟合IMU自身坐标系，实现wMPS与IMU观测对象的统一；

102：将刚性连接后的位姿测靶放置在被测目标上并随目标运动，利用激光发射站测量各接收器扫描角，构建光平面几何约束关系；将初始位置处的wMPS位姿观测作为IMU状态更新初值，利用IMU输出的角速度和加速度逐次更新IMU状态；

103：若相邻两次IMU状态更新过程中有新的wMPS测量结果，则进行wMPS与IMU位姿测量数据融合，得到融合后的系统状态量，并结合IMU自身更新输出最终的位姿。

其中，上述步骤101包括：

利用三坐标测量机的接触式测头分别测量相互正交的三个平面上的多个特征点在三坐标测量机坐标系下的空间坐标，利用不同平面上的特征点分别拟合空间平面，进而确定坐标轴方向；三个平面的交点即为IMU坐标系原点；利用N个特征点拟合方程转化为使第一目标函数最小，通过最优化得到平面方程和法向量；

利用接触式测头测量接收器球面上的多个特征点坐标；利用M个特征点拟合球面转化为使第二目标函数最小：通过最优化得到球心和半径。

其中，上述步骤103包括：

构建系统方程和观测方程，根据IMU机械编排得到矩阵F和G；基于矩阵F和G将系统方程和观测方程离散化，得到其离散形式；

将wMPS的观测噪声v_k分解为白噪声v_{k_w}和时间相关噪声v_{k_c}，利用一阶自回归模型对时间相关噪声v_{k_c}建模得到观测方程；

对系统状态量进行扩维，以[δx_k v_{k-1_c}]^T新的系统状态量得到新的系统状态方程和观测方程；

对新的系统方程和观测方程进行变形，并进一步分析统计学特性，最终得到递推方程实现对系统状态量的连续估计。

综上所述，本发明实施例在分析wMPS动态测量瓶颈问题的基础上，在测量系统中引入IMU，利用其提供的被测目标角运动和线运动信息辅助动态测量，通过数据融合补偿wMPS动态测量误差，同时利用wMPS观测的时间稳定性定期地修正IMU累积误差，实现wMPS与IMU观测误差的相互补偿，最终实现两方面的动态性能改善：1、显著提高wMPS系统动态测量精度；2、使wMPS系统的数据输出速率达到IMU同频率，提高测量实时性。

实施例2

下面结合具体的实例、图1-4、具体的计算公式对实施例1中的方案进行进一步地介绍，详见下文描述：

步骤201：根据测量精度、范围等需求在空间内布置N台发射站，利用现有的控制场标定方法或基准尺标定方法确定N台发射站与全局坐标系间的旋转和平移关系；

其中，该步骤201为本领域技术人员所公知，本发明实施例对此不做赘述。

步骤202：利用6个wMPS光电接收器构成位姿测靶，将IMU与位姿测靶刚性连接(如图1所示)，利用三坐标测量机分别测量六个光电接收器球心坐标并拟合IMU自身坐标系的坐标轴方向及坐标原点，实现wMPS与IMU观测对象的统一；

其中，步骤202包括以下步骤：

1、将IMU通过“L”型平板与位姿测靶刚性连接，“L”型板的上表面平面度由精密机械加工保证，平面度误差优于0.05mm即可满足需求。由于IMU自身底面具备平面基准功能，因此可认为“L”型板上表面与IMU底面重合。

2、标定IMU坐标系的坐标轴方向及坐标原点，如图2所示。利用三坐标测量机的接触式测头分别测量相互正交的三个平面上的多个特征点在三坐标测量机坐标系下的空间坐标，利用不同平面上的特征点分别拟合空间平面，进而确定坐标轴方向；三个平面的交点即为IMU坐标系原点。具体地，以XOY平面(Z轴，图2中平面1)拟合为例，测量得到的N个特征点坐标P_i ^xoy记为：

其中，i表示点号。

设平面方程为Ax+By+Cz+d＝0，记

则平面方程重写为：z＝a₀x+a₁y+a₂。

利用N个特征点拟合方程可以转化为使如下的目标函数最小：

通过最优化方法即可得到平面方程和法向量。IMU坐标系平面YOZ和ZOX可利用相同方法得到。

3、在同一坐标系下利用多点拟合球面的方法得到球心坐标，如图3所示。利用接触式测头测量接收器球面上的多个特征点P_i ^s记为：

设球面方程为

其中

为球心坐标，R为球半径。利用M个特征点拟合球面可以转化为使如下的目标函数最小：

通过最优化方法即可得到球心和半径。至此，实现了测靶坐标系和IMU坐标系的统一，即wMPS和IMU观测同一对象。

步骤203：将包含6个接收器和IMU的位姿测靶放置在被测目标上并随目标运动，利用激光发射站测量各接收器扫描角，构建如下的光平面几何约束关系；

其中，[a_ij b_ij c_ij d_ij]表示发射站j的第i个光平面法向量和截距；θ_ijk表示发射站j的第i个光平面对测靶上第k个接收器的扫描角；R_j和T_j表示发射站j与测量场全局坐标系之间的旋转矩阵和平移矩阵；R＝f(α,β,γ)，α、β和γ表示测靶姿态角；T＝[T_x,T_y,T_z]，T_x、T_y和T_z表示测靶在全局坐标系下的位置，与姿态角共同表征测靶六自由度状态。

基于光平面约束和旋转矩阵正交约束建立如下的目标函数，利用非线性优化方法解算得到目标位姿，同时记录位姿测量时刻t_wMPS：

其中，O_m表示旋转矩阵正交误差惩罚项；λ表示惩罚因子。

步骤204：以初始位置处的wMPS位姿观测为IMU状态更新初值，利用IMU输出的角速度和加速度逐次更新IMU姿态、速度和位置

v_IMU和p_IMU，并记录测量时刻t_IMU；

步骤205：若相邻两次IMU状态更新过程中有新的wMPS测量结果产生，则进行wMPS与IMU位姿测量数据融合，得到融合后的系统状态量，融合原理框图如图4所示；

其中，步骤205包括以下步骤：

1)构建如下所示的系统方程和观测方程：

其中，

为状态量，

δv和δp分别为姿态角误差、速度误差和位置误差，b_g和b_f分别为陀螺仪和加速度计静态偏置误差，δb_g和δb_f分别为陀螺仪和加速度计的动态偏置误差，“.”表示一阶导数。

为观测量，F为状态转移矩阵，G为噪声驱动矩阵，u＝[ω_g ω_f η_sg η_δf]为驱动噪声，ξ为零均值高斯白噪声，并且ξ服从均值为0，方差为Q的正态分布；

其中

和

分别为陀螺和加速度计随机误差的功率谱密度，

和

分别为陀螺和加速度计动态偏置的功率谱密度。H为观测矩阵，v为观测噪声，并且v服从均值为0，方差为R的正态分布；

为姿态角方差，

为位置方差。

2)根据IMU机械编排得到矩阵F和G：

其中，

表示从IMU坐标系到全局坐标系间的旋转矩阵，f^b表示三轴加速度输出值，()_×表示向量的反对称矩阵，τ_g表示陀螺仪时间常数，τ_f表示加速度计时间常数，0表示零矩阵，I表示单位阵。

3)将系统方程和观测方程离散化，得到其离散形式：

其中，Φ_k/k-1＝e^FΔt≈I+FΔt，Δt表示wMPS测量间隔，δx_k表示第k次测量的系统状态量，H_k表示第k次测量的观测矩阵。

4)将wMPS观测噪声v_k分解为白噪声v_{k_w}和时间相关噪声v_{k_c}并表示如下：

其中，

A_{k_w}表示白噪声分量系数矩阵，A_{k_c}表示时间相关噪声分量系数矩阵。

利用一阶自回归模型对v_{k_c}建模：v_{k_c}＝ψ_{k/k-1_c}v_{k-1_c}+σ_k-1，ψ_{k/k-1_c}表示一阶自回归系数；σ_k-1表示白噪声误差项。

则观测方程可以表示为：

5)对系统状态量进行扩维，以[δx_k v_{k-1_c}]^T新的系统状态量得到新的系统状态方程：

令δx'_k＝[δx_k v_{k-1_c}]^T，

u'_k-1＝[u_k-1 σ_k-2]^T，H'_k＝[H_k A_{k_c}ψ_{k/k-1_c}]，v'_k＝A_{k_w}v_{k_w}+A_{k_c}σ_k-1，则新的系统方程和观测方程可表示为：

6)对状态方程和观测方程进行如下变形：

δx'_k＝Φ'_k/k-1δx'_k-1+G'_ku'_k-1+B_k-1(δz_k-H'_kδx'_k-v'_k) (15)

令

则：

进一步分析

的统计学特性可知：

若系统噪声和观测噪声不相关，则G'_kS'_k-B_kR'_k＝0，因此，B_k＝G'_kS'_k(R'_k)^-1。

7)最终利用如下的递推方程实现对系统状态量的连续估计：

步骤206：利用融合输出和IMU自身更新得到的姿态角、速度和位置得到最终的位姿输出。

综上所述，本发明实施例通过上述步骤201-步骤206实现测靶的动态位姿实时测量，提高wMPS动态测量精度和实时性，进一步满足高端装备智能制造背景下的高性能动态测量需求。

实施例3

下面结合具体的数据对实施例1和2中的方案进行可行性验证，详见下文描述：

利用高精度直线导轨对IMU辅助下的wMPS动态测量精度进行实验验证，直线导轨的直线度误差通过激光干涉仪测量得到，误差最大值小于0.05mm。将wMPS位姿测靶固定在直线导轨位移台上做直线运动，融合wMPS与IMU数据解算位姿测靶的空间位置，IMU测量频率设置为80Hz。利用运动轨迹上的多个测量点拟合空间直线，计算各测量点位处的直线度误差，利用直线度RMSE(均方根误差)作为运动全过程动态测量误差的表征指标。分别设置不同的导轨运动速度，不同速度下的直线度均方根误差如表1所示：

表1

由上述实验结果可知：当单独使用wMPS时，导轨的直线度RMSE随着运动速度的增加而增加。进一步融合IMU观测信息后，不同速度下的直线度RMSE均显著下降，表明IMU在不同运动状态下均能够有效补偿wMPS动态误差。同时，从数据率可以看出，组合系统的数据率与IMU数据率(80Hz)相同并且远高于wMPS测量频率(20Hz)，通过数据融合从精度和数据率(实时性)两方面增强了wMPS的动态性能。

本发明实施例对各器件的型号除做特殊说明的以外，其他器件的型号不做限制，只要能完成上述功能的器件均可。

本领域技术人员可以理解附图只是一个优选实施例的示意图，上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种融合惯性测量信息的动态位置、姿态同步的测量方法，其特征在于，所述方法包括：

将IMU与wMPS光电接收器构成的位姿测靶刚性连接，利用三坐标测量机分别测量光电接收器球心坐标并拟合IMU自身坐标系，实现wMPS与IMU观测对象的统一；

将刚性连接后的位姿测靶放置在被测目标上并随目标运动，利用激光发射站测量各接收器扫描角，构建光平面几何约束关系；将初始位置处的wMPS位姿观测作为IMU状态更新初值，利用IMU输出的角速度和加速度逐次更新IMU状态；

若相邻两次IMU状态更新过程中有新的wMPS测量结果，则进行wMPS与IMU位姿测量数据融合，得到融合后的系统状态量，并结合IMU自身更新输出最终的位姿。

2.根据权利要求1所述的一种融合惯性测量信息的动态位置、姿态同步的测量方法，其特征在于，所述利用三坐标测量机分别测量光电接收器球心坐标并拟合IMU自身坐标系，实现wMPS与IMU观测对象的统一具体为：

利用三坐标测量机的接触式测头分别测量相互正交的三个平面上的多个特征点在三坐标测量机坐标系下的空间坐标，利用不同平面上的特征点分别拟合空间平面，进而确定坐标轴方向；三个平面的交点即为IMU坐标系原点；利用N个特征点拟合方程转化为使第一目标函数最小，通过最优化得到平面方程和法向量；

利用接触式测头测量接收器球面上的多个特征点坐标；利用M个特征点拟合球面转化为使第二目标函数最小：通过最优化得到球心和半径。

3.根据权利要求2所述的一种融合惯性测量信息的动态位置、姿态同步的测量方法，其特征在于，

所述第一目标函数为：

其中，

为以XOY平面拟合得到的特征点三轴坐标；a₀,a₁,a₂为平面方程z＝a₀x+a₁y+a₂的系数；

IMU坐标系的YOZ和ZOX平面的目标函数利用相同方法得到；

所述第二目标函数为：

其中，

为球心坐标，R为球半径，

为利用接触式测头测量接收器球面上的多个特征点的坐标。

4.根据权利要求1所述的一种融合惯性测量信息的动态位置、姿态同步的测量方法，其特征在于，所述若相邻两次IMU状态更新过程中有新的wMPS测量结果，则进行wMPS与IMU位姿测量数据融合，得到融合后的系统状态量，具体为：

构建系统方程和观测方程，根据IMU机械编排得到矩阵F和G；基于矩阵F和G将系统方程和观测方程离散化，得到其离散形式；

将wMPS的观测噪声v_k分解为白噪声v_{k_w}和时间相关噪声v_{k_c}，利用一阶自回归模型对时间相关噪声v_{k_c}建模得到观测方程；

对系统状态量进行扩维，以[δx_k v_{k-1_c}]^T作为新的系统状态量得到新的系统状态方程和观测方程；

对新的系统方程和观测方程进行变形，并进一步分析统计学特性，最终得到递推方程，实现对系统状态量的连续估计。

5.根据权利要求4所述的一种融合惯性测量信息的动态位置、姿态同步的测量方法，其特征在于，所述矩阵F和G为：

其中，

表示从IMU坐标系到全局坐标系间的旋转矩阵，f^b表示三轴加速度输出值，()_×表示向量的反对称矩阵，τ_g表示陀螺仪时间常数，τ_f表示加速度计时间常数，0表示零矩阵，I表示单位阵。

6.根据权利要求4所述的一种融合惯性测量信息的动态位置、姿态同步的测量方法，其特征在于，

所述观测噪声v_k分解为白噪声v_{k_w}和时间相关噪声v_{k_c}表示如下：

其中，

A_{k_w}表示白噪声分量系数矩阵，A_{k_c}表示时间相关噪声分量系数矩阵；

利用一阶自回归模型对v_{k_c}建模，则观测方程表示为：

其中，ψ_{k/k-1_c}表示一阶自回归系数；σ_k-1表示白噪声误差项；对系统状态量[δx_k v_{k-1_c}]^T进行扩维得到新的系统状态方程：

令δx'_k＝[δx_k v_{k-1_c}]^T，

u'_k-1＝[u_k-1 σ_k-2]^T，H'_k＝[H_k A_{k_c}ψ_{k/k-1_c}]，v'_k＝A_{k_w}v_{k_w}+A_{k_c}σ_k-1，则新的系统方程和观测方程表示为：

7.根据权利要求4所述的一种融合惯性测量信息的动态位置、姿态同步的测量方法，其特征在于，所述连续估计为：

Images