A large scene graph map switching method, system and device and a storage medium

Abstract

The application provides a large scene graph switching method, system and device and a storage medium, and the method comprises the following steps: S1, constructing a point cloud map and a graph database of a large scene; S2, robot positioning according to the graph map and real-time switching of the graph. The application firstly cuts the high-precision point cloud map of the complete large scene according to the graph, which is used for constructing the graph database of the complete large scene; then dynamically loads the graph around the current position of the robot according to the graph database, which is used for composing the graph map of the local large scene, and is used for real-time calculation of the position of the current robot in the large scene; and according to the current robot position and the predicted action path, it is decided which graphs in the graph database are switched to at present, and the graph map of the local large scene is updated in real time; the robot can realize fast and real-time graph switching in the super large scene without feeling, and accurate 3D space positioning can be realized on the hardware device with limited computing power.

Description

Technical Field

[0001] This invention relates to a large-scale scene map switching strategy, and more particularly to a large-scale scene primitive map switching method, system, device and storage medium based on real-time path planning, belonging to the field of substation equipment inspection robot technology. Background Technology

[0002] Currently, robot localization mainly employs ultrasonic navigation and positioning technology, visual navigation and positioning technology, GPS global positioning system, optical reflection navigation and positioning technology, WIFI-SLAM, and LIDAR-SLAM (radar mapping and localization). If robots are to perform repetitive tasks in the same environment, map building is a powerful solution. The main methods for building reusable maps include visual navigation and positioning technology, and radar localization and mapping technology.

[0003] The radar mapping and localization technology includes a mapping phase and a localization phase. First, in the mapping phase, a point cloud map of the surrounding environment is constructed using LiDAR. Then, in the localization phase, the radar point cloud map constructed in the mapping phase is used to locate the inspection robot. The main implementation scheme is as follows:

[0004] 1. Mapping stage:

[0005] 1-1. Use a lidar sensor to perceive the surrounding environment of the scene and obtain radar point cloud data frame by frame.

[0006] 1-2. Use an IMU (Inertial Measurement Unit) to remove motion distortion from radar point clouds;

[0007] 1-3. Using the radar point cloud ICP algorithm (Iterative Closest Points), the relative pose transformation between two radar point clouds is calculated. Through the pose transformation, all radar point cloud data are fused together to form a complete scene point cloud map.

[0008] 1-4. Reduce the cumulative error that occurs throughout the mapping process by using loop closure optimization.

[0009] 2. Positioning Phase:

[0010] 2-1. Using the point cloud map constructed during the mapping phase, and combining it with the real-time acquisition of each frame of radar point cloud by LiDAR, the position of the inspection robot's radar in the point cloud map is calculated in real time.

[0011] However, the above methods have certain technical drawbacks: taking solid-state LiDAR as an example, current mainstream solid-state LiDAR can scan 240,000 points per second. Merging these massive numbers of points creates a huge, dense point cloud map. However, robots with limited computing power cannot function properly based on this point cloud map. Even if each frame of the LiDAR point cloud is downsampled, the robot still cannot handle such a large amount of data. This situation severely limits the working radius of inspection robots, restricting their operation to small-scale environments. This is undoubtedly a huge disaster for robots focused on inspection, as existing robots typically inspect large areas rather than small scenes.

[0012] Therefore, this invention urgently needs to develop a technical solution that can cut and switch large scene maps in order to solve the technical bottlenecks currently encountered by inspection robots. Summary of the Invention

[0013] To address the aforementioned existing technical problems, the purpose of this invention is to provide a method, system, device, and storage medium for switching primitive maps in large scenes based on real-time path planning. This is achieved by using primitive-based point cloud maps to form local maps of large scenes and by updating local maps in real time. This enables robots to perform fast, real-time, and seamless primitive map switching in ultra-large scenes, while also enabling accurate 3D spatial positioning on hardware devices with limited computing power.

[0014] To achieve the above objectives, firstly, the present invention provides a method for switching large scene primitive maps, comprising the following steps:

[0015] S1. Construct point cloud maps and primitive databases for large-scale scenes:

[0016] S11. Use LiDAR to perceive the surrounding environment of a large scene, and construct a preliminary point cloud map of the complete large scene through the LiDAR point cloud of each frame.

[0017] S12. Based on the preliminary point cloud map, optimize the pose transformation from the radar point cloud to the global map in the current frame, and construct an optimized point cloud map of the complete large scene.

[0018] S13. Based on the optimized point cloud map, the accumulated error is eliminated by detecting loop closure information, and a high-precision point cloud map of the complete large scene is constructed.

[0019] S14. Based on the high-precision point cloud map, the primitives are obtained by cutting them according to the primitives, and a complete primitive database of large scene is constructed.

[0020] S2. Locate the robot based on the primitive map and switch primitives in real time:

[0021] S21. Real-time perception of obstacle information during the robot's inspection of large scenes;

[0022] S22. Based on the complete large scene primitive database, dynamically load the primitives around the robot's current position to form a primitive map of the local large scene, and calculate the robot's current position in the large scene in real time based on the current primitive map and LiDAR data.

[0023] S23. Based on the current obstacle information, the current robot position, and the robot target point position, plan the current robot's movement path in real time;

[0024] S24. Based on the current obstacle information and the real-time planned movement path, predict the robot's possible movement path in the next period of time;

[0025] S25. Based on the current robot position and the predicted movement path, decide which primitives in the primitive database to switch to and update the primitive map of the local large scene in real time.

[0026] As described in the above technical solution, firstly, in the mapping stage, a point cloud map of a complete large-scale scene is constructed. This point cloud map is then divided into individual primitives, and a primitive database of the complete large-scale scene is built. Then, in the localization stage, based on the primitive database, the robot can dynamically switch between surrounding primitives according to its current position and perform localization based on a minimap composed of the switched primitives. That is, localization is based on a primitive map of a local large-scale scene, rather than on the traditional point cloud map of a complete large-scale scene. Therefore, the method of this invention does not require processing massive amounts of data, and the robot's working radius is not limited, making it particularly suitable for robot inspection work in large-scale scenes. In this way, not only is fast, real-time, and seamless map switching achieved in ultra-large scenes, but accurate 3D spatial localization is also achieved on hardware devices with limited computing power.

[0027] Furthermore, in the method of the present invention, S11 includes the following steps:

[0028] S11-1. Use lidar to perceive the surrounding environment of a large scene and obtain radar point clouds frame by frame.

[0029] S11-2. Use IMU to remove motion distortion of radar point clouds;

[0030] S11-3. Calculate the pose transformation between two radar point clouds using the ICP algorithm to obtain the pose transformation matrix; transform the multiple radar point clouds to the coordinate system of the first radar point cloud using the pose transformation matrix to form a preliminary point cloud map of a complete large scene.

[0031] Furthermore, in step S12, an image is generated using the intensity information of the radar point cloud, and a visual navigation and positioning algorithm is used to optimize the pose transformation from the radar point cloud in the current frame to the global map. The specific algorithm is as follows:

[0032] S12-1. Maintain a local map, which is formed by transforming the radar point cloud of the latest 100 frames into the coordinate system of a certain radar point cloud; at the same time, maintain a global map, which is formed by transforming the radar point cloud of all frames into the global coordinate system of the first radar point cloud.

[0033] S12-2, Obtain the transformation matrix from local map to global map.

[0034] S12-3, Obtain global map point LP g According to the formula The image position obtained by projection, and the image obtained by using intensity values ​​as image pixel values, are then processed by the Canny edge extraction algorithm to obtain the edge E. i The corresponding j-th global map point gP i j ;

[0035] In the formula: π represents the projection operation, which is used to project three-dimensional points onto a two-dimensional image; f represents the processing function, which is used to transform global map points into a two-dimensional image plane;

[0036] Furthermore, obtain local map points LP. L According to the formula The normal vector n of the i-th edge obtained after the projection image is processed by the Canny edge extraction algorithm. i and image feature points q i ;

[0037] S12-4. Based on the information from the above steps, the formula for optimizing the residual is defined as follows:

[0038]

[0039] In the formula, r i Represents the residual of the i-th feature point; The Jacobian matrix represents the derivative of the i-th residual with respect to the pose; δT represents the bit update increment.

[0040] but

[0041] In the formula, This indicates that the derivative of function f with respect to p is taken. This indicates that the projection function π is differentiated with respect to P; I represents the identity matrix, which is a 3×3 matrix, constructed as follows:

[0042]

[0043] in, The operator represents constructing a three-dimensional vector w into an antisymmetric matrix, where w(x) represents taking the x-th dimension of the three-dimensional vector;

[0044] And order

[0045] In the formula, r represents the residual, J T The Jacobian matrix representing the derivative of the residual with respect to pose;

[0046] S12-5, Using maximum likelihood estimation The calculation formula is:

[0047] max δT logp(v; δT) = min δT (r+J T δT) T (r+J T δT),

[0048] The optimal solution is

[0049] and through Perform pose transformation and update from local map to global map;

[0050] In the formula, the log operator represents the logarithmic operation; The operator indicates that the calculated pose increment is updated to the current pose, thus updating the current pose;

[0051] When ||δT * When || < ε, where ε is the pose update threshold, the result converges, the optimization terminates, and the optimized parameters are... As a pose transformation from a local map to a global map.

[0052] Furthermore, in the method of the present invention, the step of cutting the graphic elements according to the graphic elements in S14 includes the following steps:

[0053] S14-1. Cut each three-dimensional point (x, y, z) of the high-precision point cloud map into two dimensions according to the coordinates of (x, y).

[0054] S14-2. Divide x and y by the primitive unit size in the x and y directions respectively to obtain regions of primitive unit size × primitive unit size, which are primitives.

[0055] Furthermore, in the method of the present invention, the algorithm for predicting the possible action path of the robot in the next period of time in S24 is as follows:

[0056] S24-1. Obtain the contextual information of the environment, estimate the probability that the robot will reach each candidate point in the next step, and select the candidate point with the highest probability.

[0057] S24-2. Based on all the selected points obtained, estimate the trajectory of the robot to each selected point;

[0058] S24-3. Score, rank, and select the trajectories of all selected points, and output the robot's most likely next movement trajectory.

[0059] Furthermore, in the method of the present invention, the decision in S25 regarding which primitives in the primitive database to switch to currently employs the following algorithm:

[0060] S25-1. Based on the robot's movement path over the next period of time, determine whether the primitive has been loaded from the primitive database: if it has been loaded, proceed to the next step; if it has not been loaded, load the corresponding primitive and then proceed to the next step.

[0061] S25-2. Determine whether the currently loaded primitive information is rich enough. The formula for the judgment index f is as follows:

[0062] f=f1(x1)+f2(x2)+f3(x3)+f4(x4),

[0063] In the formula, x1 is the coefficient of variation calculated using the three-dimensional coordinates of each radar point cloud of a primitive, x2 is the number of planes detected by the RANSAC algorithm for the current primitive, x3 is the number of clusters of the current radar point cloud, and x4 is the coefficient of variation calculated using the intensity information of all three-dimensional points of a primitive.

[0064] If the result value of f is ≥ 0.5, it means that the current primitive information is rich enough;

[0065] If the result of f is less than 0.5, it means that the current primitive information is not rich enough, so continue to recursively load primitive information, as shown in the following formula:

[0066]

[0067]

[0068]

[0069]

[0070] In the formula, σ1 is the standard deviation; μ1 is the mean. σ⁴ is the standard deviation; μ⁴ is the mean; e is the natural constant;

[0071] Furthermore, if f1 < 0, then f1 = 0; if f4 < 0, then f4 = 0.

[0072] S25-3. Remove primitive information that the old robot will not use in the future.

[0073] Secondly, the present invention also provides a large-scale scene map switching system, including a mapping part and a positioning part; wherein:

[0074] The mapping component is used to construct point cloud maps and primitive databases for large-scale scenes, including a mapping module, an optimization module, a refinement module, and a segmentation module; wherein:

[0075] The mapping module is used to perceive the surrounding environment of a large scene using LiDAR and to construct a preliminary point cloud map of the complete large scene using the LiDAR point cloud of each frame.

[0076] The optimization module is used to optimize the pose transformation from the radar point cloud to the global map in the current frame based on the preliminary point cloud map, and to build an optimized point cloud map of the complete large scene.

[0077] The refinement module is used to build a high-precision point cloud map of a complete large scene by eliminating accumulated errors through the detection of loop closure information based on the optimized point cloud map.

[0078] The cutting module is used to cut the high-precision point cloud map according to the primitives to obtain primitives and build a complete primitive database of a large scene.

[0079] The localization section is used to locate the robot based on the primitive map and switch primitives in real time. It includes a perception module, a localization module, a real-time path planning module, a prediction module, and a switching decision module; wherein:

[0080] The perception module is used to perceive the current obstacle information of the robot during the inspection of large scenes in real time and send it to the prediction module and the real-time path planning module.

[0081] The positioning module is used to dynamically load the primitives around the robot's current position based on the primitive database of the complete large scene to form a primitive map of the local large scene. Based on the current primitive map and LiDAR data, it calculates the robot's current position in the large scene in real time and sends it to the real-time path planning module and the switching decision module.

[0082] The real-time path planning module is used to plan the robot's movement path in real time based on the current obstacle information, the current robot position, and the robot target point position, and send it to the prediction module.

[0083] The prediction module is used to predict the robot's possible movement path in the next period of time based on the current obstacle information and the real-time planned movement path, and send it to the switching decision module.

[0084] The switching decision module is used to decide which primitives in the primitive database to switch to based on the current robot position and the predicted movement path, and to update the primitive map of the local large scene in real time.

[0085] Furthermore, in the system of the present invention, the mapping module includes an acquisition unit, a correction unit, and a calculation unit; wherein:

[0086] The acquisition unit is used to use lidar to perceive the surrounding environment of a large scene and obtain radar point clouds frame by frame.

[0087] The correction unit is used to remove motion distortion of the radar point cloud using an IMU;

[0088] The computing unit is used to calculate the pose transformation between two radar point clouds according to the ICP algorithm to obtain the pose transformation matrix; the multi-frame radar point clouds are transformed into the coordinate system of the first frame radar point cloud through the pose transformation matrix to form a preliminary point cloud map of a complete large scene.

[0089] Furthermore, the present invention provides a large scene primitive map switching device, comprising: a memory and at least one processor, wherein the memory stores instructions, and the memory and the at least one processor are interconnected via a line;

[0090] The at least one processor invokes the instructions in the memory to cause the mobile robot to perform the steps of any of the above-described large scene primitive map switching methods.

[0091] In addition, the present invention provides a computer-readable storage medium storing instructions, characterized in that the instructions, when executed by a processor, implement the steps of the large scene primitive map switching method described above.

[0092] In summary, this invention adopts a primitive-based method for cutting large-scale scene point cloud maps, combined with a flexible and dynamic primitive switching strategy. This enables the robot to quickly, in real-time, and seamlessly switch local large-scale scene primitive maps even when facing ultra-large scenes. At the same time, it enables fast and accurate 3D spatial positioning on hardware devices with limited computing power, thus freeing the inspection robot from the limitations of scene size and easily realizing the robot's inspection of ultra-large spaces.

[0093] Compared with the prior art, the technical advantages of the present invention are as follows:

[0094] 1. By using the intensity information of the LiDAR to obtain an intensity image and correcting the pose of the LiDAR, the point cloud map of the complete large scene was optimized, improving the positioning accuracy of the robot using LiDAR.

[0095] 2. By adopting a point cloud map segmentation method based on primitives, the point cloud map of a complete large scene is segmented into primitive structures. At the same time, based on the current position of the robot, nine primitives around the robot are dynamically loaded, and the primitive map of a local large scene is formed by the nine primitives for local positioning. This eliminates the need for positioning based on the point cloud map of the entire large scene as in traditional technology, thus improving the robot's ability to perform ultra-large scene inspection work even when computing power is limited.

[0096] 3. A primitive switching strategy is adopted. Based on a custom point cloud richness judgment index for the power industry scenario, it is determined whether the current primitive data loaded by the robot is sufficient for localization. If not, primitive data can be iteratively loaded, which improves the robot's reaction speed and accuracy in 3D space localization.

[0097] 4. From constructing a complete point cloud map of a large scene, cutting the point cloud map into individual primitives, to the robot using a primitive map of a local large scene for positioning, and quickly, in real time, and seamlessly switching primitives according to a switching strategy, this invention forms a complete set of technical solutions for positioning of inspection robots in large scenes. This greatly expands the actual working radius of the robot, ensures the normal inspection work of the inspection robot in ultra-large spaces, and improves the efficiency of inspection work. Attached Figure Description

[0098] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0099] Figure 1 This is a flowchart illustrating step S1 in an embodiment of the method of the present invention;

[0100] Figure 2 This is a flowchart illustrating step S2 in an embodiment of the method of the present invention;

[0101] Figure 3 This is a 2D schematic diagram of step S14 in the method embodiment of the present invention, which involves cutting three-dimensional points into primitives.

[0102] Figure 4 This is a schematic diagram of step S24, selecting candidate points with high probability, in an embodiment of the method of the present invention.

[0103] Figure 5 A block diagram illustrating the module principle of the mapping component provided in an embodiment of the present invention;

[0104] Figure 6 A block diagram illustrating the module principle of the positioning section provided in an embodiment of the present invention;

[0105] Figure 7 This is a hardware composition principle block diagram provided for an embodiment of the device of the present invention. Detailed Implementation

[0106] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.

[0107] It should be understood that, when used in this application specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, modules and / or units, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, modules, units and / or collections thereof.

[0108] It should also be understood that the term “and / or” as used in this application specification and the appended claims means any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0109] Example 1: An embodiment of the method of the present invention:

[0110] This embodiment is mainly applied to inspection robots in large-scale scenarios, but it can also be applied to other electronic devices, specifically smart devices, servers, or general computers. Smart devices include, but are not limited to, autonomous vehicles, drones, and smart robots.

[0111] This invention provides a method for switching between primitive maps in a large scene, including a mapping stage and a localization stage. First, in the mapping stage, a point cloud map of the complete large scene is constructed, then the point cloud map is divided into primitive blocks, and a primitive database of the complete large scene is built. Then, in the localization stage, based on the primitive database, the robot can dynamically switch between surrounding primitives according to its current position, and use the switched primitives to form a minimap for localization. The specific steps are described below.

[0112] like Figure 1 As shown, S1, the mapping stage: constructing a point cloud map and primitive database for a large scene, including the following steps:

[0113] S11. Use LiDAR to perceive the surrounding environment of a large scene, and construct a preliminary point cloud map of the complete large scene through the LiDAR point cloud of each frame.

[0114] In specific implementation, S11 includes the following steps:

[0115] S11-1. Use lidar to perceive the surrounding environment of a large scene and obtain radar point clouds frame by frame.

[0116] S11-2. Use an IMU (Inertial Measurement Unit) to remove motion distortion from radar point clouds.

[0117] S11-3. Calculate the pose transformation between two radar point clouds using the ICP algorithm (Iterative Closest Points) to obtain the pose transformation matrix. Transform the multiple radar point clouds into the coordinate system of the first radar point cloud using the pose transformation matrix to form a preliminary point cloud map of a complete large scene.

[0118] S12. Based on the preliminary point cloud map, optimize the pose transformation from the radar point cloud of the current frame to the global map, and construct an optimized point cloud map of the complete large scene.

[0119] It should be noted that this step introduces a new pose optimization method. Considering the high density of point clouds in solid-state LiDAR, an image is generated using the intensity information of the radar point cloud. Then, relevant localization algorithms from visual navigation technology are used to optimize the pose transformation from the radar point cloud in the current frame to the global map, including the following steps:

[0120] S12-1. Maintain a local map, which is formed by transforming the radar point cloud of the latest 100 frames into the coordinate system of a certain radar point cloud; at the same time, maintain a global map, which is formed by transforming the radar point cloud of all frames into the global coordinate system of the first radar point cloud.

[0121] S12-2, Obtain the transformation matrix from local map to global map.

[0122] S12-3, Obtain global map points L P g According to the formula The image position obtained by projection, and the image obtained by using intensity values ​​as image pixel values, are then processed by the Canny edge extraction algorithm to obtain the edge E. i The corresponding j-th global map point g P i j ;

[0123] In the formula: π represents the projection operation, which is used to project three-dimensional points onto a two-dimensional image; f represents the processing function, which is used to transform global map points into a two-dimensional image plane.

[0124] Furthermore, obtain local map points. L P L According to the formula The normal vector n of the i-th edge obtained after the projection image is processed by the Canny edge extraction algorithm. i and image feature points q i .

[0125] S12-4. Based on the information from the above steps, the formula for optimizing the residual is defined as follows:

[0126]

[0127] In the formula, r i Represents the residual of the i-th feature point; Let δT represent the Jacobian matrix of the derivative of the i-th residual with respect to pose; δT represents the bit update increment.

[0128] but

[0129] In the formula, This indicates that the derivative of function f with respect to p is taken. This indicates that the projection function π is differentiated with respect to P; I represents the identity matrix, which is a 3×3 matrix, constructed as follows:

[0130]

[0131] in, The operator represents constructing a three-dimensional vector w into an antisymmetric matrix, where w(x) represents taking the x-th dimension of the three-dimensional vector.

[0132] And order

[0133] In the formula, r represents the residual, J T This represents the Jacobian matrix representing the derivative of the residual with respect to the pose.

[0134] S12-5, Using maximum likelihood estimation The calculation formula is:

[0135] max δT logp(v; δT) = min δT (r+J T δT) T (r+J T δT),

[0136] The optimal solution is

[0137] and through Perform pose transformation and update from local map to global map;

[0138] In the formula, the log operator represents the logarithmic operation; The operator indicates that the calculated pose increment is updated to the current pose, thus updating the current pose.

[0139] When ||δT * When || < ε, ε is the pose update threshold, which is taken as 1e-3 here. The result converges, the optimization is terminated, and the optimized parameters are... This serves as a pose transformation from a local map to a global map, resulting in more accurate pose estimation. Furthermore, the intensity image obtained using the intensity information from the LiDAR is used for LiDAR pose correction, improving the positioning accuracy of the LiDAR.

[0140] S13. Since the optimized point cloud map has accumulated errors, the accumulated errors of the map built using the LiDAR are eliminated by detecting loop closure information, that is, the LiDAR device returns to the scanned area after running for a long time, thereby constructing a high-precision point cloud map of the complete large scene.

[0141] S14. Based on the high-precision point cloud map, the primitives are obtained by cutting them according to the primitives, and a complete primitive database of large scene is constructed.

[0142] In specific implementation, the step of cutting the graphic elements according to the graphic elements in S14 includes the following steps:

[0143] S14-1. Cut each three-dimensional point (x, y, z) of the high-precision point cloud map into two dimensions according to the coordinates of (x, y).

[0144] S14-2. Divide x and y by the primitive unit size in the x and y directions respectively to obtain regions of primitive unit size × primitive unit size, which are primitives.

[0145] In this way, all the 3D points of the complete and massive point cloud map, after dividing x and y by the primitive unit size, are categorized into regions of primitive unit size × primitive unit size. These regions are then defined as primitives. For example... Figure 3 As shown, the actual size of the primitive unit is defined as 50 meters. Along the x and y directions of the 3D point cloud map, the entire ultra-large high-precision point cloud map is cut into local point cloud regions of 50 meters × 50 meters at 50-meter intervals. These regions are then used to construct a primitive database for robot positioning during large-scale scene inspection.

[0146] The implementation process of step S1 above constructs a preliminary point cloud map of a complete large scene, and optimizes and refines it. Then, the huge high-precision point cloud map is cut into a piece of primitive structure to build a primitive database that can be reused by the robot when inspecting large scenes. This makes it easier for the robot to dynamically load surrounding primitives for localization based on its current position, rather than loading the entire point cloud map in the traditional technology, thereby reducing the amount of data that the robot operates on.

[0147] like Figure 2 As shown, S2, the localization stage: the robot is localized based on the primitive map and the primitives are switched in real time, including the following steps:

[0148] S21. Real-time perception of obstacle information during the robot's inspection of large-scale scenes.

[0149] S22. Based on the complete large-scale scene primitive database, dynamically load the primitives around the robot's current position to form a primitive map of the local large-scale scene, and calculate the robot's current position in the large-scale scene in real time based on the current primitive map and LiDAR data.

[0150] In practice, nine surrounding primitives can be dynamically loaded based on the robot's current position, and the robot can be positioned using a small map composed of these nine primitives. This means positioning is achieved using a primitive map of a large local scene, rather than the traditional point cloud map based on the entire large scene, thus reducing the amount of data the robot processes. In this way, the robot can perform inspection work in extremely large scenes even with limited computing power.

[0151] S23. Based on the current obstacle information, the current robot position, and the robot target point position, plan the current robot's movement path in real time.

[0152] S24. Based on the current obstacle information and the real-time planned movement path, predict the robot's possible movement path in the next period of time.

[0153] It should be noted that, based on the current obstacle information and the real-time planned movement path, the accuracy of predicting the robot's next location when it is currently at this position, and the global planned path information, it is possible to predict the robot's next global movement path and its movement position over a period of time. Specifically, the algorithm used in S24 to predict the robot's possible movement path over the next period of time is as follows:

[0154] S24-1. Obtain the contextual information of the environment, estimate the probability that the robot will reach each candidate point in the next step, and select the candidate point with the highest probability. For example... Figure 4 As shown, white triangles represent candidate points, black triangles represent points with a high probability of being selected, and rectangles represent robots.

[0155] S24-2. Based on all the selected points obtained, estimate the trajectory of the robot to each selected point.

[0156] S24-3. Score, rank, and select the trajectories of all selected points, and output the robot's most likely next movement trajectory.

[0157] S25. Based on the current robot position and the predicted movement path, decide which primitives in the primitive database to switch to and update the primitive map of the local large scene in real time.

[0158] In specific implementation, the following algorithm is used to determine which primitives in the primitive database to switch to in step S25:

[0159] S25-1. Based on the robot's movement path over the next period of time, determine whether the primitive has been loaded from the primitive database: if it has been loaded, proceed to the next step; if it has not been loaded, load the corresponding primitive and then proceed to the next step.

[0160] S25-2. Determine whether the currently loaded primitive information is rich enough. The formula for the judgment index f is as follows:

[0161] f=f1(x1)+f2(x2)+f3(x3)+f4(x4),

[0162] In the formula, x1 is the coefficient of variation calculated using the three-dimensional coordinates of each radar point cloud of a primitive, x2 is the number of planes detected by the current primitive through the RANSAC (RAndom Sampling Consensus) algorithm, x3 is the number of clusters of the current radar point cloud, and x4 is the coefficient of variation calculated using the intensity information of all three-dimensional points of a primitive.

[0163] If the result value of f is ≥ 0.5, it means that the current primitive information is rich enough;

[0164] If the result of f is less than 0.5, it means that the current primitive information is not rich enough, so continue to recursively load primitive information, as shown in the following formula:

[0165]

[0166]

[0167]

[0168]

[0169] In the formula, σ1 is the standard deviation; μ1 is the mean. σ⁴ is the standard deviation; μ⁴ is the mean; e is the natural constant.

[0170] Furthermore, if f1 < 0, then f1 = 0; if f4 < 0, then f4 = 0.

[0171] S25-3. Remove primitive information that the old robot will not use in the future.

[0172] In this way, based on the aforementioned customized radar point cloud richness judgment index for the power industry scenario, it is possible to determine whether the current map data loaded by the robot is sufficient for positioning. If not, the map data is iteratively loaded, thereby improving the robot's reaction speed and accuracy in 3D spatial positioning.

[0173] The implementation process of step S2 above utilizes the point cloud map and primitive database of the large scene established in the mapping stage, which makes it easier for the robot to dynamically load the surrounding primitives based on the current information and to locate itself based on the small map composed of the loaded primitives. This reduces the amount of data the robot operates on and enables the robot to switch maps quickly, in real time and seamlessly in ultra-large scenes.

[0174] In summary, the method of this invention enables robots to perform rapid, real-time, and seamless map switching in ultra-large scenes, while also enabling accurate 3D spatial positioning on hardware devices with limited computing power.

[0175] Example 2: An embodiment of the system of the present invention:

[0176] Based on the same concept as the method embodiments of this application, this application also provides a large-scale scene primitive map switching system.

[0177] This invention provides a large-scale scene primitive map switching system, including a mapping part and a positioning part, which are described in detail below.

[0178] like Figure 5 As shown, the mapping component is used to construct point cloud maps and primitive databases for large-scale scenes, including a mapping module, an optimization module, a refinement module, and a segmentation module; wherein:

[0179] The mapping module is used to perceive the surrounding environment of a large scene using LiDAR. It constructs a preliminary point cloud map of the complete large scene using the LiDAR point cloud of each frame.

[0180] The optimization module is used to optimize the pose transformation from the radar point cloud of the current frame to the global map based on the preliminary point cloud map, and to build an optimized point cloud map of the complete large scene.

[0181] The refinement module is used to construct a high-precision point cloud map of a complete large scene by eliminating accumulated errors through the detection of loop closure information based on the optimized point cloud map.

[0182] The cutting module is used to cut the high-precision point cloud map according to the primitives to obtain primitives and build a complete primitive database of a large scene.

[0183] like Figure 5 As shown, in other embodiments of the system of the present invention, the mapping module includes an acquisition unit, a correction unit, and a calculation unit; wherein:

[0184] The acquisition unit is used to acquire surrounding environmental data using lidar to obtain radar point clouds frame by frame.

[0185] The aforementioned correction unit is used to remove radar point cloud motion distortion using an IMU to obtain a radar point cloud free of motion distortion.

[0186] The computing unit is used to calculate the pose transformation matrix between two radar point clouds using the ICP algorithm, and then transform the multi-frame radar point clouds into the coordinate system of the first frame radar point cloud to obtain a point cloud map.

[0187] like Figure 6 As shown, the positioning section is used to locate the robot based on the primitive map and switch primitives in real time, including a perception module, a positioning module, a real-time path planning module, a prediction module, and a switching decision module; wherein:

[0188] The perception module is used to perceive the current obstacle information of the robot during the inspection of large scenes in real time and send it to the prediction module and the real-time path planning module.

[0189] The localization module is used to dynamically load the primitives around the robot's current position based on the primitive database of the complete large scene, so as to form a primitive map of the local large scene. Based on the current primitive map and LiDAR data, it calculates the current position of the robot in the large scene in real time and sends it to the real-time path planning module and the switching decision module.

[0190] The real-time path planning module is used to plan the robot's movement path in real time based on the current obstacle information, the current robot position, and the robot target point position, and then send the plan to the prediction module.

[0191] The prediction module is used to predict the robot's possible movement path in the next period of time based on the current obstacle information and the real-time planned movement path, and then send it to the switching decision module.

[0192] The switching decision module is used to decide which primitives in the primitive database to switch to based on the current robot position and the predicted movement path, and to update the primitive map of the local large scene in real time.

[0193] Example 3: An embodiment of the device of the present invention:

[0194] like Figure 7 As shown, the present invention also provides a large scene primitive map switching device, including: at least one processor, and a memory communicatively connected to the at least one processor; in addition, depending on the specific application, any other suitable components may be included.

[0195] The memory stores instructions that can be executed by the at least one processor, which, when executed by the at least one processor, enables the at least one processor to perform the large scene primitive map switching method described above.

[0196] The processor may be a central processing unit (CPU) or other form of processing unit with data processing capability and / or instruction execution capability, and may control other components in the device to perform the desired function.

[0197] The memory may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may include, for example, random access memory (RAM) and / or cache memory. The non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and a processor may execute the instructions to implement the functions of the methods and / or systems of the various embodiments described above.

[0198] Example 4: An embodiment of the storage medium of the present invention:

[0199] The present invention also provides a computer-readable storage medium storing instructions, wherein the instructions, when executed by a processor, implement the steps of the large scene primitive map switching method as described in the above method embodiments.

[0200] The instructions can be written in any combination of one or more programming languages ​​to execute the operations of the embodiments of this application. The programming languages ​​include object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0201] The computer-readable storage medium may be any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may, for example, include, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatuses, or devices, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: electrical connections having one or more wires, portable disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0202] The block diagrams of the systems and apparatuses involved in this invention are merely illustrative examples and are not intended to require or imply that they must be connected, arranged, or configured in the manner shown in the block diagrams. As those skilled in the art will recognize, these systems and apparatuses can be connected, arranged, and configured in any manner. Words such as “comprising,” “including,” “having,” etc., are open-ended terms meaning “including but not limited to,” and are used interchangeably with them. The terms “or” and “and” as used herein refer to the terms “and / or,” and are used interchangeably with them unless the context clearly indicates otherwise. The term “such as” as used herein refers to the phrase “such as but not limited to,” and is used interchangeably with it.

[0203] Furthermore, in the methods, systems, and apparatus of this application, each component or step can be disassembled and / or recombined. These disassemblies and / or recombinations should be considered equivalent solutions of this application.

[0204] It should also be noted that those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0205] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use this application. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein can be applied to other aspects without departing from the scope of this application. Therefore, this application is not intended to be limited to the aspects shown herein, but rather to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0206] Furthermore, this description is not intended to limit the embodiments of this application to the forms disclosed herein. Although numerous exemplary aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, alterations, additions, and sub-combinations thereto. These variations, modifications, alterations, additions, and sub-combinations do not cause the essence of the corresponding technical solutions to depart from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A method for switching large scene primitive maps, characterized in that, Includes the following steps: S1. Construct point cloud maps and primitive databases for large-scale scenes: S11. Use LiDAR to perceive the surrounding environment of a large scene, and construct a preliminary point cloud map of the complete large scene through the LiDAR point cloud of each frame. S12. Based on the preliminary point cloud map, optimize the pose transformation from the radar point cloud to the global map in the current frame, and construct an optimized point cloud map of the complete large scene. S13. Based on the optimized point cloud map, the accumulated error is eliminated by detecting loop closure information, and a high-precision point cloud map of the complete large scene is constructed. S14. Based on the high-precision point cloud map, the primitives are obtained by cutting them according to the primitives, and a complete primitive database of large scene is constructed. S2. Locate the robot based on the primitive map and switch primitives in real time: S21. Real-time perception of obstacle information during the robot's inspection of large scenes; S22. Based on the complete large scene primitive database, dynamically load the primitives around the robot's current position to form a primitive map of the local large scene, and calculate the robot's current position in the large scene in real time based on the current primitive map and LiDAR data. S23. Based on the current obstacle information, the current robot position, and the robot target point position, plan the current robot's movement path in real time; S24. Based on the current obstacle information and the real-time planned movement path, predict the robot's possible movement path in the next period of time; S25. Based on the current robot position and the predicted movement path, decide which primitives in the primitive database to switch to and update the primitive map of the local large scene in real time.

2. The method for switching large scene primitive maps according to claim 1, characterized in that, S11 includes the following steps: S11-1. Use lidar to perceive the surrounding environment of a large scene and obtain radar point clouds frame by frame. S11-2. Use IMU to remove motion distortion of radar point clouds; S11-3. Calculate the pose transformation between two radar point clouds using the ICP algorithm to obtain the pose transformation matrix; transform the multiple radar point clouds to the coordinate system of the first radar point cloud using the pose transformation matrix to form a preliminary point cloud map of a complete large scene.

3. The method for switching large-scale scene primitive maps according to claim 1, characterized in that, S12 generates an image using the intensity information of the radar point cloud and optimizes the pose transformation from the radar point cloud to the global map using a visual navigation and positioning algorithm. The specific algorithm is as follows: S12-1. Maintain a local map, which is formed by transforming the radar point cloud of the latest 100 frames into the coordinate system of a certain radar point cloud; at the same time, maintain a global map, which is formed by transforming the radar point cloud of all frames into the global coordinate system of the first radar point cloud. S12-2, Obtain the transformation matrix from local map to global map. S12-3, Obtain global map points L P g According to the formula The image position obtained by projection, and the image obtained by using intensity values ​​as image pixel values, are then processed by the Canny edge extraction algorithm to obtain the edge E. i The corresponding j-th global map point g P i j ; In the formula: π represents the projection operation, which is used to project three-dimensional points onto a two-dimensional image; f represents the processing function, which is used to transform global map points into a two-dimensional image plane; Furthermore, obtain local map points. L P L According to the formula The normal vector n of the i-th edge obtained after the projection image is processed by the Canny edge extraction algorithm. i and image feature points q i ; S12-4. Based on the information from the above steps, the formula for optimizing the residual is defined as follows: In the formula, r i This represents the residual at the i-th feature point; The Jacobian matrix represents the derivative of the i-th residual with respect to the pose; δT represents the bit update increment. but In the formula, This indicates that the derivative of function f with respect to p is taken. This indicates that the projection function π is differentiated with respect to P; I represents the identity matrix, which is a 3×3 matrix, constructed as follows: in, The operator represents constructing a three-dimensional vector w into an antisymmetric matrix, where w(x) represents taking the x-th dimension of the three-dimensional vector; And order In the formula, r represents the residual, J T The Jacobian matrix represents the derivative of the residual with respect to pose; S12-5, Using maximum likelihood estimation The calculation formula is: max δT logp(v;δT)=min δT (r+J T δT) T (r+J T δT), The optimal solution is and through Perform pose transformation and update from local map to global map; In the formula, the log operator represents the logarithmic operation; The operator indicates that the calculated pose increment is updated to the current pose, thus updating the current pose; When ||δT * When || < ε, where ε is the pose update threshold, the result converges, the optimization terminates, and the optimized parameters are... As a pose transformation from a local map to a global map.

4. The method for switching large scene primitive maps according to claim 1, characterized in that, The step of S14, which involves cutting the graphic elements to obtain the graphic elements, includes the following steps: S14-1. Cut each three-dimensional point (x, y, z) of the high-precision point cloud map into two dimensions according to the coordinates of (x, y). S14-2. Divide x and y by the primitive unit size in the x and y directions respectively to obtain regions of primitive unit size × primitive unit size, which are primitives.

5. A method for switching large-scale scene primitive maps according to claim 1, characterized in that, The algorithm used in S24 to predict the robot's possible movement path in the next period of time is as follows: S24-1. Obtain the contextual information of the environment, estimate the probability that the robot will reach each candidate point in the next step, and select the candidate point with the highest probability. S24-2. Based on all the selected points obtained, estimate the trajectory of the robot to each selected point; S24-3. Score, rank, and select the trajectories of all selected points, and output the robot's most likely next movement trajectory.

6. A method for switching large-scale scene primitive maps according to claim 1, characterized in that, The decision in S25 regarding which primitives in the primitive database to switch to currently uses the following algorithm: S25-1. Based on the robot's movement path over the next period of time, determine whether the primitive has been loaded from the primitive database: if it has been loaded, proceed to the next step; if it has not been loaded, load the corresponding primitive and then proceed to the next step. S25-2. Determine whether the currently loaded primitive information is rich enough. The formula for the judgment index f is as follows: f=f1(x1)+f2(x2)+f3(x3)+f4(x4), In the formula, x1 is the coefficient of variation calculated using the three-dimensional coordinates of each radar point cloud of a primitive, x2 is the number of planes detected by the RANSAC algorithm for the current primitive, x3 is the number of clusters of the current radar point cloud, and x4 is the coefficient of variation calculated using the intensity information of all three-dimensional points of a primitive. If the result value of f is ≥ 0.5, it means that the current primitive information is rich enough; If the result of f is less than 0.5, it means that the current primitive information is not rich enough. Then, the primitive information is recursively loaded, as shown in the following formula: In the formula, σ1 is the standard deviation; μ1 is the mean. σ⁴ is the standard deviation; μ⁴ is the mean; e is the natural constant; Furthermore, if f1 < 0, then f1 = 0; if f4 < 0, then f4 = 0. S25-3. Remove primitive information that the old robot will not use in the future.

7. A large-scale scene primitive map switching system, characterized in that, It includes mapping and positioning components; among which: The mapping component is used to construct point cloud maps and primitive databases for large-scale scenes, including a mapping module, an optimization module, a refinement module, and a segmentation module; wherein: The mapping module is used to perceive the surrounding environment of a large scene using LiDAR and to construct a preliminary point cloud map of the complete large scene using the LiDAR point cloud of each frame. The optimization module is used to optimize the pose transformation from the radar point cloud to the global map in the current frame based on the preliminary point cloud map, and to build an optimized point cloud map of the complete large scene. The refinement module is used to build a high-precision point cloud map of a complete large scene by eliminating accumulated errors through the detection of loop closure information based on the optimized point cloud map. The cutting module is used to cut the high-precision point cloud map according to the primitives to obtain primitives and build a complete primitive database of a large scene. The localization section is used to locate the robot based on the primitive map and switch primitives in real time. It includes a perception module, a localization module, a real-time path planning module, a prediction module, and a switching decision module; wherein: The perception module is used to perceive the current obstacle information of the robot during the inspection of large scenes in real time and send it to the prediction module and the real-time path planning module. The positioning module is used to dynamically load the primitives around the robot's current position based on the primitive database of the complete large scene to form a primitive map of the local large scene. Based on the current primitive map and LiDAR data, it calculates the robot's current position in the large scene in real time and sends it to the real-time path planning module and the switching decision module. The real-time path planning module is used to plan the robot's movement path in real time based on the current obstacle information, the current robot position, and the robot target point position, and send it to the prediction module. The prediction module is used to predict the robot's possible movement path in the next period of time based on the current obstacle information and the real-time planned movement path, and send it to the switching decision module. The switching decision module is used to decide which primitives in the primitive database to switch to based on the current robot position and the predicted movement path, and to update the primitive map of the local large scene in real time.

8. A large-scale scene primitive map switching system according to claim 7, characterized in that, The mapping module includes an acquisition unit, a correction unit, and a calculation unit; wherein: The acquisition unit is used to use lidar to perceive the surrounding environment of a large scene and obtain radar point clouds frame by frame. The correction unit is used to remove motion distortion of the radar point cloud using an IMU; The computing unit is used to calculate the pose transformation between two radar point clouds according to the ICP algorithm to obtain the pose transformation matrix; the multi-frame radar point clouds are transformed into the coordinate system of the first frame radar point cloud through the pose transformation matrix to form a preliminary point cloud map of a complete large scene.

9. A large-scale scene primitive map switching device, characterized in that, include: A memory and at least one processor, wherein the memory stores instructions and the memory and the at least one processor are interconnected via a circuit; The at least one processor invokes the instructions in the memory to cause the mobile robot to perform the steps of the large scene primitive map switching method as described in any one of claims 1-6.

10. A computer-readable storage medium storing instructions thereon, characterized in that, When the instruction is executed by the processor, it implements the steps of the large scene primitive map switching method as described in any one of claims 1-6.

Images