A barrier pole condition monitoring method based on radar point cloud data and NDT algorithm

Abstract

The present invention relates to a barrier pole state monitoring method based on radar point cloud data and NDT algorithm. The method comprises the following steps: using radar point cloud data to monitor the state of a barrier pole in real time and the benchmark of historical point cloud data, performing point cloud matching and energy matching, and judging whether the pole has completely fallen. Then, the NDT algorithm is used to perform point cloud preprocessing, region and energy screening, and the current point cloud is matched with the historical point cloud by maximum likelihood estimation. The matching degree is evaluated and the historical point cloud data is dynamically updated to realize accurate monitoring of the barrier pole state. The present invention not only improves the accuracy of the barrier system in judging the barrier pole state, but also enables the barrier system to dynamically adapt to environmental changes, thereby ensuring the reliability of the barrier system in long-term operation and reducing the risk of accidental smashing of the barrier pole.

Description

Technical Field

[0001] The present invention relates to the technical field of millimeter wave radar, and in particular to a gate pole status monitoring method based on radar point cloud data and NDT algorithm. Background Art

[0002] As the most commonly used control device in parking lots and access control systems, barrier poles are mainly used to manage vehicle access and ensure orderly access. However, barrier poles may fall when they should not, causing safety accidents that may cause accidental injuries to vehicles and pedestrians.

[0003] Traditional barrier systems mostly rely on simple control methods such as limit switches or photoelectric sensors to determine the status of the barrier pole. Traditional methods are easily affected by environmental factors, such as rain, snow, dust, or light changes, which can cause radar misjudgment or slow response, thereby increasing the risk of accidents. Summary of the invention

[0004] In view of this, the present invention provides a barrier pole status monitoring method based on radar point cloud data and NDT algorithm, which effectively improves the accuracy of the existing barrier system in judging the barrier pole status, adapts to the dynamic changes of the environment, and improves the reliability of the long-term operation of the system.

[0005] To achieve the above object, the present invention provides a barrier pole status monitoring method based on radar point cloud data and NDT algorithm, comprising the following steps:

[0006] S1. Record the characteristic points in the rod area when the gate rod is in the fully fallen state to form a historical point set H, extract the distance energy RP corresponding to the historical points, and form historical benchmark data ;

[0007] S2. Collect point cloud data of the rod area in the current frame ;

[0008] If the number of point clouds in the current frame exceeds the preset threshold, the point cloud data of the current frame is used as the current point set, and the corresponding distance energy RP is extracted;

[0009] S3, calculating the Euclidean distance to match the current point set collected in real time with the historical point set;

[0010] S4, screening all matching pairs and confirming the characteristic points of the rod;

[0011] S401. Preset matching condition: preset distance threshold and energy difference threshold ;

[0012] S402, screening matching pairs;

[0013] The filter criteria are:

[0014] ;

[0015] Keep matching pairs that meet the filter criteria , current point It is the characteristic point of the rod;

[0016] S5, calculating the ratio of successful matching;

[0017] If the ratio is higher than the preset threshold, it is determined that the barrier pole is in a completely down state; if the ratio is not higher than the preset threshold, it is determined that the barrier pole is not completely down at present;

[0018] S6. Use NDT algorithm to dynamically update historical point cloud data;

[0019] S601, data collection and point cloud preprocessing;

[0020] S602, constructing an NDT model and matching the current point cloud;

[0021] Divide the historical point cloud data H into multiple voxels , each voxel The point cloud data in the image is represented by Gaussian distribution, and a voxel grid model is established. ;

[0022] Find each point in the current frame point cloud data P through maximum likelihood estimation In voxel grid model The best matching position in , which maximizes the objective function;

[0023] S603, evaluating the matching degree and updating the point cloud;

[0024] Calculate the matching degree between the current point cloud and the historical point cloud , if the matching degree Above the preset threshold , then it is determined that the current point cloud and the historical point cloud match well, and the current point cloud P is updated to the historical point cloud data H: .

[0025] Preferably, the current frame point cloud data The expression is:

[0026] ;

[0027] in, Indicates the current frame points;

[0028] The distance energy corresponding to each point is:

[0029] ;

