Boring machine pose measurement and control method and system based on bidirectional vision

Abstract

The application relates to a tunneling machine pose measurement and control method and system based on bidirectional vision. The system used by the method comprises a base station end measurement system in a roadway, a mobile end measurement system on a tunneling machine body and a motion control module, and the method comprises the following steps: S1, establishing a base station end coordinate system; S2, establishing a mobile end coordinate system; S3, the base station end identifies the three-dimensional position information of a mobile end feature light source in the base station end coordinate system through a binocular vision sensor; S4, the mobile end identifies the vector information of a base station end laser beam through a vision sensor; S5, performing combined navigation calculation based on an EKF algorithm; and S6, performing pose control based on the combined navigation calculation result. The application adopts a method combining bidirectional vision measurement and inertial measurement, and reduces the cost demand for a high-precision inertial measurement unit.

Description

Technical Field

[0001] This invention relates to the field of tunneling machine positioning, navigation and control technology; specifically, this invention relates to a method and system for tunneling machine posture measurement and control based on bidirectional vision. Background Technology

[0002] Coal still occupies a major proportion of my country's energy structure, and this situation is unlikely to change in the short term. As the most important type of mining machinery for integrated underground tunneling in coal mines, cantilever roadheaders are widely used in various tunnel excavations. However, they face challenges such as harsh working environments at the tunneling face, high labor intensity, and stringent requirements for directional controllability. Therefore, intelligent and automated integrated tunneling technology is an urgent need for the development of tunnel excavation.

[0003] In the development of intelligent integrated tunneling, accurate measurement of the position and attitude of cantilever tunneling machines is a primary issue, directly determining the accuracy of the tunnel direction and the quality of tunnel formation. Existing technologies mainly rely on communication ranging and inertial navigation. For example, one method uses millimeter-wave radars installed on both ends of the tunneling machine to measure the distance between the coal walls and the sides, calculating the three-dimensional attitude correction. This method requires the coal walls to be vertical, smooth, and strictly aligned with a specified straight line; otherwise, errors in the millimeter-wave radar ranging will lead to errors in the attitude correction calculation. Another method relies on gyro total stations and inertial navigation equipment to achieve high-precision estimation of the tunneling machine's position and attitude, but this method suffers from extremely high hardware costs. Summary of the Invention

[0004] In view of this, the present invention provides a method and system for measuring and controlling the position and posture of a tunneling machine based on bidirectional vision, thereby solving or at least alleviating one or more of the above-mentioned problems and other problems existing in the prior art.

[0005] To achieve the aforementioned objectives, a first aspect of the present invention provides a method for measuring and controlling the pose of a tunneling machine based on bidirectional vision. The method utilizes a tunneling machine pose measurement and control system, which includes a base station measurement system, a mobile measurement system, and a motion control module. The base station measurement system is installed inside the tunneling machine's operating tunnel, and the mobile measurement system is installed on the tunneling machine's body. The base station measurement system includes a binocular vision sensor and a laser light, and the mobile measurement system includes a vision sensor, a feature light source, and an inertial measurement unit. The method includes the following steps: Step S1: Establish a base station coordinate system using the base station measurement system as a reference. Step S2: Using the mobile terminal measurement system as a reference, establish a mobile terminal coordinate system; Step S3: The binocular vision sensor is used to identify and measure the three-dimensional position information of the feature light source in the coordinate system of the base station. The three-dimensional position information includes the vector information of the feature light source in the coordinate system of the base station and the distance information between the feature light source and the binocular vision sensor. Step S4: The visual sensor is used to identify and measure the vector information of the laser beam emitted by the laser lamp in the coordinate system of the mobile terminal; Step S5: Based on the three-dimensional position information, the vector information of the laser beam in the coordinate system of the mobile terminal, and the measurement data of the inertial measurement unit, a combined navigation solution is performed based on the extended Kalman filter algorithm to obtain the optimal estimate of the tunneling machine's pose. Step S6: The motion control module calculates the pose deviation by combining the optimal estimated pose and the real-time planned pose data, and performs pose control.

[0006] In the method described above, optionally, step S1 includes: the base station coordinate system takes the direction of the laser beam emitted by the laser light as the Z-axis, and the binocular vision sensor coordinate system is aligned with the base station coordinate system through calibration; Step S2 includes: the mobile terminal coordinate system takes the installation position of the inertial measurement unit as the origin, the rightward direction of the tunneling machine as the X-axis, and the forward direction of the tunneling machine as the Y-axis. The X-axis, Y-axis, and Z-axis of the mobile terminal coordinate system conform to the right-hand screw rule. The coordinate system of the vision sensor is consistent with the mobile terminal coordinate system through calibration.

[0007] In the method described above, step S5 may optionally include: Step S51: The inertial measurement unit is used to perform navigation calculation on the tunneling machine to obtain real-time navigation calculation results. In the extended Kalman filter algorithm framework, the pose strapdown calculation based on inertial measurement data is used as the state recursion process of the extended Kalman filter. Step S52: Based on the three-dimensional position information, construct the first set of observations under the extended Kalman filter algorithm framework; Step S53: Based on the vector information of the characteristic light source in the coordinate system of the base station and the vector information of the laser beam in the coordinate system of the mobile terminal, construct the second set of observations under the extended Kalman filter algorithm framework; Step S54: Under the extended Kalman filter algorithm framework, after the first set of observation corrections and the second set of observation corrections, the real-time optimal pose estimation result of the tunneling machine is obtained.

[0008] In the method described above, optionally, in step S51, the real-time navigation solution result includes real-time position information, real-time speed information, and real-time attitude information.

[0009] In the method described above, optionally, in step S51, the pose strapdown calculation based on inertial measurement data includes: Construct the attitude quaternion state update equation: , in, The attitude quaternion needs to be valued. This is data measured by a three-axis gyroscope. To measure the zero bias of a three-axis gyroscope, Noise measurement for a three-axis gyroscope; Construct the velocity state update equation: , in, The estimated value of the three-dimensional velocity in the mobile coordinate system is as follows. For triaxial accelerometer measurement data, To estimate the zero bias value for triaxial accelerometer measurements, Noise measurement for triaxial accelerometers; Construct the position state update equation: , in, The estimated value is the three-dimensional position in the mobile terminal coordinate system. Construct the sensor error state update equation: ; Step S52 includes: Construct observation equations based on location information: , in, The measurement value of the location of the mobile terminal measurement system by the base station measurement system. Noise for position measurement; Step S53 includes: Construct attitude observation equations based on vector observation information: , in, This refers to the unit vector measurement information of the mobile terminal measurement system in the coordinate system of the base station. This refers to the unit vector measurement information of the origin of the laser beam in the coordinate system of the moving end. This is vector observation noise; Step S54 includes: Assume the state equation and measurement equation after linear discretization are as follows: , , in, The current state. For the state at the next moment, Here is the state transition matrix. For noise driving matrix, State noise, For measurement value, For the measurement matrix, For measuring noise; Through the state noise variance matrix and measurement noise variance matrix Describe the statistical characteristics of the state noise and the measurement noise: ; The state transition matrix is ​​obtained after discretization. The noise driving matrix The measurement matrix Combining the state noise matrix and the measurement noise matrix, the Kalman filter algorithm process is as follows: Based on the current state, predict the state at the next moment. The predicted state is: , The uncertainty of the predicted state is estimated based on the error covariance matrix, where the error covariance matrix is: Calculate Kalman gain , The predicted state is corrected to obtain the optimal state estimate. , By correcting the error covariance matrix, the optimal covariance estimate is obtained. ,in It is an identity matrix.

[0010] To achieve the aforementioned objectives, a second aspect of the present invention provides a bidirectional vision-based tunneling machine pose measurement and control system using the method described in any one of the first aspects above. The system includes a base station measurement system, a mobile measurement system, and a motion control module. The base station measurement system is installed inside the tunnel where the tunneling machine operates, and the mobile measurement system is installed on the tunneling machine body. The base station measurement system includes a binocular vision sensor and a laser light, and the mobile measurement system includes a vision sensor, a feature light source, and an inertial measurement unit. The laser light is within the field of view of the visual sensor, and the feature light source is within the field of view of the binocular visual sensor.

[0011] In the system described above, optionally, the base station measurement system is installed at the center line of the roof of the roadway, and the mobile measurement system is installed at the rear of the tunneling machine.

[0012] Optionally, in the system described above, the base station measurement system includes a visual feature plate positioned adjacent to the binocular vision sensor.

[0013] In the system described above, optionally, the feature light source and the mobile terminal measurement system are arranged separately at different positions after the lever measurement.

