基于卡爾曼濾波的加速度計(jì)陀螺儀水平角度測(cè)量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及基于卡爾曼濾波的水平傾角測(cè)量方法，屬于動(dòng)態(tài)設(shè)備系統(tǒng)的測(cè)量技術(shù) 領(lǐng)域。
【背景技術(shù)】
[0002] 在控制系統(tǒng)中，水平角度測(cè)量值是極其重要的測(cè)量信號(hào)，通常作為控制系統(tǒng)的反 饋信號(hào)，為系統(tǒng)進(jìn)行校正，因此其精度及穩(wěn)定性極其重要。目前常見(jiàn)的水平角度測(cè)量元件多 為傾角傳感器，常用的傾角測(cè)量傳感器分為固體式、液體式和氣體擺式三大類(lèi)，它們都是基 于牛頓第二定律來(lái)研制的。在靜態(tài)情況下測(cè)量，它們均可以獲得較高的精度。但是當(dāng)載體在 動(dòng)態(tài)情況下，因?yàn)榧铀俣鹊母蓴_會(huì)在傳感器的敏感軸上加入分量，從而造成測(cè)量上的誤差， 使系統(tǒng)的測(cè)量精度降低，進(jìn)而對(duì)系統(tǒng)的控制會(huì)產(chǎn)生一定的影響。重力擺式的傾角傳感器在 動(dòng)態(tài)情況下無(wú)法精確測(cè)量，而以利用定軸性原理制作的陀螺儀在原理上適用于動(dòng)態(tài)測(cè)量系 統(tǒng)的角速度。在靜態(tài)時(shí)，加速度計(jì)直接測(cè)量物體重力加速度能夠運(yùn)用反三角函數(shù)的公式換 算成系統(tǒng)的傾斜角度，但是在動(dòng)態(tài)情況下受到干擾較大。而陀螺儀的輸出值是其該方向上 相對(duì)于靈敏軸的角速率，利用角速率對(duì)時(shí)間的積分得到其角度值，動(dòng)態(tài)性良好，但是會(huì)隨時(shí) 間的推移累積漂移誤差。

【發(fā)明內(nèi)容】

[0003] 本發(fā)明的目的是為了解決加速度計(jì)陀螺儀測(cè)量水平角度在動(dòng)態(tài)測(cè)量環(huán)境下精度 差的問(wèn)題，本發(fā)明提供一種基于卡爾曼濾波的加速度計(jì)陀螺儀水平角度測(cè)量方法。
[0004] 本發(fā)明的基于卡爾曼濾波的加速度計(jì)陀螺儀水平角度測(cè)量方法，
[0005] 所述方法包括如下步驟：
[0006] 步驟一：利用加速度計(jì)和陀螺儀采集關(guān)于X軸的數(shù)據(jù)，獲得加速度和角速率；
[0007] 步驟二：對(duì)獲得的加速度和角速率進(jìn)行數(shù)字濾波，濾除在傳輸過(guò)程中產(chǎn)生的高頻 干擾；
[0008] 步驟三：將濾波后的加速度轉(zhuǎn)換成角度；
[0009] 步驟四：根據(jù)轉(zhuǎn)換的角度，代入溫度補(bǔ)償因素，得到補(bǔ)償后的角度；
[0010] 步驟五：將補(bǔ)償后的角度和濾波后的角速率進(jìn)行卡爾曼濾波，并進(jìn)行數(shù)據(jù)融合，獲 得水平角度的最優(yōu)估計(jì)值。
[0011] 所述步驟五的過(guò)程包括：
[0012] 步驟五一：設(shè)置參數(shù)的初值：
[0013] 矩陣
，〖表示當(dāng)前時(shí)刻，dt表示積分，矩陣
，過(guò)程噪聲的協(xié) 方差矩陣！
93表示加速度計(jì)的測(cè)量協(xié)方差，q g表示陀螺儀的測(cè)量協(xié)方差，R =
[rj，^表示測(cè)量誤差的協(xié)方差，水平角度估計(jì)值的協(xié)方差初值
，水平角度 估計(jì)初值-
[0014] 步驟五二：預(yù)測(cè)水平角度估計(jì)值協(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)，計(jì)算卡爾曼增益Kg (t):
[0018] Kg (t) = P(t|t-l)HT/[HP(t|t-l)HT+R]；
[0019] 觀測(cè)矩陣 H =[10];
[0020] 步驟五四：預(yù)測(cè)水平角度估計(jì)值X (t I t-Ι):
[0021] X (t 11-1) = AX (t-111-1)+BU (t)；
[0022] X(t 11-1)表示根據(jù)t-1時(shí)刻的預(yù)測(cè)t時(shí)刻的水平角度估計(jì)值，U(t)表示t時(shí)刻的 控制量；
[0023] 步驟五五：根據(jù)當(dāng)前時(shí)刻的角速率測(cè)量值Z(t)，結(jié)合X(t|t-1)和kg(t)，獲得水平 角度的最優(yōu)估計(jì)值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表示η階單位矩陣，即對(duì)角線元素全為1的矩陣；
[0028] 轉(zhuǎn)入步驟五二，預(yù)測(cè)下一時(shí)刻的協(xié)方差。
[0029] 本發(fā)明的有益效果在于，本發(fā)明中，首先對(duì)從加速度計(jì)和陀螺儀中采集到的數(shù)據(jù) 進(jìn)行滑動(dòng)加權(quán)濾波，濾除信號(hào)傳輸過(guò)程中產(chǎn)生的高頻干擾，再將處理過(guò)的角度和角速率信 號(hào)在進(jìn)行角度解算，采用卡爾曼數(shù)據(jù)融合技術(shù)，將加速度計(jì)和陀螺的測(cè)量數(shù)據(jù)進(jìn)行數(shù)據(jù)融 合。最終，得到實(shí)時(shí)水平角度的最優(yōu)估計(jì)值本發(fā)明采用陀螺儀測(cè)量的角速率預(yù)知角度的變 化，進(jìn)行動(dòng)態(tài)下的測(cè)量。由此來(lái)對(duì)動(dòng)態(tài)傾角測(cè)量進(jìn)行優(yōu)勢(shì)互補(bǔ)，保證了測(cè)量精度。
【附圖說(shuō)明】
[0030] 圖1為【具體實(shí)施方式】一的所述的基于卡爾曼濾波的加速度計(jì)陀螺儀水平角度測(cè) 量方法的流程示意圖。
【具體實(shí)施方式】
【具體實(shí)施方式】 [0031] 一：結(jié)合圖1說(shuō)明本實(shí)施方式，本實(shí)施方式所述的基于卡爾曼濾波 的加速度計(jì)陀螺儀水平角度測(cè)量方法，所述方法包括如下步驟：
[0032] 步驟一：利用加速度計(jì)和陀螺儀采集關(guān)于X軸的數(shù)據(jù)，獲得加速度和角速率；
[0033] 步驟二：對(duì)獲得的加速度和角速率進(jìn)行數(shù)字濾波，濾除在傳輸過(guò)程中產(chǎn)生的高頻 干擾；
[0034] 步驟三：將濾波后的加速度轉(zhuǎn)換成角度；
[0035] 步驟四：根據(jù)轉(zhuǎn)換的角度，代入溫度補(bǔ)償因素，得到補(bǔ)償后的角度；
[0036] 步驟五：將補(bǔ)償后的角度和濾波后的角速率進(jìn)行卡爾曼濾波，并進(jìn)行數(shù)據(jù)融合，獲 得水平角度的最優(yōu)估計(jì)值。
[0037] 所述步驟五的過(guò)程包括：
[0038] 步驟五一：設(shè)置參數(shù)的初值：
[0039] 矩陣.
> t表示當(dāng)前時(shí)刻，dt表示積分，矩陣
，過(guò)程噪聲的協(xié) 方差矩_
，qa表示加速度計(jì)的測(cè)量協(xié)方差，q g表示陀螺儀的測(cè)量協(xié)方差，R =
[rj，^表示測(cè)量誤差的協(xié)方差，水平角度估計(jì)值的協(xié)方差初值
水平角度 估計(jì)初值
[0040] 步驟五二：預(yù)測(cè)水平角度估計(jì)值協(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)，計(jì)算卡爾曼增益Kg (t):
[0044] Kg (t) = P(t|t-l)HT/[HP(t|t-l)HT+R]；
[0045] 觀測(cè)矩陣 H =[10];
[0046] 步驟五四：預(yù)測(cè)水平角度估計(jì)值X (t I t-Ι):
[0047] X (t |t-l) = AX(t-l |t-l)+BU(t)；
[0048] X(t 11-1)表示根據(jù)t-1時(shí)刻的預(yù)測(cè)t時(shí)刻的水平角度估計(jì)值，U(t)表示t時(shí)刻的 控制量；
[0049] 步驟五五：根據(jù)當(dāng)前時(shí)刻的角速率測(cè)量值Z(t)，結(jié)合X(t|t-1)和Kg(t)，獲得水平 角度的最優(yōu)估計(jì)值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):
[