CN111025908B - 基于自适应机动加速度扩展卡尔曼滤波器的航姿参考系统 - Google Patents

Abstract

一种航姿参考系统，系统包括微控制器和N个惯性传感器，所述惯性传感器中的一个，记为第一惯性传感器位于飞行器上的标志点，其余的N‑1个惯性传感器均匀且对称位于以所述飞行器标志点为圆心的圆周上，N是正整数。所述参考系统还包括存储器和通信模块。微控制器被配置为执行存储在所述存储器中的指令，执行以下操作：

其中，X_k是第k个惯性传感器的输出值，X是N个惯性传感器输出值的平均值。

Description

基于自适应机动加速度扩展卡尔曼滤波器的航姿参考系统

技术领域

本发明属于飞行控制技术领域，特别涉及基于自适应机动加速度扩展卡尔曼滤波器的航姿参考系统。

背景技术

航姿参考系统(Attitude and heading reference system简称AHRS)用来为飞行器提供准确可靠的姿态与航行信息。MEMS惯性传感器以其体积小、能耗低、价格低的特点被广泛应用到了ARHS中，但其缺点是测量噪声较大。航姿参考系统通过解算惯性传感器的测量数据得到姿态与航行信息，通常来说有两种方法获得：

一、通过加速度计和磁力计数据直接计算获得姿态，但加速度计和磁力计的动态特性差，如果只用重力和磁场直接计算姿态作为输出姿态，则会有较大的偏差。

二、通过角速度积分更新获得姿态，但是角速度传感器的噪声因为姿态解算中的积分运算而被累积放大，即有累积误差，使得ARHS的误差迅速发散，导致系统精度降低。例如在静止情况下，通过初始对准获得初始姿态后使用角速度数值积分对姿态进行更新，会在几秒中内产生很大的误差，让姿态发生变化，而这种变化实际上是累积误差，所以必须要对姿态进行校正。

ARHS内部的校正方法一般是卡尔曼滤波器，结合上述两种获得姿态的方法，对姿态进行滤波校正。由于ARHS对姿态进行校正依赖于地球磁场和重力加速度，传统卡尔曼滤波器只能在目标低加速或者均速运动时对姿态进行校正，在系统高加速运动时无法进行滤波或者容易引起姿态较大的误差，也会导致系统精度降低。如果考虑机动加速度，在加速度计输出中减去机动加速度则可以避免传统方法的缺点。但是加速度计只能测得目标的合加速度而无法知道哪一部分是机动加速度，哪一部分是重力加速度。

发明内容

本发明实施例提供了一种航姿参考系统，目的在于解决现有的航姿参考系统在飞行器高速机动时，采用卡尔曼滤波器对于惯性传感器数据滤波效果不佳的问题。

本发明实施例之一，一种航姿参考系统，该系统包括微控制器和N个惯性传感器，以及存储器和通信模块。所述惯性传感器中的一个，记为第一惯性传感器位于飞行器上的标志点，其余的N-1个惯性传感器均匀且对称位于以所述飞行器标志点为圆心的圆周上，N是正整数。

微控制器被配置为执行存储在所述存储器中的指令，执行以下操作：

其中，X_k是第k个惯性传感器的输出值，X是N个惯性传感器输出值的平均值。

本发明提出一种自适应机动加速度的扩展卡尔曼滤波器，结合自适应技术和扩展卡尔曼滤波器，将机动加速度建模成系统状态变量，不断的估计机动加速度，然后在更新姿态时将机动加速度减去，可以在机动时进行姿态矫正滤波。

附图说明

通过参考附图阅读下文的详细描述，本发明示例性实施方式的上述以及其他目的、特征和优点将变得易于理解。在附图中，以示例性而非限制性的方式示出了本发明的若干实施方式，其中：

图1为根据本发明实施例之一的系统硬件框图。

图2为根据本发明实施例之一的惯性传感器安装示意图。

具体实施方式

本发明所要解决的技术问题是，对卡尔曼滤波技术进行扩展和融合，结合扩展卡尔曼滤波器和自适应技术，提供一种基于自适应机动加速度扩展卡尔曼滤波器的航姿参考系统，通过加速度计、陀螺仪以及磁力计的测量数据融合解算出参考系上的姿态角。该系统不断的估计机动加速度，然后在更新姿态时将机动加速度减去，可以在机动时进行姿态矫正滤波，避免机动加速度对系统造成不可滤波或者滤波发散的影响，并且通过多个MEMS传感器以特定安装方式结合相应的数据处理方法降低噪声的方差。最终，得到高精度的姿态与航行信息。

