惯性定位如何解决陀螺仪漂移和磁场干扰问题？.docxVIP

惯性定位如何解决陀螺仪漂移和磁场干扰问题？ 针对在导航系统姿态解算中，（陀螺仪）和电子罗盘在解算姿态时分别存在积分误差和磁场干扰的问题，提出了利用Kalman滤波和互补滤波相融合的算法进行定位。首先将电子罗盘和（陀螺仪）通过Kalman滤波得出最优估计四元数，然后利用互补滤波算法对陀螺仪的漂移进行补偿得到校正后的四元数，将此次得到的四元数和Kalman滤波得出最优估计四元数再次通过Kalman滤波对四元数进行第二次最优估计，进而输出姿态角。实验中对比了本算法和互补滤波算法、无滤波算法的效果。实验证明，该算法不仅可以有效解决方位角误差发散问题，还有效解决了磁场干扰问题，实现了高精度的方位输出。 随着以MEMS(Micro-electromechanicalSys（te）ms)（传感器）为代表的微型化惯性器件的发展，基于捷联式惯导原理和MEMS（传感器）的惯性定位技术也日益受到重视，特别是在难以接收卫星信号的室内、地下、矿井、水下、战场等场合[1]。针对上述问题往往采用利用电子罗盘对陀螺进行校正的方法，在室内、地下、矿井、水下等过程中磁强计更加容易受到干扰，造成方位更大的偏差。针对磁强计易受干扰和陀螺积分漂移的问题已经有众多融合的算法出现，比如卡尔曼滤波、无迹卡尔曼滤波（UKF）、扩展卡尔曼滤波（EKF）等[2-4]，这些滤波方法需要建立准确的状态方程和观测方程。还有一种滤波算法是在互补滤波的基础上进行扩展，比如经典互补滤波、基于梯度下降法的互补滤波算法等[3-6]，但是这种滤波算法适用的精度不高。面对这些问题，本文提出了一种Kalman滤波和互补滤波融合的惯性定位算法，该算法在设计Kalman滤波中，将加速度计和磁强计融合得出的四元数作为观测值，利用陀螺仪得出的四元数作为状态值，通过数据的融合进行滤波，完成四元数的第一次最优估计，针对陀螺漂移问题则利用所设计的互补滤波对陀螺漂移进行补偿，得到校正后的角速度，进而求得校正后不断更新的四元数，然后和第一次完成的最优估计四元数通过第二次Kalman滤波进行估计，进而输出高精度的姿态角。 1算法总体设计 Kalman滤波和互补滤波融合的惯性定位算法的总体思路如图1所示。 首先，将磁强计和加速度计测量的磁场强度和加速度利用高斯牛顿迭代法解算出的四元数送入Kalman滤波中当作观测值，将陀螺仪测量的角速度值利用四阶-库塔法解算出的四元数作为Kalman滤波的状态值进而得到四元数的第一次最优估计值，在Kalman滤波过程中既可以去掉随机噪声，通过电子罗盘对陀螺仪进行校正，又可以避免由于磁场干扰带来的干扰数据。 其次，利用载体坐标系和地理坐标系之间的转换矩阵，将地理坐标系下的重力和地磁分量转换到载体坐标系下，然后与加速度计和磁强计在载体坐标系下测量得到的加速度和磁场强度做向量积的运算，将二者向量积的和相加经过比例积分低通滤波，可以将电子罗盘测量姿态中的高频抖动信号滤出，然后和陀螺仪测量的角速度进行融合，得到对陀螺仪补偿后的角速度，利用四阶龙格库塔法得到校正后的四元数。 最后，将互补滤波算法得到的校正四元数作为状态量，将第一次Kalman滤波得到的最优四元数作为观测量进行第二次Kalman滤波，完成四元数的第二次优化，然后通过四元数建立姿态矩阵求得更加精确的姿态角。 2卡尔曼滤波设计 卡尔曼滤波（Kalmanfiltering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。建立系统的状态方程和测量方程是卡尔曼滤波的主要工作。本次卡尔曼滤波设计中用四元数的不断更新量作为卡尔曼滤波中的状态量。 下面建立测量方程，由重力加速度和三轴加速度计 式（4）中ax、ay、az表示重力加速度的测量值，重力加速度用g表示。 由地磁场和磁强计测量信息之间的关系可得： 由式（6）、式（7）并通过高斯牛顿迭代法求得四元数，作为观测量。观测量取为： 式（8）中观测量方程Z（k）=H（k）X（k）+V（k），其中H（k）为观测量的增益矩阵，V（k）为观测噪声，方差R（k）可以通过传感器的测量数据获得。由于第一次Kalman滤波和第二次Kalman滤波都是对四元数的最优估计，故上述滤波公式和建立的方程均保持不变。 3互补滤波算法设计 由于陀螺仪通过积分得到的方位角存在长时间误差积问题，但其动态响应好，不易受外界干扰，而与之对应的电子罗盘易受外界干扰，但它具有无误差累积的优点。互补滤波就是利用陀螺仪的高频特性和电子罗盘的低频特性进行融合，通过比例环节进行频率特性的调节，如图2所示。 在图2中，a、m表示载体坐标系下加速度计和磁强计测量得到的加速度和磁场强度；将地理坐标系下的重力分量和地磁分量通过四元数构成的姿态转换矩阵变换为载体坐标系下的重力分量和地磁分量，分别用as和ms表示。在载体坐标