CN117705119A - Navigation positioning method, equipment and storage medium for dynamic loading of large-scene 3D map - Google Patents
Navigation positioning method, equipment and storage medium for dynamic loading of large-scene 3D map Download PDFInfo
- Publication number
- CN117705119A CN117705119A CN202311727414.XA CN202311727414A CN117705119A CN 117705119 A CN117705119 A CN 117705119A CN 202311727414 A CN202311727414 A CN 202311727414A CN 117705119 A CN117705119 A CN 117705119A
- Authority
- CN
- China
- Prior art keywords
- map
- navigation
- loading
- sub
- point cloud
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 239000002245 particle Substances 0.000 claims description 41
- 238000004422 calculation algorithm Methods 0.000 claims description 22
- 238000001914 filtration Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 10
- 230000004888 barrier function Effects 0.000 claims description 5
- 238000013507 mapping Methods 0.000 claims description 4
- 238000012952 Resampling Methods 0.000 claims description 3
- 230000008447 perception Effects 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 2
- 239000002131 composite material Substances 0.000 claims 1
- 238000002360 preparation method Methods 0.000 claims 1
- 238000012795 verification Methods 0.000 abstract description 3
- 238000004891 communication Methods 0.000 description 8
- 238000007689 inspection Methods 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 5
- 230000009471 action Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000006835 compression Effects 0.000 description 2
- 238000007906 compression Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 238000012549 training Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000000802 evaporation-induced self-assembly Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000036316 preload Effects 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000000638 solvent extraction Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D10/00—Energy efficient computing, e.g. low power processors, power management or thermal management
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a navigation positioning method, equipment and a storage medium for dynamically loading a large-scene 3D map, which comprise the following steps of constructing a 3D cloud map; identifying a feasible region and an infeasible region; dividing the map according to the navigation points; preloading, loading and releasing the sub map according to the real-time pose of the robot; repositioning and updating the map continues to perform tasks. When the large scene environment map is dynamically loaded, the method adopts a dynamic block loading mode, the delay time of loading the map is reduced by setting the buffer area pre-loading sub-map, the purpose of real-time loading is realized, the actual application requirement is met, and the method can be suitable for the large scene environment with the area of about 100 ten thousand square meters through verification.
Description
Technical Field
The invention relates to the technical field of positioning, in particular to a navigation positioning method, equipment and a storage medium for dynamically loading a large-scene 3D map.
Background
With the development of SLAM (Simultaneous Localization and Mapping) technology, a robot can complete self-positioning and map construction in an unknown environment through a portable sensor such as a laser radar, a high-definition camera, an IMU (inertial measurement unit) and the like, a navigation function is realized based on a generated scene map, and the autonomous navigation capability of the robot is widely applied to the intelligent inspection field to replace manual work to complete more complex and high-risk inspection operation.
For map files established by SLAM technology in a large scene environment, loading the map to realize positioning navigation is not only time-consuming and serious, but also occupies a large amount of system memory, and has high performance requirements on an industrial personal computer, so that the map files need to be processed. In practical application, in order to provide position and environment information for a robot to realize efficient large-scene map loading, the current common optimization method mainly comprises the following steps:
1. laser point cloud map compression: the storage space of map data is reduced by adopting a compression technology of extracting semantic features and constructing a semantic map, so that the loading speed is improved, but a large amount of data is required for training a corresponding network model, otherwise, the predefining of semantic information for training possibly does not occur, so that the map features are sparse, and the generalization performance of the method is poor;
2. caching technology: the loaded map data is stored in the cache to reduce the time and resource consumption of repeated loading, but the cache scheme cannot control the delay of loading the map, and the real-time performance of navigation positioning is affected.
Disclosure of Invention
The navigation positioning method, the navigation positioning equipment and the storage medium for dynamically loading the large-scene 3D map can at least solve one of the technical problems in the background technology.
In order to achieve the above purpose, the present invention adopts the following technical scheme:
a navigation positioning method for dynamically loading a large scene 3D map comprises the following steps executed by computer equipment,
comprises the steps of,
constructing a 3D cloud map;
identifying a feasible region and an infeasible region;
dividing the map according to the navigation points;
preloading, loading and releasing the sub map according to the real-time pose of the robot;
repositioning and updating the map continues to perform tasks.
Further, the method comprises the following steps,
step one, constructing a 3D point cloud map of a field environment;
step two, preprocessing the 3D point cloud map;
step three, converting the processed 3D point cloud map into a 2D grid map;
step four, adding n navigation points g in the 2D grid map 1 ,g 2 ,…,g n Setting a threshold value threshold, dividing a buffer area, a loading map area and a direct navigation area for the divided sub-map, performing preloading, loading map judgment and direct navigation, dividing the whole map into blocks by taking the added navigation points as a map center and recording sub-map numbers map corresponding to each navigation point by taking twice of the laser radar scanning range R and the threshold value threshold sum as map side length map_size 1 ,map 2 ,…,map n ；
Step five, planning a path according to the navigation points, acquiring the navigation sequence of each navigation point through a path planning algorithm, and determining the loading sequence of the map according to the map numbers of the sub-maps corresponding to the navigation points;
step six, reloading the pcd point cloud map and initializing a voxel map;
step seven, scanning a map generated by a scene environment, and identifying a feasible region and an infeasible region;
step eight, calculating the distance of the nearest obstacle of each voxel grid;
step nine, real-time positioning based on a priori map adopts a positioning strategy of a Monte Carlo particle filter algorithm, positioning of the Monte Carlo particle filter algorithm is based on given priori map information and a perception state quantity, a group of random particles with weights are sampled according to the state quantity to represent posterior probability of pose estimation, and the current state quantity is calculated and corrected according to the particles and the weight;
the particle filtering positioning algorithm adopts a mode of combining a beam model and a likelihood domain model, performs particle filtering on the real-time point cloud data frame through two model algorithms to obtain respective weights and poses, and then performs weighted calculation according to the respective weights to perform resampling to obtain final poses;
step ten, just start to acquire initial pose_phase, judging sub map number map of the initial position of the robot i The loading includes map i Map of sub map inside i 、map i+2 、map i+3 ；
Step eleven, starting navigation, positioning by using a particle filtering algorithm, selecting a mode of loading a point cloud map and a distance voxel map by a particle weight calculation part, directly searching a nearest obstacle distance value stored in a file, directly loading and opening the file for weight calculation if the voxelMap_dis.txt file exists under a specified path, and creating and generating the voxelMap_dis.txt file if the voxelMap_dis.txt file does not exist;
step twelve, sub map i+3 Loaded and map i Release is completed, voxelmap_dis generated with greater resolution of the entire map 0 Repositioning the txt file to obtain the current pose of the robot, and mapping the current pose of the robot to the map of the sub-map of the robot i+1 Corresponding voxelmap_dis i+1 Calculating particle weight by the txt file, and positioning and navigating by using particle filtering;
step thirteen, update sub map i 、map i+1 And map i+2 And repeating the step eleven to the step twelve, and continuously executing the task until the task is ended.
In yet another aspect, the invention also discloses a computer readable storage medium storing a computer program which, when executed by a processor, causes the processor to perform the steps of the method as described above.
In yet another aspect, the invention also discloses a computer device comprising a memory and a processor, the memory storing a computer program which, when executed by the processor, causes the processor to perform the steps of the method as above.
Compared with the prior art, the navigation positioning method for dynamically loading the large-scene 3D map has the beneficial effects that:
1. when the large scene environment map is dynamically loaded, the method adopts a dynamic block loading mode, the delay time of loading the map is reduced by setting the buffer area pre-loading sub-map, the purpose of real-time loading is realized, the actual application requirement is met, and the method can be suitable for the large scene environment with the area of about 100 ten thousand square meters through verification.
2. According to the invention, the distribution characteristics of the point cloud map are considered, the point cloud map is processed by identifying the feasible region and the infeasible region, the space occupation of the invalid point cloud is removed, the complexity of the point cloud map is further reduced, and the size of the map file is reduced.
3. When Monte Carlo particle filter positioning is used, the distance value of the nearest barrier to each voxel grid is calculated and recorded in the txt text file, so that direct searching is facilitated when the weight is calculated in the subsequent particle filter positioning, the time for calculating the particle weight is reduced, and the positioning efficiency is improved.
Drawings
FIG. 1 is a flow chart of a system according to an embodiment of the present invention;
FIG. 2 is a map dynamic loading mechanism diagram according to an embodiment of the present invention;
figures 3a, 3b, 3c are based on path planningMap partitioning dynamic loading schematic diagram; wherein FIG. 3a is a map of a robot on a sub-map i A direct navigation area, and not in the buffer area and the loading map area; FIG. 3b is a map of a robot on a sub-map i A buffer region; FIG. 3c is a map of a robot on a sub-map i+1 Loading the map area.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention.
As shown in fig. 1 and fig. 2, the embodiment of the invention provides a navigation positioning method based on dynamic loading of a large scene 3D map, which specifically comprises the following steps:
step one, constructing a 3D point cloud map of a field environment, wherein the process can select a related SLAM algorithm according to a sensor carried by a robot;
step two, preprocessing the map, wherein uneven distribution of the point density of the obtained dense point cloud can mislead a point cloud matching algorithm, and a plurality of points in the established point cloud file are overlapped, so that the volume of the point cloud file can be reduced by adopting proper resolution downsampling, and the transmission and loading are convenient. The principle of the point cloud downsampling method is that three-dimensional space data are gridded, namely voxelized, each voxel is checked, and if points exist in the voxel, the center coordinates of the voxel or the expectation of the coordinates of the point sets are used for replacing the point sets in the voxel;
and thirdly, path planning is carried out according to the two-dimensional grid map during navigation, so that the processed 3D point cloud map is required to be converted into a 2D grid map. The principle of map conversion is that the received point cloud information is distributed in the form of a "/map" topic, the map_server is used for receiving the "/map" topic, a 2D grid map is generated, map.pgm and map.yaml files used for navigation are stored, specific information of the 2D grid map is stored in the pgm-format file, and information such as paths, resolution, initial positions of robots and the like of the pgm file are stored in the yaml file and are used for determining the positions of the robots;
step four, adding n navigation points g in the 2D grid map 1 ,g 2 ,…,g n Setting a threshold value threshold, dividing a buffer area, a loading map area and a direct navigation area for the divided sub-map, performing preloading, loading map judgment and direct navigation, dividing the whole map into blocks by taking the added navigation points as a map center and recording sub-map numbers map corresponding to each navigation point by taking twice of the laser radar scanning range R and the threshold value threshold sum as map side length map_size 1 ,map 2 ,…,map n ；
Step five, planning a path according to the navigation points, acquiring the navigation sequence of each navigation point through a path planning algorithm, and determining the loading sequence of the map according to the map numbers of the sub-maps corresponding to the navigation points;
loading a pcd point cloud map, removing NaN points in the point cloud, setting the point cloud into an octree structure, obtaining maximum and minimum values of the pcd point cloud map in the x, y and z directions, and calculating the map side lengths of the whole pcd map in the x, y and z directions according to the set resolution of the voxel map. The voxel map is initialized by dividing the voxel map into z=0 or more and z=0 or less.
And step seven, a map generated by a scene environment is scanned, only a feasible region where laser energy is hit is considered, and for a region where laser energy is not hit, such as a region behind a wall, the infeasible region has no great effect on scanning matching and positioning, occupies a large amount of storage space and is unfavorable for realizing map loading, so that the feasible region and the infeasible region need to be identified. After the initialization of the voxel map is completed, the feasible region and the infeasible region are identified, under the condition that the navigation point position sequence is known, each voxel grid occupied by a planned path is obtained according to the path, and two adjacent navigation points g are connected i And g i+1 Where i=1, 2, …, n. Taking the center of a voxel grid occupied by connecting two adjacent navigation points as a path planning foot to make a vertical line perpendicular to the connecting line, and marking the first occupied grid serial number of the vertical line contacted with two sides of the footAll the marked occupied grids are boundaries of the feasible region and the infeasible region. And after the pcd point cloud map is processed into a feasible area and an infeasible area, only the grid occupation of the boundary is reserved, and the pcd point cloud map of the whole environment is updated.
And step eight, reloading the point cloud map as in step six, and initializing the voxel map. Calculating the distance of the nearest obstacle of each voxel grid, storing the searched distance value of the nearest obstacle of the current voxel grid in an array by adopting an octree neighbor searching method, and providing a scheme for creating a voxelMap_dis.txt text file, storing the distance value in the array and writing the distance value in the text file. Generating voxelmap_dis for relocation with greater resolution for whole map 0 Txt file, while generating voxelMap_dis for particle filter positioning in navigation process with smaller resolution for sub-map of each navigation point corresponding to segmentation i Txt (i=1, 2, …, n) file;
step nine, real-time positioning based on a priori map adopts a positioning strategy of a Monte Carlo particle filter (MCL) algorithm, MCL positioning is based on given priori map information and a perception state quantity, a group of weighted random particles are sampled according to the state quantity to represent posterior probability of pose estimation, and the corrected current state quantity is calculated according to the particles and the weight, so that the model and Gaussian assumption limit is avoided.
The particle filtering positioning algorithm adopts a mode of combining a beam model and a likelihood domain model, performs particle filtering on the real-time point cloud data frame through two model algorithms to obtain respective weights and poses, and then performs weighted calculation according to the respective weights to perform resampling to obtain final poses;
step ten, just start to acquire initial pose_phase, judging sub map number map of the initial position of the robot i AddingThe carrier includes map i Map of sub map inside i 、map i+2 、map i+3 ；
Step eleven, starting navigation, positioning by using a particle filtering algorithm, wherein a particle weight calculation part can select a mode of loading a point cloud map and a distance voxel map and directly searching a nearest obstacle distance value stored in a file, directly loading and opening the file for weight calculation if the voxelMap_dis.txt file exists in a specified path, and creating and generating the voxelMap_dis.txt file if the voxelMap_dis.txt file does not exist in the specified path; and only the point cloud map can be selected to be loaded, and the distance value of the nearest barrier in the grid where the particle is currently located is obtained in real time by an octree neighbor search method, so that the reading, writing and loading operations of the earlier-stage file are omitted. The whole navigation process judges that the robot is positioned according to the real-time pose of the robot: 1. the method can directly navigate, wherein the method needs to preload the map, and the method needs to directly load and release the map, and the sub-map is divided by taking the navigation point as the center of the map, and the path is known, so that only the current position of the robot is considered. Case one: map of current sub-map of robot i Is not in the buffer area and the loading map area, wherein i=1, 2, …, n, then the navigation is direct, as shown in the diagram of fig. 3 a; and a second case: the robot runs to the map of the current sub-map i In the buffer area, the map is preloaded according to the navigation sequence obtained by path planning while navigating i+3 A corresponding sub-map, as shown in fig. 3 b; and a third case: the robot moves out of the map of the current sub-map i And map i+1 Buffer area in (a) and enter sub map i+1 Is used for loading the preloaded sub map while navigating i+3 And releasing sub map i As shown in fig. 3 c;
step twelve, sub map i+3 Loaded and map i Release is completed, voxelmap_dis generated with greater resolution of the entire map 0 Repositioning the txt file to obtain the current pose of the robot, and mapping the current pose of the robot to the map of the sub-map of the robot i+1 Corresponding voxelmap_dis i+1 Calculating particle weight from txt file, and determining by particle filteringNavigation of the position;
step thirteen, update sub map i 、map i+1 And map i+2 And repeating the step eleven to the step twelve, and continuously executing the task until the task is ended.
The following examples are given:
example 1:
the navigation positioning method and system for dynamically loading the large-scene 3D map described in the embodiment are based on a wheel type inspection robot, and comprise the following steps:
firstly, constructing a 3D point cloud map of a field environment, controlling the robot to move according to sensors (laser radar, IMU and the like) carried by the robot, and generating the 3D point cloud map by selecting related SLAM algorithms such as FASTER-LIO, ORB-SLAM3, R3LIVE and the like;
step two, removing NaN points in the point cloud, and downsampling the 3D point cloud map with the resolution of 0.2 to obtain a new point cloud map map_pcd;
step three, converting the point cloud map map_pcd into a 2D grid map;
loading a 2D grid map, and adding n inspection point bits g for navigation into the grid map 1 ,g 2 ,…,g n ；
Step five, planning a path by using a Dijkstra algorithm according to the added navigation points;
step six, loading a point cloud map_pcd, setting the resolution octree_resolution of the octree map to be 0.2, setting the resolution voxel_resolution of the distance voxel map to be 0.1, and initializing the voxel map;
step seven, identifying a feasible region and an infeasible region in the map, reserving boundary voxel grid occupation, and updating a point cloud map of the whole environment into map_pcd_new;
step eight, setting the larger resolution as 0.5, and downsampling a map_pcd_new point cloud map to obtain a pcd point cloud map file map_pcd0;
step nine, reading a map_pcd0 file, setting the resolution octree_resolution of the octree map to 0.2, and setting the resolution voxel_resolution of the voxel map to 0.1, and initializing the resolution octree_resolution with the size of a z-axis valueConverting the voxel map with z=0 or more and the voxel map with z=0 or less, calculating the distance of the nearest obstacle to each voxel grid by using octree neighbor search algorithm, and generating the voxel map_dis for repositioning only 0 Txt file;
step ten, setting the smaller resolution as 0.2, and downsampling the map_pcd_new point cloud map again to obtain a point cloud map file map_pcd1 with the smaller resolution;
step eleven, setting a buffer area with threshold=15 for pre-loading map judgment, setting a loading map area with threshold=15 for loading map judgment, dividing the whole map into n sub-maps with each inspection point as a map center by taking twice of the sum of the laser radar scanning range r=120 and the threshold value threshold=15 as sub-map side length map_size, and recording the map number map of each sub-map 1 ,map 2 ,…,map n . Judging whether the side length of the sub map exceeds the whole map boundary in the map blocking process, and taking the boundary as the boundary of the sub map which is currently divided if the side length of the sub map exceeds the whole map boundary;
step twelve, as in step nine, reading pcd map files corresponding to each inspection point, and generating txt file voxelMap_dis for particle filtering positioning corresponding to each sub map 1 .txt,voxelMap_dis 2 .txt,…,voxelMap_dis n .txt；
Thirteenth, acquiring an initial position of the robot, and loading a map including sub-maps to which the initial position belongs i Map of the inside i 、map i+2 、map i+3 Three maps and corresponding voxelmap_dis i .txt、voxelMap_dis i+1 Txt and voxelmap_dis i+2 .txt；
Fourteen, issuing a patrol task, wherein the patrol task is point-by-point navigation, and the navigation sequence, namely the loading sequence of the sub map, of the robot in the whole process of navigating from the current point to the target point can be obtained by combining path planning;
fifteen, starting navigation positioning to perform tasks, acquiring the pose of the robot in real time, judging whether the map needs to be preloaded, and when the robot is at the map i Is not needed, a pre-loaded map is not needed,where i=1, 2, …, n; when the robot runs to the sub map i Pre-loading sub-map i+3 And the corresponding voxelmap_dis i+3 Txt file; map when robot enters sub map i+1 In the loading map area of (2), loading the sub-map i+3 And the corresponding voxelmap_dis i+3 Txt file, releasing sub map simultaneously i And the corresponding voxelmap_dis i Txt file.
Sixteen sub map i+3 And the corresponding voxelmap_dis i+3 Txt file loading is complete and sub map i And the corresponding voxelmap_dis i Txt file release is completed, voxelmap_dis 0 Txt repositioning the robot;
seventeenth, iteratively updating the sub map, repeatedly executing fifteen and sixteenth, and continuously executing the inspection task until the task is finished.
In summary, the embodiment of the invention has the following advantages:
1. when the large scene environment map is dynamically loaded, the method adopts a dynamic block loading mode, the delay time of loading the map is reduced by setting the buffer area pre-loading sub-map, the purpose of real-time loading is realized, the actual application requirement is met, and the method can be suitable for the large scene environment with the area of about 100 ten thousand square meters through verification.
2. According to the invention, the distribution characteristics of the point cloud map are considered, the point cloud map is processed by identifying the feasible region and the infeasible region, the space occupation of the invalid point cloud is removed, the complexity of the point cloud map is further reduced, and the size of the map file is reduced.
3. When Monte Carlo particle filter positioning is used, the distance value of the nearest barrier to each voxel grid is calculated and recorded in the txt text file, so that direct searching is facilitated when the weight is calculated in the subsequent particle filter positioning, the time for calculating the particle weight is reduced, and the positioning efficiency is improved.
In yet another aspect, the invention also discloses a computer readable storage medium storing a computer program which, when executed by a processor, causes the processor to perform the steps of the method as described above.
In yet another aspect, the invention also discloses a computer device comprising a memory and a processor, the memory storing a computer program which, when executed by the processor, causes the processor to perform the steps of the method as above.
In yet another embodiment provided herein, there is also provided a computer program product containing instructions that, when run on a computer, cause the computer to perform the navigation positioning method of dynamic loading of a large scene 3D map of any of the above embodiments.
It may be understood that the system provided by the embodiment of the present invention corresponds to the method provided by the embodiment of the present invention, and explanation, examples and beneficial effects of the related content may refer to corresponding parts in the above method.
The embodiment of the application also provides an electronic device, which comprises a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory are communicated with each other through the communication bus,
a memory for storing a computer program;
and the processor is used for realizing the navigation positioning method for dynamically loading the large scene 3D map when executing the program stored in the memory.
The communication bus mentioned by the above electronic device may be a peripheral component interconnect standard (english: peripheral Component Interconnect, abbreviated: PCI) bus or an extended industry standard architecture (english: extended Industry Standard Architecture, abbreviated: EISA) bus, or the like. The communication bus may be classified as an address bus, a data bus, a control bus, or the like.
The communication interface is used for communication between the electronic device and other devices.
The Memory may include random access Memory (Random Access Memory, abbreviated as RAM) or nonvolatile Memory (NVM), such as at least one disk Memory. Optionally, the memory may also be at least one memory device located remotely from the aforementioned processor.
The processor may be a general-purpose processor, including a central processing unit (Central Processing Unit, CPU for short), a network processor (Network Processor, NP for short), etc.; it may also be a digital signal processor (English: digital Signal Processing; DSP; for short), an application specific integrated circuit (English: application Specific Integrated Circuit; ASIC; for short), a Field programmable gate array (English: field-Programmable Gate Array; FPGA; for short), or other programmable logic device, discrete gate or transistor logic device, discrete hardware components.
In the above embodiments, it may be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented in software, may be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When loaded and executed on a computer, produces a flow or function in accordance with embodiments of the present application, in whole or in part. The computer may be a general purpose computer, a special purpose computer, a computer network, or other programmable apparatus. The computer instructions may be stored in or transmitted from one computer-readable storage medium to another, for example, by wired (e.g., coaxial cable, optical fiber, digital Subscriber Line (DSL)), or wireless (e.g., infrared, wireless, microwave, etc.). The computer readable storage medium may be any available medium that can be accessed by a computer or a data storage device such as a server, data center, etc. that contains an integration of one or more available media. The usable medium may be a magnetic medium (e.g., floppy Disk, hard Disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid State Disk (SSD)), etc.
It is noted that relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
In this specification, each embodiment is described in a related manner, and identical and similar parts of each embodiment are all referred to each other, and each embodiment mainly describes differences from other embodiments. In particular, for system embodiments, since they are substantially similar to method embodiments, the description is relatively simple, as relevant to see a section of the description of method embodiments.
The above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims (10)
1. The navigation positioning method for dynamically loading the large-scene 3D map is characterized by comprising the following steps of,
constructing a 3D cloud map;
identifying a feasible region and an infeasible region;
dividing the map according to the navigation points;
preloading, loading and releasing the sub map according to the real-time pose of the robot;
repositioning and updating the map continues to perform tasks.
2. The navigation positioning method for dynamic loading of a large scene 3D map according to claim 1, wherein: comprises the steps of,
step one, constructing a 3D point cloud map of a field environment;
step two, preprocessing the 3D point cloud map;
step three, converting the processed 3D point cloud map into a 2D grid map;
step four, adding n navigation points g in the 2D grid map 1 ,g 2 ,…,g n Setting a threshold value threshold, dividing a buffer area, a loading map area and a direct navigation area for the divided sub-map, performing preloading, loading map judgment and direct navigation, dividing the whole map into blocks by taking the added navigation points as a map center and recording sub-map numbers map corresponding to each navigation point by taking twice of the laser radar scanning range R and the threshold value threshold sum as map side length map_size 1 ,map 2 ,…,map n ；
Step five, planning a path according to the navigation points, acquiring the navigation sequence of each navigation point through a path planning algorithm, and determining the loading sequence of the map according to the map numbers of the sub-maps corresponding to the navigation points;
step six, reloading the pcd point cloud map and initializing a voxel map;
step seven, scanning a map generated by a scene environment, and identifying a feasible region and an infeasible region;
step eight, calculating the distance of the nearest obstacle of each voxel grid;
step nine, real-time positioning based on a priori map adopts a positioning strategy of a Monte Carlo particle filter algorithm, positioning of the Monte Carlo particle filter algorithm is based on given priori map information and a perception state quantity, a group of random particles with weights are sampled according to the state quantity to represent posterior probability of pose estimation, and the current state quantity is calculated and corrected according to the particles and the weight;
the particle filtering positioning algorithm adopts a mode of combining a beam model and a likelihood domain model, performs particle filtering on the real-time point cloud data frame through two model algorithms to obtain respective weights and poses, and then performs weighted calculation according to the respective weights to perform resampling to obtain final poses;
step ten, just start to acquire initial pose_phase, judging sub map number map of the initial position of the robot i The loading includes map i Map of sub map inside i 、map i+2 、map i+3 ；
Step eleven, starting navigation, positioning by using a particle filtering algorithm, selecting a mode of loading a point cloud map and a distance voxel map by a particle weight calculation part, directly searching a nearest obstacle distance value stored in a file, directly loading and opening the file for weight calculation if the voxelMap_dis.txt file exists under a specified path, and creating and generating the voxelMap_dis.txt file if the voxelMap_dis.txt file does not exist;
step twelve, sub map i+3 Loaded and map i Release is completed, voxelmap_dis generated with greater resolution of the entire map 0 Repositioning the txt file to obtain the current pose of the robot, and mapping the current pose of the robot to the map of the sub-map of the robot i+1 Corresponding voxelmap_dis i+1 Calculating particle weight by the txt file, and positioning and navigating by using particle filtering;
step thirteen, update sub map i 、map i+1 And map i+2 And repeating the step eleven to the step twelve, and continuously executing the task until the task is ended.
3. The navigation positioning method for dynamic loading of a large scene 3D map according to claim 2, characterized in that: and thirdly, converting the processed 3D point cloud map into a 2D grid map, wherein the method comprises the steps of distributing the received point cloud information in the form of a "/map" topic, receiving the "/map" topic by using a map_server, generating the 2D grid map, and storing map.pgm and map.yaml files for navigation, wherein specific information of the 2D grid map is stored in the pgm file, and the yaml file stores the information of the path, the resolution and the initial position of the pgm file and is used for determining the position of the robot.
4. The navigation positioning method for dynamic loading of a large scene 3D map according to claim 2, characterized in that:
loading a pcd point cloud map, removing NaN points in the point cloud, setting the point cloud into an octree structure, obtaining maximum and minimum values of the pcd point cloud map in the x, y and z directions, and calculating the map side lengths of the whole pcd map in the x, y and z directions according to the set resolution of the voxel map; the voxel map is initialized by dividing the voxel map into z=0 or more and z=0 or less.
5. The navigation positioning method for dynamic loading of a large scene 3D map according to claim 2, characterized in that: step seven, after the initialization of the voxel map is completed, the feasible area and the infeasible area are identified, under the condition that the navigation point position sequence is known, each voxel grid occupied by the path is obtained according to the planned path, and two adjacent navigation points g are connected i And g i+1 Where i=1, 2, …, n; taking the center of a voxel grid occupied by a path planning connecting line of two adjacent navigation points as a vertical leg, making a vertical line perpendicular to the connecting line, marking a first occupied grid serial number contacted with the vertical line relative to two sides of the vertical leg, and marking all occupied grids as boundaries of a feasible region and an infeasible region; and after the pcd point cloud map is processed into a feasible area and an infeasible area, only the grid occupation of the boundary is reserved, and the pcd point cloud map of the whole environment is updated.
6. The navigation positioning method for dynamic loading of a large scene 3D map according to claim 2, characterized in that: in the eighth step, a neighbor searching method of octree is adopted, the searched distance value of the barrier closest to the current voxel grid is stored in an array, a voxelMap_dis.txt text file is created, and the distance value is stored in the arrayIn a text file, directly searching distance values through the text file during the calculation of subsequent particle filter positioning weights, and generating a voxelMap_dis for repositioning the whole map with a larger resolution 0 Txt file, while generating voxelMap_dis for particle filter positioning in navigation process with smaller resolution for sub-map of each navigation point corresponding to segmentation i Txt (i=1, 2, …, n) file.
7. The navigation positioning method for dynamic loading of a large scene 3D map according to claim 2, characterized in that: the eleventh step can also select to load only the point cloud map, and acquire the distance value of the nearest barrier in the grid where the particle is currently located in real time through an octree neighbor search method, thereby omitting the reading, writing and loading operations of the earlier-stage file;
the whole navigation process judges that the robot is positioned according to the real-time pose of the robot: a. the method can directly navigate, b, a pre-loaded map, c and a direct loaded release map, and only considers the current position current_phase of the robot because the sub-map is divided by taking the navigation point as the center of the map and the path is known; in particular to the preparation method of the composite material,
case one: map of current sub-map of robot i Is not in the buffer area and the loading map area, wherein i=1, 2, …, n, then the navigation is direct;
and a second case: the robot runs to the map of the current sub-map i In the buffer area, the map is preloaded according to the navigation sequence obtained by path planning while navigating i+3 A corresponding sub-map;
and a third case: the robot moves out of the map of the current sub-map i And map i+1 Buffer area in (a) and enter sub map i+1 Is used for loading the preloaded sub map while navigating i+3 And releasing sub map i 。
8. The navigation positioning method for dynamic loading of a large scene 3D map according to claim 2, characterized in that: step one, constructing a 3D point cloud map of a field environment, controlling the robot to move according to a sensor carried by the robot, and selecting a related SLAM algorithm including FASTER-LIO, ORB-SLAM3 and R3LIVE to generate the 3D point cloud map.
9. A computer readable storage medium storing a computer program which, when executed by a processor, causes the processor to perform the steps of the method of any one of claims 1 to 8.
10. A computer device comprising a memory and a processor, the memory storing a computer program that, when executed by the processor, causes the processor to perform the steps of the method of any of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311727414.XA CN117705119A (en) | 2023-12-14 | 2023-12-14 | Navigation positioning method, equipment and storage medium for dynamic loading of large-scene 3D map |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311727414.XA CN117705119A (en) | 2023-12-14 | 2023-12-14 | Navigation positioning method, equipment and storage medium for dynamic loading of large-scene 3D map |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117705119A true CN117705119A (en) | 2024-03-15 |
Family
ID=90151091
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311727414.XA Pending CN117705119A (en) | 2023-12-14 | 2023-12-14 | Navigation positioning method, equipment and storage medium for dynamic loading of large-scene 3D map |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117705119A (en) |
-
2023
- 2023-12-14 CN CN202311727414.XA patent/CN117705119A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6745328B2 (en) | Method and apparatus for recovering point cloud data | |
| JP7166383B2 (en) | Method and apparatus for creating high-precision maps | |
| US20190370989A1 (en) | Method and apparatus for 3-dimensional point cloud reconstruction | |
| WO2019062649A1 (en) | Adaptive region division method and system | |
| US11534917B2 (en) | Methods, systems, articles of manufacture and apparatus to improve resource utilization for binary tree structures | |
| CN112669463A (en) | Method for reconstructing curved surface of three-dimensional point cloud, computer device and computer-readable storage medium | |
| US11860846B2 (en) | Methods, systems and apparatus to improve spatial-temporal data management | |
| CN113838005B (en) | Intelligent identification and three-dimensional reconstruction method and system for rock mass fracture based on dimension conversion | |
| CN113192200B (en) | Method for constructing urban real scene three-dimensional model based on space-three parallel computing algorithm | |
| US11238641B2 (en) | Architecture for contextual memories in map representation for 3D reconstruction and navigation | |
| CN112734931B (en) | Method and system for assisting point cloud target detection | |
| WO2022087916A1 (en) | Positioning method and apparatus, and electronic device and storage medium | |
| CN115457492A (en) | Target detection method and device, computer equipment and storage medium | |
| CN111445472A (en) | Laser point cloud ground segmentation method and device, computing equipment and storage medium | |
| CN115512175A (en) | Model training method, point cloud data processing device, point cloud data processing equipment and storage medium | |
| CN112233231B (en) | Urban three-dimensional live-action roaming method and system based on cloud computing | |
| CN111812670B (en) | Single photon laser radar space transformation noise judgment and filtering method and device | |
| CN117705119A (en) | Navigation positioning method, equipment and storage medium for dynamic loading of large-scene 3D map | |
| Lu et al. | Image-based 3D reconstruction for Multi-Scale civil and infrastructure Projects: A review from 2012 to 2022 with new perspective from deep learning methods | |
| De Geyter et al. | Automated training data creation for semantic segmentation of 3D point clouds | |
| CN114398455A (en) | Heterogeneous multi-robot cooperative SLAM map fusion method | |
| Meng et al. | Precise determination of mini railway track with ground based laser scanning | |
| CN112101538A (en) | Graph neural network hardware computing system and method based on memory computing | |
| CN117078825B (en) | Rendering modification method, system, equipment and medium on point cloud data line | |
| Peng et al. | PL-Net: towards deep learning-based localization for underwater terrain |
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 |