基于卡爾曼濾波的云臺自適應姿態(tài)優(yōu)化算法

劉 義，楊 鵬

（南京理工大學 瞬態(tài)物理國家重點實驗室，南京 210094）

云臺開發(fā)的核心就是對姿態(tài)進行實時解析與傳送。姿態(tài)檢測技術利用集成慣性測量單元（inertial measurement unit，IMU）對目標的姿態(tài)進行精確解算[1]。在IMU 中，陀螺儀負責采集姿態(tài)變化的角速度，理論上只對陀螺儀捕捉的角速度進行積分就可以得到負載的姿態(tài)角。但由于噪聲誤差的存在，直接積分會影響測量精度。因此，開發(fā)者需采用合理的算法對IMU 的信息進行融合解算。

常見的姿態(tài)解算算法有互補濾波、卡爾曼濾波等。線性互補濾波原理簡單、易實現(xiàn)，但精度不高[2]。非線性互補濾波采用誤差對角速度進行修正，能夠改善姿態(tài)更新的精度和穩(wěn)定性。

常規(guī)擴展卡爾曼濾波利用四元數(shù)進行姿態(tài)解算，這種方法能調(diào)用四元數(shù)的優(yōu)勢改善姿態(tài)解算的精度，但常規(guī)EKF 的系統(tǒng)噪聲和量測噪聲固定，難以削弱有害加速度對精度的影響，同時EKF 處理非線性系統(tǒng)時仍存在弊端，限制了卡爾曼濾波器的性能。

本文搭建模型對卡爾曼濾波器的系統(tǒng)噪聲和量測噪聲進行自適應調(diào)整，有效提高姿態(tài)的解算精度。同時，本文利用非線性互補濾波并行更新姿態(tài)，利用二者解算結果的變化率對濾波結果進行自適應融合，提高算法所得姿態(tài)的穩(wěn)定性，以便為伺服電機輸出平穩(wěn)的角度補償信號。

1 旋轉(zhuǎn)動作的表示方法

1.1 歐拉角和旋轉(zhuǎn)矩陣

根據(jù)慣導理論[3]，本文定義模型的x，y，z 軸分別為橫滾軸、俯仰軸、航向軸，n 系為地理坐標系，b 系為機體坐標系。任一時刻的機體坐標系都能轉(zhuǎn)換為地理坐標系依次繞z，y，x 軸旋轉(zhuǎn)一定角度得到，如圖1所示。

圖1 坐標系間的姿態(tài)轉(zhuǎn)換Fig.1 Attitude conversion between coordinate systems

以繞橫滾軸（x 軸）旋轉(zhuǎn)為例，旋轉(zhuǎn)動作投影到y(tǒng)Oz 面的坐標等效轉(zhuǎn)換關系，如圖2所示。

圖2 繞x 軸旋轉(zhuǎn)的坐標轉(zhuǎn)換Fig.2 Coordinate transformation of rotation around x axis

結合幾何原理，空間中一點（xn，yn，zn）從地理坐標系到機體坐標系的轉(zhuǎn)換關系式為

式中：α 為機體繞橫滾軸（x 軸）旋轉(zhuǎn)的歐拉角。同理可得旋轉(zhuǎn)體發(fā)生俯仰和偏航動作的坐標轉(zhuǎn)換矩陣分別為，，故地理坐標系到載體坐標系的坐標轉(zhuǎn)換陣Cnb表示為

展開可得Cnb的三角函數(shù)表達矩陣如式（4）所示：

綜上，用歐拉角表示旋轉(zhuǎn)動作的方法原理簡單、表象直觀，但此方法存在“萬向節(jié)死鎖”的現(xiàn)象[4]，且旋轉(zhuǎn)軸順序的改變也會影響旋轉(zhuǎn)的結果，存在一定的弊端。

1.2 四元數(shù)

四元數(shù)（Quaternions）相較于歐拉角表示旋轉(zhuǎn)，能避免萬向鎖的情況出現(xiàn)，而且運用于計算機運算時，使用四元數(shù)表示旋轉(zhuǎn)僅需存儲四個浮點數(shù)，相比矩陣更為輕量。因此，本文選用四元數(shù)進行姿態(tài)解算。

