算法理論計(jì)算

1、導(dǎo)航算法理論計(jì)算1、捷聯(lián)算法1.1原理圖圖1.1 捷聯(lián)算法原理圖1.2捷聯(lián)解算步驟1) 子午圈曲率半徑 公式(1.1) 公式(1.2) 公式(1.3)2) 卯酉圈曲率半徑 公式(1.4)3) 重力加速度計(jì)算 公式(1.5)其中，4) 導(dǎo)航坐標(biāo)系下位移角速率由于飛行器的運(yùn)動(dòng)引起的地理坐標(biāo)系相對(duì)地球坐標(biāo)系的角速度 公式(1.6) 公式(1.7) 公式(1.8)5) 地球角速率由于地球轉(zhuǎn)動(dòng)引起的地理坐標(biāo)系的角速度 公式(1.9) 公式(1.10) 公式(1.11)其中， 6) 四元數(shù)法求姿態(tài)矩陣 公式(1.13) 公式(1.14) 公式(1.15) 公式(1.16) 公式(1.17) 公式(1.18

2、)其中，7) 姿態(tài)更新俯仰角 ： 公式(1.19)橫滾角： 公式(1.20)航向角： 公式(1.21)8) 速度更新 東向速度：公式(1.22)北向速度：公式(1.23)天向速度：公式(1.24)9) 位置更新 經(jīng)度： 公式(1.25)緯度： 公式(1.26) 2、高動(dòng)態(tài)捷聯(lián)算法當(dāng)運(yùn)載體存在線振動(dòng)和角振動(dòng)，或運(yùn)載體做機(jī)動(dòng)運(yùn)動(dòng)時(shí)，在姿態(tài)解算中會(huì)引起圓錐誤差，在速度解算中會(huì)引起劃槳誤差，在位置解算中會(huì)引起渦卷誤差，在相應(yīng)算法中需要做嚴(yán)格的補(bǔ)償。旋轉(zhuǎn)矢量法可采用多子樣算法實(shí)現(xiàn)對(duì)不可交換誤差做有效補(bǔ)償，特別適用于角機(jī)動(dòng)頻繁激烈或存在嚴(yán)重角振動(dòng)的運(yùn)載體的姿態(tài)更新。姿態(tài)更新計(jì)算的四元數(shù)皮卡算法實(shí)質(zhì)上就是單

3、子樣旋轉(zhuǎn)矢量算法。因此，多子樣旋轉(zhuǎn)矢量算法比四元數(shù)皮卡算法的精度高。2.1 基于旋轉(zhuǎn)矢量法的姿態(tài)更新由導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的姿態(tài)矩陣： 構(gòu)造四元數(shù)，由該四元數(shù)可確定出載體系至導(dǎo)航系的坐標(biāo)變換矩陣為： 如果表征載體系至導(dǎo)航系的旋轉(zhuǎn)四元數(shù)已確定，按上式便可計(jì)算出姿態(tài)陣。因此，捷聯(lián)慣導(dǎo)中的姿態(tài)更新實(shí)質(zhì)上是如何計(jì)算四元數(shù)。記時(shí)刻機(jī)體坐標(biāo)系至?xí)r刻機(jī)體坐標(biāo)系的旋轉(zhuǎn)四元數(shù)為，時(shí)刻導(dǎo)航坐標(biāo)系至的旋轉(zhuǎn)四元數(shù)為，時(shí)刻導(dǎo)航坐標(biāo)系至的旋轉(zhuǎn)四元數(shù)為，其中。則 其中 為至的等效旋轉(zhuǎn)矢量，。 單子樣旋轉(zhuǎn)矢量算法（即為四元數(shù)法的畢卡逼近算法）： 雙子樣旋轉(zhuǎn)矢量算法：其中“”表示矢量叉乘，、分別表示、兩個(gè)時(shí)間段內(nèi)的角增量。

4、三子樣旋轉(zhuǎn)矢量算法：其中、分別表示、三個(gè)時(shí)間段內(nèi)的角增量。 四子樣旋轉(zhuǎn)矢量算法：其中、分別表示、四個(gè)時(shí)間段內(nèi)的角增量。旋轉(zhuǎn)矢量構(gòu)造的姿態(tài)變化四元數(shù)為：其中， 即：依據(jù)公式可得：于是求出時(shí)刻的姿態(tài)更新四元數(shù)，進(jìn)一步求取姿態(tài)矩陣，獲得此時(shí)刻的姿態(tài)角。2.2捷聯(lián)慣導(dǎo)系統(tǒng)的速度更新算法取地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，速度方程為： 式中，為載體在導(dǎo)航坐標(biāo)系下的速度矢量；為載體的姿態(tài)矩陣；為加速度計(jì)輸出的在載體坐標(biāo)系下的比力；和分別是位置速率和地球自轉(zhuǎn)速率；為導(dǎo)航坐標(biāo)系下的重力加速度矢量。當(dāng)載體同時(shí)存在線運(yùn)動(dòng)和角運(yùn)動(dòng)時(shí)，采用速度增量解算必須補(bǔ)償速度的旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)。速度旋轉(zhuǎn)效應(yīng)補(bǔ)償項(xiàng)為： 其中是加速度計(jì)在時(shí)

