Charging pile port orientation positioning method and system based on UWB

Abstract

The invention provides a UWB-based charging pile port orientation positioning method and system, the UWB-based charging pile port orientation positioning method comprises five steps: through dynamic coordinate system mapping, DS-TWR ranging algorithm, extended Kalman filter (EKF) fusion and gyroscope auxiliary compensation, high-precision real-time calculation of a charging port normal orientation angle is realized in a metal environment. A UWB-based charging pile port orientation positioning system comprises a UWB base station network, label hardware, a dynamic coordinate mapping module and a pose error compensation module, the data updating frequency is larger than or equal to 20 Hz, the positioning error is smaller than 5 cm, the orientation angle precision is + / -2 degrees, and the system is suitable for an automatic charging scene.

Description

Technical Field

[0001] The present invention relates to the technical field of electric vehicle charging positioning, and in particular to a charging pile port orientation positioning method and system based on UWB. Background Art

[0002] As the number of new energy vehicles in the world exceeds 20 million, the commercialization of automatic charging systems faces severe challenges. Accurately locating the direction of the charging pile port is the core technical bottleneck for realizing unmanned charging. However, existing technical solutions have significant defects in dynamic scene adaptability, anti-interference in metal environments, and system cost-effectiveness. The failure probability of traditional visual recognition solutions exceeds 60% when the light is insufficient, and additional fill light equipment is required; although the lidar has high precision characteristics, the point cloud distortion error caused by its metal surface reflection will amplify over time, and it cannot work normally in rainy and snowy weather. Infrared guidance technology is seriously interfered by solar radiation. When the signal-to-noise ratio is lower than 3:1, the transmission distance drops sharply to less than 2 meters, which is difficult to meet the actual application needs.

[0003] Although traditional UWB positioning technology achieves ±5cm ranging accuracy with 3-10GHz ultra-wideband signals, its performance in dynamic scenarios is unsatisfactory. Actual measured data show that when the vehicle moves at a constant speed of 0.5m / s, the system cumulative error can reach 12cm, far exceeding the sub-centimeter accuracy required for automatic charging and docking. In addition, the existing UWB solution can only provide two-dimensional plane positioning information and lacks the ability to solve the normal orientation angle of the charging port, resulting in a risk of ±15° directional deviation during the automatic plugging and unplugging of the robotic arm. What is more serious is that the multipath effect caused by the metal casing of the charging pile causes the signal RMS delay to extend by more than 20ns, and the path loss fluctuation amplitude reaches more than 8dB, further weakening the robustness of the positioning system.

[0004] In this context, the industry urgently needs a new positioning solution that can maintain the high-precision characteristics of UWB while breaking through the limitations of dynamic scenes and metal environments. Studies have shown that the introduction of a dynamic coordinate system mapping mechanism can reduce the coordinate offset error by 83%, and the fusion of gyroscope IMU data can improve the accuracy of the orientation solution to within ±2°. However, there is no effective solution in terms of base station-tag attribute exchange mechanism, DS-TWR ranging optimization algorithm, metal multipath suppression strategy, etc., which makes it difficult for the system to break through the 15Hz update frequency in real time, and the hardware cost remains high. This patent is aimed at the above-mentioned technical pain points and proposes a charging pile entrance orientation positioning method based on real-time conversion of relative coordinates. Summary of the invention

[0005] In order to solve the problems existing in the background technology, the present invention provides a charging pile port orientation positioning method and system based on UWB.

[0006] The technical solution adopted by the present invention is: a method for positioning the orientation of a charging pile port based on UWB, including the following steps:

[0007] S1: The system starts, and the UWB base station network is initialized;

[0008] S2: Tag signal capture, and signal validity detection is performed. If a signal is detected, the next step is carried out; if no signal is detected, it returns to the tag signal capture link;

[0009] S3: The captured valid signal is solved by the DS-TWR algorithm and processed by the EKF algorithm, and it is judged whether the preset distance is reached. If so, it enters the positioning algorithm link; if not, it returns to the signal validity detection link;

[0010] S4: Positioning algorithm processing link: The input signal is successively subjected to relative coordinate conversion between the base station and the tag, dynamic coordinate system mapping, calculation of the orientation angle of the charging pile port, pose error compensation, and output of the positioning result;

[0011] S5: After the positioning algorithm processing link, it is judged whether to continue positioning. If continuous positioning is required, it returns to the tag signal capture link; if continuous positioning is not required, the system is shut down.

[0012] Further, dynamic coordinate system mapping is realized by swapping the attributes of the base station and the tag;

[0013] The DS-TWR algorithm is used to eliminate clock deviation, and the formula is:

[0014] T flight =(T round1 ×T round2 -T reply1 ×T reply2 ) / (T round1 +T round2 +T reply1 +T reply2 +Δ)

[0015] T flight : Flight time

[0016] T round1 : The difference between t1 and t0

[0017] T reply1 : The difference between t3 and t2

[0018] T round2 : The difference between t5 and t3

[0019] T reply2 : The difference between t4 and t1

[0020] Δ = α1×(Tround1 -T reply2 ) + α2×(T round2 -T reply1 )

[0021] Where Δ is the dynamic compensation term, and α1, α2 are compensation coefficients;

[0022] Using the EKF algorithm to fuse UWB ranging data, gyroscope attitude data, and wheel speed data, the state update formula is:

[0023]

[0024] Where represents the corrected state estimate, represents the predicted state, K k represents the Kalman gain, z k represents the observed value, and h is the nonlinear observation function; dynamic mapping is realized through the coordinate total transformation matrix, and the formula is:

[0025] T = T 旋转 ×T 平移

[0026] Where T is the total transformation matrix, T 旋转 is the rotation transformation matrix, T 平移 is the translation transformation matrix;

[0027] Furthermore, the trigger condition for dynamic coordinate system mapping is that the distance between the base station and the tag is less than the preset threshold, and attribute exchange is realized by sending instructions through the UWB serial port.

[0028] Furthermore, it includes an in-vehicle UWB base station, a main control chip STM32, and a charging interface tag, and integrates a 6-axis gyroscope and a dynamic coordinate mapping module; the tag supports the DS-TWR algorithm for ranging, and the ranging process includes obtaining the original timestamp, calculating the time interval, and compensating for the improved clock deviation;

[0029] The system converts the orientation angle through relative coordinates. The azimuth angle φ is the angle between the projection of the normal vector on the horizontal plane and the X axis, and the calculation formula is as follows:

[0030]

[0031] Where represents the component of the normal vector on the XY plane;

[0032] The vertical angle θ1: the angle between the normal vector and the horizontal plane, and the calculation formula is as follows:

[0033]

[0034] Where It is expressed as the component of the normal vector on the Z-axis.

[0035] Furthermore, the pose error compensation module adopts an adaptive weight function α:

[0036]

[0037] In the formula, the dynamic index can be selected as the angular velocity modulus or the acceleration change rate. k is used to control the steepness of the mapping, combined with gyroscope MAD detection and UWB M estimation robust filtering.

[0038] The present invention effectively solves the key pain points of traditional UWB systems in the charging pile positioning scenario through multiple technological innovations. First, aiming at the problem of charging port attitude recognition, the system realizes the real-time calculation of the normal orientation angle of the charging port through the dynamic coordinate system mapping mechanism between the base station and the tag, combined with gyroscope motion compensation and coordinate inverse transformation algorithm, breaking through the technical limitation that traditional UWB can only provide two-dimensional plane positioning.

[0039] Secondly, in terms of adaptability to the metal environment, an improved DS-TWR ranging algorithm and a signal time delay compensation model are adopted to effectively suppress the multipath interference caused by the metal shell of the charging pile, improving the positioning stability by more than 10% and reducing the path loss fluctuation amplitude to an acceptable range for engineering applications. In terms of real-time performance, the system optimizes through a hardware trigger synchronization mechanism and a lightweight EKF filtering algorithm, improving the data update frequency to 20Hz and controlling the response delay within 50 milliseconds, meeting the strict requirements for high real-time performance in the automatic charging docking process.

[0040] In addition, an innovative single-chip integrated design scheme is adopted to integrate the functions of multipath suppression, motion compensation, and coordinate conversion on the same computing platform, significantly reducing the hardware deployment cost and algorithm development complexity, and reducing the overall cost of the system by more than 15% compared with similar solutions, providing important support for large-scale commercial applications. Description of the Drawings

[0041] Figure 1 It is a flowchart of a method for positioning the orientation of a charging pile port based on UWB;

[0042] Figure 2 It is a flowchart of relative coordinate conversion.

[0043] Abbreviations and Definitions of Key Terms:

[0044] UWB: UltraWide Band;

[0045] TWR: Two-way Ranging;

[0046] DS-TWR: Double-sided Two-way Ranging;

[0047] TDOA: Time Difference of Arrival;

[0048] IMU: Inertial Measuring Unit;

[0049] INS: Inertial Navigation System;

[0050] EKF: Extended Kalman Filter;

[0051] RMS: Root Mean Square;

[0052] NDT: Normal Distributions Transform;

[0053] ICP: Iterative Closest Point;

[0054] PnP: Perspective-n-Point;

[0055] AoA: Angle of Arrival;

[0056] SLAM: Simultaneous Localization and Mapping;

[0057] FMCW: Frequency Modulated Continuous Wave;

[0058] SNR: Signal-to-Noise Ratio;

[0059] TOF: Time of Flight;

[0060] NLOS: Non Line of Sight. Detailed implementation

[0061] Combined with the accompanying drawings of the embodiments of the present invention, the implementation manners and automatic processes of the embodiments of the present invention will be described completely and in detail. The described embodiments are only a part of the embodiments in the specific use process of the present invention, rather than all the embodiments. The protection scope of the present invention is not limited to the described embodiments.

[0062] Refer to Figure 1 and Figure 2 , this embodiment provides a method for positioning the orientation of a charging pile port based on UWB, including the following steps:

[0063] S1: The system starts, and the UWB base station network is initialized;

[0064] S2: The tag signal is captured, and the signal validity is detected. If a signal is detected, the next step is carried out. If no signal is detected, it returns to the tag signal capture link;

[0065] The specific process of signal validity detection is: after capturing the tag signal, a method combining energy detection and correlation detection is used to confirm the validity of the transmitted signal.

[0066] First, the cross-correlation value of the received signal S sig and the local sequence S loc is calculated through a matched filter. The role of the matched filter is to enhance the effective signal component and suppress noise by maximizing the SNR, so as to effectively detect the known signal in a noisy environment. The calculation formula is as follows:

[0067]

[0068] In the formula, S corr (n) represents the cross-correlation value at time n, τ represents different time delays, S sig (n + τ) represents the sampling value of the received signal at time n + τ, and S loc (τ) represents the sampling value of the local sequence at time τ.

[0069] Then, inter-symbol filtering is performed using an IIR filter, where the coefficient is set to ω = 1 / 4. Finally, the peak index is extracted and the signal validity is judged by threshold comparison.

[0070] S3: The captured valid signal is solved by the DS-TWR algorithm and processed by the EKF algorithm, and it is judged whether the preset distance is reached. If so, it enters the positioning algorithm link. If not, it returns to the signal validity detection link;

[0071] The DS-TWR algorithm is adopted for ranging calculation. This algorithm is an extension of one-way two-way ranging, mainly calculating the distance by measuring the round-trip time of the signal between devices. Compared with traditional single or one-way two-way ranging, it can significantly reduce the error caused by clock deviation.

[0072] The ranging calculation algorithm process is as follows:

[0073] Obtaining the original time measurement value: The timestamp recorded by device A

[0074] t0: The moment when Poll is sent; t1: The moment when Response is received; t4: The timestamp recorded by device B when Final is sent:

[0075] t2: The moment when Poll is received; t3: The moment when Response is sent; t5: The moment when Final is received.

[0076] Calculating the time interval:

[0077] Tround1: The difference between t1 and t0; Treply1: The difference between t3 and t2; Tround2: The difference between t5 and t3; Treply2: The difference between t4 and t1.

[0078] The flight time T flight Calculation is mainly carried out using an improved clock deviation compensation algorithm, and the calculation formula is as follows:

[0079] T flight =(T round1 ×T round2 -T reply1 ×T reply2 ) / (T round1 +T round2 +T reply1 +T reply2 +Δ)

[0080] Δ = α1×(T round1 -T reply2 )+α2×(T round2 -T reply1 )

[0081] In the formula, Δ is the dynamic compensation term, and α1, α2 are compensation coefficients; the calculation of this Δ is mainly achieved through the following steps:

[0082] Establishing a clock drift model:

[0083] δ=(T round1 -k1×T reply2 ) / (T round2 -k1×T reply1 )

[0084] Wherein, k1 is a preset antenna delay ratio, which is used to compensate for the influence of hardware delay.

[0085] Query the pre-stored compensation table according to the environmental temperature to obtain the value of α2. The example table is as follows:

[0086] Temperature -20 -19 ... 20 21 ... 40 41 ... 80 Compensation 0.00 0.01 ... 0.01 0.02 ... 0.10 0.11 ... 0.50

[0087] Calculate the value of α1 through a sliding window. The calculation formula is as follows:

[0088] α1 = avg(δ last_5 ) × ω

[0089] Wherein, δ last_5 represents the δ values of the last 5 measurements, and ω represents the weight coefficient, which is used to adjust the weights of historical data and current data.

[0090] Perform distance d calculation based on the above data. The calculation formula is as follows:

[0091] d = T flight × c

[0092] Wherein, c represents the speed of light, approximately 3 × 10 8 m / s.

[0093] The EKF algorithm is mainly based on the recursive prediction-update mechanism of Kalman filtering. The key steps when processing UWB data are as follows:

[0094] Initialization: Initialize the state vector (position, velocity) of the target and the error covariance matrix.

[0095] Establish a system model: mainly to describe the motion model of the target and the observation model that maps UWB ranging data to the target state.

[0096] State prediction: Predict the state and covariance at the next moment according to the system model. The calculation formula is as follows:

[0097]

[0098] Wherein, represents the predicted state, f represents the non-linear state transition function, represents the state estimate at the previous moment, u k-1 represents the control input; represents the predicted covariance, F k-1 represents the Jacobian matrix of the state transition function, P k-1 represents the covariance at the previous moment, Q k-1 represents the process noise covariance.

[0099] State update: Use the UWB measurement value to correct the state estimate, which specifically includes the following three steps:

[0100] Calculate the Kalman gain:

[0101]

[0102] In the formula, K k is expressed as the Kalman gain, H k is expressed as the Jacobian matrix of the observation function, R k is expressed as the observation noise covariance.

[0103] State correction:

[0104]

[0105] In the formula, is expressed as the corrected state estimate, z k is expressed as the observed value, and h is the non-linear observation function.

[0106] Covariance correction:

[0107]

[0108] In the formula, P k is the corrected covariance matrix, and I is the identity matrix;

[0109] Iteration: Repeat the prediction and update steps to update the state estimate in real time.

[0110] S4: Location algorithm processing link: successively perform relative coordinate conversion between the base station and the tag, dynamic coordinate system mapping, charging pile port orientation angle calculation, pose error compensation, and output the location result for the input signal;

[0111] As Figure 2 shown, the specific process of relative coordinate conversion is as follows:

[0112] When the vehicle equipped with the base station detects that the distance to the tag position meets a certain threshold, the attribute exchange between the base station and the tag is realized, and the coordinate system is converted to a coordinate system centered on the tag. The steps are as follows:

[0113] Trigger condition for attribute exchange: The base station continuously monitors the tag position, and when the tag enters the preset threshold range, the attribute exchange is triggered.

[0114] The base station coordinates change from the fixed coordinate point to the position point relative to the tag, and the tag coordinates change from the moving point to the center of the new coordinate system.

[0115] When the tag moves, the base station needs to dynamically calculate its own coordinates in the new coordinate system. The base station coordinate system is O-XY, and the tag coordinate system is O`-X`Y`. The conversion process can be realized through the transformation matrix, rotating a certain angle θ around the origin, and the rotation transformation matrix T旋转 As follows:

[0116]

[0117] Shift the origin from O to O`, and the translation transformation matrix T 平移 As follows:

[0118]

[0119] In the formula, (Δx, Δy) represents the coordinates of O` in the original coordinate system.

[0120] Calculation method of the total transformation matrix:

[0121] T = T 旋转 ×T 平移

[0122] Trigger mechanism: The system uses UWB ranging data to calculate the distance between the base station and the tag in real time. When the distance is less than a certain threshold, an attribute exchange instruction is sent; the instruction is sent through the UWB serial port to ensure low latency and high reliability. The relative coordinate conversion process is as Figure 2 shown.

[0123] After completing the relative coordinate system conversion, it is necessary to map the new coordinate data back to the original coordinate system and achieve dynamic mapping. To map the coordinates (x′, y′) in the new coordinate system back to the original coordinate system (x, y), it is necessary to calculate the inverse transformation matrix T -1 , and the calculation steps are as follows:

[0124] Inverse rotation transformation:

[0125]

[0126] Inverse translation transformation:

[0127]

[0128] Mapping formula:

[0129]

[0130] To achieve dynamic mapping, the transformation matrix T needs to be updated in real time, and the inverse matrix T needs to be calculated efficiently -1 .

[0131] During the calculation of the normal direction angle of the charging pile port, steps such as relative coordinate system conversion and dynamic coordinate system mapping need to be passed through. The calculation method is as follows:

[0132] Relative coordinate system conversion: For the relative coordinate system conversion that only includes rotation and translation, the vertex transformation matrix can be directly used to transform the normal, because the rotation matrix is an orthogonal matrix, and its inverse matrix is the transpose matrix.

[0133] Dynamic coordinate system mapping: Usually involves real-time updating of the local coordinate system to eliminate cumulative errors. In a dynamic coordinate system, the representation of the normal vector needs to consider the real-time changes of the coordinate system, and the normal vector may need to be updated frequently.

[0134] Calculation of the normal vector orientation angle: In the transformed coordinate system, it is first necessary to determine the normal vector of the charging pile port. If the normal vector is not correctly transformed during the conversion process, the transpose of the inverse matrix needs to be applied for correction first.

[0135] Azimuth angle φ: The angle between the projection of the normal vector on the horizontal plane (such as the XY plane) and the X axis. The calculation formula is as follows:

[0136]

[0137] In the formula, represents the component of the normal vector on the XY plane.

[0138] Vertical angle θ1: The angle between the normal vector and the horizontal plane. The calculation formula is as follows:

[0139]

[0140] In the formula, represents the component of the normal vector on the Z axis.

[0141] Combining the azimuth angle and the vertical angle, a complete representation of the normal vector orientation angle can be obtained.

[0142] The specific steps for calculating the normal vector orientation angle of the charging pile port are as follows:

[0143] Obtain the normal vector: Determine the normal vector of the charging pile port in the original coordinate system;

[0144] Transform the normal vector: According to the coordinate system transformation matrix, apply the transpose of the inverse matrix to transform the normal vector to ensure its perpendicularity in the transformed coordinate system;

[0145] Calculate the orientation angle: Use the transformed normal vector to calculate the azimuth angle and the vertical angle respectively;

[0146] Dynamic update: During the dynamic coordinate system mapping process, the normal vector and the orientation angle are updated in real time to adapt to the changes of the coordinate system.

[0147] Although the UWB data filtered by EKF suppresses a part of the noise, there may still be the following residuals:

[0148] System model error: The truncation error introduced by the EKF linearization assumption;

[0149] Observation model error: The deviation between the UWB ranging model and the actual situation;

[0150] Dynamic error caused by EKF tracking lag in high-dynamic scenarios;

[0151] Environmental error: ranging deviation in NLOS scenarios.

[0152] These errors will be transmitted to the final output through the state estimation process and need to be compensated by establishing kinematic constraints using gyroscope data. The gyroscope realizes attitude perception by measuring angular velocity, and its compensation effect is mainly reflected in the following three aspects:

[0153] Short-term motion constraint: Establish a short-term kinematic model using the gyroscope output, and the calculation formula is as follows:

[0154]

[0155] In the formula, θ, φ, and ψ respectively represent the pitch angle, roll angle, and yaw angle of the carrier, represents their respective change rates, and ω x , ω y , ω z represents the angular velocities measured by the gyroscope around the x-axis, y-axis, and z-axis of the carrier.

[0156] Error propagation suppression, suppressing EKF filter lag through high-frequency attitude updates;

[0157] Sensor characteristic complementarity, the gyroscope is good at capturing high-frequency dynamics and forms a complement with the low-frequency static estimation of EKF;

[0158] Perform pose error compensation through the following steps:

[0159] Time synchronization optimization: Use hardware-triggered synchronization of the UWB and gyroscope sampling clocks, and compensate for the timestamp deviation Δt sync according to the sampling times of the two, and the calculation formula is as follows:

[0160] Δt sync = PPS offset + sampling delay

[0161] In the formula, the PPS offset represents the time difference between the rising edge of the PPS signal and the system clock reference.

[0162] Dynamic weight adjustment: Design an adaptive weight function α:

[0163]

[0164] In the formula, the dynamic index can be selected as the angular velocity modulus or the acceleration change rate, and k is used to control the steepness of the mapping.

[0165] Outlier processing: Use a sliding window MAD detection for gyroscope data, and the calculation formula is as follows:

[0166] MAD = median(|ω - median(ω)|)

[0167] Where ω represents the gyroscope data points within the sliding window. M-estimation robust filtering is applied to the UWB data.

[0168] Finally, the position information of the charging pile port is output, and the relevant data is updated in real time.

[0169] S5: After the positioning algorithm processing link, a judgment is made on whether to continue positioning. If continuous positioning is required, return to the tag signal capture link. If continuous positioning is not required, the system is shut down.

[0170] In order to implement the above method, the present invention provides a charging pile port orientation positioning system, which adopts a modular integrated design at the hardware level to adapt to the vehicle-mounted environment. Specifically: The base station is equipped with an STM32 main control chip, and the UWB signal transceiver and DS-TWR protocol parsing are realized based on the on-board circuit. The power module uses 3.3V regulated power supply to reduce energy consumption. The tag integrates a 6-axis IMU gyroscope (including three-axis accelerometer and three-axis gyroscope), and cooperates with the STM32 chip to complete the motion attitude acquisition and DS-TWR two-way ranging. The metal shielding shell design takes into account both electromagnetic compatibility and waterproof performance (IP67 protection level). The dynamic coordinate mapping and pose compensation functions are realized by FPGA acceleration, and the coordinate transformation and Kalman filtering efficiency are improved by using the hardware parallel computing ability. In particular, both the base station and the tag are configured with GNSS-assisted timing modules, and the ranging error is controlled within the nanosecond level through the time synchronization mechanism, providing a timing reference for high-precision positioning. The hardware selection pays attention to the balance between cost and performance, and the core chips all use automotive-grade devices to ensure the reliable operation of the system in the wide temperature range of -40°C to 85°C.

[0171] The system software architecture realizes the high-precision positioning function through hierarchical software design. The core modules include two subsystems: dynamic coordinate mapping and pose error compensation. In the dynamic coordinate mapping layer, the UWB base station deployed on the vehicle and the charging interface tag interact with each other's ranging information in real time through the DS-TWR protocol. When the tag enters the preset activation range, the attribute exchange mechanism is triggered, and the relative coordinates of the tag are dynamically mapped to the global coordinate system, thereby outputting the normal orientation information of the charging pile port. The pose error compensation layer is based on the extended Kalman filter (EKF) algorithm, which fuses the UWB ranging data, gyroscope attitude information, and driving data of the vehicle wheel speedometer. Through dynamic weight adjustment and multi-source data fusion, the coordinate offset error caused by vehicle movement is compensated in real time. In particular, the system designs a two-stage coordinate conversion strategy: in the initial stage, a local coordinate system is quickly constructed through the RSSI signal strength, and then the accurate regression of the global coordinate system is realized by relying on the convergence of the EKF filter, ensuring the positioning stability and orientation calculation accuracy in dynamic scenarios.

[0172] Although embodiments of the present invention have been shown and described, it will be understood by those of ordinary skill in the art that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the present invention, and the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A method for positioning the orientation of a charging pile port based on UWB, characterized in that: It includes the following steps: S1: The system starts, and the UWB base station network is initialized. S2: Tag signal capture is performed, and signal validity detection is carried out. If a signal is detected, the next step is taken; if no signal is detected, it returns to the tag signal capture link. S3: The captured valid signal is solved by the DS-TWR algorithm and processed by the EKF algorithm, and it is judged whether the preset distance is reached. If so, it enters the positioning algorithm link; if not, it returns to the signal validity detection link. S4: Positioning algorithm processing link: The input signal is sequentially subjected to relative coordinate conversion between the base station and the tag, dynamic coordinate system mapping, calculation of the orientation angle of the charging pile port, pose error compensation, and output of the positioning result. S5: After the positioning algorithm processing link, it is judged whether to continue positioning. If continuous positioning is required, it returns to the tag signal capture link; if continuous positioning is not required, the system is shut down.

2. A method for positioning the orientation of a charging pile port based on UWB according to claim 1, characterized in that: In step S3, the DS-TWR algorithm is used to eliminate clock deviation, and the formula is: T flight =(T round1 ×T round2 -T reply1 ×T reply2 ) / (T round1 +T round2 +T reply1 +T reply2 +Δ) Where, T flight : Time of flight; T round1 : The difference between t1 and t0; T reply1 : The difference between t3 and t2; T round2 : The difference between t5 and t3; T reply2 : The difference between t4 and t1; Δ = α1×(T round1 - T reply2 ) + α2×(T round2 - T reply1 ) In the formula, Δ is the dynamic compensation term, and α1, α2 are compensation coefficients. The EKF algorithm is used to fuse UWB ranging data, gyroscope attitude data, and wheel speedometer data for state update, and the formula is: In the formula, is represented as the corrected state estimate, is represented as the predicted state, K k is represented as the Kalman gain, z k is represented as the observed value, and h is the non-linear observation function; dynamic mapping is achieved through the coordinate total transformation matrix, and the formula is: T = T 旋转 × T 平移 where T is the total transformation matrix, T 旋转 is the rotation transformation matrix, and T 平移 is the translation transformation matrix; In step S4, dynamic coordinate system mapping is realized by swapping the attributes of the base station and the tag.

3. The method for positioning the orientation of a charging pile port based on UWB according to claim 1, wherein: The triggering condition for the dynamic coordinate system mapping is that the distance between the base station and the tag is less than the preset threshold, and the attribute swap is realized by sending an instruction through the UWB serial port.

4. A charging pile port orientation positioning system for implementing the method according to any one of claims 1-3, characterized in that: It includes an in-vehicle UWB base station, a charging interface tag, and a dynamic coordinate mapping module; The tag supports ranging by the DS-TWR algorithm, and the ranging process includes obtaining the original timestamp, calculating the time interval, and improved clock deviation compensation. The system calculates the orientation angle through relative coordinate conversion. The azimuth angle φ is the angle between the projection of the normal vector on the horizontal plane and the X axis, and the calculation formula is as follows: In the formula, represents the component of the normal vector on the XY plane; Vertical angle θ1: The angle between the normal vector and the horizontal plane, and the calculation formula is as follows: In the formula, represents the component of the normal vector on the Z-axis.

5. The charging pile port orientation positioning system according to claim 4, characterized in that: The pose error compensation module adopts an adaptive weight function α: In the formula, the dynamic index can be selected as the angular velocity modulus or the acceleration change rate, and k is used to control the steepness of the mapping, combined with gyroscope MAD detection and UWB M-estimation robust filtering.

6. The charging pile port orientation positioning system according to claim 4, characterized in that: The system adopts a two-stage coordinate conversion strategy. In the initial stage, a local coordinate system is quickly constructed through the RSSI signal strength, and in the subsequent stage, accurate regression of the global coordinate system is achieved by relying on the convergence of EKF filtering.

Images