CN110146910B - Positioning method and device based on data fusion of GPS and laser radar - Google Patents

Abstract

The invention discloses a positioning method based on data fusion of a GPS and a laser radar, which comprises the following steps: acquiring a GPS positioning coordinate; acquiring road environment information, and establishing a road environment characteristic model; dividing the map into grid maps according to a certain resolution ratio according to the GPS error, and determining a key grid area according to the GPS positioning coordinate; matching the road environment characteristic model with road key point characteristic parameters in the key grid area; calculating to obtain longitude and latitude coordinates of the vehicle according to the longitude and latitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the laser radar; and performing data fusion on the GPS positioning coordinate, the vehicle longitude and latitude coordinate, the vehicle speed and the course angle by adopting an extended Kalman filter to obtain a final positioning result. The invention combines the advantages of GPS in global positioning and the advantages of laser radar in local positioning, and greatly improves the positioning accuracy of the GPS positioning system.

Description

A positioning method and device based on GPS and laser radar data fusion

technical field

The invention belongs to the technical field of positioning, and in particular relates to a positioning method and device based on fusion of GPS and laser radar data.

Background technique

Vehicle localization is a key issue in research in the field of autonomous vehicles and plays an extremely important role. A high-precision positioning effect not only makes the trajectory following control of autonomous vehicles easier, but also enables vehicle-to-vehicle (V2V), vehicle-to-roadside infrastructure (V2I) and vehicle-to-city network interconnection based on location sharing , is the only way to realize intelligent transportation.

In recent years, navigation and positioning technology has continued to develop. According to the different sensor devices used, it can be divided into positioning technology based on magnetic sensor arrays, dead reckoning (DR), inertial navigation (Inertial navigation), satellite positioning, visual positioning, based on Laser radar positioning technology, etc. In the process of implementing a specific system, various positioning technologies can be used independently of each other, or can be used in combination with each other.

According to the research and application status of vehicle positioning technology, there are still many deficiencies in the existing positioning methods. For the field of single-sensor positioning, GPS positioning is convenient to apply, but there are large errors in its measurement principle; although differential GPS positioning can improve positioning accuracy, it requires a reference station within a certain range, which is difficult to popularize; positioning methods based on inertial navigation systems exist The cumulative error cannot be ignored; although the navigation method based on the magnetic sensor array is more accurate, it needs to be laid in advance, and the flexibility is extremely poor. Among the existing multi-sensor fusion positioning methods, the fusion positioning method of GPS and wireless local area network or 4G network can effectively solve the problem of indoor GPS signal occlusion and realize seamless positioning, but the improvement of positioning accuracy is very limited; others such as GPS Although the fusion positioning method with inertial navigation system, GPS and visual sensing can improve the positioning accuracy to a certain extent, it still cannot meet the positioning requirements of future self-driving cars.

Contents of the invention

In view of the shortcomings of the prior art described above, the present invention provides a positioning method and device based on fusion of GPS and laser radar data to solve the defect that the accuracy of the existing positioning technology is not high enough.

In order to achieve the above purpose and other related purposes, the present invention provides a positioning method based on fusion of GPS and laser radar data. The positioning method includes:

Obtain GPS positioning coordinates;

Obtain road environment information and establish a road environment characteristic model;

According to the GPS error, the map is divided into a grid map according to a certain resolution, and the key grid area is determined according to the GPS positioning coordinates;

Matching the road environment feature model with the road key point feature parameters in the key grid area;

According to the longitude and latitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the lidar, the longitude and latitude coordinates of the vehicle are calculated;

进行数据融合，得到最终的定位结果。Using an extended Kalman filter to perform the GPS positioning coordinates, the latitude and longitude coordinates of the vehicle, the vehicle speed v and the heading angle

Perform data fusion to obtain the final positioning result.

Optionally, the 2D lidar is used for detection, the road environment information is obtained, and the road environment characteristic model is established, which specifically includes:

Obtain 2D lidar data, take the location of the lidar device as the coordinate origin, and the environmental data as point cluster data;

Preprocessing the point cluster data;

The split-merge algorithm is used to cluster and model the preprocessed point cluster data.

Optionally, the preprocessing of the point cluster data specifically includes:

Perform median filtering on the point cluster data to obtain filtered point cluster data;

Error compensation for lidar data;

Taking the center of the vehicle as the origin, the horizontal axis as the horizontal direction of the vehicle, and the vertical axis as the straight direction of the vehicle, the lidar data in polar coordinates is converted into rectangular coordinates.

Optionally, matching the road environment feature model with the road key point feature parameters in the key grid area specifically includes:

Taking the key point {X _j , Y _j } in the road environment model as the origin, extract the characteristic angle θ and distance d in the road environment feature model clockwise to obtain the feature set {θ ₁ ,θ ₂ ,θ ₃ ,d ₁ , θ ₄ ,d ₂ ...};

Compare the feature set {θ ₁ , θ ₂ , θ ₃ , d ₁ , θ ₄ , d ₂ ...} with the road key point information in the key grid area in sequence, and match within a certain threshold range If successful, it is considered that the key points of the road are successfully matched.

Optionally, the latitude and longitude coordinates (long _L , lat _L ) of the vehicle are expressed as:

Among them, longk represents the longitude coordinates of key points on the road, latk represents the latitude coordinates of key points on the road, d is the Euclidean distance between the key point and the lidar; α is the azimuth of the vehicle, and ARC is the average radius of the earth.

进行数据融合，得到最终的定位结果，其中，扩展卡尔曼滤波器的状态方程和观测方程如下：Optionally, the GPS positioning coordinates, the vehicle latitude and longitude coordinates, the vehicle speed v and the heading angle are analyzed using an extended Kalman filter

Carry out data fusion to obtain the final positioning result, among which, the state equation and observation equation of the extended Kalman filter are as follows:

Where: f() is the state transition function of the nonlinear system, H() is the observation function of the nonlinear system, w(k) and v(k) are system noises; U(k) is the input value, X(k) is State variable, Z(k) is the observed value;

Then the positioning system model is as follows:

In the formula: t is the sampling interval time, x _k , y _k are the longitude and latitude of the desired location respectively.

In order to achieve the above purpose and other related purposes, the present invention also provides a positioning device based on fusion of GPS and laser radar data, which includes:

A coordinate acquisition module, configured to acquire GPS positioning coordinates;

The model building module is used to obtain road environment information and establish a road environment characteristic model;

The feature extraction module is used to divide the map into a grid map according to a certain resolution according to the GPS error, and determine the key grid area according to the GPS positioning coordinates;

A feature matching module, configured to match the road environment feature model with the road key point feature parameters in the key grid area;

The coordinate calculation module is used to calculate the longitude and latitude coordinates of the vehicle according to the latitude and longitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the laser radar;

进行数据融合，得到最终的定位结果。The data fusion module is used to adopt the extended Kalman filter to the GPS positioning coordinates, the vehicle latitude and longitude coordinates, vehicle speed v and heading angle

Perform data fusion to obtain the final positioning result.

To achieve the above object and other related objects, the present invention also provides a readable storage medium storing a computer program, and the computer program executes the positioning method when run by a processor.

To achieve the above object and other related objects, the present invention also provides an electronic terminal, including: a processor and a memory;

The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the terminal executes the positioning method.

As mentioned above, a positioning method and device based on the fusion of GPS and laser radar data of the present invention has the following beneficial effects:

A positioning method based on GPS and laser radar data fusion in the present invention combines the advantages of GPS in global positioning and laser radar in local positioning, uses GPS to perform rough positioning to determine the approximate area, and then uses laser radar to analyze the surrounding environment Carry out modeling, extract features, match with historical environmental information, obtain local accurate positioning, and finally fuse data from various sensors to obtain the final positioning result, which greatly improves the positioning accuracy of the GPS positioning system.

