A lightweight point cloud feature map turnout lane identification method

Abstract

The present application relates to a kind of lightweight point cloud feature map fork lane identification method, belong to intelligent mobile platform positioning and navigation field.When approaching fork, corresponding lightweight map is loaded, laser radar point cloud and IMU data are collected and preprocessed, and point cloud integration is carried out to preprocessed point cloud;Real-time point cloud after integration and map point cloud are carried out feature extraction;According to feature set, laser odometry estimation result and IMU prediction result are fused in pose;Pose fusion result is converted to Frenet coordinate system and obtains multiple lane information alternative value;According to multiple groups of alternative values, real-time point cloud feature and map feature are associated, and the probability vector corresponding to each lane of vehicle is obtained, and the lane corresponding to the maximum value of probability vector is used as the lane where vehicle is currently located.The method does not need to rely on large-scale, complete high-precision map, can effectively realize lane identification of intelligent vehicle at fork, provides path constraint information to improve the performance of vehicle automatic driving in urban road.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of intelligent mobile platform positioning and navigation, and relates to a light point cloud feature map split lane recognition method. BACKGROUND

[0002] Lane recognition technology is one of the key technologies in multiple application fields. The visual lane recognition technology mainly includes deep learning technology schemes based on network recognition and semantic segmentation, and there are also schemes based on high-definition maps (HD map). In complex scenes such as split junctions, the lane at the split junction is complex. As a main perception sensor, the laser radar has important role in the perception and positioning functions of autonomous unmanned systems due to its high geometric information accuracy, strong anti-interference ability, and good light adaptability.

[0003] Chinese patent application: variable tidal lane automatic recognition method and device applied to automatic driving (application number: CN202211458204.0) discloses a lane recognition method using vision through deep learning, which includes: identifying the tidal lane sign by using a target detection model to obtain position recognition information and content recognition information, and obtaining the map position information of the tidal lane sign within a preset distance in front of the current position of the vehicle from the map. This method needs to rely on the lane sign at the split junction for recognition, and cannot be applied to the scene of ordinary split without lane sign. Chinese patent application: lane recognition method, device and storage medium (application number: CN202210743328.7) discloses a lane recognition method based on lane semantic segmentation model, which includes: extracting lane semantic graph by using lane semantic segmentation model, fitting lane line, and obtaining the best lane line by optimal variable weight regression. The lane recognition method provided by this method is suitable for self-lane recognition, and the split lane recognition effect is not good. Chinese patent application: lane recognition method, device and equipment in high-definition map (application number: CN202211532458.2) discloses a lane recognition method suitable for high-definition map, which is characterized by: generating a plurality of line segments by querying a plurality of reference points of high-definition map data, repeatedly screening the line segments to obtain a best line segment combination as a target lane line. This method relies on large-scale and complete high-definition map information. SUMMARY

[0004] Therefore, the purpose of the present application is to provide a light point cloud feature map split lane recognition method.

[0005] To achieve the above purpose, the present application provides the following technical scheme:

[0006] A light point cloud feature map split lane recognition method, comprising the following steps:

[0007] S1: Data preprocessing: Select point clouds of the region of interest, perform distortion correction on the point clouds, and simultaneously estimate the state of the IMU to obtain the current state. pass Obtain the initial state information x of the point cloud P at the current time. k ;

[0008] S2: Point cloud integration: By searching the historical frame queue, the point cloud at multiple consecutive moments is integrated over time to obtain the enhanced point cloud P';

[0009] S3: Feature Extraction: Based on the coarse latitude and longitude information, read the local point cloud map information of the fork in the pre-stored lightweight map of the fork. According to the feature point set extraction rules, divide the enhanced point cloud P' and the map point cloud into point sets to obtain the feature points.

[0010] S4: Pose Fusion: Combining point cloud features and Feature correlation is performed to obtain the laser odometry estimation results. Using an error-state Kalman filter to estimate the state of the IMU And laser odometry estimation results Fusion yields X k Through X k Update the state x of point cloud P' k ;

[0011] S5: Lane Determination: Pre-read the intersection lane information from the lightweight map to obtain the number of lanes n, lane width width, and lane direction dir information; then fuse the pose result X. k Transforming to the Frenet coordinate system yields multiple alternative lane information values ​​k:{(s,d±Δd1),(s,d±Δd2),...,(s,d±Δd...} n )};Will Transform to a ground-fixed coordinate system and the map feature set of the fork. Correlation is used to construct constraint equations and perform residual optimization to obtain position confidence vectors (scores) under multiple candidate values; combined with lane information related to the intersection, the probability vectors of the current vehicle in each lane are estimated.

[0012] S6: Lane Keeping Filter: Based on the lane probability vector Establish the Markov state transition probability matrix P ij(n·n) Lane-keeping filtering is performed, and the filtered result is weighted and fused with the estimated result to obtain the optimal estimated vector.

[0013] Further, the step S2 specifically comprises the following steps:

[0014] S21: coordinate system initialization: the map coordinate system is the mapping coordinate system W of the lightweight map; the pose transformation relationship between the map coordinate system and the ground fixed coordinate system is established through GNSS to obtain the pose in the ground fixed coordinate system, and the map data corresponding to the turnout is selected and read; the real-time point cloud of the laser radar is converted to the ground fixed coordinate system and matched with the turnout map, if the continuous convergence is obtained, it is considered that the turnout map range is entered, otherwise, no data processing is performed;

[0015] S22: establishing the state information of each frame of point cloud:

[0016] x k =[posi,pose,k] T

[0017] Wherein the position information posi=[x,y,z] represents the x position, y position and z position of the current point cloud P in the ground fixed coordinate system, the attitude information pose=[roll,pitch,yaw] represents the roll angle, pitch angle and yaw angle of the current point cloud in the ground fixed coordinate system; k represents the serial number of the current point cloud in the historical queue, which is used for point cloud integration to query the historical frame queue;

[0018] S23: establishing an IMU pre-integration model to obtain the initial state x k of the point cloud: obtaining the position and attitude state at two time points corresponding to two frames of radar data according to IMU pre-integration Wherein, p represents the position, q represents the attitude, and v represents the velocity; according to the initial pose state x k of the point cloud in the ground fixed coordinate system at the corresponding time is obtained.

[0019] S24: point cloud time integration: for the case that the single frame point cloud feature of the laser radar degenerates, the point cloud time integration is adopted for feature enhancement, according to the size of the feature point set extracted from the current frame, the point cloud integration window size window is adaptively determined through the mapping function F:

[0020] window=F(size)

[0021] (1) window=1: the window size is 1, the current environment does not exist degeneration, and the point cloud integration is not needed;

[0022] (2) window>1: the window size is greater than 1, the current environment appears degeneration, and the point cloud integration is needed;

[0023] The state information x k, in the historical frame queue, find the historical point cloud with the sequence number {k-window,..., k-1}, integrate and strengthen the current frame P to obtain the real-time point cloud P' after feature strengthening; wherein F is a window adaptive function, and the point cloud integration window size is determined by the feature point set size.

[0024] Further, the step S5 specifically comprises the following steps:

[0025] S51: read the lightweight map: obtain the lane number n, lane width width, and lane direction dir information corresponding to the turnout in advance;

[0026] S52: establish a feature matching initial estimate: associate the point cloud feature set of the current k time with the feature set of the previous time k-1 to obtain the laser odometry estimation result adopt an error state Kalman filter to fuse the IMU state and the laser odometry estimation result to obtain the k time pose fusion result X k =[x, y, z, roll, pitch, yaw] T as the initial value of lane position matching; wherein G k represents the ground point set of the current frame, P k represents the planar point set in the non-ground point set, and L k represents the corner point set;

[0027] S53: convert the pose fusion result X k to the Frenet coordinate system to obtain the position representation (s, d) in the Frenet coordinate system. At k time, by setting a changing Δd, a plurality of candidate values of lane information k: {(s, d±Δd1), (s, d±Δd2),..., (s, d±Δd n )} are obtained;

[0028] S54: according to the plurality of candidate values obtained in step S53, convert the real-time feature point cloud set to the ground-fixed coordinate system, associate it with the map feature set of the turnout , construct a constraint equation and optimize it to obtain a plurality of position confidences and normalize score=[score1, score2,..., score n ], score∈[0,1]

[0029] S55: Estimate the probability vector of the current vehicle in each lane by combining the lane-related information obtained in step S51, the number of lanes n, the lane width width, the lane direction dir, and the position confidence score score

[0030] Further, step S52 specifically includes the following steps:

[0031] S521: Obtain the extrinsic matrix of the laser radar coordinate system L and the IMU coordinate system I, i.e., the fixed coordinate transformation matrix T of the IMU coordinate system to the laser radar coordinate system, through laser radar and IMU extrinsic calibration L_I , the IMU information is converted to the radar coordinate system for data preprocessing:

[0032]

[0033]

[0034] wherein R L_I and t L_I represent the rotation matrix and the translation vector of the coordinate transformation matrix T L_I ; ω imu =[ω ix ,ω iy ,ω iz ] and a imu =[a ix ,a iy ,a iz ] represent the angular velocity and acceleration information of the xyz axes in the IMU coordinate system; ω lidar and a lidar represent the angular velocity and acceleration information in the radar coordinate system

[0035] S522: Obtain the initial state of the point cloud at the corresponding time point quickly through the establishment of the IMU pre-integration model:

[0036]

[0037] wherein, represents the position, velocity and attitude of the IMU in the ground-fixed coordinate system at the i-th time point; represents the position, velocity and attitude of the IMU in the ground-fixed coordinate system at the j-th time point; represents the attitude of the IMU in the i-th system at the j-th time point; represents the multiplication operation between two quaternions; according to the IMU pre-integration, the position, attitude and other states at the two time points corresponding to the two frames of radar data can be obtained wherein p represents the position, q represents the attitude in the form of a quaternion, and v represents the velocity

[0038] ​S523: The following extraction rules are established for real-time point clouds and map point clouds of intersections, specifically including:

[0039] Establish the feature set information for each frame of point cloud:

[0040] V k ={G k ,NG k}={G k ,P k ,L k}

