A robot safe path planning method and system based on expansion distance

Abstract

The present invention provides a robot safe path planning method and system based on expansion distance, which relates to the field of robot path planning technology, and solves the problem of engineering application limitations caused by excessive or too small expansion distance in path planning when the starting and ending points are close to obstacles. The specific approach of the present invention is to respectively establish new starting points and end points near the original starting point and end point, and ensure that these new points meet specific large expansion distance requirements; based on these newly set starting points and end points, as well as large expansion distance constraints, path planning is performed to obtain a collision-free path; then, the paths from the original starting point to the new starting point, the new starting point via the new end point and then to the original end point are seamlessly connected to form a collision-free path solution. The present invention can not only ensure that a feasible path planning solution can be obtained when the starting and ending points are close to obstacles, but also maintain a larger expansion distance in the middle path section, making the obtained path safer.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and in particular to a robot safe path planning method and system based on expansion distance. Background Art

[0002] As the core support for intelligent systems, robotic path planning technology plays a key role in industrial automation, service robotics, medical surgery, and other fields. This technology builds a spatial representation model of the environment, combines kinematic constraints with collision detection algorithms, and finds collision-free trajectories connecting the starting and ending points for various robots (including mobile robots and multi-axis manipulators). Balancing path safety and planning success in complex and dynamic environments remains a common challenge for the industry.

[0003] Current mainstream path safety enhancement solutions generally employ an obstacle inflation strategy, which employs two technical approaches: one is to construct a virtual obstacle map with a safety margin by extending the geometric boundaries of the original obstacle; the other is to equivalently amplify the robot's collision model to form expanded motion constraints within the configuration space. Both approaches sacrifice a portion of the feasible solution space in exchange for improved safety, ensuring that a predetermined buffer distance is maintained between the planned path and the actual obstacle. This technical approach demonstrates excellent engineering practicality in common application scenarios and has become a standard preprocessing module in motion planning algorithm libraries.

[0004] However, the obstacle expansion mechanism has inherent contradictions that are difficult to overcome: on the one hand, when the expansion distance is set too large (see Figure 1 ), the algorithm is prone to planning failure in narrow passages or scenarios where the starting and ending points are close to obstacles. This phenomenon is due to excessive encroachment of the safety margin, resulting in the feasible path in the original physical space being completely blocked by virtual obstacles or the expanded robot. Especially in the high-dimensional configuration space planning of the manipulator, this problem will cause the solution space to be fragmented, significantly reducing the convergence efficiency of the planner. On the other hand, if the expansion coefficient is too small or there is no expansion (see Figure 2 ), it is impossible to effectively eliminate the potential collision risks caused by realistic factors such as sensor noise and control errors, so that the planning results remain at the theoretical safety level and lack engineering robustness.

[0005] To address this contradiction, the industry has attempted to optimize by dynamically adjusting expansion parameters. For example, these methods employ layered expansion coefficients based on environmental characteristics or introduce adaptive scaling strategies based on distance fields. However, these approaches fundamentally fail to break away from the linear framework of static expansion and, in unstructured scenarios such as those with obstacles adjacent to the starting and ending points or sudden changes in channel width, still face the dilemma of achieving both safety and feasibility. Summary of the Invention

[0006] Based on the current state of the art, the present invention aims to address the engineering application limitations of path planning caused by excessively large or small expansion distances when the starting and ending points are in close proximity to obstacles. Therefore, a method and system for safe robot path planning based on expansion distance are proposed. This invention not only ensures that a feasible path planning solution can be obtained when the starting and ending points are in close proximity to obstacles, but also maintains a large expansion distance in the intermediate path segments, making the resulting path safer.

[0007] The present invention adopts the following technical solutions to achieve the purpose:

[0008] A robot safe path planning method based on expansion distance includes the following steps:

[0009] S1, generate a new starting point s1 in the vicinity of the original starting point s, the new starting point s1 satisfies the large expansion distance constraint, and the starting path segment s-s1 satisfies the collision-free condition;

[0010] S2. Generate a new endpoint g1 in the vicinity of the original endpoint g. The new endpoint g1 satisfies the large expansion distance constraint, and the endpoint path segment g-g1 satisfies the collision-free condition.

