CN103591949A

CN103591949A - 三轴姿态测量系统非正交性误差的正交补偿方法

Info

Publication number: CN103591949A
Application number: CN201310517875.4A
Authority: CN
Inventors: 程为彬; 潘萌; 汤楠; 汪跃龙; 霍爱清
Original assignee: Xian Shiyou University
Current assignee: Xian Shiyou University
Priority date: 2013-10-25
Filing date: 2013-10-25
Publication date: 2014-02-19
Anticipated expiration: 2033-10-25
Also published as: CN103591949B

Abstract

本发明公开了一种三轴姿态测量传感器非正交性误差的正交补偿方法，通过对每一个测量实轴建立各自独立的虚拟正交仪器坐标系，得到各自的旋转矩阵，再融合得到三轴非正交性的旋转矩阵，目的是通过虚拟正交建模，得到准确的非正交性误差模型，之后通过特殊位置的测量值和理论值求得一个补偿矩阵，将三轴传感器测量结果正交解耦，以减小非正交性及其所造成的测量误差，提高姿态测量精确度。

Description

三轴姿态测量系统非正交性误差的正交补偿方法

技术领域

本发明应用于导航、制导与控制工程，属于精密测量仪器技术领域，特别涉及一种三轴姿态测量系统非正交性误差的正交补偿方法。

背景技术

姿态测量和控制广泛应用于国防、军事、工业、农业和商业领域，减小测量误差是提高姿态测量精度的重要技术手段。从误差性质来分，有系统误差、随机误差和坏差。坏差明显歪曲了测量结果，这里不讨论。随机误差服从统计规律，一般由不确定性因素造成，如电磁场的变化、零件的摩擦与间隙、热起伏、空气扰动、气压、温湿度和测量人员的感官生理变化等，最重要的表现为抵偿性，即随机误差是可以通过数据处理来减小或消除的。系统误差是固定的，或服从一定的函数规律，产生原因众多，通常涉及测量仪器设备、测量装置对象、测量技术方法、测量者操作水平等，一般可以通过校正或补偿来减小。

姿态测量系统中存在各类来源误差，如零位误差、非正交误差（包括位置/位移误差、指向/方位误差、安装误差等）、标度因子误差、温度漂移误差、传感器测量误差和数据处理误差等，其中结构产生的非正交误差占到整体误差的70%，本发明主要研究其中的非正交误差。所谓非正交误差是指由于机械制造、加工所形成的各轴之间不正交，地理坐标系和仪器坐标系初始不重合，以及传感器安转或测量装置二次安装等形成的各轴空间偏差，是各测量轴与理想地理坐标系之间不正交度的表现。

经典误差理论证明：对于系统误差要采用合适的校正补偿方法，对随机误差可以采用各种基于数据统计的拟合方法。

部分文献综合零位误差、标度因子误差、非正交误差、温度漂移误差、测量误差、数据处理误差等，形成一个统一的校正模型。以上各类误差包括系统误差和随机误差，而它们的处理方法是不同的，一个统一的模型显然难以同时满足两类性质完全不同误差的校正。

部分文献采用最小二乘法、共轭梯度法、最大似然法、卡尔曼滤波、神经网络、参数估计、微分进化等算法，以及它们的改进算法，对姿态测量结果进行处理，而这些算法主要适合随机误差的处理，对系统误差的处理难以奏效。

部分文献对姿态传感器的位置/位移、指向/方位、安装误差进行了独立建模，给出了各轴的相对误差角，但同时也存在缺陷，如假定地理坐标系S轴和仪器坐标系初始Z轴重合，这本身就是忽略了这2个轴的轴不重合误差，显然不全面。

还有文献利用各类方法对三轴的非正交性进行解耦校正，即将测量结果正交化，但在处理时采用了简化方案，或前提条件不严谨，或采用公式简化，人为引入了方法误差，降低了校正补偿精度。

发明内容

为克服上述现有姿态测量系统中非正交性误差补偿方法的不足，本发明的目的在于提供一种三轴姿态测量系统非正交性误差的正交补偿方法，综合非正交性误差的形成，将传感器三轴看成3个独立的空间轴，通过包含这3个实轴的虚拟正交坐标系的多次坐标变换，获得传感器非正交性误差的补偿矩阵，最终将三轴传感器测量结果正交化，减小非正交性及其误差，提高测量精度。

为了实现上述目的，本发明采用的技术方案是：

一种三轴姿态测量系统非正交性误差的正交补偿方法，包括如下步骤：

步骤1，在姿态测量系统中建立理想正交的地理坐标系，并将该坐标系通过欧拉旋转得到一个初始虚拟、正交的理想仪器坐标系，此时的仪器姿态角即方位角、倾斜角和工具面角；

步骤2，该理想仪器坐标系分别以3组不同的姿态误差角做3组坐标变换，每组坐标变换均为3个不同姿态误差角的欧拉旋转，从得到的3个虚拟正交仪器坐标系中分别抽取X轴、Y轴和Z轴，构成实际的测量仪器坐标系；

步骤3，根据3组坐标变换所得到的相应实轴输出与正交虚拟的理想仪器坐标系理论输出的关系矩阵，得到非正交仪器坐标系各测量轴的实际输出与理想正交仪器坐标系理论输出的关系矩阵；

步骤4，通过特殊位置的实际仪器坐标系测量输出和正交仪器坐标系理论输出，逆向求解关系矩阵，得到一个校正矩阵，该矩阵表示三轴正交理论输出与实际非正交测量结果之间的关系，即将非正交测量结果进行正交解耦，得到补偿的三轴正交化测量结果。

所述三轴姿态测量传感器包括加速度计、陀螺仪、磁强计及MEMS惯性器件。

所述地理坐标系包括北西天、北东地坐标系，其旋转正方向由右手定则或左手定则决定。

所述步骤2中，每组坐标变换是指将所述虚拟正交仪器坐标系绕第一个轴旋转第一姿态误差角，再绕第二个轴旋转第二姿态误差角，再绕第三个轴旋转第三姿态误差角，最后得到一个新的虚拟正交仪器坐标系，3组坐标变换的旋转顺序相同，共得到三个新的虚拟正交仪器坐标系。

所述步骤2中，从3个新的虚拟正交仪器坐标系中分别抽取X轴、Y轴和Z轴，构成实际并不完全正交的测量仪器坐标系，3组变换所形成的9个姿态误差角构成实际仪器坐标系的姿态误差角。

所述步骤4中，特殊位置是指使得三轴理论正交输出中有两个为0、另一个绝对值为最大的姿态位置，即保证三轴理论正交[g_x,g_y,g_z]输出为[0,0,±g]、[0,±g,0]和[±g,0,0]的各姿态角。

本发明与现有技术相比，具有以下优点：

正交补偿充分考虑了非正交性误差的形成机理，首次定义了三轴独立的3个姿态误差参数，综合构成9个姿态误差参数；通过多次虚拟正交坐标系的欧拉旋转，理清了非正交性误差形成过程；通过求解逆矩阵将测量结果正交解耦，减小非正交性及其误差，可获得高精度的姿态参数。

附图说明

图1是初始仪器坐标系到理想仪器坐标系的坐标旋转变换示意图。

图2是本发明中三个姿态传感器的虚拟正交坐标变换示意图，i=X、Y、Z。

具体实施方式

下面结合附图和实施例详细说明本发明的实施方式。

首先以北西天坐标系中加速度计传感器为例介绍姿态测量的基本知识。

