CN101221447A - A kind of mechanical automatic steering control method - Google Patents

Abstract

The invention relates to a mechanical automatic steering control method. The method comprises the steps of: determining its position deviation and heading deviation; according to the change of the actual front wheel rotation angle of the agricultural machinery, the PID parameters are adjusted online through the parameter self-tuning PID control algorithm, and the next step is calculated. Always expect the front wheel angle, so as to realize the automatic steering control of agricultural machinery. On the basis of the conventional PID navigation control method, the present invention makes full use of the fuzzy control method, based on the change of the actual front wheel rotation angle of the agricultural machinery, and satisfies the different requirements of the PID control parameters under different errors and error change rates, and online tuning PID parameters. It not only has the advantages of flexibility and adaptability of fuzzy control, but also has the characteristics of high precision of PID control, which can improve the stability and precision of automatic steering control of agricultural machinery and the robustness of the control system.

Description

A kind of mechanical automatic steering control method

technical field

The invention relates to the field of navigation control of agricultural machinery, in particular to a control method for automatic steering of machinery.

Background technique

The main purpose of agricultural machinery navigation control is to determine the positional relationship between the agricultural machinery itself and the predefined path based on the relatively accurate navigation and positioning results obtained by each sensor, and to determine the appropriate front wheel angle in combination with the movement state of the agricultural machinery and the corresponding control algorithm. , to correct the path tracking error, so that the current heading of the agricultural machinery coincides with the target heading quickly. Navigation control is divided into longitudinal control and lateral control, wherein lateral control mainly refers to steering control, and longitudinal control mainly refers to speed adjustment. The automatic steering control method of agricultural machinery is the basis for realizing its precise heading tracking and automatic navigation control.

Commonly used navigation control methods include linear model control methods, optimal control methods and fuzzy control methods. The University of Illinois in the United States has done in-depth and extensive research on the automatic control of agricultural machinery and the information fusion of multi-sensors. It has successfully developed a tractor that can realize a variety of farming operations, and uses the electro-hydraulic control system to perform steering actions. Unmanned driving in gaps. The University of Tokyo in Japan uses machine vision technology to conduct research on automatic navigation systems. According to the linear steering control model, the target direction angle and the longitudinal angle of the vehicle are fused to calculate the steering wheel deflection angle and complete the turning control. Noguchi N et al. in the United States established a control system for agricultural vehicles with self-learning ability by applying neural network and genetic algorithm. The experimental results show that this model has a good control effect on vehicles running on flat roads, but it is not suitable for roads. Inclined situation. Benson, Dong ZL and others applied the PID (proportion-integral-differential proportion-integral-derivative) control method to design the PID controller. This algorithm does not depend on the precise mathematical model and avoids the tedious modeling process. The response characteristics of the algorithm are combined to control, and the proportional parameters, integral parameters and differential parameters of the algorithm are adjusted reasonably. Experimental results show that the method has good path tracking effect. Based on the idea of Kalman filtering, domestic Zhou Jun and Liu Chengliang and others combined the observation values of various sensors to give a predictive tracking control method for agricultural robot navigation, avoiding the state feedback caused by time-consuming calculations based on the visual system The adverse effects caused by the lag can improve the robustness and accuracy of the navigation control. South China Agricultural University integrated GPS technology, computer technology and multi-sensor technology to develop an agricultural intelligent mobile operation platform powered by electric motors. Experiments have proved that the path tracking control of this platform is relatively difficult.

Through the analysis, it can be known that the focus and difficulty of navigation control is to improve the stability of steering control and the accuracy of path tracking. Conventional PID control method can obtain high-precision path tracking effect, and has certain robustness and reliability, but the anti-interference ability of this method is weak.

The fuzzy control method is a new type of control method developed in recent years. Its advantage is that it does not need to master the precise mathematical model of the controlled object. According to the manual control rules, the organizational control decision determines the size of the control amount and can obtain good dynamic characteristics. The static characteristics are relatively poor. Under field conditions, it is difficult to establish kinematics and dynamics models of agricultural machinery. As the operating environment and operating conditions change, its motion characteristics change with time, and various disturbance factors have a greater impact on conventional control methods.

Contents of the invention

The purpose of the present invention is to improve and improve the deficiencies in the existing automatic steering control technology of agricultural machinery, and provide a mechanical automatic steering control method with relatively high navigation tracking accuracy and good stability.

To achieve the above object, the present invention adopts the following technical solutions:

A mechanical automatic steering control method, the mechanical storage device stores the proportional constant, differential constant and integral constant of the PID controller, the method comprises the following steps:

(1) Determine its position deviation and course deviation according to the target point to be reached by the mechanical steering;

(2) Determine the current expected front wheel angle according to the position deviation and heading deviation, and control the steering mechanism to control the front wheel steering according to the current expected front wheel angle, and simultaneously collect the current actual front wheel angle through the angle sensor;

(3) Calculate the error between the current expected front wheel angle and the current actual front wheel angle and the rate of change of the error, and the PID controller determines the increment of the PID proportional constant, the increment and the integral of the differential constant according to the error and the rate of change of the error Increment of constants to obtain adjusted proportional constants, differential constants and integral constants;

(4) Use the adjusted proportional constant, differential constant and integral constant to obtain the increment of the current expected front wheel angle, obtain the adjusted expected front wheel angle, and control the steering mechanism according to the adjusted expected front wheel angle. Control the steering of the front wheels.

Wherein, a stepping motor is connected between the PID controller and the steering mechanism, and the PID sends a control command to the stepping motor after determining the current expected front wheel angle, and the stepping motor controls the steering according to the control command The mechanism makes it control the steering of the front wheels according to the current desired front wheel rotation angle.

Wherein, in step (1), the optimal look-ahead distance of the machine is determined according to the travel speed of the machine at the current moment, and the target heading angle is determined according to the optimal look-ahead distance, and the target heading angle and the maximum The optimal look-ahead distance determines the coordinates of the target point of the machine.

Wherein, the method for determining the optimal forward-looking distance of the machine according to the running speed of the machine at the current moment is:

$\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 2</span>}& \mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; 1.5</span>}\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; the\; s</span>}\\ \mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 0.75</span>}& \mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; 1.5</span>}\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; the\; s</span>}\end{array}\right.$

Among them, L is the best forward-looking distance of the machine at the current moment, and v is the traveling speed.

Wherein, the target point coordinates of machinery are determined by the target heading angle and the best look-ahead distance:

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

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

Among them, (x _pre , y _pre ) represents the coordinates of the target point, (x, y) represents the coordinates of the current position of the machine, L represents the optimal look-ahead distance, and θ _p represents the target heading angle of the machine.

Wherein, the error change rate EC between the current actual front wheel angle and the current expected front wheel angle is obtained by the differential of the error E between the current actual front wheel angle and the current expected front wheel angle.

Wherein, in step (4), the method of obtaining the increment of the front wheel angle by using the adjusted integral constant, differential constant and integral constant is:

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

Among them, e _i , e _i-1 , and e _i-2 are the deviations between the current moment i, the first moment i-1, and the second moment i-2, respectively, between the expected front wheel angle and the current actual front wheel angle, where the first The moment i-1 is the moment before the current moment i, the second moment i-2 is the moment before the first moment i-1, K _P is a proportional constant, K _I =K _P *T/ _TI , K _D =K _P *T _D /T, T is the sampling period, T _I is the integral constant, T _D is the differential constant, wherein the sampling period is 1 second.

Wherein, the value range of the stored proportional constant K _P is 0≤K _P ≤1000, the value range of the integral constant T _I is 0≤T _I ≤0.5, and the value range of the differential constant T _D is The value range is 0≤T _D ≤10.

Wherein, the stored proportional constant is 75 for K _P , the integral constant is 0.01 for T _I , and the differential constant T _D is 8.

The mechanical automatic steering control method based on the parameter self-tuning PID controller of the present invention not only has the advantages of fuzzy control flexibility and adaptability, but also has the characteristics of high PID control precision. Compared with the current automatic steering control method of agricultural machinery, it has the following advantages:

(1) On the basis of the existing navigation control method, the fuzzy control method is fully utilized to improve the stability and accuracy of the automatic steering control of agricultural machinery.

(2) According to the change of the front wheel angle of agricultural machinery, meet its different requirements for PID control parameters under different errors and error change rates, and use the steering control method based on parameter self-tuning PID controller to realize automatic steering of agricultural machinery control. This method is superior to the conventional PID control method and can improve the robustness of the control system.

Description of drawings

Fig. 1 is an analytical diagram of the positional relationship between machinery and a predefined path in the present invention;

Fig. 2 is a system block diagram of the mechanical automatic steering control method of the present invention;

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

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

Fig. 5 is the working flowchart of the mechanical automatic steering control method of the present invention;

Fig. 6 is a schematic diagram of the composition of the mechanical automatic steering control part in the present invention;

Detailed ways

The following examples are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

The agricultural machinery in the present embodiment has DGPS positioning technology according to the prior art, and the positioning accuracy provided by the GPS system is better than 10 meters at present, and in order to obtain higher positioning accuracy, usually adopt differential GPS technology, that is, the DGPS positioning technology: a A GPS receiver is placed on the base station for observation. According to the known precise coordinates of the base station, the distance correction number from the base station to the satellite is calculated, and the data is sent by the base station in real time. While the user receiver is performing GPS observation, it also receives the correction number sent by the reference station, and corrects its positioning result, thereby improving the positioning accuracy, and using DGPS positioning technology to collect the position coordinates of agricultural machinery for dead reckoning or positioning Algorithms provide raw data.

According to the prior art, the agricultural machinery in this embodiment also includes an electronic compass, an angle sensor, an accelerometer, and the like. Among them, the electronic compass measures the heading angle of the agricultural machinery; the angle sensor obtains the front wheel angle of the agricultural machinery; the accelerometer measures the acceleration of the agricultural machinery, so as to obtain the current speed of the agricultural machinery.

As shown in Figure 6, the automatic steering control part of the agricultural machinery in this embodiment mainly involves two parts, the lower computer and the upper computer. The angle sensor of the lower computer obtains the front wheel angle of the agricultural machinery. The acquisition and processing of sensor data, the realization of control algorithms and the output of control instructions, etc., the ARM chip is connected to the motor driver, and the control drive mechanism in the motor driver can timely and accurately control the steering stepping motor according to the steering control instructions of the host computer. Action, the host computer can also connect to the handwriting book and other information processing terminals through the RS232 serial port, and transmit data with each other.

The automatic steering control method for agricultural machinery in this embodiment is specifically as follows:

1. Predefined path setting

The predefined path of the agricultural machinery in this embodiment is represented by a data set {P ₀ , P ₁ , ..., P _k-1 , P _k , P _k+1 , ..., P _n }, where Adding different subscripts to P indicates different position points on the predefined path, which are represented by different coordinates during analysis, where the coordinates of each position point are Gaussian projection plane coordinates, respectively expressed as {(x ₀ , y ₀ ) , (x ₁ , y ₁ ), ..., (x _k-1 , y _k-1 ), (x _k , y _k ), (x _k+1 , y _k+1 ), ..., ( x _n , y _n )}, where x represents the abscissa and y represents the ordinate.

Use DGPS positioning technology to collect the coordinates of each location point on the predefined path, namely {(x ₀ , y ₀ ), (x ₁ , y ₁ ),..., (x _k-1 , y _k-1 ), (x _k , y _k ), (x _k+1 , y _k+1 ), ..., (x _n , y _n )}. If the predefined path is a straight line, you only need to select the starting point _PA and the ending point P _B of the path, use DGPS positioning technology to obtain positioning data, and obtain the Gaussian plane coordinates of the two points through coordinate conversion, which are expressed as (x _A , y _A ) and (x _B , y _B ).

DGPS positioning technology is used in advance to collect the coordinates of each point on the predefined path. During the specific driving process of agricultural machinery, the method of automatic steering control is as follows.

2. Automatic steering control method

(1) Obtain the current position point P _c , heading angle θ _c and travel speed v of the agricultural machinery

As shown in Figure 1, it is an analytical diagram of the positional relationship between the machine and the predefined path in the present invention. During the driving process of the agricultural machine, the data acquisition method of the position point _Pc , the heading angle _θc and the travel speed v of the agricultural machine at the current moment are as follows :

①Current location point P _c of agricultural machinery: In this embodiment, the current location point coordinates of the agricultural machinery are first obtained through DGPS positioning technology, and then the current location point P _c of agricultural machinery is obtained through dead reckoning or positioning correction;

