An underwater robot model predictive adaptive trajectory tracking control method

Abstract

The present application relates to the field of underwater robot automatic control, and aims to provide a kind of underwater robot model predictive adaptive trajectory tracking control method.It includes: setting the expected trajectory of underwater robot, obtaining target tracking waypoint sequence and target heading sequence by sampling;Establish, solve quadratic programming problem and estimate the flow velocity of sea current, calculate control law using adaptive control method according to the dynamics model of underwater robot;The output control quantity is applied to the actuator of underwater robot, for adjusting the running track of underwater robot.The present application combines model predictive control and adaptive control organically, uses model predictive control to deal with system constraints while reducing system order, and uses adaptive control to deal with underwater disturbance and model uncertainty, realizes efficient and stable trajectory tracking control, can improve the reliability and efficiency of underwater robot when executing task, and meets the requirement of real-time.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of underwater robot automatic control, and in particular to an underwater robot model predictive adaptive trajectory tracking control method. BACKGROUND

[0002] With the progress of science and technology, underwater robots play an increasingly important role in the fields of ocean exploration, resource exploration and environmental monitoring. However, to achieve stable and efficient trajectory tracking, various difficulties such as system constraints, current disturbances and model uncertainties need to be overcome. Traditional control methods are difficult to cope with these problems, and more advanced and flexible control strategies are needed to ensure the accuracy and stability of underwater robot trajectory tracking.

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

[0004] Therefore, in order to realize efficient and stable trajectory tracking control, the present application proposes to combine model predictive control and adaptive control in a processing method; it has great value and significance to improve the reliability and efficiency of underwater robots in performing tasks. SUMMARY

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

[0006] To achieve the above technical purpose, the solution of the present application is:

[0007] An underwater robot model predictive adaptive trajectory tracking control method is provided, comprising the following steps:

[0008] (1) Set the expected trajectory of the underwater robot, and obtain the target tracking waypoint sequence and target heading sequence by sampling;

[0009] (2) According to the kinematic model of the underwater robot, use the model predictive method to establish a quadratic programming problem;

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

[0011] (4) Use the current observer to estimate the current flow velocity;

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

[0013] (6) The control amount output by the control law is applied to the actuator of the underwater robot for adjusting the running track of the underwater robot;

[0014] (7) It is judged whether the target tracking point at the current time is the last target tracking point in the target tracking sequence, if yes, the tracking is completed; if not, the next target tracking point is updated, and the operations of steps (2)-(6) are repeated.

[0015] The application further provides a computer device, comprising at least one processor, and a memory in communication connection with 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 to make the at least one processor execute the underwater robot model predictive adaptive trajectory tracking control method.

[0016] The application further provides a computer readable storage medium, which stores computer instructions, and the computer instructions are used to make the computer execute the underwater robot model predictive adaptive trajectory tracking control method.

[0017] Compared with the prior art, the application has the beneficial effects that:

[0018] 1、The application innovatively combines model predictive control and adaptive control, uses model predictive control to process system constraints, reduces system order, and then uses adaptive control to deal with underwater interference and model uncertainty problems; through the innovative scheme, efficient and stable trajectory tracking control is realized.

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

[0020] 3、The application combines the model predictive method to construct a quadratic programming problem, effectively solving the physical constraint problem.

[0021] 4、The application uses a current estimator to effectively solve the current disturbance problem.

[0022] 5、The application combines the adaptive control method to effectively solve the physical constraint problem.

[0023] 6、The algorithm of the application has high running efficiency and can meet the real-time requirement. BRIEF DESCRIPTION OF DRAWINGS

[0024] Figure 1 is the algorithm flowchart of the present application.

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

[0026] First of all, it should be pointed out that the present application relates to underwater robot navigation technology, which is an application of computer technology in the field of automatic driving technology. In the implementation process of the present application, the application of multiple software function modules will be involved. The applicant believes that, after carefully reading the application file and accurately understanding the implementation principle and the purpose of the present application, and in combination with the existing known technology, the skilled person in the art can certainly implement the present application by using the software programming skills mastered by them. Whatever mentioned in the present application file is within this category, and the applicant will not list them one by one.

[0027] First part of the implementation scheme of the present application

[0028] The underwater robot model predictive adaptive trajectory tracking control method provided by the present application is to obtain the target speed and target angular velocity by using the model predictive control method and the advanced ant lion optimization algorithm, estimate the sea current velocity by using the sea current estimator, and then solve the final control law by using the 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 the target heading sequence by sampling; specifically including:

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

[0031] Wherein, 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 that the sampling time is T, the above target trajectory and target heading are sampled to obtain the target waypoint sequence x d [k], y d [k], z d [k] and the target heading sequence ψ d [k] as follows:

[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] where k is the sampling index.

[0040] 2. According to the kinematic model of the underwater robot, a quadratic programming problem is established using a model prediction method; specifically including:

[0041] (a) The following discrete form of kinematic equation of the underwater robot is established:

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

[0043] where,

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

[0045] (b) The following prediction model is established:

[0046]

[0047] where the predicted state vector and Δη[k] = η[k] - η[k-1];

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

[0049] and

[0050] (c) Let the prediction time domain be N. p The control time domain is N c Calculate the subsequent N based on the prediction model p Predicted state at each sampling time:

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

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

[0053] (d) The quadratic programming problem is as follows:

[0054]

[0055] in, And the target position and heading angle vector η are included. 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 weighting coefficient that is greater than 0. For a size of N c ×N c The identity matrix; U * [k] is the solution to this quadratic programming problem, and has the form Where Δν * [k+j-1], j=1,2,…N c To find the j-th optimal input, where the 1st optimal input is... It was used in calculations for subsequent target tracking.

[0056] 3. Solve quadratic programming problems using the advanced dung beetle optimization algorithm; specifically including:

[0057] (a) Set the maximum number of iterations to Iter, the number of candidate solutions to N, the cost function to J, and the upper limit of the solution to Ub, 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 counter m = 1;

[0060] (d) judge whether the current iteration counter m is greater than the maximum iteration number 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 step 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 minimum 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 counter m = 1, the global optimal solution o * is equal to the optimal solution of the current iteration step ; 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 last iteration step, such as is smaller, then update Otherwise, o * remains unchanged;

[0063] (f) update the candidate solutions, the update method is as follows:

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

[0065]

[0066] Wherein, C1 is a normally distributed random number, C2 is a normally distributed random vector, and the 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] Wherein, S is a random number greater than 0, g is a normally distributed random vector, and the dimension is the same as that of the candidate solution;

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

[0071] The difference operation is:

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

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

[0074] The evolutionary operation is:

[0075]

[0076] wherein r is a random number uniformly distributed in 0 to 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) Comparing the cost function value J(o(i)) of the selected half candidate solution with the corresponding J(e(i)), if J(e(i)) is smaller, then updating o(i) = e(i), otherwise keeping the candidate solution unchanged;

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

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

[0080] 4. Estimating the sea current velocity using the sea current observer; specifically comprising:

[0081] (a) Assuming that the sea current is steady and non-rotational, the sea current velocity in the inertial coordinate system is wherein and are the sea current velocity values in each direction; the estimation of the sea current is wherein and are the estimated values of the velocity in each direction;

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

[0083]

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

[0085] (b) Let the position and heading of the underwater vehicle in the inertial frame be η = [x, y, z, ψ] T The estimate of the position and heading is The velocity and angular velocity of the underwater vehicle in the body frame are v = [u, v, w, r], where u, v, w are the velocities in each direction, and r is the angular velocity of the heading;

[0086] (c) Establish the model of the current observer as follows:

[0087]

[0088] where v r = [u r , v r , w r , r] is the velocity and angular velocity of the underwater vehicle relative to the current in the body frame, and is calculated by:

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

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

[0091] (d) Real-time solve the current flow velocity estimate in the inertial frame according to the model of the current observer And then calculate the current flow velocity estimate in the body frame according to equation (4.1)

[0092] 5. Based on the calculation results of steps (3) and (4), use an adaptive control method to calculate the control law according to the dynamics model of the underwater vehicle; specifically including:

[0093] (a) First, establish the dynamics model of the underwater vehicle relative to the current flow velocity as follows:

[0094]

[0095] where θ i1 and θ i2 (i = 1, 2, 3, 4) are constant linear and nonlinear coefficients, respectively; is the input coefficient, and m is the mass of the underwater vehicle, I z is the moment of inertia, and are added mass coefficients; B is the buoyancy of the underwater vehicle in water, and G is the gravity of the underwater vehicle (these quantities can be measured);

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

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

[0098]

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

[0100] (c) The linear and nonlinear coefficients are estimated as follows:

[0101]

[0102]

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

[0104] (d) The formula for calculating the control law is as follows:

[0105]

[0106] Where k i (i = 1, 2, 3, 4) are constants greater than zero.

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

[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, the tracking is completed; otherwise, update the next target tracking point and repeat steps 2 to 7.

[0109] Part Two: A Specific Implementation Example

[0110] For ease of description, we will first introduce the coordinate system used and its corresponding symbols. For example... Figure 2 As shown, the inertial coordinate system is a fixed coordinate system with its origin at a fixed point on the horizontal plane; while the carrier coordinate system is a moving coordinate system with its origin at the underwater robot's center of buoyancy, moving with the underwater robot. The position and heading angle of the underwater robot in the carrier coordinate system are described by η = [x, y, z, ψ]. TIn the carrier coordinate system, the velocity and heading angular velocity of the underwater robot are described as ν = [u,v,w,r].

[0111] This implementation example provides a variable parameter active 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 this invention, which includes the following ten steps in total:

[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. This includes the following steps:

[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) represents the target trajectory in three directions.

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

[0116] (c) Let the sampling time be T. Sample the above target trajectory and target heading to obtain the target waypoint sequence and target heading sequence.

[0117] The sequence of target waypoints in 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 time.

[0119] Both the target position and the 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] Thirdly, the position and heading angle of the underwater vehicle at the current sampling time and the last sampling time are obtained.

[0121] The position and heading angle of the underwater vehicle are obtained by the sensors carried by the underwater vehicle. Let the position of the underwater vehicle at the current sampling time be x, y, z, and the heading angle be ψ, and the position of the underwater vehicle at the last sampling time be x', y', z', and the heading angle be ψ'.

[0122] Fourthly, a quadratic programming problem is established by using a model prediction method according to the kinematic model of the underwater vehicle. Specifically, the following steps are included:

[0123] (a) The kinematic equation of the underwater vehicle in discrete form is established as follows:

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

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

[0126]

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

[0128]

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

[0130] and

[0131] (c) Let the prediction time domain be N p , the control time domain be N c , and the prediction state at the subsequent N p sampling time be calculated according to the prediction model as follows:

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

[0133] wherein and h[k+i|k], i = 1, 2,...N p is the i-th predicted output vector after k samples; and Δν[k+j], j = 0, 1,...N c -1is the N c -1input vector after k samples;

[0134] (d) Establish a quadratic programming problem as follows:

[0135]

[0136] where

[0137] where 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;

[0138] and λ is a weight coefficient greater than 0, is an N c ×N c unit matrix;

[0139] U * [k] is the solution of the quadratic programming problem, and has the form where Δν * [k+j], j = 0, 1,...N c -1is the N c -1optimal input obtained by solving.

[0140] (e) Bring the specific values into the constructed quadratic programming problem.

[0141] The underwater robot position and heading angle data obtained in the third step are brought in, and the above (a) can be obtained

[0142] The above (b) can be obtained

[0143] η = [x, y, z, ψ] T , Δη = [x-x', y-y', z-z', ψ-ψ'] T , From which the matrix in the subsequent steps and F and Φ are all converted to constant matrices.

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

[0145] In step (d) above, η d =[x d ,y d ,z d ,ψ d ] T .

[0146] Therefore, the matrix It is also a constant matrix.

[0147] Therefore, the quadratic optimization problem constructed in (d) above is:

[0148]

[0149] stU l ≤U[k]≤U u

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

[0151] The fifth step involves solving the quadratic programming problem using the advanced dung beetle optimization algorithm. This includes the following steps:

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

[0153] (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.

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

[0155] (d) Determine if the current iteration count m is greater than the maximum iteration count Iter. If the result is yes, output the global optimal solution o. * ,and Otherwise, proceed with the following steps.

[0156] (e) Update the optimal solution for the current iteration step. and the global optimal solution o * The update method is as follows:

[0157] Iterate through all candidate solutions in the current iteration step, and select the candidate solution o(i) with the smallest cost function value J(o(i)). This candidate solution o(i) is the optimal solution for the current iteration step. For the global optimal solution, if the current iteration count m = 1, then the global optimal solution is o. * The optimal solution of the current iteration step If they are equal; if m is any other value, then the cost function value of the optimal solution in the current iteration step needs to be compared. The cost function value J(o*) of the global optimal solution in the previous iteration step, such as If smaller, then update. Otherwise o * It remains unchanged.

[0158] (f) Update candidate solutions, using the following method:

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

[0160]

[0161] Where C1 is a normally distributed random number and C2 is a normally distributed random vector, and their dimensions are the same as those of the candidate solutions.

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

[0163]

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

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

[0166] The difference operation is as follows:

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

[0168] Where r1, r2, and r3 are distinct random numbers, and all are greater than 0 and less than or equal to N.

[0169] The evolution operation is as follows:

[0170]

[0171] Where 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; and j represents the j-th component of the solution.

[0172] (h) Compare the cost function value J(o(i)) of the selected half of the candidate solutions with the corresponding J(e(i)), if J(e(i)) is smaller, then update o(i) = e(i), otherwise keep the candidate solution unchanged.

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

[0174] Step 6, get the current speed of the underwater robot, heading angle speed, and calculate the target speed and target heading angle speed. Specifically, it includes the following steps:

[0175] (a) The speed and heading angle speed of the underwater robot at the current time are obtained through the sensor, and the speed of the underwater robot at the current time is set as u, v, w; the heading angle speed is set as r.

[0176] (b) Get the optimal speed increment and optimal heading angle speed increment from the optimal solution obtained in the last step

[0177] The optimal solution obtained in the last step is Take out the corresponding elements from it

[0178] Δν * [k], the components of the element contain the optimal speed increment Δu, Δv, Δw and the optimal heading angle speed increment Δr required at the current time.

[0179] (c) Calculate the target speed and target heading angle speed as follows:

[0180] u d =u+Δu,

[0181] v d =v+Δv,

[0182] w d =w+Δw,

[0183] r d =r+Δr

[0184] Combine the target speed and target heading angle speed into a vector, ν d =[u d ,v d ,w d ,r d ] T .

[0185] Step 7, estimate the current speed using the current observer and calculate the target relative speed. Specifically, it includes the following steps:

[0186] (a) Set the current as constant and irrotational, the current velocity in the inertial coordinate system is

[0187] where and are the current velocity values in each direction. The estimate of the current is

[0188] where and are the current velocity estimates in each direction.

[0189] The current velocity in the body frame is v c = [u c , v c , w c , 0] T The transformation of the current velocity between the two frames is

[0190] The equations are as follows:

[0191]

[0192] where and ψ is the heading angle of the underwater vehicle.

[0193] (b) Let the estimate of the underwater vehicle's position and heading be The model of the current observer is designed as follows:

[0194]

[0195] where v r = [u r , v r , w r , r] is the underwater vehicle's velocity and angular velocity relative to the current in the body frame, and can be calculated by

[0196] v r = v - v c

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

[0198] (c) The target relative velocity is calculated as follows:

[0199] Through the current observer, the estimate of the current velocity is The target relative velocity can be calculated as:

[0200]

[0201] Step 8, based on the calculation results of steps (3) and (4), and the relative velocity dynamics model, use adaptive control method to calculate the control law. Specifically, it includes the following steps:

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

[0203]

[0204] where θ i1 and θ i2 (i = 1, 2, 3, 4) are constant linear and nonlinear coefficients, respectively, but the specific values need to be estimated; is an input coefficient, and m is the mass of the underwater robot, I z is the moment of inertia, and are added mass coefficients; B is the buoyancy of the underwater robot in water, and G is the gravity of the underwater robot, which can be measured.

[0205] (b) The linear and nonlinear coefficients are estimated as follows:

[0206]

[0207] where and are the estimated values of θ i1 and θ i2 (i = 1, 2, 3, 4), respectively.

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

[0209]

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

[0211] Ninth, the control is applied to the underwater robot and its state is updated.

[0212] According to the control law calculated in the last step, the control is applied to the underwater robot through its actuators. The state of the underwater robot, including position, velocity, heading, and heading angular velocity, etc., will change accordingly due to the control.

[0213] Tenth, it is determined whether it is the last tracking target of the target sequence. If it is the last tracking target, the entire control algorithm ends, otherwise, it returns to the second step to continue the corresponding steps.

[0214] The application designs a model predictive adaptive trajectory tracking control method, first, a quadratic programming problem is constructed through a model prediction method, and an advanced ant lion optimization algorithm is used to solve it efficiently to obtain the target tracking speed; then a current observer is used to estimate the current flow rate in real time; finally, an adaptive control law is designed to control the underwater robot. The method can not only effectively solve the problems of physical constraints, current disturbance and model uncertainty, realize stable and accurate trajectory tracking, but also has high algorithm running efficiency and can meet the real-time requirements in industrial scenes.

[0215] Finally, it should be noted that: the application examples combine specific implementation steps to elaborate the application details. The above examples are only used to illustrate the technical solutions of the application, but not to limit them; although the application has been described in detail with reference to the foregoing examples, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing examples, or make equivalent replacement for part or all of the technical features; 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 application.

Claims

1. A model predictive adaptive trajectory tracking control method for an underwater robot, characterized by, The method comprises the following steps: (1) setting an expected trajectory of the underwater robot, obtaining a target tracking waypoint sequence and a target heading sequence by sampling; (2) establishing a quadratic programming problem by using a model prediction method according to a kinematics model of the underwater robot; (3) solving the quadratic programming problem by using a dung beetle optimization algorithm; (4) estimating a sea current flow velocity by using a sea current observer; (5) calculating a control law according to a dynamics model of the underwater robot by using an adaptive control method based on calculation results of steps (3) and (4); a calculation formula of the control law is specifically as follows: where τ u , τ v , τ w , τ r correspond to the control law of the underwater robot in the carrier coordinate system in each direction velocity u, v, w and the heading angle velocity r respectively; k i is a constant greater than zero, i = 1, 2, 3, 4; u rd , v rd , w rd , r d are the relative velocities in each direction and the target heading angle velocity respectively; u r , v r , w r , r are the velocities and angular velocities relative to the sea current respectively; is the estimated value of the constant linear coefficient; is the estimated value of the constant nonlinear coefficient; B is the buoyancy of the underwater robot under water; G is the gravity of the underwater robot; b1, b2, b3, b4 are input coefficients; (6) applying a control amount output by the control law to an executing mechanism of the underwater robot, and adjusting a running trajectory of the underwater robot; (7) judging whether a current time point is a last target tracking point in the target tracking sequence, if yes, completing tracking, and if not, updating a next target tracking point, and repeating operations of steps (2)-(6).

2. The method of claim 1, wherein, The step (1) specifically comprises the following steps: (a) assuming in an inertial coordinate system, the intended trajectory to be tracked is Y d (t) = [x d (t),y d (t),z d (t)] ; wherein x d (t), y d (t), and z d (t) are target trajectories in three directions, respectively; (b) calculating a target heading ψ(t) according to the following formula: (c) assuming a sampling time of T, sampling the above target trajectory and target heading to obtain a sequence of target waypoints x d [k], y d [k], z d [k] and target heading sequence ψ d [k] : x d [k] = x d (kT), k = 1, 2, 3,... (1.2) y 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) wherein k is a sampling sequence number.

3. The method of claim 1, wherein, The step (2) specifically comprises the following steps: (a) establishing a discrete form kinematics equation of the underwater robot as follows: η[k + 1] = Aη[k] + Bν[k] (2.1) wherein η[k] = [x[k], y[k], z[k], ψ[k]] T is the position and heading vector of the underwater vehicle at the kth sample; v[k] = [u[k], v[k], z[k], r[k]] T is the velocity vector of the underwater vehicle in the body frame at the kth sample, 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) establishing a prediction model as follows: where the predicted state vector and Δη[k] = η[k] - η[k - 1] Input vector Δν[k] = [Δu[k], Δv[k], Δw[k], Δr[k]] T = v[k] - v[k-1], output vector is h[k]; and (c) set the prediction horizon to N p , the control horizon to N c , and compute the predicted state at the next N p sampling instants according to the prediction model H[k] = Fs[k] + ΦU[k] (2.3) wherein and h[k + i|k], i = 1, 2,... N p is the i-th predicted output vector after k samples; and Δν[k + j], j = 0, 1,... N c -1 is the N c -1 input vectors after k samples; (d) establishing a quadratic programming problem as follows: wherein and wherein 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, is a unit matrix of size N c × N c ; U * [k] is the solution of this quadratic programming problem and has the form wherein Δν * [k+j-1], j = 1, 2,... N c is the jth optimal input obtained by the solution, wherein the 1st optimal input is used for the calculation of the subsequent tracking target.

4. The method of claim 1, wherein, The step (3) specifically comprises the following steps: (a) setting a maximum iteration number as Iter, a candidate solution number as N, a cost function as J, an upper limit of the solution as Ub and a lower limit of the solution as Lb, parameters S, F and CR; (b) initializing N candidate solutions o(i), i = 1, 2, … N, and calculating N corresponding cost function values J(o(i)); (c) initializing a current iteration number m = 1; (d) determining whether the current iteration count m is greater than the maximum iteration count Iter; if the result is yes, outputting the global optimum solution o * otherwise performing the following steps; (e) updating the optimal solution of the current iteration step and the global optimal solution o * ; the updating method is as follows: Iterate through all candidate solutions in the current iteration step, and select the candidate solution o(i) with the smallest cost function value J(o(i)). This candidate solution o(i) is the optimal solution for the current iteration step. For the global optimal solution, if the current iteration count m = 1, then the global optimal solution is o. * The optimal solution of the current iteration step If they are equal; if m is any other value, then the cost function value of the optimal solution in the current iteration step needs to be compared. The cost function value J(o) of the global optimal solution in the previous iteration step * ),like If smaller, then update. Otherwise o * Remain unchanged; (f) updating the candidate solutions, and the updating method is as follows: for the first half of the candidate solutions, an updating formula is as follows: wherein C1 is a random number of a normal distribution, C2 is a random vector of a normal distribution, and a dimension is same as that of the candidate solution; for the second half of the candidate solutions, an updating formula is as follows: wherein S is a random number greater than 0, g is a random vector of a normal distribution, and a dimension is same as that of the candidate solution; (g) randomly selecting a half of the candidate solutions, and performing a difference operation and an evolution operation: the difference operation is as follows: d(i) = o(r1) + F × (o(r2) - o(r3)) (3.3) wherein r1, r2 and r3 are random numbers different from each other, and greater than 0 and less than or equal to N; the evolution operation is as follows: wherein r is a random number of a uniform distribution between 0 and 1; G is a random positive integer, less than or equal to a dimension of the candidate solution; j represents a jth component of the solution; (h) comparing the cost function values J(o(i)) and corresponding J(e(i)) of the selected half of the candidate solutions, if J(e(i)) is smaller, updating o(i) = e(i), otherwise, keeping the candidate solution unchanged; (i) m = m + 1, and returning to execute step (d); The final global optimal solution o * is equivalent to U described in sub-step (d) in step 2 * [k].