[0014] In the system described above, optionally, the base station measurement system includes an inclinometer, which, along with the laser light, adjusts the coordinate system of the binocular vision sensor to coincide with the coordinate system of the base station. Both the base station measurement system and the mobile measurement system include a communication data transmission device and a data processing and computing unit. The communication data transmission device is used for data transmission between the base station measurement system and the mobile measurement system, and the data processing and computing unit is used for data processing and calculation when performing tunneling machine pose measurement.

[0015] The present invention provides a tunneling machine pose measurement and control method based on bidirectional vision, which reduces the cost requirement for high-precision inertial measurement units by combining bidirectional vision with inertial measurement.

[0016] The present invention further provides a tunneling machine position and posture measurement and control system based on bidirectional vision, and therefore the system also has the above-mentioned advantages. Attached Figure Description

[0017] The disclosure of this invention will become more apparent from the accompanying drawings. It should be understood that these drawings are for illustrative purposes only and are not intended to limit the scope of protection of this invention. In the drawings: Figure 1 This is a schematic diagram of an embodiment of the tunneling machine posture measurement and control system based on bidirectional vision of the present invention; Figure 2 for Figure 1 A schematic diagram of the base station measurement system in the diagram; Figure 3 for Figure 1 A schematic diagram of the mobile measurement system in the diagram; and Figure 4 This is a schematic diagram of the combined navigation process based on the fusion of two-way vision and inertial navigation, which is an embodiment of the tunneling machine pose measurement and control method based on two-way vision of the present invention.

[0018] Reference numerals: 1-Base station measurement system; 2-Mobile measurement system; 3-Binocular vision sensor; 4-Laser light; 5-Inclinometer; 6-First data processing and calculation unit; 7-First communication and data transmission device; 8-Vision sensor; 9-Feature light source; 10-Inertial measurement unit; 11-Second communication and data transmission device; 12-Second data processing and calculation unit. Detailed Implementation

[0019] Referring to the accompanying drawings and specific embodiments, the structure, composition, features, and advantages of the tunneling machine posture measurement and control method and system of the present invention will be described below by way of example. However, all descriptions should not be construed as limiting the present invention in any way.

[0020] Furthermore, for any single technical feature described or implied in the embodiments mentioned herein, or any single technical feature shown or implied in the various figures, the present invention still allows for any combination or deletion of these technical features (or their equivalents) without any technical obstacle, and thus these further embodiments according to the present invention should also be considered within the scope of this description.

[0021] It should also be noted that the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined with "first" or "second" may explicitly or implicitly include at least one of those features.

[0022] Figure 1 This is a schematic diagram of an embodiment of the tunneling machine posture measurement and control system based on bidirectional vision of the present invention.

[0023] Figure 1 The diagram shows the tunneling machine, base station measurement system 1, mobile measurement system 2, base station coordinate system O-XnYnZn, and mobile coordinate system O-XbYbZb.

[0024] The tunneling machine operates in an underground coal mine roadway (not shown in the diagram), such as... Figure 1 As shown, the tunneling machine in this embodiment is a cantilever tunneling machine widely used in various roadway excavations. In an optional embodiment, the tunneling machine posture measurement and control system based on bidirectional vision of the present invention can also be applied to other types of tunneling machines, such as full-face tunneling machines.

[0025] like Figure 1As shown, the base station measurement system 1 is installed inside the tunnel. The base station coordinate system O-XnYnZn, with the base station measurement system 1 as the reference, can serve as the tunnel reference coordinate system. In an optional embodiment, the base station measurement system 1 is installed at the centerline of the tunnel roof to provide a better field of view during the observation of the tunnel boring machine without obstructing other equipment within the tunnel. Exemplarily, the front axis Zn of the base station coordinate system faces the direction of the tunnel boring machine, and the Xn and Yn axes lie on the tunnel cross-sectional plane, with their directions being horizontal and vertical, respectively.

[0026] like Figure 1 As shown, the mobile measurement system 2 is mounted on the body of the tunneling machine, and the mobile coordinate system O-XbYbZb, with the mobile measurement system 2 as the reference, is consistent with the tunneling machine's coordinate system. In an optional embodiment, the mobile measurement system 2 is mounted at the rear of the tunneling machine to facilitate mutual observation with the base station measurement system 1. In an optional embodiment, the mobile measurement system 2 can also be mounted on the top of the tunneling machine or other easily observable locations, depending on the tunneling machine model, roadway environment, etc. Exemplarily, the mobile coordinate system takes the tunneling machine's inertial navigation system mounting position as the origin, the rightward direction of the tunneling machine as the Xb axis, the forward direction of the tunneling machine as the Yb axis, and then determines the Zb axis using the right-hand screw rule.

[0027] like Figure 1 As shown by the bidirectional arrow between the base station measurement system 1 and the mobile measurement system 2, the base station measurement system 1 and the mobile measurement system 2 collect each other's visual information.

[0028] The system also includes Figure 1 The motion control module of the tunneling machine (not shown) can be integrated into the control system of the tunneling machine. It uses the position and posture information of the tunneling machine obtained by the base station measurement system 1 and the mobile terminal measurement system 2, combined with the optimal estimation of the position and posture data and the real-time planned position and posture data to calculate the position and posture deviation, drive the execution structure of the tunneling machine, and perform precise position and posture control of the tunneling machine.

[0029] Figure 2 for Figure 1 A schematic diagram of the base station measurement system.

[0030] Figure 2 The diagram shows the binocular vision sensor 3, laser light 4, inclinometer 5, first data processing and calculation unit 6, and first communication data transmission device 7 of the base station measurement system 1.

[0031] The binocular vision sensor 3 is a high-precision direction-finding vision sensor used to identify and measure the three-dimensional position information of the feature light source 9 of the mobile terminal measurement system 2. This three-dimensional position information includes both vector and distance information. The installation position and orientation of the binocular vision sensor 3 must ensure that the feature light source 9 of the mobile terminal measurement system 2 is within its field of view. In optional embodiments, a binocular vision sensor with a suitable field of view can be selected, and an appropriate installation direction and angle can be set. For example, while ensuring that the feature light source 9 is simultaneously within the field of view of both cameras in real time, the installation angle of the vision sensor can be adjusted so that the feature light source 9 is close to the center position within the field of view of both cameras. Furthermore, a binocular vision sensor with a sufficient but not excessive field of view can be selected to reduce useless areas in the acquired visual information, thereby improving recognition efficiency. The coordinate system of the binocular vision sensor 3 is adjusted by the inclinometer 5 and the laser light 4 to coincide with the coordinate system of the inclinometer 5 and also with the base station coordinate system O-XnYnZn, reducing computational complexity.

[0032] The laser light 4 is mounted facing the mobile measurement system 2, and the mounting method ensures that the emitted laser beam can be used by the mobile measurement system 2 to acquire visual information. In an optional embodiment, the front axis Zn of the base station coordinate system is precisely calibrated to ensure that the laser beam is aligned. Compared with ordinary light sources, lasers have better monochromaticity and directionality, higher brightness, and are easier to be visually recognized.

[0033] The first data processing and calculation unit 6 is a built-in calculation unit of the base station measurement system 1, used to perform data processing and calculation within the base station measurement system 1, such as executing a binocular vision positioning algorithm. Then, the first communication data transmission device 7 communicates with the mobile terminal and sends the aforementioned three-dimensional position information to the mobile terminal measurement system 2.

[0034] like Figure 2 As shown, the first communication data transmission device 7 can be installed on the opposite side of the lens direction of the first vision sensor 3 to avoid obstructing its field of view. The inclinometer 5 and the first data processing and calculation unit 6 can be built into the main body of the base station measurement system 1 to reduce space occupation and protect them from external environmental interference and damage.

[0035] Furthermore, the base station measurement system 1 also includes a visual feature plate, which is located next to the binocular vision sensor 3, to help the mobile terminal perform camera calibration.

[0036] Figure 3 for Figure 1 A schematic diagram of the mobile measurement system.

[0037] Figure 3 The mobile measurement system 2 includes a vision sensor 8, a feature light source 9, an inertial measurement unit 10, a second communication data transmission device 11, and a second data processing and computing unit 12.

[0038] The visual sensor 8 is used to identify the laser beam emitted by the laser light 4 of the measurement system 1 at the base station, and calculate the vector information of its point and line features in the mobile terminal coordinate system. This vector information can be combined with the vector information of the mobile terminal feature light source 9 in the base station coordinate system, increasing the attitude correction information of the tunneling machine. This can curb the heading divergence trend in long-term low-dynamic scenarios of integrated navigation, improve the attitude estimation accuracy and stability of the tunneling machine over a long period of time, and reduce the cost requirement for high-precision inertial measurement units. The installation position and orientation of the visual sensor 8 must ensure that the laser light 4 is within its field of view and that it can capture and identify the point and line features of the laser beam. In optional embodiments, similar to the optional selection and installation scheme of the aforementioned binocular visual sensor 3, provided that the field of view of the visual sensor 8 meets the aforementioned requirements, a visual sensor with a relatively small field of view can be selected to reduce useless areas in the collected visual information, thereby improving recognition efficiency. The coordinate system of the visual sensor 8 is precisely aligned with the aforementioned mobile terminal coordinate system O-XbYbZb to reduce computational complexity. The visual sensor 8 can be calibrated using the aforementioned visual feature plate at the base station.

[0039] The characteristic light source 9 is oriented towards the base station measurement system 1, and this light source is within the field of view of the binocular vision sensor 3. For example... Figure 3 As shown, the feature light source 9 in this embodiment is a visual feature lamp. Optionally, the visual feature lamp and the main body of the mobile measurement system 2 can be arranged separately after the precise lever measurement.

[0040] The inertial measurement unit 10 can perform independent navigation calculations for the tunnel boring machine (TBM), and perform high-frequency attitude strapdown calculations based on inertial measurement data. These calculations are then used in conjunction with visual sensor measurements and communication ranging for integrated navigation calculations. The real-time navigation results include the TBM's real-time position, real-time speed, and real-time attitude information. The coordinate system of the inertial measurement unit 10 is the aforementioned mobile terminal coordinate system O-XbYbZb. As mentioned earlier, bidirectional visual measurement can improve the accuracy and stability of the TBM's attitude estimation over long periods, reducing the cost requirement for high-precision inertial measurement units. Therefore, low-cost inertial measurement units can be selected, significantly reducing costs compared to existing technologies based on gyro total stations / high-precision inertial navigation combined measurements.

[0041] The second communication data transmission device 11 can communicate with the first communication data transmission device 7 of the base station measurement system 1 to receive information such as the three-dimensional position of the mobile terminal in the base station coordinate system.

[0042] The second data processing and calculation unit 12 uses the information sent by the base station measurement system 1 and the information measured by the vision sensor 8 and the inertial measurement unit 10 to perform integrated navigation calculation based on the extended Kalman filter (EKF) algorithm.

[0043] like Figure 3 As shown, the second communication data transmission device 11 can be installed on the opposite side of the vision sensor 8's lens to avoid obstructing its field of view. The inertial measurement unit 10 and the second data processing and calculation unit 12 can be built into the main body of the mobile measurement system 2, reducing space occupation and protecting it from external environmental interference and damage.

[0044] An embodiment of the tunnel boring machine pose measurement and control method based on bidirectional vision of the present invention, using the above-described system, includes the following steps: Step S1: Establish the base station coordinate system; Step S2: Establish the mobile coordinate system; Step S3: The feature light source 9 is identified and measured by the binocular vision sensor 3, and the three-dimensional position information of the feature light source 9 in the coordinate system of the base station is calculated. Step S4: The laser beam emitted by the laser lamp 4 is identified and measured by the vision sensor 8, and the vector information of the laser beam in the coordinate system of the mobile end is calculated. Step S5: Based on the above three-dimensional position information, vector information and measurement data of the inertial measurement unit 10, the combined navigation solution is performed based on the extended Kalman filter (EKF) algorithm to obtain the optimal estimate of the tunneling machine's pose. Step S6: The tunneling machine motion control module performs position control based on the above position information.

[0045] In step S1, the base station coordinate system O-XnYnZn is the tunnel reference coordinate system. With the base station measurement system 1 as the reference object, the front axis Zn is aligned with the direction of the laser beam emitted by the laser lamp 4. Through precise calibration, the coordinate systems of the binocular vision sensor 3 and the inclinometer 5 are both aligned with the base station coordinate system, reducing computational complexity.

[0046] In step S2, the coordinate system O-XbYbZb of the inertial measurement unit 10 built into the mobile measurement system 2 can be used as the mobile coordinate system. This coordinate system has its origin O at the installation position of the inertial measurement unit 10, with the Xb axis pointing to the right of the tunneling machine, the Yb axis pointing forward, and the Zb axis following a right-hand screw rule with the Xb and Yb axes. The coordinate system of the vision sensor 8 is precisely calibrated to coincide with it, and both are kept consistent with the tunneling machine's coordinate system, reducing computational complexity.

[0047] In step S3, the binocular vision sensor 3 identifies the 3D position information of the target (feature light source 9) based on a binocular vision algorithm, thereby obtaining the 3D position information of the mobile terminal in the base station coordinate system. Specifically, the intrinsic and extrinsic parameters and baseline distance of the two cameras are obtained in advance through calibration; during the identification process, two images are captured simultaneously and distortion correction is performed to align the two images on the same plane; then, corresponding points are found in the two images to obtain the disparity of each pixel; the depth (distance) information of the target is calculated based on the principle of triangulation, and then the 3D position of the target is calculated. Compared with a monocular camera, a binocular camera can not only obtain the target vector information, but also obtain the target depth information based on the disparity, thereby obtaining the 3D position of the target.

[0048] In step S4, the laser beam point and line vector information of the base station in the mobile terminal coordinate system is measured in reverse by the mobile terminal vision sensor 8. Combined with the vector information of the mobile terminal in the base station coordinate system, the attitude correction information of the tunneling machine is increased. This can curb the heading divergence trend in long-term low-dynamic scenarios of integrated navigation, improve the attitude estimation accuracy and stability of the tunneling machine over a long period of time, and reduce the cost requirement for high-precision inertial measurement units.

[0049] In step S5, the specific steps of the integrated navigation solution are as follows: Figure 4 As shown. Figure 4 This is a schematic diagram of the combined navigation process based on the fusion of two-way vision and inertial navigation in this embodiment. According to... Figure 4 Step S5 includes the following steps S51-S54.

[0050] Step S51: The inertial measurement unit 10 built into the mobile terminal measurement system 2 is used to perform separate navigation calculation on the tunneling machine, thereby obtaining the original high-frequency real-time navigation calculation results of the tunneling machine, including the real-time position information, real-time speed information and real-time attitude information of the tunneling machine. In the EKF algorithm framework, the high-frequency pose strapdown calculation based on inertial measurement data is used as the state recursion process of EKF.

[0051] Specifically, the strapdown solution of the state equations based on inertial measurement data is arranged as follows: The attitude quaternion state update equation of the carrier (in this invention, the tunneling machine) is as follows: , in, The attitude quaternion needs to be valued. This is measurement data from a three-axis gyroscope. To measure the zero bias of a three-axis gyroscope, Noise measurement for a three-axis gyroscope; The carrier velocity state update equation is as follows: , in, The estimated value of the three-dimensional velocity in the mobile coordinate system. For triaxial accelerometer measurement data, To estimate the zero bias value for triaxial accelerometer measurements, Noise measurement for triaxial accelerometers; The carrier position state update equation is as follows: , in, The estimated value is for the 3D position in the mobile coordinate system. The sensor error state update equation is as follows: .

[0052] Step S52: Based on the three-dimensional position of the mobile terminal in the base station coordinate system obtained by the aforementioned binocular vision at the base station, construct the first set of observations under the EKF fusion filtering algorithm framework, namely the position observation information.

[0053] Specifically, the observation equation based on location information is as follows: , in, For the location measurement values ​​of the mobile terminal by the base station measurement system 1, Noise for position measurement.

[0054] Step S53: Based on the vector information of the mobile terminal in the coordinate system of the base station and the point and line feature vector information of the laser beam of the base station in the coordinate system of the mobile terminal identified by the visual sensor 8 (user terminal monocular vision), the second set of observations, namely attitude observation information, is constructed under the EKF fusion filtering algorithm framework.

[0055] Specifically, the attitude observation equation based on vector observation information is as follows: , in, This refers to the unit vector measurement information of the mobile terminal in the coordinate system of the base station. This represents the unit vector measurement information of the laser beam origin in the coordinate system of the moving end. This is vector observation noise.

[0056] Step S54: Under the EKF fusion filtering algorithm framework, after position observation information correction and attitude observation information correction, the real-time optimal navigation result of the tunneling machine is obtained, i.e., the real-time optimal pose measurement result. Furthermore, due to the existence of different coordinate systems (including the base station coordinate system and the mobile terminal coordinate system), therefore... Figure 4 As shown, the EKF fusion filtering algorithm framework performs coordinate transformation on both the input information and the output results.

[0057] Specifically, the EKF algorithm process is as follows: Assume the state equation and measurement equation after linear discretization are as follows: Equations of state: , Measurement equation: , in, The current state. For the state at the next moment, Here is the state transition matrix. For noise driving matrix, State noise, For measurement value, For the measurement matrix, For measuring noise; The statistical characteristics of the state noise and the measurement noise are described by the state noise variance matrix and the measurement noise variance matrix, respectively. and measurement noise variance matrix They are respectively: ; The system state transition matrix can be obtained after discretization. System noise driving matrix Measurement matrix Combining the state noise matrix and the measurement noise matrix, the Kalman filter algorithm process is as follows: Based on the current state, predict the state at the next moment. The predicted state is: , The uncertainty of the predicted state is estimated based on the error covariance matrix, which is: Calculate the Kalman gain as the optimal gain: , The predicted state is corrected to obtain the optimal state estimate. , By correcting the error covariance matrix, the optimal covariance estimate is obtained. ,in It is an identity matrix.

[0058] In step S6, based on the above-mentioned integrated navigation solution, the motion control module calculates the pose deviation by combining the optimal estimated pose with the real-time planned pose data, drives the execution structure of the tunneling machine, and performs precise pose control on the tunneling machine.

[0059] Some embodiments of the bidirectional vision-based tunneling machine posture measurement and control method of the present invention address the challenges of harsh working environments, high labor intensity, and high requirements for directional controllability in tunneling operations. They employ posture measurement based on the fusion of bidirectional vision measurement and inertial navigation to solve the problem of accurate position and posture measurement and control of cantilever tunneling machines during autonomous coal cutting in underground mines. This ensures accurate measurement of the tunneling machine's three-dimensional position and three-axis posture data during real-time cutting and advancement in the roadway, and enables precise posture control of the tunneling machine. This solves the problem of decreased posture control accuracy caused by low accuracy and poor reliability of three-dimensional position and three-axis posture measurement of the tunneling machine in the roadway.

[0060] The technical scope of this invention is not limited to the contents of the above specification. Those skilled in the art can make various modifications and variations to the above embodiments without departing from the technical concept of this invention, and all such modifications and variations should fall within the scope of this invention.

Claims

1. A method for measuring and controlling the pose of a tunneling machine based on bidirectional vision, characterized in that, The method uses a tunneling machine posture measurement and control system, which includes a base station measurement system (1), a mobile measurement system (2), and a motion control module. The base station measurement system (1) is installed in the tunnel where the tunneling machine is operating, and the mobile measurement system (2) is installed on the tunneling machine body. The base station measurement system (1) includes a binocular vision sensor (3) and a laser light (4), and the mobile measurement system (2) includes a vision sensor (8), a feature light source (9), and an inertial measurement unit (10). The method includes the following steps: Step S1: Establish a base station coordinate system using the base station measurement system (1) as a reference. Step S2: Using the mobile terminal measurement system (2) as a reference, establish a mobile terminal coordinate system; Step S3: The binocular vision sensor (3) identifies and measures the three-dimensional position information of the feature light source (9) in the coordinate system of the base station. The three-dimensional position information includes the vector information of the feature light source (9) in the coordinate system of the base station, and the distance information between the feature light source (9) and the binocular vision sensor (3). Step S4: The laser beam emitted by the laser lamp (4) is identified and measured by the vision sensor (8), and its point and line features are calculated in the coordinate system of the mobile terminal. The vector information is combined with the vector information of the feature light source of the mobile terminal in the coordinate system of the base station terminal to increase the attitude correction information of the tunneling machine. Step S5: Based on the three-dimensional position information, the vector information of the laser beam in the coordinate system of the mobile end, and the measurement data of the inertial measurement unit (10), the combined navigation solution is performed based on the extended Kalman filter algorithm to obtain the optimal estimate of the tunneling machine's pose. Step S6: The motion control module calculates the pose deviation by combining the optimal estimated pose and the real-time planned pose data, and performs pose control. The step S1 includes: the coordinate system of the base station is based on the direction of the laser beam emitted by the laser lamp (4) as the Z-axis, and the coordinate system of the binocular vision sensor (3) is aligned with the coordinate system of the base station through calibration. Step S2 includes: the mobile terminal coordinate system takes the installation position of the inertial measurement unit (10) as the origin, the rightward direction of the tunneling machine as the X-axis, and the forward direction of the tunneling machine as the Y-axis. The X-axis, Y-axis, and Z-axis of the mobile terminal coordinate system conform to the right-hand screw rule. The coordinate system of the vision sensor (8) is consistent with the mobile terminal coordinate system through calibration.

2. The method as described in claim 1, characterized in that, Step S5 includes: Step S51: The inertial measurement unit (10) is used to perform navigation calculation on the tunneling machine to obtain real-time navigation calculation results. In the extended Kalman filter algorithm framework, the pose strapdown calculation based on inertial measurement data is used as the state recursion process of the extended Kalman filter. Step S52: Based on the three-dimensional position information, construct the first set of observations under the extended Kalman filter algorithm framework; Step S53: Based on the vector information of the feature light source (9) in the coordinate system of the base station and the vector information of the laser beam in the coordinate system of the mobile terminal, construct the second set of observations under the extended Kalman filter algorithm framework; Step S54: Under the extended Kalman filter algorithm framework, after the first set of observation corrections and the second set of observation corrections, the real-time optimal pose estimation result of the tunneling machine is obtained.

3. The method as described in claim 2, characterized in that, In step S51, the real-time navigation solution result includes real-time position information, real-time speed information, and real-time attitude information.

4. The method as described in claim 2, characterized in that, In step S51, the pose strapdown calculation based on inertial measurement data includes: Construct the attitude quaternion state update equation: ， in, The attitude quaternion needs to be valued. This is data measured by a three-axis gyroscope. To measure the zero bias of a three-axis gyroscope, Noise measurement for a three-axis gyroscope; Construct the velocity state update equation: ， in, The estimated value of the three-dimensional velocity in the mobile coordinate system is as follows. For triaxial accelerometer measurement data, To estimate the zero bias value for triaxial accelerometer measurements, Noise measurement for triaxial accelerometers; Construct the position state update equation: ， in, The estimated value is the three-dimensional position in the mobile terminal coordinate system. Construct the sensor error state update equation: ； Step S52 includes: Construct observation equations based on location information: ， in, The measurement value of the location of the mobile terminal measurement system (2) by the base station measurement system (1) is the measurement value of the location of the mobile terminal measurement system (2). Noise for position measurement; Step S53 includes: Construct attitude observation equations based on vector observation information: ， in, This refers to the unit vector measurement information of the mobile terminal measurement system (2) in the coordinate system of the base station. This refers to the unit vector measurement information of the origin of the laser beam in the coordinate system of the moving end. This is vector observation noise; Step S54 includes: Assume the state equation and measurement equation after linear discretization are as follows: ， ， in, The current state. For the state at the next moment, Here is the state transition matrix. For noise driving matrix, State noise, For measurement value, For the measurement matrix, For measuring noise; Through the state noise variance matrix and measurement noise variance matrix Describe the statistical characteristics of the state noise and the measurement noise: ; The state transition matrix is ​​obtained after discretization. The noise driving matrix The measurement matrix Combining the state noise matrix and the measurement noise matrix, the Kalman filter algorithm process is as follows: Based on the current state, predict the state at the next moment. The predicted state is: , The uncertainty of the predicted state is estimated based on the error covariance matrix, where the error covariance matrix is: Calculate Kalman gain , The predicted state is corrected to obtain the optimal state estimate. , By correcting the error covariance matrix, the optimal covariance estimate is obtained. ,in It is an identity matrix.

5. A tunnel boring machine pose measurement and control system based on bidirectional vision using the method described in any one of claims 1-4, characterized in that, The system includes a base station measurement system (1), a mobile measurement system (2), and a motion control module. The base station measurement system (1) is installed in the tunnel where the tunneling machine operates, and the mobile measurement system (2) is installed on the body of the tunneling machine. The base station measurement system (1) includes a binocular vision sensor (3) and a laser light (4), and the mobile measurement system (2) includes a vision sensor (8), a feature light source (9), and an inertial measurement unit (10). The laser light (4) is within the field of view of the vision sensor (8), and the feature light source (9) is within the field of view of the binocular vision sensor (3).

6. The system as described in claim 5, characterized in that, The base station measurement system (1) is installed at the center line of the roof of the tunnel, and the mobile measurement system (2) is installed at the rear of the tunneling machine.

7. The system as described in claim 5, characterized in that, The base station measurement system (1) includes a visual feature plate, which is located close to the binocular vision sensor (3).

8. The system as described in claim 5, characterized in that, The characteristic light source (9) and the mobile terminal measurement system (2) are arranged separately at different positions after the lever arm measurement.

9. The system as described in claim 5, characterized in that, The base station measurement system (1) includes an inclinometer (5). The inclinometer (5) and the laser light (4) are used to adjust the coordinate system of the binocular vision sensor (3) to coincide with the coordinate system of the base station. Both the base station measurement system (1) and the mobile measurement system (2) include communication data transmission devices (7, 11) and data processing and calculation units (6, 12). The communication data transmission devices (7, 11) are used for data transmission between the base station measurement system (1) and the mobile measurement system (2), and the data processing and calculation units (6, 12) are used for data processing and calculation when performing tunneling machine pose measurement.

Images