Indoor mobile robot autonomous positioning method and device for multiple dynamic scenes

Abstract

The invention provides an indoor mobile robot autonomous positioning method and device for multiple dynamic scenes, and the method comprises the steps: carrying out the preprocessing of point cloud data obtained by an indoor mobile robot, and obtaining first point cloud data; performing pyramid preheating registration processing on the point cloud data of adjacent time to obtain an initial speedometer of the indoor mobile robot, and selecting a plurality of key frames to generate a sub-graph; performing pyramid preheating registration processing on point cloud data obtained in real time and the sub-graphs to obtain a real-time odometer of the indoor mobile robot; performing optimization processing on the real-time odometer by using a multi-constraint factor graph to obtain an optimized odometer; and constructing an environment model based on the optimized speedometer and the point cloud data obtained by the indoor mobile robot to realize autonomous positioning of the indoor mobile robot.

Description

Technical Field

[0001] The present application relates to the field of indoor mobile robot positioning technology, and in particular to an indoor mobile robot autonomous positioning method and device for multiple dynamic scenes. Background Art

[0002] In recent years, mobile robots have been increasingly used in complex indoor space scenarios, involving many fields such as public safety inspection, smart home services, warehousing and logistics, and medical care. In indoor environments, there are often complex scenarios where multiple dynamic targets (such as people, pets, mobile devices, etc.) appear at the same time and move randomly.

[0003] However, in indoor scenarios with multiple dynamic obstacles, GPS is unavailable and there are many environmental interference factors, making it difficult for indoor mobile robots to perform autonomous positioning.

[0004] Therefore, the present application provides an autonomous positioning method and device for an indoor mobile robot in multiple dynamic scenarios. Summary of the invention

[0005] The embodiments of the present application provide an autonomous positioning method and device for an indoor mobile robot in multi-dynamic scenarios, so as to solve the problem in the prior art that an indoor mobile robot has difficulty in autonomous positioning in indoor multi-dynamic obstacle scenarios.

[0006] In a first aspect, an embodiment of the present application provides an autonomous positioning method for an indoor mobile robot in multiple dynamic scenarios, including:

[0007] Preprocessing the point cloud data obtained by the indoor mobile robot to obtain first point cloud data;

[0008] Performing pyramid preheating registration processing on point cloud data at adjacent times to obtain the initial odometer of the indoor mobile robot, and selecting multiple key frames to generate sub-graphs;

[0009] Performing pyramid preheating registration processing on the point cloud data obtained in real time and the sub-image to obtain a real-time odometer of the indoor mobile robot;

[0010] Optimizing the real-time odometer using a multi-constraint factor graph to obtain an optimized odometer;

[0011] An environmental model is constructed based on the optimized odometer and the point cloud data obtained by the indoor mobile robot to achieve autonomous positioning of the indoor mobile robot.

[0012] In a feasible implementation, the point cloud data obtained by the indoor mobile robot is preprocessed, and the preprocessing includes:

[0013] Performing motion compensation processing on the point cloud data, the motion compensation comprising:

[0014] Using a spherical linear interpolation method to compensate for the rotation vector of the point cloud data and using a linear interpolation method to perform motion compensation on the translation vector;

[0015] The method of using a spherical linear interpolation method to compensate for the rotation vector of the point cloud data includes:

[0016] Assume that the initial time and end time of a certain frame radar scan are t start and t end ;

[0017] The pose transformation matrix of the indoor mobile robot corresponding to the initial time and the end time is T start With T end ; At any time t∈[t start ,t end ] The corresponding posture transformation matrix is ​​recorded as T(t), and the time scale coefficient is defined as

[0018] Let q start ,q end They represent the quaternions of the starting posture and the ending posture respectively, and θ represents the angle between the two on the sphere. Then the quaternion interpolation corresponding to any time α can be written as

[0019]

[0020] The interpolation result Slerp(q start ,q end ,α) is converted into R(α), which is used to describe the posture of the indoor mobile robot at time α;

[0021] The method of using a linear interpolation method to perform motion compensation on the translation vector includes:

[0022] Assume t start ,t end

[0023] are the translation vectors at the start and end times respectively, then the translation vector at time α is

[0024] t(α)=t start +α(t end -t start ).

[0025] The corresponding pose transformation matrix is

[0026]

[0027] Perform motion compensation on any laser point p to obtain the distortion-free point cloud p′:

[0028] p′=T(α)p,

[0029] where α depends on the sampling timestamp of the laser point in the current frame.

[0030] In a feasible implementation, pyramid preheating registration processing is performed on point cloud data at adjacent times to obtain an initial odometer of the indoor mobile robot, including:

[0031] Use an adaptive voxel filter to divide the point cloud data into a preheating layer and a registration layer according to the resolution;

[0032] The preheating layer completes a fast estimation of the posture of the indoor mobile robot based on the low-resolution data;

[0033] Reusing the covariance matrix of the point cloud data using the rigid invariant attribute, and processing the point cloud data in layers and levels;

[0034] In the registration layer, a sparse covariance matrix is ​​constructed by associating the initial down-sampled points with the nearest neighbor points of the original point cloud to mine geometric features. In the stage of eliminating mismatched point pairs, a filter combining distance, normal vector and curvature information is introduced to accurately shield obvious mismatched areas in dynamic environments.

[0035] In a feasible implementation, the step of selecting a key frame to generate a sub-image includes:

[0036] The discrete coefficient and the pose change coefficient are introduced, and the anti-slip strategy is used to dynamically adjust the selection distance of the key frame, including:

[0037] Let σ d and μ d They represent the standard deviation and average value of the distance between points in the current scan point cloud, respectively. The environmental dispersion coefficient is recorded as

[0038]

[0039] Let σ p With μ p Represent the standard deviation and average value of the pose difference norm ‖ΔT‖ of the most recent frames, respectively. Then the pose change coefficient is

[0040]

[0041] Normalize the environmental dispersion coefficient and the pose change coefficient to the interval [0,1] respectively;

[0042] Then, the key frame selects the distance D k The dynamic adjustment formula is:

[0043] D k =max(D min ,min(D max ,D max ·[1-exp(-β(C d -C p ))])),

[0044] Where D max is the maximum allowed distance between key frames, D min is the preset minimum distance, β represents the curve shape factor, which is used to control the key frame distance with C d -C p growth rate;

[0045] If, if C d -C p The exponential term is small, so the key frame distance is kept at a low level, thus avoiding missing key information in narrow or multi-twist scenes; if C d >C p , it means that the environment gradually becomes empty and the posture changes are not drastic. The key frame selection distance can be increased to reduce the number of redundant key frames and speed up the subsequent calculation efficiency.

[0046] In a feasible implementation, the step of selecting a key frame to generate a sub-image further includes:

[0047] The composite similarity evaluation method combining KL divergence and Jaccard index is used to divide the subgraphs;

[0048] The KL divergence includes:

[0049] Let the current keyframe point cloud be P and the nearest anchor point keyframe be Q, so that they satisfy the Gaussian distribution N(μ p ,Σ p ) and N(μ q ,Σ q ). Based on the approximate matrix form of Taylor expansion, the KL divergence is:

[0050]

[0051] Where tr(·) represents the trace operation, det(·) represents the determinant, d is the data dimension, and D KL The smaller it is, the more similar the probability distributions of P and Q are;

[0052] Jaccard index, including:

[0053]

[0054] Among them, |·| represents the number of points in the point cloud data, P∩Q and P∪Q are the intersection and union of the two point clouds, respectively. The larger the J(P,Q), the more overlapping areas the two point clouds have.

[0055] The composite similarity evaluation method comprises:

[0056] Composite evaluation index Λ(P,Q)=w KL (1-D KL (P‖Q))+w JC J(P,Q),

[0057] Among them, w KL With w JC are the weight coefficients of KL divergence and Jaccard index respectively.

[0058] In a feasible implementation, the optimizing process of the real-time odometer using a multi-constraint factor graph includes:

[0059] Multi-level constraints are established between keyframes, keyframes and subgraphs, and subgraphs and subgraphs, and compensation constraints are applied when closed loops are detected to optimize real-time odometry.

[0060] In a feasible implementation, constraints are established between key frames, including:

[0061] The source key frame and the target key frame are denoted as K s and K t , the transformation matrix T of the two is obtained through pyramid preheating registration processing st :

[0062] T st =argmin T (F(K s ,K t ,T)),

[0063] Where F represents the cost function of the registration:

[0064]

[0065] Where p and q represent the corresponding point clouds in the source and target point clouds respectively, Σ p and Σ q is the estimated covariance matrix corresponding to the two, and the pose constraint factor between the key frames is calculated.

[0066] In a feasible implementation, constraints are established between the keyframe and the subgraph, including:

[0067] The jth sub-graph is composed of several frames, including the most recent M key frames; let one of the key frames be K s, the target subgraph is denoted as G t , after registration, the transformation matrix is ​​obtained:

[0068] T s_g =argmin T (F(K s ,G t ,T)),

[0069] Where K s The covariance matrix corresponding to each point in G t The covariance matrices of corresponding points in jointly participate in the weighted error measurement.

[0070] In a feasible implementation, in the stage of eliminating mismatched points, a filter combining distance, normal vector and curvature information is introduced to accurately shield obvious mismatched areas in a dynamic environment, including:

[0071] The judgment condition for whether to remove the matching point is:

[0072]

[0073] Among them, n s ,n t Represents the normal vector between the source point cloud and the target point cloud, p s ,p t represents the point coordinates, κ is the curvature, T is the transformation matrix of the current registration, and δ is the set threshold. If the condition is not met, it is judged as a mismatched point pair and is removed.

[0074] In a second aspect, an embodiment of the present application provides an autonomous positioning device for an indoor mobile robot for multiple dynamic scenes, applying the autonomous positioning method for an indoor mobile robot for multiple dynamic scenes described in the first aspect, the device comprising:

[0075] Controller;

[0076] A preprocessing module, electrically connected to the controller, for preprocessing the point cloud data obtained by the indoor mobile robot to obtain first point cloud data;

[0077] A sub-graph generation module is electrically connected to the controller and is used to perform pyramid preheating registration processing on point cloud data at adjacent times, obtain an initial odometer of the indoor mobile robot, and select multiple key frames to generate a sub-graph;

[0078] A real-time odometer acquisition module is electrically connected to the controller and is used to perform pyramid preheating registration processing on the point cloud data obtained in real time and the sub-image to obtain the real-time odometer of the indoor mobile robot;

[0079] An optimization module, electrically connected to the controller, for optimizing the real-time odometer using a multi-constraint factor graph to obtain an optimized odometer;

[0080] The environment modeling module is electrically connected to the controller and is used to build an environment model based on the optimized odometer and the point cloud data obtained by the indoor mobile robot to achieve autonomous positioning of the indoor mobile robot.

[0081] The embodiment of the present application provides an autonomous positioning method for an indoor mobile robot for multiple dynamic scenarios, including preprocessing point cloud data obtained by the indoor mobile robot to obtain first point cloud data; performing pyramid preheating alignment processing on point cloud data at adjacent times to obtain an initial odometer of the indoor mobile robot, and selecting multiple key frames to generate a sub-graph; performing pyramid preheating alignment processing on the point cloud data obtained in real time and the sub-graph to obtain a real-time odometer of the indoor mobile robot; optimizing the real-time odometer using a multi-constraint factor graph to obtain an optimized odometer; constructing an environmental model based on the optimized odometer and the point cloud data obtained by the indoor mobile robot to achieve autonomous positioning of the indoor mobile robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] The drawings described herein are used to provide further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present application and do not constitute improper limitations on the present invention.

[0083] In the attached picture:

[0084] Figure 1 It is a flow chart of an autonomous positioning method for an indoor mobile robot facing multiple dynamic scenes provided by an embodiment of the present application;

[0085] Figure 2 It is a schematic diagram of pyramid preheating registration processing provided by an embodiment of the present application;

[0086] Figure 3 is a schematic diagram of calculating a covariance matrix of a non-planar region provided by an embodiment of the present application;

[0087] Figure 4 It is a schematic diagram of a ground tiny obstacle detection device for an indoor mobile robot provided in one embodiment of the present application.

[0088] Description of reference numerals:

[0089] 100-controller; 200-preprocessing module; 300-sub-graph generation module; 400-real-time odometer acquisition module; 500-optimization module; 600-environment modeling module. DETAILED DESCRIPTION

[0090] In order to enable those skilled in the art to better understand the technical solutions in the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work should fall within the scope of protection of the present application.

[0091] In the description of the embodiments of the present application, the terms "first" and "second" are used for descriptive purposes only and are not to be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present application, "plurality" means at least two, for example, two, three, etc., unless otherwise clearly and specifically defined.

[0092] In this application, unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", "fixed" and the like should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements, unless otherwise clearly defined. For ordinary technicians in this field, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0093] In the present application, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may mean that the first and second features are in direct contact, or the first and second features are in indirect contact through an intermediate medium. Moreover, a first feature being "above", "above" or "above" a second feature may mean that the first feature is directly above or obliquely above the second feature, or simply means that the first feature is higher in level than the second feature. A first feature being "below", "below" or "below" a second feature may mean that the first feature is directly below or obliquely below the second feature, or simply means that the first feature is lower in level than the second feature.

[0094] In recent years, mobile robots have been increasingly used in complex indoor space scenarios, involving many fields such as public safety inspection, smart home services, warehousing and logistics, and medical care. In indoor environments, there are often complex scenarios where multiple dynamic targets (such as people, pets, mobile devices, etc.) appear at the same time and move randomly.

[0095] However, in indoor multi-dynamic obstacle scenarios, there are often multiple dynamic targets (such as people, pets, mobile devices, etc.) appearing at the same time and moving randomly, GPS is unavailable and there are many environmental interference factors, so indoor mobile robots have difficulty in autonomous positioning.

[0096] For example, in an indoor environment with multiple dynamic targets, LiDAR will generate a massive amount of three-dimensional point cloud data. Due to the limited computing resources of the mobile robot itself, it is difficult to efficiently complete point cloud registration and pose estimation, let alone continuously achieve real-time tracking under the interference of multiple dynamic obstacles. Secondly, in order to accelerate LiDAR point cloud processing, existing methods usually use feature extraction modules to reduce the data size. However, the non-rigid motion of multi-dynamic target scenes and the sparse and occlusion effects of point cloud data will significantly weaken the stability of feature extraction, resulting in a large amount of manual post-processing to correct the feature information destroyed by dynamic interference. Furthermore, keyframe selection and sub-graph generation are incompatible with sudden changes: threshold-based keyframe selection and sub-graph generation strategies perform well in structured and relatively static environments. However, in indoor scenes with multiple dynamic targets, sudden environmental changes may cause the loss of keyframe constraint information, which in turn affects the integrity of pose constraints between sub-graphs. Finally, the lack of multi-level factor graph constraints: factor graph optimization is currently the only way to improve the accuracy of pose estimation and construction. Figure 1 However, in multi-dynamic target environments, there are a large number of non-adjacent keyframes and potential observation constraints between subgraphs that have not been fully utilized, resulting in the need to improve the accuracy and robustness of local and global optimization.

[0097] Therefore, the present application provides an autonomous positioning method and device for an indoor mobile robot for multiple dynamic scenarios to solve the above-mentioned problems. The solution provided in the embodiments of the present application will be described in detail below in conjunction with the drawings in the specification.

[0098] Figure 1 It is a flow chart of an autonomous positioning method for an indoor mobile robot in multiple dynamic scenes provided in one embodiment of the present application.

[0099] Reference Figure 1 As shown, the embodiment of the present application provides an autonomous positioning method for an indoor mobile robot in multiple dynamic scenes, including:

[0100] S100: Preprocessing the point cloud data obtained by the indoor mobile robot to obtain first point cloud data.

[0101] Specifically, the point cloud data obtained by the indoor mobile robot is preprocessed, and the preprocessing includes:

[0102] Perform motion compensation on point cloud data. Motion compensation includes:

[0103] The spherical linear interpolation method is used to compensate the rotation vector of the point cloud data and the linear interpolation method is used to compensate the translation vector;

[0104] The spherical linear interpolation method is used to compensate the rotation vector of the point cloud data, including:

[0105] Assume that the initial time and end time of a certain frame radar scan are t start and t end ;

[0106] The pose transformation matrix of the indoor mobile robot corresponding to the initial time and the end time is T start With T end ; At any time t∈[t start ,t end ] The corresponding posture transformation matrix is ​​recorded as T(t), and the time scale coefficient is defined as

[0107] Let q start ,q end They represent the quaternions of the starting posture and the ending posture respectively, and θ represents the angle between the two on the sphere. Then the quaternion interpolation corresponding to any time α can be written as

[0108]

[0109] The interpolation result Slerp(q start ,q end ,α) is converted to R(α), which is used to describe the posture of the indoor mobile robot at time α. It is used to describe the posture of the robot at time α. Since Slerp can ensure the equal angle characteristics of the rotation path on the unit quaternion sphere, it is suitable for frequent maneuvering and partial occlusion in indoor multi-dynamic target scenes, providing higher posture accuracy for subsequent mapping and positioning.

[0110] After interpolating the rotation part, the translation vector can also be regarded as linear interpolation between the start and end time. The linear interpolation method is used to compensate the translation vector for motion, including:

[0111] Assume t start ,t end

[0112] are the translation vectors at the start and end times respectively, then the translation vector at time α is

[0113] t(α)=t start +α(t end -t start ).

[0114] The corresponding pose transformation matrix is

[0115]

[0116] Perform motion compensation on any laser point p to obtain the distortion-free point cloud p′:

[0117] p′=T(α)p,

[0118] where α depends on the sampling timestamp of the laser point in the current frame.

[0119] Different from verifying Slerp only in relatively static or simple motion scenes, this application has made further expansion and optimization:

[0120] First, combined with the characteristics of high-frequency multi-dynamic target interference, the synergistic effect of spherical linear interpolation and translation linear interpolation can minimize local motion distortion, laying a more accurate initial data foundation for subsequent pyramid warm-up alignment and key frame screening.

[0121] Secondly, it is compatible with a variety of indoor trajectory models, including non-uniform small acceleration and deceleration and rotation drift, which can be adapted by adjusting the interpolation parameters.

[0122] Finally, the problem of traditional feature extraction easily losing information under dynamic interference is avoided. The entire compensation process can directly correct the original point cloud without feature extraction, achieve a more complete preservation of the real environment, and have stronger robustness in multi-dynamic target scenarios.

[0123] In summary, Slerp-based motion compensation plays an important role in the present invention, ensuring the spatiotemporal consistency and distortion-free nature of the point cloud collected by the lidar in an indoor environment with multiple dynamic targets, providing solid data support for subsequent pyramid warm-up alignment, key frame selection, and factor graph optimization processes, and also highlighting the significant innovative value of the present invention in complex dynamic environments.

[0124] In order to ensure the consistency of single-frame LiDAR scanning within the sampling period under the interference of multiple dynamic targets, this solution simultaneously introduces Slerp in the form of quaternion and linear translation interpolation to maximize the elimination of point cloud "tailing" or "stretching" distortion caused by carrier motion. It avoids the problem that traditional linear interpolation alone is prone to interpolation errors in the rotation part, thereby affecting the stability of subsequent point cloud registration. It is compatible with a variety of indoor trajectory models (including acceleration and deceleration and small-range rotation changes), and can complete accurate correction of the original point cloud without relying on external feature extraction, providing reliable input for subsequent high-precision SLAM.

[0125] S200: performing pyramid preheating registration processing on point cloud data at adjacent times to obtain an initial odometer of the indoor mobile robot, and selecting multiple key frames to generate a sub-graph.

[0126] Traditional point cloud registration algorithms need to calculate the covariance matrix of the target point cloud from scratch when processing each new LiDAR scan, which often leads to excessive computational burden, high sensitivity of initial guesses, and easy to fall into local optimality. This application performs pyramid preheating registration processing on point cloud data at adjacent times, which can cope with the diversity and real-time requirements of point clouds in a multi-dynamic target environment, and takes into account both computational efficiency and the integrity of original geometric information.

[0127] With the support of voxel filters, the original point cloud is divided into the bottom layer (i.e., preheating layer) and the registration layer corresponding to the original resolution according to different resolutions. The preheating layer can complete the fast estimation of the initial guess on the lower resolution data, reduce the local mismatch of the point cloud caused by multiple dynamic targets, and make the registration easier to converge globally in the subsequent high-resolution layer.

[0128] The covariance matrix of the point cloud data is reused by using the rigid invariant properties, and the point cloud data is processed hierarchically and step by step. That is, with the help of rigid body invariant properties such as normal vectors and curvature, the covariance matrix can be reused in the target point cloud, rather than starting from scratch for each scan update. This part still follows the basic framework of GICP, but through hierarchical and step-by-step processing, the impact of the point cloud density on the registration speed is reduced; at the same time, under the interference of multiple dynamic targets, it can also more flexibly cope with the diversity of local planar and non-planar areas.

[0129] In the original resolution registration layer, by associating the initial downsampled points with the nearest neighbor points of the original point cloud and constructing a sparse covariance matrix, the geometric features can be quickly and fully mined; finally, in the stage of eliminating mismatched point pairs, a filter combining distance, normal vector and curvature information is introduced to accurately shield obvious mismatched areas in dynamic environments, further improving the registration accuracy. Different from the traditional idea of ​​performing full-process registration only at low or single resolution, the pyramid preheating registration process uses a coarser point cloud for "preheating" iterations at the bottom layer, and quickly converges for posture or deformation disturbances caused by multiple dynamic targets, and then uses the original resolution for fine alignment at the registration layer. This not only shortens the overall optimization search time, but also greatly reduces the risk of local optimality caused by inaccurate initial values, laying the foundation for high-precision, autonomous positioning of multiple dynamic targets in indoor scenes.

[0130] In complex scenes with multiple dynamic targets, the temporal and spatial distribution and density of the input point cloud are often very uneven, and the registration speed is usually proportional to the amount of data. In order to achieve fast and stable warm-up registration, this paper proposes a filtering strategy that can dynamically adjust the voxel grid size, which automatically adjusts the grid size of each layer based on the ratio of the expected number of points to the actual number of points and the density smoothing coefficient of the local point cloud. The initial voxel size is denoted as v 0 , the expected number of points is N desire , the actual number of points is N actual , the smoothing coefficient is γ, then the dynamic adjustment formula is as follows:

[0131]

[0132] Among them, γ will be set according to the local point cloud density; when the input point cloud density is extremely high, γ can be appropriately increased to quickly reduce invalid or redundant points to ensure the real-time performance of subsequent warm-up registration. According to the resolution requirements at different levels, a multi-layer pyramid point cloud structure can be constructed to provide support for subsequent rapid convergence.

[0133] Figure 2 It is a schematic diagram of pyramid preheating registration processing provided by an embodiment of the present application.

[0134] The above multi-layer pyramid point cloud structure can be referred to Figure 2 As shown, from bottom to top: levels m-1 to 2 are preheating layers, corresponding to resolutions from res m-1 to res 2 Levels 1 and 0 are registration layers, corresponding to resolutions from res 1 to res 0 Levels 0 and 1 correspond to the original point cloud and the first downsampled point cloud, respectively.

[0135] In order to avoid recalculating the covariance matrix of the target point cloud and incurring huge overhead when scanning each frame, the present invention reuses the calculated local point cloud structure information based on the invariant normal vector and curvature properties of the rigid body.

[0136] For a plane point cloud, the eigenvector represents the main direction of change, while the eigenvalue quantifies its amount of change, thereby describing the plane structure. Therefore, the covariance matrix of the plane point Σ c It can be approximated by eigenvectors and eigenvalues. Since the eigenvector e corresponding to the minimum eigenvalue 1 represents the direction of the minimum change in the plane, and the point cloud normal vector n is perpendicular to the plane. The normal vector can be approximated by the eigenvector corresponding to the minimum eigenvalue, that is, e 1 = n. The confidence level of the measured point in the normal direction is higher, while the confidence level in the plane is lower. Therefore, the eigenvalue α in the normal direction 1 It can be approximated by a small constant δ, while relatively large eigenvalues ​​(variances) are assigned to other eigenvectors to characterize this uncertainty, i.e., α 1 =α 2 =1.

[0137] Define any normalized vector p that passes through the origin and lies in the plane, satisfying the constraint ||p|| = 1. Thus, the family of vectors e that are perpendicular to the normal vector and pass through the origin 2 It can be expressed as:

[0138]

[0139] in represents the projection length of vector p on normal vector n. The above formula represents the vector p after removing the normal vector projection, ensuring that vector e 2 Orthogonal to the normal vector. Although the eigenvector e 2 It is not necessarily the main direction of variation of the point cloud, but in registration, its direction has little effect on the result, because plane-to-plane or point-to-plane registration mainly focuses on the normal direction.

[0140] Considering that the eigenvector should cover the entire three-dimensional space, we can use the eigenvector e 1 and e 2 The cross product of the eigenvector e is obtained 3 , that is, e 3 =e 1 ×e 2 . Finally, the covariance matrix is ​​reconstructed as follows:

[0141]

[0142] where Λ is given by α 1 , α 2 and α 3 The diagonal matrix formed.

[0143] For non-planar point clouds, this application proposes a high-resolution neighborhood covariance matrix construction method. Figure 3 Schematic diagram of non-planar region covariance matrix calculation, see Figure 3 As shown in Figure 1, instead of searching for K nearest neighbors for each downsampled point in the downsampled point cloud, we search for K nearest neighbors for each downsampled point in the original resolution point cloud. The advantage is that the number of times the covariance matrix is ​​constructed is only related to the number of points in the low-resolution point cloud, while searching for the nearest neighbor points from the original resolution point cloud can capture as much original geometric information as possible, thereby improving the calculation speed and registration accuracy.

[0144] In this application, the curvature σ=α is used 1 / (α 1 +α 2 +α 3 ) is used to quickly evaluate whether a local point cloud is planar or non-planar. The covariance matrix calculation method is then selected by threshold comparison. It should be noted that the curvature of each point is pre-calculated when the LiDAR point cloud first enters the registration system and reused when needed.

[0145] Under the interference of multiple dynamic targets, the point cloud may have large density differences and occlusions, which makes it easier for mismatches to occur in edges or discontinuous areas. To this end, the present invention introduces a mismatch filter that integrates distance, normal vector and curvature information. The specific judgment conditions are:

[0146]

[0147] where n s ,n t Represents the normal vector between the source point cloud and the target point cloud, p s ,p t represents the point coordinates, κ is the curvature, T is the transformation matrix of the current registration, and δ is the set threshold. If this condition is not met, it is judged as a mismatched point pair and removed, thereby reducing the accumulation of malicious matches caused by local dynamics and further improving the global accuracy of the final registration.

[0148] It is understandable that by quickly obtaining the initial guess through the preheating layer and finely aligning it in the registration layer, the optimization convergence is significantly accelerated and the risk of local optimality caused by interference from multiple dynamic targets is reduced. By using the rigid body invariant normal vector and curvature properties, differential calculations are performed on planar and non-planar areas, which greatly reduces redundant operations between frames and adapts to diverse geometric structures in dynamic scenes. In the final registration stage, the original high-resolution information can still be retained, and the registration drift caused by environmental mutations, dynamic occlusion, etc. can be overcome with the help of the error point pair elimination mechanism. Voxel filtering can be adaptively adjusted with the input point cloud density, so that the algorithm can still meet the real-time requirements in a multi-dynamic target environment, with both high precision and high efficiency. In an indoor environment with a large number of point clouds and frequent dynamic disturbances, the present invention can still efficiently obtain accurate and robust registration results, providing strong data support for subsequent key frame selection, factor graph optimization, and autonomous navigation, and demonstrating significant positioning advantages for complex scenes with multiple dynamic targets.

[0149] Select keyframes to generate subgraphs, including:

[0150] The discrete coefficient and the pose change coefficient are introduced, and the anti-slip strategy is used to dynamically adjust the selection distance of the key frame, including:

[0151] Let σ d and μ d They represent the standard deviation and average value of the distance between points in the current scan point cloud, respectively. The environmental dispersion coefficient is recorded as

[0152]

[0153] Let σ p With μ p Represent the standard deviation and average value of the pose difference norm ‖ΔT‖ of the most recent frames, respectively. Then the pose change coefficient is

[0154]

[0155] Normalize the environmental dispersion coefficient and the pose change coefficient to the interval [0,1] respectively;

[0156] Then, the key frame selects the distance Dk The dynamic adjustment formula is:

[0157] D k =max(D min ,min(D max ,D max ·[1-exp(-β(C d -C p ))])),

[0158] Where D max is the maximum allowed distance between key frames, D min is the preset minimum distance, β represents the curve shape factor, which is used to control the key frame distance with C d -C p growth rate;

[0159] If, if C d -C p The exponential term is small, so the key frame distance is kept at a low level, thus avoiding missing key information in narrow or multi-twist scenes; if C d >C p , it means that the environment gradually becomes empty and the posture changes are not drastic. The key frame selection distance can be increased to reduce the number of redundant key frames and speed up the subsequent calculation efficiency.

[0160] It should be noted that in practical applications, the environment can be judged to be spacious or narrow based on the median distance from the environmental point cloud to the mobile robot, and the β value can be adaptively updated to further improve the flexibility of key frame distance adjustment.

[0161] It can be understood that by integrating the distribution of point clouds and changes in posture, the adaptability of key frame selection to multiple dynamic target environments is significantly improved, which prevents the loss of key frames when the mobile robot turns quickly in an open environment, and improves the ability to capture real environmental information in the dynamic process; the key frame distance changes smoothly within a reasonable range, avoiding the "too sparse" and "too dense" problems common in fixed threshold methods.

[0162] In indoor multi-dynamic target scenes, if sub-graphs are generated only based on the nearest neighbor keyframe or by retrieving keyframes within a radius, problems such as positioning drift or map inconsistency may occur in fast turning or large occlusion scenes. In some examples, a composite similarity evaluation method combining KL divergence and Jaccard index is used to make the sub-graph division more flexible and robust, which is conducive to the subsequent sub-graph-based registration and factor graph optimization.

[0163] Specifically, KL divergence includes:

[0164] Let the current keyframe point cloud be P and the nearest anchor point keyframe be Q, so that they satisfy the Gaussian distribution N(μp ,Σ p ) and N(μ q ,Σ q ). Based on the approximate matrix form of Taylor expansion, the KL divergence is:

[0165]

[0166] Where tr(·) represents the trace operation, det(·) represents the determinant, d is the data dimension, and D KL The smaller it is, the more similar the probability distributions of P and Q are;

[0167] Jaccard index, including:

[0168]

[0169] Among them, |·| represents the number of points in the point cloud data, P∩Q and P∪Q are the intersection and union of the two point clouds, respectively. The larger J(P,Q) is, the more overlapping areas the two point clouds have;

[0170] Composite similarity evaluation methods include:

[0171] Composite evaluation index Λ(P,Q)=w KL (1-D KL (P‖Q))+w JC J(P,Q),

[0172] Among them, w KL With w JC are the weight coefficients of KL divergence and Jaccard index, respectively, which can be adjusted according to the dynamic degree of the environment and the reliability of the data. When Λ(P,Q) exceeds the set threshold, it means that the current keyframe has a high overall similarity with the anchor keyframe, and the current keyframe and all scans in between can be included in the corresponding subgraph; if Λ(P,Q) is too low, it is judged that it is very different from the anchor keyframe, and there may be new features in the map or the environment has changed significantly, and a new subgraph needs to be opened to better track environmental changes.

[0173] In a multi-dynamic environment, if too many keyframes are merged into the same subgraph, the subgraph may become too large and increase the computational burden of subsequent registration and optimization. To this end, the present invention sets an upper threshold for the number of keyframes in the subgraph, and automatically opens a new subgraph when the threshold is exceeded to avoid the problem of too dispersed features or too much interference caused by too large a subgraph.

[0174] By introducing discrete coefficients and posture change coefficients and combining them with anti-slip strategies, the key frame selection interval can be adjusted in real time according to the openness of the environment and the intensity of the movement in multi-dynamic target scenes, taking into account both data completeness and efficiency.

[0175] The combined use of KL divergence and Jaccard index not only measures the difference between probability distributions, but also measures the spatial overlap, making the subgraph division more accurate and robust, and avoiding the susceptibility of a single indicator to noise and environmental mutations.

[0176] By appropriately limiting the size of the subgraph and controlling the range of the similarity index, the present invention can better continuously obtain accurate local subgraphs in an indoor environment with multiple motion interference and frequent turns, and effectively connect with the subsequent global factor graph optimization.

[0177] Combining the discreteness of the environmental point cloud with the change of the robot's posture can prevent the phenomenon of too sparse key frames when turning in open space and too dense key frames in complex and narrow environments, and enhance the robustness of positioning under the interference of multiple dynamic obstacles.

[0178] It can be understood that by adaptively balancing the number of key frames and the sub-graph generation strategy, the sudden changes and new features in the multi-dynamic environment can be captured to the maximum extent, thereby ensuring that the subsequent registration and factor graph optimization can converge quickly under the constraints of a relatively complete local map, greatly improving the overall accuracy and robustness of the system.

[0179] It is understandable that based on the discrete coefficient and the pose change coefficient, the key frame selection interval is dynamically adjusted for open or narrow scenes; combined with similarity evaluations such as KL divergence and Jaccard index, the sub-graphs are flexibly divided and their size is limited to avoid the problem of too large sub-graphs or serious loss of key frames. The number of key frames and the accuracy of local sub-graph division are adaptively balanced to ensure the integrity and timeliness of local map information in complex and changing dynamic scenes. By combining the point cloud distribution and the robot's pose changes, the key frame selection and sub-graph generation mechanism are deeply coupled with dynamic environmental changes, avoiding the "sometimes good, sometimes bad" phenomenon caused by the fixed threshold method.

[0180] S300: performing pyramid preheating registration processing on the point cloud data obtained in real time and the sub-image to obtain a real-time odometer of the indoor mobile robot.

[0181] Exemplarily, the point cloud data obtained in real time is processed with the sub-image through pyramid preheating registration, and combined with the robot's sensor data to obtain the real-time position and pose of the indoor mobile robot, that is, the real-time odometer.

[0182] S400: Optimizing the real-time odometer using a multi-constraint factor graph to obtain an optimized odometer.

[0183] Specifically, it includes:

[0184] Multi-level constraints are established between keyframes, keyframes and subgraphs, and subgraphs and subgraphs, and compensation constraints are applied when closed loops are detected to optimize real-time odometry.

[0185] Among them, constraining keyframes with each other includes:

[0186] The source key frame and the target key frame are denoted as K s and K t , the transformation matrix T of the two is obtained through pyramid preheating registration processing st :

[0187] T st =argmin T (F(K s ,K t ,T)),

[0188] Where F represents the cost function of the registration:

[0189]

[0190] Where p and q represent the corresponding point clouds from the target point cloud, Σ p and Σ q is the estimated covariance matrix corresponding to the two, and the pose constraint factor between the key frames is calculated.

[0191] Establish constraints between keyframes and subgraphs, including:

[0192] The jth sub-graph is composed of several frames, including the most recent M key frames; let one of the key frames be K s , the target subgraph is denoted as G t , after registration, the transformation matrix is ​​obtained:

[0193] T s_g = arg min T (F(K s ,G t ,T)),

[0194] Where K s The covariance matrix corresponding to each point in G t The covariance matrices of corresponding points in jointly participate in the weighted error measurement.

[0195] In multi-dynamic scenarios, if there are similar or overlapping areas between two subgraphs (which may be caused by loops or repeated environmental features, etc.), relying solely on the sequential connection of adjacent subgraphs or keyframe interactions is not enough to fully capture global consistency. This invention further strengthens global correlation through subgraph-subgraph factors:

[0196] T g1g2 =argmin T (F(G 1 ,G 2 ,T)),

[0197] Among them G 1 and G 2 In order to align the source sub-image and the target sub-image after similarity evaluation, the transformation T is obtained by registration. g1g2 . Unlike keyframe-keyframe or keyframe-subgraph factors, subgraph-subgraph factors are not limited to temporal adjacency, but can also be applied to non-adjacent subgraphs; if sufficient geometric overlap or similar distribution is detected, the constraint is inserted into the factor graph to improve local optimization and global consistency. This strategy can capture the location where local scenes reappear or repeat more timely in a multi-dynamic environment, thereby suppressing cumulative drift.

[0198] In a complex indoor environment with multiple dynamic targets, the odometer system may still drift significantly over time even with multiple constraints of keyframes and sub-image factors. To further correct the residual error, the present invention builds a closed-loop factor based on the ScanContext++ descriptor:

[0199] -When the new key frame K n After it arrives, the descriptor is matched through ScanContext++. If a closed-loop keyframe K is detected c and meet the similarity threshold, then the local scene is considered to be highly overlapping;

[0200] Further from K c The surrounding point cloud constructs the target point cloud P loop and compare it with K n Use pyramid preheating hotspot cloud registration to obtain the transformation T nc ;

[0201] If the registration is successful, a new closed-loop factor is inserted into the factor graph for the keyframe nodes n and c to minimize the long-term accumulated drift. loop As a local reference, it can effectively compensate for the possible registration errors or cumulative posture deviations in a multi-dynamic target environment.

[0202] It can be understood that compared with the traditional simple edge constraints that only consider adjacent keyframes, the present invention simultaneously constructs "adjacent / non-adjacent" keyframe factors, "keyframe-subgraph" factors, "subgraph-subgraph" factors and closed-loop factors in multi-dynamic target scenes, so that the factor graph has a richer correlation structure, which helps to quickly and robustly correct drift.

[0203] Keyframes, subgraphs, and closed loops are unified and integrated in the same factor graph model, supporting iSAM2 incremental optimization to achieve a balance between local updates and global consistency; even when the scene topology changes frequently and dynamic obstacles appear frequently, high mapping accuracy and positioning stability can be achieved. Alignment between non-adjacent subgraphs can capture the overlap of similar areas in a timely manner, prevent excessive drift of local maps, and enhance global consistency. Pyramid warm-up registration is used to accelerate the pose estimation of new keyframes or subgraphs, and ScanContext++ is used for efficient closed loop detection, which significantly improves the real-time and accuracy of factor graph constraint construction.

[0204] It is understandable that the traditional adjacent keyframe constraints are extended to non-adjacent keyframes, sub-graph-sub-graph, closed loop detection and other constraint types, forming a "local-global" combined factor graph model, which can correct local drift in time and enhance global local detection in a multi-dynamic target environment. Figure 1 The multi-level factor graph optimization has both local and global flexibility. It can quickly fall back and update the state after detecting new valid constraints through iSAM2 incremental optimization, thus improving the robustness and accuracy of long-term navigation.

[0205] S500: constructing an environment model based on the optimized odometer and the point cloud data obtained by the indoor mobile robot to achieve autonomous positioning of the indoor mobile robot.

[0206] Building an environmental model based on the optimized odometer and point cloud data obtained by the indoor mobile robot are both existing technologies and will not be described in detail here.

[0207] It should be understood that, although the various steps in the flowcharts involved in the above-mentioned embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps does not have a strict order restriction, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above-mentioned embodiments can include multiple steps or multiple stages, and these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily to be carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.

[0208] Based on the same inventive concept, the embodiment of the present application also provides an autonomous positioning device for an indoor mobile robot facing multiple dynamic scenes for implementing the autonomous positioning method for an indoor mobile robot facing multiple dynamic scenes involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in the embodiments of one or more autonomous positioning devices for indoor mobile robots facing multiple dynamic scenes provided below can refer to the limitations of the autonomous positioning method for indoor mobile robots facing multiple dynamic scenes above, and will not be repeated here.

[0209] Figure 4 It is a schematic diagram of an autonomous positioning device for an indoor mobile robot for multiple dynamic scenes provided by an embodiment of the present application.

[0210] Reference Figure 4 As shown, in the second aspect, an embodiment of the present application provides an autonomous positioning device for an indoor mobile robot for multiple dynamic scenes, which applies the autonomous positioning method for an indoor mobile robot for multiple dynamic scenes of the first aspect. The device includes a controller 100, a preprocessing module 200, a sub-graph generation module 300, a real-time odometer acquisition module 400, an optimization module 500 and an environment modeling module 600. Among them, the preprocessing module 200 is electrically connected to the controller 100, and is used to preprocess the point cloud data obtained by the indoor mobile robot to obtain the first point cloud data; the sub-graph generation module 300 is electrically connected to the controller 100, and is used to perform pyramid preheating and registration processing on the point cloud data of adjacent times, obtain the initial odometer of the indoor mobile robot, and select multiple key frames to generate a sub-graph; the real-time odometer acquisition module 400 is electrically connected to the controller 100, and is used to perform pyramid preheating and registration processing on the point cloud data obtained in real time and the sub-graph to obtain the real-time odometer of the indoor mobile robot; the optimization module 500 is electrically connected to the controller 100, and is used to optimize the real-time odometer using a multi-constraint factor graph to obtain the optimized odometer; the environment modeling module 600 is electrically connected to the controller 100, and is used to construct an environment model based on the optimized odometer and the point cloud data obtained by the indoor mobile robot, so as to realize the autonomous positioning of the indoor mobile robot.

[0211] Each module in the above-mentioned indoor mobile robot autonomous positioning device for multiple dynamic scenes can be implemented in whole or in part by software, hardware and their combination. The above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0212] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. Non-relational databases may include distributed databases based on blockchains, etc., but are not limited to this. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but are not limited to this.

[0213] It is easy to understand that those skilled in the art can combine, split, reorganize, etc. the embodiments of the present application to obtain other embodiments based on the several embodiments provided in the present application, and these embodiments do not exceed the protection scope of the present application.

[0214] The above specific implementation methods further explain in detail the purpose, technical solutions and beneficial effects of the embodiments of the present application. It should be understood that the above are only specific implementation methods of the embodiments of the present application and are not used to limit the protection scope of the embodiments of the present application. Any modifications, equivalent substitutions, improvements, etc. made on the basis of the technical solutions of the embodiments of the present application should be included in the protection scope of the embodiments of the present application.

Claims

1. An autonomous positioning method for indoor mobile robots in multiple dynamic scenes, characterized in that: include: Preprocessing the point cloud data obtained by the indoor mobile robot to obtain first point cloud data; Performing pyramid preheating registration processing on point cloud data at adjacent times to obtain the initial odometer of the indoor mobile robot, and selecting multiple key frames to generate sub-graphs; Performing pyramid preheating registration processing on the point cloud data obtained in real time and the sub-image to obtain a real-time odometer of the indoor mobile robot; Optimizing the real-time odometer using a multi-constraint factor graph to obtain an optimized odometer; An environmental model is constructed based on the optimized odometer and the point cloud data obtained by the indoor mobile robot to achieve autonomous positioning of the indoor mobile robot.

2. The autonomous positioning method for indoor mobile robots in multiple dynamic scenes according to claim 1 is characterized in that: The point cloud data obtained by the indoor mobile robot is preprocessed, and the preprocessing includes: Performing motion compensation processing on the point cloud data, the motion compensation comprising: Using a spherical linear interpolation method to compensate for the rotation vector of the point cloud data and using a linear interpolation method to perform motion compensation on the translation vector; The method of using a spherical linear interpolation method to compensate for the rotation vector of the point cloud data includes: Assume that the initial time and end time of a certain frame radar scan are t start and t end ; The pose transformation matrix of the indoor mobile robot corresponding to the initial time and the end time is T start With T end ; At any time t∈[t start ,t end ] The corresponding posture transformation matrix is ​​recorded as T(t), and the time scale coefficient is defined as Let q start ,q end They represent the quaternions of the starting posture and the ending posture respectively, and θ represents the angle between the two on the sphere. Then the quaternion interpolation corresponding to any time α can be written as The interpolation result Slerp(q start ,q end ,α) is converted into R(α), which is used to describe the posture of the indoor mobile robot at time α; The method of using a linear interpolation method to perform motion compensation on the translation vector includes: Assume t start ,t end are the translation vectors at the start and end times respectively, then the translation vector at time α is t(α)=t start +α(t end -t start ). The corresponding pose transformation matrix is Perform motion compensation on any laser point p to obtain the distortion-free point cloud p′: p′=T(α)p, where α depends on the sampling timestamp of the laser point in the current frame.

3. The autonomous positioning method for indoor mobile robots in multiple dynamic scenes according to claim 1 is characterized in that: Performing pyramid warm-up registration processing on point cloud data at adjacent times to obtain the initial odometer of the indoor mobile robot includes: Use an adaptive voxel filter to divide the point cloud data into a preheating layer and a registration layer according to the resolution; The preheating layer completes a fast estimation of the posture of the indoor mobile robot based on the low-resolution data; Reusing the covariance matrix of the point cloud data using the rigid invariant attribute, and processing the point cloud data in layers and levels; In the registration layer, a sparse covariance matrix is ​​constructed by associating the initial down-sampled points with the nearest neighbor points of the original point cloud to mine geometric features. In the stage of eliminating mismatched point pairs, a filter combining distance, normal vector and curvature information is introduced to accurately shield obvious mismatched areas in dynamic environments.

4. The autonomous positioning method for indoor mobile robots in multiple dynamic scenes according to claim 1 is characterized in that: The step of selecting a key frame to generate a sub-graph includes: The discrete coefficient and the pose change coefficient are introduced, and the anti-slip strategy is used to dynamically adjust the selection distance of the key frame, including: Let σ d and μ d They represent the standard deviation and average value of the distance between points in the current scan point cloud, respectively. The environmental dispersion coefficient is recorded as Let σ p With μ p Represent the standard deviation and average value of the pose difference norm ‖ΔT‖ of the most recent frames, respectively. Then the pose change coefficient is Normalize the environmental dispersion coefficient and the pose change coefficient to the interval [0,1] respectively; Then, the key frame selects the distance D k The dynamic adjustment formula is: D k =max(D min ,min(D max ,D max ·[1-exp(-β(C d -C p )))), Where D max is the maximum allowed distance between key frames, D min is the preset minimum distance, β represents the curve shape factor, which is used to control the key frame distance with C d -C p growth rate; If, if C d -C p The exponential term is small, so the key frame distance is kept at a low level, thus avoiding missing key information in narrow or multi-twist scenes; if C d >C p , it means that the environment gradually becomes empty and the posture changes are not drastic. The key frame selection distance can be increased to reduce the number of redundant key frames and speed up the subsequent calculation efficiency.

5. The autonomous positioning method for indoor mobile robots in multiple dynamic scenes according to claim 4 is characterized in that: The step of selecting key frames to generate sub-graphs further includes: The composite similarity evaluation method combining KL divergence and Jaccard index is used to divide the subgraphs; The KL divergence includes: Let the current keyframe point cloud be P and the nearest anchor point keyframe be Q, so that they satisfy the Gaussian distribution N(μ p ,Σ p ) and N(μ q ,Σ q ). Based on the approximate matrix form of Taylor expansion, the KL divergence is: Where tr(·) represents the trace operation, det(·) represents the determinant, d is the data dimension, and D KL The smaller it is, the more similar the probability distributions of P and Q are; Jaccard index, including: Among them, |·| represents the number of points in the point cloud data, P∩Q and P∪Q are the intersection and union of the two point clouds, respectively. The larger J(P,Q) is, the more overlapping areas the two point clouds have; The composite similarity evaluation method comprises: Composite evaluation index Λ(P,Q)=w KL (1-D KL (P‖Q))+w JC J(P,Q), Among them, w KL With w JC are the weight coefficients of KL divergence and Jaccard index respectively.

6. The autonomous positioning method for indoor mobile robots in multiple dynamic scenes according to claim 1, characterized in that: The method of using a multi-constraint factor graph to optimize the real-time odometer includes: Multi-level constraints are established between keyframes, keyframes and subgraphs, and subgraphs and subgraphs, and compensation constraints are applied when closed loops are detected to optimize real-time odometry.

7. The autonomous positioning method for indoor mobile robots in multiple dynamic scenes according to claim 6, characterized in that: Create constraints between keyframes, including: The source key frame and the target key frame are denoted as K s and K t , the transformation matrix T of the two is obtained through pyramid preheating registration processing st : T st =argmin T (F(K s ,K t ,T)), Where F represents the cost function of the registration: Where p and q represent the corresponding point clouds from the target point cloud, Σ p and Σ q is the estimated covariance matrix corresponding to the two, and the pose constraint factor between the key frames is calculated.

8. The autonomous positioning method for indoor mobile robots in multiple dynamic scenes according to claim 6, characterized in that: Establish constraints between keyframes and subgraphs, including: The jth sub-graph is composed of several frames, including the most recent M key frames; let one of the key frames be K s , the target subgraph is denoted as G t , after registration, the transformation matrix is ​​obtained: T s_g =argmin T (F(K s ,G t ,T)), Where K s The covariance matrix corresponding to each point in G t The covariance matrices of corresponding points in jointly participate in the weighted error measurement.

9. The autonomous positioning method for indoor mobile robots in multiple dynamic scenes according to claim 3, characterized in that: In the stage of eliminating mismatched points, a filter combining distance, normal vector and curvature information is introduced to accurately shield obvious mismatched areas in a dynamic environment, including: The judgment condition for whether to remove the matching point is: Among them, n s ,n t Represents the normal vector between the source point cloud and the target point cloud, p s ,p t represents the point coordinates, κ is the curvature, T is the transformation matrix of the current registration, and δ is the set threshold. If the condition is not met, it is judged as a mismatched point pair and is removed.

10. An autonomous positioning device for an indoor mobile robot in multiple dynamic scenes, characterized in that: The method for autonomous positioning of an indoor mobile robot for multiple dynamic scenes as described in any one of claims 1 to 9 is applied, and the device comprises: Controller; A preprocessing module, electrically connected to the controller, for preprocessing the point cloud data obtained by the indoor mobile robot to obtain first point cloud data; A sub-graph generation module is electrically connected to the controller and is used to perform pyramid preheating registration processing on point cloud data at adjacent times, obtain an initial odometer of the indoor mobile robot, and select multiple key frames to generate a sub-graph; A real-time odometer acquisition module is electrically connected to the controller and is used to perform pyramid preheating registration processing on the point cloud data obtained in real time and the sub-image to obtain the real-time odometer of the indoor mobile robot; An optimization module, electrically connected to the controller, for optimizing the real-time odometer using a multi-constraint factor graph to obtain an optimized odometer; The environment modeling module is electrically connected to the controller and is used to build an environment model based on the optimized odometer and the point cloud data obtained by the indoor mobile robot to achieve autonomous positioning of the indoor mobile robot.

Images