Method for realizing real-time generation of map information based on fusion of UWB data and laser radar data

Abstract

The invention discloses a method for realizing map information real-time generation based on UWB data and laser radar data fusion, and belongs to the technical field of map information generation. The method comprises the following steps: determining relative pose transformation between a laser radar coordinate system and an underground space positioning coordinate system; providing an initial value for inter-frame matching of the laser speedometer by using ultra wide band (UWB) positioning data; adding constraints to a map constructed after data fusion by using UWB positioning data; and loopback detection based on curvature feature coding is carried out. According to the method, the illumination condition limitation of underground space is broken through, and accumulated errors generated after sensor data and the map are fused are eliminated, so that real-time generation of map information is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of map information generation, and in particular to a method for realizing real-time generation of map information based on the fusion of UWB (ultra-wideband) data and laser radar data. Background Art

[0002] Map construction and real-time positioning are mutually reinforcing. Only with accurate positioning can current sensor data be projected from the current sensor coordinate system to the map coordinate system, fused, and updated to generate map information. However, accurate sensor positioning information is required to ensure the accuracy of the fused map information. Underground spaces suffer from weak lighting conditions, severely limiting the use of visual cameras as sensors. Furthermore, due to occlusion, integrating GPS data cannot eliminate the accumulated errors in map generation. Summary of the Invention

[0003] In response to the shortcomings of the existing technology, the present invention provides a method for real-time generation of map information based on the fusion of UWB data and lidar data. The method deploys underground UWB devices in the underground space scene, and explores the underground space scene area through unmanned equipment carrying lidar sensors, and uses a fusion algorithm that fuses lidar data and UWB positioning data to generate a map of the explored area.

[0004] The technical solution adopted in the present invention is:

[0005] A method for generating map information in real time based on the fusion of UWB data and lidar data comprises the following steps:

[0006] Determine the relative pose transformation between the LiDAR coordinate system and the underground space positioning coordinate system;

[0007] Deploy underground UWB devices in underground space scenarios to collect UWB positioning data; use underground unmanned equipment carrying lidar sensors to explore the target underground space area to collect lidar data; and generate real-time map information of the target exploration area based on the fusion of lidar data and UWB positioning data;

[0008] Based on the determined relative pose transformation, the UWB positioning data is used to provide the initial value of the displacement component (displacement vector) for the inter-frame matching of the laser odometry of the lidar data;

[0009] Based on the rotation matrix corresponding to the pose transformation, indoor observation constraints are constructed using UWB positioning data for the map information generated after data fusion to filter the positioning data;

[0010] The generated map information is subjected to loop detection based on curvature feature encoding, and the curvature is filtered and rounded according to the threshold to form the final real-time map information of the target exploration area.

[0011] Furthermore, determining the relative position transformation between the LiDAR coordinate system and the underground space positioning coordinate system specifically includes:

[0012] (1) Place the underground space positioning tag node on the lidar sensor so that the two sensors (lidar sensor and UWB positioning sensor) are considered to overlap, and carry these two sensors to maneuver in the underground space environment;

[0013] (2) Deploy positioning base station nodes (i.e., deploy anchor nodes) at each node in the underground space. Based on the deployed positioning base station nodes, an envelope area is formed to obtain the three-dimensional motion trajectory coordinate point set P of the sensor in the underground space positioning coordinate system (U) and the lidar coordinate system (L). u and P l ;

[0014] (3) According to the same timestamp, from the three-dimensional motion trajectory coordinate point set P u and P l uniformly sample p points in the , and obtain the underground space positioning sampling point set X u and the lidar positioning sampling point set X l ; Among them, the point set X u and X l It is in matrix form, and each column represents a coordinate point of a three-dimensional motion trajectory;

[0015] (4) Based on the underground space positioning sampling point set X u and the lidar positioning sampling point set X l , according to the Umeyama theorem, the relative pose transformation between the underground space positioning coordinate system (U) and the lidar coordinate system (L) is obtained.

[0016] Furthermore, the initial value of the displacement component provided by the UWB positioning data for inter-frame matching of the lidar data is as follows:

[0017] The underground space positioning position X corresponding to the two adjacent laser point cloud frames given by underground space positioning T1 and X T2 , find the spatial distance between the two positions, and based on the spatial distance, obtain the initial value of the displacement component of the posture transformation between the two frames.

[0018] Furthermore, the indoor observation constraints are constructed by using UWB positioning data to generate map information after data fusion, specifically including:

[0019] (1) Based on the rotation matrix corresponding to the posture transformation and the obtained displacement vector, the underground UWB spatial positioning base station nodes located in the underground spatial positioning coordinate system are projected into the lidar coordinate system to obtain the coordinates of each underground UWB spatial positioning base station node in the lidar coordinate system;

[0020] (2) During the underground space area movement, the underground space positioning tag node will communicate with the underground UWB space positioning base station node in real time and detect whether the current sensor position is within the envelope formed by each underground UWB space positioning base station node. If so, execute step (3); otherwise, execute step (4);

[0021] (3) The positioning coordinates X calculated by the underground UWB device dx The position component X of the key frame pose at time t t Calculate the first indoor observation constraint e1 = || X dx -X t ||, if e1 exceeds the set threshold, the corresponding positioning data is filtered out;

[0022] (4) According to the measured distance d between the underground space positioning tag node and the positioning base station node mean , calculate the second indoor observation constraint Among them, the auxiliary spacing If e2 exceeds the set threshold, the corresponding measurement distance is filtered out; thereby obtaining the effective measurement distance between the underground space positioning tag node (mobile tag node) and the base station node at the current moment.

[0023] Furthermore, the loop detection based on curvature feature encoding is:

[0024] Calculating curvature Among them, K represents the laser radar positioning sampling point set X l The current laser point i The laser point sets on the left and right sides of the same row, d i Point i Distance to the lidar center O, d j Indicates that Point is removed from the laser point set K i The distance from other points to the center O of the laser radar, α is the set proportional coefficient, which is used to control the calculated curvature C value to be within a fixed range;

[0025] If C≤C threshold , then reset the curvature C to 0 and filter it out; if C>C threshold , then the calculated curvature C is rounded.

[0026] The technical solution provided by the present invention brings at least the following beneficial effects:

[0027] Due to the weak lighting conditions in underground spaces, the use of visual cameras as sensors is severely limited. In addition, due to occlusion in underground spaces, it is impossible to eliminate the cumulative errors generated during map generation by fusing GPS data. The present invention provides a method for real-time generation of map information based on the fusion of UWB data and lidar data. It breaks through the lighting condition limitations of underground spaces and eliminates the cumulative errors generated by the fusion of sensor data and maps, thereby realizing real-time generation of map information. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0029] Figure 1 This is a flow chart of a method for achieving real-time generation of map information based on the fusion of UWB data and lidar data, provided by an embodiment of the present invention;

[0030] Figure 2 The envelope area diagram formed by each underground UWB spatial positioning base station node. DETAILED DESCRIPTION

[0031] In order to enable those skilled in the art to better understand the technical solutions in this specification, the technical solutions of the embodiments of the present invention will be described in detail and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described with reference to the drawings are exemplary and are intended to be used to explain the present invention, and should not be understood as limiting the present invention.

[0032] Map construction and real-time positioning complement each other. Only by achieving accurate positioning can the sensor data at the current moment be projected from the current sensor coordinate system to the map coordinate system, and updated to generate new map information after fusion. However, accurate sensor positioning information is required to ensure that the fused map information data is correct. The lighting conditions in underground spaces are weak, and the use of visual cameras as sensors will be severely limited. On the other hand, due to occlusion, underground spaces cannot be eliminated by fusing GPS data to eliminate the cumulative errors generated during map generation. The purpose of the method proposed in the embodiment of the present invention is to overcome the shortcomings of the existing technology and provide a method for realizing real-time generation of map information based on the fusion of UWB data and lidar data, so as to break through the lighting condition limitations of underground spaces and eliminate the cumulative errors generated after the fusion of sensor data and maps, thereby realizing real-time generation of map information.

[0033] In one embodiment, the present invention provides a method for generating map information in real time based on the fusion of UWB data and LiDAR data, including: deploying underground UWB devices, i.e., UWB-based indoor positioning sensors, in an underground space scene, exploring the underground space scene area through unmanned equipment equipped with LiDAR sensors, and generating a map of the explored area using a fusion algorithm that fuses LiDAR data and UWB positioning data. Figure 1 The method proposed in the embodiment of the present invention includes: 1) determining the relative pose transformation between the laser radar coordinate system and the underground space positioning coordinate system; 2) using UWB positioning data to provide initial values for inter-frame matching of the laser odometry; 3) using UWB positioning data to add constraints to the map constructed after data fusion; 4) implementing loop detection based on curvature feature encoding; thereby generating map information in real time based on the detection results.

[0034] In one embodiment, the method proposed in the embodiment of the present invention is applied to underground space, and map construction is achieved based on the fusion of underground space positioning and lidar data. The specific implementation steps include:

[0035] Step S1, deploy underground UWB devices in the underground space scene, and explore the underground space area by carrying underground unmanned equipment with lidar sensors, so as to generate a map of the explored area by using a fusion algorithm that fuses lidar data and UWB positioning data.

[0036] Step S2, determining the relative posture transformation between the lidar coordinate system and the underground space positioning coordinate system, so as to be applicable to the scenario where the relative posture transformation between the underground space positioning coordinate system and the lidar coordinate system is not known in advance.

