卡爾曼濾波補(bǔ)償在潛載光電跟瞄中的應(yīng)用*

張雨恒，劉宗凱，周本謀，劉禹銘

（1.南京理工大學(xué)瞬態(tài)物理國家重點(diǎn)實(shí)驗(yàn)室，南京 210094；2.南京理工大學(xué)自動化學(xué)院，南京 210094）

0 引言

潛載光電設(shè)備在目標(biāo)的捕獲跟蹤階段，由于艏搖、縱搖、橫搖的擾動，跟蹤誤差會增大。只對視軸穩(wěn)定采取優(yōu)化，無法完全消除自身擾動的影響，甚至?xí)?dǎo)致跟蹤失敗。視軸優(yōu)化中，影響光電跟蹤系統(tǒng)的主要因素除了目標(biāo)的運(yùn)動及平移，還有目標(biāo)的旋轉(zhuǎn)運(yùn)動。文獻(xiàn)［1］通過對脫靶量的濾波預(yù)測進(jìn)行延時補(bǔ)償，提高光電設(shè)備的跟蹤精度。文獻(xiàn)［2］針對艦載光電設(shè)備的擾動進(jìn)行了建模，計(jì)算出船搖擾動的角速度作為前饋補(bǔ)償，減小了跟蹤誤差。文獻(xiàn)［3］使用改進(jìn)的Singer 模型的卡爾曼濾波，研究了一種對非線性狀態(tài)目標(biāo)更有效的跟蹤算法。Singer 模型相較于傳統(tǒng)的CA（勻加速）和CV（勻速度）機(jī)動模型，將目標(biāo)的機(jī)動認(rèn)作是時間相關(guān)的有色噪聲序列而非統(tǒng)計(jì)獨(dú)立的白噪聲序列作用的結(jié)果，對應(yīng)于目標(biāo)包含勻速和勻加速之間的運(yùn)動，較CA 和CV 模型有更大的目標(biāo)機(jī)動模式覆蓋范圍［4］。本文在潛艇與地面坐標(biāo)的轉(zhuǎn)換基礎(chǔ)下，對整個隨動系統(tǒng)中的粗跟蹤系統(tǒng)，應(yīng)用PID 控制以及通過Singer 模型的Kalman 濾波的方法得出前饋補(bǔ)償，并結(jié)合精跟蹤FSM 快速反應(yīng)鏡控制系統(tǒng)，完成整個光電設(shè)備的復(fù)合控制。

1 設(shè)計(jì)原理

1.1 坐標(biāo)轉(zhuǎn)換

潛艇姿態(tài)可以用潛艇甲板坐標(biāo)系相對于大地水平坐標(biāo)系的搖擺角度（艏搖角αH，縱搖角αP，橫搖角αR）來表征。目標(biāo)點(diǎn)在大地坐標(biāo)系中的方位角為A，俯仰角為E。由于CCD 采樣周期較小，單位時間內(nèi)潛艇平移造成的方位和俯仰角度的變化也較小，相對于艇搖運(yùn)動，可以忽略平移運(yùn)動的影響。下面引用文獻(xiàn)［4］對潛艇光軸坐標(biāo)的轉(zhuǎn)換。

僅在發(fā)生艏搖、縱搖以及橫搖擾動下，目標(biāo)點(diǎn)G（x，y，z）在大地坐標(biāo)系中的方位角和俯仰角可以通過αH、αP、αR逐一解算到下一時刻視軸坐標(biāo)系中，將大地坐標(biāo)系（X，Y，Z）下目標(biāo)點(diǎn)的坐標(biāo)轉(zhuǎn)換到潛艇坐標(biāo)系（Xd，Yd，Zd），需要經(jīng)過3 次坐標(biāo)變換：1）艇體繞OZ 軸以角速度ωH轉(zhuǎn)動αH（艏搖角）；2）艇體再繞OY 軸以角速度ωP旋轉(zhuǎn)αP角（縱搖角）；3）艇體再繞OX 軸以ωR旋轉(zhuǎn)αR（橫搖角），如圖1 所示。

這3 次搖擺姿勢解算可以寫成如下矩陣形式：

經(jīng)過3 次坐標(biāo)變換得出艇體坐標(biāo)為：

圖1 坐標(biāo)轉(zhuǎn)換示意圖

將潛艇光軸坐標(biāo)系下的直角坐標(biāo)點(diǎn)（Xd，Yd，Zd）轉(zhuǎn)換為方位和俯仰角（Ad，Ed）［5］：

式中，L 為斜距，即探測點(diǎn)與目標(biāo)之間的距離。聯(lián)立方程可得：

1.2 Singer 模型的Kalman 濾波前饋控制原理

Kalman 濾波的基本方程為其中，X（k）為n 維目標(biāo)狀態(tài)向量，Z（k）為n 維目標(biāo)量測向量，In為n 維單位矩陣，G（k），H（k）以及Φ（k）為常數(shù)矩陣用于表示狀態(tài)方程。

CV 機(jī)動模型假設(shè)運(yùn)動目標(biāo)為勻速直線運(yùn)動的狀態(tài)，即目標(biāo)的加速度值為0；CA 機(jī)動模型假設(shè)運(yùn)動目標(biāo)為勻加速直線運(yùn)動的狀態(tài)，即目標(biāo)的加速度的導(dǎo)數(shù)為0，但由于干擾的存在，CV 模型中目標(biāo)的加速度不能維持在零值，CA 模型中目標(biāo)加速度的導(dǎo)數(shù)不能維持在零值，因此，在這兩個模型下都采用均值為0 的高斯白噪聲表示噪聲的變化。

在跟蹤圓形運(yùn)動時，CA 模型考慮了目標(biāo)運(yùn)動的加速度成分，在拐彎時跟蹤結(jié)果明顯好于CV 模型。跟蹤蛇形運(yùn)動時，由于加速度的噪聲分布已經(jīng)不再滿足CA 模型的假設(shè)條件，所以在拐彎處的誤差較大。Singer 機(jī)動模型的噪聲則假設(shè)為有色噪聲，這一般更符合實(shí)際噪聲，所以在處理圓形以及蛇形機(jī)動時有更好的效果。

Singer 時間相關(guān)模型提出機(jī)動目標(biāo)加速度的時間相關(guān)函數(shù)為指數(shù)衰減形式［6］：

2 前饋控制系統(tǒng)的實(shí)現(xiàn)

2.1 粗跟蹤控制系統(tǒng)介紹

粗通道主要是由直流力矩電機(jī)和轉(zhuǎn)臺構(gòu)成，數(shù)學(xué)建模和仿真時，認(rèn)為方位和俯仰角傳遞函數(shù)一致。首先，對直流力矩電機(jī)進(jìn)行建模，這里將跟蹤轉(zhuǎn)臺作為負(fù)載，將其慣量折算到直流力矩電機(jī)的轉(zhuǎn)軸上，忽略電流的波動性，則電流為直流，直流力矩電機(jī)可近似為線性元件。由電機(jī)工作原理和電樞回路電壓平衡方程可以推導(dǎo)出直流力矩電機(jī)的傳遞函數(shù)，仿真框圖如圖2 所示。

圖2 力矩電機(jī)數(shù)學(xué)模型

Ua為電機(jī)的控制電壓輸入，Re、Le分別為電樞的電阻和電感，ia為電機(jī)的驅(qū)動電流，Km、Ke分別為轉(zhuǎn)矩靈敏度系數(shù)和反電動勢系數(shù)，Mf為擾動力矩，J為電樞轉(zhuǎn)動慣量。

整個控制系統(tǒng)控制回路的建模是力矩電機(jī)電壓和角速度之間的傳遞函數(shù)，需經(jīng)速度環(huán)和位置環(huán)的轉(zhuǎn)換。為了提高響應(yīng)速度，在速度環(huán)中采用PI 控制器，位置環(huán)中采用PID 控制器［7-8］。

通過Kalman 濾波的基本方程，Singer 運(yùn)動模型的狀態(tài)方程的離散化表示為

其中，T 為采樣周期，Q 為系統(tǒng)噪聲的方差矩陣［9］，ω（k）均值為0。

其中，

以方位角控制回路為例，隨動系統(tǒng)和Simlink仿真圖如圖3 所示。

圖3 粗跟蹤系統(tǒng)仿真圖

圖中PID 控制器所在環(huán)路為位置環(huán)，PI 控制器所在環(huán)路為速度環(huán)，Input1 輸入包含了脫靶量信息，F(xiàn)iltering 輸入的是濾波之后的擾動數(shù)據(jù)，下半部虛框部分是實(shí)驗(yàn)所選用電機(jī)的傳遞函數(shù)。粗跟蹤控制系統(tǒng)通過對輸入的船搖擾動數(shù)據(jù)和當(dāng)前方位角合成的擾動輸入進(jìn)行濾波，得到濾波之后的數(shù)據(jù)，再結(jié)合脫靶量信息反饋給控制系統(tǒng)，得到粗跟蹤系統(tǒng)的跟蹤誤差。

2.2 精跟蹤FSM 快速反應(yīng)鏡控制系統(tǒng)介紹

