A three-arm drill rig drilling path planning method and system

Abstract

The present application belongs to the field of tunnel engineering and rock drilling equipment automation, and discloses a three-arm rock drilling jumbo drilling path planning method and system, which comprises the following steps: collecting tunnel section parameters and blasthole coordinates, transmitting the collected data to the jumbo control system through the OPC protocol, and obtaining the rotary joints and moving joints of the three-arm rock drilling jumbo; based on the rotary joints and moving joints of the three-arm rock drilling jumbo, an improved dual-quaternion kinematics equation is constructed to obtain the pose of the mechanical arm of the three-arm rock drilling jumbo; based on the pose of the mechanical arm of the three-arm rock drilling jumbo and the blasthole distribution of the tunnel section, the working area is dynamically adjusted; based on the adjusted working area, a three-level optimization architecture of "global-local-real-time" is adopted to plan the drilling path of the three-arm rock drilling jumbo; and based on the planned drilling path of the three-arm rock drilling jumbo, a hybrid collision detection mechanism is adopted to realize the collision risk detection of the tunnel wall.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of tunnel engineering and automation technology of rock drilling equipment, and particularly relates to a three-arm rock drilling jumbo drilling path planning method and system. BACKGROUND

[0002] As the core equipment of the drill and blast method, the drilling path planning precision and efficiency of the three-arm rock drilling jumbo directly determine the tunnel construction quality and progress. The current three-arm rock drilling jumbo drilling path planning technology has the following key problems:

[0003] Insufficient drilling positioning accuracy: the existing technology is mostly based on the D-H method to establish a kinematics model of a mechanical arm, and realizes positioning in combination with an RBF neural network, but the D-H method needs to describe the rotation and translation movement in steps, has modeling redundancy, and the RBF neural network is prone to sensor data deviation in a strong interference environment of hard rock geology, resulting in an end positioning error exceeding 3mm, a blast hole position deviation and further affecting the tunnel cross section flatness after blasting, and additional roadway repair is needed.

[0004] Low hole sequence planning efficiency: the traditional hole sequence planning converts the problem into a 3-TSP (three traveling salesman problem), and solves it by using an adaptive genetic algorithm or a basic ant colony algorithm, wherein the basic ant colony algorithm needs 65 iterations to converge, and the planning task for 193 blast holes takes an average time of more than 576s, which is difficult to meet the needs of "real-time adjustment and rapid construction" on the construction site; and some methods only divide fixed work areas through the working range of the mechanical arm, without considering the task imbalance caused by uneven blast hole distribution.

[0005] Poor adaptability of regional division: the existing fixed three-region division method (lateral uniform division along the tunnel cross section) will cause the task amount of a single arm to exceed that of other arms by more than 20% when the blast holes are concentrated in a certain region, the equipment utilization is unbalanced, and the overall construction period is prolonged by 15% to 20%.

[0006] Insufficient collision risk control: the traditional collision detection adopts a single geometric modeling (such as a cylindrical envelope box), and the time consumption of a single detection is up to 1.31ms, which cannot meet the high-frequency control demand in the dynamic scene of three-arm collaborative work, and there is a safety hazard of collision between the mechanical arms or between the mechanical arms and the tunnel wall. SUMMARY

[0007] To solve the problems existing in the prior art, the present application provides a three-arm rock drilling jumbo drilling path planning method and system, which realizes high-precision, high-efficiency and low-risk drilling path planning through four core modules of "improved dual-quaternion kinematics modeling-six-region dynamic division-hierarchical path optimization-mixed collision detection".

[0008] To achieve the above object, the present application provides the following solutions:

[0009] A three-arm rock drilling jumbo drilling path planning method, the method comprising:

[0010] Collecting tunnel section parameters and blast hole coordinates, transmitting the collected data to the jumbo control system through the OPC protocol, and obtaining the rotation joints and movement joints of the three-arm rock drilling jumbo;

[0011] Based on the rotation joints and movement joints of the three-arm rock drilling jumbo, an improved dual-quaternion kinematics equation is constructed to obtain the pose of the three-arm rock drilling jumbo mechanical arm;

[0012] Based on the pose of the three-arm rock drilling jumbo mechanical arm and the distribution of the tunnel section blast holes, the working area is dynamically adjusted;

[0013] Based on the adjusted working area, a "global-local-real-time" three-level optimization architecture is adopted to plan the drilling path of the three-arm rock drilling jumbo;

[0014] Based on the planned drilling path of the three-arm rock drilling jumbo, a hybrid collision detection mechanism is adopted to realize the tunnel wall collision risk detection.

[0015] Preferably, the method for constructing an improved dual-quaternion kinematics equation based on the rotation joints and movement joints of the three-arm rock drilling jumbo to obtain the pose of the three-arm rock drilling jumbo mechanical arm comprises:

[0016] The improved dual-quaternion kinematics equation is:

[0017] dq=q_real+εq_dual;

[0018] Wherein, dq is a dual quaternion, which uniformly describes the rotation and translation of the mechanical arm; ε is a dual unit, which satisfies ε 2 =0; q_real is a real quaternion describing joint rotation, which is defined as:

[0019]

[0020] Wherein, θ' is the corrected rotation angle, and n' is the corrected rotation axis;

[0021] A fixed coupling coefficient C=0.02 is introduced to relate the translation error and the rotation correction amount, which is defined as follows:

[0022] θ'=θ+C·Δd;

[0023] Wherein, θ is the rotation angle, and Δd is the translation error, which is expressed as:

[0024] Δd=|d 实际 -d 理论 |;

[0025] Wherein, d 实际 is the actual translation distance, d理论 is the theoretical translation distance;

[0026] Correct the rotation axis according to the preset unit vector x of the translation path:

[0027] n' = n + Δn c ;

[0028] where Δn c is the difference, n is the rotation axis unit vector,

[0029] where L is the length of the trolley arm; q_dual is the dual quaternion describing joint translation, which is defined as:

[0030]

[0031] where d is the pitch, n x is the coefficient of the imaginary part i, representing the component of the quaternion in the i direction, which is usually related to the rotation axis or translation direction, n y is the coefficient of the imaginary part j, representing the component of the quaternion in the j direction, which is also related to the rotation axis or translation direction, n z is the coefficient of the imaginary part k, representing the component of the quaternion in the k direction, which is related to the rotation axis or translation direction.

[0032] Preferably, based on the pose of the three-arm rock drilling jumbo mechanical arm and the distribution of the tunnel section blast hole, the method for dynamically adjusting the working area comprises:

[0033] Divide the working area into left and right two large areas along the tunnel section central axis Y=0, and equally divide each large area along the Z axis to form six initial sub-areas, including: left 1-3 areas and right 1-3 areas, wherein the left 1 area Y ∈ [-5.91, -3.94] m, Z ∈ [2.01, 3.01] m, the left 2 area Y ∈ [-3.94, -1.97] m, Z ∈ [0, 2.01] m, the left 3 area Y ∈ [-1.97, 0] m, Z ∈ [-5.67, 0] m, and the right area is symmetrical to the left area.

[0034] Real-time statistics of the number of blast holes in each sub-area, if the number of blast holes in a certain area exceeds the average number by 5%, adjust the boundary of the adjacent area through the boundary offset algorithm until the deviation of the number of blast holes in each area is <5%.

[0035] Preferably, based on the planned three-arm rock drilling jumbo drilling path, a hybrid collision detection mechanism is used to realize the tunnel wall collision risk detection method, which comprises:

[0036] Simplify the mechanical arm connecting rod into four cylindrical envelope boxes, and simplify the tunnel section into a closed figure of "arc segment + 3 straight line segments", wherein the radius R of the arc segment is 5.8 m;

[0037] The shortest distance d_min1 between the mechanical arms is calculated as d_min1 = d_line-(r1+r2), wherein d_line is the shortest distance between the axes of the two cylinders, r1 and r2 are the radii of the connecting rods, the shortest distance d_min2 between the mechanical arms and the tunnel wall is calculated as d_min2 = min(R-l_max, d_line')-r3, wherein l_max is the maximum distance from the center of the tunnel to the axis of the connecting rod, d_line' is the distance between the axis of the connecting rod and the straight line segment of the tunnel, and r3 is the radius of the connecting rod;

[0038] The mechanical arm trajectory of the next 5 blast holes is generated based on the improved dual quaternion interpolation, and d_min1 and d_min2 on the trajectory are predicted;

[0039] If d_min1 is less than or equal to 50 mm or d_min2 is less than or equal to 50 mm, the local hole sequence rearrangement, i.e., the traversal order of the adjacent 3 blast holes, is triggered, and the global path is kept unchanged;

[0040] The expression of the improved dual quaternion interpolation is as follows:

[0041]

[0042] dq (τ) is the dual quaternion of the mechanical arm pose at the interpolation time, dq current is the dual quaternion of the mechanical arm pose of the current blast hole, dq next is the dual quaternion of the mechanical arm pose of the next blast hole, is the inverse dual quaternion of dq current ; τ is an interpolation coefficient, which describes the trajectory progress from the current blast hole to the next blast hole, and the expression of τ is as follows:

[0043]

[0044] t is the current motion time, and T is the predicted motion time from the current blast hole to the next blast hole, which is calculated from the motion speed v of the mechanical arm:

[0045]

[0046] d current_next is the Euclidean distance between the two blast holes.

[0047] The application also provides a three-arm drill jumbo drilling path planning system, which is used to implement the method described above, and the system comprises a collection module, a construction module, an adjustment module, an optimization module and a detection module.

[0048] The collection module is used to collect the tunnel section parameters and the blast hole coordinates, and transmit the collected data to the jumbo control system through an OPC protocol to obtain the rotation joints and the movement joints of the three-arm drill jumbo.

[0049] The construction module is configured to construct an improved dual-quaternion kinematics equation based on the rotation joint and the movement joint of the three-arm drill jumbo, and obtain the pose of the three-arm drill jumbo mechanical arm;

[0050] The adjustment module is configured to dynamically adjust the operation area based on the pose of the three-arm drill jumbo mechanical arm and the tunnel section blast hole distribution;

[0051] The optimization module is configured to perform three-arm drill jumbo drilling path planning based on the adjusted operation area by adopting a three-level optimization architecture of "global-local-real-time";

[0052] The detection module is configured to realize tunnel wall collision risk detection by adopting a hybrid collision detection mechanism based on the planned three-arm drill jumbo drilling path.

[0053] Preferably, the process of constructing an improved dual-quaternion kinematics equation based on the rotation joint and the movement joint of the three-arm drill jumbo, and obtaining the pose of the three-arm drill jumbo mechanical arm includes:

[0054] The improved dual-quaternion kinematics equation is:

[0055] dq=q_real+εq_dual;

[0056] wherein dq is a dual quaternion, which uniformly describes the rotation and translation of the mechanical arm; ε is a dual unit, which satisfies ε 2 =0; q_real is a real quaternion describing joint rotation, which is defined as:

[0057]

[0058] wherein θ' is a corrected rotation angle, and n' is a corrected rotation axis;

[0059] A fixed coupling coefficient C=0.02 is introduced to associate the translation error and the rotation correction, which is defined as follows:

[0060] θ'=θ+C·Δd;

[0061] wherein θ is a rotation angle, and Δd is a translation error, which is expressed as:

[0062] Δd=|d 实际 -d 理论 |;

[0063] wherein d 实际 is an actual translation distance, and d 理论 is a theoretical translation distance;

[0064] The rotation axis is corrected according to the preset unit vector x of the translation path:

[0065] n'=n+Δnc ;

[0066] wherein, Δn c is the difference, n is the unit vector of the rotation axis,

[0067] wherein, L is the length of the trolley arm; q_dual is the dual quaternion describing the joint translation, which is defined as:

[0068]

[0069] wherein, d is the pitch, n x is the coefficient of the imaginary part i, representing the component of the quaternion in the i direction, which is usually related to the rotation axis or the translation direction, n y is the coefficient of the imaginary part j, representing the component of the quaternion in the j direction, which is also related to the rotation axis or the translation direction, n z is the coefficient of the imaginary part k, representing the component of the quaternion in the k direction, which is related to the rotation axis or the translation direction.

[0070] Preferably, based on the pose of the three-arm rock drilling jumbo mechanical arm and the distribution of the tunnel section blast hole, the process of dynamically adjusting the working area includes:

[0071] Divide the working area into left and right two large areas along the tunnel section central axis Y=0, and equally divide each large area along the Z axis into three, forming six initial sub-areas, including: left 1-3 areas and right 1-3 areas, wherein the left 1 area Y∈[-5.91,-3.94]m, Z∈[2.01,3.01]m, the left 2 area Y∈[-3.94,-1.97]m, Z∈[0,2.01]m, the left 3 area Y∈[-1.97,0]m, Z∈[-5.67,0]m, and the right area is symmetrical to the left area;

[0072] Real-time statistics of the number of blast holes in each sub-area, if the number of blast holes in a certain area exceeds the average number by 5%, then adjust the boundary of the adjacent area through the boundary offset algorithm until the deviation of the number of blast holes in each area is <5%.

[0073] Preferably, the detection module includes: a static detection unit, a dynamic detection unit, and a risk processing unit.

[0074] The static detection unit is used for simplifying the mechanical arm connecting rod into four cylindrical envelope boxes and simplifying the tunnel section into a closed figure of "arc segment + three straight line segments", wherein the radius R of the arc segment is 5.8 m; the shortest distance d_min1 between the mechanical arms is calculated as d_min1 = d_line-(r1+r2), wherein d_line is the shortest distance between two cylindrical axes, r1 and r2 are the connecting rod radii, the shortest distance d_min2 between the mechanical arm and the tunnel wall is calculated as d_min2 = min(R-l_max, d_line')-r3, wherein l_max is the maximum distance from the tunnel center to the connecting rod axis, d_line' is the distance between the connecting rod axis and the tunnel straight line segment, and r3 is the connecting rod radius;

[0075] The dynamic detection unit is used for generating the mechanical arm trajectory of the next five blast holes based on the improved dual quaternion interpolation, and predicting d_min1 and d_min2 on the trajectory.

[0076] The risk processing unit is used for triggering local hole sequence rearrangement, i.e., adjusting the traversal order of the adjacent three blast holes, while keeping the global path unchanged, if d_min1<=50 mm or d_min2<=50 mm.

[0077] The expression of the improved dual quaternion interpolation is as follows:

[0078]

[0079] dq(τ) is the mechanical arm posture dual quaternion during interpolation, dq current is the mechanical arm posture dual quaternion of the current blast hole, dq next is the mechanical arm posture dual quaternion of the next blast hole, is the inverse dual quaternion of dq current ; τ is an interpolation coefficient, which describes the trajectory progress from the current blast hole to the next blast hole, and the expression of τ is as follows:

[0080]

[0081] Wherein t is the current motion time, and T is the predicted motion time from the current blast hole to the next blast hole, which is calculated according to the mechanical arm motion speed v:

[0082]

[0083] Wherein d current_next is the Euclidean distance between two blast holes.

[0084] Compared with the prior art, the present application has the following beneficial effects:

[0085] This invention provides an improved dual quaternion kinematics modeling method, which overcomes the redundancy defects of the DH method and controls the end positioning error of the three-arm drilling rig within 1.2mm, meeting the high-precision drilling requirements of hard rock tunnels.

[0086] This invention proposes a six-region dynamic division strategy to achieve a task load deviation of less than 5% for each robotic arm, thus solving the problem of uneven equipment utilization caused by fixed region division.

[0087] This invention designs a hierarchical path optimization architecture, which improves the convergence speed of hole sequence planning by more than 35% and shortens the overall drilling cycle.

[0088] This invention constructs a hybrid collision detection mechanism, reducing the response time of a single detection to <1ms, thus ensuring the safety of three-arm collaborative operation. Attached Figure Description

[0089] To more clearly illustrate the technical solution of the present invention, the drawings used in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0090] Figure 1 This is a schematic diagram of a drilling path planning method for a three-arm rock drilling rig according to an embodiment of the present invention;

[0091] Figure 2 This is a schematic diagram of the drilling principle of the rock drilling rig in Embodiment 3 of the present invention. Detailed Implementation

[0092] 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.

[0093] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0094] Example 1

[0095] like Figure 1 , Figure 2 As shown, this invention provides a drilling path planning method for a three-arm drilling rig, applicable to high-precision, high-efficiency, and low-collision-risk multi-arm collaborative drilling operations in drill-and-blast tunnel excavation. The method includes:

[0096] Collect tunnel section parameters and blast hole coordinates, transmit the collected data to the trolley control system through the OPC protocol, and obtain the rotation joint and movement joint of the three-arm rock drilling trolley;

[0097] Based on the rotation joint and movement joint of the three-arm rock drilling trolley, an improved dual quaternion kinematics equation is constructed to obtain the pose of the mechanical arm of the three-arm rock drilling trolley;

[0098] Based on the pose of the mechanical arm of the three-arm rock drilling trolley and the distribution of the blast holes in the tunnel section, the working area is dynamically adjusted;

[0099] Based on the adjusted working area, a three-level optimization architecture of "global-local-real-time" is adopted to plan the drilling path of the three-arm rock drilling trolley;

[0100] Based on the planned drilling path of the three-arm rock drilling trolley, a hybrid collision detection mechanism is adopted to realize the collision risk detection of the tunnel wall.

[0101] In this embodiment, the improved dual quaternion kinematics modeling technology (solves the problem of insufficient positioning accuracy) is as follows:

[0102] Based on the improved dual quaternion (Dual Quaternion) unified description of the rotation and translation of the mechanical arm, the redundancy of the D-H method is avoided, and the improved dual quaternion kinematics equation is as follows:

[0103] dq = q_real + εq_dual;

[0104] Wherein, dq is a dual quaternion, which uniformly describes the rotation and translation of the mechanical arm; ε is a dual unit, which satisfies ε 2 = 0; q_real is a real quaternion that describes joint rotation, which is defined as:

[0105]

[0106] Wherein, θ' is the corrected rotation angle, and n' is the corrected rotation axis; in the traditional dual quaternion model, the real part (rotation) and the dual part (translation) are completely independent, the rotation is only determined by the theoretical rotation angle and the fixed rotation axis, and the rotation axis offset caused by the translation error is ignored, and the translation is only determined by the fixed pitch d, and the translation lag caused by the rotation angle deviation is ignored. In the tunnel rock drilling trolley, there is a fixed gap in the joint, and the independence of the gap will cause the accumulation of end positioning error (especially when the error exceeds 5% during long arm operation), and a fixed coupling coefficient C = 0.02 (the gap error accounts for the arm length ratio usually < 5%, taking the middle value 0.02) is introduced to correlate the translation error and the rotation correction, which is defined as:

[0107] θ' = θ + C·Δd;

[0108] Wherein, θ is the rotation angle, and Δd is the translation error, and its expression is:

[0109] Ad = |d 实际 -d 理论 |;

[0110] wherein d 实际 is the actual translation distance, d 理论 is the theoretical translation distance;

[0111] The greater the translation error, the more the rotation angle needs to be adjusted in synchronization to offset the rotation misalignment caused by the translation tilt. According to the preset unit vector x of the translation path, the rotation axis is corrected:

[0112] n' = n + An c ;

[0113] wherein n is the rotation axis unit vector, An c is the difference value,

[0114] wherein L is the length of the trolley arm; q_dual is a dual quaternion describing joint translation, which is defined as:

[0115]

[0116] wherein d is the pitch, n x is the coefficient of the imaginary part i, representing the component of the quaternion in the i direction, which is usually related to the rotation axis or the translation direction, n y is the coefficient of the imaginary part j, representing the component of the quaternion in the j direction, which is also related to the rotation axis or the translation direction, n z is the coefficient of the imaginary part k, representing the component of the quaternion in the k direction, which is related to the rotation axis or the translation direction.

[0117] Parameter adaptation: for the 9 joints of the three-arm rock drilling jumbo (6 rotary joints Θ1-Θ7, 3 moving joints D3, D8, D9), the improved D-H parameters are substituted (such as the intermediate arm joint 2 offset distance 480 mm, right arm -480 mm), the forward kinematics of the mechanical arm is generated, and the working space is ensured to cover the tunnel section YOZ plane (Y ∈ [-5.91, 5.88] m, Z ∈ [-5.67, 3.01] m).

[0118] Effect: Compared with the traditional D-H method, the modeling calculation efficiency is improved by 40%, the positioning accuracy is reduced from more than 3 mm to within 1.2 mm, and the accuracy of the blast hole position is ensured.

[0119] In this embodiment, the six-region dynamic partitioning algorithm (which solves the poor adaptability of region partitioning) is:

[0120] Based on the dynamic adjustment of the working region according to the blast hole distribution of the tunnel section, the task balance is realized:

[0121] Initial partition: along the tunnel section axis (Y=0) the working area is divided into left and right two large areas, each large area is divided into three along the Z axis (height direction), forming six initial sub-regions (left 1-3 and right 1-3), among which left 1 region Y∈[-5.91,-3.94]m, Z∈[2.01,3.01]m, left 2 region Y∈[-3.94,-1.97]m, Z∈[0,2.01]m, left 3 region Y∈[-1.97,0]m, Z∈[-5.67,0]m, right region is symmetrical;

[0122] Dynamic boundary adjustment: real-time statistics of the number of holes in each sub-region, if the number of holes in a certain region exceeds the average number by 5%, then adjust the boundary of adjacent regions through boundary offset algorithm (offset step 50mm) until the deviation of the number of holes in each region is <5%;

[0123] Task allocation: adopt the "left first and right second" strategy, and cooperate to complete the task, avoiding path redundancy caused by moving across large areas.

[0124] Effect: the task quantity deviation of each robot arm is reduced from more than 20% to <5%, the balance of equipment utilization is improved by 18%, and the overall construction period is shortened.

[0125] In this embodiment, the hierarchical path optimization strategy (to solve the low efficiency of hole sequence planning) is:

[0126] Adopting a "global-local-real-time" three-level closed-loop feedback architecture improves planning efficiency:

[0127] Global optimization layer: taking "shortest total moving distance", "lithology adaptation" and "device load" as the target, a multi-objective adaptive genetic algorithm is used to generate an initial feasible solution, and the multi-objective adaptive function model is as follows:

[0128]

[0129] Among them, according to the engineering priority, set the weight coefficients ω1=0.4, ω2=0.3, ω3=0.2;D total =Σd ij , that is, the total moving distance, where d ij is the Euclidean distance, as the path weight:

[0130]

[0131] Among them, (x i ,y i ,z i ), (x j ,y j ,z j ) represent the coordinates of the hole, C lithoC represents the lithological concentration, indicating the proportion of consecutive borehole sequences with the same lithology within a group. For example, if 18 out of 20 boreholes in a group are consecutive boreholes of the same lithology, then C... litho =0.9; σ load The load variance represents the variance of the number of times the robotic arm moves in each group. The smaller the value, the more balanced the load.

[0132] The population size is dynamically adjusted iteratively and can be divided into three stages: early stage (1-5 iterations, population size 80, crossover probability 0.9, mutation probability 0.08); mid-stage (6-10 iterations, population size 50, crossover probability 0.8, mutation probability 0.05); and late stage (11-15 iterations, population size 30, crossover probability 0.7, mutation probability 0.03). Both the number of iterations and the population size can be dynamically adjusted.

[0133] After 15 iterations, output the globally optimal hole order (such as the initial traversal order of the 31 holes in the left 1 area);

[0134] Local optimization layer: Introducing a dynamic candidate list ant colony algorithm to optimize detailed paths:

[0135] Candidate list: Restrict the ant's next node to "the 5 most recent unvisited holes" to reduce computational load;

[0136] Roulette wheel rewards and penalties: The probability of transition is:

[0137]

[0138] Where, η ij (t), η is (t) represents the heuristic function, τ ij (t), τ is (t) represents the pheromone concentration on edges ij and is, respectively, where i, j, and s are nodes, and allowed k The five nodes closest to the current node are used as heuristic factors, with α = 1.2 and β = 2.5. The smoothing heuristic function is:

[0139]

[0140] Where α1 = 0.6 and α2 = 0.4 are weighting coefficients. For motion smoothness, i.e. Δθ ij d represents the total change in the robot arm joint angle from the current hole to the next hole. ij Node distance is:

[0141]

[0142] wherein (x i ,y i ), (x j ,y j ) represent node coordinates;

[0143] Reward the first 30% probability nodes in transition probability (probability x 1.2), and punish the last 20% nodes (probability x 0.8) to accelerate convergence;

[0144] Real-time adjustment layer: based on the construction progress (such as the number of completed drill holes / total number), dynamically update the remaining hole sequence, if the lithology of a certain area becomes hard, causing the drilling time to be prolonged, automatically prioritize the completion of blast holes in adjacent areas to avoid the mechanical arm waiting for nothing.

[0145] Effect: Hole sequence planning convergence speed is improved, the planning time for 193 blast holes is reduced from 576s to 558.7s, and the total moving distance is shortened from 133.3mm to 128.6mm.

[0146] In this embodiment, the mixed collision detection mechanism (to solve the problem of insufficient collision risk control) is:

[0147] Combined with static geometric modeling and dynamic trajectory prediction, fast detection is realized:

[0148] Static detection:

[0149] 1. Simplify the mechanical arm connecting rod into 4 cylindrical envelope boxes (such as connecting rod 1 radius 180mm), and simplify the tunnel section into a closed figure of "circular arc segment (radius R=5.8m) + 3 straight line segments";

[0150] 2. Calculate the shortest distance d_min1 between the mechanical arms = d_line-(r1+r2) (d_line is the shortest distance between the axes of the two cylinders, r1 and r2 are the radii of the connecting rods), and the shortest distance d_min2 between the mechanical arm and the tunnel wall = min(R-l_max, d_line')-r3 (l_max is the maximum distance from the tunnel center to the connecting rod axis, d_line' is the distance between the connecting rod axis and the tunnel straight line segment, r3 is the connecting rod radius);

[0151] 3. Simplify the debris accumulation obstacle into a cuboid (according to the construction record statistics debris accumulation range), if the shortest distance between the mechanical arm cylinder and the debris cuboid is <30mm, the mechanical arm stops moving and triggers the cleaning reminder.

[0152] Dynamic prediction: the double quaternion interpolation expression is:

[0153]

[0154] wherein dq(τ) is the double quaternion of the mechanical arm pose at the interpolation time, dq currentdq is a dual quaternion of the mechanical arm pose of the current blast hole next dq is a dual quaternion of the mechanical arm pose of the next blast hole dq is a dual quaternion of the mechanical arm pose of the current blast hole current dq is an inverse dual quaternion of dq to ensure the closure of the rigid body motion of the interpolation; tau is an interpolation coefficient, which describes the progress of the trajectory from the current blast hole to the next blast hole, and its expression is:

[0155]

[0156] wherein t is the current motion time, and T is the predicted motion time from the current blast hole to the next blast hole, which is calculated by the motion speed v of the mechanical arm:

[0157]

[0158] wherein d current_next is the Euclidean distance between the two blast holes.

[0159] According to the adaptive prediction step of the dual quaternion interpolation result, when the actual drilling time is greater than 1.5 times of the theoretical time (such as slow drilling in hard rock), the prediction step is shortened to 3 blast holes, and the accumulation of uncertainty is reduced; when the actual drilling time is less than 1.2 times of the theoretical time, the prediction step is increased to 7 blast holes, and the prediction range is expanded. Predict the d_min1 and d_min2 on the trajectory.

[0160] Risk processing: if d_min1 is less than or equal to 50mm or d_min2 is less than or equal to 50mm (safety threshold), trigger local hole sequence rearrangement (adjust the traversal order of adjacent 3 to 7 blast holes), and keep the global path unchanged.

[0161] Effect: the time consumption of single collision detection is reduced from 1.31ms to <1ms, the accuracy of collision detection between mechanical arms is greatly improved, and the risk of collision with the tunnel wall is reduced.

[0162] Compared with the prior art, the technical effects of the present application are:

[0163] Precision improvement: the improved dual quaternion modeling combined with error compensation, the end positioning error is less than or equal to 1.2mm, the blast hole position deviation rate is less than 0.1%, and the overbreak and underbreak of the tunnel section after blasting is reduced to within 50mm, without the need for additional repair;

[0164] Efficiency optimization: six-region dynamic division makes the task deviation of each arm less than 5%, and hierarchical path optimization shortens the total moving distance by 3.53% (133.3mm→128.6mm), shortens the total drilling time by 3.04% (576.2s→558.7s), and improves the overall construction efficiency by 19%;

[0165] Safety enhancement: hybrid collision detection response <1ms, mechanical arm collision times from 242 times per thousand group detection to <10 times, tunnel wall collision rate from 20.8% (left arm), 12.0% (middle arm), 19.6% (right arm) to less than 5%;

[0166] Adaptability improvement: dynamic area division can adapt to blast hole density of 0.5-2 / m 2 Different distribution scenarios without manual parameter adjustment;

[0167] Resource conservation: task balancing makes the energy consumption deviation of each arm <2%, the total energy consumption is reduced from 93341.7J to 91798.7J, and the energy consumption is reduced by 1.68%.

[0168] Embodiment two

[0169] The application also provides a three-arm drill jumbo drilling path planning system, which is used to realize the method of embodiment one, and the system comprises a collection module, a construction module, an adjustment module, an optimization module and a detection module.

[0170] The collection module is used to collect tunnel section parameters and blast hole coordinates, and transmit the collected data to the jumbo control system through an OPC protocol to obtain the rotation joints and movement joints of the three-arm drill jumbo.

[0171] The construction module is used to construct an improved dual-quaternion kinematics equation based on the rotation joints and movement joints of the three-arm drill jumbo to obtain the pose of the three-arm drill jumbo mechanical arm.

[0172] The adjustment module is used to dynamically adjust the working area based on the pose of the three-arm drill jumbo mechanical arm and the tunnel section blast hole distribution.

[0173] The optimization module is used to perform three-arm drill jumbo drilling path planning based on the adjusted working area by adopting a "global-local-real-time" three-level optimization architecture.

[0174] The detection module is used to detect the tunnel wall collision risk based on the planned three-arm drill jumbo drilling path by adopting a hybrid collision detection mechanism.

[0175] In this embodiment, the process of constructing an improved dual-quaternion kinematics equation based on the rotation joints and movement joints of the three-arm drill jumbo to obtain the pose of the three-arm drill jumbo mechanical arm comprises:

[0176] The improved dual-quaternion kinematics equation is:

[0177] dq=q_real+εq_dual;

[0178] wherein dq is a dual quaternion, which uniformly describes the rotation and translation of the mechanical arm; ε is a dual unit, which satisfies ε2 = 0; q real is a real quaternion describing joint rotation, which is defined as:

[0179]

[0180] where θ' is the corrected rotation angle, and n' is the corrected rotation axis; in the traditional dual quaternion model, the real part (rotation) and the dual part (translation) are completely independent, the rotation is determined only by the theoretical rotation angle and the fixed rotation axis, ignoring the rotation axis deviation caused by the translation error, and the translation is determined only by the fixed pitch d, ignoring the translation lag caused by the rotation angle deviation. In the tunnel jumbo, there is a fixed gap in the joint, and the independence of the gap will cause the cumulative error of the end positioning (especially when the long arm is working, the error is more than 5%), and here a fixed coupling coefficient C = 0.02 (the gap error accounts for the arm length ratio usually < 5%, taking the middle value 0.02) is introduced to associate the translation error and the rotation correction, which is defined as follows:

[0181] θ' = θ + C - Δd;

[0182] where θ is the rotation angle, and Δd is the translation error, and its expression is:

[0183] Δd = |d 实际 - d 理论 |; θ' = θ + C - Δd;

[0184] where d 实际 is the actual translation distance, and d 理论 is the theoretical translation distance;

[0185] The greater the translation error, the more the rotation angle needs to be adjusted synchronously to offset the rotation misalignment caused by the translation tilt. According to the path preset unit vector x of the translation, the rotation axis is corrected as:

[0186] n' = n + Δn c ;

[0187] where n is the rotation axis unit vector, and Δn c is the difference,

[0188] where L is the length of the jumbo arm; q dual is a dual quaternion describing joint translation, which is defined as:

[0189]

[0190] where d is the pitch, n x is the coefficient of the imaginary part i, representing the component of the quaternion in the i direction, which is usually related to the rotation axis or the translation direction, n y is the coefficient of the imaginary part j, representing the component of the quaternion in the j direction, which is also related to the rotation axis or the translation direction, and n zis the coefficient of the imaginary part k, representing the component of the quaternion in the k direction, which is related to the rotation axis or the direction of translation.

[0191] In the embodiment, based on the pose of the three-arm drill jumbo mechanical arm and the tunnel section blasthole distribution, the process of dynamically adjusting the working area includes:

[0192] Along the tunnel section central axis Y=0, the working area is equally divided into left and right two large areas, each large area is equally divided into three along the Z axis, forming six initial sub-areas, including: left 1-3 areas and right 1-3 areas, wherein the left 1 area Y∈[-5.91,-3.94]m, Z∈[2.01,3.01]m, the left 2 area Y∈[-3.94,-1.97]m, Z∈[0,2.01]m, the left 3 area Y∈[-1.97,0]m, Z∈[-5.67,0]m, and the right area is symmetrical;

[0193] The number of blastholes in each sub-area is counted in real time, and if the number of blastholes in a certain area exceeds the average number by 5%, the boundary offset algorithm is used to adjust the boundary of the adjacent area until the deviation of the number of blastholes in each area is less than 5%.

[0194] In the embodiment, the detection module includes a static detection unit, a dynamic detection unit, and a risk processing unit.

[0195] Static detection:

[0196] 1. Simplify the mechanical arm connecting rod into four cylindrical envelope boxes (such as connecting rod 1 with a radius of 180 mm), and simplify the tunnel section into a closed figure of "arc segment (radius R=5.8m)+3 straight line segments";

[0197] 2. Calculate the shortest distance d_min1 between the mechanical arms =d_line-(r1+r2) (d_line is the shortest distance between the two cylindrical axes, r1 and r2 are the radii of the connecting rods), and the shortest distance d_min2 between the mechanical arm and the tunnel wall =min(R-l_max,d_line')-r3 (l_max is the maximum distance from the tunnel center to the connecting rod axis, d_line' is the distance between the connecting rod axis and the tunnel straight line segment, and r3 is the radius of the connecting rod);

[0198] 3. Simplify the rock debris accumulation obstacle into a cuboid (according to the construction record to count the rock debris accumulation range), and if the shortest distance between the mechanical arm cylinder and the rock debris cuboid is less than 30 mm, a cleaning reminder is triggered.

[0199] Dynamic detection: the double quaternion interpolation expression is:

[0200]

[0201] wherein dq(τ) is the double quaternion of the mechanical arm pose at the interpolation time, dq currentdq next is the dual quaternion of the pose of the mechanical arm for the next blast hole, is the inverse dual quaternion of dq current to ensure the closure of the interpolated rigid body motion; τ is the interpolation coefficient, which describes the progress of the trajectory from the current blast hole to the next blast hole, and its expression is:

[0202]

[0203] where t is the current motion time, and T is the predicted motion time from the current blast hole to the next blast hole, which is calculated from the motion speed v of the mechanical arm:

[0204]

[0205] where d current_next is the Euclidean distance between the two blast holes.

[0206] According to the adaptive prediction step based on the dual quaternion interpolation result, when the actual drilling time is greater than 1.5 times the theoretical time (such as slow drilling in hard rock), the prediction step is shortened to 3 blast holes, reducing the accumulation of uncertainty; when the actual drilling time is less than 1.2 times the theoretical time, the prediction step is increased to 7 blast holes, expanding the prediction range. Predict d_min1 and d_min2 on the trajectory.

[0207] Risk handling: if d_min1 ≤ 50 mm or d_min2 ≤ 50 mm (safety threshold), trigger local hole sequence rearrangement (adjust the traversal order of the adjacent 3 to 7 blast holes), and keep the global path unchanged.

[0208] Example Three

[0209] Taking a hard rock tunnel construction scene (section size: 11.7 m wide, 8.7 m high, total number of blast holes 193, of which 32 are predrilling holes, 61 are peripheral holes, and 100 are auxiliary holes) as an example, the implementation steps of the present application are described in detail:

[0210] 1. Data preprocessing

[0211] Collect the tunnel section parameters (center coordinates (0, 3.5) m, arc radius R = 5.8 m) and blast hole coordinates (such as predrilling holes concentrated in the Y ∈ [-1, 1] m, Z ∈ [-2, -1] m area, peripheral holes distributed along the arc segment), and transmit the data to the jumbo control system through the OPC protocol. The control system includes 2 CP1H-PLC and 6 sensors: 5 rotary encoders (measuring Θ1-Θ7), and 1 displacement sensor (measuring D3, D8, D9). The rotary encoders measure the rotation angles of the 7 joint degrees of freedom of the tunnel jumbo. The displacement sensor measures the extension and retraction of the jumbo's arm, the axial feed amount of the drill rod, and the fine displacement amount of the wrist.

[0212] Independent joint rotation angle measurement, for the base, large arm, small arm three joints, each equipped with an encoder. By selecting an absolute rotary encoder, when the joint rotates, the encoder internal code disc rotates synchronously, the photoelectric element collects the code disc line signal, and the absolute position code is output. The measurement method adopts "shaft-mounted + coupling connection", which ensures that the encoder shaft and the mechanical arm joint shaft rotate coaxially, avoiding radial deviation.

[0213] Integrated joint measurement, such as the compact joint of the wrist and the drill rod, cannot install an encoder alone, and two joints share an encoder. A two-channel integrated encoder is selected, which simultaneously outputs the rotation angles of two joints.

[0214] Joint displacement measurement, by selecting a multi-channel magnetostrictive displacement sensor, i.e. "magnetic ring + single sensor rod", three independent measurement points can simultaneously collect data. When measuring displacement, the sensor inside will generate a magnetic field pulse that propagates along the measurement rod. The eddy current magnetic field generated by the magnetic ring interacts with the pulse magnetic field, generating an induced signal. By measuring the time difference between "pulse emission to induced signal reception", the magnetic ring position (displacement) can be calculated. The formula is:

[0215] D = v · Δt;

[0216] Where v is the propagation speed of the magnetic field pulse, 2.998 x 10 8 m / s; Δt is the time difference.

[0217] 2. Improved dual quaternion modeling implementation

[0218] Substitute the improved D-H parameters of the three-arm: for example, the middle arm joint 1 (Θ1) range [-45°, 45°], joint 3 (D3) range [0, 1800] mm, construct the improved dual quaternion dq1 ~ dq9 of each joint;

[0219] Forward kinematics solution: the end effector pose calculation formula is:

[0220]

[0221] Where dq_total is the total pose of the end effector, which is obtained by multiplying the dual quaternions in the order of joint motion. Compare it with the target borehole pose dq_target to generate the pose error:

[0222]

[0223] PLC reads dq_total and dq_target through OPC protocol, calculates Δdq, then distinguishes the specific adjustment according to the joint type, selects the hydraulic valve type (on-off valve / proportional valve, etc.) according to the size of the adjustment, and outputs the control signal according to pulse width modulation to control the hydraulic valve (for example, 100hz, only needs to keep consistent with the sensor sampling frequency). PLC acquires the adjusted joint variable according to the sensor, repeats the above adjustment process to achieve closed-loop control until the error < threshold (such as Θ1 deviation 0.05°, adjust the hydraulic valve stroke 0.1mm), ensures that the positioning error ≤1.2mm.

[0224] In order to calculate the joint variable data of the mechanical arm in the case of known target borehole pose, the inverse kinematics of the mechanical arm pose needs to be solved. The method is to constrain the redundancy degree of freedom of the 9 degree of freedom mechanical arm, and to construct RBF neural network to solve the inverse solution.

[0225] RBF neural network structure:

[0226] ① Input preprocessing layer: normalize the input parameters (map to the [0, 1] interval);

[0227] ② Input layer: 6-dimensional pose of the end (borehole coordinates in XYZ three directions and drilling angle in XYZ three directions);

[0228] ③ Hidden layer: Gaussian activation function parameter quantization and neuron number optimization. The output of a single hidden layer neuron is:

[0229]

[0230] Where, x is the 6-dimensional normalized vector of the input layer; c j is the center vector of the jth neuron; σ j is the width of the jth neuron, that is, the "action range" of the Gaussian function; j = 1, 2, 3, …, is the number of hidden layer neurons. The number of neurons is optimized by "cross validation method": initially set 5 groups of candidate numbers: 12, 16, 20, 24, 28 neurons; Each group is trained with the same training set, and the root mean square error RMSE is calculated with the validation set, and the number with the smallest RMSE is selected.

[0231] ④ Output layer: training and error: collect 10000 samples (MATLAB rand function generates joint variables, and the pose is obtained by forward kinematics), 90% training / 10% testing, the root mean square error of the test set is:

[0232]

[0233] Where, n is the sample parameter; i is the sample number; x pred,iPredicted end-effector x-axis position for the i-th sample; x true,i True end-effector x-axis position for the i-th sample; a pred,i Predicted end-effector a-angle (x-axis) pose for the i-th sample, a true,i True end-effector a-angle (x-axis) pose for the i-th sample. y pred,i Predicted end-effector y-axis position for the i-th sample; y true,i True end-effector y-axis position for the i-th sample; b pred,i Predicted end-effector b-angle (y-axis) pose for the i-th sample, b true,i True end-effector b-angle (y-axis) pose for the i-th sample; z pred,i Predicted end-effector z-axis position for the i-th sample; z true,i True end-effector z-axis position for the i-th sample; g pred,i Predicted end-effector g-angle (z-axis) pose for the i-th sample, g true,i True end-effector g-angle (z-axis) pose for the i-th sample.

[0234] Workspace verification: Method: Monte Carlo method, 50000 sets of variables were randomly generated within the joint variable range, and the end position was calculated by forward kinematics; Results: When the vehicle body is 6m away from the tunnel section, the single-arm workspace is 11.3m x 6.6m, and the three-arm superposition is 13.2m x 7.2m (covering a tunnel section with a width of 11.7m and a height of 8.7m).

[0235] 3. Six-region division implementation

[0236] ① Single-arm hole sequence TSP problem: Method: Improved ant colony algorithm, introduced dynamic candidate list (limited next node to "the last 5 unvisited holes"), roulette wheel reward and punishment (first 30% probability x 1.2, last 20% x 0.8), cross path optimization (reverse the node order of the cross section); Key formula: The transition probability is:

[0237]

[0238] Where, η ij (t), η is (t) represent the heuristic function, τ ij (t), τ is (t) represent the pheromone concentration on edges ij, is, i, j, s are nodes, allowed k The last 5 nodes closest to the current node, a = 1.2, b = 2.5 are heuristic factors; The smooth heuristic function is:

[0239]

[0240] wherein, a1=0.6, a2=0.4 are weight coefficients, is the motion smoothness, i.e. Δθ ij is the total change of the joint angle of the robot arm from the current hole to the next hole; d ij The node distance is:

[0241]

[0242] wherein, (x i ,y i ), (x j ,y j ) represent the node coordinates;

[0243] Effect: 93 blast hole planning, optimal moving distance 45650mm, convergence algebra 43.8 generations (better than 47743mm, 65.3 generations of ordinary ant colony).

[0244] ② Three-arm cooperative planning:

[0245] Cooperative planning: first plan the hole sequence of the middle arm, set the initial position to the lower part of the working area, set the initial posture of the end to be perpendicular to the working surface or have a certain inclination, and limit the working area. At this time, the local optimization framework is suitable for hole sequence planning, and finally the final regional hole sequence is output, and the coordinates of the end and the starting point of the middle arm are given for the left and right arms to select the initial hole coordinates; then plan the left arm and the right arm. The initial hole of the left arm needs to be closest to the end of the middle arm, and the initial hole of the right arm needs to be closest to the starting point of the middle arm, so as to reduce the initial moving distance and avoid the initial position interference between arms.

[0246] Collision prediction: detection parameters:

[0247] (1) Detection object: the planned robot arm 1 and the planned robot arm 2;

[0248] (2) Safety threshold: 50mm;

[0249] (3) Safe hole distance: 1 current hole ± 2 holes, assuming that the current planned hole of arm 1 is x, then the hole numbers of arm 2 that need to be detected are x-2, x-1, x, x+1, x+2 and the paths of the corresponding holes at the working time.

[0250] Detection opportunity: after the robot arm 1 plans one hole, the robot arm trajectory from the hole to the next hole is generated (based on the improved dual quaternion interpolation calculation), and it is immediately detected whether the trajectory of the corresponding hole of the robot arm 2 collides.

[0251] Detection range: For the mechanical arm 1, detect the spatial coordinates of the connecting rod corresponding to 10 trajectory points during the detection of its hole x to hole x+1; for the mechanical arm 2, respectively detect all the connecting rod spatial positions of the four trajectory lines (10 trajectory points for each section) of hole x-2 to hole x-1, hole x-1 to hole x, hole x to hole x+1, and hole x to hole x+2.

[0252] Detection method: According to the connecting rod axes corresponding to the detected trajectory points of the mechanical arm 1 and the mechanical arm 2, calculate the shortest distance in the space, if the distance is less than the safety threshold, it is determined that the current hole sequence of arm 1 and arm 2 has a collision risk.

[0253] ③Task allocation optimization: secondary allocation optimization: calculate the three-arm hole number deviation ΔP after the initial allocation 12 = |p1-p2|, ΔP 23 = |p2-p3|, ΔP 13 = |p1-p3|, where P1 is the left arm hole number, P2 is the middle arm hole number, P3 is the right arm hole number, ΔP 12 is the absolute hole deviation number of the left arm and the middle arm, ΔP 23 is the absolute hole deviation number of the middle arm and the right arm, ΔP 13 is the absolute hole deviation number of the left arm and the right arm. If ∑ΔP>2, transfer the "hole closest to N" from the arm with the most tasks (M) to the arm with the least tasks (N), and iterate until ∑ΔP≤2; effect: after the secondary allocation of holes, the deviation of the left arm, the middle arm and the right arm is less than 5%.

[0254] ④Anti-collision path design: strategy: local hole sequence rearrangement, if the "hole A→hole B→hole C" path collision is detected, adjust it to "hole A→hole C→hole B", and the rearrangement time is less than 0.5ms.

[0255] Initial partition: divide the cross section into left zone (Y∈[-5.91,0]m) and right zone (Y∈[0,5.88]m) along Y=0, and divide each zone into three along Z axis (Z∈[-5.67,0]m, [0,2.01]m, [2.01,3.01]m), forming 6 regions;

[0256] Dynamic adjustment: count the initial partition hole number, if the right 1 zone is more than 5% above the average, offset the left boundary of the right 1 zone from Y=0 to Y=-0.5m, and the deviation of the adjusted right 1 zone and left 3 zone is less than 5%;

[0257] Task allocation: left first, right second, three-arm cooperation first completes the left region and then completes the right region.

[0258] 4. Hierarchical path optimization implementation