物體在三維空間中的任一旋轉(zhuǎn)動作可以抽象表示為“向量繞軸旋轉(zhuǎn)一定角度”的模型，如圖3所示。

圖3 空間向量旋轉(zhuǎn)模型Fig.3 Space vector rotation model

根據(jù)旋轉(zhuǎn)變換模型，推導出向量轉(zhuǎn)換關系如式（5）所示：

其中：

式中：ε 為旋轉(zhuǎn)角度；D 為轉(zhuǎn)換矩陣；N 為中心軸向量，N=（l，m，n）。令四元數(shù)（q0，q1，q2，q3）各項為

整理得坐標系轉(zhuǎn)換矩陣為

式（4）和式（8）對比整理可得，俯仰角、橫滾角、航向角的四元數(shù)法表示形式如式（9）所示：

2 四元數(shù)卡爾曼濾波器

卡爾曼濾波是一種利用系統(tǒng)狀態(tài)方程和觀測數(shù)據(jù)來估計線性系統(tǒng)狀態(tài)信息的最優(yōu)估計算法。四元數(shù)卡爾曼濾波算法流程如圖4所示[5]。

圖4 四元數(shù)EKF 算法流程Fig.4 Flow chart of quaternion EKF algorithm

四元數(shù)卡爾曼濾波算法的原理是利用四元數(shù)微分方程來更新四元數(shù)[6]，從而計算出姿態(tài)角信息。結合云臺模型和四元數(shù)理論，建立云臺姿態(tài)解算系統(tǒng)的狀態(tài)方程為

式中：qK為狀態(tài)向量；ηK，γK分別表示系統(tǒng)噪聲和量測噪聲；zK為系統(tǒng)的量測值，這里指的是加速度計的輸出；f（·），h（·）為非線性向量函數(shù)。根據(jù)捷聯(lián)式慣性導航理論，四元數(shù)微分方程的表達形式如式（11）所示：

根據(jù)卡爾曼濾波器的原理，推導出四元數(shù)的先驗估計值和噪聲先驗矩陣為式（14）和式（15）所示：

式中：QK-1為系統(tǒng)的噪聲方差；PK-1為前一時刻的后驗均方差矩陣；ΦK∣K-1為狀態(tài)方程中的Jacobi 矩陣，根據(jù)式（12）得ΦK∣K-1的數(shù)學表達式如式（16）所示：

結合旋轉(zhuǎn)矩陣和水平靜置的加速度計數(shù)據(jù)特點，得出加速度預測矩陣如式（17）所示：

進而得出預測矩陣的Jacobi 矩陣為式（18）所示：

根據(jù)卡爾曼濾波器的基礎公式中，濾波器增益公式如式（19）所示：

利用實際測量加速度值和預測加速度矩陣獲得新息，并利用新息對式（14）四元數(shù)先驗估計值進行補償?shù)玫搅繙y的最優(yōu)估計后驗值為

式中：ZK為加速度計的量測數(shù)據(jù)。下一步計算后驗均方差矩陣PK，構成完整增益解算回路。增益計算公式如式（21）所示：

式（20）所得qK為經(jīng)過濾波所得最優(yōu)估計四元數(shù)，將qK代入三軸姿態(tài)角的四元數(shù)解算公式（9）可推算出當前采樣時刻的姿態(tài)角最優(yōu)估計值。

3 融合卡爾曼濾波的姿態(tài)解算算法

云臺系統(tǒng)中影響系統(tǒng)進行IMU 數(shù)據(jù)姿態(tài)解算精度的因素主要有機體抖動、陀螺儀噪聲等?？柭鼮V波器能夠濾除高頻振動噪聲，但在實際的云臺系統(tǒng)中，非重力加速度對姿態(tài)解算結果的擾動作用必須予以重視，因為加速度計數(shù)據(jù)的統(tǒng)計學特性會發(fā)生非線性波動，同時云臺的運動狀態(tài)不能保持絕對的勻速，由此產(chǎn)生的非重力加速度會對姿態(tài)解算產(chǎn)生不利影響。假設式（15）、式（19）中噪聲Q，R 恒定的做法不能準確地模擬現(xiàn)實中的噪聲誤差，導致算法精度降低。本文提出噪聲調(diào)整模型來自適應改變量測值的可信度，改善算法的測量精度。

