Incremental mapping method, device, and storage medium

Abstract

The application discloses a kind of incremental mapping method, equipment and storage medium, the incremental mapping method includes: if it is detected that mobile device is in priori map range outside, stop using priori map to locate, and locate based on the incremental map constructed;If it is detected that mobile device moves from priori map range outside to priori map range, enable priori map to locate, and the environment perception data of current time and priori map are matched based on branch and bound algorithm, and the matching score corresponding to the pose of matching success is obtained;Add monomial edge constraint to the pose corresponding to the environment perception data of matching score greater than preset score threshold;The accumulated error optimization is carried out to the pose calculated using monomial edge constraint, to make incremental map and priori map align. By adding monomial edge matched with priori map to constrain mapping pose, the alignment effect between incremental map and priori map can be improved, to avoid the appearance of map ghosting and fault.

Description

Technical Field

[0001] This application relates to the field of robot localization technology, and in particular to an incremental mapping method, device and storage medium. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) enables mobile devices to autonomously navigate, avoid obstacles, and reach their destinations even without human intervention. Since environmental information is often partial or even completely unknown, localization and mapping must be combined. This allows the mobile device to use its own localization to build a map, and simultaneously use the built map for localization.

[0003] Incremental mapping refers to the process by which a robot builds a new map based on an existing one. Most existing incremental mapping solutions are based on traditional localization and map stitching algorithms, which are prone to losing accuracy during the matching of new and old maps, resulting in map gaps or ghosting. Summary of the Invention

[0004] To address the aforementioned technical problems, this application provides at least one incremental mapping method, device, and storage medium.

[0005] The first aspect of this application provides an incremental mapping method, which includes: calculating the pose of a mobile device based on odometry data and / or collected environmental perception data; if the pose of the mobile device indicates that the mobile device is outside the scope of a priori map, then stopping the use of the priori map for localization and constructing an incremental map for localization based on the collected environmental perception data; if the pose of the mobile device indicates that the mobile device has moved from outside the scope of the priori map to within the scope of the priori map, then enabling the priori map for localization; matching the environmental perception data and the priori map at the current moment based on a branch and bound algorithm to obtain the matching score corresponding to the successfully matched pose; adding unary edge constraints to the poses corresponding to environmental perception data with matching scores greater than a preset score threshold; wherein, the unary edge constraint is a constraint relationship between a single pose and the priori map; and using the unary edge constraints to perform cumulative error optimization on the calculated poses to align the incremental map with the prior map.

[0006] In one embodiment, calculating the pose of a mobile device based on odometer data and / or collected environmental perception data includes: acquiring the pose at the previous moment, calculating the time interval between the previous moment and the current moment, and calculating the change in odometer data between the previous moment and the current moment; wherein the previous moment is before the current moment; if the time interval is not greater than a preset time threshold and the change in odometer data is not greater than a preset change threshold, then the pose at the current moment is predicted based on the pose at the previous moment and the change in odometer data between the previous moment and the current moment; if the time interval is greater than a preset time threshold, or the change in odometer data is greater than a preset change threshold, then the initial pose at the current moment is predicted based on the pose at the previous moment and the odometer data at the current moment, acquiring environmental perception data at the current moment, and calculating the pose at the current moment based on the initial pose at the current moment and the environmental perception data at the current moment.

[0007] In one embodiment, the pose at the current moment is calculated based on the initial pose at the current moment and the environmental perception data at the current moment, including: obtaining the coordinates of the environmental perception data at the current moment in the device coordinate system of the mobile device to obtain the device coordinates of the perception data; transforming the device coordinates of the perception data to the world coordinate system corresponding to the prior map based on the initial pose at the current moment to obtain the world coordinates of the perception data; calculating the distance between the world coordinates of the perception data and obstacles in the prior map, filtering out points with distances less than a preset distance threshold from the environmental perception data at the current moment to obtain long-term features; and matching the long-term features with the prior map to obtain the pose at the current moment.

[0008] In one embodiment, the prior map stores the coordinates of each obstacle; the method further includes: calculating the maximum x-coordinate, maximum y-coordinate, minimum x-coordinate, and minimum y-coordinate of each obstacle in the prior map; if the x-coordinate of the pose of the mobile device is greater than the minimum x-coordinate and less than the maximum x-coordinate, and the y-coordinate of the pose of the mobile device is greater than the minimum y-coordinate and less than the maximum y-coordinate, then it is determined that the mobile device is within the range of the prior map.

[0009] In one embodiment, the incremental map contains multiple sub-maps; the method further includes: constructing sub-maps based on the acquired keyframes and their corresponding calculated poses; wherein, the keyframes refer to environmental perception data used for pose calculation; constructing binary edge constraints between the poses corresponding to the keyframes in the sub-maps that meet the preset edge construction conditions; wherein, the binary edge constraint is a constraint relationship between two poses; and performing cumulative error optimization on the calculated poses using unary edge constraints, including: performing cumulative error optimization on the calculated poses using unary edge constraints and binary edge constraints.

[0010] In one embodiment, constructing binary edge constraints between the poses corresponding to keyframes that satisfy preset edge construction conditions in a sub-map includes: obtaining the sub-map constructed at the current time; querying the keyframe in the sub-map constructed at the current time whose pose is closest to the keyframe added at the current time, and / or querying the keyframe in the sub-map constructed at the current time from the previous time to obtain adjacent keyframes; and adding binary edge constraints between the poses corresponding to the keyframe added at the current time and the adjacent keyframes.

[0011] In one embodiment, constructing binary edge constraints between the poses corresponding to keyframes that meet preset edge construction conditions in a sub-map includes: calculating the mean of the poses corresponding to each keyframe in each sub-map to obtain the average pose corresponding to each sub-map; constructing a quadtree based on the average poses corresponding to each sub-map; wherein the master node in the quadtree is the average pose of any sub-map, and the child node corresponding to the master node is the average pose of the other sub-map closest to the master node; setting a sub-map selection range based on the pose of the keyframe at the current time; selecting sub-maps whose average poses are within the sub-map selection range from the quadtree to obtain candidate sub-maps; matching the keyframe at the current time with the candidate sub-maps to calculate the matching score between the keyframe at the current time and the candidate sub-maps; taking the candidate sub-map with a matching score greater than a preset score threshold as the target sub-map, selecting the keyframe among all keyframes of the target sub-map whose pose is closest to the pose at the current time to obtain the target keyframe; and adding binary edge constraints between the poses corresponding to the keyframe at the current time and the target keyframe.