[0259] Global layer: improved genetic algorithm iterated 15 times to generate initial hole sequence for left 1 zone (e.g. traverse from (-5.91, 2.01) m to (-3.94, 3.01) m);

[0260] Local layer: dynamic candidate list ant colony algorithm iterated 25 times to shorten total path distance in left 1 zone from 19.5 mm to 19.3 mm;

[0261] Real-time layer: if drilling time in left 2 zone exceeds preset value by 10% (due to hardening of rock), automatically prioritize traversing the remaining 5 blast holes in left 1 zone adjacent to left 2 zone to avoid intermediate arm waiting.

[0262] 5. Mixed collision detection implementation

[0263] Tunnel cross-section modeling: model: simplified as "circular arc segment (radius R = 5.8 m, center (0, 3.5) m) + 3 straight line segments", coordinate conversion formula (tunnel cross-section -> vehicle body):

[0264]

[0265] where x c , y c , z c are the longitudinal, vertical, and lateral coordinates of the target point converted to the vehicle body coordinate system; X b , Y b , Z b are the coordinates of the origin of the vehicle body coordinate system in the tunnel longitudinal direction (axis direction, Z axis), tunnel vertical direction (Y axis), and tunnel cross-sectional lateral direction (X axis); x s , y s are the lateral and vertical coordinates of the target point in the tunnel cross-section coordinate system.

[0266] Mechanical arm link simplification: scheme: simplify 9 links into 4 cylindrical envelope boxes (link ①: radius 180 mm, link ④: radius 300 mm, ), endpoint coordinates calculated from joint pose (e.g. link ④ endpoint, where p by is the starting longitudinal coordinate of the link and p fy is the ending longitudinal coordinate of the link).

[0267] Collision detection algorithm: inter-arm collision: calculate the shortest distance d_min1 between the two cylindrical axes, collision criterion d_min1 = d_line - (r1 + r2) (d_line is the shortest straight line distance between the "center axes" of the two mechanical arm links in space, r1 and r2 are the link radii), if d_min1 ≤ 50 mm, trigger a warning; arm-wall collision: calculate the shortest distance d3 = R - l max between the mechanical arm and the tunnel circular arc segment, where R is the radius of the circle and lmax is the maximum distance from the center of the circle to the axis of the connecting rod; at the same time, the shortest distance d4 between the mechanical arm and the straight line segment of the tunnel is calculated, and then the smaller of d3 and d4 is subtracted from the radius r3 of the connecting rod of the mechanical arm to obtain the collision criterion value, which is expressed as:

[0268] d min2 = min(d3, d4) - r3.

[0269] If d min2 ≤ 50 mm, the position adjustment of the mechanical arm is triggered.

[0270] Static detection: calculate the shortest distance between the axis of the left arm connecting rod 1 (radius 180 mm) and the middle arm connecting rod 2 (radius 150 mm). For example: d_line = 350 mm, d_min1 = 350 - (180 + 150) = 20 mm ≤ 50 mm, trigger risk warning;

[0271] Dynamic prediction: through improved dual quaternion interpolation, the left arm trajectory of the next 5 holes is predicted, and it is found that the d_min1 of the adjusted path is 60 mm;

[0272] Local rearrangement: adjust the current hole sequence of the left arm "hole A → hole B → hole C" to "hole A → hole C → hole B" to avoid collision, and the rearrangement time is <0.5 ms.

[0273] 6. Execution and feedback

[0274] Through the system, the progress of each arm is displayed in real time (such as the left arm completing 21 / 21 holes, the middle arm completing 22 / 22 holes, and the right arm completing 21 / 21 holes), and after the construction is completed, the statistical indicators are: total moving distance 131.0 mm, total drilling time 566.7 s, and total energy consumption 91798.7 J, which meet the design requirements.

[0275] Example Four

[0276] Based on Example One:

[0277] Kinematics modeling alternative:

[0278] Screw theory method: describe joint motion through screw ξ = [ω; v] (ω is angular velocity vector and v is linear velocity vector), which can replace the improved dual quaternion, but the calculation complexity increases by 15%;

[0279] Lie group Lie algebra method: describe rigid body motion based on SE(3) group, which is theoretically rigorous but requires complex matrix exponential operation and is difficult to implement.

[0280] Regional division alternative:

[0281] Variable grid division method: divide the section into 10×10 mm grids, dynamically merge the grids to form the operation area, but need to occupy an additional 20% of the computing resources;

[0282] K-means clustering-based division method: automatically identify blast hole dense clusters as operation areas, but the cluster centers are easily disturbed by abnormal blast hole coordinates, and the stability needs to be verified.

[0283] Path optimization alternatives:

[0284] Quantum genetic algorithm: introduce quantum bit encoding population, stronger global search ability, but need GPU hardware support, high requirements for trolley control system hardware;

[0285] Deep reinforcement learning method: train DQN network with "total time shortest" as reward function, can adapt to lithology changes, but needs more than 100,000 training data.

[0286] Collision detection alternatives:

[0287] YOLOv8-based deep learning prediction method: real-time recognition of mechanical arm position through camera, 50ms in advance to predict collision, but needs a large number of tunnel scene training samples;

[0288] Hybrid level bounding box method: uses AABB box (axis-aligned bounding box) + OBB box (oriented bounding box) hierarchical modeling, improves calculation efficiency by 10%, but accuracy drops to 1.5 mm.

[0289] The above embodiments are only descriptions of the preferred modes of the present application, and do not limit the scope of the present application. Without departing from the design spirit of the present application, various modifications and improvements to the technical solutions of the present application made by those skilled in the art shall fall within the protection scope determined by the claims of the present application.

Claims

1. A drilling path planning method for a three-arm rock drilling rig, characterized in that, The method includes: The tunnel cross-section parameters and borehole coordinates are collected, and the collected data is transmitted to the trolley control system via the OPC protocol to obtain the rotation and movement joints of the three-arm drilling trolley. Based on the rotational and translational joints of the three-arm rock drilling rig, an improved double quaternion kinematic equation is constructed to obtain the pose of the three-arm rock drilling rig's robotic arm. The working area is dynamically adjusted based on the position of the three-arm rock drilling rig's robotic arm and the distribution of blast holes in the tunnel cross section. Based on the adjusted work area, a three-level optimization architecture of "global-local-real-time" is adopted to plan the drilling path of the three-arm drilling rig. Based on the planned drilling path of the three-arm drilling rig, a hybrid collision detection mechanism is adopted to realize the collision risk detection of the tunnel wall. Based on the rotary and translational joints of a three-arm rock drilling rig, an improved double quaternion kinematic equation is constructed to obtain the pose of the robotic arm of the three-arm rock drilling rig. The improved double quaternion kinematic equations are as follows: dq = q_real + εq_dual; Where dq is a double quaternion, uniformly describing the rotation and translation of the robotic arm; ε is the dual unit, satisfying ε 2 =0; q_real is a quaternion with real part describing joint rotation, defined as: Where θ′ is the corrected rotation angle and n′ is the corrected rotation axis; Introducing a fixed coupling coefficient C = 0.02, the translation error and rotation correction are correlated, defined as follows: θ′=θ+C·Δd; Where θ is the rotation angle and Δd is the translation error, its expression is: Δd=|d 实际 -d 理论 |; Where, d 实际 d represents the actual translation distance. 理论 This is the theoretical translation distance; The rotation axis is corrected based on the pre-defined unit vector x according to the translation path: n′=n+Δn c ; Where, Δn c The difference is n, where n is the unit vector of the rotation axis. Where L is the length of the trolley arm; q_dual is the dual quaternion describing joint translation, defined as: Where d is the pitch, n x The coefficient of the imaginary part i represents the component of the quaternion in the i-th direction, and is usually related to the rotation axis or translation direction. y The coefficient of the imaginary part j represents the component of the quaternion in the j-direction, and is also related to the rotation axis or translation direction, n z The coefficient of the imaginary part k represents the component of the quaternion in the k direction, which is related to the rotation axis or translation direction; Based on the pose of the robotic arm of a three-arm rock drilling rig and the distribution of blast holes in the tunnel cross-section, methods for dynamically adjusting the working area include: The working area is divided into two large regions, left and right, along the central axis Y=0 of the tunnel cross-section. Each large region is then divided into three equal parts along the Z-axis, forming six initial sub-regions, including: left 1 to left 3 regions and right 1 to right 3 regions. Among them, left 1 region Y∈[-5.91,-3.94]m, Z∈[2.01,3.01]m, left 2 region Y∈[-3.94,-1.97]m, Z∈[0,2.01]m, left 3 region Y∈[-1.97,0]m, Z∈[-5.67,0]m, and the right region is symmetrical to the left region. The number of boreholes in each sub-region is counted in real time. If the number of boreholes in a certain region exceeds the average by 5%, the boundaries of adjacent regions are adjusted by a boundary offset algorithm until the deviation of the number of boreholes in each region is less than 5%.

