基于二阶卡尔曼滤波的陀螺仪及加速度计信号融合的姿态角度测量

★基于陀螺仪及加速度计信号融合的姿态角度测量

1、系统组成

     本文所采用的姿态角度测控系统主要由加速度计、 陀螺仪、 微控制器、 滤波电路、 电机调速器、 无刷电机等部分组成. 姿态检测系统的硬件平台如图1, 由微处理器对陀螺仪、 滤波电路和加速度计构成的传感器组进行高速 A/D采样后, 通过卡尔曼滤波器对传感器数据进行补偿和信息融合, 得到准确的姿态角度信号, 此角度信号再通过PID控制器运算, 输出给电子调速器转换成 PWM 信号, 进而对电机进行控制.

     加速度计用于测量物体的线性加速度, 加速度计的输出值与倾角呈非线性关系, 随着倾角的增加而表现为正弦函数变化. 因此对加速度计的输出 进 行 反 正 弦 函 数 处 理, 才 能 得 到 其 倾 角值[5-6] . 测量数据噪声与带宽的平方根成正比, 即噪声会随带宽的增加而增加,即

                                                       VNoise=350ug√(B_w×1⋅5)

式中: Bw 为传感器带宽(单位为 Hz) . 因此在设计卡尔曼滤波器时, 首先要确定被测加速度的频率范围,然后再设计滤波器的参数, 尽量使滤波器的带宽略高于被测频率, 这样不仅有助于滤除高频干扰, 而且也有利于降低系统噪声干扰. 但是如果要得到精确的倾角值, 带宽就需要设置得比较小, 而这时加速度计动态响应慢, 不适合跟踪动态角度运动, 如果期望快速的响应, 又会引入较大的噪声. 再加上其测量范围的限制, 使得单独应用加速度计检测飞行器倾角并不合适, 需要与其他传感器共同使用.陀螺仪的作用是用来测量角速度信号, 通过对角速度积分, 便能得到角度值. 陀螺仪本身极易受噪声干扰, 微机械陀螺不能承受较大的震动, 同时由于温度变化、 不稳定力矩等因素, 陀螺仪会产生漂移误差,并会随着时间的推移而累加变大, 通过积分会使得误差变得很大. 因此, 也不能单独使用陀螺仪作为本系统的倾角传感器。

2卡尔曼滤波融合过程

      首先建立系统的状态方程和测量方程.由于倾角和倾角角速度存在导数关系,系统倾斜真实角度 可以用来做一个状态向量.在该系统中,采用加速度计估计出陀螺仪常值偏差b,以此偏差作为状态向量得到相应的状态方程和观测方程:

                X_(K)=[¹₀ ^-T₁]X_(K-1)+ [T 0]^Tωgyro_(k-1)+[wg (k)T  0]^T

                Z_(K)= [1 0]Y_(K)+wa_(K)          

式中,ωgyro为包含固定偏差的陀螺仪输出角速度,Z(K)为加速度计经处理后得到的角度值,wg为陀螺仪测量噪声,wa为加速度计测量噪声,wg与wa相互独立,此处假设二者为满足正态分布的白色噪声,令T为系统采样周期,其中：

                  X_(K-1) =[φ b ]^{T                                   φ}  :陀螺仪经过积分后的角度

                  Y_{(K) =} [∅ b]^{T                                         ∅}:加速度计计算得到的角度

其中b为陀螺仪漂移误差。同时,要估算k时刻的实际角度,就必须根据k-1时刻的角度值,再根据预测得到的k时刻的角度值得到k时刻的高斯噪声的方差,在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值.在此,须知道系统过程噪声协方差阵Q以及测量误差的协方差矩阵R,对卡尔曼滤波器进行校正.Q与R矩阵的形式如下：

                                                        Q=[^q_acce₀     ⁰_{q_gryo}]                  R=[r__acce]

