CN114166216B - 一种aru正置与倒置安装的水平姿态测量方法 - Google Patents

Abstract

本发明公开了一种ARU正置与倒置安装的水平姿态测量方法，包括以下步骤：1.对ARU姿态参考单元系统进行初始对准；2.以水平失准角和陀螺漂移作状态量初始化卡尔曼滤波器；3.递推k时刻的姿态四元数Q_k；4.计算修正前的k时刻的水平姿态角；5.卡尔曼滤波时间更新；6.选取加速度计输出f^p做量测量进行量测更新，利用更新后的状态量对修正前的k时刻的水平姿态角进行修正，并对其进行处理获得正确的水平姿态角。重复进行步骤3‑6就完成了ARU正置与倒置安装的水平姿态的测量。本发明解决了ARU正置与倒置安装情况下的水平姿态测量的问题。

Description

一种ARU正置与倒置安装的水平姿态测量方法

技术领域

本发明属于姿态测量技术领域，涉及一种ARU的水平姿态测量方法，特别是一种ARU正置与倒置安装的水平姿态测量方法。

背景技术

传统的惯性导航系统可以给出载体的位置、速度和姿态等信息，随着越来越多样化需求的产生，在某些场合，例如水面舰船以及陆地汽车，我们仅需要对载体或部件的水平姿态进行实时测量。ARU(姿态参考单元)就是一种基于捷联惯导技术的专门用于测量载体的水平姿态(纵摇角、横摇角)的设备。

目前，水平姿态测量的方法越来越多，例如在专利申请号为201911203231.1，名称为“基于运动参考单元的水面舰船水平姿态测量方法”的专利文件中，通过利用加速度计和陀螺仪输出进行运载体运动状态判断，根据不同的运动状态采用不同的方法计算失准角，然后利用失准角实时修正计算四元数得到准确的水平姿态。又如在专利申请号为202110428645.5，名称为“一种基于运动状态监测的自适应水平姿态测量方法”的专利文件中，通过建立新的状态空间模型，将水平姿态角作为状态变量，补偿随机常值零偏的角速度增量为系统方程的控制输入，补偿随机常值零偏的比力作为量测量，同时对载体机动状态的判断条件进行改善，综合利用MEMS-IMU输出的加速度信息和角速度信息对载体机动信息进行判断判断，实现对滤波器量测噪声阵的自动调整，有效降低了载体机动对水平姿态解算的影响。

目前的水平姿态测量方法大多是针对设备正向放置安装进行设计的，即设备内部的加速度计是沿载体坐标系安装的，并没有明确指出是否可以适用于设备倒置安装的情况。而在实际工程应用中，例如一些小型的AUV等，预留给设备的安装空间是有限的，其出线端(如数据电源线等)也是固定的，这就会导致设备有时并不能正向放置安装，而是倒置安装。当ARU倒置安装的时候，若继续采用ARU正向放置安装的程序，此时得到的水平姿态与载体的实际水平姿态是不符的。如果采用与倒置安装符合的程序，此时就可以得到与载体实际水平姿态相符合的姿态值。但是在实际应用中采用两套程序，会增加该设备的使用成本，因此研究一种能够在ARU正置与倒置安装情况下都可以测量水平姿态的方法是具有实际工程价值的。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种能够在ARU正置与倒置安装情况下都可以测量水平姿态的ARU正置与倒置安装的水平姿态测量方法。

为解决上述技术问题，本发明的一种ARU正置与倒置安装的水平姿态测量方法，包括：

步骤1：对ARU姿态参考单元系统进行初始对准得到初始水平姿态角，包括纵摇角θ₀和横摇角γ₀，并求解获得初始姿态四元数Q₀和初始捷联矩阵T₀；

步骤2：初始化卡尔曼滤波器，令X₀＝[φ₀ ε₀]^T，其中φ₀为初始水平失准角，φ₀＝[φ_E0,φ_N0]，φ_E0为东向初始失准角，φ_N0为北向初始失准角，ε₀为初始陀螺漂移，初始均方误差为P₀，初始化k＝1；

步骤3：利用k-1时刻的姿态四元数Q_k-1递推k时刻的姿态四元数Q_k；

步骤4：利用步骤3得到的Q_k求解k时刻的捷联矩阵T_k，并利用T_k反解求出修正前的k时刻的水平姿态角，包括纵摇角θ_k与横摇角γ_k；

步骤5：进行卡尔曼滤波时间更新；

步骤6：选取加速度计输出的转换到p坐标系下的f^p作为卡尔曼滤波量测量进行量测更新，实现卡尔曼滤波状态量的校正，利用校正后的卡尔曼滤波状态量对k时刻的捷联矩阵T_k进行修正从而得到修正后的k时刻的捷联矩阵T′_k，利用T′_k解出修正后的k时刻的水平姿态角，包括纵摇角θ′_k、横摇角γ′_k，再对θ′_k与γ′_k进行处理获得正确的水平姿态角，包括纵摇角θ_out、横摇角γ_out，利用正确的水平姿态角解出修正后的姿态四元数Q′_k；

步骤7：令k＝k+1，步骤3中k-1时刻的姿态四元数Q_k-1等于步骤6中得到的Q′_k，重复进行步骤3-7至导航工作状态结束。

进一步的，步骤6中对θ′_k与γ′_k进行处理获得正确的水平姿态角，包括纵摇角θ_out、横摇角γ_out具体为：

θ_out＝θ′_k

其中，代表算法判断此时ARU处于倒置安装，γ′_k∈(-90°，+90°)代表算法判断此时ARU处于正置安装，式中，θ′_k与γ′_k的表达式为：

其中，T′_ij为修正后的k时刻的捷联矩阵T′_k的第i行第j列元素(i，j＝1，2，3)。asin(·)代表·的反正弦值，atan2(·)代表·的反正切值。

本发明的有益效果：本发明提供了一种ARU正置与倒置安装的水平姿态测量方法，针对需要ARU设备倒置的情况对算法进行了改进，使得该算法在ARU设备正置和倒置安装情况下都可得到正确的水平姿态信息，有一定的工程应用价值。

附图说明

图1为本发明设备正置和倒置坐标系示意图；

图2为本发明的ARU正置与倒置安装的水平姿态测量方法流程图。

具体实施方式

下面结合说明书附图和具体实施例对本发明做进一步说明。

结合图2，本发明包括以下步骤：

步骤1.对ARU姿态参考单元系统进行初始对准得到初始水平姿态角(纵摇角θ₀、横摇角γ₀)并求解获得初始姿态四元数Q₀和初始捷联矩阵T₀。

步骤2.初始化卡尔曼滤波器，设置初始水平失准角φ_E0、φ_N0和陀螺漂移ε₀作为卡尔曼滤波状态量X₀＝[φ₀ ε₀]^T的初始值，初始均方误差为P₀。

步骤3.递推k时刻的姿态四元数Q_k。即利用k-1时刻的姿态四元数Q_k-1和k-1时刻到k时刻的角增量得到k时刻的姿态四元数Q_k为：

其中Δη_k-1为的模值；/>代表四元数乘法，cos(·)代表·的余弦值，sin(·)代表·的正弦值。

步骤4.计算修正前的k时刻的水平姿态角(纵摇角θ_k、横摇角γ_k)。利用步骤3中获得k时刻的姿态四元数Q_k求解k时刻的捷联矩阵T_k，并利用T_k反解求出修正前的k时刻的水平姿态角(纵摇角θ_k、横摇角γ_k)。

步骤5.进行卡尔曼滤波时间更新。其中相关方程的建立：

其中为k-1时刻状态最优估计，P_k-1为k-1时刻状态估计的均方误差阵，/>为k时刻状态一步预测，/>为k时刻状态一步转移矩阵，其中T₁是一行三列的行向量包含的三个元素为捷联矩阵T的第一行的三个元素，T₂是一行三列的行向量包含的三个元素为捷联矩阵T的第二行的三个元素，0_3×1是三行一列的行向量且元素全为0，β＝diag(1/τ₁1/τ₂1/τ₃)，其中τ_i(i＝1，2，3)为陀螺仪一阶马尔科夫过程随机误差相应的相关时间常数，P_k/k-1为k时刻状态一步预测均方误差阵，N_k-1为k-1时刻系统噪声方差阵。

步骤6.将加速度计输出f^b由载体坐标系b系转换到平台坐标系p系得到f^p。即f^p＝T·f^b，其中T为捷联矩阵。

步骤7.选取步骤6中的转换到p系的加速度计输出f^p做卡尔曼滤波量测量进行量测更新，实现卡尔曼滤波状态量的校正。利用校正后的卡尔曼滤波状态量对k时刻的捷联矩阵T_k进行修正从而得到修正后的k时刻的捷联矩阵T′_k。利用T′_k解出修正后的k时刻的水平姿态角(纵摇角θ′_k、横摇角γ′_k)，再对修正后的k时刻的水平姿态角进行处理获得正确的水平姿态角(纵摇角θ_out、横摇角γ_out)。利用正确的水平姿态角解出修正后的姿态四元数Q′_k。其中，具体的量测更新方程为：

