1. A unified modeling method for vehicle positioning and path planning is characterized by comprising the following steps:

s1, processing the laser point cloud data to obtain a two-dimensional grid map;

s2, mapping vehicle track information and a two-dimensional grid map in a laser coordinate system to a GPS coordinate system through calibrating the mapping relation between the point cloud and the GPS data;

s3, determining a path planning track result in the two-dimensional grid map based on the A star algorithm;

and S4, taking the path planned track as a constraint track, calculating the transverse distance between the GPS positioning result and the laser radar positioning result to the constraint track as the constraint in the Kalman filter, and fusing the GPS positioning result and the laser positioning result.

2. The unified modeling method for vehicle positioning and path planning according to claim 1, characterized in that step S1 includes:

s11, preprocessing the laser point cloud data, eliminating noise points and outliers, simultaneously filtering out road point cloud information in the scene, and only keeping inherent static obstacle information except the road information in the scene;

and S12, carrying out projection processing on the XOY plane on the preprocessed three-dimensional laser point cloud data, then uniformly dividing the whole XOY plane into grids with a certain size, and establishing that the grids with point clouds are in an occupied state and the grids without the point clouds are in a free state to construct a two-dimensional grid map.

3. The unified modeling method for vehicle positioning and path planning according to claim 2, characterized in that step S11 includes:

s111, retaining point cloud data within a certain distance from the vehicle by adopting a straight-through filter, namely: if the coordinate (x) of a certain point in the laser point cloud data_i，y_i，z_i) Satisfies the following conditions: x is the number of_i∈(X₁,X₂)∩y_i∈(Y₁,Y₂)∩z_i∈(Z₁,Z₂) Then the point is retained; wherein, X_i、Y_iAnd Z_iRespectively setting threshold values of a laser point cloud coordinate system along X, Y and Z-axis directions;

s112, performing filtering and downsampling processing on the laser point cloud data by adopting a voxel grid filter, dividing the point cloud data into a plurality of voxel grids, calculating the gravity center of each voxel grid, and representing all points in the voxel grids by using gravity center points;

s113, taking the points with the z coordinate smaller than a certain height as a candidate ground point set P', and sequencing the points according to the height z from small to large to obtain the average height z_aveRecalculated height less than z_ave+z_thresholdNormal to the point of (1), Z_thresholdRepresenting a preset threshold value, reserving points with an included angle with the z axis larger than a preset angle, and taking all the remaining points as a final ground point set P_groundThereby eliminating the final ground point set P_ground。

4. The unified modeling method for vehicle positioning and path planning according to claim 1, characterized in that step S2 includes:

s21, selecting a marker G in the laser point cloud scanning range;

s22, collecting GPS coordinate position information (Gx, Gy, Gz) of marker G^T；

S23, extracting the laser coordinate position (x, y, z) of the marker G in the laser coordinate system^T；

And S24, obtaining a rotation matrix R and a translation matrix T according to the least square problem.

5. The unified modeling method for vehicle positioning and path planning according to claim 4, characterized in that step S23 includes:

s231, removing outliers and noise points in the laser point cloud;

s232, fitting by using RANSAC algorithm to obtain a point set of the vertical rod G of the marker, and calculating a density formula of the point set to obtain a centroid coordinate position (x, y, z) of the vertical rod^TThe coordinate position is taken as the laser coordinate position (x, y, z) of the vertical rod G in the laser coordinate system^T。

6. The unified modeling method for vehicle positioning and path planning according to claim 4, characterized in that step S24 includes:

s241, the following relation exists between the coordinates of the markers:

in the formula, R is a rotation matrix from a laser coordinate system to a GPS coordinate system, and T is a translation matrix from the laser coordinate system to the GPS coordinate system;

s242, performing a process of approximating 0 to the coordinate in the Z direction, where the above equation is converted as follows:

wherein [ x, y [ ]]^TIs the coordinate position of the marker G in the GPS coordinate system, [ Gx, Gy]^TIs the coordinate position of the marker G under the laser coordinate system;

s243, the expression after the approximation processing may be:

conversion to equation set form:

and then becomes a matrix form:

i.e. can be converted to the least squares problem: ax ═ b, and solving to obtain the solution x ═ a of the equation^TA)^-1A^Tb is to solve the rotation matrix R and the translation matrix T.

7. The unified modeling method for vehicle positioning and path planning according to claim 1, characterized in that step S3 includes:

s31, setting a starting point and an end point of a path in the two-dimensional grid map, and determining the position information of the starting point and the end point in the two-dimensional grid map;

s32, selecting the optimal direction to expand and search according to the heuristic function of the A-star algorithm;

s33, calculating the Euclidean distance:

in the formula (x)_n,y_n) Coordinates representing the current node, (x)_d,y_d) Coordinates representing a target node;

and S34, searching by combining the A-algorithm through using the OPEN list and the CLOSED list, and generating a path planning track result from the starting point to the end point in the grid map.

8. The unified modeling method for vehicle positioning and path planning according to claim 1, characterized in that step S4 includes:

s41, recording the current vehicle position acquisition point as a point P, and acquiring the GPS position information [ x, y ] of the vehicle position acquisition point P in real time]^TAnd convert it to UTM coordinate system: u shape_P＝(x_P,y_P)＝trans(x,y)；(x_P,y_P) For the GPS position information of the position acquisition point P in the UTM coordinate system, trans () is a conversion relation for converting the GPS coordinate system into the UTM coordinate system;

meanwhile, according to the rotation matrix R, the translation matrix T and the transform relation trans () from the GPS coordinate system to the UTM coordinate system, the position [ Gx, Gy ] of the vehicle position acquisition point P from the laser coordinate system is obtained]^TConversion to position under UTM coordinate system: u shape_G＝(x_G,y_G)＝trans(R*[Gx,Gy]^T+T)；(x_G,y_G) Acquiring laser radar position information of a position acquisition point P in a UTM coordinate system;

converting the path planning track trace into a UTM coordinate system according to the transformation relation;

s42, respectively calculating the transverse distance between the GPS positioning result and the laser radar positioning result to the planned track;

and S43, taking the path planned track as a constraint track, taking the transverse distance between the GPS positioning result and the laser radar positioning result to the constraint track as the constraint in the Kalman filter, and fusing the GPS positioning result and the laser positioning result.

9. The unified modeling method for vehicle positioning and path planning according to claim 8, wherein step S42 includes:

s421, the path planning track is composed of straight line segments, and a straight line equation of the path planning track is calculated by a two-point straight line equation: l: a_ix+b_iy+c_i＝0；

S422, acquiring GPS position information U of the point under the UTM coordinate system according to the vehicle position_P＝(x_P,y_P) And laser radar position information U_G＝(x_G,y_G) Respectively solving the transverse distance:

10. the unified modeling method for vehicle positioning and path planning according to claim 9, wherein step S43 includes:

s431, using the current position x_kAnd taking a track formed by the N historical positions as a current state: x_k＝[x_k x_k-1 … x_k-N]Each position comprises information in horizontal and vertical directions: x is the number of_k＝[x_k ^X x_k ^Y]^T(ii) a Based on the kinematic uniform velocity formula: x_k＝2X_k-1-X_k-2Obtaining a state transition matrixPrediction state covariance matrixWherein Q is GPS positioning accuracy;

s432, fusing GPS positioning result U of the current vehicle_PAnd laser radar positioning result U_GAnd a lateral distance D₁、D₂Establishing an observed valuez：λ₁U_P+λ₂U_G(ii) a When D is present₁＜D₂When it is takenWhen D is present₂＜D₁When it is taken

S433, predicting the position of the next moment based on a kinematics uniform speed change motion formula, and fusing a GPS positioning result U of the vehicle_PAnd lidar positioning results at λ₁U_P+λ₂U_GAs an observed value, the transverse distance between the GPS positioning result and the laser radar positioning result to the path planning track is used as the constraint in the Kalman filter, and the fused positioning result [ x ] is obtained by a Kalman filtering formula_p ^X x_p ^Y]And the fused positioning result is a result in a UTM coordinate system.

Background

In recent years, with the increasing degree of automobile intelligence, the positioning problem and the route planning problem of the vehicle become hot spots for research by all parties. The currently predominant vehicle positioning methods are mainly GPS-based, vision-based and laser-based positioning. The current vehicle positioning technology based on the GPS is mature day by day, however, the positioning error is about 10 meters, the positioning accuracy is low, and in some places where the GPS signal is weak or cannot be received, the positioning accuracy is difficult to be ensured, and the simple GPS cannot meet the positioning requirement of the vehicle. Along with the wide application of laser radar sensors, the vehicle positioning technology based on laser is rapidly developed, and the laser radar has the advantages of accurate measurement, capability of providing angle and distance information accurately, capability of achieving the angle precision of <1 degree and the distance measurement precision of centimeter level, wide scanning range and the problem of positioning deviation caused by accumulated errors.

Disclosure of Invention

Aiming at the defects of the prior art, the invention provides a unified modeling method for vehicle positioning and path planning, and the positioning accuracy is improved.

The technical scheme adopted by the invention is as follows:

a unified modeling method for vehicle positioning and path planning comprises the following steps:

s1, processing the laser point cloud data to obtain a two-dimensional grid map;

s2, mapping vehicle track information and a two-dimensional grid map in a laser coordinate system to a GPS coordinate system through calibrating the mapping relation between the point cloud and the GPS data;

s3, determining a path planning track result in the two-dimensional grid map based on the A star algorithm;

and S4, taking the path planned track as a constraint track, calculating the transverse distance between the GPS positioning result and the laser radar positioning result to the constraint track as the constraint in the Kalman filter, and fusing the GPS positioning result and the laser positioning result.

Preferably, step S1 includes:

s11, preprocessing the laser point cloud data, eliminating noise points and outliers, simultaneously filtering road surface point cloud information in a scene, and only keeping inherent static obstacle information except the road surface information in the scene;

and S12, carrying out XOY plane projection processing on the preprocessed three-dimensional laser point cloud data, then uniformly dividing the whole XOY plane into grids with a certain size, and establishing that the grids with point clouds are in an occupied state and the grids without point clouds are in a free state to construct a two-dimensional grid map.

Preferably, step S11 includes:

s111, retaining point cloud data within a certain distance from the vehicle by adopting a straight-through filter, namely: if the coordinate (x) of a certain point in the laser point cloud data_i，y_i，z_i) Satisfies the following conditions: x is the number of_i∈(X₁,X₂)∩y_i∈(Y₁,Y₂)∩z_i∈(Z₁,Z₂) Then the point is retained; wherein, X_i、Y_iAnd Z_iRespectively setting threshold values of a laser point cloud coordinate system along X, Y and Z-axis directions;

s112, filtering and downsampling the laser point cloud data by adopting a voxel grid filter, dividing the point cloud data into a plurality of voxel grids, calculating the gravity center of each voxel grid, and representing all points in the voxel grids by using gravity center points;

s113, taking the points with the z coordinate smaller than a certain height as a candidate ground point set P', and sequencing the points according to the height z from small to large to obtain the average height z_aveRecalculated height less than z_ave+z_thresholdNormal to the point of (1), Z_thresholdRepresenting a preset threshold value, reserving points with an included angle with the z axis larger than a preset angle, and taking all the remaining points as a final ground point set P_groundThereby eliminating the final ground point set P_ground。

Preferably, step S2 includes:

s21, selecting a marker G in the laser point cloud scanning range;

s22, collecting GPS coordinate position information (Gx, Gy, Gz) of marker G^T；

S23, extracting the laser coordinate position (x, y, z) of the marker G in the laser coordinate system^T；

And S24, obtaining a rotation matrix R and a translation matrix T according to the least square problem.

Preferably, step S23 includes:

s231, removing outliers and noise points in the laser point cloud;

s232, fitting by using RANSAC algorithm to obtain a point set of the vertical rod G of the marker, and calculating a density formula of the point set to obtain a centroid coordinate position (x, y, z) of the vertical rod^TThe coordinate position is taken as the laser coordinate position (x, y, z) of the vertical rod G in the laser coordinate system^T。

Preferably, step S24 includes:

s241, the following relation exists between the coordinates of the markers:

in the formula, R is a rotation matrix from a laser coordinate system to a GPS coordinate system, and T is a translation matrix from the laser coordinate system to the GPS coordinate system;

s242, performing a process of approximating 0 to the coordinate in the Z direction, where the above equation is converted as follows:

wherein [ x, y [ ]]^TIs the coordinate position of the marker G in the GPS coordinate system, [ Gx, Gy]^TIs the coordinate position of the marker G under the laser coordinate system;

s243, the expression after the approximation processing may be:

conversion to equation set form:

and then becomes a matrix form:

i.e. can be converted to the least squares problem: ax ═ b, and solving to obtain the solution x ═ a of the equation^TA)^-1A^TAnd b, solving the rotation matrix R and the translation matrix T.

Preferably, step S3 includes:

s31, setting a starting point and an end point of a path in the two-dimensional grid map, and determining the position information of the starting point and the end point in the two-dimensional grid map;

s32, selecting the optimal direction to expand and search according to the heuristic function of the A-star algorithm;

s33, calculating the Euclidean distance:

in the formula (x)_n,y_n) Coordinates representing the current node, (x)_d,y_d) Coordinates representing a target node;

and S34, searching by combining the A-algorithm through using the OPEN list and the CLOSED list, and generating a path planning track result from the starting point to the end point in the grid map.

Preferably, step S4 includes:

s41, recording the current vehicle position acquisition point as a point P, and acquiring the GPS position information [ x, y ] of the vehicle position acquisition point P in real time]^TAnd convert it to UTM coordinate system: u shape_P＝(x_P,y_P)＝trans(x,y)；(x_P,y_P) For the GPS position information of the position acquisition point P in the UTM coordinate system, trans () is a conversion relation for converting the GPS coordinate system into the UTM coordinate system;

meanwhile, according to the rotation matrix R, the translation matrix T and the transform relation trans () from the GPS coordinate system to the UTM coordinate system, the position [ Gx, Gy ] of the vehicle position acquisition point P from the laser coordinate system is obtained]^TConversion to position under UTM coordinate system: u shape_G＝(x_G,y_G)＝trans(R*[Gx,Gy]^T+T)；(x_G,y_G) Acquiring laser radar position information of a position acquisition point P in a UTM coordinate system;

converting the path planning track trace into a UTM coordinate system according to the transformation relation;

s42, respectively calculating the transverse distance between the GPS positioning result and the laser radar positioning result to the planned track;

and S43, taking the path planned track as a constraint track, taking the transverse distance between the GPS positioning result and the laser radar positioning result to the constraint track as the constraint in the Kalman filter, and fusing the GPS positioning result and the laser positioning result.

Preferably, step S42 includes:

s421, the path planning track is composed of straight line segments, and a straight line equation of the path planning track is calculated by a two-point straight line equation: l: a_ix+b_iy+c_i＝0；

S422, acquiring GPS position information U of the point under the UTM coordinate system according to the vehicle position_P＝(x_P,y_P) And laser radar position information U_G＝(x_G,y_G) Respectively solving the transverse distance:

preferably, step S43 includes:

s431, using the current position x_kAnd taking a track formed by the N historical positions as a current state: x_k＝[x_k x_k-1… x_k-N]Each position comprises information in horizontal and vertical directions: x is the number of_k＝[x_k ^X x_k ^Y]^T(ii) a Based on the kinematic leveling equation: x_k＝2X_k-1-X_k-2Obtaining a state transition matrixPrediction state covariance matrixWherein Q is GPS positioning accuracy;

s432, fusing GPS positioning result U of the current vehicle_PAnd laser radar positioning result U_GAnd a lateral distance D₁、D₂And establishing an observed value z: lambda [ alpha ]₁U_P+λ₂U_G(ii) a When D is present₁＜D₂When it is takenWhen D is present₂＜D₁When it is taken

S433, predicting the position of the next moment based on a kinematics uniform speed change motion formula, and fusing a GPS positioning result U of the vehicle_PAnd lidar positioning results at λ₁U_P+λ₂U_GAs an observed value, the transverse distance between the GPS positioning result and the laser radar positioning result to the path planning track is used as the constraint in a Kalman filter, and a Kalman filtering formula is used for obtaining a fused positioning result [ x [ ]_p ^X x_p ^Y]And the fused positioning result is a result in a UTM coordinate system.

The invention has the beneficial effects that: the invention carries out unified modeling for positioning and path planning innovatively, takes the path planning track as a constraint, calculates the transverse distance between the GPS positioning result and the laser radar positioning result to the constraint track as the constraint in a Kalman filter, integrates the GPS positioning result and the laser positioning result, and corrects the GPS positioning result by utilizing the laser positioning data to obtain the accurate positioning information of the vehicle. Compared with the prior art, the method and the system have the advantages that the positioning and the path planning are correlated, the modeling is unified, the GPS positioning result and the laser radar positioning result are fused, the problems that the GPS is inaccurate in the positioning result of the area with poor signals and the laser radar positioning has accumulated errors are solved, and the accurate positioning precision can be provided for vehicle tracking navigation.

Drawings

Fig. 1 is a schematic diagram of an in-vehicle device according to an embodiment of the present invention.

Fig. 2 is a schematic diagram of coordinate calibration according to an embodiment of the present invention.

Fig. 3 is a flowchart of a unified modeling method for vehicle positioning and path planning according to an embodiment of the present invention.

In the figure: the system comprises a vehicle 1, a power supply 2, an industrial personal computer 3, a laser radar point cloud acquisition device 4, a GPS receiver 5, a display 6 and a differential GPS base station 7.

Detailed Description

The invention will be further described with reference to the accompanying drawings in which:

the invention provides a unified modeling method for vehicle positioning and path planning, which comprises the steps of firstly, carrying out relevant filtering processing and overlooking projection on three-dimensional point cloud data acquired by a laser radar to obtain a two-dimensional grid map; then mapping the track information and the grid map in the laser coordinate system to the GPS coordinate system through the mapping relation between the local coordinates of the calibration point cloud and the GPS data; and finally, determining a path planning track result in the two-dimensional grid map based on an A-star algorithm, taking the path planning track as a constraint track, calculating the transverse distance between the GPS positioning result and the constraint track and taking the transverse distance as the constraint in a Kalman filter, fusing the GPS positioning result and the laser positioning result, and correcting the GPS positioning result by using the laser positioning data to obtain the accurate positioning information of the vehicle under a global coordinate system. The method utilizes a Kalman filter to fuse results among the GPS, the laser radar and the constraint track, solves the problems that the GPS is inaccurate in the positioning result of a signal-poor area and the laser radar has accumulated errors in positioning, and can provide accurate positioning precision for the tracking navigation of the vehicle.

The unified modeling method for vehicle positioning and path planning of the embodiment of the invention comprises three stages of map construction, coordinate calibration and vehicle positioning as shown in FIG. 3, wherein the vehicle positioning comprises three parts of GPS positioning, laser radar positioning and fusion positioning. In order to implement the method of the present invention, the required hardware is shown in fig. 1, which includes:

the data acquisition module specifically comprises a vehicle 1, laser radar point cloud acquisition equipment 4 on the vehicle 1 and a GPS signal receiver 5; the laser radar point cloud acquisition equipment is used for acquiring point cloud information of a vehicle at the current moment, and the point cloud comprises a marker-upright rod used in a calibration process. The GPS signal receiver is used for receiving signals sent by the differential GPS base station 7 and realizing the positioning of the current vehicle in a GPS coordinate system.

And the data transmission module transmits the point cloud information acquired by the laser radar and the message information received by the GPS receiver to the industrial personal computer 3.

And the data processing module is used for synchronously processing the point cloud information and the coordinate information in the industrial personal computer 3, mapping the acquired point cloud information in advance, constructing a grid map, planning a path track and completing fusion of positioning results.

In addition, the power supply 2 supplies power to the equipment, and the display 6 displays the planned path and the current positioning result in real time.

In the embodiment of the invention, the unified modeling method for vehicle positioning and path planning specifically comprises the following steps:

and S1, processing the laser point cloud data to obtain a two-dimensional grid map.

S11, laser point cloud preprocessing, wherein in the preprocessing, noise points and outliers need to be removed, meanwhile, road point cloud information in a scene needs to be filtered, and only inherent static obstacle information except the road information in the scene, such as buildings, trees and the like, is reserved.

Removing point cloud data far away from the vehicle by using a straight-through filter, assuming the coordinate of a certain point in the original laser point cloud, and if the coordinate meets the following conditions: x is the number of_i∈(X₁,X₂)∩y_i∈(Y₁,Y₂)∩z_i∈(Z₁,Z₂) If not, the point is eliminated. Wherein, X_i、Y_iAnd Z_iAnd the threshold values are respectively set along the X, Y direction and the Z-axis direction of the laser point cloud coordinate system.

Adopting a voxel grid filter to carry out filtering and downsampling processing on laser original point cloud data, obtaining the laser original point cloud data through point cloud acquisition, carrying out meshing processing on the original point cloud data, dividing the point cloud data into voxel grids of 5cm multiplied by 5cm, and then representing all points in each voxel grid by using gravity center points through calculating the gravity center of each voxel grid.

Detecting a ground point set P by using a condition screening method_ground. Using the points with z smaller than a certain height as a candidate ground point set P', then sorting the points according to the height z from small to large to calculate the average height z_aveRecalculated height less than z_ave+z_thresholdNormal to the point of (1), Z_thresholdRepresenting a preset threshold value, reserving points with an included angle of more than 10 degrees with the z axis, and taking all the remaining points as a final ground point set P_groundRemoving the final ground point set P_ground。

And S12, carrying out XOY plane projection processing on the preprocessed three-dimensional laser point cloud data. And then, uniformly dividing the whole XOY plane into grids with a certain size, manually setting the grid size according to the actual situation, defining the grids with point clouds as an occupied state '1', and defining the grids without the point clouds as a free state '0', and constructing a two-dimensional grid map.

And S2, mapping the track information and the grid map in the laser coordinate system to a GPS coordinate system through the mapping relation between the calibration point cloud and the RTK/INS data.

S21, in order to calibrate the mapping relationship between the point cloud and the RTK/INS data, a marker is selected in the scanning range of the laser point cloud, as shown in FIG. 2, the upright rod is selected in the embodiment.

S22, collecting the GPS coordinate position information (Gx, Gy, Gz) of the pole by using the GPS receiver, the combined inertial navigation system and the RTK base station^T。

S23, extracting the laser coordinate position (x, y, z) of the vertical rod G in the laser coordinate system^T。

First, in obtainingIn the obtained laser scanning data, laser original point cloud data is obtained through point cloud acquisition, and outliers and noise points in the laser point cloud are removed. Finally, the position of the marker stalk G is robustly estimated using the ransac (random Sample consensus) algorithm. The vertical rod G can be approximately regarded as a thin cylinder, and the cylindrical equation can be expressed as:the point set of the vertical rod G of the marker is obtained by fitting through the RANSAC algorithm, and the centroid coordinate position (x, y, z) of the cylinder can be obtained by calculating the density formula f (x, y)^TThe coordinate position is taken as the laser coordinate position (x, y, z) of the vertical rod G in the laser coordinate system^T。

And S24, obtaining a rotation matrix R and a translation matrix T according to the least square problem.

Obtaining coordinates (Gx, Gy, Gz) of the marker vertical rod G in the GPS coordinate system^TThen, in different coordinate systems, the following relationship exists between the coordinates of the uprights:wherein R is a rotation matrix from the laser coordinate system to the GPS coordinate system, and T is a translation matrix from the laser coordinate system to the GPS coordinate system.

Considering that the vehicle runs close to the ground during normal running and the vertical rod is close to the ground, the coordinate of the Z direction can be processed to be approximately 0, so the above formula can be converted into the following formula:[x，y]^Tis the coordinate position of the vertical rod G in the GPS coordinate system, [ Gx, Gy]^TIs the coordinate position of the vertical rod G under the laser coordinate system. In the two-dimensional case, the rotation matrix is composed of two unknowns, i.e.The translation vector may be represented asThe above equation may then be changed to:conversion to equation set form:can be changed into a matrix form:i.e. can be converted to the least squares problem: ax ═ b, and solving to obtain the solution x ═ a of the equation^TA)^-1A^Tb. The rotation matrix R and the translation matrix T can be solved.

And S3, determining a track result of the path planning in the two-dimensional grid map based on the A star algorithm.

S31 sets the start point and the end point of the route on the grid map, and specifies the position information of the start point and the end point on the grid map.

And S32, selecting the optimal direction to expand and search according to the heuristic function. The heuristic function expression of the A-algorithm is as follows: (n) g (n) + h (n), where f (n) represents the total cost function from the initial node to the target node at the current node n; g (n) represents the actual cost value from the initial node to the current node n; h (n) represents the magnitude of the minimum estimated cost value to reach the target node from the current node n.

S33, calculating the Euclidean distanceWherein (x)_n,y_n) Coordinates representing a current node; (x)_d,y_d) Representing the coordinates of the target node.

And S34, searching by combining the A-algorithm through using the OPEN list and the CLOSED list, and generating a path planning track result from the starting point to the end point in the grid map.

And S4, taking the path planned track as a constraint track, calculating the transverse distance between the GPS positioning result and the constraint track as the constraint in the Kalman filter, and fusing the GPS positioning result and the laser positioning result.

