CN109141414A - A kind of identification of farm work vehicle external acceleration and posture synchronous estimation method - Google Patents
A kind of identification of farm work vehicle external acceleration and posture synchronous estimation method Download PDFInfo
- Publication number
- CN109141414A CN109141414A CN201811029509.3A CN201811029509A CN109141414A CN 109141414 A CN109141414 A CN 109141414A CN 201811029509 A CN201811029509 A CN 201811029509A CN 109141414 A CN109141414 A CN 109141414A
- Authority
- CN
- China
- Prior art keywords
- external acceleration
- work vehicle
- measurement
- farm work
- axis
- 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
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Gyroscopes (AREA)
- Navigation (AREA)
Abstract
The invention discloses a kind of identifications of farm work vehicle external acceleration and posture synchronous estimation method, include the following steps: the measurement model based on three-axis gyroscope and three axis accelerometer, the first-order low-pass wave pattern for deriving external acceleration, is fused to Kalman filter；By three-axis gyroscope measurement amount ωX、ωY、ωZState input quantity as Kalman filter；By the measurement amount a of three axis accelerometerX、aY、aZAs the observation input vector of filter, the amendment of external acceleration is carried out；System Kalman filtering state equation and measurement equation comprising 3 state vectors with 3 observation vectors are established, farm work vehicle attitude angle and identification external acceleration are accurately measured.The method of the present invention considers influence of the outside vehicle acceleration to attitude measurement, can effectively improve farm work vehicle attitude measurement accuracy, promotes operation quality.
Description
Technical field
The invention belongs to the technical fields of inertial navigation, and in particular to a kind of identification of farm work vehicle external acceleration with
Posture synchronous estimation method.
Background technique
Inertial navigation technology is the 20th century entirely autonomous formula airmanship that grows up of mid-term.Pass through inertial measurement cluster
The angular speed and acceleration information for measuring carrier relative inertness space, carrier instantaneous velocity is calculated using Newton's laws of motion automatically
With location information, have the characteristics that not depend on external information, not outwardly radiation energy, interference-free, good concealment.It can be continuous
Ground provides whole navigation of carrier, Guidance Parameter (position, linear velocity, angular speed, attitude angle), be widely used in space flight, aviation,
Navigational field, especially military field.And the accurate appraisal of agricultural machinery and implement attitude angle is the key that realize the accurate operation of agricultural machinery
Parameter.
For agricultural machinery, when such as navigation operation, due to the out-of-flatness in field face, simple dependence GNSS positioning can not be protected
The operation of tractor straight line is demonstrate,proved, only projection correction is carried out by the real-time roll angle of vehicle body and pitch angle information, can guarantee to draw
Machine is according to the accurate walking operations of planning path.When for another example land leveller is with spraying machine operation, the guarantee of operation quality depends on level land
The level of shovel and spray boom, this requires the Real-time Feedback of agricultural machinery and implement attitude detection information, to guarantee horizontal operation.Accurate appearance simultaneously
State inclination angle detection is even more the basis of agricultural machinery with agricultural machinery and implement Dynamic Modeling, the research of operation safety early warning.
Attitude angle detection at present mostly uses the methods of inertial sensor, image procossing to obtain, and uses a variety of biographies there are also some
Sensor integration technology carries out attitude monitoring.Wherein inertial sensor generally uses accelerometer and gyroscope, obliquity sensor, surpasses
Sonic sensor etc. is monitored posture.Sensor fusion techniques mainly by multiple sensors combine to attitude angle into
Row monitoring.Real using MEMS sensor is most widely used method at present to the measurement of posture inclination angle, economy, stability and
Adaptive criteria all has outstanding advantage compared to other measurement methods.
Posture inclination angle more new Algorithm is the key algorithm of inertial navigation system, and traditional posture inclination angle more new algorithm has Euler
Horn cupping, direction cosines and Quaternion method.Quaternion method needs to carry out Taylor expansion when calculating, and generallys use and ignores its higher order term general
The linear progress posture estimation of non-linear transfer, there are errors.Euler's horn cupping is the direct iteration Eulerian angles differential equation, and there are surprises
Point.There are intrinsic zero bias, long-time integration operation is not suitable for being used alone gyroscope there are accumulated error.Accelerometer can be with
Quasi-static gorge object Euler attitude angle is resolved, accumulated error problem is not present in no integral operation, but its attitude angle resolved is by outer
Portion's acceleration influences, and can not apply in a dynamic condition.So how to develop a kind of progress attitude angle fusion and external acceleration
The method for recognizing to improve inclination angle measurement accuracy, stability, is one of research direction of those skilled in the art.
Summary of the invention
The shortcomings that it is a primary object of the present invention to overcome the prior art and insufficient, is provided outside a kind of farm work vehicle
Acceleration identification and posture synchronous estimation method, to effectively improve farm work vehicle attitude measurement accuracy, promotion operation quality.
In order to achieve the above object, the invention adopts the following technical scheme:
The present invention provides a kind of identification of farm work vehicle external acceleration and posture synchronous estimation method, including following steps
It is rapid:
S1, the measurement model based on three-axis gyroscope and three axis accelerometer, derive the first-order low-pass of external acceleration
Wave pattern is fused to Kalman filter；
S2, by three-axis gyroscope measurement amount ωX、ωY、ωZState input quantity as Kalman filter；
S3, the measurement amount a by three axis accelerometerX、aY、aZAs the observation input vector of filter, external acceleration is carried out
The amendment of degree；
S4, system Kalman filtering state equation and measurement side comprising 3 state vectors with 3 observation vectors are established
Journey accurately measures farm work vehicle attitude angle and identification external acceleration.
Preferred technical solution further includes following step before step S1:
Build hardware platform, the hardware platform includes 3 axis accelerometers and 3 axis gyroscopes, 3 axis accelerometer and
The output frequency of 3 axis gyroscopes is not less than 100Hz.
Preferred technical solution, in step sl,
Three-axis gyroscope measurement model are as follows: yG=ω+nG,
Three axis accelerometer measurement model are as follows: yA=g+ α+nA,
The single order low pass model of external acceleration are as follows: αt=k αt-1+εt。
Preferred technical solution, in step s3,
External acceleration evaluated error are as follows:
Solving of attitude based on direction cosine matrix are as follows:
Wherein yG, yAIt is three-axis gyroscope and three axis accelerometer measured value respectively, thus can estimates external acceleration.
Preferred technical solution, in step s 4, the system Kalman filtering state equation are specific as follows:
The state vector of system is [c31,c32,c33]；
System mode state-transition matrix is
System mode noise matrix is
System mode noise covariance matrix is
Preferred technical solution, in step s 4, the measurement equation are as follows:
Gyroscope measures noise covariance matrix
Systematic observation matrix is
System input matrix is H=gI；
Systematic observation noise matrix is
Systematic observation noise covariance matrix is Rt-1=∑α+∑A；
The measurement error covariance matrix of external acceleration model is
Accelerometer measures noise covariance matrix is
T indicates this timing calculated, and Δ t indicates time of fusion.
Compared with the prior art, the invention has the following advantages and beneficial effects:
1, the present invention establishes external acceleration meter model, considers that external acceleration influences attitude measurement, improves measurement essence
Degree.
2, using direction cosine matrix, wherein 3 parameters carry out attitude algorithm to the present invention, reduce operand, improve operation
Efficiency.
3, the present invention builds the external expansion Kalman filtering mould with 3 state vectors and 3 observation vectors of system
Type carries out attitude angle fusion and recognizes with external acceleration, guarantees farm work vehicle attitude measurement accuracy.
In conclusion the present invention provides a kind of identification of farm work vehicle external acceleration and posture synchronous estimation method,
Using 3 axis accelerometers and 3 axis gyroscope inertial sensors；It establishes single order external acceleration model and considers external acceleration shadow
Ring, build with 3 state vectors and 3 observation vectors it is external expand Kalman filter model and carry out attitude angle merge and outside
The identification of portion's acceleration, ensure that farm work vehicle posture inclination angle measurement accuracy and stability.
Detailed description of the invention
Fig. 1 is that a kind of farm work vehicle external acceleration recognizes estimating system schematic diagram synchronous with posture；
Fig. 2 is that external acceleration recognizes test result schematic diagram；
The roll inclination angle of the present invention Fig. 3 and MTi-300 comparison schematic diagram；
The pitching inclination angle of the present invention Fig. 4 and MTi-300 comparison schematic diagram.
Specific embodiment
In order to make the objectives, technical solutions, and advantages of the present invention clearer, below in conjunction with attached drawing and example, to this
Invention is further elaborated.Specific example described herein is only used to explain the present invention, is not used to limit this
Invention.As long as in addition, technical characteristic involved in the various embodiments of the present invention described below non-structure each other
It can be combined with each other at conflict.
Embodiment
A kind of farm work vehicle external acceleration identification of the present invention and posture synchronous estimation method, are mainly based upon outside
The Kalman filtering algorithm of Fast track surgery merges 6 axis micro-inertia sensor information, realizes farm work vehicle external acceleration
Recognize estimation synchronous with posture, main technical schemes are as follows:
Using the identification of first-order low-pass wave external acceleration model realization external acceleration, and by direction cosine matrix
Method carries out obliquity and attitude resolving；
It establishes the Kalman filtering state equation comprising 3 state vectors and 3 observation vectors and measurement equation progress is real
When estimation attitude angle and identification external acceleration.
The method of the present invention considers influence of the outside vehicle acceleration to attitude measurement, can effectively improve farm work vehicle appearance
State measurement accuracy promotes operation quality.
As shown in Figure 1, a kind of above-mentioned farm work vehicle external acceleration identification is specifically wrapped with posture synchronous estimation method
Include following step:
It is building for hardware platform first, hardware platform of the invention has reliable and stable 3 axis accelerometer, 3 axis gyros
Instrument, output frequency are not less than 100Hz.Due to present invention focuses on algorithm introduction, therefore detailed hardware platform information is not herein
Expansion is introduced.
In gimbaled inertial navigation system, from navigational coordinate system n to the spin matrix of bodywork reference frame bIt is (simple hereinafter
It is written as C) it can be indicated with the direction cosine matrix of a 3x 3:
If the Eulerian angles with conventional Z-Y-X rotational order indicate that spin matrix C is equal to:
In formula: φ, θ and ψ respectively represent roll angle, pitch angle and course angle in formula.Arrange the solution that can derive attitude angle
Calculate formula are as follows:
Therefore according to direction cosine matrix, demand solution 3 elements of the 3rd row can obtain corresponding attitude angle.
The external of posture inclination angle frame of reference is established as shown in Figure 1: expands Kalman filtering state equation and measurement equation:
Wherein x is state vector, and z is measurement vector, and Φ is state-transition matrix, and H is observing matrix；W and v make an uproar for state
Sound and observation noise, in which:
The state vector of system is [c31,c32,c33]；
System mode state-transition matrix is
System mode noise matrix is
System mode noise covariance matrix is
Gyroscope measures noise covariance matrix
Systematic observation matrix is
System input matrix is H=gI；
Systematic observation noise matrix is
Systematic observation noise covariance matrix is Rt-1=∑α+∑A；
The measurement error covariance matrix of external acceleration model is
Accelerometer measures noise covariance matrix is
The process error ω of state equation as a preferred technical solution,k-1With the measurement error υ of observational equationkAssume
For zero-mean and meet the white Gaussian noise of normal distribution, corresponds to the process error covariance matrix Q of systemkAnd measurement error
Covariance matrix Rk,
Wherein t indicates this timing calculated, and Δ t indicates time of fusion.
Fig. 2 is that external acceleration recognizes test result schematic diagram；
The roll inclination angle of the present invention Fig. 3 and MTi-300 comparison schematic diagram；
The pitching inclination angle of the present invention Fig. 4 and MTi-300 comparison schematic diagram.
As can be seen from figs. 3 and 4 this method measurement accuracy is suitable with MTi-300, dynamic measurement precision is up to 0.39 °, performance
Stablize.
The above embodiment is a preferred embodiment of the present invention, but embodiments of the present invention are not by above-described embodiment
Limitation, other any changes, modifications, substitutions, combinations, simplifications made without departing from the spirit and principles of the present invention,
It should be equivalent substitute mode, be included within the scope of the present invention.
Claims (6)
1. a kind of farm work vehicle external acceleration identification and posture synchronous estimation method, which is characterized in that including following steps
It is rapid:
S1, the measurement model based on three-axis gyroscope and three axis accelerometer derive the first-order low-pass wave mould of external acceleration
Type is fused to Kalman filter；
S2, by three-axis gyroscope measurement amount ωX、ωY、ωZState input quantity as Kalman filter；
S3, the measurement amount a by three axis accelerometerX、aY、aZAs the observation input vector of filter, external acceleration is carried out
Amendment；
S4, system Kalman filtering state equation and measurement equation comprising 3 state vectors with 3 observation vectors, essence are established
Locating tab assembly farm work vehicle attitude angle and identification external acceleration.
2. farm work vehicle external acceleration identification according to claim 1 and posture synchronous estimation method, feature
It is, further includes following step before step S1:
Hardware platform is built, the hardware platform includes 3 axis accelerometers and 3 axis gyroscopes, 3 axis accelerometer and 3 axis
The output frequency of gyroscope is not less than 100Hz.
3. farm work vehicle external acceleration identification according to claim 1 and posture synchronous estimation method, feature
It is, in step sl,
Three-axis gyroscope measurement model are as follows: yG=ω+nG,
Three axis accelerometer measurement model are as follows: yA=g+ α+nA,
The single order low pass model of external acceleration are as follows: αt=k αt-1+εt。
4. farm work vehicle external acceleration identification according to claim 3 and posture synchronous estimation method, feature
It is, in step s3,
External acceleration evaluated error are as follows:
Solving of attitude based on direction cosine matrix are as follows:
Wherein yG, yAIt is three-axis gyroscope and three axis accelerometer measured value respectively, thus can estimates external acceleration.
5. farm work vehicle external acceleration identification according to claim 1 and posture synchronous estimation method, feature
It is, in step s 4, the system Kalman filtering state equation is specific as follows:
The state vector of system is [c31,c32,c33]；
System mode state-transition matrix is
System mode noise matrix is
System mode noise covariance matrix is
6. farm work vehicle external acceleration identification according to claim 1 and posture synchronous estimation method, feature
It is, in step s 4, the measurement equation is as follows:
Gyroscope measures noise covariance matrix
Systematic observation matrix is
System input matrix is H=gI；
Systematic observation noise matrix is
Systematic observation noise covariance matrix is Rt-1=∑α+∑A；
The measurement error covariance matrix of external acceleration model is
Accelerometer measures noise covariance matrix is
T indicates this timing calculated, and Δ t indicates time of fusion.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811029509.3A CN109141414A (en) | 2018-09-05 | 2018-09-05 | A kind of identification of farm work vehicle external acceleration and posture synchronous estimation method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811029509.3A CN109141414A (en) | 2018-09-05 | 2018-09-05 | A kind of identification of farm work vehicle external acceleration and posture synchronous estimation method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN109141414A true CN109141414A (en) | 2019-01-04 |
Family
ID=64826874
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811029509.3A Pending CN109141414A (en) | 2018-09-05 | 2018-09-05 | A kind of identification of farm work vehicle external acceleration and posture synchronous estimation method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109141414A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118067063A (en) * | 2024-02-28 | 2024-05-24 | 江苏大学 | Tractor tilling depth detection method based on attitude disturbance rejection estimation |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP0294811A2 (en) * | 1987-06-11 | 1988-12-14 | Applied Technologies Associates | High speed well surveying and land navigation |
| CN106197376A (en) * | 2016-09-23 | 2016-12-07 | 华南农业大学 | Car body obliqueness measuring method based on single shaft MEMS inertial sensor |
| CN106772517A (en) * | 2016-12-29 | 2017-05-31 | 华南农业大学 | Agricultural machinery roll angle method of testing based on double antenna GNSS receiver/gyroscope information fusion |
| CN106989773A (en) * | 2017-04-07 | 2017-07-28 | 浙江大学 | A kind of attitude transducer and posture renewal method |
| CN107014376A (en) * | 2017-03-01 | 2017-08-04 | 华南农业大学 | A kind of posture inclination angle method of estimation suitable for the accurate operation of agricultural machinery |
| CN107607113A (en) * | 2017-08-02 | 2018-01-19 | 华南农业大学 | A kind of two axle posture inclination angle measurement methods |
-
2018
- 2018-09-05 CN CN201811029509.3A patent/CN109141414A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP0294811A2 (en) * | 1987-06-11 | 1988-12-14 | Applied Technologies Associates | High speed well surveying and land navigation |
| CN106197376A (en) * | 2016-09-23 | 2016-12-07 | 华南农业大学 | Car body obliqueness measuring method based on single shaft MEMS inertial sensor |
| CN106772517A (en) * | 2016-12-29 | 2017-05-31 | 华南农业大学 | Agricultural machinery roll angle method of testing based on double antenna GNSS receiver/gyroscope information fusion |
| CN107014376A (en) * | 2017-03-01 | 2017-08-04 | 华南农业大学 | A kind of posture inclination angle method of estimation suitable for the accurate operation of agricultural machinery |
| CN106989773A (en) * | 2017-04-07 | 2017-07-28 | 浙江大学 | A kind of attitude transducer and posture renewal method |
| CN107607113A (en) * | 2017-08-02 | 2018-01-19 | 华南农业大学 | A kind of two axle posture inclination angle measurement methods |
Non-Patent Citations (1)
| Title |
|---|
| JUNG KEUN LEE 等: "Estimation of Attitude and External Acceleration Using Inertial Sensor Measurement During Various Dynamic Conditions", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118067063A (en) * | 2024-02-28 | 2024-05-24 | 江苏大学 | Tractor tilling depth detection method based on attitude disturbance rejection estimation |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111207774B (en) | Method and system for laser-IMU external reference calibration | |
| CN107014376B (en) | A kind of posture inclination angle estimation method suitable for the accurate operation of agricultural machinery | |
| CN103630137B (en) | A kind of for the attitude of navigational system and the bearing calibration of course angle | |
| CN107607113A (en) | A kind of two axle posture inclination angle measurement methods | |
| CN104457754B (en) | SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method | |
| CN108731670A (en) | Inertia/visual odometry combined navigation locating method based on measurement model optimization | |
| CN111596333B (en) | Underwater positioning navigation method and system | |
| CN109596144B (en) | GNSS position-assisted SINS inter-travel initial alignment method | |
| CN102538781A (en) | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method | |
| CN109186597B (en) | Positioning method of indoor wheeled robot based on double MEMS-IMU | |
| CN112697138B (en) | Bionic polarization synchronous positioning and composition method based on factor graph optimization | |
| CN108387236B (en) | Polarized light SLAM method based on extended Kalman filtering | |
| CN107797125B (en) | A method of reducing deep-sea detecting type AUV navigation positioning errors | |
| JP2005221504A (en) | Geomagnetic determination method and apparatus using compass, and azimuth determination method and apparatus for mobile object | |
| CN103363991A (en) | IMU (inertial measurement unit) and distance-measuring sensor fusion method applicable to selenographic rugged terrains | |
| CN109959374B (en) | Full-time and full-range reverse smooth filtering method for pedestrian inertial navigation | |
| CN110954102A (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN115540860A (en) | Multi-sensor fusion pose estimation algorithm | |
| CN105547300A (en) | All-source navigation system and method used for AUV (Autonomous Underwater Vehicle) | |
| CN104697520A (en) | Combined navigation method based on integrated gyroscope free strapdown inertial navigation system and GPS | |
| CN112362057A (en) | Inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation | |
| CN115855048A (en) | Multi-sensor fusion pose estimation method | |
| CN116642482A (en) | Positioning method, equipment and medium based on solid-state laser radar and inertial navigation | |
| JP2009250778A (en) | Repeated calculation control method and device in kalman filter processing | |
| CN113639722B (en) | Continuous laser scanning registration auxiliary inertial positioning and attitude determination method |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190104 |