A control method for underwater robot path tracking and obstacle avoidance based on NMPC

Abstract

The present invention relates to a control method for underwater robot path tracking and obstacle avoidance based on NMPC, including: establishing a kinematic dynamics model of the underwater robot; calculating the current speed of the underwater robot and estimating the external disturbance; predicting the motion state of the underwater robot in the next N control cycles, and predicting the trajectory of underwater obstacles in a short time; solving the optimization problem and obtaining the optimal control sequence; at the end of the sonar scanning cycle, calculating the new prior state quantity and the posterior state quantity of the Kalman filter, and also updating and calculating the uncertainty of the underwater obstacles, otherwise no update is performed and it remains unchanged; until the underwater robot performs path tracking and can complete obstacle avoidance. The present invention adopts an improved extended state observer for disturbance observation, which greatly improves the underwater robot's anti-interference ability and the robustness of motion; reduces the collision probability of the underwater robot, shortens the driving distance and expands the safe distance from obstacles.

Description

Technical Field

[0001] The invention relates to the technical field of underwater bionic robot path tracking, in particular to a NMPC-based underwater robot path tracking and obstacle avoidance control method. Background Art

[0002] The ocean is the largest resource treasure on earth. As land resources are constantly being exploited and depleted, humans are constantly conducting research and development on the ocean. Underwater robots have become one of the important tools for developing marine resources. They can replace humans in completing a series of difficult and high-risk tasks such as resource detection and collection in the ocean. Among them, the core of the relevant technology of underwater robots is motion control, which can complete high-precision path tracking and autonomous dynamic obstacle avoidance.

[0003] For decades, the path tracking control of underwater robots or unmanned surface vessels (USVs) has been a common concern of scholars around the world, and many excellent control strategies have been proposed: such as adaptive control, backstepping control, sliding mode method, neural network control, model predictive control, etc. These traditional control methods have a certain effect on the path tracking problem of underwater robots or surface ships under model parameter uncertainty and unknown disturbances, but few control methods can take into account the obstacle avoidance problem and the effective prediction of the future position of obstacles. However, in the disturbance of the natural environment of the ocean, once the predetermined trajectory is deviated, the risk of collision will greatly increase. How can underwater robots safely avoid collisions and keep tracking the predetermined path? This is a severe challenge for underactuated underwater robots that rely only on impact force and yaw torque. In addition, the control input of bionic underwater robots that rely on wave fin propulsion is different from that of traditional propeller-propelled underwater robots and surface ships, which makes the estimation of model uncertainty and unknown external disturbances more complicated. Summary of the invention

[0004] The purpose of the present invention is to provide a control method for underwater robot path tracking and obstacle avoidance based on NMPC, which greatly improves the anti-interference ability and robustness of movement of underwater robots, predicts the future position of obstacles in a short period, reduces the collision probability of underwater robots, shortens the driving distance and increases the safe distance from obstacles.

[0005] To achieve the above object, the present invention adopts the following technical solution: a control method for underwater robot path tracking and obstacle avoidance based on NMPC, the method comprising the following steps in sequence:

[0006] (1) Establish the kinematics and dynamics model of the underwater robot, establish the objective function of the optimization problem, set the global path reference point, and use Bezier curve interpolation to obtain the global tracking path , the bow angle is calculated;

[0007] (2) Measure the current position of the underwater robot, read the bow roll angle, calculate the current speed of the underwater robot through the extended state observer, estimate the external disturbance, and use sonar to scan the state of underwater obstacles, and input the detection results into the Kalman filter;

[0008] (3) Using the established kinematic dynamics model, the motion state of the underwater robot in the next N control cycles is predicted according to the current speed of the underwater robot and the external disturbance, and the trajectory of the underwater obstacle in a short time is also predicted;

[0009] (4) Based on the predicted motion state of the underwater robot in the next N control cycles and the short-term trajectory of the underwater obstacle, the solver is used to solve the optimization problem and obtain the optimal control sequence, and the first position of the optimal control sequence is converted as the control input signal of the underwater robot's wave fin thruster;

[0010] (5) If the sonar scanning cycle ends and the motion state of the underwater obstacle measured by the sonar is updated, the input of the Kalman filter is updated, and the new prior state and posterior state of the Kalman filter are calculated. The uncertainty of the underwater obstacle is also updated and calculated. If the sonar scanning cycle has not ended, the Kalman filter and the uncertainty of the underwater obstacle are not updated and remain unchanged.

[0011] (6) Repeat steps (2) to (5) until the underwater robot can perform path tracking and complete obstacle avoidance.

[0012] The step (1) specifically refers to:

[0013] Establish the kinematic dynamics model of the underwater robot under three degrees of freedom:

[0014]

[0015] in, is the state vector, V = [urv] T represents the speed of the underwater robot, x and y represent the position of the underwater robot in the earth coordinate system, represents the underwater robot's bow pitch angle, u is the underwater robot's forward speed, v is the underwater robot's lateral speed, and r is the underwater robot's bow pitch speed; is the control input, f u is the control input pitch force, f v is the lateral driving force, the underwater robot is an underactuated system, f v =0; f r is the yaw moment; τ d=[d u d v d r ] T is the external disturbance of the underwater robot, d u is the disturbance force in the underwater robot's forward direction, d v is the disturbance force in the lateral direction of the underwater robot, d r is the external environmental disturbance torque in the bow rolling direction, M is the inertial mass matrix of the underwater robot, D is the damping force diagonal matrix of the underwater robot, C(v) is the centripetal force and Coriolis matrix, and its formula is as follows:

[0016]

[0017] Among them, m 11 、m 22 、m 33 are the additional mass parameters of the underwater robot, d 11 d 22 d 33 are the hydrodynamic coefficients;

[0018] It is the rotation matrix that converts the underwater robot's lateral velocity v in the body coordinate system into the velocity η in the earth coordinate system, specifically:

[0019]

[0020] Establish the objective function of the optimization problem:

[0021]

[0022] Among them, J x (x(t),x ref (t)) is the expected state cost, and its formula is:

[0023]

[0024] J u (αu(t)) is the control input cost, and its formula is:

[0025]

