一种导航定位方法及设备专利检索-磁传感器传感器与探测器专利检索查询-专利查询网

一种导航定位方法及设备

阅读：13发布：2020-05-11

专利汇可以提供一种导航定位方法及设备专利检索，专利查询，专利分析的服务。并且本发明实施例公开了一种导航定位方法及设备，包括：当检测到卫星信号失效时，以第k个计算周期的磁航向角为观测量，通过DR/磁航向组合导航算法模型得到第一状态量组合，以第k个计算周期的横向速度为观测量，通过DR/车辆运动组合导航算法模型得到第二状态量组合；根据第k个计算周期的系统状态方程，针对第一状态量组合和第二状态量组合进行闭环卡尔曼组合滤波得到第一误差修正值；根据第一误差修正值修正第k个计算周期的初始航向角得到目标航向角，根据目标航向角和第k个计算周期的DR模型，计算得到目标速度和目标位置。采用本发明实施例，实现了卫星信号无效时的车辆定位导航，提高了车载定位的准确性。，下面是一种导航定位方法及设备专利的具体信息内容。

权利要求

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个计算周期的磁航向角组合量测状态ZMag，k，第一量测矩阵HMag，以及所述第k个计算周期的第一量测噪声VMag，k；
基于如下公式计算所述ZMag，k
ZMag,k＝[ΦDR,k-ΦMag,k]
基于如下公式计算所述HMag
HMag＝[1 0 0 0 0 0 0 0]
基于如下公式计算所述VMag，k
其中，ΦDR,k为所述第k个计算周期初始航向角，ΦMag，k为所述第k个计算周期的磁航向角，为第k个计算周期得到的航向角误差。
5.根据权利要求1所述的方法，其特征在于，所述第k个计算周期的所述第二状态量组合包括：所述第k个计算周期的车辆横向速度量测状态ZVirt，k，所述第k个计算周期的第二量测矩阵HVirt，k，以及所述第k个计算周期的第二量测噪声VVirt，k；
基于如下公式计算所述ZVirt，k
基于如下公式计算所述HVirt，k
HVirt,k＝[-νeDR,ksinΦDR,k-νnDR,kcosΦDR,k cosΦDR,k -sinΦDR,k 0 0 0 0 0 0]基于如下公式计算所述VVirt，k
VVirt,k＝[δvx,k]
其中，分别为所述第k个计算周期得到的东向和北向速度误差，veDR,k为
所述第k个计算周期的初始东向速度；vnDR,k为所述第k个计算周期的初始北向速度；δvx,k为所述第k个计算周期车辆运动时由于侧滑所引起的附加横向速度变化，其中，ΦDR,k为所述第k个计算周期初始航向角，为第k个计算周期得到的航向角误差。
6.根据权利要求3所述的方法，其特征在于，所述第k+1个计算周期的第三状态量组合包括：所述第k+1个计算周期的经度、纬度、东向速度和北向速度的组合量测状态ZSat,k+1，所述第k+1个计算周期的第三量测矩阵HSat,k+1，以及所述第k+1个计算周期的第三量测噪声VSat,k+1；
基于如下公式计算所述ZSat,k+1
基于如下公式计算所述HSat,k+1
基于如下公式计算所述VSat,k+1
其中，λDR,k+1和LDR,k+1分别为所述第k+1个计算周期的经度和纬度；λSat,k+1、LSat,k+1、veSat,k+1和vnSat,k+1分别为所述第k+1个计算周期的卫星接收机定位的经度、纬度、东向速度以及北向速度；Rm和Rn分别为子午圈半径和卯酉圈半径；NeSat,k+1、NnSat,k+1、MeSat,k+1和MnSat,k+1分别为所述第k+1个计算周期卫星接收机计算出来的定位信息本身含有的经度、纬度、东向速度和北向速度误差；
其中，veDR，k，vnDR，k分别为所述第k个计算周期得到的东向和北向速度误差。
7.根据权利要求1所述的方法，其特征在于，所述第k个计算周期的系统状态方程为：
Xk＝Ak,k-1Xk-1+Gk-1Wk-1
其中，Ak,k-1为所述第k-1个计算周期的状态转移矩阵，Xk为所述第k个计算周期的系统状态量， Gk-1为所述第k-1个计算周期的系
统噪声驱动矩阵，Wk-1为所述第k-1个计算周期的白噪声随机误差矢量，和分别为所述第k个计算周期得到的精度和纬度误差，εr为陀螺的一阶马尔科夫误差，εb随机常数误差，和分别为所述第k个计算周期的加速度计右向和前向一阶马尔科夫误差；
其中，为第k个计算周期得到的航向角误差，分别为所述第k个计
算周期得到的东向和北向速度误差。
8.根据权利要求1所述的方法，其特征在于，所述根据所述第k个计算周期的系统状态方程，针对所述第k个计算周期的所述第一状态量组合和所述第k个计算周期的所述第二状态量组合进行闭环卡尔曼组合滤波得到所述第k个计算周期的第一误差修正，包括：
通过如下公式计算得到所述第k个计算周期的第一误差修正值
其中，为所述第k个计算周期的第一误差修正值，
其中，Ak,k-1为所述第k-1个计算周期的状态转移矩阵，Gk-1为所述第k-1个计算周期的系统噪声驱动矩阵；
其中，所述第k个计算周期的所述第二状态量组合包括：所述第k个计算周期的车辆横向速度量测状态ZVirt，k，所述第k个计算周期的第二量测矩阵HVirt，k，以及所述第k个计算周期的第二量测噪声VVirt，k；
其中，所述第k个计算周期的所述第一状态量组合包括第k个计算周期磁航向角组合量测状态ZMag，k、第一量测矩阵HMag、第k个计算周期的第一量测噪声VMag，k。
9.根据权利要求3所述的方法，其特征在于，根据所述第k+1个计算周期的系统状态方程，针对所述第k+1个计算周期的所述第一状态量组合和所述第k+1个计算周期的所述第三状态量组合进行所述闭环卡尔曼组合滤波得到所述第k+1个计算周期的第二误差修正值，包括：
通过如下公式计算得到所述第k+1个计算周期的第二误差修正值
其中，为所述第k+1个计算周期的第二误差修正值，
其中，Ak+1,k为所述第k个计算周期的状态转移矩阵，Gk为所述第k个计算周期的系统噪声驱动矩阵，Qk为所述第k个计算周期的系统噪声矩阵；
其中，所述第k+1个计算周期的所述第三状态量组合包括：所述第k+1个计算周期的卫星量测状态ZSat,k+1，所述第k+1个计算周期的第三量测矩阵HSat,k+1，以及所述第k+1个计算周期的第三量测噪声VSat,k+1；
其中，所述第k+1个计算周期的所述第一状态量组合包括第k+1个计算周期磁航向角组合量测状态ZMag,k+1、所述第k+1个计算周期的第一量测矩阵HMag,k+1、第k+1个计算周期的第一量测噪声VMag,k+1。
10.根据权利要求1所述的方法，其特征在于，所述根据所述第k个计算周期的所述第一误差修正值修正所述第k个计算周期的初始航向角以得到所述第k个计算周期的目标航向角，包括：
通过如下公式计算得到所述第k个计算周期的目标航向角Φ′k，
其中，所述Φ′k为所述第k个计算周期的目标航向角；
其中，ΦDR,k为所述第k个计算周期初始航向角，为第k个计算周期得到的航向角误差。
11.根据权利要求1所述的方法，其特征在于，所述根据所述第k个计算周期的所述目标航向角和所述第k个计算周期的DR模型，计算得到所述第k个计算周期的目标速度和目标位置，包括：
通过如下公式计算得到所述第k个计算周期的目标东向速度ve，k，所述第k个计算周期目标北向速度vn，k，所述第k个计算周期的目标经度λk和所述第k个计算周期的目标纬度Lk，fe,k＝f′x,kcosΦ′k+f′y,ksinΦ′k
fn,k＝-f′x,ksinΦ′k+f′y,kcosΦ′k
ve,k＝ve,k-1+Tfe,k
vn,k＝vn,k-1+Tfn,k
Lk＝Lk-1+Tvn,k/Rm
λk＝λk-1+Tve,k/(RncosLk)
其中，f′x，k和f′y，k分别为所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值的修正值，ve，k为所述第k个计算周期的目标东向速度，vn，k为所述第k个计算周期的目标北向速度；fe,k和fn,k分别为所述第k个计算周期的东向和北向的加速度投影；
T为传感器采样时间间隔；Lk为所述第k个计算周期的目标纬度，λk为所述第k个计算周期的目标经度；
其中，Rm和Rn分别为子午圈半径和卯酉圈半径；
其中，Φ′k表示所述第k个计算周期的目标航向角。
12.根据权利要求11所述的方法，其特征在于，所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值的修正值f′x，k和f′y，k分别由所述MEMS惯性传感器安装过程中存在的俯仰角偏差和横滚角偏差修正所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值所得。
13.一种导航定位设备，其特征在于，包括处理单元和通信单元，
所述处理单元用于当通过所述通信单元检测到卫星信号失效时，以第k个计算周期的磁航向角为观测量，通过所述第k个计算周期的DR/磁航向组合导航算法模型得到所述第k个计算周期的第一状态量组合，以所述第k个计算周期的横向速度为观测量，通过所述第k个计算周期的DR/车辆运动组合导航算法模型得到所述第k个计算周期的第二状态量组合，所述k为大于2的整数；以及用于根据所述第k个计算周期的系统状态方程，针对所述第k个计算周期的所述第一状态量组合和所述第k个计算周期的所述第二状态量组合进行闭环卡尔曼组合滤波，得到所述第k个计算周期的第一误差修正值；以及用于根据所述第k个计算周期的所述第一误差修正值修正所述第k个计算周期的初始航向角，得到所述第k个计算周期的目标航向角，根据所述第k个计算周期的所述目标航向角和所述第k个计算周期的DR模型，计算得到所述第k个计算周期的目标速度和目标位置。
14.根据权利要求13所述的设备，其特征在于，所述第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个计算周期的目标位置计算得到的。
15.根据权利要求13所述的设备，其特征在于，所述处理单元还用于当通过所述通信单元检测到卫星信号有效时，以第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个计算周期的目标速度和目标位置。
16.根据权利要求13所述的设备，其特征在于，所述第k个计算周期的所述第一状态量组合包括:所述第k个计算周期的磁航向角组合量测状态ZMag，k，第一量测矩阵HMag，以及所述第k个计算周期的第一量测噪声VMag，k；
基于如下公式计算所述ZMag，k
ZMag,k＝[ΦDR,k-ΦMag,k]
基于如下公式计算所述HMag
HMag＝[1 0 0 0 0 0 0 0]
基于如下公式计算所述VMag，k
其中，ΦDR,k为所述第k个计算周期初始航向角，ΦMag，k为所述第k个计算周期的磁航向角，为第k个计算周期得到的航向角误差。
17.根据权利要求13所述的设备，其特征在于，所述第k个计算周期的所述第二状态量组合包括：所述第k个计算周期的车辆横向速度量测状态ZVirt，k，所述第k个计算周期的第二量测矩阵HVirt，k，以及所述第k个计算周期的第二量测噪声VVirt，k；
基于如下公式计算所述ZVirt，k
基于如下公式计算所述HVirt，k
HVirt,k＝[-νeDR,ksinΦDR,k-νnDR,kcosΦDR,k cosΦDR,k-sinΦDR,k 0 0 0 0 0 0]基于如下公式计算所述VVirt，k
VVirt,k＝[δvx,k]
其中，分别为所述第k个计算周期得到的东向和北向速度误差，veDR,k为
所述第k个计算周期的初始东向速度；vnDR,k为所述第k个计算周期的初始北向速度；δvx,k为所述第k个计算周期车辆运动时由于侧滑所引起的附加横向速度变化；
其中，ΦDRk为所述第k个计算周期初始航向角，为第k个计算周期得到的航向角误差。
18.根据权利要求15所述的设备，其特征在于，所述第k+1个计算周期的第三状态量组合包括：所述第k+1个计算周期的经度、纬度、东向速度和北向速度的组合量测状态ZSat,k+1，所述第k+1个计算周期的第三量测矩阵HSat,k+1，以及所述第k+1个计算周期的第三量测噪声VSat,k+1；
基于如下公式计算所述ZSat,k+1
基于如下公式计算所述HSat,k+1
基于如下公式计算所述VSat,k+1
其中，λDR,k+1和LDR,k+1分别为所述第k+1个计算周期的经度和纬度；λSat,k+1、LSat,k+1、veSat,k+1和vnSat,k+1分别为所述第k+1个计算周期的卫星接收机定位的经度、纬度、东向速度以及北向速度；Rm和Rn分别为子午圈半径和卯酉圈半径；NeSat,k+1、NnSat,k+1、MeSat,k+1和MnSat,k+1分别为所述第k+1个计算周期卫星接收机计算出来的定位信息本身含有的经度、纬度、东向速度和北向速度误差；
其中，veDR，k，vnDR，k分别为所述第k个计算周期得到的东向和北向速度误差。
19.根据权利要求13所述的设备，其特征在于，所述第k个计算周期的系统状态方程为：
Xk＝Ak,k-1Xk-1+Gk-1Wk-1
其中，Ak,k-1为所述第k-1个计算周期的状态转移矩阵，Xk为所述第k个计算周期的系统状态量， Gk-1为所述第k-1个计算周期的系
统噪声驱动矩阵，Wk-1为所述第k-1个计算周期的白噪声随机误差矢量，和分别为所述第k个计算周期得到的精度和纬度误差，εr为陀螺的一阶马尔科夫误差，εb随机常数误差，和分别为所述第k个计算周期的加速度计右向和前向一阶马尔科夫误差；
其中，为第k个计算周期得到的航向角误差，分别为所述第k个计
算周期得到的东向和北向速度误差。
20.根据权利要求13所述的设备，其特征在于，所述处理单元在用于根据所述第k个计算周期的系统状态方程，针对所述第k个计算周期的所述第一状态量组合和所述第k个计算周期的所述第二状态量组合进行闭环卡尔曼组合滤波得到所述第k个计算周期的第一误差修正方面，所述处理单元还用于：
通过如下公式计算得到所述第k个计算周期的第一误差修正值
其中，为所述第k个计算周期的第一误差修正值，
其中，Ak,k-1为所述第k-1个计算周期的状态转移矩阵，Gk-1为所述第k-1个计算周期的系统噪声驱动矩阵；
其中，所述第k个计算周期的所述第二状态量组合包括：所述第k个计算周期的车辆横向速度量测状态ZVirt，k，所述第k个计算周期的第二量测矩阵HVirt，k，以及所述第k个计算周期的第二量测噪声VVirt，k；
其中，所述第k个计算周期的所述第一状态量组合包括：第k个计算周期磁航向角组合量测状态ZMag，k、第一量测矩阵HMag、第k个计算周期的第一量测噪声VMag，k。
21.根据权利要求15所述的设备，其特征在于，所述处理单元在用于根据所述第k+1个计算周期的系统状态方程，针对所述第k+1个计算周期的所述第一状态量组合和所述第k+1个计算周期的所述第三状态量组合进行所述闭环卡尔曼组合滤波得到所述第k+1个计算周期的第二误差修正值方面，所述处理单元还用于：
通过如下公式计算得到所述第k+1个计算周期的第二误差修正值
其中，为所述第k+1个计算周期的第二误差修正值，
其中，Ak+1,k为所述第k个计算周期的状态转移矩阵，Gk为所述第k个计算周期的系统噪声驱动矩阵，Qk为所述第k个计算周期的系统噪声矩阵；
其中，所述第k+1个计算周期的所述第三状态量组合包括：所述第k+1个计算周期的卫星量测状态ZSat,k+1，所述第k+1个计算周期的第三量测矩阵HSat,k+1，以及所述第k+1个计算周期的第三量测噪声VSat,k+1；
其中，所述第k+1个计算周期的所述第一状态量组合包括第k+1个计算周期磁航向角组合量测状态ZMag,k+1、所述第k+1个计算周期的第一量测矩阵HMag,k+1、第k+1个计算周期的第一量测噪声VMag,k+1。
22.根据权利要求13所述的设备，其特征在于，所述处理单元在用于所述根据所述第k个计算周期的所述第一误差修正值修正所述第k个计算周期的初始航向角以得到所述第k个计算周期的目标航向角方面，所述处理单元还用于：
通过如下公式计算得到所述第k个计算周期的目标航向角Φ′k，
其中，所述Φ′k为所述第k个计算周期的目标航向角；
其中，ΦDR,k为所述第k个计算周期初始航向角，为第k个计算周期得到的航向角误差。
23.根据权利要求13所述的设备，其特征在于，所述处理单元在用于所述根据所述第k个计算周期的所述目标航向角和所述第k个计算周期的DR模型，计算得到所述第k个计算周期的目标速度和目标位置方面，所述处理单元还用于：
通过如下公式计算得到所述第k个计算周期的目标东向速度ve，k，所述第k个计算周期目标北向速度vn，k，所述第k个计算周期的目标经度λk和所述第k个计算周期的目标纬度Lk，fe,k＝f′x,kcosΦ′k+f′y,ksinΦ′k
fn,k＝-f′x,ksinΦ′k+f′y,kcosΦ′k
ve,k＝ve,k-1+Tfe,k
vn,k＝vn,k-1+Tfn,k
Lk＝Lk-1+Tvn,k/Rm
λk＝λk-1+Tve,k/(RncosLk)
其中，f′x，k和f′y，k分别为所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值的修正值，ve，k为所述第k个计算周期的目标东向速度，vn，k为所述第k个计算周期的目标北向速度；fe,k和fn,k分别为所述第k个计算周期的东向和北向的加速度投影；
T为传感器采样时间间隔；Lk为所述第k个计算周期的目标纬度，λk为所述第k个计算周期的目标经度；
其中，Rm和Rn分别为子午圈半径和卯酉圈半径；
其中，Φ′k表示所述第k个计算周期的目标航向角。
24.根据权利要求23所述的设备，其特征在于，所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值的修正值f′x，k和f′y，k分别由所述MEMS惯性传感器安装过程中存在的俯仰角偏差和横滚角偏差修正所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值所得。
25.一种导航定位设备，其特征在于，包括：
处理器、存储器、通信接口和通信总线，所述处理器、所述存储器和所述通信接口通过所述通信总线连接并完成相互间的通信；
所述存储器存储有可执行程序代码，所述通信接口用于无线通信；
所述处理器用于调用所述存储器中的所述可执行程序代码，执行如权利要求1-12任一项所描述的方法。
26.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行以实现权利要求1至12任意一项所述的方法。

说明书全文

一种导航定位方法及设备

技术领域

[0001] 本发明涉及车载导航技术领域，具体涉及一种导航定位方法及设备。

背景技术

[0002] 车载导航定位技术是智能交通系统的一个重要组成部分，也是关键技术之一。车载导航要求能够连续、实时、准确地确定车辆的位置，目前可用于车载导航技术的导航方式有：卫星导航(如全球定位系统、北斗卫星导航系统)、里程计、微机电系统(Microelectro Mechanical Systems,MEMS)传感器、磁传感器、地图匹配等等。卫星导航定位精度较高、技术成熟、使用方便，但是它容易受到高楼、高架、隧道、地下车库等的遮蔽而失效；而以里程计、MEMS传感器进行导航定位的航位推算算法(Dead-reckoning,DR)是一种低成本、自主的车辆导航定位方式，它的优点是抗干扰能力强，能根据提供的传感器数据，在短时间内提供车辆较精确的导航参数，但是DR算法的误差随时间积累，不适合长时间的单独导航；地图匹配算法主要作为一种辅助导航手段，结合数字地图中道路路线情况对导航结果进行辅助修正，以提高定位精度，但存在一定的局限性。

[0003] 目前，国内外的车载导航系统，大多都采用卫星导航和其他定位方法组合的方式来实现对车辆的导航和定位，主要有以下两种组合方式：地图匹配/卫星组合导航系统以及DR/卫星组合导航系统，但是在卫星信号受到干扰甚至失效的情况下，这两种组合导航系统的导航误差将会显著增大甚至不能使用。

