Dynamic environment-oriented SLAM map updating method, system and equipment and medium

Abstract

The invention belongs to the field of robot positioning, and particularly relates to a dynamic environment-oriented SLAM map updating method, system and device and a medium, and the method comprises the steps: obtaining sensor data of a robot, and generating an environment map based on the sensor data; the method comprises the following steps: acquiring point cloud data of an environment through a local laser radar, and creating a new sub-map based on the point cloud data; meanwhile, sensor data are input into a global laser radar matching module to calculate relative constraints between the scanning point cloud and a global sub-map, and the relative constraint relation between the scanning point cloud and the map is obtained through calculation; and the pose map optimization module receives the new sub-map, receives the constraint relationship from the global laser radar matching module, processes and prunes the received sub-map and the constraint relationship, stores the old sub-map in the historical map, and performs pose map sparsification and optimization to complete map updating. Fusion of multi-source sensor data is fully utilized, the noise influence in a dynamic environment is reduced, and the accuracy of a map is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot positioning, and particularly relates to a SLAM map update method, system, device and medium for a dynamic environment. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) technology is one of the core technologies for robot autonomous navigation. Its purpose is to enable a robot to sense through its own sensors in an unknown environment, construct an environmental map in real time, and determine its own position in the map. SLAM technology is widely used in fields such as autonomous driving vehicles, service robots, drones, and ground robots.

[0003] The robot obtains environmental information through a lidar, uses feature extraction and matching technologies to identify key feature points in the environment, and estimates its own position and the environmental map through matching. Once the environment changes, the matching will deviate, and the robot positioning will be inaccurate or directly lost. Therefore, the robot needs to update the map to ensure positioning accuracy to cope with changes in the dynamic environment.

[0004] In a dynamic environment, traditional SLAM map update methods are difficult to balance the accuracy and real-time performance of the map while ensuring computational efficiency, and cannot meet the high requirements for map update in a dynamic environment. Summary of the Invention

[0005] In order to meet the accurate positioning of a robot in a dynamic environment, the present invention provides a SLAM map update method, system, device and medium for a dynamic environment.

[0006] In a first aspect, the technical solution of the present invention provides a SLAM map update method for a dynamic environment, including: Obtain sensor data of the robot, and generate an environmental map based on the sensor data; When performing a positioning task, collect point cloud data of the environment through a local lidar, and create a new sub-map based on the point cloud data; transmit the created new sub-map to the pose graph optimization module; At the same time, input the sensor data into the global lidar matching module to calculate the relative constraint between the scanned point cloud and the global sub-map, and output the calculated relative constraint relationship between the scanned point cloud and the map to the pose graph optimization module; The pose graph optimization module receives the new sub-map, receives the constraint relationship from the global lidar matching module, processes the received sub-map and the constraint relationship, prunes and saves the old sub-maps in the historical map, performs pose graph sparsification and optimization, and completes the update of the environmental map.

[0007] As a further limitation of the technical solution of the present invention, the steps of obtaining the sensor data of the robot and generating an environmental map based on the sensor data include: Obtain the point cloud data collected by the lidar, the acceleration and angular velocity data collected by the IMU, and the wheel rotation information recorded by the wheel encoder; Process the point cloud data obtained by the lidar, the acceleration and angular velocity data collected by the IMU, and the wheel rotation information recorded by the wheel encoder respectively, and align the data of the IMU, lidar, and wheel encoder through timestamp synchronization; Fuse the processed lidar, IMU, and wheel encoder data to obtain the robot pose information; Based on the fused pose information and lidar point cloud data, construct an occupancy grid sub-map; specifically including: dividing the space into grids, determining the occupancy situation of each grid according to the point cloud data, if there is point cloud in the grid, mark it as occupied, and if no point cloud is detected in the grid for a set time, mark it as free, forming the current environmental map composed of multiple occupancy grid sub-maps; each sub-map includes a fixed number of lidar scan data with corresponding poses.

[0008] As a further limitation of the technical solution of the present invention, the steps of processing the point cloud data obtained by the lidar, the acceleration and angular velocity data collected by the IMU, and the wheel rotation information recorded by the wheel encoder respectively include: Preprocess the point cloud data obtained by the lidar and then perform point cloud registration, match the point clouds at adjacent times, and determine the relative pose transformation between the point clouds; Perform integral operations on the acceleration and angular velocity data collected by the IMU to obtain the speed and attitude changes of the robot in each direction; According to the wheel rotation information recorded by the wheel encoder, calculate the moving distance and angle change of the robot, and convert the pulse number into the actual displacement and rotation amount.

[0009] As a further limitation of the technical solution of the present invention, when performing the positioning task, the steps of collecting the point cloud data of the environment through the local lidar and creating a new sub-map based on the point cloud data include: When performing the positioning task, obtain the point cloud data of the surrounding environment collected by the local lidar; Preprocess the collected raw point cloud data, and extract features from the preprocessed point cloud data; Match the point cloud features of the current frame with the point cloud features in the existing local map. Take the point cloud in the local map as the target point cloud and the point cloud of the current frame as the source point cloud. By finding corresponding point pairs, calculate the transformation matrix that can make the source point cloud coincide with the target point cloud after transformation, so as to obtain the current pose estimation of the robot; According to the pose estimation result, taking the current pose of the robot as the reference, convert the point cloud data to a unified coordinate system and add it to the new sub-map.

[0010] As a further limitation of the technical solution of the present invention, the steps of transmitting the newly created sub-map to the pose graph optimization module include: Identify the key information of the newly created sub-map and record the unique identifier of this sub-map; Sort out the pose information of the sub-map, including the pose information of the sub-map in the global coordinate system; Count the number of point clouds and the number of features in the sub-map; Match the new sub-map with other sub-maps already existing in the pose graph to find the overlapping area between the new sub-map and other sub-maps; Within the overlapping area, calculate the relative pose transformation between the new sub-map and other sub-maps; Package and transmit the new sub-map data, including the identifier, pose information, point cloud and feature information of the sub-map, to the pose graph optimization module.

