Vehicle body dynamic pose calculation system, method and device for tunnel drilling operation

Abstract

The application discloses a kind of tunnel drilling operation car body dynamic pose calculation system, method and device, wherein the system includes: according to the three-dimensional coordinates of pull line point, the coordinate of line outlet in trolley coordinate system is calibrated, the line outlet is connected with pull line point, and the length of pull line is obtained;Attitude sensor is used to obtain the attitude angle of trolley in tunnel according to the trolley attitude data collected;Computing device is used to obtain rotation matrix according to the length of pull line and attitude angle;According to the coordinate of line outlet in tunnel coordinate system and the rotation matrix, the conversion relationship of the coordinate of line outlet in trolley coordinate system and the coordinate in tunnel coordinate system is obtained;According to preset algorithm and conversion relationship, the coordinate of line outlet in trolley coordinate system is iterated, and new rotation matrix is obtained, and the pose result of trolley is calculated according to new rotation matrix.The application can accelerate iterative convergence, improve calculation accuracy, and realize the dynamic pose calculation of car body in operation process.

Description

Technical Field

[0001] This invention relates to the field of positioning technology for tunnel-specific equipment, and in particular to a system, method, and apparatus for calculating the dynamic position and posture of a vehicle during tunnel drilling operations. Background Technology

[0002] This section is intended to provide background or context for the embodiments of the invention set forth in the claims. The description herein is not an admission that it is prior art simply because it is included in this section.

[0003] The movement of tunnel boring machines (TBMs) during operation is characterized by low speed, short distance, strong vibration, and long duration. Mechanical rock breaking generates a high-concentration dust environment. High-precision measurement solutions such as laser vision are only suitable for specific conditions during low-dust periods, and the calibration and implementation of the trolley posture measurement system are complex. Radio solutions such as UWB have poor measurement accuracy and cannot meet the positioning requirements of automated tunnel operations. Inertial navigation solutions require combination with other technologies for posture calibration, resulting in high system costs. Trolley posture measurement technology for tunnel-specific equipment is a fundamental key technology for the intelligent development of specialized equipment. In particular, TBMs often generate large amounts of dust during drilling operations in hard rock tunnels, causing difficulties in manually controlling over- and under-excavation and low operational efficiency. Measurement methods such as laser vision are easily affected by dust and cannot achieve dynamic posture measurement of the trolley in dusty environments. Therefore, a high-precision measurement solution that can adapt to high-dust conditions is urgently needed. Summary of the Invention

[0004] This invention provides a vehicle dynamic pose calculation system for tunnel drilling operations to accelerate iterative convergence, improve calculation accuracy, and realize vehicle dynamic pose calculation during operations. The system includes:

[0005] Hardware devices and computing devices; wherein, the hardware devices include: a wire fixing device and an attitude sensor;

[0006] The cable fixing device is used to: calibrate the coordinates of the cable outlet in the trolley coordinate system according to the three-dimensional coordinates of the cable outlet, connect the cable outlet to the cable outlet, and obtain the cable length from the cable outlet to the cable outlet. The cable outlet is a pulley cable outlet, and the guide pulley of the cable outlet rotates flexibly following the cable direction.

[0007] The attitude sensor is used to: obtain the attitude angle of the trolley in the tunnel based on the collected trolley attitude data;

[0008] The computing device is used to: obtain a rotation matrix based on the cable length and attitude angle; obtain the transformation relationship between the coordinates of the cable outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system based on the preset coordinates of the cable outlet in the tunnel coordinate system and the rotation matrix; iterate the coordinates of the cable outlet in the trolley coordinate system according to the preset algorithm and the transformation relationship to obtain a new rotation matrix; and calculate the pose result of the trolley based on the new rotation matrix.

[0009] This invention also provides a method for calculating the dynamic pose of a vehicle body during tunnel drilling operations. This method is applied to a computing device to accelerate iterative convergence, improve calculation accuracy, and realize the calculation of the dynamic pose of the vehicle body during operations. The method includes:

[0010] The rotation matrix is ​​obtained based on the cable length and the attitude angle. The cable length is measured by the cable fixing device, and the attitude angle is obtained by the attitude sensor based on the trolley attitude data.

[0011] Based on the preset coordinates of the outlet in the tunnel coordinate system and the rotation matrix, the transformation relationship between the coordinates of the outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system is obtained. The coordinates of the outlet in the trolley coordinate system are calibrated by the cable fixing device.

[0012] The coordinates of the outlet in the trolley coordinate system are iterated according to the preset algorithm and transformation relationship to obtain a new rotation matrix. Based on the new rotation matrix, the pose of the trolley is calculated.

[0013] This invention also provides a device for calculating the dynamic pose of a vehicle body during tunnel drilling operations, to accelerate iterative convergence, improve calculation accuracy, and realize the calculation of the dynamic pose of the vehicle body during operations. The device includes:

[0014] The rotation matrix acquisition module is used to acquire the rotation matrix based on the cable length and the attitude angle. The cable length is measured by the cable fixing device, and the attitude angle is obtained by the attitude sensor based on the trolley attitude data.

[0015] The transformation relationship acquisition module is used to acquire the transformation relationship between the coordinates of the outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system based on the preset coordinates of the outlet in the tunnel coordinate system and the rotation matrix. The coordinates of the outlet in the trolley coordinate system are obtained by calibration of the cable fixing device.

[0016] The pose result calculation module is used to iterate the coordinates of the outlet in the trolley coordinate system according to the preset algorithm and transformation relationship to obtain a new rotation matrix, and calculate the pose result of the trolley based on the new rotation matrix.

[0017] This invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the above-mentioned method for calculating the dynamic pose of the vehicle body in tunnel drilling operations.

[0018] This invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described method for calculating the dynamic pose of a vehicle body during tunnel drilling operations.

[0019] This invention also provides a computer program product, which includes a computer program that, when executed by a processor, implements the above-mentioned method for calculating the dynamic pose of the vehicle body during tunnel drilling operations.

[0020] In this embodiment of the invention, a hardware device and a computing device are used. The hardware device includes: a cable fixing device and an attitude sensor. The cable fixing device is used to: calibrate the coordinates of the cable outlet in the trolley coordinate system based on the three-dimensional coordinates of the cable point; connect the cable outlet to the cable point to obtain the cable length from the cable outlet to the cable point; the cable outlet is a pulley cable outlet, and the guide pulley of the cable outlet rotates flexibly following the cable direction. The attitude sensor is used to: obtain the attitude angle of the trolley in the tunnel based on the collected trolley attitude data. The computing device is used to: obtain a rotation matrix based on the cable length and attitude angle; obtain the transformation relationship between the coordinates of the cable outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system based on the preset coordinates of the cable outlet in the tunnel coordinate system and the rotation matrix; iterate the coordinates of the cable outlet in the trolley coordinate system according to the preset algorithm and the transformation relationship to obtain a new rotation matrix; and calculate the pose result of the trolley based on the new rotation matrix. In the above process, the embodiments of the present invention obtain the wire length and attitude angle through hardware devices, and then obtain the rotation matrix and transformation relationship through computing devices to obtain a new rotation matrix. Based on the new rotation matrix, the pose result of the trolley is calculated, thereby accelerating the iterative convergence, improving the calculation accuracy, and realizing the dynamic pose calculation of the vehicle body during operation. Attached Figure Description

[0021] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. In the drawings:

[0022] Figure 1 This is a schematic diagram of the vehicle dynamic pose calculation system for tunnel drilling operations in an embodiment of the present invention;

[0023] Figure 2 This is a structural diagram of the vehicle dynamic pose calculation system for tunnel drilling operations in an embodiment of the present invention;

