Tunnel blasting charging robot positioning and path planning method and system

Abstract

The application discloses a tunnel blasting charging robot positioning and path planning method and system, and belongs to the technical field of tunnel engineering construction. The application constructs a three-dimensional tunnel map in real time through laser SLAM, constructs a double closed loop dynamic fusion mechanism of global pose correction and local alignment fine adjustment, corrects long distance cumulative pose drift by taking blast hole feature points as SLAM loop detection fixed anchor points, and synchronously outputs optimal global pose of the robot and accurate target coordinates of the blast hole. The MQ-RRT* algorithm is optimized and improved for the long and narrow limited space of the tunnel, tunnel axis preferential sparse sampling, dynamic target bias and charging safety constraint cost function are adopted, a multi-section continuous path suitable for charging operation is generated, local incremental path re-planning, environment adaptive sensing and hierarchical fault degradation strategy are matched, and unmanned charging operation in the whole process is realized.

Description

Technical Field

[0001] This invention relates to the field of tunnel engineering construction technology, specifically to a method and system for positioning and path planning of a tunnel blasting charging robot. Background Technology

[0002] Tunnel engineering is a core component of my country's transportation infrastructure construction, water resource development, and mining. The drill-and-blast method, with its strong adaptability to different geological conditions and controllable construction costs, is currently the mainstream construction method for mountain tunnel excavation. The blasting charge is a crucial step in drill-and-blast construction that directly determines the blasting effect, construction efficiency, and operational safety. Traditional manual charging relies on the field experience of operators. Charging per borehole is time-consuming, and full-face charging accounts for a high proportion of a single tunnel construction cycle. Furthermore, operators must enter the tunnel face area, which carries risks of dust pollution, surrounding rock collapse, and dynamic rockfalls, posing significant safety hazards. Moreover, manual visual alignment is prone to errors in charge placement, leading to over-excavation, under-excavation, and substandard blasting results.

[0003] With the development of intelligent engineering equipment, tunnel blasting charging robots have become a core development direction for solving the pain points of manual charging. However, the closed and narrow construction environment of tunnels places stringent requirements on the robot's autonomous positioning and path planning capabilities. Global positioning system signals are completely ineffective in the enclosed space of a tunnel, making conventional ground positioning methods unsuitable. Poor lighting conditions and high dust concentrations inside the tunnel can easily cause distortion in visual sensor imaging. Furthermore, deformation of the surrounding rock and blasting vibrations during construction can cause deviations in the three-dimensional pose of the blast holes, further increasing the difficulty of robot operations.

[0004] Currently, several technical solutions have been proposed for autonomous localization and path planning of robots in underground environments. Among them, robot navigation solutions based on simultaneous localization and mapping (SLAM) can collect environmental point cloud data using LiDAR to build real-time environmental maps and calculate the robot's pose. These solutions are widely used for autonomous operation of robots in underground spaces. However, these solutions only enable the robot's own localization and obstacle avoidance; they cannot accurately identify and match the coordinates of discrete blast hole targets at the tunnel face. This results in a disconnect between the SLAM-built environmental map and the blast hole target data for explosive charging operations, failing to provide accurate target positioning benchmarks for the charging operation. Regarding the optimization of robot path planning, existing general-purpose path planning methods can optimize obstacle avoidance and smoothness through multi-weighted reward mechanisms. However, these solutions do not consider the operational characteristics of the narrow and confined space of tunnels, nor do they take into account the safety constraints and blast hole distribution characteristics of explosive charging operations. Therefore, they cannot adapt to the continuous operation requirements of densely packed blast holes in tunnel explosive charging scenarios.

[0005] Overall, existing technologies related to tunnel blasting and charging robots still face core technical bottlenecks and cannot adapt to the complex and dynamic construction environment of tunnels. On the one hand, existing positioning schemes cannot solve the problem of fusion between global pose and borehole target positioning. Relying on preset coordinates or manual calibration methods cannot adapt to the dynamic offset of the three-dimensional pose of the borehole, and the positioning accuracy cannot meet the requirements of charging operations, easily leading to charging alignment deviations. On the other hand, existing path planning algorithms have not been specifically optimized for tunnel charging scenarios, resulting in long paths, low operational efficiency, and an inability to quickly respond to dynamic environmental changes within the tunnel. The ability to balance obstacle avoidance and operational safety is also insufficient. Furthermore, existing solutions do not form a complete closed loop of positioning, planning, and execution, still requiring manual intervention to complete multiple operations. This makes it impossible to achieve fully unmanned and safe charging operations, and thus fails to meet the development needs of efficient and safe construction in tunnel engineering. Summary of the Invention

[0006] This invention addresses the core shortcomings of existing technologies, such as robot positioning failure in enclosed tunnel environments, low borehole alignment accuracy, poor adaptability of path planning scenarios, and inability to form a closed-loop operation throughout the entire process. It provides a method and system for positioning and path planning of a tunnel blasting charging robot, which completely solves the industry pain point of the separation between environmental mapping and charging target positioning, and realizes high-precision positioning, highly adaptable path planning, and fully unmanned charging operations under complex tunnel conditions.

