CN110146910B - Positioning method and device based on data fusion of GPS and laser radar - Google Patents
Positioning method and device based on data fusion of GPS and laser radar Download PDFInfo
- Publication number
- CN110146910B CN110146910B CN201910403394.8A CN201910403394A CN110146910B CN 110146910 B CN110146910 B CN 110146910B CN 201910403394 A CN201910403394 A CN 201910403394A CN 110146910 B CN110146910 B CN 110146910B
- Authority
- CN
- China
- Prior art keywords
- gps
- positioning
- road
- laser radar
- coordinate
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 230000004927 fusion Effects 0.000 title claims abstract description 36
- 238000004590 computer program Methods 0.000 claims description 16
- 230000006870 function Effects 0.000 claims description 14
- 238000004422 calculation algorithm Methods 0.000 claims description 7
- 238000007781 pre-processing Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000000605 extraction Methods 0.000 claims description 5
- 238000005070 sampling Methods 0.000 claims description 5
- 238000012546 transfer Methods 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 6
- 239000011159 matrix material Substances 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 241001134453 Lista Species 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/50—Systems of measurement based on relative movement of target
- G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
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
Technical Field
The invention belongs to the technical field of positioning, and particularly relates to a positioning method and device based on data fusion of a GPS and a laser radar.
Background
Vehicle localization is a key issue in the research of the automotive field, playing a very important role. A high-precision positioning effect not only makes the track following control of an automatic driving automobile easier, but also can realize the interconnection of the automobile and the automobile (V2V), the automobile and roadside infrastructure (V2I) and the automobile and an urban network based on position sharing, and is a necessary route for realizing intelligent traffic.
In recent years, navigation and positioning technologies have been developed, and the technologies can be classified into a positioning technology based on a magnetic sensor array, dead Reckoning (DR), inertial navigation (Inertial navigation), satellite positioning, visual positioning, a positioning technology based on a laser radar, and the like according to different sensor devices used. In the process of implementing a specific system, various positioning technologies can be used independently, or multiple technologies can be used in combination.
As known from the research and application state of the vehicle positioning technology, the existing various positioning methods still have many defects. For the field of single sensor positioning, GPS positioning is convenient to apply, but a large error exists due to the measurement principle of the GPS positioning; although the differential GPS positioning can improve the positioning accuracy, the reference station is required to be arranged in a certain range, and the popularization is difficult; the positioning method based on the inertial navigation system has a non-negligible accumulated error; although the navigation method based on the magnetic sensor array is accurate, the navigation method needs to be laid in advance, and the flexibility is very poor. In the existing multi-sensor fusion positioning method, the fusion positioning method of the GPS and the wireless local area network or the 4G network can effectively solve the problem of indoor GPS signal shielding, realize seamless positioning, but has very limited improvement on positioning accuracy; other fusion positioning methods such as a GPS and an inertial navigation system, a GPS and a visual sensing method can improve certain positioning accuracy, but cannot meet the positioning requirement of the future automatic driving automobile.
Disclosure of Invention
In view of the above disadvantages of the prior art, the present invention provides a positioning method and apparatus based on data fusion of GPS and lidar to solve the problem of insufficient accuracy of the existing positioning technology.
In order to achieve the above and other related objects, the present invention provides a positioning method based on data fusion of GPS and lidar, the positioning method comprising:
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;
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 carrying out data fusion to obtain a final positioning result.
Optionally, the detecting with the 2D laser radar obtains road environment information, and establishes a road environment characteristic model, which specifically includes:
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.
Optionally, 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.
Optionally, matching the road environment feature model with the road key point feature parameters in the key grid region specifically includes:
using key point { X in road environment model j ,Y j The feature angle theta and the distance d in the road environment feature model are extracted clockwise to obtain a feature set { theta 1 ,θ 2 ,θ 3 ,d 1 ,θ 4 ,d 2 …}；
Set the features { theta } 1 ,θ 2 ,θ 3 ,d 1 ,θ 4 ,d 2 … 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 is successfulThe road key point matching is considered to be successful.
Optionally, the vehicle latitude and longitude 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.
Optionally, an extended kalman filter is adopted to carry out positioning on the GPS positioning coordinates, the vehicle longitude and latitude coordinates, the vehicle speed v and the heading 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.
In order to achieve the above and other related objects, the present invention further provides a positioning device based on GPS and lidar data fusion, the positioning device comprising:
the coordinate acquisition module is used for acquiring a GPS positioning coordinate;
the model establishing module is used for 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 to obtain a vehicle longitude and latitude coordinate according to the successfully matched longitude and latitude coordinate of the road key point and the coordinate of the road key point relative to the laser radar;
a data fusion module for using an extended Kalman filter to determine the GPS positioning coordinates, the longitude and latitude coordinates, the vehicle speed v and the course angle
And carrying out data fusion to obtain a final positioning result.
To achieve the above and other related objects, the present invention further provides a readable storage medium storing a computer program, which when executed by a processor performs the positioning method.
To achieve the above and other related objects, the present invention also provides an electronic terminal, comprising: a processor and a memory;
the memory is used for storing computer programs, and the processor is used for executing the computer programs stored by the memory so as to enable the terminal to execute the positioning method.
As described above, the positioning method and apparatus based on data fusion of GPS and lidar of the present invention have the following beneficial effects:
the invention relates to a positioning method based on data fusion of a GPS and a laser radar, which combines the advantages of the GPS in global positioning and the advantages of the laser radar in local positioning, utilizes the GPS to perform rough positioning to determine the approximate area range, then utilizes the laser radar to perform modeling and feature extraction on the nearby environment, matches with historical environment information to obtain local accurate positioning, and finally fuses data of various sensors to obtain a final positioning result, thereby greatly improving the positioning accuracy of a GPS positioning system.
Drawings
FIG. 1 is a flow chart of a positioning method based on data fusion of GPS and lidar according to an embodiment of the present invention;
FIG. 2 is a schematic view of a lidar data correction geometry according to an embodiment of the invention;
FIG. 3 is a schematic diagram of a laser radar data split-merge clustering algorithm according to an embodiment of the present invention;
FIG. 4 is a diagram of a road environment feature model according to an embodiment of the present invention;
FIG. 5 is a schematic diagram of feature extraction for an embodiment of the present invention;
FIG. 6 is a schematic diagram of latitude and longitude calculations for an embodiment of the present invention;
FIG. 7 is an EKF-based data fusion location of an embodiment of the present invention;
FIG. 8 is three stages of data fusion for an embodiment of the present invention.
Detailed Description
The following embodiments of the present invention are provided by way of specific examples, and other advantages and effects of the present invention will be readily apparent to those skilled in the art from the disclosure herein. The invention is capable of other and different embodiments and of being practiced or of being carried out in various ways, and its several details are capable of modification in various respects, all without departing from the spirit and scope of the present invention. It is to be noted that the features in the following embodiments and examples may be combined with each other without conflict.
It should be noted that the drawings provided in the following embodiments are only for illustrating the basic idea of the present invention, and the components related to the present invention are only shown in the drawings rather than drawn according to the number, shape and size of the components in actual implementation, and the type, quantity and proportion of the components in actual implementation may be changed freely, and the layout of the components may be more complicated.
As shown in fig. 1, the positioning method based on data fusion of the GPS and the laser radar of the present embodiment improves the positioning accuracy of the GPS, and includes the following steps:
s1, acquiring a GPS positioning coordinate;
specifically, GPS raw data is converted from the WGS-84 coordinate system to the BD-09 coordinate system data (Long) G ,lat G ) Wherein long G Is longitude, lat G In terms of latitude, the GPS positioning coordinate is expressed as (long) G ,lat G )。
S2, acquiring road environment information and establishing a road environment characteristic model; specifically, a 2D laser radar is adopted for detection to obtain road environment information.
More specifically, step S2 comprises the following sub-steps:
step S21: acquiring 2D laser radar data, and taking the position of a laser radar device as a coordinate origin and the laser radar data as point cluster data { (L) i ,θ i ) I =1,2,3,.., n }, where the dot cluster data is in polar coordinate form, θ i Indicates an angle, L i Indicating the distance.
Step S22: preprocessing the laser radar point cluster data, wherein the preprocessing steps comprise:
step S221: carrying out median filtering on the point cluster data to obtain filtered point cluster data { (L) i ,θ 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, and in the 100ms period, when the vehicle is in a moving process, the origin point is changed in the laser beam scanning process, and error compensation needs to be performed on the lidar data, as shown in fig. 2, the formula is as follows:
for j＝1to m
in the formula: t is the scanning period of the laser radar; gamma is the angular resolution of the laser radar scan; v is the vehicle speed; s t The measured distance with the serial number i in the laser radar data at the current moment is obtained; s (t -T) is the measured distance with sequence number i in the lidar data at the previous time; theta j Is the included angle between the measured barrier and the abscissa axis; s. the real The corrected lidar data.
Step S223: and converting the laser radar data in a polar coordinate form into rectangular coordinates, wherein the center of the vehicle is taken as an origin, the horizontal axis is the horizontal direction of the vehicle, and the vertical axis is the straight-going direction of the vehicle.
In the formula: l is the longitudinal length of the vehicle.
Step S23: clustering the laser radar data by adopting a split-merge algorithm, and modeling; the fitting method adopts a least square method, as shown in fig. 3, and the algorithm is as follows:
wherein, an IEPF (Iterative End Point Fit) method is adopted to extract the line segment characteristics,
(1) Taking out the laser radar point cluster data and putting the laser radar point cluster data into a ListA;
(2) Fitting two points from head to tail in A to form a straight line l 1 ；
(3) Search for the distance line l in ListA 1 The farthest point, the distance d is obtained;
(4) If d is less than the segmentation threshold d max If yes, executing step (6);
(5) Otherwise, will l 1 Is divided into 2 And l 3 If yes, executing the step (2);
(6) All the points between the segments are taken and fitted by a least square method, and the least square fitting formula is as follows:
in the formula:
β 0 ,β 1 in order to be the regression coefficient, the method,
is a parameter beta 0 ,β 1 Is estimated.
Obtaining a line segment set { l ] through the step (6) 1 ,l 2 ,l 3 ......l n And determining line segment end points, as shown in fig. 4, to obtain a final road environment feature model as follows:
wherein,
is the line segment end point, (x) j ,y j ) As the point of intersection of the line segments,
and step S3: the maximum GPS error is about 30m, the map is divided into grid maps according to the GPS error according to a certain resolution ratio, and the GPS positioning coordinate (long) obtained in the step S1 is used G ,lat G ) And determining a key grid area.
And step S4: and matching the road environment characteristic model established in the step S2 with the road key point information in the key grid area.
Step S41: using key points { X in road environment characteristic model j ,Y j Using the position of the road environment feature model as an original point, and extracting an available feature angle theta and a distance d in the road environment feature model in a clockwise direction to obtain { theta } 1 ,θ 2 ,θ 3 ,d 1 ,θ 4 ,d 2 …, as shown in fig. 5, the equation 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; theta is the angle between the perpendicular lines from the origin for each line.
Step S42: obtaining a feature set { theta ] after extracting according to the features 1 ,θ 2 ,θ 3 ,d 1 ,θ 4 ,d 2 … }, comparing with the road key point characteristic parameters recorded in the key grid region in sequence, if matching is successful within a certain threshold range, determining that the key point matching is successful, otherwise, repeating the steps S2, S3 and S4.
Step S5: according to the longitude and latitude coordinates (Long) of the successfully matched key points of the road k ,lat k ) And the coordinates (x) of the key point to the lidar j ,y j ) Calculating to obtain the longitude and latitude positioning coordinates (Long) of the vehicle L ,lat L ) As shown in fig. 6, the formula is as follows:
in the formula: d is the Euclidean distance between the key point and the laser radar; the alpha position vehicle driving azimuth takes the north direction as 0 degree; ARC is the earth's mean radius of about 6371.393 kilometers; (Long) k ,lat k ) Longitude and latitude coordinates of the key points; (Long) L ,lat L ) The longitude and latitude coordinates of the vehicle are obtained.
Step S6: obtaining longitude and latitude coordinates (Long) obtained by GPS through the steps G ,lat G ) Longitude and latitude coordinates (long) obtained by lidar L ,lat L ) Combining the speed v and the course angle directly obtained from the vehicle
And fusing the data by adopting an Extended Kalman Filter (EKF) to obtain a final positioning result. As shown in fig. 7. The state equation and observation equation for EKF are as follows:
in the formula: f () is the state transfer function of a nonlinear system; h () is an observation function of a nonlinear system; 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 combined positioning system model of the invention is as follows:
in the formula: v is vehicle speed; t is the sampling interval time; theta is the azimuth angle of the vehicle course, and the true north is 0 degree; ARC is the mean radius of the earth; w (k) is system noise; x is the number of k ,y k Respectively, the longitude and latitude of the determined location.
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. The j Jacobian matrix, H, as a function f j A Jacobian matrix that is a function H; f is a state transfer function of the nonlinear system, and H is an observation function of the nonlinear system; v (k) is system noise; p is a state variable covariance matrix; q is a process noise covariance matrix; r is an observation noise covariance matrix; k () is the Kalman gain; v is vehicle speed; t is the sampling interval time; theta is the azimuth angle of the vehicle course and takes the true north as 0 degree; ARC is the earth mean radius.
The present invention divides the data fusion process into three phases, as shown in fig. 8.
Stage one: before the key points are detected, in the stage, because the laser radar does not detect the key points of the road features yet and cannot be matched with the map, the longitude and latitude coordinate data source only comes from GPS data. The observation value at this stage is the longitude and latitude coordinate (Long) obtained by GPS G ,lat G ) The specific parameters of EKF are as follows:
and a second stage: the laser radar can detect the road characteristic key points at the stage when the key points are detected, and relatively accurate longitude and latitude coordinates (long) can be obtained through the accurate positioning method based on the GPS and the laser radar L ,lat L ) Make the data asAs observed, the specific parameters of the EKF are as follows:
and a third stage: leaving the key point, the optimal estimated coordinate at the previous moment has a certain correction effect on the data in the subsequent period of time although the key point of the road is lost at the stage, and GPS data (long) is used G ,lat G ) As observed values, the specific parameters of EKF are as follows:
the invention relates to a positioning method based on data fusion of a GPS and a laser radar, which adopts the GPS to carry out global positioning to determine an approximate area, then utilizes the laser radar to carry out accurate positioning in local, and finally fuses the data of the GPS and the laser radar to obtain a positioning result. Therefore, the positioning method based on the data fusion of the GPS and the laser radar has higher practicability.
This embodiment still provides a positioner based on GPS and laser radar data fusion, and the device includes:
the coordinate acquisition module is used for acquiring a GPS positioning coordinate;
the model establishing module is used for 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 information in the key grid area;
the coordinate calculation module is used for calculating to obtain a vehicle longitude and latitude coordinate according to the successfully matched longitude and latitude coordinate of the road key point and the coordinate of the road key point relative to the laser radar;
a data fusion module for using an extended Kalman filter to determine the GPS positioning coordinates, the longitude and latitude coordinates, the vehicle speed v and the course angle
And carrying out data fusion to obtain a final positioning result.
It should be noted that, because the embodiment of the apparatus portion and the embodiment of the method portion correspond to each other, please refer to the description of the embodiment of the method portion for the content of the embodiment of the apparatus portion, which is not repeated here.
The invention also provides a storage medium storing a computer program which, when executed by a processor, performs the aforementioned positioning method.
The present invention also provides an electronic terminal, comprising:
a memory for storing a computer program;
a processor for executing the computer program stored in the memory to cause the apparatus to perform the aforementioned positioning method.
The Processor may be a Central Processing Unit (CPU), other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other programmable logic device, discrete Gate or transistor logic, 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 Media Card (SMC), a Secure Digital Card (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 for storing the computer program and other programs and data. The memory may also be used to temporarily store data that has been or will be output.
It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-mentioned division of the functional units and modules is illustrated, and in practical applications, the above-mentioned function distribution may be performed by different functional units and modules according to needs, that is, the internal structure of the apparatus is divided into different functional units or modules to perform all or part of the above-mentioned functions. Each functional unit and module in the embodiments may be integrated in one processing unit, or each unit may exist alone physically, or two or more units are integrated in one unit, and the integrated unit may be implemented in a form of hardware, or in a form of software functional unit. In addition, specific names of the functional units and modules are only for convenience of distinguishing from each other, and are not used for limiting the protection scope of the present application. The specific working processes of the units and modules in the system may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
In the above embodiments, the descriptions of the respective embodiments have respective emphasis, and reference may be made to the related descriptions of other embodiments for parts that are not described or illustrated in a certain embodiment.
Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
In the embodiments provided in the present invention, it should be understood that the disclosed apparatus/terminal device and method may be implemented in other ways. For example, the above-described embodiments of the apparatus/terminal device are merely illustrative, and for example, the division of the modules or units is only one logical division, and there may be other divisions when actually implemented, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or units, and may be in an electrical, mechanical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit may be implemented in the form of hardware, or may also be implemented in the form of a software functional unit.
The integrated modules/units, if implemented in the form of software functional units and sold or used as separate products, may be stored in a computer readable storage medium. Based on such understanding, all or part of the flow of the method according to the embodiments of the present invention may also be implemented by a computer program, which may be stored in a computer-readable storage medium, and when the computer program is executed by a processor, the steps of the method embodiments may be implemented. Wherein the computer program comprises computer program code, which may be in the form of source code, object code, an executable file or some intermediate form, etc. The computer readable medium may comprise any entity or device capable of carrying the computer program code, a recording medium, a U disk, a removable hard disk, a magnetic disk, an optical disk, a computer Memory, a Read-Only Memory (ROM), a Random Access Memory (RAM), an electrical carrier signal, a telecommunications signal, a software distribution medium, etc.
The foregoing embodiments are merely illustrative of the principles and utilities of the present invention and are not intended to limit the invention. Any person skilled in the art can modify or change the above-mentioned embodiments without departing from the spirit and scope of the present invention. Accordingly, it is intended that all equivalent modifications or changes which can be made by those skilled in the art without departing from the spirit and technical spirit of the present invention 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 1 ,θ 2 ,θ 3 ,d 1 ,θ 4 ,d 2 …}；
Set the features { theta } 1 ,θ 2 ,θ 3 ,d 1 ,θ 4 ,d 2 … 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.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910403394.8A CN110146910B (en) | 2019-05-15 | 2019-05-15 | Positioning method and device based on data fusion of GPS and laser radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910403394.8A CN110146910B (en) | 2019-05-15 | 2019-05-15 | Positioning method and device based on data fusion of GPS and laser radar |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110146910A CN110146910A (en) | 2019-08-20 |
| CN110146910B true CN110146910B (en) | 2023-02-28 |
Family
ID=67595467
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910403394.8A Active CN110146910B (en) | 2019-05-15 | 2019-05-15 | Positioning method and device based on data fusion of GPS and laser radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110146910B (en) |
Families Citing this family (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110780325B (en) * | 2019-08-23 | 2022-07-19 | 腾讯科技（深圳）有限公司 | Method and device for positioning moving object and electronic equipment |
| CN110515382A (en) * | 2019-08-28 | 2019-11-29 | 锐捷网络股份有限公司 | A kind of smart machine and its localization method |
| CN112525184A (en) * | 2019-08-28 | 2021-03-19 | 深圳拓邦股份有限公司 | Method and system for acquiring initial data of scrubber and scrubber |
| CN110749328B (en) * | 2019-08-30 | 2021-12-28 | 华为技术有限公司 | Positioning method and system, electronic device, vehicle and storage medium |
| CN110796707B (en) * | 2019-09-10 | 2022-05-06 | 腾讯科技（深圳）有限公司 | Calibration parameter calculation method, calibration parameter calculation device and storage medium |
| CN113009414B (en) * | 2019-12-20 | 2024-03-19 | 中移(成都)信息通信科技有限公司 | Signal source position determining method and device, electronic equipment and computer storage medium |
| CN111257903B (en) * | 2020-01-09 | 2022-08-09 | 广州微牌智能科技有限公司 | Vehicle positioning method and device, computer equipment and storage medium |
| CN111241224B (en) * | 2020-01-10 | 2021-05-11 | 福瑞泰克智能系统有限公司 | Method, system, computer device and storage medium for target distance estimation |
| CN111724599B (en) * | 2020-06-30 | 2021-07-16 | 暨南大学 | Method, device, equipment and medium for acquiring safe driving behavior evaluation data |
| CN111947666B (en) * | 2020-08-21 | 2022-10-28 | 广州高新兴机器人有限公司 | Automatic retrieving method for loss of outdoor laser navigation position |
| CN112731320B (en) * | 2020-12-29 | 2024-06-21 | 福瑞泰克智能系统有限公司 | Estimation method, device, equipment and storage medium of vehicle-mounted radar error data |
| CN114814780A (en) * | 2021-01-22 | 2022-07-29 | 长沙智能驾驶研究院有限公司 | Cargo platform identification method and device, electronic equipment and computer storage medium |
| CN112987061B (en) * | 2021-02-08 | 2023-12-29 | 福州大学 | Fuzzy fusion positioning method based on GPS and laser radar |
| CN113295179B (en) * | 2021-06-04 | 2022-07-05 | 清智汽车科技(苏州)有限公司 | Vehicle course angle correction method and device based on laser sensor |
| CN114786199B (en) * | 2022-04-21 | 2024-06-14 | 中国联合网络通信集团有限公司 | Method, device, equipment and storage medium for determining network problem point |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005107693A (en) * | 2003-09-29 | 2005-04-21 | Denso Corp | Device and program for detecting travel position of one's own vehicle |
| CN102819951A (en) * | 2011-06-09 | 2012-12-12 | 无锡物联网产业研究院 | Road target detection system, road target detection method and road target detection device |
| CN104520732A (en) * | 2012-02-10 | 2015-04-15 | Isis创新有限公司 | Method of locating sensor and related apparatus |
| CN105137412A (en) * | 2015-08-19 | 2015-12-09 | 重庆大学 | Accurate fitting method of line segment features in 2D laser radar distance image |
| CN107091648A (en) * | 2017-05-11 | 2017-08-25 | 江苏保千里视像科技集团股份有限公司 | A kind of data fusion method and system of laser radar and differential GPS |
| CN208149311U (en) * | 2018-04-03 | 2018-11-27 | 北京智行者科技有限公司 | A kind of context aware systems for automatic Pilot passenger car |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8473187B2 (en) * | 2009-06-01 | 2013-06-25 | Robert Bosch Gmbh | Method and apparatus for combining three-dimensional position and two-dimensional intensity mapping for localization |
| WO2014192805A1 (en) * | 2013-05-29 | 2014-12-04 | 三菱電機株式会社 | Laser radar device and method for generating laser image |
-
2019
- 2019-05-15 CN CN201910403394.8A patent/CN110146910B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005107693A (en) * | 2003-09-29 | 2005-04-21 | Denso Corp | Device and program for detecting travel position of one's own vehicle |
| CN102819951A (en) * | 2011-06-09 | 2012-12-12 | 无锡物联网产业研究院 | Road target detection system, road target detection method and road target detection device |
| CN104520732A (en) * | 2012-02-10 | 2015-04-15 | Isis创新有限公司 | Method of locating sensor and related apparatus |
| CN105137412A (en) * | 2015-08-19 | 2015-12-09 | 重庆大学 | Accurate fitting method of line segment features in 2D laser radar distance image |
| CN107091648A (en) * | 2017-05-11 | 2017-08-25 | 江苏保千里视像科技集团股份有限公司 | A kind of data fusion method and system of laser radar and differential GPS |
| CN208149311U (en) * | 2018-04-03 | 2018-11-27 | 北京智行者科技有限公司 | A kind of context aware systems for automatic Pilot passenger car |
Non-Patent Citations (2)
| Title |
|---|
| 车载LiDAR扫描系统安置误差角检校;田祥瑞 等;《红外与激光工程》;20141031;第43卷(第10期);全文 * |
| 道路交通流状态的多参数融合预测方法;马庆禄 等;《物理学报》;20121231;第61卷(第16期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110146910A (en) | 2019-08-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110146910B (en) | Positioning method and device based on data fusion of GPS and laser radar | |
| CN108303103B (en) | Method and device for determining target lane | |
| CN108303721B (en) | Vehicle positioning method and system | |
| CN110160542B (en) | Method and device for positioning lane line, storage medium and electronic device | |
| CN109791052B (en) | Method and system for classifying data points of point cloud by using digital map | |
| CN107850449B (en) | Method and system for generating and using positioning reference data | |
| EP3137850B1 (en) | Method and system for determining a position relative to a digital map | |
| CN102208035B (en) | Image processing system and position measuring system | |
| Brenner | Extraction of features from mobile laser scanning data for future driver assistance systems | |
| CN112904395B (en) | Mining vehicle positioning system and method | |
| CN112136021B (en) | System and method for constructing landmark-based high definition map | |
| US10152635B2 (en) | Unsupervised online learning of overhanging structure detector for map generation | |
| CN112740225B (en) | Method and device for determining road surface elements | |
| CN111351502B (en) | Method, apparatus and computer program product for generating a top view of an environment from a perspective view | |
| JP2015004814A (en) | Lane map generation device and program | |
| CN102208013A (en) | Scene matching reference data generation system and position measurement system | |
| CN110864696A (en) | Three-dimensional high-precision map drawing method based on vehicle-mounted laser inertial navigation data | |
| WO2022055382A1 (en) | A method and device for determining a vehicle position | |
| CN109115232B (en) | Navigation method and device | |
| CN112424568B (en) | System and method for constructing high-definition map | |
| CN111982132B (en) | Data processing method, device and storage medium | |
| EP4276493A1 (en) | Vehicle localization based on radar detections in garages | |
| US12044545B2 (en) | Method, apparatus, and computer program product for differential variable count entity output head for map generation | |
| CN113227713A (en) | Method and system for generating environment model for positioning | |
| CN111238505B (en) | Road line segment drawing method, device and related system of road map |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |