CN100541134C - 利用gps与陀螺仪、里程计的组合定位方法与装置 - Google Patents

Abstract

本发明属GPS定位技术领域，具体为一种利用GPS与MEMS陀螺仪、里程计的组合定位方法与装置。该装置在GPS信号良好时利用GPS定位信号进行定位，MEMS陀螺仪、里程计进行校正，采用卡尔曼滤波算法进行信号综合；当GPS信号丢失或者接收卫星数少于3颗时，切换至MEMS陀螺仪、里程计定位。采用组合导航可以提高GPS定位的精度，并弥补了GPS在受遮挡情况下无法定位的缺陷。

Description

利用GPS与陀螺仪、里程计的组合定位方法与装置

技术领域

本发明属于GPS定位技术领域，具体涉及一种利用MEMS陀螺仪、里程计对GPS定位信号进行校正，并在GPS丢星状态时切换定位方式，实现不间断定位的组合定位方法与装置。

背景技术

当前，GPS定位系统已经被广泛采用，GPS定位精度与接收到的卫星信号强度有关。在接收到超过3颗卫星信号时，定位准确；在接收到3颗卫星信号时，定位比较勉强；在接收到卫星信号少于3颗时，无法输出定位信息。由于地理环境的限制，在山岭、隧道、城市建筑群等遮挡环境中GPS接收器接收到的信号强度会大幅降低，甚至会找不到卫星信号。因此单独GPS定位系统定位精度在这种情况下会受到很大影响，需要其他定位方式进行补偿与校正。

发明内容

本发明的目的在于提供定位精度高，并可解决GPS在受遮挡情况下无法定位缺陷的定位方法与装置。

本发明提出的定位方法，是一种利用GPS与MEMS陀螺仪、里程计的组合定位的方法，并采用卡尔曼滤波算法对定位数据进行最优估计。其中采用MEMS陀螺仪和里程计对GPS定位进行补偿和校正，MEMS陀螺仪相对于传统陀螺仪具有在保持精度的前提下减小尺寸、减小功耗，能够有效地解决GPS在受遮挡情况下无法定位的缺陷，切换时间小，使系统能够持续不间断定位工作；同时，在GPS信号强度正常的情况下(GPS接收卫星信号为了颗时)，利用卡尔曼滤波算法对GPS、MEMS陀螺仪、里程计信号进行综合，并进行定位计算，以提高定位精度，平滑定位曲线；在GPS信号强度不正常时(GPS接收卫星信号少于3颗时)时，切换为惯性导航推算进行定位，即利用MEMS陀螺仪和里程计进行定位。

本发明中，卡尔曼滤波算法具体如下：

设状态方程为： $\stackrel{\·}{X}\left(t\right)=\mathrm{AX}\left(t\right)+W\left(t\right)+U,---\left(1\right)$

其中状态变量 $X={\left[\begin{array}{cccccccccc}e& n& \stackrel{\·}{e}& \stackrel{\·}{n}& \stackrel{\·\·}{e}& \stackrel{\·\·}{n}& {\mathrm{\ϵ}}_e& {\mathrm{\ϵ}}_n& {\mathrm{\δ}}_{\mathrm{\θ}}& {\mathrm{\δ}}_s\end{array}\right]}^T,$ e和n分别为东方向和北方向的位置，

和

分别为东方向和北方向的速度，

和

分别为东方向和北方向的加速度，ε_e和ε_n分别为各种误差源在东方向与北方向上误差的总和，δ_θ为MEMS陀螺的误差，δ_s为里程计的误差。

$A=\left[\begin{array}{ccccc}{0}_{2\×2}& I_{2\×2}& 0_{2\×2}& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& I_{2\×2}& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& A_1& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& A_2& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& A_3\end{array}\right],---\left(2\right)$

其中 $A_1=\left[\begin{array}{cc}-{\mathrm{\α}}_e& 0\\ 0& -{\mathrm{\α}}_n\end{array}\right],$ $A_2=\left[\begin{array}{cc}-{\mathrm{\τ}}_e& 0\\ 0& -{\mathrm{\τ}}_n\end{array}\right],$ $A_3=\left[\begin{array}{cc}-{\mathrm{\τ}}_{\mathrm{\θ}}& 0\\ 0& -{\mathrm{\τ}}_s\end{array}\right],$ I_2×2为2阶单位阵，O_2×2为二阶零矩阵。

$U={\left[\begin{array}{cccc}{0}_{1\×4}& {\mathrm{\α}}_e{\stackrel{\‾}{a}}_e& {\mathrm{\α}}_n{\stackrel{\‾}{a}}_n& 0_{1\×4}\end{array}\right]}^T.$

在滤波器的模型假设中：

$\stackrel{\·\·}{e}=\stackrel{\‾}{a_e}+a_e$ $\stackrel{\·\·}{n}=\stackrel{\‾}{a_n}+a_n---\left(3\right)$

此处，采用机动加速度的统计平均值来表述加速度。

其中：α_e，α_n，τ_e，τ_n，τ_θ，τ_s为相应的时间常数倒数，

分别为相应参量α_e、α_n、e、n、δ_θ、δ_s的零均值高斯白噪声，他们的方差分别为(为加速度的方差)、

将模型离散化后，建立的观测量为由GPS模块经过计算后输出的位置信息e_mn_m，速度信息

以及MEMS陀螺输出的角度θ，里程计输出的距离s。

X(k+1)＝Φ(k+1)X(k)+U(k)+W(k)            (5)

其中观测量：

观测方程是非线性的，采用扩展卡尔曼滤波算法，将其在一步预测值处展开成泰勒级数，忽略高次项。得到离散的卡尔曼滤波器递推算法：

X(k)＝X(k，k-1)+K(k){Z(k)-h[X(k，k-1)]}

K(k)＝P(k，k-1)H^T[X(k，k-1)]·{H[X(k，k-1)]P(k，k-1)·H^T[X(k，k-1)]+R(k)}^-1

P(k，k-1)＝Φ(k，k-1)P(k-1)·Φ^T(k，k-1)+Q(k-1)           (7)

P(k)＝{I-K(k)·H[X(k，k-1)]}P(k，k-1)

X(k，k-1)＝Φ′(k，k-1)+X(k-1)

其中，

${\mathrm{\Φ}}^{\′}(k,k-1)=\left[\begin{array}{ccccc}{I}_{2\×2}& {\mathrm{TI}}_{2\×2}& {0.5T^{2}I}_{2\×2}& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& I_{2\×2}& {\mathrm{TI}}_{2\×2}& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& I_{2\×2}& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& E_2& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& E_3\end{array}\right]---\left(8\right)$

$E_2=\left[\begin{array}{cc}{e}^{-{\mathrm{\τ}}_{e}T}& 0\\ 0& e^{-{\mathrm{\τ}}_{n}T}\end{array}\right],$ $E_3=\left[\begin{array}{cc}{e}^{-{\mathrm{\τ}}_{\mathrm{\θ}}T}& 0\\ 0& e^{-{\mathrm{\τ}}_{s}T}\end{array}\right].$

用此递推公式进行GPS加惯性导航的组合导航的定位计算。

若发生GPS接收卫星数少于3颗时，停止卡尔曼滤波算法，采用如下的递推公式进行惯性导航推算：

e(k)＝e(k-1)+s(k)×sin[β(k-1)+θ(k)]

n(k)＝n(k-1)+s(k)×co s[β(k-1)+θ(k)]

其中，β与θ均是以正北方向为0，顺时针方向为正的角度。

本发明提出的定位装置由如下几个模块构成：惯性导航模块、GPS模块、处理模块、输出模块。

惯性导航模块的功能为连接MEMS陀螺仪与里程计的输出，将信号进行滤波，模数转换，采用串行通信方式定时输出至处理模块。

GPS模块的功能为接收GPS卫星信号，处理后输出定位信息，采用串行通信方式定时输出至处理模块。

处理模块接收GPS定位信息以及惯性导航信息，对信息进行时间校正，判定GPS信息是否可用，选择采用卡尔曼滤波算法或者惯性推算进行定位计算，并将定位计算结果输出至输出模块。

输出模块根据通讯协议接收处理模块输出的定位显示信息，在可视化界面显示当前定位状态。

本装置采用模块化结构，各部分设计独立，基于统一的接口协议的模块可以兼容替换，配置灵活。

附图说明

图1、系统组成结构。

图2、惯性导航模块。

图3、本发明的方法流程。

其中，1为处理模块，2为输出模块，3为GPS模块，4为惯性导航模块，5为MEMS陀螺仪，6为里程计，7为A/D转换模块，8为计数器，9为单片机，10为单片机输出。

具体实施方式

结合附图，说明本发明的具体实施方式。

图1是整个系统的组成结构图，系统的主体处理模块1采用基于ARM9的嵌入式系统，CPU采用Samsung S3C2410(基于ARM920t)。处理模块1连接GPS模块3、惯性导航模块4、输出模块2。惯性导航模块4采集MEMS陀螺仪、里程计的输出信号，处理打包后通过串行接口向处理模块输出。GPS模块通过串行接口输出符合NMEA 0183 ASCII规范的串行定位数据。输出模块接收处理模块的输出信息，将定位信息结合电子地图显示在液晶显示屏。

图2是惯性导航模块4的硬件构成图。惯性导航模块4的输入为MEMS陀螺仪5(ADXRS150)的电压值输出信号以及里程计6的脉冲信号。MEMS陀螺仪5的输出电压通过模数转换器7得到数字量，而里程计6的脉冲信号由计数器转换成数字量，数字量通过MCU数据总线输入至MCU。MCU根据里程计的输入对MEMS陀螺仪输入进行同步采样，得到某一时刻的位移矢量，通过串行接口输出到处理模块2。

图3是系统的主软件流程图。处理模块1接收GPS信号模块3的与惯性导航模块4的输出信号，根据一定时间进行时间同步。GPS信号间隔时间较长，从而将惯性定位数据在离散采样时间间隔内进行取均值，作为一个离散时间间隔内的惯性定位输入。在每一个离散时间点判定GPS信号是否良好，若信号良好，则将GPS数据与惯性定位数据进行卡尔曼滤波，综合得当前定位数据；若GPS信号不好，则采用惯性推定算法由MEMS陀螺仪和里程计进行独立的航程推定，得到定位数据。采用卡尔曼滤波需要设定滤波器模型的相关参数以及初始值。本实施方式中，滤波器的初始可设定为：

τ_e＝τ_n＝0.01 α_e＝α_n＝1 ${\mathrm{\σ}}_{{\mathrm{\δ}}_s}=2$

σ_e＝σ_n＝10 ${\mathrm{\σ}}_{a_e}={\mathrm{\σ}}_{a_n}=0.5$ ${\mathrm{\σ}}_{{\mathrm{\δ}}_{\mathrm{\θ}}}=0.2$

X(0)＝[0，0，0，0，0，0，0，0，0]^T

P(0)＝diag{10²，10²，1，1，0.2²，0.2²，5²，5²，0.1²，2²}

R＝diag{10²，10²，1，1，0.05²，2²}

代入递推公式进行计算得到定位输出。

注意：本实现方式只是实现该装置的一种途径，本领域中专业人员可以按照需要对具体实现方式作出修改，如替换MEMS陀螺仪型号，MCU型号等。因而，实现实例中的某些细节不应构成对本发明的限定，本发明将以所附权利要求书界定的范围作为本发明的保护范围。

Claims (3)

1、一种利用GPS、陀螺仪、里程计的组合定位方法，其特征在于采用MEMS陀螺仪和里程计对GPS进行定位；其中：

在GPS接收卫星信号为3颗时，GPS信号强度正常，则利用卡尔曼滤波算法对GPS、MEMS陀螺仪和里程计的信号综合进行定位；

在GPS接收卫星信号少于3颗时，GPS信号不正常，则切换为惯性导航推算进行定位；

所述的卡尔曼滤波算法的步骤如下：

设状态方程为： $\stackrel{\·}{X}\left(t\right)=\mathrm{AX}\left(t\right)+W\left(t\right)+U,$

其中状态变量 $X={\left[\begin{array}{cccccccccc}e& n& \stackrel{\·}{e}& \stackrel{\·}{n}& \stackrel{\·\·}{e}& \stackrel{\·\·}{n}& {\mathrm{\ϵ}}_e& {\mathrm{\ϵ}}_n& {\mathrm{\δ}}_0& {\mathrm{\δ}}_s\end{array}\right]}^T,$ e和n分别为东方向和北方向的位置，

和

分别为东方向和北方向的速度，

和

分别为东方向和北方向的加速度，ε_e和ε_n分别为各种误差源在东方向与北方向上误差的总和，δ_θ为MEMS陀螺的误差，δ_x为里程计的误差；

$A=\left[\begin{array}{ccccc}{0}_{2\×2}& I_{2\×2}& 0_{2\×2}& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& I_{2\×2}& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& A_1& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& A_2& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& A_3\end{array}\right],$

其中 $A_1=\left[\begin{array}{cc}-{\mathrm{\α}}_e& 0\\ 0& -{\mathrm{\α}}_n\end{array}\right],$ $A_2=\left[\begin{array}{cc}-{\mathrm{\τ}}_e& 0\\ 0& -{\mathrm{\τ}}_n\end{array}\right],$ $A_3=\left[\begin{array}{cc}-{\mathrm{\τ}}_{\mathrm{\θ}}& 0\\ 0& -{\mathrm{\τ}}_s\end{array}\right],$ I_2×2为2阶单位阵，O_2×2为二阶零矩阵；

$U={\left[\begin{array}{cccc}{0}_{1\×4}& {\mathrm{\α}}_e{\stackrel{\‾}{a}}_e& {\mathrm{\α}}_n{\stackrel{\‾}{a}}_n& 0_{1\×4}\end{array}\right]}^T;$

在滤波器的模型假设中：

$\stackrel{\·\·}{e}={a^{-}}_e+a_e$ $\stackrel{\·\·}{n}={a^{-}}_n+a_n,$

此处，采用机动加速度的统计平均值来表述加速度。

其中：α_e，α_n，τ_e，τ_n，τ₀，τ_s为相应的时间常数倒数，

分别为相应参量α_e、α_n、e、n、δ_θ、δ_s的零均值高斯白噪声，他们的方差分别为

将模型离散化后，建立的观测量为由GPS模块经过计算后输出的位置信息e_m n_m，速度信息

以及MEMS陀螺输出的角度θ，里程计输出的距离s；

X(k+1)＝Φ(k+1)X(k)+U(k)+W(k)

其中观测量：

$X\left(k\right)={\left[\begin{array}{cccccccccc}e\left(k\right)& n\left(k\right)& \stackrel{\·}{e}\left(k\right)& \stackrel{\·}{n}\left(k\right)& \stackrel{\mathrm{gg}}{e}\left(k\right)& \stackrel{\mathrm{gg}}{n}\left(k\right)& {\mathrm{\ϵ}}_e\left(k\right)& {\mathrm{\ϵ}}_n\left(k\right)& {\mathrm{\δ}}_{\mathrm{\θ}}\left(k\right)& {\mathrm{\δ}}_s\left(k\right)\end{array}\right]}^T;$

观测方程是非线性的，将其在一步预测值处展开成泰勒级数，忽略高次项；得到离散的卡尔曼滤波器递推算法：

X(k)＝X(k，k-1)+K(k){Z(k)-h[X(k，k-1)]}

K(k)＝P(k，k-1)H^T[X(k，k-1)]·{H[X(k，k-1)]P(k，k-1)·H^T[X(k，k-1)]+R(k)}^-1

P(k，k-1)＝Φ(k，k-1)P(k-1)·Φ^T(k，k-1)+Q(k-1)

P(k)＝{I-K(k)·H[X(k，k-1)]}P(k，k-1)

X(k，k-1)＝Φ′(k，k-1)+X(k-1)

其中，

${\mathrm{\Φ}}^{\′}(k,k-1)=\left[\begin{array}{ccccc}{I}_{2\×2}& {\mathrm{TI}}_{2\×2}& 0.5T^2{I}_{2\×2}& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& I_{2\×2}& {\mathrm{TI}}_{2\×2}& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& I_{2\×2}& 0_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& E_2& 0_{2\×2}\\ 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& 0_{2\×2}& E_3\end{array}\right]$

$E_2=\left[\begin{array}{cc}{e}^{-{\mathrm{\τ}}_{e}T}& 0\\ 0& e^{-{\mathrm{\τ}}_{n}T}\end{array}\right],$ $E_3=\left[\begin{array}{cc}{e}^{-{\mathrm{\τ}}_{0}T}& 0\\ 0& e^{-{\mathrm{\τ}}_{s}T}\end{array}\right].$

用此递推公式进行GPS加惯性导航的组合导航的定位计算；

所述的惯性导航推算的公式如下：

e(k)＝e(k-1)+s(k)×sin[β(k-1)+θ(k)]

n(k)＝n(k-1)+s(k)×cos[β(k-1)+θ(k)]

其中，β与θ均是以正北方向为0，顺时针方向为正的角度。

2、一种利用GPS、陀螺仪、里程计的组合定位装置，其特征在于由如下几个模块构成：惯性导航模块、GPS模块、处理模块、输出模块；其中：

惯性导航模块的功能为连接MEMS陀螺仪与里程计的输出，将信号进行滤波，模数转换，采用串行通信方式定时输出至处理模块；

GPS模块的功能为接收GPS卫星信号，处理后输出定位信息，采用串行通信方式定时输出至处理模块；

处理模块接收GPS定位信息以及惯性导航信息，对信息进行时间校正，判定GPS信息是否可用，选择采用卡尔曼滤波算法或者惯性推算进行定位计算，并将定位计算结果输出至输出模块；

输出模块根据通讯协议接收处理模块输出的定位显示信息，在可视化界面显示当前定位状态。

3、根据权利要求2所述的定位装置，其特征在于所述的惯性导航模块(4)由A/D转换模块(7)、计数器(8)和单片机(9)组成；其中，惯性导航模块(4)的输入为MEMS陀螺仪(5)的电压值输出信号以及里程计(6)的脉冲信号；MEMS陀螺仪(5)的输出电压通过模数转换器(7)得到数字量，而里程计(6)的脉冲信号由计数器(8)转换成数字量，数字量通过MCU数据总线输入至MCU；MCU根据里程计的输入对MEMS陀螺仪输入进行同步采样，得到某一时刻的位移矢量，通过串行接口输出到处理模块(2)。

Images