A coal mine tunneling equipment combined inertial navigation system error compensation method and device

Abstract

The application discloses a coal mine tunneling equipment combined inertial navigation system error compensation method and device, and relates to the technical field of intelligent tunneling equipment of coal mines. The coal mine tunneling equipment combined inertial navigation system error compensation method comprises the following steps: according to different fault types of an inertial navigation system and different motion states of tunneling equipment, the current position of the combined inertial navigation system is positioned by adopting different navigation modes, the overall fault tolerance of the combined inertial navigation system is increased, so that the current position of the combined inertial navigation system can be accurately positioned when the inertial navigation system outputs a long-time fault type and when the tunneling equipment is in a motion state, and the combined inertial navigation system can keep a high-precision navigation positioning function in a full working condition range.

Description

Technical Field

[0001] This invention relates to the field of intelligent tunneling equipment technology in coal mines, and in particular to an error compensation method and device for a combined inertial navigation system for coal mine tunneling equipment. Background Technology

[0002] With the rapid development of science and technology and the support of various national policies, intelligent tunneling equipment and technology in coal mines have been continuously improved. However, due to the complex conditions of coal mines and the diversification of tunneling equipment and processes, intelligent tunneling equipment and technology still face many challenges. Among these challenges are limited perception capabilities for tunneling equipment position, working face environment, and equipment operating status, weak remote control functions for tunneling equipment, and limited practical application effects.

[0003] Among them, the tunneling face is one of the work sites with the highest accident rate, the harshest environment, and the largest number of workers in underground coal mines. The demand for automation and unmanned operation is urgent, and navigation and positioning of tunneling equipment is one of the common key technologies that needs to be overcome for intelligentization. For more than a decade, relevant enterprises and research institutions have been committed to the research of this technology. However, due to the influence of the tunneling face environment and the special process characteristics and motion characteristics of tunneling equipment, tunneling equipment is characterized by limited movement space and long-term operation. It is mainly characterized by long-term short-distance non-continuous movement, and has a variety of complex movement modes under different working conditions, such as continuous operation, continuous cutting, short and frequent advance and retreat, and long-term static placement.

[0004] Currently, domestically developed combined inertial navigation systems for tunneling equipment have emerged. These systems utilize a combination of inertial navigation and multi-sensor fusion methods, including total stations, odometers, and ultrasonic sensors, to achieve navigation and positioning. This approach overcomes environmental challenges such as high dust levels, low visibility, and confined spaces at the working surface.

[0005] Because navigation information is generated through integration, the positioning error of an inertial navigation system increases over time. The positioning error drift of a pure inertial navigation system is greater when it operates for a long time than when it operates for a short time. If the positioning error of a pure inertial navigation system is calculated using the same method as that used for calculating the positioning error of a pure inertial navigation system when it outputs a long-term fault, it will seriously affect the positioning accuracy of the combined inertial navigation system.

[0006] Furthermore, due to the lack of GPS signals underground, current inertial navigation systems commonly employ a combination of inertial navigation and correction algorithms for error compensation. However, single-system inertial navigation systems are prone to rapid accumulation of errors. Zero-velocity correction technology is an effective method for improving the long-term navigation accuracy of inertial navigation systems. It utilizes the velocity output of the inertial navigation system when the tunneling equipment is stationary to correct other errors. However, this method can only correct other errors based on the velocity output of the inertial navigation system when the tunneling equipment is stationary; it cannot compensate for errors in the combined inertial navigation system when the tunneling equipment is in motion.

[0007] In summary, due to the different types of output faults of inertial navigation systems and the different current states of tunneling equipment, if we still follow the navigation method of short-term faults in pure inertial navigation system output or the navigation method of correcting other errors by the speed output of a single inertial navigation system when the tunneling equipment is stationary, we will not be able to accurately locate the current position of the combined inertial navigation system when the inertial navigation system outputs long-term faults or when the tunneling equipment is moving. Summary of the Invention

[0008] The purpose of this invention is to provide a method and device for error compensation of a combined inertial navigation system for coal mine tunneling equipment, so as to solve the problem that the current position of the combined inertial navigation system cannot be accurately located when the inertial navigation system outputs a long-term fault type or when the tunneling equipment is moving, due to different fault types output by the inertial navigation system and different current states of the tunneling equipment.

[0009] To achieve the above objectives, the present invention provides the following solution:

[0010] A method for error compensation in a combined inertial navigation system for coal mine tunneling equipment includes:

[0011] Obtain the output fault type of the inertial navigation system; the output fault type of the inertial navigation system includes short-time faults and long-time faults;

[0012] If the output fault type of the inertial navigation system is a short-term fault type, the current position of the combined inertial navigation system is located by directly outputting the pure inertial navigation solution; the combined inertial navigation system is formed by setting an inertial navigation system on the tunneling equipment;

[0013] If the output fault type of the inertial navigation system is a long-term fault type, the current state of the tunneling equipment is obtained; the current state of the tunneling equipment is either stationary or moving.

[0014] When the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is a stationary state, the current position of the combined inertial navigation system is located by using an inertial navigation method combined with zero-speed correction.

[0015] When the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is in motion, the current position of the combined inertial navigation system is located using a navigation method that combines inertial and vehicle kinematic constraints.

[0016] Optionally, obtaining the output fault type of the inertial navigation system, previously included:

[0017] The state of the inertial navigation system is obtained; the state of the inertial navigation system includes a fault state and a fault-free state.

[0018] If the inertial navigation system is in a fault-free state, the current position of the combined inertial navigation system is located using a multi-source heterogeneous integrated navigation method;

[0019] If the inertial navigation system is in a fault state, the current position of the combined inertial navigation system is located by using a navigation method that directly outputs pure inertial navigation calculation results, a navigation method that combines inertial navigation with zero-speed correction, or a navigation method that combines inertial navigation with vehicle kinematic constraints.

[0020] Optionally, when the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is stationary, the current position of the combined inertial navigation system is located using an inertial navigation method combined with zero-velocity correction, specifically including:

[0021] The velocity error curve of the inertial navigation system is fitted with a quadratic curve, and the position error curve of the combined inertial navigation system is determined by integrating the velocity error curve of the inertial navigation system.

[0022] Obtain the position output parameters of the integrated inertial navigation system;

[0023] Based on the position error curve and the position output parameters, the corrected position parameters of the integrated inertial navigation system are determined;

[0024] The current position of the integrated inertial navigation system is located based on the corrected position parameters of the integrated inertial navigation system.

[0025] Optionally, when the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is a traveling state, the current position of the combined inertial navigation system is located using a navigation method that combines inertial navigation with vehicle kinematic constraints, specifically including:

[0026] Based on the vehicle's kinematic constraints, determine the observation inertial navigation system;

[0027] Based on the observed inertial navigation system, determine the velocity transformation matrix between the inertial navigation system coordinate system and the tunneling equipment coordinate system;

[0028] Based on the transformation matrix, the state equation of the integrated inertial navigation system is determined;

[0029] The velocity measurements of the tunneling equipment are determined by using the zero X-axis velocity and the zero Z-axis velocity of the tunneling equipment coordinate system as constraints on the state equation.

[0030] By combining the state equation and the velocity measurement value, the measurement equation for vehicle kinematic constraint assistance is determined;

[0031] The current position of the integrated inertial navigation system is located according to the measurement equation.

[0032] Optionally, the transformation matrix is:

[0033]

[0034] in, Let V be the transformation matrix. i V represents the velocity of the tunneling equipment in the coordinate system of the inertial navigation system. b The speed of the tunneling equipment is given in the coordinate system of the tunneling equipment.

[0035] Optionally, the state equation of the integrated inertial navigation system is:

[0036]

[0037] in, Let X be the X-axis velocity of the tunneling equipment in the coordinate system of the inertial navigation system. Let Y be the Y-axis velocity of the tunneling equipment in the coordinate system of the inertial navigation system. Let θ be the Z-axis velocity of the tunneling equipment in the coordinate system of the inertial navigation system. α Let θ be the pitch angle of the tunneling equipment in the coordinate system of the inertial navigation system. γ The heading angle of the tunneling equipment in the coordinate system of the inertial navigation system is given by [reference to the inertial navigation system]. Let Y be the Y-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment.

[0038] Optionally, the velocity measurement value is:

[0039]

[0040] Where Z is the measured speed of the tunneling equipment. Let X be the X-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment. Let Z be the Z-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment. The error is the X-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment. The error is the Z-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment.

[0041] Optionally, the measurement equation is:

[0042] Z = UX + V;

[0043] Where Z is the measured speed of the tunneling equipment, U is the measurement matrix, X is the state equation of the combined inertial navigation system, and V is the equivalent measurement noise.

[0044] To achieve the above objectives, the present invention also provides the following solution:

[0045] An error compensation device for a combined inertial navigation system for coal mine tunneling equipment includes:

[0046] Integrated inertial navigation system;

[0047] The combined inertial navigation system specifically includes: tunneling equipment and an inertial navigation system;

[0048] The inertial navigation system is installed on the tunneling equipment;

[0049] The inertial navigation system is used to select different navigation methods to locate the current position of the combined inertial navigation system according to the output fault type of the inertial navigation system and the current state of the tunneling equipment.

[0050] The output fault types of the inertial navigation system include short-term faults and long-term faults;

[0051] The current state of the tunneling equipment is either stationary or moving.

[0052] Optionally, if the output fault type of the inertial navigation system is a short-term fault type, the current position of the combined inertial navigation system is located by directly outputting the pure inertial navigation solution result.

[0053] When the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is a stationary state, the current position of the combined inertial navigation system is located by using an inertial navigation method combined with zero-speed correction.

[0054] When the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is in motion, the current position of the combined inertial navigation system is located using a navigation method that combines inertial and vehicle kinematic constraints.

[0055] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0056] The present invention provides a method and apparatus for error compensation of a combined inertial navigation system for coal mine tunneling equipment. Based on different fault types of the inertial navigation system and different motion states of the tunneling equipment, different navigation methods are used to locate the current position of the combined inertial navigation system, which increases the overall fault tolerance of the combined inertial navigation system. This enables accurate positioning of the combined inertial navigation system even when the inertial navigation system outputs a long-term fault type or when the tunneling equipment is in operation, thereby ensuring that the combined inertial navigation system can maintain high-precision navigation and positioning functions across the entire working condition range. Attached Figure Description

[0057] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0058] Figure 1 A flowchart illustrating the error compensation method for the combined inertial navigation system of coal mine tunneling equipment provided by this invention;

[0059] Figure 2 This is a structural block diagram of the multi-source heterogeneous combined navigation method in this invention. Detailed Implementation

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

[0061] The purpose of this invention is to provide an error compensation method and device for a combined inertial navigation system for coal mine tunneling equipment. By accurately locating the current position of the combined inertial navigation system even when the inertial navigation system outputs a long-term fault type and when the tunneling equipment is in operation, the combined inertial navigation system can maintain high-precision navigation and positioning functions across all operating conditions.

[0062] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0063] Example 1

[0064] like Figure 1 As shown, the error compensation method for the combined inertial navigation system of coal mine tunneling equipment provided by the present invention includes:

[0065] Step 101: Obtain the output fault type of the inertial navigation system; the output fault types of the inertial navigation system include short-term faults and long-term faults.

[0066] Step 102: Determine whether the output fault type of the inertial navigation system is a short-term fault type; if yes, proceed to step 103; if no, proceed to step 104.

[0067] Step 103: The current position of the combined inertial navigation system is located by directly outputting the pure inertial navigation solution results; the combined inertial navigation system is formed by setting up an inertial navigation system on the tunneling equipment.

[0068] Step 104: Obtain the current state of the tunneling equipment; the current state of the tunneling equipment is either stationary or moving.

