本發(fā)明涉及GPS航向平滑領(lǐng)域，特別涉及一種船用GPS航向平滑更新方法。

背景技術(shù)：

在裝有導(dǎo)航儀的船舶中，導(dǎo)航儀接入GPS設(shè)備。當(dāng)船舶航速較小時(shí)，導(dǎo)航儀收到的GPS航向數(shù)據(jù)不準(zhǔn)確，會(huì)出現(xiàn)相鄰點(diǎn)跳動(dòng)幅度較大的情況。GPS航向數(shù)據(jù)的跳動(dòng)，導(dǎo)致導(dǎo)航儀和其他船只會(huì)遇告警，且此告警為航向數(shù)據(jù)不準(zhǔn)確造成的虛警。

傳統(tǒng)的航向平滑算法有曲線擬合和滑動(dòng)平均算法，這兩種算法在相鄰點(diǎn)跳動(dòng)不大的情況下，均能取得較好的效果，但是在相鄰航向點(diǎn)跳動(dòng)較大時(shí)，效果很不理想，即航速較小時(shí)，相鄰航向點(diǎn)跳動(dòng)較大，效果很不理想。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明提供一種船用GPS航向平滑更新方法，解決在小航速情況下，航向數(shù)據(jù)不準(zhǔn)確的問題。

一種船用GPS航向平滑更新方法，包括如下步驟：

S1：獲取GPS第i幀數(shù)據(jù)，從GPS第i幀數(shù)據(jù)中解析出經(jīng)緯度i、航向信息i；

S2：若i為1時(shí)，將經(jīng)緯度i、航向信息i賦予給導(dǎo)航設(shè)備，導(dǎo)航設(shè)備的位置信息為經(jīng)緯度i、航向newCourse為航向信息i，經(jīng)緯度i、航向信息i作為導(dǎo)航信息后，結(jié)束更新；

S3：若i為大于1的正整數(shù)時(shí)，獲取GPS第i-1幀數(shù)據(jù)，從GPS第i-1幀數(shù)據(jù)中解析出經(jīng)緯度i-1、航向信息i-1，計(jì)算經(jīng)緯度i-1與經(jīng)緯度i的距離Dist、方位Azimuth；

S4：若距離Dist大于距離門限D(zhuǎn)istTH時(shí)，將經(jīng)緯度i、方位Azimuth賦予給導(dǎo)航設(shè)備，導(dǎo)航設(shè)備的位置信息更新為經(jīng)緯度i、航向newCourse更新為方位Azimuth，完成航向平滑更新；

S5：若距離Dist小于或等于距離門限D(zhuǎn)istTH時(shí)，則導(dǎo)航設(shè)備的位置信息為經(jīng)緯度i、航向newCourse為航向信息i，然后計(jì)數(shù)器加1，獲取下一幀GPS數(shù)據(jù)，直到獲取GPS第i+N幀數(shù)據(jù)時(shí)，距離Dist大于距離門限距離DistTH后再執(zhí)行S4。

上述方案的技術(shù)原理為：傳統(tǒng)的航向平滑更新方法有曲線擬合和滑動(dòng)平均算法，在高速航行情況下，這兩種算法的航向相鄰點(diǎn)跳動(dòng)不大，均能取得較好的效果，但是在低航速情況下，相鄰航向點(diǎn)跳動(dòng)較大，效果很不理想。本發(fā)明基于兩幀的GPS數(shù)據(jù)中的經(jīng)緯度來得出距離Dist、方位Azimuth，然后采用距離Dist比較距離門限D(zhuǎn)istTH的方式確定是否要更新前一幀狀態(tài)下的經(jīng)緯度和航向，在距離Dist＞門限D(zhuǎn)istTH的情況下，更新當(dāng)前經(jīng)緯度替換原經(jīng)緯度，更新方位Azimuth替換原方位Azimuth，由于利用兩幀的GPS數(shù)據(jù)計(jì)算獲得方位Azimuth，可以獲得更準(zhǔn)確的航向，因此，采用上述方法可以使得航向平滑更新，提高航向數(shù)據(jù)準(zhǔn)確度，例如，前一幀狀態(tài)下，船只的經(jīng)緯度為經(jīng)緯度1、航向?yàn)楹较?，在下一幀GPS數(shù)據(jù)到達(dá)時(shí)，獲得該GPS數(shù)據(jù)的經(jīng)緯度2，航向2，利用經(jīng)緯度2和經(jīng)緯度1計(jì)算得到距離Dist、方位Azimuth，當(dāng)距離Dist＞門限D(zhuǎn)istTH時(shí)，我們認(rèn)為船只行駛移動(dòng)了足夠距離，船只位置發(fā)生了變化，可以進(jìn)行上述更新，而依靠2個(gè)經(jīng)緯度計(jì)算獲得方位更加可靠；如距離Dist≤門限D(zhuǎn)istTH時(shí)，則視船只位置沒變，可以保留原經(jīng)緯度和航向，導(dǎo)航信息不更新，這樣就保證了只有在船只位置發(fā)生了變化時(shí)才進(jìn)行導(dǎo)航信息更新。

