SLAM method and device under semi-dynamic environment changes

Abstract

The present invention discloses a SLAM method and device under semi-dynamic environmental changes, which relates to the field of positioning and mapping, and includes the following steps: obtaining environmental data, constructing a priori maps based on the environmental data, obtaining the environmental data currently obtained by the positioning object and the environmental data corresponding to the priori map for local matching when starting positioning, and judging whether the matching is successful. If the matching is successful, the position and posture of the positioning object at the current position are updated to successfully locate the object. If the matching fails, the next step is entered to update the dynamic map, and the environmental data currently obtained by the positioning object is globally matched with the priori map to judge whether the matching is successful. Main technical solutions and effects: When encountering a local matching failure, the map can be automatically updated to ensure that the map can accurately reflect the actual changes in the environment. The system can update or expand the map to make the positioning result more accurate. At the same time, these semi-static objects are identified and processed through a dynamic update strategy to avoid mismatching caused by these objects.

Description

Technical Field

[0001] The present invention relates to the field of positioning and mapping, and in particular to a SLAM method and device under semi-dynamic environmental changes. Background Art

[0002] With the rapid development of the economy and technology, the demand for mobile robots in various industries continues to rise. Simultaneous Localization and Mapping (SLAM) technology, a core challenge in the mobile robot technology system, undertakes the critical task of solving the real-time positioning and mapping of robots in unknown environments. Currently, there is strong demand for AGV robots in China, with demand in a relatively concentrated area. AGVs are increasingly widely used in industries such as manufacturing and logistics. However, in dynamic environments (such as vehicles in parking lots, stacked materials, temporary sheds, and temporary fences), when positioning based on a priori maps, dynamic objects can cause deviations between current observations and the priori map, leading to positioning drift or failure.

[0003] Traditional SLAM positioning methods are generally based on the assumption of a static environment, using lidar or visual sensors to match geometric features and construct a grid map or feature point cloud map as a spatial reference. Existing dynamic SLAM strategies often use a single-cycle processing framework: early studies used extended Kalman filters (EKFs) or particle filters (PFs) to establish probabilistic motion models for dynamic targets, using time-series observation data to suppress short-term dynamic interference. Current mainstream improvements enhance system robustness through multimodal sensor fusion (such as tightly coupling an IMU (inertial measurement unit), a device used to measure and report information such as acceleration, angular velocity, and magnetic field strength of an object) with a visual odometry) or deep learning-based semantic segmentation networks (for identifying pedestrians, vehicles, etc.). However, such methods face fundamental limitations in long-term operation scenarios. Semi-static objects (such as temporarily stacked materials) lack continuous motion characteristics, making them difficult to effectively identify using existing dynamic detection mechanisms. Existing methods for detecting and processing dynamic objects have difficulty dealing with semi-static objects (such as temporarily parked vehicles, stacked materials, temporary sheds, temporary fences, etc.). In addition, existing solutions modify the original map. If the environment changes significantly, it will affect the accuracy of the original map. This impact is irreversible and will have long-term consequences. Summary of the Invention

[0004] Purpose of the Invention: This invention aims to address the existing technical problem that semi-static objects (such as temporarily stored materials) lack continuous motion and are difficult to effectively identify using existing dynamic detection mechanisms. Existing detection and processing methods for dynamic objects are difficult to address. The present invention provides a method, apparatus, device, and medium for correcting geometric distortion in MRI images.

[0005] One or more embodiments of this specification also relate to a device for correcting geometric distortion of MRI images, an electronic device, a computer-readable storage medium, and a computer program product to address technical deficiencies in the prior art.

[0006] Technical Solution

[0007] In a first aspect, the present application proposes a SLAM method under semi-dynamic environmental changes, comprising the steps of:

[0008] Step 1: Obtain environmental data and build a priori map based on the environmental data;

[0009] Step 2: When starting positioning, obtain the current environmental data of the positioning object and the environmental data corresponding to the prior map for local matching, and determine whether the match is successful. If the match is successful, update the position of the positioning object at the current position to successfully locate it. If the match fails, proceed to the next step;

