一種組合導(dǎo)航系統(tǒng)的高精度定姿方法
【專利摘要】本發(fā)明公開了一種組合導(dǎo)航系統(tǒng)的高精度定姿方法，它包括以下步驟：根據(jù)陀螺儀角速度信號和當(dāng)前狀態(tài)信息解算當(dāng)前時刻方向余弦矩陣；計算該矩陣當(dāng)前列向量對前面列向量的投影，在最優(yōu)的條件下，對當(dāng)前列進(jìn)行歸一化處理；根據(jù)正交歸一化處理后的新的方向余弦矩陣求解姿態(tài)角。本發(fā)明通過四元數(shù)法求解當(dāng)前時刻方向余弦矩陣，計算量??；在得到當(dāng)前時刻方向余弦矩陣后，采用施密特正交法對當(dāng)前時刻方向余弦矩陣進(jìn)行歸一化處理，大大降低了后續(xù)姿態(tài)信息解算的誤差，提高了定姿精度；在最優(yōu)條件下，即基于誤差的二范數(shù)的代價函數(shù)最小，對當(dāng)前時刻方向余弦矩陣采用施密特正交方法進(jìn)行歸一化處理，能得到穩(wěn)定、高效的解析解。
【專利說明】一種組合導(dǎo)航系統(tǒng)的高精度定姿方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種組合導(dǎo)航系統(tǒng)的高精度定姿方法。

【背景技術(shù)】
[0002]從慣性導(dǎo)航(INS)的工作原理和誤差分析可以看出，慣導(dǎo)系統(tǒng)的自主性很強，它可以連續(xù)的提供包括基準(zhǔn)在內(nèi)的全部導(dǎo)航參數(shù)，并且具有非常好的短期精度和穩(wěn)定性，在許多精確制導(dǎo)武器中得到了廣泛的應(yīng)用。但是由于導(dǎo)航系統(tǒng)使用的慣性傳感器的精度不高，且誤差積累而無法單獨長期工作。
[0003]衛(wèi)星導(dǎo)航(GNSS)雖然具有定位和測速精度高，定位誤差與時間無關(guān)，并能全天候?qū)Ш降膬?yōu)點，但是單GNSS不能提供載體的姿態(tài)信息，且易受干擾。由于GNSS和INS的組合導(dǎo)航系統(tǒng)能去其短而取其長，完成較高精度、長時間的導(dǎo)航任務(wù)，在近幾年獲得了廣泛的應(yīng)用。在組合導(dǎo)航系統(tǒng)中，載體的姿態(tài)角解算是一步核心工作。
[0004]現(xiàn)有姿態(tài)測量的方法有很多，如采用加速度計測出的重力分量解算姿態(tài)角信息，該測量方法雖然成本低，但其動態(tài)性能卻比較差；如專利申請?zhí)?201210487249.0所述的一種基于GPS載波雙差方程的姿態(tài)角直接求解方法，通過建立載波雙差方程，將載體姿態(tài)信息引入到載波雙差方程中進(jìn)行解算，該方法能減小誤差，但可靠性及穩(wěn)定性差。
[0005]其他的還有通過方向余弦矩陣解算姿態(tài)信息，但由于計算過程中的舍入誤差，由基于四元數(shù)的姿態(tài)傳遞方程解算后的方向余弦矩陣其正交性不能滿足，容易對后續(xù)姿態(tài)解算造成較大的誤差。


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

[0006]本發(fā)明的目的在于克服現(xiàn)有技術(shù)的不足，提供高可靠度、高穩(wěn)定性、計算量小的一種組合導(dǎo)航系統(tǒng)的高精度定姿方法，它在最優(yōu)的條件下對方向余弦矩陣進(jìn)行正交歸一化，大大降低了姿態(tài)解算的誤差，提高了定姿精度。
[0007]本發(fā)明的目的是通過以下技術(shù)方案來實現(xiàn)的:一種組合導(dǎo)航系統(tǒng)的高精度定姿方法，它包括以下步驟:
51:根據(jù)陀螺儀角速度信號和當(dāng)前狀態(tài)信息解算當(dāng)前時刻方向余弦矩陣；
52:根據(jù)方向余弦矩陣求解姿態(tài)角。
[0008]所述的當(dāng)前時刻方向余弦矩陣解算包括以下子步驟:
511:計算載體系相對于慣性系的角速度在導(dǎo)航系中的投影；
512:計算地球系相對于慣性系的角速度在導(dǎo)航系中的投影；
513:計算導(dǎo)航系相對于地球的角速度；
514:計算載體相對于導(dǎo)航系的角速度并用四元數(shù)表示；
515:根據(jù)四元數(shù)姿態(tài)傳遞方程求解當(dāng)前時刻方向余弦矩陣C。
[0009]所述的根據(jù)方向余弦矩陣求解姿態(tài)角包括如下子步驟:
S21:根據(jù)方向余弦矩陣元素C (3，2)和C (3,3)的反正切求解橫滾角； 522:根據(jù)方向余弦矩陣元素C (3,1)的反正弦求解俯仰角；
523:根據(jù)方向余弦矩陣元素C (2，I)和C (1，I)的反正切求解方位角。
[0010]在根據(jù)方向余弦矩陣求解姿態(tài)角之前它還包括如下步驟:在最優(yōu)條件下，對當(dāng)前時刻方向余弦矩陣采用施密特正交化方法進(jìn)行正交歸一化處理；
所述的采用施密特正交化方法進(jìn)行正交歸一化處理包括如下子步驟:
531:分別計算當(dāng)前列向量對前面的列向量的投影；
532:當(dāng)前列向量減去投影向量得到新向量；
533:計算新向量的二范數(shù)；
534:新向量各元素除以其二范數(shù)得到歸一化后的新向量；
S35:得到新的方向余弦矩陣；
536:判斷新的方向余弦矩陣的所有列是否均已進(jìn)行正交歸一化處理；
537:若是，跳轉(zhuǎn)到步驟S2 ;若否，則跳轉(zhuǎn)到步驟S31，依次循環(huán)。
[0011]進(jìn)一步的，所述的最優(yōu)條件指基于誤差的二范數(shù)的代價函數(shù)最小。
[0012]本發(fā)明的有益效果是:通過四元數(shù)法求解方向余弦矩陣，計算量小；在根據(jù)當(dāng)前時刻方向余弦矩陣解算姿態(tài)信息之前先對當(dāng)前時刻方向余弦矩陣采用施密特正交法進(jìn)行歸一化處理，大大降低了姿態(tài)信息解算的誤差，提高了定姿精度；在最優(yōu)條件下(即基于誤差的二范數(shù)的代價函數(shù)最小)對當(dāng)前時刻方向余弦矩陣采用施密特正交方法進(jìn)行歸一化處理，能得到穩(wěn)定、高效的解析解。

【專利附圖】

【附圖說明】
[0013]圖1為本發(fā)明的計算方法流程圖。

【具體實施方式】
[0014]下面結(jié)合附圖進(jìn)一步詳細(xì)描述本發(fā)明的技術(shù)方案，但本發(fā)明的保護(hù)范圍不局限于以下所述。
[0015]如圖1所示，一種組合導(dǎo)航系統(tǒng)的高精度定姿方法，它包括以下步驟:
S1:根據(jù)陀螺儀角速度信號和當(dāng)前狀態(tài)信息解算當(dāng)前時刻方向余弦矩陣；
S2:根據(jù)方向余弦矩陣求解姿態(tài)角。
[0016]所述的當(dāng)前時刻方向余弦矩陣解算包括以下子步驟:
511:計算載體系相對于慣性系的角速度在導(dǎo)航系中的投影Wib ；
512:計算地球系相對于慣性系的角速度在導(dǎo)航系中的投影Wie ；
513:計算導(dǎo)航系相對于地球的角速度Wen ；
514:計算載體相對于導(dǎo)航系的角速度Wnb并用四元數(shù)表示；
515:根據(jù)四元數(shù)姿態(tài)傳遞方程求解當(dāng)前時刻方向余弦矩陣C。
[0017]所述的根據(jù)方向余弦矩陣求解姿態(tài)角包括如下子步驟:
521:根據(jù)方向余弦矩陣元素C (3，2)和C (3,3)的反正切求解橫滾角；
522:根據(jù)方向余弦矩陣元素C (3,1)的反正弦求解俯仰角；
523:根據(jù)方向余弦矩陣元素C (2，I)和C (1，I)的反正切求解方位角。
[0018]在根據(jù)方向余弦矩陣求解姿態(tài)角之前它還包括如下步驟:在最優(yōu)條件下，對當(dāng)前時刻方向余弦矩陣采用施密特正交化方法進(jìn)行正交歸一化處理。
[0019]所述的采用施密特正交化方法進(jìn)行正交歸一化處理包括如下子步驟:
531:分別計算當(dāng)前列向量對前面的列向量的投影；
532:當(dāng)前列向量減去投影向量得到新向量；
533:計算新向量的二范數(shù)；
534:新向量各元素除以其二范數(shù)得到歸一化后的新向量；
535:得到新的方向余弦矩陣；
536:判斷新的方向余弦矩陣的所有列是否均已進(jìn)行正交歸一化處理；
537:若是，跳轉(zhuǎn)到步驟S2 ;若否，則跳轉(zhuǎn)到步驟S31，依次循環(huán)。
[0020]進(jìn)一步的，所述的最優(yōu)條件指基于誤差的二范數(shù)的代價函數(shù)最小。
【權(quán)利要求】
1.一種組合導(dǎo)航系統(tǒng)的高精度定姿方法，它包括以下步驟: S1:根據(jù)陀螺儀角速度信號和當(dāng)前狀態(tài)信息解算當(dāng)前時刻方向余弦矩陣； S2:根據(jù)方向余弦矩陣求解姿態(tài)角； 所述的當(dāng)前時刻方向余弦矩陣解算包括以下子步驟: 511:計算載體系相對于慣性系的角速度在導(dǎo)航系中的投影； 512:計算地球系相對于慣性系的角速度在導(dǎo)航系中的投影； 513:計算導(dǎo)航系相對于地球的角速度； 514:計算載體相對于導(dǎo)航系的角速度并用四元數(shù)表示； 515:根據(jù)四元數(shù)姿態(tài)傳遞方程求解當(dāng)前時刻方向余弦矩陣C ； 所述的根據(jù)方向余弦矩陣求解姿態(tài)角包括如下子步驟: 521:根據(jù)方向余弦矩陣元素C (3，2)和C (3,3)的反正切求解橫滾角； 522:根據(jù)方向余弦矩陣元素C (3,1)的反正弦求解俯仰角； 523:根據(jù)方向余弦矩陣元素C (2，I)和C (1，I)的反正切求解方位角； 其特征在于:在根據(jù)方向余弦矩陣求解姿態(tài)角之前它還包括如下步驟:在最優(yōu)條件下，對當(dāng)前時刻方向余弦矩陣采用施密特正交化方法進(jìn)行正交歸一化處理； 所述的采用施密特正交化方法進(jìn)行正交歸一化處理包括如下子步驟: 531:分別計算當(dāng)前列向量對前面的列向量的投影； 532:當(dāng)前列向量減去投影向量得到新向量； 533:計算新向量的二范數(shù)； 534:新向量各元素除以其二范數(shù)得到歸一化后的新向量； 535:得到新的方向余弦矩陣； 536:判斷新的方向余弦矩陣的所有列是否均已進(jìn)行正交歸一化處理； 537:若是，跳轉(zhuǎn)到步驟S2 ;若否，則跳轉(zhuǎn)到步驟S31，依次循環(huán)。
2.根據(jù)權(quán)利要求1所述的一種組合導(dǎo)航系統(tǒng)的高精度定姿方法，其特征在于:所述的最優(yōu)條件指基于誤差的二范數(shù)的代價函數(shù)最小。
【文檔編號】G01C21/20GK104180807SQ201310197914
【公開日】2014年12月3日 申請日期:2013年5月25日 優(yōu)先權(quán)日:2013年5月25日
【發(fā)明者】羅明剛 申請人:成都國星通信有限公司