[发明专利]光电吊舱三轴陀螺仪漂移实时修正方法

权利要求书

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具体由陀螺仪三轴角速率值与当前陀螺仪三轴角速率漂移估计值的代数和，再通过一个低通滤波器得到漂移修正后的陀螺仪三轴角速率。