A ground unmanned platform tracking method based on multivariate data fusion

Abstract

The present invention belongs to the field of environmental perception technology for ground unmanned platforms, and specifically relates to a ground unmanned platform tracking method based on multivariate data fusion. A target kinematic model is constructed, and the position, speed, angle and angular velocity of the desired tracking target are used as state vectors. A nonlinear state model is constructed according to motion characteristics and process noise; a laser radar and a millimeter-wave radar are selected as physical sensors, and the laser radar data is distorted and corrected according to the positioning and orientation data of the ground unmanned platform, and the region of interest is extracted by voxel filtering, through filtering, minimum segmentation, Euclidean clustering, etc.; based on the nonlinear state equation, an unscented Kalman filter is selected to fuse the laser radar and millimeter-wave radar data; the position of the target is selected as the mission path point of the ground unmanned platform, and the lateral and longitudinal driving of the ground unmanned platform is realized by local path planning, velocity profile generation, pure tracking control, etc.

Description

Technical Field

[0001] The present invention belongs to the technical field of ground unmanned platform environment perception, and in particular relates to a ground unmanned platform tracking method based on multivariate data fusion. Background Art

[0002] When a ground unmanned platform performs an unfamiliar mission for the first time, it often needs to track and guide people or vehicles to maneuver. The key is that the ground unmanned platform can perceive the spatial position of the target in real time. With the rise of deep learning, two-dimensional target detection technology is booming and has been widely used in the field of monitoring. However, it cannot provide deep features and cannot be directly used in the autonomous navigation system of ground unmanned platforms. Although the lidar detection technology based on deep learning can provide accurate three-dimensional target detection contours, it is restricted by the acceleration performance of graphics cards and lidar harnesses. Summary of the invention

[0003] In view of the defects of the prior art, the purpose of the present invention is to provide a ground unmanned platform tracking method based on multivariate data fusion. The method aims at the problem of tracking personnel or vehicles, adopts lidar and millimeter-wave radar fusion technology, performs real-time detection of the target to be tracked, takes the position of the target as the task point of autonomous navigation, and performs autonomous maneuvering through a local path planner and pure tracking control.

[0004] The technical solution of the present invention is:

[0005] A ground unmanned platform tracking method based on multivariate data fusion, the method comprising the following steps:

[0006] The first step is to collect the motion information of the ground unmanned platform, including the linear velocity and angular velocity of the ground unmanned platform, and convert the collected linear velocity and angular velocity from the global coordinate system to the lidar coordinate system;

[0007] The second step is to obtain the laser radar point cloud data and millimeter wave radar data of the target to be tracked in real time;

[0008] The third step is to use the motion information of the ground unmanned platform collected in the first step to perform distortion compensation on each frame of the point cloud data in the second step to obtain compensated point cloud data;

[0009] In the fourth step, the voxel filtering method is used to downsample the compensated point cloud data obtained in the third step, so as to reduce the data volume of the point cloud as much as possible while retaining the geometric features of the point cloud, and to remove the noise points. The specific method is as follows: first, a three-dimensional voxel network with a size of 0.02m*0.02m*0.02m is created, and the compensated point cloud data is gridded. The centroid of all point clouds in each grid is used to approximately replace other points in the grid. The calculation formula of the point cloud centroid of each grid is expressed as:

[0010]

[0011] Where h is the number of point clouds in each grid; (x i ,y i , z i ) is the coordinate of the i-th point cloud in each grid; (x center ,y center , z center ) is the centroid coordinate of the point cloud in each grid;

[0012] The fifth step is to initialize the position of the target to be tracked, and then use the minimum segmentation algorithm to segment the foreground (point cloud belonging to the target) and background (point cloud not belonging to the target) of the input point cloud according to the center point coordinates and radius of the target to be tracked. The segmentation method is:

[0013] (1) A topological relationship diagram between points is constructed in each frame of point cloud data. Each vertex is connected to the source point and the sink point through an edge, and is also connected to the vertices in its own neighborhood, such as Figure 1 ;

[0014] (2) The weights constructed based on the Euclidean distance between points are:

[0015]

[0016] In the formula, dist is the distance between points. The longer the distance, the greater the possibility that the edge will be cut. σ is the weight function, which is 0.2m.

[0017] (3) Set the foreground and background penalty functions to reduce the target range. The foreground is the weight of the edge connecting the vertex and the source point, which is set to a constant of 0.5. The background is the weight of the edge connecting the vertex and the convergence point, backgroundPenalty, which is defined by the formula:

[0018]

[0019]

[0020] Where radius is the horizontal radius of the object, which is 0.5m; (x, y) is the point cloud coordinate; (CenterX, CenterY) is the target center coordinate; distanceToCenter is the distance from the point to the target center.

[0021] (4) Based on the weights between points, the foreground and background in the point cloud are segmented, and the foreground point cloud is clustered by Euclidean clustering to obtain the center position of the target.

[0022] The sixth step is to build a kinematic model of the target to be tracked. The specific steps are:

[0023] Construct a 5-dimensional state quantity based on the position, speed, angle and angular velocity of the target to be tracked;

[0024]

[0025] In the formula, (p x , p y ) is the position of the target in the x and y directions; v is the speed of the target; ψ is the direction of the target; is the angular velocity of the target.

[0026] By derivation and integration of the state quantity, the state equation is obtained:

[0027]

[0028] Where Δt is the time difference; x k 、x k+1 are the state vectors at time k and k+1 respectively.

[0029] According to the formula, if ψ k = 0, the result will tend to infinity, that is, the problem of the target moving along a straight line cannot be solved, so this situation is considered separately:

[0030]

[0031] After determining the fixed terms of the state equation, the process noise term also needs to be considered:

[0032]

[0033] The process model is described using a two-dimensional noise vector, with uncertainty v k It consists of two independent scalar process noises. The first process noise is the longitudinal acceleration noise v a，k , can be regarded as randomly distributed white noise with a mean of 0 and a variance of Take 0.01.

[0034]

[0035] The second process noise is angular acceleration noise It can be regarded as randomly distributed white noise with a mean of 0 and a variance of Take 0.01.

[0036]

[0037] Since both longitudinal acceleration and angular acceleration will affect the position p x 、p y Usually, the angular velocity of the target does not change much, and ignoring the angular acceleration, the effect of noise on the position can be characterized as:

[0038]

[0039] The effect of noise on speed, angle, and angular velocity can be characterized as:

[0040]

[0041] The kinematic model of the target to be tracked is:

[0042]

[0043] Step 7. Since the kinematic model of the target to be tracked is nonlinear, the unscented Kalman filter is used to predict the position of the target to be tracked. The specific method is as follows: first, the distribution of the current position of the target to be tracked is sampled to ensure that the distance between the sampling point and the distribution mean is an integer multiple of the standard deviation. In general, the n-dimensional Gaussian distribution selects 2n+1 sampling points, the first point is the mean of the Gaussian distribution, and the other points are symmetric about the mean.

[0044]

[0045] Wherein, μ is the mean; λ is a hyperparameter, taking λ=3-n=-2; the larger the λ, the farther the sampling point is from the mean of the Gaussian distribution, and the smaller the λ, the closer the sampling point is to the mean of the Gaussian distribution. In the target kinematic model constructed by the present invention, noise can also have a sub-linear effect on the model, so the covariance matrix P of the state equation can be characterized as:

[0046]

[0047] Where P′ is the 5x5 covariance matrix of the target kinematic model, and the element is the unit matrix of 0.01; Q is the covariance matrix of the noise.

[0048]

[0049] Then, according to the kinematic model of the target to be tracked, the nonlinear function g(·) (i.e., the state equation) is used to predict each sampling point:

[0050] χ k+1 =g(χ k ,μ k ) (17)

[0051] In the formula, χ k+1 is the sampling point predicted at time k+1; g(·) is the state equation.

[0052] Calculate the weight of each sampling point. The weight formula can be expressed as:

[0053]

[0054] Calculate the mean x of the new distribution based on the weight of each sampling point k+1∣k and variance P k+1∣k :

[0055]

[0056]

[0057] The target position prediction value is obtained by formula (19).

[0058] Step 8: Since the laser radar transforms from the predicted state space to the measured space in a linear manner, the linear function H l Convert the predicted state space into the measured state space:

[0059]

[0060] In the formula, Z k+1∣k is the transformed measurement space; x k+1∣k is the predicted state quantity; R is the measurement noise.

[0061] Since the millimeter-wave radar prediction state space to measurement space is a nonlinear transformation, it can be obtained by the nonlinear function H r Convert prediction space to measurement space:

[0062]

[0063] Where r is the distance between the target and the millimeter-wave radar, θ is the angle between the target and the millimeter-wave radar, and r' is the rate of change of the distance between the target and the millimeter-wave radar.

[0064] Predict the prior estimate of the mean z after the unscented transformation k+1∣k , prior estimate covariance matrix S k+1∣k It is expressed as:

[0065]

[0066]

[0067] The cross-correlation matrix of the state space and measurement space is expressed as:

[0068]

[0069] Calculate the Kalman gain:

[0070]

[0071] According to the measurement results of laser radar minimum segmentation, Euclidean clustering, and millimeter wave output results z k+1 (i.e., the coordinates of the target center point), calculate the posterior state x k+1∣k+1 and the posterior estimated covariance matrix P k+1∣k+1 , the specific method is:

[0072] P k+1∣k+1 =P k+1∣k -K k+1∣k S k+1∣k (27)

[0073] x k+1∣k+1 =x k+1∣k +K k+1∣k (z k+1 -z k+1∣k ) (28)

[0074] The target position is updated using formula (28).

[0075] In the ninth step, a cluster of candidate paths is generated online according to the position of the target to be tracked and the self-positioning of the ground unmanned platform. The path model uses a cubic Bezier curve, which has continuous first-order and second-order derivatives, ensuring that the planned path is executable by the unmanned platform. The cost, maximum allowable curvature and offset distance of each candidate path are weightedly calculated to obtain the evaluation value of each candidate path, and the smallest evaluation value is selected as the optimal path.

[0076] J i =ω1c i +w2k imax +ω3Δd i (29)

[0077] Where: c i is the passing cost of the ith alternative path; k imax is the maximum curvature of the ith alternative path; Δd i The offset distance of the i-th alternative path; ω1, ω2, and ω3 are the weights of the passing cost, maximum curvature, and offset distance, respectively.

[0078] The tenth step is to plan the expected speed of each point on the trajectory according to the optimal path. For the optimal path, the target speed of the end point is set to zero. It takes three stages to go from the current speed to zero, namely, acceleration, uniform speed and deceleration. The acceleration stage corresponds to accelerating from the current speed vC to the expected speed vD, the deceleration stage corresponds to accelerating from the expected speed vD to zero speed, and the others are uniform speed stages. According to the acceleration and deceleration of the platform, the trajectory lengths corresponding to the three stages can be calculated to obtain the velocity profile.

[0079] In the eleventh step, a pure tracking algorithm is used to calculate the motion control instructions, including the platform travel speed v and angular velocity w, where (x v ,y v ) is the platform position, (x G ,y G ) is the position of the target point on the global path, O is the instantaneous turning center of the platform, R is the turning radius, L is the distance between the platform and the target point, that is, the preview distance, and α is the deviation angle between the platform heading and the target position;

[0080] The deviation angle between the heading of the ground unmanned platform and the position of the target to be tracked is:

[0081]

[0082] The arc is tangent to the platform heading, and the central angle of the arc is:

[0083] ∠GOS=2α (31)

[0084] Therefore, we get the following relationship in the triangle ΔGOS:

[0085]

[0086] Therefore, according to the control speed v output by the optimal path, the angular velocity of the ground unmanned platform can be obtained as:

[0087]

[0088] Step 12: Considering that the unmanned patrol car moves at a low speed, ignoring the tire slip angle, the steering relationship can be approximated by the Ackerman steering geometry model. Combined with the formula, in the case of a small turning angle, the turning angles of the left and right wheels are:

[0089]

[0090]

[0091] Step 13. Based on the input speed profile and the steering angles of the left and right wheels, the chassis controller can output the torque of the wheel hub motor, and the Electric Power Steering (EPS) system can output the torque of the steering motor, thereby achieving the following driving of the ground platform, including longitudinal and lateral movement.

[0092] Beneficial Effects

[0093] (1) The method of the present invention constructs a target kinematic model, takes the position, speed, angle and angular velocity of the target to be tracked as the state vector, and constructs a nonlinear state model according to the motion characteristics and process noise; selects laser radar and millimeter wave radar as physical sensors, performs distortion correction on the laser radar data according to the positioning and orientation data of the ground unmanned platform, and uses voxel filtering, pass filtering, minimum segmentation, Euclidean clustering, etc. to realize the extraction of the region of interest; based on the nonlinear state equation, selects unscented Kalman filtering to fuse the laser radar and millimeter wave radar data; selects the position of the target as the task path point of the ground unmanned platform, and realizes the lateral and longitudinal driving of the ground unmanned platform through local path planning, velocity profile generation, pure tracking control, etc.

[0094] (2) The target kinematic model is constructed as follows: the position, velocity, angle and angular velocity of the target are selected as state vectors, and the target motion process, i.e., the state equation, is derived according to the target motion characteristics and calculus techniques; considering that the target angular velocity is 0, i.e., when the target moves in a straight line, the state will tend to infinity, which is considered as a special case; in addition, the longitudinal acceleration noise and angular acceleration noise are regarded as randomly distributed white noise as process noise terms.

[0095] (3) The distortion correction of the laser radar data based on the positioning and orientation data of the ground unmanned platform is as follows: the linear velocity and angular velocity of the ground unmanned platform are collected and converted into the laser radar coordinate system; the original point cloud is rotated using the azimuth angle of the first laser point, the original point cloud is converted into the world coordinate system, and the linear velocity and angular velocity are updated; the subsequent point clouds are rotated and translated using the current scanning angle; and the dedistorted point cloud is rotated back to the laser radar coordinate system according to the initial rotation matrix.

[0096] (4) The minimum segmentation is as follows: manually initializing the position of the target, constructing a topological relationship diagram between points in each frame of point cloud data; constructing weights based on the Euclidean distance between points, and setting the weights to be smaller the farther away from the target; setting foreground and background penalty functions to segment the foreground and background of the input point cloud; and extracting the position of the target by performing Euclidean clustering on the foreground of the point cloud.

[0097] (5) The local path planning is as follows: based on the position of the output target and the self-positioning of the ground unmanned platform, a cluster of alternative paths is generated online. The path model uses a cubic Bezier curve, which has continuous first-order and second-order derivatives to ensure that the planned path is executable by the unmanned platform; the cost, maximum allowable curvature and offset distance of each alternative path are weightedly calculated to obtain the evaluation value of each alternative path, and the minimum evaluation value is selected as the optimal path.

[0098] (6) The velocity profile is generated as follows: for the desired path, the velocity of the ground unmanned platform to reach the target point is set to zero, and the current velocity of the ground unmanned platform to the target point is divided into three stages, namely, acceleration, constant speed and deceleration, so as to obtain the velocity profile.

[0099] (7) The pure tracking control is as follows: a pure tracking algorithm is used to calculate the curvature of the ground unmanned platform; the tire slip angle of the ground unmanned platform when driving at a low speed is ignored, and the steering angle of the left and right front wheels is obtained using the Ackerman steering geometry model; according to the input speed profile and the steering angle of the front wheel, the chassis controller can output the torque of the hub motor, and the electric power steering system can output the torque of the steering motor, thereby realizing the following driving of the ground platform, including longitudinal movement and lateral movement.

[0100] (8) The present invention relates to a ground unmanned platform tracking algorithm based on multivariate data fusion. The position, speed, angle and angular velocity of the desired tracking target are used as the state vector, and the target kinematic model is constructed according to the motion characteristics and process noise of the target. The original point cloud of the laser radar is distorted by the positioning and orientation data output by the ground unmanned platform integrated navigation system, and then the point cloud is structured by voxel filtering and through filtering. According to the position of the initialized target, the foreground and background of the target are segmented by the minimum segmentation algorithm, and the center position of the target is obtained by Euclidean clustering. Since the constructed target kinematic model is nonlinear, an unscented Kalman filter is introduced to predict and update the results of laser radar and millimeter wave radar detection, so as to realize the data fusion of the two, so as to continuously and effectively output the target position. The position of the target point is selected as the task path point of the ground unmanned platform, and the optimal path is generated by the local path planner. The speed and front wheel steering angle are obtained by the pure tracking algorithm and the Ackerman steering model, so as to drive the ground unmanned platform to maneuver. BRIEF DESCRIPTION OF THE DRAWINGS

[0101] Figure 1 It is the minimum segmentation topology diagram;

[0102] Figure 2 is the target kinematic model;

[0103] Figure 3 This is a schematic diagram of millimeter wave detection;

[0104] Figure 4 For local path planning;

[0105] Figure 5 is the velocity profile;

[0106] Figure 6 It is a pure tracking model;

[0107] Figure 7 For the Ackerman turn model. DETAILED DESCRIPTION

[0108] In order to make the purpose, technical solution and advantages of the present invention easier to understand, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0109] On the contrary, the present invention covers any substitution, modification, equivalent method and scheme made on the essence and scope of the present invention as defined by the claims. Further, in order to make the public have a better understanding of the present invention, some specific details are described in detail in the detailed description of the present invention below. Those skilled in the art can fully understand the present invention without the description of these details.

[0110] Example

[0111] The motion information (linear velocity, angular velocity, etc.) of the ground unmanned platform is collected to compensate the motion for each frame of point cloud data. Since the speed of the ground unmanned platform is 5-10 km / h, it is assumed that the ground unmanned platform moves at a constant speed within one sampling cycle of the laser radar, and compensation is performed through rotation and translation.

[0112] (1) Obtain motion information and transform the linear velocity and angular velocity from the global coordinate system to the lidar coordinate system.

[0113] (2) Rotate the original point cloud according to the azimuth of the first laser point, transform the original point cloud into the world coordinate system, and update the linear velocity and angular velocity.

[0114] (3) The current scanning angle is obtained based on the sequential scanning time and angular velocity, and the subsequent point cloud is rotated and translated.

[0115] (4) According to the initial rotation matrix, the dedistorted point cloud is rotated back to the lidar coordinate system.

[0116] The voxel filtering method is used to downsample the original point cloud, reduce the data volume of the point cloud as much as possible while retaining the geometric features of the point cloud, and remove noise points. First, a three-dimensional voxel network with a size of 0.02×0.02×0.02m is created, the input point cloud is meshed, and the centroid of all point clouds in each grid is used to approximate other points in the grid. The centroid calculation formula can be expressed as:

[0117]

[0118] Where h is the number of point clouds in each grid; (x i ,y i , z i ) is the coordinate of the i-th point cloud in each grid; (x center ,y center , z center ) are the centroid coordinates of the point cloud within each grid.

[0119] Initialize the position of the target point, and use the minimum segmentation algorithm to segment the foreground (point cloud belonging to the target) and background (point cloud not belonging to the target) of the input point cloud according to the center point coordinates and radius of the target.

[0120] (1) Construct a topological relationship diagram between points in each frame of point cloud data. Each vertex is connected to the source and sink points through edges, and is also connected to the vertices in its own neighborhood, such as Figure 1 shown.

[0121] (2) Construct weights based on the Euclidean distance between points, because points on the same object will not be too far apart.

[0122]

[0123] In the formula, dist is the distance between points. The longer the distance, the greater the possibility that the edge will be cut. σ is the weight function, which is 0.2m.

[0124] (3) Set the foreground and background penalty functions. This is used to reduce the target range. The foreground is the weight of the edge connecting the vertex to the source point, which is set to a constant of 0.5. The background is the weight of the edge connecting the vertex to the convergence point, backgroundPenalty, which is defined by the formula:

[0125]

[0126]

[0127] Where radius is the horizontal radius of the object, which is 0.5m; (x, y) is the point cloud coordinate; (CenterX, CenterY) is the target center coordinate; distanceToCenter is the distance from the point to the target center.

