[发明专利]一种基于卡尔曼滤波的角加速度估计方法

说明书

本发明公开了一种基于卡尔曼滤波的角加速度估计方法，用于解决增量动态逆控制(INDI)利用角加速度反馈进行解算，而一般飞控硬件采用惯性测量单元测量角速度，没有角加速度传感器直接测量角加速度的问题。直接利用角速度差分得到的角加速度噪声过大，不能用于控制律的解算，因此需要利用传感器的信号估计出角加速度信号。常规的方法是对角速度信号直接进行差分之后再低通滤波，但这样会带来额外的时间延迟。本发明提供的用卡尔曼滤波进行角加速度估计可以去除高频噪声并保证较小的延迟。该算法因涉及了实际工程背景，且简单方便易于实施，具有较高的工程应用价值。

技术领域

本发明及飞行器控制领域，尤其涉及一种飞行器角加速度估计方法。

背景技术

增量动态逆控制(INDI)是一种对于模型参数不敏感的鲁棒非线性控制方法，该方法利用角加速度反馈进行解算，而一般飞控硬件采用惯性测量单元测量角速度，没有角加速度传感器直接测量角加速度。

直接利用角速度差分得到的角加速度噪声过大，不能用于控制律的解算，因此需要利用传感器的信号估计出角加速度信号。常规的方法是对角速度信号直接进行差分之后再低通滤波，但这样会带来额外的时间延迟。

发明内容

本发明提供了一种基于卡尔曼滤波的角加速度估计方法，提高了基于增量动态逆算法的控制的稳定性。

一种基于卡尔曼滤波的角加速度估计方法，用于增量动态逆控制中对角加速度的解算，主要包括如下几个步骤：

S1：建立卡尔曼滤波模型，状态量为分别为角速度、角加速度和角加速度的导数，飞行器的状态方程和观测方程如下：

z_k+1＝[1 0 0]x_k+1+R,H＝[1 0 0]

ΔT为采样周期，z为观测量，观测量为俯仰角速度，来源为IMU的角速率。Q和R分别为过程噪声和观测噪声，状态误差协方差定义为：

S2：滤波估计过程：一步状态更新，根据运动学方程求解出下一采样时刻的状态量

S3：根据上一采样时刻状态误差协方差进行进一步更新，得到

S4：根据和H，求解卡尔曼增益；

S5：进行姿态角运动状态量和协方差校正。

进一步地，所述下一采样时刻的状态量

进一步地，所述状态协方差更新计算方法为

进一步地，所述求解卡尔曼增益公式为

进一步地，所述姿态角运动状态量和协方差校正表示方法如下：

与现有技术相比，本发明优点是：利用卡尔曼滤波，通过选取适当的运动模型，在保证去除高频噪声的同时不会带来过多的延迟，有较高的工程实用价值。

附图说明

附图出示了本公开的示例性实施方式，并与其说明一起用于解释本公开的原理，其中包括了这些附图以提供对本公开的进一步理解，并且附图包括在说明书中并构成本说明书的一部分。

图1为根据本公开至少一个实施方式的卡尔曼滤波角加速度估计方法与直接差分+低通滤波的角加速度估计方法估计结果对比图。

图2为根据本公开至少一个实施方式的两种方法估计结果局部放大图。

具体实施方案