CN104571109A - Agricultural vehicle independent navigation steering control method based on disturbance observer - Google Patents

Abstract

The invention discloses an agricultural vehicle independent navigation steering control method based on a disturbance observer, and belongs to the field of high-precision tracking control of agricultural vehicle independent navigation control. The method adopts a closed loop control mode, the practical lateral deviation of a vehicle is roughly calculated by an open loop, correction is carried out by a closed loop, an accumulative error is eliminated until accurate tracking is achieved, and a whole control cycle is achieved. In view of the diversification of an agricultural vehicle motion model, the adoptive control means are also changeable, a pole assignment mode is utilized to achieve an accuracy control over a three order linear and nonlinear model. According to the agricultural vehicle independent navigation steering control method based on the disturbance observer, the control effect, under the setting of the corresponding control means, of the agricultural vehicle motion control method can reach the fact that the overshoot is less than 5% and the adjustment time is less than 3 seconds, and tracking without a steady-state error can be finally achieved.

Description

A kind of agri-vehicle independent navigation rotating direction control method based on disturbance observer

Technical field

The invention belongs to the research field that agri-vehicle intelligence path navigation and high precision control, be specifically related to the Controller gain variations of agri-vehicle intelligent navigation and the design of the jamproof disturbance observer of active.

Background technology

The design of agri-vehicle intelligent navigation controller can consider the feature of system multivariate, non-linear, time variation, and utilize various intelligent control scheme: comprise fuzzy algorithm, integer rank Fractional Order PID algorithm, designs further to system.The control program of optimization specifically can be proposed according to the feature of system; In the process of agri-vehicle independent navigation, the change of uneven, the actuator load torque on the uncertainty of the structure of soil and soft or hard degree, random noise, road surface and friction, the operation of vehicle is affected by the external environment and the vibration of model itself, all can cause on the performance of system the impact that can not estimate.Therefore need the disturbance observer of initiatively Anti-Jamming Technique to carry out the design of wave filter according to concrete control object, guarantee system itself has good robust performance.

Agri-vehicle autonomous navigation technology mainly comprises: the design of the foundation of navigation sensor, path planning, auto model, the design of tracking control unit and disturbance observer.Agricultural vehicle farm work environment requires very high to the stuctures and properties of system, and therefore the design of tracking control unit and disturbance observer is just attracting the research interest of more and more scholar, and has done a large amount of work for this reason.

Domestic and international research shows, agri-vehicle working environment is very complicated, its motion is by the impact of many non-linear factors such as physical environment, edaphic condition, load variations, it is steady, accurate, fast that this just requires that the control system designed not only will meet track path, also will consider the uncertain factor of other influences farm machinery navigation precision.Such as: the perturbation of system model parameter, the external disturbance of low frequency and the measurement noises of high frequency, the uncertain saturated of controller is all the problem needing research.In traditional control strategy, PID is control device the most frequently used in agri-vehicle automatic navigation control system.Its control effects is relatively stable, technology maturation.But its time become control system and nonlinear system in have some limitations.The i.e. PID controller design of this control strategy attention location system tracking accuracy, and the NONLINEAR PERTURBATION of model parameter, a series of uncertain factor such as external disturbance, the measurement noises of high frequency, the saturated of controller of low frequency of ignoring or eliminated by FEEDBACK CONTROL merely system existence itself.The high precision that this control strategy requires day by day for agri-vehicle automatic navigation control system, interference rejection capability, robust stability is obviously not enough.Just based on this background, it is necessary and urgent for finding new control method and prioritization scheme.

Summary of the invention

The invention provides the method for designing of the agri-vehicle independent navigation model based on disturbance observer.Adopt different control strategies for vehicle nonlinear motion model structure simultaneously, for the path that high precision tracking is given, realize the intelligent precision navigation of agri-vehicle.The control system of design can realize DAZ gene, and regulation time is all within the scope of 3s, and overshoot, all within 5%, adds the system after disturbance and still can keep original dynamic quality and steady-state behaviour.

In order to realize above-mentioned goal of the invention, an object of the present invention is, provides a kind of and stablizes and the path tracking control method of the agri-vehicle independent navigation of practicality.Be specially: a kind of agri-vehicle independent navigation rotating direction control method based on disturbance observer, is characterized in that, comprise following several step:

1) first according to the situation of the actual navigation needs of agri-vehicle, the control flow design drawing of agri-vehicle path following control system is drafted;

2) agri-vehicle path following control system is set up, by steering controller G _c(s), disturbance observer Q (s) and these parts of vehicle kinematics model G (s) are formed;

3) combine actual demand for control, what set agri-vehicle path following control system is input as control variable u, exports as lateral deviation y, now sets desired throughput y ₁=u=1;

4) agri-vehicle path following control systematic research object is determined: the kinematics model of agri-vehicle; Now the kinematics model of agri-vehicle gets the three rank linear models and third-order non-linear model that O ' Connor proposes, and is wherein for the ease of the research to third-order non-linear model for the model treatment that three rank are linear;

5) based on three rank linear models, the controller G of POLE PLACEMENT USING design system is utilized _c(s), and this control device is used for the control of third-order non-linear model and adjusts;

6) on vehicle third-order non-linear model basis, the three class disturbances that drawing-in system essence exists: model error Δ (s), external interference d, measurement noises ζ, these three kinds of disturbances are carried out to the simulation of various combination simultaneously, analyze their impacts on agri-vehicle path following control system;

