Machine room inspection safety path planning method conforming to human body feeling based on Voronoi diagram

Abstract

The invention relates to a Voronoi diagram-based machine room inspection safety path planning method conforming to human body feeling, which comprises the following steps of: generating a global Voronoi grid map layer by utilizing a Voronoi algorithm thought based on a global static grid map, and realizing local updating of the Voronoi map layer in combination with detection of surrounding obstacles by a laser radar; after the position of the target point is obtained, searching is carried out in the global static grid map and the Voronoi map layer updated in real time through an Astar algorithm according to the Voronoi map layer updated in real time and the current pose of the vehicle, and a real-time global path is obtained; after a global path is obtained, two-stage optimization is used, human body visual feeling optimization is carried out on the path, and the path is made to accord with the path felt by the human body; and carrying out smoothness optimization on the path, so that the optimized path is suitable for tracking control. The method has the advantages that the map is processed by adopting a global Voronoi thought method according to the scanning and mapping of the laser radar on the surrounding environment, and a safe driving path can be generated in real time; and a two-stage path optimization strategy is provided, so that the problems of path redundancy and unsuitability for tracking control are effectively solved, and the robot can realize obstacle avoidance and arrive at a target point by tracking according to a given path.

Description

Technical Field

[0001] The invention belongs to the technical field of inspection path planning, and in particular relates to a computer room inspection safety path planning method based on a Voronoi diagram and in line with human perception. Background Art

[0002] With the development of Internet technology, more and more computer rooms are built to store servers. In computer rooms, the temperature is generally low, and people who stay in such an environment for a long time are prone to illness. In addition, the inspection of servers does not need to be done all the time because the demand for manpower is not high, but people are still needed to do it. In addition, manual inspections are prone to fatigue and negligence, resulting in missed inspections and false inspections. In order to ensure the safe and orderly operation of computer room equipment and reduce the waste of human resources, it has become an inevitable trend to use robots to replace manual inspections of computer rooms.

[0003] In traditional path planning, the overall map modeling method is usually used. During the planning process, the map remains unchanged. In terms of algorithm selection, there are mainly traditional algorithms and intelligent optimization algorithms. Traditional algorithms include artificial potential field method, Dijkstra algorithm, Astar algorithm, etc. Intelligent optimization algorithms include genetic algorithm, ant colony algorithm, simulated annealing algorithm, etc. These algorithms can only perform reasonable path planning when they have global map information. In the computer room environment with moving obstacles, these algorithms are currently difficult to ensure sufficient security. Summary of the invention

[0004] The technical problem to be solved by the present invention is how to reasonably plan the inspection path of the inspection robot during the inspection of the machine room so as to ensure sufficient safety.

[0005] The present invention solves the above technical problems through the following technical means: a method for planning a safe path for inspection of a computer room based on a Voronoi diagram and in accordance with human perception, characterized in that it comprises the following steps:

[0006] S1: Based on the global static grid map, the Voronoi algorithm is used to generate the global Voronoi grid map layer, and the local update of the Voronoi map layer is realized by combining the detection of surrounding obstacles by the lidar;

[0007] S2: After obtaining the location of the target point, the Astar algorithm is used to search in the global static grid map and the real-time updated Voronoi map layer according to the real-time updated Voronoi map layer and the current position of the vehicle to obtain a real-time global path;

[0008] S3: After obtaining the global path, two-level optimization is used to first optimize the path for human intuitive perception so that it conforms to the path perceived by the human body; then the smoothness is optimized so that the optimized path is suitable for tracking control.

[0009] As an optimized technical solution, step S1 specifically includes:

[0010] S101, the global grid map global_map needs to be constructed in the robot operating system in advance through the SLAM algorithm, and the Pgm image file and the Yaml map parameter file are established according to the collected environmental information. When in use, the Pgm image file and the Yaml map parameter file are loaded into Rviz through the Map_server function package, and the map information is sent out with a custom topic. After getting the sent map information, another map layer is generated according to the Voronoi algorithm idea, named Voronoi map layer;

[0011] S102: After entering the Voronoi map layer update function, first determine whether it is the first time to process the global grid map. If not, directly initialize the cost value of the stored map. If yes, read the global grid map global_map and initialize its cost value. Then, extract the contour of the global grid map global_map.

[0012] S103: Determine whether the map size has changed. If the map size has changed, re-initialize the map data to be empty. Then, combine the laser radar to scan the surrounding environment, add or remove obstacles, and update the obstacles. If the map size has not changed, directly combine the laser radar to scan the surrounding environment, add or remove obstacles, and update the obstacles.

[0013] S104: Then, determine whether it is the first time to update or receive the target;

[0014] S105: If yes, then the distance map of the entire processed map is updated. If not, only the local grid map local_map needs to be updated. After the distance map is updated, the Voronoi map layer is pruned to simplify the Voronoi edges of two grid widths to one grid width, thereby obtaining the final Voronoi map layer.

[0015] As an optimized technical solution, in step S101, the method for generating the Voronoi map layer adopts the Delaunay triangulation method, and the steps include:

[0016] 1) Automatically construct a triangulated network from discrete points, that is, construct a Delaunay triangulated network, number the discrete points and the formed triangles, and record which three discrete points each triangle is composed of;

[0017] 2) Calculate the center of the circumscribed circle of each triangle and record it;

[0018] 3) Traverse the triangle list and find the adjacent triangle that shares three sides with the current triangle;

[0019] 4) If found, connect the circumcenter of the found triangle with the circumcenter of the current triangle and store them in the Voronoi edge list. If not found, find the perpendicular bisector ray of the outermost edge and store it in the Voronoi edge list.

[0020] 5) After the traversal is completed, all Voronoi edges are found, and the Voronoi map layer is drawn based on the edges.

[0021] As an optimized technical solution, in step S103, adding and removing obstacles in combination with the laser radar scanning of the surrounding environment, and updating the obstacles specifically include:

[0022] The real-time laser point cloud data is mapped in the local grid map local_map, the radiated area is mapped and updated, the obstacle points that existed at the previous moment but are not scanned now are removed from the grid map and marked as free grids free_cell; the grids that had no obstacle points at the previous moment but are now scanned with obstacle points are marked as obstacle grids occupied_cell.

[0023] As an optimized technical solution, in step S105, the distance map update is mainly composed of two steps: distance increase and distance decrease. For the distance increase process, it is mainly aimed at the disappearance of the obstacle grid; when a new obstacle grid appears, the grids around the grid, the distances of the nearest obstacles saved will also be recalculated, so these grids are a process of distance reduction. When the distance increase and distance decrease processes meet, the grids processed by distance reduction will not be affected, but the grids processed by distance increase must consider the impact of the newly appeared obstacle grid on them, and need to recalculate the distance to the nearest obstacle. At this time, the distance increase process ends and turns into a distance reduction process. When the distance increase and distance decrease no longer progress, it means that the distance map update is over.

[0024] As an optimized technical solution, in step S2, the Astar algorithm is used to search in the global static grid map and the real-time updated Voronoi map layer to obtain the real-time global path process: mainly searching for three segments of the path, where the first segment is the vehicle's current position Ps Go to the Voronoi grid V closest to the current position s The second segment of the path Path1 is the target point P. g To the Voronoi grid V closest to the target point g The third segment of the path Path3 is the Voronoi grid V closest to the current position of the vehicle. s To the Voronoi grid P closest to the target point g Path Path2, each time a search path is obtained, it is necessary to determine whether there are obstacles at the points on the search path. If so, the search is judged to have failed. Otherwise, the three paths are concatenated to form a global planning path, and then the key path points are extracted from the global planning path using the RDP algorithm. Linear interpolation is used between adjacent key path points to obtain a global rough path.

[0025] As an optimized technical solution, in step S2, the search process of the first and third segments of the path includes: first, judging whether the incoming point is in the search space. For the first segment search, the incoming point corresponds to the starting point; for the third segment path search, the incoming point corresponds to the target point. If it is not in the search space, it is judged that the search path fails and the search needs to be replanned. Otherwise, the Astar algorithm is used to expand the node to carry out the search. Among the extended nodes, if they are not in the search space, in the previously expanded list closelist, obstacle points, and the distance between the extended node and the obstacle is less than the safe distance, then they are regarded as useless nodes and are not processed. All other nodes are put into the extended list openlist until one of the nodes has the attribute is_voronoi true, then it is judged that the target point has been searched. In the first segment path search, the target point corresponds to P s , in the third path search, the target point is P g ,After obtaining the target point, backtrack the grid points to obtain the corresponding path;

[0026] The search process of the second path includes: relying on the nearest Voronoi grid points provided by the first and third paths, taking them as the starting point and target point on the Voronoi map layer respectively, and then using the Astar algorithm to search. The rule of whether the extended node in the Astar algorithm is added to the openlist list is modified to 4 conditions: the first is to determine whether the node is in the search space, that is, whether it is in the map; the second is to determine whether the node is in the closelist list; the third is to determine whether the node is a Voronoi grid; the fourth is to determine whether the distance between the extended node and the obstacle is less than the safety threshold.

[0027] As an optimized technical solution, in step S2, the key path points are extracted from the global planning path by using the RDP algorithm, and linear interpolation is used between adjacent key path points to obtain the global rough path, which specifically includes:

[0028] 1) Select the starting point and the end point as the endpoints of the polyline segment;

[0029] 2) Calculate the distance from the point on the polyline segment to the straight line segment, and find the point p with the farthest distance;

[0030] 3) If the distance from point p to the straight line segment is less than the specified threshold, the polyline segment does not need to be further simplified and is returned;

[0031] 4) If the distance from point p to the straight line segment is greater than the specified threshold, point p is used as the inflection point and the polyline segment is divided into two sub-segments;

[0032] 5) recursively process the two sub-segments respectively to obtain simplified sub-segments;

