CN103970138A - ALV transverse control method based on active disturbance rejection and differential smoothing - Google Patents

Abstract

The invention provides an ALV transverse control method based on active disturbance rejection and differential smoothing. The control effect and robustness of the method that differential smoothing and active disturbance rejection are combined on an underactuation system are proved through simulation under different conditions. The ALV transverse control method comprises the steps that firstly, an ALV transverse kinetic model is established; then, the differential smoothing output is designed according to the kinetic model; at last, according to the differential smoothing output, the control law and an active disturbance rejection controller, a composite controller of the ALV transverse control system is designed. The active disturbance rejection controller comprises a tracking differentiator, an expansion state observer and a nonlinear feedback control law.

Description

Based on active disturbance rejection and the level and smooth horizontal control method of ALV of differential

Technical field

The invention belongs to the horizontal control field of ground autonomous land vehicle system, relate to a kind of based on active disturbance rejection and the level and smooth horizontal control method of ALV of differential.

Background technology

Ground autonomous land vehicle (Autonomous Land Vehicle, ALV) being the key components of Future Combat System (FCS) and intelligent transportation system (ITS), is one of most active research directions in field such as current intelligent robot and artificial intelligence.The conventional locomotive functions such as ALV not only should have acceleration, slows down, advances, falls back, turning, but also should there is the capacity of will such as task analysis, environment sensing, path planning, path trace, automatic obstacle-avoiding.Its research relates to the science and technology field such as machinery, kinematics and dynamics, electronics, computing machine, information processing, control and artificial intelligence.

Fliess M, Levine J, it is smoothly theoretical that Martin P and Rouchon P have proposed differential at first.For owing to drive the control of ground moving platform, when behind given initial position and target location, can utilize the level and smooth theoretical problem relocating that solves of differential.The concept that differential is level and smooth has been portrayed original system and can be equivalent to after suitable dynamic expansion the characteristic of another system.The problem that level and smooth output generates for track has vital role, if smoothly export knownly, can obtain corresponding state variable and control variable.But the shortcoming of the method is only effective to differential smoothing system, and level and smooth output is difficult for finding.The proposition of differential smoothing system is widely used in control problem the level and smooth theory of differential.Differential has smoothly been applied in the research of underactuated spacecraft as a kind of feasible orbit generation method.

Auto Disturbances Rejection Control Technique is absorption modern control theory achievement, develops PID thought marrow (eliminating error based on error), develops the novel practical technology of using Special Nonlinear effect to develop.Auto Disturbances Rejection Control Technique is totally independent of the mathematical model of controlled device, and its most outstanding feature is exactly that the effect of all uncertain factors that act on controlled device is all summed up as to " unknown disturbance " and utilizes the inputoutput data of object that it is estimated in real time and is recompensed.The meaning of active disturbance rejection is just this, outside not needing directly to measure, disturbs effect here, does not also need to realize the action rule of knowing disturbance.This also makes to require to realize in rugged environment the occasion of high-speed, high precision control, and Auto Disturbances Rejection Control Technique more can be showed its superiority.

Summary of the invention

The present invention be directed to the defect of prior art, propose a kind ofly based on active disturbance rejection and the level and smooth horizontal control method of ALV of differential, and proved that by the emulation under different condition differential smoothly combines method to owing control effect and the robustness of drive system with active disturbance rejection.

Technical scheme of the present invention is as follows:

Based on active disturbance rejection and the level and smooth horizontal control method of ALV of differential, model ground autonomous land vehicle horizontal dynamic model; And then according to this kinetic model, design its differential and smoothly export; Last according to the level and smooth output of described differential and control law and automatic disturbance rejection controller, the composite controller of the horizontal control system of design ground autonomous land vehicle.

Described automatic disturbance rejection controller comprises Nonlinear Tracking Differentiator, extended state observer and nonlinear Feedback Control rule.

Beneficial effect of the present invention:

1,, in the time that the speed of a motor vehicle is higher, mobile platform horizontal dynamic linear model can meet the requirement of its transverse movement control;

2, differential smoothly combines the control of controller with ADRC under, mobile platform has been realized steady and high-precision transverse movement in 0～40m/s velocity range, variation to self parameter, road conditions He Huan road time etc. also has very strong robustness, can meet the requirement of high performance control, thereby show that the controller that differential smoothly combines with ADRC is feasible for high speed moving platform transverse movement control;

3, the Engineering Design that the present invention can be the high motor platform of high speed of studying provides guidance.

Brief description of the drawings

Fig. 1. ground autonomous land vehicle system lateral control model figure;

Fig. 2. at controller U ₁lower system S ₁output response;

Fig. 3. at controller U ₁lower system S ₂output response;

Fig. 4. at controller U ₂lower system S ₂output response;

Fig. 5. track is expected transversal displacement figure;

Fig. 6. the reference locus of vehicle body angle;

Fig. 7 .V _x=1m/s and platform parameter are the curve of output under nominal value;

Fig. 8 .V _x=20m/s and platform parameter are the curve of output under nominal value;

Fig. 9 .V _x=13m/s and platform parameter are the curve of output under non-nominal value;

Figure 10. curve of output when platform has perturbation and disturbance.

Embodiment

Below in conjunction with accompanying drawing, the present invention is described in detail.

The horizontal control method of Vehicular system of and Auto Disturbances Rejection Control Technique level and smooth based on differential of the present invention, comprises the following steps:

The first step, set up ground autonomous land vehicle system lateral control model and see accompanying drawing 1, be described below:

$\left\{\begin{array}{c}{\stackrel{.}{v}}_y=-\frac{C_r+C_f}{{\mathrm{mv}}_x}{v}_y+(\frac{C_r{l}_r-C_f{l}_f}{{\mathrm{mv}}_x}-v_x)\stackrel{.}{\mathrm{\ψ}}+\frac{C_f}{m}{\mathrm{\δ}}_f\\ \stackrel{..}{\mathrm{\ψ}}=\frac{-l_f{C}_f+l_r{C}_r}{I_z{v}_x}{v}_y-\frac{l_f^2{C}_f+l_r^2{C}_r}{I_z{v}_x}\stackrel{.}{\mathrm{\ψ}}+\frac{l_f{C}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(1\right)$

Wherein, l _ffor the distance between barycenter and front axle, l _rfor the distance between barycenter and rear axle, m is unloaded WT, C _f, C _rthe cornering stiffness of tire before and after being respectively, δ _ffor vehicle front wheel angle, I _zrepresent the moment of inertia around Z axis, v _xrepresent longitudinal velocity, v _yrepresent transverse velocity, represent yaw velocity.

Second step, the control model of setting up according to the first step, design its differential and smoothly export:

Formula (1) has differential smoothness properties, and its differential is smoothly output as:

$F=\frac{m}{2C_{\mathrm{af}}}{V}_y-\frac{I_z}{{2l}_f{C}_{\mathrm{af}}}r---\left(2\right)$

The controlled quentity controlled variable δ of front-wheel _finput table is shown:

$\mathrm{\δ}=\frac{1}{C_1{B}_1+C_2{B}_2}\left\{\begin{array}{c}\stackrel{..}{F}-[\frac{D_2(C_1{A}_1+C_2{A}_3)}{V(C_1{D}_2-C_2{D}_1)}+\frac{D_1(C_1{A}_2-C_1{V}^2+C_2{A}_4)}{V(C_2{D}_1-C_1{D}_2)}]\stackrel{.}{F}\\ -[\frac{C_2(C_1{A}_1+C_2{A}_3)}{V(C_2{D}_1-C_1{D}_2)}+\frac{C_1(C_1{A}_2-C_1{V}^2+C_2{A}_4)}{V(C_1{D}_2-C_2{D}_1)}]\stackrel{.}{F}\end{array}\right\}---\left(3\right)$

The all states of system, V _y, r and control inputs δ _fall can be represented by the function of smoothly exporting F and derivative thereof, therefore according to the definition of differential smoothing system, owe actuation movement control system in the time implementing flight path control, there is the characteristic that differential is level and smooth.

The 3rd step, controller level and smooth based on differential and ADRC design.

1, ground autonomous platform transverse movement smoothing system Transformation of Mathematical Model

For design platform transverse movement smoothing system ADRC controller, smoothing system model (3) need be converted to Affine Incentive.For this reason, make x ₁=F, u=δ, is rewritten as formula (4) by formula (3).

$\left\{\begin{array}{c}{\stackrel{.}{x}}_1=x_2\\ {\stackrel{.}{x}}_2=f(x_1,x_2)+\mathrm{Bu}\end{array}\right.---\left(4\right)$

Wherein, $f(x_1,x_2)=[E_1+E_2]\stackrel{.}{F}+[E_3+E_4]\stackrel{.}{F},B=\frac{1}{C_1{B}_1+C_2{B}_2},E_1=\frac{D_2(C_1{A}_1+C_2{A}_3)}{V(C_1{D}_2-C_2{D}_1)}$

$E_2=\frac{D_1(C_1{A}_2-C_1{V}^2+C_2{A}_4)}{V(C_2{D}_1-C_1{D}_2)},E_3=\frac{C_2(C_1{A}_1+C_2{A}_3)}{V(C_2{D}_1-C_1{D}_2)},E_4=\frac{C_1(C_1{A}_2-C_1{V}^2+C_2{A}_4)}{V(C_1{D}_2-C_2{D}_1)}.$

In order to make system become the system of pure integration, we will design extended state observer, in order to disturb in eliminating and to disturb outward.Still write formula (4) form of expansion state spatial expression as, suc as formula (5):

$\left\{\begin{array}{c}{\stackrel{.}{x}}_1=x_2\\ {\stackrel{.}{x}}_2=x_3+\mathrm{Bu}\\ {\stackrel{.}{x}}_3=g(x_1,x_2)\end{array}\right.---\left(5\right)$

Wherein, g (x ₁, x ₂) be f (x ₁, x ₂) derivative.

2, controller level and smooth based on differential and ADRC designs

We carry out the design of autonomous platform transverse movement smoothing system controller based on formula (5) and Auto-disturbance-rejection Control below.

Autonomous platform transverse movement smoothing system automatic disturbance rejection controller based on formula (5) is expressed as formula (6)～(9):

$\left\{\begin{array}{c}\mathrm{fh}=\mathrm{fhan}(v_1-F_r,v_2,r_0,h)\\ v_1=v_1+h\·v_2\\ v_2=v_2+h\·\mathrm{fh}\end{array}\right.---\left(6\right)$

$\left\{\begin{array}{c}e=z_1-F\\ \mathrm{fe}=\mathrm{fal}(e,0.5,h),{\mathrm{fe}}_1=\mathrm{fal}(e,0.25,h)\\ z_1=z_1+h\·(z_2-{\mathrm{\β}}_{01}\·e)\\ z_2=z_2+h\·(f_0(z_1,z_2)+z_3-{\mathrm{\β}}_{02}\·\mathrm{fe}+B\·u)\\ z_3=z_3+h\·(-{\mathrm{\β}}_{03}\·{\mathrm{fe}}_1)\end{array}\right.---\left(7\right)$

$\left\{\begin{array}{c}{e}_1=v_1-z_1,e_2=v_2-z_2\\ u_0=k(e_1,e_2,p)\end{array}\right.---\left(8\right)$

$u_{\mathrm{\ψ\δ}}=u_0-\frac{f_0(z_1,z_2)+z_3}{b_{\mathrm{\ψ\δ}}}---\left(9\right)$

Wherein F _rfor the reference locus of smooth function and v _yrand r _rbe respectively the reference locus of transverse velocity and body gesture angular velocity.

3, the parameter tuning of the controller of and ADRC level and smooth based on differential

The track following of smoothly exporting and control the level and smooth output of differential by designing differential is realized the underactuated system of ground autonomous platform transverse movement.And the level and smooth output of differential of design selects active disturbance rejection (ADRC) to control herein, controller is suc as formula shown in (6)～(9).

The part of controller design most critical is parameter testing, because the level and smooth output system coefficient adjusting excessive and the excessive parameter of time scale of design is herein difficult to carry out, through repeatedly debugging and cannot directly obtain, therefore access time, method of scales was adjusted parameter.

First, first choose baseline system

$\stackrel{..}{F}+\stackrel{.}{F}+2F=u---\left(10\right)$

Adjust parameter to obtain by time scale method, the time scale p of system (10) ₁=0.241 and its parameter is regulated.

According to model G _s1aDRC controller U can adjust ₁for

r ₁₁＝0.1,r ₁₂＝0.02,h＝0.01,

h ₁＝0.02,c＝100,β ₁＝100,

β ₂＝200,β ₃＝30000.

Systematic parameter substitution system (3) is obtained

$\stackrel{..}{F}+326\stackrel{.}{F}+26490F=12.1u---\left(11\right)$

Adjust parameter to obtain by time scale method, the time scale p of system (11) ₂=2.83.First, utilize ADRC controller U ₁to this nonlinear model, i.e. system S ₁and S ₂, controlling emulation, gained output F is shown in accompanying drawing 2,3.

Accompanying drawing 2,3 results show, controller U ₁although can be preferably to S ₁implement steadily to control; But, to S ₂control procedure do not catch up with track completely and exhale, U now as seen ₁be not suitable for S ₂.

By S ₁and S ₂time scale ratio m=p ₂/ p ₁≈ 10, then according to the ADRC parameter tuning method of controlled system time scale, can be according to controller U ₁after parameter adjustment, obtain controller U ₂:

r ₁₁＝0.1,r ₁₂＝0.2,h＝0.001,

h ₁＝0.002,c＝200,β ₁＝1000,

β ₂＝2800,β ₃＝30000000.

Then utilize respectively controller U ₁and U ₂to system S ₂control emulation, gained output y accompanying drawing 4.Shown in.

Accompanying drawing 4 shows, compares controller U ₁, controller U ₂to S ₂obtain satisfied control effect, visible controller U ₂to be applicable to S ₂.

In order to verify smoothly combining with active disturbance rejection and realize the method for ground autonomous land vehicle system Lateral Controller design based on differential of above-mentioned design, and prove that by the emulation under different condition differential smoothing method is to owing control effect and the robustness of drive system.

The kinetics equation that the ground autonomous land vehicle system of setting up in the present invention is laterally controlled is as follows:

$\left\{\begin{array}{c}{\stackrel{.}{v}}_y=-\frac{C_r+C_f}{{\mathrm{mv}}_x}{v}_y+(\frac{C_r{l}_r-C_f{l}_f}{{\mathrm{mv}}_x}-v_x)\stackrel{.}{\mathrm{\ψ}}+\frac{C_f}{m}{\mathrm{\δ}}_f\\ \stackrel{..}{\mathrm{\ψ}}=\frac{-l_f{C}_f+l_f{C}_r}{I_z{v}_x}{v}_y-\frac{l_f^2{C}_f+l_r^2{C}_r}{I_z{v}_x}\stackrel{.}{\mathrm{\ψ}}+\frac{l_f{C}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.$

Wherein, l _ffor the distance 1.05m between barycenter and front axle, l _rfor the distance 1.63m between barycenter and rear axle, m is unloaded WT 1480Kg, C _ffor the cornering stiffness 67500N/rad of front tyre, C _rfor the cornering stiffness 47500N/rad of rear tyre, δ _ffor vehicle front wheel angle, I _zrepresent the moment of inertia 2350Kgim around Z axis ², v _xrepresent longitudinal velocity, v _yrepresent transverse velocity, represent yaw velocity.

The present invention is taking above-mentioned model as example, and concrete emulation implementation step is as follows:

Simulated environment

Suppose that platform does two-track line motion with a certain fixing longitudinal velocity, change track (transient process of arrangement) and see accompanying drawing 5.This track uses sine function planning algorithm to produce, and planning formula is suc as formula shown in (12).In formula, v ₀(t): expect transversal displacement; W: lane width; t ₁: from the moment in right lane port track; t ₂: on left-lane, start moment of travelling; t ₃: the moment that turns to right road from left-lane; t ₄: the moment that comes back to right lane.The tangential direction that the ideal pose of supposing vehicle body is transverse path, the reference locus of vehicle body angle is shown in accompanying drawing 6.

$v_0\left(t\right)=\left\{\begin{array}{cc}0,& t<t_1;\\ \frac{W}{2}[1-\cos \left(\frac{2\mathrm{\&pi;}}{T}(t-t_1)\right)],& t_1\&le;t\&le;t_2;\\ W,& t_2<t<t_3;\\ \frac{W}{2}[1+\cos \left(\frac{2\mathrm{\&pi;}}{T}(t-t_3)\right)],& t_3<t\&le;t_4;\\ 0,& t_4<t.\end{array}\right.---\left(12\right)$

When emulation, change parameter and the longitudinal velocity V of platform and steering mechanism _x, to examine or check the robustness of ADRC controller. wherein, tire angular rigidity C _sf, C _srchange both can represent the perturbation of the parameter of tire own also can represent the variation (disturbance) of road ground condition; Platform barycenter can represent the variation (disturbance) of mass distribution and the longitudinal unevenness of road simultaneously to the change of axle distance. therefore, arranging of above-mentioned simulation parameter can be examined or check designed ADRC controller to " inside " and " outside " probabilistic adaptive faculty.

Simulation result

See that accompanying drawing 7～8 is respectively V _x=1m/s, 40m/s and platform parameter are the simulation result under nominal value; Accompanying drawing 9 be the simulation result of following parameter: V _x=35m/s, m=2220kg (nominal value 1.5 times), I _z=3290kgm ²(nominal value 1.4 times), l _f=1.2m (moving 0.15m after barycenter), lr=1.48m, C _sf=40500N/rad (nominal value 60%), C _sr=28500N/rad (nominal value 60%).Accompanying drawing 10 is at C _sf=[0.85+0.15 (2U (0,1)-1)] C _{sf_nom}, C _sr=[0.85+0.15 (2U (0,1)-1)] C _{sr_nom}, other parameters are the result when identical with accompanying drawing 9 simulated conditions, and wherein U (0,1) is unit uniformly distributed function.The condition that Fig. 4 .7, Fig. 4 .8 are corresponding is controlled very harsh for platform transverse movement automatically.Wherein, blue track represents reference locus, the actual path of the controller control dolly of red track representative based on the design of ADRC method, the actual travel track of the controller control dolly of green track representative based on neural network and fuzzy control method design.

Accompanying drawing 7～8 results show, the controller that the method comparison differential combining with fuzzy control with neural network smoothly combines with ADRC has good adaptive faculty to the variation of platform speed, has successfully realized steady, high precision control to system transverse movement.Accompanying drawing 9～10 results show, though road shortening switching time, platform parameter and the larger change of road conditions generation, platform still has desirable transverse movement performance under the control of ADRC controller.

Claims (5)

1. based on active disturbance rejection and the level and smooth horizontal control method of ALV of differential, it is characterized in that: model ground autonomous land vehicle horizontal dynamic model; And then according to this kinetic model, design its differential and smoothly export; Last according to the level and smooth output of described differential and control law and automatic disturbance rejection controller, the composite controller of the horizontal control system of design ground autonomous land vehicle.

2. as claimed in claim 1 a kind of based on active disturbance rejection and the level and smooth horizontal control method of ALV of differential, it is characterized in that: described automatic disturbance rejection controller comprises Nonlinear Tracking Differentiator, extended state observer and nonlinear Feedback Control rule.

3. a kind of ALV transversal displacement tracker control method based on active disturbance rejection as claimed in claim 2, is characterized in that: described Nonlinear Tracking Differentiator adopts with drag:

$\left\{\begin{array}{c}{v}_1(k+1)=v_1\left(k\right)+h\&CenterDot;v_2\left(k\right)\\ v_2(k+1)=v_2\left(k\right)+h\&CenterDot;\mathrm{fst}(v_1\left(k\right)-v_0,v_2\left(k\right),r,h_0)\end{array}\right.$

Wherein $\mathrm{fst}(\&bull;)=\left\{\begin{array}{cc}-\mathrm{ra}& \left|a\right|\&le;d\\ -r\&CenterDot;\mathrm{sgn}\left(a\right),& \left|a\right|>d\end{array}\right.:$

And sgn is sign function,

$a=\left\{\begin{array}{cc}{v}_2+\frac{v_0}{h},& \left|y_0\right|>d_0\\ v_2+\frac{\mathrm{sgn}\left(y_0\right)\&CenterDot;(a_0-d)}{2},& \left|y_0\right|>d_0\end{array}\right.$

Wherein, d=rh, d ₀=dh, y ₀=v ₁-v ₀-hv ₂

Wherein, r is parameter to be adjusted, and is also the velocity factor of Nonlinear Tracking Differentiator, h ₀be filtering factor, h is sampling step length, v ₀that ground autonomous land vehicle system is laterally controlled reference input, v ₁(k) input signal for being used for following the tracks of, v ₂(k) be the approximate differential signal that obtains input signal, d, d ₀, a, a ₀, y, y ₀for the intermediate variable in equation solver process, in iteration, eliminate; Obtain approximate differential signal by solving this equation, follow the tracks of input signal on one side, obtain its approximate differential signal on one side.

4. a kind of ALV transversal displacement tracker control method based on active disturbance rejection as claimed in claim 2 or claim 3, is characterized in that: described extended state observer adopts with drag:

$\left\{\begin{array}{c}e=z_1\left(k\right)-y\left(k\right);\\ z_1(k+1)=z_1\left(k\right)+h(z_2\left(k\right)-{\mathrm{\&beta;}}_{01}e)\\ z_2(k+1)=z_2\left(k\right)+h(z_3\left(k\right)-{\mathrm{\&beta;}}_{02}\&CenterDot;\mathrm{fal}(e,{\mathrm{\&alpha;}}_1,\mathrm{\&delta;})+b_{0}u\left(k\right))\\ z_3(k+1)=z_3\left(k\right)-h\&CenterDot;{\mathrm{\&beta;}}_{03}\&CenterDot;\mathrm{fal}(e,{\mathrm{\&alpha;}}_2,\mathrm{\&delta;})\end{array}\right.$

Wherein: $\mathrm{fal}(e,\mathrm{\&alpha;},\mathrm{\&delta;})=\left\{\begin{array}{cc}{\left|e\right|}^{\mathrm{\&alpha;}}\&CenterDot;\mathrm{sgn}\left(e\right),& \left|e\right|>\mathrm{\&delta;},\\ \frac{e}{{\mathrm{\&delta;}}^{1-\mathrm{\&alpha;}}},& \left|e\right|\&le;\mathrm{\&delta;}.\end{array}\right.$

Wherein, z ₁, z ₂, z ₃the output of extended state observer, z ₁tracker state v ₁, z ₂the state v of tracker ₂, z ₃be internal disturbance and the external disturbance of estimating system, h is sampling step length, b ₀for the coefficient z of control variable ₁(k+1), z ₂(k+1), z ₃(k+1) be the output of extended state observer, z ₁(k+1) tracker state v ₁(k), z ₂(k+1) the state v of tracker ₂(k), z ₃(k+1) be internal disturbance and the external disturbance of estimating system, β ₀₁, β ₀₂, β ₀₃it is the coefficient of observer, embody the observing capacity of observer, e is state error, and u (k) is the controlled quentity controlled variable of system, and y is system output, δ is the linearity range burst length of power function f al, need to meet δ ∈ [0,1], get δ=0.01, α represents the power of power function f al, and α is expressed as α in two fal functions ₁α ₂, meet 0< α ₂< α ₁<1, gets α ₁=0.5, α ₂=0.25.

5. a kind of ALV transversal displacement tracker control method based on active disturbance rejection as claimed in claim 4, is characterized in that: described nonlinear Feedback Control rule adopts with drag:

$\left\{\begin{array}{c}{e}_1=v_1\left(k\right)-z_1\left(k\right)\\ e_2=v_2\left(k\right)-z_2\left(k\right)\\ u_0=K_p\&CenterDot;\mathrm{fal}(e_1,{\mathrm{\&alpha;}}_p,\mathrm{\&delta;})+K_D\&CenterDot;\mathrm{fal}(e_2,{\mathrm{\&alpha;}}_D,\mathrm{\&delta;}).\end{array}\right.$

Wherein, e ₁, e ₂respectively error and the differential thereof between observed quantity and input signal, K _p, K _dfor Error Feedback gain, embody the control ability of controller, in above formula, δ meets δ ∈ [0,1], gets δ=0.01, and the power of two power functions meets 0< α _p<1< α _d, get α _p=0.5, α _d=2; The expression formula that obtains automatic disturbance rejection controller control law is as follows:

u(k)＝u ₀-z ₃(k)/b ₀。