一种基于加速度匹配的航向快速初始化方法、电子设备、存储介质技术

本发明专利技术提供一种基于加速度匹配的航向快速初始化方法，包括步骤：计算GNSS水平速度，计算GNSS水平加速度，航姿算法计算IMU俯仰角、横滚角和航向角、计算MEMS

【技术实现步骤摘要】
一种基于加速度匹配的航向快速初始化方法、电子设备、存储介质


[0001]本专利技术涉及GNSS/MEMS
‑
INS组合定位
，尤其涉及一种基于加速度匹配的航向快速初始化方法、电子设备、存储介质。

技术介绍

[0002]GNSS/MEMS
‑
INS组合定位技术可确定RTK接收机的姿态和位置，高精度位置信息由RTK定位技术获取，而姿态信息需要GNSS/MEMS
‑
INS组合解算得到。一旦确定高精度的姿态信息，就可以进行高精度的RTK倾斜测量作业。GNSS/MEMS
‑
INS组合解算需要完成初始对准工作，即完成对载体姿态三个欧拉角的确定，包括航向角ψ、俯仰角θ和横滚角γ。
[0003]初始对准工作包括水平姿态对准和航向对准两部分，水平姿态对准确定俯仰角θ和横滚角γ，航向对准确定航向角ψ。水平姿态对准比较简单，可由加速度计在载体处于静止状态下基于比力方程推导计算得到。低速状态下也可由加速度计和陀螺仪基于航姿参考系统AHRS(Attitude Heading Reference System)算法滤波实现。航向对准比较复杂，MEMS惯导系统在导航初始化时，由于陀螺精度太低，不能完成方位自主对准，而只能采用地磁测量来实现，或在运动条件下依靠卫星导航测量信息完成对准。
[0004]传统的MEMS
‑
INS航向对准方法，主要方法有以下几种：(1)基于地磁测量实现航向对准；(2)基于水平位置匹配实现航向对准。当载体行进方向与载体纵轴方向保持一致时，利用GNSS定位技术推算出载体行进方向的位置变化矢量，即可确定载体航向角；(3)基于水平速度匹配实现航向对准。当载体行进方向与载体纵轴方向保持一致时，利用GNSS测速技术估计载体在载体导航系(n系)下的速度矢量即可确定载体航向角。(4)基于GNSS速度计算加速度匹配实现航向对准。利用GNSS速度计算加速度，与IMU加速度匹配计算载体航向角，该方法主要应用于无人机等运动复杂度较低场景。
[0005]上述方法存在以下问题：(1)基于地磁测量实现航向对准原理比较简单，实现也不复杂，但操作较为麻烦，需要在作业前进行地磁参数校准(例如椭球校准)，一般情况下地球磁场强度较小，地磁测量设备容易受到环境磁场干扰。在实际作业过程中环境磁场复杂多变，不同作业区域的磁场校准参数会有差异，基于地磁测量实现航向对准存在精度不稳定的问题。
[0006](2)基于水平位置匹配实现航向对准，该方法要求载体行进方向与载体纵轴方向保持一致，限制了RTK作业方式，用户体验不好。常规的RTK作业，GNSS接收机一般安置在手持的对中杆上。
[0007](3)基于水平速度匹配实现航向对准，该方法要求载体行进方向与载体纵轴方向保持一致，但在实际RTK作业中，载体前进方向与载体纵轴之间角度随时可能发生变化，利用GNSS测速技术计算的速度矢量得到航向角并非真实的载体航向角。
[0008](4)基于GNSS速度计算加速度实现航向对准，该方法利用GNSS速度计算的加速度和IMU加速度进行匹配得到载体航向角，不需要前进方向与载体方向一致。但GNSS速度输出
本身含有延迟，并且利用GNSS速度计算加速度含有噪声。在RTK应用场景中，由于手持设备运动方式比较复杂，并且加速度区间较短，而且加速度较小，该方法计算精度较低。
[0009]因此，想要在RTK应用中快速高精度的实现GNSS/MEMS
‑
INS组合航向对准，上述方法难以满足。

技术实现思路

