CN106950999A - A kind of fitup Trajectory Tracking Control method of use Auto Disturbances Rejection Control Technique - Google Patents

Abstract

The invention discloses a kind of fitup Trajectory Tracking Control method of use Auto Disturbances Rejection Control Technique, it designs the Trajectory Tracking Control of fitup using auto-disturbance rejection technology, design automatic disturbance rejection controller, by multivariable, the system decoupling of close coupling is three subsystems, automatic disturbance rejection controller is separately designed for subsystem, nonlinear system is done into LINEARIZATION WITH DYNAMIC COMPENSATION processing, the control performance of raising system, and design extended state observer and effectively to estimate and compensate in real time by the uncertain amount and system inside and outside disturbance quantity of system model, the method is respectively provided with very strong inhibitory action to disturbance and model uncertainty inside and outside system.

Description

A kind of fitup Trajectory Tracking Control method of use Auto Disturbances Rejection Control Technique

Technical field

The invention belongs to Stage Equipment Control technical field, it is related to the Trajectory Tracking Control of fitup, specifically how disappears Except the Internal system parameters external disturbance that perturbs and exist disturbs influence to fitup track following performance, realize it is a kind of effectively Use Auto Disturbances Rejection Control Technique fitup Trajectory Tracking Control method.

Background technology

In modern times performance, in order to more preferably reach artistic effect, it is necessary to control Omni-mobile type stage to coordinate performer to be drilled Go out.Omni-mobile stage is the Omni-mobile platform that a kind of four omni-directional wheels are coupled to form, according to power and course changing control demand Have to have on independent power source, each axle on eight motor compositions, each driving wheel and have independent steering mechanism.This knot The fitup of structure has the features such as turning radius is small, stable direction is easy, be particularly suitable for use in performance in complex scene setting Demand.

But for the four-wheel kinetic control system of Omni-mobile platform, belong to a multivariable, non-linear, close coupling answer Miscellaneous dynamical system, the research to its control method is always a difficult point, the especially Trajectory Tracking Control in Omni-mobile platform On, the method used at present mainly has synovial membrane control, Reverse Step Control, fuzzy control etc..Sliding moding structure method have response it is fast, Good mapping and robustness, are widely used in Trajectory Tracking Control, but are easy to chattering phenomenon occur, directly affect control Effect.Reverse Step Control is the Trajectory Tracking Control method considered earlier based on kinematics, is widely used in tracking problem, but Its control structure and design process are more complicated, and simultaneity factor must is fulfilled for Strict-feedback control structure, high to system requirements, this It is relatively difficult to achieve in actual conditions.Fuzzy control method has certain robustness, but fuzzy control rule can be by people Subjective factor influence and can not sum up completely, and due to a lack of the ability of " self-teaching ".Omni-mobile stage is in performance Need to meet site intelligent setting demand in real time, and according to the status requirement of performer and other stage properties move track with Track control, but due to by the Parameter Perturbation inside complication system itself, add performance in various external interference factors to movement Stage influences, and causes fitup track tracking performance under existing control method to be greatly reduced.

The content of the invention

The present invention is to overcome the fitup under prior art can not to meet the need of modern performance scene intelligent control A kind of Trajectory Tracking Control method there is provided fitup is sought, the modern diversified effect of performance art can be effectively lifted.This hair Bright use auto-disturbance rejection technology designs the Trajectory Tracking Control of fitup, and designs extended state observer effectively by system mould The uncertain amount and system inside and outside disturbance quantity of type are next to be estimated and compensates in real time, and the method is to disturbance and model inside and outside system Uncertainty is respectively provided with very strong inhibitory action.

The fitup Trajectory Tracking Control method of described a kind of use Auto Disturbances Rejection Control Technique, it is characterised in that described Method includes procedure below：

Step 1) set up the kinematics model of fitup：

The complex structure of fitup, is a multivariable, high coupling, nonlinear under-actuated systems, it is designed Controller, first has to set up rational kinematics model, the pose of fitup is described by three amounts (x, y, θ), sets up mobile dance The kinematics model of platform, as shown in formula (1)：

The controlled quentity controlled variable U of driving wheel linear velocity is calculated as shown in formula (2)：

Wherein：

X-- fitups world coordinate system X-axis coordinate value,

Y-- fitups world coordinate system Y-axis coordinate value,

The anglec of rotation of θ -- the fitup in world coordinate system,

θ₀-- the angle of car body side opposite the angle and side, the car body fixed for structure, θ₀For certain certain value,

R-- car bodies barycenter to steering structure pivot distance,

The radius of turn of r-- steering structures,

w_{1、2、3、4}-- the angular velocity of rotation of four steering mechanism,

ρ_{1、2、3、4}-- four steering mechanism and the angle of bodywork reference frame X-axis；

Step 2) the track following error model of fitup is set up, its process includes：

Any one moment is located at, its velocity vector in world coordinate system isIt is taken in body coordinate It is that velocity vector isIt is denoted as [u v w]^T, track following errorDescription is as shown in formula (3)：

Wherein u_r, v_rRepresent respectively by decomposing trajectories, t fitup is along bodywork reference frame X, the velocity component of Y-axis Desired value, θ_rRepresent the desired value of car body angular velocity of rotation；

Step 3) multi-variable system decoupling, process includes：

For three-three output systems of input

Note

X=[x₁ x₂ x₃]^T

F=[f₁ f₂ f₃]^T

U=[u₁ u₂ u₃]^T (5)

Introduce " virtual controlling amount "Formula (4) is changed into

The input/output relation of i-th of passage is in system

Parallel 3 automatic disturbance rejection controllers of insertion can be achieved with the decoupling control of multivariable between controlled quentity controlled variable U and output quantity y System；

Wherein x₁, x₂, x₃-- the quantity of state of three-three output systems of input,

u₁, u₂, u₃-- the control input of three-three output systems of input,

y₁, y₂, y₃-- the system output of three-three output systems of input,

f₁, f₂, f₃-- each passage disturbance term,

b₁₁,b₁₂,b₁₃,b₂₁,b₂₂,b₂₃,b₃₁,b₃₂,b₃₃-- the system gain factor；

Step 4) automatic disturbance rejection controller is designed, process includes：

According to track following error model, it is three independent subsystems by contrail tracker decoupling, is divided into e_xControl Loop processed, e_yControl loop and e_θControl loop, is that each subsystem separately designs automatic disturbance rejection controller；

Because system is first-order system, it is not necessary to Nonlinear Tracking Differentiator, automatic disturbance rejection controller include extended state observer with And state feedback control law two parts, it is specific as follows；

(1) extended state observer is designed, the state and disturbance to system carry out estimation in real time and compensation, and expansion state is seen Device equation is surveyed to be given below：

Wherein, β₀₁、β₀₂For one group of parameter to be adjusted,

The estimate z of e-- outputs₁Difference with exporting y,

The control input value of u-- systems,

b₀-- compensating factor,

z₂-- the estimate of expansion state；

(2) linear state error feedback control is restrained

Wherein, k₁For proportionality coefficient,

e₁-- reference input v₁With the estimate z of output₁Difference,

u₀-- error feedback control amount.

A kind of fitup Trajectory Tracking Control method of described use Auto Disturbances Rejection Control Technique, its feature is in step 3) In, detailed process is as follows：

By formula 3), system can regard the output system of three input three as, and with u (t), v (t), w (t) is indirect control amount

The kinematic coupling portion is made to be：

f₁,f₂,f₃For disturbance summation, static coupling unit is：

Using linear active disturbance rejection control technology, the summation that kinematic coupling portion is taken as on respective passage disturbs to be estimated And compensate, as long as ensureing that matrix B is reversible, the uneoupled control of system is can be achieved with, in systems, B determinant perseverance is -1, Uneoupled control can be realized.

A kind of fitup Trajectory Tracking Control method of described use Auto Disturbances Rejection Control Technique, its feature is in step 4) In, detailed process is as follows：

With e_xExemplified by the Auto-disturbance-rejection Controller Design of control subsystem, automatic disturbance rejection controller include extended state observer with And state feedback control law two parts：

(1) extended state observer is designed, the state and disturbance to system carry out estimation in real time and compensation, and expansion state is seen Device equation is surveyed to be given below：

Wherein, β₀₁、β₀₂For one group of parameter to be adjusted,

z₁--e_xObservation,

z₂-- the observation of expansion state amount；

(2) state error Feedback Control Laws

Wherein, k₁For proportionality coefficient.

The fitup Trajectory Tracking Control method of described a kind of use Auto Disturbances Rejection Control Technique, it is characterised in that mobile Implementation process on Stage Equipment Control computer is run by main executable portion, is mainly included the following steps that：

