A combined positioning system of a tunneling machine based on a strapdown inertial navigation system and a positioning method thereof

Abstract

The application discloses a combined positioning system of a tunneling machine based on a strapdown inertial navigation system and a positioning method thereof, and the system comprises a UWB positioning system, a strapdown inertial navigation system, an encoder, a laser positioning system and a signal processing box. The application separately measures the problem of sudden change of the advancing direction of the tunneling machine caused by uneven road conditions or uneven hardness of the coal and rock in the working face during tunneling by the laser positioning method in real time, and the problem is not added to the positioning algorithm, so that the algorithm pressure is relieved. The application fuses the positioning methods of the strapdown inertial navigation positioning, the UWB positioning and the encoder, forms a combined positioning method, effectively makes up for some self defects of the positioning methods when the positioning methods are used alone, and improves the positioning accuracy of the tunneling machine.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of combined positioning system of tunneling machine based on strapdown inertial navigation and its positioning method, in particular to a combined positioning system of tunneling machine based on strapdown inertial navigation and its positioning method. BACKGROUND

[0002] In the process of underground coal mining, tunneling machine, coal mining machine and hydraulic support are the main bearers of comprehensive mining, and among them, the tunneling machine plays an important role in tunneling. Due to the complex underground working conditions, the problem of poor tunneling direction caused by the rugged ground, coal hardness and other problems has always limited the working efficiency of the tunneling machine. Many positioning systems on the market have a unique role in positioning the tunneling machine, but they all have some defects: although the GPS system is the most widely used positioning system, it cannot be used due to the special conditions of the underground; the strapdown inertial navigation system is widely used in tunneling machines and coal mining machines, but its accuracy is difficult to improve; although the UWB positioning system has higher positioning accuracy, it is suitable for environmentally friendly indoor use, and there are many restrictions in the underground. Therefore, a combined positioning system of tunneling machine based on strapdown inertial navigation and its positioning method tries to combine multiple positioning methods and provides a use method to improve the positioning progress of the tunneling machine and provide technical support for the establishment of intelligent mines. SUMMARY

[0003] In view of the above technical deficiencies, the purpose of the present application is to provide a combined positioning system of tunneling machine based on strapdown inertial navigation and its positioning method.

[0004] To solve the above technical problems, the present application adopts the following technical scheme:

[0005] The present application provides a combined positioning system of tunneling machine based on strapdown inertial navigation, which comprises a UWB positioning system, a strapdown inertial navigation system, an encoder, a laser positioning system and a signal processing box. The laser positioning system is used to detect whether the tunneling machine is in the correct direction of travel. The UWB positioning system, the strapdown inertial navigation system and the encoder are used to detect the position signal and the motion signal of the tunneling machine body. The signal processing box is used to process the position information signal of the tunneling machine body returned by the UWB positioning system, the strapdown inertial navigation system, the encoder and the laser positioning system, and to obtain the real-time position information of the tunneling machine according to the given combined positioning position algorithm of the tunneling machine.

[0006] Preferably, the UWB positioning system comprises a mine intrinsically safe positioning base station one, a positioning base station two, a positioning base station three and a positioning terminal arranged in the roadway. The positioning base station one, the positioning base station two and the positioning base station three are distributed on one side of the roadway, and the positioning terminal is arranged on the tail seat of the tunneling machine.

[0007] Preferably, the positioning base station one, positioning base station two and positioning base station three are arranged with two adjacent ones being two meters apart.

[0008] Preferably, the strapdown inertial navigation system comprises an integrated three-axis optical fiber gyroscope inertial navigation unit, which is arranged at the bottom of the seat of the tunneling machine.

[0009] Preferably, the laser positioning system comprises a laser and a laser sensor, the laser sensor being arranged at the tail of the tunneling machine body, and the laser being arranged at the back side of the tunneling machine body corresponding to the laser sensor.

[0010] Preferably, the signal processing box is arranged at the bottom of the control console of the tunneling machine, and the signal processing box is internally provided with a microprocessor for processing the tunneling machine body position information returned by the UWB positioning system, the strapdown inertial navigation system encoder and the laser positioning system.

[0011] The application also provides a positioning method using the above device, specifically comprising the following steps:

[0012] (a) starting the tunneling machine;

[0013] (b) starting the laser positioning system;

[0014] (c) judging whether the tunneling machine is in the correct advancing direction, if yes, executing step (e), if not, executing step (d);

[0015] (d) performing active pose adjustment of the tunneling machine, and after adjustment, continuing to execute step (c);

[0016] (e) starting the combined positioning system of the tunneling machine;

[0017] (f) the strapdown inertial navigation system detects the tunneling machine body position signal W JC , the UWB positioning system detects the tunneling machine body position signal W UC , and the encoder (2) detects the tunneling machine body position signal W BC ;

[0018] (g) the position information W JC , W UC , W BC is output with a weight ratio of 50%, 50% and 50%, to obtain initial position information of the tunneling machine;

[0019] (h) the tunneling machine works;

[0020] (i) the strapdown inertial navigation system detects the tunneling machine body movement signal W JD , the UWB positioning system detects the tunneling machine body movement signal WUD, and the encoder (2) detects the tunneling machine body movement signal W BD ;

[0021] (j) judging whether the body motion signal W measured by the UWB positioning system (5) is less than or equal to the body motion signal W measured by the encoder (2) UD (j) judging whether the body motion signal W measured by the UWB positioning system (5) is less than or equal to the body motion signal W measured by the encoder (2) BD , if yes, the position information W JD , W UD , W BD is output with a weight ratio of 50%, 50%, and 50% to obtain the fusion position information of the roadheader, and if no, the position information W JD , W UD , W BD is output with a weight ratio of 75%, 25%, and 25% to obtain the fusion position information of the roadheader.

[0022] (k) the fusion position information of the roadheader obtained is subjected to Kalman filtering to obtain the final real-time position information of the roadheader. Advantages

[0023] 1. The application separately measures the sudden change of the advancing direction of the roadheader caused by uneven road conditions or uneven hardness of the coal and rock in the working face during the tunneling process in real time by the laser positioning method, and does not add it to the positioning algorithm, thereby relieving the algorithm pressure.

[0024] 2. The application fuses the strapdown inertial navigation positioning, UWB positioning, and encoder positioning methods to form a combined positioning method, effectively makes up for some self-defects of these positioning methods when used alone, and improves the positioning accuracy of the roadheader. BRIEF DESCRIPTION OF DRAWINGS

[0025] In order to more clearly illustrate the technical solutions in the embodiments of the application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings in the following description only some embodiments of the application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

[0026] Figure 1 It is a schematic diagram of the installation position of the combined positioning system of the roadheader based on strapdown inertial navigation in the example of the application.

[0027] Figure 2 It is a work flow chart of the combined positioning system of the roadheader based on strapdown inertial navigation and the positioning method thereof in the example of the application.

[0028] Figure 3 It is a combined positioning position algorithm model of the combined positioning system of the roadheader based on strapdown inertial navigation and the positioning method thereof in the example of the application.

[0029] In the diagram: 1-1, Tunneling machine cutting head; 1-2, Tunneling machine cutting arm; 1-3, shovel plate; 1-4, Tunneling machine rotary table; 1-5, Tunneling machine control console; 1-6, Control console seat; 1-7, Tunneling machine traveling mechanism; 1-8, Tunneling machine transport mechanism; 2, encoder; 3, signal processing box; 4, three-axis fiber optic gyroscope inertial navigation unit; 5, UWB positioning system; 5-1, positioning base station one; 5-2, positioning base station two; 5-3, intrinsically safe positioning terminal for mining; 5-4, positioning base station three; 6-1, laser sensor; 6-2, laser. Detailed Implementation

[0030] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0031] Example 1: As Figures 1-2 As shown, this embodiment provides a tunneling machine combined positioning system based on strapdown inertial navigation, including a UWB positioning system, a three-axis fiber optic gyroscope inertial navigation unit 4, an encoder 2, a laser positioning system, and a signal processing box 3. The laser positioning system is used to detect whether the tunneling machine is in the correct direction of travel. The UWB positioning system, the three-axis fiber optic gyroscope inertial navigation unit 4, and the encoder 2 are used to detect the position and motion signals of the tunneling machine body. The signal processing box 3 is used to process the tunneling machine body position information signals transmitted back by the UWB positioning system, the strapdown inertial navigation system, the encoder 2, and the laser positioning system, and to obtain the real-time position information of the tunneling machine according to a given tunneling machine combined positioning position algorithm.

[0032] This example is applied to a tunneling machine, which includes a tunneling machine cutting head 1-1, a tunneling machine cutting arm 1-2, a shovel plate 1-3, a tunneling machine rotary table 1-4, a tunneling machine control console 1-5, a control console seat 1-6, a tunneling machine traveling section 1-7, and a tunneling machine transport section 1-8. The tunneling machine is a conventional tunneling machine well known to those skilled in the art, and the above-mentioned components are conventional components well known to those skilled in the art. The connections between the components are made using conventional connection methods well known to those skilled in the art, and will not be described in detail here.

