Continuous positioning method for roadheader based on binocular camera and multiple infrared light targets

Abstract

This invention relates to a continuous positioning method for tunneling machines based on a binocular camera and multiple infrared targets, belonging to the technical field. The method includes: during the tunneling process, acquiring the left and right eye coordinates of each infrared target point on a first infrared target; calculating the tunneling machine's pose parameters based on camera parameters and the left and right eye coordinates of each infrared target point; when the first infrared target leaves the optimal observation range of the binocular camera, the tunneling machine stops moving, and the binocular positioning device is slid back into the optimal observation range of the binocular camera, performing reverse calibration on a second infrared target and calculating the new pose parameters of the tunneling machine; the second infrared target is moved, and the binocular camera continues to observe the first target to update the new pose parameters of the tunneling machine, continuously updating the new pose parameters of the tunneling machine to achieve continuous positioning of the tunneling machine. This invention improves the accuracy of continuous positioning of tunneling machines.

Description

Technical Field

[0001] This invention relates to the field of tunneling machine positioning technology, and in particular to a continuous positioning method for tunneling machines based on binocular cameras and multiple infrared light targets. Background Technology

[0002] Tunneling machine positioning is an essential part of coal mine tunneling operations. However, due to the special nature of coal mine tunneling scenarios (rock tunnels) and the complexity of working conditions, currently widely used vehicle positioning and navigation methods such as GPS and Beidou systems cannot be used for tunneling machine positioning and navigation because the strata block electromagnetic signals.

[0003] To address this problem, scholars have proposed various positioning and navigation schemes, such as total station positioning and navigation, UWB positioning and navigation, inertial navigation positioning and navigation, visual odometry positioning and navigation, and combined positioning and navigation methods that integrate two or more of the above devices through data fusion algorithms.

[0004] However, in the traditional vision-based positioning method, the binocular camera must be relocated and repeatedly calibrated as the tunneling machine moves. This repeated calibration process is cumbersome and inconvenient, and it cannot achieve continuous measurement of the tunneling machine's position and posture. Summary of the Invention

[0005] To address the aforementioned technical problems, this invention provides a continuous positioning method for tunneling machines based on a binocular camera and multiple infrared targets. The technical solution of this invention is as follows: A continuous positioning method for tunnel boring machines based on binocular cameras and multiple infrared targets includes: S1, Install a No. 1 infrared target on the tunneling machine, install a No. 2 infrared target on the top of the tunneling roadway, and install a track and a binocular positioning device on the top of the tunneling roadway, so that the binocular positioning device is slidably connected to the track. The No. 1 infrared target and the No. 2 infrared target include at least three non-collinear infrared target points. The binocular positioning device includes a binocular camera, which includes a left eye camera and a right eye camera. S2, construct the left camera coordinate system, right camera coordinate system, camera coordinate system, first optical target coordinate system, second optical target coordinate system, carrier coordinate system and navigation coordinate system; S3, during the tunneling process of the tunneling machine, collects the left eye coordinates of each infrared target point in the No. 1 infrared target in the left camera coordinate system and the right eye coordinates in the right camera coordinate system from behind the tunneling direction and within the best observation range of the binocular camera. S4. Based on the camera parameters of the left and right cameras, the left and right coordinates of each infrared target point, calculate the first rotation matrix and the first translation matrix between the navigation coordinate system and the carrier coordinate system to complete the first positioning of the first infrared target. S5, calculate the orientation parameters of the tunneling machine based on the first rotation matrix and the first translation matrix; S6, obtain the second rotation matrix and the second translation matrix between the navigation coordinate system and the second optical target coordinate system, and perform the first calibration of the second target; S7. When the No. 1 infrared target leaves the optimal observation range of the binocular camera, the tunneling machine stops moving, slides the binocular positioning device into the optimal observation range of the binocular camera, determines the third rotation matrix and the third translation matrix between the No. 2 target coordinate system and the camera coordinate system, and performs reverse calibration of the binocular camera based on the third rotation matrix and the third translation matrix, the second rotation matrix and the second translation matrix, and calculates the fourth rotation matrix and the fourth translation matrix between the navigation coordinate system and the camera coordinate system. S8, perform the second positioning of the No. 1 infrared target to obtain the fifth rotation matrix and the fifth translation matrix between the navigation coordinate system and the carrier coordinate system. Calculate the new pose parameters of the tunneling machine based on the fifth rotation matrix, the fifth translation matrix, the fourth rotation matrix, and the fourth translation matrix. S9 moves the second infrared target and reacquires the sixth rotation matrix and sixth translation matrix between the navigation coordinate system and the second target coordinate system. This updates the new pose parameters of the tunneling machine. During the tunneling process, the binocular positioning device and the second infrared target are continuously moved to update the new pose parameters of the tunneling machine, thus achieving continuous positioning of the tunneling machine.

[0006] Preferably, S4 includes: S41. Based on the camera parameters of the left and right cameras, and the left and right coordinates of each infrared target point in the No. 1 infrared target, calculate the camera coordinates of each infrared target point in the No. 1 infrared target in the camera coordinate system. S42, based on the camera coordinates of each infrared target point in the No. 1 infrared target and the target coordinates of each infrared target point in the No. 1 infrared target in the No. 1 infrared target coordinate system, calculate the seventh rotation matrix and the seventh translation matrix between the camera coordinate system and the No. 1 infrared target coordinate system. S43. Based on the eighth rotation matrix and the eighth translation matrix, and the seventh rotation matrix and the seventh translation matrix between the No. 1 optical target coordinate system and the navigation coordinate system, calculate the first rotation matrix and the first translation matrix between the carrier coordinate system and the navigation coordinate system to complete the first positioning of the No. 1 infrared optical target.

[0007] Preferably, S2 includes: S21, Establish the left camera coordinate system and right camera coordinate system ; where the origin of the left camera coordinate system is The camera optical center of the left eye camera. The axis points to the right side of the left-eye camera. The axis points to the lower left side of the camera. shaft and shaft and The axes form a right-handed coordinate system; the origin of the right camera coordinate system is... The camera optical center of the right eye camera. The axis points to the right side of the right eye camera. The axis points to the lower right side of the camera. shaft and shaft and The axes form a right-handed coordinate system; simultaneously, a camera coordinate system is established. And make the camera coordinate system coincide with the left camera coordinate system; S22, Establish the coordinate system of optical target No. 1 Among them, the origin of the coordinate system of the first optical target Located at the center of the light target of infrared light target No. 1, The axis points to the right side of the first infrared target. The axis points in front of the No. 1 infrared target. shaft and shaft and The axes form a right-handed coordinate system; S23, Establish the coordinate system of the second optical target. Among them, the origin of the coordinate system of the second optical target Located at the center of the light target of infrared target No. 2, The axis points to the right side of the second infrared target. The axis points in front of the second infrared target. shaft and shaft and The axes form a right-handed coordinate system; S24, Establish the carrier coordinate system And make the carrier coordinate system and the first optical target coordinate system coincide; S25, Establish navigation coordinate system The origin of the navigation coordinate system The origin of the camera coordinate system during the first positioning of the infrared target No. 1 coincide, The axis points to the right side of the tunnel. The axis points forward of the tunnel. shaft and shaft and The axes form a right-handed coordinate system.

[0008] Preferably, S41 includes: S411, Based on the left eye coordinates of the target infrared target point in the No. 1 infrared light target and the camera parameters of the left eye camera, construct the mapping relationship between the camera coordinates and the left eye coordinates of the target infrared target point in the No. 1 infrared light target in the camera coordinate system; the target infrared target point is any infrared target point in the No. 1 infrared light target. S412, based on the baseline length between the left and right cameras, the mapping relationship between the camera coordinates and the left camera coordinates of the target infrared target point in the No. 1 infrared light target in the camera coordinate system, and the left and right camera coordinates, calculate the camera coordinates of the target infrared target point in the No. 1 infrared light target in the camera coordinate system.

[0009] Preferably, step S411 is based on the target infrared target point in the first infrared light target. Left eye coordinates Using the camera parameters of the left eye camera, construct the infrared target points of the target in the first infrared target. Camera coordinates in the camera coordinate system With left eye coordinates The mapping relationship is achieved through formula (1): (1); In formula (1), This refers to the focal length in the camera parameters of the left-eye camera. S412 is based on the baseline length between the left and right cameras. The mapping relationship between the camera coordinates and the left eye coordinates of the infrared target point in the No. 1 infrared target system. With right eye coordinates Calculate the camera coordinates of the infrared target point in the camera coordinate system of the first infrared target. This can be achieved through formulas (2) and (3): (2); (3); The left and right cameras have the same focal length in their camera parameters. .

[0010] Preferably, S42 includes: S421, based on the camera coordinates of each infrared target point and the optical target coordinates of each infrared target point in the first optical target coordinate system, construct the objective function between the camera coordinate system and the first optical target coordinate system using formula (4). : (4); In formula (4), This represents the target coordinates of the i-th infrared target point in the first infrared target coordinate system. This represents the camera coordinates of the i-th infrared target point in infrared target number one. N This indicates the number of infrared target points in infrared target number one; and These are the seventh rotation matrix and the seventh translation matrix between the camera coordinate system and the first optical target coordinate system, respectively. S422, Solve the objective function to obtain the seventh rotation matrix between the camera coordinate system and the first optical target coordinate system. With the seventh translation matrix .

[0011] Preferably, step S43 involves using the eighth rotation matrix between the navigation coordinate system and the camera coordinate system. With the eighth translation matrix 7th Rotation Matrix With the seventh translation matrix Calculate the first rotation matrix between the navigation coordinate system and the vehicle coordinate system. With the first translation matrix This is achieved through formula (5): (5); In formula (5), This represents the carrier coordinates of any infrared target point in the No. 1 infrared target in the carrier coordinate system. This represents the navigation coordinates of any infrared target point in the No. 1 infrared target in the navigation coordinate system.

[0012] Preferably, step S5 is performed according to the first rotation matrix. With the first translation matrix Calculate the orientation parameters of the tunnel boring machine This is achieved through formula (6): (6); In formula (6), This represents the pitch angle in the tunnel boring machine's attitude parameters. This represents the roll angle in the tunnel boring machine's attitude parameters. This represents the heading angle in the tunnel boring machine's attitude parameters. This represents the element in the 3rd row and 2nd column of the first rotation matrix. This represents the element in the 3rd row and 1st column of the first rotation matrix. This represents the element in the 3rd row and 3rd column of the first rotation matrix. This represents the element in the 1st row and 2nd column of the first rotation matrix. This represents the element in the 2nd row and 2nd column of the first rotation matrix. Represents the arctangent function. This represents the navigation coordinates of the tunneling machine in the navigation coordinate system. This represents the arcsine function.

[0013] Preferably, S6 includes: S61, obtain the ninth rotation matrix and the ninth translation matrix between the second optical target coordinate system and the camera coordinate system; S62, based on the eighth rotation matrix between the navigation coordinate system and the camera coordinate system. With the eighth translation matrix Ninth rotation matrix With the ninth translation matrix Calculate the second rotation matrix between the navigation coordinate system and the second optical target coordinate system. With the second translation matrix This is achieved through formula (7): (7); In formula (7), and They are inverse matrices. and They are inverse matrices.

[0014] Preferably, S7 is based on the third rotation matrix. With the third translation matrix Second rotation matrix With the second translation matrix Perform reverse calibration on the second infrared target and calculate the fourth rotation matrix between the navigation coordinate system and the camera coordinate system. With the fourth translation matrix When, this is achieved through formula (8): (8).

[0015] All of the above-mentioned optional technical solutions can be combined arbitrarily, and the present invention will not provide a detailed description of the structure after each combination.

[0016] By means of the above solution, the beneficial effects of the present invention are as follows: By installing a first infrared target on the tunneling machine and a second infrared target and binocular positioning device above the tunneling roadway respectively, and constructing coordinate systems for the left camera, right camera, camera, carrier, first target, second target, and navigation, the binocular camera acquires the coordinates of the first infrared target under the camera coordinate system. After the first infrared target is positioned, the pose parameters of the tunneling machine are obtained. The positioning data of the second infrared target is then acquired. Subsequently, the binocular camera moves away from the first infrared target. When the infrared target is at its optimal observation range, the binocular positioning device is moved. Based on the positioning data of the second infrared target, the second infrared target is reverse-calibrated to obtain the new pose parameters of the tunnel boring machine. Then, the second infrared target is moved. As the tunnel boring machine continues to tunnel, the above operation is repeated. By positioning the first infrared target and calibrating the second infrared target, continuous and uninterrupted positioning of the tunnel boring machine can be achieved without the need for manual recalibration. This simplifies the process of repeated calibration in traditional methods and improves the accuracy of continuous positioning of the tunnel boring machine.

[0017] The above description is merely an overview of the technical solution of the present invention. In order to better understand the technical means of the present invention and to implement it in accordance with the contents of the specification, the preferred embodiments of the present invention are described in detail below with reference to the accompanying drawings. Attached Figure Description

[0018] Figure 1 This is a flowchart of the continuous positioning method for tunneling machines based on binocular cameras and multiple infrared targets provided by the present invention.

[0019] Figure 2 This is a schematic diagram showing the installation positions of the tunneling machine, binocular positioning device, infrared target No. 1, and infrared target No. 2 in an embodiment of the present invention.

[0020] Figure 3 This is a schematic diagram of an infrared target, an infrared target point, and the coordinate system of the infrared target in an embodiment of the present invention.

[0021] Figure 4 This is a schematic diagram illustrating the binocular vision positioning principle of the target infrared target point in the camera coordinate system, the left camera coordinate system, and the right camera coordinate system in an embodiment of the present invention. Detailed Implementation

[0022] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.

[0023] like Figure 1 As shown, this embodiment of the invention provides a continuous positioning method for tunneling machines based on a binocular camera and multiple infrared targets, including: S1, install a first infrared target on the tunneling machine, install a second infrared target on the top of the tunneling roadway, and install a track and a binocular positioning device on the top of the tunneling roadway, so that the binocular positioning device is slidably connected to the track. The first and second infrared targets include at least three non-collinear infrared target points. The binocular positioning device includes a binocular camera, which includes a left eye camera and a right eye camera.

[0024] Specifically, the binocular positioning device is suspended on a rail behind the tunneling roadway. A binocular camera is installed inside, which observes a primary infrared target fixed to the tunneling machine in front of it, thus performing positioning and attitude calibration of the tunneling machine. A secondary infrared target is positioned above the tunneling roadway in front of the binocular positioning device, while the primary infrared target is mounted on the tunneling machine body; this secondary target is used to calculate the tunneling machine's attitude. Figure 2 As shown. The binocular positioning device can slide back and forth along the track to ensure that the first infrared target is within the optimal observation range of the binocular camera.

[0025] The infrared light-emitting points on the infrared target are all composed of infrared LEDs. The number of infrared light-emitting points in the infrared target is not specifically limited in this embodiment; in practice, the number can be set to 3-8. When there are 3 infrared light-emitting points in the infrared target, the line connecting the 3 points forms a triangle. When there are 4 infrared light-emitting points in the infrared target, the line connecting the 4 points forms a rectangle. When there are 5 infrared light-emitting points in the infrared target, the line connecting 4 points forms a rectangle, and the 5th infrared light-emitting point is located at the center of one side of the rectangle. When there are 6 infrared light-emitting points in the infrared target, the line connecting 4 points forms a rectangle, and the other 2 points are located at the center of two sides of the rectangle. When there are 7 infrared light-emitting points in the infrared target, the line connecting 4 points forms a rectangle, and the other 3 points are located at the center of three sides of the rectangle.

[0026] In this embodiment of the invention, the preferred number of infrared emitting points is 8. A schematic diagram of the arrangement of the 8 infrared emitting points in the first infrared target and the coordinate system of the first target is shown below. Figure 3 As shown. The same applies to infrared target number two.

[0027] S2, construct the left camera coordinate system, right camera coordinate system, camera coordinate system, first optical target coordinate system, second optical target coordinate system, carrier coordinate system and navigation coordinate system.

[0028] In one specific embodiment, S2 includes: S21, Establish the left camera coordinate system and right camera coordinate system ; where the origin of the left camera coordinate system is The camera optical center of the left eye camera. The axis points to the right side of the left-eye camera. The axis points to the lower left side of the camera. shaft and shaft and The axes form a right-handed coordinate system; the origin of the right camera coordinate system is... The camera optical center of the right eye camera. The axis points to the right side of the right eye camera. The axis points to the lower right side of the camera. shaft and shaft and The axes form a right-handed coordinate system; simultaneously, a camera coordinate system is established. And make the camera coordinate system coincide with the left camera coordinate system; S22, Establish the coordinate system of optical target No. 1 Among them, the origin of the coordinate system of the first optical target Located at the center of the light target of infrared light target No. 1, The axis points to the right side of the first infrared target. The axis points in front of the No. 1 infrared target. shaft and shaft and The axes form a right-handed coordinate system; S23, Establish the coordinate system of the second optical target. Among them, the origin of the coordinate system of the second optical target Located at the center of the light target of infrared target No. 2, The axis points to the right side of the second infrared target. The axis points in front of the second infrared target. shaft and shaft and The axes form a right-handed coordinate system; S24, Establish the carrier coordinate system And make the carrier coordinate system and the first optical target coordinate system coincide; S25, Establish navigation coordinate system The origin of the navigation coordinate system The origin of the camera coordinate system during the first positioning of the infrared target No. 1 coincide, The axis points to the right side of the tunnel. The axis points forward of the tunnel. shaft and shaft and The axes form a right-handed coordinate system.

[0029] S3, during the tunneling process of the tunneling machine, collects the left eye coordinates of each infrared target point in the No. 1 infrared target in the left camera coordinate system and the right eye coordinates in the right camera coordinate system from behind the tunneling direction and within the optimal observation range of the binocular camera.

[0030] Specifically, the optimal observation range is the preset acquisition distance range of the binocular camera, which is generally 4-12m.

[0031] S4. Based on the camera parameters of the left and right cameras, and the left and right coordinates of each infrared target point, calculate the first rotation matrix and the first translation matrix between the navigation coordinate system and the carrier coordinate system to complete the first positioning of the first infrared target.

[0032] Specifically, the camera parameters for the left and right cameras include focal length and baseline length between them. The baseline length between the left and right cameras is a fixed value and remains unchanged during the binocular camera relocation process.

[0033] In one specific embodiment, S4 includes: S41, based on the camera parameters of the left and right cameras, and the left and right coordinates of each infrared target point in the first infrared target, calculate the camera coordinates of each infrared target point in the camera coordinate system.

[0034] In one specific embodiment, S41 includes: S411, based on the left eye coordinates of the target infrared target point in the No. 1 infrared light target and the camera parameters of the left eye camera, construct the mapping relationship between the camera coordinates and the left eye coordinates of the target infrared target point in the No. 1 infrared light target in the camera coordinate system; the target infrared target point is any infrared target point in the No. 1 infrared light target.

[0035] Specifically, based on the principle of binocular vision positioning, the left and right imaging images of the infrared target are acquired by the left and right cameras, respectively. Then, the imaging coordinates of the target infrared point in the left imaging coordinate system of the left image and the right imaging coordinate system of the right image are determined, serving as the left and right eye coordinates of the target infrared target point. The origin of the left imaging image coordinate system is the upper left corner of the left imaging image, the x-axis points to the right of the left imaging image, and the y-axis points to the bottom of the left imaging image. The construction method for the right imaging image coordinate system is similar.

[0036] In one specific embodiment, S411 is based on the target infrared target point in the first infrared light target. Left eye coordinates Using the camera parameters of the left eye camera, construct the infrared target points of the target in the first infrared target. Camera coordinates in the camera coordinate system With left eye coordinates The mapping relationship is achieved through formula (1): (1); In formula (1), This refers to the focal length in the camera parameters of the left-eye camera.

[0037] Specifically, the mapping relationship in formula (1) is determined based on the coincidence of the camera coordinate system and the left camera coordinate system. Target infrared target point The principle of binocular vision positioning in the camera coordinate system, left camera coordinate system, and right camera coordinate system is as follows: Figure 4 As shown.

[0038] S412, based on the baseline length between the left and right cameras, the mapping relationship between the camera coordinates and the left camera coordinates of the target infrared target point in the No. 1 infrared light target in the camera coordinate system, and the left and right camera coordinates, calculate the camera coordinates of the target infrared target point in the No. 1 infrared light target in the camera coordinate system.

[0039] Specifically, due to the existence of the camera baseline, the imaging positions of the same infrared target point in space are different for the left and right cameras. The two images formed by the same infrared target point are on the same straight line, that is, they only differ on the horizontal axis. Based on this characteristic, the relationship in formula (2) can be obtained. Then, S412 is based on the baseline length between the left and right cameras. The mapping relationship between the camera coordinates and the left eye coordinates of the infrared target point in the No. 1 infrared target system. With right eye coordinates Calculate the camera coordinates of the infrared target point in the camera coordinate system of the first infrared target. This can be achieved through formulas (2) and (3): (2); (3); The left and right cameras have the same focal length in their camera parameters. .

[0040] Specifically, formula (3) is derived from formulas (1) and (2). The camera coordinates of other infrared target points of infrared target No. 1 can be obtained from formulas (1) to (3).

[0041] S42, based on the camera coordinates of each infrared target point in the No. 1 infrared target and the target coordinates of each infrared target point in the No. 1 infrared target in the No. 1 infrared target coordinate system, calculate the seventh rotation matrix and the seventh translation matrix between the camera coordinate system and the No. 1 infrared target coordinate system.

[0042] In one specific embodiment, S42 includes: S421, based on the camera coordinates of each infrared target point and the optical target coordinates of each infrared target point in the first optical target coordinate system, construct the objective function between the camera coordinate system and the first optical target coordinate system using formula (4). : (4); In formula (4), This represents the target coordinates of the i-th infrared target point in the first infrared target coordinate system. This represents the camera coordinates of the i-th infrared target point in infrared target number one. N This indicates the number of infrared target points in infrared target number one; and These are the seventh rotation matrix and the seventh translation matrix between the camera coordinate system and the first optical target coordinate system, respectively. S422, Solve the objective function to obtain the seventh rotation matrix between the camera coordinate system and the first optical target coordinate system. With the seventh translation matrix .

[0043] S43. Based on the eighth rotation matrix and the eighth translation matrix, and the seventh rotation matrix and the seventh translation matrix between the No. 1 optical target coordinate system and the navigation coordinate system, calculate the first rotation matrix and the first translation matrix between the carrier coordinate system and the navigation coordinate system to complete the first positioning of the No. 1 infrared optical target.

[0044] In one specific embodiment, S43 calculates the rotation matrix between the navigation coordinate system and the camera coordinate system according to the eighth rotation matrix. With the eighth translation matrix 7th Rotation Matrix With the seventh translation matrix Calculate the first rotation matrix between the navigation coordinate system and the vehicle coordinate system. With the first translation matrix This is achieved through formula (5): (5); In formula (5), This represents the carrier coordinates of any infrared target point in the No. 1 infrared target in the carrier coordinate system. This represents the navigation coordinates of any infrared target point in the No. 1 infrared target in the navigation coordinate system.

[0045] Specifically, the rotation matrix between the camera coordinate system and the navigation coordinate system. With translation matrix Since the coordinates of the carrier and the coordinates of the first optical target are known, therefore... .

[0046] S5. Calculate the orientation parameters of the tunneling machine based on the first rotation matrix and the first translation matrix.

[0047] In one specific embodiment, S5 is based on the first rotation matrix. With the first translation matrix Calculate the orientation parameters of the tunnel boring machine This is achieved through formula (6): (6); In formula (6), This represents the pitch angle in the tunnel boring machine's attitude parameters. This represents the roll angle in the tunnel boring machine's attitude parameters. This represents the heading angle in the tunnel boring machine's attitude parameters. This represents the element in the 3rd row and 2nd column of the first rotation matrix. This represents the element in the 3rd row and 1st column of the first rotation matrix. This represents the element in the 3rd row and 3rd column of the first rotation matrix. This represents the element in the 1st row and 2nd column of the first rotation matrix. This represents the element in the 2nd row and 2nd column of the first rotation matrix. Represents the arctangent function. This represents the navigation coordinates of the tunneling machine in the navigation coordinate system. This represents the arcsine function.

[0048] S6: Obtain the second rotation matrix and the second translation matrix between the navigation coordinate system and the second optical target coordinate system, and perform the first calibration of the second target.

[0049] In one specific embodiment, S6 includes: S61, obtain the ninth rotation matrix and the ninth translation matrix between the second optical target coordinate system and the camera coordinate system.

[0050] Specifically, based on the principles of formulas (1) to (4), the ninth rotation matrix and the ninth translation matrix between the second optical target coordinate system and the camera coordinate system can be obtained similarly.

[0051] S62, based on the eighth rotation matrix between the navigation coordinate system and the camera coordinate system. With the eighth translation matrix Ninth rotation matrix With the ninth translation matrix Calculate the second rotation matrix between the navigation coordinate system and the second optical target coordinate system. With the second translation matrix This is achieved through formula (7): (7); In formula (7), and They are inverse matrices. and They are inverse matrices.

[0052] S7. When the first infrared target leaves the optimal observation range of the binocular camera, the tunneling machine stops moving, slides the binocular positioning device into the optimal observation range of the binocular camera, determines the third rotation matrix and the third translation matrix between the coordinate system of the second target and the camera coordinate system, and performs reverse calibration of the binocular camera based on the third rotation matrix and the third translation matrix, the second rotation matrix and the second translation matrix, and calculates the fourth rotation matrix and the fourth translation matrix between the navigation coordinate system and the camera coordinate system.

[0053] In one specific embodiment, S7 is based on the third rotation matrix. With the third translation matrix Second rotation matrix With the second translation matrix Perform reverse calibration on the second infrared target and calculate the fourth rotation matrix between the navigation coordinate system and the camera coordinate system. With the fourth translation matrix When, this is achieved through formula (8): (8).

[0054] S8. Perform a second positioning of the first infrared target to obtain the fifth rotation matrix and the fifth translation matrix between the navigation coordinate system and the carrier coordinate system. Calculate the new pose parameters of the tunneling machine based on the fifth rotation matrix, the fifth translation matrix, the fourth rotation matrix, and the fourth translation matrix.

[0055] Specifically, the tenth rotation matrix and the tenth translation matrix between the navigation coordinate system and the carrier coordinate system are calculated based on the fifth rotation matrix and the fifth translation matrix, the fourth rotation matrix and the fourth translation matrix. Based on the principle of formula (6), the new posture parameters of the tunneling machine are calculated based on the tenth rotation matrix and the tenth translation matrix.

[0056] S9 moves the second infrared target and reacquires the sixth rotation matrix and sixth translation matrix between the navigation coordinate system and the second target coordinate system. This updates the new pose parameters of the tunneling machine. During the tunneling process, the binocular positioning device and the second infrared target are continuously moved to update the new pose parameters of the tunneling machine, thus achieving continuous positioning of the tunneling machine.

[0057] Based on the visual perception principle of binocular cameras, this invention achieves the positioning of tunneling machines and the self-calibration of binocular positioning devices by perceiving infrared light targets No. 1 and No. 2 through binocular cameras. This enables continuous position and posture perception of tunneling machines over long distances without the need for repeated manual calibration.

[0058] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the technical principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A continuous positioning method for a roadheader based on a binocular camera and a multi-infrared light target, characterized in that, include: S1, Install a No. 1 infrared target on the tunneling machine, install a No. 2 infrared target on the top of the tunneling roadway, and install a track and a binocular positioning device on the top of the tunneling roadway, so that the binocular positioning device is slidably connected to the track. The No. 1 infrared target and the No. 2 infrared target include at least three non-collinear infrared target points. The binocular positioning device includes a binocular camera, which includes a left eye camera and a right eye camera. S2, construct the left camera coordinate system, right camera coordinate system, camera coordinate system, first optical target coordinate system, second optical target coordinate system, carrier coordinate system and navigation coordinate system; S3, during the tunneling process of the tunneling machine, collects the left eye coordinates of each infrared target point in the No. 1 infrared target in the left camera coordinate system and the right eye coordinates in the right camera coordinate system from behind the tunneling direction and within the best observation range of the binocular camera. S4. Based on the camera parameters of the left and right cameras, the left and right coordinates of each infrared target point, calculate the first rotation matrix and the first translation matrix between the navigation coordinate system and the carrier coordinate system to complete the first positioning of the first infrared target. S5, calculate the orientation parameters of the tunneling machine based on the first rotation matrix and the first translation matrix; S6, obtain the second rotation matrix and the second translation matrix between the navigation coordinate system and the second optical target coordinate system, and perform the first calibration of the second target; S7. When the No. 1 infrared target leaves the optimal observation range of the binocular camera, the tunneling machine stops moving, slides the binocular positioning device into the optimal observation range of the binocular camera, determines the third rotation matrix and the third translation matrix between the No. 2 target coordinate system and the camera coordinate system, and performs reverse calibration of the binocular camera based on the third rotation matrix and the third translation matrix, the second rotation matrix and the second translation matrix, and calculates the fourth rotation matrix and the fourth translation matrix between the navigation coordinate system and the camera coordinate system. S8, perform the second positioning of the No. 1 infrared target to obtain the fifth rotation matrix and the fifth translation matrix between the navigation coordinate system and the carrier coordinate system. Calculate the new pose parameters of the tunneling machine based on the fifth rotation matrix, the fifth translation matrix, the fourth rotation matrix, and the fourth translation matrix. S9 moves the second infrared target and reacquires the sixth rotation matrix and sixth translation matrix between the navigation coordinate system and the second target coordinate system. This updates the new pose parameters of the tunneling machine. During the tunneling process, the binocular positioning device and the second infrared target are continuously moved to update the new pose parameters of the tunneling machine, thus achieving continuous positioning of the tunneling machine.

2. The method according to claim 1, wherein, S4 includes: S41. Based on the camera parameters of the left and right cameras, and the left and right coordinates of each infrared target point in the No. 1 infrared target, calculate the camera coordinates of each infrared target point in the No. 1 infrared target in the camera coordinate system. S42, based on the camera coordinates of each infrared target point in the No. 1 infrared target and the target coordinates of each infrared target point in the No. 1 infrared target in the No. 1 infrared target coordinate system, calculate the seventh rotation matrix and the seventh translation matrix between the camera coordinate system and the No. 1 infrared target coordinate system. S43. Based on the eighth rotation matrix and the eighth translation matrix, and the seventh rotation matrix and the seventh translation matrix between the No. 1 optical target coordinate system and the navigation coordinate system, calculate the first rotation matrix and the first translation matrix between the carrier coordinate system and the navigation coordinate system to complete the first positioning of the No. 1 infrared optical target.

3. The continuous positioning method of the roadheader based on the binocular camera and the multi-infrared light target according to claim 1, characterized in that, S2 includes: S21, Establish the left camera coordinate system and right camera coordinate system ; where the origin of the left camera coordinate system is The camera optical center of the left eye camera. The axis points to the right side of the left-eye camera. The axis points to the lower left side of the camera. shaft and shaft and The axes form a right-handed coordinate system; the origin of the right camera coordinate system is... The camera optical center of the right eye camera. The axis points to the right side of the right eye camera. The axis points to the lower side of the right eye camera. shaft and shaft and The axes form a right-handed coordinate system; simultaneously, a camera coordinate system is established. And make the camera coordinate system coincide with the left camera coordinate system; S22, Establish the coordinate system of optical target No. 1 Among them, the origin of the coordinate system of the first optical target Located at the center of the light target of infrared light target No. 1, The axis points to the right side of the first infrared target. The axis points in front of the No. 1 infrared target. shaft and shaft and The axes form a right-handed coordinate system; S23, Establish the coordinate system of the second optical target. Among them, the origin of the coordinate system of the second optical target Located at the center of the light target of infrared target No. 2, The axis points to the right side of the second infrared target. The axis points in front of the second infrared target. shaft and shaft and The axes form a right-handed coordinate system; S24, establish the carrier coordinate system and make the carrier coordinate system coincide with the first light target coordinate system coincide; S25, Establish navigation coordinate system The origin of the navigation coordinate system The origin of the camera coordinate system during the first positioning of the infrared target No. 1 coincide, The axis points to the right side of the tunnel. The axis points forward of the tunnel. shaft and shaft and The axes form a right-handed coordinate system.

4. The method of claim 2, wherein, S41 includes: S411, Based on the left eye coordinates of the target infrared target point in the No. 1 infrared light target and the camera parameters of the left eye camera, construct the mapping relationship between the camera coordinates and the left eye coordinates of the target infrared target point in the No. 1 infrared light target in the camera coordinate system; the target infrared target point is any infrared target point in the No. 1 infrared light target. S412, based on the baseline length between the left and right cameras, the mapping relationship between the camera coordinates and the left camera coordinates of the target infrared target point in the No. 1 infrared light target in the camera coordinate system, and the left and right camera coordinates, calculate the camera coordinates of the target infrared target point in the No. 1 infrared light target in the camera coordinate system.

5. The continuous positioning method of the tunneling machine based on binocular vision autonomous calibration according to claim 4, characterized in that, S411 is based on the target infrared target point in infrared target No.

1. Left eye coordinates Using the camera parameters of the left eye camera, construct the infrared target points of the target in the first infrared target. Camera coordinates in the camera coordinate system With left eye coordinates The mapping relationship is achieved through formula (1): （1）； In equation (1), denotes the focal length in the camera parameters of the left-eye camera; S412 is based on the baseline length between the left and right cameras. The mapping relationship between the camera coordinates and the left eye coordinates of the infrared target point in the No. 1 infrared target system. With right eye coordinates Calculate the camera coordinates of the infrared target point in the camera coordinate system of the first infrared target. This can be achieved through formulas (2) and (3): （2）； （3）； Wherein, the focal length in the camera parameters of the left eye camera and the right eye camera is same, and is .

6. The continuous positioning method for tunneling machines based on binocular cameras and multiple infrared targets according to claim 2, characterized in that, S42 includes: S421, according to the camera coordinates of each infrared target point and the light target coordinates of each infrared target point in the first light target coordinate system, a target function between the camera coordinate system and the first light target coordinate system is constructed through formula (4) : （4）； In formula (4), This represents the target coordinates of the i-th infrared target point in the first infrared target coordinate system. This represents the camera coordinates of the i-th infrared target point in infrared target number one. N This indicates the number of infrared target points in infrared target number one; and These are the seventh rotation matrix and the seventh translation matrix between the camera coordinate system and the first optical target coordinate system, respectively; S422, solving the objective function to obtain a seventh rotation matrix between the camera coordinate system and the first light target coordinate system and the seventh translation matrix .

7. The method of continuous positioning of a roadheader according to claim 2, characterized in that, S43 is based on the eighth rotation matrix between the navigation coordinate system and the camera coordinate system. With the eighth translation matrix 7th Rotation Matrix With the seventh translation matrix Calculate the first rotation matrix between the navigation coordinate system and the vehicle coordinate system. With the first translation matrix This is achieved through formula (5): （5）； In formula (5), represents the carrier coordinates of any infrared target point in the first infrared light target under the carrier coordinate system, represents the navigation coordinates of any infrared target point in the first infrared light target under the navigation coordinate system.

8. The continuous positioning method for tunneling machines based on binocular cameras and multiple infrared targets according to claim 1, characterized in that, The S5 is calculated according to a first rotation matrix with a first translation matrix calculating the pose parameters of the roadheader by formula (6): （6）； In formula (6), This represents the pitch angle in the tunnel boring machine's attitude parameters. This represents the roll angle in the tunnel boring machine's attitude parameters. This represents the heading angle in the tunnel boring machine's attitude parameters. This represents the element in the 3rd row and 2nd column of the first rotation matrix. This represents the element in the 3rd row and 1st column of the first rotation matrix. This represents the element in the 3rd row and 3rd column of the first rotation matrix. This represents the element in the 1st row and 2nd column of the first rotation matrix. This represents the element in the 2nd row and 2nd column of the first rotation matrix. Represents the arctangent function. This represents the navigation coordinates of the tunneling machine in the navigation coordinate system. This represents the arcsine function.

9. The method of continuous positioning of a heading machine based on a binocular camera and a multi-infrared light target according to claim 1, characterized in that, S6 includes: S61, obtain the ninth rotation matrix and the ninth translation matrix between the second optical target coordinate system and the camera coordinate system; S62, based on the eighth rotation matrix between the navigation coordinate system and the camera coordinate system. With the eighth translation matrix Ninth rotation matrix With the ninth translation matrix Calculate the second rotation matrix between the navigation coordinate system and the second optical target coordinate system. With the second translation matrix When, this is achieved through formula (7): （7）； In equation (7), and are inverse matrices of each other, and are inverse matrices of each other.

10. The method of continuous positioning of a heading machine based on a binocular camera and a multi-infrared light target according to claim 1, characterized in that, S7 is based on the third rotation matrix With the third translation matrix Second rotation matrix With the second translation matrix Perform reverse calibration on the second infrared target and calculate the fourth rotation matrix between the navigation coordinate system and the camera coordinate system. With the fourth translation matrix When, this is achieved through formula (8): （8）。

Images