快速反應(yīng)鏡的跟蹤特性是當(dāng)粗跟蹤輸入的跟蹤誤差足夠小時，精跟蹤系統(tǒng)中的出光光路才能夠準(zhǔn)確。仿真研究采用的FSM 快速反應(yīng)鏡是德國PI公司的P-T04K010 壓電陶瓷偏擺鏡，快速反應(yīng)鏡的4 個運(yùn)動支點(diǎn)在搭載鏡片之后，可以實(shí)現(xiàn)俯仰和傾斜兩個自由度的微調(diào)。物理擺角幅度為±12.5 mrad，而P-T04K010 精度閉環(huán)精度為5 μrad。實(shí)驗(yàn)中通過頻譜儀測出的FSM 快反鏡系統(tǒng)閉環(huán)狀態(tài)下的頻響曲線，其閉環(huán)傳遞函數(shù)［10-11］：

仿真系統(tǒng)圖如圖4 所示。

圖4 精跟蹤系統(tǒng)仿真圖

快速反應(yīng)鏡仿真系統(tǒng)中的輸入Input2 為圖2粗跟蹤的誤差Error，Output2 為輸出。通過PID 控制器控制驅(qū)動元件的運(yùn)動來實(shí)現(xiàn)快速反射鏡的偏轉(zhuǎn)，憑借其質(zhì)量輕、響應(yīng)速度快的特點(diǎn)，可以及時修正跟蹤誤差，減小轉(zhuǎn)臺跟蹤殘差。

3 仿真分析

根據(jù)式（2）解算所得方位角以及俯仰角脫靶量輸出曲線如圖5 所示。

圖5 方位俯仰角脫靶量

仿真中，粗跟蹤通道的工作頻率為20 Hz，由于有相機(jī)圖像分辨率誤差、信號處理誤差等，通常將仿真采用的量測噪聲均方差設(shè)為4.85''，系統(tǒng)噪聲由式（14）與式（15）所得。

下頁圖6 中脫靶量的輸入為2 s，前幅值為0，2.05 s 后幅值為1 rad 的階躍信號，對Singer 模型以及CA 模型進(jìn)行了對比，階躍響應(yīng)仿真圖以及跟蹤誤差仿真結(jié)果見圖6。

可以看出，Singer 模型下的誤差結(jié)果相較于CA模型下的跟蹤誤差，擁有更快的響應(yīng)速度并且能夠更快地收斂，Singer 模型在大約2.9 s 時就已經(jīng)穩(wěn)定，而CA 模型則在大約3.2 s 時才穩(wěn)定。圖中穩(wěn)態(tài)誤差出現(xiàn)較大波動主要原因是量測噪聲，這樣的殘差可以由精跟蹤控制系統(tǒng)減小。

圖6 粗跟蹤控制系統(tǒng)階躍響應(yīng)

在圖7 的仿真結(jié)果中，輸入信號為圖5 中的方位角擾動量合成的脫靶量信息，前饋補(bǔ)償為對純擾動量濾波得到的速度補(bǔ)償。在數(shù)值仿真下對光電控制系統(tǒng)是否有速度前饋進(jìn)行了對比，在沒有速度前饋時，方位和俯仰的跟蹤誤差最大可達(dá)±0.004 5 rad；在加入濾波的速度前饋之后，方位跟蹤誤差最大約為±0.000 9 rad，俯仰跟蹤誤差最大約為±0.001 rad?？梢钥闯?，經(jīng)過卡爾曼濾波補(bǔ)償控制系統(tǒng)的跟蹤誤差明顯小于無補(bǔ)償?shù)目刂葡到y(tǒng)。

圖7 粗跟蹤誤差對比

4 結(jié)論

圖8 復(fù)合控制跟蹤輸出誤差

本文以潛載光電控制系統(tǒng)為研究對象，通過Matlab 仿真分析了艏搖、橫搖、縱搖3 種運(yùn)動對控制系統(tǒng)的影響。在潛艇坐標(biāo)轉(zhuǎn)換基礎(chǔ)上，對潛艇擾動進(jìn)行Singer 模型下的Kalman 濾波，將濾波預(yù)測到的速度值前饋到速度環(huán)，提高復(fù)合軸控制的跟蹤精度。仿真實(shí)驗(yàn)以方位角為例，表明擾動經(jīng)由坐標(biāo)變換以及粗精復(fù)合控制之后，能夠有效減少誤差，相較于CA 模型，Singer 模型擁有更快的響應(yīng)速度和收斂速度。粗通道方位角跟蹤誤差在未加入反饋時約為±0.004 5 rad，在加入反饋之后跟蹤誤差約為±0.000 9 rad，復(fù)合軸控制跟蹤誤差范圍減少至±0.000 2 rad。