Robot escape method and device based on curvature trajectory multi-layer search, computer device and storage medium

Abstract

The application relates to a robot escape method and device based on a curvature trajectory multi-layer search, a computer device and a storage medium, the method comprising the following steps: in response to an escape instruction, acquiring a current pose of a robot and an escape target point; taking the current pose of the robot as a current search node, constructing a search layer based on the current search node through curvature trajectory expansion, and searching for a target escape trajectory in the search layer based on the escape target point; if the target escape trajectory does not exist in the search layer, acquiring a plurality of expansion points generated in the search layer; taking the plurality of expansion points as the current search node, and re-executing the step of constructing the search layer based on the current search node through the curvature trajectory expansion until the target escape trajectory is searched; generating an escape plan based on the target escape trajectory, and executing the escape plan. The application can select a more direct escape path, generate a more reasonable combined escape action, adapt to a complex blocked scene, and improve the robot escape efficiency.

Description

Technical Field

[0001] This invention relates to the field of robotics, and more particularly to a method, apparatus, computer equipment, and storage medium for robot extrication based on multi-layer search of curvature trajectories. Background Technology

[0002] Currently, when robots encounter localized obstructions, have limited chassis posture, or are unable to follow their planned local trajectory within a short period due to complex surrounding obstacle distribution, they tend to repeatedly probe within the local area. This manifests as small forward and backward movements, swaying left and right, frequent braking, or continuous on-the-spot adjustments within a narrow local area, ultimately failing to effectively escape the current obstructed state. Such scenarios are common in narrow passages, L-shaped corners, localized blind spots, situations where one side of the vehicle is close to an obstacle, or when there is limited but not completely blocked space on the local map.

[0003] Existing solutions are relatively simple, such as reversing a fixed distance, turning at a fixed angle in place, simply finding the nearest free point near the robot, or searching for feasible trajectories only in one layer of candidate actions, and then judging whether the robot has successfully escaped the obstacle based on the statistics at the end of the trajectory or the entire trajectory. Although these solutions are simple to implement, their limited ability to express actions leads to low robot escape efficiency in complex, locally obstructed scenarios.

[0004] The information disclosed above in the background art of this application is only used to understand the background of the concept of this application, and may contain information that does not constitute prior art. Summary of the Invention

[0005] This application provides a robot extrication method, apparatus, computer equipment, and storage medium based on multi-layer search of curvature trajectory to solve the technical problem of low robot extrication efficiency in complex local obstruction scenarios in related technologies.

[0006] To achieve the above objectives, this application adopts the following technical solution: A robot escape method based on multi-layer curvature trajectory search, the method comprising: In response to the escape command, obtain the robot's current pose and the escape target point corresponding to the robot's current pose; The robot’s current pose is used as the current search node. A search layer is constructed based on the current search node by unfolding a curvature trajectory. The target escape trajectory is searched in the search layer based on the escape target point. If no escape trajectory for the target is found in the search layer, then several unfolding points generated in the search layer are obtained; Several unfolding points are used as the current search nodes, and the step of building a search layer based on the current search nodes through curvature trajectory unfolding is re-executed until the target escape trajectory is found; An escape plan is generated based on the target escape trajectory, and the escape plan is executed.

[0007] In one embodiment, responding to an escape command and obtaining the robot's current pose and the escape target point corresponding to the robot's current pose includes: In response to the escape command, the robot's current pose is obtained, a local map window is determined based on the robot's current pose, and a local layer corresponding to the local map window is constructed. The local layer includes: a local obstacle distance map, a robot distance map, a topology path distance map, and a direction cost map. Based on connected component constraints and local layers, the target point for escaping is determined in the local map window.

[0008] In one embodiment, determining a local map window based on the robot's current pose and constructing a local layer corresponding to the local map window includes: A local map window is captured centered on the robot's current pose, and a local obstacle binary map, robot cost map, and topological path constraint map corresponding to the local map window are generated. A local obstacle distance map is obtained from the local obstacle binary map, a robot distance map is obtained from the robot cost map, a topological path distance map is obtained from the topological path constraint map, and a direction cost map is obtained by assigning costs based on the deviation angle of each travel direction relative to the robot's current pose, using the robot's current pose as a reference.

[0009] In one embodiment, determining the escape target point on the local map window based on connected component constraints and local layers includes: Determine the locally feasible region of the local map window, and using the robot's current pose as the seed point, perform connected component extraction on the locally feasible region to obtain the reachable region connected to the robot's current pose; Obtain the obstacle safety distance cost corresponding to each idle point in the reachable area based on the local obstacle distance map, obtain the relative robot position cost corresponding to each idle point in the reachable area based on the robot distance map, obtain the topology path constraint cost corresponding to each idle point in the reachable area based on the topology path distance map, and obtain the direction cost corresponding to each idle point in the reachable area based on the direction cost map. The obstacle safety distance cost, relative robot position cost, topological path constraint cost, and orientation cost of each free point in the reachable area are fused to obtain the local fusion total cost corresponding to each free point in the reachable area. The idle point corresponding to the minimum value in the total cost of local fusion is taken as the escape target point.

[0010] In one embodiment, searching for a target escape trajectory in the search layer based on the escape target point includes: Collision evaluation is performed on multiple curvature trajectory primitives generated discretely based on the current search node within the search layer to obtain the collision-free prefix on each curvature trajectory primitive; The trajectory points on the collision-free prefix are scanned point by point, and the escape determination result of each trajectory point is determined according to the escape target point; If there is a trajectory point on the collision-free prefix that is a candidate point for escaping, then the effective trajectory segment between the initial point of the collision-free prefix and the candidate point for escaping is taken as the candidate escaping trajectory. When there is a candidate escape trajectory in the search layer, the candidate escape trajectory is taken as the target escape trajectory. When there are multiple candidate escape trajectories in the search layer, the target escape trajectory is determined from the multiple candidate escape trajectories. If none of the collision-free prefixes contain a trajectory point whose escape determination result is an escape candidate point, then there is no target escape trajectory in the search layer.

[0011] In one embodiment, the curvature trajectory primitives include: a forward straight curvature trajectory primitive, a forward arc curvature trajectory primitive, a backward straight curvature trajectory primitive, a backward arc curvature trajectory primitive, and a stationary turning curvature trajectory primitive.

[0012] In one embodiment, the trajectory points on the collision-free prefix are scanned point by point to determine the escape determination result for each trajectory point, including: The trajectory points on the collision-free prefix are scanned point by point, and the first obstacle safety margin and the distance from the trajectory point to the escape target point are calculated for each trajectory point. Determine the second obstacle safety margin corresponding to the escape target point, and determine the escape judgment result for each trajectory point based on the distance from the trajectory point to the escape target point, the first obstacle safety margin, and the second obstacle safety margin.

[0013] In one embodiment, determining the escape determination result for each trajectory point based on the distance from the trajectory point to the escape target point, the first obstacle safety margin, and the second obstacle safety margin includes: Obtain the preset first distance threshold and second distance threshold; If the distance from the trajectory point to the escape target point is less than or equal to the first distance threshold, and the difference between the second obstacle safety margin and the first obstacle safety margin is less than or equal to the second distance threshold, then the escape determination result corresponding to the trajectory point is an escape candidate point. If the distance from the trajectory point to the escape target point is greater than the first distance threshold, and / or the difference between the second obstacle safety margin and the first obstacle safety margin is greater than the second distance threshold, then the escape determination result corresponding to the trajectory point is not an escape candidate point.

[0014] In one embodiment, when multiple candidate escape trajectories exist in the search layer, determining the target escape trajectory from the multiple candidate escape trajectories includes: If there are multiple candidate escape trajectories in the search layer, then determine the distance between each candidate escape trajectory and the escape target point, the total path, and the total backtrack distance; Based on the distance between each target's escape trajectory and the escape target point, the total path, and the total retreat distance, the multiple target escape trajectories are prioritized to obtain the priority ranking result. The target escape trajectory is determined from multiple candidate escape trajectories based on the priority ranking results.

[0015] In one embodiment, generating an escape plan based on the target escape trajectory and executing the escape plan includes: Based on the target escape trajectory, backtrack upwards layer by layer along the search layer hierarchy, and match the effective trajectory segments corresponding to each previous search layer in sequence. The effective trajectory segment is the trajectory segment between the initial point without collision prefix and the expansion point of the next search layer. The effective trajectory segments of each preceding search layer are sequentially connected end-to-end with the target escape trajectory in hierarchical order to form an escape plan. Record the curvature trajectory primitive point series corresponding to the target escape trajectory and the effective trajectory segments of each preceding search layer, and construct the effective trajectory segment endpoint index corresponding to the target escape trajectory and the effective trajectory segments of each preceding search layer. The effective trajectory segment endpoint index is configured to map the effective trajectory segment endpoint, inter-layer intersection, path length accumulation, and backtracking distance accumulation. The endpoints of valid trajectory segments are read based on the endpoint index of the valid trajectory segments, and the escape plan is executed according to the endpoints of the valid trajectory segments corresponding to each valid trajectory segment.

[0016] In one embodiment, the method further includes: When executing the escape plan, a forward safety check is performed. If there is a collision risk within the preset forward safety check range, a braking command is output. If the duration of the braking command exceeds a preset duration threshold, the execution of the escape plan is deemed to have failed.

[0017] In one embodiment, the method further includes: When the robot enters the preset distance threshold where the end point of the current valid trajectory segment is located, record the real-time distance between the robot and the end point of the current valid trajectory segment. Within a preset distance threshold, the minimum distance value among all real-time distances is identified; If the minimum distance value is greater than the preset target proximity threshold, and within a predetermined duration after the minimum distance value is reached, the real-time distance between the robot and the endpoint of the current effective trajectory segment is always greater than the minimum distance value, then the execution of the current effective trajectory segment is determined to be unstable, and a failure message is returned.

[0018] In one embodiment, the method further includes: Determine the segmentation threshold for the current valid trajectory segment; When the robot reaches the segment determination threshold of the current effective trajectory segment, the robot's current speed is reduced to the predetermined speed threshold.

[0019] This application also provides a robot extrication device based on multi-layer curvature trajectory search, the device comprising: The response module is used to respond to the escape command, obtain the robot's current pose and the escape target point corresponding to the robot's current pose; The first search module is used to take the robot's current pose as the current search node, construct a search layer based on the current search node by unfolding a curvature trajectory, and search for the target escape trajectory in the search layer based on the escape target point. The generation module is used to obtain several unfolding points generated in the search layer if there is no target escape trajectory in the search layer. The second search module is used to take several unfolded points as the current search nodes and re-execute the step of building a search layer based on the current search nodes through curvature trajectory unfolding until the target escape trajectory is found. An execution module is used to generate an escape plan based on the target escape trajectory and execute the escape plan.

[0020] This application also provides a computer device, including: a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of the method described above.

[0021] This application also provides a computer-readable storage medium storing a computer program that can be executed to implement the steps of the robot extrication method based on curvature trajectory multilayer search as described above.

[0022] The beneficial effects of this invention are as follows: The method of this embodiment obtains the robot's current pose and the corresponding escape target point by responding to an escape command; it uses the robot's current pose as the current search node, constructs a search layer based on the current search node using curvature trajectory expansion, and searches for a target escape trajectory in the search layer based on the escape target point; if no target escape trajectory exists in the search layer, it obtains several expansion points generated in the search layer; it uses these expansion points as the current search node, and re-executes the step of constructing a search layer based on the current search node using curvature trajectory expansion until a target escape trajectory is found; it generates an escape plan based on the target escape trajectory and executes the escape plan. This application adopts a curvature trajectory expansion method and a multi-layer search method, with the search progressing layer by layer. The next layer is searched only after the current layer is completed, in order to select a more direct escape path and avoid the problem of entering a deep search too early, which leads to local optima. This results in a more reasonable combination of escape actions, which can adapt to complex obstructed scenarios and improve the robot's escape efficiency. Attached Figure Description

[0023] Figure 1 This is a flowchart illustrating a robot escape method based on multi-layer search of curvature trajectory in one embodiment.

[0024] Figure 2 This is a schematic diagram of a robot encountering partial obstruction in one embodiment.

[0025] Figure 3 This is a schematic diagram of the curvature trajectory primitive search space expansion in one embodiment.

[0026] Figure 4 This is a structural block diagram of a robot escape device based on multi-layer search of curvature trajectory in one embodiment.

[0027] Figure 5 This is a functional principle block diagram of a computer device in one embodiment. Detailed Implementation

[0028] To make the objectives, technical solutions, and advantages of this invention clearer and more explicit, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0029] Currently, when robots encounter localized obstructions, have limited chassis posture, or are unable to follow their planned local trajectory within a short period due to complex surrounding obstacle distribution, they tend to repeatedly probe within the local area. This manifests as small forward and backward movements, swaying left and right, frequent braking, or continuous on-the-spot adjustments within a narrow local area, ultimately failing to effectively escape the current obstructed state. Such scenarios are common in narrow passages, L-shaped corners, localized blind spots, situations where one side of the vehicle is close to an obstacle, or when there is limited but not completely blocked space on the local map.

[0030] Existing solutions are relatively simple, such as reversing a fixed distance, turning at a fixed angle, simply finding the nearest free point near the robot, or searching for feasible trajectories only in one layer of candidate actions, and then determining whether the robot has successfully escaped the obstacle based on statistics at the end of the trajectory or the entire trajectory. While these solutions are simple to implement, their limited motion expression capabilities lead to low robot escape efficiency in complex, locally obstructed scenarios. In summary, existing robot escape solutions suffer from the following problems: (1) Although the newly selected target point appears to be free, it is not connected to the robot's current position and is actually unreachable; (2) The ability to express fixed backward or fixed turning movements is limited, making it difficult to form combined movements such as retreating first and then turning, or turning first and then advancing; (3) If success is judged solely based on the statistics of the entire trajectory, it is easy for the success determination to drift. (4) If forward safety checks and segmentation control are lacking during the execution phase, it is easy to collide with obstacles again or the execution is unstable.

[0031] This application, without relying on simple fixed actions, starts from the robot's current position and combines local reachable areas, local obstacle distribution, directional costs, and hierarchical trajectory search results to generate a more stable, reasonable, and executable escape solution.

