[发明专利]一种发射惯性坐标系下的紧耦合导航方法

权利要求书

1.一种发射惯性坐标系下的紧耦合导航方法，其特征在于，包括以下步骤：

S1、利用数值更新算法更新发射惯性坐标系下的捷联惯性导航系统数值；

S2、利用步骤S1更新后的捷联惯性导航系统数值和卫星导航接收机接收数值计算得到捷联惯性导航系统下伪距和伪距率，并通过卫星导航接收机得到卫星导航系统下伪距和伪距率；

S3、利用卡尔曼滤波对根据步骤S2中捷联惯性导航系统下伪距和伪距率与卫星导航系统下伪距和伪距率计算得到的量测量进行误差分析，得到捷联惯性导航系统误差，具体包括以下分步骤：

S31、利用步骤S2得到的捷联惯性导航系统的伪距和伪距率，分别与卫星导航系统的伪距和伪距率作差，得到量测量、；

S32、以发射惯性坐标系下的捷联惯性导航系统误差与卫星定位误差作为状态量，构建卡尔曼滤波状态方程，表示为：

其中，为卡尔曼滤波的状态矢量，为姿态误差，为姿态误差的微分，为速度误差，为速度误差的微分，为位置误差，为位置误差的微分，为陀螺仪零偏，为陀螺仪零偏的微分，为加速度计零偏，为加速度计零偏的微分，为卫星收机时钟偏差造成的距离误差，卫星接收机时钟偏差造成的距离误差的微分，为卫星接收机时钟漂移，为卫星接收机时钟漂移的微分，分别为陀螺仪、加速度计、时钟偏差和时钟漂移的白噪声，分别表示矩阵维数为的零矩阵；

S33、利用步骤S31得到的量测量、，构建卡尔曼滤波量测方程，表示为：

其中，为从发射惯性坐标系下的误差矢量转换到地心地固坐标系下的转换矩阵，M为当前可见卫星颗数，为M行的列向量，、分别表示矩阵维数为的零矩阵，为卫星位置的测量误差矩阵，为卫星速度的测量误差矩阵，为卡尔曼滤波的状态矢量；

S34、利用步骤S32得到的卡尔曼状态方程与步骤S33得到的卡尔曼滤波量测方程构建卡尔曼滤波系统，并利用该卡尔曼滤波系统计算得到捷联惯性导航姿态误差、速度误差、位置误差；

S4、利用步骤S3得到的捷联惯性导航系统误差与步骤S1更新后的捷联惯性导航系统数值进行修正，得到修正后的捷联惯性导航系统数值；

S5、利用步骤S4修正后的捷联惯性导航系统数值对捷联惯性导航系统进行校正与反馈。

2.根据权利要求1所述的一种发射惯性坐标系下的紧耦合导航方法，其特征在于，所述步骤S1具体为：

根据加速度计敏感的比力信息和陀螺仪敏感的角速度信息采用数值更新算法中姿态数值更新算法、速度数值更新算法和位置数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统姿态、速度和位置信息进行数值更新，得到更新后的捷联惯性导航系统数值，其中数值更新算法采用二子样算法。

3.根据权利要求2所 述的一种发射惯性坐标系下的紧耦合导航方法，其特征在于，所述步骤S1中利用姿态数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的姿态信息进行更新，具体为：

先利用陀螺仪敏感的角速度计算时刻到时刻的姿态转换四元数，再利用该姿态转换四元数对姿态更新前时刻的姿态进行更新，得到更新后时刻的姿态四元数，表示为：

。

4.根据权利要求2所述的一种发射惯性坐标系下的紧耦合导航方法，其特征在于，所述步骤S1中利用速度数值更新算法对发射惯性坐标系下飞行器发射时刻的捷联惯性导航系统的速度信息进行更新，具体为：

先利用载体坐标系下加速度计敏感的比力计算捷联卫星导航系比力速度增量，再利用采样时段T内捷联卫星导航系统比力速度增量、采样时段T内捷联卫星导航引力速度增量对位置更新前时刻捷联惯性导航系统速度进行更新，得到更新后时刻的捷联惯性导航系统速度，表示为：

。