CN101221447A - Mechanical automatic steering control method - Google Patents

Abstract

The invention relates to a machinery automatic steering control method which comprises the following steps that: positional deviation and course deviation are determined; according to the variance in the practical front wheel rotating angle of an agricultural machine, online setting of PID parameter is completed by means of parameter self-setting PID control algorithm; moreover, the expected front wheel rotating angle of the next moment is calculated, thereby realizing automatic steering control of the agricultural machine. Based on conventional PID navigation control method, the invention makes full use of the fuzzy control method; according to the variance in the practical front wheel rotating angle of an agricultural machine, the invention meets the different requirements of the machine on PID control parameter under different errors and error change rate, thereby realizing online setting of PID parameter. The invention not only has the advantages of fuzzy control such as flexibility and adaptability, but also has the characteristics of higher precision of PID control; therefore, the invention can increase the stability and precision of agricultural machine automatic steering control and the robustness of a control system.

Description

A kind of mechanical automatic steering control method

Technical field

The present invention relates to agricultural machines navigation control field, be specifically related to the control method that a kind of machinery turns to automatically.

Background technology

The fundamental purpose of agricultural machines navigation control is the accurate relatively navigator fix result who obtains according to each sensor, determine that the agricultural machinery itself and the position in predefine path concern, motion state and corresponding control algolithm in conjunction with agricultural machinery, the suitable front wheel angle of making a strategic decision out, to revise the path trace error, the current course of agricultural machinery is overlapped fast with bogey heading.Navigation Control is divided into vertical control and laterally control, and wherein laterally control mainly refers to turn to control, vertically controls the adjusting of main finger speed.The agricultural machinery automatic steering control method is to realize that follow the tracks of in its accurate course and the basis of self-navigation control.

Navigation control method commonly used comprises linear model control method, method for optimally controlling and fuzzy control method.Has done the information fusion of the automatic control of farm machinery and multisensor and has goed deep into extensive studies U.S. University of Illinois, successfully developed the tractor that to realize multiple farming operation, and utilize electrohydraulic control system to carry out go to action, realized that farm tractor follows the unmanned of gap walking.Tokyo Univ Japan utilizes machine vision technique to carry out automated navigation system research, turns to controlling models according to linearity, and target direction angle and longitudinal direction of car angle are merged, and calculates the deflecting roller deflection angle, finishes the control of turning.People such as the Noguchi N of the U.S. use neural network and genetic algorithm, set up agri-vehicle control system with self-learning capability, experimental result shows that this model has excellent control effect to the vehicle that travels in flat road surface, but is not suitable for the situation that the road surface tilts.People's thereof using PID (proportional-integral-differential proportion-integral-derivative) control methods such as Benson, Dong ZL have designed the PID controller, this algorithm does not rely on precise math model, avoided tedious modeling course, only need the response characteristic of some objects to make up control, scale parameter, integral parameter and the differential parameter of algorithm are reasonably regulated.Experimental result shows that this method has good path trace effect.Domestic people such as Liu Zhou Junyu Cheng Liang are based on the thought of Kalman filtering, merged the observed reading of each sensor, provided the predicting tracing control method of agricultural robot navigation, avoided consuming time and cause feedback of status to lag behind and the adverse effect that produces, the robustness and the precision of Navigation Control had certain improvement based on the calculating of vision system.The integrated GPS technology of Agricultural University Of South China, computer technology and multi-sensor technology, having developed is the agricultural intelligent mobile operating platform of power with the motor, the experiment proved that the control difficulty of this platform path trace is bigger.

By analyzing as can be known, the emphasis of Navigation Control and difficult point are to improve stability and the path trace precision that turns to control.Conventional PID control method can obtain the path trace effect of degree of precision, and has certain robustness and reliability, but the antijamming capability of this method a little less than.

Fuzzy control method is the new type of control method that development in recent years is got up, its advantage is the mathematical models that does not need to be grasped controll plant, according to the manual control rule, and the size of organizational controls decision-making decision controlled quentity controlled variable, can obtain good dynamic perfromance, but its static characteristics is poor.Under the farm work condition, kinematics and the kinetic model of setting up agricultural machinery are relatively more difficult, and along with the variation of operating environment and operating condition, its kinetic characteristic changes in time, and various disturbing factors are also bigger to the influence of conventional control method.

Summary of the invention

The objective of the invention is to improve and improve existing agricultural machinery and turn to the deficiency that exists in the control technology automatically, provide that a kind of navigation tracking accuracy is higher relatively, stability mechanical automatic steering control method preferably.

For achieving the above object, the present invention adopts following technical scheme:

A kind of mechanical automatic steering control method stores proportionality constant, derivative constant and the integration constant of PID controller in this mechanical memory storage, this method may further comprise the steps:

(1) impact point that will arrive according to mechanical steering is determined its position deviation and course deviation;

(2) determine current expectation front wheel angle according to described position deviation and course deviation, and control steering mechanism and control front-wheel steer, gather current actual front wheel corner by angular transducer simultaneously according to described current expectation front wheel angle;

(3) error and the error change rate of current expectation front wheel angle of calculating and current actual front wheel corner, the PID controller is determined the increment of PID proportionality constant, the increment of derivative constant and the increment of integration constant according to described error and error change rate, obtains adjusted proportionality constant, derivative constant and integration constant;

(4) utilize adjusted proportionality constant, derivative constant and integration constant to obtain the increment of current expectation front wheel angle, obtain adjusted expectation front wheel angle, and control steering mechanism controls front-wheel steer according to described adjusted expectation front wheel angle.

Wherein, be connected with stepper motor between described PID controller and the steering mechanism, described PID determines behind the current expectation front wheel angle to described stepper motor sending controling instruction, and described stepper motor makes it control front-wheel steer according to described current expectation front wheel angle according to described steering order control steering mechanism.

Wherein, in step (1), determine the best forward sight distance of described machinery according to the travel speed of described mechanical current time, determine target course, determine the impact point coordinate of machinery by described target course and best forward sight distance according to described best forward sight distance.

Wherein, determine that according to the travel speed of described mechanical current time the method for the best forward sight distance of described machinery is:

$L=\left\{\begin{array}{cc}2& v\≤1.5m/s\\ v+0.75& v>1.5m/s\end{array}\right.$

Wherein, L is the best forward sight distance of mechanical current time, and v is a gait of march.

Wherein, determine the impact point coordinate of machinery by described target course and best forward sight distance:

x _pre＝x+L·cos(θ _p)

y _pre＝y+L·sin(θ _p)

Wherein, (x _Pre, y _Pre) expression impact point coordinate, (L represents best forward sight distance, θ for x, the y) coordinate of the mechanical current location point of expression _pThe target course of expression machinery.

Wherein, the error rate EC between described current actual front wheel corner and the current expectation front wheel angle is tried to achieve by the differential of the error E between current actual front wheel corner and the current expectation front wheel angle.

Wherein, in step (4), the method for utilizing adjusted integration constant, derivative constant and integration constant to obtain the increment of front wheel angle is:

Δu _i＝K _P(e _i-e _i-1)+K _Ie _i+K _D(e _i-2e _i-1+e _i-2)]

Wherein, e _i, e _I-1, e _I-2Be respectively current time i, first moment i-1, the deviation of second moment i-2 expectation front wheel angle and current actual front wheel corner, wherein, first moment i-1 is the previous moment of current time i, second moment i-2 is the previous moment of first moment i-1, K _PBe proportionality constant, K _I=K _P* T/T _I, K _D=K _P* T _D/ T, T are the sampling period, T _IBe integration constant, T _DBe derivative constant, the wherein said sampling period is 1 second.

Wherein, the described proportionality constant K that is stored _PSpan be 0≤K _P≤ 1000, described integration constant T _ISpan be 0≤T _I≤ 0.5, described derivative constant T _DSpan be 0≤T _D≤ 10.

Wherein, the described proportionality constant of being stored is K _PBe 75, integration constant is T _IBe 0.01, derivative constant T _DBe 8.

Mechanical automatic steering control method based on parameter self-tuning PID controller of the present invention had both had fuzzy control dirigibility and adaptive advantage, had the higher characteristics of PID control accuracy again.Compare with current farm machinery automatic steering control method, have the following advantages:

(1) on the basis of existing navigation control method, makes full use of fuzzy control method, improved stability and degree of accuracy that agricultural machinery turns to control automatically.

(2) according to the variation of agricultural machinery front wheel angle, satisfy its different requirements under different errors, error rate state to pid control parameter, utilize rotating direction control method based on parameter self-tuning PID controller, that realizes agricultural machinery turns to control automatically.This method is better than conventional PID control method, can improve the robustness of control system.

Description of drawings

Fig. 1 concerns analysis diagram for the position in machinery and predefine path among the present invention;

Fig. 2 is the system chart of mechanical automatic steering control method of the present invention;

Fig. 3 is the schematic diagram of mechanical automatic steering control method of the present invention;

Fig. 4 is the fuzzy reasoning part schematic diagram of mechanical automatic steering control method of the present invention;

Fig. 5 is the workflow diagram of mechanical automatic steering control method of the present invention;

Fig. 6 turns to the composition synoptic diagram of control section automatically for machinery among the present invention;

Embodiment

Following examples are used to illustrate the present invention, but are not used for limiting the scope of the invention.

Agricultural machinery in the present embodiment has the DGPS location technology according to prior art, the bearing accuracy that present gps system provides is to be better than 10 meters, and be to obtain higher bearing accuracy, adopting the differential GPS technology usually is the DGPS location technology: a GPS receiver is placed on the base station observes.According to the known precision coordinate of base station, calculate the layback number of base station, and in real time these data are sent by base station to satellite.Receiver user is when carrying out GPS observation, also receive the correction that base station sends, and its positioning result is corrected, thereby improve bearing accuracy, utilize the DGPS location technology to gather the position coordinates of agricultural machinery, for dead reckoning or location algorithm provide raw data.

