0052] P(t|t) = [In-Kg (t) H] P (t 11-1)；
[0053] In表示η階單位矩陣，即對(duì)角線元素全為1的矩陣；
[0054] 轉(zhuǎn)入步驟五二，預(yù)測(cè)下一時(shí)刻的協(xié)方差。
[0055] 本實(shí)施方式中，基于卡爾曼濾波的水平角度測(cè)量系統(tǒng)以STM32F103C8作為控制 器，以其最小系統(tǒng)為基礎(chǔ)，包括復(fù)位電路、晶振電路、電源電路、LMl 117線型穩(wěn)壓源、TTL轉(zhuǎn) USB下載器以及內(nèi)部嵌入加速度計(jì)和陀螺儀的MPU6050。
[0056] 在控制系統(tǒng)工作時(shí)，數(shù)字信號(hào)輸入過程中，伴隨測(cè)量數(shù)據(jù)而來的還有各種噪聲和 干擾，為了保證測(cè)量系統(tǒng)的精度，需要消除這些干擾和噪聲，本實(shí)施方式中，采用滑動(dòng)加權(quán) 濾波。在采樣中，采樣得到的η次數(shù)值的比重如果相同，對(duì)于時(shí)變信號(hào)，會(huì)引入一些之后，且 η值越大，滯后嚴(yán)重。因此，為了適當(dāng)?shù)臏p少滯后現(xiàn)象，可以在計(jì)算的過程中加入權(quán)值，進(jìn)而 增加新得到的采樣值對(duì)于濾波值的影響所占的比重。越接近現(xiàn)時(shí)刻，對(duì)應(yīng)的權(quán)值也就越大。 對(duì)于滯后時(shí)間常數(shù)大的被控對(duì)象和采樣周期短的系統(tǒng)來說，使用滑動(dòng)加權(quán)濾波算法能夠更 好的抑制系統(tǒng)的噪聲。
[0060] 對(duì)常數(shù)C的選取也是有各種各樣的方法。常用的方法有加權(quán)系數(shù)法，利用對(duì)象的 純滯后時(shí)間來選擇C值。
[0061] 滑動(dòng)加權(quán)濾波的數(shù)學(xué)模型表達(dá)式如下所示：
[0062] Yk= (B kXk+Bk iXk !+Bk 2Xk 2+··· +B0X0) /A
[0063] 其中
，且BqSB1SB 2S-S Bk。常數(shù)項(xiàng)B的設(shè)置方法是用常用的加權(quán) 系數(shù)法，假設(shè)τ是對(duì)象純滯后時(shí)間，則此時(shí)
[0066] 由此可知，當(dāng)τ越大時(shí)，δ越小，新的采樣值的權(quán)系數(shù)也就越大，而歷史數(shù)據(jù)的權(quán) 系數(shù)越小，所以，將滑動(dòng)加權(quán)濾波運(yùn)用在這種具有較大時(shí)間常數(shù)的系統(tǒng)的，其新時(shí)刻采樣得 至_值所占的比重越大，可以有效抑制測(cè)量上的滯后現(xiàn)象。
[0067] 對(duì)于一個(gè)動(dòng)態(tài)的傾角測(cè)量系統(tǒng)來說，若只使用加速度計(jì)來測(cè)量載體傾角，則會(huì)帶 來很大的偏差，因?yàn)榧铀俣扔?jì)不但能夠感受的靜態(tài)的加速度，還能夠感覺的到載體所受的 動(dòng)態(tài)加速度，測(cè)角度時(shí)只需要取走靜態(tài)下的，因此動(dòng)態(tài)造成的加速度成了一種干擾，需要來 補(bǔ)償。選擇搭配角速率陀螺是因?yàn)樗茴A(yù)知角度的變化，進(jìn)行動(dòng)態(tài)下的測(cè)量。由此來對(duì)動(dòng) 態(tài)傾角測(cè)量進(jìn)行優(yōu)勢(shì)互補(bǔ)。本發(fā)明中，采用卡爾曼濾波法對(duì)加速度計(jì)和陀螺儀的測(cè)量數(shù)據(jù) 進(jìn)行數(shù)據(jù)融合，得到水平角度的最優(yōu)估計(jì)值。
[0068] 下面是建立該系統(tǒng)的狀態(tài)空間方程組?？紤]到傾角和傾角的角速率存在導(dǎo)數(shù)的關(guān) 系，所以可以設(shè)定其作為一個(gè)狀態(tài)變量。另外一個(gè)狀體變量選擇為陀螺儀的常值偏差b。
[0069] 得到狀態(tài)方程是：
[0071] 式中Wg為陀螺的噪聲誤差，Wgyro表示陀螺的輸出角速率，此狀態(tài)方程是以對(duì)角度 求導(dǎo)得到角速度來建立的。
[0072] 接下來是觀測(cè)方程：
[0073]
[0074] 式中Wa表示為加速度計(jì)測(cè)量產(chǎn)生的噪聲，％則表示加速度計(jì)換算得到的角度。
[0075] 將系統(tǒng)表示離散化，由此可以得到陀螺角速度在離散情況下的積分公式和加速度 計(jì)的觀測(cè)方稈式：
[0078] 由此將加速度計(jì)和陀螺的測(cè)量數(shù)據(jù)聯(lián)系起來，利用加速度計(jì)作為觀測(cè)量來對(duì)傾角 進(jìn)行修正。所以可得：
[0079]

[0080] 由上式可以的到，若要求得t時(shí)刻的傾角，就必須根據(jù)t-Ι時(shí)刻的傾角來估計(jì)，由 此卡爾曼濾波器進(jìn)行遞歸運(yùn)算，估計(jì)出最優(yōu)角度值。將上式與卡爾曼空間方程比較，所以可 得：
[0081] 3作為狀態(tài)空間的輸入信號(hào)，觀測(cè)矩陣H= [1 0]，其 中單位罔
[0082] 另外還剩下P，Q，R陣需要確認(rèn)。Q陣是系統(tǒng)過程噪聲的協(xié)方差，而R陣是系統(tǒng)測(cè) 量的誤差協(xié)方差。
[0083]
，R= [rj，其中qJPqg表示加速度計(jì)和陀螺的測(cè)量協(xié)方差，其表示 對(duì)傳感器數(shù)據(jù)的信任程度，數(shù)值越小，對(duì)傳感器的信任程度越高。根據(jù)工程設(shè)計(jì)的理念和經(jīng) 驗(yàn)來設(shè)定Q與R陣，使調(diào)試后得系統(tǒng)的較好的濾波效果。P陣的定義為狀態(tài)變量誤差的協(xié)方 差陣。根據(jù)定義可知，如果知道確切的初始位置，就可以給出
；如果不知道 其最初的位置，則可以初始化成一個(gè)對(duì)角線形式的矩陣，且對(duì)角線上取一個(gè)合適的較大的 數(shù)。
[0084] 在這個(gè)過程中，可以看出卡爾曼濾波器在遞推的過程中根據(jù)最小協(xié)方差，估算出 最優(yōu)的傾角值。只需要上一刻的最優(yōu)值，系統(tǒng)偏差，預(yù)測(cè)偏差，就能進(jìn)行推進(jìn)行此刻最優(yōu)值 的計(jì)算，不會(huì)在運(yùn)算上對(duì)系統(tǒng)造成很大負(fù)擔(dān)。
【主權(quán)項(xiàng)】
1. 基于卡爾曼濾波的加速度計(jì)陀螺儀水平角度測(cè)量方法，其特征在于，所述方法包括 如下步驟： 步驟一：利用加速度計(jì)和陀螺儀采集關(guān)于X軸的數(shù)據(jù)，獲得加速度和角速率； 步驟二：對(duì)獲得的加速度和角速率進(jìn)行數(shù)字濾波，濾除在傳輸過程中產(chǎn)生的高頻干 擾； 步驟三：將濾波后的加速度轉(zhuǎn)換成角度； 步驟四：根據(jù)轉(zhuǎn)換的角度，代入溫度補(bǔ)償因素，得到補(bǔ)償后的角度； 步驟五：將補(bǔ)償后的角度和濾波后的角速率進(jìn)行卡爾曼濾波，并進(jìn)行數(shù)據(jù)融合，獲得水 平角度的最優(yōu)估計(jì)值。2. 根據(jù)權(quán)利要求1所述的基于卡爾曼濾波的加速度計(jì)陀螺儀水平角度測(cè)量方法，其特 征在于，所述步驟五的過程包括： 步驟五一：設(shè)置參數(shù)的初值：步驟五二：預(yù)測(cè)水平角度估計(jì)值協(xié)方差P(t|t-l): P(t|t-1) =AP(t-111-1)AT+Q； P(t|t-1)為X(t|t-1)的協(xié)方差； 步驟五三：根據(jù)P(t11-1)，計(jì)算卡爾曼增益Kg(t): Kg(t) =P(t|t-l)HT/[HP(t|t-l)HT+R]； 觀測(cè)矩陣H= [1 0]; 步驟五四：預(yù)測(cè)水平角度估計(jì)值X(t|t-l): X(t11-1) =AX(t-111-1)+BU(t)； X(tIt-i)表示根據(jù)t-1時(shí)刻的預(yù)測(cè)t時(shí)刻的水平角度估計(jì)值，U(t)表示t時(shí)刻的控制 量； 步驟五五：根據(jù)當(dāng)前時(shí)刻的角速率測(cè)量值Z⑴，結(jié)合X(t11-1)和Kg⑴，獲得水平角度 的最優(yōu)估計(jì)值x(t|t): X(t|t) =X(tIt-l)+Kg(t) [Z(t)-HX(t|t-l)l； 步驟五六：根據(jù)X(t11)，獲得協(xié)方差P(t11): P(t|t) = [In-Kg(t)H]P(t11-1)； In表示n階單位矩陣，即對(duì)角線元素全為1的矩陣； 轉(zhuǎn)入步驟五二，預(yù)測(cè)下一時(shí)刻的協(xié)方差。
【專利摘要】基于卡爾曼濾波的加速度計(jì)陀螺儀水平角度測(cè)量方法，屬于動(dòng)態(tài)設(shè)備系統(tǒng)的測(cè)量技術(shù)領(lǐng)域。為了解決加速度計(jì)陀螺儀測(cè)量水平角度在動(dòng)態(tài)測(cè)量環(huán)境下精度差的問題。所述方法包括：一：利用加速度計(jì)和陀螺儀采集關(guān)于X軸的數(shù)據(jù)，獲得加速度和角速率；二：對(duì)獲得的加速度和角速率進(jìn)行數(shù)字濾波，濾除在傳輸過程中產(chǎn)生的高頻干擾；三：將濾波后的加速度轉(zhuǎn)換成角度；四：根據(jù)轉(zhuǎn)換的角度，代入溫度補(bǔ)償因素，得到補(bǔ)償后的角度；五：將補(bǔ)償后的角度和濾波后的角速率進(jìn)行卡爾曼濾波，并進(jìn)行數(shù)據(jù)融合，獲得水平角度的最優(yōu)估計(jì)值。用于控制系統(tǒng)中的校正。
【IPC分類】G01C1/00, G01C9/00
【公開號(hào)】CN105043348
【申請(qǐng)?zhí)枴緾N201510405397
【發(fā)明人】杜靖, 陳興林, 魏昊楠, 張博
【申請(qǐng)人】哈爾濱工業(yè)大學(xué)
【公開日】2015年11月11日
【申請(qǐng)日】2015年7月11日