[0011] As a further limitation of the technical solution of the present invention, the steps of inputting sensor data into the global lidar matching module to calculate the relative constraints between the scanned point cloud and the global sub-map and outputting the calculated relative constraint relationship between the scanned point cloud and the map to the pose graph optimization module include: Input the lidar, IMU and wheel encoder data into the global lidar matching module for processing; In the global map, match the processed lidar scanned point cloud with the map model in the global map to determine the position and pose of the scanned point cloud in the global map; According to the matching result, calculate the relative measurement constraints between the scanned point cloud and the map; including relative position constraints and relative pose constraints; Transmit the calculated relative position constraint and relative pose constraint data to the pose graph optimization module.

[0012] As a further limitation of the technical solution of the present invention, the steps for the pose graph optimization module to receive the new sub-map from the local lidar, receive the constraint relationship from the global lidar matching module, process the received sub-map and constraint relationship, prune and save the old sub-maps in the historical map, perform pose graph sparsification and optimization, and complete the environmental map update include: The pose graph optimization module receives the newly created sub - maps from the local lidar, parses the received sub - map data, and extracts the position and feature information of the sub - maps; Receives the constraint relationships from the global lidar matching module, and classifies and organizes the constraint relationships according to the types of constraints and the sub - maps involved; Associates the newly received sub - maps with the sub - maps already existing in the pose graph, and determines the positions of the new sub - maps in the pose graph by comparing the poses and feature information of the sub - maps; Integrates the newly received constraint relationships with the existing constraint relationships in the pose graph; Evaluates the old sub - maps in the historical map, determines the sub - maps to be trimmed and marks them; For the sub - maps marked to be trimmed, removes the relevant nodes and the constraint relationships connecting the nodes from the pose graph. After completing the constraint removal, updates the data structure of the pose graph; Calculates the error between the actual poses of each pair of sub - maps with constraint relationships and the poses that should be according to the constraints, accumulates the squares of all errors to establish an objective function, and adjusts the poses of the sub - maps by minimizing the objective function. When the convergence condition is met, updates the optimized sub - map pose information to the global map data structure to complete the environmental map update.

[0013] In a second aspect, the technical solution of the present invention also provides a SLAM map update system for a dynamic environment, including a map generation module, a sub - map creation processing module, a global lidar matching module, and a pose graph optimization module; The map generation module is used to obtain the sensor data of the robot and generate an environmental map based on the sensor data; The sub - map creation module is used to, when performing the positioning task, collect the point cloud data of the environment through the local lidar and create a new sub - map based on the point cloud data, and transmit the newly created sub - map to the pose graph optimization module; The global lidar matching module is used to receive the sensor data, calculate the relative constraints between the scanned point cloud and the global sub - map, and output the calculated relative constraint relationships between the scanned point cloud and the map to the pose graph optimization module; The pose graph optimization module is used to receive the new sub - maps, receive the constraint relationships from the global lidar matching module, process the received sub - maps and constraint relationships, trim and save the old sub - maps in the historical map, and perform pose graph sparsification and optimization to complete the environmental map update.

[0014] As a further limitation of the technical solution of the present invention, the map generation module includes a data acquisition unit, a first data processing unit, a data fusion unit, and a map generation unit; A data acquisition unit for acquiring point cloud data collected by a lidar, acceleration and angular velocity data collected by an IMU, and wheel rotation information recorded by a wheel encoder; A first data processing unit for separately processing the point cloud data obtained by the lidar, the acceleration and angular velocity data collected by the IMU, and the wheel rotation information recorded by the wheel encoder, and aligning the data of the IMU, lidar, and wheel encoder through timestamp synchronization; A data fusion unit for fusing the processed lidar, IMU, and wheel encoder data to obtain robot pose information; A map generation unit for constructing an occupancy grid sub-map based on the fused pose information and lidar point cloud data; specifically including: dividing the space into grids, determining the occupancy situation of each grid according to the point cloud data, if there is point cloud in the grid, marking it as occupied, and if no point cloud is detected in the grid for a set time, marking it as free, forming a current environment map composed of multiple occupancy grid sub-maps; each sub-map includes a fixed number of lidar scan data with corresponding poses.

[0015] As a further limitation of the technical solution of the present invention, the first data processing unit is specifically used for performing point cloud registration after preprocessing the point cloud data obtained by the lidar, matching the point clouds at adjacent times, and determining the relative pose transformation between the point clouds; performing integral operations on the acceleration and angular velocity data collected by the IMU to obtain the speed and attitude changes of the robot in each direction; calculating the moving distance and angular change of the robot according to the wheel rotation information recorded by the wheel encoder, and converting the pulse number into actual displacement and rotation amount.

[0016] As a further limitation of the technical solution of the present invention, the sub-map creation processing module includes a point cloud data acquisition unit, a preprocessing unit, a pose estimation unit, and a sub-map creation unit; The point cloud data acquisition unit is used for acquiring the point cloud data of the surrounding environment collected by a local lidar when performing a positioning task; The preprocessing unit is used for preprocessing the collected raw point cloud data and extracting features from the preprocessed point cloud data; The pose estimation unit is used for matching the point cloud features of the current frame with the point cloud features in the existing local map, using the point cloud in the local map as the target point cloud and the point cloud of the current frame as the source point cloud, and calculating the transformation matrix that can make the source point cloud coincide with the target point cloud after transformation by finding corresponding point pairs, so as to obtain the current pose estimation of the robot; The sub-map creation unit is used for converting the point cloud data into a unified coordinate system based on the pose estimation result with the current pose of the robot as the reference and adding it to a new sub-map.

[0017] As a further limitation of the technical solution of the present invention, the sub-map creation processing module further includes an information processing unit and a sub-map transmission unit; The information processing unit is used to identify the key information of the newly created sub-map and record the unique identifier of the sub-map; sort out the pose information of the sub-map, including the pose information of the sub-map in the global coordinate system; count the number of point clouds and the number of features in the sub-map; match the new sub-map with other sub-maps already existing in the pose map to find the overlapping area between the new sub-map and other sub-maps; within the overlapping area, calculate the relative pose transformation between the new sub-map and other sub-maps; The sub-map transmission unit is used to pack and transmit the new sub-map data, including the identifier, pose information, point cloud and feature information of the sub-map, to the pose map optimization module.

[0018] As a further limitation of the technical solution of the present invention, the global lidar matching module includes a data receiving unit, a second data processing unit, a pose information determination unit, a constraint relationship determination unit and a constraint transmission unit; The data receiving unit is used for lidar, IMU and wheel encoder data; The second data processing unit is used to process the lidar point cloud data; The pose information determination unit is used to match the processed lidar scan point cloud with the map model in the global map in the global map to determine the position and pose of the scan point cloud in the global map; The constraint relationship determination unit is used to calculate the relative measurement constraints between the scan point cloud and the map according to the matching result; including relative position constraints and relative pose constraints; The constraint transmission unit is used to transmit the calculated relative position constraint and relative pose constraint data to the pose map optimization module.

[0019] As a further limitation of the technical solution of the present invention, the pose map optimization module includes an information receiving and processing unit, a position determination unit, an integration unit, a marking unit, a sparsification unit and an optimization unit; The information receiving and processing unit is used to receive the newly created sub-map, parse the received sub-map data, and extract the position and feature information of the sub-map; receive the constraint relationship from the global lidar matching module, and classify and sort out the constraint relationship according to the type of constraint and the sub-maps involved; The position determination unit is used to associate the newly received sub-map with the sub-maps already existing in the pose map, and determine the position of the new sub-map in the pose map by comparing the pose and feature information of the sub-maps; The integration unit is used to integrate the newly received constraint relationship with the existing constraint relationship in the pose map; A marking unit, configured to evaluate old sub - maps in a historical map, determine sub - maps to be trimmed and mark them; A sparsification unit, configured to remove relevant nodes and the constraint relationships connecting the nodes from the pose graph for the sub - maps marked to be trimmed. After completing the constraint removal, update the data structure of the pose graph; An optimization unit, configured to calculate the error between the actual pose and the pose that should be according to the constraint for each pair of sub - maps with constraint relationships, accumulate the squares of all errors to establish an objective function, and adjust the poses of the sub - maps by minimizing the objective function. When the convergence condition is met, update the optimized sub - map pose information into the global map data structure to complete the environmental map update.

[0020] In a third aspect, the technical solution of the present invention further provides an electronic device, which includes: at least one processor; and a memory communicatively connected to the at least one processor; the memory stores computer program instructions executable by the at least one processor, and the computer program instructions are executed by the at least one processor so that the at least one processor can execute the SLAM map update method for a dynamic environment as described in the first aspect.

[0021] In a fourth aspect, the technical solution of the present invention further provides a non - transitory computer - readable storage medium, which stores computer instructions that cause the computer to execute the SLAM map update method for a dynamic environment as described in the first aspect.

[0022] The beneficial effects of the technical solution of the present invention are as follows: By acquiring the sensor data of the robot and generating an environmental map based on this, making full use of the fusion of multi - source sensor data, it can effectively reduce the influence of noise and interference in a dynamic environment and improve the accuracy of the map. For example, by combining sensor data such as lidar, IMU, and wheel encoders, the perception ability of environmental information is enhanced, thereby constructing a more accurate environmental map.

[0023] When performing a positioning task, creating a new sub - map based on the point cloud data collected by a local lidar can reflect the changes in the dynamic environment in real time. Through point cloud processing and feature extraction algorithms, the speed and accuracy of sub - map creation are improved, enabling the robot to adapt to environmental changes in a timely manner.

[0024] Transmitting the new sub - map to the pose graph optimization module and combining the relative constraint relationships calculated by the global lidar matching module realizes the effective processing of the sub - map and the constraint relationships. The pose graph optimization module processes the received information, trims and saves the old sub - maps in the historical map, and performs pose graph sparsification and optimization, which can improve the calculation efficiency while ensuring the map accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] To more clearly illustrate the technical solution of the present invention, the accompanying drawings required in the description will be briefly introduced below. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other accompanying drawings can be obtained based on these drawings.

[0026] Figure 1 It is a schematic flowchart of the method provided by the embodiment of the present invention.

[0027] Figure 2 It is a schematic block diagram of the system provided by the embodiment of the present invention. Detailed implementation manners

[0028] To make the objectives, features, and advantages of the present invention more obvious and understandable, the technical solutions in the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the specific embodiments of the present invention. Obviously, the embodiments described below are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative efforts fall within the scope of protection of this application.

[0029] As Figure 1 shown, the embodiment of the present invention provides a SLAM map update method for a dynamic environment, including: S1: Obtain the sensor data of the robot, and generate an environmental map based on the sensor data; In a brand-new environment, the robot starts the data collection process. The collected sensor data includes lidar data, IMU (Inertial Measurement Unit) data, and wheel encoder data. These data are the basic information for constructing the map. Based on the collected multi-source sensor data, a map representation of the current environment is constructed. The map adopts a structure composed of multiple occupancy grid sub-maps. Each sub-map contains a fixed number of lidar scan data, and these scan data each carry corresponding pose information. In this way, each sub-map can accurately record the environmental characteristics and corresponding position information of a specific area, ensuring the integrity and accuracy of the overall map.

[0030] After the map construction stage is completed, the robot immediately switches to the execution of the positioning task and needs to execute steps S2 and S3; S2: Collect the point cloud data of the environment through the local lidar, and create a new sub-map based on the point cloud data; transmit the created new sub-map to the pose graph optimization module; During the positioning process, with the help of a local lidar, the robot creates a new sub-map in real time. These new sub-maps continuously capture the latest environmental features of the current position of the robot, providing dynamic data support for positioning and map updating.

[0031] S3: At the same time, input the sensor data into the global lidar matching module to calculate the relative constraints between the scanned point cloud and the global sub-map, and output the calculated relative constraint relationship between the scanned point cloud and the map to the pose graph optimization module; S4: The pose graph optimization module receives the new sub-map, receives the constraint relationship from the global lidar matching module, processes the received sub-map and constraint relationship, prunes and saves the old sub-maps in the historical map, performs pose graph sparsification and optimization, and completes the environmental map update.

[0032] In some embodiments, the steps of obtaining the sensor data of the robot and generating an environmental map based on the sensor data include: S11: Obtain the point cloud data collected by the lidar, the acceleration and angular velocity data collected by the IMU, and the wheel rotation information recorded by the wheel encoder; S12: Process the point cloud data obtained by the lidar, the acceleration and angular velocity data collected by the IMU, and the wheel rotation information recorded by the wheel encoder respectively, and align the data of the IMU, lidar, and wheel encoder through timestamp synchronization; Here, it specifically includes: S121: Preprocess the point cloud data obtained by the lidar and then perform point cloud registration, match the point clouds at adjacent times, and determine the relative pose transformation between the point clouds; It should be noted that in the embodiments of the present invention, the existing well-known method is used to preprocess the point cloud data obtained by the lidar to remove the noise points.

[0033] The iterative closest point (ICP) algorithm is used for point cloud registration. The specific steps include: 1. Given two sets of point cloud data, one set is the target point cloud P, and the other set is the source point cloud Q. First, preliminarily align the two sets of point clouds according to the centroid of the point clouds. Specifically, calculate the centroid of the target point cloud P and the centroid of the source point cloud Q , and translate the point cloud Q so that its centroid coincides with the centroid of the point cloud P.

[0034] , where is the rd point in the point cloud P, is the total number of points in the point cloud P; are respectively 's coordinates, coordinates, Coordinate; Center of gravity , where , , .

[0035] The center of gravity of the source point cloud Q can be calculated by the same method .

[0036] 2. After completing the initial alignment, for each point in the source point cloud Q, search for its nearest neighbor point in the target point cloud P, and establish a set of corresponding point pairs between the source point cloud and the target point cloud .

[0037] 3. According to the set of corresponding point pairs calculate the transformation matrix that can make the source point cloud Q coincide with the target point cloud after transformation. Here, the transformation matrix includes the rotation matrix and the translation vector ; Define the error function Here, n is the number of corresponding point pairs in the set, calculate the covariance matrix of the two sets of point clouds , perform singular value decomposition on the covariance matrix to obtain , obtain the rotation matrix , and the translation vector ; 4. Apply the calculated transformation matrix to the source point cloud Q to obtain the transformed source point cloud. Re-execute the step of searching for the nearest neighbor point to the new set of corresponding point pairs and calculate the transformation matrix again until the set convergence condition is met, that is, the change in the error function is less than the set threshold. The obtained transformation matrix is the final relative pose transformation matrix of the source point cloud Q relative to the target point cloud P, and the point cloud registration is completed.

[0038] S122: Integrate the acceleration and angular velocity data collected by the IMU to obtain the velocity and attitude changes of the robot in each direction; When calculating the velocity, the velocity at the next moment is calculated from the current velocity, time interval, and current acceleration.

[0039] For attitude calculation, the attitude is represented by quaternions. Given the angular velocities along , , axes at time t are respectively , and the update formula of the quaternion under discrete time The change in the quaternion within the time interval ; The quaternion at time t + 1 is , the quaternion at time t+1 is used to represent the robot's pose at this moment.

[0040] S123: According to the wheel rotation information recorded by the wheel encoder, calculate the moving distance and angle change of the robot, and convert the pulse number into the actual displacement and rotation amount.

[0041] Here, the steps to calculate the moving distance and angle change of the robot are as follows: Let the pulse number recorded by the wheel encoder be N, the wheel radius be r, and the gear ratio ; Wheel rotation angle , where the wheel rotation angle corresponding to a single pulse , is the total number of pulses generated by the wheel encoder when the wheel rotates one week, and N is the actual number of pulses recorded by the wheel encoder; Moving distance .

[0042] Use the linear interpolation method to align the IMU data, lidar data, and wheel encoder data in time.

[0043] S13: Fuse the processed lidar, IMU, and wheel encoder data to obtain the robot pose information; In this embodiment, the extended Kalman filter (EKF) is used to fuse the processed lidar, IMU, and wheel encoder data to obtain a more accurate robot pose estimation; specifically as follows: Define the state vector and the observation vector. The state vector includes the position, pose, speed, and angular velocity of the robot. The lidar observation vector identifies the position and pose information of the robot relative to the surrounding environment features obtained after processing the point cloud data. The IMU observation vector is the acceleration and angular velocity information obtained after integral operation. The wheel encoder observation vector is the calculated moving distance and angle change information of the robot.

[0044] Predict the state at the next moment according to the data of the IMU and the wheel encoder. Obtain the observation matrix by taking the Jacobian matrix of the observation function with respect to the state vector, where the observation function is determined according to the relationship between different sensor observation vectors and the state vector. Calculate the Kalman gain according to the observation matrix and the observation noise covariance matrix, where different sensors (lidar, IMU, wheel encoder) have their own corresponding observation noise covariance matrices. Update the state estimate and the covariance matrix according to the calculated Kalman gain, and complete the fusion of the lidar, IMU, and wheel encoder data through continuous iteration.