Description of drawings

Fig. 1 is a flow chart of a positioning method based on fusion of GPS and laser radar data according to an embodiment of the present invention;

Fig. 2 is a geometrical schematic diagram of laser radar data correction according to an embodiment of the present invention;

3 is a schematic diagram of a split-merge clustering algorithm for laser radar data according to an embodiment of the present invention;

Fig. 4 is a road environment feature model diagram of an embodiment of the present invention;

Fig. 5 is a schematic diagram of feature extraction according to an embodiment of the present invention;

Fig. 6 is a schematic diagram of latitude and longitude calculation according to an embodiment of the present invention;

Fig. 7 is the EKF-based data fusion positioning of the embodiment of the present invention;

Fig. 8 shows three stages of data fusion in the embodiment of the present invention.

Detailed ways

Embodiments of the present invention are described below through specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific implementation modes, and various modifications or changes can be made to the details in this specification based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that, in the case of no conflict, the following embodiments and features in the embodiments can be combined with each other.

It should be noted that the diagrams provided in the following embodiments are only schematically illustrating the basic ideas of the present invention, and only the components related to the present invention are shown in the diagrams rather than the number, shape and shape of the components in actual implementation. Dimensional drawing, the type, quantity and proportion of each component can be changed arbitrarily during actual implementation, and the component layout type may also be more complicated.

As shown in Figure 1, a positioning method based on GPS and lidar data fusion in this embodiment improves the positioning accuracy of GPS, including the following steps:

Step S1, obtaining GPS positioning coordinates;

Specifically, the original GPS data is converted from the WGS-84 coordinate system to the BD-09 coordinate system data (long _G , lat _G ), where long _G is the longitude and lat _G is the latitude, then the GPS positioning coordinates are expressed as (long _G , lat _G ).

Step S2, acquiring road environment information, and establishing a road environment feature model; specifically, using 2D laser radar for detection to obtain road environment information.

More specifically, step S2 includes the following sub-steps:

Step S21: Obtain 2D lidar data, take the location of the lidar device as the coordinate origin, and the lidar data is point cluster data {(L _i ,θ _i )|i=1,2,3,...,n}, where The point cluster data is in the form of polar coordinates, θ _i represents the angle, and _Li represents the distance.

Step S22: Preprocessing the lidar point cluster data, the preprocessing steps include:

Step S221: Perform median filtering on the point cluster data to obtain filtered point cluster data {(L _i ,θ _i )|j=1,2,3,...,m,m<n}, the algorithm is as follows:

fori=1to n;

j = 1;

L _j = mid(L _i ,L _i+1 ,L _i+2 );

j++;

Step S222: The lidar scans a frame of data for about 100ms. During this 100ms, when the vehicle is moving and the origin changes during the laser beam scanning process, it is necessary to make error compensation for the lidar data, as shown in Figure 2. The formula is as follows :

for j＝1to m

In the formula: T is the laser radar scanning period; γ is the angular resolution of the laser radar scanning; v is the vehicle speed; S _t is the measured distance with the serial number i in the current laser radar data; S _(t - T) is The measured distance with the sequence number i in the lidar data at the last moment; Θ _j is the angle between the measured obstacle and the abscissa axis; S _real is the corrected lidar data.

Step S223: Convert the lidar data in polar coordinates into rectangular coordinates, with the center of the vehicle as the origin, the horizontal axis as the horizontal direction of the vehicle, and the vertical axis as the straight direction of the vehicle.

In the formula: l is the longitudinal length of the vehicle.

Step S23: Use the split-merge algorithm to cluster and model the lidar data; the fitting method uses the least square method, as shown in Figure 3, the algorithm is as follows:

Among them, the IEPF (Iterative End Point Fit) method is used for line segment feature extraction,

