A tunneling machine tracking and positioning method based on multi-sensor fusion

Abstract

The present application relates to a kind of based on multi-sensor fusion's heading machine tracking positioning method, belong to heading machine positioning technical field.The method includes: fixedly installing total station in the roadway top behind the heading direction of heading machine, suspending tracking positioning robot in the guide rail above roadway, carrying binocular camera, strapdown inertial navigation system on tracking positioning robot, installing infrared target plane on the body of heading machine;Control tracking positioning robot moves to the best observation distance interval of binocular camera, and obtains the target image of infrared target plane collected by binocular camera;According to target image, calculate camera-carrier rotation matrix and camera-carrier translation matrix;Obtain the attitude of tracking positioning robot relative to navigation coordinate system;According to camera-carrier rotation matrix, camera-carrier translation matrix, the attitude of tracking positioning robot relative to navigation coordinate system and coordinate system conversion relationship determine the pose of heading machine.The present application can continuously track and locate heading machine.

Description

Technical Field

[0001] This invention relates to the field of tunneling machine positioning technology, and in particular to a tunneling machine tracking and positioning method based on multi-sensor fusion. Background Technology

[0002] Coal mine tunneling is a crucial step in coal mining. However, due to the unique nature of the working environment (rock tunnels) and the complexity of the conditions, widely used vehicle positioning methods such as GPS and BeiDou systems cannot be used for tracking and positioning tunneling machines because the geological formation blocks electromagnetic signals. To address this issue, researchers have proposed various tracking and positioning methods, such as total station tracking and positioning, UWB tracking and positioning, inertial navigation tracking and positioning, and visual odometry tracking and positioning.

[0003] However, existing visual odometry tracking and positioning methods for tunneling machines based on vision devices are only suitable for positioning the tunneling machine over a limited distance. After the tunnel has been excavated to a certain distance, the vision device needs to be moved and recalibrated. This repeated relocation and calibration process is cumbersome, inconvenient to operate, and cannot achieve continuous tracking and positioning of the tunneling machine. Summary of the Invention

[0004] To solve the above-mentioned technical problems, this invention provides a tunneling machine tracking and positioning method based on multi-sensor fusion. The technical solution of this invention is as follows: A tunneling machine tracking and positioning method based on multi-sensor fusion, comprising: S1, fixing a total station on the top of the roadway behind the tunneling machine in the tunneling direction; arranging a guide rail above the roadway; suspending a tracking and positioning robot in the guide rail and positioning the tracking and positioning robot between the tunneling machine and the total station; mounting a binocular camera, a strapdown inertial navigation system, and a prism on the tracking and positioning robot in a left-right distributed configuration; and installing an infrared target plane on the tunneling machine body, the infrared target plane including at least four non-collinear infrared target points; S2, constructing a carrier coordinate system, a camera coordinate system, a robot coordinate system, a total station coordinate system, and a navigation coordinate system; S3, controlling the tracking... The positioning robot moves to the optimal observation distance range of the binocular camera and acquires the target image of the infrared target plane captured by the binocular camera within its optimal observation distance range; S4, the camera-carrier rotation matrix and camera-carrier translation matrix between the camera coordinate system and the carrier coordinate system are calculated based on the target image; S5, the attitude of the tracking and positioning robot relative to the navigation coordinate system is obtained based on the strapdown inertial navigation system; S6, the pose of the tunneling machine is determined based on the camera-carrier rotation matrix, the camera-carrier translation matrix, the attitude of the tracking and positioning robot relative to the navigation coordinate system, and the coordinate system transformation relationship between the carrier coordinate system, the camera coordinate system, the robot coordinate system, the total station coordinate system, and the navigation coordinate system.

[0005] Preferably, S2 includes: S21, determining the carrier coordinate system based on the position of the infrared target plane. The origin of the carrier coordinate system Located at the center point of the infrared target plane, The axis points to the right. The axis points forward. The first axis, together with the other two axes, forms a right-handed coordinate system pointing upwards; S22, establish the left camera coordinate system for the stereo camera. With right camera coordinate system The origin of the left camera coordinate system. Located at the optical center of its camera, The axis points to the right. The axis points downwards. The right-hand coordinate system is formed by the right axis and the other two axes; the origin of the right camera coordinate system is... Located at the optical center of its camera, The axis points to the right. The axis points downwards. The first axis, together with the other two axes, forms a right-handed coordinate system; simultaneously, a camera coordinate system is established. And make it coincide with the left camera coordinate system; S23, establish the robot coordinate system The origin of the robot coordinate system. Located at the center of the prism, the three axes of the robot coordinate system are parallel to the three axes of the camera coordinate system; S24, determine the total station coordinate system based on the total station position. The origin of the total station coordinate system. Located at the measurement center of the total station, The axis points to the right. The axis points in the forward-looking direction. The S25 axis, together with the other two axes, forms a right-handed coordinate system; S25, the navigation coordinate system is determined based on the total station position. The origin of the navigation coordinate system Coinciding with the origin of the total station coordinate system, The axis points eastward. Axis northward, The x-axis, together with the other two axes, forms a right-handed coordinate system pointing upwards.

[0006] Preferably, S4 includes: S41, obtaining the carrier coordinates of each infrared target point in the infrared target plane in the carrier coordinate system; S42, calculating the camera coordinates of each infrared target point in the infrared target plane in the camera coordinate system; S43, constructing the transformation relationship function between the camera coordinate system and the carrier coordinate system; S44, determining the target function based on the carrier coordinates and camera coordinates of each infrared target point and the transformation relationship function between the camera coordinate system and the carrier coordinate system, and solving the target function to obtain the camera-carrier rotation matrix and the camera-carrier translation matrix between the camera coordinate system and the carrier coordinate system.

[0007] Preferably, S42 includes: S421, determining the depth of each infrared target point in the camera coordinate system based on the principle of triangle similarity in binocular camera imaging; S422, determining the relationship between the left camera coordinate system and the right camera coordinate system; S423, determining the camera coordinates of each infrared target point in the camera coordinate system based on the depth of each infrared target point in the camera coordinate system, the relationship between the left camera coordinate system and the camera coordinate system, and the relationship between the left camera coordinate system and the right camera coordinate system.

[0008] Preferably, in S421, the principle of triangle similarity in binocular camera imaging is expressed by formula (2): (2); In formula (2), f represents the focal length of the left camera; Represents the camera coordinates of the i-th infrared target point Axis coordinates; Represents the camera coordinates of the i-th infrared target point Axis coordinates; Represents the camera coordinates of the i-th infrared target point The axis coordinates are the same as depth; This indicates the position of the infrared target imaging point corresponding to the i-th infrared target point in the left camera coordinate system. Axis coordinates; This indicates the position of the infrared target imaging point corresponding to the i-th infrared target point in the left camera coordinate system. Axis coordinates; In S422, the relationship between the left camera coordinate system and the right camera coordinate system is determined by formula (3): (3); In formula (3), b is the baseline length of the binocular camera; This indicates the position of the infrared target imaging point corresponding to the i-th infrared target point in the right camera coordinate system. Axis coordinates; In S423, the camera coordinates of the i-th infrared target point in the camera coordinate system are determined by formula (4): (4).

[0009] Preferably, in S43, the transformation function between the camera coordinate system and the carrier coordinate system is expressed by formula (5): (5); in formula (5), This represents the camera coordinates of the i-th infrared target point in the camera coordinate system; This represents the carrier coordinates of the i-th infrared target point in carrier coordinates; Represents the camera-carrier rotation matrix; The camera-carrier translation matrix is ​​represented in S44. The target function is determined using formula (6) based on the carrier coordinates and camera coordinates of each infrared target point, as well as the transformation function between the camera coordinate system and the carrier coordinate system. (6); In formula (6), a represents the number of infrared target points in the infrared target plane.

[0010] Preferably, S5 includes: S51, acquiring the attitude angles of the tracking and positioning robot collected by the strapdown inertial navigation system; S52, determining the attitude of the tracking and positioning robot relative to the navigation coordinate system based on the attitude angles of the tracking and positioning robot.

[0011] Preferably, S6 includes: S61, solving the robot-navigation rotation matrix between the robot coordinate system and the navigation coordinate system based on the robot's attitude relative to the navigation coordinate system; S62, acquiring the three-dimensional coordinates of the tracking and positioning robot observed by the total station, determining the total station-navigation rotation matrix based on the pre-calibrated relationship between the total station coordinate system and the navigation coordinate system, and determining the robot-total station rotation matrix and robot-total station translation matrix between the robot coordinate system and the total station coordinate system based on the three-dimensional coordinates of the tracking and positioning robot, the robot-navigation rotation matrix, and the total station-navigation rotation matrix; S6 3. Determine the camera-robot translation matrix based on the pre-calibrated relationship between the robot coordinate system and the camera coordinate system. Then, calculate the carrier-navigation rotation matrix and carrier-navigation translation matrix between the carrier coordinate system and the navigation coordinate system based on the camera-robot translation matrix, the total station-navigation rotation matrix, the camera-carrier rotation matrix, and the camera-carrier translation matrix. S64. Determine the tunneling machine's attitude angle and navigation coordinates in the navigation coordinate system based on the carrier-navigation rotation matrix and the carrier-navigation translation matrix. Finally, combine the tunneling machine's attitude angle and navigation coordinates in the navigation coordinate system to obtain the tunneling machine's pose.

