Mapping and positioning processing method and robot

Abstract

The invention provides a mapping and positioning processing method and a robot. The method comprises the following steps: acquiring a current point cloud set and robot driving data; determining current feature information corresponding to the current moment according to the driving data and each point cloud in the current point cloud set; according to the driving data, the current feature information, previous feature information corresponding to a previous moment of the current moment, a pose of the robot at the previous moment and a previous trajectory point pose corresponding to a previous key frame, determining an initial pose of a current trajectory point, and determining whether a key feature point frame corresponding to the current trajectory point is taken as a current key frame; if yes, the target pose of the current track point is determined according to the current track point, the historical track point corresponding to the at least one historical key frame, the current key frame and the initial pose of the current track point, mapping is conducted on the basis of the target pose and the current key frame, a target map is obtained, and the robot is positioned on the basis of the target map. And mapping and positioning in a shielding scene are realized.

Description

Technical Field

[0001] This application relates to the field of mapping and localization technology, and more specifically, to a mapping and localization processing method and a robot. Background Technology

[0002] Mapping and localization technology is the core foundation for robots, autonomous driving and intelligent mobile systems to achieve autonomous navigation. Its goal is to enable devices to build a map of the environment in unknown or dynamic environments and at the same time determine their real-time position in the map.

[0003] In existing technologies, mapping and positioning technologies rely on data from various environmental sensors, such as the Global Navigation Satellite System (GNSS), lidar, inertial measurement units (IMUs), and cameras. GNSS-based mapping and positioning methods depend on satellite-provided positioning information. However, for outdoor robotic vacuum cleaners, their working environments typically include, but are not limited to, parks, residential areas, industrial parks, and underground parking lots, where GNSS signals are inevitably blocked, resulting in the inability to provide accurate positioning information. LiDAR-based mapping and positioning methods are widely used in both indoor and outdoor scenarios and are well-adapted to relatively static environments. However, in outdoor environments, the environmental information contains a large number of vehicle point clouds. Due to the frequent movement of vehicles, the constructed map can easily differ significantly from the real environment, leading to poor positioning performance for outdoor robotic vacuum cleaners in such scenarios. Camera-based visual mapping and positioning methods are susceptible to lighting conditions and rely heavily on environmental texture features.

[0004] Therefore, existing mapping and positioning technologies are influenced by numerous factors and cannot meet the operational requirements of outdoor robotic vacuum cleaners in scenarios with overhead obstructions, such as rooftops, eaves, underground garages, and overpasses. Thus, achieving accurate mapping and positioning of outdoor robotic vacuum cleaners in such obstructed environments is a problem that needs to be solved. Summary of the Invention

[0005] The purpose of this application is to address the shortcomings of the prior art by providing a mapping and localization processing method and robot that meets the mapping and localization requirements of scenes where the top is occluded.

[0006] To achieve the above objectives, the technical solutions adopted in the embodiments of this application are as follows: In a first aspect, embodiments of this application provide a mapping and positioning processing method, the method comprising: Acquire the current point cloud set collected by the hemispherical lidar on the robot at the current trajectory point in the target area at the current moment, as well as the robot driving data collected by the inertial measurement device; Based on the driving data and each point cloud in the current point cloud set, the current feature information corresponding to the current moment is determined, and a key feature point frame corresponding to the current trajectory point is generated based on the current feature information. The current feature information includes multiple feature points, feature point descriptions of each feature point, multiple key feature points, and key feature point descriptions of each key feature point. Based on the driving data, the current feature information, the previous feature information corresponding to the previous moment, the robot's pose at the previous moment, and the pose of the previous trajectory point corresponding to the previous keyframe, the initial pose of the current trajectory point is determined, and it is determined whether to use the key feature point frame corresponding to the current trajectory point as the current keyframe. If so, the target pose of the current trajectory point is determined based on the current trajectory point, the historical trajectory point corresponding to at least one historical keyframe, the current keyframe, and the initial pose of the current trajectory point. A map is constructed based on the target pose and the current keyframe to obtain a target map, and the robot is located based on the target map.

[0007] Optionally, determining the current feature information corresponding to the current moment based on the driving data and each point cloud in the current point cloud set includes: Based on the driving data, distortion compensation processing is performed on each point cloud in the current point cloud set, and time alignment processing is performed on each point cloud to obtain the processed point cloud set. Feature points are extracted from each point cloud in the processed point cloud set to obtain multiple feature points and feature point descriptions for each feature point. The multiple feature points belong to either edge feature point type or planar feature point type. The multiple feature points are subjected to distribution equalization processing and key feature point extraction processing to obtain the multiple key feature points and the key feature point description of each key feature point.

[0008] Optionally, the step of extracting feature points from each point cloud in the processed point cloud frame to obtain multiple feature points and feature point descriptions for each feature point includes: Based on each point cloud and its associated point cloud, the type of each point cloud is determined to obtain an initial feature point set, wherein the initial feature points in the initial feature point set belong to either edge feature point type or planar feature point type. Based on the three-dimensional coordinates of each initial feature point in the initial feature point set, determine whether each initial feature point is a feature point. If so, use the initial feature point as the feature point and determine the feature point description of each feature point.

[0009] Optionally, the step of performing distribution equalization processing and key feature point extraction processing on the plurality of feature points to obtain the plurality of key feature points and key feature point descriptions for each key feature point includes: The target region is horizontally segmented to obtain multiple leaf nodes, and the number of feature points in each leaf node satisfies a first preset condition. Based on each feature point in each leaf node and the second preset condition, determine each key feature point in each leaf node and the key feature point description of each key feature point.

[0010] Optionally, determining whether each initial feature point is a feature point based on the three-dimensional coordinates of each initial feature point in the initial feature point set includes: Traverse each of the initial feature points, and for the current initial feature point, determine the associated initial feature point corresponding to the current initial feature point; Based on the current initial feature point and the associated initial feature point, the inertia matrix corresponding to the current initial feature point is determined. The inertia matrix includes an eigenvalue matrix and an eigenvector matrix. The eigenvalue matrix includes at least one eigenvalue, and the eigenvector matrix includes a direction vector that corresponds one-to-one with each eigenvalue. Determine whether the current initial feature point is the feature point based on each eigenvalue in the eigenvalue matrix.

[0011] Optionally, determining each key feature point and its description based on each feature point in each leaf node and a second preset condition includes: Traverse each feature point, and for the current feature point, determine the parallelism between the current feature point and each of the other feature points based on the direction vector of the current feature point and the direction vectors of the other feature points. Based on the parallel results, the direction vector and three-dimensional coordinates of the current feature point, and the direction vectors and three-dimensional coordinates of the other feature points, determine the distance and angle between the current feature point and the other feature points. If the distance or the angle satisfies the second preset condition, then the current feature point is taken as the key feature point, and the feature point description of the current feature point is taken as the key feature point description.

[0012] Optionally, determining the initial pose of the current trajectory point based on the driving data, the current feature information, the previous feature information corresponding to the previous moment, the robot's pose at the previous moment, and the pose of the previous trajectory point corresponding to the previous keyframe, and determining whether to use the key feature point frame corresponding to the current trajectory point as the current keyframe, includes: Based on the driving data, the current feature information, and the previous feature information corresponding to the previous moment, the pose transformation matrix of the lidar from the previous moment to the current moment is determined; Based on the pose transformation matrix, the robot's pose at the previous moment, and the pose of the previous trajectory point corresponding to the previous keyframe, the initial pose of the current trajectory point is determined, and it is determined whether to use the key feature point frame corresponding to the current trajectory point as the current keyframe.

[0013] Optionally, determining the pose transformation matrix of the lidar from the previous moment to the current moment based on the driving data, the current feature information, and the previous feature information corresponding to the previous moment before the current moment includes: Based on the driving data, multiple key feature points in the current feature information are reprojected to obtain a reprojection set. The projected feature points in the reprojection set and the previous feature points in the previous feature information are matched. The matching previous feature points that match each of the projected feature points are extracted from each of the previous feature points in the previous feature information to obtain the matching feature point set. The pose transformation matrix is ​​determined based on each previous matching feature point in the set of matching feature points and multiple key feature points in the current feature information.

[0014] Optionally, determining the target pose of the current trajectory point based on the current trajectory point, historical trajectory points corresponding to at least one historical keyframe, the current keyframe, and the initial pose of the current trajectory point includes: Based on the distance between the current trajectory point and each of the historical trajectory points, determine whether to use the current trajectory point as a trajectory point to be looped back; If the current trajectory point is determined as the trajectory point to be looped back, then a preset number of associated historical keyframes are determined based on the current trajectory point, and an image to be matched is generated based on each of the associated historical keyframes, and multiple intermediate sub-images are generated based on the current keyframe. Each of the intermediate sub-images is sequentially aligned with the image to be matched to obtain the target overlap position and the target intermediate sub-image; The initial loop pose at the current trajectory point is determined based on the identifier of the target intermediate sub-image and the target overlap position. The initial loop pose is then optimized based on the initial loop pose, the associated historical keyframes, and the current keyframe to obtain the target pose at the current trajectory point. If it is determined that the current trajectory point will not be used as the trajectory point to be looped back, then the initial pose will be used as the target pose.

[0015] Secondly, embodiments of this application also provide a mapping and positioning processing apparatus, the apparatus comprising: The acquisition module is used to acquire the current point cloud set collected by the hemispherical lidar on the robot at the current trajectory point in the target area at the current moment, as well as the robot driving data collected by the inertial measurement device. The first determining module is used to determine the current feature information corresponding to the current time based on the driving data and each point cloud in the current point cloud set, and to generate a key feature point frame corresponding to the current trajectory point based on the current feature information. The current feature information includes multiple feature points, feature point descriptions of each feature point, multiple key feature points, and key feature point descriptions of each key feature point. The second determining module is used to determine the initial pose of the current trajectory point based on the driving data, the current feature information, the previous feature information corresponding to the previous moment, the pose of the robot at the previous moment, and the pose of the previous trajectory point corresponding to the previous keyframe, and to determine whether to take the key feature point frame corresponding to the current trajectory point as the current keyframe. The third determining module is used to determine the target pose of the current trajectory point based on the current trajectory point, the historical trajectory point corresponding to at least one historical keyframe, the current keyframe, and the initial pose of the current trajectory point, and to build a map based on the target pose and the current keyframe to obtain a target map, and to locate the robot based on the target map.

[0016] Thirdly, embodiments of this application also provide a robot, including a memory and a processor, and the robot is equipped with a hemispherical lidar and an inertial measurement device; The hemispherical lidar is used to collect point cloud data of the target area; The inertial measurement unit is used to collect the robot's driving data; The memory stores program instructions executable by the processor. When the application runs, the processor communicates with the memory via a bus, and the processor executes the program instructions to perform the steps of the mapping and positioning processing method described in the first aspect above.

[0017] Fourthly, embodiments of this application also provide a computer-readable storage medium storing a computer program, which is read and executes the steps of the mapping and positioning processing method described in the first aspect.

[0018] The beneficial effects of this application are: This application provides a mapping and localization processing method and a robot. It acquires point cloud data above the robot using an upward scanning method with a hemispherical LiDAR mounted on the robot, avoiding dynamic objects on the ground and improving the utilization rate of the acquired point cloud data. Furthermore, it obtains current feature information corresponding to the current moment based on the acquired point cloud data, avoids feature redundancy through feature filtering, improves computational efficiency, and generates key feature point frames corresponding to the current trajectory point. It also determines whether the key feature point frames corresponding to the current trajectory point can be used as keyframes for subsequent mapping. Through feature filtering and keyframe mechanisms, the amount of data for parametric mapping is reduced, improving the real-time performance of the algorithm. Moreover, to suppress accumulated errors, loop closure detection and pose optimization are introduced to ensure the consistency of the global location map, improving the accuracy and efficiency of mapping. Finally, based on the obtained target map, the robot is localized, achieving rapid localization of the robot in signal-obstructed environments. Attached Figure Description

[0019] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0020] Figure 1 A schematic diagram of a robot structure provided in an embodiment of this application; Figure 2 A schematic flowchart illustrating a mapping and positioning processing method provided in an embodiment of this application; Figure 3 A flowchart illustrating the second mapping and positioning processing method provided in this application embodiment; Figure 4 A flowchart illustrating the third mapping and positioning processing method provided in this application embodiment; Figure 5 A flowchart illustrating the fourth mapping and positioning processing method provided in this application embodiment; Figure 6 This is a schematic diagram showing the segmentation of the target region provided in this embodiment; Figure 7 A schematic diagram of key feature points in each leaf node provided in an embodiment of this application; Figure 8 This is a schematic diagram of a key feature point in a real-world scenario, provided as an embodiment of this application. Figure 9 A flowchart illustrating the fifth mapping and positioning processing method provided in this application embodiment; Figure 10 A flowchart illustrating the sixth mapping and positioning processing method provided in this application embodiment; Figure 11 A flowchart illustrating the seventh mapping and positioning processing method provided in this application embodiment; Figure 12 A flowchart illustrating the eighth mapping and positioning processing method provided in this application embodiment; Figure 13 A flowchart illustrating the ninth mapping and positioning processing method provided in this application embodiment; Figure 14 A schematic diagram of an apparatus for a mapping and positioning processing method provided in an embodiment of this application; Figure 15 This is a structural block diagram of an electronic device provided in an embodiment of this application. Detailed Implementation

[0021] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. It should be understood that the accompanying drawings in this application are for illustrative and descriptive purposes only and are not intended to limit the scope of protection of this application. Furthermore, it should be understood that the schematic drawings are not drawn to scale. The flowcharts used in this application illustrate operations implemented according to some embodiments of this application. It should be understood that the operations in the flowcharts may not be implemented in sequence, and steps without logical contextual relationships may be reversed or implemented simultaneously. In addition, those skilled in the art, guided by the content of this application, may add one or more other operations to the flowcharts, or remove one or more operations from the flowcharts.

[0022] Furthermore, the described embodiments are merely some, not all, of the embodiments of this application. The components of the embodiments of this application described and illustrated herein can typically be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0023] It should be noted that the term "comprising" will be used in the embodiments of this application to indicate the presence of the features declared thereafter, but does not exclude the addition of other features.

[0024] Figure 1 A schematic diagram of a robot structure is provided for an embodiment of this application, such as... Figure 1 As shown, the robot is equipped with a hemispherical lidar and an inertial measurement unit, such as... Figure 1 As shown, a hemispherical lidar can be mounted upwards along the central axis of the robot's top surface. This lidar has a 360° horizontal and 90° vertical field of view, capable of completely scanning the area above the hemisphere. The inertial measurement unit (IMU) can be an IMU sensor, mounted at the center of the robot's rear axle. By calibrating the sensor, the sensor coordinate system can be obtained. Chassis coordinate system Positional relationship and laser coordinate system To the sensor coordinate system pose relationship The pose relationship can include both rotation and translation information.

[0025] Among them, the type of lidar is not limited to mechanical multi-line lidar, solid-state lidar, semi-solid-state lidar, and can also use sensors such as depth cameras that can acquire environmental point cloud data.

[0026] Optionally, when the robot is moving, the lidar can collect point cloud data of an area above a certain height above the ground and send the collected point cloud data to the processor. The IMU sensor can collect the robot's driving data during the movement and send the collected driving data to the processor. Then, the processor uses the mapping and localization processing method provided in this application to construct an environmental map of the area based on the received point cloud data and driving data, and locates the robot based on the constructed environmental map.

[0027] It is worth noting that, Figure 1 The installation position of the hemispherical lidar and inertial measurement unit is only an illustration, and the installation position can be set according to actual needs.

[0028] Figure 2 This is a flowchart illustrating a mapping and localization processing method provided in an embodiment of this application. The execution subject of this method is the robot described above. Figure 2 As shown, the method includes: S101. Obtain the current point cloud set collected by the hemispherical lidar on the robot at the current trajectory point for the target area at the current moment, as well as the robot driving data collected by the inertial measurement device.

