A drilling robot arm positioning system and method based on monocular vision

Abstract

This application discloses a monocular vision-based positioning system and method for a drilling and anchoring robot's drill arm, relating to the fields of computer vision and robot drill arm positioning technology. The system comprises an industrial computer, a drilling and anchoring robot, a camera cooperative target, and an explosion-proof industrial camera. The camera cooperative target is fixed above and on both sides of the drilling and anchoring robot's body to provide a cooperative target coordinate system for the explosion-proof industrial camera. The explosion-proof industrial camera is fixed to the end of the drilling and anchoring robot's drill arm to capture an original image containing the cooperative target coordinate system. The industrial computer is fixed to the drilling and anchoring robot's body to calculate the position and pose information of the camera coordinate system in the cooperative target coordinate system based on the original image containing the cooperative target coordinate system, perform external parameter calibration and coordinate system conversion, and obtain the position and pose information of the drilling and anchoring robot's drill arm in the body coordinate system. This application can reduce the possibility of error accumulation and improve the accuracy of drilling and anchoring robot's drill arm positioning.

Description

Technical Field

[0001] The present application relates to the technical fields of computer vision and robot drill arm positioning, and in particular to a monocular vision-based drill arm positioning system and method for a drilling and anchoring robot. Background Art

[0002] With the development and application of various intelligent fully mechanized mining equipment, coal mining capacity and efficiency have significantly increased. However, since coal mine tunneling technology is still in the mechanized stage, this has led to a serious imbalance in mining efficiency, resulting in the widespread problem of "fast mining, slow excavation" in the coal mining industry. Therefore, intelligent excavation and reduced manpower to improve efficiency are the current development trends in the coal mining industry. Consequently, coal mine tunnel support work places high demands on the coordination, accuracy, and speed of tunneling and drilling and anchoring equipment, posing a greater challenge to the technical level and physical fitness of the relevant construction personnel. This further slows down tunneling efficiency, resulting in the "fast excavation, slow support" problem. Therefore, the intelligent transformation of coal mine support equipment is the key to solving this problem. However, the current mainstream method for positioning the drill arm of drilling and anchoring robots is mainly based on the DH (Denavit-Hartenberg) coordinate transformation. This method requires multiple iterations, resulting in large cumulative errors. Ultimately, this results in inaccurate and unreliable positioning of the drill arm of the drilling and anchoring robot, making it difficult to widely adopt in coal mines. Summary of the Invention

[0003] The purpose of this application is to provide a monocular vision-based positioning system and method for the drill arm of a drilling and anchoring robot, which can reduce the possibility of error accumulation and improve the accuracy of positioning the drill arm of the drilling and anchoring robot.

[0004] To achieve the above objectives, this application provides the following solutions.

[0005] In a first aspect, the present application provides a monocular vision-based drilling and anchoring robot drill arm positioning system, which includes: an industrial computer, a drilling and anchoring robot, a camera cooperative target, and an explosion-proof industrial camera.

[0006] The camera cooperation target is fixed above and on both sides of the body of the drilling and anchoring robot. The camera cooperation target is used to provide a cooperation target coordinate system for the explosion-proof industrial camera; the cooperation target coordinate system refers to a reference coordinate system established based on the camera cooperation target for assisting the positioning of the drill arm.

[0007] The explosion-proof industrial camera is fixed to the end of the drill arm of the drilling and anchoring robot. The explosion-proof industrial camera is used to collect original images containing the cooperative target coordinate system when the drilling and anchoring robot is working, and send the original images containing the cooperative target coordinate system to the industrial computer.

[0008] The industrial computer is fixed on the body of the drilling and anchoring robot. The industrial computer is used to calculate the pose information of the camera coordinate system in the cooperative target coordinate system based on the original image containing the cooperative target coordinate system, and perform external parameter calibration and coordinate system conversion on the pose information of the camera coordinate system in the cooperative target coordinate system to obtain the pose information of the drilling arm of the drilling and anchoring robot in the body coordinate system; wherein, the camera coordinate system refers to the coordinate system established with the optical center of the explosion-proof industrial camera as the origin, and the body coordinate system refers to the coordinate system established with the center of the chassis of the drilling and anchoring robot as the origin.

[0009] In the second aspect, the present application provides a method for positioning the drill arm of a drilling and anchoring robot based on monocular vision, which is applied to the drilling and anchoring robot drill arm positioning system based on monocular vision, and the method for positioning the drill arm of a drilling and anchoring robot based on monocular vision includes the following steps.

[0010] Acquire the original image containing the cooperative target coordinate system.

[0011] According to the original image containing the cooperative target coordinate system, the pose information of the camera coordinate system in the cooperative target coordinate system is calculated.

[0012] The position and posture information of the camera coordinate system in the cooperative target coordinate system is calibrated with external parameters and converted into a coordinate system to obtain the position and posture information of the drilling arm of the anchor drilling robot in the body coordinate system.

[0013] According to the specific embodiments provided in this application, this application has the following technical effects.

[0014] The present application provides a monocular vision-based positioning system and method for a drilling and anchoring robot's drill arm. The system includes an industrial computer, a drilling and anchoring robot, a camera cooperative target, and an explosion-proof industrial camera. By cleverly designing camera cooperative targets above and on both sides of the drilling and anchoring robot's body, the camera cooperative targets are used to provide a cooperative target coordinate system for the explosion-proof industrial camera, thereby facilitating the explosion-proof industrial camera to capture the original image containing the cooperative target coordinate system, thereby adding the cooperative target coordinate system to the original image. On this basis, the industrial computer can calculate the pose information of the camera coordinate system in the cooperative target coordinate system based on the original image containing the cooperative target coordinate system. The pose information of the camera coordinate system in the cooperative target coordinate system is then calibrated with external parameters and converted into a coordinate system, thereby solving the pose information of the drilling and anchoring robot's drill arm in the body coordinate system. The present application introduces a cooperative target coordinate system into the drilling arm positioning and pose calculation process through the camera cooperative target, which can quickly complete the drilling arm positioning work without the need for multiple iterations, thereby reducing the possibility of error accumulation and thus improving the accuracy of the drilling and anchoring robot's drill arm positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0016] Figure 1 A structural diagram of a monocular vision-based drilling and anchoring robot drill arm positioning system provided in one embodiment of the present application.

[0017] Figure 2 A flowchart of a method for positioning the drill arm of a drilling and anchoring robot based on monocular vision is provided in one embodiment of the present application.

[0018] Figure 3 A schematic diagram of the workflow of a monocular vision-based drilling and anchoring robot drill arm positioning system provided in one embodiment of the present application.

[0019] Figure 4 A partially enlarged view of the coordinate transformation of the drilling arm positioning system of a drilling and anchoring robot based on monocular vision provided in one embodiment of the present application.

[0020] Figure 5 This is a diagram showing the visual positioning principle of a monocular vision-based drilling and anchoring robot drill arm positioning system according to one embodiment of the present application.

[0021] Reference numerals: 1-industrial computer; 2-drilling robot; 3-camera cooperative target; 4-explosion-proof industrial camera. DETAILED DESCRIPTION

[0022] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0023] Currently, the traditional DH coordinate transformation-based positioning method for a robotic drill arm first constructs a coordinate system for each joint of the drilling and anchoring robot. For multi-DOF manipulators, this may require constructing a relatively large number of coordinate systems. Subsequently, by measuring the rotation angle or movement distance of each joint (depending on the joint type, rotational joints correspond to rotation angles, and movement distances correspond to movement distances), the transformation relationships between adjacent coordinate systems are established. These are then multiplied to obtain the transformation matrix of the drill arm relative to the body coordinate system, i.e., its relative position. For multi-DOF manipulators, this process requires multiple calculations of the adjacent coordinate system transformation matrices, ultimately requiring multiple iterations to determine the drill arm's position relative to the body coordinate system. This multiple iteration process can easily lead to error accumulation, resulting in large errors and inaccuracy in the calculated results.

[0024] The present application aims to provide a drilling and anchoring robot drill arm positioning system and method based on monocular vision, so as to reduce the possibility of error accumulation, improve the accuracy of drilling and anchoring robot drill arm positioning, and solve the problem of inaccurate positioning caused by error accumulation in traditional robot drill arm positioning methods based on DH coordinate transformation.

[0025] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0026] like Figure 1 As shown, this embodiment proposes a drilling and anchoring robot drill arm positioning system based on monocular vision, which includes: an industrial computer 1, a drilling and anchoring robot 2, a camera cooperation target 3 and an explosion-proof industrial camera 4.

[0027] The camera cooperation target 3 is fixed on the upper part and both sides of the body of the anchor drilling robot 2 . The camera cooperation target 3 is used to provide a cooperation target coordinate system for the explosion-proof industrial camera 4 .

[0028] In this embodiment, the camera cooperation target 3 is composed of three LED light strips, which can be a spatial linear structure with three LED light strips intersecting at two points and perpendicular to each other. The anchor drilling robot 2 has one LED light strip installed above its body and one LED light strip installed on each side of the body. The LED light strips on each side of the body are perpendicular to the LED light strip above the body.

[0029] In this embodiment, the LED light strip can be a high-brightness red LED light strip, or an LED light strip of other colors, but it needs to be clearly distinguished from the body color of the drilling and anchoring robot 2, for example, Figure 1As shown, in this embodiment, the body of the drilling and anchoring robot 2 is yellow, and the LED light strip of the camera cooperation target 3 adopts a high-brightness red LED light strip, so that the cooperative target coordinate system established by the camera cooperation target 3 can be displayed more obviously and clearly, and the visualization effect is better, thereby ensuring the accuracy of the final drilling arm positioning result.

[0030] In the design of the camera cooperative target 3 in this embodiment, three mutually perpendicular straight lines with LED light strips are used to establish a cooperative target coordinate system to constitute a spatial constraint. It is more suitable for the low-light environment of a mine than the traditional checkerboard target. It is not only more convenient and quick to arrange, but also serves as a reference coordinate system for auxiliary drill arm positioning, which can effectively improve the accuracy of drill arm positioning of the drilling and anchoring robot.

[0031] The explosion-proof industrial camera 4 is fixed to the end of the drill arm of the drilling and anchoring robot 2. The explosion-proof industrial camera 4 is used to collect original images containing the cooperative target coordinate system when the drilling and anchoring robot 2 is working, and send the original images containing the cooperative target coordinate system to the industrial computer 1.

[0032] In this embodiment, the original image refers to an unprocessed working image collected by the drilling arm of the anchor drilling robot during operation.

[0033] The industrial computer 1 is fixed on the body of the drilling and anchoring robot 2. The industrial computer 1 is used to calculate the pose information of the camera coordinate system in the cooperative target coordinate system based on the original image containing the cooperative target coordinate system, and perform external parameter calibration and coordinate system conversion on the pose information of the camera coordinate system in the cooperative target coordinate system to obtain the pose information of the drilling arm of the drilling and anchoring robot 2 in the body coordinate system.

[0034] In this embodiment, the cooperative target coordinate system refers to a reference coordinate system established based on the camera cooperative target 3 for assisting in the positioning of the drill arm. The camera coordinate system refers to a coordinate system established with the optical center of the explosion-proof industrial camera 4 as the origin. The fuselage coordinate system refers to a coordinate system established with the center of the fuselage chassis of the drilling and anchoring robot 2 as the origin. The drill arm coordinate system refers to a coordinate system established with the execution point at the end of the drill arm of the drilling and anchoring robot 2 as the origin. Among them, the execution point at the end of the drill arm refers to the center point of the actuator installed at the end of the drill arm and used for drilling operations. The specific information of each of the above coordinate systems is shown in Table 1.

[0035] Table 1 Coordinate systems involved in the drilling and anchoring robot arm positioning system based on monocular vision

[0036]

[0037] In an exemplary embodiment, a method for positioning a drill arm of a drilling and anchoring robot based on monocular vision is provided. The method for positioning a drill arm of a drilling and anchoring robot based on monocular vision is applied to the aforementioned positioning system for the drill arm of a drilling and anchoring robot based on monocular vision. Figure 2 As shown, the monocular vision-based drilling and anchoring robot drill arm positioning method specifically includes the following steps.

[0038] S1: Acquire the original image containing the cooperative target coordinate system.

[0039] S2: Calculating the pose information of the camera coordinate system in the cooperative target coordinate system according to the original image containing the cooperative target coordinate system.

[0040] S3: performing external parameter calibration and coordinate system conversion on the pose information of the camera coordinate system in the cooperative target coordinate system to obtain the pose information of the drilling arm of the anchor drilling robot 2 in the fuselage coordinate system.

[0041] In this embodiment, based on the original image containing the cooperative target coordinate system, the pose information of the camera coordinate system in the cooperative target coordinate system is calculated; the pose information of the camera coordinate system in the cooperative target coordinate system is calibrated with external parameters and the coordinate system is converted to obtain the pose information of the drill arm of the drilling and anchoring robot 2 in the fuselage coordinate system, which specifically includes the following steps.

[0042] (1) Based on the original image containing the cooperative target coordinate system, the coordinates of two spatial intersection points in the camera coordinate system and the direction vectors of three spatial straight lines in the camera coordinate system are determined respectively.

[0043] (2) Determine the rotation matrix of the camera coordinate system in the cooperative target coordinate system based on the coordinates of the two spatial intersection points in the camera coordinate system and the direction vectors of the three spatial straight lines in the camera coordinate system.

[0044] (3) Determine the translation vector of the camera coordinate system in the cooperative target coordinate system based on the rotation matrix of the camera coordinate system in the cooperative target coordinate system.

[0045] (4) According to the translation vector of the camera coordinate system in the cooperative target coordinate system, the position transformation matrix of the drilling arm of the anchor drilling robot 2 in the fuselage coordinate system is determined.

[0046] (5) According to the posture transformation matrix of the drilling arm of the anchor drilling robot 2 in the fuselage coordinate system, the posture information of the drilling arm of the anchor drilling robot 2 in the fuselage coordinate system is determined.

[0047] In order to make the technical solution of this embodiment clearer, the specific structure and layout of the drilling and anchoring robot drill arm positioning system based on monocular vision in this embodiment, as well as the specific implementation process of the drilling and anchoring robot drill arm positioning method based on monocular vision are described in detail below in the form of examples.

[0048] The monocular vision-based drilling and anchoring robot drill arm positioning system provided in this embodiment mainly includes four parts: an industrial computer 1, a drilling and anchoring robot 2, a camera cooperative target 3 and an explosion-proof industrial camera 4.

[0049] Among them, the explosion-proof industrial camera 4 is fixed at the end of the drilling arm of the drilling and anchoring robot facing the fuselage; the camera cooperation target 3 is a spatial straight line structure composed of three high-brightness red LED light strips that intersect at two points and are perpendicular to each other, and is fixed above and on both sides of the fuselage of the drilling and anchoring robot 2; the industrial computer 1 runs the program corresponding to the drilling arm positioning method of the drilling and anchoring robot based on monocular vision, and after image processing and calculation, it can obtain the position information of the drilling arm of the drilling and anchoring robot in the fuselage coordinate system.

[0050] like Figure 3 As shown in the figure, when the drilling arm positioning system of the drilling and anchoring robot based on monocular vision is working, the drilling arm posture information is determined by the following steps: First, the explosion-proof industrial camera 4 fixed at the end of the drilling arm of the drilling and anchoring robot collects the original image containing the camera cooperation target 3. Then, the linear equations of the three spatial straight lines of the camera cooperation target 3 in the original image in the image coordinate system are obtained by the image processing method. Then, by Figure 2 The positioning method of the drilling robot arm based on monocular vision in the paper obtains the pose information of the camera coordinate system in the cooperative target coordinate system. Figure 4 As shown, based on the pose information of the camera coordinate system in the cooperative target coordinate system, the pose information (including position coordinate information and posture information) of the drilling and anchoring robot's drill arm in the fuselage coordinate system is obtained through external parameter calibration and coordinate system conversion. Figure 4 The symbols of the coordinate systems in are consistent with those in Table 1.

[0051] For the above-mentioned monocular vision-based drilling and anchoring robot drill arm positioning system, Figure 5 As shown, the method for positioning the drill arm of a drilling and anchoring robot based on monocular vision in this embodiment specifically includes the following steps.

[0052] (1) Determine the coordinates of the two spatial intersection points in the camera coordinate system and the direction vectors of the three spatial lines in the camera coordinate system.

[0053] Assume that the three space lines of the cooperative target coordinate system The unit direction vector is And let the two intersection points of the three space lines be the first space intersection point and the second space intersection , where the first spatial intersection is In the image plane The projection on the image is marked as the first image intersection point , record the second spatial intersection In the image plane The projection on the second image is marked as the intersection point , then the first spatial intersection and the second space intersection The coordinates in the camera coordinate system can be expressed as follows.

[0054] (1).

[0055] (2).

[0056] in, Indicates the distance from the camera imaging plane to the camera optical center, that is, the focal length; is the undetermined coefficient, The first spatial intersection The distance from the camera to the optical center is The ratio of the distance to the camera's optical center; The first spatial intersection The distance from the camera to the optical center is The ratio of the distance to the camera's optical center.

[0057] (3).

[0058] in, The first spatial intersection The distance from the camera to the optical center, The first spatial intersection The distance from the camera to the optical center.

[0059] First spatial intersection and the second space intersection The coordinates of can be expressed as and .

[0060] Assume a straight line in space The projection on the image plane is the image line , and its straight line equation is , then the image line Any point on , image straight line The direction vector is .

[0061] For the same straight line equation , and Together they determine the direction of the line, represents the slope of the line; Determines the translation of the line and affects the distance between the line and the origin.

[0062] In this embodiment, the constraint conditions of the geometric constraints of the known space model are as follows.

[0063] (1) First spatial intersection and the second space intersection The distance is ,Right now .

[0064] (2) The first spatial straight line , the second spatial line and the third spatial line The two are perpendicular to each other.

[0065] (3) First spatial intersection The distance from the camera optical center is greater than the second space intersection Distance from the camera's optical center.

[0066] This embodiment uses the perspective projection model, a core mathematical model in computer vision that describes the relationship between a point in three-dimensional space and its projection onto a two-dimensional image plane. This embodiment uses the perspective projection model to transform spatial geometric constraints into image algebraic equations, thereby avoiding the iterative accumulation error problem of traditional robotic drill arm positioning methods based on DH coordinate transformation.

[0067] In this embodiment, the perspective projection model transforms the space straight line , image straight line The camera's optical center is constrained to be in a plane called the projection plane. . Perpendicular to the projection plane Normal vector You can pass the image straight line A point on With camera optical center Direction vector of the line and the image line direction vector The outer product of is obtained, and the following formula is known.

[0068] (4).

[0069] Then we have the following formula.

[0070] (5).

[0071] For the second space line , the second space line The direction vector of the first space intersection and the second space intersection It is expressed as the following formula.

[0072] (6).

[0073] For the first space line , the first space line is known and the second spatial line Vertical, and the first spatial straight line and projection plane Normal vector vertical, so the first straight line in space The direction vector of is equal to the second space line The direction vector and normal vector The outer product of is expressed as follows.

[0074] (7).

[0075] in:

[0076] ;

[0077] ;

[0078] .

[0079] For the third spatial line , the third space line is known and the second spatial line Vertical, and the third spatial straight line and projection plane Normal vector vertical, so the third spatial line The direction vector of is equal to the second space line The direction vector and normal vector The outer product of is expressed as follows.

[0080] (8).

[0081] in:

[0082] ;

[0083] ;

[0084] ;

[0085] in, 、 、 Represents a straight line in space The projection on the image plane is the image line , and its straight line equation is The coefficient of 、 、 and 、 、 Same thing.

[0086] By the first space straight line and the third spatial line The mutually perpendicular constraints can be obtained as follows.

[0087] (9).

[0088] The following formula can be obtained.

[0089] (10).

[0090] Substituting equations (7) and (8) into equation (10), equation (10) can be expressed as follows.

[0091] (11).

[0092] in:

[0093] ;

[0094] ;

[0095] ;

[0096] in, 、 、 It is a parameter set to simplify the calculation process.

[0097] Therefore, the following formula can be obtained from formula (11).

[0098] (12).

[0099] Assume the following formula.

[0100] (13).

[0101] (14).

[0102] Then we can get the following formula.

[0103] (15).

[0104] (16).

[0105] in, 、 It is a parameter set to simplify the calculation process.

[0106] By constraint (1): the first spatial intersection and the second space intersection The distance is ,Right now , then we have the following formula.

[0107] (17).

[0108] Rearranging formula (17) yields the following formula.

[0109] (18).

[0110] in, ; ; ,in 、 、 It is a parameter set to simplify the calculation process.

[0111] Substituting formula (15) into formula (17), we can obtain the following formula.

[0112] (19).

[0113] Substituting formula (16) into formula (17), we can obtain the following formula.

[0114] (20).

[0115] in, The first spatial intersection The distance from the camera to the optical center, The first spatial intersection The distance from the camera to the optical center.

[0116] because It can only take positive values, so the following formula can be obtained from formula (19) and formula (20).

[0117] (twenty one).

[0118] (twenty two).

[0119] Furthermore, substituting equations (21) and (22) into equations (15) and (16) respectively yields the following equations.

[0120] (twenty three).

[0121] (twenty four).

[0122] Combining formula (3), we have the following formula.

[0123] (25).

[0124] Or it can be expressed as the following formula.

[0125] (26).

[0126] By constraint (3): the first spatial intersection The distance from the camera optical center is greater than the second space intersection The distance from the camera's optical center must have the following formula.

[0127] (27).

[0128] Therefore, comparing Equation (24) and Equation (25) we can get a set of correct and The solution.

[0129] Then, the two spatial points can be uniquely determined by equations (1) and (2): Coordinates in the camera coordinate system , and the three spatial lines can be uniquely determined by equations (6), (7) and (8) Direction vector in camera coordinate system , then its unit direction vector is .

[0130] (2) Determine the rotation matrix of the camera coordinate system in the cooperative target coordinate system .