式中,q_acce和q_gyro分别是加速度计和陀螺仪测量的协方差, 其数值代表卡尔曼滤波器对其传感器数据的信任程度,值越小,表明信任程度越高.在该系统中陀螺仪的值更为接近准确值,因此取q_gyro的值大于q_acce的值.

                       当前状态:

                                    X_(k|k-1)=A_X(k-1|k-1)+B^TU_(k)            （公式1）

式中,A=[¹₀ ^-T₁] ,B= [T 0]，X_(k|k-1)是利用k预测的结果, X_(k-1|k-1)是k-1时刻的最优结果.

                       则有对应于 X_(k|k-1) 的协方差为:

                                  P_(k|k-1)=AP_(k-1|k-1)A^T+Q            （公式2）

式中,P_(k-1|k-1)是X_(k-1|k-1)对应的协方差,A^T表示A的转置矩阵,Q是系统过程的协方差.式子(1)、 (2)即对系统的状态更新.

                       则状态k的最优化估算值X_(k|k):

                                X_(k|k)=X_(k|k-1)+K_(k)(Z_(k)-HX_(k|k-1))  （公式3）

其中 H= [1 0].

                      K 为卡尔曼增益(KalmanGain):

                                K_(k)=P_(k|k-1)H^T/(HP_(k|k-1)H^T+R)          (公式4)

此时, 我们已经得到了k状态下最优的估算值X(k|k).但是为了使卡尔曼滤波器不断的运行下去直到找到最优的角度值, 我们还要更新

                     k状态下 X_(k|k)的协方差:

                                P_(k|k)=[I-K_(k)H]P_(k|k-1)[I-K_(k)H]^T+ K_(k)RK_(k)^T（公式5）

 可改写为：

                               P_(k|k)=[I-K_(k)H]P_(k|k-1)                   （公式6）

其中,公式（5）和公式（6）均为更新协方差，公式（6）计算量相对较小，但不能保证计算出的P_(k|k)是对称的，公式（5）的适用范围较广。I为单位矩阵, 对于本系统则有：

I=[¹₀ ⁰₁] 当系统进入k+1状态时, P_(k|k)就是式子(2)的P_(k-1|k-1).(3)、(4)、(5)式为卡尔曼滤波器状态更新方程.计算完时间更新方程和测量更新方程后, 再次重复上一次计算得到的后验估计, 作为下一次计算的先验估计, 这样,周而复始、循环反复地运算下去直至找到最优的结果.

2、卡尔曼滤波代码：

