A human trajectory tracking detection method based on millimeter wave radar

Abstract

The application discloses a human body trajectory tracking detection method based on a millimeter wave radar and relates to the technical field of millimeter wave radar detection. The application comprises the following steps: a detection algorithm is used for the millimeter wave radar, human body related information is obtained by processing echo signals, a Kalman filtering algorithm is used to predict the position of the human body to obtain a predicted target position at the next moment, a global nearest neighbor algorithm is used to match the position of the human body detected at the next moment with a position of a current human body motion trajectory predicted at the next moment, and the matched human body trajectory information is updated and the current estimated human body position is corrected. The application obtains a human body target by using millimeter wave radar detection, performs statistical analysis and extracts related information, and corrects the human body trajectory between a prediction and an actual trajectory, so that the human body trajectory tracking detection precision is improved, and the application has the advantages of high real-time performance, stable tracking, and strong portability.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of millimeter wave radar detection, and particularly relates to a human trajectory tracking detection method based on a millimeter wave radar, which can be widely applied to intelligent driving, security monitoring, smart home and the like. BACKGROUND

[0002] The millimeter wave radar refers to a radar working in a 30-300GHz frequency domain for detection, and has some advantages of microwave radars and photoelectric radars. The millimeter wave radar has a small antenna aperture, a narrow beam, a large bandwidth, a high Doppler frequency, good anti-stealthness and the like.

[0003] With the development and application of the millimeter wave radar detection technology in the fields of intelligent driving, security monitoring, smart home, military guidance and the like, in order to improve the user experience and provide better millimeter wave radar detection technology products, the millimeter wave radar detection technology is required to be higher and higher in human detection.

[0004] For example, a human detection method and device in patent No. CN202110134941.4 utilize a region detection algorithm to detect and judge that a target enters a specified region, and utilize a multiple micro-motion detection algorithm to detect the intermediate frequency signal to judge whether there is a person in the specified region, but cannot achieve human trajectory tracking and prediction. SUMMARY

[0005] The application aims to provide a human trajectory tracking detection method based on a millimeter wave radar, which continuously acquires human detection related information through a radar sensor, and establishes a motion trajectory according to the information; the human detection result is matched with the motion trajectory by using a nearest neighbor algorithm, and a Kalman filter is used to update the motion trajectory information, so that the problem of insufficient accuracy of the existing millimeter wave human detection trajectory tracking is solved.

[0006] To solve the above technical problems, the application is implemented by the following technical scheme:

[0007] The application is a human trajectory tracking detection method based on a millimeter wave radar, which comprises the following steps:

[0008] Step S1: a detection algorithm is used for the millimeter wave radar, and human related information is obtained by processing the echo signal;

[0009] Step S2: a Kalman filtering algorithm is used to predict the human position to obtain a predicted target position at the next moment;

[0010] Step S3: a global nearest neighbor algorithm is used to match the human position detected at the next moment with the next moment position predicted by the existing human motion trajectory; if there are multiple targets in the scene, the algorithm is used to assign different human motion trajectories to the targets;

[0011] Step S4: updating and correcting the current estimated human position for the matched human trajectory information, and updating and recording the un-matched human motion trajectory as invisible state;

[0012] Step S5: deleting the trajectory of the invisible total frame number exceeding the threshold and the continuous multiple frames in the invisible state, and establishing the corresponding motion trajectory for the un-matched measurement in step S3;

[0013] In step S3, the nearest neighbor algorithm is used to match the detection result and the tracking predicted trajectory, and the specific process is as follows:

[0014] Step S31: judging the number of trajectories N and the number of detections M in the current frame, and taking different solutions according to the number combination of the two;

[0015] Step S32: generating a cost matrix Cost[N][M];

[0016] Step S33: generating a padded matrix Padded_Cost[N+M][N+M];

[0017] Step S34: judging the position of Padded_Cost[rows][cols] to generate a motion trajectory.

[0018] As a preferred technical solution, in step S1, the human-related information is the position information, speed information and angle information of the target human obtained by the millimeter wave radar.

[0019] As a preferred technical solution, in step S2, the Kalman filter algorithm adopts high frequency data frame acquisition, and the human motion between frames can be regarded as uniform motion; let the horizontal and vertical coordinates of the human at time k be x(k) and y(k), and the speeds be v x (k) and v y (k), then the calculation formula at time k+1 is as follows:

[0020]

[0021] The state X k+1 at time k+1 is taken as:

[0022] X k+1 = [x(k+1) v x (k+1) y(k+1) v y (k+1)] T ;

[0023] The state equation of the system is:

[0024] X k+1 = AX k;

[0025] The state transition matrix is:

[0026]

[0027] The position information of the target is obtained during observation, and the measurement equation is:

[0028] Z k+1 = HX k+1 + V k+1 ;

[0029] In the formula, V k is measurement noise,

[0030] The measurement at the k+1 moment is:

[0031] Z k+1 = [z x (k+1) z y (k+1)] T = [x(k+1) y(k+1)] T .

[0032] As a preferred technical solution, in the step S31, the situation is judged according to the combination of the number of inner tracks N and the number of detections M as follows:

[0033] If there is no track and no detection (N=0 and M=0), the function is exited;

[0034] If there is a track but no detection (N≠0 and M=0), the serial numbers of all effective tracks in the current frame are recorded and stored in the unmatched track array, and then the function is exited;

[0035] If there is a detection but no track (N=0 and M≠0), the serial numbers of all detections in the current frame are recorded and stored in the unmatched detection array, and then the function is exited;

[0036] If there is a track and a detection (N≠0 and M≠0), the Mahalanobis distance between the track and the detection is calculated, and the cost matrix Cost[N][M] is generated.

[0037] As a preferred technical solution, in the step S32, the specific process of generating the cost matrix Cost[N][M] is as follows:

[0038] Step S321: Obtain the horizontal and vertical coordinates of the center point of the ith track;

[0039] Step S322: Calculate the Mahalanobis distance between the ith track and the jth track;

[0040]

[0041] In the formula, x, y are the horizontal and vertical coordinates of the trajectory center point, x1, y1 are the horizontal and vertical coordinates of the detection center point;

[0042] Step S323: generate the matrix Cost(i,j)=Euclidean.

[0043] Step S324: traverse the cost matrix Cost[N][M] to find the maximum value max of the matrix.

[0044] As a preferred technical solution, in the step S33, a padded matrix Padded_Cost[N+M][N+M] is generated; wherein the elements on the diagonal of Padded_Cost[N][N] and Padded_Cost[M][M] are equal to 30, and the rest of the elements are all max.

[0045] As a preferred technical solution, in the step S34,

[0046] If the padded matrix N of Padded_Cost[N+M][N+M] is greater than M, each column of the padded matrix is traversed, and if Padded_Cost[rows][cols] is the minimum element min of the cols column, then Padded_Cost[rows,cols+1:M+N]=31.

[0047] On the contrary, each row of the padded matrix is traversed, and if Padded_Cost[rows][cols] is the minimum element min of the rows column, then Padded_Cost[rows+1:N+M,cols]=31.

[0048] As a preferred technical solution, the position judgment method of Padded_Cost[rows][cols] is as follows:

[0049] When row<N&&cols<M, the cols detection is matched with the rows trajectory, the number of matched trajectories is increased by 1, the motion trajectory is updated, and it is judged whether the number of trajectories exceeds the threshold value, and if it exceeds, the first trajectory is deleted.

[0050] When rows<N&&M≤col<N+M, the rows trajectory is an unmatched trajectory, the number of unmatched trajectories is increased by 1, and the trajectory is marked as invisible and updated.

[0051] When N≤rows<N+M&&cols<M, the cols detection is an unmatched detection, the number of unmatched detections is increased by 1, and a new motion trajectory is created.

[0052] The present application has the following beneficial effects:

[0053] The application utilizes millimeter wave radar to detect human targets, carries out statistical analysis and extracts relevant information, improves human trajectory tracking detection precision through human trajectory prediction and correction between actual trajectories, and has the advantages of strong real-time performance, stable tracking and strong portability.

[0054] Of course, implementing any product of the application does not necessarily need to achieve all the advantages described above at the same time. BRIEF DESCRIPTION OF DRAWINGS

[0055] In order to more clearly illustrate the technical solutions of the embodiments of the application, the drawings needed for the embodiment description will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the application, and other drawings can be obtained by those skilled in the art without creative labor on the basis of these drawings.

[0056] Figure 1 A flow chart of a human trajectory tracking detection method based on millimeter wave radar;

[0057] Figure 2 An application schematic diagram of a human trajectory tracking detection method based on millimeter wave radar on an air conditioner;

[0058] Figure 3 A schematic diagram of a generated padded matrix Padded_Cost[N+M][N+M]. DETAILED DESCRIPTION

[0059] The technical solutions in the embodiments of the application will be described clearly and completely below with reference to the drawings in the embodiments of the application. Obviously, the described embodiments are only some of the embodiments of the application, not all the embodiments. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the application.

[0060] Please refer to Figure 1 The application is a human trajectory tracking detection method based on millimeter wave radar, which comprises the following steps:

[0061] Step S1: A detection algorithm is used for millimeter wave radar, human related information is obtained by processing echo signals, and the human related information is position information, speed information and angle information of a target human obtained by millimeter wave radar, etc.

[0062] Step S2: A Kalman filter algorithm is used to predict the position of the human to obtain a predicted target position at the next time;

[0063] In step S2, the Kalman filtering algorithm adopts high frequency to collect data frames, and the human motion between frames can be regarded as uniform motion; let the horizontal and vertical coordinates of the human body at time k be x(k) and y(k), and the velocities be v x (k) and v y (k), then the calculation formula at time k+1 is as follows:

[0064]

[0065] The state X k+1 at time k+1 is taken as:

[0066] X k+1 = [x(k+1)v x (k+1)y(k+1)v y (k+1)] T ;

[0067] The state equation of the system is:

[0068] X k+1 = AX k ;

[0069] The state transition matrix is:

[0070]

[0071] The position information of the target is obtained in the observation process, and the measurement equation is:

[0072] Z k+1 = HX k+1 + V k+1 ;

[0073] In the formula, V k is measurement noise,

[0074] The measurement at time k+1 is:

[0075] Z k+1 = [z x (k+1)z y (k+1)] T = [x(k+1)y(k+1)] T .

[0076] Step S3: adopt the global nearest neighbor algorithm to match the position of the human body detected at the next time with the position of the present human motion trajectory predicted at the next time; if there are multiple targets in the scene, then assign different human motion trajectories according to the algorithm;

[0077] In step S3, the nearest neighbor algorithm is adopted to match the detection result with the tracking predicted trajectory, and the specific process is as follows:

[0078] Step S31: judging the number of current intra-track N and the number of detection M, and taking different solutions according to the number combination of the two;

[0079] Step S32: generating a cost matrix Cost[N][M];

[0080] Step S33: generating a padded matrix Padded_Cost[N+M][N+M];

[0081] Step S34: judging the position of Padded_Cost[rows][cols] to generate a motion track.

[0082] Step S4: updating and correcting the current estimated human position based on the matched human track information, and updating the unmatched human motion track while recording it as an invisible state;

[0083] Step S5: deleting the track of the total number of frames exceeding the threshold and keeping the invisible state for a plurality of consecutive frames, and establishing a corresponding motion track for the measurement in step S3 that is not matched;

[0084] In step S31, the situation is judged according to the combination of the number of intra-track N and the number of detection M as follows:

[0085] If there is no track and no detection (N=0 and M=0), then exit this function;

[0086] If there is a track but no detection (N≠0 and M=0), record the serial number of all valid tracks in the current frame and store it in the unmatched track array, and then exit this function;

[0087] If there is detection but no track (N=0 and M≠0), record the serial number of all detections in the current frame and store it in the unmatched detection array, and then exit this function;

[0088] If there is a track and there is detection (N≠0 and M≠0), calculate the Mahalanobis distance between the track and the detection, and generate a cost matrix Cost[N][M].

[0089] In step S32, the specific process of generating the cost matrix Cost[N][M] is as follows:

[0090] Step S321: obtaining the horizontal and vertical coordinates of the center point of the ith track;

[0091] Step S322: calculating the Mahalanobis distance between the ith track and the jth track;

[0092]

[0093] In the formula, x, y are the horizontal and vertical coordinates of the track center point, and x1, y1 are the horizontal and vertical coordinates of the detection center point.

[0094] Step S323: generate the matrix Cost(i,j)=Euclidean.

[0095] Step S324: traverse the cost matrix Cost[N][M] to find the maximum value max of the matrix.

[0096] Referring to Figure 3 As shown in the figure, in step S33, a padded matrix Padded_Cost[N+M][N+M] is generated; wherein the elements on the diagonal of Padded_Cost[N][N] and Padded_Cost[M][M] are equal to 30, and the rest of the elements are all max.

[0097] In step S34, if the padded matrix N of Padded_Cost[N+M][N+M] is greater than M, each column of the padded matrix is traversed; if Padded_Cost[rows][cols] is the minimum element min of the cols column, then Padded_Cost[rows,cols+1:M+N]=31.

[0098] Conversely, each row of the padded matrix is traversed; if Padded_Cost[rows][cols] is the minimum element min of the rows column, then Padded_Cost[rows+1:N+M,cols]=31.

[0099] The position of Padded_Cost[rows][cols] is determined as follows:

[0100] When row<N&&cols<M, (i.e. min falls within the first area Cost[N][M]), the cols detection matches the rows track, the number of matched tracks is incremented by 1, the motion track is updated, and it is determined whether the number of tracks exceeds the threshold value; if so, the first track is deleted.

[0101] When rows<N&&M≤col<N+M (i.e. min falls within the second area of the padded matrix), the rows track is an unmatched track, the number of unmatched tracks is incremented by 1, and the track is marked as invisible and updated.

[0102] When N≤rows<N+M&&cols<M (i.e. min falls within the third area of the padded matrix), the cols detection is an unmatched detection, the number of unmatched detections is incremented by 1, and a new motion track is created.

[0103] Embodiment One

[0104] As Figure 1 shown, the method of human trajectory tracking detection based on millimeter wave radar, in the human body detection stage, through the sensor constantly obtains the human body detection result in the scene, and the human body related information is obtained by statistics. In the human body tracking stage, the human body position in the motion trajectory is predicted by using Kalman filter, and the nearest neighbor algorithm is used to match it with the human body position detected at the next time, and the matching result is used to select the update human motion trajectory information or create a new motion trajectory, and the motion trajectory exceeding the threshold is deleted, so as to realize the method of human trajectory tracking detection suitable for millimeter wave radar, the specific process is as follows:

[0105] The human body reference information is obtained by constantly acquiring the position information, speed information and angle information of the target human body by millimeter wave radar.

[0106] The human body position prediction adopts Kalman filter algorithm to predict the human body position, and obtains the human body prediction position at the next time. High frequency data frame acquisition is adopted, and the human body motion between frame intervals can be regarded as uniform motion. The horizontal and vertical coordinates of human body at time k are x(k) and y(k), and the speed is v x (k) and v y (k), then at time k+1:

[0107]

[0108] The state X k+1 of k+1 time is:

[0109] X k+1 = [x(k+1)v x (k+1)y(k+1)v y (k+1)] T

[0110] At this time, the system state equation is:

[0111] X k+1 = AX k

[0112] The state transition matrix is:

[0113]

[0114] The position information of the target is obtained in the observation process, so the measurement equation is:

[0115] Z k+1 = HX k+1 + V k+1

[0116] Where V k is the measurement noise,

[0117] The measurement at the k+1 moment is:

[0118] Z k+1 = [z x (k+1)z y (k+1)] T = [x(k+1)y(k+1)] T

[0119] The human position tracking matching adopts the nearest neighbor algorithm to match the detection result and the tracking predicted trajectory. The specific process is as follows:

[0120] Judge the number of trajectories N and the number of detections M in the current frame, and adopt different solutions according to the number combination.

[0121] If there is no trajectory and no detection (N=0 and M=0), exit this function;

[0122] If there is a trajectory but no detection (N≠0 and M=0), record the serial number of all valid trajectories in the current frame and store it in the unmatched trajectory array, and then exit this function.

[0123] If there is detection but no trajectory (N=0 and M≠0), record the serial number of all detections in the current frame and store it in the unmatched detection array, and then exit this function.

[0124] If there is a trajectory and there is a detection (N≠0 and M≠0), calculate the Mahalanobis distance between the trajectory and the detection, and generate a cost matrix Cost[N][M] therefrom.

[0125] I. Obtain the horizontal and vertical coordinates of the center point of the ith trajectory;

[0126] II. Calculate the Mahalanobis distance between the ith trajectory and the jth trajectory;

[0127]

[0128] In the formula, x and y are the horizontal and vertical coordinates of the trajectory center point, and x1 and y1 are the horizontal and vertical coordinates of the detection center point.

[0129] III. Generate matrix Cost(i,j)=Euclidean;

[0130] IV. Traverse the cost matrix Cost[N][M] to find the maximum value max of the matrix.

[0131] Generate the Padded_Cost[N+M][N+M] matrix with edges; the elements on the diagonal of Padded_Cost[N][N] and Padded_Cost[M][M] are equal to 30, and the remaining elements are all max.

[0132] Matching core algorithm

[0133] If the edge-adding matrix N > M, iterate through each column of the edge-adding matrix. If Padded_Cost[rows][cols] is the minimum element min in the cols-th column, then Padded_Cost[rows,cols+1:M+N] = 31. Conversely, iterate through each row of the edge-adding matrix. If Padded_Cost[rows][cols] is the minimum element min in the rows-th column, then Padded_Cost[rows+1:N+M,cols] = 31.

[0134] Determine the position of Padded_Cost[rows][cols]:

[0135] When row < N && cols < M (i.e., min falls within region 1 Cost[N][M]), the cols-th detection matches the rows-th trajectory. The number of matched trajectories is incremented by 1, the motion trajectory is updated, and it is determined whether the number of trajectories exceeds the threshold. If it does, the initial trajectory is deleted.

[0136] If rows < N && M ≤ col < N + M (i.e., min falls within the second region of the edge matrix), then the row-th trajectory is an unmatched trajectory. The number of unmatched trajectories is incremented by 1, marked as invisible, and the trajectory is updated.

[0137] When N≤rows<N+M&&cols<M (i.e., min falls within the third region of the edge matrix), the cols-th detection is an unmatched detection, the number of unmatched detections is incremented by 1, and a new motion trajectory is created.

[0138] Example 2

[0139] like Figure 2 As shown, based on Embodiment 1, this method can be applied to air conditioners. Based on trajectory management information, the location information of personnel can be obtained in real time. When the target is far away from the air conditioner, the air conditioner delivers strong air and when the target is close to the air conditioner, the air conditioner delivers gentle air. When the position of the human body changes, the air delivery angle of the air conditioner changes with the position of the human body.

[0140] This enables intelligent control of air conditioning air supply, allowing the airflow to follow the user's movements and providing gentle, long-distance airflow. It can be widely used in fields such as intelligent driving, security monitoring, and smart homes.

[0141] It is worth noting that the various units included in the above system embodiments are only divided according to functional logic, but are not limited to the above division, as long as the corresponding functions can be achieved; in addition, the specific names of each functional unit are only for easy differentiation and are not used to limit the scope of protection of the present invention.

[0142] In addition, those skilled in the art can understand that all or part of the steps in the methods of the above-mentioned embodiments can be completed by a program instructing relevant hardware, and the corresponding program can be stored in a computer readable storage medium.

[0143] The preferred embodiments of the application disclosed above are only used to help explain the application. The preferred embodiments do not describe all of the details of the application, and the application is not limited to the specific embodiments described. Obviously, many modifications and variations can be made according to the content of the specification. The specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the application, so that those skilled in the art can well understand and utilize the application. The application is limited only by the claims and their full scope and equivalents.

Claims

1. A method for human trajectory tracking and detection based on millimeter-wave radar, characterized in that, Includes the following steps: Step S1: Use a detection algorithm on the millimeter-wave radar to process the echo signal to obtain relevant information about the human body; Step S2: Use the Kalman filter algorithm to predict the human body position to obtain the predicted target position at the next moment; Step S3: Use the global nearest neighbor algorithm to match the detected human position at the next moment with the predicted position at the next moment based on the existing human motion trajectory; if there are multiple targets in the scene, they are assigned to different human motion trajectories according to the algorithm. Step S4: Update the matched human trajectory information and correct the currently estimated human position; update the unmatched human motion trajectory and record it as invisible. Step S5: Delete the trajectories of consecutive frames that are invisible and whose total number of invisible frames exceeds the threshold and remain invisible, and establish the corresponding motion trajectory for the unmatched measurements in step S3. In step S3, the nearest neighbor algorithm is used to match the detection results with the predicted tracking trajectory. The specific process is as follows: Step S31: Determine the number of trajectories in the current frame. and number of tests Different solutions are adopted based on the combination of the two numbers; Step S32: Generate the cost matrix Cost[N][M]; Step S33: Generate the edge addition matrix ; Step S34: Determine The position is used to generate a motion trajectory; In step S31, based on the number of trajectories and number of tests The combination judgment is as follows: If there is no trajectory and no detection. and If so, then exit this function; If there is a trajectory but no detection and If the function fails, it records the sequence number of all valid trajectories in the current frame and stores them in the array of unmatched trajectories, then exits the function. If there is detection but no trajectory, and Record the sequence number of all detections in the current frame and store them in the array of unmatched detections, then exit this function; If there is a trajectory and detection is performed. and Calculate the Mahalanobis distance between the trajectory and the detection, and generate the cost matrix Cost[N][M]; In step S32, the specific process for generating the cost matrix Cost[N][M] is as follows: Step S321: Obtain the first The x and y coordinates of the center point of the trajectory; Step S322: Calculate the first... Trajectory and the first Mahalanobis distance between the trajectories; ； In the formula, The x and y coordinates of the center point of the trajectory are: The x and y coordinates of the detection center point; Step S323: Generate matrix ; Step S324: Traverse the cost matrix Find the maximum value of the matrix. ; In step S33, the edge-adding matrix is ​​generated. ;in, and The element on the diagonal is 30, and all other elements are 10. ; In step S34, if Edge matrix Iterate through each column of the edge-adding matrix, if For the first Minimum element of a column ,but ; Conversely, traverse each row of the edge-adding matrix, if For the first Minimum element of a column ,but .

2. The human body trajectory tracking and detection method based on millimeter-wave radar according to claim 1, characterized in that, In step S1, the human body-related information is the location, velocity, and angle information of the target human body obtained through millimeter-wave radar.

3. The human body trajectory tracking and detection method based on millimeter-wave radar according to claim 1, characterized in that, In step S2, the Kalman filter algorithm uses high-frequency data frame acquisition, and the human body movement between frame intervals can be considered as uniform motion; let the horizontal and vertical coordinates of the human body at time k be respectively... and The speeds are respectively and ,but The formula for calculating time is as follows: ； state of time Take as: ； The state equation of the system is: ； The state transition matrix is: ； The target's position information is obtained during the observation process, and the measurement equation is: ； In the formula To measure noise, ; but The measurement of time is as follows: 。 4. The human body trajectory tracking and detection method based on millimeter-wave radar according to claim 1, characterized in that, The The location is determined as follows: when Then the first Item detection and the first For each trajectory match, the number of matched trajectories is incremented by 1, the motion trajectory is updated, and it is determined whether the number of trajectories exceeds the threshold. If it does, the initial trajectory is deleted. when Then the first If a trajectory is an unmatched trajectory, the count of unmatched trajectories is incremented by 1, the trajectory is marked as invisible, and the trajectory is updated. when Then the first If a detection is not matched, the count of unmatched detections is incremented by 1, and a new motion trajectory is created.

Images