[0029] The current point cloud set can include multiple point clouds. The hemispherical LiDAR and inertial measurement unit are all mounted on the robot, with specific installation locations as follows: Figure 1 As shown, it can also be installed in other locations. The robot's motion data collected by the inertial measurement unit is the motion data during the process of moving from the previous trajectory point to the current trajectory point, such as the robot's three-axis acceleration and angular velocity.

[0030] The target area can be the area above a park, residential area, industrial park, underground parking garage, or similar environment. While the robot is moving, the LiDAR can illuminate upwards, collecting point cloud data of the area above the robot, such as rooftops, eaves, walls, beams, overpasses, tunnels, and supports. Hemispherical LiDAR can remove dynamic object point clouds from the environment through height filtering, thus acquiring point clouds of obstructions above the robot. Furthermore, because LiDAR is an active detection sensor, it is unaffected by ambient lighting conditions, exhibiting good stability in various lighting environments, including darkness, low light, strong light, backlight, backlight, and shadow.

[0031] For example, when the robot is traveling at trajectory point 1 at time t1, the LiDAR collects point cloud set 1, and the inertial measurement unit collects the robot's travel data 1 from time t0 to t1; when the robot is traveling at trajectory point 2 at time t2, the LiDAR collects point cloud set 2, and the inertial measurement unit collects the robot's travel data 2 from time t1 to t2; when the robot is traveling at trajectory point 3 at time t3, the LiDAR collects point cloud set 3, and the inertial measurement unit collects the robot's travel data 3 from time t2 to t3.

[0032] S102. Determine the current feature information corresponding to the current moment based on the driving data and each point cloud in the current point cloud set, and generate the key feature point frame corresponding to the current trajectory point based on the current feature information.

[0033] The current feature information may include: multiple feature points, feature point descriptions for each feature point, multiple key feature points, and key feature point descriptions for each key feature point. Multiple key feature points are a subset of the multiple feature points. These multiple feature points can be of the edge feature point type or the planar feature point type; similarly, the multiple key feature points can be of the edge key feature point type or the planar key feature point type. In this embodiment, all multiple feature points belong to one type of feature point, and all multiple key feature points also belong to one type of key feature point.

[0034] Optionally, the key feature point frame corresponding to the current trajectory point is generated from multiple key feature points in the current feature information. This key feature point frame can then belong to the edge key feature point frame type or the planar key feature point frame type.

[0035] S103. Determine the initial pose of the current trajectory point based on the current feature information, the previous feature information corresponding to the previous moment, the robot's pose at the previous moment, and the pose of the previous trajectory point corresponding to the previous keyframe.

[0036] Here, a keyframe refers to a key feature point frame used for subsequent environmental map construction. In this embodiment, not every key feature point frame corresponding to every trajectory point is used as a keyframe for subsequent mapping; only key feature point frames that meet the mapping conditions are selected as keyframes. The determined initial pose of the current trajectory point refers to the robot's initial pose at the current trajectory point.

[0037] For example, if the current time is t4, the previous keyframe is, for example, the key feature point frame corresponding to trajectory point 1 at time t1, the current feature information refers to the current feature information corresponding to time t4, the previous feature information refers to the feature information corresponding to time t3, and the robot's pose at the previous time refers to the robot's pose at trajectory point 3 at time t3. Then, a preset method can be used to determine the initial pose of trajectory point 4 at time t4.

[0038] S104. Determine whether to use the key feature point frame corresponding to the current trajectory point as the current key frame.

[0039] Specifically, if it is determined that the key feature point frame corresponding to the current trajectory point will be used as the current key frame, then step S105 is executed; if it is determined that the key feature point frame corresponding to the current trajectory point will not be used as the current key frame, then step S106 is executed.

[0040] S105. Based on the driving data, the current trajectory point, the historical trajectory point corresponding to at least one historical keyframe, the current keyframe, and the initial pose of the current trajectory point, determine the target pose of the current trajectory point, and build a map based on the target pose and the current keyframe to obtain the target map, and locate the robot based on the target map.

[0041] If the current time is t4 as mentioned above, then the current trajectory point is trajectory point 4 at time t4. The historical trajectory point corresponding to at least one historical keyframe is, for example, trajectory point 1. The driving data refers to the robot's driving data from time t3 to time t4. The current keyframe is the key feature point frame corresponding to trajectory point 4 at time t4. The initial pose of the current trajectory point is the initial pose of trajectory point 4 determined in step S103 above. The target pose of the current trajectory point 4 is determined using a preset method.

[0042] Optionally, after determining the key feature point frame corresponding to the current trajectory point as the current key frame, that is, when it is necessary to use the key feature point frame corresponding to the current trajectory point for mapping, since there is accumulated error in multiple key feature points in the key feature point frame, it is also necessary to perform loop closure detection and optimize the initial pose of the current trajectory point to obtain the target pose of the current trajectory point.

[0043] S106. Take the initial pose of the current trajectory point as the target pose at the current moment, and continue to collect the point cloud set at the next moment and return to execute step S101.

[0044] In this embodiment, a hemispherical LiDAR mounted on the robot uses an upward scanning method to acquire point cloud data above the robot, avoiding dynamic objects on the ground and improving the utilization rate of the acquired point cloud data. Furthermore, based on the acquired point cloud data, the current feature information corresponding to the current moment is obtained. Through feature filtering, feature redundancy is avoided, improving computational efficiency. Key feature point frames corresponding to the current trajectory point are generated, and it is determined whether the key feature point frames corresponding to the current trajectory point can be used as key frames for subsequent mapping. Through feature filtering and key frame mechanisms, the amount of data for parametric mapping is reduced, improving the real-time performance of the algorithm. In addition, to suppress accumulated errors, loop closure detection and pose optimization are introduced to ensure the consistency of the global location map, improving the accuracy and efficiency of mapping. Then, the robot is located based on the obtained target map, realizing rapid robot positioning in signal-obstructed environments.

[0045] Figure 3 This is a flowchart illustrating the second mapping and positioning processing method provided in the embodiments of this application, as shown below. Figure 3 As shown, the process in S102 above, which determines the current feature information corresponding to the current moment based on driving data and each point cloud in the current point cloud set, may include: S201. Perform distortion compensation processing on each point cloud in the current point cloud set based on the driving data, and perform time alignment processing on each point cloud to obtain the processed point cloud set.

[0046] Optionally, the current point cloud set is as follows: Where l represents the lidar coordinate system, k represents the k-th point cloud set, n represents the n-th point in the point cloud set, and N represents the number of point clouds in the point cloud set. All point clouds in the current point cloud set can be the point clouds on a single point cloud frame. Motion distortion compensation is performed on each point cloud using the robot's driving data from the time before the current time to the current time, and each point cloud is aligned to a timestamp. , This is the timestamp of the last point cloud in the point cloud set. The processed point cloud set is then used... This means that the processed point cloud set is the processed point cloud frame.

[0047] S202. Extract feature points from each point cloud in the processed point cloud set to obtain multiple feature points and feature point descriptions for each feature point.

[0048] These multiple feature points belong to either the edge feature point type or the planar feature point type. That is, the multiple feature points in this embodiment are of one type. The description of the feature points may include the three-dimensional coordinates of the feature points, the direction vector of the feature points, and the type of the feature points, wherein the three-dimensional coordinates refer to the three-dimensional coordinates in the lidar coordinate system.

[0049] S203. Perform distribution equalization processing and key feature point extraction processing on multiple feature points to obtain multiple key feature points and key feature point descriptions for each key feature point.

[0050] Optionally, to prevent feature points extracted from the same local area from being too dense, while feature points extracted from some local areas are too sparse, leading to excessive over-matching in densely populated areas and under-matching in sparsely populated areas during subsequent alignment and matching, it is necessary to perform a feature point distribution equalization process. Furthermore, the multiple feature points extracted in step S202 are further processed for key feature point extraction, resulting in multiple key feature points. The description of each key feature point is the same as the feature point description; that is, if feature point A can be considered a key feature point, then the feature point description of feature point A is the key feature point description.

