[发明专利]一种基于加速度匹配的航向快速初始化方法、电子设备、存储介质

权利要求书

1.一种基于加速度匹配的航向快速初始化方法，其特征在于，包括以下步骤：

计算GNSS水平速度，通过GNSS位移和载体运动所用时间计算E、N方向上的速度；

计算GNSS水平加速度，通过所述E、N方向速度计算GNSS在E、N方向上的加速度；

基于航姿算法计算MEMS-IMU的俯仰角、横滚角和航向角；

计算MEMS-IMU水平加速度，通过航姿算法求解的水平姿态角计算MEMS-IMU水平加速度，所述水平姿态角包括俯仰角和横滚角；

计算GNSS和IMU的水平合加速度模值；

选取加速度相关性匹配区间，对GNSS水平合加速度的模值进行平滑处理，根据平滑后的GNSS水平合加速度模值，选取加速度相关性匹配区间；

计算GNSS水平加速度时间延迟，在匹配区间内通过GNSS与IMU的水平合加速度模值进行相关性匹配，求得GNSS水平加速度相对于IMU水平加速度的时间延迟；

匹配区间内计算航向角初值，计算GNSS水平合加速度方向与N方向夹角，计算IMU水平合加速度方向与IMU坐标系Y轴方向夹角，根据几何关系计算航向角；

匹配区间内关联航向变化量精化航向角，将不同时刻GNSS与IMU水平加速度匹配计算得到的航向角与航姿算法计算的航向变化量关联，通过取加权平均值的方式可以得到精度更高的航向角；

推算任意时刻航向角，根据匹配区间内某一时刻的精化航向角为起点，结合航姿算法推算的航向变化量计算一定时间内任意时刻的航向角。

2.如权利要求1所述的一种基于加速度匹配的航向快速初始化方法，其特征在于：所述计算GNSS水平速度步骤中，所述位移在E、N方向速度的计算公式为：

其中，为ΔL于A点处在E方向的位置分量，为ΔL于B点处在E方向的位置分量，为ΔL于A点处在N方向的位置分量,为ΔL于B点处在N方向的位置分量，分别为位移在E和N方向计算速度，ΔL为载体由A点移动至B点的位移，Δt为载体由A点移动至B点所用时间。

3.如权利要求2所述的一种基于加速度匹配的航向快速初始化方法，其特征在于：所述计算GNSS水平加速度步骤中，以速度为观测值滤波计算和随机系统状态空间模型为：

式中，X_k-1、X_k分别表示k-1时刻和k时刻下的状态向量，Z_k表示系统量测向量；F_k/k-1表示k-1时刻到k时刻的状态转移矩阵、H_k表示量测矩阵；W_k-1表示系统噪声向量，V_k表示量测噪声向量，两者都是零均值的高斯白噪声向量序列；

设置待估状态向量为：

系统状态转移矩阵和量测矩阵如下：

根据kalman滤波公式计算GNSS在E和N方向加速度值卡尔曼滤波公式如下：

P_k＝(I-K_kH_k)P_k/k-1

其中，P_k/k-1表示k时刻的先验估计协方差,K_k表示滤波增益，P_k示k时刻的后验估计协方差，Z_k表示量测值，R_k和Q_k-1分别表示量测噪声和系统噪声。