Underwater vehicle trajectory planning control method based on dual-threshold event triggering mechanism

Abstract

The invention relates to an underwater vehicle trajectory planning control method based on a dual-threshold event triggering mechanism. The method comprises the following steps: S1, performing path planning based on a three-dimensional A star algorithm; s2, solving an optimal planning track of the underwater vehicle through a nonlinear model predictive control algorithm; s3, introducing a dual-threshold event triggering mechanism; and S4, the underwater vehicle sails towards the target point according to the optimal planning track, and track tracking is carried out based on a nonlinear model prediction control algorithm in the sailing process. A non-linear model predictive control algorithm is adopted to carry out track planning and track tracking, so that the adaptability of the underwater vehicle can be improved; meanwhile, a double-threshold event triggering mechanism is introduced in the flight path tracking process, the flight path tracking precision and the real-time performance of obstacle avoidance and a control system can be guaranteed, computing resources can be effectively utilized, and the computing efficiency is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater vehicle motion control, and in particular to a trajectory planning control method for an underwater vehicle based on a double-threshold event-triggering mechanism. Background Technique

[0002] Unmanned underwater vehicles have shown extensive application potential in the fields of marine scientific research, marine development, and underwater engineering due to their relatively low construction and usage costs. Compared with manned underwater vehicles, unmanned underwater vehicles, especially autonomous underwater vehicles, have stronger maneuverability and longer navigation distances because they do not need to be connected to the vehicle body through umbilical cables. To support them in successfully completing various tasks, unmanned underwater vehicles need to be equipped with mature and reliable autonomous navigation capabilities, including intelligent planning, control, and obstacle avoidance functions, forming a new underwater navigation mode.

[0003] In the prior art, for the planning, control, and obstacle avoidance of the navigation trajectory of unmanned underwater vehicles, various sensors such as radar, sonar, and cameras are usually used to monitor the environment around the route in real time. According to the information obtained by various sensors, the navigation trajectory of the unmanned underwater vehicle is planned in real time to avoid various static or dynamic obstacles, thereby ensuring navigation safety. However, this method of planning, controlling, and avoiding obstacles for the navigation trajectory has low computational efficiency and a long response time of the control system, resulting in poor real-time performance and difficulty in applying to complex and changeable underwater navigation environments. Summary of the Invention

[0004] Based on this, it is necessary to provide a trajectory planning control method for an underwater vehicle based on a double-threshold event-triggering mechanism to solve the problems of low computational efficiency, long response time of the control system, poor real-time performance, and poor adaptability existing in the prior art when the underwater vehicle conducts navigation trajectory planning, control, and obstacle avoidance.

[0005] The technical solution adopted by the present invention is as follows:

[0006] A trajectory planning control method for an underwater vehicle based on a double-threshold event-triggering mechanism includes the following steps:

[0007] S1. Before navigation, the underwater vehicle obtains initial obstacle information through a detection component and conducts path planning based on the three-dimensional A* algorithm;

[0008] S2. Based on the path planning result, the optimal planned trajectory of the underwater vehicle is solved through a nonlinear model predictive control algorithm;

[0009] S3. Introduce a double-threshold event-triggering mechanism, and preset the value of the fixed threshold m1 of the trajectory planning trigger mechanism and the value of the fixed threshold m2 of the trajectory tracking trigger mechanism in the control system of the underwater vehicle,

[0010] S4. The underwater vehicle sails towards the target point according to the optimal planned trajectory. During the sailing process, trajectory tracking is performed based on the non - linear model predictive control algorithm, so as to obtain the deviation value threshold ||y e (k)|| between the current trajectory and the optimal planned trajectory of the underwater vehicle at time t;

[0011] S5. Compare the deviation value threshold ||y e (k)|| with the fixed threshold m1 of the trajectory planning trigger mechanism. When the deviation value threshold ||y e (k)|| > the fixed threshold m1 of the trajectory planning trigger mechanism, perform S2 again to generate a new optimal planned trajectory;

[0012] When the deviation value threshold ||y e (k)|| ≤ the fixed threshold m1 of the trajectory planning trigger mechanism, compare the deviation value threshold ||y e (k)|| with the fixed threshold m2 of the trajectory tracking trigger mechanism. When the deviation value threshold ||y e (k)|| > the fixed threshold m2 of the trajectory tracking trigger mechanism, re - solve the control input of the underwater vehicle at time t based on the non - linear model predictive control algorithm, so as to correct the current trajectory of the underwater vehicle.

[0013] In S1, according to the sailing requirements of the underwater vehicle, the starting point and the target point are determined. The discrete expected path point set between the starting point and the target point is obtained by using the three - dimensional A* algorithm. By assigning a time series starting from zero and at equal intervals to the expected path point set, the preliminary planned trajectory is obtained.

[0014] In S2, according to the kinematic model and dynamic model of the underwater vehicle, a discrete state - space model is established. The discrete state - space model is iterated to obtain the state information of the underwater vehicle within the prediction time domain;

[0015] Through the offline trajectory tracking algorithm, a dynamic planning trajectory that meets the dynamic characteristics of the underwater vehicle is planned;

[0016] Taking the state information of the underwater vehicle within the prediction time domain as input data, based on the non - linear model predictive control algorithm, a first optimal problem model considering the error between the dynamic planning trajectory and the preliminary planned trajectory, the change amount of the control quantity, and the terminal error is established. By solving the first optimal problem model, the optimal planned trajectory of the underwater vehicle is obtained;

[0017] In S3, the fixed threshold m1 of the trajectory planning trigger mechanism > the fixed threshold m2 of the trajectory tracking trigger mechanism.

[0018] In S3, the expression of the double - threshold event - triggered mechanism is:

[0019] u(t) = u(k),

[0020] t k+1 = inf{t > t k | m2 < ||y e (k) || ≤ m1 ∨ d < d M}

[0021] In the above formula, u(·) represents the control input;

[0022] t represents time;

[0023] k represents the current time step;

[0024] t k represents the time corresponding to the current time step;

[0025] t k+1 represents the time corresponding to the next time step;

[0026] m2 represents the fixed threshold of the trajectory tracking trigger mechanism;

[0027] m1 represents the fixed threshold of the trajectory planning trigger mechanism;

[0028] ||y e (k) || represents the deviation value threshold between the output vector and the desired input vector;

[0029] d represents the current distance between the underwater vehicle and static or dynamic obstacles;

[0030] d M represents the safety distance between the underwater vehicle and static or dynamic obstacles.

[0031] According to the expression of the double - threshold event - triggered mechanism, when the current distance d between the underwater vehicle and static or dynamic obstacles < the safety distance d between the underwater vehicle and static or dynamic obstacles M at time t, the control input of the underwater vehicle is re - solved based on the nonlinear model predictive control algorithm, so as to correct the current trajectory of the underwater vehicle, and then the underwater vehicle can avoid static or dynamic obstacles.

[0032] According to the expression of the double - threshold event - triggered mechanism, when the current distance d between the underwater vehicle and static or dynamic obstacles ≥ the safety distance d between the underwater vehicle and static or dynamic obstacles M and the deviation value threshold ||y e (k) || ≤ the fixed threshold m2 of the trajectory tracking trigger mechanism, the underwater vehicle continues to move forward along the current optimal planned trajectory until it reaches the target point.

[0033] In S4, according to the kinematic model and dynamic model of the underwater vehicle, a discrete state space model is established, and the discrete state space model is iterated to obtain the state information of the underwater vehicle within the prediction time domain;

[0034] Taking the state information of the underwater vehicle within the prediction time domain as input data, a second optimal problem model considering the error between the current dynamic trajectory and the dynamically planned trajectory, the change amount of the control quantity, the dynamic and static obstacle avoidance functions, and the terminal error is established based on the nonlinear model predictive control algorithm;

[0035] The expression of the second optimal problem model is:

[0036] min u(k) J2(x(k), u(k))

[0037] s.t. x(x + i + 1) = f(x(k + i), u(k + i), d(k + i)), i = 0, 1, …, N P

[0038] u(k) ∈ U

[0039] y(k) = Cx(k)

[0040] In the above, J2(x(k), u(k)) represents the optimal function;

[0041] x(·) represents the input state;

[0042] u(·) represents the control input;

[0043] d(·) represents the error vector;

[0044] y(·) represents the output state;

[0045] k represents the time step;

[0046] N P represents the prediction time domain;

[0047] U represents the control input constraint;

[0048] C represents the state output matrix;

[0049] i is a non - negative integer;

[0050] f(·) is a nonlinear function, representing the six - degree - of - freedom state space model of the input - output relationship of the underwater vehicle.

[0051] The expression of the optimal function J2(x(k), u(k)) is:

[0052]

[0053] In the above formula, x(·) represents the input state;

[0054] u(·) represents the control input;

[0055] k represents the time step;

[0056] N P represents the prediction horizon;

[0057] x ref (·) represents the reference state vector;

[0058] Q represents the state weight matrix;

[0059] x f (·) represents the terminal state vector;

[0060] P represents the terminal state weight matrix

[0061] N C represents the control horizon;

[0062] R represents the control input;

[0063] N represents the prediction horizon of static or dynamic obstacles;

[0064] r s represents the radius of the static obstacle;

[0065] cost s (·) represents the static obstacle avoidance function;

[0066] r d represents the radius of the dynamic obstacle;

[0067] cost d (·) represents the dynamic obstacle avoidance function;

[0068] i is a non - negative integer.

[0069] The beneficial effects of the present invention are as follows:

[0070] The present invention has a compact and reasonable structure and is easy to operate. By adopting the non - linear model predictive control algorithm for trajectory planning and trajectory tracking, it can improve the shipping efficiency of the underwater vehicle, thereby enhancing the adaptability of the underwater vehicle and reducing the risk of maritime accidents caused by human errors. It is an important technical guarantee for the realization of the navigation of unmanned underwater vehicles. At the same time, a dual - threshold event - triggered mechanism is introduced during the trajectory tracking process. Taking event information as the trigger basis, various situations of the trajectory error during trajectory tracking are fully considered, and the distance between the underwater vehicle and the obstacle is taken into account to comprehensively determine the trigger condition, while ensuring the trajectory tracking accuracy, obstacle avoidance, and real - time performance of the control system, and can effectively utilize computing resources and improve computing efficiency.

[0071] By introducing a non - linear model predictive control algorithm, the present invention can simultaneously consider the dynamic constraints and navigation safety constraints of the ship, balance these factors during the optimization process, and obtain a control strategy with better comprehensive performance. In addition, it can also dynamically adjust the ship's navigation strategy according to the real - time perceived environmental information, such as the positions and motion states of other ships, so as to better cope with the dynamic and uncertain navigation environment.

[0072] By introducing an event - triggered mechanism and adding a cost function for static obstacle avoidance and a cost function for dynamic obstacle avoidance to the second - order optimal problem model, the present invention can respond to environmental changes in real time, actively take collision - avoidance operations, ensure effective obstacle avoidance during the navigation of the underwater vehicle, and maximize the safety navigation ability of the underwater vehicle in complex sea areas. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 is a flowchart of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0074] The following will describe the detailed implementation of the present invention in conjunction with the drawings.

[0075] As Figure 1 shown, a trajectory planning and control method for an underwater vehicle based on a dual - threshold event - triggered mechanism includes the following steps:

[0076] S1. Before navigation, the underwater vehicle obtains initial obstacle information through a detection component and conducts path planning based on the three - dimensional A* algorithm.

[0077] S1.1. First, according to an electronic nautical chart or a detection component (such as radar, sonar, etc.), obtain the known initial obstacle information in the navigation direction within the detection range.

[0078] S1.2. Then, according to the navigation requirements of the underwater vehicle, determine the starting point and the target point, use the three - dimensional A* algorithm to obtain a discrete set of expected path points between the starting point and the target point, and assign a time series starting from zero and at equal intervals to the set of expected path points, thereby obtaining a preliminary planned track.

[0079] The preliminary planned track includes the expected positions and heading angles at each time point within a certain future time.

[0080] S2. Based on the path planning result, solve the optimal planned track of the underwater vehicle through a non - linear model predictive control algorithm, including the following steps:

[0081] S2.1. According to the kinematic model and dynamic model of the underwater vehicle, establish a discrete state - space model, and iterate the discrete state - space model to obtain the state information of the underwater vehicle within the prediction time domain.

[0082] S2.2. Through the offline trajectory tracking algorithm, plan a dynamic programming trajectory that meets the dynamic characteristics of the underwater vehicle;

[0083] S2.3. Take the state information of the underwater vehicle within the prediction time domain as input data. Based on the nonlinear model predictive control algorithm, establish a first optimal problem model considering the error between the dynamic programming trajectory and the preliminary planning trajectory, the change in the control quantity, and the terminal error. Solve the first optimal problem model to obtain the optimal planning trajectory of the underwater vehicle;

[0084] The expression of the first optimal problem model is:

[0085] min u(k) J1(x(k),u(k))

[0086] s.t.x(k+i+1)=f(x(k+i),u(k+i),d(k+i)),i=0,1,…,N P

[0087] u(k)∈U,k=0,1,…,N C -1

[0088] y(k)=Cx(k),k=0,1,…,N P -1

[0089] In the above formula, J1(x(k),u(k)) represents the optimal function;

[0090] x(·) represents the input state;

[0091] u(·) represents the control input;

[0092] d(·) represents the error vector;

[0093] y(·) represents the output state;

[0094] k represents the current time step;

[0095] N P represents the prediction time domain;

[0096] N C represents the control time domain;

[0097] U represents the control input constraint;

[0098] C represents the state output matrix;

[0099] f(·) is a nonlinear function, representing the six-degree-of-freedom state space model of the input-output relationship of the underwater vehicle;

[0100] i is a non-negative integer;

[0101] Among them, the expression of the optimal function \(J_1(x(k), u(k))\) is as follows:

[0102]

[0103] In the above formula, \(N\) P represents the prediction horizon;

[0104] \(x(\cdot)\) represents the input state;

[0105] \(x\) ref (\cdot) represents the reference state vector;

[0106] \(Q\) represents the state weight matrix;

[0107] \(x\) f (\cdot) represents the terminal state vector;

[0108] \(P\) represents the terminal state weight matrix;

[0109] \(N\) C represents the control horizon;

[0110] \(u(\cdot)\) represents the control input;

[0111] \(R\) represents the control input error weight matrix;

[0112] \(i\) is a non - negative integer;

[0113] \(j\) is a non - negative integer;

[0114] S2.4. Solve the optimal control sequence that minimizes the optimal function \(J_1(x(k), u(k))\) at each time step \(k\), and record the control quantity and state quantity at each time step \(k\);

[0115] S3. Introduce a dual - threshold event - triggered mechanism, and preset the fixed threshold value \(m_1\) of the trajectory planning trigger mechanism and the fixed threshold value \(m_2\) of the trajectory tracking trigger mechanism in the control system of the underwater vehicle;

[0116] Among them, the fixed threshold value \(m_1\) of the trajectory planning trigger mechanism and the fixed threshold value \(m_2\) of the trajectory tracking trigger mechanism are the core parameters of the dual - threshold event - triggered mechanism. The fixed threshold value \(m_1\) of the trajectory planning trigger mechanism represents the maximum threshold value allowing the underwater vehicle to deviate from the optimal planned trajectory, allowing a small deviation in the trajectory of the underwater vehicle; the setting of the fixed threshold value \(m_2\) of the trajectory tracking trigger mechanism needs to balance the computational performance and control performance of the control system, ensuring the control accuracy while reducing the triggering times of the nonlinear model predictive control algorithm; usually, the fixed threshold value \(m_1\) of the trajectory planning trigger mechanism \(>\) the fixed threshold value \(m_2\) of the trajectory tracking trigger mechanism;

[0117] The expression of the dual - threshold event - triggered mechanism is:

[0118] u(t) = u(k),

[0119] t k+1 = inf{t > t k | m2 < ||y e (k) || ≤ m1 ∨ d < d M}

[0120] In the above formula, u(·) represents the control input;

[0121] t represents time;

[0122] k represents the current time step;

[0123] t k represents the time corresponding to the current time step;

[0124] t k+1 represents the time corresponding to the next time step;

[0125] m2 represents the fixed threshold of the trajectory tracking trigger mechanism;

[0126] m1 represents the fixed threshold of the trajectory planning trigger mechanism;

[0127] ||y e (k) || represents the deviation value threshold between the output vector and the desired input vector;

[0128] d represents the current distance between the underwater vehicle and a static or dynamic obstacle;

[0129] d M represents the safety distance between the underwater vehicle and a static or dynamic obstacle;

[0130] S3.1. According to the expression of the dual-threshold event-triggered mechanism, when the current distance d between the underwater vehicle and a static or dynamic obstacle < the safety distance d between the underwater vehicle and a static or dynamic obstacle M at time t, the control input of the underwater vehicle is re-solved based on the nonlinear model predictive control algorithm, so as to correct the current trajectory of the underwater vehicle, and further enable the underwater vehicle to avoid static or dynamic obstacles.

[0131] S3.2. According to the expression of the dual-threshold event-triggered mechanism, when the current distance d between the underwater vehicle and a static or dynamic obstacle ≥ the safety distance d between the underwater vehicle and a static or dynamic obstacle M and the deviation value threshold ||y e (k) || ≤ the fixed threshold m2 of the trajectory tracking trigger mechanism, continue to move forward along the current optimal planned trajectory until the underwater vehicle reaches the target point;

[0132] S4. The underwater vehicle sails towards the target point along the optimal planned trajectory. During the sailing process, based on the non - linear model predictive control algorithm, track tracking is carried out to obtain the deviation value threshold ‖y e (k)‖ between the current trajectory and the optimal planned trajectory of the underwater vehicle at time t; The method includes the following steps:

[0133] S4.1. According to the kinematic model and dynamic model of the underwater vehicle, a discrete state - space model is established, and the discrete state - space model is iterated to obtain the state information of the underwater vehicle within the prediction time domain;

[0134] S4.2. Introduce the cost function of static obstacle avoidance and the cost function of dynamic obstacle avoidance;

[0135] The expression of the cost function of static obstacle avoidance at time step k is:

[0136]

[0137] In the above formula, cost s (·) represents the static obstacle - avoidance function;

[0138] k represents the time step;

[0139] (x, y, z) represents the position coordinates of the underwater vehicle in the fixed coordinate system;

[0140] (x s , y s , z s ) represents the position coordinates of the static obstacle in the fixed coordinate system;

[0141] c1 is a constant;

[0142] The expression of the cost function of dynamic obstacle avoidance at time step k is:

[0143]

[0144] In the above formula, cost d (·) represents the dynamic obstacle - avoidance function;

[0145] k represents the time step;

[0146] (x, y, z) represents the position coordinates of the underwater vehicle in the fixed coordinate system;

[0147] (x d , y d , z d ) represents the position coordinates of the dynamic obstacle in the fixed coordinate system;

[0148] c2 is a constant;

[0149] S4.3. Use the state information of the underwater vehicle within the prediction time domain as input data, and establish a second optimal problem model considering the error between the current dynamic trajectory and the dynamically planned trajectory, the change in the control quantity, the dynamic and static obstacle avoidance functions, and the terminal error based on the nonlinear model predictive control algorithm;

[0150] The expression of the second optimal problem model is:

[0151] min u(k) J2(x(k), u(k))

[0152] s.t. x(x + i + 1) = f(x(k + i), u(k + i), d(k + i)), i = 0, 1, …, N P

[0153] u(k) ∈ U

[0154] y(k) = Cx(k)

[0155] Among them, J2(x(k), u(k)) represents the optimal function;

[0156] x(·) represents the input state;

[0157] u(·) represents the control input;

[0158] d(·) represents the error vector;

[0159] y(·) represents the output state;

[0160] k represents the time step;

[0161] N P represents the prediction time domain;

[0162] U represents the control input constraint;

[0163] C represents the state output matrix;

[0164] i is a non - negative integer;

[0165] f(·) is a nonlinear function representing the six - degree - of - freedom state - space model of the input - output relationship of the underwater vehicle;

[0166] Among them, the expression of the optimal function J2(x(k), u(k)) is:

[0167]

[0168] In the above formula, x(·) represents the input state;

[0169] u(·) represents the control input;

[0170] k represents the time step;

[0171] N P represents the prediction horizon;

[0172] x ref (·) represents the reference state vector;

[0173] Q represents the state weight matrix;

[0174] x f (·) represents the terminal state vector;

[0175] P represents the terminal state weight matrix

[0176] N C represents the control horizon;

[0177] R represents the control input;

[0178] N represents the prediction horizon of static or dynamic obstacles;

[0179] r s represents the radius of the static obstacle;

[0180] cost s (·) represents the static obstacle avoidance function;

[0181] r d represents the radius of the dynamic obstacle;

[0182] cost d (·) represents the dynamic obstacle avoidance function;

[0183] i is a non - negative integer;

[0184] S4.4. Solve the optimal function J2(x(k), u(k)) in real - time through the second - order optimal problem model to obtain the optimal control input u(k) at the current time step k;

[0185] In addition, the underwater vehicle is a real - time dynamic system, and the disturbances it suffers will change in real - time. If the solution time of the second - order optimal problem model is too long, the solved control input u(·) will not be able to respond to the state changes of the system in time, which will affect the control accuracy. At the same time, the marine condition environment is relatively complex, and there may be other obstacles. If the calculation time is too long, it will not be able to meet the real - time requirements of safety. Therefore, the control system of the underwater vehicle must have good control performance and computing performance at the same time to meet the navigation requirements of the underwater vehicle and the berthing requirements in complex in - port scenarios;

[0186] S5. Without loss of generality, track - control the current trajectory during the underwater vehicle's navigation through the second - order optimal problem model combined with the dual - threshold event - triggered mechanism;

[0187] Compare the deviation value threshold ||y e (k)|| with the fixed threshold m1 of the trajectory planning trigger mechanism. When the deviation value threshold ||y e (k)|| > the fixed threshold m1 of the trajectory planning trigger mechanism, repeat S2 to generate a new optimal planned trajectory;

[0188] When the deviation value threshold ||y e (k)|| ≤ the fixed threshold m1 of the trajectory planning trigger mechanism, compare the deviation value threshold ||y e (k)|| with the fixed threshold m2 of the trajectory tracking trigger mechanism;

[0189] When the deviation value threshold ||y e (k)|| > the fixed threshold m2 of the trajectory tracking trigger mechanism or the current distance d between the underwater vehicle and a static or dynamic obstacle < the safe distance d between the underwater vehicle and a static or dynamic obstacle M at this time, re - solve the control input of the underwater vehicle at time t based on the non - linear model predictive control algorithm to correct the current trajectory of the underwater vehicle;

[0190] When the current distance d between the underwater vehicle and a static or dynamic obstacle ≥ the safe distance d between the underwater vehicle and a static or dynamic obstacle M and the deviation value threshold ||y e (k)|| ≤ the fixed threshold m2 of the trajectory tracking trigger mechanism, continue to move forward along the current optimal planned trajectory until the underwater vehicle reaches the target point.

[0191] The above description is an explanation of the present invention, not a limitation of the invention. The scope defined by the present invention is shown in the claims. Within the protection scope of the present invention, any form of modification can be made.

Claims

1. A method for underwater vehicle trajectory planning and control based on a dual-threshold event trigger mechanism, characterized in that: The steps include: S1. Before sailing, the underwater vehicle obtains initial obstacle information through the detection component and performs path planning based on the three-dimensional A-star algorithm; S2. Based on the path planning results, the optimal planned trajectory of the underwater vehicle is solved by a nonlinear model predictive control algorithm; S3. Introducing a dual-threshold event trigger mechanism, presetting the value of the fixed threshold m1 of the track planning trigger mechanism and the value of the fixed threshold m2 of the track tracking trigger mechanism in the control system of the underwater vehicle; S4. The underwater vehicle navigates toward the target point according to the optimal planned track. During the navigation process, the track is tracked based on the nonlinear model predictive control algorithm to obtain the deviation value threshold value || y between the current track of the underwater vehicle at time t and the optimal planned track e (k)||; S5. Compare the deviation value threshold ||y e (k)|| and the fixed threshold m1 of the trajectory planning trigger mechanism, when the deviation value threshold||y e (k) ||>When the fixed threshold m1 of the trajectory planning trigger mechanism is reached, S2 is performed again to generate a new optimal planned trajectory; When the deviation value threshold ||y e (k)||≤ fixed threshold m1 of the trajectory planning trigger mechanism, compare the deviation value threshold ||y e (k)|| and the track tracking trigger mechanism fixed threshold m2, when the deviation value threshold ||y e (k)||>When the track tracking trigger mechanism has a fixed threshold m2, the control input of the underwater vehicle at time t is re-solved based on the nonlinear model predictive control algorithm, thereby correcting the current track of the underwater vehicle.

2. The underwater vehicle trajectory planning and control method based on the dual threshold event trigger mechanism according to claim 1, characterized in that: In S1, the starting point and the target point are determined according to the navigation requirements of the underwater vehicle, and the discrete expected path point set between the starting point and the target point is obtained using the three-dimensional A-star algorithm. By assigning a time series starting from zero and with equal intervals to the expected path point set, a preliminary planned trajectory is obtained.

3. The underwater vehicle trajectory planning and control method based on the dual threshold event trigger mechanism according to claim 1, characterized in that: In S2, a discrete state space model is established according to the kinematic model and dynamic model of the underwater vehicle, and the discrete state space model is iterated to obtain the state information of the underwater vehicle in the prediction time domain; Through the offline track tracking algorithm, the dynamic planning track that meets the dynamic characteristics of the underwater vehicle is planned; The state information of the underwater vehicle in the prediction time domain is used as input data, and based on the nonlinear model predictive control algorithm, a first optimal problem model is established that takes into account the error between the dynamically planned trajectory and the preliminary planned trajectory, the change in the control quantity, and the terminal error, and the optimal planned trajectory of the underwater vehicle is obtained by solving the first optimal problem model; 4. The underwater vehicle trajectory planning and control method based on the dual threshold event trigger mechanism according to claim 1, characterized in that: In S3, the fixed threshold m1 of the track planning trigger mechanism is greater than the fixed threshold m2 of the track tracking trigger mechanism.

5. The underwater vehicle trajectory planning and control method based on the dual threshold event trigger mechanism according to claim 1, characterized in that: In S3, the expression of the dual threshold event trigger mechanism is: u(t)=u(k), t k+1 =inf{t>t k |m2<||y e (k)||≤m1∨d<d M } In the above formula, u(·) represents the control input; t represents time; k represents the current time step; t k Indicates the time corresponding to the current time step; t k+1 Indicates the time corresponding to the next time step; m2 represents the fixed threshold of the track tracking trigger mechanism; m1 represents the fixed threshold of the trajectory planning trigger mechanism; ||y e (k)|| represents the deviation value threshold between the output vector and the expected input vector; d represents the current distance between the underwater vehicle and the static or dynamic obstacle; d M Indicates the safe distance between an underwater vehicle and static or dynamic obstacles.

6. The underwater vehicle trajectory planning and control method based on the dual threshold event trigger mechanism according to claim 5, characterized in that: According to the expression of the dual threshold event trigger mechanism, when the current distance d between the underwater vehicle and the static or dynamic obstacle is less than the safety distance d between the underwater vehicle and the static or dynamic obstacle, M When the control input of the underwater vehicle at time t is re-solved based on the nonlinear model predictive control algorithm, the current track of the underwater vehicle is corrected, so that the underwater vehicle can avoid static or dynamic obstacles.

7. The underwater vehicle trajectory planning and control method based on the dual threshold event trigger mechanism according to claim 5, characterized in that: According to the expression of the double threshold event trigger mechanism, when the current distance d between the underwater vehicle and the static or dynamic obstacle ≥ the safety distance d between the underwater vehicle and the static or dynamic obstacle M And the deviation value threshold ||y e When (k)||≤the fixed threshold m2 of the track tracking trigger mechanism, the underwater vehicle continues to move forward according to the current optimal planned track until it reaches the target point.

8. The underwater vehicle trajectory planning and control method based on the dual threshold event trigger mechanism according to claim 1, characterized in that: In S4, a discrete state space model is established according to the kinematic model and dynamic model of the underwater vehicle, and the discrete state space model is iterated to obtain the state information of the underwater vehicle in the prediction time domain; The state information of the underwater vehicle in the prediction time domain is used as input data, and a second optimal problem model is established based on the nonlinear model predictive control algorithm, which takes into account the error between the dynamic current track and the dynamic planned track, the change in control quantity, the dynamic and static obstacle avoidance functions, and the terminal error. The expression of the second optimal problem model is: min u(k) J2(x(k),u(k)) s.t.x(x+i+1)=f(x(k+i),u(k+i),d(k+i)),i=0,1,…,N P u(k)∈U y(k)=Cx(k) In the above figure, J2(x(k),u(k)) represents the optimal function; x(·) represents the input state; u(·) represents the control input; d(·) represents the error vector; y(·) represents the output state; k represents the time step; N P represents the prediction time domain; U represents the control input constraint; C represents the state output matrix; i is a non-negative integer; f(·) is a nonlinear function, which represents the six-degree-of-freedom state space model of the input-output relationship of the underwater vehicle.

9. The underwater vehicle trajectory planning and control method based on the dual threshold event trigger mechanism according to claim 8, characterized in that: The expression of the optimal function J2(x(k),u(k)) is: In the above formula, x(·) represents the input state; u(·) represents the control input; k represents the time step; N P represents the prediction time domain; x ref (·) represents the reference state vector; Q represents the state weight matrix; x f (·) represents the terminal state vector; P represents the terminal state weight matrix N C represents the control time domain; R represents the control input; N represents the prediction time domain of static or dynamic obstacles; r s Indicates the radius of static obstacles; cost s (·) represents the static obstacle avoidance function; r d Indicates the radius of dynamic obstacles; cost d (·) represents the dynamic obstacle avoidance function; i is a non-negative integer.

Images