[發明專利]光電吊艙三軸陀螺儀漂移實時修正方法

有效

• 申請號: 201911037314.8
• 申請日: 2019-10-29
• 公開（公告）號: CN110986928B
• 公開（公告）日: 2021-08-20
• 發明（設計）人: 劉述超;衡思蘭
• 申請（專利權）人: 成都縱橫自動化技術股份有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01C25/00
• 代理公司: 成都行之專利代理事務所(普通合伙) 51220
• 代理人: 林菲菲

1.光電吊艙三軸陀螺儀漂移實時修正方法，其特征在于，該方法包括以下步驟：

步驟1，將光電吊艙和慣性姿態傳感器均安裝在載體上，光電吊艙基座相對于慣性姿態傳感器靜止，光電吊艙基座體軸坐標系平行于慣性姿態傳感器體軸坐標系，且光電吊艙基座體軸坐標系X軸與慣性姿態傳感器體軸坐標系X軸同向；

步驟2，慣性姿態傳感器實時輸出測量的姿態數據，包括俯仰姿態角E_{S_pitch}，滾轉姿態角E_{S_roll}和偏航姿態角E_{S_pan}；

步驟3，根據步驟2的姿態數據計算得到光電吊艙三軸陀螺儀姿態角，包括俯仰姿態角L，滾轉姿態角E_{L_roll}和偏航姿態角E_{L_pan}；

步驟4，根據陀螺儀三軸原始角速率與陀螺儀三軸角速率漂移估計值計算得到光電吊艙三軸陀螺儀三軸估計角速率矩陣M_A；

步驟5，根據光電吊艙三軸陀螺儀姿態角計算三軸陀螺儀姿態角四元素矩陣M_Z；

步驟6，根據光電吊艙三軸陀螺儀估計角速率矩陣M_A和三軸陀螺儀姿態角四元素矩陣M_Z計算得到三軸陀螺儀姿態估計角；

步驟7，根據三軸陀螺儀姿態角和陀螺儀三軸角速率漂移估計值，得到當前陀螺儀三軸角速率漂移估計值；

步驟8，根據陀螺儀三軸原始角速率與步驟7得到的當前陀螺儀三軸角速度漂移估計值，得到漂移修正后的陀螺儀三軸角速率；所述步驟3具體通過下式計算得到光電吊艙三軸陀螺儀姿態角：

式中，θ_A表示光電吊艙方位平臺旋轉角度，θ_E表示光電吊艙俯仰平臺旋轉角度，載體姿態角繞方位平臺軸旋轉θ_A得到方位平臺姿態角，包括俯仰姿態角E_{A_pitch}，滾轉姿態角E_{A_roll}和偏航姿態角E_{A_pan}，方位平臺姿態角繞俯仰平臺軸旋轉θ_E得到俯仰平臺姿態角即三軸陀螺儀姿態角，包括俯仰姿態角E_{E_pitch}，滾轉姿態角E_{E_roll}和偏航姿態角E_{E_pan}；因慣性姿態傳感器與載體相對靜止，即慣性姿態傳感器輸出的俯仰姿態角E_{S_pitch}、滾轉姿態角E_{S_roll}和偏航姿態角E_{S_pan}分別于載體俯仰姿態角E_{B_pitch}、載體滾轉姿態角E_{B_roll}和載體偏航姿態角相等E_{B_pan}；所述步驟4通過下式計算得到吊艙三軸陀螺儀估計角速率矩陣M_A：

PQR_{Esti_roll}＝PQR_{L_rolt}-D_{L_roll}

PQR_{Esti_pitch}＝PQR_{L_pitch}-D_{L_pitch}

PQR_{Esti_pan}＝PQR_{L_pan}-D_{L_pan}

式中，PQR_{L_roll}表示三軸陀螺儀原始滾轉角速率，PQR_{L_pitch}表示三軸陀螺儀原始俯仰角速率和PQR_{L_pan}表示三軸陀螺儀原始偏航角速率；D_{L_roll}表示三軸陀螺儀滾轉軸角速率漂移估計值，D_{L_pitch}表示三軸陀螺儀俯仰軸角速率漂移估計值，D_{L_pan}表示三軸陀螺儀偏航軸角速率漂移估計值，dt表示系統運行間隔時間；所述步驟5通過下式計算得到三軸陀螺儀姿態四元素矩陣M_Z：

sin Phi＝sin(E_{L_roll}/2)

cos Phi＝cos(E_{L_roll}/2)

sin Theta＝sin(E_{L_pitch}/2)

cos Theta＝cos(E_{L_pitch}/2)

sin Psi＝sin(E_{L_pan}/2)

cos Psi＝cos(E_{L_pan}/2)

式中，Phi表示三軸陀螺儀滾轉角，Theta表示三軸陀螺儀俯仰角，Psi表示三軸陀螺儀偏航角；所述步驟6具體包括以下步驟：

系統運行時，執行步驟(1)～(5)；系統正常運行時，執行步驟(2)～(5)：

步驟(1)系統開始運行，初始化矩陣Q、R、H、M_x和M_P：

Q＝0.0001*H

R＝10*H

M_P＝H

式中，H表示單位矩陣，Q表示過程噪聲，R表示測量噪聲，M_X表示狀態矩陣，M_p表示初始協方差矩陣；

步驟(2)計算狀態估計矩陣M_XP和協方差

M_XP＝M_A*M_X

式中，M_A^I表示M_A的逆矩陣；

步驟(3)計算三軸陀螺儀姿態估計角的估計增益矩陣K：

式中，H^I表示H的逆矩陣；

步驟(4)計算三軸陀螺儀姿態估計角四元素矩陣M_X與更新協方差矩陣M_P：

M_X＝M_XP+K*(M_Z-H*M_XP)

步驟(5)計算三軸陀螺儀姿態估計角：

E_{esti_L_roll}＝atan2(2*(M_X[3]*M_X[4]+M_X[1]*M_X[2])，1-2*(M_X[2]*M_X[2]+M_X[3]*M_X[3]))

E_{esti_L_pitch}＝-asin(2*(M_X[2]*M_X[4]-M_X[1]*M_X[3]))

E_{esti_L_pan}＝atan2(2*(M_X[2]*M_X[3]+M_X[1]*M_X[4])，1-2*(M_X[3]*M_X[3]+M_X[4]*M_X[4]))

E_{esti_L_pitch}表示俯仰姿態估計角，E_{esti_L_roll}表示滾轉姿態估計角，E_{esti_L_pan}表示偏航姿態估計角；所述步驟7具體由三軸陀螺儀姿態角與陀螺儀三軸速率漂移估計值的代數和，然后通過一個PI控制器輸出得到當前陀螺儀三軸角速率漂移估計值，包括俯仰軸漂移估計值D_{L_pitch}，滾轉軸漂移估計值D_{L_roll}和偏航軸漂移D_{L_pan}；

所述步驟8具體由陀螺儀三軸角速率值與當前陀螺儀三軸角速率漂移估計值的代數和，再通過一個低通濾波器得到漂移修正后的陀螺儀三軸角速率。