Shaft sinking machine attitude determination method and system

Abstract

The application aims to provide a shaft heading machine posture determination method and system, and belongs to the underground engineering monitoring field. The method establishes a fixed coordinate system and a carrier coordinate system, uses a shaft wall ultrasonic transmitter and a platform receiver to obtain a propagation time, and calculates an absolute coordinate of the receiver and an absolute posture of the platform based on a spatial spherical intersection. Meanwhile, the gyro and accelerometer are used to collect angular velocity and specific force information, and high-frequency and low-frequency predicted postures are calculated. The absolute posture and the predicted posture are combined to obtain an optimal posture angle through a fusion algorithm, and a conversion matrix is constructed to process the specific force information to calculate the optimal position deviation. The application uses the advantages of strong ultrasonic penetration and continuous inertial navigation to solve the problem of laser guidance failure in a dusty and muddy environment, and realizes high-precision, anti-interference and real-time monitoring of the shaft heading machine posture.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of underground engineering construction monitoring, and particularly relates to a shaft heading machine posture determination method and system. BACKGROUND

[0002] With the rapid development of mine and tunnel engineering, the shaft excavation depth is increasing, and the requirement for maintaining the vertical direction precision of the shaft is increasingly stringent, and the guide system of the shaft heading machine plays a decisive role. The core task of the guide system is to monitor the position and posture information of the heading machine in real time and feed it back to the control room to adjust the excavation direction.

[0003] At present, the shaft heading machine mainly relies on a laser guide system for positioning. The technology usually emits a vertical light beam through a shaft mouth laser emitter, which is received by a laser target on the heading machine, and the posture is calculated according to the deviation of the light spot from the target center. However, the shaft construction site environment is extremely poor, and the underground is full of high-concentration dust and mud splashing. The laser guide system has high requirements for the light path environment, and mud and dust can easily block or scatter the laser beam, resulting in a decrease in signal strength or even complete loss. Once the laser signal is interrupted, the guide system loses the alignment reference and cannot provide continuous guidance with high precision, thereby seriously affecting the construction safety and engineering quality of the heading machine. Therefore, there is an urgent need for a posture determination technology that can adapt to the poor underground environment and has strong anti-interference ability. SUMMARY

[0004] The present application aims to overcome the deficiencies in the prior art, and provides a shaft heading machine posture determination method and system, which establishes a measurement link of ultrasonic wave emission and object platform reception, uses the spatial spherical intersection principle to obtain the absolute posture, and combines the specific force and angular velocity information of the inertial sensor for multi-source data fusion, to realize accurate calculation and real-time prediction of the heading machine position and posture in a complex environment.

[0005] To achieve the above-mentioned purpose, in a first aspect, the present application provides a shaft heading machine posture determination method, comprising the following steps: step S1, establishing a first coordinate system for describing the spatial position and a second coordinate system following the movement of the shaft heading machine, and obtaining the ultrasonic wave propagation time information, the angular velocity information of the object platform and the specific force information.

[0006] Step S2, calculating the absolute coordinates of the ultrasonic wave receiver in the first coordinate system according to the ultrasonic wave propagation time information, and further calculating the absolute position deviation of the object platform and the unit normal vector reflecting the inclination degree of the object platform, and calculating the absolute posture angle of the object platform according to the unit normal vector.

[0007] Step S3, integrating the angular velocity information to obtain a high-frequency predicted posture angle, and calculating a low-frequency predicted posture angle according to the specific force information.

[0008] Step S4, according to the absolute attitude angle, the high-frequency predicted attitude angle and the low-frequency predicted attitude angle, an optimal attitude angle is calculated by using an optimal attitude angle fusion algorithm, and a coordinate system conversion matrix is constructed according to the optimal attitude angle.

[0009] Step S5, the specific gravity information is converted to the first coordinate system by using the coordinate system conversion matrix and the influence of gravity is removed, a predicted position deviation is obtained by integral operation, the predicted position deviation is fused with the absolute position deviation obtained in step S2, and an optimal position deviation is output.

[0010] Further, the specific way of establishing the coordinate system in step S1 is that the first coordinate system takes the center point of the shaft mouth as the origin, the vertical downward direction as the positive direction of the Z axis, the X axis positive direction horizontally points to the right side, and the Y axis is perpendicular to the X axis and the Z axis; the second coordinate system takes the center of the object carrying platform as the origin, the Z axis direction is downward along the central rigid column axis, and the X axis and Y axis directions are initially consistent with the X axis and Y axis directions in the first coordinate system.

