CN106597017A - 一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置 - Google Patents

Abstract

本发明涉及基于扩展卡尔曼滤波的无人机角加速度估计方法及装置，方法包括：S1初始化，S2预测过程，S3校正过程。本发明提供的基于扩展卡尔曼滤波的角加速度估计方法，从含有噪声的低成本传感器中估计出角加速度状态量，并且针对实际工程应用中的传感器不完全测量的问题和如何提取方位角，提供了计算策略。此外，本发明提供的基于扩展卡尔曼滤波的角加速度估计方法不仅可以估计出角加速度，还可以将陀螺仪，磁罗盘和加速度计原始值进行滤波。

Description

一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置

技术领域

本发明涉无人机飞行控制技术领域，特别涉及一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置。

背景技术

无人机是一种由无线电遥控设备或自身程序控制装置操纵的无人驾驶飞行器。目前，随着科技的发展，无人机技术日趋成熟，无人机以其速度快、操作灵活的特点被广泛应用。民用领域中，地图测绘无人机、地质勘测无人机、灾害监测无人机、气象探测无人机、空中交通管制无人机、边境控制无人机、通信中继无人机、农药喷洒无人机、救援无人机的研究和应用在国内外都在不断的发展中。

多旋翼无人机又叫多轴飞行器，由三个或三个以上的独立动力系统来进行各种控制装置，结构简单，协调电机之间的转速即可实现控制，飞行器姿态保持能力较高，具有非常广阔的使用领域和研究价值。

在无人机飞行控制技术中，飞行器稳定姿态是飞行器稳定飞行的前提条件，获取飞行器姿态数据是必要的，飞行器在自主导航中需要用到加速度计、陀螺仪、磁罗盘和GPS等传感器，加速度计用于测量加速度，陀螺仪用于测量机体围绕某个轴向的旋转角速率值，磁罗盘用于确定飞行器的航向角，GPS用于测量飞行器的经纬度、高度、航迹方向、地速等信息。目前多旋翼无人机设计中，没有针对角加速度的测量装置，无法获取可靠的角加速度信息，然而角加速度信息对无人机飞行的稳定性以及抗干扰能力具有一定影响。

卡尔曼滤波，是一种高效率的递归滤波器，能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态，并使估计均方差最小。即使并不知道模型的确切性质，也可以估计信号的过去和当前状态，甚至能估计将来的状态。针对随机线性离散系统的状态或参数估计方法如下：

在用卡尔曼滤波之前需要建立被估计信号的过程模型：

x_k＝Ax_k-1+Bu_k-1+w_k-1 (1.1)

观测方程为

z_k＝Hx_k+v_k (1.2)

其中x_k表示k时刻的系统状态，u_k-1表示k时刻对系统的控制量，z_k表示k时刻的测量值，A、B和H为状态变换矩阵，是状态变换过程中的调整系数，此处为常数，矩阵B代表可选的控制输入u的增益，矩阵H表示状态变量x_k对测量变量z_k的增益。

(当控制函数u_k-1或过程激励噪声w_k-1为零时，模型中的增益矩阵A将上一时刻k-1的状态线性映射到当前时刻k的状态。实际中可能随时间变化，但在这儿假设为常数。矩阵B代表可选的控制输入u的增益。矩阵H表示状态变量x_k对测量变量z_k的增益。实际中H可能随时间变化，这里假设为常数。)

w_k-1和v_k分别表示过程激励噪声和观测噪声。假设它们为相互独立，正态分布的白色噪声：

p(w)～N(0,Q) (1.3)

p(v)～N(0,R) (1.4)

其中，Q为过程激励噪声协方差矩阵，R为观测噪声协方差矩阵，Q和R为常数。

(实际系统中，过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R会随每次迭代计算而变化。这里假设是常数。)

