[发明专利]一种基于卡尔曼滤波的小型运输器姿态控制方法

权利要求书

1.一种基于卡尔曼滤波的小型运输器姿态控制方法，其特征在于，所述方法包括以下步骤：

S1、通过小型运输器上的陀螺仪获得小型运输器当前的三轴角度以及三轴角速度；

S2、通过陀螺仪滤波对获取的小型运输器当前的三轴角度数据进行处理，得到小型运输器当前稳定的三轴角度；

S3、采用动态的卡尔曼滤波算法对获取的小型运输器当前的三轴角速度数据进行处理，得到小型运输器当前稳定的三轴角速度；

S4、将所述的稳定的三轴角度和三轴角速度作为PID控制器的输入信号，经过PID控制器处理得到小型运输器的舵面偏转角度，所述舵面偏转角度包括X轴、Y轴和Z轴三个方向的舵面偏转角度；

S5、将小型运输器的舵面偏转角度转化成PWM信号并输送给相应的舵机，舵机动作从而调整小型运输器的飞行姿态；

所述的步骤S3中，采用动态的卡尔曼滤波算法对获取的小型运输器当前的三轴角速度数据进行处理包括状态估计和时间更新；

所述的采用动态的卡尔曼滤波算法对获取的小型运输器当前的三轴角速度数据进行处理包括：

陀螺仪的理想的姿态随机差分方程为：

X(k)＝Aw(k-1)+BU(k)+w(k)

Z(k)＝HX(k)+V(k)

其中，X(k)为k时刻系统状态，Z(k)为测量值矩阵,A、B为系统的参数矩阵,U(k)为控制量，w(k)、v(k)分别为过程噪声和系统噪声，H(k)为测量系统的参数矩阵；

状态估计：由于陀螺仪刷新频率高达50Hz，因此可认为两次测量时的三轴角速度不变，A＝1；在该系统中没有控制量，因此U(k)＝0；代入陀螺仪的理想的姿态随机差分方程得到：

其中，w_x、w_y、w_z分别为三轴角速度，k-1|k-1指k-1时刻最终滤波得到的角速度，k|k-1指由k-1时刻估计得到的k时刻的角速度；

此时系统的状态已经更新为w_k|k-1，但是对应的协方差还没更新，所以还需对协方差进行更新，由上一时刻的状态的协方差P_k-1|k-1，预估当前时刻的协方差P_k|k-1，

P_k|k-1＝AP_k-1|k-1A^T+Q

其中，P_k|k-1是由k-1时刻估计得到的k时刻的协方差矩阵，P_k-1|k-1是k-1时刻最终的协方差矩阵，A为系统的参数矩阵，A^T为A的转置矩阵；

时间更新：根据当前的测量值对估测值进行修正：

w_k|k＝w_k|k-1+k_g(Z(k)-Cw_k|k-1)

其中，

Z(k)＝Cw_k

k_g(k)为卡尔曼滤波的增益，Z(k)为测量值矩阵，

陀螺仪测量的量就是角速度，因此C＝1；Q和R是过程激励噪声和测量噪声协方差矩阵；由于三轴角速度测量误差相互之间没有影响，因此，

在本系统中，q_x＝q_y＝q_z＝0.001，r_x＝r_y＝r_z＝0.15，到现在得到了k时刻角速度的最优值，为了让卡尔曼滤波不断进行下去，需要更新对应的P_k|k，

P_k|k＝(I-Ck_g)P_k|k-1，其中I为单位矩阵；

在本系统中，w_k|k、P_k|k的初值为:

所述步骤S4中的PID控制器为：

其中，θ_x、θ_y、θ_z分别为X轴、Y轴和Z轴的PID控制器输出的舵面偏转角度，k_p为可调比例系数，k_D为可调微分系数，k_I为可调积分系数，α_x为偏航角，α_y为俯仰角，α_z为滚转角，m_x为X轴转动惯量，m_y为Y轴转动惯量，q为气动力系数，S为舵面等效面积，l为特征长度。

2.根据权利要求1所述的一种基于卡尔曼滤波的小型运输器姿态控制方法，其特征在于：所述的k_p＝0.0192，k_D＝0.01。