WO2017063387A1 - 基于九轴mems传感器的农业机械全姿态角更新方法 - Google Patents

基于九轴mems传感器的农业机械全姿态角更新方法 Download PDF

Info

Publication number
WO2017063387A1
WO2017063387A1 PCT/CN2016/088316 CN2016088316W WO2017063387A1 WO 2017063387 A1 WO2017063387 A1 WO 2017063387A1 CN 2016088316 W CN2016088316 W CN 2016088316W WO 2017063387 A1 WO2017063387 A1 WO 2017063387A1
Authority
WO
WIPO (PCT)
Prior art keywords
angle
vehicle body
model
gyroscope
data
Prior art date
Application number
PCT/CN2016/088316
Other languages
English (en)
French (fr)
Inventor
任强
王杰俊
戴文鼎
曹广节
董光阳
涂睿
Original Assignee
上海华测导航技术股份有限公司
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 上海华测导航技术股份有限公司 filed Critical 上海华测导航技术股份有限公司
Priority to EP16854770.1A priority Critical patent/EP3364153B1/en
Priority to RU2017125035A priority patent/RU2662460C1/ru
Priority to US15/538,201 priority patent/US20170350721A1/en
Priority to KR1020177023677A priority patent/KR102017404B1/ko
Publication of WO2017063387A1 publication Critical patent/WO2017063387A1/zh

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
    • 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
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • A01B69/007—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow
    • A01B69/008—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow automatic
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B79/00—Methods for working soil
    • A01B79/005—Precision agriculture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B81—MICROSTRUCTURAL TECHNOLOGY
    • B81B—MICROSTRUCTURAL DEVICES OR SYSTEMS, e.g. MICROMECHANICAL DEVICES
    • B81B7/00—Microstructural systems; Auxiliary parts of microstructural devices or systems
    • B81B7/02—Microstructural systems; Auxiliary parts of microstructural devices or systems containing distinct electrical or optical devices of particular relevance for their function, e.g. microelectro-mechanical systems [MEMS]
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C19/00—Gyroscopes; Turn-sensitive devices using vibrating masses; Turn-sensitive devices without moving masses; Measuring angular rate using gyroscopic effects
    • 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • 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
    • 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
    • G01C21/1654—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 with electromagnetic compass
    • 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/18—Stabilised platforms, e.g. by gyroscope
    • 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
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Definitions

  • the invention relates to the technical field of measurement, in particular to a method for updating a full attitude angle of an agricultural machine based on a nine-axis MEMS sensor.
  • the inertial navigation system is divided into PINS (Platform Inertial Navigation System) and SINS (Strapdown Inertial Navigation System).
  • PINS Plate Inertial Navigation System
  • SINS trapdown Inertial Navigation System
  • IMU Inertial Measuring Unit
  • SINS is mostly used in aircraft navigation control systems, and research and application in the field of agricultural machinery control are in the initial stage, and the application objects and environmental conditions of the two are quite different.
  • the implementation of strapdown inertial navigation in the aircraft control system is realized. The method is not applicable in the control of agricultural machinery.
  • the present invention provides a method for updating the full attitude angle of agricultural machinery based on a nine-axis MEMS sensor with small error, high precision, stability and reliability.
  • An agricultural machinery full attitude angle updating method based on a nine-axis MEMS sensor comprises the following steps:
  • the angle, speed, position information and heading angle of the vehicle body are calculated through the established gyro error model and the electronic compass calibration ellipse model;
  • the data fusion processing is performed on the angle, speed, position information and heading angle of the vehicle body, and the motion attitude angle of the vehicle body is updated in real time;
  • the nine-axis MEMS sensor is composed of a three-axis gyroscope, a three-axis accelerometer and a three-axis geomagnetic sensor.
  • the step of establishing a gyroscope error model, an electronic compass calibration ellipse model and a seven-dimensional EKF filter model, and setting a parameter vector of the corresponding vehicle body motion posture is specifically:
  • the gyroscope error model calculates the angular velocity of the gyroscope through the gyroscope error calculation formula.
  • ⁇ ib is the gyroscope real
  • b ⁇ r is the gyro zero drift
  • b ⁇ g is the gyroscope output white noise
  • Eliminate magnetic field interference by calibrating the elliptical model with an electronic compass; where the electronic compass calibrates the elliptical model: Mx, my is the magnetic field strength, Xoffset and Yoffset are hard magnetic interference, and Xsf and Ysf are soft magnetic interference;
  • the vehicle body attitude is updated by the seven-dimensional EKF filter model.
  • the seven-dimensional EKF filter model is an extended Kalman filter of the seven-dimensional state vector.
  • the EKF includes the state equation and the observation equation:
  • the step of acquiring acceleration, angular velocity and earth magnetic field strength data of the vehicle body motion in real time by the nine-axis MEMS sensor is specifically:
  • the geomagnetic field strength data of the vehicle body is collected by a geomagnetic sensor.
  • the angle, the speed, the position information, and the heading of the vehicle body are calculated according to the acquired acceleration, angular velocity and earth magnetic field strength data of the vehicle body through the established gyroscope error model and the electronic compass calibration ellipse model.
  • the angle step is specifically as follows:
  • the angle data is obtained by integrating the angular velocity by the gyro error model
  • the geomagnetic field strength data is calculated by the ellipse model and the vehicle body heading angle is calculated after the calibration parameter compensation and the inclination correction.
  • the data fusion processing is performed on the angle, the speed, the position information, and the heading angle of the vehicle body by the seven-dimensional EKF filtering model, and the real-time updating step of the motion posture angle of the vehicle body is specifically as follows:
  • the seven-dimensional EKF filter model calculates the vehicle body attitude data through the quaternion attitude update algorithm.
  • the EKF algorithm calculation process :
  • k is the sampling time
  • (-) is the previous moment
  • (+) is the latter moment
  • ⁇ k is the state transition matrix
  • Pk is the minimum mean square error matrix
  • Q is the covariance matrix corresponding to the state vector
  • Kk is the error Gain
  • yk is the observation vector
  • Hk is the observation matrix transfer matrix
  • Rk is the covariance matrix corresponding to the observation vector.
  • Q is a quaternion vector
  • q0, q1, q2, q3 are scalars that make up the quaternion vector
  • i, j, and k are unit vectors of the three-dimensional coordinate system
  • the updated pose matrix is:
  • ⁇ , ⁇ , and ⁇ are the roll angle, the pitch angle, and the heading angle, respectively.
  • the data fusion processing is performed on the angle, the speed, the position information, and the heading angle of the vehicle body by the seven-dimensional EKF filtering model, and the following steps are performed after the real-time updating step of the motion posture angle of the vehicle body is performed: Extracting the full attitude angle data of the vehicle body from the vehicle body posture update data, and determining the attitude angle data value, the full attitude angle of the vehicle body includes a pitch angle, a roll angle and a heading angle, wherein
  • the invention has the advantages that the acceleration and the angular velocity of the motion of the object are obtained by the MEMS sensor in real time, and the angle acceleration integral obtained by the gyroscope can obtain the angle, and the speed and the integral can be calculated by integrating the acceleration to calculate the position information.
  • the geomagnetic sensor acquires the earth's magnetic field, calculates the heading angle through the compensation algorithm and fusion with the gyroscope, and then converts the attitude into a transformation matrix, thereby realizing the conversion of the carrier coordinate system and the navigation coordinate system.
  • the transformation matrix plays a "mathematical platform".
  • the transformation matrix is particularly important, because the agricultural machinery is constantly moving, its posture is constantly changing, that is, the transformation matrix must be constantly recalculated and Update.
  • Commonly used pose update algorithms have Euler angles, directional cosines and quaternions. The quaternion has no singularity compared with the Euler angle algorithm. Compared with the direction cosine, the calculation is small, which is very suitable for use in embedded products.
  • the gyroscope error model of geomagnetic field and gyroscope error model is established in the plane of agricultural machinery, and the 7D EKF (Extended Kalman Filter) update pose matrix is established.
  • the quaternion and gyroscope zero offset are estimated, and then the acceleration and magnetic field strength are calculated.
  • the heading angle is observed, so that a higher precision three-dimensional attitude angle can be obtained.
  • the error compensation and correction algorithm is adopted, which greatly reduces the error interference of the SINS algorithm.
  • the MEMS sensor and the SINS algorithm make the invention have higher performance parameters.
  • the tractor's test output heading angle error is less than 0.1°, and the pitch and roll angle errors are less than 0.01°. Using the quaternion as the Kalman filter state vector can further improve the calculation accuracy of the target parameters.
  • Embodiment 1 is a flow chart of a method of Embodiment 1 of a method for updating a full attitude angle of an agricultural machine based on a nine-axis MEMS sensor according to the present invention
  • Embodiment 2 is a flow chart of a method of Embodiment 2 of a method for updating a full attitude angle of an agricultural machine based on a nine-axis MEMS sensor according to the present invention.
  • a nine-axis MEMS sensor based agricultural machinery full attitude angle updating method comprises the following steps:
  • Step S1 establishing a gyroscope error model, an electronic compass calibration ellipse model, and a seven-dimensional EKF filter model, and setting a parameter vector of the corresponding vehicle body motion posture;
  • Step S1 establishing a gyroscope error model, an electronic compass calibration ellipse model, and a seven-dimensional EKF filter model, and setting a parameter vector step of the corresponding vehicle body motion posture is specifically:
  • the gyroscope error model calculates the angular velocity of the gyroscope through the gyroscope error calculation formula.
  • ⁇ ib is the gyroscope real
  • b ⁇ r is the gyro zero drift
  • b ⁇ g is the gyroscope output white noise
  • Eliminate magnetic field interference by calibrating the elliptical model with an electronic compass; where the electronic compass calibrates the elliptical model: Mx, my is the magnetic field strength, Xoffset and Yoffset are hard magnetic interference, and Xsf and Ysf are soft magnetic interference;
  • the vehicle body attitude is updated by the seven-dimensional EKF filter model.
  • the seven-dimensional EKF filter model is an extended Kalman filter of the seven-dimensional state vector.
  • the EKF includes the state equation and the observation equation:
  • the electronic compass calibration ellipse model is used to eliminate the interference caused by the magnetic field.
  • the actual calibration process is through the least squares method. The acquired magnetic field strength is fitted and then the above parameters are obtained.
  • Step S2 acquiring acceleration, angular velocity and earth magnetic field strength data of the vehicle body motion in real time through a nine-axis MEMS sensor;
  • the step S2 the step of acquiring the acceleration, the angular velocity and the earth magnetic field strength data of the vehicle body motion in real time through the nine-axis MEMS sensor is specifically as follows:
  • the geomagnetic field strength data of the vehicle body is collected by a geomagnetic sensor.
  • Step S3 calculating an angle, a speed, a position information, and a heading angle of the vehicle body according to the acquired acceleration, angular velocity and earth magnetic field strength data of the vehicle body through the established gyro error model and the electronic compass calibration ellipse model;
  • Step S3 calculating the angle, speed, position information, and heading angle of the vehicle body according to the acquired acceleration, angular velocity, and earth magnetic field strength data of the vehicle body through the established gyro error model and the electronic compass calibration ellipse model.
  • the angle data is obtained by integrating the angular velocity by the gyro error model
  • the geomagnetic field strength data is calculated by the ellipse model and the vehicle body heading angle is calculated after the calibration parameter compensation and the inclination correction.
  • the MEMS sensor collects the motion information of the vehicle body in real time.
  • the angular velocity of the vehicle body collected by the gyroscope is corrected by the state estimation gyro zero offset, and the angle increment is calculated for the integral.
  • the geomagnetic sensor is compensated by soft magnetic, hard magnetic and tilt angle correction. Then calculate the heading angle.
  • Step S4 performing data fusion processing on the angle, speed, position information, and heading angle of the vehicle body through the seven-dimensional EKF filtering model, and real-time updating the motion attitude angle of the vehicle body;
  • Step S4 performing data fusion processing on the angle, speed, position information, and heading angle of the vehicle body through the seven-dimensional EKF filtering model, and real-time updating the moving attitude angle of the vehicle body is specifically as follows:
  • the seven-dimensional EKF filter model calculates the vehicle body attitude data through the quaternion attitude update algorithm.
  • the EKF algorithm calculation process :
  • k is the sampling time
  • (-) is the previous moment
  • (+) is the latter moment
  • ⁇ k is the state transition matrix
  • Pk is the minimum mean square error matrix
  • Q is the covariance matrix corresponding to the state vector
  • Kk is the error Gain
  • yk is the observation vector
  • Hk is the observation matrix transfer matrix
  • Rk is the covariance matrix corresponding to the observation vector.
  • Q is a quaternion vector
  • q0, q1, q2, q3 are scalars that make up the quaternion vector
  • i, j, and k are unit vectors of the three-dimensional coordinate system
  • the updated pose matrix is:
  • ⁇ , ⁇ , and ⁇ are the roll angle, the pitch angle, and the heading angle, respectively.
  • the nine-axis MEMS sensor is composed of a three-axis gyroscope, a three-axis accelerometer and a three-axis geomagnetic sensor.
  • a nine-axis MEMS sensor based agricultural machinery full attitude angle updating method comprises the following steps:
  • Step S1 establishing a gyroscope error model, an electronic compass calibration ellipse model, and a seven-dimensional EKF filter model, and setting a parameter vector of the corresponding vehicle body motion posture;
  • Step S1 establishing a gyroscope error model, an electronic compass calibration ellipse model, and a seven-dimensional EKF filter model, and setting a parameter vector step of the corresponding vehicle body motion posture is specifically:
  • the gyroscope error model calculates the angular velocity of the gyroscope through the gyroscope error calculation formula.
  • ⁇ ib is the gyroscope real
  • b ⁇ r is the gyro zero drift
  • b ⁇ g is the gyroscope output white noise
  • Eliminate magnetic field interference by calibrating the elliptical model with an electronic compass; where the electronic compass calibrates the elliptical model: Mx, my is the magnetic field strength, Xoffset and Yoffset are hard magnetic interference, and Xsf and Ysf are soft magnetic interference;
  • the vehicle body attitude is updated by the seven-dimensional EKF filter model.
  • the seven-dimensional EKF filter model is an extended Kalman filter of the seven-dimensional state vector.
  • the EKF includes the state equation and the observation equation:
  • the electronic compass calibration ellipse model is used to eliminate the interference caused by the magnetic field.
  • the actual calibration process is through the least squares method. The acquired magnetic field strength is fitted and then the above parameters are obtained.
  • Step S2 acquiring acceleration, angular velocity and earth magnetic field strength data of the vehicle body motion in real time through a nine-axis MEMS sensor;
  • the step S2 the step of acquiring the acceleration, the angular velocity and the earth magnetic field strength data of the vehicle body motion in real time through the nine-axis MEMS sensor is specifically as follows:
  • the geomagnetic field strength data of the vehicle body is collected by a geomagnetic sensor.
  • Step S3 calculating an angle, a speed, a position information, and a heading angle of the vehicle body according to the acquired acceleration, angular velocity and earth magnetic field strength data of the vehicle body through the established gyro error model and the electronic compass calibration ellipse model;
  • Step S3 calculating, according to the acquired acceleration, angular velocity and earth magnetic field strength data of the vehicle body through the established gyroscope error model and the electronic compass calibration ellipse model
  • the steps of the angle, speed, position information and heading angle of the car body are as follows:
  • the angle data is obtained by integrating the angular velocity by the gyro error model
  • the speed is calculated by integrating the acceleration data, and the position information is calculated again by integrating; the geomagnetic field strength data is calculated by the calibration parameter compensation and the inclination correction calculated by the ellipse model to calculate the heading angle of the vehicle body.
  • the MEMS sensor collects the motion information of the vehicle body in real time.
  • the angular velocity of the vehicle body collected by the gyroscope is corrected by the state estimation gyro zero offset, and the angle increment is calculated for the integral.
  • the geomagnetic sensor is compensated by soft magnetic, hard magnetic and tilt angle correction. Then calculate the heading angle.
  • Step S4 Data fusion processing is performed on the angle, speed, position information and heading angle of the vehicle body through the seven-dimensional EKF filtering model, and the motion attitude angle of the vehicle body is updated in real time.
  • Step S4 performing data fusion processing on the angle, speed, position information, and heading angle of the vehicle body through the seven-dimensional EKF filtering model, and real-time updating the moving attitude angle of the vehicle body is specifically as follows:
  • the seven-dimensional EKF filter model calculates the vehicle body attitude data through the quaternion attitude update algorithm.
  • the EKF algorithm calculation process :
  • k is the sampling time
  • (-) is the previous moment
  • (+) is the latter moment
  • ⁇ k is the state transition matrix
  • Pk is the minimum mean square error matrix
  • Q is the covariance matrix corresponding to the state vector
  • Kk is the error Gain
  • yk is the observation vector
  • Hk is the observation matrix transfer matrix
  • Rk is the covariance matrix corresponding to the observation vector.
  • Q is a quaternion vector
  • q0, q1, q2, q3 are scalars that make up the quaternion vector
  • i, j, and k are unit vectors of the three-dimensional coordinate system
  • the updated pose matrix is:
  • ⁇ , ⁇ , and ⁇ are the roll angle, the pitch angle, and the heading angle, respectively.
  • the nine-axis MEMS sensor is composed of a three-axis gyroscope, a three-axis accelerometer and a three-axis geomagnetic sensor.
  • Step S5 extracting vehicle body full attitude angle data from the vehicle body posture update data, and determining the attitude angle data value, the vehicle body full attitude angle includes a pitch angle, a roll angle, and a heading angle, wherein
  • the full attitude angle of the car body can be updated from the calculated attitude matrix
  • the mid-extraction includes the pitch angle, the roll angle and the heading angle. Since the pitch angle ⁇ is defined in the ⁇ 90° interval and coincides with the main value of the inverse sine function, there is no multi-value problem.
  • the roll angle ⁇ is defined in the interval [-180°, 180°], and the heading angle ⁇ is defined in the interval [0°, 360°]. Therefore, both ⁇ and ⁇ have multi-value problems. After calculating the main value, The element in the judgment is in which quadrant.
  • the invention has the advantages that the acceleration and the angular velocity of the motion of the object are obtained by the MEMS sensor in real time, and the angle acceleration integral obtained by the gyroscope can obtain the angle, and the speed and the integral can be calculated by integrating the acceleration to calculate the position information.
  • the geomagnetic sensor acquires the earth's magnetic field, calculates the heading angle through the compensation algorithm and fusion with the gyroscope, and then converts the attitude into a transformation matrix, thereby realizing the conversion of the carrier coordinate system and the navigation coordinate system.
  • the transformation matrix plays a "mathematical platform".
  • the transformation matrix is particularly important, because the agricultural machinery is constantly moving, its posture is constantly changing, that is, the transformation matrix must be constantly recalculated and Update.
  • Commonly used pose update algorithms have Euler angles, directional cosines and quaternions. The quaternion has no singularity compared with the Euler angle algorithm. Compared with the direction cosine, the calculation is small, which is very suitable for use in embedded products.
  • the gyroscope error model of geomagnetic field and gyroscope error model is established in the plane of agricultural machinery, and the 7D EKF (Extended Kalman Filter) update pose matrix is established.
  • the quaternion and gyroscope zero offset are estimated, and then the acceleration and magnetic field strength are calculated.
  • the heading angle is observed, so that a higher precision three-dimensional attitude angle can be obtained.
  • the error compensation and correction algorithm is adopted, which greatly reduces the error interference of the SINS algorithm.
  • the MEMS sensor and the SINS algorithm make the invention have higher performance parameters.
  • the tractor's test output heading angle error is less than 0.1°, and the pitch and roll angle errors are less than 0.01°. Using the quaternion as the Kalman filter state vector can further improve the calculation accuracy of the target parameters.

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Soil Sciences (AREA)
  • Environmental Sciences (AREA)
  • Mechanical Engineering (AREA)
  • Electromagnetism (AREA)
  • Manufacturing & Machinery (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Environmental & Geological Engineering (AREA)
  • Microelectronics & Electronic Packaging (AREA)
  • Computer Hardware Design (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

一种基于九轴MEMS传感器的农业机械全姿态角更新方法，所述基于九轴MEMS传感器的农业机械全姿态角更新方法包括如下步骤：建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量（S1）；实时获取车体运动的加速度、角速度与地球磁场强度数据（S2）；通过建立陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度（S3）；通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新（S4），上述方法步骤，误差小、精度高、稳定可靠。

Description

基于九轴MEMS传感器的农业机械全姿态角更新方法 技术领域
本发明涉及测量技术领域，尤其涉及一种基于九轴MEMS传感器的农业机械全姿态角更新方法。
背景技术
随着MEMS(Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展以及国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势，而在农业机械辅助驾驶控制过程中，车体的姿态，包括俯仰角、翻滚角和航向角，这些信息能够为高精度的组合导航和控制算法提供重要的数据输入。
目前，惯性导航***分为PINS(Platform Inertial Navigation System)和SINS(Strapdown Inertial Navigation System)，SINS相比于PINS是采用IMU(Inertial Measuring Unit)传感器通过计算建立一个“数学平台”来代替PINS。SINS多使用在飞行器导航控制***中，而针对农业机械控制领域的研究和应用则属于起步阶段，而且二者的应用对象和环境条件有较大的差别，飞行器控制***中捷联惯导实现的方法在农业机械控制中不尽适用。
发明内容
鉴于目前在农业机械上应用惯性导航存在的上述不足，本发明提供一种基于九轴MEMS传感器的农业机械全全姿态角更新方法，误差小、精度高、稳定可靠。
为达到上述目的，本发明的实施例采用如下技术方案：
一种基于九轴MEMS传感器的农业机械全姿态角更新方法，所述基于九轴MEMS传感器的农业机械全姿态角更新方法包括如下步骤：
建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量；
通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据；
根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度；
通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新；
其中，所述九轴MEMS传感器由三轴陀螺仪、三轴加速度计和三轴地磁传感器组成。
依照本发明的一个方面，所述建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量步骤具体为：
陀螺仪误差模型通过陀螺仪误差计算公式对陀螺仪角速度进行计算，其中，陀螺仪误差计算公式：ω＝ωib+bωr+bωg，其中ω为陀螺仪输出角速度，ωib为陀螺仪真实角速度，bωr为陀螺仪零漂，bωg为陀螺仪输出白噪声；
通过电子罗盘校准椭圆模型消除磁场干扰；其中，电子罗盘校准椭圆模型：
Figure PCTCN2016088316-appb-000001
mx，my为地磁场强度，Xoffset和Yoffset为硬磁干扰，Xsf和Ysf为软磁干扰；
通过七维EKF滤波模型对车体姿态进行更新处理，其中，七维EKF滤波模型为七维状态向量的扩展卡尔曼滤波，EKF包括状态方程与观测方程：
Figure PCTCN2016088316-appb-000002
y＝h(x)+v1
状态矩阵为x＝[q bωr]，q为四元数向量q0，q1，q2，q3，bωr为XYZ三轴陀螺仪零漂；其中ω为陀螺仪输出角速度，w1为过程噪声矩阵，v1为观测噪声矩阵，y为观测量，y＝[a ψmag]T，其中a为三轴加速度值，ψmag为电子罗盘计算的航向角，
Figure PCTCN2016088316-appb-000003
Figure PCTCN2016088316-appb-000004
依照本发明的一个方面，所述通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据步骤具体为：
通过陀螺仪获取车体的角速度，对陀螺仪零点漂移进行补偿；
通过加速度传感器采集车体的加速度数据；
通过地磁传感器采集车体的地磁场强度数据。
依照本发明的一个方面，所述根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度步骤具体为：
通过陀螺仪误差模型对角速度进行积分计算获得角度数据；
通过对加速度数据的积分计算出速度，再次积分计算出位置信息；
地磁场强度数据经椭圆模型计算出来的校准参数补偿和倾角修正后计算车体航向角。
依照本发明的一个方面，所述通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新步骤具体为：
七维EKF滤波模型通过四元数姿态更新算法对车体姿态数据进行计算，其中，EKF算法计算过程：
Figure PCTCN2016088316-appb-000005
Figure PCTCN2016088316-appb-000006
Figure PCTCN2016088316-appb-000007
Figure PCTCN2016088316-appb-000008
Pk(+)＝[I-KkHk]Pk(-)
Figure PCTCN2016088316-appb-000009
Figure PCTCN2016088316-appb-000010
k为采样时刻，
Figure PCTCN2016088316-appb-000011
为***状态估计量，(-)为前一时刻，(+)为后一时刻，Φk为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向 量对应的协方差矩阵，Kk为误差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵。
Figure PCTCN2016088316-appb-000012
Q为四元数向量，q0、q1、q2、q3为组成四元数向量的标量，i、j、k为三维坐标系单位向量，更新后的姿态矩阵为：
Figure PCTCN2016088316-appb-000013
Figure PCTCN2016088316-appb-000014
为载体坐标系到导航坐标系的旋转矩阵。
Figure PCTCN2016088316-appb-000015
其中γ、θ、ψ分别为横滚角、俯仰角和航向角。
依照本发明的一个方面，所述通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新步骤执行后执行以下步骤：从车体姿态更新数据中提取车体全姿态角数据，确定姿态角数据值，车体全姿态角包括俯仰角、翻滚角和航向角，其中，
Figure PCTCN2016088316-appb-000016
航向角：
Figure PCTCN2016088316-appb-000017
俯仰角：
θ＝θ主
横滚角：
Figure PCTCN2016088316-appb-000018
本发明实施的优点：通过MEMS传感器实时获取物体的运动的加速度和角速度，通过陀螺仪输出的角加速度积分可以得到角度，通过对加速度的积分可以计算出速度和再次积分可以计算出位置信息，通过地磁传感器获取地球磁场，通过补偿算法和与陀螺仪融合计算出航向角，然后将姿态换算成转换矩阵，从而实现载体坐标系和导航坐标系的转换，此转换矩阵起着是“数学平台”的作用，将SINS(Strapdown Inertial Navigation System)算法应用到农业机械中，转换矩阵尤为重要，由于农业机械时刻在运动，其姿态也在不停的变化，即转换矩阵也要不停地进行重新计算和更新。常用的姿态更新算法有欧拉角、方向余弦和四元数，四元数与欧拉角算法相比没有奇异点，与方向余弦相比计算量小非常适合在嵌入式产品中使用，通过针对农用机械平面内建立地磁场和陀螺仪误差模型陀螺仪误差模型，建立7维EKF(扩展卡尔曼滤波)更新姿态矩阵，对四元数和陀螺仪零偏进行估计，然后通过加速度和磁场强度计算的航向角进行观测，从而可以得到较高精度的三维姿态角，采用的误差补偿和修正算法，大大减小了SINS算法误差干扰，采用的MEMS传感器和SINS算法使得本发明具有较高的性能参数，经拖拉机测试输出航向角误差小于0.1°，俯仰和横滚角度误差小于0.01°，将四元数作为卡尔曼滤波状态向量可以进一步提高目标参数的计算精度。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明所述的一种基于九轴MEMS传感器的农业机械全姿态角更新方法的实施例1的方法流程图；
图2为本发明所述的一种基于九轴MEMS传感器的农业机械全姿态角更新方法的实施例2的方法流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1：
如图1所示，一种基于九轴MEMS传感器的农业机械全姿态角更新方法，所述基于九轴MEMS传感器的农业机械全姿态角更新方法包括如下步骤：
步骤S1：建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量；
所述步骤S1：建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量步骤具体为：
陀螺仪误差模型通过陀螺仪误差计算公式对陀螺仪角速度进行计算，其中，陀螺仪误差计算公式：ω＝ωib+bωr+bωg，其中ω为陀螺仪输出角速度，ωib为陀螺仪真实角速度，bωr为陀螺仪零漂，bωg为陀螺仪输出白噪声；
通过电子罗盘校准椭圆模型消除磁场干扰；其中，电子罗盘校准椭圆模型：
Figure PCTCN2016088316-appb-000019
mx，my为地磁场强度，Xoffset和Yoffset为硬磁干扰，Xsf和Ysf为软磁干扰；
通过七维EKF滤波模型对车体姿态进行更新处理，其中，七维EKF滤波模型为七维状态向量的扩展卡尔曼滤波，EKF包括状态方程与观测方程：
Figure PCTCN2016088316-appb-000020
y＝h(x)+v1
状态矩阵为x＝[q bωr]，q为四元数向量q0，q1，q2，q3，bωr为XYZ三轴陀螺仪零漂；其中ω为陀螺仪输出角速度，w1为过程噪声矩阵，v1为观测噪声矩阵，y为观测量，y＝[a ψmag]T，其中a为三轴加速度值，ψmag为电子罗盘计算的航向角，
Figure PCTCN2016088316-appb-000021
Figure PCTCN2016088316-appb-000022
由于地磁场强度比较微弱，容易受到周围铁质材料和电磁场的影响，所以在使用之前必须进行校准，建立电子罗盘校准椭圆模型用于消除磁场所带的干扰，实际校准过程是通过最小二乘法对采集到的磁场强度进行拟合，然后得出上述参数。
步骤S2：通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据；
所述步骤S2：通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据步骤具体为：
通过陀螺仪获取车体的角速度，对陀螺仪零点漂移进行补偿；
通过加速度传感器采集车体的加速度数据；
通过地磁传感器采集车体的地磁场强度数据。
步骤S3：根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度；
所述步骤S3：根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度步骤具体为：
通过陀螺仪误差模型对角速度进行积分计算获得角度数据；
通过对加速度数据的积分计算出速度，再次积分计算出位置信息；
地磁场强度数据经椭圆模型计算出来的校准参数补偿和倾角修正后计算车体航向角。
通过MEMS传感器实时采集车体的运动信息，陀螺仪采集的车体角速度通过状态估计量陀螺仪零偏进行修正，对其积分计算出角度增量，地磁传感器经软磁、硬磁和倾角修正补偿后计算出航向角。
步骤S4：通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新；
所述步骤S4：通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新步骤具体为：
七维EKF滤波模型通过四元数姿态更新算法对车体姿态数据进行计算，其中，EKF算法计算过程：
Figure PCTCN2016088316-appb-000023
Figure PCTCN2016088316-appb-000024
Figure PCTCN2016088316-appb-000025
Figure PCTCN2016088316-appb-000026
Pk(+)＝[I-KkHk]Pk(-)
Figure PCTCN2016088316-appb-000027
Figure PCTCN2016088316-appb-000028
k为采样时刻，
Figure PCTCN2016088316-appb-000029
为***状态估计量，(-)为前一时刻，(+)为后一时刻，Φk为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向量对应的协方差矩阵，Kk为误差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵。
Figure PCTCN2016088316-appb-000030
Q为四元数向量，q0、q1、q2、q3为组成四元数向量的标量，i、j、k为三维坐标系单位向量，更新后的姿态矩阵为：
Figure PCTCN2016088316-appb-000031
Figure PCTCN2016088316-appb-000032
为载体坐标系到导航坐标系的旋转矩阵。
Figure PCTCN2016088316-appb-000033
其中γ、θ、ψ分别为横滚角、俯仰角和航向角。
其中，所述九轴MEMS传感器由三轴陀螺仪、三轴加速度计和三轴地磁传感器组成。
实施例2：
如图2所示，一种基于九轴MEMS传感器的农业机械全姿态角更新方法，所述基于九轴MEMS传感器的农业机械全姿态角更新方法包括如下步骤：
步骤S1：建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量；
所述步骤S1：建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量步骤具体为：
陀螺仪误差模型通过陀螺仪误差计算公式对陀螺仪角速度进行计算，其中，陀螺仪误差计算公式：ω＝ωib+bωr+bωg，其中ω为陀螺仪输出角速度，ωib为陀螺仪真实角速度，bωr为陀螺仪零漂，bωg为陀螺仪输出白噪声；
通过电子罗盘校准椭圆模型消除磁场干扰；其中，电子罗盘校准椭圆模型：
Figure PCTCN2016088316-appb-000034
mx，my为地磁场强度，Xoffset和Yoffset为硬磁干扰，Xsf和Ysf为软磁干扰；
通过七维EKF滤波模型对车体姿态进行更新处理，其中，七维EKF滤波模型为七维状态向量的扩展卡尔曼滤波，EKF包括状态方程与观测方程：
Figure PCTCN2016088316-appb-000035
y＝h(x)+v1
状态矩阵为x＝[q bωr]，q为四元数向量q0，q1，q2，q3，bωr为XYZ三轴陀螺仪零漂；其中ω为陀螺仪输出角速度，w1为过程噪声矩阵，v1为观测噪声矩阵，y为观测量，y＝[a ψmag]T，其中a为三轴加速度值，ψmag为电子罗盘计算的航向角，
Figure PCTCN2016088316-appb-000036
Figure PCTCN2016088316-appb-000037
由于地磁场强度比较微弱，容易受到周围铁质材料和电磁场的影响，所以在使用之前必须进行校准，建立电子罗盘校准椭圆模型用于消除磁场所带的干扰，实际校准过程是通过最小二乘法对采集到的磁场强度进行拟合，然后得出上述参数。
步骤S2：通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据；
所述步骤S2：通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据步骤具体为：
通过陀螺仪获取车体的角速度，对陀螺仪零点漂移进行补偿；
通过加速度传感器采集车体的加速度数据；
通过地磁传感器采集车体的地磁场强度数据。
步骤S3：根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度；
所述步骤S3：根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出 车体的角度、速度、位置信息、航向角度步骤具体为：
通过陀螺仪误差模型对角速度进行积分计算获得角度数据；
通过对加速度数据的积分计算出速度，再次积分计算出位置信息；地磁场强度数据经椭圆模型计算出来的校准参数补偿和倾角修正后计算车体航向角。
通过MEMS传感器实时采集车体的运动信息，陀螺仪采集的车体角速度通过状态估计量陀螺仪零偏进行修正，对其积分计算出角度增量，地磁传感器经软磁、硬磁和倾角修正补偿后计算出航向角。
步骤S4：通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新。
所述步骤S4：通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新步骤具体为：
七维EKF滤波模型通过四元数姿态更新算法对车体姿态数据进行计算，其中，EKF算法计算过程：
Figure PCTCN2016088316-appb-000038
Figure PCTCN2016088316-appb-000039
Figure PCTCN2016088316-appb-000040
Figure PCTCN2016088316-appb-000041
Pk(+)＝[I-KkHk]Pk(-)
Figure PCTCN2016088316-appb-000042
Figure PCTCN2016088316-appb-000043
k为采样时刻，
Figure PCTCN2016088316-appb-000044
为***状态估计量，(-)为前一时刻，(+)为后一时刻，Φk为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向量对应的协方差矩阵，Kk为误差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵。
Figure PCTCN2016088316-appb-000045
Q为四元数向量，q0、q1、q2、q3为组成四元数向量的标量，i、j、k为三维坐标系单位向量，更新后的姿态矩阵为：
Figure PCTCN2016088316-appb-000046
为载体坐标系到导航坐标系的旋转矩阵。
Figure PCTCN2016088316-appb-000048
其中γ、θ、ψ分别为横滚角、俯仰角和航向角。
其中，所述九轴MEMS传感器由三轴陀螺仪、三轴加速度计和三轴地磁传感器组成。
步骤S5：从车体姿态更新数据中提取车体全姿态角数据，确定姿态角数据值，车体全姿态角包括俯仰角、翻滚角和航向角，其中，
Figure PCTCN2016088316-appb-000049
航向角：
Figure PCTCN2016088316-appb-000050
俯仰角：
θ＝θ主
横滚角：
Figure PCTCN2016088316-appb-000051
车体全姿态角可从更新计算后的姿态矩阵
Figure PCTCN2016088316-appb-000052
中提取，包括俯仰角、翻滚角和航向角，由于俯仰角θ定义在±90°区间，和反正弦函数的主值一致，不存在多值问题。而横滚角γ定义在[-180°，180°]区间，航向角ψ定义在[0°，360°]区间，故γ、ψ都存在多值问题，在计算出主值之后，可由
Figure PCTCN2016088316-appb-000053
中的元素判断是在哪个象限。
本发明实施的优点：通过MEMS传感器实时获取物体的运动的加速度和角速度，通过陀螺仪输出的角加速度积分可以得到角度，通过对加速度的积分可以计算出速度和再次积分可以计算出位置信息，通过地磁传感器获取地球磁场，通过补偿算法和与陀螺仪融合计算出航向角，然后将姿态换算成转换矩阵，从而实现载体坐标系和导航坐标系的转换，此转换矩阵起着是“数学平台”的作用，将SINS(Strapdown Inertial Navigation System)算法应用到农业机械中，转换矩阵尤为重要，由于农业机械时刻在运动，其姿态也在不停的变化，即转换矩阵也要不停地进行重新计算和更新。常用的姿态更新算法有欧拉角、方向余弦和四元数，四元数与欧拉角算法相比没有奇异点，与方向余弦相比计算量小非常适合在嵌入式产品中使用，通过针对农用机械平面内建立地磁场和陀螺仪误差模型陀螺仪误差模型，建立7维EKF(扩展卡尔曼滤波)更新姿态矩阵，对四元数和陀螺仪零偏进行估计，然后通过加速度和磁场强度计算的航向角进行观测，从而可以得到较高精度的三维姿态角，采用的误差补偿和修正算法，大大减小了SINS算法误差干扰，采用的MEMS传感器和SINS算法使得本发明具有较高的性能参数，经拖拉机测试输出航向角误差小于0.1°，俯仰和横滚角度误差小于0.01°，将四元数作为卡尔曼滤波状态向量可以进一步提高目标参数的计算精度。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本领域技术的技术人员在本发明公开的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (6)

  1. 一种基于九轴MEMS传感器的农业机械全姿态角更新方法，其特征在于，所述基于九轴MEMS传感器的农业机械全姿态角更新方法包括如下步骤：
    建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量；
    通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据；
    根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度；
    通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新；
    其中，所述九轴MEMS传感器由三轴陀螺仪、三轴加速度计和三轴地磁传感器组成。
  2. 根据权利要求1所述的基于九轴MEMS传感器的农业机械全姿态角更新方法，其特征在于，所述建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设定相应车体运动姿态的参数向量步骤具体为：
    陀螺仪误差模型通过陀螺仪误差计算公式对陀螺仪角速度进行计算，其中，陀螺仪误差计算公式：ω＝ωib+bωr+bωg，其中ω为陀螺仪输出角速度，ωib为陀螺仪真实角速度，bωr为陀螺仪零漂，bωg为陀螺仪输出白噪声；
    通过电子罗盘校准椭圆模型消除磁场干扰；其中，电子罗盘校准椭圆模型：
    Figure PCTCN2016088316-appb-100001
    mx，my为地磁场强度，Xoffset和Yoffset为硬磁干扰，Xsf和Ysf为软磁干扰；
    通过七维EKF滤波模型对车体姿态进行更新处理，其中，七维EKF滤波模型为七维状态向量的扩展卡尔曼滤波，EKF包括状态方程与观测方程：
    Figure PCTCN2016088316-appb-100002
    y＝h(x)+v1
    状态矩阵为x＝[q bωr]，q为四元数向量q0,q1,q2,q3，bωr为XYZ三轴陀螺仪零漂；其中ω为陀螺仪输出角速度，w1为过程噪声矩阵，v1为观测噪声矩阵，y为观测量，y＝[a ψmag]T，其中a为三轴加速度值，ψmag为电子罗盘计算的航向角，
    Figure PCTCN2016088316-appb-100003
    Figure PCTCN2016088316-appb-100004
  3. 根据权利要求2所述的基于九轴MEMS传感器的农业机械全姿态角更新方法，其特征在于，所述通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强度数据步骤具体为：
    通过陀螺仪获取车体的角速度，对陀螺仪零点漂移进行补偿；
    通过加速度传感器采集车体的加速度数据；
    通过地磁传感器采集车体的地磁场强度数据。
  4. 根据权利要求3所述的基于九轴MEMS传感器的农业机械全姿态角更新方法，其特征在于，所述根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度步骤具体为：
    通过陀螺仪误差模型对角速度进行积分计算获得角度数据；
    通过对加速度数据的积分计算出速度，再次积分计算出位置信息；
    地磁场强度数据经椭圆模型计算出来的校准参数补偿和倾角修正后计算车体航向角。
  5. 根据权利要求4所述的基于九轴MEMS传感器的农业机械全姿态角更新方法，其特征在于，所述通过七维EKF滤波模型对车体的角 度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进行实时更新步骤具体为：
    七维EKF滤波模型通过四元数姿态更新算法对车体姿态数据进行计算，其中，EKF算法计算过程：
    Figure PCTCN2016088316-appb-100005
    Figure PCTCN2016088316-appb-100006
    Figure PCTCN2016088316-appb-100007
    Figure PCTCN2016088316-appb-100008
    Pk(+)＝[I-KkHk]Pk(-)
    Figure PCTCN2016088316-appb-100009
    Figure PCTCN2016088316-appb-100010
    k为采样时刻，
    Figure PCTCN2016088316-appb-100011
    为***状态估计量，（-）为前一时刻，（+）为后一时刻，Φk为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向量对应的协方差矩阵，Kk为误差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵。
    Figure PCTCN2016088316-appb-100012
    Q为四元数向量，q0、q1、q2、q3为组成四元数向量的标量，i、j、k为三维坐标系单位向量，更新后的姿态矩阵为：
    Figure PCTCN2016088316-appb-100013
    Figure PCTCN2016088316-appb-100014
    为载体坐标系到导航坐标系的旋转矩阵。
    Figure PCTCN2016088316-appb-100015
    其中γ、θ、ψ分别为横滚角、俯仰角和航向角。
  6. 根据权利要求5所述的基于九轴MEMS传感器的农业机械全姿态角更新方法，其特征在于，所述通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数据融合处理，对车体的运动姿态角进 行实时更新步骤执行后执行以下步骤：从车体姿态更新数据中提取车体全姿态角数据，确定姿态角数据值，车体全姿态角包括俯仰角、翻滚角和航向角，其中，
    Figure PCTCN2016088316-appb-100016
    航向角：
    Figure PCTCN2016088316-appb-100017
    俯仰角：
    θ＝θ主
    横滚角：
    Figure PCTCN2016088316-appb-100018
PCT/CN2016/088316 2015-10-13 2016-07-04 基于九轴mems传感器的农业机械全姿态角更新方法 WO2017063387A1 (zh)

Priority Applications (4)

Application Number Priority Date Filing Date Title
EP16854770.1A EP3364153B1 (en) 2015-10-13 2016-07-04 Method for updating all attitude angles of agricultural machine on the basis of nine-axis mems sensor
RU2017125035A RU2662460C1 (ru) 2015-10-13 2016-07-04 Способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе мэмс
US15/538,201 US20170350721A1 (en) 2015-10-13 2016-07-04 Method of Updating All-Attitude Angle of Agricultural Machine Based on Nine-Axis MEMS Sensor
KR1020177023677A KR102017404B1 (ko) 2015-10-13 2016-07-04 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201510664990.3 2015-10-13
CN201510664990.3A CN105203098B (zh) 2015-10-13 2015-10-13 基于九轴mems传感器的农业机械全姿态角更新方法

Publications (1)

Publication Number Publication Date
WO2017063387A1 true WO2017063387A1 (zh) 2017-04-20

Family

ID=54950909

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2016/088316 WO2017063387A1 (zh) 2015-10-13 2016-07-04 基于九轴mems传感器的农业机械全姿态角更新方法

Country Status (6)

Country Link
US (1) US20170350721A1 (zh)
EP (1) EP3364153B1 (zh)
KR (1) KR102017404B1 (zh)
CN (1) CN105203098B (zh)
RU (1) RU2662460C1 (zh)
WO (1) WO2017063387A1 (zh)

Cited By (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107085246A (zh) * 2017-05-11 2017-08-22 深圳合优科技有限公司 一种基于mems的人体动作识别方法和装置
CN109813312A (zh) * 2019-03-05 2019-05-28 南京理工大学 一种卡片式测量物体姿态角的测量装置及方法
CN110057381A (zh) * 2019-03-20 2019-07-26 深圳市元征科技股份有限公司 一种导航***的零速修正方法及***
CN110207697A (zh) * 2019-04-29 2019-09-06 南京航空航天大学 基于角加速度计/陀螺/加速度计的惯性导航解算方法
CN110865580A (zh) * 2019-11-27 2020-03-06 中国船舶重工集团公司第七0七研究所 基于时分复用的半球谐振陀螺全差分控制***及控制方法
CN111121824A (zh) * 2020-01-03 2020-05-08 西北工业大学 一种mems传感器的标定方法
CN111426318A (zh) * 2020-04-22 2020-07-17 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111551168A (zh) * 2020-04-30 2020-08-18 江苏帝一集团有限公司 一种水下机器人位姿数据采集***及其数据融合方法
CN111707268A (zh) * 2020-06-19 2020-09-25 中国人民解放军国防科技大学 基于双欧法和四元数混合编排的无人机导航方法和***
CN111854762A (zh) * 2020-08-05 2020-10-30 翟瑞永 一种基于卡尔曼滤波算法的三维定位方法及其定位***
CN111896007A (zh) * 2020-08-12 2020-11-06 智能移动机器人(中山)研究院 一种补偿足地冲击的四足机器人姿态解算方法
CN111902693A (zh) * 2018-03-15 2020-11-06 西斯纳维 用于校准装备于车辆的陀螺仪的方法
CN112611380A (zh) * 2020-12-03 2021-04-06 燕山大学 基于多imu融合的姿态检测方法及其姿态检测装置
CN114111749A (zh) * 2021-10-29 2022-03-01 江苏凯卓立液压设备有限公司 一种利用陀螺仪判断和计算液压尾板动作的***及方法
CN114485636A (zh) * 2022-01-18 2022-05-13 中国人民解放军军事科学院国防科技创新研究院 一种仿生罗盘
CN114910045A (zh) * 2021-12-10 2022-08-16 浙江利尔达客思智能科技有限公司 一种基于六轴获取偏航角的方法

Families Citing this family (54)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105203098B (zh) * 2015-10-13 2018-10-02 上海华测导航技术股份有限公司 基于九轴mems传感器的农业机械全姿态角更新方法
CN106931965B (zh) * 2015-12-31 2021-04-13 ***通信集团吉林有限公司 一种确定终端姿态的方法及装置
CN105588567B (zh) * 2016-01-25 2018-03-27 北京航空航天大学 一种自动电子罗盘校准辅助式的航姿参考***及方法
JP6511406B2 (ja) * 2016-02-10 2019-05-15 クラリオン株式会社 キャリブレーションシステム、キャリブレーション装置
CN105975989B (zh) * 2016-05-10 2019-03-19 东南大学 一种基于九轴运动传感器的肘部运动状态识别方法
CN106444809B (zh) * 2016-10-12 2024-04-16 湖南绿野航空科技有限公司 一种无人机飞行控制器
CN106706018B (zh) * 2016-12-28 2019-06-14 重庆爱奇艺智能科技有限公司 Vr设备中九轴传感器性能的测试方法、装置及测试转台
CN108426573B (zh) * 2017-02-14 2023-04-07 中兴通讯股份有限公司 一种终端设备的行人步态检测方法及终端设备
CN107560613B (zh) * 2017-08-15 2020-03-31 江苏科技大学 基于九轴惯性传感器的机器人室内轨迹跟踪***及方法
JP7025215B2 (ja) * 2018-01-05 2022-02-24 ローム株式会社 測位システム及び測位方法
US10882446B2 (en) * 2018-01-22 2021-01-05 Henan Polytechnic University Graded early warning system for rollover of heavy-duty truck based on time-varying interactive kalman filtering and early warning method thereof
CN108693773B (zh) * 2018-04-04 2021-03-23 南京天辰礼达电子科技有限公司 一种农业机械自动驾驶滑坡偏差自适应估计方法
CN108897333A (zh) * 2018-07-06 2018-11-27 深圳臻迪信息技术有限公司 姿态估算方法、装置和无人机
CN108680189B (zh) * 2018-07-09 2024-04-12 无锡凌思科技有限公司 一种基于卡尔曼滤波的mems陀螺仪z轴零偏动态补偿方法
CN109211267B (zh) * 2018-08-14 2022-08-23 广州虚拟动力网络技术有限公司 一种惯性动作捕捉姿态快速校准方法及***
CN113038821B (zh) * 2018-10-30 2022-12-06 东京计器株式会社 误差修正装置
CN109238262B (zh) * 2018-11-05 2020-10-30 珠海全志科技股份有限公司 一种航向姿态解算及罗盘校准抗干扰方法
CN109540135B (zh) * 2018-11-09 2020-07-31 华南农业大学 水田拖拉机位姿检测和偏航角提取的方法及装置
CN109579836B (zh) * 2018-11-21 2022-09-06 阳光凯讯(北京)科技有限公司 一种基于mems惯性导航的室内行人方位校准方法
CN109724596A (zh) * 2018-12-07 2019-05-07 江苏大学 一种基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法
CN109764868A (zh) * 2019-02-28 2019-05-17 深圳市伟文无线通讯技术有限公司 一种基于六轴传感器设备安装姿态校准方法
CN110031020A (zh) * 2019-03-28 2019-07-19 广州英卓电子科技有限公司 一种平面磁场校正方法及其装置
CN110007354B (zh) * 2019-04-22 2020-11-06 成都理工大学 无人机半航空瞬变电磁接收线圈飞行参数测量装置及方法
CN110702113B (zh) * 2019-05-09 2022-12-27 中科探海(苏州)海洋科技有限责任公司 基于mems传感器的捷联惯导***数据预处理和姿态解算的方法
CN110132257B (zh) * 2019-05-15 2023-03-24 吉林大学 基于多传感器数据融合的人体行为预测方法
CN110281724A (zh) * 2019-05-31 2019-09-27 惠州市德赛西威汽车电子股份有限公司 一种主动式底盘自稳***及其车辆
CN110285830B (zh) * 2019-07-01 2022-12-27 中科探海(苏州)海洋科技有限责任公司 基于mems传感器的sins/gps速度匹配对准方法
CN110319833B (zh) * 2019-07-09 2022-07-15 哈尔滨工程大学 一种无误差的光纤陀螺捷联惯导***速度更新方法
CN110455312B (zh) * 2019-08-08 2021-05-14 中国科学院长春光学精密机械与物理研究所 一种陀螺安装误差标校***及其标校方法
CN110595434B (zh) * 2019-09-10 2021-09-03 兰州交通大学 基于mems传感器的四元数融合姿态估计方法
CN110608741A (zh) * 2019-09-25 2019-12-24 北京安达维尔科技股份有限公司 一种提高飞机航姿参考***姿态解算精度的方法
CN110887480B (zh) * 2019-12-11 2020-06-30 中国空气动力研究与发展中心低速空气动力研究所 基于mems传感器的飞行姿态估计方法及***
CN111006660A (zh) * 2019-12-11 2020-04-14 陕西瑞特测控技术有限公司 一种组合惯导姿态计算方法
CN110928324B (zh) * 2019-12-30 2023-07-14 北京润科通用技术有限公司 无人机飞行参数采集设备及其校准方法
CN111207734B (zh) * 2020-01-16 2022-01-07 西安因诺航空科技有限公司 一种基于ekf的无人机组合导航方法
CN111623768B (zh) * 2020-04-24 2022-04-12 北京航天控制仪器研究所 一种基于克雷洛夫角奇异条件下的姿态角解算方法
US20210337718A1 (en) 2020-05-01 2021-11-04 Kinze Manufacturing, Inc. Automatic field partners
CN111693019B (zh) * 2020-05-20 2021-04-20 西安交通大学 一种姿态传感装置及数据融合、姿态解算方法
US11332152B2 (en) * 2020-05-29 2022-05-17 GM Global Technology Operations LLC Method and apparatus for determining a velocity of a vehicle
CN111765906B (zh) * 2020-07-29 2022-06-14 三一机器人科技有限公司 误差校准方法及装置
CN112539743B (zh) * 2020-11-09 2023-02-28 北京电子工程总体研究所 一种基于陀螺寻北仪的连续寻北方位更新方法和***
CN112304340B (zh) * 2020-12-24 2021-04-06 北京轻威科技有限责任公司 一种基于九轴imu的姿态解算方法、装置及存储介质
CN113063416B (zh) * 2021-02-05 2023-08-08 重庆大学 一种基于自适应参数互补滤波的机器人姿态融合方法
CN113108790B (zh) * 2021-05-14 2024-06-25 深圳中智永浩机器人有限公司 机器人imu角度测量方法、装置、计算机设备及存储介质
CN113551665B (zh) * 2021-06-25 2023-08-11 中国科学院国家空间科学中心 一种用于运动载体的高动态运动状态感知***及感知方法
CN113569653A (zh) * 2021-06-30 2021-10-29 宁波春建电子科技有限公司 一种基于面部特征信息的三维头部姿态估计算法
CN113587925B (zh) * 2021-07-16 2023-07-07 湖南航天机电设备与特种材料研究所 一种惯性导航***及其全姿态导航解算方法与装置
CN113679568B (zh) * 2021-09-01 2022-10-04 南京医科大学 机器人辅助脑卒中患者上肢多模态镜像康复训练评分***
CN113640726B (zh) * 2021-10-19 2021-12-21 青岛杰瑞自动化有限公司 一种双轴磁强计的多方位椭圆拟合标定方法及***
CN113865616B (zh) * 2021-12-01 2022-04-05 成都九鼎科技(集团)有限公司 车辆悬架姿态测量方法和测量***
CN114279426B (zh) * 2021-12-30 2023-12-15 杭州电子科技大学 一种六轴优化的磁力计在线校准方法
CN114659488B (zh) * 2022-02-08 2023-06-23 东风悦享科技有限公司 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法
CN115147313B (zh) * 2022-09-01 2022-12-30 中国科学院空天信息创新研究院 椭圆轨道遥感图像的几何校正方法、装置、设备及介质
KR102521839B1 (ko) * 2022-10-11 2023-04-14 (주)아이하트 이동체의 동작에 따른 위험발생 감지방법, 장치 및 컴퓨터프로그램

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101782391A (zh) * 2009-06-22 2010-07-21 北京航空航天大学 机动加速度辅助的扩展卡尔曼滤波航姿***姿态估计方法
CN102589537A (zh) * 2012-03-05 2012-07-18 无锡汉和航空技术有限公司 一种有磁环境下无人机的电子罗盘校正方法
CN103557866A (zh) * 2013-09-30 2014-02-05 美新半导体（无锡）有限公司 一种基于地磁技术的虚拟陀螺仪及算法
US20150127259A1 (en) * 2013-11-01 2015-05-07 Invensense Incorporated Systems and methods for optical sensor navigation
CN105203098A (zh) * 2015-10-13 2015-12-30 上海华测导航技术股份有限公司 基于九轴mems传感器的农业机械全姿态角更新方法

Family Cites Families (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8594879B2 (en) * 2003-03-20 2013-11-26 Agjunction Llc GNSS guidance and machine control
US8639416B2 (en) * 2003-03-20 2014-01-28 Agjunction Llc GNSS guidance and machine control
US8583315B2 (en) * 2004-03-19 2013-11-12 Agjunction Llc Multi-antenna GNSS control system and method
US20070032950A1 (en) * 2005-08-05 2007-02-08 Raven Industries, Inc. Modular high-precision navigation system
FR2934043B1 (fr) * 2008-07-18 2011-04-29 Movea Sa Procede d'estimation ameliore de l'orientation d'un objet et centrale d'attitude mettant en oeuvre un tel procede
JP2012173190A (ja) * 2011-02-23 2012-09-10 Seiko Epson Corp 測位システム、測位方法
US9116233B2 (en) * 2012-07-10 2015-08-25 Broadcom Corporation Power mode control for sensors
RU127452U1 (ru) * 2013-01-09 2013-04-27 Общество с ограниченной ответственностью "Снэйл-Групп" Бортовой самописец для маломерного судна
KR101462007B1 (ko) * 2013-08-22 2014-11-18 한국과학기술연구원 자세 추정 장치 및 자세 추정 방법
KR101454153B1 (ko) * 2013-09-30 2014-11-03 국민대학교산학협력단 가상차선과 센서 융합을 통한 무인 자율주행 자동차의 항법시스템
CN103822633B (zh) * 2014-02-11 2016-12-07 哈尔滨工程大学 一种基于二阶量测更新的低成本姿态估计方法
CN104502942A (zh) * 2015-01-06 2015-04-08 上海华测导航技术有限公司 基于卫星导航和航位推测实现农业机械定位的***及方法
CN104677353A (zh) * 2015-03-17 2015-06-03 上海华测导航技术股份有限公司 基于mems传感器实现机械车体航向角检测的方法
CN104697526A (zh) * 2015-03-26 2015-06-10 上海华测导航技术股份有限公司 用于农业机械的捷联惯导***以及控制方法
RU160949U1 (ru) * 2015-12-14 2016-04-10 Игорь Викторович Рядчиков Устройство для стабилизации положения объемного тела в пространстве с силовой компенсацией отклоняющих воздействий

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101782391A (zh) * 2009-06-22 2010-07-21 北京航空航天大学 机动加速度辅助的扩展卡尔曼滤波航姿***姿态估计方法
CN102589537A (zh) * 2012-03-05 2012-07-18 无锡汉和航空技术有限公司 一种有磁环境下无人机的电子罗盘校正方法
CN103557866A (zh) * 2013-09-30 2014-02-05 美新半导体（无锡）有限公司 一种基于地磁技术的虚拟陀螺仪及算法
US20150127259A1 (en) * 2013-11-01 2015-05-07 Invensense Incorporated Systems and methods for optical sensor navigation
CN105203098A (zh) * 2015-10-13 2015-12-30 上海华测导航技术股份有限公司 基于九轴mems传感器的农业机械全姿态角更新方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
LV , YINXIN;: "Research on Integrated Attitude Determination System Based on Mems/Gps for Micro Uav", SCIENCE -ENGINEERING (B), CHINA MASTER'S THESES FULL-TEXT DATABASE, 15 February 2015 (2015-02-15), pages 11 - 13 , 26, 27, 35, 37, 38, 44, 59 and 66, ISSN: 1674-0246 *
See also references of EP3364153A4 *

Cited By (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107085246A (zh) * 2017-05-11 2017-08-22 深圳合优科技有限公司 一种基于mems的人体动作识别方法和装置
CN111902693A (zh) * 2018-03-15 2020-11-06 西斯纳维 用于校准装备于车辆的陀螺仪的方法
CN109813312A (zh) * 2019-03-05 2019-05-28 南京理工大学 一种卡片式测量物体姿态角的测量装置及方法
CN110057381A (zh) * 2019-03-20 2019-07-26 深圳市元征科技股份有限公司 一种导航***的零速修正方法及***
CN110207697A (zh) * 2019-04-29 2019-09-06 南京航空航天大学 基于角加速度计/陀螺/加速度计的惯性导航解算方法
CN110865580A (zh) * 2019-11-27 2020-03-06 中国船舶重工集团公司第七0七研究所 基于时分复用的半球谐振陀螺全差分控制***及控制方法
CN110865580B (zh) * 2019-11-27 2022-07-26 中国船舶重工集团公司第七0七研究所 基于时分复用的半球谐振陀螺全差分控制***及控制方法
CN111121824A (zh) * 2020-01-03 2020-05-08 西北工业大学 一种mems传感器的标定方法
CN111121824B (zh) * 2020-01-03 2022-11-22 西北工业大学 一种mems传感器的标定方法
CN111426318A (zh) * 2020-04-22 2020-07-17 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111426318B (zh) * 2020-04-22 2024-01-26 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111551168A (zh) * 2020-04-30 2020-08-18 江苏帝一集团有限公司 一种水下机器人位姿数据采集***及其数据融合方法
CN111707268A (zh) * 2020-06-19 2020-09-25 中国人民解放军国防科技大学 基于双欧法和四元数混合编排的无人机导航方法和***
CN111854762A (zh) * 2020-08-05 2020-10-30 翟瑞永 一种基于卡尔曼滤波算法的三维定位方法及其定位***
CN111896007A (zh) * 2020-08-12 2020-11-06 智能移动机器人(中山)研究院 一种补偿足地冲击的四足机器人姿态解算方法
CN112611380B (zh) * 2020-12-03 2022-07-01 燕山大学 基于多imu融合的姿态检测方法及其姿态检测装置
CN112611380A (zh) * 2020-12-03 2021-04-06 燕山大学 基于多imu融合的姿态检测方法及其姿态检测装置
CN114111749A (zh) * 2021-10-29 2022-03-01 江苏凯卓立液压设备有限公司 一种利用陀螺仪判断和计算液压尾板动作的***及方法
CN114111749B (zh) * 2021-10-29 2022-12-02 江苏凯卓立液压设备有限公司 一种利用陀螺仪判断和计算液压尾板动作的***及方法
CN114910045A (zh) * 2021-12-10 2022-08-16 浙江利尔达客思智能科技有限公司 一种基于六轴获取偏航角的方法
CN114910045B (zh) * 2021-12-10 2024-04-19 浙江利尔达客思智能科技有限公司 一种基于六轴获取偏航角的方法
CN114485636A (zh) * 2022-01-18 2022-05-13 中国人民解放军军事科学院国防科技创新研究院 一种仿生罗盘

Also Published As

Publication number Publication date
RU2662460C1 (ru) 2018-07-26
CN105203098B (zh) 2018-10-02
EP3364153B1 (en) 2020-11-25
US20170350721A1 (en) 2017-12-07
CN105203098A (zh) 2015-12-30
KR102017404B1 (ko) 2019-10-21
KR20170104621A (ko) 2017-09-15
EP3364153A1 (en) 2018-08-22
EP3364153A4 (en) 2019-06-05

Similar Documents

Publication Publication Date Title
WO2017063387A1 (zh) 基于九轴mems传感器的农业机械全姿态角更新方法
CN108225308B (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
WO2017063388A1 (zh) 一种惯导装置初始对准方法
Han et al. A novel method to integrate IMU and magnetometers in attitude and heading reference systems
CN109238262B (zh) 一种航向姿态解算及罗盘校准抗干扰方法
CN107607113B (zh) 一种两轴姿态倾角测量方法
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航***及方法
WO2017107434A1 (zh) 一种基于单天线的gnss-ins车辆定姿方法
WO2018214227A1 (zh) 一种无人车实时姿态测量方法
CN111551174A (zh) 基于多传感器惯性导航***的高动态车辆姿态计算方法及***
CN109579836B (zh) 一种基于mems惯性导航的室内行人方位校准方法
Mansoor et al. Improved attitude determination by compensation of gyroscopic drift by use of accelerometers and magnetometers
CN110487267A (zh) 一种基于vio&uwb松组合的无人机导航***及方法
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及***
JP2012173190A (ja) 測位システム、測位方法
CN110017837A (zh) 一种姿态抗磁干扰的组合导航方法
Edwan et al. Reduced DCM based attitude estimation using low-cost IMU and magnetometer triad
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN115540860A (zh) 一种多传感器融合位姿估计算法
JP2019120587A (ja) 測位システム及び測位方法
CN106595669B (zh) 一种旋转体姿态解算方法
CN111307114A (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 16854770

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 15538201

Country of ref document: US

ENP Entry into the national phase

Ref document number: 2017125035

Country of ref document: RU

Kind code of ref document: A

REEP Request for entry into the european phase

Ref document number: 2016854770

Country of ref document: EP

ENP Entry into the national phase

Ref document number: 20177023677

Country of ref document: KR

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE