角速率輸入條件下單子樣旋轉(zhuǎn)矢量姿態(tài)方法
【專利摘要】本發(fā)明公開了一種角速率輸入條件下單子樣旋轉(zhuǎn)矢量姿態(tài)方法，該方法基于角速率下多子樣旋轉(zhuǎn)矢量算法和角增量下單子樣旋轉(zhuǎn)矢量算法，將單子樣算法應(yīng)用到基于角速率的圓錐誤差補(bǔ)償算法中，具體利用當(dāng)前時(shí)刻及前N個(gè)時(shí)刻的角速率來擬合圓錐誤差補(bǔ)償項(xiàng)，最優(yōu)補(bǔ)償項(xiàng)的系數(shù)由解線性方程組得到，可以在陀螺儀只提供角速率的情況下，利用角速率輸入完成姿態(tài)解算并且保持解算頻率與采樣頻率一致。本發(fā)明的方法既沒有引入由角速率提取角增量帶來的誤差，又沒有降低系統(tǒng)的姿態(tài)更新頻率。
【專利說明】角速率輸入條件下單子樣旋轉(zhuǎn)矢量姿態(tài)方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于導(dǎo)航【技術(shù)領(lǐng)域】，具體涉及應(yīng)用于捷聯(lián)慣性導(dǎo)航系統(tǒng)的姿態(tài)解算方法?！颈尘凹夹g(shù)】
[0002]捷聯(lián)式慣性導(dǎo)航系統(tǒng)(Strap-downInertial Navigation System, SINS)是將加速度計(jì)和陀螺儀直接安裝在載體上，在計(jì)算機(jī)中實(shí)時(shí)計(jì)算姿態(tài)矩陣，即計(jì)算出載體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的關(guān)系，從而把載體坐標(biāo)系的加速度計(jì)信息轉(zhuǎn)換為導(dǎo)航坐標(biāo)系下的信息，然后進(jìn)行導(dǎo)航計(jì)算。
[0003]隨著SINS應(yīng)用越來越廣泛，對其精度及動(dòng)態(tài)性能要求也越來越高。姿態(tài)更新算法為捷聯(lián)慣導(dǎo)算法的核心，其中要解決的關(guān)鍵問題是剛體做有限轉(zhuǎn)動(dòng)時(shí)的旋轉(zhuǎn)不可交換性誤差。國內(nèi)外學(xué)者提出的多子樣旋轉(zhuǎn)矢量算法能夠有效地對不可交換性誤差進(jìn)行了補(bǔ)償。但如果在現(xiàn)有采樣頻率下直接應(yīng)用多子樣旋轉(zhuǎn)矢量姿態(tài)算法，會(huì)降低SINS姿態(tài)更新頻率；若提高采樣頻率，又會(huì)使硬件負(fù)擔(dān)加重以及量化誤差更加嚴(yán)重。
[0004]同時(shí)，上述算法均建立在陀螺儀輸出為角增量的基礎(chǔ)上，不能直接應(yīng)用于輸出為角速率的情況。若從角速率中提取出角增量再應(yīng)用于傳統(tǒng)的多子樣旋轉(zhuǎn)矢量算法，這樣會(huì)導(dǎo)致補(bǔ)償精度下降。有學(xué)者提出直接用角速率來計(jì)算旋轉(zhuǎn)矢量的算法，避免了提取角增量引入的誤差。但其本身仍是一種多子樣算法，同樣存在姿態(tài)更新頻率降低的問題。

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

[0005]為了同時(shí)解決傳統(tǒng)多子樣旋轉(zhuǎn)矢量姿態(tài)算法會(huì)降低SINS姿態(tài)更新頻率和傳統(tǒng)角增量算法不能直接應(yīng)用到角速率輸入的問題，提出了一種角速率輸入條件下的單子樣旋轉(zhuǎn)矢量姿態(tài)方法。
[0006]本發(fā)明的技術(shù)方案為:一種角速率輸入條件下的單子樣旋轉(zhuǎn)矢量姿態(tài)方法，具體包括如下步驟:
[0007]步驟S1.獲取設(shè)當(dāng)前k時(shí)刻的姿態(tài)角為
【權(quán)利要求】
1.一種角速率輸入條件下的單子樣旋轉(zhuǎn)矢量姿態(tài)方法，具體包括如下步驟: 步驟S1.獲取設(shè)當(dāng)前k時(shí)刻的姿態(tài)角為attk=[ Θ Y ψ]τ, θ、Y和ψ分別為俯仰角、橫滾角和偏航角； 步驟S2、計(jì)算旋轉(zhuǎn)矢量ΔΦ; 步驟S3、計(jì)算當(dāng)前k時(shí)刻的姿態(tài)四元數(shù)Q (tk)，具體過程為: 姿態(tài)角與坐標(biāo)轉(zhuǎn)換矩陣Cbn的關(guān)系為:
2.根據(jù)權(quán)利要求1所述的一種角速率輸入條件下的單子樣旋轉(zhuǎn)矢量姿態(tài)方法，其特征在于，步驟S2所述的旋轉(zhuǎn)矢I ΔΦ 11體為利用當(dāng)前時(shí)刻角速率和前四個(gè)時(shí)刻角速率的五階單子樣算法的旋轉(zhuǎn)矢量。
3.根據(jù)權(quán)利要求2所述的一種角速率輸入條件下的單子樣旋轉(zhuǎn)矢量姿態(tài)方法，其特征在于，所述的旋轉(zhuǎn)失I卩Δ0的具體計(jì)算公式為:

【文檔編號(hào)】G01C21/16GK103759731SQ201410021105
【公開日】2014年4月30日 申請日期:2014年1月16日 優(yōu)先權(quán)日:2014年1月16日
【發(fā)明者】黃盼, 滕云龍, 張曉
申請人:電子科技大學(xué)