CN109916399B - 一种阴影下的载体姿态估计方法 - Google Patents

Abstract

本发明公开了一种阴影下的载体姿态估计方法，包括以下步骤：一、建立载体坐标系；二、载体姿态初始参数的获取；三、基于退化四元数的观测器模型的建立并获取载体的姿态估计值。本发明方法步骤简单，设计合理且实现方便，建立基于退化四元数的观测器模型，将航向角与姿态角估计分离出来，在阴影无GPS信息情况下只利用三轴加速度计和三轴陀螺估计载体姿态角,并能在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计，在机动状态下切换到仅依赖三轴陀螺进行姿态估计,具有较高的姿态精度。

Description

一种阴影下的载体姿态估计方法

技术领域

本发明属于微惯性航姿技术领域，尤其是涉及一种阴影下的载体姿态估计方法。

背景技术

微机械陀螺(即MEMS陀螺)因具有成本低、体积小和便于批量生产等优点，在机器人、无人机等领域具有广泛的应用前景。然而，与激光陀螺和光纤陀螺相比，微机械陀螺存在较大的误差，在许多对精度要求相对较高的领域很难得到应用。

由于微机械陀螺单独工作的精度低，不能长时间提供稳定的姿态，一般需要与加速度计和GPS等辅助信息组合使用，通过姿态估计算法修正陀螺的漂移误差。典型的姿态估计算法大致分为两类：一类是从频率域来分辨和消除噪声，不需考虑信号的统计特性，如互补滤波器；另一类是使用状态空间法在时域内设计滤波器，如扩展卡尔曼滤波器和无迹卡尔曼滤波器等。姿态估计算法能够发挥各个惯性器件的优势，取长补短，提高系统精度的同时放宽了各个惯性器件的精度要求。一种典型的方法是采用加速度计和单基线GPS提供复杂观测信息，对陀螺误差实时校正。针对加速度计易受机动加速度的影响，在载体机动时会干扰加速度计对重力的测量。为了克服机动加速度对姿态估计的影响，一种可行的方法是利用GPS测量的加速度信息补偿加速度计非重力加速度的影响。但是GPS对空视要求较高，极易受到阴影遮挡或者干扰影响。上述无法得到应用于无GPS信号或者GPS信号受到干扰的情况。

发明内容

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种阴影下的载体姿态估计方法，其方法步骤简单，设计合理且实现方便，建立基于退化四元数的观测器模型，将航向角与姿态角估计分离出来，在阴影无GPS信息情况下只利用三轴加速度计和三轴陀螺估计载体姿态角,并能在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计，在机动状态下切换到仅依赖三轴陀螺进行姿态估计,具有较高的姿态精度，有效地应用于无GPS信号或者GPS信号受到干扰的情况，实用性强。

为解决上述技术问题，本发明采用的技术方案是：一种阴影下的载体姿态估计方法，该方法所采用的装置包括数据处理器、设置在载体上的三轴陀螺和三轴加速度计，其特征在于，该方法包括以下步骤：

步骤一、建立载体坐标系：

以载体的质心为原点o，ox_b轴与所述载体的纵轴重合且指向所述载体前进方向，oy_b轴与所述载体的横轴重合且指向所述载体前进方向的右侧，oz_b轴垂直于所述ox_b轴和所述oy_b轴形成的平面ox_by_b且指向所述载体下方，建立载体坐标系ox_by_bz_b；

步骤二、载体姿态初始参数的获取：

步骤201、三轴加速度计对载体初始时刻的总加速度进行检测，并将检测到的初始时刻总加速度发送至数据处理器，数据处理器根据公式和得到载体的初始俯仰角φ(0)和载体的初始横滚角θ(0)；g表示重力加速度，且g＝9.8m/s²；其中，f_x(0)表示三轴加速度计测量到的初始时刻总加速度在ox_b轴上的加速度分量，f_y(0)表示三轴加速度计测量到的初始时刻总加速度在oy_b轴上的加速度分量，f_z(0)表示三轴加速度计测量到的初始时刻总加速度在oz_b轴上的加速度分量；

步骤202、采用所述数据处理器根据公式得到初始的虚拟旋转矩阵

步骤203、采用所述数据处理器根据公式得到退化四元数中初始的标量参数q₀(0)、退化四元数中初始的第一矢量参数q₁(0)和退化四元数中初始的第二矢量参数q₂(0)；

步骤三、基于退化四元数的观测器模型的建立并获取载体的姿态估计值：

步骤301、j为自然数，即j≥0，当j≥1时，三轴陀螺对载体的角速度进行检测，并将检测到的角速度发送至数据处理器，采用所述数据处理器根据公式得到第j个采样时刻载体在ox_b轴上的角速度分量估计值采用所述数据处理器根据公式得到第j个采样时刻载体在oy_b轴上的角速度分量估计值采用所述数据处理器根据公式得到第j个采样时刻载体在oz_b轴上的角速度分量估计值其中，ω_x(j)表示三轴陀螺第j个采样时刻检测到的角速度在ox_b轴上的角速度分量，ω_y(j)表示三轴陀螺第j个采样时刻检测到的角速度在oy_b轴上的角速度分量，ω_z(j)表示三轴陀螺第j个采样时刻检测到的角速度在oz_b轴上的角速度分量，表示第j-1个采样时刻三轴陀螺在ox_b轴上的零偏估计值，表示第j-1个采样时刻三轴陀螺在oy_b轴上的零偏估计值，表示第j-1个采样时刻三轴陀螺在oz_b轴上的零偏估计值；

当j＝0时，三轴陀螺对载体的角速度进行检测，并将检测到的角速度发送至数据处理器，采用所述数据处理器根据公式得到初始时刻载体在ox_b轴上的角速度分量估计值采用所述数据处理器根据公式得到初始时刻载体在oy_b轴上的角速度分量估计值采用所述数据处理器根据公式得到初始时刻载体在oz_b轴上的角速度分量估计值其中，ω_x(0)表示三轴陀螺初始时刻检测到的角速度在ox_b轴上的角速度分量，ω_y(0)表示三轴陀螺初始时刻检测到的角速度在oy_b轴上的角速度分量，ω_z(0)表示三轴陀螺初始时刻检测到的角速度在oz_b轴上的角速度分量，表示三轴陀螺在ox_b轴上的初始零偏估计值，表示三轴陀螺在oy_b轴上的初始零偏估计值，表示三轴陀螺在oz_b轴上的初始零偏估计值；

步骤302、采用所述数据处理器根据公式得到第j个采样时刻载体的径向加速度判断门限τ_ω(j)；其中，ξ_ω(j)表示第j个采样时刻的载体径向加速度阈值；

采用所述数据处理器根据公式得到第j个采样时刻载体的切向加速度判断门限τ_a(j)；其中，ξ_a表示单位采样时刻内俯仰角的变化量阈值，f_x(j)表示第j个采样时刻三轴加速度计测量到的总加速度在ox_b轴上的加速度分量；

采用所述数据处理器综合和得到第j个采样时刻的综合判断条件其中，τ(j)表示第j个采样时刻的综合判断参数；

步骤303、采用所述数据处理器建立基于退化四元数的观测器模型并输入初始虚拟旋转矩阵和步骤302中第j个采样时刻的综合判断参数τ(j)，得到第j+1个采样时刻的虚拟旋转矩阵估计值其中，k_p表示观测器比例增益系数，且k_P＞0，k_I表示观测器积分增益系数，且k_I＞0，k_m表示观测量权值，k_m＞0，表示当地重力加速度归一化向量，且 表示三轴陀螺第j个采样时刻检测到的角速度分量向量，且 表示三轴陀螺第j个采样时刻的零偏估计向量，且 表示三轴陀螺第j个采样时刻在ox_b轴上的零偏估计值，表示三轴陀螺第j个采样时刻在oy_b轴上的零偏估计值，表示三轴陀螺第j个采样时刻在oz_b轴上的零偏估计值，表示第j个采样时刻的虚拟旋转矩阵估计值的转置，σ(j)表示第j个采样时刻的校正向量；S(·)表示斜对称矩阵,ΔT表示采样时间；

步骤303、采用所述数据处理器建立载体第j+1个采样时刻的虚拟旋转矩阵并带入步骤304中得到的第j+1个采样时刻的虚拟旋转矩阵估计值得到第j+1个采样时刻退化的四元数矩阵其中，其中，q₀(j+1)表示退化四元数中第j+1个采样时刻的标量参数，q₁(j+1)表示退化四元数中第j+1个采样时刻的第一矢量参数，q₂(j+1)表示退化四元数中第j+1个采样时刻的第二矢量参数；

步骤304、采用所述数据处理器根据公式得到第j+1个采样时刻载体的俯仰角估计值和载体的横滚角估计值

上述的一种阴影下的载体姿态估计方法，其特征在于：步骤301中三轴陀螺的初始零偏估计值的获取过程如下：

步骤3011、载体静止时，三轴陀螺对载体的角速度进行检测，并将检测到的角速度发送至数据处理器，数据处理器对三轴陀螺在ox_b轴上的多个角速度分量、三轴陀螺在oy_b轴上的多个角速度分量和三轴陀螺在oz_b轴上的多个角速度分量分别进行采集，并将数据处理器第i次采集到的三轴陀螺在ox_b轴上的角速度分量记作b′_x(i)，数据处理器第i次采集到的三轴陀螺在oy_b轴上的角速度分量记作b′_y(i)，数据处理器第i次采集到的三轴陀螺在oz_b轴上的角速度分量记作b′_z(i)；

步骤3012、采用所述数据处理器根据公式得到三轴陀螺在ox_b轴上的初始零偏估计值采用所述数据处理器根据公式得到三轴陀螺在oy_b轴上的初始零偏估计值采用所述数据处理器根据公式得到三轴陀螺在oz_b轴上的初始零偏估计值其中，N表示采集总次数，i和N均为正整数，且i的取值范围为1～N，N的取值范围为10～20。

上述的一种阴影下的载体姿态估计方法，其特征在于：所述三轴陀螺包括设置在所述载体坐标系中所述ox_b轴上且对载体在ox_b轴上的角速度分量进行检测的第一微机械陀螺、设置在所述载体坐标系中所述oy_b轴上且对载体在oy_b轴上的角速度分量进行检测的第二微机械陀螺和设置在所述载体坐标系中所述oz_b轴上且对载体在oz_b轴上的角速度分量进行检测的第三微机械陀螺，所述第一微机械陀螺、第二微机械陀螺和第三微机械陀螺的输出端均与数据处理器的输入端连接。

上述的一种阴影下的载体姿态估计方法，其特征在于：所述数据处理器为单片机、DSP微控制器或者ARM微控制器。

上述的一种阴影下的载体姿态估计方法，其特征在于：步骤302中第j个采样时刻的载体径向加速度阈值ξ_ω(j)取值为

上述的一种阴影下的载体姿态估计方法，其特征在于：步骤303中单位时间间隔内俯仰角的变化量阈值ξ_a取值为ω_max表示载体俯仰角的最大变化率，且ω_max的取值范围为2rad/秒～10rad/秒，t表示积分时间。

上述的一种阴影下的载体姿态估计方法，其特征在于：采样时间ΔT的取值范围为0.005秒～0.1秒。

上述的一种阴影下的载体姿态估计方法，其特征在于：步骤303中观测器比例增益系数k_p的取值范围为0＜k_P＜10，观测器积分增益系数k_I的取值范围为0＜k_I＜10，观测量权值k_m的取值范围为0＜k_m＜10。

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

1、所采用的阴影下的载体姿态估计方法步骤简单、实现方便且操作简便，确保载体在阴影无法获取GPS信息情况下，准确地得到载体的姿态，即载体的俯仰角估计值和载体的横滚角估计值。

2、所采用的阴影下的载体姿态估计方法操作简便且使用效果好，首先利用三轴加速度计得到载体的初始俯仰角和载体的初始横滚角，进而根据载体的初始俯仰角和载体的初始横滚角得到初始虚拟旋转矩阵和四元数初始三个参数；然后进行载体机动加速度的判断，以便于获取综合判断参数；最后建立基于退化四元数的观测器模型，并输入初始虚拟旋转矩阵和综合判断参数，实现在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计，在机动状态下切换到仅依赖三轴陀螺进行姿态估计，能有效地适应于载体的不同运动状态，准确度高。

3、所采用的载体机动加速度的判断过程中，一方面将载体的在oz_b轴上的角速度分量估计值与载体径向加速度阈值进行判断，以便于当载体的在oz_b轴上的角速度分量估计值大于载体径向加速度阈值时，使综合判断参数等于零，对基于退化四元数的观测器模型依赖三轴陀螺进行姿态估计；另一方面，当|arcsin(f_x(j))+arcsin(2q₀(j)q₂(j))|>ξ_a进行判断，以便于载体存在切向加速度时，使综合判断参数等于零，对基于退化四元数的观测器模型依赖三轴陀螺进行姿态估计；其他运动过程中，综合判断参数等于1，实现在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计。

4、所采用的通过基于退化四元数的观测器模型得到退化的四元数矩阵，就是将四元数矩阵中的第四个参数等于0状态下获取，从而能在无法获取GPS信息情况(即航向角的情况下)进行横滚角和俯仰角的获取。