[0051] In this embodiment, a complete processing chain of "point cloud purification, feature extraction, and feature optimization" is formed. Starting from the original noisy point cloud, through distortion correction, structure extraction, and spatial equalization, a set of "key feature point frames" that can be used for matching is finally generated, becoming a reliable basis for mapping and localization. This avoids feature points being overly concentrated in a certain area, ensuring the global coverage of the map representation.

[0052] Figure 4 This is a flowchart illustrating the third mapping and positioning processing method provided in the embodiments of this application, as shown below. Figure 4 As shown, in step S202 above, feature points are extracted from each point cloud in the processed point cloud set to obtain multiple feature points and feature point descriptions for each feature point, which may include: S301. Based on each point cloud and its corresponding associated point cloud, determine the type of each point cloud and obtain the initial feature point set.

[0053] The initial feature points in the initial feature point set belong to either edge feature point type or planar feature point type. The associated point clouds corresponding to each point cloud can be the point cloud with the closest Euclidean distance to each point cloud that is less than a preset distance. The number of associated point clouds is a preset number, such as 8, and the preset distance is such as 0.5 cm. For example, for point cloud 1, you can select to determine 8 associated point clouds corresponding to point cloud 1 based on the Euclidean distance between point cloud 1 and other point clouds. If point cloud 1 cannot have 8 associated point clouds extracted, then point cloud 1 is discarded, and the process continues to the next point cloud.

[0054] Optionally, the feature classification coefficient c of each point cloud can be determined by the following formula (i).

[0055] Formula (1) in, For the nth point cloud, the i-th associated point cloud is... Let c be the nth point cloud in the kth point cloud set, and c be the feature classification coefficient of the nth point cloud.

[0056] Optionally, the smaller the value of c, the more likely the point cloud is to contain planar points; the larger the value of c, the more likely the point cloud is to contain edge points. For example... The point cloud consists of edge feature points, if The point cloud consists of planar feature points, where, Threshold for edge feature points The threshold for planar feature points is used. The extracted edge feature points are saved to the initial feature point set. In the process, the extracted planar feature points are saved to the initial feature point set. middle.

[0057] S302. Determine whether each initial feature point is a feature point based on the three-dimensional coordinates of each initial feature point in the initial feature point set.

[0058] The three-dimensional coordinates of each initial feature point are in the laser coordinate system.

[0059] Specifically, if the initial feature point is determined to be a feature point, then step S303 is executed; if the initial feature point is determined not to be a feature point, then step S304 is executed.

[0060] S303. Use the initial feature points as feature points and determine the feature point description.

[0061] Specifically, the point-vector method can be used to describe the features of a feature point, where a point represents the three-dimensional spatial coordinates of the feature point, and a vector represents the three-dimensional spatial orientation of the feature point. For example, the orientation of an edge feature point is the orientation of the edge, with the direction away from the origin being positive. For planar feature points, the vector direction is the normal to the plane, with the direction away from the origin being positive.

[0062] S304. Discard the initial feature point.

[0063] Figure 5 This is a flowchart illustrating the fourth mapping and positioning processing method provided in the embodiments of this application, as shown below. Figure 5 As shown, in S203 above, the distribution equalization processing and key feature point extraction processing of multiple feature points are performed to obtain multiple key feature points and key feature point descriptions of each key feature point, which may include: S401. Perform horizontal segmentation on the target region to obtain multiple leaf nodes.

[0064] In this embodiment, a feature description method combining point and direction examples is used to facilitate subsequent geometric feature matching.

[0065] Among them, the number of feature points in each leaf node satisfies the first preset condition.

[0066] Optionally, a quadtree method can be used to horizontally segment the target region. The first preset condition can be that the number of feature points in a leaf node is greater than or equal to a preset number, or the length or width of the leaf node is greater than or equal to a preset size. The preset number can be 5, and the preset size can be 0.5m. If the number of feature points in a leaf node is less than 5, or the length or width of the leaf node is less than 0.5m, then that leaf node will not be segmented. This process results in larger leaf nodes in sparse feature point regions, ensuring that each leaf node contains at least 5 feature points. In dense feature point regions, the size of each leaf node is not less than a preset size, such as 0.5m, and excess dense feature points are removed using a preferred method. Figure 6 This is a schematic diagram showing the segmentation of the target region provided in this embodiment.

[0067] It is worth noting that for multiple feature points of the edge feature point type and multiple feature points of the planar feature point type, the target region can be segmented using step S401 respectively. That is, based on the initial feature point set... The target region is horizontally segmented, resulting in leaf nodes containing only edge feature points. This is based on the initial feature point set. The target region is horizontally segmented, and each leaf node contains only planar feature points.

[0068] S402. Based on the feature points in each leaf node and the second preset conditions, determine the key feature points in each leaf node and the key feature point descriptions of each key feature point.

[0069] Optionally, after determining each feature point in each leaf node, each key feature point in each leaf node is extracted from the feature points in each leaf node using a second preset condition. Figure 7 This is a schematic diagram of key feature points in each leaf node provided in an embodiment of this application. Figure 8 This is a schematic diagram of a real-world scenario illustrating a key feature point provided in an embodiment of this application. For example... Figure 8 As shown, the key feature points extracted using the quadtree distribution method are evenly distributed throughout the point cloud region.

[0070] In this embodiment, after extracting feature points, a quadtree distribution method is used to reselect key feature points for subsequent frame matching, ensuring that the selected feature points are evenly distributed throughout the point cloud region, thus improving matching efficiency and stability. Using a small number of geometric feature matches instead of point cloud matching significantly reduces the computational load for frame matching. Since this embodiment extracts geometric features from the environment and performs balanced and optimized distribution on these features, compared to using the entire point cloud for localization, this embodiment eliminates the influence of noise, outliers, and local overmatches. By employing matching constraints based on points on lines and points on planes, the mapping and matching accuracy can be further improved.

[0071] Figure 9 A flowchart illustrating the fifth mapping and positioning processing method provided in this application embodiment is shown below. Figure 9 As shown, S302 above, which determines whether each initial feature point is a feature point based on the three-dimensional coordinates of each initial feature point in the initial feature point set, may include: S501. Traverse each initial feature point, and for the current initial feature point, determine the associated initial feature point corresponding to the current initial feature point.

[0072] Specifically, all initial feature points in the initial feature point set can be placed into a KD-Tree. Then, a preset number of initial feature points closest to the current initial feature point can be selected from the KD-Tree as the associated initial feature points corresponding to the current initial feature point. For example, 5 associated initial feature points can be selected.

[0073] S502. Based on the current initial feature point and the associated initial feature points, determine the inertia matrix corresponding to the current initial feature point.

[0074] The inertia matrix includes an eigenvalue matrix and an eigenvector matrix. The eigenvalue matrix includes at least one eigenvalue, and the eigenvector matrix includes a direction vector that corresponds one-to-one with each eigenvalue.

[0075] Specifically, the centroid coordinates of the current initial feature point are first obtained using formula (ii). Then, the inertia matrix of the current initial feature point is obtained using the centroid coordinates of the current initial feature point and formula (iii).

[0076] Formula (II) in, Let x be the x-coordinate of the i-th initial feature point in the initial feature point set. Let y be the y-coordinate of the i-th initial feature point in the initial feature point set. Let r be the z-coordinate of the i-th initial feature point in the initial feature point set, r be the total number of the current initial feature point and associated initial feature points (e.g., if there are 4 associated initial feature points, then r = 5), and M be the centroid coordinates.

[0077] Formula (3) in, Let x be the x-coordinate of the i-th initial feature point in the initial feature point set. Let y be the y-coordinate of the i-th initial feature point in the initial feature point set. Let be the z-coordinate of the i-th initial feature point in the initial feature point set, and let r be the total number of the current initial feature point and associated initial feature points. Let x be the x-coordinate of the centroid. Let y be the centroid. Let z be the z-coordinate of the centroid.

