A method for constructing an elevation map based on frame-map matching

Abstract

The present invention discloses a method for constructing an elevation map based on frame-map matching. Using a 2.5D elevation map as the representation form of the map, it can provide a more complete modeling of the surrounding environment for the autonomous navigation planning of the robot. At the same time, the storage space is smaller and the traversal speed is faster; the registration of the frame and the map is added during the construction of the elevation map, reducing the deviation of the map construction caused by the positioning error; before the registration, the plane features of the point cloud to be registered are extracted, and the registration based on the plane features is used, which improves the accuracy and robustness of the registration result compared with the direct method registration; in addition, the construction framework of the elevation map is changed to a combination of a local elevation map and a global elevation map, and the construction step of the global elevation map is added, effectively eliminating the cumulative error brought by the construction process of the local elevation map and improving the accuracy of the global mapping. The present invention provides a sufficient perception basis for the autonomous movement of the legged robot in complex scenarios.

Description

Technical Field

[0001] The present invention belongs to the field of mobile robot map construction, and particularly relates to a method for constructing an elevation map based on frame-map matching. Background Art

[0002] Map construction is a way for mobile robots to perceive the environment. Currently, the mainstream map representation forms include grid maps, point cloud maps, feature maps, topological maps, etc.

[0003] For mobile robots, to achieve intelligence, it is first necessary to ensure the autonomy of the robot, and the realization of autonomy is mainly based on the robot's perception of the external environment. In an unknown environment, the robot has insufficient prior knowledge, and having the ability to perceive the environment is the most basic prerequisite for it to realize functions such as environmental modeling, positioning and navigation, and path planning. Currently, SLAM (Simultaneous Localization and Mapping) technology, as the mainstream method for mobile robot map construction, endows the robot with a certain degree of perception ability of the external environment to some extent, and is widely used in mobile robots such as service robots and inspection robots.

[0004] The general SLAM method usually constructs a sparse point cloud map, which does not contain complete environmental structure information, cannot reflect the real external environment, and cannot be directly used for the autonomous navigation planning of mobile robots. The realization of autonomous navigation planning is generally completed on a dense grid map. The 2.5D grid elevation map containing terrain elevation information, as a new type of grid map, can provide a more complete surrounding environment modeling for the autonomous navigation planning of the robot compared with the traditional 2D planar grid map. At the same time, compared with general three-dimensional environmental modeling methods, the storage space of the elevation map itself is also smaller. In recent years, due to these advantages of the elevation map, it has been widely used in the global or local navigation planning of mobile robots.

[0005] With the continuous improvement of the autonomy of legged robots, higher requirements are placed on the accuracy of elevation map construction. In practical applications, the autonomous navigation planning of legged robots involves the selection of foot landing points. To ensure smooth movement, the robot needs to avoid special terrain areas and choose flat areas to land. The prerequisite for accurately selecting the foot landing point is a high-precision and accurate map. There are many aspects to explore on how to improve the accuracy of elevation map construction to accurately reflect the real terrain. Currently, the mainstream elevation map construction methods are limited by the positioning accuracy of the robot and cannot guarantee the construction accuracy of local terrain. Summary of the Invention

[0006] The purpose of the present invention is to provide a method for constructing an elevation map based on frame-map matching in view of the deficiencies of the prior art.

[0007] The object of the present invention is achieved by the following technical solutions: A method for constructing an elevation map based on frame-map matching. First, RGB-D point clouds are obtained through a point cloud acquisition device, and at the same time, the current pose of the robot is obtained from the positioning system; operations of filtering and coordinate transformation are performed on the current point cloud frame; then feature extraction is performed on the current point cloud frame and the already constructed elevation map; then the pose of the current point cloud frame is adjusted; the local elevation map is updated, the input point cloud after pose adjustment is added to the local elevation map, and the updated local elevation map is published; it is judged whether the current frame meets the conditions of a key frame. If it meets the conditions of a key frame, it enters the processing flow of the global elevation map, otherwise it returns to the first step to obtain the next frame of RGB-D point cloud; the processing of the global elevation map is carried out, and the pose of the local elevation map is adjusted; then the local elevation map is added to the global elevation map, and finally the global elevation map is published.

[0008] Further, the operations of filtering and coordinate transformation on the current point cloud frame are specifically as follows:

[0009] (1.1) Perform a pass-through filter on the input point cloud to retain the point cloud within the effective range of the point cloud acquisition device.

[0010] (1.2) Perform voxel filtering downsampling on the input point cloud to make the resolution of the point cloud consistent with the grid resolution of the elevation map.

[0011] (1.3) Perform coordinate transformation on the filtered point cloud. Based on the current positioning information, transform the point cloud in the coordinate system of the point cloud acquisition device to the world coordinate system and map it to the map coordinate system.

[0012] Further, the feature extraction on the current point cloud frame and the already constructed elevation map is specifically as follows: Plane point extraction: Estimate the normal vector and curvature of the point cloud, and screen out the points that meet the threshold requirements as the in-plane points after plane feature extraction.

[0013] Further, the adjustment of the pose of the current point cloud frame is specifically as follows:

[0014] (3.1) Point cloud registration: Use the current point cloud frame after feature extraction as the source point cloud, and the already constructed elevation map after feature extraction as the target point cloud, and use the GICP method to perform registration of the two point clouds to obtain the pose transformation between them.

[0015] (3.2) Fault judgment: The obtained pose transformation matrix needs to meet the conditions that both the translation distance and the yaw change angle are less than a certain threshold to be judged as a valid registration result. If it is not judged as a valid registration result, return to step (3.1).

[0016] (3.3) Point cloud adjustment: According to the pose transformation matrix provided by the effective registration result, move the current input point cloud frame to the optimized position.

[0017] Furthermore, the updating of the local elevation map is specifically as follows:

[0018] (4.1) Calculate elevation measurement values: The elevation measurement values of the terrain follow a Gaussian distribution. The z - coordinate value of each point in the original input point cloud after adjustment represents the mean value of an elevation measurement value, and the variance of the elevation measurement value can be obtained by the following calculation formula.

[0019]

[0020] where, J s represents the Jacobian matrix measured by the point cloud acquisition device; J q represents the Jacobian matrix of the rotation of the point cloud acquisition device coordinate system around the map coordinate system; ∑ s represents the covariance matrix measured by the point cloud acquisition device; ∑ p,q represents the covariance matrix of the rotation of the point cloud acquisition device coordinate system around the inertial coordinate system;

[0021] (4.2) Multi - height data processing: If there are multiple elevation measurement values mapped to the same grid, calculate the Mahalanobis distance between each elevation measurement value and the value already stored respectively, select the measurement value with the largest Mahalanobis distance as the measurement value of the current grid, and ignore the remaining measurement values.

[0022] (4.3) Data fusion: If there is no elevation measurement value stored in the current grid, directly add the calculated elevation measurement value to the corresponding grid; if there is already an elevation measurement value stored in the current grid, use the Kalman filter to fuse the newly calculated elevation measurement value with the stored value.

[0023] Furthermore, the processing of the global elevation map is specifically as follows:

[0024] (5.1) Key frame judgment: The selection of key frames is based on the cumulative displacement of the robot's movement. The cumulative displacement of the robot's movement is calculated by the following formula:

[0025]

[0026] where, (x i , y i ) is the two - dimensional plane positioning data output by the positioning system at the previous moment, (x i+1 , y i+1) is the two-dimensional plane positioning data output by the positioning system at the current moment. When the cumulative displacement of the robot is greater than or equal to the threshold, the current point cloud frame is determined as a key frame, and step (5.2) is continued; if it is not a key frame, the global elevation map is not updated.

[0027] (5.2) Point cloud registration: Use the locally downsampled elevation map after voxel filtering as the source point cloud, and the constructed global point cloud map for positioning as the target point cloud. Use the ICP method to register the two point clouds, set the initial pose as the identity matrix, and find the pose transformation between them.

[0028] (5.3) Local elevation map adjustment: According to the pose transformation matrix provided by the effective registration result, move the local elevation map to the optimized position and copy it to the global elevation map.

[0029] Furthermore, the robot positioning method is hdl_localization using a multi-line lidar.

[0030] The beneficial effects of the present invention are as follows: The present invention uses a 2.5D elevation map as the representation form of the map. Compared with the traditional 2D planar grid map, it can provide a more complete surrounding environment modeling for the autonomous navigation planning of the robot. At the same time, compared with general three-dimensional environment modeling methods, it has a smaller storage space and a faster traversal speed; frame-to-map registration is added during the elevation map construction process, reducing the map construction deviation caused by positioning errors; plane feature extraction is performed on the point cloud to be registered before registration, and plane feature-based registration is used, which improves the accuracy and robustness of the registration result compared with direct method registration; in addition, the elevation map construction framework is changed to a combination of a local elevation map and a global elevation map, and the construction step of the global elevation map is added, effectively eliminating the cumulative error brought by the local elevation map construction process and improving the accuracy of global mapping. In addition, the elevation map constructed using the present invention can be directly used for the footfall point selection and planning of a legged robot, providing a sufficient perception basis for the autonomous movement of the legged robot in complex scenarios. Brief Description of the Drawings

[0031] Figure 1 is the flowchart of the elevation map construction method of the present invention;

[0032] Figure 2 is the system structure schematic diagram of an embodiment of the present invention. Detailed Embodiments

[0033] A method for constructing an elevation map based on frame-map matching according to the present invention realizes the real-time and accurate construction of the surrounding environment of the robot and has been experimentally tested on a legged robot.

[0034] Figure 2An exemplary instance system structure applicable to the method of the present invention is shown. This structure can be divided into the environment to be constructed 205 and the legged robot 204. The main components in the legged robot related to the method of the present invention are the point cloud acquisition device 201, the positioning sensor 202, and the host 203. In the usage example of the present invention, they are respectively: the point cloud acquisition device: Intel RealSense D435i, the positioning sensor: Velodyne VLP-16, and the host: NUC i7 BEH.

[0035] Among them, the host obtains the real-time point cloud frames collected by the point cloud acquisition device in real time. At the same time, it obtains the current pose of the robot from the data obtained by the positioning sensor in real time, constructs an elevation map in the host, and finally publishes it. The point cloud processing and mapping processes of the present invention are all completed on the host.

[0036] It should be noted that Figure 2 the above system structure is only used as an example for the method of the present invention, and the present invention is not limited thereto.

[0037] As Figure 1 shown, the present invention specifically includes the following steps:

[0038] Step S101, obtain RGB-D point cloud through the point cloud acquisition device. The Intel RealSense D435i depth camera is connected to the NUC host through the USB3.0 interface. It obtains real-time RGB image data with a resolution of 640×480 and depth image data with a resolution of 640×480, and integrates the two into RGB-D point cloud data, with a capture frame rate of 30 frames per second.

[0039] Step S102, obtain the pose of the current robot in the map coordinate system from the positioning system. The method for obtaining the robot pose is hdl_localization, which is based on the Velodyne VLP-16 lidar and is located in the constructed point cloud map.

[0040] Step S103, filter the current point cloud frame and perform coordinate transformation on the point cloud according to the current pose. The processing steps for the current point cloud frame in this step include the following operations:

[0041] (3.1) Perform a pass-through filter on the input point cloud to retain the point cloud within the effective range of the point cloud acquisition device. The point cloud acquisition accuracy of the Intel RealSense D435i is poor outside the effective range. Therefore, a pass-through filter is used to retain the point cloud within the range of 0.2m - 2m and discard the point cloud outside the threshold range.

[0042] (3.2) Perform voxel filtering downsampling on the input point cloud to make the resolution of the point cloud consistent with the grid resolution of the elevation map. In this example, the resolution of the voxel filtering is set to 2 cm, which is consistent with the grid resolution of the elevation map.

[0043] (3.3) Perform coordinate transformation on the filtered point cloud. Based on the current positioning information, transform the point cloud in the coordinate system of the point cloud acquisition device to the world coordinate system and map it to the map coordinate system. The elevation measurement value p of the point after mapping in the map coordinate system can be calculated by the following formula:

[0044]

[0045] P = [0 0 1]

[0046] where C SM refers to the rotation transformation matrix between the map coordinate system and the sensor coordinate system, and q is a unit quaternion used to parameterize this transformation matrix; S r SP refers to the position of the measurement point in the sensor coordinate system, M r SM refers to the position of the origin of the sensor coordinate system in the map coordinate system.

[0047] Step S104: Extract features from the current point cloud frame and the constructed elevation map, and extract the plane points in the current point cloud frame. Based on the current point cloud frame processed in step S103, perform feature extraction on it again. The specific operation is as follows: Estimate the normal vector and curvature of the point cloud, and filter out the points whose z-axis normal vector is greater than a certain threshold as the in-plane points after plane feature extraction. Retain the in-plane points and discard the out-of-plane points for use in the following steps.

[0048] Using the feature extraction method of this example, perform plane feature extraction on the point cloud to be registered before registration, so that the following steps can use plane feature-based registration, improving the accuracy and robustness of the registration result;

[0049] Step S105: Adjust the pose of the current point cloud frame. Based on the current point cloud frame and the constructed elevation map after feature extraction in step S104, use the point cloud registration method to find the overlapping part of the two point clouds and find the pose transformation that can maximize the coincidence degree of the two point clouds. The specific method is as follows: Use the current point cloud frame after feature extraction as the source point cloud and the constructed elevation map after feature extraction as the target point cloud, and use the GICP method to register the two point clouds to solve the pose transformation that maximizes the coincidence degree between them. Among them, the initial pose for solving is set to the identity matrix.

[0050] Furthermore, since there is a possibility of failure in solving the pose transformation using the GICP method, an operation for fault judgment is added in step S105. The obtained pose transformation matrix needs to satisfy the following three conditions simultaneously to be judged as a valid registration result. If it is not judged as a valid registration result, the above point cloud registration steps are repeated.

[0051] 1. The horizontal direction transformation displacement < 0.1 m;

[0052] 2. The vertical direction transformation displacement < 0.2 m;

[0053] 3. The Yaw angle transformation angle < 10°.

[0054] Furthermore, according to the pose transformation matrix provided by the valid registration result, the original input point cloud frame is moved to the optimized position. At this time, the coincidence degree between the original input point cloud frame and the constructed elevation map is the largest.

[0055] Using the frame-to-map registration method in step S105 can reduce the map construction deviation caused by positioning errors, make the constructed elevation map more conform to the real environment, and ensure the accuracy of map construction.

[0056] Step S106, update and publish the local elevation map. The local elevation map is a 2.5D grid map that can store elevation information. Using the 2.5D elevation map as the map representation form can provide a more complete surrounding environment modeling for the autonomous navigation planning of the robot compared with the traditional 2D plane grid map. At the same time, compared with the general three-dimensional environment modeling method, it has a smaller storage space and a faster traversal speed. This map is centered on the robot coordinate system, is updated in real time, and can reflect the three-dimensional environment around the robot. To ensure the real-time performance and accuracy of the local elevation map construction, in this embodiment, the resolution of the elevation map is set to 2 cm, that is, the size of each grid unit is 2 cm × 2 cm; the size of the local elevation map is set to 5 m × 5 m.

[0057] To update the elevation map based on the current input point cloud frame, it is necessary to first iteratively calculate the elevation measurement value represented by each point in the point cloud in the map coordinate system, find the grid unit mapped by each elevation measurement value, and then perform data fusion based on the calculated elevation measurement value and the historical measurement value stored in the corresponding grid to obtain the updated value.

[0058] Furthermore, the specific steps for updating the elevation map are as follows:

[0059] (6.1) Calculate the elevation measurement value: The present invention assumes that the elevation measurement value of the terrain follows a Gaussian distribution The z coordinate value of each point in the original input point cloud after adjustment in the map coordinate system represents the mean value of an elevation measurement value, and the variance of the elevation measurement value It can be obtained from the following calculation formula:

[0060]

[0061] Among them, J s represents the Jacobian matrix measured by the point cloud acquisition device, and J q represents the Jacobian matrix of the rotation of the coordinate system of the point cloud acquisition device around the map coordinate system; ∑ s represents the covariance matrix measured by the point cloud acquisition device, and ∑ p，q represents the covariance matrix of the rotation of the coordinate system of the point cloud acquisition device around the inertial coordinate system.

[0062] (6.2) Multi-height data processing: This step is used to process the special case where multiple elevation measurements are mapped to the same grid. If this situation occurs, calculate the Mahalanobis distance between each elevation measurement and the value that has been stored respectively, select the measurement with the largest Mahalanobis distance as the measurement value of the current grid, and ignore the remaining measurement values.

[0063] (6.3) Data fusion: If there is no elevation measurement value stored in the current grid, directly add the calculated elevation measurement value to the corresponding grid; if there is already an elevation measurement value stored in the current grid then use the Kalman filter to fuse the newly calculated elevation measurement value with the stored one:

[0064]

[0065] Among them, the one with the superscript + represents the new measurement value after fusion, and the one with the superscript - represents the measurement value stored in the map.

[0066] (6.4) Publish the updated local elevation map. Send the local elevation map to the system in the format of ROS topic in real time, and subsequent nodes can obtain the real-time local elevation map data through subscription. The publishing frame rate of the local elevation map is set to 10 frames per second.

[0067] In step S107, determine whether the current point cloud frame is a key frame. The determination of the key frame is mainly based on the cumulative displacement of the robot's movement. The cumulative displacement D of the robot's movement is calculated by the following formula:

[0068]

[0069] Among them, (x i , y i ) is the two-dimensional plane positioning data output by the positioning system at the previous moment i, (x i+1 , y i+1) is the two-dimensional plane positioning data output by the positioning system at the current moment i+1; N represents the number of accumulated positioning frames at the current moment. When the accumulated displacement of the robot is greater than or equal to the threshold, the current point cloud frame is determined as a key frame, and the subsequent global elevation map update step is entered. In this embodiment, the threshold of the accumulated displacement is set to 2m. If it is determined that it is not a key frame, jump to step S110.

[0070] Step S108, perform pose adjustment on the local elevation map. Based on the local elevation map constructed in step S106 and the constructed global point cloud map for positioning, use the point cloud registration method to find the overlapping part of the two point clouds, and obtain the pose transformation that can maximize the coincidence degree of the two point clouds. The specific method is: use the local elevation map downsampled by voxel filtering as the source point cloud, use the constructed global point cloud map for positioning as the target point cloud, use the ICP method to register the two point clouds, and solve the pose transformation that maximizes the coincidence degree between the two, where the initial pose for solving is set to the identity matrix.

[0071] Furthermore, according to the pose transformation matrix provided by the registration result, move the constructed local elevation map to the optimized position. At this time, the coincidence degree between the constructed local elevation map and the global point cloud map is the largest.

[0072] Step S109, update and publish the global elevation map. This step introduces the concept of the global elevation map, and changes the construction framework of the elevation map to a combination of the local elevation map and the global elevation map. The main advantage is to effectively eliminate the map cumulative error brought by the local elevation map construction process.

[0073] The global elevation map published in step S109 has the following differences compared with the local elevation map published in step S106:

[0074] 1. The size of the local elevation map is constant, which is 5m×5m; while the overall size of the global elevation map is uncertain and will increase as the construction range expands.

[0075] 2. The local elevation map is centered on the robot coordinate system, and the construction range moves with the movement of the robot, reflecting the local environmental information around the robot; while the global elevation map is fixed in the map coordinate system and does not move with the movement of the robot, reflecting the global environmental information.

[0076] 3. The local elevation map is updated and published in real time, and the publishing frame rate is constant at 10 frames per second; while the global elevation map has a large range and contains more information. To ensure the real-time operation of the program, the update of the global elevation map is set to be triggered by key frame conditions, and the update and publishing of the global elevation map are triggered according to the judgment conditions in step S107. Therefore, the update and publishing of the global elevation map are non-real-time.

[0077] Further, the steps for global elevation map update are as follows:

[0078] (9.1) Adding local elevation map: Copy the local elevation map adjusted in pose in step S108 into the global elevation map. Since the grid cell resolutions of the local elevation map and the global elevation map are the same, and the grid cell positions of the two can also correspond one by one, the elevation measurement values stored in the grid of the global elevation map can be updated according to the values stored in the corresponding grid of the local elevation map.

[0079] (9.2) Publishing the updated global elevation map. Send the global elevation map to the system in the format of ROS topic, and subsequent nodes can obtain the global elevation map data through subscription. The local elevation map can also be saved to the local host through ROS service operations.

[0080] Step S110: End the entire process, return to step S101, and obtain the next frame of RGB-D point cloud.

Claims

1. A method for constructing elevation maps based on frame-map matching, It is characterized in that First, obtain the RGB-D point cloud through the point cloud acquisition device, and obtain the current robot posture from the positioning system; filter and transform the current point cloud frame; extract features from the current point cloud frame and the constructed elevation map; adjust the posture of the current point cloud frame; update the local elevation map, add the input point cloud with posture adjustment to the local elevation map, and publish the updated local elevation map; determine whether the current frame meets the key frame conditions. If it meets the key frame conditions, enter the global elevation map processing flow, otherwise return to the first step to obtain the next frame of RGB-D point cloud; process the global elevation map, adjust the posture of the local elevation map; then add the local elevation map to the global elevation map, and finally publish the global elevation map; The posture adjustment of the current point cloud frame is specifically as follows: (3.1) Point cloud registration: The current point cloud frame after feature extraction is used as the source point cloud, and the constructed elevation map after feature extraction is used as the target point cloud. The GICP method is used to register the two point clouds and calculate the pose transformation between them. (3.2) Fault judgment: The obtained pose transformation matrix needs to satisfy the conditions that the translation distance and the yaw change angle are less than a certain threshold at the same time before it can be judged as a valid registration result; if it is not judged as a valid registration result, return to step (3.1); (3.3) Point cloud adjustment: according to the pose transformation matrix provided by the effective registration result, the current input point cloud frame is moved to the optimized position; The processing of the global elevation map is specifically as follows: (5.1) Key frame judgment: The key frame is selected based on the cumulative displacement of the robot motion. The cumulative displacement of the robot motion is calculated by the following formula: Among them, (x i , y i ) is the two-dimensional plane positioning data output by the positioning system at the previous moment, and (x i+1 , y i+1 ) is the two-dimensional plane positioning data output by the positioning system at the current moment; when the cumulative displacement of the robot is greater than or equal to the threshold, the current point cloud frame is determined as a key frame, and step (5.2) is continued; if it is not a key frame, the global elevation map is not updated; (5.2) Point cloud registration: The local elevation map that has been downsampled by voxel filtering is used as the source point cloud, and the global point cloud map that has been constructed for positioning is used as the target point cloud. The ICP method is used to register the two point clouds. The initial pose is set to the unit matrix, and the pose transformation between the two is calculated. (5.3) Local elevation map adjustment: According to the pose transformation matrix provided by the effective registration result, the local elevation map is moved to the optimized position and copied to the global elevation map.

2. The method for constructing an elevation map based on frame-map matching as claimed in claim 1, It is characterized in that The operations of filtering and coordinate transformation of the current point cloud frame are specifically as follows: (1.1) Perform direct filtering on the input point cloud to retain the point cloud within the effective range of the point cloud acquisition device; (1.2) Perform voxel filtering and downsampling on the input point cloud to make the resolution of the point cloud consistent with the grid resolution of the elevation map; (1.3) Perform coordinate transformation on the filtered point cloud. Based on the current positioning information, transform the point cloud in the point cloud acquisition device coordinate system into the world coordinate system and map it to the map coordinate system.

3. The method for constructing an elevation map based on frame-map matching as claimed in claim 1, It is characterized in that The feature extraction of the current point cloud frame and the constructed elevation map is specifically the extraction of plane points: the normal vector and curvature of the point cloud are estimated, and the points that meet the threshold requirements are filtered out as the in-plane points after plane feature extraction.

4. The method for constructing an elevation map based on frame-map matching according to claim 1, characterized in that the update of the local elevation map is specifically as follows: (4.1) Calculate the elevation measurement value: the elevation measurement value of the terrain follows a Gaussian distribution; the z coordinate value of each point in the original input point cloud after adjustment represents the mean value of an elevation measurement value, and the variance of the elevation measurement value can be obtained by the following calculation formula; Among them, J s represents the Jacobian matrix measured by the point cloud acquisition device; J q represents the Jacobian matrix of the rotation of the point cloud acquisition device coordinate system around the map coordinate system; Σ s represents the covariance matrix measured by the point cloud acquisition device; Σ p,q represents the covariance matrix of the rotation of the point cloud acquisition device coordinate system around the inertial coordinate system; (4.2) Processing of multi-height data: If there are multiple elevation measurement values mapped to the same grid, calculate the Mahalanobis distance between each elevation measurement value and the stored value respectively, and select the measurement value with the largest Mahalanobis distance as the measurement value of the current grid, and ignore the remaining measurement values; (4.3) Data fusion: If there is no stored elevation measurement value in the current grid, directly add the calculated elevation measurement value to the corresponding grid; if there is already a stored elevation measurement value in the current grid, use the Kalman filter to fuse the newly calculated elevation measurement value with the stored value.

5. The method for constructing an elevation map based on frame-map matching according to claim 1, characterized in that the robot positioning method is hdl_localization using a multi-line lidar.

Images