专利名称：利用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陀螺仪和里程计进行定位。
本发明中，卡尔曼滤波算法具体如下设状态方程为X·(t)=AX(t)+W(t)+U,---(1)]]>其中状态变量X=ene·n·e··n··ϵeϵnδθδsT,]]>e和n分别为东方向和北方向的位置， 和 分别为东方向和北方向的速度， 和 分别为东方向和北方向的加速度，εe和εn分别为各种误差源在东方向与北方向上误差的总和，δθ为MEMS陀螺的误差，δs为里程计的误差。
A=02×2I2×202×202×202×202×202×2I2×202×202×202×202×2A102×202×202×202×202×2A202×202×202×202×202×2A3,---(2)]]>其中A1=-αe00-αn,]]>A2=-τe00-τn,]]>A3=-τθ00-τs,]]>I2×2为2阶单位阵，02×2为二阶零矩阵。
U＝
T。
在滤波器的模型假设中e··=a‾e+ae]]>n··=a‾n+an---(3)]]>此处，采用机动加速度的统计平均值来表述加速度。
(4)其中αe，αn，τe，τn，τθ，τs为相应的时间常数倒数， 分别为相应参量αe、αn、e、n、δθ、δs的零均值高斯白噪声，他们的方差分别为2αeσae2、2αnσan2(σae2，σan2为加速度的方差)、σe2、σn2、σδθ2、σδs2。
将模型离散化后，建立的观测量为由GPS模块经过计算后输出的位置信息emnm，速度信息 以及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)HT[X(k，k-1)]·{H[X(k，k-1)]P(k，k-1)·HT[X(k，k-1)]+R(K)}-1P(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) (7)其中，Φ′(k,k-1)=I2×2TI2×20.5T2I2×202×202×202×2I2×2TI2×202×202×202×202×2I2×202×202×202×202×202×2E202×202×202×202×202×2E3]]>E2=e-τeT00e-τnT,]]>E3=e-τθT00e-τsT.]]>(8)用此递推公式进行GPS加惯性导航的组合导航的定位计算。
若发生GPS接收卫星数少于3颗时，停止卡尔曼滤波算法，采用如下的递推公式进行惯性导航推算；e(k)＝e(k-1)+s(k)×sin[β(k-1)+θ(k)]n(k)＝n(k-1)+s(k)×cos[β(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σδs=2]]>σe＝σn＝10σae=σan=0.5]]>σδθ=0.2]]>X(0)＝
TP(0)＝diag{102，102，1，1，0.22，0.22，52，52，0.12，22}R＝diag{102，102，1，1，0.052，22}代入递推公式进行计算得到定位输出。
注意本实现方式只是实现该装置的一种途径，本领域中专业人员可以按照需要对具体实现方式作出修改，如替换MEMS陀螺仪型号，MCU型号等。因而，实现实例中的某些细节不应构成对本发明的限定，本发明将以所附权利要求书界定的范围作为本发明的保护范围。
权利要求
1.一种利用GPS、陀螺仪、里程计的组合定位方法，其特征在于采用MEMS陀螺仪和里程计对GPS定位进行补偿和校正；其中在GPS接收卫星信号为3颗时，GPS信号强度正常，则利用卡尔曼滤波算法对GPS、MEMS陀螺仪和里程计的信号综合进行定位；在GPS接收卫星信号少于3颗时，GPS信号不正常，则切换为惯性导航推算进行定位。
2.根据权利要求1所述的定位方法，其特征在于所述的卡尔曼滤波算法的步骤如下设状态方程为X·(t)=AX(t)+W(t)+U,]]>其中状态变量X=ene·n·e··n··ϵeϵnδθδsT,]]>e和n分别为东方向和北方向的位置， 和 分别为东方向和北方向的速度， 和 分别为东方向和北方向的加速度，εe和εn分别为各种误差源在东方向与北方向上误差的总和，δθ为MEMS陀螺的误差，δs为里程计的误差；A=O2×2I2×2O2×2O2×2O2×2O2×2O2×2I2×2O2×2O2×2O2×2O2×2A1O2×2O2×2O2×2O2×2O2×2A2O2×2O2×2O2×2O2×2O2×2A3,]]>其中A1=-αe00-αn,]]>A2=-τe00-τn,A3=-τθ00-τs,]]>I2×2为2阶单位阵，O2×2为二阶零矩阵； U=O1×4αea‾eαna‾nO1×4T;]]>在滤波器的模型假设中e··=a-e+ae]]>n··=a-n+an,]]>此处，采用机动加速度的统计平均值来表述加速度。其中αe，αn，τe，τn，τθ，τs为相应的时间常数倒数， 分别为相应参量αe、αn、e、n、δθ、δs的零均值高斯白噪声，他们的方差分别为2αeσαe2、2αhσαn2、σe2、σn2、σδθ2、σδs2；将模型离散化后，建立的观测量为由GPS模块经过计算后输出的位置信息emnm，速度信息 以及MEMS陀螺输出的角度θ，里程计输出的距离s；X(k+1)＝Φ(k+1)X(k)+U(k)+W(k)其中观测量 观测方程是非线性的，将其在一步预测值处展开成泰勒级数，忽略高次项；得到离散的卡尔曼滤波器递推算法X(k)＝X(k，k-1)+K(k){Z(k)-h[X(k，k-1)]}K(k)＝P(k，k-1)HT[X(k，k-1)]·{H[X(k，k-1)]P(k，k-1)·HT[X(k，k-1)]+P(k)}-1P(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)其中，Φ′(k,k-1)=I2×2TI2×20.5T2I2×2O2×2O2×2O2×2I2×2TI2×2O2×2O2×2O2×2O2×2I2×2O2×2O2×2O2×2O2×2O2×2E2O2×2O2×2O2×2O2×2O2×2E3]]>E2=e-τeT00e-τnT,]]>E3=e-τθT00e-τsT]]>用此递推公式进行GPS加惯性导航的组合导航的定位计算。
3.根据权利要求2所述的定位方法，其特征在于所述的惯性导航推算的公式如下e(k)＝e(k-1)+s(k)×sin[β(k-1)+θ(k)]n(k)＝n(k-1)+s(k)×cos[β(k-1)+θ(k)]其中，β与θ均是以正北方向为0，顺时针方向为正的角度。
4.一种利用GPS、陀螺仪、里程计的组合定位装置，其特征在于由如下几个模块构成惯性导航模块、GPS模块、处理模块、输出模块；其中惯性导航模块的功能为连接MEMS陀螺仪与里程计的输出，将信号进行滤波，模数转换，采用串行通信方式定时输出至处理模块；GPS模块的功能为接收GPS卫星信号，处理后输出定位信息，采用串行通信方式定时输出至处理模块；处理模块接收GPS定位信息以及惯性导航信息，对信息进行时间校正，判定GPS信息是否可用，选择采用卡尔曼滤波算法或者惯性推算进行定位计算，并将定位计算结果输出至输出模块；输出模块根据通讯协议接收处理模块输出的定位显示信息，在可视化界面显示当前定位状态。
5.根据权利要求4所述的定位装置，其特征在于所述的惯性导航模块(4)由A/D转换模块(7)、计数器(8)和单片机(9)组成；其中，惯性导航模块(4)的输入为MEMS陀螺仪(5)的电压值输出信号以及里程计(6)的脉冲信号；MEMS陀螺仪(5)的输出电压通过模数转换器(7)得到数字量，而里程计(6)的脉冲信号由计数器(8)转换成数字量，数字量通过MCU数据总线输入至MCU；MCU根据里程计的输入对MEMS陀螺仪输入进行同步采样，得到某一时刻的位移矢量，通过串行接口输出到处理模块(2)。
全文摘要
本发明属GPS定位技术领域，具体为一种利用GPS与MEMS陀螺仪、里程计的组合定位方法与装置。该装置在GPS信号良好时利用GPS定位信号进行定位，MEMS陀螺仪、里程计进行校正，采用卡尔曼滤波算法进行信号综合；当GPS信号丢失或者接收卫星数少于3颗时，切换至MEMS陀螺仪、里程计定位。采用组合导航可以提高GPS定位的精度，并弥补了GPS在受遮挡情况下无法定位的缺陷。
文档编号G01C21/28GK1948910SQ20061011810
公开日2007年4月18日 申请日期2006年11月9日 优先权日2006年11月9日
发明者陆起涌, 林绿洲, 王力超 申请人:复旦大学