[0032] The following describes, with reference to the accompanying drawings, a robot escaping method, apparatus, computer device, and storage medium based on multi-layer curvature trajectory search according to embodiments of this application. Addressing the problem of low robot escaping efficiency in complex, locally obstructed scenarios mentioned in the background art, this application provides a robot escaping method based on multi-layer curvature trajectory search. In this method, in response to an escaping command, the robot's current pose and the corresponding escaping target point are obtained; the robot's current pose is used as the current search node; a search layer is constructed based on the current search node using curvature trajectory expansion; a target escaping trajectory is searched within the search layer based on the escaping target point; if no target escaping trajectory exists in the search layer, several expansion points generated in the search layer are obtained; these expansion points are used as the current search node, and the step of constructing the search layer based on the current search node using curvature trajectory expansion is repeated until a target escaping trajectory is found; an escaping plan is generated based on the target escaping trajectory, and the escaping plan is executed. This application employs a curvature trajectory unfolding method and a multi-layer search method. The search proceeds layer by layer, and the next layer is searched only after the current layer is completed. This allows for the selection of a more direct escape path and avoids the problem of entering a deep search too early, which could lead to local optima. As a result, a more reasonable combination of escape actions is generated, which can adapt to complex obstructed scenarios and improve the robot's escape efficiency.

[0033] The robot escape method based on curvature trajectory multi-layer search provided in this application embodiment can be applied to various automated or semi-automated equipment, such as robots performing various tasks.

[0034] Please see Figure 1 This application uses the application of this method to a robot as an example for illustration, that is, the robot acts as the execution subject. The robot escape method based on multi-layer search of curvature trajectory described in this embodiment of the invention includes the following steps S100 to S500: Step S100: Respond to the escape command and obtain the robot's current pose and the escape target point corresponding to the robot's current pose.

[0035] Among them, the escape command refers to the control command issued by the host computer or controller to enable the robot to get out of the trapped state when it is stuck between obstacles, trapped in a dead end, blocked in its path, or unable to move forward or turn normally. For example, it can obtain the robot's current pose, including the robot's current position, orientation, linear velocity, angular velocity, etc., and determine whether the robot is trapped based on the robot's current pose.

[0036] Please see Figure 2 , Figure 2This diagram illustrates a typical scenario where a robot is in a locally obstructed environment. The figure shows the robot's current position, orientation, distribution of surrounding obstacles, topological path or orientation reference, and the successfully selected target escape trajectory obtained from multiple candidate curvature trajectory primitives unfolded based on the robot's current pose. This diagram serves to illustrate the local obstruction problem addressed by this invention, and that this invention does not rely solely on fixed backward movement or a single action, but rather generates multiple candidate motion modes based on the robot's current state information, combining them with subsequent target point selection and hierarchical search processes to achieve escape.

[0037] For example, a local safety check is performed before the robot starts its escape from a difficult situation. If the robot's current pose does not meet the safe start conditions within a short distance, the escape speed will not be output immediately in this cycle (i.e., this detection). Instead, a stop command will be issued to maintain a waiting state. Only when this unsafe state continues for multiple cycles, i.e., multiple consecutive detections show an unsafe state, is it confirmed that the current position does not meet the conditions for starting to escape from a difficult situation. In this case, the escape attempt is deemed a failure and the robot exits the escape process. This strategy can effectively avoid the problems of false shutdowns and false escape failures caused by local map fluctuations, sensor jitter, or instantaneous obstacle noise.

[0038] In one embodiment of this application, the step "responding to the escape command and obtaining the robot's current pose and the escape target point corresponding to the robot's current pose" specifically includes: responding to the escape command, obtaining the robot's current pose, determining a local map window based on the robot's current pose, and constructing a local layer corresponding to the local map window, wherein the local layer includes: a local obstacle distance map, a robot distance map, a topological path distance map, and a direction cost map; and determining the escape target point in the local map window based on connected component constraints and the local layer.

[0039] For example, a connected component constraint refers to a topological restriction that requires the target region to be a completely connected, unfragmented area; the escape target point refers to the target point selected during this escape process. For the orientation cost graph, the orientation directly in front of the robot has the lowest cost, and the cost increases progressively with orientation deviation, with the orientation away from the robot having the highest cost.

[0040] This application selects a truly reachable escape target point by using connected component constraints and local layers, solving the problem of existing technologies that simply find a free point near the robot as the target point without connected component constraints.

[0041] In this embodiment of the application, the step "determine a local map window based on the robot's current pose and construct a local layer corresponding to the local map window" specifically includes: A local map window is captured centered on the robot's current pose, and a local obstacle binary map, robot cost map, and topological path constraint map corresponding to the local map window are generated.

[0042] A local obstacle distance map is obtained from the local obstacle binary map, a robot distance map is obtained from the robot cost map, a topological path distance map is obtained from the topological path constraint map, and a direction cost map is obtained by assigning costs based on the deviation angle of each travel direction relative to the robot's current pose, using the robot's current pose as a reference.

[0043] For example, pre-stored topology path information and vehicle geometric constraints are obtained. The topology path information includes the topology path or the current upper-level path direction reference; the vehicle geometric constraints include geometric constraint information such as the robot's outer boundary (footprint), circumscribed circle radius, and vehicle width. Based on the pre-stored topology path information and vehicle geometric constraints, a local obstacle binary map, a robot cost map, and a topology path constraint map corresponding to the local map window are generated.

[0044] This application constructs a local layer corresponding to a local map window to facilitate the calculation of the total cost of local fusion based on the local layer.

[0045] In one embodiment of this application, the step "determining the escape target point on the local map window based on connected component constraints and local layers" specifically includes: Determine the locally feasible region of the local map window, and use the robot's current pose as the seed point to perform connected component extraction on the locally feasible region to obtain the reachable region connected to the robot's current pose.

[0046] The locally feasible region within the local map window is a collision-free, traversable space centered on the robot's current pose, after removing obstacles, expanded safe zones, and unknown areas, and satisfying the robot's geometric, kinematic, and sensor constraints. Using the robot's current pose as a seed point, connected component extraction is performed on the locally feasible region to obtain reachable areas connected to the robot's current pose, while filtering out regions that are free but not connected to the robot's current pose.

[0047] Obtain the obstacle safety distance cost corresponding to each idle point in the reachable area based on the local obstacle distance map, obtain the relative robot position cost corresponding to each idle point in the reachable area based on the robot distance map, obtain the topological path constraint cost corresponding to each idle point in the reachable area based on the topological path distance map, and obtain the direction cost corresponding to each idle point in the reachable area based on the direction cost map.

[0048] Specifically, for obstacle safety distance cost (obs_cost), the closer the idle point is to the obstacle, the higher the cost; the farther away from the obstacle, the lower the cost. For relative robot position cost (robot_cost), the closer the idle point is to the robot, the lower the cost; the farther away from the robot, the higher the cost, used to avoid selecting an excessively distant escape target point. For topology path constraint cost (topo_cost), the further the idle point deviates from the set topology path width range, the higher the cost; the closer it is to and within this range, the lower the cost. For direction cost (direction_cost), the closer the idle point is to the robot's current facing direction, the lower the cost; the greater the angle of deviation from the current facing direction, the higher the cost; the direction cost is the highest when the idle point is behind the robot.

[0049] The obstacle safety distance cost, relative robot position cost, topological path constraint cost, and direction cost of each free point in the reachable area are fused to obtain the local fusion total cost corresponding to each free point in the reachable area; the free point corresponding to the minimum value of the local fusion total cost is taken as the escape target point.

[0050] For example, the formula for calculating the total cost of local fusion is: cost = obs_cost + topo_cost + direction_cost + robot_cost.

[0051] For example, within the reachable area, the obstacle safety distance cost, direction cost, relative robot position cost, and topological path constraint cost corresponding to each idle point can be assigned corresponding weights and then normalized and weighted to obtain the total local fusion cost. It is understood that the weights of each cost item can be adaptively adjusted according to different robot types or different scenarios.

