CN108181919A - 一种基于卡尔曼滤波的小型运输器姿态控制方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

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为特征长度。

作为本技术方案的进一步优化，本发明一种基于卡尔曼滤波的小型运输器姿态控制方法所述的k_p＝0.0192，k_D＝0.01。

本发明一种基于卡尔曼滤波的小型运输器姿态控制方法的有益效果为：

1、没有价格昂贵的导航***和复杂的技术，首次以低成本的方式实现了小型运输器的飞行控制；

2、能主动进行变轨，对错误轨道具备修正能力，能够精确运输货物；

3、不需要复杂的发射流程，即使是在无导轨的情况下，发射后也能自主进行轨道的矫正，因此可以简化小型运输器的发射流程，提小型运输器的容错能力。

附图说明

下面结合附图和具体实施方法对本发明做进一步详细的说明。

图1是本发明一种基于卡尔曼滤波的小型运输器姿态控制方法的流程图；

图2是本发明对高频振动的滤波结果；

图3是本发明对正常旋转的滤波结果。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。下面结合附图对本发明作进一步详细说明。

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

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

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

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

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

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

上述步骤S1中，通过小型运输器上的陀螺仪获得小型运输器当前的三轴角度以及三轴角速度。现有技术中小型运输器上都自带陀螺仪，陀螺仪可以自行获取小型运输器当前的三轴角度以及三轴角速度，该技术属于现有技术，广泛应用于各种小型运输器上。但是在实际情况下，陀螺仪的输出常伴有一定量的噪声(误差信号)，这些噪声都是高频的，随机的。而且在小型运输器实际飞行过程当中，由于存在气流影响、发动机工作等原因而产生的震动，也会对测量信号造成影响。通过实际测试，这些外力作用产生的震动给陀螺仪带来的信号同样也是高频的噪声信号。

上述步骤S2中，通过陀螺仪滤波对获取的小型运输器当前的三轴角度数据进行处理，得到小型运输器当前稳定的三轴角度。三轴角度滤波是通过陀螺仪自带的滤波算法进行滤波，属于现有技术。而陀螺仪中不含三轴角速度的滤波算法，需要通过本发明的算法进行滤波。

上述步骤S3中，采用动态的卡尔曼滤波算法对获取的小型运输器当前的三轴角速度数据进行处理，得到小型运输器当前稳定的三轴角速度。在本发明中采用了动态的卡尔曼滤波算法，该算法利用线性***的状态方程，通过***输入输出观测数据，对***状态进行最优估计。数学上可以证明，对于噪声服从高斯分布的测量信号，卡尔曼滤波算法可以获得最优的滤波结果，所获的值最接近实际结果。

步骤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-Ckg)P_k|k-1，其中I为单位矩阵；到此，卡尔曼滤波的全部过程就结束了，但是为了让卡尔曼滤波器开始工作，还要给w_k|k和P_k|k一个初值。

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

卡尔曼滤波算法验证，以偏航为例，对高频振动和正常旋转进行测试，经Matlab处理后得到对高频振动和正常旋转的滤波结果，图2是对高频振动的滤波结果，图2中实线为原始数据，虚线为经过卡尔曼滤波的结果，可以看出，经过卡尔曼滤波以后，***对高频振动的抵抗能力有了很大的提升。

图3是对正常旋转的滤波结果，图3中实线为原始数据，虚线为经过卡尔曼滤波的结果，可以看出，卡尔曼滤波将旋转时的抖动和噪声进行了很好的消除，得到了较为平滑的角速度数值。

上述步骤S4中，将所述的稳定的三轴角度和三轴角速度作为PID控制器的输入信号，经过PID控制器处理得到小型运输器的舵面偏转角度，所述舵面偏转角度包括X轴、Y轴和Z轴三个方向的舵面偏转角度。PID控制器(比例-积分-微分控制器)是一个在工业应用中常见的反馈回路部件，由比例单元P、积分单元I和微分单元D组成。PID控制的基础是比例控制，积分控制可消除稳态误差，但可能增加超调量；微分控制可加快大惯性***响应速度及减弱超调趋势。将稳定的三轴角度和三轴角速度作为PID控制器的输入信号。其中P控制器为小型运输器的俯仰角和偏航角与舵面输出的气动力矩成比例，即姿态偏离越大，舵面输出的力矩越大，输入的滚转力矩可以消除小型运输器的自旋。I控制器为小型运输器的舵面控制力矩与姿态偏角的积分成正比，D控制器为小型运输器的俯仰角速度以及偏航角速度与控制力矩成正比，即角速度越大，舵面输出的反向力矩越大，减小了小型运输器姿态因P控制器引起的超调量，增大了***的阻尼，降低了小型运输器的不稳定性，加速小型运输器姿态的收敛，最终达到消除自旋和偏角的目的，使小型运输器按照预定的姿态飞行。

步骤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为特征长度。所述的k_p＝0.0192，k_D＝0.01。

由于舵面的偏角与气动力矩并不是线性关系，直接将舵面偏角对应到姿态偏角会造成非线性的响应，这给PID控制器的控制带来了很大的难度。因此本发明中PID控制器将输出力矩与俯仰角角速度、偏航角角速度和滚转角角速度对应起来。以偏航方向为例，输出力矩与角度及角速度满足如下关系：

其中，为空气动力矩，k_p为可调比例系数，k_D为可调微分系数，k_I为可调积分系数，α_x为偏航角；

从而得到舵面偏角与偏航角、偏航角速度关系：

其中，为X轴P控制器的舵面偏角,为X轴D控制器的舵面偏角,为X轴I控制器的舵面偏角,α_y为俯仰角，m_x为X轴转动惯量，q为气动力系数，S为舵面等效面积，l为特征长度。

同理俯仰角满足：

其中，为Y轴P控制器舵面偏角,为Y轴D控制器舵面偏角,α_y为俯仰角。

同理滚转角满足：

其中，为Z轴P控制器舵面偏角,α_z为滚转角。

PID控制器是P、I、D控制器的线性叠加，因此输出量为：

其中，θ_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为特征长度。经计算及实验取：k_D＝0.01，k_p＝0.0192。

上述步骤S5中，将小型运输器的舵面偏转角度转化成PWM信号并输送给相应的舵机，舵机动作从而调整小型运输器的飞行姿态。舵机应当有四个，呈十字型分布，舵机动作从而带动舵面动作，进而调整小型运输器的飞行姿态。

当然，上述说明并非对本发明的限制，本发明也不仅限于上述举例，本技术领域的普通技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也属于本发明的保护范围。

Claims (5)

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

其中，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的初值为:

4.根据权利要求1所述的一种基于卡尔曼滤波的小型运输器姿态控制方法，其特征在于：所述步骤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为特征长度。

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