综上所述，本发明步骤简单，设计合理且实现方便，建立基于退化四元数的观测器模型，将航向角与姿态角估计分离出来，在阴影无GPS信息情况下只利用三轴加速度计和三轴陀螺估计载体姿态角,并能在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计，在机动状态下切换到仅依赖三轴陀螺进行姿态估计,具有较高的姿态精度，有效地应用于无GPS信号或者GPS信号受到干扰的情况，实用性强。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明所采用装置的电路原理框图。

图2为本发明的方法流程框图。

图3为利用本发明方法获得的载体的姿态角估计值与利用高精度惯导获得姿态角测量值的曲线图。

附图标记说明:

1—数据处理器； 2—三轴加速度计； 3—三轴陀螺；

3-1—第一微机械陀螺； 3-2—第二微机械陀螺； 3-3—第三微机械陀螺。

具体实施方式

如图1和图2所示的一种阴影下的载体姿态估计方法，该方法所采用的装置包括数据处理器1、设置在载体上的三轴陀螺3和三轴加速度计2，该方法包括以下步骤：

步骤一、建立载体坐标系：

以载体的质心为原点o，ox_b轴与所述载体的纵轴重合且指向所述载体前进方向，oy_b轴与所述载体的横轴重合且指向所述载体前进方向的右侧，oz_b轴垂直于所述ox_b轴和所述oy_b轴形成的平面ox_by_b且指向所述载体下方，建立载体坐标系ox_by_bz_b；

步骤二、载体姿态初始参数的获取：

步骤201、三轴加速度计2对载体初始时刻的总加速度进行检测，并将检测到的初始时刻总加速度发送至数据处理器1，数据处理器1根据公式和得到载体的初始俯仰角φ(0)和载体的初始横滚角θ(0)；g表示重力加速度，且g＝9.8m/s²；其中，f_x(0)表示三轴加速度计2测量到的初始时刻总加速度在ox_b轴上的加速度分量，f_y(0)表示三轴加速度计2测量到的初始时刻总加速度在oy_b轴上的加速度分量，f_z(0)表示三轴加速度计2测量到的初始时刻总加速度在oz_b轴上的加速度分量；

步骤202、采用所述数据处理器1根据公式得到初始的虚拟旋转矩阵

步骤203、采用所述数据处理器1根据公式得到退化四元数中初始的标量参数q₀(0)、退化四元数中初始的第一矢量参数q₁(0)和退化四元数中初始的第二矢量参数q₂(0)；

步骤三、基于退化四元数的观测器模型的建立并获取载体的姿态估计值：

步骤301、j为自然数，即j≥0，当j≥1时，三轴陀螺3对载体的角速度进行检测，并将检测到的角速度发送至数据处理器1，采用所述数据处理器1根据公式得到第j个采样时刻载体在ox_b轴上的角速度分量估计值采用所述数据处理器1根据公式得到第j个采样时刻载体在oy_b轴上的角速度分量估计值采用所述数据处理器1根据公式得到第j个采样时刻载体在oz_b轴上的角速度分量估计值其中，ω_x(j)表示三轴陀螺3第j个采样时刻检测到的角速度在ox_b轴上的角速度分量，ω_y(j)表示三轴陀螺3第j个采样时刻检测到的角速度在oy_b轴上的角速度分量，ω_z(j)表示三轴陀螺3第j个采样时刻检测到的角速度在oz_b轴上的角速度分量，表示第j-1个采样时刻三轴陀螺3在ox_b轴上的零偏估计值，表示第j-1个采样时刻三轴陀螺3在oy_b轴上的零偏估计值，表示第j-1个采样时刻三轴陀螺3在oz_b轴上的零偏估计值；

当j＝0时，三轴陀螺3对载体的角速度进行检测，并将检测到的角速度发送至数据处理器1，采用所述数据处理器1根据公式得到初始时刻载体在ox_b轴上的角速度分量估计值采用所述数据处理器1根据公式得到初始时刻载体在oy_b轴上的角速度分量估计值采用所述数据处理器1根据公式得到初始时刻载体在oz_b轴上的角速度分量估计值其中，ω_x(0)表示三轴陀螺3初始时刻检测到的角速度在ox_b轴上的角速度分量，ω_y(0)表示三轴陀螺3初始时刻检测到的角速度在oy_b轴上的角速度分量，ω_z(0)表示三轴陀螺3初始时刻检测到的角速度在oz_b轴上的角速度分量，表示三轴陀螺3在ox_b轴上的初始零偏估计值，表示三轴陀螺3在oy_b轴上的初始零偏估计值，表示三轴陀螺3在oz_b轴上的初始零偏估计值；

步骤302、采用所述数据处理器1根据公式得到第j个采样时刻载体的径向加速度判断门限τ_ω(j)；其中，ξ_ω(j)表示第j个采样时刻的载体径向加速度阈值；

采用所述数据处理器1根据公式得到第j个采样时刻载体的切向加速度判断门限τ_a(j)；其中，ξ_a表示单位采样时刻内俯仰角的变化量阈值，f_x(j)表示第j个采样时刻三轴加速度计2测量到的总加速度在ox_b轴上的加速度分量；

采用所述数据处理器1综合和得到第j个采样时刻的综合判断条件其中，τ(j)表示第j个采样时刻的综合判断参数；

步骤303、采用所述数据处理器1建立基于退化四元数的观测器模型并输入初始虚拟旋转矩阵和步骤302中第j个采样时刻的综合判断参数τ(j)，得到第j+1个采样时刻的虚拟旋转矩阵估计值其中，k_p表示观测器比例增益系数，且k_P＞0，k_I表示观测器积分增益系数，且k_I＞0，k_m表示观测量权值，k_m＞0，表示当地重力加速度归一化向量，且 表示三轴陀螺3第j个采样时刻检测到的角速度分量向量，且 表示三轴陀螺3第j个采样时刻的零偏估计向量，且 表示三轴陀螺3第j个采样时刻在ox_b轴上的零偏估计值，表示三轴陀螺3第j个采样时刻在oy_b轴上的零偏估计值，表示三轴陀螺3第j个采样时刻在oz_b轴上的零偏估计值，表示第j个采样时刻的虚拟旋转矩阵估计值的转置，σ(j)表示第j个采样时刻的校正向量；S(·)表示斜对称矩阵,ΔT表示采样时间；

步骤303、采用所述数据处理器1建立载体第j+1个采样时刻的虚拟旋转矩阵并带入步骤304中得到的第j+1个采样时刻的虚拟旋转矩阵估计值得到第j+1个采样时刻退化的四元数矩阵其中，其中，q₀(j+1)表示退化四元数中第j+1个采样时刻的标量参数，q₁(j+1)表示退化四元数中第j+1个采样时刻的第一矢量参数，q₂(j+1)表示退化四元数中第j+1个采样时刻的第二矢量参数；

步骤304、采用所述数据处理器1根据公式得到第j+1个采样时刻载体的俯仰角估计值和载体的横滚角估计值

本实施例中，步骤301中三轴陀螺3的初始零偏估计值的获取过程如下：

步骤3011、载体静止时，三轴陀螺3对载体的角速度进行检测，并将检测到的角速度发送至数据处理器1，数据处理器1对三轴陀螺3在ox_b轴上的多个角速度分量、三轴陀螺3在oy_b轴上的多个角速度分量和三轴陀螺3在oz_b轴上的多个角速度分量分别进行采集，并将数据处理器1第i次采集到的三轴陀螺3在ox_b轴上的角速度分量记作b′_x(i)，数据处理器1第i次采集到的三轴陀螺3在oy_b轴上的角速度分量记作b′_y(i)，数据处理器1第i次采集到的三轴陀螺3在oz_b轴上的角速度分量记作b′_z(i)；

步骤3012、采用所述数据处理器1根据公式得到三轴陀螺3在ox_b轴上的初始零偏估计值采用所述数据处理器1根据公式得到三轴陀螺3在oy_b轴上的初始零偏估计值采用所述数据处理器1根据公式得到三轴陀螺3在oz_b轴上的初始零偏估计值其中，N表示采集总次数，i和N均为正整数，且i的取值范围为1～N，N的取值范围为10～20。

本实施例中，所述三轴陀螺3包括设置在所述载体坐标系中所述ox_b轴上且对载体在ox_b轴上的角速度分量进行检测的第一微机械陀螺3-1、设置在所述载体坐标系中所述oy_b轴上且对载体在oy_b轴上的角速度分量进行检测的第二微机械陀螺3-2和设置在所述载体坐标系中所述oz_b轴上且对载体在oz_b轴上的角速度分量进行检测的第三微机械陀螺3-3，所述第一微机械陀螺3-1、第二微机械陀螺3-2和第三微机械陀螺3-3的输出端均与数据处理器1的输入端连接。

