[发明专利]一种基于卡尔曼滤波算法的三维定位方法及其定位系统

权利要求书

1.一种基于卡尔曼滤波算法的三维定位方法，其特征在于：包括如下步骤：

步骤一：使用中央控制器(1)获取定位目标所佩戴三维定位系统中的MEMS传感器采集的数据信息，所述数据信息包括磁强计(2)、陀螺仪(3)、加速度计(4)、气压高度计(5)、空速计(6)、GPS(7)采集的数据；

步骤二：所述中央控制器(1)对步骤一中获取的磁强计(2)、陀螺仪(3)、加速度计(4)、气压高度计(5)采集的数据进行各自的误差补偿得到观测值；

步骤三：将磁强计(2)、陀螺仪(3)、加速度计(4)的观测值输入姿态测量模块进行数据处理，所述姿态测量模块处理数据时，采用扩展卡尔曼滤波算法对处理后的姿态角误差与四元数法得到的姿态角进行误差修正，得到定位目标的实时姿态信息，所述定位目标的实时姿态信息通过姿态测量模块输入中央控制器(1)中；

步骤四：将空速计(6)、GPS(7)采集的数据输入导航测控模块进行数据处理，将气压高度计(5)的观测值通过气压温度解算并经过误差补偿后得到定位目标的高度值，将磁强计(2)、陀螺仪(3)、加速度计(4)的观测值经过姿态解算、静态精测、零位角速度更新、零位速度更新补偿后进入扩展卡尔曼滤波器进行解算，所述中央处理器(1)处理数据时采用15元素的误差状态向量导航算法对传感器数据进行误差补偿，包括姿态误差、角速度误差、位置误差、速度误差和加速度误差，得到定位目标的三维位置、姿态、速度实时信息，所述定位目标的上述信息输入中央控制器(1)中；

步骤五：中央控制器(1)输出定位目标的实时位置信息、姿态信息和速度信息并通过无线通信模块(9)传输至远程控制中心。

2.根据权利要求1所述的一种基于卡尔曼滤波算法的三维定位方法，其特征在于：所述步骤二中对磁强计(2)采集的数据进行误差补偿的方法包括硬磁干扰误差补偿和软磁干扰误差补偿,所述软磁干扰误差补偿根据如下公式计算：

上式中，(m_x,m_y)为未受到磁场干扰的磁强计数据，(m′_x,m′_y)为误差补偿后的磁强计数据，α为受到软磁干扰变成的椭圆长轴与水平直线的夹角，h为图形为标准圆时mx与my对应的比例系数。

3.根据权利要求2所述的一种基于卡尔曼滤波算法的三维定位方法，其特征在于：所述步骤二中对陀螺仪(3)采集的数据进行误差补偿的方法为对陀螺仪的漂移补偿，所述漂移补偿的具体方法为：

步骤3.1：将导航测控模块水平、静止放置在无干扰环境下，中央控制器(1)采集导航测控模块中的传感器数据并将数据进行存储；

步骤3.2：设在温度为T₁时，得到陀螺仪的零点值为S₀₁，在温度为T₂时，得到陀螺仪的零点值为S₀₂，取

各个陀螺仪的零点漂移值与温度的变化值存在以下比例关系:

ΔS＝ΔT·c+e，

上式中：e为方程的残差，c为方程的比例系数；

两组陀螺仪关于零点漂移和温度变化的数据根据以下公式计算：

最后采用最小二乘法解算得到

c＝(T^T·T)^-1·T^T·S,上式中：ΔS₀(n)为温度为第n次采样时的陀螺仪零点差值，ΔT₀(n)为第n次采样时的温度零点差值。