[0030] in, Indicate point The distance to energy, Function represents the function of extracting energy from point cloud data.

[0031] Preferably, the Euclidean distance The expression is:

[0032] ;

[0033] in, , Indicates historical point The coordinates of , Indicates the current point The coordinates of

[0034] Find the historical point The corresponding minimum Euclidean distance Value, determined as a historical point Best match point.

[0035] Preferably, the expression for the successful matching ratio is:

[0036] ;

[0037] in, Indicates the number of successfully matched pairs. Indicates the number of historical points;

[0038] when , determine that the barrier rod is in a completely dropped state;

[0039] when , determine that the barrier pole is not completely down in the current state;

[0040] in, Indicates the decision threshold.

[0041] Preferably, step S601 obtains the current point cloud data And the point cloud data of the previous cycle , the preprocessing comprises:

[0042] Remove points with speed: Use the speed information of point cloud data to remove points with a moving speed exceeding the threshold The points:

[0043] ;

[0044] in, Indicate point speed;

[0045] Filter area: Set the boundary of the area where the barrier pole is located. The area range is:

[0046] ;

[0047] Remove the points outside the area and keep the characteristic points of the gate pole.

[0048] ;

[0049] Screening energy: preset energy threshold , according to the distance of the current point, the points with too low energy are removed.

[0050] .

[0051] Preferably, the voxel grid model Each voxel in The probability density function of is:

[0052] ;

[0053] in, and Represents voxels Mean vector and covariance matrix of the midpoint cloud;

[0054] Voxel The point cloud data in can be expressed as:

[0055] ;

[0056] in, Representing voxels Point cloud data set in represents each point within a voxel.

[0057] Preferably, the objective function expression is:

[0058] ;

[0059] in, Indicate point In voxel The probability density value of .

[0060] Preferably, the matching degree The expression is:

[0061] ;

[0062] Where n represents the total number of points in the current point cloud.

[0063] Compared with the prior art, the present invention has the following beneficial effects:

[0064] The present invention adopts real-time monitoring based on radar point cloud data and matching with NDT algorithm, and through the method of matching real-time point cloud data and energy information, it can quickly determine whether the pole has completely fallen, effectively reduce misjudgment caused by environmental interference, such as rain, snow, dust, light changes, reduce erroneous operation caused by slow response or equipment aging of traditional systems, improve the accuracy of judging the status of the gate pole, and ensure the safety and reliability of the system;

[0065] The present invention adopts NDT algorithm to dynamically update historical point cloud data, so as to ensure that the system can adapt to dynamic changes of the environment in long-term operation and maintain continuous accuracy in monitoring the status of the gate pole. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 This is a flow chart of the barrier pole status monitoring of the present invention;

[0067] Figure 2 It is a flow chart of rod matching and historical point updating of the present invention;

[0068] Figure 3 This is a schematic diagram of the rod point cloud matching of the present invention;

[0069] Figure 3 In the figure, the solid circle represents the current point, the star represents the historical point, and the number in the pole area indicates that the historical point and the current point are matched successfully. 1 represents the detection range of the gate, 2 represents the detection range of the radar, and 3 represents the pole area. DETAILED DESCRIPTION

[0070] In order to further explain the technical means and effects adopted by the present invention to achieve the predetermined invention purpose, the specific implementation mode, structure, characteristics and effects of the present invention are described in detail below in combination with the accompanying drawings and preferred embodiments.

[0071] In order to improve the accuracy of the existing barrier system in judging the status of the barrier pole and adapt to the dynamic changes of the environment, the present invention provides a barrier pole status monitoring method based on radar point cloud data and NDT algorithm, comprising the following steps:

[0072] S1. Record the characteristic points in the rod area when the gate rod is in the fully fallen state, form a historical point set H, extract the distance energy RP corresponding to the historical points, and form historical benchmark data .

[0073] S2. Collect point cloud data of the rod area in the current frame ;

[0074] If the number of point clouds in the current frame exceeds the preset threshold, the point cloud data of the current frame is used as the current point set, and the corresponding distance energy RP is extracted;

[0075] Current frame point cloud data The expression is:

[0076] ;

[0077] in, Indicates the current frame points;

[0078] The distance energy corresponding to each point is:

[0079] ;

[0080] in, Indicate point The distance to energy, Function represents the function of extracting energy from point cloud data.

[0081] S3, calculating the Euclidean distance to match the current point set collected in real time with the historical point set, finding the best match between each historical point and the current point, and calculating the minimum Euclidean distance value of the match;

[0082] Euclidean distance The expression is:

[0083] ;

[0084] in, , Indicates historical point The coordinates of , Indicates the current point The coordinates of

[0085] Find the historical point The corresponding minimum Euclidean distance Value, determined as a historical point Best match point:

[0086] .

[0087] S4. Filter all matching pairs, retain matching pairs that meet the distance and distance energy difference threshold conditions, confirm the characteristic points of the rod, and provide a basis for subsequent state judgment;

[0088] S401. Preset matching condition: preset distance threshold and energy difference threshold ;

[0089] In this embodiment, the distance threshold , energy difference threshold ;

[0090] S402, screening matching pairs;

[0091] The filter criteria are:

[0092] ;

[0093] Keep matching pairs that meet the filter criteria , current point It is the characteristic point of the rod.

[0094] S5, calculating the ratio of successful matching;

[0095] The expression of the matching success ratio is:

[0096] ;

[0097] in, Indicates the number of successfully matched pairs. Indicates the number of historical points;

[0098] when , determine that the barrier rod is in a completely dropped state;

[0099] when , determine that the barrier pole is not completely down in the current state;

[0100] in, In this embodiment, the threshold value is ;

[0101] If the ratio is higher than the preset threshold, it is determined that the barrier pole is in a completely dropped state; if the ratio is not higher than the preset threshold, it is determined that the barrier pole is not completely dropped in its current state.

[0102] S6. Use NDT algorithm to dynamically update historical point cloud data to ensure that historical point cloud data remains accurate in long-term operation;

[0103] S601, data collection and point cloud preprocessing;

[0104] Get current point cloud data And the point cloud data of the previous cycle , the preprocessing includes:

[0105] Remove points with speed: Use the speed information of point cloud data to remove points with a moving speed exceeding the threshold The points:

[0106] ;

[0107] in, Indicate point speed;

[0108] Filter area: Set the boundary of the area where the barrier pole is located. The area range is:

[0109] ;

[0110] Remove the points outside the area and keep the characteristic points of the gate pole.

[0111] ;

[0112] Screening energy: preset energy threshold , according to the distance of the current point, the points with too low energy are removed.

[0113] ;

[0114] S602, constructing an NDT model and matching the current point cloud;

[0115] Divide the historical point cloud data H into multiple voxels , each voxel The point cloud data in the image is represented by Gaussian distribution, and a voxel grid model is established. ;

[0116] Voxel grid model Each voxel in The probability density function of is:

[0117] ;

[0118] in, and Represents voxels Mean vector and covariance matrix of the midpoint cloud;

[0119] Voxel The point cloud data in can be expressed as:

[0120] ;

[0121] in, Representing voxels Point cloud data set in represents each point within a voxel;

[0122] Find each point in the current frame point cloud data P through maximum likelihood estimation In voxel grid model The best matching position in the , maximizes the objective function, the objective function expression is:

[0123] ;

[0124] in, Indicate point In voxel The probability density value of ;

[0125] S603, evaluating the matching degree and updating the point cloud;

[0126] Match The expression is:

[0127] ;

[0128] Where n represents the total number of points in the current point cloud.

[0129] Calculate the matching degree between the current point cloud and the historical point cloud , if the matching degree Above the preset threshold , then it is determined that the current point cloud and the historical point cloud match well, and the current point cloud P is updated to the historical point cloud data H: ;

[0130] Complete the historical point cloud update to ensure that the barrier pole feature point cloud data remains accurate under environmental changes.

[0131] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. Although the present invention has been disclosed as a preferred embodiment as above, it is not used to limit the present invention. Any technical personnel in this field can make some changes or modify the technical contents disclosed above into equivalent embodiments without departing from the scope of the technical solution of the present invention. However, any brief modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention are still within the scope of the technical solution of the present invention.

Claims

1. A barrier pole status monitoring method based on radar point cloud data and NDT algorithm, characterized in that: The following steps are involved: S1. Record the characteristic points in the rod area when the gate rod is in the fully fallen state to form a historical point set H, extract the distance energy RP corresponding to the historical points, and form historical benchmark data ; S2. Collect point cloud data of the rod area in the current frame ; If the number of point clouds in the current frame exceeds the preset threshold, the point cloud data of the current frame is used as the current point set, and the corresponding distance energy RP is extracted; S3. Calculate Euclidean distance Match the current point set collected in real time with the historical point set; S4, screening all matching pairs and confirming the characteristic points of the rod; S401. Preset matching condition: preset distance threshold and energy difference threshold ; S402, screening matching pairs; The filter criteria are: ； Keep matching pairs that meet the filter criteria , current point It is the characteristic point of the rod; S5, calculating the ratio of successful matching; If the ratio is higher than the preset threshold, it is determined that the barrier pole is in a completely down state; if the ratio is not higher than the preset threshold, it is determined that the barrier pole is not completely down at present; S6. Use NDT algorithm to dynamically update historical point cloud data; S601, data collection and point cloud preprocessing; S602, constructing an NDT model and matching the current point cloud; Divide the historical point cloud data H into multiple voxels , each voxel The point cloud data in the image is represented by Gaussian distribution, and a voxel grid model is established. ; Find each point in the current frame point cloud data P through maximum likelihood estimation In voxel grid model The best matching position in , which maximizes the objective function; S603, evaluating the matching degree and updating the point cloud; Calculate the matching degree between the current point cloud and the historical point cloud , if the matching degree Above the preset threshold , then it is determined that the current point cloud and the historical point cloud match well, and the current point cloud P is updated to the historical point cloud data H: .

2. The method for monitoring the state of a barrier pole based on radar point cloud data and NDT algorithm according to claim 1, characterized in that: The current frame point cloud data The expression is: ； in, Indicates the current frame points; The distance energy corresponding to each point is: ； in, Indicate point The distance to energy, Function represents the function of extracting energy from point cloud data.

3. The method for monitoring the gate pole status based on radar point cloud data and NDT algorithm according to claim 1 is characterized in that: Euclidean distance The expression is: ； in, , Indicates historical point The coordinates of , Indicates the current point The coordinates of Find the historical point The corresponding minimum Euclidean distance Value, determined as a historical point Best match point.

4. The method for monitoring the state of a barrier pole based on radar point cloud data and NDT algorithm according to claim 1, characterized in that: The expression of the matching success ratio is: ； in, Indicates the number of successfully matched pairs. Indicates the number of historical points; when , determine that the barrier rod is in a completely dropped state; when , determine that the barrier pole is not completely down in the current state; in, Indicates the decision threshold.

5. The method for monitoring the state of a barrier pole based on radar point cloud data and NDT algorithm according to claim 1, characterized in that: Step S601: Obtain current point cloud data And the point cloud data of the previous cycle , the preprocessing comprises: Remove points with speed: Use the speed information of point cloud data to remove points with a moving speed exceeding the threshold Points: ； in, Indicate point speed; Filter area: Set the boundary of the area where the barrier pole is located. The area range is: ； Remove the points outside the area and keep the characteristic points of the gate pole. ； Screening energy: preset energy threshold , according to the distance of the current point, the points with too low energy are removed. 。 6. The method for monitoring the state of a barrier pole based on radar point cloud data and NDT algorithm according to claim 1, characterized in that: The voxel grid model Each voxel in The probability density function of is: ； in, and Represents voxels Mean vector and covariance matrix of the midpoint cloud; Voxel The point cloud data in is represented as: ； in, Representing voxels Point cloud data set in represents each point within a voxel.

7. The method for monitoring the gate pole status based on radar point cloud data and NDT algorithm according to claim 1, characterized in that: The objective function expression is: ； in, Indicate point In voxel The probability density value of .

8. The method for monitoring the state of a barrier pole based on radar point cloud data and NDT algorithm according to claim 1, characterized in that: The matching degree The expression is: ； Where n represents the total number of points in the current point cloud.

Images