劉 義，楊 鵬

（南京理工大學(xué) 瞬態(tài)物理國(guó)家重點(diǎn)實(shí)驗(yàn)室，南京 210094）

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1.2 四元數(shù)

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

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

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

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

其中：

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

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

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

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

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

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

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

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

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

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

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

進(jìn)而得出預(yù)測(cè)矩陣的Jacobi 矩陣為式（18）所示：

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

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

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

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

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

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

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

3.1 基于加速度的變?cè)肼暷Ｐ?/h3>

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

本文構(gòu)建模型來自適應(yīng)地改變?cè)肼暰仃嘠，R的可信度因子，調(diào)整先驗(yàn)預(yù)測(cè)值和觀測(cè)值對(duì)解算結(jié)果的影響權(quán)重。調(diào)整模型的數(shù)學(xué)原型是Sigmoid 函數(shù)。

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

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

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

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

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

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

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

3.2 非線性互補(bǔ)濾波補(bǔ)償模型

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

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

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

圖6 改進(jìn)型算法流程Fig.6 Improved algorithm flow chart

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

式中：δ 為融合權(quán)重因子，計(jì)算公式為

通過上述方式將非線性互補(bǔ)濾波與改進(jìn)型變?cè)肼旹KF 算法進(jìn)行融合，一定程度地降低系統(tǒng)處理非線性系統(tǒng)的誤差，并有效提高姿態(tài)的穩(wěn)定性，利于實(shí)際的工程應(yīng)用。

4 仿真實(shí)驗(yàn)與結(jié)果分析

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

4.1 動(dòng)態(tài)搖擺實(shí)驗(yàn)

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

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

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

圖7 搖擺實(shí)驗(yàn)姿態(tài)對(duì)比圖Fig.7 Comparison figure of swing test attitude

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

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

表2 搖擺實(shí)驗(yàn)誤差分析Tab.2 Error analysis of rocking experiment

4.2 靜態(tài)懸停實(shí)驗(yàn)

為了進(jìn)一步驗(yàn)證融合非線性互補(bǔ)濾波對(duì)解算算法的影響，模擬了云臺(tái)鎖定跟蹤目標(biāo)的工況來驗(yàn)證在姿態(tài)角為恒定值時(shí)算法模型所得結(jié)果的精度。

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

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

表3 恒定角度實(shí)驗(yàn)誤差分析Tab.3 Error analysis of constant angle experiment

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

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

5 結(jié)語

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

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