[0045] S14: Based on the fused pose information and lidar point cloud data, construct an occupancy grid submap; specifically including: dividing the space into grids, determining the occupancy of each grid according to the point cloud data. If there is point cloud in the grid, it is marked as occupied, and if no point cloud is detected in the grid for a set time, it is marked as free, forming the current environment map composed of multiple occupancy grid submaps; each submap includes a fixed number of lidar scan data with corresponding poses.

[0046] In some embodiments, when performing the positioning task, the steps of collecting the point cloud data of the environment through the local lidar and creating a new submap based on the point cloud data include: S21: When performing the positioning task, obtain the point cloud data of the surrounding environment collected by the local lidar; S22: Preprocess the collected raw point cloud data and extract features from the preprocessed point cloud data; S23: Match the point cloud features of the current frame with the point cloud features in the existing local map. Taking the point cloud in the local map as the target point cloud and the point cloud of the current frame as the source point cloud, calculate the transformation matrix that can make the source point cloud coincide with the target point cloud after transformation by finding corresponding point pairs, so as to obtain the current pose estimation of the robot; S24: According to the pose estimation result, taking the current pose of the robot as the reference, convert the point cloud data to a unified coordinate system and add it to the new submap.

[0047] Correspondingly, the steps of transmitting the newly created submap to the pose graph optimization module include: identifying the key information of the newly created submap and recording the unique identifier of the submap; for example, an incrementing digital ID used to uniquely determine the submap in the whole system, sorting out the pose information of the submap, including the pose information of the submap in the global coordinate system; counting the number of point clouds and the number of features in the submap; matching the newly created submap with other submaps existing in the pose graph to find the overlapping area between the new submap and other submaps; calculating the relative pose transformation between the new submap and other submaps in the overlapping area; packing and transmitting the new submap data, including the identifier, pose information, point cloud and feature information of the submap, to the pose graph optimization module.

[0048] In some embodiments, the steps of inputting the sensor data into the global lidar matching module to calculate the relative constraint between the scanned point cloud and the global submap and outputting the calculated relative constraint relationship between the scanned point cloud and the map to the pose graph optimization module include: S31: Input the lidar, IMU, and wheel encoder data into the global lidar matching module for processing; the processing here is roughly the same as the steps of separately processing the point cloud data obtained by the lidar, the acceleration and angular velocity data collected by the IMU, and the wheel rotation information recorded by the wheel encoder in step S12, and will not be elaborated here.

[0049] S32: In the global map, match the processed lidar scan point cloud with the map model in the global map to determine the position and pose of the scan point cloud in the global map; In the global map, match the preprocessed lidar scan point cloud with the map model in the global map. Use a feature-based matching algorithm, such as the Scale-Invariant Feature Transform (SIFT) algorithm, to extract feature points from the point cloud data, and then find the matching feature points in the global map to determine the position and pose of the scan point cloud in the global map. According to the matching result, calculate the relative measurement constraints between the scan point cloud and the map, including relative position constraints and relative pose constraints. For example, obtain the relative position constraints by calculating the coordinate differences between the matching point pairs, and obtain the relative pose constraints by calculating the differences between the rotation matrices. In some embodiments, to improve the accuracy and reliability of the constraints, the pose information provided by the IMU and the wheel encoder can be combined to verify and correct the calculated relative measurement constraints. For example, if the IMU and wheel encoder data show that the robot has a certain displacement and rotation in a certain direction, and the constraints obtained by lidar matching are quite different from it, further analysis and adjustment are required.

[0050] S33: According to the matching result, calculate the relative measurement constraints between the scan point cloud and the map; including relative position constraints and relative pose constraints; Obtain the relative position constraints by calculating the coordinate differences between the matching point pairs. Obtain the relative pose constraints by calculating the differences between the rotation matrices. Specifically, it includes: Select n groups of matching point pairs from the matching result, and for each group of matching point pairs, calculate their coordinate differences in 、 、 directions 、 、 ; Use the average value of the coordinate differences of all matching point pairs to determine the relative position constraints , , , 。Calculate the relative rotation matrix between the two rotation matrices and convert it to a more intuitive pose representation (such as Euler angles or rotation vectors) to obtain the relative pose constraints; Rotation vector , where is the wheel rotation angle, and is the rotation axis; The rotation axis is obtained by solving ; When the rotation matrix is at this time, is not 0.

[0051] S34: Transmit the calculated relative position constraint and relative pose constraint data to the pose graph optimization module.

[0052] In some embodiments, the steps for the pose graph optimization module to receive a new sub-map from a local lidar, receive constraint relationships from the global lidar matching module, process the received sub-map and constraint relationships, prune and save the old sub-maps in the historical map, perform pose graph sparsification and optimization, and complete the environmental map update include: S41: The pose graph optimization module receives the newly created sub-map from the local lidar, parses the received sub-map data, and extracts the position and feature information of the sub-map; S42: Receive the constraint relationships from the global lidar matching module, and classify and organize the constraint relationships according to the type of constraint and the sub-maps involved; S43: Associate the newly received sub-map with the sub-maps already existing in the pose graph, and determine the position of the new sub-map in the pose graph by comparing the pose and feature information of the sub-maps; S44: Integrate the newly received constraint relationships with the existing constraint relationships in the pose graph; S45: Evaluate the old sub-maps in the historical map, determine the sub-maps to be pruned and mark them; S46: For the sub-maps marked to be pruned, remove the relevant nodes and the constraint relationships connecting the nodes from the pose graph. After completing the constraint removal, update the data structure of the pose graph; S47: Calculate the error between the actual pose of each pair of sub-maps with constraint relationships and the pose that should be according to the constraints, accumulate the squares of all errors to establish an objective function, and adjust the pose of the sub-maps by minimizing the objective function. When the convergence condition is met, update the optimized sub-map pose information to the global map data structure to complete the environmental map update.

