A path planning method, device, apparatus and storage medium

Abstract

Embodiments of the present application provide a path planning method and device, equipment and a storage medium, and relate to the technical field of robots. The method comprises: determining a candidate blocking gap that blocks a task execution device from going to a terminal point based on a current environment description; simulating the robot pushing a to-be-pushed obstacle in a pushing direction and for a pushing duration based on the quality of the to-be-pushed obstacle corresponding to the candidate blocking gap, the ground friction coefficient in the current environment description, and the multi-body contact constraint between obstacles, to obtain a post-pushing environment description corresponding to the candidate blocking gap; detecting whether the task execution device is reachable from a starting point to the terminal point according to the post-pushing environment description corresponding to the target feasible gap with the lowest total consumption; if reachable, generating a passable path; otherwise, updating the current environment description to the post-pushing environment description corresponding to the target feasible gap, and returning to the step of determining the candidate blocking gap based on the current environment description. Through the present solution, the accuracy of the planned passable path can be improved.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a path planning method, apparatus, device, and storage medium. Background Technology

[0002] Large vehicles and other task-performing equipment typically move along pre-planned routes in scenarios such as warehousing and logistics centers and disaster relief sites to perform their tasks. These scenarios often contain obstacles such as pallets, boxes, and equipment. These obstacles can be moved by robots, improving traffic flow. Therefore, when planning routes for task-performing equipment, the possibility of robots being able to move obstacles in the scenario can be considered to create efficient routes.

[0003] In related technologies, when planning the aforementioned travel path, obstacles that can be moved by the robot in the scene are initially ignored. The shortest path from the starting point to the ending point of the task execution device is planned, which is an ideal path. In reality, obstacles exist in the scene described above. This ideal path is then placed back into the scene, and the obstacles are abstracted into geometric shapes such as dots and polygons to identify the positions on the path where collisions with obstacles may occur. Based on these positions, the obstacles that need to be moved by the robot are determined. Next, under the premise that the robot is idealized as a point agent with infinite driving capabilities, a suitable "placement point" is found in a local area near the obstacle as the position of the obstacle after it has been moved by the robot. Based on the position of the obstacle after it has been moved, the ideal path is adjusted to obtain the travel path.

[0004] However, in actual operation, there may be situations where the robot has difficulty moving the above obstacles, which in turn makes it difficult for the task execution equipment to move according to the above passage path. In other words, the accuracy of the passage path planned in the above way is low. Summary of the Invention

[0005] The purpose of this application is to provide a route planning method, apparatus, device, and storage medium to improve the accuracy of planned travel routes. The specific technical solution is as follows:

[0006] In a first aspect, embodiments of this application provide a path planning method, the method comprising:

[0007] If there is no passable path between the start and end points of the task to be performed that is wider than the passage width of the task execution device, a candidate blocking gap is determined based on the current environment description to prevent the task execution device from reaching the end point; the candidate blocking gap is a channel between two obstacles whose width is less than the passage width.

[0008] The obstacles to be moved, the direction of movement, and the duration of movement for the candidate blocking gaps are determined. Based on the mass of the obstacles to be moved, the ground friction coefficient in the current environment description, and the multi-body contact constraints between the obstacles, the robot is simulated to move the obstacles to be moved according to the direction of movement and the duration of movement, so as to obtain the post-movement environment description corresponding to the candidate blocking gaps.

[0009] If there is a feasible gap among the candidate blocking gaps where the width between the two obstacles reaches the passage width after the simulated pushing, the work consumption and movement consumption of the robot pushing the obstacle are obtained according to the environmental description after the pushing corresponding to the feasible gap, and the total consumption is obtained according to the movement consumption and work consumption.

[0010] Based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption, detect whether the task execution device can reach the destination from the starting point;

[0011] If reachable, generate a passable path between the starting point and the ending point that passes through the feasible gap in the target;

[0012] Otherwise, update the current environment description to the shifted environment description corresponding to the target feasible gap, and return to the step of determining the candidate blocking gaps for the blocking task execution device to reach the destination based on the current environment description.

[0013] Secondly, embodiments of this application provide a path planning device, the device comprising:

[0014] The gap determination module is used to determine candidate blocking gaps that would prevent the task execution device from reaching the destination if there is no passable path with a width equal to the passage width of the task execution device between the start and end points of the task to be executed; the candidate blocking gaps are channels with a width between two obstacles that is less than the passage width.

[0015] The simulation module is used to determine the obstacle to be moved, the direction of movement, and the duration of movement for the candidate blocking gap. Based on the mass of the obstacle to be moved, the ground friction coefficient in the current environment description, and the multi-body contact constraints between obstacles, the simulation robot moves the obstacle to be moved according to the direction of movement and the duration of movement, and obtains the post-movement environment description corresponding to the candidate blocking gap.

[0016] The calculation module is used to obtain the work consumption and movement consumption of the robot in pushing the obstacle if there is a feasible gap in the candidate blocking gaps where the width between the two obstacles reaches the passage width after the simulated pushing; and to obtain the total consumption based on the movement consumption and work consumption.

[0017] The connectivity detection module is used to detect whether the task execution device can reach the destination from the starting point, based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption.

[0018] The generation module is used to generate a traversable path between the start and end points that passes through the target feasible gap, if reachable.

[0019] The return module is used otherwise to update the current environment description to the shifted environment description corresponding to the target feasible gap, and return the steps for determining the candidate blocking gaps for the blocking task execution device to reach the destination based on the current environment description.

[0020] Thirdly, embodiments of this application provide an electronic device, including a processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory communicate with each other through the communication bus;

[0021] Memory, used to store computer programs;

[0022] When a processor executes a program stored in memory, it implements the steps of the method described in the first aspect above.

[0023] Fourthly, embodiments of this application provide a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the method described in the first aspect above.

[0024] Fifthly, embodiments of this application provide a computer program product comprising executable instructions that, when executed on a computer, cause the computer to perform the method described in the first aspect.

[0025] Beneficial effects of the embodiments in this application:

[0026] The solution provided in this application allows for the determination of the maximum static friction force of the obstacle to be moved based on its mass and the ground friction coefficient in the current environmental description. By simulating a robot moving the obstacle according to the moving direction and duration, based on the obstacle's mass, the ground friction coefficient in the current environmental description, and multi-body contact constraints between obstacles, the simulation can determine whether the robot can move the obstacle based on the relationship between the robot's maximum thrust and the obstacle's maximum static friction force. Furthermore, the multi-body contact constraints between obstacles ensure that if the obstacle encounters other obstacles during the moving process, it will not penetrate those other obstacles, thus improving the physical fidelity of the simulation. Since the simulation considers these various physical factors, the physical feasibility of overcoming each feasible gap (i.e., widening it to the passage width) after simulation is greatly improved. Therefore, selecting the gap to be overcome from the simulated feasible gaps can improve the accuracy of the planned passable path. Furthermore, by calculating the sum of the work consumed by the robot in pushing obstacles and the movement cost as the total cost, when prioritizing the target feasible gap with the lowest total cost for subsequent breakthroughs, the robot can prioritize the obstacle with the lowest physical energy consumption and the fastest path opening as the clearing target. In summary, this solution can quickly generate a feasible path with the lowest physical energy consumption and the fastest path opening while improving the accuracy of the planned feasible path.

[0027] Of course, implementing any product or method of the present invention does not necessarily require achieving all of the advantages described above at the same time. Attached Figure Description

[0028] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other embodiments can be obtained based on these drawings.

[0029] Figure 1 A flowchart of a path planning method provided in an embodiment of this application;

[0030] Figure 2 Another flowchart of the path planning method provided in the embodiments of this application;

[0031] Figure 3 Another flowchart of the path planning method provided in the embodiments of this application;

[0032] Figure 4 Another flowchart of the path planning method provided in the embodiments of this application;

[0033] Figure 5A width-connected graph provided in an embodiment of this application;

[0034] Figure 6 A flowchart illustrating a specific example of the path planning method provided in this application embodiment;

[0035] Figure 7 A flowchart illustrating step C in a specific example of the path planning method provided in this application embodiment;

[0036] Figure 8 This is a schematic diagram of the state transition of a robot provided in an embodiment of this application;

[0037] Figure 9 A schematic diagram of the initial state of a work scenario provided in an embodiment of this application;

[0038] Figure 10 A schematic diagram illustrating the end state of a work scenario provided in an embodiment of this application;

[0039] Figure 11 This is a schematic diagram of the structure of a path planning device provided in an embodiment of this application;

[0040] Figure 12 A block diagram of an electronic device for implementing the path planning method provided in the embodiments of this application. Detailed Implementation

[0041] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art based on this application are within the scope of protection of the present invention.

[0042] In scenarios such as warehousing and logistics centers and disaster relief sites, to ensure the safe passage of large vehicles from origin to destination, it is often necessary to deploy robotic teams to move movable obstacles in the environment, clearing a path from origin to destination that meets the width requirements for large vehicles to pass through. This problem of finding a path in an environment with movable obstacles is called "Navigation Among Movable Obstacles (NAMO)".

[0043] Currently, most existing NAMO methods idealize the robot as a point agent with infinite actuation capabilities and represent obstacles as abstract geometric shapes (such as dots or polygons). This purely geometric planning often results in planned paths that, while geometrically connected, are physically unenforceable. For example, the robot may be unable to push heavy obstacles along the planned path, or the contact point between the robot and the obstacle may not provide sufficient lever arm to generate the required motion. This can cause large vehicles to be unable to travel along the planned path, meaning the accuracy of the planned path is low.

[0044] To address the aforementioned issues, this application provides a path planning method, comprising: if there is no passable path with a width sufficient for the passage of the task execution device between the start and end points of the task to be executed, determining candidate obstruction gaps that would prevent the task execution device from reaching the end point based on the current environment description; the candidate obstruction gap is a channel between two obstacles with a width less than the passage width; determining the obstacle to be moved, the moving direction, and the moving time for the candidate obstruction gap; and, based on the mass of the obstacle to be moved, the ground friction coefficient in the current environment description, and the multi-body contact constraints between the obstacles, simulating a robot to move the obstacle to be moved according to the moving direction and moving time to obtain the post-movement environment corresponding to the candidate obstruction gap. Description: If a feasible gap exists among the candidate blocking gaps where the width between the two obstacles reaches the passage width after simulation and pushing, the work consumption and movement consumption of the robot pushing the obstacles are obtained according to the post-pushing environment description corresponding to the feasible gap, and the total consumption is obtained according to the movement consumption and work consumption; based on the post-pushing environment description corresponding to the target feasible gap with the lowest total consumption, it is detected whether the task execution device can reach the destination from the starting point; if it can, a passable path between the starting point and the destination through the target feasible gap is generated; otherwise, the current environment description is updated to the post-pushing environment description corresponding to the target feasible gap, and the step of determining the candidate blocking gap to block the task execution device from reaching the destination based on the current environment description is returned.

[0045] Based on the mass of the obstacle to be moved and the ground friction coefficient in the current environmental description, the maximum static friction force of the obstacle can be determined. By simulating the robot moving the obstacle according to the moving direction and duration, based on the obstacle's mass, the ground friction coefficient in the current environmental description, and multi-body contact constraints between obstacles, the simulation can determine whether the robot can push the obstacle based on the relationship between the robot's maximum thrust and the obstacle's maximum static friction force. Furthermore, the multi-body contact constraints between obstacles ensure that if the obstacle encounters other obstacles during the moving process, it will not penetrate those other obstacles, thus improving the physical fidelity of the simulation. Since the simulation considers these various physical factors, the physical feasibility of overcoming each feasible gap (i.e., widening it to the passage width) obtained after the simulation is greatly improved. Therefore, selecting the gap to be overcome from the simulated feasible gaps can improve the accuracy of the planned passable path. Furthermore, by calculating the sum of the work consumed by the robot in pushing obstacles and the movement cost as the total cost, when prioritizing the target feasible gap with the lowest total cost for subsequent breakthroughs, the robot can prioritize the obstacle with the lowest physical energy consumption and the fastest path opening as the clearing target. In summary, this solution can quickly generate a feasible path with the lowest physical energy consumption and the fastest path opening while improving the accuracy of the planned feasible path.

[0046] The path planning method provided in this application can be applied to various electronic devices, such as personal computers, servers, and other devices with data processing capabilities. Furthermore, it is understood that the path planning method provided in this application can be implemented through software, hardware, or a combination of both.

[0047] The path planning method provided in the embodiments of this application will now be described in detail with reference to the accompanying drawings.

[0048] like Figure 1 As shown, the path planning method provided in this application embodiment may include steps S101-S106:

[0049] S101, if there is no passable path between the start and end points of the task to be executed with a width that reaches the passage width of the task execution device, a candidate blocking gap is determined based on the current environment description to block the task execution device from reaching the end point; the candidate blocking gap is a channel between two obstacles with a width that is less than the passage width.

[0050] For example, the task execution device can be a large vehicle or other mobile device. In a warehousing and logistics scenario, for instance, the task execution device could be a mobile shelving unit. The task to be executed could be a transportation task or other task in the current scenario that requires movement from a starting point to a destination.

[0051] In practical applications, after assigning tasks to a task execution device, information acquisition devices such as radar and cameras can be used to obtain an environmental description of the workspace where the task execution device is located, which serves as the current environmental description. This environmental description can include information such as the location of immovable obstacles (e.g., walls, pillars), the location and geometry of movable obstacles, the robot's state (position, orientation, etc.), and the passage width required for the task execution device to pass.

[0052] After obtaining the current environment description, we can first determine whether there is a passable path between the start and end points of the task to be executed by the task execution device, that is, whether there is a path with a width sufficient for the task execution device to pass through. For example, the passable width required for the task execution device to pass through could be 1.2 times or 1.5 times the width of the task execution device, and so on.

[0053] For example, a Breadth-First Search (BFS) or Dijkstra's algorithm can be used to expand outwards from the starting point based on the current environment description. During the expansion process, a width constraint is maintained to ensure that the passable area at each step can accommodate a circular area with a diameter equal to the width required for the task execution device to pass. When the expanded area successfully covers the endpoint, it proves that a passable path with the required width exists. Alternatively, morphological dilation can be applied to obstacles in the map represented by the current environment description, with the dilation width being half the width required for the task execution device to pass. The expanded map can then be used... Algorithms such as the Dijkstra algorithm (a heuristic search algorithm) and other path search algorithms find a path from the starting point to the destination. If a path exists, then there is a walkable path from the starting point to the destination in the map represented by the current environment description.

[0054] If a passable path exists between the starting point and the ending point, it can be directly generated. If no path exists, based on the current environment description, all gaps currently blocking the task execution device from reaching the ending point are identified as candidate gaps. For example, a BFS or Dijkstra's algorithm can be used to identify gaps where the width between two obstacles encountered during the outward expansion from the starting point is less than the passable width of the task execution device. In other words, candidate gaps are bottleneck areas blocking the task execution device from reaching the ending point, with a width greater than 0 and less than the passable width of the task execution device. If the obstacle on either side of a candidate gap can be moved, pushing that obstacle can widen the candidate gap, thereby overcoming the bottleneck area blocking the task execution device from reaching the ending point.

[0055] S102, determine the obstacle to be moved, the moving direction and the moving time of the candidate blocking gap. Based on the mass of the obstacle to be moved, the ground friction coefficient in the current environment description and the multi-body contact constraints between obstacles, simulate the robot to move the obstacle to be moved according to the moving direction and the moving time to obtain the post-moving environment description corresponding to the candidate blocking gap.

[0056] For example, for each candidate blocking gap, the work required to move the obstacles on both sides of the gap can be calculated. Then, the obstacle corresponding to the minimum work consumption is determined as the obstacle to be moved corresponding to the candidate blocking gap. For the determined obstacle to be moved, the normal of the obstacle surface constituting the candidate blocking gap is determined to be the direction of widening the candidate blocking gap as the moving direction. The difference between the current width of the candidate blocking gap and the passage width is calculated to obtain the moving distance required to move the obstacle. The ratio of the moving distance to the robot's preset moving rate is calculated to obtain the moving time.

[0057] For example, if the passage between obstacle 1 and obstacle 2, with a width less than the passage width, is a candidate blocking gap A, and the mass of obstacle 1 is m1, the mass of obstacle 2 is m2 (m1 > m2), the current width of candidate blocking gap A is 1m, the passage width is 3m, and the robot's preset pushing speed is 0.1m / s, then the required width expansion of candidate blocking gap A is 2m. Pushing at the robot's preset pushing speed, the pushing time can be calculated to be 20s. The work consumed in pushing obstacle 1 is 2μm1g, and the work consumed in pushing obstacle 2 is 2μm2g. Since m1 > m2, the work consumed in pushing obstacle 2 is smaller; therefore, obstacle 2 is selected as the obstacle to be pushed. After determining that obstacle 2 is the obstacle to be pushed, the normal to the surface of obstacle 2 constituting candidate blocking gap A is directed towards the direction of widening the candidate blocking gap as the pushing direction. Here, μ is the ground friction coefficient.

[0058] After determining the obstacle to be moved, the moving direction, and the moving time corresponding to each candidate blocking gap, the robot can be simulated to move the obstacle according to the moving direction and moving time based on the mass of the obstacle to be moved, the ground friction coefficient in the current environment description, and the multi-body contact constraints between obstacles, thus obtaining the post-movement environment description corresponding to the candidate blocking gap. The multi-body contact constraints between obstacles in the current environment description include the positional information of each obstacle in the current environment.

[0059] Understandably, multi-body contact constraints between obstacles are motion restrictions imposed to prevent penetration and maintain physical plausibility when two obstacles are in contact. Based on the mass of the obstacle to be moved and the ground friction coefficient in the current environmental description, the maximum static friction force of the obstacle can be determined. By simulating a robot pushing an obstacle based on its mass, the ground friction coefficient in the current environmental description, and multi-body contact constraints, the relationship between the robot's maximum thrust and the obstacle's maximum static friction force can be used to determine whether the obstacle can be pushed. Furthermore, the multi-body contact constraints ensure that if the obstacle encounters other obstacles during the pushing process, it will not penetrate those other obstacles, thus improving the physical fidelity of the simulation.

[0060] Furthermore, if multiple robots are involved in the simulated movement, the maximum thrust of each robot is the maximum thrust of the entire robot team. It is understandable that by comprehensively considering key motion information such as obstacle mass, ground friction coefficient, and robot thrust limitations during the simulated movement, the physical feasibility of overcoming each feasible gap obtained after simulation is greatly improved. This increases the probability that the subsequently generated passable paths are executable in a real environment, thus improving the accuracy of the planned passable paths. Moreover, by considering multi-body contact constraints between obstacles during the simulation, the simulation achieves the function of clearing complex stacked channels using interactions between objects, effectively handling multi-object coupled dynamics problems in dense and chaotic environments.

[0061] For example, in practical applications, physics engines such as PyBullet (a Python-based physics simulation library) and MuJoCo (Multi-Joint dynamics with Contact, a physics simulation engine for multi-joint contact dynamics) can be used to simulate the movement of the obstacle to be moved. That is, the integrator built into the physics engine is used to perform forward extrapolation at discrete time steps. During this process, relying on the physics engine's underlying calculations of the obstacle's mass, ground friction coefficient, and multi-body contact constraints between obstacles, the state evolution results after the obstacle is moved according to the moving direction and duration are directly obtained, thus yielding a post-movement environment description corresponding to the candidate blocking gap.

[0062] This application does not limit the specific method of simulated obstacle movement in its embodiments. For example, it can also use physical calculations to perform forward extrapolation of the robot's obstacle movement process based on the mass of the obstacle to be moved, the ground friction coefficient in the current environment description, and the multi-body contact constraints between obstacles, to obtain the post-movement environment description corresponding to the candidate blocking gap. For the sake of clarity in the scheme layout, the specific method of simulated obstacle movement is described in the following embodiments, and will not be repeated here.

[0063] S103, if there is a feasible gap among the candidate blocking gaps where the width between the two obstacles reaches the passage width after the simulation push, obtain the work consumption and movement consumption of the robot pushing the obstacle according to the environmental description after the push corresponding to the feasible gap, and obtain the total consumption based on the movement consumption and work consumption.

[0064] After the simulation and shifting are carried out through the above step S102, the validity of the shifted environment description obtained by the simulation can be verified. That is, for each shifted environment description obtained by the simulation, it is detected whether the position of the corresponding obstacle to be shifted is located at the expected shifting position (i.e., the theoretical position predicted according to the shifting time and shifting direction). If it is located, the candidate blocking gap corresponding to the shifted environment description is determined as a feasible gap where the width between the two obstacles reaches the passage width after the simulation.

[0065] For each feasible gap that passes the simulation, the work consumed by the robot in moving the obstacles can be obtained based on the corresponding post-movement environment description. That is, the work consumed by the robot in moving obstacles from the initial environment description to the post-movement environment description corresponding to the feasible gap can be obtained. If the robot moves multiple obstacles from the initial environment description to the post-movement environment description corresponding to the feasible gap, the above work consumption is the total work consumed by the robot in moving all obstacles. Furthermore, based on the obstacles moved between the initial environment description and the post-movement environment description corresponding to the feasible gap, the robot's movement path from the starting point to the feasible gap can be determined, and the path length or travel time of this movement path can be determined as the robot's movement consumption from the starting point to the feasible gap.

[0066] For example, if the obstacles that are moved between the initial environmental description and the feasible gap include obstacle 1 and obstacle 2, and obstacle 1 is moved first and then obstacle 2 is moved, then the robot's movement path from the starting point to the feasible gap includes the shortest path 1 from the starting point to the position of obstacle 1 and the shortest path 2 from the position of obstacle 1 to the position of obstacle 2. The total path length of the shortest path 1 and the shortest path 2 is taken as the movement cost of the robot from the starting point to the feasible gap.

[0067] Alternatively, assuming that all gaps in the post-shift environment description corresponding to the feasible gap reach the passage width required by the task execution equipment, the path length or passage time of the shortest path from the feasible gap to the destination can be calculated as the estimated movement cost from the feasible gap to the destination. Finally, the sum of the movement cost from the starting point to the feasible gap and the estimated movement cost from the feasible gap to the destination is calculated to obtain the estimated movement cost from the starting point to the destination. This estimated movement cost from the starting point to the destination is the movement cost that needs to be obtained above.

[0068] After obtaining the work and movement costs incurred by the robot in moving obstacles, the sum of these costs can be calculated to obtain the total cost. It's understandable that by using the sum of work and movement costs as the total cost, when prioritizing the feasible gap with the lowest total cost for subsequent breakthroughs, obstacles with the lowest physical energy consumption and the fastest path clearing can be selected as the clearing targets.

[0069] In one approach, the validity of the simulated post-transition environment description can be verified in multiple dimensions, and the candidate blocking gaps corresponding to the post-transition environment descriptions that pass all multi-dimensional verifications are identified as feasible gaps.

[0070] For example, the verification content may include: (1) Displacement compliance: verify whether the obstacle to be pushed has undergone the expected displacement, and exclude the situation of "pushing but not moving"; (2) Contact retention determination: calculate the Euclidean distance between the robot end effector and the target contact point on the surface of the obstacle at the end of the simulation. If the Euclidean distance is less than the preset physical contact tolerance (e.g., 0.05 meters), and the angle deviation between the actual displacement vector of the obstacle and the direction of the thrust is within the allowable range (e.g., 5°), it is determined that effective contact has been maintained during the pushing process; otherwise, it is determined to be disengaged or slipping; (3) Deadlock detection: check whether the total kinetic energy abnormally decays to zero under the continuous application of thrust during the simulation process, so as to identify the situation where the obstacle is stuck by the environment.

[0071] Understandably, if the expected displacement is achieved, but the robot detaches or slips during the movement, it indicates that the movement was accidental. For example, if two robots are moving an obstacle, and one robot detaches from the obstacle while the other stumbles and pushes it to the expected position, although the expected displacement is achieved, the process is unstable, and stability cannot be guaranteed during actual movement. Similarly, if the simulation detects a decrease in total kinetic energy, even if the obstacle is eventually pushed to the expected position, there is a risk of deadlock during the movement, and stability cannot be guaranteed during actual movement.

[0072] By performing multi-dimensional verification on the post-shifted environment description, the candidate blocking gaps corresponding to the post-shifted environment descriptions that pass all multi-dimensional verifications are identified as feasible gaps. This makes the selected gaps have a higher probability of being breached when choosing a gap to break through from the feasible gaps, thereby improving the accuracy of the subsequently generated passable paths.

[0073] S104, based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption, detect whether the task execution device can reach the destination from the starting point.

[0074] For example, the method for detecting whether the task execution device can reach the destination from the starting point can refer to the method for determining whether there is a passable path between the starting point and the destination in step S101 above, which will not be repeated here.

[0075] S105, if reachable, generate a passable path between the starting point and the ending point that passes through the target feasible gap.

[0076] If step S104 detects that at least one passable path exists between the starting point and the ending point, it means that the task execution device can reach the ending point from the starting point. In this case, the following can be used: The algorithm, along with other path planning algorithms, generates the shortest path through the target feasible gap with the lowest total cost based on the post-shifted environment description corresponding to the target feasible gap with the lowest total cost, and uses this path as a feasible path.

[0077] S106, if unreachable, update the current environment description to the shifted environment description corresponding to the target feasible gap, and return to the step of determining the candidate blocking gap for the blocking task execution device to reach the destination based on the current environment description.

[0078] It is understandable that, since the total consumption corresponding to any feasible gap represents the cost for the robot to clear the path from the initial environmental description to the environmental description corresponding to that feasible gap, after determining that the starting point cannot reach the destination based on the shifted environmental description corresponding to the target feasible gap with the lowest total consumption, the shifted environmental description corresponding to the target feasible gap with the lowest total consumption is used as the current environmental description, and the process returns to step S101 to continue the loop. This ensures that the subsequently generated passable path is the path with the lowest cost for the robot to clear the path, thereby reducing the passage cost of the task execution equipment.

[0079] Furthermore, by simulating the movement of candidate obstruction gaps in each loop, i.e., embedding a parallel physical simulation mechanism in the planning loop, and subsequently selecting feasible gaps that can achieve the passage width after simulation for breakthrough, the feasibility and success rate of pushing the obstacles corresponding to the selected breakthrough gaps can be significantly improved in the real physical environment, thereby improving the accuracy of the passable path generated for the task execution device.

[0080] Optionally, in another embodiment of this application, step S102 above, which involves simulating a robot pushing an obstacle to be moved according to a pushing direction and a pushing duration to obtain a post-movement environment description corresponding to a candidate blocking gap, includes: determining a movement action of the robot within a preset range of the position of the obstacle to be moved in the current environment description, based on the robot's position in the current environment description, and determining a pushing action of the robot pushing the obstacle to be moved according to a pushing direction and a pushing duration; simulating the robot performing the movement action and the pushing action to obtain a post-movement environment description corresponding to a candidate blocking gap; correspondingly, in this embodiment, in Figure 1 Based on the illustrated embodiments, as Figure 2 As shown, step S103 above, based on the post-push environment description corresponding to the feasible gap, obtains the work consumption and movement consumption of the robot pushing the obstacle, including: S201, adding the simulated actions performed by the robot from the current environment description to the post-push environment description corresponding to the feasible gap to the tail of the cumulative task chain corresponding to the current environment description, to obtain the first cumulative task chain corresponding to the post-push environment description corresponding to the feasible gap; wherein, the cumulative task chain corresponding to any environment description records the sequence of simulated actions performed by the robot from the initial environment description to that environment description; S202, calculating the work consumption of pushing the obstacle to be pushed based on the obstacle to be pushed and the pushing time recorded in the first cumulative task chain; S203, calculating the first consumption of the robot executing the movement action recorded in the first cumulative task chain, and estimating the second consumption of the robot from the feasible gap to the destination based on the post-push environment description corresponding to the feasible gap, and calculating the movement consumption based on the first consumption and the second consumption. This embodiment stores the sequence of actions performed by the robot from the current environment description to any subsequent environment description. This allows the robot to determine all movement and pushing actions performed from the initial environment description to the current environment description based on the action sequence recorded in the cumulative task chain corresponding to that environment description, regardless of the environment description. Based on these recorded pushing and movement actions, the robot's work consumption and movement consumption can be quickly determined for each environment description.

[0081] Understandably, based on the determined obstacle to be moved, the moving direction, and the moving time, the robot's moving action for the obstacle can be determined according to the moving direction and the moving time. For any candidate obstacle gap, the robot performs a simulated moving action according to the determined moving action, thus obtaining the post-movement environment description corresponding to the candidate obstacle gap. Furthermore, since the robot needs to move to a preset range within the current environment description of the obstacle's position before moving it, the robot's position in the current environment description can be used to... The path planning algorithm calculates the shortest path from the robot's position to the position of the obstacle to be moved within a preset range in the current environment description. Based on the trajectory of this shortest path, the robot's movement action to move to the preset range of the obstacle to be moved in the current environment description is determined. For example, if the trajectory of the shortest path is trajectory A, then the robot's movement action is to move according to trajectory A.

[0082] For example, the aforementioned preset range could be 0.3m or 0.5m, etc. In practical applications, the center position of the gap corresponding to the obstacle to be moved can also be used as the position the robot needs to move to within the preset range of the obstacle's position. Since the robot first moves to the preset range of the obstacle's position before moving it, in the simulated action sequence executed by the robot from the current environment description to the post-movement environment description corresponding to the gap, the moving action comes first, followed by the pushing action.

[0083] Regarding step S201 above, after the path planning method provided in this application embodiment is executed, whenever a feasible gap exists after simulation, the sequence of actions simulated by the robot from the current environment description to the shifted environment description corresponding to the feasible gap is added to the tail of the cumulative task chain corresponding to the current environment description, thus obtaining the first cumulative task chain corresponding to the shifted environment description corresponding to the feasible gap. The initial cumulative task chain corresponding to the environment description is empty. During the execution of the path planning method, the cumulative task chain is continuously updated, so that the sequence of actions recorded in the cumulative task chain corresponding to each shifted environment description is: the sequence of actions simulated by the robot from the initial environment description to the shifted environment description.

[0084] For each feasible gap, the post-shift environment description can be calculated based on the action sequence recorded in the first cumulative task chain corresponding to the post-shift environment description of the feasible gap.

[0085] Regarding step S202 above, for each feasible gap, the post-push environment description is provided, and the first cumulative task chain corresponding to this post-push environment description records the pushing action. Based on the obstacle to be pushed indicated by each pushing action, the mass of the obstacle to be pushed can be obtained. Then, the product of the mass of the obstacle to be pushed and the ground friction coefficient is calculated to obtain the pushing force required to push the obstacle. Next, the product of the pushing duration indicated by the pushing action and the robot's preset pushing rate is calculated to obtain the pushing distance. Then, the product of the obtained pushing distance and the pushing force is calculated to obtain the work consumption for pushing the obstacle to be pushed. If multiple pushing actions are recorded in the first cumulative task chain corresponding to the post-push environment description, the sum of the work consumption corresponding to each of the multiple pushing actions is calculated as the required work consumption.

[0086] For step S203 above, the path length of the movement path or the travel time for the robot to complete the movement path at a preset rate can be calculated based on the movement path indicated by the movement action recorded in the first cumulative task chain, and used as the first consumption of the robot to perform the movement action recorded in the first cumulative task chain.

[0087] Based on the assumption that each gap in the post-shift environment description corresponding to the feasible gap reaches the passage width required by the task execution equipment, the following approach is adopted: The path planning algorithm plans the shortest path from the feasible gap to the destination, and calculates the path length of the shortest path from the feasible gap to the destination, or the travel time for the robot to complete the shortest path at a preset speed, as the estimated second cost for the robot to travel from the feasible gap to the destination. The sum of the first and second costs is calculated to obtain the estimated movement cost of the robot from the starting point to the destination.

[0088] Understandably, in practical applications, after widening the last feasible gap that connects the start and end points, the robot does not need to move from that gap to the end point. Assuming that all gaps in the environment description corresponding to that feasible gap reach the passage width required by the task execution device after the gap is widened, the second cost for the robot to travel from the feasible gap to the end point is estimated. This second cost can represent the passage cost of the remaining path after the feasible gap is opened. By using the total cost including the second cost as the optimization objective, feasible gaps that can significantly shorten the remaining path after being opened can be prioritized for breakthrough, enabling the generation of the shortest possible passable path from the start to the end point for the task execution device.

[0089] Optionally, in another embodiment of this application, the work consumption for moving the obstacle to be moved in step S202 above, based on the obstacle to be moved and the moving time recorded in the first cumulative task chain, includes: if the ratio of the maximum static friction of the obstacle to be moved recorded in the first cumulative task chain to the maximum thrust of the robot is greater than 1, then the product of the ratio, the moving time of the pushing action instruction, and the thrust required by the obstacle to be moved is calculated to obtain the third consumption; if the ratio is not greater than 1, then the product of the moving time of the pushing action instruction and the thrust required by the obstacle to be moved is calculated to obtain the fourth consumption; the sum of the calculated third consumption and fourth consumption is used to obtain the work consumption for moving the obstacle to be moved. By applying a penalty coefficient to the work consumption when the ratio of the maximum static friction of the obstacle to be moved to the maximum thrust of the robot is greater than 1, the work consumption of heavier objects can be amplified. This allows for the priority selection of feasible gaps corresponding to lighter obstacles when the feasible gap with the lowest total consumption is further expanded. In this way, the search speed for low-cost clearing paths can be accelerated, and thus a passable path with low clearing cost can be obtained quickly.

[0090] Understandably, if the ratio of the maximum static friction of the obstacle to be moved to the robot's maximum thrust is greater than 1, it indicates that the obstacle is too heavy and the robot team cannot move it. In this embodiment, to ensure that obstacles that cannot be moved have higher work consumption, and thus will not be prioritized for moving when selecting a feasible gap to break through, the third consumption is obtained by multiplying this ratio by the product of the pushing time indicated by the pushing action and the magnitude of the thrust required to move the obstacle.

[0091] If the ratio of the maximum static friction of the obstacle to be moved to the maximum thrust of the robot is not greater than 1, then the product of the pushing time indicated by the pushing action and the magnitude of the thrust required to move the obstacle is directly used as the fourth consumption.

[0092] In other words, this embodiment sets a penalty multiplier based on a threshold, which is the ratio of the maximum static friction of the obstacle to be moved to the robot's maximum thrust. If this ratio is greater than 1, the work consumed is amplified by this ratio, making the cost of clearing heavy obstacles with high friction significantly higher than that of light obstacles. The robot's maximum thrust is the resultant force of the maximum thrust of all robots in the robot team, which can consist of one or more robots.

[0093] After calculating the third or fourth consumption corresponding to each push action recorded in the cumulative task chain in the above manner, the sum of the calculated third and fourth consumptions is the work consumption obtained based on the post-push environment description corresponding to the feasible gap.

[0094] Alternatively, in another embodiment of this application, in Figure 1 Based on the illustrated embodiments, as Figure 3As shown, before detecting whether the task execution device can reach the end point from the starting point in step S104 based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption, the method further includes: S301, sorting the data objects of the shifted environment description corresponding to the feasible gap in order of total consumption from low to high, and storing them in a priority queue; the data objects of the shifted environment description corresponding to the feasible gap store the shifted environment description corresponding to the feasible gap, the total consumption, and the sequence of simulated actions executed by the robot from the initial environment description to the shifted environment description corresponding to the feasible gap; S302, generating a specified node in the search tree based on the data object with the highest priority in the priority queue. Child nodes; wherein, the specified node is the node corresponding to the current environment description, and the root node of the search tree is the node corresponding to the initial environment description; accordingly, in this embodiment, the above step S104, based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption, detects whether the task execution device can reach the destination from the starting point, including: based on the environment description recorded in the newly generated child node, detects whether the task execution device can reach the destination from the starting point; accordingly, in this embodiment, generating a traversable path between the starting point and the destination that passes through the target feasible gap, includes: generating a traversable path that passes through all nodes corresponding to feasible gaps on the branch from the root node in the current search tree to the newly generated child node. Through the above node expansion method, the traversable path with the lowest total consumption can be quickly searched. Furthermore, by adopting the tree structure expansion method, lightweight topology search provides global guidance for the heavy dynamic search, thereby accelerating convergence and enabling the traversable path with the lowest total consumption between the starting point and the destination to be quickly searched.

[0095] This embodiment uses a state-space search tree to perform path search. The root node of the search tree stores the initial environment description, which is the environmental state of the work scenario when the path planning method starts to execute. The environment description may include key kinematic information such as the position, orientation, instantaneous linear velocity vector, and instantaneous angular velocity of all obstacles and the robot.

[0096] For step S301, the post-shift environment description corresponding to each feasible gap can be obtained according to step S103 above, which yields the total consumption corresponding to that post-shift environment description. The data objects of the post-shift environment descriptions corresponding to the feasible gaps are sorted in ascending order of total consumption and stored in a priority queue, which is initially empty. The data objects store the post-shift environment description corresponding to the feasible gap, the total consumption, and the sequence of simulated actions performed by the robot from the initial environment description to the post-shift environment description corresponding to the feasible gap.

[0097] In other words, a data object is a complex data structure that encapsulates a complete snapshot of the physical environment and a historical sequence of actions. Specifically, each data object stores the following core information: (1) Post-shift environment description: contains the above-mentioned key kinematic information of all obstacles and robots in the scene, which enables the simulation environment to be "rolled back" to the moment recorded by the data object at any time using the "state recovery" function of the physics engine, thereby supporting the continuation of dynamics deduction from any intermediate state. (2) Action sequence: records a series of coordinated pushing actions simulated from the initial environment description to the post-shift environment description corresponding to the feasible gap, constituting the current cleanup scheme. (3) Total consumption: contains the accumulated execution cost g from the initial environment description to the post-shift environment description corresponding to the feasible gap and the estimated remaining cost h from the feasible gap to the endpoint. Among them, the execution cost g represents the accumulated physical consumption (including movement consumption and work consumption) from the initial environment description to the post-shift environment description corresponding to the feasible gap; the estimated remaining cost h is the movement consumption from the feasible gap to the endpoint.

[0098] For step S302, based on the data object ranked first in the priority queue, a child node of the specified node in the search tree is generated. That is, during the expansion of the search tree from the root node, the data object with the lowest total consumption is popped from the priority queue each time the tree expands, and this data object becomes a child node of the specified node, updating the search tree. The specified node is the node corresponding to the current environment description; in the first round of expansion, the specified node is the root node.

[0099] In this embodiment, whenever the search tree expands to a new child node, it first checks whether the post-shift environment description recorded in that child node meets the connectivity requirements, that is, it checks whether the task execution device in the environment represented by the post-shift environment description of that record can reach the destination from the starting point. The specific checking method can be referred to the relevant description in step S104 above, and will not be repeated here.

[0100] If, upon inspection, the newly expanded child node satisfies the connectivity requirements, it indicates that a feasible cleanup path has been found. Then, a walkable path can be generated, identifying all feasible gaps on the branch from the root node to the newly expanded child node. For example, this can be achieved using... The algorithm, along with other path planning algorithms, generates the shortest path through all feasible gaps on the branch from the root node to the newly expanded child node, based on the post-shift environment description corresponding to the newly expanded child node. This path serves as the passable path.

[0101] If the newly expanded child node does not meet the connectivity requirements, the expansion continues until a child node that meets the connectivity requirements is reached or the priority queue is empty.

[0102] Understandably, because the search tree is expanded by popping the data object with the lowest total cost from the priority queue each time, the aforementioned node expansion method can quickly find the feasible path with the lowest total cost. Furthermore, the tree structure expansion method utilizes lightweight topology search to provide global guidance for the heavy dynamic search, thereby accelerating convergence.

[0103] Optionally, in another embodiment of this application, the path planning method further includes: sending the action sequence recorded in the target task chain to the robot, so that the robot performs the operation according to the received action sequence; if, after the robot performs the operation according to the received action sequence, all obstacles to be moved indicated by each pushing action recorded in the target task chain reach the expected pushing position, then the step of generating a passable path between the starting point and the ending point through the target feasible gap in step S105 is executed. In this embodiment, before generating a passable path for the task execution device, the action sequence recorded in the target task chain is sent to the robot, so that the robot performs the operation according to the received action sequence to move each obstacle to be moved in the actual scene. Generating a passable path after all obstacles to be moved indicated by each pushing action recorded in the target task chain have reached the expected pushing position can further improve the accuracy of the generated passable path.

[0104] Understandably, if the starting point in the environment represented by the shifted environment description corresponding to the feasible gap with the lowest total consumption is reachable from the destination, it indicates that a passable path exists in the shifted environment description. Therefore, in order for the task execution device to successfully reach the destination from the starting point, the robot needs to shift obstacles in the actual scene, that is, perform operations according to the action sequence recorded in the target task chain.

[0105] In this embodiment, the action sequence recorded in the target task chain is sent to the robot so that the robot can perform the operation according to the order of the actions in the action sequence after receiving the action sequence, so as to push the obstacle in the actual scene.

[0106] The expected displacement position of each obstacle to be moved is the position where the gap corresponding to the obstacle reaches the passage width required by the task execution device. For example, the expected displacement position of the obstacle to be moved can be calculated based on the position, direction, and duration of the obstacle to be moved in the current environment description. That is, the displacement distance is obtained by multiplying the displacement duration by the robot's preset displacement rate, and then the position of the obstacle to be moved in the current environment description by the displacement distance is taken as the expected displacement position of the obstacle to be moved.

[0107] If, after the robot performs its work according to the received action sequence, all obstacles to be pushed as indicated by each pushing action in the target task chain reach their expected pushing positions, it means that the robot has successfully widened the gaps corresponding to each obstacle to be pushed to the required passage width for the task execution equipment. At this point, there is no bottleneck area between the start and end points that affects the passage of the task execution equipment. Therefore, referring to the relevant description in step S105 above, a passable path with the lowest total consumption through the target feasible gap between the start and end points can be generated.

[0108] Optionally, in another embodiment of this application, before performing step S105 above to generate a passable path between the starting point and the ending point through the target feasible gap, the path planning method further includes: if it is detected during the robot's operation that the actual moving trajectory of the obstacle to be moved deviates from the original moving trajectory, then taking the current position of the obstacle to be moved as the local starting point and the original target position of the obstacle to be moved as the local ending point, regenerating the moving trajectory between the local starting point and the local ending point, and redetermining the moving action that the robot needs to perform based on the newly generated moving trajectory; sending the newly determined moving action to the robot so that the robot moves the obstacle to be moved according to the newly received moving action. This embodiment detects that when the actual movement trajectory of any obstacle deviates from the original movement trajectory during the robot's operation, it performs local replanning of the robot's current original movement trajectory and generates a new pushing action, which is then sent to the robot. This allows the robot to push the obstacle according to the latest received pushing action, thus correcting the obstacle that has deviated from the original movement trajectory back to the original movement trajectory. This improves the widening efficiency of the gap corresponding to the obstacle, thereby increasing the speed of generating subsequent passable paths.

[0109] For example, in practical applications, the real-time positions of each obstacle to be moved can be periodically acquired from information acquisition devices (radar or cameras) deployed in the current work scenario. Based on the acquired real-time positions, it can be determined whether the actual moving trajectory of each obstacle deviates from the original moving trajectory. The original moving trajectory for any obstacle to be moved is the trajectory obtained by the robot moving the obstacle in the corresponding moving direction for the corresponding moving time. For example, the Euclidean distance and / or angular deviation between the acquired real-time position of the obstacle to be moved and the nearest point on the original moving trajectory can be calculated. This angular deviation is the angle between the direction of the line connecting the starting point of the actual moving trajectory to the real-time position of the obstacle to be moved and the direction of the line connecting the starting point of the original moving trajectory to the nearest point. Based on the Euclidean distance and / or angular deviation, it can be determined whether the actual moving trajectory of the obstacle to be moved deviates from the predetermined moving trajectory. For example, if the Euclidean distance exceeds a predetermined distance (e.g., 0.3 meters), it is determined that the actual movement trajectory of the obstacle to be moved deviates from the predetermined trajectory; or if the angular deviation is greater than a preset angle (15°), it is determined that the actual movement trajectory of the obstacle to be moved deviates from the predetermined trajectory; or, the average level or weighted sum of the Euclidean distance and angular deviation is calculated (the weights can be set by relevant technical personnel based on experience) to obtain a comprehensive deviation value. If the comprehensive deviation value exceeds a preset error, it is determined that the actual movement trajectory of the obstacle to be moved deviates from the predetermined trajectory.

[0110] If any obstacle to be moved deviates from its original trajectory, a local replanning is triggered. This involves regenerating the trajectory from the obstacle's current position as the local starting point to its original target position as the local ending point. The original target position of the obstacle is the position obtained after the robot has moved the obstacle for the specified duration according to its direction and preset speed.

[0111] For example, a constant screw interpolation algorithm can be used to regenerate the pushing trajectory. Specifically, assuming the obstacle to be pushed maintains a constant forward speed and rotational angular velocity in its own coordinate system, a pushing trajectory that smoothly connects the local starting point and local ending point can be directly solved through geometric analytical calculations (it automatically degenerates into a straight path if no rotation is required), and the corresponding speed control parameters are obtained. Then, based on the newly generated pushing trajectory and speed control parameters, the pushing action that the robot needs to perform is re-determined. For example, control algorithms such as MPC (Model Predictive Control) can be used to determine the pushing action command based on the pushing trajectory and speed control parameters.

[0112] Since the newly determined pushing action is based on the assumption that the obstacle to be pushed maintains a constant forward speed and rotational angular velocity, the robot can smoothly "correct" the obstacle to be pushed back to the original pushing trajectory without changing the contact mode (i.e., without needing to readjust the robot formation or release the handle).

[0113] Optionally, in another embodiment of this application, before generating a passable path between the starting point and the ending point through the target feasible gap in step S105, the path planning method further includes: if, during the moving of any obstacle to be moved, the actual moving trajectory deviates from the original moving trajectory a predetermined number of times, then the current environment description is re-acquired, and the step of determining the candidate blocking gap for the blocking task execution device to reach the ending point based on the current environment description is returned. In this embodiment, if, during the moving of any obstacle to be moved, the actual moving trajectory deviates from the original moving trajectory multiple times, it indicates that it is difficult to move the obstacle to be moved to the expected moving position. In this case, the current environment description can be re-acquired, and global replanning can be performed based on the re-acquired current environment description to re-search for passable paths in the current environment.

[0114] For example, the number of reservations could be 3 or 5 times, etc.

[0115] It is understandable that if the number of times the actual moving trajectory of any obstacle to be moved deviates from the original moving trajectory during the moving process does not reach the predetermined number, the above-mentioned local replanning will be triggered. If the predetermined number is reached, it means that it is difficult to move the obstacle to be moved to the expected moving position. At this time, the gap corresponding to the obstacle to be moved is difficult to widen to the passage width required for the task execution device to pass. In this case, in order to generate an accurate passable path, the current environment description can be obtained again, and the process can be returned to step S101 to search for the path again.

[0116] Optionally, in another embodiment of this application, in step S102 above, based on the mass of the obstacle to be moved, the ground friction coefficient in the current environmental description, and the multi-body contact constraints between obstacles, the robot is simulated to move the obstacle according to the moving direction and moving time to obtain the post-movement environmental description corresponding to the candidate blocking gap. This includes: S1021, determining the contact position between the robot and the obstacle to be moved and the robot's thrust based on the shape and moving direction of the obstacle to be moved; S1022, simulating the moving of the obstacle to be moved based on the mass of the obstacle to be moved, the ground friction coefficient in the current environmental description, and the multi-body contact constraints between obstacles, applying thrust according to the duration of continuous moving at the contact position, to obtain the post-movement environmental description corresponding to the candidate blocking gap. This embodiment simulates the moving of the obstacle to be moved by applying thrust according to the duration of continuous moving at the contact position during simulation, so that the contact position between the robot and the obstacle and the robot's thrust can be fully considered during the simulation, thereby accurately determining whether the robot can push the obstacle to be moved by applying thrust at the contact position. This improves the physical assurance of the simulation, making it more likely that the feasible gaps identified in subsequent simulations can be overcome in real-world scenarios, thereby further improving the accuracy of the planned traversable paths.

[0117] For step S1021, for example, there may be one or more robots performing the pushing task. In practical applications, a pushing pattern sampling method can be used to assign contact positions and thrust to the robots performing the pushing task. The thrust is a vector, including both magnitude and direction.

[0118] Specifically, the pushing direction and the robot's preset pushing rate can be used as the target pushing velocity vector. A physical model (such as a friction balance model under quasi-static assumptions) is used to inversely solve for the ideal net external force and net torque required to achieve this target pushing velocity vector, i.e., the target generalized force vector is obtained. Next, based on the shape of the obstacle to be pushed, the set of reachable contact points for each robot on the obstacle is traversed, and a rapid geometric screening is performed (e.g., eliminating contact points where the contact force generated is opposite to the pushing direction, i.e., doing negative work). After screening, candidate contact points between the robot and the obstacle to be pushed are obtained.

[0119] Next, if only one robot is performing the pushing task, the magnitude of the thrust is determined to be the product of the ground friction coefficient and the mass of the obstacle to be pushed. The generalized force (including torque) generated when the robot applies the thrust at each candidate contact point is calculated, and the candidate contact point with the highest cosine similarity in direction to the target generalized force vector (i.e., most conforming to the pushing direction) is selected as the contact position between the robot and the obstacle. The direction of the force applied by the robot at this contact position is the direction of the robot's thrust.

[0120] If multiple robots are performing the pushing task, for each robot, the generalized force (including torque) generated when it applies the maximum force at each candidate contact point is calculated, and the candidate contact point with the highest cosine similarity in direction to the target generalized force vector (i.e., most conforming to the pushing direction) is selected as the initial solution. To further reduce the deviation between the robot's actual resultant force and the target generalized force, an iterative optimization strategy is adopted. That is, the residual vector between the actual resultant force generated by the current multiple robots and the target generalized force is calculated, and then the contact points between the robots and the obstacle to be pushed are tried one by one. This process is repeated until the residual converges or the iteration upper limit is reached. In this way, a new combination of contact points can be found that minimizes the residual and improves the overall force vector matching degree. Each contact point in the obtained new combination of contact points is the contact position between each robot and the obstacle. At this time, the magnitude of the thrust of each robot is the maximum thrust value of the robot, and the direction of the thrust is the direction generated when the robot applies force at the above-determined contact position.

[0121] Regarding step S1022, after obtaining the contact position and thrust between the robot and the obstacle, a pushing mode including the contact position, thrust, and pushing duration can be assigned to the robot. The pushing duration is calculated based on the pushing distance required to widen the candidate blocking gap corresponding to the obstacle to be pushed to the passage width of the task execution device; that is, the ratio of the pushing distance to the robot's preset pushing rate is calculated to obtain the robot's pushing duration.

[0122] After assigning a pushing mode to the robot, the robot can be simulated to push the obstacle to be pushed according to the assigned pushing mode. For example, in practical applications, the mass of the obstacle to be pushed, the ground friction coefficient in the current environment description, the multi-body contact constraints between obstacles, and the robot's pushing mode can all be input into the physics engine. The physics engine's built-in integrator can then use the input data to deduce the position of the obstacle to be pushed. After simulation, the post-pushing environment description corresponding to the candidate blocking gap can be obtained.

[0123] Optionally, in another embodiment of this application, the step S1021 above, which determines the contact position between the robot and the obstacle to be moved and the robot's thrust based on the shape and direction of the obstacle to be moved, includes: querying a target pattern from historical patterns that records the shape and direction of the obstacle to be moved; the historical patterns record the shape, direction, contact position, and thrust used by the robot in successfully moving obstacles; and determining the contact position between the robot and the obstacle to be moved and the robot's thrust based on the thrust and contact position recorded in the target pattern. In this embodiment, before assigning a moving pattern to the robot, it first queries the historical patterns to see if a target pattern recording the shape and direction of the obstacle to be moved exists. If it does, the thrust and contact position from that target pattern are reused, thereby quickly determining the robot's moving pattern and improving the overall efficiency of path planning.

[0124] For example, in practical applications, the pushing pattern (including the robot's contact position with the obstacle and the pushing force used), the shape of the obstacle, and the pushing direction used by the robot when it successfully performed a pushing task in the past can be stored as a historical pattern. Before assigning a pushing pattern to the robot, it is first checked whether the target pattern of the shape and pushing direction of the obstacle to be pushed is recorded in the historical patterns.

[0125] If a target pattern exists, the thrust and contact position recorded in the target pattern can be directly used to determine the contact position between the robot and the obstacle to be moved, and the robot's thrust.

[0126] Optionally, in another embodiment of this application, the path planning method further includes: based on the current environment description, detecting whether the obstacle to be moved will collide with an immovable obstacle during the moving process in the moving direction and for a certain duration; if no collision occurs, then executing the step S102 above, which involves simulating the robot moving the obstacle to be moved according to the moving direction and moving duration based on the mass of the obstacle to be moved, the ground friction coefficient in the current environment description, and the multi-body contact constraints between obstacles. This embodiment, by performing rapid geometric screening before simulation, can quickly eliminate moving actions that will collide with immovable obstacles, reduce unnecessary simulations, thereby reducing simulation time and improving the overall efficiency of path planning.

[0127] For example, assuming all obstacles are swayable, the trajectory of the obstacle to be moved, moving at a predetermined direction and speed for a set duration, can be determined. Then, based on the current environment description, it can be determined whether the positions of each obstacle intersect this trajectory. If they intersect, it indicates that the obstacle to be moved will collide with an immovable obstacle during its movement. It is understood that if the obstacle to be moved collides with an immovable obstacle during its movement, the immovable obstacle will block its further movement due to the force generated by the collision, preventing it from reaching the intended position. Therefore, obstacles that will collide with immovable obstacles are not subjected to further simulation to reduce unnecessary simulation time.

[0128] Alternatively, in another embodiment of this application, in Figure 1 Based on the illustrated embodiments, as Figure 4 As shown, step S101 above, which determines each candidate obstruction gap for the obstruction task execution device to reach the endpoint based on the current environment description, includes: S401, detecting whether the line-of-sight vector from the starting point to the endpoint intersects with the width connectivity graph corresponding to the current environment description; wherein, the width connectivity graph includes a first edge and a second edge, the first edge being the edge between any two bridging nodes on the convex components, and the second edge being the edge between a bridging node on any convex component and the center node on that convex component; the bridging nodes on the two convex components are the two convex components whose distance is less than the passage width. Nearby point; convex components include convex-shaped obstacles in the current environment description, as well as convex-shaped obstacles obtained after decomposing non-convex-shaped obstacles; S402, if they intersect, the first intersecting edge on the width-connected graph is taken as the starting blocking edge, and a detour search algorithm is used to search for detour paths on the width-connected graph to determine whether there is a closed-loop topology between the starting point and the ending point; S403, if a closed-loop topology exists and the number of intersections between the line-of-sight vector and the closed-loop topology is odd, the first edge on the closed-loop topology is determined as a candidate blocking gap for the task execution device to reach the ending point. This scheme can quickly determine each candidate blocking gap for the task execution device to reach the ending point, thereby improving the search speed for passable paths.

[0129] For example, in practical applications, a width-connected graph corresponding to the current environment description can be constructed first. Specifically, the non-convex obstacles in the environment represented by the current environment description are first decomposed into several convex components using a convex decomposition algorithm. Next, a center node is created for the geometric center of each convex component. Then, all pairs of convex components in the environment are traversed, and the Euclidean shortest distance between the pairs of convex components is calculated. When the shortest distance is less than the passage width required by the task execution device, a potential passage bottleneck is identified, and a bridging node is created at the two points closest to each other on the surface of the convex component pair. Then, within the same convex component, an edge connecting the center node to all bridging nodes on that component is established as the second edge, and an edge connecting a pair of bridging nodes spanning different convex components is established as the first edge, resulting in a width-connected graph including the first and second edges.

[0130] On the width-connected graph corresponding to the current environment description, a line-of-sight vector from the starting point to the ending point is constructed. This line-of-sight vector is then geometrically intersected with the width-connected graph. If the line-of-sight vector intersects with any edge in the graph, the first intersecting edge is designated as the initial blocking edge, triggering a heuristic detour search on the width-connected graph. Detour search algorithms, such as the Bug algorithm, are used to search for detour paths on the width-connected graph. If the detour path returns to the initial blocking edge, a closed-loop topology is determined to exist between the starting and ending points.

[0131] like Figure 5 As shown, the Figure 5 The rectangles on the top, bottom, left, and right sides of the diagram represent walls, and the polygons represent movable obstacles. The line connecting points S and G is the line of sight vector, and the first edge where this line of sight vector intersects with the first edge on the width-connected graph is the initial blocking edge. As shown by the arrowed path in the diagram, starting from this initial blocking edge, the next direction of travel is determined based on the geometric angle relationship between the current edge and its adjacent edges (e.g., always choosing the left adjacent edge with the smallest relative angle). The process of circling around the outer contour of the obstacle's connected domain is simulated on the width-connected graph, as follows. Figure 5 The middle arrow passes sequentially through the edge nodes and gaps of each obstacle (i.e., the dashed lines in the diagram), forming a clockwise or counterclockwise trajectory around the obstacle cluster. This traversal process continues until the search path returns to the initial blocking edge, thus geometrically depicting a closed loop, which is the closed-loop topology between the starting and ending points.

[0132] If a closed-loop topology exists, it can be verified whether this closed-loop topology constitutes a substantial obstruction to the global path from the start point to the end point. For example, the number of geometric intersections between the line-of-sight vector from the start point to the end point and the closed-loop topology can be calculated. If the number of intersections is even, it indicates that the start point and the end point are located on the same side of the closed-loop topology (e.g., both outside the loop or both inside the loop), and the closed-loop topology is not a topological boundary that isolates the start point and the end point.

[0133] If the number of intersections is odd, it indicates that the starting point is trapped inside the closed-loop topology while the ending point is located outside (or vice versa), and the closed-loop topology strictly isolates the starting point and the ending point. In this case, each first edge on the closed-loop topology is identified as a candidate blocking gap that prevents the task execution device from reaching the ending point.

[0134] The path planning method provided in this application embodiment will now be described with reference to a specific example.

[0135] like Figure 6 As shown, the overall process of the path planning method provided in this example includes: S601, constructing a Width Clearance Connected Graph (WCCG); S602, determining whether the starting point to the target point is connected; if yes, proceed to step 606, otherwise proceed to step S603; S603, sorting the blocking gaps based on physical cost evaluation; S604, performing path search based on mixed physical information; S605, determining whether a passable path has been found; if yes, proceed to step S607, otherwise return to step S601; S606, generating a passable path from the starting point to the target point; S607, controlling the robot to clear the passage according to the obtained passable path.

[0136] The above process includes four main steps: Step A, constructing a W-width gap connectivity graph (corresponding to the width connectivity graph above) and performing initial connectivity screening; Step B, if the initial screening finds that the path from the starting point (corresponding to the starting point above) to the target point (corresponding to the ending point above) is blocked, then the blocking gaps (corresponding to the candidate blocking gaps above) are ranked based on physical cost evaluation; Step C, entering the physical perception hybrid search stage, generating feasible cooperative migration strategies through parallel physical simulation; Step D, handing over the feasible cooperative migration strategies to the online execution module for closed-loop control.

[0137] Step A is primarily used for environment modeling and initial path determination. The specific implementation involves the following steps:

[0138] A1. Environmental Information Acquisition and Component Decomposition: First, acquire the current state information of the workspace (corresponding to the current environment description above), including the positions of immovable obstacles (such as walls and pillars), the positions and geometry of the set of movable obstacles, the state of the robot set (position, orientation, etc.), and the width threshold W required for the passage of large vehicles (corresponding to the passage width required for the task execution equipment above). To facilitate geometric calculations, for irregularly shaped non-convex obstacles in the set of movable obstacles, a convex decomposition algorithm is used to decompose them into several convex components.

[0139] A2. Generate graph nodes: Based on the decomposed convex components, first generate graph nodes, including generating center nodes and bridging nodes. (1) Center node generation: Calculate the geometric center of each convex component and create a center node at that location (e.g., Figure 5 (2) Bridge node generation: Traverse all convex component pairs in the environment and calculate the Euclidean shortest distance between each pair of convex components. When the shortest distance is less than the set width threshold W, it is determined that there is a potential passage bottleneck between the two convex components (i.e., large vehicles cannot pass directly), and a bridge node is created at the two points closest to each other on the surface of the convex component pair (e.g., the circle inside the obstacle). Figure 5 (As shown by the dots at the edge of the obstacle).

[0140] A3. Graph Construction: After the graph nodes are generated, establish connecting edges between the generated nodes, including establishing center-bridge edges and bridge-bridge edges. (1) Establishing center-bridge edges (corresponding to the second edge mentioned above): Within the same convex component, establish an edge connecting the center node of the component to all bridge nodes on the surface of the component. (e.g.) Figure 5 (2) Constructing a bridge - a bridge edge (corresponding to the first edge above): establish an edge connecting a pair of bridge nodes that span different convex components. This edge represents the physical gap between two adjacent obstacles. Calculate the actual Euclidean distance of this gap and use it as the width attribute value of the edge (e.g., the black solid line inside the obstacle). Figure 5 The black dashed line connecting the two obstacles (the numbers 0.27, 0.35, etc. next to it represent the actual width of the gap) forms the graph structure described above, which is the W-width gap connectivity graph. Based on the bridging edges in the graph, bottleneck areas obstructing passage in the environment can be visually identified.

[0141] A4. Connectivity Determination: Perform a ray intersection test. On the constructed WCCG ​​graph, establish a straight path (i.e., the line-of-sight vector) from the starting point S of the large vehicle to the target point G. Perform a global geometric intersection calculation on this line-of-sight vector with all edges in the WCCG ​​graph (including center-bridge edges inside obstacles and bridge-bridge edges between obstacles). If the line-of-sight vector does not intersect with any edge in the graph, it is determined that the starting point and the target point are directly connected in the current space, without needing to detour. Directly output the straight path from the starting point to the target point and terminate.

[0142] A5. Closed-loop topology determination: If the line-of-sight vector intersects with any edge in the graph (i.e., the straight path is blocked), the first intersecting edge is taken as the starting blocking edge, triggering a heuristic detour search on the graph (e.g., using the Bug algorithm for detour path search). Figure 5The path indicated by the arrow shows that, on the WCCG ​​graph, the starting point for circumventing the obstacle is the initial barrier edge. The next direction of travel is determined based on the geometric angle relationship between the current edge and its adjacent edges (e.g., always choosing the left adjacent edge with the smallest relative angle). The process of circumventing along the outer contour of the obstacle's connected domain is simulated on the edge network of the WCCG ​​graph, as shown below. Figure 5 The middle arrow passes through the edge nodes and gaps of each obstacle in sequence, forming a clockwise or counterclockwise trajectory around the obstacle cluster. This traversal process continues until the search path returns to the initial blocking edge, thus geometrically depicting a closed loop and obtaining a closed-loop topology.

[0143] A6. Closing-Loop Topology Blockage Verification and Leading-Line Loop Locking: After the heuristic detour process returns to the initial blocking edge, forming a geometric loop, the topological properties of this loop are first verified to determine whether it constitutes a substantial blockage to the global path. The verification logic is as follows: Calculate the number of geometric intersections between the line-of-sight vector from the starting point S to the target point G and the closing-loop topology. If the number of intersections is even, it indicates that the starting point and the target point are located on the same side of the closing loop (e.g., both outside or both inside the loop), and the closing-loop topology is not a topological boundary isolating the starting point and the target point. In this case, ignore this loop and continue exploring. If the number of intersections is odd, according to the parity rule of ray casting in computational geometry, it indicates that the starting point is trapped inside the closing loop while the target point is located outside (or vice versa). At this time, the closing-loop topology strictly isolates the starting point and the target point. This closing-loop topology is formally defined as the leading-line loop, which represents the physical barrier that must be overcome.

[0144] A7. If the closed-loop topology formed by the detour constitutes a substantial obstacle (i.e., after detouring around, it is found that there is no direct path to the target point G), then extract all edges with the attribute "bridge-bridge edge" (i.e., physical gaps connecting different obstacles) on the closed-loop topology, mark them as "candidate obstacle gaps", and directly send them to the subsequent step B for priority evaluation.

[0145] Step B is mainly used to sort the candidate blocking gaps. In practice, the following steps are performed:

[0146] B1. Candidate Blocking Gap Extraction: Based on the leading edge ring identified in step A, trace all edges on the leading edge ring (i.e., the boundary of the topological connected domain where the current vehicle's starting point is located) and extract all "bridging edges" whose actual gap width is less than the width threshold W. These edges represent potential bottleneck locations that block vehicle passage and are defined as the candidate blocking gap set.

[0147] B2. Physical Cost Calculation: For each gap in the above candidate blocking gap set, calculate the estimated clearing cost, which consists of two parts: movement cost and pushing effort cost. (1) Movement Cost (corresponding to the first consumption mentioned above): Calculate the path length or time cost required for the robot team to navigate from the current position to the operation position of the gap (i.e., the edges of the obstacles at both ends of the gap). (2) Pushing Effort Cost (corresponding to the work consumption mentioned above): As a core physical consideration indicator, it is used to estimate the physical work required to widen the gap. For any candidate blocking gap consisting of two obstacles, first calculate the widening distance required for the gap to reach the width threshold W based on the geometric information. Then, obtain the physical properties of the obstacles on both sides of the gap (including mass and ground friction coefficient), estimate the physical work required to push the left obstacle and the right obstacle respectively (i.e., the product of mass, ground friction coefficient and widening distance), and take the minimum of the two as the final pushing effort cost of the gap.

[0148] B3. Heuristic Distance (corresponding to the second consumption mentioned above) Estimation: In order to evaluate the contribution of opening a gap to the global path connectivity, "virtual penetration" and recursive detection operations are performed on each candidate blocking gap to calculate the topological shortest path distance from the gap location to the global target point. The specific calculation logic is as follows: (1) Virtual node access: First, calculate the outer offset point of the current gap (combined with the small offset position generated by the direction of the gap toward the target point), and access it as a temporary node into the WCCG ​​graph. (2) Iterative blocking detection: Use the offset point as a new starting point to emit a ray to the target point. If the ray reaches the target point without collision, the heuristic distance is the straight-line distance from the offset point to the target point. If the ray is blocked again by the outer new obstacle boundary (second layer topological ring), then identify all passable next-level candidate blocking gaps on the new boundary. (3) Heuristic distance calculation: In the topological structure of WCCG, the shortest path search algorithm (such as Dijkstra) is used for recursive evaluation. This involves assuming all gaps are passable (constraint relaxation), calculating the total length of the geometric path from the current gap, through the optimal gap in the next ring, and through subsequent levels, to finally reach the target point. This total length is the heuristic distance. By recursively probing the geometric location of gaps at each level, priority is given to "critical" obstacles that, once opened, would significantly shorten the remaining path and have no dead ends.

[0149] B4. Priority List Generation (corresponding to the priority queue above): A weighted cost function is introduced to comprehensively evaluate each candidate obstacle gap. This function contains four core weight parameters: movement cost weight (default 1.0), push duration weight (default 1.0), physical resistance weight (default 1.0), and heuristic weight (default 1.0). Among them, the push duration weight and physical resistance weight are weights applied to the push effort cost, and there is a product coupling relationship between the two. The physical resistance weight participates in constructing a threshold-based penalty multiplier. Specifically, the ratio of the maximum static friction of the obstacle to the robot team's push limit is calculated, and the larger value between this ratio and 1.0 is selected as the final physical resistance weight. This mechanism means that when the obstacle is light (ratio not greater than 1), only the basic pushing effort cost (i.e., the product of pushing time and the amount of pushing force required to push the obstacle) is calculated. However, when the obstacle is too heavy, causing the ratio to exceed 1, this ratio is used as a heuristic weight. That is, the ratio is multiplied by the product of the pushing time and the amount of pushing force required to push the obstacle, resulting in a pushing effort cost that is magnified by the ratio. This makes the cost of clearing high-friction heavy obstacles significantly higher than that of light obstacles. The calculated movement cost, pushing effort cost, and heuristic distance estimate are multiplied by their respective weight parameters and summed to obtain a comprehensive score for each candidate gap (corresponding to the total cost mentioned above). Finally, a gap priority list is generated in order of comprehensive score from low to high (i.e., the lower the cost, the higher the priority). This list is directly input into the module of subsequent step C, guiding the module of step C to prioritize the gap with the lowest comprehensive score as the best breakthrough point.

[0150] Step C is the hybrid search phase based on physical perception. After gap sorting is completed, the core search and planning phase begins. For example... Figure 6 As shown, step C receives the gap priority list output from step B and performs a path search. If the search is successful, it proceeds to step D; otherwise, it triggers replanning. The specific implementation employs a state-space tree search method combined with physical simulation, such as... Figure 7 As shown, the following steps should be performed during implementation:

[0151] C1. Search Tree Initialization: Construct a state space search tree and use the current initial physical environment state (corresponding to the initial environment description above, including key kinematic information such as the position coordinates, orientation, instantaneous linear velocity vector, and instantaneous angular velocity of all obstacles and robots) as the root node of the tree. The search node is a complex data structure that encapsulates a complete physical environment snapshot and historical action sequence. Specifically, each search node (corresponding to the data object above) stores the following core information: (1) Full physical state: contains the above key kinematic information of all obstacles and robots in the scene. This enables the device executing the path planning method to use the "state recovery" function of the physics engine to "roll back" the simulation environment to the moment recorded by the node at any time, thereby supporting the continued dynamic deduction from any intermediate state. (2) Cumulative push task chain (corresponding to the cumulative task chain above): records a series of cooperative push instructions executed from the root node to the current state, which constitute the current cleanup scheme. (3) Comprehensive score data: contains the cumulative execution cost g from the root node and the estimated remaining cost h to the target. Here, the execution cost *g* represents the actual physical cost accumulated from the initial state to the current search state (including movement cost and pushing effort cost); the estimated remaining cost *h* is directly calculated by calling the algorithm module defined in step B3 above, that is, constructing a WCCG ​​graph based on the physical environment snapshot of the current node, and performing virtual penetration and topological shortest path search in step B3, using the calculated optimal topological path cost (i.e., the length of the shortest path) from the current state to the global target point as the value of *h*. This mechanism uses lightweight topological search to provide global guidance for the heavy dynamic search, thereby accelerating convergence.

[0152] At the same time, such as Figure 7 As shown on the right, a "priority queue" is established. This queue is used to store all search nodes to be expanded and is sorted in real time according to the comprehensive score of each node f = g + h, ensuring that the node with the lowest cost and the most promising path to the target point is popped and processed first at each step.

[0153] C2. Batch generation of node expansion and migration tasks: such as... Figure 7 As shown in the flowchart, the node with the highest score is popped from the priority queue as the current parent node (in the first round of search, this parent node is the root node of the initial environment). Then, step B is called to perform a full analysis of the environment snapshot recorded by this parent node, identify all feasible gaps around the current topological connectivity domain, and calculate the estimated remaining cost (h value) in this state.

[0154] Based on the priority list output in step B, corresponding collaborative pushing tasks are generated in batches for each candidate blocking gap in the list. The pushing task encapsulates the motion instructions for the obstacle corresponding to the gap, specifically including the following core parameters: (1) Target obstacle (corresponding to the object to be pushed above): specifies the obstacle to be moved. (2) Target velocity vector: includes pushing direction, linear velocity and angular velocity. (3) Pushing duration (corresponding to the pushing duration above): estimates the execution time required to complete the gap expansion.

[0155] The specific generation logic of the pushing task is as follows: (1) Pushing direction determination: Calculate the normal vector of the obstacle surface that constitutes the gap, and lock the direction of "expanding outward along the normal" as the basic pushing direction to ensure that the pushing operation can directly increase the gap width. (2) Velocity vector synthesis: Read the robot's preset pushing rate (e.g., 0.1 m / s) in the configuration file, combine it with the pushing direction, and synthesize a constant target linear velocity vector; the angular velocity is set to 0 by default (i.e., translation mode). (3) Pushing duration calculation: Calculate the difference between the current gap width and the width threshold W (i.e., the required expansion width), divide it by the preset pushing rate, and thus obtain the pushing duration.

[0156] This step transforms the high-level topology connectivity requirements into a list of specific migration tasks with clear kinematic parameters, and then matches the execution mode of each migration task to the next stage.

[0157] C3. Push Pattern Sampling and Pattern Table Acceleration: For each push task generated in step C2 (i.e., given the target obstacle and target velocity vector), the push pattern sampling stage is entered. This stage aims to match the most feasible robot execution scheme (i.e., push pattern) for the push task. In this stage, the optimal combination of contact points of multiple robots on the obstacle surface is determined.

[0158] The method for determining the contact point (corresponding to the contact position in the above text) in the push mode is as follows: (1) Desired generalized force mapping: Receive the target velocity vector passed in step C2, and use the physical model (such as the friction balance model under the quasi-static assumption) to solve the ideal resultant external force and resultant torque required to achieve the motion, i.e., the target generalized force vector. (2) Candidate contact point generation and initial screening: Traverse all reachable contact point sets of robots and perform fast geometric screening (e.g., remove the points where the generated contact force is opposite to the direction of the target velocity vector, i.e., do negative work). (3) Force vector matching based on greedy strategy: During the initial allocation, for each robot, calculate the generalized force (including torque) generated when it applies the maximum thrust at each candidate contact point, and select the contact point with the highest cosine similarity in direction to the target generalized force vector (i.e., the most conforming to the target motion direction) as the initial solution. In order to further reduce the deviation between the actual resultant force and the target resultant force, an iterative optimization strategy is adopted. This involves calculating the residual vector between the resultant force of multiple robots and the target generalized force, and then attempting to adjust the contact points between each robot and the obstacle one by one to find a new combination of contact points that can minimize the residual and improve the overall force vector matching. This process is repeated until the residual converges or the upper limit of iteration is reached.

[0159] To improve sampling efficiency, a pre-set pattern table is first queried. This pattern table stores successful pushing experiences from historical tasks. If the geometry and pushing direction of the obstacle to be pushed are the same as those recorded in the historical patterns, the pushing force (including direction and magnitude) and contact position from these successful pushing patterns will be reused first, thereby accelerating sampling.

[0160] Through steps C1-C3 above, parent node popping and batch task generation are achieved. Furthermore, by querying historical successful experiences in the pattern table during the push-mode sampling process, sampling can be accelerated. After determining the push-mode (contact point, thrust vector), the determined push-mode is input into the parallel simulation evaluation module for parallel physical simulation verification. Parallel physical simulation verification includes steps C4-C6:

[0161] C4. Rapid Geometric Collision Screening: First, a rapid geometric collision detection is performed. This checks whether the robot will directly collide with walls or other immovable obstacles on its path. If a collision occurs, the action is immediately rejected and not included in subsequent time-consuming physics calculations.

[0162] C5. Dynamic Integral Simulation: The process proceeds to the dynamic simulation stage after the selected actions have passed the screening. The current physical state snapshot and the generated push command are input into the physics engine (such as PyBullet), which uses its built-in integrator to perform forward inference at discrete time steps. During this process, the dynamic equations are not directly solved. Instead, the physics engine's underlying calculations of object mass, ground friction coefficient, and multi-body contact constraints are used to directly obtain the state evolution of the obstacle after the push is applied (i.e., the position, attitude, and whether a collision occurred at the end of the inference), serving as the basis for evaluating the effectiveness of the push mode.

[0163] C6. Validity Verification: After the simulation ends, the output physical state snapshot is validated in multiple dimensions. The check includes: (1) Displacement compliance: Verify whether the obstacle has undergone the expected displacement and exclude the case of "pushing but not moving"; (2) Contact retention judgment: Calculate the Euclidean distance between the robot end effector and the contact point on the obstacle surface at the end of the simulation. If the distance is less than the preset physical contact tolerance (e.g., 0.05 meters) and the angle deviation between the actual displacement vector of the obstacle and the direction of the thrust is within the allowable range, it is determined that effective contact has been maintained during the pushing process. Otherwise, it is determined to be disengaged or slipping; (3) Deadlock detection: Check whether the total kinetic energy has abnormally decayed to zero under the continuous application of the thrust to identify the case of the object being stuck by the environment.

[0164] C7. State Update and Multi-Branch Expansion: Based on the evaluation results of the parallel physics simulation, perform state updates and grow the search tree. For example... Figure 7 As shown, after a parent node is popped, it generates multiple different pushing tasks (corresponding to different notches or different thrust directions). After the pushing patterns of these multiple pushing tasks are sampled, the candidate actions corresponding to all pushing patterns are input into the parallel simulation evaluation module. Following steps C4-C6 above, geometric rapid screening, dynamic integral simulation, and validity verification are performed respectively. If the verification is successful, a valid branch is generated, resulting in multiple parallel child node branches. If the verification fails, no corresponding child node is generated, and the process returns to the parent node popping step.

[0165] The specific processing logic is as follows: (1) Valid branch generation (generating child nodes): For each shift pattern that has passed the validity check (displacement compliance, contact retention and no deadlock), based on the simulation end state output by the physics engine (corresponding to the post-shift environment description above), a new search node (child node) is instantiated. The cumulative cost g of the child node (the cumulative cost g of the parent node + the shift cost) and the new estimated remaining cost h are calculated to obtain the comprehensive score f = g + h. Subsequently, the child node is pushed into the priority queue and becomes a candidate subsequent path point. This means that if the parent node also tries other feasible gaps (alternative gaps), these alternative solutions will generate sibling nodes, which will coexist in the queue with the child node of the preferred solution, waiting to be selected. (2) Invalid branch pruning: For shift patterns that fail the check (such as shift failure, collision or deadlock), the specific action branch is judged to be invalid and directly discarded, and no corresponding child node will be generated. (3) Parent node removal: After the parent node has completed the simulation and branch generation of all the potential tasks mentioned above, the parent node is considered "extended" and is permanently removed from the priority queue. The search process then turns to the next node with the best overall score in the priority queue, that is, it returns to the node with the best score that was popped from the priority queue in step C1 as the current parent node, and continues the next round of loop.

[0166] C8. Whenever a new child node is generated, its corresponding environment state is checked to see if it meets the connectivity requirements (i.e., a new WCCG ​​graph is constructed to determine if the starting point to the ending point is connected). If it does, it means a feasible cleanup path has been found, the search terminates, and the complete push task sequence from the root node to the child node is output; otherwise, the search continues until a solution is found or the priority queue is empty.

[0167] Once a feasible traversal strategy is successfully found in step C, a collision-free reference trajectory (corresponding to the original traversal trajectory mentioned above) is generated, and the process proceeds to step D. The core task at this stage is to translate the planned abstract task into concrete robot actions and to address uncertainties during execution in real time. For example... Figure 8 As shown, the following steps should be performed during implementation:

[0168] D1. State Machine Initialization: Start a "hybrid controller" that maintains a finite state machine internally. This state machine defines four core states for robot operation: INIT, TRANSITION, PUSHING, and TERMINATED.

[0169] D2. Task Sequence Parsing: In the "Initialization State," the hybrid controller receives the reference trajectory from step C. This trajectory is parsed to extract the target obstacle to be pushed at each step, the pushing mode used, and the cooperative configuration of multiple robots. Next, the precise position and orientation that each robot must reach before starting to push are calculated, defined as the "positioning posture."

[0170] D3. State Transition Logic: After the reference trajectory is analyzed, the system first enters a "transition state," where the hybrid controller directs all robots to move from their current positions to their respective contact points. Once it is confirmed that all robots have accurately reached their contact points and are ready (i.e., reached their positioning posture), the state machine automatically transitions to the "pushing state" and begins coordinated pushing. If the task is successfully completed (obstacles reach the target area) or an unrecoverable error is encountered (such as task timeout or drastic environmental changes leading to an unsolvable problem), the state machine eventually transitions to the "termination state." If slippage or excessive deviation occurs during pushing (the actual pushing trajectory deviates significantly from the original pushing trajectory), the system returns to the transition state, at which point the robot proceeds to a newly determined contact point.

[0171] D4. Transition Phase (Navigation and Conflict Resolution): (1) Independent Path Planning: In the transition state, the Transition Controller is activated to independently plan a collision-free path from the current position to the in-position posture for each robot. (2) Multi-robot Trajectory Conflict Detection and Resolution: During execution, the future trajectories of multiple robots are predicted periodically (e.g., every 0.2 seconds). If the reference paths of two robots are detected to have spatial intersection or potential collision risk (i.e., the distance is less than the safety threshold and the angle between the motion directions shows a collision), the conflict resolution mechanism is triggered. (3) Target Exchange Strategy: When the conflict resolution mechanism is triggered, the target contact points of the two robots that are in conflict are directly exchanged, thereby avoiding complex detours or deadlocks and improving arrival efficiency.

[0172] D5. Shifting Phase (Cooperative Manipulation):

[0173] (1) Trajectory tracking control: In the push state, the push controller is activated. Each robot maintains contact with the obstacle according to the real-time guidance of the reference trajectory and applies the planned force or speed. (2) Timeout forced switching: Set the maximum transition time threshold. If the robot fails to complete the transition phase (i.e., not fully in place) within the specified time, the abnormal handling is triggered.

[0174] D6. Progression and Termination Judgment: Real-time monitoring of task progress. If the obstacle successfully reaches the predetermined target area, or if the movement time expires and the object has not moved for an extended period (the historical position change is calculated via a sliding window; if it falls below the set deadlock threshold, it is considered deadlocked), a command is issued to end the current control loop, and the task is completed. Simultaneously, if slippage occurs during movement or the actual trajectory deviates significantly from the planned trajectory (i.e., the Euclidean distance and angle deviation between the object's current state and the nearest point on the reference trajectory are calculated in real-time; if this combined deviation exceeds the preset tracking error threshold), a tiered "replanning" mechanism is triggered.

[0175] First, a lightweight local replanning approach is attempted: taking the current actual position of the obstacle as the new starting point (corresponding to the local starting point above) and the original local target point as the ending point (corresponding to the local ending point above), a transition trajectory is regenerated using the "constant screw interpolation algorithm." Specifically, assuming the object maintains a constant forward speed and rotational angular velocity in its own coordinate system, a circular arc path that smoothly connects the current position and the target point is directly solved through geometric analytical calculations (automatically degenerating into a straight line path if no rotation is required), and the corresponding speed control parameters are obtained. Based on this, the current task instructions are updated, enabling the robot to smoothly "correct" the object back to the predetermined trajectory without changing the contact mode (i.e., without needing to readjust the formation or release the object).

[0176] If the number of local replanning failures exceeds the limit or the deviation is too large to be repaired, the process will revert to step C to trigger global replanning. At this point, the search tree will be rebuilt based on the latest environmental state, and a new optimal cleanup strategy will be searched.

[0177] Below, in conjunction with Figure 9 and Figure 10 This section introduces a specific cleaning scenario.

[0178] like Figure 9 The image shows a complex passage clearing scenario containing various geometric obstacles (such as circles, ellipses, L-shaped polygons, triangles, and rectangles). The specific execution process of the path planning method in this scenario is as follows:

[0179] like Figure 9As shown, the initial state of the environment is first obtained. The circular area marked "S" in the upper left corner represents the starting point of the large vehicle's task, and the circular area marked "G" in the lower right corner represents the target point. Currently, the straight path from S to G is completely blocked by obstacles scattered throughout the scene. These obstacles include elliptical obstacles labeled 1 and 4, an L-shaped polygonal obstacle labeled 2, a triangular obstacle labeled 3, and rectangular obstacles labeled 5 and 6. The two square icons in the upper left corner represent the two robots responsible for the cleanup task. Based on the obtained initial state, a connectivity graph with a gap of width W is constructed at the initial moment. At this stage, it is identified that the topologically connected domain of the starting point S is surrounded by obstacles 1 and 2, as well as the long strip obstacle on the left, and cannot directly extend to the target point G; therefore, the initial state is determined to be disconnected.

[0180] Next, step B is performed to identify gaps and assess physical costs. A search is conducted along the topological boundary of the current starting area to identify potential gaps extending to the lower right of the obstructing path. In this scenario, the costs of moving different obstacles are compared: although obstacle 2 (L-shaped) appears to be a direct obstruction geometrically, physical property analysis reveals that its irregular shape and large moment of inertia make direct movement difficult. In contrast, the adjacent obstacle 1 (ellipse), although larger in volume, has smooth edges that make it more prone to rotational sliding under force, and clearing it significantly widens the entrance space of the left passage. Therefore, based on the physical cost assessment, a priority list of gaps for prioritizing obstacle 1 is generated.

[0181] Next, the tree search process in step C begins, searching for specific pushing strategies. Parallel physical simulation is used to verify candidate actions, and a multi-stage, multi-objective clearing strategy is planned. In the first stage, to break through the left-side blockade, two robots work together to apply a specific torque to obstacle 1 (ellipse), causing it to rotate while translating to the left, ultimately leaning against the left edge, thus opening the initial downward passage from point S. In the second stage, to clear the bottom corner, a pushing command is generated for obstacle 2 (L-shaped), utilizing its L-shaped structure's mechanical properties to rotate and push it to the lower left corner, completely clearing the complex structural obstruction at the passage entrance. In the third stage, to expand the central space, obstacle 3 (triangle) is planned to be pushed directly to the upper left corner, pushing it into the corner. In the fourth stage, obstacles 5 (slanted rectangle) and 6 (horizontal rectangle) at the end of the passage are moved. First, the right side of obstacle 5 is moved upward, so that the whole thing is moved upward to make room in the center area. Finally, obstacle 6 is moved slightly upward to ensure that the width of the end of the passage meets the passage standards for large vehicles.

[0182] Finally, step D proceeds to online execution and closed-loop control. The two robots execute the task according to the generated reference trajectory, precisely pushing obstacles 1, 2, 3, 5, and 6 sequentially, strictly following the planned order. Throughout the execution process, the hybrid controller monitors the pose deviations of the obstacles in real time to ensure that the actual pushing effect matches the simulation prediction. Ultimately, as... Figure 10 As shown, the complex obstacle layout was successfully cleared, and all key obstructions were moved to the sides of the passage or to dead ends, establishing a passable path connecting the starting point S and the target point G. This verifies the technical advantages of this scheme in using physical simulation to predict the motion trend of objects and guide multi-stage topology expansion.

[0183] This solution employs a "Physics-Informed Hybrid Search" architecture, achieving tight coupling between macro-level task planning (i.e., obstacle selection decision-making) and micro-level motion generation (i.e., robot movement control). By constructing a W-width gap connectivity graph, it abandons the traditional gridded environment modeling approach and directly connects all potential gaps in the environment that satisfy the passage width W of large vehicles based on geometric topological relationships, thereby quickly identifying and locating bottleneck areas obstructing passage. By adopting a gap sorting strategy, when evaluating the priority of clearing, it considers not only geometric distance but also introduces a "physical work" cost function (covering obstacle mass, frictional resistance, and ease of movement), which can prioritize the obstacles with the lowest physical energy consumption and the fastest path opening as clearing targets. In the motion planning stage, a parallel physical simulation verification mechanism (Sim-in-the-loop) is embedded. Unlike traditional geometric path planning, this solution inputs the generated candidate movement strategies (including contact points, force vectors, etc.) into the physics engine for dynamic integral simulation to predict the existence of slippage, deadlock, or motion failure risks. Only when the simulation results are verified will the corresponding actions be adopted into the final planning scheme, thus ensuring the physical feasibility of the planning results.

[0184] By embedding a parallel physical simulation mechanism into the planning loop, the feasibility and success rate of cooperative traversal operations in real physical environments are significantly improved, thereby increasing the accuracy of the final passable paths generated for large vehicles. Through the simulation mechanism, the chain traversal effect between objects is accurately predicted and utilized, realizing the function of clearing complex stacked channels by using the interaction between objects, which can effectively handle the multi-object coupled dynamics problem in dense and chaotic environments. At the same time, by combining the efficient geometric pruning of the W-width gap connectivity graph, the historical pattern table reuse mechanism, and the parallel physical simulation mechanism, the computational efficiency is greatly improved while ensuring physical fidelity, effectively solving the problem of excessive computational cost caused by the introduction of physical simulation, and realizing rapid response and efficient expansion in large-scale complex obstacle scenarios.

[0185] Corresponding to the above method embodiments, such as Figure 11 As shown in the figure, this application embodiment also provides a path planning device, the device comprising:

[0186] The gap determination module 1110 is used to determine, based on the current environment description, a candidate blocking gap that would prevent the task execution device from reaching the end point if there is no passable path with a width equal to the passage width of the task execution device between the start and end points of the task to be executed; the candidate blocking gap is a channel between two obstacles with a width less than the passage width.

[0187] Simulation module 1120 is used to determine the obstacle to be moved, the moving direction and the moving time of the candidate blocking gap. Based on the mass of the obstacle to be moved, the ground friction coefficient in the current environment description and the multi-body contact constraints between obstacles, the simulation robot moves the obstacle to be moved according to the moving direction and moving time to obtain the post-moving environment description corresponding to the candidate blocking gap.

[0188] The calculation module 1130 is used to obtain the work consumption and movement consumption of the robot in pushing the obstacle if there is a feasible gap in the candidate blocking gap where the width between the two obstacles reaches the passage width after the simulated pushing; and to obtain the total consumption based on the movement consumption and work consumption.

[0189] The connectivity detection module 1140 is used to detect whether the task execution device can reach the destination from the starting point based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption.

[0190] The generation module 1150 is used to generate a traversable path between the start and end points that passes through the target feasible gap, if reachable.

[0191] Return module 1160, otherwise, update the current environment description to the shifted environment description corresponding to the target feasible gap, and return the steps for determining the candidate blocking gap for the blocking task execution device to reach the destination based on the current environment description.

[0192] Optionally, the simulation module 1120 includes:

[0193] The first determining submodule is used to determine the robot's movement action within a preset range of the position of the obstacle to be moved in the current environment description, based on the robot's position in the current environment description, and to determine the robot's pushing action of moving the obstacle to be moved according to the pushing direction and pushing duration.

[0194] The first simulation submodule is used to simulate the robot performing movement and pushing actions to obtain the post-pushing environment description corresponding to the candidate blocking gap;

[0195] The computing module 1130 includes:

[0196] The task chain generation submodule is used to add the simulated actions that the robot performs from the current environment description to the shifted environment description corresponding to the feasible gap to the end of the cumulative task chain corresponding to the current environment description, so as to obtain the first cumulative task chain corresponding to the shifted environment description corresponding to the feasible gap; wherein, the cumulative task chain corresponding to any environment description records the sequence of simulated actions performed by the robot from the initial environment description to that environment description.

[0197] The first calculation submodule calculates the work consumption for moving the obstacle to be moved based on the obstacle to be moved and the moving time recorded in the first cumulative task chain.

[0198] The second calculation submodule is used to calculate the first cost of the robot performing the movement action recorded in the first cumulative task chain, and to estimate the second cost of the robot from the feasible gap to the endpoint based on the post-shift environment description corresponding to the feasible gap, and to calculate the movement cost based on the first cost and the second cost.

[0199] Optionally, the first computing submodule includes:

[0200] The first calculation unit is used to calculate the product of the ratio of the maximum static friction force of the obstacle to be pushed recorded in the first cumulative task chain to the maximum thrust of the robot, and the push duration of the push action and the magnitude of the thrust required by the obstacle to be pushed, to obtain the third consumption.

[0201] The second calculation unit is used to calculate the product of the pushing time indicated by the pushing action and the amount of pushing force required to push the obstacle if the ratio is not greater than 1, and obtain the fourth consumption.

[0202] The third calculation unit is used to calculate the sum of the third and fourth consumptions to obtain the work consumption for moving the obstacle to be moved.

[0203] Optionally, the device further includes:

[0204] The storage module is used to sort the data objects of the post-shifted environment descriptions corresponding to the feasible gaps in ascending order of total consumption and store them in a priority queue before the connectivity detection module 1140 performs the step of detecting whether the task execution device can reach the destination from the starting point based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption. The data objects of the post-shifted environment descriptions corresponding to the feasible gaps store the post-shifted environment descriptions corresponding to the feasible gaps, the total consumption, and the sequence of simulated actions executed by the robot from the initial environment description to the post-shifted environment descriptions corresponding to the feasible gaps.

[0205] The update module is used to generate a child node of a specified node in the search tree based on the data object that ranks first in the priority queue; wherein the specified node is the node corresponding to the current environment description, and the root node of the search tree is the node corresponding to the initial environment description.

[0206] The connectivity detection module 1140 is specifically used to: detect whether the task execution device can reach the destination from the starting point based on the post-shift environment description recorded in the latest generated child node;

[0207] The generation module 1150 is specifically used to: generate a passable path for all nodes corresponding to feasible gaps on the branch from the root node in the current search tree to the latest generated child node.

[0208] Optionally, the device further includes:

[0209] The first sending module is used to send the action sequence recorded by the target task chain to the robot so that the robot can perform the operation according to the received action sequence; the target task chain is: the cumulative task chain corresponding to the shifted environmental description corresponding to the target feasible gap.

[0210] The first execution module is used to execute the step of generating a passable path between the starting point and the end point through the target feasible gap if the robot performs its work according to the received action sequence and all the obstacles to be pushed in each pushing action instruction recorded in the target task chain reach the expected pushing position.

[0211] Optionally, the device further includes:

[0212] The local replanning module is used to regenerate the pushing trajectory between the local starting point and the local ending point if the actual pushing trajectory of the obstacle to be pushed deviates from the original pushing trajectory during the operation of the robot. The module uses the current position of the obstacle to be pushed as the local starting point and the original target position of the obstacle to be pushed as the local ending point. The module then redetermines the pushing action that the robot needs to perform based on the newly generated pushing trajectory.

[0213] The second sending module is used to send the latest determined pushing action to the robot, so that the robot can push the obstacle to be pushed according to the latest received pushing action.

[0214] Optionally, the device further includes:

[0215] The global replanning module is used to reacquire the current environment description and return the steps of determining the candidate blocking gaps for the blocking task execution device to reach the destination based on the current environment description if the actual pushing trajectory deviates from the original pushing trajectory a predetermined number of times during the pushing process of any obstacle to be pushed.

[0216] Optionally, the simulation module 1120 includes:

[0217] The second determining submodule is used to determine the contact position between the robot and the obstacle to be moved and the robot's thrust based on the shape and direction of the obstacle to be moved.

[0218] The second simulation submodule is used to simulate pushing the obstacle to be pushed based on the mass of the obstacle to be pushed, the ground friction coefficient in the current environment description, and the multi-body contact constraints between obstacles, and to apply the pushing force for the duration of continuous pushing at the contact position by the robot, so as to obtain the post-pushing environment description corresponding to the candidate blocking gap.

[0219] Optionally, the second determining submodule includes:

[0220] The query unit is used to query the target pattern that records the shape and direction of the obstacle to be moved from the historical pattern; the historical pattern records the shape, direction, contact position with the obstacle, and thrust used by the robot in successfully moving obstacles.

[0221] The determination unit is used to determine the contact position between the robot and the obstacle to be moved and the robot's thrust based on the thrust and contact position recorded in the target pattern.

[0222] Optionally, the device further includes:

[0223] The collision detection module is used to detect, based on the current environment description, whether the obstacle to be pushed will collide with an immovable obstacle during the pushing time in the pushing direction;

[0224] The second execution module is used to execute the steps in the simulation module 1120, which simulate the robot pushing the obstacle to be pushed based on the mass of the obstacle to be pushed, the ground friction coefficient in the current environment description, and the multi-body contact constraints between obstacles, according to the pushing direction and pushing time, if no collision occurs.

[0225] Optionally, the gap determination module 1110 includes:

[0226] The detection submodule is used to detect whether the line-of-sight vector from the starting point to the ending point intersects with the width connectivity graph corresponding to the current environment description. The width connectivity graph includes a first edge and a second edge. The first edge is the edge between bridging nodes on any two convex components, and the second edge is the edge between a bridging node on any convex component and the center node on that convex component. The bridging nodes on two convex components are the closest points on the two convex components whose distance is less than the passage width. The convex components include convex obstacles in the current environment description, as well as convex obstacles obtained by decomposing non-convex obstacles.

[0227] The detour submodule is used to, if there is an intersection, take the first intersecting edge on the width connected graph as the starting blocking edge, and use the detour search algorithm to search for a detour path on the width connected graph to determine whether there is a closed loop topology between the starting point and the ending point.

[0228] The third determining submodule is used to determine the first edge on the closed-loop topology as a candidate blocking gap that blocks the task execution device from reaching the destination if the closed-loop topology exists and the number of intersections between the line-of-sight vector and the closed-loop topology is odd.

[0229] This application also provides an electronic device, such as... Figure 12 As shown, it includes a processor 1201, a communication interface 1202, a memory 1203, and a communication bus 1204, wherein the processor 1201, the communication interface 1202, and the memory 1203 communicate with each other through the communication bus 1204.

[0230] Memory 1203 is used to store computer programs;

[0231] The processor 1201 is used to implement the steps of any of the above path planning methods when executing the program stored in the memory 1203.

[0232] The communication bus mentioned in the above electronic devices can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. This communication bus can be divided into address bus, data bus, control bus, etc. For ease of illustration, only one thick line is used to represent it in the diagram, but this does not mean that there is only one bus or one type of bus.

[0233] The communication interface is used for communication between the aforementioned electronic devices and other devices.

[0234] The memory may include random access memory (RAM) or non-volatile memory (NVM), such as at least one disk storage device. Optionally, the memory may also be at least one storage device located remotely from the aforementioned processor.

[0235] The processors mentioned above can be general-purpose processors, including central processing units (CPUs), network processors (NPs), etc.; they can also be digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components.

[0236] In another embodiment of the present invention, a computer-readable storage medium is also provided, wherein a computer program is stored therein, and when the computer program is executed by a processor, it implements the steps of any of the path planning methods described above.

[0237] In another embodiment of the present invention, a computer program product containing instructions is also provided, which, when run on a computer, causes the computer to execute any of the path planning methods described in the above embodiments.

[0238] In the above embodiments, implementation can be achieved entirely or partially through software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented entirely or partially as a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of this application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., coaxial cable, fiber optic, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid state disk (SSD)).

[0239] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0240] The various embodiments in this specification are described in a related manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, the embodiments of apparatus, electronic devices, and computer-readable storage media are basically similar to the method embodiments, and therefore the descriptions are relatively simple; relevant parts can be referred to the descriptions of the method embodiments.

[0241] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention are included within the scope of protection of the present invention.

Claims

1. A path planning method, characterized in that, The method includes: If there is no passable path between the start and end points of the task to be performed that is wider than the passage width of the task execution device, a candidate blocking gap is determined based on the current environment description to prevent the task execution device from reaching the end point; the candidate blocking gap is a channel between two obstacles whose width is less than the passage width. The obstacles to be moved, the direction of movement, and the duration of movement for the candidate blocking gaps are determined. Based on the mass of the obstacles to be moved, the ground friction coefficient in the current environment description, and the multi-body contact constraints between the obstacles, the robot is simulated to move the obstacles to be moved according to the direction of movement and the duration of movement, so as to obtain the post-movement environment description corresponding to the candidate blocking gaps. If there is a feasible gap among the candidate blocking gaps where the width between the two obstacles reaches the passage width after the simulated pushing, the work consumption and movement consumption of the robot pushing the obstacle are obtained according to the environmental description after the pushing corresponding to the feasible gap, and the total consumption is obtained according to the movement consumption and work consumption. Based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption, detect whether the task execution device can reach the destination from the starting point; If reachable, generate a passable path between the starting point and the ending point that passes through the feasible gap in the target; Otherwise, update the current environment description to the shifted environment description corresponding to the target feasible gap, and return to the step of determining the candidate blocking gaps for the blocking task execution device to reach the destination based on the current environment description.

2. The method according to claim 1, characterized in that, The simulation robot moves the obstacle to be moved according to the moving direction and moving time, and obtains the post-movement environment description corresponding to the candidate blocking gap, including: Based on the robot's position in the current environment description, determine the robot's movement action within a preset range to move the obstacle to be moved in the current environment description, and determine the robot's pushing action to move the obstacle to be moved according to the pushing direction and pushing duration; The simulated robot performs movement and pushing actions to obtain the post-pushing environment description corresponding to the candidate blocking gap; The step of obtaining the work and movement costs of the robot pushing the obstacle based on the post-movement environment description corresponding to the feasible gap includes: The actions simulated by the robot from the current environment description to the shifted environment description corresponding to the feasible gap are added to the end of the cumulative task chain corresponding to the current environment description to obtain the first cumulative task chain corresponding to the shifted environment description corresponding to the feasible gap; wherein, the cumulative task chain corresponding to any environment description records the sequence of actions simulated by the robot from the initial environment description to that environment description. Calculate the work consumption for moving the obstacle to be moved based on the pushing action instructions and the pushing time recorded in the first cumulative task chain; The first cost of the robot performing the movement actions recorded in the first cumulative task chain is calculated, and the second cost of the robot moving from the feasible gap to the endpoint is estimated based on the post-shift environment description corresponding to the feasible gap. The movement cost is calculated based on the first cost and the second cost.

3. The method according to claim 2, characterized in that, The step of calculating the work consumption for moving the obstacle to be moved based on the moving action instruction and moving duration recorded in the first cumulative task chain includes: If the ratio of the maximum static friction of the obstacle to be pushed by the push action instruction recorded in the first cumulative task chain to the maximum thrust of the robot is greater than 1, then the product of the ratio, the pushing time of the push action instruction, and the thrust required by the obstacle to be pushed is calculated to obtain the third consumption. If the ratio is not greater than 1, the product of the pushing time indicated by the pushing action and the amount of pushing force required to push the obstacle is calculated to obtain the fourth consumption. The sum of the calculated third and fourth consumption values ​​yields the work consumption required to move the obstacle to be moved.

4. The method according to claim 2, characterized in that, Before determining whether the task execution device can reach the endpoint from the starting point based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption, the method further includes: Sort the data objects of the post-shifted environment description corresponding to the feasible gap in order of total consumption from low to high, and store them in the priority queue. The data objects of the post-shifted environment description corresponding to the feasible gap contain the post-shifted environment description corresponding to the feasible gap, the total consumption, and the sequence of actions performed by the robot in simulation from the initial environment description to the post-shifted environment description corresponding to the feasible gap. Based on the data object that is sorted first in the priority queue, a child node of a specified node in the search tree is generated; wherein, the specified node is the node corresponding to the current environment description, and the root node of the search tree is the node corresponding to the initial environment description; The step of detecting whether the task execution device can reach the endpoint from the starting point based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption includes: Based on the post-shift environment description recorded in the latest generated child nodes, detect whether the task execution device can reach the destination from the starting point; The feasible path between the starting point and the ending point, passing through the target feasible gap, includes: Generate a passable path that passes through all nodes on the branch from the root node in the current search tree to the newly generated child node, corresponding to all feasible gaps.

5. The method according to claim 2, characterized in that, The method further includes: Send the action sequence recorded by the target task chain to the robot so that the robot can perform the operation according to the received action sequence; the target task chain is: the cumulative task chain corresponding to the shifted environmental description corresponding to the target feasible gap; If, after the robot performs its work according to the received sequence of actions, all obstacles to be moved as indicated by each pushing action recorded in the target task chain reach the expected moving position, then the step of generating a passable path between the starting point and the ending point through the target feasible gap is executed.

6. The method according to claim 5, characterized in that, The method further includes: If the actual moving trajectory of the obstacle to be moved deviates from the original moving trajectory during the operation of the robot, the moving trajectory between the local starting point and the local ending point is regenerated with the current position of the obstacle to be moved as the local starting point and the original target position of the obstacle to be moved as the local ending point. The moving action that the robot needs to perform is then re-determined based on the newly generated moving trajectory. Send the latest determined pushing action to the robot so that the robot can push the obstacle to be pushed according to the latest received pushing action.

7. The method according to claim 6, characterized in that, The method further includes: If, during the pushing process of any obstacle to be pushed, the actual pushing trajectory deviates from the original pushing trajectory a predetermined number of times, then the current environment description is reacquired, and the step of determining the candidate blocking gap for the blocking task execution device to reach the destination based on the current environment description is returned.

8. The method according to claim 1, characterized in that, The method involves simulating a robot to move an obstacle based on its mass, the ground friction coefficient in the current environmental description, and multi-body contact constraints between obstacles, according to the moving direction and duration. This yields a post-movement environmental description corresponding to the candidate blocking gap, including: Based on the shape and direction of the obstacle to be moved, determine the contact position between the robot and the obstacle and the robot's thrust. Based on the mass of the obstacle to be moved, the ground friction coefficient in the current environment description, and the multi-body contact constraints between obstacles, the robot applies the thrust for the duration specified in the contact position, and simulates the movement of the obstacle to be moved to obtain the post-movement environment description corresponding to the candidate blocking gap.

9. The method according to claim 8, characterized in that, The step of determining the contact position between the robot and the obstacle to be moved and the robot's thrust based on the shape and direction of the obstacle to be moved includes: The target pattern that records the shape and direction of the obstacle to be moved is retrieved from the historical patterns; the historical patterns record the shape, direction, contact position with the obstacle, and thrust used by the robot in successfully moving obstacles. Based on the thrust and contact position recorded in the target pattern, determine the contact position between the robot and the obstacle to be moved, and the robot's thrust.

10. The method according to claim 1, characterized in that, The method further includes: Based on the current environment description, detect whether the obstacle to be moved will collide with an immovable obstacle during the moving time in the moving direction; If no collision occurs, the steps described above are executed: based on the mass of the obstacle to be moved, the ground friction coefficient in the current environment description, and the multi-body contact constraints between obstacles, the robot simulates moving the obstacle to be moved according to the moving direction and moving time.

11. The method according to claim 1, characterized in that, The process of determining candidate obstruction gaps for the obstruction task execution device to reach its destination based on the current environment description includes: Detect whether the line-of-sight vector from the starting point to the ending point intersects with the width-connected graph corresponding to the current environment description; wherein, the width-connected graph includes a first edge and a second edge, the first edge being the edge between bridging nodes on any two convex components, and the second edge being the edge between a bridging node on any convex component and the center node on that convex component; the bridging nodes on two convex components are the closest points on the two convex components whose distance is less than the passage width; the convex components include convex-shaped obstacles in the current environment description, as well as convex-shaped obstacles obtained after decomposing non-convex-shaped obstacles; If they intersect, the first intersecting edge on the width-connected graph is taken as the starting blocking edge, and a detour search algorithm is used to search for detour paths on the width-connected graph to determine whether there is a closed loop topology between the starting point and the ending point. If the closed-loop topology exists and the number of intersections between the line-of-sight vector and the closed-loop topology is odd, then the first edge on the closed-loop topology is identified as a candidate blocking gap that would prevent the task execution device from reaching the destination.

12. A path planning device, characterized in that, The device includes: The gap determination module is used to determine candidate blocking gaps that would prevent the task execution device from reaching the destination if there is no passable path with a width equal to the passage width of the task execution device between the start and end points of the task to be executed; the candidate blocking gaps are channels with a width between two obstacles that is less than the passage width. The simulation module is used to determine the obstacle to be moved, the direction of movement, and the duration of movement for the candidate blocking gap. Based on the mass of the obstacle to be moved, the ground friction coefficient in the current environment description, and the multi-body contact constraints between obstacles, the simulation robot moves the obstacle to be moved according to the direction of movement and the duration of movement, and obtains the post-movement environment description corresponding to the candidate blocking gap. The calculation module is used to obtain the work consumption and movement consumption of the robot in pushing the obstacle if there is a feasible gap in the candidate blocking gaps where the width between the two obstacles reaches the passage width after the simulated pushing; and to obtain the total consumption based on the movement consumption and work consumption. The connectivity detection module is used to detect whether the task execution device can reach the destination from the starting point, based on the shifted environment description corresponding to the target feasible gap with the lowest total consumption. The generation module is used to generate a traversable path between the start and end points that passes through the target feasible gap, if reachable. The return module is used otherwise to update the current environment description to the shifted environment description corresponding to the target feasible gap, and return the steps for determining the candidate blocking gaps for the blocking task execution device to reach the destination based on the current environment description.

13. An electronic device, characterized in that, It includes a processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory communicate with each other through the communication bus; Memory, used to store computer programs; A processor, when executing a program stored in memory, implements the steps of the method described in any one of claims 1-11.

14. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the steps of the method described in any one of claims 1-11.

Images