[發(fā)明專利]一種激光陀螺慣導系統的系統級溫度誤差補償方法

有效

• 申請?zhí)枺?/td>: 202110304853.4
• 申請日: 2021-03-15
• 公開（公告）號: CN113029199B
• 公開（公告）日: 2022-03-15
• 發(fā)明（設計）人: 于旭東;王子超;謝元平;羅暉;魏國;王林;高春峰
• 申請（專利權）人: 中國人民解放軍國防科技大學
• 主分類號: G01C25/00
• 分類號: G01C25/00;G01C21/16
• 代理公司: 湖南企企衛(wèi)知識產權代理有限公司 43257
• 代理人: 任合明

1.一種激光陀螺慣導系統的系統級溫度誤差補償方法，采用的測試裝置包括：溫度變化試驗箱、慣性元件數據采集系統、導航解算計算機和三軸轉臺，其特征在于，該方法步驟如下：

S1：將激光陀螺慣導系統安裝在帶溫度變化試驗箱的三軸轉臺上，將慣導系統上電，設定溫度變化試驗箱初始溫度為0℃；

S2：待慣導系統中激光陀螺和加速度計的溫度監(jiān)測值波動范圍小于0.1℃后，控制三軸轉臺尋零后靜置1200s，進行慣導系統初始對準：包括解析法粗對準和卡爾曼濾波精對準，其中解析法粗對準用時120s，卡爾曼濾波精對準用時1080s；

S3：設置溫度變化試驗箱按+10℃/小時的溫度變化速率從0℃升溫至50℃；在升溫過程中控制三軸轉臺，使慣導系統按照下列19位置標定路徑循環(huán)運動兩次：1.繞中框旋轉-90°，轉動結束后置停180s；2.繞中框旋轉-180°，轉動結束后置停180s；3.繞中框旋轉-180°，轉動結束后置停180s；4.繞內框旋轉-90°，轉動結束后置停180s；5.繞內框旋轉-180°，轉動結束后置停180s；6.繞內框旋轉-180°，轉動結束后置停180s；7.繞中框旋轉-90°，轉動結束后置停180s；8.繞中框旋轉-180°，轉動結束后置停180s；9.繞中框旋轉-180°，轉動結束后置停180s；10.繞中框旋轉-90°，轉動結束后置停180s；11.繞中框旋轉-90°，轉動結束后置停180s；12.繞中框旋轉-90°，轉動結束后置停180s；13.繞內框旋轉-90°，轉動結束后置停180s；14.繞內框旋轉-90°，轉動結束后置停180s；15.繞內框旋轉-90°，轉動結束后置停180s；16.繞中框旋轉-90°，轉動結束后置停180s；17.繞中框旋轉-90°，轉動結束后置停180s；18.繞中框旋轉-90°，轉動結束后置停180s；選取加速度計表頭作為溫度采樣點，利用慣性元件數據采集系統采集加速度計的溫度值，采集激光陀螺角增量和加速度計比力增量信息，將激光陀螺角增量和加速度計比力增量信息輸入導航解算計算機進行導航解算，輸出速度誤差和位置誤差；

S4：構建激光陀螺和加速度計誤差模型，構建一個42維卡爾曼濾波器，并將S3輸出的速度誤差和位置誤差作為觀測量輸入卡爾曼濾波器，濾波估計激光陀螺和加速度計的常值零偏、標度因數誤差、安裝誤差，以及加速度計的誤差溫度系數；

具體步驟如下：

S4.1建立激光陀螺和加速度計的誤差模型：

定義i系為地心慣性系，b系為載體系，其坐標原點位于載體的質心處，坐標軸構成右手直角坐標系，n系為導航系，x軸、y軸、z軸分別指向北、東、地方向；

激光陀螺的誤差模型為：

加速度計的誤差模型為：

