Model predictive motion planning method and system for underwater vehicle control obstacle avoidance constraints

Abstract

The application provides a model prediction motion planning method and system for controlling obstacles of underwater vehicles, and relates to the technical field of safe collision avoidance motion planning of underwater vehicles in narrow waterways. The application generates a neural reachability field through a deep neural network to realize global path guidance, constructs a dynamic obstacle avoidance map in combination with a nonlinear control barrier function (CBF), and embeds a modifier consistent with the International Regulations for Preventing Collisions at Sea to ensure safe intersection and compliance with the rules. The safety distance of the application is 2-3 times higher than that of the traditional technology, the minimum distance of multiple underwater vehicles in cooperation is 0.82 meters, and the real-time requirement is 0.2 seconds, which significantly enhances the motion planning capability and compliance of underwater vehicles in a dynamic environment.

Description

Model Predictive Motion Planning Method and System for Obstacle Constraint Control of Underwater Submersibles Technical Field

[0001] This invention relates to the field of motion planning technology for safe collision avoidance of unmanned underwater vehicles in narrow waterways, and specifically to a model prediction motion planning method and system for underwater vehicles to control obstacle constraints. Background Technology

[0002] Unmanned underwater vehicles (UUVs) face significant technical challenges when operating in complex underwater environments such as narrow waterways. Drifting caused by ocean currents can easily cause UUVs to deviate from their planned routes, increasing the risk of collisions with obstacles. Traditional safety assurance methods rely on fixed safety margins, which cannot adapt to dynamic changes in ocean currents and lack comprehensive consideration of maritime priority rules such as the International Regulations for Preventing Collisions at Sea (COLREGs). Especially in scenarios involving multiple UUVs operating in coordination or coexisting with manned submarines, it is difficult to make collision avoidance decisions that comply with the rules, significantly increasing the risk of collisions.

[0003] Existing motion planning technologies for unmanned underwater vehicles (UUVs) mostly employ simple obstacle avoidance algorithms and static safety distance settings. While these may be effective in static or low-current environments, they exhibit significant limitations in high-current or dynamic waterways. For example, traditional strategies such as artificial potential field methods and linear rapid travel methods struggle to accurately predict the impact of ocean currents on the trajectory, leading to insufficient safety distances or low navigation efficiency. Furthermore, these methods do not fully consider the interactions between multiple UUVs operating collaboratively, making it difficult to simultaneously meet the requirements of real-time performance, safety, and navigation efficiency in narrow waterways and strong currents.

[0004] To address the aforementioned problems, this invention proposes a model-predictive motion planning method and system for underwater vehicles (UVs) to control obstacle constraints. A neural accessibility field aligned with ocean currents is generated using a deep neural network to provide global path guidance for the UV. A dynamic obstacle avoidance control map is constructed using a nonlinear control obstacle function (CBF), effectively improving safety and adaptability in complex environments. Furthermore, a control obstacle field corrector consistent with COLREGs is embedded to ensure safe convergence and rule compliance during multi-UV collaborative operations, significantly enhancing the motion planning capabilities of UVs in narrow waterway current environments. Summary of the Invention

[0005] To address the technical problems of existing unmanned underwater vehicles (UUVs) in narrow waterways and complex ocean current environments, where traditional safety assurance methods rely on fixed safety margins and lack comprehensive consideration of the International Regulations for Preventing Collisions at Sea (INCL), and in scenarios involving dynamic ocean current changes and multi-UUV collaboration, they struggle to adapt to dynamic environments, accurately predict trajectory impacts, and simultaneously meet real-time, safety, and navigation efficiency requirements. This invention provides a model-predictive motion planning method and system for controlling obstacle constraints in underwater vehicles.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0007] A model-predictive motion planning method for controlling obstacle constraints in underwater vehicles, the method comprising:

[0008] Construct kinematic and dynamic models for underwater vehicles;

[0009] Based on the kinematic and dynamic models, a Model Predictive Control (MPC) framework for motion planning is constructed, as shown in the following expression:

[0010]

[0011]

[0012]

[0013]

[0014] 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.

[0015] In the formula, J represents 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 guidance, 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 will be imposed if the value is not equal to 0.01, and restrictions will be imposed. <1.

[0016] On the other hand, the present invention also provides a model predictive motion planning system for underwater vehicle control obstacle constraints. The system includes a memory for storing computer program instructions and a processor for executing the program instructions. When the computer program instructions are executed by the processor, the system is triggered to execute the above-described model predictive motion planning method for underwater vehicle control obstacle constraints.

[0017] Compared with the prior art, the beneficial effects of the present invention are:

[0018] 1. Significantly Enhanced Adaptability to Dynamic Environments: Traditional methods rely on fixed safety margins, which cannot adapt to dynamic changes in ocean currents, resulting in insufficient safety distances or low navigation efficiency. This invention generates a neural reachability field through deep neural networks, quantifying the influence of ocean current direction and obstacle space in real time, enabling the submersible to plan the optimal path in accordance with the ocean current direction. Experiments show that in crosscurrent environments, the safety distance of this method is increased by 2-3 times compared to traditional methods (e.g., 0.85 meters vs. 0.24 meters), while the navigation time is shortened.

[0019] 2. Multi-submarine Cooperative Safety and Rule Compliance: Existing technologies do not fully consider multi-body interaction rules, and are prone to violating COLREGs rules during cooperative operations or coexistence with manned submarines, leading to a surge in collision risk. This invention embeds a control obstacle field corrector consistent with COLREGs, quantifying obstacle avoidance priorities through encounter classification, direction penalty, and dynamic urgency weighting mechanisms. For example, in a scenario of cooperative navigation of eight submarines, this method can maintain a minimum distance of 0.82 meters, double that of traditional methods (0.40 meters), and the trajectory 100% complies with the rule requirements for starboard intersection and overtaking of other submarines.

[0020] 3. Real-time Performance and Computational Efficiency Optimization: This invention employs a Model Predictive Control (MPC) framework, combining parallel computation of neural reachability fields and CBF to update control commands every 0.01 seconds. In the Gazebo simulator, this method completes path replanning within 0.2 seconds, meeting real-time requirements without compromising path quality.

[0021] 4. Unified Global and Local Planning in Complex Scenarios: Existing technologies often handle global path planning and local obstacle avoidance separately, leading to conflicts or efficiency losses. This invention achieves a balance between global optimization and local safety through a hierarchical architecture of global guidance using neural reachability fields and local obstacle avoidance using CBF (Cyclic Block Factor). In narrow waterways filled with reefs, this method achieves a safe distance of 1.25 meters, nearly double the obstacle control function of the artificial potential field method (0.65 meters), while simultaneously reducing travel time.

[0022] 5. Improved Anti-interference Capability and Robustness: Traditional methods are sensitive to sensor noise or sudden ocean currents, which can easily lead to planning failure. This invention's CBF field softens constraints through relaxation, maintaining feasibility while penalizing deviations. In simulator testing, even with 10% noise in the sonar data, this method still maintains a minimum safe distance of 0.86 meters without any collisions.

[0023] Other features and advantages of the embodiments of the present invention will be described in detail in the following detailed description section. Attached Figure Description

[0024] Figure 1 is a schematic diagram of the fixed coordinate system and volume coordinate system of the underwater vehicle according to the present invention;

[0025] Figure 2 is a framework diagram of a safety-critical unmanned underwater vehicle motion planner according to the present invention;

[0026] Figure 3 is a diagram of the dynamic neural network model according to the present invention, wherein (a) is a schematic diagram of a two-dimensional underwater environment, and (b) is the corresponding neural network diagram;

[0027] Figure 4 is a schematic diagram of the generation of the obstacle-oriented control barrier function according to the present invention, 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.

[0028] Figure 5 is a schematic diagram of the dynamic unmanned underwater vehicle encounter penalty according to the present invention, wherein (a) is a schematic diagram of encounter classification, and (b) is a schematic diagram of the corresponding CBF field in the case of crossing right.

[0029] Figure 6 is a comparative schematic diagram of the global motion planning algorithm according to the present invention in a narrow waterway environment;

[0030] Figure 7 is a schematic diagram of CBF fields using different algorithms according to the present invention, wherein (a) is a schematic diagram of APF-CBF, (b) is a schematic diagram of LFM-CBF, and (c) is a schematic diagram of the method proposed in the present invention;

[0031] Figure 8 is a schematic diagram of the results of local motion planning by artificial intelligence under more turbulent sea conditions according to the present invention;

[0032] Figure 9 is a schematic diagram of motion planning for multiple unmanned underwater vehicles using the proposed algorithm according to the present invention, wherein (a) is a schematic diagram of the initial steps, (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.

[0033] Figure 10 shows an example of trajectory generation for multiple unmanned underwater vehicles based on the algorithm proposed by Lewis et al. (2020), where (a) is a schematic diagram of the initial steps, (b) is a schematic diagram of the intermediate steps, (c) is a schematic diagram of a scenario where a collision almost occurs, and (d) is a schematic diagram of the final steps.

[0034] Figure 11 is a schematic diagram of the motion planning of multiple unmanned underwater vehicles in an obstacle environment according to the present invention, wherein (a) is a schematic diagram of the initial steps, (b) is a schematic diagram of collision avoidance when crossing to the right, (c) is a schematic diagram of collision avoidance when colliding head-on, and (d) is a schematic diagram of the final result.

[0035] Figure 12 is a schematic diagram of motion simulation in the underwater unmanned submersible simulator according to the present invention, wherein (a) is a schematic diagram of the simulation environment, (b) is a schematic diagram of sonar sensing, (c) is a schematic diagram of the final trajectory, and (d) is a schematic diagram of the control input curve. Detailed Implementation

[0036] To enable those skilled in the art to better understand the technical solutions of this invention, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings, so as to more clearly understand the purpose, features and advantages of this invention. It should be understood that the embodiments shown in the drawings are not intended to limit the scope of this invention, but are only for illustrating the essential spirit of the technical solutions of this invention. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this invention.

[0037] Unless the context requires otherwise, throughout the specification and claims, the word “comprising” and its variations, such as “including” and “having”, shall be understood to have an open, inclusive meaning, that is, to be interpreted as “including, but not limited to”.

[0038] Throughout this specification, references to "an embodiment" or "an embodiment" indicate that a particular feature, structure, or characteristic described in connection with the embodiment is included in at least one embodiment. Therefore, the appearance of "in an embodiment" or "an embodiment" in various places throughout the specification does not necessarily refer to the same embodiment. Furthermore, a particular feature, structure, or characteristic may be combined in any manner in one or more embodiments.

[0039] The singular forms “a” and “the” used in this specification and the appended claims include plural references unless otherwise expressly stated herein. It should be noted that the term “or” is generally used to mean “and / or” unless otherwise expressly stated herein.

[0040] 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.

[0041] 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.

[0042] 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.

[0043] The motion planning problem of unmanned underwater vehicles can be described by formula (1):

[0044]

[0045]

[0046]

[0047]

[0048]

[0049]

[0050] Where J represents 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... This represents a minute value.

[0051] Equation (1) shows that the motion planning objective of the unmanned underwater vehicle is to start from the initial pose. Reach the target pose During the process, the objective function J, such as distance, time consumption, and energy consumption, is minimized. This is constrained by the kinematic constraints of the underwater vehicle (kinematic constraint formula), and the underwater vehicle's operating speed is limited to the speed under the maximum thrust provided by the thruster (velocity set constraint). Finally, and crucially, the underwater vehicle must not collide with obstacles; that is, the intersection of the underwater vehicle's pose and the obstacle during operation must be an empty set. Therefore, this invention proposes a model prediction-based safe obstacle avoidance motion planning method based on the control obstacle function, as follows:

[0052] First, kinematic and dynamic modeling of the unmanned underwater vehicle (UUV) is performed. Specifically, the kinematic model for the underwater motion planning task of the UUV is as follows:

[0053]

[0054]

[0055] The kinematic model of the unmanned underwater vehicle describes the fixed coordinate system (O- ) and volume coordinate system (0- The conversion between () and () is shown in Figure 1, given by formulas (2) and (3). In formula (2), = This indicates the position and heading of the underwater vehicle in a fixed coordinate system. Indicates the heading angle, while = bold For vectors, is a scalar 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 converted to coordinates in a fixed coordinate system using the transformation matrix (3).

[0056] Next, the dynamic model of the unmanned underwater vehicle in the ocean current is given by equations (4) and (5):

[0057]

[0058]

[0059] in, It refers to the mass of the underwater vehicle; These represent the additional mass in the x and y degrees of freedom directions and the additional moment of inertia in the z direction, respectively; while , , and their quadratic terms , , This represents the hydrodynamic damping coefficient; It is a control input vector that includes thrust and torque; I represents the ocean current velocity in the volume coordinate system; z It is the moment of inertia along the z-axis; u r It refers to the longitudinal velocity of the water; v r It refers to the lateral velocity of the water.

[0060] After completing the kinematic and dynamic modeling of the unmanned underwater vehicle (UUV), the next stage was the construction of a safety-critical motion planning framework, as shown in Figure 2. This stage, based on information obtained from the previous kinematic and dynamic modeling, integrated the reachability regions of sonar sensing, target guidance, and obstacle guidance. Through neural field computation, the construction of a nonlinear control barrier function (CBF), and the application of the model predictive control (MPC) framework, a highly efficient and safe model predictive motion planning method for UUVs was formed. This method not only adapts to complex and variable ocean current environments but also ensures safe collision avoidance for UUVs in narrow waterways, while simultaneously meeting multiple requirements for real-time performance, safety, and navigation efficiency.

[0061] The Model Predictive Control (MPC) framework used for motion planning is as follows:

[0062]

[0063]

[0064]

[0065]

[0066] The model predictive control (MPC) construction method 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, as shown in equation (6).

[0067] In formula (6), Indicates the first Step control input, and These represent the prediction time domain and the control time domain, respectively. At point ( , The neural reachability value at (). Used to weigh the costs of guidance, Then regarding the deviation The penalty will start from 0.01. Setting it to 1000 prioritizes satisfying the control Lyapunov function inequality when optimizing the objective function. This prevents conflicts arising from control Lyapunov function constraints. Only then will it 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 quantity. (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.

[0068] In the Model Predictive Control (MPC) framework, i.e., Equation (6), the realization of the objective function depends on the accurate perception of environmental information and the real-time processing of dynamic constraints. To meet this requirement, the overall framework includes three core steps: first, by generating a neural accessibility field through (1), the spatial influence of ocean currents and obstacles is quantified; then, by using (2), it is transformed into a global path guided by the objective; finally, by constructing a nonlinear control obstacle function (CBF) through (3), dynamic obstacle avoidance and rule constraints are embedded. These three steps are progressive and jointly support the balanced optimization of real-time performance, safety, and navigation efficiency in Equation (6).

[0069] (1) Generate neural reachability field

[0070] To account for environmental factors such as ocean currents, a deep neural network (DNN) model is used to generate a neural accessibility field to guide the underwater vehicle's movement. As shown in Figure 3, 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 paths aligned with favorable ocean currents. This deep neural network model biases information propagation in the direction of favorable ocean currents. The activity of each neuron lies within the interval [0, 1], where the target neuron is 1 and obstacle neurons are 0. The neural activity of other neurons gradually decreases outward from the target neuron, being higher than 0 but lower than 1.

[0071] (2) Neural reachability field used for target guidance

[0072] 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 can be obtained. And obtain neural reachability values. ( , ) at position ( , The formula for calculating ) is:

[0073] in =1 / ( The neural reachability field () represents the reachability range of the neural network at the target location. `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 (6).

[0074] (3) Obstacle avoidance control function based on neural reachability field

[0075] The Control Barrier Function (CBF) provides a mechanism for implementing safety constraints in the navigation of safety-critical underwater vehicles. It integrates safety sets... Defined as:

[0076]

[0077]

[0078]

[0079] If 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.

[0080] In this invention, this concept is extended by embedding spatial and environmental dynamics into the expression of the nonlinear control obstacle function CBF (via a neural accessibility field). Similar to the goal-oriented accessibility field, the obstacle-oriented accessibility field is computed using the same deep neural network (DNN) propagation model, but with obstacle units as activation sources.

[0081] Nonlinear control barrier function The construction is divided into three stages as shown in Figure 4(a)-(c).

[0082] (3-1) Static obstacle field based on neural accessibility field

[0083] By using a deep neural network propagation algorithm that operates on obstacle cells as the source, an obstacle neural activity field is generated. As shown in Figures 4(a) and (b), the deep neural network generates a stable obstacle activity field. .

[0084] (3-2) Integration of penalty points for dynamic underwater vehicles

[0085] In (3-1), a basic safe obstacle avoidance environment was constructed for underwater vehicles by generating a static obstacle field of neural accessibility. However, in dynamic waterways and multi-vehicle collaborative operation scenarios, relying solely on static information cannot meet real-time safety requirements. Therefore, it is necessary to further integrate the interaction rules and risk assessment mechanisms of dynamic underwater vehicles. Through the synergistic effect of encounter classification, direction penalty, and dynamic urgency weight, the behavioral constraints and collision priorities in multi-body obstacle avoidance are quantified, providing a dynamically adjusted penalty integral basis for the subsequent construction of a nonlinear control obstacle function (CBF).

[0086] Specifically, when multiple unmanned underwater vehicles (UUVs) operate in the same area, other UUVs are treated as dynamic obstacles, and an additional penalty area (penalty function) is added to the nonlinear control obstacle function (CBF) map to enforce cooperative navigation behavior in accordance with COLREGs.

[0087] Encounter classification, direction penalty, and dynamic urgency weight are the three core components of dynamic underwater vehicle penalty integral integration. Through synergy, these three elements quantify multi-submarine interaction rules and dynamic risks into penalty terms in the nonlinear control obstacle function (CBF), thereby ensuring the safety and compliance of obstacle avoidance control. Specifically:

[0088] Encounter classification: based on relative azimuth angle (Determined from the heading of its own unmanned underwater vehicle, ranging from -180° to 180°), there are four typical encounter scenarios as shown in Figure 5(a), which are defined as follows:

[0089] Right intersection: ∈[-112.5°, -15°],

[0090] Left intersection: ∈[15°, 112.5°],

[0091] A head-on encounter: ∈[-15°, 15°],

[0092] Surpassing other underwater vehicles: Other situations.

[0093] When calculating the accessibility field of obstacles at the intersection of dynamic unmanned underwater vehicles (UUVs), the cell to the right of each dynamic UUV is also marked as blocked. This prompts other UUVs to adjust their course to the left of their own UUVs in order to meet the COLREGs' requirement to avoid the starboard intersection.

[0094] Direction penalty: for each grid cell ( , ),set up:

[0095] The location of the target (other dynamic unmanned underwater vehicles).

[0096] Target heading (radians)

[0097] = The vector from the target to this cell.

[0098] = : The unit vector of the target heading.

[0099] vector with vector The relative angle between for:

[0100]

[0101] in, Indicates a minute value;

[0102] Its angular effect is modeled using a Gaussian decay function:

[0103]

[0104] in, It controls the degree of angle spread (for example, it can be set to ±30°, corresponding to...). =π / 6).

[0105] Then, asymmetric adjustments are made based on the cross product sign:

[0106]

[0107]

[0108] in It is a deviation factor (e.g.) =0.4), in a right-hand encounter, the priority is to pass behind the target unmanned underwater vehicle (as shown in Figure 5(b)) to satisfy the COLREGs rule prohibiting entry into the area in front of other unmanned underwater vehicles from the port side.

[0109] Dynamic urgency weighting: The urgency of avoidance is quantified by the nearest approach time (TCPA) and nearest approach distance (DCPA).

[0110]

[0111] in, and These are the security thresholds for TCPA and DCPA, respectively.

[0112] Finally, in the grid cell The total dynamic penalty for the application is:

[0113]

[0114] in, To control distance decay.

[0115] (3-3) Nonlinear conversion to CBF map

[0116] 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:

[0117] Neural activity, along with total penalty, is first converted into an obstacle-to-time measure:

[0118]

[0119] in, The neural reachable field represents the obstacle;

[0120] Normalize it to the range [0, 1]:

[0121]

[0122] Create a binary mask to isolate cells within the barrier edge:

[0123]

[0124] Cells outside the mask are saturated to the maximum value within the edge:

[0125]

[0126] Convert the static and dynamic obstacle fields into the final CBF map:

[0127]

[0128] Controlling the gradient of the CBF field. Continuous CBF in The value at the point is obtained using bilinear interpolation:

[0129]

[0130] 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.

[0131] Example 2

[0132] This embodiment evaluates the performance of global and local motion planning in a narrow waterway and an environment influenced by ocean currents. The algorithms compared are tested under identical conditions. Subsequently, scenarios involving multiple unmanned underwater vehicles (UUVs) are studied to evaluate the method's performance in cooperative navigation. To match the experimental setup to real-world operating conditions, simulations are performed in an UUV simulator environment containing dense obstacles. The UUV model uses the hydrodynamic parameters of the RexROV UUV.

[0133] For deep neural networks (DNNs), the parameters are set to... =0.05 and =100.

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

[0135] For the nonlinear CBF, the parameters are: =5, =0.01. Safety time interval =6, safe distance =10.

[0136] The simulation in this embodiment was performed on a personal computer equipped with an Intel® Core™ i7-10750H processor (2.60 GHz clock speed), 32 GB of memory, and Windows 10. All optimization solutions for the objective function of model predictive control were performed in MATLAB using the fmincon-sqp solver.

[0137] The algorithms compared include the following:

[0138] Distance-Constrained Controlled Obstacle Function (DC-CBF): Calculates the minimum distance to each discrete obstacle element, where in equation (21)... This indicates the center of each element.

[0139]

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

[0141]

[0142]

[0143] Linear Fast Movement Control Barrier Function (LFM-CBF): Calculated using the fast movement method. Two-dimensional interpolation is obtained.

[0144] Pure distance constraint: Model predictive control without any control barrier function constraints.

[0145] Distance objective function: The objective function is set as the fast travel time field (Euclidean distance to the target position).

[0146] (1) Global motion planning in narrow waterways

[0147] The first scenario sets up a 100×100 meter grid environment containing a narrow channel. The unmanned underwater vehicle (UUV) starts from point (20, 50) and aims to reach point (96, 96). There is a crosscurrent within the channel, with a flow velocity half that of the UUV's maximum speed (as shown in Figure 6).

[0148] Table 1. Performance Comparison of Different Algorithms in Global Motion Planning

[0149]

[0150] Table 1 summarizes the performance metrics. The model that uses distance as the objective achieves the shortest travel distance, but takes slightly longer than the proposed model. Without any nonlinear control barrier function (CBF) constraints, pure distance constraints achieve the fastest speed (82.73 seconds), but simultaneously result in the smallest safe distance in the crossflow region (0.24 meters), posing a significant collision risk. The nonlinear control barrier function (CBF) fields generated by the artificial potential field control barrier function and the distance-constrained control barrier function have weak gradients, offering negligible safety improvements. The linear fast-moving control barrier function generates long-range repulsive forces, but suffers from significantly longer travel time (minimum travel speed); furthermore, due to the lack of current sensing capabilities, it can only achieve a safe distance of 0.61 meters. In contrast, the method proposed in this invention achieves the maximum safe distance (0.85 meters) while maintaining a relatively high travel speed.

[0151] As shown in Figure 7, the gradient generated by the artificial potential field control obstacle function is relatively gentle, while the linear fast-moving control obstacle function has a wider but farther influence. The method proposed in this invention generates a strong local gradient near obstacles and enhances the repulsive force when the water flow pushes the unmanned underwater vehicle into danger zones, thereby reducing the nonlinear control obstacle function (CBF) value in these areas. This dual effect, generated by the water flow-aware objective function and the water flow-aware nonlinear control obstacle function (CBF), shapes a navigation trajectory that ensures both safety and efficiency.

[0152] (2) Local motion planning with stronger ocean capabilities

[0153] The second scenario simulates local motion planning in a narrow waterway full of reefs. In this case, the unmanned underwater vehicle needs to travel from point (15, 2) to point (97, 96) under the influence of a strong ocean current of 1 m / s, as shown in Figure 8.

[0154] Table 2 Performance Comparison of Different Algorithms in Local Motion Planning

[0155]

[0156] Table 2 presents the results. The nonlinear control barrier function (CBF) proposed in this invention again demonstrates the existence of a significant local gradient near the obstacle, and the safe distance (1.25 meters) is significantly higher than other methods. The artificial potential field control barrier function and the linear fast-moving control barrier function completed the planning task, but due to neglecting the influence of water flow, the obtained safe distance is less than 0.65 meters—which poses a significant collision risk in the event of sudden lateral current surges. The distance-constrained control barrier function performs similarly, with negligible improvement compared to pure distance constraints.

[0157] The shorter target distance MPC reduced the trajectory length (129.37 meters), but caused the unmanned underwater vehicle to pass through more crosscurrent areas, increasing control difficulty and reducing safety margin. Although its travel time was close to that of the proposed method in this local case, the shorter safety distance still posed a higher risk due to the more balanced use of currents and detours.

[0158] Overall, all benchmark nonlinear control barrier functions (CBFs) fail to account for the effects of water flow, thus providing limited protection in crossflow regions. The proposed method, however, proactively addresses this issue with its current-sensing nonlinear control barrier function (CBF). By altering the path to avoid high-risk water flow channels, safer navigation is achieved with only a modest increase in distance and time.

[0159] (3) Simulation of cooperative navigation scenario of multiple unmanned underwater vehicles

[0160] To evaluate collaborative performance, motion planning was tested on eight unmanned underwater vehicles (UUVs) simultaneously navigating in a 100m × 100m environment, with both static and unobstructed conditions. Initial and target positions were assigned cyclically: (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 UUVs was represented by a pie chart.

[0161] As shown in Figure 9, the method proposed in this invention achieves efficient and collision-free cooperative navigation even in dense multi-UAV scenarios. In Figures 9(b) and (c), the trajectories clearly conform to the encounter rules of the International Regulations for Preventing Collisions at Sea (ICMLS), demonstrating that navigational 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 may even lead to near-collision events. Quantitatively, the method proposed in this invention maintains a minimum distance of 0.82 meters between UAVs, while the method of Lewis et al. can only maintain a distance of 0.40 meters.

[0162] Finally, Figure 11 shows that the dynamic penalty mechanism proposed in this invention can be seamlessly embedded into the reachability region of a deep neural network, thereby forming a rich guidance scenario that enables multiple unmanned underwater vehicles (UUVs) to safely conduct cooperative navigation in environments with both obstacles and ocean currents. In this scenario, the current speed is set to one-quarter of the maximum speed of the UUV. As shown in Figures 11(b) and (c), the planned trajectory complies with the International Regulations for Preventing Collisions at Sea (ICC): the UUV overtakes from the stern, and upon head-on collision, both vessels turn to starboard. The minimum distance between the two vessels is maintained at 1.06 meters, further confirming the safety performance of the framework proposed in this invention.

[0163] Example 3

[0164] This embodiment is a simulation experiment in an unmanned underwater vehicle simulator. The simulation was conducted on a computer with an Intel® Core™ i7-10750H processor (2.60GHz clock speed) and 32GB of RAM, based on the Ubuntu 18.04 operating system, and using ROS.

[0165] To further verify its real-time applicability under real hydrodynamic and sensing conditions, the proposed method was tested using the "RexROV" model in a Gazebo-based unmanned underwater vehicle simulator. As shown on the left side of Figure 12(a), the simulation environment comprises a 50m × 50m workspace filled with dense underwater piers. The unmanned underwater vehicle departs from (5, 3) with the mission of reaching (46, 42) using only local sensing data from the ship's sonar sensors (Figure 12(b)). The proposed control barrier function enables the unmanned underwater vehicle to decelerate and reorient before entering the restricted area, thereby ensuring safe passage (Figure 12(c)). Figure 12(d) shows the forward velocity during navigation. and yaw rate The curve. The total travel distance was 80.04 meters, the time was 189.1 seconds, and the minimum distance to the obstacle was 0.86 meters.

[0166] A key practical observation is that in Gazebo, the RexROV model becomes unresponsive if the speed command issuance interval exceeds 0.2 seconds. Throughout the experiment, the proposed method maintained real-time performance, meeting this requirement, without any degradation in path quality. These results confirm the effectiveness of the method. While only applicable to offline simulations, it can also be easily applied to real-world unmanned underwater vehicle platforms equipped with internal sensing and computing capabilities.

[0167] The present invention also provides a model predictive motion planning system for underwater vehicle control obstacle constraints. The system includes a memory for storing computer program instructions and a processor for executing the program instructions. When the computer program instructions are executed by the processor, the system is triggered to execute the above-described model predictive motion planning method for underwater vehicle control obstacle constraints.

[0168] This invention proposes a model predictive motion planning method and system based on a novel control obstacle function, combining two artificial intelligence components with a rolling time-domain controller. First, a reachability time neural field calculation method influenced by water flow is proposed for global guidance in accordance with the current direction. This method is then applied to generate the control obstacle field required for safe obstacle avoidance in ocean current environments. Furthermore, a control obstacle field modifier consistent with the International Regulations for Preventing Collisions at Sea (ICMR) is embedded to shape safe encounters when multiple unmanned underwater vehicles (UUVs) meet. In engineering applications, the obstacle field is embedded into a discrete model predictive controller through softened control obstacle constraints. Experiments in narrow waterways and high-fidelity underwater simulations demonstrate that this invention increases safety margins in crosscurrent regions, ensures cooperative operation complies with ICMR requirements, and is feasible in real-time.

[0169] Although the present invention has been described in detail with reference to the accompanying drawings and preferred embodiments, the invention is not limited thereto. Various equivalent modifications or substitutions can be made to the embodiments of the invention by those skilled in the art without departing from the spirit and essence of the invention. Such modifications or substitutions should all fall within the scope of the invention, or any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the invention should be covered within the protection scope of the invention. Therefore, the protection scope of the invention should be determined by the 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: constructing a kinematic model and a dynamic model of an underwater vehicle; and based on the kinematic model and the dynamic model, constructing a model predictive control (MPC) framework for motion planning, 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; where J represents 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; among which, = 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; These represent the additional mass in the x and y degrees of freedom directions and the additional moment of inertia in the z direction, respectively; while 、 、 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; It is the moment of inertia along the z-axis; 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 generation of the neural accessibility field includes: using a deep neural network (DNN) model to generate a neural accessibility field 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 towards 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 weakens 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, setting the neurons at the target location as external activation values. 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: the control barrier function (CBF) provides a mechanism for implementing safety constraints in underwater vehicle navigation, and sets safety parameters... 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 considered dynamic obstacles, and an additional penalty area is added to the nonlinear control obstacle function (CBF) map to implement cooperative navigation behavior, specifically 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: right-hand intersection: ∈[-112.5°, -15°], left intersection: ∈[15°, 112.5°], face-to-face encounter: ∈[-15°, 15°], overtaking 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 unmanned underwater vehicle is also marked as blocked, which prompts other underwater vehicles to adjust their course to the left of their own underwater vehicle to meet the requirement of avoiding the starboard intersection; 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, This represents 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 comply with the prohibition against crossing into the area in front of other underwater vehicles from the port side; Dynamic urgency weight: The urgency of avoidance is quantified by the time to closest approach (TCPA) and the distance to closest approach (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 of the obstacle is represented; it is normalized 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