[0012] In one embodiment, the sub-map selection range includes a first selection range and a second selection range, where the second selection range is larger than the first selection range. Selecting sub-maps from the quadtree whose average pose falls within the sub-map selection range to obtain candidate sub-maps includes: selecting sub-maps from the quadtree whose average pose falls within the first selection range to obtain multiple first initial sub-maps; deleting sub-maps from the multiple first initial sub-maps that contain keyframes at the current time to obtain candidate sub-maps selected within the first selection range; selecting sub-maps from the quadtree whose average pose falls within the second selection range to obtain multiple second initial sub-maps; obtaining keyframes within the historical time period corresponding to the keyframe at the current time to obtain temporally adjacent keyframes; and deleting sub-maps from the multiple second initial sub-maps that contain keyframes at the current time or temporally adjacent keyframes to obtain candidate sub-maps selected within the second selection range.

[0013] In one embodiment, the binary edge constraint includes a loop edge constraint; the calculated pose is optimized by accumulating error using both unary and binary edge constraints, including:

[0014] If the addition of a loop edge constraint is detected, or if a preset number of unary edge constraints are added, then the currently calculated pose is optimized for cumulative error. In response to the completion of cumulative error optimization, each sub-map is marked as to be reconstructed. After updating the grid obstacle probability of the currently constructed sub-map, the currently constructed sub-map is marked as reconstructed. Before matching the keyframe and candidate sub-map at the current time and calculating the matching score of the keyframe and candidate sub-map at the current time, the following steps are also included: after updating the grid obstacle probability of the candidate sub-map selected from the quadtree, the currently constructed sub-map is marked as reconstructed.

[0015] A second aspect of this application provides an incremental mapping apparatus, comprising: a pose calculation module for calculating the pose of a mobile device based on odometer data of the mobile device and / or collected environmental perception data; a first positioning module for stopping positioning using the prior map if the pose of the mobile device indicates that the mobile device is outside the range of the prior map, and constructing an incremental map for positioning based on the collected environmental perception data; a second positioning module for enabling positioning using the prior map if the pose of the mobile device indicates that the mobile device has moved from outside the range of the prior map to within the range of the prior map, and matching the environmental perception data and the prior map at the current moment based on a branch and bound algorithm to obtain a matching score corresponding to a successfully matched pose; a unary edge addition module for adding unary edge constraints to the poses corresponding to environmental perception data with matching scores greater than a preset score threshold; wherein, the unary edge constraint is a constraint relationship between a single pose and the prior map; and an error optimization module for optimizing the cumulative error of the calculated pose using unary edge constraints to align the incremental map with the prior map.

[0016] A third aspect of this application provides a mobile device including a memory and a processor, the processor being used to execute program instructions stored in the memory to implement the above-described incremental mapping method.

[0017] The fourth aspect of this application provides a computer-readable storage medium having program instructions stored thereon, which, when executed by a processor, implement the above-described incremental mapping method.

[0018] The above scheme stops using the prior map for localization when the mobile device is detected to be outside the prior map's range, and instead uses an incremental map built based on the collected environmental perception data for localization, thus reducing computation time. If the mobile device is detected to move from outside the prior map's range to within the prior map's range, the prior map is activated for localization. The current environmental perception data and the prior map are matched using a branch and bound algorithm to obtain the matching score corresponding to the successfully matched pose. Unary edge constraints are added to the poses corresponding to environmental perception data with matching scores greater than a preset score threshold. The calculated poses are then optimized for cumulative error using unary edge constraints to align the incremental map with the prior map. By adding unary edges that match the prior map to constrain the mapping poses, the alignment effect between the incremental map and the prior map can be improved, avoiding map ghosting and discontinuities.

[0019] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and are not intended to limit this application. Attached Figure Description

[0020] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with this application and, together with the specification, serve to explain the technical solutions of this application.

[0021] Figure 1 This is a schematic diagram illustrating the implementation environment of the solution in an exemplary embodiment of this application;

[0022] Figure 2 This is a flowchart illustrating an exemplary embodiment of the incremental mapping method of this application;

[0023] Figure 3 A schematic diagram illustrating the effect of cumulative error optimization based on unary edge constraints, as shown in an exemplary embodiment of this application;

[0024] Figure 4 This is a schematic diagram illustrating nearest-neighbor keyframe constraints in an exemplary embodiment of this application;

[0025] Figure 5 This is a schematic diagram illustrating a loop edge constraint in an exemplary embodiment of this application;

[0026] Figure 6 This is a block diagram illustrating an incremental mapping apparatus in an exemplary embodiment of this application;

[0027] Figure 7 This is a schematic diagram of the structure of a mobile device shown in an exemplary embodiment of this application;

[0028] Figure 8 This is a schematic diagram illustrating the structure of a computer-readable storage medium, as shown in an exemplary embodiment of this application. Detailed Implementation

[0029] The embodiments of this application will now be described in detail with reference to the accompanying drawings.

[0030] In the following description, specific details such as particular system architectures, interfaces, and technologies are presented for illustrative purposes rather than for limiting purposes, in order to provide a thorough understanding of this application.

[0031] In this document, the term "and / or" is merely a description of the association information of related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Additionally, the character " / " in this document generally indicates that the preceding and following related objects have an "or" relationship. Furthermore, "many" in this document means two or more. Moreover, the term "at least one" in this document means any combination of at least two of any one or more of a plurality of elements. For example, including at least one of A, B, and C can mean including any one or more elements selected from the set consisting of A, B, and C.

[0032] The incremental mapping method provided in the embodiments of this application will be described below.

[0033] Please refer to Figure 1 , Figure 1 This is a schematic diagram illustrating an implementation environment of the scheme according to an exemplary embodiment of this application. The implementation environment may include a mobile device 110 and a server 120, and the mobile device 110 and the server 120 are interconnected.

[0034] The mobile device 110 can be an industrial robot, an automated guided vehicle (AGV), a sweeping robot, etc., and this application does not limit it.

[0035] The mobile device 110 is equipped with an environmental sensing device, which can be one or more of the following: LiDAR, depth camera, camera, ultrasonic sensor, etc., to collect environmental information. For example, Figure 1 The mobile device 110 is equipped with a lidar on its top. The lidar scans the environmental point cloud to obtain environmental perception data.

[0036] Of course, the mobile device 110 may also include a mobile chassis, which includes a motion controller, motor, battery, embedded computer, odometer, gyroscope, etc. This application does not limit this.

[0037] Server 120 can be a standalone physical server, a server cluster or distributed system composed of multiple physical servers, or a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communication, middleware services, domain name services, security services, content delivery networks (CDN), and big data and artificial intelligence platforms.

[0038] In one example, server 120 can perform localization and mapping based on environmental perception data, odometer data, etc. obtained from mobile device 110 to obtain the pose and incremental map of mobile device 110. Server 120 can then send the pose and incremental map of mobile device 110 back to mobile device 110 so that mobile device 110 can navigate and move.

[0039] In one example, a client application for a target application is installed and running on the mobile device 110. This target application could be an application providing location and navigation functions. The target application uses the acquired environmental perception data to perform localization and mapping, obtaining the pose and incremental map of the mobile device 110 for navigation. Server 120 can be a backend server for this target application, providing background services to the client application.

[0040] The incremental mapping method provided in this application embodiment can be executed by a mobile device 110, such as the client of the target application installed and running on the mobile device 110, or by a server 120, or by the interaction and cooperation between the mobile device 110 and the server 120, that is, some steps of the method are executed by the mobile device 110 and other steps are executed by the server 120.

[0041] It should be noted that the incremental mapping method of this application can be applied to any navigation scenario, such as the shelf docking scenario of industrial robots, the floor cleaning scenario of cleaning robots, etc., and this application does not limit it.

[0042] Please see Figure 2 , Figure 2 This is a flowchart illustrating an exemplary embodiment of the incremental mapping method of this application. This incremental mapping method can be applied to... Figure 1 The implementation environment shown is specifically executed by a mobile device within that implementation environment. It should be understood that this method can also be applied to other exemplary implementation environments and executed by devices in other implementation environments; this embodiment does not limit the implementation environment to which the method is applicable.

[0043] like Figure 2As shown, the incremental mapping method includes at least steps S210 to S250, which are described in detail below:

[0044] Step S210: Calculate the pose of the mobile device based on the odometer data of the mobile device and / or the collected environmental perception data.

[0045] Odometry estimates the amount of change in the position and orientation of a mobile device over time by measuring the motion of moving elements.

[0046] Environmental perception data may include point cloud data collected by lidar, depth images collected by depth cameras, and visualization images collected by cameras, etc. This application does not limit the data type of environmental perception data.

[0047] Based on the odometer data and / or environmental perception data collected at each time point, the pose of the mobile device at each time point is calculated.

[0048] Step S220: If the pose of the mobile device indicates that the mobile device is outside the range of the prior map, then stop using the prior map for positioning, and construct an incremental map for positioning based on the collected environmental perception data.

[0049] A priori map is an environmental map that is created, stored, and loaded into a mobile device before the device begins performing a task.

[0050] The prior map can be a two-dimensional (2D) raster map, a three-dimensional (3D) point cloud map, a topological map, etc. This application does not limit the type of prior map.

[0051] The prior map can be manually surveyed or generated in the early stages through SLAM mapping, and this application does not limit the method of obtaining the prior map.

[0052] Based on the pose of the mobile device, it is detected whether the mobile device is outside the scope of the prior map. If the mobile device is outside the scope of the prior map, the positioning using the prior map is stopped, and an incremental map is constructed based on the collected environmental perception data for positioning, so as to reduce the computation time.

[0053] It should be noted that, in addition to the need to construct an incremental map for the new working area when the mobile device is outside the scope of the prior map, it is generally also necessary to construct an incremental map for the areas that have changed relative to the prior map when the mobile device is within the scope of the prior map, in order to improve positioning accuracy.

[0054] Step S230: If the pose of the mobile device indicates that the mobile device has moved from outside the prior map range to within the prior map range, then the prior map is used for localization. The environmental perception data at the current moment is matched with the prior map based on the branch and bound algorithm to obtain the matching score corresponding to the environmental perception data at the current moment.

[0055] Based on the pose of the mobile device, it is detected whether the mobile device has moved from outside the prior map range to within the prior map range. If the mobile device moves from outside the prior map range to within the prior map range, the prior map is then used for positioning. The environmental perception data at the current moment is matched with the prior map according to the branch and bound algorithm to obtain the matching score corresponding to the environmental perception data at the current moment.

[0056] In the branch and bound algorithm, "branching" refers to dividing the search space of the current node into smaller subspaces (child nodes), with each node representing a search space and the root node representing the entire range of poses to be searched. "Bounding" refers to calculating the upper bound of the highest possible score achievable for a given node. This is done by generating multiple resolution versions of the map (e.g., original resolution, 2x2 downsampling, 4x4 downsampling, etc.). For a given search space, an appropriate map resolution is selected; the larger the search space, the lower the map resolution used. Then, the score of the collected environmental perception data under the transformation that yields the highest score is calculated, resulting in the upper bound of the score. By continuously searching for nodes whose upper bound of the score is greater than a preset score threshold, the optimal solution is quickly identified, yielding the optimal estimated pose of the mobile device, which is then used as the successfully matched pose.

[0057] Obtain the matching score corresponding to the successfully matched pose.

[0058] Among them, by matching the real-time collected environmental perception data with the prior map, the matching degree is obtained, and the matching degree and the matching score are positively correlated.

[0059] It should be noted that enabling prior map for localization includes the process of calculating pose using a branch and bound algorithm, and also includes the process of combining the prior map with real-time odometry data and / or environmental perception data after obtaining the optimal estimated pose corresponding to the branch and bound algorithm.

[0060] Step S240: Add unary edge constraints to the poses corresponding to the environmental perception data whose matching scores are greater than the preset score threshold; wherein, the unary edge constraint is the constraint relationship between a single pose and the prior map.

[0061] For environment-aware data with a matching score greater than a preset score threshold, add unary edge constraints for the environment-aware data and its corresponding pose.

[0062] A unary edge is a constraint that connects a single pose node to the prior map. It represents the absolute position or measurement of the pose node and provides a reference for the absolute pose information of a pose node.

[0063] Step S250: Optimize the calculated pose by accumulating errors using unary edge constraints to align the incremental map with the prior map.

[0064] As time goes by, tiny measurement errors in the odometer (such as wheel slippage, uneven ground, and uneven tire pressure) accumulate, causing the estimated position to deviate from the actual position more and more, resulting in cumulative positioning errors.

[0065] Factor graph optimization, pose graph optimization (PGO), Kalman filter (KF), deep neural networks, and other methods can be used to optimize the cumulative error of the calculated pose by combining unary edge constraints.

[0066] Taking factor graph optimization as an example, the pose to be optimized is taken as the variable node, and the constraints on the variable (univariate edge constraints, odometry data, environmental perception data, etc.) are taken as the factor node. By adjusting the variable node, the total error is minimized.

[0067] The incremental mapping method of this application does not require searching and matching with the prior map when the device is outside the scope of the prior map, thereby reducing the computation time. When it is determined that the mobile device has returned to the scope of the prior map, the branch and bound algorithm is triggered to quickly search for the pose in the prior map. Furthermore, when the matching score with the prior map exceeds a certain threshold, unary edges matching the prior map are continuously added to constrain the mapping pose, which can improve the alignment effect between the incremental map and the prior map and avoid map ghosting and discontinuity.

[0068] The following describes some embodiments of this application in detail.

[0069] In some implementations, step S210 involves calculating the pose of the mobile device based on the odometer data of the mobile device and / or the collected environmental perception data, including steps S211 to S213.

[0070] Step S211: Obtain the pose of the previous moment, the odometer data of the current moment, calculate the time interval between the previous moment and the current moment, and calculate the change in odometer data between the previous moment and the current moment; wherein, the previous moment is before the current moment.

[0071] It should be noted that the current time and the previous time in this application do not refer to a specific time. During positioning, there may be multiple current times and previous times as time progresses.

[0072] Step S212: If the time interval is not greater than a preset time threshold and the change in odometer data is not greater than a preset change threshold, then the pose at the current moment is predicted based on the pose at the previous moment and the change in odometer data between the previous moment and the current moment.

[0073] If the time interval is not greater than the preset time threshold and the change in odometer data is not greater than the preset change threshold, it indicates that using odometer data for pose inference is relatively reliable, and odometer data can be directly used to calculate the pose at the current moment.

[0074] For example, using a motion model, the pose at the current time t can be predicted. The motion model equation is shown in Equation 1 below:

[0075]

[0076] Here, the previous time step is time t-1, and the pose at time t-1 is represented as x. t-1 =[xy θ] T x and y are the horizontal and vertical coordinates in the world coordinate system, representing the position of the mobile device in the two-dimensional plane containing the x and y axes, respectively. θ represents the direction of the mobile device in the two-dimensional plane containing the x and y axes. The current time is time t. The change in odometer data between time t-1 and time t is (dx, dy, dθ), where dx and dy represent the change in position of the mobile device in the two-dimensional plane containing the x and y axes, respectively, and dθ represents the change in direction of the mobile device in the two-dimensional plane containing the x and y axes, respectively.

[0077] Step S213: If the time interval is greater than a preset time threshold, or the change in odometer data is greater than a preset change threshold, then based on the pose of the previous moment and the odometer data of the current moment, the initial pose of the current moment is predicted, the environmental perception data of the current moment is obtained, and the pose of the current moment is calculated based on the initial pose of the current moment and the environmental perception data of the current moment.

[0078] If the time interval is greater than a preset time threshold, or the change in odometer data is greater than a preset change threshold, it indicates that using odometer data for pose inference is unreliable. In this case, to improve the accuracy of pose prediction, pose calculation is performed in conjunction with environmental perception data.

[0079] For example, the coordinates of the current environmental perception data in the device coordinate system of the mobile device are obtained to obtain the perception data device coordinates; based on the initial pose at the current moment, the perception data device coordinates are transformed to the world coordinate system corresponding to the prior map to obtain the perception data world coordinates; the distance between the perception data world coordinates and obstacles in the prior map is calculated, and points with a distance less than a preset distance threshold are selected from the current environmental perception data to obtain long-term features; the long-term features are matched with the prior map to obtain the pose at the current moment.

[0080] For example, taking environmental perception data as point cloud data, the coordinates of the perception data device can be transformed to the world coordinate system corresponding to the prior map using the following formula 2:

[0081]

[0082] In Formula 2, the k-th laser beam data in the point cloud data collected at time t The perceived data device coordinates relative to the device coordinate system of the mobile device are [x k，sens y k，sens θ k，sens ] T The initial pose at time t is calculated as x based on the odometry data at time t. t =[xy θ] T , These are the horizontal and vertical coordinates of the k-th laser beam data in the world coordinate system, i.e., the world coordinates of the sensing data.

[0083] Then, the distance between the perceived world coordinates and the obstacles in the prior map is calculated.

[0084] The prior map can be a probabilistic grid map. The entire probabilistic grid map is traversed, and the distance between each grid cell and the nearest obstacle point in the map is calculated to generate a distance map. The grid distance value at the location of the obstacle is directly set to zero.

[0085] The distance between the world coordinates of the perceived data and the obstacles in the prior map is obtained by using a distance map. If the distance is less than a preset distance threshold, it means that the laser data was generated by the original obstacles in the prior map and is classified as a long-term feature. If the distance is not less than the preset distance threshold, it means that the laser data was generated by the newly added obstacles and is classified as a short-term feature.

[0086] By matching the aforementioned long-term features with the prior map, a scan-to-map matching method can be used to estimate the pose of the current mobile device.

[0087] During the positioning process, it is detected whether the mobile device is within the range of the prior map.

[0088] For example, the prior map stores the coordinates of each obstacle. The maximum x-coordinate, maximum y-coordinate, minimum x-coordinate, and minimum y-coordinate of each obstacle in the prior map are counted. If the x-coordinate of the pose of the mobile device is greater than the minimum x-coordinate and less than the maximum x-coordinate, and the y-coordinate of the pose of the mobile device is greater than the minimum y-coordinate and less than the maximum y-coordinate, then it is determined that the mobile device is within the range of the prior map.

[0089] Specifically, after loading the prior map, the minimum and maximum values ​​of the obstacle grid coordinates are first calculated, namely the minimum horizontal coordinate minX, the minimum vertical coordinate minY, the maximum horizontal coordinate maxX, and the maximum vertical coordinate maxY. When the horizontal coordinate of the mobile device's pose is greater than minX and less than maxX, and the vertical coordinate is greater than minY and less than maxY, the mobile device is determined to be within the scope of the prior map. Otherwise, the mobile device is determined to be outside the scope of the prior map.

[0090] If the mobile device's pose was outside the prior map range at the previous moment but is within the prior map range at the next moment, it is determined that the mobile device has moved from outside the prior map range to within the prior map range, triggering a fast search step within the prior map. The fast search step uses a branch and bound algorithm to quickly search the environmental perception data and the prior map at the current moment to obtain the mobile device's pose in the prior map. This pose corresponds to a matching score. If the matching score is greater than a preset score threshold (e.g., 0.4), a unary edge is added; if the matching score is not greater than the preset score threshold, no unary edge is added.

[0091] If the matching score is not greater than the preset score threshold, the next step is still to determine whether the mobile device is in the prior map. If it is, the fast search step in the prior map is triggered again. If the matching score corresponding to the searched pose is greater than the preset score threshold, a unary edge is added to the pose as a constraint in the subsequent factor graph optimization.

[0092] In addition, if the pose obtained in the prior map at the previous moment has a matching score greater than the preset score threshold, then there is no need to perform a fast search step at the current moment. Instead, the predicted value based on the odometry data is used to match the current environmental perception data with the prior map. If the matching score between the current environmental perception data and the prior map is greater than the preset score threshold, then a unary edge constraint is added to the pose at the current moment.

[0093] The branch-and-bound pose search function is triggered by determining whether the mobile device moves from outside the prior map area to within the prior map area. Upon successful search, the pose is optimized and a matching score is calculated by matching it with the prior map. This matching score is then used to determine whether to add a unary edge. After a successful match, subsequent attempts to match within the prior map based on pose determination directly calculate a matching score and add more valid unary edges.

[0094] The calculated pose is optimized by accumulating errors using unary edge constraints to align the incremental map with the prior map.

[0095] For example, every time a preset number (e.g., 15) of unary edge constraints are added, a cumulative error optimization is triggered to promptly eliminate the cumulative error of the front-end odometer.

[0096] Please see Figure 3 , Figure 3 A schematic diagram illustrating the effect of cumulative error optimization based on unary edge constraints, as shown in an exemplary embodiment of this application, is as follows: Figure 3 As shown, without adding unary edge constraints, the incremental map (red point cloud) and the prior map (black point cloud) are not aligned. After the mobile device re-enters the range of the prior map, the accumulated error can be optimized in time by adding unary edge constraints (as shown in the colored box), which can improve the alignment effect between the incremental map and the prior map and avoid map ghosting and discontinuity.

[0097] In some implementations, the incremental map contains multiple sub-maps; the method further includes: constructing sub-maps based on acquired keyframes and their corresponding calculated poses; wherein, keyframes refer to environmental perception data used for pose calculation; constructing binary edge constraints between poses corresponding to keyframes that meet preset edge construction conditions in the sub-maps; wherein, binary edge constraints are constraint relationships between two poses; and using unary edge constraints and binary edge constraints to perform cumulative error optimization on the calculated poses so that the incremental map is aligned with the prior map.

[0098] During incremental mapping, multiple submaps are constructed. These submaps are updated using a probabilistic grid to mitigate the impact of moving objects. Each submap has a pose, which is the pose of the first keyframe inserted into that submap. When constructing a submap, only the most recent N keyframes are used to build a single submap.

[0099] Except for the first submap, each subsequent submap involves building two submaps simultaneously. Specifically, when the number of keyframes in the previous submap reaches a preset threshold N1 (e.g., N1 = N / 2), the construction of a new submap begins. This process continues until the number of keyframes in the previous submap reaches N, at which point the previous submap is complete. This method ensures that when matching the current environmental awareness data with the submap, it always matches with submaps containing more historical keyframes (i.e., submaps with more than the preset threshold N1), guaranteeing matching accuracy.

[0100] For example, the initial pose is calculated using the odometry data and / or environmental perception data at the current moment. After optimizing the initial pose using a factor graph optimization algorithm, the optimized pose is obtained. The environmental perception data at the current moment is used as a keyframe, and the keyframe and its corresponding optimized pose are inserted into the currently constructed submap.

[0101] The newly inserted keyframes will update the obstacle occupancy probability of the grid cells in the submap. The method for updating the obstacle probability of the grid cells is shown in Formula 3 below:

[0102]

[0103] Where: p is the probability that the grid cell is occupied by an obstacle; x is the position of the grid cell in the submap; M old (x), M new (x) represents the probability of an old obstacle occupying a grid position and the probability of a new obstacle occupying a grid position, respectively; P hit This is a preset value; for example, it is 0.6 if the grid is considered an obstacle and 0.4 if it is not.

[0104] For example, when the probability of an obstacle being occupied is greater than 0.65, the grid is determined to be occupied by an obstacle and marked as 1; when the probability of an obstacle being occupied is less than 0.2, the grid is determined to be free and marked as 0.

[0105] The obstacle occupancy probability of the grid perceived by the environment is updated using the above update method. When the obstacle occupancy probability increases or decreases to exceed a certain threshold, it is determined that the grid is a newly added or removed obstacle, and the likelihood probability of the grids around the obstacle needs to be updated. For example, based on the Brushfire Algorithm, the grids near the newly added obstacle grid or the removed grid are dynamically updated.

[0106] Then, binary edge constraints are constructed between the poses of keyframes in the sub-map that meet the preset edge construction conditions.

[0107] Here's an example illustrating how to construct binary edge constraints:

[0108] Example 1: Obtain the sub-map constructed at the current time; query the keyframe in the sub-map constructed at the current time whose pose is closest to the keyframe added at the current time, and / or query the keyframe in the sub-map constructed at the current time from the previous time to obtain adjacent keyframes; add binary edge constraints to the keyframe at the current time and adjacent keyframes.

[0109] Find the keyframe whose pose is closest to the pose of the keyframe added at the current time step in the currently constructed submap, and construct a binary edge constraint between the poses of the two; and / or, find the keyframe at the previous time step in the currently constructed submap, and construct a binary edge constraint between the poses of the two.

[0110] It should be noted that if two pose nodes have constructed binary edge constraints, they will only be constructed once.

[0111] Example 2: The average pose of each keyframe in each sub-map is calculated to obtain the average pose of each sub-map. A quadtree is constructed based on the average pose of each sub-map. A sub-map selection range is set based on the pose of the keyframe at the current time. If the sub-map selection range is the first selection range, sub-maps with average poses within the first selection range are selected from the quadtree to obtain multiple initial sub-maps. Sub-maps containing keyframes at the current time are deleted from the multiple initial sub-maps to obtain candidate sub-maps selected within the first selection range. The keyframe at the current time and the candidate sub-maps are matched to calculate the matching score. Candidate sub-maps with matching scores greater than a preset score threshold are selected as target sub-maps. The keyframe with the closest pose to the current pose among all keyframes of the target sub-map is selected to obtain the target keyframe. Binary edge constraints are added to the keyframe at the current time and the target keyframe.

[0112] Taking environmental perception data as point cloud data as an example, after constructing the submap, the mean of the point cloud is calculated using all the point clouds in the submap, and the calculated mean is stored as the pose of the submap in a quadtree. In the quadtree, the master node represents the average pose of any submap, and the child nodes corresponding to the master node represent the average poses of the other submaps closest to the master node.

[0113] Based on the pose of the keyframe at the current moment, a certain range from the pose at the current moment is selected as the first selection range. For example, the range within 5m from the pose at the current moment is selected as the first selection range.

[0114] Select submaps whose average pose is within the first selection range from the quadtree, and remove submaps that contain keyframes at the current time to obtain candidate submaps.

[0115] Similarly, the branch and bound algorithm is used to match the current keyframe with each candidate submap, calculating the matching degree between each candidate submap and the current keyframe to obtain initial matching values. The pose search range for branch and bound is ±0.25m, and the angle search range is ±15 degrees.

[0116] Then, the keyframe at the current moment is matched with the obtained submap using a scan-to-map matching method to obtain the matching score between the keyframe at the current moment and the submap.

[0117] If the matching score between the current keyframe and the submap is greater than the preset score threshold, then the submap is taken as the target submap. The keyframe whose pose is closest to the current pose among all keyframes of the target submap is selected to obtain the target keyframe. A binary edge constraint is added between the poses corresponding to the current keyframe and the target keyframe. This binary edge constraint is the nearest neighbor keyframe constraint between submaps.

[0118] For example, please see Figure 4 , Figure 4 This is a schematic diagram illustrating nearest-neighbor keyframe constraints in an exemplary embodiment of this application, as shown below. Figure 4 As shown, the yellow line segments represent the nearest neighbor keyframe constraints between the added sub-maps, which improve the effect of cumulative error optimization.

[0119] Example 3: The average pose of each keyframe in each sub-map is calculated to obtain the average pose of each sub-map. A quadtree is constructed based on the average pose of each sub-map. A sub-map selection range is set based on the pose of the keyframe at the current time. If the sub-map selection range is a second selection range, sub-maps with average poses within the second selection range are selected from the quadtree to obtain multiple second initial sub-maps. Keyframes within the historical time period corresponding to the keyframe at the current time are obtained to obtain time-adjacent keyframes. Sub-maps containing both the keyframe at the current time and time-adjacent keyframes are deleted from multiple second initial sub-maps to obtain candidate sub-maps selected within the second selection range. The keyframe at the current time and candidate sub-maps are matched to calculate the matching score. Candidate sub-maps with matching scores greater than a preset score threshold are selected as target sub-maps. The keyframe with the pose closest to the current pose among all keyframes of the target sub-map is selected to obtain the target keyframe. Binary edge constraints are added to the keyframe at the current time and the target keyframe.

[0120] Similar to Example 2, the difference lies in the selection method of candidate sub-maps.

[0121] Based on the pose of the keyframe at the current moment, a second selection range is defined as a certain range from the pose at the current moment. The second selection range is larger than the first selection range.

[0122] Furthermore, it retrieves keyframes within the historical time period corresponding to the current keyframe, thus obtaining temporally adjacent keyframes. For example, it uses breadth-first search to find keyframes that can be traced back from the current frame, thus obtaining temporally adjacent keyframes.

[0123] Select submaps whose average pose is within the second selection range from the quadtree, and remove submaps that contain keyframes at the current time or keyframes that are close in time, to obtain candidate submaps.

[0124] Then, the branch and bound algorithm is used to search for poses in each candidate submap and calculate the matching score. In Example 3, the search range is 8m, which is much larger than the search range of Example 2, and the angle search range is ±40° to achieve loop closure detection and ensure that binary edge constraints are added in time during loop closure detection if there are no effective constraints with neighboring frames.

[0125] Similarly, candidate sub-maps with matching scores greater than a preset score threshold are selected as target sub-maps. The key frame whose pose is closest to the current pose among all key frames of the target sub-map is selected to obtain the target key frame. Binary edge constraints are added to the key frame at the current time and the target key frame. These binary edge constraints are loop constraints between sub-maps.

[0126] In this context, the covariance matrix corresponding to the binary edge constraint is appropriately increased or decreased by the matching score, and the covariance is directly proportional to the matching score. The covariance matrix of the binary edge constraint represents the noise distribution of the relative motion observation and determines the weight of the binary edge constraint during cumulative error optimization.

[0127] For example, please see Figure 5 , Figure 5 This is a schematic diagram illustrating a lap edge constraint in an exemplary embodiment of this application, as shown below. Figure 5 As shown, the red lines represent loop edge constraints. These constraints promptly eliminate accumulated errors at the front end, ensuring that the constructed incremental map aligns with the prior map.

[0128] In Embodiments 2 and 3 above, by using a quadtree to manage the pose of the submap center point, when adding nearest neighbor keyframe constraints and binary edge constraints, it is not necessary to traverse and search each submap from scratch. Instead, the spatial location information of the quadtree is used to quickly search for submaps that meet a certain range to construct effective binary edge constraints, thereby reducing the required computing resources and improving the efficiency of binary edge constraint construction.

[0129] We combine univariate edge constraints and bivariate edge constraints to optimize the cumulative error, thereby minimizing the overall cumulative error.

[0130] Position x i With pose x j The corresponding cumulative error is calculated as shown in Formula 4 below:

[0131]

[0132] Among them, t i For pose x i The corresponding translation part; t j For pose x j The corresponding translation part; t ij For pose x j In position x i Relative pose x ij The corresponding translation part; R i For pose x i The corresponding rotation matrix; R j For pose x j The corresponding rotation matrix; R ij For relative pose x ij The corresponding rotation matrix; θ i For pose x i The corresponding angle; θ j For pose x j The corresponding angle; θ ij For relative pose x ijThe corresponding angle.

[0133] The pose x = {t} mentioned above x ,t y The transformation relationship between θ and the corresponding pose transformation matrix T is expressed by the following formula 5:

[0134]

[0135] In some implementations, after adding unary edge constraints and / or binary edge constraints between adjacent frames in the same submap, nearest keyframes of spatially adjacent submaps, and loop closure detection frames, an accumulated error optimization is triggered every time a loop closure is detected or every preset number of unary edges are added. For example, factor graphs are used to optimize each pose in each submap.

[0136] Then, in response to the completion of cumulative error optimization, each sub-map is marked as to be reconstructed; after updating the obstacle probability of the grid for the currently constructed sub-map, the currently constructed sub-map is marked as reconstructed; in the above embodiments 2 and 3, before matching the keyframe and candidate sub-map at the current time and calculating the matching score of the keyframe and candidate sub-map at the current time, the method further includes: after updating the obstacle probability of the grid for the candidate sub-map selected from the quadtree, the currently constructed sub-map is marked as reconstructed.

[0137] Specifically, after cumulative error optimization, the flag position of each submap is set to false, indicating that the submap needs to be reconstructed.

[0138] The obstacle probabilities of the raster are updated regularly for the currently constructed submap. Then, the currently constructed submap is marked as true, indicating that it has been reconstructed. The obstacle probability update method can be found in Formula 3. By updating the currently constructed submap in a timely manner, it is ensured that the correct result is obtained when the next keyframe is inserted and matched with the current submap.

[0139] When using a quadtree to find neighboring frames and loopback frames, for candidate submaps selected from the quadtree, the obstacle probability of the raster is first updated and marked as true before branch and bound search is performed. By updating the obstacle probability only for the submaps that need to be used, the time-consuming process of rebuilding all submaps after optimization is avoided. Furthermore, using a flag to mark whether a submap needs to be rebuilt avoids the time-consuming process of frequently rebuilding submaps when there is no cumulative error optimization, which would affect the front-end matching efficiency.

[0140] Optionally, after cumulative error optimization, the pose change of the submap before and after optimization will be judged. When the displacement change is less than a preset displacement threshold (e.g., 1mm) and the angle change is less than a preset angle threshold (e.g., 0.1°), the submap does not need to be reconstructed, thus reducing reconstruction time.

[0141] Figure 6 This is a block diagram illustrating an incremental mapping apparatus as shown in an exemplary embodiment of this application. Figure 6 As shown, the exemplary incremental mapping device 600 includes:

[0142] The pose calculation module 610 is used to calculate the pose of the mobile device based on the odometry data of the mobile device and / or the collected environmental perception data.

[0143] The first positioning module 620 is used to stop using the prior map for positioning if the pose of the mobile device indicates that the mobile device is outside the range of the prior map, and to build an incremental map for positioning based on the collected environmental perception data.

[0144] The second positioning module 630 is used to enable the prior map for positioning if the pose of the mobile device indicates that the mobile device has moved from outside the prior map range to within the prior map range, and to match the environmental perception data and the prior map at the current moment based on the branch and bound algorithm to obtain the matching score corresponding to the successfully matched pose.

[0145] The unary edge adding module 640 is used to add unary edge constraints to the poses corresponding to environmental perception data whose matching scores are greater than a preset score threshold; wherein, the unary edge constraint is the constraint relationship between a single pose and the prior map.

[0146] Error optimization module 650 is used to optimize the cumulative error of the calculated pose using unary edge constraints so that the incremental map is aligned with the prior map.

[0147] It should be noted that the incremental mapping device and the incremental mapping method provided in the above embodiments belong to the same concept. The specific ways in which each module and unit performs operations have been described in detail in the method embodiments and will not be repeated here. In practical applications, the incremental mapping device provided in the above embodiments 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. This is not a limitation here.

[0148] Please see Figure 7 , Figure 7This is a schematic diagram illustrating the structure of a mobile device according to an exemplary embodiment of this application. The mobile device 700 includes a memory 701 and a processor 702. The processor 702 is used to execute program instructions stored in the memory 701 to implement the steps in any of the above-described incremental mapping method embodiments.

[0149] Specifically, processor 702 controls itself and memory 701 to implement the steps in any of the incremental mapping method embodiments described above. Processor 702 can also be referred to as a Central Processing Unit (CPU). Processor 702 may be an integrated circuit chip with signal processing capabilities. Processor 702 can also be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. A general-purpose processor can be a microprocessor or any conventional processor. Furthermore, processor 702 can be implemented using integrated circuit chips.

[0150] Please see Figure 8 , Figure 8 This is a schematic diagram illustrating the structure of a computer-readable storage medium in an exemplary embodiment of this application. The computer-readable storage medium 800 stores program instructions 810 that can be executed by a processor. The program instructions 810 are used to implement the steps in any of the above-described incremental mapping method embodiments.

[0151] In some embodiments, the functions or modules of the apparatus provided in this disclosure can be used to perform the methods described in the above method embodiments. The specific implementation can be referred to the description of the above method embodiments, and for the sake of brevity, it will not be repeated here.

[0152] The description of the various embodiments above tends to emphasize the differences between the various embodiments. The similarities or similarities between them can be referred to, and for the sake of brevity, they will not be repeated here.

[0153] In the several embodiments provided in this application, it should be understood that the disclosed methods and apparatus can be implemented in other ways. For example, the apparatus implementations described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the mutual coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection of devices or units may be electrical, mechanical, or other forms.

[0154] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part 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 is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute all or part of the steps of the methods in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

Claims

1. An incremental mapping method, characterized in that, The method includes: The pose of the mobile device is calculated based on the odometer data of the mobile device and / or the collected environmental perception data. If the pose of the mobile device indicates that the mobile device is outside the range of the prior map, then the positioning using the prior map is stopped, and an incremental map is constructed based on the collected environmental perception data for positioning. The incremental map contains multiple sub-maps, constructed based on the collected keyframes and their corresponding calculated poses. The keyframes refer to the environmental perception data used for pose calculation. Binary edge constraints are constructed between the poses corresponding to keyframes in the sub-maps that satisfy preset edge construction conditions. These binary edge constraints are constraints between two poses, including loop constraints. If the pose of the mobile device indicates that the mobile device has moved from outside the prior map area to within the prior map area, then the prior map is used for positioning, and the environmental perception data at the current moment is matched with the prior map based on the branch and bound algorithm to obtain the matching score corresponding to the successfully matched pose. Add unary edge constraints to the poses corresponding to environmental perception data whose matching scores are greater than a preset score threshold; wherein, the unary edge constraint is the constraint relationship between a single pose and the prior map; If the addition of a loop edge constraint is detected, or if a preset number of unary edge constraints are added, then the cumulative error optimization is performed on the currently calculated pose. In response to the completion of cumulative error optimization, each sub-map is marked as to be reconstructed; After updating the obstacle probability of the currently constructed sub-map using a grid, the currently constructed sub-map is marked as reconstructed so that the incremental map is aligned with the prior map.

2. The method according to claim 1, characterized in that, The calculation of the pose of the mobile device based on odometer data from the mobile device and / or collected environmental perception data includes: The pose of the previous moment is obtained, the time interval between the previous moment and the current moment is calculated, and the change in odometer data between the previous moment and the current moment is calculated; wherein, the previous moment is before the current moment; If the time interval is not greater than a preset time threshold and the change in the odometer data is not greater than a preset change threshold, then the pose at the current moment is predicted based on the pose at the previous moment and the change in the odometer data between the previous moment and the current moment. If the time interval is greater than a preset time threshold, or the change in the odometer data is greater than a preset change threshold, then based on the pose of the previous moment and the odometer data of the current moment, the initial pose of the current moment is predicted, the environmental perception data of the current moment is obtained, and based on the initial pose of the current moment and the environmental perception data of the current moment, the pose of the current moment is calculated.

3. The method according to claim 2, characterized in that, The calculation of the pose at the current moment based on the initial pose at the current moment and the environmental perception data at the current moment includes: Obtain the coordinates of the current environmental perception data in the device coordinate system of the mobile device to obtain the perception data device coordinates; Based on the initial pose at the current moment, the coordinates of the sensing data device are transformed to the world coordinate system corresponding to the prior map to obtain the world coordinates of the sensing data; Calculate the distance between the world coordinates of the perceived data and the obstacles in the prior map, and filter out points with a distance less than a preset distance threshold from the environmental perception data at the current moment to obtain long-term features; The long-term features are matched with the prior map to obtain the pose at the current moment.

4. The method according to claim 1, characterized in that, The prior map stores the coordinates of each obstacle; the method further includes: Calculate the maximum x-coordinate, maximum y-coordinate, minimum x-coordinate, and minimum y-coordinate of each obstacle in the prior map; If the x-coordinate of the pose of the mobile device is greater than the minimum x-coordinate and less than the maximum x-coordinate, and the y-coordinate of the pose of the mobile device is greater than the minimum y-coordinate and less than the maximum y-coordinate, then it is determined that the mobile device is within the range of the prior map.

5. The method according to claim 1, characterized in that, The step of constructing binary edge constraints between the poses corresponding to keyframes in the sub-map that satisfy the preset edge construction conditions includes: Get the submap built at the current moment; Query the keyframe whose pose is closest to the keyframe added at the current time in the sub-map constructed at the current time, and / or query the keyframe of the previous time in the sub-map constructed at the current time to obtain adjacent keyframes; Add binary edge constraints between the poses corresponding to the keyframe added at the current time and the adjacent keyframes.

6. The method according to claim 1, characterized in that, The step of constructing binary edge constraints between the poses corresponding to keyframes in the sub-map that satisfy the preset edge construction conditions includes: The average pose of each keyframe in each sub-map is calculated to obtain the average pose of each sub-map. A quadtree is constructed based on the average pose corresponding to each sub-map; wherein, the master node in the quadtree is the average pose of any sub-map, and the child node corresponding to the master node is the average pose of the other sub-map closest to the master node; Based on the pose of the keyframe at the current moment, the selection range of the sub-map is set; From the quadtree, select the sub-maps whose average pose is within the sub-map selection range to obtain candidate sub-maps; The keyframe at the current moment and the candidate sub-map are matched to calculate the matching score between the keyframe at the current moment and the candidate sub-map. The candidate sub-map with a matching score greater than a preset score threshold is taken as the target sub-map. The key frame whose pose is closest to the pose at the current time is selected from all key frames of the target sub-map to obtain the target key frame. Add a binary edge constraint between the pose corresponding to the current keyframe and the target keyframe.

7. The method according to claim 6, characterized in that, The sub-map selection range includes a first selection range and a second selection range, wherein the second selection range is larger than the first selection range; the step of selecting sub-maps whose average pose falls within the sub-map selection range from the quadtree to obtain candidate sub-maps includes: Select sub-maps whose average pose is within the first selection range from the quadtree to obtain multiple first initial sub-maps; delete the sub-maps containing the keyframe of the current time from the multiple first initial sub-maps to obtain candidate sub-maps selected within the first selection range; Select sub-maps whose average pose is within the second selection range from the quadtree to obtain multiple second initial sub-maps; obtain keyframes within the historical time period corresponding to the keyframe at the current time to obtain time-adjacent keyframes; delete sub-maps containing the keyframe at the current time or the time-adjacent keyframes from the multiple second initial sub-maps to obtain candidate sub-maps selected within the second selection range.

8. The method according to claim 6, characterized in that, Before matching the keyframe at the current moment with the candidate sub-map and calculating the matching score between the keyframe at the current moment and the candidate sub-map, the method further includes: After updating the obstacle probability of the candidate sub-map selected from the quadtree, the currently constructed sub-map is marked as reconstructed.

9. A mobile device, characterized in that, The mobile device includes a memory and a processor, the processor being configured to execute program instructions stored in the memory to implement the steps of the method as described in any one of claims 1-8.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores program instructions that can be executed by a processor to implement the steps of the method as described in any one of claims 1-8.

Images