[0026] J c (x(t) is the collision cost, and its formula is:

[0027]

[0028] Where T is the sampling period of the underwater robot, f(x,u) is the continuous nonlinear equation describing the underwater robot, and d i (t) is expressed as the Euclidean distance to the i-th underwater obstacle, ki >0 is the parameter of smooth obstacle avoidance trajectory; is the weight coefficient matrix of the expected state cost term, is the weight coefficient matrix for controlling the input cost, Q c represents the collision cost coefficient; U is the input range of the control force and torque, as shown in the following formula:

[0029]

[0030] The underwater robot is an underactuated system, so f vmin =f vmax =0; f vmin is the minimum lateral driving force of the underwater robot, f vmax is the maximum lateral driving force of the underwater robot, f umin is the minimum longitudinal driving force of the underwater robot, f umax is the maximum longitudinal driving force of the underwater robot, f rmin is the minimum value of the underwater robot’s bow pitch torque, f rmax is the maximum bow rolling moment of the underwater robot.

[0031] The step (2) specifically refers to:

[0032] Measure and calculate the underwater robot state x(t0), and use the extended state observer to calculate the underwater robot speed and external disturbance state:

[0033]

[0034]

[0035]

[0036] Wherein, l0 to l8 are all positive integers, b0 and b1 are compensation factors, ζ = u·t and χ = v·t represent the longitudinal and lateral displacements in the body coordinate system respectively; represents the underwater robot's bow pitch angle, u is the underwater robot's forward speed, v is the underwater robot's lateral speed, and r is the underwater robot's bow pitch speed; f u is the control input pitch force, f r is the yaw moment; m 11 、m 22 、m 33 are the additional mass parameters of the underwater robot, d 11 , d 22 d 33 are the hydrodynamic coefficients; by solving equations (6), (7), and (8) respectively, we can obtain the current longitudinal velocity of the underwater robot. Lateral speed Yaw speed And vertical Horizontal Bow Estimated values ​​of external disturbances in three directions, “∧” represents the calculated estimated value and “·” represents the differential sign.

[0037] The step (3) specifically refers to: using the kinematics and dynamics model of the underwater robot to calculate and predict the future state of the underwater robot And the underwater obstacle state is predicted through the underwater obstacle linear motion model:

[0038]

[0039] in, is the predicted position of underwater obstacle i, T s is the sampling period, p obs i (t0) is the measured position of the underwater obstacle at the current moment, v o- is the a priori speed measurement of underwater obstacles, N is the number of prediction steps, t0 is the current moment when the prediction starts, and n T are predicted in the future s Sampling period;

[0040] (3) The kinematic dynamics model is used to predict the motion state of the underwater robot in the next N control cycles. Formula (9) is used to calculate the current position of the underwater obstacle measured by the sonar, plus the underwater obstacle measurement prior speed multiplied by the sampling period T. s , get the next n T s The linear motion position of the underwater obstacle within the cycle.

[0041] The step (4) specifically refers to: using a solver to solve the optimization problem and obtain the optimal control sequence u * (t+nT s ), n = 1, ..., N-1, and put the optimal control sequence first The conversion is used as the control input signal of the underwater robot wave fin thruster, u * Represents the optimal control sequence solved by the solver.

[0042] The step (5) specifically refers to: if the sonar scanning cycle ends, that is, the measured state quantity of the underwater obstacle is updated, then the underwater obstacle prior state quantity of the Kalman filter is updated. and the posterior state quantity P t Update the calculation and the uncertainty of underwater obstacles r th，k (t) Perform calculation updates, otherwise the Kalman filter and underwater obstacle uncertainty are not updated;

[0043] The Kalman filter is divided into two parts:

[0044] predict:

[0045] renew:

[0046] Among them, x t =[x obs ,y obs , v obs ] T is the state of the underwater obstacle, x obs ,y obs is the coordinate position of the underwater obstacle, v obs is the speed of the underwater obstacle, Z t =[x o ,y o , v o ] T is the underwater obstacle state measured at time t, x o ,y o is the measured coordinate position of the underwater obstacle, v o is the measured speed of the underwater obstacle, F is the state transfer matrix, H is the observation matrix, Q t is the covariance matrix of underwater obstacle motion noise, R t is the observation noise covariance matrix, I is the identity matrix;

[0047] By solving the calculation Maximum eigenvalue δ i (t), the uncertainty of the ith underwater obstacle at time t is described by formula (12):

[0048] r th，i (t) = r th +10δ i (t) (12)

[0049] Among them, r th is a constant parameter.

[0050] It can be seen from the above technical solutions that the beneficial effects of the present invention are as follows: First, for the uncertainty of model parameters and external environmental interference caused by the unique propulsion method of the bionic underwater robot, the present invention adopts an improved expanded state observer to perform disturbance observation, and uses it as a nonlinear model predictive control disturbance term for continuous calculation and update, which greatly improves the underwater robot's anti-interference ability and the robustness of its motion; Second, for the uncertainty of dynamic obstacle state observation, that is, observation error, and too long sonar scanning cycle, the present invention predicts the future position of obstacles within a short period by combining Kalman filter prior estimation, thereby reducing the collision probability of the underwater robot, shortening the driving distance and increasing the safe distance from the obstacle. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 is a schematic diagram of path tracking;

[0052] Figure 2 It is a schematic diagram of the method framework of the present invention;

[0053] Figure 3 , 4 , 5, and 6 are screenshots of important moments in the simulation 1 process;

[0054] Figure 7 It is a schematic diagram of curve tracking comparison results;

[0055] Figure 8 , 9 , 10, 11, 12, and 13 are all screenshots of multiple dynamic underwater obstacle simulations. DETAILED DESCRIPTION

[0056] like Figure 2 As shown, a control method for underwater robot path tracking and obstacle avoidance based on NMPC, the method comprises the following steps in sequence:

[0057] (1) Establish the kinematics and dynamics model of the underwater robot, establish the objective function of the optimization problem, set the global path reference point, and use Bezier curve interpolation to obtain the global tracking path Calculate the pitch angle; initialize system parameters, including a series of global tracking paths The global tracking path reference point is like Figure 1 As shown, where e x Indicates the current position coordinate x and the expected position coordinate x of the underwater machine d The error between y Indicates the current position coordinate y and the expected position coordinate y of the underwater machine d The error between Indicates the current bow angle of the underwater robot Reference angle to desired pitch angle The error between B and B is the coordinate axis of the underwater robot body coordinate system, x I and I are the coordinate axes of the geodetic coordinate system.

[0058] (2) Measure the current position of the underwater robot, read the bow roll angle, calculate the current speed of the underwater robot through the extended state observer, estimate the external disturbance, and use sonar to scan the state of underwater obstacles, and input the detection results into the Kalman filter;

[0059] (3) Using the established kinematic dynamics model, the motion state of the underwater robot in the next N control cycles is predicted according to the current speed of the underwater robot and the external disturbance, and the trajectory of the underwater obstacle in a short time is also predicted;

[0060] (4) Based on the predicted motion state of the underwater robot in the next N control cycles and the short-term trajectory of the underwater obstacle, the solver is used to solve the optimization problem and obtain the optimal control sequence, and the first position of the optimal control sequence is converted as the control input signal of the underwater robot's wave fin thruster;

[0061] (5) If the sonar scanning cycle ends and the motion state of the underwater obstacle measured by the sonar is updated, the input of the Kalman filter is updated, and the new prior state and posterior state of the Kalman filter are calculated. The uncertainty of the underwater obstacle is also updated and calculated. If the sonar scanning cycle has not ended, the Kalman filter and the uncertainty of the underwater obstacle are not updated and remain unchanged.

[0062] (6) Repeat steps (2) to (5) until the underwater robot can perform path tracking and complete obstacle avoidance.

[0063] In the present invention, path planning means: in an environment with obstacles, given a starting point and a target point of the robot, and then requiring the robot to start from a certain point, reach the path according to a certain control law, and realize its tracking motion.

[0064] NMPC stands for nonlinear model predictive control: it is a closed-loop optimization control strategy based on a nonlinear model. For strongly nonlinear systems, linear models are difficult to describe complex dynamic processes, and linearized models are not very applicable, making linear predictive control difficult to use. Therefore, the nonlinear system model predictive control is proposed.

[0065] Kalman filter: An algorithm that uses the linear system state equation to optimally estimate the system state through system input and output observation data.

[0066] Extended State Observer: A general and practical disturbance observer.

[0067] The step (1) specifically refers to:

[0068] Establish the kinematic dynamics model of the underwater robot under three degrees of freedom:

[0069]

[0070] in, is the state vector, V = [urv] T represents the speed of the underwater robot, x and y represent the position of the underwater robot in the earth coordinate system, represents the underwater robot's bow pitch angle, u is the underwater robot's forward speed, v is the underwater robot's lateral speed, and r is the underwater robot's bow pitch speed; is the control input, f u is the control input pitch force, f v is the lateral driving force, the underwater robot is an underactuated system, f v =0; f r is the yaw moment; τ d =[d u d v d r ] T is the external disturbance of the underwater robot, d u is the disturbance force in the underwater robot's forward direction, d v is the disturbance force in the lateral direction of the underwater robot, d r is the external environmental disturbance torque in the bow rolling direction, M is the inertial mass matrix of the underwater robot, D is the damping force diagonal matrix of the underwater robot, C(v) is the centripetal force and Coriolis matrix, and its formula is as follows:

[0071]

[0072] Among them, m 11 、m 22 、m 33 are the additional mass parameters of the underwater robot, d 11 d 22 d 33 are the hydrodynamic coefficients;

[0073] It is the rotation matrix that converts the underwater robot's lateral velocity v in the body coordinate system into the velocity η in the earth coordinate system, specifically:

[0074]

[0075] Establish the objective function of the optimization problem:

[0076]

[0077] Among them, J x (x(t),x ref (t)) is the expected state cost, and its formula is:

[0078]

[0079] J u (αu(t)) is the control input cost, and its formula is:

[0080]

[0081] J c (x(t) is the collision cost, and its formula is:

[0082]

[0083] Where T is the sampling period of the underwater robot, f(x,u) is the continuous nonlinear equation describing the underwater robot, and d i (t) is expressed as the Euclidean distance to the i-th underwater obstacle, k i >0 is the parameter of smooth obstacle avoidance trajectory; is the weight coefficient matrix of the expected state cost term, is the weight coefficient matrix for controlling the input cost, Q c represents the collision cost coefficient; U is the input range of the control force and torque, as shown in the following formula:

[0084]

[0085] The underwater robot is an underactuated system, so f vmin =f vmax =0; f vmin is the minimum lateral driving force of the underwater robot, f vmax is the maximum lateral driving force of the underwater robot, f umin is the minimum longitudinal driving force of the underwater robot, f umax is the maximum longitudinal driving force of the underwater robot, f rmin is the minimum value of the underwater robot’s bow pitch torque, f rmax is the maximum bow rolling moment of the underwater robot.

[0086] The step (2) specifically refers to:

[0087] Measure and calculate the underwater robot state x(t0), and use the extended state observer to calculate the underwater robot speed and external disturbance state:

[0088]

[0089]

[0090]

[0091] Wherein, l0 to l8 are all positive integers, b0 and b1 are compensation factors, ζ = u·t and χ = v·t represent the longitudinal and lateral displacements in the body coordinate system respectively; represents the underwater robot's bow pitch angle, u is the underwater robot's forward speed, v is the underwater robot's lateral speed, and r is the underwater robot's bow pitch speed; f u is the control input pitch force, f r is the yaw moment; m 11 、m 22 、m 33 are the additional mass parameters of the underwater robot, d 11 , d 22 d 33 are the hydrodynamic coefficients; by solving equations (6), (7), and (8) respectively, we can obtain the current longitudinal velocity of the underwater robot. Lateral speed Yaw speed And vertical Horizontal Bow Estimated values ​​of external disturbances in three directions, “∧” represents the calculated estimated value and “·” represents the differential sign.

[0092] The step (3) specifically refers to: using the kinematics and dynamics model of the underwater robot to calculate and predict the future state of the underwater robot And the underwater obstacle state is predicted through the underwater obstacle linear motion model:

[0093]

[0094] in, is the predicted position of underwater obstacle i, T s is the sampling period, p obs i (t0) is the measured position of the underwater obstacle at the current moment, v o- is the a priori speed measurement of underwater obstacles, N is the number of prediction steps, t0 is the current moment when the prediction starts, and n T are predicted in the future s Sampling period;

[0095] (3) The kinematic dynamics model is used to predict the motion state of the underwater robot in the next N control cycles. Formula (9) is used to calculate the current position of the underwater obstacle measured by the sonar, plus the underwater obstacle measurement prior speed multiplied by the sampling period T. s , get the next n T s The linear motion position of the underwater obstacle within the cycle.

[0096] The step (4) specifically refers to: using a solver to solve the optimization problem and obtain the optimal control sequence u * (t+nT s ), n = 1, ..., N-1, and put the optimal control sequence first The conversion is used as the control input signal of the underwater robot wave fin thruster, u * Represents the optimal control sequence solved by the solver.

[0097] The step (5) specifically refers to: if the sonar scanning cycle ends, that is, the measured state quantity of the underwater obstacle is updated, then the underwater obstacle prior state quantity of the Kalman filter is updated. and the posterior state quantity P t Update the calculation and the uncertainty of underwater obstacles r th，k (t) Perform calculation updates, otherwise the Kalman filter and underwater obstacle uncertainty are not updated;

[0098] The Kalman filter is divided into two parts:

[0099] predict:

[0100] renew:

[0101] Among them, x t =[x obs ,y obs , v obs ] T is the state of the underwater obstacle, x obs ,y obs is the coordinate position of the underwater obstacle, v obs is the speed of the underwater obstacle, Z t =[x o ,y o , v o ] T is the underwater obstacle state measured at time t, x o ,y o is the measured coordinate position of the underwater obstacle, v o is the measured speed of the underwater obstacle, F is the state transfer matrix, H is the observation matrix, Qt is the covariance matrix of underwater obstacle motion noise, R t is the observation noise covariance matrix, I is the identity matrix;

[0102] By solving the calculation Maximum eigenvalue δ i (t), the uncertainty of the ith underwater obstacle at time t is described by formula (12):

[0103] r th，i (t) = r th +10δ i (t) (12)

[0104] Among them, r th is a constant parameter.

[0105] In the simulation experiment, the present invention selects two cases, single dynamic underwater obstacle and multiple dynamic underwater obstacles, for simulation experiment. Parameter setting: m 11 =57.5, m 22 =61.3, m 33 =1.15,d 11 =53,d 22 = 58, and d 33 =3.1.

[0106] The initial state of the system is 0, and the additional disturbance is: τ d =[0.6*sin(0.075*t), 0.5* sin(0.075*t), 0.4*sin(0.075*t)] T N·m, the related parameters of nonlinear model predictive control are: T s =0.2s, N p =25, N c =1, relevant parameter weight: Q x =diag{55.0, 55.0, 5.0, 1.0, 800, 5.0} and Q u =diag{10.0, 25.0}, collision cost Q c =20, the expected longitudinal speed is 0.2m / s. The parameters of the extended state observer are: l1=3.6, l2=4.32, l3=1.728, l4=2.4, l5=1.92, l6=1.516, l7=2.4, l8=1.92, l9=0.528. Initial obstacle coordinates The movement speed is -0.15m / s, and the Gaussian distribution change added is N(0, 0.02).

[0107] Figures 3 to 6The key moment sequence diagram of path tracking and dynamic obstacle avoidance under the condition of adding external disturbance is shown. The curve is the expected path, the small radius circle is the underwater robot, the large radius circle is the dynamic underwater obstacle, and the mark symbol above the circle indicates the yaw angle at this time. Figure 3 t = 35.0s, Figure 4 t = 37.2s, Figure 5 t = 46.0s, Figure 6 At t = 52.6s, the large circle next to it represents the a posteriori position based on the Kalman filter. Figure 3 As can be seen in the figure, since there is an obstacle in front of the right side of the underwater robot simulation model, it chooses to deviate from the predetermined trajectory on the left side to avoid collision; it is worth noting that Figure 4 In the prediction of underwater obstacle trajectories, the vehicle chooses to drive to the right, which results in a lower collision probability, by combining the prior information in the Kalman filter. Figure 5 , Figure 6 It means that after avoiding dynamic underwater obstacles, the underwater robot quickly returns to the desired path.

[0108] exist Figure 7 It can be seen that path tracking without an extended state observer always has obvious errors, while the present invention can achieve error-free tracking. In addition, it can be seen that when the underwater robot can access the future trajectory predicted by the underwater obstacle, it will choose the opposite movement direction (positive x-axis direction) to the underwater obstacle to avoid collision.

[0109] In order to further verify the effectiveness of the proposed algorithm, multiple underwater obstacles were added in the same experimental scenario for further verification.

[0110] Table 1 Multi-obstacle simulation experimental data

[0111]

[0112] from Figures 8 to 13 From the simulation screenshots of multiple underwater obstacles, the present invention can stably achieve curve tracking under external disturbance conditions while stably avoiding multiple obstacles. Figure 8 For t = 7s, Fig. 9 For t = 10s, Fig.10 For t = 17s, Fig.11 For t = 23s, Fig.12 t = 28s, Fig.13The time t = 37.2s, multiple dynamic underwater obstacles are added near the expected trajectory of the underwater robot. For each added underwater obstacle, 10 simulation experiments are carried out continuously. The results of the scene experiment are shown in Table 1. After the prior results of the Kalman filter are integrated into the prediction module of the nonlinear model predictive control, the position of the obstacle is predicted in a shorter period, which will reduce the probability of collision and increase the closest distance to the obstacle, greatly improving the robustness of obstacle avoidance. Figures 8 to 13 In the figure, the starting point of the underwater robot simulation model is at the origin of the coordinate system (0,0); Figures 8 to 13 The curve in is the global tracking path , each circle represents a dynamic underwater obstacle with a different initial position.

[0113] In summary, the present invention adopts an improved expanded state observer to perform disturbance observation for the uncertainty of model parameters and external environmental interference caused by the unique propulsion method of the bionic underwater robot, and uses it as a nonlinear model predictive control disturbance term for continuous calculation and update, thereby greatly improving the anti-interference ability and robustness of the underwater robot's motion. The present invention aims at the uncertainty of dynamic obstacle state observation, that is, observation error, and too long sonar scanning cycle. By combining Kalman filter prior estimation, the future position of obstacles within a short period is predicted, thereby reducing the collision probability of the underwater robot, shortening the driving distance and increasing the safe distance from obstacles.

Claims

1. A control method for underwater robot path tracking and obstacle avoidance based on NMPC, characterized in that: The method comprises the following steps in order: (1) Establish the kinematics and dynamics model of the underwater robot, establish the objective function of the optimization problem, set the global path reference point, and use Bezier curve interpolation to obtain the global tracking path The bow angle is calculated; (2) Measure the current position of the underwater robot, read the bow roll angle, calculate the current speed of the underwater robot through the extended state observer, estimate the external disturbance, and use sonar to scan the state of underwater obstacles, and input the detection results into the Kalman filter; (3) Using the established kinematic dynamics model, the motion state of the underwater robot in the next N control cycles is predicted according to the current speed of the underwater robot and the external disturbance, and the trajectory of the underwater obstacle in a short time is also predicted; (4) Based on the predicted motion state of the underwater robot in the next N control cycles and the short-term trajectory of the underwater obstacle, the solver is used to solve the optimization problem and obtain the optimal control sequence, and the first position of the optimal control sequence is converted as the control input signal of the underwater robot's wave fin thruster; (5) If the sonar scanning cycle ends and the motion state of the underwater obstacle measured by the sonar is updated, the input of the Kalman filter is updated, and the new prior state and posterior state of the Kalman filter are calculated, and the uncertainty of the underwater obstacle is also updated and calculated. If the sonar scanning cycle has not ended, the Kalman filter and the uncertainty of the underwater obstacle are not updated and remain unchanged; (6) Repeat steps (2) to (5) until the underwater robot can perform path tracking and complete obstacle avoidance; The step (1) specifically refers to: Establish the kinematic dynamics model of the underwater robot under three degrees of freedom: in, is the state vector, V = [urv] T represents the speed of the underwater robot, x and y represent the position of the underwater robot in the earth coordinate system, represents the underwater robot's bow pitch angle, u is the underwater robot's forward speed, v is the underwater robot's lateral speed, and r is the underwater robot's bow pitch speed; is the control input, f u is the control input pitch force, f v is the lateral driving force, the underwater robot is an underactuated system, f v =0; f r is the yaw moment; τ d =[d u d v d r ] T is the external disturbance of the underwater robot, d u is the disturbance force in the underwater robot's forward direction, d v is the disturbance force in the lateral direction of the underwater robot, d r is the external environmental disturbance torque in the bow rolling direction, M is the inertial mass matrix of the underwater robot, D is the damping force diagonal matrix of the underwater robot, C(v) is the centripetal force and Coriolis matrix, and its formula is as follows: Among them, m 11 、m 22 、m 33 are the additional mass parameters of the underwater robot, d 11 ,d 22 ,d 33 are the hydrodynamic coefficients; It is the rotation matrix that converts the underwater robot's lateral velocity v in the body coordinate system into the velocity η in the earth coordinate system, specifically: Establish the objective function of the optimization problem: Among them, J x (x(t),x ref (t)) is the expected state cost, and its formula is: J u (Δu(t)) is the control input cost, and its formula is: J c (x(t) is the collision cost, and its formula is: Where T is the sampling period of the underwater robot, f(x,u) is the continuous nonlinear equation describing the underwater robot, and d i (t) is expressed as the Euclidean distance to the i-th underwater obstacle, k i >0 is the parameter of smooth obstacle avoidance trajectory; is the weight coefficient matrix of the expected state cost term, is the weight coefficient matrix for controlling the input cost, Q c represents the collision cost coefficient; U is the input range of the control force and torque, as shown in the following formula: The underwater robot is an underactuated system, so f vmin =f vmax =0; f vmin is the minimum lateral driving force of the underwater robot, f vmax is the maximum lateral driving force of the underwater robot, f umin is the minimum longitudinal driving force of the underwater robot, f umax is the maximum longitudinal driving force of the underwater robot, f rmin is the minimum value of the underwater robot’s bow pitch torque, f rmax is the maximum bow pitch moment of the underwater robot; The step (2) specifically refers to: Measure and calculate the underwater robot state x(t0), and use the extended state observer to calculate the underwater robot speed and external disturbance state: Wherein, l0 to l8 are all positive integers, b0 and b1 are compensation factors, ζ = u·t and χ = v·t represent the longitudinal and lateral displacements in the body coordinate system respectively; represents the underwater robot's bow pitch angle, u is the underwater robot's forward speed, v is the underwater robot's lateral speed, and r is the underwater robot's bow pitch speed; f u is the control input pitch force, f r is the yaw moment; m 11 、m 22 、m 33 are the additional mass parameters of the underwater robot, d 11 ,d 22 ,d 33 are the hydrodynamic coefficients; by solving equations (6), (7), and (8) respectively, we can obtain the current longitudinal velocity of the underwater robot. Lateral speed Yaw speed And vertical Horizontal Bow Estimated values ​​of external disturbances in three directions, "∧" indicates the calculated estimated value, and "·" indicates the differential sign; The step (3) specifically refers to: using the kinematics and dynamics model of the underwater robot to calculate and predict the future state of the underwater robot And the underwater obstacle state is predicted through the underwater obstacle linear motion model: in, is the predicted position of underwater obstacle i, T s is the sampling period, p obsi (t0) is the measured position of the underwater obstacle at the current moment, v o- Measure the prior speed of underwater obstacles, N is the number of prediction steps, t0 is the current moment when the prediction starts, and n T are predicted in the future s Sampling period; (3) The kinematic dynamics model is used to predict the motion state of the underwater robot in the next N control cycles. Formula (9) is used to calculate the current position of the underwater obstacle measured by the sonar, plus the underwater obstacle measurement prior speed multiplied by the sampling period T. s , get the next n T s The linear motion position of underwater obstacles within the cycle; The step (4) specifically refers to: using a solver to solve the optimization problem and obtain the optimal control sequence u * (t+nT s ), n = 1, ..., N-1, and put the optimal control sequence first The conversion is used as the control input signal of the underwater robot wave fin thruster, u * represents the optimal control sequence solved by the solver; The step (5) specifically refers to: if the sonar scanning cycle ends, that is, the measurement state of the underwater obstacle is updated, then the Kalman filter underwater obstacle prior state and the posterior state quantity P t Update the calculation and the uncertainty of underwater obstacles r th，k (t) Perform calculation updates, otherwise the Kalman filter and underwater obstacle uncertainty are not updated; The Kalman filter is divided into two parts: predict: renew: Among them, x t =[x obs ,y obs , v obs ] T is the state of the underwater obstacle, x obs ,y obs is the coordinate position of the underwater obstacle, v obs is the speed of the underwater obstacle, Z t =[x o ,y o , v o ] T is the underwater obstacle state measured at time t, x o ,y o is the measured coordinate position of the underwater obstacle, v o is the measured speed of the underwater obstacle, F is the state transfer matrix, H is the observation matrix, Q t is the covariance matrix of underwater obstacle motion noise, R t is the observation noise covariance matrix, I is the identity matrix; By solving the calculation Maximum eigenvalue δ i (t), the uncertainty of the i-th underwater obstacle at time t is described by formula (12): r th，i (t)=r th +10δ i (t) (12) Among them, r th is a constant parameter.

Images