一種基于卡爾曼濾波的船載伺服系統(tǒng)隨機(jī)誤差處理方法

陶 敏，丁求啟，張桂平

（中國(guó)衛(wèi)星海上測(cè)控部 江蘇 江陰 214431）

伺服系統(tǒng)是測(cè)角系統(tǒng)的重要組成部分，它接收來(lái)自接收機(jī)的誤差信號(hào)，經(jīng)數(shù)字處理、放大最后饋給執(zhí)行元件（伺服電機(jī)）驅(qū)動(dòng)天線移向減少誤差的方向，從而完成對(duì)目標(biāo)的跟蹤。隨著科學(xué)技術(shù)的發(fā)展，特別是計(jì)算機(jī)技術(shù)的發(fā)展和應(yīng)用，航天測(cè)量船伺服系統(tǒng)已發(fā)展成為一個(gè)集目標(biāo)搜索、捕獲、跟蹤、信號(hào)處理等各種功能為一體的自動(dòng)化計(jì)算機(jī)控制系統(tǒng)[1]。信號(hào)在傳輸與檢測(cè)過(guò)程中不可避免地受到外來(lái)干擾與設(shè)備內(nèi)部噪聲的影響，導(dǎo)致接收端收到的信號(hào)含有隨機(jī)誤差。1960年由R.E.Kalman先生首次提出的卡爾曼濾波是一種線性最小方差估計(jì)，相對(duì)于以前的幾種估計(jì)方法（如最小二乘法、最小方差估計(jì)、極大驗(yàn)后估計(jì)、貝葉斯估計(jì)、極大似然估計(jì)），卡爾曼估計(jì)從與被提取信號(hào)有關(guān)的測(cè)量值中通過(guò)算法估計(jì)出所需信號(hào)，它實(shí)際是對(duì)隨時(shí)間改變參數(shù)估計(jì)的一種順序最小二乘逼近[2]?？紤]一個(gè)隨時(shí)間變化的參數(shù)變量（狀態(tài)矢量），并通過(guò)一個(gè)線性模型（系統(tǒng)模型），卡爾曼濾波就可以提供在任何時(shí)刻對(duì)狀態(tài)矢量進(jìn)行估計(jì)的一套算法[3]。

本設(shè)計(jì)基于Singer模型，針對(duì)航天測(cè)量船的伺服控制系統(tǒng)，建立了一整套的卡爾曼濾波算法，以達(dá)到減小隨機(jī)誤差，提高測(cè)量精度的目的。

1 航天測(cè)量船伺服控制系統(tǒng)

航天測(cè)量船伺服系統(tǒng)在控制結(jié)構(gòu)上采用典型的位置環(huán)、速度環(huán)、電流環(huán)三環(huán)結(jié)構(gòu)，以改善機(jī)電結(jié)合系統(tǒng)的性能。設(shè)計(jì)電流環(huán)有利于改善電機(jī)的動(dòng)態(tài)特性，克服力矩控制死區(qū)和非線性，設(shè)計(jì)速度環(huán)以提高系統(tǒng)的抗負(fù)載擾動(dòng)的能力，位置環(huán)是保證跟蹤性能的外環(huán)，用以提高傳動(dòng)系統(tǒng)的動(dòng)態(tài)性能。

在控制方案上仍采用傳統(tǒng)的經(jīng)典控制理論設(shè)計(jì)，即陀螺穩(wěn)定環(huán)、自跟蹤環(huán)、數(shù)引環(huán)結(jié)構(gòu)。各個(gè)環(huán)的信號(hào)處理仍將是基于偏差控制，并采用頻率域的方法進(jìn)行設(shè)計(jì)校正。ACU控制環(huán)路方框圖如圖1所示。

在自跟蹤方式下，跟蹤接收機(jī)把解算出的角誤差加入位置回路。位置回路對(duì)這個(gè)誤差信號(hào)進(jìn)行校正、處理后，送給速度環(huán)和電流環(huán)，驅(qū)動(dòng)天線向較少誤差的方向運(yùn)動(dòng)，完成自跟蹤閉環(huán)。由此可以看出，角誤差電壓的精度是影響測(cè)量船測(cè)控精度的一個(gè)關(guān)鍵因素。

圖2給出了測(cè)量船伺服系統(tǒng)某次在跟蹤信標(biāo)球時(shí)的角誤差電壓：

采用變量差分法，對(duì)方位角誤差電壓以及俯仰角誤差電壓進(jìn)行隨機(jī)誤差的計(jì)算。

圖3分別給出了方位、俯仰角誤差電壓的3次差分值，從圖中可以明顯的看出方位、俯仰誤差電壓存在隨機(jī)誤差，經(jīng)過(guò)計(jì)算可以得到方位、俯仰角誤差電壓的隨機(jī)誤差為：

圖1 ACU控制環(huán)路方框圖Fig.1 Structure diagram of the ACU control loop

圖2 方位、俯仰角誤差電壓Fig.2 Angle error vo1tage of azimuth and elevation

在自跟蹤條件下，跟蹤接收機(jī)解算出的方位、俯仰角誤差電壓值基本上是在10-2水平上，因此隨機(jī)誤差對(duì)角誤差電壓的影響較大，從而影響到伺服系統(tǒng)的跟蹤性能以及跟蹤精度。

本設(shè)計(jì)中，采用隨機(jī)加速度的目標(biāo)模型和卡爾曼濾波算法，以提高估計(jì)精度和對(duì)機(jī)動(dòng)目標(biāo)的自適應(yīng)能力；通過(guò)線性化解耦濾波和穩(wěn)態(tài)增益解析計(jì)算等措施，大大減少了實(shí)時(shí)計(jì)算工作量，提高了濾波器實(shí)時(shí)計(jì)算能力。

2 卡爾曼濾波算法的設(shè)計(jì)

圖3 方位、俯仰角誤差電壓的三次差分Fig.3 Three times difference of the azimuth and elevation angle error vo1tage

由于卡爾曼濾波器是建立在狀態(tài)方程和測(cè)量方程基礎(chǔ)上的，所以建立狀態(tài)方程和測(cè)量方程是使用卡爾曼濾波器的關(guān)鍵步驟[4]。系統(tǒng)狀態(tài)變量及測(cè)量變量選擇的結(jié)果，能夠直接影響到濾波器濾波的效果。在選擇狀態(tài)變量的時(shí)候，除了要充分考慮到所選擇的狀態(tài)變量能否正確反映系統(tǒng)的特征，還要考慮到狀態(tài)變量使用的可行性以及所選擇的狀態(tài)變量能否和測(cè)量變量關(guān)聯(lián)起來(lái)，即測(cè)量變量能否用狀態(tài)變量來(lái)表示。

2.1 卡爾曼濾波算法狀態(tài)方程的建立

本設(shè)計(jì)采用Singer模型[5]，它的基本思想是：假定目標(biāo)非機(jī)動(dòng)時(shí)，作勻速直線運(yùn)動(dòng)，目標(biāo)轉(zhuǎn)彎、加速及大氣湍流等擾動(dòng)引起的加速度變化，看成是勻速直線運(yùn)動(dòng)軌道上的擾動(dòng)，這一擾動(dòng)過(guò)程用 a（t）表示，a（t）在時(shí)間上是相關(guān)的，通常假定該擾動(dòng)過(guò)程在空間方位、俯仰兩個(gè)坐標(biāo)軸的方向近似彼此獨(dú)立，現(xiàn)只研究方位通道，俯仰通道可類推。

方位通道上的狀態(tài)方程：

根據(jù)航天測(cè)量船伺服系統(tǒng)的特點(diǎn)，狀態(tài)變量x1表示方位角誤差，x2表示方位角誤差加速度，ax（t）可根據(jù)其相關(guān)統(tǒng)計(jì)特性，由白噪聲驅(qū)動(dòng)的線性系統(tǒng)的輸出求得，典型的隨機(jī)加速度相關(guān)函數(shù)為：

對(duì)式（2）進(jìn)行譜分析可得加速度的時(shí)域表達(dá)式：

式中ω（t）為白噪聲，其相關(guān)函數(shù)為：

將式（3）增廣至（1）中，得狀態(tài)方程：

將式（5）離散化，當(dāng) τm>>Δt時(shí)，簡(jiǎn)記為：

式中Δt為步長(zhǎng)，

ω（k）為高斯白噪聲序列，其方差 Q=E[ω（k）2]=2/τm

由上述可知，τm和σm是描述目標(biāo)機(jī)動(dòng)的兩個(gè)重要參數(shù)。

理論分析和仿真表明當(dāng)τm→0時(shí)，目標(biāo)可近似看作勻速運(yùn)動(dòng)模型；而τm→∞當(dāng)時(shí)，目標(biāo)可近似看作勻加速運(yùn)動(dòng)模型。

σm的大小主要反映目標(biāo)機(jī)動(dòng)強(qiáng)度，可見Singer模型實(shí)質(zhì)是一個(gè)介于勻速和勻加速運(yùn)動(dòng)模型之間的模型，而且與目標(biāo)機(jī)動(dòng)情況相聯(lián)系，用它作為伺服系統(tǒng)中的目標(biāo)運(yùn)動(dòng)模型，方便、靈活。

2.2 濾波器的測(cè)量方程

在伺服系統(tǒng)中，一般直接使用測(cè)得的目標(biāo)球形極坐標(biāo)值即方位角度、俯仰角度以及距離值。此時(shí)，測(cè)量方程的觀測(cè)誤差是彼此耦合的，導(dǎo)致測(cè)量方程彼此耦合。盡管濾波器的狀態(tài)方程根據(jù)假定彼此獨(dú)立，濾波器仍要設(shè)計(jì)為9態(tài)濾波器，其計(jì)算量是非常大的。

根據(jù)有關(guān)資料分析表明，在實(shí)際應(yīng)用中，可近似假定直角坐標(biāo)系測(cè)量誤差互不相關(guān)，則就可以將濾波器設(shè)計(jì)成二個(gè)三態(tài)濾波器，這將使濾波計(jì)算量顯著減少，而濾波性能無(wú)明顯降低[6]。在本設(shè)計(jì)中，只對(duì)這種線性化解耦后的濾波算法進(jìn)行研究。

此外，測(cè)量噪聲嚴(yán)格地說(shuō)明這是一個(gè)非平穩(wěn)隨機(jī)過(guò)程，需實(shí)時(shí)計(jì)算其方差。為了進(jìn)一步簡(jiǎn)化計(jì)算，假定各通道的測(cè)量噪聲方差σ2為常量，其值可根據(jù)實(shí)踐經(jīng)驗(yàn)或具體傳感器誤差的統(tǒng)計(jì)特性適當(dāng)選擇。

仍以方位通道為例，則測(cè)量方程可寫成：

式中 H=（1，0，0）為觀測(cè)陣，v（k）為高斯白噪聲隨機(jī)序列，均值為 0，方差為 E[v（k）2]=σ2=R，x（k）為狀態(tài)向量，x1（k）為方位角位置，x2（k）為方位角速度，x3（k）為方位角加速度。

2.3 濾波方程

可得濾波方程如下：

狀態(tài)預(yù)測(cè)為

狀態(tài)濾波為

增益為

預(yù)測(cè)誤差為

濾波誤差為

對(duì)于我們的系統(tǒng)有滿秩可控陣為 Wc=[Γ φΓ φ2Γ]， 滿秩可觀陣為 Wo=[HTφTHTφ2HT]。

可見系統(tǒng)是完全可控和完全可觀測(cè)的，由卡爾曼濾波器的穩(wěn)定性理論可知，不論如何選取初值，當(dāng)時(shí)間充分長(zhǎng)后，其濾波誤差陣將趨于一個(gè)唯一的正定陣，它的增益矩陣也將趨于一個(gè)唯一的確定陣。這樣，可在較寬范圍內(nèi)選擇x?（0/0）和p（0/0），免去對(duì)增益的遞推計(jì)算，大大減少了濾波計(jì)算量。

3 實(shí)驗(yàn)結(jié)果

由測(cè)量船伺服控制系統(tǒng)的控制環(huán)路圖可知在自跟蹤方式下，伺服系統(tǒng)是根據(jù)角誤差電壓來(lái)控制天線向減小誤差電壓的方向運(yùn)動(dòng)的。所以，角誤差電壓的精度直接影響到伺服系統(tǒng)跟蹤目標(biāo)的精度。

采用本文設(shè)計(jì)的卡爾曼濾波器對(duì)方位、俯仰角誤差電壓分別進(jìn)行濾波處理，處理后的方位、俯仰角誤差電壓如圖4、5所示。

對(duì)濾波處理后的數(shù)據(jù)進(jìn)行變量差分計(jì)算隨機(jī)誤差得到：

從方位、俯仰角誤差電壓濾波前后對(duì)比圖中可以看出，濾波后角誤差電壓中突變的數(shù)據(jù)大量減少，數(shù)據(jù)變化更加連續(xù)，這也更加符合實(shí)際跟蹤過(guò)程；對(duì)比濾波前后的隨機(jī)誤差大小，本文設(shè)計(jì)的卡爾曼濾波器能夠較大程度的減小夾雜在方位、俯仰角誤差電壓中的隨機(jī)誤差，提高了誤差電壓的精度。

4 結(jié) 論

圖4 濾波前、后方位角誤差電壓Fig.4 Azimuth angle error vo1tage between using and not using filter

圖5 濾波前、后俯仰角誤差電壓Fig.5 Elevation angle error vo1tage between using and not using filter

圖6 濾波后方位、俯仰角誤差電壓的三次差分Fig.6 Three times difference of the azimuth and elevation angle error vo1tage after using filter

本文通過(guò)對(duì)航天測(cè)量船伺服控制系統(tǒng)控制結(jié)構(gòu)的分析，基于著名的Singer模型，建立了針對(duì)航天測(cè)量船伺服系統(tǒng)的卡爾曼濾波算法，并通過(guò)計(jì)算機(jī)進(jìn)行了數(shù)字仿真研究。由于兩個(gè)通道具有相同的數(shù)學(xué)模型，它們的濾波特性相同，本文只對(duì)方位通道進(jìn)行了詳細(xì)的卡爾曼濾波器建模。采用了測(cè)量船伺服系統(tǒng)某次跟蹤信標(biāo)球的記盤數(shù)據(jù)作為仿真數(shù)據(jù)，分別對(duì)方位角誤差電壓以及俯仰角誤差電壓進(jìn)行了濾波處理和分析。從仿真結(jié)果分析可以看出，采用卡爾曼濾波器后，能夠較大程度的減小隨機(jī)誤差，提高了伺服控制系統(tǒng)控制量的精度，從而達(dá)到了預(yù)期的效果。

[1]唐華.雷達(dá)伺服自適應(yīng)濾波器設(shè)計(jì)及仿真研究[J].電訊技術(shù)，1994，34（1）：35-47.TANG Hua.Research on adaptive filter design and simulation of radar servo system[J].Telecommunication Engineering，1994，34（1）:35-47.

[2]付夢(mèng)印，鄧志紅，張繼偉.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].2版.北京：科學(xué)出版社，2010.

[3]黃波，鄭新興，劉鳳偉.一種卡爾曼濾波自適應(yīng)算法[J].大眾科技，2012，14（151）：23-25.HUANG Bo，ZHENG Xin-xing，LIU Feng-wei.A adaptive algorithm on kalman filtering[J].媒體外文缺失，2012，14（151）:23-25.

[4]周琳娜.卡爾曼濾波在目標(biāo)跟蹤中的應(yīng)用[J].伺服控制，2010，1（1）：10-15.ZHOU Lin-na.Application of kalman filtering for target tracking[J].Servo Control，2010，1（1）:10-15.

[5]周紅波.一種新的改進(jìn)“當(dāng)前”統(tǒng)計(jì)模型[J].船艦電子工程，2007（1）：26-32.ZHOU Hong-bo.A new improved “current” statistical model[J].Ship Electronic Engineering，2007（1）:26-32.

[6]劉靜，姜恒，石曉原.卡爾曼濾波在目標(biāo)跟蹤中的研究與應(yīng)用[J].信息技術(shù)，2011（11）：174-479.LIU Jing，JIANG Heng，DAN Xiao-yuan.Study and application ofkalman filtering for targettracking[J].Information Technology，2011（11）:174-479.