CN106289244A - 一种抗加速度干扰的姿态确定方法 - Google Patents

Abstract

本发明涉及设备精度评估领域，尤其涉及一种抗加速度干扰的姿态确定方法。本申请设计的一种抗加速度干扰的姿态确定方法通过建立线加速度模型和线加速度误差方程，以此来实时自适应设备的线加速度，确定设备的抗加速度干扰的姿态，与传统的姿态确定方法相比，提出的方法能够自适应在线实时估计载体线加速度。与传统的姿态确定方法相比，提出的姿态确定方法有效削弱了载体线加速度干扰，提高了姿态估计精度和稳定性。

Description

一种抗加速度干扰的姿态确定方法

技术领域

本发明涉及设备精度评估领域，尤其涉及一种抗加速度干扰的姿态确定方法。

背景技术

微小型无人机、云台或手持云台等需要测量和感知姿态的相关MEMS导航应用领域，都需要使用MEMS陀螺仪对姿态变化进行测量，但是MEMS陀螺仪在计算姿态过程中，由于器件误差的存在，积分得到姿态过程中会引入较大的计算误差，为此需要结合MEMS加速度计及MEMS磁强计对陀螺积分的姿态角进行修正。现有综合利用三种MEMS传感器进行姿态姿态确定的方法主要有互补滤波算法、卡尔曼滤波算法以及梯度下降法。当只利用MEMS陀螺仪和MEMS加速度计时，主要是利用MEMS加速度计在无线加速度情况下能够准确敏感重力加速度，从而获取载体姿态对MEMS陀螺仪积分得到的姿态进行修正，当载体有线加速度时，现有方法主要是通过减小MEMS加速度计计算的姿态对MEMS陀螺仪积分得到的姿态的修正力度，来减少线加速度的干扰。但是这种抗加速度干扰的方法，由于不能对线加速度干扰进行有效估计，因此抗干扰效果较差。针对这一问题，提出了一种抗加速度干扰的姿态确定方法，提高姿态确定方法的精度和稳定性。

发明内容

本发明的目的是提供一种抗加速度干扰的姿态确定方法，鉴于上述问题，它弥补了上述的缺陷并提供以下优点：

一种抗加速度干扰的姿态确定方法，应用于需要测量和感知姿态的设备中，其特征中，包括：

建立载体坐标系的线加速度估计模型；

建立包含所述载体坐标系的线加速度估计误差的状态方程；

建立以重力加速度矢量观测误差为量测量的量测方程；

利用所述线加速度估计误差的状态方程、所述以重力加速度矢量观测误差为量测量的量测方程和线加速度估计模型实时自适应估计线加速度，以此确定所述设备的抗加速度干扰的姿态。

上述的方法，其中，所述方法还包括：

将载体系下的线加速度变化近似为一阶马尔科夫过程。

上述的方法，其中，所述方法还包括：

选取载体姿态角估计误差、陀螺仪零偏估计误差和载体系下的线加速度估计误差作为滤波器的状态量，以此构建卡尔曼滤波状态误差向量。

上述的方法，其中，所述方法还包括：

将所述量测量选取由加速度计估计的重力加速度以及由陀螺仪估计的重力加速度的差值构成三维观测矢量。

上述的方法，其中，所述方法还包括：

自适应调整所述线加速度估计模型中的滤波衰减系数。

综上所述，本发明公开设计的一种抗加速度干扰的姿态确定方 法，与传统的姿态确定方法相比，提出的方法能够自适应在线实时估计载体线加速度。与传统的姿态确定方法相比，提出的姿态确定方法有效削弱了载体线加速度干扰，提高了姿态估计精度和稳定性。

附图说明

参考所附附图，以更加充分的描述本发明的实施例。然而，所附附图仅用于说明和阐述，并不构成对本发明范围的限制。

图1是本发明的流程图。

具体实施方式

下面结合附图和具体的实施例对本发明作进一步的说明，但是不作为本发明的限定。

如图1所示，本申请设计一种抗加速度干扰的姿态确定方法，具体的操作原理如下：

1、建立了载体坐标系线加速度估计模型；

当载体进行加减速机动或在振动环境下，载体存在线加速度，可以将载体系下的线加速度变化近似为一阶马尔科夫过程，如式(1)所示，其中为t_k时刻的载体系线加速度，为t_k-1时刻的载体系线加速度，上标B代表载体系。τ为一阶马尔科夫相关时间。在提出的方法中，c_a称为线加速度估计模型的衰减系数，从模型中可以看出c_a与τ有关。

2、建立了包含载体坐标系线加速度估计误差的状态方程；

选取载体姿态角估计误差(Δγ为横滚角误差，Δφ为俯仰角误差，为航向角误差)、陀螺仪零偏估计误差Δε_b＝[Δε_bx Δε_by Δε_bz]以及载体系下的线加速度估计误差 作为滤波器的状态量，构建的卡尔曼滤波状态误差向量为ΔX＝[Δθ Δε_b Δa^B]^T。建立的状态方程如式(2)所示，其中A_k+1/k＝0_9×9，状态估计的一步预测方程简写为式(3)，状态估值方程简写为式(4)。其中为误差状态量的先验估计值，为误差状态量的后验估计值，K_k为t_k时刻的卡尔曼滤波增益，Z_k为量测量，H_k为量测矩阵。

ΔX_k＝A_k/k-1ΔX_k-1+W_k-1 (2)

3、建立以重力加速度矢量观测误差为量测量的量测方程；

