惯导解算的流程及步骤

惯导解算就是根据加速度计和陀螺仪测得的数据和初始值(包括经纬高、东向、北向、天向速度和俯仰角、偏航角、滚转角)递推出载体在运行过程中的位置(经纬高)、速度(东北天)和姿态角(俯仰偏航滚转角)。其流程步骤如下：

第1步,设置初始参数。

(1)设置初始位置、速度与姿态角。

(2)初始输出载体坐标系(右前上坐标系)到导航坐标系(东北天坐标系)的状态矩阵(以Z、X、Y的顺序依次旋转)。

(3)初始化位置矩阵,即求Cge。

式中,Ceg＝Cge＝S′。

(4)初始化四元数并判断正负。

通过第1步,可得到参与惯导解算的初始参数。

第2步,提取陀螺仪和加速度计的数据。

陀螺仪和加速度计的数据根据本节的数据模拟方法产生。

第3步,惯导解算。

(1)提取初始参数,即提取第1步中的12个初始参数值。

(2)计算RM、RN与g(与纬度有关函数)。

式中g0＝9.78 m/s2；Re＝6 378 137.0m(84坐标系中的地球长轴)。

(3)计算。

(4)速度更新。

(www.xing528.com)

式中,Δt——时间间隔。

(5)位置更新。

(6)姿态更新。

①四元数更新。

②归一化四元数。

③姿态矩阵Cnb更新。

④姿态角更新。

姿态角真值可由表2.5确定。因此,可以得到当前时刻的位置、速度和姿态角。

图6.10与图6.11分别为陀螺仪和加速度计模拟数据,图6.12所示为某型火箭弹的真实轨迹与惯导解算轨迹,从图中可以看出两者轨迹几乎重合,说明其误差很小。图6.13所示为某型火箭弹惯导解算的射程与实际仿真的射程之差,约0.3 m,在可接受范围内,可见本节所述的模拟方法是可行的。

图6.10　陀螺仪模拟数据

图6.11　加速度计模拟数据

图6.12　某型火箭弹真实轨迹与惯导解算轨迹(书后附彩插)

图6.13　惯导解算的射程与火箭弹实际仿真的射程之差