Path planning method and related equipment

Abstract

The embodiment of the invention discloses a path planning method and related equipment. The method provided by the embodiment of the invention comprises the following steps: generating a global path by using a path search algorithm according to road sign points in a working area of first equipment; dividing the global path into a plurality of local paths; acquiring environment sensing information and a shared path track of second equipment which establishes communication connection with the first equipment; optimizing the path trajectory of the local path based on the environmental perception information and the shared path trajectory; and combining the local paths to obtain a final target planning path. According to the embodiment of the invention, direct communication among multiple devices is realized through short-distance communication, decentralized communication is realized in a multi-device working scene, the problem of single-point failure of a fault of a central server is avoided, and the system deployment and maintenance cost is reduced. Static obstacles are considered in path planning of each device, changes of dynamic obstacles such as the second device can be responded in real time, and real-time performance and accuracy of path planning are ensured.

Description

Technical Field

[0001] The embodiments of the present application relate to the field of path search technology, and in particular to a path planning method and related equipment. Background Art

[0002] With the continuous development of robotics technology, robots are increasingly used in various industries, especially in the fields of industrial automation, logistics distribution, smart home, etc. The autonomous navigation capability of robots is one of the core technologies for achieving efficient work and automated operation in the above application scenarios.

[0003] In realizing the navigation capability of robots, existing technologies are mainly based on known path planning algorithms, such as the A* algorithm, the Dijkstra algorithm, and the Rapidly-exploring Random Tree (RRT) algorithm, etc. These algorithms help robots calculate the optimal path by building an environmental map and taking into account static and dynamic obstacles. However, most of these path planning technologies only consider the independent actions of a single robot in the environment, and usually assume that there are no other robots or moving obstacles in the environment.

[0004] When multiple robots need to work together, existing technologies usually rely on central servers or distributed algorithms for path planning and coordination. Through these systems, robots are able to share path trajectories to avoid collisions and coordinate their movements. However, this approach of relying on central servers or distributed algorithms may seriously affect the collaboration between robots and even paralyze the entire system in the event of unstable network connections or server failures. In addition, in highly dynamic environments such as restaurants, the frequent movement of people may lead to reduced path planning efficiency and even deadlock due to the unpredictable behavior of guests and waiters. Summary of the invention

[0005] Based on the above problems, the embodiments of the present application provide a path planning method and related equipment, the purpose of which is to achieve efficient, flexible and intelligent path planning in a multi-device collaborative working scenario.

[0006] In a first aspect, an embodiment of the present application provides a path planning method, which is applied to a first device, including:

[0007] Generate a global path using a path search algorithm based on waypoints within the working area of ​​the first device;

[0008] Dividing the global path into a plurality of local paths;

[0009] Acquire environmental perception information and a shared path trajectory of a second device that establishes a communication connection with the first device;

[0010] Optimizing the path trajectory of the local path based on the environmental perception information and the shared path trajectory;

[0011] The local paths are combined to obtain the final target planning path.

[0012] In one embodiment, the acquiring of the environment perception information and the shared path track of the second device that establishes a communication connection with the first device includes:

[0013] In the first area, in the second device that establishes a communication connection with the first device, environmental perception information and shared path trajectory of at least one of the second devices are obtained; wherein the first area is an area centered on the first device and with an effective communication distance of the first device as a radius; the second device that establishes a communication connection with the first device is a second device in the first area that is connected to the first device through an ad hoc network.

[0014] In one embodiment, dividing the global path into a plurality of local paths includes:

[0015] Based on a preset sampling step, the global path is divided into a plurality of local paths; the preset sampling step is determined by a starting point, a target point and a preset spatial segmentation granularity of the global path.

[0016] In one embodiment, the environmental perception information includes a set of coordinate points of each obstacle; and optimizing the path trajectory of the local path based on the environmental perception information and the shared path trajectory includes:

[0017] Take the starting point of each local path as the current point;

[0018] Determine a next sampling point for the current point according to the environmental perception information, the shared path trajectory and the preset sampling step size;

[0019] Based on the coordinates of each obstacle point in the coordinate point set of each obstacle, identifying the obstacle area of ​​each obstacle;

[0020] Determine whether the connecting line between the current point and the sampling point passes through an obstacle area;

[0021] If not, the current sampling point is used as a new current point, and the step of determining the next sampling point for the current point is returned to be executed until the distance between the next sampling point and the target point is within a preset threshold;

[0022] The starting point, sampling point, and target point are connected in the order of generation to obtain the local path after optimizing the path trajectory.

[0023] In one embodiment, determining the next sampling point for the current point according to the environmental perception information, the shared path trajectory and the preset sampling step size includes:

[0024] Randomly generate multiple candidate points within the sampling range corresponding to the preset sampling step size;

[0025] Based on each of the candidate points and the target point, calculating a first probability that each of the candidate points is away from the target point;

[0026] Calculate a second probability that each candidate point is away from each obstacle based on the obstacle point closest to the candidate point in the coordinate point set of each candidate point and each obstacle;

[0027] Calculate a third probability that each candidate point is distant from the shared path trajectory based on each candidate point and the shared path point in the shared path trajectory that is closest to the candidate point;

[0028] Based on the first probability, the second probability and the third probability of each of the candidate points, a next sampling point of the current point is determined.

[0029] In one embodiment, the first probability of each candidate point being away from the target point is obtained according to the following formula:

[0030] P goal =exp(α*dist((x,y),(x e ,y e )))

[0031] Among them, P goal represents the first probability of the current point being away from the target point; α is a constant, representing the control parameter of the first probability; (x, y) represents the coordinates of the current point; (x e ,y e ) represents the coordinates of the target point; exp() represents the exponential function; dist((x, y), (x e ,y e )) means (x, y) to (x e ,y e ) is the Euclidean distance.

[0032] In one embodiment, the second probability of each candidate point being away from each obstacle is obtained according to the following formula:

[0033]

[0034] Among them, P obstrepresents the second probability of the current point being away from each obstacle; B represents the obstacle set; b represents an obstacle in the obstacle set B; β is a constant, representing the control parameter of the second probability; (x, y) represents the coordinates of the current point; exp() represents the exponential function; dist((x, y), b) represents the minimum distance from the current point (x, y) to obstacle b; area(b) is the area of ​​obstacle b.

[0035] In one embodiment, the third probability of each candidate point being away from the shared path trajectory is obtained according to the following formula:

[0036]

[0037] Among them, P traj represents the third probability of the current point being away from the shared path; B represents the set of shared path trajectories; b represents a shared path trajectory in the set B of obstacle trajectories; γ is a constant, representing the control parameter of the third probability; (x, y) represents the coordinates of the current point; exp() represents the exponential function; dist((x, y), c) represents the minimum distance from the current point (x, y) to the shared path trajectory c; length(c) is the length of the shared path trajectory c.

[0038] In one embodiment, determining the next sampling point of the current point based on the first probability, the second probability and the third probability of each candidate point includes:

[0039] Multiplying the first probability, the second probability and the third probability of each candidate point to obtain a total probability of each candidate point;

[0040] The candidate point with the largest total probability is used as the next sampling point of the current point.

[0041] In a second aspect, an embodiment of the present application further provides a path planning device, including:

[0042] A generating unit, configured to generate a global path using a path search algorithm according to the waypoints in the working area of ​​the first device;

[0043] A division unit, used for dividing the global path into a plurality of local paths;

[0044] An acquisition unit, configured to acquire environmental perception information and a shared path track of a second device that establishes a communication connection with the first device;

[0045] a processing unit, configured to optimize the path trajectory of the local path based on the environmental perception information and the shared path trajectory;

[0046] The combining unit is used to combine the local paths to obtain a final target planning path.

[0047] In a third aspect, an embodiment of the present application further provides a computer device, including:

[0048] CPU, memory, input and output interfaces;

[0049] The memory is a short-term storage memory or a persistent storage memory;

[0050] The central processing unit is configured to communicate with the memory and execute instructions in the memory to perform any one of the above-mentioned path planning methods.

[0051] It can be seen from the above technical solutions that the embodiments of the present application have the following advantages:

[0052] The embodiment of the present application realizes direct communication between multiple devices through short-distance communication. Such a decentralized collaborative control mode reduces the dependence on the central server, avoids the single point failure problem of the central server failure, and reduces the cost of system deployment and maintenance. At the same time, in the multi-device working scenario, when planning the path of each device, not only static obstacles are considered, but also the changes of the second device and other dynamic obstacles can be responded to in real time, thereby ensuring the real-time and accuracy of the path planning and achieving more efficient collaborative work. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0054] Figure 1 A schematic diagram of a path planning method flow provided in an embodiment of the present application;

[0055] Figure 2 A schematic diagram of a single robot system architecture provided in an embodiment of the present application;

[0056] Figure 3 A schematic diagram of short-distance communication between robots provided in an embodiment of the present application;

[0057] Figure 4 A schematic diagram of another short-distance communication between robots provided in an embodiment of the present application;

[0058] Figure 5 A schematic diagram of a path hierarchical planning process provided in an embodiment of the present application;

[0059] Figure 6 A schematic diagram of a low-level path planning process provided in an embodiment of the present application;

[0060] Figure 7 A distributed coordination path and time diagram provided for an embodiment of the present application;

[0061] Figure 8 A schematic diagram of the structure of a path planning device provided in an embodiment of the present application;

[0062] Fig. 9 A schematic diagram of the structure of a computer device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0063] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0064] With the continuous development of robotics technology, robots are increasingly used in various industries, especially in the fields of industrial automation, logistics distribution, smart home, etc. The autonomous navigation capability of robots is one of the core technologies for achieving efficient work and automated operation in the above application scenarios.

[0065] In realizing the navigation capability of robots, existing technologies are mainly based on known path planning algorithms, such as the A* algorithm, the Dijkstra algorithm, and the Rapidly-exploring Random Tree (RRT) algorithm, etc. These algorithms help robots calculate the optimal path by building an environmental map and taking into account static and dynamic obstacles. However, most of these path planning technologies only consider the independent actions of a single robot in the environment, and usually assume that there are no other robots or moving obstacles in the environment.

[0066] When multiple robots need to work together, existing technologies usually rely on central servers or distributed algorithms for path planning and coordination. Through these systems, robots are able to share path trajectories to avoid collisions and coordinate their movements. However, this approach of relying on central servers or distributed algorithms may seriously affect the collaboration between robots and even paralyze the entire system in the event of unstable network connections or server failures. In addition, in highly dynamic environments such as restaurants, the frequent movement of people may lead to reduced path planning efficiency and even deadlock due to the unpredictable behavior of guests and waiters.

[0067] Based on this, in each embodiment of the present application, by acquiring in real time the environmental perception information of the second device that has established a communication connection with the first device and the path trajectory shared by the second device, it is possible to process environmental changes and the movement of other robots in real time without relying on a central server, and quickly re-plan the path to avoid collisions and deadlocks, thereby improving the efficiency and safety of the overall system. It should be pointed out that in the embodiments of the present application, the first device may be a robot (such as a storage robot, a distribution robot, an inspection robot, etc.), a drone, an industrial automation equipment (such as automation equipment in an industrial workshop, an automatic guided vehicle (AGV, Automated Guided Vehicle), etc.). For the convenience of description, the following is an exemplary explanation using a robot as an example.

[0068] The various embodiments of the present application are described in further detail below in conjunction with the accompanying drawings.

[0069] The present application embodiment provides a path planning method, such as Figure 1 As shown, the method includes steps S101-S105.

[0070] S101: Generate a global path using a path search algorithm according to landmark points within a working area of ​​the first device;

[0071] The working area refers to the area or map range where the first device can move. Landmark points in each working area of ​​the first device are marking points with known location coordinates, which can be set according to the actual scenario and needs. For example, in a road network, landmark points can be set at intersections, turning points or forks; in an indoor environment, landmark points can be set at doorways, corners of corridors or other target locations.

[0072] The application process of the path search algorithm is as follows: starting from the starting point, gradually searching different nodes in the working area, so that the first device starts from the starting point and passes through different nodes to finally reach the target point. In the process of searching different nodes in the working area, a cost function can be used to calculate the node priority. Exemplarily, the path cost from the starting point to the current node and the heuristic estimated distance from the current node to the target point can be calculated. The next node of the current node is continuously selected through the cost function, and finally a global path is obtained, which can be composed of several intermediate nodes.

[0073] S102: Divide the global path into a number of local paths;

[0074] In order to facilitate real-time response to the ever-changing environment in the current working area, for example, there is a conflict between the current first device and the travel path of other robots in the working area, or there is a possibility of collision between the current robot's travel path and obstacles, the global path can be split into several segments. In this way, when there is a conflict or collision possibility, priority can be given to the local path where the current robot is located and adaptive adjustments can be made to the local path, so that only part of the local path can be adjusted in real time without recalculating the entire global path, reducing the calculation burden of a single path planning and improving the accuracy and flexibility of path planning.

[0075] S103: Acquire environmental perception information and a shared path track of a second device that establishes a communication connection with the first device;

[0076] Please refer to Figure 2 , Figure 2 Taking a single robot as an example, the system architecture of the first device (robot) in the embodiment of the present application is shown. It can be seen that in the embodiment of the present application, the first device may include: a short-range communication module, a positioning module, a computing module, an environment perception module (such as a laser radar, a camera, etc.), a control unit, and a power unit.

[0077] The following will briefly describe the composition and functions of each module / unit:

[0078] (1) Short-range communication module: responsible for direct communication with other robots. Supports Wi-Fi Direct, Bluetooth or other short-range communication protocols to establish a distributed self-organizing Ad-Hoc network. By establishing an ad-hoc network, the environmental perception information and shared path trajectory of other devices can be obtained in real time. Here, environmental perception data includes the location coordinates of obstacles; shared path trajectory includes the estimated map points and time. Environmental perception data and shared path trajectory can be used for collaborative path planning to avoid path conflicts with other devices.

[0079] (2) Positioning module: responsible for providing the device's own location coordinates (such as GPS, inertial navigation system, etc.). The positioning module can update the robot's position in real time and share it with other devices through the short-range communication module. In addition, it can also determine the relative distance and possible path conflicts based on the received location coordinates of other devices.

[0080] (3) Environmental perception module: The environmental perception module includes lidar, cameras, etc., which are responsible for sensing obstacles and dynamic targets (such as people, other robots, etc.) in the current environment. Using the environmental perception module to sense surrounding obstacles and obtain perception information from other devices can form a more comprehensive environmental view and improve the accuracy of obstacle avoidance in path planning.

[0081] (4) Control unit and power unit: The control unit is responsible for executing the path planning, including controlling the power unit to ensure that the robot moves along the planned trajectory. The power unit (such as a motor) is responsible for providing the power required for the operation of the equipment.

[0082] (5) Computing module: responsible for processing environmental perception data, communication information, and running the path planning algorithm to perform global path planning, local path planning, and dynamic obstacle avoidance calculations.

[0083] Optionally, obtaining environmental perception information and a shared path track of a second device that establishes a communication connection with the first device includes:

[0084] In the first area, in the second device that establishes a communication connection with the first device, environmental perception information and shared path trajectory of at least one second device are obtained; wherein the first area is an area centered on the first device and with an effective communication distance of the first device as a radius; the second device that establishes a communication connection with the first device is a second device in the first area that is connected to the first device through an ad hoc network.

[0085] Please refer to Figure 3 , Figure 3 A schematic diagram of short-distance communication between robots provided in an embodiment of the present application; Figure 3 It shows that multiple robots (devices) are connected to each other through short-distance communication within a certain range. Figure 3 In the figure, three devices (related devices in the left triangle area) can be directly connected to each other through the short-range communication module because the three devices are within the communication range of each other. Similarly, the two devices connected by the dotted line on the right are also within the communication range of each other, but they cannot communicate directly with the device in the left triangle area because the distance between the two devices connected by the dotted line on the right exceeds the effective range of short-range communication of the devices in the left triangle area.

[0086] In a multi-device scenario, in order to ensure the accuracy of the received environmental perception information and shared path trajectory, a communication connection can be established only with other devices within the effective communication range, and the environmental perception information and shared path trajectory of the second device with which the communication connection has been established can be obtained;

[0087] Specifically, the first area can be set to limit which devices can communicate with the first device. The first area can be defined as a circular area with the first device as the center and the effective range of its short-range communication as the radius. Within the first area, different devices can automatically establish a point-to-point distributed network through short-range communication (such as Wi-Fi Direct, Bluetooth, etc.), so that data from other devices can be directly obtained without relying on a central server, reducing the possibility of single point failures.

[0088] Please refer to Figure 4 , Figure 4 It further demonstrates how robots within the communication range can share information through short-distance communication. Within the effective communication range, i.e., the first area, the content shared by each second device may include:

[0089] (1) Self-status information: location coordinates, current speed, battery level and other status information.

[0090] (2) Planned path and schedule: the movement trajectory and time schedule of the second device.

[0091] (3) Dynamic obstacle information: the detected locations of surrounding obstacles.

[0092] (4) Collaborative path planning based on local views: Each robot dynamically plans a path within its own perception range (local view) combined with the information obtained through communication sharing (views from other devices).

[0093] The above information is shared with other devices within the effective communication range through the short-range communication module, forming a collaborative data sharing network.

[0094] S104: Optimizing the path trajectory of the local path based on the environment perception information and the shared path trajectory;

[0095] Please refer to Figure 5 , Figure 5 The following is a schematic diagram of the hierarchical path planning process. Hierarchical path planning specifically includes high-level global path planning (overall path calculation based on the global map) and low-level local path planning (fine-tuning combined with dynamic environment perception).

[0096] It can be seen that in the first stage of the embodiment of the present application, it is first necessary to prepare a working area map of the first device to provide a basis for subsequent path planning. Then, environmental information of the working area of ​​the device is sorted out using sensors, map data, or preset information. In the global map, key landmarks are marked as nodes for path planning. With landmarks as nodes, a directed graph (or topological graph) is formed. In a directed graph, nodes represent locations and edges represent feasible paths between nodes.

[0097] In the second stage, high-level path planning is implemented, specifically: calling the global path planning algorithm (such as A* algorithm, Dijkstra algorithm, etc.), using the topological map, to generate a global path from the starting point to the target point. In order to facilitate real-time adjustment and dynamic planning later, the long global path can be divided into several local paths (small segments).

[0098] Finally, in the third stage, low-level local path planning is implemented: each first device obtains the obstacle information and shared path trajectory (the planned path of the neighboring device, the estimated time to reach each point) collected by the second device through the environmental perception module (such as lidar, camera), independently runs the path planning algorithm, and combines the local views of the first device (itself) and the second device (neighbor) to optimize the local path.

[0099] S105: Combine the local paths to obtain the final target planning path.

[0100] The embodiment of the present application realizes direct communication between multiple devices through short-distance communication. Such a decentralized collaborative control mode reduces the dependence on the central server, avoids the single point failure problem of the central server failure, and reduces the cost of system deployment and maintenance. At the same time, in the multi-device working scenario, when planning the path of each device, not only static obstacles are considered, but also the changes of the second device and other dynamic obstacles can be responded to in real time, thereby ensuring the real-time and accuracy of the path planning and achieving more efficient collaborative work.

[0101] In one embodiment, the global path is divided into a number of local paths, including: dividing the global path into a number of local paths based on a preset sampling step; the preset sampling step is determined by the starting point, the target point and the preset spatial segmentation granularity of the global path.

[0102] The preset sampling step size affects the granularity of path decomposition. The specific value of the step size is determined by the following factors:

[0103] (1) Starting point, target point: If the distance between the starting point and the target point is long, a larger step size may be required to reduce the number of path segmentations. If the distance is short, the step size can be set smaller to allow for more precise control of the path trajectory of the local path.

[0104] (2) Preset spatial segmentation granularity: The preset spatial segmentation granularity is the basic unit of path segmentation, which is usually related to the complexity of the environment. The preset spatial segmentation granularity controls the lower limit of the step size. A smaller granularity means that the path can be planned more finely, but the computational overhead is greater. The specific granularity can be determined according to specific needs and application scenarios, and the embodiments of the present application do not limit this.

[0105] The calculation method for determining each sampling step size is as follows:

[0106] Step length = distance from the starting point to the end point / d. Where d is a constant, indicating the preset space segmentation granularity.

[0107] For example, d can be 100. If the distance between the starting point and the target point is 1000 meters and the spatial segmentation granularity is 100, the sampling step is 10 meters. Finally, the global path will be divided into 1000 / 100=10 local paths.

[0108] Fixing the length of the local path facilitates planning and execution segment by segment. The divided local path can be optimized in combination with real-time perception information. Optimizing only the segmented local path can reduce the complexity of re-planning and reduce the computational burden.

[0109] In one embodiment, the environmental perception information includes a set of coordinate points of each obstacle; based on the environmental perception information and the shared path trajectory, the path trajectory of the local path is optimized, including: taking the starting point of each local path as the current point; determining the next sampling point for the current point according to the environmental perception information, the shared path trajectory and the preset sampling step; identifying the obstacle area of ​​each obstacle based on the coordinates of each obstacle point in the coordinate point set of each obstacle; determining whether the connecting line between the current point and the sampling point passes through the obstacle area; if not, taking the current sampling point as the new current point, returning to execute the step of determining the next sampling point for the current point until the distance between the next sampling point and the target point is within the preset threshold; connecting the starting point, the sampling point, and the target point in sequence in the order of generation to obtain the local path after the path trajectory is optimized.

[0110] It should be understood that obstacles are usually described as polygons, and the description of each obstacle is a set of coordinate points [(x1, y1), (x2, y2), ...]. The polygonal boundary (obstacle area) of the obstacle can be obtained by connecting this set of points end to end in order. The set of coordinate points of multiple obstacles can be described as set B. The environmental perception information provides the geometric shape of each obstacle. By identifying the areas of these obstacles, these obstacle areas can be avoided in the optimization path planning.

[0111] The shared trajectory information can be described as a set C, where each shared path is provided by the surrounding robots. For the kth device, its shared path includes [(xk1, yk1, T1), (xk2, yk2, T2).........], where (xk2, yk2, T2) means that the time required to reach (xk2, yk2) from the coordinate (xk1, yk1) is T2.

[0112] Please refer to Figure 6 , the path optimization starts from the starting point of the current local path and gradually advances to the target point. According to the preset sampling step, candidate sampling points are generated from the current point. When selecting sampling points, it is necessary to ensure that the sampling points avoid obstacle areas and that the sampling points do not conflict with the paths of other devices. In addition, it is necessary to detect whether the straight line between the current point and the candidate sampling points passes through any obstacle area (collision detection). If the connecting line does not pass through the obstacle area, the sampling point is considered feasible and is used as the new current point. Return to the previous step "Determine the next sampling point for the current point based on environmental perception information, shared path trajectory and preset sampling step" until the distance between the next sampling point and the target point is less than the preset threshold, the local path optimization is considered complete. The optimized local path is connected in sequence by the starting point, each sampling point, and the target point. If the current sampling point fails the test, it is excluded, and other candidate sampling points are selected and collision detection is performed.

[0113] Furthermore, according to the environmental perception information, the shared path trajectory and the preset sampling step, the next sampling point for the current point is determined, specifically including: randomly generating multiple candidate points within the sampling range corresponding to the preset sampling step; based on each candidate point and the target point, calculating the first probability of each candidate point being away from the target point; based on each candidate point and the obstacle point closest to the candidate point in the coordinate point set of each obstacle, calculating the second probability of each candidate point being away from each obstacle; based on each candidate point and the shared path point closest to the candidate point in the shared path trajectory, calculating the third probability of each candidate point being away from the shared path trajectory; based on the first probability, the second probability and the third probability of each candidate point, determining the next sampling point for the current point.

[0114] For example, a circular or other shaped sampling range is defined around the current point with a preset sampling step size as the radius. Several candidate points (e.g., 10 to 100, depending on the scene complexity) are randomly generated within the sampling range. Each candidate point represents a possible next sampling point of the current point.

[0115] For each candidate point, its quality is evaluated by the following three probabilities:

[0116] First probability: Calculate the distance between the candidate point and the target point to indicate the possibility that the candidate point is close to the target point. The closer the candidate point is to the target point, the greater the first probability is, and the candidate point close to the target point is preferred.

[0117] Second probability: For each candidate point, find the nearest obstacle point and calculate the distance between the candidate point and the obstacle. The farther the candidate point is from the obstacle, the greater the second probability, and the candidate point far from the obstacle is preferred.

[0118] Third probability: For each candidate point, find the point closest to it on the shared path trajectory and calculate the distance between the two. The farther the candidate point is from the shared path trajectory of other devices, the greater the third probability, and the candidate point that avoids the path conflict of other devices is preferred.

[0119] Optionally, the first probability of each candidate point being away from the target point is obtained according to the following formula:

[0120] P goal =exp(α*dist((x,y),(x e ,y e )))

[0121] Among them, P goal represents the first probability of the current point being away from the target point; α is a constant, representing the control parameter of the first probability; for example, α can be -0.5. (x, y) represents the coordinates of the current point; (x e ,y e ) represents the coordinates of the target point; exp() represents the exponential function; dist((x, y), (x e ,y e )) means (x, y) to (x e ,y e ) is the Euclidean distance.

[0122] Optionally, the second probability of each candidate point being away from each obstacle is obtained according to the following formula:

[0123]

[0124] Among them, P obst represents the second probability of the current point from each obstacle; B represents the obstacle set; b represents an obstacle in the obstacle set B; β is a constant, representing the control parameter of the second probability; (x, y) represents the coordinates of the current point; exp() represents the exponential function; dist((x, y), b) represents the minimum distance from the current point (x, y) to obstacle b; area(b) is the area of ​​obstacle b. The larger the area of ​​the obstacle, the smaller its impact on the sampling point; the smaller the area, the greater the impact.

[0125] Optionally, the third probability of each candidate point being away from the shared path trajectory is obtained according to the following formula:

[0126]

[0127] Among them, P traj represents the third probability of the current point from the shared path; C represents the set of shared path trajectories; c represents a shared path trajectory in the set C of obstacle trajectories; γ is a constant, representing the control parameter of the third probability; (x, y) represents the coordinates of the current point; exp() represents the exponential function; dist((x, y), c) represents the minimum distance from the current point (x, y) to the shared path trajectory c; length(c) is the length of the shared path trajectory c. The longer the trajectory length, the wider its impact range; the shorter the trajectory, the smaller its impact range.

[0128] Then, the next sampling point of the current point is determined based on the first probability, the second probability and the third probability of each candidate point, specifically including: multiplying the first probability, the second probability and the third probability of each candidate point to obtain the total probability of each candidate point; and taking the candidate point with the largest total probability as the next sampling point of the current point.

[0129] Based on the above-mentioned embodiment of calculating the first probability, the second probability, and the third probability, the first probability, the second probability, and the third probability of each candidate point are multiplied together to obtain the total probability of each candidate point, which can be referred to the following formula:

[0130] P total =P goal +P obst +P traj

[0131] Among them, P total represents the total probability. The total probability of each candidate point can be understood as the priority of the candidate point as the next sampling point. The total probability P is selected from the candidate points total The largest point is used as the next sampling point of the current point.

[0132] In the above local path planning, shared information (such as the shared path trajectory and environmental perception information of the second device) has been used to minimize path conflicts through the planning algorithm. However, in some special cases, even if conflicts are avoided as much as possible during the planning stage, potential path conflicts may still occur due to dynamic changes in the environment (such as real-time updates of the second device path, movement of obstacles, etc.).

[0133] Therefore, please refer to Figure 7 In one embodiment, when a device (such as a robot) detects that there is a potential conflict area between its own path and the path of a neighbor, it needs to dynamically coordinate the paths to avoid collision. Specifically, dynamic coordination can be performed in the following three ways:

[0134] (1) Priority-based time scheduling: By assigning priorities to devices, determine which device can pass through the conflict area first. Specifically, high-priority devices can be set to travel first, and low-priority devices can slow down or wait to avoid entering the conflict area at the same time. For example, in a restaurant robot delivery task, a robot with a higher level of delivering food to customers can pass first.

[0135] (2) Speed ​​curve planning: By adjusting the speed curve of the device itself, the time it arrives at the conflict area is changed. For example, the first device itself can accelerate or decelerate to create a gap in time with the second device that has a potential conflict, thereby avoiding the conflict. Specifically, it can be applied to autonomous driving vehicles to pass through intersections by adjusting speed, or warehouse robots to stagger their arrival times in narrow passages.

[0136] (3) Local replanning: If the conflict cannot be resolved through priority time scheduling or speed curve planning, the local path is replanned to bypass the conflict area. Local replanning is suitable for resolving complex conflicts, but it requires certain computing resources and time.

[0137] The above method analyzes the dynamic changes of the conflict area in real time and quickly adjusts the path planning to ensure that the equipment can complete the task safely and efficiently. In actual applications, the method of adjusting potential conflicts can be selected according to the actual scenario.

[0138] In order to implement the path planning method of the embodiment of the present application, the embodiment of the present application also provides a path planning device, such as Figure 8 As shown, the device comprises:

[0139] A generating unit 801 is configured to generate a global path using a path search algorithm according to the landmarks in the working area of ​​the first device;

[0140] A division unit 802, configured to divide the global path into a plurality of local paths;

[0141] An acquisition unit 803 is used to acquire the environment perception information and the shared path track of the second device that establishes a communication connection with the first device;

[0142] A processing unit 804 is configured to optimize the path trajectory of the local path based on the environment perception information and the shared path trajectory;

[0143] The combining unit 805 is used to combine the local paths to obtain a final target planning path.

[0144] In one embodiment, in a first area, in a second device that establishes a communication connection with the first device, environmental perception information and a shared path trajectory of at least one of the second devices are obtained; wherein the first area is an area centered on the first device and with an effective communication distance of the first device as a radius; the second device that establishes a communication connection with the first device is a second device in the first area that is connected to the first device via an ad hoc network.

[0145] In one embodiment, the global path is divided into a plurality of local paths based on a preset sampling step; the preset sampling step is determined by a starting point, a target point and a preset spatial segmentation granularity of the global path.

[0146] In one embodiment, the starting point of each local path is taken as the current point;

[0147] Determine a next sampling point for the current point according to the environmental perception information, the shared path trajectory and the preset sampling step size;

[0148] Based on the coordinates of each obstacle point in the coordinate point set of each obstacle, identifying the obstacle area of ​​each obstacle;

[0149] Determine whether the connecting line between the current point and the sampling point passes through an obstacle area;

[0150] If not, the current sampling point is used as a new current point, and the step of determining the next sampling point for the current point is returned to be executed until the distance between the next sampling point and the target point is within a preset threshold;

[0151] The starting point, sampling point, and target point are connected in the order of generation to obtain the local path after optimizing the path trajectory.

[0152] In one embodiment, a plurality of candidate points are randomly generated within a sampling range corresponding to the preset sampling step size;

[0153] Based on each of the candidate points and the target point, calculating a first probability that each of the candidate points is away from the target point;

[0154] Calculate a second probability that each candidate point is away from each obstacle based on the obstacle point closest to the candidate point in the coordinate point set of each candidate point and each obstacle;

[0155] Calculate a third probability that each candidate point is distant from the shared path trajectory based on each candidate point and the shared path point in the shared path trajectory that is closest to the candidate point;

[0156] Based on the first probability, the second probability and the third probability of each of the candidate points, a next sampling point of the current point is determined.

[0157] In one embodiment, the first probability of each candidate point being away from the target point is obtained according to the following formula:

[0158] P goal =exp(α*dist((x,y),(x e ,y e )))

[0159] Among them, P goal represents the first probability of the current point being away from the target point; α is a constant, representing the control parameter of the first probability; (x, y) represents the coordinates of the current point; (x e ,y e ) represents the coordinates of the target point; exp() represents the exponential function; dist((x, y), (x e ,y e )) means (x, y) to (x e ,y e ) is the Euclidean distance.

[0160] In one embodiment, the second probability of each candidate point being away from each obstacle is obtained according to the following formula:

[0161]

[0162] Among them, P obst represents the second probability of the current point being away from each obstacle; B represents the obstacle set; b represents an obstacle in the obstacle set B; β is a constant, representing the control parameter of the second probability; β can be -5, for example. (x, y) represents the coordinates of the current point; exp() represents the exponential function; dist((x, y), b) represents the minimum distance from the current point (x, y) to obstacle b; area(b) is the area of ​​obstacle b.

[0163] In one embodiment, the third probability of each candidate point being away from the shared path trajectory is obtained according to the following formula:

[0164]

[0165] Among them, P trajrepresents the third probability of the current point from the shared path; C represents the set of shared path trajectories; c represents a shared path trajectory in the set C of obstacle trajectories; γ is a constant, representing the control parameter of the third probability; illustratively, γ can be -3. (x, y) represents the coordinates of the current point; exp() represents the exponential function; dist((x, y), c) represents the minimum distance from the current point (x, y) to the shared path trajectory c; length(c) is the length of the shared path trajectory c.

[0166] In one embodiment, the first probability, the second probability and the third probability of each candidate point are multiplied to obtain a total probability of each candidate point;

[0167] The candidate point with the largest total probability is used as the next sampling point of the current point.

[0168] In actual application, the processing unit 804 can be implemented by a processor in a computer device in combination with a communication interface, and the generating unit 801, the dividing unit 802, the acquiring unit 803, and the combining unit 805 can be implemented by a communication interface in a path planning device.

[0169] It should be noted that: the above-mentioned embodiments provide a path planning device, and only use the division of the above-mentioned program modules as an example to illustrate when performing path planning. In actual applications, the above-mentioned processing can be assigned to different program modules as needed, that is, the internal structure of the device can be divided into different program modules to complete all or part of the processing described above. In addition, the path planning device and the path planning method embodiment provided in the above-mentioned embodiments belong to the same concept, and the specific implementation process is detailed in the method embodiment, which will not be repeated here.

[0170] Based on the hardware implementation of the above program modules, and in order to implement a path planning method provided in an embodiment of the present application, an embodiment of the present application further provides a computer device, such as Fig. 9 As shown, the computer device 900 includes:

[0171] CPU 901, memory 902 and input / output interface 903;

[0172] The memory 902 is a temporary storage memory or a permanent storage memory;

[0173] The central processing unit 901 is configured to communicate with the memory 902 and execute instructions in the memory 902 to perform any one of the above-mentioned path planning methods.

[0174] Of course, in actual application, the various components in the computer device 900 are coupled together through the bus system 904. It can be understood that the bus system 904 is used to realize the connection and communication between these components. In addition to the data bus, the bus system 904 also includes a power bus, a control bus and a status signal bus. However, for the sake of clarity, Fig. 9 Various buses are labeled as bus system 904.

[0175] The memory 902 in the embodiment of the present application is used to store various types of data to support the operation of the computer device 900. Examples of such data include: any computer program used to operate on the computer device 900.

[0176] It is understandable that when the processor in the computer device described above executes the computer program, it can also implement the functions of the various units in the above-mentioned corresponding device embodiments, which will not be repeated here. Exemplarily, the computer program can be divided into one or more modules / units, one or more modules / units are stored in the memory, and executed by the processor to complete the various embodiments of the present application. One or more modules / units can be a series of computer program instruction segments that can perform specific functions, and the instruction segments are used to describe the execution process of the computer program in the computer device. For example, the computer program can be divided into the various units in the above-mentioned computer device, and each unit can implement the specific functions described in the above-mentioned corresponding computer device.

[0177] The computer device may be a computing device such as a desktop computer, a notebook, a PDA, and a cloud server. The computer device may include, but is not limited to, a processor and a memory. Those skilled in the art will appreciate that the processor and the memory are merely examples of computer devices and do not constitute a limitation on the computer device. The computer device may include more or fewer components, or a combination of certain components, or different components. For example, the computer device may also include input and output devices, network access devices, buses, etc.

[0178] The processor can be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor, etc. The processor is the control center of the computer device, and uses various interfaces and lines to connect various parts of the entire computer device.

[0179] The memory can be used to store computer programs and / or modules. The processor implements various functions of the computer device by running or executing the computer programs and / or modules stored in the memory, and calling the data stored in the memory. The memory can mainly include a program storage area and a data storage area, wherein the program storage area can store an operating system, at least one application required for a function, etc.; the data storage area can store data created according to the use of the terminal, etc. In addition, the memory can include a high-speed random access memory, and can also include a non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart memory card (SMC, Smart Media Card), a secure digital (SD, Secure Digital) card, a flash card (Flash Card), at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0180] An embodiment of the present application further provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, any one of the above-mentioned path planning methods is executed.

[0181] An embodiment of the present application also provides a computer program product having a computer program / instruction stored thereon. When the computer program / instruction is executed by a processor, it is used to implement the path planning method described in the first aspect of the embodiment of the present application or any specific implementation method of the first aspect.

[0182] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0183] In the several embodiments provided in the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, which can be electrical, mechanical or other forms.

[0184] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0185] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0186] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program code.

Claims

1. A path planning method, characterized in that: Applied to a first device, comprising: Generate a global path using a path search algorithm based on waypoints within the working area of ​​the first device; Dividing the global path into a plurality of local paths; Acquire environmental perception information and a shared path trajectory of a second device that establishes a communication connection with the first device; Optimizing the path trajectory of the local path based on the environmental perception information and the shared path trajectory; The local paths are combined to obtain the final target planning path.

2. The path planning method according to claim 1, characterized in that: The acquiring of the environment perception information and the shared path track of the second device which establishes a communication connection with the first device includes: In the first area, in the second device that establishes a communication connection with the first device, environmental perception information and shared path trajectory of at least one of the second devices are obtained; wherein the first area is an area centered on the first device and with an effective communication distance of the first device as a radius; the second device that establishes a communication connection with the first device is a second device in the first area that is connected to the first device through an ad hoc network.

3. The path planning method according to any one of claims 1 and 2, characterized in that: The step of dividing the global path into a plurality of local paths comprises: Based on a preset sampling step, the global path is divided into a plurality of local paths; the preset sampling step is determined by a starting point, a target point and a preset spatial segmentation granularity of the global path.

4. The path planning method according to claim 3, characterized in that: The environmental perception information includes a set of coordinate points of each obstacle; and optimizing the path trajectory of the local path based on the environmental perception information and the shared path trajectory includes: Take the starting point of each local path as the current point; Determine a next sampling point for the current point according to the environmental perception information, the shared path trajectory and the preset sampling step size; Based on the coordinates of each obstacle point in the coordinate point set of each obstacle, identifying the obstacle area of ​​each obstacle; Determine whether the connecting line between the current point and the sampling point passes through an obstacle area; If not, the current sampling point is used as a new current point, and the step of determining the next sampling point for the current point is returned to be executed until the distance between the next sampling point and the target point is within a preset threshold; The starting point, sampling point, and target point are connected in the order of generation to obtain the local path after optimizing the path trajectory.

5. The path planning method according to claim 4, characterized in that: The determining, according to the environmental perception information, the shared path trajectory and the preset sampling step, a next sampling point for the current point includes: Randomly generate multiple candidate points within the sampling range corresponding to the preset sampling step size; Based on each of the candidate points and the target point, calculating a first probability that each of the candidate points is away from the target point; Calculate a second probability that each candidate point is away from each obstacle based on the obstacle point closest to the candidate point in the coordinate point set of each candidate point and each obstacle; Calculate a third probability that each candidate point is distant from the shared path trajectory based on each candidate point and the shared path point in the shared path trajectory that is closest to the candidate point; Based on the first probability, the second probability and the third probability of each of the candidate points, a next sampling point of the current point is determined.

6. The path planning method according to claim 5, characterized in that: The first probability of each candidate point being away from the target point is obtained according to the following formula: P goal =exp(α*dist((x,y),(x e ,y e ))) Among them, P goal represents the first probability of the current point being away from the target point; α is a constant, representing the control parameter of the first probability; (x, y) represents the coordinates of the current point; (x e ,y e ) represents the coordinates of the target point; exp() represents the exponential function; dist((x,y),(x e ,y e )) means (x,y) to (x e ,y e ) is the Euclidean distance.

7. The path planning method according to claim 5, characterized in that: The second probability of each candidate point being away from each obstacle is obtained according to the following formula: Among them, P obst represents the second probability of the current point being away from each obstacle; B represents the obstacle set; b represents an obstacle in the obstacle set B; β is a constant, representing the control parameter of the second probability; (x, y) represents the coordinates of the current point; exp() represents the exponential function; dist((x, y), b) represents the minimum distance from the current point (x, y) to obstacle b; area(b) is the area of ​​obstacle b.

8. The path planning method according to claim 5, characterized in that: The third probability of each candidate point being away from the shared path trajectory is obtained according to the following formula: Among them, P traj represents the third probability of the current point being away from the shared path; C represents the set of shared path trajectories; c represents a shared path trajectory in the set C of obstacle trajectories; γ is a constant, representing the control parameter of the third probability; (x, y) represents the coordinates of the current point; exp() represents the exponential function; dist((x, y), c) represents the minimum distance from the current point (x, y) to the shared path trajectory c; length(c) is the length of the shared path trajectory c.

9. The path planning method according to claim 5, characterized in that: The determining the next sampling point of the current point based on the first probability, the second probability and the third probability of each candidate point includes: Multiplying the first probability, the second probability and the third probability of each candidate point to obtain a total probability of each candidate point; The candidate point with the largest total probability is used as the next sampling point of the current point.

10. A computer device, characterized in that: include: CPU, memory and input / output interface; The memory is a short-term storage memory or a persistent storage memory; The central processing unit is configured to communicate with the memory and execute instruction operations in the memory to perform the path planning method described in any one of claims 1 to 9.

Images