A motion path tracking control method for an autonomous underwater robot taking into account lateral movement

Abstract

The present invention relates to the field of underwater robot control, and aims to provide a method for tracking and controlling the lateral motion path of an autonomous underwater robot. The present invention uses a model predictive control method based on the idea of the line of sight method; adopts a control strategy that takes lateral motion errors into account to split the original high-dimensional model into two lower-dimensional models; and proposes the fusion of sensor data based on accelerometers and Doppler logs. The present invention can overcome complex underwater environments and obtain the optimal control quantity under the condition of control constraints, thereby achieving a fast and stable control effect for the autonomous underwater robot; in theory, it can reduce computational complexity, improve algorithm computational efficiency, and ensure rapid convergence of errors; it can improve the estimation accuracy of underwater position and further enhance control performance.

Description

Technical Field

[0001] The present invention relates to the field of underwater robot control, and in particular to a motion path tracking control method for an autonomous underwater robot taking into account lateral movement. Background Art

[0002] Underwater robots can be divided into two categories: remote-controlled underwater robots (with cables) and autonomous underwater robots (without cables). Compared to remote-controlled underwater robots, autonomous underwater robots have advantages such as autonomous control and a wider operating range, making them suitable for more underwater operation scenarios.

[0003] Path tracking control is a primary issue in autonomous underwater robot (AUV) operations. Only when the AUV can accurately track the preset operating path can it effectively complete the task. The Line of Sight (LOS) method is a common approach to solving the AUV's path tracking problem. This method converts the AUV's target path point into its target heading, ultimately achieving path tracking by controlling the heading. Although the LOS method is simple and widely used, it also has some drawbacks: when there are large turns in the path, the heading angle changes significantly, resulting in large deviations in path tracking. Furthermore, when coupled with the constraints imposed on the controller, the target path point may be lost.

[0004] To achieve better tracking results, intelligent control algorithms, including model predictive control (MPC), have gained increasing attention and application. MPC, also known as the rolling optimization algorithm, is a widely used intelligent control algorithm in industry. Its control principle is as follows: at the current sampling moment, an optimal control sequence with a finite time domain is solved based on the control system's optimization function. Only the first control variable of this optimal control sequence is applied to the controlled object. This process is repeated at each subsequent sampling moment until the task is completed.

[0005] The advantages of model predictive control (MPC) are: 1) Because the optimal control variable is used at every moment, the control error converges quickly. 2) Because the optimization function of the control system can be freely designed, it can effectively solve the control problems of complex systems with constraints. However, MPC also has its drawbacks: its solution efficiency is not high, and its computational complexity increases quadratically with the model dimension.

[0006] To this end, the present invention designs a model predictive control algorithm that takes lateral motion into account. It introduces the judgment of lateral motion based on the idea of the line of sight method, and splits the original high-dimensional model in the model predictive control algorithm into two low-dimensional models. In theory, it reduces the overall computational complexity and, on the other hand, ensures the convergence speed. Summary of the Invention

[0007] The technical problem to be solved by the present invention is to overcome the deficiencies in the prior art and provide a motion path tracking control method for an autonomous underwater robot taking into account lateral movement.

[0008] To solve the technical problem, the solution of the present invention is:

[0009] A method for tracking and controlling a motion path of an autonomous underwater robot including sideways movement is provided, comprising the following steps:

[0010] (1) Design two independent model predictive controllers based on the dynamic model of the autonomous underwater vehicle: the forward-steering motion controller C1 and the lateral motion controller C2.

[0011] (2) According to the pre-planned path of the autonomous underwater robot, several target waypoints and the final waypoint are set during the voyage, as well as the lateral error threshold Δ of the autonomous underwater robot during the movement along the voyage. l and the heading angle error threshold Δ Ψ ;

[0012] (3) Get the current position (x, y) and current target waypoint (x k ,y k ), calculate the distance L between the two and the critical radius R of waypoint switching, and determine whether the current target waypoint has been reached; if the current target waypoint has been reached, continue to execute step (4); if the current target waypoint has not been reached, switch to execute step (5);

[0013] (4) Set the current target waypoint (x k ,y k ) is compared with the final waypoint set in step (2). If it is the final waypoint, all tasks are terminated; if it is not the final waypoint, the position information of the current target waypoint is updated according to the target waypoint sequence set in step (2), and step (5) is continued;

[0014] (5) Get the current heading angle Ψ, current target waypoint (x k ,y k ) and the next target waypoint (x k+1 ,y k+1 ) specific data, and calculate the current lateral error dl and heading angle error dΨ; if the lateral error and heading angle error are both less than the set threshold, it is necessary to calculate the expected heading angle Ψ d and lateral displacement y d , and enable the forward-steering motion controller C1 and the lateral motion controller C2; otherwise, only the expected heading angle Ψ is calculated d , and enable the forward-steering motion controller C1;

[0015] (6) The forward-steering motion controller C1 and the lateral motion controller C2 calculate the control variables based on the judgment results of the previous step; then they issue corresponding control instructions to each thruster to control the autonomous underwater robot to approach the target waypoint;

[0016] (7) Using accelerometer to sample data a x , a y Then, it is combined with the sampling data v of the Doppler log x , v y Perform fusion filtering and estimate the forward displacement x and lateral displacement y based on this; then return to step (3);

[0017] (8) Repeat the above steps (3) to (7) to enable the autonomous underwater robot to achieve motion control and target path tracking during the movement.

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

[0019] (1) The present invention uses a model predictive control method based on the idea of the line of sight method, which can overcome the complex underwater environment and obtain the optimal control quantity under the condition of control constraints, thereby achieving a fast and stable control effect of the autonomous underwater robot.

[0020] (2) The present invention adopts a control strategy that takes lateral motion errors into account, splitting the original high-dimensional model into two lower-dimensional models. On the one hand, it can theoretically reduce the computational complexity and improve the algorithm's computational efficiency; on the other hand, it can also ensure rapid convergence of errors.

[0021] (3) The present invention proposes to fuse sensor data based on accelerometers and Doppler logs, which can improve the estimation accuracy of underwater position and further enhance control performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 is a flow chart of the control algorithm of the present invention;

[0023] Figure 2 is a schematic diagram of the underwater vehicle coordinate system;

[0024] Figure 3 This is a force analysis diagram of a certain rotary-wing autonomous underwater robot;

[0025] Figure 4 It is a flow chart of the Kalman fusion filtering method;

[0026] Figure 5 This is a comparison chart of the results of waypoint tracking using the method of the present invention and the common line-of-sight method when turning right at a large angle;

[0027] Figure 6This is a comparison chart of the results of waypoint tracking using the method of the present invention and the common line-of-sight method when turning left at a large angle;

[0028] Figure 7 This is a comparison chart of waypoint tracking errors between the method of the present invention and the common line-of-sight method. DETAILED DESCRIPTION

[0029] The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0030] To facilitate subsequent explanations, the coordinate system and conventional symbols of underwater vehicles are first introduced.

[0031] like Figure 2 As shown in Figure 1, the attitude angle and position of the underwater vehicle are defined in the geographic coordinate system. Where x represents the displacement on the x-axis, y represents the displacement on the y-axis, and z represents the displacement on the z-axis; where φ represents the angle of rotation around the x-axis, θ represents the angle of rotation around the y-axis, and ψ represents the angle of rotation around the z-axis.

[0032] Written in compact matrix form, the pose matrix η = [x, y, z, φ, θ, ψ] T .

[0033] The linear velocity and angular velocity of the underwater vehicle are defined in the body coordinate system. μ represents the displacement on the x-axis, v represents the displacement on the y-axis, and w represents the displacement on the z-axis. p represents the angular velocity around the x-axis, q represents the angular velocity around the y-axis, and r represents the angular velocity around the z-axis.

[0034] Written in compact matrix form, the velocity matrix v = [μ, v, w, p, q, r] T The mass of the underwater vehicle is m, and the inertia tensor is Diagonal element I x , I y , I z are the moments of inertia on the x, y, and z axes respectively, and the other non-diagonal elements are the corresponding products of inertia. (x G ,y G , z G ) is the coordinate of the center of mass of the underwater vehicle.

[0035] In addition, assume that the external forces and external moments acting on the underwater vehicle on the x, y, and z axes are (X, Y, Z) and (K, M, N), respectively.

[0036] Based on the derivation in Chapter 1 of Fossen's book "Marine Control System", the dynamic model of the underwater vehicle in matrix form can be obtained as follows:

[0037]

[0038] (1) Where M = M RB +M A is the mass matrix

[0039] M RB and M A Represent the rigid body inertia matrix and the additional mass matrix respectively. Among them, the rigid body inertia matrix,

[0040]

[0041] And for an underwater autonomous robot that is approximately three-axis symmetrical and has a low speed, its additional mass matrix is a diagonal matrix.

[0042]

[0043] The diagonal elements correspond to the additional mass or additional moments of inertia in the six degrees of freedom.

[0044] (2) Where C(v) = c RB +C A is the Coriolis matrix

[0045]

[0046]

[0047]

[0048]

[0049] (3) Where D(v) = D L +D NL is the fluid resistance matrix

[0050] D L =diag(X u , Y v , Z w , K p , M q , N r )

[0051] D NL =diag(X u|u| |u|,Y v|v| |v|,Z w|w| |w|,K p|p| |p|,M q|q| |q|,N r|r| |r|)

[0052] D L It is a diagonal matrix, called the linear resistance matrix, and the diagonal elements are the linear resistance coefficients on the six degrees of freedom; DNL It is also a diagonal matrix, called the nonlinear resistance matrix, and the diagonal elements are the products of the nonlinear resistance coefficients on the six degrees of freedom and the absolute value of the velocity (or angular velocity).

[0053] (4) where g(η) is the restoring force matrix

[0054]

[0055] W is the weight of the underwater vehicle, and B is the buoyancy of the underwater vehicle.

[0056] (5)where τ=[X, Y, Z, K, M, N] T is the external force matrix of the underwater vehicle.

[0057] The method for tracking and controlling a motion path of an autonomous underwater robot including lateral movement according to the present invention comprises the following steps:

[0058] In the first step, according to the dynamic model of the autonomous underwater vehicle, appropriate state vectors and input vectors are selected and two model predictive controllers are designed, namely the forward-steering motion controller C1 and the lateral motion controller C2.

[0059] The basic design methods for model predictive controllers can be found in Chapters 5 and 6 of Chen Hong's book, Model Predictive Control. The dynamic model of the autonomous underwater vehicle is described above. The forward-steering motion controller and the lateral motion controller are based on the same dynamic model. During the design process, it is only necessary to select different state vectors and corresponding input vectors in the dynamic model according to the specific control objectives to form the model used in the specific model predictive controller. Specifically, it includes:

[0060] (a) The model required for the model predictive controller is obtained from the dynamic model, state vector, and input vector:

[0061]

[0062] Among them, the state vector is x(t)∈R n , the input vector is u(t)∈R n , x0 is the initial state, and the input and state constraints are:

[0063]

[0064]

[0065] U is a finite set of input vector constraints, and X is a finite set of state vector constraints;

[0066] (b) The optimization problem is determined as follows:

[0067]

[0068] Satisfy the constraints:

[0069]

[0070]

[0071]

[0072]

[0073] in,

[0074]

[0075] T p For the prediction time domain, Q∈R n×n and R∈R m×m is a positive symmetric weighted matrix; J is the objective cost function of the control problem, U and x are the input constraint and state constraint sets respectively; are the system state variables and input variables in the prediction process of the model predictive controller, and Ω is the system state set including the equilibrium point.

[0076] (c) After selecting the state vector and input vector, the optimal control input is obtained through the model predictive controller:

[0077] Calculation of positive definite symmetric matrix P

[0078] Linearizing the model at the equilibrium point yields:

[0079]

[0080] in,

[0081] If the linearized system is controllable, the linear feedback u=Kx can be obtained so that the closed-loop system

[0082] A K =A+BK is asymptotically stable, then there must be a unique symmetric positive definite matrix P that satisfies:

[0083]

[0084] (d) At this point, by solving the above optimization problem at each sampling moment, the optimal control sequence U can be obtained. * , then the optimal control input at the sampling moment is U * The first element of (t), that is, u * =U * (t).

[0085] Example:

[0086] (1) In the velocity matrix and pose matrix of the state of the autonomous underwater robot dynamics model, the quantities related to the forward-steering motion, such as the forward velocity u and the z-axis angular velocity r, are selected to form the state vector x1. The corresponding components in the external force matrix are selected to form the input vector u1. Assuming that the target cost function of the forward-steering motion control problem is J1, the model required for the forward-steering motion controller C1 can be obtained according to the dynamics model:

[0087]

[0088] Where x1 is the initial state, Represents the derivative of x1(t), and f1 represents a second-order continuously differentiable function related to x1 and u1.

[0089] By solving a constrained optimization problem:

[0090]

[0091] constraint:

[0092]

[0093]

[0094]

[0095]

[0096] The optimal input vector can be obtained

[0097] In the above formula:

[0098]

[0099] T p For the prediction time domain, U represents the input constraint set, X represents the state constraint set, represents the variables of the prediction system in the controller, is a positive definite symmetric weighting matrix, Is a positive definite symmetric matrix, n1 is the length of the state vector x1, and m1 is the length of the input vector u1. are the system state variables and input variables in the prediction process of the model predictive controller, and Ω1 is the system state set including the equilibrium point.

[0100] (2) In the state of the autonomous underwater robot dynamic model, the quantities related to lateral motion, such as lateral velocity v and lateral displacement y, are selected from the velocity matrix and the pose matrix to form the state vector x2. The corresponding components in the external force matrix are selected to form the input vector u2. Assuming that the target cost function of the lateral motion control problem is J2, the model required for the lateral motion controller C2 can be obtained according to the dynamic model:

[0101]

[0102] Where x2 is the initial state, Represents the derivative of x2(t), and f2 represents a second-order continuously differentiable function related to x2 and u2.

[0103] By solving a constrained optimization problem:

[0104]

[0105] constraint:

[0106]

[0107]

[0108]

[0109]

[0110] The optimal input vector can be obtained

[0111] In the above formula:

[0112]

[0113] T p For the prediction time domain, U represents the input constraint set, X represents the state constraint set, represents the variables of the prediction system in the controller, is a positive definite symmetric weighting matrix, It is a positive definite symmetric matrix, n2 is the length of the state vector x2, and m2 is the length of the input vector u2. are the system state variables and input variables in the prediction process of the model predictive controller, and Ω2 is the system state set including the equilibrium point.

[0114] In the second step, according to the pre-planned path of the autonomous underwater robot, several target waypoints and the final waypoint along the voyage are set, as well as the lateral error threshold Δ of the autonomous underwater robot during the movement along the voyage. l and the heading angle error threshold Δ Ψ . Specifically including:

[0115] (a) Determine the navigation route of the autonomous underwater vehicle based on the actual mission, and set the discrete waypoints obtained by pre-sampling as the target waypoints in the tracking process. The set of all target waypoints is:

[0116] {(x1,y1),(x2.y2),(x3,y3),...,(x n ,y n )}

[0117] In the formula, (x n ,y n ) is the final waypoint;

[0118] (b) Set the lateral error threshold Δ according to the size and control requirements of the autonomous underwater vehicle. l and heading angle error threshold Δ Ψ .

[0119] The third step is to obtain the current position (x, y) and current target waypoint (x k ,y k ), calculate the distance L and the critical radius R of the waypoint switching between the two, and determine whether the current target waypoint has been reached. If the current target waypoint has been reached, execute the fourth step; if not, execute the fifth step. Specifically including:

[0120] (a) Calculate the current position (x, y) and the current target waypoint (x k ,y k ) distance L:

[0121]

[0122] (b) The critical radius is calculated as follows:

[0123]

[0124] where R min , R max are the minimum and maximum values of the critical radius, u is the current speed, θ p is the angle between two continuous paths, l is the radial length of the underwater robot, k u , k θ are the speed influence factor and the angle influence factor respectively.

[0125] (c) When the distance L is less than or equal to the critical radius R, it is considered that the current target waypoint has been reached and the following fourth step is executed.

[0126] (d) When the distance L is greater than the critical radius R, the current target waypoint has not been reached, and the process goes to step 5 below.

[0127] Step 4: Set the current target waypoint (x k ,y k ) is compared with the final waypoint set in step 2. If it is the final waypoint, all tasks are terminated; if it is not the final waypoint, the location information of the current target waypoint is updated according to the target waypoint sequence set in step 2, and the fifth step is continued. Specifically including:

[0128] (a) Get the current target waypoint (x k ,y k ) and the final waypoint (x n ,y n )

[0129] (b) If x k =x n And y k =y n , indicating that the final waypoint has been reached and all tasks have ended.

[0130] (c) If x k ≠x n or y k ≠y n , indicating that the final waypoint has not been reached, the current target waypoint is updated to the next target waypoint in the target waypoint sequence.

[0131] Step 5: Get the current heading angle Ψ and target waypoint (x k ,y k ) and the next target waypoint (x k+1 ,y k+1 ) and calculate the current lateral error dl and heading angle error dΨ; if both the lateral error and the heading angle error are less than the set threshold, it is necessary to calculate the expected heading angle Ψ d and lateral displacement y d , and enable the forward-steering motion controller C1 and the lateral motion controller C2; otherwise, only the expected heading angle Ψ is calculated d , and enable the forward-steering motion controller C1; specifically including:

[0132] (a) Calculate the current lateral error dl:

[0133] First calculate the current path point (x k ,y k ) and the next path point (x k+1 ,y k+1 ) is the equation of the line:

[0134] Ax+By=C

[0135] in:

[0136] A=y k+1 -y k

[0137] B=x k -x k+1

[0138] C=x k (y k -y k+1 )-y k (x k -x k+1 )

[0139] According to the current position (x, y), the lateral error is:

[0140]

[0141] (b) Calculate the heading angle error dΨ:

[0142] First, according to the current path point (x k ,y k ) and the next path point (x k+1 ,y k+1 )Calculate the direction of the target path:

[0143]

[0144] According to the current heading angle Ψ, the heading angle error can be calculated as:

[0145] dΨ=|Ψ-θ road |

[0146] (c) Determine whether to enable the lateral motion controller C2

[0147] When the lateral error dl is less than the lateral error threshold Δ l , and the heading angle error dΨ is less than the heading angle error threshold Δ Ψ When the forward-steering motion controller C1 and the lateral motion controller C2 need to be enabled; otherwise, only the forward-steering motion controller C1 needs to be enabled, and the lateral motion controller C2 does not need to be enabled;

[0148] (d) When the lateral motion controller C2 needs to be activated, the expected heading angle and expected lateral motion displacement are calculated as follows:

[0149] The expected heading angle is calculated as:

[0150]

[0151] The expected lateral motion displacement is:

[0152]

[0153] (e) When the lateral motion controller C2 does not need to be activated, the expected heading angle is calculated as follows:

[0154] First, set a look-ahead distance d and find an intermediate waypoint (x temp ,y temp ),satisfy:

[0155] Ax temp +By temp =C

[0156] (x temp -x) 2 +(y temp -y) 2 =dl 2 +d 2

[0157] The expected heading angle is then calculated as follows:

[0158]

[0159] And there is no need to design for expected lateral displacement.

[0160] In the sixth step, based on the judgment results of the previous step, the forward-steering motion controller C1 and the lateral motion controller C2 are activated to calculate the control quantity, and then control instructions are issued to the forward thruster or lateral thruster respectively to control the autonomous underwater robot to approach the target waypoint. Specifically, it includes:

[0161] (a) If in step 5, it is determined that the lateral motion controller C2 needs to be activated, first enter the expected heading angle Ψ d , the control quantity is calculated using the forward-steering controller C1 Send a control command to the forward thruster to control the current heading angle to approach the expected heading angle; at the same time, input the expected lateral displacement and use the lateral motion controller C2 to calculate the control amount A control command is issued to the lateral thruster to control the current lateral displacement to approach the expected lateral displacement.

[0162] (b) If in step 5, it is determined that the lateral motion controller C2 does not need to be activated, only the desired heading angle Ψ is input. d , the control quantity is calculated using the forward-steering controller C1 Send control commands to the forward thruster to control the current heading angle to approach the expected heading angle;

[0163] The seventh step is to obtain the sampling data a of the accelerometer x , a y and the sampling data v of the Doppler log x , vy ;according to Figure 4 The process shown performs Kalman fusion filtering and estimates the current position (x, y), specifically including:

[0164] (a) After executing the control instruction of the controller in step 6, the sampling data of the accelerometer is obtained. x , a y and the sampling data v of the Doppler log x , v y ;

[0165] (b) Given the measurement error E of the Doppler log m ;

[0166] (c) Determine whether it is the first fusion filter and calculate the filter gain k respectively x , k y ;

[0167] If it is the first fusion filter, then k x =k y =0.5;

[0168] If it is not the first fusion filter, it is calculated using the following two formulas:

[0169] k x =E x / (E x +E m )

[0170] k y =E y / (E y +E m )

[0171] Among them, E x , E y is the estimated error obtained from the last filtering.

[0172] (d) Calculate the filtered velocity value and The calculation formula is as follows:

[0173]

[0174]

[0175] (e) Calculate the estimated error E x and E y , the calculation formula is as follows:

[0176]

[0177]

[0178] (f) Output the final position (x, y), calculated as follows:

[0179]

[0180]

[0181] (g) Return to the content of the operation in step 3.

[0182] Step 8. Repeat the operations from steps 3 to 7 above to enable the autonomous underwater robot to achieve efficient and smooth motion control during movement and accurately track the target path.

[0183] A more specific application example:

[0184] A specific implementation of this invention will now be described using a rotary-wing autonomous underwater robot. This rotary-wing autonomous underwater robot has four vertical thrusters and two horizontal thrusters, enabling six degrees of freedom control. A force analysis diagram is shown in Figure 3, where F1, F2, ..., F6 are the thrusts of the six thrusters, respectively. The specific motion path tracking control method for this autonomous underwater robot, including lateral movement, is as follows:

[0185] 1. Design forward-steering controller and lateral motion controller.

[0186] Combined with the above-mentioned hydrodynamic motion equation of the vehicle, the state vector x1 = [x, y, ψ, u, v, r] is selected T , input vector u1=[F5,F6] T , we can derive the model of the forward-steering model predictive controller of the rotor-type autonomous underwater robot, that is, Expand as follows:

[0187]

[0188] Where l is the axial length of the rotor-type autonomous underwater robot.

[0189] Therefore, the forward-steering controller C1 can be obtained by combining the above model predictive control design method.

[0190] Similarly, select the state variable x2 = [y, φ, v, p] T , input variable u2 = [F1, F2, F3, F4] T The model of the lateral motion model predictive controller of the rotor-type autonomous underwater robot can be derived, that is, Expand as follows:

[0191]

[0192] Therefore, the lateral motion controller C2 can be obtained by combining the above model predictive control design method.

[0193] It should be pointed out that the input vectors of the two model predictive controllers C1 and C2 are composed of the thrust of the propellers, so the final controller obtains the optimal thrust of each propeller. The propeller thrust has a fixed functional relationship with the propeller speed, so the propeller speed can be inferred from the optimal thrust, and then the required propeller instructions can be obtained.

[0194] 2. Determine the target waypoint. In this example, the waypoints are as follows:

[0195] {(3, 0), (3, 15), (13, 15), (13, 5), (23, 5), (23, 15), (33, 15)}

[0196] Determine a lateral error threshold Δl = 0.5m and a heading error threshold

[0197] 3. Follow the method described above and use a computer simulation program to complete the calculations from step 3 to step 8.

[0198] In order to demonstrate the effectiveness of this method, the applicant also conducted comparative simulation experiments using the method of the present invention and the ordinary line of sight method under the same conditions. Figure 5 The comparison of the results of the proposed method and the common line-of-sight method when turning right at a large angle is shown. Figure 6 The comparison chart of the results of waypoint tracking using the method of the present invention and the common line-of-sight method when turning left at a large angle shows that the method of the present invention can track the path faster when turning at a large angle.

[0199] Figure 7 The figure further shows a comparison of the waypoint tracking errors of the proposed method and the conventional line-of-sight method. The comparison of the two curves clearly shows that the proposed method's tracking error is smaller than that of the conventional line-of-sight method over the entire path. Therefore, the proposed method is significantly superior in reducing tracking errors.

[0200] Finally, it should be noted that the above examples only illustrate the method of applying the present invention to a specific rotor-type autonomous underwater robot. In fact, the present invention is not only applicable to the rotor-type autonomous underwater robot shown in the above examples, but also to any autonomous underwater robot with six degrees of freedom that is fully controllable. A practical control system can be designed and implemented for it in accordance with the contents of the present invention.

Claims

1. A motion path tracking control method for an autonomous underwater robot taking into account lateral movement, characterized in that: The following steps are involved: (1) Design two independent model predictive controllers based on the dynamic model of the autonomous underwater vehicle: the forward-steering motion controller C1 and the lateral motion controller C2. (2) According to the pre-planned path of the autonomous underwater robot, several target waypoints and the final waypoint are set during the voyage, as well as the lateral error threshold Δ of the autonomous underwater robot during the movement along the voyage. l and the heading angle error threshold Δ Ψ ; (3) Get the current position (x, y) and current target waypoint (x k ,y k ), calculate the distance L between the two and the critical radius R of waypoint switching, and determine whether the current target waypoint has been reached; If the current target waypoint is reached, continue to step (4); If the current target waypoint has not been reached, the process goes to step (5); (4) Set the current target waypoint (x k ,y k ) is compared with the final waypoint set in step (2). If it is the final waypoint, all tasks are terminated; if it is not the final waypoint, the position information of the current target waypoint is updated according to the target waypoint sequence set in step (2), and step (5) is continued; (5) Get the current heading angle Ψ, current target waypoint (x k ,y k ) and the next target waypoint (x k+1 ,y k+1 ) and calculate the current lateral error dl and heading angle error dΨ; if both the lateral error and the heading angle error are less than the set threshold, it is necessary to calculate the expected heading angle Ψ d and lateral displacement y d , and enable the forward-steering motion controller C1 and the lateral motion controller C2; otherwise, only the expected heading angle Ψ is calculated d , and enable the forward-steering motion controller C1; (6) The forward-steering motion controller C1 and the lateral motion controller C2 calculate the control variables based on the judgment results of the previous step; then, they issue corresponding control instructions to each thruster to control the autonomous underwater robot to approach the target waypoint. This step specifically includes: (a) If it is determined in step (5) that the lateral motion controller C2 needs to be activated, first input the expected heading angle Ψ d , The control quantity is calculated using the forward-steering controller C1 Send a control command to the forward thruster to control the current heading angle to approach the expected heading angle; at the same time, input the expected lateral displacement and use the lateral motion controller C2 to calculate the control amount Sending control instructions to the lateral thrusters to control the current lateral displacement to approach the expected lateral displacement; (b) If it is determined in step (5) that the lateral motion controller C2 does not need to be activated, only the expected heading angle Ψ is input. d , The control quantity is calculated using the forward-steering controller C1 Send control commands to the forward thruster to control the current heading angle to approach the expected heading angle; (7) Using accelerometer to sample data a x ,a y Then, it is combined with the sampling data v of the Doppler log x ,v y Perform fusion filtering and estimate the forward displacement x and lateral displacement y based on this; then return to step (3); (8) Repeat the above steps (3) to (7) to enable the autonomous underwater robot to achieve motion control and target path tracking during the movement.

2. The method according to claim 1, characterized in that In step (1), the design of the model predictive controller and the realization of the control output include the following steps: (a) The model required for the model predictive controller is obtained from the dynamic model, state vector, and input vector: Among them, the state vector is x(t)∈R n , the input vector is u(t)∈R n , x0 is the initial state, the input and state constraints are U is a finite set of input vector constraints, and X is a finite set of state vector constraints; (b) The optimization problem is determined as follows: Satisfy the constraints: in, T p For the prediction time domain, Q∈R n×n and R∈R m×m is a positive symmetric weighted matrix; J is the objective cost function of the control problem, U and X are the input constraint and state constraint sets respectively; are the system state variables and input variables in the prediction process of the model predictive controller, Ω is the system state set including the equilibrium point; (c) Calculation of positive definite symmetric matrix P: Linearizing the model at the equilibrium point yields: in, If the linearized system is controllable, the linear feedback u = Kx is obtained so that the closed-loop system A K =A+BK is asymptotically stable; Then there must be a unique symmetric positive definite matrix P that satisfies: (d) Solve the above optimization problem at each sampling moment to obtain the optimal control sequence U * ; Then the optimal control input at this sampling moment is U * The first element of (t), that is, u * =U * (t).

3. The method according to claim 1, characterized in that The step (2) specifically includes: (a) Determine the navigation route of the autonomous underwater vehicle based on the actual mission, and set the discrete waypoints in the route obtained by pre-sampling as the target waypoints in the tracking process. The set of all target waypoints is: {(x1,y1),(x2.y2),(x3,y3),…,(x n ,y n )} In the formula, (x n ,y n ) is the final waypoint; (b) Set the lateral error threshold Δ according to the size and control requirements of the autonomous underwater vehicle. l and heading angle error threshold Δ Ψ .

4. The method according to claim 1, wherein The step (3) specifically includes: (a) Calculate the current position (x, y) and the current target waypoint (x k ,y k ) distance L: (b) The critical radius is calculated as follows: where R min , R max are the minimum and maximum values of the critical radius, u is the current speed, θ p is the angle between two continuous paths, l is the radial length of the underwater robot, k u , k θ are speed influence factor and angle influence factor respectively; (c) If the distance L is less than or equal to the critical radius R, it is considered that the current target waypoint has been reached and step (4) is executed; If the distance L is greater than the critical radius R, it is considered that the current target waypoint has not been reached and the process goes to step (5).

5. The method according to claim 1, wherein The step (4) specifically includes: (a) Get the current target waypoint (x k ,y k ) and the final waypoint (x n ,y n ) (b) If x k =x n And y k =y n , indicating that the final waypoint has been reached and all tasks have ended; (c) If x k ≠x n or y k ≠y n , indicating that the final waypoint has not been reached, update the current target waypoint to the target waypoint The next destination waypoint in the sequence.

6. The method according to claim 1, characterized in that The step (5) specifically includes: (a) Calculate the current lateral error dl: First calculate the current path point (x k ,y k ) and the next path point (x k+1 ,y k+1 ) is the equation of the line: Ax+By=C in: A=y k+1 -and k B=x k -x k+1 C=x k (and k -and k+1 )-and k (x k -x k+1 ) According to the current position (x, y), the lateral error is: (b) Calculate the heading angle error dΨ: First, according to the current path point (x k ,y k ) and the next path point (x k+1 ,y k+1 )Calculate the direction of the target path: According to the current heading angle Ψ, the heading angle error is calculated as: dΨ=|Ψ-θ road | (c) Determine whether to enable the lateral motion controller C2: When the lateral error dl is less than the lateral error threshold Δ l , and the heading angle error dΨ is less than the heading angle error threshold Δ Ψ When the forward-steering motion controller C1 and the lateral motion controller C2 are enabled, When using the forward-steering motion controller C1, there is no need to enable the lateral motion controller C2; (d) When the lateral motion controller C2 is activated, the expected heading angle and expected lateral motion displacement are calculated as follows: The expected heading angle is calculated as follows: The expected lateral motion displacement is: (e) When the lateral motion controller C2 does not need to be activated, the expected heading angle is calculated as follows: First, set a look-ahead distance d and find an intermediate waypoint (x temp ,y temp ),satisfy: Ax temp +By temp =C (x temp -x) 2 +(y temp -y) 2 =dl 2 +d 2 The expected heading angle is then calculated as follows: And there is no need to design for expected lateral displacement.

7. The method according to claim 1, characterized in that The step (7) specifically includes: (a) Get the sampling data of accelerometer a x ,a y and the sampling data v of the Doppler log x ,v y ; (b) Given the measurement error E of the Doppler log m ; (c) Determine whether it is the first fusion filter and calculate the filter gain k respectively x ,k y ; If it is the first fusion filter, then k x =k y =0.5; If it is not the first fusion filter, it is calculated using the following two formulas: k x =And x / (AND x +E m ) k y =And y / (AND y +E m ) Among them, E x , E y is the estimated error obtained from the last filtering; (d) Calculate the filtered velocity value and The calculation formula is as follows: (e) Calculate the estimated error E x and E y , the calculation formula is as follows: (f) Output the final position (x, y), calculated as follows: (g) Return to step (3).

Images