專利名稱：利用gps與陀螺儀、里程計(jì)的組合定位方法與裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明屬于GPS定位技術(shù)領(lǐng)域，具體涉及一種利用MEMS陀螺儀、里程計(jì)對(duì)GPS定位信號(hào)進(jìn)行校正，并在GPS丟星狀態(tài)時(shí)切換定位方式，實(shí)現(xiàn)不間斷定位的組合定位方法與裝置。
背景技術(shù)：
當(dāng)前，GPS定位系統(tǒng)已經(jīng)被廣泛采用，GPS定位精度與接收到的衛(wèi)星信號(hào)強(qiáng)度有關(guān)。在接收到超過3顆衛(wèi)星信號(hào)時(shí)，定位準(zhǔn)確；在接收到3顆衛(wèi)星信號(hào)時(shí)，定位比較勉強(qiáng)；在接收到衛(wèi)星信號(hào)少于3顆時(shí)，無法輸出定位信息。由于地理環(huán)境的限制，在山嶺、隧道、城市建筑群等遮擋環(huán)境中GPS接收器接收到的信號(hào)強(qiáng)度會(huì)大幅降低，甚至?xí)也坏叫l(wèi)星信號(hào)。因此單獨(dú)GPS定位系統(tǒng)定位精度在這種情況下會(huì)受到很大影響，需要其他定位方式進(jìn)行補(bǔ)償與校正。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供定位精度高，并可解決GPS在受遮擋情況下無法定位缺陷的定位方法與裝置。
本發(fā)明提出的定位方法，是一種利用GPS與MEMS陀螺儀、里程計(jì)的組合定位的方法，并采用卡爾曼濾波算法對(duì)定位數(shù)據(jù)進(jìn)行最優(yōu)估計(jì)。其中采用MEMS陀螺儀和里程計(jì)對(duì)GPS定位進(jìn)行補(bǔ)償和校正，MEMS陀螺儀相對(duì)于傳統(tǒng)陀螺儀具有在保持精度的前提下減小尺寸、減小功耗，能夠有效地解決GPS在受遮擋情況下無法定位的缺陷，切換時(shí)間小，使系統(tǒng)能夠持續(xù)不間斷定位工作；同時(shí)，在GPS信號(hào)強(qiáng)度正常的情況下(GPS接收衛(wèi)星信號(hào)為了顆時(shí))，利用卡爾曼濾波算法對(duì)GPS、MEMS陀螺儀、里程計(jì)信號(hào)進(jìn)行綜合，并進(jìn)行定位計(jì)算，以提高定位精度，平滑定位曲線；在GPS信號(hào)強(qiáng)度不正常時(shí)(GPS接收衛(wèi)星信號(hào)少于3顆時(shí))時(shí)，切換為慣性導(dǎo)航推算進(jìn)行定位，即利用MEMS陀螺儀和里程計(jì)進(jìn)行定位。
本發(fā)明中，卡爾曼濾波算法具體如下設(shè)狀態(tài)方程為X·(t)=AX(t)+W(t)+U,---(1)]]>其中狀態(tài)變量X=ene·n·e··n··ϵeϵnδθδsT,]]>e和n分別為東方向和北方向的位置， 和 分別為東方向和北方向的速度， 和 分別為東方向和北方向的加速度，εe和εn分別為各種誤差源在東方向與北方向上誤差的總和，δθ為MEMS陀螺的誤差，δs為里程計(jì)的誤差。
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。
在濾波器的模型假設(shè)中e··=a‾e+ae]]>n··=a‾n+an---(3)]]>此處，采用機(jī)動(dòng)加速度的統(tǒng)計(jì)平均值來表述加速度。
(4)其中αe，αn，τe，τn，τθ，τs為相應(yīng)的時(shí)間常數(shù)倒數(shù)， 分別為相應(yīng)參量αe、αn、e、n、δθ、δs的零均值高斯白噪聲，他們的方差分別為2αeσae2、2αnσan2(σae2，σan2為加速度的方差)、σe2、σn2、σδθ2、σδs2。
將模型離散化后，建立的觀測(cè)量為由GPS模塊經(jīng)過計(jì)算后輸出的位置信息emnm，速度信息 以及MEMS陀螺輸出的角度θ，里程計(jì)輸出的距離s。
X(k+1)＝Φ(k+1)X(k)+U(k)+W(k) (5)其中觀測(cè)量 觀測(cè)方程是非線性的，采用擴(kuò)展卡爾曼濾波算法，將其在一步預(yù)測(cè)值處展開成泰勒級(jí)數(shù)，忽略高次項(xiàng)。得到離散的卡爾曼濾波器遞推算法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)用此遞推公式進(jìn)行GPS加慣性導(dǎo)航的組合導(dǎo)航的定位計(jì)算。
若發(fā)生GPS接收衛(wèi)星數(shù)少于3顆時(shí)，停止卡爾曼濾波算法，采用如下的遞推公式進(jìn)行慣性導(dǎo)航推算；e(k)＝e(k-1)+s(k)×sin[β(k-1)+θ(k)]n(k)＝n(k-1)+s(k)×cos[β(k-1)+θ(k)]其中，β與θ均是以正北方向?yàn)?，順時(shí)針方向?yàn)檎慕嵌取?br> 本發(fā)明提出的定位裝置由如下幾個(gè)模塊構(gòu)成慣性導(dǎo)航模塊、GPS模塊、處理模塊、輸出模塊。
慣性導(dǎo)航模塊的功能為連接MEMS陀螺儀與里程計(jì)的輸出，將信號(hào)進(jìn)行濾波，模數(shù)轉(zhuǎn)換，采用串行通信方式定時(shí)輸出至處理模塊。
GPS模塊的功能為接收GPS衛(wèi)星信號(hào)，處理后輸出定位信息，采用串行通信方式定時(shí)輸出至處理模塊。
處理模塊接收GPS定位信息以及慣性導(dǎo)航信息，對(duì)信息進(jìn)行時(shí)間校正，判定GPS信息是否可用，選擇采用卡爾曼濾波算法或者慣性推算進(jìn)行定位計(jì)算，并將定位計(jì)算結(jié)果輸出至輸出模塊。
輸出模塊根據(jù)通訊協(xié)議接收處理模塊輸出的定位顯示信息，在可視化界面顯示當(dāng)前定位狀態(tài)。
本裝置采用模塊化結(jié)構(gòu)，各部分設(shè)計(jì)獨(dú)立，基于統(tǒng)一的接口協(xié)議的模塊可以兼容替換，配置靈活。


圖1、系統(tǒng)組成結(jié)構(gòu)。
圖2、慣性導(dǎo)航模塊。
圖3、本發(fā)明的方法流程。
其中，1為處理模塊，2為輸出模塊，3為GPS模塊，4為慣性導(dǎo)航模塊，5為MEMS陀螺儀，6為里程計(jì)，7為A/D轉(zhuǎn)換模塊，8為計(jì)數(shù)器，9為單片機(jī)，10為單片機(jī)輸出。
具體實(shí)施例方式
結(jié)合附圖，說明本發(fā)明的具體實(shí)施方式
。
圖1是整個(gè)系統(tǒng)的組成結(jié)構(gòu)圖，系統(tǒng)的主體處理模塊1采用基于ARM9的嵌入式系統(tǒng)，CPU采用Samsung S3C2410(基于ARM920t)。處理模塊1連接GPS模塊3、慣性導(dǎo)航模塊4、輸出模塊2。慣性導(dǎo)航模塊4采集MEMS陀螺儀、里程計(jì)的輸出信號(hào)，處理打包后通過串行接口向處理模塊輸出。GPS模塊通過串行接口輸出符合NMEA 0183 ASCII規(guī)范的串行定位數(shù)據(jù)。輸出模塊接收處理模塊的輸出信息，將定位信息結(jié)合電子地圖顯示在液晶顯示屏。
圖2是慣性導(dǎo)航模塊4的硬件構(gòu)成圖。慣性導(dǎo)航模塊4的輸入為MEMS陀螺儀5(ADXRS150)的電壓值輸出信號(hào)以及里程計(jì)6的脈沖信號(hào)。MEMS陀螺儀5的輸出電壓通過模數(shù)轉(zhuǎn)換器7得到數(shù)字量，而里程計(jì)6的脈沖信號(hào)由計(jì)數(shù)器轉(zhuǎn)換成數(shù)字量，數(shù)字量通過MCU數(shù)據(jù)總線輸入至MCU。MCU根據(jù)里程計(jì)的輸入對(duì)MEMS陀螺儀輸入進(jìn)行同步采樣，得到某一時(shí)刻的位移矢量，通過串行接口輸出到處理模塊2。
圖3是系統(tǒng)的主軟件流程圖。處理模塊1接收GPS信號(hào)模塊3的與慣性導(dǎo)航模塊4的輸出信號(hào)，根據(jù)一定時(shí)間進(jìn)行時(shí)間同步。GPS信號(hào)間隔時(shí)間較長(zhǎng)，從而將慣性定位數(shù)據(jù)在離散采樣時(shí)間間隔內(nèi)進(jìn)行取均值，作為一個(gè)離散時(shí)間間隔內(nèi)的慣性定位輸入。在每一個(gè)離散時(shí)間點(diǎn)判定GPS信號(hào)是否良好，若信號(hào)良好，則將GPS數(shù)據(jù)與慣性定位數(shù)據(jù)進(jìn)行卡爾曼濾波，綜合得當(dāng)前定位數(shù)據(jù)；若GPS信號(hào)不好，則采用慣性推定算法由MEMS陀螺儀和里程計(jì)進(jìn)行獨(dú)立的航程推定，得到定位數(shù)據(jù)。采用卡爾曼濾波需要設(shè)定濾波器模型的相關(guān)參數(shù)以及初始值。本實(shí)施方式中，濾波器的初始可設(shè)定為τ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}代入遞推公式進(jìn)行計(jì)算得到定位輸出。
注意本實(shí)現(xiàn)方式只是實(shí)現(xiàn)該裝置的一種途徑，本領(lǐng)域中專業(yè)人員可以按照需要對(duì)具體實(shí)現(xiàn)方式作出修改，如替換MEMS陀螺儀型號(hào)，MCU型號(hào)等。因而，實(shí)現(xiàn)實(shí)例中的某些細(xì)節(jié)不應(yīng)構(gòu)成對(duì)本發(fā)明的限定，本發(fā)明將以所附權(quán)利要求書界定的范圍作為本發(fā)明的保護(hù)范圍。
權(quán)利要求
1.一種利用GPS、陀螺儀、里程計(jì)的組合定位方法，其特征在于采用MEMS陀螺儀和里程計(jì)對(duì)GPS定位進(jìn)行補(bǔ)償和校正；其中在GPS接收衛(wèi)星信號(hào)為3顆時(shí)，GPS信號(hào)強(qiáng)度正常，則利用卡爾曼濾波算法對(duì)GPS、MEMS陀螺儀和里程計(jì)的信號(hào)綜合進(jìn)行定位；在GPS接收衛(wèi)星信號(hào)少于3顆時(shí)，GPS信號(hào)不正常，則切換為慣性導(dǎo)航推算進(jìn)行定位。
2.根據(jù)權(quán)利要求1所述的定位方法，其特征在于所述的卡爾曼濾波算法的步驟如下設(shè)狀態(tài)方程為X·(t)=AX(t)+W(t)+U,]]>其中狀態(tài)變量X=ene·n·e··n··ϵeϵnδθδsT,]]>e和n分別為東方向和北方向的位置， 和 分別為東方向和北方向的速度， 和 分別為東方向和北方向的加速度，εe和εn分別為各種誤差源在東方向與北方向上誤差的總和，δθ為MEMS陀螺的誤差，δs為里程計(jì)的誤差；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;]]>在濾波器的模型假設(shè)中e··=a-e+ae]]>n··=a-n+an,]]>此處，采用機(jī)動(dòng)加速度的統(tǒng)計(jì)平均值來表述加速度。其中αe，αn，τe，τn，τθ，τs為相應(yīng)的時(shí)間常數(shù)倒數(shù)， 分別為相應(yīng)參量αe、αn、e、n、δθ、δs的零均值高斯白噪聲，他們的方差分別為2αeσαe2、2αhσαn2、σe2、σn2、σδθ2、σδs2；將模型離散化后，建立的觀測(cè)量為由GPS模塊經(jīng)過計(jì)算后輸出的位置信息emnm，速度信息 以及MEMS陀螺輸出的角度θ，里程計(jì)輸出的距離s；X(k+1)＝Φ(k+1)X(k)+U(k)+W(k)其中觀測(cè)量 觀測(cè)方程是非線性的，將其在一步預(yù)測(cè)值處展開成泰勒級(jí)數(shù)，忽略高次項(xiàng)；得到離散的卡爾曼濾波器遞推算法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]]>用此遞推公式進(jìn)行GPS加慣性導(dǎo)航的組合導(dǎo)航的定位計(jì)算。
3.根據(jù)權(quán)利要求2所述的定位方法，其特征在于所述的慣性導(dǎo)航推算的公式如下e(k)＝e(k-1)+s(k)×sin[β(k-1)+θ(k)]n(k)＝n(k-1)+s(k)×cos[β(k-1)+θ(k)]其中，β與θ均是以正北方向?yàn)?，順時(shí)針方向?yàn)檎慕嵌取?br> 4.一種利用GPS、陀螺儀、里程計(jì)的組合定位裝置，其特征在于由如下幾個(gè)模塊構(gòu)成慣性導(dǎo)航模塊、GPS模塊、處理模塊、輸出模塊；其中慣性導(dǎo)航模塊的功能為連接MEMS陀螺儀與里程計(jì)的輸出，將信號(hào)進(jìn)行濾波，模數(shù)轉(zhuǎn)換，采用串行通信方式定時(shí)輸出至處理模塊；GPS模塊的功能為接收GPS衛(wèi)星信號(hào)，處理后輸出定位信息，采用串行通信方式定時(shí)輸出至處理模塊；處理模塊接收GPS定位信息以及慣性導(dǎo)航信息，對(duì)信息進(jìn)行時(shí)間校正，判定GPS信息是否可用，選擇采用卡爾曼濾波算法或者慣性推算進(jìn)行定位計(jì)算，并將定位計(jì)算結(jié)果輸出至輸出模塊；輸出模塊根據(jù)通訊協(xié)議接收處理模塊輸出的定位顯示信息，在可視化界面顯示當(dāng)前定位狀態(tài)。
5.根據(jù)權(quán)利要求4所述的定位裝置，其特征在于所述的慣性導(dǎo)航模塊(4)由A/D轉(zhuǎn)換模塊(7)、計(jì)數(shù)器(8)和單片機(jī)(9)組成；其中，慣性導(dǎo)航模塊(4)的輸入為MEMS陀螺儀(5)的電壓值輸出信號(hào)以及里程計(jì)(6)的脈沖信號(hào)；MEMS陀螺儀(5)的輸出電壓通過模數(shù)轉(zhuǎn)換器(7)得到數(shù)字量，而里程計(jì)(6)的脈沖信號(hào)由計(jì)數(shù)器(8)轉(zhuǎn)換成數(shù)字量，數(shù)字量通過MCU數(shù)據(jù)總線輸入至MCU；MCU根據(jù)里程計(jì)的輸入對(duì)MEMS陀螺儀輸入進(jìn)行同步采樣，得到某一時(shí)刻的位移矢量，通過串行接口輸出到處理模塊(2)。
全文摘要
本發(fā)明屬GPS定位技術(shù)領(lǐng)域，具體為一種利用GPS與MEMS陀螺儀、里程計(jì)的組合定位方法與裝置。該裝置在GPS信號(hào)良好時(shí)利用GPS定位信號(hào)進(jìn)行定位，MEMS陀螺儀、里程計(jì)進(jìn)行校正，采用卡爾曼濾波算法進(jìn)行信號(hào)綜合；當(dāng)GPS信號(hào)丟失或者接收衛(wèi)星數(shù)少于3顆時(shí)，切換至MEMS陀螺儀、里程計(jì)定位。采用組合導(dǎo)航可以提高GPS定位的精度，并彌補(bǔ)了GPS在受遮擋情況下無法定位的缺陷。
文檔編號(hào)G01C21/28GK1948910SQ20061011810
公開日2007年4月18日 申請(qǐng)日期2006年11月9日 優(yōu)先權(quán)日2006年11月9日
發(fā)明者陸起涌, 林綠洲, 王力超 申請(qǐng)人:復(fù)旦大學(xué)