[0010]为了克服现有技术的不足，本专利技术的目的在于提供一种基于加速度匹配的航向快速初始化方法，适用于RTK倾斜测量的航向粗对准，不受外部磁场环境干扰，不受限于载体行进方向与惯性模块轴向一致条件，适合手持RTK的作业方式。基于该方法完成航向粗对准，输出的航向可以作为后续GNSS/INS组合滤波的初值，完成整个滤波器的精对准，可靠性和可用性较上述方法得到了显著提高。
[0011]本专利技术提供一种基于加速度匹配的航向快速初始化方法，包括以下步骤：
[0012]计算GNSS水平速度，通过GNSS位移和载体运动所用时间计算E、N方向上的速度；
[0013]计算GNSS水平加速度，通过所述E、N方向速度计算GNSS在E、N方向上的加速度；
[0014]基于航姿算法计算MEMS
‑
IMU的俯仰角、横滚角和航向角；
[0015]计算MEMS
‑
IMU水平加速度，通过航姿算法求解的水平姿态角计算MEMS
‑
IMU水平加速度，所述水平姿态角包括俯仰角和横滚角；
[0016]计算GNSS和IMU的水平合加速度模值；
[0017]选取加速度相关性匹配区间，对GNSS水平合加速度的模值进行平滑处理，根据平滑后的GNSS水平合加速度模值，选取加速度相关性匹配区间；
[0018]计算GNSS水平加速度时间延迟，在匹配区间内通过GNSS与IMU的水平合加速度模值进行相关性匹配，求得GNSS水平加速度相对于IMU水平加速度的时间延迟；
[0019]匹配区间内计算航向角初值，计算GNSS水平合加速度方向与N方向夹角，计算IMU水平合加速度方向与IMU坐标系Y轴方向夹角，根据几何关系计算航向角；
[0020]匹配区间内关联航向变化量精化航向角，将不同时刻GNSS与IMU水平加速度匹配计算得到的航向角与航姿算法计算的航向变化量关联，通过取加权平均值的方式可以得到精度更高的航向角；
[0021]推算任意时刻航向角，根据匹配区间内某一时刻的精化航向角为起点，结合航姿算法推算的航向变化量计算一定时间内任意时刻的航向角。
[0022]进一步地，所述计算GNSS水平速度步骤中，所述位移在E、N方向速度的计算公式为：
[0023][0038]其中，P
k/k
‑1表示k时刻的先验估计协方差,K
k
表示滤波增益，P
k
示k时刻的后验估计协方差，Z
k
表示量测值，R
k
和Q
k
‑1分别表示量测噪声和系统噪声。
[0039]进一步地，所述基于航姿算法计算MEMS
‑
IMU的俯仰角、横滚角和航向角的步骤中，采用卡尔曼滤波方法对状态进行估计，选取四元数误差矢量和陀螺仪零偏误差矢量作为状态向量，由四元数可以计算欧拉角，包括俯仰角、横滚角和航向角；
[0040]状态向量X(t)＝[q
e Δε]T
，其中，q
e
表示四元数误差矢量；Δε表示陀螺仪零偏误差矢量；
[0041]系统状态方程为：
[0042][0043]其中，表示导航坐标系相对载体坐标系的角速度矢量；I
3x3
表示3
×
3的单位矩阵；W1(t)、W2(t)表示系统噪声；
[本文档来自技高网...

【技术保护点】

【技术特征摘要】
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方向速度的计算公式为：S水平速度步骤中，所述位移在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水平加速度步骤中，以速度为观测值滤波计算和随机系统状态空
(t)、W2(t)表示系统噪声；选取加速度计输出f
b
与重力加速度向量在载体坐标系中投影的差值作为观测向量，系统观测方程为：其中，Z(t)表示系统量测向量；V(t)表示量测噪声向量。5.如权利要求4所述的一种基于加速度匹配的航向快速初始化方法，其特征在于：所述计算MEMS
‑
IMU水平加速度的步骤中，由四元数计算欧拉角，通过欧拉角计算载体坐标系到水平坐标系的旋转矩阵将IMU输出的加速度数据进行归平处理，公式如下：6.如权利要求5所述的一种基于加速度匹配的航向快速初始化方法，其特征在于：所述计算GNSS和IMU的水平合加速度模值步骤中，由GNSS水平加速度计算合加速度的模值由IMU水平加速度计算合加速度的模值计算合加速度的模值计算合加速度的模值7.如权利要求6所述的一种基于加速度匹配的航向快速初始化方法，其特征在于：所述选取加速度相关性匹配时间区间步骤中，对GNSS加速度模值数据进行平滑处理，基于平滑后的GNSS水平合加速度模值，选取加速度相关性匹配时间区间；GNSS平滑窗口长度为SW(G)，平滑处理计算公式为：其中，smooth方法原理表示如下：m表示平滑窗口长度，k表示第k个平滑采样点，设定GNSS水平合加速度阈值为threshold_a，当GNSS水平合加速度时，进行加计模值匹配区间数据样本采集，采集历元数为N；设定匹配区间样本合格比率阈值为ratio_sf，当N个采样点中满足的样本占比大于ratio_sf，加速度匹配区间构造成功。8.如权利要求7所述的一种基于加速度匹配的航向快速初始化方法，其特征在于：所述计算GNSS水平加速度时间延迟步骤中，在加速度匹配区间内，...

【专利技术属性】
技术研发人员：厉宽宽，陈源军，刘猛奎，
申请(专利权)人：广州市中海达测绘仪器有限公司，
类型：发明
国别省市：