喬美英許城寬湯夏夏高翼飛史建柯

(河南理工大學(xué)電氣工程與自動(dòng)化學(xué)院，河南 焦作454000)

MEMS 三軸加速度計(jì)因成本低、體積小、低功耗，廣泛應(yīng)用于無人機(jī)、無人車、無人船、機(jī)器人、醫(yī)療輔助器械及隨鉆等設(shè)備[1]。 但由于受自身工藝及應(yīng)用環(huán)境等因素，導(dǎo)致三軸加速度計(jì)測(cè)量精度嚴(yán)重降低，無法滿足高精度測(cè)量系統(tǒng)的要求。 隨鉆軌跡測(cè)量(Trace Measurement While Drilling，TMWD)系統(tǒng)利用MEMS 加速度計(jì)解算出的姿態(tài)角繪制軸跡，為保證所繪軸跡的準(zhǔn)確性，需要不斷對(duì)提高加速度計(jì)解算精度的相關(guān)技術(shù)進(jìn)行創(chuàng)新和優(yōu)化，以減少實(shí)際作業(yè)過程中鉆孔真實(shí)軌跡與理想軌跡的偏差。

已有相關(guān)文獻(xiàn)給出加速度計(jì)的校正方法，例如加速度計(jì)的最大似然校正算法[2]，同時(shí)實(shí)現(xiàn)加速度計(jì)三軸非正交誤差的補(bǔ)償，補(bǔ)償?shù)闹攸c(diǎn)在于參數(shù)的精確估計(jì)，但該方法無法補(bǔ)償未對(duì)準(zhǔn)誤差。 較為流行的基于橢球假設(shè)的三軸加速度計(jì)誤差標(biāo)定與補(bǔ)償[3]，該方法也考慮了非正交誤差，使得測(cè)量精度更高，但橢球擬合法對(duì)于原始的數(shù)據(jù)要求較高，且無法彌補(bǔ)未對(duì)準(zhǔn)誤差。 文獻(xiàn)[4]是矢量傳感器對(duì)非對(duì)準(zhǔn)誤差及其校正，該方法利用橢球擬合及點(diǎn)積不變相結(jié)合的方法，彌補(bǔ)了橢球擬合不能校正非對(duì)準(zhǔn)誤差的缺陷，但這種方法實(shí)現(xiàn)起來需要很高的實(shí)驗(yàn)條件，且點(diǎn)積不變法對(duì)參考矢量精度有要求，否則算法效果不佳，并且該算法需要額外的矢量輔助運(yùn)算，不利于校正單一傳感器。 高斯牛頓迭代算法[5]，其優(yōu)點(diǎn)是操作簡單，優(yōu)化速度快，但優(yōu)化參數(shù)過多會(huì)發(fā)生部分參數(shù)局部最優(yōu)情況，無法滿足所有參數(shù)一同得出最優(yōu)解。 以上算法盡管在一定的條件下可以達(dá)到預(yù)期的效果，但存在以下問題:需要大量的實(shí)測(cè)數(shù)據(jù)；較多的數(shù)據(jù)易造成算法對(duì)數(shù)據(jù)的依賴性；面對(duì)非線性較強(qiáng)函數(shù)尤其是迭代參數(shù)較多的情況下，不加以約束易造成死循環(huán)，最終使得所有參數(shù)無法到達(dá)最優(yōu)點(diǎn)。

針對(duì)上述問題本文利用Dragonfly Algorithm(DA)與Levenberg-Marquardt(LM)的聯(lián)合優(yōu)化算法，DA是一種多群體智能優(yōu)化算法，能夠?qū)崿F(xiàn)目標(biāo)參數(shù)的全局尋優(yōu)，缺陷是局部搜索能力不高，但可以利用LM 算法強(qiáng)力的局部尋優(yōu)能力再次優(yōu)化參數(shù)。 校正之后的加速度計(jì)坐標(biāo)系實(shí)現(xiàn)三軸正交，但在制造的過程中受到工藝水平的限制，加速度計(jì)與載體坐標(biāo)系三軸之間存在未對(duì)準(zhǔn)角度，表現(xiàn)為兩個(gè)坐標(biāo)系之間存在旋轉(zhuǎn)矩陣，本文針對(duì)未對(duì)準(zhǔn)問題給出補(bǔ)償策略，相較于傳統(tǒng)的方法易于實(shí)現(xiàn)。 為了提高數(shù)據(jù)的擬合效果，實(shí)驗(yàn)采集數(shù)據(jù)時(shí)用等夾角法[5]旋轉(zhuǎn)雙軸轉(zhuǎn)臺(tái)，使數(shù)據(jù)在空間中呈均勻分布。

1 誤差分析和建模

MWD 的傳感器模型與通用的航姿參考系統(tǒng)(Attitude and Heading Reference System，AHRS)基本相同，因此MWD 的加速度計(jì)誤差模型也可以用統(tǒng)一誤差模型來表示。 MEMS 加速度傳感器在隨鉆中的布局可參見文獻(xiàn)[6]。 MEMS 加速度計(jì)誤差來源主要有比例系數(shù)誤差、零點(diǎn)漂移誤差、未對(duì)準(zhǔn)誤差以及非正交誤差[7]等。 依據(jù)誤差來源寫為誤差模型:

U為加速度計(jì)不受誤差干擾的無偏值；D為比例因子誤差；T為未對(duì)準(zhǔn)誤差；S為非正交誤差；B為加速度計(jì)偏差；V為加速度計(jì)測(cè)量值。 在使用DA-LM 估計(jì)誤差參數(shù)時(shí)，暫不考慮未對(duì)準(zhǔn)誤差T。讓SD＝K，由式(1)可得:

通過求得K及B矩陣參數(shù)可實(shí)現(xiàn)對(duì)加速度計(jì)誤差補(bǔ)償。

1.1 誤差模型變換

加速度計(jì)輸出三個(gè)數(shù)據(jù)量:Vx，Vy，Vz分別對(duì)應(yīng)當(dāng)?shù)刂亓?chǎng)x，y，z三個(gè)方向的分量。 對(duì)式(1)展開可將加速度計(jì)的誤差矩陣方程寫為:

ux，uy，uz為真實(shí)重力場(chǎng)分量；εx，εy，εz為非正交誤差參數(shù)；fx，fy，fz為加速度計(jì)三軸對(duì)應(yīng)的比例因子誤差，Bx，By，Bz為三軸偏差值，其中以x軸的輸出Vx為例:

當(dāng)?shù)氐闹亓?chǎng)在加速度計(jì)的三軸投影:

｜g｜為重力加速度絕對(duì)值；α，β，γ分別對(duì)應(yīng)重力三個(gè)方位之間的夾角[8]，式(5)代入式(4)寫為

利用式(5)、式(6)得出:

同理Vy，Vz也能夠?qū)懗扇缡?7)的形式。 聯(lián)立可寫為等式:

式(9)ax～cz為式(8)的輸入值，該輸入由加速度計(jì)的輸出提供。 LM 算法優(yōu)化式(8)的誤差參數(shù)，該算法局部收斂能力很強(qiáng)但全局收斂能力較弱，因此DA 先作初始優(yōu)化使誤差參數(shù)達(dá)到全局收斂，輸出的參數(shù)結(jié)果再用LM 優(yōu)化。

1.2 初始參數(shù)選取

DA 是一種仿生學(xué)算法，它模擬蜻蜓在自然界中的靜態(tài)和動(dòng)態(tài)蜂群行為[9]。 該算法利用蜻蜓的習(xí)性分為:分離、排隊(duì)、結(jié)盟、尋找獵物和躲避天敵這5 類行為。 算法中的參數(shù)值:分離權(quán)重、對(duì)齊權(quán)重、凝聚權(quán)重、獵物權(quán)重因子、天敵權(quán)重因子作為蜻蜓的行為度，以更新蜻蜓的位置，該算法的優(yōu)勢(shì)是能夠改善給定問題的初始隨機(jī)參數(shù)，收斂于全局最優(yōu)值。