5. The method of claim 1, wherein, The step (4) specifically comprises the following steps: (a) assuming that the ocean current is steady and irrotational, the ocean current velocity in the inertial coordinate system is where and are the ocean current velocity values in each direction; the estimate of the ocean current is where and are the estimated velocity values in each direction; The current velocity in the carrier coordinate system is v c = [u c , v c , w c , 0] T , and the estimated value is The conversion formula of the current velocity in the two coordinate systems is as follows: wherein, the superscript T is the transpose symbol, and ψ is the heading angle of the underwater vehicle; (b) the position and heading of the underwater vehicle in the inertial frame is η = [x, y, z, ψ] T The estimate of the position and heading is The velocity and angular velocity of the underwater vehicle in the body frame is v = [u, v, w, r], where u, v, w are the velocities in each direction and r is the angular velocity of the heading; (c) establishing a model of the sea current observer as follows: where, v r = [u r ,v r ,w r ,r] is the velocity and angular velocity of the underwater robot relative to the sea current in the body coordinate system and is calculated by the following equation: ν r = v - v c (4.3) and λ1and λ2are parameters greater than 0; (d) real-time solving according to the model of the current observer to obtain the current flow velocity estimation value in the inertial coordinate system The current flow velocity estimation value in the carrier coordinate system is calculated in combination with formula (4.1) 6. The method of claim 1, wherein, The step (5) specifically comprises: (a) first establishing a dynamic model of the relative current flow velocity of the underwater robot: where θ i1 and θ i2 are constant linear and nonlinear coefficients, respectively, i = 1, 2, 3, 4; is an input coefficient, and m is the mass of the underwater vehicle, I z is the moment of inertia, and are added mass coefficients; B is the buoyancy of the underwater vehicle in water, and G is the gravity of the underwater vehicle; (b) according to the calculation results of steps (3) and (4), setting the target relative velocity and the target course angular velocity as: v rd = [u rd ,v rd ,w rd ,r d ](4.5) r d = r + Δr (4.9) (c) estimating the linear coefficient and the nonlinear coefficient as follows: wherein and are the estimates of θ i1 and θ i2 respectively, i = 1,2,3,4.

7. A computer device, comprising: comprise: At least one processor, and a memory connected in communication with 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 to cause the at least one processor to perform the underwater robot model predictive adaptive trajectory tracking control method in 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 cause the computer to execute the underwater robot model predictive adaptive trajectory tracking control method in any one of claims 1 to 6.

Images