基于PI自适应两级卡尔曼滤波的无人机姿态估计方法与流程

基于pi自适应两级卡尔曼滤波的无人机姿态估计方法
技术领域
1.本发明涉及无人机姿态估计技术领域，特别涉及一种基于pi自适应两级卡尔 曼滤波的无人机姿态估计方法。


背景技术：

2.四旋翼无人机越来越多的进入人们的生活当中，比起传统的直升机，四旋翼 无人机具有结构简单，体积较小，质量较轻，造价便宜，可全自主飞行控制和定 点悬停，垂直起降等优势。四旋翼能够节约资源，且便于携带，能进入更多的狭 小空间采集所需数据。所以近几年来，四旋翼无人机应用越来越广泛，如物流运 输、影视航拍、通讯中继、精准农业、环境保护、地理勘测、排弹作业等。尤其 在人工智能技术研究热潮中，四旋翼无人机技术的研究成果也越来越丰富，此外， 在疫情防控期间，无人机物流配送，以及无人机高空喊话等，给人民带来了很大 的便利。因而，四旋翼无人机的研究是现今发展的趋势之一。四旋翼无人机系统 是一种典型的非线性、欠驱动、强耦合系统，在实际飞行过程中易受到空气动力、 重力、陀螺效应等因素影响，还可能受到外界气流干扰，所以无人机的姿态估计 是热门研究问题，因为姿态估计的精准性对无人机的飞行控制至关重要，解算精 度的高低将直接影响无人机的控制稳定性与飞行安全性。一般情况下，常用捷联 惯性测量单元(inertial measurement unit,imu)测量无人机的姿态，其中，陀螺仪 用来测量角速率，长时间工作后由于累计误差会发生漂移，同时在受到载体振动 噪声、运动加速度和磁场环境等因素影响后将导致姿态角解算精度低、稳定性差。 加速度计的测量值不仅只有重力加速度还包括了运动加速度信息，飞行过程中产 生的高频误差会使加速度计的短时精度受到较大影响。磁力计的测量误差主要来 自观测噪声，且在实际飞行过程中磁场容易受到干扰，对测量造成影响。因此， 融合惯性传感器数据，提高姿态估计精度，实现更高精度的导航，是捷联惯性导 航的重要研究方向。
3.目前姿态估计方法主要有扩展卡尔曼滤波，梯度下降法滤波，自适应卡尔曼 滤波和互补滤波等。扩展卡尔曼滤波是固定的观测噪声协方差矩阵，在无人机实 际飞行过程中会导致滤波精度下降，姿态估计误差较大；梯度下降法易受磁场干 扰，自适应卡尔曼滤波和互补滤波计算繁琐，工程量大。
4.文献[1]提出了一种基于扩展卡尔曼滤波器的算法，结合加速度计与陀螺仪数 据进行姿态估计，虽然该算法对横滚角与俯仰角具有良好的估计，但仅结合加速 度数据，对偏航角的校正精度偏低。文献[2]通过梯度下降法计算姿态最优估计， 虽然利用变步长动量梯度下降的方法消除了陀螺仪累计误差，但当出现磁场干扰 时，不能够有效滤波，计算结果误差较大。文献[3]将互补滤波器与梯度下降法进 行融合，提高了解算精度，但只适用于无人机平稳运动的场合，如果无人机做加 速运动，且姿态角变化剧烈，该算法的解算精度会大幅下降。文献[4]设计了一种 四元数描述符滤波器(quaternion descriptor filter)，可以解决任意方向存在运动加 速度的情况。该算法以四元数和运动加速度的函数为状态向量构建状态方程，采 用加权最小二乘法对状态进行估计，在加速度剧烈变化的
情况下也有较高的精度， 但需要计算加权矩阵，计算过程繁琐。文献[5]结合互补滤波算法，提出了一种 改进的自适应增量滤波算法，把经过互补滤波后的数据增量作为卡尔曼滤波器观 测量，解算精度有所提高，但计算量较大，工程上较难实现。文献[6
‑
7]基于互补 滤波对姿态进行解算，计算量小，但算法中固定的滤波系数不能解决传感器噪声 对姿态估计结果的影响，导致姿态估计精度不高。
[0005]
参考文献
[0006]
[1]ghobadi,mostafa,singla,等.robust att
‑
itude estimation from uncertain observations of inertial sensors using covariance inflate
‑
d multiplicative extended kalman filter[j].ieee transactions on instrumentation&measurement,2018.
[0007]
[2]张帅,郑芳,李霞,等.基于变步长动量梯度下降法的姿态估计算法[j].电光与 制,2020,27(9)：66
‑
70,111.
[0008]
[3]wu j,zhou z,fourati h,et al.generalized linear quaternion complementary filter for attitude estimation from multise
‑
nsor observations:an optimization approach[j].ieee transactions on automation science a
‑
nd engineering,2019:1330
‑
1343.
[0009]
[4]makni a,kibangou a y,fourati h.data fusion
‑
based descriptor approach for attitude estimation under accelerated maneuv
‑
ers[j].asian journal of control, 2019(4).
[0010]
[5]刘字,杨晓辉,郭俊启,等.一种基于aikf的姿态测量算法[j].压电与声 光,2018,40(3):454
‑
459.
[0011]
[6]wu z,sun z,zhang w.a novel app
‑
roach forattitude estimationbased on mfms inertial sensors using nonlinear complem
‑
entary filters[j].ieee sensors journal,2016,16(10):3856
‑
386.
[0012]
[7]杜瑾,赵华超,郑哲,等.捷联惯导互补滤波姿态融合算法设计[j].传感技术学 报,2018,31(10):91
‑
96.


技术实现要素：

[0013]
本发明针对现有技术的缺陷，提供了一种基于pi自适应两级卡尔曼滤波的无 人机姿态估计方法。
[0014]
为了实现以上发明目的，本发明采取的技术方案如下：
[0015]
一种基于pi自适应两级卡尔曼滤波的无人机姿态估计方法，包括以下步骤：
[0016]
s1、建立导航坐标系与机体坐标系，并基于牛顿
‑
欧拉法建立了如下的非线性 数学模型：
[0017][0018]
[0019][0020][0021][0022][0023]
式中：x,y,z是导航坐标系下无人机的位置，p,q,r是机体坐标下的角速度，m 为机身质量，u1为四旋翼提供的总推力，i
x
,i
y
,i
z
和u2,u3，u4分别为绕机体坐标系 3个坐标轴的转动惯量和力矩。
[0024]
s2、以四选取四元数q＝(q0，q1，q2,q3)
t
作为状态向量，第一级滤波的观测变 量为重力加速度g＝(a
x
,a
y
，a
z
)
t
，由加速度计测得。第二级滤波的观测变量为磁 感应强度m＝(m
x
,m
y
,m
z
)
t
，由磁力计测得。状态方程与观测方程为：
[0025][0026]
式中：a为状态转移矩阵，为第一级滤波器的观测矩阵，为第二级滤 波器的观测矩阵。w
k
‑1为系统的过程噪声，v
k1
，分别为观测噪声，它们均服 从零均值高斯分布。
[0027]
s3、求解状态转移矩阵和观测矩阵。将状态方程用四元数微分形式表示为：
[0028][0029]
式中：
[0030][0031]
其中，ω
x
，ω
y
，ω
z
是陀螺仪测得的角速度。将状态方程从连续时间形式转换 为离散时间形式，通过差分近似代替微分求得：
[0032][0033]
进而得到状态转移矩阵为:
[0034]
[0035]
使用方向余弦矩阵结合加速度数据得到第一级滤波阶段观测矩阵：
[0036][0037]
其中，
[0038][0039]
式中：h1(q
k
)为系统第一级滤波观测方程，q
k
为k时刻的状态变量，为方 向余弦矩阵。h1(q
k
)是非线性的，需要通过扩展卡尔曼滤波将系统线性化，用雅 克比矩阵代替观测矩阵。雅克比矩阵为：
[0040][0041]
同理，第二级滤波阶段观测矩阵为：
[0042][0043]
线性化得
[0044][0045]
s4、先验估计：
[0046]
先验状态估计值是滤波的中间计算结果，即根据(k
‑
1)时刻的最优估计预测k 时刻的结果。计算步骤如下：
[0047]
s41、读取陀螺仪传感器数据，获得角速度ω
x
，ω
y
，ω
z
。
[0048]
s42、计算状态转移矩阵a
k
。
[0049]
s43、计算系统的先验状态估计：
[0050][0051]
s44、计算先验噪声协方差矩阵。为了计算卡尔曼增益k
k
，首先计算先验误 差协方差矩阵
[0052][0053]
式中：q
k
是过程噪声协方差矩阵，它表示陀螺仪噪声及陀螺仪其他误差源。
[0054]
s5、第一级滤波，步骤如下：
[0055]
s51、线性化h1(q
k
)计算雅可比矩阵h
k1
。
[0056]
s52、计算卡尔曼增益k
k1
。由于观测矩阵h1(q
k
)是非线性的，所以通过扩展 卡尔曼
滤波获得卡尔曼滤波增益k
k1
为：
[0057][0058]
式中：r
k1
是测量噪声协方差矩阵，表示加速度计噪声和加速度计的其他误差 源。v
k1
是关于非线性方程组h1(q
k
)的观测噪声v
k1
的偏导数的雅克比矩阵。
[0059]
s53、获取加速度数据，z
k1
＝g
[0060]
s54、计算第一级滤波阶段观测方程
[0061]
s55、计算校正因子。先验状态估计仅使用陀螺仪数据更新角位置的先验等 式，需要通过校正因子得到角位置的后验方程式，根据卡尔曼滤波理论，卡尔曼 滤波器的校正方程：
[0062][0063]
式中：z
k
代表实际测量值，在该算法中，第一级滤波使用加速度计数据，第 二级滤波使用磁力计数据。是从先验系统状态计算出的预期测量值。用卡尔 曼增益k
k
对残差加权，以计算系统状态的后验估计
[0064]
第一级滤波使用加速度计数据校正，校正因子为：
[0065][0066]
式中：z
k1
是加速度计测量值，是根据先验系统状态计算出的重力加速 度预测值，k
k1
是第一级滤波的卡尔曼增益。通过测量重力加速度，仅可以校正横 滚角和俯仰角。为了保证偏航角在校正中不受影响，将校正四元数q的第三矢量部 分设置为零。
[0067]
s56、计算后验状态估计。
[0068][0069]
s57、计算后验误差协方差矩阵。
[0070][0071]
s6、第二级滤波，步骤如下：
[0072]
s61、线性化h2(q
k
)计算雅可比矩阵h
k2
。
[0073]
s62、计算卡尔曼增益k
k2
。
[0074][0075]
式中：r
k2
是测量噪声协方差矩阵，表示磁力计噪声及其他误差源。v
k2
是观 测噪声v
k2
的雅克比矩阵。
[0076]
s63、读取加速度计数据，z
k2
＝m
[0077]
s64、计算第二级滤波阶段观测方程
[0078]
s65、计算校正因子。
[0079][0080]
q
∈2,1
＝0,q
∈2，2
＝0
[0081]
式中：z
k2
是磁力计测量值，是根据先验系统状态计算出的预期磁场强 度，
k
k2
是第二级滤波卡尔曼增益。为了使磁异常不影响横滚角和俯仰角的估计而 仅影响偏航角，令校正四元数q
∈2
的第一、二两个部分矢量为零。
[0082]
s66、计算后验状态估计。
[0083][0084]
s67、计算后验误差协方差矩阵。
[0085]
p
k
＝(i
‑
k
k2
h
k2
)p
k1
[0086]
s7、基于pi自适应的参数调节
[0087]
第k次迭代后的第一级滤波观测噪声协方差矩阵的估计值为：
[0088]
r
k1
＝(1 α
k
)r
k1
‑1[0089]
式中：α
k
为观测噪声协方差矩阵的调整系数。
[0090]
定义残差方差的理论值为：
[0091][0092]
定义残差方差的实际值为：
[0093][0094]
残差方差的理论值与实际值之差为：
[0095]
e
k
＝trace(d
k
)
‑
trace(c
k
)
[0096]
式中：trace()表示矩阵的迹。
[0097]
卡尔曼滤波器正常工作时，如果e
k
较大，则说明加速度计噪声特性发生了变 化，通过调节α
k
间接调节r
k1
来改变d
k
，使d
k
与c
k
保持一致，从而使e
k
维持在0 附近，达到实时修正观测噪声协方差矩阵r
k1
的目的。
[0098][0099]
式中：k
p
为比例系数，k
i
为积分系数。β为积分开关系数，决定是否使用积 分环节。采用积分分离式pi控制算法，计算α
k
。
[0100][0101]
式中：ε为开关阈值，当误差大于阈值时，不使用积分环节，防止饱和；当 误差小于阈值时，积分环节开始介入，消除余差。
[0102]
与现有技术相比，本发明的优点在于：
[0103]
将卡尔曼滤波器设计为两级滤波，可以以分离磁异常对横滚角和俯仰角估计 精度的影响，同时也减小了计算量。第一级滤波使用加速度计数据，通过pi自适 应算法实时修正观测噪声协方差矩阵，解决了由于加速度计噪声特性发生变化导 致滤波发散的问题。第二级滤波使用磁力计数据对姿态角进行校正，利用校正因 子修正了陀螺仪零位漂移引起的误差，提高了姿态估计的精度和鲁棒性。
附图说明
[0104]
图1是本发明实施例导航坐标系与机体坐标系图；
[0105]
图2是本发明实施例基于pi自适应两级卡尔曼滤波的无人机姿态估计方法流 程图；
[0106]
图3是本发明实施例静态实验结果图，其中(a)表示横滚角，(b)表示俯仰角， (c)表示偏航角；
[0107]
图4是本发明实施例仿真结果图，其中(a)表示横滚角，(b)表示俯仰角，(c) 表示偏航角；
[0108]
图5是本发明实施例三个姿态角的角度误差图，其中(a)表示横滚角误差，(b) 表示俯仰角误差，(c)表示偏航角误差。
具体实施方式
[0109]
为使本发明的目的、技术方案及优点更加清楚明白，以下根据附图并列举实 施例，对本发明做进一步详细说明。
[0110]
本实施例的姿态估计方法是基于牛顿
‑
欧拉法建立四旋翼无人机的非线性系 统模型，选取四元数q＝(q0,q1,q2,q3)
t
作为状态向量，将卡尔曼滤波器设计为两 级滤波，第一级滤波使用加速度计数据，通过pi自适应算法实时修正观测噪声协 方差矩阵。第二级滤波使用磁力计数据对姿态角进行校正。
[0111]
首先，建立导航坐标系与机体坐标系如图1所示。
[0112]
并基于牛顿
‑
欧拉法建立了如下所示的非线性数学模型：
[0113][0114][0115][0116][0117][0118][0119]
式中：x,y,z是导航坐标系下无人机的位置，p,q,r是机体坐标下的角速度，m 为机身质量，u1为四旋翼提供的总推力，i
x
,i
y
,i
z
和u2,u3，u4分别为绕机体坐标系 3个坐标轴的转动惯量和力矩。
[0120]
以四选取四元数q＝(q0，q1,q2,q3)
t
作为状态向量，第一级滤波的观测变量为 重力加速度g＝(a
x
,a
y
,a
z
)
t
，由加速度计测得。第二级滤波的观测变量为磁感应 强度m＝(m
x
,m
y
,
m
z
)
t
，由磁力计测得。状态方程与观测方程为：
[0121][0122]
式中：a为状态转移矩阵，为第一级滤波器的观测矩阵，为第二级滤 波器的观测矩阵。w
k
‑1为系统的过程噪声，v
k1
，分别为观测噪声，它们均服 从零均值高斯分布。
[0123]
为了后续滤波阶段，先求解状态转移矩阵和观测矩阵。将状态方程用四元数 微分形式表示为：
[0124][0125]
式中：
[0126][0127]
其中，ω
x
，ω
y
，ω
z
是陀螺仪测得的角速度。将状态方程从连续时间形式转换 为离散时间形式，通过差分近似代替微分求得：
[0128][0129]
进而得到状态转移矩阵为:
[0130][0131]
使用方向余弦矩阵结合加速度数据得到第一级滤波阶段观测矩阵：
[0132][0133]
其中，
[0134][0135]
式中：h1(q
k
)为系统第一级滤波观测方程，q
k
为k时刻的状态变量，为方 向余弦矩阵。h1(q
k
)是非线性的，需要通过扩展卡尔曼滤波将系统线性化，用雅 克比矩阵代替观测矩阵。雅克比矩阵为：
[0136][0137]
同理，第二级滤波阶段观测矩阵为：
[0138][0139]
线性化得
[0140][0141]
接下来是滤波器的设计，设计的基于pi自适应两级卡尔曼滤波的无人机姿态 估计方法，首先是将陀螺仪数据与加速度计数据融合进行第一级滤波，其中引入 pi控制算法实时修正观测噪声协方差矩阵。第一级滤波后的校正因子结合陀螺仪 与磁力计数据进行第二级滤波以得到更好的姿态角估计值，重复上述迭代过程直 到获得最优估计值，流程如图2所示。
[0142]
系统的先验估计：
[0143]
先验状态估计值是滤波的中间计算结果，即根据(k
‑
1)时刻的最优估计预测k 时刻的结果。计算步骤如下所示：
[0144]
1)读取陀螺仪传感器数据，获得角速度ω
x
，ω
y
，ω
z
。
[0145]
2)计算状态转移矩阵a
k
。
[0146]
3)计算系统的先验状态估计：
[0147][0148]
4)计算先验噪声协方差矩阵。为了计算卡尔曼增益k
k
，首先计算先验误差协 方差矩阵
[0149][0150]
式中：q
k
是过程噪声协方差矩阵，它表示陀螺仪噪声及陀螺仪其他误差源。
[0151]
(1)第一级滤波。
[0152]
第一级滤波是将陀螺仪数据与加速度计数据融合进行滤波，步骤如下：
[0153]
1)线性化h1(q
k
)计算雅可比矩阵h
k1
。
[0154]
2)计算卡尔曼增益k
k1
。由于观测矩阵h1(q
k
)是非线性的，所以通过扩展卡尔 曼滤波获得卡尔曼滤波增益k
k1
为：
[0155][0156]
式中：r
k1
是测量噪声协方差矩阵，表示加速度计噪声和加速度计的其他误差 源。v
k1
是关于非线性方程组h1(q
k
)的观测噪声v
k1
的偏导数的雅克比矩阵。
[0157]
3)获取加速度数据，z
k1
＝g
[0158]
4)计算第一级滤波阶段观测方程
[0159]
5)计算校正因子。先验状态估计仅使用陀螺仪数据更新角位置的先验等式， 需要通过校正因子得到角位置的后验方程式，根据卡尔曼滤波理论，卡尔曼滤波 器的校正方程：
[0160][0161]
式中：z
k
代表实际测量值，在该算法中，第一级滤波使用加速度计数据，第 二级滤波使用磁力计数据。是从先验系统状态计算出的预期测量值。用卡尔 曼增益k
k
对残差加权，以计算系统状态的后验估计
[0162]
在扩展卡尔曼两级滤波算法中，有两个校正方程式。第一级滤波使用加速度 计数据校正，校正因子为：
[0163][0164]
式中：z
k1
是加速度计测量值，是根据先验系统状态计算出的重力加速 度预测值，k
k1
是第一级滤波的卡尔曼增益。通过测量重力加速度，仅可以校正横 滚角和俯仰角。为了保证偏航角在校正中不受影响，将校正四元数q的第三矢量部 分设置为零。
[0165]
6)计算后验状态估计。
[0166][0167]
7)计算后验误差协方差矩阵。
[0168][0169]
(2)第二级滤波。
[0170]
第二级滤波是将陀螺仪数据与磁力计数据融合进行滤波，步骤如下：
[0171]
1)线性化h2(q
k
)计算雅可比矩阵h
k2
。
[0172]
2)计算卡尔曼增益k
k2
。
[0173][0174]
式中：r
k2
是测量噪声协方差矩阵，表示磁力计噪声及其他误差源。v
k2
是观 测噪声v
k2
的雅克比矩阵。
[0175]
3)读取加速度计数据，z
k2
＝m
[0176]
4)计算第二级滤波阶段观测方程
[0177]
5)计算校正因子。
[0178][0179]
q
∈2，1
＝0,q
∈2，2
＝0
[0180]
式中：z
k2
是磁力计测量值，是根据先验系统状态计算出的预期磁场强 度，k
k2
是第二级滤波卡尔曼增益。为了使磁异常不影响横滚角和俯仰角的估计而 仅影响偏航角，令校正四元数q
∈2
的第一、二两个部分矢量为零。
[0181]
6)计算后验状态估计。
[0182][0183]
7)计算后验误差协方差矩阵。
[0184]
p
k
＝(i
‑
k
k2
h
k2
)p
k1
[0185]
(3)基于pi自适应的参数调节
[0186]
第k次迭代后的第一级滤波观测噪声协方差矩阵的估计值为：
[0187]
r
k1
＝(1 α
k
)r
k1
‑1[0188]
式中：α
k
为观测噪声协方差矩阵的调整系数。
[0189]
定义残差方差的理论值为：
[0190][0191]
定义残差方差的实际值为：
[0192][0193]
残差方差的理论值与实际值之差为：
[0194]
e
k
＝trace(d
k
)
‑
trace(c
k
)
[0195]
式中：trace()表示矩阵的迹。
[0196]
卡尔曼滤波器正常工作时，如果e
k
较大，则说明加速度计噪声特性发生了变 化，通过调节α
k
间接调节r
k1
来改变d
k
，使d
k
与c
k
保持一致，从而使e
k
维持在0 附近，达到实时修正观测噪声协方差矩阵r
k1
的目的。
[0197][0198]
式中：k
p
为比例系数，k
i
为积分系数。β为积分开关系数，决定是否使用积 分环节。采用积分分离式pi控制算法，计算α
k
。
[0199][0200]
式中：ε为开关阈值，当误差大于阈值时，不使用积分环节，防止饱和；当 误差小于阈值时，积分环节开始介入，消除余差。
[0201]
为了验证所研究滤波方法的滤波效果，使用匿名拓空者pro飞控，搭建轴距 为450mm的四旋翼无人机实验平台。将滤波方法编程并写进飞控中，利用数传将 传感器的数据通过串口发送到上位机，采样时间为0.01s。将上位机数据导入到 matlab中处理，分别通过静态实验和动态实验分析横滚角φ、俯仰角θ和偏航 角ψ的滤波效果。
[0202]
(1)静态实验
[0203]
姿态角初始值为(0,0,0)，陀螺仪常值漂移为0.05，初始误差协方差矩阵p(0)＝ 10
‑4×
i4×4，过程噪声协方差矩阵q＝10
‑6×
i4×4,第一级滤波中加速度计测量噪 声协方差矩阵r1＝2
×
i3×3，第二级滤波中磁力计测量噪声协方差矩阵r2＝i3×3。 过程噪声和测量噪声均为高斯噪声，w
k
，v
k1
，v
k2
根据q，r1，r2随机赋值，实 验结果如图3所示，姿态误差数据如表1所示。
[0204]
表1静态时姿态误差数据
[0205][0206]
(2)动态实验验证
[0207]
为模拟无人机实际飞行过程姿态角变化，对无人机姿态角随机赋值，其他参 数与静态实验保持一致，将pi自适应两级卡尔曼滤波算法与扩展卡尔曼滤波算法 进行对比，仿真结果如图4、图5所示，图4分别为横滚角、俯仰角、偏航角的 解算数据。图5分别为三个姿态角的角度误差。
[0208]
从实验结果可以看出，pi自适应两级卡尔曼滤波算法与传统的扩展卡尔曼滤 波算法都能够跟随姿态角变化，但基于pi自适应两级卡尔曼滤波算法的姿态角解 算值比通过传统的扩展卡尔曼滤波算法得到的姿态角解算值更加接近真实值。即 使在姿态角快速变化时，所研究姿态估计方法也能使姿态角在较小误差范围内变 化，从而提高了解算精度。
[0209]
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解 本发明的实施方法，应被理解为本发明的保护范围并不局限于这样的特别陈述和 实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不 脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保 护范围内。