Agricultural machinery also comprises electronic compass, angular transducer and accelerometer etc. according to prior art in the present embodiment.Wherein electronic compass is measured the course heading of agricultural machinery; Angular transducer obtains the front wheel angle of agricultural machinery; The acceleration of accelerometer measures agricultural machinery, thus the gait of march of agricultural machinery current time obtained.

As shown in Figure 6, the control section that turns to automatically of agricultural machinery relates generally to slave computer and host computer two parts in the present embodiment, the slave computer angular transducer obtains the front wheel angle of agricultural machinery, host computer adopts the ARM chip, be connected collection and the processing that realizes sensing data with accelerometer, the realization of control algolithm and the output of steering order etc., the ARM chip is connected with motor driver, controlling and driving mechanism in this motor driver is according to the steering order that turns to of host computer, control timely and accurately turns to stepper motor to carry out action, host computer can also connect this grade of person's handwriting information processing terminal by the RS232 serial ports, with its mutual data transmission.

The automatic steering control method to agricultural machinery in the present embodiment is specific as follows:

1. predefine path setting

Data set { P is adopted in the predefine path of the agricultural machinery in the present embodiment ₀, P ₁..., P _K-1, P _k,, P _K+1..., P _nExpression, the different subscript of P filling is wherein represented location points different on the predefine path, with different coordinate representation, wherein the coordinate of each location point is the Gauss projection planimetric coordinates, is expressed as { (x respectively when analyzing ₀, y ₀), (x ₁, y ₁) ..., (x _K-1, y _K-1), (x _k, y _k), (x _K+1, y _K+1) ..., (x _n, y _n), wherein x represents horizontal ordinate, y represents ordinate.

Utilize the DGPS location technology to gather predefine path each location point coordinate, i.e. { (x ₀, y ₀), (x ₁, y ₁) ..., (x _K-1, y _K-1), (x _k, y _k), (x _K+1, y _K+1) ..., (x _n, y _n).As the predefine path is straight line, only needs selected road from a P _AWith terminal point P _B, utilize the DGPS location technology to obtain locator data, obtain this Gauss plane coordinate of 2 by coordinate conversion, be expressed as (x respectively _A, y _A) and (x _B, y _B).

Adopt the DGPS location technology to gather the coordinate of each point on the predefine path in advance, in the concrete driving process of agricultural machinery, turn to the method for control as described below automatically it.

2. automatic steering control method

(1) obtains agricultural machinery current location point P _c, course angle θ _cAnd gait of march v

The position that is illustrated in figure 1 as among the present invention machinery and predefine path concerns analysis diagram, agricultural machinery in the process of moving, the location point P of current time agricultural machinery _c, course angle θ _cAs follows with the data capture method of gait of march v:

1. agricultural machinery current location point P _c: the location point coordinate of the agricultural machinery current time that at first obtains by the DGPS location technology in the present embodiment obtains agricultural machinery current location point P through dead reckoning or positioning correcting again _c

2. the current course heading θ of agricultural machinery _c: present embodiment adopts electronic compass to obtain, and is converted to the angle value under the Gaussian plane;

3. agricultural machinery current driving speed v: at first obtain the current accekeration of agricultural machinery, obtain current gait of march through integral and calculating again by accelerometer.

(2) determine the impact point P ' of agricultural machinery according to the predefine path _p

Adopt the dynamic route searching algorithm in the present embodiment, extrapolate best forward sight distance L of agricultural machinery and impact point coordinate, and then definite impact point P ' _p

Described dynamic route searching algorithm is based on takes aim at follower theory in advance, by the best forward sight distance L of dynamic calculation agricultural machinery current time, determines its impact point coordinate on the predefine path, and concrete steps are as follows:

(21) determine the best forward sight distance L of agricultural machinery: the present invention mainly considers the influence of agricultural machinery gait of march to its forward sight distance, is determined by experiment out the relation of gait of march and forward sight distance, is expressed as:

$L=\left\{\begin{array}{cc}2& v\≤1.5m/s\\ v+0.75& v>1.5m/s\end{array}\right.---\left(1\right)$

L is the best forward sight distance of agricultural machinery current time in the formula, and v is a gait of march;

(22) determine the impact point P ' of agricultural machinery _pCoordinate: according to the best forward sight distance L of making a strategic decision out, the impact point P ' of agricultural machinery _pThe coordinate Calculation formula can be expressed as:

x _pre＝x+L·cos(θ _p) (2)

y _pre＝y+L·sin(θ _p) (3)

In the formula, (x _Pre, y _Pre) expression impact point P ' _pCoordinate, (x, y) expression agricultural machinery current location point P _cCoordinate, L represents best forward sight distance, θ _pExpression agricultural machinery bogey heading.

As shown in Figure 1, wherein:

Agricultural machinery bogey heading θ _pDefinite method be: ${\mathrm{\θ}}_p=\stackrel{\→}{P_c{P^{\′}}_p},$ Be θ _pBe agricultural machinery current location point P _cDefinite object point P ' _pVector;

(3) determine the current location deviation D of agricultural machinery _eWith course deviation θ _e

Referring to Fig. 1, the current location deviation D of agricultural machinery wherein _eWith course deviation θ _eBe defined as follows:

The course deviation θ that agricultural machinery is current _e: be defined as agricultural machinery bogey heading θ _pWith current course angle θ _cDifference, be expressed as θ _e=θ _p-θ _c

Agricultural machinery position deviation D _e: be defined as agricultural machinery current location point P _cAnd the subpoint P ' on the predefine path _cBetween distance, be expressed as $D_e=\left|\stackrel{\→}{P_c{P}_c^{\′}}\right|,$ Current location point P wherein _cSubpoint P ' on the predefine path _cObtain by the intersection point of asking mutually perpendicular two straight lines;

In the present embodiment regulation when agricultural machinery when the predefine path direction is advanced, as if agricultural machinery current location point P _cBe positioned at the right side in predefine path, then position deviation D _eFor just; If its current location point P _cBe positioned at the left side in predefine path, then position deviation D _eFor negative.

So position deviation D _eWith course deviation θ _eThe determination methods of direction is as follows:

Position deviation D _eDirection determining method: if course deviation θ _eFor just, position deviation D then _eFor just, otherwise then for negative.

Course deviation θ _eDirection determining method: if course deviation θ _eFor just, agricultural machinery the right in the predefine path then is described, on the contrary on the left side then.

(4) the PID controller initial expectation front wheel angle of at first determining agricultural machinery according to the position deviation and the course deviation of agricultural machinery, and send instruction to stepper motor according to this initial expectation front wheel angle, stepper motor makes its control front-wheel turn to by initial expectation front wheel angle according to this instruction control steering mechanism, simultaneously, gather actual front wheel angle by angular transducer, variation according to agricultural machinery actual front wheel corner, by the parameter self-tuning pid control algorithm, the on-line tuning pid parameter, extrapolate next and expect front wheel angle constantly, and to stepper motor transmission instruction, stepper motor makes its control front-wheel turn to by the expectation front wheel angle according to this instruction control steering mechanism, repeat the continuous online adjustment that above process realizes the expectation front wheel angle, finish automatic steering procedure.

Agricultural machinery turns to controlled step to be based on parameter self-tuning PID controller, parameter self-tuning PID controller on-line tuning pid parameter K automatically in the present embodiment _P, T _I, T _D, extrapolate next and expect that constantly the front wheel angle method is included as:

(41) the pid control parameter K of initialization front wheel angle _P, T _I, T _DWith sampling period T;

Present embodiment adopts the increment type PID control algolithm, according to Simulation results, pid parameter K is set _P, T _I, T _DInitial value, be respectively 75,0.01 and 8, choosing sampling period T is 1 second, the increment type PID control method only has relation with nearest twice front wheel angle deviation, when existing error or computational accuracy not enough, less to the influence degree that controlled quentity controlled variable is calculated, the purpose of increment type PID control method is the expectation front wheel angle of current time i agricultural machinery is adjusted, next is turned to by the control of the expectation front wheel angle after adjusting constantly, specifically obtains according to following formula:

Δu _i＝u _i-u _i-1＝K _P(e _i-e _i-1)+K _Ie _i+K _D(e _i-2e _i-1+e _i-2)](4)

Wherein, u _i, u _I-1Be respectively current time i, the expectation front wheel angle of first moment i-1 agricultural machinery, e _i, e _I-1, e _I-2Be respectively the deviation of current time i, first moment i-1, second moment i-2 expectation front wheel angle and actual front wheel corner, wherein, first moment i-1 is the previous moment at current time i, and second moment i-2 is the previous moment at first moment i-1, K _PBe scale-up factor, K _I=K _P* T/T _I, K _D=K _P* T _D/ T, wherein, T is the sampling period, preferred initial value scope is 0.1～1s, T _IBe integration time constant, T _DBe derivative time constant, 0≤K _P≤ 1000,0≤T _I≤ 0.5.

(42) determine the fuzzy logic ordination table and the fuzzy logic control table of the front wheel angle controlled quentity controlled variable of PID controller;

The PID controller adopts the adaptive Fuzzy PID Control device in the present embodiment, realizes three parameter K of PID _P, T _I, T _DOn-line tuning, and then the front wheel angle of decision-making agricultural machinery is to improve precision and the stability that agricultural machinery turns to control automatically.

(421) determine the fuzzy logic ordination table of the front wheel angle controlled quentity controlled variable of PID controller;

The core of adaptive Fuzzy PID Control device design is to sum up the technical know-how and the practical operation experience of human pilot, is converted into fuzzy control rule, sets up the fuzzy logic decision table, realizes the decision-making of pid parameter variable quantity.Consider error, error rate and three parameter K of PID controller _P, K _P, K _PVariation delta k _p, Δ k _i, Δ k _dThe positive negative characteristic of variable, the span of each variable is divided into 7 kinds, be respectively: honest (PB), center (PM), just little (PS), zero (ZO), negative little (NS), negative in (NM) and negative (NB) greatly, finally obtain the variation delta k of ambiguity error E, ambiguity error rate of change EC and three parameters of PID controller _p, Δ k _i, Δ k _dFuzzy control table (shown in table 1, table 2, table 3), in table 1, table 2, the table 3 corresponding to the honest PB1 of ambiguity error E, center PM1, just little PS1, negative little NS1, negative in NM1 and negative big NB1 totally 7 grades, corresponding to the honest PB2 of ambiguity error rate of change EC, center PM2, just little PS2, negative little NS2, negative in NM2 and negative big NB2 totally 7 grades, corresponding to variation delta k _pHonest PB3, center PM3, just little PS3, negative little NS3, negative in NM3 and negative big NB3 totally 7 grades, corresponding to the honest PB4 of variation delta kp, center PM4, just little PS4, negative little NS4, negative in NM4 and negative big NB4 totally 7 grades, corresponding to variation delta k _pHonest PB5, center PM5, just little PS5, negative little NS5, negative in NM5 and negative big NB5 totally 7 grades.

Be not limited in the present embodiment each variation in above-mentioned is divided into 7 numerical ranges, also can further be refined as more numerical range, but the fuzzy rule described in the table below meeting.

Table 1 Δ K _PFuzzy reasoning table

Table 2 Δ K _IFuzzy reasoning table

Table 3 Δ K _DFuzzy reasoning table

The parameter tuning principle is in the present embodiment: the output quantity of fuzzy decision is the variation delta K of pid parameter _P, Δ K _I, Δ K _DAbsolute value according to different ambiguity error E | the absolute value of E| and ambiguity error rate of change EC | EC| is to K _P, K _I, K _DAdjust, principle is as follows:

★ is when ambiguity error | when E| is big, as 3≤| E|≤5, have tracking performance preferably for making system, accelerate the response speed of system, should get bigger K _P, as 500≤K _P≤ 1000; Simultaneously for avoiding system when initial, the saturated control action that makes of differential that may occur owing to the instantaneous increase of error exceeds allowed band, and should get less K this moment _D, as 0≤K _D≤ 3; For avoiding system responses big overshoot to occur, it is saturated to produce integration simultaneously, and the reply integral action is limited, and gets K usually _I=0.

★ works as ambiguity error | and E| is moderate, as 1≤| E|≤3, have less overshoot for making system responses, should get slightly little K _P, as 0≤K _P≤ 100; This moment K _DValue bigger to the influence of system responses, be of moderate size, as 3≤K _D≤ 8. to guarantee the response speed of system; Can increase simultaneously of the effect of some integrations to control, but if K _IToo big, easily cause integration saturated, then can not accelerate system response time too for a short time, so K _IValue want suitably, as 0≤K _I≤ 0.3.

★ works as ambiguity error | E| hour, as 0≤| E|≤1, have stable state preferably for making system, should get bigger K _PWith K _I, as K _PAs 500≤K _P≤ 1000, as 0.3≤K _I≤ 0.5; Simultaneously for avoiding system vibration, K near setting value, to occur _DThe selection of value is extremely important, generally can basis | EC| determines: when the ambiguity error rate of change | EC| value than hour, as 0≤| EC|≤2, K _DDesirable big, as 5≤K _D≤ 10; When | EC| value is big, as 2≤| EC|≤5, K _DDesirable littler, as 0≤K _D≤ 5, common K _DBe median size.

(422) determine the fuzzy logic control table of the front wheel angle controlled quentity controlled variable of PID controller;

In experiment simulation,, be that central value is got a hop count value scope with the exact value of error e, error rate e ' respectively promptly with fuzzy fuzzy quantity error E, the fuzzy quantity error rate EC of changing into of the exact value of error e and error rate e ', obtain fuzzy quantity error E, fuzzy quantity error rate EC, the fuzzy quantity error E is chosen fuzzy subset { NB1, NM1, NS1, ZO1, PS1, PM1, PB1}, EC chooses fuzzy subset { NB2 to the fuzzy quantity error rate, NM2, NS2, ZO2, PS2, PM2, PB2}.Choose pid control parameter K _PVariation delta K _PThe fuzzy subset NB3, NM3, NS3, ZO3, PS3, PM3, PB3} chooses pid control parameter K _IVariation delta K _IThe fuzzy subset NB4, NM4, NS4, ZO4, PS4, PM4, PB4} chooses pid control parameter K _DVariation delta K _DFuzzy subset { NB5, NM5, NS5, ZO5, PS5, PM5, PB5} gets fuzzy quantity error E and fuzzy quantity error rate EC variation range, is domain on the fuzzy set with this scope definition, is expressed as E '={ 15 ,-12 ,-9,-6 ,-3,0,3,6,9,12,15}, EC '={ 5 ,-4 ,-3,-2 ,-1,0,1,2,3,4,5}, wherein each integer is all corresponding to the hop count value scope among the ambiguity error E among the E ', and each integer is all corresponding to the hop count value scope in the ambiguity error rate of change among the EC '.

Experience based on expertise and site operation personnel is formulated fuzzy rule, opens the fuzzy rule device, imports different discrete magnitude E ', EC ', obtains pid parameter variation delta K _P, Δ K _I, Δ K _DRespective value, constitute fuzzy control table (shown in table 4, table 5, table 6).

Table 4 Δ K _PFuzzy control table

Table 5 Δ K _IFuzzy control table

Table 6 Δ K _DFuzzy control table

Three of initial p ID controller parameter K at first in the present embodiment _P, K _I, K _P, K _PBe scale-up factor, K _I=K _P* T/T _I, K _D=K _P* T _D/ T, wherein, T is the sampling period, T _IBe integration time constant, T _DBe derivative time constant, present embodiment is provided with pid parameter K according to Simulation results _P, T _I, T _DInitial value be respectively 75,0.01 and 8, choosing sampling period T is 1 second, after having determined the fuzzy logic ordination table and fuzzy logic control table of front wheel angle controlled quentity controlled variable of PID controller, as shown in Figure 5, in the agricultural machinery traveling process, turn to control procedure automatically in conjunction with foregoing detailed description:

A: get current sampled value;

B: determine the location point P that agricultural machinery is current according to foregoing method _cWith impact point P ' _pCoordinate, determine the position deviation D of current agricultural machinery _eWith course deviation θ _e

C: by the position deviation D of current time agricultural machinery _eWith course deviation θ _eDetermine the current expectation front wheel angle u of agricultural machinery _i, and according to expectation front wheel angle u before deserving _iSend instruction to stepper motor, stepper motor makes its control front-wheel by current expectation front wheel angle u according to this instruction control steering mechanism _iTurn to, simultaneously, gather actual front wheel angle by angular transducer, the error of this actual front wheel corner and current expectation front wheel angle is e _i, to the error e of current expectation front wheel angle _iDifferentiate (de/dt), obtain error rate e ';

D: to the error e of current expectation front wheel angle _iBe input to the PID controller with error rate e ' as input quantity, the PID controller is with error e _iExact value be converted to ambiguity error E, the exact value of error rate e ' is converted to ambiguity error rate of change EC

With the input quantity E after the gelatinization, the EC input as fuzzy reasoning part, again by E, EC and total control law R, composition rule carries out fuzzy reasoning and obtains fuzzy control quantity U and be by inference: $U={(E\&times;\mathrm{EC})}^{{\mathrm{<figure-callout\; id="1"\; label="T"\; filenames="S2008100564780E00011.png"\; state="\{\{state\}\}">T</figure-callout>}}_1}\&CenterDot;R$

Wherein, T ₁Represent certain control law, U is that the control corresponding amount (is represented Δ k respectively in an embodiment _p, Δ k _i, Δ k _d).

E. defuzzification: the controlled quentity controlled variable that after FUZZY ALGORITHMS FOR CONTROL is calculated, obtains, be the value in the domain of controlled quentity controlled variable linguistic variable, directly controlling object must be converted into the value in the basic domain of controlled quentity controlled variable.The scale factor of controlled quentity controlled variable is defined as follows:

k _u＝u _max/l

Wherein, l is that controlled quentity controlled variable is at 0～u _MaxThe shelves number that is divided into after quantizing in the scope adopts gravity model appoach that output fuzzy control quantity U is carried out defuzzification.

Error e, error rate e ' when system's moment in the present embodiment are known, can be by determining the value of parameter variable quantity, the parameter K of on-line tuning PID controller current time _P, K _I, K _D；

F: to current K _P, K _I, K _DAdjust; K _P=K _P'+Δ K _P, K _I=K _I'+Δ K _I, K _D=K _D'+Δ K _D, wherein, K ' _P, K ' _I, K ' _DPid parameter for a last moment;

The g:PID controller obtains controlled variable Δ u according to following formula _i:

Δu _i＝K _P(e _i-e _i-1)+K _Ie _i+K _D(e _i-2e _i-1+e _i-2)](4)