初值優(yōu)化算法采用橢球方程給出評(píng)價(jià)函數(shù)[10]，因?yàn)榻?jīng)仿真分析DA 與橢球擬合的聯(lián)合校正法可以消除零偏誤差及白噪聲的干擾，并且方程簡便，易于處理。 式(2)作平方可得:

G＝(K-1)T·K-1＝sTs，構(gòu)造DA 的評(píng)價(jià)函數(shù):

式中:A＝‖U‖2是加速度計(jì)無偏模值的平方，V＝(V2xV2yV2zVxVyVyVzVzVxVxVyVz)，η＝(abcdeflmng)T，a～g是帶有誤差信息的橢球參數(shù)。 DA 求解(11)中的參數(shù)向量η，可構(gòu)成如下矩陣:

s可通過奇異值分解求得:

ΣTΣ為G的特征值組成的對(duì)角陣，W是G特征值對(duì)應(yīng)特征向量組成的矩陣，因此s寫成:

求得偏差B:

根據(jù)式(2)可獲得經(jīng)過DA 優(yōu)化的無偏值U′:

根據(jù)式(3)求得LM 算法的初值:

在DA 優(yōu)化后的參數(shù)結(jié)果中，經(jīng)過仿真實(shí)驗(yàn)，偏差B可精確求得，所以待優(yōu)化的誤差參數(shù)由原先的9 個(gè)變?yōu)榱? 個(gè):

其值分別對(duì)應(yīng)于誤差參數(shù)fx～εz。

2 LM 最優(yōu)估計(jì)

LM 法是解決非線性最小二乘問題的一種方法，獲得非線性方程的最優(yōu)解。 LM 算法同時(shí)具備梯度及牛頓法的特征，對(duì)于連續(xù)微分的線性系統(tǒng)，LM 法可以通過更改步長找到局部最優(yōu)解。 首先定義幾個(gè)參數(shù):Jk是對(duì)應(yīng)于θk的Jacobi 矩陣，dk是每次更新θk的步長，優(yōu)化精度εk＝1e-4，λk是學(xué)習(xí)因子，αk是增益系數(shù)。

2.1 LM 算法實(shí)現(xiàn)流程

Step 1 經(jīng)過1.2 重新定義目標(biāo)函數(shù):

Bv已成為無偏量，θk作為初值，計(jì)算Jk，并且令λk＝1/N×trace(JTk·Jk)為初始學(xué)習(xí)因子:Step 2 如果‖JkF(θk)‖≤εk，停止迭代，輸出估計(jì)的參數(shù)值。 如果條件未達(dá)成，則計(jì)算步長:

I是與Jk具有相同階數(shù)的單位矩陣。F(θk)對(duì)每個(gè)待求參數(shù)求偏導(dǎo):

n個(gè)數(shù)據(jù)6 個(gè)參數(shù)構(gòu)成n×6 的雅可比矩陣Jk。

Step 3 計(jì)算增益系數(shù):

如果αk>0，則θk＋1＝θk＋dk；若不滿足該條件，則θk＋1＝θk。 Aredk表示目標(biāo)函數(shù)實(shí)際差值，Predk表示目標(biāo)函數(shù)預(yù)測(cè)差值。αk用來檢驗(yàn)當(dāng)前預(yù)測(cè)參數(shù)的好壞。 更新λk:

Step 4 令k＝k＋1，并返回Step 2。

2.2 LM 算法改進(jìn)

實(shí)際獲得的數(shù)據(jù)帶有噪聲等不確定因素的影響，在第1 節(jié)給出誤差模型，沒有考慮時(shí)變等誤差[11]，這些誤差將直接作用于原始數(shù)據(jù)，數(shù)據(jù)的異常使得雅可比矩陣Jk出現(xiàn)偏差，隨著時(shí)間的推移，更多的異常值最后會(huì)導(dǎo)致算法局部尋優(yōu)失效。 因此LM 算法的核心是調(diào)整學(xué)習(xí)率，其目的是實(shí)現(xiàn)兩種算法適當(dāng)?shù)慕M合:梯度下降法的一致誤差減小與高斯-牛頓算法的局部收斂性。 所以重點(diǎn)是對(duì)學(xué)習(xí)率λk的調(diào)整。 本文使用一種新穎的λk調(diào)整策略[12]:

式中:mu＝max[0.4，min(αk，10)]，Qu為:

式(22)至式(24)對(duì)于學(xué)習(xí)因子λk的策略相比于式(22)增強(qiáng)了收斂性，對(duì)于二次方的目標(biāo)函數(shù)提高了收斂速度。 求得的誤差參數(shù)通過式(2)進(jìn)行補(bǔ)償。 綜上所述DA-LM 算法流程如圖1 所示。

圖1 DA-LM 算法流程圖

3 未對(duì)準(zhǔn)誤差校正

3.1 未對(duì)準(zhǔn)誤差分析

未對(duì)準(zhǔn)誤差是傳感器坐標(biāo)系與載體坐標(biāo)系之間的三軸未對(duì)準(zhǔn)偏差[13]，第1、2 小節(jié)DA-LM 估計(jì)并未考慮未對(duì)準(zhǔn)誤差，而傳感器框架的未對(duì)準(zhǔn)會(huì)大大降低導(dǎo)航系統(tǒng)的準(zhǔn)確性，在常規(guī)的校準(zhǔn)策略中無法確定這些失準(zhǔn)，但是可以將該誤差與加速度計(jì)其他誤差區(qū)別開來進(jìn)行估計(jì)。 該誤差來源主要由安裝工藝造成，如圖2 所示，其中G代表加速度計(jì)坐標(biāo)，P代表載體坐標(biāo)。

圖2 加速度計(jì)及載體之間的未對(duì)準(zhǔn)

對(duì)于未對(duì)準(zhǔn)校準(zhǔn)策略，如地磁場(chǎng)矢量與重力場(chǎng)矢量之間的恒定角度求得旋轉(zhuǎn)矩陣[14]，但是該方法需要額外的磁傳感器信息作為輔助，利用已知的無偏地磁場(chǎng)信息校準(zhǔn)傳感器，分別旋轉(zhuǎn)平臺(tái)的三個(gè)坐標(biāo)軸導(dǎo)出旋轉(zhuǎn)矩陣。 這種方法對(duì)于校準(zhǔn)設(shè)備要求較高，每次校準(zhǔn)都需要平臺(tái)的一個(gè)軸與地磁場(chǎng)相應(yīng)方向?qū)?zhǔn)。 但是對(duì)此方法可作改進(jìn)運(yùn)用于加速度計(jì)的校準(zhǔn)。

3.2 未對(duì)準(zhǔn)校正策略

定義加速度傳感器框架為一個(gè)正交框架:Dm，定義平臺(tái)框架為Dp。Dm與Dp三軸之間存在有小角度(δxy，δyz，δxz)，所以平臺(tái)框架相對(duì)于正交框架存在一個(gè)旋轉(zhuǎn)矩陣R:是加速度計(jì)測(cè)量值，Dpx，Dpy，Dpz是地球重力場(chǎng)的三個(gè)分量。 定義[px，py，pz]分別為平臺(tái)的三個(gè)旋轉(zhuǎn)軸。 首先繞py軸旋轉(zhuǎn)，此時(shí)Dpy輸出為常值，收集加速度計(jì)數(shù)據(jù)，然后通過最小二乘法估計(jì):

R21，R22，R23分別為R第二行參數(shù)，通過該行參數(shù)可以求解第一行參數(shù)，加速度計(jì)數(shù)據(jù)可求得橫滾角γ，而因此有:

γ作為真實(shí)的橫滾角，使平臺(tái)處于不同的姿態(tài)，收集傳感器數(shù)據(jù)，利用最小二乘法估計(jì)R的第一行參數(shù)。 同理，利用俯仰角及前兩步估計(jì)的參數(shù)可求得R第三行參數(shù):

式中:θ為真實(shí)的俯仰角數(shù)值，該值通過θ＝求得，旋轉(zhuǎn)矩陣R的所有參數(shù)均已求出。 該校準(zhǔn)策略的優(yōu)點(diǎn)是不需要平臺(tái)的每個(gè)軸都與參考矢量的方向?qū)R。

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

4.1 仿真實(shí)驗(yàn)

MATLAB 仿真實(shí)驗(yàn)，單位球面上均勻取點(diǎn)，每個(gè)點(diǎn)對(duì)應(yīng)在球面上的各個(gè)方向，采用等夾角均勻取點(diǎn)法形成64 個(gè)點(diǎn)，設(shè)置的誤差參數(shù)如表1 所示。

表1 誤差參數(shù)設(shè)置

將以上參數(shù)作用到數(shù)據(jù)點(diǎn)上，生成加速度干擾數(shù)據(jù)。 用DA 獲得誤差參數(shù)初值，首先需要設(shè)置DA初始參數(shù)，如表2 所示。

表2 DA 初始參數(shù)設(shè)置

當(dāng)數(shù)據(jù)未加入旋轉(zhuǎn)矩陣干擾的優(yōu)化結(jié)果如表3所示，可以看出優(yōu)化的部分參數(shù)能夠達(dá)到設(shè)定值，不過有些參數(shù)與設(shè)定值偏離較大。 但是DA 能為下面的工作提供強(qiáng)有力的幫助:能夠精確估計(jì)偏差，獲得橢球參數(shù)的全局最優(yōu)解，這為LM 算法提供了良好的初值運(yùn)算。

表3 加速度計(jì)校正參數(shù)(未加入旋轉(zhuǎn)矩陣)

當(dāng)數(shù)據(jù)加入了旋轉(zhuǎn)矩陣干擾的優(yōu)化結(jié)果如表4。

表4 加速度計(jì)校正參數(shù)(加入旋轉(zhuǎn)矩陣)

為了凸顯算法的作用，根據(jù)不同情況分別給出仿真結(jié)果，如圖3～圖7。

圖3 原始與干擾數(shù)據(jù)

圖4 DA 補(bǔ)償數(shù)據(jù)

圖5 LM-DA 補(bǔ)償數(shù)據(jù)

圖6 未對(duì)準(zhǔn)補(bǔ)償

圖7 算法綜合對(duì)比

圖3加入了誤差參數(shù)的數(shù)據(jù)呈現(xiàn)出傾斜的橢球體，所有數(shù)據(jù)點(diǎn)都偏離了單位球體，因偏差的作用干擾數(shù)據(jù)的原點(diǎn)偏離了中心坐標(biāo)點(diǎn)，比例因子，非正交誤差與未對(duì)準(zhǔn)誤差讓球體發(fā)生了畸變，數(shù)據(jù)點(diǎn)的模值均大于1。

圖4 中，經(jīng)過了DA 優(yōu)化的數(shù)據(jù)球體大小驟減明顯，全局搜索已起到作用，并且校正后的球體圓點(diǎn)已在坐標(biāo)軸中心，但是球體依然處于傾斜狀態(tài)，由于不能完全校準(zhǔn)參數(shù)，使得大量數(shù)據(jù)其模值小于1。

圖5 所示LM 優(yōu)化DA 的結(jié)果參數(shù)，球體幾乎恢復(fù)正常情況，數(shù)據(jù)點(diǎn)的模值均在1 附近，但是LM 優(yōu)化后的球體依然處于傾斜狀態(tài)，因?yàn)槟壳斑€未校正未對(duì)準(zhǔn)誤差。

圖6 中是對(duì)數(shù)據(jù)點(diǎn)分布在不同方向的擬合情況，在這一步中已加入了3.2 小節(jié)的未對(duì)準(zhǔn)校正策略，可以發(fā)現(xiàn)經(jīng)過了旋轉(zhuǎn)矩陣R的補(bǔ)償后，干擾后的數(shù)據(jù)點(diǎn)能夠擬合在球面上。

圖7 所示經(jīng)過DA 的數(shù)據(jù)集能夠回到坐標(biāo)軸中心，但數(shù)據(jù)分布在單位球體的內(nèi)部，再次運(yùn)用LM 算法可以將數(shù)據(jù)集擬合在球面上，但是與原數(shù)據(jù)的分布在方向上有偏差，經(jīng)過旋轉(zhuǎn)矩陣R的補(bǔ)償，受干擾的數(shù)據(jù)集能夠擬合到原數(shù)據(jù)集上。 在有關(guān)磁傳感器的校正方法中[15-18]同樣能夠獲得類似于圖3～圖7 的仿真結(jié)果，但本文校正策略不適用于磁傳感器誤差校正，因?yàn)轭~外存在的軟硬誤差，無法建立起1.1 小節(jié)的誤差模型，必須給出針對(duì)性的校正方法實(shí)現(xiàn)類似于本文圖5 的仿真結(jié)果。 但本文給出的對(duì)準(zhǔn)策略能夠用于磁傳感器，實(shí)現(xiàn)類似于圖7 的仿真結(jié)果，這為以后校正磁傳感器的工作提供幫助。

4.2 實(shí)測(cè)數(shù)據(jù)驗(yàn)證

為進(jìn)一步驗(yàn)證算法的有效性，采用自主研發(fā)的隨鉆測(cè)斜儀進(jìn)行現(xiàn)場(chǎng)實(shí)驗(yàn)， 該測(cè)斜儀主要由STC15F60S2 單片機(jī)；ADI 公司推出的基于iMEMS 技術(shù)的三軸、數(shù)字輸出加速度傳感器:ADXL345；還包括震動(dòng)傳感器、Flash 存儲(chǔ)器(w25x16)、外部時(shí)鐘模塊(DS1302)、藍(lán)牙模塊(hc-06)、鋰電池(18650)及本安型的電源電路等組成。 其中ADXL345 加速度計(jì)傳感器擁有最高13 位分辨率，功耗小，靈敏度高，最高達(dá)3.9 mg/LSB，以及量程可變范圍廣等優(yōu)點(diǎn)。 實(shí)驗(yàn)過程中ADXL345 測(cè)量的數(shù)據(jù)通過UART 協(xié)議傳輸?shù)絾纹瑱C(jī)，并存儲(chǔ)到Flash 中，最后由藍(lán)牙發(fā)送到上位機(jī)，用MATLAB 語言編寫代碼處理數(shù)據(jù)。 隨鉆測(cè)斜儀外殼，及內(nèi)部的測(cè)量短節(jié)如圖8 所示。

圖8 隨鉆測(cè)斜儀

4.3 參數(shù)擬合校正

測(cè)斜儀安置固定于無磁轉(zhuǎn)臺(tái)上，如圖9 所示。操作上位機(jī)使轉(zhuǎn)臺(tái)處于不同姿態(tài)，通過加速度計(jì)獲得轉(zhuǎn)臺(tái)在不同姿態(tài)下三個(gè)方位的重力分量。

圖9 雙軸轉(zhuǎn)臺(tái)

轉(zhuǎn)臺(tái)X，Y，Z三軸分別轉(zhuǎn)過一定角度，操作轉(zhuǎn)臺(tái)使其俯仰角、航向角旋轉(zhuǎn)次數(shù)、航向角間隔分別為:30°，8，45°；60°，14，25.7143°；90°，16，22.5°；120°，14，25.7143°；150°，8，45°；180°，1。 存儲(chǔ)芯片共記錄以上68 個(gè)加速度計(jì)輸出點(diǎn)。 利用本文提出算法估計(jì)誤差參數(shù)，其結(jié)果如表5。

表5 加速度計(jì)校正參數(shù)結(jié)果

4.4 俯仰角補(bǔ)償結(jié)果

分別采用不同算法補(bǔ)償誤差參數(shù):①單獨(dú)采用DA補(bǔ)償數(shù)據(jù)點(diǎn)，不采用LM 法與未對(duì)準(zhǔn)補(bǔ)償；②DA-LM擬合數(shù)據(jù)，不采用未對(duì)準(zhǔn)補(bǔ)償，；③采用DA-LM 并加入未對(duì)準(zhǔn)補(bǔ)償。 三種補(bǔ)償方式的實(shí)驗(yàn)結(jié)果依次見圖10～圖12。 在圖10 可以發(fā)現(xiàn)DA 擬合的數(shù)據(jù)點(diǎn)可以補(bǔ)償誤差，俯仰角誤差有所減小，但是部分誤差角仍然較大，因?yàn)镈A 可以估計(jì)傳感器誤差參數(shù)，但是由于局部尋優(yōu)能力較差，部分誤差參數(shù)并不能估算出最優(yōu)解，尤其是不能校正未對(duì)準(zhǔn)偏差。 圖11 經(jīng)過了DA-LM 的優(yōu)化其俯仰角誤差明顯減小，DA 提供的初值給予LM 很大的幫助，LM 優(yōu)秀的局部搜索能力能夠快速找到極值點(diǎn)，誤差參數(shù)的估計(jì)精度有所提高。但是加速度計(jì)與測(cè)斜儀之間的三軸未對(duì)準(zhǔn)角度影響加速度計(jì)角度的解算。

圖10 DA 補(bǔ)償結(jié)果

圖11 DA-LM 補(bǔ)償結(jié)果

圖12 DA-LM 及加入未對(duì)準(zhǔn)補(bǔ)償?shù)慕Y(jié)果

加速度計(jì)與測(cè)斜儀三軸存在角度偏差，因此加速計(jì)坐標(biāo)系與測(cè)斜儀坐標(biāo)系之間存在一個(gè)旋轉(zhuǎn)矩陣。 圖12 所示沒有經(jīng)過未對(duì)準(zhǔn)補(bǔ)償?shù)牟糠謹(jǐn)?shù)據(jù)點(diǎn)仍然存在較大偏差，在20～30 測(cè)量數(shù)據(jù)點(diǎn)之間俯仰角誤差達(dá)到0.85°。 用3.2 小節(jié)的未對(duì)準(zhǔn)策略，旋轉(zhuǎn)轉(zhuǎn)臺(tái)的X軸，讓Y軸上升20°，然后旋轉(zhuǎn)Y 軸估計(jì)旋轉(zhuǎn)矩陣的第二行元素，然后隨意旋轉(zhuǎn)平臺(tái)的三軸分別估計(jì)旋轉(zhuǎn)矩陣的第一行和第三行元素。 圖12 中補(bǔ)償結(jié)果明顯，俯仰角不存在較大的偏差值。 補(bǔ)償前俯仰角最大誤差為4.7°，補(bǔ)償后俯仰角最大誤差為0.39°，其精度提高了一個(gè)數(shù)量級(jí)。

4.5 橫滾角補(bǔ)償結(jié)果

圖13 轉(zhuǎn)臺(tái)水平旋轉(zhuǎn)橫滾角誤差

轉(zhuǎn)臺(tái)保持水平不變，讓轉(zhuǎn)臺(tái)Z軸旋轉(zhuǎn)一周取得62 個(gè)數(shù)據(jù)點(diǎn)，觀察加速度計(jì)解算橫滾角的情況。 如圖13 所示，橫滾角在其起始旋轉(zhuǎn)階段及結(jié)束旋轉(zhuǎn)階段偏差較大，最大偏差超過了0.3°。 在置于平臺(tái)俯仰角分別為30°(水平旋轉(zhuǎn)8 次)；60°(水平旋轉(zhuǎn)14次)；90°(水平旋轉(zhuǎn)16 次)；120°(水平旋轉(zhuǎn)14 次)；150°(水平旋轉(zhuǎn)8 次)；180°(水平旋轉(zhuǎn)1 次)時(shí)分別記錄橫滾角輸出(如圖14 所示)，橫滾角最大偏差達(dá)超過了0.9°，能夠說明俯仰角對(duì)橫滾角的解算是有影響的，尤其是未對(duì)準(zhǔn)誤差，經(jīng)過DA-LM 并加入未對(duì)準(zhǔn)補(bǔ)償?shù)那闆r如圖14。

圖14 補(bǔ)償前后橫滾角誤差

可以看出補(bǔ)償結(jié)果明顯，說明傳感器誤差能夠有效估計(jì)，可將橫滾角誤差控制在0.3°左右。 以均方根誤差(Root Mean Square Error，RMSE)衡量校正前后加速度計(jì)解算姿態(tài)角的偏差。

式中:wk代表實(shí)驗(yàn)中真實(shí)的俯仰角與橫滾角，Δ代表由三軸加速度計(jì)解算得到的俯仰角及橫滾角。

由表6 所示，本文整套算法優(yōu)于DA-LM 算法，證明未對(duì)準(zhǔn)對(duì)姿態(tài)的解算影響不小。 本文提出的整套算法相比于DA 估計(jì)精度提高了4 倍。

表6 不同補(bǔ)償下姿態(tài)角的均方根誤差

圖15 所示，校正后的數(shù)據(jù)點(diǎn)模值正常，同時(shí)也證明了本文算法的有效性。

圖15 校準(zhǔn)前后重力場(chǎng)模值比較

4.6 算法計(jì)算效率

由圖1，DA 中適應(yīng)度值計(jì)算至更新蜻蜓位置的迭代環(huán)節(jié)決定了該算法計(jì)算效率，最大迭代次數(shù)Max 越大算法運(yùn)行時(shí)間越長；LM 算法運(yùn)行效率的高低在于增益系數(shù)αk的計(jì)算、參數(shù)θk以及學(xué)習(xí)率λK更新，但LM 算法本身計(jì)算效率較高，且本文所用λK的更新策略極大提高了算法的收斂速度，故LM在計(jì)算效率方面要遠(yuǎn)高于DA。 采用雙軸轉(zhuǎn)臺(tái)實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證DA-LM 算法的計(jì)算效率，DA 最大迭代次數(shù)100，LM 算法最大迭代次數(shù)500，用MATLAB 中etime( )與clock 的組合代碼計(jì)算程序運(yùn)行時(shí)間。驗(yàn)證計(jì)算效率時(shí)不考慮未對(duì)準(zhǔn)策略，因?yàn)樵摬呗圆捎玫氖亲钚《斯烙?jì)，輸出結(jié)果很快，對(duì)本文整套算法的運(yùn)行時(shí)間無太大影響。

表7 所示，LM 算法相比于DA 其運(yùn)行效率超出60%，但DA-LM 算法整體運(yùn)行時(shí)間不超過3 s。

表7 不同算法運(yùn)行時(shí)間

5 結(jié)論

本文通過對(duì)加速度計(jì)的誤差建模，分析誤差參數(shù)與誤差方程的關(guān)系，提出DA-LM 算法。 首先利用DA對(duì)誤差參數(shù)的全局尋優(yōu)然后利用LM 算法局部尋優(yōu)獲得最優(yōu)值，通過估計(jì)旋轉(zhuǎn)矩陣實(shí)現(xiàn)加速度計(jì)坐標(biāo)系與載體坐標(biāo)系的對(duì)準(zhǔn)。 本文所提出對(duì)加速度計(jì)誤差的一種完整補(bǔ)償方案，從實(shí)驗(yàn)結(jié)果可以看出，加速度計(jì)對(duì)俯仰角與橫滾角的解算精度明顯提高，證明該方法在隨鉆測(cè)量系統(tǒng)中的應(yīng)用具有一定的參考性。