Anti-collision method and device for multi-arm drill jumbo

Abstract

The invention relates to a multi-arm drill jumbo anti-collision method and device, and the method comprises the steps: building a tunnel three-dimensional coordinate system in an actual tunnel; generating a tunnel collision model in the tunnel three-dimensional coordinate system according to the tunnel design contour; generating an initial cuboid envelope model for each key joint of each mechanical arm of the drill jumbo; measuring the body pose of the drill jumbo in the tunnel three-dimensional coordinate system according to the tunnel collision model; according to the motion data, the DH parameters of the mechanical arm, the vehicle body pose and the initial cuboid envelope model, obtaining an actual cuboid envelope model of each key joint; and according to the actual cuboid envelope models and the tunnel collision model, collision pre-judgment detection is carried out between the actual cuboid envelope models and between the actual cuboid envelope models and the inner surface of the tunnel collision model. According to the method, more comprehensive and efficient collision early warning can be realized, the model is simpler, and the collision detection efficiency can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of tunnel construction equipment, and in particular to a multi-arm rock drilling trolley anti-collision method and device. Background Art

[0002] Drilling and blasting is a construction method that uses explosives to break up rock and create caverns. Most tunnel projects use this method. Drilling and blasting has a harsh working environment, high labor intensity, high risk factor, and a large demand for labor. There is an urgent need to improve the mechanization level of the construction process. The automation and intelligence of construction equipment can greatly reduce the dependence of the construction process on manpower.

[0003] The rock drilling rig is an excavation equipment that uses the drill and blast method. It is widely used in various tunnels and tunnels. It is relatively flexible and often equipped with multiple booms (mechanical arms) to perform rock drilling operations simultaneously. Since each boom has a different operating position, the boom may collide during the displacement process. The collision of the boom will not only interrupt normal construction, but also damage the boom and the rock drill.

[0004] The hydraulic system of the drilling rig has hysteresis, and the communication frequency between the encoder, electric control valve and other components of the drilling rig and the upper control system is generally not high, resulting in the failure of the drilling rig boom to achieve precise control of the motion trajectory. Therefore, the boom anti-collision generally runs at a high frequency in the background, and an alarm is issued when a collision risk is detected, that is, a collision warning is realized. This puts high demands on the monitoring efficiency and resource usage of anti-collision.

[0005] In recent years, with the development of science and technology and the integration of disciplines, some anti-collision technologies for rock drilling rig booms have emerged, such as an anti-collision method and system for booms with application number 201610206372.9, which monitors the distance between booms; outputs a warning prompt when the distance between any two booms is less than the second preset value and greater than the first preset value; when the distance between any two booms is equal to or less than the second preset value, the displacement trajectory of the boom is predicted and the displacement trajectory of the boom is locked according to the prediction result, wherein only the displacement that increases the distance between the booms is effective and the other displacements are locked. Although this method can theoretically achieve anti-collision between booms, each time a collision is detected, the distance between all possible collision parts needs to be calculated. The calculation of this quantitative distance value is very resource-intensive, resulting in low efficiency of collision detection. In addition, the patent does not mention the boom model simplification method, which further increases the detection time and places high performance requirements on hardware such as industrial computers. In addition, when the parameters of the boom of the rock drilling rig actually used change, this method is not convenient for making corresponding adjustments. Summary of the invention

[0006] The object of the present invention is to provide a multi-arm drilling trolley anti-collision method and device, which can achieve more comprehensive and efficient collision warning, and the model is simpler, which is more conducive to improving the efficiency of collision detection.

[0007] The purpose of the present invention can be achieved by adopting the following technical solutions:

[0008] The present invention provides a multi-arm rock drilling trolley anti-collision method, comprising:

[0009] Establish a three-dimensional tunnel coordinate system in the actual tunnel;

[0010] Generate a tunnel collision model in the tunnel three-dimensional coordinate system according to the tunnel design profile;

[0011] An initial cuboid envelope model is generated for each key joint of each mechanical arm of the drilling rig;

[0012] The position of the drilling rig in the three-dimensional coordinate system of the tunnel is measured according to the tunnel collision model;

[0013] The motion data of the robot arm is collected in real time, and the actual rectangular enveloping model of each key joint in the three-dimensional coordinate system of the tunnel is obtained according to the motion data, the DH parameters of the robot arm, the vehicle body posture and the initial rectangular enveloping model;

[0014] According to the actual rectangular enveloping model and the tunnel collision model, collision prediction detection is performed between the actual rectangular enveloping models and between the actual rectangular enveloping models and the inner surface of the tunnel collision model.

[0015] The present invention also provides a multi-arm rock drilling trolley anti-collision device, the multi-arm rock drilling trolley anti-collision device comprising:

[0016] A tunnel model generation module, used to generate a tunnel collision model according to the tunnel design profile;

[0017] A mechanical arm model generation module is used to generate an initial rectangular enveloping model for each key joint of each mechanical arm of the rock drilling rig;

[0018] The vehicle body posture measurement module is used to measure the vehicle body posture of the drilling rig in the actual tunnel according to the tunnel collision model;

[0019] Joint data acquisition module, used to collect the motion data of the robot arm in real time;

[0020] The collision alarm module is used to obtain the actual rectangular envelope model of the actual motion position according to the motion data, the DH parameters of the robot arm, the vehicle posture and the initial rectangular envelope model; and to perform collision prediction detection between each actual rectangular envelope model and between each actual rectangular envelope model and the inner surface of the tunnel collision model according to the actual rectangular envelope model and the tunnel collision model.

[0021] As described above, the anti-collision method and device of the present invention can predict whether there is a collision between the mechanical arms of the rock drilling trolley and between the mechanical arms and the tunnel rock wall, thereby realizing a more comprehensive and efficient collision warning. At the same time, a rectangular enveloping model is generated for the key joints of the mechanical arm. The use of the rectangular enveloping model makes the model simpler and more conducive to improving the efficiency of collision detection. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] The following drawings are only intended to illustrate and explain the present invention, and are not intended to limit the scope of the present invention.

[0023] in:

[0024] Figure 1 : Schematic diagram of the rock drilling rig and tunnel rock wall provided by the present invention.

[0025] Figure 2 : Schematic diagram of the tunnel design profile and rectangular profile of the "horseshoe straight line" tunnel provided by the present invention.

[0026] Figure 3 : A schematic diagram of a double rectangular enveloping model of each key joint on the robot arm provided by the present invention, which only illustrates the initial inner rectangular enveloping model.

[0027] Figure 4 : A schematic diagram of the double rectangular envelope model of the key joint provided by the present invention in the three-dimensional coordinate system of the trolley.

[0028] Figure 5 : A schematic diagram of another angle of the double rectangular envelope model of the key joint provided by the present invention in the three-dimensional coordinate system of the trolley.

[0029] Figure 6 : It is a schematic diagram of the body posture measurement module on the rock drilling rig provided by the present invention when it is working.

[0030] Figure 7 : A schematic diagram of two adjacent joints in the robotic arm provided by the present invention.

[0031] Figure 8 : A schematic diagram of the tunnel collision model provided by the present invention after it is divided into multiple quadrangular prism models.

[0032] Fig. 9 : A schematic diagram of a tetrahedron constructed by selecting four of the 64 new point coordinates provided by the present invention.

[0033] Fig.10 : A schematic flow chart of the anti-collision method for a multi-arm drilling trolley provided by the present invention.

[0034] Description of Figure Numbers:

[0035] 1. Rock drilling rig; 11. Drilling rig; 12. Mechanical arm;

[0036] 21. Tunnel model generation module; 22. Robotic arm model generation module; 23. Vehicle posture measurement module; 24. Joint data acquisition module; 25. Collision alarm module; 26. Movement restriction module;

[0037] 31. Tunnel design outline; 32. Rectangular outline; 33. Four-prism model;

[0038] 41. Initial inner cuboid envelope model; 42. Initial outer cuboid envelope model;

[0039] 5. Tunnel rock wall. DETAILED DESCRIPTION

[0040] In order to have a clearer understanding of the technical features, purposes and effects of the present invention, specific embodiments of the present invention are now described with reference to the accompanying drawings.

[0041] like Figures 1 to 10 As shown, the present application provides a multi-arm drilling rig anti-collision method, comprising the following steps:

[0042] Step 1: Establish a three-dimensional coordinate system of the tunnel in the actual tunnel.

[0043] In this embodiment, the tunnel three-dimensional coordinate system uses any fixed point in the actual tunnel as the tunnel coordinate origin O, and has an X-axis along the tunnel excavation direction, a Y-axis along the tunnel width direction, and a Z-axis along the vertical direction. The tunnel three-dimensional coordinate system can also use the earth three-dimensional coordinate system.

