Robot posture estimation method, device and robot under non-flat terrain

Abstract

The present invention belongs to the field of robot posture estimation. In order to solve the problem of inaccurate initial posture estimation of the front end, a method, device and robot for robot posture estimation under non-flat terrain are provided. Among them, a method for robot posture estimation under non-flat terrain includes forming a laser radar point cloud with the laser radar data calibrated at the current moment, and then performing feature extraction, forward position adjustment, voxel matching, rotation estimation, translation estimation and conversion fusion processing in sequence to obtain the key frame at the current moment; then aligning the key frames accumulated in the historical time period to the local map to obtain the transformation matrix at the current moment, and optimizing the transformation matrix using the factor graph; using the optimized transformation matrix and the laser radar data calibrated at the current moment to jointly construct a map, output the map and the odometer, and then estimate the robot's motion posture at the next moment, which can more accurately estimate the robot's posture on uneven terrain.

Description

Technical Field

[0001] The present invention belongs to the field of robot posture estimation, and in particular relates to a robot posture estimation method, a device and a robot under non-flat terrain. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] When navigating in uneven terrain, the movement of the robot and the sensors fixed on the robot will inevitably be affected by fluctuations, which increases the difficulty of robot positioning. At present, the LiDAR-based SLAM (Simultaneous Localization and Mapping) framework usually uses the geometric features of the point cloud to recover the motion of the LiDAR, and usually consists of two modules: the front-end LiDAR odometer and the back-end mapping optimization. The front-end provides an initial pose estimate through frame-to-frame registration, and the back-end uses alignment and optimization methods on a global scale to refine the pose estimate and reconstruct the surrounding environment. This approach enables the framework to achieve coarse-to-fine positioning and is commonly used to solve normal scenes such as cities or flat roads.

[0004] In the case of uneven terrain, the robot's body shakes in response to the terrain surface, and the posture component in the vertical direction changes significantly. The robot will produce non-negligible positioning drift, resulting in distortion or tilt of the reconstructed map, which directly leads to a reduction in the consistent set in the frame-to-frame matching process and inaccurate initial posture estimation of the front end. Summary of the invention

[0005] In order to solve the above technical problems, the present invention provides a robot posture estimation method, device and robot under uneven terrain, which can more accurately estimate the posture of the robot on uneven terrain.

[0006] In order to achieve the above object, the present invention adopts the following technical solution:

[0007] A first aspect of the present invention provides a method for estimating a robot posture under non-flat terrain.

[0008] In one or more embodiments, a method for estimating a robot posture under a non-flat terrain is provided, comprising:

[0009] Obtain continuous multi-frame lidar data around the robot's current motion posture and perform calibration;

[0010] The calibrated lidar data at the current moment is converted into a lidar point cloud, and then feature extraction, forward position adjustment, voxel matching, rotation estimation, translation estimation and conversion fusion processing are performed in sequence to obtain the key frame at the current moment;

[0011] Align the key frames at the current moment and the key frames accumulated in the historical time period to the local map, obtain the transformation matrix at the current moment, and optimize the transformation matrix using a factor graph;

[0012] The optimized transformation matrix and the laser radar data calibrated at the current moment are used to build a map together, and the posture transformation data of the robot during movement is transmitted to the laser radar odometer;

[0013] Based on the map and the odometer, an initial posture of the robot at the next moment is estimated.

[0014] As an embodiment, during the forward position adjustment operation, the position of the robot is estimated in advance based on the timestamp of each lidar data scan.

[0015] As an implementation method, a single index query method is used to perform voxel matching operations on the lidar point cloud.

[0016] As an implementation method, the steps of performing voxel matching operations on a lidar point cloud using a single index query method include: a spatial voxelization step and a matching operation step; the spatial voxelization step is used to generate a voxel set from the corresponding lidar point cloud; and the matching operation step is used to perform index matching based on the voxel set.

[0017] As an implementation method, the spatial voxelization step is:

[0018] Divide the LiDAR point cloud into a number of voxels and calculate the statistical properties of each voxel;

[0019] For each point in the lidar point cloud, first calculate its voxel index. If the index is not in the current voxel set, add it to the current voxel set, initialize the statistical properties of the voxel, and then update the statistical properties of the voxel. Finally, normalize the statistical properties in the voxel and update the current voxel set.

[0020] As an implementation method, the matching operation steps are:

[0021] First, an empty point pair set is initialized, and then the target point cloud is voxelized through the spatial voxelization step to obtain a voxel set;

[0022] Then, for each point in the target point cloud, calculate its voxel index, and check whether the index is in the voxel set and whether the number of points in the voxel is greater than or equal to the preset threshold. If the conditions are met, add the point pair consisting of the point and the mean of the voxel to the point pair set, and finally return the point pair set.

[0023] A second aspect of the present invention provides a robot posture estimation device under non-flat terrain.

[0024] In one or more embodiments, a robot posture estimation device under non-flat terrain includes:

[0025] The laser radar data calibration module is used to obtain continuous multi-frame laser radar data around the robot's current motion posture and perform calibration;

[0026] The key frame extraction module is used to form a lidar point cloud from the lidar data calibrated at the current moment, and then perform feature extraction, forward position adjustment, voxel matching, rotation estimation, translation estimation and conversion fusion processing in sequence to obtain the key frame at the current moment;

[0027] A transformation matrix optimization module is used to align the key frames at the current moment and the key frames accumulated in the historical time period to the local map, obtain the transformation matrix at the current moment, and optimize the transformation matrix using a factor graph;

[0028] The map and odometer output module is used to construct a map using the optimized transformation matrix and the laser radar data calibrated at the current moment, and transmit the posture transformation data of the robot during movement to the laser radar odometer;

[0029] A motion posture estimation module is used to estimate the initial posture of the robot at the next moment based on the map and the odometer.

[0030] As an implementation mode, in the key frame extraction module, a single index query method is used to perform voxel matching operations on the lidar point cloud.

[0031] As an implementation mode, in the key frame extraction module, the step of performing voxel matching operation on the lidar point cloud using a single index query method includes: a spatial voxelization step and a matching operation step; the spatial voxelization step is used to generate a voxel set from the corresponding lidar point cloud; the matching operation step is used to perform index matching based on the voxel set.

[0032] A third aspect of the present invention provides a robot.

[0033] In one or more embodiments, a robot comprising:

[0034] Robot body, laser radar and controller;

[0035] The laser radar and the controller are both mounted on the robot body;

[0036] The laser radar is used to scan continuous multi-frame laser radar data around the robot's current motion posture and transmit it to the controller;

[0037] The controller is configured to execute the steps in the robot posture estimation method under non-flat terrain as described above.

[0038] Compared with the prior art, the present invention has the following beneficial effects:

[0039] The present invention utilizes the lidar point cloud formed by lidar scanning, and performs feature extraction, forward position adjustment, voxel matching, rotation estimation, translation estimation and conversion fusion processing in sequence, thereby realizing the continuous transformation of independent rotation and translation registration, ensuring that the ray centers of the two data frames are roughly aligned, and also combining historical accumulation key frame alignment and factor graph optimization to construct a map and output lidar odometer-related data to form a closed loop, thereby realizing accurate estimation of the initial posture of the robot on uneven terrain at future moments. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0041] Figure 1 is a flow chart of a method for estimating a robot posture under a non-flat terrain according to an embodiment of the present invention;

[0042] Figure 2 is a schematic diagram of robot posture estimation under non-flat terrain according to an embodiment of the present invention;

[0043] Figure 3 is a snapshot of an embodiment of the present invention driving in a rough terrain, recording the postures of any two robots in the same time interval;

[0044] Figure 4 is a rotational registration model of an embodiment of the present invention;

[0045] Figure 5 is the corresponding point alignment of the embodiment of the present invention;

[0046] Figure 6 is a factor graph model of an embodiment of the present invention;

[0047] Figure 7 Schematic diagram of the structure of a robot posture estimation device under non-flat terrain according to an embodiment of the present invention.

[0048] Among them, 1. target point, 2. source point, 3. alignment direction, 4. lidar, 5. multi-layer sphere, 6. 3D Gaussian surface. DETAILED DESCRIPTION

[0049] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0050] It should be noted that the following detailed descriptions are all illustrative and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0051] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0052] In the present invention, the robot may be a vehicle or other movable device (eg, an AGV, etc.).

[0053] When the robot is a vehicle, the robot posture estimation method under non-flat terrain of the embodiment of the present invention can be applied to the autonomous driving scenario. In the context of autonomous driving, positioning is of great significance and is a basic component of safe and efficient navigation, enabling the vehicle to accurately determine its position in the environment.

[0054] Embodiment 1

[0055] Figure 1 is a flow chart of a method for estimating a robot posture under a non-flat terrain according to an embodiment of the present invention. Figure 1 As shown, a robot posture estimation method under non-flat terrain according to an embodiment of the present invention specifically comprises the following steps:

[0056] S101: Acquire continuous multi-frame laser radar data around the robot's current motion posture and perform calibration;

[0057] S102: The calibrated laser radar data at the current moment is formed into a laser radar point cloud, and then feature extraction, forward position adjustment, voxel matching, rotation estimation, translation estimation and conversion fusion processing are performed in sequence to obtain a key frame at the current moment;

[0058] S103: aligning the key frames at the current moment and the key frames accumulated in the historical time period to the local map, obtaining the transformation matrix at the current moment, and optimizing the transformation matrix using a factor graph;

[0059] S104: constructing a map using the optimized transformation matrix and the laser radar data calibrated at the current moment, and transmitting the posture transformation data during the robot's movement to the laser radar odometer;

[0060] S105: Based on the map and the odometer, an initial posture of the robot at the next moment is estimated.

[0061] This embodiment uses the lidar point cloud formed by lidar scanning to perform feature extraction, forward position adjustment, voxel matching, rotation estimation, translation estimation and conversion fusion processing in sequence, thereby realizing the continuous transformation of independent rotation and translation registration, ensuring that the ray centers of the two data frames are roughly aligned, and also combining historical accumulation key frame alignment and factor graph optimization to construct a map and output lidar odometer-related data to form a closed loop, thereby realizing accurate estimation of the robot's initial posture on uneven terrain at future moments.

[0062] Combination Figure 2 , the specific process of the robot posture estimation method under non-flat terrain of this embodiment is given below.

[0063] In step S101, the laser radar data is calibrated to correct the motion distortion of the robot, which helps the front end to approach the rough odometer of the laser radar.

[0064] in, Figure 3 A snapshot of LiDAR data driving in rough terrain is given, and the poses of any two robots are recorded at the same time interval.

[0065] It should be noted here that the lidar data can be calibrated by using Kalman filtering or other state estimation methods to perform motion compensation on the radar data; other existing methods, such as machine learning or statistical methods, can also be used for calibration.

[0066] In step S102, during feature extraction of the LiDAR point cloud, the geometric features of the LiDAR point cloud are identified by smoothness based on the edge and plane characteristics of the LiDAR point cloud.

[0067] After extracting the geometric features of the LiDAR point cloud, the LiDAR translation is quickly estimated using forward position adjustment to achieve a loose decoupling of rotation and translation.

[0068] Specifically, during the forward position adjustment operation, the position of the robot is estimated in advance based on the timestamp of each lidar data scan.

[0069] Among them, the principle of forward position adjustment is:

[0070] Assuming that the robot is always driving on the ground, the robot's wheels are in constant contact with the ground, especially on uneven terrain. Given that the observation frequency of most lidars exceeds 10 , we can assume that the robot moves at a constant speed during the short scanning interval. x Axis and z The axis undergoes position changes. When the scan interval is very short, it can be assumed that these changes are small. In this case, the robot's motion model can be assumed to be uniform motion, so the robot will move as it passes through Position after radar scan It can be obtained by the following formula

[0071]

[0072] in, Indicates The timestamp of the scan; Indicates The timestamp of the scan. Indicates that the robot is passing Position after the first radar scan; Indicates that the robot is passing The position after the first radar scan.

[0073] In scanning The lidar data frame Before arriving, the robot is estimated position, thereby effectively reducing the scanning The lidar data frame and The lidar data frame This process of estimating the position helps to later estimate the rotation and translation independently.

[0074] After the forward position adjustment operation, voxel matching is performed, which effectively determines the correspondence between data frames.

[0075] Before voxel matching, it is assumed that the position of the point in the three-dimensional space is affected by Gaussian noise. This noise has a specific covariance matrix, which reflects the correlation between the intensity and direction of the noise in different directions.

[0076] In order to cope with the vertical discreteness in LiDAR observations, a spatial sparse voxel map is developed. In this map, the joint Gaussian distribution information of the points is recorded inside each voxel. Each voxel stores the number of points contained in it, the spatial average position of these points (i.e., the spatial centroid), and the average covariance matrix of the positions of these points. This helps to effectively describe and process the spatial distribution and noise characteristics of the data in the voxel map.

[0077] In the specific implementation process, a single index query method is used to perform voxel matching operations on the lidar point cloud. Specifically, the target point cloud The voxel set is formed by spatial voxelization When the voxel index of the source point cloud is in the index set , which means that the point pairs match and the corresponding points correspond to the set Will be updated. The spatial voxelization and matching operations are independent for each voxel, so they allow efficient and convenient parallel processing to quickly build and match voxels.

[0078] Among them, the steps of performing voxel matching operations on the lidar point cloud using a single index query method include: a spatial voxelization step and a matching operation step; the spatial voxelization step is used to generate a voxel set from the corresponding lidar point cloud; and the matching operation step is used to perform index matching based on the voxel set.

[0079] In the specific implementation process, the spatial voxelization step is:

[0080] LiDAR point cloud Divide into a number of voxels (i.e., 3D grid cells) and calculate the statistical properties of each voxel (such as the number of points, the mean and covariance of the points);

[0081] For LiDAR point clouds For each point in , first calculate its voxel index. If the index is not in the current voxel set, add it to the current voxel set and initialize the statistical properties of the voxel (such as the number of points, the mean and covariance of the points). Then update the statistical properties of the voxel, and finally normalize the statistical properties in the voxel and update the current voxel set.

[0082] The matching operation steps are:

[0083] First, initialize an empty point pair set , and then the target point cloud After the spatial voxelization step, voxelization is performed to obtain a voxel set ;

[0084] For the target point cloud For each point in , calculate its voxel index and check whether the index is in the voxel set Is the number of points in the voxel greater than or equal to the preset threshold? , if the condition is met, then the point pair consisting of the point and the mean of the voxel is added to the point pair set Finally, the point pair set is returned .

[0085] The threshold here It is preset by those skilled in the art according to actual conditions.

[0086] After the forward position adjustment and voxel matching process, the center-aligned adjacent data frames can estimate the attitude transformation individually. At this time, the lidar rotation registration problem can be approximated as the alignment problem of multi-layer spherical points, such as Figure 4 As shown in Figure 2, two pairs of source points 2 and target points 1 are located on different spherical layers, where the target point is the expected position of the matching voxel. Intuitively, rotational alignment can be regarded as the process of the source point sliding between multiple spherical layers and aligning to the target point. Figure 4 In the figure, 3 is the alignment direction, 4 is the lidar, 5 is the multi-layer sphere, and 6 is the 3D Gaussian surface.

[0087] The specific implementation process of rotation estimation is given below:

[0088] For a pair of source and target points ( ), they can be approximately regarded as being on the same sphere with the same center O. Rotational registration is achieved by minimizing the spherical angle difference of all point pairs, which can be expressed as: ;in, is the rotation matrix. represents the spherical angle between corresponding points, Indicates finding a rotation matrix , so that the objective function Reached minimum value.

[0089] In order to simplify the calculation, the following registration method using degenerate spherical Gaussian distribution and distance metric is as follows Figure 5 As shown, assuming and are located on the same registration sphere, and is through (Source After the rotation matrix The spherical tangent plane of the transformed point position vector) is as its normal vector. 'It is from arrive Projection.

[0090] ;

[0091] , ;

[0092] in, ,yes and The distance between the projections is then calculated using the general point-plane relationship :

[0093] ;

[0094] in, express Without loss of generality, the objective function is constructed using the Mahalanobis distance form: ,in, represents the Mahalanobis distance, is the covariance matrix. is the inverse sine trigonometric function. In addition, the surfel approximation method is used to approximate the spatial manifold of the spherical Gaussian distribution. and Respectively associate the spatial Gaussian distribution, through , to obtain The covariance of ,and They are and The covariance matrix of . Then, Perform singular value decomposition and reconstruct covariance for

[0095] ;

[0096] in, is the maximum eigenvalue, is an infinitesimal quantity used to maintain the positive definiteness of the matrix and ensure that there is a certain covariance in all directions. and are two matrices obtained by the singular value decomposition operation. From the manifold point of view, this operation approximates the spherical Gaussian distribution as a circular surface.

[0097] The specific implementation process of translation estimation and conversion fusion is given below:

[0098] The state front propagation process relies on the last translation to provide an initial guess for center alignment and rotation estimation towards adjacent frames. For the robot, it is assumed to maintain continuous and normal motion while driving on rough terrain during the observation interval of adjacent sensor data. In addition, two adjacent point clouds have no obvious pose difference through rotation registration. Using the principle of continuous and uniform motion, a continuous trajectory translation estimation method with continuous state front propagation is given below.

[0099] Using symbols and Represent the transformation matrices without translation and without rotation respectively. With respect to the state forward propagation and rotation estimation, an initial translation guess can be obtained Rotation Transformation Translation Estimation The overall translation estimation is further optimized by the developed continuous trajectory method. The overall translation estimation is modeled as the following optimization process:

[0100] ;

[0101] in, is the optimized shift estimate;

[0102] The objective function consists of two components: is the point-to-distribution loss function, and is the loss function for the continuous trajectory constraint. yes Adjustable weight.

[0103] is an optimization function, which means finding the function that minimizes the overall function. In order to facilitate iterative solution, the Mahalanobis distance is used represents the loss function. For each corresponding ,have:

[0104] ; ;

[0105] in, It is a voxel , the number of interior points.

[0106] Since the robot has short-term speed invariance when walking on the terrain, for each point of It can be written as: ;

[0107] ;

[0108] in, is the zero vector used to extract the last moment of translation, represents the outer product. In addition, and are two adjacent translation vectors, and their right superscripts represent time series. yes The tensor product of , means that corresponding points with larger translations are penalized more. is used to solve the geometric alignment of sensor data, and The robot is forced to maintain continuous and uniform motion as much as possible. The final translation transformation of the two sensor frames :pass get.

[0109] In steps S103 and S104, the robot backend builds submaps by aggregating keyframes. These are used for scan-to-submap alignment, and factor graphs are used to further optimize the lidar global pose and point cloud map. Based on the relatively accurate transformation estimates output by the frontend, the backend mapping becomes more efficient in optimizing these estimates, and high-quality global pose and environment maps can be quickly generated.

[0110] Mapping includes two optimization steps: scan-to-subgraph optimization (i.e., step S103) and global pose optimization (i.e., step S104). Among them, the scan-to-subgraph optimization adopts a more detailed registration method to align the latest data frame with the accumulated local subgraph to obtain a more accurate lidar odometer. On a global scale, a global factor graph is gradually constructed through the accumulated keyframes, and the pose of each historical keyframe is fine-tuned to minimize the overall historical error.

[0111] In S103, in order to achieve a more accurate laser radar odometer, the global map is composed of edge maps and plane maps accumulated from the feature points extracted from the original data frame. After these feature points F are extracted, they are converted to world coordinates W to obtain a data frame of converted coordinates. Expressed as ;in, and The edge feature point set and plane feature point set of the transformed coordinates are represented respectively. Then a specific data frame is selected as the key frame F to be inserted into the global map. The key frame is selected based on the predetermined thresholds of distance, angle deviation and time interval to extract a fixed number of k The nearest neighbor data frame to form the subgraph .

[0112] in, ;

[0113] ;

[0114] ;

[0115] in, and The corresponding edge sub-graph and plane sub-graph are respectively. Scan-to-sub-graph optimization uses point-to-line and point-to-surface constraints to achieve accurate registration from the current scan to the sub-graph, which can be expressed as:

[0116] ;

[0117] in, and They are the loss functions of edge features and plane features respectively. and yes and a specific point in . It is the index used to traverse all point pairs (or data pairs); the target variable is the transformation matrix of the current scan to the sub-image in the world coordinates ; is an optimization function, which means finding the function that minimizes the overall function. .

[0118] in, Describes the current edge characteristics and The distance between the inner global edges, and Quantify the current plane features and The distances between features within a global plane. They can be calculated as follows:

[0119] ;

[0120] ;

[0121] ;

[0122] ;

[0123] in, and represents the absolute value and L2 norm. and They are located in and The feature points in . is the direction vector of the line feature, and is the normal vector of the plane feature; is the transposed vector of the normal vector of the plane feature; represents a point in the edge graph, and correspond; for In addition, and is the weight parameter of a specific residual, focusing on features with a small distance to the corresponding edge or plane. This process of scanning into sub-graphs is performed less frequently to make a better trade-off between real-time performance and accuracy.

[0124] The entire optimization on a global scale can be modeled as a maximum a posteriori problem. Factor graph models are often used to represent the nonlinear optimization structure of SLAM and are conveniently solved by incremental smoothing and mapping algorithms. The entire factor graph consists of nodes and edges with factors. Each node stores the state of the lidar at any time. Define the first Frame LiDAR Status for: ;in, and is a translation vector and a rotation matrix, representing the position and attitude relative to the world origin. In addition, With keyframes association.

[0125] In this invention, two basic factors are defined: odometry factor and closed-loop factor. The odometry factor is applicable to nodes that are continuous in time and close in physical location. The closed-loop factor is used in the case where there is temporal discontinuity but the spatial location shows similarity. The entire factor graph model consists of Figure 6 Description. Figure 6 In the sub-graph Superscript Indicates Iterations.

[0126] The odometry factor constrains the transformation relationship between adjacent states and keyframes, similar to a Markov chain. It can reject abnormal states estimated by the scan-to-subgraph process and smooth the entire motion trajectory. The transformation associated with two adjacent nodes is given by the combined transformation of the front-end and the scan-to-subgraph process. The accumulated error is non-negligible in long-term and large-scale scenarios. However, this problem can be effectively solved by closed-loop techniques.

[0127] Next, we use an efficient closed-loop method based on Euclidean distance search. Specifically, we set a search box of stable size centered on the current lidar state. , is searched in the box The states and keyframes of the enclosure are represented as These keyframes , forming a search queue, indexed by insertion time. During the LiDAR motion, a loop closure detection thread is executed to check each keyframe With the current keyframe Once similarities are found and , we use fast registration methods, such as the GICP (Generalized Iterative Closest Point) algorithm, to calculate the transformation between them .