[0078] Optionally, after obtaining the inertia matrix, eigenvalue decomposition is performed on the inertia matrix to obtain... , where D is the eigenvalue matrix of I, and A is the eigenvector.

[0079] S503. Determine whether the current initial feature point is a feature point based on the eigenvalues ​​in the eigenvalue matrix.

[0080] Optionally, for line features, points are distributed along the main direction, while there are almost no points in the other two directions. Therefore, for initial feature points of the edge feature point type, if the difference between one eigenvalue in D and zero is greater than a preset difference, and the other two eigenvalues ​​are close to zero, then the initial feature point is a feature point; otherwise, the initial feature point is discarded. Furthermore, for edge feature points, the direction vector corresponding to the largest eigenvalue in D is taken as the direction vector of the feature point.

[0081] Optionally, for an initial feature point of planar feature point type, if the minimum eigenvalue in D of the initial feature point is close to zero, and the difference between the other two eigenvalues ​​and zero is greater than a preset difference, then the initial feature point is retained, i.e., it is used as a feature point. Otherwise, the initial feature point is discarded. Furthermore, the direction vector corresponding to the minimum eigenvalue in D is used as the direction vector of the feature point.

[0082] Figure 10 This is a flowchart illustrating the sixth mapping and positioning processing method provided in the embodiments of this application, as shown below. Figure 10As shown, in step S402 above, determining each key feature point in each leaf node and its key feature point description based on each feature point in each leaf node and the second preset condition may include: S601. Traverse each feature point. For the current feature point, determine the parallelism between the current feature point and other feature points based on the direction vector of the current feature point and the direction vectors of other feature points.

[0083] Here, each feature point refers to a feature point in a leaf node.

[0084] Optionally, for edge feature point types: it can be done through the formula To calculate the parallel results between the current feature point and all other feature points, where, The direction vector of the current feature point. Let be the direction vector of each of the other feature points. If the line containing the current feature point is parallel to the lines containing other feature points, then it means that the line containing the current feature point is parallel to the lines containing other feature points; otherwise, it means that the line containing the current feature point is parallel to the lines containing other feature points. The line containing the current feature point is not parallel to the lines containing other feature points.

[0085] Optionally, for planar feature types, the parallelism result can be determined by calculating the normal angle between the plane containing the current feature point and the planes containing other feature points. Specifically, this is done using the formula... Calculate the included normal angle, if If the angle is less than the preset threshold, the parallel result is considered parallel; if... If the angle is greater than or equal to the preset angle threshold, the parallel result will be non-parallel.

[0086] S602. Based on the parallel results, the direction vector and three-dimensional coordinates of the current feature point, and the direction vectors and three-dimensional coordinates of other feature points, determine the distance and angle between the current feature point and other feature points.

[0087] Optionally, for edge feature point types: the angle between the current feature point and all other feature points refers to the angle between the line containing the current feature point and the lines containing all other feature points. For planar feature point types, the angle between the current feature point and all other feature points refers to the angle between the face containing the current feature point and the faces containing all other feature points. Specifically, if the parallel result is parallel, then the formula is used. This is used to calculate the distance between the current feature point and other feature points; if the parallel result is not parallel, then the formula is used. This is used to calculate the distance between the current feature point and other feature points. The angle between the current feature point and each other feature point is calculated using the formula... This can be calculated.

[0088] in, The direction vector of the current feature point. For the direction vectors of the other feature points, The three-dimensional coordinates of the current feature point. These are the three-dimensional coordinates of the other feature points.

[0089] Optionally, for planar feature point types: when the parallel result is parallel, it is determined by the formula. To calculate the distance between the face containing the current feature point and the faces containing all other feature points, the angle between the face containing the current feature point and the faces containing all other feature points is calculated using S601. If the parallel results are not parallel, then there is no need to calculate the distance.

[0090] S603. If the distance or angle meets the second preset condition, then the current feature point is taken as the key feature point, and the feature point description of the current feature point is taken as the key feature point description.

[0091] Optionally, for edge feature point types, the second preset condition is that the distance between the current feature point and other feature points is greater than a first preset distance threshold or the angle is greater than a first preset angle threshold. If the distance or angle between the current feature point and each other feature point meets the second preset condition, then the current feature point is regarded as a key feature point. The number of key feature points extracted from each leaf node cannot exceed a preset value, such as 5. If the number of feature points that meet the second preset condition exceeds the preset value, they can be sorted in ascending order of z-coordinate, and a preset number of key feature points can be selected sequentially in ascending order of z-coordinate.

[0092] Optionally, for planar feature point types, the second preset condition is that the distance between the current feature point and other feature points is greater than a second preset distance threshold or the angle is greater than a second preset angle threshold. If the distance or angle between the current feature point and other feature points meets the second preset condition, then the current feature point is regarded as a key feature point. The number of key feature points extracted from each leaf node cannot exceed a preset value, such as 5. If the number of feature points that meet the second preset condition exceeds the preset value, they can be sorted in ascending order of their z-coordinates, and a preset number of key feature points can be selected sequentially in ascending order of their z-coordinates.

[0093] The purpose of prioritizing the z-axis is to ensure that the selected key feature points represent the overall contour of the feature points distributed along the z-axis.

[0094] In this embodiment, by extracting key feature points, the number of feature points for subsequent parameter matching can be significantly reduced, improving real-time performance. Furthermore, evenly distributed key feature points are beneficial for detecting distance loop closures.

[0095] Figure 11 A flowchart illustrating the seventh mapping and positioning processing method provided in this application embodiment is shown below. Figure 11 As shown, in S103 above, determining the initial pose of the current trajectory point based on driving data, current feature information, the previous feature information corresponding to the previous moment, the robot's pose at the previous moment, and the pose of the previous trajectory point corresponding to the previous keyframe can include: S701. Based on driving data, current feature information, and the previous feature information corresponding to the previous moment, determine the pose transformation matrix of the lidar from the previous moment to the current moment.

[0096] Specifically, the current feature information used in step S701 refers to multiple key feature points in the current feature information and the key feature point description of each key feature point; the previous feature information refers to multiple feature points in the previous feature information and the feature point description of each feature point.

[0097] Optionally, after determining the pose transformation matrix from the previous moment to the current moment, a factor map can be constructed based on this pose transformation matrix and the driving data collected from the previous moment to the current moment. The factor map is then optimized using the GTSAM library to obtain the zero-bias error of the IMU at the current moment. During motion compensation in S201, the zero-bias error at the current moment is used to correct the IMU data at the next moment, thereby improving the motion compensation accuracy. That is, in step S201, the zero-bias error at the previous moment is used to compensate for the driving data at the current moment, and then the compensated driving data is used to perform distortion compensation processing on each point cloud in the current point cloud set.

[0098] S702. Based on the pose transformation matrix, the robot's pose at the previous moment, and the pose of the previous trajectory point corresponding to the previous keyframe, determine the initial pose of the current trajectory point, and determine whether to use the key feature point frame corresponding to the current trajectory point as a keyframe.

[0099] Alternatively, it can be done through a formula The initial pose of the current trajectory point is obtained, where, Here is the pose transformation matrix. This represents the pose of the lidar in the robot's coordinate system. This represents the robot's pose at the previous moment. This represents the initial pose of the current trajectory point. For the current moment, This refers to the previous moment.

[0100] If the distance between the current trajectory point and the previous trajectory point corresponding to the previous keyframe is greater than a third preset distance threshold, or if the angular change of the current trajectory point is greater than a third preset angle threshold, then the key feature point frame corresponding to the current trajectory point is taken as a keyframe. The distance between the current trajectory point and the previous trajectory point corresponding to the previous keyframe can be calculated based on the initial pose of the current trajectory point and the pose of the previous trajectory point.

[0101] Optionally, after determining the key feature point frame corresponding to the current trajectory point as the key frame, the three-dimensional coordinates of each key feature point in the key feature point frame in the laser coordinate system can be converted into three-dimensional coordinates in the map coordinate system. Specifically, this can be done using the formula... Perform coordinate transformation, where, This represents the robot's pose at the previous moment. This represents the pose of the lidar in the robot's coordinate system. This refers to the three-dimensional coordinates of each key feature point in the key feature point frame in the laser coordinate system.