[0010] Step 3: Update the dynamic map and globally match the environmental data currently acquired by the positioning object with the prior map to determine whether the match is successful. If the match is successful, update the position of the positioning object at the current location to successfully locate it. If the match fails, proceed to the next step.

[0011] Step 4: Globally match the environmental data currently acquired by the positioning object with the updated dynamic map to determine whether the match is successful. If the match is successful, the position of the positioning object at the current position is updated to successfully locate it. If the match fails, the positioning fails.

[0012] Preferably, step 1 includes: removing highly dynamic objects using a method of calculating grid probability.

[0013] Preferably, performing dynamic map updating includes:

[0014] When the map needs to be updated dynamically, the map is deleted, expanded, and temporary map point weights are assigned in sequence to update the grid.

[0015] Preferably, deleting the map includes:

[0016] When the number of grids inserted into the system exceeds the preset maximum value, a deletion strategy is initiated to gradually delete the grids inserted earlier. When deleting grid data, the corresponding point cloud data is searched through the grid index, so that the point cloud data of the corresponding grid and the probability data of grid occupancy are deleted, and the corresponding grid data is removed from the dynamic map stack.

[0017] Preferably, the expansion of the map includes:

[0018] Traverse the point cloud data of the laser scan of the current frame;

[0019] Convert point cloud data and relative pose to raster map coordinate system;

[0020] Calculate the index of the grid where the point cloud data is located and determine whether the grid exists. If the grid index already exists, the system directly updates the point cloud data in the grid. If the grid does not exist, the map needs to be expanded.

[0021] If the number of point clouds in a single grid exceeds a preset threshold, stop adding new point clouds.

[0022] Preferably, a temporary map point weight is assigned to the newly added point cloud, including:

[0023] A weight is added to each point cloud updated in the dynamic map, and the weight is smaller than the weight of the prior point cloud at the corresponding position in the prior map.

[0024] Preferably, determining whether the obtained local environment data and the prior map match fails includes:

[0025] The local environment posture is obtained through the local environment data. When the posture movement satisfies any of the following conditions: the posture movement distance exceeds 1 cm, the posture rotation exceeds 1 degree, and the interval between the current time and the previous frame positioning time exceeds 2 seconds, it is determined whether the obtained local environment data and the prior map match have failed.

[0026] Preferably, performing global matching includes:

[0027] Combine the currently acquired environmental data with existing maps, including prior maps or dynamic maps, and the old grid will be given a higher weight than the new grid;

[0028] Acquire multiple positions of the currently positioned object through the environmental data currently acquired by the positioned object;

[0029] Select the map object to be matched, including a priori map or dynamic map;

[0030] Use the point cloud matching algorithm to calculate the matching degree between the current observation data and the point cloud in the map. During the matching process, the new raster information will be scored higher than the raster information in the map after the combination.

[0031] Calculate the matching score based on the matching results between the current pose candidate and the map;

[0032] When the matching score of a candidate pose exceeds a preset threshold, the pose is considered valid;

[0033] Based on the current candidate valid pose, a small incremental change is made to generate multiple sub-candidate poses;

[0034] Calculate the degree of matching between multiple sub-candidate poses and the map, and determine whether to continue optimizing the sub-candidate pose to obtain the best matching pose based on the matching score.

[0035] Preferably, when the matching scores of the candidate poses are all lower than a preset threshold, the global matching fails; otherwise, the global matching succeeds and the best matching pose is obtained.

[0036] Preferably, updating the posture of the positioning object at the current position includes: updating the posture of the current positioning object through the best matching posture.

[0037] In a second aspect, in some embodiments, a SLAM device under semi-dynamic environmental changes is further proposed, comprising:

[0038] An acquisition unit, used to acquire environmental data and construct a priori maps based on the environmental data;

[0039] The first matching unit is used to obtain the current environmental data of the positioning object and the environmental data corresponding to the prior map for local matching when starting positioning, and determine whether the match is successful. If the match is successful, the position and posture of the positioning object at the current position are updated to successfully locate the object. If the match fails, the next step is entered;

[0040] The second matching unit is used to update the dynamic map, perform global matching of the environmental data currently acquired by the positioning object with the prior map, and determine whether the match is successful. If the match is successful, the position of the positioning object at the current position is updated to successfully locate it. If the match fails, the next step is entered;

[0041] The third matching unit is used to globally match the environmental data currently obtained by the positioning object with the updated dynamic map to determine whether the match is successful. If the match is successful, the position of the positioning object at the current position is updated to successfully locate it. If the match fails, the positioning fails.

[0042] In a third aspect, an embodiment of the present invention provides an electronic device comprising a processor and a memory. The memory is configured to store one or more computer programs; when the processor executes the one or more computer programs stored in the memory, the electronic device is capable of implementing any of the possible design methods of the first aspect.

[0043] In a fourth aspect, the present invention provides a computer-readable storage medium, in which a computer program is stored. When the computer program is executed by a processor, the method as described in any one of the above embodiments is implemented.

[0044] In a fifth aspect, an embodiment of the present invention further provides a computer program product, which, when running on an electronic device, enables the electronic device to execute any possible design method of any of the above aspects.

[0045] Beneficial effects: The present invention introduces a dynamic map update strategy, which can automatically update the map when a local matching fails, ensuring that the map can accurately reflect the actual changes in the environment. When the environment changes (such as temporarily stacked goods, parked vehicles, etc.), the traditional prior map cannot adapt to these changes, resulting in a decrease in positioning accuracy. Through dynamic updating, the system can update or expand the map, making the positioning result more accurate. At the same time, these semi-static objects are identified and processed through the dynamic update strategy, avoiding mismatches caused by these objects.

[0046] The matching strategy proposed in this paper combines the weight distribution of new and old grids with an improved branch-and-bound method, which enhances the robustness of the system in dynamic environments. When the map is updated, different weights are assigned to the new and old grids, ensuring that the newly acquired environmental information has a certain gain in positioning without affecting the positioning accuracy due to its instability.

[0047] To avoid excessive computational burden during dynamic map updates, the system controls the number of newly added point clouds to ensure that the number of grids in the map remains within a reasonable range. By deleting outdated grids, the system avoids waste of computing resources and improves the efficiency of map updates. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 Provide a schematic diagram of the method framework of the present invention;

[0049] Figure 2 Provide an overall matching flow chart for the present invention;

[0050] Figure 3 Provides a map update process diagram for the present invention;

[0051] Figure 4 Provides a schematic diagram for comparing before and after adding a dynamic map to the present invention;

[0052] Figure 5 A schematic diagram showing a comparison before and after the improved matching strategy is provided for the present invention;

[0053] Figure 6 It is a structural diagram of a device provided in one embodiment of the present application;

[0054] Figure 7 This is a structural block diagram of an electronic device provided in one embodiment of the present application. DETAILED DESCRIPTION

[0055] In order to make the technical solution of the present invention clearer, the present invention is further described in detail below with reference to the specific embodiments of the drawings. Example

[0056] In order to make the purpose, technical solutions and advantages of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. Unless otherwise defined, the technical terms or scientific terms used herein should be the common meanings understood by people with ordinary skills in the field to which the present invention belongs. The words "including" and similar words used in this article mean that the elements or objects appearing before the word cover the elements or objects listed after the word and their equivalents, without excluding other elements or objects.

[0057] In view of the problems existing in the existing technology, such as Figure 1 and Figure 2 As shown, a SLAM method under semi-dynamic environment changes includes the following steps:

[0058] Step 1: Acquire environmental data and construct a priori map based on the environmental data. This includes: using a grid probability calculation method to remove highly dynamic objects, acquiring environmental data through sensors such as lidar, IMU, and wheel speedometers, and constructing an initial static priori map. The map is stored in a grid format, with each grid containing point cloud data and occupancy probability.

[0059] Step 2: When starting positioning, obtain the current environmental data of the positioning object and the environmental data corresponding to the prior map for local matching to determine whether the match is successful. If the match is successful, update the position of the positioning object at the current position to successfully locate it. If the match fails, proceed to the next step. During the positioning process, if a local matching failure occurs in a certain frame, the dynamic map update strategy is started. If the local matching is successful, it means that there is no obvious change in the environment, and then directly perform the positioning of the current position of the positioning object, and then successfully locate it.

[0060] Step 3: Update the dynamic map, globally match the currently acquired environmental data of the positioned object with the prior map, and determine whether the match is successful. If the match is successful, update the position of the positioned object at the current position to successfully locate it. If the match fails, proceed to the next step and update the dynamic map based on the currently acquired environmental data. At this time, the system will adjust and supplement the map according to the new environmental changes, remove outdated data, and integrate the new data into the dynamic map, and globally match the environmental data of the currently positioned object with the updated dynamic map. Global matching will take into account changes in the entire map, matching the entire environment rather than the local area. If the global match is successful, it means that the environmental data of the currently positioned object matches the dynamic map, the position of the positioned object at the current position is updated, and the positioning is successful. If the global match fails, it means that the difference between the current environment and the dynamic map is large, and the positioning fails. The system then proceeds to the next step to further adapt to environmental changes and make new attempts, that is, to perform global matching through the dynamic map.

[0061] Step 4: Globally match the currently acquired environmental data of the positioning object with the updated dynamic map to determine whether the match is successful. If the match is successful, the position of the positioning object at the current position is updated to successfully locate it. If the match fails, the positioning fails. Globally match the currently acquired environmental data with the updated dynamic map. Global matching will take into account the changes of the entire dynamic map and match the overall structure and characteristics of the current environment and the dynamic map. If the global match is successful, it means that the current environmental data is highly consistent with the updated dynamic map, the position of the positioning object at the current position is updated, and the positioning is successful. If the global match fails, it means that there is a large difference between the current environmental data and the dynamic map, and the system cannot accurately locate, resulting in positioning failure.

[0062] In some preferred embodiments, Figure 3 As shown, dynamic map updates include:

[0063] When the map needs to be updated dynamically, the map is deleted, expanded, and temporary map point weights are assigned in sequence to update the grid.

[0064] In some preferred embodiments, deleting the map includes:

[0065] When the number of grids inserted into the system exceeds the preset maximum value, a deletion strategy is initiated to gradually delete the grids inserted earlier. When deleting grid data, the corresponding point cloud data is searched through the grid index, and the point cloud data and grid occupancy probability data of the corresponding grid are deleted, and the corresponding grid data is removed from the dynamic map stack.

[0066] The number of grids in the dynamic map exceeds a preset threshold (e.g. 10,000);

[0067] Outdated or invalid raster data needs to be removed to control the map scale;

[0068] Find the earliest inserted raster: Determine the earliest added raster by timestamp or insertion order;

[0069] Deleting raster data removes the point cloud data of the raster from pointsMap, removes the occupancy probability of the raster from probMap, and removes the index of the raster from the stack of the dynamic map;

[0070] Update map status: Reduce the number of grid cells in the dynamic map to ensure that the map size is within a reasonable range.

[0071] In some preferred embodiments, the expansion of the map includes:

[0072] Traverse the point cloud data of the laser scan in the current frame. The laser point cloud data of the current frame includes the coordinates of the point cloud in the sensor coordinate system. The relative pose represents the pose of the sensor relative to the global coordinate system. Traverse each point cloud data in the current frame and perform subsequent projection, grid index calculation and map update operations on each point cloud.

[0073] Convert point cloud data and relative pose to the grid map coordinate system, and convert point cloud from the sensor coordinate system to the global grid map coordinate system for subsequent processing;

