基于STM32的雷達波流速儀輔助定位系統(tǒng)

薛森杰，武利生，張金柱

（太原理工大學(xué)機械與運載工程學(xué)院，山西 太原 030024）

1 引言

隨著社會的不斷發(fā)展，我國的水文工作也進展到一個新得層面，水文測流方式也日新月異。文獻[1]將GPS 定位應(yīng)用到ADCP流儀速中，通過GPS定位獲取ADCP流速儀位置，計算儀器的位移來計算流速、流量。文獻[2]將GPS應(yīng)用在多功能測流航標(biāo)中，通過GPS定位獲取航標(biāo)的位置以及計算水的流速。因此將GPS 定位系統(tǒng)應(yīng)用到測流技術(shù)中已經(jīng)成為水文測流必然趨勢。目前，第三代雷達波流速儀已實現(xiàn)遠程測流，其運行簡圖，如圖1所示。

圖1 流速儀運行簡圖Fig.1 Operation Diagram of Current Meter

在測流過程中存在以下問題：（1）在采用遠程客戶端進行測流操作時，軟件界面顯示儀器與測流起始點的距離（簡稱為：起點距），起點距值通過步進電機脈沖解算得到，而不是儀器所在地理位置的差值。在實際測流過程中，當(dāng)發(fā)生故障（流速儀碰到障礙物），起點距值根據(jù)步進電機的脈沖解算一直在變，而儀器的實際位置不發(fā)生變化；（2）當(dāng)測站現(xiàn)場發(fā)生斷電情況，測站軟件自動重啟，起點距顯示為設(shè)置的默認值，而不是儀器所在的空間位置。由此可知，需要將GPS定位技術(shù)應(yīng)用在流速儀中，獲取流速儀的地理坐標(biāo)位置[3]。

由于單一的GPS 定位系統(tǒng)存在信號易受遮擋和干擾的缺點，提出使用GPS∕INS組合定位的方法獲取流速儀的位置信息。GPS∕INS松耦合結(jié)合兩者的優(yōu)點，使組合后的導(dǎo)航精度優(yōu)于各系統(tǒng)單獨工作的精度[4-5]。卡爾曼濾波在GPS∕INS組合導(dǎo)航系統(tǒng)中被廣泛應(yīng)用，但是卡爾曼濾波需要得知準(zhǔn)確的系統(tǒng)模型以及系統(tǒng)噪聲、量測噪聲，在實際應(yīng)用中，無法獲取精確的系統(tǒng)模型和觀測噪聲，使用卡爾曼濾波將影響濾波的精度，甚至導(dǎo)致濾波結(jié)果發(fā)散[6]。

擴展卡爾曼濾波算法對非線性函數(shù)進行線性化近似，對高階項采用忽略或逼近的方式，但當(dāng)系統(tǒng)非線性度較強時，Taylor展開式的高階項無法忽略，容易造成濾波結(jié)果不準(zhǔn)確[7]。針對這一問題可使用自適應(yīng)卡爾曼濾波，自適應(yīng)卡爾曼濾波根據(jù)信息估計系統(tǒng)噪聲和量測噪聲，從而對一步預(yù)測均方差作出調(diào)整，有效抑制濾波發(fā)散[8]。

2 系統(tǒng)架構(gòu)

根據(jù)系統(tǒng)的設(shè)計目標(biāo)和需求分析，流速儀定位系統(tǒng)主要由電源模塊、主控單元、GPS定位模塊，慣性導(dǎo)航模塊，數(shù)據(jù)收發(fā)模塊以及上位機組成。硬件設(shè)計圖，如圖2所示。

圖2 系統(tǒng)硬件結(jié)構(gòu)框圖Fig.2 System Hardware Structure Block Diagram

2.1 系統(tǒng)控制單元

硬件系統(tǒng)的主控單元主要用來對數(shù)據(jù)進行采集以及數(shù)據(jù)的傳輸，在此選擇STM32F103系列控制器。STM32F103系列單片機具有多種接口，通過USART采集GPS信息并與ZigBee模塊進行交互，將GPS 信息上傳至上位機。F103 系列單片機頻率為72MHz，可滿足系統(tǒng)對GPS位置的采集和發(fā)送需求。

2.2 GPS定位模塊

GPS定位模塊選山竹定位模塊RTK-MOUSEIII（山竹）。模塊內(nèi)嵌GNSS接收機，自帶AGNSS輔助定位，具有接受靈敏度高、定位時間短等特點。該產(chǎn)品通過使用RTK定位技術(shù)可以使其定位精度達到亞米級，可以滿足流速儀定位需求。

2.3 慣性測量模塊

慣性測量模塊選用JY901 高精度慣性模塊，集成了3 軸加速度計、3軸陀螺儀和3軸磁力計。該模塊通過卡爾曼濾波算法將9 軸數(shù)據(jù)融合獲得更加精確的姿態(tài)信息，其姿態(tài)測量精度可達0.01°。

2.4 Zigbee傳輸模塊

雷達波流速儀工作范圍在300m以內(nèi)，且對數(shù)據(jù)傳輸速率要求較低，綜合考慮成本、功耗以及數(shù)據(jù)傳輸可靠性等因素，選擇ZigBee無線模塊，本次設(shè)計選擇的模塊工作電壓為5V，在空曠地帶傳輸距離可達2000m，完全滿足系統(tǒng)使用要求［9］。

4.5 出院指導(dǎo) 在患者出院前，仔細講解出院后隨訪的重要性，并建立隨訪檔案，積極監(jiān)督、督促患者定期完成隨訪，放療結(jié)束后1個月復(fù)查1次，以后3個月1次，1年后每半年復(fù)查1次。口腔要保持清潔，3年內(nèi)不能拔牙，避免誘發(fā)頜骨骨髓炎，堅持張口鍛煉，防止咀嚼肌及周圍組織纖維化，以防止張口困難。

3 組合導(dǎo)航系統(tǒng)研究

3.1 數(shù)據(jù)獲取

山竹模塊默認輸出NMEA0183格式的數(shù)據(jù)信息，將山竹模塊通過串口配置定位時間間隔為500ms，JY901 輸出頻率為10Hz。數(shù)據(jù)接收流程圖，如圖3所示。

圖3 數(shù)據(jù)采集流程圖Fig.3 Data Acquisition Flow Chart

3.2 數(shù)據(jù)預(yù)處理

在組合導(dǎo)航中GPS為濾波提供量測信息，但GPS系統(tǒng)受環(huán)境等干擾因素，GPS 模塊接收到的數(shù)據(jù)常常會嚴(yán)重偏離目標(biāo)真值，稱其為“野值”，如果不予以剔除這些野值，將直接影響濾波的精度和穩(wěn)定性。目前在目標(biāo)實時跟蹤與剔除野值數(shù)據(jù)等方面，已經(jīng)有很多方法：速度過濾算法、簡單經(jīng)緯度法等，但其存在缺陷，當(dāng)收到的第一個測量值是野值，給后邊的數(shù)據(jù)判斷提供了錯誤導(dǎo)向，導(dǎo)致正確的值被當(dāng)做野值而過濾掉［10］。

3.3 組合導(dǎo)航結(jié)構(gòu)

根據(jù)設(shè)備成本以及系統(tǒng)需求，這里選取松耦合結(jié)構(gòu)。將慣導(dǎo)系統(tǒng)以及GPS的誤差參數(shù)作為狀態(tài)向量輸出到濾波器中進行組合。組合導(dǎo)航方式分為直接濾波法和間接濾波法，間接濾波方式又分為輸出校正和反饋校正方式，原理圖，如圖4所示。

圖4 間接濾波法原理圖Fig.4 Schematic Diagram of Indirect Filtering Method

采用混合校正的方式提高其定位精度，如圖4所示。混合校正綜合輸出校正和反饋校正兩種方式的優(yōu)點，在不增加計算量的情況下，提高了GPS∕INS的組合精度［12］。

3.4 組合導(dǎo)航模型

狀態(tài)方程為：

選取姿態(tài)角、速度誤差、位置誤差以及陀螺儀和加速度計零偏作為狀態(tài)變量：

式中：vGE，vGN，vGU—GPS沿東北天方向的速度量測誤差；

NGE，NGN，NGU—GPS沿東北天方向上的位置誤差。

3.5 自適應(yīng)漸消卡爾曼濾波算法

常規(guī)卡爾曼濾波的遞推過程如下所示：

自適應(yīng)漸消卡爾曼濾波就是在計算一步預(yù)測均方誤差Pk,k-1時，加入遺忘因子λ(k)：

基于殘差定義的最佳漸消因子λ(k) 如下：

式中：Mk—理論估計的新息協(xié)方差矩陣；Nk—實際計算所得的新息協(xié)方差矩陣[13]。漸消因子λk的作用是減少觀測值對遞推過程中噪聲特性不準(zhǔn)確對濾波結(jié)果造成的影響，當(dāng)殘差εk誤差增大時，漸消因子λk增大，起到抑制噪聲影響、抑制濾波發(fā)散的作用。

3.6 組合導(dǎo)航算法

流速儀沿著纜道進行測流工作，將纜道的兩端確定為A點和B點，則AB為導(dǎo)航線。由起始點A和終點B確定一條導(dǎo)航直線，導(dǎo)航AB直線方程為：

假設(shè)流速儀GPS接收到位置信息為C點，將其轉(zhuǎn)換為坐標(biāo)(x3，y3)，則C點到導(dǎo)航AB線的直線距離為d：

4 實驗分析

4.1 野值過濾分析

將GPS靜置采集數(shù)據(jù)一分鐘，由于GPS受環(huán)境干擾嚴(yán)重，出現(xiàn)野值，經(jīng)抗野值卡爾曼過濾之后，如圖5所示。

圖5 抗野值卡爾曼濾波后緯度值Fig.5 Latitude Value After Outliers Kalman Filtering

分析圖5可得，GPS采集到的原始數(shù)據(jù)隨機誤差較大，漂移嚴(yán)重，經(jīng)抗野值卡爾曼濾波過濾剔除后，可以有效提高數(shù)據(jù)質(zhì)量，提高流速以定位精度。

4.2 定位信息融合

利用真實軌跡仿真兩種算法，仿真初值為：初值位置37.86°，112.515587°，初始速度為0，GPS 采樣頻率為1Hz，INS 采樣頻率為10Hz。仿真結(jié)果，如圖6、圖7所示。

圖6 EKF、AKF速度誤差對比Fig.6 EKF and AKF Speed Error Comparison

圖7 EKF、AKF位置誤差對比Fig.7 EKF And AKF Position Error Comparison

EKF 和AKF 算法的均值（mean）和均方根（RMS），如表1 所示。可以得出AKF算法在東向速度、北向速度、精度、緯度誤差較EKF 算法上均值提高了71%，92%，13%，55%，均方根分別降低了58%，86%，46%，36%。

表1 算法性能比較Tab.1 Comparison of Algorithm Performance

通過上述仿真結(jié)果可得，當(dāng)系統(tǒng)噪聲和量測噪聲不準(zhǔn)確時，使用擴展卡爾曼濾波會產(chǎn)生誤差，導(dǎo)致新的量測值對估計值的修正作用下降，采用自適應(yīng)卡爾曼濾波可以實時調(diào)整系統(tǒng)的一步預(yù)測方差矩陣，較好的抑制濾波結(jié)果的誤差，提高濾波精度。

4.3 GPS靜態(tài)精度測試

將GPS靜置放在間距為0.6m的點上進行靜態(tài)位置采集，監(jiān)測$GNRMC的定位狀態(tài)，當(dāng)定位狀態(tài)為R時，表示當(dāng)前定位模式為固定解，定位精度為分米級，此時進行1min數(shù)據(jù)采集，采集數(shù)據(jù)結(jié)果，如表2所示。

表2 靜態(tài)GPS位置計算Tab.2 Static GPS Position Calculation

分析表2可得，GPS靜態(tài)采集定位誤差最大為0.19m，可以用來測纜道兩端的經(jīng)緯度，為組合導(dǎo)航系統(tǒng)提供初始值。

4.4 定位性能分析

流速儀一般用在小河道測流，河道寬度較小。現(xiàn)以某地進行實地測試，設(shè)纜道的兩端為A，B，將GPS放置在A，B兩端進行靜態(tài)采集，數(shù)據(jù)結(jié)果，如表3所示。

表3 A、B 兩點經(jīng)緯度及距離Tab.3 Longitude，Latitude and Distance at Points A and B

以A、B兩點的連線作為真實軌跡，將GPS模塊輸出的位置信息和經(jīng)組合導(dǎo)航濾波后輸出的經(jīng)緯度轉(zhuǎn)換到平面坐標(biāo)系中（方便計算距離），其結(jié)果，如圖8、圖9所示。

圖8 真實軌跡、GPS軌跡、濾波后軌跡Fig.8 Real Trajectory，GPS Trajectory，F(xiàn)iltered Trajectory

圖9 濾波前后位置點到直線AB的距離Fig.9 Distance Between Position Points and Line AB Before and After Filtering

分析圖8、圖9，經(jīng)過GPS∕INS組合導(dǎo)航后，系統(tǒng)輸出的位置更加貼合真實軌跡，能更好的跟蹤流速儀的運行軌跡。

5 結(jié)束語

針對獲取流速儀地理空間坐標(biāo)的問題，提出一種基于STM32的位置采集系統(tǒng)，相較于依靠步進電機脈沖解算得到流速儀的位置的方式，該輔助定位系統(tǒng)輸出GPS∕INS組合后的經(jīng)緯度信息，即獲取到流速儀在空間中的地理坐標(biāo)位置。實驗結(jié)果表明組合導(dǎo)航相較于單系統(tǒng)來說，輸出位置更加可靠準(zhǔn)確，可以將該系統(tǒng)用于流速儀測流中，保障測流結(jié)果的準(zhǔn)確性以及延長測流設(shè)備的使用周期，具有實際使用意義。