A vehicle positioning method and system based on UWB and vehicle constraints

Abstract

This application discloses a vehicle positioning method and system based on UWB and vehicle constraints, which relates to the field of vehicle positioning, including: deploying UWB base stations in an underground mining environment and installing UWB tags on vehicles; obtaining ranging data from at least two UWB base stations, detecting and compensating the ranging data to obtain the vehicle's UWB position; collecting on-board IMU data and obtaining the vehicle's attitude quaternion through attitude solution; collecting vehicle wheel speedometer data and correcting the vehicle wheel speedometer data according to vehicle motion constraints; and obtaining the vehicle's position through an adaptive Kalman filter fusion algorithm based on the vehicle's UWB position, vehicle attitude quaternion, and the corrected vehicle wheel speedometer data. In view of the fact that traditional inertial navigation systems in closed spaces in underground mining are prone to accumulated errors and drift, resulting in low positioning accuracy, this application reduces the impact of multipath effects on positioning accuracy in traditional inertial navigation systems.

Description

Technical Field

[0001] The present application relates to the field of vehicle positioning, and in particular to a vehicle positioning method and system based on UWB and vehicle constraints. Background Art

[0002] With the continuous advancement of intelligent and automated mining, autonomous driving technology for underground vehicles has become a key technology for improving mine production efficiency and safety. In underground mining environments, a high-precision, reliable vehicle positioning system is a core prerequisite for autonomous driving, directly impacting the effective implementation of vehicle navigation, path planning, and scheduling management. Accurate vehicle positioning not only improves production efficiency but also effectively reduces the occurrence of underground safety accidents, which is of great significance to the sustainable development of mining enterprises.

[0003] The underground environment is distinct from the surface environment: confined and narrow spaces, complex tunnel structures, high humidity, high dust levels, and significant electromagnetic interference. These characteristics make common surface positioning technologies difficult to apply underground, necessitating adaptive technological improvements and innovations tailored to the specific underground environment.

[0004] At present, the following technical solutions are mainly used for underground vehicle positioning:

[0005] SLAM positioning technology based on point cloud maps: Laser SLAM (Simultaneous Localization and Mapping) technology is typically used to create a 3D point cloud map of the mine's underground, and then repositioning is performed based on this map. This method often integrates information from inertial sensors such as IMUs to provide stable position and attitude output, supporting subsequent autonomous vehicle scheduling and path planning.

[0006] UWB-based positioning technology: A UWB base station network is pre-deployed in the underground environment. Vehicles or personnel carry UWB tags. The cloud platform receives UWB ranging observations, calculates the tag position, and sends the location information to the user end to achieve real-time positioning function.

[0007] However, these existing technologies have obvious shortcomings in the complex environment of underground mines:

[0008] Underground mines typically have long tunnels with monotonous structures, sparse features, and high repetitiveness. This makes feature-based point cloud matching technologies (such as laser SLAM and visual SLAM) prone to matching failures in these environments, leading to false positives and false positives. In these situations, the inter-frame matching and loop detection mechanisms that traditional SLAM algorithms rely on fail, and accumulated errors gradually amplify, ultimately leading to map drift or complete positioning loss, making it impossible to support continuous and reliable autonomous driving operations.

[0009] Satellite signal shielding and IMU integration errors: In underground tunnel environments, GPS and other satellite navigation signals are completely ineffective, unable to provide baseline positioning information. In these situations, systems often rely on IMUs (Inertial Measurement Units) for dead reckoning. However, IMUs can incur significant integration errors over extended periods and distances. Low-precision MEMS-IMU dead reckoning errors can reach up to 15% of the mileage per hour. While high-precision IMUs can reduce these errors, their high cost makes them unsuitable for large-scale project rollout and product replication.

[0010] UWB positioning has limited environmental adaptability: Although UWB technology offers high distance measurement accuracy, in the complex underground environment, the signal is susceptible to multipath effects, resulting in increased ranging errors. Furthermore, underground metal equipment and support structures can significantly interfere with UWB signals, affecting positioning accuracy and stability.

[0011] Limitations of traditional fusion algorithms: Existing sensor fusion algorithms (such as traditional Kalman filtering) generally do not consider vehicle-specific kinematic constraints, and the weights of multiple sensors are fixed. They cannot dynamically adjust the credibility of each sensor according to environmental changes, resulting in insufficient adaptability in complex and changing underground environments and an inability to fully utilize the complementary advantages of multi-source sensors. Summary of the Invention

[0012] In view of the fact that traditional inertial navigation systems in closed spaces of mines and underground factories in the existing technology are prone to accumulated errors and drift, resulting in low positioning accuracy, the present application provides a vehicle positioning method and system based on UWB and vehicle constraints. By deploying a UWB base station network and multiple sensors, and utilizing the unique motion constraints of vehicles in mine environments to perform positioning corrections, the impact of multipath effects on positioning accuracy in traditional inertial navigation systems is reduced.

[0013] One aspect of the present application provides a vehicle positioning method based on UWB and vehicle constraints, which is used in an underground mining environment, including: deploying UWB base stations in the underground mining environment and installing UWB tags on vehicles; acquiring ranging data from at least two UWB base stations, detecting and compensating the ranging data to obtain the UWB position of the vehicle; collecting on-board IMU data, and obtaining the vehicle attitude quaternion through attitude solution; collecting vehicle wheel speedometer data, and correcting the vehicle wheel speedometer data according to vehicle motion constraints; and obtaining the vehicle position through an adaptive Kalman filter fusion algorithm based on the vehicle UWB position, the vehicle attitude quaternion, and the corrected vehicle wheel speedometer data.

[0014] Furthermore, UWB reference stations are deployed in the underground mining environment, and UWB tags are installed on vehicles, including: deploying a UWB reference station every 100 meters in the underground mining tunnel and installing a UWB reference station at the tunnel bend; and performing time and space calibration on the deployed UWB reference stations.

[0015] Furthermore, ranging data from at least two UWB reference stations is obtained, and the ranging data is detected and compensated to obtain the UWB position of the vehicle, including: detecting whether the UWB signal strength of the ranging data is greater than a preset threshold; performing multipath effect compensation on the ranging data that passes the detection, including: using a channel impulse response analysis method to perform time domain decomposition on the UWB received signal, identifying the time delay characteristics of the direct signal and the multipath reflected signal, and generating a channel characteristic diagram containing the time delay and amplitude of each signal path; based on the generated channel characteristic diagram, using a threshold-adaptive first-path detection algorithm to process the signal, by setting a dynamic noise threshold and searching for the first signal peak that exceeds the threshold, extracting the earliest arriving signal component representing the direct path and its precise arrival time; using the obtained direct signal arrival time and the identified multipath reflection signal characteristics, using a multipath compensation model based on the relationship between signal strength and time delay, calculating and eliminating the ranging deviation introduced by the reflected signal, and applying the correction value to the original ranging data to obtain a compensated UWB ranging value, thereby achieving the purpose of eliminating the influence of the multipath effect. According to the compensated ranging data, the UWB tag position is calculated using trilateration or least squares method as the vehicle UWB position.

[0016] Furthermore, the vehicle-mounted IMU data is collected and the vehicle attitude quaternion is obtained through attitude solution, including: obtaining the three-axis acceleration and three-axis angular velocity of the IMU data; performing zero-bias calibration and scale correction on the three-axis acceleration and three-axis angular velocity to obtain the corrected three-axis acceleration and three-axis angular velocity; numerically integrating the quaternion differential equation based on the corrected three-axis angular velocity to obtain the initial vehicle attitude quaternion; using the corrected three-axis acceleration to perform gravity vector constraint correction on the initial vehicle attitude quaternion to obtain the final vehicle attitude quaternion. Obtain the three-axis acceleration and three-axis angular velocity of the IMU data; perform zero-bias calibration and scale correction on the three-axis acceleration and three-axis angular velocity to obtain the corrected three-axis acceleration and three-axis angular velocity. Scale correction refers to converting the original output signal of the sensor into a standard physical unit through a calibrated proportional coefficient, so that the acceleration data unit is m / s² and the angular velocity data unit is rad / s.

[0017] Based on the corrected three-axis angular velocity, numerical integration is performed through the quaternion differential equation: ,in, is a quaternion, is the angular velocity vector, Represents quaternion multiplication to obtain the initial vehicle posture quaternion;

[0018] The corrected three-axis acceleration is used to construct the gravity observation vector. The error between the calculated and theoretical gravity vector is then corrected using a proportional-integral feedback correction mechanism to constrain the initial vehicle attitude quaternion to suppress integral drift and obtain the final vehicle attitude quaternion.

[0019] Furthermore, the vehicle wheel speedometer data is collected and corrected according to the vehicle motion constraint conditions, including: establishing a vehicle motion model based on the rear wheel speed of the vehicle perpendicular to the direction of travel being less than a threshold in an underground mining environment: ,in, represents the speed of the vehicle's rear wheels in the navigation coordinate system n, Represents the speed of the vehicle's rear wheels in the vehicle coordinate system V; Represents the rotation matrix from the navigation coordinate system to the vehicle coordinate system.

