Path planning method for air-ground cooperative inspection system of large tunnel

Abstract

The invention discloses a large tunnel air-ground cooperative inspection system path planning method, which comprises the steps of scanning tunnel point cloud through a laser radar carried by an unmanned vehicle, fusing built-in six-axis IMU data, and estimating a yaw angle of the unmanned vehicle in real time; calculating a dynamic step length based on the yaw angle, and generating an initial path point sequence of the unmanned vehicle; the path points are converted from a radar coordinate system to a global coordinate system, and vehicle body inclination is compensated by combining an IMU pitch angle and a rolling angle; detecting continuous contraction of the point cloud in the X-axis direction, and triggering a path planning stop protection mechanism according to a preset condition; detecting obstacles in a path voxel space, and adjusting path points through progressive interpolation; constructing a safety communication sphere according to the unmanned vehicle path points, and generating an unmanned aerial vehicle initial path under the constraint of the sphere; and performing multi-objective optimization on the path of the unmanned aerial vehicle, and outputting an optimized path set. According to the method, the technical effects of lightweight deployment based on a single sensor, path-obstacle avoidance integrated decision making, equipment collaborative decoupling control and multi-modal risk blocking are realized.

Description

Technical Field

[0001] This invention relates to the field of automatic control technology, and more specifically to a path planning method for a large-scale tunnel air-ground collaborative inspection system. Background Technology

[0002] Safety inspection of large tunnels (including water conservancy culverts and urban underground utility tunnels) is a core link in ensuring the reliable operation of infrastructure. The complexity and high-risk nature of their working environment places stringent demands on inspection efficiency, coverage completeness, and risk control capabilities. Current manual inspection methods reveal significant shortcomings in enclosed spaces spanning several kilometers: workers are limited by the range of their flashlights, and defects at heights are difficult to observe, leading to missed or false detections. Furthermore, they must directly confront safety hazards such as toxic gas leaks and structural collapses. Although intelligent inspection equipment has gradually replaced manpower, such as using unmanned vehicles or drones for tunnel inspection, single devices have inherent limitations: a single unmanned vehicle inspection is not comprehensive enough, high-altitude defects cannot be observed at close range, and its low degree of freedom of movement restricts its flexibility. Single drone inspections, on the other hand, have requirements regarding sensor weight and cannot be powered for extended periods. Therefore, the collaborative operation of unmanned vehicles and drones has become an inevitable choice. However, the unique enclosed, long, straight structure of tunnels, the distribution of dynamic obstacles, and the constraints of multi-device collaboration present the following key challenges to traditional path planning methods:

[0003] (1) Due to the characteristics of tunnels, such as large depth (length ≥ 5km) and strong structural repeatability (period recurrence rate of pipe sections > 80%), existing SLAM mapping methods are prone to cumulative positioning drift, forcing the system to be equipped with high-precision sensors such as multi-line lidar (64 lines or more), vision sensors, and optical flow sensors, resulting in high cost per set of equipment. If traditional local planning methods are used, it is usually necessary to equip sensors on unmanned vehicles and drones to obtain local fields of view, which increases the load.

[0004] (2) Due to the kinematic characteristics of the wheeled chassis, the unmanned vehicle only has two degrees of freedom control capability: lateral translation and longitudinal movement. This mechanical constraint is transformed into a rigid path geometric boundary in the semi-circular cross-section of the tunnel. Under non-obstacle avoidance conditions, it must strictly follow the centerline of the bottom of the pipeline to meet the dynamic balance condition between the center of gravity projection and the normal force of the contact surface. Traditional path planning algorithms construct motion models based on the assumption of a two-dimensional plane, which leads to cumulative pose deviations in the planned trajectory in the actual three-dimensional curved surface environment. When the lateral offset exceeds the critical threshold, an irreversible rollover accident will be triggered.

[0005] (3) Temporary obstacles that change dynamically in the tunnel (such as construction machinery and sandbags left in the water) cannot be updated in real time on the global map. If local update planning is adopted, it will face problems such as frequent path oscillation, a surge in the probability of multi-agent motion conflict, and increased computational load, which seriously weakens the real-time and effective response capability in dynamic scenarios.

[0006] (4) Path planning for heterogeneous multi-agent systems requires deep coupling of their physical constraints and scenario risk characteristics. For tunnel terrain abrupt change zones (such as vertical shaft connection sections), dynamic anchor points need to be pre-set based on the equipment dynamics model, and fall-prevention safety boundary constraints need to be generated through trajectory curvature-gravitational potential energy coupling analysis. For aerial agents, a joint optimization space of communication attenuation field and path feasible region must be constructed to ensure that the flight trajectory is always within the link stability threshold, while meeting the obstacle avoidance margin requirements under rotor airflow disturbance.

[0007] In summary, due to the long, straight, and enclosed structure of tunnels, dynamic obstacle interference, and the heterogeneous characteristics of multiple agents, the unmanned vehicle-drone collaborative operation system suffers from systemic contradictions at the path planning level, such as decreased mapping accuracy, motion model mismatch, oscillations in obstacle avoidance decisions, and collaborative safety boundaries. There is an urgent need for path planning methods tailored to the characteristics of tunnels, and to construct an intelligent planning system that adapts to the constraints of three-dimensional curved surfaces and the linkage of multimodal risks. Summary of the Invention

[0008] In view of this, in order to address the shortcomings of poor path planning adaptability, high equipment dependence and insufficient real-time performance of algorithms when unmanned vehicles and drones conduct collaborative inspections in complex environments such as large tunnels, this invention provides a path planning method for a large tunnel air-ground collaborative inspection system. Through environmental feature hierarchical mapping and a mechanism of complementary advantages of heterogeneous equipment, it achieves the technical effects of simultaneous and efficient multi-machine path search, dynamic environmental adaptive adjustment and full coverage inspection of tunnel space without relying on external navigation equipment.

[0009] To achieve the above objectives, the present invention adopts the following technical solution:

[0010] This invention provides a path planning method for a large-scale tunnel air-ground collaborative inspection system, comprising the following steps:

[0011] S10: The unmanned vehicle scans the tunnel point cloud using the lidar onboard the vehicle and integrates the built-in six-axis IMU data to estimate the unmanned vehicle's yaw angle in real time.

[0012] S20. Calculate the dynamic step size based on the yaw angle and generate the initial path point sequence of the unmanned vehicle;

[0013] S30. Transform the path points from the radar coordinate system to the global coordinate system, and combine the IMU pitch angle and roll angle to compensate for the vehicle tilt, so as to realize the unmanned vehicle path planning;

[0014] S40. Detect the continuous shrinkage of the point cloud in the X-axis direction, trigger the path planning stop protection mechanism according to preset conditions; and detect obstacles in the path voxel space, and achieve dynamic obstacle avoidance by progressively interpolating and adjusting the path points.

[0015] S50. Construct a safe communication sphere based on the unmanned vehicle waypoints, and generate the initial path of the UAV under the constraints of the sphere;

[0016] S60: Perform multi-objective optimization on the UAV path and output the optimized path set.

[0017] As can be seen from the above technical solution, compared with the prior art, the present invention has the following technical advantages:

[0018] This invention achieves groundbreaking technical effects based on lightweight deployment of a single sensor, integrated path-obstacle avoidance decision-making, decoupled control of equipment collaboration, and multimodal risk blocking, enabling the system to ensure inspection safety and execution efficiency even in environments without preset external beacons or GPS denial. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0020] Figure 1 The flowchart of the path planning method for the large-scale tunnel air-ground collaborative inspection system provided by the present invention is shown.

[0021] Figure 2 The flowchart for generating unmanned vehicle paths provided by this invention.

[0022] Figure 3 The schematic diagram of the tunnel yaw angle estimation algorithm based on lidar provided by this invention.

[0023] Figure 4 The actual tunnel point cloud map provided for this invention.

[0024] Figure 5 This is a schematic diagram of the dynamic step-length path planning for straight pipelines provided by the present invention.

[0025] Figure 6 A flowchart for missing value fitting provided by this invention.

[0026] Figure 7 This is a schematic diagram of the sliding window path planning for curved pipes provided by the present invention.

[0027] Figure 8This is a schematic diagram of the pipeline bottom path point projection method provided by the present invention.

[0028] Figure 9 The diagram shows the actual pipeline planning effect provided by this invention.

[0029] Figure 10 The present invention provides the principle and effect diagram of obstacle avoidance path planning in actual engineering.

[0030] Figure 11 The flowchart for generating drone paths provided by this invention.

[0031] Figure 12 This is a schematic diagram of the initial path point generation provided by the present invention.

[0032] Figure 13 This is a schematic diagram illustrating the principle of the method for generating initial path points in a real tunnel experiment provided by the present invention.

[0033] Figure 14 The flowchart for selecting the optimal path point provided by this invention.

[0034] Figure 15 The experimental results of the UAV obstacle detection process provided by this invention in a real tunnel are shown in the figure.

[0035] Figures 16a-16e The final experimental results provided for this invention are shown in the diagram. Detailed Implementation

[0036] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0037] This invention discloses a path planning method for a large-scale tunnel air-ground collaborative inspection system, providing a real-time, safe, and effective path for a collaborative inspection system of unmanned vehicles and drones in large tunnels based on a single lidar. (Refer to...) Figure 1 As shown, it includes:

[0038] S10: The unmanned vehicle scans the tunnel point cloud using the lidar onboard the vehicle and integrates the built-in six-axis IMU data to estimate the unmanned vehicle's yaw angle in real time.

[0039] S20. Calculate the dynamic step size based on the yaw angle and generate the initial path point sequence of the unmanned vehicle;

[0040] S30. Transform the path points from the radar coordinate system to the global coordinate system, and combine the IMU pitch angle and roll angle to compensate for the vehicle tilt, so as to realize the unmanned vehicle path planning;

[0041] S40. Detect the continuous shrinkage of the point cloud in the X-axis direction, trigger the path planning stop protection mechanism according to preset conditions; and detect obstacles in the path voxel space, and achieve dynamic obstacle avoidance by progressively interpolating and adjusting the path points.

[0042] S50. Construct a safe communication sphere based on the unmanned vehicle waypoints, and generate the initial path of the UAV under the constraints of the sphere;

[0043] S60: Perform multi-objective optimization on the UAV path and output the optimized path set.

[0044] This invention proposes a hierarchical collaborative planning architecture led by unmanned ground vehicles (UGVs). The UGVs establish a baseline trajectory through a LiDAR-IMU fusion sensing module, and unmanned aerial vehicles (UAVs) perform three-dimensional airspace expansion planning under the constraints of this trajectory. Thanks to the integrated sensor design at the hardware level (a multi-line LiDAR with a built-in six-axis IMU), the system can achieve full-state estimation and path planning through a single sensing unit, significantly reducing the configuration complexity of multi-source heterogeneous sensors.

[0045] Unlike traditional methods that rely on prior SLAM maps, this invention addresses the SLAM mapping failure problem caused by the strong repetitiveness of tunnel structure geometric features (typically manifested as loop closure detection failure and pose estimation divergence). It employs a dynamic local path update mechanism. By generating incremental trajectories through real-time calculation of the lidar point cloud manifold features, the system achieves autonomous navigation without a global positioning reference. This characteristic enables the planning algorithm to exhibit stronger scene adaptability in unstructured tunnel environments.

[0046] The technical solution of the present invention will be described in detail from two main aspects below:

[0047] 1. Autonomous vehicle path planning methods

[0048] like Figure 2 As shown, the unmanned vehicle's power is first turned on inside the tunnel, activating the LiDAR and its internal six-axis IMU. At this point, the ROS control system can subscribe to point cloud data and IMU data to obtain the coordinate information of each scanned point in the tunnel, as well as the current acceleration and attitude information (pitch angle, roll angle). Since the IMU only has six axes, it cannot obtain the yaw angle; therefore, a separate algorithm needs to be designed for yaw angle estimation.

[0049] (1) Yaw angle estimation

[0050] Figure 3This illustrates the principle of yaw angle estimation in the straight section (upper half) and curved section (lower half) of a tunnel. When the autonomous vehicle yaws, the point cloud scanned by the detection box in the lidar coordinate system will be offset. At this time, the average normal vector on both sides is calculated as the vertical direction of the tunnel at the current position. Only the angle between this direction and the direction directly in front (x-axis) in the lidar coordinate system needs to be calculated to obtain the current yaw angle. Figure 3 As shown, to minimize computation time, a small detection box centered on the LiDAR was used. Then, each point on either side of the detection box forms a surface with its K neighbors (KNN), and their properties (surface normals and curvature) are estimated. To reduce errors introduced by wall obstacles, the tunnel wall normals are estimated using the average normal vectors of the 30 flattest surfaces. Yaw angle deviation (in radians) is calculated as follows:

[0051]

[0052] in, This represents the unit vector of the wall normal in the lidar coordinate system. Specifically, it represents the unit vector of the average normal direction of the point cloud on both sides of the tunnel walls scanned by the lidar. Since curves are common in tunnels, it is necessary to estimate both scanning surfaces to reduce interference from curvature. Therefore, shifting the projection viewpoint along the y-direction ensures that the normal vectors of the wall point cloud within the detection frame all point in the same general direction, providing some compensation even if the wall is curved. The yaw direction can then be calculated using the following formula:

[0053]

[0054] Where, q b The quaternion q represents the attitude of the autonomous vehicle's body coordinate system relative to the world coordinate system. d This is the yaw coordinate system relative to the world coordinate system. This is the quaternion multiplication operator.

[0055] The algorithm described above can be used to calculate the yaw angle of the unmanned vehicle in its current state, thus completing the missing attitude information. Combined with the pitch angle, roll angle, and acceleration in the six-axis IMU, the complete state can be obtained.

[0056] (2) Initial Path Planning Algorithm

[0057] Real tunnel point clouds, such as Figure 4 As shown, the left side represents the point cloud of a straight section of the pipeline, and the right side represents the point cloud of a curved section of the pipeline. From top to bottom, the diagram shows the top view, front view, and axonometric view; different colors represent different distances. The steps of the unmanned vehicle initial path planning algorithm designed in this invention are as follows:

[0058] ① Point cloud preprocessing and feature extraction: First, invalid points (NaN values) are filtered out, and the 3D point cloud is reduced to 2D planar points along the z-axis, i.e., z-axis information is deleted while x and y-axis information is retained. Then, the step size is calculated based on the current yaw angle (calculated in the previous step), using the following formula:

[0059]

[0060] In the formula, θ is the current yaw angle, c1∈(0,1) is the initial reference step size coefficient, which controls the maximum step size value in the non-yaw state; c2>0 is the critical yaw angle, which determines the turning threshold of the piecewise function; c3∈(0,c1) is the minimum step size protection value, which ensures system stability; κ≥1 is the attenuation exponent, which adjusts the polynomial attenuation rate. The larger the exponent, the steeper the attenuation curve; λ∈(0,0.1) is the cubic term gain coefficient, which is used to compensate for the nonlinear effect in the small angle region; μ∈(-0.05,0) is the over-limit attenuation rate, which controls the step size descent gradient after the yaw angle exceeds the critical value.

[0061] In the formula, when |θ|≤c2, the main attenuation term is c1·(1-|θ| / c2). κ The dominant step size variation, whose power function form ensures that the step size monotonically decreases as the yaw angle increases; the cubic compensation term λ|θ| 3 Used to smoothly transition the curvature when |θ|≈0.8c2, thus avoiding mechanical decay; when |θ|>c2, the linear protection term c3+μ·(|θ|-c2) is activated, which prevents abrupt changes in step size through weak linear constraints while maintaining the minimum guaranteed value of c3.

[0062] The above formula sets the step size of the path planning points based on the current yaw direction of the autonomous vehicle. The larger the yaw, the smaller the step size. Specific parameter values ​​can be obtained through actual debugging. Taking the current test data as an example, we set c1 = 0.2, c2 = 1.5, c3 = 0.1, κ = 1.8, λ = 0.02, and μ = -0.01. When the autonomous vehicle is moving straight, the step size is 0.2 meters. When the vehicle deviates, the step size gradually decreases until it reaches 0.1 meters. Designing a dynamic step size ensures that the path points can be planned more precisely when the vehicle deviates, reducing the interference caused by yaw angle estimation errors. It also provides more detailed path guidance for the autonomous vehicle to return to center, which is more in line with the characteristics of autonomous vehicle steering. In addition, the parameters of this formula can be used as output values ​​to be integrated into neural networks or reinforcement learning for training in subsequent development, demonstrating theoretical extensibility.

[0063] After obtaining the specific step size, refer to Figure 5As shown, the initial path planning method is illustrated. The difference between the left and right images lies in the yaw angle of the autonomous vehicle. The step size calculation formula shows that a larger yaw angle results in a smaller step size, leading to more path points and more refined path planning. A dynamic binning strategy divides the area along the X-axis, extracting the minimum and maximum points in the Y-direction of each area as candidate sets for left and right edge points. For straight pipes, the number of left and right edge points is equal and represents the actual pipe edge. However, due to the large undetectable area in curved pipes, extreme points on the inner side of the curve will inevitably be smaller than the actual pipe edge extremes. Therefore, if a sudden change occurs between adjacent edge points (Y-value difference exceeds a threshold), edge collection on that side is stopped. At this point, a dynamic polynomial fitting mechanism based on Kalman preprocessing is used to process the collected missing side path points. Then, the Y-coordinate of the missing points is predicted based on the X-coordinate provided by the complete side to fill in the missing edge points and ensure the number of left and right points is consistent. The specific process is as follows: Figure 6 As shown.

[0064] This processing mechanism is robust to changes in the curvature of actual curved pipes. When a sudden increase in curvature is detected, it automatically triggers a reduction in the sliding window, increases the process noise of the Kalman filter, and initiates a polynomial fitting operation. Compared to traditional filtering and fitting methods, this mechanism has better noise resistance and dynamic curvature adaptability, while significantly reducing computational power consumption compared to Kalman filtering alone.

[0065] ② Path centerline generation: After obtaining the complete set of left and right edge points, the coordinates of each point are averaged to generate the initial path centerline point sequence, and the path origin is inserted at the starting point, such as... Figure 7 , Figure 8 As shown. Among them, Figure 7 This diagram illustrates the principle (left) and correction method for path deviation in a curved tunnel (right). Due to visual obstruction, the point cloud is sparse in the distance of the actual curved section. Therefore, when calculating the extreme values ​​in the y-direction, one side will inevitably not be the actual tunnel wall. The average value calculated in this way will guide the autonomous vehicle to one side of the tunnel, which is clearly unreasonable. The correction method stops calculation when a significant deviation in the point cloud extreme values ​​on one side is detected. The current correct mechanism is then fitted using a polynomial, and the missing extreme value points are inferred using the step size. After obtaining the complete points, the average value is calculated again to pull the path back to the tunnel center. Linear regression is used to fit the pipe height distribution of the point cloud in the XZ plane, obtaining the ground plane slope and intercept for Z-axis coordinate calculation. The X-values ​​of the path points are input into the fitted model to obtain the Z-values ​​that fit the bottom of the pipe wall. Combining this with the averaged Y-values ​​yields the complete path points at the bottom of the pipe in the local coordinate system. Raising the bottom path along the Z-axis to the pipe radius height yields the pipe's central axis path, as shown below. Figure 9The figures illustrate the actual planning results under the three scenarios described above. The first figure shows the path planned for the most common straight section, the second figure shows the path planned for the curved section, and the third figure shows the smooth transition path when the autonomous vehicle veers off course.

[0066] ③ Path coordinate transformation:

[0067] Since the path planning described above only plans path points in the LiDAR coordinate system, it cannot reflect whether the autonomous vehicle is currently at the correct altitude. Assuming the autonomous vehicle is currently moving forward without deviation along the sidewall of the pipe, the point cloud scanned by the LiDAR will actually have the same shape as the one scanned at the bottom of the pipe, indicating pipe consistency. However, moving along the sidewall of the pipe is clearly an incorrect path. To solve this problem, this design integrates a six-axis IMU from the LiDAR for path coordinate transformation, aiming to convert local path points into path points in the global coordinate system. First, it checks whether the number of input path points is sufficient (at least three points are required), and uses the yaw angle calculated above to calculate the number of transition points and the yaw angle change at each transition point. The formula for calculating transition points is as follows:

[0068]

[0069]

[0070] Where, N transition N represents the number of transition points. points Let θ be the total number of planned waypoints, θ be the yaw angle of the autonomous vehicle, and Δ be the yaw angle of the vehicle. yaw The deflection amount for each path point.

[0071] The above formula primarily smooths the autonomous vehicle's trajectory based on its current yaw rate and the number of planned waypoints. Without this smoothing mechanism, a large yaw angle could cause the vehicle to overturn due to excessive angular velocity in an attempt to quickly return to the path. This mechanism, combined with the dynamic step-size planning in section ②, provides dual protection for the vehicle's refined control when yaw occurs, ensuring sufficient waypoints are provided for real-time feedback during the correction process.

[0072] In the roll and pitch degrees of freedom, this design directly reads the attitude from the IMU, combines the yaw angle change at each point to form a complete quaternion q, and gradually converts the path points into global points using the following formula:

[0073]

[0074] In the formula, q is the attitude quaternion including yaw, pitch, and roll; P global Point data parameters in the global coordinate system; P local These are the point data parameters in the radar coordinate system; This is the quaternion multiplication operator.

[0075] The initial path of the autonomous vehicle can be obtained through the above three steps. This path is based on the current position of the autonomous vehicle as the origin, and the path points are planned dynamically according to the yaw angle. Then, the tilt is corrected by combining the IMU. The planning effect in the actual pipeline is as follows. Figure 9 As shown, because there is water, silt, etc. at the bottom of the actual pipe, there is usually a narrow and long area in front that cannot be scanned. Based on this, the location of the bottom of the pipe can be determined. Figure 9 The left-middle part is the planning in a straight pipe, where the path is a straight line; the middle part is the planning in a curved pipe, where enough path points can still be planned even with sparse point clouds on the left; the right part is the planned path on the right wall of the pipe, where the path gradually shifts until it returns to the bottom of the pipe and continues straight, which is consistent with the actual movement expectation.

[0076] (3) Path planning stop protection mechanism

[0077] The path planning method described above is sufficient for autonomous vehicles in practical tasks, but abrupt changes in the shape of the pipeline often occur. For autonomous vehicles, vertical abrupt changes must be considered, especially downward abrupt changes. Without a timely stopping mechanism, the vehicle may continue forward, potentially falling into a lower section and causing damage. The goal of this design's stopping planning mechanism is to detect the continuous shrinking of the maximum value of the point cloud along the X-axis. When the accumulated amount of this phenomenon exceeds a threshold, it is considered that there is an obstacle or danger zone ahead, triggering path planning to stop. The specific method is as follows:

[0078] ① Define the maximum coordinate value of the point cloud in the X-axis direction as a dynamic monitoring variable:

[0079]

[0080] Where x i This represents the X-coordinate of the i-th point in the point cloud of frame t.

[0081] ② Single-frame shrinkage calculation: Used to calculate the shrinkage or expansion of the sensor in the direction within the point cloud.

[0082]

[0083] The above formula Positive values ​​indicate that the sensor is contracting, while negative values ​​indicate that it is expanding.

[0084] ③ Cumulative Update Rule: To prevent false termination due to sudden changes in pathpoint distances caused by noise, a cumulative update rule is designed. Only changes exceeding a certain value are considered valid changes, and termination is triggered only after a certain amount of valid changes have accumulated. The cumulative reduction amount S and the number of accumulations C are defined, and the continuous reduction process is described by the following formula:

[0085]

[0086] In the formula I (·) This is an indicator function, set to 1 if the condition is met, and 0 otherwise. α is the change detection threshold to filter out minor fluctuations caused by noise. The risk value is defined as:

[0087]

[0088] Where β is a learnable parameter used to adjust the trigger sensitivity. The trigger condition is defined as follows:

