CN104132662A - 基于零速修正的闭环卡尔曼滤波惯性定位方法 - Google Patents

Abstract

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

Description

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

(一)技术领域

本发明涉及的是一种测量方法，尤其涉及的是一种基于零速修正的闭环卡尔曼滤波惯性定位方法。

(二)背景技术

惯性导航***，利用惯性元件来测量运载体本身的加速度，经过积分和运算得到速度和位置，从而达到对运载体导航定位的目的。组成惯性导航***的设备都安装在运载体内，工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰，是一种自主式导航***。

但是不可避免的问题是，惯性导航***中的惯性元件是由陀螺仪和加速度计组成，而陀螺仪和加速度计自身存在不可避免的误差因数导致采用积分和运算得到速度和位置的信息中包含了随时间积累的误差，这将直接影响惯导***提供速度和位置信息的精度，如何在现有惯性器件精度基础上实现更高精度定位成为当前研究热点。

目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计，也可以是对于将来位置的估计。状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。采用单一工作模式的惯性导航***是没有外部信息可为其提供参考，导致采用卡尔曼实现误差估计无法实现，针对如上问题，本发明专利提出一种采用载体静止状态下的速度信息作为真实速度参考，完成卡尔曼滤波组合解算，以此实现高精度惯性定位。

(三)发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种基于零速修正的闭环卡尔曼滤波惯性定位方法。

本发明的技术解决方案为：一种基于零速修正的闭环卡尔曼滤波惯性定位方法，其特征在于根据水平加速度计测量的当地重力加速度分量确定水平姿态角，根据比力幅值的均值和方差确定载体运动状态，通过闭环卡尔曼滤波实现惯性导航***的误差修正，一次实现更高精度定位。其具体步骤如下：

(1)利用全球定位***GPS确定载体的初始位置参数，将它们装订至导航计算机中；

(2)***进行预热准备，采集陀螺仪和加速度计输出的数据并对数据进行处理；

(3)惯性导航***初始对准；

根据加速度计的输出可完成水平对准，由加速度计输出解算初始的俯仰角和横滚角，计算公式如下：

$\left\{\begin{array}{c}\mathrm{\θ}=\arcsin (f_x/g_0)\\ \mathrm{\γ}=\arcsin [-f_z/(\cos \mathrm{\θ}\·g_0)]\end{array}\right.$

式中：g₀为地表面的重力加速度，f_x、f_z分别表示x向和y向的加速度计输出比力信息，θ、γ分别表示对准结束时刻得到的俯仰角和横滚角估计结果；

(4)载体的静态检测；

1)计算比力幅值：在惯性导航***输出的每个离散时刻，计算当前时刻t_m处的加速度计输出比力幅值：

$\left|f_m\right|=\sqrt{{\left(f_x{t}_m\right)}^2+{\left(f_y{t}_m\right)}^2+{\left(f_z{t}_m\right)}^2}$

式中：f_xt_m、f_yt_m、f_zt_m分别表示t_m时刻x向、y向和z向加速度计输出比力；

2)计算判断指标：求取计算区间时间段内的比力幅值的均值及滑动方差Var_m：

$\left\{\begin{array}{c}{\stackrel{\‾}{f}}_m=\frac{1}{m_1+1}\underset{i=m-m_1}{\overset{m}{\mathrm{\Σ}}}\left|f_i\right|\\ {\mathrm{Var}}_m=\frac{1}{m_1+1}\underset{i=m-m_1}{\overset{m}{\mathrm{\Σ}}}{\left(\right|f_i|-{\stackrel{\‾}{f}}_m)}^2\end{array}\right.$

式中，m₁表示区间长度，根据加速度计的输出频率进行确定；

3)运动状态判定：根据加速度计输出噪声的方差特性，设定方差阈值为Gate_V，当Var_m＜Gate_V，则判定当前时刻运动状态为静止状态，否则运动状态判定为运动状态；

(5)闭环卡尔曼滤波器设计；

零速修正由载体处于静止状态时进行检测触发，即通过在检测为静止的时间区间内将速度计算结果重置为零，达到修正卡尔曼组合滤波中速度误差的目的；为充分利用静止检测的检测结果估计更多的误差参数，设计零速修正卡尔曼滤波器，结合零速修正工作原理，对卡尔曼滤波器做了如下改良：在卡尔曼更新时刻，若静态检测结果为运动状态，则滤波器只进行时间更新；若静态检测结果为静止状态，则滤波器做完整更新(即时间更新+量测更新)，并闭环修正惯性导航***的速度误差、位置误差、姿态误差及器件误差；

1)离散化后的状态方程和量测方程分别为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

式中，X_k、X_k-1分别表示k时刻、k-1时刻的状态估计；Z_k为离散化***观测矩阵；Φ_k，k-1为离散化状态转移矩阵；H_k为离散化***量测矩阵；W_k-1和V_k分别为离散化***状态噪声向量和量测噪声向量；

2)时间更新

状态一步预测X_k/k-1为：

X_k/k-1＝Φ_k，k-1X_k-1

一步预测均方误差P_k/k-1为：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

式中，P_k-1表示k-1时刻的均方误差；Q_k-1表示***噪声协方差矩阵；

3)量测更新

滤波增益K_k为：

$K_k=P_{k/k-1}{H}_k^T{[H_k{P}_{k/k-1}{H_k}^T+R_k]}^{-1}$

k时刻状态估计X_k为：

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

估计参数误差协方差矩阵P_k为：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

式中，R_k表示量测噪声协方差矩阵；I表示单位矩阵；

(6)速度与位置信息更新；

传统惯性***的速度更新中需要从加速度计输出中去除有害加速度的影响，惯性导航***的加速度计零偏较大，并远远大于有害加速度的影响，因此微惯性导航***速度更新公式可简化为：

$\left\{\begin{array}{c}{V}_F^n(k+1)=V_F^n\left(k\right)+T_s{f}_F^n(k+1)\\ V_U^n(k+1)=V_U^n\left(k\right)+T_s[f_U^n(k+1)-g_0]\\ V_R^n(k+1)=V_R^n\left(k\right)+T_s{f}_R^n(k+1)\end{array}\right.$

式中：表示k+1时刻导航坐标系三个方向速度的计算值；表示k时刻导航坐标系三个方向速度；T_s表示更新周期； 表示k+1时刻得到的导航坐标系内的加速度值；

采用直角坐标系的位置更新算法如下：

$\left\{\begin{array}{c}F(k+1)=F\left(k\right)+\frac{T_s}{2}[V_F^n\left(k\right)+V_F^n(k+1)]\\ U(k+1)=U\left(k\right)+\frac{T_s}{2}[V_U^n\left(k\right)+V_U^n(k+1)]\\ R(k+1)=R\left(k\right)+\frac{T_s}{2}[V_R^n\left(k\right)+V_R^n(k+1)]\end{array}\right.$

式中：F(k+1)、U(k+1)、R(k+1)分别表示k+1时刻导航系三个方向的位移计算值；F(k)、U(k)、R(k)分别表示k时刻导航系三个方向的位移计算值。

本发明与现有技术相比的优点在于：本发明打破了传统惯性导航***解算载***置信息时，由于惯性器件偏差的随时间积累导致产生的位置信息无法满足实际应用的问题，通过利用加速度计敏感重力分量实现惯性导航***初始化，并依据比力均值和方差实现惯性导航***运动状态的检测，通过闭环卡尔曼滤波实现惯导***的高精度位置提取。

对本发明有益的效果说明如下：

在VC++条件下，对该方法进行实验：为有效验证本文提出的误差修正技术对于提高惯性导航***定位精度的可行性及其对不同空间环境的适应性，开展二维平面行走实验。试验人员所行走的路径为走廊区域，具有典型的代表性，可直观地检测出惯性导航***所给出的定位结果与实际路径的符合程度。利用本发明所述方法得到惯性导航***解算位移与预订行走路线具有较高的重合度，采用本发明方法可以获得较高的定位精度，北向位移与东向位移的最大偏差小于0.4米(附图3)。

(四)附图说明

图1为本发明的基于零速修正的闭环卡尔曼滤波惯性定位方法流程图；

图2为本发明的加速度计比力输出的幅值变化图；

图3为本发明的惯性导航***解算位置与理论位置对比曲线；

(五)具体实施方式

下面结合附图对本发明的具体实施方式进行详细地描述：

(1)利用全球定位***GPS确定载体的初始位置参数，将它们装订至导航计算机中；

(2)***进行预热准备，采集陀螺仪和加速度计输出的数据并对数据进行处理；

(3)惯性导航***初始对准；

根据加速度计的输出可完成水平对准，由加速度计输出解算初始的俯仰角和横滚角，计算公式如下：

$\left\{\begin{array}{c}\mathrm{\θ}=\arcsin (f_x/g_0)\\ \mathrm{\γ}=\arcsin [-f_z/(\cos \mathrm{\θ}\·g_0)]\end{array}\right.---\left(1\right)$

式中：g₀为地表面的重力加速度，f_x、f_z分别表示x向和y向的加速度计输出比力信息，θ、γ分别表示对准结束时刻得到的俯仰角和横滚角估计结果；

(4)载体的静态检测；

1)计算比力幅值：在惯性导航***输出的每个离散时刻，计算当前时刻t_m处的加速度计输出比力幅值：

$\left|f_m\right|=\sqrt{{\left(f_x{t}_m\right)}^2+{\left(f_y{t}_m\right)}^2+{\left(f_z{t}_m\right)}^2}---\left(2\right)$

式中：f_xt_m、f_yt_m、f_zt_m分别表示t_m时刻x向、y向和z向加速度计输出比力；

2)计算判断指标：求取计算区间时间段内的比力幅值的均值及滑动方差Var_m：

$\left\{\begin{array}{c}{\stackrel{\‾}{f}}_m=\frac{1}{m_1+1}\underset{i=m-m_1}{\overset{m}{\mathrm{\Σ}}}\left|f_i\right|\\ {\mathrm{Var}}_m=\frac{1}{m_1+1}\underset{i=m-m_1}{\overset{m}{\mathrm{\Σ}}}{\left(\right|f_i|-{\stackrel{\‾}{f}}_m)}^2\end{array}\right.---\left(3\right)$

式中，m₁表示区间长度，根据加速度计的输出频率进行确定；

3)运动状态判定：根据加速度计输出噪声的方差特性，设定方差阈值为Gate_V，当Var_m＜Gate_V，则判定当前时刻运动状态为静止状态，否则运动状态判定为运动状态；

(5)闭环卡尔曼滤波器设计；

零速修正由载体处于静止状态时进行检测触发，即通过在检测为静止的时间区间内将速度计算结果重置为零，达到修正卡尔曼组合滤波中速度误差的目的；为充分利用静止检测的检测结果估计更多的误差参数，设计零速修正卡尔曼滤波器，结合零速修正工作原理，对卡尔曼滤波器做了如下改良：在卡尔曼更新时刻，若静态检测结果为运动状态，则滤波器只进行时间更新；若静态检测结果为静止状态，则滤波器做完整更新(即时间更新+量测更新)，并闭环修正惯性导航***的速度误差、位置误差、姿态误差及器件误差；

1)离散化后的状态方程和量测方程分别为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.---\left(4\right)$

式中，X_k、X_k-1分别表示k时刻、k-1时刻的状态估计；Z_k为离散化***观测矩阵；Φ_k，k-1为离散化状态转移矩阵；H_k为离散化***量测矩阵；W_k-1和V_k分别为离散化***状态噪声向量和量测噪声向量；

2)时间更新

状态一步预测X_k/k-1为：

X_k/k-1＝Φ_k，k-1X_k-1  (5)

一步预测均方误差P_k/k-1为：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}---\left(6\right)$

式中，P_k-1表示k-1时刻的均方误差；Q_k-1表示***噪声协方差矩阵；

3)量测更新

滤波增益K_k为：

$K_k=P_{k/k-1}{H}_k^T{[H_k{P}_{k/k-1}{H_k}^T+R_k]}^{-1}---\left(7\right)$

k时刻状态估计X_k为：

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)  (8)

估计参数误差协方差矩阵P_k为：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(9\right)$

式中，R_k表示量测噪声协方差矩阵；I表示单位矩阵；

(6)速度与位置信息更新；

传统惯性***的速度更新中需要从加速度计输出中去除有害加速度的影响，微惯性导航***的加速度计零偏较大，并远远大于有害加速度的影响，因此微惯性导航***速度更新公式可简化为：

$\left\{\begin{array}{c}{V}_F^n(k+1)=V_F^n\left(k\right)+T_s{f}_F^n(k+1)\\ V_U^n(k+1)=V_U^n\left(k\right)+T_s[f_U^n(k+1)-g_0]\\ V_R^n(k+1)=V_R^n\left(k\right)+T_s{f}_R^n(k+1)\end{array}\right.---\left(10\right)$

式中：表示k+1时刻导航坐标系三个方向速度的计算值；表示k时刻导航坐标系三个方向速度；T_s表示更新周期； 表示k+1时刻得到的导航坐标系内的加速度值；

采用直角坐标系的位置更新算法如下：

$\left\{\begin{array}{c}F(k+1)=F\left(k\right)+\frac{T_s}{2}[V_F^n\left(k\right)+V_F^n(k+1)]\\ U(k+1)=U\left(k\right)+\frac{T_s}{2}[V_U^n\left(k\right)+V_U^n(k+1)]\\ R(k+1)=R\left(k\right)+\frac{T_s}{2}[V_R^n\left(k\right)+V_R^n(k+1)]\end{array}\right.---\left(11\right)$

式中：F(k+1)、U(k+1)、R(k+1)分别表示k+1时刻导航系三个方向的位移计算值；F(k)、U(k)、R(k)分别表示k时刻导航系三个方向的位移计算值。

Claims (1)

1.一种基于零速修正的闭环卡尔曼滤波惯性定位方法，其特征在于包括以下步骤：

(1)利用全球定位***GPS确定载体的初始位置参数，将它们装订至导航计算机中；

(2)***进行预热准备，采集陀螺仪和加速度计输出的数据并对数据进行处理；

(3)惯性导航***初始对准；

根据加速度计的输出可完成水平对准，由加速度计输出解算初始的俯仰角和横滚角，计算公式如下：

$\left\{\begin{array}{c}\mathrm{\θ}=\arcsin (f_x/g_0)\\ \mathrm{\γ}=\arcsin [-f_z/(\cos \mathrm{\θ}\·g_0)]\end{array}\right.$

式中：g₀为地表面的重力加速度，f_x、f_z分别表示x向和y向的加速度计输出比力信息，θ、γ分别表示对准结束时刻得到的俯仰角和横滚角估计结果；

(4)载体的静态检测；

1)计算比力幅值：在惯性导航***输出的每个离散时刻，计算当前时刻t_m处的加速度计输出比力幅值：

$\left|f_m\right|=\sqrt{{\left(f_x{t}_m\right)}^2+{\left(f_y{t}_m\right)}^2+{\left(f_z{t}_m\right)}^2}$

式中：f_xt_m、f_yt_m、f_zt_m分别表示t_m时刻x向、y向和z向加速度计输出比力；

2)计算判断指标：求取计算区间时间段内的比力幅值的均值及滑动方差Var_m：

$\left\{\begin{array}{c}{\stackrel{\‾}{f}}_m=\frac{1}{m_1+1}\underset{i=m-m_1}{\overset{m}{\mathrm{\Σ}}}\left|f_i\right|\\ {\mathrm{Var}}_m=\frac{1}{m_1+1}\underset{i=m-m_1}{\overset{m}{\mathrm{\Σ}}}{\left(\right|f_i|-{\stackrel{\‾}{f}}_m)}^2\end{array}\right.$

式中，m₁表示区间长度，根据加速度计的输出频率进行确定；

3)运动状态判定：根据加速度计输出噪声的方差特性，设定方差阈值为Gate_V，当Var_m＜Gate_V，则判定当前时刻运动状态为静止状态，否则运动状态判定为运动状态；

(5)闭环卡尔曼滤波器设计；

零速修正由载体处于静止状态时进行检测触发，即通过在检测为静止的时间区间内将速度计算结果重置为零，达到修正卡尔曼组合滤波中速度误差的目的；为充分利用静止检测的检测结果估计更多的误差参数，设计零速修正卡尔曼滤波器，结合零速修正工作原理，对卡尔曼滤波器做了如下改良：在卡尔曼更新时刻，若静态检测结果为运动状态，则滤波器只进行时间更新；若静态检测结果为静止状态，则滤波器做完整更新(即时间更新+量测更新)，并闭环修正惯性导航***的速度误差、位置误差、姿态误差及器件误差；

1)离散化后的状态方程和量测方程分别为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

式中，X_k、X_k-1分别表示k时刻、k-1时刻的状态估计；Z_k为离散化***观测矩阵；Φ_k，k-1为离散化状态转移矩阵；H_k为离散化***量测矩阵；W_k-1和V_k分别为离散化***状态噪声向量和量测噪声向量；

2)时间更新

状态一步预测X_k/k-1为：

X_k/k-1＝Φ_k，k-1X_k-1

一步预测均方误差P_k/k-1为：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

式中，P_k-1表示k-1时刻的均方误差；Q_k-1表示***噪声协方差矩阵；

3)量测更新

滤波增益K_k为：

$K_k=P_{k/k-1}{H}_k^T{[H_k{P}_{k/k-1}{H_k}^T+R_k]}^{-1}$

k时刻状态估计X_k为：

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

估计参数误差协方差矩阵P_k为：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

式中，R_k表示量测噪声协方差矩阵；I表示单位矩阵；

(6)速度与位置信息更新；

传统惯性***的速度更新中需要从加速度计输出中去除有害加速度的影响，微惯性导航***的加速度计零偏较大，并远远大于有害加速度的影响，因此微惯性导航***速度更新公式可简化为：

$\left\{\begin{array}{c}{V}_F^n(k+1)=V_F^n\left(k\right)+T_s{f}_F^n(k+1)\\ V_U^n(k+1)=V_U^n\left(k\right)+T_s[f_U^n(k+1)-g_0]\\ V_R^n(k+1)=V_R^n\left(k\right)+T_s{f}_R^n(k+1)\end{array}\right.$

式中：表示k+1时刻导航坐标系三个方向速度的计算值；表示k时刻导航坐标系三个方向速度；T_s表示更新周期； 表示k+1时刻得到的导航坐标系内的加速度值；

采用直角坐标系的位置更新算法如下：

$\left\{\begin{array}{c}F(k+1)=F\left(k\right)+\frac{T_s}{2}[V_F^n\left(k\right)+V_F^n(k+1)]\\ U(k+1)=U\left(k\right)+\frac{T_s}{2}[V_U^n\left(k\right)+V_U^n(k+1)]\\ R(k+1)=R\left(k\right)+\frac{T_s}{2}[V_R^n\left(k\right)+V_R^n(k+1)]\end{array}\right.$

式中：F(k+1)、U(k+1)、R(k+1)分别表示k+1时刻导航系三个方向的位移计算值；F(k)、U(k)、R(k)分别表示k时刻导航系三个方向的位移计算值。