[0131] The first spatial straight line in the cooperative target coordinate system The unit direction vector is , the second space line The direction vector is , the third space straight line The direction vector is , if we assume that the rotation matrix of the camera coordinate system in the cooperative target coordinate system is , then we have the following formula.

[0132] (28).

[0133] in, 、 、 Represent three spatial lines in the camera coordinate system 、 、 The unit direction vector of .

[0134] According to constraint (2): the first space line , the second spatial line and the third spatial line Since the three straight lines in space are perpendicular to each other, They are perpendicular to each other, so we have the following formula.

[0135] (29).

[0136] (3) Determine the translation vector of the camera coordinate system in the cooperative target coordinate system .

[0137] The first spatial point in the cooperative target coordinate system The coordinates are , if we assume that the translation vector of the camera coordinate system in the cooperative target coordinate system is , then we have the following formula.

[0138] (30).

[0139] Among them, since the first spatial point in the cooperative target coordinate system The coordinates are ,therefore is a zero vector, that is, .

[0140] Therefore, the translation vector of the camera coordinate system in the cooperative target coordinate system is as follows.

[0141] (31).

[0142] in, 、 、 They respectively represent the displacement of the translation vector of the camera coordinate system in the cooperative target coordinate system along the coordinate axis.

[0143] (4) Determine the pose transformation matrix of the drilling arm of the anchor drilling robot in the body coordinate system .

[0144] In this embodiment, according to the translation vector of the camera coordinate system in the cooperative target coordinate system, combined with the coordinate system conversion relationship between the fuselage coordinate system, the cooperative target coordinate system, the camera coordinate system and the drill arm coordinate system, the pose transformation matrix from the fuselage coordinate system to the cooperative target coordinate system is determined. , the pose transformation matrix from the cooperative target coordinate system to the camera coordinate system and the pose transformation matrix from the camera coordinate system to the drill arm coordinate system , thus combining the above three posture transformation matrices, we can further determine the posture transformation matrix of the drilling arm of the anchor drilling robot 2 in the fuselage coordinate system: .

[0145] Depend on Figure 4 As shown, the pose transformation matrix from the fuselage coordinate system to the cooperative target coordinate system is Is the following formula.

[0146] (32).

[0147] in, represents the pose transformation matrix from the fuselage coordinate system to the cooperative target coordinate system, and It is the translation extrinsic parameter from the fuselage coordinate system to the cooperative target coordinate system.

[0148] Pose transformation matrix from the cooperative target coordinate system to the camera coordinate system Is the following formula.

[0149] (33).

[0150] in, Represents the pose transformation matrix from the cooperative target coordinate system to the camera coordinate system.

[0151] Pose transformation matrix from camera coordinate system to drill arm coordinate system Is the following formula.

[0152] (34).

[0153] in, represents the pose transformation matrix from the camera coordinate system to the drill arm coordinate system, 、 and is the translation extrinsic parameter from the camera coordinate system to the drill arm coordinate system.

[0154] The pose transformation matrix of the drilling arm of the anchor drilling robot 2 in the body coordinate system is Is the following formula.

[0155] (35).

[0156] in, It represents the pose transformation matrix of the drilling arm of the anchor drilling robot 2 in the body coordinates, which is a 4*4 matrix. For example, , the third-order matrix in the upper left corner It represents the rotation matrix, which represents the posture information of the drilling arm of the anchor drilling robot 2 in the fuselage coordinate system (that is, the rotation angle of the drilling arm of the anchor drilling robot 2 along the three coordinate axes in the fuselage coordinate system). The matrix on the right is represents the translation matrix, which represents the coordinate position information of the drilling arm of the anchor drilling robot 2 in the fuselage coordinate system (i.e., the movement distance of the drilling arm of the anchor drilling robot 2 along the three coordinate axes in the fuselage coordinate system). The coordinate position information and posture information of the drilling arm of the anchor drilling robot 2 in the fuselage coordinate system are combined to form the complete posture information, which is the final drilling arm positioning result.

[0157] Compared with the traditional robot drill arm positioning method based on DH coordinate transformation, the present embodiment proposes a drilling robot drill arm positioning system and method based on monocular vision, which adopts a visual positioning algorithm based on monocular vision, directly solves the posture information by projecting the plane normal vector and spatial geometric constraints, and calculates the posture transformation matrix from the cooperative target coordinate system to the camera coordinate system. , only two steps of iteration are needed to solve the pose information, which effectively reduces the number of iterations and avoids the cumulative error caused by multiple iterations in the traditional robot drill arm positioning method based on DH coordinate transformation, thereby reducing the possibility of error accumulation and improving the accuracy of the drilling and anchor robot drill arm positioning.

