A multi-transmitter sensing error compensation system for a shearer running posture

Abstract

This invention discloses a multi-sensor perception error compensation system for the operating attitude of a coal mining machine, relating to the field of coal mine machinery automation control technology. The data processing unit, based on a contact dynamics model, decomposes lateral contact pressure into inertial centrifugal and elastic contact terms, analyzing the macroscopic geometric curvature. Simultaneously, using speed-following bandpass filtering and Hilbert transform techniques, it extracts phase shifts from the meshing vibration signal to invert the microscopic geometric curvature. The system establishes a dual threshold discrimination mechanism based on feature consistency and vibration energy to identify strong truncation interference, generates fused curvature observations using an adaptive weighting model, and weights the observation noise covariance matrix. Finally, based on the fused curvature and travel speed, a virtual track geometric constraint is constructed. A fifteen-dimensional error state Kalman filter is used to estimate the attitude misalignment angle and sensor zero bias, enabling real-time closed-loop correction of the strapdown inertial navigation system.

Description

Technical Field

[0001] This invention belongs to the field of coal mine machinery automation control, specifically relating to a multi-sensor sensing error compensation system for the operating posture of a coal mining machine. Background Technology

[0002] As the core equipment in fully mechanized mining faces, the precise perception and control of the coal mining machine's operating posture is fundamental to achieving unmanned and intelligent underground mining. During fully mechanized mining operations, the coal mining machine reciprocates along the scraper conveyor rails laid on the floor, and the straightness of its trajectory directly determines the advancement quality of the working face and the effectiveness of the hydraulic supports in moving with the machine. Therefore, real-time and accurate acquisition of the coal mining machine's position, speed, and attitude information in three-dimensional space is crucial for ensuring automatic correction of the machine's course and preventing equipment damage and engineering quality accidents caused by excessive tilting or yaw.

[0003] In the existing coal mine underground positioning technology system, the strapdown inertial navigation system (SINS) has become the mainstream solution for attitude perception of coal mining machines due to its advantages such as strong autonomy, high data update rate, and independence from external signal sources. Typically, the industry uses a dead reckoning mode combining the SINS with an odometer. This involves using gyroscopes and accelerometers to measure the angular velocity and specific force of the aircraft, calculating the instantaneous motion state through integration, and using an odometer installed on the running gear to provide velocity assistance information, aiming to suppress the divergence of inertial navigation calculation errors to some extent. In addition, some technical solutions attempt to use lidar or visual sensors to scan and model the surrounding environment to help correct the accumulated errors of the inertial navigation system.

[0004] However, strapdown inertial navigation systems (INS) inherently possess integral accumulation error characteristics. As operating time increases, their positioning and attitude determination accuracy tends to diverge over time, necessitating closed-loop correction using high-precision external observation information. In the specific confined space of a longwall mining face in a coal mine, satellite navigation signals cannot provide coverage, and the high dust, water mist, and low illumination severely limit the effectiveness of optical and laser sensors, making the scraper conveyor's guide rail the only available relative geometric reference. However, existing auxiliary positioning methods based on guide rail geometric constraints often rely on the observation of a single physical quantity, such as judging the track direction solely by the force exerted on the guide shoe, or estimating the travel distance solely by the odometer. During heavy cutting operations, the enormous reaction force generated by the coal wall on the drum is directly transmitted to the machine body and guide shoes, resulting in strong non-geometric interference noise mixed in with the sensor signals. This makes it difficult for a single sensor to distinguish the actual motion state of the machine body from abnormal fluctuations caused by external loads, hindering the construction of stable and reliable virtual track geometric constraints to effectively suppress the error divergence of the INS and failing to meet the precise positioning requirements for long-term continuous operation of the coal mining machine. Summary of the Invention

[0005] This invention provides a multi-sensor sensing error compensation system and method for the operating attitude of a coal mining machine, aiming to solve the problems of accumulated error divergence caused by the lack of an external absolute position reference in the existing inertial navigation system of coal mining machines during long-term operation, and the difficulty of a single sensor to adapt to strong cutting resistance interference underground.

[0006] The first aspect of the present invention provides a multi-sensor sensing error compensation system for the operating attitude of a coal mining machine. This system mainly includes an inertial navigation unit, an odometer unit, a contact force sensing unit, a meshing vibration monitoring unit, and a data processing unit.

[0007] An inertial navigation unit is installed on the coal mining machine body, responsible for collecting the three-axis specific force and three-axis angular velocity of the coal mining machine, and performing strapdown inertial navigation calculations to deduce the prior motion state; an odometer unit is responsible for monitoring the real-time travel speed of the coal mining machine; a contact force sensing unit is installed on the side of the coal mining machine guide shoe, responsible for detecting the lateral contact pressure between the shoe and the scraper conveyor pin rail; a meshing vibration monitoring unit is installed in the coal mining machine traveling box, responsible for collecting the vibration acceleration signal generated by the meshing of the traveling wheels and the pin rail. A data processing unit is communicatively connected to each of the above units, and is configured to perform macroscopic mechanical inversion, microscopic meshing feature extraction, multi-source feature consistency fusion, and error state Kalman filter correction.

[0008] In terms of macroscopic mechanical inversion, the data processing unit, based on the contact dynamics model, decomposes the lateral contact pressure into inertial centrifugal force components and elastic contact force components. The specific calculation logic is as follows: First, the inertial centrifugal coefficient is calculated, its value equal to the product of the total mass of the coal mining machine and the square of its real-time travel speed. Second, the static structural elastic coefficient is calculated, its value equal to one-eighth of the product of the equivalent lateral contact stiffness of the guide shoe and the square of the effective contact length of the guide shoe. Then, the inertial centrifugal coefficient and the static structural elastic coefficient are added to obtain the system's total curvature response coefficient. Simultaneously, the effective guiding force is calculated, its value equal to the quasi-static lateral contact force signal after low-pass filtering minus the lateral frictional resistance constant. Finally, the macroscopic geometric curvature is calculated by dividing the effective guiding force by the system's total curvature response coefficient.

[0009] In terms of micro-meshing feature extraction, the data processing unit first calculates the theoretical reference meshing frequency, which dynamically changes with traction speed, based on the real-time read feedback value of the coal mining machine drive motor speed and the transmission ratio. Then, a finite-length unit impulse response filter with linear phase characteristics is constructed as an adaptive digital bandpass filter. The center frequency of the adaptive digital bandpass filter follows the theoretical reference meshing frequency in real time, and the passband width covers the first and second-order sideband frequencies. The data processing unit uses the adaptive digital bandpass filter to output a narrowband vibration signal and constructs an analytical signal using a digital Hilbert transform algorithm based on the fast Fourier transform. The instantaneous wrapped phase is calculated using the four-quadrant arctangent function, and phase unwrapping is performed to obtain the cumulative full phase. This is then subtracted from the theoretical reference linear phase generated by discrete-time numerical integration over the theoretical reference meshing frequency, thereby demodulating the phase offset. Finally, the data processing unit calculates the increment of the phase offset within the current sampling period, divides the phase offset increment by the travel arc length increment, and multiplies it by the micro-mapping coefficient to obtain the micro-geometric curvature. The value of the micro-mapping coefficient here is equal to the reciprocal of the pitch circle radius of the coal mining machine's drive wheel, or it can be obtained through actual measurement and calibration.