发明内容

[0004] 本发明实施例公开了一种导航定位方法及设备，以期实现导航信号无效时的车辆定位导航，提高车载导航定位的准确性。

[0005] 第一方面，本发明实施例公开一种导航定位方法。方法包括：

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

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

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

[0009] 由上可见，本发明实施例公开的导航定位方法，当设备检测到卫星信号失效时，以第k个计算周期的磁航向角为观测量，通过DR/磁航向组合导航算法模型得到第一状态量组合，以第k个计算周期的横向速度为观测量，通过DR/车辆运动组合导航算法模型得到第二状态量组合；根据第k个计算周期的系统状态方程，针对第一状态量组合和第二状态量组合进行闭环卡尔曼组合滤波得到第一误差修正值；根据第一误差修正值修正第k个计算周期的初始航向角得到目标航向角，根据目标航向角和第k个计算周期的DR模型，计算得到目标速度和目标位置。可见，通过DR/磁航向组合导航算法模型和DR/车辆运动组合导航算法模型在卫星信号失效时对航向角进行修正，进而根据DR模型推算出目标速度和目标位置信息，实现了在卫星信号无效时的车辆定位导航，提高了车载定位的准确性。

[0010] 在一个可能的设计中，所述第k个计算周期的DR/磁航向组合导航算法模型是根据所述第k个计算周期的初始航向角以及所述第k个计算周期的磁航向角而建立的，所述第k个计算周期的磁航向角是根据磁传感器输出数据计算得到的，所述第k个计算周期的初始航向角是根据第k-1个计算周期的目标航向角和第k个计算周期获取的角速度计算得到的，所述第k个计算周期的DR/车辆运动组合导航算法模型是根据所述第k个计算周期的横向速度而建立的，所述第k个计算周期的横向速度是根据所述第k个计算周期的初始速度和所述第k个计算周期的初始航向角计算得到的，所述第k个计算周期的初始速度是根据所述第k个计算周期的初始航向角、第k-1个计算周期的目标速度以及所述第k个计算周期的MEMS惯性传感器加速度计输出值计算得到；

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

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

[0013] 在一个可能的设计中，所述方法还包括：

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

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

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

[0017] 由上可见，本可能的设计中公开的导航定位方法，除了实现了在卫星信号无效时的车辆定位导航，提高了车载定位的准确性之外，设备在城市峡谷、高架遮挡等卫星信号受到干扰但非失效环境下，利用第二误差修正值对航向角进行修正，提高了卫星信号受到干扰情况下的导航定位的精度性。

[0018] 在一个可能的设计中，所述第k个计算周期的所述第一状态量组合包括:所述第k个计算周期的磁航向角组合量测状态ZMag，k，第一量测矩阵HMag，以及所述第k个计算周期的第一量测噪声VMag，k；

[0019] 基于如下公式计算所述ZMag，k

[0020] ZMag,k＝[ΦDR,k-ΦMag,k]

[0021] 基于如下公式计算所述HMag

[0022] HMag＝[1 0 0 0 0 0 0 0]

[0023] 基于如下公式计算所述VMag，k

[0024]

[0025] 其中，ΦDR,k为所述第k个计算周期初始航向角，ΦMag，k为所述第k个计算周期的磁航向角，为第k个计算周期得到的航向角误差。

[0026] 在一个可能的设计中，所述第k个计算周期的所述第二状态量组合包括：所述第k个计算周期的车辆横向速度量测状态ZVirt，k，所述第k个计算周期的第二量测矩阵HVirt，k，以及所述第k个计算周期的第二量测噪声VVirt，k；

[0027] 基于如下公式计算所述ZVirt，k

[0028]

[0029] 基于如下公式计算所述HVirt，k

[0030] HVirt,k＝[-νeDR,ksinΦDR,k-νnDR,kcosΦDR,k cosΦDR,k-sinΦDR,k 0 0 0 0 0 0][0031] 基于如下公式计算所述VVirt，k

[0032] VVirt,k＝[δvx,k]

[0033] 其中，分别为所述第k个计算周期得到的东向和北向速度误差，veDR,k为所述第k个计算周期的初始东向速度；vnDR,k为所述第k个计算周期的初始北向速度；
δvx,k为所述第k个计算周期车辆运动时由于侧滑所引起的附加横向速度变化。

[0034] 在一个可能的设计中，所述第k+1个计算周期的第三状态量组合包括：所述第k+1个计算周期的经度、纬度、东向速度和北向速度的组合量测状态ZSat,k+1，所述第k+1个计算周期的第三量测矩阵HSat,k+1，以及所述第k+1个计算周期的第三量测噪声VSat,k+1；

[0035] 基于如下公式计算所述ZSat,k+1

[0036]

[0037] 基于如下公式计算所述HSat,k+1

[0038]

[0039] 基于如下公式计算所述VSat,k+1

[0040]

[0041] 其中，λDR,k+1和LDR,k+1分别为所述第k+1个计算周期的经度和纬度；λSat,k+1、LSat,k+1、veSat,k+1和vnSat,k+1分别为所述第k+1个计算周期的卫星接收机定位的经度、纬度、东向速度以及北向速度；Rm和Rn分别为子午圈半径和卯酉圈半径；NeSat,k+1、NnSat,k+1、MeSat,k+1和MnSat,k+1分别为所述第k+1个计算周期卫星接收机计算出来的定位信息本身含有的经度、纬度、东向速度和北向速度误差。

