A rock drilling trolley propulsion beam posture control system and control method

By designing the propulsion beam attitude control system of sensors and hydraulic control modules in tunnel rock drilling robots, using dual quaternion calculation and hydraulic control, the problem of inconstant attitude angle of propulsion beams is solved, and the degree of automation of rock drilling trolleys is improved and control efficiency, a more stable operation process is achieved.

CN114637308BActive Publication Date: 2024-12-03XUZHOU HIRSCHMANN ELECTRONICS
View PDF 0 Cites 2 Cited by

Patent Information

Application Number
CN202210242227.1
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-03-11
Publication Date
2024-12-03
Estimated Expiration
2042-03-11

AI Technical Summary

Technical Problem

The existing tunnel rock drilling robot cannot automatically adjust the rotating cylinder, pitch cylinder and swing cylinder of the propulsion beam when the main arm is swinging and pitching, resulting in the inconstant attitude angle at the end of the propulsion beam, affecting the efficient operation of the rock drilling trolley.

Method used

A propulsion beam attitude control system including sensor module, data acquisition and processing module, hydraulic control module and display module was designed. By collecting and processing angle signals in real time, using dual quaternions to establish kinematic equations of single triangle drilling arm, and calculating the propulsion. The beam attitude angle is automatically adjusted through the hydraulic control module to ensure that the attitude angle remains constant.

Benefits of technology

It improves the automation degree and control efficiency of rock drilling trolleys, reduces the labor intensity of the operator, avoids jitter and impact problems in traditional methods, and ensures control accuracy and operation stability.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114637308B_ABST
    Figure CN114637308B_ABST
Patent Text Reader

Abstract

The present invention provides a control system and control method for the attitude control of a propulsion beam of a rock drilling trolley. The system includes a sensor module, a data acquisition and processing module, a hydraulic control module and a display module. The data processing module is respectively connected to the sensor module, the hydraulic control module and the display module. The control method includes calculating the attitude of the end of the propulsion beam of the drill arm according to the dual quaternion. During the movement of the boom, the propulsion attitude of the propulsion beam is kept unchanged by the pitch angle Ba of the propulsion beam and the swing angle Oe of the propulsion beam. The speed and position are used for double closed-loop control. The system can improve the automation of the rock drilling trolley. The method can automatically adjust the actions of the propulsion beam rotating cylinder, the propulsion beam pitching cylinder and the propulsion beam swinging cylinder when the boom swing and boom pitch actions are manually operated, so as to ensure that the attitude angle of the end of the propulsion beam can be kept constant during the operation, thereby improving the efficient operation of the rock drilling trolley.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of rock drilling trolleys, and in particular relates to a rock drilling trolley propulsion beam posture control system and a control method. Background Art

[0002] At present, more and more fully hydraulic rock drilling rigs have been used for drilling operations in the process of drilling and blasting tunnel construction. The accurate positioning of the drilling position during the rock drilling process is the key to drilling construction. In the traditional construction process, the positioning of the drill arm needs to be achieved by the operator operating the movement of the drill arm. This method will produce large errors and the blasting effect is not ideal. In order to solve the shortcomings in traditional construction, tunnel rock drilling robots have quickly become the main equipment for drilling and blasting construction. During construction, tunnel rock drilling robots do not need to draw blasting holes on the working section, and can accurately control the position, angle and depth of the blastholes, effectively reducing the positioning time of the drill arm, reducing the probability of over-excavation and under-excavation, and greatly improving the working environment and work safety of workers, significantly improving the quality and economic benefits of tunnel construction, and is an inevitable trend for the mechanization of tunnel construction in the future. However, the tunnel drilling robot in the prior art has a low degree of automation, and is unable to automatically adjust the thrust beam rotation cylinder, thrust beam pitch cylinder and thrust beam swing cylinder when the boom swing and boom pitch movements are manually operated. It is difficult to ensure that the attitude angle of the end of the thrust beam can remain constant during the operation, which is not conducive to the efficient operation of the drilling trolley. Summary of the invention

[0003] In view of the problems existing in the above-mentioned prior art, the present invention provides a control system and a control method for the thrust beam posture of a rock drilling rig, which can improve the degree of automation of the rock drilling rig, help to improve the control efficiency, and reduce the labor intensity of the operator; the method can automatically adjust the actions of the thrust beam rotation cylinder, the thrust beam pitch cylinder and the thrust beam swing cylinder when the boom swing and boom pitch actions are manually operated, so as to ensure that the posture angle of the end of the thrust beam can be kept constant during the operation, thereby improving the efficient operation of the rock drilling rig.