[0010] In terms of multi-source feature consistency fusion, the data processing unit executes a dual threshold discrimination logic. First, the absolute value of the difference between the macroscopic and microscopic geometric curvatures is calculated as the consistency discrimination index, and the vibration energy value is obtained by summing the squares of the vibration acceleration signal. When the consistency discrimination index is greater than a preset consistency tolerance threshold, and the vibration energy value is greater than a preset truncated vibration energy threshold, the system is determined to be in a high-load truncated condition. Under this condition, the data processing unit uses an expansion coefficient greater than one to weight and amplify the observation noise covariance matrix of the error state Kalman filter to suppress interference. Simultaneously, the data processing unit constructs an adaptive weight generation model based on the Sigmoid function. This model defines a nonlinear mapping relationship between the deviation of the consistency discrimination index and the vibration energy value from their respective thresholds and the fusion weight coefficient. When the deviation is positive, the fusion weight coefficient monotonically decreases and tends towards zero, thereby reducing the weight of macroscopic geometric curvature when generating fused curvature observations and prioritizing the adoption of microscopic geometric curvature.

[0011] For error correction, the data processing unit constructs an error-state Kalman filter. The filter's error-state vector contains fifteen components: three-dimensional position error, three-dimensional velocity error, three-dimensional attitude misalignment angle, three-dimensional gyroscope bias error, and three-dimensional accelerometer bias error in the navigation coordinate system. The data processing unit calculates the virtual geometric heading rate by multiplying the fused curvature observation value by the real-time walking speed. It then subtracts the virtual geometric heading rate from the measured heading rate calculated by the inertial navigation unit to obtain the observation residual, thereby constructing the virtual orbit observation equation. The Jacobian matrix of the virtual orbit observation equation is a row vector. The sub-matrix elements corresponding to the attitude misalignment angle are formed by the third row elements of the antisymmetric matrix corresponding to the navigation coordinate system angular velocity, and the sub-matrix elements corresponding to the gyroscope bias error are formed by the negatives of the third row elements of the rotation matrix.

[0012] For closed-loop feedback correction, the data processing unit performs corrections using the posterior error state estimate output by the Kalman filter: correcting the prior attitude quaternion of the strapdown inertial navigation system using the quaternion left multiplication rule; correcting position and velocity using vector subtraction; and updating the zero bias values ​​of the gyroscope and accelerometer using an accumulation method. After correction, the data processing unit forces the posterior error state estimate in the filter to a zero vector to maintain the small error linearization assumption.

[0013] The second aspect of this invention provides a method for compensating for multi-sensor sensing errors in the operating attitude of a coal mining machine. This method is based on the aforementioned system and includes the following steps: Simultaneously acquiring multi-source heterogeneous data through an inertial navigation unit, an odometer unit, a contact force sensing unit, and a meshing vibration monitoring unit; using a contact mechanics model, analyzing the macroscopic geometric curvature reflecting the macroscopic bending trend of the track based on lateral contact pressure, real-time travel speed, and system structural stiffness parameters; using signal demodulation technology, extracting phase shift features from the meshing vibration signal, and mapping the microscopic geometric curvature reflecting the microscopic deformation of the track through spatiotemporal transformation; identifying the cutting resistance interference condition by comparing the consistency of macroscopic and microscopic features and assessing the current vibration energy, and dynamically allocating fusion weights accordingly to generate anti-interference fused curvature observation values; constructing virtual track geometric constraints based on the fused curvature observation values, estimating the state error of the inertial navigation system and sensor zero bias through an error state Kalman filter; and feeding back the estimated error state to the strapdown inertial navigation solution process to perform real-time closed-loop correction of the coal mining machine's motion state.

[0014] Compared with the prior art, the beneficial effects of the present invention are that by constructing a contact dynamics model that includes inertial centrifugal coefficient and static structural elastic coefficient, the macroscopic geometric curvature is analyzed, and the phase offset is extracted from the narrowband vibration signal by combining the digital Hilbert transform algorithm based on fast Fourier transform to invert the microscopic geometric curvature. The macroscopic bending trend of the track reflected by lateral contact pressure and the microscopic geometric deformation reflected by the meshing phase of the traveling wheel are complementarily fused, which overcomes the perception limitations of a single sensor under specific working conditions and realizes accurate perception of the geometry of the scraper conveyor pin rail under all working conditions without the need for an external absolute position reference.

[0015] By establishing a dual threshold discrimination logic based on the consistency discrimination index of macro-micro curvature difference and vibration energy value, when abnormal fluctuations in lateral contact pressure caused by high-load cutting conditions are detected, the adaptive weight generation model based on exponential function is used to automatically reduce the fusion weight coefficient of macro-geometric curvature, and the expansion coefficient is used to weight and amplify the observation noise covariance matrix of the error state Kalman filter. This achieves effective isolation of non-geometric interference signals caused by coal wall cutting resistance and a significant improvement in the system's anti-interference robustness.

[0016] By calculating the virtual geometric heading rate using the weighted fused curvature observations and real-time walking speed, and subtracting it from the measured heading rate calculated by the inertial navigation system, a virtual orbit observation equation is constructed. A 15-dimensional error state Kalman filter incorporating sensor zero bias is designed for closed-loop feedback. The attitude quaternion, velocity, and position of the strapdown inertial navigation system are corrected in real time using the posterior error state estimate. Simultaneously, the zero bias drift of the gyroscope and accelerometer is compensated online, effectively suppressing the time-varying divergence characteristics of the inertial navigation system and eliminating the cumulative error during long-distance operation. Attached Figure Description

[0017] Figure 1 This is a system flowchart according to one embodiment of the present invention. Detailed Implementation

[0018] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings, but it should be understood that the scope of protection of the present invention is not limited to the specific embodiments.

[0019] The coal mining machine operating attitude multi-sensor sensing error compensation system provided in this embodiment includes: an inertial navigation unit, an odometer unit, a contact force sensing unit, a meshing vibration monitoring unit, and a data processing unit.

[0020] Reference Figure 1The inertial navigation unit is rigidly fixed to the geometric center or center of gravity of the coal mining machine. The inertial navigation unit integrates a three-axis fiber optic gyroscope or microelectromechanical system gyroscope, as well as a three-axis accelerometer, to collect three-axis specific force signals and three-axis angular velocity signals of the coal mining machine body.

[0021] The odometer unit is installed on the output shaft of the drive motor or the axle of the traveling wheel of the coal mining machine. The odometer unit is used to collect the instantaneous traveling speed and cumulative travel data of the coal mining machine as it travels along the scraper conveyor.

[0022] The contact force sensing unit is located on the side of the coal mining machine's guide shoe facing the scraper conveyor's guide rail. The contact force sensing unit includes an array of pressure sensors or a strain pin array embedded between the wear-resistant layer and the shoe substrate. The sensing surface of the contact force sensing unit is perpendicular to the coal mining machine's travel direction to detect the lateral contact pressure between the guide shoe and the guide rail.

[0023] The meshing vibration monitoring unit is fixed to the surface of the coal mining machine's traveling box via rigid bolt connection or magnetic adsorption, and is installed near the drive wheel bearing seat of the traveling section. The meshing vibration monitoring unit is a high-frequency piezoelectric accelerometer sensor used to collect high-frequency vibration acceleration signals generated when the coal mining machine's traveling drive wheel meshes with the rack and pinion of the scraper conveyor.

[0024] The data processing unit is an explosion-proof industrial computer or embedded controller located inside the electrical control box of the coal mining machine. The data processing unit is connected to the inertial navigation unit, odometer unit, contact force sensing unit, and meshing vibration monitoring unit via an industrial fieldbus or shielded cable to receive signals collected by each unit and perform attitude error compensation calculations.

[0025] In terms of physical fit, the coal mining machine body spans the scraper conveyor's guide slipper at the bottom, and the drive wheel of the coal mining machine's traveling unit meshes with the scraper conveyor's guide rail. Lateral contact pressure data detected by the contact force sensing unit reflects the macroscopic geometric bending morphology of the scraper conveyor in the horizontal plane; the phase characteristics of the vibration signal collected by the meshing vibration monitoring unit reflect the microscopic geometric misalignment state of the meshing point between the drive wheel and the guide rail.

[0026] The multi-sensor sensing error compensation method for the operating posture of a coal mining machine provided by this invention relies on the aforementioned data processing unit and its connected sensor units, and specifically includes the following steps:

[0027] Step S100: Synchronous Acquisition of Multi-Source Heterogeneous Data During each sampling cycle of the system, the data processing unit synchronously triggers and reads data from each sensor via the hardware interface. Specifically, this includes: reading the three-axis specific force signal and three-axis angular velocity signal of the coal mining machine body output by the inertial navigation unit; reading the instantaneous travel speed signal and cumulative travel signal of the traveling unit output by the odometer unit; reading the lateral contact pressure signal of the guide shoe output by the contact force sensing unit; and reading the high-frequency vibration acceleration signal of the traveling drive unit output by the meshing vibration monitoring unit.

[0028] Step S200: The strapdown inertial navigation system (SINS) calculation and prior state derivation data processing unit uses the system state (including attitude, velocity, and position) after error correction from the previous sampling time as the initial conditions for the current time. Combining the current inertial data and odometer data collected in step S100, the data processing unit runs the SINS calculation algorithm, and through attitude update, velocity update, and position update operations, calculates the prior attitude, prior velocity, and prior position of the coal mining machine at the current time. The prior state obtained in this stage includes the drift error of the inertial devices accumulated over time.

[0029] Step S300: The macroscopic track inversion data processing unit based on contact mechanics uses the collected lateral contact pressure signal as the observation input, combines it with the pre-stored overall mass parameters of the coal mining machine and the real-time operating speed, and substitutes it into the preset slipper-pin rail contact dynamics model. The data processing unit performs inverse calculation on the contact dynamics model to separate the lateral force component caused by track geometric deformation and calculates the macroscopic geometric curvature reflecting the macroscopic bending degree of the scraper conveyor's pin rail.

[0030] Step S40: The micro-constraint extraction data processing unit based on meshing time-frequency characteristics performs bandpass filtering on the acquired high-frequency vibration acceleration signal. The data processing unit calculates the reference meshing frequency between the gear and the guide rail based on the real-time rotational speed of the travel drive motor and the number of gear teeth, and extracts a narrow-band vibration signal centered on this frequency. Subsequently, the instantaneous phase of the narrow-band vibration signal is demodulated using Hilbert transform, and the phase offset relative to the theoretical linear travel phase is calculated. This phase offset is then mapped to a micro-geometric curvature reflecting the micro-misalignment state of the guide rail meshing point.

[0031] Step S500: Feature Consistency Judgment and Confidence Assessment. The data processing unit calculates the consistency difference between the macroscopic geometric curvature obtained in step S300 and the microscopic geometric curvature obtained in step S400. If the consistency difference is less than a preset difference threshold, the data processing unit determines that the macroscopic and microscopic features match and generates a high-confidence fused curvature observation. If the consistency difference is greater than the preset difference threshold, and the rate of change of the macroscopic geometric curvature exceeds a preset fluctuation threshold, the data processing unit determines that the current lateral force change originates from coal wall cutting resistance interference. In this case, the fusion weight of the observation at that moment is automatically reduced or the observation is directly discarded to prevent erroneous correction.

[0032] Step S600: The error state Kalman filter correction data processing unit multiplies the fused curvature verified in step S500 by the real-time operating speed, converting it into a virtual heading angular rate, and inputs this virtual heading angular rate as measurement information into the error state Kalman filter. The data processing unit calculates the observation residual between the virtual heading angular rate and the heading angular rate calculated by inertial navigation, updates the system state using the Kalman gain matrix, and estimates the system error state vector including attitude error, position error, and gyroscope zero bias.

[0033] Step S700: The closed-loop feedback and trajectory generation data processing unit feeds back the system error state vector estimated in step S600 to the strapdown inertial navigation system, performing subtraction correction on the prior attitude, prior velocity, and prior position from step S200 to obtain the accurate posterior motion state. Simultaneously, it compensates for the raw output data of the inertial navigation unit using the estimated gyroscope zero bias. Finally, the data processing unit outputs the corrected coal mining machine trajectory data.

[0034] The contact mechanics model construction method provided by this invention is executed by the data processing unit based on Newton's second law and Hertz's contact theory.

[0035] The data processing unit establishes the dynamic equilibrium equations of the entire coal mining machine in the horizontal plane. When the coal mining machine runs along the curved path laid by the scraper conveyor, the data processing unit processes the lateral contact pressure detected by the contact force sensing unit. It is decomposed into the superposition of inertial centrifugal force components and elastic contact force components.

[0036] The data processing unit reads the preset overall mass parameters of the coal mining machine from the storage module. and the effective contact length parameter of the guide shoe .

[0037] The data processing unit acquires the real-time walking speed transmitted by the odometer unit. .

