Vehicle positioning method and system based on UWB and vehicle constraint

Abstract

The invention discloses a vehicle positioning method and system based on UWB and vehicle constraint, and relates to the field of vehicle positioning, and the method comprises the steps: deploying a UWB reference station in a well mining environment, and enabling a vehicle to be provided with a UWB tag; obtaining distance measurement data of at least two UWB reference stations, and detecting and compensating the distance measurement data to obtain a vehicle UWB position; collecting vehicle-mounted IMU (Inertial Measurement Unit) data, and obtaining a vehicle attitude quaternion through attitude calculation; collecting vehicle wheel speed meter data, and correcting the vehicle wheel speed meter data according to vehicle motion constraint conditions; according to the vehicle UWB position, the vehicle attitude quaternion and the corrected vehicle wheel speed meter data, the vehicle position is obtained through an adaptive Kalman filtering fusion algorithm. Aiming at low positioning precision caused by easy accumulation of errors and drifting of a traditional inertial navigation system in a well mine closed space in the prior art, the invention reduces the influence of a multipath effect in the traditional inertial navigation system on the positioning precision.

Description

Technical Field

[0001] This application relates to the field of vehicle positioning, and particularly to a vehicle positioning method and system based on UWB and vehicle constraints. Background Art

[0002] With the continuous improvement of the intelligent and automated levels of mines, the underground vehicle autonomous driving technology has become a key technology to improve the production efficiency and safety of mines. In the underground working conditions of mines, a high-precision and reliable vehicle positioning system is the core prerequisite for realizing autonomous driving, which directly affects the effective implementation of vehicle navigation, path planning, and dispatching management. Accurate vehicle positioning can not only improve production efficiency but also effectively reduce the occurrence of underground safety accidents, which is of great significance to the sustainable development of mining enterprises.

[0003] The underground environment has obvious particularities compared with the ground environment: the space is enclosed and narrow, the roadway structure is complex, the humidity is high, the dust is much, and the electromagnetic interference is serious. These particularities make it difficult to directly apply the commonly used positioning technologies in the ground environment in the underground environment, and it is necessary to carry out adaptive technical improvements and innovations for the underground special environment.

[0004] Currently, the following technical solutions are mainly adopted for underground vehicle positioning:

[0005] SLAM positioning technology based on point cloud map: Usually, the laser SLAM (Simultaneous Localization and Mapping) technology is first used to establish a three-dimensional point cloud map of the underground mine, and then repositioning is carried out based on this map. This method usually fuses information from inertial sensors such as IMU to provide a stable pose output to support the subsequent scheduling and path planning of vehicle autonomous driving.

[0006] UWB-based positioning technology: In the underground environment, a UWB base station network is pre-deployed. Vehicles or personnel carry UWB tags. After the cloud platform receives the UWB ranging observations, it calculates the tag positions and sends the position information to the user terminal to achieve the real-time positioning function.

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

[0008] The problem of single feature in the long roadway environment underground: Underground mines usually have long tunnel structures, with monotonous structures, few features, and high repeatability in the environment, resulting in the easy occurrence of matching failures of point cloud matching technologies based on feature extraction (such as laser SLAM and visual SLAM) in this environment, and the misjudgment phenomenon of "staying in place". In this case, the frame-to-frame matching and loop detection mechanisms relied on by traditional SLAM algorithms fail, and the cumulative error gradually amplifies, eventually leading to map drift or complete loss of positioning, and unable to support the continuous and reliable operation of autonomous driving.

[0009] Satellite signal shielding and IMU integration error: In the underground tunnel environment, satellite navigation signals such as GPS completely fail and cannot provide reference positioning information. At this time, the system often turns to rely on IMU (Inertial Measurement Unit) for dead reckoning. However, IMU will generate serious integration errors during long-term and long-distance use. The dead reckoning error of low-precision MEMS-IMU can reach 15% of the driving mileage per hour. Although high-precision IMU can reduce errors, its cost is too high and it is not suitable for large-scale promotion and product replication of the project.

[0010] Insufficient environmental adaptability of UWB positioning: Although UWB technology has high ranging accuracy, in the complex underground environment, the signal is easily affected by the multipath effect, resulting in an increase in ranging error. In addition, underground metal equipment, support structures, etc. will also cause obvious interference to the UWB signal, affecting the positioning accuracy and stability.

[0011] Limitations of traditional fusion algorithms: Existing sensor fusion algorithms (such as traditional Kalman filtering) usually do not consider the specific kinematic constraints of the vehicle, and the weights of multiple sensors are fixed, and cannot dynamically adjust the credibility of each sensor according to environmental changes, resulting in insufficient adaptability in the complex and changeable underground environment and unable to give full play to the complementary advantages of multi-source sensors. Summary of the Invention

[0012] Aiming at the problem that in the existing technology, in the closed space of underground coal mines, the traditional inertial navigation system is prone to cumulative errors and drifts, resulting in low positioning accuracy. This 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 using the specific motion constraint conditions of vehicles in the underground coal mine environment for positioning correction, etc., the influence of the multipath effect on the positioning accuracy in the traditional inertial navigation system is reduced.

[0013] One aspect of this application provides a vehicle positioning method based on UWB and vehicle constraints for the underground coal mine environment, including: deploying UWB reference stations in the underground coal mine environment, and installing UWB tags on the vehicle; obtaining ranging data of at least two UWB reference stations, detecting and compensating the ranging data to obtain the vehicle UWB position; collecting on-vehicle 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 the vehicle motion constraint conditions; according to the vehicle UWB position, the vehicle attitude quaternion, and the corrected vehicle wheel speedometer data, obtaining the vehicle position through an adaptive Kalman filter fusion algorithm.

[0014] Furthermore, deploy UWB reference stations in the underground mine environment and install UWB tags on vehicles, including: deploy one UWB reference station every 100 meters in the underground mine roadway and install one UWB reference station at the turning of the roadway; perform time and space calibration on the deployed UWB reference stations.

[0015] Furthermore, obtain the ranging data of at least two UWB reference stations, detect and compensate the ranging data to obtain the UWB position of the vehicle, including: detect whether the UWB signal strength of the ranging data is greater than the preset threshold; for the ranging data that passes the detection, perform multipath effect compensation, including: perform time-domain decomposition on the UWB received signal by using the channel impulse response analysis method, identify the delay characteristics of the direct signal and the multipath reflection signal, and generate a channel characteristic map containing the delay and amplitude of each signal path; based on the generated channel characteristic map, use the first-path detection algorithm with adaptive threshold to process the signal, extract the earliest arrival signal component representing the direct path and its accurate arrival time by setting a dynamic noise threshold and searching for the first signal peak exceeding the threshold; use the obtained arrival time of the direct signal and the identified characteristics of the multipath reflection signal, adopt a multipath compensation model based on the relationship between signal strength and delay, calculate and eliminate the ranging deviation introduced by the reflection signal, apply the correction value to the original ranging data to obtain the compensated UWB ranging value, so as to achieve the purpose of eliminating the influence of the multipath effect. According to the compensated ranging data, use the trilateration method or the least squares method to calculate the UWB tag position as the UWB position of the vehicle.

[0016] Furthermore, collect vehicle-borne IMU data and obtain the vehicle attitude quaternion through attitude solution, including: 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; according to the corrected three-axis angular velocity, perform numerical integration through the quaternion differential equation to obtain the initial vehicle attitude quaternion; use 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, where the scale correction refers to converting the original output signal of the sensor into a standard physical unit through the calibrated scale factor, so that the unit of acceleration data is m / s² and the unit of angular velocity data is rad / s;

[0017] Based on the corrected three-axis angular velocity, perform numerical integration through the quaternion differential equation: , where, is a quaternion, is the angular velocity vector, represents quaternion multiplication to obtain the initial vehicle attitude quaternion;

[0018] Construct a gravity observation vector using the calibrated triaxial accelerations, calculate the error between the vector and the theoretical gravity vector, and use a proportional-integral feedback correction mechanism to perform gravity vector constraint correction on the initial vehicle attitude quaternion to suppress integral drift and obtain the final vehicle attitude quaternion.

