基于ARM的動(dòng)態(tài)傾角傳感器

杜秋宇 張方超 程思堯 康二兵

摘要?動(dòng)態(tài)傾角傳感器的功能主要體現(xiàn)在運(yùn)動(dòng)狀態(tài)下傾角測(cè)量的實(shí)時(shí)性與跟隨性，本文采用ADIS16485慣性組合測(cè)量單元、STM32F107微控制器為核心設(shè)計(jì)了一種動(dòng)態(tài)傾角傳感器，通過(guò)對(duì)傳感器數(shù)據(jù)讀取、處理、融合等解算出姿態(tài)信息，利用CAN口通信將解算數(shù)據(jù)發(fā)送至PC機(jī)，實(shí)現(xiàn)對(duì)載體姿態(tài)的實(shí)時(shí)顯示。文中采用的數(shù)據(jù)融合技術(shù)彌補(bǔ)了不同傳感器之間的誤差，提高了動(dòng)態(tài)傾角傳感器的測(cè)量精度。

【關(guān)鍵詞】加速度計(jì) 角速度計(jì) 動(dòng)態(tài)傾角傳感器 ARM

隨著武器系統(tǒng)的更新?lián)Q代，新的測(cè)控理念大量應(yīng)用，原有用于靜態(tài)測(cè)量的傾角傳感器逐漸出現(xiàn)局限性，如測(cè)量范圍小、響應(yīng)時(shí)間長(zhǎng)、運(yùn)動(dòng)環(huán)境下無(wú)法實(shí)時(shí)響應(yīng)等，現(xiàn)急需動(dòng)態(tài)環(huán)境測(cè)量的傾角測(cè)量設(shè)備。

1 動(dòng)態(tài)傾角傳感器硬件設(shè)計(jì)

動(dòng)態(tài)傾角傳感器測(cè)量單元為ADIS16485，控制電路以ARM單片機(jī)STM32F107為核心設(shè)計(jì)，硬件電路結(jié)構(gòu)框圖如圖1所示，主要包含電源模塊、微處理器電路、通訊接口電路和存儲(chǔ)電路。

2 動(dòng)態(tài)傾角傳感器軟件設(shè)計(jì)

動(dòng)態(tài)傾角傳感器軟件主要實(shí)現(xiàn)數(shù)據(jù)讀取、濾波、姿態(tài)解算、信息融合、角度輸出等功能，軟件運(yùn)行時(shí)，首先進(jìn)行初始化，完成傳感器、外圍器件及系統(tǒng)的初始化，通過(guò)外部SPI接口讀取傳感器數(shù)據(jù)，對(duì)讀取數(shù)據(jù)進(jìn)行濾波，獲取傳感器初始姿態(tài)，然后判斷載體是否運(yùn)動(dòng)，如果判斷載體進(jìn)行了運(yùn)動(dòng)，整個(gè)姿態(tài)更新解算模塊就開(kāi)始啟動(dòng)，進(jìn)行姿態(tài)角更新，而后進(jìn)行卡爾曼濾波進(jìn)行信息融合，最終輸出姿態(tài)角信息。

下面對(duì)軟件濾波技術(shù)、姿態(tài)解算、信息融合三個(gè)核心部分進(jìn)行詳細(xì)介紹：

2.1 濾波技術(shù)

配置傳感器ADIS16485時(shí)參照功能控制寄存器，使得寄存器啟動(dòng)Y軸的濾波器器組D，同理也相應(yīng)的配置X，Z軸陀螺儀的FIR濾波器，這樣從寄存器讀出的三軸陀螺儀數(shù)據(jù)就是經(jīng)過(guò)了低通濾波去除高頻白噪聲的信號(hào)，對(duì)傳感器的輸出信號(hào)進(jìn)行初步降噪。

為了進(jìn)一步提高采樣數(shù)據(jù)精度，對(duì)讀取的采樣數(shù)據(jù)繼續(xù)做了一個(gè)均值濾波，讀10組數(shù)據(jù)后做均值處理，每組數(shù)據(jù)包含了三軸陀螺儀數(shù)據(jù)，三軸加速度計(jì)數(shù)據(jù)，讀完10組后算成一組傳感器數(shù)據(jù)。

2.2 姿態(tài)解算

姿態(tài)解算模塊是整個(gè)軟件的核心部分，包括傳感器的初始對(duì)準(zhǔn)，初始四元數(shù)轉(zhuǎn)化，四元數(shù)更新，卡爾曼信息融合，四元數(shù)轉(zhuǎn)換姿態(tài)角等模塊。