[0011] Further, the process of calculating the absolute position deviation and the absolute attitude angle of the object carrying platform in step S2 includes: according to the formula the distance between the nth ultrasonic receiver and the mth ultrasonic transmitter is calculated , where is the speed of sound, is the signal propagation time from the mth ultrasonic transmitter to the nth ultrasonic receiver. A space sphere is constructed with the known coordinates of the ultrasonic transmitter as the center and the distance as the radius, and the absolute coordinates of the nth ultrasonic receiver in the first coordinate system are calculated by the spherical intersection principle , where n represents the nth measurement, and

[0012] represents the absolute coordinates of the jth receiver in the first coordinate system at the initial moment; the average value is calculated according to all the absolute coordinates of the ultrasonic receivers to obtain the current coordinates of the center of the object carrying platform , and the current coordinates are subtracted from the initial platform coordinates to obtain the absolute position deviation . .

[0013] Further, the calculation of the absolute attitude angle in step S2 specifically includes: based on the absolute coordinates of the ultrasonic receivers ​​and the current coordinates of the center of the object platform construct two non-collinear vectors cross-multiply and normalize the non-collinear vectors to obtain a unit normal vector perpendicular to the object platform ; calculate the absolute attitude angle according to the components of the unit normal vector, the formula is: absolute roll angle ; absolute pitch angle ; absolute heading angle ; wherein, is the vector the components on the X-axis and Y-axis, is the fixed value of the angle between the initial installation position of the receiver and the X-axis of the second coordinate system.

[0014] Further, the specific formula for calculating the low-frequency predicted attitude angle in step S3 is:

[0015] low-frequency roll angle ; low-frequency pitch angle ; wherein, are the components of the specific force information on the three axes of the second coordinate system, respectively.

[0016] Further, the specific formula for the optimal attitude angle fusion algorithm in step S4 is:

[0017] ; wherein, when k is 1, 2, or 3, represent the optimal roll angle, the optimal pitch angle, and the optimal heading angle, respectively; is the filter coefficient; is the absolute attitude angle; is the high-frequency predicted attitude angle; is the time step; is the angular velocity; when k = 3, the low-frequency heading angle cannot be obtained from the specific force information, so the weighted average of the absolute heading angle and the low-frequency heading angle cannot be used to correct the high-frequency heading angle. Therefore, the fusion algorithm formula executed by the attitude fusion module is modified as:

[0018] ;

[0019] Further, the coordinate system conversion matrix constructed in step S4 is Specifically:

[0020] ;

[0021] wherein, are the optimal roll angle, the optimal pitch angle, and the optimal heading angle obtained in step S4, respectively, represents the cosine function , representing a sine function .

[0022] Further, the formula for calculating the predicted position deviation in step S5 is:

[0023] ; wherein, is the coordinate system conversion matrix, is the vector composed of the specific force information, is the gravity vector, The corresponding discrete result is the predicted position deviation .

[0024] Further, the formula for calculating the optimal position deviation in step S5 is:

[0025] ; wherein, is the predicted position deviation, is the absolute position deviation, is a weight coefficient, and the value range is .

[0026] Further, the value of the filter coefficient is 0.98, and the time step is set to 8 seconds, which is consistent with the update interval of the ultrasonic wave propagation time information.

[0027] In a second aspect, the present application provides a shaft heading machine attitude determination system, comprising a computer, a control box, an ultrasonic wave transmitter arranged on the shaft wall, and an ultrasonic wave receiver, a gyroscope and an accelerometer arranged on the load platform of the shaft heading machine; the computer is in communication connection with the control box, and the control box is in communication connection with the ultrasonic wave transmitter, the ultrasonic wave receiver, the gyroscope and the accelerometer respectively; the computer is configured to execute the shaft heading machine attitude determination method as described in any one of the preceding aspects.

