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

Abstract

The invention relates to a heading machine pose measurement and control method and system based on bidirectional vision. A system used by the method comprises a base station end measuring system in a roadway, a mobile end measuring system on a heading machine body and a motion control module. The method comprises the following steps: S1, establishing a base station end coordinate system; s2, establishing a mobile terminal coordinate system; s3, the base station end identifies and measures the three-dimensional position information of the feature light source of the mobile end under the coordinate system of the base station end through a binocular vision sensor; s4, the mobile terminal identifies and measures the vector information of the laser beam of the base station end through a visual sensor; s5, carrying out combined navigation calculation based on an EKF algorithm; and S6, performing pose control based on a combined navigation calculation result. According to the invention, a means of combining bidirectional vision measurement and inertia measurement is adopted, so that the cost requirement on a high-precision inertia measurement unit is reduced.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of tunneling machine positioning navigation and control; in particular, the present application relates to a tunneling machine pose measurement and control method and system based on bidirectional vision. BACKGROUND

[0002] Coal still accounts for a major proportion in the energy structure in China, and this situation cannot be changed in the short term. As the most important comprehensive mining and tunneling machine in coal mine underground roadway, the cantilever type tunneling machine is widely used in various roadway tunneling, but there are problems such as poor working environment of tunneling working face, high labor intensity and high controllability requirement of tunneling direction, so the intelligent and automatic comprehensive tunneling technology is an urgent demand for the development of roadway tunneling.

[0003] In the process of comprehensive tunneling intelligent development, the accurate measurement of the position and attitude of the cantilever type tunneling machine is the primary problem, which directly determines the accuracy of the roadway direction and the quality of the roadway forming. The existing technology is mainly based on communication ranging and inertial navigation. For example, one method measures the distance between the two sides of the coal wall by installing millimeter wave radar on the front and rear ends of the tunneling machine to solve the three-dimensional attitude correction amount of the tunneling machine. This method requires that the two sides of the coal wall must be vertical and smooth and strictly in the specified straight line direction, otherwise the ranging error of the millimeter wave radar will cause the attitude correction calculation error. Another method is based on gyro total station and inertial navigation equipment to realize high-precision estimation of the position and attitude of the tunneling machine, which has the problem of high hardware cost. SUMMARY

[0004] Therefore, the present application provides a tunneling machine pose measurement and control method and system based on bidirectional vision, so as to solve or at least alleviate one or more of the above problems and other aspects in the prior art.

[0005] In order to achieve the above-mentioned purpose, the first aspect of the present application provides a tunneling machine pose measurement and control method based on bidirectional vision, wherein the method uses a tunneling machine pose measurement and control system, the system includes a base station end measurement system, a mobile end measurement system and a motion control module, the base station end measurement system is installed in the roadway where the tunneling machine works, the mobile end measurement system is installed on the tunneling machine body, the base station end measurement system includes a binocular vision sensor and a laser lamp, the mobile end measurement system includes a vision sensor, a feature light source and an inertial measurement unit; the method includes the following steps: Step S1, taking the base station end measurement system as a reference, a base station end coordinate system is established; Step S2, taking the mobile end measurement system as a reference, a mobile end coordinate system is established; Step S3, identifying and measuring the three-dimensional position information of the feature light source in the base station end coordinate system through the binocular vision sensor, the three-dimensional position information including vector information of the feature light source in the base station end coordinate system and distance information of the feature light source from the binocular vision sensor; Step S4, identifying and measuring the vector information of the laser beam emitted by the laser lamp in the mobile end coordinate system through the vision sensor; Step S5, based on the three-dimensional position information, the vector information of the laser beam in the mobile end coordinate system and the measurement data of the inertial measurement unit, performing combined navigation calculation based on an extended Kalman filtering algorithm to obtain the optimal estimation of the pose of the roadheader; Step S6, the motion control module combines the optimal estimation of the pose and the real-time planned pose data to calculate the pose deviation and perform pose control.

[0006] In the method as described above, optionally, the step S1 includes: the base station end coordinate system taking the direction of the laser beam emitted by the laser lamp as the Z axis, and the coordinate system of the binocular vision sensor being consistent with the base station end coordinate system through calibration. The step S2 includes: the mobile end coordinate system taking the installation position of the inertial measurement unit as the origin, taking the right direction of the roadheader as the X axis and taking the forward direction of the roadheader as the Y axis, the X axis, the Y axis and the Z axis of the mobile end coordinate system meeting the right-hand screw rule, and the coordinate system of the vision sensor being consistent with the mobile end coordinate system through calibration.