優(yōu)選的，所述距離門限D(zhuǎn)istTH為：5米至30米。

優(yōu)選的，S1步驟前還需執(zhí)行計(jì)算航速步驟，當(dāng)航速小于或等于3米/秒時(shí)，再執(zhí)行S1、S2、 S3、 S4、 S5。

本發(fā)明的優(yōu)點(diǎn)有：傳統(tǒng)的航向平滑算法中的曲線擬合和滑動(dòng)平均算法，在相鄰點(diǎn)跳動(dòng)較大的情況下，效果很不理想。本文提出了一種GPS航向平滑算法，該算法在相鄰點(diǎn)跳動(dòng)較大情況下，實(shí)現(xiàn)較好的平滑效果，且該算法計(jì)算量小，易于工程實(shí)現(xiàn)。

附圖說明

圖1本發(fā)明實(shí)施例2的數(shù)據(jù)匹配圖。

圖2本發(fā)明實(shí)施例3的數(shù)據(jù)匹配圖。

具體實(shí)施方式

下面結(jié)合實(shí)施例及附圖，對本發(fā)明作進(jìn)一步地的詳細(xì)說明，但本發(fā)明的實(shí)施方式不限于此。

實(shí)施例1：

如圖1和圖2所示。

一種船用GPS航向平滑更新方法，包括如下步驟：

S1：獲取GPS第i幀數(shù)據(jù)，從GPS第i幀數(shù)據(jù)中解析出經(jīng)緯度i、航向信息i；

S2：若i為1時(shí)，將經(jīng)緯度i、航向信息i賦予給導(dǎo)航設(shè)備，導(dǎo)航設(shè)備的位置信息為經(jīng)緯度i、航向newCourse為航向信息i，經(jīng)緯度i、航向信息i作為導(dǎo)航信息后，結(jié)束更新；

S3：若i為大于1的正整數(shù)時(shí)，獲取GPS第i-1幀數(shù)據(jù)，從GPS第i-1幀數(shù)據(jù)中解析出經(jīng)緯度i-1、航向信息i-1，計(jì)算經(jīng)緯度i-1與經(jīng)緯度i的距離Dist、方位Azimuth；

S4：若距離Dist大于距離門限D(zhuǎn)istTH時(shí)，將經(jīng)緯度i、方位Azimuth賦予給導(dǎo)航設(shè)備，導(dǎo)航設(shè)備的位置信息更新為經(jīng)緯度i、航向newCourse更新為方位Azimuth，完成航向平滑更新；

S5：若距離Dist小于或等于距離門限D(zhuǎn)istTH時(shí)，則導(dǎo)航設(shè)備的位置信息為經(jīng)緯度i、航向newCourse為航向信息i，然后計(jì)數(shù)器加1，獲取下一幀GPS數(shù)據(jù)，直到獲取GPS第i+N幀數(shù)據(jù)時(shí)，距離Dist大于距離門限距離DistTH后再執(zhí)行S4。

