航姿參考系統(tǒng)的改進(jìn)桿臂效應(yīng)補(bǔ)償方法

譚強(qiáng)俊，程永生，唐 彬，劉顯學(xué)，周 浩，李寅鑫

(中國(guó)工程物理研究院 電子工程研究所,四川 綿陽(yáng) 621000)

由于加速度計(jì)的安裝位置很難與載體搖擺參考點(diǎn)相重合，在角運(yùn)動(dòng)環(huán)境下加速度計(jì)輸出會(huì)產(chǎn)生相對(duì)參考點(diǎn)的干擾加速度，即桿臂效應(yīng)[1-2].航姿參考系統(tǒng)(Attitude and Heading Reference System，AHRS)中加速度計(jì)測(cè)量重力加速度在載體系各坐標(biāo)軸上的投影分量從而獲取水平姿態(tài)角，同時(shí)修正陀螺漂移[3-5].在靜態(tài)條件下可獲得較高的姿態(tài)測(cè)量精度.但在動(dòng)態(tài)環(huán)境，疊加在加速度計(jì)敏感軸上的干擾加速度會(huì)對(duì)加速度計(jì)測(cè)姿精度造成極大影響.

本文基于AHRS系統(tǒng)，通過(guò)對(duì)桿臂效應(yīng)的誤差機(jī)理以及現(xiàn)有濾波、補(bǔ)償方法的不足進(jìn)行分析，提出了一種改進(jìn)的桿臂效應(yīng)補(bǔ)償方法，與現(xiàn)有方法相對(duì)比，顯著提高了桿臂效應(yīng)誤差補(bǔ)償精度.

1 桿臂效應(yīng)的誤差機(jī)理

定義慣性坐標(biāo)系(i系)為oixiyizi，載體坐標(biāo)系(b系)為obxbybzb，加速度計(jì)安裝在載體P處，ob點(diǎn)為載體的參考點(diǎn)，R為P點(diǎn)在慣性系中的位置矢量，R1為載體參考點(diǎn)ob在慣性系中的位置矢量，r為從載體參考點(diǎn)到P點(diǎn)的位置矢量，即為桿臂.位置示意圖如圖1所示.

在角運(yùn)動(dòng)環(huán)境下，由于載體參考點(diǎn)ob與加速度計(jì)安裝點(diǎn)P不重合，在加速度計(jì)的輸出中就含有相對(duì)參考點(diǎn)的桿臂效應(yīng)誤差，如式(1)所示

(1)

桿臂效應(yīng)引起的干擾加速度表示為

(2)

其中第1項(xiàng)定義為桿臂效應(yīng)的向心加速度項(xiàng)，第2項(xiàng)定義為桿臂效應(yīng)的切向加速度項(xiàng).

圖1 坐標(biāo)系間矢量關(guān)系

桿臂效應(yīng)對(duì)加速度計(jì)測(cè)姿的影響航姿參考系統(tǒng)中加速度計(jì)通過(guò)測(cè)量重力加速度在載體系各坐標(biāo)軸上的投影分量來(lái)獲取水平姿態(tài)角，以y軸為例

(3)

由式(2)、(3)可以得到，桿臂效應(yīng)造成的橫滾角誤差Δγ為

(4)

在橫滾角γ=0°時(shí)，由Δf1,Δf2分別造成的橫滾角誤差Δγ1,Δγ2如圖2所示.

圖2中，Δγ1為向心加速度造成的姿態(tài)誤差；Δγ2為切向加速度造成的姿態(tài)誤差.由圖2可見(jiàn)隨著角速度、角加速度以及桿臂長(zhǎng)度的增加，桿臂效應(yīng)相應(yīng)增大，加速度計(jì)測(cè)姿誤差也越大，只有通過(guò)對(duì)加速度計(jì)桿臂效應(yīng)進(jìn)行有效的補(bǔ)償，才能保證AHRS系統(tǒng)測(cè)姿精度.

圖2 桿臂效應(yīng)造成的測(cè)姿誤差

2 現(xiàn)有桿臂效應(yīng)補(bǔ)償方法的不足

目前處理?xiàng)U臂效應(yīng)的解析方法主要為低通濾波法和力學(xué)補(bǔ)償法.在實(shí)際工程中二者應(yīng)用廣泛，但也存在其自身的不足.

2.1 低通濾波法的不足

從現(xiàn)有文獻(xiàn)看，采用低通濾波法主要應(yīng)用在初始對(duì)準(zhǔn)場(chǎng)景中.此時(shí)載體一般沒(méi)有大的角機(jī)動(dòng)，只有微幅晃動(dòng)(振動(dòng)).該環(huán)境下的干擾頻率遠(yuǎn)高于舒拉頻率以及地球自轉(zhuǎn)頻率，可視為高頻干擾[6].但是低通濾波無(wú)法濾除振動(dòng)產(chǎn)生的直流分量.

設(shè)在振動(dòng)環(huán)境下，載體沿俯仰軸、橫滾軸有頻率相同，相位差為90°的角振動(dòng)，即

(5)

式中：Ω為振動(dòng)的角頻率，θ為俯仰角，θm為俯仰角的振動(dòng)幅值，γ為橫滾角，γm為橫滾角的振動(dòng)幅值，ψ為方位角.

由歐拉角微分方程，振動(dòng)環(huán)境下的角速度為

(6)

在微幅振動(dòng)環(huán)境下，可以把θ,γ當(dāng)作小角度處理，即

(7)

(8)

由式(8)可見(jiàn)，振動(dòng)環(huán)境下向心加速度項(xiàng)存在直流分量，依靠低通濾波無(wú)法將直流分量濾除.

對(duì)式(6)求角加速度，可得

在振動(dòng)環(huán)境下角加速度仍為振蕩形式，通過(guò)低通濾波可濾除切向加速度.但在機(jī)動(dòng)環(huán)境下，若角加速度也包含有直流項(xiàng)時(shí)，低通濾波無(wú)法濾除角加速度直流項(xiàng).

2.2 力學(xué)補(bǔ)償法的不足

(9)

(10)

var(a)=2Qω/ΔT2.

(11)

3 一種改進(jìn)的桿臂效應(yīng)補(bǔ)償方法

利用低通濾波法，桿臂效應(yīng)中的向心加速度項(xiàng)會(huì)殘存整流誤差項(xiàng)；再者當(dāng)角加速度也含有直流分量時(shí)，桿臂效應(yīng)中的切向加速度項(xiàng)也會(huì)出現(xiàn)低通濾波無(wú)法濾除的整流誤差.采用力學(xué)補(bǔ)償法，向心加速度項(xiàng)的求解誤差會(huì)受到陀螺測(cè)量誤差的影響；而通過(guò)角速度微分獲取角加速度，會(huì)引起切向加速度項(xiàng)誤差激劇放大.考慮到多加速度計(jì)可以直接解析地求解角加速度[17-18]，可避免角速度微分計(jì)算造成的誤差放大.因此綜合考慮低通濾波法、力學(xué)補(bǔ)償法以及多加速度計(jì)解析求解法，提出一種改進(jìn)的桿臂效應(yīng)補(bǔ)償方法見(jiàn)圖3.對(duì)于桿臂效應(yīng)中的向心加速度，在低于給定角頻率閾值工況下采用低通濾波法，在高于給定角頻率閾值工況下采用力學(xué)補(bǔ)償+低通濾波法.對(duì)桿臂效應(yīng)中的切向加速度項(xiàng)，通過(guò)多加速度計(jì)解析求解角加速度.

圖3 改進(jìn)的桿臂效應(yīng)補(bǔ)償方法

3.1 基于角頻率閾值判決的向心加速度項(xiàng)補(bǔ)償

(12)

則根據(jù)式(12)，經(jīng)過(guò)小波去噪后，可認(rèn)為

(13)

(14)

將式(7)代入式(14)，即

(15)

(16)

將式(7)代入式(16)，即

(17)

(18)

將式(7)代入式(18)，即

(19)

設(shè)角頻率閾值Ω0為

Ω0=8|b|/(3γmθm)，

(20)

當(dāng)Ω≥Ω0時(shí)，有

(21)

而在Ω<Ω0時(shí)，有

(22)

由式(21)、(22)可見(jiàn)，當(dāng)角頻率Ω大于角頻率閾值Ω0時(shí)，采用力學(xué)補(bǔ)償+低通濾波法造成的殘存整流誤差小于直接采用低通濾波造成的整流誤差；而在角頻率Ω小于角頻率閾值Ω0時(shí)，直接采用低通濾波造成的整流誤差小于采用力學(xué)補(bǔ)償+低通濾波法造成的殘存整流誤差.

3.2 利用多加速度計(jì)補(bǔ)償桿臂效應(yīng)切向加速度項(xiàng)

因微分法求解角加速度會(huì)放大誤差，而低通濾波無(wú)法濾除直流分量，因此提出兩種利用多加速度計(jì)解析求解角加速度的改進(jìn)方法.

3.2.1 6加速度計(jì)構(gòu)型1

設(shè)置6加速度計(jì)構(gòu)型，如圖4所示.6個(gè)加速度計(jì)安裝在立方體6個(gè)面的中心，其敏感軸沿著每個(gè)面的對(duì)角線(xiàn).令θb表示加速度計(jì)在載體系中敏感方向矢量，在6加速度計(jì)構(gòu)型其中心位于P點(diǎn)，立方體的邊長(zhǎng)為2l時(shí)，由式(1)可得單個(gè)加速度計(jì)的輸出表達(dá)式為

(23)

圖4 六加速度計(jì)構(gòu)型1

該構(gòu)型方案的安裝參數(shù)矩陣

將安裝參數(shù)矩陣代入式(23)，可得角加速度

(24)

(25)

3.2.2 6加速度計(jì)構(gòu)型2

構(gòu)型1中加速度計(jì)的布局相對(duì)6個(gè)基準(zhǔn)面為傾斜安裝.文獻(xiàn)[20]指出斜置IMU，存在大安裝誤差角，安裝誤差角二次項(xiàng)的影響不能忽略.文獻(xiàn)[21]指出斜裝形式下的標(biāo)定需要：1)設(shè)計(jì)傾斜的標(biāo)定工裝；2)設(shè)計(jì)專(zhuān)門(mén)的斜裝標(biāo)定算法.由此可見(jiàn)構(gòu)型1中加速度計(jì)的布局對(duì)實(shí)際安裝以及標(biāo)定都帶來(lái)一定的困難.文獻(xiàn)[22]指出正交雙加速度計(jì)可消除安裝誤差角影響.文獻(xiàn)[23]中給出了一種便于安裝的6加速度計(jì)構(gòu)型，但該方案將多個(gè)加速度計(jì)安裝在同一點(diǎn)，若采用體積較大的石英撓性加速度計(jì)，會(huì)產(chǎn)生較大的安裝誤差.為此設(shè)計(jì)了另一種構(gòu)型：6加速度計(jì)構(gòu)型2，如圖5所示.將各加速度計(jì)的安裝位置移至各基準(zhǔn)面的中心，避免了將多個(gè)加速度計(jì)安裝在同一點(diǎn)造成的安裝誤差.各加速度計(jì)距離質(zhì)心的距離均為l.同時(shí)構(gòu)型2不存在大安裝誤差角問(wèn)題.

圖5 6加速度計(jì)構(gòu)型2

該構(gòu)型方案的安裝參數(shù)矩陣

代入式(23)，可得角加速度為

(26)

(27)

相比構(gòu)型1，構(gòu)型2在振動(dòng)環(huán)境下會(huì)有整流殘差，且隨著角頻率、振幅的增加，其殘留整流誤差也隨之增大.對(duì)比式(24)及式(26)，在角加速度項(xiàng)含有直流分量的情況下，構(gòu)型1能很好地補(bǔ)償切向加速度，而構(gòu)型2求解角加速度的誤差項(xiàng)中含有角速度信息，也即是構(gòu)型2求解角加速度的誤差會(huì)隨著角速度的增大而增大.

4 仿真驗(yàn)證

4.1 桿臂效應(yīng)向心加速度項(xiàng)補(bǔ)償

由圖6可見(jiàn)，對(duì)于向心加速度的補(bǔ)償，在角頻率大于角頻率閾值時(shí)，改進(jìn)算法的殘存整流誤差小于低通濾波法殘存整流誤差.隨著振幅增大，角頻率閾值迅速減小，也即是在大振幅工況下，改進(jìn)算法遠(yuǎn)勝于低通濾波法.

圖6 改進(jìn)算法與低通濾波算法的比較

Fig.6 Comparison between improved algorithm and low-pass filtering algorithm

4.2 桿臂效應(yīng)切向加速度項(xiàng)補(bǔ)償

在實(shí)際工程上應(yīng)用的MEMS加速度計(jì)其精度已達(dá)到1×10-4g[19].因此在仿真中設(shè)定加速度計(jì)常值漂移服從Gauss分布，均方差為10-4g.

4.2.1 非振動(dòng)低動(dòng)態(tài)環(huán)境

4.2.2 非振動(dòng)高動(dòng)態(tài)環(huán)境

從圖7可見(jiàn)，構(gòu)型1解析求解的切向加速度比直接微分法的精度提高了1個(gè)數(shù)量級(jí).構(gòu)型2的求解誤差隨著角速度的增大而增大，但在低動(dòng)態(tài)環(huán)境下構(gòu)型2的求解誤差是可以接受的.在仿真650 s時(shí)，構(gòu)型2的求解誤差達(dá)到-0.042(m/s2)，與直接微分法的求解誤差相仿，而此時(shí)角速度達(dá)到325(°/s).也即是除了高動(dòng)態(tài)環(huán)境下，一般的工程應(yīng)用也可采用構(gòu)型2.

4.2.3 振動(dòng)環(huán)境

設(shè)定振動(dòng)頻率范圍為0～20 Hz，俯仰角的振幅θm與橫滾角的振幅γm設(shè)為10°，仿真時(shí)間為40 s，兩種加速度計(jì)構(gòu)型在振動(dòng)環(huán)境下的求解桿臂效應(yīng)的切向加速度項(xiàng)誤差如圖9所示(以x軸角加速度為例)

從圖9可看到，振動(dòng)頻率對(duì)構(gòu)型1影響較小，而構(gòu)型2的切向加速度誤差隨著振動(dòng)頻率增加而增大.

圖7 非振動(dòng)環(huán)境下切向加速度誤差對(duì)比

Fig.7 Comparison of tangential acceleration errors in non-vibrating environment

圖8 振動(dòng)環(huán)境下切向加速度誤差對(duì)比

Fig.8 Comparison of tangential acceleration errors in vibrating environment

結(jié)合圖7、圖8可看到，無(wú)論是振動(dòng)環(huán)境或非振動(dòng)環(huán)境，低動(dòng)態(tài)或高動(dòng)態(tài)，構(gòu)型1的性能總是優(yōu)于構(gòu)型2.但是構(gòu)型1的安裝方式為斜置安裝，需要設(shè)計(jì)專(zhuān)用的標(biāo)定工裝以及斜裝標(biāo)定算法，給工程實(shí)現(xiàn)帶來(lái)一定的難度.構(gòu)型2為正交安裝，可以利用重力加速度的幅值平方g2作為參考基準(zhǔn)，在標(biāo)定過(guò)程中不受安裝誤差角的影響.同時(shí)無(wú)須設(shè)計(jì)專(zhuān)用標(biāo)定工裝，給工程實(shí)現(xiàn)帶來(lái)一定便利.

實(shí)際工程中，慣性器件經(jīng)過(guò)一定時(shí)間的使用其自身性能參數(shù)會(huì)隨時(shí)間的推移與應(yīng)用環(huán)境的變化而發(fā)生改變，同時(shí)在使用過(guò)程中由于摩擦、振動(dòng)等因素的影響，慣性器件的安裝位置、安裝誤差角也可能發(fā)生變化，需重新標(biāo)定才能滿(mǎn)足使用要求.因此對(duì)現(xiàn)場(chǎng)無(wú)精密標(biāo)定設(shè)備、且工況為低振動(dòng)、低動(dòng)態(tài)環(huán)境，優(yōu)先采用構(gòu)型2.而高振動(dòng)、高動(dòng)態(tài)環(huán)境優(yōu)先采用構(gòu)型1.

4.2.4 內(nèi)桿臂對(duì)多加速度計(jì)測(cè)姿的影響

由式(24)、式(26)可看到，多加速度計(jì)的內(nèi)桿臂長(zhǎng)度越長(zhǎng)，多加速度計(jì)測(cè)姿精度越高.以非振動(dòng)低動(dòng)態(tài)環(huán)境下測(cè)姿為例，內(nèi)桿臂l長(zhǎng)度范圍為1 cm～4 cm，構(gòu)型1和構(gòu)型2的切向加速度誤差如圖9所示.

圖9 內(nèi)桿臂長(zhǎng)度對(duì)加速度計(jì)測(cè)姿的影響

Fig.9 Influence of inner lever arm on accelerometer attitude measurement

從圖9可以看到構(gòu)型1和構(gòu)型2內(nèi)桿臂l越大，求解的角加速度誤差越小.但是在實(shí)際工程中，儀器安裝的空間總是有限的，為提高求解精度在有限的安裝空間里應(yīng)盡可能增大內(nèi)桿臂長(zhǎng)度.

5 結(jié) 論

本文研究了桿臂效應(yīng)誤差對(duì)加速度計(jì)測(cè)姿精度的影響，針對(duì)低通濾波法與力學(xué)補(bǔ)償法各自的不足，提出了一種改進(jìn)的桿臂效應(yīng)誤差補(bǔ)償方案.1)對(duì)向心加速度項(xiàng)，以振動(dòng)角頻率為判別標(biāo)準(zhǔn)，在低于給定角頻率閾值時(shí)采用低通濾波法，在高于給定角頻率閾值時(shí)采用力學(xué)補(bǔ)償+低通濾波法.與傳統(tǒng)的低通濾波法對(duì)比，改進(jìn)算法顯著提高了向心加速度項(xiàng)的補(bǔ)償精度.2)對(duì)切向加速度項(xiàng)，設(shè)計(jì)了兩種多加速度計(jì)構(gòu)型方式，通過(guò)多加速度計(jì)解析求解角加速度，避免了角速度直接微分造成的誤差放大.3)對(duì)加速度計(jì)構(gòu)型1和構(gòu)型2在三種工況環(huán)境下的求解精度進(jìn)行了分析，同時(shí)根據(jù)構(gòu)型1、構(gòu)型2標(biāo)定參數(shù)的難易，確定了構(gòu)型1和構(gòu)型2的適用范圍.