A method for constructing an interactive map

Abstract

The present invention relates to an interactive map construction method, comprising: S1, acquiring map initial data based on a plurality of sensor data acquired in advance; S2, receiving a user's adjustment instruction for any pair of frame point cloud data that meets a preset condition in the map initial data, adjusting the map initial data, and acquiring the adjusted relative pose of the pair of frame point cloud data; S3, using the adjusted relative pose of the pair of frame point cloud data as the initial value of an iterative closest point algorithm, iterating until a preset first condition is met, and acquiring a new relative pose of the pair of frame point cloud data; S4, performing pose graph optimization processing based on the new relative pose of the pair of frame point cloud data, and acquiring a processed map.

Description

Technical Field

[0001] The present invention relates to the technical field of map construction of inspection robots, and in particular to an interactive map construction method. Background Art

[0002] Map building is an important part of the mobile inspection robot system. Its function is to generate environmental maps for inspection robot positioning and navigation. At present, due to the increasingly complex working environment and the limited robot trajectory during map building, loop matching will be missed during the map building process. When missed detection occurs, it will cause map ghosting, thus affecting the positioning effect and stability.

[0003] Although, in Chinese patent CN109710724B, the posture state corresponding to each frame of point cloud is obtained through the inertial measurement unit (IMU), and then the point cloud frames are superimposed through coordinate transformation to obtain the point cloud map. However, this method relies on the accuracy of the IMU, and the map cannot be adjusted further when there is a problem.

[0004] In Chinese patent CN113674411A, the fitting line segments of erroneous point cloud data in the point cloud map and the adjustment relationship between the target point cloud data line segments are constructed to re-optimize the map, which depends on the fitting effect, and the fitting process is not transparent. Summary of the invention

[0005] 1. Technical issues to be resolved

[0006] In view of the above-mentioned shortcomings and deficiencies of the prior art, the present invention provides an interactive map construction method, which solves the technical problem of ghosting phenomenon generated when the map construction algorithm of the inspection robot in the prior art automatically constructs the map.

[0007] (II) Technical solution

[0008] In order to achieve the above object, the main technical solutions adopted by the present invention include:

[0009] An embodiment of the present invention provides an interactive map construction method, comprising:

[0010] S1. Acquire initial map data based on multiple sensor data acquired in advance;

[0011] S2, receiving an adjustment instruction from a user for any pair of frame point cloud data in the map initial data, adjusting the map initial data, and obtaining the adjusted relative position of the pair of frame point cloud data;

[0012] S3, using the adjusted relative pose of the pair of frame point cloud data as the initial value of the iterative closest point algorithm, and iterating until a preset first condition is satisfied, thereby obtaining a new relative pose of the pair of frame point cloud data;

[0013] S4. Based on the new relative pose of the pair of frame point cloud data, perform pose graph optimization processing to obtain a processed map.

[0014] Preferably, the S1 comprises:

[0015] Perform outlier processing on multiple sensor data acquired in advance to acquire sensor data after the outlier processing;

[0016] A three-dimensional mapping algorithm is used to fuse the sensor data after the abnormality processing to obtain initial map data.

[0017] Preferably,

[0018] The map initial data satisfies formula (1); wherein the formula (1) is:

[0019] M{{map},{keyframe0,...,keyframe m}};

[0020] M represents the initial map data;

[0021] map represents the global point cloud map;

[0022] keyframe m Represents the mth frame point cloud data;

[0023] keyframe m ={id m , T m , P m};

[0024] id m The ID representing the point cloud data of the mth frame;

[0025] T m Represents the point cloud pose of the mth frame of point cloud data;

[0026] P m Represents the point cloud of the mth frame of point cloud data.

[0027] Preferably,

[0028] The user's adjustment instruction in S2 for any pair of frame point cloud data in the initial map data is specifically:

[0029] The user manually adjusts the relative posture between the point clouds in any pair of frame point cloud data in the initial map data so that the point clouds in the pair of frame point cloud data overlap.

[0030] Preferably,

[0031] The first preset condition is that the value of the error function of the iterative closest point algorithm is less than a preset threshold;

[0032] The error function of the iterative closest point algorithm is:

[0033]

[0034] Where n is the number of nearest point pairs in the pair of frame point data used as the iterative closest point algorithm;

[0035] p i is the point to be matched in the i-th pair of nearest neighboring points of the point cloud to be matched in the iterative closest point algorithm in the pair of frame point data;

[0036] q i is the template point in the i-th pair of nearest neighboring points of the point cloud to be matched in the iterative closest point algorithm in the pair of frame point data;

[0037] E(R, t) is the value of the error function of the iterative closest point algorithm.

[0038] Preferably,

[0039] The first preset condition is that the number of iterations of the iterative closest point algorithm reaches a preset value.

[0040] Preferably,

[0041] The multiple sensors include: an odometer, an inertial navigation device and a laser radar.

[0042] Preferably, S4 includes:

[0043] The adjusted relative pose of the pair of frame point cloud data is added as a new edge to the pose graph composed of the initial map data to obtain a new pose graph, and pose graph optimization processing is performed on the new pose graph to obtain a processed map.

[0044] Preferably,

[0045] The pose graph optimization process includes: using the Gauss-Newton method to optimize through a preset objective function to adjust the point cloud pose of all point cloud data until a preset accuracy requirement is met or a maximum number of iterations is reached to obtain a processed map;

[0046] The objective function is:

[0047]

[0048] Among them, C is the set of all edges in the new pose graph;

[0049] Ω ijRepresents the weight value of the edge between any frame point cloud data i and another frame point cloud data j in the new pose graph;

[0050] <i,j> It is the edge between any frame point cloud data i and another frame point cloud data j in the new pose graph;

[0051] e ij represents the error; where

[0052] z ij is the measurement value between any pair of frame point cloud data;

[0053] is the expected value between any pair of frame point cloud data.

[0054] Preferably, the method further comprises:

[0055] S5. Repeat steps S2-S4 until the relative position between any pair of frame point cloud data in the processed map meets the preset accuracy, and the final processed map is obtained.

[0056] (III) Beneficial effects

[0057] The beneficial effects of the present invention are as follows: an interactive map construction method of the present invention receives a user's adjustment instruction for any pair of frame point cloud data that meets preset conditions in the map initial data based on the map initial data, adjusts the map initial data, obtains the adjusted relative position of the pair of frame point cloud data, and provides the initial value of the iterative closest point algorithm, thereby saving matching time and greatly improving the matching accuracy. In addition, a post-processing function for map data is provided, which expands the map's processable space and can cope with map construction work in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 A flow chart of an interactive map construction method of the present invention;

[0059] Figure 2 This is a schematic diagram of initial map data in an embodiment of the present invention;

[0060] Figure 3 It is a schematic diagram of the difference comparison of any pair of frame point cloud data in the initial map data in the embodiment of the present invention;

[0061] Figure 4 The diagram is a final processed map diagram obtained by using an interactive map construction method of the present invention. DETAILED DESCRIPTION

[0062] In order to better explain the present invention and facilitate understanding, the present invention is described in detail below through specific implementation modes in conjunction with the accompanying drawings.

[0063] In order to better understand the above technical solution, exemplary embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present invention are shown in the accompanying drawings, it should be understood that the present invention can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided to enable a clearer and more thorough understanding of the present invention and to fully convey the scope of the present invention to those skilled in the art.

[0064] See also Figure 1 , this embodiment provides an interactive map construction method, including:

[0065] S1. Based on the pre-acquired multiple sensor data, obtain the initial map data, see Figure 2 .

[0066] S2, see Figure 3 , receiving a user's adjustment instruction for any pair of frame point cloud data that meets preset conditions in the initial map data, adjusting the initial map data, and obtaining the adjusted relative posture of the pair of frame point cloud data.

[0067] S3. The adjusted relative pose of the pair of frame point cloud data is used as the initial value of the iterative closest point algorithm, and it is iterated until a pre-set first condition is met, thereby obtaining a new relative pose of the pair of frame point cloud data.

[0068] S4. Based on the new relative pose of the pair of frame point cloud data, perform pose graph optimization processing to obtain a processed map.

[0069] An interactive map construction method in this embodiment is implemented based on a manual interaction interface. Specifically, the manual interaction interface receives a user's adjustment instruction for any pair of frame point cloud data in the initial map data that meets preset conditions, and adjusts the initial map data. This embodiment assists manual adjustment of the initial map data through visual interaction technology.

[0070] In the practical application of this embodiment, S1 includes:

[0071] For the plurality of sensor data acquired in advance, outlier processing is performed to acquire sensor data after the outlier processing.

[0072] A three-dimensional mapping algorithm is used to fuse the sensor data after the abnormality processing to obtain the initial map data.

[0073] In the practical application of this embodiment, the initial map data satisfies formula (1); wherein the formula (1) is:

[0074] M={{map},{keyframe0,...,keyframe m}}.

[0075] M represents the initial map data.

[0076] map represents the global point cloud map.

[0077] keyframe m Represents the mth frame of point cloud data.

[0078] keyframe m ={id m , T m , P m}.

[0079] id m The ID of the mth frame of point cloud data.

[0080] T m Represents the point cloud pose of the mth frame of point cloud data.

[0081] P m Represents the point cloud of the mth frame of point cloud data.

[0082] keyframe a and keyframe b The relative position T ab :T ab =T a -1 ×T b .

[0083] In the practical application of this embodiment, the adjustment instruction of the user in S2 for any pair of frame point cloud data that meets the preset conditions in the initial map data is specifically:

[0084] The user manually adjusts the relative posture between the point clouds in any pair of frame point cloud data that meets the preset conditions in the initial map data so that the point clouds in the pair of frame point cloud data overlap.

[0085] The preset condition is two frames of point cloud data that are close in position but have a certain interval in time sequence in the initial map data.

[0086] Specifically, suppose there are two keyframes with similar positions. a and keyframe b, the user sees through the interface that there is a deviation when these two frames of point cloud data describe the same scene, that is, T ab There is an error. Select keyframe a As a reference frame, keyframe b As the frame to be adjusted, artificially change T ab_manul =(x, y, z, roll, pitch, yaw), (where x, y, z are coordinate values ​​in the three-dimensional coordinate system, roll, pitch, and yaw represent: roll angle, the angle of rotation around the x-axis; pitch angle, the angle of rotation around the y-axis; yaw angle, the angle of rotation around the z-axis) to adjust the relative positions of the two frames of point cloud to make them overlap. The adjusted T ab_manul This is the new keyframe. a and keyframe b The relative position relationship of the adjusted T ab_manul =(R ab_manul , t ab_manul ).

[0087] In practical applications of this embodiment, the preset first condition is that the value of the error function of the iterative closest point algorithm is less than a preset threshold.

[0088] The error function of the iterative closest point algorithm is:

[0089]

[0090] Wherein, n is the number of the nearest point pairs in the pair of frame point data used as the iterative closest point algorithm.

[0091] p i is the point to be matched in the i-th pair of nearest neighboring points of the point cloud to be matched in the iterative closest point algorithm in the pair of frame point data.

[0092] q i It is the template point in the i-th pair of nearest neighboring points of the point cloud to be matched in the iterative closest point algorithm in the pair of frame point data.

[0093] E(R, t) is the value of the error function of the iterative closest point algorithm.

[0094] Specifically in S3, the matching process of the Iterative Closet Point algorithm is as follows:

[0095] 1. Calculate the nearest points of each point in the template point cloud in the point cloud to be matched and form point pairs.

[0096] 2. Calculate the rotation matrix R and translation matrix t in the iterative closest point algorithm to minimize the value of the error function.

[0097] 3. Transform the template point cloud by calculating the value of the error function. Repeat the above 1 and 2 processes for the transformed template point cloud until E(R, t) is less than the set threshold or the number of iterations reaches the set value.

[0098] The key parameter in the iterative closest point algorithm is the initial value. This method uses manual assisted matching to provide the initial value, thus saving matching time and greatly improving the matching accuracy.

[0099] Specifically, the adjusted relative pose T ab_manul =(R ab_manul , t ab_manul ) as the initial value of the iterative closest point algorithm, combined with the two frames of point cloud data P a and P b , iterative calculation is performed with the purpose of minimizing the error function, and finally a "more accurate relative pose" is obtained.

[0100] In practical applications of this embodiment, the preset first condition is that the number of iterations performed by the iterative closest point algorithm reaches a preset value.

[0101] In practical applications of this embodiment, the multiple sensors include: an odometer, an inertial navigation device and a laser radar.

[0102] In the practical application of this embodiment, S4 includes:

[0103] The new relative pose of the pair of frame point cloud data is added as a new edge to the pose graph composed of the initial map data to obtain a new pose graph, and pose graph optimization processing is performed on the new pose graph to obtain a processed map.

[0104] In the practical application of this embodiment, the pose graph optimization processing includes: using the Gauss-Newton method to optimize a preset objective function to adjust the poses of all nodes until a preset accuracy requirement is met or a maximum number of iterations is reached to obtain a processed map.

[0105] The objective function is:

[0106]

[0107] Among them, C is the set of all edges in the new pose graph.

[0108] Ω ij Represents the weight value of the edge between any frame point cloud data i and another frame point cloud data j in the new pose graph.

[0109] <i,j> It is the edge between any frame point cloud data i and another frame point cloud data j in the new pose graph.

[0110] e ij represents the error; where

[0111] z ij is the measurement value between any pair of frame point cloud data.

[0112] is the expected value between any pair of frame point cloud data.

[0113] Specifically, this "more accurate relative pose" is added as a new edge to the pose graph, the pre-set objective function is solved by the Gauss-Newton method, and the poses of all nodes are adjusted to obtain more accurate and consistent point cloud poses of each frame and the global map.

[0114] In the practical application of this embodiment, the method further includes:

[0115] S5. Repeat steps S2-S4 until the relative position between any pair of frame point cloud data in the processed map meets the preset accuracy, and the final processed map is obtained. Figure 4 .

[0116] In this embodiment, an interactive map construction method receives a user's adjustment instruction for any pair of frame point cloud data that meets preset conditions in the map initial data, adjusts the map initial data, obtains the adjusted relative position of the pair of frame point cloud data, and provides the initial value of the iterative closest point algorithm, thereby saving matching time and greatly improving the matching accuracy. In addition, a post-processing function for map data is provided, which expands the map's processable space and can cope with map construction work in complex environments.

[0117] Since the system described in the above embodiment of the present invention is a system used to implement the method of the above embodiment of the present invention, a person skilled in the art can understand the specific structure and deformation of the system based on the method described in the above embodiment of the present invention, and thus will not be described in detail here. All systems used in the method of the above embodiment of the present invention belong to the scope of protection of the present invention.

[0118] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0119] The present invention is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems) and computer program products according to the embodiments of the present invention. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions.

[0120] It should be noted that in the claims, any reference numerals placed between brackets shall not be construed as limiting the claims. The word "comprising" does not exclude the presence of components or steps not listed in the claims. The word "a" or "an" preceding a component does not exclude the presence of a plurality of such components. The invention may be implemented by means of hardware comprising several different components and by means of a suitably programmed computer. In the claims enumerating several means, several of these means may be embodied by the same hardware. The use of the words first, second, third, etc., is for convenience of expression only and does not indicate any order. These words may be understood as part of the component name.

[0121] In addition, it should be noted that, in the description of this specification, the description of the terms "one embodiment", "some embodiments", "embodiment", "example", "specific example" or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, unless they are contradictory.

[0122] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments after knowing the basic creative concept. Therefore, the claims should be interpreted as including the preferred embodiments and all changes and modifications falling within the scope of the present invention.

[0123] Obviously, those skilled in the art can make various modifications and variations to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention should also include these modifications and variations.

Claims

1. An interactive map construction method, characterized in that: include: S1. Acquire initial map data based on multiple sensor data acquired in advance; S2, receiving an adjustment instruction from a user for any pair of frame point cloud data in the map initial data, adjusting the map initial data, and obtaining the adjusted relative position of the pair of frame point cloud data; S3, using the adjusted relative pose of the pair of frame point cloud data as the initial value of the iterative closest point algorithm, and iterating until a preset first condition is satisfied, thereby obtaining a new relative pose of the pair of frame point cloud data; S4, performing pose graph optimization processing based on the new relative pose of the pair of frame point cloud data to obtain a processed map; The S1 includes: Perform outlier processing on multiple sensor data acquired in advance to acquire sensor data after the outlier processing; A three-dimensional mapping algorithm is used to fuse the sensor data after the abnormal processing to obtain initial map data; The map initial data satisfies formula (1); wherein the formula (1) is: M={{map},{keyframe0,...,keyframe m }}; M represents the initial map data; map represents the global point cloud map; keyframe m Represents the mth frame point cloud data; keyframe m ={id m ,T m ,P m ); id m The ID representing the point cloud data of the mth frame; T m Represents the point cloud pose of the mth frame of point cloud data; P m The point cloud representing the mth frame of point cloud data; The user's adjustment instruction in S2 for any pair of frame point cloud data in the initial map data is specifically: An adjustment instruction for a user to manually adjust the relative posture between the point clouds in any pair of frame point cloud data in the initial map data so that the point clouds in the pair of frame point cloud data overlap; The method further comprises: S5. Repeat steps S2-S4 until the relative position between any pair of frame point cloud data in the processed map meets the preset accuracy, and the final processed map is obtained.

2. The method according to claim 1, characterized in that The first preset condition is that the value of the error function of the iterative closest point algorithm is less than a preset threshold; The error function of the iterative closest point algorithm is: Where n is the number of nearest point pairs in the pair of frame point data used as the iterative closest point algorithm; p i is the point to be matched in the i-th pair of nearest neighboring points of the point cloud to be matched in the iterative closest point algorithm in the pair of frame point data; q i is the template point in the i-th pair of nearest neighboring points of the point cloud to be matched in the iterative closest point algorithm in the pair of frame point data; E(R, t) is the value of the error function of the iterative closest point algorithm.

3. The method according to claim 2, characterized in that The first preset condition is that the number of iterations of the iterative closest point algorithm reaches a preset value.

4. The method according to claim 2 or 3, characterized in that: The multiple sensors include: an odometer, an inertial navigation device and a laser radar.

5. The method according to claim 4, characterized in that The S4 includes: The adjusted relative pose of the pair of frame point cloud data is added as a new edge to the pose graph composed of the initial map data to obtain a new pose graph, and pose graph optimization processing is performed on the new pose graph to obtain a processed map.

6. The method according to claim 5, characterized in that The pose graph optimization process includes: using the Gauss-Newton method to optimize through a preset objective function to adjust the point cloud pose of all point cloud data until a preset accuracy requirement is met or a maximum number of iterations is reached to obtain a processed map; The objective function is: Among them, C is the set of all edges in the new pose graph; Ω ij Represents the weight value of the edge between any frame point cloud data i and another frame point cloud data j in the new pose graph; <i,j> It is the edge between any frame point cloud data i and another frame point cloud data j in the new pose graph; e ij represents the error; where z ij is the measurement value between any pair of frame point cloud data; is the expected value between any pair of frame point cloud data.

Images