對基于衛(wèi)星定位及傳感器的互補運動狀態(tài)感知系統(tǒng)的研究與設(shè)計

代龍

（北方民族大學電子信息工程專業(yè)，銀川 750000）

0 引言

傳統(tǒng)的對物體運動狀態(tài)確定的方法依賴單一的數(shù)據(jù)，而在復雜情況下，如在可接受衛(wèi)星數(shù)較少或外界氣象條件不允許時，無法獲得準確的衛(wèi)星數(shù)據(jù)或氣壓數(shù)據(jù)，同時由于衛(wèi)星定位原理所致我們只能二維平面速率，當我們在三維空間運動時只能通過數(shù)學推算出速率。

因此，針對這些弊端，筆者設(shè)計出了一套由氣壓傳感器、溫度傳感器、雙模定位模塊及陀螺儀構(gòu)成的運動狀態(tài)感知系統(tǒng)。氣壓傳感器推算出的高度與定位模塊的得到的高度相互驗證后得到修正后的高度，陀螺儀加速度計與定位模塊的數(shù)據(jù)經(jīng)濾波、相互驗證獲得修正后的速率、方向，實現(xiàn)多數(shù)據(jù)融合，從而獲得相對準確的物體運動狀態(tài)。

1 系統(tǒng)總體設(shè)計方案

以恩智浦MK60 作為MCU，使用BMP180 氣壓溫度一體式傳感器模塊測定溫度及氣壓，MPU6050 采集加速度值和歐拉角。溫度、壓力、加速度及歐拉角均通過I2C 總線傳輸給MCU 進行分析，最后將得到的數(shù)據(jù)通過SPI 接口顯示到LCD 上并存入Flash。同時可以通過藍牙將數(shù)據(jù)實時發(fā)送至PC 控制臺的上位機軟件，以便實現(xiàn)實時監(jiān)控。詳見圖1。

2 系統(tǒng)硬件設(shè)計

2.1 微控制器

選用恩智浦公司推出的的MK60，選擇理由如下：

（1）MK60 是基于 ARM 公司的 Cortex-M4 內(nèi)核設(shè)計的芯片，此內(nèi)核是由ARM 公司專門開發(fā)的32 位處理器，具有強大的模擬數(shù)字計算能力。

（2）具備多種低功耗運行模式，允許外設(shè)在CPU處于休眠的情況下正常運行，從而降低功耗。

（3）各種通信協(xié)議（包括 I2C、SPI、藍牙等）被集成在芯片內(nèi)部。

基于以上這三點，此芯片能夠滿足設(shè)計需求。

2.2 數(shù)據(jù)采集傳感器

（1）溫度及氣壓傳感器采用博世公司的BMP280一體式傳感器，它將溫度傳感器、氣壓傳感器集成在一起，整體僅僅需要一個供電接口，一根信號線（采用I2C通訊），相對于分體式傳感器具有功耗低、維護調(diào)試方便等優(yōu)勢，同時分辨率最高達1.7cm，可以自動補償溫度偏差，完全滿足設(shè)計需求。

圖2

（2）陀螺儀加速度計采用InvenSense 公司推出的MPU6050，此傳感器具有精度高（加速度最高達16384LSB/g，角度最高達 0.1°），功耗低的特點，DMP（數(shù)字運動處理器）集成在里面，融合了各種姿勢，MCU 能夠直接通過I2C 讀取輸出結(jié)果，能夠滿足設(shè)計需求。

圖3

（3）定位采用SkyTra 的s1216 模塊，此模塊能夠進行雙模定位（北斗、GPS），帶有有源天線，具有追蹤靈敏度高（最高達-165dBm）、定位精度高（最高達2.5CEP）、冷啟動時間短（30s 左右）、更新速率快（最快達20KHz）的特點，能夠通過UART 串口與MCU 進行數(shù)據(jù)傳輸，故能夠滿足設(shè)計需求。

圖4

2.3 藍牙通信模塊

藍牙方案采用HC-08 藍牙模塊，該模塊使用藍牙4.0 協(xié)議，一主一從配對通訊，與微控制器間采用串口通訊，有通訊距離遠（最遠達 80m）、靈敏度高（-93dBm@1Mbps）連接穩(wěn)定、功耗低的特點。

圖5

2.4 LCD

采用1.8 寸TFT 液晶顯示屏，采用ST7735S 芯片驅(qū)動能夠以SPI 通信協(xié)議與單片機進行通信。

3 系統(tǒng)軟件設(shè)計

3.1 系統(tǒng)軟件總體設(shè)計流程

圖6

微控制器負責控制傳感器及定位模塊采集數(shù)據(jù)并回傳，其中數(shù)據(jù)采集采用中斷方式進行，如設(shè)定每30ms 進行一次采集，將數(shù)據(jù)輸入到數(shù)據(jù)分析計算系統(tǒng)，進行分析計算，如果可靠，將數(shù)據(jù)寫入Flash 并傳輸?shù)絃CD 及藍牙，否則將數(shù)據(jù)再次分析計算，確定產(chǎn)生誤差的原因，同時再次進行數(shù)據(jù)采集并進行分析計算，得到的數(shù)據(jù)與上一次的數(shù)據(jù)進行驗證、融合、取舍后將最終的數(shù)據(jù)輸出。

3.2 數(shù)據(jù)分析計算系統(tǒng)

數(shù)據(jù)分析計算系統(tǒng)主要先對定位模塊和陀螺儀加速度計獲得的數(shù)據(jù)進行濾波、解算及多數(shù)據(jù)融合計算，其主要流程如圖7。

圖7

（1）濾波器：

①背景：

卡爾曼濾波器是一種狀態(tài)估計算法，其在工業(yè)界得到了廣泛應(yīng)用，可以綜合利用不同傳感器回傳的數(shù)據(jù)，相對精確地推算出一個系統(tǒng)狀態(tài)的估計值。由于物體具有慣性，運動狀態(tài)的各個分量之間都具有相關(guān)性，這就使得卡爾曼濾波可以利用狀態(tài)量和觀測量對定位目標和相鄰時刻的位置狀態(tài)進行最優(yōu)的估計，從而使濾波后的定位結(jié)果更加準確，它有一個小的內(nèi)存優(yōu)勢（除了以前的狀態(tài)量，不需要保持其他歷史數(shù)據(jù)），而且速度非?？?，非常適合實時和嵌入式系統(tǒng)的問題。

②工作原理：

簡而言之，卡爾曼濾波器是能夠分析上一時刻的狀態(tài)，估算當前時刻的狀態(tài)，而后把當前時刻的測量值與估算的狀態(tài)進行分析并加權(quán)，加權(quán)后的結(jié)果作為當前的實際工作狀態(tài)，而不是僅僅取決于當前的測量值。

③簡化后的算法流程：

a 設(shè)定參數(shù)。

b根據(jù)時間t（K-1）的狀態(tài)預(yù)測第t（K）時刻的狀態(tài) X（K|K-1）。

c根據(jù)時間t（K-1）的系統(tǒng)預(yù)測誤差來估計時間t（K）的系統(tǒng)預(yù)測誤差P（K|K-1）。

d 計算卡爾曼增益Kg。

e 計算系統(tǒng)的最加估計值X（K|K）。

f 計算系統(tǒng)目前的的系統(tǒng)預(yù)測誤差P（K|k）。

g 回到步驟b。

④實現(xiàn)代碼：

（2）姿態(tài)解算：

姿態(tài)解算包括了解算歐拉角、加速度及速度。對于速度我們知道加速度對時間的積分為速度，在這個系統(tǒng)中，我們把對加速度的采集放入中斷（此中斷級別應(yīng)為最高級以保證加速度的實時性）并把加速度放入數(shù)組，中斷時間設(shè)為1ms，六個中斷作為一個周期，一個周期后對六個加速度數(shù)據(jù)用逐差法處理后作為平均加速度再對一個周期的時間積分得到速度。

歐拉角、加速度測定的實現(xiàn)代碼可參考《基于嵌入式系統(tǒng)的數(shù)字濾波器、傾角傳感器設(shè)計》這里不再贅述。

速度實現(xiàn)推算代碼：

（3）數(shù)據(jù)融合：

數(shù)據(jù)融合首先對濾波后的陀螺儀加速度計進行融合，獲得相對準確的數(shù)據(jù)后與濾波后的定位模塊的數(shù)據(jù)氣壓計數(shù)據(jù)進行驗證取舍。

①背景：

因為陀螺儀受到偏置不穩(wěn)定性的影響，由于初始零個讀取裝置固有的缺陷和噪聲，隨著時間的推移陀螺儀漂移。我們可以在已知的溫度范圍內(nèi)校準偏置不穩(wěn)定，但積分常數(shù)偏角會導致不穩(wěn)定的錯誤。這種誤差隨著陀螺儀旋轉(zhuǎn)或角度估計的長期漂移而積累。漂移航向計算錯誤造成不良后果將會繼續(xù)增加，而不是減少。而加速度計則相反，其對振動和其他非重力以及加速度變化敏感。因此，實際應(yīng)用中，數(shù)據(jù)通過它們?nèi)诤?，相輔相成。

對于零漂我們閾值限定的方法修正，溫漂采用程序進行補償?shù)姆椒ㄐＵ?，溫漂系?shù)通過建立溫漂補償模型測定（一般情況下，傳感器的溫漂系數(shù)會被廠商標定，但由于各傳感器溫漂系數(shù)不同，而各傳感器在電路的組合導致溫漂系數(shù)改變，故無法直接使用給定溫漂系數(shù)。溫漂補償模型的建立可參考北微傳感器公司的文章——MEMS 陀螺儀如何進行信號溫漂補償，溫度數(shù)據(jù)直接使用集成到BMP280 中的溫度傳感器的數(shù)據(jù)）。對于原始數(shù)據(jù)的不平滑我們在之前已經(jīng)采用了卡爾曼濾波進行校正。

②實現(xiàn)代碼：

可參考《一種基于六軸MEMS 器件的低成本動態(tài)傾角傳感器設(shè)計》，這里不再贅述。

③數(shù)據(jù)驗證與取舍：

由于在復雜情況下，如衛(wèi)星數(shù)較少、短時間內(nèi)氣壓變化劇烈時，數(shù)據(jù)可能存在問題，不能反映真實情況，所以我們采取對各傳感器的數(shù)據(jù)驗證、融合、取舍的策略，具體步驟見如圖8-圖9 的程序框圖。

4 結(jié)語

本系統(tǒng)使用了定位模塊及多個傳感器對數(shù)據(jù)測定，并利用了成熟的卡爾曼濾波、數(shù)據(jù)融合算法實現(xiàn)了在復雜情況下對物體運動狀態(tài)的精確測定，同時能夠通過藍牙將數(shù)據(jù)回傳便于實時監(jiān)測。在實驗中與依靠單數(shù)據(jù)測定值進行對比，精度有較大幅度的提高。

最后感謝國家、學校的經(jīng)費支持，以及學校對實驗設(shè)備資料的提供，為本項目的完成奠定了基礎(chǔ)。

圖8

圖9