在理想地理坐标系（北西天ONWS坐标系）中建立初始仪器坐标系（OXYZ坐标系）。两个坐标系初始重合，N轴沿当地子午线指北，W轴沿当地纬线指西，S轴沿当地地垂线指天，NW面为当地水平面，NS面为当地子午面。定义方位角A为仪器在水平面内从地理北向旋转到仪器轴线在水平面投影的角，倾斜角I为仪器轴线与重力矢量之间的夹角（锐角），工具面角T为从重力高边旋转到仪器高边的夹角。ONWS坐标系绕OS（OZ₀）轴旋转角A到OX₁Y₁Z₁，绕OY₁轴旋转角I到OX₂Y₂Z₂，再绕OZ₂轴旋转角T到OX₃Y₃Z₃，就得到当前理想的OXYZ仪器坐标系，如图1所示。

每一次的旋转相当于一次坐标变换，可用相应的变换矩阵来表示，则相应的绕OZ₀轴、OY₁轴和OZ₂轴旋转矩阵为

R_{A} = | \begin{matrix} \cos A & \sin A & 0 \\ - \sin A & \cos A & 0 \\ 0 & 0 & 1 \end{matrix} |, R_{I} = | \begin{matrix} \cos I & 0 & - \sin I \\ 0 & 1 & 0 \\ \sin I & 0 & \cos I \end{matrix} |, R_{T} = | \begin{matrix} \cos T & \sin T & 0 \\ - \sin T & \cos T & 0 \\ 0 & 0 & 1 \end{matrix} | - - - (1)

则三轴传感器输出为

[\begin{matrix} g_{x} \\ g_{y} \\ g_{z} \end{matrix}] = R_{T} \cdot R_{I} \cdot R_{A} \cdot [\begin{matrix} g_{N} \\ g_{W} \\ g_{S} \end{matrix}] = R_{T} \cdot R_{I} \cdot R_{A} \cdot [\begin{matrix} 0 \\ 0 \\ - g \end{matrix}] - - - (2)

其中，g_x、g_y和g_z为传感器在XYZ轴线方向的输出。最终可得三轴传感器的输出为

\{\begin{matrix} g_{x} = g \sin I \cos T \\ g_{y} = - g \sin I \sin T \\ g_{z} = - g \cos I \end{matrix} - - - (3)

以上结果是在理想正交的情况下获得的，实际上由于三个轴的非正交性，导致了每个测量轴和理想的位置之间都存在误差。

本发明的方案是将以上理想的正交仪器坐标系OX₃Y₃Z₃再绕OZ₃轴旋转角A_X到OX'_XY_X'Z'_X，绕OY_X'轴旋转角I_X到OX''_XY_X''Z''_X，再绕OZ_X''轴旋转角T_X到OX_X'''Y_X'''Z'''_X，最后得到的一个虚拟正交仪器坐标系OX'''_XY_X'''Z'''_X，其中X_X'''轴构成实际测量仪器坐标系的X轴。

同样，将理想的正交仪器坐标系OX₃Y₃Z₃再绕OZ₃轴旋转角A_Y到OX'_YY_Y'Z'_Y，绕OY_Y'轴旋转角I_Y到OX_Y''Y_Y''Z_Y''，再绕OZ_Y''轴旋转角T_Y到OX_Y'''Y_Y'''Z_Y'''，最后得到的一个虚拟正交仪器坐标系OX_Y'''Y_Y'''Z_Y'''，其中Y_Y'''轴构成实际测量仪器坐标系的Y轴。再将理想的正交仪器坐标系OX₃Y₃Z₃绕OZ₃轴旋转角A_Z到OX'_ZY_Z'Z'_Z，绕OY_Z'轴旋转角I_Z到OX_Z''Y_Z''Z_Z''，再绕OZ_Z''轴旋转角T_Z到OX_Z'''Y_Z'''Z_Z'''，最后得到的一个虚拟正交仪器坐标系OX_Z'''Y_Z'''Z_Z'''，其中Z_Z'''轴构成实际的测量仪器坐标系的Z轴。

这样，理想的正交仪器坐标系OX₃Y₃Z₃分别经过独立的3组各3次欧拉旋转，得到3个虚拟正交的测量仪器坐标系，各自抽取其中的X_X'''轴、Y_Y'''轴和Z_Z'''轴构成实际测量的仪器坐标系。则三轴姿态测量传感器输出可表示为

[\begin{matrix} g_{xx} \\ g_{yy} \\ g_{zz} \end{matrix}] = R (A_{x}, I_{x}, T_{x}, A_{y}, I_{y}, T_{y}, A_{z}, I_{z}, T_{z}) \cdot [\begin{matrix} g_{x} \\ g_{y} \\ g_{z} \end{matrix}],

其中A_x,I_x,T_x,A_y,I_y,T_y,A_z,I_z,T_z分别为3次旋转的相应误差角，即方位误差角、倾斜误差角和工具面误差角。

最后，利用特殊位置的测量数据和理论数据，求得逆矩阵R^-1(A_x,I_x,T_x,A_y,I_y,T_y,A_z,I_z,T_z)，则任何姿态下的XYZ传感器理想输出可校正为

[\begin{matrix} g_{x} \\ g_{y} \\ g_{z} \end{matrix}] = R^{- 1} (A_{x}, I_{x}, T_{x}, A_{y}, I_{y}, T_{y}, A_{z}, I_{z}, T_{z},) \cdot [\begin{matrix} g_{xx} \\ g_{yy} \\ g_{zz} \end{matrix}],

再利用公式（3）即可得到实际仪器姿态参数。

本发明的原理为：

根据非正交性误差的形成机理，将非正交各轴姿态定义为初始正交的理想地理坐标系经正常测量欧拉旋转后的再次旋转变换，即认为正交的地理坐标系经过通常姿态旋转后得到一个虚拟、理想的正交仪器坐标系，再分别经过若干旋转，得到3个分别以各测量轴为实轴的虚拟正交仪器坐标系，最后分别抽取3个虚拟正交仪器坐标系中的实轴，构成一个实际非正交的测量仪器坐标系。

根据实际非正交仪器坐标系下传感器测量输出，及其与理想虚拟正交仪器坐标系的关系，得到一个补偿矩阵，对各轴传感器测量结果进行补偿校正，即对测量结果进行正交化解耦，以此得到消除了非正交性误差的测量数据。

下面再对本发明的步骤进行进一步的详细阐述。

首先，由正交的地理坐标系经过正常的3次欧拉旋转后得到一个初始虚拟正交仪器坐标系OX₃Y₃Z₃，即得到一组姿态角：方位角A、倾斜角I和工具面角T，则该虚拟正交仪器坐标系OX₃Y₃Z₃下各轴输出为

[\begin{matrix} g_{x 0} \\ g_{y 0} \\ g_{z 0} \end{matrix}] = R_{T} \cdot R_{I} \cdot R_{A} \cdot [\begin{matrix} g_{N} \\ g_{W} \\ g_{S} \end{matrix}] = R_{T} \cdot R_{I} \cdot R_{A} \cdot [\begin{matrix} 0 \\ 0 \\ - g \end{matrix}] = [\begin{matrix} g \cdot \sin I \cdot \cos T \\ - g \cdot \sin I \cdot \sin T \\ - g \cdot \cos I \end{matrix}] - - - (4)

然后，该初始的虚拟正交仪器坐标系OX₃Y₃Z₃再绕OZ₃轴旋转角A_x到OX'_XY_X'Z'_X，绕OY_X'轴旋转角I_x到OX_X''Y_X''Z_X''再绕OZ_X''轴旋转角T_x，最后得到一个虚拟正交仪器坐标系OX_X'''Y_X'''Z_X'''，旋转过程如图2所示（i=X），则该坐标系下测量输出为

[\begin{matrix} g_{xx 0} \\ g_{yx 0} \\ g_{zx 0} \end{matrix}] = R_{T_{x}} \cdot R_{I_{x}} \cdot R_{A_{x}} \cdot [\begin{matrix} g_{x 0} \\ g_{y 0} \\ g_{z 0} \end{matrix}] - - - (5)

该坐标系中输出g_xx0为实际的非正交仪器坐标系下X实轴的真实输出。

同样，初始的虚拟正交仪器坐标系OX₃Y₃Z₃绕OZ₃轴旋转角A_y到OX'_YY_Y'Z'_Y，绕OY_Y'轴旋转角I_y到OX_Y''Y_Y''Z_Y''，再绕OZ_Y''轴旋转角T_y，最后得到一个虚拟正交仪器坐标系OX_Y'''Y_Y'''Z_Y'''，旋转过程如图2所示（i=Y），则该坐标系下测量输出为

[\begin{matrix} g_{xy 0} \\ g_{yy 0} \\ g_{zy 0} \end{matrix}] = R_{T_{y}} \cdot R_{I_{y}} \cdot R_{A_{y}} \cdot [\begin{matrix} g_{y 0} \\ g_{y 0} \\ g_{z 0} \end{matrix}] - - - (6)

该坐标系中输出g_yy0为实际的非正交仪器坐标系下Y实轴的真实输出。

依次，初始的虚拟正交仪器坐标系OX₃Y₃Z₃绕OZ₃轴旋转角A_z到OX'_ZY_Z'Z'_Z，绕OY_Z'轴旋转角I_z到OX_Z''Y_Z''Z_Z''，再绕OZ_Z''轴旋转角T_z，最后得到一个虚拟正交仪器坐标系OX_Z'''Y_Z'''Z_Z'''，旋转过程如图2所示（i=Z），则该坐标系下测量输出为

[\begin{matrix} g_{xz 0} \\ g_{yz 0} \\ g_{zz 0} \end{matrix}] = R_{T_{z}} \cdot R_{I_{z}} \cdot R_{A_{z}} \cdot [\begin{matrix} g_{x 0} \\ g_{y 0} \\ g_{z 0} \end{matrix}] - - - (7)

该坐标系中输出g_zz0为实际的非正交坐标系下Z实轴的真实输出。

将公式（5）、（6）和（7）中的X、Y、Z实轴测量输出抽出，构成一个新的矩阵，可表示为

[\begin{matrix} g_{xx 0} \\ g_{yy 0} \\ g_{zz 0} \end{matrix}] = [\begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix}] \cdot [\begin{matrix} g_{x 0} \\ g_{y 0} \\ g_{z 0} \end{matrix}] = R \cdot [\begin{matrix} g_{x 0} \\ g_{y 0} \\ g_{z 0} \end{matrix}] - - - (8)

\begin{matrix} R = \\ [\begin{matrix} \cos A_{x} \cos I_{x} \cos T_{x} - \sin A_{x} \sin T_{x} & \sin A_{x} \cos I_{x} \cos T_{x} + \cos A_{x} si T_{x} & - \sin I_{x} \cos T_{x} \\ - \cos A_{y} \cos I_{y} \sin T_{y} - \sin A_{y} \cos T_{y} & - \sin A_{y} \cos I_{y} \sin T_{y} + \cos A_{y} c os T_{y} & \sin I_{y} \sin T_{y} \\ \cos A_{z} \sin I_{z} & \sin A_{z} \sin I_{z} & \cos I_{z} \end{matrix}] - - - (9) \end{matrix}

由于实际仪器坐标系的不正交性，虽然理想正交仪器坐标系中[g_x0,g_y0,g_z0]^T的三个量之间本身正交，而实际非正交仪器坐标系[g_xx0,g_yy0,g_zz0]^T中三个量之间是不正交的，可通过一些特殊位置的测量值和理论值获得相应的矩阵系数。

由公式（4）可知，在(I,T)为特殊角度(0°,k*90°)[k=0,1,2,3]或(180°,k*90°)[k=0,1,2,3]时，[g_x0,g_y0,g_z0]输出分别为[0,0,-g]或[0,0,g]，由此得到系数r₁₃、r₂₃和r₃₃。

在(I,T)为特殊角度(90°,90°)或(270°,270°)时，[g_x0,g_y0,g_z0]输出为[0,-g,0]，或者(I,T)为特殊角度(90°,270°)或(270°,90°)时，[g_x0,g_y0,g_z0]输出为[0,g,0]，由此可得到系数r₁₂、r₂₂和r₃₂。

在(I,T)为特殊角度(90°,180°)或(270°,0°)时，[g_x0,g_y0,g_z0]输出为[-g,0,0]，或者(I,T)为特殊角度(90°,0°)或(270°,180°)时，[g_x0,g_y0,g_z0]输出为[g,0,0]，由此可得到系数r₁₁、r₂₁和r₃₁。

以上9个系数r₁₁、r₂₁、r₃₁、r₁₂、r₂₂、r₃₂、r₁₃、r₂₃和r₃₃构成矩阵R，对R求得逆矩阵R^-1，这就是可将姿态测量系统中非正交性解耦的校正矩阵，正常姿态测量过程中就可通过校正矩阵得到任意姿态下三轴测量传感器的解耦、补偿校正结果。

[\begin{matrix} g_{x} \\ g_{y} \\ g_{z} \end{matrix}] = R^{- 1} \cdot [\begin{matrix} g_{xx} \\ g_{yy} \\ g_{zz} \end{matrix}] - - - (9)

应用该校正方法，可以实现姿态测量系统中非正交性误差的正交解耦和补偿，可以将测量结果正交化，减小、消除非正交性及其误差，提高姿态测量精确度，并可拓展应用于导弹、飞机、舰艇、水下等载体姿态测量中。

对于其他类型的姿态测量传感器，特殊位置的选择依然可以设法使其中的若干个量为0，或者若干量为极大值或极小值，其目的是既提高计算准确度，又减少计算工作量，也可以采用非零的所有量进行求解。

Claims

1.一种三轴姿态测量系统非正交性误差的正交补偿方法，包括如下步骤：

2.根据权利要求1所述的三轴姿态测量系统非正交性误差的正交补偿方法，其特征在于，所述三轴姿态测量传感器包括加速度计、陀螺仪、磁强计及MEMS惯性器件。

3.根据权利要求1所述的三轴姿态测量系统非正交性误差的正交补偿方法，其特征在于，所述地理坐标系包括北西天、北东地坐标系，其旋转正方向由右手定则或左手定则决定。

4.根据权利要求1所述的三轴姿态测量系统非正交性误差的正交补偿方法，其特征在于，所述步骤2中，每组坐标变换是指将所述虚拟正交仪器坐标系绕第一个轴旋转第一姿态误差角，再绕第二个轴旋转第二姿态误差角，再绕第三个轴旋转第三姿态误差角，最后得到一个新的虚拟正交仪器坐标系，3组坐标变换的旋转顺序相同，共得到三个新的虚拟正交仪器坐标系。

5.根据权利要求1所述的三轴姿态测量系统非正交性误差的正交补偿方法，其特征在于，所述步骤2中，从3个新的虚拟正交仪器坐标系中分别抽取X轴、Y轴和Z轴，构成实际并不完全正交的测量仪器坐标系，3组变换所形成的9个姿态误差角构成实际仪器坐标系的姿态误差角。

6.根据权利要求1所述的三轴姿态测量系统非正交性误差的正交补偿方法，其特征在于，所述步骤4中，特殊位置是指使得三轴理论正交输出中有两个为0、另一个绝对值为最大的姿态位置，即保证三轴理论正交[g_x,g_y,g_z]输出为[0,0,±g]、[0,±g,0]和[±g,0,0]的各姿态角。