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

说明书

本发明一种基于卡尔曼滤波的小型运输器姿态控制方法，控制成本低，可以主动进行变轨，提高飞行精度。包括以下步骤：S1、通过小型运输器上的陀螺仪获得小型运输器当前的三轴角度以及三轴角速度；S2、通过陀螺仪滤波对获取的小型运输器当前的三轴角度数据进行处理，得到小型运输器当前稳定的三轴角度；S3、采用动态的卡尔曼滤波算法对获取的小型运输器当前的三轴角速度数据进行处理，得到小型运输器当前稳定的三轴角速度；S4、将所述的稳定的三轴角度和三轴角速度作为PID控制器的输入信号，经过PID控制器处理得到小型运输器的舵面偏转角度；S5、将小型运输器的舵面偏转角度转化成PWM信号并输送给相应的舵机，舵机动作从而调整小型运输器的飞行姿态。

技术领域

本发明涉及一种姿态控制方法，更具体的说是一种基于卡尔曼滤波的小型运输器姿态控制方法。

背景技术

现有的小型运输器飞行控制多为被动控制，即在运输器的尾部加上用于稳定飞行的尾翼。对于方向的控制，常在起飞时采用发射导轨进行方向的引导。

通过尾翼来稳定飞行姿态的技术只能起到稳定运输器飞行轨道的作用，但并不能主动进行变轨，即没有对错误轨道的修正能力，也就不具备精确货物运输的能力。而对于导轨引导方向的技术，因为其发射后轨道不可控，而且易受到如风力登外界的干扰，也不能够进行精确货物运输。

现有的运输器飞控系统中，常因为导航系统价格昂贵，技术复杂等原因，因此很少应用于小型运输器当中。

本发明针对以上问题提出了一种低成本的小型运输器姿态控制方法，能够对一系列小型运输器进行飞行控制，在一定条件下可以进行精确导引。

发明内容

本发明为了解决现有的小型运输器姿态控制方法中导航系统价格昂贵，技术复杂，不能主动进行变轨等问题，提供一种基于卡尔曼滤波的小型运输器姿态控制方法，可以以低成本的方式实现小型运输器的飞行控制，并且可以主动进行变轨，提高了飞行精度。

本发明采用以下技术方案来实现：

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

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)为测量系统的参数矩阵；