本实施例中，所述数据处理器1为单片机、DSP微控制器或者ARM微控制器。

本实施例中，步骤302中第j个采样时刻的载体径向加速度阈值ξ_ω(j)取值为

本实施例中，步骤303中单位时间间隔内俯仰角的变化量阈值ξ_a取值为ω_max表示载体俯仰角的最大变化率，且ω_max的取值范围为2rad/秒～10rad/秒，t表示积分时间。

本实施例中，采样时间ΔT的取值范围为0.005秒～0.1秒。

本实施例中，步骤303中观测器比例增益系数k_p的取值范围为0＜k_P＜10，观测器积分增益系数k_I的取值范围为0＜k_I＜10，观测量权值k_m的取值范围为0＜k_m＜10。

本实施例中，实际使用时还建立，以载体的质心为原点o，过原点o且指向北为x_n轴，过原点o且指向东为y_n轴，过原点o且垂直向下指向地球中心为z_n轴，建立北东地坐标系ox_ny_nz_n。

本实施例中，需要说明的是，四元数中的第一矢量参数表示沿ox_n轴方向的旋转参数，四元数中的第二矢量参数表示沿oy_n轴方向的旋转参数，四元数中的第三矢量参数表示沿oz_n轴方向的旋转参数，退化四元数即将四元数中的第三矢量参数等于零，即退化四元数中的第一矢量参数表示沿ox_n轴方向的旋转参数，退化四元数中的第二矢量参数表示沿oy_n轴方向的旋转参数。

本实施例中，需要说明的是，虚拟旋转矩阵即在第三矢量参数恒等于零时得到的退化四元数所对应的旋转矩阵，该旋转矩阵是描述一个虚拟的载体的虚拟旋转载体，它与真实的载体有着相同的俯仰角和横滚角，但是却有着不同的航向角。

本实施例中，需要说明的是，当j＝0，又称为初始时刻。

本实施例中，需要说明的是，假设带入第j个采样时刻的虚拟旋转矩阵估计值和得到则假设带入k_p、τ(j)和σ(j)，得到则

本实施例中，所述第一微机械陀螺3-1、第二微机械陀螺3-2和第三微机械陀螺3-3均CRS03微机械陀螺。

本实施例中，进一步地，所述数据处理器1为STM32F407ARM微控制器，所述第一微机械陀螺3-1、第二微机械陀螺3-2、第三微机械陀螺3-3和三轴加速度计2的输出端均与数据处理器1的I/O端口连接。

本实施例中，进一步地，三轴加速度计2为ADXL355三轴加速度计。

本实施例中，需要说明的是，步骤303中观测器模型：当j＝0时，代入初始虚拟旋转矩阵以及各个初始时刻的值，得到第一个采样时刻的虚拟旋转矩阵估计值这样依次迭代，从而由上一个采样时刻的虚拟旋转矩阵估计值得到下一个采样时刻的虚拟旋转矩阵估计值，实现各个采样时刻的虚拟旋转矩阵估计值，从而能得到载体的各个采样时刻的姿态角。

本实施例中，如图3所示，利用本发明方法获得的载体的姿态俯仰角和横滚角(即图3的观测器估计值)与利用高精度惯导获得姿态角测量值(即图3的理论参考值)的曲线图，利用本发明方法获得的载体的横滚角的最大误差为0.78°，载利用本发明方法获得的载体的俯仰角最大误差为0.89°，满足载体姿态检测需求。

综上所述，本发明建立基于退化四元数的观测器模型，将航向角与姿态角估计分离出来，在阴影无GPS信息情况下只利用三轴加速度计和三轴陀螺估计载体姿态角,并能在低动态下通过融合三轴加速度计和三轴陀螺输出进行姿态估计，在机动状态下切换到仅依赖三轴陀螺进行姿态估计,具有较高的姿态精度，实用性强。

以上所述，仅是本发明的较佳实施例，并非对本发明作任何限制，凡是根据本发明技术实质对以上实施例所作的任何简单修改、变更以及等效结构变化，均仍属于本发明技术方案的保护范围内。

Claims (8)

1.一种阴影下的载体姿态估计方法，该方法所采用的装置包括数据处理器(1)、设置在载体上的三轴陀螺(3)和三轴加速度计(2)，其特征在于，该方法包括以下步骤：

步骤一、建立载体坐标系：

以载体的质心为原点o，ox_b轴与所述载体的纵轴重合且指向所述载体前进方向，oy_b轴与所述载体的横轴重合且指向所述载体前进方向的右侧，oz_b轴垂直于所述ox_b轴和所述oy_b轴形成的平面ox_by_b且指向所述载体下方，建立载体坐标系ox_by_bz_b；

步骤二、载体姿态初始参数的获取：

步骤201、三轴加速度计(2)对载体初始时刻的总加速度进行检测，并将检测到的初始时刻总加速度发送至数据处理器(1)，数据处理器(1)根据公式和得到载体的初始俯仰角φ(0)和载体的初始横滚角θ(0)；g表示重力加速度，且g＝9.8m/s²；其中，f_x(0)表示三轴加速度计(2)测量到的初始时刻总加速度在ox_b轴上的加速度分量，f_y(0)表示三轴加速度计(2)测量到的初始时刻总加速度在oy_b轴上的加速度分量，f_z(0)表示三轴加速度计(2)测量到的初始时刻总加速度在oz_b轴上的加速度分量；

步骤202、采用所述数据处理器(1)根据公式得到初始的虚拟旋转矩阵

步骤203、采用所述数据处理器(1)根据公式得到退化四元数中初始的标量参数q₀(0)、退化四元数中初始的第一矢量参数q₁(0)和退化四元数中初始的第二矢量参数q₂(0)；

步骤三、基于退化四元数的观测器模型的建立并获取载体的姿态估计值：

步骤301、j为自然数，即j≥0，当j≥1时，三轴陀螺(3)对载体的角速度进行检测，并将检测到的角速度发送至数据处理器(1)，采用所述数据处理器(1)根据公式得到第j个采样时刻载体在ox_b轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到第j个采样时刻载体在oy_b轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到第j个采样时刻载体在oz_b轴上的角速度分量估计值其中，ω_x(j)表示三轴陀螺(3)第j个采样时刻检测到的角速度在ox_b轴上的角速度分量，ω_y(j)表示三轴陀螺(3)第j个采样时刻检测到的角速度在oy_b轴上的角速度分量，ω_z(j)表示三轴陀螺(3)第j个采样时刻检测到的角速度在oz_b轴上的角速度分量，表示第j-1个采样时刻三轴陀螺(3)在ox_b轴上的零偏估计值，表示第j-1个采样时刻三轴陀螺(3)在oy_b轴上的零偏估计值，表示第j-1个采样时刻三轴陀螺(3)在oz_b轴上的零偏估计值；

当j＝0时，三轴陀螺(3)对载体的角速度进行检测，并将检测到的角速度发送至数据处理器(1)，采用所述数据处理器(1)根据公式得到初始时刻载体在ox_b轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到初始时刻载体在oy_b轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到初始时刻载体在oz_b轴上的角速度分量估计值其中，ω_x(0)表示三轴陀螺(3)初始时刻检测到的角速度在ox_b轴上的角速度分量，ω_y(0)表示三轴陀螺(3)初始时刻检测到的角速度在oy_b轴上的角速度分量，ω_z(0)表示三轴陀螺(3)初始时刻检测到的角速度在oz_b轴上的角速度分量，表示三轴陀螺(3)在ox_b轴上的初始零偏估计值，表示三轴陀螺(3)在oy_b轴上的初始零偏估计值，表示三轴陀螺(3)在oz_b轴上的初始零偏估计值；

步骤302、采用所述数据处理器(1)根据公式得到第j个采样时刻载体的径向加速度判断门限τ_ω(j)；其中，ξ_ω(j)表示第j个采样时刻的载体径向加速度阈值；

采用所述数据处理器(1)根据公式得到第j个采样时刻载体的切向加速度判断门限τ_a(j)；其中，ξ_a表示单位采样时刻内俯仰角的变化量阈值，f_x(j)表示第j个采样时刻三轴加速度计(2)测量到的总加速度在ox_b轴上的加速度分量；

采用所述数据处理器(1)综合和得到第j个采样时刻的综合判断条件其中，τ(j)表示第j个采样时刻的综合判断参数；