[0011] S3. Based on the large expansion distance constraint, a path is planned between the new starting point s1 and the new end point g1 to generate an intermediate path P1.

[0012] S4. Perform path synthesis: connect the original starting point s with the new starting point s1, connect the original end point g with the new end point g1, and connect the new starting point s1 and the new end point g1 through the intermediate path P1 to construct the planned final path P: s→s1→P1→g1→g.

[0013] Specifically, in step S1, the new starting point s1 satisfies the starting point large expansion distance constraint condition and is expressed as the following function:

[0014] isValid_s(m,s1,padding=large)=true

[0015] Where m represents the obstacle map input of the function, and padding = large represents expanding the robot by the preset expansion distance. When the robot is at the new starting point s1, the function outputs true, indicating that the robot is legal, that is, there is no collision between the robot and the obstacle.

[0016] The starting path segment s-s1 satisfies the starting point collision-free condition and is expressed as the following function:

[0017] checkMotion_s(m,s,s1,padding=0.0)=true

[0018] Where padding = 0.0 means that the robot is not expanded. When the robot is at any position on the starting path segment s-s1, the function outputs true, indicating that the robot is legal, that is, there is no collision between the robot and the obstacle.

[0019] Specifically, in step S2, the new endpoint g1 satisfies the endpoint large expansion distance constraint and is expressed as the following function:

[0020] isValid_g(m,g1,padding=large)=true

[0021] In the formula, when the robot is at the new end point g1, the function outputs true, indicating that it is legal, that is, the robot does not collide with the obstacle;

[0022] The end path segment g-g1 satisfies the end point collision-free condition and is expressed as the following function:

[0023] checkMotion_g(m,g,g1,padding=0.0)=true

[0024] In the formula, when the robot is at any position on the end path segment g-g1, the function outputs true to indicate legality, that is, the robot does not collide with the obstacle.

[0025] Specifically, in step S3, the intermediate path P1 consists of n path points p i Composition, that is, P1={s1,p1,p2,...,p n ,g1}; for any path segment p in the intermediate path P1 i →p i+1 , all satisfy the middle no-collision condition, as follows:

[0026] checkMotion_P1(m,p i ,p i+1 ,padding=large)=true

[0027] Where m represents the obstacle map input of the function, padding = large represents the expansion of the robot with the preset expansion distance; when the robot is in the path segment p i →p i+1 When the robot is at any position on the obstacle, the function outputs true, indicating that the robot is legal, that is, there is no collision between the robot and the obstacle.

[0028] Preferably, in steps S1 and S2, the methods for generating the new starting point s1 and the new end point g1 both include: randomly generating candidate points within a preset radius around the original starting point s or the original end point g using a probabilistic sampling algorithm, and screening the candidate points through dual collision detection, that is, simultaneously satisfying static collision detection under conditions of large expansion distance and dynamic path detection under conditions of small expansion or no expansion distance.

[0029] Preferably, in step S3, when performing path planning, the path planning algorithm adopts a hierarchical planning strategy, including: generating an initial path based on the improved RRT* algorithm, introducing a large expansion distance constraint when the node is expanded; using a B-spline curve to smooth the initial path to ensure the continuity of the trajectory curvature; and performing segmented collision detection on the smoothed path through a reverse verification method, and verifying that the verification interval is no greater than a preset percentage value of the path length.

[0030] Preferably, the large expansion distance constraint conditions in steps S1, S2, and S3 adopt a dynamic adjustment mechanism to monitor the environmental obstacle density ρ in real time. When the environmental obstacle density ρ exceeds a preset threshold, the expansion distance is dynamically adjusted according to the following formula:

[0031] padding=base_padding*(1+α*ρ)

[0032] Where base_padding represents the base expansion distance, and α represents the density influence coefficient.

[0033] Preferably, a hierarchical detection strategy is adopted for both the isValid function and the checkMotion function: the primary detection uses fast Euclidean distance field calculation to establish an obstacle distance map; the secondary detection performs precise geometric envelope calculation, taking into account the robot kinematic constraints; the final verification is achieved by sampling a preset number of detection points at equal intervals on the path segment through the Monte Carlo sampling method.

[0034] Preferably, after step S4, the final path P is subjected to trajectory optimization processing, including: using cubic spline interpolation to eliminate path cusps in the final path P; applying time optimal parameterization under velocity-acceleration constraints; generating a space-time trajectory including a timestamp to meet the constraints of dynamic obstacle avoidance.

[0035] Preferably, the method also includes an adaptive replanning mechanism: when an environmental change is detected, the feasibility of the starting path segment s-s1 and the end path segment g-g1 is preferentially retained, and only local replanning is performed on the intermediate path P1. An incremental RRT algorithm is used during replanning, and a preset percentage interval of the original path points is retained as the basic topology structure for replanning.

[0036] The present invention also provides a robot safety path planning system based on expansion distance, comprising:

[0037] The starting point generation module is used to generate a new starting point s1 in the vicinity of the original starting point s, which satisfies the large expansion distance constraint and ensures that the starting point path segment s-s1 has no collision;

[0038] The endpoint generation module is used to generate a new endpoint g1 in the vicinity of the original endpoint g that meets the large expansion distance constraint and ensures that the endpoint path segment g-g1 has no collision;

[0039] A path planning module, connected to the starting point generation module and the end point generation module, is used to plan a path between the new starting point s1 and the new end point g1 based on the large expansion distance constraint to generate an intermediate path P1;

[0040] The path synthesis module is connected to the starting point generation module, the end point generation module and the path planning module, and is used to connect the original starting point s with the new starting point s1, connect the original end point g with the new end point g1, and connect the new starting point s1 and the new end point g1 through the intermediate path P1 to construct the planned final path P: s→s1→P1→g1→g.

[0041] In summary, due to the adoption of this technical solution, the beneficial effects of the present invention are as follows:

[0042] This method specifically addresses the problem of robots operating in complex environments with their starting and ending points near obstacles, significantly improving the safety and feasibility of their paths. First, a larger expansion distance is used in the middle of the path. This not only increases the path's safety factor but also effectively avoids the risks associated with mid-points being too close to obstacles. This ensures the safety of the generated path, even in environments with numerous obstacles.

[0043] Secondly, for situations where the starting point or end point is close to an obstacle, traditional algorithms often find it difficult to find a suitable path. The method of the present invention can still find a feasible path in this case. The specific approach is to set up new starting points and end points near the original starting point and end point, respectively, and ensure that these new points meet specific large expansion distance requirements. Based on these newly set starting points and end points, as well as the large expansion distance constraint, path planning is performed to obtain a collision-free path. Subsequently, the paths from the original starting point to the new starting point, the new starting point via the new end point, and then to the original end point are seamlessly connected to form a collision-free path solution.

[0044] In summary, this invention, by introducing an optimization strategy based on expansion distance, addresses the engineering limitations caused by excessively large or small expansion distances. It not only ensures an effective path planning solution when the starting and ending points are in close proximity to obstacles, but also maintains a large expansion distance in the middle of the path, ensuring overall path safety. Therefore, this invention provides a more reliable and practical path planning method for safe robot navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 Schematic diagram of the algorithm planning scenario when the expansion distance is set too large;

[0046] Figure 2 Schematic diagram of the algorithm planning scenario when the expansion coefficient is too small or there is no expansion;

[0047] Figure 3 A schematic diagram briefly describing the overall process of the method of the present invention;

[0048] Figure 4 Schematic diagram of the algorithm planning scenario corresponding to the method of the present invention;

[0049] Figure 5 Schematic diagram of the algorithm flow corresponding to the method of the present invention. DETAILED DESCRIPTION

[0050] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings herein can be arranged and designed in various different configurations.

[0051] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention as claimed, but rather merely represents selected embodiments of the present invention. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without creative effort shall fall within the scope of protection of the present invention.

[0052] Example 1

[0053] A robot safe path planning method based on expansion distance, Figure 3 The overall process of the method is briefly described in FIG. 1 , which can be viewed simultaneously. The various steps of the method can be summarized as follows:

[0054] S1, generate a new starting point s1 in the vicinity of the original starting point s, the new starting point s1 satisfies the large expansion distance constraint, and the starting path segment s-s1 satisfies the collision-free condition;

[0055] S2. Generate a new endpoint g1 in the vicinity of the original endpoint g. The new endpoint g1 satisfies the large expansion distance constraint, and the endpoint path segment g-g1 satisfies the collision-free condition.

[0056] S3. Based on the large expansion distance constraint, a path is planned between the new starting point s1 and the new end point g1 to generate an intermediate path P1.

[0057] S4. Perform path synthesis: connect the original starting point s with the new starting point s1, connect the original end point g with the new end point g1, and connect the new starting point s1 and the new end point g1 through the intermediate path P1 to construct the planned final path P: s→s1→P1→g1→g.

[0058] After the method is executed, the effect in the planning scenario can be seen in Figure 4 The specific execution process of the path planning algorithm can be found in Figure 5 , which corresponds to the detailed description of the process of this embodiment.

[0059] In the path planning task, the given known information includes: obstacle map m, original starting point s and original end point g. A safe path without obstacle collision needs to be planned between the original starting point s and the original end point g.

[0060] In step S1, the new starting point s1 satisfies the starting point large expansion distance constraint condition and is expressed as the following function:

[0061] isValid_s(m,s1,padding=large)=true

[0062] Where m represents the obstacle map input of the function, and padding = large means that the robot is expanded by the preset expansion distance. When the robot is at the new starting point s1, the function outputs true to indicate legality, that is, the robot does not collide with the obstacle. Similarly, when the robot is at the new starting point s1, if its expanded virtual space has collided with an obstacle, the function outputs false to indicate illegality. At this time, the generated new starting point s1 will not meet the starting point large expansion distance constraint and needs to be regenerated.

[0063] At the same time, the starting path segment s-s1 satisfies the starting point collision-free condition, which is the following function:

[0064] checkMotion_s(m,s,s1,padding=0.0)=true

[0065] Where padding = 0.0 means that the robot is not expanded. When the robot is at any position on the starting path segment s-s1, the function outputs true to indicate legality, that is, the robot does not collide with obstacles. Similarly, if the linear trajectory of the starting path segment s-s1 causes the uninflated robot to collide with an obstacle, the function outputs false to indicate illegality, that is, the starting path segment s-s1 does not meet the starting point collision-free condition, and a feasible path segment needs to be regenerated.

[0066] In step S2, the new starting point g1 satisfies the large expansion distance constraint of the end point, which is the following function:

[0067] isValid_g(m,g1,padding=large)=true

[0068] In the formula, when the robot is at the new starting point g1, the function outputs true, indicating that the robot is legal, that is, the robot does not collide with the obstacle. Similarly, when the robot is at the new starting point g1, if its expanded virtual space has collided with an obstacle, the function outputs false, indicating that the new starting point g1 generated at this time will not meet the endpoint maximum expansion distance constraint and needs to be regenerated.

[0069] At the same time, the end path segment g-g1 satisfies the starting point non-collision condition, which is the following function:

[0070] checkMotion_g(m,g,g1,padding=0.0)=true

[0071] In the formula, when the robot is at any position on the terminal path segment g-g1, the function outputs true to indicate legality, that is, the robot does not collide with the obstacle. Similarly, if the linear trajectory of the terminal path segment g-g1 causes the uninflated robot to collide with the obstacle, the function outputs false to indicate illegality, and the terminal path segment g-g1 does not meet the terminal no-collision condition, and a feasible path segment needs to be regenerated.

[0072] In step S3, the intermediate path P1 consists of n path points p i Composition, that is, P1={s1,p1,p2,...,p n ,g1}; for any path segment p in the intermediate path P1 i →p i+1 , all satisfy the middle no-collision condition, as follows:

[0073] checkMotion_P1(m,p i ,p i+1 ,padding=large)=true

[0074] In the formula, when the robot is in the path segment p i →p i+1 When the robot is at any position on the path, the function outputs true, which means it is legal, that is, the robot does not collide with the obstacle; in the path planning process for the intermediate path P1, each path point p is solved in turn. i The trajectories between them can be made so that each trajectory meets the no-collision condition in the middle, thereby ensuring the legality of the function output; when the function outputs false, it means illegal, that is, it is necessary to re-plan the illegal trajectory points.

[0075] After the path synthesis in step S4, the final path P obtained can ensure that it has a solution when it is close to the original starting point s and the original end point g, and that the middle section of the path has enough expansion distance to ensure safety. Figure 4 The effect is shown.

[0076] Example 2

[0077] Based on Example 1, this example introduces the details and preferred contents of each step in the method. First, regarding the method for generating the new starting point s1 and the new end point g1 in steps S1 and S2, both include the following preferred methods:

[0078] A probabilistic sampling algorithm is used to randomly generate candidate points within a preset radius around the original starting point s or the original end point g, and the candidate points are screened through dual collision detection, that is, static collision detection under large expansion distance conditions (corresponding to the isValid function of the large expansion distance constraint condition) and dynamic path detection under small expansion or no expansion distance conditions (corresponding to the checkMotion function of the no collision condition).

[0079] In this embodiment, the preset radius can be dynamically set according to the robot's motion characteristics and can be calculated by the following formula:

[0080] R=k*(robot_radius+max_speed*Δt)

[0081] Where k is the safety factor, which can be 1.2-2.0; robot_radius is the radius of the robot body; max_speed is the maximum movement speed of the robot; and Δt is the path planning period.

[0082] For the path planning algorithm in step S3, a hierarchical planning strategy can be adopted, including: generating an initial path based on the improved RRT* algorithm, introducing a large expansion distance constraint when the node is expanded; using a B-spline curve to smooth the initial path to ensure the continuity of the trajectory curvature; performing segmented collision detection on the smoothed path through a reverse verification method, and verifying that the verification interval is no greater than a preset percentage value of the path length, for example, no greater than 5% of the path length.

[0083] In addition, as a preference of this embodiment, the large expansion distance constraint conditions in steps S1, S2, and S3 can all adopt a dynamic adjustment mechanism, specifically: real-time monitoring of the environmental obstacle density ρ, when the environmental obstacle density ρ exceeds a preset threshold, dynamically adjusting the expansion distance according to the following formula:

[0084] padding=base_padding*(1+α*ρ)

[0085] Where base_padding represents the base expansion distance, and α represents the density influence coefficient, which can range from 0.1 to 0.3.

[0086] In this embodiment, a hierarchical detection strategy is adopted for both the isValid function and the checkMotion function used in Example 1: the primary detection uses fast Euclidean distance field calculation to establish an obstacle distance map; the secondary detection performs precise geometric envelope calculation, taking into account the robot's kinematic constraints; and the final verification is achieved by using the Monte Carlo sampling method to sample a preset number of detection points (it is recommended to sample at least 20 detection points) at equal intervals on the path segment. The detailed implementation process of this hierarchical detection strategy can be carried out as follows:

[0087] During primary detection, obstacle data in the robot's working environment is collected. This data can be collected in real time by sensors (such as lidar and cameras) or obtained from pre-loaded map information. Based on this data, the FEDF algorithm is then used to calculate the distance from each obstacle to the nearest point. This generates a map that accurately reflects the distance from each location in the environment to the nearest obstacle. This map provides a key reference for subsequent path planning and obstacle avoidance.

[0088] In order to further improve the safety and accuracy of path planning, secondary detection can be performed on the basis of primary detection. This stage of the example embodiment mainly involves precise geometric envelope calculation, that is, a detailed geometric shape analysis of the robot and the obstacles it may encounter. Taking into account the actual size, shape and kinematic limitations of the robot (such as turning radius, speed variation range, etc.), precise geometric envelope calculation is used to determine whether the robot can safely bypass obstacles without collision. In addition, it is necessary to evaluate the space required by the robot when performing specific actions to ensure that it maintains a safe distance throughout the operation.

[0089] After completing the primary and secondary tests, the planned path needs to be finally verified to ensure its safety. To this end, this embodiment adopts the Monte Carlo sampling method. Specifically, at least 20 test points are randomly selected as sample points at equal intervals on the predetermined path segment. For each selected sample point, the above primary and secondary test processes must be repeated to assess whether there is a potential risk at that point. If all sample points are confirmed to be safe, the entire path can be considered feasible and safe; otherwise, the path must be replanned until the safety standards are met.

[0090] As a preferred embodiment of this embodiment, after step S4, the final path P may also be subjected to trajectory optimization, including: using cubic spline interpolation to eliminate path cusps in the final path P; applying time-optimal parameterization under velocity-acceleration constraints; and generating a spatiotemporal trajectory including timestamps to satisfy the dynamic obstacle avoidance constraints. The above trajectory optimization process is a mature optimization method commonly used by those skilled in the art, and therefore will not be described in detail.

[0091] In addition to trajectory planning for a single robot, the method of this embodiment can also be easily expanded to support the collaborative planning process of multiple robots; by assigning an independent large expansion distance constraint condition to each robot, corresponding to each robot's respective expansion distance parameters, additional mutual collision avoidance constraints are designed during intermediate path planning in step S3, and a distributed model predictive control method is further adopted to achieve trajectory coordination.

[0092] As a preferred embodiment of this invention, the method may further include an adaptive replanning mechanism: when an environmental change is detected, the feasibility of the starting path segment s-s1 and the ending path segment g-g1 is preferentially retained, and only local replanning is performed on the intermediate path P1. During replanning, an incremental RRT algorithm is used to retain a preset percentage interval (recommended 50%-70%) of the original path points as the basic topology structure for replanning.

[0093] Example 3

[0094] This embodiment provides a robot safety path planning system based on expansion distance. The system can execute the method in embodiment 1 or 2, specifically including:

[0095] The starting point generation module is used to generate a new starting point s1 in the vicinity of the original starting point s, which satisfies the large expansion distance constraint and ensures that the starting point path segment s-s1 has no collision;

[0096] The endpoint generation module is used to generate a new endpoint g1 in the vicinity of the original endpoint g that meets the large expansion distance constraint and ensures that the endpoint path segment g-g1 has no collision;

[0097] A path planning module, connected to the starting point generation module and the end point generation module, is used to plan a path between the new starting point s1 and the new end point g1 based on the large expansion distance constraint to generate an intermediate path P1;

[0098] The path synthesis module is connected to the starting point generation module, the end point generation module and the path planning module, and is used to connect the original starting point s with the new starting point s1, connect the original end point g with the new end point g1, and connect the new starting point s1 and the new end point g1 through the intermediate path P1 to construct the planned final path P: s→s1→P1→g1→g.

[0099] To sum up, in the solution of the present invention, the intermediate path P1 will obtain the safety guarantee of a larger expansion distance that meets actual needs, thereby making the planned path safer and eliminating the risk of the intermediate path point of the trajectory being too close to an obstacle; at the same time, for the situation where the starting point or end point of the planning requirements is too close to an obstacle, the present invention can also solve a feasible path based on meeting the expansion distance requirements, so that the safety and feasibility of path planning can be taken into account, and it is suitable for promotion and implementation in actual engineering applications.

Claims

1. A robot safe path planning method based on expansion distance, characterized in that: The steps include: S1, generate a new starting point s1 in the vicinity of the original starting point s, the new starting point s1 satisfies the large expansion distance constraint, and the starting path segment s-s1 satisfies the collision-free condition; The new starting point s1 satisfies the starting point large expansion distance constraint and is expressed as the following function: isValid_s(m, s1, padding=large)=true Where m represents the obstacle map input of the function, and padding=large represents expanding the robot by the preset expansion distance. When the robot is at the new starting point s1, the function outputs true, indicating that the robot is legal, that is, there is no collision between the robot and the obstacle. The starting path segment s-s1 satisfies the starting point collision-free condition and is expressed as the following function: checkMotion_s(m, s, s1, padding=0.0)=true Where padding = 0.0 means that the robot is not expanded. When the robot is at any position on the starting path segment s-s1, the function outputs true, indicating that the robot is legal, that is, it does not collide with the obstacle. S2. Generate a new endpoint g1 in the vicinity of the original endpoint g. The new endpoint g1 satisfies the large expansion distance constraint, and the endpoint path segment g-g1 satisfies the collision-free condition. The new endpoint g1 satisfies the endpoint large expansion distance constraint and is expressed as the following function: isValid_g(m, g1, padding=large)=true In the formula, when the robot is at the new end point g1, the function outputs true, indicating that it is legal, that is, the robot does not collide with the obstacle; The end path segment g-g1 satisfies the end point collision-free condition and is expressed as the following function: checkMotion_g(m, g, g1, padding=0.0)=true In the formula, when the robot is at any position on the end path segment g-g1, the function outputs true, indicating that the robot is legal, that is, there is no collision between the robot and the obstacle. S3. Based on the large expansion distance constraint, a path is planned between the new starting point s1 and the new end point g1 to generate an intermediate path P1. The large expansion distance constraint in steps S1, S2, and S3 uses a dynamic adjustment mechanism to monitor the environmental obstacle density ρ in real time. When the environmental obstacle density ρ exceeds the preset threshold, the expansion distance is dynamically adjusted according to the following formula: padding=base_padding*(1+α*ρ) Where base_padding represents the base expansion distance, and α represents the density influence coefficient; S4. Perform path synthesis: connect the original starting point s with the new starting point s1, connect the original end point g with the new end point g1, and connect the new starting point s1 and the new end point g1 through the intermediate path P1 to construct the planned final path P: s→s1→P1→g1→g.

2. The robot safety path planning method according to claim 1, characterized in that: In step S3, the intermediate path P1 consists of n path points p i Composition, that is, P1={s1,p1,p2,...,p n ,g1}; for any path segment p in the intermediate path P1 i →p i+1 , all satisfy the middle no-collision condition, as follows: checkMotion_P1(m, p i , p i+1 , padding=large)=true Where m represents the obstacle map input of the function, padding=large represents expanding the robot with a preset expansion distance; when the robot is in path segment p i →p i+1 When the robot is at any position on the obstacle, the function outputs true, indicating that the robot is legal, that is, there is no collision between the robot and the obstacle.

3. The robot safety path planning method according to claim 1, characterized in that: In steps S1 and S2, the methods for generating the new starting point s1 and the new end point g1 both include: using a probabilistic sampling algorithm to randomly generate candidate points within a preset radius around the original starting point s or the original end point g, and screening the candidate points through dual collision detection, that is, simultaneously satisfying static collision detection under large expansion distance conditions and dynamic path detection under small expansion distance conditions or no expansion distance conditions.

4. The robot safety path planning method according to claim 1, characterized in that: In step S3, when performing path planning, the path planning algorithm adopts a hierarchical planning strategy, including: generating an initial path based on the improved RRT* algorithm, introducing a large expansion distance constraint when expanding nodes; using a B-spline curve to smooth the initial path to ensure the continuity of the trajectory curvature; and performing segmented collision detection on the smoothed path through a reverse verification method, and verifying that the verification interval is no greater than a preset percentage value of the path length.

5. The robot safety path planning method according to claim 1, characterized in that: For both the isValid function and the checkMotion function, a hierarchical detection strategy is adopted: the primary detection uses fast Euclidean distance field calculation to establish an obstacle distance map; The secondary inspection performs precise geometric envelope calculations, taking into account the robot's kinematic constraints; the final verification is achieved by sampling a preset number of inspection points at equal intervals on the path segment using the Monte Carlo sampling method.

6. The robot safety path planning method according to claim 1, characterized in that: After step S4, the final path P is subjected to trajectory optimization, including: using cubic spline interpolation to eliminate path cusps in the final path P; applying time optimal parameterization under velocity-acceleration constraints; and generating a spatiotemporal trajectory including timestamps to meet the constraints of dynamic obstacle avoidance.

7. The robot safety path planning method according to claim 1, characterized in that: The method also includes an adaptive replanning mechanism: when an environmental change is detected, the feasibility of the starting path segment s-s1 and the ending path segment g-g1 is prioritized, and only the intermediate path P1 is locally replanned. During replanning, an incremental RRT algorithm is used, and a preset percentage interval of the original path points is retained as the basic topology structure for replanning.

8. A robot safety path planning system based on expansion distance, characterized in that: The system is used to execute the robot safety path planning method according to claim 1, and the system includes: The starting point generation module is used to generate a new starting point s1 in the vicinity of the original starting point s, which satisfies the large expansion distance constraint and ensures that the starting point path segment s-s1 has no collision; The endpoint generation module is used to generate a new endpoint g1 in the vicinity of the original endpoint g that meets the large expansion distance constraint and ensures that the endpoint path segment g-g1 has no collision; A path planning module, connected to the starting point generation module and the end point generation module, is used to plan a path between the new starting point s1 and the new end point g1 based on the large expansion distance constraint to generate an intermediate path P1; The path synthesis module is connected to the starting point generation module, the end point generation module and the path planning module, and is used to connect the original starting point s with the new starting point s1, connect the original end point g with the new end point g1, and connect the new starting point s1 and the new end point g1 through the intermediate path P1 to construct the planned final path P: s→s1→P1→g1→g.

Images