[0024] Figure 3 This is a structural diagram of the coordinates in the trolley coordinate system and the coordinates in the tunnel coordinate system in an embodiment of the present invention;

[0025] Figure 4 This is a structural diagram of the pulley structure in an embodiment of the present invention;

[0026] Figure 5 This is a flowchart illustrating the application of the vehicle dynamic pose calculation method for tunnel drilling operations to a computing device in an embodiment of the present invention.

[0027] Figure 6 This is a schematic diagram of the calculation device for the dynamic pose of the vehicle body during tunnel drilling operations in an embodiment of the present invention. Detailed Implementation

[0028] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings. Here, the illustrative embodiments of the present invention and their descriptions are used to explain the present invention, but are not intended to limit the present invention.

[0029] Figure 1 This is a schematic diagram of a vehicle dynamic pose calculation system for tunnel drilling operations according to an embodiment of the present invention. The system includes: a hardware device and a computing device; wherein, the hardware device includes: a cable fixing device and an attitude sensor;

[0030] The cable fixing device 01 is used to: calibrate the coordinates of the cable outlet in the trolley coordinate system according to the three-dimensional coordinates of the cable outlet, connect the cable outlet to the cable outlet, and obtain the cable length from the cable outlet to the cable outlet. The cable outlet is a pulley cable outlet, and the guide pulley of the cable outlet rotates flexibly following the cable direction.

[0031] The attitude sensor 02 is used to: obtain the attitude angle of the trolley in the tunnel based on the collected trolley attitude data;

[0032] The computing device 03 is used to: obtain a rotation matrix based on the cable length and attitude angle; obtain the transformation relationship between the coordinates of the cable outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system based on the preset coordinates of the cable outlet in the tunnel coordinate system and the rotation matrix; iterate the coordinates of the cable outlet in the trolley coordinate system according to the preset algorithm and the transformation relationship to obtain a new rotation matrix; and calculate the pose result of the trolley based on the new rotation matrix.

[0033] In a specific embodiment, the vehicle dynamic pose calculation system for tunnel drilling operations includes two parts: a hardware device and a computing device. The hardware device includes a wire fixing device and an attitude sensor. The computing device includes an industrial control computer. The wire fixing device includes a wire point fixer and a wire measuring device. Figure 2 This is a structural diagram of the vehicle dynamic pose calculation system for tunnel drilling operations in this embodiment of the invention. A total of 6 wire fixing points 1-6 are arranged around the trolley on the upper part of the tunnel wall. A total of 6 wire measuring devices 8-13, attitude sensors 14, and industrial control computers 15 are arranged on the suspended trolley platform 7. The wires of the 6 wire measuring devices 8-13 are connected to the 6 wire fixing points 1-6. The measurement data of the 6 wire measuring devices 8-13 and attitude sensors 14 are transmitted to the industrial control computer 15 for vehicle pose calculation.

[0034] In a specific embodiment, the three-dimensional coordinates of the tunnel for the six guy wire fixing points (1-6) are known as follows:

[0035] ;

[0036] Given that the coordinates of the 6 outlets (8-13) in the trolley coordinate system are... Let the pull-wire measurement values ​​of the 6 pull-wire measuring devices be... The attitude angles are (α, β, θ), where α is the pitch angle of the vehicle body, β is the roll angle of the vehicle body, and θ is the heading angle of the vehicle body. The three-dimensional coordinates of the six exit points on the trolley in the tunnel coordinate system are calculated, and thus the pose of the trolley in the tunnel measurement coordinate system is obtained as follows:

[0037] ;

[0038] In one embodiment, any number of the pull points are not on the same plane, and any number of the outlets are not on the same plane.

[0039] In a specific embodiment, the pull points of the six pull point fixers are not required. Located in the same spatial plane, six wire measuring device outlets are not required. It lies on a spatial plane.

[0040] Figure 3 This is a structural diagram showing the coordinates in the trolley coordinate system and the coordinates in the tunnel coordinate system in an embodiment of the present invention. - A point whose three-dimensional coordinates are known in the tunnel coordinate system; - To determine the distances between the lines connecting the six points in the trolley coordinate system, given the known three-dimensional coordinates of the trolley points, we can then determine the distances between the lines connecting the six points. , , , , , , , , , , , , , , The values ​​for all 15 sides are related to string measurement. , , , , , With a total of 21 observations across 6 sides, calculate the 6 points on the trolley. The three-dimensional coordinates in the tunnel coordinate system are used to calculate the pose of the trolley in the tunnel measurement coordinate system; based on... Figure 3 The spatial boundary relationships are shown, with 21 observations, and 6 points on the trolley are solved. Corresponding tunnel spatial coordinates There are 18 unknowns in total.

[0041] Figure 4 The diagram shows the structure of the pulley structure in one embodiment of the present invention. In one embodiment, the cable outlet is a pulley cable outlet, which includes: a cable pull hole 101, a small pulley 102, an anti-detachment structure 103, a rotating structure 104, and a bearing 105.

[0042] The wire through hole 101 is formed by two small pulleys 102 arranged side by side in the middle;

[0043] The anti-detachment structure 103 is located on the outside of the pulley and has a chamfered design to prevent scratching the cable;

[0044] The rotating structure 104 and bearing 105 cause the pulley to change as the direction of the pull line changes, and enable the pulley to follow the movement of the trolley to change the direction of the pull line.

[0045] This invention also provides a method for calculating the dynamic pose of a vehicle body during tunnel drilling operations, applied to a computing device. Figure 5 This is a flowchart illustrating the method for calculating the dynamic pose of a vehicle body during tunnel drilling operations, as described in this invention. The method includes:

[0046] Step 501: Obtain the rotation matrix based on the cable length and attitude angle. The cable length is measured by the cable fixing device, and the attitude angle is obtained by the attitude sensor based on the trolley attitude data.

[0047] Step 502: Based on the preset coordinates of the outlet in the tunnel coordinate system and the rotation matrix, obtain the transformation relationship between the coordinates of the outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system. The coordinates of the outlet in the trolley coordinate system are obtained by calibration of the cable fixing device.

[0048] Step 503: Iterate the coordinates of the outlet in the trolley coordinate system according to the preset algorithm and transformation relationship to obtain a new rotation matrix. Calculate the trolley pose result based on the new rotation matrix.

[0049] In a specific embodiment, the iterative calculation method for the rotation and translation of the point set is as follows:

[0050] Let the coordinates of the six outlets on the trolley in the tunnel coordinate system be... Then these 6 points have the transformation relationship shown in equation (1) in the trolley coordinate system and the tunnel coordinate system;

[0051] (1)

[0052] in 3 The rotation matrix of 3, and its relationship with the trolley attitude angle are shown in equation (2). 3 1. Translation vector;

[0053] (2)

[0054] Based on the coordinates of the tunnel coordinate system of the 6 exits Six points with coordinates relative to the known tunnel coordinate system The distance relationship determines the constraints for iterative calculation. As shown in equation (3), where n =6, The length of the line between corresponding points;

[0055] (3)

[0056] The relationship of the point set rotation and translation iteration process is established according to equation (3), as shown in equation (4).

[0057] (4)

[0058] The iterative calculation method based on point set rotation and translation is shown in equation (5). j Number of iterations

[0059] (5)

[0060] make for The correction value is calculated as shown in equation (6). The correction value, k coefficient ;

[0061] (6)

[0062] Then, the Singular Value Decomposition (SVD) algorithm was used to calculate the coordinates of the six trolley points. arrive rotation matrix Translation vector The iteration termination condition is shown in equation (7);

[0063] (7)

[0064] To accelerate iterative convergence, when j In the initial iteration, the rotation matrix is ​​initialized using the angle values ​​(α, β, θ) from the trolley tilt attitude sensor. Using the coordinates of 6 guy wire fixing points inside the tunnel Initial translation of the centroid coordinates Considering the measured value of the string There is measurement error, typically taking k =0.5, =0.001 enables fast iterative convergence calculation to obtain the rotation matrix. , and .in Given the three-dimensional coordinates of the tunnel at the six exit points on the trolley, the trolley's attitude angles and heading angles (α, β, θ) can be determined by the rotation matrix. The calculation is shown in Equation (8), thus realizing the pose calculation of the trolley.

[0065] ; (8)

[0066] This invention also provides a vehicle dynamic pose calculation device for tunnel drilling operations, as described in the following embodiments. Since the principle by which this device solves the problem is similar to the vehicle dynamic pose calculation method for tunnel drilling operations, the implementation of this device can refer to the implementation of the vehicle dynamic pose calculation method for tunnel drilling operations; repeated details will not be elaborated further. Figure 6 This is a schematic diagram of a device for calculating the dynamic pose of a vehicle during tunnel drilling operations, as described in an embodiment of the present invention. The device includes:

[0067] The rotation matrix acquisition module 601 is used to acquire a rotation matrix based on the pull wire length and the attitude angle. The pull wire length is measured by the pull wire fixing device, and the attitude angle is obtained by the attitude sensor based on the trolley attitude data.

[0068] The conversion relationship acquisition module 602 is used to acquire the conversion relationship between the coordinates of the outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system based on the preset coordinates of the outlet in the tunnel coordinate system and the rotation matrix. The coordinates of the outlet in the trolley coordinate system are obtained by calibration of the cable fixing device.

[0069] The pose result calculation module 603 is used to iterate the coordinates of the outlet in the trolley coordinate system according to the preset algorithm and transformation relationship to obtain a new rotation matrix, and calculate the pose result of the trolley according to the new rotation matrix.

[0070] In one embodiment, the preset algorithm is an iterative method of rotating and translating a point set.

[0071] The system proposed in this embodiment uses six guy wire fixing devices and attitude sensors. The attitude sensors provide initial values ​​for the iterative method, accelerating the iterative convergence calculation. According to the point-set-based iterative method, using the vehicle pitch angle α and roll angle β provided by the tilt sensor can reduce the number of guy wires by 1-2 to calculate the vehicle's position and heading angles. That is, using 4-5 guy wires and fixing the tilt angles α and β can still achieve the vehicle's position and heading angle calculations, but this iterative calculation result depends on a high-precision tilt angle. Since the vehicle body often vibrates significantly during dynamic operation, low-performance tilt sensors generally cannot provide high-precision tilt angles, leading to large errors in the iterative calculation results. Therefore, the low-configuration scheme using 4-5 guy wires is suitable for static positioning but not for dynamic positioning. Other distributed guy wire position calculation methods, with more than six guy wires, essentially add observation conditions to this scheme.

[0072] The beneficial effects of this invention are as follows: Based on the pull-wire measuring device, this invention can overcome the influence of environmental dust. The system uses six pull-wire measuring devices distributed on a trolley.

[0073] 1. It can overcome the impact of environmental dust without requiring complex structural design;

[0074] 2. The iterative calculation method based on point set rotation and translation can improve the iteration convergence speed and calculation accuracy, and is not affected by the vibration of the trolley during dynamic operation, thus realizing the dynamic pose calculation of the vehicle body during operation.

[0075] This invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the above-mentioned method for calculating the dynamic pose of the vehicle body in tunnel drilling operations.

[0076] This invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described method for calculating the dynamic pose of a vehicle body during tunnel drilling operations.

[0077] This invention also provides a computer program product, which includes a computer program that, when executed by a processor, implements the above-mentioned method for calculating the dynamic pose of the vehicle body during tunnel drilling operations.

[0078] In this embodiment of the invention, a hardware device and a computing device are used. The hardware device includes: a cable fixing device and an attitude sensor. The cable fixing device is used to: calibrate the coordinates of the cable outlet in the trolley coordinate system based on the three-dimensional coordinates of the cable point; connect the cable outlet to the cable point to obtain the cable length from the cable outlet to the cable point; the cable outlet is a pulley cable outlet, and the guide pulley of the cable outlet rotates flexibly following the cable direction. The attitude sensor is used to: obtain the attitude angle of the trolley in the tunnel based on the collected trolley attitude data. The computing device is used to: obtain a rotation matrix based on the cable length and attitude angle; obtain the transformation relationship between the coordinates of the cable outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system based on the preset coordinates of the cable outlet in the tunnel coordinate system and the rotation matrix; iterate the coordinates of the cable outlet in the trolley coordinate system according to the preset algorithm and the transformation relationship to obtain a new rotation matrix; and calculate the pose result of the trolley based on the new rotation matrix. In the above process, the embodiments of the present invention obtain the wire length and attitude angle through hardware devices, and then obtain the rotation matrix and transformation relationship through computing devices to obtain a new rotation matrix. Based on the new rotation matrix, the pose result of the trolley is calculated, thereby accelerating the iterative convergence, improving the calculation accuracy, and realizing the dynamic pose calculation of the vehicle body during operation.

[0079] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0080] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0081] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0082] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0083] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A vehicle dynamic pose calculation system for tunnel drilling operations, characterized in that, include: Hardware devices and computing devices; wherein, the hardware devices include: a wire fixing device and an attitude sensor; The guy wire fixing device includes multiple guy wire point fixers and multiple guy wire measuring devices. The guy wire point fixers are installed on the tunnel wall, and the three-dimensional coordinates of their guy wire points in the tunnel coordinate system are known. The guy wire measuring devices are installed on a trolley, and the coordinates of their outlets in the trolley coordinate system are known in advance. The guy wires of the guy wire measuring devices are correspondingly connected to the guy wire point fixers to measure the length of the guy wire from the outlet to the guy wire point. The cable fixing device is used to: calibrate the coordinates of the cable outlet in the trolley coordinate system according to the three-dimensional coordinates of the cable outlet, connect the cable outlet to the cable outlet, and obtain the cable length from the cable outlet to the cable outlet. The cable outlet is a pulley cable outlet, and the guide pulley of the cable outlet rotates flexibly following the cable direction. The attitude sensor is used to: obtain the attitude angle of the trolley in the tunnel based on the collected trolley attitude data; The computing device is used to: obtain a rotation matrix based on the cable length and attitude angle; obtain the transformation relationship between the coordinates of the cable outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system based on the preset coordinates of the cable outlet in the tunnel coordinate system and the rotation matrix; iterate the coordinates of the cable outlet in the trolley coordinate system according to the preset algorithm and the transformation relationship to obtain a new rotation matrix; and calculate the pose result of the trolley based on the new rotation matrix. The preset algorithm is an iterative method for point set rotation and translation. The iterative calculation method for point set rotation and translation is as follows: Let the coordinates of the six outlets on the trolley in the tunnel coordinate system be... Then these 6 points have the transformation relationship shown in equation (1) in the trolley coordinate system and the tunnel coordinate system; （1） The coordinates of the six outlets in the trolley coordinate system are as follows: , 3 The rotation matrix of 3, and its relationship with the trolley attitude angle are shown in equation (2). 3 1. Translation vector; attitude angle is (α,β,θ), where α is the vehicle's pitch angle, β is the vehicle's roll angle, and θ is the vehicle's heading angle; （2） Based on the coordinates of the tunnel coordinate system of the 6 exits Six points with coordinates relative to the known tunnel coordinate system The distance relationship determines the constraints for iterative calculation. As shown in equation (3), where n =6, The length of the guy wire between corresponding points; the three-dimensional coordinates of the guy wire points in the tunnel for the six guy wire fixing points are: ; （3） The relationship of the point set rotation and translation iteration process is established according to equation (3), as shown in equation (4). （4） The iterative calculation method based on point set rotation and translation is shown in equation (5). j Number of iterations （5） make for The correction value is calculated as shown in equation (6). The correction value, k coefficient ; （6） Then, the Singular Value Decomposition (SVD) algorithm was used to calculate the coordinates of the six trolley points. arrive rotation matrix Translation vector The iteration termination condition is shown in equation (7); （7） To accelerate iterative convergence, when j In the initial iteration, the rotation matrix is ​​initialized using the angle values ​​(α, β, θ) from the trolley tilt attitude sensor. Using the coordinates of 6 guy wire fixing points inside the tunnel Initial translation of the centroid coordinates Considering the measured value of the string There is measurement error, take k =0.5, =0.001 enables fast iterative convergence calculation to obtain the rotation matrix. , and ;in Given the three-dimensional coordinates of the tunnel at the six exit points on the trolley, the trolley's attitude angles and heading angles (α, β, θ) can be determined by the rotation matrix. The calculation is shown in Equation (8), and the pose calculation of the trolley is thus realized; ； （8）。 2. The system as described in claim 1, characterized in that, The cable outlet is a pulley cable outlet, which includes: a cable pull hole, a small pulley, an anti-detachment structure, a rotating structure, and a bearing; The pull wire through hole is formed by two small pulleys arranged side by side in the middle; The anti-detachment structure is located on the outside of the pulley and has a chamfered design to prevent scratching the cable; The rotating structure and bearings allow the pulley to change direction as the cable pull direction changes, and also allow the pulley to follow the movement of the trolley to change the cable pull direction.

