Underwater vehicle obstacle avoidance method based on model predictive control and cost potential field

Abstract

The invention relates to an underwater vehicle obstacle avoidance method based on model predictive control and a cost potential field, and the method comprises the following steps: building a first optimal problem model of the track tracking of an underwater vehicle based on a model predictive control algorithm; adding a static cost function and a dynamic cost function into the first optimal problem model, and establishing a second optimal problem model; acquiring initial obstacle information through a detection assembly carried on the underwater vehicle; planning a global expected trajectory by combining the initial obstacle information acquired by the detection assembly according to the task requirement of the navigation of the underwater vehicle; the underwater vehicle sails according to the global expected trajectory until the terminal point is reached; and in the navigation process of the underwater vehicle, real-time obstacle information is acquired through the detection assembly, and when the real-time obstacle information acquired by the detection assembly shows that an obstacle is found, obstacle avoidance is performed according to the second optimal problem model. By establishing the second optimal problem model, unknown obstacles and dynamic obstacles can be effectively avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater vehicle motion control, and in particular to an obstacle avoidance method for an underwater vehicle based on model predictive control and cost potential field. Background Art

[0002] Unmanned underwater vehicles are one of the important tools used in current ocean development and can perform tasks such as underwater target exploration, disposal operations, seabed mapping, and marine scientific investigations. During the navigation process, unmanned underwater vehicles may encounter various static and dynamic obstacles.

[0003] In the prior art, for known static obstacles, unmanned underwater vehicles will avoid them in advance when planning the route to ensure the safety of the voyage. However, for unknown static obstacles or dynamic and uncertain obstacles, traditional unmanned underwater vehicles are difficult to avoid during the mission execution, so there is still a relatively high risk of damage and it is difficult to meet the current increasingly complex mission requirements. Summary of the Invention

[0004] Based on this, it is necessary to provide an obstacle avoidance method for an underwater vehicle based on model predictive control and cost potential field to solve the problem that unmanned underwater vehicles in the prior art are difficult to avoid unknown and dynamic obstacles during the mission execution.

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

[0006] An obstacle avoidance method for an underwater vehicle based on model predictive control and cost potential field, comprising the following steps:

[0007] S1. Based on the model predictive control algorithm, establish a first optimal problem model for the underwater vehicle trajectory tracking;

[0008] S2. Add a static cost function and a dynamic cost function to the first optimal problem model. The static cost function reflects the cost potential field formed by static obstacles, and the dynamic cost function reflects the cost potential field formed by dynamic obstacles, so as to introduce an obstacle avoidance function into the first optimal problem model to establish a second optimal problem model;

[0009] S3. Through the detection component carried on the underwater vehicle, obtain the initial obstacle information in the navigation direction within the detection range;

[0010] According to the mission requirements of the underwater vehicle navigation, determine the starting point and the target point, and combine the initial obstacle information obtained by the detection component to plan a global desired trajectory between the starting point and the target point;

[0011] S4. The underwater vehicle navigates according to the global desired trajectory until it reaches the end point;

[0012] Meanwhile, during the navigation of the underwater vehicle, real-time obstacle information is obtained through the detection component. When the real-time obstacle information obtained by the detection component shows that an obstacle is found, obstacle avoidance is performed according to the second optimal problem model.

[0013] As a further improvement of the above technical solution:

[0014] In S1, when establishing the first optimal problem model, the following steps are included:

[0015] S1.1 Establish a discrete state space model according to the kinematic and dynamic models of the unmanned underwater vehicle;

[0016] S1.2 Recursively optimize the discrete state space model through the model predictive control algorithm, so as to solve the optimal control sequence that can minimize the objective function, that is, the first optimal problem model, which is represented by (Equation 1):

[0017]

[0018] In (Equation 1), J1(x(k), u(k)) represents the optimal function;

[0019] x(·) represents the input state of the underwater vehicle;

[0020] u(·) represents the control input of the underwater vehicle;

[0021] k represents the time step;

[0022] d(·) represents the environmental disturbance received by the underwater vehicle;

[0023] y(·) represents the output state of the underwater vehicle;

[0024] N P represents the prediction horizon;

[0025] U represents the control input constraint;

[0026] C represents the state output matrix;

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

[0028] i is a non-negative integer.

[0029] In S2, when establishing the second optimal problem model, the following steps are included:

[0030] S2.1 The static cost function at the k time step is represented by (Equation 2):

[0031]

[0032] In Equation (2), cost s (·) represents the static obstacle avoidance function;

[0033] k represents the time step;

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

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

[0036] σ1 represents the standard deviation;

[0037] c1 is a constant;

[0038] S2.2. The dynamic cost function at time step k is represented by Equation (3):

[0039]

[0040] In Equation (3), cost d (·) represents the dynamic obstacle avoidance function;

[0041] k represents the time step;

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

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

[0044] σ2 represents the standard deviation;

[0045] c2 is a constant;

[0046] S2.3. Substitute Equation (2) and Equation (3) into Equation (1) to obtain the second optimal problem model, which is represented by Equation (4):

[0047]

[0048] In Equation (4), J2(x(k), u(k)) represents the optimal function;

[0049] x(·) represents the input state of the underwater vehicle;

[0050] u(·) represents the control input of the underwater vehicle;

[0051] k represents the time step;

[0052] d(·) represents the environmental disturbance suffered by the underwater vehicle;

[0053] y(·) represents the output state of the underwater vehicle;

[0054] N P represents the prediction horizon;

[0055] U represents the control input constraint;

[0056] C represents the state output matrix;

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

[0058] i is a non - negative integer.

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

[0060]

[0061] In the above formula, x(·) represents the input state of the underwater vehicle;

[0062] u(·) represents the control input of the underwater vehicle;

[0063] k represents the time step;

[0064] N P represents the prediction horizon;

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

[0066] Q represents the state weight matrix;

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

[0068] P represents the terminal state weight matrix;

[0069] N C represents the control horizon;

[0070] R represents the control variation weight matrix;

[0071] N represents the prediction horizon of static and dynamic obstacles;

[0072] N s represents the number of static obstacles;

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

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

[0075] N d represents the number of dynamic obstacles;

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

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

[0078] i is a non - negative integer;

[0079] j is a non - negative integer.

[0080] The global desired trajectory includes the desired positions and heading angles at each time point within a certain future time.

[0081] The detection component includes radar, sonar, and an underwater camera.

[0082] In S4., according to the position information and velocity information of the obstacle, determine the type of the obstacle, and simplify the obstacle into a sphere with the center as the center of the sphere and the farthest point from the center as the radius.

[0083] The types of the obstacles include static obstacles and dynamic obstacles.

[0084] In S4., according to the second - order optimal problem model, the underwater vehicle sails along the direction with a lower cost potential field of the obstacle during the sailing process, so as to achieve obstacle avoidance.

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

[0086] The structure of the present invention is compact, reasonable, and easy to operate. By combining the model predictive control algorithm with the cost function, the second - order optimal problem model (Equation 4) is obtained, which can effectively avoid unknown obstacles and dynamic obstacles, and has a clear principle, a simple form, and is easy to implement. At the same time, the dynamic characteristics of the underwater vehicle, model uncertainty, input saturation, environmental interference and other factors are comprehensively considered, which is more suitable for the marine environment and the motion characteristics of the underwater vehicle.

[0087] According to the type of the obstacle, the present invention can adopt different types of obstacle avoidance strategies. For known static obstacles, when planning the route, these static obstacles on the path will be avoided to ensure the safety of the voyage. For unknown static obstacles or dynamic obstacles, the underwater vehicle needs to avoid them in real - time during the navigation and motion control process. Once it is found that there is a collision risk around, the underwater vehicle plans the obstacle avoidance path according to its own position state, target state and environmental conditions through the second - order optimal problem model, and calculates a safe and optimized navigation trajectory to avoid collisions.

[0088] In the present invention, by adding a cost function, the collision risk can be predicted in real time, so that it is convenient for an underwater vehicle to calculate avoidance actions such as deceleration or steering according to the second optimal problem model to optimize the route and maintain navigation within a safe distance, which helps to improve the shipping efficiency and can reduce the accident risk caused by human errors, and is an important technical guarantee for realizing the navigation of an unmanned underwater vehicle.

[0089] In the present invention, the first optimal problem model (Equation 1) is based on a model predictive control algorithm, which can suppress the uncertainty of model parameters and external disturbances to a certain extent, thereby improving the robustness of the control system of the underwater vehicle; it can also consider the dynamic constraints of the ship and the obstacle cost at the same time, and balance these factors during the optimization process to obtain a control strategy with better comprehensive performance; in addition, the model predictive control algorithm can dynamically adjust the navigation strategy of the ship according to the real-time perceived environmental information, such as the position and motion state of other ships, so as to better cope with the dynamic and uncertain navigation environment; the model predictive control can dynamically optimize the navigation trajectory of the ship based on the current state and the predicted future state, and achieve more efficient and optimized navigation on the premise of meeting safety constraints. BRIEF DESCRIPTION OF THE DRAWINGS

[0090] Figure 1 is a flowchart of the present invention.

[0091] Figure 2 is a schematic diagram of the cost potential field of dynamic obstacles in the present invention DETAILED DESCRIPTION OF THE INVENTION

[0092] The following will describe the detailed implementation manners of the present invention with reference to the drawings.

[0093] As Figure 1 shown, an underwater vehicle obstacle avoidance method based on model predictive control and cost potential field includes the following steps:

[0094] S1. Based on the model predictive control algorithm, establish a first optimal problem model for the underwater vehicle trajectory tracking;

[0095] When establishing the first optimal problem model, the following steps are included:

[0096] S1.1 According to the kinematic and dynamic models of the unmanned underwater vehicle, establish a discrete state space model;

[0097] S1.2. Recursively optimize the discrete state space model through the model predictive control algorithm, so as to solve the optimal control sequence that can minimize the objective function, that is, the first optimal problem model, which is represented by (Equation 1):

[0098]

[0099] In (Equation 1), J1(x(k), u(k)) represents the optimal function;

[0100] x(·) represents the input state of the underwater vehicle;

[0101] u(·) represents the control input of the underwater vehicle;

[0102] k represents the time step;

[0103] d(·) represents the environmental disturbance on the underwater vehicle;

[0104] y(·) represents the output state of the underwater vehicle;

[0105] N P represents the prediction horizon;

[0106] U represents the control input constraint;

[0107] C represents the state output matrix;

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

[0109] i is a non - negative integer;

[0110] The expression of the optimal function J1(x(k), u(k)) is as follows:

[0111]

[0112] In the above formula, N P represents the prediction horizon;

[0113] x(·) represents the input state of the underwater vehicle;

[0114] reference input state;

[0115] Q represents the state error weight matrix;

[0116] xf(·) represents the terminal state vector;

[0117] P represents the terminal state error weight matrix;

[0118] N C represents the control horizon;

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

[0120] R represents the control input change weight matrix;

[0121] i is a non - negative integer;

[0122] j is a non - negative integer;

[0123] In addition, when establishing the first optimal problem model, factors such as input saturation, model uncertainty, and environmental interference need to be considered;

[0124] S2. Add a static cost function and a dynamic cost function to the first optimal problem model. The static cost function reflects the cost potential field formed by static obstacles, and the dynamic cost function reflects the cost potential field formed by dynamic obstacles, thereby introducing an obstacle avoidance function into the first optimal problem model to establish the second optimal problem model;

[0125] When establishing the second optimal problem model, the following steps are included:

[0126] S2.1. The static cost function at the k-th time step is represented by (Equation 2):

[0127]

[0128] (In Equation 2), cost s (·) represents the static obstacle avoidance function;

[0129] k represents the time step;

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

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

[0132] σ1 represents the standard deviation;

[0133] c1 is a constant;

[0134] By selecting the center position of the three-dimensional Gaussian function as the center of the static obstacle, a cost potential field is formed around the static obstacle, making the underwater vehicle tend to move along the low potential field area to avoid static obstacles;

[0135] S2.2. As Figure 2 shown, the dynamic cost function at the k-th time step is represented by (Equation 3):

[0136]

[0137] (In Equation 3), cost d (·) represents the dynamic 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 d , yd , z d ) represents the position coordinates of the dynamic obstacle in the fixed coordinate system;

[0141] σ2 represents the standard deviation;

[0142] c2 is a constant;

[0143] S2.3. Substitute (Equation 2) and (Equation 3) into (Equation 1) to obtain the second optimal problem model, which is represented by (Equation 4):

[0144]

[0145] In (Equation 4), J2(x(k), u(k)) represents the optimal function;

