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

薛森杰，武利生，張金柱

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

1 引言

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

圖1 流速儀運(yùn)行簡(jiǎn)圖Fig.1 Operation Diagram of Current Meter

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

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

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

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

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

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

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

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

2.2 GPS定位模塊

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

2.3 慣性測(cè)量模塊

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

2.4 Zigbee傳輸模塊

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

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

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

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

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

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

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

在組合導(dǎo)航中GPS為濾波提供量測(cè)信息，但GPS系統(tǒng)受環(huán)境等干擾因素，GPS 模塊接收到的數(shù)據(jù)常常會(huì)嚴(yán)重偏離目標(biāo)真值，稱其為“野值”，如果不予以剔除這些野值，將直接影響濾波的精度和穩(wěn)定性。目前在目標(biāo)實(shí)時(shí)跟蹤與剔除野值數(shù)據(jù)等方面，已經(jīng)有很多方法：速度過濾算法、簡(jiǎn)單經(jīng)緯度法等，但其存在缺陷，當(dāng)收到的第一個(gè)測(cè)量值是野值，給后邊的數(shù)據(jù)判斷提供了錯(cuò)誤導(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)向量輸出到濾波器中進(jìn)行組合。組合導(dǎo)航方式分為直接濾波法和間接濾波法，間接濾波方式又分為輸出校正和反饋校正方式，原理圖，如圖4所示。

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

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

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

狀態(tài)方程為：

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

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

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

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)分析

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ù)隨機(jī)誤差較大，漂移嚴(yán)重，經(jīng)抗野值卡爾曼濾波過濾剔除后，可以有效提高數(shù)據(jù)質(zhì)量，提高流速以定位精度。

4.2 定位信息融合

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

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

圖7 EKF、AKF位置誤差對(duì)比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)噪聲和量測(cè)噪聲不準(zhǔn)確時(shí)，使用擴(kuò)展卡爾曼濾波會(huì)產(chǎn)生誤差，導(dǎo)致新的量測(cè)值對(duì)估計(jì)值的修正作用下降，采用自適應(yīng)卡爾曼濾波可以實(shí)時(shí)調(diào)整系統(tǒng)的一步預(yù)測(cè)方差矩陣，較好的抑制濾波結(jié)果的誤差，提高濾波精度。

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

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

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

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

4.4 定位性能分析

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

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

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

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

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

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

5 結(jié)束語(yǔ)

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