Structured path planning method and device, equipment and medium

Abstract

The invention discloses a structured path planning method, device and equipment and a medium, and relates to the technical field of robots, and the method comprises the steps: selecting key nodes in a passable area of a global map, and generating a global road network; according to the weight between the key nodes in the global road network, constructing a weighted graph of the global road network; planning a global path from the starting point to the target point in the global road network by taking the shortest path as an index according to the weighted graphs of the starting point, the target point and the global road network; and performing local obstacle avoidance path planning on the global path to control the robot to perform dynamic obstacle avoidance according to the environment sensing part when the robot moves in the global path. Therefore, the range of the robot during path planning can be limited, more available space in a scene can be ensured, a single shortest safe path can be obtained, the calculation amount is small, the requirements of short path, few break points and the like are met, the consistency of each path planning is ensured, and the safety of robot obstacle avoidance is improved.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a structured path planning method, device, equipment and medium. Background Art

[0002] With the rapid development of robotics, path planning is a crucial component in achieving robotic intelligence. Traditional path planning algorithms often produce paths with numerous inflection points and inefficient planning. When robots plan paths in certain scenarios, they must ensure that the planned paths remain consistent when performing the same task. However, when the environment changes, traditional path planning algorithms struggle to guarantee consistent paths. Summary of the Invention

[0003] The purpose of the present invention is to provide a structured path planning method, device, equipment and medium, which can obtain a single shortest safe path with low computational effort, meet the requirements of short path and few turning points, ensure the consistency of each planned path, and improve the safety of the robot's obstacle avoidance.

[0004] In order to solve the above technical problems, the present invention provides a structured path planning method, comprising:

[0005] Select key nodes in the traversable area of the global map to generate a global road network;

[0006] Constructing a weighted graph of the global road network according to the weights between key nodes in the global road network;

[0007] According to the starting point, the target point and the weighted graph of the global road network, a global path from the starting point to the target point is planned in the global road network with the shortest path as an indicator;

[0008] Perform local obstacle avoidance path planning on the global path to control the robot to perform dynamic obstacle avoidance according to the environment perception part when moving in the global path.

[0009] In a first aspect, in the structured path planning method provided by the present invention, key nodes are selected in a traversable area of a global map to generate a global road network, including:

[0010] Get a global map of the scene environment;

[0011] Recording key node positions in the traversable area of the global map;

[0012] The horizontal and vertical coordinates and yaw angles of the key nodes are saved at the key node positions in a set order, and the positions and postures of the key nodes are written into a parameter file to generate a global road network.

[0013] On the other hand, in the structured path planning method provided by the present invention, constructing a weighted graph of the global road network according to the weights between key nodes in the global road network includes:

[0014] Obtaining a connection sequence between key nodes in the global road network;

[0015] Determine whether to use straight lines or curves to connect key nodes based on the connection sequence and pre-set connection rules;

[0016] Calculate the weights between key nodes through the connectivity and distance between key nodes;

[0017] According to the calculated weights between the key nodes, a weighted graph of the global road network is constructed.

[0018] On the other hand, in the structured path planning method provided by the present invention, based on the starting point, the target point and the weighted graph of the global road network, a global path from the starting point to the target point is planned in the global road network with the shortest path as an indicator, including:

[0019] Searching for the key node closest to the starting point and the target point in the global road network; the coordinate system of the target point is the map coordinate system;

[0020] If the starting point and the target point are at the same distance from the nearest key node, determine the connection mode of the starting point and the target point in the global road network, and use the calculation formula corresponding to the determined connection mode to perform interpolation to obtain the path from the starting point to the target point;

[0021] If the starting point and the target point are at different distances from the nearest key nodes, the Dijkstra algorithm is used to search for the shortest path in the weighted graph of the global road network to calculate the key nodes to be passed through; the connection method of adjacent key nodes in the global road network is determined, and the calculation formula corresponding to the determined connection method is used for interpolation to obtain the path between adjacent key nodes; the paths between all the obtained key nodes are merged to obtain the path between the starting point and the target point.

[0022] On the other hand, in the structured path planning method provided by the present invention, interpolation is performed using a calculation formula corresponding to a determined connection mode to obtain a path between adjacent key nodes, including:

[0023] If the connection between adjacent key nodes is a straight line connection, the corresponding linear function is calculated according to the posture between the adjacent key nodes, and interpolation calculation is performed through the linear function to obtain the straight line path between the adjacent key nodes.

[0024] On the other hand, in the structured path planning method provided by the present invention, interpolation is performed using a calculation formula corresponding to a determined connection mode to obtain a path between adjacent key nodes, including:

[0025] If the connection between adjacent key nodes is a curve connection, the intersection of the tangents of the adjacent key nodes is calculated according to the postures between the adjacent key nodes. A second-order Bezier curve is constructed through the intersection of the adjacent key nodes and their tangents to obtain the curve path between the adjacent key nodes.

[0026] On the other hand, in the structured path planning method provided by the present invention, performing local obstacle avoidance path planning on the global path includes:

[0027] Determine the kinematic model of the robot chassis;

[0028] Determine a velocity space according to the kinematic model and the robot velocity limit, and perform sampling in the velocity space to obtain a dynamic window;

[0029] Using the key nodes in the global path as temporary target points of the dynamic window algorithm;

[0030] The trajectory in the dynamic window is comprehensively evaluated according to the evaluation function, and the optimal trajectory from the starting point to the temporary target point is selected; the evaluation function includes an azimuth evaluation function, a distance evaluation function, a speed evaluation function and an obstacle evaluation function.

[0031] In order to solve the above technical problems, the present invention further provides a structured path planning device, comprising:

[0032] The global road network generation module is used to select key nodes in the traversable area of the global map and generate a global road network;

[0033] A weighted graph construction module, configured to construct a weighted graph of the global road network based on weights between key nodes in the global road network;

[0034] A global path planning module is used to plan a global path from the starting point to the target point in the global road network based on the starting point, the target point and the weighted graph of the global road network, taking the shortest path as an indicator;

[0035] The obstacle avoidance path planning module is used to perform local obstacle avoidance path planning on the global path, so as to control the robot to perform dynamic obstacle avoidance according to the environment perception part when moving in the global path.

[0036] In order to solve the above technical problems, the present invention further provides an electronic device, comprising:

[0037] memory for storing computer programs;

[0038] A processor is configured to implement the steps of the structured path planning method described above when executing the computer program.

[0039] In order to solve the above technical problems, the present invention also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned structured path planning method are implemented.

[0040] It can be seen from the above technical solution that the present invention provides a structured path planning method, which includes: selecting key nodes in the traversable area of the global map to generate a global road network; constructing a weighted graph of the global road network based on the weights between the key nodes in the global road network; planning a global path from the starting point to the target point in the global road network based on the starting point, the target point and the weighted graph of the global road network, with the shortest path as the indicator; performing local obstacle avoidance path planning on the global path to control the robot to perform dynamic obstacle avoidance according to the environmental perception part when moving in the global path.

[0041] The beneficial effect of the present invention is that the structured path planning method provided by the present invention generates a global road network by selecting key nodes in the traversable area of the global map, which can limit the scope of the robot path planning and ensure that there is more available space in the scene; construct a weighted graph of the global road network according to the weights between the key nodes in the global road network; plan a global path from the starting point to the target point in the global road network based on the starting point, the target point and the weighted graph of the global road network, with the shortest path as the indicator, so that a single shortest safe path can be obtained with small calculation amount, and the requirements of short path and few turning points are met, and the consistency of each planned path is ensured, which is suitable for general scenarios such as factories, warehouses, and chicken farms; local obstacle avoidance path planning is performed on the global path to control the robot to move in the global path, and dynamic obstacle avoidance is performed according to the environmental perception part, which can ensure the robot's perception of the surrounding environment and its own position, improve the safety of the robot's obstacle avoidance, avoid the inaccuracy of the global path planning algorithm in avoiding unknown obstacles and the problem that the local path planning algorithm is easily trapped in the local optimum, and is better suitable for scenarios where the robot has high requirements for inspection routes.

[0042] In addition, the present invention also provides a corresponding structured path planning device, electronic device and computer-readable storage medium for the structured path planning method, which have the same or corresponding technical features as the above-mentioned structured path planning method and have the same effects as above. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] In order to more clearly illustrate the embodiments of the present invention, the following is a brief introduction to the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0044] Figure 1 A flow chart of a structured path planning method provided by an embodiment of the present invention;

[0045] Figure 2 A global road network diagram generated by recording points provided in an embodiment of the present invention;

[0046] Figure 3 A schematic diagram showing a path planned in a global road network provided by an embodiment of the present invention;

[0047] Figure 4 A schematic diagram showing a global path provided by an embodiment of the present invention;

[0048] Figure 5 A schematic diagram showing a local obstacle avoidance path provided by an embodiment of the present invention;

[0049] Figure 6 A schematic diagram of the structure of a structured path planning device provided by an embodiment of the present invention;

[0050] Figure 7 A schematic structural diagram of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0051] With the rapid development of robotics, path planning is a crucial component in achieving robot intelligence. Traditional path planning algorithms often produce paths with numerous breakpoints and low planning efficiency. Robots need to plan paths when performing autonomous navigation tasks. This is particularly true in scenarios such as factories, chicken coops, industrial parks, and warehouses. Path planning during robot operations requires consideration of specific rules to ensure efficient and successful path planning within a defined range.

[0052] In the related technical solution, path planning is performed in a structured environment by The algorithm, the azimuth heuristic function of the environment map and the current heading angle of the robot are used to plan the path. In the process of path planning, it is still necessary to search the grid in the environment map and optimize the path based on the inflection point backtracking, which is relatively inefficient. Algorithms are integrated with deep learning and used The algorithm searches and inputs the acquired path width image into the neural network to determine whether it can pass normally, reducing the safety issues of the robot in the road network. When the environment changes, the path planned with the same starting point and end point may be different; the teaching path can ensure that the path is the same each time the task is performed, but the starting point and end point coordinates of the path are fixed. When the robot performs a task, the starting point and end point may not be fixed each time, so the universality of the teaching path is poor.

[0053] When robots perform path planning in certain scenarios, they need to ensure that the planned paths are consistent when performing the same task. When encountering obstacles, they must be able to dynamically avoid them based on the global path. However, when the environment changes, traditional methods cannot guarantee consistent paths each time they are planned. To address these issues, the present invention provides a structured path planning method.

[0054] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0055] In order to enable those skilled in the art to better understand the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific implementation methods. Figure 1 A flow chart of a structured path planning method provided by an embodiment of the present invention is shown in FIG. Figure 1 As shown, the method includes the following steps:

[0056] S101. Select key nodes in a traversable area of a global map to generate a global road network.

[0057] It should be noted that step S101 is the step for generating a global road network. The global road network is a comprehensive network structure that covers all roads and related traffic elements within the traversable area of the global map. The present invention can select key nodes within the traversable area of the global map to generate the global road network, limiting the scope of the robot's path planning.

[0058] S102: Construct a weighted graph of the global road network based on the weights between key nodes in the global road network.

[0059] In practice, the present invention can construct a weighted graph of a global road network based on the weights between key nodes in the global road network. A weighted graph of a global road network can be understood as a graphical structure used to represent the global road network, where each key node represents a location in the network (e.g., an intersection, a road endpoint, etc.), edges represent connections between nodes (e.g., road segments), and each edge is assigned a weight.

[0060] S103. Based on the starting point, the target point and the weighted graph of the global road network, a global path from the starting point to the target point is planned in the global road network with the shortest path as an indicator.

[0061] In practice, the present invention employs the Dijkstra algorithm to search for paths within a global network. Path planning is performed based on the starting point, target point, and the weighted graph of the global network. During path planning, the robot plans a global path from the starting point to the target point within the global network, using the minimum distance as a metric. A global path is a path planned from a given starting point to the target point within a complete network or space containing all relevant information. The planned global path is constrained within the global network, ensuring that the shortest path planned by the robot is consistent each time it performs the same task.

[0062] S104: Perform local obstacle avoidance path planning on the global path to control the robot to perform dynamic obstacle avoidance based on the environment perception part when moving in the global path.

[0063] In practice, when performing path planning, in order to ensure the robot's ability to perceive the surrounding environment and its own position, the present invention can use the dynamic window approach to perform local dynamic obstacle avoidance path planning on the path planned in the global road network.

[0064] In the structured path planning method provided by the embodiment of the present invention, by selecting key nodes in the traversable area of the global map and generating a global road network, the scope of the robot path planning can be limited to ensure that there is more available space in the scene; according to the weights between the key nodes in the global road network, a weighted graph of the global road network is constructed; based on the starting point, the target point and the weighted graph of the global road network, a global path from the starting point to the target point is planned in the global road network with the shortest path as the indicator. In this way, a single shortest safe path can be obtained with low computational effort, meeting the requirements of short path and few turning points, ensuring the consistency of each planned path, and being suitable for general scenarios such as factories, warehouses, and chicken farms; local obstacle avoidance path planning is performed on the global path to control the robot to move in the global path, and dynamic obstacle avoidance is performed according to the environmental perception part. This can ensure the robot's perception of the surrounding environment and its own position, improve the safety of the robot's obstacle avoidance, avoid the inaccuracy of the global path planning algorithm in avoiding unknown obstacles and the problem that the local path planning algorithm is prone to falling into local optimality, and is well suitable for scenarios where the robot has high requirements for inspection routes.

[0065] Furthermore, in a specific implementation, in the above-mentioned structured path planning method provided in an embodiment of the present invention, step S101 selects key nodes in the passable area of the global map to generate a global road network, which may specifically include: obtaining a global map of the on-site environment; recording key node positions in the passable area of the global map; saving the horizontal and vertical coordinates and yaw angle (yaw) of the key nodes at the key node positions in a set order, and writing the position and posture of the key nodes into a parameter file to generate a global road network.

[0066] During implementation, the present invention can first map the surrounding environment to obtain a global map, and then select key nodes in the passable area of the global map to construct a global road network. The global road network can cover all target points that the robot may reach when performing a task. The road network includes two parts, curves and straight lines, to adapt to the kinematic models of different chassis. Among them, the key nodes can be selected by recording key points in the passable area of the global map, saving the x, y coordinates and yaw angles of the key nodes in sequence, or directly writing the posture of the key nodes into the parameter file. After the point recording is completed, the connection sequence between the key nodes is input.