[0042] 由上可见，本可能的设计中公开的导航定位方法，除了实现了在卫星信号无效时的车辆定位导航，提高了车载定位的准确性之外，设备通过每个计算周期更新的初始速度和初始位置不断地更新所述第一状态量组合，所述第二状态量组合，所述第三状态量组合，提高了所述第一修正值，所述第二修正值的输出精度，保持了每个计算周期导航信息的持续可靠输出。

[0043] 在一个可能的设计中，所述第k个计算周期的系统状态方程为：

[0044] Xk＝Ak,k-1Xk-1+Gk-1Wk-1

[0045] 其中，Ak,k-1为第所述k个计算周期的状态转移矩阵，Xk为所述第k个计算周期的系统状态量， Gk-1为所述第k-1个计算周期的系统噪声矩阵，Wk-1为所述第k-1个计算周期的白噪声随机误差矢量，和分别为所
述第k个计算周期得到的精度和纬度误差，εr为陀螺的一阶马尔科夫误差，εb随机常数误差，和分别为所述第k个计算周期的加速度计右向和前向一阶马尔科夫误差。

[0046] 在一个可能的设计中，所述根据所述第k个计算周期的系统状态方程，针对所述第k个计算周期的所述第一状态量组合和所述第k个计算周期的所述第二状态量组合进行闭环卡尔曼组合滤波得到所述第k个计算周期的第一误差修正，包括：

[0047] 通过如下公式计算得到所述第k个计算周期的第一误差修正值

[0048]

[0049]

[0050]

[0051]

[0052] 其中，为所述第k个计算周期的第一误差修正值，

[0053] 在一个可能的设计中，根据所述第k+1个计算周期的系统状态方程，针对所述第k+1个计算周期的所述第一状态量组合和所述第k+1个计算周期的所述第三状态量组合进行所述闭环卡尔曼组合滤波得到所述第k+1个计算周期的第二误差修正值，包括：

[0054] 通过如下公式计算得到所述第k+1个计算周期的第二误差修正值

[0055]

[0056]

[0057]

[0058]

[0059] 其中，为所述第k+1个计算周期的第二误差修正值，

[0060] 在一个可能的设计中，所述根据所述第k个计算周期的所述第一误差修正值修正所述第k个计算周期的初始航向角以得到所述第k个计算周期的目标航向角，包括：

[0061] 通过如下公式计算得到所述第k个计算周期的目标航向角Φ′k，

[0062]

[0063] 其中，所述Φ′k为所述第k个计算周期的目标航向角。

[0064] 由上可见，本可能的设计中公开的导航定位方法，除了实现了在卫星信号无效时的车辆定位导航，提高了车载定位的准确性之外，设备通过闭环组合卡尔曼滤波算法，对航向角进行修正，进一步提高了导航定位信息的输出精度。

[0065] 在一个可能的设计中，所述根据所述第k个计算周期的所述目标航向角和所述第k个计算周期的DR模型，计算得到所述第k个计算周期的目标速度和目标位置，包括：

[0066] 通过如下公式计算得到所述第k个计算周期的目标东向速度ve，k，所述第k个计算周期目标北向速度vn，k，所述第k个计算周期的目标经度λk和所述第k个计算周期的目标纬度Lk，

[0067] fe,k＝fx′,kcosΦ′k+f′y,ksinΦ′k

[0068] fn,k＝-f′x,ksinΦ′k+f′y,kcosΦ′k

[0069] ve,k＝ve,k-1+Tfe,k

[0070] vn,k＝vn,k-1+Tfn,k

[0071] Lk＝Lk-1+Tvn,k/Rm

[0072] λk＝λk-1+Tve,k/(RncosLk)

[0073] 其中，f′x，k和f′y，k分别为所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值的修正值，ve，k为所述第k个计算周期的目标东向速度，vn，k为所述第k个计算周期的目标北向速度；fe,k和fn,k分别为所述第k个计算周期的东向和北向的加速度投影；T为传感器采样时间间隔；Lk为所述第k个计算周期的目标纬度，λk为所述第k个计算周期的目标经度。

[0074] 在这个可能的设计中，所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值的修正值f′x，k和f′y，k分别由所述MEMS惯性传感器安装过程中存在的俯仰角偏差和横滚角偏差修正所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值所得。

[0075] 由上可见，本可能的设计中公开的导航定位方法，除了实现了在卫星信号无效时的车辆定位导航，提高了车载定位的准确性之外，设备通过对低精度的MEMS惯性传感器安装过程中存在的俯仰角偏差和横滚角偏差进行修正，提高了MEMS惯性传感器水平加速度计的输出精度，进而高了导航定位的目标速度和目标位置的输出精度。

[0076] 第二方面，本发明实施例公开一种设备，该设备具有实现上述方法设计中设备的行为的功能。所述功能可以通过硬件实现，也可以通过硬件执行相应的软件实现。所述硬件或软件包括一个或多个与上述功能相对应的模块。

[0077] 在一个可能的设计中，设备包括处理器，所述处理器被配置为支持设备执行上述方法中相应的功能。进一步的，设备还可以包括接收器和发射器，所述接收器和发射器用于设备与其他设备如导航卫星之间的通信。进一步的，设备还可以包括存储器，所述存储器用于与处理器耦合，其保存设备必要的程序指令和数据。

[0078] 第三方面，本发明实施例公开一种计算机可读存储介质，其中，所述计算机可读存储介质存储有用于计算机设备执行的程序代码，该程序代码具体包括执行指令，所述执行指令用于执行本发明实施例第一方面任一方法中所描述的部分或全部步骤。

[0079] 由上可见，本发明实施例公开的导航定位方法，当设备检测到卫星信号失效时，以第k个计算周期的磁航向角为观测量，通过DR/磁航向组合导航算法模型得到第一状态量组合，以第k个计算周期的横向速度为观测量，通过DR/车辆运动组合导航算法模型得到第二状态量组合；根据第k个计算周期的系统状态方程，针对第一状态量组合和第二状态量组合进行闭环卡尔曼组合滤波得到第一误差修正值；根据第一误差修正值修正第k个计算周期的初始航向角得到目标航向角，根据目标航向角和第k个计算周期的DR模型，计算得到目标速度和目标位置。可见，通过DR/磁航向组合导航算法模型和DR/车辆运动组合导航算法模型在卫星信号失效时对航向角进行修正，进而根据DR模型推算出目标速度和目标位置信息，实现了在卫星信号无效时的车辆定位导航，提高了车载定位的准确性。附图说明