[0019] Further, collect vehicle wheel speedometer data and correct the vehicle wheel speedometer data according to vehicle motion constraint conditions, including: based on the fact that the rear wheel speed perpendicular to the traveling direction of the vehicle in an underground mine environment is less than a threshold value, establish a vehicle motion model: , where represents the speed of the vehicle's rear wheel in the navigation coordinate system n, represents the speed of the vehicle's rear wheel in the vehicle body coordinate system V; represents the rotation matrix from the navigation coordinate system to the vehicle body coordinate system.

[0020] Perform zero bias compensation on the vehicle wheel speedometer data: , where 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), usually the local horizontal coordinate system, b represents the vehicle body coordinate system (Body frame), usually fixedly connected to the vehicle's IMU sensor, represents the original rotation matrix from the navigation coordinate system to the vehicle body coordinate system; represents the small rotation matrix in the navigation coordinate system for zero bias compensation; I represents the 3×3 identity matrix; represents the zero bias error vector, including small rotation angles in three directions; × represents the skew-symmetric matrix operator of the vector for converting the rotation vector into a rotation matrix.

[0021] Perform installation angle compensation on the vehicle wheel speedometer data: , where, represents the error or increment symbol, 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 speed, e represents the earth coordinate system (Earth frame), usually the ECEF coordinate system, represents the skew-symmetric matrix form of the vector; represents the attitude error vector.

[0022] Further, the rotation matrix from the navigation coordinate system to the vehicle body coordinate system, , where, represents the roll angle; represents the pitch angle; represents the yaw angle.

[0023] Furthermore, obtaining the vehicle position includes: initializing the state vector and the covariance matrix ; wherein, the state vector includes the vehicle position, speed, and attitude; obtaining the vehicle UWB position, the vehicle attitude quaternion, and the corrected vehicle wheel speedometer data; performing state prediction based on the vehicle attitude quaternion, the currently collected IMU data, and the corrected vehicle wheel speedometer data; performing observation update based on the vehicle UWB position, calculating the measurement residual; adjusting the weights of each sensor according to the measurement residual; updating the state vector and the covariance matrix according to the adjusted weights to obtain the updated state vector; extracting the vehicle position from the updated state vector.

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

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

[0026] Another aspect of the present application also provides a vehicle positioning system based on UWB and vehicle constraints for executing a vehicle positioning method based on UWB and vehicle constraints of the present application.

[0027] Compared with the prior art, the advantages of the present application are:

[0028] In the underground mine environment, it is difficult to locate due to problems such as enclosed and narrow space, complex roadway structure with single characteristics, and severe electromagnetic interference. Existing technologies generally use laser SLAM or single UWB positioning systems, but there are defects such as feature matching failure, multipath effect interference, and inertial navigation cumulative error. In this application, first, by detecting the signal strength of UWB ranging data and compensating for the multipath effect, the ranging error caused by multipath propagation in the underground complex environment is effectively reduced. Then, using IMU data calibration and quaternion attitude solution, combined with gravity vector constraint correction, the system effectively suppresses the cumulative error of the traditional inertial navigation system. Finally, based on the characteristic that the speed of the rear wheel of the vehicle perpendicular to the traveling direction is almost zero in the underground mine environment, 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 through an adaptive Kalman filtering algorithm, the system can dynamically adjust the weights of each sensor, automatically reduce its influence when the UWB signal quality deteriorates, and maintain the positioning accuracy of the system. This multi-sensor fusion method not only improves the adaptability of the system in complex environments but also solves the limitations that cannot be overcome by traditional single positioning technologies. Brief Description of the Drawings

[0030] Figure 1 is an exemplary flowchart of a vehicle positioning method based on UWB and vehicle constraints shown in some embodiments of the present application;

[0031] Figure 2 is a schematic diagram of UWB base station layout shown in some embodiments of the present application;

[0032] Figure 3 is a schematic diagram of the UWB multipath effect in the underground mine environment shown in some embodiments of the present application;

[0033] Figure 4 is an analysis effect diagram of the channel impulse response shown in some embodiments of the present application. Detailed Embodiments

[0034] The methods and systems provided in the embodiments of the present application will be described in detail below with reference to the accompanying drawings.

[0035] As Figure 1As shown in the figure, deploy UWB reference stations in the underground mine environment and install UWB tags on vehicles; obtain the ranging data of at least two UWB reference stations, detect and compensate the ranging data to obtain the UWB position of the vehicle; collect on-vehicle IMU data and obtain the vehicle attitude quaternion through attitude calculation; collect vehicle wheel speed meter data and correct the vehicle wheel speed meter data according to the vehicle motion constraint conditions; according to the vehicle UWB position, the vehicle attitude quaternion, and the corrected vehicle wheel speed meter data, obtain the vehicle position through an adaptive Kalman filter fusion algorithm.

[0036] As Figure 2 shown in the figure, deploy UWB reference stations every 100 meters in the roadway, and add UWB reference stations at the roadway bends to test the UWB signal strength. When it is detected that the UWB is affected by multipath or interference greatly, a new location needs to be selected. Considering the signal attenuation, the spacing should not be greater than 200 meters in principle. When the UWB tag receives the UWB ranging observation, it automatically calculates its own position and needs to receive the observations of 2 base stations simultaneously. For the convenience of subsequent fusion calculation, the UWB reference station coordinates need to be calibrated to a unified map coordinate system, and here the point cloud map coordinates are used as the reference system. When the UWB device is used in different environments, time and space calibration also needs to be done in advance to ensure that the ranging observations are accurate.

[0037] In the underground mine environment, the UWB signal will be reflected, blocked, and attenuated by objects such as roadway walls, equipment, and supports, resulting in unstable signal quality. In this embodiment, the signal strength of the ranging data is detected: set the signal strength threshold to usually be from -80 dBm to -75 dBm; obtain the received signal strength indication (RSSI) of each UWB reference station signal in real time; automatically exclude the signal data below the threshold to ensure that only high-quality ranging data is used for subsequent calculations; when multiple reference stations are available, give priority to the data of the reference station with higher signal strength; this mechanism ensures that the UWB positioning system only uses high-quality signal data and improves the reliability of position calculation.

[0038] In addition, as Figure 3 shown in the figure, the underground mine environment is narrow in space and there are a large number of metal devices, resulting in a significant multipath effect of the UWB signal. The signal arrives at the receiving end through multiple paths, causing ranging errors. The following compensation mechanism is adopted in this implementation method: channel impulse response (CIR) analysis: identify and extract the earliest arriving signal (direct wave); establish a dynamic environment model for the ranging data of each reference station, and construct a multipath effect compensation model according to the historical ranging and spatial position relationship. The compensation formula: ; where is the multipath effect estimation value, 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 adopted: Establish a non-linear equation system: ; where, (x, y, z) is the tag position to be solved, is the reference station position, is the compensated ranging value; Solve the equation system by an analytical method or an iterative method. When the number of available reference stations is greater than 3, the weighted least squares method is adopted: Construct an error function ; The weight is proportional to the signal strength, and the stronger the signal, the greater the weight; Use optimization methods such as the Levenberg-Marquardt algorithm to solve for the optimal position. As Figure 4 shown, through CIR analysis and multipath model compensation in this application, the ranging error caused by the multipath effect is effectively reduced, and the original positioning accuracy is improved.

[0040] Collect vehicle-mounted IMU data, and obtain the vehicle attitude quaternion through attitude solution. First, vehicle-mounted IMU sensors usually collect three-axis acceleration and three-axis angular velocity data: The sampling frequency is set to 100Hz to 200Hz to ensure capturing the rapid movement of the vehicle; Use a digital low-pass filter (such as a Butterworth filter) to preprocess the original data and filter out high-frequency noise.

[0041] IMU sensors have inherent zero bias and scale errors, which need to be accurately calibrated: On the one hand, when the vehicle is stationary, utilize the characteristic that the angular velocity theory is zero under static conditions to identify the angular velocity zero bias, and the calibration formula: , where, represents the vector of the original angular velocity measurement values directly obtained from the IMU sensor (rad / s); represents the angular velocity zero bias vector, which represents the non-zero value output by the sensor in the stationary state (rad / s); represents the calibrated angular velocity vector, with the zero bias removed and the scale factor correction applied (rad / s); The acceleration zero bias is determined by the constraint condition that the magnitude of the gravitational acceleration is g. On the other hand, establish a scale factor matrix K to correct the proportional relationship between the sensor output and the actual physical quantity, and the calibration formula: , and the same for angular velocity: , The calibration parameters can be obtained through Allan variance analysis and multi-attitude 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 vector of the original acceleration measurement values directly obtained from the IMU sensor (m / s²); represents the acceleration bias vector, which is the non-gravitational acceleration (m / s²) output by the sensor in the stationary state; represents the calibrated acceleration vector, with the bias removed and the 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 and includes the cross-axis coupling correction.

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

[0043] When the vehicle is static or moving at a constant speed, the accelerometer measurement value is mainly the gravitational acceleration. Therefore, to suppress the integration drift, this application introduces a gravity vector constraint to calculate the projection of the theoretical gravity vector in the vehicle body coordinate system under the current attitude 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 vehicle dynamic characteristics, where, represents the gravity acceleration vector in the vehicle body coordinate system; represents the inverse of the quaternion q. For a unit quaternion, . represents the three-axis acceleration vector measured by the IMU in the vehicle body coordinate system; represents the magnitude of the measured acceleration vector; represents the magnitude of the gravity vector in the vehicle body coordinate system; represents the intermediate attitude quaternion result obtained by numerical integration of the quaternion differential equation; represents the final attitude quaternion after correction by the gravity vector constraint.

[0044] Among them, this application introduces a gravity vector constraint, provides an external reference, and effectively suppresses the long-term attitude drift. In addition, the quaternion representation method is adopted to avoid the gimbal lock problem in the Euler angle representation, ensuring the continuity and stability of the attitude solution at any attitude.

[0045] In an underground mine environment, vehicles usually travel at low speeds (usually < 20 km / h) and operate in well-designed roadways. In this specific environment, vehicles have the following kinematic characteristics: The speed of the rear wheels perpendicular to the direction of travel is almost zero (non-holonomic constraint); on a flat road surface, the wheels hardly jump or experience severe sideslip; the vehicle mainly moves along the axis of the roadway, and the steering angle changes relatively slowly; these motion characteristics provide important constraint conditions: when the vehicle is traveling normally, the lateral velocity component of the rear wheels in the vehicle body coordinate system V should be close to zero, that is (ε is a preset threshold, with a typical value of 0.05 m / s).

[0046] Based on the above constraint conditions, a vehicle motion model is established: , where: : represents the velocity vector of the rear wheels of the vehicle in the navigation coordinate system (n), represents the velocity component of the vehicle in the eastward direction in the navigation coordinate system; represents the velocity component of the vehicle in the northward direction in the navigation coordinate system; represents the velocity component of the vehicle in the vertically upward direction in the navigation coordinate system; : represents the velocity vector of the rear wheels of the vehicle in the vehicle body coordinate system (V), represents the lateral velocity component of the vehicle in the vehicle body coordinate system; represents the longitudinal velocity component of the vehicle in the vehicle body coordinate system; represents the vertical velocity component of the vehicle in the vehicle body coordinate system; represents the rotation matrix from the navigation coordinate system to the vehicle body coordinate system; Key constraint: . Among them, this application utilizes the non-holonomic constraint characteristics of the vehicle (lateral velocity ≈ 0) to provide additional observation information, significantly reducing the speed and position cumulative errors of the inertial navigation system during long-term operation.

[0047] Rotation matrix is constructed through the Euler angles , and the expression is as follows: , where represents the roll angle of the vehicle (roll), rotation around the x-axis; represents the pitch angle of the vehicle (pitch), rotation around the y-axis; represents the yaw angle of the vehicle (yaw), rotation around the z-axis; these Euler angles are obtained from quaternion attitude calculations and are realized through the standard quaternion-to-Euler angle conversion formula.

[0048] Due to the existence of installation errors, there is an incomplete alignment problem between the vehicle body coordinate system and the sensor coordinate system, and zero-offset compensation is required: , where: represents the rotation matrix from the navigation coordinate system to the body coordinate system after zero-offset compensation; represents the original rotation matrix from the navigation coordinate system to the body coordinate system; represents the small rotation matrix in the navigation coordinate system for zero-offset compensation; n represents the navigation coordinate system (Navigation frame), usually defined as the local horizontal coordinate system (east - north - up); b represents the body coordinate system (Body frame), the coordinate system fixed to the vehicle's IMU sensor; I: represents the 3×3 identity matrix; : represents the zero-offset error vector, containing small rotation angles in three directions; represents the skew-symmetric matrix of, that is: , the zero-offset error vector is obtained through the initial calibration process by measuring the difference between the gravity direction and the theoretical gravity direction when the vehicle is stationary; represents the small rotation angle around the x-axis (roll direction error); represents the small rotation angle around the y-axis (pitch direction error); represents the small rotation angle around the z-axis (yaw direction error).

[0049] To further improve the accuracy, installation angle compensation is performed on the wheel speed sensor data: , where: the speed error in the body coordinate system; the speed error in the earth coordinate system; the rotation matrix from the earth coordinate system to the body coordinate system; represents the skew-symmetric matrix form of the vector; ζ represents the attitude error vector; e represents the earth coordinate system (Earth frame); The installation angle compensation mainly solves 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 during turning.

[0050] Among them, the zero-offset compensation and installation angle compensation mechanisms of this application solve the systematic error caused by sensor installation deviation, reducing the system's requirement for installation accuracy. In addition, the installation angle compensation considering the lever arm effect enables the vehicle to maintain high-precision positioning during turning. At the same time, compared with complex point cloud matching or visual feature extraction, the wheel speed correction based on motion constraints has a small computational load and good real-time performance, and can operate efficiently in a low-computing resource environment.

[0051] The adaptive Kalman filter algorithm is a recursive estimation method for optimally fusing measurement data from different sensors. The key difference between it and the traditional Kalman filter is that it can dynamically adjust the weights of each sensor according to the real-time measurement residuals, enabling the system to adapt to environmental changes and sensor performance fluctuations. This application is based on the state-space model and realizes 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 of 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: ; where: : represents the three-dimensional position of the vehicle in the navigation coordinate system, with the unit of meter; : represents the three-dimensional velocity of the vehicle in the navigation coordinate system, with the unit of meter per second; : represents the attitude quaternion from the vehicle body coordinate system to the navigation coordinate system.

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

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

[0055] Then, position and velocity prediction are carried out. Using the IMU acceleration data and the predicted attitude quaternion, the vehicle velocity and position are updated: ; ; where: represents the rotation matrix from the vehicle body 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 velocity vector (m / s) in the navigation coordinate system at time k + 1; represents the vehicle velocity vector (m / s) in the navigation coordinate system at time k; Predicted vehicle position vector in the navigation coordinate system at time k+1 (m); Vehicle position vector in the navigation coordinate system at time k (m); Acceleration component in the x-axis direction of the vehicle body coordinate system; Acceleration component in the y-axis direction of the vehicle body coordinate system; Acceleration component in the z-axis direction of the vehicle body coordinate system.

[0056] Apply wheel speed constraints, use the corrected wheel speedometer data in S4 as the vehicle longitudinal speed measurement value, and use the nonholonomic motion constraint (rear wheel lateral speed ≈ 0) to correct the predicted speed: ; (Constraint condition); (Wheel speedometer measurement value); . The state transition matrix F is calculated through the Jacobian matrix of the nonlinear system model; the process noise covariance matrix Q characterizes the IMU measurement noise and modeling error and is constructed as a diagonal matrix.

[0057] Construct the observation equation, and the UWB position observation equation is: , where: is the UWB observed position vector; is the UWB observation matrix, in the form of ( is the 3×3 identity matrix); is the UWB measurement noise.

[0058] The wheel speedometer nonholonomic constraint observation equation is: , where Extract the lateral speed component in the vehicle body coordinate system; represents the wheel speedometer nonholonomic constraint observation value; represents the wheel speedometer 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 residual, , 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 weights 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 value is 10 to 20).

[0061] Weight calculation, , where: 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 reciprocals of the noise variances of all sensors.

[0062] UWB signal quality adaptive mechanism: If UWB_signal_strength < threshold, then , where: threshold is the UWB signal strength threshold, and a typical value is -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 vehicle position information: , that is, the first three components of the state vector, representing the three-dimensional position in the navigation coordinate system.

[0064] Among them, in this application, on the one hand, through the measurement residual-driven adaptive mechanism, the system can automatically identify and reduce the weights of abnormal sensor data. For example, in the UWB occlusion area, the weights of the IMU and wheel speedometer are automatically increased, and in the area with good UWB signals, the UWB weight is correspondingly increased, significantly enhancing the system robustness. On the other hand, the parameters of traditional fusion algorithms are fixed and cannot adapt to the environmental characteristics of different areas in the underground mine (such as spacious stope, narrow roadway, turning area, etc.). In this application, the parameters are adjusted in real time according to the measurement residuals, enabling the system to adapt to different underground mine environments. In addition, electromagnetic interference in the underground mine environment can cause abnormal IMU data or UWB signal fluctuations. The adaptive mechanism of this application can detect sudden anomalies and reduce their impact through weight adjustment.

[0065] In summary, this application effectively suppresses the cumulative error through vehicle motion constraints and an adaptive fusion mechanism, and solves the problem of the decline in vehicle long-term operation positioning accuracy in the underground mine environment. This technical solution first reduces the ranging error in the complex underground mine environment based on UWB signal strength detection and multipath effect compensation; secondly, suppresses the cumulative error of the inertial navigation system through IMU quaternion attitude solution and gravity vector constraint correction; thirdly, utilizes the characteristic that the speed of the rear wheels of the vehicle perpendicular to the traveling direction is close to zero in the underground mine environment to construct a vehicle motion model and a zero-bias compensation mechanism, improving the reliability of wheel speedometer data; finally, adopts an adaptive Kalman filter algorithm to achieve multi-sensor data fusion, dynamically adjusts the weights of each sensor, and automatically reduces the impact when the UWB signal quality deteriorates.

[0066] The above has schematically described the present invention and its implementation manners. This description is not restrictive. Without departing from the spirit or basic characteristics of the present invention, the present invention can be implemented in other specific forms. What is shown in the drawings is only one of the implementation manners of the present invention, and the actual structure is not limited thereto. Therefore, if those of ordinary skill in the art are inspired by it and, without departing from the purpose of this creation, design structures and embodiments similar to this technical solution without creative efforts, they shall fall within the protection scope of this application. In addition, the term "including" does not exclude other elements or steps, and the term "a" before an element does not exclude including "a plurality of" such elements. Terms such as first and second are used to represent names and do not represent any specific order.

Claims

1. A vehicle positioning method based on UWB and vehicle constraints, which is used in the underground mine environment, is characterized in that Including: Deploy UWB reference stations in the underground mine environment and install UWB tags on vehicles; Obtain ranging data of at least two UWB reference stations, detect and compensate the ranging data to obtain the vehicle UWB position; Collect on-vehicle IMU data and obtain the vehicle attitude quaternion through attitude calculation; Collect vehicle wheel speedometer data and correct the vehicle wheel speedometer data according to the vehicle motion constraint conditions; According to the vehicle UWB position, the vehicle attitude quaternion, and the corrected vehicle wheel speedometer data, use the adaptive Kalman filter fusion algorithm to obtain the vehicle position.

2. The vehicle positioning method based on UWB and vehicle constraints according to claim 1, characterized in that: Deploying UWB reference stations in the underground mine environment and installing UWB tags on vehicles includes: Deploy one UWB reference station every 100 meters in the underground mine roadway and install one UWB reference station at the roadway turning point; 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: Obtaining the vehicle UWB position includes: Detect whether the UWB signal strength of the ranging data is greater than a preset threshold; Perform multipath effect compensation on the detected ranging data; According to the compensated ranging data, use the trilateration method or the least squares method to calculate the UWB tag position as the vehicle UWB position.

4. The vehicle positioning method based on UWB and vehicle constraints according to claim 2, characterized in that: Obtaining the vehicle attitude quaternion through attitude calculation includes: 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; According to the corrected three-axis angular velocity, perform numerical integration through the quaternion differential equation to obtain the initial vehicle attitude quaternion; Use the corrected three-axis acceleration to perform gravity vector constraint correction on the initial vehicle attitude quaternion to obtain the final vehicle attitude quaternion.

5. The vehicle positioning method based on UWB and vehicle constraints according to claim 4, characterized in that: Correcting the vehicle wheel speedometer data includes: Based on the fact that the speed of the rear wheels of the vehicle perpendicular to the traveling direction in the underground mine environment is less than the threshold, a vehicle motion model is established: , where represents the speed of the rear wheels of the vehicle in the navigation coordinate system n, represents the speed of the rear wheels of the vehicle in the vehicle body coordinate system V; represents the rotation matrix from the navigation coordinate system to the vehicle body coordinate system; Perform zero bias compensation on vehicle wheel speedometer data: , where represents the rotation matrix from the navigation coordinate system to the body coordinate system after zero bias compensation; n represents the navigation coordinate system, b represents the body coordinate system, represents the original rotation matrix from the navigation coordinate system to the body coordinate system; represents the small rotation matrix in the navigation coordinate system; I represents the 3×3 identity matrix; represents the zero bias error vector; × represents the skew-symmetric matrix operator of the vector; Perform installation angle compensation on vehicle wheel speedometer data: , where represents the error or increment symbol, 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 speed, and e represents the earth coordinate system, represents the skew-symmetric matrix form of the vector; represents the attitude error vector.

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

7. The vehicle positioning method based on UWB and vehicle constraints according to claim 5, characterized in that: Obtaining the vehicle position includes: Initialize the state vector and the covariance matrix ; wherein, the state vector includes the vehicle position, speed, and attitude; Obtain the vehicle UWB position, the vehicle attitude quaternion, and the corrected vehicle wheel speedometer data; Perform state prediction according to the vehicle attitude quaternion, the currently collected IMU data, and the corrected vehicle wheel speedometer data; Perform observation update according to the vehicle UWB position, calculate the measurement residual; Adjust the weights of each sensor according to the measurement residual; Update the state vector and covariance matrix according to the adjusted weights to obtain the updated state vector; Extract the vehicle position from the updated state vector.

8. The vehicle positioning method based on UWB and vehicle constraints according to claim 7, characterized in that: Calculating the measurement residual includes: Take the calculated vehicle attitude quaternion as the initial attitude at the current moment, and combine it with the currently collected IMU angular velocity data to predict the attitude quaternion at the next moment; Based on the predicted attitude quaternion and the currently collected IMU acceleration data, predict the position and velocity of the vehicle; Use the corrected vehicle wheel speedometer data to constrain and correct the predicted vehicle speed, and constrain the speed of the vehicle's rear wheel perpendicular to the traveling direction to zero; According to the vehicle motion model, calculate the state transition matrix F and the process noise covariance matrix Q; Based on the vehicle UWB position data, construct an observation equation, and the observation equation includes an observation matrix H and a measurement noise covariance matrix R; Calculate the Kalman gain according to the observation equation , where P is the state prediction covariance matrix; The calculated measurement residual is , where Z is the obtained vehicle UWB position observation value and X is the predicted state vector; Update the state vector using the Kalman gain K and the measurement residual and the covariance matrix .

9. The vehicle positioning method based on UWB and vehicle constraints according to claim 8, characterized in that: Adjust the weights of each sensor according to the measurement residual through the following formula: , where 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 value corresponding to UWB.

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

Images