CN109443349A - 一种姿态航向测量系统及其融合方法、存储介质 - Google Patents

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

Info

Publication number
CN109443349A
CN109443349A CN201811353218.XA CN201811353218A CN109443349A CN 109443349 A CN109443349 A CN 109443349A CN 201811353218 A CN201811353218 A CN 201811353218A CN 109443349 A CN109443349 A CN 109443349A
Authority
CN
China
Prior art keywords
gnss
angle
accelerometer
car body
gyroscope
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201811353218.XA
Other languages
English (en)
Inventor
杨伟伟
林国利
韩伟浩
潘久辉
吴贵龙
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
HI-TARGET POSITIONING Co Ltd
Original Assignee
HI-TARGET POSITIONING Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by HI-TARGET POSITIONING Co Ltd filed Critical HI-TARGET POSITIONING Co Ltd
Priority to CN201811353218.XA priority Critical patent/CN109443349A/zh
Publication of CN109443349A publication Critical patent/CN109443349A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

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

Description

一种姿态航向测量系统及其融合方法、存储介质
技术领域
本发明涉及农业机械导航领域，具体涉及一种基于加速度计、陀螺仪和三天线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＝Ψ+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时刻的协方差最优解；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＝Ψ+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，融合算法的矩阵参数满足以下：
状态转移矩阵
系统噪声驱动阵
系统过程协方差矩阵
系统量测噪声矩阵
预测协方差阵
状态向量
本发明通过以陀螺仪为主惯导系统，将三天线接收机输出的姿态角和三轴加速度计输出的姿态角进行加权，利用卡尔曼滤波对陀螺仪输出的角度进行融合估计。
一种计算机存储介质，其上存储有计算机程序，包括姿态航向测量系统，所述计算机程序被处理器执行时实现如权利要求上述的姿态航向测量系统融合方法。
对本领域的技术人员来说，可根据以上描述的技术方案以及构思，做出其它各种相应的改变以及形变，而所有的这些改变以及形变都应该属于本发明权利要求的保护范围之内。

Claims (10)

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＝Ψ+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、加速度计观测俯仰角度白噪声的加权值。
4.根据权利要求2所述的姿态航向测量系统融合方法，其特征在于，通过陀螺仪获取车体真实姿态角，所述车体真实姿态角包括车体航向角Ψ、车体横滚角γ和车体俯仰角θ，满足以下公式：
其中，Ψ为车体航向角(°)，γ为车体横滚角(°)，θ为车体俯仰角(°)；为车头真实航向角的导数(°/s)；为车头横滚角的导数(°/s)；为车体俯仰角的导数(°/s)；ωx、ωy、ωz分别为陀螺仪输出的角速度(°/s)；Δbx、Δby、Δbz分别为陀螺仪的零位偏差(°/s)；wgx、wgy、wgz分别为陀螺仪的测量高斯白噪声(dB)。
5.根据权利要求4所述的姿态航向测量系统融合方法，其特征在于，所述陀螺仪零位偏差的导数满足以下公式：
其中，wbx、wby、wbz分别为三天线GNSS和加速度计加权估计陀螺仪零位偏差的零均值高斯白噪声(dB)。
6.根据权利要求2所述的姿态航向测量系统融合方法，其特征在于，通过三天线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。
7.根据权利要求2所述的姿态航向测量系统融合方法，其特征在于，设定卡尔曼滤波采样周期T为0.0005s，融合算法的矩阵参数满足以下：
状态转移矩阵
系统噪声驱动阵
系统过程协方差矩阵
系统量测噪声矩阵
预测协方差阵状态向量
8.一种姿态航向测量系统，其特征在于，包括三天线GNSS接收机和AU7554姿态测量模块，其中所述的AU7554姿态测量模块包括加速度计和陀螺仪，所述三天线GNSS接收机和AU7554姿态测量模块将测量车体姿态角的的数据通过串口服务器传送给处理器，以处理器对获取的数据进行分析计算得到车体姿态角的最优估计值。
9.根据权利要求8所述的姿态航向测量系统，其特征在于，其中，三天线GNSS接收机包括两个安装在车体上的双天线接收机以及安装在车体上的GNSS信号功分器，所述两个双天线接收机还分别与GNSS基准站连接，以双天线接收机接收GNSS基准站发送的差分信号；两个双天线接收机分别和横滚天线和俯仰天线连接，所述GNSS信号功分器与定位天线连接，所述定位天线安装在车体的顶部，所述俯仰天线安装在车头方向处，所述横滚天线安装在车体一侧。
10.一种计算机存储介质，其上存储有计算机程序，其特征在于，包括姿态航向测量系统，所述计算机程序被处理器执行时实现如权利要求1-7任一项所述的姿态航向测量系统融合方法。
CN201811353218.XA 2018-11-14 2018-11-14 一种姿态航向测量系统及其融合方法、存储介质 Pending CN109443349A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811353218.XA CN109443349A (zh) 2018-11-14 2018-11-14 一种姿态航向测量系统及其融合方法、存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811353218.XA CN109443349A (zh) 2018-11-14 2018-11-14 一种姿态航向测量系统及其融合方法、存储介质

