基于反向姿態(tài)跟蹤的自主式慣性導(dǎo)航行進(jìn)間初始對(duì)準(zhǔn)方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明提出基于反向姿態(tài)跟蹤的自主式慣性導(dǎo)航行進(jìn)間初始對(duì)準(zhǔn)方法，它涉及一 種慣性導(dǎo)航行進(jìn)間初始對(duì)準(zhǔn)方法，屬于慣性導(dǎo)航初始對(duì)準(zhǔn)技術(shù)領(lǐng)域。 二、
【背景技術(shù)】
[0002] 慣性導(dǎo)航行進(jìn)間初始對(duì)準(zhǔn)是指載體在運(yùn)動(dòng)過程中完成慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的 技術(shù)，因此，它是動(dòng)基座初始對(duì)準(zhǔn)技術(shù)的一種。慣性導(dǎo)航行進(jìn)間初始對(duì)準(zhǔn)技術(shù)對(duì)于增強(qiáng)載體 的機(jī)動(dòng)能力和快速反應(yīng)能力具有不可估量的意義和作用。因此，如何在載體運(yùn)動(dòng)過程中實(shí) 現(xiàn)初始對(duì)準(zhǔn)是一個(gè)值得研究的課題。
[0003] 與傳統(tǒng)的靜基座初始對(duì)準(zhǔn)環(huán)境不同，在載體運(yùn)動(dòng)狀態(tài)下，載體的位置、速度、加速 度以及角速度都在不斷地發(fā)生變化，其對(duì)初始對(duì)準(zhǔn)的影響主要具體表現(xiàn)為:一方面，線運(yùn)動(dòng) 會(huì)使慣性導(dǎo)航基本方程中的對(duì)地加速度、哥式加速度等參量時(shí)刻變化，因此在運(yùn)動(dòng)狀態(tài)下 無(wú)法利用加速度計(jì)輸出數(shù)據(jù)測(cè)得重力加速度的精確信息；另一方面，運(yùn)動(dòng)條件下載體振動(dòng) 使得干擾角速度具有很寬的頻帶，陀螺儀輸出信號(hào)信噪比較低，無(wú)法從陀螺儀輸出數(shù)據(jù)中 將地球自轉(zhuǎn)角速度這一對(duì)準(zhǔn)的有用信息提取出來(lái)。
[0004] 可見，在載體運(yùn)動(dòng)條件下，就不能單純依靠陀螺儀和加速度計(jì)的直接測(cè)量信息進(jìn) 行初始對(duì)準(zhǔn)，而需要引入測(cè)距或測(cè)速信息，以補(bǔ)償運(yùn)動(dòng)過程中有害加速度對(duì)初始對(duì)準(zhǔn)精度 的影響。目前，慣性導(dǎo)航行進(jìn)間對(duì)準(zhǔn)方法目前主要有捷聯(lián)羅經(jīng)法、慣性系對(duì)準(zhǔn)方法以及最優(yōu) 估計(jì)對(duì)準(zhǔn)方法等。捷聯(lián)羅經(jīng)法應(yīng)用了成熟的經(jīng)典控制理論方法實(shí)現(xiàn)行進(jìn)間初始對(duì)準(zhǔn)，原理 簡(jiǎn)單但對(duì)準(zhǔn)時(shí)間較長(zhǎng)且對(duì)陀螺的低頻干擾較為敏感，需要根據(jù)運(yùn)動(dòng)環(huán)境選取合適的控制參 數(shù)。慣性系對(duì)準(zhǔn)方法以慣性空間為中間過渡坐標(biāo)系，隔離載體角運(yùn)動(dòng)對(duì)初始對(duì)準(zhǔn)的干擾，但 該方法僅對(duì)測(cè)量誤差做簡(jiǎn)單處理，因而對(duì)準(zhǔn)精度不高且不能獲取載體的位置信息。最優(yōu)估 計(jì)對(duì)準(zhǔn)方法建立起慣性導(dǎo)航誤差方程，利用里程計(jì)等測(cè)速傳感器的測(cè)速信息作為量測(cè)信息 進(jìn)行卡爾曼(Ka lman)濾波，估計(jì)出平臺(tái)失準(zhǔn)角等關(guān)鍵誤差從而實(shí)現(xiàn)行進(jìn)間初始對(duì)準(zhǔn)。該方 法多是基于慣性導(dǎo)航線性化誤差模型的，需要先在靜止條件下獲取粗略的初始姿態(tài)矩陣才 可進(jìn)行，因此，在一定程度上削弱了載體的機(jī)動(dòng)性優(yōu)勢(shì)。
[0005] 由此可見，各方法各有其特點(diǎn)和適用性。為了既能在運(yùn)動(dòng)過程中獲取粗略姿態(tài)矩 陣，又能實(shí)現(xiàn)高精度姿態(tài)對(duì)準(zhǔn)和位置導(dǎo)航，本發(fā)明單純以里程計(jì)速度采樣數(shù)據(jù)為對(duì)準(zhǔn)輔助 信息提出一種基于反向姿態(tài)跟蹤的自主式慣性導(dǎo)航行進(jìn)間對(duì)準(zhǔn)方法。該方法在僅知道初始 位置的條件下，即可實(shí)現(xiàn)高精度的慣性導(dǎo)航行進(jìn)間初始對(duì)準(zhǔn)。 三、

【發(fā)明內(nèi)容】

[0006] 針對(duì)現(xiàn)有技術(shù)中存在的問題，本發(fā)明提出基于反向姿態(tài)跟蹤的自主式慣性導(dǎo)航行 進(jìn)間初始對(duì)準(zhǔn)方法。首先在載體運(yùn)動(dòng)過程中，利用里程計(jì)的測(cè)速信息為輔助進(jìn)行粗對(duì)準(zhǔn)獲 取粗略姿態(tài)矩陣，同時(shí)保存慣性測(cè)量元件(包括陀螺儀和加速度計(jì))和里程計(jì)的采樣數(shù)據(jù)。 然后進(jìn)行反向姿態(tài)跟蹤求得初始對(duì)準(zhǔn)開始時(shí)刻的姿態(tài)矩陣。最后在此基礎(chǔ)上，利用保存的 慣性測(cè)量元件采樣數(shù)據(jù)和里程計(jì)速度采樣數(shù)據(jù)進(jìn)行Kalman濾波精對(duì)準(zhǔn)。最終實(shí)現(xiàn)高精度的 姿態(tài)對(duì)準(zhǔn)和位置導(dǎo)航。
[0007] 本發(fā)明所提出的基于反向姿態(tài)跟蹤的自主式慣性導(dǎo)航行進(jìn)間初始對(duì)準(zhǔn)方法，用于 車載慣性導(dǎo)航系統(tǒng)，該系統(tǒng)包括傳感器子系統(tǒng)、數(shù)據(jù)存儲(chǔ)模塊、粗對(duì)準(zhǔn)計(jì)算模塊、反向姿態(tài) 跟蹤計(jì)算模塊和精對(duì)準(zhǔn)計(jì)算模塊。它們之間的關(guān)系是:傳感器子系統(tǒng)的采樣數(shù)據(jù)分別傳遞 給數(shù)據(jù)存儲(chǔ)模塊和粗對(duì)準(zhǔn)計(jì)算模塊;粗對(duì)準(zhǔn)計(jì)算模塊計(jì)算出粗略初始姿態(tài)矩陣并將其傳遞 給反向姿態(tài)跟蹤計(jì)算模塊;反向姿態(tài)跟蹤計(jì)算模塊利用數(shù)據(jù)存儲(chǔ)模塊的傳感器數(shù)據(jù)進(jìn)行反 向姿態(tài)跟蹤，并將其計(jì)算結(jié)果傳遞給精對(duì)準(zhǔn)計(jì)算模塊。精對(duì)準(zhǔn)計(jì)算模塊再利用數(shù)據(jù)存儲(chǔ)模 塊的數(shù)據(jù)進(jìn)行精對(duì)準(zhǔn)計(jì)算獲取精確對(duì)準(zhǔn)姿態(tài)矩陣和位置導(dǎo)航結(jié)果。
[0008] 本發(fā)明提出基于反向姿態(tài)跟蹤的自主式慣性導(dǎo)航行進(jìn)間初始對(duì)準(zhǔn)方法，具體包括 以下步驟：
[0009]步驟一:建立過渡參考坐標(biāo)系；
[0010] 整個(gè)對(duì)準(zhǔn)算法建立的四個(gè)重要的過渡參考坐標(biāo)系為初始時(shí)刻慣性坐標(biāo)系（i〇系）、 初始時(shí)刻地球坐標(biāo)系（eo系）、初始時(shí)刻導(dǎo)航坐標(biāo)系(no系）和初始時(shí)刻載體坐標(biāo)系（ib0系）， 其定義如下：
[0011] (1)初始時(shí)刻慣性坐標(biāo)系（io系）
[0012]初始對(duì)準(zhǔn)開始時(shí)，〇xlQ在當(dāng)?shù)刈游缑鎯?nèi)，與赤道平面平行;0ZlQ指向地球自轉(zhuǎn)軸方 向；OYiQ與OXiQ和0ZiQ組成右手螺旋坐標(biāo)系；初始對(duì)準(zhǔn)開始后，io系三軸相對(duì)于慣性空間不 變；
[0013] ⑵初始時(shí)亥I」地球坐標(biāo)系(eo系）
[0014] 初始時(shí)刻地球坐標(biāo)系以地球中心為坐標(biāo)系原點(diǎn)，OXm在赤道平面內(nèi)指向當(dāng)?shù)刈游?線方向；OZio指向地球自轉(zhuǎn)軸方向；OYi〇與OXi〇和OZi〇組成右手螺旋坐標(biāo)系；該坐標(biāo)系相對(duì)于 地球表面靜止不動(dòng)；
[0015] (3)初始時(shí)刻導(dǎo)航坐標(biāo)系(n〇系）
[0016] 將初始對(duì)準(zhǔn)開始時(shí)刻的東-北-天導(dǎo)航坐標(biāo)系作為初始時(shí)刻導(dǎo)航坐標(biāo)系;初始對(duì)準(zhǔn) 開始后，該坐標(biāo)系相對(duì)于地球表面不動(dòng)；
[0017] ⑷初始時(shí)亥懺體坐標(biāo)系（ib0系）
[0018] 將初始對(duì)準(zhǔn)開始時(shí)刻的右-前-上載體坐標(biāo)系作為初始時(shí)刻載體坐標(biāo)系;初始對(duì)準(zhǔn) 開始后，該坐標(biāo)系相對(duì)于慣性空間不動(dòng)；
[0019] 步驟二:在載體運(yùn)動(dòng)過程中，根據(jù)傳感器子系統(tǒng)采樣數(shù)據(jù)計(jì)算粗略姿態(tài)矩陣；
[0020] 以io系為過渡參考坐標(biāo)系，導(dǎo)航坐標(biāo)系(η系)相對(duì)于載體坐標(biāo)系(b系)姿態(tài)矩陣 的計(jì)算分為兩個(gè)部分，其計(jì)算過程如式(1)所示
[0021] (1)
[0022]其中，C,；；為n系相對(duì)于iQ系的轉(zhuǎn)換矩陣；Cf為i〇系相對(duì)于b系的轉(zhuǎn)換矩陣；
[0023] a計(jì)算矩陣q
[0024] 通過過渡參考坐標(biāo)系no系、eo系，該矩陣的計(jì)算過程又可分為四個(gè)部分，即：
[0025] ⑵
[0026] 式中，^丨為!!系相對(duì)于地理坐標(biāo)系（e系）的轉(zhuǎn)換矩陣；?；為e系相對(duì)于nQ系的轉(zhuǎn)換 矩陣；
[0027] 為nQ系相對(duì)于eQ系的轉(zhuǎn)換矩陣；C；?為eQ系相對(duì)于 iQ系的轉(zhuǎn)換矩陣；
[0028] 由于在粗對(duì)準(zhǔn)模塊中無(wú)法精確獲取載體位置，因此，式(2)近似為：
[0033]式中，Lo為初始對(duì)準(zhǔn)開始時(shí)的地理煒度；coie為地球自轉(zhuǎn)角速度；Δ t為當(dāng)前時(shí)刻t 與初始對(duì)準(zhǔn)開始時(shí)刻tstart的時(shí)間差，即Δ t = t_tstart;將式(4-5)代入式(3)中后，可得：
[0035] b計(jì)算矩陣0
[0036] 以ibQ系為過渡參考坐標(biāo)系，io系相對(duì)于b系的轉(zhuǎn)換矩陣的計(jì)算分為兩個(gè)部分， 即：
[0037] Q =Cl-Ct (7)
[0038] 其中，為iQ系相對(duì)于ibQ系的轉(zhuǎn)換矩陣；為ibQ系相對(duì)于b系的轉(zhuǎn)換矩陣；
[0039] 利用陀螺儀的測(cè)量角速度通過姿態(tài)矩陣微分方程(8)可直接得到矩陣
[0041] 式中，由于ib〇系與io系均相對(duì)于慣性坐標(biāo)系（i系)不動(dòng)，因此可用慣性測(cè)量元件的 角速度采樣數(shù)據(jù)直接代替ω?;另外，在初始對(duì)準(zhǔn)開始時(shí)刻，i bQ系與b系重合，所以矩陣 Cp的積分計(jì)算初值為單位陣I3X3;
[0042] 矩陣的計(jì)算需要用到地球重力加速度在慣性系中慢漂的性質(zhì)；其計(jì)算方法如 下：
[0043] i b〇系和i 〇系均相對(duì)慣性空間不動(dòng)，因此易知，矩陣<^°。為以常值矩陣;對(duì)載體速度 V" =?：·ν6兩邊求導(dǎo)可得：
[0044] r = c^-fk+ci：·^ (69)
[0045]其中，為載體加速度在η系中的分量為載體加速度在b系中的分量；
[0046]將其代入慣性導(dǎo)航基本方程中，則慣性導(dǎo)航基本方程可改寫如式（10)所示的形 式；
[0047] c； . νΛ + Cl V + (ω；； + ω；；,)χ v,f-Γ = g" (10)
[0048] 其中，gn為地球重力加速度在η系中的分量;為地球自轉(zhuǎn)角速度在η系中的分量； <4為η系相對(duì)于i系的旋轉(zhuǎn)角速度;Γ為慣性測(cè)量元件的比力采樣數(shù)據(jù)在η系中的分量；
[0049] 又已知姿態(tài)矩陣微分方程Q X]，其中為b系相對(duì)于η系的旋轉(zhuǎn)角速 度。將其代入(10)中，可得：
[0050] q ·0)：*,X +Q wb + (ω：：, + ω；；,)χνη -?η = gn (Π )
[0051] 式中，可忽略不計(jì)，上式進(jìn)一步改寫為式（12);
[0052] V6 + X -f^ -g^' ( 12)
[0053] 其中，gb為地球重力加速度在b系中的分量;fb為慣性測(cè)量元件的比力采樣數(shù)據(jù);vb 用里程計(jì)速度采樣數(shù)據(jù)4構(gòu)成的速度測(cè)量矢量[0 v| of代替;而礦用里程計(jì)速度測(cè)量矢 量微分[0 V, of代替；需要說明的是，由于里程計(jì)速度采樣數(shù)據(jù)中含有噪聲，直接微分會(huì) 放大噪聲而導(dǎo)致對(duì)準(zhǔn)精度下降，因此需要使用跟蹤微分器對(duì)4進(jìn)行微分處理得到噪聲干擾 較小的讒;
[0054] 為了獲取矩陣g?。，需要通過gb與#間的重要關(guān)系式（13);
[0055] Qa gb = C；M · C： gB tl3)
[0056] 式中，地球重力加速度在b系下的分量gn=[0 0 _g]T為已知量;矩陣均可 由前面的計(jì)算得到，因此矩陣Ct的求解只需根據(jù)式（13)構(gòu)造三個(gè)不在同一平面的向量即 可；
[0057] 為了構(gòu)造向量，同時(shí)削弱噪聲，將（13)式兩邊進(jìn)行積分處理，得到：
[0060]得到轉(zhuǎn)換矩陣Ct，需要通過0與8"間的重要關(guān)系式
[0062] 其中，CT、均可由前面的計(jì)算得到;將上式兩邊積分得
[0063] = C^u ·if°: (16)
[0064] 取兩個(gè)時(shí)刻t#Pt2的積分值<°、C和&