[0028] The specific embodiments include the following modules: a data acquisition module, configured to control an ultrasonic transmitter and an ultrasonic receiver to acquire ultrasonic propagation time data, and to control an accelerometer and a gyroscope to acquire three-axis specific force information and three-axis angular velocity information of a carrier platform respectively; an absolute pose solving module, configured to calculate an absolute coordinate of the ultrasonic receiver according to the ultrasonic propagation time data, and to solve an absolute position deviation and a unit normal vector of the carrier platform according to the absolute coordinate, and further to calculate an absolute attitude angle; a predicted attitude solving module, configured to integrate the three-axis angular velocity information to obtain a high-frequency predicted attitude angle, and to calculate a low-frequency predicted attitude angle according to the three-axis specific force information; an attitude fusion module, configured to fuse the absolute attitude angle, the high-frequency predicted attitude angle and the low-frequency predicted attitude angle by using a fusion algorithm to obtain an optimal attitude angle, and to construct a coordinate system conversion matrix; and a position fusion module, configured to convert the three-axis specific force information to a first coordinate system by using the coordinate system conversion matrix and remove the influence of gravity, to obtain a predicted position deviation by integration, and to fuse the absolute position deviation and the predicted position deviation to output an optimal position deviation.

[0029] Further, the absolute pose solving module is configured to construct two non-collinear vectors by using an absolute coordinate of the ultrasonic receiver and a center coordinate of the carrier platform, to obtain a unit normal vector by cross-multiplying and normalizing the two non-collinear vectors, and to calculate an absolute roll angle, an absolute pitch angle and an absolute yaw angle by using inverse trigonometric functions according to components of the unit normal vector on different axes.

[0030] Further, the fusion algorithm formula executed by the attitude fusion module is:

[0031] ;

[0032] wherein when k is 1, represents an optimal roll angle, when k is 2, represents an optimal pitch angle, when k is 3, represents an optimal yaw angle, is a filter coefficient, is an absolute attitude angle, is the high-frequency predicted attitude angle, is the absolute attitude angle, is the low-frequency predicted attitude angle, when k is 3, the low-frequency yaw angle cannot be obtained according to the specific force information, so the weighted average of the absolute yaw angle and the low-frequency yaw angle cannot be used to correct the high-frequency yaw angle, and therefore the fusion algorithm formula executed by the attitude fusion module is modified as:

[0033] ;

[0034] Further, the position fusion module is configured to convert the three-axis specific force information to a first coordinate system by using the coordinate system conversion matrix and remove the influence of gravity by using a formula calculating the predicted position deviation , and by formula calculating the optimal position deviation ; wherein is the coordinate system conversion matrix, is the triaxial specific force information, is the gravity vector, is the absolute position deviation, is a weight coefficient.

[0035] Compared with the prior art, the present application has at least the following beneficial effects: (1) The present application adopts a posture determination method based on fusion of ultrasonic wave and inertial sensor data, effectively solving the technical problem that existing laser guiding technology is prone to failure in deep well high dust and high humidity environment. The present application establishes a first fixed coordinate system and a second carrier coordinate system, uses the strong penetration of ultrasonic wave in medium, and the physical characteristics of not being affected by light and mud blocking to obtain absolute position information, and combines the specific force information of the accelerometer and the angular velocity information of the gyroscope for auxiliary calculation. This combination of absolute measurement and inertial prediction not only overcomes the limitations of single sensor in harsh environment, but also realizes real-time, continuous and high robustness monitoring of the posture of the shaft boring machine through the complementation of multiple source information, ensuring the verticality accuracy of the excavation process.

[0036] (2) The present application significantly improves the geometric accuracy and calculation efficiency of the attitude solution through the space spherical intersection principle and vector geometric solution means. The present application uses the propagation time between multiple ultrasonic receivers and transmitters to calculate the distance, constructs a space sphere with the transmitter as the center and the propagation distance as the radius, and obtains the absolute coordinates of the receiver in the fixed coordinate system by solving the spherical intersection point. On this basis, the present application further adopts vector analysis means, constructs non-collinear vectors using receiver coordinates, and directly obtains the unit normal vector reflecting the inclination degree of the carrier platform through vector cross multiplication and normalization. This geometric solution method converts the complex space posture problem into intuitive vector operation, which can directly deduce the absolute roll angle, pitch angle and heading angle, and the multi-point measurement and averaging method effectively eliminates the random error of single-point measurement, greatly improving the accuracy of angle measurement.

[0037] (3) The application constructs a multi-source data deviation fusion model based on a coordinate system conversion matrix, and realizes high-precision optimal estimation of the position and attitude of the heading machine. The application uses the calculated attitude angle to construct a coordinate system conversion matrix, creatively converts the accelerometer specific force information in the carrier coordinate system to the fixed coordinate system, and removes the influence of the gravity component to obtain pure motion acceleration, and obtains the position prediction value by double integration. More importantly, the application adopts a specific attitude angle and position deviation fusion algorithm, and uses the low-frequency absolute observation value of the ultrasonic wave to periodically correct the high-frequency prediction value of the gyroscope and the accelerometer. This means not only takes advantage of the dynamic response and high bandwidth of the inertial sensor, but also uses the ultrasonic wave system to suppress the integral dispersion, thereby outputting optimal attitude data with dynamic performance and long-term accuracy.

[0038] (4) The application maximizes the stability and anti-interference ability of data processing through an optimized filtering weight coefficient and a timing control mechanism. The application specifically sets the filtering coefficient to 0.98, and strictly matches the time step of 8 seconds and the data update interval. This specific parameter setting method, on the one hand, gives the gyroscope prediction data a higher weight in the fusion algorithm, ensuring the smoothness of the attitude output and avoiding the severe disturbance of the observation noise to the system; on the other hand, the longer data update step cooperates with the time-sharing emission strategy, effectively solving the interference problems of ultrasonic wave multipath effect and echo aliasing in the long and narrow space of the shaft, ensuring the strict synchronization of the inertial data integration step and the absolute measurement update period, so that the system achieves the best running stability on the premise of ensuring the accuracy. BRIEF DESCRIPTION OF DRAWINGS

[0039] The accompanying drawings, which form a part of the specification, illustrate embodiments of the present application and, together with the description, serve to explain the principles of the present application.

[0040] Referring to the drawings, the present application can be more clearly understood in light of the following detailed description.

[0041] Figure 1 is a schematic diagram of the whole structure of the shaft heading machine attitude determination system provided by the embodiment of the application.

[0042] Figure 2 is a schematic diagram of the layout of the attitude acquisition device and the ultrasonic receiver on the carrier platform provided by the embodiment of the application.

[0043] Figure 3 is a projection schematic diagram for calculating the roll angle according to the specific force information measured by the accelerometer in the embodiment of the application.

[0044] Figure 4 is a schematic diagram for calculating the pitch angle according to the specific force information measured by the accelerometer in the embodiment of the application.

[0045] Figure 5 is a schematic diagram of the principle of calculating the coordinates of the ultrasonic receiver according to ultrasonic ranging in the embodiment of the application.

[0046] Figure 6 is a Y-Z plane projection diagram of calculating the roll angle according to ultrasonic ranging in the embodiment of the application.

[0047] Figure 7 is an X-Z plane projection diagram of calculating the pitch angle according to ultrasonic ranging in the embodiment of the application.

[0048] Figure 8 is an X-Y plane projection diagram of calculating the heading angle according to ultrasonic ranging in the embodiment of the application.

[0049] Figure 9 is a flow chart of the attitude determination method provided by the embodiment of the application.

[0050] BRIEF DESCRIPTION OF DRAWINGS: 1, ultrasonic transmitter; 2, computer; 3, support platform; 4, ultrasonic receiver; 5, object carrying platform; 6, control box; 7, gyroscope; 8, accelerometer. DETAILED DESCRIPTION

[0051] The technical solutions of the application will be described in detail below with the aid of the drawings and specific embodiments. It should be understood that the specific features in the embodiments and the specific features in the embodiments are detailed descriptions of the technical solutions of the application, and not limitations of the technical solutions of the application. In the case of no conflict, the technical features in the embodiments and the technical features in the embodiments can be combined with each other. The following embodiments are only used to more clearly illustrate the technical solutions of the application, and cannot limit the protection scope of the application.

[0052] The term "and / or" in this paper is only a description of the association relationship between the associated objects, which means that there can be three relationships, for example, A and / or B, which can represent the three cases of A alone, A and B together, and B alone. In addition, the character " / " in this paper generally represents that the associated objects before and after are an "or" relationship.

[0053] As shown in Figure 9 , the embodiment provides a shaft tunneling machine attitude determination method, which specifically comprises the following steps: first, the computer 2 establishes a first coordinate system for describing the spatial position and a second coordinate system following the movement of the shaft tunneling machine. Specifically, the first coordinate system takes the center point of the shaft portal as the origin, the vertically downward direction as the positive direction of the Z axis, the X axis positive direction horizontally points to the right side, and the Y axis is perpendicular to the X axis and the Z axis. The coordinate system is fixed and does not change, and the ideal attitude is defined in the coordinate system, that is, the initial position deviation is denoted as , and the initial roll angle, pitch angle and heading angle are denoted as The second coordinate system has its origin at the center of the loading platform 5, with the Z-axis pointing downwards along the central rigid column axis. The X and Y axes are initially aligned with the X and Y axes of the first coordinate system. This coordinate system changes as the tunneling machine moves and rotates. During the installation of the accelerometer 8 and the gyroscope 7, the three axes of the accelerometer 8 and the gyroscope 7 should be aligned with the three axes of the first coordinate system.

