(19)中华人民共和国国家知识产权局
(12)发明专利申请
(10)申请公布号 (43)申请公布日 (21)申请号 202010780014.5
(22)申请日 2020.08.05
(71)申请人 翟瑞永
地址 066007 河北省秦皇岛市燕山大街195
号
(72)发明人 翟瑞永 王海生 于丽君
(74)专利代理机构 太原高欣科创专利代理事务
所(普通合伙) 14109
代理人 崔浩 冷锦超
(51)Int.Cl.
G01C 21/20(2006.01)
G01C 21/16(2006.01)
G01C 21/08(2006.01)
(54)发明名称
一种基于卡尔曼滤波算法的三维定位方法
及其定位系统
(57)摘要
本发明一种基于卡尔曼滤波算法的三维定
位方法及其定位系统，属于三维定位方法及其定
位系统技术领域；所要解决的技术问题为：提供
一种基于卡尔曼滤波算法的三维定位方法的改
进；解决上述技术问题采用的技术方案为：获取
定位目标所佩戴三维定位系统中的MEMS传感器
算法对处理后的传感器数据误差进行修正；采用
15元素的误差状态向量导航算法对传感器数据
进行误差补偿；将步骤三、步骤四中经过修正和
误差补偿的传感器数据输入卡尔曼滤波器进行
滤波，输出定位目标的实时位置、姿态和速度信
息；本发明应用于密闭、GPS信号较弱及网络信号
较弱环境中的目标定位。权利要求书5页 说明书16页 附图4页CN 111854762 A 2020.10.30
C N 111854762
A
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：设在温度为T1时，得到陀螺仪的零点值为S01，在温度为T2时，得到陀螺仪的零点值为S02，取
各个陀螺仪的零点漂移值与温度的变化值存在以下比例关系:
ΔS＝ΔT·c+e，
上式中：e为方程的残差，c为方程的比例系数；
两组陀螺仪关于零点漂移和温度变化的数据根据以下公式计算：
最后采用最小二乘法解算得到
c＝(T T·T)-1·T T·S，上式中：ΔS0(n)为温度为第n次采样时的陀螺仪零点差值，ΔT0 (n)为第n次采样时的温度零点差值。
4.根据权利要求3所述的一种基于卡尔曼滤波算法的三维定位方法，其特征在于：所述步骤二中对加速度计(4)采集的数据进行误差补偿的方法具体通过标定加速度计(4)的零偏和刻度系数误差的方法实现，包括如下标定步骤：
步骤4.1：将加速度计(4)安装在旋转转台上，其中转台的旋转轴水平且与加速度计的敏感轴垂直；
步骤4.2：控制旋转转台以角速率ω匀速旋转，使加速度计(4)的敏感轴在铅锤面内转动；
步骤4.3：将重力加速度在不同位置的分量的输入表示为：
将加速度计的输出表示为：将式
带入式可得：
上式中：g为重力加速度，θ为旋转转台的转角传感器输出，Aout为加速度计的信号输出，k0为加速度计零偏，k1为刻度系数，k2为刻度系数的二次非线性误差系数，k3为刻度系数的三次非线性误差系数，其中利用递推最小二乘法估计k0、k1、k2、k3的值。
5.根据权利要求4所述的一种基于卡尔曼滤波算法的三维定位方法，其特征在于：所述步骤二中对气压高度计(5)采集的数据进行误差补偿的具体方法为：
定义在从海平面H＝0m到H＝11000m范围内的大气压强P和不同高度处的标准气压高度H的关系式为：上式中P0为海平面大气压力，T0为海平面大气温度，β1为温度梯度，β1＝0.0065K/m，gn为重力加速度；
根据如下公式计算补偿后的温度值Temp，
上式中：D2为温度值，C5为参考温度值，C6为测量温度系数；
对上述温度补偿后的温度值Temp进行非线性补偿，具体通过二阶修正系数对温度值和压力值进行二次修正，计算公式如下：
Temp＝Temp-T2
上式中：T2为温度补偿常数，C1为压力灵敏度值，C2为压力偏置值，C3为压力灵敏度的温度系数，C4为压力偏置温度系数，C5为参考温度值，C6为测量温度系数，D1为压力值，D2为温度值，TCS为压敏温度系数，SENS2为温度补偿灵敏度，OFF2为温度补偿误差。
6.根据权利要求5所述的一种基于卡尔曼滤波算法的三维定位方法，其特征在于：所述步骤三中采用扩展卡尔曼滤波算法对处理后的姿态角误差与四元数法得到的姿态角进行误差修正包括通过加速度计(4)和磁强计(2)的数据修正陀螺仪(3)积分角误差、通过加速度计(4)和陀螺仪(3)逻辑计算对定位目标的静态步态检测进行重置和补偿，所述静态步态检测的具体步骤如下：
步骤6.1：将加速度计(4)采集的加速度数据和陀螺仪(3)采集的角速度数据作为静态步态检测的输入信号发送至姿态测量模块；
步骤6.2：姿态测量模块将X轴加速度信号和Y轴角速度信号通过逻辑“与”关系后的逻辑值作为判断定位目标处于静态或者动态的依据发送至中央控制器(1)，所述逻辑值的具体判断步骤为：
定义C1为X轴加速度信号的逻辑阈值，定义C2为Y轴角速度信号的逻辑阈值，其中
C＝C1&C2，其中C为静态步态检测逻辑值。
7.根据权利要求6所述的一种基于卡尔曼滤波算法的三维定位方法，其特征在于：所述步骤四中的15元素的误差状态向量导航算法包括如下步骤：
步骤7.1：设15元素误差状态向量在k时刻为：
上式中为k时刻的三维姿态误差，为k时刻的角速率误差，δrk为k时刻的位置误差，δvk为k时刻的速度误差，为k时刻的加速度误差；
步骤7.2：将公式线性化后得到状态方程为：δX k/k-1＝ΦkδX k-1/k-1+w k-1，其中，δX k/k-1是预测误
差值，δX k-1/k-1是k-1时刻卡尔曼滤波最优估计值，w k-1是方差为Q k＝E(w k w k T)的过程噪声，Φk是15×15的状态转移矩阵，Φk的计算公式如下式所示：
上式中为经过坐标转换后加速度计经过误差补偿后的输出，
是基于加速度计测量的斜对称矩阵，的计算公式如下：
步骤7.3：建立观测方程：Z k＝HδX k/k+n k，
其中：Z k是误差测量值，H是测量矩阵，δX k/k为k时刻滤波误差估计值，n k是方差为R k＝E (n k n k T)的测量噪声，
所述测量矩阵H的计算公式为：
所述测量矩阵H对应的观测向量为m k，m k的计算公式为：
m k＝[ΔA k，Δωk，Δr zk，Δv k]，
其中：ΔA k为三维姿态角至，Δωk为三维角速度值，Δr zk为高度值，Δv k为三维速度值；
所述δX k/k通过卡尔曼滤波器状态更新方程由前一时刻得到，δX k/k的计算公式为：
δX k/k＝δX k/k-1+K k·[m k-HδX k/k-1]，
其中K k为卡尔曼滤波增益，m k为实际误差测量值，δX k/k-1为一步预测误差状态值，卡尔曼滤波增益K k通过以下公式计算：
K k＝P k/k-1H T(HP k/k-1H T+R k)-1，
其中：P k/k-1为误差估计协方差矩阵，由k-1时刻的测量值计算得到，所述P k/k-1的计算公式为：
P k/k-1＝Φk-1P k-1/k-1Φk-1T+Q k-1；
步骤7.4：根据步骤7.1、步骤7.2的线性化后得到状态方程得到协方差矩阵P k/k的计算公式为：
P k/k＝(I15×15-K k H)P k/k-1(I15×15-K k H)T+R k，
其中：I15×15为15*15的矩阵。
步骤7.5：在确定定位目标当前的速度、姿态、位置信息后将15元素误差状态向量δX k/k 置0，三维定位系统的观测量根据卡尔曼滤波方程不断更新系统的状态，从而得到定位目标的实时三维信息。
8.一种基于卡尔曼滤波算法的三维定位系统，其特征在于：包括佩戴于定位目标上的环形壳体，所述壳体内部设置有电路控制板、姿态测量模块、导航测控模块，所述电路控制板上集成有中央控制器(1)，
所述姿态测量模块包括磁强计(2)、陀螺仪(3)、加速度计(4)，所述导航测控模块包括气压高度计(5)、空速计(6)、GPS(7)；
所述中央控制器(1)通过导线分别与时钟模块(8)、无线通信模块(9)、AD转换模块(10)、数据存储模块(11)、电源管理模块(12)相连；
所述AD转换模块(10)通过导线分别与磁强计(2)、陀螺仪(3)、加速度计(4)、气压高度计(5)、空速计(6)、GPS(7)相连；
所述壳体外侧设置有SD卡接口、USB接口，所述壳体上还设置有显示屏(13)，所述显示屏(13)通过导线与中央控制器(1)连接。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议