卡爾曼濾波補償在潛載光電跟瞄中的應用*

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

（1.南京理工大學瞬態物理國家重點實驗室，南京 210094；2.南京理工大學自動化學院，南京 210094）

0 引言

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

1 設計原理

1.1 坐標轉換

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

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

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

經過3 次坐標變換得出艇體坐標為：

圖1 坐標轉換示意圖

將潛艇光軸坐標系下的直角坐標點（Xd，Yd，Zd）轉換為方位和俯仰角（Ad，Ed）［5］：

式中，L 為斜距，即探測點與目標之間的距離。聯立方程可得：

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

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

CV 機動模型假設運動目標為勻速直線運動的狀態，即目標的加速度值為0；CA 機動模型假設運動目標為勻加速直線運動的狀態，即目標的加速度的導數為0，但由于干擾的存在，CV 模型中目標的加速度不能維持在零值，CA 模型中目標加速度的導數不能維持在零值，因此，在這兩個模型下都采用均值為0 的高斯白噪聲表示噪聲的變化。

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

Singer 時間相關模型提出機動目標加速度的時間相關函數為指數衰減形式［6］：

2 前饋控制系統的實現

2.1 粗跟蹤控制系統介紹

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

圖2 力矩電機數學模型

Ua為電機的控制電壓輸入，Re、Le分別為電樞的電阻和電感，ia為電機的驅動電流，Km、Ke分別為轉矩靈敏度系數和反電動勢系數，Mf為擾動力矩，J為電樞轉動慣量。

整個控制系統控制回路的建模是力矩電機電壓和角速度之間的傳遞函數，需經速度環和位置環的轉換。為了提高響應速度，在速度環中采用PI 控制器，位置環中采用PID 控制器［7-8］。

通過Kalman 濾波的基本方程，Singer 運動模型的狀態方程的離散化表示為

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

其中，

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

圖3 粗跟蹤系統仿真圖

圖中PID 控制器所在環路為位置環，PI 控制器所在環路為速度環，Input1 輸入包含了脫靶量信息，Filtering 輸入的是濾波之后的擾動數據，下半部虛框部分是實驗所選用電機的傳遞函數。粗跟蹤控制系統通過對輸入的船搖擾動數據和當前方位角合成的擾動輸入進行濾波，得到濾波之后的數據，再結合脫靶量信息反饋給控制系統，得到粗跟蹤系統的跟蹤誤差。

2.2 精跟蹤FSM 快速反應鏡控制系統介紹

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

仿真系統圖如圖4 所示。

圖4 精跟蹤系統仿真圖

快速反應鏡仿真系統中的輸入Input2 為圖2粗跟蹤的誤差Error，Output2 為輸出。通過PID 控制器控制驅動元件的運動來實現快速反射鏡的偏轉，憑借其質量輕、響應速度快的特點，可以及時修正跟蹤誤差，減小轉臺跟蹤殘差。

3 仿真分析

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

圖5 方位俯仰角脫靶量

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

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

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

圖6 粗跟蹤控制系統階躍響應

在圖7 的仿真結果中，輸入信號為圖5 中的方位角擾動量合成的脫靶量信息，前饋補償為對純擾動量濾波得到的速度補償。在數值仿真下對光電控制系統是否有速度前饋進行了對比，在沒有速度前饋時，方位和俯仰的跟蹤誤差最大可達±0.004 5 rad；在加入濾波的速度前饋之后，方位跟蹤誤差最大約為±0.000 9 rad，俯仰跟蹤誤差最大約為±0.001 rad。可以看出，經過卡爾曼濾波補償控制系統的跟蹤誤差明顯小于無補償的控制系統。

圖7 粗跟蹤誤差對比

4 結論

圖8 復合控制跟蹤輸出誤差

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