A robot path planning method based on the fusion of local exploration and global exploration

Abstract

The present invention discloses a robot path planning method based on the fusion of local exploration and global exploration, which obtains the global exploration space, divides the global exploration space into several subspaces, takes the centroid of the subspace as the global path point, and obtains the global exploration path; takes the space within a preset distance around the robot as the local exploration space, takes the intersection of the global exploration path and the boundary of the local exploration space as the boundary point, uniformly samples the local exploration space through radar and extracts candidate path points, selects the candidate path points through a local path point selection algorithm, obtains local path points; and obtains the local exploration path; replaces the path between two boundary points in the global exploration path with the local exploration path, and obtains the final planning path. The hierarchical search framework can not only perform accurate path planning in the local space, but also prevent falling into local optimization through the guidance of the global exploration planning, while saving computing resources.

Description

Technical Field

[0001] The present invention relates to robot path planning, and in particular to a robot path planning method based on the fusion of local exploration and global exploration. Background Art

[0002] As the application scenarios of mobile robots become more and more complex, the amount of external environmental information received by robots through sensors increases. Manually guiding robots to build environmental maps cannot meet some specific occasions, such as underground mines, radioactive nuclear power plants, and planetary surface operations. In these scenarios, wireless communication is not stable, and manual guidance can easily cause mobile robots to lose control and is dangerous. In order to meet the needs of autonomy and intelligence of mobile robots, it is necessary to introduce autonomous exploration path planning technology to allow mobile robots to autonomously complete exploration and mapping tasks. In ordinary life scenarios, mobile robots are also increasingly appearing in shopping malls, hospitals, and home environments. Since these environments are constantly changing, building a map only once cannot continuously update and modify the environment. The accuracy and efficiency of robot exploration path planning technology in the existing technology are still insufficient. Summary of the invention

[0003] Purpose of the invention: In view of the above shortcomings, the present invention provides an accurate and computationally efficient robot path planning method based on the fusion of local exploration and global exploration.

[0004] Technical solution: To solve the above problems, the present invention adopts a robot path planning method based on the fusion of local exploration and global exploration, which is characterized by comprising the following steps:

[0005] (1) Obtain the robot's current position and environment map, use the environment map as the global exploration space, divide the global exploration space into several subspaces, use the centroid of the subspace as the global path point, and use the dynamic path planning algorithm to calculate the shortest distance between all global path points and the robot's current position; solve the traveling salesman problem through a greedy strategy to obtain the visit order between global path points and obtain the global exploration path;

[0006] (2) The space within a preset distance around the robot is used as the local exploration space, and the intersection of the global exploration path and the local exploration space boundary is used as the boundary point for entering and leaving the local exploration space. The local exploration space is uniformly sampled by radar and candidate path points are extracted. The candidate path points are selected by the local path point selection algorithm to obtain local path points. A dynamic path planning algorithm is used to calculate the shortest distance between all local path points and boundary points. The traveling salesman problem is solved by a greedy strategy to obtain the visit order between local path points and obtain the local exploration path.

[0007] (3) Replace the path between two boundary points in the global exploration path with the local exploration path to obtain the final planning path.

[0008] Furthermore, in the step (2), the local exploration path is divided into a number of path sub-segments through segmentation points, each path sub-segment is optimized for the trajectory of the robot during movement through a piecewise Bezier curve, and the robot turns on the spot at the segmentation point to switch to the next path sub-segment.

[0009] Furthermore, the specific steps of dividing the local exploration path into a number of path sub-segments are: r , local path points r -1 and local waypoints r +1 The distance between two of them, calculate the local path point r The angle at which the local path point r The angle at which is less than the threshold, then the local path point r As a segmentation point, the robot turns on the spot at this point.

[0010] Furthermore, the specific steps of optimizing the trajectory of the robot during motion using the piecewise Bezier curve are as follows:

[0011] Define an interval , in the interval, by Control points determined Order Bezier curve Defined as:

[0012] ;

[0013] in, For the control points, is the Bernstein coefficient;

[0014] The piecewise Bezier curve is represented as:

[0015] ;

[0016] in, For the Piecewise Bezier curve, For the The first segment of the piecewise Bezier curve control points, is the time taken by the piecewise Bezier curve, For the Scaling factor of piecewise Bezier curve;

[0017] The objective function is to smooth the robot trajectory. It is expressed as:

[0018] ;

[0019] in, is a column vector of control points, is the Hessian matrix. The robot trajectory is adjusted by setting the range of the objective function value and solving the piecewise Bezier curve according to the constraint conditions.

[0020] The constraints to ensure the smoothness of the robot's motion trajectory are:

[0021] The endpoint constraints and continuity constraints are:

[0022] ;

[0023] ;

[0024] in, For the The position and last control point of the piecewise Bezier curve For the The position of the first control point of the piecewise Bezier curve, is the derivative order, ; According to the property that the Bezier curve is in the convex space generated by the control points, the control points are constrained in the safety corridor, then the Bezier curve constrained by the control points will also be in the safety corridor, that is:

[0025] ;

[0026] in, and It is The position constraints of the control points of the piecewise Bezier curve are generated. The area of ​​a safe space.

[0027] Furthermore, the steps of determining the safe space are:

[0028] (2.1) For each path segment, first use two adjacent path points as diagonals to generate an initial rectangular area;

[0029] (2.2) The initial rectangle expands outward along the four sides until it encounters an obstacle or reaches the upper limit of expansion, resulting in a rectangular space;

[0030] (2.3) For the next path point, if the path point is in the rectangular space generated by the previous two adjacent path points, it is skipped; otherwise, the path point and its previous adjacent path point are used as diagonals to generate the initial rectangular area, and then step (2.2) is repeated;

[0031] (4) Check whether the current rectangular space contains all the path points in the previous rectangular space. If all are included, delete the previous rectangular space;

[0032] (5) When all path points have generated rectangular spaces, if the x-1th rectangular space is x +1 rectangular space intersects, then delete the x Rectangular spaces, all reserved rectangular spaces form a safe space.

[0033] Furthermore, in step (2), the specific method of uniformly sampling the local exploration space by radar and extracting candidate path points is as follows: sampling the local exploration space by a lidar sensor to obtain point cloud data, calculating the size of the area that can be covered by all possible path points in the point cloud data, and taking the size of the area that can be covered by the unexplored area as the reward of the path point, and sorting the path points in descending order based on the size of the reward to obtain a priority queue;

[0034] When the boundary of the unexplored area in the local space is captured by the lidar sensor, its center point represents , the corresponding normal vector is , Indicates the location of the waypoint; if the boundary is located at , the path points are covered, and the following conditions need to be met, indicating that the boundary of the unexplored area has been covered:

[0035] ;

[0036] ;

[0037] in, and Represents constraints on plane distance and direction;

[0038] The calculation method of the waypoint reward is as follows:

[0039] ;

[0040] in, For the selected Waypoints, For the selected Waypoints Indicates waypoints The area that can be covered, Indicates waypoints The area that can be covered; The function represents the union of all sets in the brackets from b=1 to b=a-1;

[0041] Waypoints are selected according to the priority order in the priority queue, and their rewards are updated when new waypoints are selected in the priority queue; when the priority queue is empty or the reward for adding a new candidate waypoint is less than 0.01, the waypoint selection process ends and the selected local waypoint is obtained.

[0042] Furthermore, the selected local path points are stored in a priority queue set U, and the dynamic path planning algorithm sorts the path points in the priority queue set U, and obtains the shortest distance between all local path points and the boundary points according to the sorting result; the dynamic path planning algorithm uses Value and The value describes the cost of a node, which includes the starting point, the end point, and the path points between the starting point and the end point of the planned path, where The value is the minimum cost from the starting point to the current node in all current calculations. Value is the cost from the starting point to the current node in this calculation;

[0043] The specific steps of the dynamic path planning algorithm for updating each node are as follows:

[0044] (1) Update each node The value is to calculate the value of the node among all other nodes around the node. Add the value of this node to the target node goal of The minimum sum of values; Representation Node n To the target node goal The residual is calculated as follows:

[0045] ;

[0046] in, For Node n Location, is the location of the target node goal, For Node n The unit normal vector of the plane formed by the target node goal;

[0047] (2) If this node belongs to the priority queue set , then remove it from the priority queue set Delete from;

[0048] (3) If the node Value If the value is inconsistent, it will be placed in a priority queue set reassessment in the

[0049] Priority Queue Set According to the node Sort the values, The values ​​are defined as follows:

[0050] ;

[0051] For the priority queue set When sorting the nodes in The node with the smallest value; when there are multiple When the minimum nodes are equal, select Nodes with the smallest value are sorted.

[0052] Furthermore, the greedy strategy for solving the traveling salesman problem is to traverse all selected path points in the local exploration space from the current path point through the mathematical model of the traveling salesman problem, and each path point is visited once and only once, and the shortest distance between all path points is calculated by the dynamic path planning algorithm, and the path point with the closest distance is selected as the next node, and then the current path point is marked as having been passed, and the next path point is used as the current path point, and this is repeated until the access order of all path points in the local exploration space is calculated.

[0053] Furthermore, in step (1), the global exploration space is divided into several subspaces, each of which is divided into three states: unexplored, exploring and explored according to the exploration status, and is updated as the robot exploration progresses; the scanning information of the robot in each subspace is recorded, if there is no scanning information of any robot in a subspace, it is marked as an unexplored subspace, if there is space that the robot has not scanned in a subspace, it is marked as an exploring subspace, if a subspace only has space that the robot has scanned but no unscanned space, it is marked as an explored subspace, in the strategy of global exploration path planning, only the subspace marked as being explored needs to be considered; the centroid of the exploring subspace is obtained as the global path point.

[0054] Furthermore, velocity and acceleration constraints are set to make the robot move better according to the calculated trajectory, namely:

[0055] ;

[0056] ;

[0057] in, and are the minimum and maximum values ​​of the robot speed, and are the minimum and maximum values ​​of the robot’s acceleration; For the robot The position corresponding to the moment t when the segmented Bezier curve moves.

[0058] Beneficial effects: Compared with the prior art, the present invention has the following significant advantages: (1) The hierarchical search framework can not only perform accurate path planning in the local space, but also prevent falling into local optimization through the guidance of global exploration planning, while saving computing resources; (2) The mobile robot's running trajectory is constrained by the mobile robot trajectory curvature optimization method based on Bezier curves, making the exploration efficiency higher and the movement smoother; (3) The dynamic path planning algorithm reuses the previous search result information, so that it can adapt to changes in the space without recalculating the entire exploration space, which can greatly reduce the computing resource consumption of exploration path planning in a dynamic environment. (4) The use of a greedy strategy to solve the traveling salesman problem reduces the processing time of the algorithm compared to the heuristic algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 It is a schematic diagram of the workflow of the path planning method of the present invention.

[0060] Figure 2 It is a schematic diagram of the robot performing local path exploration in the present invention.

[0061] Figure 3 It is a schematic diagram of the global path exploration performed by the robot in the present invention.

[0062] Figure 4 It is a schematic diagram of the path explored by the robot in the first environment of the present invention.

[0063] Figure 5 It is a schematic diagram of the path explored by the robot in the second environment of the present invention. DETAILED DESCRIPTION

[0064] In this embodiment, a robot path planning method based on the fusion of local exploration and global exploration is used. The system used is Ubuntu 20.04, ROS-Noetic, the industrial computer is the Yanling Nano-N18 industrial computer, the sensor is a Suteng RS-Helios-16P 16-line laser radar, and the mobile robot is an autonomous platform. It is programmed based on C++ language, and RVIZ is used to present the positioning and mapping results. It can be applied to many fields such as firefighting and disaster relief, mine disaster relief, and military rescue.

[0065] In this embodiment, a robot path planning method based on the fusion of local exploration and global exploration is used to divide autonomous planning into two modules. One is to construct a large-scale global sparse exploration path in the space that needs to be explored, and calculate a rough path to guide the general direction of the robot's travel. The second is to construct a local precise exploration path around the robot to find a route that allows the robot's sensors to completely cover the local exploration area. First, the global sparse exploration path is calculated, and then the global sparse exploration path is used to guide the calculation of the local precise exploration path, which can avoid repeated exploration caused by short-sightedness when exploring the space. This can not only meet the accuracy of the local exploration path, but also reduce the consumption of computing resources.

[0066] like Figure 1 As shown, in this embodiment, a robot path planning method based on the fusion of local exploration and global exploration includes the following steps:

[0067] (1) Obtain the robot's current position and environment map, use the environment map as the global exploration space, divide the global exploration space into several subspaces, use the centroid of the subspace as the global path point, and use the dynamic path planning algorithm to calculate the shortest distance between all global path points and the robot's current position; solve the traveling salesman problem through a greedy strategy to obtain the visit order between global path points and obtain the global exploration path.

[0068] The space to be explored is divided into multiple rectangular subspaces to manage the state of global exploration. Each subspace is divided into three states: unexplored, exploring, and explored according to the state of exploration, and is updated as the robot's exploration progresses. Each subspace records the information that the robot has scanned and has not scanned. If there is no robot scanning information in a subspace, it is marked as an unexplored space. If there is unexplored space in a subspace, it is marked as an exploring space. If a subspace only has scanned space and no unscanned space, it is marked as an explored space. In the strategy of global exploration path planning, only the subspace marked as being explored needs to be considered.

[0069] The planning of the global exploration path requires first determining the path points, and then planning the order of passing through the path points by calculating the shortest distance between all path points and solving the traveling salesman problem. Regarding the selection of global path points, the center point of the subspace under exploration is taken as the path point for global planning, so that local path exploration can be guided at the global level to prevent repeated exploration caused by the emergence of local optimal solutions. A dynamic path planning algorithm is used to calculate the shortest distance between all global path points and the current position of the robot; the traveling salesman problem is solved through a greedy strategy to obtain the visit order between global path points, thereby obtaining a global approximate exploration path.

[0070] (2) If Figure 2 As shown in the figure, the green box represents the local exploration area, the green block is the subspace under exploration, the space within a preset distance around the robot is used as the local exploration space, and the intersection of the global exploration path and the local exploration space boundary is used as the boundary point for entering and leaving the local exploration space. The local exploration space is uniformly sampled by radar and candidate path points are extracted. The candidate path points are selected by the local path point selection algorithm to obtain local path points; the dynamic path planning algorithm is used to calculate the shortest distance between all local path points and boundary points; the traveling salesman problem is solved by the greedy strategy to obtain the visit order between local path points and obtain the local exploration path.

[0071] The lidar samples the surrounding environment at a frequency of 10 Hz. The local path point selection algorithm is as follows: a local exploration space centered on the mobile robot is set, and all possible candidate path points are uniformly sampled and extracted in the local exploration space. For each candidate path point, the size of the unexplored area that it can cover is calculated, and the size of the area that can be covered is used as the reward for the candidate path point. The rewards of the candidate path points are sorted in descending order of priority and stored in a priority queue.

[0072] The previous step is iterated to find a set of path points with the optimal exploration path. When the boundary of the unexplored area in the space is collected by the lidar sensor, its center point represents , the corresponding normal vector is , Indicates the location of the waypoint. If the unexplored area boundary is located at , the path points are covered, and the following conditions need to be met:

[0073] (1);

[0074] in, and Represents a constraint on the distance and direction of the plane, which means that only the boundaries that are close enough to the sensor and tangent to the sensor plane are covered.

[0075] Waypoint rewards are calculated as follows:

[0076] (2);

[0077] in, For the selected Waypoints, For the selected Waypoints, Indicates waypoints The area that can be covered, Indicates waypoints The area that can be covered; The function represents the union of all sets in the brackets from b=1 to b=a-1.