[0128] At the same time, a closed loop is established , expressed as: ;in, Represents the state of the lidar in the current frame; this closed-loop pair will be converted into a closed-loop factor inserted into the factor graph to establish constraints between nodes inserted at different time instances. By using the factor graph for global optimization, the trajectory of the lidar can be refined, effectively eliminating the accumulated error and prompting all nodes to make adaptive adjustments in the pursuit of minimizing the global error.

[0129] In order to verify the robot posture estimation effect under non-flat terrain of the present invention, the present invention is compared with the HDL Graph SLAM algorithm (High Definition LiDAR Graph Simultaneous Localization and Mapping, a graph optimization simultaneous positioning and mapping algorithm based on high-definition laser radar) and the LOAM algorithm (Lidar Odometry and Mapping, a laser radar odometer and map construction algorithm). The results of the comparative experiment are shown in Table 1.

[0130] Table 1 Root mean square error between the predicted values ​​and the true values ​​of different methods in different experimental scenarios (translation error (meter) / rotation error (radian))

[0131]

[0132] In Table 1, both the HDL Graph SLAM algorithm and the LOAM algorithm do not independently implement the two operations of rotation and translation registration. The present invention adopts continuous transformation of independent rotation and translation registration to ensure that the ray centers of the two data frames are roughly aligned. Compared with the existing HDL Graph SLAM algorithm and LOAM algorithm, the error is smaller and the accuracy is higher.

[0133] Embodiment 2

[0134] Figure 7 Schematic diagram of the structure of a robot posture estimation device under non-flat terrain according to an embodiment of the present invention.

[0135] like Figure 7 As shown, an embodiment of the present invention provides a robot posture estimation device under non-flat terrain, which includes:

[0136] A laser radar data calibration module 701 is used to obtain continuous multi-frame laser radar data around the robot's current motion posture and perform calibration;

[0137] The key frame extraction module 702 is used to form a laser radar point cloud from the laser radar data calibrated at the current moment, and then perform feature extraction, forward position adjustment, voxel matching, rotation estimation, translation estimation and conversion fusion processing in sequence to obtain the key frame at the current moment;

[0138] The transformation matrix optimization module 703 is used to align the key frames at the current moment and the key frames accumulated in the historical time period to the local map, obtain the transformation matrix at the current moment, and optimize the transformation matrix using a factor graph;

[0139] A map and odometer output module 704, which is used to construct a map using the optimized transformation matrix and the laser radar data calibrated at the current moment, and transmit the posture transformation data of the robot during movement to the laser radar odometer;

[0140] The motion posture estimation module 705 is used to estimate the initial posture of the robot at the next moment based on the map and the odometer.

[0141] In the specific implementation process, in the key frame extraction module 702, a single index query method is used to perform voxel matching operation on the laser radar point cloud. In the key frame extraction module 702, the step of performing voxel matching operation on the laser radar point cloud using the single index query method includes: a spatial voxelization step and a matching operation step; the spatial voxelization step is used to generate a voxel set from the corresponding laser radar point cloud; the matching operation step is used to perform index matching based on the voxel set.

[0142] It should be noted here that each module in the embodiment of the present invention corresponds one-to-one to each step in the first embodiment, and the specific implementation process is the same, which will not be described in detail here.

[0143] Embodiment 3

[0144] This embodiment provides a robot, which includes:

[0145] Robot body, laser radar and controller;

[0146] The laser radar and the controller are both mounted on the robot body;

[0147] The laser radar is used to scan continuous multi-frame laser radar data around the robot's current motion posture and transmit it to the controller;

[0148] The controller is configured to execute the steps in the robot posture estimation method under non-flat terrain as described in the first embodiment above.

[0149] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A robot posture estimation method under non-flat terrain, characterized in that: include: Obtain continuous multi-frame lidar data around the robot's current motion posture and perform calibration; The calibrated lidar data at the current moment is converted into a lidar point cloud, and then feature extraction, forward position adjustment, voxel matching, rotation estimation, translation estimation and conversion fusion processing are performed in sequence to obtain the key frame at the current moment; Align the key frames at the current moment and the key frames accumulated in the historical time period to the local map, obtain the transformation matrix at the current moment, and optimize the transformation matrix using a factor graph; The optimized transformation matrix and the calibrated lidar data at the current moment are used to build a map together, and the posture transformation data of the robot during movement is transmitted to the lidar odometer; Based on the map and the odometer, an initial posture of the robot at the next moment is estimated.

2. The robot posture estimation method under non-flat terrain as claimed in claim 1, characterized in that: During the forward position adjustment operation, the robot's position is estimated in advance based on the timestamp of each lidar data scan.

3. The robot posture estimation method under non-flat terrain as claimed in claim 1, characterized in that: A single index query method is used to perform voxel matching operations on lidar point clouds.

4. The robot posture estimation method under non-flat terrain as claimed in claim 3, characterized in that: The steps of performing voxel matching operations on the lidar point cloud using a single index query method include: a spatial voxelization step and a matching operation step; the spatial voxelization step is used to generate a voxel set from the corresponding lidar point cloud; and the matching operation step is used to perform index matching based on the voxel set.

5. The robot posture estimation method under non-flat terrain as claimed in claim 4, characterized in that: The spatial voxelization steps are: Divide the LiDAR point cloud into a number of voxels and calculate the statistical properties of each voxel; For each point in the lidar point cloud, first calculate its voxel index. If the index is not in the current voxel set, add it to the current voxel set, initialize the statistical properties of the voxel, and then update the statistical properties of the voxel. Finally, normalize the statistical properties in the voxel and update the current voxel set.

6. The robot posture estimation method under non-flat terrain as claimed in claim 4, characterized in that: The matching operation steps are: First, an empty point pair set is initialized, and then the target point cloud is voxelized through the spatial voxelization step to obtain a voxel set; Then, for each point in the target point cloud, calculate its voxel index, and check whether the index is in the voxel set and whether the number of points in the voxel is greater than or equal to the preset threshold. If the conditions are met, add the point pair consisting of the point and the mean of the voxel to the point pair set, and finally return the point pair set.

7. A robot posture estimation device under non-flat terrain, characterized in that: include: The laser radar data calibration module is used to obtain continuous multi-frame laser radar data around the robot's current motion posture and perform calibration; The key frame extraction module is used to form a lidar point cloud from the lidar data calibrated at the current moment, and then perform feature extraction, forward position adjustment, voxel matching, rotation estimation, translation estimation and conversion fusion processing in sequence to obtain the key frame at the current moment; A transformation matrix optimization module is used to align the key frames at the current moment and the key frames accumulated in the historical time period to the local map, obtain the transformation matrix at the current moment, and optimize the transformation matrix using a factor graph; The map and odometer output module is used to construct a map using the optimized transformation matrix and the laser radar data calibrated at the current moment, and transmit the posture transformation data of the robot during movement to the laser radar odometer; A motion posture estimation module is used to estimate the initial posture of the robot at the next moment based on the map and the odometer.

8. The robot posture estimation device under non-flat terrain as claimed in claim 7, characterized in that: In the key frame extraction module, a single index query method is used to perform voxel matching operations on the lidar point cloud.

9. The robot posture estimation device under non-flat terrain as claimed in claim 8, characterized in that: In the key frame extraction module, the step of performing voxel matching operation on the lidar point cloud using a single index query method includes: a spatial voxelization step and a matching operation step; the spatial voxelization step is used to generate a voxel set from the corresponding lidar point cloud; the matching operation step is used to perform index matching based on the voxel set.

10. A robot, characterized in that: include: Robot body, laser radar and controller; The laser radar and the controller are both mounted on the robot body; The laser radar is used to scan continuous multi-frame laser radar data around the robot's current motion posture and transmit it to the controller; The controller is configured to execute the steps in the robot posture estimation method under non-flat terrain as claimed in any one of claims 1 to 6.

Images