基于零速修正的闭环卡尔曼滤波惯性定位方法技术

本发明专利技术提供的是一种基于零速修正的闭环卡尔曼惯性定位方法。通过GPS确定载体的初始位置参数，并装订至导航计算机中；惯性导航系统进行预热准备，采集陀螺仪和加速度计输出的数据并对数据进行处理；利用水平加速度计敏感的重力分量确定载体水平姿态角；在惯性导航系统输出的每个离散时刻，根据加速度均值和方差确定载体运动状态；载体处于静止状态时，采用闭环卡尔曼滤波实现惯导系统解算误差的修正；采用直角坐标系的位置更新算法完成载体高精度定位。

【技术实现步骤摘要】
【专利摘要】本专利技术提供的是一种基于零速修正的闭环卡尔曼惯性定位方法。通过GPS确定载体的初始位置参数，并装订至导航计算机中；惯性导航系统进行预热准备，采集陀螺仪和加速度计输出的数据并对数据进行处理；利用水平加速度计敏感的重力分量确定载体水平姿态角；在惯性导航系统输出的每个离散时刻，根据加速度均值和方差确定载体运动状态；载体处于静止状态时，采用闭环卡尔曼滤波实现惯导系统解算误差的修正；采用直角坐标系的位置更新算法完成载体高精度定位。【专利说明】(—)
本专利技术涉及的是一种测量方法，尤其涉及的是一种。 (二)
技术介绍
惯性导航系统，利用惯性元件来测量运载体本身的加速度，经过积分和运算得到速度和位置，从而达到对运载体导航定位的目的。组成惯性导航系统的设备都安装在运载体内，工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰，是一种自主式导航系统。 但是不可避免的问题是，惯性导航系统中的惯性元件是由陀螺仪和加速度计组成，而陀螺仪和加速度计自身存在不可避免的误差因数导致采用积分和运算得到速度和位置的信息中包含了随时间积累的误差，这将直接影响惯导系统提供速度和位置信息的精度，如何在现有惯性器件精度基础上实现更高精度定位成为当前研究热点。 目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计，也可以是对于将来位置的估计。状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。采用单一工作模式的惯性导航系统是没有外部信息可为其提供参考，导致采用卡尔曼实现误差估计无法实现，针对如上问题，本专利技术专利提出一种采用载体静止状态下的速度信息作为真实速度参考，完成卡尔曼滤波组合解算，以此实现高精度惯性定位。 (三)
技术实现思路
本专利技术的技术解决问题是:克服现有技术的不足，提供一种。 本专利技术的技术解决方案为:一种，其特征在于根据水平加速度计测量的当地重力加速度分量确定水平姿态角，根据比力幅值的均值和方差确定载体运动状态，通过闭环卡尔曼滤波实现惯性导航系统的误差修正，一次实现更高精度定位。其具体步骤如下: (I)利用全球定位系统GPS确定载体的初始位置参数，将它们装订至导航计算机中； (2)系统进行预热准备，采集陀螺仪和加速度计输出的数据并对数据进行处理； (3)惯性导航系统初始对准； 根据加速度计的输出可完成水平对准，由加速度计输出解算初始的俯仰角和横滚角，计算公式如下: ^ = arcsin(/x /g0) 7 = arcsin 式中:g(l为地表面的重力加速度，fx、fz分别表示X向和y向的加速度计输出比力信息，Θ、y分别表示对准结束时刻得到的俯仰角和横滚角估计结果； (4)载体的静态检测； I)计算比力幅值:在惯性导航系统输出的每个离散时刻，计算当前时刻tm处的加速度计输出比力幅值: I fm I=知J2+(/Λ)2+(Λ U2 式中:fxtm、fytm、fztm分别表示tm时亥|J X向、y向和ζ向加速度计输出比力； 2)计算判断指标:求取计算区间时间段内的比力幅值的均值TL及滑动方差Varm:Z1=-lT Σ I/I 所I + I i=m~m, < mVarm =J Qfi1-Jm)2 w, +1 式中，IH1表示区间长度，根据加速度计的输出频率进行确定； 3)运动状态判定:根据加速度计输出噪声的方差特性，设定方差阈值为Gatev,当Varm < Gatev,则判定当前时刻运动状态为静止状态,否则运动状态判定为运动状态； (5)闭环卡尔曼滤波器设计； 零速修正由载体处于静止状态时进行检测触发，即通过在检测为静止的时间区间内将速度计算结果重置为零，达到修正卡尔曼组合滤波中速度误差的目的；为充分利用静止检测的检测结果估计更多的误差参数，设计零速修正卡尔曼滤波器，结合零速修正工作原理，对卡尔曼滤波器做了如下改良:在卡尔曼更新时刻，若静态检测结果为运动状态，则滤波器只进行时间更新；若静态检测结果为静止状态，则滤波器做完整更新(即时间更新+量测更新)，并闭环修正惯性导航系统的速度误差、位置误差、姿态误差及器件误差； I)离散化后的状态方程和量测方程分别为: r ^ =H1+L i [Zk= HkXk+Vk 式中，Xk、Xlri分别表示k时亥IJ、k-Ι时刻的状态估计；Zk为离散化系统观测矩阵；fI3kjk-1为离散化状态转移矩阵；Hk为离散化系统量测矩阵和Vk分别为离散化系统状态噪声向量和量测噪声向量； 2)时间更新 状态一步预测Xk^1为: Xk/H = Φ,,ηΧη 一步预测均方误差Pk^为: Pklk-' = Φ^Α-1Kk-1 + Qki 式中，Pk_!表不k-Ι时刻的均方误差！Qlrf表不系统噪声协方差矩阵； 3)量测更新 滤波增益Kk为: Kk = Pklk_'HTk _〗 k时刻状态估计Xk为: Xk = UKk(Zk-HkX1^1) 估计参数误差协方差矩阵Pk为: Pk=(1-KkHk)PkJ1-KkHJ +KkRkKl 式中，Rk表示量测噪声协方差矩阵；1表示单位矩阵； (6)速度与位置信息更新； 传统惯性系统的速度更新中需要从加速度计输出中去除有害加速度的影响，惯性导航系统的加速度计零偏较大，并远远大于有害加速度的影响，因此微惯性导航系统速度更新公式可简化为: ^(k+i)=rF(k)+TMk+\) K(k+r)=W+Ts rR(k+i)=rR(k)+TMk+i) 式中:F;(A: + 1)、FJ(A: +1), FR”(A: + 1)表示k+1时刻导航坐标系三个方向速度的计算值；G(幻、K W、幻表示k时刻导航坐标系三个方向速度；Ts表示更新周期；f；(k+ 1) ,/^(^ +1)、Λ”(Α: + 1)表示k+ι时刻得到的导航坐标系内的加速度值； 采用直角坐标系的位置更新算法如下: F(/c + l) = F(k) + j- .U(k +1) 二 U(Jc) + ^ + 1) = ^) + 1 式中:F(k+l)、U(k+1)、R(k+1)分别表示k+1时刻导航系三个方向的位移计算值；F (k)、U (k)、R (k)分别表示k时刻导航系三个方向的位移计算值。 本专利技术与现有技术相比的优点在于:本专利技术打破了传统惯性导航系统解算载体位置信息时，由于惯性器件偏差的随时间积累导致产生的位置信息无法满足实际应用的问题，通过利用加速度计敏感重力分量实现惯性导航系统初始化，并依据比力均值和方差实现惯性导航系统运动状态的检测，通过闭环卡尔曼滤波实现惯导系统的高精度位置提取。 对本专利技术有益的效果说明如下: 在VC++条件下，对该方法进行实验:为有效验证本文提出的误差修正技术对于提高惯性导航系统定位精度的可行性及其对不同空间环境的适应性，开展二维平面行走实验。试验人员所行走的路径为走廊区域，具有典型的代表性，可直观地检测出惯性导航系统本文档来自技高网...

【技术保护点】
一种基于零速修正的闭环卡尔曼滤波惯性定位方法，其特征在于包括以下步骤：(1)利用全球定位系统GPS确定载体的初始位置参数，将它们装订至导航计算机中；(2)系统进行预热准备，采集陀螺仪和加速度计输出的数据并对数据进行处理；(3)惯性导航系统初始对准；根据加速度计的输出可完成水平对准，由加速度计输出解算初始的俯仰角和横滚角，计算公式如下：θ=arcsin(fx/g0)γ=arcsin[-fz/(cosθ·g0)]]]>式中：g0为地表面的重力加速度，fx、fz分别表示x向和y向的加速度计输出比力信息，θ、γ分别表示对准结束时刻得到的俯仰角和横滚角估计结果；(4)载体的静态检测；1)计算比力幅值：在惯性导航系统输出的每个离散时刻，计算当前时刻tm处的加速度计输出比力幅值：|fm|=(fxtm)2+(fytm)2+(fztm)2]]>式中：fxtm、fytm、fztm分别表示tm时刻x向、y向和z向加速度计输出比力；2)计算判断指标：求取计算区间时间段内的比力幅值的均值及滑动方差Varm：f‾m=1m1+1Σi=m-m1m|fi|Varm=1m1+1Σi=m-m1m(|fi|-f‾m)2]]>式中，m1表示区间长度，根据加速度计的输出频率进行确定；3)运动状态判定：根据加速度计输出噪声的方差特性，设定方差阈值为GateV，当Varm＜GateV，则判定当前时刻运动状态为静止状态，否则运动状态判定为运动状态；(5)闭环卡尔曼滤波器设计；零速修正由载体处于静止状态时进行检测触发，即通过在检测为静止的时间区间内将速度计算结果重置为零，达到修正卡尔曼组合滤波中速度误差的目的；为充分利用静止检测的检测结果估计更多的误差参数，设计零速修正卡尔曼滤波器，结合零速修正工作原理，对卡尔曼滤波器做了如下改良：在卡尔曼更新时刻，若静态检测结果为运动状态，则滤波器只进行时间更新；若静态检测结果为静止状态，则滤波器做完整更新(即时间更新+量测更新)，并闭环修正惯性导航系统的速度误差、位置误差、姿态误差及器件误差；1)离散化后的状态方程和量测方程分别为：Xk=Φk,k-1Xk-1+Wk-1Zk=HkXk+Vk]]>式中，Xk、Xk‑1分别表示k时刻、k‑1时刻的状态估计；Zk为离散化系统观测矩阵；Φk，k‑1为离散化状态转移矩阵；Hk为离散化系统量测矩阵；Wk‑1和Vk分别为离散化系统状态噪声向量和量测噪声向量；2)时间更新状态一步预测Xk/k‑1为：Xk/k‑1＝Φk，k‑1Xk‑1一步预测均方误差Pk/k‑1为：Pk/k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1]]>式中，Pk‑1表示k‑1时刻的均方误差；Qk‑1表示系统噪声协方差矩阵；3)量测更新滤波增益Kk为：Kk=Pk/k-1HkT[HkPk/k-1HkT+Rk]-1]]>k时刻状态估计Xk为：Xk＝Xk/k‑1+Kk(Zk‑HkXk/k‑1)估计参数误差协方差矩阵Pk为：Pk=(I-KkHk)Pk/k-1(I-KkHk)T+KkRkKkT]]>式中，Rk表示量测噪声协方差矩阵；I表示单位矩阵；(6)速度与位置信息更新；传统惯性系统的速度更新中需要从加速度计输出中去除有害加速度的影响，微惯性导航系统的加速度计零偏较大，并远远大于有害加速度的影响，因此微惯性导航系统速度更新公式可简化为：VFn(k+1)=VFn(k)+TsfFn(k+1)VUn(k+1)=VUn(k)+Ts[fUn(k+1)-g0]VRn(k+1)=VRn(k)+TsfRn(k+1)]]>式中：表示k+1时刻导航坐标系三个方向速度的计算值；表示k时刻导航坐标系三个方向速度；Ts表示更新周期；表示k+1时刻得到的导航坐标系内的加速度值；采用直角坐标系的位置更新算法如下：F(k+1)=F(k)+Ts2[VFn(k)+VFn(k+1)]U(k+1)=U(k)+Ts2[VUn(k)+VUn(k+1)]R(k+1)=R(k)+Ts2[VRn(k)+VRn(k+1)]]]>式中：F(k+1)、U(k+1)、R(k+1)分别表示k+1时刻导航系三个方向的位移计算值；F(k)、U(k)、R(k)分别表示k时刻导航系三个方向的位移计算值。...

【技术特征摘要】

【专利技术属性】
技术研发人员：孙伟，
申请(专利权)人：辽宁工程技术大学，
类型：发明
国别省市：辽宁;21