[0074] Calculate the index of the grid where the point is located and determine whether the grid exists. If the grid index already exists, the system directly updates the point cloud data in the grid. If the grid does not exist, it is necessary to expand the map and determine the grid to which the point cloud belongs to facilitate finding or creating the grid in the map. According to the global coordinates and grid size, calculate the grid index. If the grid exists, update the point cloud data directly in the grid and check whether the number of point clouds in the grid exceeds the threshold (for example, 3 points). If not, add a new point cloud. If the grid does not exist, create a new grid and add the point cloud data to the grid. Mark the grid as dynamically added (isOld=-1) and assign an initial weight. If the number of point clouds in a single grid exceeds the preset threshold, stop adding new point clouds.

[0075] In some preferred embodiments, a temporary map point weight is assigned to a newly added point cloud, including:

[0076] Add a weight to each point cloud updated in the dynamic map, and the weight is smaller than the weight of the prior point cloud at the corresponding position in the prior map;

[0077] Old point cloud weight: point cloud existing in the prior map, marked as isOld>0;

[0078] Assigning a higher weight (e.g., +10 points) indicates high credibility;

[0079] New point cloud weight: dynamically added point cloud, marked as isOld=-1;

[0080] Assigning a lower weight (e.g., +1 point) indicates lower credibility;

[0081] The specific implementation of weight distribution, traversing the point cloud in the dynamic map:

[0082] For each newly added point cloud, check the isOld flag of the grid it belongs to.

[0083] Determine the point cloud type:

[0084] If isOld>0: the point cloud exists in the prior map and is given a higher weight (for example, +10 points).

[0085] If isOld=-1: the point cloud is added dynamically and is given a lower weight (for example, +1 point);

[0086] Store weight values: store weight values ​​in point cloud attributes for subsequent matching scores;

[0087] Update the weight information of the dynamic map to ensure that the weight value is synchronized with the point cloud data. In the branch and bound algorithm, the matching score of the candidate pose is determined by the point cloud weight. When the old point cloud is successfully matched, the score is higher (for example, +10 points), and when the new point cloud is successfully matched, the score is lower (for example, +1 point).

[0088] In some preferred embodiments, determining whether the acquired local environment data fails to match the prior map includes:

[0089] The local environment posture is obtained through the local environment data. When the posture movement satisfies any of the following conditions: the posture movement distance exceeds 1 cm, the posture rotation exceeds 1 degree, and the interval between the current time and the previous frame positioning time exceeds 2 seconds, it is determined whether the obtained local environment data and the prior map match have failed.

[0090] Obtain local environmental data. The system obtains environmental data of the current frame and obtains the current environmental information of the positioned object through sensors such as lidar, IMU, and wheel speed meter.

[0091] Calculate the local environment pose. Based on sensor data (such as laser scanning point cloud and IMU data), the system calculates the local environment pose of the positioned object. The local pose usually includes the coordinate position and orientation (i.e., displacement and rotation) of the object.

[0092] Determine the pose change conditions, compare the pose of the current frame with the pose of the previous frame, and determine whether any of the following conditions are met:

[0093] Displacement exceeds 1 cm: If the displacement between the current frame and the previous frame is greater than 1 cm, it is considered that the posture has changed significantly;

[0094] Rotation greater than 1 degree: If the rotation angle between the current frame and the previous frame is greater than 1 degree, it is considered that the posture has undergone a large rotation;

[0095] Time interval exceeds 2 seconds: If the time interval between the current frame and the previous frame exceeds 2 seconds, it means that the positioning object has changed significantly during this period;

[0096] Determining a match failure: If any of the above conditions are met, the system will determine that the currently acquired local environment data and the prior map have failed to match. At this point, the system needs to proceed to the next step, performing a dynamic map update and further matching.

[0097] Triggering dynamic map updates: In the event of matching failure, the system triggers a dynamic map update process, recalculates the pose, and attempts to update the map to ensure positioning accuracy and robustness.

[0098] In some preferred embodiments, performing global matching includes:

[0099] Combine the currently acquired environmental data with existing maps, including prior maps or dynamic maps, and the old grid will be given a higher weight than the new grid;

[0100] Acquire multiple positions of the currently positioned object through the environmental data currently acquired by the positioned object;

[0101] Select the map object to be matched, including the prior map or dynamic map. The prior map is used first. If the environmental changes are small, the static prior map is matched first to preserve the stability of the original map. The dynamic map is used as a supplement. If an environmental change is detected (such as a local matching failure), switch to the dynamic map for secondary matching.

[0102] Use the point cloud matching algorithm to calculate the matching degree between the current observation data and the point cloud in the map. During the matching process, the new raster information will receive a higher weight in the score than the raster information in the map after the combination, thereby enhancing the influence of the new environment on the global matching;

[0103] The matching score is calculated based on the matching results between the current pose candidate and the map, and the scoring is performed using the following algorithm:

[0104] ICP (Iterative Closest Point): Optimizes pose by minimizing the distance error of the point cloud;

[0105] NDT (Normal Distribution Transform): Matching point clouds based on probability distribution;

[0106] When the matching score of a candidate pose exceeds a preset threshold, the pose is considered valid. A preset matching score threshold (for example, >70% of the point cloud matches successfully) is set. If the candidate pose score exceeds the threshold, it is considered a valid pose and the low-scoring candidate pose is discarded to avoid introducing errors.

[0107] Based on the current valid candidate pose, the system makes small incremental changes (such as fine-tuning the position or rotation angle) to generate multiple sub-candidate poses. These sub-candidate poses may provide more accurate positioning results;

[0108] Calculate the degree of match between multiple sub-candidate poses and the map, and determine whether to continue optimizing the sub-candidate pose to obtain the best matching pose based on the matching score. For each sub-candidate pose, the system calculates its degree of match with the map and generates a matching score. Based on the matching score of the sub-candidate pose, the system determines whether to continue optimizing the sub-candidate pose. The goal of optimization is to find the best matching pose, that is, the pose with the highest degree of match with the map.

[0109] In some preferred embodiments, when the matching scores of the candidate poses are all lower than a preset threshold, the global matching fails; otherwise, the global matching succeeds and the best matching pose is obtained.

[0110] In some preferred embodiments, updating the posture of the positioning object at the current position includes: updating the posture of the current positioning object through the best matching posture.

[0111] In some preferred embodiments, combined Figure 4 , dynamically update the map before and after the comparison Figure 4 As shown, Figure 5 The left part is the map that has not been dynamically updated, that is, the unmodified prior map. When the prior map was created, the en route area had not yet been stocked with goods, so this area was blank in the prior map. However, as production operations progressed, the environment changed significantly, requiring temporary storage and retrieval of goods in this area. Therefore, the unupdated prior map could not accurately describe these changes, resulting in errors in the positioning and navigation process. To adapt to these environmental changes and improve positioning accuracy, dynamic map updates were introduced;

[0112] Figure 5The right side shows the dynamically updated map. In the updated map, the environment in these areas has been corrected to reflect the real-world production environment. The red dots in the figure represent locations in both the prior map and the dynamically updated map. As can be seen, the red dots mark areas of environmental change, and the dynamically updated map successfully fills in these gaps. The blue dots represent point clouds where the current frame successfully matches the map. The updated map better matches the current radar point cloud, providing more accurate positioning results.

[0113] In some preferred embodiments, combined Figure 5 , the comparison effect of the new matching strategy is as follows Figure 5 As shown, Figure 5 The left part shows the matching results before the improved strategy, where the blue points represent the point cloud with successful matching, and the yellow points represent the point cloud with unsuccessful matching. As can be seen from the figure, the empty area in the prior map fails to match after the temporary stacking of goods. Figure 5 The right part shows the matching effect obtained by dynamically updating the map. The map reflects the environmental changes of the real scene in real time. In addition, Figure 5 The right part also combines the weights of the new and old grids and introduces a branch-and-bound method, which significantly improves the matching results. The new strategy can better cope with environmental changes and ensure matching accuracy.

[0114] In some embodiments, a SLAM device under semi-dynamic environment changes is also proposed, combining Figure 6 ,include:

[0115] An acquisition unit 301 is used to acquire environmental data and construct a priori maps based on the environmental data;

[0116] The first matching unit 302 is used to obtain the current environmental data of the positioning object and the environmental data corresponding to the prior map when starting positioning, perform local matching, and determine whether the match is successful. If the match is successful, the position and posture of the positioning object at the current position are updated to successfully locate the object. If the match fails, the next step is entered;

[0117] The second matching unit 303 is used to update the dynamic map, perform global matching of the environmental data currently acquired by the positioning object with the prior map, and determine whether the match is successful. If the match is successful, the position of the positioning object at the current position is updated to successfully locate it. If the match fails, the process proceeds to the next step;

[0118] The third matching unit 304 is used to globally match the environmental data currently obtained by the positioning object with the updated dynamic map to determine whether the match is successful. If the match is successful, the position of the positioning object at the current position is updated to successfully locate it. If the match fails, the positioning fails.

[0119] All relevant contents of each step involved in the above method embodiment can be referred to the functional description of the corresponding functional module and will not be repeated here.

[0120] In other embodiments of the present invention, an electronic device 400 is disclosed. Figure 7 As shown, the electronic device may include: one or more processors 401; a memory 402; a display 403; one or more applications (not shown); and one or more computer programs 404. The above components may be connected via one or more communication buses 405. The one or more computer programs 404 are stored in the memory 402 and configured to be executed by the one or more processors 401. The one or more computer programs 404 include instructions, which may be used to execute the following: Figures 1 to 5 and each step in the corresponding embodiment.

[0121] Through the description of the above embodiments, those skilled in the art will clearly understand that for the sake of convenience and brevity, only the division of the above functional modules is used as an example. In actual applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device can be divided into different functional modules to complete all or part of the functions described above. The specific working processes of the above-described systems, devices, and units can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0122] Each functional unit in each embodiment of the present invention may be integrated into a processing unit, each unit may exist physically separately, or two or more units may be integrated into a single unit. The above-mentioned integrated units may be implemented in the form of hardware or software functional units.

[0123] If the integrated unit is implemented as a software functional unit and sold or used as a standalone product, it can be stored on a computer-readable storage medium. Based on this understanding, the technical solution of the embodiments of the present invention, or the portion that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product, stored on a storage medium, includes instructions for causing a computer device (such as a personal computer, server, or network device) or processor to execute all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as flash memory, mobile hard disks, read-only memory, random access memory, magnetic disks, or optical disks.

[0124] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions within the technical scope disclosed in the present invention should be covered by the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A SLAM method under semi-dynamic environment changes, characterized in that: Including steps: Step 1: Obtain environmental data and build a priori map based on the environmental data; Step 2: When starting positioning, obtain the current environmental data of the positioning object and the environmental data corresponding to the prior map for local matching, and determine whether the match is successful. If the match is successful, update the position of the positioning object at the current position to successfully locate it. If the match fails, proceed to the next step; Step 3: Update the dynamic map and globally match the environmental data currently acquired by the positioning object with the prior map to determine whether the match is successful. If the match is successful, update the position of the positioning object at the current location to successfully locate it. If the match fails, proceed to the next step. Step 4: Globally match the environmental data currently acquired by the positioning object with the updated dynamic map to determine whether the match is successful. If the match is successful, the position of the positioning object at the current position is updated to successfully locate it. If the match fails, the positioning fails. Dynamic map updates include: When the map needs to be dynamically updated, the map is deleted, expanded, and temporary map point weights are assigned to update the grid. Deletion of the map includes: When the number of grids inserted into the system exceeds the preset maximum value, a deletion strategy is initiated to gradually delete the grids inserted earlier. When deleting grid data, the corresponding point cloud data is searched through the grid index, and the point cloud data and grid occupancy probability data of the corresponding grid are deleted, and the corresponding grid data is removed from the dynamic map stack. Extensions to the map include: Traverse the point cloud data of the laser scan of the current frame; Convert point cloud data and relative pose to raster map coordinate system; Calculate the index of the grid where the point cloud data is located and determine whether the grid exists. If the grid index already exists, the system directly updates the point cloud data in the grid. If the grid does not exist, the map is expanded. If the number of point clouds in a single grid exceeds the preset threshold, stop adding new point clouds; Assign temporary map point weights to newly added point clouds, including: A weight is added to each point cloud updated in the dynamic map, and the weight is smaller than the weight of the prior point cloud at the corresponding position in the prior map.

2. The method according to claim 1, characterized in that Step 1 includes: removing highly dynamic objects using a method of calculating grid probabilities.

3. The method according to claim 1, characterized in that Determine whether the obtained local environment data and the prior map match fails, including: The local environment posture is obtained through the local environment data. When the posture movement satisfies any of the following conditions: the posture movement distance exceeds 1 cm, the posture rotation exceeds 1 degree, and the interval between the current time and the previous frame positioning time exceeds 2 seconds, it is determined whether the obtained local environment data and the prior map match have failed.

4. The method according to claim 1, wherein Performing global matching includes: Combine the currently acquired environmental data with existing maps, including prior maps or dynamic maps, and the old grid will be given a higher weight than the new grid; Acquire multiple positions of the currently positioned object through the environmental data currently acquired by the positioned object; Select the map object to be matched, including a priori map or dynamic map; Use the point cloud matching algorithm to calculate the matching degree between the current observation data and the point cloud in the map. During the matching process, the new raster information will be scored higher than the raster information in the map after the combination. Calculate the matching score based on the matching results between the current pose candidate and the map; When the matching score of a candidate pose exceeds a preset threshold, the pose is considered valid; Based on the current candidate valid pose, a small incremental change is made to generate multiple sub-candidate poses; Calculate the degree of matching between multiple sub-candidate poses and the map, and determine whether to continue optimizing the sub-candidate pose to obtain the best matching pose based on the matching score.

5. The method according to claim 1, wherein When the matching scores of the candidate poses are all lower than the preset threshold, the global matching fails; otherwise, the global matching succeeds and the best matching pose is obtained.

6. A SLAM device under semi-dynamic environmental changes, characterized in that: include: An acquisition unit, used to acquire environmental data and construct a priori maps based on the environmental data; The first matching unit is used to obtain the current environmental data of the positioning object and the environmental data corresponding to the prior map for local matching when starting positioning, and determine whether the match is successful. If the match is successful, the position and posture of the positioning object at the current position are updated to successfully locate the object. If the match fails, the next step is entered; The second matching unit is used to update the dynamic map, perform global matching of the environmental data currently acquired by the positioning object with the prior map, and determine whether the match is successful. If the match is successful, the position of the positioning object at the current position is updated to successfully locate it. If the match fails, the next step is entered; The third matching unit is used to globally match the environmental data currently acquired by the positioning object with the updated dynamic map, and determine whether the match is successful. If the match is successful, the position and posture of the positioning object at the current position are updated to successfully locate the object. If the match fails, the positioning fails. Dynamic map updates include: When the map needs to be updated dynamically, the map is deleted, expanded, and temporary map point weights are assigned in sequence to update the grid; Map deletions include: When the number of grids inserted into the system exceeds the preset maximum, a deletion strategy is initiated to gradually delete the grids inserted earlier. When deleting grid data, the corresponding point cloud data is found through the grid index, and the point cloud data and grid occupancy probability data of the corresponding grid are deleted. The corresponding grid data is then removed from the dynamic map stack. The map extensions include: Traverse the point cloud data of the laser scan of the current frame; Convert point cloud data and relative pose to raster map coordinate system; Calculate the index of the grid where the point cloud data is located and determine whether the grid exists. If the grid index already exists, the system directly updates the point cloud data in the grid. If the grid does not exist, the map is expanded. If the number of point clouds in a single grid exceeds the preset threshold, stop adding new point clouds; Assign temporary map point weights to newly added point clouds, including: A weight is added to each point cloud updated in the dynamic map, and the weight is smaller than the weight of the prior point cloud at the corresponding position in the prior map.

Images