A robot mapping and positioning method, device, equipment and storage medium

Abstract

This application discloses a robot mapping and positioning method, apparatus, device, and storage medium, relating to the field of synchronous positioning and mapping technology, including: adaptively filtering the two-dimensional lidar point cloud of the current environment, screening the filtered point cloud to obtain corner points and line points; fitting the line points to obtain fitted line segments, obtaining a local submap, matching local correlation features based on the corner points, fitted line segments, and point cloud features of each area of ​​the current environment, the local submap, and the filtered point cloud to obtain the robot's rough pose; obtaining the robot's pose through feature constraints determined based on the robot's rough pose, constructing a grid map and a feature map based on the robot's pose; obtaining and optimizing the historical robot pose at different times, and adjusting the grid map and feature map based on the optimized robot pose. This application achieves high-precision mapping and positioning.

Description

Technical Field

[0001] The present invention relates to the field of synchronous positioning and mapping technology, and in particular to a robot mapping and positioning method, device, equipment and storage medium. Background Art

[0002] Simultaneous localization and mapping is the process by which a logistics robot, in an unknown environment, uses sensor observations and historical map information to determine its current position and builds a map based on that historical position. This solves the problem of simultaneous localization and mapping. While numerous mapping and localization methods exist, many practical applications still face challenges, including positioning loss or instability, mapping failures or suboptimal results, and poor localization and mapping accuracy. Therefore, a stable and highly accurate mapping and localization method is needed to meet the requirements of complex scenarios. Summary of the Invention

[0003] In view of this, the purpose of the present invention is to provide a robot mapping and positioning method, device, equipment and storage medium that can stably and accurately complete mapping and positioning to meet the requirements of complex actual scenarios. The specific solution is as follows:

[0004] In a first aspect, the present application discloses a robot mapping and positioning method, comprising:

[0005] Obtaining a two-dimensional laser radar point cloud of the current environment, performing adaptive filtering on the two-dimensional laser radar point cloud to obtain a filtered point cloud, and screening the filtered point cloud to obtain corner points and line points;

[0006] Fitting the line points to obtain fitted line segments, obtaining a local submap based on the historical map, and performing local correlation feature matching on the local submap and the filtered point cloud based on the corner points, the fitted line segments, and the point cloud features of each area of ​​the current environment to obtain a rough pose of the robot; the local submap includes a grid submap and a feature submap;

[0007] Determining feature constraints based on the rough posture of the robot, obtaining the posture of the robot through the feature constraints, and constructing a grid map and a feature map according to the posture of the robot;

[0008] Acquire historical robot postures at different moments, optimize each of the historical robot postures, and adjust the grid map and the feature map according to the corresponding optimized robot posture.

[0009] Optionally, performing adaptive filtering on the two-dimensional lidar point cloud to obtain a filtered point cloud includes:

[0010] Dividing the two-dimensional lidar point cloud according to a grid based on the current first resolution to obtain a divided point cloud;

[0011] Calculate the center of all divided point clouds in each grid, adjust the current first resolution to obtain a new current first resolution, and jump again to the step of dividing the two-dimensional lidar point cloud according to the grid based on the current first resolution, until the number of jumps meets the preset conditions, determine the filtered point cloud according to the center of the divided point cloud.

[0012] Optionally, screening the filtered point cloud to obtain corner points and line points includes:

[0013] Calculating the smoothness of the filtered point cloud, and screening the filtered point cloud according to the smoothness and a target threshold to obtain corner points and line points; wherein, if the number of the filtered point cloud is less than the target threshold, determining the filtered point cloud as the corner point.

[0014] Optionally, fitting the line points to obtain fitted line segments includes:

[0015] Dividing the line points into regions according to the second resolution to obtain a plurality of divided regions;

[0016] Clustering the line points in each of the divided areas to obtain a plurality of clustered line points;

[0017] Line segment fitting is performed on each of the clustered line points in each of the divided areas to obtain a fitting line segment.

[0018] Optionally, the performing local correlation feature matching based on the corner points, the fitting line segments, the point cloud features of each area of ​​the current environment, the local submap, and the filtered point cloud to obtain a rough pose of the robot includes:

[0019] Determining a local search space, and calculating a first matching degree between the filtered point cloud and the grid submap in the local search space;

[0020] Calculating a second matching degree between the corner points, the fitting line segments, and the point cloud features of each area of ​​the current environment in the local search space and the feature submap;

[0021] A target matching degree is determined according to the first matching degree and the second matching degree, and a rough position pose of the robot corresponding to the target matching degree is obtained.

[0022] Optionally, determining a feature constraint based on the rough pose of the robot and obtaining the pose of the robot through the feature constraint includes:

[0023] Calculating the feature constraints based on the rough pose of the robot, the filtered point cloud, the grid submap, the feature submap, and the corner points, the fitted line segments, and the point cloud features of each area of ​​the current environment;

[0024] Calculating a cost function based on the feature constraints, the rough pose of the robot, the filtered point cloud, the grid submap, the feature submap, the corner points, the fitted line segments, and point cloud features of each area of ​​the current environment;

[0025] The robot pose is obtained based on the cost function and the rough pose of the robot.

[0026] Optionally, optimizing each of the historical robot poses and adjusting the grid map and the feature map according to the corresponding optimized robot poses includes:

[0027] Calculating constraints between key nodes based on the robot posture;

[0028] Determining constraints between key nodes and submaps based on the robot posture and the local submap;

[0029] determining inter-submap constraints based on the local submap;

[0030] Determining inter-node constraint residuals based on the key inter-node constraints and the robot pose;

[0031] Determine the constraint residual between the key node and the submap based on the robot pose, the local submap, and the constraint between the key node and the submap;

[0032] Determining an inter-submap constraint residual based on the local submap and the inter-submap constraint;

[0033] Performing constraint residual optimization through the inter-node constraint residual, the inter-key node and sub-map constraint residual, and the inter-sub-map constraint residual to adjust and optimize each of the historical robot poses;

[0034] The grid map and the feature map are adjusted according to a conversion relationship between the optimized robot posture and the historical robot posture.