Expectation front wheel angle Δ u after obtaining adjusting _i+ u _i, the expectation front wheel angle after adjusting according to this sends instruction to stepper motor, and stepper motor makes its control front-wheel by the expectation front wheel angle u after adjusting according to this instruction control steering mechanism _iTurn to, finish step control;

H: wait for sampling next time then, repeated execution of steps b～g, so circulation when in the error range that the position deviation and the course deviation of agricultural machinery are being set, finishes this process, and that can finish agricultural machinery turns to control automatically

As shown in Figure 3, in the present embodiment adaptive Fuzzy PID Control device with the rate of change e ' of error e between the current time front wheel angle of agricultural machinery and the expectation front wheel angle and error e as input quantity, by constantly detecting e and e ', the fuzzy logic decision table of determining according to fuzzy control principle comes 3 parameter K to controller in the agricultural machinery traveling process _P, T _I, T _DCarry out online modification, make it have good dynamic and static properties.

Below in the hope of K _PBe example explanation inference method:

(1) according to table 1, can be with every K _PAdjustment law writes out, and for example, article one can be written as: R ₁: if E=NB and EC=NB then K _P=PS, the computing method of this rule degree of membership are:

${\mathrm{\&mu;}}_{K_{p1}}\left(C_P\right)={\mathrm{\&mu;}}_{\mathrm{NB},E}\left(E\right)^{\mathrm{\&mu;}}_{\mathrm{NB},\mathrm{EC}}\left(\mathrm{EC}\right)---\left(5\right)$

