Method and system for pose measurement and control of a roadheader

Abstract

The present application relates to a kind of tunneling machine pose measurement and control method and system.The system used by the method includes base station end measurement system in roadway, mobile end measurement system on the body of tunneling machine and motion control module, and the method includes the following steps: S1, establishes base station end coordinate system;S2, establishes mobile end coordinate system;S3, base station end is identified through visual sensor the vector information of measurement mobile end feature light source;S4, base station end and mobile end measure the distance between them through communication ranging equipment;S5, solve the three-dimensional position of mobile end in base station end coordinate system;S6, mobile end is identified through visual sensor the vector information of measurement base station end laser beam;S7, combined navigation is solved based on EKF algorithm;S8, pose control is carried out based on combined navigation solution result.The present application adopts the means that two-way visual measurement, communication ranging and inertial measurement are combined, improves measurement precision, and does not need high-cost 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 tunneling machine posture measurement and control method and system. 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, 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 posture of a tunneling machine. The method utilizes a tunneling machine posture measurement and control system, comprising a base station measurement system, a mobile terminal measurement system, and a motion control module. The base station measurement system is installed within the tunnel in which the tunneling machine operates, and the mobile terminal measurement system is installed on the tunneling machine body. The base station measurement system includes a first visual sensor, a first communication ranging device, and a laser light; the mobile terminal measurement system includes a second visual sensor, a second communication ranging device, 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 first visual sensor is used to identify and measure the feature light source, and the vector information of the feature light source in the coordinate system of the base station is calculated to obtain the first vector information; Step S4: Measure the distance information between the base station measurement system and the mobile terminal measurement system using the first communication ranging device and the second communication ranging device; Step S5: Combine the first vector information and the distance information to calculate the three-dimensional position of the mobile terminal measurement system in the coordinate system of the base station; Step S6: The laser beam emitted by the laser lamp is identified and measured by the second vision sensor, and the vector information of the laser beam in the coordinate system of the mobile end is calculated to obtain the second vector information; Step S7: Based on the three-dimensional position, the first vector information, the distance information, the second vector information, 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 S8: 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 coordinate system of the base station is based on the direction of the laser beam emitted by the laser light as the Z-axis, and the coordinate system of the first visual sensor 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 as the origin, the tunneling machine is positioned to the right as the X-axis, the tunneling machine is positioned forward 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, and the coordinate system of the second vision sensor is aligned with the mobile terminal coordinate system through calibration.

[0007] In the method described above, optionally, the characteristic light source includes multiple light sources, and step S3 includes: Step S31: The first visual sensor measures and calculates the light source vector in the computation space based on the original sensor data and based on Gaussian filtering, centroid extraction and intrinsic parameter calibration compensation algorithms. Step S32: Based on the pose of the first visual sensor, the distance between the feature light source and the first visual sensor, and the constraint information of the cross-sectional dimensions of the alleyway, the area of ​​the feature light source within the field of view of the first visual sensor is estimated to obtain the estimated area. Step S33: Within the estimated region, based on the angular distance information between each pair of the multiple light source vectors and the configuration characteristics of the multiple light sources, determine and filter the vector information of the feature light source.

[0008] In the method described above, step S7 may optionally include: Step S71: 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 S72: Based on the three-dimensional position, construct the first set of observations under the extended Kalman filter algorithm framework; Step S73: Based on the first vector information and the second vector information, construct the second set of observations under the extended Kalman filter algorithm framework; Step S74: 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.

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

[0010] In the method described above, optionally, in step S71, 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 S72 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 S73 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 S74 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.

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

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

[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 arm 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 first visual 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 tunneling machine posture measurement and control method of the present invention adopts a combination of two-way vision, communication ranging and inertial measurement, which improves the measurement accuracy and does not require a high-cost inertial measurement unit.

[0016] The present invention further provides a tunneling machine position and attitude measurement and control system, which 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 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; Figure 4 This is a schematic diagram illustrating the process of acquiring target light source vector information using a visual sensor in an embodiment of the tunneling machine pose measurement and control method of the present invention; and Figure 5 This is a schematic diagram of a combined navigation system based on bidirectional vision, communication ranging, and inertial navigation fusion, which is an embodiment of the tunneling machine posture measurement and control method of the present invention.

[0018] Reference numerals in the attached figures: 1-Base station measurement system; 2-Mobile measurement system; 3-First visual sensor; 4-Laser light; 5-Inclinometer; 6-First data processing and calculation unit; 7-First communication data transmission device; 8-First communication ranging device; 9-Second visual sensor; 10-Characteristic light source; 11-Inertial measurement unit; 12-Second communication ranging device; 13-Second communication data transmission device; 14-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 position and posture measurement and control system of the present invention.

[0023] Figure 1The 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 tunnel excavation methods. In optional embodiments, the tunneling machine posture measurement and control system of the present invention can also be applied to other types of tunneling machines, such as full-face tunneling machines.

[0025] like Figure 1 As 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 be used as the tunnel reference coordinate system. Preferably, 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 and to avoid obstructing other equipment in 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 horizontal and vertical directions, respectively.

[0026] like Figure 1 As shown, the mobile measurement system 2 is installed 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 coordinate system of the tunneling machine body. Preferably, the mobile measurement system 2 is installed at the rear of the tunneling machine to facilitate mutual observation with the base station measurement system 1. In optional embodiments, the mobile measurement system 2 can also be installed 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 inertial navigation system installation 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 the Zb axis is determined by 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 base station measurement system 1 includes a first visual sensor 3, a laser light 4, an inclinometer 5, a first data processing and calculation unit 6, a first communication data transmission device 7, and a first communication ranging device 8.

[0031] The first visual sensor 3 is a high-precision direction-finding visual sensor used to identify and measure the vector information of the feature light source 10 of the mobile measurement system 2, hereinafter referred to as the first vector information. The first vector information can be used to calculate the position of the mobile measurement system 2 in the base station coordinate system, thereby obtaining the position information of the tunneling machine. The installation position and orientation of the first visual sensor 3 must ensure that the feature light source 10 of the mobile measurement system 2 is within its field of view. Preferably, a visual sensor with a suitable field of view can be selected, and a suitable installation direction and angle can be set. For example, under the premise of ensuring that the feature light source 10 is within the field of view in real time, the installation angle of the visual sensor can be adjusted so that the feature light source 10 is centered in its field of view, and a visual sensor with a sufficient but not too large field of view can be selected to reduce the useless area in the collected visual information, thereby improving the recognition efficiency. The coordinate system used by the first visual sensor 3 is adjusted by the inclinometer 5 and the laser light 4 to make it coincide with the coordinate system of the inclinometer 5, and both coincide with the base station coordinate system O-XnYnZn.

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

[0033] The first communication ranging device 8 is used to perform communication ranging with the communication device of the mobile terminal measurement system 2 to measure the precise distance information between the mobile terminal and the base station. Exemplarily, this embodiment employs ultra-wideband (UWB) ranging, and the first communication ranging device 8 is a UWB ranging device. The distance between the first communication ranging device 8 and the UWB ranging device of the mobile terminal measurement system 2 is measured using the time-of-flight (ToF) measurement principle, and this distance is used as the distance between the base station and the mobile terminal.

[0034] The first data processing and calculation unit 6 is a built-in calculation unit of the base station measurement system 1. It can calculate the three-dimensional position of the mobile terminal in the base station coordinate system by combining the aforementioned first vector information and the distance information between the mobile terminal and the base station. Then, the first communication data transmission device 7 communicates with the mobile terminal and sends the aforementioned first vector information, distance information, and three-dimensional position to the mobile terminal measurement system 2. The method of measuring the mobile terminal position by combining a high-precision visual sensor and a long-distance communication ranging module has higher position measurement accuracy and a longer range of action compared to technologies such as binocular vision and lidar.

[0035] like Figure 2 As shown, the first communication data transmission device 7 and the first communication ranging device 8 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 it from external environmental interference and damage.

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

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

[0038] The second visual sensor 9 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, hereinafter referred to as the second vector information. The second vector information can be combined with the first vector information to increase the attitude correction information of the tunneling machine, which can curb the tendency of heading divergence 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 second visual sensor 9 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. Preferably, similar to the preferred selection and installation scheme of the first visual sensor 3, under the premise that the field of view of the second visual sensor 9 meets the aforementioned requirements, a visual sensor with a relatively small field of view can be selected to reduce the useless areas in the collected visual information, thereby improving the recognition efficiency. The coordinate system of the second visual sensor 9 is consistent with the aforementioned mobile terminal coordinate system O-XbYbZb through precise calibration.

[0039] The characteristic light source 10 is oriented towards the base station measurement system 1, and the light source is within the field of view of the first visual sensor 3. For example... Figure 3 As shown, the feature light source 10 in this embodiment is a visual feature lamp. To ensure that the first visual sensor 3 can uniquely identify the target feature light source 10, the visual feature lamp is designed to have multiple light sources arranged at a certain distance and in a certain configuration. The arrangement configuration between the multiple light sources is not limited to triangles, squares, etc. The first visual sensor 3 determines the target light source by the angular distance between each pair of identified light sources and the configuration features. Optionally, the visual feature lamp and the main body of the mobile terminal measurement system 2 can be arranged separately after precise lever measurement.

[0040] The inertial measurement unit 11 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. 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, a low-cost inertial measurement unit can be selected, significantly reducing costs compared to existing technologies based on gyro total stations / high-precision inertial navigation combined measurements. The coordinate system of the inertial measurement unit 11 is the aforementioned mobile terminal coordinate system O-XbYbZb.

[0041] The second communication ranging device 12 is used to communicate and measure distances with the first communication ranging device 8 to determine the precise distance between the mobile terminal and the base station. As mentioned above, exemplarily, both the first communication ranging device 8 and the second communication ranging device 12 are UWB ranging devices.

[0042] The second communication data transmission device 13 can communicate with the first communication data transmission device 7 of the base station measurement system 1 to receive information such as the first vector information, the distance information between the mobile terminal and the base station, and the three-dimensional position of the mobile terminal in the coordinate system of the base station.

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

[0044] like Figure 3 As shown, the second communication ranging device 12 and the second communication data transmission device 13 can be installed on the opposite side of the lens direction of the second vision sensor 9 to avoid obstructing its field of view. The inertial measurement unit 11 and the second data processing and calculation unit 14 can be built into the main body of the mobile terminal measurement system 2 to reduce space occupation and protect it from external environmental interference and damage.

[0045] An embodiment of the tunneling machine posture measurement and control method 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 10 is identified and measured by the first visual sensor 3, and the vector information of the feature light source 10 in the coordinate system of the base station is calculated, namely the first vector information. Step S4: The distance between the base station measurement system 1 and the mobile measurement system 2 is measured by communicating between the first communication ranging device 8 and the second communication ranging device 12. Step S5: Combine the first vector information and distance information to calculate the three-dimensional position of the mobile terminal measurement system 2 in the base station coordinate system; Step S6: The laser beam emitted by the laser lamp 4 is identified and measured by the second vision sensor 9, and the vector information of the laser beam in the coordinate system of the mobile end is calculated, that is, the second vector information. Step S7: Based on the above three-dimensional position, first vector information, distance information, second vector information and measurement data of inertial measurement unit 11, combined navigation calculation is performed based on extended Kalman filter (EKF) algorithm to obtain the optimal estimate of tunneling machine pose; Step S8: The tunneling machine motion control module performs position control based on the above position information.

[0046] 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 points in the same direction as the laser beam emitted by the laser lamp 4. Through precise calibration, the coordinate systems of the first vision sensor 3 and the inclinometer 5 are both aligned with the base station coordinate system.

[0047] In step S2, the coordinate system O-XbYbZb of the inertial measurement unit 11 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 11, 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 second vision sensor 9 is precisely calibrated to coincide with it, and both coordinate systems are aligned with the tunneling machine's coordinate system.

[0048] In step S3, the specific method for the first visual sensor 3 to identify the target (feature light source 10) vector information is as follows: Figure 4 As shown. Figure 4 This is a schematic diagram of the process for obtaining target light source vector information using a visual sensor in this embodiment. The method shown can also be used in step S6. Figure 4 Step S3 includes the following steps S31-S33.

[0049] In step S31, the first visual sensor 3 measures the original sensor pixel layer data, calculates the target vector based on the Gaussian filtering algorithm and the centroid extraction algorithm, and improves the accuracy based on the intrinsic parameter calibration compensation algorithm. Specifically, the original pixel layer data is denoised by Gaussian filtering, the geometric center of the light source is found from the image by the centroid extraction algorithm, and then the intrinsic parameter calibration compensation algorithm is used to correct the internal parameter error of the camera (first visual sensor 3) to improve the accuracy of vector measurement.

[0050] Step S32: Track and estimate the imaging area based on camera pose and target estimated position information. Furthermore, when estimating the approximate area of ​​the target light source within the camera's field of view, constraint information such as target distance and tunnel cross-sectional dimensions can be added to improve accuracy, precision, and robustness in underground coal mine tunnel scenarios.

[0051] In step S33, multiple light sources are combined with distance information to determine the target through angular distance, and the target is filtered based on feature judgment, thus completing the target recognition based on multi-condition judgment of parallel light sources. Specifically, to ensure that the camera can uniquely identify the target light source, the feature light source 10 has multiple light sources arranged at a certain distance and in a certain configuration. The arrangement configuration between the multiple light sources is not limited to triangles, squares, etc. The first visual sensor 3 uses the pairwise angular distance information of the multiple light sources and the configuration features of the light sources to judge and filter the target light source vector information.

[0052] And, as Figure 4 As shown, the target vector obtained through step S3 above, i.e. the first vector information, can be combined with the ranging information to complete the target position calculation.

[0053] In step S4, for example, the first communication ranging device 8 and the second communication ranging device 12 are UWB ranging devices, and the distance is calculated by measuring the propagation time of the signal between the two communication ranging devices 8 and 12.

[0054] In step S5, after obtaining the first vector information, and combining it with the distance information obtained from communication ranging, the target's three-dimensional position in the base station coordinate system is obtained by extending the distance from the optical center of the first visual sensor 3 along the direction of the vector relative to the optical center. The base station measurement system 1 sends this three-dimensional position, the first vector information, and the distance information to the mobile measurement system 2 for subsequent integrated navigation calculations. Using a combination of a high-precision visual sensor and a long-range communication ranging module as the method for mobile position measurement offers higher position measurement accuracy and a longer effective range compared to technologies such as binocular vision and lidar.

[0055] In step S6, the specific method for the second visual sensor 9 to identify the second vector information of the laser beam can be referred to the method for calculating the first vector information in step S3 above, and will not be repeated here. By reverse-measuring the point and line vector information of the laser beam at the base station from the mobile terminal, and combining it with the first vector information, the attitude correction information of the tunneling machine is increased. This can curb the tendency of heading divergence 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.

[0056] In step S7, the specific steps of the integrated navigation solution are as follows: Figure 5 As shown. Figure 5 This is a schematic diagram of the integrated navigation process based on the fusion of two-way vision, communication ranging, and inertial navigation in this embodiment. According to... Figure 5 Step S7 includes the following steps S71-S74.

[0057] Step S71: The inertial measurement unit 11 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.

[0058] 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: .

[0059] Step S72: Based on the aforementioned three-dimensional position of the mobile terminal in the base station coordinate system, construct the first set of observations under the EKF fusion filtering algorithm framework, namely, location observation information. For example... Figure 5 As shown, the three-dimensional position is calculated based on the first vector information identified by the first visual sensor 3 (high-precision direction-finding visual sensor at the base station) and the distance information obtained by communication ranging.

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

[0061] Step S73: Based on the aforementioned first vector information and the second vector information identified by the second vision sensor 9 (user-end high-precision vision sensor), construct the second set of observations under the EKF fusion filtering algorithm framework, namely attitude observation information.

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

[0063] Step S74: 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 5 As shown, the EKF fusion filtering algorithm framework performs coordinate transformation on both the input information and the output results.

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

[0065] In step S8, based on the above-mentioned integrated navigation solution, the motion control module calculates the pose deviation by combining the optimally estimated pose with the real-time planned pose data, and drives the execution structure of the tunneling machine to perform precise pose control. The pose measurement result determines the effect of pose control. Therefore, when pose measurement has beneficial effects such as high precision and stability, pose control will correspondingly have beneficial effects such as high precision and stability.

[0066] Some embodiments of the 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 visual measurement, communication ranging, 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, real-time, and wide-range measurement of the tunneling machine's three-dimensional position and three-axis posture data during real-time cutting and advancement in the roadway. Based on this, precise posture control of the tunneling machine is achieved, resolving 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.

[0067] 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 position and attitude of a tunneling machine, 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 body of the tunneling machine. The base station measurement system (1) includes a first visual sensor (3), a first communication ranging device (8), and a laser light (4). The mobile measurement system (2) includes a second visual sensor (9), a second communication ranging device (12), a feature light source (10), and an inertial measurement unit (11). 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 first visual sensor (3) is used to identify and measure the vector information of the feature light source (10) in the coordinate system of the base station to obtain the first vector information; Step S4: The distance information between the base station measurement system (1) and the mobile terminal measurement system (2) is measured by the first communication ranging device (8) and the second communication ranging device (12); Step S5: Combine the first vector information and the distance information to calculate the three-dimensional position of the mobile terminal measurement system (2) in the coordinate system of the base station terminal; Step S6: The second visual sensor (9) identifies and measures the vector information of the laser beam emitted by the laser lamp (4) in the coordinate system of the mobile end to obtain the second vector information; Step S7: Based on the three-dimensional position, the first vector information, the second vector information and the measurement data of the inertial measurement unit (11), 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 S8: 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 characteristic light source (10) includes multiple light sources, and step S3 includes: Step S31, the first visual sensor (3) measures the light source vector in the computation space based on the original sensor data and based on Gaussian filtering, centroid extraction and intrinsic parameter calibration compensation algorithm; Step S32: Based on the pose of the first visual sensor (3), the distance between the feature light source (10) and the first visual sensor (3), and the constraint information of the cross-sectional size of the alley, the area of ​​the feature light source (10) within the field of view of the first visual sensor (3) is estimated to obtain the estimated area. Step S33: Within the estimated region, based on the angular distance information between each pair of the multiple light source vectors and the configuration characteristics of the multiple light sources, determine and filter the vector information of the feature light source (10); Step S7 includes: Step S71: The inertial measurement unit (11) 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 S72: Based on the three-dimensional position, construct the first set of observations under the extended Kalman filter algorithm framework; Step S73: Based on the first vector information and the second vector information, construct the second set of observations under the extended Kalman filter algorithm framework; Step S74: 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.

2. The method as described in claim 1, characterized in that, 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 first visual sensor (3) is aligned with the coordinate system of the base station through calibration. Step S2 includes: the mobile end coordinate system takes the installation position of the inertial measurement unit (11) 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 end coordinate system conform to the right-hand screw rule. The coordinate system of the second vision sensor (9) is consistent with the mobile end coordinate system through calibration.

3. The method as described in claim 1, characterized in that, In step S71, the real-time navigation solution results include real-time position information, real-time speed information, and real-time attitude information.

4. The method as described in claim 1, characterized in that, In step S71, 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 S72 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 S73 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 S74 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 tunneling machine position and attitude measurement and control system 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 first visual sensor (3), a first communication ranging device (8), and a laser light (4). The mobile measurement system (2) includes a second visual sensor (9), a second communication ranging device (12), a feature light source (10), and an inertial measurement unit (11). The laser light (4) is within the field of view of the second visual sensor (9), and the feature light source (10) is within the field of view of the first visual 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 characteristic light source (10) and the mobile terminal measurement system (2) are arranged separately at different positions after the lever arm measurement.

8. 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 first visual 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, 13) and data processing and calculation units (6, 14). The communication data transmission devices (7, 13) 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, 14) are used for data processing and calculation when performing tunneling machine pose measurement.

Images