[0052] The smaller the total cost of local fusion for an idle point, the more suitable that idle point is as a target point for escaping trouble. Therefore, this application selects the idle point with the minimum total cost of local fusion as the target point for escaping trouble in this round.

[0053] This application first constructs a local layer, and then combines the connected component results to determine the escape target point and search area, ensuring that the escape target point not only comprehensively considers the relationship between obstacles, directions and positions, but is also truly connected to the robot's current position and is actually reachable.

[0054] like Figure 1 As shown, the robot escape method based on multi-layer curvature trajectory search further includes the following steps: Step S200: Take the robot's current pose as the current search node, construct a search layer based on the current search node by unfolding the curvature trajectory, and search for the target escape trajectory in the search layer based on the escape target point.

[0055] like Figure 3 As shown, Figure 3 This represents a single layer of curvature trajectory primitive search space based on the robot's current position and orientation. Green represents valid points without collision, red represents collision points, blue represents the selected trajectory, yellow outlines the robot footprint, and purple represents surrounding obstacles. Figure 3 This invention illustrates how it organizes the candidate search space using curvature and performs collision filtering simultaneously during the unfolding process, thereby retaining safe candidates and eliminating invalid ones. The figure only shows one layer of unfolding for ease of understanding; the search framework of this invention is not limited to a single layer and can be extended to deeper layers as needed.

[0056] This application adopts a curvature-based trajectory unfolding method instead of directly unfolding candidate actions based on speed limits and angular velocities. This reduces the number of candidate trajectories and lowers the search space while ensuring the extrication effect.

[0057] In this embodiment of the application, the step of "searching for the target escape trajectory in the search layer based on the escape target point" specifically includes: Collision evaluation is performed on multiple curvature trajectory primitives generated discretely based on the current search node within the search layer to obtain a collision-free prefix on each curvature trajectory primitive.

[0058] In path planning, a curvature trajectory primitive is a basic unit of smooth trajectory defined by curvature characteristics. For example, collision evaluation of a curvature trajectory primitive involves performing obstacle collision detection point-by-point and segment-by-segment along the path of this primitive, identifying trajectory segments that do not collide with obstacles and those that will. After collision evaluation, the collision-free prefix and the farthest collision-free point on the curvature trajectory primitive are obtained. The collision-free prefix refers to a continuous, completely passable trajectory primitive sub-segment starting from the initial pose of the trajectory. The farthest collision-free point is the terminating endpoint of this trajectory primitive sub-segment.

[0059] The trajectory points on the collision-free prefix are scanned point by point, and the escape determination result of each trajectory point is determined according to the escape target point.

[0060] The escape determination result is used to indicate whether the trajectory point meets the preset escape conditions. The preset escape conditions not only require that the current trajectory point is close enough to the escape target point, but also require that the current trajectory point and the escape target point have similar obstacle safety margins.

[0061] If there is a trajectory point on the collision-free prefix that is a candidate point for escape, then the effective trajectory segment between the initial point of the collision-free prefix and the candidate point for escape is taken as the candidate escape trajectory.

[0062] Specifically, if a trajectory point meets a preset escape condition, it is designated as an escape candidate point, and the valid trajectory segment from the initial point of the collision-free prefix to this escape candidate point is designated as a candidate escape trajectory. For example, if the first escape candidate point meeting the preset escape condition is obtained by scanning the collision-free prefix, scanning of that prefix can be stopped. Alternatively, scanning of trajectory points on the collision-free prefix can continue. If multiple trajectory points exist, the optimal escape candidate point is selected from these candidates, and this escape candidate point becomes the endpoint of the candidate escape trajectory.

[0063] When there is a candidate escape trajectory in the search layer, the candidate escape trajectory is taken as the target escape trajectory. When there are multiple candidate escape trajectories in the search layer, the target escape trajectory is determined from the multiple candidate escape trajectories.

[0064] In other words, if there are multiple candidate escape points on the collision-free prefix of multiple curvature trajectory primitives in the search layer, then multiple candidate escape trajectories will be obtained, and the optimal candidate escape trajectory will be selected as the target escape trajectory.

[0065] If none of the collision-free prefixes contain a trajectory point whose escape determination result is an escape candidate point, then there is no target escape trajectory in the search layer.

[0066] In other words, if none of the collision-free prefixes of all curvature trajectory primitives in the search layer have escape candidate points, it means that there is no target escape trajectory in the search layer.

[0067] This application uses a point-by-point judgment method, employing the current scan point rather than the statistics of the entire trajectory to determine whether the preset escape conditions have been met. This solves the problem of low accuracy in traditional escape methods, which rely on the statistics of the end of the trajectory or the entire trajectory to determine success.

[0068] In this embodiment of the application, the curvature trajectory primitives include: forward straight curvature trajectory primitives, forward arc curvature trajectory primitives, backward straight curvature trajectory primitives, backward arc curvature trajectory primitives, and in-place turning curvature trajectory primitives.

[0069] For example, the curvature of the forward straight curvature trajectory primitive is 0, indicating that the robot moves forward in a straight line without turning and maintaining its heading. The curvature of the forward arc curvature trajectory primitive is a constant, not 0, indicating that the robot moves forward while turning at a constant speed, following a standard circular arc trajectory. The curvature of the backward straight curvature trajectory primitive is 0, indicating that the robot reverses in a straight line with its heading remaining unchanged. The curvature of the backward arc curvature trajectory primitive is a constant, not 0, indicating that the robot reverses while turning, following a circular arc trajectory. The curvature of the stationary turning curvature trajectory primitive approaches infinity, indicating that the robot's center point remains stationary, and it rotates in place to change its heading without translation. To avoid excessively long backward movements for a single trajectory, independent length limits can be set for the backward straight curvature trajectory primitive and the backward arc curvature trajectory primitive. For example, the unfolding step size of the translation curvature trajectory primitive, the single length limit of the backward curvature trajectory primitive, and the representative posture extraction method of the stationary turning curvature trajectory primitive can be adjusted according to different chassis.

[0070] It is understood that the curvature trajectory primitives of this application include at least forward straight curvature trajectory primitives, forward arc curvature trajectory primitives, backward straight curvature trajectory primitives, backward arc curvature trajectory primitives, and in-place turning curvature trajectory primitives, and may also include other types of curvature trajectory primitives.

[0071] This application, by employing various types of curvature trajectory primitives, can generate more natural combinations of escape actions, avoiding the problem of monotonous action patterns caused by the use of fixed backward movement or fixed turning in place in traditional escape methods.

[0072] In one embodiment of this application, the step "scanning the trajectory points on the collision-free prefix point by point and determining the escape determination result for each trajectory point" specifically includes: The trajectory points on the collision-free prefix are scanned point by point, and the first obstacle safety margin and the distance from the trajectory point to the escape target point are calculated for each trajectory point.

[0073] For example, this application recalculates the first obstacle safety margin, the distance from the trajectory point to the escape target point, and the cumulative prefix length for each trajectory point on a collision-free prefix. The first obstacle safety margin refers to the shortest distance from the trajectory point to the nearest obstacle expansion boundary, taking into account the robot's external radius and the obstacle expansion boundary.

[0074] Determine the second obstacle safety margin corresponding to the escape target point, and determine the escape judgment result for each trajectory point based on the distance from the trajectory point to the escape target point, the first obstacle safety margin, and the second obstacle safety margin.

[0075] For example, the factors required for this application to determine whether a trajectory point meets the preset escape conditions include: the distance from the trajectory point to the escape target point, the first obstacle safety margin, and the second obstacle safety margin. The second obstacle safety margin corresponding to the escape target point refers to the shortest distance from the escape target point to the nearest obstacle expansion boundary, taking into account the robot's external radius and the obstacle expansion boundary.

[0076] This application determines whether a trajectory point can escape from a difficult situation by using the distance from the trajectory point to the target point, the safety margin of the first obstacle, and the safety margin of the second obstacle, thereby improving the success rate of escaping difficulties.

[0077] In this embodiment of the application, the step "determine the escape judgment result of each trajectory point based on the distance from the trajectory point to the escape target point, the first obstacle safety margin, and the second obstacle safety margin" specifically includes: obtaining a preset first distance threshold and a second distance threshold; if the distance from the trajectory point to the escape target point is less than or equal to the first distance threshold, and the difference between the second obstacle safety margin and the first obstacle safety margin is less than or equal to the second distance threshold, then the escape judgment result corresponding to the trajectory point is an escape candidate point; if the distance from the trajectory point to the escape target point is greater than the first distance threshold, and / or the difference between the second obstacle safety margin and the first obstacle safety margin is greater than the second distance threshold, then the escape judgment result corresponding to the trajectory point is not an escape candidate point.

[0078] For example, the preset escape conditions are: the distance from the trajectory point to the escape target point is ≤0.1m, and the difference between the second obstacle safety margin and the first obstacle safety margin is ≤0.05m. If the preset escape conditions are met, the trajectory point is determined to be a candidate escape point.

[0079] This application not only requires the current point to be close enough to the target point for escaping the predicament, but also requires the current point and the target point to have similar obstacle safety margins, thereby improving the success rate of escaping the predicament.

[0080] In this embodiment of the application, the step "when there are multiple candidate escape trajectories in the search layer, determine the target escape trajectory among the multiple candidate escape trajectories" specifically includes: if there are multiple candidate escape trajectories in the search layer, determine the distance, total path, and total backtracking distance of each candidate escape trajectory to the escape target point; prioritize the multiple target escape trajectories based on the distance, total path, and total backtracking distance of each target escape trajectory to the escape target point, and obtain a priority ranking result; determine the target escape trajectory among the multiple candidate escape trajectories according to the priority ranking result.

[0081] For example, if candidate escape trajectories already exist in the search layer, then only within that search layer are all candidate escape trajectories sorted and optimized, without further expansion to deeper layers. During sorting, candidate escape trajectories that are closer to the escape target point, have a shorter total path, and a smaller total backtrack distance can be prioritized. The distance between the target escape trajectory and the escape target point refers to calculating the minimum distance among all trajectory points on the target escape trajectory and their distances to the escape target point.

[0082] This application improves the efficiency of escaping difficulties by selecting the optimal target escaping trajectory from multiple candidate trajectories.

[0083] Step S300: If there is no target escape trajectory in the search layer, then obtain several unfolding points generated in the search layer; Here, the unfolded point refers to the deep unfolded point of the next round of trajectory generation. For example, for translational curvature trajectory primitives (such as forward straight curvature trajectory primitives, forward arc curvature trajectory primitives, backward straight curvature trajectory primitives, and backward arc curvature trajectory primitives), an unfolded point can be sampled at a preset fixed distance according to the cumulative prefix length, and the farthest non-collision point is also used as an unfolded point; for in-situ turning curvature trajectory primitives, it is not necessary to expand all angle samples, but only a small number of representative attitudes can be retained as unfolded points.

[0084] Step S400: Take several unfolded points as the current search nodes, and re-execute the step of constructing the search layer based on the current search nodes through curvature trajectory unfolding, until the target escape trajectory is found.

[0085] When no target escape trajectory exists in the search layer, this application uses the unfolded point generated by the search layer as the subsequent search front, and continues to discretize the next segment of curvature trajectory primitives from the unfolded point, thereby realizing hierarchical curvature trajectory primitive search. Furthermore, the search proceeds layer by layer, only entering the next layer after the current layer search is completed, prioritizing more direct escape paths and avoiding premature entry into deep searches that could lead to local optima. In addition, the hierarchical search framework can be further expanded to more layers to adapt to more complex obstructed scenarios.

[0086] like Figure 1 As shown, the robot escape method based on multi-layer curvature trajectory search further includes the following steps: Step S500: Generate an escape plan based on the target escape trajectory and execute the escape plan.

[0087] In this embodiment of the application, step S500 specifically includes: Based on the target escape trajectory, the search layers are traced back upwards layer by layer, and the effective trajectory segments corresponding to each previous search layer are matched and associated in sequence. The effective trajectory segment is the trajectory segment between the initial point without collision prefix and the expansion point of the next search layer. The effective trajectory segments of each previous search layer and the target escape trajectory are connected end to end in the order of hierarchical progress to form an escape plan.

[0088] In this application, a plan for escaping difficulties consisting of multiple effective trajectory segments is formed through hierarchical curvature trajectory primitive search.

[0089] Record the curvature trajectory primitive point series corresponding to the target escape trajectory and the effective trajectory segments of each preceding search layer. Construct the effective trajectory segment endpoint index corresponding to the target escape trajectory and the effective trajectory segments of each preceding search layer. The effective trajectory segment endpoint index is configured to map the effective trajectory segment endpoint, inter-layer intersection, path length accumulation, and backtracking distance accumulation. Read the effective trajectory segment endpoint based on the effective trajectory segment endpoint index, and execute the escape plan according to the effective trajectory segment endpoints corresponding to each effective trajectory segment.

[0090] For example, each escape trajectory retains a complete set of curvature trajectory primitive points for display and recording, but the actual endpoint of the valid segment is uniformly defined by the valid trajectory segment endpoint index. Inter-layer transitions, path length accumulation, backtracking distance accumulation, and the endpoint of the valid trajectory segment are all based on the point corresponding to this index. That is, the end of the complete point set and the actual execution endpoint may differ; execution and searching are based on the uniformly defined valid trajectory segment endpoint. After the escape plan is generated, the system executes the escape plan segment by segment. During the execution phase, the system first reads the valid trajectory segment endpoint corresponding to the valid trajectory segment endpoint index of the current segment, and then proceeds towards the valid trajectory segment endpoint.

[0091] This application generates more natural combined escape actions through hierarchical curvature trajectory primitive search, solving the problem that traditional escape methods only perform single-layer search and are difficult to express combined actions; and by uniformly defining the real valid endpoint of each segment through the valid trajectory segment endpoint index, the semantics of the search, inter-layer handover and execution stages are consistent.

[0092] In this embodiment of the application, the method further includes: performing a forward safety check when executing the escape plan; if there is a collision risk within a preset forward safety check range, then outputting a braking command; if the duration of the braking command is longer than a preset duration threshold, then determining that the execution of the escape plan has failed.

[0093] For example, when performing a forward safety check, if there is a collision risk within the preset forward safety check range, the braking command will be output first in the current check cycle; if the blocked state occurs for multiple consecutive check cycles, the current round of escape execution will be determined to have failed.

[0094] This application improves the stability of executing the escape plan through forward-looking safety checks, and solves the problem of traditional escape methods where actions are generated and then executed directly without forward-looking safety checks.

[0095] In one embodiment of this application, the method further includes: when the robot enters a preset distance threshold where the endpoint of the current effective trajectory segment is located, recording the real-time distance between the robot and the endpoint of the current effective trajectory segment; within the preset distance threshold, identifying the minimum distance value among the real-time distances; if the minimum distance value is greater than a preset target proximity threshold, and within a predetermined duration after the occurrence of the minimum distance value, the real-time distance between the robot and the endpoint of the current effective trajectory segment is always greater than the minimum distance value, then it is determined that the execution of the current effective trajectory segment is unstable, and a failure message is returned.