步骤303、采用所述数据处理器(1)建立基于退化四元数的观测器模型并输入初始虚拟旋转矩阵和步骤302中第j个采样时刻的综合判断参数τ(j)，得到第j+1个采样时刻的虚拟旋转矩阵估计值其中，k_p表示观测器比例增益系数，且k_P＞0，k_I表示观测器积分增益系数，且k_I＞0，k_m表示观测量权值，k_m＞0，表示当地重力加速度归一化向量，且 表示三轴陀螺(3)第j个采样时刻检测到的角速度分量向量，且 表示三轴陀螺(3)第j个采样时刻的零偏估计向量，且 表示三轴陀螺(3)第j个采样时刻在ox_b轴上的零偏估计值，表示三轴陀螺(3)第j个采样时刻在oy_b轴上的零偏估计值，表示三轴陀螺(3)第j个采样时刻在oz_b轴上的零偏估计值，表示第j个采样时刻的虚拟旋转矩阵估计值的转置，σ(j)表示第j个采样时刻的校正向量；S(·)表示斜对称矩阵,ΔT表示采样时间；

步骤303、采用所述数据处理器(1)建立载体第j+1个采样时刻的虚拟旋转矩阵并带入步骤304中得到的第j+1个采样时刻的虚拟旋转矩阵估计值得到第j+1个采样时刻退化的四元数矩阵其中，其中，q₀(j+1)表示退化四元数中第j+1个采样时刻的标量参数，q₁(j+1)表示退化四元数中第j+1个采样时刻的第一矢量参数，q₂(j+1)表示退化四元数中第j+1个采样时刻的第二矢量参数；

步骤304、采用所述数据处理器(1)根据公式得到第j+1个采样时刻载体的俯仰角估计值和载体的横滚角估计值

2.按照权利要求1所述的一种阴影下的载体姿态估计方法，其特征在于：步骤301中三轴陀螺(3)的初始零偏估计值的获取过程如下：

步骤3011、载体静止时，三轴陀螺(3)对载体的角速度进行检测，并将检测到的角速度发送至数据处理器(1)，数据处理器(1)对三轴陀螺(3)在ox_b轴上的多个角速度分量、三轴陀螺(3)在oy_b轴上的多个角速度分量和三轴陀螺(3)在oz_b轴上的多个角速度分量分别进行采集，并将数据处理器(1)第i次采集到的三轴陀螺(3)在ox_b轴上的角速度分量记作b′_x(i)，数据处理器(1)第i次采集到的三轴陀螺(3)在oy_b轴上的角速度分量记作b′_y(i)，数据处理器(1)第i次采集到的三轴陀螺(3)在oz_b轴上的角速度分量记作b′_z(i)；

步骤3012、采用所述数据处理器(1)根据公式得到三轴陀螺(3)在ox_b轴上的初始零偏估计值采用所述数据处理器(1)根据公式得到三轴陀螺(3)在oy_b轴上的初始零偏估计值采用所述数据处理器(1)根据公式得到三轴陀螺(3)在oz_b轴上的初始零偏估计值其中，N表示采集总次数，i和N均为正整数，且i的取值范围为1～N，N的取值范围为10～20。

3.按照权利要求1所述的一种阴影下的载体姿态估计方法，其特征在于：所述三轴陀螺(3)包括设置在所述载体坐标系中所述ox_b轴上且对载体在ox_b轴上的角速度分量进行检测的第一微机械陀螺(3-1)、设置在所述载体坐标系中所述oy_b轴上且对载体在oy_b轴上的角速度分量进行检测的第二微机械陀螺(3-2)和设置在所述载体坐标系中所述oz_b轴上且对载体在oz_b轴上的角速度分量进行检测的第三微机械陀螺(3-3)，所述第一微机械陀螺(3-1)、第二微机械陀螺(3-2)和第三微机械陀螺(3-3)的输出端均与数据处理器(1)的输入端连接。

4.按照权利要求1所述的一种阴影下的载体姿态估计方法，其特征在于：所述数据处理器(1)为单片机、DSP微控制器或者ARM微控制器。

5.按照权利要求1所述的一种阴影下的载体姿态估计方法，其特征在于：步骤302中第j个采样时刻的载体径向加速度阈值ξ_ω(j)取值为

6.按照权利要求1所述的一种阴影下的载体姿态估计方法，其特征在于：步骤303中单位时间间隔内俯仰角的变化量阈值ξ_a取值为ω_max表示载体俯仰角的最大变化率，且ω_max的取值范围为2rad/秒～10rad/秒，t表示积分时间。

7.按照权利要求1所述的一种阴影下的载体姿态估计方法，其特征在于：采样时间ΔT的取值范围为0.005秒～0.1秒。

8.按照权利要求1所述的一种阴影下的载体姿态估计方法，其特征在于：步骤303中观测器比例增益系数k_p的取值范围为0＜k_P＜10，观测器积分增益系数k_I的取值范围为0＜k_I＜10，观测量权值k_m的取值范围为0＜k_m＜10。