CN101846734B - Agricultural machinery navigation and position method and system and agricultural machinery industrial personal computer - Google Patents

Abstract

The invention relates to an agricultural machinery navigation and position method and a system and an agricultural machinery industrial personal computer. The method comprises the following steps: respectively receiving the current agricultural machinery positioning information obtained by a global satellite positioning system, an inertia measurement unit and an electronic compass; converting the positioning information to the positioning information under the same coordinate system; and using a Kalman filtering algorithm to carry out data fusion on the positioning information under the coordinate system in the coordinate system, and using a double-fuzzy control self-adapting algorithm to regulate the Kalman filtering algorithm in real time to obtain the current agricultural machinery position information. The system comprises the global satellite positioning system, the inertia measurement unit, the electronic compass and the industrial personal computer. In the invention, the defect of low position accuracy of a single sensor and a combined method is overcome. And the invention is applicable to complex and inconstant field operation environment and has high positioning accuracy.

Description

Agricultural machinery navigation and position method, system and agricultural machinery industrial personal computer

Technical field

The present invention relates to communication technical field, particularly a kind of agricultural machinery navigation and position method, system and agricultural machinery industrial personal computer.

Background technology

Navigator fix is a kind of technology that combines satellite and communication; Mainly utilize Navsat to test and find range; Along with developing rapidly of the communication technology; The navigator fix technology is applied to aspects such as engineering construction, exploration mapping, accurate timing, delivery vehicle and military weapon more and more widely, has characteristics such as round-the-clock, high precision, robotization and high benefit.

A kind of navigator fix of prior art adopts Global Positioning System (GPS) (Global Position Systems; Hereinafter to be referred as: GPS), the GPS location technology adopts mutual positioning principle, through the distance of known several points, can obtain the residing position of unknown point.Wherein, known point is an Aerospace Satellite, and unknown point is a certain moving target in ground.The location of GPS and measuring accuracy are high, and do not receive the region time restriction.But pickup electrode might interrupt when gps antenna runs into veil; Such as the navigator fix that is applied to farm machinery; When farm machinery during in farm work; Regular meeting runs into tree shade or other block, and the excessive locating information that causes of the as easy as rolling off a log GPS of causing positioning error lost efficacy, thereby influences the precision of navigator fix.

A kind of navigator fix of prior art adopts the inertial navigation location, and the inertial navigation location is a kind of autonomous locator meams fully, and it utilizes Inertial Measurement Unit (Inertia Measurement Unite; Hereinafter to be referred as: IMU) output speed, attitude and positional information continuously.Precision is high in short-term for it, does not receive the interference of external environment factor, can be widely used in the navigator fix of space flight, navigation and road vehicles.What the inertial navigation location recorded is angular velocity signal and acceleration signal, needs further angle speed and acceleration to carry out integration and obtains the course heading signal, still, in integral process, is easy to generate cumulative errors.Therefore, the inertial navigation location only is applicable to exports hi-Fix information within a short period of time, and along with the prolongation of time, cumulative errors constantly increase, and cause bearing accuracy to descend.

A kind of navigator fix of prior art adopts the electronic compass location, and electronic compass can obtain course angle information in real time in navigation positioning system, but its precision receives the influence of surrounding magnetic field or magnet easily.

The another kind of navigator fix of prior art adopts integrated positioning, and integrated positioning adopts the Kalman filtering method that the locating information of each sensor is carried out data fusion usually.But when using the Kalman wave filter, the supposing the system noise is a zero-mean white noise series with measuring noise usually; And known variance battle array is Q and R; But in fact, the impossible entirely accurate of system model, the value of Q and R also can change according to the signal quality of measuring error and sensor.In addition, filter gain matrix of coefficients K hypothesis calculates when pre-filter is in optimum state, and in the farmland operation environment of complicacy, environmental change can exert an influence to the sensor signal quality.

Summary of the invention

The purpose of this invention is to provide a kind of agricultural machinery navigation and position method, system and agricultural machinery industrial personal computer; To be applicable to the farm machinery navigator fix of farm work; Overcome single-sensor location and the low defective of combined method bearing accuracy, be applicable to farm work environment complicated and changeable, bearing accuracy height.

For realizing above-mentioned purpose, the invention provides a kind of agricultural machinery navigation and position method, comprising:

Receive the current locating information of farm machinery that Global Positioning System (GPS), Inertial Measurement Unit and electronic compass get access to respectively;

Convert said locating information under the same coordinate system locating information;

Adopt the Kalman filtering algorithm that the locating information under the said the same coordinate system is carried out data fusion under said coordinate system; And adopt Double Fuzzy control adaptive algorithm that said Kalman filtering algorithm is adjusted in real time, obtain said farm machinery current position information;

Wherein, said employing Double Fuzzy control adaptive algorithm is adjusted in real time said Kalman filtering algorithm and is specially:

Adjust filter gain in real time and measure noise according to first FUZZY ALGORITHMS FOR CONTROL; Said first FUZZY ALGORITHMS FOR CONTROL is input as said Global Positioning System (GPS) and the alternate position spike of said Inertial Measurement Unit acquisition and the position dilution of precision of said Global Positioning System (GPS), is output as filter gain and the adjustment matrix of coefficients of measuring noise;

According to second FUZZY ALGORITHMS FOR CONTROL process noise is adjusted; It is vectorial that said second FUZZY ALGORITHMS FOR CONTROL is input as the new breath that lateral deviation that said farm machinery departs from predetermined driving trace and the new breath value of course angle form, and is output as the adjustment matrix of coefficients of process noise.

The present invention also provides a kind of agricultural machinery industrial personal computer, comprising:

Receiver module is used for receiving respectively the current locating information of farm machinery that Global Positioning System (GPS), Inertial Measurement Unit and electronic compass get access to;

Modular converter is used for converting said locating information under the same coordinate system locating information;

Computing module; Be used to adopt the Kalman filtering algorithm that the locating information under the said the same coordinate system is carried out data fusion under said coordinate system; And adopt Double Fuzzy control adaptive algorithm that said Kalman filtering algorithm is adjusted in real time, obtain said farm machinery current position information;

Wherein, said employing Double Fuzzy control adaptive algorithm is adjusted in real time said Kalman filtering algorithm and is specially:

Adjust filter gain in real time and measure noise according to first FUZZY ALGORITHMS FOR CONTROL; Said first FUZZY ALGORITHMS FOR CONTROL is input as said Global Positioning System (GPS) and the alternate position spike of said Inertial Measurement Unit acquisition and the position dilution of precision of said Global Positioning System (GPS), is output as filter gain and the adjustment matrix of coefficients of measuring noise;

According to second FUZZY ALGORITHMS FOR CONTROL process noise is adjusted; It is vectorial that said second FUZZY ALGORITHMS FOR CONTROL is input as the new breath that lateral deviation that said farm machinery departs from predetermined driving trace and the new breath value of course angle form, and is output as the adjustment matrix of coefficients of process noise.

Description of drawings

The present invention provides a kind of farm machinery navigation positioning system again, comprising:

Global Positioning System (GPS) is used to obtain current longitude of farm machinery and latitude information;

Inertial Measurement Unit is used to obtain current angular velocity of said farm machinery and acceleration information;

Electronic compass is used to obtain the current course angle information of said farm machinery;

Industrial computer; Be used for receiving respectively the current locating information of farm machinery that said Global Positioning System (GPS), said Inertial Measurement Unit and said electronic compass get access to; Convert said locating information under the same coordinate system locating information; And adopt the Kalman filtering algorithm that the locating information under the said the same coordinate system is carried out data fusion under said coordinate system; And adopt Double Fuzzy control adaptive algorithm that said Kalman filtering algorithm is adjusted in real time, obtain said farm machinery current position information;

Wherein, said employing Double Fuzzy control adaptive algorithm is adjusted in real time said Kalman filtering algorithm and is specially:

Adjust filter gain in real time and measure noise according to first FUZZY ALGORITHMS FOR CONTROL; Said first FUZZY ALGORITHMS FOR CONTROL is input as said Global Positioning System (GPS) and the alternate position spike of said Inertial Measurement Unit acquisition and the position dilution of precision of said Global Positioning System (GPS), is output as filter gain and the adjustment matrix of coefficients of measuring noise;

According to second FUZZY ALGORITHMS FOR CONTROL process noise is adjusted; It is vectorial that said second FUZZY ALGORITHMS FOR CONTROL is input as the new breath that lateral deviation that said farm machinery departs from predetermined driving trace and the new breath value of course angle form, and is output as the adjustment matrix of coefficients of process noise.

Therefore; Agricultural machinery navigation and position method provided by the invention, system and agricultural machinery industrial personal computer receive the locating information that Global Positioning System (GPS), Inertial Measurement Unit and electronic compass obtain respectively, adopt the Kalman filtering algorithm that these locating information are carried out data fusion; And adopt Double Fuzzy control adaptive algorithm to adjust the convergence strategy of Kalman filtering algorithm in real time; Draw the positional information of current farm machinery, be applicable to the navigator fix of the farm machinery of farm work complicated and changeable, overcome prior art single-sensor location and the low defective of combined method bearing accuracy; Realize coordinating each other between each sensor; Remedy the weak point of single-sensor and combined method, enlarged the scope of application of positioning system, had bearing accuracy height, stable high advantage.

Fig. 1 is the agricultural machinery navigation and position method process flow diagram of first embodiment provided by the invention;

Fig. 2 is the agricultural machinery navigation and position method process flow diagram of second embodiment provided by the invention;

Embodiment

The synoptic diagram that locating information is converted to the same coordinate system that Fig. 3 provides for the embodiment of the invention;

The vehicle two-wheeled kinematics model synoptic diagram that Fig. 4 provides for the embodiment of the invention;

The Kalman filtering algorithm calculation flow chart that Fig. 5 provides for the embodiment of the invention;

Fig. 6 A is the first FUZZY ALGORITHMS FOR CONTROL synoptic diagram that the embodiment of the invention provides;

Fig. 6 B is the second FUZZY ALGORITHMS FOR CONTROL synoptic diagram that the embodiment of the invention provides;

The first FUZZY ALGORITHMS FOR CONTROL input and output subordinate function synoptic diagram that Fig. 7 provides for the embodiment of the invention;

The second FUZZY ALGORITHMS FOR CONTROL input and output subordinate function synoptic diagram that Fig. 8 provides for the embodiment of the invention;

Fig. 9 is the agricultural machinery industrial personal computer structural representation of the 3rd embodiment provided by the invention;

Figure 10 is the 4th an embodiment farm machinery navigation positioning system structural representation provided by the invention.

Through accompanying drawing and embodiment, technical scheme of the present invention is done further detailed description below.

Fig. 1 is the agricultural machinery navigation and position method process flow diagram of first embodiment provided by the invention, and is as shown in Figure 1, and this method comprises:

S101, receive the current locating information of farm machinery that GPS, IMU and electronic compass get access to respectively;

S102, convert locating information under the same coordinate system locating information;

S103, employing Kalman filtering algorithm carry out data fusion to the locating information under the same coordinate system under this coordinate system, and adopt Double Fuzzy control adaptive algorithm that the Kalman filtering algorithm is adjusted in real time, obtain the farm machinery current position information.

Wherein, Obtain three sensor GPS, IMU and the electronic compasss current locating information of detected farm machinery separately respectively; These locating information are converted into the locating information in the same coordinate system; This same coordinate system can be the earth plane coordinate system etc., and the initial point of this coordinate system can be set at a known location point, and the x direction of principal axis of this coordinate system can be set to the travel direction of farm machinery.After the locating information that each sensor is obtained is transformed into the same coordinate system; These locating information are carried out data fusion under this coordinate system; Data fusion is carries out optimal treatment with the locating information of each sensor; Make between these locating information and coordinate mutually, remedy, so that the positional information after the fusion of gained is more near the current actual position of farm machinery.Blending algorithm adopts the Kalman filtering algorithm; This algorithm is an optimization recurrence Processing Algorithm; Be used for target detection and following the tracks of, can estimate, carrying out the search of target then within the specific limits next possible position constantly of target; To dwindle the deviation of target physical location, improve degree of accuracy.Adopt data fusion method finally can obtain the farm machinery current position information, this positional information possibly be information such as position coordinate value, speed or course angle.Simultaneously; Adopt Double Fuzzy control adaptive algorithm that the Kalman filtering algorithm is adjusted in real time; Double Fuzzy control can be adjusted the convergence strategy of Kalman filtering algorithm automatically according to the variation of measuring noise and observation noise; Promptly can carry out self-adaptation adjustment, change convergence strategy, the locator meams of various sensors can be replenished and proofread and correct each other with the conversion of the degree of reliability of each sensor according to the variation of environment.

The agricultural machinery navigation and position method that present embodiment provides; Be applicable to the navigator fix of the farm machinery of farm work complicated and changeable; Overcome prior art single-sensor and the low defective of combined method bearing accuracy, realized coordinating each other between each sensor, remedied the weak point of single-sensor and combined method; Enlarge the scope of application of positioning system, had bearing accuracy height, stable high advantage.

Fig. 2 is the agricultural machinery navigation and position method process flow diagram of second embodiment provided by the invention, and is as shown in Figure 2, and this method comprises:

Current longitude and the latitude information of farm machinery that S201, reception GPS get access to receives current angular velocity and the acceleration information of farm machinery that IMU gets access to, and receives the current course angle information of farm machinery that electronic compass gets access to;

S202, convert longitude and latitude information in the earth plane coordinate system coordinate figure, angle speed and acceleration carry out integration and obtain the current speed of farm machinery, course angle and location coordinate information;

Wherein, What GPS got access to is current longitude and the latitude information of farm machinery; Can adopt Gauss-Krieger projection mathematics model to convert this longitude and latitude information in the earth plane coordinate system coordinate figure then, this Gauss-Krieger projection mathematics model is:

$\left\{\begin{array}{c}{X}_{\mathrm{GPS}}=X+\frac{1}{2}N\·t\·{\cos }^{2}B\·l^2+\frac{1}{24}N\·t(5-t^2+9{\mathrm{\η}}^2+4{\mathrm{\η}}^4){\cos }^{4}B\·l^4\\ +\frac{1}{720}N\·t(61-58t^2+t^4+270{\mathrm{\η}}^2-330{\mathrm{\η}}^2{t}^2){\cos }^{6}B\·l^6\\ Y_{\mathrm{GPS}}=N\·\cos B\·l+\frac{1}{6}N(1-t^2+{\mathrm{\η}}^2){\cos }^{3}B\·l^3+\frac{1}{120}N(5-18t^2+t^4\\ +{14\mathrm{\η}}^2-58{\mathrm{\η}}^2{t}^2){\cos }^{5}B\·l^5\end{array}\right.$

In the formula, B is the geodetic latitude of tested farm machinery at the earth subpoint; L=L-L ₀, L is the geodetic longitude of subpoint, L ₀Be the meridianal geodetic longitude of earth axis; T=tanB; η=e ' cosB; $e^{\′2}=\frac{a^2-b^2}{b^2},$ E ' is second excentricity of earth ellipsoid; When X is l=0, the meridian arc length that begins to calculate from the equator, the general type of its computing formula is:

X＝a(1-e ²)(A ₀B+A ₂sin2B+A ₄sin4B+A ₆sin6B+A ₈sin8B)；

Wherein, $A_0=1+\frac{3}{4}{e}^2+\frac{45}{64}{e}^4+\frac{350}{512}{e}^6+\frac{11025}{16384}{e}^8;$

$A_2=-\frac{1}{2}(\frac{3}{4}{e}^2+\frac{60}{64}{e}^4+\frac{525}{512}{e}^6+\frac{17640}{16384}{e}^8);$

$A_4=+\frac{1}{4}(\frac{15}{64}{e}^4+\frac{210}{512}{e}^6+\frac{8820}{16384}{e}^8);$

$A_6=-\frac{1}{6}(\frac{35}{512}{e}^6+\frac{2520}{16384}{e}^8);$

$A_8=+\frac{1}{8}\left(\frac{315}{16384}{e}^8\right);$

In the formula, $e^2=\frac{a^2-b^2}{a^2},$ E is earth ellipsoid first excentricity.

IMU get access to for current angular velocity and the acceleration information of farm machinery, can angle speed and acceleration carry out dead reckoning (Dead Reckoning; Hereinafter to be referred as: DR), obtain the current course of farm machinery, speed and location coordinate information, this reckoning process is drawn by following formula:

$\left\{\begin{array}{c}{v}_{n+1}=v_n+a\·T\\ {\mathrm{\θ}}_{n+1}={\mathrm{\θ}}_n+T\·{\mathrm{\ω}}_n\\ x_{n+1}=x_n+T\·v_n\·\cos \mathrm{\θ}\\ y_{n+1}=y_n+T\·v_n\·\sin \mathrm{\θ}\end{array}\right.$

In the formula, (x _n, y _n) and v _nBe respectively the position coordinates and the speed of current time farm machinery, (x _N+1, y _N+1) and v _N+1Be respectively next position coordinates and speed of farm machinery constantly, the time interval of T for calculating, ω _nBe the course angle speed of current time farm machinery, θ _nAnd θ _N+1Be respectively the course angle of current time and next moment farm machinery.

S203, with the coordinate figure in the earth plane coordinate system, speed, course angle and position coordinates value information, the current course angle information translation of the farm machinery that electronic compass gets access to is the locating information under the same coordinate system;

The synoptic diagram that locating information is converted to the same coordinate system that Fig. 3 provides for the embodiment of the invention, as shown in Figure 3, the same coordinate system can be set to the earth plane coordinate system usually, and the initial point of the same coordinate system is a known point, and establishing its coordinate figure is (X ₀, Y ₀), the direction of its x axle is a farm machinery driving path direction, the coordinate figure of coordinate figure in the same coordinate system in the earth plane coordinate system that GPS obtains is:

$\left[\begin{array}{c}{X}_{\mathrm{GPS}}\\ Y_{\mathrm{GPS}}\end{array}\right]=\left[\begin{array}{cc}\cos {\mathrm{\&theta;}}_0& \sin {\mathrm{\&theta;}}_0\\ -\sin {\mathrm{\&theta;}}_0& \cos {\mathrm{\&theta;}}_0\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{GPS}}\\ Y_{\mathrm{GPS}}\end{array}\right]+\left[\begin{array}{c}{X}_0\\ {\mathrm{<figure-callout\; id="0"\; label="Y"\; filenames="H2009100807414E00031.png,H2009100807414E00052.png"\; state="\{\{state\}\}">Y</figure-callout>}}_0\end{array}\right]$

In the formula, x _GPSAnd y _GPSBe respectively the horizontal ordinate of farm machinery in the same coordinate system that GPS obtains, X _GPSAnd Y _GPSBe respectively the horizontal ordinate of farm machinery in the earth plane coordinate system that GPS obtains, θ ₀Be the angle between the same coordinate system and the earth plane coordinate system.

The speed that IMU obtains, course angle and position coordinates value information coordinate figure and the course angle in the same coordinate system is:

$\left\{\begin{array}{c}{x}_{n+1}=x_n+T\&CenterDot;v_n\&CenterDot;\cos \mathrm{\&theta;}\\ y_{n+1}=y_n+T\&CenterDot;v_n\&CenterDot;\sin \mathrm{\&theta;}\end{array}\right.$

In the formula, (x _n, y _n) be respectively the position coordinates of current time farm machinery in the same coordinate system, v _nBe the current speed of farm machinery that the DR method is obtained, (x _N+1, y _N+1) be respectively next farm machinery position coordinates in the same coordinate system constantly, v _N+1Next speed constantly of farm machinery, in the time interval of T for calculating, θ is the course angle of farm machinery.

The course angle that electronic compass obtains is expressed as in the same coordinate system:

θ _c＝θ _compass-θ ₀

In the formula, θ _cBe the course angle of farm machinery in the same coordinate system, θ _CompassThe course angle information of the farm machinery that obtains for electronic compass, θ ₀Be the angle between the same coordinate system and the earth plane coordinate system.

S204, the state vector of getting system are X=[x, y, θ, α, v] ^T, measuring vector is Z=[x _DR, y _DR, x _GPS, y _GPS, θ _DR, θ _c, α] ^T

X wherein; Y is the position coordinates after the data fusion; θ is the course angle after the data fusion; α is the corner of the deflecting roller of the farm machinery that obtains in advance, and this corner information can be obtained before data fusion or after the data fusion, for example: can obtain corner information through the front wheel angle sensor.V is the travel speed of farm machinery, x _DR, y _DRAnd θ _DRBe respectively angle speed and acceleration and carry out current position coordinates and the course angle of farm machinery that integration obtains, x _GPS, y _GPSBe the coordinate figure in the earth plane coordinate system, θ _cThe current course angle information of farm machinery that gets access to for electronic compass;

S205, set up state equation and measurement equation according to vehicle two-wheeled kinematics model, state vector and measurement vector;

S206, draw the Kalman filtering equations according to state equation and measurement equation;

S207, obtain the farm machinery current position information according to the Kalman filtering equations, this positional information is position, speed and course information.

S204～S207 has described the locating information that GPS, IMU and electronic compass are got access to current farm machinery and has adopted the Kalman filtering algorithm to carry out the process of data fusion, and this process is specially:

At first according to state vector that is converted to the locating information selecting system under the same coordinate system and measurement vector; Suppose that farm machinery goes in the plane; And do not consider the effect on farm machinery wheel and ground, think farm machinery left-right symmetric and do not have situation such as side hectare, pitching, sideslip, so can its motion think a kind of plane rigid body translation and turn campaign two take turns model; The vehicle two-wheeled kinematics model synoptic diagram that Fig. 4 provides for the embodiment of the invention; As shown in Figure 4, wherein, (x ₁, y ₁) be the corresponding coordinate figure of this farm machinery front-wheel, (x ₂, y ₂) be the corresponding coordinate figure of this farm machinery trailing wheel, this motion model is:

In the formula, ψ is a course angle, and δ is a front wheel steering angle, and l is a wheelbase, and v is the speed of a motor vehicle.

The state equation that can draw system according to state vector and motion model is:

$\left[\begin{array}{c}{x}^{\&prime;}\\ y^{\&prime;}\\ {\mathrm{\&theta;}}^{\&prime;}\\ {\mathrm{\&alpha;}}^{\&prime;}\\ v^{\&prime;}\end{array}\right]=\left[\begin{array}{c}v\&CenterDot;\cos \mathrm{\&theta;}\\ v\&CenterDot;\sin \mathrm{\&theta;}\\ \frac{v}{l}\&CenterDot;\tan \mathrm{\&theta;}\\ -\frac{1}{{\mathrm{\&tau;}}_{\mathrm{\&alpha;}}}\&CenterDot;\mathrm{\&alpha;}\\ -\frac{1}{{\mathrm{\&tau;}}_v}\&CenterDot;v\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ w_{\mathrm{\&alpha;}}\\ w_v\end{array}\right]$

In the formula, τ _aAnd τ _vBe time constant, w _αAnd w _vBe respectively the noise of farm machinery front wheel steering angle and speed.

With the state equation discretize, the state equation that the system that obtains disperses is:

X _k＝f[X _k-1，k-1]+W _k-1

According to the Kalman filtering principle, have:

X _k＝Φ _k，k-1·X _k-1+W _k-1

In the formula,

${\mathrm{\&Phi;}}_{k,k-1}=\left[\begin{array}{ccccc}1& 0& -T\&CenterDot;v_k\&CenterDot;{\sin \mathrm{\&theta;}}_k& 0& T\&CenterDot;{\cos \mathrm{\&theta;}}_{\mathrm{<figure-callout\; id="0"\; label="k"\; filenames="H2009100807414E00031.png,H2009100807414E00052.png"\; state="\{\{state\}\}">k</figure-callout>}}\\ 0& 1& T\&CenterDot;v_k\&CenterDot;{\cos \mathrm{\&theta;}}_k& 0& T\&CenterDot;{\sin \mathrm{\&theta;}}_{\mathrm{<figure-callout\; id="0"\; label="k"\; filenames="H2009100807414E00031.png,H2009100807414E00052.png"\; state="\{\{state\}\}">k</figure-callout>}}\\ 0& 0& 1& \frac{T\&CenterDot;v_k}{l}\&CenterDot;{\sec }^2{\mathrm{\&alpha;}}_k& \frac{T}{l}\&CenterDot;\tan {\mathrm{\&theta;}}_{\mathrm{<figure-callout\; id="0"\; label="k"\; filenames="H2009100807414E00031.png,H2009100807414E00052.png"\; state="\{\{state\}\}">k</figure-callout>}}\\ 0& 0& 0& 1-\frac{T}{{\mathrm{\&tau;}}_{\mathrm{\&alpha;}}}& 0\\ 0& 0& 0& 0& 1-\frac{T}{{\mathrm{\&tau;}}_v}\end{array}\right]$

Wherein, T is the recurrence time interval of wave filter.

Measuring vector is Z=[x _DR, y _DR, x _GPS, y _GPS, θ _DR, θ _c, α] ^T

Wherein, x, y are the position coordinates after the data fusion, and θ is the course angle after the data fusion, and α is the corner of the deflecting roller of the farm machinery that obtains in advance, and v is the travel speed of farm machinery.

Then measurement equation is: Z _k=HX _k+ X _k

In the formula,

$H=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\end{array}\right]$ $V_k=\left[\begin{array}{c}{v}_{\mathrm{DR}}\\ v_{\mathrm{DR}}\\ v_{\mathrm{GPS}}\\ v_{\mathrm{GPS}}\\ v_{\mathrm{DR}}\\ v_C\\ v_{\mathrm{\&alpha;}}\end{array}\right]$

Wherein, v is the observation noise of sensor, be approximately (0, σ ²) white Gaussian noise;

Draw thus, the Kalman filtering equations is:

State one-step prediction equation: ${\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}$

One-step prediction error variance battle array: $P_{k/k-1}={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+Q_{k-1}$

The filter gain matrix: $K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$

The state estimation equation: ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k{\hat{Z}}_k$

Estimation error variance battle array: P _k=[I-K _kH _k] P _{K, k-1}

Can draw position coordinates, speed and the course angle information of current farm machinery according to the Kalman filtering equations of gained, the Kalman filtering algorithm calculation flow chart that Fig. 5 provides for the embodiment of the invention is referring to Fig. 5.

S208, the Kalman filtering algorithm is carried out the Real-Time Filtering adjustment according to Double Fuzzy control adaptive algorithm.

Fig. 6 A is the first FUZZY ALGORITHMS FOR CONTROL synoptic diagram that the embodiment of the invention provides; Fig. 6 B is the second FUZZY ALGORITHMS FOR CONTROL synoptic diagram that the embodiment of the invention provides; Shown in Fig. 6 A and Fig. 6 B; Adjust filter gain in real time and measure noise according to first FUZZY ALGORITHMS FOR CONTROL (Fuzzy Control-1), first FUZZY ALGORITHMS FOR CONTROL is input as GPS and the alternate position spike Δ y (Position) of IMU acquisition and the position dilution of precision (PDOP) of GPS, is output as the weights μ that GPS and IMU signal quality are judged _GPSAnd μ _IMU, these two weights are formed Kalman filtering algorithm filter gain and the adjustment matrix of coefficients μ that measures noise _r

(Fuzzy Control-2) adjusts process noise according to second FUZZY ALGORITHMS FOR CONTROL; Second FUZZY ALGORITHMS FOR CONTROL is input as farm machinery and departs from the lateral deviation of predetermined driving trace and the new breath value Z (x) of course angle; Z (y) and Z (θ), they form new breath vector Z _k, be output as process noise adjustment coefficient Q (x), Q (y) and Q (θ), process noise adjustment coefficient is formed the adjustment matrix of coefficients μ of process noise _q

Because conventional Kalman filtering algorithm is easy to generate the shortcoming of filtering divergence, adopt Double Fuzzy control adaptive algorithm difference system noise and the observation noise and the filter gain of adjustment Kalman filtering algorithm in real time, remedied this defective.

Detailed process is: adopt the gain matrix K of fuzzy logic control to Kalman filtering, measuring error covariance R and observational error covariance Q revise in real time, and the Kalman filtering algorithm is adjusted to optimum state.Confirm the adjustment matrix of coefficients μ of filter gain, measurement noise respectively through two FUZZY ALGORITHMS FOR CONTROL _rAdjustment matrix of coefficients μ with system noise _q

Set up K _kAnd R (t) coefficient fuzzy control:

Filter gain K _kWith measure noise R (t) and all receive the influence of sensor signal quality, have certain fuzzy relation between the position dilution of precision of their same GPS, position difference DELTA y that two kinds of locator meamss of dead reckoning obtain and GPS.This fuzzy controller be input as alternate position spike Δ y that two kinds of locator meamss of GPS, dead reckoning obtain and the position dilution of precision of GPS, be output as the weights μ of GPS and IMU signal quality judgement _GPSAnd μ _IMU, then filter gain, measure the adjustment matrix of coefficients μ of noise _rFor:

${\mathrm{\&mu;}}_r=\left[\begin{array}{ccccccc}{\mathrm{\&mu;}}_{\mathrm{DR}}& & & & & & \\ & {\mathrm{\&mu;}}_{\mathrm{DR}}& & & & & \\ & & {\mathrm{\&mu;}}_{\mathrm{GPS}}& & & & \\ & & & {\mathrm{\&mu;}}_{\mathrm{GPS}}& & & \\ & & & & {\mathrm{\&mu;}}_{\mathrm{<figure-callout\; id="1"\; label="DR"\; filenames="H2009100807414E00031.png,H2009100807414E00041.png"\; state="\{\{state\}\}">DR</figure-callout>}}& & \\ & & & & & 1& \\ & & & & & & 1\end{array}\right]$

Wherein, the first FUZZY ALGORITHMS FOR CONTROL input and output subordinate function synoptic diagram that Fig. 7 provides for the embodiment of the invention is referring to Fig. 7.

Foundation is based on new breath Z _kQ (t) the coefficient fuzzy control that changes:

Q (t) is the variance intensity battle array of the process noise of system, receives the systematic procedure The noise, and this value is difficult for directly recording, often through the system noise of each prediction signal is estimated to obtain.And this estimation is to be based upon on the basis that new breath is the zero-mean white noise.Because new breath is the poor of observed reading and predicted value, so when filtering algorithm was in optimum state, the value of new breath was the white noise of one group of zero-mean.And in the farm work process; Owing to receive the influence of various interference, the value of new breath can be mingled with coloured noise in various degree, and the value of new breath is big more; Observed reading and predicted value difference that system's this moment is described are big more; The process noise that is system is big more, causes the value of Q (t) also big more, so the value of Q (t) and new breath Z _kSize have certain fuzzy relation.The fuzzy control that foundation changes based on new breath is carried out the real-time online adjustment to the value of Q (t), makes the Kalman filtering algorithm be in optimum state all the time.The new breath value Z (x) that is input as lateral deviation and course angle of this fuzzy control, Z (y) and Z (θ) are output as process noise adjustment coefficient Q (x), Q (y) and Q (θ).The adjustment matrix of coefficients μ of process noise Q (t) then _qFor:

${\mathrm{\&mu;}}_q=\left[\begin{array}{ccccc}Q\left(x\right)& & & & \\ & Q\left(y\right)& & & \\ & & Q\left(\mathrm{\&theta;}\right)& & \\ & & & 1& \\ & & & & 1\end{array}\right]$

Wherein, the second FUZZY ALGORITHMS FOR CONTROL input and output subordinate function synoptic diagram that Fig. 8 provides for the embodiment of the invention is referring to Fig. 8.

The farm machinery navigation and positioning algorithm that present embodiment provides; Adopt the Kalman filtering algorithm that the locating information that various sensors obtain is carried out data fusion; Obtain more position, speed and course angle information, simultaneously, adopt Double Fuzzy control adaptive algorithm to adjust observation noise in the Kalman filtering algorithm in real time, measure noise and filter gain near the current actual position of farm machinery; At any time adjust the filtering strategy of Kalman filtering algorithm; Make the positional information degree of accuracy of gained higher, and enlarged the scope of application of system, improved the stability of system.

Fig. 9 is the agricultural machinery industrial personal computer structural representation of the 3rd embodiment provided by the invention, and is as shown in Figure 9, and this industrial computer comprises: receiver module 31, modular converter 32 and computing module 33; Wherein, receiver module 31 is used for receiving respectively the current locating information of farm machinery that GPS, IMU and electronic compass get access to; Modular converter 32 is used for converting locating information under the same coordinate system locating information; Computing module 33 is used to adopt the Kalman filtering algorithm that the locating information under the same coordinate system is carried out data fusion under this coordinate system; And adopt Double Fuzzy control adaptive algorithm that the Kalman filtering algorithm is adjusted in real time, obtain the farm machinery current position information.

Wherein, receiver module 31 obtains three sensor GPS, IMU and the electronic compasss current locating information of detected farm machinery separately respectively, converts these locating information in the same coordinate system locating information through modular converter 32.The locating information unification that each sensor is obtained is behind the same coordinate system; Under the same coordinate system; Through computing module 33 these locating information are carried out data fusion; Data fusion is carries out optimal treatment with the locating information of each sensor, makes between these locating information and coordinates mutually, remedies, so that the locating information after the fusion of gained is more near the current actual position of farm machinery.This blending algorithm adopts the Kalman filtering algorithm, and adopts Double Fuzzy control adaptive algorithm that the Kalman filtering algorithm is adjusted in real time.Finally can obtain the farm machinery current position information, this positional information is the information such as coordinate figure, speed and course angle of position.

The agricultural machinery industrial personal computer that the embodiment of the invention provides; Be applicable to the navigator fix of the farm machinery of farm work; Overcome prior art single-sensor location and the low defective of integrated positioning precision, realized coordinating each other between each sensor, remedied the weak point of single-sensor; Enlarge the scope of application of positioning system, had bearing accuracy height, stable high advantage.

On the basis of the 3rd embodiment, the computing module 33 in this agricultural machinery industrial personal computer can also specifically comprise Kalman wave filter and Double Fuzzy Controller; Wherein, the Kalman wave filter is used for according to the Kalman filtering algorithm locating information under the same coordinate system being carried out data fusion under this coordinate system, obtains the farm machinery current position information; Double Fuzzy Controller is used for according to Double Fuzzy control adaptive algorithm Kalman filter filtering algorithm being adjusted in real time.

Adopt the Kalman wave filter to carry out data fusion; Adopt Double Fuzzy Controller to adjust observation noise, measurement noise and filter gain in the Kalman wave filter in real time simultaneously; At any time adjust the filtering strategy of Kalman wave filter, referring to Fig. 6 A and Fig. 6 B, corresponding two fuzzy controller Fuzzy Control-1 of Double Fuzzy control algolithm and Fuzzy Control-2; Specifically, repeat no more referring to second embodiment.

Adopt Double Fuzzy Controller to adjust observation noise, measurement noise and filter gain in the Kalman wave filter in real time; At any time adjust the filtering strategy of Kalman wave filter; Make the positional information degree of accuracy of gained higher, and enlarged the scope of application of system, improved the stability of system.

Figure 10 is the 4th an embodiment farm machinery navigation positioning system structural representation provided by the invention, and shown in figure 10, this system comprises: GPS41, IMU42, electronic compass 43 and industrial computer 44; Wherein, GPS41 is used to obtain current longitude of farm machinery and latitude information; IMU42 is used to obtain current angular velocity of farm machinery and acceleration information; Electronic compass 43 is used to obtain the current course angle information of farm machinery; Industrial computer 44 is used for receiving respectively the current locating information of farm machinery that GPS, IMU and electronic compass get access to; Convert locating information under the same coordinate system locating information; Adopt the Kalman filtering algorithm that the locating information under the same coordinate system is carried out data fusion under this coordinate system; And adopt Double Fuzzy control adaptive algorithm that the Kalman filtering algorithm is adjusted in real time, obtain the farm machinery current position information.

Wherein, GPS41, IMU42, electronic compass 43 are connected with industrial computer 44 respectively; Industrial computer 44 will merge from the laggard line data of the current locating information of the various farm machineries that GPS41, IMU42, electronic compass 43 receive; Get access to the positional information that more approaches the current actual position of farm machinery, concrete data fusion process is referring to previous embodiment.

The farm machinery navigation positioning system that the embodiment of the invention provides; Be applicable to the navigator fix of the farm machinery of farm work; Overcome prior art single-sensor location and the low defective of integrated positioning precision, realized coordinating each other between each sensor, remedied the weak point of single-sensor; Enlarge the scope of application of positioning system, had bearing accuracy height, stable high advantage.

What should explain at last is: above embodiment is only in order to explaining technical scheme of the present invention, but not to its restriction; Although with reference to previous embodiment the present invention has been carried out detailed explanation, those of ordinary skill in the art is to be understood that: it still can be made amendment to the technical scheme that aforementioned each embodiment put down in writing, and perhaps part technical characterictic wherein is equal to replacement; And these are revised or replacement, do not make the spirit and the scope of the essence disengaging various embodiments of the present invention technical scheme of relevant art scheme.

Claims (10)

1. an agricultural machinery navigation and position method is characterized in that, comprising:

Receive the current locating information of farm machinery that Global Positioning System (GPS), Inertial Measurement Unit and electronic compass get access to respectively;

Convert said locating information under the same coordinate system locating information;

Adopt the Kalman filtering algorithm that the locating information under the said the same coordinate system is carried out data fusion under said coordinate system; And adopt Double Fuzzy control adaptive algorithm that said Kalman filtering algorithm is adjusted in real time, obtain said farm machinery current position information;

Wherein, said employing Double Fuzzy control adaptive algorithm is adjusted in real time said Kalman filtering algorithm and is specially:

Adjust filter gain in real time and measure noise according to first FUZZY ALGORITHMS FOR CONTROL; Said first FUZZY ALGORITHMS FOR CONTROL is input as said Global Positioning System (GPS) and the alternate position spike of said Inertial Measurement Unit acquisition and the position dilution of precision of said Global Positioning System (GPS), is output as filter gain and the adjustment matrix of coefficients of measuring noise;

According to second FUZZY ALGORITHMS FOR CONTROL process noise is adjusted; It is vectorial that said second FUZZY ALGORITHMS FOR CONTROL is input as the new breath that lateral deviation that said farm machinery departs from predetermined driving trace and the new breath value of course angle form, and is output as the adjustment matrix of coefficients of process noise.

2. agricultural machinery navigation and position method according to claim 1 is characterized in that, saidly receives the current locating information of farm machinery that Global Positioning System (GPS), Inertial Measurement Unit and electronic compass get access to respectively and is specially:

Receive current longitude and the latitude information of said farm machinery that said Global Positioning System (GPS) gets access to; Receive current angular velocity and the acceleration information of said farm machinery that said Inertial Measurement Unit gets access to, and receive the current course angle information of said farm machinery that said electronic compass gets access to.

3. agricultural machinery navigation and position method according to claim 2 is characterized in that, the said locating information that said locating information is converted under the same coordinate system comprises:

Convert said longitude and latitude information in the earth plane coordinate system coordinate figure, said angular velocity and acceleration information are carried out integration obtain the current speed of said farm machinery, course angle and location coordinate information;

With the coordinate figure in the said the earth plane coordinate system; Said speed, course angle and position coordinates value information that said farm machinery is current, the current course angle information translation of the said farm machinery that said electronic compass gets access to is the locating information under the same coordinate system.

4. agricultural machinery navigation and position method according to claim 3 is characterized in that, said employing Kalman filtering algorithm carries out data fusion to the locating information under the said the same coordinate system and comprises under said coordinate system:

The state vector of getting system is X=[x, y, θ, α, v] ^T, measuring vector is Z=[x _DR, y _DR, x _GPS, y _GPS, θ _DR, θ _c, α] ^TX wherein, y is the position coordinates after the said data fusion, and θ is the course angle after the said data fusion, and α is the corner of the deflecting roller of the said farm machinery that obtains in advance, and v is the travel speed of said farm machinery, x _DR, y _DRAnd θ _DRBe respectively said angular velocity and acceleration are carried out current position coordinates and the course angle of said farm machinery that integration obtains, x _GPS, y _GPSBe the coordinate figure in the said the earth plane coordinate system, θ _cThe current course angle information of farm machinery that gets access to for said electronic compass;

Set up state equation and measurement equation according to vehicle two-wheeled kinematics model, said state vector and said measurement vector;

Draw the Kalman filtering equations according to said state equation and said measurement equation;

Obtain said farm machinery current position information according to said Kalman filtering equations, said positional information is position, speed and course information.

5. agricultural machinery navigation and position method according to claim 3 is characterized in that, the said coordinate figure that said longitude and latitude information are converted in the earth plane coordinate system adopts Gauss-Krieger projection mathematics model.

6. agricultural machinery navigation and position method according to claim 3 is characterized in that, saidly said angular velocity and acceleration are carried out integration obtains the current speed of said farm machinery, course angle and location coordinate information and adopts dead reckoning.

7. an agricultural machinery industrial personal computer is characterized in that, comprising:

Receiver module is used for receiving respectively the current locating information of farm machinery that Global Positioning System (GPS), Inertial Measurement Unit and electronic compass get access to;

Modular converter is used for converting said locating information under the same coordinate system locating information;

Computing module; Be used to adopt the Kalman filtering algorithm that the locating information under the said the same coordinate system is carried out data fusion under said coordinate system; And adopt Double Fuzzy control adaptive algorithm that said Kalman filtering algorithm is adjusted in real time, obtain said farm machinery current position information;

Wherein, said employing Double Fuzzy control adaptive algorithm is adjusted in real time said Kalman filtering algorithm and is specially:

Adjust filter gain in real time and measure noise according to first FUZZY ALGORITHMS FOR CONTROL; Said first FUZZY ALGORITHMS FOR CONTROL is input as said Global Positioning System (GPS) and the alternate position spike of said Inertial Measurement Unit acquisition and the position dilution of precision of said Global Positioning System (GPS), is output as filter gain and the adjustment matrix of coefficients of measuring noise;

According to second FUZZY ALGORITHMS FOR CONTROL process noise is adjusted; It is vectorial that said second FUZZY ALGORITHMS FOR CONTROL is input as the new breath that lateral deviation that said farm machinery departs from predetermined driving trace and the new breath value of course angle form, and is output as the adjustment matrix of coefficients of process noise.

8. agricultural machinery industrial personal computer according to claim 7 is characterized in that, said computing module comprises:

The Kalman wave filter is used for according to said Kalman filtering algorithm the locating information under the said the same coordinate system being carried out data fusion under said coordinate system, obtains said farm machinery current position information;

Double Fuzzy Controller is used for according to Double Fuzzy control adaptive algorithm said Kalman filter filtering algorithm being adjusted in real time.

9. a farm machinery navigation positioning system is characterized in that, comprising:

Global Positioning System (GPS) is used to obtain current longitude of farm machinery and latitude information;

Inertial Measurement Unit is used to obtain current angular velocity of said farm machinery and acceleration information;

Electronic compass is used to obtain the current course angle information of said farm machinery;

Industrial computer; Be used for receiving respectively the current locating information of said farm machinery that said Global Positioning System (GPS), said Inertial Measurement Unit and said electronic compass get access to; Convert said locating information under the same coordinate system locating information; And adopt the Kalman filtering algorithm that the locating information under the said the same coordinate system is carried out data fusion under said coordinate system; And adopt Double Fuzzy control adaptive algorithm that said Kalman filtering algorithm is adjusted in real time, obtain said farm machinery current position information;

Wherein, said employing Double Fuzzy control adaptive algorithm is adjusted in real time said Kalman filtering algorithm and is specially:

Adjust filter gain in real time and measure noise according to first FUZZY ALGORITHMS FOR CONTROL; Said first FUZZY ALGORITHMS FOR CONTROL is input as said Global Positioning System (GPS) and the alternate position spike of said Inertial Measurement Unit acquisition and the position dilution of precision of said Global Positioning System (GPS), is output as filter gain and the adjustment matrix of coefficients of measuring noise;

According to second FUZZY ALGORITHMS FOR CONTROL process noise is adjusted; It is vectorial that said second FUZZY ALGORITHMS FOR CONTROL is input as the new breath that lateral deviation that said farm machinery departs from predetermined driving trace and the new breath value of course angle form, and is output as the adjustment matrix of coefficients of process noise.

10. farm machinery navigation positioning system according to claim 9 is characterized in that, also comprises: interface module is used for respectively said Global Positioning System (GPS), said Inertial Measurement Unit and said electronic compass being connected with said industrial computer.

Images