捷聯(lián)慣導(dǎo)-優(yōu)質(zhì)課件

捷聯(lián)慣導(dǎo)數(shù)學(xué)模型及慣導(dǎo)解算原理捷聯(lián)慣導(dǎo)數(shù)學(xué)模型及慣導(dǎo)解算原理1坐標(biāo)系的定義1.地理坐標(biāo)系（下標(biāo)為t）——：取載體質(zhì)心，軸指向東，軸指向北，軸沿垂線指向天。2.導(dǎo)航坐標(biāo)系（下標(biāo)為n）——：取載體質(zhì)心，與重合，與，與相差一個游動方位角。3.平臺坐標(biāo)系（小標(biāo)為p）——：取載體質(zhì)心，當(dāng)慣導(dǎo)系統(tǒng)不存在誤差時，平臺坐標(biāo)系與導(dǎo)航坐標(biāo)系重合；存在誤差時平臺坐標(biāo)系相對導(dǎo)航坐標(biāo)系存在三個姿態(tài)失準(zhǔn)角，，。4.機體坐標(biāo)系（下標(biāo)為b）——：取載體質(zhì)心，沿載體橫軸指向右，沿載體縱軸向前，垂至于，并沿載體指向上。5.地心慣性坐標(biāo)系——：坐標(biāo)原點取在地心，6.地球坐標(biāo)系——：地球坐標(biāo)系固連在地球的坐標(biāo)系,它相對于地心慣性坐標(biāo)系以地球自傳角速度以上坐標(biāo)系間關(guān)系見下圖坐標(biāo)系的定義1.地理坐標(biāo)系（下標(biāo)為t）——2捷聯(lián)慣導(dǎo)-優(yōu)質(zhì)課件3捷聯(lián)慣導(dǎo)系統(tǒng)程序編排示意圖捷聯(lián)慣導(dǎo)系統(tǒng)程序編排示意圖4捷聯(lián)式慣導(dǎo)系統(tǒng)數(shù)學(xué)模型的推導(dǎo)1.系統(tǒng)的速率地球自轉(zhuǎn)引起地理坐標(biāo)系轉(zhuǎn)動的三個方向的角速度分量為：其中為地球自轉(zhuǎn)角速度，L為緯度。E，N，U分別表示東北天三個方向

飛行器運動引起地理坐標(biāo)系相對于地球坐標(biāo)系的三個角速度分量為：

捷聯(lián)式慣導(dǎo)系統(tǒng)數(shù)學(xué)模型的推導(dǎo)1.系統(tǒng)的速率5接上文

將前兩式求和得：上式為指北方位系統(tǒng)跟蹤角速率。其中：接上文6方向余弦矩陣（位置矩陣）平臺坐標(biāo)系與地球坐標(biāo)系轉(zhuǎn)動關(guān)系為：

其中導(dǎo)航坐標(biāo)系到平臺坐標(biāo)系的轉(zhuǎn)換矩陣由于姿態(tài)角是小角度,則可近似表示為:方向余弦矩陣（位置矩陣）平臺坐標(biāo)系與地球坐標(biāo)系轉(zhuǎn)動關(guān)系為：7確定緯度L和經(jīng)度利用方向余弦矩陣可確定緯度和經(jīng)度的真值。先求其主值：又因定義域：1.求緯度的真值L反正弦函數(shù)的主值域與L的定義域一致，因此：2.求經(jīng)度的真值反正切函數(shù)的主值域是與的定義域不一致，因此需要在的定義域內(nèi)確定經(jīng)度的真值。由：由于在L的定義域內(nèi)cosL永遠(yuǎn)為正，所以利用的正負(fù)值可確定真值：確定緯度L和經(jīng)度8方向余弦矩陣的即時修正方向余弦矩陣的變化是由平臺坐標(biāo)系相對地球坐標(biāo)系運動的角速率（又稱位置速率）引起的方向余弦矩陣的即時修正9位置速率位置速率是由飛行器地速的水平分量引起的，由于平臺坐標(biāo)系與地理坐標(biāo)系相差一個游動方位角，可得：可寫成綜合上兩式得：位置速率位置速率是由飛行器地速的水平分量引起的，由于平臺坐標(biāo)10指令角速率

將上式寫成投影分量形式，并結(jié)合位置矩陣的形式可得：指令角速率11速度方程慣導(dǎo)基本方程：其中：整理上式可得：速度方程12捷聯(lián)矩陣（姿態(tài)矩陣）實現(xiàn)由機體坐標(biāo)系到平臺坐標(biāo)系的坐標(biāo)轉(zhuǎn)換的方向余弦矩陣稱為捷聯(lián)矩陣，用T表示；根據(jù)捷聯(lián)矩陣的元素可以確定飛行器姿態(tài)角，因此捷聯(lián)矩陣又稱姿態(tài)矩陣。滿足方程：得到矩陣T后，沿機體坐標(biāo)系測量的比力就可以轉(zhuǎn)換到平臺坐標(biāo)系上，得到：捷聯(lián)矩陣（姿態(tài)矩陣）實現(xiàn)由機體坐標(biāo)系到平臺坐標(biāo)系的坐標(biāo)轉(zhuǎn)換的13由姿態(tài)矩陣T確定飛行器姿態(tài)角根據(jù)矩陣T中的元素可以確定各角的主值：再根據(jù)定義域與T中元素的符號得到各角真值：由姿態(tài)矩陣T確定飛行器姿態(tài)角根據(jù)矩陣T中的元素可以確定各角的14高度及重力加速度g的計算1.捷聯(lián)式慣導(dǎo)系統(tǒng)的高度通道是發(fā)散的，需要引入外部高度信息進(jìn)行阻尼。2.重力加速度g的計算其中h為載體飛行高度高度及重力加速度g的計算1.捷聯(lián)式慣導(dǎo)系統(tǒng)的高度通道是發(fā)散的15用四元數(shù)法對捷聯(lián)矩陣進(jìn)行即時修正機體坐標(biāo)系相對平臺坐標(biāo)系的轉(zhuǎn)動可用轉(zhuǎn)動四元數(shù)Q來表示：從而四元數(shù)的微分方程為將上式寫成矩陣形式，得：對它求解便可實時地求出用四元數(shù)法對捷聯(lián)矩陣進(jìn)行即時修正機體坐標(biāo)系相對平臺坐標(biāo)系的轉(zhuǎn)16用四元數(shù)法對捷聯(lián)矩陣進(jìn)行即時修正簡稱四元數(shù)的即時修正姿態(tài)速率計算用四元數(shù)法對捷聯(lián)矩陣進(jìn)行即時修正簡稱四元數(shù)的即時修正姿態(tài)速率17四元數(shù)的最佳正交化轉(zhuǎn)動四元數(shù)變換定理可知：由于計算誤差的存在使得：從而歸一化條件不再存在，應(yīng)進(jìn)行歸一化處理，設(shè)即時修正獲得的四元數(shù)為：其模為除以它的?？傻茫核脑獢?shù)的最佳正交化18四元數(shù)最佳正交化由上式四元數(shù)的模方可得：顯然滿足四元數(shù)的歸一條件，同時也完成了捷聯(lián)矩陣T的的正交化簡略的公式四元數(shù)最佳正交化簡略的公式19四元數(shù)初始值的確定捷聯(lián)矩陣也可表示為四元數(shù)初始值的確定捷聯(lián)矩陣也可表示為20捷聯(lián)慣導(dǎo)系統(tǒng)程序流程圖捷聯(lián)慣導(dǎo)系統(tǒng)程序流程圖21軌跡發(fā)生器的生成

導(dǎo)航解算算法是建立在慣性儀表的輸出的基礎(chǔ)上,為了驗證導(dǎo)航解算的正確與否,及后續(xù)的組合導(dǎo)航研究,需要設(shè)計模擬載體運動的軌跡發(fā)生器.軌跡發(fā)生器的設(shè)計思路是根據(jù)假定載體運動軌跡,在特定的坐標(biāo)系下,采用適當(dāng)?shù)淖藨B(tài)矩陣算法推導(dǎo)出慣性儀表的理論輸出值,同時模擬出載體運動的位置,速度,姿態(tài)以及陀螺儀的輸出值,加速度計的輸出值,再加上相應(yīng)的隨機誤差值.軌跡發(fā)生器的生成導(dǎo)航解算算法是22捷聯(lián)慣導(dǎo)初值的給定1.給定初始位置其中:2.給定初始速度其中:3.給定初始姿態(tài)角其中:上述初始誤差由慣導(dǎo)系統(tǒng)初始對準(zhǔn)條件決定.捷聯(lián)慣導(dǎo)初值的給定1.給定初始位置23比力的模擬計算載體在導(dǎo)航坐標(biāo)系中的真實速度變化矢量及載體姿態(tài)角的真實變化按一定規(guī)律給出；載體的真實位置初值：

真實速度初值：

真實姿態(tài)初值：均給定，則有：

比力的模擬計算24比力模擬計算上式中：加速度計測量值的模擬計算公式為：

是含有加速度計常值測量偏差及服從正態(tài)分布的隨機噪聲。比力模擬計算上式中：25姿態(tài)角速度的模擬計算根據(jù)給定的可求出載體相對于導(dǎo)航坐標(biāo)系的角速度。則陀螺儀測得的載體相對于慣性空間角速度可用下式模擬：姿態(tài)角速度的模擬計算根據(jù)給定的26捷聯(lián)慣導(dǎo)數(shù)學(xué)模型及慣導(dǎo)解算原理捷聯(lián)慣導(dǎo)數(shù)學(xué)模型及慣導(dǎo)解算原理27坐標(biāo)系的定義1.地理坐標(biāo)系（下標(biāo)為t）——：取載體質(zhì)心，軸指向東，軸指向北，軸沿垂線指向天。2.導(dǎo)航坐標(biāo)系（下標(biāo)為n）——：取載體質(zhì)心，與重合，與，與相差一個游動方位角。3.平臺坐標(biāo)系（小標(biāo)為p）——：取載體質(zhì)心，當(dāng)慣導(dǎo)系統(tǒng)不存在誤差時，平臺坐標(biāo)系與導(dǎo)航坐標(biāo)系重合；存在誤差時平臺坐標(biāo)系相對導(dǎo)航坐標(biāo)系存在三個姿態(tài)失準(zhǔn)角，，。4.機體坐標(biāo)系（下標(biāo)為b）——：取載體質(zhì)心，沿載體橫軸指向右，沿載體縱軸向前，垂至于，并沿載體指向上。5.地心慣性坐標(biāo)系——：坐標(biāo)原點取在地心，6.地球坐標(biāo)系——：地球坐標(biāo)系固連在地球的坐標(biāo)系,它相對于地心慣性坐標(biāo)系以地球自傳角速度以上坐標(biāo)系間關(guān)系見下圖坐標(biāo)系的定義1.地理坐標(biāo)系（下標(biāo)為t）——28捷聯(lián)慣導(dǎo)-優(yōu)質(zhì)課件29捷聯(lián)慣導(dǎo)系統(tǒng)程序編排示意圖捷聯(lián)慣導(dǎo)系統(tǒng)程序編排示意圖30捷聯(lián)式慣導(dǎo)系統(tǒng)數(shù)學(xué)模型的推導(dǎo)1.系統(tǒng)的速率地球自轉(zhuǎn)引起地理坐標(biāo)系轉(zhuǎn)動的三個方向的角速度分量為：其中為地球自轉(zhuǎn)角速度，L為緯度。E，N，U分別表示東北天三個方向

飛行器運動引起地理坐標(biāo)系相對于地球坐標(biāo)系的三個角速度分量為：

捷聯(lián)式慣導(dǎo)系統(tǒng)數(shù)學(xué)模型的推導(dǎo)1.系統(tǒng)的速率31接上文

將前兩式求和得：上式為指北方位系統(tǒng)跟蹤角速率。其中：接上文32方向余弦矩陣（位置矩陣）平臺坐標(biāo)系與地球坐標(biāo)系轉(zhuǎn)動關(guān)系為：

其中導(dǎo)航坐標(biāo)系到平臺坐標(biāo)系的轉(zhuǎn)換矩陣由于姿態(tài)角是小角度,則可近似表示為:方向余弦矩陣（位置矩陣）平臺坐標(biāo)系與地球坐標(biāo)系轉(zhuǎn)動關(guān)系為：33確定緯度L和經(jīng)度利用方向余弦矩陣可確定緯度和經(jīng)度的真值。先求其主值：又因定義域：1.求緯度的真值L反正弦函數(shù)的主值域與L的定義域一致，因此：2.求經(jīng)度的真值反正切函數(shù)的主值域是與的定義域不一致，因此需要在的定義域內(nèi)確定經(jīng)度的真值。由：由于在L的定義域內(nèi)cosL永遠(yuǎn)為正，所以利用的正負(fù)值可確定真值：確定緯度L和經(jīng)度34方向余弦矩陣的即時修正方向余弦矩陣的變化是由平臺坐標(biāo)系相對地球坐標(biāo)系運動的角速率（又稱位置速率）引起的方向余弦矩陣的即時修正35位置速率位置速率是由飛行器地速的水平分量引起的，由于平臺坐標(biāo)系與地理坐標(biāo)系相差一個游動方位角，可得：可寫成綜合上兩式得：位置速率位置速率是由飛行器地速的水平分量引起的，由于平臺坐標(biāo)36指令角速率