姿態(tài)解算流程為采用粗對(duì)準(zhǔn)解算初始姿態(tài)，計(jì)算初始四元數(shù)，然后讀取三組三軸陀螺儀數(shù)據(jù)，計(jì)算新四元數(shù)，提取四元數(shù)更新作為狀態(tài)變量，結(jié)合初始四元數(shù)進(jìn)行卡爾曼信息融合，獲取最新姿態(tài)信息。

傳感器正常上電后，內(nèi)部寄存器就會(huì)按照設(shè)定的更新速率保存獲得的角速度、線(xiàn)速度信息，STM32F107通過(guò)外部SPI接口讀取這些參數(shù)進(jìn)行實(shí)時(shí)姿態(tài)解算。

2.3 數(shù)據(jù)融合

由于從陀螺儀的角速度獲得角度信息，需要經(jīng)過(guò)積分運(yùn)算。如果角速度信號(hào)存在微小的偏差，經(jīng)過(guò)積分運(yùn)算之后，變化形成積累誤差。這個(gè)誤差會(huì)隨著時(shí)間延長(zhǎng)逐步增加，最終導(dǎo)致電路飽和，無(wú)法形成正確的角度信號(hào)，進(jìn)而保證不了動(dòng)態(tài)傾角傳感器實(shí)時(shí)角度的可靠性和精度。選用卡爾曼濾波技術(shù)進(jìn)行加速度計(jì)和角速度的信息融合，利用加速度計(jì)靜態(tài)下的優(yōu)勢(shì)彌補(bǔ)角速度計(jì)的積累誤差。

3 試驗(yàn)結(jié)果

3.1 靜態(tài)測(cè)試

將產(chǎn)品置于光柵分度頭上，通過(guò)CAN分析儀與計(jì)算機(jī)相連，上電后產(chǎn)品將角度數(shù)據(jù)發(fā)送至PC機(jī)，通過(guò)CAN調(diào)試軟件讀取數(shù)據(jù)。動(dòng)態(tài)傾角傳感器常溫線(xiàn)性度及重復(fù)精度的測(cè)試結(jié)果：常溫+25℃，線(xiàn)性度0 240，重復(fù)精度0.030。

3.2 動(dòng)態(tài)測(cè)試

車(chē)體和船體的搖擺周期通常為數(shù)秒到數(shù)十秒，頻率為0.5Hz以下，姿態(tài)變化在10°左右范圍，因此采用高精度位置、高精度速率轉(zhuǎn)臺(tái)模擬船體的典型運(yùn)動(dòng)狀態(tài)，俯仰角在±10°。內(nèi)進(jìn)行0.5Hz的正弦搖擺，同時(shí)轉(zhuǎn)臺(tái)橫滾角在±10°內(nèi)進(jìn)行0.5Hz的正弦搖擺，以此考核動(dòng)態(tài)傾角儀的測(cè)試精度。

由于轉(zhuǎn)臺(tái)所模擬的角度變化為俯仰角在±10°內(nèi)進(jìn)行0.5Hz的正弦搖擺，同時(shí)轉(zhuǎn)臺(tái)滾轉(zhuǎn)角在±10°內(nèi)進(jìn)行0.5Hz的正弦搖擺，其數(shù)學(xué)模型如式1～3所示：

θ=10°sm（2π×0.5t+ψpitch_i） （1）

γ=10°sin （2πx0.5t+ψroll_i） （2）

考慮到測(cè)試數(shù)據(jù)中的誤差項(xiàng)，采用一階傅里葉級(jí)數(shù)模型進(jìn)行數(shù)據(jù)擬合：

f（t） =a0+alsin2πft+blcos2πft （3）

橫滾角和俯仰角測(cè)試結(jié)果擬合時(shí)選擇95%置信區(qū)間，動(dòng)態(tài)傾角傳感器動(dòng)態(tài)測(cè)試結(jié)果橫滾角測(cè)量誤差0.0575°，俯仰角誤差0.4421°。

4 結(jié)論

利用數(shù)據(jù)融合算法設(shè)計(jì)的互補(bǔ)濾波器，有效去除加速度計(jì)動(dòng)態(tài)情況下的干擾和陀螺儀累積漂移誤差，選擇合適的測(cè)試模型，得到比較準(zhǔn)確的動(dòng)態(tài)角度值，確定了該動(dòng)態(tài)傾角傳感器測(cè)量的準(zhǔn)確性。

參考文獻(xiàn)

[1]何秀鳳，袁信，汪淑華.Kalman濾波技術(shù)在磁航向和捷聯(lián)慣性組合系統(tǒng)初始對(duì)準(zhǔn)中的應(yīng)用[J].南京航空航天大學(xué)學(xué)報(bào)，1995 （06）：363-369.

[2] Kalman R E.A New Approach to Linearfiltering and Prediction Theory[J].Trans. ASME. Journal of BasicEng，1960，82D： 35-46.