本發(fā)明涉及導(dǎo)航
技術(shù)領(lǐng)域：
，特別是涉及一種導(dǎo)航定位方法及裝置。
背景技術(shù)：
：現(xiàn)如今，全球定位系統(tǒng)(GlobalPositioningSystem，GPS)技術(shù)已經(jīng)廣泛應(yīng)用于車輛的定位與導(dǎo)航。GPS定位系統(tǒng)可以確定地球上任一目標(biāo)任一時(shí)段的三維坐標(biāo)、三維速度和準(zhǔn)確時(shí)間，在車輛上安裝GPS接收機(jī)，便能獲知車輛的位置、運(yùn)行速度和運(yùn)行方向?，F(xiàn)有技術(shù)中，車載GPS接收機(jī)在無法接收到GPS信號或者GPS信號被遮擋時(shí)，將無法進(jìn)行定位，且車載GPS接收機(jī)所接收的GPS信號的更新周期較長，存在著定位精確度低的問題。技術(shù)實(shí)現(xiàn)要素：本發(fā)明實(shí)施例解決的技術(shù)問題是如何在無法接收到GPS信號或者GPS信號被遮擋進(jìn)行定位，并提高導(dǎo)航定位的精確度。為解決上述問題，本發(fā)明實(shí)施例提供了一種導(dǎo)航定位方法，所述方法包括：在預(yù)設(shè)時(shí)間段內(nèi)，基于車輛在初始時(shí)刻接收到的GPS定位衛(wèi)星發(fā)射的GPS信號，確定所述車輛在初始時(shí)刻的第一初始位置的信息，并基于車輛在初始時(shí)刻接收到的移動基站發(fā)送的定位信號，確定所述車輛在所述初始時(shí)刻的第二初始位置的信息；其中，所述預(yù)設(shè)時(shí)間段的時(shí)長與所述GPS信號的更新頻率對應(yīng)；基于所獲取的車輛在前一時(shí)刻的最終位置的信息，以及當(dāng)前時(shí)刻測量得到的里程計(jì)測量值和方位角測量值，計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息；其中，當(dāng)前時(shí)刻與前一時(shí)刻與之間的時(shí)間間隔與所述里程計(jì)測量值和方位角測量值的更新頻率對應(yīng)；將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行融合，得到所述車輛在當(dāng)前時(shí)刻的最終位置?？蛇x地，所述基于所獲取的車輛在前一時(shí)刻的最終位置的信息，以及當(dāng)前時(shí)刻測量得到的里程計(jì)測量值和方位角測量值，計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息，包括：基于所獲取的車輛在前一時(shí)刻的最終位置的信息，以及當(dāng)前時(shí)刻測量得到的里程計(jì)測量值和方位角測量值，采用航位推算法計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息?？蛇x地，所述基于所獲取的車輛在前一時(shí)刻的最終位置的信息，以及當(dāng)前時(shí)刻測量得到的里程計(jì)測量值和方位角測量值，采用航位推算法計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息，包括：其中，λi、Li、hi分別表示當(dāng)前時(shí)刻i的經(jīng)度、緯度和高度，λi-1、Li-1、hi-1分別表示前一時(shí)刻(i-1)的經(jīng)度、緯度和高度，分別表示當(dāng)前時(shí)刻ti與前一時(shí)刻ti-1的里程數(shù)差值的東向分量、北向分量和天向分量，RM、RN分別為載體所在地理位置對應(yīng)的地球子午圈曲率半徑和卯酉圈曲率半徑?？蛇x地，所述將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行融合，得到所述車輛在當(dāng)前時(shí)刻的最終位置，包括：將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行卡爾曼濾波，得到所述車輛在當(dāng)前時(shí)刻的最終位置?？蛇x地，所述將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行卡爾曼濾波，得到所述車輛在當(dāng)前時(shí)刻的最終位置，包括：且：β1=0.99PDOP≤22/PDOP2<PDOP<51/PDOP5≤PDOP<100.5/PDOP10≤PDOP;]]>β2=λ5≤PDOPλ*(1-(5-PDOP)/3)2≤PDOP<50PDOP<2;]]>β3＝1-β1-β2；λ={0.5W>-85dBm0.5*(100-W)/15-100dBm≤W<-85dBm0W<-100dBm;]]>其中，i表示當(dāng)前時(shí)刻ti，表示車輛在所述當(dāng)前時(shí)刻ti的最終位置，表示車輛在初始時(shí)刻t0的第一初始位置，表示車輛在初始時(shí)刻t0的第二初始位置，P(i)表示當(dāng)前時(shí)刻ti對應(yīng)的協(xié)方差矩陣，PDOP表示GPS信號的精度因子，W表示移動基站發(fā)送的定位信號的信號強(qiáng)度。可選地，所述預(yù)設(shè)時(shí)間段的時(shí)長為1s，當(dāng)前時(shí)刻與前一時(shí)刻之間的時(shí)間間隔為0.1s?？蛇x地，所述方法還包括：通過無線方式將所述車輛在當(dāng)前時(shí)刻的最終位置的信息發(fā)送至其他車輛。本發(fā)明實(shí)施例還提供了一種導(dǎo)航定位裝置，所述裝置包括：初始位置確定單元，適于在預(yù)設(shè)時(shí)間段內(nèi)，基于車輛在初始時(shí)刻接收到的GPS定位衛(wèi)星發(fā)射的GPS信號，確定所述車輛在初始時(shí)刻的第一初始位置的信息，并基于車輛在初始時(shí)刻接收到的移動基站發(fā)送的定位信號，確定所述車輛在所述初始時(shí)刻的第二初始位置的信息；其中，所述預(yù)設(shè)時(shí)間段的時(shí)長與所述GPS信號的更新頻率對應(yīng)；計(jì)算單元，適于基于所獲取的車輛在前一時(shí)刻的最終位置的信息，以及當(dāng)前時(shí)刻測量得到的里程計(jì)測量值和方位角測量值，計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息；其中，當(dāng)前時(shí)刻與前一時(shí)刻與之間的時(shí)間間隔與所述里程計(jì)測量值和方位角測量值的更新頻率對應(yīng)；融合定位單元，適于將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行融合，得到所述車輛在當(dāng)前時(shí)刻的最終位置。可選地，所述初始位置確定單元，適于基于所獲取的車輛在前一時(shí)刻的最終位置的信息，以及當(dāng)前時(shí)刻測量得到的里程計(jì)測量值和方位角測量值，采用航位推算法計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息?？蛇x地，所述計(jì)算單元，適于采用如下的公式計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息：其中，λi、Li、hi分別表示當(dāng)前時(shí)刻ti的經(jīng)度、緯度和高度，λi-1、Li-1、hi-1分別表示前一時(shí)刻ti-1的經(jīng)度、緯度和高度，n表示導(dǎo)航坐標(biāo)系，分別表示當(dāng)前時(shí)刻ti與前一時(shí)刻ti-1的里程數(shù)差值的東向分量、北向分量和天向分量，RM、RN分別為載體所在地理位置對應(yīng)的地球子午圈曲率半徑和卯酉圈曲率半徑?？蛇x地，所述融合定位單元，適于將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行卡爾曼濾波，得到所述車輛在當(dāng)前時(shí)刻的最終位置?？蛇x地，所述融合定位單元，適于計(jì)算得到所述車輛在當(dāng)前時(shí)刻的最終位置，包括：且：β1=0.99PDOP≤22/PDOP2<PDOP<51/PDOP5≤PDOP<100.5/PDOP10≤PDOP;]]>β2=λ5≤PDOPλ*(1-(5-PDOP)/3)2≤PDOP<50PDOP<2;]]>β3＝1-β1-β2；λ={0.5W>-85dBm0.5*(100-W)/15-100dBm≤W<-85dBm0W<-100dBm;]]>其中，i表示當(dāng)前時(shí)刻ti，表示車輛在所述當(dāng)前時(shí)刻ti的最終位置，表示車輛在初始時(shí)刻t0的第一初始位置，表示車輛在初始時(shí)刻t0的第二初始位置，P(i)表示當(dāng)前時(shí)刻ti對應(yīng)的協(xié)方差矩陣，PDOP表示GPS信號的精度因子，W表示移動基站發(fā)送的定位信號的信號強(qiáng)度。可選地，所述預(yù)設(shè)時(shí)間段的時(shí)長為1s，當(dāng)前時(shí)刻與前一時(shí)刻之間的時(shí)間間隔為0.1s?？蛇x地，所述導(dǎo)航定位裝置還包括：發(fā)送單元，適于通過無線方式將所述車輛在當(dāng)前時(shí)刻的最終位置的信息發(fā)送至其他車輛。與現(xiàn)有技術(shù)相比，本發(fā)明的技術(shù)方案具有以下的優(yōu)點(diǎn)：上述的方案，在預(yù)設(shè)時(shí)間段內(nèi)的各個(gè)預(yù)設(shè)時(shí)刻，將所述車輛在所述初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在各個(gè)預(yù)設(shè)時(shí)刻的估算位置進(jìn)行融合，得到所述車輛在各個(gè)預(yù)設(shè)時(shí)刻的最終位置，由于提高了車輛的位置信息的更新頻率，即使車輛在GPS信號的信號強(qiáng)度較弱或者無法接收GPS信號時(shí)，也可以得到車輛在預(yù)設(shè)時(shí)間段內(nèi)的各個(gè)時(shí)刻的最終位置，因而可以提高定位的精確度，提升用戶的使用體驗(yàn)。附圖說明圖1是本發(fā)明實(shí)施例中的一種導(dǎo)航定位方法的流程圖；圖2是本發(fā)明實(shí)施例中的另一種導(dǎo)航定位方法的流程圖；圖3是本發(fā)明實(shí)施例中的一種導(dǎo)航定位裝置的結(jié)構(gòu)示意圖。具體實(shí)施方式為解決現(xiàn)有技術(shù)中存在的上述問題，本發(fā)明實(shí)施例采用的技術(shù)方案通過在預(yù)設(shè)時(shí)間段內(nèi)的各個(gè)預(yù)設(shè)時(shí)刻，將所述車輛在所述初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在各個(gè)預(yù)設(shè)時(shí)刻的估算位置進(jìn)行融合，得到所述車輛在各個(gè)預(yù)設(shè)時(shí)刻的最終位置，由于提高了車輛的位置信息的更新頻率，即使車輛在GPS信號的信號強(qiáng)度較弱或者無法接收GPS信號時(shí)，也可以得到車輛在預(yù)設(shè)時(shí)間段內(nèi)的各個(gè)時(shí)刻的最終位置，因而可以提高定位的精確度，提升用戶的使用體驗(yàn)。為使本發(fā)明的上述目的、特征和優(yōu)點(diǎn)能夠更為明顯易懂，下面結(jié)合附圖對本發(fā)明的具體實(shí)施例做詳細(xì)的說明。圖1示出了本發(fā)明實(shí)施例中的一種導(dǎo)航定位方法的流程圖。參見圖1，在具體實(shí)施中，本發(fā)明實(shí)施例中的導(dǎo)航定位方法可以包括如下的步驟：步驟S101：在預(yù)設(shè)時(shí)間段內(nèi)，基于車輛在初始時(shí)刻接收到的全球定位系統(tǒng)(GlobalPositioningSystem，GPS)定位衛(wèi)星發(fā)射的GPS信號，確定所述車輛在初始時(shí)刻的第一初始位置的信息，并基于車輛在初始時(shí)刻接收到的移動基站發(fā)送的定位信號，確定所述車輛在所述初始時(shí)刻的第二初始位置的信息。在本發(fā)明一實(shí)施例中，所述預(yù)設(shè)時(shí)間段的時(shí)長與所述GPS信號的更新頻率對應(yīng)。例如，當(dāng)所述GPS信號的更新頻率為1s/次時(shí)，所述預(yù)設(shè)的時(shí)間段的時(shí)長為1s。步驟S102：基于所獲取的車輛在前一時(shí)刻的最終位置的信息，以及當(dāng)前時(shí)刻測量得到的里程計(jì)測量值和方位角測量值，計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息。在本發(fā)明一實(shí)施例中，當(dāng)前時(shí)刻與前一時(shí)刻與之間的時(shí)間間隔與所述里程計(jì)測量值和方位角測量值的更新頻率對應(yīng)，如當(dāng)里程計(jì)測量值和方位角測量值的更新頻率為10次/s時(shí)，所述預(yù)設(shè)的時(shí)間段的時(shí)長為0.1s。步驟S103：將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行融合，得到所述車輛在當(dāng)前時(shí)刻的最終位置。在發(fā)明一實(shí)施例中，可以采用卡爾曼濾波將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行融合，得到所述車輛在當(dāng)前時(shí)刻的最終位置，具體請參見圖2。下面將結(jié)合圖2對本發(fā)明實(shí)施例中的導(dǎo)航定位方法做進(jìn)一步詳細(xì)的介紹。圖2示出了本發(fā)明實(shí)施例中的另一種導(dǎo)航定位方法的流程圖。參見圖2，在具體實(shí)施中，本發(fā)明實(shí)施例中的導(dǎo)航定位方法可以包括如下的步驟：步驟S201：在預(yù)設(shè)時(shí)間段的初始時(shí)刻，分別通過直接接收的GPS信號和移動基站發(fā)送的定位信號，確定車輛在所述初始時(shí)刻的第一初始位置和第二初始位置。在本發(fā)明一實(shí)施例中，所述預(yù)設(shè)時(shí)間段的時(shí)長與GPS信號的更新頻率相關(guān)。例如，當(dāng)GPS信號的更新頻率為1次/s時(shí)，也即GPS信號每1s更新一次時(shí)，預(yù)設(shè)時(shí)間段的時(shí)長為1s。在具體實(shí)施中，車輛可以通過自身的接收機(jī)直接接收到的GPS信號獲取車輛在初始時(shí)刻的第一初始位置，也即所述初始時(shí)刻的第一初始位置的東向位置坐標(biāo)egps和北向位置坐標(biāo)ngps的信息。在具體實(shí)施中，車輛還可以通過移動基站發(fā)送的定位信號，如基于位置的定位服務(wù)(LocationBasedService，LBS)獲取車輛在初始時(shí)刻的第二初始位置，也即所述初始時(shí)刻的第二初始位置的東向位置坐標(biāo)elbs(i)和北向位置坐標(biāo)nlbs(i)的信息。其中，LBS為通過電信、移動等通信網(wǎng)絡(luò)運(yùn)營商的無線電通訊網(wǎng)絡(luò)如GSM、CDMA，或者通過如GPS的外部定位方式獲取移動終端的位置信息，在地理信息系統(tǒng)(GeographicInformationSystem，GIS)平臺的支持下，為用戶提供的一種增值業(yè)務(wù)。步驟S202：從所述初始時(shí)刻開始，基于車輛在前一時(shí)刻的最終位置的信息，以及通過當(dāng)前時(shí)刻測量得到的里程計(jì)測量值和方位角測量值，采用航位推算法計(jì)算得到當(dāng)前時(shí)刻的估算位置的信息。在具體實(shí)施中，分別采用里程計(jì)傳感器和陀螺儀測量得到車輛在所述預(yù)設(shè)時(shí)間的當(dāng)前時(shí)刻的里程計(jì)測量值和方位角測量值。其中，所述初始時(shí)間段內(nèi)相鄰兩個(gè)時(shí)刻之間的時(shí)間間隔與里程計(jì)測量值和方位角測量值的更新頻率相關(guān)。例如，當(dāng)里程計(jì)測量值和方位角測量值的更新頻率的更新頻率為10次/s時(shí)，也即里程計(jì)測量值和方位角測量值的更新頻率信號每0.1s更新一次時(shí)，預(yù)設(shè)時(shí)間段的時(shí)長為0.1s。在具體實(shí)施中，可以首先利用陀螺儀測量得到的方位角測量值確定運(yùn)載體的轉(zhuǎn)動，并從中推算出運(yùn)載體相對于參考坐標(biāo)系，即載體坐標(biāo)系的姿態(tài)信息。將導(dǎo)航參考坐標(biāo)系，記為n系，與東—北—天(ENU)坐標(biāo)系重合；慣組坐標(biāo)系，記為b系，與載體坐標(biāo)系重合；里程計(jì)坐標(biāo)系，記為m系。在具體實(shí)施中，GPS坐標(biāo)系，即導(dǎo)航坐標(biāo)系與載體坐標(biāo)系的具體表示方法不同，因而需要將得到的載體的運(yùn)動位置和狀態(tài)從載體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系。具體而言，假定載體運(yùn)動的方位角為ψ，縱擺角為θ，橫擺角為γ，載體在初始時(shí)刻的載體坐標(biāo)系表示為(oxb，yb，zb)，并假設(shè)其與導(dǎo)航坐標(biāo)系(oxn，yn，zn)完全重合。在載體坐標(biāo)系經(jīng)過一定的時(shí)間發(fā)生變化后，可以通過三次地理坐標(biāo)系旋轉(zhuǎn)得到在新的載體坐標(biāo)系的坐標(biāo)，即首先將(xn，yn，zn)首先沿著繞zb軸旋轉(zhuǎn)ψ得到(x'，y'，z')，將(x'，y'，z')沿著繞x'軸旋轉(zhuǎn)θ得到(x"，y"，z")，最后將(x"，y"，z")沿著繞y"軸旋轉(zhuǎn)γ得到(xb，yb，zb)，由此可得：xbybzb=cosγ0-sinγ010sinγ0cosγ1000cosθsinθ0-sinθcosθcosψsinψ0-sinψcosψ0011xnynzn---(1)]]>從而可以得到兩坐標(biāo)系間的變化矩陣為：Cnb=cosγcosψ-sinγsinθsinψcosγsinψ+sinγsinθcosψ-sinγcosθ-cosθsinψcosθcosψsinθsinγcosψ+cosγsinθsinψsinγsinψ-cosγsinθcosψcosγcosθ---(2)]]>Cbn=(Cnb)T---(3)]]>其中，Cb"表示對應(yīng)的姿態(tài)矩陣。在具體實(shí)施中，當(dāng)?shù)玫讲煌鴺?biāo)系之間的對應(yīng)的姿態(tài)矩陣時(shí)，可以當(dāng)前時(shí)刻與前一時(shí)刻之間的里程增量的信息，計(jì)算得到前一時(shí)刻和當(dāng)前時(shí)刻在導(dǎo)航坐標(biāo)系的里程增量。將導(dǎo)航參考坐標(biāo)系、慣組坐標(biāo)系、里程計(jì)坐標(biāo)系分別記為n系、b系、m系。其中選取n系與東—北—天(ENU)坐標(biāo)系重合，b系與載體坐標(biāo)系重合，則當(dāng)前時(shí)刻ti與前一時(shí)刻ti-1之間的里程增量ΔSi在m系的投影可以表示為：ΔSim=0ΔSi0T---(4)]]>假定表示航向安裝偏差角，θr表示俯仰安裝偏差角，γr表示橫滾安裝偏差角，則當(dāng)前時(shí)刻ti與前一時(shí)刻ti-1之間的里程增量ΔSi在b系上里程增量的投影可以表示為：同理，當(dāng)前時(shí)刻ti與前一時(shí)刻ti-1之間的里程增量ΔSi在n系的投影可以表示為：ΔSin=Cbn(i)ΔSib---(6)]]>其中，表示當(dāng)前時(shí)刻ti時(shí)刻的姿態(tài)矩陣。在采用上述的公式計(jì)算得到對應(yīng)的里程增量之后，基于前一時(shí)刻的最終位置的信息，采用航位推算法，即可得到當(dāng)前時(shí)刻的估算位置的信息，也即采用如下的公式基于前一時(shí)刻的最終位置的信息推算得到當(dāng)前時(shí)刻的估算位置：λi=λi-1+ΔSiEnsecLi-1RN+hi-1---(7)]]>Li=Li-1+ΔSiNnRM+hi-1---(8)]]>hi=hi-1+ΔSiUn---(9)]]>其中，λi、Li、hi分別表示當(dāng)前時(shí)刻ti的經(jīng)度、緯度和高度，λi-1、Li-1、hi-1分別表示前一時(shí)刻ti-1的經(jīng)度、緯度和高度，n表示導(dǎo)航坐標(biāo)系，分別表示當(dāng)前時(shí)刻ti與前一時(shí)刻ti-1的里程數(shù)差值的東向分量、北向分量和天向分量，RM、RN分別為載體所在地理位置對應(yīng)的地球子午圈曲率半徑和卯酉圈曲率半徑，RM,RN的計(jì)算公式可以近似為：RM＝Re(1-2f+3fsin2L)(10)RN＝Re(1+f+fsin2L)(11)其中，Re為地球參考橢球的長半軸，f為參考橢球的扁率。步驟S203：將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置分別進(jìn)行卡爾曼濾波，并將第一初始位置的卡爾曼濾波結(jié)果、第二初始位置的卡爾曼濾波結(jié)果和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置的卡爾曼濾波結(jié)果進(jìn)行融合，得到所述車輛在當(dāng)前時(shí)刻的最終位置。在本發(fā)明一實(shí)施例中，采用卡爾曼濾波器將車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行融合，得到所述車輛在當(dāng)前時(shí)刻的最終位置。具體而言，選取東北天坐標(biāo)系作為參考坐標(biāo)系，假定系統(tǒng)狀態(tài)變量X1＝[eveaenvnan]T，其中，(e,n)是東北位置分量，單位為m；(ve,vn)是速度分量，單位為m/s；(ae,an)是速度分量，單位為m/s2。在本發(fā)明一實(shí)施例中，基于車輛在初始時(shí)刻接收到的GPS信號，采用如下的公式，對計(jì)算得到第一初始位置進(jìn)行卡爾曼濾波，也即采用卡爾曼濾波器中的第一子濾波器對第一初始位置進(jìn)行卡爾曼濾波：X1(i)＝Φ1(i|i-1)X1(i-1)+U1(i-1)(12)Z1(i)＝H1(i)X1(i)+V1(i)(13)X^1(i|i-1)=Φ1(i-1)X^1(i-1)---(14)]]>P1(i|i-1)=Φ1(i-1)P1(i-1)Φ1T(i-1)+Q1(i-1)---(15)]]>X^1(i)=P1(i)[H1T(i)R1-1(i)Z1(i)+P1-1(i|i-1)X^1(i|i-1)]---(16)]]>P1-1(i|i-1)=P1-1(i|i-1)+H1T(i)R1-1(i)H1(i)---(17)]]>且：Z1(i)=egps(i),ngps(i)T---(18)]]>H1(i)=100000000100---(19)]]>V1(i)=ve(i),vn(i)T---(20)]]>R1(i)=diagσ12,σ22---(21)]]>其中，表示第一初始位置經(jīng)過卡爾曼濾波器中的第一子濾波器處理輸出的最終位置信息，表示前一時(shí)刻ti-1的最終位置經(jīng)過卡爾曼濾波器中的第一子濾波器處理輸出的最終位置信息，Z1(i)表示所述第一初始位置，P1(i-1)表示X1(i|i-1)對應(yīng)的第一協(xié)方差，Q1(i-1)表示對應(yīng)的第一系統(tǒng)協(xié)方差矩陣，Φ3(i|i-1)表示對應(yīng)的第一狀態(tài)轉(zhuǎn)移矩陣，U1(i-1)表示對應(yīng)的第一系統(tǒng)噪聲，H1(i-1)表示前一時(shí)刻ti-1對應(yīng)的第一觀察矩陣，V1(i)表示當(dāng)前時(shí)刻ti對應(yīng)的第一系統(tǒng)觀測噪聲，分別表示當(dāng)前時(shí)刻ti對應(yīng)的GPS接收機(jī)的東向位置測量噪聲和北向位置測量噪聲，分別表示對應(yīng)的高斯白噪聲，R1(i)表示當(dāng)前時(shí)刻ti對應(yīng)的第一測量噪聲協(xié)方差矩陣。同理，基于車輛在初始時(shí)刻接收到移動基站發(fā)送的定位信號，可以采用如下的公式，對計(jì)算得到第二初始位置進(jìn)行卡爾曼濾波，也即采用卡爾曼濾波器中的第二子濾波器對第二初始位置進(jìn)行卡爾曼濾波：X2(i)＝Φ2(i|i-1)X2(i-1)+U2(i-1)(22)Z2(i)＝H2(i)X2(i)+V2(i)(23)X^2(i|i-1)=Φ2(i-1)X^2(i-1)---(24)]]>P2(i|i-1)=Φ2(i-1)P2(i-1)Φ2T(i-1)+Q2(i-1)---(25)]]>X^2(i)=P2(i)[H2T(i)R2-1(i)Z2(i)+P2-1(i|i-1)X^2(i|i-1)]---(26)]]>P2-1(i|i-1)=P2-1(i|i-1)+H2T(i)R2-1(i)H2(i)---(27)]]>且：Z2(i)=elbs(i),nlbs(i)T---(28)]]>H2(i)=100000000100---(29)]]>V2(i)=ve2(i),vn2(i)T---(30)]]>R2(i)=diagσ32,σ42---(31)]]>其中，表示第二初始位置經(jīng)過卡爾曼濾波器中的第二子濾波器處理輸出的最終位置信息，表示前一時(shí)刻ti-1的最終位置經(jīng)過卡爾曼濾波器中的第二子濾波器處理輸出的最終位置信息，Z2(i)表示所述第二初始位置，P2(i-1)表示Xi(i-1|i)對應(yīng)的第二協(xié)方差，Q2(i-1)表示對應(yīng)的第二系統(tǒng)協(xié)方差矩陣，Φ2(i|i-1)表示對應(yīng)的第二狀態(tài)轉(zhuǎn)移矩陣，U2(i-1)表示對應(yīng)的第二系統(tǒng)噪聲，H2(i-1)表示前一時(shí)刻ti-1對應(yīng)的第二觀察矩陣，V2(i)表示當(dāng)前時(shí)刻ti對應(yīng)的第二觀測噪聲，ve2(i)、vn2(i)分別表示當(dāng)前時(shí)刻ti對應(yīng)的LBS模塊輸出的東向位置測量噪聲和北向位置測量噪聲，分別表示對應(yīng)的高斯白噪聲，R2(i)表示當(dāng)前時(shí)刻ti對應(yīng)的第二測量噪聲協(xié)方差矩陣。類似地，基于車輛在所述預(yù)算時(shí)間段中的各個(gè)當(dāng)前時(shí)刻的估算位置，采用如下的公式，對計(jì)算得到的當(dāng)前時(shí)刻的估算位置進(jìn)行卡爾曼濾波，也即采用卡爾曼濾波器中的第三子濾波器對當(dāng)前時(shí)刻的估算位置進(jìn)行卡爾曼濾波：X3(i)＝Φ3(i|i-1)X3(i-1)+U3(i-1)(32)Z3(i)＝H3(i)X3(i)+V3(i)(33)X^3(i|i-1)=Φ3(i-1)X^3(i-1)---(34)]]>P3(i|i-1)=Φ3(i-1)P3(i-1)Φ3T(i-1)+Q3(i-1)---(35)]]>X^3(i)=X^3(i-1)+K3(i)[Z3(i)-H3(i)X3(i|i-1)]---(36)]]>K3(i)=P3(i|i-1)H3T(i)[H3(i)P3(i|i-1)H3T(i)+R3(i)]---(37)]]>P3(i+1)=[I-K3(i)H3(i)]P3(i|i-1)---(38)]]>且：Z3(i)=eins+dr(i),nins+dr(i)T---(39)]]>H3(i,X(i))=[arctan[Ve(i)Ve(i)]TVe32(i)+Vn32(i)]T---(40)]]>V3(i)=ve3(i),vn3(i)T---(41)]]>R3(i)=diagσ52,σ62---(42)]]>其中，X3(i)表示當(dāng)前時(shí)刻ti的估算位置經(jīng)卡爾曼濾波器中的第三濾波器處理輸出的最終位置信息，表示前一時(shí)刻ti-1的最終位置經(jīng)過卡爾曼濾波器中的第三子濾波器處理輸出的最終位置信息，Z3(i)表示當(dāng)前時(shí)刻ti的估算位置，P3(i-1)表示X3(i|i-1)對應(yīng)的第三協(xié)方差，Q3(i-1)表示對應(yīng)的第三系統(tǒng)協(xié)方差矩陣，eins+dr(i)表示當(dāng)前時(shí)刻ti的估算位置對應(yīng)的東向位置，nins+dr(i)表示當(dāng)前時(shí)刻ti的估算位置對應(yīng)的北向位置，Φ3(i|i-1)表示對應(yīng)的第三系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，U3(i-1)表示對應(yīng)的第三系統(tǒng)噪聲，H3(i-1)表示對應(yīng)的第三系統(tǒng)觀察矩陣，V3(i)表示對應(yīng)的第三系統(tǒng)觀測噪聲，ve3(i)、en3(i)分別表示當(dāng)前時(shí)刻的估算位置對應(yīng)的東向位置測量噪聲和北向位置測量噪聲，分別表示對應(yīng)的高斯白噪聲，R3(i)表示對應(yīng)的第三測量噪聲協(xié)方差矩陣，I是單位矩陣。在具體實(shí)施中，當(dāng)?shù)玫娇柭鼮V波器中的第一子濾波器、第二子濾波器和第三子濾波分別對所述車輛的第一初始位置、第二初始位置和當(dāng)前時(shí)刻的估算位置進(jìn)行濾波得到的結(jié)果時(shí)，可以采用如下的公式，將所述車輛的第一初始位置、第二初始位置和當(dāng)前時(shí)刻的估算位置在卡爾曼濾波器中主濾波器進(jìn)行融合：Q-1(i)=Q1-1(i)+Q2-1(i)+Q3-1(i)---(43)]]>P-1(i)=P1-1(i)+P2-1(i)+P3-1(i)---(44)]]>X^(i)=P-1(i)(P1-1(i)X^1(i)+P2-1(i)X^2(i)+P3-1(i)X^3(i))---(45)]]>其中，表示當(dāng)前時(shí)刻的最終位置。在進(jìn)行下一次遞推之前，將主濾波器的輸出，即當(dāng)前時(shí)刻的第一初始位置、第二初始位置和估算位置采用卡爾曼濾波得到的結(jié)果，即當(dāng)前時(shí)刻ti的最終位置當(dāng)前時(shí)刻ti的最終位置對應(yīng)的協(xié)方差Pi、當(dāng)前時(shí)刻ti的最終位置對應(yīng)的協(xié)方差矩陣Qi的信息，在第一子濾波器、第二子濾波器和第三子濾波器之間按照如下的規(guī)則進(jìn)行分配：且：β1=0.99PDOP≤22/PDOP2<PDOP<51/PDOP5≤PDOP<100.5/PDOP10≤PDOP---(47)]]>λ={0.5W>-85dBm0.5*(100-W)/15-100dBm≤W<-85dBm0W<-100dBm---(48)]]>β2=λ5≤PDOPλ*(1-(5-PDOP)/3)2≤PDOP<50PDOP<2---(49)]]>β3＝1-β1-β2(50)其中，i取1、2、3，PDOP表示GPS信號的PDOP精度因子，W表示移動基站的信號強(qiáng)度。這里需要指出的是，進(jìn)行第一次卡爾曼濾波時(shí)，所使用的全局狀態(tài)的初始值，即X0、P0和Q0可以基于經(jīng)驗(yàn)進(jìn)行設(shè)定。在具體實(shí)施中，本發(fā)明實(shí)施例中的導(dǎo)航定位方法還可以包括：步驟S204：通過無線方式將所述車輛在當(dāng)前時(shí)刻的最終位置的信息發(fā)送至其他車輛。在具體實(shí)施中，為了使得所述車輛附近的其他車輛可以方便地獲取當(dāng)前位置的信息，所述車輛可以在通過上述步驟S201至S203推算得到的車輛在預(yù)設(shè)時(shí)間段中當(dāng)前時(shí)刻的最終位置的信息通過無線的方式發(fā)送給附近的其他車輛。上述對本發(fā)明實(shí)施例中的導(dǎo)航定位方法做了相應(yīng)的介紹，下面將對上述的方法對應(yīng)的裝置做進(jìn)一步詳細(xì)的介紹。圖3示出了本發(fā)明實(shí)施例中的一種導(dǎo)航定位裝置的結(jié)構(gòu)。參見圖3，在具體實(shí)施中，本發(fā)明實(shí)施例中的導(dǎo)航定位裝置300可以包括初始位置確定單元301、計(jì)算單元302和融合定位單元303，其中：所述初始位置確定單元301，適于在預(yù)設(shè)時(shí)間段內(nèi)，基于車輛在初始時(shí)刻接收到的GPS定位衛(wèi)星發(fā)射的GPS信號，確定所述車輛在初始時(shí)刻的第一初始位置的信息，并基于車輛在初始時(shí)刻接收到的移動基站發(fā)送的定位信號，確定所述車輛在所述初始時(shí)刻的第二初始位置的信息；其中，所述預(yù)設(shè)時(shí)間段的時(shí)長與所述GPS信號的更新頻率對應(yīng)。所述計(jì)算單元302，適于基于所獲取的車輛在前一時(shí)刻的最終位置的信息，以及當(dāng)前時(shí)刻測量得到的里程計(jì)測量值和方位角測量值，計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息；其中，當(dāng)前時(shí)刻與前一時(shí)刻與之間的時(shí)間間隔與所述里程計(jì)測量值和方位角測量值的更新頻率對應(yīng)。所述融合定位單元303，適于將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行融合，得到所述車輛在當(dāng)前時(shí)刻的最終位置。在具體實(shí)施中，所述初始位置確定單元301，適于基于所獲取的車輛在前一時(shí)刻的最終位置的信息，以及當(dāng)前時(shí)刻測量得到的里程計(jì)測量值和方位角測量值，采用航位推算法計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息。在本發(fā)明一實(shí)施例中，所述計(jì)算單元302適于采用如下的公式計(jì)算得到所述車輛在當(dāng)前時(shí)刻的估算位置的信息：其中，λi、Li、hi分別表示當(dāng)前時(shí)刻ti的經(jīng)度、緯度和高度，λi-1、Li-1、hi-1分別表示前一時(shí)刻ti-1的經(jīng)度、緯度和高度，n表示導(dǎo)航坐標(biāo)系，分別表示當(dāng)前時(shí)刻ti與前一時(shí)刻ti-1的里程數(shù)差值的東向分量、北向分量和天向分量，RM、RN分別為載體所在地理位置對應(yīng)的地球子午圈曲率半徑和卯酉圈曲率半徑。在本發(fā)明一實(shí)施例中，所述融合定位單元303，適于將所述車輛在初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在當(dāng)前時(shí)刻的估算位置進(jìn)行卡爾曼濾波，得到所述車輛在當(dāng)前時(shí)刻的最終位置。在本發(fā)明一實(shí)施例中，所述融合定位單元303，適于計(jì)算得到所述車輛在當(dāng)前時(shí)刻的最終位置，包括：且：β1=0.99PDOP≤22/PDOP2<PDOP<51/PDOP5≤PDOP<100.5/PDOP10≤PDOP;]]>β2=λ5≤PDOPλ*(1-(5-PDOP)/3)2≤PDOP<50PDOP<2;]]>β3＝1-β1-β2；λ={0.5W>-85dBm0.5*(100-W)/15-100dBm≤W<-85dBm0W<-100dBm;]]>其中，i表示當(dāng)前時(shí)刻ti，表示車輛在所述當(dāng)前時(shí)刻ti的最終位置，表示車輛在初始時(shí)刻t0的第一初始位置，表示車輛在初始時(shí)刻t0的第二初始位置，P(i)表示當(dāng)前時(shí)刻ti對應(yīng)的協(xié)方差矩陣，PDOP表示GPS信號的精度因子，W表示移動基站發(fā)送的定位信號的信號強(qiáng)度。在本發(fā)明一實(shí)施例中，所述預(yù)設(shè)時(shí)間段的時(shí)長為1s，當(dāng)前時(shí)刻與當(dāng)前時(shí)刻之間的時(shí)間間隔為0.1s。在具體實(shí)施中，所述導(dǎo)航定位裝置300還可以包括發(fā)送單元304，其中：發(fā)送單元304，適于通過無線方式將所述車輛在當(dāng)前時(shí)刻的最終位置的信息發(fā)送至其他車輛。上述的方案，在預(yù)設(shè)時(shí)間段內(nèi)的各個(gè)預(yù)設(shè)時(shí)刻，將所述車輛在所述初始時(shí)刻的第一初始位置、第二初始位置和計(jì)算到的車輛在各個(gè)預(yù)設(shè)時(shí)刻的估算位置進(jìn)行融合，得到所述車輛在各個(gè)預(yù)設(shè)時(shí)刻的最終位置，由于提高了車輛的位置信息的更新頻率，即使車輛在GPS信號的信號強(qiáng)度較弱或者無法接收GPS信號時(shí)，也可以得到車輛在預(yù)設(shè)時(shí)間段內(nèi)的各個(gè)時(shí)刻的最終位置，因而可以提高定位的精確度，提升用戶的使用體驗(yàn)。本領(lǐng)域普通技術(shù)人員可以理解上述實(shí)施例的各種方法中的全部或部分步驟是可以通過程序來指令相關(guān)的硬件來完成，該程序可以存儲于計(jì)算機(jī)可讀存儲介質(zhì)中，存儲介質(zhì)可以包括：ROM、RAM、磁盤或光盤等。以上對本發(fā)明實(shí)施例的方法及系統(tǒng)做了詳細(xì)的介紹，本發(fā)明并不限于此。任何本領(lǐng)域技術(shù)人員，在不脫離本發(fā)明的精神和范圍內(nèi)，均可作各種更動與修改，因此本發(fā)明的保護(hù)范圍應(yīng)當(dāng)以權(quán)利要求所限定的范圍為準(zhǔn)。當(dāng)前第1頁1 2 3