[0038] The data processing unit defines the macroscopic geometric curvature of the current position of the scraper conveyor to be solved as . .

[0039] The data processing unit constructs lateral contact pressure. With macroscopic geometric curvature The dynamic equations.

[0040] The data processing unit determines the inertial centrifugal force component and the overall mass parameters of the coal mining machine. Real-time walking speed The square of and macroscopic geometric curvature The product is directly proportional.

[0041] The data processing unit defines the equivalent lateral contact stiffness between the wear-resistant layer of the guide shoe and the guide rail as follows: .

[0042] The data processing unit calculates the geometric interference between the slipper and the guide rail caused by track curvature. .

[0043] The data processing unit uses the chord height formula to approximate the geometric interference. With macroscopic geometric curvature The mapping relationship, that is ,in The effective contact length of the guide shoe.

[0044] The data processing unit integrates the inertial centrifugal force component and the elastic contact force component to generate the contact dynamics model, which is expressed as follows:

[0045]

[0046] in, The real-time lateral contact pressure value output by the contact force sensing unit; the first item The second term represents the centrifugal force generated by the coal mining machine's curvilinear motion. Characterizes the elastic restoring force generated by the compression of the guide pin due to the slipper chord height effect; This is the system's preset lateral frictional resistance constant or the sensor's zero bias correction value.

[0047] The data processing unit utilizes the aforementioned contact dynamics model, under known conditions... , and system structure parameters , , Under the given conditions, the unique macroscopic geometric curvature is solved by algebraic inverse operations. .

[0048] The macroscopic geometric curvature calculation method provided by the present invention is executed by a data processing unit and specifically includes the following processing steps: the data processing unit performs digital signal conditioning on the raw lateral contact pressure signal received from the contact force sensing unit.

[0049] Because the broadband random vibration generated by the coal cutting operation is superimposed on the force signal of the guide shoe, the data processing unit uses a preset digital low-pass filter to process the raw signal of the lateral contact pressure.

[0050] The data processing unit reads the speed feedback signal of the coal mining machine's cutting motor in real time and calculates the real-time rotational frequency of the cutting drum.

[0051] The data processing unit dynamically sets the cutoff frequency of the digital low-pass filter to a level lower than the real-time rotational frequency of the cutting drum to filter out high-frequency cutting noise and output a quasi-static lateral contact force signal that reflects changes in track geometry. .

[0052] Based on the aforementioned contact dynamics model, the data processing unit performs macroscopic geometric curvature analysis. Perform analytical solution.

[0053] The data processing unit reads preset system parameters from the memory, including the overall mass of the coal mining machine. Effective contact length of guide shoe Equivalent lateral contact stiffness and lateral friction resistance constant .

[0054] The data processing unit synchronously reads the real-time walking speed output by the odometer unit. .

[0055] The data processing unit calculates the current inertial centrifugal coefficient, the value of which is equal to the total mass of the coal mining machine. squared with real-time walking speed The product of.

[0056] The data processing unit calculates the static structural elasticity coefficient term, the value of which is equal to the equivalent lateral contact stiffness. Square of effective contact length with guide shoe One-eighth of the product, that is .

[0057] The data processing unit adds the inertial eccentricity coefficient term to the static structural elasticity coefficient term to obtain the system's total curvature response coefficient at the current moment. Since the static structural elasticity coefficient is a constant and greater than zero, the total curvature response coefficient of the system is... The value remains non-zero and positive whether the coal mining machine is stationary or in motion, ensuring the numerical stability of the division operation.

[0058] The data processing unit calculates the effective guiding force, which is equal to the quasi-static lateral contact force signal. Subtract the lateral friction constant .

[0059] The data processing unit divides the effective guiding force by the total curvature response coefficient of the system. The macroscopic geometric curvature of the current position is calculated. .

[0060] The final calculation formula executed by the data processing unit is as follows:

[0061]

[0062] The data processing unit refreshes the macroscopic geometric curvature in real time during each sampling period. The calculation results are then stored in a cache area as input data for the subsequent multi-source feature consistency discrimination module.

[0063] The signal preprocessing method provided by the present invention is executed by a data processing unit and specifically includes the following steps: the data processing unit receives the analog voltage signal transmitted by the meshing vibration monitoring unit through a high-speed analog-to-digital conversion interface.

[0064] The data processing unit discretizes the analog voltage signal according to a preset sampling frequency, generating a digital vibration signal in the form of a time series. The sampling frequency is set according to the Nyquist sampling theorem and is set to be greater than 2.56 times the theoretical maximum meshing frequency of the traveling wheel and the guide rail, in order to avoid high-frequency aliasing and ensure waveform reconstruction accuracy.

[0065] The data processing unit performs detrending processing on the discretized digital vibration signal. The data processing unit uses the least squares method to perform linear fitting on the digital vibration signal within the sampling window, calculating the DC component and linear trend component of the signal.

[0066] The data processing unit subtracts the DC component and the linear trend component from the digital vibration signal and corrects the baseline of the digital vibration signal to the zero axis to eliminate low-frequency interference caused by sensor zero-point drift or temperature changes.

[0067] The data processing unit performs a center frequency calculation based on speed tracking. The data processing unit reads the drive motor speed feedback value from the frequency converter of the coal mining machine's traveling mechanism in real time via an industrial fieldbus. .

[0068] The data processing unit retrieves the transmission parameters of the traveling mechanism stored in its internal registers. These transmission parameters include the number of teeth on the traveling gears. and the total reduction ratio of the gearbox .

[0069] The data processing unit calculates the theoretical reference meshing frequency at the current moment based on the gear transmission principle. Theoretical reference meshing frequency The calculation formula is:

[0070]

[0071] Theoretical reference meshing frequency It is dynamically adjusted in real time according to the changes in the traction speed of the coal mining machine.

[0072] The data processing unit constructs an adaptive digital bandpass filter. To prevent the filter from introducing phase distortion that would affect the subsequent phase demodulation accuracy, the data processing unit selects a finite-length unit impulse response (FIR) filter with linear phase characteristics.

[0073] The data processing unit sets the center frequency of the adaptive digital bandpass filter in real time to the calculated theoretical reference meshing frequency. .

[0074] The data processing unit sets the passband width of the adaptive digital bandpass filter. Passband width The value range is set to cover the theoretical reference meshing frequency. The first and second order sideband frequencies are used to preserve the amplitude modulation and frequency modulation characteristics caused by the impact of the gap between the traveling wheel and the pin rail.

[0075] The data processing unit inputs the detrended digital vibration signal into the adaptive digital bandpass filter.

[0076] The data processing unit uses an adaptive digital bandpass filter to filter out the vibration components of the cutting motor, the vibration components of the scraper chain, and the noise components of the hydraulic pump station that are outside the passband range. It outputs a narrowband vibration signal that contains only the meshing characteristics of the traveling wheel and the pin rail, and then transmits the narrowband vibration signal to the subsequent phase demodulation module.

[0077] The phase-locking and demodulation method provided by this invention is executed by a data processing unit and specifically includes the following steps: The data processing unit receives a discrete-time narrowband vibration signal output by an adaptive digital bandpass filter. ,in This is the index for discrete sampling points.

[0078] The data processing unit employs a digital Hilbert transform algorithm based on Fast Fourier Transform (FFT) to construct narrowband vibration signals. Analyzed signal Analyzing the signal Defined as a complex sequence, its real part is the original narrowband vibration signal. The imaginary part is the orthogonal signal after Hilbert transform. .

[0079] The data processing unit generates an analytical signal by setting the negative frequency component to zero and doubling the positive frequency component in the frequency domain, thereby eliminating the interference of negative frequencies on phase calculation.

[0080] The data processing unit calculates the instantaneous wrap phase at the current sampling time. To preserve complete phase quadrant information, the data processing unit uses the four-quadrant arctangent function (Atan2) to calculate the ratio of the imaginary to the real part of the analytic signal.

[0081] Instantaneous wrap phase The calculation formula is as follows:

[0082]

[0083] in, Function output range coverage Interval.

[0084] The data processing unit processes the instantaneous wrapping phase. The phase unwrapping algorithm is executed. The data processing unit calculates the phase difference between adjacent sampling points. .

[0085] When the phase difference The absolute value is greater than the preset threshold. At that time, the data processing unit compensates for the current and subsequent phase values. Integer multiples of the phase truncation eliminate discontinuities caused by phase cutoff and generate a cumulative full phase that changes continuously and monotonically in time. .

[0086] The data processing unit constructs a theoretical reference linear phase. The data processing unit obtains the real-time theoretical reference meshing frequency calculated in the preceding steps. Then, the discrete-time numerical integration (summation) is performed to generate a reference phase reference corresponding to the ideal misalignment-free meshing state.

[0087] The data processing unit performs phase locking and differential calculations. The data processing unit will accumulate the full phase. Subtracting the theoretical reference linear phase, eliminating the main motion phase component caused by the rotation of the drive wheel, and demodulating the phase offset caused solely by track geometry deformation, we can obtain the phase offset. .

[0088] Phase offset The calculation formula is as follows:

[0089]

[0090] in, The sampling period of the system, For the first The theoretical reference meshing frequency at each sampling time.

[0091] The data processing unit outputs the calculated phase offset. Phase offset This characterizes the dynamic lead or lag of the contact point between the traveling wheel and the guide rail of the coal mining machine relative to the theoretical center position, and also represents the phase offset. It is then transmitted to the subsequent micro-curvature mapping module.

[0092] The micro-curvature mapping method provided by this invention is executed by a data processing unit and specifically includes the following steps: The data processing unit receives the discrete-time phase offset output by the phase-locking and demodulation module. Discrete-time phase offset Characterizes the engagement point between the traveling wheel and the guide rail at time [time]. The cumulative phase error relative to the theoretical position.

[0093] The data processing unit synchronously reads the instantaneous travel speed of the coal mining machine output from the odometer unit. .

[0094] The data processing unit utilizes instantaneous walking speed With system sampling period The product of the two numbers is used to calculate the increment of the walking arc length within the current sampling period. .

[0095] The data processing unit retrieves preset micro-mapping coefficients from memory. Microscopic mapping coefficients Defined as the conversion ratio between the change in phase angle and the curvature of spatial geometry.

[0096] In this embodiment, the microscopic mapping coefficient Determined in the following manner:

[0097]

[0098] in, The pitch circle radius of the driving wheel of the coal mining machine. Alternatively, in another embodiment, the micro-mapping coefficient. It is obtained through field calibration on a standard curved track with known curvature.

[0099] The data processing unit constructs a micro-curvature calculation model based on spatiotemporal transformation. The data processing unit calculates the increment of phase offset within the current sampling period and divides this increment by the increment of the travel arc length. Then multiply by the micro-mapping coefficient This allows us to calculate the microscopic geometric curvature at the current position. .

[0100] The micro curvature calculation formula executed by the data processing unit is as follows:

[0101]

[0102] in, This represents the differential increment of the phase shift. This represents the corresponding differential increment of spatial distance. This formula realizes the mapping from the phase change rate in the time domain to the geometric curvature in the spatial domain.

[0103] The data processing unit calculates the microscopic geometric curvature. Perform moving average filtering. The data processing unit has a construction length of [length missing]. Sliding windows (e.g.) The curvature sequence within the sliding window is arithmetically averaged to suppress high-frequency quantization noise introduced by discrete difference, and the smoothed microscopic geometric curvature is output. .

[0104] The data processing unit will smooth the micro-geometric curvature. Transmitted to the multi-source feature consistency discrimination module for comparison with macroscopic geometric curvature Perform mutual verification.

[0105] The feature consistency discrimination method provided by this invention is executed by a data processing unit, and specifically includes the following discrimination and processing steps: Within each sampling period, the data processing unit synchronously reads the macroscopic geometric curvature output by the macroscopic curvature calculation module. and the microscopic geometric curvature output by the microscopic curvature mapping module .

[0106] The data processing unit calculates the macroscopic geometric curvature. With microscopic geometric curvature The absolute value of the difference, and the absolute value of the difference between macroscopic geometric curvature and microscopic geometric curvature is defined as the consistency criterion. .

[0107] The data processing unit performs energy quantization processing on the raw vibration acceleration signal acquired by the meshing vibration monitoring unit. The data processing unit selects a length of... By using a sliding time window, the discrete vibration acceleration sequence within the window is summed by squares to calculate the vibration energy value characterizing the intensity of the current cutting condition. .

[0108] The data processing unit retrieves a preset consistency tolerance threshold from memory. and the threshold of truncated vibration energy .

[0109] The data processing unit executes a dual threshold discrimination logic. First, the data processing unit sets the consistency discrimination index... With consistency tolerance threshold Comparison:

[0110] If the consistency judgment index Less than or equal to the consistency tolerance threshold The data processing unit determines that the lateral contact pressure change characteristics and the travel wheel engagement phase offset characteristics are consistent in geometric trend. At this point, the data processing unit confirms that there is actual geometric curvature in the scraper conveyor's guide rail and generates a high-confidence status flag.

[0111] If the consistency judgment index Greater than the consistency tolerance threshold The data processing unit further processes the vibration energy value. With the cutting vibration energy threshold Comparison:

[0112] If the vibration energy value Greater than the cutting vibration energy threshold The data processing unit determines that the current cutting condition is under high load. Under this condition, the data processing unit determines that the change in lateral force detected by the contact force sensing unit originates from the impact of the coal wall reaction force on the cutting drum, which is a non-geometric interference signal.

[0113] Based on the above determination results, the data processing unit adaptively adjusts the observation noise covariance matrix of the error state Kalman filter. .

[0114] When generating high-confidence status flags, the data processing unit maintains the observation noise covariance matrix. The preset nominal value .

[0115] When the signal is determined to be non-geometric interference, the data processing unit uses the expansion coefficient. (in The observation noise covariance matrix is ​​weighted and amplified, i.e., a weighted amplification is set. .

[0116] The data processing unit increases the observation noise covariance matrix. The value of the error state Kalman filter is reduced to decrease the gain weight of the fused curvature observation at the current moment, thereby suppressing the impact of truncation drag interference on the accuracy of system attitude calculation.

[0117] The dynamic weight allocation method provided by this invention is executed by a data processing unit and specifically includes the following steps: the data processing unit reads multi-source feature data of the current sampling period from memory, the multi-source feature data including macroscopic geometric curvature. Microscopic geometric curvature Consistency discrimination index and vibration energy value .

[0118] At the same time, the data processing unit retrieves the preset consistency tolerance threshold. Cutting vibration energy threshold and weighted adjustment sensitivity coefficient and .

[0119] The data processing unit constructs an adaptive weight generation model based on the Sigmoid function.

[0120] The data processing unit uses an adaptive weight generation model to calculate the fusion weight coefficients of the macroscopic geometric curvature. The adaptive weight generation model defines a consistency metric. With vibration energy value Deviation relative to their respective thresholds and weighting coefficients The nonlinear mapping relationship between them.

[0121] Weighting coefficient The calculation formula is as follows:

[0122]

[0123] in, Represents the natural exponential function. Sensitivity coefficient. and As a preset positive number, the value of the sensitivity coefficient determines the weighting coefficient. The decay rate as a function of the deviation.

[0124] According to the above formula, when the consistency discrimination index Greater than the consistency tolerance threshold Or vibration energy value Greater than the cutting vibration energy threshold When the exponent of the exponent term is positive, it increases the denominator, leading to an increase in the weighting coefficient. It decreases monotonically and tends towards 0.

[0125] Conversely, when the consistency judgment index Less than the consistency tolerance threshold And vibration energy value Less than the cutting vibration energy threshold When the exponent of the exponent term is negative, the denominator tends to be negative, resulting in a change in the weighting coefficient. It increases monotonically and tends towards a certain value.

[0126] The data processing unit calculates the fusion weight coefficients. For macroscopic geometric curvature With microscopic geometric curvature Perform linear weighted fusion and calculate the final fused curvature observations. .

[0127] Fusion curvature observations The calculation formula is as follows:

[0128]

[0129] Through the aforementioned dynamic allocation mechanism, the system achieves adaptive adjustment of the observation source weights: when the coal mining machine is operating smoothly with low resistance and the force characteristics of the slipper tend to be consistent with the meshing characteristics, the system increases the weight coefficient. Prioritize the adoption of macroscopic geometric curvature When the coal mining machine encounters strong cutting that interferes with the lateral contact pressure signal, the system automatically reduces the weighting coefficient. By increasing The proportion of this makes the system primarily dependent on microscopic geometric curvature that is less susceptible to external load disturbances. .

[0130] The data processing unit will calculate the fused curvature observations. The data is transmitted to the error state Kalman filter module as the observation input for correcting the inertial navigation attitude error.

[0131] The filter design method provided by this invention is executed by a data processing unit and specifically includes the following design and calculation steps: The data processing unit constructs an error state vector for estimating the error of the strapdown inertial navigation system. .

[0132] Error state vector Defined as a column vector containing 5 components, the error state vector The components include, in order: three-dimensional position error in the navigation coordinate system. 3D velocity error 3D attitude misalignment angle 3D gyroscope zero bias error and the zero bias error of the three-dimensional accelerometer .

[0133] Error state vector The mathematical expression is as follows:

[0134]

[0135] Among them, superscript Indicates the projection of the variable into the navigation coordinate system, superscript. This represents the matrix transpose operation.

[0136] The data processing unit establishes the discrete-time system state transition equations. Based on the error propagation dynamics model of the strapdown inertial navigation system, the data processing unit calculates the state transition matrix using the first-order Taylor expansion method. .

[0137] The discretized state transition equation describes the evolution of the error state over time. The expression for the discretized state transition equation is:

[0138]

[0139] in, For the present The prior error state at time 1. This represents the posterior error state from the previous time step. The system noise driving matrix, This is the system process noise vector containing gyroscope random walk noise and accelerometer random walk noise.

[0140] The data processing unit constructs observation equations based on virtual orbit constraints. The data processing unit utilizes the fused curvature observations output by the dynamic weight allocation module. Combined with the instantaneous walking speed output by the odometer unit Calculate the virtual reference heading angular rate .

[0141] The formula for calculating the virtual reference heading angular rate is:

[0142]

[0143] Data processing unit defines measurement residuals The data processing unit reads the output from the inertial navigation unit and calculates the measured heading angular rate through attitude update. .

[0144] The data processing unit calculates the measured heading angular rate. With virtual reference heading angular rate The difference between them, and use the difference as the measurement residual. .

[0145] The data processing unit establishes linearized observation equations:

[0146]

[0147] in, For the observation matrix, To observe noise.

[0148] The data processing unit determines the observation matrix. The elemental structure. Due to measurement residuals. Essentially, it reflects angular velocity error; the data processing unit is based on the angular velocity error equation. Constructing the observation matrix .

[0149] Observation matrix for A 3D matrix, where the 3D attitude misalignment angles correspond to the 3D matrices. The column elements consist of angular velocity components in the navigation frame, corresponding to the zero bias error of the three-dimensional gyroscope. The column elements are rotation matrices from the carrier coordinate system to the navigation coordinate system. The remaining columns corresponding to position, velocity, and accelerometer zero bias are set to zero.

[0150] The data processing unit performs the measurement update step using Kalman filtering. The data processing unit then calls the observation noise covariance matrix, which has been adaptively adjusted by the consistency discrimination module. .

[0151] The data processing unit calculates the Kalman gain matrix. And utilize measurement residuals By correcting the prior error state, the optimal posterior error state estimate is obtained. .

[0152] Finally, the data processing unit updates the estimation error covariance matrix. and the posterior error state estimate Feedback is sent to the strapdown inertial navigation system (SINS) calculation module for closed-loop correction.

[0153] The virtual orbit observation equation construction method provided by the present invention is executed by a data processing unit and specifically includes the following steps: the data processing unit establishes a virtual reference observation based on kinematic constraints.

[0154] The data processing unit calls the fused curvature observations output by the dynamic weight allocation module. and the instantaneous walking speed output by the odometer unit. .

[0155] Based on the principles of circular kinematics, the data processing unit calculates the virtual geometric heading angular rate of the coal mining machine as it travels along the pin rail of the scraper conveyor. .

[0156] Virtual geometric heading angular rate The calculation formula is:

[0157]

[0158] Virtual geometric heading angular rate It provides a heading rate of change benchmark independent of inertial navigation calculations.

[0159] The data processing unit acquires the attitude calculation data from the inertial navigation unit.

[0160] The data processing unit uses the rotation matrix determined by the current strapdown inertial navigation attitude. The angular velocity of the carrier coordinate system output by the inertial navigation unit Projected onto the navigation coordinate system, and after subtracting the Earth's rotational angular velocity component and the entrainment angular velocity component, the measured heading angular velocity perpendicular to the horizontal plane is extracted. .

[0161] The data processing unit constructs the measurement residual equation.

[0162] The data processing unit calculates and measures the heading angular velocity. With virtual geometric heading angular rate The difference between them yields the observed residual. .

[0163] Observation residuals The expression is:

[0164]

[0165] The data processing unit constructs linearized virtual orbit observation equations.

[0166] Observation residuals Modeled as an error state vector The linear function is superimposed with observation noise, and the observation equation is expressed as follows:

[0167]

[0168] in, for The Jacobian matrix of the virtual orbit observation in dimension, To observe the noise sequence.

[0169] The data processing unit analyzes and calculates the Jacobian matrix of the virtual orbit observations. The non-zero elements.

[0170] According to the error propagation principle of strapdown inertial navigation, the heading angular velocity error with attitude misalignment angle and gyroscope zero bias The following linearization relationship exists:

[0171]

[0172] in, Let ω be the angular velocity of the navigation coordinate system relative to the inertial coordinate system. for The corresponding antisymmetric matrix.

[0173] Based on the above linearization relationship, the data processing unit determines the Jacobian matrix. Specific structural form:

[0174]

[0175] in, for Submatrix, whose elements are composed of vectors The third row of the corresponding antisymmetric matrix constitutes the matrix; for Submatrix, whose elements are derived from rotation matrices It is formed by the opposite of the elements in the third row; This represents a submatrix consisting entirely of zeros.

[0176] The data processing unit determines the observation noise based on the confidence level output by the aforementioned consistency discrimination module. Statistical characteristics.

[0177] When the system is under strong truncation interference, the data processing unit increases the observation noise covariance matrix. The numerical value of the virtual orbit observation equation is reduced to decrease the weight of the state estimation and suppress filter divergence caused by false observations.

[0178] The feedback correction method provided by this invention is executed by a data processing unit and specifically includes the following steps: The data processing unit reads the posterior error state estimate at the current time from the error state Kalman filter module. .

[0179] The data processing unit extracts the three-dimensional position error vector based on the definition of the error state vector. 3D velocity error vector 3D attitude misalignment angle vector 3D gyroscope zero bias error vector and the three-dimensional accelerometer zero bias error vector .

[0180] The data processing unit performs linear compensation on the prior state of the strapdown inertial navigation system.

[0181] The data processing unit acquires the prior position calculated by the strapdown inertial navigation system. and prior speed .

[0182] The data processing unit utilizes prior location Subtract the position error vector The corrected posterior position is obtained. .

[0183] The data processing unit utilizes prior speed Subtract the velocity error vector The corrected posterior velocity is obtained. .

[0184] The data processing unit performs nonlinear correction on the attitude quaternions.

[0185] The data processing unit utilizes the attitude misalignment angle vector in the navigation coordinate system. Constructing the error quaternion for attitude correction .

[0186] Based on the small angle assumption, error quaternion The scalar part takes the value, and the vector part is composed of the attitude misalignment angle vector. It consists of half of, that is:

[0187]

[0188] The data processing unit reads the prior attitude quaternions obtained from the strapdown inertial navigation system. .

[0189] Due to attitude misalignment angle vector Defined in the navigation coordinate system, the data processing unit uses the quaternion left multiplication rule to convert the error quaternion. Multiply by the prior attitude quaternion The corrected posterior pose quaternion is obtained. .

[0190] The attitude correction formula is as follows:

[0191]

[0192] in, This represents quaternion multiplication operations.

[0193] The data processing unit performs cumulative updates of the inertial sensor's zero bias.

[0194] The data processing unit reads the gyroscope's zero bias from the previous moment stored in the memory. and accelerometer zero bias .

[0195] The data processing unit will use the currently estimated gyroscope bias error vector Superimposed to the previous moment's gyroscope zero bias The updated gyroscope zero bias was calculated. .

[0196] Similarly, the data processing unit will process the accelerometer zero bias error vector. Superimposed to the previous moment's accelerometer zero bias The updated accelerometer zero bias was calculated. .

[0197] The data processing unit will update the gyroscope's zero bias. and accelerometer zero bias The data preprocessing module fed back to the inertial navigation unit is used to perform real-time compensation on the original inertial data in the strapdown calculation of the next sampling period.

[0198] The data processing unit performs an error state reset of the filter.

[0199] After correcting the nominal state of the system (position, velocity, attitude) and the sensor zero bias, the data processing unit converts the posterior error state estimate in the error state Kalman filter. Forced to be a zero vector.

[0200] The error state reset operation ensures that the error state is always kept near zero, satisfying the small error linearization approximation condition of the extended Kalman filter.

[0201] The data processing unit outputs the corrected posterior position. Posterior speed and posterior pose quaternions This data serves as the final motion status data of the coal mining machine at the current moment and is sent to the coal mining machine's cutting height and straightness controller.

[0202] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0203] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A multi-sensor sensing error compensation system for the operating posture of a coal mining machine, characterized in that, include: Inertial navigation unit, odometer unit, contact force sensing unit, meshing vibration monitoring unit, and data processing unit; The inertial navigation unit is installed on the coal mining machine body and is used to collect the three-axis specific force and three-axis angular velocity of the coal mining machine, and perform strapdown inertial navigation calculations. The odometer unit is used to monitor the real-time travel speed of the coal mining machine; the contact force sensing unit is installed on the side of the guide shoe of the coal mining machine and is used to detect the lateral contact pressure between the guide shoe and the pin rail of the scraper conveyor. The meshing vibration monitoring unit is installed in the coal mining machine traveling box and is used to collect the vibration acceleration signal generated by the meshing of the traveling wheel and the pin rail; The data processing unit is communicatively connected to each of the above units, and the data processing unit is configured to perform the following operations: based on the lateral contact pressure and real-time walking speed, calculate the macroscopic geometric curvature using a contact dynamics model; The vibration acceleration signal is bandpass filtered and phase demodulated to extract the phase offset, and the micro-geometric curvature is calculated by combining the micro-mapping coefficients. The macroscopic and microscopic geometric curvatures are subjected to consistency discrimination and dynamic weighted fusion to generate fused curvature observations. An error state Kalman filter is constructed, and a virtual orbit observation equation is built using the fused curvature observation values ​​to estimate and correct the error state of the strapdown inertial navigation solution.

