An unmanned surface vehicle obstacle avoidance navigation path generation and tracking method and device

Abstract

The application provides an unmanned surface ship obstacle avoidance navigation path generation and tracking method and device. The method provided by the application comprises the following steps: constructing a ship kinematics model according to navigation motion parameters of an unmanned surface ship; identifying obstacles on an initial navigation path and constructing a dangerous area; calculating the geometric relationship between the initial navigation path and the dangerous area based on the ship kinematics model, and generating an obstacle avoidance navigation path; adjusting the obstacle avoidance navigation path based on the interaction relationship between the obstacle avoidance navigation path and the plurality of dangerous areas; detecting the curvature of the obstacle avoidance navigation path at the entrance and exit port, generating a tangent circle optimization path tangent to the boundary of the dangerous area at the entrance and exit port based on the curvature, and obtaining an obstacle avoidance navigation optimization path; generating a desired navigation heading angle according to the obstacle avoidance navigation optimization path based on the finite time theory and the ship kinematics model; and constructing a navigation tracking adjustment module, and driving the unmanned surface ship to track the obstacle avoidance navigation optimization path based on the navigation tracking instruction output by the ship kinematics model.

Description

Technical Field

[0001] This application relates to the field of unmanned surface vessel navigation technology, and in particular to a method and apparatus for generating and tracking obstacle avoidance navigation paths for unmanned surface vessels. Background Technology

[0002] With the rapid development of marine resource development and maritime transport, the demand for autonomous navigation of unmanned surface vessels (USVs) in environmental monitoring, maritime patrol, target tracking, and water transport is becoming increasingly prominent. When performing missions in complex sea areas, vessels need to achieve autonomous navigation based on accurate orientation perception and path calculation. The accuracy of their navigation path planning and their real-time obstacle location capabilities directly affect the safety and effectiveness of mission completion. Especially in high-risk areas such as densely packed waterways and congested ports, if USVs cannot accurately identify obstacle locations and optimize avoidance paths through navigation, they are highly susceptible to collisions, groundings, and other accidents due to course deviations and position calculation errors. This highlights the core supporting role of navigation technology in complex environments. Therefore, improving the accuracy of navigation path generation and obstacle orientation perception capabilities of USVs in dynamic scenarios has become one of the key issues in ensuring their autonomous navigation performance.

[0003] Currently, navigation path planning and obstacle avoidance for unmanned surface vessels typically rely on traditional navigation methods, such as path guidance based on artificial potential fields, azimuth constraints based on obstacle control functions, and route search using the A* algorithm. While these methods can achieve basic navigation path planning to some extent, they have significant limitations: First, the artificial potential field method is prone to azimuth judgment errors in areas with dense obstacles, leading to the navigation path getting stuck in local optima. Second, the obstacle control function method requires extremely high accuracy in obstacle azimuth modeling, and the computational load of azimuth parameters in complex scenarios is too large, affecting navigation real-time performance. Third, the A* algorithm is mainly suitable for static navigation scenarios, and it does not adequately consider dynamic obstacle azimuth changes and heading constraints during ship turns, making it difficult to meet the navigation needs of complex sea areas. Furthermore, some methods fail to fully integrate real-time navigation perception information such as satellite positioning and radar detection, resulting in delayed obstacle azimuth updates and an inability to dynamically adjust the navigation path.

[0004] Therefore, there is an urgent need for a method based on precise navigation perception that can generate obstacle avoidance paths that meet navigation accuracy requirements by integrating real-time location information of obstacles with ship heading and maneuvering constraints, thereby improving navigation path tracking accuracy and system stability under actual navigation conditions. Summary of the Invention

[0005] In view of this, this application provides a method and apparatus for generating and tracking obstacle avoidance navigation paths for unmanned surface vessels, which generates obstacle avoidance paths that meet navigation accuracy requirements by fusing real-time location information of obstacles with the course control constraints of the vessel, thereby improving navigation path tracking accuracy and system stability under actual navigation conditions.

[0006] Specifically, this application is implemented through the following technical solution:

[0007] The first aspect of this application provides a method for generating and tracking obstacle avoidance navigation paths for unmanned surface vessels, the method comprising:

[0008] Based on the navigation motion parameters of the unmanned surface vessel, a kinematic model of the vessel is constructed;

[0009] Identify obstacles on the initial navigation path and construct a danger zone with the center of the obstacle as the origin;

[0010] The geometric relationship between the initial navigation path and the danger zone is calculated based on the ship's kinematics model, and an obstacle avoidance navigation path is generated based on the geometric relationship.

[0011] Determine the overlap of multiple danger zones, and based on the positional relationship of each obstacle and the geometric characteristics of the obstacle avoidance navigation path, determine the interaction relationship between the obstacle avoidance navigation path and multiple danger zones, and adjust the obstacle avoidance navigation path based on the interaction relationship;

[0012] Based on the minimum turning radius of the ship, the curvature of the obstacle avoidance navigation path at the entrance and exit ports is detected. When the curvature does not meet the turning capability requirements, an optimized tangent circle path tangent to the boundary of the danger zone is generated at the entrance and exit ports. The paths are combined to obtain the optimized obstacle avoidance navigation path.

[0013] Based on the finite-time theory, an integral line-of-sight guidance law is designed. Combined with the ship kinematics model, the desired navigation heading angle is generated according to the obstacle avoidance navigation optimization path.

[0014] A navigation tracking adjustment module is constructed, which outputs navigation tracking commands based on the ship kinematics model, the desired navigation heading angle, and the ship's real-time navigation status;

[0015] The navigation tracking command drives the unmanned surface vessel to follow the obstacle avoidance navigation optimization path.

[0016] The second aspect of this application provides an unmanned surface vessel obstacle avoidance navigation path generation and tracking device, the device comprising a construction module, a generation module, an adjustment module, a combination module, a determination module and a driving module;

[0017] The construction module is used to construct a ship kinematic model based on the navigation motion parameters of the unmanned surface vessel.

[0018] The construction module is also used to identify obstacles on the initial navigation path and construct a danger zone with the center of the obstacle as the origin;

[0019] The generation module is used to calculate the geometric relationship between the initial navigation path and the danger zone based on the ship's kinematics model, and generate an obstacle avoidance navigation path based on the geometric relationship.

[0020] The adjustment module is used to determine the overlap of multiple danger zones, and based on the positional relationship of each obstacle and the geometric characteristics of the obstacle avoidance navigation path, to determine the interaction relationship between the obstacle avoidance navigation path and multiple danger zones, and to adjust the obstacle avoidance navigation path based on the interaction relationship.

[0021] The combined module is used to detect the curvature of the obstacle avoidance navigation path at the entrance and exit ports based on the minimum turning radius of the ship. When the curvature does not meet the turning capability requirements, it generates an optimized path segment of the tangent circle at the entrance and exit ports that is tangent to the boundary of the danger zone, and combines them to obtain an optimized obstacle avoidance navigation path.

[0022] The generation module is also used to design an integral line-of-sight guidance law based on the finite-time theory, and generate the desired navigation heading angle according to the obstacle avoidance navigation optimization path in combination with the ship kinematics model.

[0023] The determining module is used to construct the navigation tracking adjustment module, and output navigation tracking commands based on the ship kinematics model, the desired navigation heading angle and the real-time motion state of the ship;

[0024] The drive module is used to drive the unmanned surface vessel to follow the obstacle avoidance navigation optimization path based on the navigation tracking command.

[0025] The method and apparatus for generating and tracking obstacle avoidance navigation paths for unmanned surface vessels provided in this application, in its first aspect, firstly constructs a ship kinematic model based on the navigation motion parameters of the unmanned surface vessel, providing a physical and kinematic basis for subsequent obstacle avoidance navigation. Subsequently, by identifying obstacles on the initial navigation path, a danger zone is constructed with the obstacle center as the origin, establishing a spatial risk model to avoid ignoring the influence of obstacles during path generation. Specifically, this application is not limited to generating avoidance paths for single obstacles, but further determines the overlapping relationship between multiple danger zones and analyzes the interaction between the obstacle avoidance navigation path and multiple danger zones by combining the relative positions of obstacles and the geometric properties of the path. Based on this, the direction and shape of the obstacle avoidance navigation path are dynamically adjusted, enabling the path to reasonably traverse between multiple danger zones without falling into local dead zones or repeatedly detouring, avoiding collision risks caused by the path traversing overlapping areas. This overcomes the bottleneck of obstacle avoidance navigation paths being "detached from local context and difficult to coordinate globally" in multi-obstacle environments, achieving a deep integration of path planning and environmental perception. Even in environments with dense obstacle distribution or dynamic changes, this application can still generate spatially feasible obstacle avoidance navigation paths that satisfy ship maneuverability, thus significantly improving the robustness and safety of unmanned surface vessels (USVs) in obstacle avoidance navigation in complex waters. Secondly, this application optimizes path curvature based on ship kinematics characteristics to enhance path controllability and tracking stability. After obtaining the obstacle avoidance navigation path, this application further detects the local curvature of the path at the "entry and exit ports" based on the minimum turning radius defined in the ship's kinematic model. Considering that USVs are typical underactuated systems with strong non-holonomic constraints, if the path curvature exceeds its minimum turning capability range, the ship will be unable to travel along the predetermined trajectory, or even experience heading oscillations or deviations from the expected track. Therefore, when the curvature does not meet the requirements, this application introduces a "tangent circle tangent to the boundary of the danger zone" mechanism to construct a smooth transition section at the path's entry and exit from the danger zone, ensuring that the path has a geometrically continuous first derivative, meeting the physical maneuverability conditions of the ship. This path curvature optimization strategy organically combines the mathematical and geometric characteristics of path planning with the ship's own physical execution capabilities, ensuring that the generated path not only avoids obstacles in space but also has high feasibility in dynamic execution. Its significant effects are reflected in: avoiding situations where sharp turns or acute angles in the path cause drastic changes in control signals, resulting in smoother and more precise control commands generated by the navigation tracking and adjustment module; effectively suppressing yaw or sideslip phenomena caused by abrupt trajectory changes; and thus enhancing the unmanned surface vessel's heading maintenance capability and tracking stability in disturbed environments. Attached Figure Description

[0026] Figure 1 A flowchart of the obstacle avoidance navigation path generation and tracking method for unmanned surface vessels provided in Embodiment 1 of this application;

[0027] Figure 2 This is a schematic diagram of the unmanned surface vessel obstacle avoidance navigation path generation and tracking device provided in Embodiment 2 of this application. Detailed Implementation

[0028] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application.

[0029] The terminology used in this application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. The singular forms “a,” “the,” and “the” used herein are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used herein refers to and includes any and all possible combinations of one or more of the associated listed items.

[0030] It should be understood that although the terms first, second, third, etc., may be used in this application to describe various information, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from one another. For example, without departing from the scope of this application, first information may also be referred to as second information, and similarly, second information may also be referred to as first information. Depending on the context, the word "if" as used herein may be interpreted as "when," "when," or "in response to determination."

[0031] The following specific embodiments are given to illustrate the technical solution of this application in detail.

[0032] Figure 1 The flowchart illustrates the obstacle avoidance navigation path generation and tracking method for unmanned surface vessels provided in Embodiment 1 of this application. Please refer to... Figure 1 The method provided in this embodiment may include:

[0033] S101. Construct a ship kinematics model based on the navigation motion parameters of the unmanned surface vessel.

[0034] Specifically, the navigation motion parameters of an unmanned surface vessel (USV) refer to the physical quantities that describe the motion state and characteristics of the USV, including attitude, velocity and angular velocity, and control input. Attitude includes the USV's longitudinal position, lateral position, and bow angle in the inertial coordinate system. Velocity and angular velocity include the USV's longitudinal velocity, lateral velocity, and bow angular velocity in the hull coordinate system. Control input includes the USV's longitudinal and bow control input torques.

[0035] It should be noted that in this embodiment, the vessels referred to are all unmanned surface vessels, and in the following content, the term "vessel" will be used to refer to unmanned surface vessels.

[0036] Furthermore, the ship kinematics model includes a first ship kinematics model and a second ship kinematics model. The second ship kinematics model is a mathematical model based on Newton's second law, describing the relationship between the ship and the external forces acting on it. Its core is to establish the causal relationship between force and changes in motion. The first ship kinematics model is a mathematical model describing the geometric relationship between the ship's motion state (position) and its motion rate (velocity and angular velocity). Its core is to establish the differential relationship between motion state and rate.

[0037] In specific implementation, the step of constructing a ship kinematics model based on the navigation motion parameters of the unmanned surface vessel includes: decomposing the navigation motion parameters to obtain multiple velocity components and multiple pose components; calculating the pose change rate of each velocity component in the inertial coordinate system based on the velocity components and the heading angle in the pose components; determining the transformation matrix from the hull coordinate system to the inertial coordinate system based on the pose change rate; the transformation matrix is ​​used to convert the velocity components into the pose components; and constructing a first ship kinematics model based on the transformation matrix.

[0038] Specifically, the transformation matrix is ​​a mathematical matrix used to describe the coordinate transformation relationship between different coordinate systems. In constructing the first ship kinematics model, the core function of the transformation matrix is ​​to realize the transformation of motion state between the hull coordinate system and the inertial coordinate system.

[0039] In practice, navigation motion parameters of the ship are collected by sensors and decomposed in both the inertial coordinate system and the hull coordinate system to obtain velocity components (longitudinal velocity, lateral velocity, and bow angular velocity) in the hull coordinate system and pose components (position coordinates and heading angle) in the inertial coordinate system. Furthermore, each velocity component is decomposed along the inertial coordinate system using the heading angle. For the longitudinal velocity... , longitudinal speed Decompose along the X and Y axes of the inertial coordinate system to obtain and ; Regarding lateral speed , lateral speed Decompose along the X and Y axes of the inertial coordinate system to obtain and Regarding bow roll rate Directly change the bow roll angular velocity The pose change rate is determined. Based on the mapping relationship between the pose change rate and the velocity components, a transformation matrix is ​​constructed, where each element corresponds to the projection coefficient of the velocity component onto the pose change rate. Finally, the transformation matrix is ​​combined with the velocity and pose components to obtain the first ship kinematic model. The first ship kinematic model can be expressed as:

[0040] ;

[0041] Among them, the The vertical position; The horizontal position; The heading angle; the aforementioned The longitudinal velocity; The lateral velocity; ω is the bow roll angular velocity.

[0042] Optionally, the step of constructing a ship kinematic model based on the navigation motion parameters of the unmanned surface vessel includes: constructing an inertial matrix based on the inertial mass and added mass effect of the unmanned surface vessel in the longitudinal, lateral, and bow directions; constructing a Coriolis force and centripetal force matrix based on velocity components; constructing a damping matrix based on nonlinear damping characteristics; determining an input vector based on the control input torque of the unmanned surface vessel in the longitudinal and bow directions; determining an external disturbance vector based on the unknown time-varying disturbances of the external environment experienced by the unmanned surface vessel in the longitudinal, lateral, and bow directions; and constructing a second ship kinematic model based on Newton's second law, combined with the inertial matrix, the Coriolis force and centripetal force matrix, the damping matrix, the input vector, the external disturbance vector, and the velocity components; the second ship kinematic model characterizes the dynamic relationship between control input, environmental disturbances, and acceleration.

[0043] Specifically, the inertia matrix describes the inertial characteristics of a ship in the longitudinal, lateral, and bow directions, including the ship's own inertial mass and additional mass effects (such as the additional inertial effect of water on the ship's motion). The inertia matrix includes longitudinal and lateral inertial masses (ship mass and water-induced mass), bow moment of inertia (moment of inertia about the vertical axis and additional moment of inertia), and off-diagonal elements reflecting the inertial coupling between different directions of motion (such as the inertial effect of lateral motion on bow roll). The inertia matrix is ​​used to quantify the inertial drag of a ship during motion and is a core parameter in the force-acceleration relationship in the second ship kinematics model.

[0044] The Coriolis force and centripetal force matrix describes the Coriolis force and centripetal force effects during ship motion, reflecting the nonlinear coupling between velocity components. The matrix includes cross-product terms based on longitudinal velocity, lateral velocity, and yaw rate; matrix elements represent the coupling of Coriolis force and centrifugal force in different directions of motion (such as the yaw moment generated by the interaction of longitudinal and lateral velocities). The Coriolis force and centripetal force matrix characterizes the dynamic coupling forces generated by velocity changes during ship rotation or translation, affecting the stability and control characteristics of ship motion.

[0045] The damping matrix is ​​a matrix that describes the nonlinear damping characteristics of a ship during motion, including dissipative forces such as hydrodynamic damping and air resistance. The damping matrix includes longitudinal damping (resistance proportional to longitudinal velocity, such as the frictional resistance between the hull and water), lateral damping (hydrodynamic damping during lateral movement), yaw damping (rotational damping during turning, such as the damping torque generated by the rudder and hull), and nonlinear terms (such as the component of damping force that varies with the square of velocity). The damping matrix is ​​used to quantify energy loss during ship motion and to model the damping and drag characteristics of a ship in different directions.

[0046] The external disturbance vector is a vector describing the unknown, time-varying disturbances experienced by a ship in the longitudinal, lateral, and bow directions due to the external environment. External disturbance vectors include longitudinal disturbances (such as the components of water currents and wind forces in the ship's direction of travel), lateral disturbances (such as the lateral forces generated by crosswinds and cross waves), and bow disturbances (such as the bow moment caused by asymmetric environmental forces, such as unilateral swells). The external disturbance vector is used to characterize the impact of the external environment on the ship's motion.

[0047] In practice, the ship's longitudinal and lateral mass parameters (including its own mass and the mass added by the water flow), as well as its bow roll moment of inertia parameters (including its own moment of inertia and the added moment of inertia), are collected and constructed into an inertia matrix in diagonal form. Further, based on the dynamic formulas of Coriolis force and centripetal force, matrix elements are calculated using longitudinal velocity, lateral velocity, and bow roll angular velocity. Off-diagonal elements consist of cross-product terms of velocity components, resulting in the Coriolis force and centripetal force matrix. By analyzing the ship's damping characteristics in the longitudinal, lateral, and bow roll directions, linear and nonlinear damping coefficients are obtained, and a damping matrix is ​​constructed in diagonal form, with diagonal elements representing combinations of linear and nonlinear damping in each direction. Control inputs are extracted from the navigation motion parameters to obtain the ship's longitudinal and bow roll control input torques, forming an input vector. Longitudinal disturbance force, lateral disturbance force, and bow roll disturbance torque are acquired through sensors, forming an external disturbance vector. Finally, according to Newton's second law, the inertia matrix, Coriolis force and centripetal force matrices, damping matrix, input vector, external disturbance vector, and velocity components are substituted into the dynamic equations to obtain the second ship kinematic model. The second ship kinematic model can be expressed as:

[0048] ;

[0049] Among them, the The longitudinal velocity; The lateral velocity; The bow roll angular velocity; the , , These are the ship's inertial masses in the longitudinal, lateral, and bow roll directions, respectively; , These are the longitudinal and bow roll control input torques of the ship, respectively; , , These refer to the unknown time-varying disturbances from the external environment experienced by the ship in the longitudinal, lateral, and bow directions; These are the nonlinear dynamic terms of the ship in the longitudinal, lateral, and bow roll directions, respectively. , , The , , , , All are nonlinear damping terms.

[0050] S102. Identify obstacles on the initial navigation path and construct a danger zone with the center of the obstacle as the origin.

[0051] Specifically, the danger zone is an extended circular area constructed with the center of the obstacle as the origin, with a radius larger than the original size of the obstacle. The danger zone is used to characterize the safe distance range that ships need to avoid when navigating.

[0052] In specific implementation, the step of identifying obstacles on the initial navigation path and constructing a danger zone with the center of the obstacle as the origin includes: identifying all obstacles on the initial navigation path of the unmanned surface vessel and extracting the geometric parameters of each obstacle; the geometric parameters include the center coordinates of the obstacle and the characteristic size of the obstacle; constructing a circular region with the center of the obstacle as the origin and half of the characteristic size of the obstacle as the first radius; calculating the expansion radius based on the size of the unmanned surface vessel, its maneuverability parameters, and safety margin coefficient; and expanding the circular region radially based on the expansion radius to generate the danger zone.

[0053] Specifically, the initial navigation path is scanned using radar, visual sensors, and other equipment to identify obstacles and obtain the center coordinates (e.g., two-dimensional planar coordinates) and characteristic dimensions (diameter or side length for regularly shaped obstacles; bounding box dimensions for irregularly shaped obstacles) of each obstacle. Further, for each obstacle, a first radius is calculated using the center coordinates as the origin and half of the characteristic dimension, constructing a circular region with the first radius as its radius. Combining the ship's own dimensions (e.g., length and beam), maneuverability parameters (minimum turning radius), and safety margin coefficient, the extended radius is calculated using the extended radius calculation formula. The extended radius calculation formula can be expressed as:

[0054] ;

[0055] Among them, the For the extended radius; the , For the length and width of the ship; the aforementioned The minimum turning radius; The safety margin factor; This represents the uncertainty caused by environmental disturbances.

[0056] Furthermore, the radius of the circular area is expanded from the first radius to an extended radius, and a circular danger zone with the extended radius is generated with the center of the obstacle as the origin.

[0057] For example, It is a circular area of ​​obstacles. To ensure navigational safety, Expand into a larger circular area , as a dangerous area serving as an obstacle; yes , The coordinates of the common center point; , These are the obstacle areas. and The radius. Before the ship has entered... If the vessel is within a safe zone, then it is considered to be in a safe zone; when the vessel enters the circular area... Within this range, there is a risk of collision between the vessel and the obstacle; when the vessel enters... Within this range, a collision between the ship and the obstacle is considered to have occurred. Danger Zone It can be represented as:

[0058] ;

[0059] Among them, the express Any point in the text; let the ideal navigation path in this text be... , wherein For any path variable. ,satisfy In this way, ships can effectively avoid dangerous areas. .

[0060] S103. Calculate the geometric relationship between the initial navigation path and the danger zone based on the ship's kinematics model, and generate an obstacle avoidance navigation path based on the geometric relationship.

[0061] Specifically, the step of calculating the geometric relationship between the initial navigation path and the danger zone based on the ship's kinematics model, and generating an obstacle avoidance navigation path based on the geometric relationship, includes: calculating the geometric relationship between the initial navigation path and each danger zone, and determining whether the initial navigation path crosses a danger zone; when the initial navigation path does not intersect with any danger zone, determining the initial navigation path as an obstacle avoidance navigation path; when the initial navigation path intersects with a single danger zone, determining the intersection point between the initial navigation path and the danger zone, using the center of the obstacle as the projection source point, projecting the intersection point interval within the danger zone in the initial navigation path along the line connecting the projection source point and the path point to the boundary of the danger zone, obtaining an obstacle avoidance navigation path with a minor arc projection path segment; when the initial navigation path intersects with two adjacent danger zones and the two adjacent danger zones overlap, repeating the projection step, adjusting the minor arc based on the interaction between the minor arc projected to the first danger zone and the second danger zone, obtaining an obstacle avoidance navigation path with an adjusted major arc projection path segment; the obstacle avoidance navigation path is located outside any danger zone.

[0062] In practical implementation, the coordinate point sequence of the initial navigation path is obtained using the first ship kinematics model. For each danger zone, the positional relationship between each coordinate point sequence and the danger zone is calculated. The shortest distance from each coordinate point sequence to the center of the circle is calculated and compared with the radius of the danger zone. If all shortest distances are greater than the radius, the initial navigation path does not cross the danger zone; otherwise, it is determined that the initial navigation path intersects with the danger zone. When it is detected that the initial navigation path has no intersection with any danger zones, the initial navigation path is directly used as the obstacle avoidance navigation path without adjustment. When it is detected that the initial navigation path intersects with a single danger zone, the intersection point between the initial navigation path and the boundary of the danger zone is first solved, resulting in two intersection points, which determine the intersection point interval of the initial navigation path within the danger zone. Further, with the obstacle center as the projection source point, the path points within the intersection point interval are projected onto the boundary of the danger zone along the line connecting the projection source point and the path points, generating a minor arc as the obstacle avoidance path segment, replacing the intersection point interval of the original path. When the initial navigation path is detected to intersect with adjacent overlapping danger zones, for the adjacent first and second danger zones, the intersection interval of the initial navigation path within the first danger zone is first projected onto the boundary corresponding to the center of the first danger zone, resulting in a minor arc segment. By checking whether the minor arc segment intersects with the second danger zone, if they do, the center of the second danger zone is used as the projection source point, and the portion of the minor arc segment intersecting with the second danger zone is projected again onto the boundary of the second danger zone. By extending the projection path or selecting a superior arc that bypasses the overlapping portion of the two danger zones, continuous obstacle avoidance path segments are generated, ensuring that the final path is completely outside all danger zones.

[0063] For example, when the initial navigation path No crossing of dangerous areas At that time, that is , ;when Time travel At that time, that is In order to achieve obstacle avoidance and ensure that the obstacle avoidance navigation path is continuous, the vessel must perform the following procedures when entering and leaving the area. The location must meet the following conditions: , Among them, the aforementioned , For obstacle avoidance navigation path; the , This is the initial navigation path; This is the entrance port for entering the hazardous area; This is the exit port for leaving the danger zone.

[0064] When the ship enters At that time, Within range Partial path, along vector Direction projection to Generate obstacle avoidance navigation paths on the boundaries. . It can be calculated using the following formula:

[0065] ;

[0066] Among them, the , The projection angle variable; This is the entrance port for entering the hazardous area; The center of the obstacle; The This is the initial navigation path; The for The maximum value.

[0067] when In one possible implementation, the initial navigation path Crossing any adjacent danger zone and There is no intersection, that is Ships in order to avoid dangerous areas The generated obstacle avoidance navigation path will not enter areas where the vehicle is bound by obstacles. Adjacent danger zones Inside, the generated obstacle avoidance navigation path The obstacle avoidance task can be completed, and the obstacle avoidance navigation path is as follows. It can be calculated using the following formula:

[0068] ;

[0069] Among them, the For obstacle avoidance navigation path; the The center of the obstacle; The The , , The value of is based on the following formula:

[0070] ;

[0071] Among them, the The x-coordinate of the inlet port; The x-coordinate of the obstacle's center; The ordinate of the inlet port; The vertical coordinate of the obstacle's center; , The projection angle variable; The x-coordinate of the output port; The vertical coordinate of the output port.

[0072] In another possible implementation, when the initial navigation path Crossing any adjacent danger zone and There is an intersection, that is Ships in order to navigate dangerous areas During obstacle avoidance, the generated obstacle avoidance navigation path may enter areas where obstacles are encountered. Adjacent obstacle danger zone However, this can lead to the obstacle avoidance navigation path obtained by the above methods not being able to avoid all obstacles. In this case, the obstacle avoidance navigation path... It can be calculated using the following formula:

[0073] ;

[0074] Among them, the For obstacle avoidance navigation path; the For superior arc; the It is a minor arc; the aforementioned The shortest path; The radius of the danger zone; The center of the obstacle; The The , For the projection angle variable, the , The value of is based on the following formula:

[0075] ;

[0076] Among them, the , , , , For the projection angle variable, the and The calculation method and The calculation method is the same; the The x-coordinate of the inlet port; The x-coordinate of the output port; The ordinate of the inlet port; This represents the ordinate of the exit port. The obstacle avoidance navigation path can be calculated. middle The angle range is The ,but middle The angle range is , and The mapping relationship is as follows: .

[0077] S104. Determine the overlap of multiple danger zones, and based on the positional relationship of each obstacle and the geometric characteristics of the obstacle avoidance navigation path, determine the interaction relationship between the obstacle avoidance navigation path and multiple danger zones, and adjust the obstacle avoidance navigation path based on the interaction relationship.

[0078] Specifically, during ship navigation, multiple hazard areas may overlap due to the proximity of obstacles, causing the obstacle avoidance navigation path to enter adjacent hazard areas while bypassing a single hazard area. For example, after the ship projects an obstacle avoidance path to generate a minor arc path around the first hazard area, this path may intersect with an adjacent overlapping hazard area. In this case, obstacle avoidance treatment targeting only a single hazard area cannot guarantee overall navigational safety; the path must be further adjusted based on the actual interaction between the obstacle avoidance navigation path and multiple hazard areas.

[0079] In specific implementation, determining the overlap of multiple danger zones, based on the positional relationship of each obstacle and the geometric characteristics of the obstacle avoidance navigation path, and judging the interaction relationship between the obstacle avoidance navigation path and the multiple danger zones, and adjusting the obstacle avoidance navigation path based on the interaction relationship, includes: calculating the distance between each point on the obstacle avoidance navigation path and the boundaries of the multiple danger zones; determining the interaction relationship between the obstacle avoidance navigation path and the multiple danger zones based on the distance; when the interaction relationship is that the obstacle avoidance navigation path does not enter any danger zone, for each danger zone, determining the intersection point of the obstacle avoidance navigation path and the boundary of the danger zone, and using the minor arc enclosed by the intersection point on the circumference of the danger zone as the projection path; when the interaction relationship is that the obstacle avoidance navigation path enters a target danger zone, for each target danger zone, using the major arc enclosed by the intersection point on the circumference of the target danger zone as the projection path; and splicing the non-projected portion of the obstacle avoidance navigation path based on the projection path to obtain the adjusted obstacle avoidance navigation path.

[0080] Specifically, for each discrete point on the obstacle avoidance navigation path, its distance to the center of each danger zone is calculated and compared with the radius of the danger zone to obtain a set of distances. If the distance of all path points is greater than the radius of the danger zone, the interaction relationship is "not entering any danger zone"; if the distance of at least one path point is not greater than the radius of the danger zone, the interaction relationship is "entering the target danger zone". When the interaction relationship is determined to be that the obstacle avoidance navigation path has not entered any danger zone, for each danger zone, the intersection point between the obstacle avoidance navigation path and the boundary of each danger zone is calculated to obtain the intersection point. Taking the center of the obstacle as the vertex, the central angle formed by the intersection point on the circumference is calculated. If the central angle is less than 180°, the minor arc is taken as the projected path segment, replacing the straight line segment in the intersection point interval of the original path.

[0081] When the interaction relationship is determined to be that the obstacle avoidance navigation path enters any target danger zone, the intersection point of the obstacle avoidance navigation path entering and leaving the target danger zone is determined, and the central angle formed by the two intersection points on the circumference is calculated. If the central angle is greater than 180°, the dominant arc (the long arc that bypasses the outside of the danger zone) is taken as the projected path segment, replacing the line segments in the intersection interval of the original path. Furthermore, the projected path segments (minor or dominant arcs) corresponding to each danger zone are sequentially spliced ​​with the non-projected part of the obstacle avoidance navigation path (i.e., the path segments that do not intersect with the danger zone) to form a continuous adjusted obstacle avoidance navigation path.

[0082] For example, combining the above description, when At that time, the For projection angle variables:

[0083] In one possible implementation, when adjacent danger zones do not overlap, i.e. The adjusted obstacle avoidance navigation path can be calculated using the following formula:

[0084] ;

[0085] Among them, the The center of the obstacle in the current danger zone; The center of the obstacle in the adjacent danger zone; The radius of the current danger zone; The radius of the adjacent danger zone; The adjusted obstacle avoidance navigation path; For superior arc.

[0086] In another possible implementation, when adjacent danger zones overlap, i.e. Obstacle avoidance navigation path It can be calculated using the following formula:

[0087] ;

[0088] Among them, the The adjusted obstacle avoidance navigation path; For superior arc; the It is a minor arc; the aforementioned The center of the obstacle in the current danger zone; The radius of the current danger zone; This is the shortest path.

[0089] S105. Based on the minimum turning radius of the ship, detect the curvature of the obstacle avoidance navigation path at the entrance and exit ports. When the curvature does not meet the turning capability requirements, generate an optimized tangent circle path at the entrance and exit ports that is tangent to the boundary of the danger zone. Combine the paths to obtain the optimized obstacle avoidance navigation path.

[0090] Specifically, detecting and optimizing the curvature of the obstacle avoidance navigation path at the entry and exit points is to ensure that the vessel meets its maneuverability limitations during actual navigation, avoiding situations where excessive curvature of the obstacle avoidance navigation path prevents the vessel from turning and encountering dangerous areas. Since the ship's kinematic model dictates a minimum turning radius, if the path curvature is too small (i.e., the turning radius is less than the minimum permissible value), the vessel cannot physically turn, potentially causing its trajectory to deviate from the obstacle avoidance navigation path or even enter a dangerous area. By detecting curvature, sections of the obstacle avoidance navigation path that do not meet the vessel's maneuverability requirements (such as turns) can be identified in advance.

[0091] In specific implementation, the step of generating a tangent circle optimized path tangent to the boundary of the danger zone at the entry / exit port when the curvature does not meet the turning capability requirements includes: determining the tangent circle radius based on the turning circle diameter of the unmanned surface vessel; matching the corresponding tangent circle center calculation method according to the tangent circle type, and calculating the tangent circle center based on the tangent circle center calculation method and the center point of the area where the obstacle avoidance navigation path is located; constructing a tangent circle equation with the tangent circle center as the origin and the tangent circle radius as the radius, and simultaneously solving the tangent circle equation and the boundary equation of the danger zone to calculate the tangent point coordinates; determining relevant symbols based on the size relationship between the tangent point coordinates and the tangent circle center coordinates, and determining a first vector parameter based on the relevant symbols; calculating angle parameters based on the distance between the tangent point and the tangent circle center, and the distance between different tangent points; and generating a tangent circle optimized path based on the tangent circle center, the first vector parameter, and the angle parameter.

[0092] Specifically, based on the ship's turning radius (the minimum turning diameter when the ship turns at full speed and with full rudder), the tangent circle radius is calculated, and this radius is not less than the minimum turning radius. Further, based on the positional relationship between the obstacle avoidance navigation path and the danger zone, the corresponding tangent circle type (inscribed circle or circumscribed circle) is determined. Using the center point of the area where the obstacle avoidance navigation path is located as a reference, and combining the center and radius of the danger zone, the coordinates of the tangent circle's center are calculated through geometric relationships. Using the tangent circle's center as the origin and the tangent circle's radius, a tangent circle equation is constructed, and the danger zone boundary equation is simultaneously solved. The coordinates of the tangent points of the two circles are then obtained using the geometric projection method. By comparing the coordinates of the tangent circle's center and the tangent points, the vector direction, i.e., the first vector parameter, is determined. The angle parameter is calculated by calculating the distance between the tangent point and the tangent circle's center, as well as the distances between different tangent points. Finally, using the tangent circle's center as the center, and based on the central angle calculated using the direction determined by the first vector parameter and the angle parameter, an arc path between different tangent points is generated, resulting in the optimized tangent circle path.

[0093] Based on the above description, the center of the tangent circle can be calculated using the following formula:

[0094] ;

[0095] Among them, the The center of the tangent circle; The center point of the area where the obstacle avoidance navigation path is located; This indicates that the tangent circle is the external tangent circle. This indicates that the tangent circle is the incircle.

[0096] The first vector parameter can be calculated using the following formula:

[0097] ;

[0098] Among them, the Let x be the x-coordinate of the center of the tangent circle; the stated The x-coordinate of the tangent point; The ordinate of the center of the tangent circle; The ordinate of the point of tangency is denoted by y.

[0099] The angle parameter can be calculated using the following formula:

[0100] ;

[0101] Among them, the , The tangent point; the The center of the tangent circle; For angle parameters.

[0102] The optimized path for tangent circles can be calculated using the following formula:

[0103] ;

[0104] Among them, the Optimize the path for tangent circles; The center of the tangent circle; The first vector parameter, the The For angle parameters.

[0105] S106. Based on the finite-time theory, design an integral line-of-sight guidance law, and combine it with the ship kinematics model to generate the desired navigation heading angle according to the obstacle avoidance navigation optimization path.

[0106] Specifically, the step of designing an integral line-of-sight guidance law based on finite-time theory, combined with the ship's kinematics model, and generating a desired navigation heading angle based on the obstacle avoidance navigation optimization path includes: acquiring the coordinate point sequence of the obstacle avoidance navigation optimization path and the current position and heading angle of the unmanned surface vessel; calculating the lateral deviation between the current position and the coordinate point sequence; designing control rules based on the finite-time convergence principle, based on the lateral deviation and the cumulative value, and generating an instruction to adjust the heading angle; and converting the instruction into a desired navigation heading angle through integral calculation.

[0107] In practical implementation, a series of discrete coordinate points are first extracted from the obstacle avoidance navigation optimization path to form an ordered sequence of path points. Simultaneously, the ship's current position coordinates and heading angle are acquired in real time using sensors onboard the vessel. Further, using the current ship position as a reference point, for each target point in the path point sequence, the lateral deviation between the ship's current position and the target point is calculated based on the line-of-sight coordinate system principle. Lateral deviation refers to the ship's position offset in the direction perpendicular to the path, used to quantify the degree and direction of the ship's deviation from the path. For example, if the path is a straight line, the lateral deviation can be understood as the vertical distance from the ship to the straight line; if the path is a curve, the vertical offset needs to be calculated based on the local tangent direction. Further, the lateral deviation calculated in the first step is accumulated over time to obtain the cumulative value of the deviation. Based on the finite-time convergence principle, and combining the real-time and cumulative values ​​of the lateral deviation, a proportional-integral control rule is designed. The control rule generates corresponding heading angle adjustment commands based on the magnitude and trend of the lateral deviation. For example, when the lateral deviation is large and the cumulative value continues to increase, the control rules will output a larger adjustment command, prompting the ship to turn at a faster speed and shorten the time to return to the path. When the deviation is small and the cumulative value gradually decreases, the adjustment command will decrease accordingly to avoid over-turning. Finally, the heading angle adjustment command generated in the second step is integrated over time and converted into a specific desired navigation heading angle. This transforms the instantaneous adjustment command into an angle value that changes continuously over time, making the ship's turning maneuvers smooth and controllable. During the integration process, the desired navigation heading angle is dynamically adjusted based on the geometric characteristics of the obstacle avoidance navigation optimization path. For example, when the path is a straight segment, the desired navigation heading angle remains constant to ensure the ship travels in a straight line; when the path is a curved segment, the desired navigation heading angle changes in real time according to the radius of curvature of the path and the turning direction to generate the desired navigation heading angle.

[0108] For example, based on the description above, longitudinal deviation and lateral deviation can be defined using the following formulas:

[0109] ;

[0110] Among them, the The current position of the vessel; For longitudinal deviation; the For lateral deviation; the The tangent angle for optimizing the obstacle avoidance navigation path, the The , ;

[0111] , About The first derivative can be expressed as the following formula:

[0112] ;

[0113] Among them, the The combined speed of the ship; The sideslip angle; The For longitudinal deviation; the For lateral deviation; the The tangent angle for optimizing the path in obstacle avoidance navigation; The longitudinal velocity; The lateral velocity; This is the heading angle.

[0114] The FTILOS guidance law can be expressed as the following formula:

[0115] ;

[0116] Among them, the The desired navigation heading angle; The tangent angle for optimizing the path in obstacle avoidance navigation; The sideslip angle; For lateral deviation; the The guidance integral term represents the distance between a virtual path and the actual navigation path; The vector length is the forward look-ahead distance; The combined speed of the ship; For longitudinal deviation; the , , These are the design parameters that are greater than zero.

[0117] It should be noted that the update speed It can be used to design virtual control laws to reduce longitudinal deviation. Stay calm. Therefore, the finite-time theory will be introduced. Designed as follows:

[0118] ;

[0119] Among them, the For update speed; the The combined speed of the ship; The vector length is the forward look-ahead distance; For lateral deviation; the The parameters to be designed; For guidance integral term; the For longitudinal deviation; the , These are the parameters to be designed.

[0120] Furthermore, the performance analysis of FTILOS using Lyapunov preselection functions can be expressed as follows:

[0121] ;

[0122] Among them, the These are the constructed Lyapunov preselected functions; the... For longitudinal deviation; the For lateral deviation; the The sideslip angle; The parameter to be designed is greater than zero; This is the guidance integral term.

[0123] right Find its derivative and substitute it into the above formula to get:

[0124] ;

[0125] Among them, the These are the constructed Lyapunov preselected functions; the... , , , The parameter to be designed is greater than zero; For longitudinal deviation; the For lateral deviation; the For guidance integral term; the The combined speed of the ship; The vector length is the forward look-ahead distance; It is the sideslip angle.

[0126] By introducing parameters , The above formula can be rewritten as:

[0127] ;

[0128] Among them, the These are the constructed Lyapunov preselected functions; the... , , , The parameter to be designed is greater than zero; For longitudinal deviation; the For lateral deviation; the For guidance integral term; the The combined speed of the ship; The vector length is the forward look-ahead distance; Sideslip angle; ; .

[0129] Based on Young's inequality, the above formula can be derived as follows:

[0130] ;

[0131] Among them, the These are the constructed Lyapunov preselected functions; the... , , , The parameter to be designed is greater than zero; For longitudinal deviation; the For lateral deviation; the For guidance integral term; the The combined speed of the ship; The vector length is the forward look-ahead distance; Sideslip angle; ; .

[0132] Among them, the and It can be calculated using the following formula:

[0133] ;

[0134] Among them, the , , The parameter to be designed is greater than zero; .

[0135] ;

[0136] in, ; The , , These are the design parameters that are greater than zero.

[0137] S107. Construct a navigation tracking adjustment module, which outputs navigation tracking commands based on the ship kinematics model, the desired navigation heading angle, and the ship's real-time motion state.

[0138] Specifically, the navigation tracking adjustment module, based on the ship's kinematics model, the desired navigation heading angle, and the ship's real-time motion state, outputs navigation tracking commands. This includes: inputting the desired navigation heading angle and the ship's real-time motion state into the ship's kinematics model and calculating the error variable between the actual state and the desired state; constructing a navigation tracking adjustment module comprising an input layer, a hidden layer, and an output layer; wherein the input layer receives the error variable and ship kinematics model parameters, the hidden layer approximates unknown disturbances in ship dynamics using a nonlinear activation function, and the output layer generates virtual control quantities; using an adaptive algorithm to adjust the connection weights between the hidden layer and the output layer in the navigation tracking adjustment module in real time, optimizing the weight parameters based on the ship's actual response and the error feedback output by the navigation tracking adjustment module; and fusing the virtual control quantities output by the navigation tracking adjustment module with the theoretical control quantities calculated by the ship's kinematics model to generate navigation tracking commands.

[0139] In practice, the desired navigation heading angle and real-time ship motion data are first acquired, including the actual heading angle, longitudinal velocity, and bow roll rate. The acquired data is then input into the ship's kinematics model, which calculates the difference between the actual and desired states. This includes calculating the difference between the actual and desired heading angles (heading angle error) and the difference between the actual and target longitudinal velocity (velocity error). Furthermore, a three-layer navigation tracking and adjustment module is constructed: the input layer receives two types of data: first, the error variables calculated in the previous step (such as bow angle error and velocity error); second, the relevant parameters of the ship's kinematics model (such as ship mass, moment of inertia, and hydrodynamic coefficients). This input data provides the initial calculation basis for the navigation tracking and adjustment module. The hidden layer processes the input data using nonlinear activation functions (such as the Sigmoid function or the ReL function). By utilizing the mapping capability of nonlinear functions, it approximates unknown disturbances in ship dynamics that are difficult to describe precisely using mathematical formulas, such as changes in fluid resistance, and interference from environmental wind or wave forces. The output layer generates virtual control quantities based on the calculation results of the hidden layer, such as the desired thrust (the magnitude of thrust required by the propeller) and the desired rudder angle change rate (the speed at which the rudder angle is adjusted by the rudder). These virtual control quantities are the preliminary calculation results of the navigation tracking and adjustment module for the ship's control input, and do not yet consider the theoretical calculation values ​​and physical constraints of the model. An adaptive algorithm (such as gradient descent) is used to calculate the error between the virtual control output of the navigation tracking and adjustment module and the actual ship response in real time. After inputting the virtual control output into the ship's actuators, the actual ship motion state (such as actual bow angle changes and speed changes) is collected and compared with the expected response of the navigation tracking and adjustment module to obtain an error feedback signal. Based on this error feedback signal, the connection weights between the hidden layer and the output layer are adjusted: if the error is large, the weight adjustment range is increased to allow the navigation tracking and adjustment module to adapt quickly; if the error is small, the adjustment range is decreased to avoid over-adjustment. Through continuous iteration of this process, the weight parameters are optimized, enabling the navigation tracking and adjustment module to dynamically compensate for the uncertainty of the ship's kinematic model, improving its adaptability to changes in the ship's motion state and control accuracy. Finally, the virtual control output of the navigation tracking and adjustment module is fused with the control output obtained from the ship's kinematic model based on theoretical calculations. For example, the ship kinematics model can calculate the theoretically required control quantities (such as theoretical thrust and theoretical rudder angle) based on Newton's second law and ship force analysis, while the navigation tracking adjustment module outputs virtual control quantities based on real-time error and learning. The two are fused by weighted averaging or linear combination, taking into account the theoretical predictions of the model and the real-time learning results of the navigation tracking adjustment module, to generate the final actual navigation tracking command.After generating navigation tracking commands, physical constraint verification is required. For example, check whether the propeller thrust exceeds its maximum output capacity and whether the rudder angle is within the mechanically permissible travel range. If the constraints are exceeded, the navigation tracking commands are corrected to ensure that the commands can be executed safely and effectively by the ship's actuators.

[0140] Based on the above description, the error variable is defined as:

[0141] ;

[0142] ;

[0143] ;

[0144] Among them, the , and These are the desired longitudinal velocity, desired navigation heading angle, and desired bow roll rate, respectively; and They can be regarded as respectively and The virtual control law; , , For error variables; the The longitudinal velocity; The bow roll angular velocity; the This is the heading angle.

[0145] Combining the kinematic model and the calculation formula of the guidance law, the virtual control law for the longitudinal and yaw angular velocities is designed as follows:

[0146] ;

[0147] ;

[0148] Among them, the For vertical virtual control law; the The virtual control law for bow roll rate; The maximum longitudinal speed of the ship; The minimum desired longitudinal speed of the ship; , , , The design parameter is positive; For lateral deviation; the The desired heading angle; This represents the heading angle error.

[0149] To address the external time-varying disturbances and model dynamic uncertainties encountered by ships during path tracking in obstacle avoidance scenarios, longitudinal velocity control laws are designed respectively. and bow roll rate control law :

[0150] ;

[0151] The adaptive law is designed as follows:

[0152] ;

[0153] Among them, the For longitudinal velocity control law; the The bow roll rate control law; , , , , , , , , , All are design parameters that are greater than 0; the and All are known scalar functions; and These are the radial basis function vectors of the two navigation tracking adjustment modules, respectively. and These represent the number of nodes in the two navigation tracking adjustment modules, respectively; and These are the input vectors for the two navigation tracking adjustment modules, respectively; and For virtual parameters greater than 0, the and These are the approximation errors of the two navigation tracking adjustment modules, respectively. , The and They are respectively and The maximum value.

[0154] Using virtual parameter learning techniques, we obtain:

[0155] ;

[0156] Among them, the and For virtual parameters greater than 0, the and These are the approximation errors of the two navigation tracking adjustment modules, respectively. , The and They are respectively and The maximum value; the and All are known scalar functions; and These are the radial basis function vectors of the two navigation tracking adjustment modules, respectively. and These represent the number of nodes in the two navigation tracking adjustment modules, respectively; and .

[0157] To avoid virtual control laws The computational complexity caused by direct differentiation is addressed using dynamic surface techniques to obtain... The first derivative information is introduced into a first-order filter to obtain:

[0158] ;

[0159] Among them, the It is a first-order filter; the The filtering error is a time constant greater than 0. satisfy The The The For a continuous bounded function, its upper bound is defined as follows: The This is a virtual control law.

[0160] Based on the ship kinematics model, the error variables and Differentiating each, we get:

[0161] ;

[0162] Among them, the , For error variables; the , Let be an unknown function. By approximating the two unknown functions respectively, we obtain:

[0163] ;

[0164] Among them, the and These are the radial basis function vectors of the two navigation tracking adjustment modules, respectively. and These represent the number of nodes in the two navigation tracking adjustment modules, respectively; and These are the approximation errors of the two navigation tracking adjustment modules, respectively. , The and They are respectively and The maximum value.

[0165] By constructing Lyapunov preselection functions and performing performance analysis on the neural network controller, we obtain the following results:

[0166] ;

[0167] Among them, the For Lyapunov preselected functions; the , For error variables; the For longitudinal velocity control law; the For filtering error; the and The virtual parameter is greater than 0; , The parameters to be designed; This refers to the longitudinal inertial mass of the ship.

[0168] Differentiating the above formula, we get:

[0169] ;

[0170] Among them, the For Lyapunov preselected functions; the , , , , , , , All are design parameters that are greater than 0; the , , All are control parameters greater than 0; The longitudinal inertial mass of the ship; , For error variables; the For longitudinal velocity control law; the and The virtual parameter is greater than 0; This represents the upper bound of the filter error.

[0171] We obtain the following from Young's inequality:

[0172] ;

[0173] Among them, the For filtering error; the and The virtual parameter is greater than 0; The longitudinal inertial mass of the ship; , , All are control parameters greater than 0; , , All are control parameters that are greater than 0.

[0174] Substituting the above formula into Young's inequality, we get:

[0175] ;

[0176] in, , and It is calculated using the following formula:

[0177] ;

[0178] ;

[0179] ;

[0180] The stability proof must satisfy the following conditions. , , Then, appropriate design parameters must be selected to make , , .

[0181] ;

[0182] Among them, the for The initial value. It is uniformly and ultimately bounded, therefore, the stated , , , , , It is also bounded; by , , Given the boundedness of the structure, the actual position of the ship is... and the ship's bow angle All are bounded; based on the finite-time stability proof of all signals in the FTILOS guidance system, the boundedness of the USV's lateral tracking error indicates that the longitudinal velocity virtual control law is bounded. Therefore, the ship's forward speed... and bow roll rate It is also bounded, and thus the control law can be derived. and Both are bounded.

[0183] S108. Based on the navigation tracking command, drive the unmanned surface vessel to track the obstacle avoidance navigation optimization path.

[0184] In practice, the final navigation tracking commands, including propeller thrust commands and rudder angle commands, are obtained from the navigation tracking and adjustment module. These commands are then parsed to extract control parameters for different actuators. For example, the thrust command is converted into the target propeller speed or motor power, and the rudder angle command is converted into the target deflection angle and rotation speed of the servo motor. These commands are then transmitted to the corresponding actuator controllers (such as the propulsion system controller and servo motor controller) via the ship's internal control bus. Finally, based on the control parameters of each actuator, the actuators are controlled to steer the ship to track the optimized obstacle avoidance navigation path.

[0185] The method provided in this embodiment, firstly, by constructing an accurate ship kinematic model, ensures that obstacle avoidance navigation not only considers the ship's trajectory but also integrates factors such as the ship's turning ability, acceleration limitations, and environmental disturbances, thereby improving the physical feasibility of the obstacle avoidance navigation path. Simultaneously, by introducing obstacle centers to construct hazardous areas, the method fully considers the characteristic dimensions of obstacles and the ship's maneuverability, reasonably expanding the safety buffer zone, effectively preventing path crossing risks, and improving obstacle avoidance safety margins. Secondly, this application is not limited to single obstacle handling but can automatically identify the overlapping relationships between multiple hazardous areas and dynamically adjust them based on the interaction between the initial path and multiple hazardous areas, achieving local optimization and coherent reconstruction of the path. Employing minor / major arc projection and tangent circle path design, it balances obstacle avoidance performance with path curvature smoothness, ensuring that the path has a continuous and feasible geometric structure while satisfying obstacle avoidance constraints. Furthermore, by utilizing the integral line-of-sight guidance law based on finite-time convergence theory, yaw can be adjusted to the desired heading in a short time, improving response speed and robustness. Thirdly, this application introduces a navigation tracking and adjustment module, enabling the control system to self-learn and compensate for unknown dynamic modeling errors and external disturbances when facing complex, time-varying nonlinear disturbances in the marine environment. The controller integrates the desired navigation heading angle with the real-time motion state to achieve coordinated output of virtual and theoretical control, ensuring that the unmanned surface vessel can smoothly and accurately track obstacle avoidance paths under different operating conditions, thereby improving the overall intelligence and stability of navigation.

[0186] Corresponding to the aforementioned embodiment of the unmanned surface vessel obstacle avoidance navigation path generation and tracking method, this application also provides an embodiment of an unmanned surface vessel obstacle avoidance navigation path generation and tracking device.

[0187] Figure 2 This is a schematic diagram of the unmanned surface vessel obstacle avoidance navigation path generation and tracking device provided in Embodiment 2 of this application. Please refer to... Figure 2 The apparatus provided in this embodiment includes a construction module 210, a generation module 220, an adjustment module 230, a combination module 240, a determination module 250, and a driving module 260.

[0188] The construction module 210 is used to construct a ship kinematic model based on the navigation motion parameters of the unmanned surface vessel.

[0189] The construction module 210 is also used to identify obstacles on the initial navigation path and construct a danger zone with the center of the obstacle as the origin;

[0190] The generation module 220 is used to calculate the geometric relationship between the initial navigation path and the danger zone based on the ship's kinematics model, and generate an obstacle avoidance navigation path based on the geometric relationship.

[0191] The adjustment module 230 is used to determine the overlap of multiple dangerous areas, and based on the positional relationship of each obstacle and the geometric characteristics of the obstacle avoidance navigation path, to determine the interaction relationship between the obstacle avoidance navigation path and multiple dangerous areas, and to adjust the obstacle avoidance navigation path based on the interaction relationship.

[0192] The combined module 240 is used to detect the curvature of the obstacle avoidance navigation path at the entrance and exit ports based on the minimum turning radius of the ship. When the curvature does not meet the turning capability requirements, it generates a tangent circle optimized path segment at the entrance and exit ports that is tangent to the boundary of the danger zone, and combines them to obtain an optimized obstacle avoidance navigation path.

[0193] The generation module 220 is also used to design an integral line-of-sight guidance law based on the finite-time theory, and generate the desired navigation heading angle according to the obstacle avoidance navigation optimization path in combination with the ship kinematics model.

[0194] The determining module 250 is used to construct a navigation tracking adjustment module, and output navigation tracking commands based on the ship kinematics model, the desired navigation heading angle and the real-time motion state of the ship;

[0195] The drive module 260 is used to drive the unmanned surface vessel to follow the obstacle avoidance navigation optimization path based on the navigation tracking command.

[0196] The apparatus of this embodiment can be used to perform... Figure 1The steps of the method embodiment shown are similar in principle and process, and will not be repeated here.

[0197] The specific implementation process of the functions and roles of each unit in the above device can be found in the implementation process of the corresponding steps in the above method, and will not be repeated here.

[0198] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to in the description of the method embodiments. The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this application according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0199] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. An unmanned surface vehicle obstacle avoidance navigation path generation and tracking method, characterized in that, The method includes: Based on the navigation motion parameters of the unmanned surface vessel, a kinematic model of the vessel is constructed; Identify obstacles on the initial navigation path and construct a danger zone with the center of the obstacle as the origin; The geometric relationship between the initial navigation path and the danger zone is calculated based on the ship's kinematics model, and an obstacle avoidance navigation path is generated based on the geometric relationship. Determine the overlap of multiple danger zones, and based on the positional relationship of each obstacle and the geometric characteristics of the obstacle avoidance navigation path, determine the interaction relationship between the obstacle avoidance navigation path and multiple danger zones, and adjust the obstacle avoidance navigation path based on the interaction relationship; Based on the minimum turning radius of the ship, the curvature of the obstacle avoidance navigation path at the entrance and exit ports is detected. When the curvature does not meet the turning capability requirements, an optimized tangent circle path tangent to the boundary of the danger zone is generated at the entrance and exit ports. The paths are combined to obtain the optimized obstacle avoidance navigation path. Based on the finite-time theory, an integral line-of-sight guidance law is designed. Combined with the ship kinematics model, the desired navigation heading angle is generated according to the obstacle avoidance navigation optimization path. A navigation tracking adjustment module is constructed, which outputs navigation tracking commands based on the ship kinematics model, the desired navigation heading angle, and the ship's real-time navigation status; The navigation tracking command drives the unmanned surface vessel to follow the obstacle avoidance navigation optimization path. The process of determining the overlap of multiple danger zones, judging the interaction relationship between the obstacle avoidance navigation path and multiple danger zones based on the positional relationship of each obstacle and the geometric characteristics of the obstacle avoidance navigation path, and adjusting the obstacle avoidance navigation path based on the interaction relationship includes: Calculate the distance between each point on the obstacle avoidance navigation path and the boundaries of multiple danger zones, and determine the interaction relationship between the obstacle avoidance navigation path and the multiple danger zones based on the distance; When the interaction relationship is that the obstacle avoidance navigation path does not enter any dangerous area, for each dangerous area, the intersection point of the obstacle avoidance navigation path and the boundary of the dangerous area is determined, and the minor arc enclosed by the intersection point on the circumference of the dangerous area is used as the projection path. When the interaction relationship is that the obstacle avoidance navigation path enters the target danger area, for each target danger area, the superior arc enclosed by the intersection point on the circumference of the target danger area is used as the projection path. The adjusted obstacle avoidance navigation path is obtained by stitching together the non-projected portion of the obstacle avoidance navigation path with the projected path.

2. The method of claim 1, wherein, The process of constructing a ship kinematic model based on the navigation motion parameters of the unmanned surface vessel includes: The navigation motion parameters are decomposed to obtain multiple velocity components and multiple pose components; Based on the velocity components and the heading angle in the pose components, calculate the pose change rate of each velocity component in the inertial coordinate system. Based on the pose change rate, a transformation matrix is ​​determined from the hull coordinate system to the inertial coordinate system; the transformation matrix is ​​used to convert the velocity component into the pose component. Based on the transformation matrix, a first ship kinematics model is constructed.

3. The method of claim 1, wherein, The process of constructing a ship kinematic model based on the navigation motion parameters of the unmanned surface vessel includes: Based on the inertial mass and added mass effect of the unmanned surface vessel in the longitudinal, lateral, and bow directions, an inertial matrix is ​​constructed. Based on the velocity components, construct the Coriolis force and centripetal force matrices; Based on the nonlinear damping characteristics, a damping matrix is ​​constructed; The input vector is determined based on the control input torque of the unmanned surface vessel in the longitudinal and bow directions, and the external disturbance vector is determined based on the unknown time-varying disturbance of the external environment experienced by the unmanned surface vessel in the longitudinal, lateral and bow directions. Based on Newton's second law, and combining the inertia matrix, the Coriolis force and centripetal force matrix, the damping matrix, the input vector, the external disturbance vector, and the velocity components, a second ship kinematic model is constructed; the second ship kinematic model characterizes the dynamic relationship between control input, environmental disturbance, and acceleration.

