[go: up one dir, main page]

CN118962682B - Target course angle fitting processing method of 4D millimeter wave Lei Dadian cloud based on data fusion - Google Patents

Target course angle fitting processing method of 4D millimeter wave Lei Dadian cloud based on data fusion Download PDF

Info

Publication number
CN118962682B
CN118962682B CN202411440777.XA CN202411440777A CN118962682B CN 118962682 B CN118962682 B CN 118962682B CN 202411440777 A CN202411440777 A CN 202411440777A CN 118962682 B CN118962682 B CN 118962682B
Authority
CN
China
Prior art keywords
target
heading angle
course angle
millimeter wave
point cloud
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202411440777.XA
Other languages
Chinese (zh)
Other versions
CN118962682A (en
Inventor
王茜
周明宇
董文
陈虎
薛旦
史颂华
张显宏
王海涛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Geometry Partner Intelligent Driving Co ltd
Original Assignee
Shanghai Geometry Partner Intelligent Driving Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Geometry Partner Intelligent Driving Co ltd filed Critical Shanghai Geometry Partner Intelligent Driving Co ltd
Priority to CN202411440777.XA priority Critical patent/CN118962682B/en
Publication of CN118962682A publication Critical patent/CN118962682A/en
Application granted granted Critical
Publication of CN118962682B publication Critical patent/CN118962682B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

The invention relates to a target course angle fitting processing method based on 4D millimeter wave Lei Dadian cloud of data fusion, which comprises the steps of obtaining 4D millimeter wave Lei Dadian cloud information, carrying out point cloud clustering on the obtained 4D millimeter wave Lei Dadian cloud information, calculating point cloud cluster characteristics, correlating a current tracking point with a previous frame of course, judging whether the course vanishing condition is met, respectively calculating a first course angle, a second course angle and a third course angle on the course with correlated point cloud through principal component analysis, lshape fitting and the ratio of the transverse absolute speed to the longitudinal absolute speed of a target vehicle, respectively designing data fusion weights of the first course angle, the second course angle and the third course angle according to the clustering length and width of the current target and the prior information of the speed to the ground, calculating the fusion course angle of the current frame, and taking the fusion course angle as a measurement value to a course angle filter to calculate the target course angle. By adopting the method of the invention, the accuracy of the target course angle is obviously improved.

Description

Target course angle fitting processing method of 4D millimeter wave Lei Dadian cloud based on data fusion
Technical Field
The invention belongs to the field of 4D millimeter wave radars, particularly relates to the field of target course angle fitting, and particularly relates to a target course angle fitting processing method, device, processor and computer readable storage medium of 4D millimeter wave Lei Dadian cloud based on data fusion.
Background
With the rapid development of automatic driving technology, the perception capability of intelligent driving automobiles to the surrounding environment is increasingly important. At present, an intelligent driving automobile mainly carries out environment sensing through two modes of a camera and a radar. The millimeter wave radar has the advantages of high ranging and speed measuring precision, small weather interference, moderate cost and the like, and is widely applied to the intelligent driving field. The heading angle of the vehicle is very important information, and is important to predicting the state and behavior analysis of the target vehicle at the next moment.
The prior art CN202410594751.4 proposes a method, a device and a medium for calculating a target course angle based on millimeter wave Lei Dadian cloud, and the patent requires that a reference vehicle determines a millimeter wave radar course angle based on a transverse-longitudinal absolute speed ratio obtained by agreeing to target multi-frame filtering association. For a low-speed target, the method directly adopts the transverse and longitudinal absolute speed ratio to determine the course angle because the noise of the filtering speed is very large, and the accuracy of the course angle result can be influenced.
In the prior art CN202210471195.2, after analyzing the characteristics of a sensor, the patent uses different course angle fitting algorithms for targets at different positions, and uses different weights under different conditions in combination with the course angle of the target after reference tracking and the course angle formed by the external frame of the target, so as to calculate the target course angle, but compared with a laser radar, the point cloud density of the millimeter wave radar is related to the radial distance and angle of the target, and the single frame Lshape fitting and PCA (principal component analysis) fitting have larger error of the target course angle, so that the accuracy of the course angle result is affected.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a target course angle fitting processing method, device, processor and computer readable storage medium thereof based on 4D millimeter wave Lei Dadian cloud of data fusion.
In order to achieve the above objective, the method, the device, the processor and the computer readable storage medium for processing the target course angle fitting of the 4D millimeter wave Lei Dadian cloud based on data fusion according to the present invention are as follows:
The target course angle fitting processing method of the 4D millimeter wave Lei Dadian cloud based on data fusion is mainly characterized by comprising the following steps of:
(1) Acquiring 4D millimeter wave Lei Dadian cloud information;
(2) Performing point cloud clustering on the acquired 4D millimeter wave Lei Dadian cloud information, and calculating the point cloud cluster characteristics;
(3) Associating the current tracking point with the track of the previous frame, and judging whether the track vanishing condition is met;
(4) Calculating a first course angle, a second course angle and a third course angle respectively for the tracks with associated point clouds through principal component analysis, lshape fitting and the ratio of the transverse absolute speed to the longitudinal absolute speed of the target vehicle;
(5) Respectively designing data fusion weights of a first course angle, a second course angle and a third course angle according to the cluster length and width of the current target and the priori information of the speed to the ground, so as to calculate a frame fusion course angle;
(6) And taking the fused course angle as a measurement value to be brought into a course angle filter for calculation to obtain a target course angle.
Preferably, the step (1) is to calculate the prior state information of the current target according to the target tracking state information of the previous frameWherein, the method comprises the steps of, wherein,The position, the speed and the acceleration of the target in the vehicle coordinate system are respectively:
;
;
;
Wherein, In the state of the last moment in time,For the a priori state at this moment,For the estimated covariance of the last moment in time,For the prior estimated covariance at this time, a is the transition matrix,Representing a transpose of a, Q being the variance of the process noise, T being the frame duration.
Preferably, the step (2) specifically includes:
Clustering the 4D millimeter wave point cloud data by adopting a DBSCAN clustering algorithm to obtain a clustering result, and simultaneously calculating cluster related information including cluster center point state information Namely the x-direction and y-direction position of the target under the coordinate system of the vehicle and the radial speed information, BOX informationI.e., size information of the point cloud cluster, including length, width, height, and heading angle of the BOX.
Preferably, the step (3) specifically includes the following steps:
(3.1) calculating whether the association point cloud exists between the tracking target and the cluster according to the Hungary algorithm, if so, continuing to track the target and filter the state of the target, and entering the step (4) at the same time, otherwise, entering the step (3.2);
(3.2) further judging whether the current tracking point meets the track vanishing condition, if so, extrapolating the target track and keeping the BOX information unchanged, otherwise, directly deleting the track;
(3.3) obtaining a matching pair formed between the ID of the tracking target and the cluster ID number of the perception target according to the successful association result 。
Preferably, the step (3) further includes updating the state equation according to the obtained optimal matching pair:
;
;
;
Wherein, ,,Represents the transpose of H and,Representing the calculated kalman gain at this time,In order to be the posterior state at this time,For the a priori state at this moment,Covariance is estimated for the posterior at this time instant,For the a priori estimated covariance at the instant,Is an identity matrix.
Preferably, the first course angleThe method comprises the following steps:
Recording a plurality of point clouds contained in the 4D millimeter wave Lei Dadian cloud cluster observation data as [ A two-dimensional position feature point set P is established according to the method:
;
Calculating covariance matrix of P :
Wherein;
Calculating eigenvalues and eigenvectors of the covariance matrix C:
;
Wherein the columns of the matrix U are covariance matrices The characteristic values are arranged from large to small, and the first k characteristic values and the corresponding characteristic vectors are reserved, namely the first k main component vector directions;
And calculating a covariance matrix of the single vehicle point cloud coordinate data (x k,yk), and taking a maximum eigenvalue, wherein the corresponding eigenvector is the heading direction of the vehicle.
Preferably, the second course angleThe method comprises the following steps:
Recording a plurality of point clouds contained in 4D millimeter wave Lei Dadian cloud cluster observation data as Selecting two furthest outliersAndDrawing a straight line Ld between two points, taking the two points as an upper section and a lower section, projecting all points of the current obstacle to the straight line Ld to obtain a length L i of each point from the straight line Ld, and marking a point with the maximum value of L i asTo、、Drawing a rectangle for the vertex, wherein the obtained long-side direction is the second course angle.
Preferably, the third heading angle is specifically a ratio of a lateral absolute speed to a longitudinal absolute speed of the target vehicle as the third heading angle of the target vehicle:
;
According to the vehicle information acquired by the current 4D millimeter wave radar and the posterior information at the moment, acquiring a new ground speed (v gx,vgy) of the current target, and updating the third course angle。
Preferably, the step (5) is specifically to calculate the frame fusion heading angle in the following manner:
;
Wherein, 、、Are all information weights.
Preferably, the step (6) is that the current frame fusion course angle is taken as a measurement value to be brought into a course angle filter to calculate a target course angle, and the course angle filter uses a Kalman filter to carry out multi-frame filtering, specifically:
;
;
;
;
;
Wherein, For the course angle state at the previous moment,For the heading angle state predicted at the present time,The covariance is estimated for the last time instant,As a variance of the process noise,For a priori estimated covariance of the instant prediction,In order to measure the variance of the noise,The kalman gain calculated for this instant,For the target course angle of the vehicle,For the current time course angle posterior state,And the current time course angle posterior error covariance is obtained.
The target course angle fitting processing device of the 4D millimeter wave Lei Dadian cloud based on data fusion is mainly characterized by comprising the following components:
a processor configured to execute computer-executable instructions;
and the memory stores one or more computer executable instructions which, when executed by the processor, realize the steps of the target course angle fitting processing method of the 4D millimeter wave Lei Dadian cloud based on data fusion.
The processor for the target course angle fitting processing of the 4D millimeter wave Lei Dadian cloud based on data fusion is mainly characterized in that the processor is configured to execute computer executable instructions, and when the computer executable instructions are executed by the processor, the steps of the target course angle fitting processing method of the 4D millimeter wave Lei Dadian cloud based on data fusion are realized.
The computer readable storage medium is mainly characterized in that a computer program is stored on the computer readable storage medium, and the computer program can be executed by a processor to realize the steps of the target course angle fitting processing method of the 4D millimeter wave Lei Dadian cloud based on data fusion.
By adopting the target course angle fitting processing method, device, processor and computer readable storage medium of the 4D millimeter wave Lei Dadian cloud based on data fusion, an equation or an error optimization function is established to estimate the radar installation angle by placing corner reflectors at any position of a radar public irradiation area. By adopting the method provided by the invention, the characteristics of millimeter wave radar are fully utilized, data fusion and multi-frame association are carried out on single-frame Lshape fitting course angle, PCA (principal component analysis) fitting target course angle and motion speed estimation fitting course angle data, and the accuracy of the target course angle is obviously improved.
Drawings
Fig. 1 is a flowchart of the target course angle fitting processing method of the 4D millimeter wave Lei Dadian cloud based on data fusion.
Fig. 2 is a flowchart of the target heading angle fitting processing method of the 4D millimeter wave Lei Dadian cloud based on data fusion according to an embodiment of the present invention.
FIG. 3 is a diagram illustrating a second heading angle calculation according to the present invention.
Fig. 4 is a schematic diagram of actual measurement results of the target course angle fitting processing method of the 4D millimeter wave Lei Dadian cloud based on data fusion.
Detailed Description
In order to more clearly describe the technical contents of the present invention, a further description will be made below in connection with specific embodiments.
Before describing in detail embodiments that are in accordance with the present invention, it should be observed that the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus.
Referring to fig. 1, the method for fitting and processing the target course angle of the 4D millimeter wave Lei Dadian cloud based on data fusion includes the following steps:
(1) Acquiring 4D millimeter wave Lei Dadian cloud information;
(2) Performing point cloud clustering on the acquired 4D millimeter wave Lei Dadian cloud information, and calculating the point cloud cluster characteristics;
(3) Associating the current tracking point with the track of the previous frame, and judging whether the track vanishing condition is met;
(4) Calculating a first course angle, a second course angle and a third course angle respectively for the tracks with associated point clouds through principal component analysis, lshape fitting and the ratio of the transverse absolute speed to the longitudinal absolute speed of the target vehicle;
(5) Respectively designing data fusion weights of a first course angle, a second course angle and a third course angle according to the cluster length and width of the current target and the priori information of the speed to the ground, so as to calculate a frame fusion course angle;
(6) And taking the fused course angle as a measurement value to be brought into a course angle filter for calculation to obtain a target course angle.
As a preferred embodiment of the present invention, the step (1) is to calculate the prior state information of the current target according to the target tracking state information of the previous frameWherein, the method comprises the steps of, wherein,The position, the speed and the acceleration of the target in the vehicle coordinate system are respectively:
;
;
;
Wherein, In the state of the last moment in time,For the a priori state at this moment,For the estimated covariance of the last moment in time,For the prior estimated covariance at this time, a is the transition matrix,Representing a transpose of a, Q being the variance of the process noise, T being the frame duration.
As a preferred embodiment of the present invention, the step (2) specifically includes:
Clustering the 4D millimeter wave point cloud data by adopting a DBSCAN clustering algorithm to obtain a clustering result, and simultaneously calculating cluster related information including cluster center point state information Namely the x-direction and y-direction position of the target under the coordinate system of the vehicle and the radial speed information, BOX informationI.e., size information of the point cloud cluster, including length, width, height, and heading angle of the BOX.
As a preferred embodiment of the present invention, the step (3) specifically includes the following steps:
(3.1) calculating whether the association point cloud exists between the tracking target and the cluster according to the Hungary algorithm, if so, continuing to track the target and filter the state of the target, and entering the step (4) at the same time, otherwise, entering the step (3.2);
(3.2) further judging whether the current tracking point meets the track vanishing condition, if so, extrapolating the target track and keeping the BOX information unchanged, otherwise, directly deleting the track;
(3.3) obtaining a matching pair formed between the ID of the tracking target and the cluster ID number of the perception target according to the successful association result 。
As a preferred embodiment of the present invention, the step (3) further includes updating the state equation according to the obtained optimal matching pair:
;
;
;
Wherein, ,,Represents the transpose of H and,Representing the calculated kalman gain at this time,In order to be the posterior state at this time,For the a priori state at this moment,Covariance is estimated for the posterior at this time instant,For the a priori estimated covariance at the instant,Is an identity matrix.
As a preferred embodiment of the present invention, the first heading angleThe method comprises the following steps:
Recording a plurality of point clouds contained in the 4D millimeter wave Lei Dadian cloud cluster observation data as [ A two-dimensional position feature point set P is established according to the method:
;
Calculating covariance matrix of P :
Wherein;
Calculating eigenvalues and eigenvectors of the covariance matrix C:
;
Wherein the columns of the matrix U are covariance matrices The characteristic values are arranged from large to small, and the first k characteristic values and the corresponding characteristic vectors are reserved, namely the first k main component vector directions;
And calculating a covariance matrix of the single vehicle point cloud coordinate data (x k,yk), and taking a maximum eigenvalue, wherein the corresponding eigenvector is the heading direction of the vehicle.
As a preferred embodiment of the present invention, the second heading angleThe method comprises the following steps:
Recording a plurality of point clouds contained in 4D millimeter wave Lei Dadian cloud cluster observation data as Selecting two furthest outliersAndDrawing a straight line Ld between two points, taking the two points as an upper section and a lower section, projecting all points of the current obstacle to the straight line Ld to obtain a length L i of each point from the straight line Ld, and marking a point with the maximum value of L i asTo、、Drawing a rectangle for the vertex, wherein the obtained long-side direction is the second course angle.
As a preferred embodiment of the present invention, the third heading angle is specifically a ratio of a lateral and longitudinal absolute speed of the target vehicle as the third heading angle of the target vehicle:
;
According to the vehicle information acquired by the current 4D millimeter wave radar and the posterior information at the moment, acquiring a new ground speed (v gx,vgy) of the current target, and updating the third course angle。
As a preferred embodiment of the present invention, the step (5) is specifically to calculate the present frame fusion heading angle as follows:
;
Wherein, 、、Are all information weights.
In the preferred embodiment of the invention, the step (6) is that the frame fusion course angle is taken as a measurement value to be brought into a course angle filter to calculate a target course angle, and the course angle filter uses a Kalman filter to carry out multi-frame filtering, specifically:
;
;
;
;
;
Wherein, For the course angle state at the previous moment,For the heading angle state predicted at the present time,The covariance is estimated for the last time instant,As a variance of the process noise,For a priori estimated covariance of the instant prediction,In order to measure the variance of the noise,The kalman gain calculated for this instant,For the target course angle of the vehicle,For the current time course angle posterior state,And the current time course angle posterior error covariance is obtained.
As shown in fig. 1, in practical application, the processing flow of the present technical solution is as follows:
1. acquiring 4D Lei Dadian cloud information;
2. clustering the obtained point cloud information and extracting state information such as position, speed, RCS and the like;
3. Associated with the last frame track:
deleting the track which has no associated point cloud and meets the track vanishing condition;
Extrapolating tracks which do not have associated point clouds and do not meet the track vanishing conditions, wherein the information (course angle, length, width and height) of the track BOX is unchanged;
and filtering the track states with the associated point clouds.
4. Calculating a first course angle, a second course angle and a third course angle for a track with an associated point cloud:
the first heading angle is the result of PCA (principal component analysis) calculation:
Lei Dadian cloud cluster observations contain multiple point clouds, denoted as [ X ｉ,Ｙｉ ], that can create a two-dimensional set of location feature points:
;
calculating covariance matrix of P:
Wherein ;
Calculating eigenvalues and eigenvectors of the covariance matrix:
as the covariance matrix is a real symmetric square matrix and is a regular matrix, the diagonalization can be directly carried out, and diagonalization main line elements are characteristic values. According to the structural theorem of the regular matrix, there is the following matrix decomposition:
;
Wherein the columns of U are unit orthogonal eigenvectors of covariance matrix sigma, and eigenvalues are arranged from large to small. Therefore, the first k eigenvalues and corresponding eigenvectors are reserved, namely the first k principal component vector directions.
In summary, the covariance matrix of the single vehicle point cloud coordinate data (x k,yk) is calculated, and then the maximum eigenvalue is taken, and the corresponding eigenvector is the heading direction of the vehicle.
The second course angle is Lshape calculated
As shown in fig. 3, lei Dadian cloud clusters include a plurality of point clouds, which are denoted asSelecting two furthest outliersAndDrawing a straight line Ld between the two points, taking the two points as an upper section and a lower section, projecting the straight line Ld from all points of the obstacle to obtain a point with the length L ｉ, of each point to obtain the maximum L ｉ, and marking the point asTo、、Drawing a rectangle for the vertex, wherein the long-side direction is the second course angle.
The third course angle is the ratio of the transverse and longitudinal absolute speeds of the target vehicle to indirectly obtain the course angle of the target vehicle
The ratio of the target vehicle lateral-longitudinal absolute speed indirectly obtains a third heading angle of the target vehicle, namely:
。
5. designing data fusion weights of the first course angle, the second course angle and the third course angle according to prior information such as cluster length and width, ground speed and the like, and carrying out data fusion on the data fusion weights to calculate a fusion course angle of the frame:
;
、、 and the information weight is adjusted in engineering according to the target characteristics and the scene information.
6. And taking the fused course angle as a measurement value to a course angle filter to calculate a target course angle, and performing multi-frame filtering by the course angle filter by using a Kalman filter.
In one embodiment of the invention, the deceleration target course angle fitting calculation
The radar is installed forwards, the self-vehicle approaches to a front deceleration target, and according to the method provided by the invention, the course angle fitting flow is as follows:
1. Calculating the prior state information of the target according to the target tracking state information of the previous frame The position, speed and acceleration of the target in the vehicle coordinate system are as follows:
;
;
Wherein, In the state of the last moment in time,For the a priori state at this moment,For the estimated covariance of the last moment in time,For the prior estimated covariance at this time, a is the transition matrix,Representing the transpose of a, Q is the variance of the process noise, the method actually uses the CA model, namely:
T is the frame duration.
2. Clustering the 4D point cloud data by adopting an improved DBSCAN clustering algorithm to obtain a clustering result, and simultaneously calculating cluster related information including cluster center point state informationNamely, the x-direction and y-direction position and radial speed information of the target in the vehicle coordinate system and BOX informationNamely the size information of the point cloud cluster, the length, width and height of the BOX formed by the point cloud cluster, PCA (principal component analysis), and the course angle obtained by Lshape fitting,。
3. Calculating the association of the tracking target and the cluster according to the Hungary algorithm to obtain a matching pair of the tracking target and the perception target。
4. Updating the state equation according to the optimal matching pair:
;
;
;
wherein, measure I.e. the x, y direction position and radial velocity information of the object in the vehicle coordinate system,,,Transpose of HRepresenting the calculated kalman gain at this time,In order to be the posterior state at this time,For the a priori state at this moment,Covariance is estimated for the posterior at this time instant,For the a priori estimated covariance at the instant,Is an identity matrix.
5. And calculating the ground speed (v gx,vgy) of the target according to the vehicle information acquired by the radar and the posterior state information calculated in the last step.
6. According to cluster clusters,And carrying out data fusion on prior information such as the ground speed and the like to calculate a fusion course angle of the frame, namely:
;
Wherein, 、、As a weight of the information it is possible to provide,,,Fitting course angles for the clusters PCA (principal component analysis) and Lshape respectively,。
7. Taking the fused course angle as a measurement value to a course angle filter to calculate a target course angle, wherein the course angle filter uses a Kalman filter to carry out multi-frame filtering, and the target course angleAnd recording as a target state.
The target course angle fitting processing device of the 4D millimeter wave Lei Dadian cloud based on data fusion, wherein the device comprises:
a processor configured to execute computer-executable instructions;
and the memory stores one or more computer executable instructions which, when executed by the processor, realize the steps of the target course angle fitting processing method of the 4D millimeter wave Lei Dadian cloud based on data fusion.
The processor of the target course angle fitting processing of the 4D millimeter wave Lei Dadian cloud based on data fusion is configured to execute computer executable instructions, and when the computer executable instructions are executed by the processor, the steps of the target course angle fitting processing method of the 4D millimeter wave Lei Dadian cloud based on data fusion are realized.
The computer readable storage medium has a computer program stored thereon, the computer program being executable by a processor to implement the steps of the method for fitting a target course angle of a 4D millimeter wave Lei Dadian cloud based on data fusion.
Any process or method descriptions in flow charts or otherwise described herein may be understood as representing modules, segments, or portions of code which include one or more executable instructions for implementing specific logical functions or steps of the process, and further implementations are included within the scope of the preferred embodiment of the present invention in which functions may be executed out of order from that shown or discussed, including substantially concurrently or in reverse order, depending on the functionality involved, as would be understood by those reasonably skilled in the art of the present invention.
It is to be understood that portions of the present invention may be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, the various steps or methods may be implemented in software or firmware stored in a memory and executed by a suitable instruction execution device.
Those of ordinary skill in the art will appreciate that all or a portion of the steps carried out in the method of the above-described embodiments may be implemented by a program to instruct related hardware, and the program may be stored in a computer readable storage medium, where the program when executed includes one or a combination of the steps of the method embodiments.
The above-mentioned storage medium may be a read-only memory, a magnetic disk or an optical disk, or the like.
In the description of the present specification, reference to the terms "one embodiment," "some embodiments," "examples," "specific examples," or "embodiments," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiments or examples. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
While embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and not to be construed as limiting the invention, and that variations, modifications, alternatives and variations may be made to the above embodiments by one of ordinary skill in the art within the scope of the invention.
By adopting the target course angle fitting processing method, device, processor and computer readable storage medium of the 4D millimeter wave Lei Dadian cloud based on data fusion, an equation or an error optimization function is established to estimate the radar installation angle by placing corner reflectors at any position of a radar public irradiation area. By adopting the method provided by the invention, the characteristics of millimeter wave radar are fully utilized, data fusion and multi-frame association are carried out on single-frame Lshape fitting course angle, PCA (principal component analysis) fitting target course angle and motion speed estimation fitting course angle data, and the accuracy of the target course angle is obviously improved.
In this specification, the invention has been described with reference to specific embodiments thereof. It will be apparent that various modifications and variations can be made without departing from the spirit and scope of the invention. The specification and drawings are, accordingly, to be regarded in an illustrative rather than a restrictive sense.

Claims (9)

1.一种基于数据融合的4D毫米波雷达点云的目标航向角拟合处理方法，其特征在于，所述的方法包括以下步骤：1. A target heading angle fitting processing method for 4D millimeter wave radar point cloud based on data fusion, characterized in that the method comprises the following steps: （1）获取4D毫米波雷达点云信息；(1) Obtain 4D millimeter wave radar point cloud information; （2）对获取到的4D毫米波雷达点云信息进行点云聚类，并计算点云簇特征；(2) Cluster the acquired 4D millimeter-wave radar point cloud information and calculate the point cloud cluster features; （3）将当前跟踪点与上一帧航迹进行关联，并判断是否满足航迹消失条件；(3) Associate the current tracking point with the track of the previous frame and determine whether the track disappearance condition is met; （4）对存在关联点云的航迹通过主成分分析、Lshape拟合以及目标车辆横向与纵向绝对速度的比值分别计算第一航向角、第二航向角以及第三航向角；(4) For the track with associated point clouds, the first heading angle, the second heading angle, and the third heading angle are calculated respectively by principal component analysis, Lshape fitting, and the ratio of the lateral and longitudinal absolute velocities of the target vehicle; （5）根据当前目标的聚类长宽、对地速度的先验信息分别设计第一航向角、第二航向角以及第三航向角的数据融合权重，以此计算本帧融合航向角；(5) According to the prior information of the cluster length and width of the current target and the ground speed, the data fusion weights of the first heading angle, the second heading angle and the third heading angle are designed respectively, so as to calculate the fusion heading angle of the current frame; （6）将所述的融合航向角作为量测值带入航向角滤波器计算得到目标航向角；(6) Substituting the fused heading angle as a measurement value into a heading angle filter to calculate a target heading angle; 所述的步骤（1）为：根据上一帧目标跟踪状态信息，计算当前目标的先验状态信息，其中，分别为车辆坐标系下目标的位置、速度及加速度，具体为：The step (1) is: according to the target tracking state information of the previous frame, calculate the prior state information of the current target ,in, They are the position, velocity and acceleration of the target in the vehicle coordinate system, specifically: ； ; ； ; ； ; 其中，为上一时刻的状态，为本时刻的先验状态，为上一时刻的估计协方差，为本时刻的先验估计协方差，A为转态转移矩阵，表示A的转置，Q为过程噪声的方差，T为帧时长；in, is the state at the previous moment, is the prior state at this moment, is the estimated covariance at the previous moment, is the prior estimated covariance at this moment, A is the transition matrix, represents the transpose of A , Q is the variance of process noise, and T is the frame duration; 所述的步骤（2）具体为：The step (2) is specifically as follows: 采用DBSCAN聚类算法对4D毫米波点云数据进行聚类，得到聚类结果，同时计算聚类簇相关信息，包括聚类簇中心点状态信息，即目标在车辆坐标系下的x、y方向位置及径向速度信息；BOX信息，即点云簇的尺寸信息，包括BOX的长、宽、高以及航向角；The DBSCAN clustering algorithm is used to cluster the 4D millimeter wave point cloud data to obtain the clustering results and calculate the cluster related information, including the cluster center point status information. , i.e. the x, y position and radial velocity information of the target in the vehicle coordinate system; BOX information , that is, the size information of the point cloud cluster, including the length, width, height and heading angle of the BOX; 所述的步骤（3）具体包括以下步骤：The step (3) specifically includes the following steps: （3.1）根据匈牙利算法计算跟踪目标与聚类簇之间是否存在关联点云，如果是，则继续进行目标跟踪并对目标状态进行滤波处理，同时进入步骤（4）；否则，进入步骤（3.2）；(3.1) According to the Hungarian algorithm, calculate whether there is a related point cloud between the tracked target and the cluster. If so, continue to track the target and filter the target state, and enter step (4); otherwise, enter step (3.2); （3.2）进一步判断当前该跟踪点是否满足航迹消失条件，如果是，则将目标航迹外推，并保持所述的BOX信息不变，否则，直接进行航迹删除；(3.2) Further determine whether the current tracking point meets the track disappearance condition. If so, the target track is extrapolated and the BOX information is kept unchanged. Otherwise, the track is deleted directly. （3.3）根据关联成功结果，得到跟踪目标ID与感知目标的聚类簇ID编号之间形成的匹配对；(3.3) According to the association success result, the matching pair between the tracking target ID and the cluster ID number of the perceived target is obtained. ; 所述的步骤（3）还包括根据获取到的最优匹配对更新状态方程：The step (3) further includes updating the state equation according to the obtained optimal matching pair: ； ; ； ; ； ; 其中，，，表示H的转置，表示本时刻计算的卡尔曼增益，为本时刻的后验状态，为本时刻的先验状态，为本时刻的后验估计协方差，为本时刻的先验估计协方差，为单位矩阵。in, , , represents the transpose of H , represents the Kalman gain calculated at this moment, is the posterior state at this moment, is the prior state at this moment, is the posterior estimated covariance at this moment, is the prior estimated covariance at this moment, is the identity matrix. 2.根据权利要求1所述的基于数据融合的4D毫米波雷达点云的目标航向角拟合处理方法，其特征在于，所述的第一航向角具体为：2. The target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to claim 1 is characterized in that the first heading angle Specifically: 将4D毫米波雷达点云簇观测数据中包含的多个点云记为[]，并以此建立一个二维位置特征点集P：The multiple point clouds contained in the 4D millimeter wave radar point cloud cluster observation data are denoted as [ ], and use this to establish a two-dimensional position feature point set P: ； ; 计算P的协方差矩阵：Calculate the covariance matrix of P : ，其中； ,in ; 计算协方差矩阵C的特征值和特征向量:Calculate the eigenvalues and eigenvectors of the covariance matrix C: ； ; 其中，矩阵U的列为协方差矩阵的单位正交特征向量，特征值按从大到小排列，保留前k个特征值及对应的特征向量，即为前k个主成分矢量方向；Among them, the columns of matrix U are covariance matrices The unit orthogonal eigenvectors of , the eigenvalues are arranged from large to small, and the first k eigenvalues and corresponding eigenvectors are retained, which are the directions of the first k principal component vectors; 通过计算单个车辆点云坐标数据(xk，yk)的协方差矩阵，取最大特征值，其对应的特征向量即为车辆航向方向。By calculating the covariance matrix of a single vehicle point cloud coordinate data (x k , y k ), the maximum eigenvalue is taken, and its corresponding eigenvector is the vehicle heading direction. 3.根据权利要求2所述的基于数据融合的4D毫米波雷达点云的目标航向角拟合处理方法，其特征在于，所述的第二航向角具体为：3. The target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to claim 2 is characterized in that the second heading angle Specifically: 将4D毫米波雷达点云簇观测数据中包含的多个点云记为，选择两个最远的离群点和，在两点之间绘制一条直线Ld，以这两个点为上下区间，将当前障碍物的所有点均向直线Ld做投影，得到每个点距直线Ld的长度记为Li，将Li最大数值的点记为，以、、为顶点绘制矩形，其中得到的长边方向即为所述的第二航向角。The multiple point clouds contained in the 4D millimeter wave radar point cloud cluster observation data are recorded as , select the two farthest outliers and , draw a straight line Ld between the two points, take these two points as the upper and lower intervals, project all the points of the current obstacle onto the straight line Ld, and record the length of each point from the straight line Ld as Li , and record the point with the maximum value of Li as ,by , , A rectangle is drawn for the vertex, wherein the direction of the long side obtained is the second heading angle. 4.根据权利要求3所述的基于数据融合的4D毫米波雷达点云的目标航向角拟合处理方法，其特征在于，所述的第三航向角具体为，将目标车辆横向和纵向绝对速度的比值作为所述的目标车辆的第三航向角：4. The target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to claim 3 is characterized in that the third heading angle is specifically the ratio of the lateral and longitudinal absolute speeds of the target vehicle as the third heading angle of the target vehicle. : ； ; 根据当前4D毫米波雷达获取到的车辆信息以及本时刻的后验信息，获取当前目标新的对地速度（vgx,vgy），并以此更新所述的第三航向角。According to the vehicle information obtained by the current 4D millimeter wave radar and the a posteriori information at this moment, the new ground speed of the current target (v gx , v gy ) is obtained, and the third heading angle is updated accordingly . 5.根据权利要求4所述的基于数据融合的4D毫米波雷达点云的目标航向角拟合处理方法，其特征在于，所述的步骤（5）具体为按照以下方式计算所述的本帧融合航向角：5. The target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to claim 4 is characterized in that the step (5) is specifically to calculate the current frame fusion heading angle in the following manner: : ； ; 其中，、、均为信息权重。in, , , are all information weights. 6.根据权利要求5所述的基于数据融合的4D毫米波雷达点云的目标航向角拟合处理方法，其特征在于，所述的步骤（6）为：将所述的本帧融合航向角作为量测值带入航向角滤波器计算目标航向角，所述的航向角滤波器使用卡尔曼滤波器进行多帧滤，具体为：6. The target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to claim 5 is characterized in that the step (6) is: bringing the fused heading angle of the current frame as a measurement value into the heading angle filter to calculate the target heading angle, and the heading angle filter uses a Kalman filter to perform multi-frame filtering, specifically: ； ; ； ; ； ; ； ; ； ; 其中，为上一时刻航向角状态，为本时刻预测的航向角状态，为上一时刻估计协方差，为过程噪声的方差，为本时刻预测的先验估计协方差，为量测噪声的方差，为本时刻计算的卡尔曼增益，为目标航向角，为本时刻航向角后验状态，为本时刻航向角后验误差协方差。in, is the heading angle state at the previous moment, is the heading angle state predicted at this moment, is the estimated covariance at the previous moment, is the variance of process noise, is the prior estimated covariance of the forecast at this moment, is the variance of the measurement noise, is the Kalman gain calculated at this moment, is the target heading angle, is the posterior state of the heading angle at this moment, is the posterior error covariance of the heading angle at this moment. 7.一种基于数据融合的4D毫米波雷达点云的目标航向角拟合处理装置，其特征在于，所述的装置包括：7. A target heading angle fitting processing device for 4D millimeter wave radar point cloud based on data fusion, characterized in that the device comprises: 处理器，被配置成执行计算机可执行指令；a processor configured to execute computer executable instructions; 存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1～6中任一项所述的基于数据融合的4D毫米波雷达点云的目标航向角拟合处理方法的步骤。A memory storing one or more computer executable instructions, wherein when the computer executable instructions are executed by the processor, the steps of the target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to any one of claims 1 to 6 are implemented. 8.一种基于数据融合的4D毫米波雷达点云的目标航向角拟合处理的处理器，其特征在于，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1～6中任一项所述的基于数据融合的4D毫米波雷达点云的目标航向角拟合处理方法的步骤。8. A processor for target heading angle fitting processing of 4D millimeter wave radar point cloud based on data fusion, characterized in that the processor is configured to execute computer executable instructions, and when the computer executable instructions are executed by the processor, the steps of the target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to any one of claims 1 to 6 are implemented. 9.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现权利要求1～6中任一项所述的基于数据融合的4D毫米波雷达点云的目标航向角拟合处理方法的步骤。9. A computer-readable storage medium, characterized in that a computer program is stored thereon, and the computer program can be executed by a processor to implement the steps of the target heading angle fitting processing method of 4D millimeter-wave radar point cloud based on data fusion according to any one of claims 1 to 6.
CN202411440777.XA 2024-10-16 2024-10-16 Target course angle fitting processing method of 4D millimeter wave Lei Dadian cloud based on data fusion Active CN118962682B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202411440777.XA CN118962682B (en) 2024-10-16 2024-10-16 Target course angle fitting processing method of 4D millimeter wave Lei Dadian cloud based on data fusion

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202411440777.XA CN118962682B (en) 2024-10-16 2024-10-16 Target course angle fitting processing method of 4D millimeter wave Lei Dadian cloud based on data fusion

Publications (2)

Publication Number Publication Date
CN118962682A CN118962682A (en) 2024-11-15
CN118962682B true CN118962682B (en) 2025-01-17

Family

ID=93407906

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202411440777.XA Active CN118962682B (en) 2024-10-16 2024-10-16 Target course angle fitting processing method of 4D millimeter wave Lei Dadian cloud based on data fusion

Country Status (1)

Country Link
CN (1) CN118962682B (en)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114841263A (en) * 2022-04-29 2022-08-02 重庆长安汽车股份有限公司 Information fusion method and storage medium for target course angle of automatic driving
CN114839615A (en) * 2022-04-28 2022-08-02 重庆长安汽车股份有限公司 Target course angle fitting method for 4D millimeter wave radar and storage medium

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102334641B1 (en) * 2019-01-30 2021-12-03 바이두닷컴 타임즈 테크놀로지(베이징) 컴퍼니 리미티드 Map Partitioning System for Autonomous Vehicles
KR102376709B1 (en) * 2019-01-30 2022-03-18 바이두닷컴 타임즈 테크놀로지(베이징) 컴퍼니 리미티드 Point cloud registration system for autonomous vehicles
CN114137509B (en) * 2021-11-30 2023-10-13 南京慧尔视智能科技有限公司 Millimeter wave Lei Dadian cloud clustering method and device
CN117554988A (en) * 2023-11-13 2024-02-13 中冶赛迪工程技术股份有限公司 Method for detecting and tracking dynamic obstacle target of inspection robot
CN118485992A (en) * 2024-05-15 2024-08-13 安徽台创智能科技有限公司 All-weather vehicle-mounted sensing system based on multi-millimeter wave radar

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114839615A (en) * 2022-04-28 2022-08-02 重庆长安汽车股份有限公司 Target course angle fitting method for 4D millimeter wave radar and storage medium
CN114841263A (en) * 2022-04-29 2022-08-02 重庆长安汽车股份有限公司 Information fusion method and storage medium for target course angle of automatic driving

Also Published As

Publication number Publication date
CN118962682A (en) 2024-11-15

Similar Documents

Publication Publication Date Title
CN107341819B (en) Target tracking method and storage medium
CN110988912B (en) Road target and distance detection method, system and device for automatic driving vehicle
WO2020154970A1 (en) Deep learning–based feature extraction for lidar localization of autonomous driving vehicles
CN110674705B (en) Small-sized obstacle detection method and device based on multi-line laser radar
US11400940B2 (en) Crosswind risk determination
CN114332221B (en) Semantic-based loop detection method, device, electronic device and storage medium
JP7418476B2 (en) Method and apparatus for determining operable area information
CN114325682A (en) Vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar
WO2022078342A1 (en) Dynamic occupancy grid estimation method and apparatus
CN114556419B (en) Three-dimensional point cloud segmentation method and device, and movable platform
Liu et al. Vehicle detection and ranging using two different focal length cameras
CN112446466A (en) Measuring confidence in deep neural networks
CN114994635A (en) A kind of intelligent driving drivable area detection method and device
CN114693909A (en) Microcosmic vehicle track sensing equipment based on multi-sensor machine vision fusion
CN115792891A (en) Target track tracking method based on fusion of multi-millimeter-wave radar and laser radar
CN117392173A (en) BEV-based 3D multi-target tracking method, system, equipment and medium
KR102831462B1 (en) Apparatus for detecting object of vehicle and method thereof
CN114049767B (en) Edge computing method and device and readable storage medium
CN115649178A (en) Road boundary detection method and electronic equipment
JP2021026683A (en) Distance estimation apparatus
CN119694162A (en) Collision warning method, device, electronic device, readable storage medium and program product
CN118962682B (en) Target course angle fitting processing method of 4D millimeter wave Lei Dadian cloud based on data fusion
CN118968465A (en) Target tracking method, device, vehicle and storage medium
CN112050829B (en) Motion state determination method and device
CN118521984A (en) Lane center line determining method, device, equipment and storage medium

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant