A mobile positioning method and a mobile positioning device

Abstract

A mobile positioning method and a mobile positioning device are disclosed. The mobile positioning method comprises the following steps: obtaining point cloud data of a surrounding environment by a mobile sensor; constructing a global point cloud map of the surrounding environment based on the point cloud data; constructing loop constraints of the global point cloud map by using global descriptors; and minimizing an error function by using a nonlinear optimization function and the loop constraints to optimize the global point cloud map and the pose of the mobile sensor. By performing these steps, the mobile positioning method can improve the accuracy and real-time performance of mobile positioning in an unknown environment, and can also improve the ability of the system to effectively identify the observed area, obtain more accurate detection loops, and thus reduce the cumulative error.

Description

Technical Field

[0001] This invention relates to mobile robot localization and mapping technology, specifically to a mobile localization method, a mobile localization device, and a computer-readable storage medium. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) refers to placing a robot in an unknown location within an unknown environment. The robot uses its onboard sensors to move and build a map of its surroundings while simultaneously estimating its own position within the environment. With rapid technological advancements and the advantages of lidar, such as its wide ranging range, high accuracy, and strong anti-interference capabilities, lidar SLAM algorithms have been applied in fields such as autonomous driving, mobile robots, and drones, playing a crucial role in environmental detection and disaster relief.

[0003] Lidar SLAM can be divided into 2D Lidar SLAM and 3D Lidar SLAM based on the different lidar beams used. A commonly used classic algorithm in 3D Lidar SLAM is the Lidar Odometry and Mapping (LOAM) algorithm. Its core idea is to separate the localization and mapping parts, and combine the parallel results of the two algorithms to obtain high-precision, real-time results. However, due to the limited computing power of robot platforms, the LOAM algorithm is not the optimal solution for embedded mobile robot platforms. The later proposed LeGO-LOAM (Lightweight and Ground Optimized Lidar Odometry and Mapping) algorithm improved the LOAM algorithm to adapt to embedded mobile robot platforms with limited computing power. However, in existing technologies, the LeGO-LOAM algorithm still has the problem of not being able to effectively identify the observed area, thus failing to perform loop closure detection properly. This situation still leads to an increase in accumulated error, which cannot be eliminated.

[0004] To address the aforementioned problems in existing technologies, there is an urgent need in this field for a mobile positioning technology that can improve the accuracy and real-time performance of mobile positioning in unknown environments, and also enhance the system's ability to effectively identify observed areas, obtain more accurate loop closure detection, and thus reduce cumulative errors. Summary of the Invention

[0005] The following provides a brief overview of one or more aspects to offer a basic understanding of them. This overview is not an exhaustive summary of all conceived aspects, nor is it intended to identify key or decisive elements of all aspects, nor to define the scope of any or all aspects. Its sole purpose is to present some concepts of one or more aspects in a simplified form as a prelude to the more detailed descriptions that follow.

[0006] To overcome the aforementioned deficiencies in the prior art, the present invention provides a mobile positioning method, a mobile positioning device, and a computer-readable storage medium, which can improve the accuracy and real-time performance of mobile positioning in unknown environments, and also improve the system's ability to effectively identify observed areas, obtain more accurate detection loops, and thus reduce cumulative errors.

[0007] Specifically, the mobile positioning method provided by the first aspect of the present invention includes the following steps: acquiring point cloud data of the surrounding environment through a mobile sensor; constructing a global point cloud map of the surrounding environment based on the point cloud data; constructing a loop closure constraint of the global point cloud map using a global descriptor; and minimizing an error function using a nonlinear optimization function and the loop closure constraint to optimize the pose of the global point cloud map and the mobile sensor.

[0008] Furthermore, the mobile positioning device described above is provided according to a second aspect of the present invention. This mobile positioning device includes a memory and a processor. The processor is connected to the memory and configured to implement the mobile positioning method described above according to the first aspect of the present invention.

[0009] Furthermore, according to a third aspect of the present invention, a computer-readable storage medium is provided on which computer instructions are stored. When the computer instructions are executed by a processor, the above-described motion positioning method provided in the first aspect of the present invention is implemented. Attached Figure Description

[0010] The above-described features and advantages of the present invention will be better understood after reading the following detailed description of embodiments of the present disclosure in conjunction with the accompanying drawings. In the drawings, components are not necessarily drawn to scale, and components having similar related characteristics or features may have the same or similar reference numerals.

[0011] Figure 1 A flowchart of a mobile positioning method provided according to some embodiments of the present invention is shown;

[0012] Figure 2 A schematic diagram illustrating the workflow of a mobile positioning method provided according to some embodiments of the present invention is shown;

[0013] Figure 3AA schematic diagram of the coordinate system of a lidar sensor provided according to some embodiments of the present invention is shown;

[0014] Figure 3B A schematic diagram illustrating the rotation mode of laser beam scanning of a lidar sensor provided according to some embodiments of the present invention is shown;

[0015] Figure 4 A schematic diagram of point cloud distortion of a lidar sensor provided according to some embodiments of the present invention is shown;

[0016] Figure 5 A flowchart illustrating the construction of a global point cloud map of the surrounding environment is shown, according to some embodiments of the present invention;

[0017] Figure 6 A schematic diagram of a two-step optimized calculation process for laser odometry provided according to some embodiments of the present invention is shown;

[0018] Figure 7 A structural block diagram of a mobile positioning device provided according to some embodiments of the present invention is shown.

[0019] Figure label:

[0020] Steps S110 to S140;

[0021] Steps S121 to S123;

[0022] 300 LiDAR sensor;

[0023] 700 Mobile Positioning Device

[0024] 710 Memory;

[0025] 720 processor. Detailed Implementation

[0026] The following specific embodiments 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. Although the description of the present invention is presented in conjunction with preferred embodiments, this does not mean that the features of the invention are limited to these embodiments. On the contrary, the purpose of describing the invention in conjunction with embodiments is to cover other options or modifications that may be derived based on the claims of the present invention. To provide a thorough understanding of the invention, many specific details will be included in the following description. The invention may also be implemented without using these details. Furthermore, to avoid confusion or obscuring the focus of the invention, some specific details will be omitted in the description.

[0027] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0028] Furthermore, the terms "upper," "lower," "left," "right," "top," "bottom," "horizontal," and "vertical" used in the following description should be understood as the orientations shown in the relevant paragraphs and accompanying drawings. These relative terms are for illustrative purposes only and do not imply that the described apparatus must be manufactured or operated in a specific orientation, and therefore should not be construed as limiting the invention.

[0029] It is understood that although terms such as "first," "second," and "third" may be used herein to describe various components, regions, layers, and / or parts, these components, regions, layers, and / or parts should not be limited by these terms, and these terms are only used to distinguish different components, regions, layers, and / or parts. Therefore, the first components, regions, layers, and / or parts discussed below may be referred to as second components, regions, layers, and / or parts without departing from some embodiments of the present invention.

[0030] As mentioned above, laser SLAM can be divided into 2D laser SLAM and 3D laser SLAM based on the different laser radar beams used. A commonly used classic algorithm in 3D laser SLAM is the Lidar Odometry and Mapping (LOAM) algorithm. Its core idea is to separate the localization and mapping parts, and combine the parallel results of the two algorithms to obtain high-precision, real-time results. However, due to the limited computing power of robot platforms, the LOAM algorithm is not the optimal solution for embedded mobile robot platforms. The later proposed LeGO-LOAM algorithm improved the LOAM algorithm to adapt to embedded mobile robot platforms with limited computing power. However, in existing technologies, the LeGO-LOAM algorithm still has the problem of not being able to effectively identify the observed area, thus failing to perform loop closure detection properly. This situation still leads to an increase in accumulated error, which cannot be eliminated.

[0031] To address the aforementioned problems in the prior art, this invention provides a mobile positioning method, a mobile positioning device, and a computer-readable storage medium, which can improve the accuracy and real-time performance of mobile positioning in unknown environments, and also enhance the system's ability to effectively identify observed areas, obtain more accurate detection loopbacks, and thus reduce cumulative errors.

[0032] In some non-limiting embodiments, the mobile positioning method provided in the first aspect of the present invention can be implemented by the mobile positioning device provided in the second aspect of the present invention.

[0033] Please refer to Figure 1 , Figure 1 A flowchart of a mobile positioning method provided according to some embodiments of the present invention is shown. Figure 1 As shown, in some embodiments of the present invention, the mobile positioning method mainly includes the following steps:

[0034] S110: Acquires point cloud data of the surrounding environment through a mobile sensor.

[0035] Specifically, please refer to the following: Figure 2 , Figure 2 A schematic diagram illustrating the workflow of a mobile positioning method provided according to some embodiments of the present invention is shown.

[0036] like Figure 2 As shown, in some embodiments, to achieve simultaneous localization and mapping (SLAM) of a mobile robot in unknown environments, at least one motion sensor can be installed on the robot to collect information about its surrounding environment as the robot moves, thereby obtaining input information from the motion sensor. Optionally, the motion sensor can be a LiDAR sensor, such as a 16-line LiDAR VLP-16 with a vertical field of view of 30°, a horizontal field of view of 360°, and providing a vertical angular resolution of 2° and a horizontal angular resolution of 0.2°. As the robot moves at multiple angles, the LiDAR sensor can move at multiple angles to collect point cloud data of the surrounding environment and objects.

[0037] Furthermore, in some preferred embodiments, before performing point cloud segmentation on the acquired point cloud data, in response to point cloud distortion in the acquired point cloud data, the point cloud coordinates of the point cloud data are corrected based on the pose change of the point cloud data at the end time relative to its start time.

[0038] Specifically, please see Figure 3A and Figure 3B , Figure 3A A schematic diagram of the coordinate system of a lidar sensor provided according to some embodiments of the present invention is shown. Figure 3B A schematic diagram of the rotation mode of laser beam scanning of a lidar sensor provided according to some embodiments of the present invention is shown.

[0039] In some embodiments of the present invention, since the lidar sensor 300 follows the robot's movement, point cloud distortion may occur. It is preferable to process the point cloud distortion first. For example... Figure 3BAs shown, the coordinate system of the lidar sensor 300 is Y-axis forward, X-axis to the right, and Z-axis upward. Figure 3A ω in, and Figure 3B In this context, α represents the angle value of the current point in polar coordinates. Specifically, ω represents the angle between the current point and the XOY plane, and α represents the angle between the projection of the current point onto the XY plane and the Y-axis. For example... Figure 3B As shown, the laser beam scanning rotation mode of the lidar sensor 300 can be clockwise from the top view direction.

[0040] Further, please see Figure 4 , Figure 4 A schematic diagram of point cloud distortion of a lidar sensor provided according to some embodiments of the present invention is shown. For example... Figure 4 As shown, firstly, the pose O at the start of a point cloud frame is obtained. t and the pose at the end time O t+1 Calculate the end time O t+1 Relative to the initial time O t The pose transformation is then performed. Based on the horizontal angle of the point cloud, the relative time of a point i in a frame of the point cloud can be calculated. The pose transformation of point i relative to the starting time is then interpolated based on the relative time. Finally, the corrected point cloud coordinates are calculated.

[0041] S120: Construct a global point cloud map of the surrounding environment based on point cloud data.

[0042] Please refer to Figure 5 , Figure 5 A flowchart illustrating the construction of a global point cloud map of a surrounding environment, according to some embodiments of the present invention, is shown. Specifically, in conjunction with... Figure 2 and Figure 5 In some embodiments, step S120 can be specified as steps S121 to S123.

[0043] S121: Perform point cloud segmentation on the point cloud data.

[0044] Specifically, in some embodiments, the acquired point cloud data can be first mapped onto a range image to obtain the row and column indices of each point cloud data point. The range image is a two-dimensional matrix representation, where each element represents the distance from that point to the center point of the LiDAR sensor. In other words, this step converts a frame of point cloud data into a two-dimensional matrix. Optionally, in this embodiment, the two-dimensional matrix is ​​16 rows × 1800 columns, meaning the range image resolution is 1800*16. Here, 16 refers to the line length of the LiDAR sensor, and 1800 columns are chosen based on the horizontal angular resolution of the LiDAR sensor being 0.2°. A full scan is 360°, and dividing by this gives 1800.

[0045] Subsequently, column-level evaluation is performed on the obtained range image to determine the ground point cloud data and non-ground point cloud data in the point cloud data. Column-level evaluation refers to determining whether the point corresponding to the element in each column is a ground point, and if it is, it is marked accordingly. For example, taking the above embodiment as an example, a 16-line LiDAR sensor is selected, and these laser beams can be labeled from 0 to 15 from bottom to top. Assuming that the laser beams between 0 and 7 may contain ground points, the point can be determined as a ground point by calculating the corresponding angle of the point corresponding to the element between rows 0 and 7 in each column of the range image. The identified ground point cloud data is treated as a separate class and is not segmented. The non-ground point cloud data can be clustered to divide its point cloud into multiple clusters, and the point cloud data belonging to the same cluster after segmentation is assigned a unique label. That is, all point cloud data in the same cluster share the same label. Optionally, for fast and reliable feature extraction, point cloud clusters with fewer than 30 points can be omitted.

[0046] The point cloud segmentation described above removes noise points and unstable feature points from the input data. When a mobile robot operates in an outdoor environment, small, unreliable features such as leaves and grass may exist. These should be removed and not included in subsequent feature point extraction to reduce outlier data and improve the accuracy of robot localization in unknown environments. It's important to note that since ground points were already labeled during the column-level evaluation stage, all ground points share the same label. Therefore, point cloud data labeled as ground points are not included in this step for point cloud segmentation to avoid redundant operations. After this step, the label of each point and its column and row indices in the range image can be obtained.

[0047] S122: Extract features from the segmented point cloud data to obtain feature points.

[0048] In some embodiments, feature extraction is performed on ground point cloud data and non-ground point cloud data, the roughness of each point cloud data is calculated, and then the feature points can be distinguished into edge feature points and planar feature points according to the detection threshold.

[0049] Specifically, the range image can be horizontally divided into several equal sub-images. Then, the points in each sub-image are sorted according to their roughness. Points with roughness greater than a given threshold are marked as edge feature points, and those with roughness less than the given threshold are marked as planar feature points. For example, the range image can be divided into 6 sub-images, each with a resolution of 300*16. The formula for calculating the roughness of each point can be as follows:

[0050]

[0051] Where |S| represents the size of the set of consecutive points in the same row of the range image, r i Point p i The Euclidean distance to the lidar sensor, c th The roughness detection threshold is c, which can be greater than the detection threshold. th Feature points are defined as edge feature points, and those smaller than the detection threshold c are considered edge feature points. th The feature points are planar feature points.

[0052] S123: Construct a global point cloud map of the surrounding environment based on edge feature points and planar feature points.

[0053] Based on the obtained edge feature points and planar feature points, the pose change of the moving sensor between adjacent frames is estimated. In some embodiments, the motion of the lidar sensor between two consecutive laser scan beams can be estimated by laser odometry, a first constraint relationship between edge feature points and the corresponding lines formed by them, and a second constraint relationship between planar feature points and the corresponding surfaces formed by them are constructed, and point cloud matching is performed between adjacent frames based on the first constraint relationship and the second constraint relationship to estimate the pose change of the moving sensor.

[0054] Specifically, the first constraint relationship is mainly the correspondence between points and edges. Assume the point cloud obtained by the lidar sensor in the k-th scan is P. k It is reprojected onto the point cloud of the (k+1)th scan as The point cloud of the (k+1)th scan is P. k+1 Assuming and P k+1 Given that a point i is P k+1 A point on the edge of a point cloud, Find the point j closest to point i, and then find the point l closest to point i in two consecutive scans of point j. A straight line is determined based on these two points; then (j, l) is the edge corresponding to point i. Here, the edge is the line determined by the edge feature points. The second constraint relationship is mainly the point-to-surface correspondence, which means that, assuming the point cloud of the lidar sensor in the k-th scan is P... k It is reprojected onto point 6 of the (k+1)th scan. The point cloud of the (k+1)th scan is P. k+1 Assuming and P k+1 Given that a point i is P k+1 A specific planar point in a point cloud, Find the point j closest to point i. Then, during the scan of point j, find the point l closest to point i. Finally, during two consecutive scans of point j, find the point m closest to point i. Based on these three points, a plane is defined; (j, l, m) is the plane corresponding to point i. Here, the plane is defined by the feature points of the plane.

[0055] Subsequently, in some embodiments, label matching and two-step LM optimization can be used to improve the accuracy and efficiency of point-to-edge and point-to-surface matching. Specifically, firstly, for labeled feature points, associated feature points with the same label are searched in the previous frame for label matching. Label matching uses the label of each point obtained after point cloud segmentation for matching, searching for associated points only in point cloud clusters with the same label. For edge feature points, a correspondence is constructed only in the point cloud labeled as segmentation points (i.e., the first constraint relationship mentioned above), while for planar feature points, a correspondence is constructed only in the point cloud labeled as ground points (i.e., the second constraint relationship mentioned above) for matching, to ensure that ground information remains unchanged in adjacent frames.

[0056] The two-step LM (LeGO-LOAM) optimization front-end is divided into two steps, each estimating a three-degree-of-freedom variable. Please refer to [link / reference needed]. Figure 6 , Figure 6 A schematic diagram of a two-step optimized calculation process for laser odometry, according to some embodiments of the present invention, is shown. For example... Figure 6 As shown, in some embodiments, a global vector can be defined first. In the first optimization, the planar feature points are optimized to obtain the first estimation result [t]. z ,θ roll ,θ pitch After that, the first estimation result obtained from the first optimization [t] z ,θ roll ,θ pitch The result will be updated in the global vector, and the result of the previous optimization will be used in the second optimization to obtain the second estimate [t]. x ,t y ,θ yaw In other words, in some embodiments of the present invention, the two-step LM optimization can estimate [t] by matching planar features and planar points. z ,θ roll ,θ pitch ], by matching edge features and edge points and using [t z ,θ roll ,θ pitch As a constraint, [t] can be estimated. x ,t y ,θ yaw ].

[0057] Furthermore, in some embodiments, the first estimation result [t] can be integrated. z ,θ roll ,θ pitch ] and the second estimation result [t x ,t y ,θ yaw ], to obtain the optimized estimation results of the pose change of the moving sensor [t] x ,t y ,t z ,θ roll ,θ pitch ,θ yaw ], where [t x ,t y ,t z ,θ roll ,θ pitch ,θ yaw This refers to the transformation relationship obtained by using a two-step LM optimization method via laser odometer.

[0058] Subsequently, point cloud matching is performed between the feature points of the current frame and the surrounding point cloud map to obtain the optimal pose change of the current frame. In some embodiments, point cloud registration can be performed between the feature points and the surrounding point cloud map to further optimize the pose transformation. Specifically, the point cloud matching process includes feature matching of edge feature points according to the above-mentioned point-to-edge correspondence, and feature matching of planar feature points according to the above-mentioned point-to-face correspondence. However, this is not the point cloud of the (k+1)th scan, but all point clouds of the (k+1)th scan and all previous scans. The formula for constructing the distance from a point to a line is:

[0059]

[0060] in, Let i, j, and l be the coordinates of points i, j, and l in the lidar coordinate system, respectively.

[0061] Construct the formula for the distance from a point to a surface:

[0062]

[0063] in, Let m be the coordinates of point m in the lidar coordinate system.

[0064] Determine if the distances from a point to a line and from a point to a plane meet the set values. If they do, add the point to the point cloud.

[0065] After that, continue as Figure 6As shown, each successfully matched feature point is registered in the global point cloud map to obtain a global point cloud map of the surrounding environment. In some embodiments, a sensor-view-based and graph-optimized method can be used to register each feature point stored according to its label into the global point cloud map to obtain the global point cloud map.

[0066] Specifically, firstly, based on the field of view of the mobile sensor, a feature set can be obtained within its operating measurement range, which can include a preset distance within the measurement angles in the vertical and horizontal directions in front of it. For example, a feature set within 100 meters of the sensor's current position can be obtained. Then, these feature sets are transformed and fused into a single map. Graph optimization-based methods integrate the pose graph, using the pose of the mobile sensor as nodes, and utilize the optimized estimation result obtained by a two-step LM optimization method, i.e., the transformation relationship [t]. x ,t y ,t z ,θ roll ,θ pitch ,θ yaw As edge constraints, distance constraints, time constraints, and loop closure constraints are also used as constraint edges to reduce cumulative errors. Here, distance and time constraints mean that when a loop occurs, the distance between the two points must be sufficiently close, and the time interval between collecting data from these two points must be sufficiently long. The loop closure constraint means that when the robot passes through a place it has visited before, a loop has occurred, and the robot needs to be aware that it has been there before.

[0067] Figure 6 In the illustrated embodiment, the transformation fusion involves publishing the pose information obtained from the laser odometry, the corrected pose information obtained from laser mapping, and the point cloud map, all of which are published as topics for easy viewing in the robot visualization tool (rviz).

[0068] S130: Construct closure constraints for the global point cloud using a global descriptor.

[0069] In some embodiments, a scene-based global descriptor can be used to scan the context, thereby effectively identifying the observed region, and constructing loop closure constraints if loop closure detection is successful.

[0070] Specifically, the point cloud of each frame can first be divided into point cloud segments to generate a global descriptor for scanning the context. In some optional embodiments, the point cloud space can be equally divided into N segments along the direction of increasing radius. r There are N rings, and each ring is then cut into N pieces. s Divide the point cloud into equal parts and map the resulting point cloud to N. r×N s In matrix I, each row represents a ring, each column represents a sector, and the value of each element represents the maximum height of all 3D points in that partition unit. Then, a preliminary search is performed using a vector nearest neighbor search algorithm to obtain preliminary results, followed by a more precise search using scan similarity scoring. Specifically, in some embodiments, the distance between two point clouds can be calculated based on a global descriptor to perform similar frame search, where the distance formula is as follows:

[0071]

[0072] Among them, I q The global descriptor representing the current frame, I p N represents the global descriptor of the history frame. s The number of fan-shaped parts into which the point cloud is divided. For I q The j-th column in For I p The j-th column in the array.

[0073] The more similar all corresponding column vectors are between two point clouds, the more similar the two point clouds are. In some embodiments, when the distance function between two point clouds is less than a preset threshold, it can be determined that the similarity between the two point clouds is high, the loop closure detection is successful, and the historical frame is used as the loop closure frame for loop closure detection to construct loop closure constraints.

[0074] The above embodiments of the present invention use a global descriptor-assisted system based on scene recognition for loop closure detection. This not only considers the distance and time constraints between point cloud data, but also the global features of each point, reducing the cumulative error of the system and improving the accuracy of system positioning. At the same time, it can run on an embedded mobile platform, meeting the real-time requirements of system computation.

[0075] S140: Minimize the error function using a nonlinear optimization function and the closure constraint to optimize the global point cloud map and the pose of the moving sensor.

[0076] In some embodiments, the minimized error function can be obtained through a nonlinear optimization function and closure constraints. The optimal solution for the pose of the moving sensor is obtained by solving the nonlinear least squares problem using the Gauss-Newton method, minimizing the overall error function. The expression for the overall error function is as follows:

[0077] F(x)=∑e k (x k ,z k ) T ω k e k(x k ,z k )

[0078] Where F(x) is the sum of errors over all constraints, x is the pose set, and e k Indicates the xth k pose and z-th k The error between constraints, ω k Is and x k Related information matrix.

[0079] In some embodiments, the results of laser odometry and laser mapping modules can be fused to obtain accurate pose and map data. The received high-frequency, low-precision laser odometry results are transformed and fused with the low-frequency, high-precision laser mapping results to obtain high-frequency, high-precision result data.

[0080] In the above embodiments of the present invention, the point cloud map of the environment that is finally constructed can be converted into a two-dimensional grid map in an indoor environment and into an octree map in an outdoor environment, which can serve higher-level tasks such as robot motion planning and navigation.

[0081] In this invention, the LeGO-LOAM algorithm is improved by incorporating other methods for loop closure detection. Since this invention focuses on a single LiDAR sensor, it employs a motion distortion correction method distinct from traditional inertial measurement units (IMUs). Furthermore, in the traditional LeGO-LOAM algorithm, the loop closure detection module is disabled by default; when enabled, detection only satisfies constraints of sufficiently close distance and sufficiently long time span. If the system's accumulated error is large, such detection may be erroneous. In contrast, this invention simultaneously applies a global descriptor suitable for position recognition, enhancing the system's loop closure detection capability, providing loop closure constraints for backend optimization, further correcting pose, reducing accumulated error, and improving positioning accuracy, thereby improving system performance. Therefore, this invention is based on LeGO-LOAM, integrating other methods to improve upon its shortcomings and modifying it to address discrepancies in practical applications.

[0082] Although the methods described above are illustrated and depicted as a series of actions for the sake of simplicity, it should be understood and appreciated that these methods are not limited by the order of the actions, as some actions may occur in a different order and / or concurrently with other actions from the illustrations and descriptions herein or not illustrated and described herein but which may be understood by those skilled in the art, according to one or more embodiments.

[0083] This concludes the description of the mobile positioning method provided in the first aspect of the present invention. Another aspect of the present invention provides a mobile positioning device. Please refer to... Figure 7 , Figure 7 A structural block diagram of a mobile positioning device provided according to some embodiments of the present invention is shown.

[0084] like Figure 7 As shown, in some embodiments, the mobile positioning device 700 may be configured with a memory 710 and a processor 720. The memory 710 includes, but is not limited to, the computer-readable storage medium described in the third aspect of the present invention, on which computer instructions are stored. The processor 720 is connected to the memory 710 and configured to execute the computer instructions stored in the memory 710 to implement the mobile positioning method described in the first aspect of the present invention.

[0085] Those skilled in the art will understand that the above-described embodiments of mobile positioning methods are merely non-limiting implementations of the present invention, intended to clearly demonstrate the main concepts of the invention and provide specific solutions that are convenient for the public to implement, rather than limiting all operating modes or functions of the mobile positioning device 700. Similarly, the mobile positioning device 700 is also only one non-limiting implementation of the present invention and does not constitute a limitation on the entities implementing the steps in these mobile positioning methods.

[0086] In summary, the present invention provides a mobile positioning method, a mobile positioning device, and a computer-readable storage medium, which can improve the accuracy and real-time performance of mobile positioning in unknown environments, and also improve the system's ability to effectively identify observed areas, obtain more accurate detection loops, and thus reduce cumulative errors.

[0087] The prior description of this disclosure is provided to enable any person skilled in the art to make or use this disclosure. Various modifications to this disclosure will be apparent to those skilled in the art, and the general principles defined herein may be applied to other variations without departing from the spirit or scope of this disclosure. Therefore, this disclosure is not intended to be limited to the examples and designs described herein, but should be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A mobile positioning method, characterized in that, Includes the following steps: Point cloud data of the surrounding environment is acquired by moving sensors; Based on the point cloud data, a global point cloud map of the surrounding environment is constructed; Using global descriptors, construct the loop closure constraints of the global point cloud graph; as well as The error function is minimized using a nonlinear optimization function and the closure constraint to optimize the global point cloud map and the pose of the moving sensor. In the step of constructing the global point cloud map of the surrounding environment, unique labels are assigned to point cloud data belonging to the same class after point cloud segmentation. This ensures that during feature matching, associated feature points are found only in point cloud clusters with the same label, thus establishing corresponding constraint relationships. The step of constructing a global point cloud map of the surrounding environment based on the point cloud data includes: Point cloud segmentation of the point cloud data includes: mapping the acquired point cloud data onto a range image to obtain the row and column indices of each point cloud data; performing column-level evaluation on the range image to determine ground point cloud data and non-ground point cloud data in the point cloud data; and treating the ground point cloud data as a separate category, segmenting the non-ground point cloud data, and assigning a unique label to the segmented point cloud data belonging to the same category. The step of constructing the loop closure constraints of the global point cloud map using a global descriptor includes: Perform point cloud segmentation on each frame of point cloud; The distance between two point clouds is calculated based on the global descriptor to perform similar frame search. The distance formula is as follows: in, The global descriptor representing the current frame. A global descriptor representing historical frames. The number of sectors into which the point cloud is divided. for The j-th column in for The j-th column in; and In response to the distance between the two point clouds being less than a preset threshold, it is determined that the similarity between the two point clouds is high, the loop closure detection is successful, and the historical frame is used as the loop closure frame for the loop closure detection to construct the loop closure constraint.

2. The mobile positioning method as described in claim 1, characterized in that, The mobile sensor includes a lidar sensor, and the step of acquiring point cloud data of the surrounding environment through the mobile sensor includes: Point cloud data of the surrounding environment and objects collected by the multi-angle movement of the lidar sensor; and In response to point cloud distortion in the collected point cloud data, the point cloud coordinates of the point cloud data are corrected based on the pose change of the point cloud data at the end time relative to its start time.

3. The mobile positioning method as described in claim 2, characterized in that, The step of constructing a global point cloud map of the surrounding environment based on the point cloud data includes: Perform point cloud segmentation on the point cloud data; Feature extraction is performed on the segmented point cloud data to obtain feature points, wherein the feature points include edge feature points and planar feature points; and Based on the edge feature points and planar feature points, a global point cloud map of the surrounding environment is constructed.

4. The mobile positioning method as described in claim 3, characterized in that, The step of extracting features from the segmented point cloud data to obtain feature points includes: Feature extraction is performed on the ground point cloud data and the non-ground point cloud data; Based on the roughness of each point cloud data point, the feature points are distinguished into edge feature points and planar feature points, wherein the roughness is calculated using the following formula: Among them, | S | represents the size of the set of consecutive points in the same row of the range image. Point Euclidean distance to the laser detection device c th The detection threshold for the roughness is greater than the detection threshold. c th The feature points are the edge feature points, which are smaller than the detection threshold. c th The feature points are the planar feature points.

5. The mobile positioning method as described in claim 3, characterized in that, The step of constructing a global point cloud map of the surrounding environment based on the edge feature points and planar feature points includes: Based on the edge feature points and the planar feature points, the pose change of the motion sensor between adjacent frames is estimated; Point cloud matching is performed between the feature points of the current frame and the surrounding point cloud map to obtain the optimal pose change of the current frame; and Each successfully matched feature point is registered in the global point cloud map to obtain a global point cloud map of the surrounding environment.

6. The mobile positioning method as described in claim 5, characterized in that, The step of estimating the pose change of the motion sensor between adjacent frames based on the edge feature points and the planar feature points includes: Construct a first constraint relationship from the edge feature points to the corresponding lines formed by them, and a second constraint relationship from the planar feature points to the corresponding surfaces formed by them; and Based on the first constraint relationship and the second constraint relationship, point cloud matching is performed between two adjacent frames to estimate the pose change of the motion sensor.

7. The mobile positioning method as described in claim 6, characterized in that, The step of performing point cloud matching between two adjacent frames based on the first constraint relationship and the second constraint relationship to estimate the pose change of the motion sensor includes: For the labeled feature points, search for related feature points with the same label in the previous frame to perform label matching; The planar feature points are optimized using the LeGO-LOAM two-step optimization method to obtain the first estimation result [t]. z ,θ roll ,θ pitch ] ; Based on the edge feature points and the constraints of the first estimation result, a second estimation result [t] is obtained. x ,t y ,θ yaw ];as well as The first estimation result and the second estimation result are integrated to obtain an optimized estimation result of the pose change of the moving sensor.

8. The mobile positioning method as described in claim 7, characterized in that, The step of registering each successfully matched feature point to a global point cloud map to obtain a global point cloud map of the surrounding environment includes: Based on the field of view of the mobile sensor, a feature set of preset distances within the measurement angles in the vertical and horizontal directions in front of it is obtained; The feature set is transformed and fused into a single map; and Using the pose of the mobile sensor as a node, and the optimized estimation result, distance constraint, time constraint, and loop closure constraint as constraint edges, each successfully matched and labeled feature point is registered in the global point cloud map to obtain the global point cloud map.

9. The mobile positioning method as described in claim 1, characterized in that, The step of minimizing the error function using a nonlinear optimization function and the closure constraint to optimize the global point cloud map and the pose of the moving sensor includes: Using the nonlinear optimization function and the lapsing constraint, the error function is minimized; and The nonlinear least squares problem is solved using the Gauss-Newton method to minimize the overall error function and obtain the optimal value for the pose of the moving sensor. The expression for the overall error function is as follows: in, This is the sum of errors over all constraints. x It is a set of poses. Indicates the first Posture and first Error between constraints Is and Related information matrix.

10. The mobile positioning method as described in claim 9, characterized in that, The step of minimizing the error function using a nonlinear optimization function and the closure constraint to optimize the global point cloud map and the pose of the moving sensor further includes: Acquire high-frequency, low-precision pose information of the motion sensor, optimized low-frequency, high-precision pose information of the motion sensor, and constrained global point cloud information; and By transforming and fusing the two pose information from the mobile sensor and the global point cloud information, high-frequency and high-precision optimization result data is obtained.

11. A mobile positioning device, characterized in that, include: Memory; as well as A processor, connected to the memory, and configured to implement the mobile positioning method as described in any one of claims 1 to 10.

12. A computer-readable storage medium storing computer instructions thereon, characterized in that, When the computer instructions are executed by the processor, the mobile positioning method as described in any one of claims 1 to 10 is implemented.

Images