[0078] The waypoint is selected according to the priority order in the priority queue, and the probability of the waypoint being selected is proportional to its reward. Because adjacent waypoints have overlapping fields of view, the reward for selecting additional waypoints decreases as more waypoints are selected. Therefore, the reward of a waypoint depends on the waypoints that have been selected before. For waypoints that are still in the queue, their rewards need to be updated through equation (2). When the priority queue is empty or the reward for adding a new candidate waypoint is negligible (less than 0.01), the waypoint selection process ends.

[0079] The path points selected in the above steps are stored in the priority queue set U. The dynamic path planning algorithm sorts the path points in the priority queue set U, and obtains the shortest distance between all local path points and boundary points according to the sorting results. The design method of the dynamic path planning algorithm is: use and Two values ​​describe the cost of a node, where It is the minimum cost value from the starting point to the current node in all current calculations. is the cost from the starting point to the current node in this calculation. and The size of the node is defined as:

[0080] (1) In the initial state, each node The value is equal to value, is in a locally consistent state, that is, .

[0081] When the obstacles in the map change, the node The value of the node will change. Value The values ​​will become inconsistent. Value Nodes with inconsistent values ​​are called locally inconsistent nodes. There are two cases for local inconsistency:

[0082] (2) The node that was originally an obstacle has its obstacles removed, which is a local over-consistent state, indicating that the node is now There is a more ideal parent node that makes the cost of getting to the starting point smaller, that is, .

[0083] (3) New obstacles are added to the originally blank nodes, which is a local inconsistent state, that is, .

[0084] For the point that was originally an obstacle, its Value and The value is infinite. After the obstacle is removed, it will be updated. Similarly, a blank node The value should be equal to value, but when it becomes an obstacle, its The value will also become infinite, which satisfies the condition of local inconsistency.

[0085] For locally over-consistent points, Update the value to The value is set, and the information of the points around the locally over-consistent points is updated at the same time. For the locally under-consistent points, The value is updated to infinity, and then the information of the points around the locally inconsistent point is updated.

[0086] The dynamic path planning algorithm updates each node in three steps:

[0087] (1) Update each node The value is calculated to query all other nodes around the node. Add the value of this node to the target node goal of The minimum sum of values; Representation Node n To the target node goal The residual is calculated as follows:

[0088] ;

[0089] in, is the location of node n, is the location of the target node goal, It is the unit normal vector of the plane formed by node n and target node goal.

[0090] (2) If this node belongs to the priority queue set , then remove it from the priority queue set Delete, because each point that needs to be updated is removed from the set The points selected from the above will naturally be removed first for the traversed points.

[0091] (3) If this point Value If the value is inconsistent, it will be placed in the priority queue set Re-evaluate in the priority queue collection. According to the node Sort the values, The values ​​are defined as follows:

[0092] (3);

[0093] For the priority queue set When sorting the nodes in The node with the smallest value, when there are multiple When the minimum nodes are equal, select Nodes with the smallest value are sorted.

[0094] The dynamic path planning algorithm reuses the previous search result information, so that it can adapt to the changes in the space without recalculating the entire exploration space, which can greatly reduce the computational resource consumption of exploration path planning in dynamic environments.

[0095] Solving the traveling salesman problem is to calculate the optimal access order for all path points so that each path point is visited once and only once. Because calculating the shortest distance between path points and optimizing the trajectory curvature will take up a lot of computing resources, in order to reduce the processing time of the algorithm, the heuristic solution method with higher computational complexity is not used. Instead, a greedy strategy is used to solve the traveling salesman problem to minimize the algorithm calculation time. The mathematical model of the traveling salesman problem can be expressed in the following linear programming form:

[0096] (4);

[0097] in, for For all non-empty subsets of For collection The number of vertices in , the solution that satisfies the above constraints constitutes a Hamiltonian circuit.

[0098] The greedy strategy for solving the traveling salesman problem is to traverse all the selected path points in the local exploration space from the current path point, select the closest path point as the next node, then mark the current path point as having been visited, and use the next path point as the current path point, and repeat this greedy strategy until the visit order of all path points in the local exploration space is calculated.

[0099] Failure to introduce curvature constraints in path planning will lead to discontinuity of path curvature, thus affecting the robot's driving. Therefore, a trajectory curvature constraint method based on piecewise Bezier curves is proposed to constrain the curvature of the trajectory between path points and optimize the robot's driving trajectory.

[0100] The segment points of a piecewise Bezier curve are calculated as follows:

[0101] Calculate local waypoints and The straight-line distance and as well as and The straight-line distance forms a triangle, and the lengths of the three sides are obtained. The cosine theorem is used to find the local path point The angle at which the path point is located is set to 90°. When the angle at is less than 90°, the path point is set as the segmentation point of the segmented Bezier curve, and the mobile robot turns in place here to reduce the problem of decreased computing performance and small influence of control points caused by the high order of the Bezier curve. The Bezier curve calculates a new trajectory optimization from here.

[0102] A safe space is generated using multiple adjacent path points, and then a curvature-continuous trajectory is generated within the safe space using Bezier curves. The specific method for generating a safe space is divided into five steps:

[0103] (1) First, use two adjacent path points as diagonals to generate an initial rectangular area.

[0104] (2) The initial rectangle expands outward along the four sides until it encounters an obstacle or reaches its upper limit.

[0105] (3) Then select the next path point and make a judgment. If the path point is in the rectangular space generated by the previous two adjacent path points, skip it. Otherwise, select the previous path point and the current path point as the diagonals to generate the initial rectangular area, and then repeat step (2).

[0106] (4) Check whether the current rectangular space contains all the path points in the previous space. If so, delete the previous rectangular space.

[0107] (5) When all path points have generated a rectangular space, x -1 space with the x +1 space intersects, then delete the x spaces to reduce the number of segments of the generated Bezier curve.

[0108] Define an interval , in the interval, by The order Bezier curve determined by the control points is defined as:

[0109] (5);

[0110] in, is the control point, is the Bernstein coefficient.

[0111] The piecewise Bezier curve is represented as:

[0112] (6);

[0113] in, For the A Bezier curve. For the The first segment of the Bezier curve control points, is the time taken by the segmented curve, is the scale factor.

[0114] The objective function is expressed as:

[0115] (7);

[0116] Normalizing the time, the above formula can be expressed as:

[0117] (8);

[0118] in, is a column vector of control points, is the Hessian matrix. The robot trajectory is adjusted by setting the range of the objective function value and solving the piecewise Bezier curve according to the constraints.

[0119] According to the multi-order differentiable nature of the Bezier curve, the position, velocity, and acceleration of the starting and ending points must meet the expected state and the position, velocity, and acceleration of the multi-end curve connection points are equal and continuous, which can ensure the smooth endpoint constraint and continuity constraint of the robot motion trajectory:

[0120] ;

[0121] ;

[0122] in, For the The position and last control point of the piecewise Bezier curve For the The position of the first control point of the piecewise Bezier curve, is the derivative order, .

[0123] According to the property that the Bezier curve is in the convex space generated by the control points, if the control points are constrained in the safety corridor, then the Bezier curve constrained by the control points will also be in the safety corridor, that is:

[0124] (11);

[0125] in, and It is The position constraints of the control points of the Bezier curve segment, that is, the generated The area of ​​a safe space.

[0126] According to the actual hardware conditions of the robot, set the speed and acceleration constraints so that the robot can better move according to the calculated trajectory, that is:

[0127] (12);

[0128] in, and are the minimum and maximum values ​​of the velocity, and are the minimum and maximum values ​​of acceleration; For the robot The position corresponding to the moment t when the segmented Bezier curve moves.

[0129] (3) If Figure 3 As shown in , the path between two boundary points in the global exploration path is replaced with the local exploration path to obtain the final planning path. After the global exploration path is generated, it is necessary to connect the global path with the local path. The intersection of the global exploration path and the local exploration space is used as the boundary point for entering and leaving the local exploration space. These two boundary points are added to the path points of the local exploration path to connect the local exploration path with the global exploration path. The global exploration path between the boundary points of the local exploration space is replaced with the local exploration path, and the local exploration is guided by the global exploration path to generate a globally consistent exploration path, as shown in Figure 4 and Figure 5 shown.

Claims

1. A robot path planning method based on the fusion of local exploration and global exploration, characterized in that: The following steps are involved: (1) Obtain the robot's current position and environment map, use the environment map as the global exploration space, divide the global exploration space into several subspaces, use the centroid of the subspace as the global path point, and use the dynamic path planning algorithm to calculate the shortest distance between all global path points and the robot's current position; solve the traveling salesman problem through a greedy strategy to obtain the visit order between global path points and obtain the global exploration path; (2) The space within a preset distance around the robot is used as the local exploration space, and the intersection of the global exploration path and the local exploration space boundary is used as the boundary point for entering and leaving the local exploration space. The local exploration space is uniformly sampled by radar and candidate path points are extracted. The candidate path points are selected by the local path point selection algorithm to obtain local path points. A dynamic path planning algorithm is used to calculate the shortest distance between all local path points and boundary points. The traveling salesman problem is solved by a greedy strategy to obtain the visit order between local path points and obtain the local exploration path. (3) Replace the path between two boundary points in the global exploration path with the local exploration path to obtain the final planning path; In the step (2), the local exploration path is divided into a number of path sub-segments through segmentation points, each path sub-segment is optimized for the trajectory of the robot during movement through a piecewise Bezier curve, and the robot turns in situ at the segmentation point to switch to the next path sub-segment; The specific steps of optimizing the trajectory of the robot during motion using the piecewise Bezier curve are as follows: Define an interval , in the interval, by Control points determined Order Bezier curve Defined as: ； in, For the control points, is the Bernstein coefficient; The piecewise Bezier curve is represented as: ； in, For the Piecewise Bezier curve, For the The first segment of the piecewise Bezier curve control points, is the time taken by the piecewise Bezier curve, For the Scaling factor of piecewise Bezier curve; The objective function is to smooth the robot trajectory. It is expressed as: ； in, is a column vector of control points, is the Hessian matrix. The robot trajectory is adjusted by setting the range of the objective function value and solving the piecewise Bezier curve according to the constraint conditions. The constraints to ensure the smoothness of the robot's motion trajectory are: The endpoint constraints and continuity constraints are: ； ； in, For the The position and last control point of the piecewise Bezier curve For the The position of the first control point of the piecewise Bezier curve, is the derivative order, ; According to the property that the Bezier curve is in the convex space generated by the control points, the control points are constrained in the safety corridor, then the Bezier curve constrained by the control points will also be in the safety corridor, that is: ； in, and It is The position constraints of the control points of the piecewise Bezier curve are generated. The area of ​​the safe space; The steps for determining the safe space are: (2.1) For each path segment, first use two adjacent path points as diagonals to generate an initial rectangular area; (2.2) The initial rectangle expands outward along the four sides until it encounters an obstacle or reaches the upper limit of expansion, resulting in a rectangular space; (2.3) For the next path point, if the path point is in the rectangular space generated by the previous two adjacent path points, it is skipped. Otherwise, the path point and its previous adjacent path point are used as diagonals to generate the initial rectangular area, and then step (2.2) is repeated; (4) Check whether the current rectangular space contains all the path points in the previous rectangular space. If all are included, delete the previous rectangular space; (5) When all path points have generated rectangular spaces, if the x-1th rectangular space is x +1 rectangular space intersects, then delete the x Rectangular spaces, all reserved rectangular spaces form a safe space.

2. The robot path planning method according to claim 1, characterized in that: The specific steps of dividing the local exploration path into a number of path sub-segments through segmentation points are: r , local path points r -1 and local waypoints r +1 The distance between two of them, calculate the local path point r The angle at which the local path point r The angle at which is less than the threshold, then the local path point r As a segmentation point, the robot turns on the spot at this point.

3. The robot path planning method according to claim 2, characterized in that: The specific method of uniformly sampling the local exploration space by radar and extracting candidate path points in step (2) is as follows: sampling the local exploration space by a lidar sensor to obtain point cloud data, calculating the size of the area that can be covered by all possible path points in the point cloud data, and taking the size of the area that can be covered by the unexplored area as the reward of the path point, and sorting the path points in descending order based on the size of the reward to obtain a priority queue; When the boundary of the unexplored area in the local space is captured by the lidar sensor, its center point represents , the corresponding normal vector is , Indicates the location of the waypoint; if the boundary is located at , the path points are covered, and the following conditions need to be met, indicating that the boundary of the unexplored area has been covered: ； ； in, and Represents constraints on plane distance and direction; The calculation method of the waypoint reward is as follows: ； in, For the selected Waypoints, For the selected Waypoints Indicates waypoints The area that can be covered, Indicates waypoints The area that can be covered; The function represents the union of all sets in the brackets from b=1 to b=a-1; Waypoints are selected according to the priority order in the priority queue, and their rewards are updated when new waypoints are selected in the priority queue; when the priority queue is empty or the reward for adding a new candidate waypoint is less than 0.01, the waypoint selection process ends and the selected local waypoint is obtained.

4. The robot path planning method according to claim 3, characterized in that: The selected local path points are stored in the priority queue set U. The dynamic path planning algorithm sorts the path points in the priority queue set U, and obtains the shortest distance between all local path points and the boundary points according to the sorting result. The dynamic path planning algorithm uses Value and The value describes the cost of a node, which includes the starting point, the end point, and the path points between the starting point and the end point of the planned path, where The value is the minimum cost from the starting point to the current node in all current calculations. Value is the cost from the starting point to the current node in this calculation; The specific steps of the dynamic path planning algorithm for updating each node are as follows: (1) Update each node The value is to calculate the value of the node among all other nodes around the node. Add the value of this node to the target node goal of The minimum sum of values; Representation Node n To the target node goal The residual is calculated as follows: ； in, For Node n Location, is the location of the target node goal, For Node n The unit normal vector of the plane formed by the target node goal; (2) If this node belongs to the priority queue set , then remove it from the priority queue set Delete from; (3) If the node Value If the value is inconsistent, it will be placed in a priority queue set reassessment in the Priority Queue Set According to the node Sort the values, The values ​​are defined as follows: ； in For node n value, For node n value; For the priority queue set When sorting the nodes in The node with the smallest value; when there are multiple When the minimum nodes are equal, select Nodes with the smallest value are sorted.

5. The robot path planning method according to claim 1, characterized in that: The method for solving the traveling salesman problem with the greedy strategy is to traverse all the selected path points in the local exploration space from the current path point through the mathematical model of the traveling salesman problem, and each path point is visited once and only once, and the shortest distance between all path points is calculated by the dynamic path planning algorithm, and the path point with the closest distance is selected as the next node, and then the current path point is marked as having been passed, and the next path point is used as the current path point, and it is repeated until the access order of all path points in the local exploration space is calculated.

6. The robot path planning method according to claim 1, characterized in that: In the step (1), the global exploration space is divided into a number of subspaces, each of which is divided into three states: unexplored, exploring and explored according to the exploration status, and is updated as the robot exploration progresses; the scanning information of the robot in each subspace is recorded, if there is no scanning information of any robot in a subspace, it is marked as an unexplored subspace, if there is space that the robot has not scanned in a subspace, it is marked as an exploring subspace, if a subspace only has space that the robot has scanned and no unscanned space, it is marked as an explored subspace, in the strategy of global exploration path planning, only the subspace marked as being explored needs to be considered; the centroid of the exploring subspace is obtained as the global path point.

7. The robot path planning method according to claim 4, characterized in that: Setting velocity and acceleration constraints makes the robot move better according to the calculated trajectory, namely: ； ； in, and are the minimum and maximum values ​​of the robot speed, and are the minimum and maximum values ​​of the robot’s acceleration; For the robot The position corresponding to the moment t when the segmented Bezier curve moves.

Images