Drill boom control method, electronic equipment, storage medium, drilling machine and device

Abstract

The invention discloses a drill boom control method, electronic equipment, a storage medium, a drilling machine and a device. The drill boom control method comprises the steps that DH parameters of a plurality of movement joints are obtained; based on the DH parameters of the multiple motion joints, a first DH parameter model of the drill boom is established; obtaining a pose transformation matrix of the target point location; resolving a first DH parameter model according to the pose transformation matrix of the target point location to obtain the horizontal movement distance of the telescopic arm of the first movement joint; calculating a gap of the first movement joint according to the horizontal movement distance; correcting a DH parameter of a movement joint adjacent to the first movement joint by using the horizontal movement distance and the gap; establishing a second DH parameter model based on the DH parameters of the plurality of second motion joints; and the second DH parameter model is solved according to the pose transformation matrix of the target point position, the control quantities of multiple second movement joints are obtained, and the control quantities of the second movement joints are used for controlling the second movement joints of the drill boom.

Description

Technical Field

[0001] The present application relates to the technical field of intelligent control of tunneling equipment, and in particular to a drill arm control method, electronic equipment, storage medium, drilling rig and device. Background Art

[0002] With the development of production technology, the mining process has been upgraded to become more and more intelligent, and the drilling rig, as the main equipment for tunnel development, has developed rapidly. When the drill arm of the drilling rig is automatically controlled, the control system has insufficient control accuracy of the drill arm due to the gaps between the various moving joints of the drill arm. Summary of the invention

[0003] The present application aims to provide a drill arm control method, electronic equipment, storage medium, drilling rig and device, which can improve the control accuracy of the drill arm.

[0004] In a first aspect, an embodiment of the present application provides a method for controlling a drill arm, wherein the drill arm has a plurality of motion joints, and the method comprises:

[0005] Obtaining DH parameters of a plurality of the motion joints;

[0006] Based on the DH parameters of the plurality of motion joints, establishing a first DH parameter model of the drill arm;

[0007] Obtaining a posture transformation matrix of a target point, wherein the target point is a point that the end of the drill arm needs to reach;

[0008] Solve the first DH parameter model according to the posture transformation matrix of the target point to obtain the horizontal movement distance of the telescopic arm of the first motion joint, where the first motion joint is the motion joint for gap compensation;

[0009] Calculating the clearance of the first motion joint according to the horizontal movement distance;

[0010] Using the horizontal movement distance and the gap, correcting the DH parameter of the motion joint adjacent to the first motion joint;

[0011] Establishing a second DH parameter model based on DH parameters of a plurality of second motion joints, wherein the second motion joints are the motion joints other than the first motion joints;

[0012] The second DH parameter model is solved according to the posture transformation matrix of the target point to obtain the control quantities of multiple second motion joints, and the control quantities of the second motion joints are used to control the second motion joints of the drill arm.

[0013] According to some embodiments of the present application, the gap is calculated by the following formula:

[0014] DV=(DL / L–1)*W,

[0015] Wherein, W is the gap, DV is the amount of descent of the telescopic arm of the first motion joint due to the gap and gravity, DL is the telescopic length of the telescopic arm, and L is the total length of the telescopic arm.

[0016] According to some embodiments of the present application, the DH parameters of the motion joint include the rotation angle of the motion joint on the first coordinate axis, the translation amount of the motion joint on the first coordinate axis, the rotation angle of the motion joint on the second coordinate axis, and the translation amount of the motion joint on the second coordinate axis;

[0017] The method of modifying the DH parameter of the motion joint adjacent to the first motion joint by using the horizontal movement distance and the gap comprises:

[0018] According to the gap and the horizontal movement distance, correcting the translation amount of the previous motion joint of the first motion joint on the second coordinate axis;

[0019] According to the gap and the horizontal movement distance, correcting the translation amount of the next motion joint of the first motion joint on the first coordinate axis;

[0020] According to the gap and the horizontal movement distance, the rotation angle of the next motion joint of the first motion joint on the first coordinate axis is corrected.

[0021] According to some embodiments of the present application, the translation amount of the previous motion joint of the first motion joint on the second coordinate axis is corrected by the following formula:

[0022] d2_2=d2_2_0+d3*cos(arcsin((W+(d3 / L–1)*W) / L)),