根据一个或者多个实施例，一种基于自适应机动加速度扩展卡尔曼滤波器的航姿参考系统，包括上位机、MEMS惯性传感器、微控制器、无线模块、存储模块、电源模块。MEMS惯性传感器、微控制器、无线模块、存储模块、电源模块构成的测量装置与上位机处在同一局域网下，上位机通过无线发送开始或停止命令控制测量装置进行测量，开始测量后，微控制器将MEMS惯性传感器的数据读出并存入存储器；停止测量后，上位机可通过无线或者串口将测量数据读出；读出测量数据后，上位机再对测量数据进行姿态解算和数据融合以获得参考系的三轴加速度和三轴姿态角。

根据一个或者多个实施例，一种自适应机动加速度扩展卡尔曼滤波器的姿态解算和数据融合方法，其特征在于，方法包括如下步骤：

1)用测量装置进行测量以获取加速度、角速度以及磁场信号；

2)测量装置平动时，多个传感器的测量真实值一致，而当转动时，多个传感器的测量真实值不一致，将多个传感器按照图2安装方式进行安装，使得多个传感器的均值等于中心处的真实值。假设中心处的传感器测得信号X1的真实值(加速度，角速度)为

在以中心为圆心的圆上四个以中心对称的点上测得信号X2、X3、X4和X5的真实值(加速度，角速度)为

和/>

则：

/>

每个传感器输出信号为真实值加噪声(真实值和噪声相互独立)，在一般情况下，每个传感器的噪声可视为均值为零，方差均为σ²的高斯白噪声。每个传感器输出相互独立，则X的均值为

方差为/>

n为0均值的噪声所以/>

保证了多个传感器取均值不会因为平动或转动的影响产生误差。

3)自适应机动加速度的扩展卡尔曼滤波器的数据融合解算；

取东北天为参考坐标系，右前上为载体坐标系。传感器输出的角速度和加速度模型为：

其中w为测量装置输出角速度，

为真实角速度，σw为角速度零偏误差，n1为角速度白噪声；a_b为测量装置输出加速度，g_b为载体系上的重力加速度，σa载体系上机动加速度(加速度零偏误差可视为被机动加速度所包含)，n2为加速度白噪声。

n1，n2对姿态的影响较小，在测量中可以忽略，一般认为对角速度积分产生误差的最大影响来自σw角速度零偏误差。而在用加速度直接估计姿态时，机动加速度产生最大误差影响。

系统的状态方程和量测方程为：

其中，f(·)和g(·)分别为系统的状态转移函数和状态观测函数，w(k)和v(k)为均值为零，协方差矩阵分别为Q(k)和R(k)的高斯白噪声，L(k)和M(k)为噪声分配矩阵。

扩展卡尔曼滤波器的基本公式如下：

X(k|k-1)＝f(X(k-1|k-1)，0)

X(k|k)＝X(k|k-1)+K(k)[Z(k)-h(X(k|k-1)，0)]

K(k)＝P(k|k-1)H(k)^T[H(k)P(k|k-1)H(k)^T+M(k)R(k)M(k)^T]^-1

P(k|k-1)＝A(k)P(k-1|k-1)A(k)^T+L(k)Q(k)L(k)^T

P(k|k)＝(I-K(k)H(k))P(k|k-1)

其中，X(k|k-1)和X(k|k)分别为k时刻系统状态的先验估计和最优估计；X(k-1|k-1)为k-1时刻系统状态的最优估计；K(k)为k时刻卡尔曼滤波增益；P(k|k-1)和P(k|k)分别为k时刻一步预测的误差方差矩阵和估计的误差方差矩阵；P(k-1|k-1)为k-1时刻估计的误差方差矩阵；Z(k)为k时刻观测向量；Q(k)与R(k)分别为过程噪声矩阵和观测噪声矩阵；h(X(k|k-1)，0)表示观测噪声为零时，关于先验估计X(k|k-1)的系统观测函数；f(X(k-1|k-1)，0)表示过程噪声为零时，关于k-1时刻的最优估计X(k-1|k-1)状态转移函数；A(k)和H(k)分别为f(·)和g(·)关于状态的一阶导数。

本发明实施例的有益效果包括，通过一种自适应卡尔曼滤波器，可以自适应机动加速度，避免了系统机动时的不滤波或者滤波发散的问题，并且通过多个MEMS传感器以特定安装方式结合相应的数据处理方法降低噪声的方差。最终，得到高精度的姿态与航行信息。