And S41, coordinate transformation. To facilitate the calculation of the lateral distance, the GPS coordinate system and the lidar coordinate system are converted to the utm (universal Transverse mercator) coordinate system, i.e., the universal lateral-axis mercator projection system. The position of the current vehicle test acquisition point is marked as a point P, and GPS position information [ x, y ] of the vehicle test point P is acquired in real time by using GPS acquisition equipment]^TConvert it to the bottom U of UTM coordinate system_P＝(x_P,y_P)＝trans(x,y),(x_P,y_P) For testing the UTM coordinate position information of the acquisition point P, trans () is a conversion relation for converting the GPS coordinate system into the UTM coordinate system. Meanwhile, according to the rotation matrix R, the translation matrix T and the transfer relation trans () from the GPS coordinate system to the UTM coordinate system, the position [ Gx, Gy ] of the vehicle test acquisition point P from the laser coordinate system can be obtained]^TConversion to position U in UTM coordinate system_G＝(x_G,y_G)＝trans(R*[Gx,Gy]^T+ T). And simultaneously converting the path planning track into a UTM coordinate system according to the transformation relation.

And S42, calculating the transverse distance between the positioning result and the planned track through GPS positioning and laser radar positioning. The path planning track is composed of straight line segments, and a linear equation l: a of the path planning track can be calculated by a two-point linear equation_ix+b_iy+c_i0, according to the GPS position information U of the current time of the vehicle acquisition point in the last step under the UTM coordinate system_P＝(x_P,y_P) And laser radar position information U_G＝(x_G,y_G) The transverse distance can be solved

And S43, taking the path planned track as a constraint track, taking the transverse distance between the GPS positioning result and the constraint track from the laser positioning result in the last step as the constraint in a Kalman filter, and fusing the GPS positioning result and the laser positioning result. The Kalman filter fusion positioning specifically comprises the following steps:

with the current location and the N historical locations,a trajectory consisting of N +1 positions as the current state (in this example, N is taken to be 2): x_k＝[x_k x_k-1 … x_k-N]Each position comprises information in horizontal and vertical directions: x is the number of_k＝[x_k ^X x_k ^Y]^TBased on the kinematic uniform velocity formula: x_k＝2X_k-1-X_k-2Thereby obtaining a state transition matrixPrediction state covariance matrixWherein Q is the GPS positioning accuracy. Here, Q is 10.

The result U of the current vehicle under the GPS positioning_PAnd result U under laser positioning_GAnd a lateral distance D₁、D₂Lambda of a handle₁U_P+λ₂U_GAs observed value z, when D₁＜D₂When it is takenWhen D is present₂＜D₁When it is takenTo solve the observation matrix H, an observation value z ═ H [ x ] is determined from the relationship_k x_k-1x_k-2]^T(ii) a Wherein H is an observation matrix, and is obtained by solvingObserving a noise covariance matrixWherein delta₁ ²，δ₂ ²，...，δ_n ²The variance of each measurement error data for a plurality of measurements.

According to the Kalman filtering formula:

wherein, X_kThe method is a result of fusing the current vehicle position after positioning, the error precision can reach centimeter level, and the current vehicle positioning requirement is met.

The prediction data obtained based on the kinematics uniform variable speed motion formula and the result of the vehicle under the GPS positioning and the result lambda under the laser positioning are fused in the steps₁U_P+λ₂U_GAs an observation value, the transverse distance between the GPS positioning result and the laser positioning result to the path planning track is used as the constraint in the Kalman filter, and the fused positioning result [ x ] is obtained by the Kalman filtering formula_p ^X x_p ^Y]And the fusion result is a result in a UTM coordinate system, and then the relationship is converted according to the coordinates: (x, y) ═ trans^-1(x_p ^X,x_p ^Y) And obtaining a precise fusion positioning result under the global coordinate system. The method solves the problems that the GPS is inaccurate in the positioning result of the area with poor signals and the laser radar positioning has accumulated errors, has simple equipment and high robustness of the positioning result, can provide accurate positioning precision for vehicle tracking navigation, is suitable for the navigation positioning of the tracking vehicle in a complex environment, and is particularly suitable for the positioning of the intelligent vehicle in unmanned driving.

It will be understood by those skilled in the art that the foregoing is merely a preferred embodiment of the present invention, and is not intended to limit the invention, and that any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included within the scope of the present invention.