CN101846734A - 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 in conjunction with 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 PositionSystems; Hereinafter to be referred as: GPS), the GPS location technology adopts mutual positioning principle, by 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 height, and be not subjected to 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; the excessive locating information that causes of the as easy as rolling off a log GPS of causing positioning error lost efficacy, thereby influenced 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.It is the precision height in short-term, is not subjected to the interference of external environment factor, can be widely used in the navigator fix of space flight, navigation and road vehicles.What inertial navigation location recorded is angular velocity signal and acceleration signal, needs that further angular velocity and acceleration are carried out integration and obtains the course heading signal, still, is easy to generate cumulative errors in integral process.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 is subjected to 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, supposing the system noise and measurement noise are zero-mean white noise series 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 complexity, 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 achieving the above object, 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;

Described locating information is converted to locating information under the same coordinate system;

Adopt the Kalman filtering algorithm that the locating information under the described the same coordinate system is carried out data fusion under described coordinate system, and adopt bimodulus to stick with paste the control adaptive algorithm described Kalman filtering algorithm is adjusted in real time, obtain described farm machinery current position information.

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 described locating information is converted to locating information under the same coordinate system;

Computing module, be used to adopt the Kalman filtering algorithm that the locating information under the described the same coordinate system is carried out data fusion under described coordinate system, and adopt bimodulus to stick with paste the control adaptive algorithm described Kalman filtering algorithm is adjusted in real time, obtain described farm machinery current position information.

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 described farm machinery and acceleration information;

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

Industrial computer, be used for receiving respectively the current locating information of farm machinery that described Global Positioning System (GPS), described Inertial Measurement Unit and described electronic compass get access to, described locating information is converted to locating information under the same coordinate system, and adopt the Kalman filtering algorithm that the locating information under the described the same coordinate system is carried out data fusion under described coordinate system, and adopt bimodulus to stick with paste the control adaptive algorithm described Kalman filtering algorithm is adjusted in real time, obtain described farm machinery current position information.

Therefore, agricultural machinery navigation and position method provided by the invention, system and agricultural machinery industrial personal computer, receive Global Positioning System (GPS) respectively, the locating information that Inertial Measurement Unit and electronic compass obtain, adopt the Kalman filtering algorithm that these locating information are carried out data fusion, and adopt bimodulus to stick with paste the convergence strategy that the control adaptive algorithm is adjusted the 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, prior art single-sensor location and the low defective of combined method bearing accuracy have been overcome, realize coordinating mutually between each sensor, remedied the weak point of single-sensor and combined method, enlarge the scope of application of positioning system, had the bearing accuracy height, the advantage that stability is high.

Description of drawings

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;

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;

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

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

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.

Embodiment

Below by drawings and Examples, technical scheme of the present invention is described in further detail.

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

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

S102, locating information is converted to locating information under the same coordinate system;

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

Wherein, obtain three sensor GPS, IMU and the electronic compass current locating information of detected farm machinery separately respectively, these locating information are converted into locating information in the same coordinate system, this the same coordinate system can be the earth plane coordinate system etc., 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 next possible position constantly of target, carry out the search of target then within the specific limits, 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 may be information such as position coordinate value, speed or course angle.Simultaneously, adopting bimodulus to stick with paste the control adaptive algorithm adjusts in real time to the Kalman filtering algorithm, bimodulus is stuck with paste the convergence strategy that control can be adjusted the Kalman filtering algorithm automatically according to the variation of measuring noise and observation noise, promptly can carry out the self-adaptation adjustment according to the variation of environment, conversion with the degree of reliability of each sensor changes convergence strategy, and the locator meams of various sensors can be replenished and proofread and correct mutually.

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, prior art single-sensor and the low defective of combined method bearing accuracy have been overcome, realize coordinating mutually 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 as shown in Figure 2, 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, longitude and latitude information are converted to coordinate figure in the earth plane coordinate system, angular velocity and acceleration are carried out integration 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 that this longitude and latitude information are converted to coordinate figure in the earth plane coordinate system 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 ' 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 ₆sin?6B+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 is earth ellipsoid first excentricity.

What IMU got access to is current angular velocity and the acceleration information of farm machinery, can carry out dead reckoning (Dead Reckoning to angular velocity and acceleration; 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{\θ}}_0& {\sin \mathrm{\θ}}_0\\ -{\sin \mathrm{\θ}}_0& \cos {\mathrm{\θ}}_0\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{GPS}}\\ Y_{\mathrm{GPS}}\end{array}\right]+\left[\begin{array}{c}{X}_0\\ Y_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\·v_n\·\cos \mathrm{\θ}\\ y_{n+1}=y_n+T\·v_n\·\sin \mathrm{\θ}\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, this corner information can be obtained before data fusion or after the data fusion, for example: can obtain corner information by the front wheel angle sensor.V is the travel speed of farm machinery, x _DR, y _DRAnd θ _DRBe respectively angular velocity and acceleration are carried 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 travels 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 situations such as side hectare, pitching, sideslip, can think that two of a kind of translation of rigid body of plane and turn campaign take turns model to its motion so, 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 coordinate figure of this farm machinery front-wheel correspondence, (x ₂, y ₂) be the coordinate figure of this farm machinery trailing wheel correspondence, 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}^{\′}\\ y^{\′}\\ {\mathrm{\θ}}^{\′}\\ {\mathrm{\α}}^{\′}\\ v^{\′}\end{array}\right]=\left[\begin{array}{c}v\·\cos \mathrm{\θ}\\ v\·\sin \mathrm{\θ}\\ \frac{v}{l}\·\tan \mathrm{\θ}\\ -\frac{1}{{\mathrm{\τ}}_{\mathrm{\α}}}\·\mathrm{\α}\\ -\frac{1}{{\mathrm{\τ}}_v}\·v\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ w_{\mathrm{\α}}\\ w_v\end{array}\right]$

In the formula, τ _αAnd τ _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="H2009100807414E0000031.png,H2009100807414E0000052.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="H2009100807414E0000031.png,H2009100807414E0000052.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="H2009100807414E0000031.png,H2009100807414E0000052.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+ V _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, stick with paste the control adaptive algorithm according to bimodulus the Kalman filtering algorithm is carried out the Real-Time Filtering adjustment.

The first FUZZY ALGORITHMS FOR CONTROL synoptic diagram that Fig. 6 A provides for the embodiment of the invention, the second FUZZY ALGORITHMS FOR CONTROL synoptic diagram that Fig. 6 B provides for the embodiment of the invention, 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 are measured the adjustment matrix of coefficients μ of 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 and adjust coefficient Q (x), Q (y) and Q (θ), process noise is adjusted the adjustment matrix of coefficients μ that coefficient is formed process noise _q

Because conventional Kalman filtering algorithm is easy to generate the shortcoming of filtering divergence, adopt bimodulus to stick with paste system noise and observation noise and filter gain that the control adaptive algorithm is adjusted the Kalman filtering algorithm respectively 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.Determine the adjustment matrix of coefficients μ of filter gain, measurement noise respectively by 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 be subjected to 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 μ that GPS and IMU signal quality are judged _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="H2009100807414E0000031.png,H2009100807414E0000041.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, is subjected to the systematic procedure The noise, and this value is difficult for directly recording, often by 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 be subjected to the influence of various interference, the value of new breath can be mingled with coloured noise in various degree, 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 there is certain fuzzy relation in the value of Q (t) with the new size that ceases Zk.Foundation is carried out the real-time online adjustment based on the fuzzy control that new breath changes 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 and adjust 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 near the current actual position of farm machinery, simultaneously, employing bimodulus paste control adaptive algorithm is adjusted observation noise, measurement noise and the filter gain in the Kalman filtering algorithm in real time, 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 as shown in Figure 9, 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 locating information is converted to locating information under the same coordinate system; 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 bimodulus to stick with paste the control adaptive algorithm 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 compass current locating information of detected farm machinery separately respectively, and these locating information are converted to locating information in the same coordinate system by modular converter 32.The locating information unification that each sensor is obtained is behind the same coordinate system, under the same coordinate system, by 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, make between these locating information and coordinate mutually, remedy, 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 bimodulus to stick with paste the control adaptive algorithm 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, prior art single-sensor location and the low defective of integrated positioning precision have been overcome, realize coordinating mutually 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 sticking with paste the control adaptive algorithm according to bimodulus Kalman filter filtering algorithm is adjusted in real time.

Adopt the Kalman wave filter to carry out data fusion, adopt Double Fuzzy Controller to adjust observation noise in the Kalman wave filter in real time simultaneously, measure noise and filter gain, 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 bimodulus fuzzy control algorithm and Fuzzy Control-2, specifically, repeat no more referring to second embodiment.

The employing Double Fuzzy Controller is adjusted observation noise, measurement noise and the 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 as 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, locating information is converted to locating information under the same coordinate system, 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 bimodulus to stick with paste the control adaptive algorithm 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, prior art single-sensor location and the low defective of integrated positioning precision have been overcome, realize coordinating mutually 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.

It should be noted that at last: above embodiment only in order to technical scheme of the present invention to be described, is not intended to limit; Although with reference to previous embodiment the present invention is had been described in detail, 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 modifications or replacement do not make the essence of appropriate technical solution break away from the spirit and scope of various embodiments of the present invention technical 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;

Described locating information is converted to locating information under the same coordinate system;

Adopt the Kalman filtering algorithm that the locating information under the described the same coordinate system is carried out data fusion under described coordinate system, and adopt bimodulus to stick with paste the control adaptive algorithm described Kalman filtering algorithm is adjusted in real time, obtain described farm machinery current position information.

2. agricultural machinery navigation and position method according to claim 1 is characterized in that, describedly 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 described farm machinery that described Global Positioning System (GPS) gets access to, receive current angular velocity and the acceleration information of described farm machinery that described Inertial Measurement Unit gets access to, and receive the current course angle information of described farm machinery that described electronic compass gets access to.

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

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

With the coordinate figure in the described the earth plane coordinate system, the current course angle information translation of the described farm machinery that described speed, course angle and position coordinates value information, described electronic compass get 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, described employing Kalman filtering algorithm carries out data fusion to the locating information under the described the same coordinate system and comprises under described 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 described data fusion, and θ is the course angle after the described data fusion, and α is the corner of the deflecting roller of the described farm machinery that obtains in advance, and v is the travel speed of described farm machinery, x _DR, y _DRAnd θ _DRBe respectively described angular velocity and acceleration are carried out current position coordinates and the course angle of described farm machinery that integration obtains, x _GPS, y _GPSBe the coordinate figure in the described the earth plane coordinate system, θ _cThe current course angle information of farm machinery that gets access to for described electronic compass;

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

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

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

5. agricultural machinery navigation and position method according to claim 4 is characterized in that, described employing bimodulus paste control adaptive algorithm is adjusted in real time described Kalman filtering algorithm and is specially:

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

According to second FUZZY ALGORITHMS FOR CONTROL process noise is adjusted, it is vectorial that described second FUZZY ALGORITHMS FOR CONTROL is input as the new breath that lateral deviation that described 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.

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

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

8. 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 described locating information is converted to locating information under the same coordinate system;

Computing module, be used to adopt the Kalman filtering algorithm that the locating information under the described the same coordinate system is carried out data fusion under described coordinate system, and adopt bimodulus to stick with paste the control adaptive algorithm described Kalman filtering algorithm is adjusted in real time, obtain described farm machinery current position information.

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

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

Double Fuzzy Controller is used for sticking with paste the control adaptive algorithm according to bimodulus described Kalman filter filtering algorithm is adjusted in real time.

10. 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 described farm machinery and acceleration information;

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

Industrial computer, be used for receiving respectively the current locating information of described farm machinery that described Global Positioning System (GPS), described Inertial Measurement Unit and described electronic compass get access to, described locating information is converted to locating information under the same coordinate system, and adopt the Kalman filtering algorithm that the locating information under the described the same coordinate system is carried out data fusion under described coordinate system, and adopt bimodulus to stick with paste the control adaptive algorithm described Kalman filtering algorithm is adjusted in real time, obtain described farm machinery current position information.

Images