A multi-target real-time positioning method based on millimeter wave
By acquiring point cloud data at the fire scene, clustering is performed using DBSCAN and BIRCH algorithms, combined with RKF Kalman filtering, which solves the problems of long positioning time and low non-line-of-sight detection accuracy of millimeter-wave radar at fire scenes, and realizes real-time multi-target positioning and high-precision tracking.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- JILIN UNIVERSITY
- Filing Date
- 2023-06-25
- Publication Date
- 2026-07-14
AI Technical Summary
In the process of indoor positioning at fire scenes, existing millimeter-wave radars increase computational load and process noise during the Kalman filtering process, resulting in longer positioning times and difficulty in accurately detecting objects or people under non-line-of-sight conditions.
By acquiring point cloud data from the fire scene, clustering is performed using the DBSCAN and BIRCH algorithms, and location tracking is achieved by combining RKF Kalman filtering. Real-time location tracking is realized using asynchronous multithreading, reducing computational load and process noise, and improving location accuracy.
It enables real-time multi-target localization at fire scenes, improves the target detection accuracy of millimeter-wave radar under non-line-of-sight conditions, reduces the impact of noise points, and lowers computational load and process noise.
Smart Images
Figure CN116774211B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to a positioning method, specifically a method for real-time positioning of multiple targets in an indoor scene based on millimeter-wave radar, belonging to the field of positioning and tracking. Background Technology
[0002] Traditional target recognition, positioning, and tracking technologies often utilize infrared and laser technologies. However, in indoor positioning and tracking scenarios with abundant smoke, such as fires, millimeter-wave seekers offer superior penetration of fog, smoke, and dust compared to infrared and laser optical seekers. Their anti-interference and anti-stealth capabilities are also superior to other microwave seekers, making millimeter-wave seekers more suitable for such environments. Since millimeter waves can penetrate certain non-metallic materials, they are installed inside smoke sensors. Millimeter-wave-based wireless sensing technology is highly versatile and can be implemented using millimeter-wave radar. Therefore, in fires with abundant smoke, current technologies utilize millimeter-wave radar data for indoor multi-scene detection and tracking. However, research on this method is currently very limited. Aside from models with low positioning accuracy, some existing models with higher accuracy convert polar coordinates to Kalman coordinates during Kalman filtering, increasing computational load and process noise, resulting in slower and more time-consuming positioning calculations.
[0003] Furthermore, during fire rescue, there is a certain probability that the human body will be blocked by objects. Therefore, even under line-of-sight conditions, radar cannot detect the human body. Moreover, in the research on radar indoor detection and tracking, the detection of objects or human bodies under non-line-of-sight conditions is often overlooked due to the difficulty of data processing, and existing research is even more limited. Therefore, it is necessary to accurately detect objects or human bodies under non-line-of-sight conditions. Summary of the Invention
[0004] In order to address the problem that existing millimeter-wave radar indoor positioning methods in the event of a fire convert polar coordinates to Kalman coordinates for calculation during the Kalman filtering process, which increases the computational load and process noise, resulting in a long positioning time and the difficulty of accurately detecting objects or people under non-line-of-sight conditions, this invention proposes a multi-target real-time positioning method based on millimeter waves.
[0005] The technical solution adopted in this invention is:
[0006] It includes the following steps:
[0007] S1. Use millimeter-wave radar to acquire point cloud data of a fire scene. The point cloud data consists of multiple frames and includes point cloud data under line-of-sight conditions and point cloud data under non-line-of-sight conditions.
[0008] S2. In the case of millimeter wave reflection, update all point cloud data under non-line-of-sight conditions to obtain updated point cloud data. Use all updated point cloud data and point cloud data under line-of-sight conditions as actual point cloud data. Then the actual point cloud data consists of multiple frames.
[0009] S3. Set the number of frames according to the frame rate of the actual point cloud data, and fuse the actual point cloud data sequentially according to the number of frames until the number of remaining actual point cloud data is less than the set number of frames. Discard the remaining actual point cloud data to obtain multiple frames of new actual point cloud data. Filter each frame of new actual point cloud data sequentially to obtain multiple frames of point cloud data 1. Use the DBSCAN algorithm and BIRCH algorithm to cluster the multiple frames of point cloud data 1 to obtain the final clustering result and new point cloud data. The new point cloud data consists of multiple frames. Determine the position of each human body based on the final clustering result and the new point cloud data.
[0010] S4. Based on the final clustering results, DCC data association processing is used to connect the data points in the two adjacent frames of new point cloud data obtained in S3, and then RKF Kalman filtering is used for localization and tracking to obtain the localization and tracking trajectory points of each human body.
[0011] S5. Utilize asynchronous multithreading to make real-time calls to S1 to S4 to complete real-time location tracking.
[0012] Furthermore, the specific process of S1 is as follows:
[0013] Start the IWR1843 millimeter-wave radar, configure the serial port between the computer and the millimeter-wave radar hardware, and establish a connection. Use the millimeter-wave radar to collect the structure of a fire scene, obtain multiple frames of point cloud data, and send the multiple frames of point cloud data to the computer. The computer stores the three-dimensional coordinates of each point cloud in each frame of point cloud data as a txt format, thus obtaining the current point cloud data of the fire scene. The point cloud data includes point cloud data under line-of-sight conditions and point cloud data under non-line-of-sight conditions.
[0014] Furthermore, the specific process of S2 is as follows:
[0015] S21. Place the millimeter-wave radar into the fire scene and obtain a two-dimensional plan view of the fire scene. Establish a Cartesian coordinate system on the two-dimensional plan view with the millimeter-wave radar as the origin, and assign different arrays to different positions on the two-dimensional plan view. The format of the array is [class, k x The class is 1 if a reflective surface exists at the current position, and 0 otherwise. x This represents the slope of the reflecting surface in the Cartesian coordinate system, which is 0 when there is no reflecting surface, resulting in the planar matrix;
[0016] S22. Assuming that any object in the current fire scene can serve as a reflector when the millimeter wave beam propagates, then the millimeter wave beam will be reflected once every time it encounters a reflector. The beam propagation path can be regarded as being composed of multiple line segments, each of which corresponds to a specific straight line equation. That is, the beam propagation path is composed of multiple straight line equations. At the same time, each reflector also corresponds to a straight line equation, and the reflection point is the intersection of the straight line equation of the current beam propagation path and the straight line equation of the reflector.
[0017] S23. In the case of millimeter wave reflection, in the Cartesian coordinate system, the line connecting the millimeter wave radar O(0,0) and a non-line-of-sight target point Q(x0,y0) detected by the millimeter wave radar O is taken as the initial beam, and the direction of the initial beam points to the target point Q, which is a point cloud data. Then, according to the mathematical geometric definition, the linear equation of the initial beam is:
[0018] y = k0x + b0
[0019] Where k0 is the slope of the initial beam. b0 is the intercept of the initial beam, b0 = 0;
[0020] The initial beam needs to travel a distance of...
[0021]
[0022] Define the wavefront as the leading edge of the initial beam. The parameter array of the wavefront is [xt, yt, k, b, xs, ys], where (xt, yt) are the wavefront coordinates, (xs, ys) are the source coordinates of the band, and the initial values of the wavefront coordinates and the source coordinates of the band are both (0,0). k is the slope of the line equation of the current band, with an initial value of k0; b is the intercept of the line equation of the current band, with an initial value of b0.
[0023] S24. Based on S21-S23, update the parameter array for the current wavefront position according to the wavefront position in the planar graph matrix and the actual propagation direction of the wave. The specific process is as follows:
[0024] ① If the class of a certain position on the initial beam propagation path is 0, it means that there is no reflecting surface. Then, the xt in the parameter array of the current wavefront position is increased by Δx, Δx = x0 / |x0|, the yt is increased by Δy, Δy = k*Δx, and the other parameters remain unchanged, resulting in a new parameter array [xt+Δx, yt+Δy, k, b, xs, ys];
[0025] ② If the class of a certain position on the initial beam propagation path is 1, it indicates that a reflecting surface exists at that position. According to the principle of light propagation, the coordinates of the current reflection point will become the coordinates of the new waveband source.
[0026] [xs′, ys′] = [xt, yt]
[0027] The distance the wave needs to travel is then updated to:
[0028]
[0029] Then update the remaining parameters of the parameter array according to either a or b below:
[0030] a. If k x If the value is greater than (-1 / k), then after reflection in the current band, it will propagate in the reverse direction in the x direction, i.e., Δx = -Δx. Update the parameters according to the following procedure:
[0031] Δx=-Δx
[0032] [xt′, yt′]=[xt+Δx, yt+Δy]
[0033]
[0034] b′=yt-k*xt
[0035] b. If k x If <(-1 / k), then after reflection in the current band, the x-direction remains unchanged, and the parameters are updated according to the following procedure:
[0036] [xt′, yt′]=[xt+Δx, yt+Δy]
[0037]
[0038] b′=yt-k*xt
[0039] ③ Update the distance the current wave needs to travel:
[0040]
[0041] If d" > 0, then repeat the process of S24 above; otherwise, output (xt, yt) where the current wave is located as the real target point of the non-line-of-sight target point Q(x0, y0);
[0042] For each non-line-of-sight target point, repeat steps S23-S24 to obtain all updated non-line-of-sight target points, i.e., real target points. This will give you all updated point cloud data. Use all updated point cloud data and point cloud data under line-of-sight conditions as the actual point cloud data.
[0043] Furthermore, the specific process of S3 is as follows:
[0044] S31. Place all actual point cloud data in the same Cartesian coordinate system, set the number of frames according to the frame rate of the actual point cloud data, and fuse the actual point cloud data in sequence according to the number of frames, that is, fuse every three frames into one frame, until the number of remaining actual point cloud data is less than the set number of frames, discard the remaining actual point cloud data, and obtain multiple frames of new actual point cloud data.
[0045] S32. Use a pass-through filter to filter each frame of new actual point cloud data in sequence to obtain the processed point cloud data.
[0046] S33. Use radius filtering to filter the point cloud data of each frame sequentially to obtain the processed point cloud data 1.
[0047] S34. Use the DBSCAN algorithm to cluster all point cloud data 1 to obtain clustered point cloud data. Count the number of points in each class of the clustered point cloud data, set a limit value for the number of points, delete classes with fewer points than the limit value, and obtain the clustering results.
[0048] S35. Obtain the centroid of each cluster in the clustering results. Perform BIRCH clustering on the centroid of each cluster. If several centroids cluster into one cluster, it means that the clusters corresponding to these centroids belong to the same class. Merge all point cloud data of the clusters to obtain one class. Solve for the centroid of the current class. One centroid represents one human body, thus obtaining the position of the human body. Repeat the above process to obtain the final clustering results and new point cloud data, as well as the centroid corresponding to each cluster in the clustering results, thus obtaining the position of each human body. The new point cloud data consists of multiple frames.
[0049] Furthermore, the specific process of S4 is as follows:
[0050] S41. Use DCC data association processing to perform correlation connection on the data points in the two adjacent new point cloud data obtained in S3:
[0051] The Cartesian coordinate system is converted to a polar coordinate system using the cosine theorem. In the polar coordinate system, each data point in the first frame of the new point cloud data obtained in S3 is taken as each human body in the millimeter-wave radar detection range of a certain fire scene in S1.
[0052] Determine which human body in the first frame of point cloud data corresponds to each data point in the second frame of point cloud data obtained from S3, that is, calculate the distance between the centroid of each human body in the first frame of point cloud data and the centroid of each human body in the second frame of point cloud data.
[0053] For each human body in the second frame of point cloud data, based on the distance between the current centroid and each centroid in the first frame of point cloud data, the two centroids corresponding to the shortest distance are associated. The above process is repeated to associate each data point in the second frame of point cloud data with the corresponding human body in the first frame of point cloud data. In this way, the human bodies in two adjacent frames of point cloud data in the multiple new frames of point cloud data obtained in S3 are associated to obtain the trajectory of each human body.
[0054] S42. Based on the trajectory of each human body, RKF Kalman filter is used for positioning and tracking to obtain the positioning and tracking trajectory points of each human body.
[0055] Furthermore, the specific process for calculating the distance between the centroid of each human body in the first frame of point cloud data and the centroid of each human body in the second frame of point cloud data is as follows:
[0056]
[0057] Where g is the distance between the centroids, r1 is the distance from the centroid of the current human body to the origin in the second frame of point cloud data, r2 is the distance from the centroid of the current human body to the origin in the first frame of point cloud data, θ1 is the angle between the line connecting the centroid of the current human body to the origin in the second frame of point cloud data and the x-axis, and θ2 is the angle between the line connecting the centroid of the current human body to the origin in the first frame of point cloud data and the x-axis.
[0058] Beneficial effects:
[0059] This invention utilizes millimeter-wave radar to acquire multiple point cloud data images of a fire scene. The point cloud data includes data under line-of-sight (LAS) and non-LAS conditions, thus obtaining the overall environment of the fire scene. A Cartesian coordinate system is established on a two-dimensional plan view of the fire scene, and the presence and slope of reflecting surfaces are marked at different locations on the plan view, resulting in a plan view matrix. Since any object within the fire scene can act as a reflecting surface during millimeter-wave beam propagation, the beam propagation path in the Cartesian coordinate system consists of multiple straight line equations. Each reflecting surface also corresponds to a straight line equation, and the reflection point is the intersection of the straight line equation of the current beam propagation path and the straight line equation of the reflecting surface. For the propagation of millimeter waves between radar and non-line-of-sight target points, considering the presence of millimeter wave reflection, the parameter array of the current wavefront position is updated in real time based on the wavefront position in the planar diagram matrix and the actual propagation direction of the wave. This continues until the required propagation distance d" ≤ 0, at which point the coordinates of the current wave are output as the updated non-line-of-sight target point, thus obtaining the accurate and true target point position. This process is repeated for all non-line-of-sight target points, and all updated point cloud data are combined with the point cloud data under line-of-sight conditions to form the actual point cloud data. All actual point cloud data are placed in the same Cartesian coordinate system. The frame rate of the actual point cloud data is set, and the actual point cloud data is fused sequentially according to the frame rate until the number of remaining actual point cloud data is less than the set frame rate. The remaining actual point cloud data is discarded, resulting in multiple frames of new actual point cloud data. These new actual point cloud data are then filtered sequentially using pass-through filtering and radius filtering to obtain multiple frames of point cloud data. Finally, using D... The BSCAN algorithm clusters multi-frame point cloud data, counts the number of points in each cluster, and deletes clusters with fewer points than a certain threshold, leaving only the target cluster as the clustering result. The centroid of each cluster is obtained, and BIRCH clustering is performed on each centroid. If several centroids cluster together, it means that the corresponding clusters belong to the same class. All point cloud data in the classes containing these centroids are merged to obtain a single class. The centroid of the current class is calculated, with each centroid representing a human body. This allows the determination of the location of each human body, resulting in the final clustering result and new point cloud data. Based on the final clustering result, DCC data association processing is used to connect the data points in adjacent frames of new point cloud data, and RKF Kalman filtering is used for localization and tracking to obtain the location points of the human body trajectory. Finally, asynchronous multithreading is used to call the above steps in real time, enabling real-time localization and tracking.
[0060] This invention utilizes frame fusion to make the sparse point cloud of the target human body relatively denser, reducing the probability of effective points being filtered out as noise points, thus improving the target localization accuracy of millimeter-wave radar to a certain extent. By fusing DBscan and Birch clustering algorithms, combined with effective denoising techniques such as frame fusion direct-pass filtering and radius filtering, the target localization accuracy of millimeter-wave radar is significantly improved. Furthermore, the design and iteration of Kalman filtering are completed in polar coordinates, reducing the computational load and process noise in the human body tracking process. This invention proposes a method for human body localization under non-line-of-sight conditions where echo reflection exists, enabling millimeter-wave radar to detect objects or human bodies with relatively high accuracy under non-line-of-sight conditions. Attached Figure Description
[0061] Figure 1 This is a legend of point cloud update under non-line-of-sight conditions;
[0062] Figure 2 This is a schematic diagram of the straight-line equations of the beam before and after reflection;
[0063] Figure 3 This is a schematic diagram of a frame of point cloud data;
[0064] Figure 4 This is a schematic diagram of the three frames of data after frame fusion;
[0065] Figure 5 This is a schematic diagram of actual point cloud data from the experimental test;
[0066] Figure 6 This is a schematic diagram of the point cloud data after pass-through filtering;
[0067] Figure 7 This is a schematic diagram of the point cloud data after radius filtering;
[0068] Figure 8 This is a schematic diagram of the final point cloud data to be clustered;
[0069] Figure 9 This is a schematic diagram of the preliminary clustering results from DBscan;
[0070] Figure 10 This is a diagram illustrating the DBscan clustering results;
[0071] Figure 11 This is a diagram illustrating the results of DBscan clustering into two classes;
[0072] Figure 12 This is a schematic diagram showing the centroid results for each cluster in DBscan clustering;
[0073] Figure 13 This is a schematic diagram showing the results of Birch clustering where the centroids are distinct.
[0074] Figure 14This is a schematic diagram showing the results of Birch clustering where the centroids belong to the same class; Detailed Implementation
[0075] Specific implementation method one: Combining Figures 1-14 This embodiment describes a multi-target real-time positioning method based on millimeter waves, which includes the following steps:
[0076] S1. Use millimeter-wave radar to acquire point cloud data of a fire scene. The point cloud data consists of multiple frames and includes point cloud data under line-of-sight conditions and point cloud data under non-line-of-sight conditions.
[0077] The TI IWR1843 millimeter-wave radar is powered on. The serial port between the computer and the radar is configured and established. The radar is then used to collect data on the structure of a fire scene. Because the size of the fire scene exceeds the measurement range of the radar, it's impossible to measure the point cloud data of the fire scene in a single coordinate system. Furthermore, objects in the environment may obstruct each other, preventing the radar from completely scanning all objects. Therefore, the radar needs to collect multiple frames of point cloud data to obtain the overall environment of the fire scene. These multiple frames of point cloud data are sent to the computer. The computer stores the 3D coordinates (x, y, z) of each point cloud in each frame in a dictionary, in TXT format, for later processing. This yields the point cloud data for the fire scene. Since this data represents the overall environment of the fire scene, it includes point cloud data from both line-of-sight and non-line-of-sight conditions.
[0078] S2. In the case of millimeter wave reflection, update all point cloud data under non-line-of-sight conditions to obtain updated point cloud data. Use all updated point cloud data and point cloud data under line-of-sight conditions as actual point cloud data. Then the actual point cloud data consists of multiple frames.
[0079] During fire rescue operations, there is a certain probability that a human body will be obstructed by objects, preventing radar sensors from detecting the target at line-of-sight. Therefore, target detection is necessary in non-line-of-sight situations. Since this invention is primarily used in indoor environments where millimeter waves have a short propagation distance, it mainly considers the possibility of echo reflection. For situations involving non-line-of-sight targets, the overall principle is as follows:
[0080] S21. Mimicking the geometric optics propagation model, place a millimeter-wave radar within the fire scene to obtain a two-dimensional plan view of the fire scene. Establish a Cartesian xoy coordinate system on the two-dimensional plan view with the millimeter-wave radar as the origin (this coordinate system is consistent with the coordinate system established by the millimeter-wave radar hardware itself). Assign different arrays to different positions on the two-dimensional plan view, with the array format being [class, k...]. x The class is 1 if a reflective surface exists at the current position, and 0 otherwise. x This represents the slope of the reflecting surface in the Cartesian xoy coordinate system, which is 0 when there is no reflecting surface, resulting in the planar matrix.
[0081] S22. Assuming any object within the current fire scene can serve as a reflector during millimeter-wave beam propagation, the beam will reflect once upon encountering each reflector, resulting in a phase shift each time it passes through a reflection. Therefore, the beam propagation path can be considered as composed of multiple line segments, each corresponding to a specific linear equation. Similarly, each reflector also corresponds to a linear equation. Since the map of the fire scene and the established coordinate system are known, the linear equation corresponding to each reflector is also known. The reflection point is the intersection of the linear equation of the current beam propagation path and the linear equation of the reflector.
[0082] S23. In the case of millimeter wave reflection, in the Cartesian xoy coordinate system, the line connecting the millimeter wave radar O(0,0) and a specific non-line-of-sight target point Q(x0,y0) detected by the millimeter wave radar O is taken as the initial beam. The direction of the initial beam points towards the target point Q, which is a point cloud data. According to the mathematical geometric definition, the linear equation of the initial beam is:
[0083] y = k0x + b0
[0084]
[0085] Where k0 is the slope of the initial beam and b0 is the intercept of the initial beam.
[0086] The initial beam needs to travel a distance of...
[0087]
[0088] Define the wavefront as the leading edge of the initial beam. The parameter array of the wavefront is [xt, yt, k, b, xs, ys], where (xt, yt) are the wavefront coordinates, (xs, ys) are the source coordinates of the band, and the initial values of the wavefront coordinates and the source coordinates of the band are both (0,0). k is the slope of the line equation of the current band, with an initial value of k0; b is the intercept of the line equation of the current band, with an initial value of b0.
[0089] S24. Based on S21-S23, according to the wavefront position in the planar diagram matrix and the actual propagation direction of the wave, the parameter array of the current wavefront position is updated in real time. The specific process is as follows:
[0090] Set Δx = x0 / |x0|, xt always increases along the beam propagation direction at intervals (steps) of Δx, and yt increases at intervals of Δy = k*Δx.
[0091] ① If the class of a certain position on the initial beam propagation path is 0, it means that there is no reflecting surface. Then, the xt in the parameter array of the current wavefront position is increased by Δx, the yt is increased by Δy, and the other parameters remain unchanged, resulting in a new parameter array [xt+Δx, yt+Δy, k, b, xs, ys].
[0092] ② If the class of a certain position on the initial beam propagation path is 1, it indicates that a reflecting surface exists at that position. According to the principle of light propagation, the coordinates of the current reflection point will become the coordinates of the new waveband source.
[0093] [xs′, ys′] = [xt, yt]
[0094] The distance the wave needs to travel is then updated to:
[0095]
[0096] Then update the remaining parameters of the parameter array according to either a or b below:
[0097] a. If k x >(-1 / k), k x Let be the slope of the current reflecting surface, and k be the slope of the wavefront parameters. Then, after reflection of the current waveband, it will propagate in the reverse direction in the x-direction, i.e., Δx = -Δx. Update the parameters according to the following process:
[0098] Δx=-Δx
[0099] [xt′, yt′]=[xt+Δx, yt+Δy]
[0100]
[0101] b′=yt-k*xt
[0102] b. If k x <(-1 / k), k x Let be the slope of the current reflecting surface, and k be the slope of the wavefront parameters. After reflection of the current waveband, the parameters remain unchanged in the x-direction and are updated according to the following process:
[0103] [xt′, yt′]=[xt+Δx, yt+Δy]
[0104]
[0105] b′=yt-k*xt
[0106] ③ Update the distance the current wave needs to travel, and determine if it is positive:
[0107]
[0108] If d" > 0, then repeat the process of S24 above; otherwise, output (xt, yt) where the current wave is located as the true target point of the non-line-of-sight target point Q(x0, y0), thus obtaining the accurate target point position.
[0109] like Figure 1 As shown, when the millimeter-wave beam propagates to point Q′, d" < 0, and Q′ at this point is the actual coordinate of the target point (the human body obscured by the object). real (x real y real ), Q real (x real y real Replace the non-line-of-sight target point Q(x0,y0).
[0110] For each non-line-of-sight target point, repeat steps S23-S24 to obtain all updated non-line-of-sight target points, i.e., real target points. This will give you all updated point cloud data. Use all updated point cloud data and point cloud data under line-of-sight conditions as the actual point cloud data.
[0111] If a three-dimensional plane needs to be considered, the three-dimensional plane can be mapped to three two-dimensional planes for separate calculations.
[0112] S3. Set the number of frames according to the frame rate of the actual point cloud data, and fuse the actual point cloud data sequentially according to the number of frames until the number of remaining actual point cloud data is less than the set number of frames. Discard the remaining actual point cloud data to obtain multiple frames of new actual point cloud data. Filter each frame of new actual point cloud data sequentially to obtain multiple frames of point cloud data 1. Use the DBSCAN algorithm and BIRCH algorithm to cluster the multiple frames of point cloud data 1 to obtain the final clustering result and new point cloud data. The new point cloud data consists of multiple frames. Determine the position of each human body based on the final clustering result and the new point cloud data.
[0113] Step 1: Data Frame Fusion
[0114] Since the data points collected in each frame of point cloud data are relatively sparse, this invention places all actual point cloud data in the same Cartesian coordinate system, sets the number of frames according to the frame rate of the actual point cloud data, and fuses the actual point cloud data sequentially according to the number of frames, that is, fused every three frames into one frame, until the number of remaining actual point cloud data is less than the set number of frames, discarding the remaining actual point cloud data, and obtaining multiple new actual point cloud data frames so that they can form complete environmental point cloud data. For example, for a frame rate of 250ms, 3-5 frames of data need to be fused. Here, 3 frames are obtained, so 1-3 are fused into one frame, 4-6 are fused into one frame, and so on, until the number of remaining actual point cloud data is less than the set number of frames, discarding the remaining actual point cloud data, and obtaining multiple new actual point cloud data frames. Figure (3) is a diagram of one frame of point cloud data, and Figure (4) is a diagram of three frames of data after frame fusion.
[0115] Step 2: Noise removal using pass-through filtering and radius filtering
[0116] Each frame of new actual point cloud data is filtered sequentially using a pass-through filter to obtain processed point cloud data. The specific process is as follows:
[0117] The process involves reading in all new actual point cloud data, creating a filter object, setting the filter field range, iterating through each point in the point cloud data, performing filtering, and deleting points whose values are outside the range. The remaining points constitute the filtered point cloud; the filtering result is then saved, i.e., the processed point cloud data. The result is as follows: Figure 5 and Figure 6 As shown.
[0118] Direct-pass filtering: This is used to filter out noise points introduced by the millimeter-wave radar itself. When acquiring point cloud data, some noise points will inevitably appear in the point cloud data due to the influence of millimeter-wave radar accuracy, operator experience, environmental factors, electromagnetic wave diffraction characteristics, changes in the surface properties of the measured object, and the data stitching and registration process. The advantage of direct-pass filtering is that it can determine the area of the point cloud causing interference based on the environment (such as disturbance from irrelevant objects in a certain area of the scene) or the situation of the millimeter-wave radar (changes in the surface properties of the equipment), and thus directly filter out the interfering point cloud through direct-pass filtering.
[0119] Radius filtering is used to filter the processed point cloud data of each frame sequentially to obtain processed point cloud data 1. The specific process is as follows:
[0120] Read in the processed point cloud data → Create a filter object → Set the filter radius and calculate the number of other points within the radius of each point → Points with fewer than a certain set threshold of other points within the radius will be filtered out. Perform filtering → Save the filtering result, i.e., the processed point cloud data 1. The results are shown in Figure (6) and Figure (7).
[0121] Radius filtering: While millimeter-wave radar can automatically filter out point clouds containing static objects, a small number of static point clouds will inevitably still be acquired. Therefore, radius filtering is used to remove static outliers. This invention uses radius filtering to remove spatial noise points, avoids inter-frame jumps, and improves accuracy.
[0122] Step 3: DBSCAN-BIRCH Clustering Recognition Optimization Algorithm
[0123] DBSCAN is a density-based clustering algorithm. Its advantage lies in that it does not require prior knowledge of the number of clusters to be formed. This type of density-based clustering algorithm generally assumes that the number of clusters can be determined by the density of the sample distribution. Samples of the same cluster are closely connected; that is, any sample in a given cluster will have other samples of the same cluster nearby. Based on this, this invention designs a DBSCAN-BIRCH clustering identification optimization algorithm. Its specific implementation idea is as follows:
[0124] First, the DBSCAN algorithm is used to cluster all point cloud data 1 to obtain clustered point cloud data. The number of points in each class of the clustered point cloud data is counted. This is done because the frame fusion performed in the early stage to increase the number of points in each frame inevitably causes the fusion of noise points, which easily clusters noise points into one class. Therefore, the number of points in each class is counted. Since the number of points in a class of noise points is less than that in the target class, this invention sets a limit value for the number of points. Classes with fewer points than the limit value are deleted, that is, noise classes are deleted, leaving only the target class as the clustering result. This is equivalent to completing further denoising work and improving the accuracy of clustering. The results are shown in Figure (9) and Figure (10).
[0125] Obtain the centroid of each cluster in the clustering results, which is the centroid of each human body in each frame of the fire scene. Perform BIRCH clustering on the centroid of each cluster. If several centroids cluster into one cluster, it means that the clusters corresponding to these centroids belong to the same class. Merge all point cloud data of the clusters to obtain one class. Solve for the centroid of the current class. One centroid represents one human body, which means obtaining the position of the human body. Repeat the above process to obtain the final clustering results and new point cloud data, as well as the centroid corresponding to each cluster in the clustering results, which means obtaining the position of each human body. The new point cloud data consists of multiple frames.
[0126] This invention cleverly combines the BIRCH algorithm for further clustering. BIRCH stands for Balanced Iterative Reducing and Clustering Using Hierarchies, which uses hierarchical methods to cluster and reduce data.
[0127] As shown in Figure (11), taking the two clusters of the new clustering results obtained by DBSCAN as an example, BIRCH(Threshold,Ranching_factor):
[0128] Threshold: Maximum radius threshold. Set the appropriate value according to the application scenario. This value determines whether the two clusters in Figure (11) are of the same type.
[0129] Ranching_factor: The maximum number of leaf clustering features (CFs) allowed. Since DBSCAN and BIRCH are combined, there are fewer BIRCH sample points using centroids as point cloud data. Therefore, the maximum number of leaf clustering features (CFs) allowed only needs to be greater than 1.
[0130] The Threshold value should be set according to different application scenarios: In densely populated scenarios, where individuals are close together, a smaller Threshold value should be used to prevent centroids that do not belong to the same category from clustering into one category, while still allowing multiple centroids that are very close to each other to be clustered into one category. This determines whether two very close centroids belong to the same category. In sparsely populated scenarios, where individuals are far apart, the Threshold value does not need to be too small and can be set slightly larger, thus allowing centroids that originally belong to the same category to be clustered into one category using BIRCH.
[0131] When BIRCH clustering is performed, if the results show that the class labels of each class are different, then BIRCH clustering ends. Figure 13 As shown. If the results show that some classes have the same class label, then the clusters with the same class label are merged, that is... Figure 14 As shown, the centroid is then calculated, which is the final cluster centroid point, thus obtaining the corresponding human body's location.
[0132] Taking the movement of two people as an example for testing, we processed the point cloud data through the DBscan-BIRCH clustering algorithm and displayed the cluster points of all frames on a single canvas. The test results are shown in Figure (15). It can be seen that the denoising and clustering localization effects of the present invention are highly accurate.
[0133] S4. Based on the final clustering results, DCC data association processing is used to connect the data points in the two adjacent new point cloud data obtained in S3, and then RKF Kalman filtering is used for positioning and tracking to obtain the positioning and tracking trajectory points of each human body.
[0134] This invention employs a recursive Kalman filter algorithm for indoor personnel tracking using a constant velocity model, while also considering an acceleration model under random noise. The processes S1-S3 are performed in Cartesian coordinates, while S4 maintains the raw data from detection to tracking in polar coordinates. S4 is designed to locate and track the positions of people moving within the fire scene space, ensuring accurate and reliable measurement data.
[0135] First, DCC data association processing is used to associate the centroids in the new frames with existing human figures. An RKF Kalman filter is then designed to predict and track the positions of multiple human figures, resulting in the final human motion trajectory. The specific process is as follows:
[0136] Step 1: DCC data association processing:
[0137] Kalman filtering can only track one person at a time. Since multiple people may appear at a fire scene at any time in reality, data association processing is required to turn a single-person Kalman filter into a multi-person tracker.
[0138] First, the Cartesian coordinate system is converted to a polar coordinate system using the cosine theorem. In the polar coordinate system, each data point in the first frame of the new point cloud data obtained in S3 is taken as each human body in the millimeter-wave radar detection range of a certain fire scene in S1. Each human body is labeled, that is, one human body corresponds to the trajectory of a type of human body.
[0139] Determine which human body in the first frame of point cloud data corresponds to each data point in the second frame of point cloud data obtained from S3, i.e., calculate the distance between the centroid of each human body in the first frame of point cloud data and the centroid of each human body in the second frame of point cloud data:
[0140]
[0141] Where g is the distance between the centroids, r1 is the distance from the centroid of the current human body to the origin in the second frame of point cloud data, r2 is the distance from the centroid of the current human body to the origin in the first frame of point cloud data, θ1 is the angle between the line connecting the centroid of the current human body to the origin in the second frame of point cloud data and the x-axis, and θ2 is the angle between the line connecting the centroid of the current human body to the origin in the first frame of point cloud data and the x-axis.
[0142] For each human body in the second frame of point cloud data, based on the distance between the current centroid and each centroid in the first frame of point cloud data, the two centroids corresponding to the shortest distance are associated. This process is repeated to associate each data point in the second frame of point cloud data with the corresponding human body in the first frame of point cloud data. This process is repeated to associate the human bodies in adjacent frames of point cloud data obtained in S3, thus obtaining the trajectory of each human body. This step is named DCC (Distance-Centroid Correlation) data association processing in this invention.
[0143] Step 2: Based on the trajectory of each person, use an RKF Kalman filter for localization and tracking to obtain the localization and tracking trajectory points of each person:
[0144] This invention selects the RKF (recursive) Kalman filter for final localization and tracking. After DCC data association, the system state at the k-th step of the centroid of each human body in the polar coordinate system can be expressed as:
[0145]
[0146] The motion state model and observation model of the human body (center of mass) can be established as follows:
[0147] x k =Fx k-1 +Q
[0148] y k =Hx k +R
[0149]
[0150]
[0151] Where F is the state transition matrix, H is the observation matrix, and x k It represents the motion state of the human body at time k, y k δ represents the observed value of the human body at time k. Q and R are the system noise covariance matrix and measurement noise covariance matrix, respectively. These two parameters are designed based on specific circumstances and are not fixed. Their values can be changed in the code for debugging to select the one with good performance. δ is the sampling time interval of the millimeter-wave radar sensor, set to 50 milliseconds.
[0152] The process of the RKF Kalman filter is as follows:
[0153] Step 1: Initialization Steps
[0154] The initial state of the human body is State error, initial covariance matrix Q,R.
[0155] Set the mean value X0+ and covariance matrix P0+ of the state to k=0.
[0156] Step 2: Prediction Steps
[0157] Predicted values of human body condition:
[0158]
[0159] Prior covariance matrix:
[0160]
[0161] Project the state and covariance at k-1(X+k-1,P+k-1) one step forward to obtain the prior estimate at k(Xk,Pk).
[0162] Step 3: Update Steps
[0163] Based on prior estimates, the actual measured value z k The result is compared with the predicted measurement to obtain an improved posterior estimate. The residual y is then obtained. k Total error S k :
[0164]
[0165]
[0166] symbol z k and S k These are the measurement vector and the new covariance, respectively. The Kalman gain K is calculated recursively. k And calculate the state of the current data frame. (Correction value) and updated state error covariance matrix
[0167]
[0168]
[0169]
[0170] Step 4: In the next moment, let K = K + 1, and repeat Step 2 and Step 3 in sequence. Finally, the location tracking trajectory points of each human body are obtained.
[0171] Recalculating the Kalman gain and error covariance can make the estimation system more robust and practical. If no measurement data is available, the estimated values are used as the updated values.
[0172] S5. Utilize asynchronous multithreading to make real-time calls to S1 to S4, thereby performing real-time location tracking.
[0173] The backend of the system of this invention uses the Django framework, with the backend database as the server and the above algorithm model as the client. The number of frames transmitted and processed each time is set for the clustering recognition algorithm and the tracking algorithm, respectively. Real-time communication and database updates can be performed through TCP-Socket, thereby achieving real-time positioning and tracking effects.
[0174] This invention achieves asynchronous multithreading through four calls to the view function. This invention has a total of four threads.
[0175] The execution logic of multithreading is as follows:
[0176] Thread 1 (calls S1 and S2)
[0177] First, we trigger thread 1. The system backend sends a command to the millimeter-wave radar board, and the millimeter-wave radar begins to collect structural data from the fire scene, transmitting the data back to the PC in frames. Real-time preprocessing (S1 and S2) is then performed on the PC, and the data collected by the millimeter-wave radar is stored in the database. After the millimeter-wave radar's data acquisition algorithm collects the data, the system uses TCP to send the collected data to the database storage algorithm. In this TCP connection, the millimeter-wave radar's data acquisition algorithm is the client, and the database storage algorithm is the server. A non-persistent connection is used for synchronous access; each time the millimeter-wave radar sends a frame of data to the database storage algorithm, the TCP connection is established and then closed.
[0178] Thread 2 (calling S3)
[0179] After thread 2 is triggered, data processing begins. The system backend first retrieves the raw data from the database and then writes it frame by frame to the file 101.txt. The synchronization method used is to check whether the next frame of raw data to be written already exists in the database. If the next frame exists, then the current frame should have been collected from the radar. If the next frame does not exist, the query is repeated until the next frame exists. After the next frame is found, the current frame is written to the file 101.txt. The algorithm in S3 executes sequentially, using the data in 101.txt for processing. The processed data is then divided into non-centroid coordinates (Is_Final = 1) and centroid coordinates (Is_Final = 2), and stored sequentially in the database, with the Is_Final field value marked. Data type differentiation is performed in subsequent threads. After this thread is triggered, it executes continuously in a while 1 loop. After the next frame of data is retrieved from the database, it is stored in the file 101.txt by overwriting the old data. All threads terminate when the algorithm stops running.
[0180] Thread 3 (calls S4)
[0181] Thread 3 is similar to Thread 2. The algorithm file called by the system backend is the DCC-RKF algorithm in S4. The difference between Thread 3 and Thread 2 is that each processing cycle uses 50 frames of clustered data from the database as a fixed processing period, and each time it traces back 25 frames of data. On the first call, it processes the clustered data of frames 1-50 and returns the results of frames 1-50 to the database for storage. On the second call, it processes the data of frames 25-75 and returns the data of frames 51-75 to the database for storage, and so on. The results after executing the algorithm file are also stored in the database, and a new Is_Final field value is marked (Is_Final = 3).
[0182] Thread 4 (Front-end display of trajectory)
[0183] After the first triggering of thread 4, thread 4 will be triggered periodically thereafter. The interval between these triggering intervals is roughly the same as the data acquisition and processing cycle of thread 1, in order to achieve a near-real-time processing effect. Thread 4 will read the real-time data generated from the database and, based on the different values of the Is_Final field, divide the data into four categories: original radar acquisition points (Is_Final=0), cluster non-centroid points (Is_Final=1), cluster centroid points (Is_Final=2), and positioning and tracking trajectory points (Is_Final=3). The data is then transmitted to the front end. The front end marks each data point with a dot, using different colors to distinguish different data types.
[0184] This concludes the explanation of the millimeter-wave-based real-time multi-target localization and tracking algorithm (DBscan-BIRCH-DCC-RKF-Tracking).
Claims
1. A multi-target real-time positioning method based on millimeter waves, characterized in that: It includes the following steps: S1. Use millimeter-wave radar to acquire point cloud data of a fire scene. The point cloud data consists of multiple frames and includes point cloud data under line-of-sight conditions and point cloud data under non-line-of-sight conditions. S2. In the case of millimeter wave reflection, update all point cloud data under non-line-of-sight conditions to obtain updated point cloud data. Use all updated point cloud data and point cloud data under line-of-sight conditions as actual point cloud data. Then the actual point cloud data consists of multiple frames. A Cartesian coordinate system is established on a two-dimensional plan view of the fire scene, and the presence and slope of the reflecting surface are marked at different locations on the two-dimensional plan view to obtain the plan view matrix; Based on the wavefront position in the planar graph matrix and the actual propagation direction of the wave, the parameter array for the current wavefront position is updated in real time until the required propagation distance of the wave is reached. At that time, the coordinates of the current wave are output as the updated non-line-of-sight target point; S3. Set the number of frames according to the frame rate of the actual point cloud data, and fuse the actual point cloud data sequentially according to the number of frames until the number of remaining actual point cloud data is less than the set number of frames. Discard the remaining actual point cloud data to obtain multiple frames of new actual point cloud data. Filter each frame of new actual point cloud data sequentially to obtain multiple frames of point cloud data 1. Use the DBSCAN algorithm and BIRCH algorithm to cluster the multiple frames of point cloud data 1 to obtain the final clustering result and new point cloud data. The new point cloud data consists of multiple frames. Determine the position of each human body based on the final clustering result and the new point cloud data. S4. Based on the final clustering results, DCC data association processing is used to connect the data points in the two adjacent frames of new point cloud data obtained in S3, and then RKF Kalman filtering is used for localization and tracking to obtain the localization and tracking trajectory points of each human body. S5. Utilize asynchronous multithreading to make real-time calls to S1~S4 to complete real-time location tracking.
2. The multi-target real-time positioning method based on millimeter waves according to claim 1, characterized in that: The specific process of S1 is as follows: Start the IWR1843 millimeter-wave radar, configure the serial port between the computer and the millimeter-wave radar hardware, and establish a connection. Use the millimeter-wave radar to collect the structure of a fire scene, obtain multiple frames of point cloud data, and send the multiple frames of point cloud data to the computer. The computer stores the three-dimensional coordinates of each point cloud in each frame of point cloud data as a txt format, thus obtaining the current point cloud data of the fire scene. The point cloud data includes point cloud data under line-of-sight conditions and point cloud data under non-line-of-sight conditions.
3. The multi-target real-time positioning method based on millimeter waves according to claim 2, characterized in that: The specific process of S2 is as follows: S21. Place the millimeter-wave radar into the fire scene and obtain a two-dimensional plan view of the fire scene. Establish a Cartesian coordinate system on the two-dimensional plan view with the millimeter-wave radar as the origin, and assign different arrays to different locations on the two-dimensional plan view. The format of the arrays is as follows: , The value is 1 if a reflective surface exists at the current location, and 0 otherwise. This represents the slope of the reflecting surface in the Cartesian coordinate system, which is 0 when there is no reflecting surface, resulting in the planar matrix; S22. Assuming that any object in the current fire scene can serve as a reflector when the millimeter wave beam propagates, then the millimeter wave beam will be reflected once every time it encounters a reflector. The beam propagation path can be regarded as being composed of multiple line segments, each of which corresponds to a specific straight line equation. That is, the beam propagation path is composed of multiple straight line equations. At the same time, each reflector also corresponds to a straight line equation, and the reflection point is the intersection of the straight line equation of the current beam propagation path and the straight line equation of the reflector. S23. In the case of millimeter wave reflection, in the Cartesian coordinate system, the millimeter wave radar... With millimeter-wave radar A non-line-of-sight target point was detected. The line connecting the points forms the initial beam, and the direction of the initial beam points towards the target point. Target point If the data is a point cloud, then according to the mathematical and geometric definitions, the linear equation of the initial beam is: in, It is the slope of the initial beam. , It is the intercept of the initial beam. ; The initial beam needs to travel a distance of... Define the wavefront as the very front of the initial beam, and the wavefront parameter array is as follows: ,in, For wavefront coordinates, The source coordinates are the wavefront coordinates, and the initial values for both the wavefront coordinates and the source coordinates are (0,0). The slope of the line equation containing the current band, with an initial value of ; The intercept of the equation of the line containing the current band, with an initial value of ; S24. Based on S21-S23, update the parameter array for the current wavefront position according to the wavefront position in the planar graph matrix and the actual propagation direction of the wave. The specific process is as follows: ①If at a certain position on the initial beam propagation path A value of 0 indicates the absence of a reflecting surface, and the parameter array for the current wavefront position is... Increase , , Increase , With all other parameters remaining unchanged, we obtain a new parameter array. ; ②If at a certain position on the initial beam propagation path A value of 1 indicates the presence of a reflecting surface at the current location. According to the principles of light propagation, the coordinates of the current reflection point will become the coordinates of the new wavelength source. The distance the wave needs to travel is then updated to: Then update the remaining parameters of the parameter array according to either a or b below: a. If Then, after reflection in the current band, at The direction will propagate in the reverse direction, that is Update the parameters according to the following process: b. If Then, after reflection in the current band, at With the direction unchanged, update the parameters according to the following process: ③ Update the distance the current wave needs to travel: like If the condition is met, repeat step S24 above; otherwise, output the current wave position. As a non-line-of-sight target point The true target point; For each non-line-of-sight target point, repeat steps S23-S24 to obtain all updated non-line-of-sight target points, i.e., real target points. This will give you all updated point cloud data. Use all updated point cloud data and point cloud data under line-of-sight conditions as the actual point cloud data.
4. The multi-target real-time positioning method based on millimeter waves according to claim 3, characterized in that: The specific process of S3 is as follows: S31. Place all actual point cloud data in the same Cartesian coordinate system, set the number of frames according to the frame rate of the actual point cloud data, and fuse the actual point cloud data in sequence according to the number of frames, that is, fuse every three frames into one frame, until the number of remaining actual point cloud data is less than the set number of frames, discard the remaining actual point cloud data, and obtain multiple frames of new actual point cloud data. S32. Use a pass-through filter to filter each frame of new actual point cloud data in sequence to obtain the processed point cloud data. S33. Use radius filtering to filter the point cloud data of each frame sequentially to obtain the processed point cloud data 1. S34. Use the DBSCAN algorithm to cluster all point cloud data 1 to obtain clustered point cloud data. Count the number of points in each class of the clustered point cloud data, set a limit value for the number of points, delete classes with fewer points than the limit value, and obtain the clustering results. S35. Obtain the centroid of each cluster in the clustering results. Perform BIRCH clustering on the centroid of each cluster. If several centroids cluster into one cluster, it means that the clusters corresponding to these centroids belong to the same class. Merge all point cloud data of the clusters to obtain one class. Solve for the centroid of the current class. One centroid represents one human body, thus obtaining the position of the human body. Repeat the above process to obtain the final clustering results and new point cloud data, as well as the centroid corresponding to each cluster in the clustering results, thus obtaining the position of each human body. The new point cloud data consists of multiple frames.
5. The multi-target real-time positioning method based on millimeter waves according to claim 4, characterized in that: The specific process of S4 is as follows: S41. Use DCC data association processing to perform correlation connection on the data points in the two adjacent new point cloud data obtained in S3: The Cartesian coordinate system is converted to a polar coordinate system using the cosine theorem. In the polar coordinate system, each data point in the first frame of the new point cloud data obtained in S3 is taken as each human body in the millimeter-wave radar detection range of a certain fire scene in S1. Determine which human body in the first frame of point cloud data corresponds to each data point in the second frame of point cloud data obtained from S3, that is, calculate the distance between the centroid of each human body in the first frame of point cloud data and the centroid of each human body in the second frame of point cloud data. For each human body in the second frame of point cloud data, based on the distance between the current centroid and each centroid in the first frame of point cloud data, the two centroids corresponding to the shortest distance are associated. The above process is repeated to associate each data point in the second frame of point cloud data with the corresponding human body in the first frame of point cloud data. In this way, the human bodies in two adjacent frames of point cloud data in the multiple new frames of point cloud data obtained in S3 are associated to obtain the trajectory of each human body. S42. Based on the trajectory of each human body, RKF Kalman filter is used for positioning and tracking to obtain the positioning and tracking trajectory points of each human body.
6. The multi-target real-time positioning method based on millimeter waves according to claim 5, characterized in that: The specific process for calculating the distance between the centroid of each human body in the first frame of point cloud data and the centroid of each human body in the second frame of point cloud data is as follows: in, The distance between the centers of mass, The distance from the center of mass of the current human body to the origin in the second frame of point cloud data. The distance from the center of mass of the current human body to the origin in the first frame of point cloud data. The angle between the line connecting the current human body's center of mass to the origin and the x-axis in the second frame of point cloud data. The angle between the line connecting the current center of mass of the human body to the origin in the first frame of point cloud data and the x-axis.