In like manner, can obtain about K _PThe degree of membership μ of strictly all rules _Kpi(c _p) (i=1,2 ..., n), wherein, n is about K _PThe bar number of strictly all rules, c _PiBe the central value that institute's delivery is stuck with paste set in the i bar rule, μ _{NB, E}(E) degree of membership of representative when E gets NB, μ _{NB, EC}(EC) degree of membership of representative when EC gets NB.

(2) when system carves error E at a time, error rate EC is known, K _PComputing formula be:

$K_P=\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}({\mathrm{\&mu;}}_{K_{\mathrm{pi}}}\left(c_p\right)\&times;c_{\mathrm{pi}})}{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}_{K_{\mathrm{pi}}}\left(c_p\right)}---\left(6\right)$

In like manner, can obtain K _I, K _DComputing formula, as (7), shown in (8).Wherein, m, l are respectively about K _I, K _DThe bar number of strictly all rules.

$K_I=\frac{\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}({\mathrm{\&mu;}}_{K_{\mathrm{ii}}}\left(d_i\right)\&times;d_{\mathrm{ii}})}{\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}_{K_{\mathrm{ii}}}\left(d_i\right)}---\left(7\right)$

$K_D=\frac{\underset{i=1}{\overset{1}{\mathrm{\&Sigma;}}}({\mathrm{\&mu;}}_{K_{\mathrm{di}}}\left(g_d\right)\&times;g_{\mathrm{di}})}{\underset{i=1}{\overset{l}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}_{K_{\mathrm{dii}}}\left(g_d\right)}---\left(8\right)$