[0067] Furthermore, in a specific implementation, in the above-mentioned structured path planning method provided in an embodiment of the present invention, step S102 constructs a weighted graph of the global road network based on the weights between key nodes in the global road network, which may specifically include: obtaining a connection sequence between key nodes in the global road network; determining whether to use straight lines or curves to connect key nodes based on the connection sequence and pre-set connection rules; calculating the weights between key nodes through the connectivity and distance between key nodes; and constructing a weighted graph of the global road network based on the calculated weights between key nodes.

[0068] In implementation, after entering the connection sequence between key nodes, the connection rules between key nodes can be entered to determine whether to use straight lines or curved lines. This allows the appropriate connection method to be selected based on different environmental requirements and robot motion characteristics. The weights between key nodes are then calculated based on their connectivity and distance, more accurately reflecting the cost or difficulty of different path segments in the actual environment. Finally, based on the calculated weights between key nodes, a weighted graph of the global road network is constructed. This weighted graph of the global road network can more realistically simulate actual scenarios, allowing the robot to plan an optimal path that better suits its actual conditions. The weighted graph of the global road network also provides a macroscopic perspective of the entire environment, facilitating global analysis and decision-making regarding the robot's path.

[0069] Figure 2 The global road network diagram generated by recording points is provided in the embodiment of the present invention. Figure 2As shown, the lines shown in the global road network include straight lines and curves. In this way, generating a global road network by recording points is simple and convenient.

[0070] Furthermore, in a specific implementation, in the above-mentioned structured path planning method provided by an embodiment of the present invention, step S103 plans a global path from the starting point to the target point in the global road network based on the starting point, the target point and the weighted graph of the global road network, with the shortest path as an indicator. Specifically, it may include: searching for the key node closest to the starting point and the target point in the global road network; the coordinate system of the target point is a map coordinate system; if the starting point and the target point are the same in terms of the key node closest to the starting point and the target point, determining the connection method of the starting point and the target point in the global road network, using the calculation formula corresponding to the determined connection method to interpolate, and obtaining the path from the starting point to the target point; if the starting point and the target point are different in terms of the key node closest to the starting point and the target point, using the Dijkstra algorithm to search for the shortest path in the weighted graph of the global road network, and calculating the key nodes to be passed; determining the connection method of adjacent key nodes in the global road network, using the calculation formula corresponding to the determined connection method to interpolate, and obtaining the path between adjacent key nodes; merging the paths between all the obtained key nodes to obtain the path between the starting point and the target point.

[0071] During implementation, the coordinate system of the target point is first determined to ensure that the coordinate system of the target point is the map coordinate system, and the key node closest to the starting point and target point in the global road network is searched. If the key nodes closest to the starting point and target point are the same, it is necessary to determine how they are connected in the global road network to determine whether a straight line or curve is used to connect them, and interpolate according to the calculation formula of the straight line or curve to obtain the path from the starting point to the target point. If the key nodes closest to the starting point and target point are different, use the Dijkstra algorithm to search for the shortest path in the weighted graph, calculate the key nodes to be passed through, determine how the adjacent key nodes are connected in the global road network to determine whether a straight line or curve is used to connect them, and interpolate according to the calculation formula of the straight line or curve to obtain the path between the adjacent key nodes, calculate the path between all the key nodes planned by the Dijkstra algorithm, and merge them to obtain the path between the starting point and the target point.

[0072] Figure 3 This is a schematic diagram showing the paths planned in the global road network provided by the embodiment of the present invention. Figure 3 As shown in the figure, the circle on the left represents the starting point posture, the circle on the right represents the target point posture, the light color represents the global road network, and the dark color represents the path planned by the robot, which includes both straight lines and curves.

[0073] Furthermore, in the specific implementation, in the above steps, the calculation formula corresponding to the determined connection method is used for interpolation to obtain the path between adjacent key nodes, which may specifically include: if the connection method between adjacent key nodes is a straight line connection, the corresponding linear function (also called a linear function) is calculated according to the posture between the adjacent key nodes, and the linear function is used for interpolation calculation to obtain the straight line path between the adjacent key nodes.

[0074] In implementation, if there is a straight path between the key nodes, the y=kx+b function (k is the slope and b is the intercept) is calculated based on the posture between the two key nodes, and the straight path between the key nodes is obtained by interpolation calculation through this function.

[0075] Similarly, interpolation is performed using the calculation formula corresponding to the determined connection method to obtain the path from the starting point to the target point. Specifically, it may include: if the connection method between the starting point and the target point is a straight line connection, the corresponding linear function is calculated according to the posture between the starting point and the target point, and interpolation calculation is performed through the linear function to obtain the straight line path between the starting point and the target point.