[0035] In a second aspect, the present application discloses a robot mapping and positioning device, comprising:

[0036] A corner point and line point acquisition module is used to obtain a two-dimensional laser radar point cloud of the current environment, perform adaptive filtering on the two-dimensional laser radar point cloud to obtain a filtered point cloud, and filter the filtered point cloud to obtain corner points and line points;

[0037] A rough pose acquisition module is configured to fit the line points to obtain fitted line segments, obtain a local submap based on the historical map, and perform local correlation feature matching between the local submap and the filtered point cloud based on the corner points, the fitted line segments, and the point cloud features of each area of ​​the current environment to obtain the rough pose of the robot; the local submap includes a grid submap and a feature submap;

[0038] a map construction module, configured to determine feature constraints based on the rough pose of the robot, obtain the pose of the robot through the feature constraints, and construct a grid map and a feature map according to the pose of the robot;

[0039] The map determination module is used to obtain historical robot postures at different times, optimize each of the historical robot postures, and adjust the grid map and the feature map according to the corresponding optimized robot postures.

[0040] In a third aspect, the present application discloses an electronic device, comprising:

[0041] Memory, used to store computer programs;

[0042] A processor is used to execute the computer program to implement the robot mapping and positioning method as described above.

[0043] In a fourth aspect, the present application discloses a computer-readable storage medium for storing a computer program; wherein, when the computer program is executed by a processor, it implements the robot mapping and positioning method as described above.

[0044] When performing mapping and positioning, the present application first obtains a two-dimensional lidar point cloud of the current environment, performs adaptive filtering on the two-dimensional lidar point cloud to obtain a filtered point cloud, and screens the filtered point cloud to obtain corner points and line points; then the line points are fitted to obtain fitted line segments, and a local submap is obtained according to the historical map. Based on the corner points, the fitted line segments and the point cloud features of each area of ​​the current environment, the local submap and the filtered point cloud are matched with local correlation features to obtain a rough posture of the robot; the local submap includes a grid submap and a feature submap; then, feature constraints are determined based on the rough posture of the robot, the posture of the robot is obtained through the feature constraints, and a grid map and a feature map are constructed according to the posture of the robot; finally, the historical robot postures at different times are obtained, each of the historical robot postures is optimized, and the grid map and the feature map are adjusted according to the corresponding optimized robot posture. It can be seen that this application adaptively filters the lidar point cloud, screens the point cloud, and obtains corner points and line points; uses point cloud features and environmental map features to perform feature matching and constraint optimization precision matching, calculates the robot pose, and establishes a grid map and feature map. Finally, it calculates the constraints, optimizes the pose according to the constraints, and adjusts the grid map and feature map. In this way, by providing more accurate and reliable pose information, this application can improve the stability of the mapping algorithm, solve the problems of unstable and biased robot mapping, ensure the quality of mapping, and be more suitable for actual application scenarios. At the same time, it provides more accurate maps and positioning, which can meet the needs of precise docking scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are merely embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any creative work.

[0046] Figure 1 This is a flow chart of a robot mapping and positioning method disclosed in this application;

[0047] Figure 2 A schematic diagram of a two-dimensional laser radar point cloud disclosed in this application;

[0048] Figure 3 This is a schematic diagram of a filtered point cloud disclosed in this application;

[0049] Figure 4 A schematic diagram of a corner point disclosed in this application;

[0050] Figure 5 A line-point schematic diagram disclosed in this application;

[0051] Figure 6 A schematic diagram of a fitting line segment disclosed in this application;

[0052] Figure 7 A schematic diagram of a grid sub-map disclosed in this application;

[0053] Figure 8 This is a schematic diagram of a characteristic sub-map disclosed in this application;

[0054] FIG9(a) is a schematic diagram of a line segment slope disclosed in the present application; FIG9(b) is a schematic diagram of a distance between adjacent line segments disclosed in the present application; FIG9(c) is a schematic diagram of an angle between adjacent line segments disclosed in the present application;

[0055] Figure 10 A schematic diagram of regional characteristics disclosed in this application;

[0056] FIG11( a ) is a schematic diagram of a line-line distance constraint disclosed in the present application; FIG11( b ) is a schematic diagram of a point-line distance constraint disclosed in the present application;

[0057] Figure 12 This is a flowchart of a specific robot mapping and positioning method disclosed in this application;

[0058] Figure 13 A schematic diagram of constraints between key nodes disclosed in this application;

[0059] Figure 14 A schematic diagram of key nodes and local sub-map constraints disclosed in this application;

[0060] Figure 15 This is a schematic diagram of constraints between sub-maps disclosed in this application;

[0061] Figure 16 This is a schematic structural diagram of a robot mapping and positioning device disclosed in this application;

[0062] Figure 17 This is a structural diagram of an electronic device disclosed in this application. DETAILED DESCRIPTION

[0063] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0064] Current mapping and positioning methods can suffer from positioning loss or instability, mapping failure or unsatisfactory results, and poor positioning and mapping accuracy. To address these technical issues, this application discloses a robot mapping and positioning method, apparatus, device, and storage medium that can stably and accurately perform mapping and positioning, meeting the requirements of complex real-world scenarios.

[0065] See also Figure 1 As shown, an embodiment of the present invention discloses a robot mapping and positioning method, including:

[0066] Step S11: Obtain a two-dimensional laser radar point cloud of the current environment, perform adaptive filtering on the two-dimensional laser radar point cloud to obtain a filtered point cloud, and filter the filtered point cloud to obtain corner points and line points.

[0067] In this embodiment, the present application first obtains the two-dimensional lidar point cloud information of the current environment as: ,like Figure 2 As shown in the figure, the two-dimensional laser point cloud information is the point cloud in the robot coordinate system, which is obtained by converting the point cloud in the laser radar coordinate system. Then the two-dimensional laser radar point cloud is adaptively filtered to obtain the filtered point cloud. ,like Figure 3 Specifically, the two-dimensional laser radar point cloud is divided according to the grid based on the current first resolution to obtain the divided point cloud; the center of all the divided point clouds in each grid is calculated, the current first resolution is adjusted to obtain a new current first resolution, and the step of dividing the two-dimensional laser radar point cloud according to the grid based on the current first resolution is jumped again until the number of jumps meets the preset conditions, and the filtered point cloud is determined according to the center of the divided point cloud. Specifically,

[0068] According to resolution 2D LiDAR point cloud Divide by grid and obtain the point cloud G after division: ;

[0069] Then, calculate the center of all point clouds in each grid : ;

[0070] Adjust resolution , loop through the above two steps to get the filtered point cloud : ;

[0071] in, Represents a grid partitioning function (a function provided by the program library for partitioning the grid, such as the meshgrid function), represents the grid point cloud computing function (including but not limited to the mean function), n represents the number of grids, Indicates a loop function (including but not limited to the while loop function) that meets the conditions End the loop when Represents the point cloud after the i-th division. It can be understood as a parameter. If it is 2, it means looping twice, and if it is 3, it means looping 3 times.

[0072] After obtaining the filtered point cloud, the filtered point cloud is screened to obtain corner points and line points. When a small number of filtered point clouds are selected, the smoothness of the filtered point cloud is calculated, and the filtered point cloud is screened based on the smoothness and the target threshold to obtain corner points and line points. If the number of filtered point clouds is less than the target threshold, the filtered point cloud is determined to be a corner point. Specifically, the point cloud smoothness S1 is calculated as follows: ; Filter corner points according to the smoothness threshold th1 ; Line point :

[0073] ;

[0074] in, Represents the smoothness function (the library provides a function for calculating smoothness, including but not limited to the smooth function and calculateSmoothness() function), the filtered point cloud When the number is less than the preset point cloud threshold th2, it is processed as a corner point by default. Represents a filtering function (a function that implements filtering by numerical comparison, for example, if the point cloud after filtering The number of is less than the preset point cloud number threshold th2, then The point cloud is considered as a corner point by default, and the point cloud smoothness comparison is no longer performed; if the point cloud after filtering If the number of points is greater than or equal to the preset point cloud number threshold th2, a point cloud smoothness comparison is performed: if the point cloud smoothness S1 of a filtered point cloud is 3, if the smoothness threshold th1 is 1, then the point cloud smoothness S1 is greater than the smoothness threshold th1, and the filtered point cloud is a line point; if the smoothness threshold th1 is 4, then the point cloud smoothness S1 is less than the smoothness threshold th1, and the filtered point cloud is a corner point). The filtered point cloud with a small smoothness belongs to a corner point, such as Figure 4 As shown, the line points are Figure 5 shown.

[0075] Step S12: Fit the line points to obtain fitted line segments, obtain a local submap based on the historical map, and perform local correlation feature matching on the local submap and the filtered point cloud based on the corner points, the fitted line segments, and the point cloud features of each area of ​​the current environment to obtain a rough pose of the robot; the local submap includes a grid submap and a feature submap.

[0076] In this embodiment, after obtaining line points and corner points, the line points are fitted, and the line points are divided into regions according to the second resolution to obtain a number of divided regions; the line points in each divided region are clustered to obtain a number of clustered line points; and line segment fitting is performed on each clustered line point in each divided region to obtain a fitted line segment. Specifically, according to the resolution Divide the laser point cloud into regions and obtain several divided regions R: ; Then cluster the line points in each divided area to obtain several clustered line points : ; Then perform line segment fitting on each clustered line point in each divided area to obtain the fitted line segment : ;in, Represents a region division function (a function provided by a library for dividing regions, such as the cut function in the Pandas library). Represents point cloud clustering functions, including but not limited to Euclidean clustering functions, Kmeans clustering functions, etc. Represents a line segment fitting function, including but not limited to the least squares fitting function, RANSAC (Random Sample Consensus) fitting function, etc. The result is as follows Figure 6 As shown, n1 represents the number of laser areas.

[0077] Then, the local submap is obtained from the historical map based on the robot pose and the submap origin pose. , in this application, the local submap is as follows Figure 7 and Figure 8 As shown, including raster submap and feature submaps , which is composed of laser point clouds of local key nodes. Specifically, when obtaining the grid sub-map and feature sub-map, define the sub-map origin pose and the current robot pose, and then perform relative transformation calculation on the sub-map origin pose and the current robot pose to convert the robot pose to the local coordinate system of the sub-map. Then obtain the sub-map origin adjacent to the robot pose, and obtain the grid sub-map from the global grid map according to the first sub-map index. Similarly, obtain the sub-map origin adjacent to the robot pose, and obtain the grid sub-map from the global feature map according to the second sub-map index. At the same time, calculate the feature , including corner features, line features, and region features; calculate corner feature CF from corner points, including but not limited to the angle and distance of corner points. Calculate line segment features LF from fitted line segments, as shown in Figure 9 (a) Line segment slope (in the figure is the inclination angle of the line segment, and the slope of the line segment can be determined according to the inclination angle), Figure 9 (b) The distance between line segments (in the figure is the angle between the extension lines of the two line segments, L is the distance between the two line segments) and the angle between the line segments in Figure 9 (c) (in the figure Represents the angle between two line segments), including but not limited to length, slope, endpoints, angle between line segments, distance between line segments, etc. The regional feature RF is calculated from the point cloud in the region, as shown in Figure 10 As shown, including but not limited to the mean value and normal distribution of the point cloud in the region, the horizontal axis represents the distance from the point cloud to the center in the region, and the vertical axis represents the probability density.

[0078] Then, based on the rough matching of the point cloud features of the corner points, fitted line segments and each area of ​​the current environment, the local sub-map and the local correlation features of the filtered point cloud, the rough position and posture of the robot are obtained. In this process, the local search space is first determined, and the first matching degree between the filtered point cloud and the grid sub-map in the local search space is calculated; the second matching degree between the corner points, fitted line segments and each area of ​​the current environment in the local search space and the feature sub-map is calculated; the target matching degree is determined based on the first matching degree and the second matching degree, and the rough position and posture of the robot corresponding to the target matching degree is obtained. Specifically:

