基于卡爾曼濾波的加速度計陀螺儀水平角度測量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及基于卡爾曼濾波的水平傾角測量方法，屬于動態(tài)設(shè)備系統(tǒng)的測量技術(shù) 領(lǐng)域。
【背景技術(shù)】
[0002] 在控制系統(tǒng)中，水平角度測量值是極其重要的測量信號，通常作為控制系統(tǒng)的反 饋信號，為系統(tǒng)進行校正，因此其精度及穩(wěn)定性極其重要。目前常見的水平角度測量元件多 為傾角傳感器，常用的傾角測量傳感器分為固體式、液體式和氣體擺式三大類，它們都是基 于牛頓第二定律來研制的。在靜態(tài)情況下測量，它們均可以獲得較高的精度。但是當(dāng)載體在 動態(tài)情況下，因為加速度的干擾會在傳感器的敏感軸上加入分量，從而造成測量上的誤差， 使系統(tǒng)的測量精度降低，進而對系統(tǒng)的控制會產(chǎn)生一定的影響。重力擺式的傾角傳感器在 動態(tài)情況下無法精確測量，而以利用定軸性原理制作的陀螺儀在原理上適用于動態(tài)測量系 統(tǒng)的角速度。在靜態(tài)時，加速度計直接測量物體重力加速度能夠運用反三角函數(shù)的公式換 算成系統(tǒng)的傾斜角度，但是在動態(tài)情況下受到干擾較大。而陀螺儀的輸出值是其該方向上 相對于靈敏軸的角速率，利用角速率對時間的積分得到其角度值，動態(tài)性良好，但是會隨時 間的推移累積漂移誤差。
【發(fā)明內(nèi)容】
[0003] 本發(fā)明的目的是為了解決加速度計陀螺儀測量水平角度在動態(tài)測量環(huán)境下精度 差的問題，本發(fā)明提供一種基于卡爾曼濾波的加速度計陀螺儀水平角度測量方法。
[0004] 本發(fā)明的基于卡爾曼濾波的加速度計陀螺儀水平角度測量方法，
[0005] 所述方法包括如下步驟：
[0006] 步驟一：利用加速度計和陀螺儀采集關(guān)于X軸的數(shù)據(jù)，獲得加速度和角速率；
[0007] 步驟二：對獲得的加速度和角速率進行數(shù)字濾波，濾除在傳輸過程中產(chǎn)生的高頻 干擾；
[0008] 步驟三：將濾波后的加速度轉(zhuǎn)換成角度；
[0009] 步驟四：根據(jù)轉(zhuǎn)換的角度，代入溫度補償因素，得到補償后的角度；
[0010] 步驟五：將補償后的角度和濾波后的角速率進行卡爾曼濾波，并進行數(shù)據(jù)融合，獲 得水平角度的最優(yōu)估計值。
[0011] 所述步驟五的過程包括：
[0012] 步驟五一：設(shè)置參數(shù)的初值：
[0013] 矩陣
，〖表示當(dāng)前時刻，dt表示積分，矩陣
，過程噪聲的協(xié) 方差矩陣！
93表示加速度計的測量協(xié)方差，q g表示陀螺儀的測量協(xié)方差，R =
[rj，^表示測量誤差的協(xié)方差，水平角度估計值的協(xié)方差初值
，水平角度 估計初值-
[0014] 步驟五二：預(yù)測水平角度估計值協(xié)方差P (t It-Ι):
[0015] P (t 11-1) = AP (t-111-1) AT+Q ；
[0016] P (t I t-i)為 X (t I t-i)的協(xié)方差；
[0017] 步驟五三：根據(jù)P (t I t-i)，計算卡爾曼增益Kg (t):
[0018] Kg (t) = P(t|t-l)HT/[HP(t|t-l)HT+R]；
[0019] 觀測矩陣 H =[10];
[0020] 步驟五四：預(yù)測水平角度估計值X (t I t-Ι):
[0021] X (t 11-1) = AX (t-111-1)+BU (t)；
[0022] X(t 11-1)表示根據(jù)t-1時刻的預(yù)測t時刻的水平角度估計值，U(t)表示t時刻的 控制量；
[0023] 步驟五五：根據(jù)當(dāng)前時刻的角速率測量值Z(t)，結(jié)合X(t|t-1)和kg(t)，獲得水平 角度的最優(yōu)估計值X (t 11):
[0024] X (t 11) = X (t 11-1) +Kg (t) [Z (t) -HX (t I t-ι)];
[0025] 步驟五六：根據(jù)X (t 11)，獲得協(xié)方差P (t 11):
[0026] P(t|t) = [In-Kg (t) H] P (t 11-1)；
[0027] In表示η階單位矩陣，即對角線元素全為1的矩陣；
[0028] 轉(zhuǎn)入步驟五二，預(yù)測下一時刻的協(xié)方差。
[0029] 本發(fā)明的有益效果在于，本發(fā)明中，首先對從加速度計和陀螺儀中采集到的數(shù)據(jù) 進行滑動加權(quán)濾波，濾除信號傳輸過程中產(chǎn)生的高頻干擾，再將處理過的角度和角速率信 號在進行角度解算，采用卡爾曼數(shù)據(jù)融合技術(shù)，將加速度計和陀螺的測量數(shù)據(jù)進行數(shù)據(jù)融 合。最終，得到實時水平角度的最優(yōu)估計值本發(fā)明采用陀螺儀測量的角速率預(yù)知角度的變 化，進行動態(tài)下的測量。由此來對動態(tài)傾角測量進行優(yōu)勢互補，保證了測量精度。
【附圖說明】
[0030] 圖1為【具體實施方式】一的所述的基于卡爾曼濾波的加速度計陀螺儀水平角度測 量方法的流程示意圖。
【具體實施方式】
【具體實施方式】 [0031] 一：結(jié)合圖1說明本實施方式，本實施方式所述的基于卡爾曼濾波 的加速度計陀螺儀水平角度測量方法，所述方法包括如下步驟：
[0032] 步驟一：利用加速度計和陀螺儀采集關(guān)于X軸的數(shù)據(jù)，獲得加速度和角速率；
[0033] 步驟二：對獲得的加速度和角速率進行數(shù)字濾波，濾除在傳輸過程中產(chǎn)生的高頻 干擾；
[0034] 步驟三：將濾波后的加速度轉(zhuǎn)換成角度；
[0035] 步驟四：根據(jù)轉(zhuǎn)換的角度，代入溫度補償因素，得到補償后的角度；
[0036] 步驟五：將補償后的角度和濾波后的角速率進行卡爾曼濾波，并進行數(shù)據(jù)融合，獲 得水平角度的最優(yōu)估計值。
[0037] 所述步驟五的過程包括：
[0038] 步驟五一：設(shè)置參數(shù)的初值：
[0039] 矩陣.
> t表示當(dāng)前時刻，dt表示積分，矩陣
，過程噪聲的協(xié) 方差矩_
，qa表示加速度計的測量協(xié)方差，q g表示陀螺儀的測量協(xié)方差，R =
[rj，^表示測量誤差的協(xié)方差，水平角度估計值的協(xié)方差初值
水平角度 估計初值
[0040] 步驟五二：預(yù)測水平角度估計值協(xié)方差P (t I t-i):
[0041] P (t lt-1) = AP(t-l |t-l)AT+Q ；
[0042] P (t 11-1)為 X (t 11-1)的協(xié)方差；
[0043] 步驟五三：根據(jù)P (t 11-1)，計算卡爾曼增益Kg (t):
[0044] Kg (t) = P(t|t-l)HT/[HP(t|t-l)HT+R]；
[0045] 觀測矩陣 H =[10];
[0046] 步驟五四：預(yù)測水平角度估計值X (t I t-Ι):
[0047] X (t |t-l) = AX(t-l |t-l)+BU(t)；
[0048] X(t 11-1)表示根據(jù)t-1時刻的預(yù)測t時刻的水平角度估計值，U(t)表示t時刻的 控制量；
[0049] 步驟五五：根據(jù)當(dāng)前時刻的角速率測量值Z(t)，結(jié)合X(t|t-1)和Kg(t)，獲得水平 角度的最優(yōu)估計值X (t 11):
[0050] X (t 11) = X (t 11-1) +Kg (t) [Z (t) -HX (t I t-i)]；
[0051] 步驟五六：根據(jù)X (t It)，獲得協(xié)方差P (t It):
[