[0007] In the method as described above, optionally, the step S5 includes: Step S51, performing navigation calculation on the roadheader by using the inertial measurement unit to obtain a real-time navigation calculation result, and taking the pose strapdown calculation based on the inertial measurement data as a state recursion process of the extended Kalman filtering in the framework of the extended Kalman filtering algorithm; Step S52, constructing a first group of observations under the framework of the extended Kalman filtering algorithm based on the three-dimensional position information; Step S53, constructing a second group of observations under the framework of the extended Kalman filtering algorithm based on the vector information of the feature light source in the base station end coordinate system and the vector information of the laser beam in the mobile end coordinate system; Step S54, obtaining the real-time optimal pose estimation result of the roadheader through the first group of observation correction and the second group of observation correction under the framework of the extended Kalman filtering algorithm.

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

[0009] In the method as described above, in the step S51, the inertial measurement data-based pose strapdown solution comprises: A pose quaternion state update equation is constructed: , wherein, is a pose quaternion to be estimated, is three-axis gyroscope measurement data, is three-axis gyroscope measurement bias to be estimated, is three-axis gyroscope measurement noise; A velocity state update equation is constructed: , wherein, is three-dimensional velocity to be estimated in the mobile end coordinate system, is three-axis accelerometer measurement data, is three-axis accelerometer measurement bias to be estimated, is three-axis accelerometer measurement noise; A position state update equation is constructed: , wherein, is three-dimensional position to be estimated in the mobile end coordinate system; A sensor error state update equation is constructed: ; The step S52 comprises: An observation equation based on position information is constructed: , wherein, is a measurement value of the mobile end measurement system position by the base station end measurement system, is position measurement noise; The step S53 comprises: A pose observation equation based on vector observation information is constructed: , wherein, is unit vector measurement information of the mobile end measurement system in the base station end coordinate system, is unit vector measurement information of the laser beam origin in the mobile end coordinate system, is vector observation noise; The step S54 comprises: Suppose that the linear discretized state equation and the measurement equation are respectively: , , wherein, is the current time state, is the next time state, is the state transition matrix, is the noise driven matrix, is the state noise, is the measurement value, is the measurement matrix, is the measurement noise; statistical properties of the state noise and the measurement noise are described by a state noise variance matrix and a measurement noise variance matrix ; ; the state transition matrix , the noise driven matrix , the measurement matrix , combined with the state noise matrix and the measurement noise matrix, the Kalman filtering algorithm process is as follows: based on the current state, the next time state is estimated, and the predicted state is , the uncertainty of the predicted state is estimated based on the error covariance matrix, and the error covariance matrix is the Kalman gain is calculated , the predicted state is corrected to obtain the optimal state estimation , the error covariance matrix is corrected to obtain the optimal covariance estimation , wherein is the unit matrix.

[0010] To achieve the foregoing purpose, a second aspect of the present application provides a two-way vision-based pose measurement and control system of a heading machine, which uses the method of any one of the preceding first aspects, wherein the system comprises a base station end measurement system, a mobile end measurement system and a motion control module, the base station end measurement system is installed in a roadway where the heading machine works, the mobile end measurement system is installed on the body of the heading machine, the base station end measurement system comprises a binocular vision sensor and a laser lamp, the mobile end measurement system comprises a vision sensor, a feature light source and an inertial measurement unit, the laser lamp is within the field of view of the vision sensor, and the feature light source is within the field of view of the binocular vision sensor.

[0011] In the system as described above, optionally, the base station end measurement system is installed at the roof centerline position of the tunnel, and the mobile end measurement system is installed at the rear of the tunneling machine body.

[0012] In the system as described above, optionally, the base station end measurement system comprises a visual feature plate, which is located next to the binocular vision sensor.

[0013] In the system as described above, optionally, the feature light source and the mobile end measurement system are arranged at different positions after being measured by a lever arm.

[0014] In the system as described above, optionally, the base station end measurement system comprises an inclinometer, through which and the laser lamp, the coordinate system of the binocular vision sensor is adjusted to coincide with the base station end coordinate system, Both the base station end measurement system and the mobile end measurement system comprise a communication data transmission device and a data processing and calculation unit, the communication data transmission device is used for data transmission between the base station end measurement system and the mobile end measurement system, and the data processing and calculation unit is used for data processing and calculation when measuring the pose of the tunneling machine.

[0015] The tunneling machine pose measurement and control method based on bidirectional vision of the present application adopts the means of combining bidirectional vision with inertial measurement, thereby reducing the cost demand for high-precision inertial measurement units.

[0016] The present application further provides a tunneling machine pose measurement and control system based on bidirectional vision, and therefore the system also has the above-mentioned advantages. BRIEF DESCRIPTION OF DRAWINGS

[0017] The disclosure of the present application will be more apparent with reference to the accompanying drawings. It should be understood that these drawings are only for illustrative purposes, and are not intended to limit the scope of protection of the present application. In the drawings: Figure 1 a schematic diagram of an embodiment of the tunneling machine pose measurement and control system based on bidirectional vision of the present application; Figure 2 a schematic diagram of the base station end measurement system in Figure 1 Figure 3 a schematic diagram of the mobile end measurement system in Figure 1 Figure 4 a flowchart of combined navigation based on fusion of bidirectional vision and inertial navigation of an embodiment of the tunneling machine pose measurement and control method based on bidirectional vision of the present application.

[0018] ​​Reference signs: 1 - base station end measurement system; 2 - mobile end measurement system; 3 - binocular vision sensor; 4 - laser light; 5 - tilt meter; 6 - first data processing calculation unit; 7 - first communication data transmission device; 8 - vision sensor; 9 - characteristic light source; 10 - inertial measurement unit; 11 - second communication data transmission device; 12 - second data processing calculation unit. DETAILED DESCRIPTION

[0019] With reference to the drawings and specific embodiments, the structure, composition, features and advantages of the tunneling machine pose measurement and control method and system of the present application will be described below in an exemplary manner, however, all the descriptions shall not be used to form any limitation on the present application.

[0020] In addition, for any single technical feature described or implied in the embodiments mentioned herein, or any single technical feature shown or implied in the drawings, the present application still allows any combination or deletion to be continued between these technical features (or their equivalents) without any technical obstacles, so it should be considered that more embodiments according to the present application are also within the scope of the description herein.

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

[0022] Figure 1 Schematic diagram of an embodiment of the tunneling machine pose measurement and control system based on bidirectional vision of the present application.

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

[0024] The tunneling machine works in a coal mine underground tunnel not shown in the figure, as shown in Figure 1 The tunneling machine of this embodiment is a cantilever type tunneling machine widely used in various tunneling. In alternative embodiments, the tunneling machine pose measurement and control system based on bidirectional vision of the present application can also be applied to other types of tunneling machines, such as full-face tunneling machines, etc.

[0025] As shown in 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, which is used to identify the three-dimensional position information of the feature light source 9 of the mobile terminal measurement system 2, and the three-dimensional position information contains both vector information and distance information. The installation position and direction of the binocular vision sensor 3 need to ensure that the feature light source 9 of the mobile terminal measurement system 2 is within its field of view. In an optional embodiment, a binocular vision 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 9 is in the field of view of the two cameras at the same time, the installation angle of the vision sensor is adjusted so that the feature light source 9 is close to the central position in the field of view of the two cameras, and a binocular vision sensor with a sufficient but not excessive field of view is selected to reduce the useless area in the collected vision information, thereby improving the identification efficiency. The coordinate system of the binocular vision sensor 3 is adjusted by the inclinometer 5 and the laser lamp 4 to coincide with the coordinate system of the inclinometer 5, and both coincide with the base station coordinate system O-XnYnZn, thereby reducing the calculation complexity.

[0032] The laser lamp 4 is installed towards the mobile terminal measurement system 2, and the installation manner is such that the laser beam emitted by the laser lamp 4 can be collected by the mobile terminal measurement system 2. In an optional embodiment, the front axis Zn of the base station coordinate system is calibrated to ensure that it is consistent with the direction of the laser beam. The laser has better monochromaticity, directionality and higher brightness than ordinary light sources, and is easier to be identified by vision.

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

[0034] As shown in Figure 2 , the first communication data transmission device 7 can be installed on the other side of the lens direction of the first vision sensor 3 to avoid blocking the field of view thereof. 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 the space occupation, and can protect them from external environmental interference and damage.

[0035] Further, the base station measurement system 1 further comprises a vision feature plate next to the binocular vision sensor 3 to help the mobile terminal to calibrate the camera.

[0036] Figure 3 is a schematic view of the mobile terminal measurement system in Figure 1 .

[0037] Figure 3 The vision sensor 8, the feature light source 9, the inertial measurement unit 10, the second communication data transmission device 11 and the second data processing and calculation unit 12 of the mobile terminal measurement system 2 are shown.

[0038] The visual sensor 8 is used to identify the laser beam emitted by the laser lamp 4 of the base station end measurement system 1, and to calculate the vector information of the point and line features in the mobile end coordinate system. The vector information can be combined with the vector information of the mobile end feature light source 9 in the base station end coordinate system, and the attitude correction information of the roadheader is added, which can curb the trend of heading divergence in the long-time low dynamic scene of the combined navigation, improve the attitude estimation accuracy and stability of the roadheader for a long time, and reduce the cost demand for high-precision inertial measurement units. The installation position and direction of the visual sensor 8 need to ensure that the laser lamp 4 is within its field of view, and it can capture and identify the point and line features of the laser beam. In the alternative embodiment, similar to the selection and installation scheme of the aforementioned binocular visual sensor 3, a visual sensor with a field of view that is not too large can be selected to reduce the useless area in the collected visual information, thereby improving the identification efficiency, on the premise that the field of view of the visual sensor 8 meets the aforementioned requirements. The coordinate system of the visual sensor 8 coincides with the aforementioned mobile end coordinate system O-XbYbZb through accurate calibration, reducing the calculation complexity. The visual sensor 8 can be calibrated by the aforementioned base station end visual feature board.

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

[0040] The inertial measurement unit 10 can perform separate navigation calculation for the roadheader, high-frequency attitude strapdown calculation based on inertial measurement data, and combined navigation calculation combined with visual sensor measurement and communication ranging. The real-time calculation navigation result includes real-time position information, real-time speed information and real-time attitude information of the roadheader. The coordinate system of the inertial measurement unit 10 is the aforementioned mobile end coordinate system O-XbYbZb. As mentioned earlier, two-way visual measurement can improve the attitude estimation accuracy and stability of the roadheader for a long time, and reduce the cost demand for high-precision inertial measurement units, so a low-cost inertial measurement unit can be selected. Compared with the existing technology based on gyro total station / high-precision inertial navigation combined measurement, the cost can be greatly reduced.

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

[0042] ​The second data processing and computing unit 12 uses the information sent by the base station end measurement system 1 and the information measured by the visual sensor 8 and the inertial measurement unit 10 to realize combined navigation calculation based on an extended Kalman filter (EKF) algorithm.

[0043] As shown in Figure 3 The second communication data transmission device 11 can be installed on the other side of the lens direction of the visual sensor 8 to avoid blocking the field of view. The inertial measurement unit 10 and the second data processing and computing unit 12 can be built-in in the main body of the mobile end measurement system 2 to reduce the space occupation and protect them from the interference and damage of the external environment.

[0044] One embodiment of the tunneling machine pose measurement and control method based on bidirectional vision of the application uses the above system and comprises the following steps: Step S1, establishing a base station end coordinate system; Step S2, establishing a mobile end coordinate system; Step S3, identifying and measuring the feature light source 9 through the binocular visual sensor 3 to calculate the three-dimensional position information of the feature light source 9 in the base station end coordinate system; Step S4, identifying and measuring the laser beam emitted by the laser lamp 4 through the visual sensor 8 to calculate the vector information of the laser beam in the mobile end coordinate system; Step S5, based on the above three-dimensional position information, vector information and measurement data of the inertial measurement unit 10, performing combined navigation calculation based on an extended Kalman filter (EKF) algorithm to obtain the optimal estimation of the pose of the tunneling machine; Step S6, the tunneling machine motion control module performs pose control based on the above pose information.

[0045] In step S1, the base station end coordinate system O-XnYnZn is the roadway reference coordinate system, and the base station end measurement system 1 is the reference object. The front axis Zn is consistent with the direction of the laser beam emitted by the laser lamp 4. Through accurate calibration, the coordinate system of the binocular visual sensor 3 and the coordinate system of the inclinometer 5 are consistent with the base station end coordinate system, thereby reducing the calculation complexity.

[0046] In step S2, the coordinate system O-XbYbZb of the built-in inertial measurement unit 10 of the mobile end measurement system 2 can be used as the mobile end coordinate system. The coordinate system takes the installation position of the inertial measurement unit 10 as the origin O, the right direction of the tunneling machine as the Xb axis, the front direction of the tunneling machine as the Yb axis, and the Zb axis as the right-hand screw rule with the Xb axis and the Yb axis. The coordinate system of the visual sensor 8 is consistent with the coordinate system of the inclinometer 5 through accurate calibration, and all of them are consistent with the coordinate system of the tunneling machine body, thereby reducing the calculation complexity.

[0047] In step S3, the binocular vision sensor 3 identifies the three-dimensional position information of the target (characteristic light source 9) based on a binocular vision algorithm, thereby obtaining the three-dimensional position information of the mobile terminal in the base station end coordinate system. Specifically, the internal and external parameters of the two cameras and the baseline distance are obtained through calibration in advance; in the identification process, two images are synchronously captured 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 parallax of each pixel; the depth (distance) information of the target is calculated based on the principle of triangulation, and then the three-dimensional position of the target is calculated. Compared with a monocular camera, the binocular camera can not only obtain target vector information, but also obtain target depth information according to the parallax, thereby obtaining the three-dimensional position of the target.

[0048] In step S4, the base station end laser beam point and line vector information in the mobile terminal coordinate system is reversely measured by the mobile terminal vision sensor 8, and the vector information of the mobile terminal in the base station end coordinate system is combined to increase the posture correction information of the roadheader, so as to curb the trend of heading divergence in a long-time low dynamic scene of the combined navigation, improve the posture estimation accuracy and stability of the roadheader in a long time, and reduce the cost demand for a high-precision inertial measurement unit.

[0049] In step S5, the specific steps of the combined navigation solution are as shown in Figure 4 . Figure 4 The flowchart of the combined navigation based on the fusion of bidirectional vision and inertial navigation of this embodiment is shown in Figure 4 . Step S5 includes steps S51-S54.

[0050] In step S51, the inertial measurement unit 10 built in the mobile terminal measurement system 2 is used to perform a separate navigation solution for the roadheader, so as to obtain the original high-frequency real-time navigation solution result of the roadheader, including the real-time position information, real-time speed information and real-time attitude information of the roadheader, and in the EKF algorithm framework, the high-frequency attitude strapdown solution based on the inertial measurement data is taken as the state recursion process of EKF.

[0051] Specifically, the strapdown solution state equation based on the inertial measurement data is arranged as follows: The carrier (in this embodiment, the roadheader) attitude quaternion state update equation is as follows: , wherein, is the attitude quaternion to be estimated, is the three-axis gyro measurement data, is the three-axis gyro measurement bias to be estimated, is the three-axis gyro measurement noise; The carrier velocity state update equation is as follows: , wherein, are three-dimensional velocity to-be-estimated values in the mobile terminal coordinate system, are three-axis accelerometer measurement data, are three-axis accelerometer measurement bias to-be-estimated values, are three-axis accelerometer measurement noises; The carrier position state update equation is as follows: , wherein, are three-dimensional position to-be-estimated values in the mobile terminal 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 base station end binocular vision, a first group of observations, i.e., position observation information, in the EKF fusion filtering algorithm framework is constructed.

[0053] Specifically, the position information-based observation equation is as follows: , wherein, is a position measurement value of the base station measurement system 1 to the mobile terminal, is position measurement noise.

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

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

[0056] Step S54: In the EKF fusion filtering algorithm framework, after the position observation information correction and the attitude observation information correction, the real-time optimal navigation result of the roadheader, i.e., the real-time optimal attitude measurement result, is obtained. Moreover, since there are different coordinate systems (including the base station coordinate system and the mobile terminal coordinate system), as shown in FIG. 6, the coordinate conversion is performed when the EKF fusion filtering algorithm framework inputs information and outputs results. Figure 4

[0057] ​Specifically, the EKF algorithm process is as follows: Suppose that the linear discretized state equation and the measurement equation are as follows: State equation: , Measurement equation: , wherein, is the current state, is the next state, is the state transition matrix, is the noise driving matrix, is the state noise, is the measurement value, is the measurement matrix, is the measurement noise; The statistical characteristics of the state noise and the measurement noise are described by a state noise variance matrix and a measurement noise variance matrix, respectively, and the state noise variance matrix and the measurement noise variance matrix are as follows: ; After the system is discretized, the system state transition matrix , the system noise driving matrix , and the measurement matrix are obtained, and in combination with the state noise matrix and the measurement noise matrix, the Kalman filtering algorithm process is as follows: Based on the current state, the next state is estimated, and the predicted state is , The uncertainty of the predicted state is estimated based on an error covariance matrix, and the error covariance matrix is The Kalman gain is calculated as the optimal gain: , The predicted state is corrected to obtain the optimal estimation of the state , The error covariance matrix is corrected to obtain the optimal estimation of the covariance , wherein is an identity matrix.

[0058] In step S6, based on the above combined navigation solution, the motion control module calculates the pose deviation in combination with the optimal estimated pose and the real-time planned pose data, drives the execution structure of the roadheader, and accurately controls the pose of the roadheader.

[0059] Some embodiments of the tunneling machine pose measurement and control method based on bidirectional vision of the application aim at the problems of poor working environment, high labor intensity and high controllability requirement of the direction of tunneling, adopt pose measurement based on bidirectional vision measurement and inertial navigation fusion, are used to solve the problems of accurate position and attitude measurement and control of the boom-type tunneling machine in the process of autonomous coal cutting in the mine, can ensure accurate measurement of the three-dimensional position and three-axis attitude data of the tunneling machine in the process of real-time cutting and advancing in the roadway, and realize accurate pose control of the tunneling machine on the basis, and can solve the problem of the decline of pose control accuracy caused by low measurement accuracy and poor reliability of the three-dimensional position and three-axis attitude of the tunneling machine in the roadway.

[0060] The technical scope of the present application is not limited to the above description, and those skilled in the art can make various modifications and changes to the above embodiments without departing from the technical idea of the present application, and these modifications and changes should belong to the scope of the present application.

Claims

1. A method for pose measurement and control of a roadheader based on bidirectional vision, characterized in that, The method uses a tunneling machine pose measurement and control system, the system includes a base station end measurement system (1), a mobile end measurement system (2) and a motion control module, the base station end measurement system (1) is installed in the roadway where the tunneling machine works, the mobile end measurement system (2) is installed on the tunneling machine body, the base station end measurement system (1) includes a binocular vision sensor (3) and a laser lamp (4), the mobile end 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, taking the base station end measurement system (1) as a reference, a base station end coordinate system is established; Step S2, taking the mobile end measurement system (2) as a reference, a mobile end coordinate system is established; Step S3, the three-dimensional position information of the feature light source (9) in the base station end coordinate system is recognized and measured by the binocular vision sensor (3), the three-dimensional position information includes the vector information of the feature light source (9) in the base station end coordinate system and the distance information of the feature light source (9) and the binocular vision sensor (3); Step S4, the vector information of the laser beam emitted by the laser lamp (4) in the mobile end coordinate system is recognized and measured by the vision sensor (8); Step S5, based on the three-dimensional position information, the vector information of the laser beam in the mobile end coordinate system and the measurement data of the inertial measurement unit (10), combined navigation solution is carried out based on extended Kalman filtering algorithm to obtain the optimal estimation of the pose of the tunneling machine; Step S6, the motion control module combines the optimal estimated pose and the real-time planned pose data to calculate the pose deviation and carry out pose control.

2. The method of claim 1, wherein, The step S1 includes that the base station end coordinate system takes the direction of the laser beam emitted by the laser lamp (4) as the Z axis, and the binocular vision sensor (3) coordinate system coincides with the base station end coordinate system through calibration; The step S2 includes that the mobile end coordinate system takes the installation position of the inertial measurement unit (10) as the origin, takes the right direction of the tunneling machine as the X axis and takes the forward direction of the tunneling machine as the Y axis, the X axis, the Y axis and the Z axis of the mobile end coordinate system meet the right-hand screw rule, and the coordinate system of the vision sensor (8) coincides with the mobile end coordinate system through calibration.

3. The method of claim 1, wherein, The step S5 includes: Step S51, the inertial measurement unit (10) is used to carry out navigation solution on the tunneling machine to obtain real-time navigation solution result, and in the framework of extended Kalman filtering algorithm, the strapdown solution based on inertial measurement data is taken as the state recursion process of extended Kalman filtering; Step S52, based on the three-dimensional position information, a first group of observations under the framework of extended Kalman filtering algorithm is constructed; Step S53, based on the vector information of the feature light source (9) in the base station end coordinate system and the vector information of the laser beam in the mobile end coordinate system, a second group of observations under the framework of extended Kalman filtering algorithm is constructed; Step S54, under the framework of the extended Kalman filter algorithm, after the first group of observation correction and the second group of observation correction, the real-time optimal pose estimation result of the heading machine is obtained.

4. The method of claim 3, wherein, In the step S51, the real-time navigation solution result includes real-time position information, real-time speed information and real-time attitude information.

5. The method of claim 3, wherein, In the step S51, the attitude strapdown solution based on inertial measurement data includes: The attitude quaternion state update equation is constructed: ， wherein, is a pose quaternion to be estimated, is a three-axis gyroscope measurement data, is a three-axis gyroscope measurement bias to be estimated, is a three-axis gyroscope measurement noise; The velocity state update equation is constructed: ， wherein, is the three-dimensional velocity to be estimated in the mobile frame, is the three-axis accelerometer measurement data, is the three-axis accelerometer measurement bias to be estimated, is the three-axis accelerometer measurement noise. The position state update equation is constructed: ， wherein, is the three-dimensional position to be estimated in the mobile coordinate system; The sensor error state update equation is constructed: ； The step S52 includes: The position information-based observation equation is constructed: ， wherein is the measurement value of the base station end measurement system (1) for the position of the mobile end measurement system (2), is the position measurement noise; The step S53 includes: The attitude observation equation based on vector observation information is constructed: ， wherein, is the unit vector measurement information of the mobile terminal measurement system (2) in the base station coordinate system, is the unit vector measurement information of the laser beam origin in the mobile terminal coordinate system, is the vector observation noise; The step S54 includes: Assume that the linear discretized state equation and the measurement equation are respectively: ， ， wherein, is the current time state, is the next time state, is the state transition matrix, is the noise driving matrix, is the state noise, is the measurement value, is the measurement matrix, is the measurement noise; by the state noise variance matrix and the measurement noise variance matrix statistical properties of the state noise and the measurement noise are described: ; The state transition matrix is obtained after discretization , the noise driving matrix , the measurement matrix , combined with the state noise matrix and the measurement noise matrix, the Kalman filtering algorithm process is as follows: Based on the current state, the next time state is estimated, and the predicted state is , based on an estimate of an uncertainty of the predicted state, the estimate of the uncertainty of the predicted state being based on an error covariance matrix, the error covariance matrix being Computing the Kalman gain , correcting the predicted state to obtain a state optimal estimate , correcting the error covariance matrix to obtain a covariance optimal estimate wherein is the identity matrix.

6. A bidirectional vision based pose measurement and control system for a roadheader using the method according to any one of claims 1 - 5, characterized in that, The system includes a base station end measurement system (1), a mobile end measurement system (2) and a motion control module, the base station end measurement system (1) is installed in the roadway where the heading machine works, the mobile end measurement system (2) is installed on the heading machine body, the base station end measurement system (1) includes a binocular vision sensor (3) and a laser lamp (4), the mobile end measurement system (2) includes a vision sensor (8), a feature light source (9) and an inertial measurement unit (10), The laser lamp (4) is within the visual field range of the vision sensor (8), and the feature light source (9) is within the visual field range of the binocular vision sensor (3).

7. The system of claim 6, wherein, The base station end measurement system (1) is installed at the top plate center line position of the roadway, and the mobile end measurement system (2) is installed at the rear part of the heading machine body.

8. The system of claim 6, wherein, The base station end measurement system (1) includes a visual feature plate, and the visual feature plate is located next to the binocular vision sensor (3).

9. The system of claim 6, wherein, The feature light source (9) and the mobile end measurement system (2) are arranged at different positions after arm measurement.

10. The system of claim 6, wherein, The base station end measurement system (1) includes an inclinometer (5), the coordinate system of the binocular vision sensor (3) is adjusted to coincide with the base station end coordinate system through the inclinometer (5) and the laser lamp (4), The base station end measurement system (1) and the mobile end measurement system (2) both include communication data transmission equipment (7, 11) and data processing calculation unit (6, 12), the communication data transmission equipment (7, 11) is used for data transmission between the base station end measurement system (1) and the mobile end measurement system (2), and the data processing calculation unit (6, 12) is used for data processing and calculation when the heading machine pose measurement is performed.

Images