[0033] 6) Connect the two simplified sub-segments to obtain the simplified result of the entire polyline segment;

[0034] After obtaining the key points of the path using the RDP algorithm, linear interpolation is performed on any two adjacent path points to supplement the path points of the entire path.

[0035] As a technical solution for optimization, in step S3: in the first level optimization, the distance between the path and the obstacle, the smoothness of the optimized path, and the curvature of the path are considered; in the second level optimization, the smoothness of the path, the length of the path, and the degree of overlap with the original path are considered. The cost terms in the two levels of optimization are designed as convex functions.

[0036] In the first level of optimization, the minimum distance between the path and the obstacle is set. If the distance is greater than the set value, the cost of the corresponding item increases sharply. For the curvature of the path, the cost is summed by setting the curvature that the path can accept.

[0037] In the second-level optimization, there are no artificially set values ​​for the smoothness, length, and overlap of the path. The corresponding cost terms are set as quadratic equations, with position constraints and curvature constraints added. Among the position constraints, it is necessary to set the optimization degree of the path points in the horizontal and vertical axes, the average sampling length, and the maximum curvature constraints.

[0038] As an optimized technical solution, step S3 specifically includes:

[0039] S301: After obtaining the path points planned and filtered in the Voronoi diagram, the first level optimization is performed to optimize it into a path that conforms to human perception, and gradient optimization is performed on it. The optimization objective function f is:

[0040] f=w s f smooth +w d f distance +w c f curvatrue

[0041] Among them, f smooth is the smoothness cost function, which is used to measure the smoothness of the curve, w s Indicates the weight of the cost; f distance is the cost function of the distance obstacle, w d Used to measure the safety of the curve; f curvatrue represents the curvature cost function of the path, w c Used to measure the curvature of the curve, κ max represents the maximum value of the desired path curvature; where,

[0042]

[0043] Then, the above cost terms are differentiated respectively. After obtaining the gradient, the gradient descent method is used to make the cost value of f smaller each time. The iterative process is executed repeatedly until the upper limit of the number of iterations is reached or the difference between the two cost values ​​of f drops to the specified threshold. Then the update is stopped to obtain the optimized path point.

[0044] S302: After obtaining the first-level optimized path point, the path point is represented as P k (x k ,y k ), set the cost function to:

[0045] f=w s f smooth +w l f length +w e f error

[0046] Among them, P k (x k ,y k ) represents the x and y coordinates of the kth point on the path, f length is the length cost function, which is used to measure the length of the curve, w l Indicates the weight of the cost; f error is the error cost function relative to the original path, which is used to measure the error between the optimized path point and the reference point.e Represents the weight of the cost, where:

[0047]

[0048] During the optimization, two constraints are added: position constraint and curvature constraint. The position constraint and position constraint equation are as follows:

[0049] x i-ref -x l ≤x i ≤x i-ref +x u

[0050] y i-ref -y l ≤y i ≤y i-ref +y u

[0051] (x i-1 +x i+1 -2*x i ) 2 +(y i-1 +y i+1 -2*y i ) 2 ≤(Δs 2 *cur sctr ) 2 .

[0052] The advantages of the present invention are:

[0053] 1. In view of the problem that it is difficult to consider safety in the current global path planning of robots, based on the scanning and mapping of the surrounding environment by the lidar, the global Voronoi idea method is used to process the map, which can generate a safe driving path in real time.

[0054] 2. Aiming at the defect of path planning in the global Voronoi diagram, when the channel widths in the environment vary greatly, the planned path does not conform to the path intuitively felt by the human body. Aiming at this situation, the present invention proposes a two-level path optimization strategy, which effectively solves the problems of path redundancy and unsuitability for tracking control, so that the robot can avoid obstacles and reach the target point by tracking according to the given path.

[0055] 3. The path planning method described in the present invention has excellent flexibility in actual operation, and can quickly provide a path that conforms to human perception and safety according to the needs of staff and work tasks, and realize real-time obstacle avoidance during work. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1It is an overall flow chart of a method for planning a safe path for inspection of a computer room based on a Voronoi diagram that conforms to human perception according to an embodiment of the present invention;

[0057] Figure 2 is a logic flow chart of Voronoi map layer generation according to an embodiment of the present invention;

[0058] Figure 3 It is a logic flow chart of the Astar algorithm real-time planning module of an embodiment of the present invention;

[0059] Figure 4 is a search flow chart of the first and third segments of the path according to an embodiment of the present invention;

[0060] Figure 5 is a search flow chart of the second path of an embodiment of the present invention;

[0061] Figure 6 is a schematic diagram of the global path secondary optimization of an embodiment of the present invention; DETAILED DESCRIPTION

[0062] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in combination with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0063] like Figure 1 As shown, it is an overall flow chart of the computer room inspection safety path planning method based on the Voronoi diagram that conforms to human perception of the present invention. The computer room inspection safety path planning method based on the Voronoi diagram that conforms to human perception of the present invention mainly includes Voronoi map layer generation and local update, Astar algorithm real-time planning and a two-level optimization algorithm for the global path. It includes the following steps:

[0064] S1: Based on the collected global grid map global_map, the global Voronoi map layer is initialized using the Voronoi algorithm; the Voronoi map layer is locally updated by scanning the surrounding environment through the laser radar installed on the inspection robot, providing a basis for subsequent planning and obstacle avoidance. The specific process is:

[0065] S101, the global grid map global_map needs to be constructed in advance in the Robot Operating System (ROS) through the Gmapping algorithm or SLAM algorithms such as Cartographer, and the Pgm image file and the Yaml map parameter file are established according to the collected environmental information. When in use, the Pgm image file and the Yaml map parameter file are loaded into Rviz through the Map_server function package, and the map information is sent out with a custom topic, such as / map. After getting the sent map information, another map layer can be generated according to the Voronoi algorithm idea, named Voronoi map layer.

[0066] The principle of generating Voronoi map layers according to the Voronoi algorithm is that the perpendicular bisectors of the straight lines connecting two adjacent points form continuous polygons. 1, …,P n The generator P in k According to the above principle, the corresponding Voronoi region R can be obtained k :

[0067] R k ={x∈X|d(x,P k ) <d(x,P j ),j={1,2,…,n},j≠k}

[0068] The biggest advantage of the Voronoi map layer is that the paths in the graph are composed of Voronoi edges, and the Voronoi edge is the perpendicular bisector of the discrete center of the obstacle, which can ensure that the inspection robot is as far away from obstacles as possible during operation. The application of Voronoi map layers in many fields has prompted researchers to successfully design a variety of methods for constructing Voronoi map layers. There are currently two mainstream construction methods: vector method and grid method. The more commonly used vector methods can be divided into Delaunay triangulation method, divide-and-conquer method, and scan line algorithm. According to the size of the experimental map, the generation method of the Voronoi map layer in this paper adopts the Delaunay triangulation method with less algorithm time complexity. The basic idea is to first generate the dual element Delaunay triangulation network, then find the center of the circumscribed circle of each triangle in the triangulation network, and then connect the centers of the circumscribed circles of adjacent triangles to form a polygonal network with each triangle vertex as the generator. The steps to establish the Voronoi map layer include:

[0069] 1) Automatically construct a triangulated network from discrete points, that is, construct a Delaunay triangulated network, number the discrete points and the formed triangles, and record which three discrete points each triangle is composed of;

[0070] 2) Calculate the center of the circumscribed circle of each triangle and record it;

[0071] 3) Traverse the triangle list and find the adjacent triangle that shares three sides with the current triangle;

[0072] 4) If found, connect the circumcenter of the found triangle with the circumcenter of the current triangle and store them in the Voronoi edge list. If not found, find the perpendicular bisector ray of the outermost edge and store it in the Voronoi edge list.

[0073] 5) After the traversal is completed, all Voronoi edges are found, and the Voronoi map layer is drawn based on the edges.

[0074] S102: See Figure 2 As shown, after entering the Voronoi map layer update function, first determine whether it is the first time to process the global grid map. If not, directly initialize the cost value of the stored map. If so, read the global grid map global_map and initialize its cost value. After that, extract the contour of the global grid map global_map.

[0075] S103: Determine whether the map size has changed. If so, the map data needs to be reinitialized to be empty to prevent the last stored map data from affecting the current update process. Then, obstacles are added or removed and updated in combination with the laser radar scanning of the surrounding environment. If the map size has not changed, obstacles are added or removed and updated in combination with the laser radar scanning of the surrounding environment.

[0076] In this step, the main functions of initialization are to clear historical data, open up content space, set all grids to be unoccupied, and assign initial values ​​to distinguish the mapping on the local grid map local_map when the lidar scans the surrounding environment, indicating that the environment has changed, and then edit it to the grid that needs to be updated.

[0077] The steps of adding, removing and updating obstacles mainly rely on the laser radar scanning of the surrounding environment. When the laser radar scanning point changes in the local grid map local_map, the relevant obstacle grid is added or removed.

[0078] Combined with the laser radar scanning of the surrounding environment, adding and removing obstacles, updating obstacles specifically includes:

[0079] The real-time laser point cloud data is mapped in the local grid map local_map, the radiated area is mapped and updated, the obstacle points that existed at the previous moment but are not scanned now are removed from the grid map and marked as free grids free_cell; the grids that had no obstacle points at the previous moment but are now scanned with obstacle points are marked as obstacle grids occupied_cell.

[0080] S104: Then, determine whether it is the first time to update or receive the target;

[0081] S105: If yes, then the distance map (DM) of the entire processed map is updated. If no, then only the local grid map local_map needs to be updated with DM. After the DM is updated, the Voronoi map layer is pruned to reduce the Voronoi edges of two grid widths to one grid width. Through the above steps, the final Voronoi map layer can be obtained.