[0033] The UWB positioning system comprises a mine intrinsically safe positioning base station 5-1, a positioning base station 5-2, a positioning base station 5-4 and a positioning terminal 5-3 arranged in the roadway, the positioning base station 5-1, the positioning base station 5-2 and the positioning base station 5-4 are distributed on one side of the roadway, and the positioning terminal 5-3 is arranged on the tailstock of the heading machine. The positioning base station 5-1, the positioning base station 5-2 and the positioning base station 5-4 are arranged at a distance of two meters from each other. The strapdown inertial navigation system comprises an integrated three-axis fiber-optic gyroscope inertial navigation unit 4 arranged at the bottom of the seat 1-6 of the heading machine. The laser positioning system comprises a laser 6-2 and a laser sensor 6-1, the laser sensor 6-1 is arranged at the tail of the body of the heading machine, and the laser 6-2 is arranged in the roadway behind the body of the heading machine and corresponds to the laser sensor 6-1. The signal processing box 3 is arranged at the bottom of the control console 1-5 of the heading machine, and the signal processing box 3 is internally provided with a microprocessor for processing the position information of the body of the heading machine returned by the UWB positioning system 5, the strapdown inertial navigation system, the encoder 2 and the laser positioning system.

[0034] The positioning base station, the positioning terminal, the encoder 2, the three-axis fiber-optic gyroscope inertial navigation unit 4, the laser 6-2, the laser sensor 6-1 and the signal processing box 3 of the present example are selected from existing products or structures known to those skilled in the art, and the connection relationship between them and the heading machine is also known to those skilled in the art, which will not be described in detail.

[0035] The three-axis fiber-optic gyroscope inertial navigation unit 4 comprises a three-axis accelerometer and a three-axis gyroscope, which are used to output three-axis acceleration signals (unit g, i.e. gravitational acceleration) and three-axis angular velocity signals (unit rad / s), and the attitude and position information of the vehicle body is obtained according to the strapdown inertial navigation update algorithm;

[0036] The UWB nodes are divided into beacon nodes (around the roadway) and unknown nodes (vehicle body), which are actually the same kind of sensing elements, and are divided into two kinds according to different functions, which are used to receive / send ultra-wideband wireless signals, measure the distance between nodes according to the TW-TOF method, and then measure the moving distance of the vehicle body;

[0037] The encoder is internally provided with a motor, and the speed of the vehicle body is obtained by calculating the pulse frequency and period of the motor;

[0038] When the laser sensor receives the laser signal, it sends a logical signal "yes (1)" to the control system, so that the control system can continue to operate, and when the laser sensor cannot receive the laser signal, it sends a logical signal "no (0)" to the control system, so as to determine that the attitude of the heading machine is changed at this time, and the subsequent action is continued after the attitude is adjusted.

[0039] The present embodiment also provides a positioning method using the above device, which specifically comprises the following steps:

[0040] (a) start the tunneling machine;

[0041] (b) start the laser positioning system;

[0042] (c) determine whether the tunneling machine is in the correct direction of travel, when the laser sensor receives a laser signal, send a logic signal "yes (1)" to the control system, so that the control system can continue to run, when the laser sensor cannot receive a laser signal, send a logic signal "no (0)" to the control system, determine that the posture of the tunneling machine is changed at this time, adjust the posture and continue the subsequent action, if yes, execute step (e), if no, execute step (d);

[0043] (d) active posture adjustment of the tunneling machine, active adjustment according to the detection result of the laser sensor, the adjustment action needs manual operation, first observe the accurate position of the laser emitted by the laser emitter on the receiver through the human eye, if the laser hits the left side of the receiver, adjust the tunneling machine to the left side, otherwise adjust to the right side; if the laser hits above the receiver, adjust the emitter downward, otherwise adjust upward; continue to execute step (c) after adjustment;

[0044] (e) start the combined positioning system of the tunneling machine based on strapdown inertial navigation of the embodiment;

[0045] (f) the strapdown inertial navigation system detects the body position signal W JC , the UWB positioning system detects the body position signal W BC , and the encoder (2) detects the body position signal W JC ;

[0046] The strapdown inertial navigation system calculates the position information according to the direction cosine matrix method known to those skilled in the art, compensates the angular increment and specific force output by the angular velocity and accelerometer for errors, obtains the posture, velocity update formula of the tunneling machine, and performs matrix multiplication operation with the initial position, velocity, and attitude to obtain the updated attitude, velocity, and position, and outputs the position signal W jcx =[roll angle Y pitch angle Θ heading angle Ψ velocity V jcy velocity V jcz velocity V jcx position P jcy position P jcz ];

[0047] The UWB positioning system first calculates the distance between the beacon node and the unknown node according to the double-lateral double-way ranging principle known to those skilled in the art, and then calculates the specific vehicle body position information according to the three-edge positioning principle, and outputs the position signal W UC =[position P ucx position Pucy Position P ucz ];

[0048] The encoder measures the speed by M / T method well known to those skilled in the art, calculates the rotating speed by detecting the number and frequency of the encoder pulses, and calculates the actual moving speed according to the radius of the motor, and outputs the position signal W BC =[speed V bcx Speed V bcy Speed V bcz ];

[0049] (g) Position information W JC , W UC , W BC Output as the initial attitude angle AC, initial speed VC and initial position PC with the weight ratio of 50%, 50% and 50% to obtain the initial position information of the roadheader, wherein AC=[roll angle Y pitch angle Θ heading angle Ψ], PC=50%*[P jcx +P ucx P jcy +P ucy P jcz +P ucz ] and VC=50%*[V jcx +V bcx V jcy +V bcy V jcz +V bcz ];

[0050] (h) The roadheader works;

[0051] (i) When the roadheader is normally running, the strapdown inertial navigation system detects the body movement signal W JD , the UWB positioning system detects the body movement signal W UD , and the encoder 2 detects the body movement signal W BD ; the detection method and steps are the same as (f), and the position signal W JD =[Y Θ Ψ V jdx V jdy V jdz P jdx P jdy P jdz ] output by the strapdown inertial navigation system, the position information W UD =[P udx P udy P udz ] output by the UWB positioning system, and the position signal W BD =[V bdx V bdy V bdz ] output by the encoder.

[0052] (j) judging whether the body motion signal W measured by the UWB positioning system 5 is less than or equal to the body motion signal W measured by the encoder 2 UD , if yes, the position information W BD , W JD , W UD , W BD is outputted with a weight ratio of 50%, 50%, 50% as the updated attitude angle AD, the updated speed VD and the updated position PD, and the updated position information of the roadheader is obtained, wherein AD=[Y Θ Ψ], VD=50%*[V jdx +V bdx V jdy +V bdy V jdz +V bdz ], PD=50%*[P jdx +P udx P jdy +P udy P jdz +P udz ]; JD , if no, the position information W UD , W BD is outputted with a weight ratio of 75%, 25%, 25% as the updated attitude angle AD, the updated speed VD and the updated position PD, and the updated position information of the roadheader is obtained, wherein AD=[Y Θ Ψ], VD=[75%*V jdx +25%*V bdx 75%*V jdy +25%*V bdy 75%*V jdz +25%*V bdz ],

[0053] PD=[75%*P jdx +25%*P udx 75%*P jdy +25%*P udy 75%*P jdz +25%*P udz ];

[0054] As shown in the accompanying drawings Figure 3As shown, the position information is calculated by the strapdown inertial navigation system, the angular increment and specific force output by the angular velocity and accelerometer are compensated for errors by using the prior art method known to those skilled in the art, and the heading and velocity update formula of the heading machine is obtained, wherein the initial heading and the heading update formula are used to obtain the heading of the heading machine by matrix operation known to those skilled in the art; the velocity update formula output by the strapdown inertial navigation system and the velocity update formula output by the encoder are added by using the fusion weight ratio to obtain the fusion velocity of the heading machine; the position information obtained by integrating the fusion velocity according to the prior art method known to those skilled in the art and the position obtained by the UWB positioning system are added by using the fusion weight ratio to obtain the fusion position of the heading machine.

[0055] (k) The fused position information is subjected to Kalman filtering to obtain the final real-time position information of the heading machine.

[0056] The Kalman filtering method of the embodiment is the prior art method known to those skilled in the art, and will not be described in detail here.

[0057] The embodiment uses the laser positioning system to determine the heading direction of the heading machine, uses the multi-element combined positioning method of the strapdown inertial navigation, UWB positioning and encoder, and uses the combined positioning position algorithm to obtain the position information of the heading machine in the heading process, thereby effectively improving the positioning accuracy and intelligent degree of the heading machine.

[0058] Obviously, those skilled in the art can make various modifications and variations to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalent technologies, the present application also intends to include these modifications and variations.

Claims

1. A combined positioning system for a tunneling machine based on strapdown inertial navigation, characterized in that: The system includes a UWB positioning system, a strapdown inertial navigation system, an encoder (2), a laser positioning system, and a signal processing box (3). The laser positioning system is used to detect whether the tunneling machine is in the correct direction of travel. The UWB positioning system, the strapdown inertial navigation system, and the encoder (2) are used to detect the position and motion signals of the tunneling machine body. The laser positioning system includes a laser (6-2) and a laser sensor (6-1). The laser sensor (6-1) is placed at the rear of the tunneling machine body, and the laser (6-2) is placed behind the tunneling machine body, corresponding to the laser sensor (6-1), for real-time detection of deviation in the direction of travel. The signal processing box (3) has a built-in microprocessor, which is used to process the position information of the tunneling machine body transmitted back by the laser positioning system. When the laser sensor cannot receive the laser signal, it sends a logic signal "No" to the control system to determine that the attitude of the tunneling machine has changed. After adjusting the attitude, it continues to perform subsequent actions. The position signals of the UWB positioning system, the strapdown inertial navigation system, and the encoder are fused according to dynamic weights to determine the body motion signal W measured by the UWB positioning system. UD Is it less than or equal to the fuselage motion signal W measured by encoder (2)? BD If yes, then the location information W JD W UD W BD Outputting the tunnel boring machine's fused position information with weights of 50%, 50%, and 50% respectively, yields the final position information W. JD W UD W BD The tunnel boring machine's fused position information is obtained by outputting the information with weight ratios of 75%, 25%, and 25%; then, the real-time position information of the tunnel boring machine is output through Kalman filtering.

2. The tunneling machine combined positioning system based on strapdown inertial navigation as described in claim 1, characterized in that: The UWB positioning system includes an intrinsically safe positioning base station 1 (5-1), a positioning base station 2 (5-2), a positioning base station 3 (5-4), and a positioning terminal (5-3) arranged in the roadway. The positioning base station 1 (5-1), the positioning base station 2 (5-2), and the positioning base station 3 (5-4) are distributed on one side of the roadway, and the positioning terminal (5-3) is placed on the tailstock of the tunneling machine.

3. The tunneling machine combined positioning system based on strapdown inertial navigation as described in claim 2, characterized in that: The adjacent positioning base stations 1 (5-1), 2 (5-2), and 3 (5-4) are arranged at a distance of two meters.

4. The tunneling machine combined positioning system based on strapdown inertial navigation as described in claim 1, characterized in that: The strapdown inertial navigation system includes an integrated three-axis fiber optic gyroscope inertial navigation unit (4), which is placed at the bottom of the tunneling machine seat (1-6).

5. A tunneling machine combined positioning system based on strapdown inertial navigation as described in claim 1, characterized in that: The signal processing box (3) is placed at the bottom of the tunneling machine control console (1-5). The signal processing box (3) has a built-in microprocessor for processing the tunneling machine body position information transmitted by the UWB positioning system (5), strapdown inertial navigation system, encoder (2) and laser positioning system.

6. A positioning method for a tunneling machine combined positioning system based on strapdown inertial navigation as described in any one of claims 1-5, characterized in that: Includes the following steps: (a) Start the tunneling machine; (b) Activate the laser positioning system; (c) Determine whether the tunneling machine is in the correct direction of travel. If yes, proceed to step (e); if no, proceed to step (d). The laser sensor outputs a logic signal "1" or "0" to determine whether the direction is correct. (d) Perform active pose adjustment of the tunneling machine, and continue to step (c) after adjustment; (e) Activate the tunneling machine's combined positioning system; (f) Strapdown inertial navigation system detects fuselage position signal W JC The UWB positioning system detects the fuselage position signal W. UC The encoder (2) detects the body position signal W. BC ; (g) Location information W JC W UC W BC The initial position information of the tunneling machine is obtained by outputting the values ​​with a weight ratio of 50%, 50%, and 50%. (h) The tunneling machine is in operation; (i) The strapdown inertial navigation system detects the fuselage motion signal W JD The UWB positioning system detects the motion signal of the fuselage. UD The encoder (2) detects the motion signal W of the fuselage. BD ; (j) Determine the fuselage motion signal W measured by the UWB positioning system (5). UD Is the fuselage motion signal W measured by encoder (2) less than or equal to the value of the signal? BD If yes, then the location information W JD W UD W BD The tunnel boring machine's fused position information is output with a weighting ratio of 50%, 50%, and 50%. If not, the position information W is... JD W UD W BD The tunnel boring machine's fused position information is output with weight ratios of 75%, 25%, and 25%. The weight switching condition is the UWB signal error threshold W. UD ≤ W BD ; (k) The fused position information of the tunneling machine is then processed by Kalman filtering to obtain the final real-time position information of the tunneling machine.

Images