A combined calibration method for positioning and orientation of a coal mine underground heading machine

Abstract

The application discloses a kind of combined calibration methods for underground coal mine heading machine positioning and pose, including step one, obtain the calibration relationship of heading machine coordinate system and heading machine inertial navigation coordinate system;Two, obtain the calibration relationship of heading machine inertial navigation coordinate system and ultrasonic array coordinate system;Three, measure the distance of each ultrasonic sensor in ultrasonic array to smooth wall;Four, calculate the attitude error of heading machine inertial navigation coordinate system and navigation coordinate system;Five, calculate the attitude error of ultrasonic array coordinate system and navigation coordinate system;Six, calculate the plane formula of smooth wall;Seven, calculate the distance of each ultrasonic sensor in ultrasonic array to smooth wall;Eight, construct optimization equation and solve, obtain the optimal value of calibration relationship.The calibration system of the application is reasonable in design, combined with combined calibration method, can effectively complete the relative positioning and pose of heading machine, attitude conversion relationship error and the positioning and pose deviation of heading machine are small, effect is remarkable, and it is convenient for promotion.

Description

Technical Field

[0001] This invention belongs to the field of multi-sensor calibration technology, specifically relating to a joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines. Background Technology

[0002] In the process of underground tunneling in coal mines, the development of unmanned tunneling machines has gradually become a hot topic. Due to the high dust concentration during real-time tunneling, unmanned tunneling machine solutions typically use ultrasonic arrays to measure the distance between the tunneling machine and the roadway wall; at the same time, the tunneling machine is equipped with inertial navigation sensors to measure its own attitude, and the relative positioning and attitude determination of the tunneling machine is completed by the combination of ultrasonic arrays and inertial navigation sensors.

[0003] In existing technologies, before applying positioning and attitude determination to tunneling machines, mechanical installation is generally used to calibrate these two types of sensors to obtain the initial coordinate transformation relationship between them. However, in practical applications, it has been found that while mechanical installation can meet the error requirements for calibrating the translational relationship between the sensors, the attitude transformation error is relatively large, leading to significant positioning and attitude determination deviations in the tunneling machine. Therefore, a reasonably designed joint calibration method is still lacking for completing the relative positioning and attitude determination of the tunneling machine. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines, which addresses the shortcomings of the prior art. The calibration system is simple, reasonably designed, and easy to implement. Combined with the joint calibration method, it can effectively complete the relative positioning and attitude determination of the tunneling machine, with small errors in attitude conversion relationship and small positioning and attitude deviation of the tunneling machine. The effect is significant and easy to promote.

[0005] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is: a joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines, applicable to calibration systems. The calibration system includes a three-axis turntable for mounting the tunneling machine and parallel smooth walls, as well as an ultrasonic array for measuring the distance from the tunneling machine to the smooth walls. A first inertial navigation sensor for establishing the turntable's inertial navigation coordinate system is installed on the three-axis turntable, a second inertial navigation sensor for establishing the tunneling machine's inertial navigation coordinate system is installed on the tunneling machine, and multiple laser rangefinders for measuring the plane normal vector and distance of the tunneling machine are installed on the smooth walls.

[0006] The specific steps of the joint calibration method include:

[0007] Step 1: Calibrate the three-axis turntable and the tunneling machine to obtain the calibration relationship between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system;

[0008] Step 2: Calibrate the tunneling machine and the ultrasonic array to obtain the calibration relationship between the tunneling machine's inertial navigation coordinate system and the ultrasonic array's coordinate system;

[0009] Step 3: Adjust the attitude angle of the three-axis turntable so that the attitude error between the turntable's inertial navigation coordinate system and the navigation coordinate system is the set value. Use the laser rangefinder to measure the distance from the smooth wall to the center of the ultrasonic array, as well as the distance from each ultrasonic sensor in the ultrasonic array to the smooth wall.

[0010] Step 4: Calculate the attitude error between the tunneling machine's inertial navigation coordinate system and the navigation coordinate system based on the calibration relationship between the tunneling machine's coordinate system and the inertial navigation coordinate system.

[0011] Step 5: Based on the calibration relationship and translation relationship between the tunneling machine's inertial navigation coordinate system and the ultrasonic array coordinate system, calculate the attitude error between the ultrasonic array coordinate system and the navigation coordinate system.

[0012] Step 6: Based on the attitude errors of the tunneling machine's inertial navigation coordinate system and the navigation coordinate system, and the attitude errors of the ultrasonic array coordinate system and the navigation coordinate system, calculate the planar formula of the smooth wall in the ultrasonic array coordinate system.

[0013] Step 7: Based on the calibration relationship between each ultrasonic sensor and the ultrasonic array and the planar formula of the smooth wall, calculate the distance from each ultrasonic sensor in the ultrasonic array to the smooth wall.

