CN110888125A - Automatic driving vehicle positioning method based on millimeter wave radar - Google Patents
Automatic driving vehicle positioning method based on millimeter wave radar Download PDFInfo
- Publication number
- CN110888125A CN110888125A CN201911237088.8A CN201911237088A CN110888125A CN 110888125 A CN110888125 A CN 110888125A CN 201911237088 A CN201911237088 A CN 201911237088A CN 110888125 A CN110888125 A CN 110888125A
- Authority
- CN
- China
- Prior art keywords
- grid
- sub
- particle
- vehicle
- millimeter wave
- 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.)
- Granted
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
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 initial position needs to be provided by means of external information, the calculation force is high, and the mapping accuracy of a high-precision map is also required. The common particle filter algorithm directly carries out weighted summation on all particles and outputs single positioning data; in addition, in the structured road, the particle group is easily dispersed due to the similarity of the environmental information, and the robustness of the algorithm is low.
Disclosure of Invention
The purpose of the invention is as follows: the invention provides an automatic driving vehicle positioning method based on a millimeter wave radar, aiming at the problems in the prior art.
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 formulaCalculating 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 collected by the millimeter wave radar and generating the local raster map in the step 2 comprises 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
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
Step 403: if it isIs less thanMinimum distance record of unoccupied sub-grid occupying m rows and n columns in grid map a prioriOtherwise, the system is still
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 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:
in the formulaAndrespectively represent according toAbscissa and ordinate, x, of the jth particle generated by GPSgpsAnd ygpsOutputting the abscissa and ordinate of the vehicle, h, for the current GPSroadWhich is the orientation of the road in question,andrespectively, the minor and major radii of the ellipse generated by the jth particle according to the 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 land map occupying a grid 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, two tails and an included angle of 120 degrees among the three radars; 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 the 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 which the Radar scattering sectional area RCS (Radar Cross-Section) in DIS _ SAFT meters in the front and back of the vehicle 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 preferred range of M _ REC is 60-80 meters. N _ REC represents the width of a local rectangular plane of the environment which needs 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 state of each particle in the particle swarm at the current moment 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 last 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:
in the formula (I), the compound is shown in the specification,indicating the heading angle of the ith particle at time t,indicating the heading angle of the ith particle at time t-1,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,represents the abscissa of the ith particle in the UTM coordinate system at time t,denotes the abscissa, v, of the ith particle at time t-1 in the UTM coordinate systemtA value representing the speed of the vehicle output from the vehicle ODOM (odometer) at time t with gaussian noise added thereto,represents the ordinate of the ith particle in the UTM coordinate system at time t,represents the ordinate of the ith particle in the UTM coordinate system at time t-1.
And step 3: according to the formulaCalculating 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, dkThe shortest distance of the sub-grid representing 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
Step 302: the distance from the unoccupied sub-grid of the m row and the n column in the range of d _ max to the l +1 occupying sub-grid in the transverse direction is calculated again by taking the l +1 occupying sub-grid in the prior occupying grid map as the center,is marked as
Step 303: if it isIs less thanMinimum distance record of unoccupied sub-grid occupying m rows and n columns in grid map a prioriOtherwise, the system is still
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:
wherein X represents the vehicle pose state vector output by the algorithm, wherein 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 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:
in the formulaAndrespectively 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,andrespectively, the minor and major radii of the ellipse generated by the jth particle according to the gaussian distribution, where j denotes the label 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 long axis of the ellipse is transverse and the short 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 adopting 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:
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 in that: the method 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 formulaCalculating 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.
2. The millimeter wave radar-based autonomous vehicle positioning method of claim 1, wherein: the millimeter radars in the step 1 are 3 and are respectively arranged at the head and the tail of the vehicle, wherein the head is arranged at one, the tail of the vehicle is arranged at two, and the included angle between the three millimeter radar beams is 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: 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.
4. The millimeter wave radar-based autonomous vehicle positioning method of claim 1, wherein: in the step 4, the shortest distance d of the sub-grid in 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
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
Step 403: if it isIs less thanMinimum distance record of unoccupied sub-grid occupying m rows and n columns in grid map a prioriOtherwise, the system is still
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。
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:
in the formulaAndrespectively 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,andrespectively, 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.
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 true CN110888125A (en) | 2020-03-17 |
CN110888125B 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) |
Cited By (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111487918A (en) * | 2020-04-26 | 2020-08-04 | 天地科技股份有限公司 | 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 |
CN112034445A (en) * | 2020-08-17 | 2020-12-04 | 东南大学 | Vehicle motion trail tracking method and system based on millimeter wave radar |
CN112201040A (en) * | 2020-09-29 | 2021-01-08 | 同济大学 | Traffic data cleaning method and system based on millimeter wave radar data |
CN112505671A (en) * | 2021-02-07 | 2021-03-16 | 北方工业大学 | Millimeter wave radar target positioning method and device under GNSS signal missing environment |
CN112698345A (en) * | 2020-12-04 | 2021-04-23 | 江苏科技大学 | Robot simultaneous positioning and mapping optimization method for laser radar |
CN112731450A (en) * | 2020-08-19 | 2021-04-30 | 深圳市速腾聚创科技有限公司 | Method, device and system for motion compensation of point cloud |
CN112771352A (en) * | 2020-03-19 | 2021-05-07 | 华为技术有限公司 | Vehicle positioning method and device |
CN112762928A (en) * | 2020-12-23 | 2021-05-07 | 重庆邮电大学 | ODOM and DM landmark combined mobile robot containing laser SLAM and navigation method |
CN113419235A (en) * | 2021-05-28 | 2021-09-21 | 同济大学 | Unmanned aerial vehicle positioning method based on millimeter wave radar |
CN113535863A (en) * | 2020-04-13 | 2021-10-22 | 阿里巴巴集团控股有限公司 | Moving track rendering method and device and storage medium |
CN114199251A (en) * | 2021-12-03 | 2022-03-18 | 江苏集萃智能制造技术研究所有限公司 | Anti-collision positioning method for robot |
CN115222767A (en) * | 2022-04-12 | 2022-10-21 | 广州汽车集团股份有限公司 | Space parking stall-based tracking method and system |
Citations (6)
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 |
US20170311127A1 (en) * | 2016-04-25 | 2017-10-26 | Viavi Solutions Uk Limited | Sensor-based geolocation of a user device |
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 |
-
2019
- 2019-12-05 CN CN201911237088.8A patent/CN110888125B/en active Active
Patent Citations (6)
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 |
US20170311127A1 (en) * | 2016-04-25 | 2017-10-26 | Viavi Solutions Uk Limited | Sensor-based geolocation of a user device |
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)
Title |
---|
JINJIE YAO ET AL.: ""Application of Particle Swarm Optimization with Stochastic Inertia Weight and Adaptive Mutation in Target Localization"", 《2010 INTERNATIONAL CONFERENCE ON COMPUTER APPLICATION AND SYSTEM MODELING》 * |
张万绪 等: ""基于改进粒子群算法的智能机器人路径规划"", 《计算机应用》 * |
黄辰: ""基于智能优化算法的移动机器人路径规划与定位方法研究"", 《中国博士学位论文全文数据库 信息科技辑》 * |
Cited By (23)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112771352A (en) * | 2020-03-19 | 2021-05-07 | 华为技术有限公司 | Vehicle positioning method and device |
CN112771352B (en) * | 2020-03-19 | 2022-04-08 | 华为技术有限公司 | Vehicle positioning method and device |
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 |
CN113535863A (en) * | 2020-04-13 | 2021-10-22 | 阿里巴巴集团控股有限公司 | 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 |
CN111487918A (en) * | 2020-04-26 | 2020-08-04 | 天地科技股份有限公司 | 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 |
CN112034445A (en) * | 2020-08-17 | 2020-12-04 | 东南大学 | Vehicle motion trail tracking method and system based on millimeter wave radar |
CN112731450B (en) * | 2020-08-19 | 2023-06-30 | 深圳市速腾聚创科技有限公司 | Point cloud motion compensation method, device and system |
CN112731450A (en) * | 2020-08-19 | 2021-04-30 | 深圳市速腾聚创科技有限公司 | Method, device and system for motion compensation of point cloud |
CN112201040A (en) * | 2020-09-29 | 2021-01-08 | 同济大学 | Traffic data cleaning method and system based on millimeter wave radar data |
CN112698345A (en) * | 2020-12-04 | 2021-04-23 | 江苏科技大学 | Robot simultaneous positioning and mapping optimization method for laser radar |
CN112698345B (en) * | 2020-12-04 | 2024-01-30 | 江苏科技大学 | Laser radar robot simultaneous positioning and map building optimization method |
CN112762928A (en) * | 2020-12-23 | 2021-05-07 | 重庆邮电大学 | ODOM and DM landmark combined mobile robot containing laser SLAM and navigation 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 |
CN112505671A (en) * | 2021-02-07 | 2021-03-16 | 北方工业大学 | 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 |
CN114199251A (en) * | 2021-12-03 | 2022-03-18 | 江苏集萃智能制造技术研究所有限公司 | Anti-collision positioning method for robot |
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 |
CN115222767A (en) * | 2022-04-12 | 2022-10-21 | 广州汽车集团股份有限公司 | Space parking stall-based tracking method and system |
Also Published As
Publication number | Publication date |
---|---|
CN110888125B (en) | 2020-06-19 |
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 | |
CN108955702B (en) | Lane-level map creation system based on three-dimensional laser and GPS inertial navigation system | |
Wu et al. | Automatic background filtering and lane identification with roadside LiDAR data | |
CN111537990B (en) | Lane calibration method and device and electronic equipment | |
CN101975951B (en) | Field environment barrier detection method fusing distance and image information | |
CN112380312B (en) | Laser map updating method based on grid detection, terminal and computer equipment | |
CN110349192B (en) | Tracking method of online target tracking system based on three-dimensional laser point cloud | |
CN111027430B (en) | Traffic scene complexity calculation method for intelligent evaluation of unmanned vehicles | |
CN106199558A (en) | Barrier method for quick | |
CN103679655A (en) | LiDAR point cloud filter method based on gradient and area growth | |
CN104182957B (en) | Traffic video information detecting method and device | |
WO2020264060A1 (en) | Determining weights of points of a point cloud based on geometric features | |
CN110807924A (en) | Multi-parameter fusion method and system based on full-scale full-sample real-time traffic data | |
LU504084B1 (en) | A non-rigid registration method of vehicle-borne LiDAR point clouds by fusing multiple road features | |
CN106097718B (en) | Signalized intersections region transit time method of estimation based on GPS data | |
Li et al. | AutoMine: An unmanned mine dataset | |
Zhu et al. | A path planning algorithm based on fusing lane and obstacle map | |
CN114926984A (en) | Real-time traffic conflict collection and road safety evaluation method | |
Lin et al. | Construction of Traffic Moving Object Detection System Based on Improved YOLOv5 Algorithm | |
CN116913081A (en) | Vehicle queuing length detection method based on roadside laser radar | |
CN113902864A (en) | Vector map generation method and system for mine field and computer system | |
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 |
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 |
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. |
|
CP02 | Change in the address of a patent holder |