Indoor scene laser vision fusion data acquisition method and system based on closed loop path

Abstract

The application discloses a kind of indoor scene radar vision fusion data acquisition method and system based on closed loop path, the method of the present application includes: S1, the data of current position is collected by the camera and radar of equipment;S2, according to the point cloud data of radar acquisition, the loop feature of current position is calculated;S3, according to loop feature, whether current position is the position of completed acquisition is judged, if current position is the position of completed acquisition, then end acquisition and exit;Otherwise, the device is moved to new current position, and jump to step S1.The present application aims to solve the problems of existing SLAM scheme, such as incomplete loop feature, large loop detection error, etc., the loop feature of current position is calculated according to the point cloud data collected by radar, which is more suitable for the data mode of real physical world, and has the advantages of high completeness of loop feature and small error.

Description

Technical Field

[0001] This invention relates to path planning technology in SLAM, specifically to a method and system for indoor scene radar-visual fusion data acquisition based on closed-loop paths. Background Technology

[0002] Path planning in SLAM (Simultaneous Localization and Mapping) technology refers to the process by which a robot starts from an unknown location in an unknown environment and builds a large-scale incremental map by repeatedly observing environmental features during its movement. In the process of map building, it can plan a collision-free path from the starting point to the target point in an environment with collisions, or plan an optimal path that meets certain conditions.

[0003] There are currently two main types of SLAM solutions: 1) Visual SLAM. Represented by RTAB-Map, visual SLAM solutions are functionally divided into five parts: image data acquisition, visual odometry, backend nonlinear optimization, loop closure detection, and mapping. The image data acquisition module acquires 2D visual data of the environment through a camera. Visual odometry predicts and calculates 3D stereo information from 2D images at different times and locations (image changes due to motion). Backend nonlinear optimization and loop closure detection are used to estimate the user's pose. The input is images or video sequences, and the output is camera motion trajectory and a local map. During mapping, the currently calculated camera motion trajectory and local map are matched and stitched into the existing map. Map fusion stitches the new data from the LiDAR into the original map, ultimately updating the map. 2) LiDAR SLAM. Represented by LIO-SAM, LiDAR SLAM differs from visual SLAM solutions in that it uses 3D point clouds as direct input data. Functionally, the SLAM solution is divided into five parts: point cloud data acquisition, laser odometry, backend nonlinear optimization, loop closure detection, and mapping. Point cloud data acquisition uses LiDAR or other sensors to obtain environmental information about the location. The raw LiDAR data is then optimized by removing problematic data or applying filters. Laser odometry no longer predicts 3D stereo information; instead, it directly matches the point cloud data of the current local environment to the existing map. The quality of this matching directly impacts the accuracy of the SLAM-built map. During SLAM, the point cloud data acquired by the LiDAR is stitched into the existing map. The backend nonlinear optimization, loop closure detection, and mapping modules are consistent with the visual SLAM solution. Because the visual SLAM solution directly acquires 2D images, the calculated 3D stereo information has lower accuracy, higher computational cost, and slower speed. The laser SLAM solution, lacking visual data as input, generates a map that lacks color information, significantly limiting its application in real-world outdoor environments. In addition, both technical solutions require cumbersome calibration to calibrate the external parameters between sensors in actual use. In loop closure detection, they only consider image data, which leads to problems such as incomplete loop closure features and large loop closure detection errors in existing SLAM solutions. Summary of the Invention

[0004] The technical problem to be solved by this invention is to provide an indoor scene radar-visual fusion data acquisition method and system based on closed-loop path, which addresses the above-mentioned problems in the existing SLAM schemes. This invention aims to solve the problems of incomplete loop closure features and large loop closure detection errors in the existing SLAM schemes. The loop closure features calculated based on the point cloud data collected by radar are more suitable for the data patterns of the real physical world and have the advantages of high loop closure feature integrity and small error.

[0005] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows:

[0006] A method for indoor scene radar-visual fusion data acquisition based on closed-loop path includes:

[0007] S1, collects current location data through the device's camera and radar;

[0008] S2, calculate the loop closure characteristics of the current position based on the point cloud data collected by the radar;

[0009] S3, determine whether the loop closure feature of the current position matches the saved loop closure feature. If they match, determine that the current position is a position where the acquisition has been completed, then end the acquisition and exit; otherwise, save the loop closure feature of the current position, move the device to the new current position, and jump to step S1.

[0010] Optionally, the functional expression for calculating the loop closure feature at the current position in step S2 is:

[0011]

[0012] In the above formula, c P This represents the loop closure feature of the current position P, where α and β are weighting coefficients, S is the set of neighborhood point clouds of the current position P acquired by radar, and p and q are point clouds in the neighborhood point cloud set S of the current position P, r p and r p C represents the average coordinates of the neighborhood point sets of point clouds p and q, respectively. p and C q Let r, g, and b be the average values ​​of the color values ​​r, g, and b of the neighboring points of point clouds p and q, respectively, and let |S| be the number of points in the neighboring point cloud set S. p || represents the 2-norm of point p.

[0013] Optionally, the functional expression for calculating the loop closure feature at the current position in step S2 is:

[0014] or

[0015] In the above formula, c P Let S represent the loop closure feature of the current position P, where S is the set of neighborhood point clouds of the current position P acquired by radar, and p and q are the point clouds in the neighborhood point cloud set S of the current position P. q and r q C represents the average coordinates of the neighborhood point sets of point clouds p and q, respectively. p and C q Let r, g, and b represent the average color values ​​r, g, and b of the neighboring points of point clouds p and q, respectively, and |S| be the number of points in the neighboring point cloud set S.

[0016] Optionally, in step S3, when determining whether the loop closure feature at the current position matches the saved loop closure feature, the function expression for the condition of a match is:

[0017]

[0018] In the above formula, c P This represents the loop closure feature at the current position P. For a given location P of a completed dataset i The cyclic feature is ∈, where ∈ is the preset threshold judgment coefficient.

[0019] Optionally, in step S1, data on the current location is acquired using the device's cameras and radar. This device includes multiple cameras C1,...,C n And a radar L, and the plurality of cameras C1,...,C n The positions of the radar L and the radar L are fixed to each other.

[0020] Optionally, the plurality of cameras C1,...,C n These are sub-lenses of the panoramic camera.

[0021] Optionally, after acquiring the current location data through the device's camera and radar in step S1, the method further includes using any k-th camera C. k Pixels in the acquired image Transformation to 3D point coordinates in radar coordinate system This allows for the fusion of data collected from the camera and radar at the current location.

[0022] Optionally, the step of setting any k-th camera C k Pixels in the acquired image Transformation to 3D point coordinates in radar coordinate system At that time, pixel and its three-dimensional point coordinates The functional relationship is as follows:

[0023]

[0024] In the above formula, ∧ represents logical AND, w represents image width, and h represents image height; based on pixel points and its three-dimensional point coordinates The function relationship is used to obtain the pixel points and its three-dimensional point coordinates The transformation matrix T between them, and has

[0025] Furthermore, the present invention also provides an indoor scene radar-visual fusion data acquisition system based on closed-loop path, including a microprocessor and a memory interconnected, wherein the microprocessor is programmed or configured to execute the steps of the indoor scene radar-visual fusion data acquisition method based on closed-loop path.

[0026] Furthermore, the present invention also provides a computer-readable storage medium storing a computer program, the computer program being programmed or configured by a microprocessor to perform the steps of the indoor scene radar-visual fusion data acquisition method based on closed-loop path.

[0027] Compared with existing technologies, the present invention has the following advantages: The method of the present invention includes: S1, acquiring data of the current position through the camera and radar of the device; S2, calculating the loop closure feature of the current position based on the point cloud data acquired by the radar; S3, determining whether the current position is a position that has been acquired completely based on the loop closure feature. If the current position is a position that has been acquired completely, the acquisition ends and exits; otherwise, the device is moved to a new current position, and the process jumps to step S1. The indoor scene radar-visual fusion data acquisition method based on closed-loop path of the present invention calculates the loop closure feature of the current position based on the point cloud data acquired by the radar, so that the loop closure feature contains the information of the point cloud acquired by the radar, thus being more suitable for data patterns of the real physical world. It can effectively solve the problems of incomplete loop closure features and large loop closure detection errors in existing SLAM schemes, and has the advantages of high loop closure feature completeness and small error, which can improve the efficiency and accuracy of indoor scene radar-visual fusion data acquisition based on closed-loop path. Attached Figure Description

[0028] Figure 1 This is a schematic diagram of the basic process of the method in Embodiment 1 of the present invention. Detailed Implementation

[0029] Example 1:

[0030] like Figure 1 As shown, the indoor scene radar-visual fusion data acquisition method based on closed-loop path in this embodiment includes:

[0031] S1, collects current location data through the device's camera and radar;

[0032] S2, calculate the loop closure characteristics of the current position based on the point cloud data collected by the radar;

[0033] S3, determine whether the loop closure feature of the current position matches the saved loop closure feature. If they match, determine that the current position is a position where the acquisition has been completed, then end the acquisition and exit; otherwise, save the loop closure feature of the current position, move the device to the new current position, and jump to step S1.

[0034] In this embodiment, the functional expression for calculating the loop closure feature at the current position in step S2 is:

[0035]

[0036] In the above formula, c P This represents the loop closure feature of the current position P, where α and β are weighting coefficients, S is the set of neighborhood point clouds of the current position P acquired by radar, and p and q are point clouds in the neighborhood point cloud set S of the current position P, r p and r p C represents the average coordinates of the neighborhood point sets of point clouds p and q, respectively. p and C q Let r, g, and b be the average values ​​of the color values ​​r, g, and b of the neighboring points of point clouds p and q, respectively, and let |S| be the number of points in the neighboring point cloud set S. p The symbol || represents the 2-norm of point p, i.e., the modulus of the coordinate vector of point p, and represents the distance from point p to the origin. The expression on the right-hand side of the above equation contains two factors weighted by coefficients α and β. The first factor represents the difference between the coordinates of the point at the current position P and its neighbors, which can be defined as the ratio of the average distance from each point in the neighborhood point cloud set S to the current position P to the distance to the origin. The second factor represents the difference between the color of the point at the current position P and its neighbors, which can be defined as the average of the absolute values ​​of the color differences between each point in the neighborhood point cloud set S and the current position P. The sum of the weights α and β is 1. The two factors are weighted and summed using coefficients α and β; in this embodiment, α = β = 0.5. Overall, these two factors together form the loop closure feature. Specifically, the two factors describe the non-smoothness of the point cloud path; the higher the non-smoothness, the higher the discontinuity, indicating a lower probability of forming a loop.

[0037] In this embodiment, when determining whether the loop closure feature at the current position matches the saved loop closure feature in step S3, the function expression for the condition of a match is:

[0038]

[0039] In the above formula, c P This represents the loop closure feature at the current position P. For a given location P of a completed dataset i The loop closure feature is defined by ∈, which is a preset threshold judgment coefficient. The preset threshold judgment coefficient ∈ can be selected as a constant value as needed. Generally speaking, the closer ∈ is to 0, the smaller the tolerance for loop closure feature error. When ∈ is equal to 0, the tolerance for loop closure feature error is zero. The closer ∈ is to 0, the greater the tolerance for loop closure feature error.

[0040] In this embodiment, step S1 involves acquiring current location data using the device's cameras and radar. This device includes multiple cameras C1,...,C n One radar L, multiple cameras C1,...,C n The positions of the radar and L are fixed. It should be noted that the "device" mentioned here refers to an indoor scene radar-visual fusion data acquisition device. This device is generally a walking device, for example, it may have multiple wheels at the bottom, which can be driven by a controller to move on the ground, and the differential speed control of the wheels on both sides can achieve turning; or it may have multiple tracks at the bottom, which can be driven by a controller to move on the ground, and the differential speed control of the tracks on both sides can achieve turning. This indoor scene radar-visual fusion data acquisition device can be primarily for data acquisition, or it may also have data processing capabilities. Its form can be machine-like, biomimetic, or partially biomimetic robot-like. Alternatively, this indoor scene radar-visual fusion data acquisition device can also be a non-walking device, fixed to a human or machine as a workload. In addition, multiple cameras C1,...,C n Further additional equipment can be added, including supplementary lighting sources, etc. In short, its additional functions and external form should not constitute a specific limitation on the "equipment" mentioned here.

[0041] As an optional implementation, in this embodiment, multiple cameras C1,...,C n These are sub-lenses of the panoramic camera.

[0042] In this embodiment, after collecting the current location data through the device's camera and radar in step S1, the method further includes using any k-th camera C. k Pixels in the acquired image Transformation to 3D point coordinates in radar coordinate system This allows for the fusion of data collected from the camera and radar at the current location. Alternatively, after collecting the current location data using the device's camera and radar in step S1, the data can be directly output for later processing.

[0043] For a single radar L and multiple cameras C1,...,C n The system is constructed such that the transformation matrix from the radar coordinate system to the coordinate system of a certain camera (let's assume it's camera C1) is denoted as R. L1 And record any shot C k The transformation matrix from one coordinate system to another reference coordinate system W is: Since our multiple lenses are sub-lenses on a prefabricated panoramic camera, W can be taken as a coordinate system with the center point inside the panoramic camera as the origin, and any lens C k Transformation matrix from one coordinate system to another reference coordinate system W This can be calculated from the technical parameters of the panoramic camera itself. In this case, the radar coordinate system to any lens C... k The transformation matrix R of the coordinate system Lk satisfy:

[0044]

[0045] In the above formula, R 1W The transformation matrix from the camera's coordinate system C1 to another reference coordinate system W. The inverse matrix, R Wk For any camera C k Transformation matrix from one coordinate system to another reference coordinate system W The inverse matrix.

[0046] If the coordinates of a point in space in the radar coordinate system are p L =(x L ,y L ,z L If the image of that point is on camera C, then the image of that point is captured by camera C. k The corresponding coordinates Satisfy the following formula:

[0047]

[0048] In the above formula, R kL From the radar coordinate system to any lens C k The transformation matrix R of the coordinate system Lk The inverse matrix, R Lk From the radar coordinate system to any lens C k The transformation matrix of the coordinate system. In this embodiment, the k-th camera C is transformed. k Pixels in the acquired image Transformation to 3D point coordinates in radar coordinate system At that time, pixel and its three-dimensional point coordinates The functional relationship is as follows:

[0049]

[0050] In the above formula, ∧ represents logical AND, w represents image width, and h represents image height; based on pixel points and its three-dimensional point coordinates The function relationship is used to obtain the pixel points and its three-dimensional point coordinates The transformation matrix T between them, and has By manually labeling the correspondence between pixels in several images and points in the point cloud, the pixel points in the image can be finally obtained using the method described above. and point cloud coordinates The correspondence between them. For each pixel in the image. Convert it to 3D point coordinates in point cloud coordinate system And perform 3D reconstruction. In this embodiment, the 3D point coordinates are... The transformation points in three-dimensional space are obtained by using a pre-calibrated transformation matrix T, which ultimately constitute the transformation of the image, that is, the transformation of the image to three-dimensional space. The function expression is as follows:

[0051] M i-1 =T*I i-1 ,

[0052] In the above formula, M i-1 I represents the set of points in three-dimensional space obtained at time i-1. i-1 This represents a certain image at time i-1.

[0053] In summary, the indoor scene radar-visual fusion data acquisition method based on closed-loop paths in this embodiment calculates the loop closure features of the current position based on the point cloud data acquired by radar. This ensures that the loop closure features include information from the point cloud acquired by radar, making it more suitable for data patterns in the real physical world. It can effectively solve the problems of incomplete loop closure features and large loop closure detection errors in existing SLAM schemes. It has the advantages of high loop closure feature completeness and small error, and can improve the efficiency and accuracy of indoor scene radar-visual fusion data acquisition based on closed-loop paths.

[0054] Furthermore, this embodiment also provides an indoor scene radar-visual fusion data acquisition system based on a closed-loop path, including a microprocessor and a memory interconnected. The microprocessor is programmed or configured to execute the steps of the aforementioned indoor scene radar-visual fusion data acquisition method based on a closed-loop path. Additionally, this embodiment also provides a computer-readable storage medium storing a computer program for being programmed or configured by the microprocessor to execute the steps of the aforementioned indoor scene radar-visual fusion data acquisition method based on a closed-loop path.

[0055] Example 2:

[0056] This embodiment is basically the same as Embodiment 1, with the main difference being: In this embodiment, the function expression for calculating the loop closure feature at the current position in step S2 is:

[0057]

[0058] In the above formula, cP Let S represent the loop closure feature of the current position P, where S is the set of neighborhood point clouds of the current position P acquired by radar, and p and q are the point clouds in the neighborhood point cloud set S of the current position P. q and r q Let be the average coordinates of the neighborhood point sets of point clouds p and q, respectively, and let |S| be the number of points in the neighborhood point cloud set S. The expression on the right side of the above equation only contains the first factor, which is more computationally efficient than Example 1, but has disadvantages in terms of loop closure feature completeness and error.

[0059] Furthermore, this embodiment also provides an indoor scene radar-visual fusion data acquisition system based on a closed-loop path, including a microprocessor and a memory interconnected. The microprocessor is programmed or configured to execute the steps of the aforementioned indoor scene radar-visual fusion data acquisition method based on a closed-loop path. Additionally, this embodiment also provides a computer-readable storage medium storing a computer program for being programmed or configured by the microprocessor to execute the steps of the aforementioned indoor scene radar-visual fusion data acquisition method based on a closed-loop path.

[0060] Example 3:

[0061] This embodiment is basically the same as Embodiment 1, with the main difference being: In this embodiment, the function expression for calculating the loop closure feature at the current position in step S2 is:

[0062]

[0063] In the above formula, c P Let S represent the loop closure feature of the current position P, where S is the set of neighborhood point clouds of the current position P acquired by radar, and p and q are the point clouds in the neighborhood point cloud set S of the current position P. q and r q C represents the average coordinates of the neighborhood point sets of point clouds p and q, respectively. p and C q Let r, g, and b represent the average color values ​​r, g, and b of the neighboring points of point clouds p and q, respectively, and |S| be the number of points in the neighboring point cloud set S. The expression on the right side of the above equation only contains the second factor, which is more computationally efficient than Example 1, but has disadvantages in terms of loop closure feature completeness and error.

[0064] Furthermore, this embodiment also provides an indoor scene radar-visual fusion data acquisition system based on a closed-loop path, including a microprocessor and a memory interconnected. The microprocessor is programmed or configured to execute the steps of the aforementioned indoor scene radar-visual fusion data acquisition method based on a closed-loop path. Additionally, this embodiment also provides a computer-readable storage medium storing a computer program for being programmed or configured by the microprocessor to execute the steps of the aforementioned indoor scene radar-visual fusion data acquisition method based on a closed-loop path.

[0065] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-readable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create a machine for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to operate in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The functions specified in one or more boxes. These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable apparatus for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0066] The above description is merely a preferred embodiment of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should also be considered within the scope of protection of the present invention.

Claims

1. A method for indoor scene radar-visual fusion data acquisition based on closed-loop path, characterized in that, include: S1, collects current location data through the device's camera and radar; S2, calculate the loop closure characteristics of the current position based on the point cloud data collected by the radar; S3, determine whether the loop closure feature of the current position matches the saved loop closure feature. If they match, determine that the current position is a position where the data acquisition has been completed, then end the data acquisition and exit; otherwise, save the loop closure feature of the current position, move the device to the new current position, and jump to step S1. The function expression for calculating the loop closure feature at the current position in step S2 is: ， In the above formula, This represents the loop closure feature at the current position P. and These are the weighting coefficients. The radar acquires a set of point clouds in the neighborhood of the current location P. and Let S be the point cloud in the neighborhood point cloud set of the current position P. and Point clouds And point cloud The average coordinates of the set of neighborhood points. and They represent point clouds respectively. and neighborhood point color value The average value, Set up neighborhood point clouds The number of midpoint clouds, express The norm of a point; Alternatively, the function expression for calculating the loop closure feature at the current position in step S2 is: ， Alternatively, the function expression for calculating the loop closure feature at the current position in step S2 is: ； In step S1, data on the current location is collected using the device's cameras and radar. The device includes multiple cameras. And a radar L, the multiple cameras The positions of the radar L and the radar L are fixed to each other; After acquiring current location data through the device's camera and radar in step S1, the process also includes using any k-th camera... Pixels in the acquired image Transformation to 3D point coordinates in radar coordinate system This allows for the fusion of data collected from the camera and radar at the current location; The kth camera Pixels in the acquired image Transformation to 3D point coordinates in radar coordinate system At that time, pixel and its three-dimensional point coordinates The functional relationship is as follows: , In the above formula, This represents logical AND. Indicates the image width. Represents image height; based on pixels. and its three-dimensional point coordinates The function relationship is used to obtain the pixel points and its three-dimensional point coordinates Transformation matrix between And there are .

2. The indoor scene radar-visual fusion data acquisition method based on closed-loop path according to claim 1, characterized in that, In step S3, when determining whether the loop closure feature at the current position matches the saved loop closure feature, the function expression for the condition of a match is: ， In the above formula, This represents the loop closure feature at the current position P. For a location where data collection has been completed The cyclical characteristics, This is the preset threshold judgment coefficient.

3. The indoor scene radar-visual fusion data acquisition method based on closed-loop path according to claim 1, characterized in that, The multiple cameras These are sub-lenses of the panoramic camera.

4. An indoor scene radar-visual fusion data acquisition system based on closed-loop path, comprising a microprocessor and a memory interconnected, characterized in that, The microprocessor is programmed or configured to perform the steps of the indoor scene radar-visual fusion data acquisition method based on closed-loop path as described in any one of claims 1 to 3.

5. A computer-readable storage medium storing a computer program, characterized in that, The computer program is used to be programmed or configured by a microprocessor to perform the steps of the indoor scene radar-visual fusion data acquisition method based on closed-loop path as described in any one of claims 1 to 3.

Images