[0014] Step 8: Based on the distances from each ultrasonic sensor to the smooth wall measured by the laser rangefinder in Step 1 and the distances from each ultrasonic sensor to the smooth wall calculated in Step 5, construct an optimization equation and solve it to obtain the optimal value of the calibration relationship.

[0015] The aforementioned joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines uses an ultrasonic array arranged in a 4*3 configuration in both horizontal and vertical directions.

[0016] The aforementioned joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines uses three laser rangefinders.

[0017] The aforementioned joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines includes the following specific steps in step one: calibrating the three-axis turntable and the tunneling machine to obtain the calibration relationship between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system:

[0018] Step 101: Initially align the three-axis turntable to obtain the initial attitude error angle from the tunneling machine coordinate system to the navigation coordinate system;

[0019] Step 102: Align the turntable inertial navigation coordinate system with the tunneling machine inertial navigation coordinate system to obtain the rotation matrix between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system;

[0020] Step 103: Determine the calibration relationship between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system based on the rotation matrix.

[0021] The above-mentioned joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines, wherein the transfer alignment in step 102 adopts a transfer alignment method based on attitude acceleration matching using Kalman filtering.

[0022] The aforementioned joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines, specifically the second step of calibrating the tunneling machine and the ultrasonic array to obtain the calibration relationship between the tunneling machine's inertial navigation coordinate system and the ultrasonic array coordinate system, includes the following steps:

[0023] Step 201: Adjust the three-axis turntable so that the inertial navigation coordinate system of the tunneling machine coincides with the navigation coordinate system. Use a laser rangefinder to measure the ultrasonic array plane and use three points to calculate the plane normal vector.

[0024] Step 202: Calculate the rotation matrix between the ultrasonic array coordinate system and the tunneling machine inertial navigation coordinate system based on the plane normal vector;

[0025] Step 203: Illuminate the center position of the ultrasonic array with a single laser rangefinder and record the distance; at the same time, record the distance of each ultrasonic sensor in the ultrasonic array and calculate the solution space in the normalized coordinate system.

[0026] Step 204: Adjust the attitude angle of the three-axis turntable, use a single laser rangefinder to illuminate the center position of the ultrasonic array and record the distance; at the same time, record the distance from each ultrasonic sensor to the wall, and further calculate the solution space in the normalized coordinate system.

[0027] Step 205: Solve for the intersection of the solution space, use the gradient descent method to solve for the optimal solution, obtain the optimal normalized coordinates of each ultrasonic sensor in the ultrasonic coordinate system, and then obtain the pitch and yaw error angles.

[0028] Step 206: Repeat steps 201 to 205 to obtain the calibration relationship between the tunneling machine inertial navigation coordinate system and the ultrasonic array coordinate system.

[0029] In the above-mentioned joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines, the translation relationship described in step five is determined according to the mechanical installation method.

[0030] Compared with the prior art, the present invention has the following advantages: the calibration system of the present invention is simple, reasonably designed, and easy to implement. Combined with the joint calibration method, it can effectively complete the relative positioning and attitude determination of the tunneling machine. The attitude conversion relationship error and the positioning and attitude deviation of the tunneling machine are small, the effect is significant, and it is easy to promote.

[0031] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0032] Figure 1 This is a flowchart of the method of the present invention. Detailed Implementation

[0033] The present invention provides a joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines. This method is applicable to calibration systems, which include a three-axis turntable for mounting the tunneling machine and parallel smooth walls, as well as an ultrasonic array for measuring the distance from the tunneling machine to the smooth walls. The three-axis turntable is equipped with a first inertial navigation sensor for establishing the turntable's inertial navigation coordinate system, and the tunneling machine is equipped with a second inertial navigation sensor for establishing the tunneling machine's inertial navigation coordinate system. Multiple laser rangefinders are installed on the smooth walls for measuring the normal vector and distance to the plane above the tunneling machine.

[0034] like Figure 1 As shown, the specific steps of the joint calibration method include:

[0035] Step 1: Calibrate the three-axis turntable and the tunneling machine to obtain the calibration relationship between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system;

[0036] Step 2: Calibrate the tunneling machine and the ultrasonic array to obtain the calibration relationship between the tunneling machine's inertial navigation coordinate system and the ultrasonic array's coordinate system;

[0037] Step 3: Adjust the attitude angle of the three-axis turntable so that the attitude error between the turntable's inertial navigation coordinate system and the navigation coordinate system is the set value. Use the laser rangefinder to measure the distance from the smooth wall to the center of the ultrasonic array, as well as the distance from each ultrasonic sensor in the ultrasonic array to the smooth wall.

[0038] Step 4: Calculate the attitude error between the tunneling machine's inertial navigation coordinate system and the navigation coordinate system based on the calibration relationship between the tunneling machine's coordinate system and the inertial navigation coordinate system.

[0039] Step 5: Based on the calibration relationship and translation relationship between the tunneling machine's inertial navigation coordinate system and the ultrasonic array coordinate system, calculate the attitude error between the ultrasonic array coordinate system and the navigation coordinate system.

[0040] Step 6: Based on the attitude errors of the tunneling machine's inertial navigation coordinate system and the navigation coordinate system, and the attitude errors of the ultrasonic array coordinate system and the navigation coordinate system, calculate the planar formula of the smooth wall in the ultrasonic array coordinate system.

[0041] Step 7: Based on the calibration relationship between each ultrasonic sensor and the ultrasonic array and the planar formula of the smooth wall, calculate the distance from each ultrasonic sensor in the ultrasonic array to the smooth wall.

[0042] Step 8: Based on the distances from each ultrasonic sensor to the smooth wall measured by the laser rangefinder in Step 1 and the distances from each ultrasonic sensor to the smooth wall calculated in Step 5, construct an optimization equation and solve it to obtain the optimal value of the calibration relationship.

[0043] In this embodiment, the ultrasonic array is arranged in a 4*3 pattern in both the horizontal and vertical directions.

[0044] In this embodiment, the number of laser rangefinders is three.

[0045] In this embodiment, the specific process of calibrating the three-axis turntable and the tunneling machine in step one to obtain the calibration relationship between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system includes:

[0046] Step 101: Initially align the three-axis turntable to obtain the initial attitude error angle from the tunneling machine coordinate system to the navigation coordinate system;

[0047] In practice, the tunneling machine coordinate system is regarded as the turntable inertial navigation coordinate system, and the attitude angle error is 0.

[0048] Step 102: Align the turntable inertial navigation coordinate system with the tunneling machine inertial navigation coordinate system to obtain the rotation matrix between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system;

[0049] Step 103: Determine the calibration relationship between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system based on the rotation matrix.

[0050] In this embodiment, the transfer alignment in step 102 adopts a transfer alignment method based on attitude acceleration matching using Kalman filtering.

[0051] In practical implementation, the system equations are as follows:

[0052] State variables:

[0053] Where φ n For attitude angle, δv n For velocity error, δp n For position error, δp n For gyroscope error, To account for error, λ s The main inertial navigation system installation deviation angle is increased compared to the traditional 15-dimensional matrix.

[0054] Equations of state:

[0055] Where F(t) and G(t) are deterministic matrix functions, and W(t) is a zero-mean Gaussian white noise vector, all three of which are the same as those in the traditional fifteen-dimensional state equation.

[0056] Observation equation:

[0057] In this embodiment, the specific process of calibrating the tunneling machine and the ultrasonic array in step two to obtain the calibration relationship between the tunneling machine's inertial navigation coordinate system and the ultrasonic array's coordinate system includes:

[0058] Step 201: Adjust the three-axis turntable so that the inertial navigation coordinate system of the tunneling machine coincides with the navigation coordinate system. Use a laser rangefinder to measure the ultrasonic array plane and use three points to calculate the plane normal vector.

[0059] Step 202: Calculate the rotation matrix between the ultrasonic array coordinate system and the tunneling machine inertial navigation coordinate system based on the plane normal vector;

[0060] Step 203: Illuminate the center position of the ultrasonic array with a single laser rangefinder and record the distance; at the same time, record the distance of each ultrasonic sensor in the ultrasonic array and calculate the solution space in the normalized coordinate system.

[0061] Step 204: Adjust the attitude angle of the three-axis turntable, use a single laser rangefinder to illuminate the center position of the ultrasonic array and record the distance; at the same time, record the distance from each ultrasonic sensor to the wall, and further calculate the solution space in the normalized coordinate system.

[0062] Step 205: Solve for the intersection of the solution space, use the gradient descent method to solve for the optimal solution, obtain the optimal normalized coordinates of each ultrasonic sensor in the ultrasonic coordinate system, and then obtain the pitch and yaw error angles.

[0063] Step 206: Repeat steps 201 to 205 to obtain the calibration relationship between the tunneling machine inertial navigation coordinate system and the ultrasonic array coordinate system.

[0064] In this embodiment, the translation relationship described in step five is determined based on the mechanical installation method.

[0065] The above description is merely a preferred embodiment of the present invention and does not constitute any limitation on the present invention. Any simple modifications, alterations, or equivalent structural changes made to the above embodiments based on the technical essence of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines, characterized in that, The calibration system is applicable to a calibration system comprising a three-axis turntable for mounting a tunneling machine and a parallel smooth wall, and an ultrasonic array for measuring the distance from the tunneling machine to the smooth wall; a first inertial navigation sensor for establishing the turntable's inertial navigation coordinate system is mounted on the three-axis turntable, a second inertial navigation sensor for establishing the tunneling machine's inertial navigation coordinate system is mounted on the tunneling machine, and multiple laser rangefinders for measuring the plane normal vector and distance on the smooth wall are mounted on the smooth wall; The specific steps of the joint calibration method include: Step 1: Calibrate the three-axis turntable and the tunneling machine to obtain the calibration relationship between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system; The specific process of calibrating the three-axis turntable and the tunneling machine in step one to obtain the calibration relationship between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system includes: Step 101: Initially align the three-axis turntable to obtain the initial attitude error angle from the tunneling machine coordinate system to the navigation coordinate system; Step 102: Align the turntable inertial navigation coordinate system with the tunneling machine inertial navigation coordinate system to obtain the rotation matrix between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system; Step 103: Determine the calibration relationship between the tunneling machine coordinate system and the tunneling machine inertial navigation coordinate system based on the rotation matrix; Step 2: Calibrate the tunneling machine and the ultrasonic array to obtain the calibration relationship between the tunneling machine's inertial navigation coordinate system and the ultrasonic array's coordinate system; Step two, which involves calibrating the tunneling machine and the ultrasonic array to obtain the calibration relationship between the tunneling machine's inertial navigation coordinate system and the ultrasonic array's coordinate system, includes the following specific steps: Step 201: Adjust the three-axis turntable so that the inertial navigation coordinate system of the tunneling machine coincides with the navigation coordinate system. Use a laser rangefinder to measure the ultrasonic array plane and use three points to calculate the plane normal vector. Step 202: Calculate the rotation matrix between the ultrasonic array coordinate system and the tunneling machine inertial navigation coordinate system based on the plane normal vector; Step 203: Illuminate the center position of the ultrasonic array with a single laser rangefinder and record the distance; at the same time, record the distance of each ultrasonic sensor in the ultrasonic array and calculate the solution space in the normalized coordinate system. Step 204: Adjust the attitude angle of the three-axis turntable, use a single laser rangefinder to illuminate the center position of the ultrasonic array and record the distance; at the same time, record the distance from each ultrasonic sensor to the wall, and further calculate the solution space in the normalized coordinate system. Step 205: Solve for the intersection of the solution space, use the gradient descent method to solve for the optimal solution, obtain the optimal normalized coordinates of each ultrasonic sensor in the ultrasonic coordinate system, and then obtain the pitch and yaw error angles. Step 206: Repeat steps 201 to 205 to obtain the calibration relationship between the tunneling machine inertial navigation coordinate system and the ultrasonic array coordinate system; Step 3: Adjust the attitude angle of the three-axis turntable so that the attitude error between the turntable's inertial navigation coordinate system and the navigation coordinate system is the set value. Use the laser rangefinder to measure the distance from the smooth wall to the center of the ultrasonic array, as well as the distance from each ultrasonic sensor in the ultrasonic array to the smooth wall. Step 4: Calculate the attitude error between the tunneling machine's inertial navigation coordinate system and the navigation coordinate system based on the calibration relationship between the tunneling machine's coordinate system and the inertial navigation coordinate system. Step 5: Based on the calibration relationship and translation relationship between the tunneling machine's inertial navigation coordinate system and the ultrasonic array coordinate system, calculate the attitude error between the ultrasonic array coordinate system and the navigation coordinate system. Step 6: Based on the attitude errors of the tunneling machine's inertial navigation coordinate system and the navigation coordinate system, and the attitude errors of the ultrasonic array coordinate system and the navigation coordinate system, calculate the planar formula of the smooth wall in the ultrasonic array coordinate system. Step 7: Based on the calibration relationship between each ultrasonic sensor and the ultrasonic array and the planar formula of the smooth wall, calculate the distance from each ultrasonic sensor in the ultrasonic array to the smooth wall. Step 8: Based on the distances from each ultrasonic sensor to the smooth wall measured by the laser rangefinder in Step 1 and the distances from each ultrasonic sensor to the smooth wall calculated in Step 5, construct an optimization equation and solve it to obtain the optimal value of the calibration relationship.

2. The joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines according to claim 1, characterized in that: The ultrasonic array is arranged in a 4*3 configuration in both the horizontal and vertical directions.

3. The joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines according to claim 1, characterized in that: The number of laser rangefinders is three.

4. The joint calibration method for positioning and attitude determination of underground tunneling machines in coal mines according to claim 1, characterized in that, The transfer alignment described in step 102 adopts a transfer alignment method based on attitude acceleration matching using Kalman filtering.

5. A joint calibration method for positioning and attitude determination of an underground tunneling machine in a coal mine, as described in claim 1, characterized in that... The translation relationship described in step five is determined based on the mechanical installation method.

Images