【正文】 開 始 仿真算法流程圖 設(shè)置常值讀取初始姿態(tài)，速度，位置參數(shù)及其誤差值；讀取時(shí)間常數(shù)；讀取陀螺及加速度計(jì)誤差參數(shù)，初始四元數(shù)及姿態(tài)矩陣計(jì)算角增量計(jì)算 四元數(shù)更新計(jì)算 四元數(shù)規(guī)范化姿態(tài)矩陣更新計(jì)算，姿態(tài)角更新計(jì)算速度更新計(jì)算位置更新計(jì)算輸出結(jié) 束5. 仿真結(jié)果 仿真初始數(shù)據(jù)設(shè)定 ，加速度計(jì)的零偏為le4*g。所以加速度計(jì)誤差模一般考慮為: ()式中=0，（）為噪聲過(guò)程。但在組合導(dǎo)航設(shè)計(jì)中，一般只考慮隨機(jī)常值漂移，即偏置誤差()，而忽略相關(guān)誤差。這種漂移分量可抽象為白噪聲過(guò)程，即 ()。這種分量可用一階馬爾科夫過(guò)程中來(lái)描述： ()(3)快變漂移表現(xiàn)在上述兩種分量基礎(chǔ)上的雜亂無(wú)章的高頻跳變。 ()(2)慢變漂移陀螺在工作過(guò)程中，環(huán)境條件、電氣參數(shù)都在作隨機(jī)改變，所以陀螺是漂移在隨機(jī)常數(shù)分量的基礎(chǔ)上以較慢的速率變化。陀螺的誤差主要體現(xiàn)為隨機(jī)漂移，隨機(jī)漂移是十分復(fù)雜的隨機(jī)過(guò)程，大致可概述為三種分量。系統(tǒng)的隨機(jī)誤差很多，主要討論陀螺漂移和加速度計(jì)的偏差。2. 隨機(jī)誤差源引起的系統(tǒng)誤差對(duì)于上面討論的確定性誤差源引起的誤差特性可以設(shè)法通過(guò)補(bǔ)償加以消除。但因振蕩周期很長(zhǎng)，遠(yuǎn)遠(yuǎn)大于一次工作時(shí)間，此時(shí)，在系統(tǒng)工作時(shí)間誤差是隨時(shí)間增長(zhǎng)的。綜上所述 ，慣導(dǎo)系統(tǒng)確定性誤差源引起的誤差特性包括三種振蕩，即舒勒周期振蕩，地球周期振蕩和傅科周期振蕩。對(duì)應(yīng)角頻率的振蕩叫舒勒振蕩，而對(duì)應(yīng)的的振蕩稱為傅科振蕩，其振蕩周期為。例如，即產(chǎn)生一個(gè)角頻率為的振蕩，其幅值為。由特征方程可得： （）： 這是一個(gè)等幅振蕩，振蕩周期為：（即地球自轉(zhuǎn)周期）.，因?yàn)?即，故可近似分解為：所以得： 這四個(gè)根說(shuō)明系統(tǒng)中還包括有角頻率為和的兩種振蕩運(yùn)動(dòng)。經(jīng)度誤差在系統(tǒng)的回路之外，不影響系統(tǒng)的動(dòng)態(tài)特性，也不予以考慮。這樣，在靜基座下，則有： （），可得： （）在初始對(duì)準(zhǔn)中，作為控制信息，常用的是水平加速度計(jì)信息，而不是垂直信息，展開上式，得水平通道速度誤差方程為： （）式中 捷聯(lián)慣導(dǎo)系統(tǒng)的基本誤差特性分析從系統(tǒng)的主要誤差源可以看出，捷聯(lián)慣導(dǎo)系統(tǒng)的基本誤差可以概括為兩大類：確定性的和隨機(jī)性的，下面分別進(jìn)行討論[9]。 由加速度計(jì)測(cè)得的加速度信息經(jīng)過(guò)數(shù)學(xué)解析平臺(tái)變換成計(jì)算地理坐標(biāo)系下的計(jì)算值為 （）加速度計(jì)測(cè)信息中含有g(shù)、等成分。另外在安裝于載體上的陀螺儀輸出的測(cè)量信號(hào)中，除了地球自轉(zhuǎn)角速率之外還包含陀螺漂移誤差ε和干擾角速度誤差ωd， （）令 δ=+ε則有 +δ （）稱δ為測(cè)量誤差。 系統(tǒng)的誤差方程 由于系統(tǒng)存在著各種誤差源，導(dǎo)航計(jì)算機(jī)實(shí)時(shí)所計(jì)算出的姿態(tài)矩陣并不能理想地描述b系相對(duì)t系的關(guān)系，而只能描述b系相對(duì)t系附近的計(jì)算地理坐標(biāo)系的關(guān)系，這里的系和t系存在小的誤差角α、β、γ，簡(jiǎn)記為 Φ=（α β γ）T 規(guī)定符號(hào)“”表示計(jì)算值，“∽”表示測(cè)量值由于艦船系泊于碼頭的緯度可以足夠精確的測(cè)得，則地球自轉(zhuǎn)角速度在該處地理坐標(biāo)系中的投影分量也可以精確的表示出來(lái)。3．計(jì)算機(jī)的算法誤差對(duì)于捷聯(lián)式慣導(dǎo)系統(tǒng)，當(dāng)加速度計(jì)與陀螺的輸出被采集到計(jì)算機(jī)中以后，剩下的工作由計(jì)算機(jī)承擔(dān)，而所有的捷聯(lián)計(jì)算都存在著算法誤差，從而導(dǎo)致系統(tǒng)的誤差。2．慣性儀表的誤差 慣性儀表（包括陀螺及加速度計(jì)）由于原理、加工與裝配工藝的不完善等均可造成儀表輸出的誤差，從而導(dǎo)致系統(tǒng)的誤差。1．?dāng)?shù)學(xué)模型的近似性所引起的誤差當(dāng)捷聯(lián)系統(tǒng)的數(shù)學(xué)模型建立得不夠精確時(shí)會(huì)引起系統(tǒng)誤差。與此同時(shí)，以陀螺儀的量測(cè)輸出估計(jì)計(jì)算出艦船搖擺角速度，對(duì)艦船搖擺的角度進(jìn)行隔離。 Φ=（α β γ）T Φ （）這里的Φ為計(jì)算地理坐標(biāo)系與真實(shí)地理坐標(biāo)系的誤差角，α、β、γ分別為地理坐標(biāo)系與真實(shí)坐標(biāo)系的北向、東向、方位的誤差角。捷聯(lián)式慣性導(dǎo)航系統(tǒng)的計(jì)算姿態(tài)矩陣與真實(shí)姿態(tài)矩陣的誤差，可以直接由數(shù)學(xué)解析平臺(tái)的不對(duì)準(zhǔn)誤差角來(lái)表示。 （）式中表示計(jì)算地理坐標(biāo)系（系）和載體坐標(biāo)系（b系）間的方向余弦矩陣，即為計(jì)算的捷聯(lián)矩陣，它同樣起著平臺(tái)作用。解析系統(tǒng)利用這一控制信息，一方面控制解析平臺(tái)減小水平傾斜角；另一方面又控制解析平臺(tái)減小方位失準(zhǔn)角，從而使數(shù)學(xué)解析平臺(tái)能穩(wěn)定在地理坐標(biāo)系上。數(shù)學(xué)解析平臺(tái)的工作過(guò)程與穩(wěn)定平臺(tái)式系統(tǒng)類似，當(dāng)數(shù)學(xué)解析平臺(tái)有方位失準(zhǔn)角γ時(shí)，則地球自轉(zhuǎn)角速率分量ωiecosφsinγ就耦合到解析平臺(tái)的東軸上，從而產(chǎn)生了羅經(jīng)效應(yīng)。我們應(yīng)該采用穩(wěn)定系統(tǒng)控制原理，以重力矢量g和地球自轉(zhuǎn)速率ωie 為控制信號(hào)，使捷聯(lián)系統(tǒng)穩(wěn)定在地理坐標(biāo)系上。用g、ωie可以構(gòu)成一個(gè)新的矢量E： E=gωie根據(jù)載體坐標(biāo)系與地理坐標(biāo)系的變換矩陣可以得： g′=gb （）考慮到捷聯(lián)矩陣具有正交性，即有： 故可表示成： = （）式中g(shù)b 、是由加速度計(jì)、陀螺儀直接測(cè)得的，而Eb、。捷聯(lián)慣導(dǎo)系統(tǒng)精對(duì)準(zhǔn)是粗對(duì)準(zhǔn)的繼續(xù)，這個(gè)階段的主要任務(wù)是: 通過(guò)處理慣性敏感器的輸出信息，精確校正計(jì)算參考坐標(biāo)系與真實(shí)參考坐標(biāo)系之間的小失準(zhǔn)角，從而建立起準(zhǔn)確的初始變換矩陣，為導(dǎo)航計(jì)算提供精確的初始條件，以便正常地進(jìn)行導(dǎo)航工作。粗對(duì)準(zhǔn)階段用重力矢量g和地球自轉(zhuǎn)速率ωie 的測(cè)量值，直接估算載體坐標(biāo)系到地理坐標(biāo)系的變換矩陣，大約需要一分鐘，這樣對(duì)準(zhǔn)的精度為: 方位角在幾度以內(nèi)，兩個(gè)水平角在1度以內(nèi)。因此，自主對(duì)準(zhǔn)技術(shù)以其重要的價(jià)值和意義，從控制理論觀點(diǎn)分析，自主對(duì)準(zhǔn)技術(shù)的基本困難是系統(tǒng)不完全可觀測(cè)。傳統(tǒng)的導(dǎo)航，即定位的概念正在被新的綜合性定位定向概念所取代。自主對(duì)準(zhǔn)加強(qiáng)了慣導(dǎo)系統(tǒng)的自主性、隱蔽性，在現(xiàn)代戰(zhàn)爭(zhēng)中具有非自主對(duì)準(zhǔn)不可代替的作用。自主對(duì)準(zhǔn)是利用系統(tǒng)本身的慣性元件，結(jié)合系統(tǒng)作用原理，自動(dòng)進(jìn)行對(duì)準(zhǔn)的方法。一般性的設(shè)計(jì)原則是在保證初始對(duì)準(zhǔn)精度的前提下設(shè)法縮短對(duì)準(zhǔn)時(shí)間。顯然上述措施的實(shí)現(xiàn)，都需要容量大、速度快的計(jì)算機(jī)給以保證。為提高捷聯(lián)系統(tǒng)的精度，還希望初始對(duì)準(zhǔn)能對(duì)陀螺漂移、加速度計(jì)零位誤差以及它們的標(biāo)度系數(shù)進(jìn)行測(cè)定和補(bǔ)償。初始對(duì)準(zhǔn)的要求包括精度和快速兩個(gè)方面。由于平臺(tái)系統(tǒng)和捷聯(lián)系統(tǒng)基本原理相同，因此，它們的自對(duì)準(zhǔn)原理也是相同的。而對(duì)于捷聯(lián)系統(tǒng)，初始對(duì)準(zhǔn)就是確定初始時(shí)刻的姿態(tài)陣。對(duì)準(zhǔn)精度的高低，直接影響導(dǎo)航性能。第三項(xiàng)任務(wù)在陀螺性能比較穩(wěn)定的情況下，不一定每次啟動(dòng)都要進(jìn)行。 初始對(duì)準(zhǔn)的一般要求 慣性導(dǎo)航系統(tǒng)的初始化包括:給定初始速度和初始位置，慣導(dǎo)平臺(tái)的初始對(duì)準(zhǔn)，陀螺儀的測(cè)漂和定標(biāo)。與此相反，捷聯(lián)陀螺的信息具有極高的角分辨率，可直接用于初始對(duì)準(zhǔn)。4．有更多的可用信息。3．有利于在對(duì)準(zhǔn)過(guò)程中標(biāo)定慣性儀表的參數(shù)捷聯(lián)式慣性系統(tǒng)與平臺(tái)式慣性系統(tǒng)相比，能夠輸出更多的可用信息，這樣，捷聯(lián)式慣性系統(tǒng)就能在對(duì)準(zhǔn)過(guò)程中標(biāo)定更多的慣性儀表誤差系數(shù)。2．對(duì)瞬時(shí)方向余弦陣的測(cè)定捷聯(lián)式慣性系統(tǒng)與平臺(tái)式慣性系統(tǒng)的另一個(gè)主要差別就是前者在初始對(duì)準(zhǔn)時(shí)，必須求出瞬時(shí)方向余弦陣，而不是平均方向余弦陣。 捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)的主要特點(diǎn)1．大的初始不對(duì)準(zhǔn)角捷聯(lián)慣導(dǎo)系統(tǒng)直接安裝在基座上，其不對(duì)準(zhǔn)角由基座的姿態(tài)和航向決定，一般不能將不對(duì)準(zhǔn)角視為小量，這就帶來(lái)數(shù)學(xué)處理上的麻煩。這是因?yàn)樵诮萋?lián)系統(tǒng)中慣性敏感器直接測(cè)量得到的是載體相對(duì)慣性坐標(biāo)系的各軸向的加速度大小，而捷聯(lián)系統(tǒng)數(shù)學(xué)解析平臺(tái)軟件程序中所需的應(yīng)是載體坐標(biāo)系相對(duì)于地理坐標(biāo)系的加速度信息。要進(jìn)行積分必須知道初始條件，例如初始速度和初始位置。的計(jì)算利用到載體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的轉(zhuǎn)動(dòng)角速度，記是從tm1時(shí)刻到tm時(shí)刻載體坐標(biāo)系b相對(duì)于導(dǎo)航坐標(biāo)系n的等效旋轉(zhuǎn)矢量，則： （）由于相對(duì)于來(lái)說(shuō)變化很小，若認(rèn)為和在時(shí)間內(nèi)基本不變，并取為tm1時(shí)刻的值，則有 （）其中是tm1時(shí)刻的姿態(tài)矩陣，是tm1時(shí)刻導(dǎo)航坐標(biāo)系相對(duì)于慣性空間的轉(zhuǎn)動(dòng)角速度，都是已知量，并且記 （）的計(jì)算可用不同子樣數(shù)角增量算法，得到利用變換四元數(shù)和等效旋轉(zhuǎn)矢量之間的轉(zhuǎn)換關(guān)系是： （）可求得，完成姿態(tài)更新。 ，Q(t)和ΔQ(t)之間和為Q(t+Δt),故有： Q(t+Δt)=ΔQ(t)Q(t) （） 或Qc（t+Δt）=Qc(t)?ΔQc(t) （） 考慮Δt很小，在Δt時(shí)間內(nèi)動(dòng)坐標(biāo)系轉(zhuǎn)動(dòng)的角度為Δα，則有 ΔQ(t)=cos(Δα/2)+nsin(Δα/2)≈1+n(Δα/2) 由于Δα是由ω在Δt時(shí)間內(nèi)轉(zhuǎn)過(guò)的角度，故 nΔα=ωΔt其中ω表示為： ω=ωbxib+ωbxjb+ωbzkb =ωtxit+ωtyjt+ωtxkt =ωt由于ωb=ωbxit+ωbxjt+ωbzkt 為 的“映像”，故有： ΔQc(t)=1+(ωbΔt)/2 Q(t)= Qc(t)：ΔQc(t+Δt)= Qc(t)?= Qc(t)+ Qc(t)(ωbΔt)/2我們定義 ≌l(shuí)im= Qc(t)ωb/2 ()，則有 ΔQ(t)= 1+(ωtΔt)/2所以 Q(t+Δt)=(1+ωtΔt )?Q(t)取 = lim= Q(t)ωt/2 () ，是四元數(shù)相乘。 四元數(shù)的實(shí)時(shí)值可以通過(guò)求解四元數(shù)微分方程得到。 假定矢量r繞通過(guò)頂點(diǎn)O的某一瞬時(shí)軸轉(zhuǎn)動(dòng)了一個(gè)角度α，則和矢量固聯(lián)的動(dòng)坐標(biāo)系和參考坐標(biāo)系之間的變換四元數(shù)為： Q=cos(α/2)+nsin(α/2)式中 n轉(zhuǎn)軸方向的單位矢量 轉(zhuǎn)動(dòng)前的矢量用r表示，轉(zhuǎn)動(dòng)后的矢量用r′表示，則r′和r的關(guān)系可由四元數(shù)來(lái)描述，即 r′=Q?r?Q* () 下面來(lái)研究固定矢量坐標(biāo)變換的四元數(shù)描述?？捎胷tx、rty、rtz來(lái)表示r的四元數(shù)形式為： Rt=0+ rtxi +rtyj +rtzk =0+r Rt就叫做矢量r在地理坐標(biāo)系上的四元數(shù)映象。 Q=cos(α/2)+nsin(α/2)=e () 如果把n寫成分量形式，則： Q=cos(α/2)+nxsin(α/2)i+nysin(α/2)j+nzsin(α/2)k =q0+q1i+q2j+q3k ()式中 q0=cos(α/2),q1=nxsin(α/2),q2=nysin(α/2),q3=nzsin(α/2)顯然，即 h= = 這樣的四元數(shù)又叫做“規(guī)范數(shù)”的四元數(shù)；用來(lái)描述剛體定點(diǎn)轉(zhuǎn)動(dòng)的四元數(shù)。3）矩陣形式 若把四元數(shù)Q、P、N的四個(gè)元寫成矢量形式，既表示為： Q(q)= P(p)= N(n)=： 或 N(n)=M(q)P(p) ()： 或 N(n)=M*(p)Q(q) ()