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

技术特征：
1.一种基于pi自适应两级卡尔曼滤波的无人机姿态估计方法，其特征在于，包括以下步骤：s1、建立导航坐标系与机体坐标系，并基于牛顿
‑
欧拉法建立了如下的非线性数学模型：型：型：型：型：型：式中：x,y,z是导航坐标系下无人机的位置，p,q,r是机体坐标下的角速度，m为机身质量，u1为四旋翼提供的总推力，i
x
,i
y
,i
z
和u2,u3，u4分别为绕机体坐标系3个坐标轴的转动惯量和力矩；s2、以四选取四元数q＝(q0，q1，q2，q3)
t
作为状态向量，第一级滤波的观测变量为重力加速度g＝(a
x
,a
y
,a
z
)
t
，由加速度计测得；第二级滤波的观测变量为磁感应强度m＝(m
x
,m
y
,m
z
)
t
，由磁力计测得；状态方程与观测方程为：式中：a为状态转移矩阵，为第一级滤波器的观测矩阵，为第二级滤波器的观测矩阵；w
k
‑1为系统的过程噪声，分别为观测噪声，它们均服从零均值高斯分布；s3、求解状态转移矩阵和观测矩阵；将状态方程用四元数微分形式表示为：式中：
其中，ω
x
，ω
y
，ω
z
是陀螺仪测得的角速度；将状态方程从连续时间形式转换为离散时间形式，通过差分近似代替微分求得：进而得到状态转移矩阵为:使用方向余弦矩阵结合加速度数据得到第一级滤波阶段观测矩阵：其中，式中：h1(q
k
)为系统第一级滤波观测方程，q
k
为k时刻的状态变量，为方向余弦矩阵；h1(q
k
)是非线性的，需要通过扩展卡尔曼滤波将系统线性化，用雅克比矩阵代替观测矩阵；雅克比矩阵为：同理，第二级滤波阶段观测矩阵为：线性化得s4、先验估计：先验状态估计值是滤波的中间计算结果，即根据(k
‑
1)时刻的最优估计预测k时刻的结果；计算步骤如下：s41、读取陀螺仪传感器数据，获得角速度ω
x
，ω
y
，ω
z
；s42、计算状态转移矩阵a
k
；s43、计算系统的先验状态估计：
s44、计算先验噪声协方差矩阵；为了计算卡尔曼增益k
k
，首先计算先验误差协方差矩阵，首先计算先验误差协方差矩阵式中：q
k
是过程噪声协方差矩阵，它表示陀螺仪噪声及陀螺仪其他误差源；s5、第一级滤波，步骤如下：s51、线性化h1(q
k
)计算雅可比矩阵h
k1
；s52、计算卡尔曼增益k
k1
；由于观测矩阵h1(q
k
)是非线性的，所以通过扩展卡尔曼滤波获得卡尔曼滤波增益k
k1
为：式中：r
k1
是测量噪声协方差矩阵，表示加速度计噪声和加速度计的其他误差源；v
k1
是关于非线性方程组h1(q
k
)的观测噪声v
k1
的偏导数的雅克比矩阵；s53、获取加速度数据，z
k1
＝gs54、计算第一级滤波阶段观测方程s55、计算校正因子；先验状态估计仅使用陀螺仪数据更新角位置的先验等式，需要通过校正因子得到角位置的后验方程式，根据卡尔曼滤波理论，卡尔曼滤波器的校正方程：式中：z
k
代表实际测量值，在该算法中，第一级滤波使用加速度计数据，第二级滤波使用磁力计数据；是从先验系统状态计算出的预期测量值；用卡尔曼增益k
k
对残差加权，以计算系统状态的后验估计第一级滤波使用加速度计数据校正，校正因子为：式中：z
k1
是加速度计测量值，是根据先验系统状态计算出的重力加速度预测值，k
k1
是第一级滤波的卡尔曼增益；通过测量重力加速度，仅可以校正横滚角和俯仰角；为了保证偏航角在校正中不受影响，将校正四元数q的第三矢量部分设置为零；s56、计算后验状态估计；s57、计算后验误差协方差矩阵；s6、第二级滤波，步骤如下：s61、线性化h2(q
k
)计算雅可比矩阵h
k2
；s62、计算卡尔曼增益k
k2
；式中：r
k2
是测量噪声协方差矩阵，表示磁力计噪声及其他误差源；v
k2
是观测噪声v
k2
的雅克比矩阵；
s63、读取加速度计数据，z
k2
＝ms64、计算第二级滤波阶段观测方程s65、计算校正因子；q
∈2,1
＝0,q
∈2，2
＝0式中：z
k2
是磁力计测量值，是根据先验系统状态计算出的预期磁场强度，k
k2
是第二级滤波卡尔曼增益；为了使磁异常不影响横滚角和俯仰角的估计而仅影响偏航角，令校正四元数q
∈2
的第一、二两个部分矢量为零；s66、计算后验状态估计；s67、计算后验误差协方差矩阵；p
k
＝(i
‑
k
k2
h
k2
)p
k1
s7、基于pi自适应的参数调节第k次迭代后的第一级滤波观测噪声协方差矩阵的估计值为：r
k1
＝(1 α
k
)r
k1
‑1式中：α
k
为观测噪声协方差矩阵的调整系数；定义残差方差的理论值为：定义残差方差的实际值为：残差方差的理论值与实际值之差为：e
k
＝trace(d
k
)
‑
trace(c
k
)式中：trace()表示矩阵的迹；卡尔曼滤波器正常工作时，如果e
k
较大，则说明加速度计噪声特性发生了变化，通过调节α
k
间接调节r
k1
来改变d
k
，使d
k
与c
k
保持一致，从而使e
k
维持在0附近，达到实时修正观测噪声协方差矩阵r
k1
的目的；式中：k
p
为比例系数，k
i
为积分系数；β为积分开关系数，决定是否使用积分环节；采用积分分离式pi控制算法，计算α
k
；式中：ε为开关阈值，当误差大于阈值时，不使用积分环节，防止饱和；当误差小于阈值时，积分环节开始介入，消除余差。

技术总结
本发明公开了一种基于PI自适应两级卡尔曼滤波的无人机姿态估计方法，包括：无人机因陀螺仪零位漂移和加速度计噪声特性发生变化而导致滤波发散、滤波精度降低的问题时，将卡尔曼滤波器设计为两级滤波，第一级滤波使用加速度计数据，通过PI自适应算法实时修正观测噪声协方差矩阵，解决由于加速度计噪声特性发生变化导致滤波发散的问题。第二级滤波使用磁力计数据对姿态角进行校正，利用校正因子修正陀螺仪零位漂移引起的误差，提高无人机姿态估计精度。精度。精度。


技术研发人员：廖坤男 郭玉英
受保护的技术使用者：西南科技大学
技术研发日：2021.08.20
技术公布日：2021/11/21