基于梯度下降的自適應(yīng)姿態(tài)融合算法*

陳 卓， 任久春， 朱 謙

(復(fù)旦大學(xué) 信息科學(xué)與工程學(xué)院，上海 200433)

0 引 言

慣性導(dǎo)航系統(tǒng)(inertial navigation system， INS)基于慣性傳感器與姿態(tài)解算方法測量物體的姿態(tài)信息，以進(jìn)行相應(yīng)控制。在國內(nèi)外學(xué)者的研究推動下，姿態(tài)融合算法體系已較為成熟。其中廣泛采用的擴(kuò)展卡爾曼濾波(extended Kalman filtering，EKF)[1]，無跡卡爾曼濾波(unscented Kalman filtering，UKF)[2]以及一些自適應(yīng)卡爾曼濾波(adaptive Kalman filtering，AKF)[3,4]算法都基于傳統(tǒng)卡爾曼濾波器進(jìn)行設(shè)計，具有較高的預(yù)測精度，但其涉及協(xié)方差矩陣的計算，復(fù)雜度高，在預(yù)測過程中難以建立可靠穩(wěn)定的狀態(tài)方程。對于加速度的高頻干擾，文獻(xiàn)[4]設(shè)計的第一代帆船運動姿態(tài)測量系統(tǒng)雖然基于卡爾曼濾波算法設(shè)置了自適應(yīng)因子，但其作用范圍有限，魯棒性較差，物體動態(tài)測量性能受到限制。互補濾波融合算法[5,6]利用陀螺儀的高頻特性與加速度計的低頻特性優(yōu)勢互補，設(shè)計通帶濾波，簡單高效，但難以確定濾波器的截止頻率，易受噪聲與運動加速度的干擾。文獻(xiàn)[7]利用加速度計、磁力計測量得到的場量變化，基于梯度下降算法(gradient descent algorithm，GDA)，修正角速度測量姿態(tài)。該方法通用性強，測量精度優(yōu)于卡爾曼濾波算法，計算復(fù)雜度較低，適用于小型嵌入式系統(tǒng)。文獻(xiàn)[8]在此基礎(chǔ)上設(shè)計了一種動態(tài)步長梯度下降+運動加速度抑制處理的(dynamic gradient descent algorithm+motion acceleration processing，DGDA+MAP)姿態(tài)算法，通過求解物體運動加速度模長，為梯度下降設(shè)計動態(tài)變化步長，從而提升了四旋翼飛行器在強機(jī)動、高速運動狀態(tài)下的姿態(tài)解算性能。

本文針對帆船運動訓(xùn)練中穩(wěn)定性和實時性的實際需求，基于梯度下降姿態(tài)融合算法設(shè)計了兩種改進(jìn)的自適應(yīng)方法，分別根據(jù)前一時間段中的平均運動加速度模長和加速度增量的變化得到當(dāng)前的自適應(yīng)因子控制優(yōu)化步長，從而提高了系統(tǒng)在高動狀態(tài)中的穩(wěn)定性，實驗結(jié)果表明其測量精確度優(yōu)于文獻(xiàn)[4，8]，并已成功應(yīng)用于第二代帆船運動姿態(tài)測量系統(tǒng)中。

1 慣導(dǎo)系統(tǒng)中的運動表示

慣性導(dǎo)航系統(tǒng)依靠慣性測量單元(inertial measurement unit，IMU)得到物體的姿態(tài)信息進(jìn)行導(dǎo)航控制。IMU包含三軸加速度計、三軸陀螺儀等慣性測量元件[9]。在慣性導(dǎo)航系統(tǒng)中，物體的運動可以抽象為載體坐標(biāo)系S相對于參考坐標(biāo)系E的運動，兩者之間的相互關(guān)系可用方向余弦矩陣[10]來表示。圖1的歐拉角[11]、橫滾角φ、俯仰角θ與航向角ψ在慣性導(dǎo)航系統(tǒng)中可以用于確定物體的任意姿態(tài)，又稱為姿態(tài)角。

圖1 三維坐標(biāo)系下的歐拉角示意

在實際工程應(yīng)用中，由于四元數(shù)法[12]求解微分方程式計算量較小，多用其求解運動姿態(tài)。

2 姿態(tài)融合算法

2.1 基于梯度下降的姿態(tài)融合算法

2.2 改進(jìn)的自適應(yīng)算法

考慮到在高速運動狀態(tài)下加速度計易受高頻干擾，因此本文在文獻(xiàn)[7]算法基礎(chǔ)上設(shè)計了兩種加速度控制的自適應(yīng)算法，加速度控制的自適應(yīng)梯度下降算法(acceleration-controlled adaptive gradient descent algorithm，ACAGDA)和增量加速度控制的自適應(yīng)梯度下降算法(delta-acceleration-controlled adaptive gradient descent algorithm，ΔACAGDA)。通過考察當(dāng)前時刻之前N個采樣點(包含當(dāng)前時刻)的運動加速度及加速度的變化量從而得到穩(wěn)定的自適應(yīng)梯度下降步長，減少了瞬時噪聲的影響，提高算法可信度。

2.2.1 ACAGDA

(2)

2.2.2 ΔACAGDA

