一种基于卡尔曼滤波的小型运输器姿态控制方法与流程

文档序号:14725824发布日期:2018-06-19 07:07阅读:来源:国知局
技术特征:

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

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

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

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

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

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

2.根据权利要求1所述的一种基于卡尔曼滤波的小型运输器姿态控制方法,其特征在于:所述的步骤S3中,采用动态的卡尔曼滤波算法对获取的小型运输器当前的三轴角速度数据进行处理包括状态估计和时间更新。

3.根据权利要求1或2所述的一种基于卡尔曼滤波的小型运输器姿态控制方法,其特征在于:所述的采用动态的卡尔曼滤波算法对获取的小型运输器当前的三轴角速度数据进行处理包括:

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

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;代入陀螺仪的理想的姿态随机差分方程得到:

其中,wx、wy、wz分别为三轴角速度,k-1|k-1指k-1时刻最终滤波得到的角速度,k|k-1指由k-1时刻估计得到的k时刻的角速度;

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

Pk|k-1=APk-1|k-1AT+Q

其中,Pk|k-1是由k-1时刻估计得到的k时刻的协方差矩阵,Pk-1|k-1是k-1时刻最终的协方差矩阵,A为系统的参数矩阵,AT为A的转置矩阵;

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

wk|k=wk|k-1+kg(Z(k)-Cwk|k-1)

其中,

Z(k)=Cwk

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

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

在本系统中,qx=qy=qz=0.001,rx=ry=rz=0.15,到现在得到了k时刻角速度的最优值,为了让卡尔曼滤波不断进行下去,需要更新对应的Pk|k,

Pk|k=(I-Ckg)Pk|k-1,其中I为单位矩阵;

在本系统中,wk|k、Pk|k的初值为:

4.根据权利要求1所述的一种基于卡尔曼滤波的小型运输器姿态控制方法,其特征在于:所述步骤S4中的PID控制器为:

其中,θx、θy、θz分别为X轴、Y轴和Z轴的PID控制器输出的舵面偏转角度,kp为可调比例系数,kD为可调微分系数,kI为可调积分系数,αx为偏航角,αy为俯仰角,αz为滚转角,mx为X轴转动惯量,my为Y轴转动惯量,q为气动力系数,S为舵面等效面积,l为特征长度。

5.根据权利要求4所述的一种基于卡尔曼滤波的小型运输器姿态控制方法,其特征在于:所述的kp=0.0192,kD=0.01。

当前第2页1 2 3 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1