[0096] For example, this application also includes proximity and distance monitoring. When the robot enters a predetermined distance threshold near the end point of the current valid trajectory segment, it begins recording the real-time distance. If the minimum distance value is greater than a preset target proximity threshold, it indicates that the robot has not yet approached the end point of the current valid trajectory segment. If, within a predetermined duration after the occurrence of the minimum distance value, the real-time distance between the robot and the end point of the current valid trajectory segment remains greater than the minimum distance value, it indicates that the robot has not further approached the end point of the current valid trajectory segment, but instead continues to move away from it. In this case, the execution of the current valid trajectory segment is determined to be unstable, and a failure message is returned.

[0097] This application further enhances the stability of executing the escape plan by being closer to and further away from monitoring.

[0098] In this embodiment of the application, the method further includes: determining a segmentation determination threshold for the current valid trajectory segment; when the robot travels to the segmentation determination threshold for the current valid trajectory segment, reducing the robot's current speed to a predetermined speed threshold.

[0099] The segment determination threshold refers to the position on the current valid trajectory segment that is close to the end of the valid trajectory segment. For example, when the robot approaches the end of the valid trajectory segment and reaches the segment determination threshold, it does not directly switch to the next valid trajectory segment. Instead, it reduces its current speed to near zero before switching to the next valid trajectory segment. For instance, if the valid trajectory segment type is translation, the segment determination threshold is mainly determined based on the distance to the end of the current valid trajectory segment; if the valid trajectory segment type is turning in place, the segment determination threshold is mainly determined based on the yaw error.

[0100] This application improves the stability of executing the escape plan through segmented deceleration control, and solves the problem of traditional escape methods that directly execute actions after generating them, lacking segmented deceleration control.

[0101] This application, by constructing local layers, connected component constraints, and hierarchical trajectory search, makes it easier for the robot to select realistically reachable escape target points and generate more reasonable combinations of escape actions. Simultaneously, by unifying the semantics of effective trajectory segment endpoints through effective trajectory segment endpoint indexing, and by implementing forward-looking safety, approach / distance monitoring, and segment-based deceleration control during the execution phase, it improves the stability, safety, and executability of the escape plan. It is understood that the segment-based judgment threshold, the preset distance threshold for approach / distance monitoring, and the forward-looking safety check range during the execution phase can be adjusted according to different chassis dynamic constraints.

[0102] In one embodiment, such as Figure 4 As shown, based on the above-mentioned robot escape method based on multi-layer curvature trajectory search, the present invention also provides a robot escape device based on multi-layer curvature trajectory search, the device comprising: The response module 100 is used to respond to the escape command and obtain the current pose of the robot and the escape target point corresponding to the current pose of the robot; The first search module 200 is used to take the robot's current pose as the current search node, construct a search layer based on the current search node by unfolding a curvature trajectory, and search for the target escape trajectory in the search layer based on the escape target point. The generation module 300 is used to obtain several unfolding points generated in the search layer if there is no target escape trajectory in the search layer. The second search module 400 is used to take several unfolded points as the current search nodes and re-execute the step of constructing a search layer based on the current search nodes through curvature trajectory unfolding until the target escape trajectory is found. The execution module 500 is used to generate an escape plan based on the target escape trajectory and execute the escape plan.

[0103] In one embodiment, the response module 100 specifically includes: The layer construction unit is used to respond to the escape command, obtain the robot's current pose, determine a local map window based on the robot's current pose, and construct a local layer corresponding to the local map window. The local layer includes: a local obstacle distance map, a robot distance map, a topology path distance map, and a direction cost map. The target point determination unit is used to determine the escape target point in the local map window based on connected component constraints and local layers.

[0104] In one embodiment, the layer building unit specifically includes: The cropping subunit is used to crop a local map window centered on the robot's current pose, and generate a local obstacle binary map, a robot cost map, and a topological path constraint map corresponding to the local map window; A sub-unit is constructed to obtain a local obstacle distance map based on the local obstacle binary map, a robot distance map based on the robot cost map, a topological path distance map based on the topological path constraint map, and a direction cost map based on the robot's current pose and the deviation angle of each travel direction relative to the robot's current pose.

[0105] In one embodiment, the target point determination unit specifically includes: The region extraction subunit is used to determine the locally feasible region of the local map window, and to perform connected component extraction on the locally feasible region using the robot's current pose as the seed point to obtain the reachable region connected to the robot's current pose. The cost determination subunit is used to obtain the obstacle safety distance cost corresponding to each free point in the reachable area based on the local obstacle distance map, the relative robot position cost corresponding to each free point in the reachable area based on the robot distance map, the topology path constraint cost corresponding to each free point in the reachable area based on the topology path distance map, and the direction cost corresponding to each free point in the reachable area based on the direction cost map. The total cost calculation unit is used to fuse the obstacle safety distance cost, relative robot position cost, topological path constraint cost, and orientation cost of each idle point in the reachable area to obtain the local fusion total cost corresponding to each idle point in the reachable area. The target point determination sub-unit is used to select the idle point corresponding to the minimum value in the total cost of local fusion as the escape target point.

[0106] In one embodiment, the first search module 200 includes: The collision evaluation unit is used to evaluate the collision of multiple curvature trajectory primitives generated discretely based on the current search node within the search layer, and obtain the collision-free prefix on each curvature trajectory primitive; The scanning unit is used to scan the trajectory points on the collision-free prefix point by point and determine the escape judgment result of each trajectory point according to the escape target point; The candidate trajectory determination unit is used to determine the effective trajectory segment between the initial point of the collision-free prefix and the candidate point of escape if there is a trajectory point on the collision-free prefix whose escape determination result is an escape candidate point. The target trajectory determination unit is used to determine the target escape trajectory when there is a candidate escape trajectory in the search layer, and to determine the target escape trajectory among the multiple candidate escape trajectories when there are multiple candidate escape trajectories in the search layer; and to determine that there is no target escape trajectory in the search layer if there are no trajectory points with escape determination results as escape candidate points on each of the non-collision prefixes.

[0107] In one embodiment, the curvature trajectory primitives include: forward straight curvature trajectory primitives, forward arc curvature trajectory primitives, backward straight curvature trajectory primitives, backward arc curvature trajectory primitives, and in-place turning curvature trajectory primitives.

[0108] In one embodiment, the scanning unit specifically includes: The distance calculation subunit is used to scan the trajectory points on the collision-free prefix point by point, and calculate the first obstacle safety margin corresponding to each trajectory point and the distance from the trajectory point to the escape target point. The result determination subunit is used to determine the second obstacle safety margin corresponding to the escape target point. Based on the distance from the trajectory point to the escape target point, the first obstacle safety margin, and the second obstacle safety margin, the escape determination result of each trajectory point is determined.

[0109] In one embodiment, the result determination subunit is further configured to obtain a preset first distance threshold and a second distance threshold; if the distance from the trajectory point to the escape target point is less than or equal to the first distance threshold, and the difference between the second obstacle safety margin and the first obstacle safety margin is less than or equal to the second distance threshold, then the escape determination result corresponding to the trajectory point is an escape candidate point; if the distance from the trajectory point to the escape target point is greater than the first distance threshold, and / or the difference between the second obstacle safety margin and the first obstacle safety margin is greater than the second distance threshold, then the escape determination result corresponding to the trajectory point is not an escape candidate point.

[0110] In one embodiment, the target trajectory determination unit is further configured to determine the distance, total path, and total backtracking distance between each candidate escape trajectory and the escape target point if there are multiple candidate escape trajectories in the search layer; prioritize the multiple target escape trajectories based on the distance, total path, and total backtracking distance between each target escape trajectory and the escape target point to obtain a priority ranking result; and determine the target escape trajectory among the multiple candidate escape trajectories according to the priority ranking result.

[0111] In one embodiment, the execution module 500 specifically includes: The matching unit is used to backtrack upwards along the search layer by layer based on the target escape trajectory, and sequentially match the effective trajectory segments corresponding to each previous search layer. The effective trajectory segment is the trajectory segment between the initial point without collision prefix and the unfolding point of the next search layer. The splicing unit is used to connect the effective trajectory segments of each previous search layer with the target escape trajectory in a hierarchical order to form an escape plan. An index building unit is used to record the curvature trajectory primitive point column corresponding to the target escape trajectory and the effective trajectory segment of each preceding search layer, and to build the effective trajectory segment endpoint index corresponding to the target escape trajectory and the effective trajectory segment of each preceding search layer. The effective trajectory segment endpoint index is configured to map the effective trajectory segment endpoint, inter-layer intersection, path length accumulation, and backtracking distance accumulation. The execution unit is used to read the endpoint of the valid trajectory segment based on the endpoint index of the valid trajectory segment, and execute the escape plan according to the endpoint of the valid trajectory segment corresponding to each valid trajectory segment.

[0112] In one embodiment, the device further includes a forward-looking safety check module, used to perform a forward-looking safety check when executing an escape plan; if there is a collision risk within a preset forward-looking safety check range, a braking command is output; if the duration of the braking command is longer than a preset duration threshold, the execution of the escape plan is determined to have failed.

[0113] In one embodiment, the device further includes: a distance monitoring module, configured to record the real-time distance between the robot and the endpoint of the current effective trajectory segment when the robot enters a preset distance threshold; within the preset distance threshold, identify the minimum distance value among the real-time distances; if the minimum distance value is greater than a preset target proximity threshold, and within a predetermined duration after the occurrence of the minimum distance value, the real-time distance between the robot and the endpoint of the current effective trajectory segment is always greater than the minimum distance value, then the execution of the current effective trajectory segment is determined to be unstable, and a failure message is returned.

[0114] In one embodiment, the device further includes: a segmentation control module, configured to determine a segmentation threshold for the current valid trajectory segment; when the robot travels to the segmentation threshold of the current valid trajectory segment, the robot's current speed is reduced to a predetermined speed threshold.

[0115] It should be noted that the foregoing explanation of the robot escape method based on curvature trajectory multi-layer search also applies to the robot escape device based on curvature trajectory multi-layer search in this embodiment, and will not be repeated here.

[0116] This invention discloses a robot escape device based on multi-layer curvature trajectory search. Upon responding to an escape command, the device acquires the robot's current pose and the corresponding escape target point. The current pose is used as the current search node. A search layer is constructed based on the current search node using curvature trajectory expansion. A target escape trajectory is searched within this search layer based on the escape target point. If no target escape trajectory exists in the search layer, several expanded points generated within the search layer are acquired. These expanded points are then used as the current search node, and the process of constructing the search layer based on the current search node using curvature trajectory expansion is repeated until the target escape trajectory is found. An escape plan is generated based on the target escape trajectory and executed. This application employs a curvature trajectory expansion method and a multi-layer search method. The search proceeds layer by layer, with the next layer only being searched after the current layer is completed. This allows for the selection of a more direct escape path, avoiding premature entry into deep searches that could lead to local optima. Consequently, a more rational combination of escape actions is generated, enabling the device to adapt to complex obstructed scenarios and improving robot escape efficiency.

[0117] Figure 5 A schematic diagram of the structure of a computer device provided in an embodiment of this application. The device may include: The memory 501, the processor 502, and the computer program stored on the memory 501 and capable of running on the processor 502.

[0118] When the processor 502 executes the program, it implements the robot escape method based on multi-layer search of curvature trajectory provided in the above embodiments.

[0119] Furthermore, the equipment also includes: Communication interface 503 is used for communication between memory 501 and processor 502.

[0120] The memory 501 is used to store computer programs that can run on the processor 502.

[0121] The memory 501 may include high-speed RAM memory, and may also include non-volatile memory, such as at least one disk storage device.

[0122] If the memory 501, processor 502, and communication interface 503 are implemented independently, they can be interconnected via a bus to communicate with each other. The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, etc. Buses can be categorized as address buses, data buses, control buses, etc. For ease of representation, only one line is used in the diagram, but this does not imply that there is only one bus or one type of bus.

[0123] Optionally, in a specific implementation, if the memory 501, processor 502, and communication interface 503 are integrated on a single chip, then the memory 501, processor 502, and communication interface 503 can communicate with each other through an internal interface.

[0124] Processor 502 may be a central processing unit (CPU), an application specific integrated circuit (ASIC), or one or more integrated circuits configured to implement the embodiments of this application.

[0125] This embodiment also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described robot escape method based on multi-layer search of curvature trajectory.

[0126] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0127] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this application, "N" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0128] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or N executable instructions for implementing custom logic functions or processes, and the scope of the preferred embodiments of this application includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of this application pertain.

[0129] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can read and execute instructions from or in conjunction with such an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by or in conjunction with an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). In addition, computer-readable media can even be paper or other suitable media on which programs can be printed, because programs can be obtained electronically by optically scanning paper or other media, then editing, interpreting or otherwise processing them as necessary, and then storing them in computer memory.

[0130] It should be understood that the various parts of this application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, the N steps or methods can be implemented using software or firmware stored in memory and executed by a suitable instruction execution system. If implemented in hardware, as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0131] Those skilled in the art will understand that all or part of the steps of the methods described in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0132] Furthermore, the functional units in the various embodiments of this application can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0133] The storage medium mentioned above can be a read-only memory, a disk, or an optical disk, etc. Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of this application.

Claims

1. A robot escape method based on multi-layer curvature trajectory search, characterized in that, The method includes: In response to the escape command, obtain the robot's current pose and the escape target point corresponding to the robot's current pose; The robot’s current pose is used as the current search node. A search layer is constructed based on the current search node by unfolding a curvature trajectory. The target escape trajectory is searched in the search layer based on the escape target point. If no escape trajectory for the target is found in the search layer, then several unfolding points generated in the search layer are obtained; Several unfolding points are used as the current search nodes, and the step of building a search layer based on the current search nodes through curvature trajectory unfolding is re-executed until the target escape trajectory is found; An escape plan is generated based on the target escape trajectory, and the escape plan is executed.

2. The method according to claim 1, characterized in that, In response to an escape command, the robot's current pose and the corresponding escape target point are obtained, including: In response to the escape command, the robot's current pose is obtained, a local map window is determined based on the robot's current pose, and a local layer corresponding to the local map window is constructed. The local layer includes: a local obstacle distance map, a robot distance map, a topology path distance map, and a direction cost map. Based on connected component constraints and local layers, the target point for escaping is determined in the local map window.

3. The method according to claim 2, characterized in that, Based on the robot's current pose, a local map window is determined, and a local layer corresponding to the local map window is constructed, including: A local map window is captured centered on the robot's current pose, and a local obstacle binary map, robot cost map, and topological path constraint map corresponding to the local map window are generated. A local obstacle distance map is obtained from the local obstacle binary map, a robot distance map is obtained from the robot cost map, a topological path distance map is obtained from the topological path constraint map, and a direction cost map is obtained by assigning costs based on the deviation angle of each travel direction relative to the robot's current pose, using the robot's current pose as a reference.

4. The method according to claim 2, characterized in that, The step of determining the escape target point on the local map window based on connected component constraints and local layers includes: Determine the locally feasible region of the local map window, and using the robot's current pose as the seed point, perform connected component extraction on the locally feasible region to obtain the reachable region connected to the robot's current pose; Obtain the obstacle safety distance cost corresponding to each idle point in the reachable area based on the local obstacle distance map, obtain the relative robot position cost corresponding to each idle point in the reachable area based on the robot distance map, obtain the topology path constraint cost corresponding to each idle point in the reachable area based on the topology path distance map, and obtain the direction cost corresponding to each idle point in the reachable area based on the direction cost map. The obstacle safety distance cost, relative robot position cost, topological path constraint cost, and orientation cost of each free point in the reachable area are fused to obtain the local fusion total cost corresponding to each free point in the reachable area. The idle point corresponding to the minimum value in the total cost of local fusion is taken as the escape target point.

5. The method according to claim 1, characterized in that, Based on the escape target point, search for the target escape trajectory in the search layer, including: Collision evaluation is performed on multiple curvature trajectory primitives generated discretely based on the current search node within the search layer to obtain the collision-free prefix on each curvature trajectory primitive; The trajectory points on the collision-free prefix are scanned point by point, and the escape determination result of each trajectory point is determined according to the escape target point; If there is a trajectory point on the collision-free prefix that is a candidate point for escaping, then the effective trajectory segment between the initial point of the collision-free prefix and the candidate point for escaping is taken as the candidate escaping trajectory. When there is a candidate escape trajectory in the search layer, the candidate escape trajectory is taken as the target escape trajectory. When there are multiple candidate escape trajectories in the search layer, the target escape trajectory is determined from the multiple candidate escape trajectories. If none of the collision-free prefixes contain a trajectory point whose escape determination result is an escape candidate point, then there is no target escape trajectory in the search layer.

6. The method according to claim 5, characterized in that, The curvature trajectory primitives include: forward straight curvature trajectory primitives, forward arc curvature trajectory primitives, backward straight curvature trajectory primitives, backward arc curvature trajectory primitives, and in-place turning curvature trajectory primitives.

7. The method according to claim 5, characterized in that, The trajectory points on the collision-free prefix are scanned point by point to determine the escape determination result for each trajectory point, including: The trajectory points on the collision-free prefix are scanned point by point, and the first obstacle safety margin and the distance from the trajectory point to the escape target point are calculated for each trajectory point. Determine the second obstacle safety margin corresponding to the escape target point, and determine the escape judgment result for each trajectory point based on the distance from the trajectory point to the escape target point, the first obstacle safety margin, and the second obstacle safety margin.

8. The method according to claim 7, characterized in that, The determination of the escape judgment result for each trajectory point based on the distance from the trajectory point to the escape target point, the first obstacle safety margin, and the second obstacle safety margin includes: Obtain the preset first distance threshold and second distance threshold; If the distance from the trajectory point to the escape target point is less than or equal to the first distance threshold, and the difference between the second obstacle safety margin and the first obstacle safety margin is less than or equal to the second distance threshold, then the escape determination result corresponding to the trajectory point is an escape candidate point. If the distance from the trajectory point to the escape target point is greater than the first distance threshold, and / or the difference between the second obstacle safety margin and the first obstacle safety margin is greater than the second distance threshold, then the escape determination result corresponding to the trajectory point is not an escape candidate point.

9. The method according to claim 5, characterized in that, When multiple candidate escape trajectories exist in the search layer, the target escape trajectory is determined from among the multiple candidate escape trajectories, including: If there are multiple candidate escape trajectories in the search layer, then determine the distance between each candidate escape trajectory and the escape target point, the total path, and the total backtrack distance; Based on the distance between each target's escape trajectory and the escape target point, the total path, and the total retreat distance, the multiple target escape trajectories are prioritized to obtain the priority ranking result. The target escape trajectory is determined from multiple candidate escape trajectories based on the priority ranking results.

10. The method according to claim 1, characterized in that, Generate an escape plan based on the target escape trajectory, and execute the escape plan, including: Based on the target escape trajectory, backtrack upwards layer by layer along the search layer hierarchy, and match the effective trajectory segments corresponding to each previous search layer in sequence. The effective trajectory segment is the trajectory segment between the initial point without collision prefix and the expansion point of the next search layer. The effective trajectory segments of each preceding search layer are sequentially connected end-to-end with the target escape trajectory in hierarchical order to form an escape plan. Record the curvature trajectory primitive point series corresponding to the target escape trajectory and the effective trajectory segments of each preceding search layer, and construct the effective trajectory segment endpoint index corresponding to the target escape trajectory and the effective trajectory segments of each preceding search layer. The effective trajectory segment endpoint index is configured to map the effective trajectory segment endpoint, inter-layer intersection, path length accumulation, and backtracking distance accumulation. The endpoints of valid trajectory segments are read based on the endpoint index of the valid trajectory segments, and the escape plan is executed according to the endpoints of the valid trajectory segments corresponding to each valid trajectory segment.

11. The method according to claim 1, characterized in that, The method further includes: When executing the escape plan, a forward safety check is performed. If there is a collision risk within the preset forward safety check range, a braking command is output. If the duration of the braking command exceeds a preset duration threshold, the execution of the escape plan is deemed to have failed.

12. The method according to claim 10, characterized in that, The method further includes: When the robot enters the preset distance threshold where the end point of the current valid trajectory segment is located, record the real-time distance between the robot and the end point of the current valid trajectory segment. Within a preset distance threshold, the minimum distance value among all real-time distances is identified; If the minimum distance value is greater than the preset target proximity threshold, and within a predetermined duration after the minimum distance value is reached, the real-time distance between the robot and the endpoint of the current effective trajectory segment is always greater than the minimum distance value, then the execution of the current effective trajectory segment is determined to be unstable, and a failure message is returned.

13. The method according to claim 10, characterized in that, The method further includes: Determine the segmentation threshold for the current valid trajectory segment; When the robot reaches the segment determination threshold of the current effective trajectory segment, the robot's current speed is reduced to the predetermined speed threshold.

14. A robot escape device based on multi-layer curvature trajectory search, characterized in that, The device includes: The response module is used to respond to the escape command, obtain the robot's current pose and the escape target point corresponding to the robot's current pose; The first search module is used to take the robot's current pose as the current search node, construct a search layer based on the current search node by unfolding a curvature trajectory, and search for the target escape trajectory in the search layer based on the escape target point. The generation module is used to obtain several unfolding points generated in the search layer if there is no target escape trajectory in the search layer. The second search module is used to take several unfolded points as the current search nodes and re-execute the step of building a search layer based on the current search nodes through curvature trajectory unfolding until the target escape trajectory is found. An execution module is used to generate an escape plan based on the target escape trajectory and execute the escape plan.

15. A computer device, characterized in that, include: A memory and a processor, the memory storing a computer program, the processor executing the computer program to implement the steps of the method according to any one of claims 1 to 13.

16. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that can be executed to implement the steps of the robot extrication method based on multi-layer search of curvature trajectory as described in any one of claims 1 to 13.

Images