[0082] In this step, the purpose is to perform a global static map update only after receiving the target or when the Voronoi map layer is updated for the first time. If it is not the case, then the local Voronoi map layer is updated based on the lidar's scanning of the surrounding environment and its mapping. In this way, the amount of calculation can be greatly reduced, the frequency of planning and map updates can be increased, and latency can be reduced.

[0083] In the DM update step, it mainly consists of two steps: raise and lower. For the raise process, it is mainly aimed at the disappearance of the obstacle grid. When this happens, the corresponding binary occupancy grid will be updated. Because the old obstacle disappears, the grids around the grid that is the nearest obstacle will temporarily have no nearest obstacle, and the saved nearest obstacle distance will also be considered invalid, so the state update of these grids is a process of increasing distance (raise); similarly, when a new obstacle grid appears, the grids around the grid, the saved nearest obstacle distance will also be recalculated, so these grids are a process of reducing distance (lower). When the raise and lower processes meet, the grids processed by the lower will not be affected, but the grids processed by the raise will consider the impact of the newly appeared obstacle grid on it, and the distance to the nearest obstacle must be recalculated, so the raise process ends and turns into the lower process. When both raise and lower no longer progress, it means that the DM update is over.

[0084] After the above steps, each grid of the Voronoi map layer contains the following information: (x, y, dist, is_voronoi, obs_x, obs_y), where x and y represent the coordinates in the global grid map global_map, dist represents the distance between the current grid and the nearest obstacle, is_voronoi represents whether the current grid is a Voronoi grid, and this flag will play an important role in the subsequent path search. obs_x and obs_y represent the grid coordinates of the nearest obstacle of the current grid.

[0085] S2: After obtaining the location of the target point, in the ROS system, subscribe to the real-time update data of the static global grid map, the target point location, the current location of the vehicle, and the Voronoi map layer. According to the real-time changes of different target points set by the user and the vehicle's own position, the Astar algorithm is used to search in the global static grid map and the real-time updated Voronoi map layer. The search is carried out in three steps, and the search results of the three steps are spliced ​​to obtain a real-time global path.

[0086] Figure 3 The real-time planning module of the Astar algorithm is shown. It mainly searches for three paths. The first path is the current position of the vehicle P. s Go to the Voronoi grid V closest to the current position s The second segment of the path Path1 is the target point P. g To the Voronoi grid V closest to the target point g The third segment of the path Path3 is the Voronoi grid V closest to the current position of the vehicle. s To the Voronoi grid P closest to the target point g Path 2. Each time a search path is obtained, it is necessary to determine whether there are obstacles at the points on the search path. If so, the search is considered to have failed. Otherwise, the three paths are concatenated to form a global planning path. Then, the RDP algorithm is used to extract key path points from the global planning path. Linear interpolation is used between adjacent key path points to obtain a global rough path as the basis for subsequent optimization. The specific process is as follows:

[0087] S201: Figure 4The search process of the first and third paths is shown. First, determine whether the incoming point is in the search space. For the first search, the incoming point corresponds to the starting point; for the third path search, the incoming point corresponds to the target point. If it is not in the search space, then it is determined that the search path has failed and the search needs to be replanned. Otherwise, use the Astar algorithm to expand the nodes and conduct the search. Among the extended nodes, if they are not in the search space, in the previously expanded list closelist, or are obstacle points, and the distance between the extended node and the obstacle is less than the safe distance, then they are regarded as useless nodes and are not processed. All other nodes are put into the extended list openlist until one of the nodes has the attribute is_voronoi set to true, then it can be determined that the target point has been searched. In the first path search, the target point corresponds to P s , in the third path search, the target point is P g After obtaining the target point, the corresponding path can be obtained by backtracking the grid points.

[0088] S202: Figure 5 The search process of the second path is shown. In this path search process, the closest Voronoi grid points provided by the first and third paths are mainly relied on, which are used as the starting point and target point on the Voronoi map layer respectively, and then the Astar algorithm is used for search. The rule of whether the extended node is added to the openlist list in the Astar algorithm has been modified to 4 conditions. The first is to determine whether the node is in the search space, that is, whether it is in the map; the second is to determine whether the node is in the closelist list. The third is to determine whether the node is a Voronoi grid. The fourth is to determine whether the distance between the extended node and the obstacle is less than the safety threshold. The safety threshold here is usually based on the vehicle radius as a parameter, which is also the main key point for the Astar algorithm to search for a safe path in the Voronoi map layer.

[0089] S203: After obtaining the three paths, the three paths are spliced ​​according to the connectivity between the starting point and the end point of each path, and then it is determined whether the starting point and the target point of the spliced ​​path match the given starting point and the target point. If they do not match, it is determined that the search fails. If they match, then proceed to the next step.

[0090] S204: After obtaining the three-segment splicing path, in order to make the path smoother, it is necessary to use the RDP algorithm and linear interpolation to further roughly process the path. The RDP algorithm is a commonly used broken line simplification algorithm, which can be used to smooth a broken line segment composed of several points to obtain a more concise broken line segment. The basic idea is that for a given broken line segment, first find the farthest point in the broken line segment and use this point as the turning point. Then, divide the broken line segment into two sub-segments, and recursively process these two sub-segments until the distance of all points in the sub-segments is less than the specified threshold. Specifically, the process of the RDP algorithm is as follows:

[0091] 1) Select the starting point and the end point as the endpoints of the polyline segment;

[0092] 2) Calculate the distance from the point on the polyline segment to the straight line segment, and find the point p with the farthest distance;

[0093] 3) If the distance from point p to the straight line segment is less than the specified threshold, the polyline segment does not need to be further simplified and is returned;

[0094] 4) If the distance from point p to the straight line segment is greater than the specified threshold, point p is used as the inflection point and the polyline segment is divided into two sub-segments;

[0095] 5) recursively process the two sub-segments respectively to obtain simplified sub-segments;

[0096] 6) Connect the two simplified sub-segments to obtain the simplified result of the entire polyline segment.

[0097] After using the RDP algorithm to obtain the key points of the path, linear interpolation is performed on the adjacent path points in pairs to supplement the path points of the entire path to make it denser for subsequent optimization.

[0098] S3: In view of the problem that the processed global path in a specific scenario may not conform to the human intuitive feeling and is not suitable for tracking control, a two-stage optimization method is proposed, that is, firstly optimize the path for human intuitive feeling to make it conform to the human feeling; then optimize its smoothness to make the optimized path suitable for tracking control. Specifically:

[0099] S301: After obtaining the path points planned and filtered in the Voronoi diagram, perform the first level optimization to optimize it into a path that meets the human body's perception and perform gradient optimization on it. The optimization objective function f is:

[0100] f=w s f smooth +w d f distance +w c f curvatrue

[0101] Among them, f smooth is the smoothness cost function, which is used to measure the smoothness of the curve, w s Indicates the weight of the cost; f distance is the cost function of the distance obstacle, w d Used to measure the safety of the curve; f curvatrue represents the curvature cost function of the path, w c Used to measure the curvature of the curve, κ max Indicates the maximum desired path curvature.

[0102] in,

[0103]

[0104] Then, the above cost terms are derived respectively, and after obtaining the gradient, the gradient descent method is used to make the cost value of f smaller each time. The iterative process is executed repeatedly until the upper limit of the number of iterations is reached or the difference between the two cost values ​​of f drops to the specified threshold, then the update is stopped and the optimized path point is obtained.

[0105] S302: After obtaining the first-level optimized path, the path will be more in line with the intuitive feeling of the human body. However, the performance of the path is not suitable for vehicle tracking, and further optimization processing of the path is required, such as Figure 6 As shown, it is optimized into a path that is more suitable for tracking control.

[0106] After obtaining the first-level optimized path point, the path point can be expressed as P k (x k ,y k ), set the cost function to:

[0107] f=w s f smooth +w l f length +w e f error

[0108] Among them, P k (x k ,y k ) represents the x and y coordinates of the kth point on the path. smooth is the smoothness cost function, which is used to measure the smoothness of the curve, w s Indicates the weight of the cost; f length is the length cost function, which is used to measure the length of the curve, w l Indicates the weight of the cost; f erroris the error cost function relative to the original path, which is used to measure the error between the optimized path point and the reference point. e Indicates the weight of the cost.

[0109]

[0110] During the optimization, two constraints are added: position constraint and curvature constraint. The position constraint and position constraint equation are as follows:

[0111] x i-ref -x l ≤x i ≤x i-ref +x u

[0112] y i-ref -y l ≤y i ≤y i-ref +y u

[0113] (x i-1 +x i+1 -2*x i ) 2 +(y i-1 +y i+1 -2*y i ) 2 ≤(Δs 2 *cur sctr ) 2 .

[0114] Based on the same inventive concept, the present invention also provides a computer device, which includes: one or more processors, and a memory for storing one or more computer programs; the program includes program instructions, and the processor is used to execute the program instructions stored in the memory. The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, which is used to implement one or more instructions, specifically for loading and executing one or more instructions in a computer storage medium to implement the above method.

[0115] It needs to be further explained that, based on the same inventive concept, the present invention also provides a computer storage medium, on which a computer program is stored, and the computer program executes the above method when executed by a processor.

[0116] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for planning a safe path for inspection in a computer room based on a Voronoi diagram that conforms to human perception, characterized in that: The following steps are involved: S1: Based on the global static grid map, the Voronoi algorithm is used to generate the global Voronoi grid map layer, and the local update of the Voronoi map layer is realized by combining the detection of surrounding obstacles by the lidar; S2: After obtaining the location of the target point, the Astar algorithm is used to search in the global static grid map and the real-time updated Voronoi map layer according to the real-time updated Voronoi map layer and the current position of the vehicle to obtain a real-time global path; S3: After obtaining the global path, two-level optimization is used to first optimize the path for human intuitive perception so that it conforms to the path perceived by the human body; then the smoothness is optimized so that the optimized path is suitable for tracking control.

2. The method for planning a safe path for inspection in a computer room based on a Voronoi diagram and in accordance with human perception as claimed in claim 1, characterized in that: The step S1 specifically includes: S101, the global grid map global_map needs to be constructed in the robot operating system in advance through the SLAM algorithm, and the Pgm image file and the Yaml map parameter file are established according to the collected environmental information. When in use, the Pgm image file and the Yaml map parameter file are loaded into Rviz through the Map_server function package, and the map information is sent out with a custom topic. After getting the sent map information, another map layer is generated according to the Voronoi algorithm idea, named Voronoi map layer; S102: After entering the Voronoi map layer update function, first determine whether it is the first time to process the global grid map. If not, directly initialize the cost value of the stored map. If yes, read the global grid map global_map and initialize its cost value. Then, extract the contour of the global grid map global_map. S103: Determine whether the map size has changed. If the map size has changed, re-initialize the map data to be empty. Then, combine the laser radar to scan the surrounding environment, add or remove obstacles, and update the obstacles. If the map size has not changed, directly combine the laser radar to scan the surrounding environment, add or remove obstacles, and update the obstacles. S104: Then, determine whether it is the first time to update or receive the target; S105: If yes, then the distance map of the entire processed map is updated. If not, only the local grid map local_map needs to be updated. After the distance map is updated, the Voronoi map layer is pruned to simplify the Voronoi edges of two grid widths to one grid width, thereby obtaining the final Voronoi map layer.

3. The method for planning a safe path for inspection in a computer room based on a Voronoi diagram and in accordance with human perception as claimed in claim 2, characterized in that: In step S101, the method for generating the Voronoi map layer adopts the Delaunay triangulation method, and the steps include: 1) Automatically construct a triangulated network from discrete points, that is, construct a Delaunay triangulated network, number the discrete points and the formed triangles, and record which three discrete points each triangle is composed of; 2) Calculate the center of the circumscribed circle of each triangle and record it; 3) Traverse the triangle list and find the adjacent triangle that shares three sides with the current triangle; 4) If found, connect the circumcenter of the found triangle with the circumcenter of the current triangle and store them in the Voronoi edge list. If not found, find the perpendicular bisector ray of the outermost edge and store it in the Voronoi edge list. 5) After the traversal is completed, all Voronoi edges are found, and the Voronoi map layer is drawn based on the edges.

4. The method for planning a safe path for inspection in a computer room based on a Voronoi diagram and in accordance with human perception as claimed in claim 2, characterized in that: In step S103, adding and removing obstacles in combination with the laser radar scanning of the surrounding environment, and updating the obstacles specifically include: The real-time laser point cloud data is mapped in the local grid map local_map, the radiated area is mapped and updated, the obstacle points that existed at the previous moment but are not scanned now are removed from the grid map and marked as free grids free_cell; the grids that had no obstacle points at the previous moment but are now scanned with obstacle points are marked as obstacle grids occupied_cell.

5. The method for planning a safe path for inspection in a computer room based on a Voronoi diagram and in accordance with human perception as claimed in claim 2, characterized in that: In step S105, the distance map update mainly consists of two steps: distance increase and distance decrease. For the distance increase process, it is mainly aimed at the disappearance of the obstacle grid; when a new obstacle grid appears, the grids around the grid, and the distances of the nearest obstacles saved will also be recalculated, so these grids are a process of distance reduction. When the distance increase and distance decrease processes meet, the grids processed by distance reduction will not be affected, but the grids processed by distance increase must consider the impact of the newly appeared obstacle grid on them, and need to recalculate the distance to the nearest obstacle. At this time, the distance increase process ends and turns into the distance reduction process. When the distance increase and distance decrease no longer progress, it means that the distance map update is over.

6. The method for planning a safe path for inspection in a computer room based on a Voronoi diagram and in accordance with human perception as claimed in claim 1, characterized in that: In step S2, the Astar algorithm is used to search in the global static grid map and the real-time updated Voronoi map layer to obtain the real-time global path process: mainly searching for three segments of the path, where the first segment is the vehicle's current position P s Go to the Voronoi grid V closest to the current position s The second segment of the path Path1 is the target point P. g To the Voronoi grid V closest to the target point g The third segment of the path Path3 is the Voronoi grid V closest to the current position of the vehicle. s To the Voronoi grid P closest to the target point g Path Path2, each time a search path is obtained, it is necessary to determine whether there are obstacles at the points on the search path. If so, the search is judged to have failed. Otherwise, the three paths are concatenated to form a global planning path, and then the key path points are extracted from the global planning path using the RDP algorithm. Linear interpolation is used between adjacent key path points to obtain a global rough path.

7. The method for planning a safe path for inspection in a computer room based on a Voronoi diagram and in accordance with human perception as claimed in claim 6, characterized in that: In step S2, the search process of the first and third segments of the path includes: first, judging whether the incoming point is in the search space. For the first segment search, the incoming point corresponds to the starting point; for the third segment path search, the incoming point corresponds to the target point. If it is not in the search space, it is judged that the search path fails and the search needs to be replanned. Otherwise, the Astar algorithm is used to expand the node to carry out the search. Among the extended nodes, if they are not in the search space, in the previously expanded list closelist, or are obstacle points, and the distance between the extended node and the obstacle is less than the safe distance, then they are regarded as useless nodes and are not processed. All other nodes are put into the extended list openlist until one of the nodes has the attribute is_voronoi true, then it is judged that the target point has been searched. In the first segment path search, the target point corresponds to P s , in the third path search, the target point is P g ,After obtaining the target point, backtrack the grid points to obtain the corresponding path; The search process of the second path includes: relying on the nearest Voronoi grid points provided by the first and third paths, taking them as the starting point and target point on the Voronoi map layer respectively, and then using the Astar algorithm to search. The rule of whether the extended node in the Astar algorithm is added to the openlist list is modified to 4 conditions: the first is to determine whether the node is in the search space, that is, whether it is in the map; the second is to determine whether the node is in the closelist list; the third is to determine whether the node is a Voronoi grid; the fourth is to determine whether the distance between the extended node and the obstacle is less than the safety threshold.

8. The method for planning a safe path for inspection in a computer room based on a Voronoi diagram and in accordance with human perception as claimed in claim 6, characterized in that: In step S2, the key path points are extracted from the global planning path by using the RDP algorithm, and linear interpolation is used between adjacent key path points to obtain the global rough path, which specifically includes: 1) Select the starting point and the end point as the endpoints of the polyline segment; 2) Calculate the distance from the point on the polyline segment to the straight line segment, and find the point p with the farthest distance; 3) If the distance from point p to the straight line segment is less than the specified threshold, the polyline segment does not need to be further simplified and is returned; 4) If the distance from point p to the straight line segment is greater than the specified threshold, point p is used as the inflection point and the polyline segment is divided into two sub-segments; 5) recursively process the two sub-segments respectively to obtain simplified sub-segments; 6) Connect the two simplified sub-segments to obtain the simplified result of the entire polyline segment; After obtaining the key points of the path using the RDP algorithm, linear interpolation is performed on any two adjacent path points to supplement the path points of the entire path.

9. The method for planning a safe path for inspection in a computer room based on a Voronoi diagram and in accordance with human perception as claimed in claim 1, characterized in that: In step S3: in the first level optimization, the distance between the path and the obstacle, the smoothness of the optimized path, and the curvature of the path are considered; in the second level optimization, the smoothness of the path, the length of the path, and the degree of overlap with the original path are considered. The cost terms in the two levels of optimization are designed as convex functions; In the first level of optimization, the minimum distance between the path and the obstacle is set. If the distance is greater than the set value, the cost of the corresponding item increases sharply. For the curvature of the path, the cost is summed by setting the curvature that the path can accept. In the second-level optimization, there are no artificially set values ​​for the smoothness, length, and overlap of the path. The corresponding cost terms are set as quadratic equations, with position constraints and curvature constraints added. Among the position constraints, it is necessary to set the optimization degree of the path points in the horizontal and vertical axes, the average sampling length, and the maximum curvature constraints.

10. The method for planning a safe path for inspection in a computer room based on a Voronoi diagram and in accordance with human perception as claimed in claim 9, characterized in that: Step S3 specifically includes: S301: After obtaining the path points planned and filtered in the Voronoi diagram, the first level optimization is performed to optimize it into a path that conforms to human perception, and gradient optimization is performed on it. The optimization objective function f is: f=w s f smooth +w d f distance +w c f curvatrue Among them, f smooth is the smoothness cost function, which is used to measure the smoothness of the curve, w s Indicates the weight of the cost; f distance is the cost function of the distance obstacle, w d Used to measure the safety of the curve; f curvatrue represents the curvature cost function of the path, w c Used to measure the curvature of the curve, κ max represents the maximum value of the desired path curvature; where, Then, the above cost terms are differentiated respectively. After obtaining the gradient, the gradient descent method is used to make the cost value of f smaller each time. The iterative process is executed repeatedly until the upper limit of the number of iterations is reached or the difference between the two cost values ​​of f drops to the specified threshold. Then the update is stopped to obtain the optimized path point. S302: After obtaining the first-level optimized path point, the path point is represented as P k (x k ,y k ), set the cost function to: f=w s f smooth +w l f length +w e f error Among them, P k (x k ,y k ) represents the x and y coordinates of the kth point on the path, f length is the length cost function, which is used to measure the length of the curve, w l Indicates the weight of the cost; f error is the error cost function relative to the original path, which is used to measure the error between the optimized path point and the reference point. e Represents the weight of the cost, where: During the optimization, two constraints are added: position constraint and curvature constraint. The position constraint and position constraint equation are as follows: x i-ref -x l ≤x i ≤x i-ref +x u and i-ref -and l ≤y i ≤y i-ref +y u (x i-1 +x i+1 -2*x i ) 2 +(and i-1 +y i+1 -2*y i ) 2 ≤(Δs 2 *cur sctr ) 2 。

Images