Model prediction motion planning method and system for controlling obstacle constraint of underwater vehicle

Abstract

The invention provides an underwater vehicle control obstacle constraint model prediction motion planning method and system, and relates to the technical field of safe collision avoidance motion planning of an underwater vehicle in a narrow water channel. Aiming at a narrow water channel complex ocean current environment and a scene that multiple underwater vehicles cooperate, a neural reachability field is generated through a deep neural network to realize global path guidance, a dynamic obstacle avoidance map is constructed in combination with a nonlinear control barrier function (CBF), and a corrector consistent with an international maritime collision avoidance rule is embedded to ensure safe intersection and rule adherence. Compared with the prior art, the safety distance is increased by 2-3 times, the cooperative minimum distance between multiple underwater vehicles reaches 0.82 m, the real-time performance meets the 0.2-second-level requirement, and the motion planning capacity and compliance of the underwater vehicles in the dynamic environment are remarkably improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of safe collision avoidance motion planning of unmanned underwater vehicles in narrow waterways, and in particular to a model predictive motion planning method and system for underwater vehicles with control obstacle constraints. BACKGROUND

[0002] In complex underwater environments such as narrow waterways, unmanned underwater vehicles face great technical challenges when operating. Drift caused by ocean currents can easily cause underwater vehicles to deviate from the planned route, increasing the risk of collision with obstacles. Traditional safety methods rely on fixed safety margins, which cannot adapt to dynamic ocean currents, and lack comprehensive consideration of maritime navigation priority rules such as the International Regulations for Preventing Collisions at Sea (COLREGs). In particular, in the scenario of multiple underwater vehicles cooperating or coexisting with manned submarines, it is difficult to make collision avoidance decisions that comply with the rules, and the risk of collision is significantly increased.

[0003] Existing motion planning techniques for unmanned underwater vehicles mostly use simple obstacle avoidance algorithms and static safety distance settings, which may be effective in static or low flow rate environments, but have obvious limitations in high flow rate or dynamic waterways. For example, traditional strategies such as artificial potential field method and linear fast marching method cannot accurately predict the impact of ocean currents on the trajectory, resulting in insufficient safety distance or low navigation efficiency. At the same time, these methods do not fully consider the interaction of multiple underwater vehicles in cooperative operation, and in narrow waterways and strong ocean current conditions, it is difficult to meet the requirements of real-time, safety and navigation efficiency at the same time.

[0004] To solve the above problems, the present application provides a model predictive motion planning method and system for underwater vehicles with control obstacle constraints. A neural reachability field that conforms to the direction of ocean currents is generated by a deep neural network to provide global path guidance for underwater vehicles; a dynamic obstacle avoidance control map is constructed by combining a nonlinear control barrier function (CBF) to effectively improve safety and adaptability in complex environments; and a control barrier field modifier consistent with COLREGs is embedded to realize safe intersection and rule compliance in the cooperative operation of multiple underwater vehicles, significantly enhancing the motion planning capability of underwater vehicles in narrow waterway ocean current environments. SUMMARY

[0005] To solve the above problems, the present application provides a model predictive motion planning method and system for underwater vehicles with control obstacle constraints. A neural reachability field that conforms to the direction of ocean currents is generated by a deep neural network to provide global path guidance for underwater vehicles; a dynamic obstacle avoidance control map is constructed by combining a nonlinear control barrier function (CBF) to effectively improve safety and adaptability in complex environments; and a control barrier field modifier consistent with COLREGs is embedded to realize safe intersection and rule compliance in the cooperative operation of multiple underwater vehicles, significantly enhancing the motion planning capability of underwater vehicles in narrow waterway ocean current environments.

[0006] To achieve the above purposes, the technical solution adopted by the present application is: A model predictive motion planning method for underwater vehicle control obstacle constraint, the method comprises: Constructing kinematic model and dynamic model of underwater vehicle; Based on the kinematic model and dynamic model, a model predictive control (MPC) framework for motion planning is constructed, expressed as follows: The construction of model predictive control (MPC) can optimize the speed command, meet the obstacle avoidance constraint condition, and use neural reachability field for global guidance; it can minimize the arrival time while meeting the dynamic constraint motion sequence; In the formula, J J represents the objective function, x represents the pose of the underwater vehicle during operation, f represents , u represents the control input speed of the first step, u represents the speed set, and and represent the prediction time domain and the control time domain respectively, and is the neural reachability value at point , ); is used to weigh the guidance cost, and penalizes the deviation from 0.01; is set to 1000, which makes it a priority to meet the control Lyapunov function inequality when optimizing the objective function; when the control Lyapunov function constraint conflicts, only then will it be activated; is set to 0.01, which is used to control the expected drop of the reachability value; the function is a nonlinear control obstacle function encoding for obstacle avoidance safety; the relaxation amount (0, 1) softens the nonlinear control obstacle function constraint to maintain feasibility, while penalizes not equal to 0.01, and limits <1.

[0007] In another aspect, the present application also provides a model predictive motion planning system for underwater vehicle control obstacle constraint, which comprises a memory for storing computer program instructions and a processor for executing the program instructions, wherein when the computer program instructions are executed by the processor, the system is triggered to perform the above-mentioned model predictive motion planning method for underwater vehicle control obstacle constraint.

[0008] Compared with the prior art, the present application has the following beneficial effects: 1. Dynamic environment adaptability is significantly enhanced: the traditional method relies on fixed safety margin and cannot adapt to the dynamic changes of ocean currents, resulting in insufficient safety distance or low navigation efficiency. The present application generates a neural reachability field through a deep neural network, quantifies the influence of ocean current direction and obstacle space in real time, so that the underwater vehicle can conform to the ocean current direction to plan the optimal path. Experiments show that in a cross-flow environment, the safety distance of this method is 2-3 times higher than that of the traditional method (such as 0.85 meters vs. 0.24 meters), and the navigation time is shortened.

[0009] 2. Multi-underwater vehicle cooperative safety and rule compliance: the prior art does not fully consider the multi-body interaction rules, and is prone to violate the COLREGs rules when cooperating or coexisting with manned submarines, resulting in a sharp increase in collision risk. The present application embeds a control obstacle field modifier consistent with COLREGs, quantifies the obstacle avoidance priority through encounter classification, direction penalty and dynamic emergency weight mechanism. For example, in the scenario of eight underwater vehicles cooperating in navigation, this method can maintain a minimum distance of 0.82 meters, which is twice that of the traditional method (0.40 meters), and the trajectory meets the requirements of the rules such as right-hand intersection and overtaking other underwater vehicles.

[0010] 3. Real-time and computational efficiency optimization: the present application uses a model predictive control (MPC) framework, combines parallel computing of neural reachability field and CBF, and updates control instructions every 0.01 seconds. In the Gazebo simulator, this method completes path re-planning within 0.2 seconds, meets the real-time requirements, and the path quality does not decrease.

[0011] 4. Global and local planning unified in complex scenarios: the prior art separates the processing of global path and local obstacle avoidance, resulting in conflicts or efficiency loss. The present application realizes the unification of global optimization and local safety through the hierarchical architecture of neural reachability field global guidance and CBF local obstacle avoidance. In a narrow waterway full of reefs, the safety distance of this method is 1.25 meters, which is nearly twice that of the control obstacle function of the artificial potential field method (0.65 meters), and the navigation time is shortened.

[0012] 5. Anti-interference ability and robustness improvement: Traditional methods are sensitive to sensor noise or sudden current, which can easily lead to planning failure. The CBF field of the present application softens the constraint by relaxing the amount, maintains feasibility while punishing deviation. In the simulator test, even if there is 10% noise in the sonar data, this method can still maintain a minimum safety distance of 0.86 meters, and no collision occurs.

[0013] Other features and advantages of the embodiments of the present application will be described in detail in the following detailed description. BRIEF DESCRIPTION OF DRAWINGS

[0014] Figure 1 is a schematic diagram of the underwater vehicle fixed coordinate system and body coordinate system according to the present application; Figure 2 is a schematic diagram of the motion planner framework of the unmanned underwater vehicle with safety criticality according to the present application; Figure 3 is a schematic diagram of the dynamic neural network model according to the present application, wherein (a) is a schematic diagram of a two-dimensional underwater environment, and (b) is a corresponding neural network diagram; Figure 4 is a schematic diagram of the obstacle-oriented control barrier function generation according to the present application, wherein (a) is a schematic diagram of the obstacle source unit, (b) is a schematic diagram of the obstacle neural activity field, and (c) is a schematic diagram of the normalized and masked CBF field; Figure 5 is a schematic diagram of the dynamic unmanned underwater vehicle encounter penalty according to the present application, wherein (a) is a schematic diagram of the encounter classification, and (b) is a schematic diagram of the corresponding CBF field in the case of crossing right lane; Figure 6 is a schematic diagram of the comparison of global motion planning algorithms in narrow waterway environment according to the present application; Figure 7 is a schematic diagram of the CBF field of different algorithms according to the present application, wherein (a) is a schematic diagram of the APF-CBF, (b) is a schematic diagram of the LFM-CBF, and (c) is a schematic diagram of the method proposed in the present application; Figure 8 is a schematic diagram of the artificial intelligence local motion planning result in more turbulent sea conditions according to the present application; Figure 9 is a schematic diagram of the motion planning of multiple unmanned underwater vehicles using the proposed algorithm according to the present application, wherein (a) is a schematic diagram of the initial step, (b) is a schematic diagram of the first collision avoidance, (c) is a schematic diagram of the second collision avoidance, and (d) is a schematic diagram of the last step; Figure 10are trajectory generation example plots for multiple unmanned underwater submersibles based on the algorithm proposed by Lewis et al. (2020), where (a) is an initial step schematic, (b) is an intermediate step schematic, (c) is a near collision scenario schematic, and (d) is a final step schematic; Figure 11 are motion planning schematics for multiple unmanned underwater submersibles in an obstacle environment according to the present invention, where (a) is an initial step schematic, (b) is a collision avoidance schematic while traversing to the right, (c) is a collision avoidance schematic while head-on collision, and (d) is a final result schematic; Figure 12 are motion simulation schematics in an underwater unmanned submersible simulator according to the present invention, where (a) is a simulation environment schematic, (b) is a sonar perception schematic, (c) is a final trajectory schematic, and (d) is a control input curve schematic. DETAILED DESCRIPTION

[0015] In order to better understand the technical solutions in the present application, the technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the accompanying drawings in the embodiments of the present application, so that the purpose, characteristics and advantages of the present application can be better understood. It should be understood that the embodiments shown in the drawings are not a limitation on the scope of the present application, but only to illustrate the essential spirit of the technical solutions of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor should belong to the scope of protection of the present application.

[0016] Unless the context clearly requires otherwise, throughout the description and the claims, the words "comprise", "comprising", and the like are to be construed in an inclusive sense as opposed to an exclusive or exhaustive sense; that is to say, in the sense of "including, but not limited to".

[0017] Reference throughout this specification to "one embodiment" or "an embodiment" means that a particular feature, structure, or characteristic described in connection with the embodiment is included in at least one embodiment. Thus, the appearances of the phrase "in one embodiment" or "in an embodiment" in various places throughout this specification are not necessarily all referring to the same embodiment. Furthermore, the particular features, structures, or characteristics can be combined in any suitable manner in one or more embodiments.

[0018] As used in this specification and the appended claims, the singular forms "a", "an" and "the" include plural referents unless the content clearly dictates otherwise. It should be noted that the term "comprising" or "comprises" as used in this specification and the appended claims is not meant to exclude the presence of elements other than those listed in the claims.

[0019] In the following description, in order to clearly demonstrate the structure and working method of the present invention, a number of directional terms will be used. However, terms such as "front", "back", "left", "right", "outside", "inside", "outward", "inward", "up", and "down" should be understood as convenient terms and not as limiting terms.

[0020] The implementation details of the embodiments of the present invention will be described in detail below with reference to the accompanying drawings. The following content is only for the convenience of understanding the implementation details and is not necessary for implementing this solution.

[0021] Addressing the multiple challenges faced by existing unmanned underwater vehicle (UUV) motion planning technologies, conventional methods exhibit significant limitations in handling narrow waterways, complex ocean currents, and collaborative operations involving multiple UUVs. To overcome these technical bottlenecks, this invention innovatively proposes a systematic solution for UUV motion planning. The core objectives and constraints of UUV motion planning will be outlined below, followed by a complete description of the technical solution of this invention.

[0022] The motion planning problem of unmanned underwater vehicles can be described by formula (1): in, J Describe the objective function. This indicates the position and attitude of the underwater vehicle during operation. express This indicates the speed of the underwater vehicle during operation. Represents the set of velocities. Indicates an obstacle. Indicates the initial pose. Indicates the target pose. This represents the neighborhood of the target pose plus or minus a small value, where... It represents a tiny value.

[0023] Equation (1) shows that the motion planning objective of the unmanned underwater vehicle is to start from the initial pose. Reach the target pose In the process, minimize objective functions such as distance, time consumption, and energy consumption. J. And limited to the kinematic constraints of underwater vehicle (kinematic constraints), the operating speed of underwater vehicle is limited to the maximum thrust provided by the speed of the propeller (speed set constraints). Finally, it is crucial that the underwater vehicle cannot collide with obstacles, that is, the intersection of the pose of the underwater vehicle during operation and the obstacle is empty set. Therefore, the present application proposes a model predictive safe obstacle avoidance motion planning method based on control barrier function, as follows: First, the kinematics and dynamics of the unmanned underwater vehicle are modeled. In the task of underwater motion planning of unmanned underwater vehicle, the kinematics model is as follows: The kinematics model of the unmanned underwater vehicle describes the transformation between the fixed coordinate system (O- ) and the body coordinate system (0- ), as shown in Figure 1 , which is given by equations (2) and (3). In equation (2), represents the position and heading of the underwater vehicle in the fixed coordinate system, represents the heading angle, and , where the bold is a vector, is a scalar, representing the forward, lateral linear velocity and yaw angle rate in the body coordinate system. This coordinate transformation is also applied to the sonar-based obstacle detection. Sonar data provides obstacle position information relative to the body coordinate system, which is then converted to coordinates in the fixed coordinate system by the conversion matrix (3).

[0024] Next, the dynamics model of the unmanned underwater vehicle in the ocean current is given by equations (4) and (5): where is the mass of the underwater vehicle; are the added masses in x , y and z directions of the six degrees of freedom, respectively; and 、 、 , and their quadratic terms 、 、 ​​where denotes the hydrodynamic damping coefficient; is the control input vector containing the forces and moments; denotes the ocean current velocity in the body-fixed frame; I z is the moment of inertia about the z-axis; u r is the longitudinal velocity of the vehicle through the water; v r is the lateral velocity of the vehicle through the water.

[0025] After completing the kinematic and dynamic modeling of the UUV, the construction phase of the safety-critical motion planning framework is entered, as shown in Figure 2 Based on the information obtained from the previous kinematic and dynamic modeling, the reachable regions of sonar perception, target guidance, and obstacle guidance are integrated, and a high-efficiency and safe UUV model predictive motion planning method is formed through the neural field calculation method, the construction of the nonlinear control barrier function CBF (Control Barrier Function), and the application of the model predictive control MPC (Model Predictive Control) framework. This method not only conforms to the complex and variable ocean current environment, but also ensures the safe collision avoidance of the UUV in narrow waterways, while meeting the multiple demands of real-time performance, safety, and navigation efficiency.

[0026] The model predictive control MPC framework for motion planning is as follows: The construction method of the model predictive control MPC can optimize the velocity command, meet the obstacle avoidance constraint condition, and use the neural reachable field for global guidance. It can minimize the arrival time while meeting the dynamic constraint motion sequence, as shown in formula (6).

[0027] In formula (6), denotes the control input at the th step, and represent the prediction time domain and the control time domain, respectively, and is the neural reachability value at the point , . is used to weigh the guidance cost, and penalizes the deviation from 0.01. is set to 1000, which makes the optimization objective function prioritize satisfying the control Lyapunov function inequality. When the control Lyapunov function constraint conflicts, are activated. is set to 0.01, which is used to control the expected drop of reachability value. The function is a nonlinear control barrier function encoding for obstacle avoidance safety. The slack variable (0, 1) softens the nonlinear control barrier function constraint to maintain feasibility while penalizing when it is not equal to 0.01, and limits <1.

[0028] In the model predictive control (MPC) framework, i.e., equation (6), the implementation of the optimization objective function relies on accurate perception of environmental information and real-time processing of dynamic constraints. To meet this demand, the overall framework includes three core steps: first, generate a neural reachability field to quantify the spatial influence of ocean currents and obstacles through (1); then, convert it into a globally guided path for the target through (2); finally, embed dynamic obstacle avoidance and rule constraints by constructing a nonlinear control barrier function (CBF) through (3). These three steps progress layer by layer, collectively supporting the balance between real-time performance, safety, and navigation efficiency in equation (6).

[0029] (1) Generating a Neural Reachability Field To consider environmental factors such as ocean currents, a deep neural network (DNN) model is used to generate a neural reachability field to guide the motion of underwater vehicles. As Figure 3 shown, a two-dimensional environment is discretized into a grid, where each cell corresponds to a neuron. Neural activity propagates through the grid from the target point, naturally tending to follow paths consistent with favorable ocean current directions. The deep neural network model biases information propagation towards directions consistent with favorable ocean current directions. The activity of each neuron is in the interval [0, 1], where the target neuron is 1 and the obstacle neuron is 0. The neural activity of other neurons gradually weakens from the target neuron outward, being higher than 0 but lower than 1.

[0030] (2) Neural Reachability Field for Target Guidance For the generation of the target region, the neuron at the target position is set as the external activation , while the detected obstacles are set as . After convergence, the neuron activity matrix is obtained, and the neural reachability value ( , ) at position , ) is calculated as: where =1 / ( ) is the neural reachability field of the target position, representing the reachable range of the neural network. interp2 is used to query the bilinear interpolation of continuous two-dimensional coordinates. Smaller neural reachability values indicate shorter estimated times to reach the target position, and thus the resulting neural reachability field as a global motion guide map, is directly embedded into the MPC cost function (6).

[0031] (3) Obstacle avoidance control barrier function based on neural reachability field The control barrier function CBF provides a mechanism to implement safety constraints in safety-critical underwater vehicle navigation. The safety set is defined as: If the function is continuously differentiable and satisfies: where is a function, then the function is an effective nonlinear control barrier function CBF that can guarantee the forward invariance of the safety set .

[0032] In the present invention, this concept is extended by embedding the spatial and environmental dynamics into the nonlinear control barrier function CBF expression (through the neural reachability field). Similar to the goal-oriented reachability field, the obstacle-oriented reachability field is computed using the same deep neural network (DNN) propagation model, but with obstacle cells as the source of activation.

[0033] Construction of the nonlinear control barrier function is divided into three stages as shown in (a)-(c) in Figure 4 .

[0034] (3-1) Static obstacle field based on neural reachability field By having the deep neural network propagation algorithm operate with obstacle cells as the source, an obstacle neural activity field is generated. As shown in (a) and (b) in Figure 4 , the deep neural network generates a stable obstacle activity field .

[0035] (3-2) Punishment integral integration of dynamic underwater vehicle ​​In (3-1), a basic safe obstacle avoidance environment is constructed for underwater vehicles by generating a static obstacle field of neural reachability field. However, in the dynamic waterway and multi-vehicle cooperative operation scene, only relying on static information cannot meet the real-time safety requirements. Therefore, it is necessary to further integrate the interaction rules and risk assessment mechanism of dynamic underwater vehicles, through the synergistic effect of encounter classification, direction penalty and dynamic emergency weight, to quantify the behavior constraints and collision priority in multi-body obstacle avoidance, and to provide a dynamically adjusted penalty integral basis for subsequent construction of nonlinear control barrier function (CBF).

[0036] Specifically, when multiple unmanned underwater vehicles are active in the same area, other unmanned underwater vehicles are considered as dynamic obstacles, and additional penalty areas (penalty functions) are added in the nonlinear control barrier function (CBF) map to enforce cooperative navigation behavior according to the COLREGs.

[0037] Encounter classification, direction penalty and dynamic emergency weight are the three core components of dynamic underwater vehicle penalty integral integration. Through synergistic effect, the multi-vehicle interaction rules and dynamic risk are quantified as penalty terms in the nonlinear control barrier function (CBF), thereby ensuring the safety and compliance of obstacle avoidance control. Specifically as follows: Encounter classification: based on relative bearing (measured from the heading of the own unmanned underwater vehicle, ranging from -180° to 180°), there are four typical encounter situations as shown in (a) of Figure 5 The four encounter situations are defined as: Right intersection: ∈[-112.5°, -15°], Left intersection: ∈[15°, 112.5°], Head-on encounter: ∈[-15°, 15°], Overtaking other vehicles: other cases.

[0038] In the calculation of dynamic unmanned underwater vehicle intersection point obstacle reachability field, each cell on the right side of the dynamic underwater unmanned vehicle is also marked as blocked, which prompts other unmanned underwater vehicles to adjust the heading to the left side of the own unmanned underwater vehicle to meet the COLREGs requirement of avoiding right intersection.

[0039] Direction penalty: for each grid cell , , let: : position of the target (other dynamic unmanned underwater vehicle), : Target heading (radians), = : Vector from target to this cell, = : Unit vector of target heading.

[0040] Vector Relative angle between vector and vector is: where, is a small value; The model of its angular influence is a Gaussian decay function: where, controls the degree of spread of the angle (e.g., can be set to ±30°, corresponding to = π / 6).

[0041] Then asymmetrically adjust according to the cross product sign: where is a bias factor (e.g., = 0.4) that prioritizes passing behind the target UUV in right-hand junctions (e.g., as shown in (b) of Figure 5 to satisfy the COLREGs rule that prohibits crossing in front of other UUVs from the left side.

[0042] Dynamic urgency weight: The urgency of evasion is quantified by the time-to-closest-point-of-approach (TCPA) and the distance-to-closest-point-of-approach (DCPA): where, and are the safety thresholds for TCPA and DCPA, respectively.

[0043] Finally, the total dynamic penalty applied at grid cell is: where, is a control distance decay.

[0044] (3-3) Nonlinear conversion to CBF map After completing the encounter classification, directional penalty, and dynamic urgency weight analysis of dynamic underwater vehicles, the behavioral compliance and collision risk in multi-vehicle interaction scenarios have been quantified. To transform these quantified indicators into real-time obstacle avoidance control commands, it is necessary to further construct a nonlinear control obstacle function (CBF) to map the static obstacle field, dynamic obstacle field, and comprehensive penalty value into a continuous CBF map. This will enable explicit expression and dynamic optimization of safety constraints within the model predictive control (MPC) framework. Specifically: Neural activity, along with total penalty, is first converted into an obstacle-to-time measure: in, The neural reachable field represents the obstacle; Normalize it to the range [0, 1]: Create a binary mask to isolate cells within the barrier edge: Cells outside the mask are saturated to the maximum value within the edge: Convert the static and dynamic obstacle fields into the final CBF map: Controlling the gradient of the CBF field. Continuous CBF in The value at the point is obtained using bilinear interpolation: This embodiment details the proposed underwater vehicle (UV) obstacle constraint model predictive motion planning method. First, a kinematic and dynamic model of the UV is constructed. Then, a neural reachability field conforming to the ocean current direction is generated through a deep neural network, providing dynamic guidance for global path planning. Simultaneously, a dynamic obstacle avoidance control map is constructed by combining a nonlinear control obstacle function (CBF), effectively integrating static and dynamic obstacle information. Furthermore, through encounter classification, direction penalty, and dynamic urgency weighting mechanisms, interaction rules and risk quantification are achieved for multi-UV cooperative operations. This method significantly improves the safety margin of UV in narrow waterways, increasing the safe distance several times compared to traditional methods while maintaining relatively high navigation speeds. In multi-UV cooperative navigation scenarios, this method effectively avoids collisions, ensuring trajectories comply with the International Maritime Collision Prevention Regulations (COLREGs), with a significantly increased minimum distance compared to other methods, demonstrating its real-time performance, safety, and efficiency in complex ocean current environments.

[0045] Example 2 This example performs performance evaluation of global and local motion planning in an environment with narrow waterways and ocean current effects. The algorithms compared are tested under the same conditions. Subsequently, a multi-unmanned underwater vehicle scenario is investigated to evaluate the performance of the method in cooperative navigation. To match the experimental setup with real-world operating conditions, the simulation is performed in an unmanned underwater vehicle simulator environment with dense obstacles. The unmanned underwater vehicle model uses the hydrodynamic parameters of the “Rex ROV” vehicle.

[0046] For the deep neural network (DNN), the parameter settings are = 0.05 and = 100.

[0047] The MPC parameters are as follows: sampling time step 0.01 seconds, control period = 1, prediction period = 10, maximum linear velocity 1.5 meters / second, maximum turning rate 0.5 radian / second, linear velocity increment range [-0.2, 0.15] meters / second, angular velocity increment range [-0.15, 0.15] radian / second, = 1, and .

[0048] For the nonlinear CBF, the parameters are = 5, = 0.01. Safety time interval = 6, safety distance = 10.

[0049] The simulation of this example is performed on a personal computer equipped with an Intel® Core™ i7-10750H processor (clock frequency 2.60 GHz), 32 GB of memory, and a Windows 10 system. All implementations of the model predictive control objective function optimization solution are performed in MATLAB using the fmincon-sqp solver.

[0050] The algorithms compared include the following: Distance constraint control barrier function (DC-CBF): the minimum distance to each discrete obstacle element is computed, where denotes the center of each element.

[0051] Artificial potential field control barrier function (APF-CBF): the artificial potential field formula in (22) is used, where is the minimum distance to a single obstacle element, = 8, and = 0.5.

[0052] Linear Fast Marching Control Barrier Function (LFM-CBF): The 2D interpolation of is obtained by using the Fast Marching method.

[0053] Pure Distance Constraint: Model Predictive Control without any Control Barrier Function constraint.

[0054] Distance Objective Function: The objective function is set as the Fast Marching Time Field (Euclidean distance to the goal position).

[0055] (1) Global Motion Planning in Narrow Waterway The first scenario sets up a 100x100m grid environment with a narrow waterway. The UUV starts from the point (20, 50) and the goal is to reach the point (96, 96). There is a side current in the waterway with a speed of half the maximum speed of the UUV (as shown in Figure 6 ).

[0056] Table 1 Performance comparison of different algorithms in global motion planning Table 1 summarizes the performance indicators. The model that only takes distance as the goal achieves the shortest travel distance, but takes slightly longer time than the proposed model. Without any nonlinear Control Barrier Function (CBF) constraint, the pure distance constraint can achieve the fastest speed (82.73 seconds), but at the same time the safety distance in the side current area is the smallest (0.24 meters), which brings great collision risk. The artificial potential field control barrier function and the distance constraint control barrier function generate nonlinear control barrier function (CBF) fields with weak gradients, and the safety improvement is minimal. The linear fast marching control barrier function generates long-range repulsive forces, but has significantly longer travel time (minimum travel speed); in addition, since there is no current perception capability, it can only achieve a safety distance of 0.61 meters. In contrast, the proposed method can achieve the maximum safety distance (0.85 meters) while maintaining a relatively fast travel speed.

[0057] As shown in Figure 7As shown, the gradient of the artificial potential field control barrier function is relatively flat, while the linear rapid-exploring control barrier function has a wider but weaker influence. The proposed method generates a strong local gradient near the obstacle and increases the repulsive force when the current pushes the AUV towards dangerous regions, thus reducing the nonlinear control barrier function (CBF) value of these regions. The dual effects of the current-aware objective function and the current-aware nonlinear control barrier function (CBF) shape a safe and efficient trajectory.

[0058] (2) Local motion planning with stronger oceanic capability The second scenario simulates local motion planning in a narrow channel with rocks, where the AUV needs to navigate from the point (15, 2) to the point (97, 96) under the influence of a strong current of 1 m / s, as shown in Figure 8

[0059] Table 2 Performance comparison of different algorithms in local motion planning Table 2 shows the results. The proposed nonlinear control barrier function (CBF) again shows a clear local gradient near the obstacle, and the safety distance (1.25 m) is significantly higher than other methods. The artificial potential field control barrier function and the linear rapid-exploring control barrier function complete the planning task, but due to the neglect of the influence of the current, the safety distance is less than 0.65 m, which poses a high risk of collision when a sudden lateral current surge occurs. The distance-constrained control barrier function performs similarly, and its improvement is minimal compared to the pure distance constraint.

[0060] The distance-to-goal MPC shortens the trajectory length (129.37 m), but causes the AUV to pass through more lateral current regions, increasing the control difficulty and reducing the safety margin. Although its travel time is close to the proposed method in this local situation, the risk of shorter safety distance is still high due to the more balanced use of current and detour path.

[0061] Overall, all benchmark nonlinear control barrier functions (CBF) fail to consider the influence of the current, so they provide limited protection in lateral current regions. The current-aware nonlinear control barrier function (CBF) of the proposed method actively addresses this issue. By changing the path to avoid high-risk current channels, it achieves safer navigation with only a moderate increase in distance and time.

[0062] (3) Simulation in multi-AUV cooperative navigation scenarios ​To evaluate the cooperative performance, the motion planning of eight AUVs sailing in a 100 m x 100 m environment simultaneously was tested, with both static obstacles and no obstacles. The initial and target positions were assigned in a cyclic manner: (5, 5)→(95, 95), (50, 5)→(50, 95), (95, 5)→(5, 95), (95, 50)→(5, 50), (95, 95)→(5, 5), (50, 95)→(50, 5), (5, 95)→(95, 50), and (5, 50)→(95, 5). In all cases, the sonar detection range of the AUVs was represented by a sector.

[0063] As Figure 9 shown, the proposed method can achieve efficient and collision-free cooperative navigation in dense AUV scenarios. In (b) and (c) of Figure 9 , the trajectories obviously comply with the rules of encounter in the International Regulations for Preventing Collisions at Sea, which proves that the navigation rules are naturally embedded in the planning process. In contrast, Figure 10 shows that the distributed model predictive control (DMPC) method proposed by Lewis et al. cannot ensure rule compliance and even leads to near-collision events. In terms of quantification, the proposed method can maintain a minimum distance of 0.82 m between AUVs, while the method of Lewis et al. can only maintain a distance of 0.40 m.

[0064] Finally, Figure 11 shows that the dynamic penalty mechanism proposed in the present application can be seamlessly embedded into the deep neural network reachability region, forming a rich guidance landscape that enables multiple AUVs to safely navigate cooperatively in an environment with both obstacles and ocean currents. In this scenario, the current speed is set to one quarter of the maximum speed of the AUV. As shown in (b) and (c) of Figure 11 , the planned trajectories comply with the International Regulations for Preventing Collisions at Sea: the AUVs overtake from the stern, and when meeting head-on, both ships turn to the right. The minimum distance between the two ships is maintained at 1.06 m, further demonstrating the safety performance of the proposed framework.

[0065] Example Three This example is a simulation test in an AUV simulator. This example is simulated on a computer with an Intel® Core™ i7-10750H processor (clock frequency 2.60 GHz), 32 GB of memory, based on the Ubuntu 18.04 operating system, and using ROS for simulation.

[0066] To further verify the real-time applicability under real hydrodynamic and sensory conditions, the proposed method was tested in a Gazebo-based unmanned underwater vehicle simulator using the "RexROV" model. As shown in (a) left side of FIG. 10, Figure 12 The simulation environment contains a 50m x 50m workspace filled with dense underwater piers. The unmanned underwater vehicle starts from (5, 3) and the task is to reach (46, 42) using only the local perception data of the onboard sonar sensor Figure 12 The proposed control barrier function enables the unmanned underwater vehicle to slow down and reorient before entering the restricted area, ensuring safe passage Figure 12 (c) of FIG. 10. Figure 12 (d) of FIG. 10 shows the curves of forward speed and yaw rate during navigation. The total travel distance is 80.04 meters, the time taken is 189.1 seconds, and the minimum distance to obstacles is 0.86 meters.

[0067] An important practical observation is that in Gazebo, the "RexROV" model will not respond if the publishing interval of the velocity command exceeds 0.2 seconds. Throughout the experiment, the proposed method maintained real-time performance, meeting this requirement without a decrease in path quality. These results confirm that the method is effective. It is only applicable to offline simulation, but can also be easily applied to real-world unmanned underwater vehicle platforms equipped with internal sensors and computing capabilities.

[0068] The present application also provides an underwater vehicle control obstacle-constrained model predictive motion planning system, which comprises a memory for storing computer program instructions and a processor for executing the program instructions, wherein when the computer program instructions are executed by the processor, the system is triggered to perform the above-mentioned underwater vehicle control obstacle-constrained model predictive motion planning method.

[0069] The application proposes a model predictive motion planning method and system based on a novel control barrier function, which combines two artificial intelligence components and a rolling horizon controller. First, a reachable time neural field calculation method affected by water flow is proposed to conform to the global guidance of the sea current direction, and the control barrier field required for safe obstacle avoidance of obstacles in the sea current environment is generated by applying the reachable time neural field calculation method. In addition, a control barrier field modifier consistent with the International Regulations for Preventing Collisions at Sea is embedded to shape the safe intersection when multiple unmanned underwater vehicles meet. In engineering applications, the barrier field is embedded in a discrete model predictive controller through softened control barrier constraints. Experiments in narrow waterways and high-fidelity underwater simulations show that the application increases the safety margin in the cross-flow area, the cooperative operation meets the requirements of the International Regulations for Preventing Collisions at Sea, and it is real-time feasible.

[0070] Although the application has been described in detail with reference to the preferred embodiments, the application is not limited to the preferred embodiments. Those skilled in the art can make various equivalent modifications or replacements to the embodiments of the application without departing from the spirit and essence of the application, and these modifications or replacements should be within the scope of the application or any skilled person in the art can easily think of changes or replacements within the technical scope disclosed by the application, which should be covered by the protection scope of the application. Therefore, the protection scope of the application should be subject to the protection scope of the claims.

Claims

1. A model-predictive motion planning method for controlling obstacle constraints in underwater vehicles, characterized in that, The method includes: Construct kinematic and dynamic models for underwater vehicles; Based on the kinematic and dynamic models, a Model Predictive Control (MPC) framework for motion planning is constructed, as shown in the following expression: The construction of Model Predictive Control (MPC) can simultaneously optimize velocity commands, satisfy obstacle avoidance constraints, and utilize neural reachability fields for global guidance; it can minimize arrival time while satisfying dynamic constraints in motion sequences. In the formula, J Describe the objective function. This indicates the position and attitude of the underwater vehicle during operation. express , Indicates the first Step control input speed, Represents the set of velocities. and These represent the prediction time domain and the control time domain, respectively. At point ( , The neural reachability value at point ( ); Used to weigh the costs of initiation, Then regarding the deviation The penalty starts at 0.01; Setting it to 1000 prioritizes satisfying the control Lyapunov function inequality when optimizing the objective function; when conflicts arise in the control Lyapunov function constraints... It will then be activated; Setting it to 0.01 controls the expected decrease in reachability values; function It is the encoding of obstacle avoidance safety by the nonlinear control obstacle function; relaxation amount (0,1) softens the nonlinear control barrier function constraint to maintain feasibility, while also... A penalty is imposed and restrictions are applied when the value is not equal to 0.

01. <1.

2. The method according to claim 1, characterized in that, The kinematic model expression is as follows: The kinematic model of an underwater vehicle describes the transformation between a fixed coordinate system and a volume coordinate system; in, = This indicates the position and heading of the underwater vehicle in a fixed coordinate system. Indicates the heading angle, while = bold For vectors, Let be scalars representing the forward and lateral linear velocities and yaw rates in the body coordinate system; this coordinate transformation is also applied to sonar-based obstacle detection; sonar data provides obstacle position information relative to the body coordinate system, which is then transformed using a transformation matrix. Convert it to coordinates in a fixed coordinate system.

3. The method according to claim 2, characterized in that, The dynamic model expression is as follows: in, It refers to the mass of the underwater vehicle; They are respectively x , y Additional mass in the direction of degrees of freedom and z Additional rotational inertia in the direction; and 、 、 and their quadratic terms 、 、 This represents the hydrodynamic damping coefficient; It is a control input vector that includes thrust and torque; Represents the ocean current velocity in volume coordinates; yes z Moment of inertia along the axial direction; u r It refers to the longitudinal velocity of the water; v r It refers to the lateral velocity of the water.

4. The method according to claim 3, characterized in that, The proposed model predictive control (MPC) framework for motion planning comprises three steps: first, generating a neural accessibility field to quantify the spatial influence of ocean currents and obstacles; then, transforming the neural accessibility field into a global path guided by the target; and finally, constructing a nonlinear obstacle control function (CBF) to embed dynamic obstacle avoidance and rule constraints.

5. The method according to claim 4, characterized in that, The generated neural reachability field includes: A neural reachability field is generated using a deep neural network (DNN) model to guide the movement of the underwater vehicle. The two-dimensional environment is discretized into a grid, where each cell corresponds to a neuron. Neural activity propagates from the target point through the grid, naturally tending towards a path consistent with the direction of favorable ocean currents. The deep neural network model biases information propagation in the direction consistent with favorable ocean currents. The activity of each neuron is located in the interval [0, 1], where the target neuron is 1 and the obstacle neuron is 0. The neural activity of other neurons gradually decreases from the target neuron outwards, being higher than 0 but lower than 1.

6. The method according to claim 5, characterized in that, The process of transforming the neural reachability field into a target-guided global path includes: For the generation of the target region, the neurons at the target location are set as external activation variables. The detected obstacles are set as After convergence, the neuron activity matrix is ​​obtained. And obtain neural reachability values. ( ， ) at position ( ， The formula for calculating ) is: ( ， )= in =1 / ( The neural reachability field represents the reachability range of the neural network; interp2 is used to query the bilinear interpolation of continuous two-dimensional coordinates; a smaller neural reachability value indicates a shorter estimated time to reach the target location, thus resulting in a smaller neural reachability field. ( , As a global motion guidance graph, it is directly embedded into the MPC cost function.

7. The method according to claim 6, characterized in that, The construction of the nonlinear control barrier function (CBF) includes: Control Barrier Function (CBF) provides a mechanism for implementing safety constraints in underwater vehicle navigation, integrating safety sets. Defined as: function It is continuously differentiable and satisfies: ;in, It is one function, then function It is an efficient nonlinear control barrier function (CBF) that can guarantee a safe set. Forward invariance.

8. The method according to claim 7, characterized in that, The function The construction includes: 3-1, generating a static obstacle field based on neural reachability: by allowing a deep neural network propagation algorithm to operate with obstacle cells as the source, an obstacle neural activity field is generated; the deep neural network generates a stable obstacle activity field. ; 3-2. Penalty Integral Integration for Dynamic Underwater Vehicles: When multiple underwater vehicles operate in the same area, other underwater vehicles are treated as dynamic obstacles, and an additional penalty area is added to the nonlinear control obstacle function (CBF) map to implement cooperative navigation behavior, as follows: Encounter classification: based on relative azimuth angle Based on the course measurement of its own underwater vehicle, within the range of -180° to 180°, there are four encounter scenarios, defined as follows: Right intersection: ∈[-112.5°, -15°], left intersection: ∈[15°, 112.5°], A head-on encounter: ∈[-15°, 15°], beyond other underwater vehicles: other cases; When calculating the accessibility field of obstacles at the intersection of dynamic underwater vehicles, the cell to the right of each dynamic underwater unmanned vehicle is also marked as blocked. This prompts other underwater vehicles to adjust their course to the left of their own underwater vehicles in order to meet the requirement of avoiding the intersection on the starboard side. Direction penalty: for each grid cell ( , ),set up: : Target location, which is the location of other dynamic underwater vehicles; Target heading; = : The vector from the target to this cell; = : The unit vector of the target heading; vector with vector The relative angle between for: ， in, Indicates a minute value; Its angular effect is modeled using a Gaussian decay function: in, It controls the degree of angular spread; then it makes asymmetrical adjustments based on the cross product sign: in, It is a deviation factor that prioritizes passing behind the target underwater vehicle in right-hand encounters to ensure that crossing the area in front of other underwater vehicles from the port side is prohibited. Dynamic urgency weighting: The urgency of avoidance is quantified by the nearest approach time (TCPA) and nearest approach distance (DCPA). in, and These are the security thresholds for TCPA and DCPA, respectively. Finally, in the grid cell The total dynamic penalty for the application is: in, To control distance decay.

9. The method according to claim 8, characterized in that, The function The construction also includes: 3-3, nonlinear conversion to CBF map: neural activity, along with total penalty, is first converted into obstacle-to-time measurements: in, The neural reachable field represents the obstacle; Normalize it to the range [0, 1]: Create a binary mask to isolate cells within the barrier edge: Cells outside the mask are saturated to the maximum value within the edge: Convert the static and dynamic obstacle fields into the final CBF map: The gradient of the CBF field is controlled; continuous CBF in The value at the point is obtained using bilinear interpolation: 。 10. A model predictive motion planning system for obstacle constraint control of an underwater vehicle, the system comprising a memory for storing computer program instructions and a processor for executing the program instructions, wherein, When the computer program instructions are executed by the processor, the system is triggered to execute the model prediction motion planning method for underwater vehicle control obstacle constraints as described in any one of claims 1 to 9.

Images