阅读说明：本技术 一种多毫米波雷达协同定位跟踪方法 (Multi-millimeter-wave radar cooperative positioning tracking method ) 是由 陈文亮 袁常顺 王俊 向洪 李晨龙 王峰 周杨 罗雨泉 赵建峰 于 2021-09-09 设计创作，主要内容包括：本发明公开了一种多毫米波雷达协同定位跟踪方法。为了克服现有技术存在因多径现象产生的虚拟目标的问题；本发明包括以下步骤：S1：安装多毫米波雷达协同定位跟踪系统,在定位场景中设置若干波束范围存在重叠的雷达；S2：根据多雷达坐标系参数标定算法,获得各雷达坐标与绝对坐标系的旋转角与平移矩阵；S3：基于雷达坐标系与绝对坐标系的旋转角与平移矩阵,将雷达探测到的点云信息映射到绝对坐标系中；S4：根据雷达探测到的点云信息与预测轨迹点的距离,计算轨迹信息。通过点与轨迹的匹配,在一定程度上能够消除多径、扰动产生的孤立点。(The invention discloses a multi-millimeter wave radar cooperative positioning and tracking method. In order to overcome the problem of virtual target generated by multipath phenomenon in the prior art; the invention comprises the following steps: s1: installing a multi-millimeter wave radar cooperative positioning and tracking system, and setting a plurality of radars with overlapped wave beam ranges in a positioning scene; s2: obtaining rotation angles and translation matrixes of each radar coordinate and an absolute coordinate system according to a multi-radar coordinate system parameter calibration algorithm; s3: mapping point cloud information detected by a radar into an absolute coordinate system based on rotation angles and translation matrixes of the radar coordinate system and the absolute coordinate system; s4: and calculating track information according to the distance between the point cloud information detected by the radar and the predicted track point. Through the matching of the points and the tracks, isolated points generated by multipath and disturbance can be eliminated to a certain extent.)

1. A multi-millimeter wave radar cooperative positioning tracking method is characterized by comprising the following steps:

s1: installing a multi-millimeter wave radar cooperative positioning and tracking system, and setting a plurality of radars with overlapped wave beam ranges in a positioning scene;

s2: obtaining rotation angles and translation matrixes of each radar coordinate and an absolute coordinate system according to a multi-radar coordinate system parameter calibration algorithm;

s3: mapping point cloud information detected by a radar into an absolute coordinate system based on rotation angles and translation matrixes of the radar coordinate system and the absolute coordinate system;

s4: and calculating track information according to the distance between the point cloud information detected by the radar and the predicted track point.

2. The method according to claim 1, wherein the system comprises a monitoring unit and a plurality of detection units which are in communication connection; the detection unit comprises a radar, a singlechip and a communication module, the radar is connected with the signal input end of the singlechip, and the communication module is connected with the communication port of the singlechip; at least one of the communication modules is a communication gateway and is used for sending the radar data of other detection units to the monitoring unit.

3. The method for co-locating and tracking by using multiple millimeter wave radars according to claim 1 or 2, wherein the a radar coordinate system is used as an absolute coordinate system, and the rotation angle matrix of the points in the B radar coordinate system mapped to the a radar coordinate system is as follows:

wherein θ is a rotation angle;

the translation matrix is:

wherein, t_BAxIs the x-axis translation;

t_BAyis the y-axis translation.

4. The multi-millimeter wave radar co-location tracking method according to claim 3, wherein the point in the B radar coordinate system is mapped to the point in the A radar as represented by:

A_Bi(R_BA，T_BA)＝R_BAB_i+T_BA

wherein, B_iAnd the coordinates of the ith point in the radar coordinate system B.

5. The multi-millimeter wave radar co-location tracking method according to claim 4, wherein an objective function is established:

wherein A is_iThe coordinate of the ith point in the radar coordinate system A is obtained;

n is the total number of points detected by the radar.

6. The multi-millimeter wave radar co-location tracking method according to claim 5, wherein a partial derivative of the objective function to the rotation angle and the translation amount is obtained according to the objective function;

the partial derivative of the objective function over the rotation angle is:

wherein R'_BAIs R_BADerivative to θ:

the partial derivative of the objective function with respect to the translation is:

obtaining an iterative formula of the rotation angle and the translation amount according to the partial derivative of the target function to the rotation angle and the translation amount:

wherein m is the number of iteration steps;

η is the iteration step of the rotation angle θ;

μ_BAis the iteration step size of the translation matrix.

7. The multi-millimeter wave radar co-location tracking method according to claim 1, 4, 5 or 6, wherein the step S4 comprises the steps of:

s401: acquiring point cloud data acquired by a radar in real time, wherein the radar point cloud data set is S₀；

S402: a radar point cloud data set S₀Each point in the point cloud is mapped to an absolute coordinate system, and the mapped point cloud set is S₁；

S403: computing a point cloud set S₁Point P in_iIf the distance between the point and all track prediction points is smaller than a threshold value, the point is added into a detection point set of the track, and if the distance is larger than the threshold value, an isolated point set is added;

s404: taking the average value of the detection point set of the track as a detection point of the track, and bringing the detection point into a historical track point of the track;

s405: calculating a predicted point of each track by adopting a Kalman filtering algorithm according to the historical track point of each track;

s406: obtaining a plurality of classes by adopting a DBscan clustering algorithm for all the points in the isolated point set, and calculating the average value of each class to be determined as a new track prediction point;

s407: judging whether the time difference between the last updating time and the current time of each track is greater than a time threshold, if so, judging that the track is lost, and deleting the track information;

s408: the process returns to step S401.

8. The multi-millimeter wave radar co-location tracking method according to claim 7, wherein in the step S403, all track prediction point sets U and a point cloud set S are recorded₁Point P in_iThe point with the shortest distance is Q; if point P_iIf the distance from the point Q is less than the threshold value, the point P is determined_iAnd adding the point Q into a detection point set of the track where the point Q is located, otherwise, putting the point Q into an isolated point set.

9. The method according to claim 7, wherein the step S405 specifically includes the following steps:

sequentially taking a track, determining system parameters of a Kalman filtering algorithm according to historical observation points of the track and calculating a covariance matrix of Kalman estimation errors;

calculating a predicted value of the historical track to the current target occurrence position according to the system parameters of the Kalman filtering algorithm and the covariance matrix of Kalman estimation errors;

calculating Kalman gain, recording the mean value of a detection point set of the track as a target current observation value, and predicting the position of the target at the next moment according to the current observation value and the Kalman gain;

and updating the covariance matrix of the Kalman estimation error.

Technical Field

The invention relates to the field of radar positioning, in particular to a multi-millimeter wave radar cooperative positioning and tracking method.

Background

With the temperature rise of concepts such as smart cities, smart homes and smart internet of things, the smart homes can bring intelligent experience to users by relying on positioning and tracking technology, and quality and life of the users are improved; also, smart traffic relies on location and tracking technology to bring new changes to citizens' safety and urban traffic order maintenance. The existing positioning and tracking technology is often dependent on a single sensor for monitoring, the single sensor is often limited by environment, functions, action range and the like, and the application scene is limited. The method is further directed to the defects of a single millimeter wave radar positioning and tracking system, such as short acting distance, small acting range, virtual targets generated indoors due to multipath phenomenon and the like.

For example, a "radar target positioning method based on multipath utilization" disclosed in chinese patent literature, whose publication number CN107918115B includes: 1. generating a transmitting signal to obtain echo data; 2. processing echo data to obtain arrival time of different paths; 3. establishing a radar target geometric positioning model; 4. bringing the obtained arrival time of different paths into the established geometric positioning model; 5. initializing an initial search point; 6. inputting the geometric positioning model and the initial search point into a search function lsqnolin, and searching by using the search function lsqnolin to obtain an accurate target position; 7. and expanding the target position obtained by searching to a three-dimensional space. The scheme has the defects of virtual targets and the like caused by multipath phenomenon.

Disclosure of Invention

The invention mainly solves the problem of virtual target generated by multipath phenomenon in the prior art; the method for the multi-millimeter wave radar cooperative positioning tracking is provided, false alarms are eliminated, targets are lost temporarily and continuously, and continuous tracking is achieved in a large range.

The technical problem of the invention is mainly solved by the following technical scheme:

a multi-millimeter wave radar cooperative positioning tracking method comprises the following steps:

s1: installing a multi-millimeter wave radar cooperative positioning and tracking system, and setting a plurality of radars with overlapped wave beam ranges in a positioning scene;

s2: obtaining rotation angles and translation matrixes of each radar coordinate and an absolute coordinate system according to a multi-radar coordinate system parameter calibration algorithm;

s3: mapping point cloud information detected by a radar into an absolute coordinate system based on rotation angles and translation matrixes of the radar coordinate system and the absolute coordinate system;

s4: and calculating track information according to the distance between the point cloud information detected by the radar and the predicted track point.

According to the scheme, through the matching of points and tracks, isolated points generated by multipath and disturbance can be eliminated to a certain extent; through the cooperation of multiple radars, the detection range can be enlarged to realize target positioning and tracking; target tracking under shielding can be realized through cooperation of partial radars; according to the prediction of the target tracking track, the target can be resumed after being temporarily lost to a certain extent.

Preferably, the multi-millimeter wave radar cooperative positioning and tracking system comprises a monitoring unit and a plurality of detection units which are in communication connection; the detection unit comprises a radar, a singlechip and a communication module, the radar is connected with the signal input end of the singlechip, and the communication module is connected with the communication port of the singlechip; at least one of the communication modules is a communication gateway and is used for sending the radar data of other detection units to the monitoring unit. The millimeter wave radar is RC7701N 32; the communication module can be Bluetooth, wifi or lora; the monitoring unit is responsible for data collection, coordinate calibration of the multi-millimeter wave radar, target tracking, interface display and other functions.

Preferably, taking the a radar coordinate system as an absolute coordinate system, the rotation angle matrix of the points in the B radar coordinate system mapped to the a radar coordinate system is:

wherein θ is a rotation angle;

the translation matrix is:

wherein, t_BAxIs the x-axis translation;

t_BAyis the y-axis translation.

Preferably, the expression that points in the B radar coordinate system are mapped to points in the a radar is:

A_Bi(R_BA，T_BA)＝R_BAB_i+T_BA

wherein, B_iAnd the coordinates of the ith point in the radar coordinate system B.

Because of each individual millimeter wave radar, the detected point is based on its own coordinate system, rather than a point in an absolute coordinate system. Therefore, in order to realize the positioning of the target, the coordinate system transformation parameters of each millimeter wave radar need to be calibrated. One radar is taken as an absolute coordinate system, and the coordinate system of the other radar is mapped below the absolute coordinate system through rotation and translation.

Preferably, an objective function is established:

wherein A is_iThe coordinate of the ith point in the radar coordinate system A is obtained;

n is the total number of points detected by the radar.

And the target function is to make the track point mapped to the A radar coordinate system under the B radar coordinate system as close as possible to the track point sampled by the A radar, and is measured by root mean square error.

Preferably, a partial derivative of the target function to the rotation angle and the translation amount is obtained according to the target function;

the partial derivative of the objective function over the rotation angle is:

wherein R'_BAIs R_BADerivative to θ:

the partial derivative of the objective function with respect to the translation is:

obtaining an iterative formula of the rotation angle and the translation amount according to the partial derivative of the target function to the rotation angle and the translation amount:

wherein m is the number of iteration steps;

η is the iteration step of the rotation angle θ;

μ_BAis the iteration step size of the translation matrix.

Preferably, the step S4 includes the following steps:

s401: acquiring point cloud data acquired by a radar in real time, wherein the radar point cloud data set is S₀；

S402: a radar point cloud data set S₀Each point in the point cloud is mapped to an absolute coordinate system, and the mapped point cloud set is S₁；

S403: computing a point cloud set S₁Point P in_iIf the distance between the point and all track prediction points is smaller than a threshold value, the point is added into a detection point set of the track, and if the distance is larger than the threshold value, an isolated point set is added;

s404: taking the average value of the detection point set of the track as a detection point of the track, and bringing the detection point into a historical track point of the track; s405: calculating a predicted point of each track by adopting a Kalman filtering algorithm according to the historical track point of each track;

s406: obtaining a plurality of classes by adopting a DBscan clustering algorithm for all the points in the isolated point set, and calculating the average value of each class to be determined as a new track prediction point;

s407: judging whether the time difference between the last updating time and the current time of each track is greater than a time threshold, if so, judging that the track is lost, and deleting the track information;