[0102] Figure 12 A flowchart illustrating the eighth mapping and positioning processing method provided in this application embodiment is shown below. Figure 12 As shown, in step S701 above, determining the pose transformation matrix of the lidar from the previous moment to the current moment based on driving data, current feature information, and the previous feature information corresponding to the previous moment can include: S801. Based on the driving data, perform reprojection processing on multiple key feature points in the current feature information to obtain a reprojection set.

[0103] Among them, multiple key feature points in the current feature information can be used To represent it. Specifically, it can be based on the previous moment. up to the current moment From the driving data, a rough motion change of the lidar is obtained. and based on this Will Reprojection to time The reprojection set is obtained. At this moment, the lidar has moved, but based on the lidar's observations... Compared with the previous feature information Feature points at the same location in the same region remain at the same location; it is only necessary to... Find the match Keypoint matching can be completed using the nearest feature point in the middle. Among them, It refers to a set of multiple feature points in the previous feature information, which includes multiple previous feature points.

[0104] S802. Perform matching processing on each projection feature point in the reprojection set and each previous feature point in the previous feature information, extract each matching previous feature point that matches each projection feature point from each previous feature point in the previous feature information, and obtain a set of matching feature points.

[0105] Specifically, iterate through each projection feature point in the reprojection set. For the current projection feature point encountered during the reprojection process, it can be obtained from... Extract the five nearest preceding feature points to the current projected feature point. Compare the feature point descriptions of the current projected feature point with those of each of the five preceding feature points, considering differences in feature type, distance, and angle. If a preceding feature point exists among the five preceding feature points that matches the current projected feature point, then that preceding feature point is saved to the matching feature point set. In the middle. After traversing all the projected feature points in the reprojection set, it can be obtained from Extract all of the same The previous feature point is matched for each projected feature point in the matrix, thus obtaining the set of matched feature points. .

[0106] S803. Determine the pose transformation matrix based on each previous matching feature point in the matching feature point set and multiple key feature points in the current feature information.

[0107] Optionally, Each matching previous feature point in the data and Each key feature point in the image is a matching point that corresponds one-to-one with the edge features or planar features in the laser point cloud of the two frames. laser point cloud data at any given time and The laser point cloud data corresponds one-to-one with the matching points. Then, based on... The three-dimensional coordinates of each matching previous feature point and The pose transformation matrix is ​​determined by analyzing the three-dimensional coordinates of each key feature point.

[0108] In this embodiment, a geometric feature matching method is used, which improves computational efficiency and real-time performance compared to the traditional computationally intensive NDT or ICP methods.

[0109] Figure 13 A flowchart illustrating the ninth mapping and positioning processing method provided in this application embodiment is shown below. Figure 13 As shown, in step S105 above, determining the target pose of the current trajectory point based on driving data, the current trajectory point, historical trajectory points corresponding to at least one historical keyframe, the current keyframe, and the initial pose of the current trajectory point may include: S901. Based on the distance between the current trajectory point and each historical trajectory point, determine whether to use the current trajectory point as the trajectory point to be looped back.

[0110] Specifically, the distances between the current trajectory point and each historical trajectory point are compared. If the distance between the current trajectory point and a certain historical trajectory point is less than a fourth preset distance threshold, and the current time corresponding to the current trajectory point exceeds the previous loop duration by a preset duration, then the current trajectory point is designated as a trajectory point to be looped back, and step S902 is executed below. If the current trajectory point is not designated as a trajectory point to be looped back, then step S905 is executed below.

[0111] S902. If the current trajectory point is used as the trajectory point to be looped back, then a preset number of associated historical keyframes are determined based on the current trajectory point, and an image to be matched is generated based on each associated historical keyframe, and multiple intermediate sub-images are generated based on the current keyframe.

[0112] Among them, the preset number of associated historical keyframes are local loopback data.

[0113] Specifically, a preset number of associated historical keyframes before and after the current trajectory point can be extracted. The maximum 3D coordinates of all key feature points in each associated historical keyframe in the map coordinate system are then calculated. and minimum three-dimensional coordinates It can use 0.1 pixels as the width and height, and... For pixel values, Using the image origin, project all key feature points from each associated historical keyframe onto the image to generate the image to be matched. If there are multiple points at the same pixel, retain the key feature point with the smallest z-coordinate.

[0114] Optionally, the current keyframe is projected into each key feature point in each associated historical keyframe, and each key feature point in the current keyframe is projected into an image. The resulting image is then rotated 360° according to a preset angle step, with each rotation step corresponding to an intermediate sub-image, thereby obtaining multiple intermediate sub-images.

[0115] S903. Align each intermediate sub-image with the image to be matched in turn to obtain the target overlap position and the target intermediate sub-image.

[0116] Optionally, to prevent matching failures due to quantization errors and to make it easier for intermediate sub-images to match the image to be matched, Gaussian blurring is applied to both the image to be matched and each intermediate sub-image, causing pixel values ​​to diffuse outwards in a decreasing manner. Specifically, a Gaussian kernel is used to perform convolution operations on the image to be matched and the intermediate sub-images. Through image projection and Gaussian blurring, the Gaussian distribution characteristics of key feature points at the lowest Z-axis of the feature point cloud within the plane can be obtained. The purpose of this processing is to ensure that when an intermediate sub-image matches near a feature point, a small cumulative matching error is generated; when no match is possible, a large cumulative matching error is generated; and the higher the matching degree, the smaller the cumulative matching error.

[0117] Optionally, the center pixels of each intermediate sub-image are sequentially aligned with each pixel of the image to be matched. By traversing each pixel value of the intermediate sub-image and the image to be matched, the pixel value deviation between them is calculated, and the cumulative matching error between the intermediate sub-image and the image to be matched is obtained by accumulating the deviation. Specifically, the cumulative matching error can be calculated using the following formula (iv) to obtain the cumulative matching deviation between each intermediate sub-image and the image to be matched at different positions.

[0118] Formula (IV) in, For the m-th intermediate sub-image and the image to be matched in Accumulated mismatch at location, Let be the pixel value of the m-th intermediate sub-image. The pixel values ​​of the image to be matched.

[0119] Optionally, after calculating the cumulative matching deviation between each intermediate sub-image and the image to be matched at different positions using the above formula (iv), the minimum cumulative matching deviation value is selected from the cumulative matching deviations, and the matching position corresponding to the minimum cumulative matching deviation value is set to the position. The intermediate subgraph corresponding to the minimum cumulative mismatch value will be used as the target overlap location. As the target intermediate subgraph.

[0120] S904. Determine the initial loop pose at the current trajectory point based on the identifier of the target intermediate sub-image and the target overlap position. Optimize the initial loop pose based on the initial loop pose, associated historical keyframes, and the current keyframe to obtain the target pose of the current trajectory point.

[0121] Optionally, after obtaining the identifier of the target intermediate sub-image and the target overlap position, the following formula can be used. The initial loop pose of the current trajectory point is calculated, where, The external parameter matrix of the laser relative to the robot chassis coordinate system. This represents the pose of the lidar in the map coordinate system. ,in, , . Where r is the image resolution. , Let be the map coordinates of the origin of the image to be matched, where The target overlaps at the same location. ,in, This is the identifier for the intermediate subgraph of the target.

[0122] Optionally, after obtaining the initial loop closure pose of the current trajectory point, the initial loop closure pose can be optimized based on the initial loop closure pose, all key feature points in a preset number of associated historical keyframes, and key feature points in the current keyframe to obtain the target pose. Specifically, all key feature points in the associated historical keyframes can be used as the new current point cloud set, and the current keyframe can be used as the previous feature information corresponding to the previous moment. Then, steps S102 and S103 are re-executed. That is, using the initial loop closure pose as the initial value, the precise robot target pose can be solved by constructing an optimization equation for the matching error between the point cloud in the new current point cloud set and the point cloud in the current keyframe. .

[0123] Optionally, after obtaining the target pose of the current trajectory point and the current keyframe, the loop closure optimization of the global map can be performed using the factor graph method, thereby eliminating the accumulated error in the lidar mapping process.

[0124] Optionally, during the mapping process, the direction vectors of each key feature point in the current keyframe are expressed using a formula. Projected onto the map coordinate system, where These are the direction vectors of each key feature point. It is the extrinsic parameter of the lidar in the robot's coordinate system. It is the target pose of the robot in the map corresponding to the current keyframe.

[0125] S905. Use the initial pose of the current trajectory point as the target pose.

[0126] In this embodiment, by converting local loop closure data into planar images and performing loop closure matching through image matching, the three-dimensional matching problem is reduced to a two-dimensional matching problem, which improves the success rate and efficiency of loop closure matching. This matching method can be easily accelerated in parallel using GPUs to achieve faster matching speeds.

[0127] The map constructed in this application is based on environmental boundaries and planar geometric feature points. After distribution equalization processing, the feature points are optimized, resulting in a dense concentration of feature points in prominent areas and a sparser concentration in less prominent areas. This allows for the removal of useless point clouds while retaining as much environmental information as possible, reducing map storage space and improving positioning matching efficiency and accuracy. Furthermore, compared to point cloud positioning in existing technologies, the map in this application, because it stores key feature points and feature descriptions, allows for the filtering of poorly performing matches during feature point matching based on feature point type and orientation deviation, thus achieving higher positioning accuracy. Since key feature point positioning requires significantly fewer calculations compared to point cloud matching, the computational load is also reduced.

[0128] The above-described specific implementation method illustrates the process of creating an environmental map. After the map is created, the robot can be located based on the map. The following specific embodiments will describe the robot localization process in detail.

[0129] Optionally, during the localization process, point cloud data collected by the robot is acquired, processed into a planar image, and then matched with keyframes in the aforementioned map to obtain the robot's first pose in the global map. After obtaining the first pose, as the robot moves, its pose can be predicted using motion data collected by an inertial measurement unit. The collected point cloud data is placed into a KD-Tree, and key feature points are obtained through feature processing. For each key feature point, the five nearest points are extracted in the KD-Tree. By verifying the feature point type and orientation angle, a feature point matching pair corresponding to the key feature point is obtained. Then, the matching error is optimized based on the feature point matching pair, and the robot's target position is calculated. If the robot loses its matching relationship with the map during operation due to laser obstruction, manual remote control, or other reasons, resulting in excessive matching error, relocalization can be achieved by re-initializing the localization process.

[0130] In this embodiment, location initialization and location retrieval do not require manual initialization commands; they can be performed autonomously through a global search. Furthermore, the search process can be easily accelerated in parallel using hardware such as GPUs.

[0131] Figure 14 This is a schematic diagram of an apparatus for a mapping and positioning processing method provided in an embodiment of this application, as shown below. Figure 14 As shown, the device includes: The acquisition module 1001 is used to acquire the current point cloud set collected by the hemispherical lidar on the robot at the current trajectory point in the target area at the current moment, as well as the robot driving data collected by the inertial measurement device. The first determining module 1002 is used to determine the current feature information corresponding to the current time based on the driving data and each point cloud in the current point cloud set, and to generate a key feature point frame corresponding to the current trajectory point based on the current feature information. The current feature information includes multiple feature points, feature point descriptions of each feature point, multiple key feature points, and key feature point descriptions of each key feature point. The second determining module 1003 is used to determine the initial pose of the current trajectory point based on the driving data, the current feature information, the previous feature information corresponding to the previous moment of the current moment, the pose of the robot at the previous moment, and the pose of the previous trajectory point corresponding to the previous key frame, and to determine whether to take the key feature point frame corresponding to the current trajectory point as the current key frame. The third determining module 1004 is used to determine the target pose of the current trajectory point based on the current trajectory point, the historical trajectory point corresponding to at least one historical keyframe, the current keyframe, and the initial pose of the current trajectory point, and to build a map based on the target pose and the current keyframe to obtain a target map, and to locate the robot based on the target map.

[0132] Optionally, the first determining module 1002 is specifically used for: Based on the driving data, distortion compensation processing is performed on each point cloud in the current point cloud set, and time alignment processing is performed on each point cloud to obtain the processed point cloud set. Feature points are extracted from each point cloud in the processed point cloud set to obtain multiple feature points and feature point descriptions for each feature point. The multiple feature points belong to either edge feature point type or planar feature point type. The multiple feature points are subjected to distribution equalization processing and key feature point extraction processing to obtain the multiple key feature points and the key feature point description of each key feature point.

[0133] Optionally, the first determining module 1002 is specifically used for: Based on each point cloud and its associated point cloud, the type of each point cloud is determined to obtain an initial feature point set, wherein the initial feature points in the initial feature point set belong to either edge feature point type or planar feature point type. Based on the three-dimensional coordinates of each initial feature point in the initial feature point set, determine whether each initial feature point is a feature point. If so, use the initial feature point as the feature point and determine the feature point description of each feature point.

[0134] Optionally, the first determining module 1002 is specifically used for: The target region is horizontally segmented to obtain multiple leaf nodes, and the number of feature points in each leaf node satisfies a first preset condition. Based on each feature point in each leaf node and the second preset condition, determine each key feature point in each leaf node and the key feature point description of each key feature point.

[0135] Optionally, the first determining module 1002 is specifically used for: Traverse each of the initial feature points, and for the current initial feature point, determine the associated initial feature point corresponding to the current initial feature point; Based on the current initial feature point and the associated initial feature point, the inertia matrix corresponding to the current initial feature point is determined. The inertia matrix includes an eigenvalue matrix and an eigenvector matrix. The eigenvalue matrix includes at least one eigenvalue, and the eigenvector matrix includes a direction vector that corresponds one-to-one with each eigenvalue. Determine whether the current initial feature point is the feature point based on each eigenvalue in the eigenvalue matrix.

[0136] Optionally, the first determining module 1002 specifically includes: Traverse each feature point, and for the current feature point, determine the parallelism between the current feature point and each of the other feature points based on the direction vector of the current feature point and the direction vectors of the other feature points. Based on the parallel results, the direction vector and three-dimensional coordinates of the current feature point, and the direction vectors and three-dimensional coordinates of the other feature points, determine the distance and angle between the current feature point and the other feature points. If the distance or the angle satisfies the second preset condition, then the current feature point is taken as the key feature point, and the feature point description of the current feature point is taken as the key feature point description.

[0137] Optionally, the second determining module 1003 is specifically used for: Based on the driving data, the current feature information, and the previous feature information corresponding to the previous moment, the pose transformation matrix of the lidar from the previous moment to the current moment is determined; Based on the pose transformation matrix, the robot's pose at the previous moment, and the pose of the previous trajectory point corresponding to the previous keyframe, the initial pose of the current trajectory point is determined, and it is determined whether to use the key feature point frame corresponding to the current trajectory point as the current keyframe.

[0138] Optionally, the second determining module 1003 is specifically used for: Based on the driving data, multiple key feature points in the current feature information are reprojected to obtain a reprojection set. The projected feature points in the reprojection set and the previous feature points in the previous feature information are matched. The matching previous feature points that match each of the projected feature points are extracted from each of the previous feature points in the previous feature information to obtain the matching feature point set. The pose transformation matrix is ​​determined based on each previous matching feature point in the set of matching feature points and multiple key feature points in the current feature information.

[0139] Optionally, the third determining module 1004 has the function of: Based on the distance between the current trajectory point and each of the historical trajectory points, determine whether to use the current trajectory point as a trajectory point to be looped back; If the current trajectory point is determined as the trajectory point to be looped back, then a preset number of associated historical keyframes are determined based on the current trajectory point, and an image to be matched is generated based on each of the associated historical keyframes, and multiple intermediate sub-images are generated based on the current keyframe. Each of the intermediate sub-images is sequentially aligned with the image to be matched to obtain the target overlap position and the target intermediate sub-image; The initial loop pose at the current trajectory point is determined based on the identifier of the target intermediate sub-image and the target overlap position. The initial loop pose is then optimized based on the initial loop pose, the associated historical keyframes, and the current keyframe to obtain the target pose at the current trajectory point. If it is determined that the current trajectory point will not be used as the trajectory point to be looped back, then the initial pose will be used as the target pose.

