A vehicle motion state estimation method based on three-dimensional point cloud in a curved road environment

Abstract

The application provides a vehicle motion state estimation method based on three-dimensional point clouds in a curved road environment, and the method comprises the following steps: S1, a road center curve model is established according to the spatial structure characteristics of a curb; S2, the conversion relationship among a self-coordinate system, a global coordinate system and a road curve coordinate system is determined through observation and calculation; S3, a vehicle target motion model in the road curve coordinate system is constructed; and S4, the vehicle target is tracked by using an unscented Kalman filter, and motion state parameters are estimated. The application improves the vehicle target motion state estimation precision by constructing a road model, and especially in a curved road environment with continuously changing curvatures, accurate and stable vehicle target tracking is realized.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a method for estimating vehicle motion state based on three-dimensional point clouds in a curved road environment. Background Technology

[0002] With the rapid development of artificial intelligence technology, autonomous vehicles have become a research hotspot in the field of intelligent transportation. Reliable environmental perception capabilities are a prerequisite for autonomous vehicles to complete subsequent behavioral decisions and autonomous control tasks.

[0003] Currently, environmental perception sensors mainly include cameras, millimeter-wave radar, and lidar. Compared to cameras and millimeter-wave radar, lidar can effectively identify various targets, including stationary or moving vehicles and pedestrians, by collecting rich point cloud information. It is also unaffected by lighting conditions and has good robustness in environmental perception.

[0004] Existing methods for estimating vehicle target motion state mainly include: The invention with application number 201910498228.0, "A Tracking Method for an Online Target Tracking System Based on 3D Laser Point Clouds," obtains road boundary points using a Gaussian process regression algorithm and a road boundary map, then uses clustering and data association algorithms to match independent point cloud clusters for each frame, models the independent point cloud clusters using incremental Gaussian mixtures, fits the point cloud clusters using a rectangular model, and applies Kalman filtering to obtain the tracking results; The invention with application number 202210109436.9, "A Vehicle Speed ​​Estimation Method, Device, Equipment, and Storage Medium Based on Point Clouds," matches vehicle target anchor points in the current point cloud frame using target annotation information, calculates the temporal information of the vehicle target position, and estimates the vehicle target motion state using a trajectory-constrained motion model. Most of these existing vehicle motion state estimation methods only consider the vehicle's own motion state and do not consider the influence of road curvature. Compared to straight roads, the curvature of curved roads increases the uncertainty of vehicle motion state changes. At the same time, the intersection and union ratio of the bounding boxes of the same vehicle point cloud target in adjacent frames is small, making it difficult for traditional target tracking algorithms to accurately and stably track surrounding vehicle targets. The estimation error of vehicle motion state parameters is large, and it cannot provide reliable behavioral decision-making basis for the vehicle.

[0005] Therefore, in curved road environments, constructing a road curve model can be considered to improve the accuracy of vehicle target motion state estimation. Currently, no reports on such techniques have been found. Summary of the Invention

[0006] The purpose of this invention is to address the shortcomings of the prior art by proposing a method for estimating the motion state of vehicle targets based on three-dimensional point clouds in a curved road environment.

[0007] To achieve the above objectives, the present invention is implemented through the following technical solution:

[0008] A method for estimating vehicle motion state based on 3D point cloud in a curved road environment includes the following steps:

[0009] S1. Candidate curb point clouds are selected based on the differences in point cloud structure features between the curb and the ground. Then, curb point clouds are obtained by filtering through absolute height threshold and outliers. K-means algorithm is used to cluster and generate curb point cloud sets on both sides. The least squares method is used to fit the curb curve function and construct a road center curve model based on cubic spline interpolation function.

[0010] S2, construct the vehicle coordinate system, global coordinate system and curve coordinate system respectively, and determine the transformation relationship between the coordinate systems through observation and calculation, and transform the vehicle target state parameters of the independent frames output by the vehicle target detection network from the vehicle coordinate system to the road curve coordinate system;

[0011] S3. The motion state equation of the vehicle target in the curve coordinate system is constructed using a constant turning rate and acceleration model. The detected state parameters of the surrounding vehicle targets are transformed into the road curve coordinate system as observation values ​​to construct the observation equation.

[0012] S4 predicts the motion state of vehicle targets in the road curve coordinate system based on unscented Kalman filtering. Data is correlated by the Mahalanobis distance between the predicted state value and the observed value. The Kuhn-Munkres algorithm is used to find the optimal matching solution. Then, the motion state parameters of the vehicle target in the next frame are updated by filtering.

[0013] Preferably, step S1 further includes the following steps:

[0014] S11, based on differences in spatial structural features, the curb and ground point clouds are segmented. Candidate curb point clouds are obtained by using constraints such as horizontal continuity, vertical continuity, and angle threshold of the 3D point cloud. Then, by combining the absolute height threshold, obstacles such as vehicles and pedestrians in the environment are filtered out, and outliers are further removed to obtain the curb point cloud set P. c ;

[0015] S12, the K-means algorithm is used to cluster the points on both sides of the road edge to obtain the set P. c1 and P c2 ;

[0016] S13, least squares fitting is used to fit the discretely distributed curb points on both sides of the road, and the optimal matching function f for the curb curves on both sides is found by minimizing the sum of squared deviations. p1 (x), f p2 (x);

[0017] S14, derived from the optimal curve function f of the roadside curbs on both sides.p1 (x), f p2 (x) obtains a series of road center coordinate points, and constructs a road center curve model through cubic spline interpolation function.

[0018] Preferably, step S11 further includes the following step:

[0019] S111, establish a horizontal continuity constraint on adjacent point clouds of the same laser beam in the circumferential direction on the xy plane, and set a horizontal distance threshold l. xyt When the distance between adjacent scan points is l xy >l xyt At that time, the adjacent point may be a curb point;

[0020] S112, establish a vertical continuity constraint on adjacent point clouds of the same laser beam in the circumferential direction in the xy plane, and set a vertical distance threshold l. zt When the vertical distance between adjacent scan points is l z >l zt At that time, the adjacent point may be a curb point;

[0021] S113, for adjacent scanning points {p} of the same laser beam in the circumferential direction i-1 ,p i ,p i+1}, calculate the vector u between adjacent scan points ia u ib The included angle ε i Set the angle threshold ε t When ε i <ε t At that time, scan point p i It may be a curb point;

[0022] S114. The point cloud that simultaneously satisfies conditions S111, S112, and S113 is denoted as the road edge candidate point cloud set P. c0 Set the height threshold H. R Calculate adjacent consecutive scan points {p i ,p i+1 ,p i+2 ,···,p i+m Maximum vertical height difference H of road curb i If H i <H R And H i+1 >H R Then {p i ,p i+1 ,p i+2 ,···,p i+m Let} be the curb point, and generate the curb point set P. c ;

[0023] S115, by setting the spatial neighborhood radius r n Calculate a certain scan point p j The point cloud distance threshold d is obtained by comparing the mean and standard deviation of the distances between point clouds within the neighborhood. max If any scan point p i and p j Distance d between i Greater than d max Then remove p i .

[0024] Preferably, step S2 further includes the following steps:

[0025] S21, Taking a starting center point O of the curved road as the origin, and setting the tangent direction of the road center curve at the origin as the Y-axis and the normal direction as the X-axis, establish a global coordinate system X. G OY G ; Establish a curve coordinate system SOC with the starting center point O of the curved road as the origin, the curve direction as the S-axis, and the curve normal direction as the C-axis; with the center point O of the ego vehicle (EV) as the origin. E With the origin as the Y-axis and the direction perpendicular to the EV velocity vector as the X-axis, construct the vehicle coordinate system X. E O E Y E ;

[0026] S22, Calculate coordinate system X based on the positional relationship between coordinate systems. E O E Y E and X G OY G The coordinate transformation matrix will transform X E O E Y E Transform the target vehicle (TV) state parameters in the coordinate system to the global coordinate system X. G OY G Calculate X in the coordinate system G OY G The distance from TV to the road center curve in the coordinate system and the relative position between TV and EV along the road center curve direction are used to obtain the coordinates of TV in the curve coordinate system SOC. Then, based on the coordinate system SOC and X... E O E Y E The vector transformation matrix between the vectors is used to calculate the TV velocity vector in the SOC coordinate system.

[0027] Preferably, step S22 further includes the following step:

[0028] S221, Establish coordinate system X based on the relative positional relationship between coordinate systems. E O E Y E X G OY G The coordinate transformation matrix between coordinate systems X and X transforms the coordinate system X into X. E O E Y E Transform the detected surrounding TV state parameters to the X coordinate system G OY G middle;

[0029] S222, Global coordinate system X G OY G The shortest distance c from TV to the center curve of the road is calculated by the curve tangent point normal, and the piecewise function integral value is calculated by the infinitesimal method to obtain the relative distance s between TV and EV along the center curve direction in the curve coordinate system SOC. The coordinates of TV in the curve coordinate system SOC are then converted.

[0030] S223, according to coordinate system SOC and X E O E Y E The vector transformation matrix between the two coordinate systems is used to calculate the TV velocity vector in the SOC curvilinear coordinate system.

[0031] Preferably, step S3 further includes the following steps:

[0032] S31, represent the motion state of the j-th target vehicle in the k-th frame point cloud scene as X. j,k This includes TV coordinates in the curvilinear coordinate system, velocity components, acceleration components, heading angle, and rate of change of heading angle;

[0033] S32, using a constant turning rate and acceleration model, construct a vehicle target motion model in a curvilinear coordinate system and calculate the state transition matrix A. k,k-1 Generate the vehicle target motion state equation;

[0034] S33, transform the vehicle target detection result of the k-th frame output by the vehicle target detection network to the curvilinear coordinate system, and calculate the vehicle target observation value Z of the k-th frame by constructing the observation equation. k .

[0035] Preferably, step S4 further includes the following steps:

[0036] S41, use lossless transformation to linearize the state equation to obtain the sampling point χ and the corresponding weight φ of each sampling point;

[0037] S42, obtain the state sampling points according to the motion state equation. Calculate the vehicle target state estimate in frame k. The covariance matrix Σ k|k-1 ; Obtain the observation sampling points according to the observation equation Calculate the prediction of the observation in the k-th frame And the new information covariance matrix Γ k|k-1 ;

[0038] S43, Calculate the state prediction value for the k-th frame. and observed value prediction The Mahalanobis distance between them is used to obtain the optimal matching solution using the Kuhn-Munkres algorithm;

[0039] S44, Calculate the predicted state value and observed value prediction Covariance Υ k And through the new information covariance Γ k and covariance Υ k Calculate the filter gain K k Update the status value The covariance matrix Σ k . Attached Figure Description

[0040] Figure 1 The flowchart shows the overall framework of the method for estimating the motion state of a vehicle target in a 3D point cloud environment under curved road conditions provided by this invention.

[0041] Figure 2 This is a schematic diagram for extracting curb points.

[0042] Figure 3 This is a schematic diagram of the road center curve fitting.

[0043] Figure 4 This is a diagram illustrating coordinate transformation. Detailed implementation method:

[0044] To more clearly illustrate the technical solutions of this invention, the invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0045] Figure 1 This is a flowchart illustrating the overall framework of the vehicle motion state estimation method based on 3D point clouds in a curved road environment proposed in this invention. The main steps include:

[0046] S1. Candidate curb point clouds are selected based on the differences in point cloud structure features between the curb and the ground. Then, curb point clouds are obtained by filtering through absolute height threshold and outliers. K-means algorithm is used to cluster and generate curb point cloud sets on both sides. The least squares method is used to fit the curb curve function and construct a road center curve model based on cubic spline interpolation function.

[0047] S2 constructs a vehicle coordinate system, a global coordinate system, and a curve coordinate system respectively. Through observation and calculation, the transformation relationship between each coordinate system is determined, and the vehicle target state parameters of the independent frames output by the vehicle target detection network are transformed from the vehicle coordinate system to the road curve coordinate system.

[0048] S3 uses a constant turning rate and acceleration model to construct the vehicle target motion state equation, and uses the surrounding vehicle target state parameters in the road curve coordinate system as observation values ​​to construct the observation equation.

[0049] S4 predicts the motion state of the vehicle target based on unscented Kalman filtering, associates the data through the Mahalanobis distance between the predicted state value and the observed value, uses the Kuhn-Munkres algorithm to find the optimal matching solution, and then updates the motion state parameters of the vehicle target in the next frame through filtering.

[0050] In this embodiment, the specific steps for constructing the road center curve function in step S1 include:

[0051] S11, the curb and ground point clouds are segmented based on differences in spatial structure characteristics. Candidate curb point clouds are obtained by filtering based on the horizontal continuity, vertical continuity, and included angle threshold of the 3D point cloud. Then, vehicles, obstacles, and other targets in the environment are filtered by the absolute height threshold, and outliers are further removed to obtain the curb point cloud set P. c .

[0052] In this embodiment, step S11 refers to Figure 2 As shown, the specific steps include:

[0053] On the S111, xy plane, adjacent scanning points of the same laser scanning beam along the circumferential direction should have horizontal continuity. When curb points exist, the horizontal distance between adjacent point clouds is l. xy This will change, therefore a horizontal distance threshold l is constructed by calculating the theoretical distance between adjacent point clouds in the xy dimension. xyt When l xy >l xyt If so, the current point cloud may be a road edge point, and the specific calculation is as follows:

[0054]

[0055]

[0056] Among them, (x i ,y i ), (x i+1 ,y i+1 ) represents the X and Y coordinates of adjacent point clouds; H L θ is the height of the lidar sensor above the road surface. g θ is the angle between the current lidar scanning beam and the Z-axis.r This represents the horizontal angular resolution of the scanning beam.

[0057] S112, the variation range of adjacent scanning points along the circumferential direction of the same laser scanning beam in the vertical direction of the xy plane should be kept within a small range. When scanning to the curb, the vertical distance l between adjacent scanning points should be kept within a small range. z It will increase significantly, therefore, by calculating the theoretical vertical spacing l between adjacent scanning points in the circumferential direction... zt When l z >l zt If so, the current scan point may be a curb point. The specific calculation is as follows:

[0058]

[0059] l zt =l xy ·sinθ z

[0060] Among them, z i z i+1 θ represents the Z-axis coordinates of adjacent scan points; z The vertical angle threshold between adjacent scan points is set, taking into account uneven road surfaces or slight slopes, and θ is... z Set to 10°.