[0037] Step S3: Place the underground space positioning tag node on the lidar so that the two sensors are considered to overlap, and carry the two sensors to explore the underground space environment;

[0038] Step S4: before integrating the UWB positioning data into the fusion algorithm, within the envelope area formed by each underground UWB spatial positioning base station node (such as Figure 2 As shown in the figure, that is, in the dotted area surrounded by the underground space base station nodes represented by A0~A3), the three-dimensional motion trajectory coordinate point set P of the sensor in the underground space positioning coordinate system (U) and the lidar coordinate system (L) is obtained. U and P L :

[0039]

[0040] In formulas (1) and (2), and is the coordinate point of the corresponding three-dimensional motion trajectory. k is the trajectory point number, and m is the number of trajectory points.

[0041] Step S5: Based on the same timestamp, p points are uniformly sampled from the two 3D motion trajectory coordinate point sets obtained based on formulas (1) and (2) to form the following two point sets: UWB positioning sampling point set Xu and LiDAR positioning sampling point set X L .

[0042]

[0043]

[0044] In formula (3) and formula (4), each column in the matrix is the corresponding three-dimensional motion trajectory coordinate point.

[0045] Step S6: the two point sets Xu and X obtained by sampling L Perform sparse processing;

[0046] Step S7, using the two point sets Xu and X L ,According to the Umeyama theorem, the relative pose transformation between the underground space positioning coordinate system (U) and the lidar coordinate system (L) is obtained.

[0047] Step S8: Using the underground space positioning position X corresponding to the two adjacent laser point cloud frames given by underground space positioning T1 and X T2 , find the spatial distance between the two positions, and you can get the initial value of the displacement component of the pose transformation between the two frames: tr = X T2 -X T1 .

[0048] Step S9, using the obtained rotation matrix R and displacement vector t l , project the underground UWB space positioning base station nodes in the underground space positioning coordinate system into the laser radar coordinate system, and obtain the coordinates of each underground UWB space positioning base station node in the laser radar coordinate system;

[0049] Step S10: During the exploration of the underground space area, the underground space positioning tag node will communicate with the underground UWB space positioning base station node in real time;

[0050] Step S11: If the current sensor position is within the envelope formed by each underground UWB spatial positioning base station node, the positioning position coordinates Xuwb calculated by the underground UWB device are added as positioning observation constraints to the map construction after data fusion:

[0051] e1=||X dx -X t||(5)

[0052] In formula (5), X t is the key frame pose P at time t t The position component of , e1 is the first indoor observation constraint;

[0053] That is, if the calculated e1 exceeds the given threshold, the corresponding data is filtered out.

[0054] Step S12: If the current sensor position is outside the envelope area formed by the underground space base station nodes, the following method is used:

[0055] Step S13: obtaining the ranging information; then, filtering the original ranging information according to the underground space positioning signal strength threshold and the distance threshold between the mobile tag and the base station:

[0056] d t =(d1,d2,…,d j ) T (6)

[0057] In formula (6), d t represents the measured distance between the mobile tag node and j valid base station nodes at time t, d1, d2, ..., d j They represent the measured distances between the mobile tag node and each valid base station node at time t;

[0058] Step S14: The nearest base station coordinate x measured at time t mean It is added to the underground space positioning map as a landmark, and is measured according to the distance d mean is the key frame pose P at time t t Construct observation constraints for backend graph optimization:

[0059]

[0060] in,

[0061]

[0062] Among them, e2 is the second indoor observation constraint. That is, only when the e2 corresponding to the measured distance meets the specified condition (does not exceed the specified threshold), the corresponding measured distance is considered valid. t is the key frame pose P at time t t The position component of .

[0063] Step S15: perform loop detection based on curvature feature encoding:

[0064]

[0065] In formula (9), K represents the current laser point Point i The set of laser points on the left and right sides of the same row, d i Representative point i Distance to the lidar center O, d j Represents K except Point i The distances from other points to the center O of the laser radar, α represents the proportional coefficient, which is adjusted according to the size of the actual K set so that the curvature obtained is within a fixed range;

[0066] Filter and round the curvature based on the threshold:

[0067]

[0068] In formula (10), C thres h old is the set threshold; when C is less than the set threshold, that is, the Round(C) condition is taken, the key frame point clouds corresponding to the two encoding matrices are considered similar, that is, the position of the current key frame is close to the position of the candidate historical key frame, thereby realizing the position recognition function and meeting the conditions for adding loop constraints, otherwise the data is eliminated.

[0069] In the description of this specification, the reference terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" mean 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 can be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0070] Furthermore, the terms "first," "second," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the quantity of the technical features being referred to. Thus, a feature defined as "first," "second," etc., may explicitly or implicitly include at least one of the features.

