Multi-algorithm fusion obstacle avoidance method for construction equipment in tunnel environment

Abstract

The invention discloses a multi-algorithm fusion obstacle avoidance method for construction equipment in a tunnel environment, and relates to the technical field of automation of tunnel construction equipment. Comprising the following steps: acquiring point cloud data of tunnel internal environment information, and establishing a grid map; generating a global path through a global path planner integrated with an A * algorithm; establishing a local cost map, inputting the global path into the local cost map, and judging whether the obstacle is located in the global path by detecting cost values of path points on the global path; if the obstacle is located on the global path, correcting the global path through a local path device, and generating a local path for avoiding the obstacle; otherwise, directly endowing the global path to the local path; and based on the generated local path, a speed instruction is generated and fed back to the motion chassis, so that accurate obstacle avoidance of the construction equipment is realized. According to the method, the global path is generated through the global path planner integrating the A * algorithm, and autonomous path planning of the construction equipment in the tunnel environment is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of tunnel construction equipment automation, and particularly to an obstacle avoidance method for multi-algorithm fusion of construction equipment in a tunnel environment. Background Art

[0002] With the rapid development of tunnel construction in China, tunnel construction has become an important field of infrastructure construction. However, the tunnel construction environment is complex and harsh, facing a series of challenges such as narrow space, poor ventilation, and frequent obstacles. This makes the traditional manual operation mode not only inefficient but also has great potential safety hazards. Therefore, unmanned construction has gradually become the future development trend of tunnel construction, which can effectively improve construction efficiency, reduce personnel risks, and achieve higher construction accuracy.

[0003] However, when performing unmanned construction in a tunnel environment, construction equipment faces various uncertain factors such as dynamic obstacles and complex terrain, and a single obstacle avoidance algorithm often has difficulty dealing with these complex situations. For example, traditional obstacle avoidance methods are prone to problems such as inaccurate path planning and slow obstacle avoidance reaction in the case of dense obstacles or large environmental changes. Therefore, relying solely on a single algorithm for obstacle avoidance can no longer meet the actual needs of tunnel construction.

[0004] Therefore, proposing an obstacle avoidance method for multi-algorithm fusion of construction equipment in a tunnel environment to solve the difficulties existing in the prior art is an urgent problem to be solved by those skilled in the art. Summary of the Invention

[0005] In view of this, the present invention provides an obstacle avoidance method for multi-algorithm fusion of construction equipment in a tunnel environment, which solves the problems of inaccurate path planning and slow obstacle avoidance reaction in the tunnel construction environment by a single obstacle avoidance algorithm.

[0006] In order to achieve the above object, the present invention adopts the following technical solutions:

[0007] An obstacle avoidance method for multi-algorithm fusion of construction equipment in a tunnel environment, comprising the following steps:

[0008] Use a lidar to scan the tunnel environment, obtain the point cloud data of the internal environment information of the tunnel, and establish a grid map with obstacle information and movement boundaries;

[0009] Based on the established grid map, generate a global path from the starting point to the ending point through a global path planner integrating the A* algorithm;

[0010] Through the lidar mounted on the construction equipment, the point cloud data around the construction equipment is obtained in real time, a local cost map is established, the global path is input into the local cost map, and by detecting the cost values of the path points on the global path, it is judged whether there are obstacles on the global path;

[0011] If the obstacle is on the global path, the local path planner integrating the APF and TEB algorithms corrects the global path to generate a local path that avoids the obstacle; otherwise, the global path is directly assigned to the local path.

[0012] Based on the generated local path, through the path tracking algorithm, speed commands are generated and fed back to the motion chassis, thereby achieving accurate obstacle avoidance for the construction equipment.

[0013] Optionally, the specific content of using a lidar to scan the tunnel environment, obtain the point cloud data of the internal tunnel environment information, and establish a grid map with obstacle information and movement boundaries is as follows:

[0014] Use a lidar to scan the tunnel environment to obtain the tunnel contour point cloud data and obstacle point cloud data. Use the Gmapping algorithm based on particle filtering to construct a grid map of the tunnel environment, and generate a two-dimensional grid map containing obstacle information and movement boundaries.

[0015] Optionally, the specific content of generating a global path from the starting point to the end point through a global path planner integrating the A* algorithm based on the established grid map is as follows:

[0016] The A* algorithm is used to calculate the optimal path from the starting point to the end point in the grid map containing obstacle information and movement boundaries. By heuristic search to balance the path length and estimated cost, the path with the lowest cost is found, which is the global path. The formula of the A* algorithm is:

[0017] f(n) = g(n) + h(n)

[0018] In the formula, f(n) is the total evaluation cost of a node n, balancing the known cost and future estimated cost, and is used for sorting and selection in the priority queue; g(n) is the actual path cost from the starting point to the current node n, reflecting the known cost of the path and representing the true cost of reaching the current node; h(n) is the cost estimate value from the current node n to the target node, reflecting the future cost of the path.

[0019] Optionally, through the lidar mounted on the construction equipment, the point cloud data around the construction equipment is obtained in real time, a local cost map is established, and the global path is input into the local cost map. By detecting the cost values of the path points on the global path, the specific content of determining whether the obstacle is on the global path is as follows:

[0020] Through the lidar mounted on the construction equipment, the point cloud data of the surrounding environment of the construction equipment is collected in real time, and a local cost map is constructed using the point cloud data. The local cost map represents the surrounding environment of the construction equipment in a grid form, where the cost value of each grid reflects the passability or danger level of the path position.

[0021] Through the method of coordinate transformation, the global path is mapped from the global coordinate system to the coordinate system of the local cost map. Using the real-time position and attitude information of the construction equipment, a transformation matrix between the global coordinate system and the local cost map coordinate system is constructed. The formula of the transformation matrix is:

[0022]

[0023] In the formula, (x g , y g ) is a point on the global coordinate system, (x l , y l ) is a point on the local cost map coordinate system, (x t , y t ) is the position of the construction equipment on the global coordinate system, and θ is the orientation angle of the construction equipment on the global coordinate system;

[0024] The cost value of the grid where the path point is located is detected in real time, so as to judge whether the obstacle is on the global path.

[0025] Optionally, when the obstacle is on the global path, the global path is corrected by the local pathfinder integrating the APF and TEB algorithms, and the specific steps for generating the local path to avoid the obstacle are as follows:

[0026] a. Initialization: Input the global path, local cost map and the pose of the construction equipment;

[0027] b. Target point extraction: Extract the point with the closest distance and no obstacle in the global path as the target point;

[0028] c. Potential field calculation: Use the APF algorithm to calculate the attraction of the target point and the repulsion of the obstacle, and generate a resultant force field;

[0029] d. Path generation: Generate a path according to the direction of the resultant force field;

[0030] e. Path smoothing: Use the TEB algorithm to optimize the generated path by minimizing the path length, obstacle avoidance cost and speed smoothing cost;

[0031] f. Path publishing: Publish the optimized local path to obtain the local path for avoiding obstacles.

[0032] Optionally, based on the generated local path, through the path tracking algorithm, a speed command is generated and fed back to the motion chassis, so as to realize the accurate obstacle avoidance of the construction equipment. The specific content is:

[0033] Through coordinate transformation, the local path is mapped to the coordinate system of the local cost map to obtain the specific positions of each coordinate point in the local path. Based on the relative position relationship between the construction equipment and the local path coordinate points, accurate obstacle avoidance of the construction equipment is achieved. The path tracking algorithm formula is as follows:

[0034] ω = α·(θ path - θ)

[0035] v = β·d

[0036] In the formula, ω is the angular velocity, v is the linear velocity, α and β are gain coefficients, d is the distance from the equipment to the path point, θ path and θ are the path direction angle and the equipment orientation angle respectively.

[0037] As can be seen from the above technical solutions, compared with the prior art, the present invention provides an obstacle avoidance method for multi-algorithm fusion of construction equipment in a tunnel environment, having the following beneficial effects:

[0038] (1) The present invention generates a global path through the global path planner integrating the A* algorithm, realizing the autonomous route planning of the construction equipment in the tunnel environment;

[0039] (2) The present invention generates a local path through the local path planner integrating the APF and TEB algorithms, realizing obstacle avoidance and path adjustment when the construction equipment encounters obstacles;

[0040] (3) The present invention generates and publishes speed commands through the path tracking algorithm, realizing the precise path tracking and control of the construction equipment;

[0041] (4) The present invention provides a solid foundation for the automation of tunnel construction equipment, forming an obstacle avoidance method with high reliability and high accuracy, effectively reducing the cost and danger in the tunnel construction process. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are only the embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained according to the provided drawings without creative efforts.

[0043] Figure 1 It is a flowchart of an obstacle avoidance method for multi-algorithm fusion of construction equipment in a tunnel environment provided by the present invention;

[0044] Figure 2 It is a flowchart of the local path planner for correcting the path provided by the present invention;

[0045] Figure 3Schematic diagram of the local cost map provided by the present invention. Detailed implementation manners

[0046] Next, in combination with the accompanying drawings in the embodiments of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0047] Referring to Figure 1 As shown, the present invention discloses an obstacle avoidance method for multi-algorithm fusion of construction equipment in a tunnel environment, including the following steps:

[0048] Use a lidar to scan the tunnel environment, obtain the point cloud data of the internal environment information of the tunnel, and establish a grid map with obstacle information and action boundaries;

[0049] Based on the established grid map, through a global path planner integrating the A* algorithm, generate a global path from the starting point to the ending point;

[0050] Through the lidar mounted on the construction equipment, the point cloud data around the construction equipment is obtained in real time, a local cost map is established, the global path is input into the local cost map, and by detecting the cost values of the path points on the global path, it is judged whether there are obstacles on the global path;

[0051] If there are obstacles on the global path, correct the global path through a local path planner integrating the APF and TEB algorithms to generate a local path for avoiding obstacles; otherwise, directly assign the global path to the local path;

[0052] Based on the generated local path, through a path tracking algorithm, generate a speed command and feedback it to the motion chassis, so as to realize accurate obstacle avoidance of the construction equipment.

[0053] Further, the specific content of using a lidar to scan the tunnel environment, obtaining the point cloud data of the internal environment information of the tunnel, and establishing a grid map with obstacle information and action boundaries is as follows:

[0054] Use a lidar to scan the tunnel environment, obtain the tunnel contour point cloud data and obstacle point cloud data, and use the Gmapping algorithm based on particle filtering to construct a grid map of the tunnel environment to generate a two-dimensional grid map containing obstacle information and action boundaries.

[0055] Specifically, a lidar is used to scan the tunnel to collect the point cloud data of the tunnel contour and obstacles. By processing the point cloud data, the structural features inside the tunnel are obtained, including the walls on both sides of the tunnel and other fixed obstacles. After the point cloud data is collected, the Gmapping algorithm based on particle filtering is used to construct a grid map of the tunnel environment, generating a two-dimensional grid map containing obstacle information and movement boundaries. The grid map represents the environment in the form of a grid, where the value of each grid corresponds to its occupancy status: black represents the obstacle area (such as tunnel walls and fixed equipment), white represents the passable free area, and gray represents the unknown area (not covered by the sensor).

[0056] Furthermore, based on the established grid map, the specific content of generating the global path from the starting point to the ending point through the global path planner integrating the A* algorithm is as follows:

[0057] The A* algorithm is used to calculate the optimal path from the starting point to the ending point in the grid map containing obstacle information and movement boundaries. By heuristic search to balance the path length and estimated cost, the path with the lowest cost is found, which is the global path. The formula of the A* algorithm is:

[0058] f(n) = g(n) + h(n)

[0059] In the formula, f(n) is the total evaluation cost of a node n, balancing the known cost and future estimated cost, and is used for the sorting and selection of the priority queue; g(n) is the actual path cost from the starting point to the current node n, reflecting the known cost of the path and representing the real cost of reaching the current node; h(n) is the cost estimated value from the current node n to the target node, reflecting the future cost of the path.

[0060] Specifically, in the path search problem, the cost is the distance.

[0061] Furthermore, as Figure 3 shown, through the lidar mounted on the construction equipment, the point cloud data around the construction equipment is obtained in real time, a local cost map is established, and the global path is input into the local cost map. By detecting the cost values of the path points on the global path, the specific content of judging whether there are obstacles on the global path is as follows:

[0062] Through the lidar mounted on the construction equipment, the point cloud data of the surrounding environment of the construction equipment is collected in real time, and the local cost map is constructed using the point cloud data. The local cost map represents the surrounding environment of the construction equipment in the form of a grid, where the cost value of each grid reflects the passability or danger level of the path position;

[0063] By means of coordinate transformation, the global path is mapped from the global coordinate system to the coordinate system of the local cost map. Using the real-time position and attitude information of the construction equipment, a transformation matrix between the global coordinate system and the local cost map coordinate system is constructed. The formula of the transformation matrix is:

[0064]

[0065] In the formula, (x g , y g ) is a point on the global coordinate system, (x l , y l ) is a point on the local cost map coordinate system, (x t , y t ) is the position of the construction equipment on the global coordinate system, and θ is the orientation angle of the construction equipment on the global coordinate system;

[0066] The cost value of the grid where the path point is located is detected in real time, so as to judge whether there is an obstacle on the global path.

[0067] Specifically, the local cost map represents the surrounding environment of the construction equipment in the form of grids. The cost value of each grid reflects the passability or danger degree of the path position. A cost value of 0 for a grid indicates that the area is a passable area and the construction equipment can pass freely; a cost value of 254 indicates that the area is an obstacle area and the construction equipment cannot enter; a cost value of 253 indicates that the area is an inflated no-go area and the construction equipment cannot enter, serving as a safety reserve for distance; a cost value of 1 - 252 indicates that the area is a cost-decreasing area, and the construction equipment will calculate the cost when passing through this area. By means of coordinate transformation, the global path is mapped from the global coordinate system to the coordinate system of the local cost map. The transformation matrix is applied to each path point in the global path to complete the mapping from the global coordinates to the local cost map coordinates, thereby converting the global path points to the local cost map. During the forward movement of the construction equipment, the cost value of the grid where the path point is located is detected in real time, and it can be dynamically judged whether there is an obstacle on the global path. If the cost value is high, it indicates that the current path may be blocked by an obstacle, and obstacle avoidance measures need to be taken.

[0068] Furthermore, as Figure 2 shown, when an obstacle is on the global path, the global path is corrected by the local path planner integrating the APF and TEB algorithms, and the specific steps to generate a local path to avoid the obstacle are as follows:

[0069] a. Initialization: Input the global path, local cost map, and the pose of the construction equipment;

[0070] b. Target point extraction: Extract the point closest to the global path and without obstacles as the target point;

[0071] c. Potential field calculation: Use the APF algorithm to calculate the attraction of the target point and the repulsion of the obstacles, and generate a resultant force field;

[0072] d. Path generation: Generate a path according to the direction of the resultant force field;

[0073] e. Path smoothing: Use the TEB algorithm to optimize the generated path by minimizing the path length, obstacle avoidance cost, and speed smoothing cost;

[0074] f. Path publishing: Publish the optimized local path to obtain a local path that avoids obstacles.

[0075] Specifically, the formula for using the APF algorithm to calculate the attraction of the target point and the repulsion of the obstacles and generate a resultant force field is:

[0076] F att =-k att ·(p - p goal )

[0077]

[0078] F total =F att +∑F rep

[0079] In the formula, F att is the attraction vector, representing the attraction of the target point to the construction equipment, k att is the attraction gain coefficient, p is the current position of the construction equipment, p goal is the position of the target point, F rep is the repulsion vector, representing the repulsion of the obstacle to the construction equipment, k rep is the repulsion gain coefficient, d(p) is the distance from the construction equipment to the obstacle, d 0 is the threshold distance of the repulsion effect, p obs is the position of the obstacle, F total is the resultant force vector, and ∑F rep represents the resultant force of the repulsions generated by all obstacles. The APF algorithm optimizes the path by defining the target and obstacle potential fields for the moving object, minimizing the target potential energy, and avoiding obstacles.

[0080] The formula for using the TEB algorithm to optimize the generated path by minimizing the path length, obstacle avoidance cost, and speed smoothing cost is:

[0081]

[0082] In the formula, p is the path parameter, representing the coordinates of each key point on the path, c smooth is the cost of path smoothness, c collision is the obstacle avoidance cost, ccontrol To control the cost of input minimization, ω smooth , ω collision , ω control are corresponding weights to adjust the influence of each optimization objective. The TEB algorithm adjusts the path through iterative optimization to minimize the cost function and generate an efficient and safe path.

[0083] Furthermore, based on the generated local path, through the path tracking algorithm, speed commands are generated and fed back to the motion chassis, so as to achieve the specific content of accurate obstacle avoidance of construction equipment as follows:

[0084] Through coordinate transformation, the local path is mapped to the coordinate system of the local cost map to obtain the specific positions of each coordinate point in the local path. Based on the relative position relationship between the construction equipment and the coordinate points of the local path, accurate obstacle avoidance of the construction equipment is achieved. The formula of the path tracking algorithm is:

[0085] ω = α·(θ path - θ)

[0086] v = β·d

[0087] In the formula, ω is the angular velocity, v is the linear velocity, α and β are gain coefficients, d is the distance from the device to the path point, θ path and θ are the path direction angle and the device orientation angle respectively.

[0088] In this specification, each embodiment is described in a progressive manner. The key point of each embodiment is to illustrate the differences from other embodiments. The same or similar parts among the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple. For the relevant parts, refer to the description of the method part.

[0089] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present invention. Various modifications to these embodiments will be obvious to those skilled in the art. The general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to these embodiments shown herein, but will be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. An obstacle avoidance method for construction equipment in a tunnel environment by integrating multiple algorithms, characterized in that: The following steps are involved: Use laser radar to scan the tunnel environment, obtain point cloud data of the tunnel's internal environment information, and establish a grid map with obstacle information and action boundaries; Based on the established grid map, a global path from the start point to the end point is generated through a global path planner integrated with the A* algorithm; Through the laser radar carried by the construction equipment, the point cloud data around the construction equipment is obtained in real time, and a local cost map is established. The global path is input into the local cost map, and the cost value of the path point on the global path is detected to determine whether the obstacle is located on the global path; If the obstacle is on the global path, the global path is corrected by the local path generator integrating the APF and TEB algorithms to generate a local path that avoids the obstacle; otherwise, the global path is directly assigned to the local path; Based on the generated local path, a speed instruction is generated through a path tracking algorithm and fed back to the motion chassis, thereby achieving accurate obstacle avoidance for the construction equipment.

2. According to claim 1, a method for avoiding obstacles by integrating multiple algorithms for construction equipment in a tunnel environment is characterized in that: The specific contents of using laser radar to scan the tunnel environment, obtain point cloud data of the tunnel internal environment information, and establish a grid map with obstacle information and action boundaries are as follows: The tunnel environment is scanned by LiDAR to obtain the tunnel contour point cloud data and obstacle point cloud data. The Gmapping algorithm based on particle filtering is used to construct a grid map of the tunnel environment to generate a two-dimensional grid map containing obstacle information and action boundaries.

3. The obstacle avoidance method of multi-algorithm fusion for construction equipment in a tunnel environment according to claim 1 is characterized in that: Based on the established grid map, the global path planner integrating the A* algorithm is used to generate the specific content of the global path from the start point to the end point: The A* algorithm is used to calculate the optimal path from the starting point to the end point in the grid map containing obstacle information and action boundaries. The path with the lowest cost, that is, the global path, is found by heuristic search to balance the path length and estimated cost. The formula of the A* algorithm is: f(n)=g(n)+h(n) Where f(n) is the total evaluation cost of a node n, balancing the known cost and the future estimated cost, and is used for sorting and selection of the priority queue. g(n) is the actual path cost from the starting point to the current node n, reflecting the known cost of the path, and indicating the true cost of reaching the current node. h(n) is the estimated cost from the current node n to the target node, reflecting the future cost of the path.

4. The obstacle avoidance method for construction equipment using multiple algorithms in a tunnel environment according to claim 1, characterized in that: Through the laser radar carried by the construction equipment, the point cloud data around the construction equipment is obtained in real time, and a local cost map is established. The global path is input into the local cost map, and the cost value of the path point on the global path is detected to determine whether the obstacle is located on the global path. The specific content is as follows: Through the laser radar carried by the construction equipment, point cloud data of the surrounding environment of the construction equipment is collected in real time, and the point cloud data is used to build a local cost map. The local cost map represents the surrounding environment of the construction equipment in the form of a grid, where the cost value of each grid reflects the passability or danger level of the path location; Through the method of coordinate transformation, the global path is mapped from the global coordinate system to the coordinate system of the local cost map. The real-time position and posture information of the construction equipment is used to construct the transformation matrix between the global coordinate system and the local cost map coordinate system. The formula of the transformation matrix is: In the formula, (x g ,y g ) is a point in the global coordinate system, (x l ,y l ) is a point on the local cost map coordinate system, (x t ,y t ) is the position of the construction equipment in the global coordinate system, and θ is the orientation angle of the construction equipment in the global coordinate system; The cost value of the grid where the path point is located is detected in real time to determine whether the obstacle is on the global path.

5. The obstacle avoidance method of multi-algorithm fusion for construction equipment in a tunnel environment according to claim 1 is characterized in that: If the obstacle is on the global path, the global path is corrected by the local path generator integrating the APF and TEB algorithms. The specific steps for generating a local path to avoid the obstacle are as follows: a. Initialization: input the global path, local cost map and construction equipment pose; b. Target point extraction: Extract the closest point without obstacles in the global path as the target point; c. Potential field calculation: Use the APF algorithm to calculate the attraction of the target point and the repulsion of the obstacle to generate a resultant force field; d. Path generation: Generate a path according to the direction of the resultant force field; e. Path smoothing: Use the TEB algorithm to optimize the generated path by minimizing the path length, obstacle avoidance cost, and speed smoothing cost; f. Path publishing: Publish the optimized local path to obtain a local path that avoids obstacles.

6. The obstacle avoidance method of multi-algorithm fusion for construction equipment in a tunnel environment according to claim 1, characterized in that: Based on the generated local path, the path tracking algorithm is used to generate speed instructions and feed them back to the motion chassis, thereby achieving accurate obstacle avoidance for the construction equipment. The specific contents are as follows: Through coordinate transformation, the local path is mapped to the coordinate system of the local cost map to obtain the specific position of each coordinate point in the local path. Based on the relative position relationship between the construction equipment and the local path coordinate point, accurate obstacle avoidance of the construction equipment is achieved. The path tracking algorithm formula is: ω=α·(θ path -i) v=β·d Where ω is the angular velocity, v is the linear velocity, α and β are gain coefficients, d is the distance from the device to the path point, and θ path and θ are the path direction angle and the device orientation angle, respectively.

Images