Publications (1)

Publication Number Publication Date
CN109443349A true CN109443349A (zh) 2019-03-08

Family

ID=65553534

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811353218.XA Pending CN109443349A (zh) 2018-11-14 2018-11-14 一种姿态航向测量系统及其融合方法、存储介质

Country Status (1)

Country Link
CN (1) CN109443349A (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110986937A (zh) * 2019-12-19 2020-04-10 北京三快在线科技有限公司 用于无人设备的导航装置、方法及无人设备
CN111007863A (zh) * 2019-12-06 2020-04-14 广州市申迪计算机系统有限公司 一种机器人航向角的测量方法、装置及存储介质
CN112230192A (zh) * 2020-12-21 2021-01-15 东方微电科技(武汉)有限公司 基于磁传感与定位系统的雷达航向角测量方法及装置
CN112698374A (zh) * 2020-12-17 2021-04-23 广州中海达卫星导航技术股份有限公司 基于bds-3/磁力计的多频双天线车载测姿方法和设备
CN112859121A (zh) * 2021-01-07 2021-05-28 华南农业大学 一种gnss动态测量作业机具高程的方法
CN113188505A (zh) * 2021-04-14 2021-07-30 湖南三一智能控制设备有限公司 姿态角度的测量方法、装置、车辆及智能臂架
CN113267800A (zh) * 2021-05-21 2021-08-17 中国联合网络通信集团有限公司 定位定姿方法、装置、设备、存储介质及其系统
CN113566850A (zh) * 2021-07-29 2021-10-29 深圳元戎启行科技有限公司 惯性测量单元的安装角度标定方法、装置和计算机设备
CN114167458A (zh) * 2021-12-07 2022-03-11 中国船舶重工集团公司第七0七研究所 一种gnss航迹角降噪计算方法
CN114659488A (zh) * 2022-02-08 2022-06-24 东风悦享科技有限公司 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法
CN114910045A (zh) * 2021-12-10 2022-08-16 浙江利尔达客思智能科技有限公司 一种基于六轴获取偏航角的方法

Citations (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101403620A (zh) * 2008-09-10 2009-04-08 深圳市同洲电子股份有限公司 导航装置及方法
CN101762805A (zh) * 2008-07-02 2010-06-30 凹凸电子（武汉）有限公司 组合导航系统以及导航方法
CN101950027A (zh) * 2010-08-18 2011-01-19 东莞市泰斗微电子科技有限公司 一种导航卫星信号接收模块及用于该模块的信息处理方法
CN103123487A (zh) * 2011-11-21 2013-05-29 上海航天控制工程研究所 一种航天器姿态确定方法
CN103185578A (zh) * 2011-12-30 2013-07-03 上海博泰悦臻电子设备制造有限公司 移动定位装置和导航装置
CN103675859A (zh) * 2012-09-10 2014-03-26 迈实电子(上海)有限公司 卫星导航接收机、设备和用于定位卫星导航接收机的方法
CN104345326A (zh) * 2013-07-24 2015-02-11 古野电气株式会社 状态计算装置、移动体、状态计算方法
CN104570033A (zh) * 2015-01-06 2015-04-29 中电科航空电子有限公司 一种飞机机载gps与惯性导航系统组合定位方法
CN105222772A (zh) * 2015-09-17 2016-01-06 泉州装备制造研究所 一种基于多源信息融合的高精度运动轨迹检测系统
CN105353772A (zh) * 2015-11-16 2016-02-24 中国航天时代电子公司 一种无人机机动目标定位跟踪中的视觉伺服控制方法
CN105823481A (zh) * 2015-12-21 2016-08-03 上海华测导航技术股份有限公司 一种基于单天线的gnss-ins车辆定姿方法
CN106153073A (zh) * 2016-06-21 2016-11-23 东南大学 一种全姿态捷联惯导系统的非线性初始对准方法
CN106767767A (zh) * 2016-11-23 2017-05-31 上海航天控制技术研究所 一种微纳多模星敏感器系统及其数据融合方法
CN107014376A (zh) * 2017-03-01 2017-08-04 华南农业大学 一种适用于农业机械精准作业的姿态倾角估计方法
CN107045137A (zh) * 2016-02-06 2017-08-15 苏州宝时得电动工具有限公司 自动工作系统，自移动设备及其控制方法
CN107110979A (zh) * 2014-12-26 2017-08-29 古野电气株式会社 姿态角计算装置、姿态角计算方法及姿态角计算程序
CN107607113A (zh) * 2017-08-02 2018-01-19 华南农业大学 一种两轴姿态倾角测量方法
CN107797131A (zh) * 2017-09-25 2018-03-13 华南理工大学 基于gps载波相位姿态的无人船数据融合姿态测量方法
CN207380245U (zh) * 2017-11-15 2018-05-18 国科天成（北京）科技有限公司 Gnss差分兼容机
CN108225324A (zh) * 2017-12-27 2018-06-29 中国矿业大学 一种基于智能终端的地磁匹配与pdr融合的室内定位方法
CN108426577A (zh) * 2018-05-02 2018-08-21 千寻位置网络有限公司 基于可穿戴设备的辅助骑行增强定位的方法和装置

Patent Citations (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101762805A (zh) * 2008-07-02 2010-06-30 凹凸电子（武汉）有限公司 组合导航系统以及导航方法
CN101403620A (zh) * 2008-09-10 2009-04-08 深圳市同洲电子股份有限公司 导航装置及方法
CN101950027A (zh) * 2010-08-18 2011-01-19 东莞市泰斗微电子科技有限公司 一种导航卫星信号接收模块及用于该模块的信息处理方法
CN103123487A (zh) * 2011-11-21 2013-05-29 上海航天控制工程研究所 一种航天器姿态确定方法
CN103185578A (zh) * 2011-12-30 2013-07-03 上海博泰悦臻电子设备制造有限公司 移动定位装置和导航装置
CN103675859A (zh) * 2012-09-10 2014-03-26 迈实电子(上海)有限公司 卫星导航接收机、设备和用于定位卫星导航接收机的方法
CN104345326A (zh) * 2013-07-24 2015-02-11 古野电气株式会社 状态计算装置、移动体、状态计算方法
CN107110979A (zh) * 2014-12-26 2017-08-29 古野电气株式会社 姿态角计算装置、姿态角计算方法及姿态角计算程序
CN104570033A (zh) * 2015-01-06 2015-04-29 中电科航空电子有限公司 一种飞机机载gps与惯性导航系统组合定位方法
CN105222772A (zh) * 2015-09-17 2016-01-06 泉州装备制造研究所 一种基于多源信息融合的高精度运动轨迹检测系统
CN105353772A (zh) * 2015-11-16 2016-02-24 中国航天时代电子公司 一种无人机机动目标定位跟踪中的视觉伺服控制方法
CN105823481A (zh) * 2015-12-21 2016-08-03 上海华测导航技术股份有限公司 一种基于单天线的gnss-ins车辆定姿方法
CN107045137A (zh) * 2016-02-06 2017-08-15 苏州宝时得电动工具有限公司 自动工作系统，自移动设备及其控制方法
CN106153073A (zh) * 2016-06-21 2016-11-23 东南大学 一种全姿态捷联惯导系统的非线性初始对准方法
CN106767767A (zh) * 2016-11-23 2017-05-31 上海航天控制技术研究所 一种微纳多模星敏感器系统及其数据融合方法
CN107014376A (zh) * 2017-03-01 2017-08-04 华南农业大学 一种适用于农业机械精准作业的姿态倾角估计方法
CN107607113A (zh) * 2017-08-02 2018-01-19 华南农业大学 一种两轴姿态倾角测量方法
CN107797131A (zh) * 2017-09-25 2018-03-13 华南理工大学 基于gps载波相位姿态的无人船数据融合姿态测量方法
CN207380245U (zh) * 2017-11-15 2018-05-18 国科天成（北京）科技有限公司 Gnss差分兼容机
CN108225324A (zh) * 2017-12-27 2018-06-29 中国矿业大学 一种基于智能终端的地磁匹配与pdr融合的室内定位方法
CN108426577A (zh) * 2018-05-02 2018-08-21 千寻位置网络有限公司 基于可穿戴设备的辅助骑行增强定位的方法和装置

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111007863B (zh) * 2019-12-06 2023-05-02 广州市申迪计算机系统有限公司 一种机器人航向角的测量方法、装置及存储介质
CN111007863A (zh) * 2019-12-06 2020-04-14 广州市申迪计算机系统有限公司 一种机器人航向角的测量方法、装置及存储介质
CN110986937A (zh) * 2019-12-19 2020-04-10 北京三快在线科技有限公司 用于无人设备的导航装置、方法及无人设备
CN112698374A (zh) * 2020-12-17 2021-04-23 广州中海达卫星导航技术股份有限公司 基于bds-3/磁力计的多频双天线车载测姿方法和设备
CN112230192A (zh) * 2020-12-21 2021-01-15 东方微电科技(武汉)有限公司 基于磁传感与定位系统的雷达航向角测量方法及装置
CN112859121A (zh) * 2021-01-07 2021-05-28 华南农业大学 一种gnss动态测量作业机具高程的方法
CN113188505A (zh) * 2021-04-14 2021-07-30 湖南三一智能控制设备有限公司 姿态角度的测量方法、装置、车辆及智能臂架
CN113267800A (zh) * 2021-05-21 2021-08-17 中国联合网络通信集团有限公司 定位定姿方法、装置、设备、存储介质及其系统
CN113566850A (zh) * 2021-07-29 2021-10-29 深圳元戎启行科技有限公司 惯性测量单元的安装角度标定方法、装置和计算机设备
CN113566850B (zh) * 2021-07-29 2024-03-08 深圳元戎启行科技有限公司 惯性测量单元的安装角度标定方法、装置和计算机设备
CN114167458A (zh) * 2021-12-07 2022-03-11 中国船舶重工集团公司第七0七研究所 一种gnss航迹角降噪计算方法
CN114910045A (zh) * 2021-12-10 2022-08-16 浙江利尔达客思智能科技有限公司 一种基于六轴获取偏航角的方法
CN114910045B (zh) * 2021-12-10 2024-04-19 浙江利尔达客思智能科技有限公司 一种基于六轴获取偏航角的方法
CN114659488A (zh) * 2022-02-08 2022-06-24 东风悦享科技有限公司 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法
CN114659488B (zh) * 2022-02-08 2023-06-23 东风悦享科技有限公司 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法

Similar Documents

Publication Publication Date Title
CN109443349A (zh) 一种姿态航向测量系统及其融合方法、存储介质
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN105606094B (zh) 一种基于mems/gps组合系统的信息条件匹配滤波估计方法
CN106767786A (zh) 基于卫星通信的组合惯导姿态计算方法
CN104698485B (zh) 基于bd、gps及mems的组合导航系统及导航方法
CN106647791A (zh) 三维姿态测控装置、机械设备及三维姿态的测控方法
CN105509740A (zh) 一种农业机械车辆姿态测量方法及测量模块
CN104880189B (zh) 一种动中通天线低成本跟踪抗干扰方法
CN110057356B (zh) 一种隧道内车辆定位方法及装置
CN106767925B (zh) 带双轴转位机构的惯导系统三位置参数辨识对准方法
CN111272158B (zh) 复杂磁扰动场景mems电子罗盘的动态方位角解算方法
CN105910606A (zh) 一种基于角速度差值的方向修正方法
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
CN110440830A (zh) 动基座下车载捷联惯导系统自对准方法
CN112833917B (zh) 基于磁航向角和最小二乘法的三轴磁传感器标定方法
CN110530397A (zh) 一种基于九轴imu的磁场自动标定方法
KR101211703B1 (ko) 시선벡터를 이용한 자장계 오차 보정방법 및 이를 이용한 통합 항법 시스템
CN103712598A (zh) 一种小型无人机姿态确定系统与确定方法
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
Gao et al. An integrated land vehicle navigation system based on context awareness
CN110779514B (zh) 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置
CN116147624A (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN111141285A (zh) 一种航空重力测量装置
CN112683265B (zh) 一种基于快速iss集员滤波的mimu/gps组合导航方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication

Application publication date: 20190308

RJ01 Rejection of invention patent application after publication