本發(fā)明涉及涉及衛(wèi)星導(dǎo)航定位技術(shù)。
背景技術(shù):
隨著導(dǎo)航定位技術(shù)的發(fā)展,導(dǎo)航定位在日常生活中日漸普及,但是受天氣和使用環(huán)境的影響,經(jīng)常發(fā)生南轅北轍的情況,定位精度的高低直接影響導(dǎo)航的結(jié)果。本發(fā)明裝置支持北斗和GPS衛(wèi)星定位裝置的接入,通過陀螺儀、加速度計和磁力計確立定位載體的方向,角速度和加速度,經(jīng)計算校正最終輸出定位載體的經(jīng)緯度和高度信息,經(jīng)實際定位測試誤差小于1米。
目前為了解決定位誤差過大的問題,而發(fā)射更多的導(dǎo)航定位衛(wèi)星。發(fā)射更多的導(dǎo)航定位衛(wèi)星固然可以提高導(dǎo)航定位的精度,但是成本高,在接收導(dǎo)航定位衛(wèi)星信號比較差的環(huán)境,導(dǎo)航定位的精度仍很難保證。
故,需要一種新的技術(shù)方案以解決上述問題。
技術(shù)實現(xiàn)要素:
本發(fā)明的目的在于克服上述方案的缺點,通過陀螺儀、加速度計和磁力計確立定位載體的方向,角速度和加速度,經(jīng)計算校正最終輸出定位載體的經(jīng)緯度和高度信息。
為實現(xiàn)上述目的,本發(fā)明可采用如下技術(shù)方案:
一種高精度衛(wèi)星定位的實現(xiàn)方法,包括以下步驟:
(1)、在定位載體上設(shè)置高精度定位裝置;該高精度定位裝置中包括處理器、輸入串口、輸入串口波特率撥碼開關(guān)、輸入串口協(xié)議撥碼開關(guān)、輸出串口、輸出串口波特率撥碼開關(guān)、輸出串口協(xié)議撥碼開關(guān);所述輸入串口、輸入串口波特率撥碼開關(guān)、輸入串口協(xié)議撥碼開關(guān)、輸出串口、輸出串口波特率撥碼開關(guān)、輸出串口協(xié)議撥碼開關(guān)均與處理器電性連接;其特征在于:還包括陀螺儀、加速度計、磁力計,陀螺儀、加速度計、磁力計也與處理器電性連接;
(2)、采集陀螺儀、加速度計和磁力計的數(shù)據(jù),采用陀螺儀和加速度計互為校正確立定位載體的各軸向的角度和加速度,最后結(jié)合磁力計計算各方向上的位移;
(3)、解析北斗/GPS通信協(xié)議,獲取當(dāng)前定位載體的經(jīng)緯度和高度信息;
(4)、計算輸出定位載體的經(jīng)緯度和高度信息。
相對與現(xiàn)有技術(shù),本發(fā)明具有如下優(yōu)點:
1)導(dǎo)航定位精度高,經(jīng)實際定位測試誤差小于1米;
2)本發(fā)明裝置體積較小,便于安裝,直接串聯(lián)在串口線上即可;
3)支持多種衛(wèi)星導(dǎo)航定位協(xié)議,如:NAEA-0183、NAEA-0180和NAEA-0182等;
4)通過設(shè)置輸入串口波特率和協(xié)議撥碼開關(guān)可以支持不同廠家的衛(wèi)星導(dǎo)航定位設(shè)備;
5)通過設(shè)置輸出串口波特率和協(xié)議撥碼開關(guān),可以和現(xiàn)有系統(tǒng)實現(xiàn)無縫對接,不需任何軟件修改。
附圖說明
圖1是本發(fā)明中高精度定位裝置的結(jié)構(gòu)示意圖。
圖2是本發(fā)明中軟件模塊的關(guān)系示意圖。
圖3是傳感器采集計算模塊工作流程圖。
圖4是北斗/GPS協(xié)議解析模塊工作流程圖。
圖5是計算校正輸出模塊工作流程圖。
具體實施方式
下面結(jié)合附圖,進一步闡明本發(fā)明,應(yīng)理解這些實施例僅用于說明本發(fā)明而不用于限制本發(fā)明的范圍,在閱讀了本發(fā)明之后,本領(lǐng)域技術(shù)人員對本發(fā)明的各種等價形式的修改均落于本申請所附權(quán)利要求所限定的范圍。
本發(fā)明公開一種高精度衛(wèi)星定位的實現(xiàn)方法,包括以下步驟:
(1)、如圖1所示,在定位載體上設(shè)置高精度定位裝置;該高精度定位裝置中包括處理器、輸入串口、輸入串口波特率撥碼開關(guān)、輸入串口協(xié)議撥碼開關(guān)、輸出串口、輸出串口波特率撥碼開關(guān)、輸出串口協(xié)議撥碼開關(guān);所述輸入串口、輸入串口波特率撥碼開關(guān)、輸入串口協(xié)議撥碼開關(guān)、輸出串口、輸出串口波特率撥碼開關(guān)、輸出串口協(xié)議撥碼開關(guān)均與處理器電性連接;其特征在于:還包括陀螺儀、加速度計、磁力計,陀螺儀、加速度計、磁力計也與處理器電性連接;
(2)、采集陀螺儀、加速度計和磁力計的數(shù)據(jù),采用陀螺儀和加速度計互為校正確立定位載體的各軸向的角度和加速度,最后結(jié)合磁力計計算各方向上的位移;
(3)、解析北斗/GPS通信協(xié)議,獲取當(dāng)前定位載體的經(jīng)緯度和高度信息;
(4)、計算輸出定位載體的經(jīng)緯度和高度信息。
其中,步驟(2)至步驟(4)中,需要設(shè)置軟件模塊進行實現(xiàn),軟件模塊包括傳感器采集計算模塊、北斗/GPS協(xié)議解析模塊、計算校正輸出模塊,各模塊之間的關(guān)系如圖2所示。
傳感器采集計算模塊負責(zé)采集陀螺儀、加速度計和磁力計的數(shù)據(jù),加速度計讀數(shù)受振動影響較大,采用陀螺儀和加速度計互為校正確立定位載體的各軸向的角度和加速度,最后結(jié)合磁力計計算各方向上的位移,其實現(xiàn)過程如圖3,即實現(xiàn)步驟(2),該步驟(2)中,包括如下細分步驟:
(2a)讀取加速度傳感器的當(dāng)前值:AxADCn、AyADCn、AzADCn,讀取時間:T1、T2、T3,最后一次讀取時間:T1n、T2n、T3n;
(2b)讀取陀螺儀傳感器的當(dāng)前值:∠xADCn、∠yADCn、∠zADCn;
(2c)重復(fù)步驟(2a)和(2b)100次并將其累計后,求其平均值分別為:AxADC、AyADC、AzADC和∠xADC、∠yADC、∠zADC;
(2d)分別將AxADC、AyADC、AzADC代入(ADC*Vref/1023-Vzg)/S計算得到Rx、Ry、Rz方向上的加速度;
上式中ADC為AxADC、AyADC、AzADC;
Vref為傳感器的參考電壓;
1023為2^傳感器的分辨率位數(shù)-1計算得到,如當(dāng)前傳感器分辨率為10,固有2^10-1=1023;
Vzg為傳感器的零加速度的電壓值;
S為傳感器的靈敏度;
(2e)由三維空間勾股定理通過Rx、Ry、Rz可以計算得到X、Y和Z軸的角度∠X1、∠Y1、∠Z1;
(2f)和步驟(2d)中的公式類似可以計算出陀螺儀傳感器在X、Y和Z軸的角度∠X2、∠Y2、∠Z2;
(2g)計算比較∠X1/∠X2、∠Y1/∠Y2、∠Z1/∠Z2的值,如果3個值都在1以內(nèi)繼續(xù),否則重復(fù)(2a)~(2g);
(2h)由位移=VoT+1/2*A*T^2令Vo為0,T=T1n-T1,將Rx、Ry、Rz代入,計算各軸的位移:Sx、Sy、Sz;
(2i)采集磁力計的當(dāng)前值,計算校正各軸的角度得到角度∠x、∠y、∠z。
北斗/GPS協(xié)議解析模塊負責(zé)解析北斗/GPS通信協(xié)議,獲取當(dāng)前定位載體的經(jīng)緯度和高度信息,其實現(xiàn)過程如圖4,即實現(xiàn)步驟(3),該步驟(3)中,包括如下細分步驟:
(3a)建立環(huán)形數(shù)據(jù)緩沖區(qū);
(3b)將輸入串口的數(shù)據(jù)存入環(huán)形數(shù)據(jù)緩沖區(qū),同時移動保存數(shù)據(jù)位置指針,當(dāng)保存數(shù)據(jù)位置指針移到環(huán)形數(shù)據(jù)緩沖區(qū)末尾時,令保存數(shù)據(jù)位置指針指向環(huán)形數(shù)據(jù)緩沖區(qū)頭;
(3c)從環(huán)形數(shù)據(jù)緩沖區(qū)首位置讀取數(shù)據(jù),解析數(shù)據(jù),同時移動讀取數(shù)據(jù)位置指針,當(dāng)讀取數(shù)據(jù)位置指針移到環(huán)形數(shù)據(jù)緩沖區(qū)末尾時,令讀取數(shù)據(jù)位置指針指向環(huán)形數(shù)據(jù)緩沖區(qū)頭;
(3d)將解析出來的經(jīng)緯度和高度信息保存到X、Y、Z。
計算校正輸出模塊將傳感器采集計算模塊輸出的位移信息:Sx、Sy、Sz,角度信息:∠x、∠y、∠z,加速度信息:Rx、Ry、Rz和北斗/GPS協(xié)議解析模塊輸出的經(jīng)緯度和高度信息:X、Y、Z,進行互為校正,最終計算輸出定位載體的經(jīng)緯度和高度信息,其實現(xiàn)過程如圖5所示,即實現(xiàn)步驟(4),該步驟(4)中,包括如下細分步驟:
(4a)讀取位移信息:Sx、Sy、Sz,角度信息:∠x、∠y、∠z,加速度信息:Rx、Ry、Rz,經(jīng)緯度和高度信息:X、Y、Z分別存入緩沖區(qū):FSx、FSy、FSz、F∠x、F∠y、F∠z、FRx、FRy、FRz、Fx、Fy、Fz;
(4b)讀取位移信息:Sx、Sy、Sz,角度信息:∠x、∠y、∠z,加速度信息:Rx、Ry、Rz,經(jīng)緯度和高度信息:X、Y、Z;
(4c)將Sx、Sy、∠x、∠y、Fx、Fy代入下式
Xn=Fx+Sx*cos(∠x);
Yn=Fy+Sy*sin(∠y);
(4d)根據(jù)Xn、Yn和高斯算法計算出新點的經(jīng)緯度和高度:X2,Y2,Z2;
(4e)根據(jù)Sx、Sy、Sz、∠x、∠y、∠z、Rx、Ry、Rz、X、Y、Z與FSx、FSy、FSz、F∠x、F∠y、F∠z、FRx、FRy、FRz、Fx、Fy、Fz計算判斷X、Y、Z的合法性;若合法根據(jù)X2、Y2、Z2、X、Y和Z計算后,按照輸出串口協(xié)議和波特率輸出定位載體的經(jīng)緯度和高度信息;若不合法直接將X2、Y2和Z2按照輸出串口協(xié)議和波特率輸出定位載體的經(jīng)緯度和高度信息。
綜上所述,本發(fā)明的關(guān)鍵點是利用陀螺儀、加速度計和磁力計計算當(dāng)前定位載體的方向,角速度和加速度信息,與北斗/GPS解析出來的經(jīng)緯度和高度信息進行互為校正最終輸出定位載體的經(jīng)緯度和高度信息。
本發(fā)明應(yīng)用于高精度導(dǎo)航定位的場合,體現(xiàn)了如下優(yōu)點:
1)導(dǎo)航定位精度高,經(jīng)實際定位測試誤差小于1米;
2)本發(fā)明裝置體積較小,便于安裝,直接串聯(lián)在串口線上即可;
3)支持多種衛(wèi)星導(dǎo)航定位協(xié)議,如:NAEA-0183、NAEA-0180和NAEA-0182等;
4)通過設(shè)置輸入串口波特率和協(xié)議撥碼開關(guān)可以支持不同廠家的衛(wèi)星導(dǎo)航定位設(shè)備;
5)通過設(shè)置輸出串口波特率和協(xié)議撥碼開關(guān),可以和現(xiàn)有系統(tǒng)實現(xiàn)無縫對接,不需任何軟件修改。