s408: the process returns to step S401.

Through the matching of points and tracks, isolated points generated by multipath and disturbance can be eliminated to a certain extent; through the cooperation of multiple radars, the detection range can be enlarged to realize target positioning and tracking; target tracking under shielding can be realized through cooperation of partial radars; according to the prediction of the target tracking track, the target can be resumed after being temporarily lost to a certain extent.

Preferably, in step S403, all the trajectory prediction point sets U and the point cloud set S are recorded₁Point P in_iThe point with the shortest distance is Q; if point P_iIf the distance from the point Q is less than the threshold value, the point P is determined_iAnd adding the point Q into a detection point set of the track where the point Q is located, otherwise, putting the point Q into an isolated point set. Can eliminate the isolated points generated by multipath and disturbance.

Preferably, the step S405 specifically includes the following steps:

sequentially taking a track, determining system parameters of a Kalman filtering algorithm according to historical observation points of the track and calculating a covariance matrix of Kalman estimation errors;

calculating a predicted value of the historical track to the current target occurrence position according to the system parameters of the Kalman filtering algorithm and the covariance matrix of Kalman estimation errors;

calculating Kalman gain, recording the mean value of a detection point set of the track as a target current observation value, and predicting the position of the target at the next moment according to the current observation value and the Kalman gain;

and updating the covariance matrix of the Kalman estimation error.

The specific process of calculating the predicted point of the trajectory.

The invention has the beneficial effects that:

1. according to the invention, through the matching of points and tracks, isolated points generated by multipath and disturbance can be eliminated to a certain extent;

2. through the cooperation of multiple radars, the detection range can be enlarged to realize target positioning and tracking;

3. target tracking under shielding can be realized through cooperation of partial radars;

4. according to the prediction of the target tracking track, the target can be resumed after being temporarily lost to a certain extent.

Drawings

Fig. 1 is a flowchart of a multi-millimeter wave radar co-location tracking method according to the present invention.

Detailed Description

The technical scheme of the invention is further specifically described by the following embodiments and the accompanying drawings.

Example (b):

as shown in fig. 1, the method for tracking multiple millimeter wave radars in a coordinated positioning manner in this embodiment includes the following steps:

s1: and installing a multi-millimeter wave radar cooperative positioning and tracking system, and setting a plurality of radars with overlapped beam ranges in a positioning scene.

The multi-millimeter wave radar cooperative positioning and tracking system comprises a monitoring unit and a plurality of detection units which are in communication connection.

The detection unit comprises a radar, a singlechip and a communication module, the radar is connected with the signal input end of the singlechip, and the communication module is connected with the communication port of the singlechip; at least one of the communication modules is a communication gateway and is used for sending the radar data of other detection units to the monitoring unit.

In the present embodiment, the millimeter wave radar is RC7701N 32; the communication module can be Bluetooth, wifi or lora; the monitoring unit is responsible for data collection, coordinate calibration of the multi-millimeter wave radar, target tracking, interface display and other functions.

S2: and obtaining a rotation angle and a translation matrix of each radar coordinate and an absolute coordinate system according to a multi-radar coordinate system parameter calibration algorithm.

Because of each individual millimeter wave radar, the detected point is based on its own coordinate system, rather than a point in an absolute coordinate system. Therefore, in order to realize the positioning of the target, the coordinate system transformation parameters of each millimeter wave radar need to be calibrated. One radar is taken as an absolute coordinate system, and the coordinate system of the other radar is mapped below the absolute coordinate system through rotation and translation.

Taking the A radar coordinate system as an absolute coordinate system, and mapping the points in the B radar coordinate system to the rotation angle matrix of the A radar coordinate system as follows:

wherein θ is a rotation angle;

the translation matrix is:

wherein, t_BAxIs the x-axis translation;

t_BAyis the y-axis translation.

The point under the B radar coordinate system is mapped to the point under the A radar and expressed as:

A_Bi(R_BA，T_BA)＝R_BAB_i+T_BA

wherein, B_iAnd the coordinates of the ith point in the radar coordinate system B.

Establishing an objective function:

wherein A is_iThe coordinate of the ith point in the radar coordinate system A is obtained;