在式(1)和式(2)中，下標x、y、z分別表示沿x軸、y軸、z軸方向的分量，為簡便起見，后面在定義時，帶下標x、y、z的參數都沒有分開定義，f^b分別為激光陀螺角增量和加速度計比力增量的理論值在b系下的分量，δf^b分別為激光陀螺角增量和加速度計比力增量的誤差量在b系下的分量，B_g為激光陀螺常值零偏，B_a為加速度計常值零偏，δK_g為激光陀螺標度因數誤差，δK_a為加速度計標度因數誤差，δM_gyx為y軸激光陀螺與載體系xy平面的安裝誤差角，δM_gzx為z軸激光陀螺與載體系xz平面的安裝誤差角，δM_gzy為z軸激光陀螺與載體系yz平面的安裝誤差角，δM_ayx為y軸加速度計與載體系xy平面的安裝誤差角，δM_azx為z軸加速度計與載體系xz平面的安裝誤差角，δM_axy為x軸加速度計與載體系xy平面的安裝誤差角，δM_azy為z軸加速度計與載體系yz平面的安裝誤差角，δM_axz為x軸加速度計與載體系xz平面的安裝誤差角，δM_ayz為y軸加速度計與載體系yz平面的安裝誤差角，TB_a為加速度計零偏一階溫度系數，TK_a為加速度計標度因數誤差一階溫度系數，TM_ayx為y軸加速度計與載體系xy平面的安裝誤差角的一階溫度系數，TM_azx為z軸加速度計與載體系xz平面的安裝誤差角的一階溫度系數，TM_axy為x軸加速度計與載體系xy平面的安裝誤差角的一階溫度系數，TM_azy為z軸加速度計與載體系yz平面的安裝誤差角的一階溫度系數，TM_axz為x軸加速度計與載體系xz平面的安裝誤差角的一階溫度系數，TM_ayz為y軸加速度計與載體系yz平面的安裝誤差角的一階溫度系數，T_a為加速度計的溫度值；

S4.2建立卡爾曼濾波器的系統狀態(tài)方程和觀測方程：

S4.2.1包含北、東、地三個方向的姿態(tài)誤差速度誤差δV_N,δV_E,δV_D，緯度、經度、高度誤差δL,δλ,δh和激光陀螺、加速度計誤差模型中33個誤差參數的42維卡爾曼濾波器狀態(tài)方程為：

其中，42維狀態(tài)向量X為：

為42維狀態(tài)向量X的微分；

F為狀態(tài)轉移矩陣，其表達式如下：

其中各分塊矩陣的表達式為：

式(6)至式(24)中，ω_ie為地球自轉角速率，V_N,V_E,V_D分別為北向、天向和垂向速度，L為緯度，h為高度，R_N,R_E分別為地球子午圈、卯酉圈曲率半徑，f_N,f_E,f_D分別為加速度計輸出比力在導航系北向、東向和垂向的投影，為b系至n系的姿態(tài)變換矩陣，表示姿態(tài)變換矩陣的第i行第j列元素；

式(3)中，G為系統噪聲驅動矩陣，其表達式為：

W(t)為系統噪聲矩陣，其表達式為：

式(26)中，W_g(t)為激光陀螺輸出白噪聲，W_a(t)為加速度計輸出白噪聲，設Q_g為激光陀螺輸出白噪聲方差，Q_a為加速度計輸出白噪聲方差，滿足如下條件：

S4.2.2以速度誤差和位置誤差作為觀測量構建卡爾曼濾波器的觀測方程：

Z＝HX+v(t) (29)

Z為包含速度誤差和位置誤差的觀測量，其表達式為：

Z＝[δV_N δV_E δV_D δL δλ δh]^T (30)

H為觀測矩陣，其表達式為：

v(t)為觀測噪聲矩陣，其表達式為：

ν(t)＝[ν_VN(t) ν_VE(t) ν_VD(t) ν_L(t) ν_λ(t) ν_h(t)]^T (32)

式(32)中，ν_VN(t),ν_VE(t),ν_VD(t)分別為北向、東向、垂向速度觀測噪聲，ν_L(t),ν_λ(t),ν_h(t)分別為緯度L、經度λ和高度h位置觀測噪聲，設R_V為速度觀測白噪聲方差，R_p為位置觀測白噪聲方差，滿足如下條件：

S4.3利用卡爾曼濾波算法估計狀態(tài)參量：

S4.2構建了連續(xù)條件下的卡爾曼濾波器的系統狀態(tài)方程和觀測方程，為了能夠使用計算機編程實現，需要將卡爾曼濾波器離散化；狀態(tài)轉移矩陣F的離散化過程為：