[0012] Preferably, in S61, the robot-navigation rotation matrix is ​​solved using formula (7) according to the orientation of the tracking and positioning robot relative to the navigation coordinate system in the ZXY order. : (7); in formula (7), This indicates the pitch angle of the tracking and positioning robot relative to the navigation coordinate system; This indicates the roll angle of the tracking and positioning robot relative to the navigation coordinate system; This represents the heading angle of the tracking and positioning robot relative to the navigation coordinate system; in S62, based on the three-dimensional coordinates of the tracking and positioning robot... Robot-Navigation Rotation Matrix Total station - navigation rotating matrix The robot-total station rotation matrix is ​​determined using formula (8). With robot-total station translation matrix : (8); in formula (8), Representing the robot-navigation rotation matrix The transpose of S63; in S63, the carrier-navigation rotation matrix is ​​calculated using formula (9). With the carrier-navigation translation matrix : (9); in formula (9), This represents the navigation coordinates of the i-th infrared target point in the navigation coordinate system; This represents the carrier coordinates of the i-th infrared target point in the carrier coordinate system; Represents the camera-carrier translation matrix; Represents the camera-carrier rotation matrix; Represents the camera-robot translation matrix; Represents total station-navigation rotation matrix The transpose of S64; In S64, the attitude angle of the tunneling machine and the navigation coordinates of the tunneling machine in the navigation coordinate system are combined to obtain the position and posture of the tunneling machine, which is expressed by formula (1): (1); in formula (1), This indicates the pitch angle of the tunneling machine in the navigation coordinate system; This indicates the roll angle of the tunneling machine in the navigation coordinate system; The heading angle of the tunneling machine in the navigation coordinate system is represented by ; P represents the coordinates of the tunneling machine in the navigation coordinate system. Representation of the carrier-navigation rotation matrix The element in the e-th row and j-th column.

[0013] Preferably, the infrared target plane includes eight non-collinear infrared target points, four of which form the four vertices of a rectangle, and the other four are located at the midpoints of the four sides of the rectangle.

[0014] 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.

[0015] The beneficial effects of this invention, achieved through the above scheme, are as follows: By arranging guide rails above the tunnel and suspending the tracking and positioning robot within them, the robot moves along the guide rails during the tunneling machine's excavation process. Based on the target image of the infrared target plane acquired by the binocular camera mounted on the tracking and positioning robot, and the relationship between the infrared target plane, the binocular camera, the strapdown inertial navigation system, and the total station, continuous tracking and positioning of the tunneling machine can be achieved. Compared to traditional vision-based tunneling machine tracking and positioning methods, this eliminates the need for re-stationing and calibration, simplifies operation, and improves the efficiency of continuous tracking and positioning of the tunneling machine. By acquiring the target image of the infrared target plane acquired by the binocular camera within its optimal observation distance range, ensuring clear target images, the tunneling machine pose determined based on the target image is more accurate, thereby significantly improving positioning accuracy.

[0016] 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

[0017] Figure 1 This is a flowchart of a tunneling machine tracking and positioning method based on multi-sensor fusion provided by the present invention.

[0018] Figure 2 This is a schematic diagram showing the relationship between the various sensors, the tunnel, and the tunneling machine in this invention.

[0019] Figure 3 This is a schematic diagram of an infrared target plane according to the present invention.

[0020] Figure 4 This is a schematic diagram of the principle of triangle similarity in this invention. Detailed Implementation

[0021] 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.

[0022] Example 1: A method for tracking and locating a tunneling machine based on multi-sensor fusion, such as... Figure 1 As shown, the process includes: S1, fixing a total station on the top of the roadway behind the tunneling machine in the tunneling direction; arranging a guide rail above the roadway; suspending the tracking and positioning robot in the guide rail and positioning the tracking and positioning robot between the tunneling machine and the total station; mounting binocular cameras, strapdown inertial navigation systems, and prisms arranged left and right on the tracking and positioning robot; and installing an infrared target plane on the tunneling machine body, the infrared target plane including at least four non-collinear infrared target points; S2, constructing a carrier coordinate system, a camera coordinate system, a robot coordinate system, a total station coordinate system, and a navigation coordinate system; S3, controlling the tracking and positioning robot to move to the optimal position of the binocular cameras. S4. Observe the target distance range and acquire the target image of the infrared target plane collected by the binocular camera within its optimal observation distance range; S5. Calculate the camera-carrier rotation matrix and camera-carrier translation matrix between the camera coordinate system and the carrier coordinate system based on the target image; S6. Obtain the attitude of the tracking and positioning robot relative to the navigation coordinate system based on the strapdown inertial navigation system; S7. Determine the pose of the tunneling machine based on the camera-carrier rotation matrix, camera-carrier translation matrix, the attitude of the tracking and positioning robot relative to the navigation coordinate system, and the coordinate system transformation relationship between the carrier coordinate system, camera coordinate system, robot coordinate system, total station coordinate system, and navigation coordinate system.

[0023] In this embodiment, the specific arrangement of each sensor in S1 is as follows: Figure 2As shown. The infrared target plane refers to a rigid planar structure mounted on the body of the tunneling machine, consisting of at least four non-collinear infrared target points. Regarding the specific number of infrared target points included in the infrared target plane, this embodiment of the invention does not impose a specific limitation; specifically, the infrared target plane can be set to include 4-8 infrared target points as needed. Each infrared target point actively emits infrared light, possessing high brightness and high contrast characteristics, and can be clearly identified by a binocular camera even in complex environments (such as dust and low light).

[0024] In this embodiment, the carrier coordinate system is determined by the position of the infrared target plane; the camera coordinate system is constructed with the left camera of the binocular camera mounted on the tracking and positioning robot as the origin; the robot coordinate system is constructed with the center of the prism mounted on the tracking and positioning robot as the origin; the origin of the total station coordinate system is located at the measurement center of the total station; and the origin of the navigation coordinate system coincides with the origin of the total station coordinate system.

[0025] In this embodiment, the optimal observation distance range is the effective range within which the binocular camera can clearly and stably acquire target images of the infrared target plane. Typically, the optimal observation distance range for a binocular camera is a distance of 4-12 meters. When the distance between the binocular camera and the infrared target plane exceeds the optimal observation distance range, a tracking and positioning robot moves along the guide rail to bring the infrared target plane back within the optimal observation distance range. The target image is the image of the infrared target plane acquired by the binocular camera within the optimal observation distance range. The target image includes a left target image and a right target image; the left target image is acquired by the left camera of the binocular camera, and the right target image is acquired by the right camera of the binocular camera.

[0026] In this embodiment, the attitude of the tracking and positioning robot relative to the navigation coordinate system refers to the robot's position and orientation within the navigation coordinate system. The tunneling machine's pose includes its pitch angle, roll angle, heading angle, and coordinates in the navigation coordinate system.

[0027] The beneficial effects of this invention are as follows: by arranging guide rails above the roadway and suspending the tracking and positioning robot in the guide rails, the tracking and positioning robot can move in the guide rails during the tunneling process. Based on the target image of the infrared target plane collected by the binocular camera on the tracking and positioning robot and the relationship between the infrared target plane, the binocular camera, the strapdown inertial navigation system and the total station, continuous tracking and positioning of the tunneling machine can be achieved. The tunneling machine tracking and positioning process does not require re-stationing and recalibration, the operation is simple, and the efficiency of continuous tracking and positioning of the tunneling machine can be improved.

[0028] Example 2: Based on the above examples, S2 includes: S21, determining the carrier coordinate system according to the position of the infrared target plane. The origin of the carrier coordinate system Located at the center point of the infrared target plane, The axis points to the right. The axis points forward. The first axis, together with the other two axes, forms a right-handed coordinate system pointing upwards; S22, establish the left camera coordinate system for the stereo camera. With right camera coordinate system The origin of the left camera coordinate system. Located at the optical center of its camera, The axis points to the right. The axis points downwards. The right-hand coordinate system is formed by the right axis and the other two axes; the origin of the right camera coordinate system is... Located at the optical center of its camera, The axis points to the right. The axis points downwards. The first axis, together with the other two axes, forms a right-handed coordinate system; simultaneously, a camera coordinate system is established. And make it coincide with the left camera coordinate system; S23, establish the robot coordinate system The origin of the robot coordinate system. Located at the center of the prism, the three axes of the robot coordinate system are parallel to the three axes of the camera coordinate system; S24, determine the total station coordinate system based on the total station position. The origin of the total station coordinate system. Located at the measurement center of the total station, The axis points to the right. The axis points in the forward-looking direction. The S25 axis, together with the other two axes, forms a right-handed coordinate system; S25, the navigation coordinate system is determined based on the total station position. The origin of the navigation coordinate system Coinciding with the origin of the total station coordinate system, The axis points eastward. Axis northward, The x-axis, together with the other two axes, forms a right-handed coordinate system pointing upwards.

[0029] The beneficial effects of this invention are as follows: by accurately establishing multiple coordinate systems and strictly unifying the direction rules, not only can coordinate ambiguity be avoided, but also the data acquired by the total station, strapdown inertial navigation system and binocular camera can be aligned, providing an accurate coordinate basis for the subsequent positioning of the tunneling machine.

[0030] Example 3: Based on the above examples, S4 includes: S41, obtaining the carrier coordinates of each infrared target point in the infrared target plane in the carrier coordinate system; S42, calculating the camera coordinates of each infrared target point in the infrared target plane in the camera coordinate system; S43, constructing the transformation relationship function between the camera coordinate system and the carrier coordinate system; S44, determining the objective function based on the carrier coordinates and camera coordinates of each infrared target point and the transformation relationship function between the camera coordinate system and the carrier coordinate system, and solving the objective function to obtain the camera-carrier rotation matrix and camera-carrier translation matrix between the camera coordinate system and the carrier coordinate system.

[0031] In this embodiment, the transformation function between the camera coordinate system and the carrier coordinate system is a mathematical model describing the rigid body transformation relationship between the two coordinate systems. The objective function is an error optimization function based on the least squares method, which is used to solve for the camera-carrier rotation matrix and the camera-carrier translation matrix.

[0032] The beneficial effects of this invention are as follows: by constructing the transformation relationship function between the camera coordinate system and the carrier coordinate system through the carrier coordinates and camera coordinates of the infrared target point, and solving the target function based on the carrier coordinates, camera coordinates and the transformation relationship function between the camera coordinate system and the carrier coordinate system, the transformation matrix between the camera coordinate system and the carrier coordinate system can be solved automatically, reducing manual calibration steps, improving engineering efficiency, and is suitable for real-time pose monitoring needs in complex scenarios such as tunnel boring machines.

[0033] Example 4: Based on the above examples, S42 includes: S421, determining the depth of each infrared target point in the camera coordinate system according to the principle of triangle similarity in binocular camera imaging; S422, determining the relationship between the left camera coordinate system and the right camera coordinate system; S423, determining the camera coordinates of each infrared target point in the camera coordinate system according to the depth of each infrared target point in the camera coordinate system, the relationship between the left camera coordinate system and the camera coordinate system, and the relationship between the left camera coordinate system and the right camera coordinate system.

[0034] In this embodiment, the camera coordinates are the three-dimensional coordinates of the infrared target point in the camera coordinate system. The infrared target imaging point is the imaging point corresponding to the infrared target point in the target image. There is a one-to-one correspondence between the infrared target point and the infrared target imaging point, that is, the imaging point of the i-th infrared target point in the target image is the i-th infrared target imaging point.

[0035] Example 5: Based on the above examples, in S421, the principle of triangle similarity in binocular camera imaging is expressed by formula (2): (2); In formula (2), f represents the focal length of the left camera; Represents the camera coordinates of the i-th infrared target point Axis coordinates; Represents the camera coordinates of the i-th infrared target point Axis coordinates; Represents the camera coordinates of the i-th infrared target point The axis coordinates are the same as depth; This indicates the position of the infrared target imaging point corresponding to the i-th infrared target point in the left camera coordinate system. Axis coordinates; This indicates the position of the infrared target imaging point corresponding to the i-th infrared target point in the left camera coordinate system. Axis coordinates; In S422, the relationship between the left camera coordinate system and the right camera coordinate system is determined by formula (3): (3); In formula (3), b is the baseline length of the binocular camera; This indicates the position of the infrared target imaging point corresponding to the i-th infrared target point in the right camera coordinate system. Axis coordinates; In S423, the camera coordinates of the i-th infrared target point in the camera coordinate system are determined by formula (4): (4).

[0036] In this embodiment, such as Figure 4 As shown, taking point P1 in the infrared target as an example, the depth of point P1 in the camera coordinate system can be determined by formula (2) according to the principle of triangle similarity. Due to the existence of the baseline of the binocular camera, the imaging positions of the same infrared target point in space are different in the left and right cameras. The infrared target imaging point corresponding to the same infrared target point is located on the same straight line in the left and right target images. Therefore, formula (3) is derived. Combining formula (2) and formula (3), formula (4) is derived. Then, the camera coordinates of point P1 in the camera coordinate system are obtained. . Figure 4 middle, and P1 in the left camera coordinate system Axis coordinates and in the right camera coordinate system Axis coordinates.

[0037] Example 6: Based on the above examples, in S43, the transformation function between the camera coordinate system and the carrier coordinate system is expressed by formula (5): (5); in formula (5), This represents the camera coordinates of the i-th infrared target point in the camera coordinate system; This represents the carrier coordinates of the i-th infrared target point in carrier coordinates; Represents the camera-carrier rotation matrix; The camera-carrier translation matrix is ​​represented in S44. The target function is determined using formula (6) based on the carrier coordinates and camera coordinates of each infrared target point, as well as the transformation function between the camera coordinate system and the carrier coordinate system. (6); In formula (6), a represents the number of infrared target points in the infrared target plane.

[0038] Example 7: Based on the above examples, S5 includes: S51, acquiring the attitude angles of the tracking and positioning robot collected by the strapdown inertial navigation system; S52, determining the attitude of the tracking and positioning robot relative to the navigation coordinate system based on the attitude angles of the tracking and positioning robot.

[0039] In this embodiment, the attitude angles of the tracking and positioning robot include pitch angle, roll angle and heading angle, and the pitch angle, roll angle and heading angle are used as the attitude of the tracking and positioning robot relative to the navigation coordinate system.

[0040] The beneficial effects of this invention are as follows: Based on the high-frequency output of strapdown inertial navigation, the attitude parameters such as pitch angle, roll angle and heading angle of the tracking and positioning robot are dynamically calculated. This not only has a fast response speed and can make up for the delay of visual measurement, but also ensures that the pose calculation is not interrupted.

[0041] Example 8: Based on the above examples, S6 includes: S61, solving the robot-navigation rotation matrix between the robot coordinate system and the navigation coordinate system based on the robot's attitude relative to the navigation coordinate system; S62, acquiring the three-dimensional coordinates of the tracking and positioning robot observed by the total station, determining the total station-navigation rotation matrix based on the pre-calibrated relationship between the total station coordinate system and the navigation coordinate system, and determining the robot-total station rotation matrix and the robot-total station plane between the robot coordinate system and the total station coordinate system based on the three-dimensional coordinates of the tracking and positioning robot, the robot-navigation rotation matrix, and the total station-navigation rotation matrix. S63, determine the camera-robot translation matrix based on the pre-calibrated relationship between the robot coordinate system and the camera coordinate system, and calculate the carrier-navigation rotation matrix and carrier-navigation translation matrix between the carrier coordinate system and the navigation coordinate system based on the camera-robot translation matrix, total station-navigation rotation matrix, camera-carrier rotation matrix, and camera-carrier translation matrix; S64, determine the attitude angle of the tunneling machine and the navigation coordinates of the tunneling machine in the navigation coordinate system based on the carrier-navigation rotation matrix and the carrier-navigation translation matrix, and obtain the position and pose of the tunneling machine by combining the attitude angle of the tunneling machine and the navigation coordinates of the tunneling machine in the navigation coordinate system.

[0042] The beneficial effects of this invention are as follows: by linking the data collected by strapdown inertial navigation, total station and binocular camera through the transformation relationship between multiple coordinate systems, the cumulative error of strapdown inertial navigation and short-term fluctuations of vision equipment can be effectively suppressed, thereby improving the robustness of the system.

[0043] Example 9: Based on the above examples, in S61, the robot-navigation rotation matrix is ​​solved by formula (7) according to the orientation of the tracking and positioning robot relative to the navigation coordinate system in the ZXY order. : (7); in formula (7), This indicates the pitch angle of the tracking and positioning robot relative to the navigation coordinate system; This indicates the roll angle of the tracking and positioning robot relative to the navigation coordinate system; This represents the heading angle of the tracking and positioning robot relative to the navigation coordinate system; in S62, based on the three-dimensional coordinates of the tracking and positioning robot... Robot-Navigation Rotation Matrix Total station - navigation rotating matrix The robot-total station rotation matrix is ​​determined using formula (8). With robot-total station translation matrix : (8); in formula (8), Representing the robot-navigation rotation matrix The transpose of S63; in S63, the carrier-navigation rotation matrix is ​​calculated using formula (9). With the carrier-navigation translation matrix : (9); in formula (9), This represents the navigation coordinates of the i-th infrared target point in the navigation coordinate system; This represents the carrier coordinates of the i-th infrared target point in the carrier coordinate system; Represents the camera-carrier translation matrix; Represents the camera-carrier rotation matrix; Represents the camera-robot translation matrix; Represents total station-navigation rotation matrix The transpose of S64; In S64, the attitude angle of the tunneling machine and the navigation coordinates of the tunneling machine in the navigation coordinate system are combined to obtain the position and posture of the tunneling machine, which is expressed by formula (1): (1); in formula (1), This indicates the pitch angle of the tunneling machine in the navigation coordinate system; This indicates the roll angle of the tunneling machine in the navigation coordinate system; The heading angle of the tunneling machine in the navigation coordinate system is represented by ; P represents the coordinates of the tunneling machine in the navigation coordinate system. Representation of the carrier-navigation rotation matrix The element in the e-th row and j-th column.

[0044] In this embodiment, the pitch angle, roll angle, and heading angle are Euler angles of the tracking and positioning robot relative to the navigation coordinate system (rotated in ZXY order).

[0045] The beneficial effects of this invention are as follows: by using the Euler angle sequence of ZXY in formula (7) and the chain coordinate change in formula (9), the attitude and position of the tunneling machine can be measured, thereby obtaining the position and posture of the tunneling machine, providing a reliable basis for the tunneling machine's autonomous correction and path planning, and significantly improving the efficiency and safety of tunnel construction.

[0046] Example 10: Based on the above examples, the infrared target plane includes eight non-collinear infrared target points. Four of the eight infrared target points form the four vertices of a rectangle, and the other four infrared target points are located at the midpoints of the four sides of the rectangle, as shown below. Figure 3 As shown.

[0047] In this embodiment, among the eight infrared target points, four infrared target points form the four vertices of a rectangle, which is the basis of the infrared target plane. The midpoints of the four sides of the rectangle can be reasonably changed according to the needs of the scene.

[0048] In addition, the infrared target plane can also be configured to include five, six, seven, or four non-collinear infrared target points. A four-point infrared target plane is arranged such that the four points form a rectangle; a five-point infrared target plane is arranged such that the four points form a rectangle, with one point located at the intersection of the rectangle's diagonals; a six-point infrared target plane is arranged such that the four points form a rectangle, with two points located at the midpoints of opposite sides of the rectangle; a seven-point infrared target plane is arranged such that the four points form a rectangle, with two points located at the midpoints of opposite sides of the rectangle, and one point located at the intersection of the rectangle's diagonals.

[0049] The beneficial effects of this invention are: by setting the infrared target plane as... Figure 3 The eight non-collinear infrared target points shown make the infrared target plane geometrically symmetrical, optimizing the calibration reliability of camera-carrier coordinate transformation. Even if any two points fail, the remaining six points can still stably calculate the pose, adapting to complex scenarios such as tunnel dust and mechanical obstruction. The combination of midpoint and vertex can provide redundant data, reduce the impact of single-point noise, and improve the robustness of tunneling machine pose positioning.

[0050] 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 tunneling machine tracking and positioning method based on multi-sensor fusion, characterized in that, include: S1. A total station is fixedly installed on the top of the roadway behind the tunneling machine in the tunneling direction. A guide rail is arranged above the roadway. The tracking and positioning robot is suspended in the guide rail and positioned between the tunneling machine and the total station. A binocular camera, a strapdown inertial navigation system and a prism are mounted on the tracking and positioning robot in a left-right arrangement. An infrared target plane is installed on the body of the tunneling machine. The infrared target plane includes at least four non-collinear infrared target points. S2, construct the carrier coordinate system, camera coordinate system, robot coordinate system, total station coordinate system, and navigation coordinate system; S3, control the tracking and positioning robot to move to the optimal observation distance range of the binocular camera, and acquire the target image of the infrared target plane collected by the binocular camera in its optimal observation distance range; S4, calculate the camera-carrier rotation matrix and camera-carrier translation matrix between the camera coordinate system and the carrier coordinate system based on the target image; S5, obtain the attitude of the tracking and positioning robot relative to the navigation coordinate system based on the strapdown inertial navigation system; S6. Determine the orientation of the tunneling machine based on the camera-carrier rotation matrix, camera-carrier translation matrix, the robot's orientation relative to the navigation coordinate system, and the coordinate system transformation relationships between the carrier coordinate system, camera coordinate system, robot coordinate system, total station coordinate system, and navigation coordinate system.

2. The tunneling machine tracking and positioning method based on multi-sensor fusion according to claim 1, characterized in that, S2 include: S21, Determine the carrier coordinate system based on the position of the infrared target plane. The origin of the carrier coordinate system Located at the center point of the infrared target plane, The axis points to the right. The axis points forward. The first axis, together with the other two axes, forms a right-handed coordinate system pointing upwards; S22, establish the left camera coordinate system for the stereo camera. With right camera coordinate system The origin of the left camera coordinate system. Located at the optical center of its camera, The axis points to the right. The axis points downwards. The right-hand coordinate system is formed by the right axis and the other two axes; the origin of the right camera coordinate system is... Located at the optical center of its camera, The axis points to the right. The axis points downwards. The first axis, together with the other two axes, forms a right-handed coordinate system; simultaneously, a camera coordinate system is established. And make it coincide with the left camera coordinate system; S23, establish the robot coordinate system The origin of the robot coordinate system. Located at the center of the prism, the three axes of the robot coordinate system are parallel to the three axes of the camera coordinate system; S24, determine the total station coordinate system based on the total station position. The origin of the total station coordinate system. Located at the measurement center of the total station, The axis points to the right. The axis points in the forward-looking direction. The S25 axis, together with the other two axes, forms a right-handed coordinate system; S25, the navigation coordinate system is determined based on the total station position. The origin of the navigation coordinate system Coinciding with the origin of the total station coordinate system, The axis points eastward. Axis northward, The x-axis, together with the other two axes, forms a right-handed coordinate system pointing upwards.

3. The method for tracking and positioning a tunneling machine based on multi-sensor fusion according to claim 2, characterized in that, S4 includes: S41, Obtain the carrier coordinates of each infrared target point in the infrared target plane in the carrier coordinate system; S42, calculate the camera coordinates of each infrared target point in the infrared target plane in the camera coordinate system; S43, construct the transformation relationship function between the camera coordinate system and the carrier coordinate system; S44, determine the target function based on the carrier coordinates and camera coordinates of each infrared target point and the transformation relationship function between the camera coordinate system and the carrier coordinate system, and solve the target function to obtain the camera-carrier rotation matrix and camera-carrier translation matrix between the camera coordinate system and the carrier coordinate system.

4. The tunneling machine tracking and positioning method based on multi-sensor fusion according to claim 3, characterized in that, S42 includes: S421, determining the depth of each infrared target point in the camera coordinate system based on the principle of triangle similarity in binocular camera imaging; S422, determining the relationship between the left camera coordinate system and the right camera coordinate system; S423, determining the camera coordinates of each infrared target point in the camera coordinate system based on the depth of each infrared target point in the camera coordinate system, the relationship between the left camera coordinate system and the camera coordinate system, and the relationship between the left camera coordinate system and the right camera coordinate system.

5. The tunneling machine tracking and positioning method based on multi-sensor fusion according to claim 4, characterized in that, In S421, the principle of triangle similarity in binocular camera imaging is expressed by formula (2): (2); In formula (2), f represents the focal length of the left camera; Represents the camera coordinates of the i-th infrared target point Axis coordinates; Represents the camera coordinates of the i-th infrared target point Axis coordinates; Represents the camera coordinates of the i-th infrared target point The axis coordinates are the same as depth; This indicates the position of the infrared target imaging point corresponding to the i-th infrared target point in the left camera coordinate system. Axis coordinates; This indicates the position of the infrared target imaging point corresponding to the i-th infrared target point in the left camera coordinate system. Axis coordinates; In S422, the relationship between the left camera coordinate system and the right camera coordinate system is determined by formula (3): (3); In formula (3), b is the baseline length of the binocular camera; This indicates the position of the infrared target imaging point corresponding to the i-th infrared target point in the right camera coordinate system. Axis coordinates; In S423, the camera coordinates of the i-th infrared target point in the camera coordinate system are determined by formula (4): (4).

6. The tunneling machine tracking and positioning method based on multi-sensor fusion according to claim 5, characterized in that, In S43, the transformation function between the camera coordinate system and the carrier coordinate system is expressed by formula (5): (5); in formula (5), This represents the camera coordinates of the i-th infrared target point in the camera coordinate system; This represents the carrier coordinates of the i-th infrared target point in carrier coordinates; Represents the camera-carrier rotation matrix; The camera-carrier translation matrix is ​​represented in S44. The target function is determined using formula (6) based on the carrier coordinates and camera coordinates of each infrared target point, as well as the transformation function between the camera coordinate system and the carrier coordinate system. (6); In formula (6), a represents the number of infrared target points in the infrared target plane.

7. The method for tracking and positioning a tunneling machine based on multi-sensor fusion according to claim 1, characterized in that, S5 This includes: S51, acquiring the attitude angles of the tracking and positioning robot collected by the strapdown inertial navigation system; S52, determining the attitude of the tracking and positioning robot relative to the navigation coordinate system based on the attitude angles of the tracking and positioning robot.

8. A method for tracking and positioning a tunneling machine based on multi-sensor fusion according to claim 1 or 7, characterized in that, S6 include: S61, Solve the robot-navigation rotation matrix between the robot coordinate system and the navigation coordinate system based on the robot's attitude relative to the navigation coordinate system; S62, obtain the three-dimensional coordinates of the tracking and positioning robot observed by the total station, determine the total station-navigation rotation matrix according to the pre-calibrated relationship between the total station coordinate system and the navigation coordinate system, and determine the robot-total station rotation matrix and robot-total station translation matrix between the robot coordinate system and the total station coordinate system according to the three-dimensional coordinates of the tracking and positioning robot, the robot-navigation rotation matrix, and the total station-navigation rotation matrix. S63. Determine the camera-robot translation matrix based on the pre-calibrated relationship between the robot coordinate system and the camera coordinate system. Calculate the carrier-navigation rotation matrix and the carrier-navigation translation matrix between the carrier coordinate system and the navigation coordinate system based on the camera-robot translation matrix, the total station-navigation rotation matrix, the camera-carrier rotation matrix, and the camera-carrier translation matrix. S64. Determine the attitude angle of the tunneling machine and the navigation coordinates of the tunneling machine in the navigation coordinate system based on the carrier-navigation rotation matrix and the carrier-navigation translation matrix. Combine the attitude angle of the tunneling machine and the navigation coordinates of the tunneling machine in the navigation coordinate system to obtain the position and posture of the tunneling machine.

9. A method for tracking and locating a tunneling machine based on multi-sensor fusion according to claim 8, characterized in that, In S61, the robot-navigation rotation matrix is ​​solved by formula (7) according to the orientation of the tracking and positioning robot relative to the navigation coordinate system in the order of ZXY. : (7); in formula (7), This indicates the pitch angle of the tracking and positioning robot relative to the navigation coordinate system; This indicates the roll angle of the tracking and positioning robot relative to the navigation coordinate system; This represents the heading angle of the tracking and positioning robot relative to the navigation coordinate system; in S62, based on the three-dimensional coordinates of the tracking and positioning robot... Robot-Navigation Rotation Matrix Total station - navigation rotating matrix The robot-total station rotation matrix is ​​determined using formula (8). With robot-total station translation matrix : (8); in formula (8), Representing the robot-navigation rotation matrix The transpose of S63; in S63, the carrier-navigation rotation matrix is ​​calculated using formula (9). With the carrier-navigation translation matrix : (9); in formula (9), This represents the navigation coordinates of the i-th infrared target point in the navigation coordinate system; This represents the carrier coordinates of the i-th infrared target point in the carrier coordinate system; Represents the camera-carrier translation matrix; Represents the camera-carrier rotation matrix; Represents the camera-robot translation matrix; Represents total station-navigation rotation matrix The transpose of S64; In S64, the attitude angle of the tunneling machine and the navigation coordinates of the tunneling machine in the navigation coordinate system are combined to obtain the position and posture of the tunneling machine, which is expressed by formula (1): (1); in formula (1), This indicates the pitch angle of the tunneling machine in the navigation coordinate system; This indicates the roll angle of the tunneling machine in the navigation coordinate system; The heading angle of the tunneling machine in the navigation coordinate system is represented by ; P represents the coordinates of the tunneling machine in the navigation coordinate system. Representation of the carrier-navigation rotation matrix The element in the e-th row and j-th column.

10. A method for tracking and positioning a tunneling machine based on multi-sensor fusion according to claim 1, characterized in that, The infrared target plane includes eight non-collinear infrared target points. Four of the eight infrared target points form the four vertices of a rectangle, and the other four infrared target points are located at the midpoints of the four sides of the rectangle.

Images