[0128] (4) Based on the weights between points, the foreground and background in the point cloud are segmented, and the foreground point cloud is clustered by Euclidean clustering to obtain the center position of the target.

[0129] Construct a target kinematic model. Since the movement of people is random, a simple constant velocity model cannot predict the posture of people or vehicles. Construct a 5-dimensional state quantity based on the position, velocity, angle, and angular velocity of the target, such as Figure 2 shown.

[0130]

[0131] In the formula, (p x , p y ) is the position of the target in the x and y directions; v is the speed of the target; ψ is the direction of the target; is the angular velocity of the target.

[0132] Derivative and integral of the state quantity:

[0133]

[0134] Where Δt is the time difference; x k 、x k+1 are the state vectors at time k and k+1 respectively.

[0135] According to the formula, if When , the result will tend to infinity, that is, the problem of the target moving in a straight line cannot be solved. Therefore, this situation is considered separately:

[0136]

[0137] After determining the fixed terms of the state equation, the process noise term also needs to be considered:

[0138]

[0139] The process model is described using a two-dimensional noise vector, with uncertainty v k It consists of two independent scalar process noises. The first process noise is the longitudinal acceleration noise v a，k , can be regarded as randomly distributed white noise with a mean of 0 and a variance of Take 0.01.

[0140]

[0141] The second process noise is angular acceleration noise It can be regarded as randomly distributed white noise with a mean of 0 and a variance of Take 0.01:

[0142]

[0143] Since both longitudinal acceleration and angular acceleration will affect the position p x 、p y Usually, the angular velocity of the target does not change much, ignoring the angular acceleration, and the effect of noise on the position can be characterized as.

[0144]

[0145] The effect of noise on speed, angle, and angular velocity can be characterized as:

[0146]

[0147] The dynamic model of the target to be tracked can be characterized as:

[0148]

[0149] Since the kinematic model of the target is nonlinear, the unscented Kalman filter is used to predict the target position. First, the current distribution is sampled to ensure that the distance between the sampling point and the distribution mean is an integer multiple of the standard deviation. In general, 2n+1 n-dimensional Gaussian distributions are selected, the first point is the mean of the Gaussian distribution, and the other points are symmetric about the mean.

[0150]

[0151] Wherein, μ is the mean; λ is a hyperparameter, taking λ=3-n=-2; the larger the λ, the farther the sampling point is from the mean of the Gaussian distribution, and the smaller the λ, the closer the sampling point is to the mean of the Gaussian distribution. In the target kinematic model constructed by the present invention, noise can also have a sub-linear effect on the model, so the covariance matrix P of the state equation can be characterized as:

[0152]

[0153] Where P′ is the 5x5 covariance matrix of the target kinematic model, and the element is the unit matrix of 0.01; Q is the covariance matrix of the noise.

[0154]

[0155] Then, according to the target kinematic model, the nonlinear function g(·) (i.e., the state equation) is used to predict each sampling point:

[0156] χk+1 =g(χ k ,μ k ) (17)

[0157] In the formula, χ k+1 is the sampling point predicted at time k+1; g(·) is the state equation.

[0158] Calculate the weight of each sampling point. The weight w formula can be expressed as:

[0159]

[0160] Calculate the mean x of the new distribution based on the weight of each sampling point k+1∣k and variance P k+1∣k :

[0161]

[0162]

[0163] The target position prediction value is obtained by formula (19).

[0164] Since the laser radar transforms linearly from the prediction state space to the measurement space, it can be transformed by the linear function H l Convert the predicted state space into the measured state space:

[0165]

[0166] In the formula, Z k+1∣k is the transformed measurement space; x k+1∣k is the predicted state quantity; R is the measurement noise.

[0167] Since the millimeter-wave radar prediction state space to measurement space is a nonlinear transformation, it can be obtained by the nonlinear function H r Convert prediction space to measurement space:

[0168]

[0169] In the formula, r is the distance between the target and the millimeter wave radar, θ is the angle between the target and the millimeter wave radar, and r' is the rate of change of the distance between the target and the millimeter wave radar. Figure 3 As shown;

