[发明专利]基于PI自适应两级卡尔曼滤波的无人机姿态估计方法

权利要求书

1.一种基于PI自适应两级卡尔曼滤波的无人机姿态估计方法，其特征在于，包括以下步骤：

S1、建立导航坐标系与机体坐标系，并基于牛顿-欧拉法建立了如下的非线性数学模型：

式中：x,y,z是导航坐标系下无人机的位置，p,q,r是机体坐标下的角速度，m为机身质量，U₁为四旋翼提供的总推力，I_x,I_y,I_z和U₂,U₃，U₄分别为绕机体坐标系3个坐标轴的转动惯量和力矩；

S2、以四选取四元数q＝(q₀，q₁，q₂，q₃)^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是陀螺仪测得的角速度；将状态方程从连续时间形式转换为离散时间形式，通过差分近似代替微分求得：

进而得到状态转移矩阵为:

使用方向余弦矩阵结合加速度数据得到第一级滤波阶段观测矩阵：

其中，

式中：h₁(q_k)为系统第一级滤波观测方程，q_k为k时刻的状态变量，为方向余弦矩阵；h₁(q_k)是非线性的，需要通过扩展卡尔曼滤波将系统线性化，用雅克比矩阵代替观测矩阵；雅克比矩阵为：

同理，第二级滤波阶段观测矩阵为：

线性化得

S4、先验估计：

先验状态估计值是滤波的中间计算结果，即根据(k-1)时刻的最优估计预测k时刻的结果；计算步骤如下：

S41、读取陀螺仪传感器数据，获得角速度ω_x，ω_y，ω_z；

S42、计算状态转移矩阵A_k；

S43、计算系统的先验状态估计：

S44、计算先验噪声协方差矩阵；为了计算卡尔曼增益K_k，首先计算先验误差协方差矩阵

式中：Q_k是过程噪声协方差矩阵，它表示陀螺仪噪声及陀螺仪其他误差源；

S5、第一级滤波，步骤如下：

S51、线性化h₁(q_k)计算雅可比矩阵H_k1；

S52、计算卡尔曼增益K_k1；由于观测矩阵h₁(q_k)是非线性的，所以通过扩展卡尔曼滤波获得卡尔曼滤波增益K_k1为：

式中：R_k1是测量噪声协方差矩阵，表示加速度计噪声和加速度计的其他误差源；V_k1是关于非线性方程组h₁(q_k)的观测噪声v_k1的偏导数的雅克比矩阵；

S53、获取加速度数据，z_k1＝G

S54、计算第一级滤波阶段观测方程

S55、计算校正因子；先验状态估计仅使用陀螺仪数据更新角位置的先验等式，需要通过校正因子得到角位置的后验方程式，根据卡尔曼滤波理论，卡尔曼滤波器的校正方程：

式中：z_k代表实际测量值，在该算法中，第一级滤波使用加速度计数据，第二级滤波使用磁力计数据；是从先验系统状态计算出的预期测量值；用卡尔曼增益K_k对残差加权，以计算系统状态的后验估计

第一级滤波使用加速度计数据校正，校正因子为：

式中：z_k1是加速度计测量值，是根据先验系统状态计算出的重力加速度预测值，K_k1是第一级滤波的卡尔曼增益；通过测量重力加速度，仅可以校正横滚角和俯仰角；为了保证偏航角在校正中不受影响，将校正四元数q的第三矢量部分设置为零；

S56、计算后验状态估计；

S57、计算后验误差协方差矩阵；

S6、第二级滤波，步骤如下：

S61、线性化h₂(q_k)计算雅可比矩阵H_k2；

S62、计算卡尔曼增益K_k2；

式中：R_k2是测量噪声协方差矩阵，表示磁力计噪声及其他误差源；V_k2是观测噪声v_k2的雅克比矩阵；

S63、读取加速度计数据，z_k2＝M

S64、计算第二级滤波阶段观测方程

S65、计算校正因子；

q_∈2,1＝0,q_∈2，2＝0

式中：z_k2是磁力计测量值，是根据先验系统状态计算出的预期磁场强度，K_k2是第二级滤波卡尔曼增益；为了使磁异常不影响横滚角和俯仰角的估计而仅影响偏航角，令校正四元数q_∈2的第一、二两个部分矢量为零；

S66、计算后验状态估计；

S67、计算后验误差协方差矩阵；

P_k＝(I-K_k2H_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；

式中：ε为开关阈值，当误差大于阈值时，不使用积分环节，防止饱和；当误差小于阈值时，积分环节开始介入，消除余差。