CN109164792B - Prediction fault-tolerant tracking control method for unmanned submersible model - Google Patents

Abstract

The embodiment of the invention is applied to the technical field of unmanned submersible fault-tolerant tracking, and discloses an unmanned submersible model prediction fault-tolerant tracking control method, which comprises the following steps: obtaining current state parameters of the unmanned submersible vehicle; determining the relation between the given control voltage and the corresponding rotating speed; if the given control voltage and the rotating speed are 0, judging that the propeller has complete fault, and executing: deleting the corresponding thrust distribution matrix information of the fault thruster according to the thrust distribution matrix; if the actual rotating speed is not consistent with the preset rotating speed, executing the following steps: calculating a propeller fault weight coefficient, judging whether the normalized thrust value is greater than 1, if so, performing thrust solution space calculation by adopting quantum particle swarm optimization to obtain a second normalized thrust value; and tracking the track of the unmanned submersible according to the reconstructed total thrust value. By applying the embodiment of the invention, the fault-tolerant tracking under the condition of complete failure/partial failure of the propeller can be considered, and the unmanned submersible can be ensured to complete the tracking control task smoothly.

Description

A fault-tolerant tracking control method for unmanned submersible model prediction

technical field

The invention relates to the technical field of fault tolerance of underwater robots, in particular to an unmanned submersible model prediction fault tolerance tracking control method.

Background technique

As an indispensable tool for human exploration and development of the ocean, Unmanned Underwater Vehicle (UUV) is playing an increasingly important role. The advancement of unmanned underwater vehicles is mainly driven by propellers. With the advancement of science and technology, the development and application of unmanned submersibles has a significant impact on my country's marine industry, marine exploration and development, and has become a hot spot in the field of marine engineering in the world today.

At present, the traditional UUV tracking control adopts the following strategy: when the UUV is operating underwater, the current UUV pose information is obtained through the sensor, and the error value is obtained by subtracting the desired reference trajectory information. The UUV tracking control law, under the action of the control law, the UUV attitude error gradually tends to zero and the tracking is stable. However, traditional methods generally do not consider the following issues: (1) The maximum thrust constraint that the UUV's own energy can provide; (2) During the UUV tracking process, the propeller is exposed for a long time, and it is very likely to encounter seaweed, fishing nets, etc. entanglement, or motor Faults, etc. lead to failure of normal operation. At this time, traditional tracking control cannot solve such problems, and a new method is urgently needed to solve the above problems.

Therefore, how to effectively track and control the unmanned submersible has become an urgent technical problem to be solved.

SUMMARY OF THE INVENTION

In view of the above-mentioned shortcomings of the prior art, the purpose of the present invention is to provide an unmanned submersible model prediction fault-tolerant tracking control method, which can perform fault-tolerant tracking in the case of complete/partial failure of the propeller, and ensure the smooth operation of the unmanned submersible vehicle. Complete the tracking control task.

In order to achieve the above object and other related objects, the present invention provides an unmanned submersible model prediction fault-tolerant tracking control method, the method includes the steps:

Obtain the current state parameters of the unmanned submersible through the unmanned submersible control system, wherein the state parameters at least include the position, acceleration, speed and thrust value of the propeller of the unmanned submersible;

Determine the relationship between the given control voltage and the corresponding speed;

If the given control voltage and rotational speed are 0, it is judged that the thruster is completely faulty, and the steps are executed: according to the thrust distribution matrix, delete the corresponding thrust distribution matrix information of the faulty thruster, and obtain the remaining deployable thrusters through pseudo-inverse solution. a first normalized thrust value, wherein the thrust distribution matrix is a matrix composed of all thrusters; the total thrust value is reconstructed according to the thrust distribution matrix and the first normalized thrust value;

If the actual rotational speed is inconsistent with the preset rotational speed, perform the following steps: Calculate the thruster fault weight coefficient according to the ratio of the faulty thruster's rotational speed after failure to the original normal rotational speed, and obtain the normalized thrust value of each thruster through pseudo-inverse solution. , and judge whether the thrust value of the thruster is greater than 1. If so, use quantum particle swarm optimization to calculate the thrust solution space to obtain the second normalized thrust value; according to the thrust distribution matrix and the second normalized thrust value Reconstruct the total thrust value;

