CN-122015831-A - Factor graph optimization-based high-precision pose positioning system for electric shovel of strip mine

CN122015831ACN 122015831 ACN122015831 ACN 122015831ACN-122015831-A

Abstract

The invention relates to a factor graph optimization-based high-precision pose positioning system for an electric shovel of a strip mine, and belongs to the technical field of electric shovel positioning. The system comprises a data acquisition module, a denoising module, a working condition identification module, a GNSS data processing module, a model construction module and a pose determination module, wherein the data acquisition module is used for acquiring an original GNSS observation sequence and an original IMU observation sequence, performing space-time alignment to obtain the GNSS observation sequence to be corrected and the IMU observation sequence to be denoised, the denoising module is used for denoising the IMU observation sequence to be denoised by adopting a pre-trained denoising diffusion model, the working condition identification module is used for identifying the current working condition of the electric shovel, the GNSS data processing module is used for correcting the GNSS observation sequence to be corrected according to the current working condition of the electric shovel, the model construction module is used for constructing a multi-source fusion factor graph optimization model according to the denoised IMU observation sequence and the corrected GNSS observation sequence, and the pose determination module is used for solving the factor graph optimization model to obtain the pose of the electric shovel. The invention can realize the high-precision pose positioning of the electric shovel of the strip mine.

Inventors

LIU YU
SHI LEI
GUO YUNFEI
SUN XINGYU
SU JINGUO
WANG SHENGLIN
WANG QINGGAO
WANG YONGPENG
Guo Ruhan
TIAN XIAOJIAN
LI RONG
ZHANG JIANTING
LI GUANG

Assignees

太原理工大学
山西太重智能采矿装备技术有限公司

Dates

Publication Date: 20260512
Application Date: 20260323

Claims (10)

1. The utility model provides a strip mine electric shovel high accuracy position appearance positioning system based on factor graph optimization which characterized in that includes: The data acquisition module is used for acquiring an original GNSS observation sequence and an original IMU observation sequence which are acquired by an antenna and an IMU which are arranged on the electric shovel in advance, and carrying out space-time alignment on the original GNSS observation sequence and the original IMU observation sequence to obtain a GNSS observation sequence to be corrected and an IMU observation sequence to be denoised; The denoising module is used for denoising the IMU observation sequence to be denoised by adopting a pre-trained denoising diffusion model to obtain a denoised IMU observation sequence; the working condition identification module is used for identifying the current working condition of the electric shovel according to the GNSS observation sequence to be corrected and the denoised IMU observation sequence; The GNSS data processing module is used for correcting the GNSS observation sequence to be corrected according to the current working condition of the electric shovel to obtain a corrected GNSS observation sequence; The model construction module is used for constructing a multi-source fusion factor graph optimization model according to the denoised IMU observation sequence and the corrected GNSS observation sequence; And the pose determining module is used for solving the factor graph optimization model to obtain the pose of the electric shovel.
2. The factor graph optimization-based strip mine electric shovel high-precision pose positioning system according to claim 1, wherein the working condition identification module comprises: The statistical characteristic calculation unit is used for calculating the horizontal speed, the course angle change rate and the vertical acceleration variance of the current moment of the electric shovel according to the denoised IMU observation sequence; The walking working condition judging unit is used for judging that the current working condition of the electric shovel is a walking working condition when the horizontal speed of the current moment of the electric shovel is larger than a preset horizontal speed threshold value, the course angle change rate of the current moment is smaller than a preset course angle change rate threshold value, and the vertical acceleration variance of the current moment of the electric shovel is smaller than a preset vertical acceleration variance threshold value; The turning working condition judging unit is used for judging that the current working condition of the electric shovel is a turning working condition when the horizontal speed of the electric shovel at the current moment is less than or equal to a preset horizontal speed threshold value and the course angle change rate at the current moment is more than or equal to a preset course angle change rate threshold value; the excavating working condition judging unit is used for judging that the current working condition of the electric shovel is an excavating working condition when the signal-to-noise ratio of the electric shovel at the current moment in the GNSS observation sequence to be corrected continuously drops or is interrupted.
3. The factor graph optimization-based strip mine electric shovel high precision pose positioning system according to claim 2, wherein the GNSS observation sequence to be corrected comprises a pseudo range sequence to be corrected, and the GNSS data processing module comprises: The power shovel comprises a walking working condition pseudo-range correction unit, a power shovel and a power shovel, wherein the walking working condition pseudo-range correction unit is used for calculating a pseudo-range mean value and a pseudo-range standard deviation of a first preset number of pseudo-ranges in a neighborhood of any pseudo-range to be corrected when the current working condition of the power shovel is the walking working condition, determining whether any pseudo-range is an abnormal pseudo-range according to the pseudo-range mean value and the pseudo-range standard deviation, if any pseudo-range is the abnormal pseudo-range, replacing any pseudo-range with the previous pseudo-range to obtain a primarily corrected pseudo-range sequence, and fitting the primarily corrected pseudo-range sequence through a quadratic polynomial fitting model to obtain the corrected pseudo-range as a corrected GNSS observation sequence; the pseudo-range correction unit of the turning working condition is used for carrying out pseudo-range smooth correction on any pseudo-range in the pseudo-range sequence to be corrected according to a second preset number of pseudo-ranges and corresponding carrier phases in the neighborhood of the pseudo-range sequence to be corrected when the current working condition of the electric shovel is the turning working condition, and obtaining a corrected pseudo-range sequence as a corrected GNSS observation sequence; The mining working condition pseudo-range correction unit is used for carrying out interpolation processing on the pseudo-range sequence to be corrected when the current working condition of the electric shovel is the mining working condition, obtaining an interpolated pseudo-range sequence, calculating the position change quantity of the electric shovel through the IMU observation sequence after denoising, and calculating the corrected pseudo-range sequence as a corrected GNSS observation sequence according to the preliminary corrected pseudo-range sequence and the position change quantity of the electric shovel.
4. The factor graph optimization-based strip mine electric shovel high-precision pose positioning system according to claim 1, wherein the model building module comprises: the state definition unit is used for defining a global state vector to be estimated of the factor graph optimization model; The factor node construction unit is used for constructing inertia factors, GNSS factors, dual-antenna baseline factors and motion constraint factors according to the denoised IMU observation sequences, the corrected GNSS observation sequences and the sliding window; The model construction unit is used for combining the global state vector to be estimated, the inertia factor, the GNSS factor, the dual-antenna baseline factor and the motion constraint factor to obtain a factor graph optimization model.
5. The factor graph optimization-based strip mine electric shovel high-precision pose positioning system according to claim 4, wherein the global state vector to be estimated Expressed as: ; ; Wherein, the The size of the sliding window is indicated, The state at the time of k is indicated, 、 And Respectively representing the position, speed and posture of a b system relative to an e system at the k moment, wherein b is a carrier coordinate system, e is a ground center ground rectangular coordinate system, And Respectively representing deviation vectors of the moment k of the accelerometer and the moment k of the gyroscope in the IMU; representing the inter-GNSS system bias vector at time k, Represents the tropospheric wet delay error component in the zenith direction of the mobile station at time k, And Respectively representing the inter-station difference and the inter-satellite difference, Indicating the integer ambiguity after the double difference, Representing the number of consecutive ambiguities within the sliding window, And Respectively, a mobile station and a reference station, IF represents ionosphere-free combining, Representing the double difference ambiguity of the mobile station and the reference station after no ionospheric combination.
6. The factor graph optimization-based strip mine electric shovel high-precision pose positioning system according to claim 5, wherein the factor node construction unit is used for constructing inertial factors: according to the denoised IMU observation sequence, the time interval is calculated recursively through the following formula IMU pre-integral term within 、 And : ; Wherein, the , The time interval between adjacent observed values of the IMU observation sequence after denoising is set; 、 And For time intervals of The IMU within it pre-integrates the terms, 、 And For time intervals of The IMU within it pre-integrates the terms, A rotation matrix representation of the quaternion; And Respectively a specific force sequence and an angular velocity sequence in the IMU observation sequence after denoising, Representing the multiplication of the quaternion, Representing accelerometers in an IMU A time offset vector; According to time intervals IMU pre-integral term within 、 And The inertia factor is calculated by the following formula : ; Wherein, the Representing a set of denoised IMU observations in a sliding window; representing the state change quantity of adjacent time points which are obtained by IMU pre-integration from time point k to time point k+1 under the system b; Is that Is represented by a rotation matrix of (a); 、 And The position, velocity and attitude of the b system relative to the e system at time k+1 are respectively shown, Representing the gravity vector under the e system; ; representing the rotation vector from which the quaternion is extracted, And The deviation vectors at time k+1 of the accelerometer and gyroscope in the IMU are shown, respectively.
7. The factor graph optimization-based strip mine electric shovel high-precision pose positioning system according to claim 5, wherein the factor node construction unit is used for constructing GNSS factors: Constructing GNSS factors including pseudo-range factors, carrier phase factors, troposphere and intersystem deviation factors according to the corrected GNSS observation sequences Expressed as: ; wherein G represents a corrected GNSS observation sequence, G represents a corrected GNSS observation value in the corrected GNSS observation sequence, Representing the GNSS residual terms constructed from the corrected GNSS observation sequences, P, L and T representing the pseudorange, carrier phase, time series and satellite sequence sets of the tropospheric and intersystem biases, respectively, i, j representing the index of the observation satellite, And The observation satellite i and the observation satellite j are represented, A robust loss function is represented and, Representing covariance matrixes corresponding to GNSS factors; 、 And Respectively representing a pseudo-range factor, a carrier phase factor, a troposphere and an intersystem deviation factor, 、 And Respectively representing a covariance matrix corresponding to a k moment pseudo-range factor, a carrier phase factor, a troposphere and an intersystem deviation factor; ; Wherein, the Is shown in the observation satellite Single difference between the mobile station and the reference station predicted by the INS is lower; Representing an observed satellite predicted by an INS With mobile station A distance therebetween; Representing an observation satellite With reference station A distance therebetween; Representing the position of the mobile station predicted by the INS; Representing an observation satellite Is a position of (2); Indicating a reference station Is a position of (2); Expressed in observation satellite Single difference between the mobile station and the reference station predicted by the INS is lower; Representing a dual difference tropospheric residual; representing double-difference pseudo-ranges after ionosphere-free combining; Representation concerning observation satellites And (3) with A geometric distance between the mobile station and the reference station; Representing double-difference pseudo-range observation noise; ; Wherein, the Representing wavelength; representing double-difference ambiguity without ionosphere combination; Representing the double difference carrier phase after ionosphere-free combining; representing the wavelength of the IF combined; representing double-difference carrier phase observation noise; ; Wherein, the 、 And Indicating the intersystem deviation parameters of BDS, galileo and GLONASS respectively, Is the tropospheric wet delay error component in the zenith direction of the mobile station, k and k+1 represent the k time instant and k+1 time instant, respectively.
8. The factor graph optimization-based strip mine electric shovel high-precision pose positioning system according to claim 5, wherein the factor node construction unit is configured to, when constructing a dual-antenna baseline factor: Constructing a dual-antenna baseline factor according to the corrected GNSS observation sequence Expressed as: ; Wherein, the And Respectively represent the observation satellites And Single difference between lower mobile station antennas A, B; representing a dual difference tropospheric residual error between mobile station antennas A, B; representing the wavelength of the IF combined; representing double-difference ambiguity between mobile station antennas A, B after ionosphere-free combining; Expressed in observation satellite And Dual differential carrier phase between lower mobile station antennas A, B; , And Respectively represent the observation satellites And The unit observation vector at the receiver, A baseline vector between two antennas A, B for the mobile station; Representing wavelength; And Respectively representing mobile stations Is related to the observation satellite by two antennas A, B And Is used to determine the dual difference ambiguity and carrier phase observation noise.
9. The factor graph optimization-based strip mine electric shovel high-precision pose positioning system according to claim 5, wherein the factor node construction unit is used for constructing a motion constraint factor: Constructing motion constraint factors including gyration motion factors, walking motion factors and mining motion factors Expressed as: ; Wherein, the Representing a set of motion constraint factors within a sliding window, Representing one of the motion constraint factors, Representing a covariance matrix corresponding to the motion constraint factor; s, W and D respectively represent a set of time sequences in the processes of turning, walking and excavating; 、 And Respectively representing a gyration motion factor, a walking motion factor and an excavation motion factor; 、 And Respectively representing covariance matrixes corresponding to the gyration motion factor, the walking motion factor and the mining motion factor at the moment k; ; Wherein, the And Indicating the cab position and the bucket position, respectively At the moment in time under e Direction and direction The position of the direction is determined by the position of the orientation, And When the cab position and the bucket position are at time k+1, the system e is Direction and direction The position of the direction; Representing the change in yaw angle measured by the IMU from time k to time k +1 during the revolution, Representing changes in heading angle observed through dual antenna cooperative orientation; ; Wherein, the And The positions of the cab position and the bucket position in the vertical direction under the e-series at the time k and the time k+1 are shown, Representing the vertical velocity expected during walking; Representing the conversion of the quaternion into a lie algebraic vector, Representing an expected amount of change in attitude; And The posture of the b system relative to the e system at the k time and the k+1 time are respectively shown; ; Wherein, the 、 And Three-dimensional coordinates of three characteristic points of the bucket tip, the left side and the right side under the e system are respectively represented; And Respectively indicating that the bucket tip is under the e series Direction and direction The position of the direction; And Respectively show the left side of the bucket under the e series Direction and direction The position of the direction; And Respectively represent the right side of the bucket under the e series Direction and direction The position of the direction; Representing the vertical height of the bucket tip under the e-line; Representing a terrain elevation query function; representing the normal vector to the bottom surface of the bucket By rotating the matrix From the slave Transition to Tying; Representing the normal vector of the topographical surface at the point of contact.
10. The factor graph optimization-based strip mine electric shovel high-precision pose positioning system according to claim 1, wherein the pose determining module comprises: The global optimization unit is used for constructing a global optimization objective function of the factor graph optimization model, and solving the global optimization objective function through the maximum posterior probability to obtain the optimal state estimation of the global state vector to be estimated; and the marginalization unit is used for converting the optimal state estimation at the oldest moment in the sliding window and the corresponding observation constraint into prior information after the global optimization is completed, and adding the prior information into the global optimization objective function at the next moment.

Description

Factor graph optimization-based high-precision pose positioning system for electric shovel of strip mine Technical Field The invention relates to the technical field of electric shovel positioning, in particular to a factor graph optimization-based high-precision pose positioning system for an electric shovel of a strip mine. Background In a strip mine operation scene, pose determination of an electric shovel is important to operation efficiency and safety. The traditional positioning method based on a single GNSS (global navigation satellite system) or an IMU (inertial measurement unit) needs to initialize alignment in a dynamic environment, but has long initialization time, and the drift of a gyroscope causes the divergence of a course angle in a static state, so that the positioning precision is reduced or even fails, and the IMU has large measurement noise and nonlinear amplification of measurement errors in a strong vibration and strong impact environment. Patent CN118311629B proposes an indoor and outdoor seamless positioning method based on UWB/GNSS/IMU, and accurate positioning under different indoor and outdoor environments is realized by loose coupling of GNSS and IMU and combining an extended Kalman filtering algorithm. The patent CN120491132a proposes a CTG neural network assisted navigation method and device under a GNSS weak signal scene, synchronously acquiring IMU original data and GNSS satellite positioning data of a mobile carrier under a GNSS signal stable scene, constructing a space-time feature dataset containing dynamic acceleration, angular velocity and position increment, then constructing a CTG network, performing offline training on the CTG network by using the space-time feature dataset to obtain an optimal network weight and model structure, and obtaining a smooth positioning result by a tight coupling algorithm. However, the method is realized in a vehicle-mounted environment with small vibration, and for strip mine electric shovel equipment in a complex operation environment, the pose of the electric shovel body and the pose of the bucket are difficult to solve in real time due to the influence of strong vibration and strong impact in the operation process. Therefore, for the complex open-pit mining operation environment, a solution which can achieve high-precision positioning, multi-component attitude synchronous measurement, strong interference resistance and reasonable cost is needed. Disclosure of Invention In order to solve the technical problems, the invention provides a factor graph optimization-based high-precision pose positioning system for an electric shovel of a strip mine. The technical scheme of the invention is as follows: a factor graph optimization-based high-precision pose positioning system of an electric shovel of an open pit mine comprises the following components: The data acquisition module is used for acquiring an original GNSS observation sequence and an original IMU observation sequence which are acquired by an antenna and an IMU which are arranged on the electric shovel in advance, and carrying out space-time alignment on the original GNSS observation sequence and the original IMU observation sequence to obtain a GNSS observation sequence to be corrected and an IMU observation sequence to be denoised; The denoising module is used for denoising the IMU observation sequence to be denoised by adopting a pre-trained denoising diffusion model to obtain a denoised IMU observation sequence; the working condition identification module is used for identifying the current working condition of the electric shovel according to the GNSS observation sequence to be corrected and the denoised IMU observation sequence; The GNSS data processing module is used for correcting the GNSS observation sequence to be corrected according to the current working condition of the electric shovel to obtain a corrected GNSS observation sequence; The model construction module is used for constructing a multi-source fusion factor graph optimization model according to the denoised IMU observation sequence and the corrected GNSS observation sequence; And the pose determining module is used for solving the factor graph optimization model to obtain the pose of the electric shovel. Preferably, the working condition identification module includes: The statistical characteristic calculation unit is used for calculating the horizontal speed, the course angle change rate and the vertical acceleration variance of the current moment of the electric shovel according to the denoised IMU observation sequence; The walking working condition judging unit is used for judging that the current working condition of the electric shovel is a walking working condition when the horizontal speed of the current moment of the electric shovel is larger than a preset horizontal speed threshold value, the course angle change rate of the current moment is smaller than a preset course angle change rate threshold valu