Laser SLAM fusion positioning method with environmental adaptability

Abstract

The invention relates to a laser SLAM fusion positioning method with environmental adaptability, and belongs to the technical field of autonomous mobile platform positioning. The method comprises the following steps: firstly, constructing a plurality of parallel sub-models comprising different laser SLAM positioning methods, then calculating the observation likelihood value of each sub-model in real time based on the matching quality of the current laser point cloud and a prior map, and updating the conformity of each sub-model in combination with the transfer relationship between the models; taking the conformity as a weight, carrying out weighted fusion on the pose estimation and covariance output by each sub-model, and generating a global positioning result; and finally, feeding back the posterior pose, the covariance and the conformity of each sub-model to the next period, and carrying out positioning information interaction to form closed-loop adaptive fusion positioning. According to the method, the robustness and the positioning precision of the laser SLAM system in a complex, dynamic or structure degradation environment are improved through adaptive fusion of a plurality of sub-models, and the problem of positioning drift or failure caused by incompatibility of the environment and the model is effectively avoided.

Description

Technical Field

[0001] This invention belongs to the field of autonomous mobile platform positioning technology, and relates to a laser SLAM fusion positioning method with environmental adaptability. Background Technology

[0002] Laser SLAM positioning technology is a core means for autonomous mobile platforms, such as intelligent vehicles and intelligent robots, to achieve autonomous positioning. It uses lidar sensors to capture environmental features and enable autonomous mobile platforms to locate themselves in unknown environments.

[0003] In existing technologies, most methods employ a single algorithm or a hard handover strategy based on a simple threshold. These methods have significant drawbacks: first, a single algorithm generally cannot adapt to all environments, leading to performance degradation or even failure when the environment changes due to incompatibility; second, the hard handover mechanism lacks a smooth transition, easily causing abrupt changes in positioning results, and the handover threshold usually relies on manual experience for setting, making it difficult to generalize to unknown or complex and variable scenarios, and failing to reflect the evolutionary trend of the environmental state.

[0004] Prior art, disclosed in publication number CN120313606A, presents a positioning method, system, and storage medium based on multi-sensor fusion. It aims to address the degradation problem of laser SLAM positioning caused by changes in the warehouse environment through strategies of local dynamic adaptation, global static correction, and multi-source fusion. However, this patent does not support adaptive fusion positioning of multiple SLAM models.

[0005] The prior art disclosed in CN118348506A discloses a laser SLAM localization method, device and storage medium based on a planar map. It uses a method of voxelization from coarse to fine segmentation + region growing to extract planar features, which aims to solve the problem of insufficient real-time performance in laser SLAM due to its reliance on a large number of point cloud matching. However, this patent does not support adaptive fusion localization of multiple SLAM models.

[0006] Laser SLAM positioning technology plays an important role in the field of autonomous mobile platforms. However, current technologies lack the ability to adapt to environmental changes and cannot guarantee stable and effective positioning in complex and ever-changing scenarios. Summary of the Invention

[0007] In view of this, the purpose of the present invention is to provide an environmentally adaptive laser SLAM fusion positioning method.

[0008] To achieve the above objectives, the present invention provides the following technical solution: An environment-adaptive laser SLAM fusion localization method, comprising: S1, Construction The system generates SLAM localization sub-models with different parallel laser SLAM localization strategies and initializes the initial pose estimation vector, covariance matrix, and conformity of each sub-model. S2. In subsequent time steps, acquire the posterior information of all SLAM localization sub-models, and calculate the transition probabilities between each model from the sub-models. Switch to sub-model The conditional probability is then used to calculate the sub-model. The initial pose of the mixture and the mixture covariance matrix are calculated. S3. Based on the initial pose of the sub-model and the laser point cloud collected at the current time, calculate its pose estimation vector and its corresponding covariance matrix, and calculate the matching error value in the process of matching its laser point cloud with the prior map. S4. Calculate the observed likelihood value based on the matching error value of the sub-model, and update the conformity of the sub-model at the current time step by combining the transition probability and the conformity at the previous time step. S5. Perform weighted fusion based on the conformity of each sub-model and its pose estimation vector at the current time to obtain the global localization result at the current time; S6. Use the pose estimation vector, covariance matrix and conformity of each laser SLAM localization sub-model at the current moment as the initial pose of the mixture at the next moment, and repeat steps S2 to S6 for continuous localization.

[0009] Furthermore, in step S1, at the initial time... At that time, build A parallel laser SLAM localization sub-model, Each sub-model corresponds to a laser SLAM localization strategy; initialize the sub-model. Initial pose estimation vector and its covariance matrix and initial values ; These represent mobile platforms. Coordinates and azimuth.

[0010] Furthermore, in step S2, the posterior information of all sub-models includes the pose estimation vector. Covariance matrix and compliance , Then, calculate the current state of the system at the specified time. Given the first laser SLAM positioning sub-model, the system at the previous time step conformed to the first... Conditional probabilities of a laser SLAM localization sub-model :

[0011] in, Indicates from the first The laser SLAM positioning submodel was switched to the [number]th [model]. Preset transition probabilities for each laser SLAM positioning sub-model; Then each laser SLAM localization sub-model at the current time The mixed initial pose and mixed covariance are expressed as:

[0012]

[0013] in, Indicates the time step index at the current moment; Indicates the first The laser SLAM positioning sub-model at the previous moment The posterior pose estimation vector; Indicates the first The laser SLAM positioning sub-model at the previous moment The posterior covariance matrix.

[0014] Furthermore, in step S3, the sub-model positioning information update process includes: S31. For each laser SLAM positioning sub-model With its mixed initial pose As the initial pose, the laser point cloud acquired at the current moment will be used. With prior maps The matching process is performed, and the pose transformation at the current moment is solved using the laser SLAM localization sub-model. The pose estimation vector of the laser SLAM localization sub-model is then output. and its corresponding covariance matrix ; S32. Calculate the matching error based on the native matching quality index output by each laser SLAM positioning sub-model. The matching error is uniformly defined as a non-negative scalar value used to characterize the current matching quality of the laser SLAM positioning sub-model.

[0015] Furthermore, in step S32, the matching error The calculation process is as follows: Obtain the native matching quality index of each laser SLAM localization sub-model :

[0016] in, This represents the log-likelihood value output by the laser SLAM positioning sub-model. The mean square error between points. The number of interior point features, The total number of features involved in the matching; The original matching quality index is normalized to obtain the normalized error. :

[0017] in, For laser SLAM positioning sub-model In recent The maximum value of the native matching quality index within each time step; Calculate the matching error:

[0018] in, This is a preset upper limit threshold for error, used to suppress error divergence caused by abnormal matching.

[0019] Furthermore, in step S4, the sub-model and map conformity update process is as follows: First, for each laser SLAM positioning sub-model Based on its output matching error value Calculate the observed likelihood value :

[0020] in, This is the scaling factor; Then, combining the transition probability Consistency with the previous moment Update the conformity of each laser SLAM positioning sub-model at the current moment. :

[0021] in, Indicates the first The degree of conformity of each laser SLAM positioning sub-model at the current moment.

[0022] Furthermore, in step S5, the process of weighted fusion of all sub-models to obtain the global localization result is represented as follows: Based on the conformity of each sub-model Using the weights, the posterior poses of each laser SLAM localization sub-model are weighted and fused to generate the global localization result at the current time step:

[0023] in, This is the pose estimate for the final output.

[0024] Furthermore, in step S6, the posterior pose of each laser SLAM localization sub-model at the current time is... Covariance and conformity Time index from Convert to And as the initial pose for the next moment, repeat steps S2-S6 to continue global localization.

[0025] The beneficial effects of this invention are as follows: This invention proposes a laser SLAM localization method based on multi-model adaptive fusion. This method constructs multiple parallel laser SLAM localization sub-models and dynamically updates the conformity of each sub-model based on observation likelihood and model transition relationships, thereby achieving adaptive selection of a more reliable localization strategy for the current environment. In the prediction phase, through an information exchange mechanism, each sub-model can fuse historical information, effectively enhancing its adaptability to environmental changes and maintaining high localization stability even in complex, dynamic, or structurally degraded environments.

[0026] Furthermore, this invention uses conformity as a weight to perform weighted fusion of the positioning information and covariance of each laser SLAM positioning sub-model. The output not only has significantly better accuracy than a single laser SLAM positioning sub-model, but its uncertainty is also quantitatively evaluated. This invention can effectively avoid positioning drift or failure caused by incompatibility between the environment and the model, greatly improving the robustness and positioning accuracy of laser SLAM systems in complex environments, and providing more reliable technical support for fields such as autonomous driving and robot navigation.

[0027] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0028] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein: Figure 1 This is a schematic diagram of the overall process architecture of an environmentally adaptive laser SLAM fusion localization method according to an embodiment of the present invention. Detailed Implementation

[0029] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0030] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0031] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0032] Please see Figure 1 This is an environmentally adaptive laser SLAM fusion positioning method.

[0033] Example This embodiment details the specific process of an environment-adaptive laser SLAM fusion localization method, such as... Figure 1 As shown, the overall process includes six steps: initializing parameters and building sub-models, interaction of positioning information, updating the positioning information of sub-models, updating the conformity between sub-models and maps, weighted fusion of multiple models, and time transformation feedback loop. Specifically, it includes: Step 1: Initialize parameters and build sub-models When the positioning system starts, build Several parallel laser SLAM localization sub-models, among which Each sub-model corresponds to a laser SLAM localization strategy; for each sub-model Initialize its initial pose estimation vector and its covariance matrix And the conformity of each sub-model, its initial value is These initialization parameters will serve as the initial data for subsequent processing.

[0034] More specifically, it includes the following sub-steps: Step 1.1: Building Several parallel laser SLAM localization sub-models, among which Each sub-model corresponds to a laser SLAM localization strategy.

[0035] Step 1.2: Initialize its initial pose estimation vector and its covariance matrix And the conformity of each sub-model, its initial value is ; These represent mobile platforms. Coordinates and azimuth.

[0036] Step 2: Location Information Interaction At that moment At that time, it receives the posterior information of each laser SLAM localization sub-model from the previous moment, including the pose estimation vector. Covariance matrix and compliance Subsequently, based on the pre-defined inter-model transition probability model... ,in This indicates the location sub-model from laser SLAM. Switch to laser SLAM positioning sub-model The prior probability satisfies Calculate the conditional probability of the sub-model Next, for each laser SLAM localization sub-model at the current moment... Calculate the mixed initial pose And calculate the mixture covariance matrix Output of the mixed initial pose And calculate the mixture covariance matrix Used as the initial value for updating the sub-model's positioning information.

[0037] This embodiment provides a sub-step in step 2 to calculate the mixed initial pose and mixed covariance: Step 2.1: Calculate the current state of the system at the specified time. Given the first laser SLAM positioning sub-model, the system at the previous time step conformed to the first... Conditional probabilities of a laser SLAM localization sub-model :

[0038] in, Indicates from the first The laser SLAM positioning submodel was switched to the [number]th [model]. The preset transition probability of each laser SLAM positioning sub-model.

[0039] Step 2.2: Locate each laser SLAM sub-model at the current time. Calculate the mixed initial pose and the mixed covariance:

[0040]

[0041] in, Indicates the time step index at the current moment; Indicates the first The laser SLAM positioning sub-model at the previous moment The posterior pose estimation vector; Indicates the first The laser SLAM positioning sub-model at the previous moment The posterior covariance matrix.

[0042] Step 3: Sub-model positioning information update After the information exchange is completed, each laser SLAM positioning sub-model is processed. With its mixed initial pose As the initial pose, the laser point cloud acquired at the current moment will be used. With prior maps The matching process is performed, and the pose transformation at the current moment is solved using the laser SLAM localization sub-model. The pose estimation vector of the laser SLAM localization sub-model is then output. and its corresponding covariance matrix .

[0043] Then, a matching error value is generated based on the matching process of each laser SLAM positioning sub-model. This value is used to characterize the current matching quality. It is calculated based on the matching error generated by the laser SLAM localization sub-model during the matching process, and is obtained through normalization and scaling. This value is used in subsequent steps to evaluate the conformity between the laser SLAM localization sub-model and the prior map.

[0044] The output of this step , The initial values ​​are respectively used as inputs and feedbacks to the localization and hybrid pose at the next time step, which are then used as inputs and feedbacks to the multi-model weighted fusion. It will be used as a matching quality index to calculate the observed likelihood value in the conformity between the laser SLAM positioning sub-model and the map.

[0045] Specifically, the steps for updating the sub-model localization information include: Step 3.1: For each laser SLAM positioning sub-model With its mixed initial pose As the initial pose, the laser point cloud acquired at the current moment will be used. With prior maps The matching process is performed, and the pose transformation at the current moment is solved using the laser SLAM localization sub-model. The pose estimation vector of the laser SLAM localization sub-model is then output. and its corresponding covariance matrix .

[0046] Step 3.2: Calculate the matching error based on the native matching quality index output by each laser SLAM positioning sub-model. The uniform error is a non-negative scalar value used to characterize the current matching quality of the laser SLAM positioning sub-model.

[0047] More specifically, the calculation of the matching error in step 3.2 also includes the following sub-steps: Step 3.2.1: Obtain the native matching quality index of each laser SLAM localization sub-model :

[0048] in, This represents the log-likelihood value output by the laser SLAM positioning sub-model. The mean square error between points. The number of interior point features, This represents the total number of features involved in the matching.

[0049] Step 3.2.2: Normalize the original matching quality index to obtain the normalized error. :

[0050] in, For laser SLAM positioning sub-model In recent The maximum value of the native matching quality index within each time step.

[0051] Step 3.2.3: Calculate the matching error:

[0052] in, This is a preset upper limit threshold for error, used to suppress error divergence caused by abnormal matching.

[0053] Step 4: Sub-model and map alignment update Based on the matching quality of each laser SLAM positioning sub-model, its observation likelihood value is calculated. Subsequently, by combining the transition probabilities with the consistency of the previous time step, the consistency of each laser SLAM localization sub-model at the current time step is updated. The output of this step They will be used as inputs and feedbacks to the conditional probabilities in the localization hybrid pose at the next time step, respectively, for multi-model weighted fusion.

[0054] Specifically, the sub-model and map alignment update process in step 4 includes: Step 4.1: For each laser SLAM positioning sub-model Based on its output matching error value Calculate the observed likelihood value :

[0055] in, This is the scaling factor.

[0056] Step 4.2: Combine transition probabilities Consistency with the previous moment Update the conformity of each laser SLAM positioning sub-model at the current moment. :

[0057] in, Indicates the first The degree of conformity of each laser SLAM positioning sub-model at the current moment.

[0058] Step 5: Weighted Fusion of Multiple Models First, the conformity of each laser SLAM positioning sub-model was assessed. As the weights, the posterior poses of each laser SLAM localization sub-model are... Weighted fusion is used to generate the global localization result at the current moment. .

[0059] The steps of multi-model weighted fusion are: based on compliance Using the weights, the posterior poses of each laser SLAM localization sub-model are weighted and fused to generate the global localization result at the current time step:

[0060] in, This is the pose estimate for the final output.

[0061] Step 6: Unit Delay: Calculate the posterior pose of each laser SLAM localization sub-model at the current time. Covariance and conformity Time index from Convert to And use it as the initial pose for the next moment to repeat steps 2 to 6.

[0062] The systems, apparatuses, methods, modules, or units described in the above embodiments may be implemented by computer chips or entities, or by products with certain functions.

[0063] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A laser SLAM fusion localization method with environmental adaptability, characterized in that: The method includes: S1, Construction The system generates SLAM localization sub-models with different parallel laser SLAM localization strategies and initializes the initial pose estimation vector, covariance matrix, and conformity of each sub-model. S2. In subsequent time steps, acquire the posterior information of all SLAM localization sub-models, and calculate the transition probabilities between each model from the sub-models. Switch to sub-model The conditional probability is then used to calculate the sub-model. The initial pose of the mixture and the mixture covariance matrix are calculated. S3. Based on the hybrid initial pose of the sub-model and the laser point cloud collected at the current time, calculate its pose estimation vector and its corresponding covariance matrix, and calculate the matching error value in the process of matching its laser point cloud with the prior map. S4. Calculate the observed likelihood value based on the matching error value of the sub-model, and update the conformity of the sub-model at the current time step by combining the transition probability and the conformity at the previous time step. S5. Perform weighted fusion based on the conformity of each sub-model and its pose estimation vector at the current time to obtain the global localization result at the current time; S6. Use the pose estimation vector, covariance matrix and conformity of each laser SLAM localization sub-model at the current moment as the initial pose of the mixture at the next moment, and repeat steps S2 to S6 for continuous localization.

2. The laser SLAM fusion localization method with environmental adaptability according to claim 1, characterized in that: In step S1, at the initial time At that time, build A parallel laser SLAM localization sub-model, Each sub-model corresponds to a laser SLAM localization strategy; initialize the sub-model. Initial pose estimation vector and its covariance matrix and initial values ; These represent mobile platforms. Coordinates and azimuth.

3. The laser SLAM fusion positioning method with environmental adaptability according to claim 2, characterized in that: In step S2, the posterior information of all sub-models includes the pose estimation vector. Covariance matrix and compliance , Then, calculate the current state of the system at the specified time. Given the first laser SLAM positioning sub-model, the system at the previous time step conformed to the first... Conditional probabilities of a laser SLAM localization sub-model : in, Indicates from the first The laser SLAM positioning submodel was switched to the [number]th [model]. Preset transition probabilities for each laser SLAM positioning sub-model; Then each laser SLAM localization sub-model at the current time The mixed initial pose and mixed covariance are expressed as: in, Indicates the time step index at the current moment; Indicates the first The laser SLAM positioning sub-model at the previous moment The posterior pose estimation vector; Indicates the first The laser SLAM positioning sub-model at the previous moment The posterior covariance matrix.

4. The environment-adaptive laser SLAM fusion positioning method according to claim 3, characterized in that: In step S3, the sub-model positioning information update process includes: S31. For each laser SLAM positioning sub-model With its mixed initial pose As the initial pose, the laser point cloud acquired at the current moment will be used. With prior maps The matching process is performed, and the pose transformation at the current moment is solved using the laser SLAM localization sub-model. The pose estimation vector of the laser SLAM localization sub-model is then output. and its corresponding covariance matrix ; S32. Calculate the matching error based on the native matching quality index output by each laser SLAM positioning sub-model. The matching error is uniformly defined as a non-negative scalar value used to characterize the current matching quality of the laser SLAM positioning sub-model.

5. The environment-adaptive laser SLAM fusion localization method according to claim 4, characterized in that: In step S32, the matching error The calculation process is as follows: Obtain the native matching quality index of each laser SLAM localization sub-model : in, This represents the log-likelihood value output by the laser SLAM positioning sub-model. For point-to-point mean square error, The number of interior point features, The total number of features involved in the matching; The original matching quality index is normalized to obtain the normalized error. : in, For laser SLAM positioning sub-model In recent The maximum value of the native matching quality index within each time step; Calculate the matching error: in, This is a preset upper limit threshold for error, used to suppress error divergence caused by abnormal matching.

6. The environment-adaptive laser SLAM fusion positioning method according to claim 4, characterized in that: In step S4, the sub-model and map conformity update process is as follows: First, for each laser SLAM positioning sub-model Based on its output matching error value Calculate the observed likelihood value : in, This is the scaling factor; Then, combining the transition probability Consistency with the previous moment Update the conformity of each laser SLAM positioning sub-model at the current moment. : in, Indicates the first The degree of conformity of each laser SLAM positioning sub-model at the current moment.

7. The environment-adaptive laser SLAM fusion positioning method according to claim 6, characterized in that: In step S5, the process of weighted fusion of all sub-models to obtain the global localization result is represented as follows: Based on the conformity of each sub-model Using the weights, the posterior poses of each laser SLAM localization sub-model are weighted and fused to generate the global localization result at the current time step: in, This is the pose estimate for the final output.

8. The environment-adaptive laser SLAM fusion localization method according to claim 7, characterized in that: In step S6, the posterior pose of each laser SLAM localization sub-model at the current time is obtained. Covariance and conformity Time index from Convert to And as the initial pose for the next moment, repeat steps S2-S6 to continue global localization.

Images