5、間段內(nèi)輸出的速度增量；是陀螺儀在時(shí)間段內(nèi)輸出的角度增量。旋轉(zhuǎn)效應(yīng)是由運(yùn)載體的線運(yùn)動(dòng)方向在空間旋轉(zhuǎn)引起的。速度劃槳效應(yīng)補(bǔ)償項(xiàng)的三子樣算法為： 式中，是內(nèi)角增量的三個(gè)等間隔采樣值，是內(nèi)速度增量的三個(gè)等間隔采樣值。2.3捷聯(lián)慣導(dǎo)系統(tǒng)的位置更新設(shè)位置更新周期為，和分別為時(shí)刻和時(shí)刻的導(dǎo)航坐標(biāo)系，坐標(biāo)系至坐標(biāo)系的旋轉(zhuǎn)矢量可寫成： 其中，在計(jì)算時(shí)所用的緯度是在時(shí)間點(diǎn)上緯度的線性外推值： 若已求得，則有 由于很小，所以上式可取一階近似： 時(shí)刻的位置矩陣可按下式確定： 式中為上一位置更新時(shí)間點(diǎn)上確定的位置矩陣。位置矩陣與經(jīng)度和緯度關(guān)系如下： 高度求取如下： 式中，是沿導(dǎo)航坐標(biāo)系z(mì)軸的分量。3、靜態(tài)初始對(duì)準(zhǔn)3.1

6、粗對(duì)準(zhǔn)基本原理捷聯(lián)系統(tǒng)是將IMU與載體固聯(lián)，安裝方式對(duì)應(yīng)的坐標(biāo)系為右前上。載體靜止水平時(shí)，導(dǎo)航坐標(biāo)系下的載體三軸的加速度值為，當(dāng)存在姿態(tài)角時(shí)，有載體坐標(biāo)系與導(dǎo)航坐標(biāo)系的關(guān)系如下所示： 公式(3.1) 公式(3.2) 載體坐標(biāo)系下x軸的加表測(cè)量值; 載體坐標(biāo)系下y軸的加表測(cè)量值; 載體坐標(biāo)系下z軸的加表測(cè)量值; 載體俯仰角 載體橫滾角 載體偏航角這樣通過以下公式可求出水平的兩個(gè)姿態(tài)角： 公式(3.3) 公式(3.5)3.2精對(duì)準(zhǔn)基本原理精對(duì)準(zhǔn)是粗對(duì)準(zhǔn)的繼續(xù)，主要任務(wù)是在盡可能使“數(shù)學(xué)平臺(tái)”趨于水平狀態(tài)的同時(shí)，能夠自動(dòng)檢測(cè)出陀螺的漂移，并精確的計(jì)算出姿態(tài)角，為導(dǎo)航計(jì)算提供準(zhǔn)確的初始條件。精對(duì)準(zhǔn)與組

7、合導(dǎo)航的思想一致，構(gòu)建狀態(tài)誤差模型，引入GPS的速度觀測(cè)信息，構(gòu)建觀測(cè)方程，對(duì)初始的姿態(tài)角進(jìn)行修正。選取的狀態(tài)向量為： x軸速度誤差 y軸速度誤差 俯仰誤差角 橫滾誤差角 偏航誤差角 x軸加表零偏 y軸加表誤差 x軸陀螺零偏 y軸陀螺零偏 z軸陀螺零偏狀態(tài)方程： 公式(3.6)公式(3.7)其中： 地球自轉(zhuǎn)角速率 載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣 x軸加表噪聲 y軸加表噪聲 x軸陀螺噪聲 y軸陀螺噪聲 z軸陀螺噪聲觀測(cè)向量： 公式(3.8)觀測(cè)方程： 公式(3.9) 公式(3.10)1) 系統(tǒng)方程離散化 公式(3.11) 公式(3.12)表示濾波周期。2) 采用卡爾曼濾波進(jìn)行數(shù)據(jù)融合采用Kal

