[发明专利]基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法

说明书

技术领域

本发明涉及基于卡尔曼滤波的水平倾角测量方法，属于动态设备系统的测量技术领域。

背景技术

在控制系统中，水平角度测量值是极其重要的测量信号，通常作为控制系统的反馈信号，为系统进行校正，因此其精度及稳定性极其重要。目前常见的水平角度测量元件多为倾角传感器，常用的倾角测量传感器分为固体式、液体式和气体摆式三大类，它们都是基于牛顿第二定律来研制的。在静态情况下测量，它们均可以获得较高的精度。但是当载体在动态情况下，因为加速度的干扰会在传感器的敏感轴上加入分量，从而造成测量上的误差，使系统的测量精度降低，进而对系统的控制会产生一定的影响。重力摆式的倾角传感器在动态情况下无法精确测量，而以利用定轴性原理制作的陀螺仪在原理上适用于动态测量系统的角速度。在静态时，加速度计直接测量物体重力加速度能够运用反三角函数的公式换算成系统的倾斜角度，但是在动态情况下受到干扰较大。而陀螺仪的输出值是其该方向上相对于灵敏轴的角速率，利用角速率对时间的积分得到其角度值，动态性良好，但是会随时间的推移累积漂移误差。

发明内容

本发明的目的是为了解决加速度计陀螺仪测量水平角度在动态测量环境下精度差的问题，本发明提供一种基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法。

本发明的基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法，

所述方法包括如下步骤：

步骤一：利用加速度计和陀螺仪采集关于X轴的数据，获得加速度和角速率；

步骤二：对获得的加速度和角速率进行数字滤波，滤除在传输过程中产生的高频干扰；

步骤三：将滤波后的加速度转换成角度；

步骤四：根据转换的角度，代入温度补偿因素，得到补偿后的角度；

步骤五：将补偿后的角度和滤波后的角速率进行卡尔曼滤波，并进行数据融合，获得水平角度的最优估计值。

所述步骤五的过程包括：

步骤五一：设置参数的初值：

矩阵A=1-dt01,]]>t表示当前时刻，dt表示积分，矩阵B=dt0,]]>过程噪声的协方差矩阵Q=qa00qg,]]>q_a表示加速度计的测量协方差，q_g表示陀螺仪的测量协方差，R＝[r_a]，r_a表示测量误差的协方差，水平角度估计值的协方差初值P(0/0)=0000,]]>水平角度估计初值X(0/0)=00;]]>