2. The coal mining machine operating attitude multi-sensor sensing error compensation system according to claim 1, characterized in that, The data processing unit is specifically used for: solving macroscopic geometric curvature. Calculate the inertial centrifugal coefficient term, the value of which is equal to the product of the total mass of the coal mining machine and the square of the real-time travel speed; Calculate the static structural elasticity coefficient term, the value of which is equal to one-eighth of the product of the equivalent lateral contact stiffness of the guide shoe and the square of the effective contact length of the guide shoe. Add the inertial eccentricity coefficient term to the static structural elasticity coefficient term to obtain the total curvature response coefficient of the system; Calculate the effective guiding force, the value of which is equal to the quasi-static lateral contact force signal after low-pass filtering minus the lateral friction resistance constant; The macroscopic geometric curvature is calculated by dividing the effective guiding force by the total curvature response coefficient of the system.

3. The coal mining machine operating attitude multi-sensor sensing error compensation system according to claim 1, characterized in that, When processing vibration acceleration signals, the data processing unit is specifically used for: The speed feedback value of the coal mining machine drive motor is read in real time, and the theoretical reference meshing frequency that dynamically changes with the traction speed is calculated based on the number of teeth of the traveling wheel and the total reduction ratio of the gearbox. A finite-length unit impulse response filter with linear phase characteristics is constructed as an adaptive digital bandpass filter. The center frequency of the adaptive digital bandpass filter is set in real time to the theoretical reference engagement frequency, and the passband width is set to cover the first and second order sideband frequencies of the theoretical reference engagement frequency. The vibration acceleration signal is filtered using the adaptive digital bandpass filter to output a narrowband vibration signal.

4. The coal mining machine operating attitude multi-sensor sensing error compensation system according to claim 3, characterized in that, When extracting the phase offset, the data processing unit is specifically used for: An analytical signal of the narrowband vibration signal is constructed using a digital Hilbert transform algorithm based on fast Fourier transform. The instantaneous wrapped phase of the analytic signal is calculated using the four-quadrant arctangent function, and phase unwrapping is performed on the instantaneous wrapped phase to obtain the cumulative full phase; The theoretical reference meshing frequency is integrated over discrete time to generate a theoretical reference linear phase; The phase offset is obtained by subtracting the theoretical reference linear phase from the accumulated full phase.

5. The coal mining machine operating attitude multi-sensor sensing error compensation system according to claim 1, characterized in that, The data processing unit is specifically used for: solving microscopic geometric curvature. Calculate the increment of the phase offset within the current sampling period and divide the increment by the travel arc length within the current sampling period; Multiplying the above calculation result by the microscopic mapping coefficient yields the microscopic geometric curvature; wherein the value of the microscopic mapping coefficient is equal to the reciprocal of the pitch circle radius of the coal mining machine's travel drive wheel, or is obtained through actual measurement and calibration.

6. The coal mining machine operating attitude multi-sensor sensing error compensation system according to claim 1, characterized in that, When performing consistency determination, the data processing unit is specifically used for: Calculate the absolute value of the difference between macroscopic geometric curvature and microscopic geometric curvature, and define this absolute value as the consistency discrimination index; The vibration acceleration signal is squared and summed to calculate the vibration energy value; when the consistency discrimination index is greater than the preset consistency tolerance threshold and the vibration energy value is greater than the preset cutting vibration energy threshold, it is determined that the current condition is a high-load cutting condition. Under the high-load truncation condition, the observation noise covariance matrix of the error state Kalman filter is weighted and amplified using an expansion coefficient greater than one.

7. The coal mining machine operating attitude multi-sensor sensing error compensation system according to claim 6, characterized in that, When generating fused curvature observations, the data processing unit is specifically used for: An adaptive weight generation model based on an exponential function is constructed to calculate the fusion weight coefficient. The adaptive weight generation model defines a nonlinear mapping relationship between the consistency discrimination index and the deviation of the vibration energy value from their respective thresholds and the fusion weight coefficient, such that when the deviation is positive, the fusion weight coefficient monotonically decreases and tends to zero. The macroscopic geometric curvature and microscopic geometric curvature are linearly weighted and fused using the fusion weighting coefficients to calculate the fused curvature observation value.

8. The coal mining machine operating attitude multi-sensor sensing error compensation system according to claim 1, characterized in that, The error state vector of the error state Kalman filter constructed by the data processing unit contains fifteen components, namely, the three-dimensional position error, three-dimensional velocity error, three-dimensional attitude misalignment angle, three-dimensional gyroscope zero bias error, and three-dimensional accelerometer zero bias error in the navigation coordinate system. The observation matrix of the error state Kalman filter is a matrix constructed based on the angular velocity error equation. The column elements corresponding to the three-dimensional attitude misalignment angle are composed of the angular velocity components in the navigation coordinate system, and the column elements corresponding to the three-dimensional gyroscope zero bias error are composed of the rotation matrix from the carrier coordinate system to the navigation coordinate system.

9. The coal mining machine operating attitude multi-sensor sensing error compensation system according to claim 1, characterized in that, The data processing unit is specifically used to construct the virtual orbit observation equations: The virtual geometric heading angular rate is calculated by multiplying the fused curvature observation value with the real-time walking speed; The observation residual is obtained by subtracting the measured heading angular velocity calculated by the inertial navigation unit from the virtual geometric heading angular rate. A virtual orbit observation Jacobian matrix is ​​constructed. The virtual orbit observation Jacobian matrix is ​​a row vector, wherein the sub-matrix elements corresponding to the attitude misalignment angle are composed of the third row elements of the antisymmetric matrix corresponding to the angular velocity of the navigation coordinate system, and the sub-matrix elements corresponding to the gyroscope zero bias error are composed of the negative numbers of the third row elements of the rotation matrix.

10. The coal mining machine operating attitude multi-sensor sensing error compensation system according to claim 1, characterized in that, The data processing unit is also used to perform feedback correction, specifically including: The posterior error state estimate is obtained from the error state Kalman filter; an error quaternion is constructed using the attitude misalignment angle vector in the navigation coordinate system, and the error quaternion is multiplied by the prior attitude quaternion calculated by the strapdown inertial navigation system using the quaternion left multiplication rule to obtain the corrected posterior attitude quaternion. The estimated gyroscope zero bias error vector and accelerometer zero bias error vector are respectively added to the sensor zero bias value of the previous moment, and the updated zero bias value is fed back to the inertial navigation unit. After the correction is completed, the posterior error state estimate in the error state Kalman filter is forcibly set to a zero vector, and the error state is reset.

Images