[0061] S113, compared to a horizontal road surface, there is an angle difference between adjacent scan points in the circumferential direction at the curb. For the point set P in the k-th frame... j ={p1,p2,···,p n Adjacent scanning points of the same laser beam along the circumferential direction {p i-1 ,p i ,p i+1}, construct the adjacent scan point vector u i-1 u i+1 :

[0062] u i-1 =[(x i-1 -x i ),(y i-1 -y i ),(z i-1 -z i )]

[0063] u i+1 =[(x i+1 -x i ),(y i+1 -y i ),(z i+1 -z i )]

[0064] Among them, (xi ,y i ,z i ) represents the scan point p i The three-dimensional spatial coordinates. The angle between adjacent scan points is expressed as:

[0065]

[0066] Set angle threshold ε t =150°, when ε i <ε t If both horizontal and vertical continuity constraints are satisfied, then the scan point p will be... i Marked as roadside candidate points, the roadside candidate point cloud set P is obtained. c0 .

[0067] S114, the curb and road surface are usually perpendicular. The absolute height of adjacent scanning points of the same laser beam on the curb changes continuously and monotonically. The curb height range is generally 10cm-20cm. Considering that the absolute height difference between adjacent scanning points exceeding this range may indicate an obstacle or a sloping road surface, a height difference threshold range H is set. R = [0.1, 0.2]m, for the candidate set P of the curb point cloud C0 Mid-scan point p i Calculate adjacent consecutive scan points {p i ,p i+1 ,p i+2 ,···,p i+m Maximum vertical height difference H i If H i <H R And H i+1 >H R Then {p i ,p i+1 ,p i+2 ,···,p i+m Let} be the curb point, and generate the curb point set P. c .

[0068] S115, because obstacles in the point cloud scene may cause set P to be affected. c Outliers exist in the scene, so it's necessary to filter out outliers and noise points that don't belong to the road edge point cloud. This is achieved by setting the spatial neighborhood radius r. n Calculate the cloud p at a certain point j The point cloud distance threshold d is obtained by comparing the mean and standard deviation of the distances between point clouds within the neighborhood. max If any scan point p i and p j Distance d between i Greater than d max Then remove p i Specifically:

[0069]

[0070]

[0071]

[0072] in, σ is the average distance between scan points in the neighborhood; σ is the standard deviation; γ is the threshold coefficient.

[0073] S12, since both road edge points are stored in set P c In the process, it is necessary to further segment the road edge point sets on both sides. Considering that the filtered road edge point set is free from noise interference and the number of centroids and clusters is easy to determine, the K-means algorithm is used for clustering to obtain the road edge point set P on both sides. c1 and P c2 , specifically Figure 3 As shown.

[0074] S13, Reference Figure 3 As shown, the least squares method is used to fit curves to the discretely distributed road edge points on both sides, and the optimal matching function f for the road edge curves on both sides is found by minimizing the sum of squared deviations. p1 (x), f p2 (x), assuming the curb curve fitting polynomial function is:

[0075] f p (x)=b0+b1x+b2x 2 +···+b k x k

[0076] Then the sum of squared deviations from the curve to each discrete point in the curb point set, F(b0,b1,...,b...), is... k )for:

[0077]

[0078] To solve for b that satisfies the conditions of a polynomial function i Value, for b in the above formula i Taking the partial derivatives separately, we can obtain:

[0079]

[0080] The matrix form is then expressed as:

[0081]

[0082] Matrix simplification yields:

[0083]

[0084] Solving for the coefficient matrix b yields the coefficient matrix. i =[b1,b2,···,b k ] T That is, to obtain the best matching function f for the curb curve. p1 (x), f p2 (x).

[0085] S14, a series of road center coordinate points are obtained by calculating the normals of the tangent points of the curb curves on both sides. Specifically: for one side curb curve f p1 Any point p on (x) c1i Calculate the slope k of the tangent line at that point. c1i Generate the normal function f ni (x), the normal f ni (x) and the other side curb curve f p2 The intersection point of (x) is denoted as p. c2i , point p c1i and p c2i The center point along the normal direction is recorded as the road center point. Obtain the set of coordinates of points on the centerline of the road.

[0086] Considering the curvature continuity of the road model, a road center curve function model is constructed using a cubic spline interpolation function. For points on the road centerline... and points The interval is [x0, x n Divide the x-axis into n sub-intervals [(x0,x1),(x1,x2),...,(x...], ... ... n-1 ,x n For each subinterval, construct a cubic polynomial of the following form:

[0087] S(x)=q (0) +q (1) x+q (2) x 2 +q (3) x 3

[0088] The spline curve function and its first and second order differentials are:

[0089]

[0090] From the curve interpolation conditions and the first-order and differential continuity of the function, the following constraints can be obtained:

[0091]

[0092] Assume adjacent x i and x i+1The unit step size is h i S i (x i The second-order differential is ζ i =S i "(x i Substituting the curve interpolation conditions and related constraints into the curve function and the first and second order differentials, we can obtain information about the unknown parameter ζ. i The system of linear equations:

[0093]

[0094] Considering the endpoint information is known, we can solve the system of equations using boundary conditions. Since the second differential value at the endpoints is 0, the system of equations can be expressed in the following form:

[0095]

[0096] Given the first-order differential value at the endpoint of the curve, assume the first-order differential at x0 is S0′(x0)=B, x n The first-order differential is S n ′ -1 (x n From C, we can deduce:

[0097]

[0098] The new matrix equation is:

[0099]

[0100] Because the curvature of the function exists continuously in adjacent intervals, that is, the values ​​of S″′0(x1) and S″′1(x1) at x1 are equal, x n-1 S″′ n-2 (x n-1 ) and S″′ n-1 (x n-1 Since the values ​​of ) are equal, we can conclude that:

[0101]

[0102] The new matrix equation is:

[0103]

[0104] Solving for the corresponding coefficients of the spline curve yields the following:

[0105]

[0106] Then for each subinterval x∈[x] on the center curve of the road, i ,x i+1 Constructing a cubic spline interpolation function gi (x), which is the function for obtaining the road center curve:

[0107]

[0108] In this embodiment, the specific steps of coordinate system construction and coordinate transformation in step S2 include:

[0109] S21. Taking a starting center point O of the curved road as the origin, and setting the tangent direction of the road center curve at the origin as the Y-axis and the normal direction as the X-axis, establish a global coordinate system X. G OY G With the starting center point O of the curved road as the origin, the direction of the road center curve is set as the S-axis, and the direction of the curve normal is set as the C-axis, establishing a curve coordinate system SOC. The coordinates of the j-th target vehicle in the curve coordinate system SOC are represented by the corresponding positions (s, c) on the S-axis and C-axis; with the EV center point O E With the origin as the Y-axis and the direction perpendicular to the EV velocity vector as the X-axis, construct the vehicle coordinate system X. E O E Y E , specifically Figure 4 As shown.

[0110] S22, Calculate coordinate system X based on the positional relationship between coordinate systems. E O E Y E and X G OY G The coordinate transformation matrix will transform X E O E Y E The state transition of the target vehicle (TV) in the coordinate system to X G OY G In the coordinate system, then in the X G OY G The distance from TV to the center curve and the relative position of TV on the center curve and EV are calculated to obtain the coordinates of TV in the SOC curve coordinate system. Then, the SOC and X are calculated. E O E Y E Use the vector transformation matrix between the two coordinate systems to obtain the TV velocity vector in the SOC coordinate system.

[0111] In this embodiment, step S22 refers to Figure 4 As shown, the specific steps include:

[0112] S221, Establish coordinate system X based on the relative positional relationship between coordinate systems. G OY G X E OE Y E The coordinate transformation matrix between coordinate systems X and X transforms the coordinate system X into X. E O E Y E Transform the detected surrounding TV state parameters to the global coordinate system X G OY G In the middle, specifically:

[0113]

[0114] Among them, (x Gj ,y Gj ) is X G OY G The x and y coordinates of the j-th target vehicle in the coordinate system; (x Ej ,y Ej ) is X E O E Y E The x and y coordinates of the j-th target vehicle in the coordinate system; (x GE ,y GE For the vehicle in X G OY G The x and y coordinates in the coordinate system; θ E For X G OY G The yaw angle of the vehicle relative to the Y-axis in the coordinate system.

[0115] S222, the shortest distance from the j-th target vehicle to the center curve of the road is calculated using the curve tangent point normal, specifically expressed as:

[0116]

[0117] Where g t (x Sj Let x be the point of tangency of the piecewise function of the t-th segment of the road center curve corresponding to the j-th target vehicle. Sj The function value, and Further, by using the infinitesimal method to calculate the integral value of the piecewise function, the relative distance s between the j-th target vehicle and the self-vehicle along the road center curve is obtained:

[0118]

[0119] S223, the target velocity vector is calculated using the coordinate transformation matrix, specifically:

[0120]

[0121] in, Let the coordinate system be X E O E Y EThe velocity components of the j-th target vehicle along the X and Y axes; These represent the velocity components of the j-th target vehicle along the tangent and normal directions of the road center curve in coordinate system SOC; θ Gj Let the coordinate system be X G OY G The heading angle of the j-th target vehicle:

[0122]

[0123] Where, θ j Let the coordinate system be X E O E Y E The heading angle of the j-th target vehicle.

[0124] S3 uses a constant turning rate and acceleration model to construct the vehicle motion state equation in the curve coordinate system, and transforms the detected surrounding vehicle state parameters into the road curve coordinate system as observation values ​​to construct the observation equation.

[0125] In this embodiment, the specific steps for constructing the vehicle motion state equation and observation equation in step S3 include:

[0126] S31, to more accurately model the changes in vehicle motion state under the road curve coordinate system, the set of all vehicle targets U in the k-th frame point cloud scene is... k Let the motion state of the j-th target vehicle be represented as X. j,k :

[0127]

[0128]

[0129] Among them, (s j ,c j Let be the position coordinates of the j-th target vehicle in the SOC (Side Center Curve) coordinate system. and These are the velocity components of the j-th target vehicle along the S and C axes in the road center curve coordinate system; and These are the acceleration components of the j-th target vehicle along the S and C coordinate axes in the road center curve coordinate system, respectively. Let ω be the angular velocity of the j-th target vehicle; j Let W be the rate of change of the angular velocity of the j-th target vehicle; j ,L j ) are the width and length of the bounding box of the j-th target vehicle, respectively.

[0130] S32. First-order motion models, such as the constant velocity model and the constant acceleration model, are linear models. They assume the target speed is fixed, i.e., moving in a straight line. When the vehicle's heading angle changes, the vehicle motion state equation struggles to accurately predict the target vehicle's motion state. In second-order motion models, the constant turning rate and speed model constructs the target vehicle's motion state equation using constant curvature and speed. When the curvature of the vehicle's trajectory changes continuously or the road curvature changes significantly, the mean square error in predicting the target position and speed is large. Considering the continuous variation of road curvature in real curved road environments, this paper uses a constant turning rate and acceleration model to construct the target vehicle's motion state equation to improve the estimation accuracy of the target vehicle's motion state in curved road environments. For any target vehicle in the k-th frame, the vehicle motion model is constructed as follows:

[0131]

[0132] Where Δt is the time interval between adjacent frames. The motion state equation of the target vehicle is:

[0133] X k =A k,k-1 ·X k-1 +M k

[0134] Among them, X k and X k-1 M represents the motion state of the target vehicle in frame k and frame (k-1), respectively; k For process noise; A k,k-1 The state transition matrix for the target vehicle from frame k-1 to frame k is as follows:

[0135]

[0136] Among them, A vs A vc For the velocity component parameters of the S and C coordinate axes; A as A ac The acceleration component parameters for the S and C coordinate axes are calculated as follows:

[0137]

[0138] Assume process noise M k The covariance matrix is ​​Q, and M k Assuming the distribution follows a Gaussian distribution with zero mean, the state prediction covariance matrix is:

[0139] Σ k =A·Σ k-1 ·A T +Q

[0140] Where, Σ kLet be the predicted state covariance matrix from frame (k-1) to frame k.

[0141] S33, using a vehicle target detection network, outputs the vehicle target detection results in the k-th frame point cloud scene, and extracts X. E O E Y E The target vehicle's observation parameters in the coordinate system include center point coordinates, velocity, acceleration, heading angle, and rate of change of heading angle. After coordinate transformation to obtain the observation values ​​in the curvilinear coordinate system (SOC), the target's observation equation is expressed as:

[0142] Z k =H k ·X k +V k

[0143] Among them, Z k H represents the target vehicle state observation value in the k-th frame; k The observation matrix H is the observation matrix. Since the observations are direct measurements of the detection result state of the current frame, the observation matrix H is... k =[I 0] 10×8 V K For observation noise. Assume V K The covariance matrix is ​​T, and V K Assuming the new information follows a Gaussian distribution with zero mean, the covariance matrix is:

[0144] Γ k =H k ·Σ k ·H k T +T

[0145] Among them, Γ k This is the new information covariance matrix, used to predict the uncertainty of the next frame's observations.

[0146] S4. Based on the unscented Kalman filter, the motion state of the target vehicle in the road curve coordinate system is predicted. Data is correlated by the Mahalanobis distance between the predicted state value and the observed value. The Kuhn-Munkres algorithm is used to find the optimal matching solution. Then, the motion state parameters of the target vehicle in the next frame are updated by filtering.

[0147] In this embodiment, the specific steps for predicting the target vehicle state in step S4 include:

[0148] S41, initialize the filter state and variance matrix, assuming frame 0. The filter is then initialized as follows:

[0149]

[0150] Where P0 is the variance matrix at the initial time step; This is the estimated state value at the initial time. For a mean of... For an n-dimensional state vector with variance P, the state transition equation is linearized using a lossless transformation to obtain the sampling points χ and their corresponding weights φ:

[0151]

[0152] Where, χ (i) This represents the i-th sampling point; n represents the dimension of the state vector, set to 8; λ is the scaling parameter used to adjust the prediction error, and its calculation process is as follows:

[0153] λ=α 2 (n+τ)-n

[0154] Where α is the sampling radius, used to adjust the distribution of sampling points around the center point; τ is used to constrain the matrix (n+λ)P to be a positive semi-definite matrix. Considering the accuracy of the mean, parameters α = 0.01 and τ = -3 are set. The weight calculation of each sampling point is expressed as:

[0155]

[0156] in, The weight represents the mean of the i-th sampling point; β represents the weight of the covariance P of the i-th sampling point; β is the state distribution parameter, which can combine the dynamic errors of higher-order terms in the equation. Considering the reduction of the influence of dynamic errors of higher-order terms, β = 2 is set.

[0157] S42, Propagation sampling points based on the target vehicle's state of motion equation:

[0158]

[0159] in, The calculation result is given for the i-th sampling point from frame (k-1) to frame k, where f is the motion state equation. Then, the predicted motion state and variance of the target vehicle in the next frame are respectively:

[0160]

[0161]

[0162] in, Σ represents the estimated motion state of the target vehicle from frame (k-1) to frame k; k|k-1 Estimate the covariance matrix for the state from frame (k-1) to frame k. Propagate the sampling points using the observation equation to obtain the observed predicted sampling points:

[0163]

[0164] in, Let h be the prediction of the observation sampling points from frame (k-1) to frame k, and h be the observation equation. Then, the predicted observation values ​​of the target vehicle from frame (k-1) to frame k are... And the new information covariance matrix Γ k|k-1 for:

[0165]

[0166]

[0167] in, The prediction results for observations from frame (k-1) to frame k; Γ k|k-1 This is the information covariance matrix for predicting observations.

[0168] S43. Considering the possibility of multiple target vehicles in the scene, the observed values ​​and state predictions output by the detection network may correspond to different target vehicles. Therefore, it is necessary to perform data association on the targets in the scene to ensure stable target tracking and filter outliers. Due to the large variation in the bounding boxes of target vehicles between adjacent frames in a curved road environment, traditional data association algorithms based on intersection-union ratio (IU) struggle to accurately match observed values ​​and state predictions. Mahalanobis distance, however, can consider the similarity and uncertainty between the data to be associated. By using a distance metric between data, it can accurately associate targets even when the IU of the same target is low in adjacent frames. Therefore, Mahalanobis distance is used for data association. The Mahalanobis distance between the observed values ​​and the state predictions is:

[0169]

[0170] For the problem of finding the optimal solution for associating observed values ​​and predicted states, the traditional Hungarian algorithm suffers from poor prediction accuracy due to constraints imposed by the motion model and object detection model. The Kuhn-Munkres algorithm, however, optimizes the confidence score prediction by adding appropriate weights to each relationship connection, thereby improving both the matching success rate and accuracy. Therefore, this paper adopts the Kuhn-Munkres algorithm to process the predicted state values. and observed values The binary matching problem between adjacent frames is used to obtain the optimal data association result for the same target vehicle in adjacent frames.

[0171] S44, based on data association, outputs the best match between each group of state predictions and observed values, and the cross-covariance Υ between state predictions and observed values. k for:

[0172]

[0173] Using the new covariance matrix Γ k and cross-covariance matrix Y k The filter update equation can be derived as follows:

[0174] K k =Υ k ·Γ k -1

[0175]

[0176] Σ k|k =Σ k|k-1 -K k ·Γ k ·K k T

[0177] Among them, K k Z is the filter gain value for the k-th frame; k Σ represents the observation value of the k-th frame; k|k This is the updated state covariance matrix.

[0178] After three consecutive successful matches of the same target in a curved road environment using unscented Kalman filtering, the trajectory of the target is initialized and generated. When the target fails to match for two consecutive frames, the trajectory is terminated, so as to output the accurate driving trajectory of all target vehicles in the curved road environment.

[0179] The embodiments of the present invention have been described above with reference to the accompanying drawings, but the present invention is not limited to the specific embodiments described above. For those skilled in the art, various changes and modifications can be made without departing from the principles and scope of the claims of the present invention, but these modifications still fall within the protection scope of the present invention.

Claims

1. A method for estimating vehicle motion state based on 3D point clouds in a curved road environment, characterized in that, Includes the following steps: S1. Candidate curb point clouds are selected based on the differences in point cloud structure features between the curb and the ground. Then, curb point clouds are obtained by filtering through absolute height threshold and outliers. K-means algorithm is used to cluster and generate curb point cloud sets on both sides. The least squares method is used to fit the curb curve function and construct a road center curve model based on cubic spline interpolation function. S2, construct the vehicle coordinate system, global coordinate system and curve coordinate system respectively, and determine the transformation relationship between the coordinate systems through observation and calculation, and transform the vehicle target state parameters of the independent frames output by the vehicle target detection network from the vehicle coordinate system to the road curve coordinate system; S3. The motion state equation of the vehicle target in the curve coordinate system is constructed using a constant turning rate and acceleration model. The detected state parameters of the surrounding vehicle targets are transformed into the road curve coordinate system as observation values ​​to construct the observation equation. S4 predicts the motion state of vehicle targets in the road curve coordinate system based on unscented Kalman filtering. Data is correlated by the Mahalanobis distance between the predicted state value and the observed value. The Kuhn-Munkres algorithm is used to find the optimal matching solution. Then, the motion state parameters of the vehicle target in the next frame are updated by filtering.

2. The method for estimating vehicle motion state based on three-dimensional point clouds in a curved road environment according to claim 1, characterized in that, Step S1 further includes the following steps: S11. Based on the differences in spatial structural features, the curb and ground point clouds are segmented. Candidate curb point clouds are obtained by using the horizontal continuity, vertical continuity, and angle threshold constraints of the 3D point clouds. Then, the absolute height threshold is used to filter out vehicle, pedestrian, and other obstacle targets in the environment, and outliers are further removed to obtain the curb point cloud set. ; S12, K-means algorithm is used to cluster the points on both sides of the road edge to obtain the set of points. and ; S13, using least squares fitting, curves are fitted to the discretely distributed curb points on both sides, and the optimal matching function for the curb curves on both sides is found by minimizing the sum of squared deviations. , ; S14, derived from the optimal curve function of the roadside curbs on both sides. , A series of road center coordinate points are obtained, and a road center curve model is constructed using a cubic spline interpolation function.

3. The method for estimating vehicle motion state based on three-dimensional point clouds in a curved road environment according to claim 2, characterized in that, Step S11 further includes the following steps: S111, Establish a horizontal continuity constraint on adjacent point clouds of the same laser beam in the circumferential direction on the xy plane, and set a horizontal distance threshold. When the distance between adjacent scan points At that time, the adjacent point may be a curb point; S112, Establish a vertical continuity constraint on adjacent point clouds of the same laser beam in the circumferential direction in the xy plane, and set a vertical distance threshold. When the vertical distance between adjacent scan points At that time, the adjacent point may be a curb point; S113, for adjacent scanning points of the same laser beam in the circumferential direction Calculate the vector between adjacent scan points , The included angle Set angle threshold ,when At that time, the scanning point It may be a curb point; S114. Point clouds that simultaneously satisfy conditions S111, S112, and S113 are denoted as the road edge candidate point cloud set. Set height threshold Calculate adjacent consecutive scan points Maximum vertical height difference of road curb ,like and ,but Generate a set of curb points. ; S115, by setting the spatial neighborhood radius Calculate a certain scan point The point cloud distance threshold is obtained by comparing the mean and standard deviation of the distances between point clouds within the neighborhood. If any scan point and Distance between Greater than Then remove .

4. The method for estimating vehicle motion state based on three-dimensional point clouds in a curved road environment according to claim 1, characterized in that, Step S2 further includes the following steps: S21. Taking a starting center point O of the curved road as the origin, establish a global coordinate system with the tangent direction of the road center curve at the origin as the Y-axis and the normal direction as the X-axis. A curve coordinate system is established with the starting center point O of the curved road as the origin, the direction of the curve centerline as the S-axis, and the direction of the curve normal as the C-axis. Center point of the EV A vehicle coordinate system is constructed with the origin as the Y-axis along the EV velocity vector direction and the X-axis perpendicular to the EV velocity vector direction. ; S22, Calculate the coordinate system based on the positional relationship between the coordinate systems. and The coordinate transformation matrix will Target vehicle TV state parameters transformed from coordinate system to global coordinate system Calculate in coordinate system The distance from TV to the road center curve in the coordinate system and the relative position between TV and EV along the road center curve direction are used to obtain the curve coordinate system. TV coordinates, then according to the coordinate system and Calculation of vector transformation matrix between TV velocity vector in the coordinate system.

5. The method for estimating vehicle motion state based on three-dimensional point clouds in a curved road environment according to claim 1, characterized in that, Step S22 further includes the following steps: S221, Establish a coordinate system based on the relative positional relationship between coordinate systems. , The coordinate transformation matrix between coordinate systems Transform the detected surrounding TV state parameters to the coordinate system middle; S222, Global Coordinate System The shortest distance from TV to the center curve of the road is calculated using the curve tangent point normal. c The integral value of the piecewise function is calculated using the infinitesimal method to obtain the curvilinear coordinate system. The relative distance between TV and EV along the lower edge of the road center curve s The curvilinear coordinate system is obtained by conversion. TV coordinates; S223, according to the coordinate system and Vector transformation matrix between coordinate systems, calculate curvilinear coordinate system Lower TV velocity vector.

6. The method for estimating vehicle motion state based on three-dimensional point clouds in a curved road environment according to claim 1, characterized in that, Step S3 further includes the following steps: S31, the first k In the first frame point cloud scene j The motion state of each target vehicle is represented as follows: This includes TV coordinates in the curvilinear coordinate system, velocity components, acceleration components, heading angle, and rate of change of heading angle; S32, using a constant turning rate and acceleration model, constructs a vehicle target motion model in a curvilinear coordinate system and calculates the state transition matrix. Generate the vehicle target motion state equation; S33, the output of the vehicle target detection network k The vehicle target detection results of the first frame are transformed into a curvilinear coordinate system, and the first frame is calculated by constructing observation equations. k Frame vehicle target observations .

7. The method for estimating vehicle motion state based on three-dimensional point clouds in a curved road environment according to claim 1, characterized in that, Step S4 further includes the following steps: S41, using lossless transformation to linearize the state equation to obtain sampling points. and the corresponding weights of each sampling point ; S42, obtain the state sampling points according to the motion state equation. Calculate the first k Frame vehicle target state estimation Covariance Matrix ; Obtain the observation sampling points according to the observation equation Calculate the first k Prediction of frame observations and the new covariance matrix ; S43, calculate the... k Frame state prediction value and observed value prediction The Mahalanobis distance between them is used to obtain the optimal matching solution using the Kuhn-Munkres algorithm; S44, Calculate the predicted state value and observed value prediction cross-variance And through the new information covariance and mutual covariance Calculate the filter gain Update the status value Covariance Matrix .

Images