请参阅图1，图1为系统硬件结构框图，本发明的基于自适应机动加速度扩展卡尔曼滤波器的航姿参考系统包括9轴惯性传感器、微控制单元MCU、无线WIFI模块、串口模块、电源模块、存储模块。

图2为惯性传感器的安装示意图，5个9轴惯性传感器按照图2中标记1、2、3、4、5的位置安装在圆形底板上，标记1的位置在圆心，标记2、3、4、5位置在以标记1位置为圆心的圆上，且关于圆心对称。这种安装设置在技术上的目的在于使得通过均值降低噪声的方差而不影响真实值，例如平动时各点加速度一致，做圆周运动时，每点到圆心的距离不一样而导致各个传感器的向心加速度不一样。

在本实施例中，9轴传感器用来测量的3轴加速度、3轴角速度、3轴磁场强度，在本实例中优选MPU9250；传感器的采样频率优选1KHz，优选5个采样点取算术平均作为输出，即数据输出频率200Hz；MCU通过IIC或SPI接口控制和读取传感器测量信号并写入存储器中，在本实例中优选STM32F103系列单片机；存储模块用来保存数据以便离线读取，在本实例中优选W25QXX系列FLASH存储芯片；WIFI模块优选ESP8266系列模块。

在本实例中，上位机通过串口或者WIFI读取测量数据。

具体测量步骤为：

步骤1，提供所述的基于自适应机动加速度扩展卡尔曼滤波器的航姿参考系统装置；

步骤2，通过串口或者无线发送指令控制所述MCU读取所述传感器测量数据。

步骤3，通过所述装置的所述MCU计算5个9轴传感器的各轴测量数据的算术平均，并存入存储器中。

步骤4，通过所述装置的所述MCU将测量数据从存储器中读出，并通过串口或者无线WIFI发送给上位机。

步骤5，上位机对测量数据进行自适应机动加速度扩展卡尔曼滤波器姿态融合解算：

记q＝[q₀ q₁ q₂ q₃]^T为转动四元数表示载体坐标系(b系)相对于导航坐标系(n系)的转动。四元数转欧拉角公式：

其中

θ和φ分别为横滚、俯仰和航偏角。

由四元数微分方程：

其中wx，wy，wz为陀螺仪测得的三轴角速度减去陀螺仪零偏。

四元数微分方程的离散时间形式：

h1(q)与h2(q)为参考坐标系下重力与磁场强度经过旋转到q姿态的载体坐标系的函数。

将h1(q)，h2(q)对q求导得：

在本实例中作为优选，系统状态和观测向量分别为：

X(k)＝[q(k) σw(k) σa(k)]^T (9)

Z(k)＝[a_b(k) m_b(k)]^T (10)

其中q(k)为载体的姿态四元数，σw(k)为角速度零偏误差，σa(k)为机动加速度，a_b(k)为加速度计输出，m_b(k)为磁力计输出。

作为优选，系统的状态方程和观测方程的雅可比矩阵分别为：

其中T为采样间隔时间，I为单位矩阵，0为零矩阵。

作为优选，M(k)＝I_10×10，L(k)＝I_10×10。

(1)计算A(k)；

(2)计算k时刻预测X(k|k-1)＝f(X(k-1|k-1)，0)，即由于函数线性，所以X(k|k-1)＝A(k)X(k-1|k-1)；

(3)计算一步预测误差方差P(k|k-1)；

(4)计算雅可比矩阵H1(k)、H2(k)和H(k)；

(5)计算卡尔曼增益K(k)；

(6)计算k时刻期望观测h(X(k|k-1)，0)；

(7)计算X_∈＝K(k)[Z(k)-h(X(k|k-1)，0)]；

(8)计算k时刻最优估计X(k|k)＝X(k|k-1)+X_∈；

(9)从X(k|k)中提取q(k)，并计算欧拉角。

本发明实施例基于自适应机动加速度扩展卡尔曼滤波器的航姿参考系统，传统卡尔曼滤波器姿态融合没有考虑机动加速度的影响，一般在机动情况下不进行滤波处理，使得系统精度降低。基于自适应机动加速度的扩展卡尔曼滤波器的姿态融合方法比传统卡尔曼滤波器的姿态融合具有较强的鲁棒性和环境适应性，考虑了机动加速度的影响，在所有时刻都进行滤波处理，给出高精度的航向信息与姿态角。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (1)

1.一种航姿参考系统，其特征在于，该系统包括上位机、5个MEMS惯性传感器、微控制器、无线模块、存储模块、电源模块，