4. The method of claim 1, wherein, The process of identifying obstacles on the initial navigation path and constructing a danger zone with the center of the obstacle as the origin includes: Identify all obstacles on the initial navigation path of the unmanned surface vessel and extract the geometric parameters of each obstacle; the geometric parameters include the center coordinates of the obstacle and the feature dimensions of the obstacle. A circular region is constructed with the center of the obstacle as the origin and half the feature size of the obstacle as the first radius; The extended radius is calculated based on the unmanned surface vessel's dimensions, maneuverability parameters, and safety margin coefficient. The circular region is expanded radially based on the expansion radius to generate a danger zone.

5. The method of claim 1, wherein, The calculation of the geometric relationship between the initial navigation path and the danger zone based on the ship's kinematics model, and the generation of an obstacle avoidance navigation path based on the geometric relationship, includes: Calculate the geometric relationship between the initial navigation path and each danger zone to determine whether the initial navigation path crosses a danger zone; When the initial navigation path does not intersect with any danger zone, the initial navigation path is determined as an obstacle avoidance navigation path; When the initial navigation path intersects with a single danger zone, the intersection point of the initial navigation path and the danger zone is determined. Taking the center of the obstacle as the projection source point, the intersection point interval in the initial navigation path located in the danger zone is projected to the boundary of the danger zone along the line connecting the projection source point and the path point to obtain the obstacle avoidance navigation path of the minor arc projection path segment. When the initial navigation path intersects with two adjacent danger zones and the two adjacent danger zones overlap, the projection step is repeated. The minor arc is adjusted based on the interaction between the minor arc projected to the first danger zone and the second danger zone to obtain the obstacle avoidance navigation path of the adjusted major arc projection path segment; the obstacle avoidance navigation path is located outside any danger zone.

6. The method according to claim 1, characterized in that, The step of generating an optimized tangent circle path tangent to the boundary of the danger zone at the entry / exit port when the curvature does not meet the steering capability requirements includes: The radius of the tangent circle is determined based on the turning diameter of the unmanned surface vessel; The center of the tangent circle is calculated based on the tangent circle type and the corresponding tangent circle center calculation method, and the center point of the area where the obstacle avoidance navigation path is located. With the center of the tangent circle as the origin and the radius of the tangent circle as the radius, construct the equation of the tangent circle, and solve the equation of the tangent circle and the boundary equation of the dangerous area simultaneously to calculate the coordinates of the tangent point; Based on the relationship between the coordinates of the tangent point and the coordinates of the center of the tangent circle, a relevant symbol is determined, and a first vector parameter is determined based on the relevant symbol. Calculate the angle parameters based on the distance between the tangent point and the center of the tangent circle, and the distance between different tangent points; Based on the center of the tangent circle, the first vector parameter, and the angle parameter, an optimized path for the tangent circle is generated.

7. The method according to claim 1, characterized in that, The integral line-of-sight guidance law designed based on finite-time theory, combined with the ship kinematics model, generates the desired navigation heading angle according to the obstacle avoidance navigation optimization path, including: Obtain the coordinate point sequence of the obstacle avoidance navigation optimization path and the current position and heading angle of the unmanned surface vessel, and calculate the lateral deviation between the current position and the coordinate point sequence; Based on the finite-time convergence principle, control rules are designed based on the lateral deviation and cumulative value to generate instructions for adjusting the heading angle; The instruction is converted into the desired navigation heading angle through integration.

8. The method of claim 1, wherein, The navigation tracking adjustment module, based on the ship's kinematic model, the desired navigation heading angle, and the ship's real-time motion state, outputs navigation tracking commands, including: The desired navigation heading angle and the real-time motion state of the ship are input into the ship kinematics model to calculate the error variable between the actual state and the desired state; A navigation tracking and adjustment module is constructed, comprising an input layer, a hidden layer, and an output layer; wherein, the input layer receives the error variables and ship kinematics model parameters, the hidden layer approximates the unknown disturbances in ship dynamics through a nonlinear activation function, and the output layer generates virtual control variables; An adaptive algorithm is used to adjust the connection weights between the hidden layer and the output layer in the navigation tracking adjustment module in real time, and the weight parameters are optimized based on the error feedback between the actual ship response and the output of the navigation tracking adjustment module. The virtual control quantity output by the navigation tracking adjustment module is fused with the theoretical control quantity calculated by the ship's kinematics model to generate navigation tracking commands.

9. An unmanned surface vehicle obstacle avoidance navigation path generation and tracking device, characterized by, The device includes a construction module, a generation module, an adjustment module, a combination module, a determination module, and a driving module; The construction module is used to construct a ship kinematics model based on the navigation motion parameters of the unmanned surface vessel. The construction module is also used to identify obstacles on the initial navigation path and construct a danger zone with the center of the obstacle as the origin; The generation module is used to calculate the geometric relationship between the initial navigation path and the danger zone based on the ship's kinematics model, and generate an obstacle avoidance navigation path based on the geometric relationship. The adjustment module is used to determine the overlap of multiple danger zones, and based on the positional relationship of each obstacle and the geometric characteristics of the obstacle avoidance navigation path, to determine the interaction relationship between the obstacle avoidance navigation path and multiple danger zones, and to adjust the obstacle avoidance navigation path based on the interaction relationship. The combined module is used to detect the curvature of the obstacle avoidance navigation path at the entrance and exit ports based on the minimum turning radius of the ship. When the curvature does not meet the turning capability requirements, it generates an optimized path segment of the tangent circle at the entrance and exit ports that is tangent to the boundary of the danger zone, and combines them to obtain an optimized obstacle avoidance navigation path. The generation module is also used to design an integral line-of-sight guidance law based on the finite-time theory, and generate the desired navigation heading angle according to the obstacle avoidance navigation optimization path in combination with the ship kinematics model. The determining module is used to construct the navigation tracking adjustment module, and output navigation tracking commands based on the ship kinematics model, the desired navigation heading angle and the real-time motion state of the ship; The drive module is used to drive the unmanned surface vessel to follow the obstacle avoidance navigation optimization path based on the navigation tracking command; The process of determining the overlap of multiple danger zones, judging the interaction relationship between the obstacle avoidance navigation path and multiple danger zones based on the positional relationship of each obstacle and the geometric characteristics of the obstacle avoidance navigation path, and adjusting the obstacle avoidance navigation path based on the interaction relationship includes: Calculate the distance between each point on the obstacle avoidance navigation path and the boundaries of multiple danger zones, and determine the interaction relationship between the obstacle avoidance navigation path and the multiple danger zones based on the distance; When the interaction relationship is that the obstacle avoidance navigation path does not enter any dangerous area, for each dangerous area, the intersection point of the obstacle avoidance navigation path and the boundary of the dangerous area is determined, and the minor arc enclosed by the intersection point on the circumference of the dangerous area is used as the projection path. When the interaction relationship is that the obstacle avoidance navigation path enters the target danger area, for each target danger area, the superior arc enclosed by the intersection point on the circumference of the target danger area is used as the projection path. The adjusted obstacle avoidance navigation path is obtained by stitching together the non-projected portion of the obstacle avoidance navigation path with the projected path.

Images