其中为k时刻量测矩阵，g为重力加速度，R_k为k时刻量测噪声方差阵，K_k为k时刻滤波增益，/>为k时刻状态估计，Z_k为k时刻量测量，P_k为k时刻状态估计均方误差阵，I为单位矩阵。

对修正后的k时刻的水平姿态角(纵摇角θ_k′、横摇角γ′_k)进行处理获得正确的水平姿态角(纵摇角θ_out、横摇角γ_out)，即对反三角函数求出的水平姿态角主值θ_k′与γ′_k进行判断与处理，从而得到水平姿态角真值θ_out与γ_out，其具体表达式为：

θ_out＝θ′_k (5)

其中，θ_k′与γ′_k为修正后的k时刻的水平姿态角(水平姿态角主值)，θ_out与γ_out为正确的水平姿态角(水平姿态角真值)，代表算法判断此时ARU处于倒置安装，γ′_k∈(-90°，+90°)代表算法判断此时ARU处于正置安装。式中，θ′_k与γ′_k的表达式为：

其中，T′_ij为修正后的k时刻的捷联矩阵T′_k的第i行第j列元素(i，j＝1，2，3)；asin(·)代表·的反正弦值，atan2(·)代表·的反正切值。

步骤8.利用步骤7中求解的修正后的姿态四元数Q_k′作为步骤3中k-1时刻的姿态四元数Q_k-1，重复进行步骤3-8至导航工作状态结束，至此，就完成了ARU正置和倒置安装情况下的水平姿态测量。

为了验证所述方法的有益效果，针对ARU正置情况，本发明利用转台实验数据进行了仿真实验。通过与转台解码的水平姿态信息做差值得到水平姿态误差范围，其结果如表1所示；针对ARU倒置安装情况，本发明将ARU倒置转变为与其效果相同的部分程序添加在方法中进行仿真实验。在给定舰船水平姿态的情况下，观察ARU设备输出的水平姿态角；仿真结果如表2所示，在ARU设备倒置时其水平姿态经过处理后得到的ARU水平姿态输出与舰船实际水平姿态相等。

表1算法误差

表2 ARU设备倒置时其水平姿态输出与舰船实际水平姿态对比

Claims (2)

1.一种ARU正置与倒置安装的水平姿态测量方法，其特征在于，包括：

步骤1：对ARU姿态参考单元系统进行初始对准得到初始水平姿态角，包括纵摇角θ₀和横摇角γ₀，并求解获得初始姿态四元数Q₀和初始捷联矩阵T₀；

步骤2：初始化卡尔曼滤波器，令X₀＝[φ₀ ε₀]^T，其中φ₀为初始水平失准角，φ₀＝[φ_E0,φ_N0]，φ_E0为东向初始失准角，φ_N0为北向初始失准角，ε₀为初始陀螺漂移，初始均方误差为P₀，初始化k＝1；

步骤3：利用k-1时刻的姿态四元数Q_k-1递推k时刻的姿态四元数Q_k；

步骤4：利用步骤3得到的Q_k求解k时刻的捷联矩阵T_k，并利用T_k反解求出修正前的k时刻的水平姿态角，包括纵摇角θ_k与横摇角γ_k；

步骤5：进行卡尔曼滤波时间更新；

步骤6：选取加速度计输出的转换到p坐标系下的f^p作为卡尔曼滤波量测量进行量测更新，实现卡尔曼滤波状态量的校正，利用校正后的卡尔曼滤波状态量对k时刻的捷联矩阵T_k进行修正从而得到修正后的k时刻的捷联矩阵T′_k，利用T′_k解出修正后的k时刻的水平姿态角，包括纵摇角θ′_k、横摇角γ′_k，再对θ′_k与γ′_k进行处理获得正确的水平姿态角，包括纵摇角θ_out、横摇角γ_out，利用正确的水平姿态角解出修正后的姿态四元数Q′_k；

步骤7：令k＝k+1，步骤3中k-1时刻的姿态四元数Q_k-1等于步骤6中得到的Q′_k，重复进行步骤3-7至导航工作状态结束。

2.根据权利要求1所述的一种ARU正置与倒置安装的水平姿态测量方法，其特征在于：步骤6所述对θ′_k与γ′_k进行处理获得正确的水平姿态角，包括纵摇角θ_out、横摇角γ_out具体为：

θ_out＝θ′_k

其中，代表算法判断此时ARU处于倒置安装，γ′_k∈(-90°，+90°)代表算法判断此时ARU处于正置安装，式中，θ′_k与γ′_k的表达式为：

其中，T′_ij为修正后的k时刻的捷联矩阵T′_k的第i行第j列元素(i，j＝1，2，3),asin(·)代表·的反正弦值，atan2(·)代表·的反正切值。