方旭 張偉 郭子龍

1.北京信息科技大學傳感器重點實驗室，北京 100101；2.西安工業(yè)大學光電工程學院，陜西西安 710021

0 引言

隨著現(xiàn)代戰(zhàn)爭要求不斷提高，采用導彈對目標進行精準打擊成為目前軍備發(fā)展的迫切需求。其中，采用單通道控制的旋轉彈更是近程武器發(fā)展方向，可大量裝備部隊，實現(xiàn)范圍火力覆蓋[1-2]。區(qū)別于常規(guī)戰(zhàn)術導彈的三通道控制，旋轉彈彈體在自旋情況下具備低通濾波特性。因此，可通過單通道同時控制彈體的俯仰和偏航方向的運動，即通過滾轉角位置信息計算出舵機的張開時間與擺幅，實現(xiàn)對旋轉彈的彈道修正，從而精準打擊目標。這意味著要想實現(xiàn)對旋轉彈的精準制導，需要得到測量精度較高的滾轉角數(shù)據(jù)[3-4]。

本文采用的無驅動結構MEMS 陀螺儀，由于其依靠載體的旋轉驅動，無自旋耦合，載體自旋轉速越高，陀螺儀的性能越穩(wěn)定，精度越高，這一特點非常適合應用于旋轉彈的姿態(tài)測量。無驅動結構MEMS 陀螺儀輸出的信號為正弦信號，通過峰值點的判斷可以進行重新積分，避免了常規(guī)陀螺儀的累積誤差。無驅動MEMS 陀螺結構圖如圖1 所示。

1 濾波算法介紹

在濾波算法的選擇上，常用算法主要有算術平均濾波、加權平均值濾波[5]、滑動平均算法[6]、中位數(shù)平均濾波[7]、限幅濾波[8]、一階滯后濾波[9]，以及卡爾曼濾波[10]等改進算法。

由于系統(tǒng)的應用場景為高轉速的旋轉體滾轉角解算，電壓參數(shù)變化較快，中位數(shù)濾波和消抖類濾波并不適用；同時，要求解算的實時性要好，一階滯后濾波會造成相位滯后，影響系統(tǒng)的實時性，這里也不予考慮；陀螺儀的輸出信號為正弦周期信號，普通限幅濾波算法適用性較差。綜合應用場景以及解算算法從采樣數(shù)據(jù)處理到解算數(shù)據(jù)處理等各方面的需求，下面介紹幾種常用的濾波算法。

1.1 中位數(shù)平均算法

中位數(shù)平均算法需要先采樣N個值，并對N個值進行排序，去掉最大和最小的L個值，最終計算N-2L個值的平均值。在排序算法的選擇上，鑒于N值取值較小，且單片機內存較小，考慮到空間復雜度和算法的實現(xiàn)復雜度，一般選擇冒泡排序，對N個值進行排序。

1.2 滑動平均濾波

滑動平均濾波結合了算術平均濾波的優(yōu)勢，傳感器連續(xù)采樣N個數(shù)據(jù)放入緩存隊列中，并計算算術平均值，隊列長度固定為N，此后，每次通過傳感器采樣得到一個新的數(shù)據(jù)就會刪除隊首數(shù)據(jù)，并且把新采樣數(shù)據(jù)放入隊尾，再計算隊列的算術平均值。滑動平均濾波對于周期性干擾具有很好的抑制作用，但靈敏度低。

1.3 加權平均濾波

加權平均濾波是通過連續(xù)觀測N個采樣值，按照其出現(xiàn)的次數(shù)為權重，計算觀測值的加權算術平均數(shù)。給予新采樣值的權系數(shù)越大，靈敏度越高，信號平滑度越低。加權平均濾波適用于系統(tǒng)純滯后時間常數(shù)τ較大，采樣周期較短的過程；對于純滯后時間常數(shù)τ較小，采樣周期較長，變化緩慢的信號，不能迅速反應系統(tǒng)當前所受干擾的嚴重程度，濾波效果差。加權平均算法的公式為：

1.4 卡爾曼濾波

卡爾曼（Kalman）濾波是一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法。由于觀測數(shù)據(jù)包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計也可看作是濾波過程。

采用的卡爾曼濾波公式如下：

其中，Kk為卡爾曼增益；H 為測量矩陣；R為測量噪聲協(xié)方差。

其中，I 為單位矩陣。

2 滾轉角測量算法設計

滾轉角測量算法包括AD 采樣濾波算法、波峰波谷點判定算法、滾轉角速度和滾轉角解算算法。首先，對不同的濾波算法效果進行對比，設計出合適的AD采樣濾波算法；通過五點法對采樣得到的正弦數(shù)字信號的波峰波谷點進行判定；針對判定得到的波峰波谷點可以計算得到當前的滾轉角速度，對滾轉角速度進行積分，得到滾轉角位置信息。

2.1 AD 采樣濾波算法設計

AD 采樣后得到的陀螺和加速度計的數(shù)字電壓量會存在一些噪聲信號和異常值，需要通過合適的濾波算法對這些數(shù)字量進行處理，得到較為平滑的正弦波形，同時由于系統(tǒng)解算的實時性，濾波后的波形與實際波形相比，不能有較大的相位滯后與失真。對于后續(xù)算法解算的滾轉角速度同樣也需要進行濾波處理，使得解算的角速度收斂接近真實值。

對于模擬信號，經(jīng)過AD 采樣后得到的數(shù)字信號，數(shù)據(jù)處理速率要求很高，因此，需要結構簡單、高效的數(shù)字濾波器。由上文可知，對于AD 采樣可以選用加權平均濾波、滑動平均濾波以及中位數(shù)平均濾波。

通過測量系統(tǒng)的AD 采樣實驗后，分析數(shù)據(jù)可得，靜止狀態(tài)下，未采用濾波算法的采樣數(shù)據(jù)與實際零位電壓轉換數(shù)字量差值分布圖如圖2 所示，經(jīng)過計算后得到均方差為571.72，采樣值與實際值的平均誤差為520.30。

靜止狀態(tài)下，采用加權平均濾波算法的采樣數(shù)據(jù)與實際零位電壓轉換數(shù)字量差值分布圖如圖3 所示，經(jīng)過計算后所得均方差為403.04，與實際值的平均誤差為544.66。

運動狀態(tài)下，未采用濾波算法和采用加權平均濾波算法的波形對比圖如圖4 所示。

靜止狀態(tài)下，采用滑動平均濾波算法的采樣數(shù)據(jù)與實際零位電壓轉換數(shù)字量差值分布圖如圖5 所示，經(jīng)過計算后所得均方差為297.79，與實際值的平均誤差為430.76。

運動狀態(tài)下，未采用濾波算法和采用滑動平均濾波算法的波形對比圖如圖6 所示。

靜止狀態(tài)下，采用中位數(shù)平均濾波算法的采樣數(shù)據(jù)與實際零位電壓轉換數(shù)字量差值分布圖如圖7所示，經(jīng)過計算后所得均方差為349.20，與實際值的平均誤差為494.98。

運動狀態(tài)下，未采用濾波算法和采用中位數(shù)平均濾波算法的波形對比圖如圖8 所示。

由上文可知，采用加權平均濾波算法處理的采樣數(shù)據(jù)，在靜止狀態(tài)下，數(shù)據(jù)的均方差減小，但與實際零位電壓的誤差變大；在運動狀態(tài)下，噪聲點減少，同時也產(chǎn)生了新的噪聲點，但數(shù)據(jù)的曲線總體趨向是更為平滑。采用中位數(shù)平均濾波算法處理的采樣數(shù)據(jù)，在靜止狀態(tài)下，數(shù)據(jù)的均方差與平均誤差減??；在運動狀態(tài)下，噪聲點沒有明顯的減少，正弦曲線的平滑度也沒有得到顯著地提升。盡管中位數(shù)平均濾波在靜止狀態(tài)下數(shù)據(jù)的濾波效果較好，但系統(tǒng)主要用于測量旋轉體運動狀態(tài)下的滾轉角，且該濾波算法需要取多個值經(jīng)過排序處理后取平均值，需要消耗較多的運算時間，對于算法整體運算實時性有較大影響，所以本文不予考慮。采用滑動平均濾波算法處理的采樣數(shù)據(jù)，在靜止狀態(tài)下，有著顯著的提升，均方差與平均誤差最?。辉谶\動狀態(tài)下噪聲點明顯減少，正弦曲線也變得更加平滑；同時，在算法時間方面，滑動平均濾波僅需在算法末端對取值點進行滑動操作即可，不占用太多運算時間，整體運算速度得到提升。

通過上述分析對比可以看出，滑動平均濾波在靜止狀態(tài)下和運動狀態(tài)下表現(xiàn)良好。由于滾轉角測量系統(tǒng)的應用場景是高轉速工況下，滑動取值求平均值的方法盡管保證了正弦曲線的平滑度，但在高轉速工況下可能會導致數(shù)據(jù)有偏差，所以，本文綜合滑動平均濾波算法和加權平均濾波算法，對于這一采樣電壓值采用滑動加權平均算法，定義4 個過去點以及1 個當前點，在循環(huán)中進行滑動取值，根據(jù)每個點的權重取最終的加權平均值，保證當前點有較高的權重，距離當前點間隔越大，權重越低，從而避免測量值的失真。

靜止狀態(tài)下，采用滑動加權平均濾波算法的零位電壓圖如圖9 所示，經(jīng)過計算后，所得均方差約為0.0176，轉換數(shù)字量為280.77。

運動狀態(tài)下，未采用濾波算法和采用滑動加權平均濾波算法的波形對比局部放大圖如圖10 所示。

從上文計算數(shù)據(jù)以及波形圖可知，采用滑動加權平均濾波后，均方差減小，運動狀態(tài)下，異常峰值點被削去，曲線趨于平滑，但與模擬電壓的正弦曲線相比仍有出入，需要進一步改進濾波算法。

2.2 卡爾曼濾波算法設計

上文中，陀螺儀和加速度計采樣數(shù)據(jù)的濾波算法需要進行改進，選擇卡爾曼濾波算法對采樣數(shù)據(jù)進行二次濾波?？柭鼮V波算法能夠對數(shù)據(jù)進行實時的更新與處理。

本文采用的卡爾曼濾波算法根據(jù)公式（2）～（6）簡化變形。在更新加速度計和陀螺儀的數(shù)字電壓值時，設計了一個k值，并在循環(huán)中對k值進行累加。當k值小于等于4 時，卡爾曼濾波狀態(tài)值設為經(jīng)過滑動加權平均濾波后的數(shù)字電壓值。當k值大于4 時，第1 步，計算先驗估計協(xié)方差P，P為程序預設值加上設置的過程噪聲協(xié)方差；第2 步，計算卡爾曼增益，代入計算完成的P和設置的測量噪聲協(xié)方差；第3 步，計算濾波后最優(yōu)估計值，代入實際觀測值（采樣得到的數(shù)字電壓值）和預測值，得到殘差；第4 步，將最優(yōu)估計值賦給觀測值并代入后續(xù)參數(shù)計算中，最后更新P。

采用卡爾曼濾波算法時，需要預先設置幾個參數(shù)，并通過調整這幾個參數(shù)的值，改變系統(tǒng)收斂速度趨于正常，使得輸出的估計值更加接近真實值。先驗估計值，在k小于等于4 的時候，預設為每次循環(huán)采樣值轉換后的數(shù)字電壓值；先驗估計協(xié)方差P預設為與理想值同一數(shù)量級或較小的數(shù)，預設為0.1，來達到較快的初始收斂速度，當系統(tǒng)進入穩(wěn)態(tài)之后，P值會收斂成一個最小的估計方差矩陣，這個時候的卡爾曼增益也是最優(yōu)的。

