基于Kalman濾波解調(diào)的半球諧振陀螺全角控制方案及仿真

楊小龍, 馬官營, 李建朋, 王琪偉

北京控制工程研究所, 北京100094

0 引 言

半球諧振陀螺(hemispherical resonator gyro, HRG)作為一種基于科氏振動效應(yīng)的新型固體振動陀螺,具有結(jié)構(gòu)簡單、高可靠、長壽命和獨(dú)特的抗輻射能力等特點(diǎn),其在陸?？仗斓阮I(lǐng)域的導(dǎo)航定位系統(tǒng)中得到廣泛應(yīng)用[1-3].

半球諧振陀螺的控制方式主要可分為力平衡模式和全角模式兩種[4-5].全角模式與目前主要使用的力平衡模式相比,最大的特點(diǎn)在于其速率積分特性.工作在全角模式下的半球諧振陀螺在外界載體發(fā)生轉(zhuǎn)動時,其駐波振型在外界輸入角速度的激勵下可以自由進(jìn)動.理想情況下,駐波振型的進(jìn)動角度θ與載體實(shí)際轉(zhuǎn)角φ之間滿足

θ=kφ

(1)

其中,k為角度增益因子(約為0.3).載體的實(shí)際轉(zhuǎn)角可根據(jù)半球諧振子駐波振型的進(jìn)動角得到.全角模式在實(shí)現(xiàn)控制時由于不施加反饋力,因而具有較寬的測量寬帶和較大的動態(tài)測量范圍[6].

半球諧振陀螺處于全角模式下,半球諧振子駐波振型不再被約束在固定位置,而是處于自由進(jìn)動狀態(tài),這使得全角模式控制中的信號處理和控制環(huán)路更為復(fù)雜[7-8].此外,半球諧振子的振型信號包含幅值、頻率和相位信息,是實(shí)現(xiàn)全角控制和角度計(jì)算的基礎(chǔ).因此實(shí)現(xiàn)振型信號的高精度數(shù)字解算成為全角控制的關(guān)鍵所在,其解算精度將直接影響全角控制效果和角度檢測精度.

LYNCH在文獻(xiàn)[9]中提出了帶有頻率裂解和阻尼不均勻等非理想因素的半球諧振陀螺動力學(xué)模型,并給出了基于乘法相干解調(diào)的信號解調(diào)方案,實(shí)現(xiàn)了半球諧振陀螺的全角模式控制.在此基礎(chǔ)上,后續(xù)學(xué)者進(jìn)行了針對半球諧振陀螺全角控制的研究.文獻(xiàn)[10]從理論上分析了全角模式的控制方法,使用乘法相干解調(diào)的方法提取諧振子的振型參數(shù),采用參數(shù)激勵的方法對陀螺進(jìn)行驅(qū)動和控制.文獻(xiàn)[11]針對PID控制下陀螺橢圓參數(shù)的控制精度和收斂快速性難以提升的問題,提出了將離散滑?？刂埔氲娇刂葡到y(tǒng)中的方法.文獻(xiàn)[12]針對一種金屬筒狀科氏振動陀螺,提出了全數(shù)字控制技術(shù)方案,其中采用Kalman濾波方法對振動信號進(jìn)行數(shù)字解算.

上述研究多集中于對控制環(huán)節(jié)進(jìn)行改進(jìn),而在信號解調(diào)方法上基本沿用乘法相干解調(diào)方法,缺少新的數(shù)字解調(diào)方法的研究.

本文針對半球諧振陀螺全角控制方案及數(shù)字解調(diào)方法開展研究.以非理想半球諧振子動力學(xué)模型為控制對象提出一種半球諧振陀螺的全角控制方案.針對振型信號的高精度數(shù)字解算問題,提出一種基于Kalman濾波的數(shù)字解調(diào)方法.在此基礎(chǔ)上搭建全角模式控制系統(tǒng)模型,對基于Kalman濾波解調(diào)的全角模式控制方案進(jìn)行仿真實(shí)現(xiàn),驗(yàn)證全角控制方案和數(shù)字解調(diào)方法的可行性.