2. The method according to claim 1, characterized in that, Based on the planned drilling path of the three-arm drilling rig, a hybrid collision detection mechanism is used to achieve tunnel wall collision risk detection. The methods include: The robotic arm linkage is simplified into 4 cylindrical envelope boxes, and the tunnel cross-section is simplified into a closed figure of "circular arc segment + 3 straight line segments", where the radius of the circular arc segment R = 5.8m; Calculate the shortest distance between the robotic arms d_min1 = d_line - (r1 + r2), where d_line is the shortest distance between the axes of the two cylinders, and r1 and r2 are the radii of the connecting rod. Calculate the shortest distance between the robotic arm and the tunnel wall d_min2 = min(R - l_max, d_line') - r3, where l_max is the maximum distance from the center of the tunnel to the axis of the connecting rod, d_line' is the distance between the axis of the connecting rod and the straight section of the tunnel, and r3 is the radius of the connecting rod. Based on improved dual quaternion interpolation, the robotic arm trajectory for the next 5 boreholes is generated, and d_min1 and d_min2 on the trajectory are predicted. If d_min1≤50mm or d_min2≤50mm, a local hole sequence rearrangement is triggered, which means adjusting the traversal order of three adjacent holes while keeping the global path unchanged. The expression for the improved biquaternion interpolation is as follows: Where dq(τ) is the biquaternion of the robot arm pose during interpolation, dq current The current quaternion for the robot arm pose at the borehole is dq. next The robot arm pose for the next borehole is represented by a dual quaternion. For dq current The inverse double quaternion; τ is the interpolation coefficient, describing the trajectory progress from the current borehole to the next borehole, and its expression is: Where t is the current movement time, and T is the estimated movement time from the current position to the next borehole, calculated from the robotic arm's movement speed v: Where, d current-next The distance between the two gun holes is the Euclidean distance.

3. A drilling path planning system for a three-arm rock drilling rig, the system being used to implement the method described in any one of claims 1-2, characterized in that, The system includes: a data acquisition module, a data construction module, an adjustment module, an optimization module, and a detection module; The acquisition module is used to acquire tunnel cross-section parameters and borehole coordinates, and transmit the acquired data to the trolley control system via the OPC protocol to obtain the rotation and movement joints of the three-arm drilling trolley. The construction module is used to construct an improved double quaternion kinematic equation based on the rotational and translational joints of the three-arm rock drilling rig, and obtain the pose of the three-arm rock drilling rig's robotic arm. The adjustment module is used to dynamically adjust the working area based on the position and orientation of the three-arm rock drilling rig's robotic arm and the distribution of blast holes in the tunnel cross-section. The optimization module is used to plan the drilling path of the three-arm drilling rig based on the adjusted working area and using a three-level optimization architecture of "global-local-real-time". The detection module is used to detect tunnel wall collision risks based on the planned drilling path of the three-arm drilling rig using a hybrid collision detection mechanism. Based on the rotary and translational joints of the three-arm rock drilling rig, an improved double quaternion kinematic equation is constructed. The process of obtaining the pose of the three-arm rock drilling rig's robotic arm includes: The improved double quaternion kinematic equations are as follows: dq = q_real + εq_dual; Where dq is a double quaternion, uniformly describing the rotation and translation of the robotic arm; ε is the dual unit, satisfying ε 2 =0; q_real is a quaternion with real part describing joint rotation, defined as: Where θ′ is the corrected rotation angle and n′ is the corrected rotation axis; Introducing a fixed coupling coefficient C = 0.02, the translation error and rotation correction are correlated, defined as follows: θ′=θ+C·Δd; Where θ is the rotation angle and Δd is the translation error, its expression is: Δd=|d 实际 -d 理论 |; Where, d 实际 d represents the actual translation distance. 理论 This is the theoretical translation distance; The rotation axis is corrected based on the pre-defined unit vector x according to the translation path: n′=n+Δn c ; Where, Δn c The difference is n, where n is the unit vector of the rotation axis. Where L is the length of the trolley arm; q_dual is the dual quaternion describing joint translation, defined as: Where d is the pitch, n x The coefficient of the imaginary part i represents the component of the quaternion in the i-th direction, and is usually related to the rotation axis or translation direction. y The coefficient of the imaginary part j represents the component of the quaternion in the j-direction, and is also related to the rotation axis or translation direction, n z The coefficient of the imaginary part k represents the component of the quaternion in the k direction, which is related to the rotation axis or translation direction; The process of dynamically adjusting the working area based on the pose of the three-arm rock drilling rig's robotic arm and the distribution of blast holes in the tunnel cross-section includes: The working area is divided into two large regions, left and right, along the central axis Y=0 of the tunnel cross-section. Each large region is then divided into three equal parts along the Z-axis, forming six initial sub-regions, including: left 1 to left 3 regions and right 1 to right 3 regions. Among them, left 1 region Y∈[-5.91,-3.94]m, Z∈[2.01,3.01]m, left 2 region Y∈[-3.94,-1.97]m, Z∈[0,2.01]m, left 3 region Y∈[-1.97,0]m, Z∈[-5.67,0]m, and the right region is symmetrical to the left region. The number of boreholes in each sub-region is counted in real time. If the number of boreholes in a certain region exceeds the average by 5%, the boundaries of adjacent regions are adjusted by a boundary offset algorithm until the deviation of the number of boreholes in each region is less than 5%.

4. The system according to claim 3, characterized in that, The detection module includes: a static detection unit, a dynamic detection unit, and a risk handling unit; The static detection unit is used to simplify the robotic arm links into four cylindrical envelope boxes, and the tunnel cross-section into a closed shape of "circular arc segment + three straight line segments", wherein the radius of the circular arc segment R = 5.8m; calculate the shortest distance between the robotic arms d_min1 = d_line - (r1 + r2), where d_line is the shortest distance between the axes of the two cylinders, r1 and r2 are the radii of the links, and the shortest distance between the robotic arm and the tunnel wall d_min2 = min(R - l_max, d_line') - r3, where l_max is the maximum distance from the center of the tunnel to the axis of the link, d_line' is the distance between the axis of the link and the straight line segment of the tunnel, and r3 is the radius of the link; The dynamic detection unit is used to generate the robotic arm trajectory for the next 5 boreholes based on improved double quaternion interpolation, and to predict d_min1 and d_min2 on the trajectory. The risk handling unit is used to trigger a local hole sequence rearrangement if d_min1≤50mm or d_min2≤50mm, that is, to adjust the traversal order of three adjacent holes while keeping the global path unchanged. The expression for the improved biquaternion interpolation is as follows: Where dq(τ) is the biquaternion of the robot arm pose during interpolation, dq current The current quaternion for the robot arm pose at the borehole is dq. next The robot arm pose for the next borehole is represented by a dual quaternion. For dq current The inverse double quaternion; τ is the interpolation coefficient, describing the trajectory progress from the current borehole to the next borehole, and its expression is: Where t is the current movement time, and T is the estimated movement time from the current position to the next borehole, calculated from the robotic arm's movement speed v: Where, d current-next The distance between the two gun holes is the Euclidean distance.

Images