[0069] Step 105: Determine whether the current state of the tunneling equipment is stationary; if yes, proceed to step 106; if no, proceed to step 107.

[0070] Step 106: Use an inertial navigation method combined with zero-velocity correction to locate the current position of the combined inertial navigation system.

[0071] Step 107: Use an inertial navigation method combined with vehicle kinematic constraints to locate the current position of the combined inertial navigation system.

[0072] Furthermore, obtaining the output fault type of the inertial navigation system previously included:

[0073] Obtain the state of the inertial navigation system; the state of the inertial navigation system includes a faulty state and a non-faulty state.

[0074] If the inertial navigation system is in a fault-free state, a multi-source heterogeneous integrated navigation method is used to determine the current position of the integrated inertial navigation system. Specifically, such as... Figure 2 As shown, using a multi-source heterogeneous combined navigation method to locate the current position of the combined inertial navigation system requires the integration of strapdown inertial navigation technology, as well as instruments such as odometers, millimeter-wave radar, total stations, and ultrasonic radar.

[0075] If the inertial navigation system is in a fault state, the current position of the combined inertial navigation system can be located by using a navigation method that directly outputs the pure inertial navigation solution, a navigation method that combines inertial navigation with zero-speed correction, or a navigation method that combines inertial navigation with vehicle kinematic constraints.

[0076] Furthermore, when the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is stationary, the current position of the combined inertial navigation system is determined using a navigation method that combines inertial navigation with zero-velocity correction. Specifically, this includes:

[0077] The velocity error curve of the inertial navigation system is fitted with a quadratic curve, and the position error curve of the combined inertial navigation system is determined by integrating the velocity error curve of the inertial navigation system.

[0078] Obtain the position output parameters of the integrated inertial navigation system.

[0079] Based on the position error curve and position output parameters, the corrected position parameters of the integrated inertial navigation system are determined.

[0080] Based on the corrected position parameters of the integrated inertial navigation system, determine the current position of the integrated inertial navigation system.

[0081] Furthermore, when the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is in motion, a navigation method combining inertial navigation with vehicle kinematic constraints is used to locate the current position of the combined inertial navigation system, specifically including:

[0082] Based on the vehicle's kinematic constraints, the observation inertial navigation system is determined.

[0083] Based on the observational inertial navigation system, the velocity transformation matrix between the inertial navigation system coordinate system and the tunneling equipment coordinate system is determined.

[0084] Based on the transformation matrix, the state equation of the integrated inertial navigation system is determined.

[0085] Using the zero velocity along both the X-axis and Z-axis of the tunneling equipment coordinate system as constraints in the state equation, the velocity measurement values ​​of the tunneling equipment are determined.

[0086] By combining the state equation and velocity measurements, the measurement equations for vehicle kinematic constraint assistance are determined.

[0087] The current position of the integrated inertial navigation system is determined based on the measurement equations.

[0088] Furthermore, the transformation matrix is:

[0089]

[0090] in, V is the transformation matrix. i V represents the velocity of the tunneling equipment in the inertial navigation system coordinate system. b The speed of the tunneling equipment is given in the coordinate system of the tunneling equipment.

[0091] Furthermore, the state equation of the integrated inertial navigation system is:

[0092]

[0093] in, Let X be the X-axis velocity of the tunneling equipment in the inertial navigation system coordinate system. Let Y be the Y-axis velocity of the tunneling equipment in the coordinate system of the inertial navigation system. Let θ be the Z-axis velocity of the tunneling equipment in the inertial navigation system coordinate system. α Let θ be the pitch angle of the tunneling equipment in the coordinate system of the inertial navigation system. γ The heading angle of the tunneling equipment in the coordinate system of the inertial navigation system is given by [reference to the inertial navigation system]. This represents the Y-axis velocity of the tunneling equipment in the tunneling equipment coordinate system.

[0094] Furthermore, the velocity measurement value is:

[0095]

[0096] Where Z represents the speed measurement value of the tunneling equipment. Let X be the X-axis velocity of the tunneling equipment in the tunneling equipment coordinate system. Let Z be the speed of the tunneling equipment along the tunneling equipment coordinate system. The error is the X-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment. The error is the Z-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment.

[0097] Furthermore, the measurement equation is:

[0098] Z = UX + V

[0099] Where Z is the measured speed of the tunneling equipment, U is the measurement matrix, X is the state equation of the combined inertial navigation system, and V is the equivalent measurement noise.

[0100] It should be noted that the determination of whether the tunneling equipment is in motion or stationary is complex due to the characteristics of its movement and the complexity of its process. Furthermore, the equipment is easily affected by the surrounding environment of the tunnel and the distribution of its center of gravity. Therefore, this invention uses the movement signals of the tunneling equipment itself for judgment. The start / stop signals and the movement signals of the left and right tracks are used to determine the stationary state of the tunneling equipment. When either the left or right track of the tunneling equipment has a movement signal, it is determined that the tunneling equipment is in motion. When neither the left nor right track has a movement signal, it is determined that the tunneling equipment is stationary.

[0101] Example 2

[0102] This embodiment details the process of locating the current position of the combined inertial navigation system using a navigation method that combines inertial navigation with vehicle kinematic constraints when the output fault type of the inertial navigation system is a long-term fault and the tunneling equipment is currently in a moving state.

[0103] Let the tunneling equipment's frame coordinate system be system b. The X-axis points to the right along the machine's transverse axis, the Y-axis points forward along the machine's longitudinal axis, and the Z-axis is perpendicular to the plane formed by the X and Y axes and conforms to a right-hand rectangular coordinate system. Vehicle kinematic constraints indicate that when the tunneling equipment is traveling normally in the tunnel without sideslip or jumping, it only has velocity in the forward Y direction, while its velocities in the X and Z axes are zero, as shown in the following equation. This constraint condition is used to construct an observational inertial navigation system.

[0104] The constraints are:

[0105]

[0106] Let the coordinate system of the inertial navigation system be frame i. Due to the deviation angle when the inertial navigation system is installed on the tunneling equipment, it cannot be guaranteed that the coordinate system i of the inertial navigation system and the coordinate system b of the tunneling equipment will completely coincide, and a transformation matrix exists. The relationship is as follows:

[0107]

[0108] In the formula, V is the transformation matrix. i V represents the velocity of the tunneling equipment in the inertial navigation system coordinate system. b The speed of the tunneling equipment is given in the coordinate system of the tunneling equipment.

[0109] Wherein, the transformation matrix Composed of pitch angle α, roll angle β, and yaw angle γ, it can be expressed as [θ α θ β θ γ ] TConsidering the constraints of vehicle kinematics, the transformation between the inertial navigation system coordinate system i and the tunneling equipment coordinate system b can be expressed as:

[0110]

[0111] In the formula, θ α Let θ be the pitch angle of the tunneling equipment in the coordinate system of the inertial navigation system. β Let θ be the roll angle of the tunneling equipment in the coordinate system of the inertial navigation system. γ The heading angle of the tunneling equipment in the coordinate system of the inertial navigation system is given by [reference to the inertial navigation system]. Let X be the X-axis velocity of the tunneling equipment in the inertial navigation system coordinate system. Let Y be the Y-axis velocity of the tunneling equipment in the coordinate system of the inertial navigation system. Let Z be the Z-axis velocity of the tunneling equipment in the coordinate system of the inertial navigation system. This represents the Y-axis velocity of the tunneling equipment in the tunneling equipment coordinate system.

[0112] Let the coordinate system output by the inertial navigation system be the n-system (north-southeast coordinate system). The state equation of the combined inertial navigation system is:

[0113]

[0114] In the formula, δV n For the velocity error of the inertial navigation system, σ n Let δL be the attitude error vector, δL be the position error, and φ be the gyroscope zero drift error. For accelerometer zero position, δθ α δθ represents the pitch angle error of the tunneling equipment in the coordinate system of the inertial navigation system. γ The error of the heading angle of the tunneling equipment in the coordinate system of the inertial navigation system is 15-dimensional.

[0115] Considering the influence of installation error angles and expanding it into state variables, the state variables of the vehicle kinematics-constrained integrated navigation model are 17-dimensional.

[0116] Installation error angle δθ α and δθ γ It can be considered as a random constant, and its error equation is:

[0117]

[0118] The velocity components of the tunneling equipment coordinate system are as follows:

[0119]

[0120] In the formula, V n The velocity in the northeast-sky coordinate system output by the inertial navigation system. This is the attitude transformation matrix. The unit matrix is ​​used when the installation deviation itself is small.

[0121] Using the X-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment Z-axis velocity of tunneling equipment in the coordinate system of tunneling equipment The zero constraint constitutes the velocity measurement value of the tunneling equipment:

[0122]

[0123] In the formula, Z represents the measured speed of the tunneling equipment. Let X be the X-axis velocity of the tunneling equipment in the tunneling equipment coordinate system. Let Z be the speed of the tunneling equipment along the tunneling equipment coordinate system. The error is the X-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment. The error is the Z-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment.

[0124] When the installation deviation of the inertial navigation system is small, then This is the identity matrix. Velocity measurements are directly derived from the i-series velocities calculated by the inertial navigation system.

[0125]

[0126] In the formula, Z represents the measured speed of the tunneling equipment. Let X be the X-axis velocity of the tunneling equipment in the inertial navigation system coordinate system. The Z-axis velocity of the tunneling equipment is given in the coordinate system of the inertial navigation system.

[0127] Taking the total differential of the velocity components in the fuselage coordinate system of the tunneling equipment, we can obtain:

[0128]

[0129] Meanwhile, the above formula can be expressed as δV b =P1δV n +P2σ n +P3δθ.

[0130] In the formula,

[0131] From the above equation, and combining the velocity measurement formula and the state variable formula, the measurement equation for vehicle kinematic constraint assistance is obtained as follows:

[0132] Z = UX + V.

[0133] In the formula, V is the equivalent measurement noise, X is the state equation of the integrated inertial navigation system, and U is the measurement matrix.

[0134] Specifically, U is represented as:

[0135]

[0136] In the formula, P1(1,×) means taking the first row of the 3×3 matrix P1, P2(1,×) means taking the first row of the 3×3 matrix P2, P1(3,×) means taking the third row of the 3×3 matrix P1, and P2(3,×) means taking the third row of the 3×3 matrix P2.

[0137] Example 3

[0138] This embodiment details the process of locating the current position of the combined inertial navigation system using a navigation method that combines inertial navigation with zero-velocity correction when the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is stationary.

[0139] The inertial navigation system is zero-velocity corrected using a quadratic curve fitting method. Specifically, within a short zero-velocity correction time interval, the velocity error curve of the inertial navigation system is fitted with a quadratic curve. The position error curve is obtained by integrating the velocity error curve, thus obtaining the position error. The position error is then summed with the position output parameters of the inertial navigation system to obtain the corrected position parameters.

[0140] Let the velocity error of the tunneling equipment when it is stationary be dv, then we have:

[0141]

[0142] In the formula, dv i For the tth i The observed velocity error of the inertial navigation system at the moment of stopping; k0, k1, and k2 are three undetermined coefficients of the quadratic curve; t j (j = 1, 2, 3...) represents the parking time.

[0143] By continuously measuring the tunneling machine speed output by the inertial navigation system during m (3≤m≤q, where q is the total number of stops) stops, we can obtain:

[0144]

[0145] In the formula: dv j For the tth j The velocity error of the inertial navigation system at time (j = 1, 2, ..., q) when the system stops; t j This represents the time corresponding to the j-th parking session.

[0146] By the least squares method, we get:

[0147] K = (T T T) -1 T T dV.

[0148] Where dV = TK, dV = [dv j dv j+1 … dv j+m-1 ] T K = [k0 k1 k2] T ,

[0149] Solve for the three undetermined coefficients of the quadratic curve based on the above equations, for the velocity error within (t) j , t j+1 The interval integral can be used to find (t) j , t j+1 The position error within a time interval, and thus the position error calculated by the inertial navigation system. j+1 By summing the location data at different times, the location information can be corrected, thus improving positioning accuracy.

[0150] Example 4

[0151] The coal mine tunneling equipment combined inertial navigation system error compensation device provided by the present invention includes: a combined inertial navigation system.

[0152] The integrated inertial navigation system specifically includes: tunneling equipment and inertial navigation system.

[0153] The inertial navigation system is installed on the tunneling equipment.

[0154] The inertial navigation system is used to locate the current position of the combined inertial navigation system by selecting different navigation methods based on the output fault type of the inertial navigation system and the current state of the tunneling equipment.

[0155] The output fault types of inertial navigation systems include short-term faults and long-term faults.

[0156] The tunneling equipment is currently in a stationary or moving state.

[0157] Furthermore, if the output fault type of the inertial navigation system is a short-term fault type, the current position of the combined inertial navigation system is located by directly outputting the pure inertial navigation solution.

[0158] When the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is stationary, the current position of the combined inertial navigation system is located by using an inertial navigation method with zero-speed correction.

[0159] When the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is in motion, the current position of the combined inertial navigation system is determined by using a navigation method that combines inertial and vehicle kinematic constraints.

[0160] This invention employs a multi-source information fusion-based error compensation method for the combined inertial navigation system of tunneling equipment. It fully considers the working characteristics of the tunneling equipment and the requirements of the mining process. Under the combined navigation method, when the inertial navigation system and odometer are used for navigation and positioning, a speed-constrained combined navigation error compensation method is introduced to address situations where the odometer slips or fails, and the tunneling machine is in various working states such as moving and stationary. When the odometer experiences a short-term abnormal output failure, the controller directly outputs the pure inertial navigation calculation result; when the odometer fails for a longer period, the combined navigation system switches to a speed-constrained auxiliary combination scheme. When the tunneling equipment is moving, an inertial method combined with vehicle kinematic constraints (NHC) is executed; when the tunneling equipment is stationary, an inertial method combined with zero-speed correction (ZUPT) is executed. Different combined navigation system error compensation methods are executed according to different motion states and different fault types of the tunneling equipment, ensuring that the combined navigation system maintains high-precision navigation and positioning functions even when the odometer fails.

[0161] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0162] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A method for error compensation in a combined inertial navigation system for coal mine tunneling equipment, characterized in that, include: Obtain the output fault type of the inertial navigation system; the output fault type of the inertial navigation system includes short-time faults and long-time faults; If the output fault type of the inertial navigation system is a short-term fault type, the current position of the combined inertial navigation system is located by directly outputting the pure inertial navigation solution; the combined inertial navigation system is formed by setting an inertial navigation system on the tunneling equipment; If the output fault type of the inertial navigation system is a long-term fault type, the current state of the tunneling equipment is obtained; the current state of the tunneling equipment is either stationary or moving. When the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is stationary, the current position of the combined inertial navigation system is located using an inertial navigation method combined with zero-velocity correction. Specifically, this includes: fitting the velocity error curve of the inertial navigation system using a quadratic curve; determining the position error curve of the combined inertial navigation system by integrating the velocity error curve; obtaining the position output parameters of the combined inertial navigation system; determining the corrected position parameters of the combined inertial navigation system based on the position error curve and the position output parameters; and locating the current position of the combined inertial navigation system based on the corrected position parameters. When the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is in motion, the current position of the combined inertial navigation system is located using a navigation method that combines inertial navigation with vehicle kinematic constraints. Specifically, this includes: determining the observation inertial navigation system based on vehicle kinematic constraints; determining the velocity transformation matrix between the inertial navigation system coordinate system and the tunneling equipment coordinate system based on the observation inertial navigation system; determining the state equation of the combined inertial navigation system based on the transformation matrix; determining the velocity measurement value of the tunneling equipment by using the zero X-axis velocity and zero Z-axis velocity of the tunneling equipment coordinate system as constraints on the state equation; determining the measurement equation assisted by vehicle kinematic constraints by combining the state equation and the velocity measurement value; and locating the current position of the combined inertial navigation system based on the measurement equation.

2. The error compensation method for the combined inertial navigation system of coal mine tunneling equipment according to claim 1, characterized in that, Obtaining the output fault type of the inertial navigation system, previously also included: The state of the inertial navigation system is obtained; the state of the inertial navigation system includes a fault state and a fault-free state. If the inertial navigation system is in a fault-free state, the current position of the combined inertial navigation system is located using a multi-source heterogeneous integrated navigation method; If the inertial navigation system is in a fault state, the current position of the combined inertial navigation system is located by using a navigation method that directly outputs pure inertial navigation calculation results, a navigation method that combines inertial navigation with zero-speed correction, or a navigation method that combines inertial navigation with vehicle kinematic constraints.

3. The error compensation method for the combined inertial navigation system of coal mine tunneling equipment according to claim 1, characterized in that, The transformation matrix is: ； in, The transformation matrix is... The velocity of the tunneling equipment in the coordinate system of the inertial navigation system is given. The speed of the tunneling equipment is given in the coordinate system of the tunneling equipment.

4. The error compensation method for the combined inertial navigation system of coal mine tunneling equipment according to claim 1, characterized in that, The state equation of the integrated inertial navigation system is: ； in, Let X be the X-axis velocity of the tunneling equipment in the coordinate system of the inertial navigation system. Let Y be the Y-axis velocity of the tunneling equipment in the coordinate system of the inertial navigation system. The Z-axis velocity of the tunneling equipment in the coordinate system of the inertial navigation system is given. Let be the pitch angle of the tunneling equipment in the coordinate system of the inertial navigation system. The heading angle of the tunneling equipment in the coordinate system of the inertial navigation system is given by [reference to the inertial navigation system]. Let Y be the Y-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment.

5. The error compensation method for the combined inertial navigation system of coal mine tunneling equipment according to claim 1, characterized in that, The velocity measurement value is: ； Where Z is the measured speed of the tunneling equipment. Let X be the X-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment. Let Z be the Z-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment. The error is the X-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment. The error is the Z-axis velocity of the tunneling equipment in the coordinate system of the tunneling equipment.

6. The error compensation method for the combined inertial navigation system of coal mine tunneling equipment according to claim 1, characterized in that, The measurement equation is as follows: ； Where Z is the measured speed of the tunneling equipment, U is the measurement matrix, X is the state equation of the combined inertial navigation system, and V is the equivalent measurement noise.

7. An error compensation device for a combined inertial navigation system of coal mine tunneling equipment, characterized in that, The error compensation device for the combined inertial navigation system of the coal mine tunneling equipment adopts the error compensation method for the combined inertial navigation system of the coal mine tunneling equipment as described in any one of claims 1-6. The error compensation device for the combined inertial navigation system of the coal mine tunneling equipment includes: a combined inertial navigation system. The combined inertial navigation system specifically includes: tunneling equipment and an inertial navigation system; The inertial navigation system is installed on the tunneling equipment; The inertial navigation system is used to select different navigation methods to locate the current position of the combined inertial navigation system according to the output fault type of the inertial navigation system and the current state of the tunneling equipment. The output fault types of the inertial navigation system include short-term faults and long-term faults; The current state of the tunneling equipment is either stationary or moving.

8. The error compensation device for the combined inertial navigation system of coal mine tunneling equipment according to claim 7, characterized in that, If the output fault type of the inertial navigation system is a short-term fault type, the current position of the combined inertial navigation system is located by directly outputting the pure inertial navigation solution result. When the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is a stationary state, the current position of the combined inertial navigation system is located by using an inertial navigation method combined with zero-speed correction. When the output fault type of the inertial navigation system is a long-term fault type and the current state of the tunneling equipment is in motion, the current position of the combined inertial navigation system is located using a navigation method that combines inertial and vehicle kinematic constraints.

Images