式中，F_k/k-1表示離散化的從k-1時刻至k時刻的狀態(tài)轉移矩陣，為描述簡便，此處進行如下定義：帶下標k-1、k的參數分別表示該參數在k-1時刻和k時刻的值，帶下標k/k-1的參數表示該參數從k-1時刻至k時刻的一步預測；I為單位矩陣，T為濾波周期；

根據離散卡爾曼濾波器的基本原理，在已知k-1時刻的狀態(tài)量估計值的基礎上，可以估計出k時刻的狀態(tài)量，由此可得，在給定初始時刻的狀態(tài)量后，可以遞推估計出任意時刻的狀態(tài)量，狀態(tài)量的估計過程表示為：

在式(36)中，表示狀態(tài)量的估計值，K為濾波增益；濾波增益K的計算方式為：

在式(37)中，P為濾波估計的誤差方差矩陣，P的更新過程滿足遞推算法，在給定濾波估計的誤差方差矩陣P的初始值P₀后，可以遞推估計出任意時刻的P；Q₀為激光陀螺和加速度計輸出白噪聲的均方誤差矩陣的初始值，R₀為觀測噪聲方差陣的初始值；

綜上，在給定系統狀態(tài)量初始值X₀、濾波估計的誤差方差矩陣初始值P₀、激光陀螺和加速度計輸出白噪聲的方差矩陣初始值Q₀和觀測噪聲方差陣初始值R₀，并且確定狀態(tài)轉移矩陣F、觀測量Z和觀測矩陣H具體形式的條件下，由式(35)至(37)就可以遞推估計出任意時刻的狀態(tài)量，即可以得到激光陀螺和加速度計的常值零偏、標度因數誤差、安裝誤差，以及加速度計的誤差溫度系數在任意時刻的濾波估計值；

S5：建立激光陀螺和加速度計的標定參數誤差反饋模型；將S4卡爾曼濾波器估計得到的激光陀螺和加速度計的常值零偏、標度因數誤差、安裝誤差，以及加速度計的誤差溫度系數代入標定參數誤差反饋模型，完成誤差修正；

具體步驟如下：

S5.1建立激光陀螺和加速度計的標定參數誤差反饋模型：

激光陀螺的標度因數誤差反饋模型的表達式為：

為激光陀螺的誤差修正前的標度因數，K_g為誤差修正后的標度因數；

激光陀螺的安裝關系矩陣誤差反饋模型的表達式為：

為激光陀螺的誤差修正前的安裝關系矩陣，M_g為誤差修正后的安裝關系矩陣；

加速度計的標度因數誤差反饋模型的表達式為：

為加速度計的誤差修正前的標度因數，K_a為誤差修正后的標度因數，ΔT_a為加速度計的溫度變化率值；

加速度計的安裝關系矩陣誤差反饋模型的表達式為：

為加速度計的誤差修正前的安裝關系矩陣，M_a為修正后的安裝關系矩陣；

加速度計常值零偏誤差反饋模型的表達式為：

為誤差修正前的加速度計常值零偏，B_a為經過誤差修正后的加速度計常值零偏；

S5.2將S4卡爾曼濾波器估計得到的激光陀螺標度因數誤差δK_g，激光陀螺安裝誤差角δM_g，加速度計標度因數誤差δK_a，加速度計安裝誤差角δM_a，以及加速度計標度因數誤差一階溫度系數TK_a，加速度計安裝誤差一階溫度系數TM_a和加速度計零偏一階溫度系數TB_a代入S5.1所建立的標定參數誤差反饋模型，完成誤差修正。

2.一種根據權利要求1所述激光陀螺慣導系統的系統級溫度誤差補償方法，其特征在于：S4中，系統狀態(tài)量初始值設置為X₀＝[0_1×42]^T；

濾波估計的誤差方差矩陣P的初始值P₀設置為：

激光陀螺和加速度計輸出白噪聲的均方誤差矩陣Q的初始值Q₀設置為：

Q₀＝diag{(0.0005°/h)²,(0.0005°/h)²,(0.0005°/h)²,(1ug)²,(1ug)²,(1ug)²}

觀測噪聲方差陣R的初始值R₀設置為：

R₀＝diag{(0.01m/s)²,(0.01m/s)²,(0.01m/s)²,(0.001°)²,(0.001°)²,(0.001°)²}。