[0170] Predict the prior estimate of the mean z after the unscented transformation k+1∣k , prior estimate covariance matrix S k+1∣k It is expressed as:

[0171]

[0172]

[0173] The cross-correlation matrix of the state space and measurement space is expressed as:

[0174]

[0175] Calculate the Kalman gain:

[0176]

[0177] According to the measurement results of laser radar minimum segmentation, Euclidean clustering, and millimeter wave output results z k+1 (i.e., the coordinates of the target center point), calculate the posterior state x k+1∣k+1 and the posterior estimated covariance matrix P k+1∣k+1 , the specific method is:

[0178] P k+1∣k+1 =P k+1∣k -K k+1∣k S k+1∣k (27)

[0179] x k+1∣k+1 =x k+1∣k +K k+1∣k (z k+1 -z k+1∣k ) (28)

[0180] The target position is updated using formula (28).

[0181] According to the position of the above output target and the self-positioning of the ground unmanned platform, a cluster of alternative paths is generated online, such as Figure 4 As shown. The path model uses a cubic Bezier curve, which has continuous first-order and second-order derivatives, ensuring that the planned path is executable by the unmanned platform. The cost, maximum allowable curvature and offset distance of each alternative path are weighted to obtain the evaluation value of each alternative path, and the minimum evaluation value is selected as the optimal path.

[0182] J i =ω1c i +w2k imax +ω3Δd i (29)

[0183] Where: c i is the passing cost of the ith alternative path; k imax is the maximum curvature of the ith alternative path; Δd i The offset distance of the i-th alternative path; ω1, ω2, and ω3 are the weights of the passing cost, maximum curvature, and offset distance, respectively.

[0184] The expected speed of each point on the trajectory is planned according to the optimal local path. For a given path, the target speed of the end point is set to zero, and it takes three stages from the current speed to zero, namely, acceleration, constant speed and deceleration. Figure 5 As shown in the figure, the acceleration stage corresponds to accelerating from the current speed vC to the desired speed vD, the deceleration stage corresponds to accelerating from the desired speed vD to zero speed, and the rest is a uniform speed stage. According to the acceleration and deceleration of the platform, the trajectory lengths corresponding to the three stages can be calculated to obtain the velocity profile.

[0185] A pure tracking algorithm is used to calculate motion control instructions, including the platform travel speed v and angular velocity w, such as Figure 6 As shown. Among them, (x v ,y v ) is the platform position, (x G ,y G ) is the position of the target point on the global path, O is the instantaneous turning center of the platform, R is the turning radius, L is the distance between the platform and the target point, that is, the preview distance, and α is the deviation angle between the platform heading and the target position.

[0186] The deviation angle between the platform heading and the target position is:

[0187]

[0188] The arc is tangent to the platform heading, and the central angle of the arc is:

[0189] ∠GOS=2α (31)

[0190] Therefore, we get the following relationship in the triangle ΔGOS:

[0191]

[0192] Therefore, according to the control speed v output by the local path planner, the angular velocity of the platform can be obtained as:

[0193]

[0194] Considering that the unmanned patrol car moves in a low-speed scenario and ignoring the tire slip angle, the steering relationship can be approximated by the Ackerman steering geometry model, such as Figure 7 Combined with the formula, in the case of a small turning angle, the turning angles of the left and right wheels are:

[0195]

[0196]

[0197] According to the input speed profile and the steering angle of the front wheels, the chassis controller can output the torque of the wheel hub motor, and the Electric Power Steering (EPS) system can output the torque of the steering motor, thereby achieving the following driving of the ground platform, including longitudinal and lateral movement.

Claims