[0071] Any process or method description in a flowchart or otherwise described in this specification may be understood to represent a module, segment or portion of code comprising one or more executable instructions for implementing the steps of a custom logical function or process, and the scope of the preferred embodiments of the present invention includes alternative implementations in which functions may be performed out of the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present invention pertain.

[0072] It should be understood that various parts of the present invention can be implemented using hardware, software, firmware, or a combination thereof. In the above-described embodiments, multiple steps or methods can be implemented using software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented using hardware, as in another embodiment, any one of the following technologies known in the art or a combination thereof can be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application-specific integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0073] Those skilled in the art will understand that all or part of the steps in the method of the above embodiment can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiment.

[0074] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the application, rather than to limit them. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A method for generating map information in real time based on the fusion of UWB data and lidar data, characterized in that: The following steps are involved: Determine the relative pose transformation between the LiDAR coordinate system and the underground space positioning coordinate system; Deploy underground UWB devices in underground space scenarios to collect UWB positioning data; use underground unmanned equipment carrying lidar sensors to explore the target underground space area to collect lidar data; and generate real-time map information of the target exploration area based on the fusion of lidar data and UWB positioning data; Based on the determined relative pose transformation, the UWB positioning data is used to provide the initial value of the displacement component for the inter-frame matching of the laser odometry of the lidar data; Based on the rotation matrix corresponding to the pose transformation, indoor observation constraints are constructed using UWB positioning data for the map information generated after data fusion to filter the positioning data; The generated map information is subjected to loop detection based on curvature feature encoding, and the curvature is filtered and rounded according to the threshold to form the final real-time map information of the target exploration area.

2. The method according to claim 1, wherein Determining the relative pose transformation between the LiDAR coordinate system and the underground space positioning coordinate system specifically includes: (1) Place the underground space positioning tag node on the lidar sensor, and then make the sensor maneuver in the underground space environment; (2) Deploy positioning base station nodes at each node in the underground space, form an envelope area based on the deployed positioning base station nodes, and obtain the three-dimensional motion trajectory coordinate point set P of the sensor in the underground space positioning coordinate system and the lidar coordinate system respectively. u and P l ; (3) According to the same timestamp, from the three-dimensional motion trajectory coordinate point set P u and P l uniformly sample p points in the , and obtain the underground space positioning sampling point set X u and the lidar positioning sampling point set X l ; Among them, the point set X u and X l It is in matrix form, and each column represents a coordinate point of a three-dimensional motion trajectory; (4) Based on the underground space positioning sampling point set X u and the lidar positioning sampling point set X l , according to the Umeyama theorem, the relative pose transformation between the underground space positioning coordinate system and the lidar coordinate system is obtained.

3. The method according to claim 2, wherein The initial value of the displacement component provided by UWB positioning data for inter-frame matching of lidar data is as follows: The underground space positioning position X corresponding to the two adjacent laser point cloud frames given by underground space positioning T1 and X T2 , find the spatial distance between the two positions, and based on the spatial distance, obtain the initial value of the displacement component of the posture transformation between the two frames.

4. The method according to claim 3, wherein Using UWB positioning data to generate map information after data fusion to construct indoor observation constraints specifically includes: (1) Based on the rotation matrix corresponding to the posture transformation and the obtained displacement vector, the underground UWB spatial positioning base station nodes located in the underground spatial positioning coordinate system are projected into the lidar coordinate system to obtain the coordinates of each underground UWB spatial positioning base station node in the lidar coordinate system; (2) During the underground space area movement, the underground space positioning tag node will communicate with the underground UWB space positioning base station node in real time and detect whether the current sensor position is within the envelope formed by each underground UWB space positioning base station node. If so, execute step (3); otherwise, execute step (4); (3) The positioning coordinates X calculated by the underground UWB device dx The position component X of the key frame pose at time t t Calculate the first indoor observation constraint e1 = || X dx -X t ||, if e1 exceeds the set threshold, the corresponding positioning data is filtered out; (4) According to the measured distance d between the underground space positioning tag node and the positioning base station node mean , calculate the second indoor observation constraint Among them, the auxiliary spacing If e2 exceeds the set threshold, the corresponding measurement distance is filtered out.

5. The method according to claim 4, wherein The loop detection based on curvature feature encoding is: Calculating curvature Among them, K represents the laser radar positioning sampling point set X l The current laser point i The laser point sets on the left and right sides of the same row, d i Point i Distance to the lidar center O, d j Indicates that Point is removed from the laser point set K i The distance from other points to the laser radar center O, α is the set proportional coefficient, which is used to control the calculated curvature C value to be within a fixed range; If C≤C threshold , then reset the curvature C to 0; if C>C threshold , then the calculated curvature C is rounded.

Images