卡尔曼滤波器包括两个主要过程：预估与校正。预估过程主要是利用时间更新方程建立对当前状态的先验估计，及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计值；校正过程负责反馈，利用测量更新方程在预估过程的先验估计值及当前测量变量的基础上建立起对当前状态的改进的后验估计，这一过程为预估-校正过程，对应的这种估计算法为预估-校正算法。

如图1所示显示了滤波器的整个操作流程，并给出了离散卡尔曼滤波的时间更新方程和状态更新方程：

在上述图1中，各量说明如下：

A：作用在x_k-1上的n×n状态矩阵

B：作用在控制向量u_k-1上的n×1输入控制矩阵

H：m×n观测模型矩阵，把真实状态空间映射成观测空间

为n×n先验估计误差协方差矩阵

p_k:为n×n后验估计误差协方差矩阵

Q：n×n过程噪声协方差矩阵

R：m×m过程噪声协方差矩阵

I：n×n阶单位矩阵

Kk：n×m阶矩阵，称为卡尔曼增益或混合因数，作用是使后验估计误差协方差最小。

如果被估计的过程和(或)观测变量与状态变量的关系是非线性的，将状态转移矩阵线性化的卡尔曼滤波器称作扩展卡尔曼滤波器，简称EKF。面对非线性关系时，可以通过求过程方程和量测方程的偏导来线性化并计算当前估计量。其状态方程则变为非线性随机差分方程的形式。如图2所示为扩展卡尔曼滤波器的基本运行流程图，图中的各量说明可参考上述。

滤波器计算原型从建立的系统数学模型导出卡尔曼滤波的计算原型，包括时间更新方程和测量更新方程

定义(^-代表先验，^代表估计)为在第k步之前的状态已知的情况下第k步的先验状态估计值；

定义为测量变量z_k已知情况下第k步的后验状态估计值；

由此定义先验估计误差和后验估计误差：

先验估计误差的协方差矩阵为：

后验估计误差的协方差矩阵为：

则可以构造出卡尔曼滤波器的表达式：先验估计和加权的测量变量z_k及其预测之差的线性组合构成了后验状态估计

式中测量变量及其预测值之差反映了预测值和实际值之间的不一致程度，被称为测量过程的残余。n×m阶矩阵K叫做残余的增益或混合因数，它的作用是使1.6式中的后验估计误差协方差最小。其表达式如下(具体推导方式此处不做赘述):

对卡尔曼增益K的确定是建立滤波模型的关键步骤，能显著影响模型的效率。

由此可见，能否基于现有技术的不足并结合卡尔曼滤波器对非线性关系时计算当前估计量提供一种无人机角加速度估计方法，有效获取无人机角加速度信息，以提高无人机飞行稳定性以及抗干扰能力，成为本领域技术人员亟待解决的技术难题。

发明内容

本发明要解决的技术问题是，提供能够从含有噪声的低成本传感器中估计出角加速度状态量，获取无人机角加速度信息，有效应用于无人机的姿态控制和位置控制，提高了无人机的稳定性和抗干扰能力的基于扩展卡尔曼滤波的无人机角加速度估计方法。

解决上述技术问题，本发明提供了一种基于扩展卡尔曼滤波的无人机角加速度估计方法，包括如下步骤：

S1初始化，用以确定状态矢量和观测矢量；

S2预测过程，根据上述状态矢量和观测矢量转化得到非线性动力学模型；

基于非线性动力学模型进行初始估计，向前推算得到第一步的k+1先验状态估计：

向前推算出误差协方差：

其中，测量噪声矩阵Q_k保持变量σ²作为状态量对角项，用以作为滤波器的变量，

S3校正过程，建立观测方程，计算卡尔曼增益：

由观测变量z_k更新后验估计：

更新误差协方差：

继续步骤S2，进行迭代。

更进一步，方法还包括：对于第一次迭代的更新后验估计，初始值的选择如下：

第一次误差协方差矩阵初始值为P_k,init＝100I。

更进一步，观测矩阵H_k和噪声矩阵R_k根据某个新测量值获得而改变，当只能够获得陀螺仪测量值时观测变量：