n is the total number of points detected by the radar.

And the target function is to make the track point mapped to the A radar coordinate system under the B radar coordinate system as close as possible to the track point sampled by the A radar, and is measured by root mean square error.

Obtaining a partial derivative of the target function to the rotation angle and the translation amount according to the target function;

the partial derivative of the objective function over the rotation angle is:

wherein R'_BAIs R_BADerivative to θ:

the partial derivative of the objective function with respect to the translation is:

obtaining an iterative formula of the rotation angle and the translation amount according to the partial derivative of the target function to the rotation angle and the translation amount:

wherein m is the number of iteration steps;

η is the iteration step of the rotation angle θ;

μ_BAis the iteration step size of the translation matrix.

S3: and mapping the point cloud information detected by the radar into the absolute coordinate system based on the rotation angle and the translation matrix of the radar coordinate system and the absolute coordinate system.

S4: and calculating track information according to the distance between the point cloud information detected by the radar and the predicted track point.

S401: acquiring point cloud data acquired by a radar in real time, wherein the radar point cloud data set is S₀。

S402: a radar point cloud data set S₀Each point in the point cloud is mapped to an absolute coordinate system, and the mapped point cloud set is S₁。

S403: computing a point cloud set S₁Point P in_iAnd (4) adding the distance between the distance and all track prediction points into a detection point set of the track if the distance is less than a threshold value, and putting into an isolated point set if the distance is greater than the threshold value.

Recording all track prediction point sets U and point cloud set S₁Point P in_iThe point with the shortest distance is Q; if point P_iIf the distance from the point Q is less than the threshold value, the point P is determined_iAnd adding the point Q into a detection point set of the track where the point Q is located, otherwise, putting the point Q into an isolated point set.

S404: and taking the average value of the detection point set of the track as a detection point of the track, and bringing the detection point into the historical track point of the track.

S405: and calculating the predicted points of the tracks by adopting a Kalman filtering algorithm according to the historical track points of each track.

Sequentially taking a track, determining system parameters of a Kalman filtering algorithm according to historical observation points of the track and calculating a covariance matrix of Kalman estimation errors;

calculating a predicted value of the historical track to the current target occurrence position according to the system parameters of the Kalman filtering algorithm and the covariance matrix of Kalman estimation errors;

calculating Kalman gain, recording the mean value of a detection point set of the track as a target current observation value, and predicting the position of the target at the next moment according to the current observation value and the Kalman gain;

and updating the covariance matrix of the Kalman estimation error.

S406: and (3) obtaining a plurality of classes for all the points in the isolated point set by adopting a DBscan clustering algorithm, and calculating the average value of each class to be determined as a new track prediction point.

Here, the track prediction point is the same in nature as the track prediction point in step S405, but is generated in a different manner, for example: the formation of a track is bound to be composed of a plurality of historical track points, the position of a target which is possibly generated is predicted through the historical track, the meaning of the track prediction point in the step S405 is the meaning of the track prediction point, at the beginning of the generation of any track, the track is bound to be formed by continuously accumulating isolated points, a plurality of classes are separated through a DBscan clustering algorithm, the center of each class is marked as a new track prediction point, and the track prediction point is used for converging point clouds detected by a radar in the next frame. In step S403, the points in the point cloud sequentially calculate the distance to each trajectory prediction point in the set of trajectory prediction points, so that the trajectory prediction points mentioned in step S403 include both types of trajectory prediction points.

S407: and judging whether the time difference between the last updating time and the current time of each track is greater than a time threshold, if so, judging that the track is lost, and deleting the track information.

S408: the process returns to step S401.

According to the scheme of the embodiment, through the matching of the points and the tracks, isolated points generated by multipath and disturbance can be eliminated to a certain extent; through the cooperation of multiple radars, the detection range can be enlarged to realize target positioning and tracking; target tracking under shielding can be realized through cooperation of partial radars; according to the prediction of the target tracking track, the target can be resumed after being temporarily lost to a certain extent.

It should be understood that the examples are for illustrative purposes only and are not intended to limit the scope of the present invention. Further, it should be understood that various changes or modifications of the present invention may be made by those skilled in the art after reading the teaching of the present invention, and such equivalents may fall within the scope of the present invention as defined in the appended claims.