角速率輸入下的雙子樣二次迭代圓錐補(bǔ)償算法

任雪佳，丁立波，馮 陽(yáng)，路 坦

(1 南京理工大學(xué)機(jī)械工程學(xué)院，江蘇 南京 210094；2 安徽湘凡科技有限公司，安徽 合肥 231200)

0 引言

捷聯(lián)慣性導(dǎo)航具有結(jié)構(gòu)簡(jiǎn)單、功耗低、可靠性高等優(yōu)點(diǎn),對(duì)于單兵彈藥,考慮到低成本與高精度的要求,能夠滿足其導(dǎo)航需求。研究單兵彈藥純慣性導(dǎo)航的姿態(tài)解算,可以在較低成本下提高單兵作戰(zhàn)的打擊能力,具有較高的實(shí)際應(yīng)用價(jià)值。

姿態(tài)更新解算作為捷聯(lián)慣導(dǎo)系統(tǒng)的核心,已進(jìn)行了大量的研究,姿態(tài)解算方法主要有:四元數(shù)法、旋轉(zhuǎn)矢量法、歐拉角法、方向余弦法和羅德利格參數(shù)法。1971年Bortz[1]提出的旋轉(zhuǎn)矢量微分方程,為補(bǔ)償載體在高動(dòng)態(tài)環(huán)境下的圓錐誤差提供了理論基礎(chǔ)?；诖?國(guó)內(nèi)外學(xué)者對(duì)基于旋轉(zhuǎn)矢量的圓錐補(bǔ)償算法進(jìn)行了大量的研究,可概括為以下幾個(gè)方面:增加子樣數(shù)或前次周期項(xiàng)信息的圓錐算法系數(shù)設(shè)計(jì)[2-5]、通過(guò)補(bǔ)償截?cái)嗾`差提高精度[6-7]、對(duì)周期項(xiàng)進(jìn)行補(bǔ)償[8]和圓錐算法結(jié)構(gòu)設(shè)計(jì)[9]。但這些圓錐補(bǔ)償算法都是以角增量作為輸入,而目前廣泛使用的光纖陀螺、微機(jī)械陀螺等輸出為角速度,傳統(tǒng)的圓錐補(bǔ)償算法無(wú)法直接使用。

對(duì)于角速率輸入,通常采用分段線性化,認(rèn)為每一個(gè)時(shí)間段ΔT內(nèi)的角速度為常量,由Δθ=ω·ΔT來(lái)計(jì)算角增量,不能有效地提高姿態(tài)角度精度[10]。黃昊、杜燁等[11-12]利用梯形公式、辛普森公式或其他數(shù)值積分方式得到角增量,代入到傳統(tǒng)圓錐補(bǔ)償算法會(huì)使補(bǔ)償精度下降。針對(duì)以上問(wèn)題,曾慶化等[13]推導(dǎo)出一類具有純角速率輸入的圓錐算法;Ben等[14]將Miller的思想引入圓錐算法設(shè)計(jì),提出一種以角速率作為算法輸入的改進(jìn)錐面算法的廣義方法;劉佳樂(lè)[15]直接利用角速率推導(dǎo)了雙子樣算法和加入前次周期信息的改進(jìn)算法,測(cè)量精度得到了提高;湯傳業(yè)等[16]在計(jì)算優(yōu)化系數(shù)時(shí)采用最小二乘法,進(jìn)一步提高了算法精度;盧世昕等[17]在前一周期的基礎(chǔ)上對(duì)周期項(xiàng)進(jìn)行了二次優(yōu)化;丁鵬等[18]對(duì)單軸旋轉(zhuǎn)捷聯(lián)慣導(dǎo)系統(tǒng),分析了基于角速率的多子樣旋轉(zhuǎn)矢量算法;Ding等[19]針對(duì)圓錐誤差中的高階補(bǔ)償項(xiàng),在計(jì)算等效旋轉(zhuǎn)矢量時(shí)補(bǔ)償了圓錐修正項(xiàng)和三交叉乘積項(xiàng);邢麗等[20]建立了推力速度坐標(biāo)系并引入等效螺旋矢量,并采用梯形數(shù)字積分方法計(jì)算等效螺旋矢量,對(duì)圓錐和劃船誤差同時(shí)進(jìn)行了補(bǔ)償。

目前,圓錐算法結(jié)構(gòu)設(shè)計(jì)的研究分為雙速結(jié)構(gòu)、單速結(jié)構(gòu)兩類[20]。在分析角速率輸入的雙子樣單速算法基礎(chǔ)上,文中引入雙速回路,研究了雙子樣的二次迭代算法,在推導(dǎo)優(yōu)化系數(shù)時(shí)進(jìn)行簡(jiǎn)化,減少了計(jì)算量。在經(jīng)典圓錐運(yùn)動(dòng)環(huán)境下,對(duì)算法的可行性和有效性進(jìn)行了仿真驗(yàn)證,仿真結(jié)果表明,該算法對(duì)提高捷聯(lián)慣導(dǎo)系統(tǒng)的圓錐誤差計(jì)算精度具有積極作用。

1 經(jīng)典圓錐運(yùn)動(dòng)

基于圓錐誤差可以由旋轉(zhuǎn)矢量的微分方程表示,工程中經(jīng)常通過(guò)分析旋轉(zhuǎn)矢量的微分方程來(lái)設(shè)計(jì)算法。等效旋轉(zhuǎn)矢量Φ表征載體系從tk-1至tk時(shí)刻的等效旋轉(zhuǎn),其微分方程可近似表示為[1]:

(1)

式中:ω為tk-1至tk時(shí)間段內(nèi)的機(jī)體角速率;Φ為載體系由tk-1至tk時(shí)刻角位置變化所對(duì)應(yīng)的等效旋轉(zhuǎn)矢量。

假設(shè)t時(shí)刻載體僅存在錐運(yùn)動(dòng),旋轉(zhuǎn)矢量為[21]:

(2)

式中:α為錐運(yùn)動(dòng)的半錐角;Ω為錐運(yùn)動(dòng)的頻率。

對(duì)應(yīng)的錐運(yùn)動(dòng)四元數(shù)Q(t)為:

(3)

設(shè)從t至(t+h)時(shí)刻載體系的更新四元數(shù)為q(h),旋轉(zhuǎn)四元數(shù)關(guān)系為[21]:

Q(t+h)=Q(t)?q(h)

(4)

即有

(5)

式中:h為姿態(tài)更新周期,其值為tk-1-tk。

(6)

2 基于角速率的二次迭代優(yōu)化算法

2.1 雙子樣二次迭代算法推導(dǎo)

相比一次迭代的圓錐補(bǔ)償算法,二次迭代算法是將姿態(tài)更新周期分為兩個(gè)迭代周期,每個(gè)迭代周期內(nèi)有N個(gè)角增量擬合采樣點(diǎn)。對(duì)于角速率的雙子樣二次迭代算法,設(shè)姿態(tài)更新周期為h,迭代周期為T,采樣周期為t,則有以下關(guān)系:

h=2T=4t

(7)

第一個(gè)迭代周期T1為[tk-1,tk-1+T],第二個(gè)迭代周期T2為[tk-1+T,tk-1+2T]。

在迭代周期T1內(nèi),假設(shè)載體系的角速率用二次曲線表示為:

ω(tk-1+τ)=a1+2b1τ+3c1τ2

(8)

式中:a1,b1,c1為多項(xiàng)式系數(shù);0≤τ≤T。

將T1內(nèi)的等效旋轉(zhuǎn)矢量Φ(tk-1+T)作泰勒級(jí)數(shù)展開(kāi):

(9)

(10)

反解出的多項(xiàng)式系數(shù)可表示為:

(11)

將式(8)對(duì)τ求各階導(dǎo)數(shù):

(12)

其中i≥3。聯(lián)立式(1)和式(12)可得:

(13)

其中i≥6。代入式(9)得:

(14)

將式(11)代入式(14)可得到T1內(nèi)的旋轉(zhuǎn)矢量:

(15)

在迭代周期T2內(nèi),假設(shè)載體系的角速率用拋物線表示為:

ω(tk-1+T+τ)=a2+2b2τ+3c2τ2

(16)

式中:a2,b2,c2為多項(xiàng)式系數(shù);0≤τ≤T。

對(duì)應(yīng)的旋轉(zhuǎn)矢量用泰勒級(jí)數(shù)展開(kāi):

(17)

(18)

2.2 系數(shù)優(yōu)化

上面推導(dǎo)出的角速率交叉項(xiàng)系數(shù)是以拋物線擬合角速度得出的,但實(shí)際角速度不一定如此,因此需對(duì)這些系數(shù)進(jìn)行修正以保證擬合所得的算法漂移最小。

當(dāng)系統(tǒng)繞x軸作圓錐運(yùn)動(dòng)時(shí),設(shè)

(19)

式中:k1,k2,k3,k4為待定優(yōu)化系數(shù),使得在圓錐運(yùn)動(dòng)環(huán)境下的算法漂移最小?？紤]到計(jì)算量和工程化,圓錐誤差項(xiàng)僅計(jì)算3個(gè)叉乘項(xiàng)足以保證算法的精度,過(guò)多反而增加計(jì)算負(fù)擔(dān)且精度提升不明顯,因此將式(19)簡(jiǎn)化為:

(20)

僅考慮x軸直流分量,將式(6)代入式(20),得

(21)

根據(jù)精度準(zhǔn)則[2],有:

(22)

(23)

由于Ωh遠(yuǎn)小于1,令(ΩT)3、(ΩT)5和(ΩT)7項(xiàng)系數(shù)為零,解得:

(24)

綜上所述,基于角速率的雙子樣二次迭代算法可表示為:

(25)

算法漂移為:

(26)

3 仿真實(shí)驗(yàn)與分析

把典型圓錐運(yùn)動(dòng)直接作為測(cè)試輸入環(huán)境,將傳統(tǒng)雙子樣算法(Method 1)[21]、傳統(tǒng)雙子樣二次迭代算法(Method 2)[22]、基于角速率的雙子樣優(yōu)化算法(Method 3)[15]、基于角速率的三子樣優(yōu)化算法(Method 4)[15]、與文中算法(Method 5)進(jìn)行比較,驗(yàn)證文中提出的姿態(tài)算法的性能。

設(shè)置3組仿真條件:1)半錐角α為1°,圓錐運(yùn)動(dòng)頻率f為1 Hz,姿態(tài)更新周期h為24 ms,仿真總時(shí)長(zhǎng)為10 s。2)半錐角α為1°,圓錐運(yùn)動(dòng)頻率f為10 Hz,姿態(tài)更新周期h為2.4 ms,仿真總時(shí)長(zhǎng)為10 s。姿態(tài)角解算誤差如圖1～圖2所示;仿真時(shí)間內(nèi)Method 1～Method 5在兩種圓錐運(yùn)動(dòng)環(huán)境下的最大姿態(tài)角誤差分別如表1～表2所示。3)半錐角α為1°,陀螺采樣間隔為2 ms,改變圓錐運(yùn)動(dòng)頻率,對(duì)Method 2和 Method 5兩種算法在x軸上的俯仰角誤差的最大值進(jìn)行仿真,結(jié)果如表3所示。

圖1 各算法在條件1下的姿態(tài)角誤差Fig.1 Attitude angle error of each algorithm under condition 1

圖2 各算法在條件2下的姿態(tài)角誤差Fig.2 Attitude angle error of each algorithm under condition 2

表1 條件1下各算法姿態(tài)角誤差最大值

表2 條件2下各算法姿態(tài)角誤差最大值

對(duì)比條件1和條件2可知,在不考慮陀螺采樣上限的情況下,隨著圓錐運(yùn)動(dòng)頻率的增大,姿態(tài)更新周期相應(yīng)減小,算法的精度相差僅一個(gè)數(shù)量級(jí),驗(yàn)證了算法的可行性和有效性,說(shuō)明文中算法能在高動(dòng)態(tài)環(huán)境下保證算法性能。

對(duì)比Method 1和Method 3,Method 2和Method 5可看出,在相同仿真條件下,當(dāng)陀螺輸出為角速率時(shí),采用梯形公式或辛普森公式等提取角增量再代入傳統(tǒng)的旋轉(zhuǎn)矢量算法,不僅在精度上存在局限,而且求出的角增量之和存在誤差,基于角速率的圓錐補(bǔ)償算法能夠明顯提高計(jì)算精度。對(duì)比Method 4和Method 5可看出,相比基于角速率的雙子樣和三子樣的一次迭代算法,文中提出的改進(jìn)算法精度更高,且叉乘運(yùn)算量和三子樣優(yōu)化算法相同,使姿態(tài)精度和穩(wěn)定性得到了進(jìn)一步的改善。

為了更加清晰和明確地說(shuō)明文中算法與傳統(tǒng)算法相比較的優(yōu)勢(shì),表3統(tǒng)計(jì)了Method 2和Method 5兩種算法在相同圓錐條件下,姿態(tài)解算誤差的最大值對(duì)比結(jié)果。

表3 2 ms采樣間隔下x軸上俯仰角誤差最大值

從表3中可以看出,隨著圓錐運(yùn)動(dòng)頻率的增大,導(dǎo)航解算誤差的最大值均小于傳統(tǒng)算法1～2個(gè)數(shù)量級(jí),即載體的運(yùn)動(dòng)幅值變大時(shí),文中算法的姿態(tài)解算精度更優(yōu)于傳統(tǒng)算法。

4 結(jié)論

為解決傳統(tǒng)姿態(tài)算法在角速率輸入時(shí)計(jì)算精度較低、不能有效補(bǔ)償圓錐誤差的問(wèn)題,文中在雙子樣一次迭代的基礎(chǔ)上,提出了采用雙子樣二次迭代的旋轉(zhuǎn)矢量姿態(tài)算法,推導(dǎo)了能夠滿足工程運(yùn)用的算法簡(jiǎn)化表達(dá)式。仿真結(jié)果表明,提出的姿態(tài)算法與傳統(tǒng)算法、基于角速率的雙子樣和三子樣的一次迭代算法相比,具有更高的姿態(tài)解算精度,且叉乘運(yùn)算量和三子樣優(yōu)化算法相同。故提出的角速率輸入下二次迭代的雙子樣旋轉(zhuǎn)矢量姿態(tài)算法具有一定的應(yīng)用價(jià)值。