CN115079168B - Mapping method, device and equipment based on fusion of laser radar and millimeter wave radar - Google Patents
Mapping method, device and equipment based on fusion of laser radar and millimeter wave radar Download PDFInfo
- Publication number
- CN115079168B CN115079168B CN202210846412.1A CN202210846412A CN115079168B CN 115079168 B CN115079168 B CN 115079168B CN 202210846412 A CN202210846412 A CN 202210846412A CN 115079168 B CN115079168 B CN 115079168B
- Authority
- CN
- China
- Prior art keywords
- millimeter wave
- wave radar
- laser radar
- point cloud
- environment
- 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
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/88—Radar or analogous systems specially adapted for specific applications
- G01S13/89—Radar or analogous systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
-
- 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
- G01S13/865—Combination of radar systems with lidar 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
- 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/88—Radar or analogous systems specially adapted for specific applications
- G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
- G01S13/937—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of marine craft
-
- 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/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T11/00—2D [Two Dimensional] image generation
- G06T11/20—Drawing from basic elements, e.g. lines or circles
- G06T11/206—Drawing of charts or graphs
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T11/00—2D [Two Dimensional] image generation
- G06T11/40—Filling a planar surface by adding surface attributes, e.g. colour or texture
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T11/00—2D [Two Dimensional] image generation
- G06T11/60—Editing figures and text; Combining figures or text
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/762—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using clustering, e.g. of similar faces in social networks
- G06V10/763—Non-hierarchical techniques, e.g. based on statistics of modelling distributions
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Software Systems (AREA)
- Probability & Statistics with Applications (AREA)
- Automation & Control Theory (AREA)
- Ocean & Marine Engineering (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Computing Systems (AREA)
- Databases & Information Systems (AREA)
- Evolutionary Computation (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Multimedia (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
The embodiment of the invention discloses a method, a device and equipment for constructing a graph based on fusion of a laser radar and a millimeter wave radar, wherein the method comprises the following steps: determining the current position and posture of the unmanned ship through a positioning system; filtering out millimeter wave radar point cloud clutter points by using laser radar point cloud; constructing an environment two-dimensional grid map of the millimeter wave radar by using the millimeter wave radar point cloud; constructing an environment two-dimensional grid map of the laser radar by using the laser radar point cloud; fusing the constructed environment two-dimensional grid pattern of the millimeter wave radar and the environment two-dimensional grid pattern of the laser radar to obtain a fused grid pattern; the north east coordinate system established by the position of the unmanned ship when the unmanned ship is started for the first time is used as the coordinate origin of the environment two-dimensional grid graph of the millimeter wave radar, the environment two-dimensional grid graph of the laser radar and the grid graph after fusion. The invention can construct a high-precision environment map with lower cost.
Description
Technical Field
The invention relates to the field of unmanned ship sensing and planning, in particular to a mapping method, a mapping device and mapping equipment based on fusion of a laser radar and a millimeter wave radar.
Background
In recent years, thanks to the rapid development of intelligent technology, the precision of sensing equipment is continuously improved, and the navigation intelligence degree of unmanned ships is continuously improved. The planning algorithm taking the environment map as input in the field of unmanned ship planning is increasingly mature at present, the requirement of the unmanned ship for planning a route in autonomous navigation is preliminarily met, and meanwhile, the more accurate the environment map is, the more robust and better the planning path of the planning algorithm is.
However, the sensing precision of the horizontal dimension of the millimeter wave radar in the current mainstream sensor is not high enough, the calculation force required by visual perception is too large, and the millimeter wave radar cannot be used at night, and the laser radar is high enough, but the price is too expensive, so that the sensing system of the laser radar with the full view field is difficult to form. Therefore, how to fuse various sensors to construct a full-view-angle sensing environment map is a technical difficulty of autonomous navigation of the current unmanned ship.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a mapping method, a mapping device and mapping equipment based on the fusion of a laser radar and a millimeter wave radar.
In order to achieve the purpose, the invention adopts the following technical scheme:
in a first aspect, a mapping method based on the fusion of a laser radar and a millimeter wave radar includes:
determining the current position and posture of the unmanned ship through a positioning system;
filtering out clutter points of the millimeter wave radar point cloud by using the laser radar point cloud;
constructing an environment two-dimensional grid map of the millimeter wave radar by using the millimeter wave radar point cloud;
constructing an environment two-dimensional grid map of the laser radar by using the laser radar point cloud;
fusing the constructed environment two-dimensional grid map of the millimeter wave radar and the environment two-dimensional grid map of the laser radar to obtain a fused grid map;
the north east coordinate system established by the position of the unmanned ship when the unmanned ship is started for the first time is used as the coordinate origin of the environment two-dimensional grid graph of the millimeter wave radar, the environment two-dimensional grid graph of the laser radar and the grid graph after fusion.
The further technical scheme is as follows: the use laser radar point cloud filtering millimeter wave radar point cloud noise point includes:
performing space-time registration on the laser radar and the millimeter wave radar;
constructing the point cloud coordinates of the laser radar into a corresponding KD tree data structure;
calculating the distance between the millimeter wave radar and the nearest point cloud of the laser radar;
and matching the millimeter wave radar point cloud with the laser radar point cloud, and filtering the millimeter wave radar point cloud which cannot be matched with the laser radar point cloud.
The further technical scheme is as follows: the method for constructing the environment two-dimensional grid map of the millimeter wave radar by using the millimeter wave radar point cloud comprises the following steps:
filtering out near clutter points and clutter points at horizontal large-angle positions of the millimeter wave radar point cloud by adopting a straight-through filtering algorithm;
putting all millimeter wave radar point clouds under the same coordinate system according to the relative position relations of all millimeter wave radars installed on the unmanned ship;
filtering out the filtering points by using a DBSCAN density clustering algorithm to obtain effective millimeter wave radar point cloud;
filling the effective millimeter wave radar point cloud into a north east coordinate system of a grid map corresponding to the millimeter wave radar to obtain an environment two-dimensional grid map of the millimeter wave radar at the current moment.
The further technical scheme is as follows: the method for filling the effective millimeter wave radar point cloud into the north east coordinate system of the grid map corresponding to the millimeter wave radar to obtain the environment two-dimensional grid map of the millimeter wave radar at the current moment comprises the following steps:
calculating the coordinates of the millimeter wave radar point cloud in the millimeter wave radar grid map at the current moment according to the current position and posture of the unmanned ship;
filling the millimeter wave radar point cloud into a grid map corresponding to the millimeter wave radar according to the calculated coordinates of the millimeter wave radar point cloud in the millimeter wave radar grid map at the current moment, and resetting the unseen times of the corresponding grid to-1 time;
counting the grids with obstacles in the grid map of the millimeter wave radar in the visible area of the millimeter wave radar at the current moment, and increasing the unseen times of the grids with obstacles by 1 time;
and resetting the grid with the obstacle grid and with the number of times of non-seeing exceeding N times as the obstacle-free grid.
The further technical scheme is as follows: the method for constructing the environment two-dimensional grid map of the laser radar by using the laser radar point cloud comprises the following steps:
using a voxel filtering algorithm to carry out down-sampling on the laser radar point cloud, and using an amplitude limiting filtering algorithm to filter out water clutter points in the laser radar point cloud so as to obtain effective laser radar point cloud;
and filling the effective laser radar point cloud into a northeast coordinate system of a grid map corresponding to the laser radar to obtain an environment two-dimensional grid map of the laser radar at the current moment.
The further technical scheme is as follows: the method for filling the effective laser radar point cloud into the northeast coordinate system of the grid map corresponding to the laser radar to obtain the environment two-dimensional grid map of the laser radar at the current moment comprises the following steps:
calculating the coordinates of the laser radar point cloud in the laser radar grid map at the current moment according to the current position and the current posture of the unmanned ship;
clearing grid values of a corresponding area of the current laser radar visual range in a laser radar grid map;
and filling the laser radar point cloud into the grid map corresponding to the laser radar according to the calculated coordinate of the laser radar point cloud in the laser radar grid map at the current moment.
The further technical scheme is as follows: the fusion-constructed environment two-dimensional grid diagram of the millimeter wave radar and the environment two-dimensional grid diagram of the laser radar to obtain a fused grid diagram comprises the following steps:
acquiring a two-dimensional grid map of a millimeter wave radar environment at the current moment, and initializing all grids in the current detection range of a laser radar in the two-dimensional grid map of the millimeter wave radar environment into barrier-free grids to obtain a processed two-dimensional grid map of the millimeter wave radar environment;
acquiring an environment two-dimensional grid map of the laser radar at the current moment, and initializing grids at the side and the rear part of the ship to be barrier-free grids to obtain a processed environment two-dimensional grid map of the laser radar;
and performing grid-by-grid OR operation on the processed two-dimensional grid graph of the laser radar environment and the processed two-dimensional grid graph of the millimeter wave radar environment for fusion.
In a second aspect, the mapping device based on the fusion of the laser radar and the millimeter wave radar comprises a determining unit, a matching unit, a first constructing unit, a second constructing unit and a fusion unit;
the determining unit is used for determining the current position and posture of the unmanned ship through the positioning system;
the matching unit is used for filtering out millimeter wave radar point cloud clutter points by using laser radar point cloud;
the first construction unit is used for constructing an environment two-dimensional grid map of the millimeter wave radar by using the millimeter wave radar point cloud;
the second construction unit is used for constructing an environment two-dimensional grid map of the laser radar by using the laser radar point cloud;
the fusion unit is used for fusing the constructed environment two-dimensional grid map of the millimeter wave radar and the environment two-dimensional grid map of the laser radar to obtain a fused grid map;
the north east coordinate system established by the position of the unmanned ship when the unmanned ship is started for the first time is used as the coordinate origin of the environment two-dimensional grid graph of the millimeter wave radar, the environment two-dimensional grid graph of the laser radar and the grid graph after fusion.
In a third aspect, a computer device includes a memory, a processor, and a computer program stored on the memory and executable on the processor, and the processor executes the computer program to implement the steps of the mapping method based on the fusion of lidar and millimeter wave radar as described above.
In a fourth aspect, a computer-readable storage medium is characterized in that the storage medium stores a computer program, the computer program comprises program instructions, when executed by a processor, the processor is caused to execute the mapping method steps based on the fusion of lidar and millimeter wave radar.
Compared with the prior art, the invention has the beneficial effects that: the current position and the attitude of the unmanned ship are determined through a positioning system; constructing an environment two-dimensional grid map of the millimeter wave radar by using the millimeter wave radar point cloud; constructing an environment two-dimensional grid map of the laser radar by using the laser radar point cloud; and fusing the constructed environment two-dimensional grid pattern of the millimeter wave radar and the environment two-dimensional grid pattern of the laser radar to obtain a fused grid pattern. Because the perception visual field of the laser radar is small, the price is high, but the perception precision is high, and the perception visual field of the millimeter wave radar is large, and the price is low, therefore, through the fusion of the laser radar and the millimeter wave radar, the defect that the perception visual field of the laser radar is small can be made up, and because the millimeter wave radar has a wide visual field perception, the unmanned ship can acquire the information of the surrounding environment in severe weather (rainy days, haze days and the like), and the fusion of the laser radar and the millimeter wave radar plays a complementary role, so that the high-precision environment map can be constructed at low cost.
The foregoing description is only an overview of the technical solutions of the present invention, and in order to make the technical solutions of the present invention more clearly understood, the present invention can be implemented in accordance with the content of the description, and in order to make the above and other objects, features, and advantages of the present invention more apparent, the following detailed description of the preferred embodiments is given as follows.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
Fig. 1 is a flowchart of a mapping method based on the fusion of a laser radar and a millimeter wave radar according to an embodiment of the present invention;
fig. 2 is a schematic block diagram of a mapping method based on the fusion of a laser radar and a millimeter wave radar according to an embodiment of the present invention;
fig. 3 is a schematic block diagram of a computer device according to an embodiment 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 embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. 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.
Referring to fig. 1, fig. 1 is a flowchart of a mapping method based on the fusion of a laser radar and a millimeter wave radar according to an embodiment of the present invention. The mapping method based on the fusion of the laser radar and the millimeter wave radar is mainly applied to the unmanned ship and used for providing a high-precision environment map for autonomous navigation of the unmanned ship.
As shown in fig. 1, the mapping method based on the fusion of the laser radar and the millimeter wave radar includes the following steps: S10-S40.
And S10, determining the current position and posture of the unmanned ship through a positioning system.
In this embodiment, two GPS antennas are used and configured in a Moving-base operation mode to obtain high-precision longitude and latitude coordinates and a bow attitude (bow orientation).
And setting the longitude coordinate of the unmanned ship at that time as Lng _ init and the latitude coordinate as Lat _ init when the unmanned ship is started for the first time, and using a northeast coordinate system established at the position of the longitude coordinate system and the latitude coordinate system as the coordinate origin of the environment two-dimensional grid map of the millimeter wave radar, the environment two-dimensional grid map of the laser radar and the fused grid map. And subsequently, when the three environment grid maps are updated each time, calculating the coordinate difference (x _ t, y _ t) between the longitude and latitude coordinates (Lng, lat) of the unmanned ship at the current moment and the longitude and latitude coordinates (Lng _ init, lat _ init) of the unmanned ship at the initial start in the northeast coordinate system.
And S20, filtering out millimeter wave radar point cloud clutter points by using laser radar point cloud.
In an embodiment, step S20 specifically includes the following steps: S201-S204.
S201, performing space-time registration on the laser radar and the millimeter wave radar.
In this embodiment, one solid-state laser radar and a plurality of millimeter-wave radars are used, and therefore, the sensor data needs to be registered in time and space. The specific time dimension registration is: setting the frequency of output data of all sensors to be 10Hz, and then selecting millimeter wave radar data closest to a time stamp of the laser radar as a reference to combine the millimeter wave radar data into one frame of data. The spatial dimension registration is: the method comprises the steps of carrying out rough registration through the position relation in an unmanned ship sensor installation structure diagram, then using a calibration device to bypass around an unmanned ship with the installed sensors to collect data, and aligning a laser radar point cloud cluster and a millimeter wave radar point cloud cluster through fine adjustment of rough registration parameters to carry out fine registration.
S202, constructing the point cloud coordinates of the laser radar into a corresponding KD tree data structure.
In this embodiment, because the lidar point cloud amount is greater than the millimeter wave radar point cloud amount, the lidar point cloud coordinates with more point clouds are constructed into a KD (k-dimensional) tree, thereby implementing an algorithm for fast search matching.
S203, calculating the distance between the millimeter wave radar and the nearest laser radar point cloud.
In the embodiment, the millimeter-wave radars are traversed, and the distance between each millimeter-wave radar and the nearest laser radar point cloud is searched by searching the KD tree constructed by the laser radar point cloud.
And S204, matching the millimeter wave radar point cloud with the laser radar point cloud, and filtering the millimeter wave radar point cloud which cannot be matched with the laser radar point cloud.
In this embodiment, the millimeter wave radar point cloud having a distance between the millimeter wave radar and the closest laser radar point cloud larger than the threshold is screened by setting the threshold, and is filtered out as the clutter point.
And S30, constructing an environment two-dimensional grid map of the millimeter wave radar by using the millimeter wave radar point cloud.
In the embodiment, a plurality of millimeter wave radars with carrier frequency of 60GHz and bandwidth of 4GHz are arranged around the unmanned ship to form a sensing area of a full field of view composed of the millimeter wave radars.
In an embodiment, step S30 specifically includes the following steps: S301-S304.
S301, filtering out near clutter points and clutter points at horizontal large-angle positions of the millimeter wave radar point cloud by adopting a straight-through filtering algorithm.
In this embodiment, a straight-through filtering algorithm is first used for the millimeter wave radar point cloud to filter its near spurious point and spurious points at a horizontal wide-angle position, and the point cloud that uses ± 60 degrees in the horizontal direction directly in front of the millimeter wave radar is selected as the effective point cloud.
S302, putting all millimeter wave radar point clouds in the same coordinate system according to the relative position relations of all millimeter wave radars installed on the unmanned ship.
And putting all effective point clouds of the millimeter wave radar under the same coordinate system.
S303, filtering out the filtering points by using a DBSCAN density clustering algorithm to obtain effective millimeter wave radar point cloud.
DBSCAN (sensitivity-Based Spatial Clustering of Applications with Noise) is used as a Density Clustering algorithm, and the parameters mainly comprise two parameters: radius eps, minimum number of points in class min _ simple. The millimeter wave radar point cloud is characterized by being more dense at a near place and more sparse at a far place.
In this embodiment, a traditional DBSCAN algorithm is improved according to the characteristics of dense proximity and sparse distance of the millimeter wave radar, specifically, the radius used by the point cloud is calculated adaptively according to the distance between the point cloud and the radar, and a radius eps _ imp calculation formula is as follows:
wherein eps _ imp is the calculation result of the radius in the algorithm; scale is the scaling size in the algorithm and needs to be adjusted according to different radars and different environments; d _ max is the current radar farthest detection distance; d is the distance between the point cloud currently requiring filtering processing and the radar.
After through filtering and improved DBSCAN filtering, effective millimeter wave radar point cloud is obtained.
S304, filling the effective millimeter wave radar point cloud into a northeast coordinate system of the grid map corresponding to the millimeter wave radar to obtain an environment two-dimensional grid map of the millimeter wave radar at the current moment.
In an embodiment, step S304 specifically includes the following steps: S3041-S3044.
S3041, calculating coordinates of the millimeter wave radar point cloud in the millimeter wave radar grid map at the current moment according to the current position and posture of the unmanned ship.
In this embodiment, the coordinates (x _ t, y _ t) of the unmanned ship in the grid map at the current moment and the included angle yaw between the bow and the due north direction are obtained.
S3042, filling the millimeter wave radar point cloud into a grid diagram corresponding to the millimeter wave radar according to the calculated coordinates of the millimeter wave radar point cloud in the millimeter wave radar grid diagram at the current moment, and resetting the unseen times of the corresponding grid to-1 time.
Calculating the coordinates of the millimeter wave radar point cloud in the millimeter wave radar grid map at the current moment in the following specific mode:
firstly, the default coordinate system of the millimeter wave radar point cloud is that the detection direction of the millimeter wave radar is used as a y axis, the right side of the millimeter wave radar is used as an x axis, meanwhile, the included angle between the bow of the current unmanned ship and the due north direction is obtained as yaw, and at the moment, the coordinate of the millimeter wave radar point cloud under the current moment is firstly rotated
：
Wherein
The coordinates after the millimeter wave radar point cloud is rotated.
Secondly, translating the original point of the millimeter wave radar point cloud coordinate system after rotation to (x _ t, y _ t) to obtain the final millimeter wave radar point cloud coordinate
：
Due to the fact that the millimeter wave radar has poor robustness, long targets such as shoreside, lake center islands and the like cannot be observed completely. Therefore, when the millimeter wave radar point cloud is used for mapping, the number N of times that the obstacle grid in the radar detection field of view is not observed by the millimeter wave radar needs to be counted, namely after a certain grid is observed by the millimeter wave once, if the certain grid is observed again in the subsequent N times, the number of times that the grid is not observed is refreshed to be 0, and when the grid is not detected yet in the subsequent N +1, the obstacle grid is reset to be a non-obstacle grid. Specifically, the method comprises the following steps: filling the millimeter wave radar point cloud into the millimeter wave radar grid map, and resetting the unseen times of the corresponding grid to-1 time.
S3043, counting the grids with obstacles in the grid pattern of the millimeter wave radar in the visible area of the millimeter wave radar at the current time, and increasing the number of times that the grids with obstacles are not seen by 1 time.
In the present embodiment, the number of times of non-visibility of the obstacle grids is increased by 1 by counting the obstacle grids in the raster image of the millimeter wave radar that are within the visible area of the millimeter wave radar at the present time.
S3044, the grid with the obstacle grid whose number of times of non-observation exceeds N times is reset to the obstacle-free grid.
In this embodiment, the grid with the number of times of non-visibility exceeding N times of the obstacle grid is reset to the obstacle-free grid, and thus the construction of the environment two-dimensional grid map of the millimeter wave radar is completed.
And S40, constructing an environment two-dimensional grid map of the laser radar by using the laser radar point cloud.
Firstly, preprocessing laser radar point cloud, and then constructing an environment grid map by using the laser radar point cloud.
In some embodiments, step S40 specifically includes the following steps: S401-S402.
S401, downsampling the laser radar point cloud by using a voxel filtering algorithm, and filtering out water clutter points in the laser radar point cloud by using an amplitude limiting filtering algorithm to obtain the effective laser radar point cloud.
Due to the inherent characteristics of the laser radar, the data volume received each time is large, so that the point cloud of the laser radar is required to be downsampled by using a voxel filtering algorithm to reduce the number of the point clouds; meanwhile, under the water surface environment, water clutter points are inevitably generated by the laser radar, so that the water clutter points in the laser radar point cloud are filtered by using an amplitude limiting filtering algorithm.
And processing by using a voxel filtering algorithm and an amplitude limiting filtering algorithm to obtain a final sensing result of the laser radar on the surrounding environment of the unmanned ship, namely effective laser radar point cloud.
S402, filling effective laser radar point clouds into a northeast coordinate system of a grid map corresponding to the laser radar to obtain an environment two-dimensional grid map of the laser wave radar at the current moment.
In an embodiment, step S402 specifically includes the following steps S4021 to S4023.
S4021, calculating coordinates of the laser radar point cloud in the laser radar grid map at the current moment according to the current position and posture of the unmanned ship.
In this embodiment, an included angle yaw between coordinates (x _ t, y _ t) of the unmanned ship in the grid map at the current moment and the positive north direction of the bow is obtained.
Calculating the coordinates of the laser radar point cloud in the laser radar grid map at the current moment, wherein the specific mode is as follows:
firstly, the default coordinate system of the laser radar point cloud is that the detection direction of the laser radar is x, the left side of the laser radar is y, the included angle between the bow of the current unmanned ship and the due north direction is obtained as yaw, and at the moment, the laser radar point cloud coordinate at the current moment is firstly rotated
：
Secondly, translating the rotated point cloud coordinate system origin of the laser radar point to (x _ t, y _ t) to obtain the final point cloud coordinate of the laser radar point
：
S4022, clearing grid values of the corresponding area of the current laser radar visible range in the laser radar grid map.
In this embodiment, the grid value of the corresponding area of the current lidar visible range in the lidar grid map is cleared, i.e., set to 0.
S4023, filling the laser radar point cloud into the grid image corresponding to the laser radar according to the calculated coordinates of the laser radar point cloud in the laser radar grid image at the current moment.
And filling the calculated coordinates of the laser radar point cloud at the current moment in the laser radar grid map into the grid map corresponding to the laser radar.
Meanwhile, aiming at the condition that the solid-state laser radar cannot obtain 360-degree omnidirectional sensing, the millimeter wave radar is used near the periphery of the unmanned ship to maintain the raster image of the laser radar, so that the raster image constructed by the laser radar is more accurate. And completing the construction of the two-dimensional grid map of the environment of the laser radar.
And S50, fusing the constructed environment two-dimensional grid map of the millimeter wave radar and the environment two-dimensional grid map of the laser radar to obtain a fused grid map.
The detection effect of the laser radar is very robust, and the detection effect of the millimeter wave radar is poor in robustness, so that the grid image of the laser radar is used as the only grid image in the detection range of the laser radar in front of the unmanned ship; meanwhile, most laser radars cannot complete the 360-degree omnidirectional sensing function, so that the millimeter wave radar-sensed environment raster image is used in laser radar view dead zones such as the side and the rear of the unmanned ship; and obtaining a grid map by fusing the laser radar and the millimeter wave radar grid map in other areas of the unmanned ship.
In an embodiment, step S50 specifically includes the following steps: S501-S503.
S501, obtaining a two-dimensional grid map of the millimeter wave radar environment at the current moment, and initializing all grids in the current detection range of the laser radar in the two-dimensional grid map of the millimeter wave radar environment into barrier-free grids to obtain the processed two-dimensional grid map of the millimeter wave radar environment.
S502, obtaining an environment two-dimensional grid map of the laser radar at the current moment, and initializing grids at the side and the rear near position of the ship into barrier-free grids to obtain a processed environment two-dimensional grid map of the laser radar.
And S503, performing grid-by-grid OR operation on the processed two-dimensional grid map of the laser radar environment and the processed two-dimensional grid map of the millimeter wave radar environment for fusion.
And marking the grid as a state with obstacles as long as one of the laser radar or millimeter wave radar grid maps at the fused grid position shows obstacles.
Through the fusion of the laser radar and the millimeter wave radar, the defect that the perception view of the solid laser radar is small can be overcome, and due to the wide view perception of the millimeter wave radar, the unmanned ship can acquire information of the surrounding environment in severe weather (rainy days, haze days and the like), and the fusion of the laser radar and the millimeter wave radar plays a complementary role, so that a high-precision environment map can be constructed at low cost.
Fig. 2 is a schematic block diagram of a drawing establishing device based on fusion of a laser radar and a millimeter wave radar according to an embodiment of the present invention; corresponding to the mapping method based on the fusion of the laser radar and the millimeter wave radar, the embodiment of the invention also provides a mapping device 100 based on the fusion of the laser radar and the millimeter wave radar.
As shown in fig. 2, the mapping apparatus 100 based on the fusion of the lidar and the millimeter-wave radar includes a determining unit 110, a matching unit 120, a first constructing unit 130, a second constructing unit 140, and a fusing unit 150.
And the determining unit 110 is used for determining the current position and attitude of the unmanned ship through the positioning system.
In this embodiment, two GPS antennas are used and configured in a Moving-base operating mode to obtain high-precision longitude and latitude coordinates and a bow attitude (bow heading direction).
And setting the longitude coordinate of the unmanned ship at that time as Lng _ init and the latitude coordinate as Lat _ init when the unmanned ship is started for the first time, and using a northeast coordinate system established at the position of the longitude coordinate system and the latitude coordinate system as the coordinate origin of the environment two-dimensional grid map of the millimeter wave radar, the environment two-dimensional grid map of the laser radar and the fused grid map. And subsequently, when the three environment grid maps are updated each time, calculating the coordinate difference (x _ t, y _ t) between the longitude and latitude coordinates (Lng, lat) of the unmanned ship at the current moment and the longitude and latitude coordinates (Lng _ init, lat _ init) of the unmanned ship at the initial start in the northeast coordinate system.
And the matching unit 120 is used for filtering out millimeter wave radar point cloud clutter points by using laser radar point cloud.
In an embodiment, the matching unit 120 comprises a registration module, a construction module, a calculation module and a filtering module.
And the registration module is used for performing space-time registration on the laser radar and the millimeter wave radar.
In this embodiment, one solid-state laser radar and a plurality of millimeter-wave radars are used, and therefore, the sensor data needs to be registered in time and space. The specific time dimension registration is: setting the frequency of output data of all sensors to be 10Hz, and then selecting millimeter wave radar data closest to a time stamp of the laser radar as a reference to combine the millimeter wave radar data into one frame of data. The spatial dimension registration is: the method comprises the steps of roughly registering through the position relation in an unmanned ship sensor installation structure diagram, then using a calibration device to bypass around an unmanned ship with sensors installed, collecting data, and finely adjusting rough registration parameters to enable a laser radar point cloud cluster and a millimeter wave radar point cloud cluster to be aligned for fine registration.
And the building module is used for building the point cloud coordinates of the laser radar into a corresponding KD tree data structure.
In this embodiment, because the lidar point cloud amount is greater than the millimeter wave radar point cloud amount, the lidar point cloud coordinates with more point clouds are constructed into a KD tree, thereby implementing an algorithm for fast search and matching.
And the calculation module is used for calculating the distance between the millimeter wave radar and the closest point cloud of the laser radar.
In the embodiment, the millimeter wave radars are traversed, and the distance between each millimeter wave radar and the nearest laser radar point cloud is searched through a KD tree constructed by searching the laser radar point cloud.
And the filtering module is used for matching the millimeter wave radar point cloud with the laser radar point cloud and filtering the millimeter wave radar point cloud which cannot be matched with the laser radar point cloud.
In this embodiment, the millimeter wave radar point cloud having a distance between the millimeter wave radar and the closest laser radar point cloud larger than the threshold is screened by setting the threshold, and is filtered out as the clutter point.
A first construction unit 130, configured to construct an environmental two-dimensional grid map of the millimeter wave radar using the millimeter wave radar point cloud.
In the embodiment, a plurality of millimeter wave radars with carrier frequency of 60GHz and bandwidth of 4GHz are arranged around the unmanned ship to form a sensing area of a full field of view composed of the millimeter wave radars.
In an embodiment, the first building unit 130 includes a first filtering module, a dropping module, a second filtering module, and a first filling module.
And the first filtering module is used for filtering the near clutter points and the clutter points at the horizontal large-angle positions of the millimeter wave radar point cloud by adopting a straight-through filtering algorithm.
In this embodiment, a straight-through filtering algorithm is first used for the millimeter wave radar point cloud to filter its near spurious point and spurious points at a horizontal wide-angle position, and the point cloud that uses ± 60 degrees in the horizontal direction directly in front of the millimeter wave radar is selected as the effective point cloud.
And the releasing module is used for releasing all the millimeter wave radar point clouds in the same coordinate system according to the relative position relations of all the millimeter wave radars in the unmanned ship.
And placing all effective point clouds of the millimeter wave radar under the same coordinate system.
And the second filtering module is used for filtering out the filtering points by using a DBSCAN density clustering algorithm to obtain effective millimeter wave radar point cloud.
DBSCAN (sensitivity-Based Spatial Clustering of Applications with Noise) is used as a Density Clustering algorithm, and the parameters mainly comprise two parameters: radius eps, minimum number of points in class min _ simple. The millimeter wave radar point cloud has the characteristics of being more dense at a near place and more sparse at a far place.
In this embodiment, a traditional DBSCAN algorithm is improved according to the characteristics of dense proximity and sparse distance of the millimeter wave radar, specifically, the radius used by the point cloud is calculated adaptively according to the distance between the point cloud and the radar, and a radius eps _ imp calculation formula is as follows:
After the direct filtering and the improved DBSCAN filtering, the effective millimeter wave radar point cloud is obtained.
The first filling module is used for filling effective millimeter wave radar point cloud into a north-east coordinate system of a grid map corresponding to the millimeter wave radar to obtain an environment two-dimensional grid map of the millimeter wave radar at the current moment.
In one embodiment, the first padding module includes a first computation submodule, a first padding submodule, a statistics submodule, and a reset submodule;
and the first calculation submodule is used for calculating the coordinates of the millimeter wave radar point cloud in the millimeter wave radar grid map at the current moment according to the current position and the current attitude of the unmanned ship.
In this embodiment, the coordinates (x _ t, y _ t) of the unmanned ship in the grid map at the current moment and the included angle yaw between the bow and the due north direction are obtained.
And the first filling sub-module is used for filling the millimeter wave radar point cloud into the grid map corresponding to the millimeter wave radar according to the calculated coordinates of the millimeter wave radar point cloud in the millimeter wave radar grid map at the current moment, and resetting the unseen times of the corresponding grid to-1 time.
Calculating the coordinates of the millimeter wave radar point cloud in the millimeter wave radar grid map at the current moment in the following specific mode:
firstly, the default coordinate system of the millimeter wave radar point cloud is that the detection direction of the millimeter wave radar is used as a y axis, the right side of the millimeter wave radar is used as an x axis, meanwhile, the included angle between the bow of the current unmanned ship and the due north direction is obtained as yaw, and at the moment, the coordinate of the millimeter wave radar point cloud under the current moment is firstly rotated
：
Secondly, translating the origin of the rotated millimeter wave radar point cloud coordinate system to (x _ t, y _ t) to obtain the final millimeter wave radar point cloud coordinate
：
Due to the fact that the millimeter wave radar has poor robustness, long targets such as shoreside, lake center islands and the like cannot be observed completely. Therefore, when the millimeter wave radar point cloud is used for mapping, the number N of times that the obstacle grid in the radar detection field is not observed by the millimeter wave radar needs to be counted, namely after a certain grid is observed by the millimeter wave once, if the certain grid is observed again in the subsequent N times, the number of times that the grid is not observed is refreshed to be 0, and when the grid is not detected in the subsequent N +1, the obstacle grid is reset to be a non-obstacle grid. Specifically, the method comprises the following steps: filling the millimeter wave radar point cloud into the millimeter wave radar grid map, and resetting the unseen times of the corresponding grid to-1 time.
And the counting submodule is used for counting the grids with obstacles in the grid diagram of the millimeter wave radar in the visible area of the millimeter wave radar at the current moment and increasing the unseen times of the grids with obstacles by 1 time.
In the present embodiment, the number of times of non-visibility of the obstacle grids is increased by 1 by counting the obstacle grids in the raster image of the millimeter wave radar that are within the visible area of the millimeter wave radar at the present time.
A reset submodule for resetting the grid having the number of times of non-seeing more than N times of the obstacle grid to the obstacle-free grid.
In this embodiment, the grid with the number of times of non-visibility exceeding N times of the obstacle grid is reset to the obstacle-free grid, and thus the construction of the environment two-dimensional grid map of the millimeter wave radar is completed.
A second construction unit 140 for constructing an environmental two-dimensional grid map of the lidar using the lidar point cloud.
Firstly, preprocessing the laser radar point cloud, and then constructing an environment grid map by using the laser radar point cloud.
In some embodiments, the second building unit 140 comprises a downsampling module and a second padding module.
And the sampling module is used for down-sampling the laser radar point cloud by using a voxel filtering algorithm and filtering water clutter points in the laser radar point cloud by using an amplitude limiting filtering algorithm so as to obtain effective laser radar point cloud.
Due to the inherent characteristics of the laser radar, the data volume received each time is large, so that the point cloud of the laser radar is required to be downsampled by using a voxel filtering algorithm to reduce the number of the point clouds; meanwhile, under the water surface environment, water clutter points are inevitably generated by the laser radar, so that the water clutter points in the laser radar point cloud are filtered by using an amplitude limiting filtering algorithm.
And processing by using a voxel filtering algorithm and an amplitude limiting filtering algorithm to obtain a final sensing result of the laser radar on the surrounding environment of the unmanned ship, namely effective laser radar point cloud.
And the second filling module is used for filling effective laser radar point clouds into a north-east coordinate system of a grid map corresponding to the laser radar to obtain an environment two-dimensional grid map of the laser radar at the current moment.
In an embodiment, the second fill module includes a second compute submodule, an empty submodule, and a second fill submodule.
And the second calculation submodule is used for calculating the coordinates of the laser radar point cloud in the laser radar grid map at the current moment according to the current position and the current attitude of the unmanned ship.
In this embodiment, the coordinates (x _ t, y _ t) of the unmanned ship in the grid map at the current moment and the included angle yaw between the bow and the due north direction are obtained.
Calculating the coordinates of the laser radar point cloud in the laser radar grid map at the current moment, wherein the specific mode is as follows:
firstly, the default coordinate system of the laser radar point cloud is that the detection direction of the laser radar is x, the left side of the laser radar is y, the included angle between the bow of the current unmanned ship and the due north direction is obtained as yaw, and at the moment, the laser radar point cloud coordinate at the current moment is firstly rotated
：
Secondly, translating the rotated point cloud coordinate system origin of the laser radar point to (x _ t, y _ t) to obtain the final point cloud coordinate of the laser radar point
：
And the clearing submodule is used for clearing the grid values of the corresponding area of the current laser radar visible range in the laser radar grid map.
In this embodiment, the grid value of the corresponding area of the current lidar visible range in the lidar grid map is cleared, i.e., set to 0.
And the second filling sub-module fills the laser radar point cloud into the grid image corresponding to the laser radar according to the calculated coordinate of the laser radar point cloud in the laser radar grid image at the current moment.
And filling the calculated coordinates of the laser radar point cloud at the current moment in the laser radar grid map into the grid map corresponding to the laser radar.
Meanwhile, aiming at the condition that the solid-state laser radar cannot obtain 360-degree omnidirectional perception, millimeter wave radars are used near the periphery of the unmanned ship to maintain a raster image of the laser radar, so that the raster image constructed by the laser radar is more accurate, and the construction of an environment two-dimensional raster image of the laser radar is completed.
And the fusion unit 150 is configured to fuse the constructed environment two-dimensional grid map of the millimeter wave radar and the environment two-dimensional grid map of the laser radar to obtain a fused grid map.
The detection effect of the laser radar is very robust, while the detection effect of the millimeter wave radar is poor in robustness, so that the grid image of the laser radar is used as the only grid image in the detection range of the laser radar right in front of the unmanned ship; meanwhile, most laser radars cannot complete the 360-degree omnidirectional sensing function, so that the millimeter wave radar-sensed environment raster image is used in laser radar view dead zones such as the side and the rear of the unmanned ship; and using a raster image obtained by fusing the laser radar and the millimeter wave radar raster images in other areas of the unmanned ship.
In one embodiment, the fusion unit 150 includes a first obtaining module, a second obtaining module, and a fusion module.
The first acquisition module is used for acquiring a millimeter wave radar environment two-dimensional grid map at the current moment, and initializing all grids in the current detection range of the laser radar in the millimeter wave radar environment two-dimensional grid map into barrier-free grids so as to obtain a processed millimeter wave radar environment two-dimensional grid map.
And the second acquisition module is used for acquiring the two-dimensional environment grid map of the laser radar at the current moment, and initializing the grids close to the side and the rear of the ship into barrier-free grids so as to obtain the two-dimensional environment grid map of the laser radar after processing.
And the fusion module is used for performing grid-by-grid OR operation on the processed two-dimensional grid map of the laser radar environment and the processed two-dimensional grid map of the millimeter wave radar environment one by one to fuse the two-dimensional grid maps.
And marking the grid as an obstacle state as long as one of the laser radar grid graphs or the millimeter wave radar grid graphs on the fused grid position shows an obstacle.
Through the fusion of the laser radar and the millimeter wave radar, the defect that the perception view of the solid laser radar is small can be overcome, and due to the wide view perception of the millimeter wave radar, the unmanned ship can acquire information of surrounding environment in severe weather (rainy days, haze days and the like), and the fusion of the laser radar and the millimeter wave radar plays a complementary role, so that a high-precision environment map can be constructed at low cost.
The mapping device based on the fusion of the laser radar and the millimeter wave radar can be implemented in the form of a computer program, and the computer program can be run on a computer device as shown in fig. 3.
Referring to fig. 3, fig. 3 is a schematic block diagram of a computer device according to an embodiment of the present application. The computer device 700 may be a server, wherein the server may be an independent server or a server cluster composed of a plurality of servers.
As shown in fig. 3, the computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, and when the processor executes the computer program, the steps of the mapping method based on the fusion of the lidar and the millimeter wave radar are implemented.
The computer device 700 may be a terminal or a server. The computer device 700 includes a processor 720, memory, and a network interface 750, which are connected by a system bus 710, where the memory may include non-volatile storage media 730 and internal memory 740.
The non-volatile storage medium 730 may store an operating system 731 and computer programs 732. The computer program 732, when executed, may cause the processor 720 to perform any of the mapping methods based on the fusion of lidar and millimeter wave radar.
The processor 720 is used to provide computing and control capabilities, supporting the operation of the overall computer device 700.
The internal memory 740 provides an environment for the operation of the computer program 732 in the non-volatile storage medium 730, and when the computer program 732 is executed by the processor 720, the processor 720 may be enabled to execute any mapping method based on the fusion of the lidar and the millimeter wave radar.
The network interface 750 is used for network communication such as sending assigned tasks and the like. Those skilled in the art will appreciate that the configuration shown in fig. 3 is a block diagram of only a portion of the configuration relevant to the present teachings and is not intended to limit the computing device 700 to which the present teachings may be applied, and that a particular computing device 700 may include more or less components than those shown, or may combine certain components, or have a different arrangement of components. Wherein the processor 720 is configured to run the program code stored in the memory to implement the above method steps:
it should be understood that, in the embodiment of the present Application, the Processor 720 may be a Central Processing Unit (CPU), and the Processor 720 may also be other general-purpose processors, digital Signal Processors (DSPs), application Specific Integrated Circuits (ASICs), field Programmable Gate Arrays (FPGAs) or other Programmable logic devices, discrete Gate or transistor logic devices, discrete hardware components, and the like. Wherein a general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
Those skilled in the art will appreciate that the configuration of computer device 700 depicted in FIG. 3 is not intended to be limiting of computer device 700 and may include more or less components than those shown, or some components in combination, or a different arrangement of components.
In another embodiment of the present invention, a computer-readable storage medium is provided. The computer readable storage medium may be a non-volatile computer readable storage medium. The computer readable storage medium stores a computer program, wherein the computer program is executed by a processor to implement the mapping method based on the fusion of the laser radar and the millimeter wave radar disclosed by the embodiment of the invention.
It can be clearly understood by those skilled in the art that, for convenience and simplicity of description, the specific working processes of the above-described devices, apparatuses and units may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again. Those of ordinary skill in the art will appreciate that the elements and algorithm steps of the examples described in connection with the embodiments disclosed herein may be embodied in electronic hardware, computer software, or combinations of both, and that the components and steps of the examples have been described in a functional general in the foregoing description for the purpose of illustrating clearly the interchangeability of hardware and software. 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 several embodiments provided in the present invention, it should be understood that the disclosed apparatus, device and method may be implemented in other manners. For example, the above-described embodiments of the apparatus are merely illustrative, and for example, the division of the units is only a logical division, and there may be other divisions when the actual implementation is performed, or units having the same function may be grouped into one unit, for example, a plurality of units or components may be combined or may be 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 also be an electric, mechanical or other form of connection.
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 of the present invention.
While the invention has been described with reference to specific embodiments, the invention is not limited thereto, and various equivalent modifications and substitutions can be easily made by those skilled in the art within the technical scope of the invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.
Claims (9)
1. The mapping method based on the fusion of the laser radar and the millimeter wave radar is characterized by comprising the following steps:
determining the current position and the current posture of the unmanned ship through a positioning system;
filtering out millimeter wave radar point cloud clutter points by using laser radar point cloud;
constructing an environment two-dimensional grid map of the millimeter wave radar by using the millimeter wave radar point cloud;
constructing an environment two-dimensional grid map of the laser radar by using the laser radar point cloud;
fusing the constructed environment two-dimensional grid map of the millimeter wave radar and the environment two-dimensional grid map of the laser radar to obtain a fused grid map;
the north east coordinate system established by the position of the unmanned ship when the unmanned ship is started for the first time is used as the coordinate origin of the environment two-dimensional grid graph of the millimeter wave radar, the environment two-dimensional grid graph of the laser radar and the fused grid graph;
the fusion-constructed environment two-dimensional grid pattern of the millimeter wave radar and the environment two-dimensional grid pattern of the laser radar to obtain a fusion-constructed grid pattern comprises the following steps:
acquiring a millimeter wave radar environment two-dimensional grid map at the current moment, and initializing all grids in the current detection range of the laser radar in the millimeter wave radar environment two-dimensional grid map into barrier-free grids to obtain a processed millimeter wave radar environment two-dimensional grid map;
acquiring an environment two-dimensional grid map of the laser radar at the current moment, and initializing grids at the side and the rear part of the ship to form barrier-free grids so as to obtain a processed environment two-dimensional grid map of the laser radar;
and performing grid-by-grid OR operation on the processed laser radar environment two-dimensional grid graph and the processed millimeter wave radar environment two-dimensional grid graph one by one for fusion.
2. The mapping method based on the fusion of the laser radar and the millimeter wave radar according to claim 1, wherein the filtering of the millimeter wave radar point cloud clutter points by using the laser radar point cloud comprises:
performing space-time registration on the laser radar and the millimeter wave radar;
constructing the point cloud coordinates of the laser radar into a corresponding KD tree data structure;
calculating the distance between the millimeter wave radar and the nearest laser radar point cloud;
and matching the millimeter wave radar point cloud with the laser radar point cloud, and filtering the millimeter wave radar point cloud which cannot be matched with the laser radar point cloud.
3. The mapping method based on the fusion of the laser radar and the millimeter wave radar as claimed in claim 1, wherein the constructing the environment two-dimensional raster map of the millimeter wave radar by using the millimeter wave radar point cloud comprises:
filtering out near clutter points and clutter points at horizontal large-angle positions of the millimeter wave radar point cloud by adopting a straight-through filtering algorithm;
putting all millimeter wave radar point clouds under the same coordinate system according to the relative position relation of all millimeter wave radars installed on the unmanned ship;
filtering out the filtering points by using a DBSCAN density clustering algorithm to obtain effective millimeter wave radar point cloud;
and filling the effective millimeter wave radar point cloud into a northeast coordinate system of a grid map corresponding to the millimeter wave radar to obtain an environment two-dimensional grid map of the millimeter wave radar at the current moment.
4. The mapping method based on the fusion of the laser radar and the millimeter wave radar as claimed in claim 3, wherein the step of filling the effective millimeter wave radar point cloud into the north-east coordinate system of the grid map corresponding to the millimeter wave radar to obtain the two-dimensional grid map of the millimeter wave radar at the current moment comprises the steps of:
calculating the coordinates of the millimeter wave radar point cloud in the millimeter wave radar grid map at the current moment according to the current position and posture of the unmanned ship;
filling the millimeter wave radar point cloud into a grid map corresponding to the millimeter wave radar according to the calculated coordinates of the millimeter wave radar point cloud in the millimeter wave radar grid map at the current moment, and resetting the unseen times of the corresponding grid to-1 time;
counting the grids with obstacles in the grid map of the millimeter wave radar in the visible area of the millimeter wave radar at the current moment, and increasing the unseen times of the grids with obstacles by 1 time;
and resetting the grid with the barrier grid and with the number of times of non-seeing exceeding N times as the barrier-free grid.
5. The mapping method based on the fusion of the lidar and the millimeter wave radar according to claim 1, wherein the constructing the environmental two-dimensional raster map of the lidar by using the lidar point cloud comprises:
using a voxel filtering algorithm to carry out down-sampling on the laser radar point cloud, and using an amplitude limiting filtering algorithm to filter water clutter points in the laser radar point cloud so as to obtain an effective laser radar point cloud;
and filling the effective laser radar point cloud into a northeast coordinate system of a grid map corresponding to the laser radar to obtain an environment two-dimensional grid map of the laser radar at the current moment.
6. The mapping method based on the fusion of the lidar and the millimeter wave radar according to claim 5, wherein the step of filling effective lidar point cloud into a north-east coordinate system of a grid map corresponding to the lidar to obtain an environmental two-dimensional grid map of the lidar at the current moment comprises the steps of:
calculating the coordinates of the laser radar point cloud in the laser radar grid map at the current moment according to the current position and the current posture of the unmanned ship;
clearing grid values of a corresponding area of the current laser radar visible range in a laser radar grid map;
and filling the laser radar point cloud into the grid map corresponding to the laser radar according to the calculated coordinate of the laser radar point cloud in the laser radar grid map at the current moment.
7. The mapping device based on the fusion of the laser radar and the millimeter wave radar is characterized by comprising a determining unit, a matching unit, a first constructing unit, a second constructing unit and a fusion unit;
the determining unit is used for determining the current position and the current attitude of the unmanned ship through a positioning system;
the matching unit is used for filtering out millimeter wave radar point cloud clutter points by using laser radar point cloud;
the first construction unit is used for constructing an environment two-dimensional grid map of the millimeter wave radar by using the millimeter wave radar point cloud;
the second construction unit is used for constructing an environment two-dimensional grid map of the laser radar by using the laser radar point cloud;
the fusion unit is used for fusing the constructed environment two-dimensional grid pattern of the millimeter wave radar and the environment two-dimensional grid pattern of the laser radar to obtain a fused grid pattern;
the method comprises the following steps that a northeast coordinate system established by the position of an unmanned ship when the unmanned ship is started for the first time is used as a coordinate origin of an environment two-dimensional grid map of a millimeter wave radar, an environment two-dimensional grid map of a laser radar and a grid map after fusion;
the fusion unit comprises a first acquisition module, a second acquisition module and a fusion module;
the first acquisition module is used for acquiring a two-dimensional grid map of the millimeter wave radar environment at the current moment, and initializing all grids in the current detection range of the laser radar in the two-dimensional grid map of the millimeter wave radar environment into barrier-free grids so as to obtain a processed two-dimensional grid map of the millimeter wave radar environment;
the second acquisition module is used for acquiring an environment two-dimensional grid map of the laser radar at the current moment, and initializing grids at the side and the rear part of the ship to be barrier-free grids so as to obtain a processed environment two-dimensional grid map of the laser radar;
and the fusion module performs grid-by-grid OR operation on the processed laser radar environment two-dimensional grid map and the processed millimeter wave radar environment two-dimensional grid map one by one for fusion.
8. Computer device, comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the steps of the method for mapping based on the fusion of lidar and millimeter wave radar according to any of claims 1 to 6 when executing the computer program.
9. A computer-readable storage medium, characterized in that the storage medium stores a computer program comprising program instructions which, when executed by a processor, cause the processor to carry out the steps of the method of mapping based on lidar and millimeter wave radar fusion according to any one of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210846412.1A CN115079168B (en) | 2022-07-19 | 2022-07-19 | Mapping method, device and equipment based on fusion of laser radar and millimeter wave radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210846412.1A CN115079168B (en) | 2022-07-19 | 2022-07-19 | Mapping method, device and equipment based on fusion of laser radar and millimeter wave radar |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115079168A CN115079168A (en) | 2022-09-20 |
| CN115079168B true CN115079168B (en) | 2022-11-22 |
Family
ID=83260454
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210846412.1A Active CN115079168B (en) | 2022-07-19 | 2022-07-19 | Mapping method, device and equipment based on fusion of laser radar and millimeter wave radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115079168B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115641462B (en) * | 2022-12-26 | 2023-03-17 | 电子科技大学 | Radar image target identification method |
| CN116381674B (en) * | 2023-06-02 | 2023-08-22 | 陕西欧卡电子智能科技有限公司 | Fusion method of water surface laser radar point cloud and millimeter wave Lei Dadian cloud |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111578932A (en) * | 2020-05-28 | 2020-08-25 | 长沙中联重科环境产业有限公司 | Image establishing method, device, medium and equipment based on multi-line laser radar |
| CN111856498A (en) * | 2020-07-30 | 2020-10-30 | 南京超达信息科技有限公司 | System and method for three-dimensional measurement of contour in carriage |
| CN112986982A (en) * | 2021-05-12 | 2021-06-18 | 长沙万为机器人有限公司 | Environment map reference positioning method and device and mobile robot |
| CN113064134A (en) * | 2019-12-16 | 2021-07-02 | 南京理工大学 | Laser radar echo processing method based on improved fuzzy neural network filtering |
| CN113665500A (en) * | 2021-09-03 | 2021-11-19 | 南昌智能新能源汽车研究院 | All-weather-operation environment sensing system and method for unmanned transport vehicle |
| WO2021238306A1 (en) * | 2020-05-25 | 2021-12-02 | 华为技术有限公司 | Method for processing laser point cloud and related device |
| CN114076918A (en) * | 2020-08-20 | 2022-02-22 | 北京万集科技股份有限公司 | Millimeter wave radar, laser radar and camera combined calibration method and device |
| CN114397654A (en) * | 2022-03-24 | 2022-04-26 | 陕西欧卡电子智能科技有限公司 | Unmanned ship obstacle avoidance method based on multi-radar sensing |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111382768B (en) * | 2018-12-29 | 2023-11-14 | 华为技术有限公司 | Multi-sensor data fusion method and device |
| CN112415518B (en) * | 2020-11-20 | 2023-09-26 | 南京理工大学 | Passable space detection method based on vehicle-mounted millimeter wave radar |
-
2022
- 2022-07-19 CN CN202210846412.1A patent/CN115079168B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113064134A (en) * | 2019-12-16 | 2021-07-02 | 南京理工大学 | Laser radar echo processing method based on improved fuzzy neural network filtering |
| WO2021238306A1 (en) * | 2020-05-25 | 2021-12-02 | 华为技术有限公司 | Method for processing laser point cloud and related device |
| CN111578932A (en) * | 2020-05-28 | 2020-08-25 | 长沙中联重科环境产业有限公司 | Image establishing method, device, medium and equipment based on multi-line laser radar |
| CN111856498A (en) * | 2020-07-30 | 2020-10-30 | 南京超达信息科技有限公司 | System and method for three-dimensional measurement of contour in carriage |
| CN114076918A (en) * | 2020-08-20 | 2022-02-22 | 北京万集科技股份有限公司 | Millimeter wave radar, laser radar and camera combined calibration method and device |
| CN112986982A (en) * | 2021-05-12 | 2021-06-18 | 长沙万为机器人有限公司 | Environment map reference positioning method and device and mobile robot |
| CN113665500A (en) * | 2021-09-03 | 2021-11-19 | 南昌智能新能源汽车研究院 | All-weather-operation environment sensing system and method for unmanned transport vehicle |
| CN114397654A (en) * | 2022-03-24 | 2022-04-26 | 陕西欧卡电子智能科技有限公司 | Unmanned ship obstacle avoidance method based on multi-radar sensing |
Non-Patent Citations (5)
| Title |
|---|
| A Survey on Sensors for Autonomous Systems;Linga Reddy Cenkeramaddi;<2020 15th IEEE Conference on Industrial Electronics and Applications (ICIEA)>;20201109;全文 * |
| Are We Ready for Unmanned Surface Vehicles in Inland Waterways? The USVInland Multisensor Dataset and Benchmark;Yuwei Cheng;《IEEE Robotics and Automation Letters 》;20210318;全文 * |
| 多传感器融合的环境建模方法与应用研究;谈守富;《中国硕士学位论文 信息科技辑》;20220115;全文 * |
| 毫米波和激光雷达数据融合的SLAM算法研究;崔巍杰;《中国硕士学位论文 信息科技辑》;20200115;全文 * |
| 毫米波雷达与激光雷达在无人船上的应用;庄加兴等;《船舶工程》;20191125(第11期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115079168A (en) | 2022-09-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115079168B (en) | Mapping method, device and equipment based on fusion of laser radar and millimeter wave radar | |
| CN109598765B (en) | Monocular camera and millimeter wave radar external parameter combined calibration method based on spherical calibration object | |
| CN112051590B (en) | Detection method and related device for laser radar and inertial measurement unit | |
| CN112051591B (en) | Detection method and related device for laser radar and inertial measurement unit | |
| CN109435955B (en) | Performance evaluation method, device and equipment for automatic driving system and storage medium | |
| Cappelle et al. | Virtual 3D city model for navigation in urban areas | |
| WO2021097983A1 (en) | Positioning method, apparatus, and device, and storage medium | |
| CN112558023A (en) | Calibration method and device of sensor | |
| CN112013877B (en) | Detection method and related device for millimeter wave radar and inertial measurement unit | |
| CN114111774B (en) | Vehicle positioning method, system, equipment and computer readable storage medium | |
| CN110243364B (en) | Unmanned aerial vehicle course determining method and device, unmanned aerial vehicle and storage medium | |
| CN115187798A (en) | Multi-unmanned aerial vehicle high-precision matching positioning method | |
| CN110160503B (en) | Unmanned aerial vehicle landscape matching positioning method considering elevation | |
| CN114527490A (en) | Detecting three-dimensional structural models while a vehicle is in motion | |
| Panicker et al. | Multisensor data fusion for an autonomous ground vehicle | |
| CN112836698A (en) | Positioning method, positioning device, storage medium and electronic equipment | |
| CN115183762A (en) | Airport warehouse inside and outside mapping method, system, electronic equipment and medium | |
| Zhang et al. | Online ground multitarget geolocation based on 3-D map construction using a UAV platform | |
| Deraz et al. | Deep learning based on LSTM model for enhanced visual odometry navigation system | |
| CN112130151A (en) | Arc synthetic aperture ground radar coordinate projection rapid calculation method | |
| CN112255604A (en) | Method and device for judging accuracy of radar data and computer equipment | |
| CN115932823A (en) | Aircraft ground target positioning method based on heterogeneous region feature matching | |
| CN113589848B (en) | Multi-unmanned aerial vehicle detection, positioning and tracking system and method based on machine vision | |
| CN116400349A (en) | Calibration method of low-resolution millimeter wave radar and optical camera | |
| CN115629374A (en) | Unmanned ship under-bridge positioning method based on millimeter wave radar and related equipment |
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 |