[0080] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

[0081] 图1是本发明实施例公开的一种导航定位方法的流程示意图；

[0082] 图2A是本发明实施例公开的一种导航控制设备的功能单元框图；

[0083] 图2B是本发明实施例公开的一种导航控制设备的结构示意图。

具体实施方式

[0084] 下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行描述。

[0085] 请参阅图1，图1是本发明实施例公开了一种导航定位方法的流程示意图，该方法应用于导航定位系统，该导航定位系统可以是由安装于车辆中的导航显示屏、导航控制设备、卫星接收机、传感器(如磁传感器、惯性传感器(加速度计和陀螺仪)等)组成，如图所示，本导航定位方法包括：

[0086] S101，当导航控制设备检测到卫星信号失效时，以第k个计算周期的磁航向角为观测量，通过所述第k个计算周期的航位推算算法(Dead-reckoning,DR)/磁航向组合导航算法模型得到所述第k个计算周期的第一状态量组合，以所述第k个计算周期的横向速度为观测量，通过所述第k个计算周期的DR/车辆运动组合导航算法模型得到所述第k个计算周期的第二状态量组合；

[0087] 其中，所述k为大于2的整数，所述第k个计算周期的DR/磁航向组合导航算法模型是根据所述第k个计算周期的初始航向角以及所述第k个计算周期的磁航向角而建立的，所述第k个计算周期的磁航向角是根据磁传感器输出数据计算得到的，所述第k个计算周期的初始航向角是根据第k-1个计算周期的目标航向角和第k个计算周期获取的角速度计算得到的，所述第k个计算周期的DR/车辆运动组合导航算法模型是根据所述第k个计算周期的横向速度而建立的，所述第k个计算周期的横向速度是根据所述第k个计算周期的初始速度和所述第k个计算周期的初始航向角计算得到的，所述第k个计算周期的初始速度是根据所述第k个计算周期的初始航向角、第k-1个计算周期的目标速度以及所述第k个计算周期的MEMS惯性传感器加速度计输出值计算得到。

[0088] 具体的，所述第k个计算周期的DR/磁航向组合导航算法模型是根据所述第k个计算周期的初始航向角以及所述第k个计算周期的磁航向角而建立的，包括：

[0089] 首先，所述导航控制设备根据第k个计算周期水平方向磁传感器输出数据，按下式计算第k个计算周期的磁航向角ΦMag，k：

[0090]

[0091] 式中，magx，k和magy，k分别为第k个计算周期磁传感器X轴和Y轴的输出数据。

[0092] 同时，第k个计算周期的初始航向角以及第k个计算周期的磁传感器输出的航向角信息可分别表示为：

[0093]

[0094] ΦMag,k＝Φk-EMag,k

[0095] 式中，ΦDR,k为第k个计算周期的初始航向角，为第k个计算周期得到的航向角误差，EMag，k为第k个计算周期得到的磁航向角误差，Φk表示第k个计算周期的航向角的真实值。

[0096] 其中，第k个计算周期的初始航向角的递推公式为：

[0097] ΦDR,k＝Φ′k-1-Tωz,k

[0098] 式中，第一个计算周期的初始航向角的初始值与第一个计算周期的磁航向角近似相等，所述第一个计算周期的磁航向角是通过所述导航定位系统的磁传感器获取的，Φ′k-1为第k-1个计算周期的目标航向角，ωz,k为第k个计算周期的竖直方向的所述MEMS惯性传感器中的陀螺仪输出的角速度。

[0099] 其次，所述导航控制设备建立第k个计算周期磁航向角组合量测状态方程，包括第一状态量组合ZMag，k、HMag、VMag，k；

[0100] 其中，第k个计算周期磁航向角组合量测状态方程为：

[0101]

[0102] 其中，第一量测矩阵HMag为：

[0103] HMag＝[1 0 0 0 0 0 0 0]

[0104] 第k个计算周期的第一量测噪声VMag，k为：

[0105]

[0106] 具体的，所述第k个计算周期的DR/车辆运动组合导航算法模型是根据所述第k个计算周期的横向速度而建立的，包括：

[0107] 首先，所述导航控制设备建立第k个计算周期的车辆运动速度的量测状态方程模型：

[0108] 根据第k个计算周期的初始东向速度veDR，k和初始北向速度vnDR，k，计算出第k个计算周期的车辆横向速度参数为，

[0109]

[0110] 其中，veDR，k，vnDR，k分别为所述第k个计算周期得到的东向和北向速度误差。

[0111] 其次，所述导航控制设备将所述横向速度进行最小量化展开，获得线性化的量测状态方程：

[0112]

[0113] 再次，在实际车辆运动情况下，考虑到路面不平整以及转弯侧滑等因素，则第k个计算周期的横向速度约束方程为：

[0114] vx,CAR,k＝vx,k-δvx,k＝(veDR,kcosΦDR,k-vnDR,ksinΦDR,k)-δvx,k

[0115] 式中，vx,k为第k个计算周期的横侧向运动速度真实值，δvx,k表示第k个计算周期车辆运动时由于侧滑所引起的附加横向速度变化，建模为白噪声，其取值大小和车辆的具体运动特性有关，当车辆运行速度大时，侧滑速度大。

[0116] 最后，所述导航控制设备根据所述线性化横向速度量测状态方程和所述横向速度约束方程，可以构造第k个计算周期的横向速度虚拟量测方程为：

[0117]

[0118] 从而获得第k个计算周期的车辆横向速度量测状态方程，包括第二状态量组合ZVirt，k、HVirt，k、VVirt，k。

[0119] 所述第k个计算周期的车辆横向速度量测状态方程为：

[0120] ZVirt,k＝HVirt,kXk+VVirt,k

[0121] 其中，第k个计算周期的第二量测矩阵HVirt,k为：

[0122] HVirt,k＝[-νeDR,ksinΦDR,k-νnDR,kcosΦDR,k cosΦDR,k-sinΦDR,k 0 0 0 0 0 0][0123] 第k个计算周期的第二量测噪声VVirt,k为：

[0124] VVirt,k＝[δvx,k]

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

[0126] 其中，所述系统状态方程为：

[0127]

[0128] 式中，A为状态转移矩阵，G为系统噪声矩阵，W白噪声随机误差矢量，其中：

[0129]

[0130]

[0131] W＝[ωgz ωrz ωax ωay]

[0132] 式中，fx和fy分别为静止情况下DR模型中获取的MEMS惯性传感器的加速度计输出的右向和前向加速度值；Rm和Rn分别为子午圈半径和卯酉圈半径；LDR为初始纬度；veDR为初始东向速度；ωgz为陀螺白噪声，ωrz为陀螺一阶马尔科夫驱动白噪声，ωax和ωay为加速度计一阶马尔科夫驱动白噪声；Tε和Ta分别为MEMS惯性传感器中陀螺仪和加速度计的一阶马尔科夫相关时间。

[0133] 其中，所述系统状态方程以航向角误差水平速度误差和位置误差和陀螺的一阶马尔科夫误差εr和随机常数误差εb以及加速度计一阶马尔科夫误差和作为系统状态量，建立系统状态方程，所述系统状态量为：

[0134]

[0135] 将所述系统状态方程离散化处理得第k个计算周期的系统状态方程：

[0136] Xk＝Ak,k-1Xk-1+Gk-1Wk-1

[0137] 其中，

[0138] 具体的，在卫星信号失效时，所述导航控制设备根据所述第k个计算周期的车辆系统状态方程，针对所述第k个计算周期的所述第一状态量组合和所述第k个计算周期的所述第二状态量组合进行闭环卡尔曼组合滤波，得到所述第k个计算周期的第一误差修正值包括：

[0139]

[0140]

[0141]

[0142]

[0143] 式中，

[0144] 其中：

[0145]

[0146]

[0147] P,Q的初始值与MEMS传感器的误差参数有关。

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

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

[0150] 首先，所述导航控制设备根据所述第k个计算周期的所述第一误差修正值修正所述第k个计算周期的初始航向角，得到所述第k个计算周期的目标航向角，包括：

[0151] 通过所述第k个计算周期的所述第一误差修正值中的如下所述公式计算得到所述第k个计算周期的目标航向角Φ′k，

[0152]

[0153] 其中，所述Φ′k为所述第k个计算周期的目标航向角。

[0154] 其次，所述导航控制设备根据所述第k个计算周期的所述目标航向角和所述第k个计算周期的DR模型，计算得到所述第k个计算周期的目标速度和目标位置，包括：

[0155] 根据所述第k个计算周期的初始速度和初始位置建立所述第k个计算周期的DR模型；

[0156] 具体的，所述第k个计算周期的DR模型的导航坐标系选择东北天坐标系，载体坐标系选为右前上坐标系；

[0157] 通过如下第k个计算周期的DR模型公式计算得到所述第k个计算周期的目标东向速度ve，k，所述第k个计算周期目标北向速度vn,k，所述第k个计算周期的目标经度λk和所述第k个计算周期的目标纬度Lk，

[0158] fe,k＝f′x,kcosΦ′k+f′y,ksinΦ′k

[0159] fn,k＝-f′x,ksinΦ′k+f′y,kcosΦ′k

[0160] ve,k＝ve,k-1+Tfe,k

[0161] vn,k＝vn,k-1+Tfn,k

[0162] Lk＝Lk-1+Tvn,k/Rm

[0163] λk＝λk-1+Tve,k/(RncosLk)

[0164] 其中，f′x,k和f′y,k分别为所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值的修正值，ve,k为所述第k个计算周期的目标东向速度，vn,k为所述第k个计算周期的目标北向速度；fe,k和fn,k分别为所述第k个计算周期的东向和北向的加速度投影；T为传感器采样时间间隔；Lk为所述第k个计算周期的目标纬度，λk为所述第k个计算周期的目标经度。

[0165] 其中，所述第k个计算周期微机电系统(Microelectro Mechanical Systems,MEMS)惯性传感器中加速度计输出的右向和前向加速度值的修正值f′x,k和f′y,k分别由所述MEMS惯性传感器安装过程中存在的俯仰角偏差和横滚角偏差修正所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值所得，包括：

[0166] 通过如下所述公式获得修正后第k个计算周期的加速度计输出f′x,k和f′y,k为：

[0167] f'＝(C1C2)-1f

[0168] 其中：

[0169] f′＝[f′x,k f′y,k f′z,k]T

[0170] f＝[fx,k fy,k fz,k]T

[0171]

[0172]

[0173] 式中，Δθ为俯仰角偏差；Δγ为横滚角偏差；fx,k，fy,k和fz,k分别为修正前的右向，前向和上向的加速度计输出值。

[0174] 其中，所述fx,k，fy,k和fz,k为静止情况下第k个计算周期获得的加速度计输出值，根据加速度投影关系，所述Δθ和Δγ可以计算为：

[0175] sinΔθ＝fy,k/g

[0176] tanΔγ＝-fx,k/fz

[0177] 式中，g为重力加速度。

[0178] 进一步的，所述第k个计算周期的第一误差修正值还用于修正下列信息：

[0179]

[0180]

[0181]

[0182]

[0183]

[0184] ωz,k+1＝ωz,k-εb-εr

[0185]

[0186]

[0187] 具体的，通过第k个计算周期的第一误差修正值中的εb,εr，的数据对第k个计算周期的ΦDR,k，veDR,k，vnDR,k，λDR,k，
LDR,k，ωz,k，fx,k，fy,k进行修正，得到第k+1个计算周期的ΦDR,k+1，veDR,k+1，vnDR,k+1，λDR,k+1，LDR,k+1，ωz,k+1，fx,k+1，fy,k+1值，用于第k+1个计算周期的导航定位方法中。

[0188] 本发明实施例公开的导航定位方法，当导航控制设备检测到卫星信号失效时，以第k个计算周期的磁航向角为观测量，通过DR/磁航向组合导航算法模型得到第一状态量组合，以第k个计算周期的横向速度为观测量，通过DR/车辆运动组合导航算法模型得到第二状态量组合；根据第k个计算周期的系统状态方程，针对第一状态量组合和第二状态量组合进行闭环卡尔曼组合滤波得到第一误差修正值；根据第一误差修正值修正第k个计算周期的初始航向角得到目标航向角，根据目标航向角和第k个计算周期的DR模型，计算得到目标速度和目标位置。可见，通过DR/磁航向组合导航算法模型和DR/车辆运动组合导航算法模型在卫星信号失效时对航向角进行修正，进而根据DR模型推算出目标速度和目标位置信息，实现了在卫星信号无效时的车辆定位导航，提高了车载定位的准确性。

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

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

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