[0004] In order to achieve the above-mentioned object, the present invention provides a rock drilling trolley propulsion beam posture control system, including a sensor module, a data acquisition and processing module, a hydraulic control module and a display module;

[0005] The sensor module is arranged on the drilling trolley, and is used for collecting the boom swing angle signal, the boom pitch angle signal, the propulsion beam rotation angle signal, the propulsion beam pitch angle signal and the propulsion beam swing angle signal in real time, and sending the collected signals to the data acquisition and processing module in real time;

[0006] The data acquisition and processing module is used to obtain the boom swing angle Sw, the boom pitch angle Lf, the propulsion beam rotation angle Rt, the propulsion beam pitch angle Ba and the propulsion beam swing angle Oe according to the received angle signal; to establish the kinematic equation of the single triangle drill arm using the dual quaternion according to the obtained angle value to obtain the position and posture of all key points on the trolley arm, establish the trolley arm three-dimensional model, and calculate the propulsion beam pitch angle Ba and the propulsion beam swing angle Oe when the propulsion beam attitude angle remains unchanged; to send a control signal to the hydraulic control module according to the calculated Ba and Oe; to control the display module to display information; and to receive parameters input through the display module;

[0007] The hydraulic control module is connected to the data acquisition and processing module, receives and executes the command of the data processing module, controls the electric proportional pressure reducing valve of the hydraulic system, and then realizes the control of the thruster pitch and flip cylinder and the thruster swing hydraulic cylinder;

[0008] The display module is installed in the cab of the rock drilling rig and is used to display data according to the control of the data acquisition and processing module, and at the same time, to provide an interface for operators to set parameters.

[0009] This system can easily obtain the thrust beam pitch angle Ba and thrust beam swing angle Oe when the thrust beam attitude angle remains unchanged, which is conducive to automatically controlling the actions of the thrust beam rotation cylinder, thrust beam pitch cylinder and thrust beam swing cylinder through the hydraulic control module, and intuitively displaying the control process data, which is convenient for the operator to observe. The system can improve the control efficiency and reduce the labor intensity of the operator.

[0010] The present invention also provides a method for controlling the posture of a propulsion beam of a rock drilling trolley, comprising the following steps:

[0011] Step 1: Calculate the position of the end of the drill arm propulsion beam according to the dual quaternion;

[0012] S11: Establishing a vehicle body coordinate system;

[0013] The positive direction of the Z axis is vertically upward from the center of the left and right wing arms, the intersection with the ground is the origin of the coordinate system, the positive direction of the X axis is along the direction of the arm, and the positive direction of the Y axis is determined according to the right-hand rule;

[0014] S12: Obtain the dual quaternion from the vehicle body coordinate system O to the boom swing coordinate system C;

[0015] According to formula (1), the quaternion representing the posture of the propulsion beam in the current state is calculated. The rotation axis is the Z axis. After normalization, it is expressed as v = [0 0 1]. The dual quaternion dualquat1 is generated from the quaternion and a point. v1 is the coordinate value of point C in the coordinate system O. v(1), v(2), and v(3) are the components of v on the X axis, Y axis, and Z axis respectively. The quaternion representing the posture is expressed as quat1.

[0016] quat1=[cos(Sw / 2),v(1)*sin(Sw / 2),v(2)*sin(Sw / 2),v(3)*sin(Sw / 2)] (1);

[0017] In the formula, Sw represents the swing angle of the upper arm, which is determined by manual operation;

[0018] S13: Obtain the dual quaternion from the boom swing coordinate system C to the boom pitch coordinate system D;

[0019] First, the quaternion representing the posture of the propulsion beam in the current state is calculated according to formula (2). The rotation axis is the Y axis, and after unitization, it is expressed as v = [0 1 0],

[0020] Generate the dual quaternion dualquat2_1 from the quaternion and the coordinates of point D, where v2 is the coordinate value of point D in coordinate system C;

[0021] Secondly, according to formula (3), the dual quaternion dualquat2 from the vehicle body coordinate system to the boom pitch coordinate system is obtained:

[0022] Multiply the above two dual quaternions to get the dual quaternion from the vehicle coordinate system to the boom pitch coordinate system:

[0023] quat2=[cos(Lf / 2),v(1)*sin(Lf / 2),v(2)*sin(Lf / 2),v(3)*sin(Lf / 2)] (2);

[0024] dualquat2=dualquat1 x dualquat2_1 (3);

[0025] Where, Lf represents the pitch angle of the boom, which is determined by manual operation;

[0026] S14: According to the method in S13, obtain the dual quaternion Dualquat3 from the boom pitch coordinate system D to the boom telescopic coordinate system E;

[0027] S15: According to the method in S13, obtain the dual quaternion Dualquat4 from the boom telescopic coordinate system E to the propulsion beam rotation coordinate system F;

[0028] S16: According to the method in S13, obtain the dual quaternion Dualquat5 from the propulsion beam rotation coordinate system F to the propulsion beam pitch coordinate system G;

[0029] S17: According to the method in S13, obtain the dual quaternion Dualquat6 from the propulsion beam pitch coordinate system G to the propulsion beam swing coordinate system H;

[0030] S18: Calculate the position and posture of the rear end point K and the front end point J of the propulsion beam;

[0031] The coordinate value of the rear end point K in the H coordinate system is P_K=[K_x K_y K_z], and the coordinate value of the front end point J in the H coordinate system is P_J=[J_x J_y J_z]. The dual quaternion Dualquat6 is used to obtain the coordinate values ​​of the points K and J in the vehicle coordinate system, and the swing angle H and pitch angle V are calculated based on the coordinate values.

[0032] Step 2: During the movement of the boom, the propulsion posture of the propulsion beam is kept unchanged by the pitch angle Ba of the propulsion beam and the swing angle Oe of the propulsion beam;

[0033] When the hole position is a non-peripheral hole and there is no collision between the arms, the propulsion beam rotation angle Rt is not adjusted. At this time, the attitude angle is a function of the propulsion beam pitch angle and the propulsion beam swing angle. Ba and Oe are obtained by inversely solving formula (4) and formula (5) respectively;

[0034] When the hole is a peripheral hole or there is a collision between the arms, the rotation angle Rt of the propulsion beam is first determined by formula (6) according to Sw, Lf and the drilling posture angles H and V, and then Ba and Oe are obtained by inverse solution according to formulas (4) and (5); formula (6) is obtained based on a large amount of empirical data to ensure that the rock drill is always close to the tunnel face;

[0035] The formulas are:

[0036] tan(H)=v(2) / v(1) (4);

[0037] tan(V)=v(3) / (v(1)^2+v(2)^2)^0.5 (5);

[0038] Rt=f(Sw,Lf,H,V) (6);

[0039] Step 3: Use speed and position for double closed-loop control;

[0040] S31: angular velocities u1 and u2 of Ba and Oe calculated according to the angular velocities of Sw and Lf when the propulsion beam posture remains unchanged;

[0041] S32: firstly calculate the error Error1 between the set value and the actual value of Ba and Oe that keeps the propulsion beam attitude angle unchanged according to the inverse solution, and then correct u1 and u2 according to the error to obtain the corrected angular velocities u3 and u4;

[0042] S33: Use the data acquisition and processing module to control the electric proportional pressure reducing valve according to the values ​​of u3 and u4, and then control the action of the thruster pitch and flip cylinder and the thruster swing hydraulic cylinder, and determine the action direction of Ba and Oe in advance according to the direction of manual operation Sw and Lf, and ensure that Ba or Oe only changes direction once when changing direction.

[0043] As a preferred embodiment, in step 2, if multiple solutions appear during the solution process, the optimal Ba angle and Oe angle are determined based on whether interference occurs between the arms and whether the moving path is the shortest.

[0044] As a preferred embodiment, in S33 of step three, when the extreme position occurs and the Ba or Oe angle reaches the mechanical limit and cannot meet the thrust beam attitude angle maintenance function, the data acquisition and processing module controls the display module to issue an alarm reminder.

[0045] This method obtains the dual quaternions from the vehicle body coordinate system to the boom swing coordinate system, from the boom swing coordinate system to the boom pitch coordinate system, from the boom pitch coordinate system to the boom propulsion beam rotation coordinate system, from the propulsion beam rotation coordinate system to the propulsion beam pitch coordinate system, and from the propulsion beam pitch coordinate system to the propulsion beam swing coordinate system in sequence; then the propulsion beam attitude angle is inversely solved through the dual quaternions to obtain the propulsion beam rotation, pitch and swing angles, thereby automatically adjusting the actions of the propulsion beam rotation cylinder, the propulsion beam pitch cylinder and the propulsion beam swing cylinder, ensuring that the attitude angle at the end of the propulsion beam can remain constant during the operation, thereby improving the efficient operation of the rock drilling rig. This method avoids the jitter and impact problems in the traditional method, making the adjustment process smoother and more stable, and ensuring that the control accuracy is higher than the traditional position adjustment method. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 It is the control principle diagram of the system in the present invention;

[0047] Figure 2 is a flow chart of the calculation method of the present invention;

[0048] Figure 3 is a schematic diagram of a vehicle body coordinate system in the present invention;

[0049] Figure 4 is a schematic diagram of the posture of the propulsion beam in the vehicle body coordinate system in the present invention;

[0050] Figure 5 is a schematic diagram of angular velocity setting in the present invention;

[0051] Figure 6 It is a flow chart of the closed-loop control process in the present invention. DETAILED DESCRIPTION

[0052] The present invention will be further described below in conjunction with the accompanying drawings.

[0053] In the prior art, a single triangular drill arm in a rock drilling rig has 5 rotating joints and 3 moving joints, wherein the 5 rotating joints are: boom swing joint, boom amplitude change joint, propulsion beam rotation joint, propulsion beam pitch joint and propulsion beam swing joint, and the relative angles of the above joints are: boom swing angle Sw, boom pitch angle Lf, propulsion beam rotation angle Rt, propulsion beam pitch angle Ba, propulsion beam swing angle Oe. Among them, the 3 moving joints are: boom telescopic length Bom_Ext, propulsion beam telescopic length Fed_Ext, rock drill telescopic length Depth.

[0054] like Figure 1 As shown, a rock drilling rig propulsion beam posture control system includes a sensor module, a data acquisition and processing module, a hydraulic control module and a display module;

[0055] The sensor module is arranged on the drilling trolley, and is used for collecting the boom swing angle signal, the boom pitch angle signal, the propulsion beam rotation angle signal, the propulsion beam pitch angle signal and the propulsion beam swing angle signal in real time, and sending the collected signals to the data acquisition and processing module in real time;

[0056] The data acquisition and processing module is used to obtain the boom swing angle Sw, the boom pitch angle Lf, the propulsion beam rotation angle Rt, the propulsion beam pitch angle Ba and the propulsion beam swing angle Oe according to the received angle signal; to establish the kinematic equation of the single triangle drill arm using the dual quaternion according to the obtained angle value to obtain the position and posture of all key points on the trolley arm, establish the trolley arm three-dimensional model, and calculate the propulsion beam pitch angle Ba and the propulsion beam swing angle Oe when the propulsion beam attitude angle remains unchanged; to send a control signal to the hydraulic control module according to the calculated Ba and Oe; to control the display module to display information; and to receive parameters input through the display module;

[0057] The hydraulic control module is connected to the data acquisition and processing module, receives and executes the command of the data processing module, controls the electric proportional pressure reducing valve of the hydraulic system, and then realizes the control of the thruster pitch and flip cylinder and the thruster swing hydraulic cylinder;

[0058] The display module is installed in the cab of the rock drilling rig and is used to display data according to the control of the data acquisition and processing module, and at the same time, to provide an interface for operators to set parameters.

[0059] like Figure 2 As shown, the present invention also provides a method for controlling the posture of a propulsion beam of a rock drilling rig, comprising the following steps:

[0060] Step 1: If Figure 3 As shown, the position of the end of the drilling arm propulsion beam is calculated based on the dual quaternion;

[0061] S11: Establishing a vehicle body coordinate system;

[0062] The positive direction of the Z axis is vertically upward from the center of the left and right wing arms, the intersection with the ground is the origin of the coordinate system, the positive direction of the X axis is along the direction of the arm, and the positive direction of the Y axis is determined according to the right-hand rule;

[0063] S12: Obtain the dual quaternion from the vehicle body coordinate system O to the boom swing coordinate system C;

[0064] The mathematical form of ordinary quaternion is: q = [cos(θ / 2)nx*sin(θ / 2)ny*sin(θ / 2)nz*sin(θ / 2)]; it can only represent spatial rotation transformation, where [nx, ny and nz] are unit vectors, representing the rotation axis through the origin, and θ represents the rotation angle. The dual number form is And satisfy ε 2 = 0, where r and d represent the real part and the dual part respectively, and ε represents the dual operator. A dual quaternion is a dual number whose real part and dual part are both quaternions. A dual quaternion can represent any combination of rotation and translation in space. Its general form is: u = q r +q d ε, where the quaternion q r Used to describe rotation, quaternion q d =p / 2*q, where p is the translation vector representing the translation, and q represents the imaginary part of the quaternion qd.

[0065] According to formula (1), the quaternion representing the posture of the propulsion beam in the current state is calculated. The rotation axis is the Z axis. After normalization, it is expressed as v =

[001] . The dual quaternion dualquat1 is generated from the quaternion and a point. v1 is the coordinate value of point C in the coordinate system O. v(1), v(2), and v(3) are the components of v on the X axis, Y axis, and Z axis respectively. The quaternion representing the posture is expressed as quat1.

[0066] quat1=[cos(Sw / 2),v(1)*sin(Sw / 2),v(2)*sin(Sw / 2),v(3)*sin(Sw / 2)] (1);

[0067] Where, Sw represents the swing angle of the boom, which is determined by manual operation;

[0068] S13: Obtain the dual quaternion from the boom swing coordinate system C to the boom pitch coordinate system D;

[0069] First, the quaternion representing the posture of the propulsion beam in the current state is calculated according to formula (2). The rotation axis is the Y axis, which is expressed as v =

[010] after normalization. The dual quaternion dualquat2_1 is generated by the quaternion and the coordinates of point D. v2 is the coordinate value of point D in the coordinate system C.

[0070] Secondly, according to formula (3), the dual quaternion dualquat2 from the vehicle body coordinate system to the boom pitch coordinate system is obtained:

[0071] Multiply the above two dual quaternions to get the dual quaternion from the vehicle coordinate system to the boom pitch coordinate system:

[0072] quat2=[cos(Lf / 2),v(1)*sin(Lf / 2),v(2)*sin(Lf / 2),v(3)*sin(Lf / 2)] (2);

[0073] dualquat2=dualquat1 x dualquat2_1 (3);

[0074] Where, Lf represents the pitch angle of the boom, which is determined by manual operation;

[0075] S14: According to the method in S13, obtain the dual quaternion Dualquat3 from the boom pitch coordinate system D to the boom telescopic coordinate system E;

[0076] S15: According to the method in S13, obtain the dual quaternion Dualquat4 from the boom telescopic coordinate system E to the propulsion beam rotation coordinate system F;

[0077] S16: According to the method in S13, obtain the dual quaternion Dualquat5 from the propulsion beam rotation coordinate system F to the propulsion beam pitch coordinate system G;

[0078] S17: According to the method in S13, obtain the dual quaternion Dualquat6 from the propulsion beam pitch coordinate system G to the propulsion beam swing coordinate system H;

[0079] S18: Calculate the position and posture of the rear end point K and the front end point J of the propulsion beam;

[0080] The coordinate value of the rear end point K in the H coordinate system is P_K=[K_x K_y K_z], and the coordinate value of the front end point J in the H coordinate system is P_J=[J_x J_y J_z]. The dual quaternion Dualquat6 is used to obtain the coordinate values ​​of the points K and J in the vehicle coordinate system, and the swing angle H and pitch angle V are calculated based on the coordinate values.

[0081] Step 2: Two Euler angles are needed to describe the posture of the drill arm during positioning, namely the Yaw angle and the Pitch angle. The Yaw angle is also called the H angle, and the Pitch angle is also called the V angle. These two angles are called the posture angles of the drill arm. In actual drilling operations, since the posture angles of adjacent holes are basically the same, it is expected that when the boom swing and boom pitch movements are manually operated, the hydraulic data acquisition and processing module can automatically adjust the propulsion beam rotation cylinder, the propulsion beam pitch cylinder, and the propulsion beam swing cylinder to ensure that the posture angle at the end of the propulsion beam remains unchanged.

[0082] During the movement of the boom, the propulsion posture of the propulsion beam is kept unchanged by the pitch angle Ba of the propulsion beam and the swing angle Oe of the propulsion beam.

[0083] Obtaining the inverse solution of the equation: Figure 4 As shown in the figure, the dotted line is the posture of the propulsion beam in the vehicle coordinate system, the projection of the dotted line in the XOY plane is the line segment OD, the angle between OD and the positive direction of the X-axis is the posture angle H, the angle between OD and the dotted line OJ is the posture angle V, the posture angles H and V are functions of the five rotational joints, among which the two angles Sw and Lf have been determined by manual operation.

[0084] Inverse solution: Two attitude angles determine two equations, but there are three unknowns. Considering that the propulsion beam rotation joint is to facilitate drilling holes around the tunnel and avoid collision, the following solution is adopted for the propulsion beam rotation joint:

[0085] When the hole position is a non-peripheral hole and there is no collision between the arms, the propulsion beam rotation angle Rt is not adjusted. At this time, the attitude angle is a function of the propulsion beam pitch angle and the propulsion beam swing angle. Ba and Oe are obtained by inversely solving formula (4) and formula (5) respectively;

[0086] When the hole position is a peripheral hole or the arms are about to collide, the propulsion beam rotation angle Rt is first determined by formula (6) according to Sw, Lf and the drilling posture angles H and V, and then Ba and Oe are obtained by inverse solution according to formulas (4) and (5); formula (6) is obtained based on a large amount of empirical data to ensure that the rock drill is always close to the tunnel face;

[0087] The formulas are:

[0088] tan(H)=v(2) / v(1) (4);

[0089] tan(V)=v(3) / (v(1)^2+v(2)^2)^0.5 (5);

[0090] Rt=f(Sw,Lf,H,V) (6);

[0091] Get the inverse solution: The solution is in the form of Ba = atan2(p,q), Oe = atan(f(Ba)), where p and q are parameters determined by the angles of Sw, Lf, and Rt. The actual angle range of Ba is -180° to 180°, and the angle range of Oe is -40° to 40°. If multiple solutions appear during the solution process, select the optimal solution according to the following principles: ①, determine whether there will be interference between the arms; ②, determine whether the moving path is the shortest; find the optimal Ba and Oe angles according to the above principles.

[0092] Step 3: Use speed and position for double closed-loop control;

[0093] The hydraulic system has a large inertial mass, slow response speed, and the flow rate is distributed with the load, resulting in poor controllability of the hydraulic system. The present application controls through a dual closed-loop method of speed and position, which can avoid the jitter and impact problems in traditional methods. The adjustment process is smooth and stable, and the control accuracy is higher than the traditional position adjustment method.

[0094] The target angular velocity setting will reduce the boom swing and boom pitch speeds after entering the push beam hold function to ensure that the push beam adjustment speed can keep up with the upper boom movement speed; Figure 5 As shown in the figure, the angular velocity is set to 3 stages. The first stage is the uniform acceleration starting stage (AB stage), which ensures that the angle is stable at the beginning of the adjustment stage; the second stage is the uniform speed stage (BC stage), which ensures that the adjustment can be carried out quickly to reduce the error; the third stage (CD stage) is the uniform deceleration stage. In this stage, the adjustment accuracy should be guaranteed as much as possible, so the time is longer than the starting stage.

[0095] S31: angular velocities u1 and u2 of Ba and Oe calculated according to the angular velocities of Sw and Lf when the propulsion beam posture remains unchanged;

[0096] S32: firstly calculate the error Error1 between the set value and the actual value of Ba and Oe that keeps the propulsion beam attitude angle unchanged according to the inverse solution, and then correct u1 and u2 according to the error to obtain the corrected angular velocities u3 and u4; the actual value is obtained by the data acquisition processing module according to the received angle signal;

[0097] S33: Figure 6 As shown, the data acquisition and processing module is used to control the electric proportional pressure reducing valve according to the values ​​of u3 and u4, thereby controlling the action of the thruster pitch and flip cylinder and the thruster swing hydraulic cylinder, and determining the action direction of Ba and Oe in advance according to the direction of manual operation Sw and Lf, and ensuring that Ba or Oe only changes direction once when changing direction. When the extreme position occurs because the Ba or Oe angle reaches the mechanical limit and cannot meet the thrust beam attitude angle maintenance function, the data acquisition and processing module controls the display module to issue an alarm reminder.

[0098] This method obtains the dual quaternions from the vehicle body coordinate system to the boom swing coordinate system, from the boom swing coordinate system to the boom pitch coordinate system, from the boom pitch coordinate system to the boom propulsion beam rotation coordinate system, from the propulsion beam rotation coordinate system to the propulsion beam pitch coordinate system, and from the propulsion beam pitch coordinate system to the propulsion beam swing coordinate system in sequence; then the propulsion beam attitude angle is inversely solved through the dual quaternions to obtain the propulsion beam rotation, pitch and swing angles, thereby automatically adjusting the actions of the propulsion beam rotation cylinder, the propulsion beam pitch cylinder and the propulsion beam swing cylinder, ensuring that the attitude angle at the end of the propulsion beam can remain constant during the operation, thereby improving the efficient operation of the rock drilling rig. This method is conducive to avoiding the jitter and impact problems in the traditional method, can make the adjustment process smoother and more stable, and ensures that the control accuracy is higher than the traditional position adjustment method.

Claims

1. A method for controlling the posture of a propulsion beam of a rock drilling trolley, the control system based on which includes a sensor module, a data acquisition and processing module, a hydraulic control module and a display module; The sensor module is arranged on the drilling trolley, and is used for collecting the boom swing angle signal, the boom pitch angle signal, the propulsion beam rotation angle signal, the propulsion beam pitch angle signal and the propulsion beam swing angle signal in real time, and sending the collected signals to the data acquisition and processing module in real time; The data acquisition and processing module is used to obtain the boom swing angle Sw, the boom pitch angle Lf, the propulsion beam rotation angle Rt, the propulsion beam pitch angle Ba and the propulsion beam swing angle Oe according to the received angle signal; according to the obtained angle value, the dual quaternion is used to establish the kinematic equation of the single triangle drill arm to obtain the posture of all key points on the trolley arm, establish the trolley arm three-dimensional model, and calculate the propulsion beam pitch angle Ba and the propulsion beam swing angle Oe when the propulsion beam posture angle remains unchanged; according to the calculated Ba and Oe, a control signal is sent to the hydraulic control module; it is used to control the display module to display information; it is used to receive parameters input through the display module; The hydraulic control module is connected to the data acquisition and processing module, receives and executes the command of the data processing module, controls the electric proportional pressure reducing valve of the hydraulic system, and then realizes the control of the thruster pitch and flip cylinder and the thruster swing hydraulic cylinder; The display module is installed in the cab of the rock drilling rig and is used to display data according to the control of the data acquisition and processing module, and at the same time, to provide an interface for operators to set parameters; It is characterized in that The following steps are involved: Step 1: Calculate the position of the end of the drill arm propulsion beam according to the dual quaternion; S11: Establishing a vehicle body coordinate system: the center position of the left and right wing arms pointing vertically upward is the positive direction of the Z axis, the intersection with the ground is the origin of the coordinate system, the direction along the boom is the positive direction of the X axis, and the positive direction of the Y axis is determined according to the right-hand rule; S12: Obtain the dual quaternion from the vehicle body coordinate system O to the boom swing coordinate system C: Calculate the quaternion representing the propulsion beam posture in the current state according to formula (1), with the rotation axis being the Z axis, and the unitized quaternion is expressed as v Z =[0 0 1], the dual quaternion dualquat1 is generated from the quaternion and a point, v1 is the coordinate value of point C in the coordinate system O, v(1), v(2), v(3) are the components of v on the X-axis, Y-axis and Z-axis respectively, and the quaternion representing the posture is denoted as quat1; quat1=[cos(Sw / 2),0,0, sin(Sw / 2)] (1); In the formula, Sw represents the swing angle of the upper arm, which is determined by manual operation; S13: Obtain the dual quaternion from the boom swing coordinate system C to the boom pitch coordinate system D: First, the quaternion representing the posture of the propulsion beam in the current state is calculated according to formula (2), with the rotation axis being the Y axis, and the unitized quaternion is expressed as v Y = [0 1 0], the dual quaternion dualquat2_1 is generated by the quaternion and the coordinates of point D, where v2 is the coordinate value of point D in coordinate system C; Secondly, according to formula (3), the dual quaternion dualquat2 from the vehicle body coordinate system to the boom pitch coordinate system is obtained; Multiply the above two dual quaternions to get the dual quaternion from the vehicle coordinate system to the boom pitch coordinate system: quat2=[cos(Lf / 2),0,sin(Lf / 2),0] (2); dualquat2 =dualquat1 x dualquat2_1 (3); Where, Lf represents the pitch angle of the boom, which is determined by manual operation; S14: According to the method in S13, obtain the dual quaternion Dualquat3 from the boom pitch coordinate system D to the boom telescopic coordinate system E; S15: According to the method in S13, obtain the dual quaternion Dualquat4 from the boom telescopic coordinate system E to the propulsion beam rotation coordinate system F; S16: According to the method in S13, obtain the dual quaternion Dualquat5 from the propulsion beam rotation coordinate system F to the propulsion beam pitch coordinate system G; S17: According to the method in S13, obtain the dual quaternion Dualquat6 from the propulsion beam pitch coordinate system G to the propulsion beam swing coordinate system H; S18: Calculate the position and posture of the rear end point K and the front end point J of the propulsion beam; The coordinate value of the rear end point K in the H coordinate system is P_K = [K_x K_y K_z], and the coordinate value of the front end point J in the H coordinate system is P_J = [J_x J_y J_z]. The dual quaternion Dualquat6 is used to obtain the coordinate values ​​of the points K and J in the vehicle coordinate system, and the swing angle H and pitch angle V are calculated based on the coordinate values; Step 2: During the movement of the boom, the propulsion posture of the propulsion beam is kept unchanged by the pitch angle Ba of the propulsion beam and the swing angle Oe of the propulsion beam; When the hole position is a non-peripheral hole and there is no collision between the arms, the propulsion beam rotation angle Rt is not adjusted. At this time, the attitude angle is a function of the propulsion beam pitch angle and the propulsion beam swing angle. Ba and Oe are obtained by inversely solving formula (4) and formula (5) respectively; When the hole position is a peripheral hole or the arms are about to collide, the propulsion beam rotation angle Rt is first determined by formula (6) according to Sw, Lf and the swing angle H and pitch angle V of the drilling, and then Ba and Oe are obtained by inverse solution according to formula (4) and formula (5). The inverse solution result is in the form of Ba=atan2(p,q),Oe = atan(f(Ba)), where p and q are parameters determined by the angles of Sw, Lf and Rt; the actual angle range of Ba is -180° to 180°, and the angle range of Oe is -40° to 40°; where formula (6) is obtained based on a large amount of empirical data to ensure that the rock drill is always close to the tunnel face; The formulas are: tan(H) = v(2) / v(1) (4); tan(V) = v(3) / (v(1)^2+v(2)^2)^0.5 (5); Rt =f(Sw,Lf,H,V) (6); Step 3: Use speed and position for double closed-loop control; S31: angular velocities u1 and u2 of Ba and Oe calculated according to the angular velocities of Sw and Lf when the propulsion beam posture remains unchanged; S32: firstly calculate the error Error1 between the set value and the actual value of Ba and Oe that keeps the propulsion beam attitude angle unchanged according to the inverse solution, and then correct u1 and u2 according to the error to obtain the corrected angular velocities u3 and u4; S33: Use the data acquisition and processing module to control the electric proportional pressure reducing valve according to the values ​​of u3 and u4, and then control the action of the thruster pitch and flip cylinder and the thruster swing hydraulic cylinder, and determine the action direction of Ba and Oe in advance according to the direction of manual operation Sw and Lf, and ensure that Ba or Oe only changes direction once when changing direction.

2. A method for controlling the posture of a propulsion beam of a rock drilling trolley according to claim 1, characterized in that: In step 2, if multiple solutions appear during the solution process, the optimal Ba angle and Oe angle are determined based on whether there is interference between the arms and whether the moving path is the shortest.

3. A method for controlling the posture of a propulsion beam of a rock drilling trolley according to claim 1 or 2, characterized in that: In step three, in S33, when the extreme position occurs and the Ba or Oe angle reaches the mechanical limit and cannot meet the thrust beam posture angle maintenance function, the data acquisition and processing module controls the display module to issue an alarm.

Citation Information

Cited By

  • Drill jumbo propelling beam composite motion control method

    CN117846510A

  • A method for controlling the composite motion of a rock drilling rig's propulsion beam

    CN117846510B