[0020] Perform zero bias compensation on vehicle wheel speedometer data: ,in It represents the rotation matrix from the navigation coordinate system to the vehicle body coordinate system after zero bias compensation; n represents the navigation coordinate system (Navigation frame), which is usually a local horizontal coordinate system, and b represents the body coordinate system (Body frame), which is usually fixed to the vehicle IMU sensor. Represents the original rotation matrix from the navigation coordinate system to the vehicle coordinate system; Represents a small rotation matrix in the navigation coordinate system, used for zero bias compensation; I represents a 3×3 unit matrix; represents the zero bias error vector, which contains small rotation angles in three directions; × represents the antisymmetric matrix operator of the vector, which is used to convert the rotation vector into a rotation matrix.

[0021] Perform installation angle compensation on vehicle wheel speedometer data: ,in, Indicates the error or increment sign, represents the rotation matrix from the earth coordinate system to the vehicle body coordinate system, represents the speed measured in the vehicle body coordinate system, Indicates the speed in the Earth coordinate system, v represents the speed, and e represents the Earth coordinate system (Earthframe), usually the ECEF coordinate system. express Antisymmetric matrix form of a vector; represents the attitude error vector.

[0022] Furthermore, the rotation matrix from the navigation coordinate system to the vehicle coordinate system is , the expression is as follows: ,in, Indicates the roll angle (roll); Indicates the pitch angle (pitch); Indicates the yaw angle (yaw).

[0023] Further, the vehicle position is obtained, including: initializing the state vector and covariance matrix ; Among them, the state vector Including vehicle position, speed and attitude; obtaining vehicle UWB position, vehicle attitude quaternion, and corrected vehicle wheel speedometer data; performing state prediction based on vehicle attitude quaternion and currently collected IMU data, as well as corrected vehicle wheel speedometer data; performing observation update and calculating measurement residuals based on vehicle UWB position; adjusting the weights of each sensor based on the measurement residuals; updating the state vector and covariance matrix based on the adjusted weights to obtain an updated state vector; extracting the vehicle position from the updated state vector.

[0024] Furthermore, the measurement residual is calculated, including: predicting the attitude quaternion at the next moment; predicting the position and speed of the vehicle based on the predicted attitude quaternion and the currently collected IMU acceleration data; using the corrected vehicle wheel speedometer data, constraining the predicted vehicle speed to constrain the speed of the vehicle's rear wheels perpendicular to the direction of travel to zero; calculating the state transfer matrix F and the process noise covariance matrix Q based on the vehicle motion model; constructing the observation equation based on the vehicle UWB position data, the observation equation includes the observation matrix H and the measurement noise covariance matrix R; and calculating the Kalman gain based on the observation equation. ; Calculate the measurement residual as , where Z is the vehicle UWB position observation value and X is the predicted state vector; the state vector is updated using the Kalman gain K and the measurement residual and covariance matrix .

[0025] Furthermore, the weight of each sensor is adjusted according to the measurement residual, using the following formula: ,in, is the weight of the i-th sensor, is the measurement noise variance of sensor i, which is updated in real time through the measurement residual estimation; when the UWB signal strength is less than the threshold, the UWB corresponding value.

[0026] Another aspect of the present application further provides a vehicle positioning system based on UWB and vehicle constraints, which is used to execute a vehicle positioning method based on UWB and vehicle constraints of the present application.

[0027] Compared with the existing technology, the advantages of this application are:

[0028] In underground mining environments, positioning is difficult due to problems such as closed and narrow space, complex tunnel structure and single features, and severe electromagnetic interference. Existing technologies generally use laser SLAM or a single UWB positioning system, but there are defects such as feature matching failure, multipath effect interference, and inertial navigation cumulative error. This application first performs signal strength detection and multipath effect compensation on UWB ranging data, effectively reducing the ranging error caused by multipath propagation in complex underground environments. Then, by using IMU data calibration and quaternion attitude solution, combined with gravity vector constraint correction, the system effectively suppresses the cumulative error of traditional inertial navigation systems. Finally, based on the characteristic that the speed of the rear wheels of vehicles perpendicular to the direction of travel is almost zero in underground mining environments, a vehicle motion model and zero bias compensation mechanism are established to make the wheel speed meter data more accurate and reliable.

[0029] By fusing these three sensor data using an adaptive Kalman filter algorithm, the system dynamically adjusts the weight of each sensor, automatically reducing the impact of UWB signal degradation and maintaining system positioning accuracy. This multi-sensor fusion approach not only improves the system's adaptability in complex environments but also overcomes limitations that traditional single-positioning technologies cannot overcome. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 is an exemplary flow chart of a vehicle positioning method based on UWB and vehicle constraints according to some embodiments of the present application;

[0031] Figure 2 is a schematic diagram of UWB base station deployment according to some embodiments of the present application;

[0032] Figure 3 This is a schematic diagram of UWB multipath effects in an underground mining environment according to some embodiments of the present application;

[0033] Figure 4 This is a diagram showing the effect of channel impulse response analysis according to some embodiments of the present application. DETAILED DESCRIPTION