Track the trajectory of the unmanned submersible according to the reconstructed total thrust value.

In an implementation manner of the present invention, before the step of obtaining the current state parameters of the unmanned underwater vehicle through the unmanned underwater vehicle control system, the method further includes:

(21) Obtain the current state parameters of the unmanned submersible through the unmanned submersible control system;

(22) the current state of the unmanned submersible is input into a linear error model, and a discretized prediction output result is obtained, wherein the linear error model is to establish an error model according to the actual state and the desired state of the unmanned submersible;

(23) Use the preset reference trajectory and the predicted output result as the input of the objective function, and solve the objective function to obtain the solution result of the objective function in the control period, wherein the objective function is a preset function, the solution result is a plurality of control input increment values in the time domain of the control period;

(24) Select the first of the multiple control input increment values as the target increment value, and send it to the unmanned submersible control system to drive the unmanned submersible to move, and obtain an unmanned submersible Update status parameters of the submersible;

(25) Take the updated state of the unmanned submersible as the current state parameter of the unmanned submersible, and return to step (22).

As described above, an unmanned submersible model prediction fault-tolerant tracking control method provided by the embodiment of the present invention can take into account the fault-tolerant tracking in the case of complete/partial failure of the propeller, and ensure that the unmanned submersible successfully completes the tracking control task.

Description of drawings

1 is a schematic flow chart of an unmanned submersible model prediction fault-tolerant tracking control method according to an embodiment of the present invention;

Fig. 2 is another schematic flow chart of the unmanned submersible model prediction fault-tolerant tracking control method according to the embodiment of the present invention;

3 is another schematic flow chart of an unmanned submersible model prediction fault-tolerant tracking control method according to an embodiment of the present invention;

FIG. 4 is a schematic flowchart of still another method for predicting a fault-tolerant tracking control method for an unmanned submersible vehicle model according to an embodiment of the present invention.

Detailed ways

The embodiments of the present invention are described below through specific specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention.

Please see attached image. It should be noted that the drawings provided in this embodiment are only to illustrate the basic concept of the present invention in a schematic way, so the drawings only show the components related to the present invention rather than the number, shape and the number of components in actual implementation. For dimension drawing, the type, quantity and proportion of each component can be changed at will in actual implementation, and the component layout may also be more complicated.

As shown in Figure 1 and Figure 2, an unmanned submersible model prediction fault-tolerant tracking control method includes the following steps:

S101, obtain the current state parameters of the unmanned submersible through the unmanned submersible control system, wherein the state parameters at least include the position, acceleration, velocity and thrust value of the thruster of the unmanned submersible.

In the case of operation, the unmanned submersible can obtain its own current position information, speed information, thrust value of the thruster, etc. through the sensors carried by itself. Specifically, under normal operating conditions, the UUV obtains the current pose information and speed information according to its own sensor information, obtains and outputs the effective tracking control quantity to the UUV by solving the model predictive control method with the constraint objective function, and the UUV plays the role of the control signal. The tracking of the global reference trajectory is realized.

The model predictive control method used in the trajectory tracking control process is shown in Figure 3, in which the dashed box is the main body of the MPC controller, which is mainly composed of a linear error model, system constraints and objective functions. Specifically, the error model is first established through the actual and expected states of the UUV, and discretized; secondly, the control amount constraints and control increment constraints are added to the optimization objective function, and the adjustment of the control amount is completed in each control cycle. The optimal solution of the objective function obtains a series of control input increments in the control time domain, and acts on the UUV system as the first element in the control sequence as the actual control input increment. The control law acts on the UUV to generate the actual UUV state at the next moment. This cycle is repeated until the final tracking control is completed.

S102, determine the relationship between the given control voltage and the corresponding rotational speed.

S103 , if the given control voltage and the rotational speed are 0, it is determined that the propeller is completely faulty, and step S104 is performed. If the actual rotational speed is inconsistent with the preset rotational speed, step S105 is performed.

Specifically, the preset rotational speed can be a pre-calculated theoretical rotational speed, which is stored after the calculation. When a comparison is required, it is taken out and compared with the actual measured rotational speed to confirm whether it is consistent, and the specific standard for judging whether it is consistent or not. It can be: the preset speed can be a numerical range, for example, any range between the first numerical value and the second numerical value; for each actual speed, it is judged whether it falls within the numerical range, and if so, it means the same , otherwise, it is inconsistent.

S104, delete the corresponding thrust distribution matrix information of the faulty thruster according to the thrust distribution matrix, and obtain the first normalized thrust value of the remaining deployable thrusters through pseudo-inverse solution, wherein the thrust distribution matrix is composed of all thrusters matrix; reconstructing the total thrust value according to the thrust distribution matrix and the first normalized thrust value.

As shown in Fig. 4, exemplarily, when there is a complete failure of the ith thruster T _i , according to the thrust distribution matrix, the corresponding thrust distribution matrix information corresponding to the ith thruster is deleted, and the residual information is obtained by pseudo-inverse solution. The thrust distribution of the thruster can be arranged, that is, the normalized first normalized thrust value T _i can be obtained, so as to realize fault-tolerant tracking under the complete failure of the thruster.

S105, according to the ratio of the rotating speed after the failure of the faulty thruster to the original normal rotating speed, calculate the fault weight coefficient of the thruster, obtain the normalized thrust value of each thruster through the pseudo-inverse solution, and judge whether there is a thrust value of the thruster greater than 1 , if it exists, quantum particle swarm optimization is used to calculate the thrust solution space, and the second normalized thrust value is obtained; the total thrust value is reconstructed according to the thrust distribution matrix and the second normalized thrust value.

Assuming that when there is a partial failure of the thruster Ti, that is, the thruster can still output part of the thrust, according to the ratio of the rotating speed after each thruster failure to the original normal rotating speed, the thruster failure weight coefficient W _i is deduced _. If each thruster is partially faulty, then each thruster fault weight coefficient is obtained in turn, and the thruster fault weight coefficient matrix W=diag[W ₁ ,W ₂ ,W _i ,...W _j ...] is obtained by extension. The thrust value of each thruster is a matrix T, and the thrust distribution matrix is B. Considering the partial failure of the thruster, the relationship between the normalized thrust value of the thruster and the resultant force/moment can be expressed as follows:

First, the normalized thrust value of each thruster is obtained by pseudo-inverse solution. If all thrust values are less than 1, it can directly act on the unmanned submersible for tracking control. If the thrust value of any thruster T _i exceeds the maximum If the value is 1, then quantum particle swarm optimization is used to optimize the thrust solution space. Take the thrust value matrix T to be solved as the solution space to be solved, determine the initial parameters such as the size N of the particle swarm, the dimension D of the particle (same as the number of thrusters) and the maximum number of iterations, and calculate the adaptation of each particle. degree value, the fitness function takes the form of infinite norm to ensure that the maximum thrust value among multiple thrusters is the smallest. For each particle, its fitness value is compared to the fitness value of the best position it has experienced. If it is better, take it as the individual historical optimal value of the particle, and update the individual historical optimal position with the current position. For each particle, compare its fitness value with the fitness value of the best position experienced by the swarm. If better, update the best location.

Adjust the particle's position according to the iterative update formula.

X _(t+1) =P _i -β*(mbest-X _t )*ln(1/z)if z≥0.5

X _(t+1) = P _i +β*(mbest-X _t )*ln(1/z)if z<0.5

为(0,1)间的系数值。Among them, X _(t) and X _(t+1) correspond to the particle position information at time t and t+1, respectively, mbest is the optimal average value of the individual, β is the contraction and expansion factor, pbest and gbest are the individual optimal and the group, respectively. Optimal, z is a random number on the interval (0,1), N is the dimension of the search space,

is a coefficient value between (0,1).

Until the end condition is reached (the thrust value is within the constraint range or the maximum number of iterations, the optimized thrust value acts on the unmanned diving system to perform tracking control in the event of partial failure.

S106, track the trajectory of the unmanned submersible according to the reconstructed total thrust value.

Specifically, the process of tracking the trajectory of the unmanned submersible vehicle may be performed by using an existing solution, which is not repeated in this embodiment of the present invention.

In addition, the embodiment of the present invention also provides a specific method for trajectory tracking, the steps include:

(21) Obtain the current state parameters of the unmanned submersible through the unmanned submersible control system;

(22) Input the current state of the unmanned submersible into a discretized linear error model to obtain a discretized prediction output result, wherein the linear error model is to establish an error model according to the actual state and the expected state of the unmanned submersible ;

其中，k是采样时刻，

是状态误差，

是控制量差值，A和B分别对应状态误差的转换矩阵和控制量差值的转换矩阵。Specifically, in an implementation manner, the linear error model is:

where k is the sampling time,

is the state error,

is the control variable difference, A and B respectively correspond to the conversion matrix of the state error and the conversion matrix of the control variable difference.

(23) Use the preset reference trajectory and the predicted output result as the input of the objective function, and solve the objective function to obtain the solution result of the objective function in the control period, wherein the objective function is a preset function, the solution result is a plurality of control input increment values in the time domain of the control period;

Exemplarily, the expression of the objective function can be as follows,

其中ΔV(t)为带约束增量值，H_t和G_t为对应的转换矩阵。在每个控制周期内完成等式求解后，得到了控制时域内的一系列控制输入增量：

Moreover, the objective function can be preset and adjusted as needed, so the constrained optimization solution problem of model predictive control at each step is equivalent to solving the following quadratic programming problem:

where ΔV(t) is the incremental value with constraints, and H _t and G _t are the corresponding transformation matrices. After solving the equations in each control cycle, a series of control input increments in the control time domain are obtained:

(24) Select the first of the multiple control input increment values as the target increment value, and send it to the unmanned submersible control system to drive the unmanned submersible to move, and obtain an unmanned submersible Update status parameters of the submersible;

(25) Take the updated state of the unmanned submersible as the current state parameter of the unmanned submersible, and return to step (22).

The above-mentioned embodiments merely illustrate the principles and effects of the present invention, but are not intended to limit the present invention. Anyone skilled in the art can modify or change the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or changes made by those with ordinary knowledge in the technical field without departing from the spirit and technical idea disclosed in the present invention should still be covered by the claims of the present invention.

Claims (1)

1. an unmanned submersible model prediction fault-tolerant tracking control method, is characterized in that, described method comprises the steps: Obtain the current state parameters of the unmanned submersible through the unmanned submersible control system, wherein the state parameters at least include the position, acceleration, speed and thrust value of the propeller of the unmanned submersible; Determine the relationship between the given control voltage and the corresponding speed; If the given control voltage and rotational speed are 0, it is judged that the thruster is completely faulty, and the steps are executed: according to the thrust distribution matrix, delete the corresponding thrust distribution matrix information of the faulty thruster, and obtain the remaining deployable thrusters through pseudo-inverse solution. The first normalized thrust value, where the thrust distribution matrix is a matrix composed of all thrusters; the total thrust value is reconstructed according to the thrust distribution matrix and the first normalized thrust value; If they are inconsistent, perform the steps: calculate the propeller failure weight coefficient according to the ratio of the faulty propeller’s rotational speed after the failure to the original normal rotational speed, obtain the normalized thrust value of each propeller through pseudo-inverse solution, and determine whether there is a The thrust value is greater than 1. If it exists, quantum particle swarm optimization is used to calculate the thrust solution space, and the second normalized thrust value is obtained; the total thrust value is reconstructed according to the thrust distribution matrix and the second normalized thrust value; Track the trajectory of the unmanned submersible with the total thrust value after the configuration; Before the step of obtaining the current state parameters of the unmanned underwater vehicle through the unmanned underwater vehicle control system, the method further includes: (21) Obtain the current state parameters of the unmanned submersible through the unmanned submersible control system; (22) the current state of the unmanned submersible is input into a linear error model, and a discretized prediction output result is obtained, wherein the linear error model is to establish an error model according to the actual state and the desired state of the unmanned submersible; (23) Use the preset reference trajectory and the predicted output result as the input of the objective function, and solve the objective function to obtain the solution result of the objective function in the control period, wherein the objective function is a preset function, the solution result is a plurality of control input increment values in the time domain of the control period; (24) Select the first of the multiple control input increment values as the target increment value, and send it to the unmanned submersible control system to drive the unmanned submersible to move, and obtain an unmanned submersible Update status parameters of the submersible; (25) Take the updated state of the unmanned submersible as the current state parameter of the unmanned submersible, and return to step (22).

Images