[0076] Furthermore, in specific implementation, in the above-mentioned structured path planning method provided in an embodiment of the present invention, interpolation is performed using a calculation formula corresponding to a determined connection method to obtain the path between adjacent key nodes. Specifically, it can also include: if the connection method between adjacent key nodes is a curve connection, the intersection of the tangents of adjacent key nodes is calculated according to the posture between the adjacent key nodes, and a second-order Bezier curve is constructed through the intersection of the adjacent key nodes and their tangents to obtain the curve path between the adjacent key nodes.

[0077] In practice, if the paths between key nodes are curved, it is necessary to calculate the intersection of the tangents of the two key nodes based on their position and orientation. A second-order Bezier curve is constructed through the two key nodes and their intersection to calculate the path points. Finally, a global road network is generated for the robot to perform path planning in actual application scenarios. The formula for the second-order Bezier curve is as follows:

[0078] ;

[0079] in, Represents a parameter used to control the position of the value on the Bezier curve; Indicates that the parameter The corresponding point on the second-order Bezier curve. varies between 0 and 1, It will traverse points at different positions on the curve. 、 Represents the two endpoints (key nodes) of a second-order Bezier curve, corresponding to the starting and ending points of the curve respectively. In the robot path planning scenario, they can represent the starting and ending positions of the path. Represents the control points of a second-order Bezier curve, which determines the shape of the curve. When constructing a curve based on key nodes, the intersection of the tangents at two key nodes is used to control the direction and degree of the curve.

[0080] Similarly, interpolation is performed using the calculation formula corresponding to the determined connection method to obtain the path from the starting point to the target point. Specifically, it may also include: if the connection method between the starting point and the target point is a curve connection, the intersection of the tangents of the starting point and the target point is calculated according to the posture between the starting point and the target point, and a second-order Bezier curve is constructed through the intersection of the starting point and the target point and their tangents to obtain the curve path between the starting point and the target point.

[0081] It should be noted that second-order Bezier curves have excellent smoothness. The present invention constructs second-order Bezier curves to obtain curved paths between adjacent key nodes, avoiding sharp corners in the path. This is crucial for robot motion control, ensuring smoother movement, reducing mechanical wear and energy consumption, and improving the robot's stability and safety during operation. By constructing Bezier curves based on key nodes and intersections to generate a global path network, the present invention can find a more optimal path solution while satisfying the robot's motion constraints.

[0082] Furthermore, in specific implementation, in the above-mentioned structured path planning method provided in an embodiment of the present invention, step S104 performs local obstacle avoidance path planning on the global path, which may specifically include: determining the kinematic model of the robot chassis; determining the speed space based on the kinematic model and the robot speed limit, and sampling in the speed space to obtain a dynamic window; using the key nodes in the global path as temporary target points of the dynamic window algorithm; comprehensively evaluating the trajectory within the dynamic window according to the evaluation function, and selecting the optimal trajectory from the starting point to the temporary target point; the evaluation function includes an azimuth evaluation function, a distance evaluation function, a speed evaluation function and an obstacle evaluation function.

[0083] In implementation, the present invention can use the DWA algorithm to control the robot's movement in the global path. According to the environmental perception part, if there are obstacles in the global path, dynamic obstacle avoidance can be performed. After avoiding the obstacles, the task in the global path can be restored. It mainly includes three parts: motion model, speed sampling and evaluation function.

[0084] First, determine the kinematic model of the robot chassis (two-wheel differential model). The kinematic model can be expressed as a motion differential equation, which is as follows:

[0085] ;

[0086] ;

[0087] ;

[0088] ;

[0089] ;

[0090] in, 、 Respectively indicate at time , the horizontal and vertical coordinates of the robot in the Cartesian coordinate system, which are used to determine the position of the robot on the plane. 、 Respectively indicate at time , the horizontal and vertical coordinates of the robot in the Cartesian coordinate system, reflecting the time interval position changes. Indicates at time , the linear speed of the robot, that is, the speed of the robot's overall movement. Indicates at time , the robot's linear speed, reflecting the time interval The subsequent linear speed changes. Indicates at time , the angular velocity of the robot, which describes how fast the robot rotates around its own center. Indicates at time , the angular velocity of the robot, reflecting the time interval The angular velocity changes after . Indicates at time , the robot's heading speed. Indicates at time , the robot's heading speed, showing the time interval After the change of direction. Indicates at time , the robot's linear acceleration, used to describe how fast the linear velocity changes. Indicates at time , the robot's angular acceleration, used to describe how quickly the angular velocity changes.

[0091] The speed space is determined according to the robot's speed limit, and a dynamic window is sampled in the speed space to obtain the optimal trajectory. The optimal trajectory is selected based on the evaluation function. The evaluation function formula is as follows: heading(v,w) is the azimuth evaluation function, which represents the angular deviation between the end point of the trajectory generated by the sampling and the target point, and is used to correct the robot's heading angle; dist(v,w) is the distance evaluation function, which represents the distance between the robot and the nearest obstacle on the sampling trajectory, and is used to avoid obstacles; velocity(v,w) is the speed evaluation function, which represents the speed of the robot, so that the robot can reach the target point at a faster speed. In order to avoid any evaluation function being too large, the three are normalized.

[0092] ;

[0093] Different obstacle avoidance distances are set for different types of obstacles, and an obstacle evaluation function is added to the evaluation function. The value range of this evaluation function is ,The evaluation function formula is as follows, d represents the radius of the obstacle area, d0 represents the minimum distance between the trajectory and the obstacle in the sampling speed space, and d1 represents the obstacle avoidance distance set for different obstacle areas. The overall evaluation function formula is as follows;

[0094] ;

[0095] ;

[0096] Since the global path obtained by the Dijkstra algorithm is inaccurate for avoiding unknown obstacles, the DWA algorithm is prone to falling into local optimality. In this case, the present invention uses the key nodes in the global path as temporary target points of the Dynamic Window Approach (DWA), which can improve the safety of the robot during movement.

[0097] Figure 4 A schematic diagram of the global path display provided by an embodiment of the present invention. Figure 5 A schematic diagram showing a local obstacle avoidance path provided by an embodiment of the present invention. Figure 4 and Figure 5 The red dots in the figure represent the starting point and the target point, the blue lines represent the global path planned in the global road network, the red lines represent the path of the local path planning algorithm, the yellow lines represent the path the robot has moved on, the blue dots represent the key nodes in the global road network as temporary target points for local path planning, the green lines represent the DWA algorithm sampling trajectory, and the red hollow circles represent obstacles. Figure 5As can be seen in the figure, the robot avoids obstacles while moving along the global path. This invention can improve the robot's obstacle avoidance safety by introducing different obstacle avoidance distances for different types of obstacles. It also integrates the global path planning algorithm with the local path planning algorithm, thereby avoiding the inaccuracy of the global path planning algorithm in avoiding unknown obstacles and the problem of the local path planning algorithm easily falling into the local optimum.

[0098] While the above embodiments describe a structured path planning method in detail, the present invention also provides corresponding embodiments of a structured path planning device and electronic device. It should be noted that the present invention describes the device embodiments from two perspectives: one based on functional modules and the other based on hardware.

[0099] Figure 6 This is a schematic diagram of the structure of the structured path planning device provided by an embodiment of the present invention. This embodiment is based on the perspective of functional modules, such as Figure 6 As shown, the device includes:

[0100] A global road network generation module 10 is used to select key nodes in the traversable area of the global map to generate a global road network;

[0101] A weighted graph construction module 11 is used to construct a weighted graph of the global road network based on the weights between key nodes in the global road network;

[0102] A global path planning module 12 is used to plan a global path from the starting point to the target point in the global road network based on the starting point, the target point and the weighted graph of the global road network, taking the shortest path as an indicator;

[0103] The obstacle avoidance path planning module 13 is used to perform local obstacle avoidance path planning on the global path, so as to control the robot to perform dynamic obstacle avoidance according to the environment perception part when moving in the global path.

[0104] In the above-mentioned structured path planning device provided in the embodiment of the present invention, global road network generation, global path planning, and local dynamic obstacle avoidance path planning can be performed through the interaction of the above-mentioned four modules, which can limit the scope of the robot's path planning and ensure that there is more available space in the scene; a single shortest safe path can be obtained with small calculation amount, meeting the requirements of short path and few turning points, ensuring the consistency of each planned path, and is suitable for general scenarios such as factories, warehouses, and chicken farms; and can ensure the robot's perception of the surrounding environment and its own position, improve the safety of the robot's obstacle avoidance, avoid the inaccuracy of the global path planning algorithm in avoiding unknown obstacles and the problem that the local path planning algorithm is prone to falling into local optimality, and is better suitable for scenarios where the robot has high requirements for inspection routes.

[0105] Since the embodiments of the apparatus part correspond to the embodiments of the method part, the embodiments of the apparatus part refer to the description of the embodiments of the method part and are not described here in detail. The embodiments of the apparatus part have the same beneficial effects as the structured path planning method mentioned above.

[0106] Furthermore, in specific implementation, in the above-mentioned structured path planning device provided in an embodiment of the present invention, the global road network generation module 10 can be specifically used to obtain a global map of the on-site environment; record key node positions in the passable area of the global map; save the horizontal and vertical coordinates and yaw angles of the key nodes at the key node positions in a set order, and write the posture of the key nodes into a parameter file to generate a global road network.

[0107] Furthermore, in a specific implementation, in the above-mentioned structured path planning device provided in an embodiment of the present invention, the weighted graph construction module 11 can be specifically used to obtain a connection sequence between key nodes in a global road network; determine whether to use straight lines or curves to connect key nodes based on the connection sequence and pre-set connection rules; calculate the weights between key nodes through the connectivity and distance between key nodes; and construct a weighted graph of the global road network based on the calculated weights between key nodes.

[0108] Furthermore, in a specific implementation, in the above-mentioned structured path planning device provided in an embodiment of the present invention, the global path planning module 12 can be specifically used to search for the key node closest to the starting point and the target point in the global road network; the coordinate system of the target point is the map coordinate system; if the starting point and the target point are the same in distance to the closest key node, determine the connection method of the starting point and the target point in the global road network, and use the calculation formula corresponding to the determined connection method to interpolate and obtain the path from the starting point to the target point; if the starting point and the target point are different in distance to the closest key node, use the Dijkstra algorithm to search for the shortest path in the weighted graph of the global road network to calculate the key nodes to be passed through; determine the connection method of adjacent key nodes in the global road network, and use the calculation formula corresponding to the determined connection method to interpolate and obtain the path between adjacent key nodes; merge the paths between all the obtained key nodes to obtain the path between the starting point and the target point.

[0109] Furthermore, in specific implementation, in the above-mentioned structured path planning device provided in an embodiment of the present invention, the obstacle avoidance path planning module 13 can be specifically used to determine the kinematic model of the robot chassis; determine the speed space according to the kinematic model and the robot speed limit, and sample in the speed space to obtain a dynamic window; use the key nodes in the global path as temporary target points of the dynamic window algorithm; comprehensively evaluate the trajectory in the dynamic window according to the evaluation function, and select the optimal trajectory from the starting point to the temporary target point; the evaluation function includes an azimuth evaluation function, a distance evaluation function, a speed evaluation function and an obstacle evaluation function.

[0110] Figure 7 This is a schematic diagram of the structure of an electronic device provided by an embodiment of the present invention. This embodiment is based on the hardware perspective, such as Figure 7 As shown, the electronic equipment includes:

[0111] Memory 20, for storing computer programs;

[0112] The processor 21 is configured to implement the steps of the structured path planning method mentioned in the above embodiment when executing a computer program.

[0113] The processor 21 may include one or more processing cores, such as a quad-core processor or an octa-core processor. The processor 21 may be implemented in at least one of the following hardware forms: a digital signal processor (DSP), a field-programmable gate array (FPGA), or a programmable logic array (PLA). The processor 21 may also include a main processor and a coprocessor. The main processor is a processor used to process data in the awake state, also known as a CPU; the coprocessor is a low-power processor used to process data in the standby state. In some embodiments, the processor 21 may integrate a graphics processing unit (GPU), which is responsible for rendering and drawing content required to be displayed on the display screen. In some embodiments, the processor 21 may also include an artificial intelligence (AI) processor, which is used to handle computational operations related to machine learning.

[0114] The memory 20 may include one or more computer-readable storage media, which may be non-transitory. The memory 20 may also include a high-speed random access memory, and a non-volatile memory, such as one or more disk storage devices, flash memory storage devices. In this embodiment, the memory 20 is at least used to store the following computer program 201, wherein, after the computer program is loaded and executed by the processor 21, it can implement the relevant steps of the structured path planning method disclosed in any of the aforementioned embodiments. In addition, the resources stored in the memory 20 may also include an operating system 202 and data 203, etc., and the storage method may be temporary storage or permanent storage. Among them, the operating system 202 may include Windows, Unix, Linux, etc. The data 203 may include but is not limited to the data involved in the structured path planning method mentioned above.

[0115] In some embodiments, the electronic device may further include a display screen 22, an input / output interface 23, a communication interface 24, a power supply 25, and a communication bus 26. Those skilled in the art will appreciate that Figure 7 The structure shown in the figure does not constitute a limitation on the electronic device, and may include more or fewer components than shown. The electronic device provided by the embodiment of the present invention includes a memory and a processor. When the processor executes the program stored in the memory, it can implement the structured path planning method mentioned above, with the same effect as above.

[0116] Finally, the present invention also provides an embodiment corresponding to a computer-readable storage medium. The computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps described in the above method embodiment.

[0117] It is understandable that if the method in the above embodiment 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 invention, 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. The computer software product is stored in a storage medium and executes all or part of the steps of the above method in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM), random access memory (RAM), magnetic disk or optical disk, etc. Various media that can store program codes. The computer-readable storage medium provided by the present invention can implement the above-mentioned structured path planning method, and the effect is the same as above.

[0118] Finally, the present invention also provides a corresponding embodiment of a computer program product. The computer program product includes a computer program / instructions that, when executed by a processor, implement the steps described in the above-mentioned structured path planning method embodiment. The computer program product provided by the present invention can implement the structured path planning method described above, achieving the same effects as described above.

[0119] It should also be noted that, in this specification, relational terms such as first and second, etc., are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or device comprising a series of elements includes not only those elements, but also other elements not explicitly listed, or elements inherent to such process, method, article, or device. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of additional identical elements in the process, method, article, or device comprising the aforementioned elements.

[0120] The structured path planning method, device, equipment and medium provided by the present invention are introduced in detail above. The various embodiments in the specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same and similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part description. It should be pointed out that for ordinary technicians in this technical field, without departing from the principle of the present invention, the present invention can also be improved and modified in several ways, and these improvements and modifications also fall within the scope of protection of the present invention.

Claims

1. A structured path planning method, characterized in that: include: Select key nodes in the traversable area of the global map to generate a global road network; Constructing a weighted graph of the global road network according to the weights between key nodes in the global road network; According to the starting point, the target point and the weighted graph of the global road network, a global path from the starting point to the target point is planned in the global road network with the shortest path as an indicator; Perform local obstacle avoidance path planning on the global path to control the robot to perform dynamic obstacle avoidance according to the environment perception part when moving in the global path.

2. The structured path planning method according to claim 1, characterized in that: Select key nodes in the traversable area of the global map to generate a global road network, including: Get a global map of the scene environment; Recording key node positions in the traversable area of the global map; The horizontal and vertical coordinates and yaw angles of the key nodes are saved at the key node positions in a set order, and the positions and postures of the key nodes are written into a parameter file to generate a global road network.

3. The structured path planning method according to claim 1, characterized in that: Constructing a weighted graph of the global road network according to weights between key nodes in the global road network, including: Obtaining a connection sequence between key nodes in the global road network; Determine whether to use straight lines or curves to connect key nodes based on the connection sequence and pre-set connection rules; Calculate the weights between key nodes through the connectivity and distance between key nodes; According to the calculated weights between the key nodes, a weighted graph of the global road network is constructed.

4. The structured path planning method according to claim 1, characterized in that: According to the starting point, the target point and the weighted graph of the global road network, a global path from the starting point to the target point is planned in the global road network with the shortest path as an indicator, including: Searching for the key node closest to the starting point and the target point in the global road network; the coordinate system of the target point is the map coordinate system; If the starting point and the target point are at the same distance from the nearest key node, determine the connection mode of the starting point and the target point in the global road network, and use the calculation formula corresponding to the determined connection mode to perform interpolation to obtain the path from the starting point to the target point; If the starting point and the target point are at different distances from the nearest key nodes, the Dijkstra algorithm is used to search for the shortest path in the weighted graph of the global road network to calculate the key nodes to be passed through; the connection method of adjacent key nodes in the global road network is determined, and the calculation formula corresponding to the determined connection method is used for interpolation to obtain the path between adjacent key nodes; the paths between all the obtained key nodes are merged to obtain the path between the starting point and the target point.

5. The structured path planning method according to claim 4, characterized in that: Interpolation is performed using the calculation formula corresponding to the determined connection method to obtain the path between adjacent key nodes, including: If the connection between adjacent key nodes is a straight line connection, the corresponding linear function is calculated according to the posture between the adjacent key nodes, and interpolation calculation is performed through the linear function to obtain the straight line path between the adjacent key nodes.

6. The structured path planning method according to claim 4, characterized in that: Interpolation is performed using the calculation formula corresponding to the determined connection method to obtain the path between adjacent key nodes, including: If the connection between adjacent key nodes is a curve connection, the intersection of the tangents of the adjacent key nodes is calculated according to the postures between the adjacent key nodes. A second-order Bezier curve is constructed through the intersection of the adjacent key nodes and their tangents to obtain the curve path between the adjacent key nodes.

7. The structured path planning method according to claim 1, characterized in that: Performing local obstacle avoidance path planning on the global path, including: Determine the kinematic model of the robot chassis; Determine a velocity space according to the kinematic model and the robot velocity limit, and perform sampling in the velocity space to obtain a dynamic window; Using the key nodes in the global path as temporary target points of the dynamic window algorithm; The trajectory in the dynamic window is comprehensively evaluated according to the evaluation function, and the optimal trajectory from the starting point to the temporary target point is selected; the evaluation function includes an azimuth evaluation function, a distance evaluation function, a speed evaluation function and an obstacle evaluation function.

8. A structured path planning device, characterized in that: include: The global road network generation module is used to select key nodes in the traversable area of the global map and generate a global road network; A weighted graph construction module, configured to construct a weighted graph of the global road network based on weights between key nodes in the global road network; A global path planning module is used to plan a global path from the starting point to the target point in the global road network based on the starting point, the target point and the weighted graph of the global road network, taking the shortest path as an indicator; The obstacle avoidance path planning module is used to perform local obstacle avoidance path planning on the global path, so as to control the robot to perform dynamic obstacle avoidance according to the environment perception part when moving in the global path.

9. An electronic device, characterized in that: include: memory for storing computer programs; A processor, configured to implement the steps of the structured path planning method according to any one of claims 1 to 7 when executing the computer program.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the structured path planning method according to any one of claims 1 to 7 are implemented.

Images