[0044] Step 2: Generate a tunnel collision model: Generate a tunnel collision model in the tunnel three-dimensional coordinate system according to the tunnel design profile 31. This step specifically includes:

[0045] Determine the tunnel design profile31;

[0046] According to the tunnel design profile 31, the maximum ordinate and the minimum ordinate on the Y axis, and the maximum vertical coordinate and the minimum vertical coordinate on the Z axis are obtained;

[0047] Determine a rectangular outline 32 that can surround the tunnel design outline 31, and the coordinates of the four vertices of the rectangular outline 32 are (Y min -T, Z min -T)、(Y min -T, Z max +T)、(Y max +T,Z min -T) and (Y max +T,Z max +T); where Ymax is the maximum ordinate, Y min is the minimum ordinate, Z max is the maximum vertical coordinate, Z min is the minimum vertical coordinate, T is the virtual thickness of the tunnel;

[0048] The rectangular outline 32 and the tunnel design outline 31 are stretched along the X-axis by a preset length to obtain a tunnel collision model in the tunnel three-dimensional coordinate system.

[0049] Generally, T can be set to a value of 1-10 m, for example, in this embodiment, T is 1 m; the preset length is greater than 20 m, and after stretching, a three-dimensional model is formed, that is, the above-mentioned tunnel collision model.

[0050] More specifically, the tunnel design profile 31 is a two-dimensional drawing of the cross section of the designed tunnel inner surface, which is composed of a series of arcs and / or straight line segments, that is, composed of at least one curve, which is a straight line segment or an arc; an arc can be represented by a starting point, an end point and a radius, and a straight line segment can be represented by a starting point and an end point. Therefore, the above-mentioned maximum ordinate, minimum ordinate, maximum vertical coordinate and minimum vertical coordinate can be accurately obtained by calculation, as follows:

[0051] The tunnel design profile 31 includes at least one straight line segment and / or at least one circular arc;

[0052] The maximum ordinate and minimum ordinate on the Y axis, and the maximum vertical coordinate and minimum vertical coordinate on the Z axis are obtained according to the tunnel design profile 31, specifically including:

[0053] Determine a plurality of first points; wherein the plurality of first points include the starting point and the end point of each straight line segment, and / or the starting point, the end point of each circular arc, and the point on each circular arc where the derivative is infinite;

[0054] Determine the maximum and minimum values ​​in the ordinates of each first point and use them as the maximum ordinate and the minimum ordinate, respectively;

[0055] Determine a plurality of second points; wherein the plurality of second points include the starting point and the end point of each straight line segment, and / or the starting point, the end point of each circular arc and the point on each circular arc where the derivative is 0;

[0056] The maximum and minimum values ​​of the vertical coordinates of the second points are determined and used as the maximum vertical coordinate and the minimum vertical coordinate, respectively.

[0057] It can be understood that if the tunnel design profile 31 includes both straight line segments and circular arcs, it is necessary to determine the starting point and end point of each straight line segment, the starting point and end point of each circular arc, and the points on each circular arc where the derivative is infinite, all of which are recorded as first points, and then the maximum and minimum values ​​in the longitudinal coordinates of all these first points are determined; similarly, it is also necessary to determine the starting point and end point of each straight line segment, the starting point and end point of each circular arc, and the points on each circular arc where the derivative is 0, all of which are recorded as second points, and then the maximum and minimum values ​​in the longitudinal coordinates of all these second points are determined.

[0058] Furthermore, the points where the derivative is infinite on each arc are determined specifically including:

[0059] The equation of the circle where the arc is located is obtained according to the coordinates of the starting point, the end point and the radius. The equation of the circle is (ya) 2 +(zb) 2 =R 2 ;

[0060] According to the circle equation, we can get the point on the arc where the derivative is infinite; let the coordinates of the point on the arc where the derivative is infinite be (y P ,z P ), then the coordinate (y P ,z P ) satisfies: y P =aR,z P = b or y P =a+R,z P = b, and min(y1,y2)≤y P ≤max(y1,y2),min(z1,z2)≤z P ≤max(z1,z2). The points on each arc where the derivative is 0 are specifically determined as follows:

[0061] The equation of the circle where the arc is located is obtained according to the coordinates of the starting point, the end point and the radius. The equation of the circle is (ya) 2 +(zb) 2 =R 2 ;

[0062] According to the circle equation, we can get the point on the arc where the derivative is 0; let the coordinates of the point on the arc where the derivative is 0 be (y P ′,z P ′), then the coordinate (y P ′,z P ′) satisfies: y P ′=a,z P ′=bR or y P ′=a,z P ′=b+R, and min(y1,y2)≤y P′≤max(y1,y2),min(z1,z2)≤z P ′≤max(z1,z2).

[0063] Among them, (y1,z1) represents the coordinates of the starting point of the arc, (y2,z2) represents the coordinates of the end point of the arc, R represents the radius of the arc, b represents the vertical coordinate of the center of the circle, and a represents the vertical coordinate of the center of the circle.

[0064] Although the real outer surface of the tunnel is a complex shape composed of mountains and vegetation, the working area of ​​the rock drilling rig 1 is limited to the inside of the tunnel. Therefore, in the entire step 2, according to the designed tunnel design outline 31, a rectangular outer surface is artificially virtualized, as long as the rectangle can cover the inner surface of the tunnel; that is, a sufficiently large rectangular outline 32 is used to surround the tunnel design outline 31, and the tunnel design outline 31 is completely inside the rectangular outline 32; the rectangular outline 32 and the tunnel design outline 31 are stretched into a three-dimensional model. In this way, it is more convenient to simulate and generate a tunnel collision model. The tunnel design outline 31 is the cross-section of the inner surface of the tunnel collision model, and the rectangular outline 32 is the cross-section of the outer surface of the tunnel collision model (that is, the virtual outer surface of the tunnel), which is more conducive to the analysis and calculation of the subsequent steps.

[0065] For example, Figure 2 What is illustrated is the cross section of the tunnel collision model in which the tunnel design profile 31 is a "horseshoe straight line", that is, the cross section of the inner surface of the tunnel is a horseshoe straight line (composed of a top circular arc, two side oblique line segments and a bottom horizontal straight line segment), and the cross section of the outer surface of the tunnel is a rectangular profile 32.

[0066] Step 3: Generate a rectangular parallelepiped envelope of key joints of the robot arm 12: Generate an initial rectangular parallelepiped envelope model for each key joint of each robot arm 12 of the drilling rig 1.

[0067] In a preferred embodiment, the initial rectangular envelope model is a double rectangular envelope model, and includes an initial inner rectangular envelope model 41 and an initial outer rectangular envelope model 42. The initial inner rectangular envelope model 41 is a minimum rectangular block that can completely wrap the corresponding key joints, and the initial outer rectangular block envelope model 42 is a rectangular block that is mounted on the outside of the initial inner rectangular block envelope model 41 and each face has a preset safety distance from the corresponding side faces of the initial inner rectangular block envelope model 41.

[0068] Specifically, an initial rectangular parallelepiped envelope model is generated for each key joint of each mechanical arm 12 of the drilling rig 1, specifically including:

[0069] A three-dimensional coordinate system of the drilling trolley 1 is established on the drilling trolley 1. The three-dimensional coordinate system of the trolley takes the center point of the drilling trolley 1 as the trolley coordinate origin O′, and has an X′ axis along the tunnel excavation direction, a Y′ axis along the tunnel width direction, and a Z′ axis along the vertical direction.

[0070] Return all joints on the robot arm 12 to zero, and determine the coordinates of the eight vertices of the initial inner cuboid envelope model 41 in the three-dimensional coordinate system of the trolley;

[0071] According to the preset safety distance, the coordinates of the eight vertices of the initial outer cuboid envelope model 42 in the three-dimensional coordinate system of the trolley are determined;

[0072] The initial inner cuboid envelope model 41 is established according to the coordinates of the eight vertices of the initial inner cuboid envelope model 41 , and the initial outer cuboid envelope model 42 is established according to the coordinates of the eight vertices of the initial outer cuboid envelope model 42 .

[0073] More specifically, see Figure 4 and Figure 5 , when all joints on the robot arm 12 are returned to zero, the sides of each initial outer cuboid envelope model 42 and each initial inner cuboid envelope model 41 are parallel to the corresponding coordinate axes in the three-dimensional coordinate system of the trolley;

[0074] In this state, the coordinates of the eight vertices of the initial inner cuboid envelope model 41 are (X min ′,Y min ′,Z min ′)、(X min ′,Y min ′,Z max ′)、(X min ′,Y max ′,Z min ′)、(X min ′,Y max ′,Z max ′)、(X max ′,Y min ′,Z min ′)、(X max ′,Y min ′,Z max ′)、(X max ′,Y max ′,Z min ′)、(X max ′,Y max ′,Z max ′);

[0075] Then the coordinates of the eight vertices of the initial outer rectangular parallelepiped envelope model 42 are (X min ′-ds,Y min ′-ds,Zmin ′-ds), (X min ′-ds,Y min ′-ds,Z max ′+ds)、(X min ′-ds,Y max ′+ds,Z min ′-ds), (X min ′-ds,Y max ′+ds,Z max ′+ds)、(X max ′+ds,Y min ′-ds,Z min ′-ds), (X max ′+ds,Y min ′-ds,Z max ′+ds)、(X max ′+ds,Y max ′+ds,Z min ′-ds), (X max ′+ds,Y max ′+ds,Z max ′+ds);

[0076] Among them, X min ′ is the minimum value of the projection of the corresponding key joint on the X′ axis, X max ′ is the maximum value of the projection of the corresponding key joint on the X′ axis, Y min ′ is the minimum value of the projection of the corresponding key joint on the Y′ axis, max ′ is the maximum value of the projection of the corresponding key joint on the Y′ axis, Z min is the minimum value of the projection of the corresponding key joint on the Z′ axis, Z max ′ is the maximum value of the projection of the corresponding key joint on the Z′ axis, and ds is the preset safety distance.

[0077] Generally, the preset safety distance should be greater than the motion error of the corresponding key joint of the corresponding robot arm 12. Generally, the preset safety distance can be set to 5-30 cm. For example, the preset safety distance in this embodiment is 20 cm.

[0078] It can be understood that a robot arm 12 has multiple joints, some of which are prone to collision with other joints or the inner wall of the tunnel during movement, while some are not. The key joints mentioned in this embodiment refer to the joints among the multiple joints of the robot arm 12 that are prone to collision with other joints or the inner wall of the tunnel during movement. In this embodiment, it is only necessary to generate a rectangular envelope model for these key joints and perform collision prediction. For example, in this embodiment, the robot arm 12 includes a boom reference part, a boom telescopic part, and a propulsion beam that are sequentially connected. The boom reference part, the boom telescopic part, and the propulsion beam are the three key joints on the robot arm 12. The initial rectangular envelope models corresponding to these three key joints are as follows: Figure 3 As shown, Figure 3 The three numbers 41 in Figure 3 The orientation shown in the figure represents, from bottom to top, the initial inner cuboid envelope model 41 corresponding to the boom reference part, the boom telescopic part and the propulsion beam of the corresponding robot arm 12.

[0079] For each key joint, the initial inner rectangular envelope model 41 is a small-sized envelope, which is the smallest rectangular block that can completely wrap the key joint; the initial outer rectangular envelope model 42 is a large-sized envelope, which is a rectangular block obtained by extending a preset safety distance in eight directions including left, right, up, down, front and back on the basis of the small-sized envelope.

[0080] For example, Figure 4 and Figure 5 Schematic diagram of the cross-sectional and longitudinal sections of an initial inner cuboid envelope model 41 and an initial outer cuboid envelope model 42 generated for one of the key joints.

[0081] In the entire step three, a double rectangular block is used to envelop the key joints of the robot arm 12, so the robot arm model is simplified. In the subsequent steps, the collision risk can be divided into levels according to whether there is a collision between the large-size envelopes, between the small-size envelopes, between the large-size envelopes and the inner surface of the tunnel collision model, and between the small-size envelopes and the inner surface of the tunnel collision model. This is more conducive to taking corresponding treatment measures in a targeted manner and more effectively preventing the occurrence of collisions.

[0082] Step 4: Vehicle body posture measurement: The vehicle body posture of the drilling rig 1 in the three-dimensional coordinate system of the tunnel is measured according to the tunnel collision model.

[0083] The above-mentioned vehicle body posture is specifically the drilling rig 1 in a real tunnel (i.e. Figure 1The position and angle of the drilling rig 1 in the tunnel rock wall 5) are as follows: the position here refers to the three-dimensional coordinates of the drilling rig 1 in the real tunnel (the coordinates in the three-dimensional coordinate system of the tunnel mentioned above), and the angle here refers to the angle between the central axis of the drilling rig 1 along the tunnel excavation direction and the tunnel axis. When the drilling rig 1 is placed straight, its central axis along the tunnel excavation direction coincides with the tunnel axis; when the drilling rig 1 has a rotation angle in the left-right direction (i.e., the tunnel width direction) and the up-down direction, the central axis of the drilling rig 1 no longer coincides with the tunnel axis, and there is a certain angle.

[0084] Step 4 is to measure the body posture of the drilling rig 1 in the real tunnel, so as to facilitate the analysis and calculation of the subsequent steps. During the movement of the mechanical arm 12, the body posture measurement results need to be combined with the joint angles and DH parameters of the mechanical arm 12 to calculate the real-time position of the rectangular envelope. In this embodiment, measuring the body posture of the drilling rig 1 in the three-dimensional coordinate system of the tunnel specifically includes:

[0085] Collecting point cloud data of the rock wall behind the drilling rig 1;

[0086] The inner surface of the tunnel collision model is converted into theoretical point cloud data;

[0087] The rock wall point cloud data is compared with the theoretical point cloud data to obtain the body position of the drilling rig 1 in the three-dimensional coordinate system of the tunnel.

[0088] More specifically, a laser scanner installed on the drilling rig 1 can be used to laser scan the rock wall point cloud data behind the drilling rig 1; the rock wall point cloud data is compared with the generated tunnel collision model (converted into a point cloud format) using point cloud registration technology to obtain the body posture (position and attitude, 5D here, excluding position information along the front and rear directions of tunnel excavation) of the drilling rig 1.

[0089] Among them, point cloud registration technology is the process of finding the mapping relationship between different point clouds under different perspectives and using a certain algorithm to transform different point clouds of the same target scene into the same coordinate system to form a more complete point cloud. 3D point cloud registration is an important part of point cloud processing technology.

[0090] The ICP (Iterative Closest Point) algorithm was proposed by Besl, McKay and others in 1992. As the most basic and classic method for three-dimensional point cloud registration, the algorithm takes the distance from point to plane or point to point as the optimization target, and estimates the rigid body transformation matrix between the source point cloud and the target point cloud through continuous iteration. In this embodiment, the ICP algorithm can be used to calculate the distance between the corresponding points of the source point cloud (i.e., theoretical point cloud data) and the target point cloud (i.e., rock wall point cloud data), construct a rotation and translation matrix (RT), transform the source point cloud through RT, and calculate the mean square error after the transformation. If the mean square error meets the threshold condition, the algorithm ends and the vehicle posture can be obtained. Otherwise, the iteration is repeated until the error meets the threshold condition or the number of iterations is terminated, and the vehicle posture can be obtained. The ICP algorithm is an existing algorithm, and the process of obtaining the vehicle posture using the ICP algorithm can be obtained according to the analysis of the existing algorithm, which will not be repeated here.

[0091] Step 5: Calculate the actual position of the rectangular envelope: collect the motion data of the robot 12 in real time, and obtain the actual rectangular envelope model of each key joint in the three-dimensional coordinate system of the tunnel based on the motion data, the DH parameters of the robot 12, the vehicle posture and the initial rectangular envelope model.

[0092] Specifically, the motion data of the robot arm 12 includes the rotation angles of all joints of the robot arm 12;

[0093] According to the motion data, the DH parameters of the robot 12, the vehicle posture and the initial rectangular enveloping model, the actual rectangular enveloping model of each key joint in the three-dimensional coordinate system of the tunnel is obtained, which specifically includes:

[0094] According to the DH parameters of the robot arm 12 and the motion data of the robot arm 12 collected in real time, the homogeneous transformation matrix from one joint to another joint of two adjacent joints of the robot arm 12 is obtained, and satisfies the following formula:

[0095]

[0096] Among them, θ i =θ0+θ Δ , represents the homogeneous transformation matrix from the i-1th joint to the i-th joint, θ0 represents the initial joint rotation angle in the DH parameters, θ Δ represents the rotation angle of the i-th joint collected in real time, a i-1 represents the connecting rod length in DH parameters, α i-1 Indicates the connecting rod torsion angle in DH parameters, d i Represents the connecting rod offset in the DH parameters, i≥1 and is a positive integer;

[0097] According to the initial rectangular enveloping model, the axis system rectangular enveloping model corresponding to the i-th joint in the joint coordinate system established on the i-th joint is obtained;

[0098] According to the vehicle body posture, homogeneous transformation matrix and axis system cuboid envelope model, the actual coordinates of each vertex in the actual cuboid envelope model corresponding to the i-th joint are obtained, and the following formula is satisfied:

[0099]

[0100] Where w represents the three-dimensional coordinate system of the tunnel, w P represents the actual coordinates of the corresponding vertices in the actual rectangular envelope model in the tunnel three-dimensional coordinate system, represents the body posture of the drilling rig in the three-dimensional coordinate system of the tunnel (also represents the homogeneous transformation matrix of the drilling rig in the three-dimensional coordinate system of the tunnel), i P represents the coordinates of the corresponding vertex in the axis system rectangular envelope model in the joint coordinate system established on the i-th joint;

[0101] According to the actual coordinates of each vertex in the actual rectangular parallelepiped envelope model corresponding to the ith joint, an actual rectangular parallelepiped envelope model corresponding to the ith joint is established in the three-dimensional coordinate system of the tunnel.

[0102] It can be understood that the above-mentioned initial rectangular envelope model, actual rectangular envelope model, axis rectangular envelope model and the following theoretical rectangular envelope model are models of the formation of rectangular envelopes constructed outside the corresponding key joints in different coordinate systems and different states. The initial rectangular envelope model is a model corresponding to the rectangular envelope when all joints of the robot 12 are zeroed in the three-dimensional coordinate system of the trolley; the actual rectangular envelope model is a model corresponding to the rectangular envelope of the actual position of the robot 12 during the actual movement process in the three-dimensional coordinate system of the tunnel; the axis rectangular envelope model is a model corresponding to the rectangular envelope in the joint coordinate system established on the i-th joint (the coordinates of each vertex of the model are irrelevant to whether each joint of the robot 12 is zeroed and the current motion state); the theoretical rectangular envelope model is a model corresponding to the rectangular envelope of the theoretical motion position of the robot 12 after the theoretical simulation movement in the three-dimensional coordinate system of the tunnel.

[0103] Reference Figure 7 , L i-1 represents the axis where the i-1th joint is located, L i Indicates the axis where the i-th joint is located. The DH parameters of the robot 12 include the following four parts (all fixed values):

[0104] Connecting rod length a i-1 : Axis and Length of the common normal to the axis

[0105] Connecting rod torsion angle α i-1 : Joint axis arrive Corner

[0106] Connecting rod offset d i :from With axis The signed distance (positive direction) from the intersection of direction)

[0107] Initial joint rotation angle θ0: The rotation angle of the i-th joint when all joints on the robot 12 are back to zero. The rotation angle of the axis, that is, from Axis and The rotation angle of the axis, Place measurement

[0108] in, Figure 7 In the three-dimensional coordinate system represents the joint coordinate system established on the i-1th joint, The axis is along the axis direction of the i-1th joint. The axis is along the common normal line of the axes of the i-1th joint and the i-th joint. Axis perpendicular to Axis and Axis plane; three-dimensional coordinate system represents the joint coordinate system established on the i-th joint, The axis is along the axis direction of the i-th joint. The axis is along the common normal line of the axes of the i-th joint and the i+1-th joint. Axis perpendicular to Axis and The plane of the axis.

[0109] The determination of each joint coordinate system and the calculation of the DH parameters of the robot arm 12 are existing technologies and will not be described in detail here.

[0110] Generally, all joints on the robot arm 12 are recorded as the 0th joint to the 1st joint, ..., the ith joint, ... from the root to the end of the robot arm 12, the root of the robot arm 12 refers to the end of the robot arm 12 close to the rock drilling trolley 1, and the tail refers to the end away from the rock drilling trolley 1; the 0th joint generally refers to the joint fixed on the rock drilling trolley 1. According to which joint the key joint on the robot arm 12 corresponds to, the coordinates of each vertex of the actual rectangular envelope model corresponding to the key joint can be calculated.

[0111] For example, assume that the position of the drilling rig 1 relative to the tunnel is If there are three joints from the drilling rig 1 to one of the vertices of the cuboid envelope, the corresponding DH parameters can be listed (four parameters are required between every two joints). The coordinates of this vertex in the joint coordinate system corresponding to the third joint are 3 P, then the three-dimensional coordinates of this vertex in the actual tunnel are

[0112] According to the above method, the coordinates of the eight vertices of the rectangular envelope are obtained in sequence to determine the position of the rectangular envelope in the tunnel.

[0113] That is to say, if one of the key joints corresponds to the third joint on the robot 12, and it is necessary to calculate the coordinates of the corresponding vertices in the actual rectangular envelope corresponding to the third joint, the formula should be used. To calculate, 3 P represents the coordinates of the corresponding vertices in the axis system rectangular envelope model in the joint coordinate system established on the third joint.

[0114] Step 6, collision warning during the movement of the robot arm 12: based on the actual rectangular enveloping model and the tunnel collision model, collision prediction detection is performed between each actual rectangular enveloping model and between each actual rectangular enveloping model and the inner surface of the tunnel collision model.

[0115] In a preferred embodiment, the initial cuboid envelope model is a double cuboid envelope model, and the actual cuboid envelope model includes an actual outer cuboid envelope model and an actual inner cuboid envelope model;

[0116] Obtaining the actual rectangular envelope model of each key joint in the three-dimensional coordinate system of the tunnel includes obtaining the actual outer rectangular envelope model of each key joint in the three-dimensional coordinate system of the tunnel according to motion data, DH parameters of the robot arm, vehicle posture and initial outer rectangular envelope model 42, and obtaining the actual inner rectangular envelope model of each key joint in the three-dimensional coordinate system of the tunnel according to motion data, DH parameters of the robot arm, vehicle posture and initial inner rectangular envelope model 41.

[0117] The above-mentioned collision prediction detection specifically includes:

[0118] Determine whether collision occurs between each actual outer cuboid envelope model and each actual inner cuboid envelope model and the inner surface of the tunnel collision model, between each actual outer cuboid envelope model, and between each actual inner cuboid envelope model;

[0119] If not, there is no risk of collision;

[0120] If there is a collision between at least two of the actual outer cuboid envelope models and / or there is a collision between at least one of the actual outer cuboid envelope models and the inner surface of the tunnel collision model, and there is no collision between the actual inner cuboid envelope models and between any actual inner cuboid envelope model and the inner surface of the tunnel collision model, then there is a first-level collision risk;

[0121] If there is a collision between at least two of the actual inner cuboid envelope models and / or there is a collision between at least one of the actual inner cuboid envelope models and the inner surface of the tunnel collision model, there is a secondary collision risk.

[0122] More specifically, determining whether collision occurs between each actual outer cuboid envelope model and each actual inner cuboid envelope model and the inner surface of the tunnel collision model, between each actual outer cuboid envelope model, and between each actual inner cuboid envelope model specifically includes:

[0123] The tunnel collision model is divided into a plurality of quadrangular prism models 33, and the coordinates of each vertex of each quadrangular prism model 33 in the three-dimensional coordinate system of the tunnel are determined; wherein the height direction of each quadrangular prism model 33 extends along the tunnel excavation direction, and at least part of the wall surface of the quadrangular prism model 33 is the inner surface of the tunnel collision model; in this step, the specific segmentation method when segmenting the tunnel collision model can be determined according to needs, as long as at least part of the wall surface of the quadrangular prism model 33 includes the inner surface of the tunnel collision model and its height direction is along the tunnel excavation direction; in this embodiment, in order to facilitate the segmentation of the quadrangular prism model 33, Figure 8 Taking the tunnel collision model shown in as an example, the two vertices of the straight segment of the tunnel design profile 31 and the vertices or horizontal nearest points of the rectangular profile 32 are stretched into a quadrangular prism model 33, and the arc segment is discretized into a series of points at a certain interval (such as 0.5m), and then the adjacent points plus the horizontal nearest points or the vertical nearest points are stretched into a quadrangular prism model 33;

[0124] Any quadrangular prism model 33 and any actual outer cuboid envelope model, any quadrangular prism model 33 and any actual inner cuboid envelope model, any two actual outer cuboid envelope models, and any two actual inner cuboid envelope models are respectively recorded as the first collision body and the second collision body; it can be understood that in this step, if it is determined whether any quadrangular prism model 33 collides with any actual outer cuboid envelope model, the corresponding quadrangular prism model 33 and the corresponding actual outer cuboid envelope model are respectively recorded as the first collision body and the second collision body; the judgment of other situations is similar to this;

[0125] Subtract the eight vertex coordinates of the second collision body from the eight vertex coordinates of the first collision body to obtain 64 new point coordinates; wherein, in this step, if one of the vertex coordinates of the first collision body is (x 11 ,y 11, z 11 ), one of the vertices of the second collision body has a coordinate (x 22 ,y 22 , z 22 ), then the new point coordinates obtained by subtracting the vertex coordinates of the second collision body from the vertex coordinates of the first collision body are (x 11 -x 22 ,y 11 -y 22 , z 11 -z 22 ); in this way, the first vertex coordinates of the first collision body are subtracted from the first vertex coordinates of the second collision body, and 8 new point coordinates can be obtained; then the second vertex coordinates of the first collision body are subtracted from the first vertex coordinates of the second collision body, and 8 new point coordinates can be obtained; the eight vertex coordinates of the first collision body are calculated in this way, and a total of 64 new point coordinates can be obtained;

[0126] Determine whether the tunnel coordinate origin O is inside the three-dimensional structure formed by the coordinates of the 64 new points;

[0127] If so, there is a collision between the first collision body and the second collision body;

[0128] If not, there is no collision between the first collision body and the second collision body.

[0129] Further, judging whether the tunnel coordinate origin O is inside the three-dimensional structure formed by the coordinates of the 64 new points includes:

[0130] Select four new point coordinates from the 64 new point coordinates and construct a tetrahedron to determine whether the tunnel coordinate origin O is within the tetrahedron;

[0131] If so, the tunnel coordinate origin is inside the three-dimensional structure formed by the coordinates of the 64 new points;

[0132] If not, repeat the above steps (i.e., repeat the steps of constructing a tetrahedron and determining whether the tunnel coordinate origin O is within the tetrahedron), and determine in turn whether the tunnel coordinate origin O is within the remaining constructed tetrahedrons;

[0133] If all are negative, then the tunnel coordinate origin O is not inside the three-dimensional structure formed by the coordinates of the 64 new points.

[0134] Further, judging whether the tunnel coordinate origin O is within the tetrahedron specifically includes:

[0135] Select any one of the four surfaces of the tetrahedron and determine whether the coordinate origin of the tunnel and the coordinates of another new point outside the surface in the tetrahedron are located on the same side of the surface;

[0136] If not, the tunnel coordinate origin O is not inside the tetrahedron;

[0137] If yes, repeat the above steps (i.e., repeat the steps of selecting a surface and determining whether the coordinates of the tunnel coordinate origin O and another new point outside the surface in the tetrahedron are located on the same side of the surface), and sequentially determine whether the coordinates of the tunnel coordinate origin O and another new point outside the remaining surfaces in the tetrahedron are located on the same side of the remaining corresponding surfaces;

[0138] If all are true, then the tunnel coordinate origin O is inside the tetrahedron.

[0139] Further, see Fig. 9 , determine whether the coordinates of the tunnel coordinate origin O and another new point outside the surface in the tetrahedron are on the same side of the surface, specifically including:

[0140] Determine the normal vector of the selected surface, the normal vector Among them, ABC represents the three vertices corresponding to the selected surface;

[0141] According to the normal vector of the selected surface and a vertex on the surface, the plane equation of the plane where the surface is located is obtained. The plane equation is d x ×x+d y ×y+d z ×zG=0;where G=d x ×A x +d y ×A y +d z ×A z , A x A y A z They represent the three coordinates of vertex A respectively; of course, when calculating G, the three coordinates of vertex B or vertex C of the surface can also be used;

[0142] If G×(d x ×D x +d y ×D y +d z ×D z -G)>0, then the coordinate origin O of the tunnel and the coordinates of another new point outside the surface of the tetrahedron (i.e., the fourth point D) are located on both sides of the plane (i.e., not on the same side of the plane);

[0143] If G×(d x ×D x +d y ×D y +d z ×D z -G)≤0, then the coordinate origin O of the tunnel and the coordinates of another new point outside the surface of the tetrahedron (i.e., the fourth point D) are located on the same side of the plane.

[0144] In a feasible embodiment, in order to simplify the judgment calculation, an exclusion group consisting of two envelopes that do not need to be detected will be set up, namely, the small-size envelope and the large-size envelope of the same key joint, and the upper arm reference arm and the upper arm telescopic arm of the same robotic arm 12; for the two key joints that are not in the exclusion group, it is first determined whether their corresponding two large-size envelopes collide. If not, the two key joints do not collide, that is, there is no collision risk; if the two large-size envelopes collide, it is necessary to further detect whether the two small-size envelopes collide. If they collide, it is a secondary collision risk; if the two large-size envelopes collide and the two small-size envelopes do not collide, it is a primary collision risk.

[0145] Step 7, post-collision motion restriction: If there is no collision risk, no motion restriction is performed; if there is a first-level collision risk, the movement speed of the corresponding robot arm 12 is reduced to limit the maximum movement speed of the robot arm 12; if there is a second-level collision risk, the corresponding robot arm 12 is shut down.

[0146] Furthermore, when there is a secondary collision risk, the corresponding robot arm 12 action is stopped immediately. When a re-action instruction is received, the collision state of the robot arm 12 needs to be predicted based on the possible joint data of the robot arm 12 after the movement. Only instructions that can enter the primary collision risk or no collision risk are allowed to be executed.

[0147] Generally, after the robotic arm 12 stops, the operator can manually operate the robotic arm 12. The theoretical movement position of the joint after a communication cycle is obtained by multiplying the joint movement speed corresponding to the swing amplitude of the operating handle on the remote control by the communication time interval. This position is used for post-action collision detection (prejudgment). If there is a second-level collision risk, the joint will not move. If there is a first-level collision risk, the speed will be automatically reduced. If there is no collision risk, it will move normally.

[0148] Specifically, when there is a secondary collision risk and the corresponding robot arm 12 is shut down, the following steps are also included:

[0149] Operate the robot arm 12 after it stops;

[0150] According to the movement speed of the joint corresponding to the swing amplitude of the robot arm 12, the theoretical movement position of the joint after one communication cycle is obtained; according to the theoretical movement position of the joint, the DH parameters of the robot arm 12, the vehicle body posture and the initial rectangular envelope model, the theoretical rectangular envelope model of the theoretical movement position in the three-dimensional coordinate system of the tunnel is obtained;

[0151] According to the theoretical cuboid envelope model and the tunnel collision model, collision prediction detection is performed between each theoretical cuboid envelope model and between each theoretical cuboid envelope model and the inner surface of the tunnel collision model;

[0152] If there is no risk of collision, the robot arm 12 moves normally;

[0153] If there is a level 1 collision risk, the movement speed of the robot arm 12 is reduced;

[0154] If there is a secondary collision risk, the joint remains immobile.

[0155] The collision prediction detection mentioned here is similar to the collision prediction detection step performed during the actual movement of the aforementioned robot arm 12. Specifically, the initial rectangular parallelepiped envelope model is a double rectangular parallelepiped envelope model, and the theoretical rectangular parallelepiped envelope model includes a theoretical outer rectangular parallelepiped envelope model and a theoretical inner rectangular parallelepiped envelope model;

[0156] Obtaining the theoretical cuboid envelope model of the theoretical motion position in the three-dimensional coordinate system of the tunnel includes obtaining the theoretical outer cuboid envelope model according to the theoretical motion position of the joint, the DH parameters of the robot, the body posture and the initial outer cuboid envelope model 42, and obtaining the theoretical inner cuboid envelope model according to the theoretical motion position of the joint, the DH parameters of the robot, the body posture and the initial inner cuboid envelope model 41.

[0157] The collision prediction detection here specifically includes:

[0158] Determine whether collision occurs between each theoretical outer cuboid envelope model and each theoretical inner cuboid envelope model and the inner surface of the tunnel collision model, between each theoretical outer cuboid envelope model, and between each theoretical inner cuboid envelope model. It is also divided into three situations: no collision risk, first-level collision risk, and second-level collision risk.

[0159] Furthermore, the present application also provides a multi-arm drilling trolley anti-collision device, the multi-arm drilling trolley anti-collision device comprising:

[0160] A coordinate system establishment module is used to establish a three-dimensional coordinate system of the tunnel in the actual tunnel;

[0161] A tunnel model generation module 21, for generating a tunnel collision model according to a tunnel design profile 31;

[0162] A mechanical arm model generating module 22, for generating an initial rectangular parallelepiped envelope model for each key joint of each mechanical arm 12 of the drilling rig 1;

[0163] A vehicle body posture measurement module 23, used to measure the vehicle body posture of the drilling rig 1 in the actual tunnel (that is, the vehicle body posture in the three-dimensional coordinate system of the tunnel) according to the tunnel collision model;

[0164] The joint data acquisition module 24 is used to acquire the motion data of the robot arm 12 in real time (specifically, to acquire the rotation angles and displacements of all joints of the robot arm 12);

[0165] The collision alarm module 25 is used to obtain the actual rectangular enveloping model of the actual motion position (that is, the actual rectangular enveloping model of each key joint in the three-dimensional coordinate system of the tunnel) according to the motion data, the DH parameters of the manipulator 12, the vehicle body posture and the initial rectangular enveloping model; and perform collision prediction detection between each actual rectangular enveloping model and between each actual rectangular enveloping model and the inner surface of the tunnel collision model according to the actual rectangular enveloping model and the tunnel collision model;

[0166] The motion limiting module 26 is used to reduce the motion speed of the corresponding robot arm 12 or shut down the corresponding robot arm 12 when there is a risk of collision.

[0167] Specifically, the above-mentioned tunnel model generation module 21, mechanical arm model generation module 22, vehicle body posture measurement module 23, collision alarm module 25 and motion restriction module 26 are all arranged on the vehicle body 11 of the drilling trolley 1. The tunnel model generation module 21 can automatically generate a three-dimensional collision body model of the tunnel rock wall 5 according to the tunnel cross-sectional form, and tunnels with the same cross-section can share the model. The vehicle body posture measurement module 23 includes a laser scanner, which is installed at the rear of the drilling trolley 1. The parking posture (left and right position, up and down position, left and right swing angle, up and down pitch angle, and flip angle) of the trolley in the tunnel is obtained from the scanned point cloud data of the tunnel rock wall 5.

[0168] The joint data acquisition module 24 includes a plurality of angle sensors and a plurality of displacement sensors, which are respectively arranged on the corresponding joints of the robot arm 12, so as to collect the motion state of each joint of the robot arm 12 (the angle of the rotating joint and the displacement of the moving joint) in real time. The specific number of sensors is determined according to the actual situation, for example, according to Figure 1 As shown in the figure, the boom (mechanical arm 12) of the drilling rig 1 has five rotating joints and one moving joint, an angle sensor is installed at each rotating joint, and a displacement sensor is installed at each moving joint. Therefore, the mechanical arm 12 is equipped with five angle sensors (such as angle encoders) and one displacement sensor (such as a wire sensor).

[0169] The collision alarm module 25 can judge whether there is a collision between parts at the current moment and the next moment according to the joint motion data collected in real time; the motion restriction module 26 can restrict the motion of the robot arm 12 according to the collision situation. During the motion of multiple robot arms 12, the joint data acquisition module 24 collects joint angle and displacement data in real time, and the collision alarm module 25 judges whether there is a collision between the double rectangular parallelepiped envelopes every very short time (100ms) according to these data. The collision state can be divided into three levels according to the collision level: no collision with large envelope (no collision risk), collision with large envelope and no collision with small envelope (level 1 collision risk), and collision with small envelope (level 2 collision risk).

[0170] Furthermore, the collision warning module includes:

[0171] An actual model determination unit, used to obtain an actual rectangular parallelepiped envelope model of an actual motion position according to motion data, DH parameters of the robot 12, vehicle body posture and an initial rectangular parallelepiped envelope model;

[0172] A tunnel segmentation unit, used to segment the tunnel collision model into a plurality of quadrangular prism models 33, and determine the coordinates of each vertex of each quadrangular prism model 33 in the three-dimensional coordinate system of the tunnel; wherein the height direction of each quadrangular prism model 33 extends along the tunnel excavation direction, and at least part of the wall surface of the quadrangular prism model 33 is the inner surface of the tunnel collision model;

[0173] A collision body determination unit, used to record any quadrangular prism model 33 and any actual outer cuboid envelope model, any quadrangular prism model 33 and any actual inner cuboid envelope model, any two actual outer cuboid envelope models and any two inner cuboid envelope models as a first collision body and a second collision body respectively;

[0174] A new point determination unit, used for subtracting the eight vertex coordinates of the second collision body from the eight vertex coordinates of the first collision body to obtain 64 new point coordinates;

[0175] A collision prediction unit is used to determine whether the tunnel coordinate origin O of the tunnel three-dimensional coordinate system is inside the three-dimensional structure formed by the coordinates of the 64 new points;

[0176] The alarm unit is used to sound an alarm when there is a risk of collision; generally, the alarm unit may include a first alarm unit, which is used to sound a first alarm when there is a primary collision risk; and a second alarm unit, which is used to sound a second alarm when there is a secondary collision risk.

[0177] The whole device implements the above-mentioned multi-arm drilling trolley anti-collision method, and the specific steps have been described above and will not be repeated here. The device has the same advantages as the above-mentioned collision method.

[0178] In summary, the anti-collision method and device in this embodiment have the following advantages:

[0179] (1) It is possible to predict whether there is a collision between the mechanical arms 12 of the rock drilling rig 1 and between the mechanical arms 12 and the tunnel rock wall 5, thereby achieving a more comprehensive and efficient collision warning. At the same time, a rectangular enveloping model is generated for the key joints of the mechanical arms 12. The use of the rectangular enveloping model makes the model simpler and more conducive to improving the efficiency of collision detection.

[0180] (2) According to the designed tunnel design outline 31, a rectangular outer surface is virtualized, and the tunnel collision model is generated by stretching it into a three-dimensional model, which makes it more convenient to generate the model; the key joints of the mechanical arm 12 are encapsulated by a double rectangular enveloping model, and the model is simpler; the calculation resources are relatively low, which is more conducive to improving the efficiency of collision detection. At the same time, the double rectangular enveloping can be used to divide the collision risks into levels, which is more conducive to taking corresponding treatment measures in a targeted manner, and can achieve efficient collision warning of the boom of the rock drilling rig 1.

[0181] (3) The multi-arm drilling rig 1 can realize collision warning of the mechanical arm 12 itself, between multiple mechanical arms 12, and between the mechanical arm 12 and the tunnel rock wall 5. The multi-arm drilling rig 1 mechanical arm 12 anti-collision method and device can be used when the mechanical arm 12 is manually controlled and the mechanical arm 12 is automatically moved, and has the advantages of high anti-collision detection efficiency, gradable collision risk, and convenient parameter adjustment.

[0182] (4) The problems of low anti-collision detection efficiency, complex model, and inconvenient parameter adjustment of the existing rock drilling rig 1 are solved, thereby providing protection for the boom and rock drill safety during the construction of the rock drilling rig 1.

[0183] The above are only exemplary embodiments of the present invention and are not intended to limit the scope of the present invention. Any equivalent changes and modifications made by any person skilled in the art without departing from the concept and principle of the present invention shall fall within the scope of protection of the present invention.

Claims

1. A multi-arm drilling trolley anti-collision method, characterized in that: include: Establish a three-dimensional tunnel coordinate system in the actual tunnel; Generating a tunnel collision model in the three-dimensional coordinate system of the tunnel according to the tunnel design profile (31); An initial rectangular parallelepiped envelope model is generated for each key joint of each mechanical arm (12) of the drilling rig (1); Measuring the body posture of the drilling rig (1) in the three-dimensional coordinate system of the tunnel according to the tunnel collision model; Collecting the motion data of the mechanical arm (12) in real time, and obtaining the actual rectangular parallelepiped envelope model of each of the key joints in the three-dimensional coordinate system of the tunnel according to the motion data, the DH parameters of the mechanical arm (12), the vehicle body posture and the initial rectangular parallelepiped envelope model; According to the actual rectangular parallelepiped envelope model and the tunnel collision model, collision prediction detection is performed between the actual rectangular parallelepiped envelope models and between the actual rectangular parallelepiped envelope models and the inner surface of the tunnel collision model.

2. The anti-collision method for a multi-arm drilling rig according to claim 1, characterized in that: The initial rectangular parallelepiped envelope model is a double rectangular parallelepiped envelope model, and includes an initial inner rectangular parallelepiped envelope model (41) and an initial outer rectangular parallelepiped envelope model (42), wherein the initial inner rectangular parallelepiped envelope model (41) is a minimum rectangular parallelepiped that can completely wrap the corresponding key joint, and the initial outer rectangular parallelepiped envelope model (42) is a rectangular parallelepiped that is sleeved on the outer side of the initial inner rectangular parallelepiped envelope model (41) and each face of which has a preset safety distance from the corresponding side face of the initial inner rectangular parallelepiped envelope model (41); the actual rectangular parallelepiped envelope model includes an actual outer rectangular parallelepiped envelope model and an actual inner rectangular parallelepiped envelope model; Obtaining the actual rectangular parallelepiped envelope model of each of the key joints in the three-dimensional coordinate system of the tunnel comprises obtaining the actual outer rectangular parallelepiped envelope model of each of the key joints in the three-dimensional coordinate system of the tunnel according to the motion data, the DH parameter of the mechanical arm (12), the vehicle body posture and the initial outer rectangular parallelepiped envelope model (42), and obtaining the actual inner rectangular parallelepiped envelope model of each of the key joints in the three-dimensional coordinate system of the tunnel according to the motion data, the DH parameter of the mechanical arm (12), the vehicle body posture and the initial inner rectangular parallelepiped envelope model (41); The collision prediction detection specifically includes: Determine whether a collision occurs between each of the actual outer cuboid envelope models and each of the actual inner cuboid envelope models and the inner surface of the tunnel collision model, between each of the actual outer cuboid envelope models, and between each of the actual inner cuboid envelope models; If not, there is no risk of collision; If there is a collision between at least two of the actual outer cuboid envelope models and / or there is a collision between at least one of the actual outer cuboid envelope models and the inner surface of the tunnel collision model, and there is no collision between the actual inner cuboid envelope models and between any of the actual inner cuboid envelope models and the inner surface of the tunnel collision model, then there is a first-level collision risk; If there is a collision between at least two of the actual inner cuboid envelope models and / or there is a collision between at least one of the actual inner cuboid envelope models and the inner surface of the tunnel collision model, there is a secondary collision risk.

3. The anti-collision method for a multi-arm drilling rig according to claim 2, characterized in that: The three-dimensional coordinate system of the tunnel takes any fixed point in the actual tunnel as the origin of the tunnel coordinates, and has an X-axis along the tunnel excavation direction, a Y-axis along the tunnel width direction, and a Z-axis along the vertical direction; Generating a tunnel collision model in the three-dimensional coordinate system of the tunnel according to the tunnel design profile (31) specifically includes: Determining the tunnel design profile (31); According to the tunnel design profile (31), the maximum ordinate and the minimum ordinate on the Y axis, and the maximum vertical coordinate and the minimum vertical coordinate on the Z axis are obtained; Determine a rectangular outline (32) that can surround the tunnel design outline (31), the coordinates of the four vertices of the rectangular outline (32) are respectively (Y min -T, Z min -T)、(Y min -T, Z max +T)、(Y max +T,Z min -T) and (Y max +T,Z max +T); where Y max is the maximum ordinate, Y min is the minimum ordinate, Z max is the maximum vertical coordinate, Z min is the minimum vertical coordinate, T is the virtual thickness of the tunnel; The rectangular outline (32) and the tunnel design outline (31) are stretched along the X-axis by a preset length to obtain the tunnel collision model in the three-dimensional coordinate system of the tunnel.

4. The anti-collision method for a multi-arm drilling trolley according to claim 3, characterized in that: The tunnel design profile (31) comprises at least one straight line segment and / or at least one circular arc; According to the tunnel design profile (31), the maximum ordinate and the minimum ordinate on the Y axis, and the maximum vertical coordinate and the minimum vertical coordinate on the Z axis are obtained, which specifically include: Determine a plurality of first points; wherein the plurality of first points include the starting point and the end point of each of the straight line segments, and / or the starting point, the end point of each of the circular arcs and the point on each of the circular arcs where the derivative is infinite; Determine the maximum value and the minimum value in the ordinate of each of the first points, and use them as the maximum ordinate and the minimum ordinate, respectively; Determine a plurality of second points; wherein the plurality of second points include the starting point and the end point of each of the straight line segments, and / or the starting point, the end point of each of the circular arcs and the point on each of the circular arcs where the derivative is 0; The maximum value and the minimum value of the vertical coordinates of each of the second points are determined and used as the maximum vertical coordinate and the minimum vertical coordinate, respectively.

5. The anti-collision method for a multi-arm drilling rig according to claim 4, characterized in that: Determining the points on each of the arcs where the derivative is infinite specifically includes: The equation of the circle where the arc is located is obtained according to the starting point coordinates, the end point coordinates and the radius of the arc. The equation of the circle is (ya) 2 +(zb) 2 =R 2 ; According to the circle equation, the point on the arc where the derivative is infinite is obtained; wherein, the coordinates of the point on the arc where the derivative is infinite are set to (y P , z P ), then the coordinate (y P , z P ) satisfies: y P =aR,z P = b or y P =a+R,z P = b, and min(y1, y2) ≤ y P ≤max(y1,y2),min(z1,z2)≤z P ≤max(z1,z2); Among them, (y1, z1) represents the starting coordinates of the arc, (y2, z2) represents the ending coordinates of the arc, R represents the radius of the arc, b represents the vertical coordinate of the center of the circle, and a represents the vertical coordinate of the center of the circle.

6. The anti-collision method for a multi-arm drilling rig according to claim 4, characterized in that: Determining the points on each of the arcs where the derivative is 0 specifically includes: The equation of the circle where the arc is located is obtained according to the starting point coordinates, the end point coordinates and the radius of the arc. The equation of the circle is (ya) 2 +(zb) 2 =R 2 ; According to the circle equation, the point on the arc where the derivative is 0 is obtained; wherein, the coordinates of the point on the arc where the derivative is 0 are set to (y P ′,z P ′), then the coordinate (y P ′,z P ′) satisfies: y P ′=a,z P ′=bR or y P ′=a,z P ′=b+R,and min(y1,y2)≤y P ′≤max(y1,y2), min(z1,z2)≤z P ′≤max(z1,z2); Among them, (y1, z1) represents the starting coordinates of the arc, (y2, z2) represents the ending coordinates of the arc, R represents the radius of the arc, b represents the vertical coordinate of the center of the circle, and a represents the vertical coordinate of the center of the circle.

7. The anti-collision method for a multi-arm drilling rig according to claim 2, characterized in that: The initial rectangular parallelepiped envelope model is generated for each key joint of each mechanical arm (12) of the drilling rig (1), specifically comprising: A trolley three-dimensional coordinate system is established on the drilling trolley (1), wherein the trolley three-dimensional coordinate system takes the center point of the drilling trolley (1) as the trolley coordinate origin and has an X′ axis along the tunnel excavation direction, a Y′ axis along the tunnel width direction, and a Z′ axis along the vertical direction; Returning all joints on the mechanical arm (12) to zero, and determining the coordinates of eight vertices of the initial inner cuboid envelope model (41) in the three-dimensional coordinate system of the trolley; According to a preset safety distance, determining the coordinates of eight vertices of the initial outer rectangular parallelepiped envelope model (42) in the three-dimensional coordinate system of the trolley; The initial inner cuboid envelope model (41) is established according to the coordinates of the eight vertices of the initial inner cuboid envelope model (41), and the initial outer cuboid envelope model (42) is established according to the coordinates of the eight vertices of the initial outer cuboid envelope model (42).

8. The anti-collision method for a multi-arm drilling rig according to claim 1, characterized in that: The measuring of the body posture of the drilling rig (1) in the three-dimensional coordinate system of the tunnel specifically comprises: Collecting point cloud data of the rock wall behind the drilling rig (1); The inner surface of the tunnel collision model is converted into theoretical point cloud data; The rock wall point cloud data is compared with the theoretical point cloud data to obtain the body position of the drilling rig (1) in the three-dimensional coordinate system of the tunnel.

9. The anti-collision method for a multi-arm drilling rig according to claim 1, characterized in that: The motion data of the robotic arm (12) includes the rotation angles of all joints of the robotic arm (12); According to the motion data, the DH parameters of the mechanical arm (12), the vehicle body posture and the initial rectangular parallelepiped envelope model, an actual rectangular parallelepiped envelope model of each key joint in the three-dimensional coordinate system of the tunnel is obtained, specifically comprising: A homogeneous transformation matrix from one joint to another joint of two adjacent joints of the robotic arm (12) is obtained according to the DH parameters of the robotic arm (12) and the motion data of the robotic arm (12) collected in real time, and satisfies the following formula: Among them, θ i =θ0+θ Δ , represents the homogeneous transformation matrix from the i-1th joint to the i-th joint, θ0 represents the initial joint rotation angle in the DH parameters, θ Δ represents the rotation angle of the i-th joint collected in real time, a i-1 represents the connecting rod length in DH parameters, α i-1 Indicates the connecting rod torsion angle in DH parameters, d i Represents the connecting rod offset in the DH parameters, i≥1 and is a positive integer; Obtaining, according to the initial rectangular enveloping model, an axis system rectangular enveloping model corresponding to the i-th joint in the joint coordinate system established on the i-th joint; The actual coordinates of each vertex in the actual rectangular parallelepiped envelope model corresponding to the i-th joint are obtained according to the vehicle body posture, the homogeneous transformation matrix and the axis system rectangular parallelepiped envelope model, and the following formula is satisfied: Wherein, w represents the three-dimensional coordinate system of the tunnel, w P represents the actual coordinates of the corresponding vertices in the actual rectangular parallelepiped envelope model in the three-dimensional coordinate system of the tunnel, represents the body position of the drilling rig (1) in the three-dimensional coordinate system of the tunnel, i P represents the coordinates of the corresponding vertices in the axis system rectangular parallelepiped envelope model in the joint coordinate system established on the i-th joint; According to the actual coordinates of each vertex in the actual rectangular parallelepiped envelope model corresponding to the ith joint, the actual rectangular parallelepiped envelope model corresponding to the ith joint is established in the three-dimensional coordinate system of the tunnel.

10. The anti-collision method for a multi-arm drilling rig according to claim 2, characterized in that: Determining whether collision occurs between each of the actual outer cuboid envelope models and each of the actual inner cuboid envelope models and the inner surface of the tunnel collision model, between each of the actual outer cuboid envelope models, and between each of the actual inner cuboid envelope models, specifically includes: The tunnel collision model is divided into a plurality of quadrangular prism models (33), and the coordinates of each vertex of each quadrangular prism model (33) in the three-dimensional coordinate system of the tunnel are determined; wherein the height direction of each quadrangular prism model (33) extends along the tunnel excavation direction, and at least part of the wall surface of the quadrangular prism model (33) is the inner surface of the tunnel collision model; Any one of the four-prism models (33) and any one of the actual outer rectangular parallelepiped envelope models, any one of the four-prism models (33) and any one of the actual inner rectangular parallelepiped envelope models, any two of the actual outer rectangular parallelepiped envelope models, and any two of the actual inner rectangular parallelepiped envelope models are respectively recorded as a first collision body and a second collision body; Subtract the eight vertex coordinates of the second collision body from the eight vertex coordinates of the first collision body to obtain 64 new point coordinates; Determine whether the tunnel coordinate origin of the tunnel three-dimensional coordinate system is inside the three-dimensional structure formed by the 64 new point coordinates; If so, there is a collision between the first collision body and the second collision body; If not, there is no collision between the first collision body and the second collision body.

11. The anti-collision method for a multi-arm drilling rig according to claim 10, characterized in that: Determining whether the tunnel coordinate origin is inside the three-dimensional structure formed by the 64 new point coordinates specifically includes: Select four of the 64 new point coordinates and construct a tetrahedron to determine whether the tunnel coordinate origin is within the tetrahedron; If yes, the tunnel coordinate origin is inside the three-dimensional structure formed by the 64 new point coordinates; If not, repeat the above steps to determine in turn whether the tunnel coordinate origin is within the remaining constructed tetrahedrons; If all are negative, then the tunnel coordinate origin is not inside the three-dimensional structure formed by the 64 new point coordinates.

12. The anti-collision method for a multi-arm drilling rig according to claim 11, characterized in that: Determining whether the tunnel coordinate origin is within the tetrahedron specifically includes: Select any one of the four surfaces of the tetrahedron, and determine whether the coordinate origin of the tunnel and the coordinates of another new point outside the surface in the tetrahedron are located on the same side of the surface; If not, the tunnel coordinate origin is not within the tetrahedron; If yes, repeat the above steps to determine in turn whether the coordinate origin of the tunnel and the coordinates of another new point outside the remaining surfaces in the tetrahedron are located on the same side of the remaining corresponding surfaces; If all are true, the tunnel coordinate origin is within the tetrahedron.

13. The anti-collision method for a multi-arm drilling rig according to claim 2, characterized in that: The anti-collision method of the multi-arm drilling rig (1) further comprises: If there is a primary collision risk, reducing the movement speed of the corresponding mechanical arm (12); If there is a secondary collision risk, the corresponding robotic arm (12) is shut down.

14. The anti-collision method for a multi-arm drilling rig according to claim 13, characterized in that: After the risk of secondary collision exists and the corresponding mechanical arm (12) is shut down, the method further includes: Operating the mechanical arm (12) after it stops; According to the movement speed of the joint corresponding to the swing amplitude of the mechanical arm (12), the theoretical movement position of the joint after one communication cycle is obtained; according to the theoretical movement position of the joint, the DH parameter of the mechanical arm (12), the vehicle body posture and the initial rectangular parallelepiped envelope model, a theoretical rectangular parallelepiped envelope model of the theoretical movement position in the three-dimensional coordinate system of the tunnel is obtained; According to the theoretical cuboid envelope model and the tunnel collision model, collision prediction detection is performed between the theoretical cuboid envelope models and between the theoretical cuboid envelope models and the inner surface of the tunnel collision model; If there is no risk of collision, the robotic arm (12) moves normally; If there is a primary collision risk, reducing the movement speed of the mechanical arm (12); If there is a secondary collision risk, the joint remains immobile.

15. A multi-arm drilling trolley anti-collision device, characterized in that: The anti-collision device of the multi-arm drilling trolley (1) comprises: A tunnel model generation module (21) is used to generate a tunnel collision model according to a tunnel design profile (31); A mechanical arm model generation module (22) is used to generate an initial rectangular parallelepiped envelope model for each key joint of each mechanical arm (12) of the drilling rig (1); A vehicle body posture measurement module (23), used for measuring the vehicle body posture of the drilling rig (1) in an actual tunnel according to the tunnel collision model; A joint data acquisition module (24) for acquiring motion data of the mechanical arm (12) in real time; A collision alarm module (25) is used to obtain an actual rectangular parallelepiped envelope model of an actual motion position based on the motion data, the DH parameters of the mechanical arm (12), the vehicle body posture and the initial rectangular parallelepiped envelope model; and to perform collision prediction detection between each of the actual rectangular parallelepiped envelope models and between each of the actual rectangular parallelepiped envelope models and the inner surface of the tunnel collision model based on the actual rectangular parallelepiped envelope model and the tunnel collision model.

16. The anti-collision device for a multi-arm drilling trolley according to claim 15, characterized in that: The initial rectangular enveloping model is a double rectangular enveloping model, and comprises an initial inner rectangular enveloping model (41) and an initial outer rectangular enveloping model (42), wherein the initial inner rectangular enveloping model (41) is a minimum rectangular enveloping model that can completely envelop the corresponding key joint, and the initial outer rectangular enveloping model (42) is a rectangular enveloping model that is sleeved on the outer side of the initial inner rectangular enveloping model (41) and each face of which has a preset safety distance from the corresponding side face of the initial inner rectangular enveloping model (41).

17. The anti-collision device for a multi-arm drilling trolley according to claim 16, characterized in that: The collision warning module (25) comprises: An actual model determination unit, used for obtaining an actual rectangular parallelepiped envelope model of an actual motion position according to the motion data, the DH parameter of the mechanical arm (12), the vehicle body posture and the initial rectangular parallelepiped envelope model; A tunnel segmentation unit, used for segmenting the tunnel collision model into a plurality of quadrangular prism models (33), and determining the coordinates of each vertex of each quadrangular prism model (33) in the three-dimensional coordinate system of the tunnel; wherein the height direction of each quadrangular prism model (33) extends along the tunnel excavation direction, and at least part of the wall surface of the quadrangular prism model (33) is the inner surface of the tunnel collision model; A collision body determination unit, used for recording any one of the four-prism models (33) and any one of the actual outer cuboid envelope models, any one of the four-prism models (33) and any one of the actual inner cuboid envelope models, any two of the actual outer cuboid envelope models, and any two of the inner cuboid envelope models (41) as a first collision body and a second collision body respectively; A new point determination unit, used for subtracting the eight vertex coordinates of the second collision body from the eight vertex coordinates of the first collision body to obtain 64 new point coordinates; The collision prediction unit is used to determine whether the tunnel coordinate origin of the tunnel three-dimensional coordinate system is inside the three-dimensional structure formed by the 64 new point coordinates.

18. The anti-collision device for a multi-arm drilling rig according to claim 16, characterized in that: The anti-collision device of the multi-arm drilling trolley (1) further comprises: A movement limiting module (26) is used to reduce the movement speed of the corresponding mechanical arm (12) or shut down the corresponding mechanical arm (12) when there is a risk of collision.

Images