一种姿态航向测量系统及其融合方法、存储介质技术方案

本发明专利技术公开了一种姿态航向测量系统及其融合方法、存储介质。本发明专利技术融合方法包括以下步骤：通过三天线GNSS、加速度计加权获取车体真实姿态角Ψ、γ、θ，其中，Ψ为车体的航向角，γ为车体的横滚角，θ为车体的俯仰角；获取陀螺仪的零位偏差Δbx、Δby、Δbz；对GNSS、加速度计加权获取的车体姿态角Ψ、γ、θ和陀螺仪零位偏差Δbx、Δby、Δbz通过卡尔曼滤波模型进行融合处理，得到车体姿态角的最优估计值。本发明专利技术将加速度计和三天线GNSS接收机输出的角度对陀螺仪输出角度进行加权观测，基于卡尔曼滤波理论进行融合，测量精度高，可满足现有农业机械导航对姿态角精度要求高的问题。

An Attitude and Heading Measurement System and Its Fusion Method and Storage Media

The invention discloses an attitude and heading measurement system, a fusion method and a storage medium thereof. The fusion method of the invention includes the following steps: obtaining the real attitude angles_, gamma and theta of the vehicle body by weighting three antennas GNSS and accelerometers, in which_is the heading angle of the vehicle body, gamma is the roll angle of the vehicle body and theta is the pitch angle of the vehicle body; obtaining the zero deviation of the gyroscope bx, by, bz; and weighting the attitude angles_, gamma, theta of the vehicle body and the zero deviation of the gyroscope by weighting the GNSS and accelerometer. The difference bx, by and BZ are fused by Kalman filter model to obtain the optimal estimation of vehicle body attitude angle. The output angle of the accelerometer and the three-antenna GNSS receiver is weighted to observe the output angle of the gyroscope, and is fused based on Kalman filter theory. The measurement accuracy is high, which can meet the high requirement of the attitude angle precision of the existing agricultural machinery navigation.

【技术实现步骤摘要】
一种姿态航向测量系统及其融合方法、存储介质
本专利技术涉及农业机械导航领域，具体涉及一种基于加速度计、陀螺仪和三天线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时刻的协方差最优解；AT为A矩阵的转置矩阵；为系统过程协方差矩阵，其为非负定矩阵，Qgx、Qgy、Qgz为陀螺仪测量误差的协方差；Qbx、Qby、Qbz分别为GNSS和加速度计估计陀螺仪零位偏差的协方差的加权值；式(3)中，Kk为k时刻卡尔曼增益,为量测矩阵；HT是H的转置矩阵；为测量误差的协方差矩阵，其为正定矩阵，Rx、Ry、Rz为GNSS和加速度计的量测噪声加权值；式(4)中，为更新k时刻的状态量，Zk为k时刻系统观测向量；式(5)中，为更新k时刻的协方差：为单位矩阵。进一步的，通过三天线GNSS、加速度计加权获取车体真实姿态角Ψ、γ、θ时，所述车体真实姿态角Ψ、γ、θ满足以下公式：ψgnss＝Ψ+wgnssk1*γgnss+k2*γacc＝γ+m1*wgnss+m2*wacck3*θ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姿态测量模块包括加速度计和陀本文档来自技高网...

【技术保护点】
1.一种姿态航向测量系统融合方法，其特征在于，包括以下步骤：通过三天线GNSS、加速度计加权获取车体真实姿态角Ψ、γ、θ，其中，Ψ为车体的航向角，γ为车体的横滚角，θ为车体的俯仰角；获取陀螺仪的零位偏差Δbx、Δby、Δbz；对GNSS、加速度计加权获取的车体姿态角Ψ、γ、θ和陀螺仪零位偏差Δbx、Δby、Δbz通过卡尔曼滤波模型进行融合处理，得到车体姿态角的最优估计值。

【技术特征摘要】
1.一种姿态航向测量系统融合方法，其特征在于，包括以下步骤：通过三天线GNSS、加速度计加权获取车体真实姿态角Ψ、γ、θ，其中，Ψ为车体的航向角，γ为车体的横滚角，θ为车体的俯仰角；获取陀螺仪的零位偏差Δbx、Δby、Δbz；对GNSS、加速度计加权获取的车体姿态角Ψ、γ、θ和陀螺仪零位偏差Δbx、Δby、Δbz通过卡尔曼滤波模型进行融合处理，得到车体姿态角的最优估计值。2.根据权利要求1所述的姿态航向测量系统融合方法，其特征在于，通过卡尔曼滤波模型进行融合处理的方法具体包括：以GNSS、加速度计加权获取的车体姿态角Ψ、γ、θ和陀螺仪零位偏差Δbx、Δby、Δbz作为状态向量Xk的六个状态分量，建立系统的卡尔曼滤波状态方程和测量方程；所述卡尔曼滤波状态方程和测量方程为：其中,为k时刻状态向量；为(k-1)时刻的状态向量；为k时刻陀螺仪输出角速度向量；为k时刻系统激励噪声序列；为k时刻系统观测向量；为k时刻系统观测噪声向量；卡尔曼滤波预测方程组和校正方程组为：且，式(1)中，为系统状态转移矩阵；为输入矩阵；为k时刻的预测结果；为(k-1)时刻的最优解；式(2)中，为预测协方差；为k时刻的协方差最优解；AT为A矩阵的转置矩阵；为系统过程协方差矩阵，其为非负定矩阵，Qgx、Qgy、Qgz为陀螺仪测量误差的协方差；Qbx、Qby、Qbz分别为GNSS和加速度计估计陀螺仪零位偏差的协方差的加权值；式(3)中，Kk为k时刻卡尔曼增益,为量测矩阵；HT是H的转置矩阵；为测量误差的协方差矩阵，其为正定矩阵，Rx、Ry、Rz为GNSS和加速度计的量测噪声加权值；式(4)中，为更新k时刻的状态量，Zk为k时刻系统观测向量；式(5)中，为更新k时刻的协方差：为单位矩阵。3.根据权利要求2所述的姿态航向测量系统融合方法，其特征在于，通过三天线GNSS、加速度计加权获取车体真实姿态角Ψ、γ、θ时，所述车体真实姿态角Ψ、γ、θ满足以下公式：ψgnss＝Ψ+wgnssk1*γgnss+k2*γacc＝γ+m1*wgnss+m2*wacck3*θ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、加速度计观测俯仰角度白噪声的加权值。4.根据权利要求2所述的姿态航向测量系统融合方法，其特征在于，通过陀螺仪获取车体真实姿态角，所述...

【专利技术属性】
技术研发人员：杨伟伟，林国利，韩伟浩，潘久辉，吴贵龙，
申请(专利权)人：广州中海达定位技术有限公司，
类型：发明
国别省市：广东,44