1. A ground unmanned platform tracking method based on multivariate data fusion, characterized in that The steps of the method include: The first step is to collect the motion information of the ground unmanned platform and transform the collected motion information from the global coordinate system to the lidar coordinate system; The second step is to obtain the laser radar point cloud data and millimeter wave radar data of the target to be tracked in real time; The third step is to use the motion information of the ground unmanned platform collected in the first step to perform distortion compensation on the laser radar point cloud data in the second step to obtain compensated point cloud data; The fourth step is to downsample the compensated point cloud data obtained in the third step and remove the noise points; The fifth step is to initialize the position of the target to be tracked, and then use the minimum segmentation algorithm to segment the foreground and background of the input point cloud according to the center point coordinates and radius of the target to be tracked, and extract the center position of the target through Euclidean clustering; Step 6: Construct a kinematic model of the target to be tracked; Step 7: Predict the position of the target to be tracked by using the unscented Kalman filter algorithm; Step 8: According to the fusion results of LiDAR and millimeter-wave radar data, the posterior state quantity and the posterior estimated covariance matrix are calculated to update the position of the target; The method also includes the steps of generating a cluster of candidate paths online according to the position of the target to be tracked and the self-positioning of the ground unmanned platform, performing weighted calculation on the cost, maximum allowable curvature and offset distance from the global path of each candidate path, obtaining an evaluation value of each candidate path, selecting the minimum evaluation value as the optimal path, and finally completing the ground unmanned platform tracking task through a velocity profile generation algorithm and a pure tracking algorithm; In the sixth step, the specific steps of constructing the kinematic model of the target to be tracked are: Construct a 5-dimensional state vector x based on the position, speed, angle and angular velocity of the target to be tracked; In the formula, (p x , p y ) is the position of the target in the x and y directions; v is the speed of the target; ψ is the direction of the target; is the angular velocity of the target; By derivation and integration of the state quantity, the state equation is obtained: Where Δt is the time difference; x k 、x k+1 are the state vectors at time k and k+1 respectively; If the target angular velocity When: Noise term v k for: The process model is described using a two-dimensional noise vector, with uncertainty v k It consists of two independent scalar process noises. The first process noise is the longitudinal acceleration noise v a，k , is regarded as randomly distributed white noise with a mean of 0 and a variance of Take 0.01; The second process noise is angular acceleration noise is considered as randomly distributed white noise with a mean of 0 and a variance of Take 0.01; The effect of noise on position is characterized by: The effect of noise on speed, angle, and angular velocity can be characterized as follows: The kinematic model of the target to be tracked is:

2. The method for tracking a ground unmanned platform based on multivariate data fusion according to claim 1, characterized in that: In the first step, the motion information of the ground unmanned platform is collected, including the linear velocity and angular velocity of the ground unmanned platform; In the fourth step, the method for removing noise points is as follows: first, create a three-dimensional voxel network with a size of 0.02m*0.02m*0.02m, mesh the compensated point cloud data, and use the centroid of all point clouds in each grid to approximately replace other points in the grid. The calculation formula of the point cloud centroid of each grid is expressed as: Where h is the number of point clouds in each grid; (x i ,y i , z i ) is the coordinate of the i-th point cloud in each grid; (x center ,y center , z center ) are the centroid coordinates of the point cloud within each grid.

3. A method for tracking a ground unmanned platform based on multivariate data fusion according to claim 2, characterized in that: In the fifth step, the minimum segmentation algorithm is used to segment the foreground and background of the input point cloud. The segmentation method is: (1) Construct a topological relationship graph between points in each frame of point cloud data. Each vertex is connected to the source point and sink point through an edge, and is also connected to the vertices in its own neighborhood. (2) According to the Euclidean distance between points, the weight smoothCost is constructed as follows: In the formula, dist is the distance between points. The longer the distance, the greater the possibility of edge cutting. σ is the weight function, which is 0.2m. (3) Set foreground and background penalty functions to reduce the target range; the foreground is the weight of the edge connecting the vertex and the source point, which is set to a constant 0.5, and the background is the weight of the edge connecting the vertex and the convergence point. The backgroundPenalty is defined by the formula: In the formula, radius is the horizontal radius of the object, which is 0.5m; (x, y) is the point cloud coordinate; (CenterX, CenterY) is the target center coordinate; distanceToCenter is the distance from the point to the target center; (4) Based on the weights between points, the foreground and background in the point cloud are segmented, and the foreground point cloud is clustered by Euclidean clustering to obtain the center position of the target.

Images