[0041] Where {G k} represents the set of ground points in the current frame, {NG k} represents the set of non-ground points {P k ,L k}; where P k L represents the set of face points that are not on the ground. k ={left, right, normal} represents the set of corner points, which is further subdivided into three categories: left corner points, right corner points, and normal point sets; feature sets are extracted from the current frame and the local prior map to obtain the feature point set of the current frame. and local map feature point set

[0042] Feature extraction rules for point clouds: Based on the accurate geometric information provided by LiDAR, and targeting several typical features, the following extraction rules are established for the local point cloud map information at intersections in real-time point clouds and lightweight maps:

[0043] For the set {G}, the extraction rules are constructed as follows: the laser point cloud is subjected to double-threshold ground filtering, the preprocessed point cloud is projected onto the horizontal reference plane XOY of the lidar, the reference plane is divided into 2D grids of equal size, and the g of each grid is recorded. i Average minimum height and the minimum average height of the 8-neighbor grid Using the dual thresholds h1 and h2 to measure g i Each point p inside (i,j) Roughly divided into ground points and non-ground points, the specific calculation expression is as follows:

[0044]

[0045] In the above formula, p (i,j) G represents i The j-th point, h j Point p (i,j)the height value of the point; the random sample consensus algorithm RANSAC is used to complete the fitting of the plane equation; the direction of the IMU gravity vector and the actual installation height of the laser radar are used to complete the preliminary selection of the parameters h1 and h2;

[0046] For the set {NG}, the extraction rule is constructed: for L k in the non-ground points, the angle formed by the target point and the line segment between two adjacent points is considered, and the extraction of L k is completed by the preset parameters {τ1, τ2, τ3, τ4}:

[0047]

[0048]

[0049]

[0050] where p (i,j) represents the jth point of the ith scanning line of the laser radar in the non-ground points, λ i,j represents the depth value of the point, τ1, τ3, and τ4 are angle thresholds, and τ2 is a depth threshold; points not belonging to any of the above sets are divided into the set {P k}, and the random sample consensus algorithm is also used to complete the fitting of the plane equation;

[0051] S524: Establish an error function of association: transform the current point set at time k to time k-1 to obtain the representation of the current point set at the previous time;

[0052] For the set {G}, the distance d g between points is constructed; for the set {P}, the distance d p between points and planes is constructed; and for the set {L}, the distance d l between points and lines is constructed:

[0053]

[0054] wherein, represents the coordinates of point i at time k in the previous radar coordinate system; represents the coordinates of point i at time k in the current radar coordinate system;

[0055] The residuals constructed for the far and near distance point clouds are weighted:

[0056]

[0057] The optimization objective function is:

[0058]

[0059] Finally, the pose transformation matrix T that minimizes the sum of all distances is obtained;

[0060] S525: Establish the state transition model and measurement model of the target, and perform pose fusion of the laser odometry and IMU state to obtain a pose fusion result X k :

[0061]

[0062] wherein δx k =[δp,δθ,δv,δb a ,δb w ] T represents the state of the target at time k, including position deviation, attitude deviation, velocity deviation, and bias deviation of the IMU, represents the measurement of the target at time k, p x is a position prediction value calculated by the IMU, and p z is a position observation value calculated by the laser odometry; wherein is a rotation matrix calculated by the IMU, i.e., an attitude prediction value; is a rotation matrix calculated by the laser odometry; A is a state transition matrix, B is a matrix for converting input into state, H is a measurement matrix, w k-1 and v k are process noise and measurement noise, respectively, which are independent of each other;

[0063] The state at time k-1 is predicted to obtain one-step prediction value and covariance matrix of prediction error:

[0064]

[0065] wherein is the optimal state estimation of the target at time k-1, is the state prediction value of the target at time k, Q is process noise subject to normal distribution, P k - is the prior estimation covariance at time k, and P k-1 represents the posterior estimation covariance at time k-1;

[0066] The Kalman gain K k is calculated, the optimal state estimation of the target is obtained through the prior estimation and measurement information, and the optimal estimation covariance is updated:

[0067]

[0068] wherein R is measurement noise covariance, H is a conversion matrix of state variable to measurement, and I is an identity matrix.

[0069] Further, the step S6 specifically comprises the following steps:

[0070] S61: initial time, optimal estimation value of the lane

[0071] S62: according to the optimal estimation value of the lane at k-1 time through the Markov state transition probability matrix P ij(n·n) , the lane prediction vector p at k time is obtained k ;

[0072] S63: the lane prediction vector p at k time is weighted and fused with the lane estimation vector k , to obtain the optimal value of the current lane estimation

[0073] S64: according to the optimal estimation vector , the lane corresponding to the maximum probability value is taken as the lane where the current vehicle is located.

[0074] The beneficial effects of the present application are that the present application is based on the sliding window integration method of point cloud feature matching, and the point cloud integration is used to enhance the features of ordinary single-frame point cloud, so that more stable feature set is obtained through feature extraction, and the efficiency of feature matching is improved. At the same time, a sliding window is added to limit the size of the integration window, so as to ensure the real-time performance of the system. The lane calculation method of the present application obtains real-time feature matching results, obtains the probability of the current vehicle in each lane, and uses the Markov state transition probability matrix to perform lane keeping filtering, so as to greatly ensure the stability of the lane estimation. The method does not need to rely on large-scale and complete high-precision maps, and can effectively realize the lane recognition of intelligent vehicles at the fork, and provide path constraint information to improve the automatic driving performance of vehicles in urban roads.

[0075] Other advantages, objects, and features of the present application will be in part apparent and in part pointed out hereinafter in the specification, and it is to be understood that various changes can be made therein without departing from the scope of the present application. The objects and other advantages of the present application will be realized and attained by the structure particularly pointed out in the specification. BRIEF DESCRIPTION OF DRAWINGS

[0076] In order to make the objects, technical solutions and advantages of the present application clearer, the preferred embodiments of the present application will be described in detail below with reference to the accompanying drawings, in which:

[0077] Figure 1 is the overall algorithm flow of a fork lane recognition method of a lightweight point cloud feature map provided by the present application;

[0078] ​Figure 2 is a flowchart of the lane probability calculation method of the present application;

[0079] Figure 3 is a schematic diagram of the initial state acquisition of the point cloud;

[0080] Figure 4 is a flowchart of the pose fusion method of the present application. DETAILED DESCRIPTION

[0081] The present application can also be embodied or implemented by different specific embodiments, and the details of the specification can be modified or changed based on different views and applications without departing from the spirit of the present application. It should be noted that the drawings provided in the following examples only illustrate the basic concept of the present application in a schematic manner, and the following examples and features in the examples can be combined with each other without conflict.

[0082] The drawings are only used for exemplary illustration, and the representation is only a schematic diagram, not a physical diagram, and cannot be understood as a limitation on the present application; in order to better illustrate the embodiments of the present application, some components of the drawings are omitted, enlarged or reduced, and do not represent the size of the actual product; for those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings can be omitted.

[0083] The same or similar reference numerals in the drawings of the embodiments of the present application correspond to the same or similar components; in the description of the present application, it should be understood that if the terms "upper", "lower", "left", "right", "front", "back" and the like indicate the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, therefore the terms describing the positional relationship in the drawings are only used for exemplary illustration, and cannot be understood as a limitation on the present application, for those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0084] Please refer to Figures 1-4 , Figure 1 The specific implementation process of a lightweight point cloud feature map junction lane recognition method of the present application is shown, which includes the following steps:

[0085] (1) Data preprocessing: screening out the point cloud of the region of interest, distorting the point cloud, and simultaneously estimating the state of the IMU to obtain the current state By the initial state information x of the current point cloud P is obtainedk ;

[0086] (2) Point cloud integration: By searching the historical frame queue, the point cloud at multiple consecutive moments is integrated over time to obtain the enhanced point cloud P';

[0087] (3) Feature Extraction: Based on the coarse latitude and longitude information, the local point cloud map information of the fork in the pre-stored lightweight map of the fork is read. According to the feature point set extraction rules, the enhanced point cloud P' and the map point cloud are divided into point sets to obtain the feature points.

[0088] (4) Pose fusion: combining point cloud features and Feature correlation is performed to obtain the laser odometry estimation results. Using an error-state Kalman filter to estimate the state of the IMU And laser odometry estimation results Fusion yields X k Through X k Update the state x of point cloud P' k ;

[0089] (5) Lane identification: Pre-read the intersection lane information from the lightweight map to obtain information such as the number of lanes (n), lane width (width), and lane direction (dir). Then fuse the pose result X... k Transforming to the Frenet coordinate system yields multiple alternative lane information values ​​k:{(s,d±Δd1),(s,d±Δd2),...,(s,d±Δd...} n )};Will Transform to a ground-fixed coordinate system and the map feature set of the fork. Correlation is used to construct constraint equations and perform residual optimization to obtain position confidence vectors (scores) under multiple candidate values; combined with lane information related to the intersection, the probability of the current vehicle being in each lane is estimated.

[0090] (6) Lane keeping filter: Based on the lane probability vector obtained in (5) Establish the Markov state transition probability matrix P ij(n·n) Lane-keeping filtering is performed, and the filtered result is weighted and fused with the estimated result to obtain the optimal estimated vector. Ensure the smoothness and stability of lane estimation results.

[0091] Figure 2 This is a flowchart of the lane discrimination and filtering calculation method described in this invention. The flowchart specifically includes the following steps:

[0092] (1) First, the coordinate system correlation is performed. The map coordinate system is the mapping coordinate system W of the lightweight map. The pose transformation relationship between the map coordinate system and the ground fixed coordinate system is established through GNSS to obtain the pose in the ground fixed coordinate system, and the map data corresponding to the turnout is selected and read. The real-time point cloud of the laser radar is converted to the ground fixed coordinate system and matched with the turnout map. If the connection converges continuously, it is considered that the turnout map range is entered, otherwise no data processing is performed;