8、man濾波對(duì)模型進(jìn)行組合解算獲得姿態(tài)角信息，解算方程組如下所示： 公式(3.13) 狀態(tài)誤差方差陣 觀測(cè)誤差方差陣 估計(jì)誤差方差陣 最優(yōu)增益矩陣或者加權(quán)矩陣4、松耦合設(shè)計(jì)圖4.1 松組合導(dǎo)航設(shè)計(jì)流程圖松組合導(dǎo)航設(shè)計(jì)即利用IMU的信息和GPS的信息相結(jié)合，在捷聯(lián)解算計(jì)算出的導(dǎo)航信息與GPS接收的速度和位置信息，利用Kalman濾波算法進(jìn)行數(shù)據(jù)融合，估計(jì)出導(dǎo)航信息誤差值，再采用閉環(huán)修正法對(duì)捷聯(lián)解算的導(dǎo)航信息進(jìn)行修正，獲得更準(zhǔn)確的導(dǎo)航信息。捷聯(lián)解算的周期比GPS數(shù)據(jù)的周期短，因此需要判斷GPS數(shù)據(jù)是否有效，當(dāng)GPS數(shù)據(jù)無效時(shí)，僅進(jìn)行捷聯(lián)解算，圖中表示為黑色的箭頭流程，輸出為純捷聯(lián)解算結(jié)果;當(dāng)GPS數(shù)

9、據(jù)有效時(shí)，進(jìn)行組合導(dǎo)航，算出姿態(tài)誤差、速度誤差和位置誤差，反饋給系統(tǒng)進(jìn)行修正，最后獲得導(dǎo)航數(shù)據(jù)，圖中表示為紅色的箭頭流程，輸出為組合導(dǎo)航結(jié)果?？紤]到車載組合導(dǎo)航系統(tǒng)，測(cè)量水平的速度和位置信息，對(duì)高度不做要求，去掉高度相關(guān)參數(shù)，因此可以選用十二維系統(tǒng)，降低維數(shù)，減少計(jì)算量，也可以減輕數(shù)字?jǐn)?shù)據(jù)處理運(yùn)算負(fù)擔(dān)。而對(duì)于飛行載體，則不可忽略高度值，需要完整的三維信息，選用十五維導(dǎo)航系統(tǒng)。下面分別對(duì)十二維和十五維算法的原理進(jìn)行闡述，同時(shí)驗(yàn)證算法的有效性。4.1十二維組合導(dǎo)航系統(tǒng)設(shè)計(jì)選取的狀態(tài)向量為： 緯度誤差 經(jīng)度誤差 其他變量與初始對(duì)準(zhǔn)定義一致。狀態(tài)方程： 公式(4.1) 觀測(cè)向量：觀測(cè)方程： 公式(4.

10、2)4.2十五維組合導(dǎo)航系統(tǒng)設(shè)計(jì)選取的狀態(tài)向量為： 狀態(tài)方程： 公式(4.3) 觀測(cè)向量：觀測(cè)方程： 公式(4.4) 公式(4.5)5、緊耦合設(shè)計(jì)與松耦合相比，緊耦合是雙向信息傳輸?shù)模匆环矫鍳PS信號(hào)用于修正INS；另一方面，INS信號(hào)在衛(wèi)星星歷的輔助下，也用于計(jì)算載體相對(duì)于GPS衛(wèi)星的偽距和偽距率，并用該信息輔助GPS信號(hào)的接受和鎖相過程，以提高GPS的接收精度和動(dòng)態(tài)性能。緊耦合GPS/INS組合導(dǎo)航系統(tǒng)的結(jié)構(gòu)示意圖如圖5.1所示。圖5.1緊耦合GPS/INS組合導(dǎo)航系統(tǒng)結(jié)構(gòu)示意圖緊耦合GPS/SINS組合導(dǎo)航系統(tǒng)取消了GPS接收機(jī)內(nèi)部的卡爾曼濾波器，將其功能放在SINS/GPS組合導(dǎo)航卡

11、爾曼濾波器中實(shí)現(xiàn)，能更精確地對(duì)慣導(dǎo)的位置和速度誤差以及慣性傳感器的誤差進(jìn)行估計(jì)；并且該系統(tǒng)將慣導(dǎo)產(chǎn)生的載體位置和速度信息饋送到GPS接收機(jī)的載波跟蹤環(huán)和碼跟蹤環(huán)中，提高了GPS接收機(jī)的抗干擾能力；同時(shí)由于緊耦合系統(tǒng)的量測(cè)值直接取自GPS接收機(jī)的直接觀測(cè)量，即衛(wèi)星與載體之間的偽距和偽距率信息，無量測(cè)輸入相關(guān)問題，且在GPS可見衛(wèi)星個(gè)數(shù)小于四顆時(shí)仍然能夠提供一定精度的導(dǎo)航參數(shù)。與松耦合組合導(dǎo)航系統(tǒng)一樣，緊耦合系統(tǒng)中的捷聯(lián)解算的周期同樣比GPS數(shù)據(jù)的周期短，因此需要判斷GPS數(shù)據(jù)是否有效，當(dāng)GPS數(shù)據(jù)無效時(shí)，僅進(jìn)行捷聯(lián)解算，當(dāng)GPS數(shù)據(jù)有效時(shí)，進(jìn)行組合導(dǎo)航，算出姿態(tài)誤差、速度誤差和位置誤差反饋給系統(tǒng)

12、進(jìn)行修正，最后獲得導(dǎo)航數(shù)據(jù)。緊耦合組合導(dǎo)航系統(tǒng)的設(shè)計(jì)流程圖如圖5.2所示。圖5.2 松組合導(dǎo)航設(shè)計(jì)流程圖5.1二十維緊耦合組合導(dǎo)航系統(tǒng)的狀態(tài)方程緊耦合SINS/GPS組合導(dǎo)航系統(tǒng)狀態(tài)方程中的變量為SINS誤差狀態(tài)和GPS誤差狀態(tài)的組合，其中SINS的誤差狀態(tài)包括數(shù)學(xué)平臺(tái)誤差角、速度誤差和位置誤差以及慣性儀表誤差；GPS的誤差狀態(tài)通常取兩個(gè)與時(shí)間有關(guān)的誤差：一個(gè)是與時(shí)鐘鐘差等效的距離誤差，另一個(gè)是與時(shí)鐘頻率誤差等效的距離率誤差。當(dāng)取地理坐標(biāo)系（東北天坐標(biāo)系）為導(dǎo)航坐標(biāo)系時(shí)，捷聯(lián)慣導(dǎo)系統(tǒng)的數(shù)學(xué)平臺(tái)誤差角方程為： (5-1)由比力方程可以得到速度誤差方程為： (5-2)位置誤差方程為： (5-3)陀

13、螺儀和加速度計(jì)的測(cè)量誤差包括安裝誤差、刻度因子誤差和隨機(jī)誤差，前兩項(xiàng)可以通過轉(zhuǎn)臺(tái)試驗(yàn)標(biāo)定并補(bǔ)償，這里只考慮隨機(jī)誤差。通常取陀螺漂移為： (5-4)式中為隨機(jī)常值漂移；為一階馬爾科夫過程；為高斯白噪聲。陀螺誤差模型可表達(dá)為： (5-5)式中為相關(guān)時(shí)間，為白噪聲。加速度計(jì)的測(cè)量誤差為零偏和隨機(jī)噪聲，其中隨機(jī)噪聲可表示為： (5-6)式中為相關(guān)時(shí)間，為白噪聲。GPS的誤差狀態(tài)和的微分方程為： (5-7)將SINS的誤差方程式(5-1) 式(5-6)與GPS誤差方程式(5-7)合并，寫為矩陣形式，即可得到緊耦合SINS/GPS組合導(dǎo)航系統(tǒng)的狀態(tài)方程： (5-8)其中：為緊耦合SINS/GPS組合導(dǎo)航系

14、統(tǒng)的狀態(tài)變量；為階的系統(tǒng)狀態(tài)矩陣 、為階的動(dòng)態(tài)噪聲矩陣、為11維的過程白噪聲矢量。5.2二十維緊耦合組合導(dǎo)航系統(tǒng)的觀測(cè)方程在組合導(dǎo)航系統(tǒng)中，假設(shè)通過捷聯(lián)解算得到的SINS的位置為，SINS位置的坐標(biāo)真值為，由衛(wèi)星星歷確定的GPS衛(wèi)星位置為。由SINS載體到GPS衛(wèi)星的距離可由下式表達(dá)： (5-9)將在處展開成泰勒級(jí)數(shù)可得： (5-10)其中GPS系統(tǒng)的測(cè)碼偽距是由衛(wèi)星發(fā)射的測(cè)距碼（C/A碼或P碼）到用戶接收機(jī)的傳播時(shí)間（時(shí)間延遲）乘以光速所獲得的距離觀測(cè)量，如下式所示： (5-11)式中為衛(wèi)星到用戶機(jī)的幾何距離；是與時(shí)鐘鐘差等效的距離誤差；為電離層折射對(duì)測(cè)碼偽距的影響；為大氣對(duì)流層折射對(duì)測(cè)碼偽距的影響。將電離層誤差和對(duì)流層誤差看作高斯白噪聲，式(5-11)可寫為： (5-12)則緊耦合系統(tǒng)的偽距差量測(cè)方程為： (5-13)捷聯(lián)慣性導(dǎo)航系統(tǒng)安