CN109901622B - Autonomous underwater robot prediction S-surface control method based on mechanism model - Google Patents
Autonomous underwater robot prediction S-surface control method based on mechanism model Download PDFInfo
- Publication number
- CN109901622B CN109901622B CN201910277211.2A CN201910277211A CN109901622B CN 109901622 B CN109901622 B CN 109901622B CN 201910277211 A CN201910277211 A CN 201910277211A CN 109901622 B CN109901622 B CN 109901622B
- Authority
- CN
- China
- Prior art keywords
- control
- prediction
- auv
- output
- underwater robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
Abstract
An autonomous underwater robot prediction S-surface control method based on a mechanism model relates to a control method of an autonomous underwater robot. The method aims to solve the problem that the existing AUV S-surface control method is difficult to obtain optimal control parameters or adapt to complex changing marine environment, so that the motion control effect is influenced. The invention aims at an AUV control model, adopts a classical S-surface control method to carry out closed-loop control on the AUV, an S-surface control link outputs control quantity in each control beat, and a control parameter k of the S-surface control link in the controller1And k is2The prediction structure completes setting and adjustment in each parameter setting beat through a bacterial foraging algorithm. The invention is suitable for controlling the autonomous underwater robot.
Description
Technical Field
The invention belongs to the technical field of control, and particularly relates to a control method of an autonomous underwater robot.
Background
With the increase of the strategic position of the ocean, the importance of Autonomous Underwater Vehicles (AUV) has become increasingly prominent in recent years. The AUV can independently complete operation contents according to task requirements, has important research value and wide application prospect, and is indispensable in ocean development and exploration at present.
The AUV relates to a plurality of subject fields such as computer, control, material and the like, and integrates a plurality of key technologies such as advanced design and manufacturing technology, energy and propulsion technology, underwater navigation technology, underwater communication technology and the like. The motion control technology is an important content of the AUV technology, and the smooth completion of operation tasks in a complex marine environment can be ensured only if the AUV has good control performance.
AUV motion has the characteristics of strong nonlinearity, complex and changeable environment and the like, and the current motion control method applied to AUV has PID control, fuzzy control, sliding mode variable structure control, self-adaptive control and the like. Because of the limitations of the control methods and the difficulty in adapting to the motion characteristics of the AUV, the control methods have respective defects in the actual engineering, and the effect of controlling the motion of the AUV is still poor.
In order to enhance the performance of the AUV motion controller, researchers have proposed an S-plane control method. The method integrates the ideas of fuzzy control and PID control, adopts sigmoid surface functions to fit a control object, and is successfully applied to the multi-type AUV at present. However, in current engineering applications, the S-plane controller is mainly designed by the designer through experience to complete parameter setting and adjustment. The parameter adjusting mode has low efficiency, and is often difficult to obtain an optimal or even good group of control parameters, and even the motion control effect of the controller is influenced due to improper parameter setting.
Therefore, the invention develops research aiming at the AUV motion Control technology, and provides a mechanism Model-based prediction S-plane Control method by combining prediction Control (MPC) and Bacterial Foraging Algorithm (BFO) on the basis of classical S-plane Control, so as to improve the Control performance of the AUV motion Control system and enhance the autonomous regulation capability and adaptability of the AUV motion Control system.
Disclosure of Invention
The invention aims to solve the problem that the existing AUV S-surface control method is difficult to obtain optimal control parameters or adapt to complex changing marine environment, thereby influencing the motion control effect.
Mechanism-based modelThe method for controlling the prediction S surface of the autonomous underwater robot is characterized in that an AUV control model is subjected to closed-loop control by a classical S surface control method, control quantity is output by an S surface control link in each control beat, and a control parameter k of the S surface control link in a controller1And k is2The prediction structure completes setting and adjustment in each parameter setting beat through a bacterial foraging algorithm; in the bacterial foraging algorithm, a position vector thetai(j,k,l)＝[xk,yk]In the formula xkAnd ykFor coordinates, respectively corresponding to control parameters k in S-plane control1And k is2。
Further, the bacterial foraging algorithm is adopted
To calculate an adaptation value; in the formula, alpha is an overshoot penalty function, alpha is taken to be 1 in a non-overshoot state, and alpha is taken to be more than 1 in an overshoot state; t represents time; e.g. of the typeΦAnd (t) is a deviation which represents the difference between the AUV state quantity predicted at a certain time in the prediction time domain and the current time target quantity.
The invention has the following beneficial effects:
compared with the existing S-surface control method based on experimental trials, the method has the problems of low on-site engineering efficiency and difficulty in obtaining good control parameters to influence the motion control effect of the controller. The prediction S-plane control based on the mechanism model can independently complete the setting and adjustment of control parameters, thereby effectively improving the field engineering efficiency and ensuring the motion control effect of the AUV.
Compared with the problem that the off-line calculation mode of the S-surface control method based on off-line optimization hardly ensures that the optimization result can adapt to the complex changing marine environment, the prediction S-surface control based on the mechanism model can be combined with navigation data acquired by the last control beat to carry out feedback correction, once rolling optimization calculation is completed in each parameter adjustment beat, and the parameters of the S-surface controller are set and adjusted on line, so that the method can better adapt to the actual navigation condition and has better motion control performance.
Drawings
FIG. 1 is a schematic diagram of a controller;
FIG. 2 is a schematic flow diagram of a bacterial foraging algorithm;
FIG. 3 is a diagram of the variation of control parameters output by the S-plane control link by the prediction structure inside the S-plane controller for longitudinal speed control;
FIG. 4 is a control effect diagram of a predictive S-plane control method and a classical S-plane control method for longitudinal speed control;
FIG. 5 is a diagram of the variation of control parameters output by the S-plane control link by the prediction structure inside the S-plane controller for heading control;
FIG. 6 is a control effect diagram of a prediction S-plane control method and a classical S-plane control method for heading control;
FIG. 7 is a control parameter variation diagram of the prediction S-plane controller internal prediction structure for depth control on the output of the S-plane control link;
fig. 8 is a control effect diagram of a prediction S-plane control method and a classical S-plane control method of depth control.
Detailed Description
The first embodiment is as follows:
before describing the present embodiment, the parameters will be described;
controller structure-related parameters:
k1,k2respectively are control parameters of the S-surface controller; u is the control quantity output by the S-plane control module (i.e. T in S-plane control)c)；yinControlling the target amount for the AUV movement; y ismThe predicted value of the AUV state output by the prediction model module; y ispThe predicted value of the AUV state output by the feedback correction module; y isoutThe state quantity is actually output by the AUV; n is the number of control beats contained in each parameter adjustment beat;
relevant parameters of an S-surface control link:
Osis a control output;
is AUV actual state quantityA rate of change of deviation from a target amount; t ismaxThe maximum thrust (moment) can be provided for the autonomous underwater robot; t iscThe thrust (moment) which is actually output after inverse normalization; delta is a fixed interference force obtained in a self-adaptive mode;
other link related parameters:
emthe correction quantity of the output value of the feedback correction module to the prediction model module; phipIs a performance index evaluation function; e.g. of the typeΦIs a deviation; alpha is an overshoot penalty coefficient; i is the number of the bacteria individual; j is the number of bacterial chemotaxis; k is the bacterial replication frequency; l is the number of bacterial migrations; thetai(j, k, l) is the coordinate point of the bacteria in solution space; c (i) is bacterial step size; Δ (i) is the bacteria random direction vector; j (i, J, k, l) is the bacterial fitness value; g (-) is a function for calculating the bacterial fitness value according to the coordinate points; viIs the sensitivity of bacteria; xmax,XminIs the most value of the variable; j. the design is a squareiAdapting the bacteria; j. the design is a squaremaxThe best fitness is obtained; k is a radical ofrandIs a random number; smaxCalculating the maximum number of steps; siCalculating the current step number;
is a bacterial health value;
maximum and minimum healthy values in the bacterial population; pedThe elimination threshold value; pselfIs an adaptive migration probability; x is the number ofk,ykThe abscissa and ordinate of the position vector.
A method for controlling a predicted S surface of an autonomous underwater robot based on a mechanism model is characterized in that aiming at an AUV control model, a basic structure (controller structure overall design) of a predicted S surface controller based on the mechanism model is shown in figure 1, the predicted structure of the S surface controller comprises a prediction link (realized by the prediction model), a feedback correction link and a rolling optimization link, and the prediction link, the feedback correction link and the rolling optimization link are set and adjusted through a bacterial foraging algorithm; z-1Indicating the number of histories at the time of the last invocationAccordingly. The controller is based on closed-loop control of the classical S-surface control method on the AUV, and the control quantity is output by an S-surface control link in each control beat. But different from the classical S-surface control, the control parameter k of the S-surface control link in the controller1And k is2The setting and adjustment are finished in each parameter setting beat by a prediction structure without manual presetting.
It should be emphasized that the prediction structure here is different from the conventional prediction controller in nature, and the structure is only responsible for outputting control parameters for the S-plane controller, but not for outputting control quantity for the controlled object. The frequent adjustment of the control parameters not only can not effectively improve the control effect, but also can greatly increase the operation burden of the system. Therefore, the prediction structure adopts an independent parameter setting beat, and each parameter setting beat corresponds to N control beats of the corresponding S-plane controller, namely: after the prediction structure completes one-time parameter setting, the S-surface control link adopts the group of parameters to complete the control quantity calculation of N control beats until the next parameter adjustment beat resets the control parameters by the prediction structure.
In summary, the predicted S-plane controller based on the mechanism model provided by the present invention mainly includes an S-plane control link and a prediction structure. And in each control beat, the S-surface control link outputs control quantity for a control object to realize closed-loop motion control of the AUV. And in each parameter setting beat, the optimal control parameters in a limited time domain are solved by the prediction structure, so that the control parameter setting of the S-plane control module is realized. The prediction structure comprises three links of a prediction model, feedback correction and rolling optimization.
And S-surface control link:
s-surface control adopts a smooth Sigmoid surface to replace a broken line surface of the whole fuzzy control rule base, and eliminates fixed deviation by adjusting S-surface deviation, wherein a function expression of the S-surface control is as follows
In the formula, OsRepresents the control output, and takes the value of-1, 1 after normalization processing](ii) a e and
representing control input, namely deviation between the actual AUV state quantity and the target quantity at the current moment and a corresponding change rate respectively, and performing normalization processing; k is a radical of1And k is2Representing control parameters, and taking (0, infinity) according to the deviation and the deviation change rate; t ismaxRepresents the maximum thrust (moment) that the autonomous underwater robot can provide; t iscThe thrust (moment) which is actually output after the denormalization is shown, namely the control quantity u (t) of the S-surface control output; δ is the fixed disturbance force obtained by the adaptive approach. In fact yinIs the current target value and is the output thrust value. In closed-loop control, the state quantity and y at the current moment are passedinCan calculate e and
obtaining the actual thrust T of the controller through the formula (1)cThe thrust acts on the AUV, and the AUV outputs a response quantity (state quantity) at the next time, based on the state quantity and yinThen calculate e and of the next time
Then substituting the formula (1) to form a closed loop.
The adjusting process of the fixed disturbance force delta is as follows:
1) is the rate of change of deviation
Setting a threshold value, and determining
Whether the value is smaller than a set threshold value, if so, turning to the step 2), otherwise, turning to the step 3);
2) storing the deviation value e of the degree of freedom into a storage array, adding 1 to a counter, judging whether the current counter reaches a trigger threshold, if so, turning to the step 4), and if not, turning to the step 3);
3) removing the first bit of the storage array, shifting all the following numerical values forward by one bit, subtracting 1 from the counter, and turning to the step 1);
4) and calculating the weighted average of the numerical values in the storage array, and calculating the offset of the motion control output of the autonomous underwater robot, so that the output of the controller is adaptively adjusted to eliminate the fixed control deviation, and the storage array and the counter are reset to execute the next cycle.
A prediction link:
the prediction model link is responsible for providing prediction data of future states of the AUV in a certain time domain, and the input and the output of each calculation of the link are as follows
ym(t+d/t)＝fm[ym(t+d-1/t),u(t+d/t)] (2)
In the formula, ym(t + d/t) is the prediction of AUV state at t + d in the prediction time domain at t time, and when d is 1, ym(t+d-1/t)＝yout(t-1) calculating an initial time model output using an actual output of the AUV at a previous time; u (t + d/t) is the control quantity at the t + d moment in the prediction time domain output by the S-surface controller at the t moment; f. ofm[·]Is a nonlinear function of the mechanism model.
A feedback correction link:
AUV belongs to a strong nonlinear system, and the ocean environment is complex and changeable, so that the difference between model prediction output and system actual output inevitably exists. Therefore, a feedback correction mechanism is introduced to modify the model prediction data to a certain extent, so that the output of the prediction structure is established on the basis of more practical data.
The feedback correction module corrects the prediction model output in the current parameter setting beat according to the deviation between the model prediction output at the previous moment and the actual output of the autonomous underwater robot, and the feedback correction module is specifically as follows
yp(t+d/t)＝ym(t+d/t)+em(t) (3)
em(t)＝yout(t-1)-ym(t-1/t-2) (4)
ym(t-1/t-2)＝fm[yout(t-2),u(t-1)] (5)
In the formula, yp(t + d/t) represents the output of the corrected model predicted value at the t + d moment in the prediction period at the t moment; e.g. of the typem(t) is a correction amount at time t; y ism(t-1/t-2) is a predicted value at the t-1 moment calculated by the prediction model according to historical data; y isoutAnd (t-1) is the state quantity actually output by the AUV at the time t-1.
And (3) a rolling optimization link:
in the calculation of the rolling optimization link, firstly, an evaluation value of the current control parameter is calculated by combining the target state quantity and the corrected group of predicted values, and then, the search is carried out in a solution space according to the evaluation value, so that a group of optimal control parameters is obtained.
In order to evaluate the control effect of a group of control parameters, the rolling optimization link selects an improved ITAE criterion as a performance index function. On the basis of ITAE (International IT AE) criterion, an overshoot penalty coefficient is introduced to improve the sensitivity to overshoot, so that the overshoot inhibition capability of the controller is enhanced, and the expression is as follows
In the formula, α is an overshoot penalty function, α is 1 in a non-overshoot state, and α > 1 in an overshoot state.
An improved bacterial foraging algorithm is adopted in the rolling optimization link to solve the optimization problem, and the algorithm mainly comprises three steps of chemotaxis operation, replication operation and migration operation.
In chemotaxis operation, a direction vector is randomly generated as the current search direction, and the maximum advance number N of a single direction is definedSThen, comparing the fitness of the new position and the old position, if the fitness of the new position is higher, continuing to search along the direction, otherwise, regenerating a new direction vector and ending the chemotaxis, and specifically calculating the following formula
J(i,j,k,l)＝g(θi(j,k,l)) (8)
In the formula, i is an individual number; j is the number of chemotaxis; k is the number of copies; l is the number of migrations; thetai(j, k, l) is a coordinate point in solution space; c (i) is the bacterial step size, and Δ (i) is the random direction vector; j (i, J, k, l) is a fitness value; g (-) is a function of calculating the fitness value from the coordinate points.
The concept of bacterial sensitivity was introduced, each bacterium operating as follows:
1) and (3) sensitivity assignment:
in the formula, ViFor the sensitivity of the bacterium, XmaxAnd XminIs the most significant variable, JiFor the suitability of the bacterium, JmaxTo the best fitness, krandIs a random number.
2) Turning: and updating the bacterial position and the fitness value.
3) Swimming: if the fitness value is better after the turnover, the user continues to move until the fitness value is not improved any more, and the moving step length is adjusted according to the following formula.
Ci＝Ci·Vi (10)
4) The sensitivity was adjusted as follows:
in the formula, smaxFor maximum calculation of the number of steps, siThe number of steps is calculated for the current time.
In the copying operation, the health values of all individuals are calculated and sequenced, the half with lower health degree is directly eliminated, and the other half is copied at the same position in a solution space, so that more bacteria are kept to search in a region with higher adaptability on the premise of keeping the number of individuals in a population unchanged. Wherein the health value calculation formula is as follows
In the migration operation, a elimination threshold is set firstly, then a random number is generated for each bacterium, and the self-adaptive migration probability of each bacterium is calculated. If the random number is larger than the self-adaptive migration probability, the bacteria are eliminated and a new bacteria is generated in a solution space randomly, so that the algorithm is prevented from being premature. Wherein, the self-adaptive migration probability calculation formula is as follows
In the formula (I), the compound is shown in the specification,
and
individual, population maximum and population minimum health values, P, respectivelyedThe threshold is eliminated.
In order to apply the improved bacterial foraging algorithm specifically to the rolling optimization segment of the present invention, the fitness value in the algorithm is calculated according to the improved ITAE criterion, wherein eΦ(t) is a deviation, in particular yp(t + d/t) and yinDifference of (a), yinControlling the target amount for the AUV movement; and defining a position vector as follows
θi(j,k,l)＝[xk,yk] (14)
In the formula, xkAnd ykAs coordinates, corresponding to parameters k of the S-plane control module respectively1And k is2。
In conclusion, the rolling optimization link adopts an improved bacterial foraging algorithm to perform optimization calculation, and the optimization is performed in a parameter adjusting beat to obtain a position vector with the minimum performance index function, so that a group of optimal control parameters k is output for the S-surface control module1And k is2The algorithm flow is specifically as follows:
(1) initializing parameters:
Nd: predicting the number of control beats contained in a time domain;
s: the number of bacterial populations;
NC: tending to the upper limit of the operation times;
NS: maximum forward step number in one direction in the trend operation;
Nre: an upper limit of the number of copy operations;
Ned: an upper limit of migration operation times;
Ped: eliminating a threshold value;
c (i): step length;
Vi: sensitivity of bacteria;
and simultaneously, the values of l is 0, k is 0, j is 0,
j: the number of bacterial chemotaxis;
k: the number of bacterial replications;
l: the number of bacterial migrations;
(2) and (3) migrating operation circulation: l + 1;
(3) and (3) copying operation circulation: k is k + 1;
(4) tending to cycle operation:
(4.1) j ═ j + 1; the following trends were performed for bacterium i;
(4.2) calculating the fitness value J (i, J, k, l):
(4.2.1) time domain cycle d ═ d + 1;
(4.2.2) setting parameter k of the S-plane control Module according to equation (14)1And k is2(ii) a Here, it is to be noted that: within a certain time domain cycle, the parameter k1And k is2Is constant;
(4.2.3) calculating the deviation eΦ(t + d/t) and derivative of deviation
The deviation e (t + d/t) represents the difference between the AUV state quantity predicted in the prediction time domain and the target quantity at the current moment;
(4.2.4) calling an S-surface control module, and calculating u (t + d/t) according to the formula (1);
(4.2.5) calling a prediction model module,according to the formula (2), y is calculatedm(t+d/t)；
(4.2.6) calling the feedback correction module, and calculating y according to the formulas (3), (4) and (5)p(t+d/t)；
(4.2.7) if d < NdThen return to (4.2.1); otherwise, executing (4.2.8);
(4.2.8) calculating the fitness value J (i, J, k, l) according to the equations (6) and (8);
here, it is to be noted that:
with integral sign, i.e. for calculating phipA series of e is obtainedΦ(t) predicting a set of deviations in the time domain, wherein the set of deviations is obtained by (4.2.1) - (4.2.7); so g (-) of formula (8) is actually a process, namely: first, a series of e is obtained through (4.2.1) - (4.2.7)Φ(t) subsequently according to
Calculating phipFinally, will phipAs J (i, J, k, l). In abstract terms, a certain set of parameters k is first defined1And k is2Is the same as a certain phipIn connection with this, will thenpAssociated with J (i, J, k, l), thus establishing a set of parameters k1And k is2Connection to J (i, J, k, l), i.e. J (i, J, k, l) is g (k)1,k2) Since theta is defined in (4.2.2)i(j,k,l)＝[xk,yk]Therefore, this relationship can be written as J (i, J, k, l) g (θ)i(j, k, l)).
(4.3) recording the current best fitness value Jlast＝J(i,j,k,l)；
(4.4) rotation: generating a random direction vector Δ (i);
(4.5) swimming: calculating new positions and step lengths according to the equations (7) to (11);
(4.6) calculating fitness value J (i, J +1, k, l) with reference to (4.2) said step;
(4.7) swimming:
(4.7.1) making m 0;
(4.7.2) if m is less than NS，
Let m be m +1,
if J (i, J +1, k, l) < JlastThen let JlastJ (i, J +1, k, l), and θ is updated according to equations (7) to (11)i(j +1, k, l) and step size, return to (4.6), and use θ at this timei(J +1, k, l) calculating new J (i, J +1, k, l), and returning to make m ═ m +1 operation continue to execute;
if J (i, J +1, k, l) is not less than JlastLet m equal to NSEnding the operation of the current step, and executing (4.8);
(4.8) if i < S, making i ═ i +1, and returning to (4.2); otherwise, executing (5);
(5) if j < NCReturning to (4); otherwise, executing the step (6);
(6) copying:
calculating the health degree of each bacterium
Sorting is carried out, one half with poor health degree is eliminated, and the other half is copied into two identical individuals;
(7) if k is less than NreReturning to (3); otherwise, executing (8);
(8) migration:
the elimination threshold P of each bacterium was calculated according to the formula (13)self(i) Judging, and randomly putting the eliminated bacteria into a solution space again;
(9) if l is less than NedReturning to (2); otherwise, executing (9);
(10) according to the best fitness value JlastThe corresponding position vector outputs a control parameter k of the S-surface control module1And k is2；
(11) And finishing the algorithm.
In fact, the above process may be represented as a flow shown in fig. 2, the process of the bacterial foraging algorithm of the present invention is not limited to the above process, and may be slightly different in the logic of the loop, but the essence of the above process can be implemented, and the scheme of the present invention can be implemented by adjusting the structure of the loop, so the present invention is not limited to the above process, and it should be understood that the scheme of adjusting the control parameter of the bacterial foraging algorithm is within the protection scope of the present invention.
The second embodiment is as follows:
the AUV control model described in this embodiment may have various forms, that is, the control method of the present invention may be applied to various forms of AUV control models. In some embodiments, the autonomous underwater robot control modeling process is as follows:
methods for establishing nonlinear models can be roughly divided into three major categories: mechanism modeling, experimental modeling, and nonlinear hybrid modeling combining the mechanism modeling and the experimental modeling, wherein the mechanism modeling refers to a mathematical model derived through physical-chemical theorem. The nonlinear function of the prediction model link adopts mechanism modeling, namely the AUV control model established based on the Newton-Euler equation.
The following two right-hand coordinate systems are established: firstly, fixing a coordinate system E-xi eta zeta on the earth; secondly, the motion coordinate system O-xyz moves along with the underwater robot [4]. An origin E of a fixed coordinate system E-xi eta zeta can select any point on the earth, a xi axis is positioned on a horizontal plane, and the projection of the main course of the underwater robot on the horizontal plane is taken as a positive direction; the eta axis is also positioned on the horizontal plane, and the E xi axis is rotated by 90 degrees clockwise according to the right hand rule; the zeta axis is perpendicular to the xi E eta coordinate plane and points to the geocentric as positive. Defining the position vector of the underwater robot as [ xi eta ζ ] under a fixed coordinate system]The attitude vector is
The origin O of the motion coordinate system O-xyz is generally selected at the gravity center of the underwater robot, x, y and z axes pass through the point O and are respectively positioned on a water plane, a transverse section and a longitudinal and middle section, and the positive directions respectively point to the head end, the right side and the bottom of the autonomous underwater robot according to the specification of a right-handed system. The linear velocity vector of the autonomous underwater robot is defined as [ uv w ] under the motion coordinate system]Angular velocity vector of [ p q r]。
Assuming that the fixed coordinate system coincides with the moving coordinate system, the respective attitude angles are defined as follows: heading angle
The included angle between the xi axis and the x axis on the horizontal plane is formed, and the right turn is positive; the pitch angle theta is an included angle of a xi axis and an x axis on a vertical plane, and the tail inclination is positive; the roll angle ψ is the angle between the plane xOz and the vertical plane xO ζ passing through the x-axis, the right roll being positive.
Unifying the position and attitude angle in the fixed coordinate system as a vector
The linear velocity and the angular velocity in the motion coordinate system are unified into a vector v ═ u v w p q r]TAccording to reference [4]]The autonomous underwater robot has a kinematic formula of
Where the conversion matrix J (η) ═ diag (J)1(η),J2(η)), wherein the linear velocity conversion matrix is
The angular velocity conversion matrix is
The matrix J is converted when the pitch angle theta is +/-90 DEG2(η) has no meaning, and thus the pitch angle is defined:
the underwater robot control model commonly used at home and abroad is as follows [5]
Wherein M is an inertia matrix containing an additional mass; c (upsilon) is a Coriolis centripetal force matrix which contains the additional mass; d (upsilon) is a fluid damping matrix; g (eta) is a force and moment vector of gravity and buoyancy; τ is the force and moment vector of the actuator.
Inertia matrix M ═ MRB+MAWherein M isRBIs a rigid body mass matrix, as follows
Wherein m is the mass, I is the inertia term, [ x ]G yG zG]The center of gravity is the coordinate under the motion coordinate system.
For an autonomous underwater robot completely submerged in the navigation process, a mass matrix M is addedAAll internal coefficients are constants as follows
In the formula (I), the compound is shown in the specification,
and
the data are hydrodynamic derivatives, and are acquired by the constraint model experimental data of the AUV and the combination of computational fluid dynamics and system identification technology.
The coriolis centripetal force matrix C (v) ═ CRB(v)+CA(v) In which C isRB(v) Is a rigid centripetal force matrix, as follows
CA(v) The Coriolis force matrix is
Wherein each coefficient is as follows
Fluid damping matrix D (v) ═ Dl+Dn(v) Wherein D islIs a linear damping matrix as follows
Dl＝-diag{Xu Yv Zw Kp Mq Nr} (24)
Nonlinear set inverse matrix Dn(v) Is composed of
Dn(v)＝-diag{Xu|u|u| Yv|v||v| Zw|w||w| Kp|p||p| Mq|q||q| Nr|r||r|} (25)
The force and moment vector g (eta) of gravity and buoyancy is as follows
Wherein W is gravity, B is buoyancy, [ x ]B,yB,zB]Coordinates of floating center in motion coordinate system
The force and moment vector tau of the actuator is as follows
τ＝[X Y Z K M N]T (27)
Wherein X, Y and Z are three-axis thrust forces and K, M and N are three-axis torque forces.
In the actual engineering situation, the following simplification is performed on the AUV motion model:
(1) setting the gravity center to coincide with the origin of the motion coordinate system;
(2) the gravity and the buoyancy are configured to be equal, and the floating center is right above the gravity center;
(3) the structure is assumed to have symmetry, i.e. left-right symmetry in the plane xGz and top-bottom symmetry in the plane yGz;
(4) ignoring roll motion;
(5) the force and moment which can be generated by the actuating mechanism only comprise longitudinal thrust, vertical thrust, heading turning moment and pitching moment.
In addition, the AUV six-degree-of-freedom motion model has higher complexity, so that the controller is further convenient to design and is decomposed into a horizontal plane and a vertical plane [6 ].
In summary, the AUV control model is established herein as follows:
the in-plane control model is
The vertical in-plane control model is
The model is controlled in the same manner as in the first embodiment.
Examples
In order to illustrate the effect of the present invention, the present invention is compared with the prior art, and the following is specific:
(a) s-plane control method based on experimental check
In the current engineering application, the parameters of the S-plane controller are set mainly by adopting an experimental and trial mode, namely, a designer selects a group of initial values according to experience and repeatedly adjusts the initial values according to the field conditions [9-11 ].
The method comprises the steps of firstly, repeatedly carrying out navigation experiments on an engineering site, then analyzing the motion control response of the autonomous underwater robot under multiple groups of control parameters, and finally selecting a group of better results to carry out appropriate adjustment to obtain the control parameters. The whole process consumes a great deal of time and energy, and the engineering efficiency on site is seriously reduced.
In addition, in key links such as selecting a contrast value and determining an adjustment amount, the actual operation lacks a guiding theory and depends heavily on experience, so that an optimal or even good group of control parameters is often difficult to obtain, and the parameter setting is improper, thereby affecting the motion control effect of the controller.
In contrast, the prediction S-plane control based on the mechanism model can autonomously complete the setting and adjustment of control parameters, thereby effectively improving the engineering efficiency on site and ensuring the motion control effect of the AUV.
(b) S-plane control method based on off-line optimization
Document [9] proposes solving the controller parameters by using an immune genetic algorithm and giving an evaluation function of the control parameters as shown in the following formula
In the formula, eoverIs the maximum overshoot of the system, tsIn order to reach the target value time for the first time,
for the integration value of the squared error in 100 control beats after the target is reached for the first time, α, β, and γ are all adjustment coefficients.
In addition, the above formulas are adopted as evaluation functions in the documents [10] and [11], and the particle swarm optimization and the simulated annealing algorithm are respectively improved and applied to off-line optimization of control parameters.
However, the off-line calculation method hardly ensures that the optimization result can adapt to the complex changing marine environment, so the control parameters obtained by the method are often only used for reference and are difficult to be applied on site. In contrast, the predicted S-plane control based on the mechanism model can be combined with navigation data acquired by the last control beat to perform feedback correction, one-time rolling optimization calculation is completed in each parameter adjustment beat, and parameters of the S-plane controller are set and adjusted on line, so that the S-plane controller can better adapt to actual navigation conditions and has better motion control performance.
And (3) simulation process:
a. simulation preparation:
in order to verify the motion control performance of the control method provided by the invention, a motion control simulation experiment is carried out in an MATLAB environment, and the motion control simulation experiment specifically comprises speed and heading control in a horizontal plane and depth control in a vertical plane.
Wherein the controller parameters are set as follows:
1) the control beat is selected to be 0.1s, the parameter adjustment beat is 3s, and the prediction time domain is 8 s.
2) An S-surface control module:
according to equation (1), the S-plane controller has only two control parameters k1And k is2It needs to be set. The initial time is the first parameter prediction period, namely the prediction structure autonomously completes the S-surface control parameter k1And k is2Without manually selecting the initial value.
3) A prediction model module:
calculating to obtain a hydrokinetic coefficient according to model constraint experimental data of certain AUV of Harbin engineering university, and setting the coefficients in the formulas (28) and (29), wherein the coefficient values are shown in Table 1;
TABLE 1 summary of hydrodynamic coefficients
4) A feedback correction module:
no parameters need to be set.
5) And (3) rolling optimization:
for the improved bacterial foraging algorithm, the bacterial population quantity S is set to be 30, and the upper limit N of the operation times is approachedC20, maximum number of forward steps in one direction in the trend operation NSUpper limit of number of copying operations N3re4, upper limit of migration operation number N ed2, threshold value PedStep c (i) is 0.01, 0.25. For the improved ITAE criterion, the overshoot penalty factor α is taken to be 10 in the overshoot state.
In addition, in the simulation experiment, the motion control object also adopts an AUV control model established based on a Newton-Euler equation, and the parameter assignment is as shown in the table 1.
b. And (3) simulation results:
b1, longitudinal speed control
In the horizontal plane, taking the initial velocity u 00, target speed ud1.2 m/s. FIG. 3 is a diagram of control parameter variation of an internal prediction structure of a prediction S-surface controller to S-surface control link output, and FIG. 4 is a diagram of control effects of a prediction S-surface control method and a classical S-surface control method, wherein control parameters of the classical S-surface control are selected according to experience [ k [ ]1,k2]＝[8,5]。
b2, heading control
In the horizontal plane, the cruising speed u is taken00.5m/s, initial heading
Target heading
FIG. 5 is a diagram of control parameter variation of an internal prediction structure of a prediction S-plane controller to S-plane control link output, and FIG. 6 is a diagram of control effects of a prediction S-plane control method and a classical S-plane control method, wherein control parameters of the classical S-plane control are selected according to experience [ k [ ]1,k2]＝[6,4]。
B3, depth control
In the vertical plane, the cruising speed u is taken00.5m/s, initial depth d 00, target heading dd5 m/s. FIG. 7 is a diagram of control parameter variation of an internal prediction structure of a prediction S-plane controller to S-plane control link output, and FIG. 8 is a diagram of control effects of a prediction S-plane control method and a classical S-plane control method, wherein control parameters of the classical S-plane control are selected according to experience [ k [ ]1,k2]＝[3.5,8]。
c. Simulation analysis
In the process of controlling the longitudinal speed, the heading and the depth, the analysis of the control parameter change graph can show that: in the initial control stage, the output of the prediction structure is often relatively high due to relatively large deviationLarge k1To accelerate the convergence speed and shorten the rise time; over time, the controller will decrease k appropriately1And increase k2Thereby inhibiting the control overshoot in the middle period in advance; and finally, after the controlled variable approaches the control target, the control parameter gradually tends to be stable.
In the aspect of control effect, both the classical S-surface control method and the prediction S-surface control method provided by the invention can more stably reach the control target without steady-state errors. In longitudinal speed control, the maximum overshoot of classical S-surface control reaches 0.0812m/S, and slight oscillation occurs, while the maximum overshoot of the algorithm of the invention is only 0.0260 m/S. In heading control, the classical S-plane control requires 9.8S to go up from the steady state value of 10% to 90%, whereas the algorithm of the present invention requires only 7.4S. In depth control, the maximum overshoot of classical S-plane control reaches 0.170m/S, while the algorithm of the invention hardly overshoots.
For classical S-surface control, if the control is improperly set, the phenomena of low convergence speed, large overshoot or slight oscillation can be caused, and the motion control of the autonomous underwater robot is adversely affected. However, the algorithm can autonomously complete the setting process of the parameters of the S-surface controller through an internal prediction structure, and can inhibit the overshoot of the system or improve the convergence speed while having no steady-state error and oscillation phenomenon.
In conclusion, the prediction S-plane control method provided by the invention effectively overcomes the main defects of the classical S-plane control, can automatically complete reasonable setting and adjustment of control parameters, and has good control performance.
Reference to the literature
[1] Liu academic aptitude, Xuyue, an S-surface control method for underwater robot movement [ J ]. ocean engineering, 2001,19(3):81-84.
[2] Liu is built in male China, Xuyue, an improved S-surface control method of an underwater robot [ J ]. proceedings of Harbin engineering university, 2002,23(1):33-36.
[3]Passino K M.Biomimicry of bacterial foraging for distributed optimization and control[J].IEEE Control Systems,2002,22(3):52-67.
[4] Schengda. submarine manoeuvrability [ M ]. national defense industry press, 1995.
[5] Hassan k. khalil. nonlinear system (third edition) [ M ].2005.
[6] Research on nonlinear robust control strategy of Pythium anisopliae.under-actuated autonomous underwater vehicle [ D ]. Harbin Industrial university, 2010.
[7] Improvement and application study of bacterial foraging optimization algorithm in hujie [ D ]. university of wuhan physics, 2012.
[8] Liu Xiao Long, Li Rong Jun, Yan Lian, bacterial foraging optimization algorithm [ J ] based on Gaussian distribution estimation, 2011,26(8): 1233-.
[9] Li bright, Pangyong, Wan Lei, et al, S-plane controller of underwater robot immune genetic algorithm optimization [ J ] Harbin university of engineering proceedings 2006,27(S1):324 and 330.
[10] Application of improved PSO algorithm in the S-surface motion control parameter setting of underwater robot in Tangxudong, Pangyong, Wangyanjie [ J ] application basis and Proc of engineering science, 2009,17(1): 153-160).
[11] The application of the improved simulated annealing algorithm in the optimization of the S-surface motion control parameters of the underwater robot is J, war institute, 2013,34(11):1418 + 1423.
Claims (7)
1. The utility model provides an autonomic underwater robot prediction S face control method based on mechanism model, to AUV control model, carries out closed-loop control to AUV with classical S face control method, its characterized in that:
the control quantity is output by an S-surface control link in each control beat, and a control parameter k of the S-surface control link in the controller1And k is2The prediction structure completes setting and adjustment in each parameter setting beat through a bacterial foraging algorithm; in the bacterial foraging algorithm, a position vector thetai(j,k,l)＝[xk,yk]In the formula xkAnd ykFor coordinates, respectively corresponding to control parameters k in S-plane control1And k is2；
Wherein j is the number of chemotaxis; k is the number of copies; l is the number of migrations;
the concrete process of calculating the fitness value of the bacterial foraging algorithm comprises the following steps:
(4.2.1) time domain cycle d ═ d + 1;
(4.2.2) according to θi(j,k,l)＝[xk,yk]Setting parameter k of S-plane control1And k is2；
(4.2.3) calculating the deviation eΦ(t + d/t) and derivative of deviation
(4.2.4) for S-plane control, calculating u (t + d/t) according to equation (1);
(4.2.5) prediction is made and y is calculatedm(t + d/t); the links of the prediction model are as follows:
the prediction model link is responsible for providing prediction data of future states of the AUV in a certain time domain, and the input and the output of each calculation of the link are as follows
ym(t+d/t)＝fm[ym(t+d-1/t),u(t+d/t)] (2)
In the formula, ym(t + d/t) is the prediction of AUV state at t + d in the prediction time domain at t time, and when d is 1, ym(t+d-1/t)＝yout(t-1) calculating an initial time model output using an actual output of the AUV at a previous time; u (t + d/t) is the control quantity at the t + d moment in the prediction time domain output by the S-surface controller at the t moment; f. ofm[·]A non-linear function of the mechanism model;
(4.2.6) feedback correction is performed to calculate yp(t + d/t); the feedback correction procedure is as follows:
correcting the output of the prediction model in the current parameter setting beat according to the deviation between the prediction output at the previous moment and the actual output of the autonomous underwater robot, which is specifically as follows
yp(t+d/t)＝ym(t+d/t)+em(t) (3)
em(t)＝yout(t-1)-ym(t-1/t-2) (4)
ym(t-1/t-2)＝fm[yout(t-2),u(t-1)] (5)
In the formula, yp(t + d/t) represents the output of the corrected model predicted value at the t + d moment in the prediction period at the t moment; e.g. of the typem(t) is a correction amount at time t; y ism(t-1/t-2) is a predicted value at the t-1 moment calculated by the prediction model according to historical data; y isout(t-1) is the state quantity actually output by the AUV at the time of t-1, and u (t-1) is the control quantity of the S-plane control output at the time of t-1;
(4.2.7) if d < NdThen return to (4.2.1); otherwise, executing (4.2.8);
wherein N isdIs the number of control beats contained in the prediction time domain;
(4.2.8) according to formula
Calculating a fitness value J (i, J, k, l);
J(i,j,k,l)＝g(θi(j, k, l)) wherein i is the bacterial individual number; j is the number of chemotaxis; k is the number of copies; l is the number of migrations; thetai(j, k, l) is a coordinate point in solution space, i.e., a position vector; j (i, J, k, l) is a fitness value; g (-) is a function for calculating fitness value from coordinate points, which is a process that first obtains a series of e by (4.2.1) - (4.2.7)Φ(t) subsequently according to
Calculating phipFinally, will phipAs J (i, J, k, l), where the deviation eΦ(t) is yp(t + d/t) and yinDifference of (a), ym(t + d/t) is the prediction of AUV state at t + d in the prediction time domain at t time, yinThe target amount is controlled for the AUV motion.
2. The method for predicating the S-surface control of the autonomous underwater robot based on the mechanism model according to claim 1, characterized in that a bacterial foraging algorithm is adopted
To calculate an adaptation value; in the formula, alpha is an overshoot penalty function, alpha is taken to be 1 in a non-overshoot state, and alpha is taken to be more than 1 in an overshoot state; t represents time; e.g. of the typeΦAnd (t) is a deviation which represents the difference between the AUV state quantity predicted at a certain time in the prediction time domain and the current time target quantity.
3. The method for predicting the S-surface control of the autonomous underwater robot based on the mechanism model according to claim 2, characterized in that the S-surface control process is as follows:
s-surface control adopts a smooth Sigmoid surface to replace a broken line surface of the whole fuzzy control rule base, and eliminates fixed deviation by adjusting S-surface deviation, wherein a function expression of the S-surface control is as follows
In the formula, OsRepresents the control output, and takes the value of-1, 1 after normalization processing](ii) a e and
respectively obtaining the deviation between the actual AUV state quantity and the current moment target quantity and the corresponding change rate, and performing normalization processing; k is a radical of1And k is2Representing the control parameters, take (0, + ∞); t ismaxRepresenting the maximum thrust that the autonomous underwater robot can provide; t iscThe thrust force which is actually output after the inverse normalization is shown, namely the control quantity u (t) of the S-surface control output; δ is the fixed disturbance force obtained by the adaptive approach.
4. The method for predicting the S-surface control of the autonomous underwater robot based on the mechanism model according to claim 3, characterized in that the bacterial foraging algorithm is updated in position according to the following formula:
wherein C (i) is the bacterial step size, Δ (i) is the random direction vector, ΔT(i) Is the transpose of Δ (i).
5. The method for predicting the S-surface control of the autonomous underwater robot based on the mechanism model according to claim 4, wherein the step size of the bacterial location updating process in the bacterial foraging algorithm comprises the following steps:
and (3) sensitivity assignment:
in the formula, ViFor the sensitivity of the bacterium, XmaxAnd XminIs the most significant variable, JiFor the suitability of the bacterium, JmaxTo the best fitness, krandIs a random number;
updating the position and the fitness value of the bacteria in the process of overturning the bacteria;
if the fitness value is better after turning, the swimming is continued until the fitness value is not improved any more, and the step length of the swimming is as follows
Ci＝Ci·Vi
The sensitivity was then adjusted as follows:
in the formula, smaxFor maximum calculation of the number of steps, siThe number of steps is calculated for the current time.
6. The method for predicting the S-surface control of the autonomous underwater robot based on the mechanism model according to one of claims 4 to 5, wherein in the bacterial foraging algorithm, the replication operation process is as follows:
calculating and sequencing health values of all individuals, wherein one half with lower health degree is directly eliminated, and the other half is copied at the same position in a solution space, so that more bacteria are kept to search in a region with higher fitness degree on the premise of keeping the number of individuals in a population unchanged; the health value is calculated as follows
Wherein N isCTending towards the upper limit of the number of operations.
7. The method for controlling the predicted S surface of the autonomous underwater robot based on the mechanism model according to claim 6, wherein in the bacterial foraging algorithm, the migration operation process is as follows:
firstly, setting a elimination threshold, then generating a random number for each bacterium, and calculating the self-adaptive migration probability of each bacterium; if the random number is larger than the self-adaptive migration probability, eliminating the bacteria and randomly generating a new bacterium in a solution space;
wherein, the self-adaptive migration probability calculation formula is as follows
In the formula (I), the compound is shown in the specification,
and
individual, population maximum and population minimum health values, P, respectivelyedThe threshold is eliminated.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910277211.2A CN109901622B (en) | 2019-04-08 | 2019-04-08 | Autonomous underwater robot prediction S-surface control method based on mechanism model |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910277211.2A CN109901622B (en) | 2019-04-08 | 2019-04-08 | Autonomous underwater robot prediction S-surface control method based on mechanism model |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109901622A CN109901622A (en) | 2019-06-18 |
| CN109901622B true CN109901622B (en) | 2021-09-24 |
Family
ID=66955437
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910277211.2A Active CN109901622B (en) | 2019-04-08 | 2019-04-08 | Autonomous underwater robot prediction S-surface control method based on mechanism model |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109901622B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112859891B (en) * | 2021-01-29 | 2023-03-03 | 西北工业大学 | AUV course angle control method for optimizing self-adaptive sliding mode control parameters based on particle swarm optimization |
| CN113420954A (en) * | 2021-05-08 | 2021-09-21 | 中国电建集团华东勘测设计研究院有限公司 | Engineering management service informatization method based on mechanism model |
| CN116880519B (en) * | 2023-08-15 | 2024-03-08 | 武汉船舶职业技术学院 | Submersible vehicle motion control method based on sliding mode variable structure |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1718378A (en) * | 2005-06-24 | 2006-01-11 | 哈尔滨工程大学 | S face control method of flotation under water robot motion |
| CN102843743A (en) * | 2012-07-18 | 2012-12-26 | 南京邮电大学 | Hierarchical routing method of wireless sensor network based on flora foraging optimization algorithm |
-
2019
- 2019-04-08 CN CN201910277211.2A patent/CN109901622B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1718378A (en) * | 2005-06-24 | 2006-01-11 | 哈尔滨工程大学 | S face control method of flotation under water robot motion |
| CN102843743A (en) * | 2012-07-18 | 2012-12-26 | 南京邮电大学 | Hierarchical routing method of wireless sensor network based on flora foraging optimization algorithm |
Non-Patent Citations (3)
| Title |
|---|
| 基于SVM的水下机器人预测S面控制;张国成等;《华中科技大学学报（自然科学版）》;20120331;第40卷(第3期);正文第1-2章 * |
| 基于细菌觅食行为的分布估计算法在预测控制中的应用;王雪松等;《电子学报》;20100228;第38卷(第2期);正文第1-4章 * |
| 基于预测模型的模糊参数自寻优S面控制器;何斌等;《哈尔滨工程大学学报》;20140331;第35卷(第3期);正文第1-5章 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109901622A (en) | 2019-06-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109901403B (en) | Autonomous underwater robot neural network S-plane control method | |
| CN109901622B (en) | Autonomous underwater robot prediction S-surface control method based on mechanism model | |
| WO2021103392A1 (en) | Confrontation structured control-based bionic robotic fish motion control method and system | |
| CN110238839B (en) | Multi-shaft-hole assembly control method for optimizing non-model robot by utilizing environment prediction | |
| CN112462792B (en) | Actor-Critic algorithm-based underwater robot motion control method | |
| CN111290270B (en) | Underwater robot backstepping speed and heading control method based on Q-learning parameter adaptive technology | |
| CN111176122B (en) | Underwater robot parameter self-adaptive backstepping control method based on double BP neural network Q learning technology | |
| CN111650832B (en) | Method for tracking and controlling mechanical foot posture of underwater multi-foot walking robot | |
| CN112965371B (en) | Water surface unmanned ship track rapid tracking control method based on fixed time observer | |
| Shen et al. | Dynamic surface control for tracking of unmanned surface vessel with prescribed performance and asymmetric time-varying full state constraints | |
| Tang et al. | Unscented Kalman-filter-based sliding mode control for an underwater gliding snake-like robot | |
| CN113093526A (en) | Overshoot-free PID controller parameter setting method based on reinforcement learning | |
| CN113954077B (en) | Underwater swimming mechanical arm trajectory tracking control method and device with energy optimization function | |
| Karthick et al. | Relative analysis of controller effectiveness for vertical plane control of an autonomous underwater vehicle | |
| Xiong et al. | Motion control and path optimization of intelligent AUV using fuzzy adaptive PID and improved genetic algorithm | |
| Sun et al. | An anti-rolling control method of rudder fin system based on ADRC decoupling and DDPG parameter adjustment | |
| CN109946972A (en) | Underwater robot Predictive Control System and method based on on-line study modelling technique | |
| CN115951693B (en) | Robust track tracking control method for under-actuated underwater robot | |
| CN116149179A (en) | Non-uniform track length differential evolution iterative learning control method for robot fish | |
| Zhang et al. | MPC for 3-D Trajectory Tracking of UUV with Constraints Using Laguerre Functions | |
| Wang et al. | Target-following control of a biomimetic autonomous system based on predictive reinforcement learning | |
| CN110703792A (en) | Underwater robot attitude control method based on reinforcement learning | |
| Ma et al. | Fixed-time sliding-mode reaching based trajectory tracking control of unmanned underwater vehicles | |
| Li et al. | Robust model predictive ship heading control with event-triggered strategy | |
| Zhou et al. | Robust adaptive control of underwater glider for bottom sitting-oriented soft landing |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |