Coal mining machine working face end position and posture calibration device and method based on ring array IMU and UWB

Abstract

The application discloses a kind of ring array IMU and UWB's coal winning machine working face end pose calibration device and method, including coal winning machine body, ring fixing device, explosion-proof housing, main inertial navigation positioning device, high-precision attitude sensor, data display system, industrial computer, data processor, UWB positioning system, IMU positioning system, base station group autonomous migration system and hydraulic height adjustment system;After coal winning machine completes end positioning, base station group autonomous migration is controlled using base station group autonomous migration system, and the position coordinates of base station group after migration can be quickly solved by navigation migration algorithm;End positioning is carried out to coal winning machine using ring array IMU and UWB, and the pose of main inertial navigation positioning device is independently calibrated using positioning result, the positioning precision of coal winning machine independent positioning is improved, manual calibration is avoided when coal winning machine stops, long-term continuous independent cyclic positioning of coal winning machine is realized, and it is helpful to realize intelligent coal mining.

Description

Technical Field

[0001] This invention belongs to the field of underground coal mining machine end calibration technology, specifically relating to a coal mining machine working face end pose calibration device and method using a ring array IMU and UWB. Background Technology

[0002] Achieving intelligent, unmanned mining is a cutting-edge technology pursued by the international coal mining industry and represents a technological leap forward in solving the problems of safe, efficient, and green coal mining in my country. In fully mechanized mining faces, the coal mining machine is the core equipment. It directly acts on the coal and rock to be mined, while the scraper conveyor and hydraulic supports serve the machine, enabling coal cutting and loading. The key to intelligent mining is the intelligentization of the coal mining machine. The three-dimensional positioning of the coal mining machine on the working face is crucial for realizing intelligent fully mechanized mining faces; therefore, the accurate three-dimensional coordinates of the coal mining machine play a decisive role in achieving intelligent operation. Although many domestic and international scholars have proposed various positioning methods for coal mining machines in recent years, the current positioning technology used for coal mining machines has a large positioning error and cannot accurately determine the three-dimensional coordinates of the machine, which directly affects the intelligent development of coal mining machines. Currently, strapdown inertial navigation technology is widely used for coal mining machine positioning, but inertial measurement elements drift over time and distance. After several cycles, the positioning error of the inertial navigation will accumulate and increase, gradually decreasing the positioning accuracy. Therefore, other positioning technologies are needed to calibrate the inertial navigation device.

[0003] Ultra-wideband (UWB) positioning technology has advantages such as strong anti-interference ability, strong multipath resistance, strong penetration, and centimeter-level error within the line-of-sight range, making it possible to position underground coal mining machines. Application number CN201911254913.5 discloses an autonomous navigation system and method for coal mining machines. This system involves installing a group of fixed UWB base stations at the intersection of the fully mechanized mining face and the transport roadway, and a group of mobile UWB base stations installed below the top plate of the hydraulic support. The frequent movement of the hydraulic support causes continuous changes in the coordinates of the base station group, making it difficult to accurately obtain the position coordinates of the base station group, resulting in a large positioning error. Application number CN201410749250.5 discloses a method and device for precise calibration of the absolute position of a coal mining machine based on UWB. Several UWB base stations are evenly arranged in the roadway. A precise calibration model of the absolute position of the coal mining machine is established in the host computer based on the absolute coordinates of the UWB base stations. This method can achieve end-face positioning calibration of the coal mining machine. However, after the coal mining machine completes one cut, the UWB base stations need to be manually moved, making continuous cyclic cutting of the coal mining machine impossible. Application number 201910235259.7 discloses a device and method for end-face calibration of a coal mining machine based on UWB. This method deploys UWB base stations in the roadway to perform end-face calibration of the coal mining machine. However, this method cannot automatically adjust the height and relative position of the UWB base stations in the roadway.

[0004] The precise position coordinates of the coal mining machine are crucial for intelligent mining. Existing positioning and calibration technologies require manual calibration of the coal mining machine during practical applications, making it difficult to achieve long-term autonomous cyclic cutting. Moreover, the positioning accuracy of the coal mining machine is relatively low, failing to meet the requirements of automation in fully mechanized mining faces. There is still a gap between the current technology and the goal of autonomous and precise positioning. New methods are needed to solve the problem of long-term autonomous cyclic positioning of the coal mining machine. Summary of the Invention

[0005] Technical problem to be solved: In view of the above-mentioned technical problems, the present invention provides a device and method for calibrating the pose of the working face of a coal mining machine using a ring array IMU and UWB, which can effectively solve the problems of low positioning accuracy of inertial navigation positioning devices and the inability of coal mining machines to achieve automatic cyclic cutting in the prior art.

[0006] Technical solution: In the first aspect, the present invention provides a coal mining machine working face end posture calibration device with ring array IMU and UWB, including coal mining machine body, ring fixing device, explosion-proof shell, data processor, main inertial navigation and positioning device, high-precision attitude sensor, data display system, industrial control computer, UWB positioning system, IMU positioning system, base station group autonomous migration system and hydraulic height adjustment system;

