CN110888125B - Automatic driving vehicle positioning method based on millimeter wave radar - Google Patents

Automatic driving vehicle positioning method based on millimeter wave radar Download PDF

Info

Publication number
CN110888125B
CN110888125B CN201911237088.8A CN201911237088A CN110888125B CN 110888125 B CN110888125 B CN 110888125B CN 201911237088 A CN201911237088 A CN 201911237088A CN 110888125 B CN110888125 B CN 110888125B
Authority
CN
China
Prior art keywords
grid
sub
particle
gps
vehicle
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
Application number
CN201911237088.8A
Other languages
Chinese (zh)
Other versions
CN110888125A (en
Inventor
李赵
刘心刚
袁瑞廷
张旸
陈诚
刘洁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
AutoCore Intelligence Technology Nanjing Co Ltd
Original Assignee
AutoCore Intelligence Technology Nanjing Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by AutoCore Intelligence Technology Nanjing Co Ltd filed Critical AutoCore Intelligence Technology Nanjing Co Ltd
Priority to CN201911237088.8A priority Critical patent/CN110888125B/en
Publication of CN110888125A publication Critical patent/CN110888125A/en
Application granted granted Critical
Publication of CN110888125B publication Critical patent/CN110888125B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Traffic Control Systems (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

The invention discloses an automatic driving vehicle positioning method based on a millimeter wave radar, which is mainly based on vehicle information fed back by sensors such as the millimeter wave radar and a Global Positioning System (GPS) and is combined with an improved particle filter algorithm to realize vehicle lane-level positioning. The linear velocity and the angular velocity of the vehicle in the vehicle ODOM are input into a prediction model of a filtering algorithm, point cloud data preprocessed by a millimeter wave radar and a constructed occupancy grid map are subjected to data matching, and new particle weight is calculated. In addition, in order to improve the robustness of the positioning algorithm, the traditional algorithm is improved, and the positioning result and the corresponding confidence coefficient are calculated according to different weight sums of particles; and in the resampling stage, adding part of new particles calculated according to the GPS position information into the particle swarm. The invention can ensure the positioning precision of the vehicle, effectively reduce the cost of the whole positioning system and is more beneficial to the popularization of the automatic driving technology.

Description

Automatic driving vehicle positioning method based on millimeter wave radar
Technical Field
The invention belongs to the field of automatic driving, and particularly relates to an automatic driving vehicle positioning method based on a millimeter wave radar.
Background
High-precision positioning is an important research subject in the field of automatic driving, lane-level positioning is realized by combining sensors such as high-precision integrated navigation, multi-line laser radar, Camera and the like with a high-precision map at present, and algorithms such as Kalman filtering, particle filtering, SLAM and the like are mainly adopted. The conventional Kalman filtering algorithm needs expensive high-precision RTK and IMU, and the positioning precision is low in places where GPS signals are unstable, such as viaducts, tunnels and the like; the SLAM algorithm is less robust on motorways where vehicles run fast, and in addition it requires a high performance computing platform.
Compared with Camera, the millimeter wave radar can acquire the position information of the measured object; compared with a multi-line laser radar, the millimeter wave radar has the advantages of low price, convenience in installation and the like. The Camera-based positioning method is mainly a visual odometer at present, IMU compensation is needed to correct relative motion between image frames, the method cannot work at night, robustness is low, and the method is suitable for low-speed scenes. The positioning azimuth based on the multi-line laser radar is mainly NDT at present, an initialization position needs to be provided by means of external information, the calculation force is high, and the drawing accuracy of a high-precision map is also high in requirement. The common particle filter algorithm directly carries out weighted summation on all particles and outputs single positioning data; in addition, in the structured road, due to the similarity of environmental information, the particle swarm is easily dispersed, and the robustness of the algorithm is low.
Disclosure of Invention
The purpose of the invention is as follows: aiming at the problems in the prior art, the invention provides an automatic driving vehicle positioning method based on a millimeter wave radar.
The technical scheme is as follows: in order to achieve the aim, the invention provides an automatic driving vehicle positioning method based on a millimeter wave radar, which comprises the following steps:
step 1: the millimeter wave radar, the GPS and the ODOM sensors arranged on the vehicle acquire the information of the vehicle in real time;
step 2: preprocessing point cloud data acquired by a millimeter wave radar to generate a local grid map;
and step 3: generating a particle swarm, inputting the state of each particle in the particle swarm at the previous moment into a constant conversion rate and speed model, and calculating the current state of each particle in the particle swarm;
and 4, step 4: according to the formula
Figure GDA0002363188780000011
Calculating the weight of each particle; wherein wiRepresenting the weight of the ith particle, n representing the total number of sub-grids whose pre-processed state is the occupied state, dkRepresenting the shortest distance of the sub-grids of the k-th occupation state in the grid local map in the prior occupation grid map, wherein sigma represents the precision of the prior occupation grid map, and η represents the adjustment coefficient of the millimeter wave radar;
and 5: calculating the weight sum of all the particles, wherein if the total weight sum of the particles is smaller than the minimum weight sum, the vehicle pose state is the average value of all the particle states; otherwise, calculating the state of the vehicle according to the weighted summation of all the particle weights; calculating the confidence coefficient of the current positioning result according to the weight of the particles;
step 6: generating a new particle swarm, and repeating the steps 3-5 according to the new particle swarm.
Further, the millimeter radars in step 1 have 3 to set up respectively at locomotive and rear of a vehicle, and wherein the locomotive sets up one, and the rear of a vehicle sets up two, and the contained angle between the three millimeter wave radars is 120 degrees.
Further, the method for preprocessing the point cloud data acquired by the millimeter wave radar in step 2 and generating the local grid map includes the following steps:
step 201: and eliminating point cloud data of which the radar scattering sectional area is larger than a first threshold value in the safety distance right in front of and behind the vehicle.
Step 201: mapping the rest point cloud data into a plane space, and averagely dividing the plane space into a plurality of grids;
step 203: counting the number of point clouds in each grid, if the number of point clouds in each grid is smaller than a second threshold value, marking the grid as 0, and otherwise, marking the grid as 1;
step 204: respectively averagely dividing each grid into a plurality of sub-grids, wherein the marks in the sub-grids are consistent with the marks of the grid in which the sub-grids are positioned; thereby obtaining a local grid map. Therefore, the problems of small quantity of output point clouds of the millimeter wave radar, instability, easy jumping and the like of data acquisition are effectively solved.
Further, in the step 4, the shortest distance d of the sub-grid of the k-th occupation state in the grid local map in the prior occupation grid mapkThe obtaining method comprises the following steps:
step 401: setting the maximum distance threshold as d _ max, taking the ith occupied sub-grid in the prior occupied grid map as the center, calculating the distance from the m-th row and n-th column of the unoccupied sub-grid to the ith occupied sub-grid within the range of transverse and longitudinal directions d _ max, and recording the distance as
Figure GDA0002363188780000021
Step 402: taking the (l + 1) th occupied sub-grid in the prior occupied grid map as the center, calculating the distance from the (m) th row and n columns of the unoccupied sub-grid to the (l + 1) th occupied sub-grid within the range of the transverse direction and the longitudinal direction as d _ max, and marking as the distance
Figure GDA0002363188780000022
Step 403: if it is
Figure GDA0002363188780000023
Is less than
Figure GDA0002363188780000024
Minimum distance record of unoccupied sub-grid occupying m rows and n columns in grid map a priori
Figure GDA0002363188780000025
Otherwise, the system is still
Figure GDA0002363188780000026
Step 404: traversing all the occupied sub-grids in the prior occupied grid map according to the steps 401-403, so as to obtain a minimum distance from the unoccupied sub-grid in the prior occupied grid map to the occupied sub-grid in the prior occupied grid map, where the minimum distance of the occupied sub-grid is 0;
step 405: covering the local grid map on the prior occupying grid map, and obtaining the minimum distance of the corresponding sub-grid on the prior occupying grid map according to the state of the corresponding sub-grid on the prior occupying grid map because the occupied sub-grid in the local grid map occupies the sub-grid, namely the shortest distance d of the kth radar data sub-grid in the prior occupying grid mapk. Therefore, the efficiency of the whole positioning method can be effectively improved, and the requirement on a computing platform is effectively reduced while the positioning precision is ensured.
Further, the method for generating the new particle swarm in the step 6 comprises the following steps: sorting each particle according to the weight from big to small, wherein the top 10% of the particles are copied by 3 parts; particles ranked 10% -20%, each replicated 2 parts; ranking 20% -60% of the particles, each copying 1 part, and adding 10% of new particles according to the position information of the GPS; the newly added GPS particles are elliptical, and the long radius of the ellipse is vertical to the road; the position of new particles of 10% of the total particles added is calculated as:
Figure GDA0002363188780000031
Figure GDA0002363188780000032
in the formula
Figure GDA0002363188780000033
And
Figure GDA0002363188780000034
respectively representing the abscissa and ordinate, x, of the jth particle generated from GPSgpsAnd ygpsOutputting the abscissa and ordinate of the vehicle, h, for the current GPSroadWhich is the orientation of the road in question,
Figure GDA0002363188780000035
and
Figure GDA0002363188780000036
respectively, the minor and major radii of the ellipse generated by the jth particle according to a gaussian distribution, where j denotes the number of the new particle generated according to GPS. Therefore, the particle degradation can be effectively prevented, and the positioning robustness is improved.
The working principle is as follows: the invention realizes vehicle lane-level positioning by combining an improved particle filter algorithm based on vehicle information fed back by sensors such as a millimeter wave radar and a GPS. The linear velocity and the angular velocity of the vehicle in the vehicle ODOM are input into a prediction model of a filtering algorithm, point cloud data preprocessed by a millimeter wave radar and a constructed occupancy grid map are subjected to data matching, and new particle weight is calculated. In addition, in order to improve the robustness of the positioning algorithm, the traditional algorithm is improved, and the positioning result and the corresponding confidence coefficient are calculated according to different weight sums of particles; and in the resampling stage, adding part of new particles calculated according to the GPS position information into the particle swarm.
Has the advantages that: compared with the prior art, the invention has the following advantages:
1. the millimeter wave radar and the GPS adopted by the invention are low-cost sensing devices, have obvious price advantage and are beneficial to popularization of an automatic driving technology.
2. The positioning algorithm adopted by the invention has low calculation power and does not need a high calculation platform.
3. The data collected by the millimeter wave radar is preprocessed, so that the problems of small quantity of output point clouds of the millimeter wave radar, instability, easy jumping and the like of data collection are effectively solved.
4. The invention adopts the improved conventional particle filter algorithm, adds the elliptic particle points according to the position of the GPS in the resampling stage, and simultaneously outputs the confidence corresponding to the positioning result by the algorithm, thereby being convenient for other modules to use. The robustness is effectively increased.
5. And in the resampling stage, new particles are added according to the GPS information, so that the particle degradation can be effectively prevented, and the positioning robustness is improved.
Drawings
Fig. 1 is a schematic view of the arrangement of a millimeter wave radar in a vehicle according to the present invention.
Fig. 2 is a flow chart of the operation of the present invention.
FIG. 3 is a diagram illustrating the vehicle positioning effect of the positioning method of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the examples of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
In the vehicle positioning method provided by the invention, the millimeter wave radar, the GPS and the ODOM sensor are mainly adopted for collecting the vehicle parameters. The millimeter wave radar adopts the following models: ARS 408-21; the GPS adopts the following models: GN 200B; ODOM is the linear and angular velocity information sent by the vehicle over the can bus. The horizontal field angle of the millimeter wave radar is 120 degrees, and the detection of the road edges on two sides is convenient, such as the reasons of guardrails on highways, curbstones on urban roads, green belts and the like. As shown in fig. 1, three millimeter wave radars are employed in the present invention: one head and two tails, and the included angle between the three radars is 120 degrees; one millimeter wave radar is arranged at each of the head and tail of the vehicle under normal conditions; only one millimeter wave radar at the head of the vehicle is installed in a structured road with fewer vehicles.
The embodiment discloses an automatic driving vehicle positioning method based on a millimeter wave radar, which mainly comprises the following steps:
step 1: preprocessing point cloud data acquired by a millimeter wave radar to generate a local grid map; wherein the pretreatment mainly comprises the following steps:
step 101: and eliminating point cloud data of Radar scattering sectional area RCS (Radar Cross-Section) in DIS _ SAFT meters in the front and back of the vehicle, wherein the Radar scattering sectional area RCS is larger than a Radar scattering sectional area threshold value RCS _ H. Where DIS _ SAFT represents the safe distance to the front and rear vehicles, which value is normally chosen in relation to the vehicle speed, the greater the safe distance, the lower the speed, and the smaller the safe distance. Wherein, the value range of RCS _ H is 20-40 dBms.
Step 102: and mapping the rest point cloud data into a plane space with the length of M _ REC and the width of N _ REC, wherein each GRID in the plane space is marked as GRID _ BIG, and the length of each GRID is M _ GRID and the width of each GRID is N _ GRID. Wherein M _ REC represents the length of a local rectangular plane of the environment needing to be sensed currently, and the preferable range of M _ REC is 60-80 meters. N _ REC represents the width of a local rectangular plane of the environment needing to be sensed currently, and the preferable range of the N _ REC is 10-30 meters. M _ GRID represents the length of the GRID, with a preferred range of M _ GRID being 0.75-1.25 meters. N _ GRID represents the width of the GRID, with a preferred range of N _ GRID being 0.25-0.5 meters. N _ GRID is integral multiple of M _ GRID, so that the strip-shaped objects such as green belts, railings, fences and the like on two sides of the road can be detected. M _ REC is an integer multiple of M _ GRID, and N _ REC is an integer multiple of N _ GRID.
Step 103: counting the number of point clouds in each GRID _ BIG, and if the number of the point clouds in each GRID _ BIG is smaller than NUM _ MAX, marking the GRID as 0, namely indicating that the GRID is in an unoccupied state; otherwise, the grid is marked as 1, which means that the grid is in the occupied state. Wherein NUM _ MAX indicates a threshold value of the number of point clouds included in each GRID _ BIG. The preferable range of NUM _ MAX is 3-5.
Step 104: in the GRID _ BIG, dividing the sub-GRID into GRID _ SML, if the GRID _ BIG is 0, wherein the sub-GRID _ SML is all 0, namely the sub-GRID _ SM is in an unoccupied state; if the GRID _ BIG is 1, all the sub-GRIDs GRID _ SML therein are 1, which means that the sub-GRID _ SM is in the occupied state. And obtaining a local grid map, wherein the size of the sub-grid in the local grid map is consistent with the size of the grid in the prior occupied grid map.
Step 2: on the prior occupancy grid map, taking the collected longitude and latitude of the vehicle as the center of a circle, and taking the GPS positioning deviation lambda as the radius to make a circle; and arranging particle swarms in the circle according to Gaussian distribution; and calculating the current state of each particle in the particle swarm according to the linear velocity and the angular velocity output by the vehicle ODOM and the state of each particle in the particle swarm at the previous moment. Wherein the state of each particle comprises position coordinates, speed information and heading information of each particle;
the method for calculating the current state of each particle in the particle swarm mainly comprises the following steps: inputting speed information output by an ODOM (odometer) of a vehicle into a CTRV (constant rate and speed model) running model, wherein the CTRV running model outputs state information of each particle, and the CTRV running model comprises the following steps:
Figure GDA0002363188780000051
Figure GDA0002363188780000052
Figure GDA0002363188780000053
in the formula (I), the compound is shown in the specification,
Figure GDA0002363188780000061
indicating the heading angle of the ith particle at time t,
Figure GDA0002363188780000062
indicating the heading angle of the ith particle at time t-1,
Figure GDA0002363188780000063
a value representing the angular velocity of the heading angle of the vehicle output by the IMU at time t-1, plus gaussian noise, at represents a time difference,
Figure GDA0002363188780000064
represents the abscissa of the ith particle in the UTM coordinate system at time t,
Figure GDA0002363188780000065
denotes the abscissa, v, of the ith particle at time t-1 in the UTM coordinate systemtA value representing the speed of the vehicle outputted from the vehicle ODOM (odometer) at time t with gaussian noise added thereto,
Figure GDA0002363188780000066
represents the ordinate of the ith particle in the UTM coordinate system at time t,
Figure GDA0002363188780000067
represents the ordinate of the ith particle in the UTM coordinate system at time t-1.
And step 3: according to the formula
Figure GDA0002363188780000068
Calculating the weight of each particle; wherein wiRepresenting the weight of the ith particle, n representing the total number of sub-grids whose pre-processed state is the occupied state, dkWherein the shortest distance d of the sub-grid of the k-th occupation state in the local grid map in the prior occupation grid map, the sigma represents the precision of the prior occupation grid map, and η represents the adjustment coefficient of the millimeter wave radarkThe calculation method comprises the following steps:
step 301: setting the maximum distance threshold as d _ max, taking the ith occupied sub-grid in the prior occupied grid map as the center, calculating the distance from the m-th row and n-th column of the unoccupied sub-grid to the ith occupied sub-grid within the range of transverse and longitudinal directions d _ max, and recording the distance as
Figure GDA0002363188780000069
Step 302: taking the (l + 1) th occupied sub-grid in the prior occupied grid map as the center, calculating the distance from the (m) th row and the (n) th column of the unoccupied sub-grid to the (l + 1) th occupied sub-grid within the range of the transverse direction and the longitudinal direction as d _ max again, and marking as the distance
Figure GDA00023631887800000610
Step 303: if it is
Figure GDA00023631887800000611
Is less than
Figure GDA00023631887800000612
Minimum distance record of unoccupied sub-grid occupying m rows and n columns in grid map a priori
Figure GDA00023631887800000613
Otherwise, the system is still
Figure GDA00023631887800000614
Step 304: traversing all occupied sub-grids in the prior occupied grid map according to the step 301-. Where d _ max is set to 3-5 meters.
Step 305: covering the local grid map generated in the step 1 on a prior occupation grid map, and obtaining the minimum distance of the sub-grid according to the state of the sub-grid corresponding to the sub-grid occupying the sub-grid on the prior occupation grid map in the local grid map, namely obtaining the shortest distance d of the kth radar data sub-grid in the prior occupation grid mapk。
And 4, step 4: calculating the weight sum of all the particles, wherein if the total weight sum of the particles is smaller than the minimum weight sum W _ MIN, the vehicle pose state is the average value of all the particle states; otherwise, calculating the state of the vehicle according to the weighted summation of all the particle weights; and calculating the confidence of the current positioning result according to the weight of the particles. The calculation formulas are respectively as follows:
Figure GDA0002363188780000071
Figure GDA0002363188780000072
Figure GDA0002363188780000073
wherein X represents a vehicle pose state vector output by the algorithmWherein X comprises the position and the course angle of the vehicle, B represents the confidence coefficient of the output pose state, m is the total number of particles, XiPosition state of ith particle, wiIs the weight of the ith particle, wsumIs the sum of the weights of all particles, wfitCompletely matching all sub-grids after preprocessing the millimeter wave radar with the grid positions in the occupied grid map (i.e. d in step 3)k0), and m is the total number of particles. The preferred range of the minimum weight and W _ MIN is 1.5 ~ 2.5.
And 5: and in the resampling stage, generating a new particle swarm, and repeating the steps 2-4 according to the new particle swarm. Sorting each particle according to the weight from big to small, wherein the top 10% of the particles are copied by 3 parts; particles ranked 10% -20%, each replicated 2 parts; the particles are ranked 20% -60%, each copy is 1 part, and in addition, new particles accounting for 10% of the total number are added according to the position information of the GPS to form a new particle group. The newly added GPS particles are elliptical, and the long radius of the ellipse is vertical to the road; the position of new particles of 10% of the total particles added is calculated as:
Figure GDA0002363188780000074
Figure GDA0002363188780000075
in the formula
Figure GDA0002363188780000076
And
Figure GDA0002363188780000077
respectively representing the abscissa and ordinate, x, of the jth particle generated from GPSgpsAnd ygpsOutputting the abscissa and ordinate of the vehicle, h, for the current GPSroadWhich is the orientation of the road in question,
Figure GDA0002363188780000078
and
Figure GDA0002363188780000079
respectively, the minor and major radii of the ellipse generated by the jth particle according to a gaussian distribution, where j denotes the number of the new particle generated according to GPS.
In the step, the weight calculation method in the step 3 mainly restrains the transverse definite effect of the algorithm, and on the structured road, the particle swarm is easy to disperse at the longitudinal position due to the similarity of the road structure; and 5, adding elliptical particles according to the GPS position information in the step 5, wherein the elliptical particles are mainly used for improving the longitudinal positioning effect of the algorithm, and adding elliptical particle groups, wherein the major axis of the ellipse is transverse and the minor axis is longitudinal, and the positioning accuracy of the algorithm can be improved by combining the two parts. And 4, calculating the pose state of the vehicle according to the particle weight, and simultaneously calculating the confidence coefficient of the vehicle, so that the positioning jitter phenomenon caused by incomplete road edges on two sides of a road can be solved, the robustness is high, and abundant data interfaces are provided for other modules.
The map of the vehicle positioning effect by the positioning method provided by the invention is shown in fig. 3, wherein white points are particle swarms, gray arrows are positioning tracks, gray points are occupied sub-grids in a grid map a priori, and black points are occupied sub-grids in a local grid map. Some data results of the present invention are shown in table 1.
Table 1:
Figure GDA0002363188780000081
as can be seen from table 1, the positioning accuracy of the positioning method provided by the present invention is at lane level, and meets the requirement of automatic driving positioning accuracy. The positioning method provided by the invention is not influenced by weather and environment, operates in sheltered places such as tunnels and bridge openings, and has strong robustness.

Claims (5)

1. An automatic driving vehicle positioning method based on a millimeter wave radar is characterized by comprising the following steps:
step 1, millimeter wave radar, GPS and ODOM sensors arranged on a vehicle acquire information of the vehicle in real time;
step 2, preprocessing point cloud data acquired by the millimeter wave radar to generate a local grid map;
step 3, generating a particle swarm, inputting the state of each particle in the particle swarm at the previous moment into a constant rotation rate and speed model, and calculating the current state of each particle in the particle swarm;
step 4, according to the formula
Figure DEST_PATH_IMAGE001
Calculating the weight of each particle; whereinw i The weight of the ith particle is represented,nrepresenting the total number of sub-grids whose pre-processed state is the occupied state,d k showing the first in the grid sectionkThe sub-grid of the individual occupancy states occupies the shortest distance in the grid map a priori,σindicating the accuracy of the a priori occupancy grid map,ηexpressing an adjustment coefficient of the millimeter wave radar;
step 5, calculating the weight sum of all the particles, wherein if the total weight sum of the particles is smaller than the minimum weight sum, the vehicle pose state is the average value of all the particle states; otherwise, calculating the state of the vehicle according to the weighted summation of all the particle weights; calculating the confidence coefficient of the current positioning result according to the weight of the particles;
and 6, generating a new particle swarm, and repeating the steps 3-5 according to the new particle swarm.
2. The millimeter wave radar-based autonomous vehicle positioning method of claim 1, wherein: the number of the millimeter-wave radars in the step 1 is three, the millimeter-wave radars are respectively arranged at the head and the tail of the vehicle, one head is arranged at the head, two tails are arranged at the tail of the vehicle, and included angles among the three millimeter-wave radars are 120 degrees.
3. The millimeter wave radar-based autonomous vehicle positioning method of claim 1, wherein: the method for preprocessing the point cloud data acquired by the millimeter wave radar and generating the local grid map in the step 2 comprises the following steps:
step 201, eliminating point cloud data of which the radar scattering sectional area is larger than a first threshold value within a safety distance right in front of and behind a vehicle;
step 202, mapping the remaining point cloud data to a plane space, and averagely dividing the plane space into a plurality of grids;
step 203, counting the number of point clouds in each grid, if the number is smaller than a second threshold value, marking the grid as 0, otherwise, marking the grid as 1;
step 204, equally dividing each grid into a plurality of sub-grids respectively, wherein marks in the sub-grids are consistent with marks of the grids in which the sub-grids are located; thereby obtaining a local grid map.
4. The millimeter wave radar-based autonomous vehicle positioning method of claim 1, wherein: in the grid partial map in the step 4kShortest distance of sub-grid of each occupation state in prior occupation grid mapd k The obtaining method comprises the following steps:
step 401, setting the maximum distance threshold as d _ max, taking the ith occupied sub-grid in the prior occupied grid map as the center, calculating the distance from the m-th row and n-th column of the unoccupied sub-grid to the ith occupied sub-grid within the range of transverse and longitudinal directions as d _ max, and recording asd l mn ；
Step 402, taking the (l + 1) th occupied sub-grid in the prior occupied grid map as the center, calculating the distance from the (l + 1) th occupied sub-grid to the (m) th unoccupied sub-grid in the row and the column within the range of d _ max in the transverse direction and the longitudinal direction, and recording the distance as the distanced l+1 mn ；
In step 403, ifd l+1 mn Is less thand l mn Minimum distance of an unoccupied sub-grid occupying m rows and n columns of the grid map a priorid l+1 mn Otherwise, it is stilld l mn
Step 404, traversing all the occupied sub-grids in the prior occupied grid map according to steps 401-403, so as to obtain a minimum distance from the unoccupied sub-grid in the prior occupied grid map to the occupied sub-grid in the prior occupied grid map, wherein the minimum distance from the occupied sub-grid in the prior occupied grid map is 0;
step 405, covering the local grid map on the prior occupying grid map, and obtaining the minimum distance of the corresponding sub-grid on the prior occupying grid map according to the state of the sub-grid corresponding to the sub-grid occupying the sub-grid on the prior occupying grid map in the local grid map, wherein the minimum distance is the shortest distance of the kth radar data sub-grid in the prior occupying grid mapd k 。
5. The millimeter wave radar-based autonomous vehicle positioning method of claim 1, wherein: the method for generating the new particle swarm in the step 6 comprises the following steps: sorting each particle according to the weight from big to small, wherein the top 10% of the particles are copied by 3 parts; particles ranked 10% -20%, each replicated 2 parts; ranking 20% -60% of particles, copying 1 part of each particle, and adding 10% of new particles in total according to the position information of the GPS to form a new particle group; the newly added GPS particles are elliptical, and the long radius of the ellipse is vertical to the road; the position of new particles of 10% of the total particles added is calculated as:
x j gps = x gps + r j sht *cos(-h road )+ r j lon *sin(-h road )
y j gps = y gps + r j lon *cos(-h road )- r j sht *sin(-h road )
in the formulax j gps Andy j gps respectively representing the abscissa and ordinate of the jth particle generated from GPS,x gps andy gps outputs the abscissa and ordinate of the vehicle for the current GPS,h road is the orientation of the roadr j sht Andr j lon are respectively the firstjThe particles have a short radius and a long radius of an ellipse generated according to a Gaussian distribution, whereinjReference numbers indicate new particles generated from GPS.
CN201911237088.8A 2019-12-05 2019-12-05 Automatic driving vehicle positioning method based on millimeter wave radar Active CN110888125B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911237088.8A CN110888125B (en) 2019-12-05 2019-12-05 Automatic driving vehicle positioning method based on millimeter wave radar

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911237088.8A CN110888125B (en) 2019-12-05 2019-12-05 Automatic driving vehicle positioning method based on millimeter wave radar

Publications (2)

Publication Number Publication Date
CN110888125A CN110888125A (en) 2020-03-17
CN110888125B true CN110888125B (en) 2020-06-19

Family

ID=69750662

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911237088.8A Active CN110888125B (en) 2019-12-05 2019-12-05 Automatic driving vehicle positioning method based on millimeter wave radar

Country Status (1)

Country Link
CN (1) CN110888125B (en)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021184320A1 (en) * 2020-03-19 2021-09-23 华为技术有限公司 Vehicle positioning method and device
CN113535863B (en) * 2020-04-13 2022-06-14 阿里巴巴集团控股有限公司 Moving track rendering method and device and storage medium
CN111487918B (en) * 2020-04-26 2021-08-20 天地科技股份有限公司 Bulk material unmanned loading control method, system and device based on multi-line laser radar
CN111562570A (en) * 2020-04-30 2020-08-21 南京理工大学 Vehicle sensing method for automatic driving based on millimeter wave radar
CN112034445B (en) * 2020-08-17 2022-04-12 东南大学 Vehicle motion trail tracking method and system based on millimeter wave radar
CN111708048B (en) * 2020-08-19 2021-02-05 深圳市速腾聚创科技有限公司 Method, device and system for motion compensation of point cloud
CN112201040B (en) * 2020-09-29 2022-12-16 同济大学 Traffic data cleaning method and system based on millimeter wave radar data
CN112698345B (en) * 2020-12-04 2024-01-30 江苏科技大学 Laser radar robot simultaneous positioning and map building optimization method
CN112762928B (en) * 2020-12-23 2022-07-15 重庆邮电大学 ODOM and DM landmark combined mobile robot containing laser SLAM and navigation method
CN112505671B (en) * 2021-02-07 2021-05-25 北方工业大学 Millimeter wave radar target positioning method and device under GNSS signal missing environment
CN113419235A (en) * 2021-05-28 2021-09-21 同济大学 Unmanned aerial vehicle positioning method based on millimeter wave radar
CN114199251B (en) * 2021-12-03 2023-09-15 江苏集萃智能制造技术研究所有限公司 Anti-collision positioning method for robot
CN115222767B (en) * 2022-04-12 2024-01-23 广州汽车集团股份有限公司 Tracking method and system based on space parking space

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105865449A (en) * 2016-04-01 2016-08-17 深圳杉川科技有限公司 Laser and vision-based hybrid location method for mobile robot
US9721181B2 (en) * 2015-12-07 2017-08-01 The Climate Corporation Cloud detection on remote sensing imagery
CN107991683A (en) * 2017-11-08 2018-05-04 华中科技大学 A kind of robot autonomous localization method based on laser radar
CN109682382A (en) * 2019-02-28 2019-04-26 电子科技大学 Global fusion and positioning method based on adaptive Monte Carlo and characteristic matching
CN110243358A (en) * 2019-04-29 2019-09-17 武汉理工大学 The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10057725B2 (en) * 2016-04-25 2018-08-21 Viavi Solutions Uk Limited Sensor-based geolocation of a user device

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9721181B2 (en) * 2015-12-07 2017-08-01 The Climate Corporation Cloud detection on remote sensing imagery
CN105865449A (en) * 2016-04-01 2016-08-17 深圳杉川科技有限公司 Laser and vision-based hybrid location method for mobile robot
CN107991683A (en) * 2017-11-08 2018-05-04 华中科技大学 A kind of robot autonomous localization method based on laser radar
CN109682382A (en) * 2019-02-28 2019-04-26 电子科技大学 Global fusion and positioning method based on adaptive Monte Carlo and characteristic matching
CN110243358A (en) * 2019-04-29 2019-09-17 武汉理工大学 The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
"Application of Particle Swarm Optimization with Stochastic Inertia Weight and Adaptive Mutation in Target Localization";Jinjie Yao et al.;《2010 International Conference on Computer Application and System Modeling》;20101231;全文 *
"基于改进粒子群算法的智能机器人路径规划";张万绪 等;《计算机应用》;20140210;第34卷(第2期);全文 *
"基于智能优化算法的移动机器人路径规划与定位方法研究";黄辰;《中国博士学位论文全文数据库 信息科技辑》;20190430(第04期);全文 *

Also Published As

Publication number Publication date
CN110888125A (en) 2020-03-17

Similar Documents

Publication Publication Date Title
CN110888125B (en) Automatic driving vehicle positioning method based on millimeter wave radar
CN111307162B (en) Multi-sensor fusion positioning method for automatic driving scene
Wu et al. Automatic background filtering and lane identification with roadside LiDAR data
CN111537990B (en) Lane calibration method and device and electronic equipment
CN112380312B (en) Laser map updating method based on grid detection, terminal and computer equipment
CN108961758B (en) Road junction widening lane detection method based on gradient lifting decision tree
CN102208013A (en) Scene matching reference data generation system and position measurement system
CN101975951A (en) Field environment barrier detection method fusing distance and image information
CN111027430A (en) Traffic scene complexity calculation method for intelligent evaluation of unmanned vehicles
CN104182957A (en) Traffic video detection information method and device
CN113888850B (en) Sand-dust meteorological municipal road congestion situation multi-factor early warning method
CN114926984A (en) Real-time traffic conflict collection and road safety evaluation method
DE102020118318A1 (en) Method and device for identifying a traffic junction on the basis of trajectory data
CN104156969A (en) Plane exploration method based on panoramic image depth map
Lin et al. Construction of Traffic Moving Object Detection System Based on Improved YOLOv5 Algorithm
CN113848878B (en) Indoor and outdoor three-dimensional pedestrian road network construction method based on crowd source data
CN113393676B (en) Traffic detection method and device based on unmanned aerial vehicle vision and millimeter wave radar
Ryan et al. Evaluation of small unmanned aerial system highway volume and speed‐sensing applications
CN107588760A (en) Suitable for the high-precision flight path generation method of fixed height of fixed-wing unmanned plane tracking road
CN112905856B (en) Method for constructing high-speed traffic data set with space-time dependence
US20230101472A1 (en) Methods and Systems for Estimating Lanes for a Vehicle
DE102022200936A1 (en) Method and control device for operating a sensor system of a vehicle
Kang et al. Urban road quality design supported by multi-source data and deep learning
Xie et al. Vehicle Automatic Driving Path Based on AdaBoost Algorithm
CN115626181A (en) Route planning system and method for self-driving

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
CP02 Change in the address of a patent holder
CP02 Change in the address of a patent holder

Address after: 210012 room 401-404, building 5, chuqiaocheng, No. 57, Andemen street, Yuhuatai District, Nanjing, Jiangsu Province

Patentee after: AUTOCORE INTELLIGENT TECHNOLOGY (NANJING) Co.,Ltd.

Address before: 211800 building 12-289, 29 buyue Road, Qiaolin street, Jiangbei new district, Pukou District, Nanjing City, Jiangsu Province

Patentee before: AUTOCORE INTELLIGENT TECHNOLOGY (NANJING) Co.,Ltd.