#define T  0.005
float A[2][2]={{1,-T},{0,1}};                //系统系数
float B[2]={T,0},H[2]={1,0};                 //系统系数
float Last_result;            //上一次的角度值
float Bias_angle;             //陀螺仪积分出的角度相对于加速度计的偏移量
float Now_Groy_palstance;            //当前包含固定偏差的陀螺仪输出角速度
float Gyro_measure_noise;                      //为陀螺仪测量噪声
float Acceleration_measure_noise;              //为加速度计测量噪声
float Measure;                               //当前的测量值
floatQ[2][2]={{0.00000003,0},{0,0.00000001}},R=0.5;
//系统过程噪声协方差阵Q以及测量误差的协方差矩阵R
float Now_measure_result[2];        //当前预测结果
float Now_best_result[2];          //当前最优化估算值
float Now_covariance[2][2];       //对应于Now_measure_result的协方差
floatlast_covariance[2][2]={{0.005,0.005},{0.005,0.005}}
//对应于Last_best_result的协方差
float Kalman_Gain[2];              //卡尔曼增益
float I[2][2]={{1,0},{0,1}};         //I为单位阵     
float Real_angle;          
void Kalman_LvBo()
{
  float  P[2][2],K[2]，D[2][2],S[2][2];   //过渡矩阵
  Now_Groy_palstance=(Gyro_X);       //陀螺仪角速度
  Last_result=Now_best_result[0];        //Ψ
  Bias_angle=Now_best_result[1];        //b
//测量方程         Z(K)
  Measure=angle;                           //加速度角度
//公式（1）    当前状态方程       X(K|K-1)
  Now_measure_result[0]= A[0][0]*Last_result+A[0][1]* Bias_angle+B[0]*Now_Groy_palstance+Gyro_measure_noise*T;
  Now_measure_result[1]=A[1][0]*Last_result+A[1][1]* Bias_angle；
//公式（2）    当前状态协方差          P(K/K-1)
  P[0][0]=A[0][0]*last_covariance[0][0]+A[0][1]*last_covariance[1][0];
  P[0][1]=A[0][0]*last_covariance[0][1]+A[0][1]*last_covariance[1][1];
  P[1][0]=A[1][0]*last_covariance[0][0]+A[1][1]*last_covariance[1][0];
  P[1][1]=A[1][0]*last_covariance[0][1]+A[1][1]*last_covariance[1][1];
  Now_covariance[0][0]=P[0][0]*A[0][0]+P[0][1]*A[0][1]+Q[0][0];
  Now_covariance[0][1]=P[0][0]*A[1][0]+P[0][1]*A[1][1]+Q[0][1];
  Now_covariance[1][0]=P[1][0]*A[0][0]+P[1][1]*A[0][1]+Q[1][0];
  Now_covariance[1][1]=P[1][0]*A[1][0]+P[1][1]*A[1][1]+Q[1][1];
//公式（4）    卡尔曼增益
  K[0]=H[0]*Now_covariance[0][0]+H[1]*Now_covariance[1][0];
  K[1]=H[0]*Now_covariance[0][1]+H[1]*Now_covariance[1][1];
  Kalman_Gain[0]=(Now_covariance[0][0]*H[0]+Now_covariance[0][1]*H[1])/(H[0]*K[0]+H[1]*K[1]+R);
  Kalman_Gain[1]=(Now_covariance[1][0]*H[0]+Now_covariance[1][1]*H[1])/(H[0]*K[0]+H[1]*K[1]+R);
//公式（3）   最优化估算值              X(K|K)
  Now_best_result[0]=Now_measure_result[0]+Kalman_Gain[0]*(Measure-(H[0]*Now_measure_result[0]+H[1]*Now_measure_result[1]));
  Now_best_result[1]=Now_measure_result[1]+Kalman_Gain[1]*(Measure-(H[0]*Now_measure_result[0]+H[1]*Now_measure_result[1]));
//公式（5）   更新当前状态的协方差      P(K|K)
  D[0][0]=I[0][0]-Kalman_Gain[0]*H[0];
  D[0][1]=I[0][1]-Kalman_Gain[0]*H[1];
  D[1][0]=I[1][0]-Kalman_Gain[1]*H[0];
  D[1][1]=I[1][1]-Kalman_Gain[1]*H[1];
  S[0][0]=D[0][0]*Now_covariance[0][0]+D[0][1]*Now_covariance[1][0];
  S[0][1]=D[0][0]*Now_covariance[0][1]+D[0][1]*Now_covariance[1][1];
  S[1][0]=D[1][0]*Now_covariance[0][0]+D[1][1]*Now_covariance[1][0];
  S[1][1]=D[1][0]*Now_covariance[0][1]+D[1][1]*Now_covariance[1][1];
  last_covariance[0][0]=S[0][0]*D[0][0]+S[0][1]*D[0][1]+Kalman_Gain[0]*Kalman_Gain[0]*R;
  last_covariance[0][1]=S[0][0]*D[1][0]+S[0][1]*D[1][1]+Kalman_Gain[0]*Kalman_Gain[1]*R;
  last_covariance[1][0]=S[1][0]*D[0][0]+S[1][1]*D[0][1]+Kalman_Gain[1]*Kalman_Gain[0]*R;
  last_covariance[1][1]=S[1][0]*D[1][0]+S[1][1]*D[1][1]+Kalman_Gain[1]*Kalman_Gain[1]*R;  
  Real_angle=Now_best_result[0];  //输出最终角度
}