[0192] 其中，所述第k+1个计算周期的DR/卫星组合导航算法模型是根据所述第k+1个计算周期的所述初始速度和所述初始位置而建立的，包括：

[0193] 以第k+1个计算周期的初始经度、第k+1个计算周期的初始纬度、第k+1个计算周期的初始东向速度和第k+1个计算周期的初始北向速度作为观测量，建立第k+1个计算周期的量测方程，包括第三状态量组合ZSat,k+1、HSat,k+1、VSat,k+1；

[0194] 所述第k+1个计算周期的量测方程为：

[0195] ZSat,k＝HSat,kXk+VSat,k

[0196] 其中，

[0197] 第k+1个计算周期的第三量测矩阵HSat,k+1为：

[0198]

[0199] 第k+1个计算周期的第三量测噪声VSat,k+1为：

[0200]

[0201] 式中，λDR,k+1和LDR,k+1分别为所述第k+1个计算周期的经度和纬度；λSat,k+1、LSat,k+1、veSat,k+1和vnSat,k+1分别为所述第k+1个计算周期的卫星接收机定位的经度、纬度、东向速度以及北向速度；NeSat,k+1、NnSat,k+1、MeSat,k+1和MnSat,k+1分别为所述第k+1个计算周期卫星接收机计算出来的定位信息本身含有的经度、纬度、东向速度和北向速度误差。

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

[0203] 通过如下公式计算得到所述第k+1个计算周期的第二误差修正值

[0204]

[0205]

[0206]

[0207]

[0208] 式中，

[0209] 其中：

[0210] 具体的，根据所述第k+1个计算周期的所述第二误差修正值修正所述第k+1个计算周期的初始航向角，得到所述第k+1个计算周期的目标航向角，根据所述第k+1个计算周期的所述目标航向角和所述第k+1个计算周期的DR模型，计算得到所述第k+1个计算周期的目标速度和目标位置如S103所描述的方法。

[0211] 由上可见，本示例中提供的导航定位方法，实现了在卫星信号无效时的车辆定位导航，提高了车载定位的准确性之外，设备在城市峡谷、高架遮挡等卫星信号受到干扰但非失效环境下，利用第二误差修正值对初始航向角进行修正，提高了卫星信号受到干扰情况下的导航定位的精度性。

[0212] 上述主要从设备执行过程的角度对本发明实施例的方案进行了介绍。可以理解的是，设备为了实现上述功能，其包含了执行各个功能相应的硬件结构和/或软件模块。本领域技术人员应该很容易意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，本发明能够以硬件或硬件和计算机软件的结合形式来实现。某个功能究竟以硬件还是计算机软件驱动硬件的方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

[0213] 本发明实施例可以根据上述方法示例对设备进行功能单元的划分，例如，可以对应各个功能划分各个功能单元，也可以将两个或两个以上的功能集成在一个处理单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。需要说明的是，本发明实施例中对单元的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。

[0214] 在采用集成的单元的情况下，图2A示出了上述实施例中所涉及的设备的一种可能的导航控制设备的结构示意图。导航控制设备200包括：处理单元202和通信单元203。处理单元202用于对导航控制设备的动作进行控制管理，例如，处理单元202用于支持设备执行图1中的步骤S101至104和/或用于本文所描述的技术的其它过程。通信单元203用于支持移动终端与其他设备如导航卫星之间的通信。导航控制设备还可以包括存储单元201，用于存储设备的程序代码和数据。

[0215] 其中，处理单元202可以是处理器或控制器，例如可以是中央处理器(Central Processing Unit，CPU)，通用处理器，数字信号处理器(Digital Signal Processor，DSP)，专用集成电路(Application-Specific Integrated Circuit，ASIC)，现场可编程门阵列(Field Programmable Gate Array，FPGA)或者其他可编程逻辑器件、晶体管逻辑器件、硬件部件或者其任意组合。其可以实现或执行结合本发明公开内容所描述的各种示例性的逻辑方框，模块和电路。所述处理器也可以是实现计算功能的组合，例如包含一个或多个微处理器组合，DSP和微处理器的组合等等。通信单元203可以是通信接口、收发器、收发电路等，其中，通信接口是统称，可以包括一个或多个接口。存储单元201可以是存储器。

[0216] 当处理单元202为处理器，通信单元203为通信接口，存储单元401为存储器时，本发明实施例所涉及的设备可以为图2B所示的导航控制设备。

[0217] 参阅图2B所示，该导航控制设备210包括：处理器212、通信接口213、存储器211。可选的，导航控制设备210还可以包括总线214。其中，通信接口213、处理器212以及存储器211可以通过总线214相互连接；总线214可以是外设部件互连标准(Peripheral Component Interconnect，简称PCI)总线或扩展工业标准结构(Extended Industry Standard Architecture，简称EISA)总线等。所述总线214可以分为地址总线、数据总线、控制总线等。
为便于表示，图2B中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。

[0218] 上述图2A或图2B所示的导航控制设备也可以理解为一种用于导航控制设备的装置，本发明实施例不限定。

[0219] 本发明实施例还公开一种计算机存储介质，其中，该计算机存储介质可存储有程序，该程序执行时包括上述方法实施例中记载的任何一种导航定位方法的部分或全部步骤。

[0220] 需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本发明所必须的。

[0221] 在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

[0222] 在本发明所公开的几个实施例中，应该理解到，所揭露的装置，可通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性或其它的形式。

[0223] 所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

[0224] 另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

[0225] 所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储器中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储器包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

[0226] 以上对本发明实施例进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

标题	发布/更新时间	阅读量
室内定位融合算法	2020-05-08	583
一种基于振动信号的奶牛个体行为监测装置及方法	2020-05-08	252
全频段磁传感器	2020-05-11	352
基坑水平位移自动监测系统及方法	2020-05-08	629
一种定向方法、设备及系统	2020-05-08	230
一种基于双CAN总线的无人机飞行控制系统	2020-05-08	163
角度感测装置	2020-05-08	120
一种导航定位方法及设备	2020-05-11	13
一种姿态控制笔形鼠标	2020-05-08	226
一种智能停车位监测装置	2020-05-08	784

一种导航定位方法及设备

一种导航定位方法及设备

技术领域

背景技术

发明内容

具体实施方式

该功能需要专业版企业版VIP权限，您可以：