CN118031983B - Automatic driving fusion positioning method and system - Google Patents

Abstract

The invention relates to the technical field of automatic driving, and particularly discloses an automatic driving fusion positioning method and system, comprising the following steps: acquiring inertial measurement data of an IMU, and constructing an IMU pre-integration residual error according to a vehicle state prediction pose obtained after the vehicle state prediction is carried out on the inertial measurement data of the IMU; acquiring laser point cloud data, and constructing a laser odometer residual error after performing distortion correction processing on the laser point cloud data according to the vehicle state prediction pose; vector map information is obtained, vector region extraction and vector data screening are carried out on the vector map information according to the vehicle state prediction pose, and a vector map matching residual error is constructed; and carrying out joint pose optimization according to the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error to obtain the vehicle optimization pose. The automatic driving fusion positioning method provided by the invention can improve the positioning precision and the robustness of the automatic driving vehicle.

Description

Automatic driving fusion positioning method and system

Technical Field

The invention relates to the technical field of automatic driving, in particular to an automatic driving fusion positioning method and an automatic driving fusion positioning system.

Background

In recent years, autopilot technology research has become a hotspot and trend. The automatic driving technology comprises environment sensing, positioning navigation, path planning and decision control, and the vehicle high-precision positioning technology is a precondition for realizing vehicle decision and control. Common positioning techniques for autonomous vehicles fall into two categories: 1) Positioning is based on a global satellite navigation system (Global Navigation SATELLITE SYSTEM, abbreviated as GNSS). GNSS positioning accuracy is higher, but is easy to be influenced by shielding of service environments such as high buildings, tunnels, overhead garages, underground garages and the like and is invalid. 2) Based on autonomous sensor positioning. The SLAM (Simultaneous Localization AND MAPPING ) algorithm is to use laser radar or camera to realize real-time localization of the automatic driving vehicle, and has the problem of accumulated drift, while the track calculation based on IMU or wheel speed meter is a low-cost localization mode, which has the advantages of providing high-precision vehicle localization information in a short time according to sensor data, but the errors of the track calculation localization algorithm can be accumulated continuously with time, thus being not suitable for long-time independent localization.

In the face of complex traffic scenes, based on the information assistance of a high-precision map, an automatic driving vehicle can judge the position, a drivable area, a driving direction and the relative position of a front vehicle more easily, and meanwhile, the perception capability of beyond-the-horizon can be obtained, and the front gradient, curvature and transverse slope can be ascertained. The high-precision map can provide full-link assistance for perception, positioning, decision making, path planning and control for the autopilot system.

In summary, it is difficult for a single sensor to meet the positioning accuracy and robustness requirements of an autonomous vehicle. Therefore, how to provide an automatic driving fusion positioning method based on a high-precision vector map to improve the positioning precision and robustness of an automatic driving vehicle is a technical problem to be solved by those skilled in the art.

Disclosure of Invention

The invention provides an automatic driving fusion positioning method and an automatic driving fusion positioning system, which solve the problem that the positioning precision and the robustness of an automatic driving vehicle in the related technology are difficult to meet the requirements.

As a first aspect of the present invention, there is provided an autopilot fusion positioning method, including:

Acquiring inertial measurement data of an IMU, and constructing an IMU pre-integration residual error according to a vehicle state prediction pose obtained after the vehicle state prediction is carried out on the inertial measurement data of the IMU;

Acquiring laser point cloud data, and constructing a laser odometer residual error after performing distortion correction processing on the laser point cloud data according to the vehicle state prediction pose;

Vector map information is obtained, vector region extraction and vector data screening are carried out on the vector map information according to the vehicle state prediction pose, and a vector map matching residual error is constructed;

And carrying out joint pose optimization according to the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error to obtain the vehicle optimization pose.

Further, constructing an IMU pre-integration residual according to a vehicle state prediction pose obtained after the vehicle state prediction is performed on the IMU inertial measurement data, including:

according to the vehicle optimized pose at the previous moment, and combining the IMU inertial measurement data to predict the vehicle state, so as to obtain a vehicle state predicting pose;

constructing an IMU pre-integration amount according to the vehicle state prediction pose;

and constructing an IMU pre-integration residual according to the IMU pre-integration quantity.

Further, constructing an IMU pre-integration residual according to the IMU pre-integration amount, including:

Taking the IMU pre-integral quantity as a measured value, and restraining the state quantity between two moments to obtain an IMU pre-integral residual, wherein the calculation formula of the IMU pre-integral residual is as follows:

，

Wherein, Representing the pre-integral residual,/>Representing translation residual,/>Representing rotation residual,/>Representing the velocity residual,/>Residual error representing zero offset of accelerometer,/>Residual error representing zero bias of gyroscope,/>Rotation matrix representing world coordinate system at moment i to IMU coordinate system,/>Representing translation transformation from IMU coordinate system to world coordinate system at moment j,/>Representing translation transformation from IMU coordinate system to world coordinate system at moment i,/>Representing the velocity in world coordinate system at time i,/>Representing gravitational acceleration in world coordinate System,/>Representing a translation-related pre-integration quantity,/>Rotation matrix representing IMU coordinate system from moment j to moment iRepresenting the rotation matrix from IMU coordinate system to world coordinate system at time j,Representing the velocity in world coordinate system at time j,/>Representing a rotation-dependent pre-integration quantity,/>, ofRepresenting zero bias of accelerometer at moment j,/>Representing i moment accelerometer zero bias,/>Represents zero bias of gyroscope at moment j,/>Indicating the zero offset of the gyroscope at the moment i.

Further, constructing a laser odometer residual after performing distortion correction processing on the laser point cloud data according to the vehicle state prediction pose, including:

Performing point cloud distortion correction on the laser point cloud data according to the vehicle state prediction pose to obtain de-distorted point cloud data;

Extracting point cloud characteristics of the undistorted point cloud data to obtain point cloud characteristic points including corner points, mutation points and plane points;

sampling the laser point cloud data according to a frame sampling preset rule to form a key frame, and constructing a local map according to point cloud characteristic points corresponding to the key frame;

And matching the current frame formed by the current laser point cloud data with the local map to construct a laser odometer residual error.

Further, matching a current frame formed by current laser point cloud data with the local map to construct a laser odometer residual, including:

Performing pose transformation of the point cloud feature points according to the relative constraint relation between the current frame and the local map;

searching a straight line corresponding to the corner feature of the current frame and a plane corresponding to the plane point feature of the current frame in the local map;

and constructing laser odometer residual errors according to the distances from the corner points to the straight lines and the distances from the plane points to the plane.

Further, vector region extraction and vector data screening are performed on the vector map information according to the vehicle state prediction pose, and then vector map matching residual errors are constructed, including:

Searching a global vector map database according to the vehicle state prediction pose, obtaining a vector map comprising the current vehicle position, and taking a vector in the vector map comprising the current vehicle position as a candidate vector;

vector data screening is carried out on the candidate vectors, and a vector data screening result is obtained;

and matching the current frame formed by the current laser point cloud data with the vector data screening result to construct a vector matching residual error.

Further, vector data screening is performed on the candidate vector, and a vector data screening result is obtained, which includes:

Determining the effectiveness of candidate vectors according to the laser point cloud data corresponding to the current frame, and obtaining effective vectors;

And counting the number of the effective vectors, and uniformly dividing the space area to count the data of the effective vectors in each uniform area, so as to obtain a vector data screening result.

Further, matching the current frame formed by the current laser point cloud data with the vector data screening result to construct a vector matching residual, including:

selecting different types of characteristics matched with the vector data according to the vector data screening result, and constructing vector matching residual errors by utilizing the distances from point to straight line, point to plane and point to curved surface; wherein,

Matching of posts, street lamps and trees: i) The expression form is a linear equation, and a line segment k nearest neighbor algorithm is utilized to find corresponding points of a straight line in the corner points and the abrupt points; ii) the expression form is a plane equation, and a plane nearest neighbor algorithm is utilized to find the corresponding points of the plane in the corner points and the abrupt points; iii) The expression form is a curved surface equation, and a corresponding point of a curved surface is found in the corner points and the mutation points by utilizing a line segment k nearest neighbor algorithm;

For the matching of traffic lights, the expression form is plane, and the corresponding point of the plane is searched in the plane points by using a plane k nearest neighbor algorithm;

For the matching of the traffic sign board and the advertising board, the expression form is a plane, and a plane k nearest neighbor algorithm is utilized to find the corresponding point of the plane in the plane points;

For matching of house corners, the expression form is a straight line, and a line segment k nearest neighbor algorithm is utilized to find a corresponding point of the straight line in the corner points;

for matching of house corners, the expression form is plane, and a plane k nearest neighbor algorithm is utilized to find the corresponding point of the plane in the plane points;

Matching for guard rails: i) The expression form is a plane, and a plane k nearest neighbor algorithm is utilized to find a corresponding point of the plane in the mutation points; ii) the expression form is a straight line, and a corresponding point of the straight line is searched for in the abrupt points by using a line segment k nearest neighbor algorithm.

Further, performing joint pose optimization according to the IMU pre-integration residual, the laser odometer residual and the vector map matching residual to obtain a vehicle optimized pose, including:

constructing a cost function according to the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error;

And calculating the minimum value of the cost function to obtain the maximum posterior estimation of the state quantity to be optimized, and obtaining the vehicle optimization pose.

As another aspect of the present invention, there is provided an autopilot fusion positioning system for implementing the autopilot fusion positioning method described above, including:

The IMU pre-integration residual error construction module is used for acquiring IMU inertia measurement data and constructing an IMU pre-integration residual error according to a vehicle state prediction pose obtained after the vehicle state prediction is carried out on the IMU inertia measurement data;

the laser odometer residual error construction module is used for acquiring laser point cloud data, and constructing a laser odometer residual error after performing distortion correction processing on the laser point cloud data according to the vehicle state prediction pose;

the vector map matching residual error construction module is used for acquiring vector map information, extracting vector areas of the vector map information according to the vehicle state prediction pose and constructing vector map matching residual errors after vector data screening;

And the joint optimization module is used for carrying out joint pose optimization according to the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error to obtain the vehicle optimization pose.

According to the automatic driving fusion positioning method provided by the invention, inertial measurement data are acquired through the IMU, and the IMU is utilized to pre-integrate and predict the vehicle motion state and construct a pre-integrated residual error; then, the laser point cloud data are obtained, and then distortion correction is carried out on the laser point cloud data according to the vehicle state prediction pose, so that a laser odometer residual error is constructed; vector region extraction and vector data screening are carried out on vector map information through the vehicle state prediction pose, and then vector map matching residual errors are constructed; and finally, carrying out joint pose optimization through the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error to obtain the vehicle optimized pose. The automatic driving fusion positioning method fuses the laser odometer, the IMU pre-integration and the vector map matching, and can effectively improve the positioning accuracy and the robustness of the automatic driving vehicle.

Drawings

The accompanying drawings are included to provide a further understanding of the invention, and are incorporated in and constitute a part of this specification, illustrate the invention and together with the description serve to explain, without limitation, the invention.

Fig. 1 is a flowchart of an autopilot fusion positioning method provided by the present invention.

Fig. 2 is a flowchart for constructing IMU pre-integration residuals provided by the present invention.

Fig. 3 is a flow chart of the laser odometer residual error construction provided by the invention.

Fig. 4 is a flowchart of constructing a vector map matching residual error provided by the present invention.

FIG. 5 is a flow chart of joint pose optimization provided by the present invention.

Fig. 6 is a block diagram of an autopilot fusion positioning system provided by the present invention.

Fig. 7 is a schematic diagram of an autopilot fusion positioning system according to the present invention.

Detailed Description

It should be noted that, without conflict, the embodiments of the present invention and features of the embodiments may be combined with each other. The invention will be described in detail below with reference to the drawings in connection with embodiments.

In order that those skilled in the art will better understand the present invention, a technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in which it is apparent that the described embodiments are only some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present invention without making any inventive effort, shall fall within the scope of the present invention.

It should be noted that the terms "first," "second," and the like in the description and the claims of the present invention and the above figures are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate in order to describe the embodiments of the invention herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.

In this embodiment, an autopilot fusion positioning method is provided, and fig. 1 is a flowchart of the autopilot fusion positioning method provided in an embodiment of the present invention, as shown in fig. 1, including:

S100, acquiring inertial measurement data of an IMU, and constructing an IMU pre-integration residual error according to a vehicle state prediction pose obtained after vehicle state prediction of the IMU inertial measurement data;

in the embodiment of the invention, the IMU pre-integration quantity can be constructed through IMU inertial measurement data, and then the IMU pre-integration residual error is constructed based on the IMU pre-integration quantity.

S200, acquiring laser point cloud data, and constructing a laser odometer residual error after performing distortion correction processing on the laser point cloud data according to the vehicle state prediction pose;

in the embodiment of the invention, the laser point cloud data is subjected to distortion correction through the vehicle state prediction pose obtained in the previous step so as to remove distortion points, and then a laser odometer residual error is constructed according to the corrected point cloud data.

S300, acquiring vector map information, extracting a vector region of the vector map information according to the vehicle state prediction pose, and screening vector data to construct a vector map matching residual error;

In the embodiment of the invention, vector map matching residual errors are constructed by extracting vector areas, carrying out vector screening and carrying out vector feature matching according to the result of vector data screening.

And S400, carrying out joint pose optimization according to the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error to obtain the vehicle optimization pose.

And finally, constructing a joint positioning cost function by integrating the IMU pre-integral residual, the laser odometer residual and the vector map matching residual, and performing joint nonlinear optimization by using a gradient descent method to obtain the vehicle optimization pose.

Therefore, according to the automatic driving fusion positioning method provided by the invention, inertial measurement data are acquired through the IMU, and the IMU is utilized to pre-integrate and predict the motion state of the vehicle and construct a pre-integrated residual error; then, the laser point cloud data are obtained, and then distortion correction is carried out on the laser point cloud data according to the vehicle state prediction pose, so that a laser odometer residual error is constructed; vector region extraction and vector data screening are carried out on vector map information through the vehicle state prediction pose, and then vector map matching residual errors are constructed; and finally, carrying out joint pose optimization through the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error to obtain the vehicle optimized pose. The automatic driving fusion positioning method fuses the laser odometer, the IMU pre-integration and the vector map matching, and can effectively improve the positioning accuracy and the robustness of the automatic driving vehicle.

Specifically, an IMU pre-integration residual is constructed according to a vehicle state prediction pose obtained after the vehicle state prediction is performed on the IMU inertia measurement data, as shown in fig. 2, and includes:

s110, according to the vehicle optimization pose at the previous moment, and combining the IMU inertia measurement data to predict the vehicle state, and obtaining a vehicle state prediction pose;

in the embodiment of the invention, the IMU consists of a gyroscope and an accelerometer, angular velocity and acceleration are measured, and an IMU measurement equation is constructed:

，

，

Wherein the measured value is 、/>The true value is/>、/>，/>Rotation matrix representing world coordinate system to IMU coordinate system,/>Representing gravitational acceleration in world coordinate System,/>、/>Representing the bias of the gyroscope and accelerometer respectively,、/>Representing the measurement noise of the gyroscope and accelerometer, respectively.

By integrating theoretical values of angular velocity and accelerationVehicle prediction pose at time:

，

，

，

Wherein, Representing gravitational acceleration in a world coordinate system; /(I)Representing the time interval between adjacent laser frames; representation/> Rotation of the moment IMU coordinate system to the world coordinate system; /(I)And/>All represent integral quantities related to IMU measurements only.

S120, an IMU pre-integration quantity is constructed according to the vehicle state prediction pose;

In the embodiment of the invention, IMU measurement values among key frames are integrated into relative motion constraint, namely pre-integration quantity, so that the waste of calculation resources caused by repeated integration in the optimization process is avoided. The pre-integral quantity includes translation, speed, and rotation, and is related only to IMU measurements. The IMU pre-integral is calculated as follows:

，

，

，

Wherein, Representation/>Time IMU coordinate System to/>Rotation of the IMU coordinate system changes at the moment.

For the calculation of the pre-integral quantity, a median method is used, namely, the pose between adjacent moments k and k+1 is calculated by using the average value of the corresponding measured values of the vehicle moments:

，

，

，

，

，

，

。

S130, constructing an IMU pre-integration residual according to the IMU pre-integration quantity.

Specifically, constructing an IMU pre-integration residual according to the IMU pre-integration amount, including: and taking the IMU pre-integral quantity as a measured value, and restraining the state quantity between two moments to obtain an IMU pre-integral residual error.

In the embodiment of the invention, the state quantity between two moments is constrained by using the pre-integral quantity constructed by the IMU as a measured value. The IMU pre-integral residual in the embodiment of the invention is calculated according to the following formula:

，

Wherein, Representing the pre-integral residual,/>Representing translation residual,/>Representing rotation residual,/>Representing the velocity residual,/>Residual error representing zero offset of accelerometer,/>Residual error representing zero bias of gyroscope,/>Rotation matrix representing world coordinate system at moment i to IMU coordinate system,/>Representing translation transformation from IMU coordinate system to world coordinate system at moment j,/>Representing translation transformation from IMU coordinate system to world coordinate system at moment i,/>Representing the velocity in world coordinate system at time i,/>Representing gravitational acceleration in world coordinate System,/>Representing a translation-related pre-integration quantity,/>Rotation matrix representing IMU coordinate system from moment j to moment iRepresenting the rotation matrix from IMU coordinate system to world coordinate system at time j,Representing the velocity in world coordinate system at time j,/>Representing a rotation-dependent pre-integration quantity,/>, ofRepresenting zero bias of accelerometer at moment j,/>Representing i moment accelerometer zero bias,/>Represents zero bias of gyroscope at moment j,/>Indicating the zero offset of the gyroscope at the moment i.

Wherein the displacement, the speed and the bias in the error are all obtained by subtracting,Representing a rotation error with respect to a quaternion, wherein/>Representing taking only the imaginary part of the quaternion/>And (5) forming a three-dimensional vector.

，

The deduction can be obtained:

，

Consider the state quantity between adjacent time 、/>And (3) constantly, constructing an error recurrence formula based on first-order Taylor expansion, and acquiring a covariance matrix corresponding to IMU pre-integral data between two adjacent frames of lasers.

In the embodiment of the present invention, after performing distortion correction processing on the laser point cloud data according to the vehicle state prediction pose, a laser odometer residual error is constructed, as shown in fig. 3, including:

s210, carrying out point cloud distortion correction on the laser point cloud data according to the vehicle state prediction pose to obtain de-distorted point cloud data;

specifically, in order to ensure accuracy of point cloud matching, point cloud distortion correction is required. And correcting the motion distortion of the laser point cloud by using the vehicle prediction pose obtained by the previous method through a linear interpolation method. The de-distorted point cloud is used for subsequent local curvature calculation and point cloud matching.

S220, extracting point cloud characteristics of the undistorted point cloud data to obtain point cloud characteristic points including corner points, mutation points and plane points;

In the embodiment of the invention, the harness relation of the point cloud is determined according to the angle information, and a neighborhood point set is utilized on each scanning line And (4) calculating the curvature of the point cloud, wherein the calculation is shown in the following formula:

，

Wherein, Representing the local curvature of a point cloud,/>Representing point cloud measurements,/>Representing the neighborhood point measurement value.

The curvature threshold value in the embodiment of the invention is set to be 0.1, and three types of characteristic points are extracted through the curvature value and the distribution condition around the reference point: 1) Corner points, wherein the curvature is larger than a threshold value, and the periphery of the reference points is free from abrupt change; 2) The abrupt change point has abrupt change around the reference point when the curvature is larger than the threshold value; 3) The plane point, curvature is less than the threshold value while there is no abrupt change around the reference point. And calculating the local curvature in the double neighborhood range, and extracting the corner points, the abrupt points and the plane point characteristics which simultaneously meet the threshold value according to the local curvature. The double-neighborhood feature extraction algorithm avoids the influence of neighborhood size on local curvature, and improves the feature extraction stability.

In order to realize uniform sampling of feature points, each wire harness is uniformly divided into a plurality of areas, each area is provided with a plurality of corner points and plane points, and the mutation points are not required to be uniformly distributed. It is desirable to avoid selecting the following categories of points when selecting feature points: 1) Points that may be occluded; 2) Points around the point have been selected; 3) At a plane point where the laser lines are nearly parallel.

S230, sampling the laser point cloud data according to a frame sampling preset rule to form a key frame, and constructing a local map according to point cloud characteristic points corresponding to the key frame;

In the embodiment of the invention, the local point cloud map is used for determining an accurate characteristic corresponding relation, and key frames are screened according to the variation of translation and rotation. Historical key frame corresponding corner feature set Plane point feature set/>Unified transformation to keyframe/>, based on relative pose between different momentsAnd constructing the corner local map and the plane point local map corresponding to the laser radar coordinate system. Wherein/>Index corresponding to the median of m key frames.

And S240, matching the current frame formed by the current laser point cloud data with the local map to construct a laser odometer residual error.

Vehicle state prediction based on IMU data is utilized to provide good initial values for frame to local map matching. And carrying out pose transformation of the feature point cloud by utilizing the relative constraint relation between the current frame and the local map. And (3) rapidly searching a straight line corresponding to the corner point characteristic and a plane corresponding to the plane point characteristic in the local map by using a KD tree algorithm, so as to construct a laser odometer residual error by using the distances from point to straight line and from point to plane. Therefore, in the embodiment of the invention, based on the constructed local map and the initial estimation of the vehicle pose, the laser odometer residual error is constructed by adopting the matching of the frame and the local map.

Specifically, matching a current frame formed by current laser point cloud data with the local map to construct a laser odometer residual, including:

Performing pose transformation of the point cloud feature points according to the relative constraint relation between the current frame and the local map;

searching a straight line corresponding to the corner feature of the current frame and a plane corresponding to the plane point feature of the current frame in the local map;

and constructing laser odometer residual errors according to the distances from the corner points to the straight lines and the distances from the plane points to the plane.

Further specifically, the current time feature point cloud setAnd/>The point cloud in the model is used for realizing point cloud projection according to the relative pose relation, namely, the characteristic point cloud is converted into/>The time corresponds to a laser radar coordinate system. Based on the feature point clouds in the two types of local maps, constructing corresponding kd-trees and searching feature lines corresponding to the corner points and feature faces corresponding to the plane points. Namely, 1) dotted line ICP: using kd tree algorithm to quickly find two nearest points of each corner point, constructing straight line by using the nearest points and calculating the vertical foot coordinates of the point-to-straight line; 2) Dot-surface ICP: three nearest points of each plane point are quickly found by using a kd-Tree algorithm, planes are constructed by using the nearest points, and the drop coordinates/>, point-to-plane, are calculated。

For the followingCharacteristic points in the time laser point cloud are projected to/>The values in the time lidar coordinate system are:

，

Wherein the subscript Representing a lidar coordinate system,/>Representation/>Three-dimensional coordinates of moment laser feature points under laser radar coordinate system,/>Representation will/>Moment laser feature point conversion to/>Theoretical value in time laser radar coordinate system,/>Representing a transformation matrix between two coordinate systems, comprising a rotation matrix and a translation vector, namely:

，

The three-dimensional coordinate form is:

，

laser odometer residual errors are constructed by constraining measured values of the laser radar at the same moment, and the laser odometer residual errors are represented by using the point-to-straight line and the point-to-plane distances ：

，

Wherein,Representation will/>Time of day laser measurement Point transition to/>Projection points in a time laser radar coordinate system.Representing the projection point at/>Corresponding points in the time lidar coordinate system.

Namely:

，

Wherein, And representing the corresponding relation of the characteristic point cloud determined by matching the frame with the local map.

System state quantity bias derivative construction Jacobian matrix through laser odometer residual error：

，

The deduction can be obtained:

。

In the embodiment of the present invention, vector map matching residuals are constructed after vector region extraction and vector data screening are performed on the vector map information according to the vehicle state prediction pose, as shown in fig. 4, and the method includes:

s310, searching a global vector map database according to the vehicle state prediction pose, obtaining a vector map comprising the current vehicle position, and taking a vector in the vector map comprising the current vehicle position as a candidate vector;

It should be appreciated that using state prediction based on IMU measurements to provide an initial estimate of vehicle pose, a vector map near the current vehicle position is searched and extracted from a global vector map database. And setting a buffer zone based on the maximum value and the minimum value of the x, y and z terms in the corresponding point cloud data of the current frame, thereby ensuring that the constructed cuboid meets the range requirement, and all vectors positioned in the cuboid belong to candidate vectors.

S320, vector data screening is carried out on the candidate vectors, and a vector data screening result is obtained;

Specifically, in the embodiment of the invention, in order to ensure the effectiveness and high efficiency of vector matching, candidate vector data needs to be screened. And determining the effectiveness of the candidate vectors according to the laser point cloud information corresponding to the current frame, and simultaneously counting the number of the effective vectors and the uniformity of the spatial distribution. And uniformly dividing the space region and counting the number of vectors in each region. And calculating the uniformity of the spatial distribution based on a variance formula.

Therefore, vector data screening is carried out on the candidate vector, and a vector data screening result is obtained, which comprises the following steps:

Determining the effectiveness of candidate vectors according to the laser point cloud data corresponding to the current frame, and obtaining effective vectors;

And counting the number of the effective vectors, and uniformly dividing the space area to count the data of the effective vectors in each uniform area, so as to obtain a vector data screening result.

The candidate vector belongs to the following cases:

1) The plane range corresponding to the vector is smaller than the threshold value.

2) The length of the line segment corresponding to the vector is smaller than the threshold value.

3) The plane corresponding to the vector is approximately parallel to the laser scan line.

4) The position corresponding to the vector does not have point cloud information in a certain range.

5) The attributes corresponding to the vectors are obviously different from the distribution of surrounding point clouds.

6) Vector correspondence tags do not provide efficient, stable line or plane features.

7) The vector correspondence tag is an element that contributes less to SLAM, such as a lane line and a crosswalk.

Due to the diversity of vector map formats, a classification discussion is required. To extract line segments, planes and curved surfaces for vector matching, the corresponding processing is performed for several typical obstacles:

1) For feature extraction of upright posts, street lamps and trees, i) given the center line and the radius of a cylinder, a line segment corresponding to a laser scanning point needs to be calculated according to the pose, the center line position and the radius of the vehicle. ii) given the cylinder centerline and cross-section, the cross-section equations for smaller sized objects can be directly utilized. For larger objects, it is necessary to calculate the surface equation for the obstacle. iii) Given a cylindrical surface equation, no additional processing is required.

2) For feature extraction of traffic lights, i) the tangent plane of a given object, rasterizing and projecting by utilizing laser point clouds corresponding to the plane, and fitting a vector plane by utilizing plane points based on a least square method. ii) no additional processing is required given the section equation.

3) And for feature extraction of the traffic sign board and the advertising board, the traffic sign board and the advertising board are considered to belong to a standard plane, and a plane equation is calculated based on the center position and the size of the obstacle.

4) For feature extraction of house corners, i) two planes are given, and a straight line equation is calculated by using the intersection of the planes. ii) the corresponding line segment is given directly, without additional processing.

5) For feature extraction of house walls i) no additional processing is required for a given cross section. ii) given a plurality of planes, a plane needs to be selected according to the coordinate values.

6) For feature extraction of the guard rail, i) no additional processing is required for a given plane equation. ii) no additional processing is required given the line segment equation.

As for other possible obstacle types, targeted analysis is needed, so that information provided by a vector map can be fully utilized, and accuracy and robustness of vehicle positioning are ensured.