[0054] Subsequently, data acquisition is performed. The control box 6 sends detection commands to the accelerometer 8 and the gyroscope 7 according to a predetermined sequence. The accelerometer 8 acquires the sum of the motion acceleration of the platform 5 and the projection of the gravity vector in the second coordinate system within a data update interval, i.e., the specific force information, specifically including... , , There are four groups in total. The gyroscope 7 collects the instantaneous rotational speed, i.e., angular velocity information, of the platform 5 relative to its own axis within a data update interval, specifically including... , , There are four groups in total.

[0055] Simultaneously, the control box 6 sequentially triggers the four ultrasonic transmitters 1 installed on the well wall to emit ultrasonic waves downwards according to a predetermined preferred timing sequence. Taking one ultrasonic transmitter 1 as an example ( The control box 6 is at a certain time. Upon receiving the command, the ultrasonic transmitter 1 transmits a signal to the three ultrasonic receivers 4 (numbered 4) mounted on the circumferential support of the loading platform 5. Receive signals and retain only the first valid signal received, recording the reception time. To avoid interference from the previous transmission to the next measurement, this embodiment preferably uses a transmission interval of 2 seconds. That is, after confirming that all three ultrasonic receivers 4 have completed signal reception, there is a delay of approximately 2 seconds before triggering the next ultrasonic transmitter 1. The above process is repeated once to obtain a total of [number missing] measurements. This is a time data point, therefore the data update interval for the entire system is [number] times. The time is 8 seconds. After data acquisition is completed, the control box 6 preprocesses the data. The control box 6 takes the average of the four sets of force information and four sets of angular velocity information collected, and uses the average force as the ratio. and mean angular velocity This represents the data within that time interval. Then, the formula is used... Calculate the first The ultrasonic receiver 4 and the first The distance between the ultrasonic transmitters 1 ,in is the sound velocity, is the propagation time.

[0056] As Figure 5 shown, the distance is used to construct a spatial sphere with the ultrasonic transmitter 1 as the center of the sphere in the first coordinate system. For a single ultrasonic receiver 4, four spatial spheres intersect at a point in space, based on which the absolute coordinates of the first ultrasonic receiver 4 in the second measurement are calculated . Then, the current coordinates of the center of the object platform 5 are calculated using the mean value based on the absolute coordinates of the three ultrasonic receivers 4 . The absolute position deviation of the object platform 5 is calculated by subtracting the initial platform coordinates from the current coordinates .

[0057] As Figure 6 , Figure 7 , Figure 8 shown, two non-collinear vectors and are constructed based on the absolute coordinates of the ultrasonic receivers 4 and the current coordinates of the center of the object platform 5. The cross product of the two non-collinear vectors is calculated and normalized to obtain a unit normal vector perpendicular to the object platform 5. In the ideal vertical state, the unit normal vector should point straight down, and its deflection directly reflects the inclination of the object platform 5. The absolute attitude angles are calculated based on the components of the unit normal vector: as shown, the absolute roll angle ; as Figure 6 shown, the absolute pitch angle ; as Figure 7 shown, the absolute heading angle , where Figure 8 is the angle between the initial installation position of the ultrasonic receiver 4 and the X-axis, which is a fixed value. At the same time, the three-axis angular velocity information collected by the gyroscope 7 is integrated to obtain high-frequency predicted attitude angles, i.e., roll angle , pitch angle , and heading angle .

[0058] As Figure 3 , Figure 4 shown, the specific force information ​​​​The low-frequency predicted attitude angle is calculated using the following formula: Low-frequency roll angle. Low-frequency pitch angle .

[0059] Next, the optimal attitude angle is calculated using an attitude angle fusion algorithm. The fusion formula is:

[0060] When k is 1, This represents the optimal roll angle; when k is 2, This represents the optimal pitch angle; when k is 3, Represents the optimal heading angle; The filter coefficient is preferably set to 0.98 in this embodiment, and is used to determine the weight of the gyroscope prediction data and the absolute observation data; The time step is set to 8 seconds, consistent with the ultrasonic data update interval, in this embodiment. When k=3, the low-frequency heading angle cannot be obtained from the force ratio information, thus the weighted average of the absolute heading angle and the low-frequency heading angle cannot be used to correct the high-frequency heading angle. Therefore, the fusion algorithm formula executed by the attitude fusion module is modified as follows: Based on the obtained optimal attitude angle Construct coordinate system transformation matrix :

[0061] ;in, represent , represent Using the coordinate system transformation matrix The force information After transforming to the first coordinate system and removing the influence of gravity, the predicted position deviation is obtained through double integration. The formula is: .

[0062] Finally, the predicted position deviation With respect to the absolute position deviation Perform fusion and calculate the optimal positional deviation. The formula is: ;in The weighting coefficient has a range of values. In this embodiment, the preferred embodiment is... The system repeats the above process to... The time interval outputs the optimal position deviation The optimal attitude angle is also displayed through the data software interface of the computer 2.

[0063] like Figure 1As shown in the figure, as one embodiment, the present invention also provides a shaft tunneling machine attitude determination system, applicable to the shaft tunneling machine attitude determination method described above. It mainly includes a computer 2, a control box 6, an ultrasonic transmitter 1 mounted on the shaft wall, and an ultrasonic receiver 4, a gyroscope 7, and an accelerometer 8 mounted on the shaft tunneling machine's carrying platform 5. The support platform 3 has a multi-layer structure, and in terms of height, the support platform 3 is located above the carrying platform 5. The carrying platform 5 is used to install an attitude acquisition module, which includes the accelerometer 8, the gyroscope 7, and the control box 6. The control box 6 is communicatively connected to the accelerometer 8, the gyroscope 7, the ultrasonic transmitter 1, and the ultrasonic receiver 4 via a bus. The computer 2 is installed on the top layer of the support platform 3 and connected to the control box 6 via a bus, used for real-time data acquisition and attitude calculation.

[0064] like Figure 2 As shown, to obtain the absolute position information of the tunneling machine, the number of ultrasonic transmitters 1 is set to be greater than or equal to 4, preferably 4 in this embodiment, and they are kept at the same height and installed on the laid well wall. To obtain the absolute orientation information of the tunneling machine in space, the number of ultrasonic receivers 4 is set to be greater than or equal to 2, preferably 3 in this embodiment. The ultrasonic receivers 4 are circumferentially distributed and installed on the edge of the loading platform 5, and are tightly connected to the loading platform 5.

[0065] This system employs a circumferential distribution, mounting the ultrasonic receivers 4 along the edge of the platform. This layout maximizes the baseline distance between receivers, resulting in more precise intersection points of the constructed spatial spheres and thus improving the sensitivity of normal vector calculation. Combined with the ultrasonic transmitters 1 fixed to the well wall, the system forms a stable transceiver array. Integrating with a control box and computer terminal containing preprocessing functions, it achieves integrated operation from data acquisition and signal denoising to attitude display, providing a hardware solution for deep well excavation projects that is highly resistant to interference and easy to install and maintain.

[0066] The above description is only a preferred embodiment of 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 method for determining the attitude of a vertical shaft tunneling machine, characterized in that, Includes the following steps: Step S1: Establish a first coordinate system to describe the spatial position and a second coordinate system to follow the movement of the shaft tunneling machine, and obtain ultrasonic wave propagation time information, angular velocity information of the loading platform, and specific force information; Step S2: Based on the ultrasonic wave propagation time information, calculate the absolute coordinates of the ultrasonic receiver in the first coordinate system, and then calculate the absolute position deviation of the platform and the unit normal vector reflecting the tilt of the platform. Calculate the absolute attitude angle of the platform based on the unit normal vector. Step S3: Integrate the angular velocity information to obtain the high-frequency predicted attitude angle, and calculate the low-frequency predicted attitude angle based on the specific force information; Step S4: Calculate the optimal attitude angle using the optimal attitude angle fusion algorithm based on the absolute attitude angle, the high-frequency predicted attitude angle, and the low-frequency predicted attitude angle, and construct a coordinate system transformation matrix based on the optimal attitude angle; Step S5: Use the coordinate system transformation matrix to transform the specific force information to the first coordinate system and remove the influence of gravity. Obtain the predicted position deviation through integration. Combine the predicted position deviation with the absolute position deviation obtained in step S2 to output the optimal position deviation. The specific formula for the optimal attitude angle fusion algorithm in step S4 is as follows: ； Where k takes the values ​​1, 2, or 3, These represent the optimal roll angle, optimal pitch angle, and optimal yaw angle, respectively. These are the filter coefficients; This is the absolute attitude angle; The high-frequency predicted attitude angle; For time step; Angular velocity; the filter coefficients The value is 0.98, and the time step is... The interval is set to 8 seconds, consistent with the update interval of the ultrasonic propagation time information. When k=3, the low-frequency heading angle cannot be obtained from the force ratio information, thus the weighted average of the absolute heading angle and the low-frequency heading angle cannot be used to correct the high-frequency heading angle. Therefore, the optimal attitude angle fusion algorithm formula is modified as follows: ； The coordinate system transformation matrix constructed in step S4 Specifically: ； in, These are the optimal roll angle, optimal pitch angle, and optimal heading angle obtained in step S4, respectively. Represents the cosine function , Represents the sine function ; In step S5, the predicted position deviation is calculated. The formula is: ； in, The coordinate system transformation matrix is... The vector composed of the force information. The gravity vector The corresponding discrete result is the predicted position deviation. ; In step S5, the optimal position deviation is calculated. The formula is: ； in, The predicted position deviation, The absolute position deviation is... The weighting coefficient has a range of values. .

2. The method for determining the attitude of a vertical shaft tunneling machine according to claim 1, characterized in that, The specific method for establishing the coordinate system in step S1 is as follows: the first coordinate system takes the center point of the shaft opening as the origin, the vertical downward direction as the positive Z-axis, the positive X-axis pointing horizontally to the right, and the Y-axis perpendicular to the X-axis and Z-axis; the second coordinate system takes the center of the loading platform as the origin, the Z-axis direction is downward along the central rigid column axis, and the X-axis and Y-axis directions are initially consistent with the X-axis and Y-axis directions in the first coordinate system.

3. The method for determining the attitude of a vertical shaft tunneling machine according to claim 1, characterized in that, The process of calculating the absolute position deviation and absolute attitude angle of the platform in step S2 includes: According to the formula Calculate the first The ultrasonic receiver and the first The distance between the ultrasonic transmitters ,in For the speed of sound, For the first The ultrasonic transmitter transmits a signal to the first... The signal propagation time received by each ultrasonic receiver; With the known coordinates of the ultrasonic transmitter as the center, the distance Construct a spatial sphere with radius, and calculate the first sphere using the principle of spherical intersection. The absolute coordinates of the ultrasonic receivers in the first coordinate system Where n represents the nth measurement, such as Represents the absolute coordinates of the j-th receiver in the first coordinate system at the initial moment; The current coordinates of the platform center are obtained by calculating the average of the absolute coordinates of all ultrasonic receivers. , the current coordinates with initial platform coordinates The difference is calculated to obtain the absolute position deviation. .

4. The method for determining the attitude of a vertical shaft tunneling machine according to claim 3, characterized in that, The calculation of the absolute attitude angle in step S2 specifically includes: based on the absolute coordinates of the ultrasonic receiver. and the current coordinates of the center of the cargo platform Construct two non-collinear vectors The non-collinear vectors are cross-producted and normalized to obtain a unit normal vector perpendicular to the platform. The absolute attitude angle is calculated based on the components of the unit normal vector, using the following formula: Absolute roll angle ; Absolute pitch angle ; Absolute heading angle ; in, For vectors Components on the X and Y axes The angle between the initial installation position of the receiver and the X-axis of the second coordinate system is a fixed value.

5. The method for determining the attitude of a vertical shaft tunneling machine according to claim 1, characterized in that, The specific formula for calculating the low-frequency predicted attitude angle in step S3 is as follows: Low-frequency roll angle ; Low-frequency pitch angle ; in, These are the components of the specific force information on the three axes of the second coordinate system.

6. A shaft boring machine attitude determination system, characterized in that, It includes a computer, a control box, an ultrasonic transmitter installed on the shaft wall, and an ultrasonic receiver, gyroscope, and accelerometer installed on the cargo platform of the shaft tunneling machine. The computer is communicatively connected to the control box, and the control box is communicatively connected to the ultrasonic transmitter, the ultrasonic receiver, the gyroscope, and the accelerometer, respectively. The computer is configured to perform the shaft boring machine attitude determination method as described in any one of claims 1 to 5.

Images