(1) Take out the lidar point cluster data and put it into ListA;

(2) Take the first and last two points in A and fit them into a straight line l ₁ ;

(3) Search for the point farthest from the straight line l ₊₁ in ListA to obtain the distance d;

(4) If d is less than the segmentation threshold d _max , then perform step (6);

(5) Otherwise, divide l ₁ into l ₂ and l ₃ , then perform step (2);

(6) Take all the points between the segments, and use the least squares method to fit them. The least squares fitting formula is as follows:

In the formula:

为数据算术平均值；

is the arithmetic mean of the data;

为参数β₀,β₁的最小二乘估计值.β ₀ , β ₁ are regression coefficients,

is the least squares estimate of parameters β ₀ , β ₁ .

After step (6), the set of line segments {l ₁ , l ₂ , l ₃ ...... l _n } is obtained, and the endpoints of the line segments are determined, as shown in Figure 4. The final road environment characteristic model is obtained as follows:

为线段端点，(x_j,y_j)为线段交点,in,

is the end point of the line segment, (x _j ,y _j ) is the intersection point of the line segment,

Step S3: The maximum GPS error is about 30m. According to the GPS error, the map is divided into a grid map with a certain resolution, and the key grid area is determined according to the GPS positioning coordinates (long _G , lat _G ) obtained in step S1.

Step S4: Use the road environment feature model established in step S2 to match the road key point information in the key grid area.

Step S41: Taking the key point {X _j , Y _j } in the road environment feature model as the origin, extract the available feature angle θ and distance d in the road environment feature model in a clockwise direction to obtain {θ ₁ , θ ₂ , θ ₃ , d ₁ ,θ ₄ ,d ₂ ...}, as shown in Figure 5, the formula is as follows:

In the formula: k is the slope of the straight line; d is the distance from the origin to the straight line; θ is the angle between the perpendicular lines drawn from the origin.

Step S42: Get the feature set {θ ₁ , θ ₂ , θ ₃ , d ₁ , θ ₄ , d ₂ ...} after extracting the above features, and compare them with the key point feature parameters of the road recorded in the key grid area, If the matching of key points is successful within a certain threshold range, it is considered that the key point matching is successful, otherwise, steps S2, S3, and S4 are repeated.

Step S5: Calculate the longitude and latitude positioning coordinates (long _L , lat _L ) of the vehicle according to the latitude and longitude coordinates (long _k , lat _k ) of the key points on the road that have been successfully matched and the coordinates (x _j , y _j ) of the key points for the lidar, as shown in the figure 6, the formula is as follows:

In the formula: d is the Euclidean distance between the key point and the lidar; the azimuth angle of the vehicle at α position is 0° in the direction of true north; ARC is the average radius of the earth, which is about 6371.393 kilometers; (long _k ,lat _k ) is The latitude and longitude coordinates of key points; (long _L , lat _L ) is the obtained vehicle latitude and longitude coordinates.

采用扩展卡尔曼滤波器(EKF)对以上数据进行融合，获得最终的定位结果。如图7所示。EKF的状态方程和观测方程如下：Step S6: Obtain the latitude and longitude coordinates (long _G , lat _G ) obtained by GPS through the above steps, and the latitude and longitude coordinates (long _L , lat _L ) obtained by laser radar, combined with the velocity v and heading angle directly obtained by the vehicle

The extended Kalman filter (EKF) is used to fuse the above data to obtain the final positioning result. As shown in Figure 7. The state equation and observation equation of EKF are as follows:

In the formula: f() is the state transition function of the nonlinear system; H() is the observation function of the nonlinear system; w(k) and v(k) are the system noise; U(k) is the input value, X(k) is The state variable, Z(k) is the observed value.

The combined positioning system model of the present invention is as follows:

In the formula: v is the vehicle speed; t is the sampling interval time; θ is the heading and azimuth angle of the vehicle, taking true north as 0°; ARC is the average radius of the earth; w( _{k )} is the system noise; Find the location longitude and latitude.

The specific calculation formula based on the extended Kalman filter data fusion is as follows:

X(k)＝X(k|k-1)+K(k)×(Z( ^k )-H(X(k|k ^-1 )))

P(k|k)＝P(k|k-1)-K(k)P(k|k-1)

In the formula: A _j is the Jacobian matrix of the function f, H _j is the Jacobian matrix of the function H; f is the state transition function of the nonlinear system, H is the observation function of the nonlinear system; v(k) is the system noise; P is the state Variable covariance matrix; Q is process noise covariance matrix; R is observation noise covariance matrix; K() is Kalman gain; v is vehicle speed; t is sampling interval time; is 0°; ARC is the average radius of the earth.

The present invention divides the data fusion process into three stages, as shown in FIG. 8 .

Phase 1: Before the key points are detected, the lidar has not detected the key points of the road features and cannot be matched with the map at this stage, so the source of the latitude and longitude coordinate data only comes from GPS data. The observations at this stage are the latitude and longitude coordinates (long _G , lat _G ) obtained by GPS, and the specific parameters of EKF are as follows:

Stage 2: Key points are detected. At this stage, lidar can detect key points of road features. Through the precise positioning method based on GPS and lidar in this paper, relatively accurate latitude and longitude coordinates (long _L , lat _L ) can be obtained. As the observed value, the specific parameters of EKF are as follows:

Stage 3: Leaving the key point. Although the key point of the road is lost in this stage, the optimal estimated coordinates at the previous moment have a certain correction effect on the data of the subsequent period of time. The GPS data (long _G , lat _G ) is used as the observation value , EKF specific parameters are as follows:

The present invention is a positioning method based on the fusion of GPS and laser radar data. GPS is used for global positioning to determine a rough area, and then laser radar is used for local precise positioning. Finally, the two data are fused to obtain a positioning result, which can effectively improve GPS positioning. The positioning accuracy of the system, and GPS and lidar are essential sensors for self-driving cars, no additional equipment is required. Therefore, a positioning method based on fusion of GPS and laser radar data in the present invention has high practicability.

This embodiment also provides a positioning device based on fusion of GPS and laser radar data, the device comprising:

A coordinate acquisition module, configured to acquire GPS positioning coordinates;

The model building module is used to obtain road environment information and establish a road environment characteristic model;

The feature extraction module is used to divide the map into a grid map according to a certain resolution according to the GPS error, and determine the key grid area according to the GPS positioning coordinates;

A feature matching module, configured to match the road environment feature model with road key point information in the key grid area;

The coordinate calculation module is used to calculate the longitude and latitude coordinates of the vehicle according to the latitude and longitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the laser radar;

进行数据融合，得到最终的定位结果。The data fusion module is used to adopt the extended Kalman filter to the GPS positioning coordinates, the vehicle latitude and longitude coordinates, vehicle speed v and heading angle

Perform data fusion to obtain the final positioning result.

It should be noted that, since the embodiment of the device part corresponds to the embodiment of the method part, please refer to the description of the embodiment of the method part for the content of the embodiment of the device part, and details will not be repeated here.

The present invention also provides a storage medium storing a computer program, and the computer program executes the aforementioned positioning method when executed by a processor.

The present invention also provides an electronic terminal, including:

memory for storing computer programs;

A processor, configured to execute the computer program stored in the memory, so that the device executes the aforementioned positioning method.

The processor can be a central processing unit (Central Processing Unit, CPU), and can also be other general-purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), on-site Programmable gate array (Field Programmable Gate Array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor, or the processor may be any conventional processor, or the like.

The memory may be an internal storage unit or an external storage device, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital card (Secure Digital, SD), a flash memory card (Flash Card) and the like. Further, the memory may also include both an internal storage unit and an external storage device. The memory is used to store the computer program as well as other programs and data. The memory can also be used to temporarily store data that has been output or will be output.

Those skilled in the art can clearly understand that for the convenience and brevity of description, only the division of the above-mentioned functional units and modules is used for illustration. In practical applications, the above-mentioned functions can be assigned to different functional units, Completion of modules means that the internal structure of the device is divided into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment may be integrated into one processing unit, or each unit may exist separately physically, or two or more units may be integrated into one unit, and the above-mentioned integrated units may adopt hardware It can also be implemented in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the protection scope of the present application. For the specific working process of the units and modules in the above system, reference may be made to the corresponding process in the foregoing method embodiments, and details will not be repeated here.

In the above-mentioned embodiments, the descriptions of each embodiment have their own emphases, and for parts that are not detailed or recorded in a certain embodiment, refer to the relevant descriptions of other embodiments.

Those skilled in the art can appreciate that the units and algorithm steps of the examples described in conjunction with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are executed by hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art may use different methods to implement the described functions for each specific application, but such implementation should not be regarded as exceeding the scope of the present invention.

In the embodiments provided in the present invention, it should be understood that the disclosed apparatus/terminal equipment and method may be implemented in other ways. For example, the device/terminal device embodiments described above are only illustrative. For example, the division of the modules or units is only a logical function division. In actual implementation, there may be other division methods, such as multiple units Or components may be combined or may be integrated into another system, or some features may be omitted, or not implemented. In another point, the mutual coupling or direct coupling or communication connection shown or discussed may be through some interfaces, and the indirect coupling or communication connection of devices or units may be in electrical, mechanical or other forms.

The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place, or may be distributed to multiple network units. Part or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, each unit may exist separately physically, or two or more units may be integrated into one unit. The above-mentioned integrated units can be implemented in the form of hardware or in the form of software functional units.

If the integrated module/unit is realized in the form of a software function unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present invention realizes all or part of the processes in the methods of the above embodiments, and can also be completed by instructing related hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer When the program is executed by the processor, the steps in the above-mentioned various method embodiments can be realized. Wherein, the computer program includes computer program code, and the computer program code may be in the form of source code, object code, executable file or some intermediate form. The computer-readable medium may include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a removable hard disk, a magnetic disk, an optical disk, a computer memory, and a read-only memory (ROM, Read-Only Memory) , Random Access Memory (RAM, Random Access Memory), electrical carrier signal, telecommunication signal and software distribution medium, etc.

The above-mentioned embodiments only illustrate the principles and effects of the present invention, but are not intended to limit the present invention. Anyone skilled in the art can modify or change the above-mentioned embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or changes made by those skilled in the art without departing from the spirit and technical ideas disclosed in the present invention should still be covered by the claims of the present invention.

Claims (8)

1. A positioning method based on data fusion of a GPS and a laser radar is characterized by comprising the following steps:

acquiring a GPS positioning coordinate;

detecting by adopting a 2D laser radar, acquiring road environment information, and establishing a road environment characteristic model;

dividing the map into grid maps according to a certain resolution ratio according to the GPS error, and determining a key grid area according to the GPS positioning coordinate;

matching the road environment characteristic model with road key point characteristic parameters in the key grid area;

calculating to obtain longitude and latitude coordinates of the vehicle according to the longitude and latitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the laser radar;

adopting an extended Kalman filter to carry out positioning on the GPS positioning coordinate, the longitude and latitude coordinate of the vehicle, the vehicle speed v and the course angle

And performing data fusion to obtain a final positioning result, wherein the state equation and the observation equation of the extended Kalman filter are as follows:

in the formula: f () is the state transfer function of the nonlinear system, H () is the observation function of the nonlinear system, and w (k) and v (k) are the system noise; u (k) is an input value, X (k) is a state variable, and Z (k) is an observed value;

the positioning system model is as follows:

in the formula: t is the sampling interval time, x _k ,y _k Respectively, the longitude and latitude of the determined location.

2. The positioning method based on GPS and lidar data fusion according to claim 1,

adopt 2D laser radar to detect, acquire road environment information, establish road environment characteristic model, specifically include:

acquiring 2D laser radar data, wherein the position of a laser radar device is taken as a coordinate origin, and environmental data is taken as point cluster data;

preprocessing the point cluster data;

and clustering the preprocessed point cluster data by adopting a split-merge algorithm, and modeling.

3. The positioning method based on GPS and lidar data fusion according to claim 2,

the preprocessing the point cluster data specifically includes:

carrying out median filtering on the point cluster data to obtain filtered point cluster data;

performing error compensation on the laser radar data;

and converting the laser radar data in a polar coordinate form into rectangular coordinates by taking the center of the vehicle as an origin, the horizontal axis as the horizontal direction of the vehicle and the vertical axis as the straight direction of the vehicle.

4. The positioning method based on GPS and lidar data fusion according to claim 1,

matching the road environment feature model with the road key point feature parameters in the key grid area, specifically comprising:

by key points { X in the road environment model _j ,Y _j Taking the angle theta as the original point, and extracting the characteristic angle theta and the distance d in the road environment characteristic model in the clockwise direction to obtain a characteristic set { theta ₁ ,θ ₂ ,θ ₃ ,d ₁ ,θ ₄ ,d ₂ …}；

Set the features { theta } ₁ ,θ ₂ ,θ ₃ ,d ₁ ,θ ₄ ,d ₂ … is compared with the road key point information in the key grid area in sequence, and if matching is successful within a certain threshold value range, the matching of the road key points is considered to be successful.

5. The positioning method based on GPS and lidar data fusion according to claim 1,

the vehicle longitude and latitude coordinates (Long) _L ,lat _L ) Expressed as:

the method comprises the following steps that (1) longk represents longitude coordinates of key points of a road, latk represents latitude coordinates of the key points of the road, and d is the Euclidean distance between the key points and a laser radar; and the alpha vehicle driving azimuth angle and ARC is the average radius of the earth.

6. A positioning device based on GPS and laser radar data fusion is characterized in that the device comprises:

the coordinate acquisition module is used for acquiring a GPS positioning coordinate;

the model establishing module is used for detecting by adopting a 2D laser radar, acquiring road environment information and establishing a road environment characteristic model;

the characteristic extraction module is used for dividing the map into a grid map according to a certain resolution ratio according to the GPS error and determining a key grid area according to the GPS positioning coordinate;

the characteristic matching module is used for matching the road environment characteristic model with the road key point characteristic parameters in the key grid area;

the coordinate calculation module is used for calculating the longitude and latitude coordinates of the vehicle according to the longitude and latitude coordinates of the successfully matched road key points and the coordinates of the road key points relative to the laser radar;

a data fusion module for adopting an extended Kalman filter to carry out positioning on the GPS positioning coordinate, the longitude and latitude coordinate of the vehicle, the vehicle speed v and the course angle

And performing data fusion to obtain a final positioning result, wherein the state equation and the observation equation of the extended Kalman filter are as follows:

in the formula: f () is a state transfer function of the nonlinear system, H () is an observation function of the nonlinear system, and w (k) and v (k) are system noise; u (k) is an input value, X (k) is a state variable, and Z (k) is an observed value;

the positioning system model is as follows:

in the formula: t is the sampling interval time, x _k ,y _k Respectively, the longitude and latitude of the determined location.

7. A storage medium storing a computer program, characterized in that the computer program, when executed by a processor, performs the positioning method according to any one of claims 1 to 5.

8. An electronic terminal, comprising: a processor and a memory;

the memory is configured to store a computer program, and the processor is configured to execute the computer program stored by the memory to cause the terminal to perform the positioning method according to any one of claims 1 to 5.

Images