[发明专利]一种导航定位方法及设备

权利要求书

1.一种导航定位方法，其特征在于，包括：

当检测到卫星信号失效时，以第k个计算周期的磁航向角为观测量，通过所述第k个计算周期的DR/磁航向组合导航算法模型得到所述第k个计算周期的第一状态量组合，以所述第k个计算周期的横向速度为观测量，通过所述第k个计算周期的DR/车辆运动组合导航算法模型得到所述第k个计算周期的第二状态量组合，所述k为大于2的整数；

根据所述第k个计算周期的系统状态方程，针对所述第k个计算周期的所述第一状态量组合和所述第k个计算周期的所述第二状态量组合进行闭环卡尔曼组合滤波，得到所述第k个计算周期的第一误差修正值；

根据所述第k个计算周期的所述第一误差修正值修正所述第k个计算周期的初始航向角，得到所述第k个计算周期的目标航向角，根据所述第k个计算周期的所述目标航向角和所述第k个计算周期的DR模型，计算得到所述第k个计算周期的目标速度和目标位置。

2.根据权利要求1所述的方法，其特征在于，所述第k个计算周期的DR/磁航向组合导航算法模型是根据所述第k个计算周期的初始航向角以及所述第k个计算周期的磁航向角而建立的，所述第k个计算周期的磁航向角是根据磁传感器输出数据计算得到的，所述第k个计算周期的初始航向角是根据第k-1个计算周期的目标航向角和第k个计算周期获取的角速度计算得到的，所述第k个计算周期的DR/车辆运动组合导航算法模型是根据所述第k个计算周期的横向速度而建立的，所述第k个计算周期的横向速度是根据所述第k个计算周期的初始速度和所述第k个计算周期的初始航向角计算得到的，所述第k个计算周期的初始速度是根据所述第k个计算周期的初始航向角、第k-1个计算周期的目标速度以及所述第k个计算周期的MEMS惯性传感器加速度计输出值计算得到；

所述第k个计算周期的系统状态方程是根据所述第k-1个计算周期的状态量而建立的；

所述第k个计算周期的DR模型是根据所述第k个计算周期的初始速度和初始位置而建立的，所述第k个计算周期的初始位置是根据所述第k个计算周期的初始速度和第k-1个计算周期的目标位置计算得到的。

3.根据权利要求1所述的方法，其特征在于，所述方法还包括：

当检测到卫星信号有效时，以第k+1个计算周期的初始速度和初始位置为观测量，通过所述第k+1个计算周期的DR/卫星组合导航算法模型得到所述第k+1个计算周期的第三状态量组合，所述第k+1个计算周期的DR/卫星组合导航算法模型是根据所述第k+1个计算周期的所述初始速度和所述初始位置而建立的；

根据所述第k+1个计算周期的系统状态方程，针对所述第k+1个计算周期的所述第一状态量组合和所述第k+1个计算周期的所述第三状态量组合进行所述闭环卡尔曼组合滤波，得到所述第k+1个计算周期的第二误差修正值；

根据所述第k+1个计算周期的所述第二误差修正值修正所述第k+1个计算周期的初始航向角，得到所述第k+1个计算周期的目标航向角，根据所述第k+1个计算周期的所述目标航向角和所述第k+1个计算周期的DR模型，计算得到所述第k+1个计算周期的目标速度和目标位置。

4.根据权利要求1所述的方法，其特征在于，所述第k个计算周期的所述第一状态量组合包括:所述第k个计算周期的磁航向角组合量测状态Z_Mag，k，第一量测矩阵H_Mag，以及所述第k个计算周期的第一量测噪声V_Mag，k；

基于如下公式计算所述Z_Mag，k

Z_Mag,k＝[Φ_DR,k-Φ_Mag,k]

基于如下公式计算所述H_Mag

H_Mag＝[1 0 0 0 0 0 0 0]

基于如下公式计算所述V_Mag，k

其中，Φ_DR,k为所述第k个计算周期初始航向角，Φ_Mag，k为所述第k个计算周期的磁航向角，为第k个计算周期得到的航向角误差。