[0023] Among them, d2_2 is the corrected translation of the motion joint in the second coordinate axis, d2_2_0 is the reference zero point of the translation in the second coordinate axis, d3 is the horizontal movement distance, W is the gap, and L is the total length of the telescopic arm of the first motion joint.

[0024] According to some embodiments of the present application, the translation amount of the next motion joint of the first motion joint on the first coordinate axis is corrected by the following formula:

[0025] a3_2=a3_2_0+(d3 / L–1)*W,

[0026] Among them, a3_2 is the corrected translation amount of the motion joint on the first coordinate axis, a3_2_0 is the reference zero point of the translation amount on the first coordinate axis, d3 is the horizontal movement distance, W is the gap, and L is the total length of the telescopic arm of the first motion joint.

[0027] According to some embodiments of the present application, the rotation angle of the next motion joint of the first motion joint on the first coordinate axis is corrected by the following formula:

[0028] theta3_2=Dtheta3_2_0+arcsin((W+(d3 / L–1)*W) / L),

[0029] Among them, theta3_2 is the corrected rotation angle of the motion joint on the first coordinate axis, d3 is the horizontal movement distance, W is the gap, and L is the total length of the telescopic arm of the first motion joint.

[0030] In a second aspect, an embodiment of the present application provides an electronic device, including:

[0031] at least one processor;

[0032] at least one memory for storing at least one program;

[0033] When at least one of the programs is executed by at least one of the processors, the drill boom control method as described above is implemented.

[0034] In a third aspect, an embodiment of the present application provides a computer-readable storage medium, in which a program executable by a processor is stored. When the program executable by the processor is executed by the processor, it is used to implement the drill arm control method as described above.

[0035] In a fourth aspect, an embodiment of the present application provides a drilling rig, comprising an MCU, which, when executed, is used to implement the drill arm control method as described above.

[0036] In a fifth aspect, an embodiment of the present application provides a drill arm control device, wherein the drill arm has a plurality of motion joints, and the device comprises:

[0037] A first acquisition module, used for acquiring DH parameters of a plurality of said motion joints;

[0038] A first modeling module, used for establishing a first DH parameter model of the drill arm based on the DH parameters of the plurality of motion joints;

[0039] A second acquisition module is used to acquire a posture transformation matrix of a target point, where the target point is the point that the end of the drill arm needs to reach;

[0040] A first solving module is used to solve the first DH parameter model according to the posture transformation matrix of the target point to obtain the horizontal movement distance of the telescopic arm of the first motion joint, where the first motion joint is the motion joint for gap compensation;

[0041] A calculation module, used for calculating the clearance of the first motion joint according to the horizontal movement distance;

[0042] a correction module, used to correct the DH parameter of the motion joint adjacent to the first motion joint by using the horizontal movement distance and the gap;

[0043] A second modeling module, used for establishing a second DH parameter model based on DH parameters of a plurality of second motion joints, where the second motion joints are the motion joints other than the first motion joints;

[0044] The second solving module is used to solve the second DH parameter model according to the posture transformation matrix of the target point to obtain the control amount of multiple second motion joints, and the control amount of the second motion joint is used to control the second motion joint of the drill arm.

[0045] In an embodiment of the present application, the horizontal movement distance of the telescopic arm of the first motion joint is calculated by using a first DH parameter model, and then the gap of the first motion joint is calculated using the horizontal movement distance. According to the horizontal movement distance and the gap, the DH parameters of the motion joint adjacent to the first motion joint are corrected, that is, the error caused by the gap is compensated using the horizontal movement distance and the gap. Then, based on the DH parameters of multiple second motion joints, a second DH parameter model is established. The control quantities of multiple second motion joints are obtained using the second DH parameter model, thereby controlling the second motion joint of the drill arm and improving the control accuracy.

[0046] Additional aspects and advantages of the present application will be given in part in the description below, and in part will become apparent from the description below, or will be learned through the practice of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The present application is further described below with reference to the accompanying drawings and embodiments, wherein:

[0048] Figure 1 A schematic diagram of the structure of eight motion joints of an embodiment of a drill arm control method provided in the present application;

[0049] Figure 2 A flowchart of an embodiment of a drill arm control method provided in the present application;

[0050] Figure 3 A schematic diagram of DH parameters of eight motion joints of an embodiment of a drill arm control method provided in the present application;

[0051] Figure 4 A schematic diagram of the structures of seven second motion joints of an embodiment of a drill arm control method provided in the present application;

[0052] Figure 5 A schematic diagram of DH parameters of seven second motion joints of an embodiment of a drill arm control method provided in the present application;

[0053] Figure 6 A schematic diagram of an electronic device embodiment provided by the present application;

[0054] Figure 7 This is a schematic diagram of an embodiment of a drill arm control device provided in the present application.

[0055] Reference numerals:

[0056] Electronic device 100, processor 110, memory 120, drill arm control device 200, first acquisition module 210, first modeling module 220, second acquisition module 230, first solution module 240, calculation module 250, correction module 260, second modeling module 270, second solution module 280. DETAILED DESCRIPTION

[0057] The embodiments of the present application are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application, and cannot be understood as limiting the present application.

[0058] In the description of the present application, it should be understood that the descriptions involving orientation, such as the orientation or positional relationship indicated as up, down, etc., are based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present application.

[0059] In the description of this application, "a plurality" means more than two. If there is a description of "first" or "second", it is only for the purpose of distinguishing technical features, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features or implicitly indicating the order of the indicated technical features.

[0060] In the description of this application, unless otherwise clearly defined, terms such as setting, installing, connecting, etc. should be understood in a broad sense, and technicians in the relevant technical field can reasonably determine the specific meanings of the above terms in this application based on the specific content of the technical solution.

[0061] Refer to the following Figures 1 to 7A drill arm control method, an electronic device 100, a storage medium, a drilling rig, and a device according to an embodiment of the present application are described.

[0062] The present application embodiment provides a drill arm control method, such as Figure 1 As shown, Figure 1 The structural diagram of the drill arm is shown in Figure 1. The drill arm has multiple motion joints, such as Figure 2 As shown, the method includes:

[0063] Step S100, obtaining DH parameters of multiple motion joints;

[0064] Step S200, establishing a first DH parameter model of the drill arm based on the DH parameters of the plurality of motion joints;

[0065] Step S300, obtaining a posture transformation matrix of a target point, where the target point is the point that the end of the drill arm needs to reach;

[0066] Step S400, solving a first DH parameter model according to the posture transformation matrix of the target point to obtain a horizontal movement distance of the telescopic arm of the first motion joint, where the first motion joint is a motion joint for gap compensation;

[0067] Step S500, calculating the clearance of the first motion joint according to the horizontal movement distance;

[0068] Step S600, using the horizontal movement distance and the gap, correcting the DH parameters of the motion joints adjacent to the first motion joint;

[0069] Step S700, establishing a second DH parameter model based on DH parameters of a plurality of second motion joints, where the second motion joints are motion joints other than the first motion joints;

[0070] Step S800, solving the second DH parameter model according to the posture transformation matrix of the target point to obtain the control amount of multiple second motion joints, and the control amount of the second motion joints is used to control the second motion joints of the drill arm.

[0071] In an embodiment of the present application, the horizontal movement distance of the telescopic arm of the first motion joint is calculated by using the first DH parameter model, and then the gap of the first motion joint is calculated using the horizontal movement distance. According to the horizontal movement distance and the gap, the DH parameters of the motion joint adjacent to the first motion joint are corrected, that is, the error caused by the gap is compensated using the horizontal movement distance and the gap. Then, based on the DH parameters of multiple second motion joints, a second DH parameter model is established. The control amount of multiple second motion joints is obtained using the second DH parameter model, thereby controlling the second motion joints of the drill arm, improving the control accuracy, and making the automatic hole finding and positioning of the drill arm more accurate.

[0072] In the above step S100, a Cartesian coordinate system is established, and the Cartesian coordinate system includes a first coordinate axis and a second coordinate axis that are perpendicular to each other, so as to obtain DH parameters of multiple motion joints in the Cartesian coordinate system. The DH parameters include the rotation angle of the motion joint on the first coordinate axis, the translation amount of the motion joint on the first coordinate axis, the rotation angle of the motion joint on the second coordinate axis, and the translation amount of the motion joint on the second coordinate axis.

[0073] In the above step S200, based on the DH parameters of multiple motion joints, a first DH parameter model of the drill arm is established, and the first DH parameter model includes the posture transformation matrix of the multiple motion joints and the posture transformation matrix of the end of the drill arm. Taking a drill arm with eight motion joints as an example, based on the DH parameters of the eight motion joints, the first DH parameter model is established as follows:

[0074] T1_01=f(theta1,a1,alpha1,d1),

[0075] T1_12=f(theta2,a2,alpha2,d2),

[0076] T1_23=f(theta3,a3,alpha3,d3),

[0077] T1_34=f(theta4,a4,alpha4,d4),

[0078] T1_45=f(theta5,a5,alpha5,d5),

[0079] T1_56=f(theta6,a6,alpha6,d6),

[0080] T1_67=f(theta7,a7,alpha7,d7),

[0081] T1_78=f(theta8,a8,alpha8,d8),

[0082] T1_08=T1_01*T1_12*T1_23*T1_34*T1_45*T1_56*T1_67*T1_78,

[0083] Among them, T1_01, T1_12, T1_23, T1_34, T1_45, T1_56, T1_67, and T1_78 represent the position transformation matrices of the eight motion joints, and T1_08 represents the position transformation matrix of the end of the drill arm. Figure 3As shown in the figure, theta1, theta2, theta3, theta4, theta5, theta6, theta7, theta8 represent the rotation angles of the eight motion joints on the first coordinate axis. a1, a2, a3, a4, a5, a6, a7, a8 represent the translation amounts of the eight motion joints on the first coordinate axis, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6, alpha7, alpha8 represent the rotation angles of the eight motion joints on the second coordinate axis. d1, d2, d3, d4, d5, d6, d7, d8 represent the translation amounts of the eight motion joints on the second coordinate axis. theta1, theta2, d3, theta4, theta5, theta6, theta7 and d8 are unknown quantities, and the remaining DH parameters are known quantities that can be obtained through sensors on the drill arm.

[0084] In the above step S300, when controlling the drill arm, the drill arm end can reach a preset target point by controlling multiple motion joints. According to the position of the preset target point, the position transformation matrix of the target point in the Cartesian coordinate system can be obtained.

[0085] In the above step S400, according to the pose transformation matrix of the target point, the first DH parameter model is solved based on the Newton-Euler method, and theta1, theta2, d3, theta4, theta5, theta6, theta7 and d8 can be solved. The pose transformation matrix corresponding to the first motion joint is T1_23, and d3 is the horizontal movement distance of the telescopic arm of the first motion joint.

[0086] In the above step S700, based on the DH parameters of the plurality of second motion joints, a second DH parameter model is established, such as Figure 4 As shown, Figure 4 It is a schematic diagram of the structure of seven second motion joints. The second DH parameter model includes the posture transformation matrix of multiple second motion joints and the posture transformation matrix of the end of the drill arm. The expression of the second DH parameter model is as follows:

[0087] T2_01=f(theta1_2,a1_2,alpha1_2,d1_2),

[0088] T2_12=f(theta2_2,a2_2,alpha2_2,d2_2),

[0089] T2_23=f(theta3_2,a3_2,alpha3_2,d3_2),

[0090] T2_34=f(theta4_2,a4_2,alpha4_2,d4_2),

[0091] T2_45=f(theta5_2,a5_2,alpha5_2,d5_2),

[0092] T2_56=f(theta6_2,a6_2,alpha6_2,d6_2),

[0093] T2_67=f(theta7_2,a7_2,alpha7_2,d7_2),

[0094] T2_07=T2_01*T2_12*T2_23*T2_34*T2_45*T2_56*T2_67,

[0095] Among them, T2_01, T2_12, T2_23, T2_34, T2_45, T2_56, and T2_67 represent the position transformation matrices of the seven second motion joints, and T2_07 represents the position transformation matrix of the end of the drill arm. Figure 5 As shown, theta1_2, theta2_2, theta3_2, theta4_2, theta5_2, theta6_2, theta7_2 represent the rotation angles of the seven second motion joints on the first coordinate axis. a1_2, a2_2, a3_2, a4_2, a5_2, a6_2, a7_2 represent the translation amounts of the seven second motion joints on the first coordinate axis, alpha1_2, alpha2_2, alpha3_2, alpha4_2, alpha5_2, alpha6_2, alpha7_2 represent the rotation angles of the seven second motion joints on the second coordinate axis. d1_2, d2_2, d3_2, d4_2, d5_2, d6_2, d7_2 represent the translation amounts of the seven second motion joints on the second coordinate axis. theta1_2, theta2_2, theta3_2, theta4_2, theta5_2, theta6_2 and d7_2 are unknown quantities, and the remaining DH parameters are known quantities.

[0096] It can be seen from the above that compared with the first DH parameter model, the second DH parameter model simplifies and removes the posture transformation matrix of the first motion joint. The DH parameters of the motion joints adjacent to the first motion joint have been corrected using the horizontal movement distance and gap, thereby eliminating the deviation caused by gap compensation.

[0097] In the above step S800, according to the posture transformation matrix of the target point, the second DH parameter model is solved based on the Newton-Euler method to obtain the control quantities of multiple second motion joints, that is, theta1_2, theta2_2, theta3_2, theta4_2, theta5_2, theta6_2 and d7_2 are solved, and the second motion joints are controlled according to the solution results.

[0098] In some implementations of the present application, in step S500, the gap is calculated using the following formula:

[0099] DV=(DL / L–1)*W,

[0100] Wherein, W is the gap, DV is the amount of descent of the telescopic arm of the first motion joint due to the gap and gravity, DL is the telescopic length of the telescopic arm, and L is the total length of the telescopic arm.

[0101] In some embodiments of the present application, the step S600 of “correcting the DH parameters of the motion joints adjacent to the first motion joint by using the horizontal movement distance and the gap” is further described, and the step S600 includes:

[0102] Step S610: correcting the translation amount of the previous motion joint of the first motion joint on the second coordinate axis according to the gap and the horizontal movement distance;

[0103] Step S620: correcting the translation amount of the next motion joint of the first motion joint on the first coordinate axis according to the gap and the horizontal movement distance;

[0104] Step S630: Correcting the rotation angle of the next motion joint of the first motion joint on the first coordinate axis according to the gap and the horizontal movement distance.

[0105] In this embodiment, according to the gap and the horizontal movement distance, the translation amount of the previous motion joint of the first motion joint on the second coordinate axis, the translation amount of the next motion joint of the first motion joint on the first coordinate axis, and the rotation angle of the next motion joint of the first motion joint on the first coordinate axis are corrected, that is, the deviation of the adjacent motion joints caused by the gap of the first motion joint is compensated.

[0106] In some implementations of the present application, the translation amount of the previous motion joint of the first motion joint on the second coordinate axis is corrected by the following formula:

[0107] d2_2=d2_2_0+d3*cos(arcsin((W+(d3 / L–1)*W) / L)),

[0108] Among them, d2_2 is the corrected translation of the motion joint in the second coordinate axis, d2_2_0 is the reference zero point of the translation in the second coordinate axis, d3 is the horizontal movement distance, W is the gap, and L is the total length of the telescopic arm of the first motion joint.

[0109] In some implementations of the present application, the translation amount of the next motion joint of the first motion joint on the first coordinate axis is corrected by the following formula:

[0110] a3_2=a3_2_0+(d3 / L–1)*W,

[0111] Among them, a3_2 is the corrected translation amount of the motion joint on the first coordinate axis, a3_2_0 is the reference zero point of the translation amount on the first coordinate axis, d3 is the horizontal movement distance, W is the gap, and L is the total length of the telescopic arm of the first motion joint.

[0112] In some implementations of the present application, the rotation angle of the next motion joint of the first motion joint on the first coordinate axis is corrected by the following formula:

[0113] theta3_2=Dtheta3_2_0+arcsin((W+(d3 / L–1)*W) / L),

[0114] Among them, theta3_2 is the rotation angle of the corrected motion joint on the first coordinate axis, d3 is the horizontal movement distance, W is the gap, and L is the total length of the telescopic arm of the first motion joint.

[0115] In addition, the present application embodiment provides an electronic device 100, such as Figure 6 As shown, including:

[0116] at least one processor 110;

[0117] At least one memory 120, used to store at least one program;

[0118] When at least one program is executed by at least one processor 110 , the above-mentioned drill boom control method is implemented.

[0119] The electronic device 100 provided in the embodiment of the present application can implement each process implemented in the above method embodiment and achieve the same beneficial effects. To avoid repetition, it will not be described again here.

[0120] In addition, an embodiment of the present application provides a computer-readable storage medium, in which a program executable by the processor 110 is stored. When the program executable by the processor 110 is executed by the processor 110, it is used to implement the drill arm control method as described above.

[0121] The computer-readable storage medium provided in the embodiment of the present application can implement each process implemented in the above method embodiment and achieve the same beneficial effects. To avoid repetition, it will not be described here.

[0122] In addition, an embodiment of the present application provides a drilling rig, including an MCU, which, when executed, is used to implement the drill arm control method as described above.

[0123] The drilling rig provided in the embodiment of the present application can implement each process implemented in the above method embodiment and achieve the same beneficial effects. To avoid repetition, it will not be described here.

[0124] In addition, the embodiment of the present application provides a drill arm control device 200, the drill arm has multiple motion joints, such as Figure 7 As shown, the device comprises:

[0125] A first acquisition module 210 is used to acquire DH parameters of multiple motion joints;

[0126] A first modeling module 220, for establishing a first DH parameter model of the drill arm based on DH parameters of a plurality of motion joints;

[0127] The second acquisition module 230 is used to acquire the posture transformation matrix of the target point, where the target point is the point that the end of the drill arm needs to reach;

[0128] A first solving module 240 is used to solve a first DH parameter model according to a posture transformation matrix of a target point to obtain a horizontal movement distance of a telescopic arm of a first motion joint, where the first motion joint is a motion joint for gap compensation;

[0129] A calculation module 250, used for calculating the clearance of the first motion joint according to the horizontal movement distance;

[0130] A correction module 260, for correcting DH parameters of a motion joint adjacent to the first motion joint using the horizontal movement distance and the gap;

[0131] A second modeling module 270, for establishing a second DH parameter model based on DH parameters of a plurality of second motion joints, where the second motion joints are motion joints other than the first motion joints;

[0132] The second solving module 280 is used to solve the second DH parameter model according to the posture transformation matrix of the target point to obtain the control amount of multiple second motion joints, and the control amount of the second motion joints is used to control the second motion joints of the drill arm.

[0133] The drill arm control device 200 provided in the embodiment of the present application can implement each process implemented in the above method embodiment and achieve the same beneficial effects. To avoid repetition, it will not be described again here.

[0134] It will be appreciated by those skilled in the art that all or some of the steps and systems in the disclosed method above may be implemented as software, firmware, hardware and appropriate combinations thereof. Some physical components or all physical components may be implemented as software executed by a processor, such as a central processing unit, a digital signal processor or a microprocessor, or may be implemented as hardware, or may be implemented as an integrated circuit, such as an application specific integrated circuit. Such software may be distributed on a computer-readable medium, which may include a computer storage medium (or a non-transitory medium) and a communication medium (or a temporary medium). As known to those skilled in the art, the term computer storage medium includes volatile and non-volatile, removable and non-removable media implemented in any method or technology for storing information (such as computer-readable instructions, data structures, program modules or other data). Computer storage media include, but are not limited to, RAM, ROM, EEPROM, flash memory or other memory technologies, CD-ROM, digital versatile disks (DVD) or other optical disk storage, magnetic cassettes, magnetic tapes, disk storage or other magnetic storage devices, or any other medium that may be used to store desired information and may be accessed by a computer. Furthermore, it is well known to those skilled in the art that communication media typically embodies computer readable instructions, data structures, program modules, or other data in a modulated data signal such as a carrier wave or other transport mechanism, and may include any information delivery media.

[0135] The embodiments of the present application are described in detail above in conjunction with the accompanying drawings, but the present application is not limited to the above embodiments, and various changes can be made within the knowledge scope of ordinary technicians in the relevant technical field without departing from the purpose of the present application.

Claims

1. A drill arm control method, characterized in that: The drill arm has a plurality of motion joints, and the method comprises: Obtaining DH parameters of a plurality of the motion joints; Based on the DH parameters of the plurality of motion joints, establishing a first DH parameter model of the drill arm; Obtaining a posture transformation matrix of a target point, wherein the target point is a point that the end of the drill arm needs to reach; Solve the first DH parameter model according to the posture transformation matrix of the target point to obtain the horizontal movement distance of the telescopic arm of the first motion joint, where the first motion joint is the motion joint for gap compensation; Calculating the clearance of the first motion joint according to the horizontal movement distance; Using the horizontal movement distance and the gap, correcting the DH parameter of the motion joint adjacent to the first motion joint; Establishing a second DH parameter model based on DH parameters of a plurality of second motion joints, wherein the second motion joints are the motion joints other than the first motion joints; The second DH parameter model is solved according to the posture transformation matrix of the target point to obtain the control quantities of the plurality of second motion joints, and the control quantities of the second motion joints are used to control the second motion joints of the drill arm.

2. The drill arm control method according to claim 1, characterized in that: The gap is calculated by the following formula: DV=(DL / L–1)*W, Wherein, W is the gap, DV is the amount of descent of the telescopic arm of the first motion joint due to the gap and gravity, DL is the telescopic length of the telescopic arm, and L is the total length of the telescopic arm.

3. The drill arm control method according to claim 1, characterized in that: The DH parameters of the motion joint include the rotation angle of the motion joint on the first coordinate axis, the translation amount of the motion joint on the first coordinate axis, the rotation angle of the motion joint on the second coordinate axis, and the translation amount of the motion joint on the second coordinate axis; The method of modifying the DH parameter of the motion joint adjacent to the first motion joint by using the horizontal movement distance and the gap comprises: According to the gap and the horizontal movement distance, correcting the translation amount of the previous motion joint of the first motion joint on the second coordinate axis; According to the gap and the horizontal movement distance, correcting the translation amount of the next motion joint of the first motion joint on the first coordinate axis; According to the gap and the horizontal movement distance, the rotation angle of the next motion joint of the first motion joint on the first coordinate axis is corrected.

4. The drill arm control method according to claim 3, characterized in that: The translation amount of the previous motion joint of the first motion joint on the second coordinate axis is corrected by the following formula: d2_2=d2_2_0+d3*cos(arcsin((W+(d3 / L–1)*W) / L)), Among them, d2_2 is the corrected translation of the motion joint in the second coordinate axis, d2_2_0 is the reference zero point of the translation in the second coordinate axis, d3 is the horizontal movement distance, W is the gap, and L is the total length of the telescopic arm of the first motion joint.

5. The drill arm control method according to claim 3, characterized in that: The translation amount of the next motion joint of the first motion joint on the first coordinate axis is corrected by the following formula: a3_2=a3_2_0+(d3 / L–1)*W, Among them, a3_2 is the corrected translation amount of the motion joint on the first coordinate axis, a3_2_0 is the reference zero point of the translation amount on the first coordinate axis, d3 is the horizontal movement distance, W is the gap, and L is the total length of the telescopic arm of the first motion joint.

6. The drill arm control method according to claim 3, characterized in that: The rotation angle of the next motion joint of the first motion joint on the first coordinate axis is corrected by the following formula: theta3_2=Dtheta3_2_0+arcsin((W+(d3 / L–1)*W) / L), Among them, theta3_2 is the corrected rotation angle of the motion joint on the first coordinate axis, d3 is the horizontal movement distance, W is the gap, and L is the total length of the telescopic arm of the first motion joint.

7. An electronic device, characterized in that: include: at least one processor; at least one memory for storing at least one program; When at least one of the programs is executed by at least one of the processors, the drill boom control method according to any one of claims 1 to 6 is implemented.

8. A computer-readable storage medium, characterized in that: A program executable by a processor is stored therein, and when the program executable by the processor is executed by the processor, it is used to implement the drill arm control method according to any one of claims 1 to 6.

9. A drilling rig, characterized in that: It includes an MCU, which is used to implement the drill arm control method according to any one of claims 1 to 6 when executed.

10. A drill arm control device, characterized in that: The drill arm has a plurality of motion joints, and the device comprises: A first acquisition module, used for acquiring DH parameters of a plurality of said motion joints; A first modeling module, used for establishing a first DH parameter model of the drill arm based on the DH parameters of the plurality of motion joints; A second acquisition module is used to acquire a posture transformation matrix of a target point, where the target point is the point that the end of the drill arm needs to reach; A first solving module is used to solve the first DH parameter model according to the posture transformation matrix of the target point to obtain the horizontal movement distance of the telescopic arm of the first motion joint, where the first motion joint is the motion joint for gap compensation; A calculation module, used for calculating the clearance of the first motion joint according to the horizontal movement distance; a correction module, used to correct the DH parameter of the motion joint adjacent to the first motion joint by using the horizontal movement distance and the gap; A second modeling module, used for establishing a second DH parameter model based on DH parameters of a plurality of second motion joints, where the second motion joints are the motion joints other than the first motion joints; The second solving module is used to solve the second DH parameter model according to the posture transformation matrix of the target point to obtain the control amount of multiple second motion joints, and the control amount of the second motion joint is used to control the second motion joint of the drill arm.

Images