1) parameter setting：Imported in parameter in interface, r, R, θ in input type (1)₀Value, and ρ₁, ρ₂, ρ₃, ρ₄It is initial Value；In automatic disturbance rejection controller parameter setting interface, extended state observer gain beta is inputted₀₁、β₀₂、β₁₁、β₁₂、β₂₁、β₂₂, with And k₁、k₂、k₃Value, wherein β₁₁、β₁₂For e_yThe observer gain of control subsystem, β₂₁、β₂₂For e_θThe observation of control subsystem Device gain, k₂,k₃Respectively e_y,e_θState error Feedback Control Laws proportionality coefficient, after input parameter confirms, by control computer Data will be set to send into Computer Storage unit F lash to preserve；

2) track is set：Imported in track in interface, set movement locus, imported after the confirmation of track, calculated by control Desired guiding trajectory data are stored in Flash memory cell by machine, and desired guiding trajectory is decomposed into [u by control computer_r v_r w_r]^T Data are simultaneously stored in preservation, wherein u in Flash memory cell by matrix_r,v_r,w_rIt is time t function；

3) offline debugging：The Debug button in configuration interface is clicked on, control system enters controller and debugs the stage, with straight Line tracking is test trails, and adjusting parameter sets the β in interface₀₁、β₀₂、β₁₁、β₁₂、β₂₁、β₂₂Value and k₁、k₂、k₃, observation Fitup track following error e_x、e_y、e_θ, thereby determine that quickly can effectively realize a fitup Trajectory Tracking Control Parameter；

4) on-line operation：Configuration interface " RUN " button is clicked on, the control computer of fitup is read from Flash storages The optimal arrange parameter of automatic disturbance rejection controller is taken, and is read from Flash storages by matrix [u after decomposing trajectories_r v_r w_r ]^TData, perform " fitup Trajectory Tracking Control program ", pass through real-time pose [the x y θ of on-line measurement fitup ]^T, motor and the rotating speed of steering motor are controlled, automatically adjusting for fitup pose is realized, in next regulating cycle During arrival, the real-time pose of on-line measurement fitup repeats whole implementation procedure, so gone round and begun again afterwards, realizes movement The auto-adjustment control of stage track following.

Compared with prior art, the advantage of the invention is that：

1) present invention is for Internal system parameters perturbation and there is the uncertainties such as external disturbance, introduces expansion state observation Device is estimated the state of system and inside and outside disturbance in real time, and is compensated using nonlinearity erron feedback rate control, eliminates The influence of inside and outside disturbance；

2) present invention designs automatic disturbance rejection controller, by multivariable, close coupling according to the own structural characteristics of fitup System decoupling is three subsystems, is that subsystem separately designs automatic disturbance rejection controller, and it is linear that nonlinear system is done into dynamic compensation Change is handled, and improves the control performance of system.

Brief description of the drawings

Fig. 1 is fitup structural representation；

Fig. 2 is fitup track following Active Disturbance Rejection Control block diagram；

Fig. 3 is first-order linear automatic disturbance rejection controller structure chart；

Fig. 4 fitup track following x-axis tracking errors e_xCurve；

Fig. 5 fitup track following y-axis tracking errors e_yCurve；

Fig. 6 fitup track following θ axle tracking errors e_θCurve.

Embodiment

In order that technical scheme, mentality of designing can become apparent from, retouched in detail again below in conjunction with the accompanying drawings State.

A kind of fitup Trajectory Tracking Control method of use Auto Disturbances Rejection Control Technique, methods described includes following mistake Journey：

Step 1) set up the kinematics model of fitup：

The structure of fitup is more complicated, is a multivariable, high coupling, nonlinear under-actuated systems, be to it Controller is designed, first has to set up rational kinematics model.The pose of fitup is described by three amounts (x, y, θ).Set up and move The kinematics model of dynamic stage, is shown in expression formula (1)

It is as follows to the controlled quentity controlled variable U of driving wheel linear velocity：

Wherein

Coordinate value of the x-- fitups in world coordinate system X-axis

Coordinate value of the y-- fitups in world coordinate system Y-axis

The anglec of rotation of θ -- the fitup in world coordinate system

θ₀-- angle (car body fixed for structure, θ of car body diagonal sum side₀For certain certain value)

Distance of the R-- car bodies barycenter to steering structure pivot

The radius of turn of r-- steering structures

w_{1、2、3、4}-- the angular velocity of rotation of four steering mechanism

ρ_{1、2、3、4}-- steering mechanism and the angle of bodywork reference frame X-axis.

Step 2) the track following error model of fitup is set up, process includes：

Any one moment is located at, its velocity vector in world coordinate system isIt is taken in body coordinate It is that velocity vector isIt is denoted as [u v w]^T, track following errorDescription is such as formula (3)：

Wherein u_r, v_rRepresent respectively by decomposing trajectories, t fitup is along bodywork reference frame X, the velocity component of Y-axis Desired value, θ_rRepresent the desired value of car body angular velocity of rotation.

Step 3) multi-variable system decoupling, process includes：

For three-three output systems of input

Note

X=[x₁ x₂ x₃]^T

F=[f₁ f₂ f₃]^T

U=[u₁ u₂ u₃]^T (5)

Introduce " virtual controlling amount "Formula (4) is changed into

The input/output relation of i-th of passage is in system

Parallel 3 automatic disturbance rejection controllers of insertion can be achieved with the decoupling control of multivariable between controlled quentity controlled variable U and output quantity y System.

x₁, x₂, x₃-- the quantity of state of three-three output systems of input

u₁, u₂, u₃-- the control input of three-three output systems of input

y₁, y₂, y₃-- the system output of three-three output systems of input

f₁, f₂, f₃-- each passage disturbance term

b₁₁,b₁₂,b₁₃,b₂₁,b₂₂,b₂₃,b₃₁,b₃₂,b₃₃-- the system gain factor

Step 4) automatic disturbance rejection controller is designed, process includes：

According to track following error model, it can be three independent subsystems by contrail tracker decoupling, be every Individual subsystem separately designs automatic disturbance rejection controller.

It is three subsystems by decoupling rule by contrail tracker decoupling, is divided into e_xControl loop, e_yControl loop and e_θControl loop.

Because system is first-order system, it is not necessary to Nonlinear Tracking Differentiator, automatic disturbance rejection controller is observed including expansion state here Device and state feedback control law two parts

(1) extended state observer is designed, the state and disturbance to system carry out estimation in real time and compensation, and expansion state is seen Device equation is surveyed to be given below：

Wherein, β₀₁、β₀₂For one group of parameter to be adjusted.

The estimate z of e-- outputs₁Difference with exporting y

The control input value of u-- systems

b₀-- compensating factor

z₂-- the estimate of expansion state

(2) linear state error feedback control is restrained

Wherein, k₁For proportionality coefficient.

e₁-- reference input v₁With the estimate z of output₁Difference

u₀-- error feedback control amount

Further, the step 3) in, detailed process is as follows：

By formula 3), system can regard the output system of three input three as, and with u (t), v (t), w (t) is indirect control Amount

The kinematic coupling portion is made to be：

f₁,f₂,f₃For disturbance summation, static coupling unit is：

Using linear active disturbance rejection control technology, the summation that kinematic coupling portion is taken as on respective passage disturbs to be estimated And compensate.As long as ensureing that matrix B is reversible, it is possible to realize the uneoupled control of system.To the system, B is necessarily reversible, therefore necessarily Uneoupled control can be realized.

Further, the step 4) in, detailed process is as follows：

With e_xExemplified by the Auto-disturbance-rejection Controller Design of control subsystem.Automatic disturbance rejection controller include extended state observer with And state feedback control law two parts

(1) extended state observer is designed, the state and disturbance to system carry out estimation in real time and compensation, and expansion state is seen Device equation is surveyed to be given below：

Wherein, β₀₁、β₀₂For one group of parameter to be adjusted.

z₁--e_xObservation

z₂-- the observation of expansion state amount

(2) state error Feedback Control Laws

Wherein, k₁For proportionality coefficient.

The present embodiment is the process of fitup track following, specific operation process：

1st, in parameter setting interface, the kinematics model basic parameter of fitup is inputted, it is as follows：

R=0.2m, R=1.0m,

Automatic disturbance rejection controller parameter is inputted, it is as follows：

β₀₁=β₀₂=β₁₁=β₁₂=β₂₁=β₂₂=1

k₁=k₂=k₃=1

2nd, imported in track in interface, set movement locus.The track that sets of this example is sat as fitup along the world Fitup itself is rotated with 0.2rad/s angular speed while mark system y-axis is moved along a straight line with 1m/s speed.By track [u is obtained after decomposition_r v_r w_r]^TMatrix (u_r,v_r,w_rIt is time t function), [u_r v_r w_r]^TMatrix parameter is as follows：

[u_r v_r w_r]^T=[0 1.0t 0.2t]^T, wherein u_r,v_rUnit be m/s, w_rUnit be rad/s.

Matrix [the u that decomposing trajectories are obtained_r v_r w_r]^TData storage is in Flash storages.

3rd, offline debugging：The Debug button in configuration interface is clicked on, control system enters controller and debugs the stage.With straight Line tracking is test trails, and adjusting parameter sets the β in interface₀₁、β₀₂、β₁₁、β₁₂、β₂₁、β₂₂Value and k₁、k₂、k₃, observation Fitup track following error e_x、e_y、e_θ, thereby determine that quickly can effectively realize a fitup Trajectory Tracking Control Parameter.The automatic disturbance rejection controller parameter obtained by debugging is as follows：

[β₀₁ β₀₂]^T=[100 2500]^T

[β₁₁ β₁₂]^T=[90 2025]^T

[β₀₁ β₀₂]^T=[110 3025]^T

k₁=1.5, k₂=2, k₃=2

4th, on-line operation：Configuration interface " RUN " button is clicked on, the control computer of fitup is read from Flash storages The optimal arrange parameter of automatic disturbance rejection controller is taken, and is read from Flash storages by matrix [u after decomposing trajectories_r v_r w_r ]^TData, perform " fitup Trajectory Tracking Control program ", pass through real-time pose [the x y θ of on-line measurement fitup ]^T, motor and the rotating speed of steering motor are controlled, automatically adjusting for fitup pose is realized.In next regulating cycle During arrival, the real-time pose of on-line measurement fitup repeats whole implementation procedure afterwards.So go round and begin again, realize movement The auto-adjustment control of stage track following.

Described above is the fitup track following that one embodiment that the present invention is provided shows premium properties Effect.It may be noted that above-described embodiment is used for illustrating the present invention, rather than limit the invention, the present invention's In spirit and scope of the claims, any modification made to the present invention both falls within protection scope of the present invention.

Claims (4)

1. a kind of fitup Trajectory Tracking Control method of use Auto Disturbances Rejection Control Technique, it is characterised in that methods described includes Procedure below：

Step 1) set up the kinematics model of fitup：

The complex structure of fitup, is a multivariable, high coupling, nonlinear under-actuated systems, to design it control Device, first has to set up rational kinematics model, the pose of fitup is described by three amounts (x, y, θ), sets up fitup Kinematics model, as shown in formula (1)：

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]={\left[\begin{array}{ccc}-\sin (\mathrm{\θ}+{\mathrm{\ρ}}_1)& \cos (\mathrm{\θ}+{\mathrm{\ρ}}_1)& r-R\cos ({\mathrm{\θ}}_0+\mathrm{\π}-{\mathrm{\ρ}}_1)\\ -\sin (\mathrm{\θ}+{\mathrm{\ρ}}_2)& \cos (\mathrm{\θ}+{\mathrm{\ρ}}_2)& r-R\cos ({\mathrm{\θ}}_0+\frac{\mathrm{\π}}{2}-{\mathrm{\ρ}}_2)\\ -\sin (\mathrm{\θ}+{\mathrm{\ρ}}_3)& \cos (\mathrm{\θ}+{\mathrm{\ρ}}_3)& r-R\cos ({\mathrm{\θ}}_0-{\mathrm{\ρ}}_3)\\ -\sin (\mathrm{\θ}+{\mathrm{\ρ}}_4)& \cos (\mathrm{\θ}+{\mathrm{\ρ}}_4)& r-R\cos ({\mathrm{\θ}}_0-\frac{\mathrm{\π}}{2}-{\mathrm{\ρ}}_4)\end{array}\right]}^{-1}\×\left(\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\\ v_4\end{array}\right]+r\left[\begin{array}{c}{w}_1\\ w_2\\ w_3\\ w_4\end{array}\right]\right)---\left(1\right)$

The controlled quentity controlled variable U of driving wheel linear velocity is calculated as shown in formula (2)：

$U=\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\\ U_4\end{array}\right]\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\\ v_4\end{array}\right]=\left[\begin{array}{ccc}-\sin (\mathrm{\θ}+{\mathrm{\ρ}}_1)& \cos (\mathrm{\θ}+{\mathrm{\ρ}}_1)& r-R\cos ({\mathrm{\θ}}_0+\mathrm{\π}-{\mathrm{\ρ}}_1)\\ -\sin (\mathrm{\θ}+{\mathrm{\ρ}}_2)& \cos (\mathrm{\θ}+{\mathrm{\ρ}}_2)& r-R\cos ({\mathrm{\θ}}_0+\frac{\mathrm{\π}}{2}-{\mathrm{\ρ}}_2)\\ -\sin (\mathrm{\θ}+{\mathrm{\ρ}}_3)& \cos (\mathrm{\θ}+{\mathrm{\ρ}}_3)& r-R\cos ({\mathrm{\θ}}_0-{\mathrm{\ρ}}_3)\\ -\sin (\mathrm{\θ}+{\mathrm{\ρ}}_4)& \cos (\mathrm{\θ}+{\mathrm{\ρ}}_4)& r-R\cos ({\mathrm{\θ}}_0-\frac{\mathrm{\π}}{2}-{\mathrm{\ρ}}_4)\end{array}\right]*\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]-r\left[\begin{array}{c}{w}_1\\ w_2\\ w_3\\ w_4\end{array}\right]---\left(2\right)$

Wherein：

X-- fitups world coordinate system X-axis coordinate value,

Y-- fitups world coordinate system Y-axis coordinate value,

The anglec of rotation of θ -- the fitup in world coordinate system,

θ₀-- the angle of car body side opposite the angle and side, the car body fixed for structure, θ₀For certain certain value,

R-- car bodies barycenter to steering structure pivot distance,

The radius of turn of r-- steering structures,

w_{1、2、3、4}-- the angular velocity of rotation of four steering mechanism,

ρ_{1、2、3、4}-- four steering mechanism and the angle of bodywork reference frame X-axis；

Step 2) the track following error model of fitup is set up, its process includes：

Any one moment is located at, its velocity vector in world coordinate system isIt is taken in body coordinate system speed Spending vector isIt is denoted as [u v w]^T, track following errorDescription is as shown in formula (3)：

$\stackrel{\·}{e}=\left[\begin{array}{c}{\stackrel{\·}{e}}_x\\ {\stackrel{\·}{e}}_y\\ {\stackrel{\·}{e}}_{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}{u}_r{\mathrm{cos\θ}}_r-v_r{\mathrm{sin\θ}}_r-u\cos \mathrm{\θ}+v\sin \mathrm{\θ}\\ u_r{\mathrm{sin\θ}}_r+v_r{\mathrm{cos\θ}}_r-u\sin \mathrm{\θ}-v\cos \mathrm{\θ}\\ {\stackrel{\·}{\mathrm{\θ}}}_r-\stackrel{\·}{\mathrm{\θ}}\end{array}\right]---\left(3\right)$

Wherein u_r, v_rRepresent respectively by decomposing trajectories, t fitup is along bodywork reference frame X, the phase of the velocity component of Y-axis Prestige value, θ_rRepresent the desired value of car body angular velocity of rotation；

Step 3) multi-variable system decoupling, process includes：

For three-three output systems of input

$\left\{\begin{array}{c}{\stackrel{\·\·}{x}}_1=f_1(x_1,{\stackrel{\·}{x}}_1,x_2,{\stackrel{\·}{x}}_2,x_3,{\stackrel{\·}{x}}_3)+b_{11}{u}_1+b_{12}{u}_2+b_{13}{u}_3\\ {\stackrel{\·\·}{x}}_2=f_2(x_1,{\stackrel{\·}{x}}_1,x_2,{\stackrel{\·}{x}}_2,x_3,{\stackrel{\·}{x}}_3)+b_{21}{u}_1+b_{22}{u}_2+b_{23}{u}_3\\ {\stackrel{\·\·}{x}}_3=f_3(x_1,{\stackrel{\·}{x}}_1,x_2,{\stackrel{\·}{x}}_2,x_3,{\stackrel{\·}{x}}_3)+b_{31}{u}_1+b_{32}{u}_2+b_{33}{u}_3\\ y_1=x_1,y_2=x_2,y_3=x_3\end{array}\right.---\left(4\right)$

Note

X=[x₁ x₂ x₃]^T

F=[f₁ f₂ f₃]^T

U=[u₁ u₂ u₃]^T (5)

Introduce " virtual controlling amount "Formula (4) is changed into

$\left\{\begin{array}{c}\stackrel{\·\·}{x}=f(x,\stackrel{\·}{x},t)+U\\ y=x\end{array}\right.---\left(6\right)$

The input/output relation of i-th of passage is in system

$\left\{\begin{array}{c}{\stackrel{\·\·}{x}}_i=f_i(x_1,{\stackrel{\·}{x}}_1,x_2,{\stackrel{\·}{x}}_2,x_3,{\stackrel{\·}{x}}_3)+U_i\\ y_i=x_i\end{array}\right.,i=1,2,3---\left(7\right)$

Parallel 3 automatic disturbance rejection controllers of insertion can be achieved with the uneoupled control of multivariable between controlled quentity controlled variable U and output quantity y；

Wherein x₁, x₂, x₃-- the quantity of state of three-three output systems of input,

u₁, u₂, u₃-- the control input of three-three output systems of input,

y₁, y₂, y₃-- the system output of three-three output systems of input,

f₁, f₂, f₃-- each passage disturbance term,

b₁₁,b₁₂,b₁₃,b₂₁,b₂₂,b₂₃,b₃₁,b₃₂,b₃₃-- the system gain factor；

Step 4) automatic disturbance rejection controller is designed, process includes：

According to track following error model, it is three independent subsystems by contrail tracker decoupling, is divided into e_xControl back Road, e_yControl loop and e_θControl loop, is that each subsystem separately designs automatic disturbance rejection controller；

Because system is first-order system, it is not necessary to Nonlinear Tracking Differentiator, automatic disturbance rejection controller includes extended state observer and shape State Feedback Control Laws two parts, it is specific as follows；

(1) extended state observer is designed, the state and disturbance to system carry out estimation in real time and compensation, extended state observer Equation is given below：

$\left\{\begin{array}{c}e=z_1-y\\ {\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_{01}e+b_{0}u\\ {\stackrel{\·}{z}}_2=-{\mathrm{\β}}_{02}e\end{array}\right.---\left(8\right)$

Wherein, β₀₁、β₀₂For one group of parameter to be adjusted,

The estimate z of e-- outputs₁Difference with exporting y,

The control input value of u-- systems,

b₀-- compensating factor,

z₂-- the estimate of expansion state；

(2) linear state error feedback control is restrained

$\left\{\begin{array}{c}{e}_1=v_1-z_1\\ u_0=k_1{e}_1\\ u=u_0-z_2/b_0\end{array}\right.---\left(9\right)$

Wherein, k₁For proportionality coefficient,

e₁-- reference input v₁With the estimate z of output₁Difference,

u₀-- error feedback control amount.

2. a kind of fitup Trajectory Tracking Control method of use Auto Disturbances Rejection Control Technique according to claim 1, its Feature is in step 3) in, detailed process is as follows：

By formula 3), system can regard the output system of three input three as, and with u (t), v (t), w (t) is indirect control amount

$\left[\begin{array}{c}{\stackrel{\·}{e}}_x\left(t\right)\\ {\stackrel{\·}{e}}_y\left(t\right)\\ {\stackrel{\·}{e}}_{\mathrm{\φ}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{u}_r{\mathrm{cos\θ}}_r-v_r{\mathrm{sin\θ}}_r-u\cos \mathrm{\θ}+v\sin \mathrm{\θ}\\ u_r{\mathrm{sin\θ}}_r+v_r{\mathrm{cos\θ}}_r-u\sin \mathrm{\θ}-v\cos \mathrm{\θ}\\ w_r-w\end{array}\right]---\left(10\right)$

The kinematic coupling portion is made to be：

$f=\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\end{array}\right]=\left[\begin{array}{c}{f}_1(u_r,v_r,w_r,{\mathrm{\θ}}_r,t)\\ f_2(u_r,v_r,w_r,{\mathrm{\θ}}_r,t)\\ f_3({\mathrm{\θ}}_r,t)\end{array}\right]=\left[\begin{array}{c}{u}_{r}cos{\mathrm{\θ}}_r-v_{r}sin{\mathrm{\θ}}_r\\ u_r{\mathrm{sin\θ}}_r+v_r{\mathrm{cos\θ}}_r\\ w_r\end{array}\right]---\left(11\right)$

f₁,f₂,f₃For disturbance summation, static coupling unit is：

$U=\left[\begin{array}{ccc}-cos\mathrm{\θ}& sin\mathrm{\θ}& 0\\ -sin\mathrm{\θ}& -cos\mathrm{\θ}& 0\\ 0& 0& -1\end{array}\right]*\left[\begin{array}{c}u\\ v\\ w\end{array}\right]---\left(12\right)$

$B=\left[\begin{array}{ccc}-cos\mathrm{\θ}& sin\mathrm{\θ}& 0\\ -sin\mathrm{\θ}& -cos\mathrm{\θ}& 0\\ 0& 0& -1\end{array}\right]---\left(13\right)$

Using linear active disturbance rejection control technology, the summation that kinematic coupling portion is taken as on respective passage disturbs to be estimated and mend Repay, as long as ensureing that matrix B is reversible, can be achieved with the uneoupled control of system, in systems, B determinant perseverance is -1, can Realize uneoupled control.

3. a kind of fitup Trajectory Tracking Control method of use Auto Disturbances Rejection Control Technique according to claim 1, its Feature is in step 4) in, detailed process is as follows：

With e_xExemplified by the Auto-disturbance-rejection Controller Design of control subsystem, automatic disturbance rejection controller includes extended state observer and shape State Feedback Control Laws two parts：

(1) extended state observer is designed, the state and disturbance to system carry out estimation in real time and compensation, extended state observer Equation is given below：

$\left\{\begin{array}{c}e=z_1-e_x\\ {\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_{01}e+b_{0}u\\ {\stackrel{\·}{z}}_2=-{\mathrm{\β}}_{02}e\end{array}\right.---\left(14\right)$

Wherein, β₀₁、β₀₂For one group of parameter to be adjusted,

z₁--e_xObservation,

z₂-- the observation of expansion state amount；

(2) state error Feedback Control Laws

$\left\{\begin{array}{c}{e}_1=e_x-z_1\\ u_0=k_1{e}_1\\ u=u_0-z_2/b_0\end{array}\right.---\left(15\right)$

Wherein, k₁For proportionality coefficient.

4. a kind of fitup Trajectory Tracking Control method of use Auto Disturbances Rejection Control Technique according to claim 1, its The implementation process being characterised by fitup control computer is run by main executable portion, is mainly included the following steps that：

1) parameter setting：Imported in parameter in interface, r, R, θ in input type (1)₀Value, and ρ₁, ρ₂, ρ₃, ρ₄Initial value； In automatic disturbance rejection controller parameter setting interface, extended state observer gain beta is inputted₀₁、β₀₂、β₁₁、β₁₂、β₂₁、β₂₂, and k₁、 k₂、k₃Value, wherein β₁₁、β₁₂For e_yThe observer gain of control subsystem, β₂₁、β₂₂For e_θThe observer of control subsystem increases Benefit, k₂,k₃Respectively e_y,e_θState error Feedback Control Laws proportionality coefficient, after input parameter confirms, will be set by control computer Put in data feeding Computer Storage unit F lash and preserve；

2) track is set：Imported in track in interface, set movement locus, imported after the confirmation of track, will by control computer In desired guiding trajectory data deposit Flash memory cell, desired guiding trajectory is decomposed into by [u by control computer_r v_r w_r]^TMatrix And data are stored in preservation, wherein u in Flash memory cell_r,v_r,w_rIt is time t function；

3) offline debugging：The Debug button in configuration interface is clicked on, control system enters controller and debugs the stage, with straight line rail Mark is test trails, and adjusting parameter sets the β in interface₀₁、β₀₂、β₁₁、β₁₂、β₂₁、β₂₂Value and k₁、k₂、k₃, observation movement Stage track following error e_x、e_y、e_θ, thereby determine that quickly can effectively realize a parameter for fitup Trajectory Tracking Control；

4) on-line operation：Configuration interface " RUN " button is clicked on, the control computer of fitup is read from from Flash storages The optimal arrange parameter of disturbance rejection control device, and read from Flash storages by matrix [u after decomposing trajectories_r v_r w_r]^T's Data, perform " fitup Trajectory Tracking Control program ", pass through the real-time pose [x y θ] of on-line measurement fitup^T, control The rotating speed of motor processed and steering motor, realizes automatically adjusting for fitup pose, is reached in next regulating cycle When, the real-time pose of on-line measurement fitup repeats whole implementation procedure, so gone round and begun again, realizes fitup afterwards The auto-adjustment control of track following.