物體在實際運動狀態(tài)中的復(fù)雜多變導(dǎo)致了加速度的不斷變化，即使處于靜止?fàn)顟B(tài)下，由于噪聲干擾，傳感器所測量得到的加速度也不能維持在某一常數(shù)，因此，本文根據(jù)實際測量經(jīng)驗設(shè)計了基于ΔACAGDA算法。設(shè)t時刻之前N個加速度采樣值矢量變化量之和為δN，根據(jù)δN求取三個軸向的均方根變化量RMSδ，RMSδ的大小表示物體在某一軸向的運動是否超出靜態(tài)范圍，因此，需要設(shè)置閾值η。同樣引入常數(shù)控制因子γδ與加速度自適應(yīng)因子λδ用于調(diào)節(jié)β

(4)

當(dāng)RMSδ未超過閾值時，表示這一時間段物體處于靜止或者擬靜止?fàn)顟B(tài)，此時加速度變化量很小，近似為重力加速度，自適應(yīng)因子不對步長進(jìn)行限制；當(dāng)RMSδ超過閾值時，加速度發(fā)生大幅變化，表示物體處于高速運動狀態(tài)，λδ與RMSδ呈負(fù)相關(guān)，通過RMSδ的大小對β進(jìn)行反向調(diào)節(jié)，本文采用平方項加快衰減。

3 實驗結(jié)果與性能分析

3.1 姿態(tài)測量系統(tǒng)

本文基于第二代帆船運動姿態(tài)傾角測量儀(以下簡稱傾角儀)進(jìn)行算法驗證測試，傾角儀以三軸陀螺儀L3G4200D與三軸加速度計LSM303DLH作為MEMS慣性測量單元，在第一代測量系統(tǒng)設(shè)計結(jié)構(gòu)上進(jìn)行改進(jìn)。在實驗過程中，根據(jù)文獻(xiàn)[4]中設(shè)計的重力擺驗證測試方法將傾角儀固定在實驗裝置上，控制儀器處于不同的運動狀態(tài)，通過藍(lán)牙導(dǎo)出姿態(tài)角數(shù)據(jù)，同時用示波器記錄重力擺裝置上角度傳感器的輸出電壓值，即可得到真實的角度參照數(shù)據(jù)進(jìn)行對比。

3.2 實驗結(jié)果分析

設(shè)計傳感器采樣率為100 Hz，為了測試物體在靜態(tài)和動態(tài)兩種狀態(tài)中的姿態(tài)解算性能，實驗?zāi)M帆船運動設(shè)計了多種姿態(tài)角度變化場景。圖2為典型的繞軸運動，即傾角快速變化時四種姿態(tài)解算方法的測量結(jié)果。實驗經(jīng)過多次快速繞軸運動與靜止?fàn)顟B(tài)交替測試，可以看出，本文提出的2種方法解算得到的曲線跟蹤度較好，尤其在運動狀態(tài)的過度階段曲線更加平滑，橫滾角測量方法與其類似。圖3為圖2相對應(yīng)的測量殘差，從比較結(jié)果中可以看出通過引入負(fù)相關(guān)平方項以及對N點運動加速度平均模長的考察，其動態(tài)自適應(yīng)性能相對單點運動加速度抑制有了一定程度的改善，加大了對高頻干擾的抑制。而通過加速度增量來進(jìn)行梯度下降步長的自適應(yīng)調(diào)制得到的殘差曲線幅度最小，性能達(dá)到最優(yōu)。

圖2 4種姿態(tài)解算下的俯仰角

圖3 4種姿態(tài)解算下的俯仰角殘差

4種算法在靜態(tài)和動態(tài)兩種狀態(tài)下其測量得到的俯仰角(θ,φ)和橫滾角相對于真實角度的均方根誤差(root mean square error，RMSE)，在靜止?fàn)顟B(tài)中，4種算法性能差距不大，其中AKF(0.078°,0.086°)與 ACAGDA(0.081°,0.092°)2種方法的誤差均達(dá)到了0.1°以內(nèi)。而在運動狀態(tài)中，ACAGDA(1.859°,1.873°)的測量誤差相對于AKF(4.741°,4.563°)、DGDA+MAP(2.301°,2.197°)兩種方法分別減少了59.87 %,16.98 %， ACAGDA(1.145°,1.166°)則相對于AKF、DGDA+MAP、ACAGDA三種方法分別減少了75.15 %,48.59 %與38.08 %，本文提出的2種自適應(yīng)方法具有更好的穩(wěn)定性與精確度，能夠更加適用于海面帆船運動船體姿態(tài)測量的實際需求。

4 結(jié) 論

本文為了提高帆船運動中船體姿態(tài)測量實際應(yīng)用中的穩(wěn)定性，減少物體在高速運動狀態(tài)下附加加速度的干擾，基于梯度下降算法，設(shè)計了兩種加速度控制步長的自適應(yīng)姿態(tài)解算方法，其性能相對于文獻(xiàn)[4,8]所提出的方法有較為明顯的改善，其中基于N點增量加速度的控制方法具有更高的穩(wěn)定性與有效性，已經(jīng)實現(xiàn)于第二代帆船運動姿態(tài)測量系統(tǒng)中。