一种姿态航向测量系统及其融合方法、存储介质与流程

本发明涉及农业机械导航领域，具体涉及一种基于加速度计、陀螺仪和三天线gnss测量的姿态航向测量系统及其融合方法、存储介质。

背景技术：

农业机械导航控制是一项推动现代精细农业进程的关键技术，目前在国内外已经广泛应用农业的耕、种、保、收全过程的推广应用。其原理主要是控制农机前轮的转向，保证农机导航路径和目标航线一致性。而在农机导航控制过程中，农机姿态精度高低直接关系农机路径跟踪偏差大小。因此，获取高精度姿态角是提高农业机械导航控制精度的关键。

农业机械姿态角度包括横滚角、俯仰角和航向角。目前测量姿态角度的方法主要由以下4种：

加速度计测量。加速度计可测量物体横滚角和俯仰角，其原理是基于重力场原理利用三轴加速度计和重力加速度的三角关系求得，该测量仅适合物体在静态情况下的测量。

陀螺仪测量。陀螺仪测量物体角度是通过对物体三个轴向的角速度的积分获得，该测量适合动态情况下的测量，但陀螺仪测量角度会出现积分误差累积。

gnss测量。gnss测量物体姿态角是采用三天线gnss位置坐标形成的基线向量求解出物体的横滚角，俯仰角和航向角。该精度不受静态和动态的外部加速度的影响，但精度受制于gnss信号质量，会出现跳变和丢失。

现有技术测量物体姿态角度大都为适应单一环境条件下的测量，难以满足农业机械导航高精度的姿态精度要求。三轴加速度计测量角度可测量物体横滚角、俯仰角。静态下可准确测量角度，并且具备自主性，不受外部条件的影响。但动态测量是由于受外部加速度的影响，其测量角度误差较大，难以满足动态精度要求。

陀螺仪测量角度可在动态情况下满足高精度的要求，但其角度是角速度积分得出，误差会出现累积，长时间测量误差较大。

三天线gnss测量物体姿态角可输出横滚角、俯仰角和航向角，其精度主要受限于gnss信号质量，当gnss信号失锁时，其姿态角便丢失，难以保证特殊环境下的姿态测量精度。

三轴磁力计只可以测量物体的航向角，其精度主要受限于环境干扰磁场的影响，使用不具备普遍性。

传统的三轴加速度计测量物体姿态角易受动态外部加速度计的影响，而三天线gnss接收机测量物体姿态角是以3个位置的gnss坐标构成的基线向量求解得到，其精度不会受限于动态外部加速度计的影响，但三天线gnss接收机测量姿态角易受gnss信号质量的影响，其姿态角度会出现跳变或丢失，而陀螺仪可保证在动态情况下对物体姿态的准确测量。因此本发明提出一种综合gnss、加速度计和陀螺仪多传感器姿态角度融合方法。

技术实现要素：

鉴于以上技术问题，本发明基于三天线gnss和惯性导航系统，提供了一种姿态航向测量系统及其融合方法、存储介质，解决了现有技术有只能适应单一环境条件下的测量，难以满足农业机械导航高精度的姿态精度要求的问题。

本发明采用以下技术方案：

一种姿态航向测量系统融合方法，包括以下步骤：

通过三天线gnss、加速度计加权获取车体真实姿态角ψ、γ、θ，其中，ψ为车体的航向角，γ为车体的横滚角，θ为车体的俯仰角；

获取陀螺仪的零位偏差δbx、δby、δbz；

对gnss、加速度计加权获取的车体姿态角ψ、γ、θ和陀螺仪零位偏差δbx、δby、δbz通过卡尔曼滤波模型进行融合处理，得到车体姿态角的最优估计值。

进一步的，通过卡尔曼滤波模型进行融合处理的方法具体包括：

以gnss、加速度计加权获取的车体姿态角ψ、γ、θ和陀螺仪零位偏差δbx、δby、δbz作为状态向量xk的六个状态分量，建立系统的卡尔曼滤波状态方程和测量方程；

所述卡尔曼滤波状态方程和测量方程为：

其中,为k时刻状态向量；为(k-1)时刻的状态向量；为k时刻陀螺仪输出角速度向量；为k时刻系统激励噪声序列；为k时刻系统观测向量；为k时刻系统观测噪声向量；

卡尔曼滤波预测方程组和校正方程组为：

且，式(1)中，为系统状态转移矩阵；为输入矩阵；为k时刻的预测结果；为(k-1)时刻的最优解；

式(2)中，为预测协方差；为k时刻的协方差最优解；a^t为a矩阵的转置矩阵；为系统过程协方差矩阵，其为非负定矩阵，qgx、qgy、qgz为陀螺仪测量误差的协方差；qbx、qby、qbz分别为gnss和加速度计估计陀螺仪零位偏差的协方差的加权值；

式(3)中，kk为k时刻卡尔曼增益,为量测矩阵；h^t是h的转置矩阵；为测量误差的协方差矩阵，其为正定矩阵，rx、ry、rz为gnss和加速度计的量测噪声加权值；

式(4)中，为更新k时刻的状态量，zk为k时刻系统观测向量；

式(5)中，为更新k时刻的协方差：为单位矩阵。

进一步的，通过三天线gnss、加速度计加权获取车体真实姿态角ψ、γ、θ时，所述车体真实姿态角ψ、γ、θ满足以下公式：

ψgnss＝ψ+wgnss

k1*γgnss+k2*γacc＝γ+m1*wgnss+m2*wacc

k3*θgnss+k4*θacc＝θ+m3*wgnss+m4*wacc

其中，

ψgnss为gnss计算得到的航向角(°)；

γgnss、γacc分别gnss、加速度计计算得到的横滚角(°)；

θgnss、θacc分别gnss、加速度计计算得到的俯仰角(°)；

wgnss、wacc为gnss、加速度计的测量高斯白噪声(db)；

k1、k2分别为gnss、加速度计观测横滚角度的加权值；

m1、m2分别为gnss、加速度计观测横滚角度白噪声的加权值；

k3、k4分别为gnss、加速度计观测俯仰角度的加权值；

m3、m4分别为gnss、加速度计观测俯仰角度白噪声的加权值。

进一步的，通过陀螺仪获取车体真实姿态角，所述车体真实姿态角包括车体航向角ψ、车体横滚角γ和车体俯仰角θ，满足以下公式：

其中，ψ为车体航向角(°)，γ为车体横滚角(°)，θ为车体俯仰角(°)；为车头真实航向角的导数(°/s)；为车头横滚角的导数(°/s)；为车体俯仰角的导数(°/s)；ωx、ωy、ωz分别为陀螺仪输出的角速度(°/s)；δbx、δby、δbz分别为陀螺仪的零位偏差(°/s)；wgx、wgy、wbz分别为陀螺仪的测量高斯白噪声(db)。

进一步的，所述陀螺仪零位偏差的导数满足以下公式：

其中，wbx、wby、wbz分别为三天线gnss和加速度计加权估计陀螺仪零位偏差的零均值高斯白噪声(db)。

进一步的，通过三天线gnss、加速度计加权获取车体真实姿态角ψ、γ、θ时，车体横滚角γ和车体俯仰角θ加权融合的权重大小根据gnss信号解状态进行固定权重调节，具体的，

当解状态为0时，代表gnss失锁，此时gnss权重为0，加速度计设计权重为1；

当解状态为1时，代表gnss单点定位，此时gnss权重为0.1，加速度计设计权重为0.9；

当解状态为2时，代表gnss码差分定位，此时gnss权重为0.2，加速度计设计权重为0.8；

当解状态为5时，代表gnss浮点解定位，此时gnss权重为0.3，加速度计设计权重为0.7；

当解状态为4时，代表gnss固定解定位，此时gnss权重为0.5，加速度计设计权重为0.5；

进一步的，设定卡尔曼滤波采样周期t为0.0005s，融合算法的矩阵参数满足以下：

状态转移矩阵

系统噪声驱动阵

系统过程协方差矩阵

系统量测噪声矩阵

预测协方差阵

状态向量

一种姿态航向测量系统，包括三天线gnss接收机和au7554姿态测量模块，其中所述的au7554姿态测量模块包括加速度计和陀螺仪，所述三天线gnss接收机和au7554姿态测量模块将测量车体姿态角的的数据通过串口服务器传送给处理器，以处理器对获取的数据进行分析计算得到车体姿态角的最优估计值。

进一步的，三天线gnss接收机包括两个安装在车体上的双天线接收机以及安装在车体上的gnss信号功分器，所述两个双天线接收机还分别与gnss基准站连接，以双天线接收机接收gnss基准站发送的差分信号；两个双天线接收机分别和横滚天线和俯仰天线连接，所述gnss信号功分器与定位天线连接，所述定位天线安装在车体的顶部，所述俯仰天线安装在车头方向处，所述横滚天线安装在车体一侧。

一种计算机存储介质，其上存储有计算机程序，包括姿态航向测量系统，所述计算机程序被处理器执行时实现上述的姿态航向测量系统融合方法。

相比现有技术，本发明的有益效果在于：

本发明的综合加速度计、陀螺仪和三天线gnss测量姿态角的互补特性，以陀螺仪为主惯导系统，将加速度计和三天线gnss接收机输出的角度对陀螺仪输出角度进行加权观测，基于卡尔曼滤波理论，提出一种基于三天线gnss和mems的姿态航向参考系统融合方法，测量精度高，可满足现有农业机械导航对姿态角精度要求高的要求，解决了现有技术中只能在单一环境条件下进行测量的问题。

附图说明

图1为本发明一种姿态航向测量系统的结构示意图；

图2为本发明一种姿态航向测量系统安装的平面结构图；

图3为本发明一种姿态航向测量系统参考坐标系的结构示意图；

图4为本发明一种姿态航向测量系统融合方法流程示意图；

图5为本发明一种姿态航向测量系统融合方法的卡尔曼滤波算法流程图。

具体实施方式

下面，结合附图以及具体实施方式，对本发明做进一步描述，需要说明的是，在不相冲突的前提下，以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例：

实施例：

请参考图1所示，一种姿态航向测量系统，包括三天线gnss接收机和au7554姿态测量模块，其中所述的au7554姿态测量模块包括加速度计和陀螺仪，所述三天线gnss接收机和au7554姿态测量模块将测量车体姿态角的的数据通过串口服务器传送给处理器，以处理器对获取的数据进行分析计算得到车体姿态角的最优估计值。

由于车体只有两个板卡，这样，不能直接与三天线gnss接收机连接，我们通过两个安装在车体上的双天线接收机以及gnss信号功分器组合来实现三天线gnss接收机，具体的，包括两个安装在车体上的双天线接收机以及安装在车体上的gnss信号功分器，所述两个双天线接收机还分别与gnss基准站连接以双天线接收机接收gnss基准站发送的差分信号；两个双天线接收机分别和横滚天线和俯仰天线连接，所述gnss信号功分器与定位天线连接，所述定位天线安装在车体的顶部，所述俯仰天线安装在车头方向处，所述横滚天线安装在车体一侧，三天线平面安装图可参考图2所示。

请参考图3所示，选东(e)、北(n)、天(u)为导航坐标系o-xyz的正方向，选右、前、上为车体坐标系o-xyz的正方向。横滚角、俯仰角和航向角均为绕各轴正向逆时针为正。三轴加速度计和陀螺仪分别按照上述车体坐标系进行安装，x轴为俯仰轴，y轴为横滚轴，z轴为航向轴。横滚天线安装在车体右侧。

一种姿态航向测量系统融合方法，可参考图4所示，包括以下步骤：

通过三天线gnss、加速度计加权获取车体真实姿态角ψ、γ、θ，其中，ψ为车体的航向角，γ为车体的横滚角，θ为车体的俯仰角；

获取陀螺仪的零位偏差δbx、δby、δbz；

对gnss、加速度计加权获取的车体姿态角ψ、γ、θ和陀螺仪零位偏差δbx、δby、δbz通过卡尔曼滤波模型进行融合处理，得到车体姿态角的最优估计值。

具体的，通过卡尔曼滤波模型进行融合处理的方法可参考图5所示，具体包括：

以gnss、加速度计加权获取的车体姿态角ψ、γ、θ和陀螺仪零位偏差δbx、δby、δbz作为状态向量xk的六个状态分量，建立系统的卡尔曼滤波状态方程和测量方程；

所述卡尔曼滤波状态方程和测量方程为：

其中,为k时刻状态向量；为(k-1)时刻的状态向量；为k时刻陀螺仪输出角速度向量；为k时刻系统激励噪声序列；为k时刻系统观测向量；为k时刻系统观测噪声向量；

卡尔曼滤波预测方程组和校正方程组为：

且，式(1)中，为系统状态转移矩阵；为输入矩阵；为k时刻的预测结果；为(k-1)时刻的最优解；

式(2)中，为预测协方差；为k时刻的协方差最优解；a^t为a矩阵的转置矩阵；为系统过程协方差矩阵，其为非负定矩阵，qgx、qgy、qgz为陀螺仪测量误差的协方差；qbx、qby、qbz分别为gnss和加速度计估计陀螺仪零位偏差的协方差的加权值；

式(3)中，kk为k时刻卡尔曼增益,为量测矩阵；h^t是h的转置矩阵；为测量误差的协方差矩阵，其为正定矩阵，rx、ry、rz为gnss和加速度计的量测噪声加权值；

式(4)中，为更新k时刻的状态量，zk为k时刻系统观测向量；

式(5)中，为更新k时刻的协方差：为单位矩阵。

进一步的，通过三天线gnss、加速度计加权获取车体真实姿态角ψ、γ、θ时，所述车体真实姿态角ψ、γ、θ满足以下公式：

ψgnss＝ψ+wgnss

k1*γgnss+k2*γacc＝γ+m1*wgnss+m2*wacc

k3*θgnss+k4*θacc＝θ+m3*wgnss+m4*wacc

其中，

ψgnss为gnss计算得到的航向角(°)；

γgnss、γacc分别gnss、加速度计计算得到的横滚角(°)；

θgnss、θacc分别gnss、加速度计计算得到的俯仰角(°)；

wgnss、wacc为gnss、加速度计的测量高斯白噪声(db)；

k1、k2分别为gnss、加速度计观测横滚角度的加权值；

m1、m2分别为gnss、加速度计观测横滚角度白噪声的加权值；

k3、k4分别为gnss、加速度计观测俯仰角度的加权值；

m3、m4分别为gnss、加速度计观测俯仰角度白噪声的加权值。

进一步的，通过陀螺仪获取车体真实姿态角，所述车体真实姿态角包括车体航向角ψ、车体横滚角γ和车体俯仰角θ，满足以下公式：

其中，ψ为车体航向角(°)，γ为车体横滚角(°)，θ为车体俯仰角(°)；为车头真实航向角的导数(°/s)；为车头横滚角的导数(°/s)；为车体俯仰角的导数(°/s)；ωx、ωy、ωz分别为陀螺仪输出的角速度(°/s)；δbx、δby、δbz分别为陀螺仪的零位偏差(°/s)；wgx、wgy、wgz分别为陀螺仪的测量高斯白噪声(db)。

进一步的，所述陀螺仪零位偏差的导数满足以下公式：

其中，wbx、wby、wbz分别为三天线gnss和加速度计加权估计陀螺仪零位偏差的零均值高斯白噪声(db)。

进一步的，通过三天线gnss、加速度计加权获取车体真实姿态角ψ、γ、θ时，车体横滚角γ和车体俯仰角θ加权融合的权重大小根据gnss信号解状态进行固定权重调节，具体的，

当解状态为0时，代表gnss失锁，此时gnss权重为0，加速度计设计权重为1；

当解状态为1时，代表gnss单点定位，此时gnss权重为0.1，加速度计设计权重为0.9；

当解状态为2时，代表gnss码差分定位，此时gnss权重为0.2，加速度计设计权重为0.8；

当解状态为5时，代表gnss浮点解定位，此时gnss权重为0.3，加速度计设计权重为0.7；

当解状态为4时，代表gnss固定解定位，此时gnss权重为0.5，加速度计设计权重为0.5；

进一步的，设定卡尔曼滤波采样周期t为0.0005s，融合算法的矩阵参数满足以下：

状态转移矩阵

系统噪声驱动阵

系统过程协方差矩阵

系统量测噪声矩阵

预测协方差阵

状态向量

本发明通过以陀螺仪为主惯导系统，将三天线接收机输出的姿态角和三轴加速度计输出的姿态角进行加权，利用卡尔曼滤波对陀螺仪输出的角度进行融合估计。

一种计算机存储介质，其上存储有计算机程序，包括姿态航向测量系统，所述计算机程序被处理器执行时实现如权利要求上述的姿态航向测量系统融合方法。

对本领域的技术人员来说，可根据以上描述的技术方案以及构思，做出其它各种相应的改变以及形变，而所有的这些改变以及形变都应该属于本发明权利要求的保护范围之内。