[0007] The UWB positioning system includes three UWB positioning terminal modules and four UWB positioning base stations. The IMU positioning system includes six IMU modules. The base station group autonomous migration system includes a first slide, a slider, a pole, a UWB positioning base station, a UWB support rod, a connecting plate, a non-standard hole connecting fastener, a base station fixing device, a vertical slider, a first stepper motor, a first coupling, a transmission shaft, an electric slide shaft, a second slide, a second stepper motor, and a second coupling. The hydraulic height adjustment system includes a fixing plate, a telescopic cylinder, and a hydraulically controllable electric pump.

[0008] The explosion-proof enclosure is installed on the body of the coal mining machine, the annular fixing device is installed inside the explosion-proof enclosure, the main inertial navigation and positioning device is installed at the center of the annular fixing device, the UWB positioning terminal module and the IMU module are both installed on the annular fixing device, the UWB positioning terminal module is connected to the data processor through a multi-serial port hub, the output end of the data processor is connected to the industrial control computer, the industrial control computer is installed in the roadway, and the signal input end of the data display system is connected to the signal output end of the industrial control computer.

[0009] The upright is connected to three UWB support rods from top to bottom via three different-hole connecting fasteners. The base station fixing device is set at either end of the UWB support rod and one end of the upright. The UWB positioning base station is set on the base station fixing device, and the height difference between two adjacent UWB positioning base stations is greater than 0.5m. The other end of the upright is set on a connecting plate. The bottom of the connecting plate is set on a slider. The slider is set on an electric slide shaft. The electric slide shaft is set on a first slide. The first stepper motor is connected to the first slide via a first coupling. The bottom of the first slide is connected to two second slides via a vertical slider. The two second slides are connected by a transmission shaft. The second stepper motor is connected to one of the second slides via a second coupling. The fixing plate is set at the bottom of the second slide. The hydraulically controllable electric pump is connected to the fixing plate via a telescopic cylinder. The hydraulically controllable electric pump, the first stepper motor, the second stepper motor, and the IMU positioning system are all connected to the input end of the data processor.

[0010] Preferably, the three UWB positioning terminal modules and the six IMU modules are arranged in a ring array on the ring fixing device, with two IMU modules sandwiched between any two UWB positioning terminal modules, and are on the same plane.

[0011] Preferably, the non-circular connection fastener is connected to the UWB support rod by a locking screw.

[0012] This invention provides a method for calibrating the pose of the working face end of a coal mining machine using a ring array IMU and UWB, comprising the following steps:

[0013] S1. Based on the working conditions of the roadway at the end of the coal mining face, the hydraulic height adjustment system is deployed in the roadway. The second slide, the first slide, and the autonomous migration system of the base station group are installed in sequence. The four UWB positioning base stations are respectively set on the base station fixing device at the top of the pole and on the base station fixing device at any end of the three UWB support poles. The relative positions of the four UWB positioning base stations are adjusted so that the wireless communication path between each UWB positioning base station and the three UWB positioning terminal modules is in a line-of-sight environment.

[0014] S2. Establish a navigation coordinate system, use a total station to measure the position coordinates of four UWB positioning base stations, and input the coordinates of the four UWB positioning base stations into the industrial control computer. At the same time, network the three UWB positioning terminal modules with the four UWB positioning base stations.

[0015] S3. Determine whether the coal mining machine has reached 10m from the end of the coal face by using limit switches. If the limit switches are not triggered, obtain the position information of the coal mining machine using the main inertial navigation and positioning device. If the coal mining machine has reached 10m from the end of the coal face, activate the IMU positioning system and UWB positioning system of the ring array. The main inertial navigation and positioning device, IMU positioning system, and UWB positioning system simultaneously locate the coal mining machine. The data from the six IMU modules and three UWB positioning terminal modules are transmitted to the industrial control computer in the roadway through the data processor. In the data processor, the main inertial navigation and positioning device obtains the position information P of the coal mining machine. INS and attitude information Ω INS The final positioning results P of the three UWB positioning terminal modules were obtained using the extended Kalman filter algorithm. UWB The position P of the IMU positioning system is calculated using navigation algorithms. IMU and posture Ω IMU ;

[0016] S4, P IMU With P UWB The difference is calculated to obtain ΔP1, and ΔP1 is input into the filter in the data processor. The filtered information ΔP IMU As a feedback quantity, the fused positioning results of the IMU positioning system are corrected and compensated to obtain the final positioning result P of the IMU positioning system. IMUR ;

[0017] S5, P INS With P IMUR The difference is used to obtain ΔP2, and Ω is then... INS With Ω IMU The difference is calculated to obtain ΔΩ. ΔP2 and ΔΩ are used as inputs to the fusion filter, and the filtered output ΔP is used... INS and ΔΩ INS The position and attitude of the coal mining machine are calibrated and compensated.

[0018] S6. Use limit switches to determine whether the coal mining machine has fully moved to the end of the coal mining face. If the coal mining machine has not moved to the end position, continue to use the ring array IMU positioning system and UWB positioning system to calibrate the position end of the coal mining machine. If the coal mining machine has moved to the end position of the coal mining face, complete the autonomous end calibration of the coal mining machine and turn off the UWB positioning system and IMU positioning system.

[0019] S7. The stepper motor-controlled base station group autonomous migration system autonomously migrates along the direction of the coal mining machine's cutting. The distance the base station group autonomous migration system migrates on the first slide is the same as the cutting amount of the coal mining machine. Based on the migration distance and the attitude information of the high-precision attitude sensor, the position coordinates of the base station group after migration are calculated.

[0020] S8. After the base station group autonomous migration system moves 8 times, the UWB base station group is re-measured and calibrated using a total station. Then, steps S2-S7 are repeated to autonomously calibrate the position and orientation of the coal mining machine.

[0021] Preferably, in step S3, the measured distance of each UWB positioning base station is reconstructed using a variational Bayesian maximum entropy unscented Kalman filter algorithm. The three-dimensional position coordinates of the three UWB positioning terminal modules are then solved using a maximum likelihood estimation algorithm or a shrinkage estimation algorithm based on the minimum mean square error criterion. After the three UWB positioning terminal modules are installed and fixed, they are numbered 1, 2, and 3. The relative distance between two modules is used as the measurement, as follows:

[0022]

[0023] Where r 12 r represents the relative distance between UWB positioning terminal module 1 and UWB positioning terminal module 2. 13 The relative distance r between UWB positioning terminal module 1 and UWB positioning terminal module 3 is indicated by the following expression: 23 The relative distance between UWB positioning terminal module 2 and UWB positioning terminal module 3 is represented by (X1,Y1,Z1), (X2,Y2,Z2) and (X3,Y3,Z3), which represent the position coordinates of the three UWB positioning terminal modules obtained by the positioning algorithm.

[0024] Performing a first-order Taylor expansion on the measurement yields the Jacobian matrix H:

[0025]

[0026] The measurement equation for the Kalman filter is further extended to Z. k =HX k +V k The output of the extended Kalman filter is used as the positioning result P of the UWB positioning system. UWB Where k represents time k, X k Denotes a state variable, and X k =[X1,Y1,Z1,X2,Y2,Z2,X3,Y3,Z3] T Z k Indicates the quantity measurement, and Z k =[r 12 ,r 23 ,r 13 ] T V k This indicates measurement noise.

[0027] Preferably, variational capacitive Kalman filtering is used to denoise the measurement data from the six IMU modules in step S3. In step S3, the data used to solve for the position and attitude of the coal mining machine are used as the fused angular velocities and accelerations output by the six IMU modules of the ring array. The fused angular velocities and accelerations of the six IMU modules are as follows:

[0028]

[0029]

[0030] In the formula w xr ,w yr ,w zr w represents the true theoretical angular velocity in the three coordinate axes. xd1 ,w xd2 ,w xd3 ,w xd4 ,w xd5 ,w xd6 These represent the angular velocity drift deviations along the x-axis of the six IMU modules, w yd1 ,w yd2 ,w yd3 ,w yd4 ,w yd5 ,w yd6 These represent the angular velocity drift deviations along the y-axis of the six IMU modules, w zd1 ,w zd2 ,w zd3 ,w zd4 ,w zd5 ,w zd6 These represent the angular velocity drift deviations of the six IMU modules along the z-axis, a xr ,a yr ,a zr Let a represent the actual theoretical acceleration in the three coordinate axes respectively. xd1 ,a xd2 ,a xd3 ,a xd4 ,a xd5 ,a xd6 These represent the acceleration drift deviations along the x-axis of the six IMU modules, a and b respectively. yd1 ,a yd2 ,a yd3 ,a yd4 ,a yd5 ,a yd6 These represent the acceleration drift deviations along the y-axis of the six IMU modules, a zd1 ,a zd2 ,a zd3 ,a zd4 ,a zd5 ,a zd6These represent the acceleration drift deviations of the six IMU modules along the z-axis.

[0031] Preferably, the method for calculating the coordinates of the base station group is as follows: assuming the distance the base station group moves on the first sliding platform is s, and the displacement increment ΔS of the base station group in the navigation coordinate system is... n Then we have:

[0032]

[0033] In the formula, θ represents the pitch angle and γ represents the roll angle. Indicates the heading angle;

[0034] The initial coordinates of the base station group were measured using a total station, with the coordinates C. i (0)=(x i ,y i ,z i After N migrations, the coordinates of the base station group are:

[0035]

[0036] In the formula C i (N) represents the coordinates of the base station group after N migrations, where i represents the number of the location base station, i = 1, 2, 3, 4.

[0037] Beneficial effects: (1) The hydraulic automatic height adjustment system of the present invention can adjust the height of the base station group freely according to the geological morphology at the end of the coal mining face by adjusting the hydraulic cylinder; and adjust the position of the base station group by controlling the moving distance of the second slide.

[0038] (2) The design of the autonomous migration system for the base station group can freely adjust the relative position of each UWB positioning base station, which can effectively avoid the coal wall blocking the propagation channel of the positioning terminal module of the base station; by using a stepper motor to control the autonomous migration system for the base station group, the base station group can be autonomously migrated, avoiding manual relocation of the base station group; by using the dead reckoning algorithm, the position coordinates of each UWB positioning base station can be quickly calculated, preparing for the next coal mining face end positioning.

[0039] (3) By using a ring array structure to arrange six IMU modules, the influence of environmental factors on the time-varying drift of the inertial unit can be reduced. The variational capacitive Kalman filter is used to denoise the IMU measurement results, which further reduces the impact of errors. The variational Bayesian maximum entropy unscented Kalman filter algorithm is used to smooth the UWB measurement distance, which improves the ranging accuracy. After the three UWB positioning terminal modules are installed and fixed, the relative distance between the two modules is used as a fixed constraint condition. The extended Kalman filter algorithm is used to obtain the final positioning result of the UWB system, which further improves the positioning accuracy of the UWB positioning system.

[0040] (4) The ring array IMU and UWB can achieve high positioning accuracy for the end positioning of the coal mining machine. The positioning results can be used to perform autonomous calibration of the inertial navigation positioning device, which can reduce the cumulative error of the inertial navigation positioning system, improve the positioning accuracy of the coal mining machine's autonomous positioning, avoid the coal mining machine from stopping for manual calibration, and realize the long-term continuous autonomous cyclic positioning of the coal mining machine. Attached Figure Description

[0041] Figure 1 This is a schematic diagram of the position and posture calibration of the coal mining machine at the end of the coal mining face according to the present invention;

[0042] Figure 2 This is a schematic diagram of the support structure for the UWB positioning base station group of the present invention;

[0043] Figure 3 This is a schematic diagram of the UWB base station group autonomous migration device of the present invention;

[0044] Figure 4 This is a schematic diagram of the sensor wiring of the present invention;

[0045] Figure 5 This is a layout diagram of the IMU module and UWB positioning terminal of the present invention;

[0046] Figure 6 This is a schematic diagram illustrating the principle of the coal mining machine position and posture calibration at the end of the coal mining face according to the present invention.

[0047] Figure 7 This is a flowchart of the coal mining machine posture calibration method at the working face end of the present invention;

[0048] In the diagram: 1. Coal mining machine body; 2. Circular fixing device; 3. IMU module; 4. UWB positioning terminal module; 5. Explosion-proof housing; 6. Data processor; 7. Main inertial navigation and positioning device; 8. First slide; 9. Slider; 10. Upright pole; 11. UWB positioning base station; 12. UWB support rod; 13. Bolt; 14. Connecting plate; 15. Locking screw; 16. Different hole connection fastener; 17. Base station fixing device; 18. Vertical slider; 19. First stepper motor; 20. First coupling; 21. Drive shaft; 22. Electric slide shaft; 23. Second slide; 24. Fixing plate; 25. Telescopic cylinder; 26. High-precision attitude sensor; 27. Data display system; 28. Industrial control computer; 29. ​​Hydraulic controllable electric pump; 30. Second stepper motor; 31. Second coupling. Detailed Implementation

[0049] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments:

[0050] Example 1

[0051] like Figures 1-5 As shown, a coal mining machine face end pose calibration device using a ring array IMU and UWB is characterized by comprising: a coal mining machine body 1, a ring fixing device 2, an explosion-proof shell 5, a data processor 6, a main inertial navigation and positioning device 7, a high-precision attitude sensor 26, a data display system 27, an industrial control computer 28, a UWB positioning system, an IMU positioning system, a base station group autonomous migration system, and a hydraulic height adjustment system;

[0052] The UWB positioning system includes three UWB positioning terminal modules 4 and four UWB positioning base stations 11. The IMU positioning system includes six IMU modules 3. The autonomous migration system of the base station group includes a first slide 8, a slider 9, a pole 10, UWB positioning base stations 11, UWB support rods 12, a connecting plate 14, a non-standard hole connecting fastener 16, a base station fixing device 17, a vertical slider 18, a first stepper motor 19, a first coupling 20, a transmission shaft 21, an electric slide shaft 22, a second slide 23, a second stepper motor 30, and a second coupling 31. The hydraulic height adjustment system includes a fixed plate 24, a telescopic cylinder 25, and a hydraulically controllable electric pump 29, used to control the lifting height of the autonomous migration system of the base station group.

[0053] The explosion-proof housing 5 is installed on the coal mining machine body 1. The annular fixing device 2 is installed inside the explosion-proof housing 5. The main inertial navigation and positioning device 7 is installed at the center of the annular fixing device 2. Three UWB positioning terminal modules 4 and six IMU modules 3 are arranged in a ring array on the annular fixing device 2, with two IMU modules 3 sandwiched between any two UWB positioning terminal modules 4, and are on the same plane, which can reduce the drift effect of environmental factors on the inertial unit. The UWB positioning terminal module 4 is connected to the data processor 6 through a multi-serial port hub. The output of the data processor 6 is connected to the industrial control computer 28, which is installed in the roadway. The signal input of the data display system 27 is connected to the signal output of the industrial control computer 28.

[0054] The upright 10 is connected to three UWB support rods 12 from top to bottom via three different-hole connecting fasteners 16. The different-hole connecting fasteners 16 are connected to the UWB support rods 12 by locking screws 15 to prevent the UWB positioning base station 11 from shifting and reducing the positioning accuracy of the UWB positioning system. The base station fixing device 17 is located at either end of the UWB support rod 12 and one end of the upright 10. The UWB positioning base station 11 is mounted on the base station fixing device 17, and the height difference between two adjacent UWB positioning base stations 11 is greater than 0.5m. The different-hole connecting fasteners 16 can move on the upright 10 to facilitate adjustment of the relative positions between the UWB positioning base stations 11. The other end of the upright 10 is mounted on a connecting plate 14. The bottom of the connecting plate 14 is connected to the upper surface of the slider 9 by bolts 13. The lower end face is set on the electric slide shaft 22, which is set on the first slide 8. The first stepper motor 19 is connected to the first slide 8 through the first coupling 20. The bottom of the first slide 8 is connected to two second slides 23 through the vertical slider 18. The two second slides 23 are connected by the transmission shaft 21. The second stepper motor 30 is connected to one of the second slides 23 through the second coupling 31. The fixing plate 24 is set on the bottom of the second slide 23. The hydraulically controllable electric pump 29 is connected to the fixing plate 24 through the telescopic cylinder 25. The hydraulically controllable electric pump 29, the first stepper motor 19, the second stepper motor 30 and the IMU positioning system are all connected to the input end of the data processor 6. The output end of the data processor 6 is connected to the input end of the industrial control computer 28 through the serial port to realize remote control.

[0055] Example 2

[0056] A method for calibrating the pose of a coal mining machine at the working face using a ring array IMU and UWB, specifically a method for calibrating the pose of a coal mining machine using the device described in Example 1. Figure 7 As shown, it includes the following steps:

[0057] S1. Based on the working conditions of the roadway at the end of the coal mining face, the hydraulic height adjustment system is deployed in the roadway. The second slide 23, the first slide 8 and the base station group autonomous migration system are installed in sequence. The four UWB positioning base stations 11 are respectively set on the base station fixing device 17 at the top of the pole 10 and the base station fixing device 17 at any end of the three UWB support poles 12. The relative positions of the four UWB positioning base stations 11 are adjusted so that the wireless communication path between each UWB positioning base station 11 and the three UWB positioning terminal modules 4 is in a line-of-sight environment.

[0058] S2. Establish a navigation coordinate system, use a total station to measure the position coordinates of four UWB positioning base stations 11, and input the coordinates of the four UWB positioning base stations 11 into the industrial control computer 28. At the same time, network the three UWB positioning terminal modules 4 with the four UWB positioning base stations 11.

[0059] S3. The limit switch is used to determine whether the coal mining machine has reached a distance of 10m from the end of the coal face. If the limit switch is not triggered, the main inertial navigation and positioning device 7 is used to obtain the position information of the coal mining machine. If the coal mining machine has reached a distance of 10m from the end of the coal face, the IMU positioning system and UWB positioning system of the ring array are activated. The main inertial navigation and positioning device 7, the IMU positioning system and the UWB positioning system simultaneously locate the coal mining machine. The data from the six IMU modules 3 and the three UWB positioning terminal modules 4 are transmitted to the industrial control computer 28 in the roadway through the data processor 6. In the data processor 6, the main inertial navigation and positioning device 7 obtains the position information P of the coal mining machine. INS and attitude information Ω INS The final positioning results P of the three UWB positioning terminal modules 4 were obtained using the extended Kalman filter algorithm. UWB The position P of the IMU positioning system is calculated using navigation algorithms. IMU and posture Ω IMU ;

[0060] Figure 5 In this study, variational capacitive Kalman filtering is used to denoise the measurement data from the six IMU modules. The data obtained by solving for the position and attitude of the coal mining machine are used as the fused angular velocities and accelerations output from the six IMU modules in the ring array. The fused angular velocities and accelerations of the six IMU modules are as follows:

[0061]

[0062]

[0063] In the formula w xr ,w yr ,w zr w represents the true theoretical angular velocity in the three coordinate axes. xd1 ,w xd2 ,w xd3 ,w xd4 ,w xd5 ,w xd6 These represent the angular velocity drift deviations along the x-axis of the six IMU modules, w yd1 ,w yd2 ,w yd3 ,w yd4 ,w yd5 ,w yd6These represent the angular velocity drift deviations along the y-axis of the six IMU modules, w zd1 ,w zd2 ,w zd3 ,w zd4 ,w zd5 ,w zd6 These represent the angular velocity drift deviations of the six IMU modules along the z-axis, a xr ,a yr ,a zr Let a represent the actual theoretical acceleration in the three coordinate axes respectively. xd1 ,a xd2 ,a xd3 ,a xd4 ,a xd5 ,a xd6 These represent the acceleration drift deviations along the x-axis of the six IMU modules, a and b respectively. yd1 ,a yd2 ,a yd3 ,a yd4 ,a yd5 ,a yd6 These represent the acceleration drift deviations along the y-axis of the six IMU modules, a zd1 ,a zd2 ,a zd3 ,a zd4 ,a zd5 ,a zd6 These represent the acceleration drift deviations along the z-axis of the six IMU modules, respectively.

[0064] like Figure 6 As shown, three UWB positioning terminal modules 4 and the UWB positioning base station 11 are networked. A variational Bayesian maximum entropy unscented Kalman filter algorithm is used to smooth the measured distance, improving the estimation accuracy. The reconstructed distance is input into the positioning model to calculate the position coordinates of the three UWB positioning terminals. An extended Kalman filter is constructed using the constraint that the relative distance between the three positioning terminal modules 4 is a fixed value. The output value of the extended Kalman filter is used as the final positioning result P of the UWB system. UWB Meanwhile, the measurement data from the six IMU positioning modules are denoised using variational capacitive Kalman filtering and then fused together. The navigation algorithm is then used to calculate the three-dimensional position information P of the coal mining machine. IMU and attitude information Ω IMU On the one hand, through P UWB and P IMU The position error ΔP1 is calculated and then used as the input value to the data fusion filter 1 to obtain the position estimation error ΔP of the coal mining machine. IMU Then ΔP IMUThe positioning results are fed back to the IMU positioning system, and the final positioning result P from the IMU positioning system is obtained. IMUR At this point, the main inertial navigation device calculates the position information P of the coal mining machine using a navigation algorithm. INS and attitude information Ω INS Through P IMUR and P INS The position error ΔP2 is calculated; on the other hand, the attitude information Ω is calculated based on the IMU positioning system. IMU The pose information Ω calculated by the main inertial navigation device INS The attitude error ΔΩ is calculated, and then ΔP2 and ΔΩ are fed into the data fusion filter 2 as input values. After filtering, the estimated position error value ΔP of the coal mining machine is obtained. INS and attitude error estimate ΔΩ INS Then ΔP INS and ΔΩ INS Feedback is sent to the main inertial navigation and positioning system, via P INS and ΔP INS The difference is used to obtain the calibrated position information, Ω INS and ΔΩ INS The difference is used to obtain the calibrated attitude information, thereby correcting and compensating for the position and attitude errors of the coal mining machine, and displaying the calibrated attitude of the coal mining machine in the data display system. In the positioning model, the position information of the three positioning terminal modules is solved by the maximum likelihood estimation algorithm or the shrinkage estimation algorithm based on the minimum mean square error criterion; an unscented Kalman filter is used as the data fusion filter.

[0065] The quantities of the extended Kalman filter can be written in the following form:

[0066]

[0067] Where r 12 r represents the relative distance between UWB positioning terminal module 1 and UWB positioning terminal module 2. 13 r represents the relative distance between UWB positioning terminal module 1 and UWB positioning terminal module 2. 23 The relative distance between UWB positioning terminal module 1 and UWB positioning terminal module 2 is represented by (X1,Y1,Z1), (X2,Y2,Z2) and (X3,Y3,Z3), which represent the coordinates of the three UWB positioning terminals obtained by the positioning algorithm.

[0068] Performing a first-order Taylor expansion on the measurement yields the Jacobian matrix H:

[0069]

[0070] Furthermore, the measurement equation for the extended Kalman filter is Z.k =HX k +V k Where k represents time k, X k Denotes a state variable, and X k =[X1,Y1,Z1,X2,Y2,Z2,X3,Y3,Z3] T Z k Indicates the quantity measurement, and Z k =[r 12 ,r 23 ,r 13 ] T V k To represent measurement noise, the output of the extended Kalman filter is used as the positioning result P of the UWB positioning system. UWB ;

[0071] S4, P IMU With P UWB The difference is calculated to obtain ΔP1, and ΔP1 is input into the filter in the data processor 6. The filtered information ΔP IMU As a feedback quantity, the fused positioning results of the IMU positioning system are corrected and compensated to obtain the final positioning result P of the IMU positioning system. IMUR ;

[0072] S5, P INS With P IMUR The difference is used to obtain ΔP2, and Ω is then... INS With Ω IMU The difference is calculated to obtain ΔΩ. ΔP2 and ΔΩ are used as inputs to the fusion filter, and the filtered output ΔP is used... INS and ΔΩ INS The position and attitude of the coal mining machine are calibrated and compensated.

[0073] S6. Use limit switches to determine whether the coal mining machine has fully moved to the end of the coal mining face. If the coal mining machine has not moved to the end position, continue to use the ring array IMU positioning system and UWB positioning system to calibrate the position end of the coal mining machine. If the coal mining machine has moved to the end position of the coal mining face, complete the autonomous end calibration of the coal mining machine and turn off the UWB positioning system and IMU positioning system.

[0074] S7. The stepper motor-controlled base station group autonomous migration system autonomously migrates along the direction of the coal mining machine's cutting advance. The distance the base station group autonomous migration system migrates on the first slide is the same as the coal mining machine's cutting advance. Based on the migration distance and the attitude information from the high-precision attitude sensor 26, the position coordinates of the base station group after migration are calculated. The method for calculating the base station group coordinates is as follows: assuming the distance the base station group moves on the first slide is s, the displacement increment ΔS of the base station group in the navigation coordinate system is... n Then we have:

[0075]

[0076] In the formula, θ represents the pitch angle and γ represents the roll angle. Indicates the heading angle;

[0077] S8. After the base station group autonomous migration system has moved 8 times, the UWB base station group is re-measured and calibrated using a total station. Then, steps S2-S7 are repeated to autonomously calibrate the coal mining machine's pose.

[0078] The initial coordinates of the base station group were measured using a total station, with the coordinates C. i (0)=(x i ,y i ,z i After N migrations, the coordinates of the base station group are:

[0079]

[0080] In the formula C i (N) represents the coordinates of the base station group after N migrations, where i represents the base station number, i = 1, 2, 3, 4.

[0081] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A coal mining machine face end pose calibration device using a ring array IMU and UWB, characterized in that: It includes the coal mining machine body, ring-shaped fixing device, explosion-proof shell, data processor, main inertial navigation and positioning device, high-precision attitude sensor, data display system, industrial control computer, UWB positioning system, IMU positioning system, base station group autonomous migration system and hydraulic height adjustment system; The UWB positioning system includes three UWB positioning terminal modules and four UWB positioning base stations. The IMU positioning system includes six IMU modules. The base station group autonomous migration system includes a first slide, a slider, a pole, a UWB positioning base station, a UWB support rod, a connecting plate, a non-standard hole connecting fastener, a base station fixing device, a vertical slider, a first stepper motor, a first coupling, a transmission shaft, an electric slide shaft, a second slide, a second stepper motor, and a second coupling. The hydraulic height adjustment system includes a fixing plate, a telescopic cylinder, and a hydraulically controllable electric pump. The explosion-proof enclosure is installed on the body of the coal mining machine, the annular fixing device is installed inside the explosion-proof enclosure, the main inertial navigation and positioning device is installed at the center of the annular fixing device, the UWB positioning terminal module and the IMU module are both installed on the annular fixing device, the UWB positioning terminal module is connected to the data processor through a multi-serial port hub, the output end of the data processor is connected to the industrial control computer, the industrial control computer is installed in the roadway, and the signal input end of the data display system is connected to the signal output end of the industrial control computer. The upright is connected to three UWB support rods from top to bottom via three different-hole connecting fasteners. The base station fixing device is set at either end of the UWB support rod and one end of the upright. The UWB positioning base station is set on the base station fixing device, and the height difference between two adjacent UWB positioning base stations is greater than 0.5m. The other end of the upright is set on a connecting plate. The bottom of the connecting plate is set on a slider. The slider is set on an electric slide shaft. The electric slide shaft is set on a first slide. The first stepper motor is connected to the first slide via a first coupling. The bottom of the first slide is connected to two second slides via a vertical slider. The two second slides are connected by a transmission shaft. The second stepper motor is connected to one second slide via a second coupling. The fixing plate is set at the bottom of the second slide. The hydraulically controllable electric pump is connected to the fixing plate via a telescopic cylinder. The hydraulically controllable electric pump, the first stepper motor, the second stepper motor, and the IMU positioning system are all connected to the input end of the data processor. Three UWB positioning terminal modules and six IMU modules are arranged in a ring array with two IMU modules sandwiched between any two UWB positioning terminal modules on a ring-shaped fixing device, and are on the same plane.

2. The coal mining machine face end pose calibration device based on a ring array IMU and UWB as described in claim 1, characterized in that: The irregular hole connection fastener is connected to the UWB support rod by a locking screw.

3. A method for calibration using the coal mining machine face end pose calibration device based on the ring array IMU and UWB as described in claim 1 or 2, characterized in that, Includes the following steps: S1. Based on the working conditions of the roadway at the end of the coal mining face, the hydraulic height adjustment system is deployed in the roadway. The second slide, the first slide, and the autonomous migration system of the base station group are installed in sequence. The four UWB positioning base stations are respectively set on the base station fixing device at the top of the pole and on the base station fixing device at any end of the three UWB support poles. The relative positions of the four UWB positioning base stations are adjusted so that the wireless communication path between each UWB positioning base station and the three UWB positioning terminal modules is in a line-of-sight environment. S2. Establish a navigation coordinate system, use a total station to measure the position coordinates of four UWB positioning base stations, and input the coordinates of the four UWB positioning base stations into the industrial control computer. At the same time, network the three UWB positioning terminal modules with the four UWB positioning base stations. S3. Determine whether the coal mining machine has reached 10m from the end of the coal face by using limit switches. If the limit switches are not triggered, obtain the position information of the coal mining machine using the main inertial navigation and positioning device. If the coal mining machine has reached 10m from the end of the coal face, activate the IMU positioning system and UWB positioning system of the ring array. The main inertial navigation and positioning device, IMU positioning system, and UWB positioning system simultaneously locate the coal mining machine. The data from the six IMU modules and three UWB positioning terminal modules are transmitted to the industrial control computer in the roadway through the data processor. In the data processor, the main inertial navigation and positioning device obtains the position information P of the coal mining machine. INS and attitude information Ω INS The final positioning results P of the three UWB positioning terminal modules were obtained using the extended Kalman filter algorithm. UWB The position P of the IMU positioning system is calculated using navigation algorithms. IMU and posture Ω IMU ; S4, P IMU With P UWB The difference is calculated to obtain ΔP1, and ΔP1 is input into the filter in the data processor. The filtered information ΔP IMU As a feedback quantity, the fused positioning results of the IMU positioning system are corrected and compensated to obtain the final positioning result P of the IMU positioning system. IMUR ; S5, P INS With P IMUR The difference is used to obtain △P2, and Ω is then... INS With Ω IMU The difference is calculated to obtain ΔΩ. ΔP2 and ΔΩ are used as inputs to the fusion filter. The filtered output ΔP... INS and △Ω INS The position and attitude of the coal mining machine are calibrated and compensated. S6. Use limit switches to determine whether the coal mining machine has fully moved to the end of the coal mining face. If the coal mining machine has not moved to the end position, continue to use the ring array IMU positioning system and UWB positioning system to calibrate the position end of the coal mining machine. If the coal mining machine has moved to the end position of the coal mining face, complete the autonomous end calibration of the coal mining machine and turn off the UWB positioning system and IMU positioning system. S7. The stepper motor-controlled base station group autonomous migration system autonomously migrates along the direction of the coal mining machine's cutting. The distance the base station group autonomous migration system migrates on the first slide is the same as the cutting amount of the coal mining machine. Based on the migration distance and the attitude information of the high-precision attitude sensor, the position coordinates of the base station group after migration are calculated. S8. After the base station group autonomous migration system moves 8 times, the UWB base station group is re-measured and calibrated using a total station. Then, steps S2-S7 are repeated to autonomously calibrate the position and orientation of the coal mining machine.

4. The method for calibration using the coal mining machine face end pose calibration device with ring array IMU and UWB as described in claim 3, characterized in that, In step S3, the measured distance of each UWB positioning base station is reconstructed using a variational Bayesian maximum entropy unscented Kalman filter algorithm. The three-dimensional position coordinates of the three UWB positioning terminal modules are then solved using either a maximum likelihood estimation algorithm or a shrinkage estimation algorithm based on the minimum mean square error criterion. After the three UWB positioning terminal modules are installed and fixed, they are numbered 1, 2, and 3. The relative distance between two modules is used as the measurement, as follows: ， in r 12 This indicates the relative distance between UWB positioning terminal module 1 and UWB positioning terminal module 2. r 13 This indicates the relative distance between UWB positioning terminal module 1 and UWB positioning terminal module 3. r 23 This indicates the relative distance between UWB positioning terminal module 2 and UWB positioning terminal module 3. , and These represent the position coordinates of the three UWB positioning terminal modules obtained by the positioning algorithm; Performing a first-order Taylor expansion on the measurement yields the Jacobian matrix. H : ， The measurement equation for the Kalman filter is further extended as follows: The output of the extended Kalman filter is used as the positioning result P of the UWB positioning system. UWB Where k represents time k, X k Represents a state variable, and Z k Indicates measurement, and V k This indicates measurement noise.

5. The method for calibration using the coal mining machine face end pose calibration device with ring array IMU and UWB as described in claim 4, characterized in that, The measurement data from the six IMU modules in step S3 are denoised using variational volume Kalman filtering. In step S3, the data from solving the position and attitude of the coal mining machine are used as the fused angular velocities and accelerations output from the six IMU modules in the ring array. The fused angular velocities and accelerations of the six IMU modules are as follows: ， In the formula This represents the true theoretical angular velocity in the three coordinate axes. These represent the six IMU modules. x Angular velocity drift deviation in the axial direction These represent the six IMU modules. y Angular velocity drift deviation in the axial direction, These represent the six IMU modules. z Angular velocity drift deviation in the axial direction, These represent the actual theoretical accelerations in the three coordinate axes, respectively. These represent the six IMU modules. x axial acceleration drift deviation, These represent the six IMU modules. y axial acceleration drift deviation, These represent the six IMU modules. z Acceleration drift deviation in the axial direction.

6. The method for calibration using the coal mining machine face end pose calibration device with ring array IMU and UWB as described in claim 4, characterized in that, The method for calculating the coordinates of a base station group is as follows: Assume the base station group has moved a distance of [distance] on the first sliding platform. s Displacement increment of base station group in navigation coordinate system Then we have: ， In the formula θ Indicates pitch angle, γ Indicates the roll angle. φ Indicates the heading angle; The initial coordinates of the base station group were measured using a total station. C i (0)=( x i , y i , z i ),migrate N After this, the coordinates of the base station group are: ， In the formula Migration of base station clusters N The subsequent coordinates, where i Indicates the location base station number. i =1,2,3,4.

Images