上述方案的技術(shù)原理為：傳統(tǒng)的航向平滑更新方法有曲線擬合和滑動(dòng)平均算法，在高速航行情況下，這兩種算法的航向相鄰點(diǎn)跳動(dòng)不大，均能取得較好的效果，但是在低航速情況下，相鄰航向點(diǎn)跳動(dòng)較大，效果很不理想。本發(fā)明基于兩幀的GPS數(shù)據(jù)中的經(jīng)緯度來得出距離Dist、方位Azimuth，然后采用距離Dist比較距離門限D(zhuǎn)istTH的方式確定是否要更新前一幀狀態(tài)下的經(jīng)緯度和航向，在距離Dist＞門限D(zhuǎn)istTH的情況下，更新當(dāng)前經(jīng)緯度替換原經(jīng)緯度，更新方位Azimuth替換原方位Azimuth，由于利用兩幀的GPS數(shù)據(jù)計(jì)算獲得方位Azimuth，可以獲得最準(zhǔn)確的航向，因此，采用上述方法可以使得航向平滑更新，提高航向數(shù)據(jù)準(zhǔn)確度，例如，前一幀狀態(tài)下，船只的經(jīng)緯度為經(jīng)緯度1、航向?yàn)楹较?，在下一幀GPS數(shù)據(jù)到達(dá)時(shí)，獲得該GPS數(shù)據(jù)的經(jīng)緯度2，航向2，利用經(jīng)緯度2和經(jīng)緯度1計(jì)算得到距離Dist、方位Azimuth，當(dāng)距離Dist＞門限D(zhuǎn)istTH時(shí)，我們認(rèn)為船只行駛移動(dòng)了足夠距離，船只位置發(fā)生了變化，可以進(jìn)行上述更新，而依靠2個(gè)經(jīng)緯度計(jì)算獲得方位更加可靠；如距離Dist≤門限D(zhuǎn)istTH時(shí)，則視船只位置沒變，可以保留原經(jīng)緯度和航向，導(dǎo)航信息不更新，這樣就保證了只有在船只位置發(fā)生了變化時(shí)才進(jìn)行導(dǎo)航信息更新。

優(yōu)選的，所述距離門限D(zhuǎn)istTH為：5米至30米。

優(yōu)選的，S1步驟前還需執(zhí)行計(jì)算航速步驟，當(dāng)航速小于或等于3米/秒時(shí)，再執(zhí)行S1、S2、 S3、 S4、 S5。

實(shí)施例2

具體的如圖1所示：

第1幀狀態(tài)下，接收到GPS第1幀數(shù)據(jù)，包括經(jīng)緯度1、航向信息1，此時(shí)，將經(jīng)緯度1、航向信息1賦值給在第1幀狀態(tài)下的導(dǎo)航設(shè)備，第1幀狀態(tài)下的導(dǎo)航設(shè)備的導(dǎo)航信息包括經(jīng)緯度1、航向信息1。

在第2幀狀態(tài)下，接收到GPS第2幀數(shù)據(jù)，包括經(jīng)緯度2、航向信息2，此時(shí)，通過經(jīng)緯度1（前一經(jīng)緯度）與經(jīng)緯度2（當(dāng)前經(jīng)緯度）計(jì)算得到距離Dist、方位Azimuth，此時(shí)的距離Dist大于距離門限D(zhuǎn)istTH，則將經(jīng)緯度2、方位Azimuth賦值給在第2幀狀態(tài)下的導(dǎo)航設(shè)備，第2幀狀態(tài)下的導(dǎo)航設(shè)備的導(dǎo)航信息包括經(jīng)緯度2、方位Azimuth，同理，之后的GPS幀數(shù)據(jù)都按此狀態(tài)執(zhí)行，每一幀狀態(tài)下的距離Dist都滿足大于距離門限D(zhuǎn)istTH的要求，因此，不斷更新當(dāng)前幀狀態(tài)下所獲得的經(jīng)緯度和方位Azimuth。

實(shí)施例3

如圖2所示：

第1幀狀態(tài)下，接收到GPS第1幀數(shù)據(jù)，包括經(jīng)緯度1、航向信息1，此時(shí)，將經(jīng)緯度1、航向信息1賦值給在第1幀狀態(tài)下的導(dǎo)航設(shè)備，第1幀狀態(tài)下的導(dǎo)航設(shè)備的導(dǎo)航信息包括經(jīng)緯度1、航向信息1。

