Quadruped robot large-scale scene positioning and mapping method based on IMU and laser radar in combination with factor graph

Abstract

The invention discloses a quadruped robot large scene positioning and mapping method based on an IMU and a laser radar in combination with a factor graph, and the method comprises the steps: a data collection step, employing the IMU to collect the inertial measurement data of a quadruped robot, and employing the laser radar to collect point cloud data; a data preprocessing step: performing pre-integration processing on the inertial measurement data of the quadruped robot collected by the IMU to obtain the initial estimation of the pose and speed of the robot; distortion correction, nearest neighbor matching and point cloud registration are carried out on the point cloud data collected by the laser radar; and a factor graph construction step: constructing a factor graph based on the processed IMU data and laser radar data, carrying out optimization solution on the constructed factor graph through a Gaussian-Newton method, and taking the negative logarithm of the joint probability of all factors in the minimum factor graph as a target to obtain the optimal estimation of the pose of the robot. The method can be effectively used for positioning and mapping of the foot-type robot in a large scene.

Description

Technical Field

[0001] The present invention relates to a large - scale scene positioning and mapping method for quadruped robots based on IMU and lidar combined with factor graph, belonging to the research field of positioning and mapping of bionic quadruped robots. Background Technique

[0002] In recent years, with the rapid progress of technology, intelligent robots have become one of the mainstream industries. In the process of realizing intelligent robot navigation, the Simultaneous Localization and Mapping (SLAM) technology plays a crucial role. The continuous innovation of sensor technology has further improved the performance of SLAM technology in terms of map construction accuracy and robustness. According to the different types of sensors, SLAM technology can be simply divided into two categories: visual SLAM and lidar SLAM. Visual SLAM relies on visual sensors to obtain environmental information. Its structure is relatively simple, but it has a large amount of computation, high cost, and is easily affected by lighting conditions. Therefore, it is not suitable for industrial environments such as storage warehouses where lighting changes significantly. In contrast, lidar SLAM technology is more mature, with accurate ranging and little influence from lighting. Compared with cameras, ultrasonic sensors, infrared sensors, etc., lidar has the advantages of strong anti - interference ability, high precision, and wide measurement range, and is more suitable for application in industrial environments.

[0003] Currently, quadruped robots are widely used in large - scale scenarios such as storage warehouses. Compared with traditional mapping methods, robots using SLAM technology can work in complex and dangerous environments, thus reducing the burden on practitioners. By using robots for large - scale scene positioning and mapping, not only can limited resources be more effectively integrated, but also work efficiency can be significantly improved. Mainstream robots are mainly equipped with low - cost and low - frame - rate lidar. However, such lidar will inevitably encounter problems such as motion distortion and mismatching during use.

[0004] Regarding the poor mapping effect caused by the incorrect matching of lidar, Besl et al. proposed the ICP (Iterative Closest Point) method, namely the Iterative Closest Point method. This method is mainly used to solve the registration problem based on free-form surfaces. Through iterative calculation, the optimal matching between the point cloud data to be registered and the reference point cloud data is satisfied under a certain metric criterion. This method has strong versatility and a simple structure and is widely used in various SLAM scenarios. However, the ICP method has high initialization requirements. If the initialization is too poor, the algorithm may fall into a local optimal solution and fail to obtain the globally optimal registration result. To address the above shortcomings, ZHAO proposed an improved ICP (Iterative Closest Point) positioning method. By using point clouds with different densities to match the ICP algorithm with the constructed Euclidean distance subgraph, this method only needs to match and eliminate the pose of part of the point cloud, thereby reducing the actual amount of point cloud matching. While ensuring the positioning accuracy, this method also ensures the real-time performance of positioning. However, the effectiveness of this method may highly depend on specific environments and application scenarios. For example, in a dense urban environment, it may be easier to find enough point cloud features for matching; while in an open warehouse environment, the point cloud may be sparser, resulting in difficult matching. LI proposed a gmapping mapping algorithm that combines odometry information and lidar. By using the relatively high-frequency odometry information to assist the low-frequency lidar in removing its motion distortion, the position of the robot is matched for each frame of lidar data to achieve high-precision environmental map construction. However, the wheeled odometry frequency mentioned in this method is only slightly higher than the radar frequency, resulting in low pose accuracy provided to the robot, which may cause cumulative errors and misalignments in the map during long-term operation or large-scale exploration such as warehousing. PENG proposed a SLAM method based on multi-sensor fusion. This method combines the advantages of lidar and RGB-D cameras and enhances the ability to identify obstacles by fusing the point cloud data of both. To overcome the cumulative error problem existing in a single odometer, this method also adopts an extended Kalman filter algorithm to fuse the data of inertial sensors (IMU) and wheeled odometers to improve the position accuracy and the robustness of the map. However, this method depends on the camera, so there are certain problems in warehousing scenarios with complex lighting conditions.

[0005] In summary, in large-scale scenarios, the tasks of positioning and mapping face many challenges, including the generally existing repetitive structure information inside, the large-scale spatial scale requirements, and the requirements for high-precision and strong-robustness positioning solutions. Due to the closed nature of large-scale buildings, GPS signals often have difficulty penetrating, resulting in the failure of traditional satellite positioning means. In view of this complex environment, it is necessary to propose an innovative solution. Summary of the Invention

[0006] The present invention provides a large-scale positioning and mapping method for quadruped robots based on IMU and lidar combined with a factor graph, which is used to achieve positioning and mapping of legged robots in large-scale scenarios.

[0007] The technical solution of the present invention is as follows:

[0008] According to the first aspect of the embodiments of the present invention, a large-scale positioning and mapping method for quadruped robots based on IMU and lidar combined with a factor graph is provided, including the following steps:

[0009] Data acquisition step: Use the IMU to collect inertial measurement data of the quadruped robot, and at the same time use the lidar to collect point cloud data;

[0010] Data preprocessing step: Perform pre-integration processing on the inertial measurement data of the quadruped robot collected by the IMU to obtain a preliminary estimate of the pose and speed of the robot; then perform distortion correction, nearest neighbor matching, and point cloud registration on the point cloud data collected by the lidar.

[0011] Factor graph construction step: Construct a factor graph based on the processed IMU data and lidar data, and optimize and solve the constructed factor graph by the Gauss-Newton method, with the goal of minimizing the negative logarithm of the joint probability of all factors in the factor graph to obtain the optimal estimate of the robot's pose; among them, in the process of constructing the factor graph, the poses of the robot at different times are used as vertices, and the factors are used as edges to represent the constraints between variables. The factors include: IMU odometry factor, pre-integration factor obtained in the pre-integration step, lidar factor, and laser odometry factor obtained by matching the key frame with the local map.

[0012] Furthermore, the data preprocessing step includes:

[0013] Perform pre-integration calculation on the collected inertial measurement data through the IMU pre-integration model to obtain the pre-integration results of rotation, translation, and linear velocity;

[0014] Perform distortion correction processing on the point cloud data obtained by lidar scanning to eliminate the point cloud distortion phenomenon caused by factors such as the characteristics of the lidar itself and movement; after distortion correction, extract edge feature points according to the curvature algorithm; on this basis, perform nearest neighbor matching, and use the K-d tree method to complete the nearest neighbor matching by analyzing statistical characteristics such as the number and distance distribution of edge feature points in the neighborhood of the points in the point cloud; then, perform point cloud registration through the NDT algorithm. After NDT registration, the position and pose of the quadruped robot at each moment can be determined, thereby constructing a complete map.

[0015] Furthermore, the distortion correction processing is specifically:

[0016] For each frame of collected point cloud data and the corresponding leg odometer data of the quadruped robot, a binary search tree is used for time synchronization to achieve timestamp alignment; the aligned environmental point cloud data and the corresponding leg odometer data of the quadruped robot are stored in a queue in chronological order, and the timestamp of this queue is greater than or equal to s and less than or equal to e; and it is assumed that the quadruped robot performs uniformly accelerated motion between two frames of environmental point cloud data, so the pose of the robot is a quadratic function of time t.

[0017] Solve the robot pose corresponding to each laser point within the time range from s to e through quadratic interpolation; transform all laser points to the same coordinate system according to the solved pose; finally, repackage and publish it as a frame of laser point cloud data.

[0018] Further, when extracting the edge feature points, based on the spatial geometric relationship between each point in the point cloud and its neighboring points, the curvature value of each point is obtained by using the curvature calculation algorithm, and the curvature value of each point is compared with a preset threshold, and the points with curvature values greater than the preset threshold are selected as edge feature points; in the embodiment of the present invention, the set preset threshold is 0.55, and the points with curvature values greater than 0.55 are used as edge feature points.

[0019] Further, the construction of the factor graph based on the processed IMU data and lidar data is specifically as follows: the lidar factors and lidar odometry factors constructed by the lidar are transmitted into the factors constructed by the IMU, and the pose estimation of the lidar is corrected through the rotation, position, angular velocity, and acceleration information updated by the IMU at high frequency, so as to obtain more accurate factors constructed by the lidar; finally, the factors constructed by the corrected lidar and the factors constructed by the IMU are jointly used to construct a factor graph to achieve accurate map construction.

[0020] According to the second aspect of the present invention, there is provided a quadruped robot large-scale scene positioning and mapping system based on IMU and lidar and combined with a factor graph, including the module of the quadruped robot large-scale scene positioning and mapping method based on IMU and lidar and combined with a factor graph described in any one of the above.

[0021] According to the third aspect of the present invention, there is provided a processor, and the processor is used to run a program, and when the program runs, it executes the quadruped robot large-scale scene positioning and mapping method based on IMU and lidar and combined with a factor graph described in any one of the above.

[0022] The beneficial effects of the present invention are:

[0023] 1. Through the data fusion of IMU and lidar and the optimization of the factor graph, the pose drift in the mapping process is effectively suppressed, and the accuracy and consistency of the map are improved, especially showing better performance in long-term and large-scale environments.

[0024] 2. It makes full use of the high-frequency motion information of the IMU and the high-precision environmental perception ability of the lidar, achieving complementary advantages, enabling the system to more accurately estimate poses and construct maps in complex environments, and providing more reliable support for applications such as autonomous navigation and positioning of robots.

[0025] 3. By adopting an incremental factor graph optimization method, the real-time performance of the system is improved, which can meet the requirements of the robot for rapid and accurate mapping and positioning in dynamic environments, enhancing the practicality and adaptability of the system.

[0026] 4. Since the present invention can achieve instant positioning and mapping in large scenes only by using a low-cost 2D lidar and a common IMU, it improves the economic efficiency to a certain extent and contributes to the development of the industry.

[0027] 5. The present invention innovatively uses a quadruped robot for positioning and mapping in large scenes. Due to the structural characteristics of the legged robot itself, it can work in complex and unstructured scenes, providing a certain guidance for the subsequent development of quadruped robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 is the technical roadmap of the present invention;

[0029] Figure 2 is a schematic diagram of the mapping drift caused by only using the lidar in the present invention;

[0030] Figure 3 is a schematic diagram of the data divergence caused by only using the IMU in the present invention;

[0031] Figure 4 is a schematic diagram of the efficiency of the K-d tree nearest neighbor search in the present invention;

[0032] Figure 5 is a schematic diagram of constructing a factor graph in the present invention;

[0033] Figure 6 is a schematic diagram of the mapping achieved by Gmapping_SLAM;

[0034] Figure 7 is a schematic diagram of the mapping achieved by Hector_SLAM;

[0035] Figure 8 is a schematic diagram of the mapping achieved by the present invention;

[0036] Figure 9 is a schematic diagram of the three regions divided after the mapping is achieved by the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0037] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the protection scope of the present invention. It should be noted that, without conflict, the embodiments in this application and the features in the embodiments can be combined with each other arbitrarily.

[0038] Embodiment 1: As Figures 1-9 shown, a large-scale localization and mapping method for a quadruped robot based on an IMU and a lidar in combination with a factor graph includes the following steps:

[0039] Data acquisition step: Use an IMU to collect inertial measurement data of the quadruped robot, and at the same time use a lidar to collect point cloud data;

[0040] Data preprocessing step: Perform pre-integration processing on the inertial measurement data of the quadruped robot collected by the IMU to obtain a preliminary estimate of the pose and velocity of the robot; then perform distortion correction, nearest neighbor matching, and point cloud registration on the point cloud data collected by the lidar;

[0041] Factor graph construction step: Construct a factor graph based on the processed IMU data and lidar data, and optimize and solve the constructed factor graph by the Gauss-Newton method, with the goal of minimizing the negative logarithm of the joint probability of all factors in the factor graph to obtain the optimal estimate of the robot pose; among them, in the process of factor graph construction, the poses of the robot at different times are used as vertices (variables), and factors are used as edges to represent the constraints between variables. The factors include: the IMU odometry factor obtained from the angular velocity and acceleration directly measured by the IMU itself; the pre-integration factor obtained in the pre-integration step; the lidar factor; the laser odometry factor obtained by matching the key frame with the local map;

[0042] Map construction and update step: According to the optimized pose estimation result, accurately fuse the point cloud data of the lidar into the map to construct a map of the environment; the representation form of the map is a grid map. As the robot moves and new data is continuously acquired, the factor graph and the map are continuously updated to ensure the accuracy and integrity of the map. During the update process, the constraints in the factor graph can be adjusted and supplemented according to the new observation data, and re-optimized and solved to achieve the dynamic update of the map.

[0043] Further, the data preprocessing step includes:

[0044] Perform pre-integration calculation on the collected inertial measurement data through the IMU pre-integration model to obtain the pre-integration results of rotation, translation, and linear velocity;

[0045] For the point cloud data obtained by lidar scanning, perform distortion correction processing to eliminate the point cloud distortion phenomenon caused by factors such as the characteristics and movement of the lidar itself; after distortion correction, an accurate object contour can be obtained, which is convenient for extracting edge feature points, and extract edge feature points based on the curvature algorithm; on this basis, perform nearest neighbor matching, and use the K-d tree method to complete the nearest neighbor matching by analyzing the statistical characteristics such as the number and distance distribution of edge feature points in the neighborhood of the points in the point cloud; then, perform point cloud registration through the NDT algorithm. After NDT registration, the position and pose of the quadruped robot at each moment can be determined, thereby constructing a complete map.

[0046] It should be noted that after obtaining the edge feature points, these points constitute the basic data set for nearest neighbor matching; in nearest neighbor matching, it is a process of finding similar point pairs between two sets of data points, and the edge feature points are the key points to be matched. When performing point cloud registration for two frames, edge feature points are extracted from the two frames of point clouds respectively, and then nearest neighbor matching is performed between these edge feature points to find the corresponding points in the two frames of point clouds, thereby establishing the association relationship between the point clouds and providing a basis for subsequent registration calculations.

[0047] Furthermore, the specific description of the data preprocessing steps is as follows:

[0048] It should be noted that for the inertial measurement data collected by the IMU, the inertial measurement data includes angular velocity and acceleration data. Integrating the inertial measurement data can obtain the linear velocity v, rotation R, and translation p. As Figure 3 shows an example of obtaining the linear velocity v, rotation R, and translation p by integrating an inertial measurement data. It can be seen from the figure that the data directly integrated without being processed by the IMU pre-integration model diverges, resulting in a decrease in positioning accuracy and further affecting the mapping effect. Therefore, the present invention introduces the IMU pre-integration model, and the specific description is as follows:

[0049] When considering the IMU data, 5 variables need to be considered: rotation R, translation p, angular velocity ω, linear velocity v, and acceleration a (for convenience of description, the superscript with ~ in the following text is the measured value). The relationships between these variables are:

[0050]

[0051] where: ω ^ is the skew-symmetric matrix form of the angular velocity vector ω. In the time period from t to t+Δt, perform Euler integration on the above formula:

[0052] R(t+Δt) = R(t)Exp(ω(t)Δt)

[0053] v(t+Δt) = v(t) + a(t)Δt

[0054]

[0055] Among them, ω and a can be measured by the IMU, but there are still noise and gravity. Let the measured value be Then:

[0056]

[0057] Among them, b g and b a are the zero biases of the gyroscope and accelerometer respectively, and η g and η a are the Gaussian noises measured by the gyroscope and accelerometer respectively. Substituting the above formula into the Euler integral formula and solving, we get:

[0058]

[0059] In the formula, η gd (t) and η ad (t) are the discretized random walk noises of η g and η a respectively; g represents the gravitational acceleration; T represents the transpose;

[0060] Assume that the zero bias is fixed at time i, and make a first-order linear change model of the pre-integration with respect to the zero bias, discarding the higher-order terms. Then:

[0061]

[0062] In the formula, represent the first-order change amounts of R, v, and p respectively during the process from time i to time j; R i and R j represent the rotations R at times i and j; v i and v j represent the linear velocities v at times i and j respectively; δφ ij is the defined random variable; δv ij is the defined velocity noise; δp ij is the defined translation noise; Δt ij represents the change time from time i to time j.

[0063] Since δφ ij , δv ij , and δp ij are unknown in the above formula, the specific values need to be deduced. Still, only the first-order term coefficients are retained. Then:

[0064]

[0065] In the formula, represents transpose of, Denotes the change in the rotation matrix from time j - 1 to time j, J r,j-1 Denotes the Jacobian matrix at time r, η gd,j-1 Denotes the gyroscope white noise at time j - 1, and Δt denotes the change time.

[0066] If the above equation is pushed to the covariance form, it can be clearly seen that the pre - integrated error will increase with data accumulation, and its observed value will diverge. Thus, δv ij 、δp ij Can be expressed as above:

[0067]

[0068] In the formula, b a,j Denotes the acceleration zero - bias at time j, η ad,j-1 Denotes the accelerometer white noise at time j - 1.

[0069] It should be noted that the above are all assumed that the IMU zero - bias remains unchanged at time i, but in the subsequent map optimization, it will be updated in real - time. Make the following corrections:

[0070]

[0071] In the formula, δb g,i Denotes the updated gyroscope zero - bias at time i, b g,i Denotes the gyroscope zero - bias at time i; δb a,i Denotes the updated acceleration zero - bias at time i.

[0072] Through the correction, the update of the zero - bias becomes taking the partial derivatives of the above three formulas, and only five unknown partial derivatives need to be obtained, that is, the Jacobian matrix:

[0073]

[0074]

[0075] The IMU pre - integration model is derived from the above. Through the IMU pre - integration model, pre - integration calculations are performed on the collected inertial measurement data to obtain the pre - integration results of rotation, translation, and linear velocity.

[0076] Furthermore, the distortion correction process is specifically:

[0077] For each frame of the collected point cloud data and the corresponding leg odometer data of the quadruped robot, a binary search tree is used for time synchronization to achieve timestamp alignment; the aligned environmental point cloud data and the corresponding leg odometer data of the quadruped robot are stored in a queue in chronological order, and the timestamp of this queue is greater than or equal to s and less than or equal to e; and it is assumed that the quadruped robot performs uniformly accelerated motion between one frame of environmental point cloud data, so the pose of the robot is a quadratic function of time t.

[0078] Solve the robot pose corresponding to each laser point within the time range from s to e through quadratic interpolation; transform all the laser points to the same coordinate system according to the solved pose; finally, repackage and publish them as a frame of laser point cloud data.

[0079] Among the above, solving the robot pose corresponding to each laser point within the time range from s to e through quadratic interpolation is specifically as follows:

[0080] Define the start and end times of the current frame of point cloud data as s and e respectively, then the midpoint time Assume that there are poses at times l and k, and l < m < k, then the pose at time m where, LinaInterp represents the linear interpolation method;

[0081] Given the poses P s 、P m 、P e at times s, m, and e, then a quadratic curve can be interpolated to solve for each point pose: P t =At 2 +Bt + C (s ≤ t ≤ e).

[0082] Assume that a frame of laser point cloud data has n laser points, and the pose corresponding to each laser point: {P 1 ,P 2 ,…,P n} is obtained by linear interpolation through the above method. Let X i be the coordinate of the laser point before transformation, and X’ i be the coordinate of the laser point after transformation. Then: represents the transpose of the robot pose corresponding to the i-th laser point obtained through quadratic interpolation; then transform the transformed coordinate into laser data and publish it:

[0083] X’ i =(e x ,e y )

[0084]

[0085] angle=arctan2(ex , e y )

[0086] where (e x , e y ) represents the coordinates of the i-th laser point after removing distortion, range represents the distance from the i-th laser point to the origin of the laser beam, and angle represents the angle of the laser beam.

[0087] Point cloud distortion correction is to ensure the accuracy of subsequent edge extraction. Through point cloud distortion correction, the point cloud can be restored to a state closer to the real scene, enabling the edge feature extraction to more accurately find the true edges of objects, such as the corners of buildings, the contours of objects, etc.

[0088] Furthermore, when extracting the edge feature points, according to the spatial geometric relationship between each point in the point cloud and its neighboring points, the curvature calculation algorithm is used to obtain the curvature value of each point. The curvature value of each point is compared with a preset threshold, and the points with a curvature value greater than the preset threshold are selected as edge feature points; in the embodiment of the present invention, the preset threshold is set to 0.55, and the points with a curvature value greater than 0.55 are used as edge feature points.

[0089] Edge feature extraction provides representative and stable features for the next nearest neighbor matching.

[0090] The K-d tree is used for nearest neighbor matching. In a two-dimensional space (K = 2), the dimensions are alternately selected first by the X-axis and then by the Y-axis. According to the median of the data points in this dimension, the data space is divided into two parts, and then the subtrees are recursively constructed. When searching for the nearest neighbor points, starting from the root node, it is decided whether to enter the left subtree or the right subtree according to the comparison between the query point cloud point and the current node. During the search process, the current nearest neighbor point and distance are recorded, and when backtracking, it is checked whether the other branch may contain closer points, so that the nearest neighbor matching can be quickly performed. A threshold t is set according to the distance between the query point cloud point and the current node. In the embodiment of the present invention, the preset threshold is set to 0.2, and the point cloud points greater than the threshold are removed, and the points less than the threshold are retained.

[0091] Such as Figure 4 is the search time for using the K-d tree to search for the nearest neighbor. It should be noted that in SLAM mapping, the K-d tree is not the fastest way to search for the nearest neighbor, but its structural characteristics are the most suitable search method for SLAM.

[0092] After finding the nearest neighbor matching point pairs, point cloud registration is performed through the NDT algorithm.

[0093] First, define the reference point cloud and the target point cloud. The reference point cloud is a point cloud data set used as a benchmark. It usually has a relatively fixed spatial distribution or more prior knowledge. The target point cloud is the point cloud that needs to be registered with the reference point cloud. It is the environmental point cloud newly scanned by the quadruped robot during movement. The position and pose of the target point cloud relative to the reference point cloud are unknown. The purpose of the NDT algorithm is to find an optimal rotation and translation transformation so that the target point cloud can be accurately aligned with the reference point cloud in space. After NDT registration, the position and pose of the quadruped robot at each moment can be determined, thus constructing a complete map.

[0094] The specific approach is as follows: Divide the target point cloud into several voxels at a resolution of 0.4; then calculate the Gaussian distribution of the point cloud in each voxel. Let the mean and variance in the k-th voxel be μ k , ∑k; during registration, first calculate which voxel each point falls into, and then establish the residual formed by this point and the μ k , ∑k of this voxel; finally, use the Levenberg-Marquardt method to iterate the estimated pose and then update the pose.

[0095] After point cloud registration, factor graph construction is carried out, as shown in Figure 5 which is the schematic diagram of the factor graph constructed by the present invention. The factors include: the IMU odometry factor and the pre-integration factor formed by the IMU, the LiDAR factor, and the LiDAR odometry factor formed by the matching of key frames and local maps. Among them, the IMU odometry provides position constraints, and the pre-integration factor provides constraints on the inter-frame pose transformation. The optimized latest pose participates in the optimization of the IMU odometry as the position constraint of the LiDAR odometry on the IMU odometry, so as to perform the positioning and mapping of the quadruped robot. Finally, the IMU odometry outputs a high-frequency pose, and the LiDAR odometry incrementally constructs a point cloud map.

[0096] The addition of the IMU odometry factor and the pre-integration factor includes: Based on the angular velocity and acceleration directly measured by the IMU, the IMU odometry factor is formed. Among them, let the positions of the quadruped robot from the i-th moment to the j-th moment be p i , p j , the angular velocity measured by the IMU is ω, the linear velocity is v, the rotation is R, and the acceleration is a. Then the established IMU odometry factor is f IMU (R, p i , p j , ω, a), and this factor represents the pose constraint based on IMU data. In the pre-integration factor, the gyroscope zero bias b g and the zero bias b a of the accelerometer, as well as the translation p, also need to be additionally added. Therefore, the established pre-integration factor is f yIMU (R, b g , ba , p, v).

[0097] The addition of the lidar factor includes: Through the NDT algorithm, the point cloud currently scanned by the lidar is matched with the constructed map or the previous scanned point cloud to obtain the constraint between the current pose of the robot and the map, forming the lidar factor. Among them, W i is the true pose variable of the quadruped robot in the world coordinate system, and W' i is the pose estimate obtained through scan matching (NDT algorithm). Then the established lidar factor is f Lidar (W i , W' i ), and this factor represents the pose constraint based on lidar scan matching.

[0098] The lidar odometry factor formed by the matching of the key frame and the local map includes: For each key frame K i , there is its corresponding pose variable X i = [p i , q i , where p i is the position and q i is the attitude. K j is the reference frame in the local map where the key frame K i is located, and the corresponding pose variable X j = [p j , q j . Then the established lidar odometry factor is f ilidar (X i , X j ), and this factor connects the pose variable X i of the key frame K i and the pose variable X j of its reference frame K j in the local map.

[0099] Therefore, after the factor construction is completed, the factors constructed by the lidar are fused with the factors constructed by the IMU. If only the factor graph constructed by the lidar is used for pose estimation, it will be affected by factors such as point cloud registration error and environmental noise. If only the factor graph constructed by the IMU is used, due to the integration drift problem, the pose estimation will gradually deviate from the true value. Therefore, by transmitting the information of the factor graph constructed by the lidar to the factor graph of the IMU, the high-precision geometric constraint of the lidar and the high-frequency constraint of the IMU can be considered, so as to obtain an accurate pose estimation. The specific method is: Transmit the factors f Lidar (W i , W' i ), f ilidar (X i , X jThe factor f constructed by the IMU is transmitted IMU (R, p i , p j , ω, v), f yIMU (R, b g , b a , p, v), the pose estimation of the lidar is corrected by the rotation, position, angular velocity and acceleration information updated at high frequency by the IMU, so as to obtain a more accurate factor constructed by the lidar. Finally, the factor graph is jointly constructed by the corrected factor constructed by the lidar and the factor constructed by the IMU to achieve accurate map construction.

[0100] Steps for optimizing and solving the factor graph: The Gaussian-Newton method is used to optimize and solve the constructed factor graph to minimize all factors f Lidar (W i , W’ i ), f ilidar (X i , X j ), f IMU (R, p i , p j , ω, v), f yIMU (R, b g , b a , p, v), taking the negative logarithm of the joint probability as the objective, to obtain the optimal estimation of the robot pose. Among them, during the optimization process, by iteratively adjusting the values of the variable nodes, calculating the gradient and Hessian matrix of the cost function, and updating the pose estimation until the cost function converges or reaches the maximum number of iterations.

[0101] The map update in the steps for optimizing and solving the factor graph includes: using the optimized pose estimation to update the transformation of the lidar scan point cloud to the global coordinate system, thereby updating the map. The present invention uses the occupancy grid map method to achieve map construction.

[0102] Furthermore, add prior factor constraints. Add odometry pose prior factors, velocity prior factors, and IMU bias prior factors to the factor graph to enhance the stability and accuracy of the optimization process. The odometry pose prior factor is the pose information converted from LiDAR data to the IMU coordinate system and added to the factor graph as prior information. The purpose of this step is to use the pose information measured by LiDAR as a prior to constrain the pose variables in the factor graph, making them closer to the true values and thus improving the accuracy of pose estimation. This prior information helps the present invention provide initial constraints in the absence of other observation data, making the optimization problem solvable. The velocity prior factor is usually set to zero velocity as a prior. When adding factors, the confidence of the normal velocity is relatively poor. The velocity prior factor is used to constrain the velocity change of the robot, keeping it relatively stable during the optimization process. Although the confidence of the velocity prior factor is not high, it still provides additional information for the factor graph, helping to improve the overall optimization accuracy. The IMU bias prior factor is used to constrain the bias parameters of the IMU, keeping them stable during the optimization process. The purpose of the IMU bias prior factor is to reduce the impact of the IMU bias on pose estimation. Since the IMU bias drifts over time in the derivation of Embodiment 2 of the present invention, adding a prior factor to constrain its range of change can improve the accuracy and stability of pose estimation.

[0103] The following further illustrates with data as follows:

[0104] This method selects the Jetson Nano development board as the control system platform, the quadruped robot is Unitree GO1, equipped with the Ubuntu18.04 system, and runs ROS (Robot Operating System) to implement the processing of sensor information and subsequent mapping and navigation functions. The LiDAR is RPLIDARA2M7 developed by Slan Technology Co., Ltd., and the scanning frequency is set to 5Hz. The IMU uses the nine-axis N100, and the data output frequency is 400Hz. Further, while obtaining LiDAR data in the ROS system, the IMU uploads odometry data.

[0105] The present invention adopts the ISAM2 (Incremental Smoothing and Mapping) incremental smoothing and mapping method. Based on the factor graph model, the pose of the robot in it can be used as vertices (variables), while the edges represent the constraint relationships between these variables (IMU odometry factor, pre-integrated factor, lidar factor, lidar odometry factor). ISAM2 processes new data frame by frame and updates the pose and map of the robot in real time. Further, reset the factor graph optimizer. After the first frame and every certain number of frames (the threshold is set to 50 frames in the present invention), it is necessary to reset the ISAM2 optimizer. This step maintains the efficiency and accuracy of the optimizer and avoids cumulative errors caused by long-term operation. When resetting, the parameters of the optimizer will be reconfigured, including relinearizeThreshold (the threshold for relinearizing non-linear factors) and relinearizeSkip (whether to skip relinearization), to ensure the smooth progress of the optimization process. After that, reset the factor objects in the factor graph. The factor graph factor objects are used to store the constructed factors (IMU odometry factor, pre-integrated factor, lidar factor, lidar odometry factor). When resetting, the existing factor graph factor objects will be cleared and a new empty factor graph will be created. This step ensures that a new frame can construct the factor graph from scratch without being affected by the previous frames. Further, reset the factor graph state variable objects. The factor graph state variable objects are used to store the estimated values of the robot's pose, speed, deviation, etc. When resetting, these state variables will be reset to the initial values or the state values of the previous frame. This step is to start state estimation at a new starting point and ensure the accuracy of state estimation.

[0106] The mapping comparison experiment is carried out by adopting the technical scheme as described above. A four-legged robot large-scale scene positioning and mapping method based on IMU and lidar and combined with factor graph disclosed by the present invention is compared with the current mainstream two-dimensional lidar simultaneous localization and mapping methods: Gmapping_SLAM and Hector_SLAM. The mapping comparison is carried out between the method of this embodiment in a self-built large-scale warehouse environment (about 430 square meters) and the above traditional methods, as Figure 6 A schematic diagram of mapping realized by Gmapping_SLAM. As Figure 7 A schematic diagram of mapping realized by Hector_SLAM. As Figure 8 A schematic diagram of mapping realized by the present invention.

[0107] After the above three mapping methods are completed, to test the local mapping effect, the map is divided into three regions, as Figure 9 shown. The error comparison of different three regions is carried out by using the algorithm of the present invention and the above traditional algorithms. It can be seen from the comparison that the method of the present invention has better mapping performance, as shown in Table 1.

[0108] Table 1 Mapping error conditions in each area of the warehouse m

[0109]

[0110] Applying the above technical solution, it can be known that the method of the present invention constructs two odometer systems with LiDAR and IMU as the core. The two are fused, optimized and iterated to achieve the in-depth fusion and utilization of information. The high-frequency attitude output ability of the IMU odometer provides a continuous and stable positioning reference for the system; the LiDAR odometer gradually constructs a fine point cloud map through a progressive matching strategy. This map not only reflects the actual situation of the warehouse environment, but also further improves the positioning accuracy of the system. The fusion of the two has both the advantages of the IMU in high-frequency positioning and fully exerts the advantages of the LiDAR in environmental perception and map construction. Finally, combined with the factor optimization constraint constructed by the lidar and IMU, a relatively accurate robot pose is matched for each frame of lidar data, achieving good robustness in positioning and mapping.

[0111] Embodiment 2: A quadruped large-scale scene positioning and mapping system based on IMU and lidar combined with a factor graph, including the module of the quadruped large-scale scene positioning and mapping method based on IMU and lidar combined with a factor graph described in any one of Embodiment 1. Specifically, it includes: a data acquisition module for using the IMU to collect inertial measurement data of the quadruped robot and simultaneously using the lidar to collect point cloud data; a data preprocessing module for pre-integrating the inertial measurement data of the quadruped robot collected by the IMU to obtain a preliminary estimate of the pose and speed of the robot; and then performing distortion correction, nearest neighbor matching and point cloud registration on the point cloud data collected by the lidar; a factor graph construction module for constructing a factor graph based on the processed IMU data and lidar data, and optimizing and solving the constructed factor graph by the Gauss-Newton method, with the goal of minimizing the negative logarithm of the joint probability of all factors in the factor graph to obtain the optimal estimate of the robot pose; wherein, during the construction of the factor graph, the poses of the robot at different times are used as vertices, and the factors are used as edges to represent the constraints between variables. The factors include: IMU odometer factors, pre-integration factors obtained in the pre-integration step, lidar factors, and lidar odometer factors obtained by matching key frames with local maps. For the parts not detailed in each module, reference can be made to the relevant descriptions of other embodiments.

[0112] Embodiment 3: A processor, the processor is used to run a program, and when the program runs, it executes the module of the quadruped large-scale scene positioning and mapping method based on IMU and lidar combined with a factor graph described in any one of Embodiment 1, and each module is located in the same processor; or, the above-mentioned each module is located in different processors in any combination form.

[0113] The specific embodiments of the present invention have been described in detail above in conjunction with the accompanying drawings. However, the present invention is not limited to the above embodiments, and various changes can be made without departing from the spirit of the present invention within the scope of knowledge possessed by those of ordinary skill in the art.

Claims

1. A large scene positioning and mapping method for a quadruped robot based on IMU and laser radar combined with factor graph, characterized in that: The following steps are involved: Data collection step: using IMU to collect inertial measurement data of the quadruped robot and using LiDAR to collect point cloud data; The data preprocessing step is to pre-integrate the inertial measurement data of the quadruped robot collected by the IMU to obtain a preliminary estimate of the robot's position and velocity; then, the point cloud data collected by the lidar is subjected to distortion correction, nearest neighbor matching and point cloud registration; The factor graph construction step constructs a factor graph based on the processed IMU data and lidar data, and optimizes and solves the constructed factor graph through the Gauss-Newton method, with the goal of minimizing the negative logarithm of the joint probability of all factors in the factor graph to obtain the optimal estimate of the robot's posture. In the process of factor graph construction, the posture of the robot at different times is used as the vertex, and the factor is used as the edge to represent the constraints between variables. The factors include: IMU odometer factor, pre-integration factor obtained in the pre-integration step, lidar factor, and laser odometer factor obtained by matching the key frame with the local map.

2. The method for large scene positioning and mapping of a quadruped robot based on IMU and laser radar combined with factor graph according to claim 1 is characterized in that: The data preprocessing step comprises: The collected inertial measurement data is pre-integrated and calculated through the IMU pre-integration model to obtain the pre-integration results of rotation, translation and linear velocity; The point cloud data obtained by the laser radar scanning is subjected to distortion correction processing to eliminate the point cloud distortion caused by the laser radar's own characteristics and movement factors; after distortion correction, the edge feature points are extracted according to the curvature algorithm; on this basis, the nearest neighbor matching is performed, and the Kd tree method is used to analyze the statistical characteristics such as the number and distance distribution of edge feature points in the neighborhood of the point cloud midpoint to complete the nearest neighbor matching; then, the point cloud is aligned through the NDT algorithm. After NDT alignment, the position and posture of the quadruped robot at each moment can be determined, thereby constructing a complete map.

3. The method for large scene positioning and mapping of a quadruped robot based on IMU and laser radar combined with factor graph according to claim 1 is characterized in that: The distortion correction process is specifically as follows: A binary search tree is used to synchronize the time of each frame of point cloud data collected and the corresponding leg odometer data of the quadruped robot to achieve timestamp alignment; the aligned environmental point cloud data and the corresponding leg odometer data of the quadruped robot are stored in a queue in chronological order, and the timestamp of the queue is greater than or equal to s and less than or equal to e; and it is assumed that the quadruped robot performs uniform acceleration motion between a frame of environmental point cloud data, so the robot's posture is a quadratic function of time t; The robot posture corresponding to each laser point within the time s to e is solved by quadratic interpolation; all laser points are converted to the same coordinate system according to the solved posture; and finally, they are repackaged into a frame of laser point cloud data for release.

4. The method for large scene positioning and mapping of a quadruped robot based on IMU and laser radar combined with factor graph according to claim 1, characterized in that: When extracting the edge feature points, the curvature value of each point is obtained by using a curvature calculation algorithm based on the spatial geometric relationship between each point in the point cloud and its neighborhood points. The curvature value of each point is compared with a preset threshold, and points greater than the preset threshold are screened out as edge feature points. In the embodiment of the present invention, the preset threshold is set to 0.55, and points with curvature values ​​greater than 0.55 are selected as edge feature points.

5. The method for large scene positioning and mapping of a quadruped robot based on IMU and laser radar combined with factor graph according to claim 1, characterized in that: The method of constructing a factor graph based on the processed IMU data and lidar data is specifically as follows: the lidar factors and laser odometer factors constructed by the lidar are transmitted to the factors constructed by the IMU; the pose estimation of the lidar is corrected by the rotation, position, angular velocity and acceleration information updated at a high frequency by the IMU, so as to obtain more accurate factors constructed by the lidar; finally, the factor graph is constructed by jointly constructing the factors constructed by the corrected lidar and the IMU, so as to realize accurate map construction.

6. A large-scene positioning and mapping system for quadruped robots based on IMU and laser radar combined with factor graphs, characterized by: A module comprising a large scene positioning and mapping method for a quadruped robot based on an IMU and a laser radar combined with a factor graph as described in any one of claims 1-5.

7. A processor for running a program, characterized in that: When the program is running, the method for large-scene positioning and mapping of a quadruped robot based on IMU and laser radar combined with factor graph as described in any one of claims 1 to 5 is executed.

Images