7) in the controller G that step 5 designs _con (s) basis, add the design of disturbance observer Q (s), now design two kinds of disturbance observers, be respectively Q _b(s) and Q _b(s), under the effect of these two kinds of control devices, the active suppression ability that discussing system disturbs to external world, and the control effects of comparative analysis two kinds of disturbance observers;

8) utilize DSP and existing agri-vehicle platform, build a kind of agri-vehicle path following control device: comprise master controller and path trace closed-loop sensors and agri-vehicle hardware platform.

Further, described step 1) in, the control flow design drawing of agri-vehicle path following control system is specially: first set vehicle tracking target y ₁with determine control inputs u, next chooses control decision CONTROLLER DESIGN G _c(s) and disturbance observer Q (s); Then agri-vehicle starts actual motion, determines actual tracking effect y, the actual motion effect of vehicle and expectation target is made comparisons: u-y simultaneously; Now system starts the comprehensive analysis of performance: calculate regulation time t respectively _swith overshoot σ %, if meet system requirements, then design effort terminates; If discontented pedal system requirement, then continue adjustment control decision part, till meeting system requirements.

Further, described step 2) in, agri-vehicle path following control system is made up of three parts: controller G _c(s), disturbance observer Q (s), agri-vehicle kinematics model G (s); According to known technology, agri-vehicle kinematics model is made up of topworks and controlled device two parts, and the derivation relation now exporting lateral deviation y and input u is as follows:

$y=\frac{G_c\left(s\right)G\left(s\right)}{1+G_c\left(s\right)G\left(s\right)}u---\left(1\right)$

Namely the relation of control inputs u and output lateral deviation y is the relationship based on signal transfer function, controller G _cthe physical relationship determining both input and output of (s).

Further, described step 3) in, the control inputs of system is u, and the output of system is lateral deviation y, desired output y ₁=u=1; The evaluation index of system is: regulation time t _swith overshoot σ %; Regulation:

T _swherein, specification error band is ± 0.05 (2) to < 3s, σ % < 5%

For meeting the performance index of system requirements, if meet, terminating design, if do not meet, continuing adjustment control decision part.

Further, described step 4) in, agri-vehicle kinematics model adopts three rank linear models and third-order non-linear model, and wherein linear model proposes for the ease of analyzing nonlinear model; According to known technology, three rank linear models can describe with state equation, as follows:

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{y}\\ \stackrel{\&CenterDot;}{\mathrm{\&Psi;}}\\ \stackrel{\&CenterDot;}{\mathrm{\&delta;}}\end{array}\right]=\left[\begin{array}{ccc}0& V_x& 0\\ 0& 0& \frac{V_x}{L}\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\&Psi;}\\ \mathrm{\&delta;}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]u---\left(3\right)$

$y=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\&Psi;}\\ \mathrm{\&delta;}\end{array}\right]---\left(4\right)$

Wherein car speed V _xget 1m/s, wheelbase L gets 2.314m; Now according to known technology, three rank linear models can also describe with transport function G (s):

$G\left(s\right)=\frac{Y\left(s\right)}{d\left(s\right)}=\frac{0.432}{s^3}---\left(5\right)$

Utilize the three rank linear models obtained to carry out the design of controller, and the controller of design is used for go during agri-vehicle third-order non-linear master mould is adjusted.

Further, described step 5) in, based on three rank linear models, utilize POLE PLACEMENT USING to obtain state feedback matrix, thus obtain controller G _c(s).Now need the controllability of judgement three rank linear model, controllability judgment matrix is:

$\mathrm{rank}\left[\begin{array}{cccc}B& \mathrm{AB}& A^2& B\end{array}\right]=\mathrm{rank}\left[\begin{array}{ccc}0& 0& \frac{{V_x}^2}{L}\\ 0& \frac{V_x}{L}& 0\\ 1& 0& 0\end{array}\right]=3---\left(6\right)$

According to known technology, now system is pure monopoly market, can pass through structural regime feedback matrix K, realize the arbitrary disposition of system pole; The closed-loop pole that now regulation system is expected is:

λ ₁ ^*＝-5，λ ^* _2,3＝-1±j (7)

The state feedback matrix K tried to achieve is:

K＝[23.148 27.778 7] (8)

After POLE PLACEMENT USING, the controller G of system _c(s) be:

$G_c\left(s\right)=\frac{{10s}^2}{0.432s^2+3.024s+5.184}---\left(9\right)$

Now third-order system model is at controller G _c(s) adjust under performance index be:

t _s＝2.3s,σ％＝4.2％ 。(10)

Further, described step 6) in, inevitably exist in agri-vehicle independent navigation course changing control: model error Δ (s), external interference d, measurement noises ζ; According to known technology, now need to discuss external interference d and measurement noises ζ to the impact exporting y,

$y_1=G_{\mathrm{uy}}\left(s\right)u=\frac{G_P\left(s\right)G_n\left(s\right)}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}u---\left(11\right)$

$y_2=G_{\mathrm{dy}}\left(s\right)d=\frac{G_P\left(s\right)G_n\left(s\right)[1-Q\left(s\right)]}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}d---\left(12\right)$

$y_3=G_{\mathrm{zy}}\left(s\right)z=\frac{G_P\left(s\right)Q\left(s\right)}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}z---\left(13\right)$

y＝y ₁+y ₂+y ₃＝G _uy(s)u+G _dy(s)d+G _zy(s)z (14)

From known technology: G _ps () is system realistic model, G _ns nominal model that () is system; Formula (11) represents that system expects that the impact that input exports system, formula (12) represent that the impact that external disturbance exports system, formula (13) represent the impact that measurement noises exports system; Now need: y ₂, y ₃be 0, guarantee y=y ₁, so just can ensure system not by external disturbance and the impact of measuring disturbance; Adopt the technological means of disturbance observer Q (s) to achieve this end, and Q (s) can ensure performance and the robust stability of system.

Further, described step 7) in, agri-vehicle third-order non-linear master pattern, what exist due to itself is non-linear, and the now system itself that result in exists model error Δ (s), first solves the impact that model error brings to system; Controller G is utilized merely by claim 6 is known _cs (), cannot realize the calm of system; Now need the impact adopting disturbance observer Q (s) this technological means to cause to overcome system model error delta (s) system; According to known technology, disturbance observer Q (s) generally adopts Butterworth type Q _b(s) and binomial coefficient type Q _b(s):

$Q_B\left(s\right)=\frac{P_{n-k}\left(s\right)}{P_n\left(s\right)}---\left(15\right)$

Now get cutoff frequency: 10Hz, 50Hz, 150Hz; According to known technology, in conjunction with the consideration of robust stability and AF panel and design cost, getting cutoff frequency is 50Hz, and time constant is 0.003s; Now Q _b(s) and Q _bs () is respectively:

$Q_B\left(s\right)=\frac{1}{{0.003}^3{s}^3+2*{0.003}^2{s}^2+2*0.003s+1}---\left(17\right)$

$Q_b\left(s\right)=\frac{1}{{0.003}^3{s}^3+3*{0.003}^2{s}^2+3*0.003s+1}---\left(18\right)$

Disturbance observer Q (s) after design and controller G _cs, under () acting in conjunction, model error Δ (s) of agri-vehicle three rank master pattern obtains and effectively overcomes; Under two kinds of disturbance observers, the performance index of system are respectively:

t _s＝2.32s,σ％＝2.1％(Q(s)＝Q _B(s)) (19)

t _s＝2.32s,σ％＝4.2％(Q(s)＝Q _b(s)) (20)

Below will at disturbance observer Q (s) and controller G _cunder (s) coefficient prerequisite, the multi-form and various combination impact that may cause system of model error Δ (s), external disturbance d, measurement noises ζ is discussed.From third-order non-linear model, model error Δ (s) is that essence exists; The typical existing way of external disturbance d has three kinds:

Low frequency sinusoidal external disturbance:

Unit pulse external disturbance: d ₂=δ (t) (22)

Random external disturbs: d ₃, mean=0, variance=0.5 (23)

The typical existing way of measurement noises ζ is high frequency measurement noise:

Now at Butterworth type disturbance observer Q _b(s) and controller G _cs, under () acting in conjunction, the performance of system does not all reach: t _sthe requirement of < 3s, σ % < 5%, and at binomial coefficient type disturbance observer Q _b(s) and controller G _cs, under () acting in conjunction, the performance of system is:

t _s＝2.34s,σ％＝4.3％ (25)

This kind of technological means, can ensure y=u=1; Add the design of disturbance observer Q (s), not only can overcome the model error of system itself, system can also be made to have the ability of Active Compensation for external interference and measurement noises.It is worthy of note, it contributes to performance index and the robust property of improvement system simultaneously, therefore based on the rotating direction control method technical feasibility of agri-vehicle independent navigation, and meets the actual conditions of automobile navigation.

Described method comprises: building of agri-vehicle path trace closed-loop control system, by the continuous correction of error, closed-loop corrected, eliminates cumulative errors, until precisely follow the tracks of, realizes the object of DAZ gene.The auto model building method adopted comprises: by the non-linear vehicle movement model on three rank, the linear vehicle motion model on three rank, these two kinds of models are determining based on people such as O ' Connor, describe the validity of this system model through a large amount of test figures and emulation.Therefore two kinds of models are all reliable and stable, are suitable as the foundation of systematic analysis.For three rank linear systems, adopt the method for feedback of status, by the mode of POLE PLACEMENT USING, achieve the design of system equivalent controller; Utilize equivalent controller and disturbance observer, achieve the calm of Third Order Nonlinear System control effects.Simultaneously for non-linear master pattern design Anti-Jamming Technique: comprise closed-loop system itself and suppress interference and the disturbance observer based on the design of active Anti-Jamming Technique.The controller of comprehensive analysis design system and disturbance observer are key points of the present invention.

Another object of the present invention is to, provide a kind of agri-vehicle path following control device, it is made up of master controller and Closed loop track sensor two parts.Master controller comprises: DSP control circuit, RTC real-time clock, digital signal input-output unit, simulating signal input block, signal amplification circuit, external memory storage, human-computer interaction device communicate with supervisory control comuter with display device, watchdog circuit, asynchronous machine, a road serial communication interface, and a road serial communication interface communicates with deflecting roller drift angle scrambler.Closed loop track sensor converts the lateral deviation signal of reality to electric signal, poor with given deflecting roller declination signal, and by signal amplification circuit amplification signal, through simulating signal input block, is gathered by DSP control circuit.

Compared with existing design, the present invention has the following advantages: (1) combines the advantage that open and close ring controls, and namely there will not be cumulative errors, also can not cause the inefficacy of tracker because of the interference of vehicle operating reality.(2) multi-modelization design, for different control objects and model, takes corresponding control strategy, ensure that dynamic quality and the steady-state behaviour of system, tracking control algorithm can ensure that the system call interception time is all less than 3s, and overshoot is no more than 5%, finally realizes DAZ gene.(3) add multiple interference to design system, the disturbance observer design of proposition, can effectively suppress these lumps to be disturbed, and guarantees that the performance of system is not by the impact of disturbance.(4) adopt DSP as processor, utilize its control algolithm being complexity and powerful arithmetic speed to provide technical support, improve precision and the speed of agri-vehicle path trace.(5) multi-model multi-functional multimode blocking process, system is convenient to integrating control, can realize further commercialization, effectively change yield-power into.(6) adopt the control thought of Active Compensation interference, widened the design dimension of control system, added the subjective initiative of system.

Accompanying drawing explanation

Fig. 1 is control principle design flow diagram of the present invention.

Fig. 2 is the design drawing of agri-vehicle path trace closed-loop control system.

Fig. 3 is the physical prototype figure of third-order model.

Fig. 4 is the state equation of agri-vehicle three rank motion.

Fig. 5 is the inner structure schematic diagram of closed-loop system after POLE PLACEMENT USING.

Fig. 6 is the system emulation figures of three rank linear systems under POLE PLACEMENT USING.

Fig. 7 is the system emulation figure of Third Order Nonlinear System under Pole Assignment Controller effect.

Fig. 8 and Fig. 9 is the ultimate principle figure of disturbance observer.

Figure 10 and Figure 11 is two kinds of wave filters at three kinds of frequency curve charts under frequency.

Figure 12 is the simulation comparison figure of Third Order Nonlinear System under two kinds of filter constructions.

Figure 13 and Figure 14 and Figure 15 introduces the simulated effect comparison diagram of Third Order Nonlinear System under two kinds of disturbance observers of three kinds of external disturbance.

Figure 16, Figure 17, Figure 18, Figure 19 are the simulated effect comparison diagram of Third Order Nonlinear System under two kinds of disturbance observer effects introducing external disturbance and measurement noises.

Figure 20 is the design drawing of agri-vehicle independent navigation path trace device.

Embodiment

Below in conjunction with accompanying drawing, the specific embodiment of the present invention is described.

In view of the agri-vehicle automatic navigation control system model of research is nonlinear in essence, so first discuss the control program of three rank linear systems.By being optimized research to the analysis and design of three rank linear models to non-linear master pattern.Third-order system is divided into nonlinear model and linear model, and the controller designed under the thought of design is linear model and observer may be used for non-linear master pattern equally.

Three rank Linear system models as shown in Figure 3 and Figure 4, are expressed according to state space, and utilization state feedback carries out POLE PLACEMENT USING: the equation of motion of tractor is as follows

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{y}\\ \stackrel{\&CenterDot;}{\mathrm{\&Psi;}}\\ \stackrel{\&CenterDot;}{\mathrm{\&delta;}}\end{array}\right]=\left[\begin{array}{ccc}0& V_x& 0\\ 0& 0& \frac{V_x}{L}\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\&Psi;}\\ \mathrm{\&delta;}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]u$

$y=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\&Psi;}\\ \mathrm{\&delta;}\end{array}\right]$

System matrix $A=\left[\begin{array}{ccc}0& V_x& 0\\ 0& 0& \frac{V_x}{L}\\ 0& 0& 0\end{array}\right],$ Input matrix $B=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right].$ Judge the controllability of this system.Ask feedback of status gain matrix K, make feedback closed loop eigenwert be l ₁ ^*=-5, l ^* _2,3=-1 ± j, the leading pole chosen is best performance under standard second order system.

Separate: (1) $\mathrm{rank}\left[\begin{array}{cccc}B& \mathrm{AB}& A^2& B\end{array}\right]=\mathrm{rank}\left[\begin{array}{ccc}0& 0& \frac{{V_x}^2}{L}\\ 0& \frac{V_x}{L}& 0\\ 1& 0& 0\end{array}\right]=3,$ Visible given system is pure monopoly market, can arbitrary disposition Closed-loop Eigenvalues by state feedback control law u=v-Kx.Ask feedback of status gain matrix K below.

(2) the hope proper polynomial of closed-loop system is determined

The hope limit of closed-loop system is λ ₁ ^*=-5, λ ^* _2,3=-1 ± j, can determine the hope proper polynomial of closed-loop system thus:

f ^*(s)＝(s-l ₁ ^*)(s-l ₂ ^*)(s-l ₃ ^*)＝(s+5)(s+1+j)(s+1-j)＝s ³+7s ²+12s+10

(3) state feedback matrix of POLE PLACEMENT USING task has been determined

If state feedback matrix is

K＝[k ₁k ₂k ₃]

Obtain closed loop proper polynomial

f(s)＝det(sI-A+BK)

$f\left(s\right)=\frac{{\mathrm{Ls}}^3+k_3*{\mathrm{Ls}}^2+k_2{V}_{x}s+k_1{V_x}^2}{L}$

Here adopt the parameter of Dongfanghong-X804 tractor, namely wheelbase gets L=2.314m, and speed is got

V _x＝1m/s。With seasonal f (s)=f ^*s (), obtains by the same power coefficient of s is equal:

k ₁＝23.148，k ₂＝27.778，k ₃＝7

(4)K＝[23.148 27.778 7]

So feedback of status gain matrix K can obtain.Now the state-space expression of closed-loop system is:

$\begin{array}{c}\stackrel{\&CenterDot;}{x}=(A-\mathrm{BK})x+\mathrm{Bv}\\ =\{\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 0.432\\ 0& 0& 0\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\left[\begin{array}{ccc}23.148& 27.778& 7\end{array}\right]\}x+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]v\\ =\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 0.432\\ -23.148& -27.778& -7\end{array}\right]x+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]v\end{array}$

y＝[1 0 0]x

Can verify that the eigenwert of closed-loop system is λ really ₁ ^*=-5, λ ^* _2,3=-1 ± j.And closed loop transfer function, matrix is: G (s)=C [sI-A+BK] ^-1b

$\frac{0.432}{s^3+{7s}^2+12s+10}$

Now internal system structure principle chart as shown in Figure 5.

Note: from the inner structure schematic diagram of system, system experienced by the process of second order anticipatory control, amplitude and energy attenuation are 0.0432, namely now the steady-state value of system is 0.0432, in order to the convenience of simulation analysis, before whole closed-loop system, seal in a power amplification link, be 23.148 by the known amplifying element coefficient of calculating, guarantee that the steady-state value of system is 1.Namely unity closed loop ssystem transfer function is now:

$\frac{10}{s^3+{7s}^2+12s+10}$

Closed loop three rank State Feedback for Linear Systems calm after time domain effect by emulation known: system call interception regulation time is 2.3s, and overshoot is 4.2%.The unit-step response of three rank linear closed-loop systems as shown in Figure 6.

According to closed-loop system and the open-loop transfer function of POLE PLACEMENT USING, concrete structure and the parameter of controller can be obtained.

The controller of system is made to be G _cs (), open-loop transfer function is C, and closed loop transfer function, is X.Then have

$\frac{{\mathrm{CG}}_c\left(s\right)}{1+{\mathrm{CG}}_c\left(s\right)}=X$

Then the result of controller is:

$G_c\left(s\right)=\frac{X}{C-\mathrm{CX}}$

Known X is:

$\frac{10}{s^3+{7s}^2+12s+10}$

C is:

$G\left(s\right)=\frac{Y\left(s\right)}{d\left(s\right)}=\frac{0.432}{s^3}$

Then controller G _cs the version of () is:

$\frac{10}{{0.432s}^2+3.024s+5.184}$

Utilize the equivalent controller to three rank linear system POLE PLACEMENT USING, carry out the calm of control effects to Third Order Nonlinear System, because nonlinear system is by the impact of initial condition, there is instability in the control effects of controller to nonlinear system.Equivalent controller to the time-domain-simulation design sketch of Third Order Nonlinear System as shown in Figure 7.

Comparative analysis: the equivalent controller designed under three rank linear systems does not have control effects superior equally for Third Order Nonlinear System, the controller only by equivalence is not enough to the dynamic quality and the steady-state behaviour that ensure nonlinear system model.Therefore be necessary new technological means to be discussed to realize adjusting further of nonlinear system.Hereafter introducing disturbance observer overcomes the impact that nonlinear model error is brought to system.

The analysis of causes: third-order non-linear model is there is systematic error in essence, and this error result in the uncertain of Systematical control quality and instability.This model error requires that the technological means introduced hereafter overcomes.

Interference ACTIVE CONTROL carries out feedforward compensation design targetedly according to the measured value disturbed or estimated value.Its feature is can to Unmarried pregnancy, and model parameter perturbs, external disturbance, and these lumps of measurement noises interference is all taken into account and carried out estimation and finally realize Compensation Design.And it introduces feedforward, relative to feedback, it can suppress interference faster, so interference ACTIVE CONTROL causes many researchers study interest greatly.The difficult point of its research is just itself measurement and estimation.

Disturbance observer is a kind of effective Interference Estimation measuring method, and the range of application of feedforward control-interference ACTIVE CONTROL has greatly been widened in its appearance, and it makes the Disturbance Rejection can not surveyed, not easily survey become possibility.To the Immunity Performance improving system, there is great benifit.

As shown in Figure 8 and Figure 9, its core is realistic model, nominal plant model for the basic structure of disturbance observer and principle, the choosing and designing, the three rank linear systems for above-mentioned of wave filter:

Realistic model is: $G\left(s\right)=\frac{Y\left(s\right)}{d\left(s\right)}=\frac{0.432}{s^3},$ Nominal plant model: $G\left(s\right)=\frac{Y\left(s\right)}{d\left(s\right)}=\frac{0.432}{{1.003s}^3}$

The Robust Stability of wave filter is: || Δ (s) Q (s) || _∞≤ 1, wave filter often adopts Butterworth type and binomial coefficient type, and concrete structure is as follows:

$Q_B\left(s\right)=\frac{P_{n-k}\left(s\right)}{P_n\left(s\right)}$

Structure according to the known two kinds of wave filters of nominal plant model is:

$Q_B\left(s\right)=\frac{1}{t^3{s}^3+2t^2{s}^2+2\mathrm{ts}+1}$

$Q_b\left(s\right)=\frac{1}{t^3{s}^3+3t^2{s}^2+3\mathrm{ts}+1}$

Stability condition || Δ (s) Q (s) || _∞≤ 1, get cutoff frequency 10Hz, 50Hz, 150Hz to judge the stability condition of two kinds of wave filters, under three kinds of cutoff frequencys, all meet stability condition as seen by Figure 10 and Figure 11.For the consideration of AF panel and design cost, getting cutoff frequency is 50Hz.Now two kinds of wave filters are respectively:

$Q_B\left(s\right)=\frac{1}{{0.003}^3{s}^3+2*{0.003}^2{s}^2+2*0.003s+1}$

$Q_b\left(s\right)=\frac{1}{{0.003}^3{s}^3+3*{0.003}^2{s}^2+3*0.003s+1}$

The controller designed under above utilizing linear model can not realize the effectively calm of nonlinear model, so emulate Third Order Nonlinear System according to two kinds of wave filters of trying to achieve, the system emulation Contrast on effect under two kinds of filter constructions as shown in figure 12.Under the effect of Butterworth mode filter, the time domain index of nonlinear system model is: regulation time is 2.32s, and overshoot is 2.1%, and steady-state value is 0.98; Under the effect of binomial coefficient mode filter, the time domain index of nonlinear system model is: regulation time is 2.32s, and overshoot is 4.2%, and steady-state value is 1.Visible nonlinear system model is under two kinds of disturbance observer effects, and the model error that nonlinear system itself exists obtains and effectively overcomes, and the dynamic quality of nonlinear system and steady-state behaviour obtain sufficient guarantee.

Actual agri-vehicle automatic navigation control system is inevitably subject to the impact of external disturbance and sensor measurement noise and vehicle vibration itself.So be necessary these interference to be incorporated in the middle of the auto model of research, by carrying out different combinations to these interference, study two kinds of disturbance observers to the inhibition of these disturbances, drawing some concrete conclusions.

These disturbances are roughly made up of three parts: the model error of vehicle, as the error that the shake of vehicle and the foundation of auto model bring; The external disturbance of agricultural operation environment: the structure of soil and soft or hard degree, serrate and be approximately the change (wind, Exposure to Sunlight, drench with rain) of sine-shaped road conditions, random signal, the gradient of excess surface water and low-lying, landform, vehicle and the instant shock of barrier, the rolling of vehicle and pitching, external environment condition; The measurement noises of sensor: the noise that electromagnetic interference (EMI), sensor fusion and measurement bring.Existing control theory and technological means are all by the conservative and mode Power suppressing of high cost or offset the impact of these disturbances on system.The present invention is intended to the thought by active disturbance compensation, goes these disturbances of Active Compensation by the design of disturbance observer.

The working environment of agri-vehicle is very complicated, and generally we go to simulate with Low Frequency Sine Signals, random signal, pulse signal the impact that external disturbance causes system.And under two kinds of wave filters, simulation external disturbance is on the impact of system, and system emulation Contrast on effect is as shown in Figure 13, Figure 14, Figure 15.

In actual physical environment, except above-mentioned external disturbance, in the process of sensor measurement and fusion, there is series of noise, usually carry out with high frequency sinusoidal measurement noises, electromagnetic interference (EMI), Gauss measurement noise the impact that analogue measurement noise causes system.During agri-vehicle real work, model error, agricultural environment external disturbance, sensor measurement noise act on simultaneously.Therefore be necessary these simulating signals to carry out organic assembling, and they are joined the corresponding region of influential system, under two kinds of disturbance observer effects, the performance that Study system is concrete.

Several combination is respectively: external low frequency sinusoidal interference+model error+high frequency sinusoidal measurement noises; External unit pulse signal+model error+high frequency sinusoidal measurement noises; Random external signal+model error+high frequency sinusoidal measurement noises; Model error+high frequency sinusoidal measurement noises.Under the organic assembling of these disturbances, the time-domain-simulation design sketch of nonlinear system is as shown in Figure 16, Figure 17, Figure 18, Figure 19.

Comparative analysis: can be found out by simulated effect comparison diagram above, when tackling the model error of Third Order Nonlinear System, Butterworth mode filter and binomial coefficient mode filter show superior control effects.Performance index: regulation time is 2.32s; Overshoot is respectively 2.1%, and 4.2%.When tackling model error and the acting in conjunction of agricultural environment external disturbance, the performance of two kinds of disturbance observers presents otherness.System responses curve under Butterworth mode filter cannot reach the steady-state value of expectation, do not have actual value, and the system responses curve under binomial coefficient mode filter reaches the control effects of expection.When tackling the organic assembling of model error and agricultural environment external disturbance and sensor measurement noise, the performance of binomial coefficient mode filter still can the impact that brings to system of this three classes disturbance of Active Compensation.System responses curve under the effect of Butterworth mode filter still cannot reach the steady-state value of expectation.

This two classes disturbance observer of Integrated comparative, for agri-vehicle automatic navigation control system, selects the disturbance observer of binomial coefficient type more to meet the reality of agricultural operation environment.It also has more generality and stability to the control of non-linear master pattern and design.

Another object of the present invention is to, provide a kind of agri-vehicle path following control device, it is made up of master controller and Closed loop track sensor two parts.Master controller comprises: DSP control circuit, RTC real-time clock, digital signal input-output unit, simulating signal input block, signal amplification circuit, external memory storage, human-computer interaction device communicate with supervisory control comuter with display device, watchdog circuit, asynchronous machine, a road serial communication interface, and a road serial communication interface communicates with deflecting roller drift angle scrambler.Concrete model structure as shown in figure 20.Reach the double effects of hardware design and software control.Adopt RS485 interface to be serial communication interface, keyboard is human-computer interface device, and liquid crystal (LCD) is display device.Its circuit connecting mode of Closed loop track sensor is: lateral deviation during its collection agri-vehicle path trace, poor with given deflecting roller declination signal, and by signal amplification circuit, electric signal is amplified, DSP control circuit detects electric signal, by the control algolithm provided in invention, drive motor rotates.Supervisory control comuter is communicated by Phototube Coupling RS485 Interface and Controler with deflecting roller drift angle scrambler.External memory storage is connected with DSP control circuit by dsp bus.Keyboard comprises the function such as numeral input, radix point input, up and down page turning, confirmation, cancellation.LCD shows liquid crystal menu, connects the output pin of DSP control circuit.

Above embodiment is to illustrate the invention and not to limit the present invention.

Claims (8)

1. based on an agri-vehicle independent navigation rotating direction control method for disturbance observer, it is characterized in that, comprise following several step:

1) first according to the situation of the actual navigation needs of agri-vehicle, the control flow design drawing of agri-vehicle path following control system is drafted;

2) agri-vehicle path following control system is set up, by steering controller G _c(s), disturbance observer Q (s) and these parts of vehicle kinematics model G (s) are formed;

3) combine actual demand for control, what set agri-vehicle path following control system is input as control variable u, exports as lateral deviation y, now sets desired throughput y ₁=u=1;

4) agri-vehicle path following control systematic research object is determined: the kinematics model of agri-vehicle; Now the kinematics model of agri-vehicle gets the three rank linear models and third-order non-linear model that O ' Connor proposes, and is wherein for the ease of the research to third-order non-linear model for the model treatment that three rank are linear;

5) based on three rank linear models, the controller G of POLE PLACEMENT USING design system is utilized _c(s), and this control device is used for the control of third-order non-linear model and adjusts;

6) on vehicle third-order non-linear model basis, the three class disturbances that drawing-in system essence exists: model error Δ (s), external interference d, measurement noises ζ, these three kinds of disturbances are carried out to the simulation of various combination simultaneously, analyze their impacts on agri-vehicle path following control system;

7) in the controller G that step 5 designs _con (s) basis, add the design of disturbance observer Q (s), now design two kinds of disturbance observers, be respectively Q _b(s) and Q _b(s), under the effect of these two kinds of control devices, the active suppression ability that discussing system disturbs to external world, and the control effects of comparative analysis two kinds of disturbance observers;

8) utilize DSP and existing agri-vehicle platform, build a kind of agri-vehicle path following control device: comprise master controller and path trace closed-loop sensors and agri-vehicle hardware platform.

2. a kind of agri-vehicle independent navigation rotating direction control method based on disturbance observer according to claim 1, it is characterized in that, described step 1) in, the control flow design drawing of agri-vehicle path following control system is specially: first set vehicle tracking target y ₁with determine control inputs u, next chooses control decision CONTROLLER DESIGN G _c(s) and disturbance observer Q (s); Then agri-vehicle starts actual motion, determines actual tracking effect y, the actual motion effect of vehicle and expectation target is made comparisons: u-y simultaneously; Now system starts the comprehensive analysis of performance: calculate regulation time t respectively _swith overshoot σ %, if meet system requirements, then design effort terminates; If discontented pedal system requirement, then continue adjustment control decision part, till meeting system requirements.

3. a kind of agri-vehicle independent navigation rotating direction control method based on disturbance observer according to claim 1, is characterized in that, described step 2) in, agri-vehicle path following control system is made up of three parts: controller G _c(s), disturbance observer Q (s), agri-vehicle kinematics model G (s); According to known technology, agri-vehicle kinematics model is made up of topworks and controlled device two parts, and the derivation relation now exporting lateral deviation y and input u is as follows:

$y=\frac{G_c\left(s\right)G\left(s\right)}{1+G_c\left(s\right)G\left(s\right)}u---\left(1\right)$

Namely the relation of control inputs u and output lateral deviation y is the relationship based on signal transfer function, controller G _cthe physical relationship determining both input and output of (s).

4. a kind of agri-vehicle independent navigation rotating direction control method based on disturbance observer according to claim 1, is characterized in that, described step 3) in, the control inputs of system is u, and the output of system is lateral deviation y, desired output y ₁=u=1; The evaluation index of system is: regulation time t _swith overshoot σ %; Regulation:

T _swherein, specification error band is ± 0.05 (2) to < 3s, σ % < 5%

For meeting the performance index of system requirements, if meet, terminating design, if do not meet, continuing adjustment control decision part.

5. a kind of agri-vehicle independent navigation rotating direction control method based on disturbance observer according to claim 1, it is characterized in that, described step 4) in, agri-vehicle kinematics model adopts three rank linear models and third-order non-linear model, and wherein linear model proposes for the ease of analyzing nonlinear model; According to known technology, three rank linear models can describe with state equation, as follows:

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{y}\\ \stackrel{\&CenterDot;}{\mathrm{\&Psi;}}\\ \stackrel{\&CenterDot;}{\mathrm{\&delta;}}\end{array}\right]=\left[\begin{array}{ccc}0& V_x& 0\\ 0& 0& \frac{V_x}{L}\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\&Psi;}\\ \mathrm{\&delta;}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]u---\left(3\right)$

$y=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\&Psi;}\\ \mathrm{\&delta;}\end{array}\right]---\left(4\right)$

Wherein car speed V _xget 1m/s, wheelbase L gets 2.314m; Now according to known technology, three rank linear models can also describe with transport function G (s):

$G\left(s\right)=\frac{Y\left(s\right)}{d\left(s\right)}=\frac{0.432}{s^3}---\left(5\right)$

Utilize the three rank linear models obtained to carry out the design of controller, and the controller of design is used for go during agri-vehicle third-order non-linear master mould is adjusted.

6. a kind of agri-vehicle independent navigation rotating direction control method based on disturbance observer according to claim 1, is characterized in that, described step 5) in, based on three rank linear models, utilize POLE PLACEMENT USING to obtain state feedback matrix, thus obtain controller G _c(s).Now need the controllability of judgement three rank linear model, controllability judgment matrix is:

$\mathrm{rank}\left[\begin{array}{cccc}B& \mathrm{AB}& A^2& B\end{array}\right]=\mathrm{rank}\left[\begin{array}{ccc}0& 0& \frac{{V_x}^2}{L}\\ 0& \frac{V_x}{L}& 0\\ 1& 0& 0\end{array}\right]=3---\left(6\right)$

According to known technology, now system is pure monopoly market, can pass through structural regime feedback matrix K, realize the arbitrary disposition of system pole; The closed-loop pole that now regulation system is expected is:

λ ₁ ^*＝-5，λ ^* _2,3＝-1±j (7)

The state feedback matrix K tried to achieve is:

K＝[23.148 27.778 7] (8)

After POLE PLACEMENT USING, the controller G of system _c(s) be:

$G_c\left(s\right)=\frac{10s^2}{0.432s^2+3.024s+5.184}---\left(9\right)$

Now third-order system model is at controller G _c(s) adjust under performance index be:

t _s＝2.3s,σ％＝4.2％。(10)

7. a kind of agri-vehicle independent navigation rotating direction control method based on disturbance observer according to claim 1, it is characterized in that, described step 6) in, inevitably exist in agri-vehicle independent navigation course changing control: model error Δ (s), external interference d, measurement noises ζ; According to known technology, now need to discuss external interference d and measurement noises ζ to the impact exporting y,

$y_1=G_{\mathrm{uy}}\left(s\right)u=\frac{G_P\left(s\right)G_n\left(s\right)}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}u---\left(11\right)$

$y_2=G_{\mathrm{dy}}\left(s\right)d=\frac{G_P\left(s\right)G_n\left(s\right)[1-Q\left(s\right)]}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}d---\left(12\right)$

$y_3=G_{\mathrm{zy}}\left(s\right)z=\frac{G_P\left(s\right)Q\left(s\right)}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}z---\left(13\right)$

y＝y ₁+y ₂+y ₃＝G _uy(s)u+G _dy(s)d+G _Zy(s)Z (14)

From known technology: G _ps () is system realistic model, G _ns nominal model that () is system; Formula (11) represents that system expects that the impact that input exports system, formula (12) represent that the impact that external disturbance exports system, formula (13) represent the impact that measurement noises exports system; Now need: y ₂, y ₃be 0, just can make y=y ₁, so just can ensure system not by external disturbance and the impact of measuring disturbance; Adopt the technological means of disturbance observer Q (s) to achieve this end, and Q (s) can ensure performance and the robust stability of system.

8. a kind of agri-vehicle independent navigation rotating direction control method based on disturbance observer according to claim 1, it is characterized in that, described step 7) in, agri-vehicle third-order non-linear master pattern, what exist due to itself is non-linear, there is model error Δ (s) in the now system itself that result in, first solves the impact that model error brings to system; Controller G is utilized merely by claim 6 is known _cs (), cannot realize the calm of system; Now need the impact adopting disturbance observer Q (s) this technological means to cause to overcome system model error delta (s) system; According to known technology, disturbance observer Q (s) generally adopts Butterworth type Q _b(s) and binomial coefficient type Q _b(s):

$Q_B\left(s\right)=\frac{P_{n-k}\left(s\right)}{P_n\left(s\right)}---\left(15\right)$

Now get cutoff frequency: 10Hz, 50Hz, 150Hz; According to known technology, in conjunction with the consideration of robust stability and AF panel and design cost, getting cutoff frequency is 50Hz, and time constant is 0.003s; Now Q _b(s) and Q _bs () is respectively:

$Q_B\left(s\right)=\frac{1}{{0.003}^3{s}^3+2*{0.003}^2{s}^2+2*0.003s+1}---\left(17\right)$

$Q_b\left(s\right)=\frac{1}{{0.003}^3{s}^3+3*{0.003}^2{s}^2+3*0.003s+1}---\left(18\right)$

Disturbance observer Q (s) after design and controller G _cs, under () acting in conjunction, model error Δ (s) of agri-vehicle three rank master pattern obtains and effectively overcomes; Under two kinds of disturbance observers, the performance index of system are respectively:

t _s＝2.32s,σ％＝2.1％(Q(s)＝Q _B(s)) (19)

t _s＝2.32s,σ％＝4.2％(Q(s)＝Q _b(s)) (20)

Below will at disturbance observer Q (s) and controller G _cunder (s) coefficient prerequisite, the multi-form and various combination impact that may cause system of model error Δ (s), external disturbance d, measurement noises ζ is discussed.From third-order non-linear model, model error Δ (s) is that essence exists; The typical existing way of external disturbance d has three kinds:

Low frequency sinusoidal external disturbance:

Unit pulse external disturbance: d ₂=δ (t) (22)

Random external disturbs: d ₃, mean=0, variance=0.5 (23)

The typical existing way of measurement noises ζ is high frequency measurement noise:

Now at Butterworth type disturbance observer Q _b(s) and controller G _cs, under () acting in conjunction, the performance of system does not all reach: t _sthe requirement of < 3s, σ % < 5%, and at binomial coefficient type disturbance observer Q _b(s) and controller G _cs, under () acting in conjunction, the performance of system is:

t _s＝2.34s,σ％＝4.3％ (25)

This kind of technological means, can ensure y=u=1; Add the design of disturbance observer Q (s), not only can overcome the model error of system itself, system can also be made to have the ability of Active Compensation for external interference and measurement noises.It is worthy of note, it contributes to performance index and the robust property of improvement system simultaneously, therefore based on the rotating direction control method technical feasibility of agri-vehicle independent navigation, and meets the actual conditions of automobile navigation.