为了求解式(4)中的状态估值，需要构建量测量Z_k的模型。将量测量选取为由加速度计估计的重力加速度以及由陀螺仪估计的重力加速度的差值所构成的3维观测矢量，估计的重力加速度差值均投影在机体载体坐标系中，构建的量测方程如式(5)所示。

式中表示由加速度计估计的重力加速度在载体系的投影，其计算过程如式(6)所示，其中为t_k时刻的加速度计输出，为 t_k-1时刻载体系下的线加速后验估计值，为t_k时刻载体系下的线加速度先验估计值。

式(5)中是陀螺仪估计的重力加速度在载体系的投影，其计算过程如式(7)所示。其中为t_k时刻导航系下的重力加速度矢量，为t_k时刻方向余弦矩阵的先验估计值，为t_k-1时刻的方向余弦矩阵后验估计值，下标N代表导航系，为陀螺输出修正陀螺零偏后的先验估计值积分得到的姿态变化矩阵。

通过量测量的获取，经过进一步的推导，可以得到量测量与状态量之间的关系如式(8)所示，而式中量测H_k可表示为式(9)，T为卡尔曼滤波周期。

4、自适应调整线加速度估计模型中的滤波衰减系数；

当姿态确定方法应用于具体载体时，当载体做加减速运动或者是在振动环境中，可以通过自适应调整式(1)所示的线加速度估计模型中的衰减系数c_a进行线加速度干扰估计。c_a的自适应调节过程见式(10)所示，其中为平稳状态下的线加速度误差估值均方差， 为t_k时刻的线加速度误差估计值。

5、利用线加速度估计误差和线加速度估计模型，实时自适应估计线加速度；

依据式(3)～式(9)构建的状态及量测方程，对载体姿态角误差Δθ、陀螺仪零偏估计误差Δε_b以及载体系下的线加速度估计误差Δa^B的估计过程如式(11)所示。Q_k-1为t_k-1时刻的状态噪声矩阵，P_k/k-1为t_k时刻的状态协方差先验估计矩阵，P_k为t_k时刻的状态协方差后验估计矩阵，R_k为t_k时刻的量测噪声矩阵。

基于估计出来的状态误差量进一步对姿态角(主要是对姿态四元数进行修正估计)、陀螺零偏和载体系下的线加速度进行修正估计。姿态角四元数和陀螺零偏的修正估计过程如式(12)所示。其中为t_k时刻的姿态四元数后验估计值，为t_k-1时刻的姿态四元数后验估计值，为t_k时刻的姿态四元数先验估计值，为由t_k时刻估计的姿态角误差求解得到的姿态四元数增量，为陀螺输出修正零偏后的先验估计值积分得到的姿态四元数增量，式(7)中的可通过进行求解，可通过进行求解。为t_k时刻的陀螺仪输出，为t_k时刻的陀螺仪零偏先验估计值，为t_k-1时刻的陀螺仪零偏后验估计值，为t_k时刻的陀螺仪零偏误差估计值。

载体系下的线加速度修正估计过程如式(13)所示。式中为t_k时刻载体系下的线加速度后验估计值，为t_k时刻载体系下的线加速度先验估计值，为t_k时刻载体系下的线加速度估计误差，为t_k-1时刻载体系下的线加速度后验估计值，c_a为线加速度估计模型中的滤波衰减系数。

本申请的抗加速度干扰的姿态确定方法可以应用于无人机、云台及手持云台、穿戴式行人导航等。

综上所述，本申请设计的一种抗加速度干扰的姿态确定方法，与传统的姿态确定方法相比，提出的方法能够自适应在线实时估计载体线加速度。与传统的姿态确定方法相比，提出的姿态确定方法有效削弱了载体线加速度干扰，提高了姿态估计精度和稳定性。

通过说明和附图，给出了具体实施方式的特定结构的典型实施例，基于本发明精神，还可作其他的转换。尽管上述发明提出了现有的较佳实施例，然而，这些内容并不作为局限。

对于本领域的技术人员而言，阅读上述说明后，各种变化和修正 无疑将显而易见。因此，所附的权利要求书应看作是涵盖本发明的真实意图和范围的全部变化和修正。在权利要求书范围内任何和所有等价的范围与内容，都应认为仍属本发明的意图和范围内。

Claims (5)

1.一种抗加速度干扰的姿态确定方法，应用于需要测量和感知姿态的设备中，其特征在于，包括：

建立载体坐标系的线加速度估计模型；

建立包含所述载体坐标系的线加速度估计误差的状态方程；

建立以重力加速度矢量观测误差为量测量的量测方程；

利用所述线加速度估计误差的状态方程、所述以重力加速度矢量观测误差为量测量的量测方程和线加速度估计模型实时自适应估计线加速度，以此确定所述设备的抗加速度干扰的姿态。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：

将载体系下的线加速度变化近似为一阶马尔科夫过程。

3.根据权利要求1所述的方法，其特征在于，所述方法还包括：

选取载体姿态角估计误差、陀螺仪零偏估计误差和载体系下的线加速度估计误差作为滤波器的状态量，以此构建卡尔曼滤波状态误差向量。

4.根据权利要求1所述的方法，其特征在于，所述方法还包括：

将所述量测量选取由加速度计估计的重力加速度以及由陀螺仪估计的重力加速度的差值构成三维观测矢量。

5.根据权利要求1所述的方法，其特征在于，所述方法还包括：

自适应调整所述线加速度估计模型中的滤波衰减系数。