An underwater robot hierarchical obstacle avoidance planning method based on shared passable semantics

Abstract

The application discloses a kind of underwater robot layered obstacle avoidance planning methods based on shared passable semantics, it is related to underwater robot autonomous navigation and obstacle avoidance technical field.Environmental semantic information, robot current pose, target information and motion state are obtained, and the environmental semantic information includes structural impassable area, structural clearance, short-time risk and unknown cost;The passable and executable semantic constraints shared by each planning layer are generated;Based on the constraint, the current pose and the target information, generate the candidate topological branch and determine the selected topological branch by combining the score and the hysteresis rule;According to the selected topological branch, determine the selected corridor;In the selected corridor, generate a local trajectory in combination with the motion state and output a speed control instruction.The present application is beneficial to reduce the problems of repeated switching of obstacle avoidance direction, false blocking and discontinuous speed control instruction in the prior art.

Description

Technical Field

[0001] This invention belongs to the field of underwater robot autonomous navigation and obstacle avoidance technology, and particularly relates to a hierarchical obstacle avoidance planning method for underwater robots based on shared navigable semantics. Background Technology

[0002] Forward-looking imaging sonar is a crucial sensor for underwater robots to achieve autonomous obstacle avoidance in murky, low-light, or deep-sea environments. Existing technologies have disclosed various obstacle avoidance methods for underwater vehicles based on forward-looking sonar or multi-beam sonar, as well as technical solutions that convert sonar observations into occupancy grid maps and combine them with path planning or local control.

[0003] In existing hierarchical obstacle avoidance planning methods, the planning system is typically divided into a high-level guidance planning layer, an intermediate corridor or passage selection layer, and a low-level local trajectory optimization layer. However, in existing hierarchical planning methods, each planning layer often makes decisions based on different map interpretations. For example, the guidance planning layer may determine that a passage on one side is passable based on the global cost map, while the local trajectory optimization layer may refuse to generate a trajectory on the same side due to excessive local risk, causing the robot to repeatedly switch obstacle avoidance directions on both sides of the obstacle. When different planning layers interpret the same area as impassable, high-risk, or unknown, problems such as repeated switching of obstacle avoidance directions, accidental blockage, and discontinuous speed control commands can easily occur.

[0004] One of the root causes of the above problems is the lack of unified semantic constraints on traversability and enforceability across different planning layers. Each layer is based on different map interpretations—some equate short-term echo risk with structural obstacles, some assume that insufficiently observed areas are traversable, and some determine the global obstacle avoidance direction based on the local maximum risk value—leading to the contradictory execution of the same planning task across different layers.

[0005] Therefore, given the environmental semantic information output by the multi-semantic map, such as short-term risks, persistent obstacles, observability, unknown costs, as well as structurally impassable areas and structural clearance, how to construct a hierarchical obstacle avoidance planning method that enables the guidance planning layer, corridor generation and selection layer, and trajectory optimization layer to adopt a consistent semantic interpretation of structurally impassable areas, structural clearance, short-term risks, and unknown costs, in order to reduce obstacle avoidance and side-switching, false blocking, and discontinuous speed control commands, is a technical problem that needs to be further solved in the existing technology. Summary of the Invention

[0006] To address the aforementioned technical problems, this invention provides a hierarchical obstacle avoidance planning method for underwater robots based on shared navigable semantics. Unlike existing methods where each planning layer makes decisions based on its own map interpretation, this method unifies structurally impassable areas, structural clearances, short-term risks, and unknown costs from multi-semantic maps into shared navigable and executable semantic constraints across all planning layers. This allows each layer—topology branching, corridor generation and selection, and trajectory optimization within corridors—to make decisions based on the same set of semantic definitions, thereby facilitating consistent judgments by each planning layer regarding the navigability and executability of the same area.

[0007] This invention provides a hierarchical obstacle avoidance planning method for underwater robots based on shared navigable semantics, comprising the following steps: Acquire environmental semantic information, which includes at least structurally impassable areas, structural clearance, short-term risks, and unknown costs; Based on the impassable areas of the structure, the structural clearance, the short-term risks, and the unknown costs, generate accessible and executable semantic constraints shared by each planning layer; Based on shared drivability and executable semantic constraints, multiple candidate topology branches are generated, the candidate topology branches are scored, and the selected topology branch is selected based on the scoring results. Based on the selected topology branch, multiple candidate corridors are generated, the candidate corridors are scored, and the selected corridor is determined based on the scoring results. Within the selected corridor, a local trajectory is generated based on the structural clearance, the short-term risk, and the unknown cost. The underwater robot's movement is controlled by outputting speed control commands based on the local trajectory.

[0008] Optionally, in the step of obtaining environmental semantic information: The impassable region of the structure is derived from the persistent obstacle map, the robot's equivalent radius, and the safety margin through hard expansion, and is used to represent the area that is geometrically unsafe for the robot to pass through. The structural clearance is derived from the persistent obstacle map and robot dimensions, and is used to represent the geometric distance margin from the robot's center location to the nearest structural obstacle. The short-term risk is derived from the short-term risk map, which is used to represent recent sonar evidence and near-field urgency. The unknown cost is derived from the unknown cost map and is used to represent the degree of planning conservatism introduced due to insufficient observation.

[0009] Optionally, in the step of generating drivable and executable semantic constraints shared across planning layers: Shared drivability and executable semantic constraints include semantics of structurally impassable regions, semantics of structural clearance, semantics of short-term risks, and semantics of unknown costs. The semantics of the structurally impassable region serve as hard collision constraints for each planning layer; the short-term risk is not used as the primary basis for determining the large-scale obstacle-avoidance topology direction, nor is it used to redefine the boundary of the structurally impassable region; the semantics of the structural clearance serve as soft cost terms or selection criteria for each planning layer; the semantics of the unknown cost serve as soft cost or degradation criteria for each planning layer. The guidance planning layer, corridor generation and selection layer, and trajectory optimization layer all use the shared traversable and executable semantic constraints as common input interfaces.

[0010] Optionally, in the steps of generating multiple candidate topology branches, scoring the candidate topology branches, and selecting the selected topology branch based on the scoring results: The planning layer performs hard collision judgment based on the structurally impassable regions in the shared traversable and executable semantic constraints, and performs soft scoring based on the structural clearance and the unknown cost to generate multiple candidate topology branches. The score of each candidate topology branch is determined based on path length, directional consistency from the branch end to the target, cumulative unknown cost, structural clearance soft cost, turning cost relative to the current heading, historical inoperability penalty, and switching hysteresis penalty.

[0011] Optionally, in the step of selecting a selected topology branch: When the currently selected topology branch is not determined to be unexecutable based on the structurally impassable region, the new candidate topology branch is switched to the selected topology branch only if the advantage of the score value of the new candidate topology branch relative to the score value of the currently selected topology branch exceeds a preset hysteresis threshold. When the currently selected topology branch is determined to be unexecutable based on the structurally impassable region, the candidate topology branch with the better score is selected as the selected topology branch from those candidate topology branches that are not determined to be unexecutable based on the structurally impassable region.

[0012] Optionally, in the steps of generating multiple candidate corridors, scoring the candidate corridors, and determining the selected corridor based on the scoring results: The corridor generation and selection layer generates multiple candidate corridors based on the selected topology branch; The score of each candidate corridor is determined based on the direction and topological consistency between each candidate corridor and the selected topological branch, the soft cost of corridor geometrical clearance based on structural clearance, short-term risk cost, unknown cost, insufficient corridor width cost, centerline curvature cost, and hysteresis penalty for switching from the currently selected corridor.

[0013] Optionally, in the step of determining the selected corridor: If the currently selected corridor is not determined to be geometrically impassable and the score of the currently selected corridor is less than or equal to the sum of the score of the current best candidate corridor and the preset corridor hysteresis threshold, the currently selected corridor remains unchanged. When the currently selected corridor is determined to be geometrically impassable, or when the score of the current best candidate corridor has an advantage over the score of the currently selected corridor that exceeds the preset corridor hysteresis threshold, or when the selected topology branch is switched and the currently selected corridor is no longer aligned with the switched selected topology branch, the selected corridor is switched to the current best candidate corridor.

[0014] Optionally, in the step of generating a local trajectory within the selected corridor based on the structural clearance, the short-term risk, and the unknown cost: The trajectory optimization layer generates multiple candidate trajectories within the selected corridor and performs hard constraint checks on each candidate trajectory; Candidate trajectories that do not meet the hard constraints are rejected based on whether each candidate trajectory falls into a structurally impassable area, whether the short-term risk of the near-field sampling point exceeds the hard rejection threshold, and whether each candidate trajectory exceeds the allowable deviation range of the selected corridor. For each candidate trajectory that passes the hard constraint check, the cost function value of each candidate trajectory is determined based on the progress cost, corridor tracking cost, orientation cost, structural clearance cost, short-term risk cost, unknown cost, smoothing cost, oscillation cost, and terminal cost.

[0015] Optionally, in the step of determining the short-term risk cost of each candidate trajectory: Different weighting coefficients are assigned to sampling points belonging to the near-field region, mid-field region, and far-field region on the candidate trajectory. The weighting coefficient of the near-field region is greater than that of the mid-field region, and the weighting coefficient of the mid-field region is greater than that of the far-field region. The short-term risk cost is determined based on the weighted cumulative value of the short-term risk value at each sampling point and the corresponding weight coefficient.

[0016] Optionally, in the step of outputting the speed control command for the underwater robot based on the local trajectory: Among the candidate trajectories that pass the hard constraint check, the candidate trajectory with the smallest cost function value is selected as the selected local trajectory; Extract at least one of the desired linear velocity and desired angular velocity corresponding to the selected local trajectory as a velocity control command output; When all candidate trajectories are rejected by hard constraints, output zero linear velocity.

[0017] Compared with the prior art, the present invention has the following advantages and technical effects: The underwater robot hierarchical obstacle avoidance planning method based on shared navigable semantics provided by this invention unifies the obtained structurally impassable areas, structural clearance, short-term risks, and unknown costs into navigable and executable semantic constraints shared by each planning layer. This allows the guidance planning layer, corridor generation and selection layer, and trajectory optimization layer to use this shared semantic constraint as a common input interface, and to judge the navigability and executability of the same grid area based on the same set of semantic definitions.

[0018] Based on this, the planning layer generates multiple candidate topology branches according to the shared semantic constraints and selects the selected topology branch through scoring and hysteresis rules, thereby determining a stable obstacle avoidance direction at the topology level; the corridor generation and selection layer generates multiple candidate corridors according to the selected topology branch and determines the selected corridor through scoring and corridor retention mechanisms, thereby maintaining the continuity of the geometric path at the channel level; the trajectory optimization layer generates a local trajectory within the selected corridor based on structural clearance, short-term risks and unknown costs, and outputs speed control commands accordingly, thereby achieving smooth obstacle avoidance motion at the local control level.

[0019] Since all planning layers use the same set of semantic constraints for decision-making, it avoids problems such as repeated switching of obstacle avoidance direction, accidental blocking, and discontinuous speed control commands caused by different layers making decisions based on different map interpretations in existing technologies. This enables underwater robots to achieve stable, continuous, and safe autonomous obstacle avoidance navigation in irregular or complex obstacle environments. Attached Figure Description

[0020] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings: Figure 1 This is a flowchart illustrating the overall process of the underwater robot hierarchical obstacle avoidance planning method based on shared navigable semantics in an embodiment of the present invention. Figure 2 This is a schematic diagram illustrating the structure and reference relationship of shared passable and executable semantic constraints in an embodiment of the present invention; Figure 3 This is a schematic diagram illustrating the hierarchical correspondence between topological branches, corridors, and trajectories in an embodiment of the present invention; Figure 4 This is a schematic diagram of the graded processing areas for short-term risks in the near field, mid field, and far field in an embodiment of the present invention. Detailed Implementation

[0021] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0022] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0023] Example 1 Figure 1 This is a flowchart illustrating the hierarchical obstacle avoidance planning method in an embodiment of the present invention. (Refer to...) Figure 1 This embodiment provides a hierarchical obstacle avoidance planning method for underwater robots based on shared navigable semantics, including the following steps: Acquire environmental semantic information, which includes at least structurally impassable areas, structural clearance, short-term risks, and unknown costs; Based on the impassable areas of the structure, the structural clearance, the short-term risks, and the unknown costs, generate accessible and executable semantic constraints shared by each planning layer; Based on shared drivability and executable semantic constraints, multiple candidate topology branches are generated, the candidate topology branches are scored, and the selected topology branch is selected based on the scoring results. Based on the selected topology branch, multiple candidate corridors are generated, the candidate corridors are scored, and the selected corridor is determined based on the scoring results. Within the selected corridor, a local trajectory is generated based on the structural clearance, the short-term risk, and the unknown cost. The underwater robot's movement is controlled by outputting speed control commands based on the local trajectory.

[0024] As a feasible implementation method, the specific steps include: In step N1, environmental semantic information is acquired. This environmental semantic information includes a short-term risk map, a persistent obstacle map, an observability map, an unknown cost map, structurally impassable areas, and structural clearance. The robot's current position, target orientation, and motion state are also acquired. In one embodiment, the aforementioned environmental semantic information may be provided by a forward-looking sonar multi-semantic map construction module; however, it is not limited to this source.

[0025] In step N2, based on the impassable areas of the structure, the structural clearance, the short-term risks, and the unknown costs, traversable and executable semantic constraints shared by each planning layer are generated.

[0026] In step N3, during the topology branching phase, multiple candidate topology branches are generated based on the shared drivable and executable semantic constraints. The selected topology branch is chosen through scoring and hysteresis rules to suppress frequent branch switching.

[0027] In step N4, during the corridor generation and selection phase, multiple candidate corridors are generated based on the selected topology branch, and the selected corridor is determined through a scoring and corridor retention mechanism.

[0028] In step N5, during the trajectory optimization phase, a local trajectory is generated within the selected corridor based on the structural clearance, short-term risks, unknown costs, orientation, progress, and smoothness, wherein different processing strategies are adopted for short-term risks in the near field, mid field, and far field.

[0029] In step N6, the speed control command for the underwater robot is output based on the local trajectory.

[0030] Implementable, the structurally impassable region is used to represent a region that is geometrically unsafe for the robot to pass through. The structurally impassable region is derived from a persistent obstacle map and robot dimensions and safety margins. Short-term risks do not change its boundaries, and it serves as a hard collision constraint for each planning layer.

[0031] The structural clearance is used to represent the geometric distance margin between the robot and the nearest structural obstacle. It is derived from the persistent obstacle map and the robot dimensions, and each planning layer uses it as a soft cost term or a selection criterion.

[0032] The short-term risk is used to represent recent sonar evidence and near-field urgency. Each planning layer uses it for deceleration, scoring penalties, or candidate trajectory rejection, but it is not the main basis for determining the large-scale obstacle avoidance topology direction, nor does it redefine the boundary of structurally impassable areas in the long term.

[0033] The unknown cost is used to represent the degree of planning conservatism introduced due to insufficient observation, and each planning layer uses it as a soft cost or a basis for downgrading. The unknown area is neither the same as free space nor the same as the area occupied by obstacles.

[0034] Unlike existing technologies that compress obstacle occupancy probability, free space probability, and unknown area information into a single layer using a single occupied grid map, leading to different interpretations of the same area by different planning layers, this invention explicitly separates these four semantic categories through shared drivability and executability semantic constraints: structurally impassable areas are derived from a persistent obstacle map, robot dimensions, and safety margins, serving as hard boundaries; structural clearance independently expresses geometric margins; short-term risks are used as transient judgments rather than long-term topological bases; and unknown costs are independent of obstacle occupancy states, expressing observational uncertainty. Using this as a common input interface for all planning layers ensures that the topology branching, corridor generation and selection, and corridor trajectory optimization layers base their judgments on the drivability and executability of the same grid area on the same set of semantic definitions.

[0035] Regarding the responsibilities of the planning layer: It guides the planning layer to use structurally impassable areas for hard collision assessment and structural clearance and unknown costs for soft scoring; short-term risk is not used as the primary basis for determining large-scale topology direction. The corridor generation and selection layer maintains separate assessment of geometric clearance and short-term risk. The trajectory optimization layer operates within the selected corridor and does not re-determine the large-scale obstacle avoidance topology direction. All planning layers share the same set of semantics regarding structurally impassable areas, structural clearance, short-term risks, and unknown costs, thereby reducing obstacle avoidance edge switching, false blocking, and discontinuities in local velocity control commands caused by inconsistent semantic interpretations among planning layers.

[0036] The following provides a detailed explanation of each of the above steps: Shared passability and executable semantic constraints: Figure 2 This is a schematic diagram illustrating shared traversable and executable semantic constraints in an embodiment of the present invention. In this step, the environmental semantic information obtained in N1 is unified into traversable and executable semantic constraints commonly referenced by all planning layers.

[0037] Components of semantic constraints: The shared practicability and executable semantic constraints consist of the following four core semantic components: Impassable areas of the structure (available) (Representation): This represents a region that is geometrically impassable for the robot. The impassable region is derived from a persistent obstacle map, the robot's equivalent radius, and a safety margin through hard expansion. In one embodiment, the hard expansion radius is... ,in For the robot's equivalent radius, For safety margin, the distance from a grid cell to the nearest structural barrier is less than or equal to... When that time, mark it as that area.

[0038] Structural clearance (available) (Representation): Derived from the persistent obstacle map and robot dimensions, it represents the geometric distance margin from the robot's center location to the nearest structural obstacle. In one embodiment, the structural clearance can be defined as... ,in This is the distance from the grid cell to the nearest structural barrier. When When, it indicates a geometric collision; when When, it indicates approaching the safety boundary; when When the distance is specified, it indicates that the safety distance requirement is met.

[0039] Short-term risks (available) (Represented by): derived from short-term risk maps, reflecting recent sonar evidence and near-field urgency. Each planning layer uses it for deceleration, scoring penalties, or candidate trajectory rejection, but it is not the primary basis for determining large-scale obstacle avoidance topology directions, nor does it redefine the boundaries of structurally impassable areas in the long term.

[0040] Unknown cost (available) (Representation): From the unknown cost map, indicating the degree of planning conservatism introduced due to insufficient observation. The unknown area is neither the same as free space nor the same as the area occupied by obstacles; each planning level uses it as a basis for soft cost or downgrade.

[0041] Based on the above four semantics, the executable status (availability) of candidate paths, candidate corridors, or candidate trajectories can also be comprehensively determined. (represented by the formula), the executable state includes at least: geometrically impassable state, short-term high risk state, insufficient observation state, insufficient execution progress state, and normal executable state.

[0042] The core features of semantic constraints: The core feature of the shared, workable, and executable semantic constraints is that: (1) Separation of semantic sources: The structural impassable area is derived from the persistent obstacle map, robot size and safety margin, and the structural clearance is derived from the persistent obstacle map and robot size. Neither of them takes short-term risk as a component of their input; short-term risk reflects recent sonar evidence and near-field urgency; unknown cost reflects the sufficiency of observation, and unknown area is not the same as free space or obstacle-occupied area.

[0043] (2) Layered unified reference: The planning layer, corridor generation and selection layer and trajectory optimization layer all use the shared drivability and executability semantic constraints as common input interfaces, and the drivability and executability judgment of the same grid area are based on the same set of semantic definitions.

[0044] (3) Clear boundaries of responsibilities: The guiding planning layer determines the large-scale topological direction based on the structurally impassable areas and unknown costs, and short-term risks are not used as the main basis for its decision-making; the corridor generation and selection layer maintains the separation of geometric clearance and short-term risks in its assessment; the trajectory optimization layer handles short-term risks in a hierarchical manner based on near field, mid field and far field, and optimizes within the selected corridor boundary without re-determining the large-scale obstacle bypass topological direction.

[0045] Therefore, the shared passable and executable semantic constraints serve as a unified semantic interface between planning layers, avoiding problems such as repeated switching of obstacle avoidance directions, accidental blocking, and discontinuous speed control commands caused by different map interpretations for the same area in existing technologies.

[0046] Topology branch guidance: Figure 3 This is a schematic diagram illustrating the hierarchical relationship between topology branches, corridors, and trajectories in an embodiment of the present invention. The topology branch guidance stage corresponds to... Figure 3 The part from the candidate topology branch to the selected topology branch has the following inputs: the robot's current position and target direction, the structurally impassable region in the shared drivable and executable semantic constraints, the structural clearance and unknown cost, and the selected topology branch information from the previous moment.

[0047] Candidate topology branch generation: The guidance planning layer generates multiple candidate topology branches based on shared drivability and executable semantic constraints. These candidate topology branches do not simply contain a single shortest path, but rather represent different topological directions from the robot's current position to bypass structural obstacles. Candidate sources include: branches directly towards the target direction, bypass branches generated from free space openings, and bypass branches on either side of the structural obstacle boundary.

[0048] Each candidate topology branch contains a set of path sampling points from the robot's current position to the local or final target.

[0049] Branch rating: For each candidate topology branch, a score is awarded based on the following factors: path length, directional consistency from the branch end to the target, cumulative unknown cost on the path, soft cost of structural clearance (the smaller the clearance, the higher the cost), turning cost relative to the current heading, historical unexecutability penalty from corridor and trajectory layer feedback, and hysteresis penalty for switching from the currently selected branch to this branch.

[0050] Let candidate topology branches be defined. Includes a set of path sampling points In one embodiment, the scoring function for the candidate topological branch can be expressed as: in, The cost is the path length. The cost of ensuring directional consistency from the end of the branch to the target; Unknown cost for each sampling point on the path The cumulative or average value; For structural clearance soft cost; The cost of turning relative to the current course; As a punishment for historical impermissibility; The hysteresis penalty for switching from the currently selected topology branch to this candidate branch, when That is, when the currently selected topology branch is ; , , , , , and These are the weighting coefficients for each item.

[0051] Among them, the structural net clearance soft cost The structural clearance at each sampling point can be determined using the mean, minimum, or a preset piecewise penalty function. In one embodiment, when the structural clearance at a sampling point is greater than or equal to a preset preferred clearance threshold, soft cost may not be considered; when the structural clearance is less than the preferred clearance threshold but greater than zero, the penalty value can be determined based on the degree of deficiency of the clearance relative to the preferred threshold, with a larger penalty for smaller clearance; when the structural clearance is less than zero, it indicates that a geometric collision has occurred at that point, and the candidate topology branch can be rejected.

[0052] The guiding planning layer uses structurally impassable areas for hard collision detection and structural clearance and unknown costs for soft scoring. Short-term risk is not used as the primary basis for determining large-scale topology direction at the guiding planning layer.

[0053] Branch selection and hysteresis: To avoid frequent switching between left and right detour directions in the guidance planning layer, a hysteresis rule is adopted.

[0054] Let the currently selected topology branch be The current optimal new candidate topology branch is , and These are the ratings for both. The preset hysteresis threshold ( In one embodiment, the branch switching rules are as follows: (1) If the currently selected topology branch Since structurally impassable regions are not deemed unexecutable, a switch is only triggered when the score advantage of a new candidate branch exceeds the hysteresis threshold, i.e., when the following formula is satisfied: Switch to : If the above conditions are not met, then keep the currently selected topology branch. constant.

[0055] (2) If the currently selected topology branch If a structurally impassable region is determined to be unexecutable, then the candidate topology branch with the better score is selected from those that are not determined to be unexecutable based on the structurally impassable region; if no such candidate topology branch exists, then it can be temporarily maintained or downgraded.

[0056] In practice, when there is uncertainty in the executable state of a candidate topology branch, the guidance planning layer can request the corridor generation and selection layer and the trajectory optimization layer to further confirm the executable state in order to avoid incorrectly switching the topology direction due to temporary local fluctuations.

[0057] The aforementioned branch hysteresis mechanism helps maintain the stability of the topology intent while preserving the ability to adjust the topology direction in a timely manner when a structurally impassable region is determined to be unexecutable.

[0058] Corridor Candidates, Selection, and Maintenance: Continue to refer to Figure 3 Corridor generation and selection process correspond Figure 3 The input to the part from the selected topology branch to the candidate corridor and then to the selected corridor includes: the selected topology branch and its guiding path, the structurally impassable area in the shared drivable and executable semantic constraints, the structural clearance, short-term risks and unknown costs, and the selected corridor information at the previous moment.

[0059] Candidate corridor generation: The centerline of the selected topology branch is sampled to generate multiple candidate corridors. Candidate corridors can be different lateral offset channels along the centerline of the guide path, or channels corresponding to different topology branch directions. Each candidate corridor includes a centerline, width profile, effective arc length, lateral identifier, and the identifier of the associated topology branch.

[0060] Corridor rating: For each candidate corridor, a score is awarded based on the following factors: orientation and topology consistency with the selected topology branch, soft cost of structural clearance (the smaller the clearance, the higher the cost), short-term risk cost (examining short-term risks within the corridor), unknown cost (the proportion of unknowns in the near field increases corridor risk), corridor width cost (insufficient width increases collision risk), centerline curvature cost (excessive curvature increases tracking difficulty), and hysteresis penalty for switching from the currently selected corridor to this candidate corridor.

[0061] Set up candidate corridors Including centerline sampling points And its corresponding width distribution. In one embodiment, the scoring function for the candidate corridor can be expressed as: in, For corridor The direction of the selected topology branch and the cost of topology consistency; The soft cost of corridor geometric clearance based on structural clearance; The cost of short-term risk derived from the short-term risk map; For unknown costs from an unknown cost graph; This comes at the cost of insufficient corridor width. The cost of the curvature of the centerline; To switch from the currently selected corridor Delayed penalty, when That is, when the currently selected corridor is ; , , , , , and These are the weighting coefficients for each item.

[0062] In one embodiment, the corridor geometric clearance soft cost The structural clearance at each sampling point along the centerline can be used to measure the clearance. The short-term risk cost is obtained by averaging or minimizing the value. Short-term risks at each sampling point along the centerline can be assessed. The weighted cumulative sum is used, where the weight of near-field sampling points is higher than that of far-field sampling points; the unknown cost By considering the unknown costs at each sampling point along the centerline The mean or the maximum unknown cost in the near field can be obtained.

[0063] In corridor scoring, it is crucial to separate the robot's geometric clearance from short-term risks: geometric clearance, derived from structural obstacles, determines whether the robot can pass geometrically; short-term risks, derived from the short-term risk map, determine whether a conservative strategy should be adopted in the short term. Avoid directly substituting short-term risks for geometric clearance assessments.

[0064] Corridor maintenance mechanism: To avoid frequent left-right switching of the corridor under localized short-term risk fluctuations, a corridor maintenance mechanism is adopted.

[0065] Let the currently selected corridor be The current optimal candidate corridor is , and These are the ratings for both. Maintain a preset hysteresis threshold for the corridor ( In one embodiment, the corridor holding mechanism can be described as follows: Conditions for maintaining: If It is not determined to be geometrically impassable and satisfies: Then keep the currently selected corridor constant.

[0066] Switching conditions: The selected corridor will be switched if any of the following conditions are met. Switch to : It was determined to be geometrically impassable; Compared to The advantage exceeds the hysteresis threshold, that is: The selected topology branch has switched, and No longer aligned with the newly selected topology branch.

[0067] Through the corridor maintenance mechanism, the corridor generation and selection layer remain stable under local short-term risk changes, avoiding frequent side switching.

[0068] Selected corridor trajectory optimization: Continue to refer to Figure 3 The trajectory optimization stage corresponds to Figure 3 The inputs to the portion from the selected corridor to the local trajectory include: the centerline and boundary of the selected corridor, the robot's current motion state, the structural clearance in the shared drivable and executable semantic constraints, short-term risks and unknown costs, and the speed control command output in the previous moment.

[0069] Candidate trajectory generation: Multiple velocity control candidate pairs are generated within a local time window, each pair including linear velocity and angular velocity. Forward integration is performed on each control candidate based on the robot motion model to obtain the corresponding candidate trajectory. The generation range of the candidate trajectories is restricted to a selected corridor.

[0070] Hard constraints on trajectory: For each candidate trajectory, perform the following hard constraint checks.

[0071] Let candidate trajectories be defined. This is a set of pose sampling points obtained by forward integration from the robot's current position. Near-field distance threshold, As a hard rejection threshold for near-field short-term risks, This represents the allowable lateral deviation tolerance for the corridor boundary. In one embodiment, if the candidate trajectory... The candidate trajectory is rejected if any of the following conditions are met: (1) Geometric collision: There are sampling points It falls into a structurally impassable region. In one embodiment, if the set of points in the structurally impassable region is used... To make a judgment, it can be expressed as: in Let be the set of points representing structurally impassable regions within the shared traversable and executable semantic constraints.

[0072] (2) High risk in the near field: There are near field sampling points. (i.e., its arc length distance) Its short-term risk exceeds the hard rejection threshold: (3) Corridor crossing: candidate trajectory The sampling point exceeds the allowable deviation range of the selected corridor, meaning there are sampling points. Lateral deviation from the selected corridor centerline exceeds .

[0073] The semantic sources of the aforementioned hard constraints are clear: geometric collision constraints come from the impassable regions of the structure, near-field high-risk constraints come from the short-term risks, and topological boundary crossing constraints come from the selected corridor boundaries.

[0074] Trajectory cost function: For candidate trajectories that are not rejected by hard constraints, a cost function is constructed to select the optimal local trajectory.

[0075] In one embodiment, candidate trajectories The cost function can be expressed as: The meanings of each item are as follows: The progress cost represents the expected progress along the selected corridor direction; the greater the progress, the smaller the cost (or it can be negative).

[0076] The corridor tracking cost represents the lateral deviation of each sampling point on the trajectory from the centerline of the selected corridor. The larger the lateral deviation, the greater the cost.

[0077] The heading cost represents the deviation between the end-of-course heading and the desired heading.

[0078] The structural clearance cost can be calculated by considering the structural clearance at each sampling point. After calculating the mean or minimum value, the segmentation method is determined according to the preset segmentation method, and the trajectory with larger clearance is preferred.

[0079] This is the cost of short-term risk. (Refer to...) Figure 4 Short-term risks in the near field, mid field, and far field are accumulated in segments using different weights. In one embodiment, the cost of short-term risk can be expressed as: in , and These represent the near-field regions (arc lengths) on the trajectory. ), midfield area ( ) and far-field region ( The set of sampling points; Sampling points The short-term risk value at the location; , , The weighting coefficients for near field, mid field, and far field are respectively, satisfying... .

[0080] The unknown cost can be determined by analyzing the unknown cost at each sampling point. The weighted average or weighted cumulative value is obtained, with the weight of the unknown cost in the near field being higher than that in the far field.

[0081] The smoothing cost represents the magnitude of change between the current speed control candidate and the speed control command at the previous moment. In one embodiment, the smoothing cost can be based on the current control candidate. Compared with the speed control command output at the previous moment The difference between them is determined, and the greater the difference, the greater the cost of smoothing.

[0082] The oscillation cost represents the degree of repeated switching of the angular velocity direction in the recent period, and is used to suppress trajectory polymorphism and angular velocity direction oscillation.

[0083] Terminal cost represents the deviation between the trajectory end pose and the desired local target pose.

[0084] , , , , , , , and These are the weighting coefficients for each item.

[0085] Figure 4 This is a schematic diagram illustrating short-term risk handling in the near field, mid field, and far field in an embodiment of the present invention. (Refer to...) Figure 4 Different strategies are used to handle short-term risks in the near, mid, and far fields: (1) Near-field short-term risk: refers to short-term risks within a relatively close range of the robot. High near-field risk may trigger hard rejection of candidate trajectories, sudden deceleration, or maintaining the current position.

[0086] (2) Mid-range short-term risk: refers to short-term risk within a medium distance range. Mid-range high risk serves as a strong scoring penalty in the trajectory cost function and can be used as a basis for reducing the executable level.

[0087] (3) Far-field short-term risk: refers to short-term risk within a relatively long distance range. Far-field high risk serves as a soft penalty or deceleration warning signal in the trajectory cost function and does not directly lead to the rejection of the current trajectory.

[0088] Through the above-mentioned hierarchical processing, the overly conservative problem of a single short-term high risk point in the far field directly causing the robot to stop moving forward is avoided, while the high risk in the near field can be responded to in a timely and effective manner.

[0089] Track selection and speed control command output: Among the candidate trajectories that satisfy all hard constraints, the candidate trajectory with the minimum cost function value is selected as the local trajectory. Let the set of valid candidate trajectories that pass the hard constraint check be . In one embodiment, a local trajectory is selected. Determined by the following formula: according to Corresponding control candidates Output speed control commands. In one embodiment, the speed control commands include the desired linear velocity of the underwater robot. and expected angular velocity At least one of them, or including a speed control quantity corresponding to the motion control interface of the underwater robot.

[0090] like If the set is empty (i.e. all candidate trajectories are rejected by hard constraints), the zero linear velocity is output. At the same time, the safe steering angular velocity or the trigger hold state can be output according to the observability state and the short-term risk state.

[0091] The trajectory optimization layer performs local trajectory optimization within the selected corridor without re-determining the large-scale obstacle avoidance topology. The topology direction is determined by the guidance planning layer and the corridor generation and selection layer. The trajectory optimization layer only performs smooth trajectory generation and velocity optimization within the selected corridor boundaries. This hierarchical division of responsibilities makes the functional boundaries of each planning layer clearer and avoids erroneous changes to the global obstacle avoidance direction due to fluctuations in single-frame observations during local optimization.

[0092] Speed ​​control command output: Based on the local trajectory selected by the trajectory optimization layer, the corresponding linear velocity and angular velocity control commands are extracted and output to the underwater robot's motion control system for execution. This process is executed cyclically in a rolling optimization manner, that is, in each control cycle, environmental semantic information and robot state are reacquired, the shared navigable and executable semantic constraints are updated, the guidance planning, corridor selection and trajectory optimization are updated, and new velocity control commands are output.

[0093] Compared with the prior art, the method provided by the present invention has the following beneficial effects: First, by unifying the structurally impassable areas, structural clearances, short-term risks, and unknown costs from multi-semantic maps into shared drivability and executability semantic constraints across all planning layers, the semantic inconsistency problem caused by separate decisions based on different map interpretations in existing technologies is avoided. This facilitates consistent judgments on the drivability and executability of the same area by the guidance planning layer, corridor generation and selection layer, and trajectory optimization layer.

[0094] Second, by treating structurally impassable areas as hard collision constraints, structural clearance as a soft cost, limiting short-term risks to transient judgments and not using them as the primary basis for determining large-scale topology, and expressing unknown costs independently of obstacle occupancy states, the four semantic categories are separated and used at each planning layer. This helps avoid the long-term impact of short-term echo artifacts on obstacle-around topology and also helps avoid blindly classifying insufficiently observed areas as passable.

[0095] Third, the topological branch hysteresis selection and corridor retention mechanism helps to suppress frequent branch switching in the guidance planning layer and local jitter in the corridor generation and selection layer, thereby improving the topological stability of obstacle avoidance planning.

[0096] Fourth, by adopting different handling strategies for short-term risks in the near field, mid field, and far field, it is beneficial to avoid the overly conservative problem of a single high point of short-term risk in the far field directly causing the current trajectory to be rejected, while enabling high risks in the near field to be decelerated or rejected in a timely manner.

[0097] As an additional implementation, the environmental semantic information acquired in step N1 may, in one embodiment, be provided by a forward-looking sonar multi-semantic map construction module. However, it is not limited to this source; the environmental semantic information may also be loaded from a pre-built map file, received from an external communication interface, or read from local storage.

[0098] Candidate topology branches can be generated based on grid path search algorithms, or based on the detection of openings in traversable areas of structures, or based on the topology graph of connected components of structural obstacles and traversable areas.

[0099] The trajectory optimization layer can be implemented using discrete velocity sampling combined with forward simulation and cost function selection, or using curve family sampling combined with corridor scoring, or using continuous optimization. All of the above implementation methods are optional and do not constitute a necessary limitation on the main embodiment of this invention.

[0100] The thresholds, weights, distance segmentation parameters, and time constants involved in the method can be preset according to the sonar range, robot size, and operating environment. In one embodiment, the near-field, mid-field, and far-field distance boundary values, branch switching hysteresis threshold, corridor holding hysteresis threshold, trajectory hard constraint threshold, and various weights of the cost function can all be adjusted according to the actual application scenario. This embodiment does not constitute a limitation on the specific values ​​of each parameter.

[0101] While the above embodiments are illustrated using underwater robots and forward-looking imaging sonar as examples, in one embodiment, the method can also be used for obstacle avoidance planning of other mobile platforms with imaging sensors having limited field of view. These optional extended implementations do not depart from the basic principles and scope of the invention.

[0102] The above are merely preferred embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A hierarchical obstacle avoidance planning method for underwater robots based on shared navigable semantics, characterized in that, Includes the following steps: The system acquires environmental semantic information, the robot's current pose, target information, and the robot's motion state. The environmental semantic information includes at least structurally impassable areas, structural clearance, short-term risks, and unknown costs. Based on the impassable areas of the structure, the structural clearance, the short-term risks, and the unknown costs, generate accessible and executable semantic constraints shared by each planning layer; Based on the shared drivable and executable semantic constraints, the robot's current pose, and the target information, multiple candidate topology branches are generated, the candidate topology branches are scored, and the selected topology branch is chosen based on the scoring results. Based on the selected topology branch, the robot's current pose, and the shared drivable and executable semantic constraints, multiple candidate corridors are generated, the candidate corridors are scored, and the selected corridor is determined based on the scoring results. Within the selected corridor, a local trajectory is generated based on the robot's motion state, the structural clearance, the short-term risk, and the unknown cost. The underwater robot's movement is controlled by outputting speed control commands based on the local trajectory.

2. The method according to claim 1, characterized in that, In the steps of obtaining environmental semantic information: The impassable region of the structure is derived from the persistent obstacle map, the robot's equivalent radius, and the safety margin through hard expansion, and is used to represent the area that is geometrically unsafe for the robot to pass through. The structural clearance is derived from the persistent obstacle map and robot dimensions, and is used to represent the geometric distance margin from the robot's center location to the nearest structural obstacle. The short-term risk is derived from the short-term risk map, which is used to represent recent sonar evidence and near-field urgency. The unknown cost is derived from the unknown cost map and is used to represent the degree of planning conservatism introduced due to insufficient observation.

3. The method according to claim 1, characterized in that, In the step of generating accessible and executable semantic constraints shared across all planning layers: Shared drivability and executable semantic constraints include semantics of structurally impassable regions, semantics of structural clearance, semantics of short-term risks, and semantics of unknown costs. The semantics of the structurally impassable region serve as hard collision constraints for each planning layer; the short-term risk is not used as the primary basis for determining the large-scale obstacle-avoidance topology direction, nor is it used to redefine the boundary of the structurally impassable region; the semantics of the structural clearance serve as soft cost terms or selection criteria for each planning layer; the semantics of the unknown cost serve as soft cost or degradation criteria for each planning layer. The guidance planning layer, corridor generation and selection layer, and trajectory optimization layer all use the shared traversable and executable semantic constraints as common input interfaces.

4. The method according to claim 1, characterized in that, In the steps of generating multiple candidate topology branches, scoring the candidate topology branches, and selecting the selected topology branch based on the scoring results: The planning layer performs hard collision judgment based on the structurally impassable regions in the shared traversable and executable semantic constraints, and performs soft scoring based on the structural clearance and the unknown cost to generate multiple candidate topology branches. The score of each candidate topology branch is determined based on path length, directional consistency from the branch end to the target, cumulative unknown cost, structural clearance soft cost, turning cost relative to the current heading, historical inoperability penalty, and switching hysteresis penalty.

5. The method according to claim 4, characterized in that, In the step of selecting a topology branch: When the currently selected topology branch is not determined to be unexecutable based on the structurally impassable region, the new candidate topology branch is switched to the selected topology branch only if the advantage of the score value of the new candidate topology branch relative to the score value of the currently selected topology branch exceeds a preset hysteresis threshold. When the currently selected topology branch is determined to be unexecutable based on the structurally impassable region, the candidate topology branch with the better score is selected as the selected topology branch from those candidate topology branches that are not determined to be unexecutable based on the structurally impassable region.

6. The method according to claim 1, characterized in that, In the steps of generating multiple candidate corridors, scoring the candidate corridors, and determining the selected corridor based on the scoring results: The corridor generation and selection layer generates multiple candidate corridors based on the selected topology branch; The score of each candidate corridor is determined based on the direction and topological consistency between each candidate corridor and the selected topological branch, the soft cost of corridor geometrical clearance based on structural clearance, short-term risk cost, unknown cost, insufficient corridor width cost, centerline curvature cost, and hysteresis penalty for switching from the currently selected corridor.

7. The method according to claim 6, characterized in that, In the steps of determining the selected corridor: If the currently selected corridor is not determined to be geometrically impassable and the score of the currently selected corridor is less than or equal to the sum of the score of the current best candidate corridor and the preset corridor hysteresis threshold, the currently selected corridor remains unchanged. When the currently selected corridor is determined to be geometrically impassable, or when the score of the current best candidate corridor has an advantage over the score of the currently selected corridor that exceeds the preset corridor hysteresis threshold, or when the selected topology branch is switched and the currently selected corridor is no longer aligned with the switched selected topology branch, the selected corridor is switched to the current best candidate corridor.

8. The method according to claim 1, characterized in that, In the step of generating a local trajectory within the selected corridor based on the structural clearance, the short-term risk, and the unknown cost: The trajectory optimization layer generates multiple candidate trajectories within the selected corridor and performs hard constraint checks on each candidate trajectory; Candidate trajectories that do not meet the hard constraints are rejected based on whether each candidate trajectory falls into a structurally impassable area, whether the short-term risk of the near-field sampling point exceeds the hard rejection threshold, and whether each candidate trajectory exceeds the allowable deviation range of the selected corridor. For each candidate trajectory that passes the hard constraint check, the cost function value of each candidate trajectory is determined based on the progress cost, corridor tracking cost, orientation cost, structural clearance cost, short-term risk cost, unknown cost, smoothing cost, oscillation cost, and terminal cost.

9. The method according to claim 8, characterized in that, In the step of determining the short-term risk cost of each candidate trajectory: Different weighting coefficients are assigned to sampling points belonging to the near-field region, mid-field region, and far-field region on the candidate trajectory. The weighting coefficient of the near-field region is greater than that of the mid-field region, and the weighting coefficient of the mid-field region is greater than that of the far-field region. The short-term risk cost is determined based on the weighted cumulative value of the short-term risk value at each sampling point and the corresponding weight coefficient.

10. The method according to claim 8, characterized in that, In the step of outputting the speed control command for the underwater robot based on the local trajectory: Among the candidate trajectories that pass the hard constraint check, the candidate trajectory with the smallest cost function value is selected as the selected local trajectory; Extract at least one of the desired linear velocity and desired angular velocity corresponding to the selected local trajectory as a velocity control command output; When all candidate trajectories are rejected by hard constraints, output zero linear velocity.

Images