[0053] In some embodiments, during long-term localization, whenever the robot re-enters a previously visited terrain, a new sub-map will be added to the global map, and the old sub-maps will be trimmed to limit the number of sub-maps. The overlap rate of the old sub-maps is calculated. If the ratio is lower than a defined threshold, the old sub-maps will not be deleted. Otherwise, they will be marked for pruning and deletion in the pose graph sparsification module. Regardless of the status of the old sub-maps, the new sub-map will be added to the pose graph.

[0054] Solve the redundancy probability of nodes to delete old nodes. Each node in the pose graph stores a frame of lidar data. For each grid i in the map, the number of hits hits(i) and the number of misses misses(i) are stored. The probability that the grid is an obstacle is represented by hits(i) / (hits(i)+misses(i)). And use the grid occupancy probability to quantitatively analyze the impact of removing grids on mapping. The number of grids whose states change before and after removing a node is used as the node redundancy. The fewer the number of grids whose states change before and after removing a node, the more redundant the frame of data means. Calculate the redundancy of each node, and when the redundancy is less than a certain threshold, select it as a node to be deleted; when deleting each node, a sub-graph will be constructed in the graph, forming many new constraint edges. As the number of deleted nodes increases, the pose graph will become denser and denser. Use the Chow-Liu tree to approximate the dense pose graph to reduce the complexity of the pose graph optimization process.

[0055] As Figure 2 shown, an embodiment of the present invention also provides a SLAM map update system for a dynamic environment, including a map generation module, a sub-map creation and processing module, a global lidar matching module, and a pose graph optimization module; The map generation module is used to obtain the sensor data of the robot and generate an environmental map based on the sensor data; The sub-map creation and processing module is used to collect the point cloud data of the environment through a local lidar when performing a positioning task, and create a new sub-map based on the point cloud data; transmit the created new sub-map to the pose graph optimization module; The global lidar matching module is used to receive the sensor data, calculate the relative constraints between the scanned point cloud and the global sub-map, and output the calculated relative constraint relationship between the scanned point cloud and the map to the pose graph optimization module; The pose graph optimization module is used to receive the new sub-map, receive the constraint relationship from the global lidar matching module, process the received sub-map and the constraint relationship, prune and save the old sub-maps in the historical map, perform pose graph sparsification and optimization, and complete the update of the environmental map.

[0056] In some embodiments, the map generation module includes a data acquisition unit, a first data processing unit, a data fusion unit, and a map generation unit; The data acquisition unit is configured to acquire point cloud data collected by a lidar, acceleration and angular velocity data collected by an IMU, and wheel rotation information recorded by a wheel encoder; The first data processing unit is configured to process the point cloud data obtained by the lidar, the acceleration and angular velocity data collected by the IMU, and the wheel rotation information recorded by the wheel encoder respectively, and align the data of the IMU, lidar, and wheel encoder through timestamp synchronization; The data fusion unit is configured to fuse the processed lidar, IMU, and wheel encoder data to obtain robot pose information; The map generation unit is configured to construct an occupancy grid sub-map based on the fused pose information and lidar point cloud data; specifically including: dividing the space into grids, determining the occupancy situation of each grid according to the point cloud data, if there is point cloud in the grid, it is marked as occupied, and if no point cloud is detected in the grid for a set time, it is marked as free, forming a current environment map composed of multiple occupancy grid sub-maps; each sub-map includes a fixed number of lidar scan data with corresponding poses.

[0057] In some embodiments, the first data processing unit is specifically configured to perform point cloud registration after preprocessing the point cloud data obtained by the lidar, match the point clouds at adjacent times, and determine the relative pose transformation between the point clouds; perform integral operations on the acceleration and angular velocity data collected by the IMU to obtain the speed and attitude changes of the robot in each direction; calculate the moving distance and angular change of the robot according to the wheel rotation information recorded by the wheel encoder, and convert the pulse number into actual displacement and rotation amount.

[0058] In some embodiments, the sub-map creation processing module includes a point cloud data acquisition unit, a preprocessing unit, a pose estimation unit, and a sub-map creation unit; The point cloud data acquisition unit is configured to acquire point cloud data of the surrounding environment collected by a local lidar when performing a positioning task; The preprocessing unit is configured to preprocess the collected raw point cloud data and extract features from the preprocessed point cloud data; The pose estimation unit is configured to match the point cloud features of the current frame with the point cloud features in the existing local map, use the point cloud in the local map as the target point cloud and the point cloud of the current frame as the source point cloud, calculate the transformation matrix that can make the source point cloud coincide with the target point cloud after transformation by finding corresponding point pairs, so as to obtain the current pose estimation of the robot; A sub-map creation unit, configured to, according to the pose estimation result, with the current pose of the robot as a reference, convert the point cloud data into a unified coordinate system and add it to a new sub-map.

[0059] In some embodiments, the sub-map creation processing module further includes an information processing unit and a sub-map transmission unit; The information processing unit is configured to identify the key information of the newly created sub-map and record the unique identifier of the sub-map; sort out the pose information of the sub-map, including the pose information of the sub-map in the global coordinate system; count the number of point clouds and the number of features in the sub-map; match the new sub-map with other sub-maps already existing in the pose map to find the overlapping area between the new sub-map and other sub-maps; within the overlapping area, calculate the relative pose transformation between the new sub-map and other sub-maps; The sub-map transmission unit is configured to pack and transmit the new sub-map data, including the identifier, pose information, point cloud, and feature information of the sub-map, to the pose map optimization module.

