A positioning method based on array 4D millimeter wave radar

Abstract

The present invention relates to the field of positioning technology, and more specifically to a positioning method based on an array-type 4D millimeter-wave radar. The method comprises the following steps: forming an array-type radar ranging system with at least three 4D millimeter-wave radars, obtaining macro-range data of a target object from each radar unit in the array over a short period of time, performing slicing processing to capture the target object's micro-motion data within each time slice, and constructing time-sliced ​​macro-range monitoring data; obtaining the spatial coordinates of each distributed radar; calculating the unit vectors between the radars based on the spatial coordinates of each distributed radar and the time-sliced ​​macro-range monitoring data, and constructing an orthogonal vector space; and calculating the spatial coordinates of the target object using spherical equations in combination with the orthogonal vector space. The present invention can achieve high-precision target positioning and improve anti-interference capabilities.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and more particularly to a positioning method based on array-type 4D millimeter-wave radar. Background Art

[0002] As the number of engineering structures, such as bridges, tunnels, and slopes, continues to grow, their operational safety and stability are receiving widespread attention. Under the influence of the natural environment and long-term operational loads, structures are prone to cracks, settlement, and displacement, requiring high-precision, real-time, automated monitoring to ensure safety.

[0003] High-precision positioning technology is at the core of structural health monitoring. It can monitor minute changes in the surface and deep displacements of key bridge components and slopes, providing data support for disaster early warning. While technologies such as BeiDou / GNSS, total stations, LiDAR, and fiber Bragg grating (FBG) offer certain advantages over traditional manual inspections and low-precision sensors, they are still susceptible to environmental factors such as weather and obstruction.

[0004] 4D millimeter-wave radar, with its all-weather monitoring capabilities, can operate stably in rain, fog, and obstructions, providing a solid perception foundation for smart transportation. While 4D millimeter-wave radar offers advantages such as strong anti-interference, strong penetration, and all-weather operation, maintaining high-resolution perception in extreme environments like rain, fog, darkness, and obstructions, single radars are limited in terms of visual coverage, accuracy consistency, and spatial geometry, making them incapable of meeting the sub-meter or even centimeter-level high-precision positioning requirements required for large-scale structural health monitoring and vehicle-infrastructure collaboration.

[0005] Therefore, how to solve technical problems such as inconsistent benchmarks, spatial drift, and overlapping target interference in the process of multi-radar data fusion is an urgent problem that technicians in this field need to solve. Summary of the Invention

[0006] In view of this, the present invention provides a positioning method based on an array-type 4D millimeter-wave radar, which can achieve high-precision target positioning and improve anti-interference capability.

[0007] In order to achieve the above object, the present invention adopts the following technical solutions:

[0008] A positioning method based on an array-type 4D millimeter-wave radar comprises the following steps:

[0009] At least three 4D millimeter-wave radars are combined into an array radar ranging system to obtain macro distance data of the target from each radar unit in the array within a short period of time. The system then performs slicing processing to capture the micro-motion data of the target within each time slice and construct time-sliced ​​macro distance monitoring data.

[0010] Obtain the self-space coordinates of each radar unit in the array;

[0011] Based on the spatial coordinates of each radar unit and the time-sliced ​​macro-range monitoring data, the unit vectors between radars are calculated to construct an orthogonal vector space.

[0012] The spatial coordinates of the target object are calculated using spherical equations combined with orthogonal vector space.

[0013] Furthermore, the ranging process of each radar unit for the target object in each time slice includes:

[0014] A mixer is used to mix the radar's receiving and transmitting signals, and the down-converted signal retained after the mixing process is used as the intermediate frequency signal;

[0015] The distance data of the target object is obtained according to the intermediate frequency signal. The calculation formula is:

[0016] ;

[0017] Where c represents the transmission speed of millimeter wave radar, that is, the speed of light; represents the intermediate frequency signal; S represents the sweep slope, that is, the relationship between frequency change and time.

[0018] Furthermore, the ranging process of each radar unit for the target object in each time slice also includes:

[0019] The intermediate frequency signal is sampled and zeros are added to the sampling sequence to obtain a time domain sequence. After adding the Hanning window, a fast Fourier transform is performed in the distance dimension to calculate the spectrum spacing: , where N is the number of sampling points;

[0020] Refine the ranging resolution to millimeter level: , where S=B / Tc, B is the bandwidth, which is 4GHz, and Tc is the time period of the intermediate frequency signal;

[0021] Under high signal-to-noise ratio conditions, that is, SNR≧40dB, the peak frequency point and the frequency points adjacent to it are interpolated, a local parabola is fitted, and the peak position is recalculated. The frequency corresponding to the peak position reflects the distance to the target object.

[0022] Furthermore, the construction process of the orthogonal vector space includes:

[0023] Assume that the three distributed radar nodes are A, B, and C, and the target node is O or O'. The three radar nodes are in the same vertical plane, AB and BC are perpendicular to each other, BD is perpendicular to the ABC vertical plane, and the positive X-axis of the three-dimensional coordinate system is established with the BC direction, the positive Y-axis of the three-dimensional coordinate system is established with the BD direction, and the positive Z-axis of the three-dimensional coordinate system is established with the BA direction.

[0024] Calculate the unit vector ex pointing from radar node A to radar node B;

[0025] Calculate the vector from radar node A to radar node C and project it onto the unit vector ex to obtain the projection component i;

[0026] Calculate the vector ey perpendicular to ex, and calculate the length j of ey and the unit vector ey1;

[0027] Calculate the orthogonal unit vector ez of the unit vector ex and the unit vector ey1;

[0028] Construct an orthogonal vector space based on unit vectors ex, ey1, and ez.

[0029] Furthermore, the calculation process of the unit vector ex includes:

[0030] Calculate the vector AB between radar node A and radar node B: AB = {Bx-Ax, By-Ay, Bz-Az};

[0031] Calculate the length d of vector AB;

[0032] Calculate the unit vector ex = {AB.x / d, AB.y / d, AB.z / d} pointing from radar node A to radar node B;

[0033] Among them, Ax, Ay, and Az represent the component data of the coordinates of radar node A on the X-axis, Y-axis, and Z-axis; Bx, By, and Bz represent the component data of the coordinates of radar node B on the X-axis, Y-axis, and Z-axis; AB.x, AB.y, and AB.z represent the component data of the coordinates of the AB vector on the X-axis, Y-axis, and Z-axis.

[0034] Furthermore, the calculation process of the projection component i includes:

[0035] Calculate the vector coordinate temp from radar node A to radar node C: temp = {Cx-Ax, Cy-Ay, Cz-Az};

[0036] Calculate the projection i of the vector coordinate temp on the unit vector ex: i=temp.x×ex.x+temp.y×ex.y+temp.z×ex.z;

[0037] Among them, Cx, Cy, and Cz represent the component data of the coordinates of the radar node C on the X-axis, Y-axis, and Z-axis; temp.x, temp.y, and temp.z represent the component data of the temp vector coordinates on the X-axis, Y-axis, and Z-axis; ex.x, ex.y, and ex.z represent the component data of the ex unit vector coordinates on the X-axis, Y-axis, and Z-axis.

[0038] Furthermore, the calculation process of the unit vector ey1 includes:

[0039] Construct a vector perpendicular to the unit vector ex: ey={temp.xi×ex.x, temp.yi×ex.y, temp.zi×ex.z};

[0040] Calculate the length of the vector ey: j = sqrt(ey.x × ey.x + ey.y × ey.y + ey.z × ey.z);

[0041] Calculate the unit vector of vector ey: ey1={ey.x / j, ey.y / j, ey.z / j};

[0042] Among them, ey.x, ey.y, and ey.z represent the component data of the ey unit vector coordinate on the X-axis, Y-axis, and Z-axis.

[0043] Furthermore, the calculation process of the unit vector ez includes:

[0044] ez={ex.y×ey1.z-ex.z×ey1.y, ex.z×ey1.x-ex.x×ey1.z, ex.x×e1.y-ex.y×ey1.x};

[0045] Among them, ey1.x, ey1.y, and ey1.z represent the component data of the ey1 unit vector coordinates on the X-axis, Y-axis, and Z-axis.

[0046] Furthermore, the spatial coordinates of the target object are calculated using the spherical equations combined with the orthogonal vector space, including:

[0047] Use the spherical equation to solve for the coordinates of the unknown point:

[0048] x=(d1×d1-d2×d2+d×d) / (2×d);

[0049] y=(d1×d1-d3×d3+i×i+j×j-2×i×x) / (2×j);

[0050] temp_val=d1×d1-x×xy×y;

[0051] z=sqrt(temp_val);

[0052] Where d1, d2, and d3 represent the distances from the unknown point O or O' to radar nodes A, B, and C, respectively. Each distance corresponds to a sphere centered at A, B, and C, with radii d1, d2, and d3, respectively. x, y, and z represent the coordinates of the unknown point in the three-dimensional coordinate system. If temp_val < 0, it means that the three spheres do not intersect, indicating an anomaly.

[0053] Map the unknown point in the three-dimensional coordinate system back to the original coordinate system to obtain the coordinates of two symmetrical points:

[0054] P1={A.x+x×ex.x+y×ey1.x+z×ez.x, A.y+x×ex.y+y×ey1.y+z×ez.y, A.z+x×ex.z+y×ey1.z+z×ez.z};

[0055] P2={A.x+x×ex.x+y×ey1.xz×ez.x, A.y+x×ex.y+y×ey1.yz×ez.y, A.z+x×ex.z+y×ey1.zz×ez.z};

[0056] By calculating the relationship between the target object node and the coordinate axis, the real coordinate point in the coordinates of the two symmetrical points is determined as the real coordinate of the target object node.

[0057] Furthermore, after calculating the spatial coordinates of the target object, the following steps are also included:

[0058] Monitor the changes in the spatial coordinates of the target object over time to determine whether position movement occurs. If position movement occurs, determine whether the position movement value exceeds the limit based on the preset threshold. If it exceeds the limit, issue an early warning, otherwise continue monitoring.

[0059] It can be seen from the above technical solutions that compared with the prior art, the present invention has the following beneficial effects:

[0060] 1. The present invention achieves high-precision spatiotemporal synchronization and joint target modeling between radars through multi-radar collaborative ranging, enabling high-precision dynamic positioning and tracking of key targets within the monitoring area. It has good scalability and real-time performance, and can adapt to various complex working conditions and multi-source perception collaboration requirements. It solves technical difficulties such as inconsistent benchmarks, spatial drift, and interference from overlapping targets in the multi-radar data fusion process. It provides key support for scenarios such as vehicle-road collaborative perception in smart transportation, assisted and intelligent driving of automobiles, highway disaster warning, urban bridge and slope safety monitoring, airspace detection, and unmanned systems (robots and drones), and has broad engineering application prospects and strategic significance.

[0061] 2. This invention achieves "time-sliced ​​macro-range monitoring" by slicing the continuous macro-range changes of a target over a short period of time. This macro-range slicing sequence effectively filters out errors caused by random disturbances, transient interference, or slight target jitter, enhancing the system's real-time tracking capabilities for micro-moving or high-speed targets. This makes it particularly suitable for V2X dynamic traffic scenarios and industrial high-precision positioning tasks.

[0062] 3. This invention introduces orthogonal vectors and constructs a local coordinate system (ex, ey, ez) using three reference points (radar stations). This fusion of multi-point ranging data into a unified geometric space allows for the spatial coordinateization of target position measurements using spherical ranging. This orthogonal vector fusion algorithm ensures the stability and solvability of the coordinate system, avoiding numerical instability issues associated with nonlinear equations and significantly improving positioning accuracy. It also transforms complex three-dimensional spherical intersection problems into an algebraic solution, addressing the inadequacy of traditional triangulation methods in three dimensions.

[0063] 4. By returning two possible symmetric points (P1 and P2), the present invention can be combined with other information sources (such as altitude constraints, additional sensors) to further screen out the most reasonable target location and enhance system robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are merely embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any creative work.

[0065] Figure 1 Flowchart of the positioning method based on array 4D millimeter wave radar provided by the present invention;

[0066] Figure 2 Schematic diagram of a linear frequency modulated sawtooth wave signal;

[0067] Figure 3 is a schematic diagram of intermediate frequency signal;

[0068] Figure 4 A schematic diagram of a three-dimensional coordinate system between the array radar and the target object provided by the present invention;

[0069] Figure 5 A flow chart of array positioning calculation is provided for the present invention. DETAILED DESCRIPTION

[0070] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0071] like Figure 1 As shown, an embodiment of the present invention discloses a positioning method based on an array 4D millimeter wave radar, comprising the following steps:

[0072] At least three 4D millimeter-wave radars are combined into an array radar ranging system to obtain the macro distance data of each radar unit to the target in a short period of time. The system then performs slicing processing to capture the micro-motion data of the target within each time slice and construct time-sliced ​​macro monitoring data.

[0073] Obtain the self-space coordinates of each radar unit;

[0074] Based on the spatial coordinates of each radar unit and the time-sliced ​​macro-range monitoring data, the unit vectors between radars are calculated to construct an orthogonal vector space.

[0075] The spatial coordinates of the target object are calculated using spherical equations combined with orthogonal vector space.

[0076] This method uses multiple radars to accurately measure the distance to the same target, achieving high-precision positioning of the target object. The core of this method is to utilize positioning data from at least three points, through the coordinated operation of multiple radar systems, combined with the three-dimensional spatial coordinates of each radar to accurately locate the target object. Each radar has fixed three-dimensional coordinates in space. Based on this coordinate information, the radar system can infer the specific location of the target object through precise ranging calculations.

[0077] The advantage of this array radar system lies in its high-precision positioning capability, which effectively overcomes the limitations of traditional radar positioning methods, especially in complex environments. By utilizing multi-point measurement, it significantly improves positioning accuracy and anti-interference capabilities, enabling high-precision target positioning even in complex environments (such as urban high-rise buildings and mountainous areas). This technology has broad application prospects in areas such as autonomous driving, unmanned driving, and intelligent transportation systems, and is particularly important for improving vehicle safety and precise perception capabilities.

[0078] Furthermore, this method leverages the collaborative work of radar arrays, mitigating the limitations of a single radar, such as limited field of view and obstruction. This multi-radar system enables comprehensive and accurate acquisition of target information, improving overall system stability and reliability. Through optimized algorithms, target positioning accuracy can reach millimeter levels, which is crucial for safety monitoring, precise navigation, and decision-making.

[0079] The present invention specifically includes four parts: radar signal generation and transmission, signal processing and data fusion, distributed positioning calculation, and monitoring and early warning platform. The technical solution is described in detail as follows:

[0080] (1) Signal generation and emission of radar waves.

[0081] 1) Signal Generation: Millimeter-wave radar uses frequency modulated continuous wave (FMCW) to generate radar waveforms. This generates a linear frequency modulated (LFM) signal that sweeps the frequency within a specific frequency band.

[0082] 2) Signal transmission: The generated frequency modulated or pulsed signal is transmitted into the environment through the antenna array, usually at a certain beam angle.

[0083] The method for generating the FMCW signal waveform is shown in the following formula:

[0084] (1);

[0085] A represents the signal amplitude, which controls the signal strength. In practical systems, it is related to the transmit power and is usually a fixed value. β represents the signal's starting frequency (also called the carrier frequency), which is the signal's frequency at t = 0. β represents the chirp rate (frequency change rate), measured in Hz / s or GHz / s. It represents the rate at which the signal's frequency changes over time and determines the FMCW signal's sweep range (bandwidth) and sweep period. The frequency change within one cycle is: chirp rate × cycle time. T represents the time variable, representing the signal propagation time. In the formula, the signal's frequency changes linearly with time t. Describes the quadratic change in phase caused by a linear change in frequency, where is the accumulated phase change, whose first derivative is directly related to frequency.

[0086] The phase calculation formula is:

[0087] (2);

[0088] in, is the initial frequency, is the end frequency, k is the frequency modulation rate, (Signal duration T), t is time.

[0089] 3) Receive reflected signal:

[0090] Reflected wave reception: When radar waves encounter target objects (such as vehicles, pedestrians, obstacles, etc.) during propagation, they are reflected. The radar receiver receives these reflected signals through the antenna array.

[0091] Signal mixing: The received echo signal is mixed with the transmitted signal to generate a mixed signal, which is further used to analyze the target's distance, speed and other information.

[0092] (2) Signal processing and data fusion

[0093] Frequency offset calculation: By comparing the frequency difference between the transmitted and received signals, the radar system can calculate the relative speed and distance of a target. For FMCW radar, the frequency difference between the received and transmitted signals (also called frequency offset) can be used to calculate the target's distance. By measuring this frequency offset, the radar system can infer the target's distance.

[0094] Doppler Effect Calculation: Due to relative motion, the frequency of the reflected wave shifts (Doppler effect). By calculating this frequency shift, the relative velocity of the target can be inferred.

[0095] Specifically, for FMCW radar, the target distance can be calculated by measuring the signal time delay (or frequency offset). The ranging process of each distributed radar for the target in each time slice includes:

[0096] The 4D millimeter wave radar transmission signal is a linear frequency modulated sawtooth wave signal, that is, a Chirp signal, such as Figure 2 As shown in the figure, the ranging principle is as follows: where TC is the signal cycle time, TRF is the sweep time, and TI is the idle time. The chip signal frequency is 76GHz~81GHz. This embodiment uses the IWR1843 radar chip, whose ADC sampling frequency is 5MHz, while the frequency of the received signal is 76~81GHz. It is impossible to process the received signal directly, and a mixer is needed to mix the received signal and the transmitted signal. After mixing, the down-converted signal is retained as the intermediate frequency signal, as shown in the figure. Figure 3 (Intermediate frequency signal schematic diagram) as shown.

[0097] The intermediate frequency signal expression is:

[0098] (3);

[0099] and Represents the angular frequency of the transmitted and received signals. For linear frequency modulated continuous wave radar, the difference between the received and transmitted signal frequencies is typically related to the target's range or velocity. For linear frequency modulated signals, the frequency difference between the two signals is proportional to the target's range or velocity. While the transmitted signal frequency varies linearly with time, the received signal frequency can shift due to the target's relative velocity (Doppler effect) or distance (delay-induced frequency shift). Therefore, the frequency difference between the intermediate frequency signals is proportional to the target's range or velocity. T represents the time variable, indicating the instant of the signal. and Indicates the initial phase of the transmitted and received signals. The difference between the two reflects the distance to the target or the influence of other environmental characteristics. The phase difference contains distance information related to the target and plays a key role in pulse compression or precise ranging.

[0100] IF signal It is a time-varying sine wave whose frequency difference and phase difference contain the target's motion and position information.

[0101] From the intermediate frequency signal diagram, we can get:

[0102] (4);

[0103] S is the sweep slope, The receiving delay, d is the target distance, and the formula for measuring the target distance is obtained by sorting out the previous formula:

[0104] The calculation formula is:

[0105] (5);

[0106] Where c represents the transmission speed of millimeter wave radar, that is, the speed of light; represents the intermediate frequency signal; S represents the sweep slope, that is, the relationship between frequency change and time.

[0107] From formula (5), we can see that the intermediate frequency signal frequency Proportional to the target distance. Sampling the intermediate frequency signal to obtain a time domain sequence, adding a Hanning window and performing a range fast Fourier transform (Range-FFT) in the distance dimension, taking the number of sampling points N as the number of FFT points, we can get:

[0108] (6);

[0109] is the sampling rate of the intermediate frequency signal; is the sampling point number of the N-point FFT; S represents the frequency sweep slope, that is, the relationship between frequency change and time.

[0110] The sampling period and bandwidth The relational expression is as follows:

[0111] (7);

[0112] (8);

[0113] Substituting formula (7) into formula (6) and formula (8) yields:

[0114] (9);

[0115] From formula (9), we can see that when B is constant, d and That is, it is proportional to the distance gate, so the distance resolution can be obtained:

[0116] (10);

[0117] The maximum measurement range of the radar Limited by the IF signal frequency:

[0118] (11);

[0119] in, is the maximum intermediate frequency signal, in Complex 2x and real sampling mode, , is the sampling rate of the intermediate frequency signal.

[0120] Next, the principle of achieving sub-millimeter precision macro monitoring by the present invention is further explained:

[0121] 1) The ranging resolution of FMCW radar is determined by the Chirp signal bandwidth B: , c is the speed of light, and its value is , when B = 4GHz, the theoretical resolution is about 3.75cm; this is the limit of resolution (whether it can distinguish two close targets).

[0122] 2) Through large-scale FFT, the spectrum bin spacing is refined, and the equivalent ranging accuracy is improved, where: N = 4096, B = 4 GHz.

[0123] Sample the intermediate frequency signal, perform an N-point fast Fourier transform (FFT), and calculate the spectrum spacing: , where N is the number of sampling points and the ADC sampling rate used when sampling the intermediate frequency signal satisfies the Nyquist criterion.

[0124] Refine the ranging resolution to millimeter level: , where c is the speed of light, that is , S is the Chirp slope, S=B / Tc, B is the bandwidth, which is 4GHz, and Tc is the time period of the intermediate frequency signal;

[0125] Padding the sampling sequence with zeros increases the FFT length without increasing the sampling rate or observation time. Increasing the observation time Tc also naturally improves FFT resolution. A larger FFT length, N, reduces the minimum resolvable frequency difference, leading to higher ranging accuracy.

[0126] 3) Through spectral peak interpolation technology, super-resolution positioning of peak values ​​is achieved.

[0127] Parabolic / quadratic interpolation: Under high signal-to-noise ratio conditions (SNR ≥ 40 dB), the peak frequency and its adjacent frequency points are interpolated to fit a local parabola and recalculate the peak position. The frequency corresponding to the peak position reflects the distance to the target, achieving an accuracy gain of 1 / 100–1 / 1000 of the bin spacing.

[0128] Sparse reconstruction: Compressed sensing uses an iterative algorithm to approximate peak values ​​on an ultra-high-resolution grid, achieving localization after convergence within a few iterations. In low SNR environments, fitting based on a signal model can further reduce peak localization errors.

[0129] For an unbiased estimator, the lower bound of its ranging variance satisfies: , indicating that the larger the bandwidth B, the higher the SNR, and the higher the theoretical accuracy, where SNR represents the signal-to-noise ratio and B is the frequency bandwidth.

[0130] In the case of high SNR, high N-point FFT, and interpolation gain G, it can be approximately written as: , where N=4096, G=100, SNR=40dB, thus It can be reduced to below 0.3mm.

[0131] The specific ranging accuracy and conditions are shown in Table 1:

[0132] Table 1

[0133]

[0134] Through large-scale FFT (including zero-padding and extended observation), spectral peak super-resolution interpolation (parabola, CS, etc.), and Cramér–Rao lower bound analysis, the present invention improves the ranging accuracy of a 77GHz / 4GHz bandwidth FMCW radar from the basic centimeter level (3.75cm) to the submillimeter level (≤0.3mm) under high SNR and reasonable parameter configuration, meeting the requirements of high-precision ranging.

[0135] (3) Array positioning calculation.

[0136] Based on a multi-radar collaborative architecture, this method acquires the spatial coordinates of each radar node and target measurement data. By calculating the unit vectors between radars (e.g., the vector influence of radar A on radars B and C), an orthogonal vector space is constructed, and the vertical component is extracted to eliminate multipath interference. Furthermore, a one-dimensional dimensionality reduction algorithm is used, combined with the input distance from each radar node to the target, to calculate its spatial position coordinates. This algorithm can adapt to target displacement or environmental disturbances. The specific positioning process includes:

[0137] 1) If Figure 4As shown, assume that the three array radar nodes are A, B, and C, and the target node is O or O'; the three radar nodes are in the same vertical plane, AB and BC are perpendicular to each other, BD is perpendicular to the ABC vertical plane, and the X-axis positive direction of the three-dimensional coordinate system is established in the BC direction, the Y-axis positive direction of the three-dimensional coordinate system is established in the BD direction, and the Z-axis positive direction of the three-dimensional coordinate system is established in the BA direction. Assume that the coordinates of point A are (0, 0, 0.5), the coordinates of point B are (0, 0, 0), and the coordinates of point C are (0.5, 0, 0). The specific positioning process is as follows Figure 5 shown.

[0138] 2) Calculate the unit vector ex pointing from radar node A to radar node B:

[0139] Calculate the vector AB between radar node A and radar node B: AB = {Bx-Ax, By-Ay, Bz-Az};

[0140] Calculate the length of vector AB: d = sqrt(AB.x × AB.x + AB.y × AB.y + AB.z × AB.z);

[0141] Calculate the unit vector ex = {AB.x / d, AB.y / d, AB.z / d} pointing from radar node A to radar node B;

[0142] Among them, Ax, Ay, and Az represent the component data of the coordinates of radar node A on the X-axis, Y-axis, and Z-axis; Bx, By, and Bz represent the component data of the coordinates of radar node B on the X-axis, Y-axis, and Z-axis; AB.x, AB.y, and AB.z represent the component data of the coordinates of the AB vector on the X-axis, Y-axis, and Z-axis.

[0143] 3) Calculate the vector from radar node A to radar node C and project it onto the unit vector ex to obtain the projected component i. The specific calculation process includes:

[0144] Calculate the vector coordinate temp from radar node A to radar node C: temp = {Cx-Ax, Cy-Ay, Cz-Az};

[0145] Calculate the projection i of the vector coordinate temp on the unit vector ex: i=temp.x×ex.x+temp.y×ex.y+temp.z×ex.z;

[0146] Among them, Cx, Cy, and Cz represent the component data of the coordinates of the radar node C on the X-axis, Y-axis, and Z-axis; temp.x, temp.y, and temp.z represent the component data of the temp vector coordinates on the X-axis, Y-axis, and Z-axis; ex.x, ex.y, and ex.z represent the component data of the ex unit vector coordinates on the X-axis, Y-axis, and Z-axis.

[0147] 4) Calculate the vector ey perpendicular to ex, and calculate the length j of ey and the unit vector ey1, including:

[0148] Construct a vector perpendicular to the unit vector ex: ey={temp.xi×ex.x, temp.yi×ex.y, temp.zi×ex.z};

[0149] Calculate the length of vector ey: j = sqrt(ey.x × ey.x + ey.y × ey.y + ey.z × ey.z); When checking that A, B, and C are collinear, an error message appears because a plane cannot be defined. However, since points ABC are not collinear during installation, this is just a redundant safety check.

[0150] Calculate the unit vector of vector ey: ey1={ey.x / j, ey.y / j, ey.z / j};

[0151] Among them, ey.x, ey.y, and ey.z represent the component data of the ey unit vector coordinate on the X-axis, Y-axis, and Z-axis.

[0152] 5) Calculate the orthogonal unit vector ez of the unit vector ex and the unit vector ey1. The specific calculation formula is:

[0153] ez={ex.y×ey1.z-ex.z×ey1.y, ex.z×ey1.x-ex.x×ey1.z, ex.x×e1.y-ex.y×ey1.x};

[0154] The x-coordinate of the orthogonal vector ez = (ex.y ​​× ey1.z - ex.z × ey1.y);

[0155] The y coordinate of the orthogonal vector ez = (ex.z×ey1.x-ex.x×ey1.z);

[0156] The z coordinate of the orthogonal vector ez = (ex.x×e1.y-ex.y×ey1.x).

[0157] Among them, ey1.x, ey1.y, and ey1.z represent the component data of the ey1 unit vector coordinates on the X-axis, Y-axis, and Z-axis.

[0158] Construct an orthogonal vector space based on unit vectors ex, ey1, and ez.

[0159] 6) Use spherical equations combined with orthogonal vector space to calculate the spatial coordinates of the target object, including:

[0160] Use the spherical equation to solve for the coordinates of the unknown point:

[0161] x=(d1×d1-d2×d2+d×d) / (2×d);

[0162] y=(d1×d1-d3×d3+i×i+j×j-2×i×x) / (2×j);

[0163] temp_val=d1×d1-x×xy×y;

[0164] z=sqrt(temp_val);

[0165] Where d1, d2, and d3 represent the distances from the unknown point O or O' to radar nodes A, B, and C, respectively. Each distance corresponds to a sphere centered at A, B, and C, with radii d1, d2, and d3, respectively. x, y, and z represent the coordinates of the unknown point in the three-dimensional coordinate system. If temp_val < 0, the three spheres do not intersect (possibly due to errors or unsolvable solutions), and the output is abnormal.

[0166] 7) Map the unknown point in the 3D coordinate system back to the original coordinate system to obtain the coordinates of two symmetrical points:

[0167] P1={A.x+x×ex.x+y×ey1.x+z×ez.x, A.y+x×ex.y+y×ey1.y+z×ez.y, A.z+x×ex.z+y×ey1.z+z×ez.z};

[0168] P2={A.x+x×ex.x+y×ey1.xz×ez.x, A.y+x×ex.y+y×ey1.yz×ez.y, A.z+x×ex.z+y×ey1.zz×ez.z};

[0169] By calculating the relationship between the target object node and the coordinate axis, the real coordinate point in the coordinates of the two symmetrical points is determined as the real coordinate of the target object node.

[0170] 8) The actual coordinate point is determined based on the two coordinate points calculated in step 7). The relationship between the target point and the coordinate axis is calculated to determine which of the two symmetrical points is the actual position. The judgment principle is as follows:

[0171] like Figure 4 As shown in the established three-dimensional coordinate system, since the real target point is located in the front BD direction of the three radars ABC, the real target point is always in the positive half-axis direction of the Y axis of the spatial coordinate system. P2 is the mirror symmetry point of P1 about the ABC vertical plane, so the Y axis of P2 must be located in the negative half-axis of the Y axis. According to this principle, it can be clearly known that the Y axis components of the two coordinate points must be positive values ​​to be the target point.

[0172] (4) Monitoring and early warning platform.

[0173] Monitor the changes in the spatial coordinates of the target object over time to determine whether position movement occurs. If position movement occurs, determine whether the position movement value exceeds the limit based on the preset threshold. If it exceeds the limit, issue an early warning, otherwise continue monitoring.

[0174] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Reference can be made to the common and similar parts between the various embodiments. For the devices disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple, and the relevant parts can be referred to the method description.

[0175] The above description of the disclosed embodiments is intended to enable one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention is not limited to the embodiments shown herein but is intended to conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A positioning method based on array 4D millimeter wave radar, characterized in that: The following steps are involved: Three 4D millimeter-wave radars are combined into an array radar ranging system to obtain macro distance data of the target from each radar unit in the array within a short period of time. Slicing is then performed to capture the micro-motion data of the target within each time slice, and time-sliced ​​macro distance monitoring data is constructed. Obtain the self-space coordinates of each radar unit; Based on the spatial coordinates of each radar unit and the time-sliced ​​macro-range monitoring data, the unit vectors between radars are calculated and an orthogonal vector space is constructed. Specifically, the following steps are performed: Assume that the three array radar nodes are A, B, and C, and the target node is O or O'; the three radar nodes are in the same vertical plane, AB and BC are perpendicular to each other, BD is perpendicular to the ABC vertical plane, the BC direction establishes the positive X-axis of the three-dimensional coordinate system, the BD direction establishes the positive Y-axis of the three-dimensional coordinate system, and the BA direction establishes the positive Z-axis of the three-dimensional coordinate system; Calculate the unit vector ex pointing from radar node A to radar node B; Calculate the vector from radar node A to radar node C and project it onto the unit vector ex to obtain the projection component i; Calculate the vector ey perpendicular to ex, and calculate the length j of ey and the unit vector ey1; Calculate the orthogonal unit vector ez of the unit vector ex and the unit vector ey1; Construct an orthogonal vector space based on unit vectors ex, ey1 and ez; The spatial coordinates of the target object are calculated using spherical equations combined with orthogonal vector space. The calculation process of using spherical equations to solve the coordinates of unknown points includes: x=(d1×d1-d2×d2+d×d) / (2×d); y=(d1×d1-d3×d3+i×i+j×j-2×i×x) / (2×j); temp_val=d1×d1-x×xy×y; z=sqrt(temp_val); Where d1, d2, and d3 represent the distances from the unknown point O or O' to radar nodes A, B, and C, respectively. Each distance corresponds to a sphere centered at A, B, and C, with radii d1, d2, and d3, respectively. x, y, and z represent the coordinates of the unknown point in the three-dimensional coordinate system. If temp_val < 0, the three spheres do not intersect, indicating an anomaly. d represents the length of the vector AB between radar nodes A and B.

2. The positioning method based on array 4D millimeter wave radar according to claim 1, characterized in that: The ranging process of each distributed radar for the target in each time slice includes: A mixer is used to mix the radar's receiving and transmitting signals, and the down-converted signal retained after the mixing process is used as the intermediate frequency signal; The distance data of the target object is obtained according to the intermediate frequency signal. The calculation formula is: ； Where c represents the transmission speed of millimeter wave radar, that is, the speed of light; represents the intermediate frequency signal; S represents the sweep slope, that is, the relationship between frequency change and time.

3. The positioning method based on array-type 4D millimeter-wave radar according to claim 2, characterized in that: The ranging process of each radar unit for the target in each time slice also includes: The intermediate frequency signal is sampled and zeros are added to the sampling sequence to obtain a time domain sequence. After adding the Hanning window, a fast Fourier transform is performed in the distance dimension to calculate the spectrum spacing: , where N is the number of sampling points; Refine the ranging resolution to millimeter level: , where S=B / Tc, B is the bandwidth, which is 4GHz, and Tc is the time period of the intermediate frequency signal; Under high signal-to-noise ratio conditions, that is, SNR≧40dB, the peak frequency point and the frequency points adjacent to it are interpolated, a local parabola is fitted, and the peak position is recalculated. The frequency corresponding to the peak position reflects the distance to the target object.

4. The positioning method based on array 4D millimeter wave radar according to claim 1, characterized in that: The calculation process of the unit vector ex includes: Calculate the vector AB between radar node A and radar node B: AB = {Bx-Ax, By-Ay, Bz-Az}; Calculate the length d of vector AB; Calculate the unit vector ex = {AB.x / d, AB.y / d, AB.z / d} pointing from radar node A to radar node B; Among them, Ax, Ay, and Az represent the component data of the coordinates of radar node A on the X-axis, Y-axis, and Z-axis; Bx, By, and Bz represent the component data of the coordinates of radar node B on the X-axis, Y-axis, and Z-axis; AB.x, AB.y, and AB.z represent the component data of the coordinates of the AB vector on the X-axis, Y-axis, and Z-axis.

5. The positioning method based on array-type 4D millimeter-wave radar according to claim 4, characterized in that: The calculation process of the projection component i includes: Calculate the vector coordinate temp from radar node A to radar node C: temp = {Cx-Ax, Cy-Ay, Cz-Az}; Calculate the projection i of the vector coordinate temp on the unit vector ex: i=temp.x×ex.x+temp.y×ex.y+temp.z×ex.z; Among them, Cx, Cy, and Cz represent the component data of the coordinates of the radar node C on the X-axis, Y-axis, and Z-axis; temp.x, temp.y, and temp.z represent the component data of the temp vector coordinates on the X-axis, Y-axis, and Z-axis; ex.x, ex.y, and ex.z represent the component data of the ex unit vector coordinates on the X-axis, Y-axis, and Z-axis.

6. The positioning method based on array-type 4D millimeter-wave radar according to claim 5, characterized in that: The calculation process of the unit vector ey1 includes: Construct a vector perpendicular to the unit vector ex: ey={temp.xi×ex.x, temp.yi×ex.y, temp.zi×ex.z}; Calculate the length of the vector ey: j = sqrt(ey.x × ey.x + ey.y × ey.y + ey.z × ey.z); Calculate the unit vector of vector ey: ey1={ey.x / j, ey.y / j, ey.z / j}; Among them, ey.x, ey.y, and ey.z represent the component data of the ey unit vector coordinate on the X-axis, Y-axis, and Z-axis.

7. The positioning method based on array 4D millimeter wave radar according to claim 6, characterized in that: The calculation process of the unit vector ez includes: ez={ex.y×ey1.z-ex.z×ey1.y, ex.z×ey1.x-ex.x×ey1.z, ex.x×e1.y-ex.y×ey1.x}; Among them, ey1.x, ey1.y, and ey1.z represent the component data of the ey1 unit vector coordinates on the X-axis, Y-axis, and Z-axis.

8. The positioning method based on array 4D millimeter wave radar according to claim 7, characterized in that: After using the spherical equation to solve the coordinates of the unknown point, it also includes: Map the unknown point in the three-dimensional coordinate system back to the original coordinate system to obtain the coordinates of two symmetrical points: P1={A.x+x×ex.x+y×ey1.x+z×ez.x, A.y+x×ex.y+y×ey1.y+z×ez.y, A.z+x×ex.z+y×ey1.z+z×ez.z}; P2={A.x+x×ex.x+y×ey1.xz×ez.x, A.y+x×ex.y+y×ey1.yz×ez.y, A.z+x×ex.z+y×ey1.zz×ez.z}; By calculating the relationship between the target object node and the coordinate axis, the real coordinate point in the coordinates of the two symmetrical points is determined as the real coordinate of the target object node.

9. The positioning method based on array 4D millimeter wave radar according to claim 1, characterized in that: After calculating the spatial coordinates of the target object, it also includes: Monitor the changes in the spatial coordinates of the target object over time to determine whether position movement occurs. If position movement occurs, determine whether the position movement value exceeds the limit based on the preset threshold. If it exceeds the limit, issue an early warning, otherwise continue monitoring.

Images