From formula (5)～(8) as can be seen, K _P, K _I, K _DAnd set up a kind of funtcional relationship between ambiguity error E and the ambiguity error rate of change EC, can satisfy system's different requirements to pid control parameter under different ambiguity error E, ambiguity error rate of change EC state, so this controller is better than conventional PID controller.

Δ K _P, Δ K _I, Δ K _DFuzzy reasoning table set up after, can carry out K _P, K _I, K _DOn-line tuning.If ambiguity error E, ambiguity error rate of change EC and Δ K _P, Δ K _I, Δ K _DEqual Normal Distribution, can draw each fuzzy subset's degree of membership, according to degree of membership assignment table and each parameter fuzzy controlling models of each fuzzy subset, use the fuzzy decision table of fuzzy compositional rule of inference design pid parameter variable quantity, find corrected parameter substitution following formula and calculate:

K _P=K ' _P+ Δ K _P, wherein, K ' _PBe the pid parameter in a last moment, Δ K _PCan check in by fuzzy reasoning table 1;

K _I=K ' _I+ Δ K _I, wherein, K ' _IBe the pid parameter in a last moment, Δ K _PCan check in by fuzzy reasoning table 2;

K _D=K ' _D+ Δ K _D, wherein, K ' _DBe the pid parameter in a last moment, Δ K _PCan check in by fuzzy reasoning table 3.

Systematic error E and error rate EC variation range are defined as discrete domain on the fuzzy set, are expressed as: E '={ 15 ,-12 ,-9 ,-6,-3,0,3,6,9,12,15}, EC '={ 5 ,-4,-3 ,-2 ,-1,0,1,2,3,4,5} opens the fuzzy rule device, imports different discrete magnitude E, EC, obtains corresponding Δ K _P, Δ K _I, Δ K _D, constitute fuzzy control table (shown in table 4, table 5, table 6).

Mechanical automatic steering control system block diagram provided by the invention as shown in Figure 2, this system is based on parameter self-tuning PID controller, parameter self-tuning PID control section mainly consists of the following components in the present embodiment:

1. fuzzy controller: being the core of Fuzzy control system, is to adopt based on the language pattern of the fuzzy control representation of knowledge and rule-based reasoning to stick with paste controller, mainly comprises obfuscation, fuzzy reasoning and defuzzification three parts of input quantity.According to the difference of controlled device and the difference that system's static state, dynamic perfromance are required, the rule of fuzzy controller is also different, thereby control algolithm is different;

2. topworks: comprise direct current generator and stepper motor etc.;

3. controlled device: can be a kind of equipment or device and their colony, under certain constraint condition, work;

4. sensor: sensor need be set the controlled variable of controlled device or various processes is converted to electric signal;

5. D/A converter is realized D/A switch and mould/number conversion.

Illustrate and illustrate in conjunction with above preferred embodiment though the present invention is a collective; but the personnel that are familiar with this technical field are appreciated that; wherein no matter still can make various changes in detail in form, this does not deviate from the tangible and scope of patent protection of spirit of the present invention.

Claims (10)

1. mechanical automatic steering control method stores proportionality constant, derivative constant and the integration constant of PID controller in this mechanical memory storage, it is characterized in that this method may further comprise the steps:

(1) impact point that will arrive according to mechanical steering is determined its position deviation and course deviation;

(2) the PID controller is determined current expectation front wheel angle according to described position deviation and course deviation, and controls steering mechanism and control front-wheel steer according to described current expectation front wheel angle, gathers current actual front wheel corner by angular transducer simultaneously;

(3) error and the error change rate of current expectation front wheel angle of calculating and current actual front wheel corner, the PID controller is determined the increment of PID proportionality constant, the increment of derivative constant and the increment of integration constant according to described error and error change rate, obtains adjusted proportionality constant, derivative constant and integration constant;

(4) the PID controller utilizes adjusted proportionality constant, derivative constant and integration constant to obtain the increment of current expectation front wheel angle, obtain adjusted expectation front wheel angle, and control steering mechanism controls front-wheel steer according to described adjusted expectation front wheel angle.

2. mechanical automatic steering control method as claimed in claim 1, it is characterized in that, be connected with stepper motor between described PID controller and the steering mechanism, described PID determines behind the current expectation front wheel angle to described stepper motor sending controling instruction, and described stepper motor makes it control front-wheel steer according to described current expectation front wheel angle according to described steering order control steering mechanism.

3. mechanical automatic steering control method as claimed in claim 1, it is characterized in that, in step (1), determine the best forward sight distance of described machinery according to the travel speed of described mechanical current time, determine target course according to described best forward sight distance, determine the impact point coordinate of machinery by described target course and best forward sight distance.

4. mechanical automatic steering control method as claimed in claim 3 is characterized in that, determines that according to the travel speed of described mechanical current time the method for the best forward sight distance of described machinery is:

$L=\left\{\begin{array}{cc}2& v\&le;1.5m/s\\ v+0.75& v>1.5m/s\end{array}\right.$

Wherein, L is the best forward sight distance of mechanical current time, and v is a gait of march.

5. mechanical automatic steering control method as claimed in claim 4 is characterized in that, is determined the impact point coordinate of machinery by described target course and best forward sight distance:

x _pre＝x+L·cos(θ _p)

y _pre＝y+L·sin(θ _p)

Wherein, (x _Pre, y _Pre) expression impact point coordinate, (L represents best forward sight distance, θ for x, the y) coordinate of the mechanical current location point of expression _pThe target course of expression machinery.

6. mechanical automatic steering control method as claimed in claim 1, it is characterized in that the error rate EC between described current actual front wheel corner and the current expectation front wheel angle is tried to achieve by the differential of the error E between current actual front wheel corner and the current expectation front wheel angle.

7. mechanical automatic steering control method as claimed in claim 1 is characterized in that, in step (4), the method for utilizing adjusted integration constant, derivative constant and integration constant to obtain the increment of front wheel angle is:

Δu _i＝K _P(e _i-e _i-1)+K _Ie _i+K _D(e _i-2e _i-1+e _i-2)]

Wherein, e _i, e _I-1, e _I-2Be respectively current time i, first moment i-1, the deviation of second moment i-2 expectation front wheel angle and current actual front wheel corner, wherein, first moment i-1 is the previous moment of current time i, second moment i-2 is the previous moment of first moment i-1, K _PBe proportionality constant, K _I=K _P* T/T _I, K _D=K _P* T _D/ T, T are the sampling period, T _IBe integration constant, T _DBe derivative constant.

8. mechanical automatic steering control method as claimed in claim 7 is characterized in that, described sampling period T is 1 second.

9. mechanical automatic steering control method as claimed in claim 1 is characterized in that, the described proportionality constant K that is stored _PSpan be 0≤K _P≤ 1000, described integration constant T _ISpan be 0≤T _I≤ 0.5, described derivative constant T _DSpan be 0≤T _D≤ 10.

10. mechanical automatic steering control method as claimed in claim 1 is characterized in that the described proportionality constant of being stored is K _PBe 75, integration constant is T _IBe 0.01, derivative constant T _DBe 8.

Images