在第2幀狀態(tài)下，接收到GPS第2幀數(shù)據(jù)，包括經(jīng)緯度2、航向信息2，此時(shí)，通過經(jīng)緯度1（前一經(jīng)緯度）與經(jīng)緯度2（當(dāng)前經(jīng)緯度）計(jì)算得到距離Dist、方位Azimuth，此時(shí)的距離Dist小于或等于距離門限D(zhuǎn)istTH，此時(shí)導(dǎo)航設(shè)備不更新；一直獲得下一幀，直到第1+N幀狀態(tài)下，接收到GPS第1+N幀數(shù)據(jù)，包括經(jīng)緯度1+N、航向信息1+N，此時(shí)，通過經(jīng)緯度1（前一經(jīng)緯度）與經(jīng)緯度1+N（當(dāng)前經(jīng)緯度）計(jì)算得到距離Dist、方位Azimuth，此時(shí)的距離Dist大于距離門限D(zhuǎn)istTH，則將經(jīng)緯度1+N、方位Azimuth賦值給在第1+N幀狀態(tài)下的導(dǎo)航設(shè)備，第1+N幀狀態(tài)下的導(dǎo)航設(shè)備的導(dǎo)航信息包括經(jīng)緯度1+N、方位Azimuth，此時(shí)的導(dǎo)航設(shè)備才完成一次更新。

實(shí)施例3

本發(fā)明的具體實(shí)施步驟如下：

第1步：當(dāng)收到第一幀數(shù)據(jù)，判斷計(jì)數(shù)器count是否等于1。若count等于1，將其經(jīng)緯度分別存儲(chǔ)為beginLon1、beginLat1，將GPS航向賦值給平滑后的航向newCourse，計(jì)數(shù)器count加1；

第2步：當(dāng)收到第二幀數(shù)據(jù)，判斷計(jì)數(shù)器count是否等于2，若count等于2，計(jì)算該幀數(shù)據(jù)中的經(jīng)緯度與beginLon1 ，beginLat1的距離Dist、方位Azimuth，若距離Dist大于距離門限D(zhuǎn)istTH，則將該幀數(shù)據(jù)中的經(jīng)緯度分別賦值給beginLon2，beginLat2，將Azimuth賦值給newCourse，完成更新，計(jì)數(shù)器count加1。若Dist小于等于距離門限D(zhuǎn)istTH，直接退出程序；

第3步：若第2步中Dist小于等于DistTH，繼續(xù)執(zhí)行第2步，直到收到第i幀數(shù)據(jù)時(shí)，Dist大于DistTH為止。

第4步：當(dāng)收到第i+1幀數(shù)據(jù)，首先將GPS經(jīng)緯度數(shù)據(jù)分別賦值給EndLon、EndLat，將BeginLat2賦值給BeginLat、BeginLon2賦值給BeginLon，根據(jù)起始點(diǎn)經(jīng)緯度BeginLon、BeginLat和結(jié)束點(diǎn)經(jīng)緯度EndLon、EndLat，計(jì)算兩點(diǎn)的距離Dist和方位Azimuth。

第5步：判斷Dist是否大于DistTH。

第6步：若第5步成立，則將BeginLat2賦值給BeginLat1、將BeginLon2賦值給BeginLon1，將本幀數(shù)據(jù)中的GPS經(jīng)緯度分別賦值給BeginLon2、BeginLat2,并且將Azimuth賦值給newCourse。

第7步：若第5步不成立，則將BeginLat1賦值給BeginLon、BeginLon1賦值給BeginLon，將本幀數(shù)據(jù)中的GPS經(jīng)緯度分別賦值給EndLon、EndLat, 根據(jù)起始點(diǎn)經(jīng)緯度BeginLon、BeginLat和結(jié)束點(diǎn)經(jīng)緯度EndLon、EndLat，計(jì)算兩點(diǎn)的距離Dist和方位Azimuth,并且將計(jì)算出的方位Azimuth賦值給newCourse。

第8步：當(dāng)收到第N(N >=i+2)幀數(shù)據(jù)，重復(fù)第4到第7步即可。

以上所述，僅是本發(fā)明的較佳實(shí)施例，并非對本發(fā)明做任何形式上的限制，凡是依據(jù)本發(fā)明的技術(shù)實(shí)質(zhì)對以上實(shí)施例所作的任何簡單修改、等同變化，均落入本發(fā)明的保護(hù)范圍之內(nèi)。