專利名稱：航位推算中初始方向的確定方法
技術(shù)領(lǐng)域：
本發(fā)明涉及導(dǎo)^^支術(shù)領(lǐng)域，尤指一種航位推算中初始方向的確定方法。
背景技術(shù)：
在導(dǎo)^iU支術(shù)領(lǐng)域，GPS (全球定位系統(tǒng))是美國(guó)第二代無(wú)線電衛(wèi)星導(dǎo)航定位系 統(tǒng)，它負(fù)^夠全球性、全天候、實(shí)時(shí)地提糾航、定位和授時(shí)信息，但它要求可見(jiàn)衛(wèi) 星數(shù)量至少iii'j 3顆才能正常定位。在高樓林立的城市道路、隧道或樹(shù)木濃密等GPS 信號(hào)遮擋比較嚴(yán)重的地域，由于可見(jiàn)衛(wèi)星數(shù)量太少，GPS將不能正常獲得準(zhǔn)確可靠 的位置數(shù)據(jù)。在這種情況下，通常將GPS與其他傳感器(如貫導(dǎo)、電子羅盤等)進(jìn) ^iEL合，來(lái)消除這種影響。即，當(dāng)GPS由于信號(hào)遮擋而不能可靠定位時(shí)，GPS/DR(航 位推算，Dead-Reckoning)系統(tǒng)自動(dòng)切換到DR ^^方式來(lái)持續(xù)輸出車輛在^—時(shí)刻 的位置信息，直至GPS十^1正常接》|1^,系統(tǒng)再回到GPS與DR的組合^M元方式。
DR是一種常用的，定位技術(shù)，其基本原理是利用方向傳感器和i!A傳感器 來(lái)推算車輛的瞬時(shí)位置，可以實(shí)現(xiàn)連續(xù)自主式定位。但由于期,算過(guò)程是一個(gè)累加 過(guò)程，方向傳感器的誤差隨時(shí)間的延長(zhǎng)而積累；另夕卜，推算只能確定相對(duì)位置和航 向。導(dǎo)航開(kāi)始時(shí)，需要預(yù)知車輛的初始絕對(duì)位置和初始方向。DR技術(shù)所使用到的硬 lti殳備包括車速傳感器和陀螺儀，其中車速傳感器推算當(dāng)前的運(yùn)動(dòng)距離，陀螺儀 推算當(dāng)前的運(yùn)動(dòng)方向，兩者的融^^得到當(dāng)前車輛的準(zhǔn)確位置。
車輛沿著待測(cè)道^^f亍駛，車載GPS/DR系統(tǒng)中的GPS設(shè)備實(shí)時(shí)接收GPS衛(wèi)星信 號(hào)，并從中獲取車輛位置、衛(wèi)星接收狀態(tài)等數(shù)據(jù)。在衛(wèi)星接收狀態(tài)良好時(shí)，將相鄰 時(shí)刻的GPS定位點(diǎn)的連線方向作為車輛的行駛方向，當(dāng)接收狀態(tài)不好時(shí)，以此方向 作為航位推算的起始基準(zhǔn)方向。也就是說(shuō)，現(xiàn)有技術(shù)M用相鄰時(shí)刻的GPS定位點(diǎn) 的連線方向作為艇位推算的初始方向。為了保證^/f立推算的初始方向的準(zhǔn)確性，一 ^^選擇一條比較開(kāi)闊無(wú)遮擋的平直道路，且車輛能夠以較高的# (如〉70km/h) 持續(xù)行駛一段時(shí)間。開(kāi)闊的環(huán)境能確保GPS接收才/W妻^^較多的GPS衛(wèi)星(如>6), 提供比較精確的定位信息，平直的道路則可以使初始方向不會(huì)有太大的變化，而較 高的車速可以讓相鄰的兩個(gè)GPS定位點(diǎn)之間的距離加大，從而提高初始方向的準(zhǔn)確性。
綜上所述，現(xiàn)有技術(shù)航位導(dǎo)航中初始方向的確定方法存在下列缺陷
首先，由于在擁擠的城市道路上，車輛太多，車速"fi4較低，而且城市街道高
樓林立，遮擋比較"fi4,比較難以滿足開(kāi)闊條件的要求。
其次，規(guī)有技術(shù)^L^用相鄰的兩個(gè)GPS定位點(diǎn)連線方向作為航位推算初始方向， 由于兩個(gè)GPS定位點(diǎn)的距離太短，例如，假定車速為100km/h，則兩GPS定位點(diǎn)之間 的距離約為28m, GPS單點(diǎn)定位精度一般在5m以上，由》bit成的方向偏差可能必ij 20度，從而導(dǎo)致初始方向誤差較大。隨著時(shí)間的推移，航位推算的誤差累積將越來(lái) 越大，其定位結(jié)果4艮快將不可用。