[0060] In some embodiments, the global lidar matching module includes a data receiving unit, a second data processing unit, a pose information determination unit, a constraint relationship determination unit, and a constraint transmission unit; The data receiving unit is configured to receive lidar, IMU, and wheel encoder data; The second data processing unit is configured to process the lidar point cloud data; The pose information determination unit is configured to match the processed lidar scan point cloud with the map model in the global map in the global map to determine the position and pose of the scan point cloud in the global map; The constraint relationship determination unit is configured to calculate the relative measurement constraints between the scan point cloud and the map according to the matching result; including relative position constraints and relative pose constraints; The constraint transmission unit is configured to transmit the calculated relative position constraint and relative pose constraint data to the pose map optimization module.

[0061] In some embodiments, the pose map optimization module includes an information receiving and processing unit, a position determination unit, an integration unit, a marking unit, a sparsification unit, and an optimization unit; The information receiving and processing unit is configured to receive the newly created sub-map, parse the received sub-map data, and extract the position and feature information of the sub-map; receive the constraint relationship from the global lidar matching module and sort out the constraint relationship according to the type of constraint and the sub-maps involved; The position determination unit is configured to associate the newly received sub-map with the sub-maps already existing in the pose map, and determine the position of the new sub-map in the pose map by comparing the pose and feature information of the sub-maps; An integration unit for integrating the newly received constraint relationships with the existing constraint relationships in the pose graph. A marking unit for evaluating the old sub-maps in the historical map, determining the sub-maps to be trimmed and marking them. A sparsification unit for removing the relevant nodes and the constraint relationships connecting the nodes from the pose graph for the sub-maps marked to be trimmed, and updating the data structure of the pose graph after completing the constraint removal. An optimization unit for calculating the error between the actual pose of each pair of sub-maps with constraint relationships and the pose that should be according to the constraints, accumulating the squares of all the errors to establish an objective function, and adjusting the poses of the sub-maps by minimizing the objective function. After meeting the convergence condition, updating the optimized sub-map pose information into the global map data structure to complete the environmental map update.

[0062] An embodiment of the present invention further provides an electronic device, which includes: a processor, a communication interface, a memory, and a communication bus. Among them, the processor, the communication interface, and the memory complete mutual communication through the communication bus. The communication bus can be used for information transmission between the electronic device and the sensor. The processor can call the logical instructions in the memory to execute the following method: obtaining the sensor data of the robot, generating an environmental map based on the sensor data; when performing a positioning task, collecting the point cloud data of the environment through a local lidar and creating a new sub-map based on the point cloud data; transmitting the created new sub-map to the pose graph optimization module; at the same time, inputting the sensor data into the global lidar matching module to calculate the relative constraints between the scanned point cloud and the global sub-map, and outputting the calculated relative constraint relationships between the scanned point cloud and the map to the pose graph optimization module; the pose graph optimization module receives the new sub-map, receives the constraint relationships from the global lidar matching module, processes the received sub-map and constraint relationships, trims and saves the old sub-maps in the historical map, performs pose graph sparsification and optimization, and completes the environmental map update.

[0063] In addition, when the logical instructions in the above-mentioned memory are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical discs that can store program codes.

[0064] An embodiment of the present invention provides a non-transitory computer-readable storage medium that stores computer instructions, and these computer instructions cause a computer to execute the method provided in the above method embodiment. For example, it includes: obtaining sensor data of a robot, generating an environmental map based on the sensor data; when performing a positioning task, collecting point cloud data of the environment through a local lidar and creating a new sub-map based on the point cloud data; transmitting the created new sub-map to a pose graph optimization module; at the same time, inputting the sensor data into a global lidar matching module to calculate the relative constraints between the scanned point cloud and the global sub-map, and outputting the calculated relative constraint relationship between the scanned point cloud and the map to the pose graph optimization module; the pose graph optimization module receives the new sub-map, receives the constraint relationship from the global lidar matching module, processes the received sub-map and the constraint relationship, trims and saves the old sub-maps in the historical map, performs pose graph sparsification and optimization, and completes the update of the environmental map.

[0065] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present invention. Various modifications to these embodiments will be obvious to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to these embodiments shown herein, but will be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A SLAM map updating method for dynamic environments, characterized in that: include: Acquire sensor data of the robot, and generate an environment map based on the sensor data; When performing positioning tasks, the point cloud data of the environment is collected through the local laser radar, and a new sub-map is created based on the point cloud data; the created new sub-map is transmitted to the pose graph optimization module; At the same time, the sensor data is input into the global lidar matching module to calculate the relative constraints between the scan point cloud and the global sub-map, and the calculated relative constraint relationship between the scan point cloud and the map is output to the pose graph optimization module; The pose graph optimization module receives the new sub-map, receives the constraints from the global lidar matching module, processes the received sub-map and constraints, prunes and saves the old sub-map in the history map, performs pose graph sparsification and optimization, and completes the environment map update.

2. The SLAM map updating method for dynamic environments according to claim 1, characterized in that: The steps of obtaining sensor data of the robot and generating an environment map based on the sensor data include: Obtain point cloud data collected by the LiDAR, acceleration and angular velocity data collected by the IMU, and wheel rotation information recorded by the wheel encoder; The point cloud data acquired by the LiDAR, the acceleration and angular velocity data collected by the IMU, and the wheel rotation information recorded by the wheel encoder are processed separately, and the data of the IMU, LiDAR, and wheel encoder are aligned through timestamp synchronization; The processed LiDAR, IMU and wheel encoder data are integrated to obtain the robot’s position and posture information; Based on the fused pose information and lidar point cloud data, an occupied grid sub-map is constructed; specifically, the space is divided into grids, and the occupancy of each grid is determined according to the point cloud data. If there is a point cloud in the grid, it is marked as occupied. If no point cloud is detected in the grid for a set time, it is marked as idle, forming a current environment map composed of multiple occupied grid sub-maps; each sub-map includes a fixed number of lidar scan data with corresponding poses.