②Current heading angle θ _c of agricultural machinery: this embodiment uses an electronic compass to obtain it, and converts it into an angle value under the Gaussian plane;

③Current driving speed v of agricultural machinery: First, the current acceleration value of agricultural machinery is obtained by the accelerometer, and then the current traveling speed is obtained through integral calculation.

(2) Determine the target point P′ _p of agricultural machinery according to the predefined path

In this embodiment, a dynamic path search algorithm is used to calculate the optimal look-ahead distance L of the agricultural machinery and the coordinates of the target point, and then determine the target point P′ _p .

The dynamic path search algorithm is based on the preview following theory, by dynamically calculating the optimal look-ahead distance L of the agricultural machinery at the current moment, and determining its target point coordinates on the predefined path, the specific steps are as follows:

(21) Determining the optimum look-ahead distance L of agricultural machinery: the present invention mainly considers the impact of the speed of travel of agricultural machinery on its look-ahead distance, and determines the relationship between travel speed and look-ahead distance through experiments, expressed as:

$\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 2</span>}& \mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; 1.5</span>}\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; the\; s</span>}\\ \mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 0.75</span>}& \mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; 1.5</span>}\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; the\; s</span>}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

In the formula, L is the best forward-looking distance of agricultural machinery at the current moment, and v is the traveling speed;

(22) Determine the coordinates of the target point P′ _p of agricultural machinery: According to the optimal look-ahead distance L determined by the decision, the coordinate calculation formula of the target point P′ _p of agricultural machinery 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 ) represents the coordinates of the target point P′ _p , (x, y) represents the coordinates of the current position point P _c of the agricultural machinery, L represents the optimal look-ahead distance, and θ _p represents the target heading of the agricultural machinery .

As shown in Figure 1, where:

The method to determine the target course θ _p of agricultural machinery is: ${\mathrm{\&theta;}}_p=\stackrel{\&Right\; Arrow;}{P_c{P^{\&prime;}}_p},$ That is, θ _p is the vector of the current position point P _c of the agricultural machinery pointing to the target point P′ _p ;

(3) Determine the current position deviation D _e and heading deviation θ _e of agricultural machinery

See Figure 1, where the current position deviation D _e and heading deviation θ _e of agricultural machinery are defined as follows:

Current heading deviation θ _e of agricultural machinery: defined as the difference between the target heading θ _p of agricultural machinery and the current heading angle θ _c , expressed as θ _e = θ _p - θ _c ;

Agricultural machinery position deviation D _e : defined as the distance between the current position point P _c of agricultural machinery and its projection point P′ _c on the predefined path, expressed as ${\mathrm{D.}}_e=\left|\stackrel{\&Right\; Arrow;}{P_c{P}_c^{\&prime;}}\right|,$ Wherein the projection point P′ _c of the current position point P _c on the predefined path is obtained by finding the intersection of two straight lines perpendicular to each other;

In this embodiment, it is stipulated that when _the agricultural machinery travels along the predefined path direction, if the current position point P _c of the agricultural machinery is on the right side of the predefined path, the position deviation D _e is positive; The left side of the path, the position deviation _De is negative.

Therefore, the judgment method of position deviation D _e and course deviation θ _e direction is as follows:

Positional deviation D _e direction determination method: If the course deviation θ _e is positive, then the positional deviation D _e is positive, otherwise it is negative.

Judgment method of heading deviation θ _e direction: if the heading deviation θ _e is positive, it means that the agricultural machinery is on the right side of the predefined path, otherwise it is on the left side.

(4) The PID controller first determines the initial expected front wheel angle of the agricultural machine according to the position deviation and course deviation of the agricultural machine, and sends instructions to the stepper motor according to the initial expected front wheel angle, and the stepper motor controls the steering mechanism according to the order Make it control the front wheels to steer according to the initial expected front wheel angle. At the same time, collect the actual front wheel angle through the angle sensor. According to the change of the actual front wheel angle of the agricultural machinery, through the parameter self-tuning PID control algorithm, adjust the PID parameters online, and calculate Expect the front wheel angle at the next moment, and send an instruction to the stepper motor, and the stepper motor controls the steering mechanism according to the instruction to control the front wheel to steer according to the expected front wheel angle, repeat the above process to achieve continuous online adjustment of the expected front wheel angle, Complete the auto steering process.