發(fā)明內(nèi)容
本發(fā)明要解決的問(wèn)題是提供一種準(zhǔn)確性高的航位推算中初始方向的確定方法。 為了解決上述問(wèn)題，本發(fā)明航位推算中初始方向的確定方法的技術(shù)方案包括 步驟1)選取有效的GPS定位點(diǎn)；
步驟2 )在有效的GPS定位點(diǎn)中選取兩個(gè)GPS定位點(diǎn)之間的距離大于等于閾值 iU巨離偏差在容差范圍內(nèi)的兩個(gè)GPS定位點(diǎn)以確定初始方向線段； 步驟3)根據(jù)所述初始方向線段計(jì)算出初始方向。 其中，所述步驟1)還進(jìn)一步包括
步驟11)讀取GPS定位點(diǎn)并判斷其定位狀態(tài)是否有效，若無(wú)效，重復(fù)纟似亍步 驟ll);
步驟12)判斷所述GPS定位點(diǎn)是否是3D定卩封莫式，若不是，纟W亍步驟11); 步驟13)判斷衛(wèi)星it量是否大于等于閾值，若否，扭J亍步驟11); 步驟14)判斷7K平坐標(biāo)的精度強(qiáng)弱vl^否小于閾值，若否，執(zhí)行步驟ll); 步驟15)所述GPS定位點(diǎn)為有效，重復(fù)才W亍步驟11)直至選取出若干有效的 GPS定位點(diǎn)。
與現(xiàn)有技^N目比，本發(fā)明航位推算中初始方向的確定方法的有益效果為 首先，由于選取不相鄰的兩個(gè)GPS點(diǎn)的方向作為初始方向，這兩個(gè)不相鄰的點(diǎn)
之間的距離可以遠(yuǎn)大于相鄰兩點(diǎn)之間的距離，從而可以削弱由于GPS定位點(diǎn)的定位
誤差造成的初始方向誤差，準(zhǔn)確性高。
其次，由于作為計(jì)算初始方向的兩個(gè)GPS定位點(diǎn)可以不是相鄰的兩個(gè)GPS定
4位點(diǎn)，消除了必須依靠相鄰的兩GPS定位點(diǎn)來(lái)確定初始方向的限制。
向，從而降低了 GPS定位點(diǎn)數(shù)據(jù)有效的判斷標(biāo)準(zhǔn)。


圖1是本發(fā)明航位推算中初始方向的確定方法的法L^呈圖； 圖2是圖1中的步驟1)的進(jìn)一步細(xì)分的^d呈圖。
具體實(shí)施例方式
如圖1所示，本發(fā)明航/f立推算中初始方向的確定方法包括 步驟1)選取有效的GPS定位點(diǎn)；
步驟2 )在有效的GPS定位點(diǎn)中選取兩個(gè)GPS定位點(diǎn)之間的距離大于等于閾值 JJ巨離偏差在容差范圍內(nèi)的兩個(gè)GPS定位點(diǎn)以確定初始方向線—度； 步驟3)根據(jù)所述初始方向線段計(jì)算出初始方向。
由上述可知，本發(fā)明M選取有效的GPS定位點(diǎn)，也就是說(shuō)只有滿足一定^^牛 的GPS定位點(diǎn)才有可能作為判斷初始方向的勤出。這樣可以進(jìn)一步地J呆證結(jié)果的準(zhǔn) 確性。
其中，如圖2所示，所述步驟l)還進(jìn)一步包括
步驟11)讀取GPS定位點(diǎn)并判斷其定位狀態(tài)是否有效，若無(wú)效，重復(fù)執(zhí)行步 驟ll);
步驟12)判斷所述GPS定位點(diǎn)是否是3D定4封莫式，若不是，4似于步驟11);
步驟13)判斷衛(wèi)星數(shù)量是否大于等于閾值，若否，執(zhí)行步驟ll);
步驟14)判斷HDOP (水平(即二維)坐標(biāo)的精度強(qiáng)弱度，Horizontal Dilution ofPrecision)是否小于閾值，若否，執(zhí)行步驟ll);
步驟15)所述GPS定位點(diǎn)為有效，重復(fù)執(zhí)行步驟11)直至選取出若干有效的 GPS定位點(diǎn)。
由上述可知，作為有效的GPS定位點(diǎn)的條件A^位狀態(tài)有效、3D定位模式、 衛(wèi)星數(shù)量和HDOP的值，所述HDOP的值指緯度和經(jīng)度等誤差平方和的開(kāi)根號(hào)值。 只有這四個(gè)條件都滿足的GPS定位點(diǎn)才被視為有效的GPS定位點(diǎn)。
一般情況下，衛(wèi)星數(shù)量的閾值設(shè)為5， HDOP的閾值設(shè)為4。有效GPS定位點(diǎn)
5的判斷力fd呈如下接4tJiJGPS數(shù)據(jù)后，先判斷該條數(shù)悟的有效性，如果無(wú)效，則該 GPS定位點(diǎn)無(wú)效，接著讀取下一個(gè)GPS定位點(diǎn)并判斷；若有效，則判斷GPS定位 模式，如果不為3D模式，則接著讀取下一個(gè)GPS定位點(diǎn)并判斷；若符合^f牛，則 判斷衛(wèi)星凄tA否符合條件，如衛(wèi)星數(shù)<5，則該GPS定位點(diǎn)無(wú)效，4妻著讀取下一個(gè) GPS定位點(diǎn)并判斷；若符合^^牛，判斷HDOP是否小于4，若不符合^^牛，GPS定 位點(diǎn)無(wú)效，接著讀耳又下一個(gè)GPS定位點(diǎn)并判斷，若符合^^牛，該GPS定位點(diǎn)被視 為有效GPS定位點(diǎn)。
在確定初始方向線段的時(shí)候，以兩個(gè)GPS定位點(diǎn)組成的線段的方向作為初始方 向，本發(fā)明航位推算中初始方向的確定方法選取的這兩個(gè)GPS定位點(diǎn)應(yīng)滿足如下條 件
1) 兩個(gè)GPS定位點(diǎn)有效
2) 兩GPS定位點(diǎn)之間的距離要大于閾值，例如〉50m
3 )由這兩個(gè)GPS定位點(diǎn)的GPS坐標(biāo)計(jì)算出的距離與由里程計(jì)測(cè)量出的這兩個(gè) GPS定位點(diǎn)之間的距離偏差在容差范圍內(nèi)，例如〈lm
螺儀角度輸出值在容差范圍內(nèi)(如-0.1度 0.1度)
當(dāng)這兩個(gè)GPS定位點(diǎn)確g，就可以計(jì)算初始方向了， "i殳這兩個(gè)GPS定位點(diǎn) 的坐標(biāo)分別為(xl,yl), (x2,y2),即可根據(jù)公式angle=arctan((yl-y2)/(xl-x2》(先計(jì)算 兩點(diǎn)在坐標(biāo)系y軸上的坐標(biāo)差(yl-y2)，然后計(jì)算兩點(diǎn)在坐標(biāo)系x軸上的坐標(biāo)差 (xl-x2)，再計(jì)算y軸坐標(biāo)差與x軸坐標(biāo)差之商(yl-y2)/(x1-x2)，(如果xl-x2^,即兩 點(diǎn)與y軸平行，如果yKy2，則初始方向90度，否則為270度)，最后對(duì)商求反正 切arctan，即可求得初始方向與x軸的夾角)來(lái)求出初始方向。
假設(shè)GPS/DR系統(tǒng)在第i時(shí)刻輸出的數(shù)據(jù)依次為x(i), y(i), angle(i), dist(i), valide(i), mode(i), star(i)， hdop(i)
其中x(i): GPS定位點(diǎn)正北向坐標(biāo)(招:影到高斯平面上的坐標(biāo)) y(i): GPS定位點(diǎn)正東向坐標(biāo)(投影到高斯平面上的坐標(biāo)) angle(i):陀螺4義輸出的角度 dist(i):里程計(jì)輸出的行馬妙巨離 valide(i): GPS輸出的數(shù)據(jù)有效性 mode(i): GPS定位模式
6star(i):接收到的衛(wèi)星數(shù) hdop(i): GPS定位點(diǎn)的hdop值
1、 如果>=2，首先判斷第i點(diǎn)是否有效，即valide(i)的定位狀態(tài)是否有效、mode(i) 是否為3D模式，star(i)是否〉二5,hdop(i)是否〈4,只有以上^^牛全部滿足，該定位點(diǎn) 才有效，否則無(wú)效。如果該點(diǎn)無(wú)效，則等待GPS/DR系統(tǒng)輸出第i+l點(diǎn)，然后重復(fù) 以上判斷；
2、 如果第i點(diǎn)有效，則回溯遍歷i-l點(diǎn)，如果其有效，且i-l、 i兩點(diǎn)之間的距 離dist(i)大于容差(如50m)，這兩點(diǎn)由GPS坐標(biāo)計(jì)算出的3巨離為 distGPS=sqi<(x(i》x(i-1)"2+(y(i)-y(i-lXT2),如果abs(distgps-dist(i))〈l(由GPS坐標(biāo)點(diǎn) 計(jì)算出來(lái)的距離和由里程計(jì)測(cè)量出的距離之差的絕對(duì)值)，且abs(angle(i-l))O.l, abs(angle(i-l)+angle(i))<l,則i、 i-l兩點(diǎn)組成的線段方向可作為初始方向。
3、 如果不滿足以上條件，則繼續(xù)遍歷i-2點(diǎn)直至第1個(gè)點(diǎn)，如果還是不滿足以 上條件，則等待GPS/DR系統(tǒng)輸出第i+l個(gè)點(diǎn)，直至找到符合作為初始方向線段的 兩個(gè)GPS定位點(diǎn)
4、 當(dāng)確定可以用來(lái)計(jì)算初始方向的兩個(gè)GPS定位點(diǎn)后(設(shè)坐標(biāo)分別為 (xl,yl),(x2，y2)),
則初始方向?yàn)閍ngle = arctn ((yl-y2)/(xl， x2》
綜上所述，本發(fā)明航位推算中初始方向的確定方法具有下列優(yōu)點(diǎn)
首先，由于選取不相鄰的兩個(gè)GPS點(diǎn)的方向作為初始方向，這兩個(gè)不相鄰的點(diǎn)
之間的距離可以遠(yuǎn)大于相鄰兩點(diǎn)之間的距離，從而可以削弱由于GPS定位點(diǎn)的定位
誤差造成的初始方向誤差，準(zhǔn)確性高。
位點(diǎn)，消除了必須依靠相鄰的兩GPS定位點(diǎn)來(lái)確定初始方向的限制。 向，從而降低了 GPS定位點(diǎn)數(shù)據(jù)有效的判斷標(biāo)準(zhǔn)。
權(quán)利要求
1、一種航位推算中初始方向的確定方法，其特征在于，包括步驟1)選取有效的GPS定位點(diǎn)；步驟2)在有效的GPS定位點(diǎn)中選取兩個(gè)GPS定位點(diǎn)之間的距離大于等于閾值且距離偏差在容差范圍內(nèi)的兩個(gè)GPS定位點(diǎn)以確定初始方向線段；步驟3)根據(jù)所述初始方向線段計(jì)算出初始方向。
2、 如權(quán)利要求1所述的航位推算中初始方向的確定方法，其特征在于，所述 步驟l)還進(jìn)一步包括步驟11)讀取GPS定位點(diǎn)并判斷其定位狀態(tài)是否有效，若無(wú)效，重復(fù)4W亍步 驟ll);步驟12)判斷所述GPS定位點(diǎn)是否是3D定^Mt式，若不是，^L^亍步驟11); 步驟13)判斷衛(wèi)星數(shù)量是否大于等于閾值，若否，執(zhí)行步驟ll); 步驟14)判斷水平坐標(biāo)的精度強(qiáng)弱AA否小于閾值，若否，執(zhí)行步驟ll); 步驟15 )所述GPS定位點(diǎn)為有效，重復(fù)4力f亍步驟11)直至選耳又出若干有效的 GPS定位點(diǎn)。
全文摘要
本發(fā)明公開(kāi)一種航位推算中初始方向的確定方法，包括選取有效的GPS定位點(diǎn)；在有效的GPS定位點(diǎn)中選取兩個(gè)GPS定位點(diǎn)之間的距離大于等于閾值且距離偏差在容差范圍內(nèi)的兩個(gè)GPS定位點(diǎn)以確定初始方向線段；根據(jù)所述初始方向線段計(jì)算出初始方向。采用本發(fā)明航位推算中初始方向的確定方法的技術(shù)方案能夠使得初始方向的確定更準(zhǔn)確。
文檔編號(hào)G01C21/26GK101464155SQ20071016102
公開(kāi)日2009年6月24日 申請(qǐng)日期2007年12月19日 優(yōu)先權(quán)日2007年12月19日
發(fā)明者楊自華 申請(qǐng)人:高德軟件有限公司