[0089]

[0090] In the formula, C min The minimum sustained frame threshold represents the boundary of the cumulative number of times.

[0091] The above formula ensures that the distance to the farthest point detected by the sensors is monitored in real time during the autonomous vehicle's movement. When the farthest point continues to shrink beyond a threshold, planning is stopped, and the first N points of the last planned path are retained as the final path. Real-world tunnel point clouds often contain noise, so a minimum fluctuation threshold is set to isolate minute noise changes. Due to other interference factors, experiments showed that occasionally the planned path was shorter in a few frames during normal planning, but it quickly returned to a normal path. To prevent false termination due to this situation, this design also incorporates a minimum shrinkage frame threshold. This threshold increases with each shrinkage of the farthest point. If the shrunken path lengthens in a frame, the threshold is reset and re-accumulated. The planning stop mechanism is only triggered when both conditions are met.

[0092] (4) Dynamic obstacle avoidance algorithm

[0093] During actual tunnel travel, autonomous vehicles may encounter unforeseen obstacles, such as sandbags left in the water or large pieces of debris washed down from upstream, which can hinder their movement and even threaten their safety. Therefore, obstacle avoidance algorithms need to be incorporated into the path planning process to ensure smooth progress. The specific methods are as follows:

[0094] ① Path space detection modeling: After obtaining the above path, a cuboid is generated between every two path points to detect whether there are obstacle point clouds during the autonomous vehicle's journey. The formulas for cuboid generation and point cloud detection are as follows:

[0095]

[0096] P in ={p i ∈P∩B k}

[0097] In the formula, w and h represent the width and height of the autonomous vehicle plus the safety distance, R(θ) is the rotation matrix of the path point orientation, which is converted from the quaternion mentioned above, P is the point cloud set region, and p i For obstacle points; {B k =[q k ,q k+1 ]×[y min ,y max ]} represents the generated set of cuboid regions for detection. q k q k+1 These represent two consecutive path points in the initial path, where k represents the k-th detection cuboid region. For example, the first detection cuboid is formed by using the first and second path points as its two endpoints, the distance between the two path points as its length, and the vehicle's body plus a safety distance as its width and height, thus creating a detection space. The presence of an obstacle in the path can be determined simply by checking if a point cloud exists within this space.

[0098] ② Obstacle Avoidance Path Point Adjustment: After a series of cuboid regions are generated within the path points, it is possible to detect whether a point cloud exists within each region. If not, the two path points before and after the cuboid do not need to be changed. If a point cloud exists within a cuboid, the average value of the point cloud in the x-direction and the extreme value in the y-direction are calculated using the following formula:

[0099]

[0100] In the formula, P in Let y be the set of point clouds detected by the cuboid in a certain detection area in the previous step. min and y max These represent the extreme values ​​in the y-direction of the point cloud set.

[0101] Since the kinematics of an autonomous vehicle has only two degrees of freedom, obstacle avoidance only requires considering whether to turn left or right. Therefore, the gradient from the path point at the beginning of the cuboid to the two extreme points is calculated, and the y-direction with the smaller gradient is chosen as the obstacle avoidance direction:

[0102]

[0103] The initial and final pathpoints of the region are updated based on the selected direction. The x-value of the final pathpoint is updated to the average x-value of the point cloud, and the y-value is updated to the extreme y-value of the point cloud, increased by half a cuboid distance (equivalent to translating the cuboid along the obstacle avoidance direction by half its width) to obtain the updated final pathpoint. The initial endpoint is updated by keeping the x-value constant. However, since the final point gets closer to the initial endpoint as the autonomous vehicle moves, if the y-value also remains constant, a sudden path change will occur when the two points are close, resulting in a large angle change for the autonomous vehicle and potentially causing danger. Therefore, the y-value of the initial endpoint uses progressive interpolation to mitigate this problem. The farther the final point is from the initial endpoint, the closer the y-value of the initial endpoint is to its original value; conversely, the closer it is to the initial point, the closer it is to the final point's y-value. This way, when two points are close, the initial and final points can be approximated as the new final point, and the previous pathpoint of the initial endpoint will be approximated as the new initial endpoint, ensuring sufficient transition space at corners. The specific formula is as follows:

[0104]

[0105]

[0106] In the formula, y adj q′ represents the adjusted y-value of the end path point. k and q′ k+1 This represents the updated first and last path points, where L is the original length of the first and last path points; w represents the vehicle width plus safety margin; x k y k These represent the original coordinates of the path points that need to be adjusted due to obstacle avoidance. In simpler terms, if an obstacle point cloud exists within a detection space, the positions of the first and last path points that make up this detection frame space need to be adjusted. Adjusting the first point only adjusts the y-coordinate, while adjusting the last point uses the mean value of the obstacle point cloud for the x-coordinate and the y-coordinate after increasing the safety distance. This ensures that the y-coordinate deviates most only when parallel to the obstacle, preventing excessive movement to the tunnel wall and potential rollover.

[0107] Furthermore, if the obstacle is too large, it may cause the obstacle avoidance point to shift significantly along the y-axis. As analyzed earlier, when the autonomous vehicle deviates too much in the pipe, it will inevitably move towards the pipe sidewall, potentially causing it to overturn while trying to avoid the obstacle. To avoid this, this design incorporates curvature constraints; that is, if the constraint is satisfied after the first update of the new path point:

[0108]

[0109] Then continue executing the obstacle avoidance plan; if the conditions are not met, execute the stop plan to ensure driving safety. The principles and effects of obstacle avoidance path planning in actual engineering are as follows: Figure 10 As shown, Figure 10This represents the actual obstacle avoidance effect. The green cuboid in the diagram represents the space for detecting obstacles. An obstacle (a person) exists in the diagram; its shape can be seen in the right image. The planned path is shown in the left image, where the path bends at the obstacle.

[0110] 2. Drone Path Planning

[0111] As analyzed above, due to the requirements of inspection tasks for drones in terms of flexibility and long endurance, large sensors cannot be mounted on drones. Therefore, drone path planning is still completed by the lidar mounted on the unmanned vehicle. The first part has already completed the path planning for the unmanned vehicle, and the drone path is planned based on the unmanned vehicle path.

[0112] like Figure 11 As shown, the lidar is activated, and the UAV path planning port subscribes to the unmanned vehicle path, the pipeline center path, and point cloud topics. Then, the flight altitude is set according to task requirements. During actual experiments, it was found that when the UAV is too far from the unmanned vehicle, communication between the devices may be lost, causing the UAV to slowly descend into the water, posing a risk of water damage. Therefore, a safe communication range must be considered during UAV path planning. This design constructs a sphere with the vehicle height as the center and the safe communication range as the radius at each path point of the unmanned vehicle, to measure the communication safety of the UAV path. Furthermore, since the inspection task requires the UAV and unmanned vehicle to move synchronously, the UAV's path points on the x-axis should be consistent with the unmanned vehicle's path points. However, because the UAV and unmanned vehicle have different degrees of freedom (UAV three degrees of freedom, unmanned vehicle two degrees of freedom), directly raising the unmanned vehicle's path is inappropriate. For example, when encountering an obstacle, the unmanned vehicle needs to turn to avoid it, but at the UAV's altitude, obstacle avoidance is unnecessary or only slight movement is required. Reusing the unmanned vehicle's path in this case would only increase control costs and energy consumption. Therefore, this design uses dynamic sampling point iterative optimization combined with voxel constraint methods to plan the UAV path.

[0113] (1) Initial path generation

[0114] Similar to the path planning methods for autonomous vehicles, path planning for drones also involves first generating an initial path, and then optimizing and updating it based on actual needs. Firstly, since the autonomous vehicle may not always travel along the pipeline direction (e.g., for obstacle avoidance or turning), the points on the central axis generated in the early stages may not all be within safe communication range. For each path point q of the autonomous vehicle... i =(x i ,y i ,z i +h) Define a secure communication sphere S with radius r. i h represents the height of the unmanned vehicle; set the flight altitude d, and pull each point on the central axis of the pipeline to the set altitude plane w. i =(xi ,y o ,d), check if they are all on sphere S i In the middle, y0 represents the reference Y-coordinate of the pipe's central axis on the horizontal plane. If so, then the path point w is directly used. i Otherwise, connect the path points on the central axis with the path points of the autonomous vehicle, and project the path points on the central axis onto the sphere along the direction of the connecting line:

[0115]

[0116] The intersection of the line and the sphere is denoted as m. i Then all safe points w i and intersection point m i Integration is the initial path P of the drone. initial ={w1,w2,…w n ,m1,m2…m n}.like Figure 12 As shown, the two sub-figures in the upper half illustrate the selection schemes for the initial path points of the UAV when a point on the tunnel's central axis is within and outside the UAV-vehicle safe communication sphere. If it is within the tunnel (upper left figure), the central axis point is directly selected as the point in the UAV's initial path. If it is outside the communication range (upper right figure), a line is constructed connecting it to the path point of the vehicle, and then the boundary point of the communication range is taken as the path point. This ensures that the UAV's path is always within the safe communication range. The lower sub-figure shows the complete initial path operation process.

[0117] The principle of the initial path point generation method in a real tunnel experiment is as follows: Figure 13 As shown.

[0118] (2) Dynamic sampling point iterative optimization combined with voxel constraint path optimization

[0119] The initial path described above only guarantees that the UAV's waypoints are within communication range and travel along the pipeline direction. However, important UAV metrics such as smoothness, control cost, and obstacle avoidance are not considered, thus requiring optimization. This invention designs a path optimization method combining dynamic sampling point iterative optimization and voxel constraints. The optimization process is as follows: Figure 14 As shown, optimization begins with the initial point as the current point. First, candidate points are obtained by sampling the vicinity of this point. Then, the loss function value w from the current point to each candidate point is calculated. ij Let $\frac{i}{j}$ represent the current point in the $i$-th iteration, and $j$ represent the $j$-th candidate point near $i$. After all calculations are completed, the point with the minimum loss is selected as the optimal point for a single iteration and used as the current point in the next iteration. This process is repeated until the loss function value $w$ of the candidate points near the current point in a certain iteration is reached. ijThe value cannot be reduced further, or the maximum number of iterations must be reached. The iteration point with the minimum loss, which is also the global optimum, is the optimized path point.

[0120] ① Obstacle detection

[0121] First, using the line connecting every two points in the initial path as the central axis, construct a cylinder C with the safe flight distance of the drone as the radius. Assuming there are n points in the path, n-1 cylinders C will be generated. Check if there is a point cloud within each cylinder. If not, proceed to the next cylinder; if there is, mark the point cloud as an obstacle and calculate the shortest distance from the point cloud to the cylinder's axis.

[0122]

[0123] Among them, R safe Indicates the safe flight range for drones. Represents obstacle point cloud c j To the axis of the obstructed cylinder The shortest distance is quickly calculated using a KD-tree, and the formula is as follows:

[0124]

[0125] p i p i+1 These represent the start and end points of the current path segment, respectively.

[0126] The process was tested in a real tunnel and the results were as follows: Figure 15 As shown.

[0127] ② Multi-objective cost function

[0128] A multi-objective cost function is constructed to optimize global path points. The function expression is as follows:

[0129] J(P)=f1J safety (P)+f2J smoothness (P)-f3J progress (P)+f4J height (P)f1, f2, f3, f4 represent the corresponding coefficients;

[0130] The components are expressed as follows:

[0131] Safety (obstacle avoidance) costs:

[0132]

[0133] Smoothness cost (penalty for path deviation from a straight line, ensuring path smoothness):

[0134]

[0135] Forward reward (normalized progress metric, incentivizing path points to move towards the goal):

[0136]

[0137] High consistency item (minimize drone altitude variation):

[0138] J height (P)=(P z -P start,z ) 2

[0139] ③ Iterative solution optimization

[0140] Solve the objective

[0141]

[0142] Adaptive sampling strategy

[0143] Since there is no map, the alternative points used for optimization need to be sampled. However, due to the large planar space, choosing a fixed resolution during sampling presents some problems: if the sampling resolution is too high, it will increase the computational cost, thereby reducing the algorithm efficiency; if the sampling resolution is too low, the optimal point may be missed, resulting in the inability to obtain the global optimal point. Therefore, this invention chooses to use an adaptive sampling strategy. Furthermore, to further improve sampling efficiency, this invention chooses to perform polar coordinate sampling near the initial point. First, the sampling radius r is initialized. sample =r samples n candidate points around the current point:

[0144]

[0145] Evaluate the cost function value J(P) of each candidate point, select the optimal point, and update the current point P. best =argminJ(P i And reduce the sampling radius r sample =γ·r sample ,γ∈(0,1), repeat the above steps until convergence or the maximum number of iterations is reached. This strategy combines global exploration capability and local optimization accuracy.

[0146] ④ Spatial search optimization

[0147] The key optimization of the algorithm in terms of computational complexity is the use of KD-trees for spatial indexing, which reduces the time complexity of obstacle detection from O(mn) of the naive method to O(mlogn), where m is the number of path segments and n is the size of the point cloud.

[0148] The mechanism for determining whether a point cloud is an obstacle is optimized using a KD-tree, specifically by determining whether the point is within the cylinder C(p).k ,p k+1 In r), first calculate the bounding box using the formula:

[0149] B = [min(p k ,p k+1 )-r,max(p k ,p k+1 )+r]

[0150] Using a KD-tree at radius |p k+1 -p k Search the point cloud set within | / 2+r. If no point cloud is found, exclude the current detection space and continue searching the next space. Otherwise, calculate; if the value is less than the value in the previous space, an obstacle is determined to exist.

[0151] ⑤ Global path optimization

[0152] Global path optimization can be formalized as an iterative optimization problem:

[0153] P k+1 =F(P k )

[0154] Among them, P k Let F be the path in the k-th iteration, and F be the optimization operator. An iterative obstacle avoidance algorithm is applied to each obstacle path segment. The convergence condition is that the path change is less than a threshold δ.

[0155] |P k+1 -P k |<δ

[0156] Or it may reach the maximum number of iterations.

[0157] The final actual experimental results are as follows Figures 16a-16e As shown. Figure 16a This indicates the final path of the autonomous vehicle and drone in a single obstacle scenario; Figure 16b A schematic diagram illustrating the detection space for unmanned vehicles and drones to detect obstacles; Figure 16c This represents the obstacle detection space generated in the initial path. Figure 16d This indicates the obstacle avoidance path generated after detecting a single obstacle. Figure 16e This indicates the optimized final path for drones and unmanned vehicles when multiple obstacles are present.

[0158] This invention solves the problem of path planning and navigation for unmanned vehicles in tunnel environments. Most current path planning methods require global maps or location information feedback, which is difficult to achieve in tunnel environments. The effectiveness of this invention has been experimentally verified, filling a technological gap. Considering the collaborative inspection nature of unmanned vehicles and drones, this invention uses only a single LiDAR and IMU to perform path planning for both types of agents separately, and takes into account secure communication range constraints, conforming to practical engineering conditions.

[0159] Furthermore, unlike conventional methods that use trajectory planning to keep the path away from obstacles, this invention directly detects the presence of obstacles within path points using voxels, facilitating subsequent trajectory planning and reducing the complexity of subsequent control operations. Moreover, considering the cost and economic aspects of UAV control, this invention performs global optimization after obtaining the UAV path to ensure the smoothness, safety, and energy efficiency of the actual UAV flight, thus better meeting the actual needs of UAV control.

[0160] The invention has low computational cost. In actual tests, the path planning time for unmanned vehicles was less than 20 milliseconds and the path planning time for drones was less than 100 microseconds on a host configuration with a 13th generation i5 CPU and a 2080s graphics card. The efficiency is very high. It can be further expanded to multi-drone or multi-unmanned vehicle collaborative scenarios and has good secondary development properties.

[0161] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

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

Claims

1. A path planning method for a large-scale tunnel air-ground collaborative inspection system, characterized in that, Includes the following steps: S10: The unmanned vehicle scans the tunnel point cloud using the lidar onboard the vehicle and integrates the built-in six-axis IMU data to estimate the unmanned vehicle's yaw angle in real time. S20. Calculate the dynamic step size based on the yaw angle and generate the initial path point sequence of the unmanned vehicle; S30. Transform the path points from the radar coordinate system to the global coordinate system, and combine the IMU pitch angle and roll angle to compensate for the vehicle tilt, so as to realize the unmanned vehicle path planning; S40. Detect the continuous shrinkage of the point cloud in the X-axis direction, trigger the path planning stop protection mechanism according to preset conditions; and detect obstacles in the path voxel space, and achieve dynamic obstacle avoidance by progressively interpolating and adjusting the path points. S50. Construct a safe communication sphere based on the unmanned vehicle waypoints, and generate the initial path of the UAV under the constraints of the sphere; S60: Perform multi-objective optimization on the UAV path and output the optimized path set.

2. The path planning method for a large-scale tunnel air-ground collaborative inspection system according to claim 1, characterized in that, In step S10, the yaw angle of the unmanned vehicle is estimated in real time, including: Construct a detection box centered on the radar, extract the point sets of the two side walls of the radar point cloud, and calculate the surface normal vector of the K-neighborhood of each point in the box. The normal vectors of the M surfaces with the smallest curvature are selected to estimate the normal vectors of the tunnel walls. Calculate the yaw angle using the formula: in, This represents the unit vector of the wall normal in the lidar coordinate system.

3. The path planning method for a large-scale tunnel air-ground collaborative inspection system according to claim 2, characterized in that, Step S20 includes: The radar point cloud is reduced from a 3D point cloud to a 2D planar point cloud, and the dynamic step size is calculated based on the yaw angle. In the formula, θ is the current yaw angle, c1∈(0,1) is the initial reference step size coefficient, which controls the maximum step size value in the non-yaw state; c2>0 is the yaw critical angle, which determines the turning threshold of the piecewise function; c3∈(0,c1) is the minimum step size protection value; κ≥1 is the attenuation exponent, which adjusts the polynomial attenuation rate; λ∈(0,0.1) is the cubic term gain coefficient, which is used to compensate for the nonlinear effects in the small angle region; μ∈(-0.05,0) is the over-limit attenuation rate, which controls the step size descent gradient after the yaw angle exceeds the critical value; The region is divided along the X-axis using a dynamic binning strategy. The minimum and maximum points in the Y-direction of each region are extracted as candidate sets of left and right edge points.

4. The path planning method for a large-scale tunnel air-ground collaborative inspection system according to claim 3, characterized in that, Step S20 further includes: If the difference in Y-values ​​between edge points in the Y direction exceeds the threshold, edge acquisition in the Y direction will stop. A dynamic polynomial fitting mechanism based on Kalman preprocessing is used to process the collected missing side path points; Predict the Y coordinate of the missing point based on the X coordinate provided by the complete side, so as to fill in the edge points and make the number of points on the left and right sides consistent. After obtaining the complete set of edge points in the X and Y directions, the coordinates of each point are averaged to generate the initial path midline point sequence.

5. The path planning method for a large-scale tunnel air-ground collaborative inspection system according to claim 4, characterized in that, After obtaining the complete set of edge points in the X and Y directions, the coordinates of each point are averaged to generate the initial path centerline point sequence; including: The pipe height distribution in the XZ plane is fitted by linear regression to obtain the ground plane slope and intercept for Z-axis coordinate calculation. The X value of the path point is input into the fitted model to obtain the Z value that fits the bottom of the pipe wall. Combined with the calculated average Y value, the complete path point of the bottom of the pipe in the local coordinate system is obtained. Raise the bottom path along the Z-axis to the height of the pipe radius to obtain the pipe centerline path.

6. The path planning method for a large-scale tunnel air-ground collaborative inspection system according to claim 1, characterized in that, Step S30 includes: Calculate the number of waypoint transitions and the change in yaw angle at each waypoint based on the yaw angle: Where, N transition N represents the number of transition points. points Let θ be the total number of planned waypoints, θ be the yaw angle of the autonomous vehicle, and Δ be the yaw angle of the vehicle. yaw The deflection amount at each path point; Based on the roll and pitch degrees of freedom from the IMU data, and combined with the yaw angle change at each point, a complete quaternion q is formed. The path points are then progressively converted into global points using the following formula, thus achieving autonomous vehicle path planning: In the formula, q is the attitude quaternion including yaw, pitch, and roll; P global Point data parameters in the global coordinate system; P local These are the point data parameters in the radar coordinate system; This is the quaternion multiplication operator.

7. The path planning method for a large-scale tunnel air-ground collaborative inspection system according to claim 1, characterized in that, In step S40, the continuous shrinkage of the point cloud in the X-axis direction is detected, and a path planning stop protection mechanism is triggered according to preset conditions; including: In each frame of point cloud data, extract the maximum coordinate value in the X-axis direction; record the coordinate difference between the current frame and the previous frame as the single frame scaling amount; When the single-frame shrinkage amount exceeds the change detection threshold, update the cumulative shrinkage amount and the cumulative count; When the cumulative shrinkage exceeds the risk value and the cumulative number of times exceeds the continuous frame threshold, the path planning stop protection mechanism is triggered, and the first N points of the last safe path are retained.

8. The path planning method for a large-scale tunnel air-ground collaborative inspection system according to claim 7, characterized in that, In step S40, obstacles within the path voxel space are detected, and dynamic obstacle avoidance is achieved by progressively interpolating and adjusting path points; this includes: A cuboid detection region is generated between every two path points; when a point cloud exists in one of the cuboid detection regions, the extreme value in the Y direction and the mean value in the X direction of the point cloud are calculated. Calculate the gradient from the path point at the beginning of the cuboid to the two extreme points, and select the y-direction with the smaller gradient as the obstacle avoidance direction. Based on the selected obstacle avoidance direction, update the corresponding cuboid region's end path point, and use a progressive interpolation method to prevent path abrupt changes to update the beginning point; If the updated path points satisfy the curvature constraints, continue with the obstacle avoidance planning.

9. The path planning method for a large-scale tunnel air-ground collaborative inspection system according to claim 1, characterized in that, Step S50 includes: For each path point q of the autonomous vehicle i =(x i ,y i ,z i +h) Define a secure communication sphere S with radius r. i h represents the height of the driverless vehicle. Set the flight altitude d, and pull each point on the central axis of the pipe to the set altitude plane w. i =(x i ,y o d), check if each point is on sphere S i In the middle, y0 represents the reference Y coordinate of the pipeline's central axis on the horizontal plane; If so, then directly use the path point w. i Otherwise, connect the path points on the central axis with the path points of the autonomous vehicle, and project the path points on the central axis onto the sphere along the direction of the connecting line: The intersection of the line and the sphere is denoted as m. i All safe points w i and intersection point m i Integration is the initial path P of the drone. initial ={w1,w2,…w n ,m1,m2…m n } 10. A path planning method for a large-scale tunnel air-ground collaborative inspection system according to claim 9, characterized in that, Step S60 includes: Construct a continuous cylindrical detection volume along the initial path of the UAV, query the point cloud of obstacles within the cylindrical detection volume using a KD-tree, and calculate the shortest distance from the point cloud to the axis. c j Represents obstacle point clouds, p represents the axis of the obstructed cylinder. i p i+1 These represent the start and end points of the current path segment, respectively. Constructing a multi-objective cost function: J(P)=f1J safety (P)+f2J smoothness (P)-f3J progress (P)+f4J height (P) In the formula, f1, f2, f3, and f4 represent the corresponding coefficients; J safety (P) represents the security cost; J smoothness (P) represents the smoothing cost; J progress (P) indicates a forward bonus item; J height (P) indicates a highly consistent item; Solution objective: Initialize sampling radius r sample =r samples n candidate points around the current point: Evaluate the cost function value J(P) of each candidate point, select the optimal point, and update the current point P. best =argminJ(P i And reduce the sampling radius r sample =γ·r sample ,γ∈(0,1); When the path change is less than the threshold or the number of iterations reaches the maximum number of iterations, the optimized path set is output.

Images