[0140] Figure 15 This is a structural block diagram of an electronic device 1100 provided in an embodiment of this application. (See diagram below.) Figure 15 As shown, the electronic device may include: a processor 1101 and a memory 1102.

[0141] Optionally, a bus 1103 may also be included, wherein the memory 1102 is used to store machine-readable instructions executable by the processor 1101. When the electronic device 1100 is running, the processor 1101 and the memory 1102 communicate with each other via the bus 1103, and the processor 1101 executes the machine-readable instructions to perform the method steps in the above method embodiments.

[0142] This application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, performs the method steps described in the above-described mapping and positioning processing method embodiments.

[0143] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems and devices described above can be referred to the corresponding processes in the method embodiments, and will not be repeated here. In the several embodiments provided in this application, it should be understood that the disclosed systems, devices, and methods can be implemented in other ways. The device embodiments described above are merely illustrative. For example, the division of modules is only a logical functional division, and in actual implementation, there may be other division methods. Furthermore, multiple modules or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed mutual coupling or direct coupling or communication connection can be through some communication interfaces; the indirect coupling or communication connection of devices or modules can be electrical, mechanical, or other forms.

[0144] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. If the functions are implemented as 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 this application, in essence, or the part that contributes to the prior art, or part of the 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 to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes: USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, optical disks, and other media capable of storing program code.

[0145] The above are merely specific embodiments of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application.

Claims

1. A mapping and localization processing method, characterized in that, The method comprises: acquiring a current point cloud set collected by a hemispherical laser radar on a robot for a target area at a current trajectory point at a current time and robot driving data collected by an inertial measurement device; determining current feature information corresponding to the current time according to the driving data and each point cloud in the current point cloud set, and generating a key feature point frame corresponding to the current trajectory point according to the current feature information, wherein the current feature information comprises a plurality of feature points, feature point descriptions of each feature point, a plurality of key feature points and key feature point descriptions of each key feature point; determining an initial pose of the current trajectory point according to the driving data, the current feature information, previous feature information corresponding to a previous time of the current time, a pose of the robot at the previous time and a previous trajectory point pose corresponding to a previous key frame, and determining whether to take the key feature point frame corresponding to the current trajectory point as a current key frame; if yes, determining a target pose of the current trajectory point according to the current trajectory point, at least one historical trajectory point corresponding to a historical key frame, the current key frame and the initial pose of the current trajectory point, and performing mapping based on the target pose and the current key frame to obtain a target map, and positioning the robot based on the target map.

2. The mapping and localization process of claim 1, wherein, The method comprises: performing distortion compensation processing on each point cloud in the current point cloud set according to the driving data, and performing time alignment processing on each point cloud to obtain a processed point cloud set; performing feature point extraction on each point cloud in the processed point cloud set to obtain a plurality of feature points and feature point descriptions of each feature point, wherein the plurality of feature points belong to an edge feature point type or a plane feature point type; performing distribution equalization processing and key feature point extraction processing on the plurality of feature points to obtain the plurality of key feature points and key feature point descriptions of each key feature point.

3. The mapping and localization process of claim 2, wherein, The method comprises: determining the type of each point cloud according to each point cloud and the associated point cloud corresponding to each point cloud to obtain an initial feature point set, wherein the initial feature points in the initial feature point set belong to an edge feature point type or a plane feature point type; determining whether each initial feature point in the initial feature point set is a feature point according to the three-dimensional coordinates of each initial feature point, and if yes, taking the initial feature point as the feature point and determining the feature point description of each feature point.

4. The mapping and localization process of claim 2, wherein, The method comprises: performing horizontal segmentation processing on the target area to obtain a plurality of leaf nodes, and the number of feature points in each leaf node satisfies a first preset condition; According to each feature point in each leaf node and a second preset condition, each key feature point in each leaf node and a key feature point description of each key feature point are determined.

5. The mapping and localization processing method of claim 3, wherein, The determining whether each initial feature point in the initial feature point set is a feature point according to the three-dimensional coordinates of each initial feature point in the initial feature point set comprises: traversing each initial feature point, and determining, for a current initial feature point traversed, an associated initial feature point corresponding to the current initial feature point; determining an inertia matrix corresponding to the current initial feature point according to the current initial feature point and the associated initial feature point, the inertia matrix comprising an eigenvalue matrix and an eigenvector matrix, the eigenvalue matrix comprising at least one eigenvalue, and the eigenvector matrix comprising a directional vector corresponding to each eigenvalue in one-to-one correspondence; determining whether the current initial feature point is the feature point according to each eigenvalue in the eigenvalue matrix.

6. The mapping and localization process method of claim 4, wherein, The determining each key feature point in each leaf node and a key feature point description of each key feature point according to each feature point in each leaf node and a second preset condition comprises: traversing each feature point, and determining, for a current feature point traversed, a parallel result between the current feature point and each other feature point according to a directional vector of the current feature point and directional vectors of each other feature point; determining a distance and an angle between the current feature point and each other feature point according to each parallel result, the directional vector and the three-dimensional coordinates of the current feature point, and the directional vectors and the three-dimensional coordinates of each other feature point; if the distance or the angle satisfies the second preset condition, regarding the current feature point as the key feature point, and regarding a feature point description of the current feature point as the key feature point description.

7. The mapping and localization process of claim 1, wherein, The determining the initial pose of the current track point and determining whether to regard a key feature point frame corresponding to the current track point as a current key frame according to the driving data, the current feature information, previous feature information corresponding to a previous moment of the current moment, a pose of the robot at the previous moment, and a previous track point pose corresponding to a previous key frame comprises: determining a pose transformation matrix of the lidar between the previous moment and the current moment according to the driving data, the current feature information, and the previous feature information corresponding to the previous moment of the current moment; determining the initial pose of the current track point and determining whether to regard the key feature point frame corresponding to the current track point as the current key frame according to the pose transformation matrix, the pose of the robot at the previous moment, and the previous track point pose corresponding to the previous key frame.

8. The mapping and localization process method of claim 7, wherein, The determining the pose transformation matrix of the lidar between the previous moment and the current moment according to the driving data, the current feature information, and the previous feature information corresponding to the previous moment of the current moment comprises: performing reprojection processing on a plurality of key feature points in the current feature information according to the driving data to obtain a reprojection set; Match each projection feature point in the projection set with each previous feature point in the previous feature information, extract each matched previous feature point from each previous feature point in the previous feature information, and obtain a matched feature point set; Determine the pose transformation matrix according to each matched previous feature point in the matched feature point set and a plurality of key feature points in the current feature information.

9. The mapping and localization process of claim 1, wherein, The method comprises the following steps: Determine whether to take the current trajectory point as a to-be-looped trajectory point according to the distance between the current trajectory point and each historical trajectory point; If it is determined to take the current trajectory point as the to-be-looped trajectory point, determine a preset number of associated historical key frames according to the current trajectory point, generate a to-be-matched image according to each associated historical key frame, and generate a plurality of intermediate subgraphs according to the current key frame; Align each intermediate subgraph with the to-be-matched image in sequence to obtain a target overlap position and a target intermediate subgraph; Determine an initial loop pose of the current trajectory point according to the target intermediate subgraph and the target overlap position, and optimize the initial loop pose according to the associated historical key frames and the current key frame to obtain the target pose of the current trajectory point; If it is determined not to take the current trajectory point as the to-be-looped trajectory point, take the initial pose as the target pose.

10. A robot, characterized in that The robot comprises a memory, a processor, a hemispherical laser radar, and an inertial measurement device; The hemispherical laser radar is configured to collect a point cloud set of a target region; The inertial measurement device is configured to collect driving data of the robot; The memory stores a computer program executable by the processor, and the processor executes the computer program to implement the steps of the mapping and positioning processing method according to any one of claims 1-9.

Images