張 波，孫 哲，歐陽(yáng)寧，聞小龍，田 躍

(北京科技大學(xué)數(shù)理學(xué)院北京市弱磁檢測(cè)及應(yīng)用工程技術(shù)研究中心，北京 100083)

0 引言

傾角傳感器，是一種用于測(cè)量物體在靜態(tài)或動(dòng)態(tài)環(huán)境下與初始狀態(tài)所成傾斜角度的測(cè)量工具，常用于航空航天、橋梁檢測(cè)、自動(dòng)控制等領(lǐng)域。目前常用的加速度式傾角傳感器、導(dǎo)電液傾角傳感器等在靜態(tài)測(cè)量中表現(xiàn)出了較高的性能，然而由于在動(dòng)態(tài)環(huán)境中引入了更多的干擾信號(hào)，使其測(cè)得的動(dòng)態(tài)傾角并不正確[1]。目前測(cè)量物體動(dòng)態(tài)運(yùn)動(dòng)條件下的傾斜角一般采用MEMS陀螺儀與加速度計(jì)作為慣性測(cè)量器件，利用互補(bǔ)濾波[2]、擴(kuò)展卡爾曼濾波[3]等方法對(duì)其輸出進(jìn)行數(shù)據(jù)融合后解算姿態(tài)角。擴(kuò)展卡爾曼濾波作為數(shù)據(jù)融合算法有著精度高的優(yōu)點(diǎn)，但其計(jì)算較復(fù)雜。而互補(bǔ)濾波充分利用陀螺儀的動(dòng)態(tài)響應(yīng)能力與加速度計(jì)的靜態(tài)精度進(jìn)行數(shù)據(jù)融合，在低成本的動(dòng)態(tài)系統(tǒng)中廣泛應(yīng)用。

本文選用低成本中較高精度的MEMS器件，設(shè)計(jì)了一種基于自適應(yīng)互補(bǔ)濾波的動(dòng)態(tài)傾角傳感器，利用加速度計(jì)輸出自適應(yīng)調(diào)整互補(bǔ)參數(shù)，對(duì)比固定參數(shù)互補(bǔ)濾波精度有所提升，并且通過(guò)建立誤差模型對(duì)MEMS慣性器件進(jìn)行輸出校正，進(jìn)一步提高精度。

1 測(cè)量原理

傾角的變化，即物體姿態(tài)的變化。首先建立參考坐標(biāo)系(常為東北天坐標(biāo)系)以描述物體在空間中所處的位置，同時(shí)以物體固定三軸為載體坐標(biāo)系。在導(dǎo)航系統(tǒng)中，常通過(guò)航向角Ψ、俯仰角θ及滾轉(zhuǎn)角γ來(lái)描述一個(gè)物體的姿態(tài)。物體在空間中的姿態(tài)變化，即任意的空間轉(zhuǎn)動(dòng)，可表示為坐標(biāo)轉(zhuǎn)換，從而通過(guò)一個(gè)旋轉(zhuǎn)矩陣來(lái)表示。根據(jù)歐拉旋轉(zhuǎn)定理，1次空間轉(zhuǎn)動(dòng)可以通過(guò)3次獨(dú)立的旋轉(zhuǎn)來(lái)實(shí)現(xiàn)。如圖1所示，載體坐標(biāo)系與導(dǎo)航坐標(biāo)系在初始時(shí)刻重合，當(dāng)物體在空間中做一次轉(zhuǎn)動(dòng)時(shí)，可通過(guò)Z-X-Y的順序進(jìn)行3次的繞軸旋轉(zhuǎn)來(lái)表示，每次轉(zhuǎn)動(dòng)可通過(guò)左乘旋轉(zhuǎn)矩陣實(shí)現(xiàn)，3次旋轉(zhuǎn)得到最終旋轉(zhuǎn)矩陣為：

(1)

由式(1)可得，當(dāng)已知姿態(tài)旋轉(zhuǎn)矩陣時(shí)，可通過(guò)其反解3個(gè)姿態(tài)角，如下式：

(2)

表示姿態(tài)轉(zhuǎn)換的方法主要有歐拉角法、九元素法及四元數(shù)法。四元數(shù)法較前兩者在工程上有著廣泛的應(yīng)用，因其沒(méi)有萬(wàn)向節(jié)死鎖，可全角度表示旋轉(zhuǎn)并且只有4個(gè)元素，解法較九元素簡(jiǎn)單，所以本文采用四元數(shù)法表示姿態(tài)轉(zhuǎn)換。用四元數(shù)所表示的姿態(tài)轉(zhuǎn)換矩陣為：