3. The system as described in claim 1, characterized in that, Any number of the stated pull points are not on the same plane, and any number of stated outlets are not on the same plane.

4. A method for calculating the dynamic pose of a vehicle body in tunnel drilling operations using the system described in any one of claims 1 to 3, characterized in that, include: The rotation matrix is ​​obtained based on the cable length and the attitude angle. The cable length is measured by the cable fixing device, and the attitude angle is obtained by the attitude sensor based on the trolley attitude data. Based on the preset coordinates of the outlet in the tunnel coordinate system and the rotation matrix, the transformation relationship between the coordinates of the outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system is obtained. The coordinates of the outlet in the trolley coordinate system are calibrated by the cable fixing device. The coordinates of the outlet in the trolley coordinate system are iterated according to the preset algorithm and transformation relationship to obtain a new rotation matrix. Based on the new rotation matrix, the pose of the trolley is calculated.

5. The method as described in claim 4, characterized in that, The preset algorithm is an iterative method of rotating and translating point sets.

6. A calculation device for the dynamic pose of a vehicle body during tunnel drilling operations using the system described in any one of claims 1 to 3, characterized in that, include: The rotation matrix acquisition module is used to acquire the rotation matrix based on the cable length and the attitude angle. The cable length is measured by the cable fixing device, and the attitude angle is obtained by the attitude sensor based on the trolley attitude data. The transformation relationship acquisition module is used to acquire the transformation relationship between the coordinates of the outlet in the trolley coordinate system and the coordinates in the tunnel coordinate system based on the preset coordinates of the outlet in the tunnel coordinate system and the rotation matrix. The coordinates of the outlet in the trolley coordinate system are obtained by calibration of the cable fixing device. The pose result calculation module is used to iterate the coordinates of the outlet in the trolley coordinate system according to the preset algorithm and transformation relationship to obtain a new rotation matrix, and calculate the pose result of the trolley based on the new rotation matrix.

7. The apparatus as claimed in claim 6, characterized in that, The preset algorithm is an iterative method of rotating and translating point sets.

8. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the method of claim 5.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the method of claim 5.

10. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor, implements the method of claim 5.

Images