Underwater robot model prediction adaptive trajectory tracking control method

Abstract

The invention relates to the field of underwater robot automatic control, and aims to provide an underwater robot model prediction adaptive trajectory tracking control method. Comprising the following steps: setting an expected trajectory of an underwater robot, and sampling to obtain a target tracking waypoint sequence and a target course sequence; establishing and solving a quadratic programming problem, estimating the ocean current velocity, and calculating a control law by using an adaptive control method according to an underwater robot dynamic model; and applying the output control quantity to an execution mechanism of the underwater robot for adjusting the running track of the underwater robot. According to the method, model predictive control and adaptive control are organically combined, the model predictive control is used for processing system constraints and reducing system orders, the adaptive control is used for dealing with underwater interference and model uncertainty problems, efficient and stable trajectory tracking control is realized, and the method is suitable for large-scale popularization and application. The reliability and efficiency of the underwater robot during task execution can be improved, and the real-time requirement is met.

Description

Technical Field

[0001] The invention relates to the field of underwater robot automatic control, and in particular to an underwater robot model prediction adaptive trajectory tracking control method. Background Art

[0002] With the advancement of science and technology, underwater robots are playing an increasingly important role in the fields of ocean exploration, resource exploration, and environmental monitoring. However, achieving stable and efficient trajectory tracking requires overcoming various difficulties such as system constraints, ocean current interference, and model uncertainty. Traditional control methods are difficult to deal with these problems, and more advanced and flexible control strategies are needed to ensure the accuracy and stability of underwater robot trajectory tracking.

[0003] The adaptive control method is a typical robust controller that can estimate the system parameters online according to the system input and output, and adjust the controller parameters accordingly to ensure the dynamic performance of the system. However, the adaptive control method cannot handle system constraints, and the higher the system order, the more complex its design. Model predictive control transforms the control problem into an optimization problem, which can effectively solve the problem of system constraints, and the algorithm itself has a certain robustness. However, the model prediction method also has the problem of high computational complexity, and it is difficult to meet the real-time requirements, which will be limited in practical applications.

[0004] Therefore, in order to achieve efficient and stable trajectory tracking control, the present invention intends to propose a processing method that organically combines model predictive control and adaptive control; this has great value and significance for improving the reliability and efficiency of underwater robots when performing tasks. Summary of the invention

[0005] The technical problem to be solved by the present invention is to overcome the deficiencies in the prior art and provide an underwater robot model prediction adaptive trajectory tracking control method.

[0006] To achieve the above technical objectives, the solution of the present invention is:

[0007] A method for model prediction adaptive trajectory tracking control of an underwater robot is provided, comprising the following steps:

[0008] (1) Setting the expected trajectory of the underwater robot and obtaining the target tracking waypoint sequence and target heading sequence through sampling;

[0009] (2) Based on the kinematic model of the underwater robot, a quadratic programming problem is established using the model prediction method;

[0010] (3) Use the dung beetle optimization algorithm to solve the quadratic programming problem;

[0011] (4) Estimate ocean current velocity using ocean current observers;

[0012] (5) based on the calculation results of steps (3) and (4), the control law is calculated using an adaptive control method according to the underwater robot dynamics model;

[0013] (6) applying the control quantity output by the control law to the actuator of the underwater robot to adjust the running trajectory of the underwater robot;

[0014] (7) Determine whether the target tracking point at the current moment is the last target tracking point in the target tracking sequence. If so, complete the tracking; if not, update the next target tracking point and repeat steps (2) to (6).

[0015] The present invention also provides a computer device, comprising: at least one processor, and a memory communicatively connected to the at least one processor, wherein the memory stores instructions executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor executes the aforementioned underwater robot model prediction adaptive trajectory tracking control method.

[0016] The present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium stores computer instructions, and the computer instructions are used to enable the computer to execute the aforementioned underwater robot model prediction adaptive trajectory tracking control method.

[0017] Compared with the prior art, the present invention has the following beneficial effects:

[0018] 1. The present invention innovatively proposes to organically combine model predictive control and adaptive control, using model predictive control to deal with system constraints while reducing the system order, and then using adaptive control to deal with underwater interference and model uncertainty problems; through this innovative solution, efficient and stable trajectory tracking control is achieved.

[0019] 2. The method of the present invention not only improves the reliability and efficiency of underwater robots in performing tasks, but also provides technical support for the development of future intelligent underwater robots, and has great value and significance.

[0020] 3. The present invention combines the model prediction method to construct a quadratic programming problem, effectively solving the physical constraint problem.

[0021] 4. The design of the present invention uses an ocean current estimator, which effectively solves the problem of ocean current interference.

[0022] 5. The present invention combines adaptive control methods to effectively solve the physical constraint problem.

[0023] 6. The algorithm of the present invention has high operating efficiency and can meet the real-time requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 It is the algorithm flow chart of the present invention.

[0025] Figure 2 It is a schematic diagram of the coordinate system. DETAILED DESCRIPTION

[0026] First of all, it should be explained that the present invention relates to underwater robot navigation technology, which is an application of computer technology in the field of autonomous driving technology. In the process of implementing the present invention, the application of multiple software function modules will be involved. The applicant believes that after carefully reading the application documents and accurately understanding the implementation principle and purpose of the present invention, in combination with the existing known technology, those skilled in the art can fully use their software programming skills to implement the present invention. All those mentioned in the application documents of the present invention belong to this category, and the applicant will not list them one by one.

[0027] Part I Implementation of the Invention

[0028] The underwater robot model prediction adaptive trajectory tracking control method provided by the present invention uses the model prediction control method and the advanced dung beetle optimization algorithm to obtain the target speed and target angular velocity, uses the ocean current estimator to estimate the ocean current velocity, and then solves the final control law through adaptive control. The method specifically includes the following steps:

[0029] 1. Set the expected trajectory of the underwater robot and obtain the target tracking waypoint sequence and target heading sequence through sampling; specifically include:

[0030] (a) Assume that in the inertial coordinate system, the expected trajectory to be tracked is Y d (t) = [x d (t),y d (t),z d (t)];

[0031] Among them, x d (t), y d (t) and z d (t) are the target trajectories in three directions respectively;

[0032] (b) Calculate the target heading ψ(t) according to the following formula:

[0033]

[0034] (c) Assuming the sampling time is T, sample the above target trajectory and target heading to obtain the following target waypoint sequence x d [k], y d [k], z d [k] and the target heading sequence ψ d [k]:

[0035] x d [k] = x d (kT), k=1,2,3,…… (1.2)

[0036] y d [k] = y d (kT), k=1,2,3,……(1.3)

[0037] z d [k] = z d (kT), k=1,2,3,…… (1.4)

[0038] ψ d [k] = ψ d (kT), k=1,2,3,……(1.5)

[0039] Among them, k is the sampling sequence number.

[0040] 2. Based on the kinematic model of the underwater robot, use the model prediction method to establish a quadratic programming problem; specifically including:

[0041] (a) Establish the following kinematic equations of the underwater robot in discrete form:

[0042] η[k+1]=Aη[k]+Bν[k] (2.1)

[0043] in,

[0044] η[k]=[x[k],y[k],z[k],ψ[k]] T , is the position and heading vector of the underwater robot for the kth sampling; ν[k]=[u[k],v[k],z[k],r[k]] T , is the velocity vector of the carrier coordinate system of the underwater robot for the kth sampling, and u[k] is the forward velocity, v[k] is the lateral velocity, z[k] is the vertical velocity, and r[k] is the heading angular velocity;

[0045] (b) Establish the following prediction model:

[0046]

[0047] Among them, the predicted state vector And Δη[k]=η[k]-η[k-1];

[0048] Input vector Δν[k]=[Δu[k],Δv[k],Δw[k],Δr[k]] T =ν[k]-ν[k-1], the output vector is h[k];

[0049] and

[0050] (c) Let the prediction time domain be N p , control time domain is N c , calculate the subsequent N according to the prediction model p The predicted state at sampling time:

[0051] H[k]=Fs[k]+ΦU[k] (2.3)

[0052] in, And h[k+i|k],i=1,2,…N p is the i-th predicted output vector after k sampling time; And Δν[k+j],j=0,1,…N c -1 is N after k sampling time c -1 input vector;

[0053] (d) Set up the quadratic programming problem as follows:

[0054]

[0055] in, And the target position and heading angle vector η d [k] = [x d [k],y d [k],z d [k],ψ d [k]] T , the superscript T is the symbol for matrix transposition; And λ is a weight coefficient greater than 0, For a size of N c ×N c The identity matrix of * [k] is the solution to the quadratic programming problem and has the form The Δν * [k+j-1],j=1,2,…N c To solve the jth optimal input, the first optimal input It is used for subsequent target tracking calculations.

[0056] 3. Use advanced dung beetle optimization algorithm to solve quadratic programming problems; specifically include:

[0057] (a) Set the maximum number of iterations to Iter, the number of candidate solutions to N, the cost function to J, the upper limit of the solution to Ub and the lower limit of the solution to Lb, and the parameters S, F, and CR;

[0058] (b) Initialize N candidate solutions o(i), i = 1, 2, ... N, and calculate the corresponding N cost function values

[0059] J(o(i)); (c) Initialize the current iteration count m = 1;

[0060] (d) Determine whether the current iteration count m is greater than the maximum number of iterations Iter; if the result is yes, output the global optimal solution o * , otherwise execute the following steps;

[0061] (e) Update the optimal solution of the current iteration and the global optimal solution o * ; The update method is as follows:

[0062] Traverse all candidate solutions in the current iteration step, select the candidate solution o(i) with the smallest cost function value J(o(i)), then the candidate solution o(i) is the optimal solution of the current iteration step For the global optimal solution, if the current iteration count m = 1, the global optimal solution o * The optimal solution of the current iteration equal; if m is other values, it is necessary to compare the cost function value of the optimal solution of the current iteration step And the cost function value J(o*) of the global optimal solution in the previous iteration step, such as Smaller, newer Otherwise * remain unchanged;

[0063] (f) Update the candidate solution. The updating method is as follows:

[0064] For the first half of the candidate solutions, the update formula is:

[0065]

[0066] Among them, C1 is a normally distributed random number, C2 is a normally distributed random vector, and its dimension is the same as that of the candidate solution;

[0067] For the second half of the candidate solutions, the update formula is:

[0068]

[0069] Where S is a random number greater than 0, g is a normally distributed random vector, and its dimension is the same as that of the candidate solution;

[0070] (g) Randomly select half of the candidate solutions and perform difference and evolution operations:

[0071] The difference operation is:

[0072] d(i)=o(r1)+F×(o(r2)-o(r3))(3.3)

[0073] Among them, r1, r2 and r3 are different random numbers, and are all greater than 0 and less than or equal to N;

[0074] The evolution operation is:

[0075]

[0076] Among them, r is a uniformly distributed random number between 0 and 1; G is a random positive integer less than or equal to the dimension of the candidate solution; j represents the jth component of the solution;

[0077] (h) Compare the cost function values ​​J(o(i)) of the selected half of the candidate solutions with the corresponding J(e(i)). If J(e(i)) is smaller, update o(i) = e(i). Otherwise, keep the candidate solution unchanged.

[0078] (i) m=m+1, return to execute step (d).

[0079] It is worth noting that the final global optimal solution o * Equivalent to U described in sub-step (d) of step 2 * [k].

[0080] 4. Use current observers to estimate current velocity; specifically:

[0081] (a) Assume that the ocean current is steady and irrotational. The velocity of the ocean current in the inertial coordinate system is in and are the current velocities in each direction respectively; the estimation of the current is in and are the estimated flow velocities in each direction respectively;

[0082] The ocean current velocity in the carrier coordinate system is ν c =[u c ,v c ,w c ,0] T , whose estimated value is The conversion formula of ocean current velocity in the two coordinate systems is as follows:

[0083]

[0084] in, The superscript T is the transposition symbol, and ψ is the heading angle of the underwater robot;

[0085] (b) Assume that the position and heading of the underwater robot in the inertial coordinate system are η = [x, y, z, ψ] T , the position and heading are estimated as The speed and angular velocity of the underwater robot in the carrier coordinate system are ν = [u, v, w, r], where u, v, w are the speeds in each direction and r is the angular velocity of the heading;

[0086] (c) Build the following ocean current observer model:

[0087]

[0088] Among them, ν r =[u r ,v r ,w r ,r] is the speed and angular velocity of the underwater robot relative to the ocean current in the carrier coordinate system, and is calculated by the following formula:

[0089] ν r =ν-ν c (4.3)

[0090] And λ1 and λ2 are parameters greater than 0.

[0091] (d) The estimated value of the ocean current velocity in the inertial coordinate system is obtained by real-time solution based on the model of the ocean current observer Combined with formula (4.1), the estimated value of the ocean current velocity in the carrier coordinate system is calculated as

[0092] 5. Based on the calculation results of steps (3) and (4), the control law is calculated using an adaptive control method according to the underwater robot dynamics model; specifically, the control law includes:

[0093] (a) First, establish the dynamic model of the underwater robot relative to the current velocity:

[0094]

[0095] Among them, θ i1 and θ i2 (i=1,2,3,4) are the constant linear coefficient and nonlinear coefficient respectively; is the input coefficient, and m is the mass of the underwater robot, I z is the moment of inertia, and is the additional mass coefficient; B is the buoyancy of the underwater robot under water, and G is the gravity of the underwater robot (these quantities can be obtained by measurement);

[0096] (b) Based on the calculation results of steps (3) and (4), the target relative speed and target heading angular velocity are set as:

[0097] ν rd =[u rd ,v rd ,w rd ,r d ](4.5)

[0098]

[0099] r d =r+Δr(4.9)

[0100] (c) Estimate the linear and nonlinear coefficients as follows:

[0101]

[0102]

[0103] in and They are θ i1 and θ i2 The estimated value of (i=1,2,3,4).

[0104] (d) The control law is calculated as follows:

[0105]

[0106] where k i (i=1,2,3,4) is a constant greater than zero.

[0107] 6. Apply the control quantity output by the control law to the actuator of the underwater robot to adjust the running trajectory of the underwater robot.

[0108] 7. Determine whether the target tracking point at the current moment is the last target tracking point in the target tracking sequence. If so, complete the tracking; if not, update the next target tracking point and repeat steps 2 to 7.

[0109] Part II: A specific implementation example

[0110] For the convenience of description, the coordinate system and corresponding symbols used are first introduced. Figure 2 As shown in the figure, the inertial coordinate system is a fixed coordinate system, and its origin is set at a fixed point on the horizontal plane; while the carrier coordinate system is a moving coordinate system, and its origin is set at the buoyancy center of the underwater robot, and moves with the movement of the underwater robot. The position and heading angle of the underwater robot described in the carrier coordinate system are η = [x, y, z, ψ] T, and the speed and angular velocity ν = [u, v, w, r] of the underwater robot are described in the carrier coordinate system.

[0111] This implementation example provides a variable parameter self-disturbance rejection trajectory tracking control method for an underwater robot. The algorithm flow is as follows: Figure 1 As shown, this process is a specific implementation example of the five steps in the present invention, which includes the following ten steps:

[0112] The first step is to set the expected trajectory of the underwater robot and sample the target position sequence and target heading angle sequence. The specific steps include:

[0113] (a) Let the expected trajectory to be tracked in the inertial coordinate system be Y d (t) = [x d (t),y d (t),z d (t)], where

[0114] x d (t), y d (t) and z d (t) are the target trajectories in three directions respectively.

[0115] (b) Calculate the target heading ψ(t), the calculation formula is:

[0116] (c) Assume that the sampling time is T, sample the above target trajectory and target heading to obtain the target waypoint sequence and target heading sequence.

[0117] The target waypoint sequence on each degree of freedom includes x d [k] = x d (kT), y d [k] = y d (kT) and z d [k] = z d (kT); the target heading angle sequence is ψ d [k] = ψ d (kT). Where k = 1, 2, 3, ... is the sampling sequence number.

[0118] The second step is to obtain the target position and target heading angle at the current sampling moment.

[0119] The target position and target heading angle are obtained from the target sequence obtained in the first step. Let the target position at the current sampling time be x d ,y d , z d ; The target heading angle is ψ d .

[0120] The third step is to obtain the position and heading angle of the underwater robot at the current sampling moment and the previous sampling moment.

[0121] The position and heading angle of the underwater robot are obtained through the sensors carried. Suppose the position of the underwater robot at the current sampling moment is x, y, z; the heading angle is ψ; and the position of the underwater robot at the previous sampling moment is x', y', z'; the heading angle is ψ'.

[0122] The fourth step is to establish a quadratic programming problem based on the kinematic model of the underwater robot using the model prediction method. This includes the following steps:

[0123] (a) The kinematic equations of the underwater robot in discrete form are as follows:

[0124] η[k+1]=Aη[k]+Bν[k]

[0125] Where η[k]=[x[k],y[k],z[k],ψ[k]] T is the position and heading vector of the underwater robot sampled at the kth time, ν[k]=[u[k],v[k],z[k],r[k]] T is the velocity vector of the underwater robot in the carrier coordinate system at the kth sampling, and u[k] is the forward velocity, v[k] is the lateral velocity, z[k] is the vertical velocity, and r[k] is the heading angular velocity.

[0126]

[0127] (b) The prediction model is established as follows:

[0128]

[0129] The predicted state vector And Δη[k]=η[k]-η[k 1-]; input vector Δν[k]=[Δu[k],Δv[k],Δw[k],Δr[k]] T =ν[k]-ν[k-1], the output vector is h[k];

[0130] and

[0131] (c) Let the prediction time domain be N p , the control time domain is N c , calculate the subsequent N according to the prediction model p The predicted status at each sampling moment is as follows:

[0132] H[k]=Fs[k]+ΦU[k]

[0133] in And h[k+i|k],i=1,2,…N p is the i-th predicted output vector after k sampling time; And Δν[k+j],j=0,1,…N c -1 is N after k sampling time c -1 input vector;

[0134] (d) Set up the quadratic programming problem as follows:

[0135]

[0136] in

[0137] Among them, the target position and heading angle vector η d [k] = [x d [k],y d [k],z d [k],ψ d [k]] T , superscript T is the matrix transpose symbol; And λ is a weight coefficient greater than 0, For a size of N c ×N c The identity matrix of U * [k] is the solution to the quadratic programming problem and has the form The Δν * [k+j],j=0,1,…N c -1 is the N obtained by solution c -1 optimal input.

[0138] (e) Substitute the specific values ​​into the constructed quadratic programming problem.

[0139] Substitute the underwater robot position and heading angle data obtained in the third step into the above (a), and we can get

[0140] We can get the above (b)

[0141] η=[x,y,z,ψ] T ,Δη=[x-x',y-y',z-z',ψ-ψ'] T , So the matrix in the subsequent step And F and Φ are converted to constant matrices.

[0142] Substituting the target position and target heading angle data obtained in the second step, we can get:

[0143] In the above step (d), n d =[x d ,y d ,z d ,ψ d ] T .

[0144] From this matrix is also a constant matrix.

[0145] The quadratic optimization problem constructed in (d) above is:

[0146]

[0147] U l ≤U[k]≤U u

[0148] This problem contains only one matrix variable, U[k], and the rest are constant matrices.

[0149] The fifth step is to use the advanced dung beetle optimization algorithm to solve the quadratic programming problem. Specifically, it includes the following steps:

[0150] (a) Set the maximum number of iterations to Iter, the number of candidate solutions to N, the cost function to J, the upper limit of the solution to Ub and the lower limit of the solution to Lb, and the parameters S, F, and CR. The cost function here is the same as the optimization function of the quadratic programming problem constructed in the previous step, that is, J = (RH) T (RH)+U T QU (remove the "[k]" which only represents the sampling time), where U is the variable of this cost function.

[0151] (b) Initialize N candidate solutions o(i), i = 1, 2, ... N, and calculate the corresponding N cost function values ​​J(o(i)). o(i) has the same dimension as the variable U to be solved.

[0152] (c) Initialize the current iteration count m=1.

[0153] (d) Determine whether the current iteration count m is greater than the maximum number of iterations Iter. If the result is yes, output the global optimal solution o * ,and Otherwise, perform the following steps.

[0154] (e) Update the optimal solution of the current iteration and the global optimal solution o * , the update method is as follows:

[0155] Traverse all candidate solutions in the current iteration step, select the candidate solution o(i) with the smallest cost function value J(o(i)), then the candidate solution o(i) is the optimal solution of the current iteration step For the global optimal solution, if the current iteration count m = 1, the global optimal solution o * The optimal solution of the current iteration equal; if m is other values, it is necessary to compare the cost function value of the optimal solution of the current iteration step And the cost function value J(o*) of the global optimal solution in the previous iteration step, such as Smaller, newer Otherwise * Remain unchanged.

[0156] (f) Update the candidate solution. The updating method is as follows:

[0157] For the first half of the candidate solutions, the update formula is:

[0158]

[0159] Where C1 is a normally distributed random number, C2 is a normally distributed random vector, and its dimension is the same as that of the candidate solution.

[0160] For the second half of the candidate solutions, the update formula is:

[0161]

[0162] Where S is a random number greater than 0, g is a normally distributed random vector, and its dimension is the same as that of the candidate solution.

[0163] (g) Randomly select half of the candidate solutions and perform difference and evolution operations.

[0164] The difference operation is:

[0165] d(i)=o(r1)+F×(o(r2)-o(r3))(6.3)

[0166] Among them, r1, r2 and r3 are different random numbers, and are all greater than 0 and less than or equal to N.

[0167] The evolution operation is:

[0168]

[0169] Where r is a uniformly distributed random number between 0 and 1; G is a random positive integer that is less than or equal to the dimension of the candidate solution; j represents the jth component of the solution.

[0170] (h) Compare the cost function values ​​J(o(i)) of the selected half of the candidate solutions with the corresponding J(e(i)). If J(e(i)) is smaller, update o(i) = e(i); otherwise, keep the candidate solution unchanged.

[0171] (i) m=m+1, return to execute (d).

[0172] The sixth step is to obtain the speed and heading angular velocity of the underwater robot at the current moment, and calculate the target speed and target heading angular velocity. Specifically, the following steps are included:

[0173] (a) The speed and angular velocity of the underwater robot at the current moment are obtained through the onboard sensors. Suppose the speed of the underwater robot at the current moment is u, v, w; the angular velocity of the heading is r.

[0174] (b) Obtain the optimal velocity increment and optimal heading angular velocity increment from the optimal solution obtained in the previous step

[0175] The optimal solution obtained in the previous step is Take the corresponding element from

[0176] Δν * [k], the components of this element include the optimal velocity increment Δu, Δv, Δw and the optimal heading angular velocity increment Δr required at the current moment.

[0177] (c) Calculate the target speed and target heading angular velocity as follows:

[0178] u d =u+Δu,

[0179] v d =v+Δv,

[0180] w d =w+Δw,

[0181] r d =r+Δr

[0182] Combine the target velocity and target heading angular velocity into one vector, and we have ν d =[u d ,v d ,w d ,r d ] T .

[0183] Step 7: Use the current observer to estimate the current speed and calculate the target relative speed. This includes the following steps:

[0184] (a) Assume that the ocean current is steady and irrotational. The velocity of the ocean current in the inertial coordinate system is

[0185] in and are the current velocities in each direction. The estimation of the current is

[0186] in and are the estimated flow velocities in each direction respectively.

[0187] The ocean current velocity in the carrier coordinate system is ν c =[u c ,v c ,w c ,0] T , the conversion of ocean current velocity between two coordinate systems

[0188] The formula is as follows:

[0189]

[0190] in And ψ is the heading angle of the underwater robot.

[0191] (b) Assume that the estimated position and heading of the underwater robot is The model for designing the ocean current observer is as follows:

[0192]

[0193] where ν r =[u r ,v r ,w r ,r] is the speed and angular velocity of the underwater robot relative to the ocean current in the carrier coordinate system, and can be calculated by the following formula:

[0194] ν r =ν-ν c

[0195] And λ1 and λ2 are parameters greater than 0.

[0196] (c) Calculate the target relative speed as follows:

[0197] Through the ocean current observer, the estimated value of the ocean current velocity can be obtained as Then the target relative speed can be calculated as:

[0198]

[0199] Step 8: Based on the calculation results of steps (3) and (4) and the relative speed dynamics model, the control law is calculated using the adaptive control method. Specifically, the following steps are included:

[0200] (a) First, the dynamic model of the underwater robot relative to the current velocity is established as follows:

[0201]

[0202] where θ i1 and θ i2 (i=1,2,3,4) are the constant linear coefficient and nonlinear coefficient respectively, but the specific values ​​need to be estimated; is the input coefficient, and m is the mass of the underwater robot, I z is the moment of inertia, and is the additional mass coefficient; B is the buoyancy of the underwater robot underwater, and G is the gravity of the underwater robot. These quantities can be obtained through measurement.

[0203] (b) Estimate the linear and nonlinear coefficients as follows:

[0204]

[0205] in and They are θ i1 and θ i2 The estimated value of (i=1,2,3,4).

[0206] (c) The control law is calculated as follows:

[0207]

[0208] where k i (i=1,2,3,4) is a constant greater than zero.

[0209] The ninth step is to exert control on the underwater robot and update its status.

[0210] According to the control law calculated in the previous step, the underwater robot is controlled through its actuators. The state of the underwater robot, including position, speed, heading, and heading angular velocity, will change accordingly due to these controls.

[0211] The tenth step is to determine whether it is the last tracking target in the target sequence. If it is the last tracking target, the entire control algorithm ends, otherwise, it returns to the second step to continue executing the corresponding steps.

[0212] The present invention designs a model prediction adaptive trajectory tracking control method. First, a quadratic programming problem is constructed through the model prediction method, and an advanced dung beetle optimization algorithm is used to efficiently solve it, and the target tracking speed is obtained; then, a current observer is used to estimate the current velocity in real time; finally, the underwater robot is controlled by designing an adaptive control law. This method can not only effectively solve the problems of physical constraints, current interference and model uncertainty, and achieve stable and accurate trajectory tracking, but also the algorithm has high operating efficiency and can meet the real-time requirements in industrial scenarios.

[0213] Finally, it should be noted that: this implementation example describes the application details of the present invention in detail in combination with specific implementation steps. The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; although the present invention is described in detail with reference to the above embodiments, ordinary technicians in this field should understand that: they can still modify the technical solutions recorded in the above embodiments, or replace some or all of the technical features therein by equivalent; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A model prediction adaptive trajectory tracking control method for an underwater robot, characterized in that: The following steps are involved: (1) Setting the expected trajectory of the underwater robot and obtaining the target tracking waypoint sequence and target heading sequence through sampling; (2) Based on the kinematic model of the underwater robot, a quadratic programming problem is established using the model prediction method; (3) Use the dung beetle optimization algorithm to solve the quadratic programming problem; (4) Estimate ocean current velocity using ocean current observers; (5) based on the calculation results of steps (3) and (4), the control law is calculated using an adaptive control method according to the underwater robot dynamics model; (6) applying the control quantity output by the control law to the actuator of the underwater robot to adjust the running trajectory of the underwater robot; (7) Determine whether the target tracking point at the current moment is the last target tracking point in the target tracking sequence. If so, complete the tracking; if not, update the next target tracking point and repeat steps (2) to (6).

2. The method according to claim 1, characterized in that The step (1) specifically comprises the following steps: (a) Assume that in the inertial coordinate system, the expected trajectory to be tracked is Y d (t) = [x d (t),y d (t),z d (t)]; Among them, x d (t), y d (t) and z d (t) are the target trajectories in three directions respectively; (b) Calculate the target heading ψ(t) according to the following formula: (c) Assuming the sampling time is T, sample the above target trajectory and target heading to obtain the following target waypoint sequence x d [k], y d [k], z d [k] and the target heading sequence ψ d [k]: x d [k]=x d (kT),k=1,2,3,……(1.2) and d [k]=y d (kT),k=1,2,3,……(1.3) z d [k]=z d (kT),k=1,2,3,……(1.4) ψ d [k]=ψ d (kT),k=1,2,3,……(1.5) Among them, k is the sampling sequence number.

3. The method according to claim 1, characterized in that The step (2) specifically comprises the following steps: (a) Establish the following kinematic equations of the underwater robot in discrete form: η[k+1]=Aη[k]+Bν[k](2.1) in, η[k]=[x[k],y[k],z[k],ψ[k]] T , is the position and heading vector of the underwater robot for the kth sampling; ν[k]=[u[k],v[k],z[k],r[k]] T , is the velocity vector of the carrier coordinate system of the underwater robot for the kth sampling, and u[k] is the forward velocity, v[k] is the lateral velocity, z[k] is the vertical velocity, and r[k] is the heading angular velocity; (b) Establish the following prediction model: Among them, the predicted state vector And Δη[k]=η[k]-η[k-1]; Input vector Δν[k]=[Δu[k],Δv[k],Δw[k],Δr[k]] T =ν[k]-ν[k-1], the output vector is h[k]; and (c) Let the prediction time domain be N p , control time domain is N c , calculate the subsequent N according to the prediction model p The predicted state at the sampling time: H[k]=Fs[k]+ΦU[k](2.3) in, And h[k+i|k],i=1,2,…N p is the i-th predicted output vector after k sampling time; And Δν[k+j],j=0,1,…N c -1 is N after k sampling time c -1 input vector; (d) Set up the quadratic programming problem as follows: in, And the target position and heading angle vector η d [k] = [x d [k],y d [k],z d [k],ψ d [k]] T , the superscript T is the matrix transpose symbol; And λ is a weight coefficient greater than 0, For a size of N c ×N c The identity matrix of * [k] is the solution to the quadratic programming problem and has the form The Δν * [k+j-1],j=1,2,…N c To solve the jth optimal input, the first optimal input It is used for subsequent target tracking calculations.

4. The method according to claim 1, characterized in that: The step (3) specifically comprises the following steps: (a) Set the maximum number of iterations to Iter, the number of candidate solutions to N, the cost function to J, the upper limit of the solution to Ub and the lower limit of the solution to Lb, and the parameters S, F, and CR; (b) Initialize N candidate solutions o(i), i = 1, 2, ... N, and calculate the corresponding N cost function values J(o(i)); (c) Initialize the current iteration count m = 1; (d) Determine whether the current iteration count m is greater than the maximum number of iterations Iter; if the result is yes, output the global optimal solution o * , otherwise execute the following steps; (e) Update the optimal solution of the current iteration and the global optimal solution o * ; The update method is as follows: Traverse all candidate solutions in the current iteration step, select the candidate solution o(i) with the smallest cost function value J(o(i)), then the candidate solution o(i) is the optimal solution of the current iteration step For the global optimal solution, if the current iteration count m = 1, the global optimal solution o * The optimal solution of the current iteration equal; if m is other values, it is necessary to compare the cost function value of the optimal solution of the current iteration step and the cost function value J(o of the global optimal solution in the previous iteration * ),like Smaller, newer Otherwise o* remains unchanged; (f) Update the candidate solution. The updating method is as follows: For the first half of the candidate solutions, the update formula is: Among them, C1 is a normally distributed random number, C2 is a normally distributed random vector, and its dimension is the same as that of the candidate solution; For the second half of the candidate solutions, the update formula is: Where S is a random number greater than 0, g is a normally distributed random vector, and its dimension is the same as that of the candidate solution; (g) Randomly select half of the candidate solutions and perform difference and evolution operations: The difference operation is: d(i)=o(r1)+F×(o(r2)-o(r3))(3.3) Among them, r1, r2 and r3 are different random numbers, and are all greater than 0 and less than or equal to N; The evolution operation is: Among them, r is a uniformly distributed random number between 0 and 1; G is a random positive integer less than or equal to the dimension of the candidate solution; j represents the jth component of the solution; (h) Compare the cost function values ​​J(o(i)) of the selected half of the candidate solutions with the corresponding J(e(i)). If J(e(i)) is smaller, update o(i) = e(i). Otherwise, keep the candidate solution unchanged. (i) m=m+1, return to step (d); The final global optimal solution o * Equivalent to U described in sub-step (d) of step 2 * [k].

5. The method according to claim 1, characterized in that The step (4) specifically comprises the following steps: (a) Assume that the ocean current is steady and irrotational. The velocity of the ocean current in the inertial coordinate system is in and are the current velocities in each direction respectively; the estimation of the current is in and are the estimated flow velocities in each direction respectively; The velocity of the ocean current in the carrier coordinate system is ν c =[u c ,v c ,w c ,0] T , whose estimated value is The conversion formula of ocean current velocity in the two coordinate systems is as follows: in, The superscript T is the transposition symbol, and ψ is the heading angle of the underwater robot; (b) Assume that the position and heading of the underwater robot in the inertial coordinate system are η = [x, y, z, ψ] T , the position and heading are estimated as The speed and angular velocity of the underwater robot in the carrier coordinate system are ν = [u, v, w, r], where u, v, w are the speeds in each direction and r is the angular velocity of the heading; (c) Build the following ocean current observer model: Among them, ν r =[u r ,v r ,w r ,r] is the speed and angular velocity of the underwater robot relative to the ocean current in the carrier coordinate system, and is calculated by the following formula: n r =n-n c (4.3) And λ1 and λ2 are parameters greater than 0; (d) The estimated value of the ocean current velocity in the inertial coordinate system is obtained by real-time solution based on the model of the ocean current observer Combined with formula (4.1), the estimated value of the ocean current velocity in the carrier coordinate system is calculated as 6. The method according to claim 1, characterized in that The step (5) specifically comprises: (a) First, establish the dynamic model of the underwater robot relative to the current velocity: Among them, θ i1 and θ i2 (i=1,2,3,4) are the constant linear coefficient and nonlinear coefficient respectively; is the input coefficient, and m is the mass of the underwater robot, I z is the moment of inertia, X u , Y v , Z w and N r is the additional mass coefficient; B is the buoyancy of the underwater robot under water, and G is the gravity of the underwater robot; (b) Based on the calculation results of steps (3) and (4), the target relative speed and target heading angular velocity are set as: ν rd =[u rd ,v rd ,w rd ,r d ](4.5) r d =r+Δr(4.9) (c) Estimate the linear and nonlinear coefficients as follows: in and They are θ i1 and θ i2 The estimated value of (i=1,2,3,4); (d) The control law is calculated as follows: where k i (i=1,2,3,4) is a constant greater than zero.

7. A computer device, characterized in that: include: At least one processor, and a memory communicatively connected to the at least one processor, wherein the memory stores instructions executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor executes the underwater robot model prediction adaptive trajectory tracking control method according to any one of claims 1 to 6.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable the computer to execute the underwater robot model prediction adaptive trajectory tracking control method according to any one of claims 1 to 6.

Images