(3)

通過(guò)捷聯(lián)慣導(dǎo)相關(guān)原理[4]，可得四元數(shù)微分方程使用一階龍格庫(kù)塔法解，可得:

(4)

式中Δt為時(shí)間間隔。

將陀螺儀測(cè)得的角速度[ωx,ωy,ωz]對(duì)四元數(shù)進(jìn)行實(shí)時(shí)更新，即可得到3個(gè)姿態(tài)角。

2 系統(tǒng)設(shè)計(jì)

硬件系統(tǒng)設(shè)計(jì)主要由微控制器STM32F103C8最小系統(tǒng)電路、ICM20602慣性測(cè)量電路、RS232/RS485通信電路以及電源電路組成。由STM32F103C8微控制器通過(guò)I2C讀取ICM20602輸出，進(jìn)行數(shù)據(jù)處理與姿態(tài)解算，再由RS232/485電路將數(shù)據(jù)包傳輸至上端設(shè)備，系統(tǒng)硬件框圖如圖2所示。

圖2 系統(tǒng)硬件框圖

2.1 STM32最小系統(tǒng)及其外圍電路

本文選擇STM32F103C8T6作為動(dòng)態(tài)傾角傳感器的主控芯片。它具有64KB FLASH、20KB SDRAM、4個(gè)TIMER以及37個(gè)通用GPIO口、兩路硬件I2C等外設(shè)。LQFP64的封裝使得傳感器的尺寸能夠進(jìn)一步縮小，且本文所設(shè)計(jì)的動(dòng)態(tài)傾角傳感器對(duì)外設(shè)的要求僅為一路I2C接口及UART口，符合系統(tǒng)設(shè)計(jì)要求。圖3為硬件系統(tǒng)STM32最小系統(tǒng)電路，包含晶振電路，復(fù)位電路及調(diào)試電路。

圖3 STM32F103C8T6主控電路

2.2 ICM20602慣性測(cè)量電路

圖4 ICM20602慣性測(cè)量電路

2.3 RS232/RS485通信電路

串口通信電路采用RS485芯片作為核心器件，通過(guò)9013 NPN三極管控制其使能發(fā)送接收端以控制輸出，圖5為傳感器通信電路。

圖5 通信電路

2.4 電源電路

對(duì)于大多數(shù)工業(yè)應(yīng)用中采用的24 V供電，本文利用78L15先將24 V降為15 V，再利用MIC5202-3.3 V將供電電壓穩(wěn)壓至3.3 V，對(duì)整體系統(tǒng)進(jìn)行供電，圖6為傳感器電源電路。

圖6 電源電路

3 軟件設(shè)計(jì)

3.1 自適應(yīng)互補(bǔ)濾波及角度解算

由式(1)～式(4)可知，可根據(jù)陀螺儀測(cè)量角速度更新四元數(shù)，而MEMS陀螺儀含有漂移誤差隨著時(shí)間積累，對(duì)積分得到的四元數(shù)中的累計(jì)誤差將越來(lái)越大?；パa(bǔ)濾波通過(guò)將式中的四元數(shù)矩陣中重力分量與加速度計(jì)輸出的重力向量做向量叉積，得到加速度誤差通過(guò)互補(bǔ)系數(shù)調(diào)整陀螺儀角速度誤差融合至陀螺儀輸出，從而達(dá)到傳感器信息融合。其主要過(guò)程如下：

(5)

(6)

(7)

(8)

以上為固定參數(shù)互補(bǔ)濾波進(jìn)行姿態(tài)估計(jì)的過(guò)程，當(dāng)系統(tǒng)運(yùn)動(dòng)狀態(tài)發(fā)生變化時(shí)，由于互補(bǔ)系數(shù)的固定，將出現(xiàn)估計(jì)值與實(shí)際值之間較大的誤差。若對(duì)于互補(bǔ)系數(shù)kP的選擇通過(guò)對(duì)當(dāng)前加速度輸出加以調(diào)整，將實(shí)現(xiàn)自適應(yīng)變化運(yùn)動(dòng)狀態(tài)下的姿態(tài)解算，其中自適應(yīng)系數(shù)的算法為：

(9)

式中：eg=(||||acccal||-9.8||)2，0.01為多組實(shí)驗(yàn)數(shù)據(jù)得到的加速度計(jì)動(dòng)靜態(tài)輸出方差閾值；acccal為校正后的加速度計(jì)輸出；kP0為初始互補(bǔ)系數(shù)，初始取值為10，在靜態(tài)情況下穩(wěn)定修正陀螺儀輸出，f(e)為自適應(yīng)函數(shù)，根據(jù)加速度計(jì)與標(biāo)準(zhǔn)重力之間的誤差增加而增加，最終使得互補(bǔ)系數(shù)kp縮減至0。

根據(jù)PI算法經(jīng)典理論，當(dāng)加速度計(jì)歸一化得到的重力分量偏差與標(biāo)準(zhǔn)重力值偏差較大時(shí)，減小互補(bǔ)系數(shù)，輸出取決于陀螺儀的動(dòng)態(tài)響應(yīng)，而偏差值較小時(shí)，增大互補(bǔ)系數(shù)，利用加速度計(jì)靜態(tài)測(cè)量修正陀螺儀靜態(tài)下的輸出，自適應(yīng)互補(bǔ)濾波算法流程框圖如圖7所示。

圖7 自適應(yīng)互補(bǔ)濾波流程圖

在實(shí)時(shí)更新得到四元數(shù)后，對(duì)四元數(shù)進(jìn)行解算得到各姿態(tài)角，在大角度即超過(guò)85°時(shí)解算將出現(xiàn)奇異值，且當(dāng)俯仰角超過(guò)85°時(shí)旋轉(zhuǎn)橫滾角等效于旋轉(zhuǎn)航向角，在姿態(tài)解算中可鎖死航向角為零，從而避免奇異值的出現(xiàn)[5]，系統(tǒng)角度解算算法流程如圖8所示。

圖8 姿態(tài)角解算流程圖

3.2 慣性測(cè)量輸出校正

在姿態(tài)角算法流程輸入加速度與角速度即可解算姿態(tài)角。大多數(shù)MEMS慣性測(cè)量芯片都會(huì)在出廠時(shí)進(jìn)行輸出校正，但對(duì)于低成本芯片其校正往往不夠準(zhǔn)確，大多數(shù)低精度傳感器往往忽略該步驟，采用芯片內(nèi)部校正，而高精度傳感器則利用外部設(shè)備進(jìn)行校正，但需要較長(zhǎng)時(shí)間(多則長(zhǎng)達(dá)幾h)的參數(shù)離線計(jì)算。通過(guò)建立誤差模型，對(duì)傳感器進(jìn)行靜止，利用靜止數(shù)據(jù)對(duì)模型進(jìn)行參數(shù)擬合可實(shí)現(xiàn)無(wú)需外部設(shè)備的快速校正。對(duì)加速度計(jì)與陀螺儀輸出建立如下誤差模型[6]：

(10)

(11)

式中：acal、ωcal分別為校正過(guò)后的加速度與角速度；araw、ωraw為ICM20602的原始輸出；Ta、Tω分別為加速度計(jì)與陀螺儀的軸正交誤差旋轉(zhuǎn)矩陣；Sa、Sω為尺度因子；aoffset、ωoffset分別為加速度計(jì)與陀螺儀的偏置。

gω_q(k)=f(Tω,Sω,ωoffset,ωk)

(12)

則定義目標(biāo)函數(shù)為

(13)

同樣通過(guò)最小二乘參數(shù)擬合得到陀螺儀誤差模型參數(shù)解[βij,sgyro,i,ostgyro,i](i≠j;i,j=x,y,z)。慣性測(cè)量輸出校正算法流程如圖9所示。

圖9 慣性測(cè)量輸出校正流程圖

3.3 系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)軟件設(shè)計(jì)包括自適應(yīng)濾波的程序?qū)崿F(xiàn)、分段解算算法程序、ICM20602器件的初始化配置程序、定時(shí)器初始化、定時(shí)器中斷程序、回調(diào)程序、RS232/485通信程序、I2C總線讀寫(xiě)程序。首先系統(tǒng)上電后執(zhí)行復(fù)位操作及系統(tǒng)初始化，然后通過(guò)I2C讀寫(xiě)操作對(duì)ICM20602進(jìn)行初始化配置，在接收到ICM20602初始化成功標(biāo)志后，啟動(dòng)定時(shí)器，在定時(shí)器達(dá)到2 ms或10 ms標(biāo)志時(shí)，在中斷服務(wù)函數(shù)中分別調(diào)用回調(diào)函數(shù)，2 ms中斷時(shí)回調(diào)函數(shù)為進(jìn)行讀取數(shù)據(jù)，輸出校正，數(shù)據(jù)融合及角度解算，在10 ms中斷時(shí)回調(diào)函數(shù)進(jìn)行角度數(shù)據(jù)根據(jù)通信協(xié)議封裝為數(shù)據(jù)包，通過(guò)RS232/485發(fā)送數(shù)據(jù)至上位機(jī)，整個(gè)軟件流程框圖如圖10所示。

圖10 傳感器軟件框圖

4 實(shí)驗(yàn)驗(yàn)證

首先對(duì)加速度計(jì)與陀螺儀原始輸出進(jìn)行校正，將傳感器6個(gè)面反轉(zhuǎn)13次，每次翻轉(zhuǎn)之間靜置間隔從2～10 s不等，整個(gè)校正過(guò)程共50 s左右，得到陀螺儀與加速度計(jì)原始輸出，如圖11所示，擬合誤差模型參數(shù)如表1所示。

表1 擬合誤差模型參數(shù)結(jié)果

(a)加速度計(jì)

其中默認(rèn)初始時(shí)傳感器X軸與校正平面X軸重合，故αxz、αxy、αyx為0。將未經(jīng)校正、數(shù)據(jù)融合采用定參數(shù)的傳感器和本文所設(shè)計(jì)的校正后采用自適應(yīng)互補(bǔ)濾波的傳感器固定于三維轉(zhuǎn)臺(tái)，對(duì)動(dòng)態(tài)傾角傳感器進(jìn)行對(duì)比測(cè)試，其測(cè)試結(jié)構(gòu)如圖12所示。

圖12 測(cè)試結(jié)構(gòu)示意圖

隨意旋轉(zhuǎn)三維轉(zhuǎn)臺(tái)，對(duì)傳感器輸出進(jìn)行記錄，得到旋轉(zhuǎn)情況下的角度輸出如圖13所示。

(a)定參數(shù)與自適應(yīng)傳感器橫滾角對(duì)比

由圖13可知，自適應(yīng)互補(bǔ)濾波傳感器可以在旋轉(zhuǎn)動(dòng)態(tài)環(huán)境下更好地適應(yīng)旋轉(zhuǎn)引起的加速度計(jì)突變，減少角度異常突變情況，更加準(zhǔn)確的計(jì)算角度，如圖13中小圖放大所示。將傳感器靜置10 min，得到其靜態(tài)條件下輸出誤差曲線如圖14所示。

(a)橫滾角輸出誤差

由圖14可得，自適應(yīng)&校正傳感器在靜態(tài)條件下輸出噪聲幅度小，自適應(yīng)互補(bǔ)濾波能夠在靜態(tài)條件下，能夠更好地通過(guò)加速度修正陀螺儀的隨機(jī)漂移，由數(shù)據(jù)計(jì)算得其噪聲方差如表2所示，由表2可知，校正后自適應(yīng)傳感器穩(wěn)定性更好。

表2 靜置條件下傳感器噪聲方差

通過(guò)對(duì)各軸旋轉(zhuǎn)不同角度，讀出傳感器輸出后，得到誤差值繪制圖如圖15所示。

圖15 傳感器各角度誤差絕對(duì)值曲線

由圖15可得，本文所設(shè)計(jì)的傳感器傾角的精度在0.36°以內(nèi)。

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

本文設(shè)計(jì)了一種基于ICM20602慣性器件的高精度動(dòng)態(tài)傾角傳感器，建立誤差模型，利用參數(shù)擬合方法進(jìn)行自校正其原始數(shù)據(jù)，并根據(jù)加速度計(jì)在互補(bǔ)濾波中低頻穩(wěn)定的特性，設(shè)計(jì)一種根據(jù)加速度計(jì)輸出自適應(yīng)的互補(bǔ)濾波進(jìn)行數(shù)據(jù)融合。通過(guò)與未校正及定參數(shù)互補(bǔ)濾波的動(dòng)態(tài)傾角傳感器對(duì)比實(shí)驗(yàn)，得出本文設(shè)計(jì)的校正輸出后利用自適應(yīng)互補(bǔ)濾波融合數(shù)據(jù)的動(dòng)態(tài)傾角傳感器靜態(tài)穩(wěn)定性較好，動(dòng)態(tài)對(duì)加速度計(jì)畸變抑制能力強(qiáng)，精度為0.36°以內(nèi)，且硬件成本較低，可廣泛應(yīng)用于工業(yè)領(lǐng)域。