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个计算周期车辆运动时由于侧滑所引起的附加横向速度变化。
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个计算周期卫星接收机计算出来的定位信息本身含有的经度、纬度、东向速度和北向速度误差。
7.根据权利要求1所述的方法,其特征在于,所述第k个计算周期的系统状态方程为:
Xk=Ak,k-1Xk-1+Gk-1Wk-1
其中,Ak,k-1为第所述k个计算周期的状态转移矩阵,Xk为所述第k个计算周期的系统状态量,Gk-1为所述第k-1个计算周期的系统噪声矩阵,Wk-1为所述第k-1个计算周期的白噪声随机误差矢量,和分别为所述第k个计算周期得到的精度和纬度误差,εr为陀螺的一阶马尔科夫误差,εb随机常数误差,▽x,k和▽y,k分别为所述第k个计算周期的加速度计右向和前向一阶马尔科夫误差。
8.根据权利要求1所述的方法,其特征在于,所述根据所述第k个计算周期的系统状态方程,针对所述第k个计算周期的所述第一状态量组合和所述第k个计算周期的所述第二状态量组合进行闭环卡尔曼组合滤波得到所述第k个计算周期的第一误差修正,包括:
通过如下公式计算得到所述第k个计算周期的第一误差修正值
其中,为所述第k个计算周期的第一误差修正值,
9.根据权利要求3所述的方法,其特征在于,根据所述第k+1个计算周期的系统状态方程,针对所述第k+1个计算周期的所述第一状态量组合和所述第k+1个计算周期的所述第三状态量组合进行所述闭环卡尔曼组合滤波得到所述第k+1个计算周期的第二误差修正值,包括:
通过如下公式计算得到所述第k+1个计算周期的第二误差修正值
其中,为所述第k+1个计算周期的第二误差修正值,
10.根据权利要求1所述的方法,其特征在于,所述根据所述第k个计算周期的所述第一误差修正值修正所述第k个计算周期的初始航向角以得到所述第k个计算周期的目标航向角,包括:
通过如下公式计算得到所述第k个计算周期的目标航向角Φ′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个计算周期的目标经度。
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个计算周期车辆运动时由于侧滑所引起的附加横向速度变化。
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个计算周期卫星接收机计算出来的定位信息本身含有的经度、纬度、东向速度和北向速度误差。
19.根据权利要求13所述的设备,其特征在于,所述第k个计算周期的系统状态方程为:
Xk=Ak,k-1Xk-1+Gk-1Wk-1
其中,Ak,k-1为第所述k个计算周期的状态转移矩阵,Xk为所述第k个计算周期的系统状态量,Gk-1为所述第k-1个计算周期的系统噪声矩阵,Wk-1为所述第k-1个计算周期的白噪声随机误差矢量,和分别为所述第k个计算周期得到的精度和纬度误差,εr为陀螺的一阶马尔科夫误差,εb随机常数误差,▽x,k和▽y,k分别为所述第k个计算周期的加速度计右向和前向一阶马尔科夫误差。
20.根据权利要求13所述的设备,其特征在于,所述处理单元在用于根据所述第k个计算周期的系统状态方程,针对所述第k个计算周期的所述第一状态量组合和所述第k个计算周期的所述第二状态量组合进行闭环卡尔曼组合滤波得到所述第k个计算周期的第一误差修正方面,所述处理单元还用于:
通过如下公式计算得到所述第k个计算周期的第一误差修正值
其中,为所述第k个计算周期的第一误差修正值,
21.根据权利要求15所述的设备,其特征在于,所述处理单元在用于根据所述第k+1个计算周期的系统状态方程,针对所述第k+1个计算周期的所述第一状态量组合和所述第k+1个计算周期的所述第三状态量组合进行所述闭环卡尔曼组合滤波得到所述第k+1个计算周期的第二误差修正值方面,所述处理单元还用于:
通过如下公式计算得到所述第k+1个计算周期的第二误差修正值
其中,为所述第k+1个计算周期的第二误差修正值,
22.根据权利要求13所述的设备,其特征在于,所述处理单元在用于所述根据所述第k个计算周期的所述第一误差修正值修正所述第k个计算周期的初始航向角以得到所述第k个计算周期的目标航向角方面,所述处理单元还用于:
通过如下公式计算得到所述第k个计算周期的目标航向角Φ′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个计算周期的目标经度。
24.根据权利要求23所述的方法,其特征在于,所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值的修正值f′x,k和f′y,k分别由所述MEMS惯性传感器安装过程中存在的俯仰角偏差和横滚角偏差修正所述第k个计算周期MEMS惯性传感器中加速度计输出的右向和前向加速度值所得。
25.一种设备,其特征在于,包括:
处理器、存储器、通信接口和通信总线,所述处理器、所述存储器和所述通信接口通过所述通信总线连接并完成相互间的通信;
所述存储器存储有可执行程序代码,所述通信接口用于无线通信;
所述处理器用于调用所述存储器中的所述可执行程序代码,执行如权利要求1-12任一项所描述的方法。