噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法

1.本发明涉及一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，属于无人机技术领域。


背景技术：

2.无人机是一种特殊的不载人飞行器，利用无线遥控设备或预设机载程序进行自主控制。随着微机电子的发展，以mems器件为核心的惯性测量系统成为无人机导航测量装置的主流应用，mems具有器件尺寸小、响应快、成本低廉等优势。基于mems传感器的imu得到广泛应用，如：无人机、无人驾驶汽车、轨迹跟踪、捷联惯性导航等具有广泛的工程应用市场。
3.四旋翼飞行器飞行的基础是精确的姿态解算，而mems传感器中陀螺仪会产生漂移现象；加速度计动态性能差，易受到外界震动影响导致产生较大噪声的测量值，使系统引入较大误差；磁力计易受到外部磁场信号干扰，对于周围环境比较敏感。上述三种误差的产生不能预料，是随机产生的，所以对四旋翼的姿态解算需要进行深入研究。


技术实现要素：

4.本发明的目的是提供一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，能够实时获取高精度姿态，进而更好的控制飞行器飞行状态。
5.为了实现上述目的，本发明采用的技术方案是：
6.一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，包括如下步骤：
7.s1：通过状态转移矩阵构建系统状态方程和观测方程，初始化系统噪声q和量测噪声初值
8.s2：将本时刻二次估计值与状态转移矩阵相乘得到下一时刻的一次预测状态
9.s3：计算新息，更新强跟踪滤波器计算过程的变量v
k+1
、m
k+1
、n
k+1
、然后计算次优渐消因子λ
k+1
，并将次优渐消因子拆分成两个开平方的形式；
10.s4：将步骤s3拆分形式的次优渐消因子分别乘在在一步预测协方差矩阵p
k+1|k
的两侧，得到改进的一步预测误差协方差矩阵p
k+1|k
；
11.s5：对下一时刻的噪声方差及噪声均值重新估计，并将估计值引入ekf公式中；
12.s6：计算卡尔曼增益矩阵k
k+1
、状态的二次估计和误差协方差p
k+1|k+1
的二次估计，判断是否循环结束，若循环结束转至步骤s7；若未结束则循环转回至步骤s2；
13.s7：输出实验结果。
14.本发明技术方案的进一步改进在于：所述步骤s1采用旋转矩阵进行构建状态方
程和观测方程，具体过程如下：
[0015][0016]
式(15)中，θ、φ、φ分别为俯仰角、偏航角、横滚角；
[0017][0018]
式(16)表示导航系相对于机体系的角速度在机体系的投影，表示的叉乘矩阵，从中提取出第三列可以得到：
[0019][0020]
其中，x为
[0021]bg,k+1
＝b
g,k
+w
g,k
(18)
[0022]
式(18)中，b
g,k
、w
g,k
分别表示陀螺仪k时刻的漂移和噪声，b
g,k+1
表示k+1时刻的陀螺仪漂移；
[0023]
定义系统状态模型：
[0024][0025]
为了在实际计算机上运行，将式(19)使用一阶差分，使函数离散化，得到系统状态方程：
[0026][0027]
式(20)中ta表示两次采样间隔时间，程序中表示积分时间。xk为k刻的状态值，ω
b,k
为陀螺仪k时刻测量值；
[0028]
对状态模型式(20)使用泰勒级数展开，得到ekf所需要的信息，则系统状态：
[0029][0030]
状态转移矩阵和噪声驱动矩阵分别为：
[0031][0032]
观测值z
k+1
为加速度计观测值可表示为：
[0033][0034]
系统的观测方程可表示：
[0035][0036]
观测矩阵为：
[0037]hk+1
＝[03×
3-gi3](25)
[0038]
本发明技术方案的进一步改进在于：所述步骤s3的具体过程为将不同时刻新息序列保持正交，并构建强跟踪滤波器，然后计算次优渐消因子λ
k+1
，并将次优渐消因子拆分成两个开平方的形式，将次优渐消因子的拆分式引入扩展卡尔曼滤波器中。
[0039]
本发明技术方案的进一步改进在于：所述步骤s5的具体过程为引入噪声自适应机制，以固定初值的噪声，实时动态调整噪声，来适应系统需要具，体公式步骤如下：
[0040][0041][0042][0043][0044][0045][0046][0047][0048][0049][0050][0051][0052][0053]
p
k+1|k+1
＝(i-k
k+1hk+1
)p
k+1|k
(14)
[0054]
上式中，分别为估计噪声方差、噪声均值，s
k+1
为新息，r0、r0分别为噪声方差和均值的初值随着k的增加，保证系统有一定的噪声初值,在迭代过程中不至于令卡尔曼增益k
k+1
过大或过小，对系统失去调节作用；v
k+1
、m
k+1
、n
k+1
为计算次优渐消因子的中间量，λ
k+1
为次优渐消因子，p
k+1|k
为一步预测协方差矩阵。
[0055]
由于采用了上述技术方案，本发明取得的技术效果有：
[0056]
本发明利用多传感器融合技术，通过旋转矩阵构建系统状态方程和观测方程，易于工程移植。
[0057]
本发明通过引入强跟踪滤波器，解决了传统扩展卡尔曼滤波器因泰勒展开近似线性化等问题导致的跟踪能力弱的问题，并通过改进次优渐消因子引入形式，保证了协方差矩阵计算结果的对称性，提高了系统跟踪速度。
[0058]
本发明针对惯性器件具有一定比例的固定噪声的特点，使用噪声自适应估计器，通过加入一定比例的初值噪声初值，同时根据新息变化情况，动态调整噪声均值和方差，解决了惯性器件容易受到环境干扰导致输出值误差较大的问题。
附图说明
[0059]
图1是本发明算法的框图；
[0060]
图2是本发明仿真条件下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器俯仰角(pitch)对比图；
[0061]
图3是本发明仿真条件下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器横滚角(roll)对比图；
[0062]
图4是本发明仿真条件下噪声自适应强跟踪扩展卡尔卡尔曼滤波器估计的系统量测噪声方差；
[0063]
图5是本发明实物下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器横滚角(roll)对比图；
[0064]
图6是本发明实物条件下噪声自适应强跟踪扩展卡尔曼滤波器估计的系统量测噪声方差。
具体实施方式
[0065]
下面结合附图及具体实施例对本发明做进一步详细说明：
[0066]
一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法，所述四旋翼包含单片机以及与单片机相连的陀螺仪、加速度计、地磁传感器，单片机与地面系统qgc地面站连接通讯；如图1所示，包括如下步骤：
[0067]
s1：通过状态转移矩阵构建系统状态方程和观测方程，初始化系统噪声q和量测噪声初值
[0068]
采用旋转矩阵进行构建状态方程和观测方程，具体过程如下：
[0069][0070]
式(15)中，θ、φ、φ分别为俯仰角、偏航角、横滚角；
[0071][0072]
式(16)表示导航系相对于机体系的角速度在机体系的投影，表示的叉乘矩阵，从中提取出第三列可以得到：
[0073][0074]
其中，x为
[0075]bg,k+1
＝b
g,k
+w
g,k
(18)
[0076]
式(18)中，b
g,k
、w
g,k
分别表示陀螺仪k时刻的漂移和噪声，将k+1时刻陀螺仪漂移b
g,k+1
也作为系统状态之一；
[0077]
定义系统状态模型：
[0078][0079]
为了在实际计算机上运行，将式(19)使用一阶差分，使函数离散化，得到系统状态方程：
[0080][0081]
式(20)中ts表示两次采样间隔时间，程序中表示积分时间。xk为k刻的状态值，ω
b,k
为陀螺仪k时刻测量值；
[0082]
对状态模型式(20)使用泰勒级数展开，得到ekf所需要的信息，则系统状态：
[0083][0084]
状态转移矩阵和噪声驱动矩阵分别为：
[0085][0086]
观测值z
k+1
为加速度计观测值可表示为：
[0087][0088]
系统的观测方程可表示：
[0089][0090]
观测矩阵为：
[0091]hk+1
＝[03×
3-gi3](25)
[0092]
s2：将本时刻二次估计值状态转移矩阵相乘得到下一时刻的一次预测状态
[0093]
s3：计算新息，更新强跟踪滤波器计算过程的变量v
k+1
、m
k+1
、n
k+1
、然后计算次优渐消因子λ
k+1
，并将次优渐消因子拆分成两个开平方的形式。
[0094]
具体过程为将不同时刻新息序列保持正交，并构建强跟踪滤波器，然后计算次优渐消因子λ
k+1
，并将次优渐消因子拆分成两个开平方的形式，将次优渐消因子的拆分式引入扩展卡尔曼滤波器中。
[0095]
s4：将步骤s3拆分形式的次优渐消因子分别乘在在一步预测协方差矩阵p
k+1|k
的两侧，得到改进的一步预测误差协方差矩阵p
k+1|k
；
[0096]
s5：对下一时刻的噪声方差及噪声均值重新估计，并将估计值引入ekf公式中。
[0097]
由于步骤s3引入的强跟踪滤波器无法判断单一传感器的准确性，若传感器易受到外界环境影响而引起的新息变化，强跟踪滤波器可能将误差一并引入到系统中，故步骤s5针对惯性传感器的特性，引入噪声自适应机制，以固定初值的噪声，实时动态调整噪声，来适应系统需要，具体公式步骤如下：
[0098][0099][0100][0101][0102][0103]
[0104][0105][0106][0107][0108][0109][0110][0111]
p
k+1|k+1
＝(i-k
k+1hk+1
)p
k+1|k
(14)
[0112]
上式中，分别为估计噪声方差、噪声均值，r0、r0分别为噪声方差和均值的初值随着k的增加，保证系统有一定的噪声初值,在迭代过程中不至于令卡尔曼增益k
k+1
过大或过小，对系统失去调节作用；v
k+1
、m
k+1
、n
k+1
为计算次优渐消因子的中间量，λ
k+1
为次优渐消因子，p
k+1|k
为一步预测协方差矩阵。
[0113]
s6：计算卡尔曼增益矩阵k
k+1
、状态的二次估计和误差协方差p
k+1|k+1
的二次估计，判断是否循环结束，若循环结束转至步骤s7；若未结束则循环转回至步骤s2。
[0114]
s7：输出实验结果。
[0115]
步骤s2、s4、s6利用扩展卡尔曼滤波器估算出最优姿态角，具体步骤如下：首先，利用步骤s2计算一步预测，通过上一时刻估计的状态值与本时刻状态转移矩阵相乘得到状态的一步骤预测，其次，通过步骤s4计算雅克比矩阵并计算改进的一步预测误差协方差矩阵p
k+1|k
，最后通过步骤s6计算卡尔曼增益矩阵k
k+1
，状态的二次估计，误差协方差p
k+1|k+1
的二次估计，为下一次迭代更新做准备。
[0116]
本发明的噪声自适应强跟踪扩展卡尔曼滤波器考虑到了惯性器件的噪声对系统的影响，强跟踪滤波器解决了扩展卡尔曼滤波器因泰勒展开，近似线性化的模型不准确，再加之历史值占比大，导致跟踪能力差的问题，同时针对惯性器件的特点对噪声自适应估计器进行动态估计，通过引入噪声初值，当系统在平稳状态向运动状态过渡时，卡尔曼增益能快速调节，不会因传感器噪声的影响将误差引入系统，提高姿态的解算精度。
[0117]
图2、图3分别是仿真环境下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器俯仰角(pitch)、横滚角(roll)对比图；可以明显看出，扩展卡尔曼滤波器跟踪速度最慢，状态产生突变后，很慢进入期望目标附近，强跟踪扩展卡尔曼滤波器，则较快的到达期望目标，但由于强跟踪滤波器的特点，惯性器件加速度计受外界环境影响较大，会将误差引入系统中，图中表现为跟踪速度快，但超调量较大，本技术提出的噪声自适应强跟踪扩展卡尔曼滤波器在跟踪速度上不低于强跟踪滤波器，但几乎无超调，平滑进入期望状态。
[0118]
图4是噪声自适应强跟踪扩展卡尔曼滤波器估计的噪声方差值，对比图3图2来看，
在系统状态有较大突变时刻，系统通过新息s
k+1
实时估计的量测方程也有较大变化，来适应系统噪声变化情况。
[0119]
图5是实物下扩展卡尔曼滤波器、强跟踪扩展卡尔曼滤波器、噪声自适应强跟踪扩展卡尔曼滤波器横滚角(roll)对比图，从图5中可以看出，实物环境下和仿真环境下具有一致结果。
[0120]
图6是实物条件下噪声自适应强跟踪扩展卡尔曼滤波器估计的系统量测噪声方差，对比图5，从图6中可以看出，在系统状态有较大变化时候，因为噪声自适应估计器的作用，使系统实时动态调节观测噪声，但与图4比较，其调节力度明显增大，这是因为仿真环境下传感器的噪声值是人为规定的高斯噪声，但实际情况中，在线实验噪声方差变化幅度更大，如图6噪声方差变化幅度达到250％，其原因是实物实验数据来源于真实传感器，以及更多环境干扰等因素的叠加，故在实物实验中有较大体现。整体在线实物实验与离线仿真实验具有一致结果。