Robot positioning and mapping method and system based on road elevation map feedforward

Abstract

The invention provides a robot positioning and mapping method and system based on road elevation map feedforward. The method comprises the following steps: S1, receiving external sensor data, fitting a front road condition by using a radial basis function, and constructing elevation map information of a front road; s2, internal sensor data are received, an optimization problem is constructed in combination with elevation map information, and pose transformation between robot frames is calculated; s3, constructing a pose factor graph, and simultaneously optimizing the overall pose information of the robot and the matching precision of a global map in the background; and S4, establishing a loopback detection process, and optimizing the overall pose and the global map of the robot by detecting a moving closed loop of the robot. According to the method, road height information is integrated into a traditional mapping and positioning algorithm of the mobile robot to serve as feedforward, and particularly in a scene with topographic relief, the algorithm precision is improved through the method.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and specifically, to a method and system for robot positioning and mapping based on road elevation map feedforward. Background Art

[0002] With the continuous in-depth development and wide application of unmanned driving technology in modern society, robot positioning and mapping (SLAM), as one of the core technologies of the autonomous driving system, has become increasingly important. In complex and changing road scenarios, a robot must have the ability to perceive its own position and the surrounding environment information in real time and accurately, which is the key to realizing efficient path planning and motion control.

[0003] However, currently, there are many challenges in achieving robust and efficient positioning and mapping in complex environments. On the one hand, the dynamic change of terrain complexity makes it difficult for the robot to accurately judge its own state; on the other hand, the inevitable noise interference in sensor data further increases the difficulty of positioning and mapping. Therefore, how to overcome these problems has become a research hotspot and technical difficulty in this field.

[0004] Regarding the positioning and mapping problems of mobile robots, many researchers at home and abroad have carried out a large number of in-depth studies, and the related methods can be roughly classified into two categories.

[0005] The first category is the method based on Kalman filtering. The core of this method is to construct a motion model and an observation model, and use the Kalman filtering theory to recursively estimate the state of the robot, and then gradually update the map information. For example, Ke Sun et al. [1] (Sun K, Mohta K, Pfrommer B, et al. Robust stereo visual inertial odometry for fast autonomous flight [J]. IEEE Robotics and Automation Letters, 2018, 3(2): 965-972.) proposed MSCKF (Multi-State Constraint Kalman Filter), which models the SLAM problem as a state estimation problem. This method has certain advantages in computational efficiency, especially suitable for small-scale and low-dimensional state space problems. However, as the environmental complexity increases sharply, its disadvantages gradually appear, that is, it is easily affected by significant non-linear and non-Gaussian noise, resulting in a significant decrease in estimation accuracy.

[0006] The second category is the method based on optimization problems. This type of method transforms the SLAM problem into an optimal estimation problem, constructs an objective function and solves the optimal solution to achieve accurate state estimation and map update. For example, Shan et al. [2] (Shan T, Englot B, Meyers D, et al. Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping [C] / / 2020 IEEE / RSJ international conference on intelligent robots and systems (IROS). IEEE, 2020:5135-5142.) proposed a tightly-coupled lidar inertial odometry framework for high-precision and real-time trajectory estimation and mapping. This framework introduced a factor graph and used the factor graph for pose optimization estimation to solve the optimal positioning result. Although the optimization-based method has high precision and flexibility, due to the extremely complex solution process of the optimization problem, it usually has high requirements for the computing power of the platform.

[0007] Through the retrieval of patent documents, it is found that the invention patent with the publication number CN118037823B discloses a robot mapping method and related device. The method includes: calculating the first pose estimation value of the robot at the current moment according to the lidar data at the previous moment, the lidar data at the current moment, and the IMU data between the two moments; performing loop detection on the first pose estimation value at the current moment. If a loop is formed between the previous moment and a historical moment, calculating the second pose estimation value of the robot at the current moment according to the first pose estimation value at the current moment and the first pose estimation value at the historical moment; performing joint factor graph optimization according to the first pose estimation value at the current moment, the second pose estimation value at the current moment, and the data representing the current height information of the robot to obtain the final pose of the robot at the current moment; updating the map according to the final pose at the current moment and the lidar data at the current moment to obtain the final map up to the current moment. This patent only uses the error state Kalman filter algorithm for pose estimation, does not mention using the feedforward information of the road elevation map, does not utilize the radial basis function, and has limitations in the calculation and optimization methods.

[0008] In summary, the existing methods based on Kalman filtering and optimization problems both have certain limitations when dealing with complex road scenarios. In order to effectively meet the mapping and positioning requirements of mobile robots in complex road scenarios, based on the research results of predecessors, it has become an urgent key task to study a robot positioning and mapping method and system based on the feedforward of the road elevation map. Summary of the Invention

[0009] Aiming at the defects in the prior art, the purpose of the present invention is to provide a robot positioning and mapping method and system based on road elevation map feedforward.

[0010] A robot positioning and mapping method based on road elevation map feedforward provided by the present invention includes the following steps:

[0011] Step S1, obtain the point cloud data in front of the robot through a lidar, and use the radial basis function fitting method to generate a ground elevation map;

[0012] Step S2, use the inertial measurement unit data to initially estimate the pose transformation between the current frame and the previous frame of the robot to obtain the initial pose of the robot; combine the robot kinematic model to calculate the contact point coordinates of the four wheels of the robot on the ground elevation map, and based on the ground elevation map information generated by fitting, analyze the height error between the contact point and the ground elevation map, and output the robot pose transformation matrix and the contact point height error;

[0013] Step S3, based on the robot pose transformation matrix and the contact point height error, use a factor graph to construct a comprehensive optimization problem and optimize the global pose and map of the robot;

[0014] Step S4, during the construction of the comprehensive optimization problem, add a loop detection module to identify the historical positions passed by the robot and detect potential closed-loop areas.

[0015] Preferably, step S1 includes the following sub-steps:

[0016] Step S1.1, obtain the point cloud data of the current frame from the lidar, denoted as P = {p i =(x i , y i , z i )|i = 1, 2, …K}, where each point p i contains three-dimensional coordinate information (x i , y i , z i );

[0017] Step S1.2, select the driving direction of the robot, and filter out a part of the point cloud in front of the robot from the point cloud data, that is, the point cloud of the region of interest Uniformly fill the center points of the radial basis function in the area where P ROI is projected onto the XOY plane, and use C = {c i =(c x,i , c y,i )|i = 1, 2, …, N} to represent the set of center points;

[0018] Step S1.3, use the point cloud of the region of interest to generate a ground elevation map through the radial basis function fitting method.

[0019] Preferably, step S1.3 includes the following sub-steps:

[0020] Step S1.3.1, use the radial basis function to model the ground elevation map and set the fitting function for P ROI

[0021] Step S1.3.2, construct the objective function to minimize the height error between the point cloud in the region of interest and the fitting plane:

[0022]

[0023] where w = [w 1 , w 2 , …, w N T represents the weight vector, h(x i , y i ) represents the elevation estimation of the fitting function for the two-dimensional coordinates (x i , y i ), z i represents the actual elevation of the two-dimensional coordinates (x i , y i ), that is, the actual height provided by the point cloud p i = (x i , y i , z i ).

[0024] Step S1.3.3, transform the optimization problem into a linear algebra form:

[0025] Aw = z

[0026] where the element A ij of the kernel matrix A is φ(||(x i , y i ) - (c x,j , c y,j )||), is the height vector of the point cloud,

[0027] Step S1.3.4, use the least squares method to solve for the weight vector w:

[0028] w = (A T A) -1 A t z

[0029] ​​Step S1.3.5: Substitute the weight w calculated in step S1.3.4 into the fitting function h(x, y), and the ground elevation map model within the region of interest can be obtained. For any coordinate (x, y) within the region of interest, based on the weight vector w and the fitting function h(x, y), its elevation estimation can be obtained:

[0030]

[0031] Preferably, in step S1.3.1, the fitting function is set as:

[0032]

[0033] where (c x,j , c y,j ) ∈ C is the center point; w j is the weight coefficient to be solved; φ(r) is the radial basis function, and the Gaussian kernel is used:

[0034]

[0035] where σ is the scale parameter of the kernel function, and r is the independent variable of the radial basis function. Specifically in this example, it refers to the Euclidean distance between two points.

[0036] Preferably, step S2 includes the following sub-steps:

[0037] Step S2.1: Use the data of the inertial measurement unit (IMU) to calculate the initial pose transformation of the current frame of the robot relative to the previous frame:

[0038] T k = T k-1 ·ΔT IMU

[0039] where T k ∈ SE(3) represents the initial pose of the robot at the k-th frame; ΔT IMU represents the pose increment between frames deduced by the IMU;

[0040] Step S2.2: According to the robot kinematic model, calculate the positions of the four wheel contact points in the robot body coordinate system where i = 1, 2, 3, 4 correspond to the four wheels, and let the wheel coordinates be:

[0041]

[0042] Convert the four wheel contact points to the world coordinate system:

[0043]

[0044] where T kis the initial pose transformation matrix;

[0045] Step S2.3, according to the generated ground elevation map by fitting Calculate the predicted height of the contact point on the ground elevation map

[0046]

[0047] The contact point height error is defined as:

[0048]

[0049] Where is the height of the wheel in the world coordinate system calculated according to the robot pose and robot kinematics;

[0050] Step S2.4, take the contact point height error the point cloud inter-frame matching error e pc and the trajectory smoothness constraint e smooth as cost terms to construct an optimization problem:

[0051]

[0052] Where w z , w pc , w smooth are weight coefficients, and through iterative optimization, solve the inter-frame pose transformation matrix T k .

[0053] Preferably, step S3 includes the following sub-steps:

[0054] Step S3.1, based on the robot pose transformation matrix and the contact point height error, construct a factor graph Where contains the pose information X of the robot i ∈ SE(3), the map points required for ground fitting and the factor set

[0055] Step S3.2, based on the factor graph, iteratively optimize the global pose and the map through a factor graph optimization algorithm (such as Levenberg-Marquardt or Dogleg).

[0056] Preferably, in step S3.1, the factor set includes: inter-frame constraint factors, IMU factors, and elevation map constraint factors;

[0057] The inter-frame pose transformation is provided by the robot pose transformation matrix output in step S2, and the inter-frame constraint factor is defined as:

[0058]

[0059] Among them, is the optimized inter-frame pose, represents the Mahalanobis weighted distance, Σ odom represents the point cloud covariance;

[0060] The IMU data provides angular velocity and acceleration, which are used to constrain the rotational and displacement changes between adjacent frames. The IMU factor is defined as:

[0061]

[0062] Among them, represents the rotational change ΔR i ∈SO(3) and velocity change h imu (X i-1 , X i ) represents the predicted values deduced from the poses X i-1 and X i , including the rotation matrix and velocity; Σ imu represents the covariance matrix of the IMU data;

[0063] The elevation map constraint factor is used to constrain the matching between the robot's contact points and the elevation map, and the robot's pose is constrained by the fitted elevation map, which is defined as follows:

[0064]

[0065] Among them, is the height value obtained from the fitted elevation map, h elevation (X i , L j ) is the elevation value deduced from the current pose X i and the map point L j , that is, Σ elevation represents the covariance of the elevation estimate.

[0066] Preferably, in step S3.2, the comprehensive optimization problem is constructed through the factor graph:

[0067]

[0068] Among them, represents the pose information of the robot, represents the map points required for ground fitting, represents the height value obtained from the fitted elevation map. Substituting the factors, the expression is expanded as follows:

[0069]

[0070] After adding a new frame to the factor graph, the nodes and edges of the factor graph are dynamically updated, and the factor graph optimization algorithm (such as Levenberg-Marquardt or Dogleg) is used to iteratively optimize the global pose and the map.

[0071] Preferably, step S4 includes the following steps:

[0072] Step S4.1, project the current point cloud P t frame onto a two-dimensional polar coordinate grid to generate a ScanContext descriptor S t ;

[0073] Step S4.2, compare the current frame descriptor S t with all historical frame descriptors and calculate the similarity between two frames using cosine similarity, and calculate the similarity score sim(S t , S i ):

[0074]

[0075] If there exists a frame j such that sim(S t , S i ) > δ, it is considered that there may be a loop between frame t and j, where δ is the similarity threshold;

[0076] Step S4.3, perform fine matching on the point cloud P i of the candidate frame and the point cloud P t of the current frame, and use a point cloud registration algorithm (NDT or ICP) to calculate the relative pose transformation T j→t ;

[0077] Step S4.4, when the state at the current time t forms a closed loop with the state at the historical time j, construct a loop factor:

[0078]

[0079] where, X j is the state at the historical time j, X t represents the state at the current time t, T j→t ∈ SE(3) represents the true value of the transformation matrix for X j transformed to X t , represents the estimated value of the transformation matrix for X j transformed to X t , Σ loopRepresents the point cloud covariance. Add the loop closure factor to the factor graph and update the global map and robot pose by minimizing the overall objective function.

[0080] The present invention also provides a robot positioning and mapping system based on road elevation map feedforward, including:

[0081] Module M1, which obtains the point cloud data in front of the robot through a lidar and generates a ground elevation map using the radial basis function fitting method;

[0082] Module M2, which initially estimates the pose transformation between the current frame and the previous frame of the robot using the inertial measurement unit data to obtain the initial pose of the robot; combines the robot kinematic model to calculate the contact point coordinates of the four wheels of the robot on the ground elevation map, and analyzes the height error between the contact point and the ground elevation map based on the information of the generated ground elevation map, and outputs the robot pose transformation matrix and the contact point height error;

[0083] Module M3, which constructs a comprehensive optimization problem using a factor graph based on the robot pose transformation matrix and the contact point height error and optimizes the global pose and map of the robot;

[0084] Module M4, which adds a loop closure detection module during the construction of the comprehensive optimization problem to identify the historical positions passed by the robot and detect potential closed-loop areas.

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

[0086] 1. In step S1 of the present invention, a method for quickly fitting lidar point cloud data using a radial basis function (RBF) is proposed to generate the road elevation map in front of the robot. During the generation of the elevation map, parallel computing based on GPU significantly improves the fitting speed and accuracy, providing accurate environmental information support for subsequent positioning and mapping.

[0087] 2. In step S2 of the present invention, the elevation map information is organically combined with the nonholonomic motion constraints of the robot, and then an optimization objective function for inter-frame point cloud matching is constructed. By minimizing this objective function, not only the robustness of point cloud registration is enhanced, but also the positioning accuracy of the robot is further improved. Especially in complex and changeable road scenarios, it shows stronger environmental adaptability. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] By reading the detailed description of the non-limiting embodiments with reference to the following drawings, other features, objects, and advantages of the present invention will become more apparent:

[0089] Figure 1 It is a flowchart of a robot positioning and mapping method based on road elevation map feedforward in an embodiment of the present invention. Detailed implementation manners

[0090] The present invention will be described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several changes and improvements can still be made. These all belong to the protection scope of the present invention.

[0091] On the basis of drawing on the research results of predecessors, the present invention creatively proposes a robot positioning and mapping method and system based on road elevation map feedforward. The method can effectively improve the positioning accuracy of the original robot mapping and positioning technology in complex road scenarios. Specifically, the method first collects lidar point cloud data, and uses radial basis function fitting to generate road elevation map information in front of the robot; then, taking the elevation map information and the nonholonomic constraints of the robot as optimization items, combining the information collected by the robot sensors, constructs an optimization problem for frame-to-frame point cloud matching, and aims to minimize the objective function to calculate the pose transformation information between robot frames; then, based on the pose factor graph, fuses the high-precision local pose transformation results into the comprehensive optimization problem. During this process, synchronously optimize the overall pose estimation of the robot and the matching accuracy of the global map. Finally, establish an independent loop detection process to detect the closed loop formed by the robot during movement in real time, and correct the overall pose of the robot and the global map through loop optimization to improve the accuracy and robustness of long-term mapping. The present invention significantly enhances the environmental adaptability and real-time response ability of the system in complex road scenarios through feedforward elevation map information, and effectively improves the positioning and mapping performance of the robot in dynamic environments.

[0092] Embodiment 1:

[0093] Figure 1 It is a flowchart of a robot positioning and mapping method based on road elevation map feedforward in an embodiment of the present invention.

[0094] As Figure 1 shown, this embodiment proposes a robot positioning and mapping method based on road elevation map feedforward, including the following steps:

[0095] Step S1, obtain the point cloud data in front of the robot through the lidar, and use the radial basis function (RBF) fitting method to generate the ground elevation map.

[0096] In this embodiment, during the fitting process, the parameters of the kernel function are adjusted based on the geometric structure of the environment to ensure the accuracy and real-time performance of the elevation map fitting. The generated elevation map can reflect key information such as the slope, depression and protrusion of the road, providing feedforward data support for subsequent point cloud matching.

[0097] Specifically, step S1 includes the following sub-steps:

[0098] Step S1.1, obtain the point cloud data of the current frame from the lidar, denoted as P = {p i =(x i , y i , z i )|i = 1, 2, …K}, where each point p i contains three-dimensional coordinate information (x i , y i , z i );

[0099] Step S1.2, select the driving direction of the robot, and filter out a part of the point cloud in front of the robot from the point cloud data, that is, the region of interest (ROI) point cloud Uniformly fill the center points of the radial basis function within the region where P ROI is projected onto the XOY plane, and use C = {c i =(c x,i , c y,i )|i = 1, 2, …, N} to represent the set of center points;

[0100] Step S1.3, use the region of interest point cloud to generate a ground elevation map through the radial basis function (RBF) fitting method;

[0101] Furthermore, step S1.3 includes the following sub-steps:

[0102] Step S1.3.1, use the radial basis function to model the ground elevation map for P ROI and set the fitting function;

[0103] The fitting function is set as:

[0104]

[0105] where (c x,i , c y,i ) ∈ C is the center point; w j is the weight coefficient to be solved; φ(r) is the radial basis function, and the Gaussian kernel is used:

[0106]

[0107] where σ is the scale parameter of the kernel function, and r is the independent variable of the radial basis function. In this embodiment, it refers to the Euclidean distance between two points.

[0108] Step S1.3.2, construct the objective function to minimize the region of interest point cloud Height error from the fitting plane:

[0109]

[0110] where w = [w 1 , w 2 , …, w M T represents the weight vector, and h(x i , y i ) represents the elevation estimation of the fitting function for the two-dimensional coordinates (x i , y i ). z i represents the actual elevation of the two-dimensional coordinates (x i , y i ), that is, the actual height provided by the point cloud p i = (x i , y i , z i ).

[0111] Step S1.3.3, transform the optimization problem into a linear algebra form:

[0112] Aw = z

[0113] where the element A ij of the kernel matrix A is φ(||(x i , y i ) - (c x,i , c y,i )||), is the height vector of the point cloud, T represents the transpose, and M corresponds to M point cloud data.

[0114] Step S1.3.4, use the least squares method to solve for the weight vector w:

[0115] w = (A T A) -1 A t z

[0116] In this embodiment, the construction of the kernel matrix A and the efficient parallel calculation of the weight W are implemented on the GPU, and parallel acceleration is performed using CUDA technology, significantly improving the efficiency of elevation map fitting.

[0117] Step S1.3.5, substitute the weight w calculated in Step S1.3.4 into the fitting function h(x, y), and the ground elevation map model within the region of interest can be obtained. For any coordinate (x, y) within the region of interest, based on the weight vector w and the fitting function h(x, y), its elevation estimation can be obtained

[0118] ​

[0119] Specifically, the fitting function h(x, y) is applied to the entire region of interest to generate a ground elevation map grid {h(x, y)}. This ground elevation map contains information such as the slopes, depressions, and protrusions of local roads, providing feedforward data support for subsequent point cloud matching and robot positioning

[0121] Step S2: Use the inertial measurement unit data to initially estimate the pose transformation between the current frame and the previous frame of the robot to obtain the initial pose of the robot; combine the robot kinematic model to calculate the contact point coordinates of the four wheels of the robot on the ground elevation map, and based on the information of the ground elevation map generated by fitting, analyze the height error between the contact points and the ground elevation map, and output the robot pose transformation matrix and the contact point height error

[0122] In this embodiment, these error terms are used as part of the cost function of the optimization problem, and at the same time, the point cloud inter-frame matching error and the trajectory smoothness constraint are introduced to construct a comprehensive optimization problem

[0123] Specifically, step S2 includes the following sub-steps

[0124] Step S2.1: Use the inertial measurement unit (IMU) data to calculate the initial pose transformation of the current frame of the robot relative to the previous frame

[0125] T k = T k-1 ·ΔT IMU

[0126] where T k ∈ SE(3) represents the initial pose of the robot in the k-th frame; ΔT IMU represents the inter-frame pose increment calculated by the IMU

[0127] Step S2.2: According to the robot kinematic model, calculate the positions of the four wheel contact points in the robot body coordinate system where i = 1, 2, 3, 4 corresponds to the four wheels, and let the wheel coordinates be

[0128]

[0129] Convert the four wheel contact points to the world coordinate system

[0130]

[0131] where T k is the initial pose transformation matrix

[0132] Step S2.3: According to the ground elevation map generated by fitting Calculate the predicted height of the contact point on the ground elevation map

[0133]

[0134] The contact point height error is defined as:

[0135]

[0136] where is the height of the wheel in the world coordinate system calculated according to the robot pose and robot kinematics;

[0137] Step S2.4, use the contact point height error the point cloud inter-frame matching error e pc and the trajectory smoothness constraint e smooth as cost terms to construct a comprehensive optimization problem:

[0138]

[0139] where, w z , w pc , w smooth are weight coefficients. Through iterative optimization, solve the inter-frame pose transformation matrix T k .

[0140] Step S3, based on the robot pose transformation matrix and the contact point height error, use a factor graph to construct a comprehensive optimization problem and optimize the global pose and map of the robot.

[0141] In this embodiment, the factor graph takes the optimization results of each frame, point cloud data, elevation map information, and IMU data as factor nodes, and combines the joint probability model of inter-frame constraints and sensor data to construct a comprehensive optimization problem. Through the factor graph optimization algorithm at the backend, globally optimize the overall matching accuracy of the robot pose and map. During the optimization process, dynamically update the factor nodes and edges to ensure that the system can process newly added sensor data in real time and correct the global pose deviation and cumulative error.

[0142] Specifically, step S3 includes the following sub-steps:

[0143] Step S3.1, based on the robot pose transformation matrix and the contact point height error, construct a factor graph where contains the pose information X i ∈ SE(3) of the robot, the map points required for ground fitting and the factor set used to represent sensor observations and inter-frame constraints, etc.

[0144] Furthermore, the factor set It includes: an inter-frame constraint factor, an IMU factor, and an elevation map constraint factor;

[0145] The inter-frame pose transformation is provided by the robot pose transformation matrix output in step S2, and the inter-frame constraint factor is defined as:

[0146]

[0147] where, is the optimized inter-frame pose, represents the Mahalanobis weighted distance, and Σ odom represents the point cloud covariance.

[0148] The IMU data provides angular velocity and acceleration, which are used to constrain the rotation and displacement changes between adjacent frames. The IMU factor is defined as:

[0149]

[0150] where, represents the rotation change ΔR i ∈SO(3) of adjacent frames calculated from the IMU data and the velocity change h imu (X i-1 ,X i ) represents the predicted values deduced from the poses X i-1 and X i , including the rotation matrix and velocity; Σ imu represents the covariance matrix of the IMU data.

[0151] The elevation map constraint factor is used to constrain the matching between the robot contact points and the elevation map, and the robot pose is constrained by the fitted elevation map, which is defined as follows:

[0152]

[0153] where, is the height value obtained from the fitted elevation map, h elevation (X i ,L j ) is the elevation value deduced from the current pose X i and the map point L j , that is, in step S2.3. elevation Σ represents the covariance of the elevation estimation.

[0154] In step S3.2, based on the factor graph, the global pose and the map are iteratively optimized through a factor graph optimization algorithm (such as Levenberg-Marquardt or Dogleg).

[0155] Specifically, a comprehensive optimization problem is constructed through a factor graph:

[0156]

[0157] Among them, represents the pose information of the robot, represents the map points required for ground fitting, represents the height value obtained from the fitted elevation map. Substituting it into the factor, the expression expands as follows:

[0158]

[0159] After adding the factor graph to the new frame, the nodes and edges of the factor graph are dynamically updated, and the factor graph optimization algorithm (such as Levenberg-Marquardt or Dogleg) is used to iteratively optimize the global pose and the map.

[0160] Step S4: During the construction of the comprehensive optimization problem, a loop detection module is added to identify the historical positions passed by the robot and detect potential closed-loop areas to improve the accuracy and real-time performance of positioning and mapping.

[0161] In this embodiment, by detecting the similarity in the pose sequence or directly comparing the matching degree between the current point cloud and the historical point cloud, it is determined whether a loop is formed. Once a loop is detected, a loop constraint is constructed and added to the factor graph to trigger the closed-loop optimization process. The closed-loop optimization adjusts the global pose and map data, reduces the drift error, improves the accuracy and consistency of the global map, and provides higher positioning and mapping quality for the mobile robot running for a long time.

[0162] Specifically, step S4 includes the following steps:

[0163] Step S4.1: Project the current point cloud P t frame onto a two-dimensional polar coordinate grid to generate a ScanContext descriptor S t ;

[0164] Step S4.2: Compare the current frame descriptor S t with all historical frame descriptors Calculate the similarity between two frames using cosine similarity, and calculate the similarity score sim(S t , S i ):

[0165]

[0166] If there exists a frame j that satisfies sim(S t , S i ) > δ, it is considered that there may be a loop between frame t and j, where δ is the similarity threshold.

[0167] Step S4.3. Perform fine matching on the point cloud P of the candidate frame i and the point cloud P of the current frame t to calculate the relative pose transformation T between the two frames using a point cloud registration algorithm (NDT or ICP). j→t .

[0168] Step S4.4. When a loop closure is formed between the state at the current time t and the state at the historical time j, construct a loop closure factor:

[0169]

[0170] where X j is the state at the historical time j, and X t represents the state at the current time t. T j→t ∈ SE(3) represents the true value of the transformation matrix for transforming X j to X t . represents the estimated value of the transformation matrix for transforming X j to X t , and Σ loop represents the point cloud covariance. Add the loop closure factor to the factor graph and update the global map and the robot pose by minimizing the overall objective function.

[0171] Embodiment 2:

[0172] The present invention also provides a robot positioning and mapping system based on road elevation map feedforward. The robot positioning and mapping system based on road elevation map feedforward can be implemented by executing the process steps of the robot positioning and mapping method based on road elevation map feedforward. That is, those skilled in the art can understand the robot positioning and mapping method based on road elevation map feedforward as a preferred embodiment of the robot positioning and mapping system based on road elevation map feedforward.

[0173] Specifically, the robot positioning and mapping system based on road elevation map feedforward includes:

[0174] Module M1. Obtain the point cloud data in front of the robot through a lidar, and generate a ground elevation map using the radial basis function fitting method;

[0175] Module M2. Use the inertial measurement unit data to initially estimate the pose transformation between the current frame and the previous frame of the robot to obtain the initial pose of the robot; combine the robot kinematic model to calculate the contact point coordinates of the four wheels of the robot on the ground elevation map, and analyze the height error between the contact point and the ground elevation map based on the information of the generated ground elevation map, and output the robot pose transformation matrix and the contact point height error;

[0176] Module M3 constructs a comprehensive optimization problem based on the robot pose transformation matrix and the contact point height error, and optimizes the global pose and map of the robot using a factor graph.

[0177] Those skilled in the art know that in addition to implementing the system and its various devices, modules, and units provided by the present invention in the form of pure computer-readable program code, the method steps can be logically programmed to enable the system and its various devices, modules, and units provided by the present invention to be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers, etc., to achieve the same functions. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered as a hardware component, and the devices, modules, and units included therein for implementing various functions can also be regarded as the structure within the hardware component; the devices, modules, and units for implementing various functions can also be regarded as either software modules for implementing the method or the structure within the hardware component.

[0178] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Without conflict, the embodiments of the present application and the features in the embodiments can be combined arbitrarily with each other.

Claims

1. A robot positioning and mapping method based on road elevation map feedforward, characterized in that: The steps include: Step S1, obtaining point cloud data in front of the robot through a laser radar, and generating a ground elevation map using a radial basis function fitting method; Step S2, using the inertial measurement unit data to make an initial estimate of the posture transformation between the current frame and the previous frame of the robot to obtain the initial posture of the robot; combining the robot kinematic model to calculate the contact point coordinates of the robot's four wheels on the ground elevation map, and based on the ground elevation map information generated by fitting, analyzing the height error between the contact point and the ground elevation map, and outputting the robot posture transformation matrix and the contact point height error; Step S3, based on the robot posture transformation matrix and the contact point height error, a factor graph is used to construct a comprehensive optimization problem and optimize the robot's global posture and map; Step S4: In the process of constructing the comprehensive optimization problem, a loop detection module is added to identify the historical positions passed by the robot and detect potential closed-loop areas.

2. The robot positioning and mapping method based on road elevation map feedforward according to claim 1 is characterized in that: The step S1 includes the following sub-steps: Step S1.1, obtain the point cloud data of the current frame from the laser radar, denoted as P = {p i =(x i ,y i ,z i )|i=1,2,…K}, where each point p i Contains three-dimensional coordinate information (x i ,y i ,z i ); Step S1.2, select the driving direction of the robot, and select a part of the point cloud in front of the robot from the point cloud data, that is, the point cloud of the area of ​​interest In P ROI The area projected onto the XOY plane is uniformly filled with the center points of the radial basis function, using C = {c i =(c x,i ,c y,i )|i=1,2,…,N} represents the set of center points; Step S1.3, using the point cloud of the region of interest, generating a ground elevation map by a radial basis function fitting method.

3. The robot positioning and mapping method based on road elevation map feedforward according to claim 2 is characterized in that: The step S1.3 includes the following sub-steps: Step S1.3.1, use radial basis function to Model the ground elevation map and set the fitting function; Step S1.3.2, construct an objective function to minimize the point cloud P of the region of interest ROI Height error from the fitted plane: Where w=[w1,w2,…,w M ] T Expressed as a weight vector, h(x i ,y i ) represents the fitting function for the two-dimensional coordinates (x i ,y i ) elevation estimate, z i Represents a two-dimensional coordinate (x i ,y i ), that is, the actual elevation of the point cloud p i =(x i ,y i ,z i ) provides the actual height; Step S1.3.3, transform the optimization problem into linear algebraic form: Aw=z Among them, the element A of the kernel matrix A ij =φ(||(x i ,y i )-(c x,j ,c y,j )||), is the height vector of the point cloud, Step S1.3.4, use the least squares method to solve the weight vector w: w=(A T A) -1 A T z Step S1.3.5, the weight w calculated in step S1.3.4 is substituted into the fitting function h(x, y), that is, the ground elevation map model in the area of ​​interest is obtained. For any coordinate (x, y) in the area of ​​interest, the elevation estimate is obtained based on the weight vector w and the fitting function h(x, y):

4. The robot positioning and mapping method based on road elevation map feedforward according to claim 3 is characterized in that: In step S1.3.1, the fitting function is set to: Among them, (c x,j ,c y,j )∈C is the center point; w j is the weight coefficient to be determined; φ(r) is the radial basis function, using a Gaussian kernel: Among them, σ is the scale parameter of the kernel function, and r is the independent variable of the radial basis function. In this case, it refers to the Euclidean distance between two points.

5. The robot positioning and mapping method based on road elevation map feedforward according to claim 4 is characterized in that: The step S2 includes the following sub-steps: Step S2.1, use the inertial measurement unit (IMU) data to calculate the initial posture transformation of the robot in the current frame relative to the previous frame: T k =T k-1 ·ΔT IMU Among them, T k ∈SE(3) represents the initial position of the robot in the kth frame; ΔT IMU Represents the inter-frame pose increment calculated by IMU; Step S2.2: Calculate the positions of the four wheel contact points in the robot body coordinate system according to the robot kinematic model. Among them, i=1,2,3,4 correspond to four wheels, let the wheel coordinates be: Convert the four wheel contact points to the world coordinate system: Among them, T k is the initial pose transformation matrix; Step S2.3, ground elevation map generated by fitting Calculate the predicted height of the contact point on the ground elevation map The contact point height error is defined as: in, is the height of the wheel in the world coordinate system calculated based on the robot pose and robot kinematics; Step S2.4, the contact point height error Point cloud frame matching error e pc and trajectory smoothness constraint e smooth As the cost term, we construct the optimization problem: Among them, w z ,w pc ,w smooth is the weight coefficient, and the inter-frame pose transformation matrix T is solved through iterative optimization. k .

6. The robot positioning and mapping method based on road elevation map feedforward according to claim 5 is characterized in that: The step S3 includes the following sub-steps: Step S3.1, constructing a factor graph based on the robot posture transformation matrix and the contact point height error in Contains the robot's position information X i ∈SE(3), map points required for ground fitting and factor set Step S3.2, based on the factor graph, iteratively optimize the global pose and map through a factor graph optimization algorithm.

7. The robot positioning and mapping method based on road elevation map feedforward according to claim 6 is characterized in that: In step S3.1, the factor set Including: inter-frame constraint factor, IMU factor and elevation map constraint factor; The inter-frame pose transformation is provided by the robot pose transformation matrix output from step S2, and the inter-frame constraint factor is defined as: in, is the optimized inter-frame pose, represents the Mahalanobis weighted distance, Σ odom represents the point cloud covariance; The IMU data provides angular velocity and acceleration, which are used to constrain the rotation and displacement changes between adjacent frames. The IMU factor is defined as: in, represents the rotation change ΔR between adjacent frames calculated from the IMU data i ∈SO(3) and speed variation h imu (X i-1 ,X i ) represents the two-frame pose X i-1 and X i The estimated values, including rotation matrix and velocity; Σ imu Represents the covariance matrix of IMU data; The elevation map constraint factor is used to constrain the matching of the robot contact point and the elevation map, and the robot posture is constrained by the fitted elevation map, and is defined as follows: in, is the height value obtained from the fitted height map, h elevation (X i ,L j ) is the current pose X i and map point L j The calculated elevation value, i.e. the value in step S2.3 Σ elevation Represents the covariance of the elevation estimates.

8. The robot positioning and mapping method based on road elevation map feedforward according to claim 7 is characterized in that: In step S3.2, a comprehensive optimization problem is constructed through a factor graph: in, Represents the robot’s position information. represents the map points needed for ground fitting, Indicates the height value obtained by fitting the elevation map. Substituting into the factor, the expression is expanded as follows: After a new frame is added to the factor graph, the nodes and edges of the factor graph are dynamically updated, and the global pose and map are iteratively optimized using a factor graph optimization algorithm.

9. The robot positioning and mapping method based on road elevation map feedforward according to claim 8 is characterized in that: The step S4 comprises the following steps: Step S4.1, for the current point cloud P t The frame is projected to project the point cloud into a two-dimensional polar coordinate grid to generate the ScanContext descriptor S t ; Step S4.2, compare the current frame descriptor S t With all historical frame descriptors Use cosine similarity to calculate the similarity between two frames and calculate the similarity score sim(S t ,S i ): If there exists a frame j that satisfies sim(S t ,S i )>δ, it is considered that there is a loop between frames t and j, where δ is the similarity threshold; Step S4.3, point cloud P of the candidate frame i With the current frame point cloud P t Perform fine matching and use point cloud registration algorithm (NDT or ICP) to calculate the relative pose transformation T of the two frames j→t ; Step S4.4, when the state at the current time t forms a closed loop with the state at the historical time j, construct the loop factor: Among them, X j The state at historical moment j, X t Represents the state at the current time t, T j→t ∈SE(3) means X j Transform to X t The true value of the transformation matrix, Indicates X j Transform to X t The estimated value of the transformation matrix, Σ loop Represent the point cloud covariance, add the loop closure factor to the factor graph, and update the global map and robot pose by minimizing the overall objective function.

10. A robot positioning and mapping system based on road elevation map feedforward, characterized in that: include: Module M1 obtains point cloud data in front of the robot through lidar and generates a ground elevation map using radial basis function fitting method; Module M2 uses the inertial measurement unit data to make an initial estimate of the robot's posture transformation between the current frame and the previous frame to obtain the robot's initial posture; calculates the contact point coordinates of the robot's four wheels on the ground elevation map in combination with the robot's kinematic model, and analyzes the height error between the contact point and the ground elevation map based on the ground elevation map information generated by fitting, and outputs the robot's posture transformation matrix and the contact point height error; Module M3, based on the robot posture transformation matrix and the contact point height error, uses a factor graph to construct a comprehensive optimization problem and optimize the robot's global posture and map; Module M4, in the process of building a comprehensive optimization problem, adds a loop detection module to identify the historical positions passed by the robot and detect potential closed-loop areas.

Images