[0146] x(·) represents the input state of the underwater vehicle;

[0147] u(·) represents the control input of the underwater vehicle;

[0148] k represents the time step;

[0149] d(·) represents the environmental disturbance received by the underwater vehicle;

[0150] y(·) represents the output state of the underwater vehicle;

[0151] N P represents the prediction horizon;

[0152] U represents the control input constraint;

[0153] C represents the state output matrix;

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

[0155] i is a non-negative integer.

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

[0157]

[0158] In the above formula, x(·) represents the input state of the underwater vehicle;

[0159] u(·) represents the control input of the underwater vehicle;

[0160] k represents the time step;

[0161] N P represents the prediction horizon;

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

[0163] Q represents the state weight matrix;

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

[0165] P represents the terminal state weight matrix;

[0166] N C represents the control time domain;

[0167] R represents the control variation weight matrix;

[0168] N represents the prediction time domain of static and dynamic obstacles;

[0169] N s represents the number of static obstacles;

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

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

[0172] N d represents the number of dynamic obstacles;

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

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

[0175] i is a non - negative integer;

[0176] j is a non - negative integer;

[0177] In this embodiment, the cost potential fields of static and dynamic obstacles are both associated with the radius parameter. Since the state information of dynamic obstacles is changing, the uncertainty of the positions of dynamic obstacles needs to be considered in the dynamic cost function. Specifically, after the k - th time step, the function value of the (1 + i)r d term increases with the increase of the prediction time domain, so that the potential field range of dynamic obstacles increases. By establishing (Equation 4), the underwater vehicle can consider the positions of obstacles during navigation, thus effectively avoiding static and dynamic obstacles;

[0178] S3. Through the detection component mounted on the underwater vehicle, obtain the initial obstacle information in the navigation direction within the detection range; the initial obstacle information is the obstacle information known before the underwater vehicle sails;

[0179] According to the mission requirements of the underwater vehicle's navigation, determine the starting point and the target point, and combine the initial obstacle information obtained by the detection component, so as to plan the global desired trajectory between the starting point and the target point;

[0180] S3.1. The detection component includes radar, sonar and underwater camera;

[0181] S3.2. The global desired trajectory includes the desired positions and heading angles at each time point within a certain future time.

[0182] S4. The underwater vehicle navigates according to the global desired trajectory until it reaches the end point;

[0183] At the same time, during the navigation of the underwater vehicle, real-time obstacle information is obtained through the detection component. When the real-time obstacle information obtained by the detection component shows that an obstacle is found, obstacle avoidance is carried out according to the second optimal problem model.

[0184] S4.1. According to the position information and speed information of the obstacle, judge the type of the obstacle, and simplify the obstacle into a sphere with the center as the center of the sphere and the farthest point from the center as the radius;

[0185] The types of obstacles include static obstacles and dynamic obstacles;

[0186] S4.2. According to the second optimal problem model, the underwater vehicle navigates along the direction with a low cost potential field of the obstacle during navigation, so as to achieve obstacle avoidance.

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

Claims

1. An underwater vehicle obstacle avoidance method based on model predictive control and cost potential field, characterized in that: The steps include: S1. Based on the model predictive control algorithm, the first optimal problem model of underwater vehicle track tracking is established; S2. Adding a static cost function and a dynamic cost function to the first optimal problem model, wherein the static cost function reflects the cost potential field formed by static obstacles, and the dynamic cost function reflects the cost potential field formed by dynamic obstacles, thereby introducing an obstacle avoidance function into the first optimal problem model to establish a second optimal problem model; S3. Obtaining initial obstacle information in the navigation direction within the detection range through the detection component carried on the underwater vehicle; According to the mission requirements of the underwater vehicle, the starting point and the target point are determined, and the initial obstacle information obtained by the detection component is combined to plan the global expected trajectory between the starting point and the target point; S4. The underwater vehicle navigates according to the global desired trajectory until it reaches the destination; At the same time, during the navigation of the underwater vehicle, real-time obstacle information is obtained through the detection component. When the real-time obstacle information obtained by the detection component shows that an obstacle is found, obstacle avoidance is performed according to the second optimal problem model.

2. The underwater vehicle obstacle avoidance method based on model predictive control and cost potential field as claimed in claim 1, characterized in that: In S1, when establishing the first optimal problem model, the following steps are included: S1.1 Establish a discrete state space model based on the kinematic and dynamic models of the unmanned underwater vehicle; S1.

2. The discrete state space model is recursively optimized by the model predictive control algorithm to solve the optimal control sequence that minimizes the objective function, that is, the first optimal problem model, which is expressed by (Formula 1): In (Equation 1), J1(x(k),u(k)) represents the optimal function; x(·) represents the input state of the underwater vehicle; u(·) represents the control input of the underwater vehicle; k represents the time step; d(·) represents the environmental interference to the underwater vehicle; y(·) represents the output state of the underwater vehicle; N P represents the prediction time domain; U represents the control input constraint; C represents the state output matrix; f(·) is a nonlinear function, representing the six-degree-of-freedom state space model of the input-output relationship of the underwater vehicle; i is a non-negative integer.

3. The underwater vehicle obstacle avoidance method based on model predictive control and cost potential field as claimed in claim 2, characterized in that: In S2, when establishing the second optimal problem model, the following steps are included: S2.

1. The static cost function at time step k is expressed by (Equation 2): In (Formula 2), cost s (·) represents the static obstacle avoidance function; k represents the time step; (x, y, z) represents the position coordinates of the underwater vehicle in a fixed coordinate system; (x s ,y s ,z s ) represents the position coordinates of the static obstacle in the fixed coordinate system; σ1 represents the standard deviation; c1 is a constant; S2.

2. The dynamic cost function at time step k is expressed by (Equation 3): In (Formula 3), cost d (·) represents the dynamic obstacle avoidance function; k represents the time step; (x, y, z) represents the position coordinates of the underwater vehicle in a fixed coordinate system; (x d ,y d ,z d ) represents the position coordinates of the dynamic obstacle in the fixed coordinate system; σ2 represents the standard deviation; c2 is a constant; S2.

3. Introduce (Equation 2) and (Equation 3) into (Equation 1) to obtain the second optimal problem model, which is expressed by (Equation 4): In (Equation 4), J2(x(k),u(k)) represents the optimal function; x(·) represents the input state of the underwater vehicle; u(·) represents the control input of the underwater vehicle; k represents the time step; d(·) represents the environmental interference to the underwater vehicle; y(·) represents the output state of the underwater vehicle; N P represents the prediction time domain; U represents the control input constraint; C represents the state output matrix; f(·) is a nonlinear function, representing the six-degree-of-freedom state space model of the input-output relationship of the underwater vehicle; i is a non-negative integer.

4. The underwater vehicle obstacle avoidance method based on model predictive control and cost potential field as claimed in claim 3, characterized in that: The expression of the optimal function J2(x(k),u(k)) is: In the above formula, x(·) represents the input state of the underwater vehicle; u(·) represents the control input of the underwater vehicle; 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 change weight matrix; N represents the prediction time domain of static obstacles and dynamic obstacles; N s Indicates the number of static obstacles; r s Indicates the radius of static obstacles; N d Indicates the number of dynamic obstacles; r d Indicates the radius of dynamic obstacles; i is a non-negative integer; j is a non-negative integer.

5. The underwater vehicle obstacle avoidance method based on model predictive control and cost potential field as claimed in claim 1, characterized in that: The global expected trajectory includes the expected position and heading angle at each time point in the future.

6. The underwater vehicle obstacle avoidance method based on model predictive control and cost potential field as claimed in claim 1, characterized in that: The detection components include radar, sonar and underwater cameras.

7. The underwater vehicle obstacle avoidance method based on model predictive control and cost potential field as claimed in claim 1, characterized in that: In S4., the type of the obstacle is determined based on the position information and speed information of the obstacle, and the obstacle is simplified into a sphere with the center as the sphere center and the farthest point from the center as the radius.

8. The underwater vehicle obstacle avoidance method based on model predictive control and cost potential field as claimed in claim 7, characterized in that: The types of obstacles include static obstacles and dynamic obstacles.

9. The underwater vehicle obstacle avoidance method based on model predictive control and cost potential field as claimed in claim 1, characterized in that: In S4., according to the second optimal problem model, the underwater vehicle navigates in the direction of the obstacle's low cost potential field during navigation, thereby achieving obstacle avoidance.

Images