[0079] First, generate the local search space S according to the interval range: ;

[0080] Compute laser point clouds and raster submaps in the search space The first match : ;

[0081] Compute point cloud features and feature submaps in the search space The second matching degree : ;

[0082] Calculate the final matching degree FS: ;

[0083] Get the rough pose of the robot based on the matching degree : ;

[0084] in: Represents the search space generation function (a function provided by the library for generating a search space. The specific implementation process is: determine the position according to the coordinates and orientation angle of the robot , with resolution 1 (the minimum adjustment step of the position coordinates), (the minimum unit of angle adjustment) to generate a search space, and then each degree of freedom is adjusted by adding or subtracting the resolution step size based on its current value to form a candidate pose. The generated search space is 、 、 、 、 、 etc.), Represents the calculation matching function (such as CSM (Correlative ScanMatching, scan matching algorithm), ICP (Iterative Closest Point, PCL (Point Cloud Library, an open source, cross-platform point cloud processing library) library provided by the point cloud registration algorithm), etc.), represents the number of poses in the search space, represents the weighted average function, Represents the pose screening function (i.e., the maximum value function, the specific implementation process is: if the search space The matching degree is 0.9, and the search space The matching degree is 0.8, then the search space corresponding to the maximum matching degree is selected, and the posture after screening is ).

[0085] Step S13: determining feature constraints based on the rough posture of the robot, obtaining the posture of the robot through the feature constraints, and constructing a grid map and a feature map according to the posture of the robot.

[0086] In this embodiment, feature constraints are calculated based on the robot's rough pose, filtered point cloud, grid submap, feature submap, corner points, fitted line segments, and point cloud features of each region of the current environment; a cost function is calculated based on the feature constraints, the robot's rough pose, filtered point cloud, grid submap, feature submap, corner points, fitted line segments, and point cloud features of each region of the current environment; and the robot's pose is obtained based on the cost function and the robot's rough pose. Specifically, the feature constraint C is calculated as follows: ;

[0087] Calculate the residual cost CF:

[0088] ;

[0089] Optimize the cost function to obtain the precise posture of the robot :

[0090] ;

[0091] Among them, B|D means that the calculation is based on B with D as the premise, for example, Indicates The precise pose of the robot is calculated based on CF. Represents a constraint calculation function (a function that calculates constraints based on matrix operations, for example, the MaybeAddConstraint function in ConstraintBuilder2D, the ComputeConstraint function in ConstraintBuilder2D, etc. In the robot pose Features and submaps in the coordinate system ( 、 ) are consistent, then calculate the feature ( The distance difference between the features in the sub-map coordinate system is obtained by multiplying a matrix (feature pose) by the inverse of another matrix (sub-map origin pose) to obtain the relative motion constraint in the coordinate system), as shown in Figure 11 (a) point-line distance constraint and Figure 11 (b) line-line distance constraint, including but not limited to point-point distance constraint, point-line distance constraint, line-line distance constraint, and point cloud grid map constraint. Represents a cost function (a function that calculates the cost based on matrix operations, such as CreateOccupiedSpaceCostFunction2D function, TranslationDeltaCostFunctor2D function, RotationDeltaCostFunctor2D function, etc.). A matrix (a Medium features or Point cloud, in Subtract another matrix (the feature constraints in the submap) from the pose under the submap, calculate the Euclidean distance, Manhattan distance, etc., and determine the value obtained by matrix subtraction combined with a specific distance metric as the cost of the difference between the two matrices). Represents the optimization function (provided by a nonlinear optimizer, which iteratively reduces the CF residual (cost) through numerical optimization methods (Gauss-Newton method, gradient descent method), such as the AutoDiffCostFunction function in ceres).

[0092] Once the robot's posture is obtained, positioning is completed. After obtaining the robot's posture, a grid map and a feature map are constructed based on the robot's posture. Specifically, based on the robot's posture in the map, the latest frame of point cloud is moved to the robot's posture to construct a grid map, and the features of the latest frame of point cloud are moved to the robot's posture to construct a feature map.

[0093] Step S14: Acquire historical robot postures at different times, optimize each of the historical robot postures, and adjust the grid map and the feature map according to the corresponding optimized robot posture.

[0094] In this embodiment, after obtaining the robot pose and constructing the grid map and feature map, the present application can further adjust the grid map and feature map by optimizing the robot pose. Specifically, the constraints between key nodes are calculated according to the robot pose; the constraints between key nodes and submaps are determined according to the robot pose and the local submap; the constraints between submaps are determined according to the local submap; the node constraint residuals are determined based on the key node constraints and the robot pose; the constraint residuals between key nodes and submaps are determined by the robot pose, the local submap, and the constraints between key nodes and submaps; the constraint residuals between submaps are determined according to the local submap and the constraints between submaps; the constraint residuals are optimized by the constraint residuals between nodes, the constraint residuals between key nodes and submaps, and the constraint residuals between submaps to adjust and optimize each historical robot pose; the grid map and feature map are adjusted according to the conversion relationship between the optimized robot pose and the historical robot pose. Among them, the historical robot pose is the robot pose at different times, and there will be a conversion relationship between different poses, such as rotation and translation. When adjusting the pose by optimizing the residual, we perform an iterative optimization solution. Simply put, the goal is to continuously reduce the residuals between constraints. For example, at a certain moment, the robot sees feature A and obtains constraint C1. Some time later, the robot moves near feature A again and obtains constraint C2. (Robot positioning is an estimation process, and the error in positioning pose increases over time.) There will be an error between constraints C1 and C2, but feature A remains unchanged. Therefore, by optimizing the residuals between the constraints, the error in the robot's positioning pose will be reduced, thereby adjusting the robot's pose.

[0095] There is a conversion relationship between the poses before and after optimization. Simply put, the pose before optimization is obtained by translation and rotation. Therefore, the grid map and feature map can be adjusted according to the conversion relationship between the optimized robot pose and the historical robot pose.

[0096] To summarize, when performing mapping and positioning, the present application first obtains a two-dimensional lidar point cloud of the current environment, adaptively filters the two-dimensional lidar point cloud to obtain a filtered point cloud, and screens the filtered point cloud to obtain corner points and line points; then the line points are fitted to obtain fitted line segments, and a local sub-map is obtained according to the historical map. Based on the corner points, the fitted line segments and the point cloud features of each area of ​​the current environment, the local sub-map and the filtered point cloud are matched with local correlation features to obtain a rough posture of the robot; the local sub-map includes a grid sub-map and a feature sub-map; then, feature constraints are determined based on the rough posture of the robot, the posture of the robot is obtained through the feature constraints, and a grid map and a feature map are constructed according to the posture of the robot; finally, the historical robot postures at different times are obtained, each of the historical robot postures is optimized, and the grid map and the feature map are adjusted according to the corresponding optimized robot posture. It can be seen that this application adaptively filters the lidar point cloud, screens the point cloud, and obtains corner points and line points; uses point cloud features and environmental map features to perform feature matching and constraint optimization precision matching, calculates the robot pose, and establishes a grid map and feature map. Finally, it calculates the constraints, optimizes the pose according to the constraints, and adjusts the grid map and feature map. In this way, by providing more accurate and reliable pose information, this application can improve the stability of the mapping algorithm, solve the problems of unstable and biased robot mapping, ensure the quality of mapping, and be more suitable for actual application scenarios. At the same time, it provides more accurate maps and positioning, which can meet the needs of precise docking scenarios.

[0097] Based on the previous embodiment, the present application obtains historical robot postures at different times, optimizes each of the historical robot postures, and adjusts the grid map and the feature map based on the corresponding optimized robot postures. Next, the process of determining the grid map and the feature map will be described in detail.

[0098] See also Figure 12 As shown, the embodiment of the present invention discloses a specific robot mapping and positioning method, including:

[0099] Step S21: Calculate constraints between key nodes based on the robot posture, determine constraints between key nodes and submaps based on the robot posture and the local submap, and determine constraints between submaps based on the local submap.

[0100] This application calculates loop constraints Among them, Figure 13 As shown, there are three key nodes, each node has a posture, these postures can be converted to each other by the rotation and translation matrix, that is, the constraints between key nodes ;like Figure 14As shown in the figure, the square represents the grid map or feature map, the hexagon represents a key node, the circle connected to the solid line represents the laser point cloud, and the point connected to the dotted line represents the submap origin. The key node and the submap origin can be converted by the rotation and translation matrix, that is, the key node and the local submap constraint: ;like Figure 15 As shown, there are two dotted boxes, each representing a submap, where the dot represents the submap origin. The overlapping part indicates that the two maps have the same part. The origins of the two maps can be converted by the rotation and translation matrix, that is, the constraint between the submaps: .

[0101] in, Represents the calculation of the inter-node constraint function (a function that obtains constraints through matrix calculation, which obtains the corresponding inter-node constraints by multiplying a node pose matrix by the inverse of another node pose matrix), n2 represents the number of key nodes, Represents the constraint function between the computation node and the submap (a function that obtains constraints through matrix calculation, which obtains the corresponding constraint between the node and the submap by multiplying a node pose matrix by the inverse of the submap origin pose matrix). n3 represents the number of submaps. Represents a function for calculating constraints between submaps (a function that obtains constraints through matrix calculation, which obtains the corresponding constraints between submaps by multiplying the origin pose matrix of one submap by the inverse of the origin pose matrix of another submap). The above constraint calculation functions are all functions that obtain constraints through matrix calculation. is the i-th robot pose; is the jth local submap.

[0102] Step S22: Determine the inter-node constraint residuals based on the inter-key node constraints and the robot pose, determine the inter-key node and sub-map constraint residuals through the robot pose, the local sub-map, and the constraints between the key nodes and the sub-map, and determine the inter-sub-map constraint residuals based on the local sub-map and the inter-sub-map constraints.

[0103] In this embodiment, the present application determines the inter-node constraint residual based on the key inter-node constraints and the robot posture :

[0104] ;

[0105] Determine the constraint residual between key nodes and submaps through the robot pose, local submap, and constraints between key nodes and submaps :

[0106] ;

[0107] Determine inter-submap constraint residuals based on local submaps and inter-submap constraints :

[0108] ;

[0109] Among them: represents the residual function for calculating the inter-node constraint (in this function, a node pose matrix is ​​multiplied by the inverse of another node pose matrix, and then the result is multiplied by the inverse of the constraint matrix to get the residual), n4 represents the number of inter-node constraints, Represents the residual function for calculating the constraints between the node and the submap (in this function, a node pose matrix is ​​multiplied by the inverse of the submap origin pose matrix, and then this result is multiplied by the inverse of the constraint matrix to get the residual), n5 represents the number of constraints between the node and the submap, Represents the residual function for calculating the constraints between submaps (in this function, the origin pose matrix of one submap is multiplied by the inverse of the origin pose matrix of another submap, and then this result is multiplied by the inverse of the constraint matrix to obtain the residual), n6 represents the number of constraints between submaps, and the above residual functions are all functions that obtain residuals by calculating Euclidean distance, Manhattan distance, etc.

[0110] Step S23: performing constraint residual optimization through inter-node constraint residuals, constraint residuals between key nodes and sub-maps, and constraint residuals between sub-maps to adjust and optimize each historical robot pose.

[0111] In this embodiment, the present application adjusts the node pose by optimizing the residual, specifically by optimizing the constraint residuals between nodes, the constraint residuals between key nodes and submaps, and the constraint residuals between submaps, so as to adjust and optimize the poses of each historical robot.

[0112] ;

[0113] ;

[0114] in, Represents the residual optimization function (provided by the nonlinear optimization library, which continuously iteratively reduces the residual (cost) through numerical optimization methods (Gauss-Newton method, gradient descent method), such as the AutoDiffCostFunction function in ceres), represents the pose adjustment function (a function that adjusts the pose through matrix calculation, such as multiplying the current pose matrix by the inverse of the pose matrix to be adjusted to obtain the adjusted pose matrix, provided by the Eigen matrix library, and achieving pose adjustment by rotating and translating the matrix); RE is the residual after optimization; The robot pose after optimization.

[0115] When adjusting the pose by optimizing the residual, we perform an iterative optimization solution. Simply put, the goal is to continuously reduce the residuals between constraints. For example, at a certain moment, the robot sees feature A and obtains constraint C1. Some time later, the robot moves near feature A again and obtains constraint C2. (Robot positioning is an estimation process, and the error in positioning pose increases over time.) There will be an error between constraints C1 and C2, but feature A remains unchanged. Therefore, by optimizing the residuals between the constraints, the error in the robot's positioning pose will be reduced, thereby adjusting the robot's pose.

[0116] Step S24: Adjust the grid map and the feature map according to the conversion relationship between the optimized robot posture and the historical robot posture.

[0117] In this embodiment, there is a conversion relationship between the postures before and after optimization. Simply put, the posture before optimization is obtained by translation and rotation to obtain the optimized posture. Therefore, the grid map and feature map can be adjusted according to the conversion relationship between the optimized robot posture and the historical robot posture.

[0118] In this way, this application further adjusts the grid map and feature map by constraining the optimized posture, thereby improving the stability of mapping and positioning, and ensuring the quality and accuracy of mapping and positioning. It can solve the problems of mapping failure or instability and deviation, and better meet the needs of complex scene applications.

[0119] See also Figure 16 As shown, an embodiment of the present invention discloses a robot mapping and positioning device, comprising:

[0120] A corner point and line point acquisition module 11 is used to acquire a two-dimensional laser radar point cloud of the current environment, perform adaptive filtering on the two-dimensional laser radar point cloud to obtain a filtered point cloud, and filter the filtered point cloud to obtain corner points and line points;

[0121] The rough pose acquisition module 12 is configured to fit the line points to obtain fitted line segments, obtain a local submap based on the historical map, and perform local correlation feature matching between the local submap and the filtered point cloud based on the corner points, the fitted line segments, and the point cloud features of each area of ​​the current environment to obtain the rough pose of the robot; the local submap includes a grid submap and a feature submap;

[0122] A map construction module 13 is configured to determine feature constraints based on the rough posture of the robot, obtain the posture of the robot through the feature constraints, and construct a grid map and a feature map according to the posture of the robot;

[0123] The map determination module 14 is configured to obtain historical robot postures at different times, optimize each of the historical robot postures, and adjust the grid map and the feature map according to the corresponding optimized robot postures.

[0124] To summarize, when performing mapping and positioning, the present application first obtains a two-dimensional lidar point cloud of the current environment, adaptively filters the two-dimensional lidar point cloud to obtain a filtered point cloud, and screens the filtered point cloud to obtain corner points and line points; then the line points are fitted to obtain fitted line segments, and a local sub-map is obtained according to the historical map. Based on the corner points, the fitted line segments and the point cloud features of each area of ​​the current environment, the local sub-map and the filtered point cloud are matched with local correlation features to obtain a rough posture of the robot; the local sub-map includes a grid sub-map and a feature sub-map; then, feature constraints are determined based on the rough posture of the robot, the posture of the robot is obtained through the feature constraints, and a grid map and a feature map are constructed according to the posture of the robot; finally, the historical robot postures at different times are obtained, each of the historical robot postures is optimized, and the grid map and the feature map are adjusted according to the corresponding optimized robot posture. It can be seen that this application adaptively filters the lidar point cloud, screens the point cloud, and obtains corner points and line points; uses point cloud features and environmental map features to perform feature matching and constraint optimization precision matching, calculates the robot pose, and establishes a grid map and feature map. Finally, it calculates the constraints, optimizes the pose according to the constraints, and adjusts the grid map and feature map. In this way, by providing more accurate and reliable pose information, this application can improve the stability of the mapping algorithm, solve the problems of unstable and biased robot mapping, ensure the quality of mapping, and be more suitable for actual application scenarios. At the same time, it provides more accurate maps and positioning, which can meet the needs of precise docking scenarios.

[0125] In some specific embodiments, the corner point and line point acquisition module 11 may specifically include:

[0126] a point cloud segmentation unit, configured to segment the two-dimensional lidar point cloud according to a grid based on a current first resolution to obtain a segmented point cloud;

[0127] A jump unit is used to calculate the center of all divided point clouds in each grid, adjust the current first resolution to obtain a new current first resolution, and jump again to the step of dividing the two-dimensional lidar point cloud according to the grid based on the current first resolution, until the number of jumps meets the preset conditions and the filtered point cloud is determined according to the center of the divided point cloud.

[0128] In some specific embodiments, the corner point and line point acquisition module 11 may specifically include:

[0129] A corner point and line point acquisition unit is used to calculate the smoothness of the filtered point cloud, and filter the filtered point cloud according to the smoothness and a target threshold to obtain corner points and line points; wherein, if the number of the filtered point cloud is less than the target threshold, the filtered point cloud is determined as the corner point.

[0130] In some specific embodiments, the rough pose acquisition module 12 may specifically include:

[0131] A region division unit, configured to divide the line points into regions according to a second resolution to obtain a plurality of divided regions;

[0132] a line point clustering unit, configured to cluster the line points in each of the divided areas to obtain a plurality of clustered line points;

[0133] The line segment fitting unit is used to perform line segment fitting on each of the clustered line points in each of the divided areas to obtain a fitting line segment.

[0134] In some specific embodiments, the rough pose acquisition module 12 may specifically include:

[0135] a first matching degree calculation unit, configured to determine a local search space and calculate a first matching degree between the filtered point cloud and the grid submap in the local search space;

[0136] A second matching degree calculation unit is used to calculate a second matching degree between the corner points, the fitting line segments and the point cloud features of each area of ​​the current environment in the local search space and the feature submap;

[0137] A rough pose acquisition unit is configured to determine a target matching degree according to the first matching degree and the second matching degree, and acquire a rough pose of the robot corresponding to the target matching degree.

[0138] In some specific embodiments, the map construction module 13 may specifically include:

[0139] a feature constraint calculation unit, configured to calculate the feature constraints based on the rough pose of the robot, the filtered point cloud, the grid submap, the feature submap, and the corner points, the fitted line segments, and point cloud features of each area of ​​the current environment;

[0140] a cost function calculation unit, configured to calculate a cost function based on the feature constraints, the rough pose of the robot, the filtered point cloud, the grid submap, the feature submap, the corner points, the fitted line segments, and point cloud features of each area of ​​the current environment;

[0141] A robot posture acquisition unit is used to acquire the robot posture based on the cost function and the rough posture of the robot.

[0142] In some specific embodiments, the map determination module 14 may specifically include:

[0143] A first constraint calculation unit, configured to calculate constraints between key nodes according to the robot posture;

[0144] A second constraint determination unit, configured to determine constraints between key nodes and submaps based on the robot posture and the local submap;

[0145] a third constraint determining unit, configured to determine inter-submap constraints based on the local submap;

[0146] a first residual determination unit, configured to determine an inter-node constraint residual based on the key inter-node constraint and the robot pose;

[0147] a second residual determination unit, configured to determine the residual of the constraint between the key node and the submap through the robot posture, the local submap, and the constraint between the key node and the submap;

[0148] a third residual determining unit, configured to determine an inter-submap constraint residual according to the local submap and the inter-submap constraint;

[0149] A posture adjustment unit, configured to perform constraint residual optimization based on the inter-node constraint residual, the constraint residual between the key node and the sub-map, and the inter-sub-map constraint residual, so as to adjust and optimize each of the historical robot postures;

[0150] A map determination unit is used to adjust the grid map and the feature map according to the conversion relationship between the optimized robot posture and the historical robot posture.

[0151] Furthermore, the embodiment of the present application also discloses an electronic device, Figure 17 This is a structural diagram of an electronic device 20 according to an exemplary embodiment. The content in the diagram should not be considered as any limitation to the scope of application of the present application.

[0152] Figure 17This is a schematic diagram of the structure of an electronic device 20 provided in an embodiment of the present application. The electronic device 20 may specifically include: at least one processor 21, at least one memory 22, a power supply 23, a communication interface 24, an input / output interface 25, and a communication bus 26. The memory 22 is used to store a computer program, which is loaded and executed by the processor 21 to implement the relevant steps of the robot mapping and positioning method disclosed in any of the aforementioned embodiments. Furthermore, the electronic device 20 in this embodiment may specifically be an electronic computer.

[0153] In this embodiment, the power supply 23 is used to provide operating voltage for each hardware device on the electronic device 20; the communication interface 24 can create a data transmission channel between the electronic device 20 and the external device. The communication protocol it follows is any communication protocol that can be applied to the technical solution of this application and is not specifically limited here; the input and output interface 25 is used to obtain external input data or output data to the outside world. Its specific interface type can be selected according to specific application needs and is not specifically limited here.

[0154] In addition, the memory 22 as a carrier for resource storage can be a read-only memory, random access memory, disk or optical disk, etc. The resources stored thereon can include an operating system 221, a computer program 222, etc., and the storage method can be temporary storage or permanent storage.

[0155] The operating system 221 is used to manage and control the hardware devices on the electronic device 20 and the computer program 222. It can be Windows Server, Netware, Unix, Linux, etc. In addition to including computer programs capable of implementing the robot mapping and positioning method performed by the electronic device 20 as disclosed in any of the aforementioned embodiments, the computer program 222 can further include computer programs capable of performing other specific tasks.

[0156] Furthermore, this application also discloses a computer-readable storage medium for storing a computer program; wherein, when executed by a processor, the computer program implements the aforementioned robot mapping and localization method. The specific steps of this method can be referred to the corresponding contents disclosed in the aforementioned embodiments and will not be repeated here.

[0157] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from the other embodiments. Reference can be made to the descriptions of the identical or similar parts between the various embodiments. For the devices disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple, and the relevant parts can be referred to the descriptions of the methods.

[0158] Professionals may further appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the above description has generally described the components and steps of each example according to their functions. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professionals and technicians may use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0159] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein may be implemented directly using hardware, a software module executed by a processor, or a combination of the two. The software module may be placed in random access memory (RAM), internal memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, a hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.

[0160] Finally, it should be noted that, in this document, relational terms such as first and second, etc., are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or device comprising a series of elements includes not only those elements, but also other elements not explicitly listed, or elements inherent to such process, method, article, or device. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of additional identical elements in the process, method, article, or device comprising the element.

[0161] The above is a detailed introduction to the technical solution provided by the present application. Specific examples are used herein to illustrate the principles and implementation methods of the present application. The description of the above embodiments is only used to help understand the method of the present application and its core idea. At the same time, for those skilled in the art, according to the ideas of the present application, there may be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as a limitation on the present application.

Claims

1. A robot mapping and positioning method, characterized in that: include: Obtaining a two-dimensional laser radar point cloud of the current environment, performing adaptive filtering on the two-dimensional laser radar point cloud to obtain a filtered point cloud, and screening the filtered point cloud to obtain corner points and line points; Fitting the line points to obtain fitted line segments, obtaining a local submap based on the historical map, and performing local correlation feature matching on the local submap and the filtered point cloud based on the corner points, the fitted line segments, and the point cloud features of each area of ​​the current environment to obtain a rough pose of the robot; the local submap includes a grid submap and a feature submap; Determine feature constraints based on the rough posture of the robot, obtain the posture of the robot through the feature constraints, and construct a grid map and a feature map according to the posture of the robot; the feature constraints include point-to-point distance constraints, point-to-line distance constraints, line-to-line distance constraints, and point cloud grid map constraints; Obtaining historical robot postures at different times, optimizing each of the historical robot postures, and adjusting the grid map and the feature map according to the corresponding optimized robot posture; The step of adaptively filtering the two-dimensional laser radar point cloud to obtain a filtered point cloud includes: Dividing the two-dimensional lidar point cloud according to a grid based on the current first resolution to obtain a divided point cloud; Calculating the centers of all divided point clouds in each grid, adjusting the current first resolution to obtain a new current first resolution, and re-jumping to the step of dividing the two-dimensional lidar point cloud according to the grid based on the current first resolution, until the number of jumps meets a preset condition, determining the filtered point cloud according to the centers of the divided point clouds; Optimizing each of the historical robot poses and adjusting the grid map and the feature map according to the corresponding optimized robot poses includes: Calculating constraints between key nodes based on the robot posture; Determining constraints between key nodes and submaps based on the robot posture and the local submap; determining inter-submap constraints based on the local submap; Determining inter-node constraint residuals based on the key inter-node constraints and the robot pose; Determine the constraint residual between the key node and the submap based on the robot pose, the local submap, and the constraint between the key node and the submap; Determining an inter-submap constraint residual based on the local submap and the inter-submap constraint; Performing constraint residual optimization through the inter-node constraint residual, the inter-key node and sub-map constraint residual, and the inter-sub-map constraint residual to adjust and optimize each of the historical robot poses; The grid map and the feature map are adjusted according to a conversion relationship between the optimized robot posture and the historical robot posture.

2. The robot mapping and positioning method according to claim 1, characterized in that: The filtering of the filtered point cloud to obtain corner points and line points includes: Calculating the smoothness of the filtered point cloud, and screening the filtered point cloud according to the smoothness and a target threshold to obtain corner points and line points; wherein, if the number of the filtered point cloud is less than the target threshold, determining the filtered point cloud as the corner point.

3. The robot mapping and positioning method according to claim 1, characterized in that: The step of fitting the line points to obtain a fitted line segment includes: Dividing the line points into regions according to the second resolution to obtain a plurality of divided regions; Clustering the line points in each of the divided areas to obtain a plurality of clustered line points; Line segment fitting is performed on each of the clustered line points in each of the divided areas to obtain a fitting line segment.

4. The robot mapping and positioning method according to claim 1, characterized in that: The obtaining of a rough pose of the robot by performing local correlation feature matching based on the corner points, the fitting line segments, the point cloud features of each area of ​​the current environment, the local submap, and the filtered point cloud includes: Determining a local search space, and calculating a first matching degree between the filtered point cloud and the grid submap in the local search space; Calculating a second matching degree between the corner points, the fitting line segments, and the point cloud features of each area of ​​the current environment in the local search space and the feature submap; A target matching degree is determined according to the first matching degree and the second matching degree, and a rough position pose of the robot corresponding to the target matching degree is obtained.

5. The robot mapping and positioning method according to claim 1, characterized in that: Determining feature constraints based on the rough pose of the robot and obtaining the pose of the robot through the feature constraints includes: Calculating the feature constraints based on the rough pose of the robot, the filtered point cloud, the grid submap, the feature submap, and the corner points, the fitted line segments, and the point cloud features of each area of ​​the current environment; Calculating a cost function based on the feature constraints, the rough pose of the robot, the filtered point cloud, the grid submap, the feature submap, the corner points, the fitted line segments, and point cloud features of each area of ​​the current environment; The robot pose is obtained based on the cost function and the rough pose of the robot.

6. A robot mapping and positioning device, characterized in that: include: A corner point and line point acquisition module is used to obtain a two-dimensional laser radar point cloud of the current environment, perform adaptive filtering on the two-dimensional laser radar point cloud to obtain a filtered point cloud, and filter the filtered point cloud to obtain corner points and line points; A rough pose acquisition module is configured to fit the line points to obtain fitted line segments, obtain a local submap based on the historical map, and perform local correlation feature matching between the local submap and the filtered point cloud based on the corner points, the fitted line segments, and the point cloud features of each area of ​​the current environment to obtain the rough pose of the robot; the local submap includes a grid submap and a feature submap; a map construction module, configured to determine feature constraints based on the rough pose of the robot, obtain the pose of the robot through the feature constraints, and construct a grid map and a feature map according to the pose of the robot; the feature constraints include point-to-point distance constraints, point-to-line distance constraints, line-to-line distance constraints, and point cloud grid map constraints; a map determination module, configured to obtain historical robot postures at different times, optimize each of the historical robot postures, and adjust the grid map and the feature map according to the corresponding optimized robot postures; The corner point and line point acquisition module includes: a point cloud segmentation unit, configured to segment the two-dimensional lidar point cloud according to a grid based on a current first resolution to obtain a segmented point cloud; a jump unit, configured to calculate the centers of all divided point clouds in each grid, adjust the current first resolution to obtain a new current first resolution, and jump again to the step of dividing the two-dimensional lidar point cloud according to the grid based on the current first resolution, until the number of jumps meets a preset condition, and determine the filtered point cloud according to the centers of the divided point clouds; The map determination module includes: A first constraint calculation unit, configured to calculate constraints between key nodes according to the robot posture; A second constraint determination unit, configured to determine constraints between key nodes and submaps based on the robot posture and the local submap; a third constraint determining unit, configured to determine inter-submap constraints based on the local submap; a first residual determination unit, configured to determine an inter-node constraint residual based on the key inter-node constraint and the robot pose; a second residual determination unit, configured to determine the residual of the constraint between the key node and the submap through the robot posture, the local submap, and the constraint between the key node and the submap; a third residual determining unit, configured to determine an inter-submap constraint residual according to the local submap and the inter-submap constraint; A posture adjustment unit, configured to perform constraint residual optimization based on the inter-node constraint residual, the constraint residual between the key node and the sub-map, and the inter-sub-map constraint residual, so as to adjust and optimize each of the historical robot postures; A map determination unit is used to adjust the grid map and the feature map according to the conversion relationship between the optimized robot posture and the historical robot posture.

7. An electronic device, characterized in that: include: Memory, used to store computer programs; A processor, configured to execute the computer program to implement the robot mapping and positioning method according to any one of claims 1 to 6.

8. A computer-readable storage medium, characterized in that Used to store a computer program; wherein, when the computer program is executed by a processor, the robot mapping and positioning method according to any one of claims 1 to 6 is implemented.

Images