卡爾曼濾波器適用于線性系統(tǒng)，但現(xiàn)實中工作系統(tǒng)多為非線性時變系統(tǒng)，因此在第二章將模型進行了非線性近似化，如式（10）所示。但僅通過泰勒展開進行模型非線性化仍會帶來額外誤差[8]。本文采用非線性互補濾波的方法，對卡爾曼濾波回路的解算結果進行融合，一定程度上提高算法處理非線性系統(tǒng)的可靠性，并在云臺姿態(tài)不變的情況下，降低對測量恒定角的均方差，提高系統(tǒng)的穩(wěn)定性。

3.1 基于加速度的變噪聲模型

針對有害加速度，常規(guī)過濾方法是通過設置加速度誤差閾值來調(diào)整濾波器性能，這種辦法雖然原理簡單，但閾值的不確定性會對濾波效果造成影響。

本文構建模型來自適應地改變噪聲矩陣Q，R的可信度因子，調(diào)整先驗預測值和觀測值對解算結果的影響權重。調(diào)整模型的數(shù)學原型是Sigmoid 函數(shù)。

Sigmoid 函數(shù)能夠把函數(shù)的輸入值變換為0 和1之間的輸出。由Sigmoid 函數(shù)變形得到的模型函數(shù)不僅可為系統(tǒng)引入非線性因素，而且能使輸出值在合理的范圍內(nèi)變化。Sigmoid 函數(shù)的數(shù)學特性如圖5所示。

圖5 Sigmoid 函數(shù)數(shù)學特性Fig.5 Mathematical characteristics of Sigmoid

利用常規(guī)EKF 算法進行解算時，系統(tǒng)的噪聲方差Q 來源于陀螺儀誤差Q0，量測噪聲R 來源于加速度計誤差R0。本文設計的系統(tǒng)噪聲和量測噪聲矩陣自適應計算公式為

式中：δQ，δR定義為自適應調(diào)整系數(shù)，作用特性為：當加速度數(shù)據(jù)的非重力部分比例減小時，需提高量測值的可信度，即增大δQ以增大Q、減小δR以減小R；反之，當非重力部分占比提高時，則需增大δR、減小δQ。聯(lián)立Sigmoid 函數(shù)特點，設計δQ和δR的模型表達式如式（23）所示：

式中：kQ，kR分別代表加速度調(diào)整系數(shù)，控制自適應調(diào)整的速度，需根據(jù)具體工況進行調(diào)整；ab%表示加速度計數(shù)據(jù)中非重力加速度部分的占比，定義為

式中：a 為三軸加速度的合成量。根據(jù)上述模型可得，當kQ=kR=0 或者ab%=0 時，代表放棄關于噪聲矩陣的自適應調(diào)整功能。

結合激活函數(shù)的數(shù)學特性，本文所提變噪聲數(shù)學模型既可以自適應調(diào)節(jié)噪聲以調(diào)整對估計值和量測值的信任度，又可以將對誤差的調(diào)節(jié)力度限制在一定的區(qū)間內(nèi)，避免因出現(xiàn)異常值而對預測結果產(chǎn)生不良的影響。

3.2 非線性互補濾波補償模型

四元數(shù)解算算法的核心是四元數(shù)的更新。在EKF 算法中，式（14）調(diào)用慣性元件所測得陀螺儀數(shù)據(jù)和四元數(shù)微分方程計算出先驗四元數(shù)，后續(xù)利用增益計算回路對結果進行補償修正。

非線性互補濾波調(diào)用PI 控制器，利用加速度在水平方向上對重力進行補償，修正陀螺儀的垂直誤差。具體原理是將加速度計數(shù)據(jù)和四元數(shù)計算得到的重力分量叉乘得到誤差，如式（25）所示，并利用PI 控制器對誤差進行比例積分運算，如式（26）所示，對陀螺儀數(shù)據(jù)進行補償[9]。

下一步根據(jù)式（12）完成四元數(shù)更新，繼而由姿態(tài)角求解公式計算姿態(tài)角。如圖6所示，非線性互補濾波算法流程參考圖中Mathony 框選部分。

圖6 改進型算法流程Fig.6 Improved algorithm flow chart

本文根據(jù)解算結果變化率對兩個計算回路所計算的姿態(tài)進行自適應融合，數(shù)據(jù)融合模型的表達式如式（27）所示：

式中：δ 為融合權重因子，計算公式為

通過上述方式將非線性互補濾波與改進型變噪聲EKF 算法進行融合，一定程度地降低系統(tǒng)處理非線性系統(tǒng)的誤差，并有效提高姿態(tài)的穩(wěn)定性，利于實際的工程應用。

4 仿真實驗與結果分析

本次的驗證實驗主要以基于Matlab 軟件平臺的仿真實驗為主。根據(jù)設定姿態(tài)產(chǎn)生6 軸IMU 傳感器數(shù)據(jù)，包括陀螺儀數(shù)據(jù)、加速度計數(shù)據(jù)，沒有對磁力計的數(shù)據(jù)進行模擬，且根據(jù)數(shù)學原理加速度計數(shù)據(jù)只能對俯仰角、橫滾角進行修正，因此本次驗證僅針對俯仰角和橫滾角進行解算。

4.1 動態(tài)搖擺實驗

搖擺姿態(tài)的初始條件如表1所示。

表1 仿真姿態(tài)初始條件Tab.1 Initial conditions of simulated attitude

根據(jù)搖擺的初始條件產(chǎn)生仿真用陀螺儀、加速度計數(shù)據(jù)。采用恒定噪聲的擴展卡爾曼濾波和改進型融合濾波算法分別對慣導數(shù)據(jù)進行姿態(tài)角解算。搖擺數(shù)據(jù)真實值以及兩種算法的濾波結果如圖7所示。

圖7 搖擺實驗姿態(tài)對比圖Fig.7 Comparison figure of swing test attitude

圖中實線部分表示姿態(tài)角理論姿態(tài)，虛線表示擴展卡爾曼濾波算法解算姿態(tài)，點虛線表示本文改進型融合濾波算法解算的姿態(tài)。

如圖7所示，EKF 算法解算的結果與真實姿態(tài)間存在較大的偏差，而改進型融合濾波算法在解算過程中利用平動加速度所占比例來動態(tài)調(diào)整系統(tǒng)對估計值和誤差值的信任度因子，因此所得姿態(tài)與真實姿態(tài)幾乎一致。算法具體的濾波效果量化分析如表2所示。

表2 搖擺實驗誤差分析Tab.2 Error analysis of rocking experiment

4.2 靜態(tài)懸停實驗

為了進一步驗證融合非線性互補濾波對解算算法的影響，模擬了云臺鎖定跟蹤目標的工況來驗證在姿態(tài)角為恒定值時算法模型所得結果的精度。

設定俯仰角為30°、姿態(tài)角為20°時，EKF 算法和改進型融合算法對恒定姿態(tài)的檢測結果，如圖8所示。

圖8中虛線表示模擬理論姿態(tài)；實線代表算法解算的實際姿態(tài)。濾波效果量化分析如表3所示。

表3 恒定角度實驗誤差分析Tab.3 Error analysis of constant angle experiment

圖8 固定姿態(tài)角解算圖Fig.8 Fixed attitude angle solution

綜上，改進算法不僅可以有效地降低姿態(tài)角測量的誤差，而且還能夠?qū)⒏┭鼋呛蜋M滾角的測量結果標準差分別降低44%和47%，證明改進算法不僅可以提高云臺姿態(tài)角恒定時的測量精度，還可以一定程度上改善系統(tǒng)的穩(wěn)定性，以便為伺服電機輸出平穩(wěn)的角度補償信號。

5 結語

本文提出了一種基于四元數(shù)擴展卡爾曼濾波器的云臺姿態(tài)解算算法，利用Sigmoid 函數(shù)的數(shù)學特性搭建噪聲自適應調(diào)整模型，并利用非線性互補濾波對解算結果進行進一步融合。實驗證明，本文設計的改進型算法能有效提高姿態(tài)角的解算精度，并能在一定程度上改善懸停工況下姿態(tài)信號輸出的穩(wěn)定性，提高算法的工程實用性。

本文算法沒有引入磁力計來進行數(shù)據(jù)融合，因此沒能引入偏航角進行全姿態(tài)的測量研究。同時，由于非線性互補濾波的引入必將會增加算法的解算時間，實時性問題也應作為下一步研究的重點方向。