[0158] The technical features of the above embodiments can be combined arbitrarily. In order to make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0159] This document uses specific examples to illustrate the principles and implementation methods of this application. The description of the above examples is only intended to help understand the method and core concept of this application. At the same time, for those skilled in the art, based on the concept of this application, there may be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A drilling and anchoring robot arm positioning system based on monocular vision, characterized in that: The monocular vision-based drilling and anchoring robot drill arm positioning system includes: an industrial computer, a drilling and anchoring robot, a camera cooperative target and an explosion-proof industrial camera; The camera cooperation target is fixed on the upper part and both sides of the body of the drilling and anchoring robot. The camera cooperation target is used to provide a cooperation target coordinate system for the explosion-proof industrial camera; the cooperation target coordinate system refers to a reference coordinate system established based on the camera cooperation target for assisting the positioning of the drill arm; The explosion-proof industrial camera is fixed to the end of the drilling arm of the anchor drilling robot. The explosion-proof industrial camera is used to collect original images containing the cooperative target coordinate system when the anchor drilling robot is working, and send the original images containing the cooperative target coordinate system to the industrial computer; The industrial computer is fixed to the fuselage of the drilling and anchoring robot, and is used to calculate the pose information of the camera coordinate system in the cooperative target coordinate system based on the original image containing the cooperative target coordinate system, and perform external parameter calibration and coordinate system conversion on the pose information of the camera coordinate system in the cooperative target coordinate system to obtain the pose information of the drilling arm of the drilling and anchoring robot in the fuselage coordinate system; wherein, the camera coordinate system refers to a coordinate system established with the optical center of the explosion-proof industrial camera as the origin, and the fuselage coordinate system refers to a coordinate system established with the center of the fuselage chassis of the drilling and anchoring robot as the origin; According to the translation vector of the camera coordinate system in the cooperative target coordinate system, in combination with the coordinate system conversion relationships among the fuselage coordinate system, the cooperative target coordinate system, the camera coordinate system and the drill arm coordinate system, respectively determine the pose transformation matrix from the fuselage coordinate system to the cooperative target coordinate system, the pose transformation matrix from the cooperative target coordinate system to the camera coordinate system, and the pose transformation matrix from the camera coordinate system to the drill arm coordinate system; the drill arm coordinate system refers to a coordinate system established with the drill arm end execution point of the drilling and anchoring robot as the origin; The pose transformation matrix from the fuselage coordinate system to the cooperative target coordinate system is expressed as: ； in, represents the pose transformation matrix from the fuselage coordinate system to the cooperative target coordinate system, and It is the translation external parameter from the fuselage coordinate system to the cooperative target coordinate system; The pose transformation matrix from the cooperative target coordinate system to the camera coordinate system is expressed as: ； in, represents the pose transformation matrix from the cooperative target coordinate system to the camera coordinate system, 、 、 They represent the displacement of the translation vector of the camera coordinate system in the cooperative target coordinate system along the coordinate axis; The pose transformation matrix from the camera coordinate system to the drill arm coordinate system is expressed as: ； in, represents the pose transformation matrix from the camera coordinate system to the drill arm coordinate system, 、 and is the translation extrinsic parameter from the camera coordinate system to the drill arm coordinate system; According to the pose transformation matrix from the fuselage coordinate system to the cooperative target coordinate system, the pose transformation matrix from the cooperative target coordinate system to the camera coordinate system, and the pose transformation matrix from the camera coordinate system to the drill arm coordinate system, the pose transformation matrix of the drill arm of the anchor drilling robot in the fuselage coordinate system is calculated, which is expressed as: ； in, Represents the pose transformation matrix of the drilling arm of the anchor drilling robot in the body coordinates.

2. The monocular vision-based drilling and anchoring robot drill arm positioning system according to claim 1 is characterized in that: The camera cooperation target is a spatial straight line structure composed of three LED light strips that intersect at two points and are perpendicular to each other.

3. The monocular vision-based drilling and anchoring robot drill arm positioning system according to claim 2, characterized in that: The LED light strip is a high-brightness red LED light strip.

4. A method for positioning the drill arm of a drilling and anchoring robot based on monocular vision, characterized in that: The monocular vision-based positioning method for the drill arm of an anchor drilling robot is applied to the monocular vision-based positioning system for the drill arm of an anchor drilling robot according to any one of claims 1 to 3, and the monocular vision-based positioning method for the drill arm of an anchor drilling robot comprises: Acquire the original image containing the cooperative target coordinate system; Calculating the pose information of the camera coordinate system in the cooperative target coordinate system according to the original image containing the cooperative target coordinate system; The position and posture information of the camera coordinate system in the cooperative target coordinate system is calibrated with external parameters and converted into a coordinate system to obtain the position and posture information of the drilling arm of the anchor drilling robot in the body coordinate system.

5. The method for positioning the drilling arm of an anchor drilling robot based on monocular vision according to claim 4, characterized in that: Calculating the pose information of the camera coordinate system in the cooperative target coordinate system based on the original image containing the cooperative target coordinate system; performing external parameter calibration and coordinate system conversion on the pose information of the camera coordinate system in the cooperative target coordinate system to obtain the pose information of the drilling arm of the anchor drilling robot in the body coordinate system, specifically including: Based on the original image containing the cooperative target coordinate system, respectively determining the coordinates of two spatial intersection points in the camera coordinate system and the direction vectors of three spatial straight lines in the camera coordinate system; Determining a rotation matrix of the camera coordinate system in the cooperative target coordinate system according to the coordinates of the two spatial intersection points in the camera coordinate system and the direction vectors of the three spatial straight lines in the camera coordinate system; Determining a translation vector of the camera coordinate system in the cooperative target coordinate system according to a rotation matrix of the camera coordinate system in the cooperative target coordinate system; Determine the pose transformation matrix of the drilling arm of the anchor drilling robot in the body coordinate system according to the translation vector of the camera coordinate system in the cooperative target coordinate system; The pose information of the drilling arm of the anchor drilling robot in the fuselage coordinate system is determined according to the pose transformation matrix of the drilling arm of the anchor drilling robot in the fuselage coordinate system.

6. The method for positioning the drilling arm of an anchor drilling robot based on monocular vision according to claim 5, characterized in that: Determining a rotation matrix of the camera coordinate system in the cooperative target coordinate system according to the coordinates of the two spatial intersection points in the camera coordinate system and the direction vectors of the three spatial lines in the camera coordinate system specifically includes: The first space straight line in the cooperative target coordinate system The unit direction vector is , the second space line The direction vector is , the third space straight line The direction vector is , if we assume that the rotation matrix of the camera coordinate system in the cooperative target coordinate system is , then: ； in, 、 、 Represent three spatial lines in the camera coordinate system 、 、 The unit direction vector of ; Since three spatial straight lines The two are perpendicular, then the rotation matrix of the camera coordinate system in the cooperative target coordinate system is Expressed as: 。 7. The method for positioning the drilling arm of an anchor drilling robot based on monocular vision according to claim 6, characterized in that: Determining a translation vector of the camera coordinate system in the cooperative target coordinate system according to a rotation matrix of the camera coordinate system in the cooperative target coordinate system specifically includes: The first spatial point in the cooperative target coordinate system The coordinates are , assuming that the translation vector of the camera coordinate system in the cooperative target coordinate system is , then the first spatial point Coordinates in the camera coordinate system Expressed as: ； in, Represents the rotation matrix of the camera coordinate system in the cooperative target coordinate system, is a zero vector, then ; The translation vector of the camera coordinate system in the cooperative target coordinate system Expressed as:

8. The method for positioning the drilling arm of an anchor drilling robot based on monocular vision according to claim 7, characterized in that: Determining the pose information of the drilling arm of the anchor drilling robot in the fuselage coordinate system according to the pose transformation matrix of the drilling arm of the anchor drilling robot in the fuselage coordinate system specifically includes: According to the posture transformation matrix of the drilling arm of the anchor drilling robot in the fuselage coordinate system, the rotation matrix and translation matrix of the drilling arm of the anchor drilling robot in the fuselage coordinate system are determined respectively, which are expressed as: ； in, The rotation matrix of the drilling arm of the anchor drilling robot in the fuselage coordinate system represents the posture information of the drilling arm of the anchor drilling robot in the fuselage coordinate system, that is, the rotation angles of the drilling arm of the anchor drilling robot along the three coordinate axes in the fuselage coordinate system; It represents the translation matrix of the drilling arm of the anchor drilling robot in the fuselage coordinate system, and characterizes the position coordinate information of the drilling arm of the anchor drilling robot in the fuselage coordinate system, that is, the movement distance of the drilling arm of the anchor drilling robot along the three coordinate axes in the fuselage coordinate system.

Images