S330, matching the current frame formed by the current laser point cloud data with the vector data screening result to construct a vector matching residual error.

In the embodiment of the present invention, matching a current frame formed by current laser point cloud data with the vector data screening result to construct a vector matching residual, including:

selecting different types of characteristics matched with the vector data according to the vector data screening result, and constructing vector matching residual errors by utilizing the distances from point to straight line, point to plane and point to curved surface; wherein,

Matching of posts, street lamps and trees: i) The expression form is a linear equation, and a line segment k nearest neighbor algorithm is utilized to find corresponding points of a straight line in the corner points and the abrupt points; ii) the expression form is a plane equation, and a plane nearest neighbor algorithm is utilized to find the corresponding points of the plane in the corner points and the abrupt points; iii) The expression form is a curved surface equation, and a corresponding point of a curved surface is found in the corner points and the mutation points by utilizing a line segment k nearest neighbor algorithm;

For the matching of traffic lights, the expression form is plane, and the corresponding point of the plane is searched in the plane points by using a plane k nearest neighbor algorithm;

For the matching of the traffic sign board and the advertising board, the expression form is a plane, and a plane k nearest neighbor algorithm is utilized to find the corresponding point of the plane in the plane points;

For matching of house corners, the expression form is a straight line, and a line segment k nearest neighbor algorithm is utilized to find a corresponding point of the straight line in the corner points;

for matching of house corners, the expression form is plane, and a plane k nearest neighbor algorithm is utilized to find the corresponding point of the plane in the plane points;

Matching for guard rails: i) The expression form is a plane, and a plane k nearest neighbor algorithm is utilized to find a corresponding point of the plane in the mutation points; ii) the expression form is a straight line, and a corresponding point of the straight line is searched for in the abrupt points by using a line segment k nearest neighbor algorithm.

It should be understood that when performing frame-to-vector matching, 1) matching for posts, street lamps, and trees, i) takes the form of a straight line equation, and finds the corresponding points of the straight line in the corner points and the abrupt points by using a line segment k nearest neighbor algorithm. In the iterative optimization process, the corresponding straight line needs to be found again. ii) the expression form is a plane equation, and the corresponding points of the plane are searched in the corner points and the abrupt points by using a plane nearest neighbor algorithm. iii) The expression form is a curved surface equation, and the corresponding points of the curved surface are searched in the corner points and the mutation points by utilizing a line segment k nearest neighbor algorithm.

2) For the matching of traffic lights, the expression form is plane, and the corresponding point of the plane is searched in the plane points by using a plane k nearest neighbor algorithm.

3) For the matching of the traffic sign board and the advertising board, the expression form is a plane, and the plane k nearest neighbor algorithm is utilized to search the corresponding point of the plane in the plane points.

4) For matching of house corners, the expression form is a straight line, and a line segment k nearest neighbor algorithm is utilized to find the corresponding point of the straight line in the corner points.

5) For matching of house corners, the appearance form is a plane, and the corresponding point of the plane is found in the plane points by using a plane k nearest neighbor algorithm.

6) For the matching of the guard rails, i) the expression form is a plane, and the corresponding point of the plane is searched for in the mutation points by using a plane k nearest neighbor algorithm. ii) the expression form is a straight line, and a corresponding point of the straight line is searched for in the abrupt points by using a line segment k nearest neighbor algorithm.

The current frame matches the vector data adding point-to-surface constraints. Different obstacles are matched with different types of characteristics, so that vector matching residual errors are constructed by utilizing the distances from point to straight line, point to plane and point to curved surface。

In the embodiment of the present invention, joint pose optimization is performed according to the IMU pre-integration residual error, laser odometer residual error and vector map matching residual error, so as to obtain a vehicle optimized pose, as shown in fig. 5, including:

s410, constructing a cost function according to the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error;

s420, calculating the minimum value of the cost function to obtain the maximum posterior estimation of the state quantity to be optimized, and obtaining the vehicle optimization pose.

The combined optimization algorithm constructs a system cost function by utilizing a laser odometer residual error, an IMU pre-integral residual error and a vector matching residual error, and obtains the state quantity of the system to be optimized by calculating the minimum value of the system cost functionIs a maximum a posteriori estimate of (b). And carrying out joint optimization of three residuals through an optimization solver, so that the constraint among data is strengthened to improve the accuracy and the robustness of automatic driving vehicle positioning.

Specifically, the cost function of the multi-source fusion vehicle positioning system is constructed as follows:

，

wherein, three residual errors are represented by the mahalanobis distance. Representing laser odometer residual error, covariance matrixDetermined by the laser radar accuracy. /(I)Representing IMU pre-integral residual, covariance matrix/>Determined by a pre-integral error transfer formula. /(I)Representing vector matching residual, covariance matrix/>Determined by the accuracy of the lidar and vector map. /(I)Representing the weight of the corresponding residual. Scheme is initially set for the sizes of three weights,/>. For vector matching residual corresponding weights/>The setting of (2) requires that the effective number of vectors and the spatial distribution uniformity be fully considered. Namely:

，

Wherein, Representing the effective number of vectors,/>Representing the variance corresponding to the spatial distribution of the effective vectors.

In summary, according to the automatic driving fusion positioning method provided by the invention, inertial measurement data are acquired based on the IMU, and the IMU is utilized to pre-integrate and predict the vehicle motion state and construct a pre-integrated residual error; calculating the local curvature of the point cloud according to the neighborhood point information, extracting the characteristics of angular points, plane points and abrupt points based on a double neighborhood strategy, constructing a local point cloud map, and constructing a laser odometer residual error through frame and local map matching; extracting a vector region, carrying out vector screening, and matching the extracted feature point cloud with vector features to construct a vector map matching residual error; and constructing a cost function of the fusion positioning system according to the IMU pre-integral residual, the laser odometer residual and the map matching residual, and carrying out joint nonlinear optimization by using a gradient descent method to obtain the vehicle optimization pose. Therefore, the automatic driving fusion positioning method provided by the invention realizes the data fusion of the laser radar, the IMU and the vector map, and the tight coupling algorithm of the laser radar, the IMU and the vector map ensures the positioning precision of the automatic driving vehicle. In addition, a basis is provided for the motion distortion correction of the point cloud through IMU pre-integration, and the accurate laser point cloud is obtained based on a linear interpolation method, so that the accuracy of data association is facilitated; providing a good initial value for a laser SLAM algorithm through IMU state prediction, avoiding the point cloud matching algorithm from sinking into a local extremum, improving the efficiency and accuracy of a positioning algorithm, and providing position information for screening a vector region; according to the method, the local curvature of the point cloud is calculated through the neighborhood point information, the characteristics of the angular points, the plane points and the abrupt points are extracted by adopting a double-neighborhood strategy, the influence of the neighborhood size on the local curvature calculation is reduced, and the accuracy and the robustness of the characteristic extraction are improved. According to the invention, different extraction and matching strategies are adopted for different types of vector features, so that the effective utilization of the vector map is realized, and the application range of the fusion positioning system is improved.

As another embodiment of the present invention, an autopilot fusion positioning system 10 is provided for implementing the autopilot fusion positioning method described above, wherein, as shown in fig. 6 and 7, the autopilot fusion positioning system includes:

the IMU pre-integration residual error construction module 100 is used for acquiring IMU inertia measurement data and constructing an IMU pre-integration residual error according to a vehicle state prediction pose obtained after the vehicle state prediction is carried out on the IMU inertia measurement data;

The laser odometer residual error construction module 200 is used for acquiring laser point cloud data, and constructing a laser odometer residual error after performing distortion correction processing on the laser point cloud data according to the vehicle state prediction pose;

The vector map matching residual error construction module 300 is used for acquiring vector map information, extracting a vector region of the vector map information according to the vehicle state prediction pose, and constructing a vector map matching residual error after vector data screening;

And the joint optimization module 400 is used for performing joint pose optimization according to the IMU pre-integration residual error, the laser odometer residual error and the vector map matching residual error to obtain the vehicle optimization pose.

According to the automatic driving fusion positioning system provided by the invention, inertial measurement data are acquired through the IMU, and the IMU is utilized to pre-integrate and predict the motion state of the vehicle and construct a pre-integrated residual error; then, the laser point cloud data are obtained, and then distortion correction is carried out on the laser point cloud data according to the vehicle state prediction pose, so that a laser odometer residual error is constructed; vector region extraction and vector data screening are carried out on vector map information through the vehicle state prediction pose, and then vector map matching residual errors are constructed; and finally, carrying out joint pose optimization through the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error to obtain the vehicle optimized pose. The automatic driving fusion positioning method fuses the laser odometer, the IMU pre-integration and the vector map matching, and can effectively improve the positioning accuracy and the robustness of the automatic driving vehicle.

The specific working principle of the autopilot fusion positioning system of the present invention may refer to the foregoing description of the autopilot fusion positioning method, and will not be repeated herein.

It is to be understood that the above embodiments are merely illustrative of the application of the principles of the present invention, but not in limitation thereof. Various modifications and improvements may be made by those skilled in the art without departing from the spirit and substance of the invention, and are also considered to be within the scope of the invention.

Claims (7)

1. An autopilot fusion positioning method, comprising:

Acquiring inertial measurement data of an IMU, and constructing an IMU pre-integration residual error according to a vehicle state prediction pose obtained after the vehicle state prediction is carried out on the inertial measurement data of the IMU;

Acquiring laser point cloud data, and constructing a laser odometer residual error after performing distortion correction processing on the laser point cloud data according to the vehicle state prediction pose;

Vector map information is obtained, vector region extraction and vector data screening are carried out on the vector map information according to the vehicle state prediction pose, and a vector map matching residual error is constructed;

performing joint pose optimization according to the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error to obtain a vehicle optimized pose;

the method for constructing the IMU pre-integration residual error according to the vehicle state prediction pose obtained after the vehicle state prediction is carried out on the IMU inertia measurement data comprises the following steps:

according to the vehicle optimized pose at the previous moment, and combining the IMU inertial measurement data to predict the vehicle state, so as to obtain a vehicle state predicting pose;

constructing an IMU pre-integration amount according to the vehicle state prediction pose;

constructing an IMU pre-integration residual according to the IMU pre-integration quantity;

constructing an IMU pre-integration residual according to the IMU pre-integration quantity, comprising:

Taking the IMU pre-integral quantity as a measured value, and restraining the state quantity between two moments to obtain an IMU pre-integral residual, wherein the calculation formula of the IMU pre-integral residual is as follows:

，

Wherein, Representing the pre-integral residual,/>Representing translation residual,/>Representing rotation residual,/>Representing the velocity residual,/>Residual error representing zero offset of accelerometer,/>Residual error representing zero bias of gyroscope,/>Rotation matrix representing world coordinate system at moment i to IMU coordinate system,/>Representing translation transformation from IMU coordinate system to world coordinate system at moment j,/>Representing translation transformation from IMU coordinate system to world coordinate system at moment i,/>Representing the velocity in world coordinate system at time i,/>Representing gravitational acceleration in world coordinate System,/>Representing a translation-related pre-integration quantity,/>Rotation matrix representing IMU coordinate system from moment j to moment iRotation matrix representing IMU coordinate system to world coordinate system at j moment,/>Representing the velocity in world coordinate system at time j,/>Representing a rotation-dependent pre-integration quantity,/>, ofRepresenting zero bias of accelerometer at moment j,/>Representing i moment accelerometer zero bias,/>Represents zero bias of gyroscope at moment j,/>Indicating zero offset of the gyroscope at the moment i;

