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
- measurement
- attitude
- matrix
- state
- 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
- 230000001133 acceleration Effects 0.000 title claims abstract description 51
- 238000000034 method Methods 0.000 title claims abstract description 29
- 230000001360 synchronised effect Effects 0.000 title description 10
- 238000005259 measurement Methods 0.000 claims abstract description 41
- 239000013598 vector Substances 0.000 claims abstract description 20
- 230000009897 systematic effect Effects 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 42
- 230000004927 fusion Effects 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims 1
- 238000001914 filtration Methods 0.000 abstract description 7
- 238000010586 diagram Methods 0.000 description 7
- 238000001514 detection method Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 239000007921 spray Substances 0.000 description 1
- 238000005507 spraying Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
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
本发明公开了一种田间作业车辆外部加速度辨识与姿态同步估计方法，包括下述步骤：基于三轴陀螺仪与三轴加速度计的测量模型，推导外部加速度的一阶低通滤波模型，融合到卡尔曼滤波器；将三轴陀螺仪测量量ωX、ωY、ωZ作为卡尔曼滤波器的状态输入量；将三轴加速度计的测量量aX、aY、aZ作为滤波器的观测输入向量，进行外部加速度的修正；建立包含3个状态向量与3个观测向量的系统卡尔曼滤波状态方程和测量方程，精准测量田间作业车辆姿态角与辨识外部加速度。本发明方法考虑车辆外部加速度对姿态测量的影响，能有效提高田间作业车辆姿态测量精度，提升作业质量。
The invention discloses a method for external acceleration identification and attitude synchronization estimation of a field operation vehicle, comprising the following steps: deriving a first-order low-pass filtering model of external acceleration based on a measurement model of a three-axis gyroscope and a three-axis accelerometer, and merging it into a Kalman filter; use the three-axis gyroscope measurement ω X , ω Y , ω Z as the state input of the Kalman filter; use the three-axis accelerometer measurement a X , a Y , a Z as the filter Observing the input vector and correcting the external acceleration; establishing a systematic Kalman filter state equation and measurement equation including 3 state vectors and 3 observation vectors to accurately measure the attitude angle of the field vehicle and identify the external acceleration. The method of the invention considers the influence of the external acceleration of the vehicle on the attitude measurement, and can effectively improve the attitude measurement accuracy of the field operation vehicle and improve the 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)
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 (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116007724A (en) * | 2022-12-14 | 2023-04-25 | 上海鉴创科技有限公司 | Real-time dynamic weighing method for vehicle |
| 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 (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116007724A (en) * | 2022-12-14 | 2023-04-25 | 上海鉴创科技有限公司 | Real-time dynamic weighing method for vehicle |
| 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 |
|---|---|---|
| CN108731670B (en) | Inertial/visual odometer integrated navigation positioning method based on measurement model optimization | |
| CN103630137B (en) | A kind of for the attitude of navigational system and the bearing calibration of course angle | |
| JP5328252B2 (en) | Position detection apparatus and position detection method for navigation system | |
| CN107014376B (en) | A kind of posture inclination angle estimation method suitable for the accurate operation of agricultural machinery | |
| CN102538781B (en) | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method | |
| CN110887481B (en) | Carrier dynamic attitude estimation method based on MEMS inertial sensor | |
| CN107797125B (en) | A method of reducing deep-sea detecting type AUV navigation positioning errors | |
| CN107607113B (en) | Method for measuring inclination angles of two-axis attitude | |
| CN110702104A (en) | An Inertial Navigation Error Correction Method Based on Vehicle Zero Speed Detection | |
| CN107063246A (en) | A Loose Combination Navigation Method of Visual Navigation/Inertial Navigation | |
| CN101900573B (en) | Method for realizing landtype inertial navigation system movement aiming | |
| CN108931244A (en) | Ins error suppressing method and system based on train kinematic constraint | |
| CN110954102A (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN106370178B (en) | Attitude measurement method and device of mobile terminal equipment | |
| JP5164645B2 (en) | Method and apparatus for repetitive calculation control in Kalman filter processing | |
| CN115540860A (en) | Multi-sensor fusion pose estimation algorithm | |
| CN110057356A (en) | Vehicle positioning method and device in a kind of tunnel | |
| CN104697520A (en) | Combined navigation method based on integrated gyroscope free strapdown inertial navigation system and GPS | |
| CN113074757A (en) | Calibration method for vehicle-mounted inertial navigation installation error angle | |
| CN115855048A (en) | Multi-sensor fusion pose estimation method | |
| CN114915913A (en) | UWB-IMU combined indoor positioning method based on sliding window factor graph | |
| CN109141414A (en) | A kind of identification of farm work vehicle external acceleration and posture synchronous estimation method | |
| CN112729283A (en) | Navigation method based on depth camera/MEMS inertial navigation/odometer combination | |
| CN116067369A (en) | Pedestrian collaborative navigation method based on distance and angle | |
| CN111947685A (en) | Coarse alignment method for movable base of polar region grid coordinate system |
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: 20190104 |
|
| RJ01 | Rejection of invention patent application after publication |