1 半球諧振子動力學(xué)模型

半球諧振陀螺正常工作時,作為敏感結(jié)構(gòu)的半球諧振子處于四波幅振動狀態(tài),諧振子的振動振型可以沿著相距45°的2個電極軸(X軸、Y軸)進(jìn)行正交分解和合成[13].

1.1 理想半球諧振子動力學(xué)模型

全角控制下的半球諧振子在理想狀態(tài)下的振動特性可以用二維簡諧振動表示,如圖1所示.其中,a表示半球諧振子的主振型振幅;q表示諧振子的正交振型振幅,其在理想情況下趨于0;2θ表示主振型波腹軸與0°電極軸(X軸)的夾角;φ0表示駐波振型的初始相位;ω表示諧振子處于四波腹振動狀態(tài)下的諧振角頻率.

圖1 半球諧振子振動特性

上述變量都是規(guī)范變量,完整清晰地表征了半球諧振子駐波振型的特征.理想狀態(tài)下,半球諧振子的動力學(xué)模型可表示為[14]

(2)

式(2)中,x和y表示諧振子2個正交模態(tài)的振動位移,Ω是外界載體輸入角速度,fx、fy分別表示施加在0°和45°電極軸的作用力.據(jù)此,可以得到在X方向上和Y方向上的振動位移信號

(3)

1.2 非理想半球諧振子動力學(xué)模型

實(shí)際上,由于半球諧振子存在各向異性,因此理想的半球諧振子并不存在.在式(2)的基礎(chǔ)上,給出帶有頻率裂解和阻尼不均勻等非理想因素的半球諧振子動力學(xué)模型

(4)

式(4)中,C和K分別表示半球諧振子的阻尼矩陣和剛度矩陣,可表示為

(5)

式(5)中:ω2=(ω12+ω22)/2,ωΔω=(ω12-ω22)/2;ω1、ω2分別為兩主振型的諧振頻率,由于不理想的半球諧振子存在頻率裂解,因此兩主振型的諧振頻率不一致;2/τ=(1/τ1+1/τ2),Δ(1/τ)=(1/τ1-1/τ2);τ1、τ2為兩阻尼軸的時間衰減常數(shù);θω是頻率最小軸與駐波方位角的夾角,θτ是阻尼最小軸與駐波方位角的夾角.

2 半球諧振陀螺全角控制方案

由于全角模式控制下的半球諧振子駐波振型處于自由進(jìn)動的狀態(tài),因此全角控制的目的在于維持駐波振型在實(shí)時進(jìn)動過程中的穩(wěn)定并解算出實(shí)時進(jìn)動角度[15-16],需要完成以下控制:

1)維持主振型振幅a為一設(shè)定值a0.

2)促使正交振型振幅q到0.

3)使參考信號的頻率與相位φ同步跟蹤到諧振子的諧振頻率與初始相位φ0.

4)獲取振型進(jìn)動角θ.

根據(jù)以上要求,給出一種半球諧振陀螺全角模式控制方案,如圖2所示,其中主要包括:數(shù)字解調(diào)、參數(shù)解算、幅度控制環(huán)路、正交控制環(huán)路、頻相控制環(huán)路、角度解算、信號調(diào)制和驅(qū)動重構(gòu).

圖2 HRG全角控制方案示意圖

2.1 信號解調(diào)

在相距45°的2個電極軸方向上,通過電容/電壓轉(zhuǎn)換電路獲取半球諧振子振動時的駐波振型信號.由于得到的振型信號耦合了振動幅度、頻率和進(jìn)動角度等信息,需要通過解調(diào)方法對其進(jìn)行數(shù)字解調(diào).

采用鎖相環(huán)(PLL)作為外部參考信號發(fā)生器以跟蹤半球諧振子的振動,振型輸出信號與參考信號的同相部分和正交部分分別在X,Y方向上的分量為Cx、Sx、Cy、Sy,可表示為

(6)

上述同相分量和正交分量可以通過對駐波振型信號進(jìn)行解調(diào)獲得,是振型的直接檢測量.

2.2 參數(shù)解算

對獲取的解調(diào)結(jié)果進(jìn)行組合運(yùn)算,得到半球諧振陀螺諧振狀態(tài)參數(shù),如式(7)所示.根據(jù)理想的無阻尼二維簡諧振動運(yùn)動方程,還可以得到振型檢測量和振型規(guī)范量之間的關(guān)系

(7)

式(7)中,δφ=φ-φ0.

在此基礎(chǔ)上,得到半球諧振子駐波振型的規(guī)范變量

(8)

2.3 控制環(huán)路

(1)幅度控制環(huán)路

半球諧振陀螺在正常工作時需要通過施加持續(xù)不斷的激勵以維持主振型振幅不衰減.幅度控制環(huán)路采用參數(shù)a作為控制判斷量,并將a與振幅設(shè)定值a0進(jìn)行比較,得到的誤差信號作為PI控制器的控制信號.

(9)

(2)正交控制環(huán)路

正交控制環(huán)路用于抑制各向異性引起的諧振子正交誤差.正交控制環(huán)路采用參數(shù)q作為控制判斷量,通過PI控制器將其控制到0.

(10)

(3)頻相控制環(huán)路

頻相控制環(huán)路用于實(shí)時跟蹤諧振子的諧振頻率及初始相位,并利用鎖相環(huán)產(chǎn)生同頻同相的參考信號,實(shí)現(xiàn)半球諧振陀螺檢測信號解調(diào)和驅(qū)動信號的調(diào)制與重構(gòu).頻相控制環(huán)路采用參數(shù)L作為控制判斷量,通過PI控制器將其控制到0.

(11)

(4)角度解算

全角模式下,由S、R參數(shù)根據(jù)式(12)可直接計(jì)算出半球諧振子駐波振型的進(jìn)動角.

(12)

2.4 信號調(diào)制與驅(qū)動重構(gòu)

根據(jù)式(9)～(12)可得各控制環(huán)路的控制參數(shù)及振型進(jìn)動角,通過正交分解將控制作用分別分解到兩電極軸上,同時將該控制信號與諧振子諧振頻率進(jìn)行調(diào)制,得到重構(gòu)的驅(qū)動作用為

(13)

3 基于Kalman濾波的數(shù)字解調(diào)方法

從圖2可以看出,HRG全角控制方案的關(guān)鍵在于對駐波振型輸出信號進(jìn)行數(shù)字解調(diào)以獲得振型的直接檢測量.因此開展振型信號的數(shù)字解調(diào)方法研究具有重要意義.

3.1 Kalman濾波基本原理

設(shè)系統(tǒng)的離散狀態(tài)方程和量測方程可寫為[17]

(14)

式(14)中,x(k)為n維狀態(tài)向量,z(k)為m維量測向量,Ф為n×n狀態(tài)變換矩陣,Γ為n×l系統(tǒng)噪聲分配矩陣,H為m×n量測矩陣,w(k)是協(xié)方差為Q(k)的l維系統(tǒng)噪聲,v(k)是協(xié)方差為R(k)的m維量測噪聲,兩者均為零均值的不相關(guān)高斯白噪聲,且它們之間互不相關(guān),其統(tǒng)計(jì)特性滿足[18]

(15)

Kalman濾波主要包括預(yù)測和校正2個步驟[19]:

(1)預(yù)測:根據(jù)當(dāng)前時刻的系統(tǒng)狀態(tài)推算下一時刻的狀態(tài)變量和協(xié)方差矩陣.

①狀態(tài)預(yù)測

(16)

②誤差協(xié)方差矩陣預(yù)測

P(k|k-1)=Φ(k,k-1)P(k-1|k-1)

ΦT(k,k-1)+Γ(k,k-1)Q(k-1)ΓT(k,k-1)

(17)

(2)校正:結(jié)合觀測的結(jié)果與預(yù)測推算的結(jié)果獲得系統(tǒng)的最優(yōu)估計(jì).

①計(jì)算Kalman增益

K(k)=P(k|k-1)HT(k)·

[H(k)P(k|k-1)HT(k)+R(k)]-1

(18)

②狀態(tài)校正

(19)

③狀態(tài)誤差協(xié)方差更新

P(k|k)=[I-K(k)H(k)]P(k|k-1)·

[I-K(k)H(k)]T+K(k)R(k)KT(k)

(20)

3.2 基于Kalman濾波的數(shù)字解調(diào)方法

根據(jù)第2節(jié)內(nèi)容,半球諧振陀螺的振型輸出信號為

g(t)=Ccos(ωt)+Ssin(ωt)+n(t)

(21)

其中,g(t)為HRG的振型輸出信號,C為HRG振型輸出信號中同相分量的幅值,S為HRG振型輸出信號中正交分量的幅值,n(t)為控制系統(tǒng)的噪聲項(xiàng),ω為HRG處于工作模態(tài)下的諧振角頻率.

數(shù)字解調(diào)的目的是將HRG振型輸出信號中的同相分量C和正交分量S提取出來用于后續(xù)控制系統(tǒng)進(jìn)行參數(shù)解算.Kalman濾波解調(diào)的核心在于根據(jù)系統(tǒng)模型建立HRG控制系統(tǒng)的狀態(tài)空間方程.根據(jù)文獻(xiàn)[13]的分析,在實(shí)際應(yīng)用中,外界輸入角速度信號的變化頻率遠(yuǎn)低于HRG的驅(qū)動頻率,因而可以假設(shè)在2個相鄰的時間間隔內(nèi),外界輸入角速度基本保持不變,此時振型輸出信號的變化僅靠系統(tǒng)的噪聲項(xiàng)驅(qū)動[13].

根據(jù)前述假設(shè),可以建立HRG控制系統(tǒng)的離散狀態(tài)空間方程為

(22)

式(22)中,

其中:x(k)為HRG振型輸出信號的同相分量幅值和正交分量幅值C和S;z(k)為系統(tǒng)的量測向量;H(k)為系統(tǒng)的解調(diào)參考信號;w(k)為系統(tǒng)狀態(tài)方程的白噪聲,在本系統(tǒng)中表征角速度的細(xì)微變化;v(k)表示系統(tǒng)觀測的白噪聲.

根據(jù)前述Kalman濾波的基本原理,可分別解算出HRG振型輸出信號的C和S.解調(diào)公式如下:

(23)

式(23)中:Q表示系統(tǒng)激勵噪聲的協(xié)方差矩陣,是由于外部干擾產(chǎn)生;R表示觀測噪聲的協(xié)方差矩陣.在本文中,Q、R的選取是綜合控制系統(tǒng)的輸入角速度、帶寬、解算精度的結(jié)果.

4 仿真驗(yàn)證及分析

為驗(yàn)證本文提出的數(shù)字解調(diào)方法及全角控制方案的有效性,根據(jù)非理想半球諧振子動力學(xué)模型和全角模式控制方案,搭建HRG全角控制系統(tǒng)仿真模型[20],如圖3所示,包括:非理想半球諧振子動力學(xué)模型、數(shù)字解調(diào)模塊、組合計(jì)算模塊、控制回路模塊、信號調(diào)制與驅(qū)動重構(gòu)模塊.

圖3 HRG全角控制系統(tǒng)仿真模型

表1列出了用于驗(yàn)證所提出的Kalman濾波解調(diào)方法和全角控制方案的仿真參數(shù).

表1 仿真參數(shù)設(shè)置

根據(jù)全角控制方案,當(dāng)控制系統(tǒng)穩(wěn)定時,將諧振子主振型振幅a穩(wěn)定控制在振幅設(shè)定值a0附近,正交振型振幅q則抑制到趨近于0.結(jié)合式(3)及式(7)可知,控制系統(tǒng)達(dá)到穩(wěn)態(tài)時,在無外界角速度輸入(即Ω=0)的情況下,振型信號同相分量幅值Cx、Cy理論上為一常值,正交分量幅值Sx、Sy理論上為0;當(dāng)有外界角速度輸入時,振型信號同相分量幅值Cx、Cy則表現(xiàn)出與外界輸入角速度有關(guān)的正弦規(guī)律變化,正交分量幅值Sx、Sy仍為0.

仿真時以常用的角速度為參考,給定外界輸入角速度Ω,設(shè)定在5 s時由0階躍為5(°)/s.

各控制環(huán)路控制參數(shù)變化如圖4所示,各控制參數(shù)均通過歸一化處理.由圖4可知,控制系統(tǒng)通過幅度、正交、頻相3個環(huán)路的控制,達(dá)到穩(wěn)態(tài).諧振子主振型振幅a穩(wěn)定控制在振幅設(shè)定值a0附近;正交振型振幅q抑制到趨近于0;頻相控制環(huán)路控制判斷量L控制在0附近.仿真結(jié)果表明控制系統(tǒng)實(shí)現(xiàn)控制目標(biāo).

圖4 各環(huán)路控制效果

圖5給出了基于Kalman濾波的數(shù)字解調(diào)結(jié)果.控制系統(tǒng)經(jīng)過控制后達(dá)到穩(wěn)態(tài),在無外界角速度輸入時(即Ω=0),Cx、Cy保持在a0附近,而Sx、Sy則被抑制到0.在5s時,采用階躍信號輸入角速度,解調(diào)出的Cx、Cy呈正弦規(guī)律變化,且幅值為a0;Sx、Sy始終保持為0不變.仿真結(jié)果與前述理論分析一致,驗(yàn)證了該解調(diào)方法的正確性.

圖5 數(shù)字解調(diào)結(jié)果

圖6(a)給出了控制系統(tǒng)在5(°)/s外界輸入角速度下的角度解算響應(yīng)曲線.對非理想半球諧振子,其駐波進(jìn)動角度θ與外界輸入角速度Ω之間滿足

(24)

當(dāng)控制系統(tǒng)穩(wěn)定時,根據(jù)圖6(a)計(jì)算出角度增益因子k=0.3,與給定的仿真參數(shù)一致.

將控制系統(tǒng)的輸出由角度解算轉(zhuǎn)換為角速度解算,其響應(yīng)曲線如圖6(b)所示.仿真結(jié)果顯示,控制系統(tǒng)角速度解算的時間小于0.1 s.

圖6 解算結(jié)果

圖7給出了控制系統(tǒng)在0.1(°)/s、1(°)/s、10(°)/s,100(°)/s外界輸入角速度下的系統(tǒng)角速度解算響應(yīng)曲線.可以發(fā)現(xiàn),在外界輸入角速度大范圍變化時,控制系統(tǒng)都能較快且穩(wěn)定解算出外界輸入角速度.

圖7 不同外界輸入角速度下的角速度解算結(jié)果

圖8給出了控制系統(tǒng)的輸入—輸出曲線,將解算出的系統(tǒng)解算角速度與對應(yīng)的外界輸入角速度曲線進(jìn)行線性擬合,得到標(biāo)度因數(shù)為1.000 0.

圖8 外界輸入角速度與系統(tǒng)解算角速度關(guān)系圖

上述仿真結(jié)果和分析驗(yàn)證了本文提出的基于Kalman濾波的數(shù)字解調(diào)方法和全角控制方案是有效的,具有可行性.

5 結(jié) 論

本文對半球諧振陀螺全角控制方案及數(shù)字解調(diào)方法開展研究,以非理想半球諧振子動力學(xué)模型為控制對象,給出一種半球諧振陀螺全角控制方案.在控制系統(tǒng)中提出了一種基于Kalman濾波的數(shù)字解調(diào)方法,實(shí)現(xiàn)對振型信號的數(shù)字解調(diào).仿真結(jié)果表明,基于Kalman濾波的數(shù)字解調(diào)方法能實(shí)現(xiàn)對振型信號的快速準(zhǔn)確解調(diào),各控制環(huán)路在較大外界輸入角速度范圍下都能實(shí)現(xiàn)穩(wěn)定控制及角度解算.從而驗(yàn)證了本文基于Kalman濾波解調(diào)的半球諧振陀螺全角控制方案的可行性.