所述惯性传感器中的一个，记为第一惯性传感器位于飞行器上的中心点，其余的4个惯性传感器均匀且对称位于以所述飞行器中心点为圆心的圆周上，

所述参考系统还包括存储器和通信模块，微控制器被配置为执行存储在所述存储器中的指令，执行以下操作：

其中，X_k是第k个惯性传感器的输出值，X是5个惯性传感器输出值的平均值，

所述通信模块将5个惯性传感器输出值传输至上位机内，由上位机对所述5个惯性传感器输出值进行计算处理，

取东北天为参考坐标系，右前上为载体坐标系， 传感器输出的角速度和加速度模型为：

其中w为测量装置输出角速度，

为真实角速度，σ_w为角速度零偏误差，n1为角速度白噪声；a_b为测量装置输出加速度，g_b为载体系上的重力加速度，σa为载体坐标系上的机动加速度，n2为加速度白噪声，

系统的状态方程和量测方程为：

其中，f(·)和g(·)分别为系统的状态转移函数和状态观测函数，w(k)和v(k)为均值为零，协方差矩阵分别为Q(k)和R(k)的高斯白噪声，L(k)和M(k)为噪声分配矩阵，

对所述惯性传感器的输出值X进行扩展卡尔曼滤波计算如下，

X(k|k-1)＝f(X(k-1|k-1),0)

X(k|k)＝X(k|k-1)+K(k)[Z(k)-h(X(k|k-1),0)]

K(k)＝P(k|k-1)H(k)^T[H(k)P(k|k-1)H(k)^T+M(k)R(k)M(k)^T]^-1

P(k|k-1)＝A(k)P(k-1|k-1)A(k)^T+L(k)Q(k)L(k)^T

P(k|k)＝(I-K(k)H(k))P(k|k-1)

其中，

X(k|k-1)和X(k|k)分别为k时刻系统状态的先验估计和最优估计；

X(k-1|k-1)为k-1时刻系统状态的最优估计；

K(k)为k时刻卡尔曼滤波增益；

P(k|k-1)和P(k|k)分别为k时刻一步预测的误差方差矩阵和估计的误差方差矩阵；

P(k-1|k-1)为k-1时刻估计的误差方差矩阵；Z(k)为k时刻观测向量；

Q(k)与R(k)分别为过程噪声矩阵和观测噪声矩阵；

h(X(k|k-1),0)表示观测噪声为零时，关于先验估计X(k|k-1)的系统观测函数；

f(X(k-1|k-1),0)表示过程噪声为零时，关于k-1时刻的最优估计X(k-1|k-1)状态转移函数；

A(k)和H(k)分别为f(·)和g(·)关于状态的一阶导数，

当上位机获得N个惯性传感器输出值，进行自适应机动加速度扩展卡尔曼滤波器姿态融合解算：

记q＝[q₀ q₁ q₂ q₃]^T为转动四元数表示载体坐标系相对于导航坐标系的转动，四元数转欧拉角公式：

其中

θ和φ分别为横滚、俯仰和航偏角，

由四元数微分方程：

其中wx，wy，wz为陀螺仪测得的三轴角速度减去陀螺仪零偏，

四元数微分方程的离散时间形式：

h1(q)与h2(q)为参考坐标系下重力与磁场强度经过旋转到q姿态的载体坐标系的函数，

将h1(q)，h2(q)对q求导得：

系统状态和观测向量分别为：

X(k)＝[q(k) σw(k) σa(k)]^T (9)

Z(k)＝[a_b(k) m_b(k)]^T (10)

其中q(k)为载体的姿态四元数，σw(k)为角速度零偏误差，σa(k)为机动加速度，a_b(k)为加速度计输出，m_b(k)为磁力计输出，

系统的状态方程和观测方程的雅可比矩阵分别为：

其中T为采样间隔时间,I为单位矩阵，0为零矩阵，

令，M(k)＝I_10×10，L(k)＝I_10×10，

(1)计算A(k)；

(2)计算k时刻预测X(k|k-1)＝f(X(k-1|k-1),0)，即由于函数线性，所以X(k|k-1)＝A(k)X(k-1|k-1)；

(3)计算一步预测误差方差P(k|k-1)；

(4)计算雅可比矩阵H1(k)、H2(k)和H(k)；

(5)计算卡尔曼增益K(k)；

(6)计算k时刻期望观测h(X(k|k-1),0)；

(7)计算X∈＝K(k)[Z(k)-h(X(k|k-1),0)]；

(8)计算k时刻最优估计X(k|k)＝X(k|k-1)+X∈；

(9)从X(k|k)中提取q(k)，并计算欧拉角。

Images