更进一步，当只能获得陀螺仪和加速度计时观测变量：

更进一步，当只能获得陀螺仪和磁强计时观测变量：

进一步还包括将陀螺仪，磁罗盘和加速度计原始值进行滤波，

采用3*3的旋转矩阵表示方位，

Z轴方向上的基准矢量为：

Y轴方向上的基准矢量为：

X轴轴方向基准矢量为：

更进一步，S3校正过程中，观测系统可以用传统的线性函数来描述：

更进一步，确定状态矢量和观测矢量如下:

其中，Bω_IB＝[ω_x,ω_y,ω_z]^T为四旋翼角速率，为四旋翼角加速度的估计值，Br_g＝[Br_g,x,Br_g,y,Br_g,z]^T为地球重力场矢量，Br_m＝[Br_m,x,Br_m,y,Br_m,z]^T为地球磁场矢量，在实际中可以测量的量为：陀螺仪获取加速度计获取 磁强计获取

基于上述，本发明还提供了基于扩展卡尔曼滤波的无人机角加速度估计装置，包括：

初始化单元被配置为，进行初始化，用以确定状态矢量和观测矢量；

预测单元被配置为，完成预测过程，根据上述状态矢量和观测矢量转化得到非线性动力学模型，基于非线性动力学模型进行初始估计，向前推算得到第一步的k+1先验状态估计：

向前推算出误差协方差：

其中，测量噪声矩阵Q_k保持变量σ²作为状态量对角项，用以作为滤波器的变量，

校正单元被配置为，建立观测方程，计算卡尔曼增益：

由观测变量z_k更新后验估计：

更新误差协方差：

继续步骤S2，进行迭代。

本发明的有益效果：

1)本发明提供的基于扩展卡尔曼滤波的角加速度估计方法，从含有噪声的低成本传感器中估计出角加速度状态量，并且针对实际工程应用中的传感器不完全测量的问题和如何提取方位角，提供了计算策略。本发明提出的方法具有精度高、成本低、实时性好等优点。此外，该套估计方法涉及到了12阶的扩展卡尔曼运算，同时可以得出12个状态量，(包括，轴陀螺仪的角速度，三轴加速度计的加速度，三轴磁罗盘的磁通量，三轴角加速度量)具有很强的工程应用性，这些状态量可以应用于无人机的姿态控制和位置控制。

2)本发明提供的基于扩展卡尔曼滤波的角加速度估计方法不仅可以估计出角加速度，还可以将陀螺仪，磁罗盘和加速度计原始值进行滤波。

附图说明

图1为滤波器的整个操作流程；

图2为扩展卡尔曼滤波器的基本运行流程图；

图3为本发明的装置结构示意图；

图4为本发明中X轴方向的角加速度真值与估计值的对比图；

图5为本发明中Y轴方向的角加速度真值与估计值的对比图；

图6为本发明中Z轴方向的角加速度真值与估计值的对比图。

具体实施方式

现在将参考一些示例实施例描述本公开的原理。可以理解，这些实施例仅出于说明并且帮助本领域的技术人员理解和实施例本公开的目的而描述，而非建议对本公开的范围的任何限制。在此描述的本公开的内容可以以下文描述的方式之外的各种方式实施。

如本文中所述，术语“包括”及其各种变体可以被理解为开放式术语，其意味着“包括但不限于”。术语“基于”可以被理解为“至少部分地基于”。术语“一个实施例”可以被理解为“至少一个实施例”。术语“另一实施例”可以被理解为“至少一个其它实施例”。

发明原理：

本实施例中提供的基于扩展卡尔曼滤波的角加速度估计方法，不仅可以估计出角加速度，还能将陀螺仪，磁罗盘和加速度计原始值进行滤波，其中，陀螺仪、磁罗盘和加速度计的值通过传感器观测得到，而角加速度的信息通过估计得到。

在应用扩展卡尔曼滤波时，首先确定状态矢量和观测矢量如下:

其中，Bω_IB＝[ω_x,ω_y,ω_z]^T为四旋翼角速率，为四旋翼角加速度的估计值，Br_g＝[Br_g,x,Br_g,y,Br_g,z]^T为地球重力场矢量，Br_m＝[Br_m,x,Br_m,y,Br_m,z]^T为地球磁场矢量。

在实际中可以测量的量为：

陀螺仪获取

加速度计获取

磁强计获取

需要注意的是：这些可以获得量都是在本体坐标系下描述得到的。

1、预测过程

假设角加速度在时间间隔Δt内保持常数，下一时刻K+1角速率Bω_IB可以预测为同理，其他两个场矢量可以预测为r_k+1＝r_k+v_kΔt。

但是，矢量变化率Bv_a,k和Bv_m,k并不能直接测量得到。它们可以从惯性坐标系下速率和位置的转换关系中推导出来：

则

因为引力矢量和磁场矢量与矢量转化率无关，这是因为作用力相对于平移运动是不变的，仅仅需要考虑旋转项因此，非线性动力学模型如下：

则矩阵A_lin,k和W_lin,k，通过计算雅可比矩阵可得：

第一步的k+1先验状态估计为：

忽略了过程噪声W，且误差协方差为：

其中，测量噪声矩阵Q_k保持变量σ²作为状态量对角项。它代表预测的不确定性。由于该变量未知且不能测量，它被看作滤波器的变量,W代表过程噪声，X代表12个状态量。

2、校正过程

与状态预测不同，观测系统可以用传统的线性函数来描述：

计算卡尔曼增益：

则后验估计为：

且在校正过程中，误差协方差的传递关系式为：

对于第一次迭代，初始值的选择如下：

第一次误差协方差矩阵初始值为P_k,init＝100I。初始值的选择对误差协方差矩阵没有任何影响，因为其是收敛的，只要初始值不为零。请参考图4为本发明中X轴方向的角加速度真值与估计值的对比图；图5为本发明中Y轴方向的角加速度真值与估计值的对比图；图6为本发明中Z轴方向的角加速度真值与估计值的对比图。

由于三个传感器(陀螺仪，加速度计，磁强计)传递测量值是相互独立的，具有不同的采样率，则观测方程中的测量矢量不能全部获得。为了避免重复使用旧的测量值，观测矩阵H_k和噪声矩阵R_k根据某个新测量值获得而立即改变。通常陀螺仪具有最快的采样率，且其能够估计出角加速度一旦估计值能够获得，则校正步骤过程就会被计算。对于不完全测量，其矩阵如下：

当只能够获得陀螺仪测量值时：

当只能获得陀螺仪和加速度计时：

当只能获得陀螺仪和磁强计时：

当哪个传感器的测量值能够获得时，将校正哪个传感器。

在每一步卡尔曼迭代中，角速率Bω_IB和加速度都被估计出来。方位角也要提取出来。为了避免任何奇异值问题，用3*3的旋转矩阵A_BI(三个单位 矢量)表示方位。

这三个基准矢量是从估计的加速度矢量和估计的磁强计矢量推导得来。注意：使用了北东地惯性坐标系。

首先，z方向上的基准矢量为：

该估计特别是在悬停时特别精确。y方向上的基准矢量为：

而另外一个x轴方向基准矢量为：

图3为本发明的装置结构示意图，本实施例中基于扩展卡尔曼滤波的无人机角加速度估计装置，包括：

初始化单元1被配置为，进行初始化，用以确定状态矢量和观测矢量；

预测单元2被配置为，完成预测过程，根据上述状态矢量和观测矢量转化得到非线性动力学模型，基于非线性动力学模型进行初始估计，向前推算得到第一步的k+1先验状态估计：

向前推算出误差协方差：

其中，测量噪声矩阵Q_k保持变量σ²作为状态量对角项，用以作为滤波器的变量，

校正单元3被配置为，建立观测方程，计算卡尔曼增益：

由观测变量z_k更新后验估计：

更新误差协方差：

继续步骤S2，进行迭代。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

总体而言，本公开的各种实施例可以以硬件或专用电路、软件、逻辑或其任意组合实施。一些方面可以以硬件实施，而其它一些方面可以以固件或软件实施，该固件或软件可以由控制器、微处理器或其它计算设备执行。虽然本公开的各种方面被示出和描述为框图、流程图或使用其它一些绘图表示，但是可以理解本文描述的框、设备、系统、技术或方法可以以非限制性的方式以硬件、软件、固件、专用电路或逻辑、通用硬件或控制器或其它计算设备或其一些组合实施。

此外，虽然操作以特定顺序描述，但是这不应被理解为要求这类操作以所示的顺序执行或是以顺序序列执行，或是要求所有所示的操作被执行以实现期望结果。在一些情形下，多任务或并行处理可以是有利的。类似地，虽然若干具体实现方式的细节在上面的讨论中被包含，但是这些不应被解释为对本公开的范围的任何限制，而是特征的描述仅是针对具体实施例。在分离的一些实施 例中描述的某些特征也可以在单个实施例中组合地执行。相反对，在单个实施例中描述的各种特征也可以在多个实施例中分离地实施或是以任何合适的子组合的方式实施。

Claims (10)

1.一种基于扩展卡尔曼滤波的无人机角加速度估计方法，其特征在于，包括如下步骤：

S1初始化，用以确定状态矢量和观测矢量；

S2预测过程，根据上述状态矢量和观测矢量转化得到非线性动力学模型；

基于非线性动力学模型进行初始估计，向前推算得到第一步的k+1先验状态估计：

${\hat{X}}_{k+1}^{-}=f({\hat{X}}_k,0),$

向前推算出误差协方差：

其中，测量噪声矩阵Q_k保持变量σ²作为状态量对角项，用以作为滤波器的变量，

S3校正过程，建立观测方程，计算卡尔曼增益：

$K_k=P_k^{-}{H}_k^T{(H_k{P}_k^{-}{H}_k^T+R_k)}^{-1},$

由观测变量z_k更新后验估计：

${\hat{X}}_k={\hat{X}}_k^{-}+K_k(z_k-H_k{\hat{X}}_k^{-})$

更新误差协方差：

$P_k=(I-K_k{H}_k)P_k^{-}.,$

继续步骤S2，进行迭代。

2.根据权利要求1所述的无人机角加速度估计方法，其特征在于，还包括：对于第一次迭代的更新后验估计，初始值的选择如下：

${\hat{X}}_{k,init}={\left[\begin{array}{c}{\mathrm{B\ω}}_{IB}\\ B{\stackrel{\·}{\mathrm{\ω}}}_{IB}\\ {\mathrm{Br}}_a\\ {\mathrm{Br}}_m\end{array}\right]}_{k,init}={\left[\begin{array}{c}B{\stackrel{\‾}{\mathrm{\ω}}}_{IB}\\ 0\\ B{\stackrel{\‾}{r}}_g\\ B{\stackrel{\‾}{r}}_m\end{array}\right]}_{k,init}.$

第一次误差协方差矩阵初始值为P_k,init＝100I。

3.根据权利要求1所述的无人机角加速度估计方法，其特征在于，所述非线性动力学模型具体为：

$X_{k+1}^{-}=f(X_k,w)=\left(\begin{array}{c}{\mathrm{\ω}}_k+{\stackrel{\·}{\mathrm{\ω}}}_k\mathrm{\Δ}t+w_{\mathrm{\ω},k}\\ {\stackrel{\·}{\mathrm{\ω}}}_k+w_{\stackrel{\·}{\mathrm{\ω}},k}\\ r_{a,k}+{\widetilde{\mathrm{\ω}}}_k{r}_{a,k}\mathrm{\Δ}t+w_{r_{a,k}}\\ r_{m,k}+{\widetilde{\mathrm{\ω}}}_k{r}_{m,k}\mathrm{\Δ}t+w_{r_{m,k}}\end{array}\right)\mathrm{..}$

4.根据权利要求1所述的无人机角加速度估计方法，其特征在于，观测矩阵H_k和噪声矩阵R_k根据某个新测量值获得而改变，当只能够获得陀螺仪测量值时观测变量：

5.根据权利要求1所述的无人机角加速度估计方法，其特征在于，当只能获得陀螺仪和加速度计时观测变量：

6.根据权利要求1所述的无人机角加速度估计方法，其特征在于，当只能获得陀螺仪和磁强计时观测变量：

7.根据权利要求1所述的无人机角加速度估计方法，其特征在于，进一步还包括将陀螺仪，磁罗盘和加速度计原始值进行滤波，

采用3*3的旋转矩阵表示方位，

Z轴方向上的基准矢量为：

${\mathrm{Be}}_{I_z}=-\frac{{\mathrm{Br}}_a}{\left|\right|{\mathrm{Br}}_a|{|}_2}.$

Y轴方向上的基准矢量为：

${\mathrm{Be}}_{I_y}=\frac{{\mathrm{Be}}_{I_z}\×{\mathrm{Br}}_m}{\left|\right|{\mathrm{Be}}_{I_z}\×{\mathrm{Br}}_m|{|}_2}.$

X轴轴方向基准矢量为：

${\mathrm{Be}}_{I_x}=\frac{{\mathrm{Be}}_{I_y}\×{\mathrm{Be}}_{I_z}}{\left|\right|{\mathrm{Be}}_{I_y}\×{\mathrm{Be}}_{I_z}|{|}_2}\mathrm{..}$

8.根据权利要求1所述的无人机角加速度估计方法，其特征在于，在所述S3校正过程中，所述观测方程具体为：

9.根据权利要求1所述的无人机角加速度估计方法，其特征在于，确定状态矢量和观测矢量如下:

$X=\left[\begin{array}{c}{\mathrm{B\ω}}_{IB}\\ B{\stackrel{\·}{\mathrm{\ω}}}_{IB}\\ {\mathrm{Br}}_g\\ {\mathrm{Br}}_m\end{array}\right],Z=\left[\begin{array}{c}B{\stackrel{\‾}{\mathrm{\ω}}}_{IB}\\ B{\stackrel{\‾}{r}}_g\\ B{\stackrel{\‾}{r}}_m\end{array}\right]$

其中，Bω_IB＝[ω_x,ω_y,ω_z]^T为四旋翼角速率，为四旋翼角加速度的估计值，Br_g＝[Br_g,x,Br_g,y,Br_g,z]^T为地球重力场矢量，Br_m＝[Br_m,x,Br_m,y,Br_m,z]^T为地球磁场矢量，在实际中可以测量的量为：陀螺仪获取加速度计获取磁强计获取

10.基于扩展卡尔曼滤波的无人机角加速度估计装置，其特征在于，包括：

初始化单元被配置为，进行初始化，用以确定状态矢量和观测矢量；

预测单元被配置为，完成预测过程，根据上述状态矢量和观测矢量转化得到非线性动力学模型，基于非线性动力学模型进行初始估计，向前推算得到第一步的k+1先验状态估计：

${\hat{X}}_{k+1}^{-}=f({\hat{X}}_k,0),$

向前推算出误差协方差：

其中，测量噪声矩阵Q_k保持变量σ²作为状态量对角项，用以作为滤波器的变量，

校正单元被配置为，建立观测方程，计算卡尔曼增益：

$K_k=P_k^{-}{H}_k^T{(H_k{P}_k^{-}{H}_k^T+R_k)}^{-1},$

由观测变量z_k更新后验估计：

${\hat{X}}_k={\hat{X}}_k^{-}+K_k(z_k-H_k{\hat{X}}_k^{-})$

更新误差协方差：

$P_k=(I-K_k{H}_k)P_k^{-}.,$

继续步骤S2，进行迭代。