[0007] The specific details of the invention are as follows: On one hand, this invention provides a method for positioning and path planning of a tunnel blasting charging robot, applicable to a tunnel blasting charging robot equipped with a mobile chassis, a multi-degree-of-freedom robotic arm, an environmental perception module, a blast hole recognition module, and an industrial control computer. The method includes environmental mapping, robot pose calculation, target recognition, path planning, and operation execution steps, comprising the following steps: S1 uses the lidar and inertial navigation unit of the environmental perception module to build a 3D point cloud map of the tunnel in real time based on laser SLAM and calculate the robot's initial global pose; it also uses the borehole recognition module to simultaneously collect images and point cloud data of the tunnel face and obtain the 3D pose data of all boreholes. S2 constructs a dual-loop dynamic fusion framework consisting of a global pose correction loop and a local alignment fine-tuning loop, which fuses and solves the tunnel 3D map and the borehole 3D pose data: In the global pose correction loop, the identified borehole feature points are used as fixed anchor points for SLAM loop closure detection to correct the cumulative pose drift of the robot during long-distance movement; In the local alignment fine-tuning loop, based on the real-time acquired borehole 3D pose data, the relative pose deviation between the robot arm end and the target borehole is iteratively corrected, and finally the optimal global pose of the robot and the accurate target coordinates of the borehole are output. S3 uses the improved MQ-RRT* algorithm to generate a charging operation path based on the fused pose data: the axis-first sparse sampling strategy is executed with the tunnel center axis as the reference, the target offset probability is dynamically adjusted based on the real-time distance between the end of the robotic arm and the target blast hole, and the blast hole operation priority and charging safety constraints are introduced to construct a path optimization cost function, and finally a multi-segment continuous operation path is generated, which is divided into a rapid approach segment, a precise alignment segment and a smooth loading segment. During the S4 operation, real-time data on robot pose, environmental changes, and loading status are collected. When path tracking deviation exceeds the threshold, environmental obstacles or borehole pose updates are detected, local incremental path replanning and operation closed-loop correction are triggered to control the mobile chassis and multi-degree-of-freedom robotic arm to complete the fully automated loading operation.

[0008] Preferably, in step S1, the three-dimensional pose data of the blast hole is obtained through the fusion detection of an industrial camera and a lidar. Specifically, under low dust conditions, the industrial camera vision detection is the main method, supplemented by lidar point cloud detection; under high dust conditions, lidar point cloud detection is the main method, supplemented by vision detection. The conversion between pixel coordinates and world coordinates is completed through joint calibration, and the three-dimensional coordinates, normal direction, and borehole diameter parameters of the blast hole are calculated. At the same time, based on the historical time series data of tunnel surrounding rock deformation and blasting vibration, an LSTM time series prediction model is constructed. The model takes into account surrounding rock deformation monitoring data, historical blast hole offset data, and geological parameters, and outputs the blast hole pose prediction offset. The target coordinates of the blast hole are pre-corrected. The LSTM time series prediction model is a 3-layer LSTM network with a 1-layer fully connected layer. The input sequence length is 10 construction cycles, and the time step is 1 day.

[0009] Preferably, in step S3, the sparse sampling strategy prioritizing the tunnel axis is calculated using the following formula:

[0010] In the formula, The distance from the sampling point to the central axis of the tunnel is the lateral distance. This is the minimum distance from the sampling point to the tunnel wall. The positive extension length of the sampling point along the tunnel axis; This is the obstacle avoidance weight coefficient, with a value ranging from 0.8 to 1.2. The axis priority coefficient ranges from 0.5 to 1.0. The sampling probability decreases exponentially with the increase of the lateral distance between the sampling point and the tunnel center axis. When the distance between the sampling point and the obstacle is less than the safety threshold, the sampling probability is set to 0.

[0011] Preferably, in step S3, the formula for calculating the dynamic target bias probability is:

[0012] In the formula, The Euclidean distance from the end of the robotic arm to the target borehole. For a safe approach distance to the blast hole, a value of 0.5m to 1.0m is used. It is the Sigmoid activation function. The safety bias coefficient ranges from 0.6 to 0.9; based on distance matching, a three-level path constraint is applied: fast approach segment. Bias probability < 0.3, robotic arm movement speed ≤ 0.8 m / s, acceleration ≤ 0.5 m / s²; precision alignment section Bias probability 0.3~0.7, robotic arm movement speed ≤0.2m / s, acceleration ≤0.2m / s², and the angle between the path and the borehole axis ≤1°; smooth loading section The bias probability is greater than 0.7, the path is a straight line segment that is completely coincident with the borehole axis, the robotic arm feed speed is less than 0.02 m / s, and the acceleration is less than 0.05 m / s².

[0013] Preferably, in step S3, the priority of borehole operations is set according to the type of borehole: the priority weight of slotting holes is 1.0, the priority weight of auxiliary holes is 0.7, and the priority weight of peripheral holes is 0.5; based on the priority weight and the spatial distribution of boreholes, an improved ant colony algorithm is used to plan a global borehole traversal order without repetition or backtracking, and a continuous operation path is generated.

[0014] Preferably, in step S3, the complete expression of the path optimization cost function is:

[0015] In the formula, The cost is the distance between nodes. For path curvature cost, For the price of speed of movement, To avoid the safety costs; The weighting coefficient is used for the fast approach segment, with a value of [value]. The values ​​for the precise alignment section and the smooth loading section are:

[0016] Preferably, in step S4, the triggering conditions for local incremental path replanning are: robot pose tracking error > 15mm, environmental map change rate > 5%, detection of dynamic obstacles, and update of borehole pose; after triggering, only the affected path segments are reconstructed, while the unaffected path segments are retained, and the replanning response time is ≤ 200ms; at the same time, graded obstacle avoidance is performed based on obstacle type and risk level: low-risk obstacles moving at low speed and being > 0.5m away from the robot are smoothly bypassed, medium-risk obstacles moving at medium speed and being 0.2m~0.5m away from the robot trigger local incremental replanning, and high-risk obstacles moving at high speed and being < 0.2m away from the robot immediately trigger emergency stop and safe retreat of the robotic arm.

[0017] Preferably, in step S4, real-time dust concentration data within the tunnel is collected, and the sensor combination weights are adaptively switched based on the dust concentration level: when the dust concentration is ≤150mg / m³, visual detection weight is 70% and laser point cloud detection weight is 30%; when the dust concentration is 150mg / m³ < dust concentration ≤350mg / m³, visual detection and laser point cloud detection weights are each 50%; when the dust concentration is >350mg / m³, laser point cloud detection weight is 80% and visual detection weight is 20%, with inertial navigation data used for pose compensation throughout the process; simultaneously, force, displacement, and visual data during the charging process are collected in real-time to obtain charging quality parameters such as charging length and charging density, and the pose correction and path planning parameters of subsequent boreholes are optimized in reverse. For minor, moderate, and severe faults, a graded degradation operation strategy is implemented, namely, redundancy switching without stopping, pausing feed for recovery, and emergency shutdown and retreat to the safe zone.

[0018] On the other hand, the present invention provides a positioning and path planning system for a tunnel blasting charging robot, including a mobile chassis, a multi-degree-of-freedom robotic arm, an environmental perception module, a blast hole identification module and an industrial control computer, as well as a data fusion module and a path planning module; The environmental perception module is used to construct a 3D map of the tunnel using laser SLAM and calculate the robot's real-time pose. The borehole recognition module is used to acquire the three-dimensional pose data of the boreholes at the working face. The data fusion module has a built-in global pose correction closed loop and local alignment fine-tuning closed loop unit, which is used to dynamically fuse the tunnel 3D map and the 3D pose data of the blast hole, and simultaneously output the robot's optimal global pose and the precise target coordinates of the blast hole. The path planning module has a built-in improved MQ-RRT* algorithm unit, which is used to perform tunnel axis priority sparse sampling, dynamic target offset and charge safety constraint optimization based on the fused pose data, generate multi-segment charge operation path, and trigger local incremental path replanning. The industrial control computer is connected to each module, the mobile chassis, and the multi-degree-of-freedom robotic arm to issue control commands and execute closed-loop control of the entire loading operation process.

[0019] Compared with the prior art, the present invention has the following outstanding substantive features and significant progress: 1. This invention, through a dual-closed-loop design that integrates real-time laser SLAM mapping with dynamic fusion of borehole pose, completely solves the core problem of the disconnect between tunnel environment mapping and target positioning in existing technologies, eliminating reliance on preset borehole 3D poses and manual on-site calibration. By correcting the cumulative pose drift caused by long-distance SLAM operations using borehole feature anchor points, and by using a borehole pose offset prediction model to pre-correct borehole offsets caused by surrounding rock deformation and blasting vibrations, the accuracy of borehole positioning is significantly improved. In actual tests, after the robot moved 50m along the tunnel, the cumulative pose drift decreased from 4.2cm to less than 0.8cm. Under high dust conditions, the average borehole positioning error remained stable within 1.8mm, reducing problems such as over- or under-excavation and charge failure caused by charge position deviations from the source.

[0020] 2. This invention presents an improved MQ-RRT* algorithm specifically optimized for the confined space of tunnels and the safety constraints of explosive loading. It significantly reduces the proportion of invalid samples through a sparse sampling strategy prioritizing the tunnel axis, and guides rapid path convergence with a bias mechanism dynamically adjusted according to target distance. Combined with a global traversal path planned based on borehole priority, the total path length is reduced by 32% compared to conventional solutions, greatly improving the efficiency of continuous operations with dense boreholes. Simultaneously, the hierarchical motion constraints of the three-segment explosive loading path mitigate the risk of damage to detonating equipment caused by sudden movements and impacts during explosive loading, balancing operational efficiency and explosive loading safety.

[0021] 3. This invention forms a closed-loop operation process of "positioning-planning-execution-feedback-optimization". Through the sensor weight switching design that adapts to dust concentration, it can adapt to extreme tunnel conditions with high dust and high water mist. Even when the visual sensor fails completely, the average error of borehole positioning can still be ≤2.5mm. With the graded obstacle avoidance and incremental path replanning mechanism, the average response time of local incremental replanning is ≤172ms, which can quickly respond to sudden dynamic obstacles and environmental changes in the tunnel. The graded fault degradation operation strategy further improves the safety redundancy of the system. The fully automatic charging operation can be completed without manual entry into the high-risk area of ​​the tunnel face, which greatly reduces the personnel safety risks during construction and effectively shortens the single-cycle construction period of the tunnel, fully meeting the development needs of intelligent and safe construction of tunnel engineering. Attached Figure Description

[0022] Figure 1 Schematic diagram of the overall architecture of the tunnel blasting loading robot positioning and path planning system; Figure 2 : Overall flowchart of the positioning and path planning method for tunnel blasting charging robots; Figure 3 : Block diagram of the dual closed-loop correction principle of dynamic fusion of SLAM and borehole positioning; Figure 4: Flowchart of the improved MQ-RRT* algorithm for path planning; Figure 5 Comparison of path planning processes between the improved MQ-RRT* and existing algorithms in tunnel explosive loading scenarios; Figure 6 Flowchart of tunnel axis priority sparse sampling and schematic diagram of node growth process; Figure 7 Comparison curves of borehole positioning accuracy under different dust concentrations; Figure 8 Bar chart comparing single-cycle operation time and path length for different schemes; Figure 9 : Diagram of borehole priority and global traversal path at the working face; Figure 10 : Velocity and acceleration constraint curves of a three-stage charging operation path; Figure 11 : Schematic diagram of obstacle avoidance and incremental replanning process in the charge path; Figure 12 Comparison curves of measured and predicted values ​​of surrounding rock deformation and borehole position offset; Figure 13 Flowchart of graded fault degradation operation for tunnel explosive loading; Figure 14 Improved MQ-RRT* algorithm framework diagram. Detailed Implementation

[0023] The following embodiments illustrate the present invention in detail. In the description of these embodiments, specific details such as particular system structures and techniques are set forth for illustrative purposes and not for limitation, so as to provide a thorough understanding of the embodiments of the present invention. However, those skilled in the art will understand that the present invention can also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods are omitted so as not to obscure the description of the present invention with unnecessary detail.

[0024] It should be understood that, when used in this specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0025] It should also be understood that the term "and / or" as used in this specification and the appended claims refers to any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0026] As used in this specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrase "if determined" or "if [described condition or event] is detected" may be interpreted, depending on the context, as meaning "once determined," "in response to determination," "once [described condition or event] is detected," or "in response to detection of [described condition or event]."

[0027] Furthermore, in the description of this invention and the appended claims, the terms "first," "second," "third," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0028] References to "one embodiment" or "some embodiments" as described in this specification mean that one or more embodiments of the invention include a specific feature, structure, or characteristic described in connection with that embodiment. Therefore, the phrases "in one embodiment," "in some embodiments," "in other embodiments," "in still other embodiments," etc., appearing in different parts of this specification do not necessarily refer to the same embodiment, but rather mean "one or more, but not all, embodiments," unless otherwise specifically emphasized. The terms "comprising," "including," "having," and variations thereof mean "including but not limited to," unless otherwise specifically emphasized. Example 1

[0029] This embodiment is a basic implementation of the present invention, which details the complete architecture and full-process execution steps of the positioning and path planning system for tunnel blasting charging robots.

[0030] The overall architecture of the tunnel blasting charging robot positioning and path planning system in this embodiment is shown in the attached figure. Figure 1 As shown, the system includes a mobile chassis, a multi-degree-of-freedom robotic arm, an environmental perception module, a borehole recognition module, a data fusion module, a path planning module, and an industrial control computer. The environmental perception module includes a 16-line mechanical LiDAR and a high-precision six-axis IMU. The LiDAR is mounted on the top of the mobile chassis, and the IMU is rigidly connected to the LiDAR. The borehole recognition module includes a global industrial camera, a binocular depth camera, and a laser rangefinder sensor mounted at the end of the robotic arm. The data fusion module and the path planning module are integrated into the industrial control computer, which communicates with the drive controllers of each sensor, the mobile chassis, and the robotic arm, with a communication cycle of ≤10ms.

[0031] The overall process of the positioning and path planning method in this embodiment is shown in the attached figure. Figure 2 As shown, the complete execution steps are as follows: Step S1: System initialization and parameter calibration The loading robot was deployed to a safe area 12m behind the tunnel face, and the system was started to complete hardware self-tests and sensor calibrations. The lidar and IMU were synchronized using hardware triggering, with a synchronization error ≤1ms. The industrial camera and laser rangefinder sensor were jointly calibrated using a checkerboard calibration method. The calibration board size was 10mm×10mm, and 20 sets of calibration images from different angles were collected to calculate intrinsic and extrinsic parameters, with a calibration error ≤0.5 pixels. The pose calibration of the loading nozzle at the end of the robotic arm was completed using the TCP calibration method, with a calibration accuracy of ±0.1mm. After calibration, the core system parameters were set as follows: borehole recognition confidence threshold 0.95, alignment deviation warning threshold 1.5cm, safe distance between the robot and the tunnel wall 50cm, path planning grid resolution 0.1m, and emergency stop deviation threshold 3cm.

[0032] Step S2: Real-time SLAM mapping and robot pose calculation The lidar collects point cloud data of the tunnel environment at a frequency of 10Hz, and the IMU collects the robot's angular velocity and acceleration data at a frequency of 100Hz. Noisy point clouds with a distance ≤0.1m or ≥10m are removed by direct filtering. The normal distribution transformation registration algorithm is used to complete the matching of point clouds between adjacent frames. Combined with IMU data, the robot's motion posture changes are compensated to construct a 3D map of the tunnel in real time. At the same time, the ICP algorithm is used to calculate the robot's real-time pose in the map coordinate system to complete the robot's autonomous localization and environmental obstacle marking. Fixed obstacles and dynamic obstacles are distinguished and sent to the path planning module simultaneously.

[0033] Step S3: 3D pose recognition and acquisition of borehole After the robot moves to the working area of ​​the tunnel face blast hole, the blast hole recognition module is activated. The industrial camera acquires images of the rock wall at a frequency of 30fps. The acquired images are preprocessed sequentially by grayscale conversion, Gaussian filtering, and edge enhancement to eliminate interference from dust and rock wall textures. The YOLOv11 target detection model optimized for tunnel blast hole scenarios is used to screen blast hole areas with a confidence level ≥0.95 and output the pixel coordinates of the blast hole images. The blast hole contour is extracted by Canny edge detection, and the least squares method is used to complete ellipse fitting with a fitting error ≤0.5 pixels. The blast hole normal direction and aperture parameters are calculated. Combining the camera pinhole imaging model and the robot's real-time pose, the pixel coordinates are converted to the world coordinate system to obtain the 3D coordinates, normal pose, and aperture parameters of the blast hole. These are then packaged into a 3D pose data package and sent to the data fusion module.

[0034] Step S4: Dynamic fusion of SLAM and borehole pose This step uses the Extended Kalman Filter algorithm to complete the dual-loop data fusion. The principle of the dual-loop correction is attached. Figure 3 As shown, the algorithm includes a global pose correction closed loop and a local alignment fine-tuning closed loop. The core algorithm execution process is as follows: 1. Define the system state vector, where the system state at time k is:

[0035] in, This represents the robot's three-dimensional position in the world coordinate system. For the robot's heading angle, For the robot's travel speed, This represents the robot's turning angular velocity.

[0036] 2. State prediction process: Based on the optimal state estimate at time k-1, predict the prior state and covariance at time k:

[0037] in, Here is the state transition matrix. To control the input matrix, Let k be the control input at time k. Let be the prior state covariance matrix. Let be the system process noise covariance matrix.

[0038] 3. In the observation update process, the robot pose calculated by SLAM and the relative pose of the borehole output by the borehole recognition module are used as dual observation sources to construct the observation vector. Complete the Kalman gain calculation and state update:

[0039] in, For Kalman gain, For the observation matrix, To observe the noise covariance matrix, It is the identity matrix. This is the optimal state estimate at time k. This is the updated state covariance matrix.

[0040] In the global pose correction closed loop, the identified fixed borehole feature points are used as anchor points for SLAM loop closure detection. The Scan-Context descriptor is used to complete the loop closure matching between the historical frame and the current frame, correcting the cumulative pose drift caused by the robot's long-distance movement. In the local alignment fine-tuning closed loop, the borehole pose acquired in real time by the end-effector camera of the robotic arm is used as the observation value. The relative pose of the robot and the borehole is updated at high frequency to correct the alignment deviation. Finally, the robot's global optimal pose and the accurate target coordinates of the borehole are output.

[0041] Step S5: Path planning based on the improved MQ-RRT* algorithm This step uses the fused pose data and an improved MQ-RRT* algorithm to complete path planning. The overall algorithm flow is shown in the attached figure. Figure 4 As shown in the attached diagram, the algorithm framework is as follows. Figure 14 As shown, the core execution steps are as follows: 1. A sparse sampling strategy prioritizing tunnel axes; the sampling process and node growth process are shown in the attached figure. Figure 6 As shown, sampling weights are set based on the tunnel's central axis. The sampling probability decreases exponentially with increasing lateral distance between the sampling point and the axis. The formula for calculating the sampling probability is:

[0042] In the formula, The distance from the sampling point to the central axis of the tunnel is the lateral distance. This is the minimum distance from the sampling point to the tunnel wall. This is the effective extension length of the sampling point along the tunnel axis, used to avoid reverse sampling; This is the obstacle avoidance weight coefficient, with a value ranging from 0.8 to 1.2. This is the axis priority coefficient, with a value ranging from 0.5 to 1.0.

[0043] Synchronously set sampling constraint rules: the distance between the sampling point and the tunnel wall, anchor bolts, pipelines, and arches must be ≥ the robot's safe radius + 50mm; generating sampling points in obstacle areas is prohibited; the coordinates of the sampling point along the tunnel axis must meet the following requirements. in This represents the robot's current axis position. The axial extension length for a single sampling is 5~10m, to avoid invalid reverse sampling.

[0044] 2. The dynamic target offset mechanism for the borehole, based on the current pose of the borehole to be charged, dynamically adjusts the target offset probability according to the distance between the robot's end effector and the borehole, guiding the path to converge towards the borehole. The formula for calculating the target offset probability is:

[0045] In the formula, The Euclidean distance from the robot's end effector to the borehole. For a safe approach distance to the borehole, a value of 0.5~1.0m is used. It is the Sigmoid activation function. This is the safety bias coefficient, with a value ranging from 0.6 to 0.9.

[0046] Based on the above bias probability model, a three-level guidance control strategy of "rapid approach - precise alignment - smooth loading" is designed. The path planning is divided into three stages according to the distance, and different path planning features and motion constraints are matched.

[0047] 3. Path optimization mechanism for detonator safety constraints: To ensure the safety of detonator clamping and loading processes, path curvature constraints, motion velocity / acceleration constraints, and end effector attitude constraints are introduced in the parent node selection and path segment generation stages of the algorithm. A safety constraint cost term is added to the original cost function for parent node selection. The optimized complete cost function is as follows:

[0048] In the formula, The cost is the distance between nodes. For path curvature cost, For the price of speed of movement, To avoid the safety costs; This is a weighting coefficient, which is dynamically adjusted according to the work stage.

[0049] Using this cost function, the algorithm automatically selects parent nodes that are close, have low curvature, meet speed requirements, and are safe in obstacle avoidance, generating the optimal path that meets the safety requirements of pyrotechnic loading. Finally, it generates a multi-segmented pyrotechnic loading operation path consisting of a rapid approach segment, a precise alignment segment, and a smooth loading segment.

[0050] Step S6: Path Execution and Loop Closure Correction The planned path is divided into a rapid approach segment, a precise alignment segment, and a smooth loading segment, and is then sent to the mobile chassis and robotic arm for execution. During the operation, the deviation between the robot's pose and the planned path is monitored at a frequency of 10Hz, and the alignment deviation between the robotic arm's end effector and the borehole is monitored at a frequency of 30Hz. When the deviation exceeds the threshold, local trajectory correction is immediately initiated. Environmental point cloud data is collected in real time. When dynamic obstacles are detected, smooth detour, local incremental path replanning, and emergency shutdown operations are performed according to the obstacle distance and risk level. When the borehole pose is updated, the map change rate exceeds 5%, or the path tracking error exceeds 15mm, local incremental path replanning is triggered, and only the affected path segment is reconstructed. The replanning response time is ≤200ms.

[0051] Step S7: Online monitoring and parameter optimization of propellant loading quality The system collects clamping force, feed displacement, and borehole image data in real time during the charging process, and obtains quality parameters such as charge length and charge density. If the parameters are not up to standard, a reloading operation is automatically triggered. After a single cycle of operation is completed, the system stores the entire process data, generates an operation report, and optimizes the algorithm weights and sensor parameters based on the operation data to improve the system's adaptability to different scenarios. Example 2

[0052] This embodiment is a field application implementation of the present invention. The application scenario is a mountain tunnel of a two-way four-lane highway. The tunnel has a designed net width of 10.25m and a net height of 5.0m. It is constructed using the full-face drill and blast method, with a single-cycle design advance of 3.5m. The surrounding rock grade is Class IV. The total number of blast holes designed at the working face is 86, including 12 slotting holes with a designed diameter of 42mm, a depth of 3.8m, and a hole spacing of 40cm; 48 auxiliary holes with a designed diameter of 42mm, a depth of 3.5m, and a hole spacing of 60cm; and 26 peripheral holes with a designed diameter of 42mm, a depth of 3.5m, and a hole spacing of 50cm. The design coordinates of all blast holes were laid out on site using a total station. The environmental parameters at the work site are: ambient temperature 18℃, relative humidity 72%, average light intensity inside the tunnel 30 lux, and dust concentration fluctuation range of 50mg / m³ to 500mg / m³ during the operation.

[0053] The tunnel blasting charging robot system used is completely consistent with the architecture described in Example 1. The entire operation process is strictly executed according to the steps described in Example 1. The existing conventional SLAM navigation charging robot scheme and the manual charging scheme are used to complete parallel comparative tests under the same working conditions and the same blast hole design parameters. All test data are collected repeatedly for 3 cycles, and the average value is taken as the final result.

[0054] In this embodiment, a comparison of the path planning process between the improved MQ-RRT* algorithm and existing conventional algorithms is attached. Figure 5 As shown, it is evident that the algorithm of this invention has a shorter path, fewer invalid samples, and a faster convergence speed. Specific tests were conducted on the borehole positioning accuracy under different dust concentrations, and the test results are attached. Figure 7As shown, under low dust concentration conditions of 50 mg / m³, the average positioning error of the borehole of the present invention is 1.2 mm, while the average positioning error of the existing conventional solution is 5.2 cm. When the dust concentration rises to 200 mg / m³, the average positioning error of the borehole of the present invention only increases slightly to 1.5 mm, while the average positioning error of the existing conventional solution has increased to 8.7 cm. When the dust concentration reaches a high concentration of 500 mg / m³, the average positioning error of the borehole of the present invention remains stable at 1.8 mm, with the error fluctuation range always controlled within 0.5 mm, while the average positioning error of the existing conventional solution has increased to 12.6 cm, with three instances of visual recognition failure. During the test, the system strictly implemented an adaptive sensor weight switching strategy based on dust concentration. When the dust concentration was ≤150mg / m³, borehole recognition was primarily based on industrial camera visual detection, accounting for 70% of the weight, with LiDAR point cloud detection as a secondary measure, accounting for 30%. When the dust concentration was >150mg / m³ and ≤350mg / m³, the weights of visual detection and LiDAR point cloud detection were adjusted to 50% each. When the dust concentration was >350mg / m³, the system automatically switched to LiDAR point cloud detection as the primary method, accounting for 80% of the weight, with visual detection as a secondary measure, accounting for 20%. IMU inertial navigation data was used throughout the test for pose compensation to ensure positioning stability under high dust conditions.

[0055] The test results for the full-process charging operation of 86 boreholes in a single cycle are attached. Figure 8 As shown, the total time for this invention's solution is 42 minutes, with an average loading time of 29 seconds per hole and a total continuous operation path length of 36.2m. The existing conventional robot solution takes 78 minutes, with an average loading time of 54 seconds per hole and a total path length of 53.2m; the manual loading solution takes 165 minutes, with an average loading time of 115 seconds per hole. During the operation, the system strictly adheres to the hole prioritization and global traversal planning strategy. The hole prioritization and global traversal path at the working face are shown in the attached figure. Figure 9 As shown, based on tunnel blasting operation specifications and the function of blast holes, operation priorities are set for different types of blast holes. Among them, the slotting holes have the highest operation priority with a weight coefficient of 1.0, followed by auxiliary holes with a weight coefficient of 0.7, and peripheral holes with the lowest weight coefficient of 0.5. The algorithm is based on priority weights and the spatial distribution of blast holes, and uses an improved ant colony algorithm to optimize the global traversal order, generating a continuous operation path without repetition or backtracking. Compared with the existing conventional solution of traversing in row and column order, the total path length is shortened by 32%, which greatly reduces the invalid movement of the robotic arm and the mobile chassis.

[0056] During the operation, the system strictly adheres to the three-stage charging path constraint strategy. The velocity and acceleration constraints of the three-stage path are shown in the attached figure. Figure 10As shown, independent motion parameter constraints were set for the rapid approach section, precise alignment section, and smooth loading section. In the rapid approach section, the robot arm's end-effector velocity was ≤0.8 m / s, acceleration ≤0.5 m / s², and path curvature radius ≥0.3 m. In the precise alignment section, the robot arm's end-effector velocity was ≤0.2 m / s, acceleration ≤0.2 m / s², and the angle between the path and the borehole axis was ≤1°. In the smooth loading section, the robot arm's end-effector velocity was ≤0.02 m / s, acceleration ≤0.05 m / s², and the path was a straight line segment completely coinciding with the borehole axis without any turning. This constraint strategy ensured the smoothness of the loading process at the trajectory level, preventing any detonator impact or compression during the entire operation. Simultaneously, combined with a dual-loop fusion strategy, after the robot moved 50 m along the tunnel, it completed a loop correction through the borehole's characteristic anchor points, reducing the cumulative pose drift from 4.2 cm before correction to 0.8 cm after correction, completely solving the pose drift problem in long-distance operations.

[0057] This embodiment also includes a special test on dynamic obstacle avoidance and local incremental path replanning. The test simulated dynamic obstacle scenarios within the work area, including worker movement and small rockfalls. The obstacle avoidance and incremental replanning process for the charge path is shown in the attached figure. Figure 11 As shown, the system identifies obstacle types and motion states using real-time SLAM point cloud data, matching corresponding risk levels to dynamic obstacles. Low-risk obstacles (moving at low speeds and >0.5m from the robot) are smoothly bypassed via a local path without interrupting operations. Medium-risk obstacles (moving at medium speeds and 0.2m-0.5m from the robot) immediately decelerate and initiate local incremental path replanning. High-risk obstacles (moving at high speeds and <0.2m from the robot) immediately trigger emergency stop and safe retraction of the robotic arm. Test results show that the success rate of the hierarchical obstacle avoidance strategy of this invention reaches 98%, and the average response time for local incremental path replanning is 172ms, far lower than the 680ms of existing conventional solutions. Throughout the operation, the system collects quality parameters such as charge length and charge density in real time. The single-cycle charge qualification rate is 100%, with no missed or incorrect charges. The average over-excavation of the tunnel cross-section after blasting is 7.2cm, which is far below the 15cm limit allowed by the highway tunnel construction specifications. At the same time, based on the charge quality data, the system optimizes the borehole alignment accuracy threshold and path planning weight coefficient for subsequent cycles, further improving the stability of the operation. Example 3

[0058] This embodiment illustrates the application of the present invention under extremely complex working conditions. The application scenario is a highway tunnel with large deformation in soft rock. The surrounding rock grade of the tunnel is Class V, with a burial depth of 820m. Affected by high ground stress, the average daily deformation of the surrounding rock reaches 12mm, and the maximum cumulative deformation of the surrounding rock within a single construction cycle reaches 8cm. The measured offset of the blast holes at the working face is 3cm~8cm. There is a dynamic risk of loose rock falling after blasting in the work area. At the same time, there is an extreme visual interference environment with high dust and high water mist in the tunnel. The dust concentration reaches up to 800mg / m³, and the water mist concentration causes the effective recognition distance of conventional visual sensors to be less than 1m. This is an extreme and complex working condition for tunnel blasting construction.

[0059] Adopting the same system architecture and workflow as Example 1, and targeting the core working condition of large deformation in soft rock, a borehole pose displacement prediction model is activated. The model input parameters include real-time monitoring data of tunnel surrounding rock deformation, measured borehole displacement data from the first three cycles, blasting vibration monitoring data, and surrounding rock geological parameters. An LSTM time-series prediction model is used to predict the borehole pose displacement in advance. A comparison of the measured and predicted values ​​of surrounding rock deformation and borehole pose displacement is attached. Figure 12 As shown, the average deviation between the predicted and measured borehole pose offset values ​​is 2.1 mm, with a prediction accuracy exceeding 96%. During operation, the system inputs the predicted offset into the data fusion module beforehand to correct the borehole target coordinates. Combined with a dual closed-loop fusion strategy of SLAM and borehole positioning, even in the extreme case of a maximum borehole offset of 8 cm, it still achieved 100% borehole alignment accuracy and a 0% charge failure rate. In contrast, the existing conventional method using preset coordinates only achieved a borehole alignment accuracy of 72% and a charge failure rate of 28% under the same conditions, with 24 boreholes failing to complete charge loading due to excessive offset.

[0060] To address extreme visual interference conditions caused by high dust and water mist, the system adaptively switches sensor combination weights based on dust concentration levels. When the dust concentration exceeds 500 mg / m³ and the visual recognition confidence level remains below 0.85, the main visual recognition detection function is automatically shut down, and the system switches entirely to the lidar point cloud borehole recognition mode. The lidar collects point cloud data from the working face, extracts the concave features of the boreholes to complete borehole location recognition and pose calculation, and simultaneously uses IMU inertial navigation data to complete pose compensation with a 100ms cycle. Even when the visual sensors completely fail, the average borehole positioning error is still ≤2.5mm, ensuring the continuity of operations and completely solving the industry pain point of visual positioning failure under extreme conditions. In response to sudden dynamic rockfall scenarios during operations, a total of 50 rockfall tests with different sizes and falling speeds were simulated. The system was able to complete obstacle recognition, risk level determination and corresponding obstacle avoidance actions within 200ms. Among them, 42 low- and medium-risk scenarios achieved uninterrupted operation detour, and 8 high-risk scenarios accurately triggered emergency shutdown and safe retreat. There were no equipment collisions, operation interruptions and loss of control. The success rate of the graded obstacle avoidance strategy was 100%.

[0061] This embodiment also completed a full-scenario specialized test of the graded fault degradation operation strategy. The graded fault degradation operation process for tunnel explosive loading operations is attached. Figure 13 As shown, four typical fault scenarios were simulated: sensor data loss, communication interruption, positioning drift, and robotic arm jamming. Corresponding degradation operation strategies were set for different fault types and levels. For minor faults such as loss of industrial camera images or abnormal single-channel laser ranging data, the system automatically switches to redundant sensor combinations without stopping the machine or interrupting operations, only triggering a fault warning. For moderate faults such as short-term communication interruption or slight positioning drift, such as communication interruption between the industrial control computer and the robotic arm ≤2s or robot pose drift ≤2cm, the system immediately pauses the feeding action, maintains the current posture, and resumes operation after communication is restored and pose is corrected, without manual intervention. For severe faults such as complete sensor failure, communication interruption >2s, robotic arm jamming, or serious misalignment, the system immediately triggers an emergency stop. The robotic arm performs actions to release the gripper and return to a safe posture, the mobile chassis automatically retreats to a safe area behind the working face, and at the same time, an audible and visual alarm and a remote fault prompt are issued. In all fault scenario tests, the system accurately executed the corresponding degradation operation strategy, and there was no instance of fault escalation, equipment damage, or safety risk.

[0062] The full-process test results show that, under extreme tunnel conditions such as large deformation of surrounding rock, high dust and water mist, and frequent dynamic rockfalls, the present invention still achieved fully automated charging operations for 72 blast holes in a single cycle, with a total time of 38 minutes, an average charging time of 32 seconds per hole, a blast hole alignment accuracy of 100%, a charging qualification rate of 100%, and no manual intervention. This fully verifies the strong robustness and high reliability of the present invention under extremely complex conditions and is fully adaptable to the blasting charging construction needs of various complex tunnels.

[0063] The above-described embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included within the protection scope of the present invention.

Claims

1. A method for positioning and path planning of a tunnel blasting charging robot, applied to a tunnel blasting charging robot equipped with a mobile chassis, a multi-degree-of-freedom robotic arm, an environmental perception module, a blast hole recognition module, and an industrial control computer, comprising environmental mapping, robot pose calculation, target recognition, path planning, and operation execution steps, characterized in that, Includes the following steps: S1 uses the lidar and inertial navigation unit of the environmental perception module to build a 3D point cloud map of the tunnel in real time based on laser SLAM and calculate the robot's initial global pose; it also uses the borehole recognition module to simultaneously collect images and point cloud data of the tunnel face and obtain the 3D pose data of all boreholes. S2 constructs a dual-loop dynamic fusion framework consisting of a global pose correction loop and a local alignment fine-tuning loop. It fuses and solves the tunnel 3D map and the borehole 3D pose data: In the global pose correction loop, the identified borehole feature points are used as fixed anchor points for SLAM loop closure detection to correct the cumulative pose drift of the robot during long-distance movement; In the local alignment fine-tuning loop, based on the real-time acquired borehole 3D pose data, the relative pose deviation between the robot arm end and the target borehole is iteratively corrected, and finally the optimal global pose of the robot and the accurate target coordinates of the borehole are output. Based on the fused pose data, S3 uses an improved MQ-RRT* algorithm to generate a charging operation path: it executes an axis-first sparse sampling strategy with the tunnel center axis as the reference, dynamically adjusts the target offset probability based on the real-time distance between the robotic arm end and the target blast hole, and introduces blast hole operation priority and charging safety constraints to construct a path optimization cost function, finally generating a multi-segment continuous operation path divided into a rapid approach segment, a precise alignment segment, and a smooth loading segment. During the S4 operation, real-time data on robot pose, environmental changes, and blasting operation status are collected. When path tracking deviation exceeds the threshold, environmental obstacles, or borehole pose updates are detected, local incremental path replanning and operation closed-loop correction are triggered to control the mobile chassis and multi-degree-of-freedom robotic arm to complete the fully automated blasting operation.

2. The method for positioning and path planning of a tunnel blasting charging robot according to claim 1, characterized in that, In step S1, the three-dimensional pose data of the blast hole is obtained through the fusion detection of an industrial camera and a lidar. Specifically, under low dust conditions, the industrial camera vision detection is the main method, supplemented by lidar point cloud detection; under high dust conditions, lidar point cloud detection is the main method, supplemented by vision detection. The conversion between pixel coordinates and world coordinates is completed through joint calibration, and the three-dimensional coordinates, normal direction, and borehole diameter parameters of the blast hole are calculated. At the same time, based on the historical time series data of tunnel surrounding rock deformation and blasting vibration, an LSTM time series prediction model is constructed. The model takes into account surrounding rock deformation monitoring data, historical blast hole offset data, and geological parameters, and outputs the blast hole pose prediction offset. The target coordinates of the blast hole are pre-corrected. The LSTM time series prediction model is a 3-layer LSTM network with a 1-layer fully connected layer. The input sequence length is 10 construction cycles, and the time step is 1 day.

3. The method for positioning and path planning of a tunnel blasting charging robot according to claim 1, characterized in that, In step S3, the sampling probability calculation formula for the tunnel axis-priority sparse sampling strategy is as follows: In the formula, The distance from the sampling point to the central axis of the tunnel is the lateral distance. This is the minimum distance from the sampling point to the tunnel wall. The positive extension length of the sampling point along the tunnel axis; k1 and k2 are obstacle avoidance weight coefficients, with values ​​ranging from 0.8 to 1.2; k3 is the axis priority coefficient, with values ​​ranging from 0.5 to 1.0; the sampling probability decreases exponentially as the lateral distance between the sampling point and the tunnel center axis increases, and the sampling probability is set to 0 when the distance between the sampling point and the obstacle is less than the safety threshold.

4. The method for positioning and path planning of a tunnel blasting charging robot according to claim 1, characterized in that, In step S3, the formula for calculating the dynamic target bias probability is: In the formula, The Euclidean distance from the end of the robotic arm to the target borehole. For a safe approach distance to the blast hole, a value of 0.5m to 1.0m is used. It is the Sigmoid activation function. The safety bias coefficient ranges from 0.6 to 0.9; based on distance matching, a three-level path constraint is applied: fast approach segment. Bias probability < 0.3, robotic arm movement speed ≤ 0.8 m / s, acceleration ≤ 0.5 m / s²; precision alignment section For the smooth loading section, d < 0.2m, the bias probability is 0.3~0.7, the robotic arm movement speed is ≤0.2m / s, the acceleration is ≤0.2m / s², and the angle between the path and the borehole axis is ≤1°; for the smooth loading section, d < 0.2m, the bias probability is > 0.7, the path is a straight line segment completely coincident with the borehole axis, and the robotic arm feed speed is ≤0.02m / s and the acceleration is ≤0.05m / s².

5. The method for positioning and path planning of a tunnel blasting charging robot according to claim 1, characterized in that, In step S3, the priority of borehole operations is set according to the type of borehole: the priority weight of slotting holes is 1.0, the priority weight of auxiliary holes is 0.7, and the priority weight of peripheral holes is 0.

5. Based on the priority weight and the spatial distribution of boreholes, an improved ant colony algorithm is used to plan a global borehole traversal order without repetition or backtracking, and to generate a continuous operation path.

6. The method for positioning and path planning of a tunnel blasting charging robot according to claim 1, characterized in that, In step S3, the complete expression of the path optimization cost function is: In the formula, The cost is the distance between nodes. For path curvature cost, For the price of speed of movement, To avoid the safety costs; The weighting coefficient is used for the fast approach segment, with a value of [value]. The values ​​for the precise alignment section and the smooth loading section are... .

7. The method for positioning and path planning of a tunnel blasting charging robot according to claim 1, characterized in that, In step S4, the triggering conditions for local incremental path replanning are: robot pose tracking error > 15mm, environmental map change rate > 5%, detection of dynamic obstacles, and update of borehole pose. After triggering, only the affected path segments are reconstructed, while the unaffected path segments are retained, and the replanning response time is ≤ 200ms. At the same time, graded obstacle avoidance is performed based on obstacle type and risk level: low-risk obstacles moving at low speed and within > 0.5m of the robot are smoothly bypassed; medium-risk obstacles moving at medium speed and within 0.2m to 0.5m of the robot trigger local incremental replanning; and high-risk obstacles moving at high speed and within < 0.2m of the robot immediately trigger emergency stop and safe retreat of the robotic arm.

8. The method for positioning and path planning of a tunnel blasting charging robot according to claim 1, characterized in that, In step S4, real-time dust concentration data within the tunnel is collected, and the sensor combination weights are adaptively switched based on the dust concentration level: when the dust concentration is ≤150mg / m³, visual detection weight is 70% and laser point cloud detection weight is 30%; when the dust concentration is 150mg / m³ < dust concentration ≤350mg / m³, visual detection and laser point cloud detection weights are each 50%; when the dust concentration is >350mg / m³, laser point cloud detection weight is 80% and visual detection weight is 20%. Inertial navigation data is used for pose compensation throughout the process. Simultaneously, force, displacement, and visual data during the charging process are collected in real-time to obtain charging quality parameters, including charging length and charging density. The pose correction and path planning parameters of subsequent boreholes are optimized in reverse. For minor, moderate, and severe faults, a graded degradation operation strategy is implemented, namely, redundancy switching without stopping, pausing feed for recovery, and emergency shutdown and retreat to the safe zone.

9. A positioning and path planning system for a tunnel blasting charging robot, used to execute the method described in any one of claims 1-8, characterized in that, include: The environmental perception module is used to construct a 3D map of the tunnel using laser SLAM and calculate the robot's real-time pose. The borehole recognition module is used to acquire the three-dimensional pose data of the boreholes at the working face. The data fusion module has a built-in global pose correction closed loop and local alignment fine-tuning closed loop unit, which is used to dynamically fuse the tunnel 3D map and the 3D pose data of the blast hole, and simultaneously output the robot's optimal global pose and the precise target coordinates of the blast hole. The path planning module has a built-in improved MQ-RRT* algorithm unit, which is used to perform tunnel axis priority sparse sampling, dynamic target offset and charge safety constraint optimization based on the fused pose data, generate multi-segment charge operation paths, and trigger local incremental path replanning. The industrial control module communicates with each module and is used to issue control commands to execute closed-loop control of the entire loading operation process.

Images