將上式寫成投影分量形式，并結(jié)合位置矩陣的形式可得：指令角速率37速度方程慣導(dǎo)基本方程：其中：整理上式可得：速度方程38捷聯(lián)矩陣（姿態(tài)矩陣）實現(xiàn)由機體坐標(biāo)系到平臺坐標(biāo)系的坐標(biāo)轉(zhuǎn)換的方向余弦矩陣稱為捷聯(lián)矩陣，用T表示；根據(jù)捷聯(lián)矩陣的元素可以確定飛行器姿態(tài)角，因此捷聯(lián)矩陣又稱姿態(tài)矩陣。滿足方程：得到矩陣T后，沿機體坐標(biāo)系測量的比力就可以轉(zhuǎn)換到平臺坐標(biāo)系上，得到：捷聯(lián)矩陣（姿態(tài)矩陣）實現(xiàn)由機體坐標(biāo)系到平臺坐標(biāo)系的坐標(biāo)轉(zhuǎn)換的39由姿態(tài)矩陣T確定飛行器姿態(tài)角根據(jù)矩陣T中的元素可以確定各角的主值：再根據(jù)定義域與T中元素的符號得到各角真值：由姿態(tài)矩陣T確定飛行器姿態(tài)角根據(jù)矩陣T中的元素可以確定各角的40高度及重力加速度g的計算1.捷聯(lián)式慣導(dǎo)系統(tǒng)的高度通道是發(fā)散的，需要引入外部高度信息進(jìn)行阻尼。2.重力加速度g的計算其中h為載體飛行高度高度及重力加速度g的計算1.捷聯(lián)式慣導(dǎo)系統(tǒng)的高度通道是發(fā)散的41用四元數(shù)法對捷聯(lián)矩陣進(jìn)行即時修正機體坐標(biāo)系相對平臺坐標(biāo)系的轉(zhuǎn)動可用轉(zhuǎn)動四元數(shù)Q來表示：從而四元數(shù)的微分方程為將上式寫成矩陣形式，得：對它求解便可實時地求出用四元數(shù)法對捷聯(lián)矩陣進(jìn)行即時修正機體坐標(biāo)系相對平臺坐標(biāo)系的轉(zhuǎn)42用四元數(shù)法對捷聯(lián)矩陣進(jìn)行即時修正簡稱四元數(shù)的即時修正姿態(tài)速率計算用四元數(shù)法對捷聯(lián)矩陣進(jìn)行即時修正簡稱四元數(shù)的即時修正姿態(tài)速率43四元數(shù)的最佳正交化轉(zhuǎn)動四元數(shù)變換定理可知：由于計算誤差的存在使得：從而歸一化條件不再存在，應(yīng)進(jìn)行歸一化處理，設(shè)即時修正獲得的四元數(shù)為：其模為除以它的?？傻茫核脑獢?shù)的最佳正交化44四元數(shù)最佳正交化由上式四元數(shù)的模方可得：顯然滿足四元數(shù)的歸一條件，同時也完成了捷聯(lián)矩陣T的的正交化簡略的公式四元數(shù)最佳正交化簡略的公式45四元數(shù)初始值的確定捷聯(lián)矩陣也可表示為四元數(shù)初始值的確定捷聯(lián)矩陣也可表示為46捷聯(lián)慣導(dǎo)系統(tǒng)程序流程圖捷聯(lián)慣導(dǎo)系統(tǒng)程序流程圖47軌跡發(fā)生器的生成

導(dǎo)航解算算法是建立在慣性儀表的輸出的基礎(chǔ)上,為了驗證導(dǎo)航解算的正確與否,及后續(xù)的組合導(dǎo)航研究,需要設(shè)計模擬載體運動的軌跡發(fā)生器.軌跡發(fā)生器的設(shè)計思路是根據(jù)假定載體運動軌跡,在特定的坐標(biāo)系下,采用適當(dāng)?shù)淖藨B(tài)矩陣算法推導(dǎo)出慣性儀表的理論輸出值,同時模擬出載體運動的位置,速度,姿態(tài)以及陀螺儀的輸出值,加速度計的輸出值,再加上相應(yīng)的隨機誤差值.