參數(shù)Q值與R值對于濾波性能的影響：當R值一定時，Q值越大，觀測值在濾波值中反應的權重越大，越能表現(xiàn)觀測值的狀態(tài)，反之亦然；當Q值一定時，R值越大，觀測值在濾波值中反應的權重越小，對于模型預測值信任度越高，反之亦然；當R值不變時，Q值越小，系統(tǒng)越容易收斂，如果Q值過于小，則容易導致濾波值發(fā)散，反之亦然；Q值不變時，R值越小，系統(tǒng)收斂得越快，但是過小的話會導致系統(tǒng)產(chǎn)生振蕩，R值太大，卡爾曼濾波響應會變慢。

綜合考慮濾波器在隨機性和慣性等方面的表現(xiàn)，首先，確定一組參數(shù)——Q值和R值；隨后，將Q從小往大調整，將R從大往小調整；先固定一個值，再去調整另外一個值，看收斂速度與波形輸出；最后，選擇收斂速度與波形最佳的參數(shù)值，保證數(shù)字電壓值波形與實際電壓波形相位差盡量小，且波形趨于正弦波形。

靜止狀態(tài)下，增加卡爾曼濾波算法的零位電壓圖如圖11 所示，經(jīng)過計算后所得均方差約為0.0091，轉換數(shù)字量為145.77。

運動狀態(tài)下，未采用濾波算法與采用滑動加權平均濾波算法增加爾曼濾波算法的波形對比局部放大圖如圖12 所示。

通過上述分析，加入卡爾曼濾波算法后，均方差大幅減小，電壓曲線平滑，趨近于模擬電壓信號的正弦波形。因此，AD 采樣算法部分最終采用滑動加權平均濾波算法結合卡爾曼濾波算法，盡管這樣做增加了算法總體的時間，但是采樣得到數(shù)字信號的正弦波形更加平滑且規(guī)范，在經(jīng)過波峰波谷點判定時，減少了誤判的可能性，能夠提高算法的解算精度。

2.3 波峰波谷點判定

上文對采樣信號進行處理和濾波，得到了較為理想的正弦波形后，需要對信號波形進行波峰波谷點的判定，以此來解算旋轉體的滾轉角速度。對于信號波形可能出現(xiàn)的異常點，采用三點法判定可能會產(chǎn)生誤判，所以本文采用了五點法[11]判定波峰波谷點，減少了誤判的可能性。

濾波后，每個采樣點電壓值在五點循環(huán)中，每個周期都會得到電壓最大值和電壓最小值。采用五點法判定需要取5 個點，這5 個點剛好就是濾波算法中滑動加權平均算法中的5 個點。波峰到波谷最少需要獲取10 個點，所以當上文所設的k值大于10，開始進入判定階段，并且設置k值恒等于11，防止不斷自增后數(shù)字溢出。進入判定階段，波峰判定需要滿足五點判定，即波峰點大于左右點，且左右點分別大于各自左邊或右邊一點，排除異常點干擾，同時通過判定波峰點電壓值與周期內獲取的電壓值最小值差值的大小來避免異常周期波形的錯誤判定，波谷點的判定同樣如此。判定成功后，進入角速度解算階段。

2.4 滾轉角速度解算

確定無驅動結構MEMS 陀螺或加速度計去噪后的數(shù)字信號的波峰或波谷點，并記錄波峰或波谷所對應的時間值。通過陀螺儀或加速度計相鄰波峰或波谷點的時間信息，由于陀螺儀和加速度計輸出電壓頻率與載體自旋頻率相等，通過公式（7）和公式（8）求得滾轉角速度：

由于采用五點法判定波峰波谷值，設置五點進行滑動循環(huán)，每當判定次數(shù)大于4 次時，所判定的下一個點才有可能為波峰或波谷點。通過上文可知，需要知道波峰波谷點的上一個成功判定點是波峰還是波谷點，算法內采用了標記值對上一個有效點進行判定。

在加速過程中，旋轉體的轉速是在迅速提升的，因此算法設置有效點判定次數(shù)這一參數(shù)。參數(shù)小于預設值時，在不同的參數(shù)范圍內對解算的角速度乘以相應的倍數(shù)；參數(shù)大于預設值時，解算角速度與上一次有效角速度求算術平均值，同時對角速度進行卡爾曼濾波，避免初期加速時間短的情況下很快達到穩(wěn)定狀態(tài)而導致解算角速度無法回到正常值。

2.5 滾轉角解算

通過上文解算得到角速度，對角速度進行積分求取滾轉角，滾轉角位置求取通過公式（9）解算得到：

其中，φ為測量的滾轉角；φ0為上一個時間點的滾轉角；為過程中某一時間點的角速度；ti為過程中某一時間間隔。

常規(guī)MEMS 陀螺儀具有累積誤差，在積分過程中，會將前一刻漂移誤差代入到下一刻的角度計算中，形成累積誤差，在高轉速狀態(tài)下誤差累積較大，無驅動結構MEMS 陀螺儀如果一直不斷積分也會存在累積誤差。但是，陀螺儀輸出的是正弦波形，因此在波峰波谷點的判定環(huán)節(jié)，如果本次循環(huán)判定的點為波峰點或波谷點，此時，φ0則為預設好的波峰波谷點相對應的滾轉角位置，通過實驗轉臺輸出正弦信號波峰波谷點所對應的滾轉角度，可以得到這一預設的滾轉角位置信息。陀螺儀與加速度計輸出電壓對應的轉臺滾轉角波形圖如圖13 所示。

由圖13 可知，當陀螺儀電壓處于波峰時，轉臺滾轉角約為200°，加速度計電壓處于波峰時，轉臺滾轉角約為20°，設置這些初始值用于滾轉角的解算。當該點未判定為波峰波谷點時，滾轉角則在上一個時間點解算的滾轉角基礎上進行積分運算。

3 實驗測試

本次實驗的測試平臺為高精度三軸轉臺。圖14 為測量系統(tǒng)樣機，第1 層背面由加速度計以及穩(wěn)壓電源組成，正面由穩(wěn)壓電路以及2 排插針組成，集成化的陀螺儀通過插針嵌入到第1 層中，構成了系統(tǒng)的第1層電路；第2 層電路主要為信號抬升電路與反相器電路，通過第2 層可以將陀螺儀信號與加速度計信號對準，方便算法解算；第3 層電路主要為數(shù)字信號電路，包含模數(shù)轉換與STM32 單片機系統(tǒng)電路。測試時，將樣機固定在轉臺上，并對樣機進行供電，以及將樣機輸出的485 信號傳輸?shù)缴衔粰C進行通訊。

為了驗證算法測量結果是否滿足精度要求，設計并進行了如下的實驗，在PC 機控制的三軸高精度轉臺上進行：將該測量系統(tǒng)安裝在三軸轉臺的內環(huán)軸心上，通過PC 機接收系統(tǒng)輸出的滾轉角速度和滾轉角度，與PC 機獲取的三軸轉臺實時的姿態(tài)角度進行對比。通過對5～10 r/s 轉速進行測量，計算滾轉角速度和滾轉角的解算誤差，繪制了如圖15 所示的誤差圖。

從圖15 可以看出，滾轉角測量算法在5～10 r/s穩(wěn)定轉速下，滾轉角速度的測量誤差基本處于5°/s 以內，僅在5 r/s 轉速下誤差為5.4°/s，略高于5°/s；滾轉角解算誤差如圖可以看到，在穩(wěn)定轉速范圍內，滾轉角的解算誤差都沒有超過8°。因此，測試樣機在5～10 r/s 穩(wěn)定轉速下，滾轉角速度解算誤差基本在5°/s 以內，滾轉角測量誤差在10°以內。

4 結束語

針對旋轉彈滾轉角精確測量的需要，本文提出了一種基于無驅動MEMS 陀螺儀的測量算法，開展了在測試轉臺模擬情況下的實驗研究。穩(wěn)定狀態(tài)下，滾轉角速度測量誤差在5°/s，滾轉角測量誤差在10°以內，實現(xiàn)了載體穩(wěn)定轉速下旋轉體滾轉角的測量，為滾轉角測量提供了一個可行的研究方向。