The agricultural machinery automatic steering control step in this embodiment is based on the parameter self-tuning PID controller. The parameter self-tuning PID controller online tunes the PID parameters K _P , T _I , T _D , and calculates the expected front wheel angle at the next moment. The method includes: for:

(41) Initialize the PID control parameters K _P , T _I , T _D and sampling period T of the front wheel rotation angle;

In this embodiment, the incremental PID control algorithm is adopted. According to the simulation test results, the initial values of the PID parameters K _P , T _I , and T _D are set to 75, 0.01, and 8 respectively, and the sampling period T is selected to be 1 second. The PID control method is only related to the latest two front wheel angle deviations. When there is an error or the calculation accuracy is insufficient, the influence on the calculation of the control quantity is small. The purpose of the incremental PID control method is to control the current i agricultural machinery The expected front wheel angle is set, so that the steering is controlled according to the set expected front wheel angle at the next moment, which can be obtained according to the following formula:

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

Among them, u _i , u _i-1 are the current moment i and the expected front wheel angle of the agricultural machinery at the first moment i-1 respectively, e _i , e _i-1 , e _i-2 are the current moment i and the first moment respectively. i-1, the deviation between the expected front wheel angle and the actual front wheel angle at the second moment i-2, wherein, the first moment i-1 is the moment before the current moment i, and the second moment i-2 is the moment before the first moment i At the moment before the moment i-1, K _P is the proportional coefficient, K _I =K _P *T/T _I , K _D =K _P *T _D /T, wherein, T is the sampling period, and the preferred initial value range is 0.1～1s, T _I is integral time constant, T _D is differential time constant, 0≤K _P ≤1000, 0≤T _I ≤0.5.

(42) determine the fuzzy logic rule table and the fuzzy logic control table of the front wheel angle control quantity of PID controller;

In this embodiment, the PID controller adopts an adaptive fuzzy PID controller to realize the online adjustment of the three PID parameters K _P , T _I , and T _D , and then decide the front wheel angle of the agricultural machinery to improve the precision of the automatic steering control of the agricultural machinery and stability.

(421) determine the fuzzy logic rule table of the front wheel angle control quantity of PID controller;

The core of adaptive fuzzy PID controller design is to summarize the driver's technical knowledge and practical operating experience, transform them into fuzzy control rules, establish a fuzzy logic decision table, and realize the decision of PID parameter variation. Considering the error, error change rate and the positive and negative characteristics of the three parameters K _P , K _P , K _P of the PID controller, the variation Δk _p , Δk _i , Δk _d variables, the value range of each variable is divided into 7 kinds, respectively: positive big (PB), positive middle (PM), positive small (PS), zero (ZO), negative small (NS), negative middle (NM) and negative big (NB), and finally get the fuzzy error E , fuzzy error rate of change EC, and the fuzzy control tables of the three parameters of the PID controller, Δk _p , Δk _i , Δk _d (as shown in Table 1, Table 2, and Table 3), Table 1, Table 2, and Table 3 There are 7 grades in the middle corresponding to the positive big PB1, positive middle PM1, positive small PS1, negative small NS1, negative middle NM1 and negative big NB1 of the fuzzy error E, and positive big PB2, positive middle PM2, positive small PS2 corresponding to the fuzzy error change rate EC , negative small NS2, negative medium NM2 and negative large NB2, and there are 7 grades in total, positive large PB3, positive medium PM3, positive small PS3, negative small NS3, negative medium NM3 and negative large NB3 corresponding to the variation Δk _p , Corresponding to the variation Δkp, there are 7 grades including positive large PB4, positive _medium PM4, positive small PS4, negative small NS4, negative medium NM4 and negative large NB4, and positive large PB5, positive medium PM5, positive small PS5, negative Small NS5, negative medium NM5 and negative large NB5 have a total of 7 grades.

This embodiment is not limited to dividing each of the above changes into 7 numerical ranges, and may be further refined into more numerical ranges, but it complies with the fuzzy rules described in the following table.

Table 1 Fuzzy rule table of ΔK _P

Table 2 Fuzzy rule table of ΔK _I

Table 3 Fuzzy rule table of ΔK _D

The parameter setting principle in this embodiment is: the output of fuzzy decision-making is the variation of PID parameters ΔK _P , ΔK _I , ΔK _D . K _P , K _I , K _D are adjusted according to the absolute value |E| of different fuzzy errors E and the absolute value |EC|

★When the fuzzy error |E| is large, such as 3≤|E|≤5, in order to make the system have better tracking performance and speed up the response speed of the system, a larger K _P should be selected, such as 500≤K _P ≤ 1000; at the same time, in order to avoid the differential saturation that may occur due to the instantaneous increase of the error at the initial stage of the system and make the control action exceed the allowable range, a smaller K _D should be selected at this time, such as 0≤K _D ≤3; at the same time, to avoid Large overshoot occurs in the system response, resulting in integral saturation, and the integral action should be limited, usually K _I =0.

★When the _fuzzy error |E| is moderate, such as 1≤|E|≤3, in order to make the system response have a small overshoot, a slightly smaller K _P should be selected, such as 0≤K _P ≤100; The value of KI has a great influence on the system response, and it should be moderate, such as 3≤K _D ≤8. To ensure the response speed of the system; at the same time, it can increase the effect of some integrals on the control, but if K _I is too large, it is easy to cause integral saturation, If it is too small, the system response speed cannot be accelerated, so the value of K _I should be appropriate, such as 0≤K _I ≤0.3.

★When the fuzzy error |E| is small, such as 0≤|E|≤1, in order to make the system have better stability, a larger K _P and K _I should be taken, such as K _P such as 500≤K _P ≤ 1000, such as 0.3≤K _I ≤0.5; at the same time, in order to avoid the system from oscillating near the set value, the choice of K _D value is very important, generally it can be determined according to |EC|: when the fuzzy error change rate |EC| value is small , such as 0≤|EC|≤2, K _D can be larger, such as 5≤K _D ≤10; when |EC| is larger, such as 2≤|EC|≤5, K _D can be smaller, such as 0 ≤K _D ≤5, usually K _D is medium size.

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

In the experimental simulation, the exact values of error e and error change rate e' are fuzzified into fuzzy quantity error E and fuzzy quantity error change rate EC, that is, the exact values of error e and error change rate e' are respectively used as the center value to take a section Numerical range, get fuzzy quantity error E, fuzzy quantity error change rate EC, select fuzzy subset {NB1, NM1, NS1, ZO1, PS1, PM1, PB1} for fuzzy quantity error E, select fuzzy quantity error change rate EC Subset {NB2, NM2, NS2, ZO2, PS2, PM2, PB2}. Select the fuzzy subset {NB3, NM3, NS3, ZO3, PS3, PM3, PB3} of the variation ΔK _P of the PID control parameter K _P , and select the fuzzy subset {NB4, NM4 of the variation ΔK _I of the PID control parameter K _I , NS4, ZO4, PS4, PM4, PB4}, select the fuzzy subset {NB5, NM5, NS5, ZO5, PS5, PM5, PB5} of the variation ΔK _D of the PID control parameter K _D , and take the fuzzy quantity error E and fuzzy Quantitative error change rate EC change range, which is defined as the domain of discourse on fuzzy sets, 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}, where each integer in E' corresponds to the Each integer in EC' corresponds to a range of values in the rate of change of the fuzzy error.

Formulate fuzzy rules based on expert knowledge and field operator experience, open the fuzzy ruler, input different discrete quantities E', EC', and obtain the corresponding values of PID parameter changes ΔK _P , ΔK _I , ΔK _D to form a fuzzy control table (As shown in Table 4, Table 5, and Table 6).

Table 4 Fuzzy control table of ΔK _P

Table 5 Fuzzy control table of ΔK _I

Table 6 Fuzzy control table of ΔK _D

In this embodiment, the three parameters K _P , K _I , and K _P of the initial PID controller are first, K _P is a proportional coefficient, K _I =K _P *T/T _I , K _D =K _P *T _D /T, Among them, T is the sampling period, T _I is the integral time constant, and T _D is the differential time constant. In this embodiment, according to the simulation test results, the initial values of the PID parameters K _P , T _I , and T _D are set to 75, 0.01, and 8 respectively. , select the sampling period T as 1 second, after determining the fuzzy logic rule table and the fuzzy logic control table of the front wheel angle control amount of the PID controller, as shown in Figure 5, combined with the above-mentioned content, it will be described in detail in the agricultural machinery. Automatic steering control process during process:

a: take the current sampling value;

b: Determine the coordinates of the current position point P _c and the target point P′ _p of the agricultural machinery according to the method described above, and determine the position deviation D _e and heading deviation θ _e of the current agricultural machinery;

c: The current expected front wheel angle u _i of the agricultural machine is determined by the position deviation D _e and heading deviation θ _e of the agricultural machine at the current moment, and according to the current expected front wheel angle u _i sends commands to the stepper motor, and the stepper motor follows This command controls the steering mechanism to control the front wheels to steer according to the current expected front wheel angle u _i , and at the same time, collects the actual front wheel angle through the angle sensor. The error between the actual front wheel angle and the current expected front wheel angle is e _i , for Differentiate (de/dt) the error e _i of the current expected front wheel angle to obtain the error change rate e';

d: The error e _i and the error change rate e' of the current expected front wheel angle are input to the PID controller as input quantities, and the PID controller converts the precise value of the error e _i into a fuzzy error E, and the Conversion of exact values to fuzzy error rate of change EC

The input quantities E and EC after gelatinization are used as the input of the fuzzy inference part, and then the fuzzy inference is carried out according to the reasoning synthesis rule from E, EC and the general control rule R to obtain the fuzzy control quantity U as: $u={(\mathrm{E.}\&times;\mathrm{EC})}^{{\mathrm{<figure-callout\; id="1"\; label="T"\; filenames="S2008100564780E00011.png"\; state="\{\{state\}\}">T</figure-callout>}}_1}\&Center\; Dot;R$

Wherein, T ₁ represents a certain control rule, and U represents a corresponding control quantity (respectively representing Δk _p , Δk _i , and Δk _d in the embodiment).

e. Defuzzification: The control quantity calculated by the fuzzy control algorithm is the value in the domain of the language variable of the control quantity, which cannot directly control the object, and must be converted into the value in the basic domain of the control quantity. The scale factor of the control quantity is defined as follows:

k _u =u _max /l

Among them, l is the number of bins after the control quantity is quantized in the range of 0 ~ u _max , and the output fuzzy control quantity U is defuzzified by using the center of gravity method.

In this embodiment, when the error e and the error change rate e' of the system at a certain moment are known, the parameters K _P , _KI , and K _D of the PID controller at the current moment can be adjusted online by determining the value of the parameter variation. ;

f: Adjust the current K _P , K _I , K _D ; K _P ＝K _P ′+ΔK _P , K _I ＝K _I ′+ΔK _I , K _D ＝K _D ′+ΔK _D , among them, K′ _P , K' _I , K' _D are the PID parameters at the previous moment;

g: The PID controller obtains the control parameter Δu _i according to the following formula:

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

Get the expected front wheel angle Δu _i + u _i after setting, and send instructions to the stepper motor according to the expected front wheel angle Wheel turning angle u _i turns to complete one-step control;

h: Then wait for the next sampling, repeat steps b~g, and repeat like this. When the position deviation and heading deviation of the agricultural machinery are within the set error range, the process ends, and the automatic steering control of the agricultural machinery can be completed

As shown in Figure 3, in this embodiment, the adaptive fuzzy PID controller takes the error e between the front wheel angle of the agricultural machinery at the current moment and the expected front wheel angle and the change rate e' of the error e as input quantities, and the agricultural machinery During the running process, the three parameters K _P , T _I , and T _D of the controller are modified online according to the fuzzy logic decision table determined by the fuzzy control principle through continuous detection of e and e', so that it has good dynamic and static performance .

The reasoning method is illustrated below by taking K _P as an example:

(1) According to Table 1, each K _P adjustment rule can be written out, for example, the first rule can be written as: R ₁ : if E=NB and EC=NB then K _P =PS, the calculation of the degree of membership of this rule The method is:

${\mathrm{<span\; class="notranslate">\; \&mu;</span>}}_{{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; P</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&mu;</span>}}_{\mathrm{<span\; class="notranslate">\; NB</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; E.</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; E.</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ^</span>{\mathrm{<span\; class="notranslate">\; \&mu;</span>}}_{\mathrm{<span\; class="notranslate">\; NB</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; EC</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; EC</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

Similarly, the degree of membership μ _Kpi (c _p )(i=1, 2, ..., n) of all rules about K _P can be obtained, wherein, n is the number of all rules about K _P , c _Pi is the central value of the fuzzy set taken in the i-th rule, μ _{NB, E} (E) represents the membership degree when E takes NB, and μ _{NB, EC} (EC) represents the membership degree when EC takes NB.

(2) When the system error E and error change rate EC are known at a certain moment, the calculation formula of K _P is:

${\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; P</span>}}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&mu;</span>}}_{{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; )</span>}{\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; \&mu;</span>}}_{{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; )</span>$

Similarly, calculation formulas of K _I and K _D can be obtained, as shown in (7), (8). Among them, m and l are the numbers of all rules about K _I and K _D respectively.

${\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; m</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&mu;</span>}}_{{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span>}{\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; m</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; \&mu;</span>}}_{{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&mu;</span>}}_{{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; di</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; di</span>}}<span\; class="notranslate">\; )</span>}{\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; l</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; \&mu;</span>}}_{{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; dii</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}<span\; class="notranslate">\; )</span>$

From formulas (5) to (8), it can be seen that K _P , K _I , K _D establish a functional relationship with the fuzzy error E and the fuzzy error change rate EC, which can meet the requirements of the system in different fuzzy errors E, fuzzy The error rate of change EC state has different requirements for PID control parameters, so this controller is superior to conventional PID controllers.

After the fuzzy rule tables of ΔK _P , ΔK _I , and ΔK _D are established, online adjustment of K _P , _KI , and K _D can be performed. Assuming that the fuzzy error E, the rate of change of the fuzzy error EC and ΔK _P , ΔK _I , ΔK _D all obey the normal distribution, the membership degree of each fuzzy subset can be obtained. According to the membership degree assignment table of each fuzzy subset and the fuzzy parameter For the control model, the fuzzy decision table of PID parameter variation is designed by using fuzzy synthetic reasoning, and the corrected parameters are found out and substituted into the following formula for calculation:

K _P ＝K′ _P +ΔK _P , where K′ _P is the PID parameter at the last moment, and ΔK _P can be found from fuzzy rule table 1;

K _I =K' _I +ΔK _I , where K' _I is the PID parameter at the last moment, and ΔK _P can be found from fuzzy rule table 2;

K _D =K' _D +ΔK _D , where K' _D is the PID parameter at the last moment, and ΔK _P can be obtained from fuzzy rule table 3.

The range of system error E and error rate of change EC is defined as a discrete domain on fuzzy sets, 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}, open the fuzzy regularizer, input different discrete quantities E, EC to get the corresponding ΔK _P , ΔK _I , ΔK _D to form a fuzzy control table (as shown in Table 4, Table 5, and Table 6).

The block diagram of the mechanical automatic steering control system provided by the present invention is as shown in Figure 2. This system is based on the parameter self-tuning PID controller. In the present embodiment, the parameter self-tuning PID control part mainly consists of the following parts:

①Fuzzy controller: as the core of the fuzzy control system, it is a language-based fuzzy controller based on fuzzy control knowledge representation and rule reasoning. It mainly includes three parts: fuzzification of input, fuzzy reasoning and defuzzification. According to the different controlled objects and the different requirements for the static and dynamic characteristics of the system, the rules of the fuzzy controller are also different, so the control algorithms are different;

② Executing agencies: including DC motors and stepping motors, etc.;

③Controlled object: It can be a kind of equipment or device and their groups, working under certain constraints;

④Sensor: It is necessary to set the sensor to convert the controlled object or the controlled quantity of various processes into an electrical signal;

⑤D/A converter realizes digital/analog conversion and analog/digital conversion.

While the present invention has been shown and described collectively in conjunction with the above preferred embodiments, it will be understood by those skilled in the art that various changes may be made therein, both in form and in detail, without departing from the spirit of the invention Reality and scope of patent protection.

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