一种基于加速度计修正的角加速度估计方法与流程

本发明涉及一种角速度估计方法，特别是一种基于加速度计修正的角加速度估计方法。

背景技术：

1、角加速度估计问题一直是工程上的重点问题；对于实际系统而言，角加速度的获取一般是通过差分运算和低通滤波器或卡尔曼滤波器等，但这些方法都有一定的局限性，如果存在干扰噪声，差分运算就会放大噪声，而其他方法或多或少都会带来时间延迟。

2、如专利号为cn110440795a，专利名称为“一种基于卡尔曼滤波的角加速度估计方法”的发明专利中，公开了采用卡尔曼滤波进行角加速度估计可以去除高频噪声并保证较小的延迟的技术方案，其虽然可以减小延迟，但是还是会存在一定的延迟现象，对于一些控制律设计还是会存在误差，影响精度。因此，亟需研发一种角加速度估计方法来进一步的减少时间延迟，提高角加速度估计精确度。

技术实现思路

1、本发明的目的在于，提供一种基于加速度计修正的角加速度估计方法。本发明具有能够进一步的减少时间延迟，在干扰情况下能够更精确有效的估计出角加速度的特点。

2、本发明的技术方案：一种基于加速度计修正的角加速度估计方法，包括以下步骤：

3、s1、建立被估计信号的过程模型，包括状态方程和观测方程；

4、s2、基于非线性动力学模型进行进行第一次预测，得到第一次的k+1先验状态估计矩阵和先验估计误差协方差矩阵

5、s3、进行第一次校正，建立观测方程，计算卡尔曼增益k，更新后验估计矩阵和后验估计误差协方差矩阵p；

6、s4、进行第二次预测；

7、通过以下公式进行预测：

8、

9、s5、进行第二次校正；

10、设经过第一次卡尔曼滤波后的后验估计矩阵赋为第二次校正过程的初始值，进行第二步修正，完成修正：

11、s6、返回步骤s2，进行迭代估计。

12、前述的一种基于加速度计修正的角加速度估计方法中，步骤s1的具体过程为：设定状态量为其中p，q，r为角速度，为角加速度，建立状态方程和观测方程；

13、状态方程为：

14、

15、观测方程为：

16、

17、其中，xk表示k时刻的系统状态矩阵，zk表示k时刻的观测值，a表示作用在xk上的6×6状态转移矩阵，h为6×6状态观测矩阵，δt为采样周期，wk为过程噪声，vk+1为观测噪声。

18、前述的一种基于加速度计修正的角加速度估计方法中，步骤s2中，基于非线性动力学模型进行初始估计的过程为：

19、推算得到步骤s1的k+1先验状态估计矩阵，

20、并且推算出先验估计误差协方差矩阵：其中，为6×6先验估计误差协方差矩阵，pk为6×6后验估计误差协方差矩阵，q为6×6过程噪声协方差矩阵。

21、前述的一种基于加速度计修正的角加速度估计方法中，步骤s3中，卡尔曼增益k的计算公式为：

22、其中rk为k时刻的测量噪声协方差矩阵；

23、更新后验估计矩阵

24、更新误差协方差矩阵p：

25、前述的一种基于加速度计修正的角加速度估计方法中，步骤s5中第二次校正的具体过程为：

26、设观测方程为其中，h(·)表示状态量到观测量的映射函数；

27、设状态观测矩阵h1为：

28、

29、卡尔曼增益k1为其中，r1k为k时刻的测量噪声协方差矩阵；

30、然后，更新后验估计矩阵

31、更新误差协方差矩阵p1，p1k＝(i6×6-k1h1)p；完成修正。

32、与现有技术相比，本发明采用两次加速度计修正的方法对角加速度进行估计(针对未知的环境噪声，采用两次扩展卡尔曼滤波对角加速度进行估计和修正)，通过进行仿真实验研究：将估计角加速度的实验结果与实际角加速度的结果进行比较分析，实验证明该方法能够有效地估计角加速度，对于工程上替代角加速度并且减小成本提供了一种解决方法。经测试，本申请能够在干扰情况下更精确有效的估计出角加速度，并且针对角加速度的时间延迟，补偿了约0.1秒的时间延迟误差，对工程实现具有一定的意义。

技术特征：

1.一种基于加速度计修正的角加速度估计方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种基于加速度计修正的角加速度估计方法，其特征在于，步骤s1的具体过程为：设定状态量为其中p，q，r为角速度，为角加速度，建立状态方程和观测方程；

3.根据权利要求1所述的一种基于加速度计修正的角加速度估计方法，其特征在于，步骤s2中，基于非线性动力学模型进行初始估计的过程为：

4.根据权利要求1所述的一种基于加速度计修正的角加速度估计方法，其特征在于，步骤s3中，卡尔曼增益k的计算公式为：其中rk为k时刻的测量噪声协方差矩阵；

5.根据权利要求1所述的一种基于加速度计修正的角加速度估计方法，其特征在于，步骤s5中第二次校正的具体过程为：

技术总结
本发明公开了一种基于加速度计修正的角加速度估计方法，包括以下步骤：S1、建立被估计信号的过程模型，包括状态方程和观测方程；S2、基于非线性动力学模型进行进行第一次预测，得到第一次的k+1先验状态估计矩阵和先验估计误差协方差矩阵S3、进行第一次校正，建立观测方程，计算卡尔曼增益K，更新后验估计矩阵和后验估计误差协方差矩阵P；S4、进行第二次预测；S5、进行第二次校正；设经过第一次卡尔曼滤波后的后验估计矩阵赋为第二次校正过程的初始值，进行第二步修正，完成修正：S6、返回步骤S2，进行迭代估计。本发明具有能够进一步的减少时间延迟，在干扰情况下能够更精确有效的估计出角加速度的特点。

技术研发人员：金颖,徐伟洪,余松,刘越,耿创,伍健,邵宏伟,潘超,马福鑫
受保护的技术使用者：浙江弘飞空天科技有限公司
技术研发日：
技术公布日：2024/1/15