資源描述:
《捷聯(lián)慣導(dǎo)系統(tǒng)算法》由會(huì)員上傳分享,免費(fèi)在線閱讀,更多相關(guān)內(nèi)容在行業(yè)資料-天天文庫。
1、捷聯(lián)慣導(dǎo)基本算法捷聯(lián)慣導(dǎo)系統(tǒng)算法概述算法:從慣性儀表輸出到導(dǎo)航與控制信息捷聯(lián)慣導(dǎo)算法的基本內(nèi)容:一、系統(tǒng)初始化:1、給定飛行器初始位置、速度等2、數(shù)學(xué)平臺(tái)的初始對(duì)準(zhǔn)3、慣性儀表的校準(zhǔn)二、慣性儀表的誤差補(bǔ)償三、姿態(tài)矩陣的計(jì)算四、導(dǎo)航計(jì)算五、導(dǎo)航控制信息的提取慣性器件誤差加速度計(jì)慣性器件的誤差補(bǔ)償姿態(tài)計(jì)算歐拉角微分方程1姿態(tài)矩陣的計(jì)算假設(shè)數(shù)學(xué)坐標(biāo)系模擬地理坐標(biāo)系飛行器姿態(tài)的描述:航向角ψ、俯仰角θ、滾動(dòng)角γ一、歐拉微分方程從地理坐標(biāo)系到載體坐標(biāo)系的旋轉(zhuǎn)順序:Ψ→θ→γ方向余弦矩陣:姿態(tài)計(jì)算歐拉角微分方程2飛行器相對(duì)地理坐標(biāo)系的角速度:姿
2、態(tài)計(jì)算歐拉角微分方程3求解歐拉角速率得注意事項(xiàng):當(dāng)θ=90度時(shí),方程出現(xiàn)奇點(diǎn)姿態(tài)計(jì)算矩陣方程精確解1二、方向余弦矩陣微分方程及其解其中由于陀螺儀直接測(cè)得的是載體相對(duì)慣性空間的角速度,所以:導(dǎo)航計(jì)算可以得到有因此得姿態(tài)計(jì)算矩陣方程精確解2的精確解(畢卡逼近):其中方向不變時(shí)的精確解九個(gè)微分方程求解,計(jì)算量大姿態(tài)計(jì)算四元數(shù)精確解1三、四元數(shù)微分方程式及其解由第一章,四元數(shù)微分方程式:對(duì)的處理類似上一節(jié)精確解:其中:姿態(tài)計(jì)算四元數(shù)精確解2其中:姿態(tài)計(jì)算姿態(tài)航向角計(jì)算1四、姿態(tài)和航向角的計(jì)算根據(jù)載體和地理坐標(biāo)系之間的方向余弦矩陣可確定姿態(tài)、
3、航向角姿態(tài)、航向角真值的判斷姿態(tài)計(jì)算姿態(tài)航向角計(jì)算2如利用四元數(shù)微分方程求解,則先利用四元數(shù)求解結(jié)果計(jì)算方向余弦矩陣的元素(1-58):姿態(tài)實(shí)時(shí)計(jì)算概述姿態(tài)矩陣的實(shí)時(shí)計(jì)算因假定“數(shù)學(xué)平臺(tái)”跟蹤地理坐標(biāo)系,因此所以可得相應(yīng)的姿態(tài)矩陣微分方程(6-12):或四元數(shù)微分方程:注意事項(xiàng):1、上述兩個(gè)方程中的角速度表達(dá)式不一樣2、方程第二項(xiàng)較小,計(jì)算時(shí)速度可以低一些增量算法矩陣方程精確解一、角增量算法角增量:陀螺儀數(shù)字脈沖輸出,每個(gè)脈沖代表一個(gè)角增量一個(gè)采樣周期內(nèi),陀螺輸出脈沖數(shù)對(duì)應(yīng)的角增量為:1、矩陣微分方程計(jì)算根據(jù)矩陣微分方程的精確解(6
4、-20),有:(解的第一項(xiàng))增量算法矩陣方程CS參數(shù)展開合并上式,得其中增量算法矩陣方程1階將前式簡(jiǎn)寫為:或離散形式:ΔC按Cn、Sn取不同的近似值,形成相應(yīng)的一階~四階算法一階算法:令可將上述算法解寫成矩陣元素的形式:增量算法矩陣方程1階——一階增量算法增量算法矩陣方程2-4階當(dāng)Cn、Sn取n=2,3,4時(shí):二階增量算法:三階增量算法:四階增量算法:增量算法四元數(shù)2、四元數(shù)微分方程的計(jì)算:其中,I為單位四元數(shù),[Δθ]如(6-24)所示:寫成迭代形式:增量算法四元數(shù)設(shè)一階算法:增量算法四元數(shù)或展開為元素形式:增量算法四元數(shù)同理,可
5、得二階算法:三階算法:四階算法:數(shù)值積分1階用一階~四階龍格-庫塔積分矩陣和四元數(shù)微分方程1、一階龍格-庫塔法一個(gè)矩陣微分方程當(dāng)初始條件已知,其一階龍格-庫塔的解為:方程的解為初始值加上以初始點(diǎn)斜率為斜率的一個(gè)增量斜率K的準(zhǔn)確度不同,解的精確度也不同數(shù)值積分1階矩陣(1)姿態(tài)矩陣微分方程簡(jiǎn)化為其一階龍格-庫塔解:展開為元素形式:與一階增量算法一致數(shù)值積分1階四元數(shù)(2)四元數(shù)微分方程或一階龍格-庫塔解數(shù)值積分2階矩陣2、二階龍格-庫塔法對(duì)一階算法適當(dāng)改進(jìn),使平均斜率更準(zhǔn)確一些二階龍格-庫塔算法的解:(1)矩陣微分方程數(shù)值積分2階矩陣
6、二階龍格-庫塔解:設(shè)則數(shù)值積分2階矩陣數(shù)值積分2階四元數(shù)(2)四元數(shù)微分方程數(shù)值積分2階四元數(shù)數(shù)值積分4階矩陣3、四階龍格-庫塔法則解(1)矩陣微分方程則解數(shù)值積分4階四元數(shù)(2)四元數(shù)微分方程則解數(shù)值積分4階四元數(shù)數(shù)值積分4階四元數(shù)角速度提取龍格-庫塔積分需要用到角速度信息而陀螺儀的數(shù)字脈沖輸出為角增量如果采用周期T很小,可以近似把角速度看成常值或線性變化1、把角速度看成常值,則周期T內(nèi)——一階角速率提取2、把角速度看成線性變化則角增量角速度提取從ti到ti+T/2的角增量:從ti到ti+T的角增量:求解,得因此ti時(shí)刻陀螺輸出置
7、零ti+T/2時(shí)刻陀螺不置零ti+T時(shí)刻陀螺置零角速度提取如果ti+T/2時(shí)刻陀螺輸出置零從ti到ti+T/2的角增量:從ti+T/2到ti+T的角增量:代入得