[0093] (2) Second, the feature matching is used to obtain the fusion pose result. The point cloud feature set of the current time k is matched with the feature set of the previous time k-1 to obtain the laser odometry estimation result The error state Kalman filter is used to fuse the IMU state and the laser odometry estimation result to obtain the current time k pose fusion result X k =[x, y, z, roll, pitch, yaw] T , which is used as the initial value X k of the lane position matching. Wherein G k represents the ground point set of the current frame, P k represents the planar point set in the non-ground point set, and L k represents the corner point set.

[0094] (2.1) The external parameter matrix of the laser radar coordinate system L and the IMU coordinate system I is obtained through the laser radar and IMU external parameter calibration, that is, the fixed coordinate transformation matrix T L_I from the IMU coordinate system to the laser radar coordinate system, and the

[0095] IMU information is converted to the radar coordinate system for data preprocessing:

[0096]

[0097] Wherein R L_I and t L_I represent the rotation matrix and translation vector of the coordinate transformation matrix T L_I . ω imu =[ω ix , ω iy , ω iz ] and a imu =[a ix , a iy , a iz ] represent the angular velocity and acceleration information of the xyz axes in the IMU coordinate system. ω lidar and a lidar represent the angular velocity and acceleration information in the radar coordinate system.

[0098] (2.2) Through the establishment of IMU pre-integration model, the initial state of the point cloud corresponding to the time point is quickly obtained:

[0099]

[0100] wherein, Position, velocity and attitude of the IMU in the ground-fixed coordinate system at the i-th time

[0101] Position, velocity and attitude of the IMU in the ground-fixed coordinate system at the j-th time

[0102] Attitude of the IMU at the j-th time in the i-th I system

[0103] Indicates the multiplication operation between two quaternions

[0104] According to the IMU pre-integration, the position, attitude and other states at the two times corresponding to the two frames of radar data can be obtained wherein, p represents the position, q is the attitude in the form of quaternion, and v is the velocity.

[0105] (2.3) The following extraction rules are established for the real-time point cloud and the map point cloud of the fork, which specifically include:

[0106] The feature set information of each frame of point cloud is established:

[0107] V k ={G k ,NG k}={G k ,P k ,L k} (3)

[0108] wherein {G k} represents the ground point set of the current frame, {NG k} represents the non-ground point set {P k , L k}. wherein P k represents the face point set in the non-ground point set, L k ={left, right, normal} represents the corner point set, and the corner point set is further subdivided into three categories: left corner point, right corner point and normal point set. The feature set is extracted from the current frame and the local prior map, to obtain the current frame feature point set and the local map feature point set

[0109] Establish feature extraction rules for point cloud: according to the accurate geometric information provided by the laser radar, for the above three typical features, the following extraction rules are established for the junction local point cloud map information of the real-time point cloud and the lightweight map:

[0110] For the set {G}, construct the extraction rule: perform double-threshold ground filtering on the laser point cloud, project the preprocessed point cloud to the horizontal reference plane XOY of the laser radar, divide the reference plane into 2D grids of equal size, and record the minimum height average value of each grid g i in the set {G} and the minimum height average value of the 8-neighbor grid Use the h1 and h2 double thresholds to roughly divide each point p i in g (i,j) into ground points and non-ground points, and the specific calculation expression is:

[0111]

[0112] In the above formula, p (i,j) represents the jth point in g i , h j represents the height value of point p (i,j) . In order to further optimize the fitting ground feature points, the random sample consensus algorithm (RANSAC) is used to complete the fitting of the plane equation. At the same time, the direction of the IMU gravity vector and the actual installation height of the laser radar can complete the preliminary selection of parameters h1 and h2.

[0113] For the set {NG}, construct the extraction rule: for the L k set in the non-ground points, consider the angle formed by the target point and the line segment between the two adjacent points, and complete the extraction of L k by presetting parameters {τ1, τ2, τ3, τ4}:

[0114]

[0115]

[0116]

[0117] Where p (i,j) represents the jth point of the i-th scan line of the laser radar in the non-ground points, λ i,j represents the depth value of the point, τ1, τ3, τ4 are angle thresholds, and τ2 is a depth threshold; points not belonging to any of the above sets are divided into the set {P k}, and the random sample consensus algorithm is also used to complete the fitting of the plane equation.

[0118] (2.4) Establish the error function of association: transform the current point set at k time to k-1 time, get the representation of the current point set at the previous time.

[0119] For the set {G}, construct the point-to-point distance d g ; for the set {P}, construct the point-to-plane distance d p ; for the set {L}, construct the point-to-line distance d l :

[0120]

[0121] where, - the coordinates of point i at k time in the previous frame radar coordinate system

[0122] - the coordinates of point i at k time in the current radar coordinate system

[0123] Considering the serious distortion of long-distance point cloud in laser point cloud, the influence of long-distance point cloud on optimization registration is reduced, and the residuals constructed by long-distance and short-distance point clouds are weighted:

[0124]

[0125] The optimization objective function is:

[0126]

[0127] Finally, the pose transformation matrix T that minimizes the sum of all distances is obtained.

[0128] (3) Establish the state transition model and measurement model of the target, perform pose fusion of laser odometry and IMU state, and obtain the pose fusion result X k :

[0129]

[0130] where, δx k = [δp, δθ, δv, δb a , δb w ] T represents the state of the target at k time, including position deviation, attitude deviation, velocity deviation, and bias deviation of IMU, represents the target measurement at k time, p x is the position prediction value calculated by IMU, p z is the position observation value calculated by laser odometry; wherein is the rotation matrix calculated by IMU, i.e. the attitude prediction value. Let be the rotation matrix obtained from the laser odometry. a is the state transition matrix, B is the matrix that transforms the input into the state, H is the measurement matrix, and w is the rotation matrix. k-1 and v k These are process noise and measurement noise, which are independent of each other.

[0131] By predicting the state at time k using time k-1, we obtain the covariance matrix of the one-step prediction value and the prediction error:

[0132]

[0133] in For the optimal state estimate at time k-1, Let P be the predicted state value at time k, Q be the process noise following a normal distribution, and P be the predicted state value at time k. k - Let P be the prior estimate of the covariance at time k. k-1 Let represent the posterior estimated covariance at time k-1.

[0134] Calculate the Kalman gain K k The optimal state estimate of the target is obtained through prior estimation and measurement information, and the optimal estimate covariance is updated:

[0135]

[0136] Where R is the measurement noise covariance, H is the transformation matrix from state variables to measurements, and I is the identity matrix.

[0137] (4) Based on the pose fusion result X at time k k As the initial pose estimate for point cloud feature matching, it is transformed to the Frenet coordinate system for point cloud feature matching to obtain the lane estimation vector.

[0138]

[0139] Based on the fused pose result X at time k k The position coordinates of the object in the ground-fixed coordinate system are transformed to the Frenet coordinate system and represented as (s,d).

[0140]

[0141] Pre-read lane information from a lightweight map, including the number of lanes n, lane width width, and lane direction dir. At time k, calculate the lane offset increment Δd of the current vehicle using the ψ(.) function, obtaining multiple candidate values ​​k for lane information: {(s,d±Δd1),(s,d±Δd2),...,(s,d±Δd...} n )};

[0142] Based on the above multiple sets of alternative values, the real-time feature point cloud set Transform to a ground-fixed coordinate system and the map feature set of the fork. Correlate the results, construct constraint equations and optimize them, calculate multiple sets of location confidence scores and normalize them to obtain: score = [score1, score2, ..., score] n ], score∈[0,1];

[0143] By combining the lane-related information and the position confidence vector (score), the probability vector of the current vehicle in each lane is estimated:

[0144]

[0145] (5) Finally, lane keeping filtering is performed using the Markov state transition probability matrix. A Markov state transition probability matrix P for n lanes is established. ij(n·n) The optimal estimated vector at time k-1 The prediction vector p at time k is obtained through the Markov state transition matrix. k , for p k Estimation results of matching point cloud features By performing weighted fusion, the optimal estimation vector at time k is obtained:

[0146]

[0147]

[0148] Along the direction of vehicle travel, the lane numbers are 1 to n from left to right, p 1n Let P represent the state transition probability from the leftmost lane to the rightmost lane. ij(n·n) The constraints of equation (18) are satisfied.

[0149] At the initial moment, the lane estimation vector obtained by matching point cloud features is... As the optimal estimation vector for lane probability

[0150]

[0151] Based on the optimal lane estimation vector at time k-1 Using the Markov state transition probability matrix P ij(n·n) The lane prediction vector p at time k is obtained. k :

[0152]

[0153] The lane prediction vector p at time k kwith the lane estimation vector The weighted fusion is performed with a weighting coefficient K to obtain an optimal estimation of the current lane probability The lane keeping probability is updated by formula (21) to take the optimal estimation vector of the current lane probability The lane corresponding to the maximum probability value of the lane estimation vector is taken as the lane in which the vehicle is located.

[0154]

[0155] Finally, it should be pointed out that the above embodiments are only used to illustrate the technical solutions of the present application and are not limiting. Although the present application has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that the technical solutions of the present application can be modified or replaced equivalently without departing from the purpose and scope of the technical solutions, and they should all be covered in the scope of the claims of the present application.

Claims

1. A method for identifying a fork lane of a lightweight point cloud feature map, characterized in that: Comprising the following steps: S1: data preprocessing: screening out point clouds of the region of interest, distorting the point clouds, and estimating the current state of the IMU to obtain the current state By obtaining the initial state information x of the current point cloud P k ; S2: Point cloud integration: through the history frame queue, the point cloud of multiple continuous time is time integrated to obtain the enhanced point cloud P'; S3: feature extraction: read the turnout local point cloud map information of the pre-stored turnout lightweight map according to the rough latitude and longitude information, and perform point set division on the enhanced point cloud P' and the map point cloud respectively to obtain S4: pose fusion: fuse the point cloud feature set with feature association to get laser odometry estimation results adopt error state Kalman filter to estimate the state of IMU and laser odometry estimation results fusion to get X k , update the state x of the point cloud P' through X k k ;​ S5: Lane discrimination: the bifurcation lane information of the lightweight map is read in advance to obtain the lane number n, lane width width, and lane direction dir information; The fused pose result X k Transforming to the Frenet coordinate system yields multiple alternative lane information values ​​k:{(s,d±Δd1),(s,d±Δd2),...,(s,d±Δd...} n )};Will Transform to a ground-fixed coordinate system and the map feature set of the fork. Correlation is used to construct constraint equations and perform residual optimization to obtain position confidence vectors (scores) under multiple candidate values; combined with lane information related to the intersection, the probability vectors of the current vehicle in each lane are estimated. S6: Lane keeping filtering: according to the lane probability vector A Markov state transition probability matrix P is established ij(n·n) Lane keeping filtering is performed, and the filtering result and the estimation result are weighted and fused to obtain an optimal estimation vector 2. The method of claim 1, wherein: The step S2 specifically comprises the following steps: S21: Coordinate system initialization: the map coordinate system is the mapping coordinate system W of the lightweight map; the pose transformation relationship between the map coordinate system and the ground fixed coordinate system is established through GNSS to obtain the pose in the ground fixed coordinate system, the map data corresponding to the bifurcation is selected and read; the real-time point cloud of the laser radar is converted to the ground fixed coordinate system and matched with the bifurcation map, if the continuous convergence is obtained, it is considered that the bifurcation map range is entered, otherwise, no data processing is performed; S22: Establish the state information of each frame of point cloud: x k = [posi, pose, k] T Wherein the position information posi=[x, y, z] represents the x position, y position and z position of the current point cloud P in the ground fixed coordinate system, the attitude information pose=[roll, pitch, yaw] represents the roll angle, pitch angle and yaw angle of the current point cloud in the ground fixed coordinate system; k represents the serial number of the current point cloud in the history queue, which is used for the query of the history frame queue by the point cloud integration; S23: Establish an IMU pre-integration model to obtain an initial state x of the point cloud k : Obtain the position and attitude state at the two time instants corresponding to the two radar data frames according to the IMU pre-integration where p represents the position, q is the attitude, and v is the velocity; according to obtain the initial pose state x of the point cloud in the ground-fixed coordinate system at the corresponding time instant k ; S24: Point cloud time integration: for the feature degradation of the single frame point cloud of the laser radar, the point cloud time integration is adopted for feature enhancement, according to the size of the feature point set extracted from the current frame, the mapping function F is adopted to adaptively determine the point cloud integration window size window: Window=F(size) (1) Window=1: the window size is 1, the current environment does not exist degradation, and the point cloud integration is not needed; (2) Window>1: the window size is greater than 1, the current environment appears degradation, and the point cloud integration is needed; Utilize the state information x of the point cloud in step S22 k , find the historical point cloud with sequence number {k-window,..., k-1} in the historical frame queue, integrate and strengthen the current frame P, and obtain the real-time point cloud P' after feature strengthening; wherein F is a window adaptive function, and the size of the point cloud integration window is determined by the size of the feature point set.

3. The method of claim 1, wherein: The step S5 specifically comprises the following steps: S51: Read the lightweight map: the lane number n, lane width width and lane direction dir information corresponding to the bifurcation are obtained in advance; S52: Establish feature matching initial estimation: the point cloud feature set of the current k moment is matched with the feature set of the previous moment k-1 Data association is performed to obtain laser odometry estimation results An error state Kalman filter is used to fuse the IMU state and the laser odometry estimation results to obtain the k moment pose fusion result X k =[x, y, z, roll, pitch, yaw] T , as the initial value of lane position matching; wherein G k represents the ground point set of the current frame, P k represents the planar point set in the non-ground point set, and L k represents the corner point set S53: fusing the pose results X k Convert to Frenet coordinate system, get the position representation (s, d) in Frenet coordinate system; at time k, by setting a changing Δd, get multiple candidate values of lane information k: {(s, d±Δd1), (s, d±Δd2),..., (s, d±Δd n )}. S54: According to the multiple sets of candidate values obtained in step S53, the real-time feature point cloud set is converted to the ground fixed coordinate system, and is associated with the map feature set of the turnout , a constraint equation is constructed and optimized to obtain multiple sets of position confidence and normalized score = [score1, score2, …, score n ], score ∈ [0, 1]; S55: Estimate the probability vector of the current vehicle in each lane by the lane-related information acquired in step S51, the lane number n, the lane width width, the lane direction dir, and the position confidence score score 4. The method of claim 3, wherein: The step S52 specifically comprises the following steps: S521: Obtain the extrinsic parameter matrix of the laser radar coordinate system L and the IMU coordinate system I through the laser radar and IMU extrinsic parameter calibration, that is, the fixed coordinate transformation matrix T of the IMU coordinate system to the laser radar coordinate system L_I , IMU information is converted to the radar coordinate system for data preprocessing: wherein R L_I and t L_I represent the rotation matrix and translation vector of the coordinate transformation matrix T L_I , respectively; ω imu = [ω ix , ω iy , ω iz ] and a imu = [a ix , a iy , a iz ] represent the angular velocity and acceleration information of the xyz axes in the IMU coordinate system, respectively; and ω lidar and a lidar represent the angular velocity and acceleration information in the radar coordinate system, respectively. S522: the initial state of the point cloud at the corresponding time is quickly obtained through the establishment of the IMU pre-integration model: wherein, represents the position, velocity and attitude of the IMU in the ground-fixed coordinate system at the i-th time instant; represents the position, velocity and attitude of the IMU in the ground-fixed coordinate system at the j-th time instant; represents the attitude of the IMU at the j-th time instant in the i-th coordinate system; represents the multiplication operation between two quaternions; according to the IMU pre-integration, the position, attitude and other states at two time instants corresponding to two frames of radar data can be obtained wherein, p represents the position, q is the attitude in the form of a quaternion, and v is the velocity; S523: the following extraction rules are established for the real-time point cloud and the map point cloud of the bifurcation, specifically including: Establish the feature set information of each frame of point cloud: V k = {G k ,NG k} = {G k , P k , L k} wherein {G k} represents a ground point set of the current frame, {NG k} represents a non-ground point set {P k ,L k} ; wherein P k represents a face point set in the non-ground point, L k ={left, right, normal} represents a corner point set, which is further subdivided into three categories: left corner point, right corner point, and normal point set; a feature set is extracted from the current frame and the local prior map, to obtain a current frame feature point set and a local map feature point set The feature extraction rules are established for the point cloud: according to the accurate geometric information provided by the laser radar, for several typical features, the following extraction rules are established for the real-time point cloud and the local point cloud map information of the bifurcation of the lightweight map: For the set {G}, the extraction rule is constructed: the laser point cloud is processed by double-threshold ground filtering, the preprocessed point cloud is projected to the horizontal reference plane XOY of the laser radar, the reference plane is divided into 2D grids of equal size, and the minimum height average value of each grid g i In the set {G} is recorded And the minimum height average value of the 8-neighbor grid Each point p i In g (i,j) Is roughly divided into ground points and non-ground points, and the specific calculation expression is: In the above formula, p (i,j) represents the jth point in g i , h j represents the height value of the point p (i,j) ; the fitting of the plane equation is completed by using the random sample consensus algorithm RANSAC; at the same time, the preliminary selection of parameters h1 and h2 is completed by using the direction of the IMU gravity vector and the actual installation height of the laser radar; For the set {NG}, construct the extraction rule: for L in NG k , consider the angle formed by the line segment between the target point and two adjacent points, and complete the extraction of L k by the preset parameters {τ1, τ2, τ3, τ4}: wherein p (i,j) represents the jth point of the ith scanning line of the laser radar in the non-ground point, λ i,j represents the depth value of the point, τ1, τ3, τ4 are angle threshold values, and τ2 is a depth threshold value; points not belonging to any of the above sets are divided into a set {P k} and fitting of the plane equation is also completed by using a random sample consensus algorithm. S524: Establish the error function of the association: transform the current point set at k time point to k-1 time point, and obtain the representation of the current point set at the previous time point. S524: Establish the error function of the association: transform the current point set at k time point to k-1 time point, and obtain the representation of the current point set at the previous time point. For the set {G}, construct the point-to-point distance d g ; for the set {P}, construct the point-to-plane distance d p ; for the set {L}, construct the point-to-line distance d l : wherein, represents the coordinate of point i at time k in the previous radar coordinate system; represents the coordinate of point i at time k in the current radar coordinate system; The residuals constructed by the long-distance and short-distance point clouds are weighted: The optimization objective function is: Finally, the pose transformation matrix T that minimizes the sum of all distances is obtained; S525: Establish the state transition model and measurement model of the target, perform pose fusion of the laser odometer and the IMU state, and obtain a pose fusion result X k : wherein, δx k = [δp, δθ, δv, δb a , δb w ] T represents the target state at time k, including position deviation, attitude deviation, velocity deviation and bias deviation of IMU, represents the target measurement at time k, p x is the position prediction value calculated by IMU, p z is the position observation value calculated by laser odometry; wherein is the rotation matrix calculated by IMU, i.e. the attitude prediction value; is the rotation matrix calculated by laser odometry; A is the state transition matrix, B is the matrix for converting input into state, H is the measurement matrix, w k-1 and v k are mutually independent process noise and measurement noise, respectively; The state at the k time is predicted by using the k-1 time to obtain the one-step prediction value and the covariance matrix of the prediction error: wherein is the optimal state estimate at time k-1, is the state prediction at time k, Q is the process noise that is normally distributed, P k - is the prior estimate covariance at time k, P k-1 denotes the posterior estimate covariance at time k-1; Computing Kalman gain K k Updating the optimal state estimate of the target by using the prior estimate and the measurement information, updating the optimal estimate covariance: Wherein R is the measurement noise covariance, H is the conversion matrix of the state variable to the measurement, and I is the unit matrix.

5. The method of claim 1, wherein: The step S6 specifically comprises the following steps: S61: Optimal estimate of the lane at the initial time S62: the optimal estimation value of the lane at time k-1 by a Markov state transition probability matrix P ij(n·n) , to obtain the lane prediction vector p at time k k ; S63: obtain the lane prediction vector p k with the lane estimation vector weighted fusion, to obtain the optimal value of the current lane estimation S64: According to the optimal estimation vector The lane corresponding to the maximum probability value is taken as the lane where the current vehicle is located.

Images