3. The SLAM map updating method for dynamic environments according to claim 2, characterized in that: The steps of processing the point cloud data acquired by the laser radar, the acceleration and angular velocity data collected by the IMU, and the wheel rotation information recorded by the wheel encoder include: After preprocessing the point cloud data acquired by the LiDAR, point cloud registration is performed to match the point clouds at adjacent moments and determine the relative position transformation between the point clouds; Integrate the acceleration and angular velocity data collected by the IMU to obtain the speed and posture changes of the robot in all directions; Based on the wheel rotation information recorded by the wheel encoder, the robot's moving distance and angle change are calculated, and the number of pulses is converted into actual displacement and rotation.

4. The SLAM map updating method for dynamic environments according to claim 3, characterized in that: When performing a positioning task, the point cloud data of the environment collected by the local laser radar and the steps of creating a new submap based on the point cloud data include: When performing positioning tasks, obtain point cloud data of the surrounding environment collected by local laser radar; Preprocess the collected original point cloud data and extract features from the preprocessed point cloud data; Match the point cloud features of the current frame with the point cloud features in the existing local map, take the point cloud in the local map as the target point cloud, and the point cloud of the current frame as the source point cloud. By finding corresponding point pairs, calculate the transformation matrix that can make the source point cloud coincide with the target point cloud after transformation, and thus obtain the current pose estimate of the robot; According to the pose estimation results, the point cloud data is converted into a unified coordinate system based on the current pose of the robot and added to the new sub-map.

5. The SLAM map updating method for dynamic environments according to claim 4, characterized in that: The steps to transfer the created new submap to the pose graph optimization module include: Identify the key information of the newly created sub-map and record the unique identifier of the sub-map; Arranging the position and posture information of the sub-map, including the position and posture information of the sub-map in the global coordinate system; Count the number of point clouds and features in the sub-map; Match the new submap with other submaps already in the pose graph and find the overlapping area between the new submap and other submaps; In the overlapping area, calculate the relative position transformation between the new submap and other submaps; The new sub-map data, including the sub-map identifier, pose information, point cloud and feature information, is packaged and transmitted to the pose graph optimization module.

6. The SLAM map updating method for dynamic environments according to claim 5, characterized in that: The steps of inputting sensor data into the global lidar matching module to calculate the relative constraints between the scan point cloud and the global sub-map, and outputting the calculated relative constraint relationship between the scan point cloud and the map to the pose graph optimization module include: Input the lidar, IMU and wheel encoder data into the global lidar matching module for processing; In the global map, the processed LiDAR scan point cloud is matched with the map model in the global map to determine the position and posture of the scan point cloud in the global map; According to the matching results, the relative measurement constraints between the scanned point cloud and the map are calculated, including relative position constraints and relative posture constraints; The calculated relative position constraint and relative posture constraint data are transmitted to the pose graph optimization module.

7. The SLAM map updating method for dynamic environments according to claim 6, characterized in that: The pose graph optimization module receives new submaps from the local lidar, receives constraints from the global lidar matching module, processes the received submaps and constraints, prunes and saves old submaps in the history map, performs pose graph sparsification and optimization, and completes the steps of updating the environment map: The pose graph optimization module receives the newly created submap from the local lidar, parses the received submap data, and extracts the location and feature information of the submap; Receive constraints from the global lidar matching module and classify them according to the type of constraints and the submaps involved; Associating the newly received submap with the submap that already exists in the pose graph, and determining the position of the new submap in the pose graph by comparing the pose and feature information of the submap; Integrate the newly received constraint relationship with the existing constraint relationship in the pose graph; Evaluate old submaps in historical maps, identify submaps that need to be pruned and mark them; For the submaps marked as to be pruned, the relevant nodes and the constraint relationships connecting the nodes are removed from the pose graph. After the constraint removal is completed, the data structure of the pose graph is updated; Calculate the error between the actual pose of each pair of sub-maps with constraints and the pose they should have according to the constraints, accumulate the squares of all errors to establish an objective function, and adjust the pose of the sub-map by minimizing the objective function. When the convergence condition is met, update the optimized sub-map pose information to the global map data structure to complete the environment map update.

8. A SLAM map updating system for dynamic environments, characterized in that: It includes map generation module, sub-map creation processing module, global lidar matching module and pose graph optimization module; A map generation module, used to obtain sensor data of the robot and generate an environment map based on the sensor data; The submap creation processing module is used to create a new submap based on the point cloud data of the environment collected by the local laser radar when performing the positioning task; and transmit the created new submap to the pose graph optimization module; The global lidar matching module is used to receive sensor data, calculate the relative constraints between the scan point cloud and the global sub-map, and output the calculated relative constraint relationship between the scan point cloud and the map to the pose graph optimization module; The pose graph optimization module is used to receive new sub-maps, receive constraints from the global lidar matching module, process the received sub-maps and constraints, prune and save old sub-maps in the historical map, perform pose graph sparsification and optimization, and complete the environment map update.

9. An electronic device, characterized in that: The electronic device includes: at least one processor; and a memory communicatively connected to the at least one processor; the memory stores computer program instructions executable by the at least one processor, and the computer program instructions are executed by the at least one processor so that the at least one processor can execute the SLAM map update method for dynamic environments as described in any one of claims 1 to 7.

10. A non-transitory computer-readable storage medium, characterized in that: The non-transitory computer-readable storage medium stores computer instructions, and the computer instructions enable the computer to execute the SLAM map updating method for dynamic environments as described in any one of claims 1 to 7.

Images