Summary of the invention
In view of the foregoing deficiencies of prior art, the purpose of the present invention is to provide a kind of unmanned submersible's model predictions
Fault-tolerant tracking and controlling method, being capable of fault-tolerant tracking in propeller complete failure/partial fault, it is ensured that unmanned submersible
Smoothly complete tracing control task.
In order to achieve the above objects and other related objects, the present invention provides a kind of fault-tolerant tracking of unmanned submersible's model prediction
Control method, the method includes the steps:
The current state parameter of unmanned submersible is obtained by unmanned submersible's control system, wherein the state parameter
Including at least the thrust magnitude of the position of unmanned submersible, acceleration, speed and propeller;
Determine the relationship of given control voltage and corresponding revolving speed;
If given control voltage, revolving speed 0, judge that complete failure occurs in propeller, and execute step: according to thrust
Allocation matrix deletes the correspondence thrust allocation matrix information of the failure propeller, and solving to obtain residue by pseudoinverse can arrange and push away
First into device normalizes thrust magnitude, wherein thrust allocation matrix is matrix composed by all propellers;It is distributed according to thrust
Matrix and the first normalization thrust magnitude reconstruct gross thrust value;
If actual speed is not of uniform size with preset rotation speed, then follow the steps: according to turning after failure propeller failure
The fast ratio with original rated speed, calculates propeller failure weight coefficient, solves to obtain each propeller normalization by pseudoinverse
Thrust magnitude, and the thrust magnitude for judging whether there is propeller is greater than 1, and if so, carrying out thrust using quantum telepotation
Solution space calculates, and obtains the second normalization pusher force value;It is total according to thrust allocation matrix and the second normalization pusher force value reconstruct
Thrust magnitude;
Track following is carried out to the unmanned submersible according to the gross thrust value after reconstruct.
In a kind of implementation of the invention, the acquisition unmanned submersible's by unmanned submersible's control system
Before the step of current state parameter, the method also includes:
(21) current state parameter of unmanned submersible is obtained by unmanned submersible's control system;
(22) the prediction output knot of discretization will in the current state input linear error model of unmanned submersible, be obtained
Fruit, wherein error model is established by unmanned submersible's virtual condition and expectation state according to the linearity error model;
(23) using pre-set reference locus and prediction output result as the input of objective function, and to described
Objective function is solved, and the solving result of objective function in the control period is obtained, wherein the objective function is to preset
Function, the solving result is multiple controls input incremental values in the time domain in the control period;
(24) first selected in the multiple control input incremental value is used as target delta value, and sends it to institute
Unmanned submersible's control system is stated, driving unmanned submersible moves, and obtains the update state parameter of unmanned submersible;
(25) it using the more new state of the unmanned submersible as the current state parameter of the unmanned submersible, and returns
Step (22).
As described above, a kind of fault-tolerant tracking and controlling method of unmanned submersible's model prediction provided in an embodiment of the present invention, energy
Enough take into account the fault-tolerant tracking in the case of propeller complete failure/partial fault, it is ensured that unmanned submersible smoothly completes tracing control
Task.
Specific embodiment
Illustrate embodiments of the present invention below by way of specific specific example, those skilled in the art can be by this specification
Other advantages and efficacy of the present invention can be easily understood for disclosed content.The present invention can also pass through in addition different specific realities
The mode of applying is embodied or practiced, the various details in this specification can also based on different viewpoints and application, without departing from
Various modifications or alterations are carried out under spirit of the invention.
Please refer to attached drawing.It should be noted that only the invention is illustrated in a schematic way for diagram provided in the present embodiment
Basic conception, only shown in schema then with related component in the present invention rather than component count, shape when according to actual implementation
Shape and size are drawn, when actual implementation kenel, quantity and the ratio of each component can arbitrarily change for one kind, and its component cloth
Office's kenel may also be increasingly complex.
As depicted in figs. 1 and 2, a kind of fault-tolerant tracking and controlling method of unmanned submersible's model prediction, includes the following steps:
S101 passes through the current state parameter of the acquisition unmanned submersible of unmanned submersible's control system, wherein the shape
State parameter include at least the position of unmanned submersible, acceleration, speed and thruster thrust magnitude.
Under handling situations, the sensor that unmanned submersible passes through entrained by itself can obtain its own current position
Information, velocity information, thrust magnitude of thruster etc..Specifically, in the case of normal operation, sensor information of the UUV according to itself
Current posture information and velocity information are obtained, the model predictive control method acquisition of constrained objective function is carried by solving
And effective tracing control amount is exported to UUV, UUV realizes the tracking to global reference locus in the case where controlling signal function.
The model predictive control method used during Trajectory Tracking Control is as shown in Figure 3, wherein dotted line frame is MPC control
The main body of device processed is mainly made of linearity error model, system restriction and objective function.Specifically, passing through nobody first
Submersible UUV virtual condition and expectation state establish error model, and carry out sliding-model control;Secondly, by control quantity constraint with
Optimization object function is added in controlling increment constraint, completes to obtain the optimization of objective function in each control cycle
A series of controls controlled in time domain input increment, input increment for first element in the control sequence as actual control
Act on UUV system.Control law acts on UUV, generates the practical UUV state of subsequent time.It so recycles repeatedly, until final
Tracing control is completed.
S102 determines the relationship of given control voltage and corresponding revolving speed.
S103, if given control voltage, revolving speed 0, judge that complete failure occurs in propeller, execute step S104,
If actual speed is not of uniform size with preset rotation speed, step S105 is executed.
Specifically, preset rotation speed can be precompute come theoretical rotational speed, calculate later stored, when need into
It when row compares, then takes out and is compared with the revolving speed obtained measured by reality, be confirmed whether unanimously, specifically to judge whether
Consistent standard may is that preset rotation speed can be a numerical intervals, for example, between the first numerical value and second value
Any one section;For each actual speed, judge whether to fall into the numerical intervals, if it is, indicate consistent, it is no
It then, then is inconsistent.
S104 deletes the correspondence thrust allocation matrix information of the failure propeller, passes through pseudoinverse according to thrust allocation matrix
It solves and obtains remaining the first normalization thrust magnitude that can arrange propeller, wherein thrust allocation matrix is all propeller institutes group
At matrix;Gross thrust value is reconstructed according to thrust allocation matrix and the first normalization thrust magnitude.
As shown in figure 4, illustrative, when there is i-th of propeller TiWhen there is complete failure situation, according to thrust distribution moments
Battle array deletes the correspondence thrust allocation matrix information of corresponding i-th of propeller, and propeller can be arranged by solving to obtain residue by pseudoinverse
Thrust profiles to get to normalization after first normalization thrust magnitude Ti, realize the fault-tolerant tracking under propeller complete failure.
S105 calculates propeller failure power according to the ratio of revolving speed and original rated speed after failure propeller failure
Coefficient solves to obtain each propeller normalization thrust magnitude by pseudoinverse, and the thrust magnitude for judging whether there is propeller is greater than
1, and if so, carrying out the calculating of thrust solution space using quantum telepotation, obtain the second normalization pusher force value;According to
Thrust allocation matrix and the second normalization pusher force value reconstruct gross thrust value.
It is assumed that when there is propeller TiThere is partial fault, i.e. propeller remains to output par, c thrust, according to each propeller
The ratio of revolving speed and original rated speed after failure, derives propeller failure weight coefficient Wi, if there is multiple propeller portions
Divide failure, then successively obtain each propeller failure weight coefficient, extension obtains propeller failure weight coefficient matrix W=diag [W1,
W2,Wi,…Wj…].Each propeller thrust value is matrix T, and thrust allocation matrix is B, considers propeller partial fault situation
Under, the relationship between normalized propeller thrust value and resultant force/torque can be expressed as follows:
Firstly, solve to obtain each propeller normalization thrust magnitude by pseudoinverse, if all thrust magnitudes are both less than 1,
Unmanned submersible can be directly acted on and carry out tracing control, if there is any propeller TiThrust magnitude exceed maximum value 1, then
The optimizing of thrust solution space is carried out using quantum telepotation at this time.Using the thrust value matrix T of required solution as to be solved
Solution space, determine that the scale N of population, the dimension D (identical as propeller number) of particle and maximum number of iterations etc. are initial
Parameter, calculates the fitness value of each particle, and fitness function takes Infinite Norm form to guarantee the maximum in multiple propellers
Thrust magnitude is minimum.For each particle, its adaptive value is compared with the adaptive value of the desired positions lived through.If more
It is good, then as the individual history optimal value of particle, with current location more new individual history desired positions.For each grain
Son compares the fitness value of its fitness value and group desired positions experienced.If more preferable, desired positions are updated.
The position of particle is adjusted according to iteration more new formula.
X(t+1)=Pi-β*(mbest-Xt)*ln(1/z)if z≥0.5
X(t+1)=Pi+β*(mbest-Xt) * ln (1/z) if z < 0.5
Wherein, X(t),X(t+1)The particle location information at t and t+1 moment is respectively corresponded, mbest is the optimal average value of individual, β
For the shrinkage expansion factor, pbest and gbest are respectively that individual is optimal and group is optimal, and z is the random number on section (0,1), N
For search space dimension,For the coefficient value between (0,1).
Until reach termination condition (thrust magnitude in restriction range or maximum number of iterations, the thrust after being optimized with this
Value acts on the tracing control in the case of unmanned diving system progress partial fault.
S106 carries out track following to the unmanned submersible according to the gross thrust value after reconstruct.
Specifically, carrying out the track following process of unmanned submersible can be executed using currently existing scheme, the present invention is implemented
This will not be repeated here for example.
In addition, the embodiment of the invention also provides a kind of concrete mode for carrying out track following, step includes:
(21) current state parameter of unmanned submersible is obtained by unmanned submersible's control system;
(22) by the linearity error model of the current state input discretization of unmanned submersible, the prediction of discretization is obtained
Export result, wherein error is established by unmanned submersible's virtual condition and expectation state according to the linearity error model
Model;
Specifically, in a kind of implementation, linearity error model are as follows:Wherein, k is
Sampling instant,It is state error,Control amount difference, A and B respectively correspond state error transition matrix and control amount it is poor
The transition matrix of value.
(23) using pre-set reference locus and prediction output result as the input of objective function, and to described
Objective function is solved, and the solving result of objective function in the control period is obtained, wherein the objective function is to preset
Function, the solving result is multiple controls input incremental values in the time domain in the control period;
Illustratively, the expression of objective function can be as follows,
And the objective function can according to need and preset and adjust, and therefore, pact of the Model Predictive Control in each step
Beam Optimization Solution problem is all equivalent to solve following quadratic programming problem:Its
Middle Δ V (t) is belt restraining incremental value, HtAnd GtFor corresponding transition matrix.After completing equation solution in each control cycle,
A series of controls input increment being under control in time domain:
(24) first selected in the multiple control input incremental value is used as target delta value, and sends it to institute
Unmanned submersible's control system is stated, driving unmanned submersible moves, and obtains the update state parameter of unmanned submersible;
(25) it using the more new state of the unmanned submersible as the current state parameter of the unmanned submersible, and returns
Step (22).
The above-described embodiments merely illustrate the principles and effects of the present invention, and is not intended to limit the present invention.It is any ripe
The personage for knowing this technology all without departing from the spirit and scope of the present invention, carries out modifications and changes to above-described embodiment.Cause
This, institute is complete without departing from the spirit and technical ideas disclosed in the present invention by those of ordinary skill in the art such as
At all equivalent modifications or change, should be covered by the claims of the present invention.