[0034] The method and system provided in the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0035] like Figure 1As shown, UWB base stations are deployed in an underground mining environment, and UWB tags are installed on vehicles; ranging data from at least two UWB base stations are acquired, and the ranging data are detected and compensated to obtain the vehicle's UWB position; on-board IMU data is collected, and the vehicle attitude quaternion is obtained through attitude solution; vehicle wheel speedometer data is collected, and the vehicle wheel speedometer data is corrected according to the vehicle motion constraints; based on the vehicle UWB position, vehicle attitude quaternion, and the corrected vehicle wheel speedometer data, the vehicle position is obtained through an adaptive Kalman filter fusion algorithm.

[0036] like Figure 2 As shown, UWB reference stations are deployed every 100 meters in the laneway, with additional stations installed at lane bends to test UWB signal strength. If UWB signals are detected to be subject to multipath or significant interference, relocation is necessary. Due to signal attenuation, spacing should not exceed 200 meters. When a UWB tag receives UWB ranging observations, it automatically calculates its own position, requiring simultaneous reception of observations from two base stations. To facilitate subsequent fusion and calculation, the coordinates of the UWB reference stations must be calibrated to a unified map coordinate system, using point cloud map coordinates as the reference system. UWB devices also require pre-calibration in time and space to ensure accurate ranging observations when used in different environments.

[0037] In underground mining environments, UWB signals can be reflected, blocked, and attenuated by objects such as tunnel walls, equipment, and supports, resulting in unstable signal quality. This embodiment tests the signal strength of ranging data by setting a signal strength threshold, typically between -80dBm and -75dBm. The received signal strength indicator (RSSI) of each UWB reference station signal is acquired in real time. Signal data below the threshold is automatically excluded, ensuring that only high-quality ranging data is used in subsequent calculations. When multiple reference stations are available, data from reference stations with higher signal strength is prioritized. This mechanism ensures that the UWB positioning system only uses high-quality signal data, improving the reliability of position solutions.

[0038] In addition, if Figure 3 As shown, the confined space and presence of numerous metal devices in underground mining environments lead to significant multipath effects on UWB signals, causing signals to reach the receiver via multiple paths, resulting in ranging errors. This implementation utilizes the following compensation mechanisms: Channel Impulse Response (CIR) analysis: This identifies and extracts the earliest arriving signal (the direct wave); a dynamic environment model is established for the ranging data of each base station, and a multipath effect compensation model is constructed based on the relationship between historical ranging and spatial position. The compensation formula is: ;in, is the estimated value of the multipath effect, which can be obtained by training from historical data using machine learning methods.

[0039] Based on the compensated high-quality ranging data, the following method is used to calculate the vehicle UWB position: When the number of available reference stations is 3, the trilateration method is used: Establish a nonlinear equation group: ; Among them, (x, y, z) is the label position to be found, is the reference station position, is the distance value after compensation; solve the equations by analytical or iterative method. When the number of available base stations is greater than 3, use the weighted least squares method: Construct the error function ; Weight It is proportional to the signal strength. The stronger the signal, the greater the weight. The optimal position is solved using optimization methods such as the Levenberg-Marquardt algorithm. Figure 4 As shown, this application effectively reduces the ranging error caused by the multipath effect and improves the original positioning accuracy through CIR analysis and multipath model compensation.

[0040] Collect vehicle-mounted IMU data and obtain vehicle attitude quaternion through attitude solution. First, the vehicle-mounted IMU sensor usually collects three-axis acceleration and the three-axis angular velocity Data: The sampling frequency is set to 100 Hz to 200 Hz to ensure that rapid vehicle movement is captured; a digital low-pass filter (such as a Butterworth filter) is used to pre-process the raw data to filter out high-frequency noise.

[0041] IMU sensors have inherent zero bias and scale errors and require precise calibration. On the one hand, when the vehicle is stationary, the angular velocity zero bias is identified by using the characteristic that the angular velocity is theoretically zero under static conditions. The calibration formula is: ,in, Represents the raw angular velocity measurement vector (rad / s) obtained directly from the IMU sensor; Represents the angular velocity zero bias vector, which represents the non-zero value (rad / s) output by the sensor when it is stationary; represents the calibrated angular velocity vector, with the zero bias removed and scale factor correction applied (rad / s); the acceleration zero bias is determined by the constraint that the gravitational acceleration modulus is g. On the other hand, the scale factor matrix K is established to correct the proportional relationship between the sensor output and the actual physical quantity. The correction formula is: , the same goes for angular velocity: , calibration parameters can be obtained by Allan variance analysis and multi-posture calibration method, and compensated at different temperatures; Represents the angular velocity scale factor matrix (3×3), which is used to correct the proportional relationship between the sensor output and the actual physical quantity, including inter-axis coupling correction; Represents the raw acceleration measurement vector (m / s²) obtained directly from the IMU sensor; Represents the acceleration bias vector, which represents the non-gravitational acceleration output by the sensor when it is at rest (m / s²); represents the calibrated acceleration vector with the bias removed and scale factor correction applied (m / s²); Represents the acceleration scale factor matrix (3×3), which is used to correct the proportional relationship between the sensor output and the actual physical quantity, including inter-axis coupling correction.

[0042] The quaternion representation avoids the universal lock problem of Euler angles and improves the stability of attitude solution: Quaternion differential equation: , where q is the attitude quaternion, is quaternion multiplication; Represents the time derivative of the attitude quaternion; Represents the three-axis angular velocity components measured in the vehicle coordinate system, corresponding to the angular velocity of the vehicle in the roll (x-axis), pitch (y-axis) and yaw (z-axis) directions;

[0043] When the vehicle is static or moving at a constant speed, the accelerometer measurement value is mainly gravity acceleration. Therefore, to suppress integral drift, this application introduces a gravity vector constraint to calculate the projection of the theoretical gravity vector in the vehicle coordinate system under the current posture quaternion: , compare the difference between the measured acceleration and the theoretical gravity vector, and construct a complementary filter to achieve correction: , K is the correction gain, which is adaptively adjusted according to the dynamic characteristics of the vehicle, where Represents the gravitational acceleration vector in the vehicle coordinate system; Represents the inverse of the quaternion q. For the unit quaternion, . Represents the three-axis acceleration vector measured by the IMU in the vehicle coordinate system; Indicates the modulus of the measured acceleration vector; Represents the modulus of the gravity vector in the vehicle coordinate system; Represents the intermediate attitude quaternion result obtained by numerical integration of the quaternion differential equation; Represents the final attitude quaternion after gravity vector constraint correction.

[0044] This application introduces a gravity vector constraint, providing an external reference and effectively suppressing long-term attitude drift. Furthermore, the use of quaternions avoids the gimbal lock problem in Euler angle representation, ensuring the continuity and stability of attitude calculations in any posture.

[0045] In underground mining environments, vehicles usually travel at low speeds (usually <20 km / h) and operate in well-designed tunnels. In this specific environment, the vehicle has the following kinematic characteristics: the speed of the vehicle's rear wheels perpendicular to the direction of travel is almost zero (non-holonomic constraint); the wheels almost never jump or slide sideways on flat roads; the vehicle moves mainly along the tunnel axis, and the steering angle changes relatively slowly. This kinematic characteristic provides important constraints: when the vehicle is driving normally, the lateral velocity component of the rear wheels in the vehicle coordinate system V is should be close to zero, i.e. (ε is the preset threshold, with a typical value of 0.05m / s).

[0046] Based on the above constraints, the vehicle motion model is established: ,in: : represents the velocity vector of the vehicle’s rear wheel in the navigation coordinate system (n), Represents the velocity component of the vehicle in the east direction in the navigation coordinate system; Represents the velocity component of the vehicle in the north direction in the navigation coordinate system; Represents the vertical upward velocity component of the vehicle in the navigation coordinate system; : represents the velocity vector of the rear wheel of the vehicle in the vehicle coordinate system (V), Represents the lateral velocity component of the vehicle in the vehicle coordinate system; It represents the longitudinal velocity component of the vehicle in the vehicle coordinate system; Represents the vertical velocity component of the vehicle in the vehicle coordinate system; Represents the rotation matrix from the navigation coordinate system to the vehicle coordinate system; key constraints: Among them, this application utilizes the nonholonomic constraint characteristics of the vehicle (lateral velocity ≈ 0) to provide additional observation information, significantly reducing the velocity and position cumulative errors of the inertial navigation system during long-term operation.

[0047] Rotation Matrix By Euler angles Construct, the expression is as follows: ,in, Represents the vehicle's roll angle (roll), which is the rotation around the x-axis; Represents the vehicle's pitch angle, which is the rotation around the y-axis; Represents the vehicle's yaw angle (yaw), a rotation around the z-axis; these Euler angles are calculated from the quaternion attitude using the standard quaternion to Euler angle conversion formula.

[0048] Due to installation errors, the vehicle coordinate system and the sensor coordinate system are not completely aligned, and zero offset compensation is required: ,in: Represents the rotation matrix from the navigation coordinate system to the vehicle body coordinate system after zero bias compensation; Represents the original rotation matrix from the navigation coordinate system to the vehicle coordinate system; Represents a small rotation matrix in the navigation coordinate system, used for zero bias compensation; n represents the navigation frame, which is usually defined as a local horizontal coordinate system (east-north-up); b represents the body frame, the coordinate system fixed to the vehicle IMU sensor; I: represents the 3×3 unit matrix; : represents the zero bias error vector, which contains small rotation angles in three directions; express The antisymmetric matrix is: , the bias error vector It is obtained through an initial calibration process by measuring the difference between the direction of gravity and the theoretical direction of gravity when the vehicle is stationary; Indicates a small rotation angle around the x-axis (roll direction error); Indicates a small rotation angle around the y-axis (pitch direction error); Indicates a small rotation angle around the z-axis (yaw error).

[0049] To further improve the accuracy, the wheel speed meter data is compensated for the installation angle: ,in: Velocity error in the vehicle body coordinate system; Velocity error in the Earth coordinate system; The rotation matrix from the earth coordinate system to the vehicle body coordinate system; express The antisymmetric matrix form of the vector; ζ represents the attitude error vector; e represents the Earth frame; installation angle compensation mainly addresses the deviation problem between the sensor installation position and the ideal reference point of the vehicle, especially the speed error caused by the lever arm effect when turning.

[0050] The zero-bias compensation and installation angle compensation mechanisms of this application address system errors caused by sensor installation deviations, reducing the system's requirements for installation accuracy. Furthermore, installation angle compensation, which takes into account the lever-arm effect, enables the vehicle to maintain high-precision positioning during cornering. Furthermore, compared to complex point cloud matching or visual feature extraction, wheel speed correction based on motion constraints requires less computation and offers better real-time performance, enabling efficient operation in low-computing resource environments.

[0051] The adaptive Kalman filter algorithm is a recursive estimation method that optimally fuses measurement data from different sensors. Its key difference from traditional Kalman filtering lies in its ability to dynamically adjust the weights of each sensor based on real-time measurement residuals, allowing the system to adapt to environmental changes and sensor performance fluctuations. This application, based on a state-space model, implements a unified fusion framework for IMU data, UWB position, and wheel speedometer constraints. Specifically, traditional filters require strict synchronization of all sensor data. This application solves the problem of asynchronous data fusion between UWB (low frequency), IMU (high frequency), and wheel speedometer (medium frequency) through a prediction-correction framework and timestamp alignment.

[0052] The state vector of this application is designed as follows: ;in: : Indicates the three-dimensional position of the vehicle in the navigation coordinate system, in meters; : Indicates the three-dimensional speed of the vehicle in the navigation coordinate system, in meters per second; : Represents the attitude quaternion from the vehicle coordinate system to the navigation coordinate system.

[0053] Initialization process: Position The initial value adopts the first valid UWB position; speed The initial value is set to zero vector; attitude The initial value is determined by the direction of the gravity vector during static acquisition by the IMU; the initial covariance matrix P is set to a diagonal matrix, and the diagonal elements correspond to the initial uncertainty of each state quantity.

[0054] State prediction is based on IMU sensor data and nonlinear system model. First, attitude prediction: the vehicle attitude quaternion calculated in step S3 is used as the initial attitude at the current moment, combined with the current IMU angular velocity , predict the posture at the next moment by numerical integration of quaternion differential equations: The actual calculation uses the fourth-order Runge-Kutta integration method to improve the accuracy.

[0055] Then perform position and velocity prediction using IMU acceleration data And the predicted attitude quaternion, update the vehicle speed and position: ; ;in: Represents the rotation matrix from the vehicle coordinate system to the navigation coordinate system corresponding to the attitude quaternion; Represents the gravity acceleration vector in the navigation coordinate system, usually ; Δt represents the sampling time interval; Represents the predicted vehicle speed vector in the navigation coordinate system at time k+1 (m / s); represents the vehicle velocity vector in the navigation coordinate system at time k (m / s); represents the predicted vehicle position vector (m) in the navigation coordinate system at time k+1; represents the vehicle position vector (m) in the navigation coordinate system at time k; Represents the acceleration component in the x-axis direction of the vehicle coordinate system; Represents the acceleration component in the y-axis direction of the vehicle coordinate system; Represents the acceleration component along the z-axis of the vehicle coordinate system.

[0056] Applying the wheel speed constraint, the corrected wheel speed meter data in S4 is used as the vehicle longitudinal velocity measurement, and the predicted velocity is corrected using the nonholonomic motion constraint (rear wheel lateral velocity ≈ 0): ; (constraints); (wheel speed meter measurement); The state transfer matrix F is calculated using the Jacobian matrix of the nonlinear system model; the process noise covariance matrix Q represents the IMU measurement noise and modeling error and is constructed as a diagonal matrix.

[0057] Construct the observation equation, the UWB position observation equation is: ,in: is the UWB observation position vector; is the UWB measurement matrix, which is as follows ( is a 3×3 identity matrix); Measuring noise for UWB.

[0058] The nonholonomic constraint observation equation of the wheel speed meter is: ,in Extract the lateral velocity component in the vehicle coordinate system; represents the nonholonomic constrained observation value of the wheel speed meter; Indicates the wheel speed meter measurement noise.

[0059] Calculate the Kalman gain, , where: P is the state prediction covariance matrix; H is the observation matrix; R is the measurement noise covariance matrix.

[0060] Calculate the measurement residuals, , where Z is the observed value (UWB position or wheel speed constraint) and X is the predicted state vector. Update the state and covariance, , Adjust the weight of each sensor in real time according to the measurement residual: Based on the measurement residual y and the innovation covariance , estimate the actual measurement noise: , where n is the sliding window length (typical values ​​are 10 to 20).

[0061] Weight calculation, ,in: is the weight of the i-th sensor; is the estimated measurement noise variance of sensor i; represents the measurement noise variance of sensor j; is the sum of the inverse of the noise variance of all sensors.

[0062] UWB signal quality adaptive mechanism: If UWB_signal_strength < threshold, then , where: threshold is the UWB signal strength threshold, with a typical value of -75dBm; α is the amplification factor, which controls the sensitivity of weight adjustment; is the UWB observation noise covariance matrix.

[0063] The updated state vector directly contains the vehicle position information: , i.e. the first three components of the state vector, represent the three-dimensional position in the navigation coordinate system.

[0064] Among them, this application, on the one hand, uses an adaptive mechanism driven by measurement residuals to enable the system to automatically identify and reduce the weight of abnormal sensor data. For example, it automatically increases the weight of the IMU and wheel speed meter in UWB occlusion areas, and correspondingly increases the UWB weight in areas with good UWB signals, significantly enhancing the robustness of the system. On the other hand, traditional fusion algorithms have fixed parameters and cannot adapt to the environmental characteristics of different areas underground (spacious stopes, narrow tunnels, turning areas, etc.). This application adjusts parameters in real time based on measurement residuals, allowing the system to adaptively adapt to different underground environments. In addition, electromagnetic interference in the underground environment can cause abnormal IMU data or fluctuations in the UWB signal. The adaptive mechanism of this application can detect sudden anomalies and reduce their impact through weight adjustment.

[0065] In summary, this application effectively suppresses cumulative errors through vehicle motion constraints and adaptive fusion mechanisms, solving the problem of decreased positioning accuracy of vehicles running for a long time in underground mining environments. This technical solution first reduces the ranging error in complex underground environments based on UWB signal strength detection and multipath effect compensation; secondly, through IMU quaternion attitude solution and gravity vector constraint correction, it suppresses the cumulative error of the inertial navigation system; thirdly, by utilizing the characteristic that the speed of the rear wheels of vehicles perpendicular to the direction of travel is close to zero in underground mining environments, a vehicle motion model and zero bias compensation mechanism are constructed, thereby improving the reliability of wheel speed meter data; finally, an adaptive Kalman filter algorithm is used to realize multi-sensor data fusion, dynamically adjust the weight of each sensor, and automatically reduce its impact when the UWB signal quality decreases.

[0066] The above schematically describes the invention and its implementation methods, which are not restrictive. Without departing from the spirit or basic characteristics of the invention, the invention can be implemented in other specific forms. What is shown in the accompanying drawings is only one of the implementation methods of the invention, and the actual structure is not limited to this. Therefore, if a person of ordinary skill in the art is inspired by it and, without departing from the purpose of the invention, designs a structural method and embodiment similar to the technical solution without creativity, they should all fall within the scope of protection of this application. In addition, the word "including" does not exclude other elements or steps, and the word "one" before an element does not exclude the inclusion of "multiple" elements. Words such as first and second are used to indicate names and do not indicate any specific order.

Claims

1. A vehicle positioning method based on UWB and vehicle constraints, used in underground mining environments, characterized in that: include: Deploy UWB base stations in underground mining environments and install UWB tags on vehicles; Obtain ranging data from at least two UWB reference stations, detect and compensate the ranging data, and obtain the vehicle's UWB position; Collect vehicle-mounted IMU data and obtain vehicle attitude quaternion through attitude calculation; Collect vehicle wheel speed meter data and correct the vehicle wheel speed meter data according to vehicle motion constraints; The vehicle position is obtained based on the vehicle UWB position, vehicle attitude quaternion, and corrected vehicle wheel speed meter data through the adaptive Kalman filter fusion algorithm; Corrected vehicle wheel speedometer data, including: Based on the fact that the rear wheel speed of the vehicle perpendicular to the direction of travel in an underground mining environment is less than a threshold, a vehicle motion model is established: ,in, represents the speed of the vehicle's rear wheels in the navigation coordinate system n, Represents the speed of the vehicle's rear wheels in the vehicle coordinate system V; Represents the rotation matrix from the navigation coordinate system to the vehicle coordinate system; Perform zero bias compensation on vehicle wheel speedometer data: ,in, It represents the rotation matrix from the navigation coordinate system to the vehicle body coordinate system after zero bias compensation; n represents the navigation coordinate system, b represents the vehicle body coordinate system, Represents the original rotation matrix from the navigation coordinate system to the vehicle coordinate system; Represents a small rotation matrix in the navigation coordinate system; I represents a 3×3 unit matrix; represents the zero bias error vector; × represents the antisymmetric matrix operator of the vector; Perform installation angle compensation on vehicle wheel speedometer data: ,in, Indicates the error or increment sign, represents the rotation matrix from the earth coordinate system to the vehicle body coordinate system, represents the speed measured in the vehicle body coordinate system, represents the speed in the earth coordinate system, v represents the speed, e represents the earth coordinate system, express Antisymmetric matrix form of a vector; represents the attitude error vector.

2. The vehicle positioning method based on UWB and vehicle constraints according to claim 1, characterized in that: Deploy UWB base stations in underground mining environments and install UWB tags on vehicles, including: Deploy a UWB reference station every 100 meters in the underground mine tunnel, and install a UWB reference station at the tunnel bend; Perform time and space calibration on the deployed UWB reference stations.

3. The vehicle positioning method based on UWB and vehicle constraints according to claim 2, characterized in that: Get the vehicle's UWB position, including: Detect whether the UWB signal strength of the ranging data is greater than a preset threshold; Perform multipath effect compensation on the ranging data that passes the detection; According to the compensated ranging data, the UWB tag position is calculated using trilateration or least squares method as the vehicle UWB position.

4. The vehicle positioning method based on UWB and vehicle constraints according to claim 2, characterized in that: The vehicle attitude quaternion is obtained through attitude calculation, including: Get the three-axis acceleration and three-axis angular velocity of IMU data; Perform zero bias calibration and scale correction on the three-axis acceleration and three-axis angular velocity to obtain the corrected three-axis acceleration and three-axis angular velocity; According to the corrected three-axis angular velocity, the initial vehicle posture quaternion is obtained by numerical integration through the quaternion differential equation; The initial vehicle attitude quaternion is corrected for gravity vector constraint using the corrected three-axis acceleration to obtain the final vehicle attitude quaternion.

5. The vehicle positioning method based on UWB and vehicle constraints according to claim 1, characterized in that: Rotation matrix from navigation coordinate system to vehicle coordinate system , the expression is as follows: ； in, Indicates the roll angle; Indicates the pitch angle; Indicates the yaw angle.

6. The vehicle positioning method based on UWB and vehicle constraints according to claim 1, characterized in that: Get the vehicle position, including: Initialize the state vector and the covariance matrix ; Among them, the state vector Including vehicle position, speed and attitude; Obtain the vehicle's UWB position, vehicle attitude quaternion, and corrected vehicle wheel speedometer data; State prediction is performed based on the vehicle attitude quaternion, the currently collected IMU data, and the corrected vehicle wheel speedometer data; Based on the vehicle's UWB position, the observation is updated and the measurement residual is calculated; Adjust the weight of each sensor according to the measurement residual; According to the adjusted weights, the state vector and covariance matrix are updated to obtain the updated state vector; Extract the vehicle position from the updated state vector.

7. The vehicle positioning method based on UWB and vehicle constraints according to claim 6, characterized in that: Calculate measurement residuals, including: The calculated vehicle attitude quaternion is used as the initial attitude at the current moment, and combined with the currently collected IMU angular velocity data, the attitude quaternion at the next moment is predicted; Predict the vehicle's position and velocity based on the predicted attitude quaternion and the currently collected IMU acceleration data; Using the corrected vehicle wheel speedometer data, the predicted vehicle speed is constrained to be corrected, and the speed of the vehicle's rear wheels perpendicular to the direction of travel is constrained to zero; According to the vehicle motion model, calculate the state transfer matrix F and process noise covariance matrix Q; According to the vehicle UWB position data, an observation equation is constructed, which includes the observation matrix H and the measurement noise covariance matrix R; According to the observation equation, calculate the Kalman gain , where P is the state prediction covariance matrix; The measurement residual is calculated as , where Z is the vehicle UWB position observation value obtained and X is the predicted state vector; Update the state vector using the Kalman gain K and the measurement residual and the covariance matrix .

8. The vehicle positioning method based on UWB and vehicle constraints according to claim 7, characterized in that: Adjust the weight of each sensor according to the measurement residual, using the following formula: ,in, is the weight of the i-th sensor, is the measurement noise variance of sensor i; represents the measurement noise variance of sensor j; When the UWB signal strength is less than the threshold, increase the UWB corresponding value.

9. A vehicle positioning system based on UWB and vehicle constraints, characterized in that: include: At least one processing unit; configured to execute instructions to implement the vehicle positioning method based on UWB and vehicle constraints as described in any one of claims 1 to 8.

Images