performing joint pose optimization according to the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error to obtain a vehicle optimized pose, including:

constructing a cost function according to the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error;

calculating the minimum value of the cost function to obtain the maximum posterior estimation of the state quantity to be optimized, and obtaining the vehicle optimization pose;

wherein, the expression of the cost function is:

，

Wherein, Representing laser odometer residual, covariance matrix/>As determined by the accuracy of the lidar,Representing IMU pre-integral residual, covariance matrix/>Determined by a pre-integral error transfer formula,/>Representing vector matching residual, covariance matrix/>Determined by the accuracy of the lidar and vector map.

2. The automated driving fusion positioning method according to claim 1, wherein constructing a laser odometer residual after performing distortion correction processing on the laser point cloud data according to the vehicle state prediction pose comprises:

Performing point cloud distortion correction on the laser point cloud data according to the vehicle state prediction pose to obtain de-distorted point cloud data;

Extracting point cloud characteristics of the undistorted point cloud data to obtain point cloud characteristic points including corner points, mutation points and plane points;

sampling the laser point cloud data according to a frame sampling preset rule to form a key frame, and constructing a local map according to point cloud characteristic points corresponding to the key frame;

And matching the current frame formed by the current laser point cloud data with the local map to construct a laser odometer residual error.

3. The automated driving fusion location method of claim 2, wherein matching a current frame formed by current laser point cloud data with the local map to construct a laser odometer residual, comprises:

Performing pose transformation of the point cloud feature points according to the relative constraint relation between the current frame and the local map;

searching a straight line corresponding to the corner feature of the current frame and a plane corresponding to the plane point feature of the current frame in the local map;

and constructing laser odometer residual errors according to the distances from the corner points to the straight lines and the distances from the plane points to the plane.

4. The automated driving fusion positioning method according to claim 1, wherein the vector map information is vector region extracted and vector data filtered according to the vehicle state prediction pose to construct a vector map matching residual, comprising:

Searching a global vector map database according to the vehicle state prediction pose, obtaining a vector map comprising the current vehicle position, and taking a vector in the vector map comprising the current vehicle position as a candidate vector;

vector data screening is carried out on the candidate vectors, and a vector data screening result is obtained;

and matching the current frame formed by the current laser point cloud data with the vector data screening result to construct a vector matching residual error.

5. The autopilot fusion positioning method of claim 4 wherein vector data screening is performed on the candidate vectors to obtain a vector data screening result, comprising:

Determining the effectiveness of candidate vectors according to the laser point cloud data corresponding to the current frame, and obtaining effective vectors;

And counting the number of the effective vectors, and uniformly dividing the space area to count the data of the effective vectors in each uniform area, so as to obtain a vector data screening result.

6. The automated driving fusion positioning method of claim 4, wherein matching a current frame formed by current laser point cloud data with the vector data screening result to construct a vector matching residual comprises:

selecting different types of characteristics matched with the vector data according to the vector data screening result, and constructing vector matching residual errors by utilizing the distances from point to straight line, point to plane and point to curved surface; wherein,

Matching of posts, street lamps and trees: i) The expression form is a linear equation, and a line segment k nearest neighbor algorithm is utilized to find corresponding points of a straight line in the corner points and the abrupt points; ii) the expression form is a plane equation, and a plane nearest neighbor algorithm is utilized to find the corresponding points of the plane in the corner points and the abrupt points; iii) The expression form is a curved surface equation, and a corresponding point of a curved surface is found in the corner points and the mutation points by utilizing a line segment k nearest neighbor algorithm;

For the matching of traffic lights, the expression form is plane, and the corresponding point of the plane is searched in the plane points by using a plane k nearest neighbor algorithm;

For the matching of the traffic sign board and the advertising board, the expression form is a plane, and a plane k nearest neighbor algorithm is utilized to find the corresponding point of the plane in the plane points;

For matching of house corners, the expression form is a straight line, and a line segment k nearest neighbor algorithm is utilized to find a corresponding point of the straight line in the corner points;

for matching of house corners, the expression form is plane, and a plane k nearest neighbor algorithm is utilized to find the corresponding point of the plane in the plane points;

Matching for guard rails: i) The expression form is a plane, and a plane k nearest neighbor algorithm is utilized to find a corresponding point of the plane in the mutation points; ii) the expression form is a straight line, and a corresponding point of the straight line is searched for in the abrupt points by using a line segment k nearest neighbor algorithm.

7. An autopilot fusion positioning system for implementing the autopilot fusion positioning method of any one of claims 1 to 6, comprising:

The IMU pre-integration residual error construction module is used for acquiring IMU inertia measurement data and constructing an IMU pre-integration residual error according to a vehicle state prediction pose obtained after the vehicle state prediction is carried out on the IMU inertia measurement data;

the laser odometer residual error construction module is used for acquiring laser point cloud data, and constructing a laser odometer residual error after performing distortion correction processing on the laser point cloud data according to the vehicle state prediction pose;

the vector map matching residual error construction module is used for acquiring vector map information, extracting vector areas of the vector map information according to the vehicle state prediction pose and constructing vector map matching residual errors after vector data screening;

And the joint optimization module is used for carrying out joint pose optimization according to the IMU pre-integral residual error, the laser odometer residual error and the vector map matching residual error to obtain the vehicle optimization pose.