Instant positioning and map construction method, device and system, and storage medium

Abstract

An embodiment of the present application provides an instant positioning and map construction method, device, system, and storage medium. The method includes: acquiring an infrared image and extracting image feature points of the infrared image; acquiring lidar data and calculating radar odometer information based on point cloud features in the lidar data; assigning depth values ​​to the image feature points to obtain infrared image feature information; acquiring inertial data from an inertial sensor and calculating pose estimation information between two adjacent infrared images based on the inertial data; and outputting fused pose information based on the infrared image feature information, the pose estimation information, and the radar odometer information.

Description

Technical Field

[0001] The present application relates to the field of computer vision technology, and in particular to a method, device and system for real-time positioning and map construction, and a computer-readable storage medium. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) is a fundamental capability required for mobile robot navigation. To improve the robustness of SLAM systems, the industry has begun to research SLAM methods based on multi-sensor fusion. For example, SLAM methods based on LiDAR can capture details of the environment over long distances; SLAM methods based on visible light image vision are suitable for location recognition and perform well in texture-rich environments. However, these methods each have their own flaws. LiDAR-based SLAM methods cannot operate in unstructured environments, such as long corridors or flat open spaces; and the performance of SLAM methods based on visible light image vision is very sensitive to lighting changes and rapid motion: when lighting conditions are poor, effective image features cannot be obtained, causing the image vision part to completely fail, greatly reducing the robustness and accuracy of the SLAM system. Summary of the Invention

[0003] To solve the existing technical problems, the present application provides a real-time positioning and mapping method, device and system, and computer-readable storage medium that are not affected by changes in illumination and can effectively improve the robustness and accuracy of the system.

[0004] To achieve the above objectives, the technical solution of the embodiment of the present application is implemented as follows:

[0005] In a first aspect, an embodiment of the present application provides a method for real-time positioning and map construction, comprising:

[0006] Acquire an infrared image, and extract image feature points of the infrared image;

[0007] Obtaining lidar data, and calculating radar odometry information based on point cloud features in the lidar data;

[0008] Assigning depth values ​​to the image feature points to obtain infrared image feature information;

[0009] Acquiring inertial data from an inertial sensor, and calculating pose estimation information between two adjacent infrared images based on the inertial data;

[0010] The fused pose information is output according to the infrared image feature information, the pose estimation information and the radar odometer information.

[0011] In a second aspect, an embodiment of the present application provides an image processing device, comprising:

[0012] An infrared data module is used to acquire infrared images and extract image feature points of the infrared images;

[0013] A radar data module, configured to acquire lidar data and calculate radar odometry information based on point cloud features in the lidar data;

[0014] A depth matching module is used to assign depth values ​​to the image feature points to obtain infrared image feature information;

[0015] An inertial data module is used to obtain inertial data from an inertial sensor and calculate pose estimation information between two adjacent infrared images based on the inertial data;

[0016] A fusion module is used to output fused pose information based on the infrared image feature information, the pose estimation information and the radar odometer information.

[0017] In a third aspect, an embodiment of the present application provides an instant positioning and mapping system, comprising a processor, a memory connected to the processor, an infrared camera, a laser radar, and an inertial sensor;

[0018] The infrared camera is used to collect infrared images and send them to the processor;

[0019] The inertial sensor is used to sense inertial data and send it to the processor;

[0020] The laser radar is used to scan and collect laser radar data and send it to the processor;

[0021] The memory stores a computer program that can be executed by the processor, and when the computer program is executed by the processor, the real-time positioning and map construction method described in any embodiment of the present application is implemented.

[0022] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by the processor, the instant positioning and map construction method as described in any embodiment of the present application is implemented.

[0023] In the above embodiment, infrared images are used to extract image feature points and assign depth values ​​to obtain infrared image feature information. The radar odometer information calculated from the lidar data and the pose estimation information between two adjacent infrared images calculated by processing the inertial data collected by the inertial sensor are combined to perform fused pose estimation and output the fused pose information. By taking advantage of the all-day and all-weather characteristics of infrared imaging, the problem of not being able to obtain image features under low illumination conditions can be avoided. In addition, pose calculation is performed by fusing the inertial data of the infrared image vision with the inertial data of the inertial sensor and the lidar data collected by the lidar scanning. Different sensors, such as image sensors, lidars, and inertial sensors, respectively collect different types of data for calculating positioning and map information, as well as different data collection frequencies. By simultaneously utilizing the characteristics of multiple sensors, the robustness and accuracy of the system can be improved.

[0024] In the above embodiments, the real-time positioning and map construction device, system and computer-readable storage medium belong to the same concept as the corresponding real-time positioning and map construction method embodiments, and thus have the same technical effects as the corresponding real-time positioning and map construction method embodiments, and will not be repeated here. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 Schematic diagram of an application scenario of the instant positioning and map building method in one embodiment;

[0026] Figure 2 is a flow chart of a method for real-time positioning and map construction in one embodiment;

[0027] Figure 3 A schematic diagram of a posture conversion between different coordinate systems in one embodiment;

[0028] Figure 4 A schematic diagram of assigning depth values ​​to image feature points in one embodiment;

[0029] Figure 5 is a schematic diagram of performing dedistortion processing on point cloud features in one embodiment;

[0030] Figure 6 This is a schematic diagram of the effect of image feature extraction in an example;

[0031] Figure 7 Schematic diagram of IMU pre-integration in one embodiment;

[0032] Figure 8 An architectural diagram of a real-time positioning and map building system in an optional specific example;

[0033] Figure 9 A flowchart of a method for real-time positioning and map construction in an optional specific example;

[0034] Figure 10 FIG. 1 is a schematic diagram of a real-time positioning and mapping device according to an embodiment. DETAILED DESCRIPTION

[0035] The technical solution of this application is further elaborated in detail below with reference to the accompanying drawings and specific embodiments.

[0036] In order to make the purpose, technical solutions and advantages of this application clearer, the application will be further described in detail below with reference to the accompanying drawings. The described embodiments should not be regarded as limiting this application. All other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0037] In the following description, the expression "some embodiments" is involved, which describes a subset of all possible embodiments. It should be noted that "some embodiments" may be the same subset or different subsets of all possible embodiments, and may be combined with each other without conflict.

[0038] In the following description, the terms "first, second, and third" are merely used to distinguish similar objects and do not represent a specific ordering of the objects. It is understandable that "first, second, and third" can be interchanged with a specific order or sequence where permitted, so that the embodiments of the present application described herein can be implemented in an order other than that illustrated or described herein.

[0039] See also Figure 1, which is a schematic diagram of an optional application scenario of the simultaneous localization and mapping (SLAM) method provided in an embodiment of the present application. Simultaneous localization and mapping refers to placing a mobile robot at an unknown position in an unknown environment and allowing the mobile robot to gradually draw a map of the environment while moving. The simultaneous localization and mapping system includes a processor 12, a memory 13 connected to the processor 12, an infrared camera 14, a lidar 15, and an inertial sensor 16. The infrared camera 14 collects infrared images in real time and sends them to the processor 12, the lidar 15 scans and collects lidar data in real time and sends it to the processor 12, and the inertial sensor 16 collects inertial data in real time and sends it to the processor 12. The memory 13 stores a computer program that implements the simultaneous localization and mapping method provided in an embodiment of the present application. By executing the computer program, the processor 12 uses the acquired infrared images, lidar data, and inertial data to perform pose estimation and output fused pose information. Among them, the real-time positioning and map construction system can be implemented by a mobile robot that integrates an infrared camera 14, a laser radar 15 and an inertial sensor 16 and has storage and processing functions. It can also be a navigation product or other autonomous mobile device, such as a sweeping robot, vehicle-mounted equipment, etc.

[0040] See also Figure 2 , which is a real-time positioning and map construction method provided by an embodiment of the present application, can be applied to Figure 1 The instant positioning and map building system in the application scenario shown in the figure includes the following steps:

[0041] S101: Acquire an infrared image and extract image feature points of the infrared image.

[0042] Infrared imaging refers to the use of infrared radiation from a target object to convert invisible radiation energy into a visible image through photoelectric conversion, where the changes in brightness of each pixel in the image correspond to changes in the intensity of the target's radiation energy. Infrared images can be obtained by continuously shooting with an infrared camera at a set shooting frequency, or by capturing infrared video with an infrared camera and extracting infrared picture frames from the infrared video. In an optional example, the infrared camera includes an infrared lens and an infrared detector, and the detector includes various cooled and uncooled short-wave, medium-wave, and long-wave detectors.

[0043] Image feature points refer to representative points selected from an image. These representative points remain unchanged even after a small change in the infrared camera's viewing angle. Feature extraction can be performed on multiple infrared images to find the same points as image feature points. Known image feature extraction algorithms can be used to extract image feature points from infrared images, such as those based on SIFT (Scale-invariant features transform), HOG (Histogram of Oriented Gradient), or LBP (Local Binary Pattern).

[0044] S103: Obtain lidar data, and calculate radar odometer information based on point cloud features in the lidar data.

[0045] LiDAR (Light Detection and Ranging) refers to a radar sensor system that uses laser beams to detect target characteristics such as position and velocity. It operates by transmitting a detection signal (a laser beam) toward the target and then comparing the received signal reflected from the target (the target echo) with the transmitted signal. After appropriate processing, it can obtain relevant target information, such as distance, direction, altitude, speed, attitude, and even shape, enabling it to detect, track, and identify various obstacles in the surrounding environment. LiDAR data refers to the point cloud data collected by the LiDAR during a single scanning cycle.

[0046] Odometry refers to a method of estimating the change in an object's position over time using data obtained from mobile sensors. It can be represented as a mobile robot (real-time positioning and mapping system) estimating the distance the mobile robot has moved relative to its initial position. Odometry can be categorized by sensor type, such as lidar odometry, ultrasonic radar odometry, inertial odometry, and visual odometry. In this embodiment, obtaining lidar data and calculating radar odometry information based on point cloud features in the lidar data refers to estimating the real-time movement distance relative to the initial position using lidar data collected by lidar scanning.

[0047] S105 , assigning depth values ​​to the image feature points to obtain infrared image feature information.

[0048] By assigning depth values ​​to image feature points, the depth values ​​can represent the corresponding distances of each image feature point in the captured scene. Infrared images with depth information can directly reflect the geometric shape of the visible surface of an object. Optionally, depth values ​​can be assigned to image feature points by leveraging the ranging information of targets in the scene contained in the lidar data to associate lidar points with image feature points in the infrared image to incorporate depth information. Alternatively, multiple consecutive infrared images can be used to align multiple infrared images from different viewpoints to calculate the depth values ​​of image feature points. Infrared image feature information includes information about image feature points with and without depth values.

[0049] S107 , acquiring inertial data from an inertial sensor, and calculating pose estimation information between two adjacent infrared images based on the inertial data.

[0050] Inertial sensors (IMU), including accelerometers and angular accelerometers, refer to sensors that can measure the angular velocity and acceleration of the sensor body. Posture refers to the Euclidean transformation relationship between any two spaces (coordinate points in different coordinate systems). In SLAM, posture can essentially be a transformation matrix, specifically referring to the change relationship between the world coordinate system and the camera coordinate system, including rotation and translation. The world coordinate system means that the coordinates of the points used in the composition are the coordinates in the world coordinate system. The camera coordinate system refers to the coordinate system with the optical center of the camera as the origin. Posture can be represented by Euclidean transformation in three-dimensional space, and the expression can be in the form of transformation matrix, rotation vector R and translation vector T. For example Figure 3 As shown in Figure 1, it is a schematic diagram showing the transformation between different coordinate systems achieved by the pose.

[0051] The inertial sensor's acquisition frequency is greater than the infrared camera's infrared image acquisition frequency. The inertial data collected by the inertial sensor can provide a better estimate of rapid motion within a short period of time. Because the inertial sensor and image frequencies are inconsistent between two adjacent infrared image frames, the inertial data between the two infrared images can be integrated to align with the visual information. Acquiring inertial data from the inertial sensor and calculating pose estimation information between two adjacent infrared images based on this inertial data can refer to acquiring inertial data from the inertial sensor after the arrival of the previous infrared image frame and before the arrival of the next infrared image frame, and calculating pose estimation information by integrating the inertial data over time, for use in outputting a high-frequency pose between the two infrared image frames.

[0052] S109 , outputting fused pose information according to the infrared image feature information, the pose estimation information, and the radar odometer information.

[0053] In the embodiment of the present application, the system that combines the collected data of the infrared camera, laser radar and inertial sensor to determine and output the fused pose information to achieve real-time positioning and map construction is called a SLAM system based on infrared, laser radar and inertia. Among them, the data collection frequency of the inertial sensor, infrared camera and laser radar decreases in sequence, and the output frequency of the fused pose information is usually based on the scanning frequency of the laser radar. For one scanning cycle of the laser radar, the inertial data collected by the inertial sensor, the laser radar odometer information, and the infrared image feature information are processed to complete the fused estimated pose of the SLAM system based on infrared, laser radar and inertia, and output the fused pose information.

[0054] Optionally, in some specific application scenarios, such as when LiDAR data is not available, the real-time positioning and map construction method can also be to use an infrared camera and an inertial sensor to form an infrared vision inertia system (IVIS), and use the infrared image and the inertial data collected by the inertial sensor to determine and output the fused pose information. Alternatively, when the infrared image feature information cannot be fused because the image feature point extraction or depth value assignment does not meet the requirements, the real-time positioning and map construction method can also be to use a LiDAR and an inertial sensor to form a LiDAR inertia system (LIS), and use the LiDAR data and the inertial data collected by the inertial sensor to determine and output the fused pose information.

[0055] In the above embodiment, infrared images are used to extract image feature points and assign depth values ​​to obtain infrared image feature information. The radar odometer information obtained by calculating the lidar data and the pose estimation information between two adjacent infrared images obtained by processing the inertial data collected by the inertial sensor are combined to perform fused pose estimation and output the fused pose information. By taking advantage of the all-day and all-weather characteristics of infrared imaging, the problem of not being able to obtain image features under low illumination conditions can be avoided. In addition, pose calculation is performed by fusing the inertial data of the infrared image vision with the inertial data of the inertial sensor and the lidar data collected by the lidar scanning. Different sensors, such as image sensors, lidars, and inertial sensors, respectively collect different types of data and data collection frequencies for calculating positioning and map information. By simultaneously utilizing the characteristics of multiple sensors, the robustness and accuracy of the system can be improved.

[0056] In some embodiments, before outputting the fused pose information according to the infrared image feature information, the pose estimation information, and the radar odometer information, the method includes:

[0057] When the initialization conditions are met, initialization is performed using the radar odometer information, and the point cloud features are associated with the depth values ​​of the image feature points through initialization to obtain the initialized infrared image feature information;

[0058] The initialization condition includes at least one of the following: when the system is initially started, when the system is restarted, when the data stream of the infrared image is unstable, and when the depth value assignment of the image feature point fails.

[0059] Optionally, fused pose information refers to using lidar data to assist in the calculation and generation of an infrared visual inertia odom (IVIO), and using the lidar odometry to assist in initializing IVIO, thereby determining and outputting fused pose information based on infrared image feature information, pose estimation information, and radar odometry information. Specifically, the initial startup of the system, the system restart, the instability of the data stream of the infrared image, and the failure to assign the depth value of the image feature point are used as initialization conditions. The point cloud features are associated with the depth values ​​of the image feature points through initialization. Through the association, the depth values ​​of the image feature points can be assigned, or the depth information of the image feature points with depth information can be verified, so that continuous accuracy can be maintained in the process of calculating the fused pose information by obtaining infrared visual features through infrared images.

[0060] In some embodiments, assigning depth values ​​to the image feature points to obtain infrared image feature information includes:

[0061] performing initialization using the radar odometer information, associating the point cloud features with the depth values ​​of the image feature points through initialization to assign depth values ​​to the image feature points, thereby obtaining infrared image feature information;

[0062] The infrared image feature information includes a set ratio of image feature points with depth values ​​and image feature points without depth values.

[0063] In this embodiment, the point cloud features are associated with the depth values ​​of the image feature points through initialization, and depth values ​​are assigned to the image feature points through this association. Infrared image feature information is obtained based on the depth value assignment results. The depth value assignment may fail for some image feature points. The infrared image feature information includes image feature points with successful depth values ​​and image feature points without depth values ​​that failed to be assigned. The ratio of image feature points with depth values ​​to image feature points without depth values ​​must meet a set ratio requirement to ensure that the final infrared image with depth information can reflect the geometric shape of the object's visible surface.

[0064] Optionally, performing initialization using the radar odometer information includes:

[0065] Determine depth points by obtaining the point cloud features in the radar odometry information, project the depth points onto a unit sphere centered on the infrared camera, evenly distribute the depth points on the unit sphere by downsampling, mark corresponding depth values ​​in a polar coordinate system, and construct a two-dimensional key value space binary tree;

[0066] Projecting the image feature points onto the unit sphere;

[0067] Find the three nearest depth points on the unit sphere to form a neighboring depth point group. If the ray connecting the image feature point and the center of the sphere intersects the plane formed by the neighboring depth point group in the Cartesian coordinate system, assign the depth value of the image feature point to the depth value of the corresponding intersection point.

[0068] Determine whether the distance difference between the depth points in the neighboring depth point group exceeds a threshold;

[0069] If the value exceeds the threshold, the depth value assignment result corresponding to the neighboring depth point group is discarded; if the value does not exceed the threshold, the depth value assignment result corresponding to the neighboring depth point group is retained;

[0070] According to the depth value assignment result, if the number of the image feature points for which the depth values ​​are successfully assigned meets the set requirements, the initialization is successfully executed.

[0071] The laser radar collects laser radar data, associates the laser radar points with depth information with the image feature points in the infrared image, and determines the depth value of the corresponding image feature point based on the association result. Figure 4 , which is a schematic diagram of assigning depth values ​​to image feature points by using the radar odometry information to perform initialization. The sphere represents a unit sphere with the infrared camera as the center Oc and a radius r=1. dot1 represents a depth point projected onto the unit sphere, dot2 represents a depth point in a Cartesian coordinate system, dot3 represents an image feature point projected onto the unit sphere, and dot4 represents an image feature point in a Cartesian coordinate system. The normalized coordinates of the depth points are stored in a k-dimensional tree (a two-dimensional key-valued space binary tree). By searching the k-dimensional tree, it is determined whether there are at least three depth points dot1 with a distance from the feature point less than or equal to a threshold in the normalized plane to form a neighboring depth point group. The ray formed by the sphere center Oc and the image feature point intersects with the plane formed by the neighboring depth point group in the Cartesian coordinate system, and the depth value of the image feature point is assigned to the depth value of the intersection point.

[0072] Among them, there may be overlap between LiDAR data at different times, that is, LiDAR data at different distances, resulting in depth ambiguity. That is, when point cloud features at different distances are projected into the same polar coordinate area, there may be overlap in the projection points, but the distances between the depth points may differ greatly. After the depth values ​​are associated with the corresponding image feature points based on the neighboring depth point group and the depth values ​​are assigned through the association, the association result is checked to determine whether the distance difference between the depth points in the neighboring depth point group exceeds a threshold to determine whether to retain the current depth value assignment result for the corresponding image feature point.

[0073] In this embodiment, by using lidar data to associate lidar points with image feature points in infrared images to bring in depth information, the dependence of three-dimensional mapping using lidar data on spatial geometric features can be reduced, and the measurement accuracy of the depth values ​​of image feature points can be improved, which is conducive to improving the image quality of three-dimensional scene mapping.

[0074] In some embodiments, before calculating radar odometry information based on point cloud features in the lidar data, the method includes:

[0075] During a formation period of a frame of point cloud data, dedistortion processing is performed on the point cloud features in the lidar data based on the pose estimation information and / or based on the infrared image features and the pose information output by the inertial data.

[0076] The laser radar forms a frame of point cloud image, which usually requires the laser to scan from one side to the other one or more times. The formation time of each frame of point cloud image is relatively long, and the posture changes in this process are usually relatively large, resulting in the point cloud features obtained in this process being significantly different from those in the real-world coordinate system, resulting in point cloud feature distortion. During the formation time period of each frame of point cloud image, the point cloud feature distortion is eliminated by using the posture estimation information obtained by collecting inertial data using inertial sensors, and / or the posture information obtained based on the infrared image feature information and the inertial data. Among them, the camera frequency and the inertial sensor frequency are much higher than the scanning frequency of the laser radar to form the point cloud image frame. The posture information obtained based on the infrared image feature information and the inertial data refers to the posture information output in real time by the infrared visual inertial subsystem using the infrared image and inertial data during the formation time period of each frame of point cloud image.

[0077] In this embodiment, the radar point cloud data is dedistorted to obtain more accurate point cloud features, thereby improving the accuracy of subsequent data calibration using the point cloud features.

[0078] Optionally, performing dedistortion processing on the point cloud features in the lidar data based on the pose estimation information and / or based on the infrared image feature information and the pose information output from the inertial data within a formation period of one frame of point cloud data includes:

[0079] Determining a starting time and a plurality of sampling times included in the starting time according to a formation period of a frame of point cloud data;

[0080] Calculating an initial pose of the laser radar at the starting moment according to the pose estimation information and / or the pose information output based on the infrared image feature information and the inertial data;

[0081] For each sampling moment, determine the pose estimation information of the two frames before and after the sampling moment and / or the pose information output based on the infrared image feature information and the inertial data, and fit the lidar pose at the sampling moment by linear interpolation;

[0082] According to the posture change of each sampling moment relative to the starting moment, the point cloud features of each sampling moment are projected onto the coordinate system of the initial posture of the lidar to complete the removal of point cloud distortion.

[0083] See also Figure 5 Arrow 1 and arrow 2 respectively represent the start and end times of a frame of point cloud data. For a sampling time contained in the starting time, such as arrow 3, the IMU pose and / or IVIO pose of the two most recent frames adjacent to arrow 3 can be determined according to time difference, where IMU is used to integrate angular velocity and IVIO is used to calculate linear displacement, thereby calculating the pose change between arrow 3 and the initial point. The calculated pose change is used to convert the pose at arrow 3 to the pose coordinate system corresponding to the starting time to complete the removal of point cloud distortion.

[0084] In this embodiment, the principle of dedistortion processing of the point cloud features in the laser radar data based on the posture estimation information and / or based on the infrared image feature information and the posture information output by the inertial data is based on the following: according to the laser radar posture at each sampling moment in a frame of point cloud data, all points in a frame of point cloud are projected to the initial posture of the laser radar, so that the positions of the point cloud features collected in a frame of point cloud are converted to the starting moment, and the posture of each frame of point cloud data is unified to the posture of the laser radar itself at the first sampling moment in the frame of point cloud data. The posture of the previous frame of point cloud data and the posture of the current frame of point cloud data are known, that is, the posture of the current frame of point cloud data at the first sampling moment and the posture at the last sampling moment are known, according to Figure 5The linear interpolation method shown is used to fit the lidar pose at each sampling moment, and the point cloud features are uniformly projected into the lidar coordinate system at the first sampling moment according to the pose change, thereby effectively eliminating the data distortion caused by motion distortion during the acquisition of each frame of point cloud data and obtaining more accurate point cloud features.

[0085] In some embodiments, the instant positioning and map building method further includes:

[0086] Constructing a bag-of-words model based on historical infrared images and / or historical lidar data;

[0087] Performing loop closure detection based on the bag-of-words model using the infrared image and / or the lidar data, and matching key frames with loop closure frames by feature descriptors to determine matching points;

[0088] If the number of matching points meets the minimum loop matching threshold, the key frame and the loop frame are spliced, and the matching points are connected to form a loop matching graph.

[0089] A bag-of-words model is a method that converts a word in a single bag into a bag-of-words vector by counting the number of times each word appears in a paragraph. The similarity between two paragraphs is then determined based on the similarity of the bag-of-words vectors. Constructing a bag-of-words model based on historical infrared images involves extracting features from the historical infrared images and clustering the features to generate "visual words." Each "visual word" is described using a corresponding feature descriptor, such as a contour, key feature point, or other visual descriptor. Similarly, constructing a bag-of-words model based on historical lidar data involves extracting features from the historical lidar data and clustering the features to generate "point cloud feature words." Each "visual word" is described using a corresponding feature descriptor, such as a spatial descriptor, such as a geometric surface shape. The bag-of-words model can be trained using an offline dataset. SLAM systems based on infrared, lidar, and inertial sensors can pre-load the bag-of-words model. During loop closure detection, the similarity between the keyframe and the looped frame is determined by matching feature descriptors, thereby determining whether the loop is successful. It should be noted that the key frames and loop frames may be image frames determined according to set time intervals.

[0090] In this embodiment, by using infrared images and / or lidar data to perform loop detection based on the bag-of-words model, the same scene can be relocated through loop closure, simplifying the calculation of the real-time positioning and map construction method and improving the accuracy.

[0091] Optionally, performing feature descriptor matching on the key frame and the loop frame to determine the matching point includes:

[0092] The infrared image is subjected to feature extraction based on the ORB algorithm to obtain a visual descriptor of the image feature points; Scan Context calculation is performed based on the lidar data to obtain a spatial descriptor of the point cloud feature; the key frame and the loop frame are matched with the visual descriptor and the spatial descriptor to determine the matching point based on the condition that the visual descriptor and the spatial descriptor match.

[0093] The ORB (Oriented Fast and Rotated Brief) algorithm is used to extract the ORB feature descriptor, and the ORB algorithm is used as the feature detection algorithm for building the bag-of-words model, so that loop detection can be based on the same image feature point extraction as the infrared image, and a unified feature descriptor is used to describe the image features, simplifying the loop detection implementation process. Figure 6 Figure 2 shows the effect of using the ORB algorithm to extract image features from infrared images. Using Scan Context to calculate the spatial descriptor of point cloud features makes the laser point cloud descriptor highly rotationally invariant, leveraging the distribution characteristics of scene point cloud features to compress scene information. Loop closure detection is performed simultaneously using infrared images and lidar data. The matching of visual and spatial descriptors is used as the condition for determining matching points. This reduces false matches in loop closure detection and improves its efficiency and accuracy.

[0094] In some embodiments, outputting fused pose information according to the infrared image feature information, the pose estimation information, and the radar odometer information includes:

[0095] Calculating infrared visual inertial odometry information based on the infrared image feature information and the pose estimation information;

[0096] Based on adjacent key frame point cloud data of the current frame point cloud data in the laser radar data, the adjacent key frame point cloud data is converted into a map coordinate system to construct a local point cloud map;

[0097] Calculate the pose change of the current frame point cloud data relative to the adjacent key frame point cloud data by using the pose estimation information and / or the infrared visual inertial odometry information between the current frame point cloud data and the adjacent key frame point cloud data;

[0098] Extract the point cloud features in the current frame point cloud data and match them with the local point cloud map, obtain the lidar pose of the current frame point cloud data in the map coordinate system according to the matching result, optimize the lidar pose according to the pose change, and output fused pose information or update the fused pose information according to the optimization result.

[0099] Among them, in order to improve real-time performance, the calculation of the front-end odometer of the laser radar may not consider all point clouds, but extract the features of the point cloud data to obtain point cloud features. In an optional specific example, the point cloud features mainly include corner points and plane points. Usually, the curvature is larger at the corner points and smaller at the plane points. The curvature can be calculated for multiple consecutive laser points before and after the current laser point to identify corner points and plane points and extract the point features and surface features of the point cloud data. The construction of the local point cloud map is to use the key point cloud data adjacent to the time and space dimensions under the corresponding posture of the current frame point cloud data. In the process of extracting the point cloud features in the current frame point cloud data and matching them with the local point cloud map, the posture estimation information of the IMU and / or the infrared visual inertial odometer information output by the infrared visual inertial subsystem are used to calculate the posture change between the two frames of key frame point cloud data, such as Figure 7 As shown in the figure, it is a schematic diagram of using IMU pre-integration. Due to the high frequency of IMU, the output of fused pose information is mainly based on the frequency of lidar. The common data between the point cloud data of the current frame (t_2) and the point cloud data of the adjacent key frame (t_1) of the previous frame are pre-integrated. The pose estimation information obtained by pre-integration calculation represents the pose change of IMU during this period. The calculated pose change is used to calibrate and optimize the lidar pose of the current frame point cloud data, and the fused pose information is output or updated according to the optimization result.

[0100] In this embodiment, the laser radar pose of the current frame point cloud data is obtained frame by frame and optimized, and the fused pose information is optimized and updated based on the optimization results, so that the drift that may occur during the movement of the system can be regularly eliminated through closure detection. The factor graph optimization method is adopted to combine the infrared visual odometry, laser radar odometry, inertial sensor calculation information and closure constraints to obtain the pose estimation information and closure constraints calculated by the front end, and the maximum a posteriori (MAP) is used for state estimation.

[0101] In order to have a more comprehensive understanding of the instant positioning and map construction method provided in the embodiment of the present application, a specific example is used below to illustrate.

[0102] like Figure 8As shown in the figure, the SLAM system based on infrared, lidar and inertial is an all-weather and all-day SLAM system, including an infrared visual-inertial subsystem and a lidar-inertial subsystem. The infrared visual-inertial subsystem uses infrared images for feature extraction and depth matching, combines radar laser data and inertial data to generate an infrared visual-inertial odometry, and assists in initializing the infrared visual-inertial odometry with lidar. The lidar-inertial subsystem uses lidar data to generate a radar odometry through feature extraction and feature matching, and dedistorts the extracted point cloud features based on the real-time output of the inertial sensor and the infrared visual-inertial odometry. The image feature points of the infrared image are assigned depth values ​​based on the dedistorted point cloud features. In this way, multiple sensor data are fused to output fused pose information. The factor graph optimization method is used to combine the infrared visual odometry, lidar odometry, IMU pre-integration, closed-loop and other constraints to optimize and update the fused pose information in real time. Please refer to Figure 9 , the instant positioning and map construction method includes:

[0103] S11, infrared camera collects infrared images;

[0104] S12, extracting image feature points from the infrared image;

[0105] S13, assigning depth values ​​to image feature points;

[0106] S14, obtaining IMU inertial data, calculating pose estimation information by integrating the IMU inertial data over time, and using this information to output a high-frequency pose before the next infrared image frame arrives, thereby generating an infrared visual inertial odometry.

[0107] S15, acquiring lidar data, extracting point cloud features and performing dedistortion processing;

[0108] S16, using the point cloud features of the current frame point cloud data after dedistortion processing, and performing feature matching to calculate radar odometry information through a local point cloud map established using the associated frame point cloud data;

[0109] S17, obtaining radar odometer information to assist initialization of infrared visual inertial odometer;

[0110] S18, acquiring feature information of the infrared image composed of feature points with depth information and feature points without depth information;

[0111] S19, if the current infrared camera data stream is unstable, restart the infrared visual inertial odometry; use the radar odometry information to assist in initializing the infrared visual inertial odometry;

[0112] S20 combines the processing of IMU inertial data, lidar odometer, and infrared image feature information to complete the fusion estimation of posture and output the fused posture information.

[0113] See also Figure 10 On the other hand, the present application provides a real-time positioning and map building device. In an exemplary embodiment, the real-time positioning and map building device can be implemented in a vehicle-mounted navigation. The real-time positioning and map building device includes: an infrared data module 131, which is used to acquire infrared images and extract image feature points of the infrared images; a radar data module 132, which is used to acquire lidar data and calculate radar odometer information based on point cloud features in the lidar data; a depth matching module 133, which is used to assign depth values ​​to the image feature points to obtain infrared image feature information; an inertial data module 134, which is used to acquire inertial data from an inertial sensor and calculate pose estimation information between two adjacent infrared images based on the inertial data; and a fusion module 135, which is used to output fused pose information based on the infrared image feature information, the pose estimation information, and the radar odometer information.

[0114] Optionally, an initialization module is further included, configured to perform initialization using the radar odometer information when an initialization condition is met, and to associate the point cloud features with the depth values ​​of the image feature points through initialization to obtain the initialized infrared image feature information;

[0115] The initialization condition includes at least one of the following: when the system is initially started, when the system is restarted, when the data stream of the infrared image is unstable, and when the depth value assignment of the image feature point fails.

[0116] Optionally, the initialization module is used to perform initialization using the radar odometer information, and through initialization, the point cloud features are associated with the depth values ​​of the image feature points to assign depth values ​​to the image feature points to obtain infrared image feature information; wherein, the infrared image feature information includes a set proportion of image feature points with depth values ​​and image feature points without depth values.

[0117] Optionally, the fusion module 135 is further configured to determine a depth point based on the point cloud features obtained in the radar odometer information, project the depth point onto a unit sphere centered on the infrared camera, uniformly distribute the depth points on the unit sphere by downsampling, and construct a two-dimensional key-value space binary tree using corresponding depth values ​​marked in a polar coordinate system; project the image feature point onto the unit sphere; search for the three nearest depth points on the unit sphere to form a neighboring depth point group, and if the ray connecting the image feature point and the center of the sphere intersects with the plane formed by the neighboring depth point group in the Cartesian coordinate system, assign the depth value of the image feature point to the depth value of the corresponding intersection point; determine whether the distance difference between the depth points in the neighboring depth point group exceeds a threshold; if the distance difference exceeds the threshold, discard the depth value assignment result corresponding to the neighboring depth point group; if the distance difference does not exceed the threshold, retain the depth value assignment result corresponding to the neighboring depth point group; based on the depth value assignment result, if the number of image feature points with successfully assigned depth values ​​meets the set requirements, the initialization is executed successfully.

[0118] Optionally, the radar data module 132 is also used to dedistort the point cloud features in the lidar data based on the pose estimation information and / or based on the infrared image feature information and the pose information output by the inertial data within a formation period of a frame of point cloud data.

[0119] Optionally, the radar data module 132 is further used to determine the starting moment and multiple sampling moments included in the starting moment based on the formation period of a frame of point cloud data; calculate the initial posture of the lidar at the starting moment based on the posture estimation information and / or the posture information output based on the infrared image feature information and the inertial data; for each sampling moment, determine the posture estimation information and / or the posture information output based on the infrared image feature information and the inertial data of the two frames before and after the sampling moment, and fit the lidar posture at the sampling moment by linear difference; according to the posture change of each sampling moment relative to the starting moment, project the point cloud features of each sampling moment into the coordinate system of the initial posture of the lidar to complete the removal of point cloud distortion.

[0120] Optionally, it also includes a loop detection module for constructing a word bag model based on historical infrared images and / or historical lidar data; performing loop detection based on the word bag model using the infrared images and / or the lidar data, matching key frames with loop frames by feature descriptors to determine matching points; if the number of matching points meets the minimum loop matching threshold, splicing the key frames with the loop frames, and connecting the matching points to form a loop matching graph.

[0121] Optionally, the loop detection module is also used to extract features from the infrared image based on the ORB algorithm to obtain visual descriptors of the image feature points; perform Scan Context calculation based on the lidar data to obtain spatial descriptors of the point cloud features; and match the visual descriptors and spatial descriptors of the key frame and the loop frame to determine the matching points based on the condition that the visual descriptor and the spatial descriptor match.

[0122] Optionally, the fusion module 135 is used to calculate infrared visual inertial odometry information based on the infrared image feature information and the pose estimation information; based on the adjacent key frame point cloud data of the current frame point cloud data in the lidar data, convert the adjacent key frame point cloud data to a map coordinate system to construct a local point cloud map; use the pose estimation information and / or the infrared visual inertial odometry information between the current frame point cloud data and the adjacent key frame point cloud data to calculate the pose change of the current frame point cloud data relative to the adjacent key frame point cloud data; extract the point cloud features in the current frame point cloud data and match them with the local point cloud map, obtain the lidar pose of the current frame point cloud data in the map coordinate system according to the matching results, optimize the lidar pose according to the pose change, and output the fused pose information or update the fused pose information according to the optimization result.

[0123] It should be noted that the above-described embodiments of the instant positioning and mapping device, in the process of determining and outputting fused pose information, are illustrated by the division of the aforementioned program modules. In actual applications, the aforementioned processing can be assigned to different program modules as needed, that is, the internal structure of the device can be divided into different program modules to complete all or part of the method steps described above. Furthermore, the instant positioning and mapping device and the instant positioning and mapping method embodiments provided in the above-described embodiments are based on the same concept. The specific implementation process is detailed in the method embodiments and will not be repeated here.

[0124] On the other hand, the present application provides an instant positioning and map construction system, including a processor, a memory connected to the processor, an infrared camera, a lidar and an inertial sensor; the infrared camera is used to collect infrared images and send them to the processor; the inertial sensor is used to sense inertial data and send it to the processor; the lidar is used to scan and collect lidar data and send it to the processor; the memory stores a computer program that can be executed by the processor, and when the computer program is executed by the processor, it implements the instant positioning and map construction method described in any embodiment of the present application and can achieve the same technical effect. To avoid repetition, it will not be repeated here.

[0125] The present application also provides a computer-readable storage medium having a computer program stored thereon. When executed by a processor, the computer program implements the various processes of the above-mentioned instant positioning and map construction method embodiment and achieves the same technical effects. To avoid repetition, the description is omitted here. The computer-readable storage medium may be, for example, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0126] It should be noted that, in this document, the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, article, or apparatus comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, method, article, or apparatus comprising the element.

[0127] Through the description of the above embodiments, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus the necessary general hardware platform, and of course can also be implemented by hardware, but in many cases the former is a better embodiment. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk), and includes a number of instructions for enabling a terminal (which can be an infrared imaging device, a mobile phone, a computer, a server or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0128] The above description is merely a specific embodiment of the present application, but the scope of protection of the present application is not limited thereto. Any modifications or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present application should be included in the scope of protection of the present application. Therefore, the scope of protection of the present application should be based on the scope of protection of the claims.

Claims

1. A method for real-time positioning and map construction, characterized in that: include: Acquire an infrared image by using a single infrared camera, and extract image feature points of the infrared image; Obtaining lidar data, and calculating radar odometry information based on point cloud features in the lidar data; Assigning depth values ​​to the image feature points to obtain infrared image feature information; Acquiring inertial data from an inertial sensor, and calculating pose estimation information between two adjacent infrared images based on the inertial data; Output fused pose information according to the infrared image feature information, the pose estimation information and the radar odometer information.

2. The instant positioning and map construction method according to claim 1, wherein: Before outputting the fused pose information according to the infrared image feature information, the pose estimation information and the radar odometer information, the method includes: When the initialization conditions are met, initialization is performed using the radar odometer information, and the point cloud features are associated with the depth values ​​of the image feature points through initialization to obtain the initialized infrared image feature information; The initialization condition includes at least one of the following: when the system is initially started, when the system is restarted, when the data stream of the infrared image is unstable, and when the depth value assignment of the image feature point fails.

3. The instant positioning and map construction method according to claim 2, wherein: Assigning depth values ​​to the image feature points to obtain infrared image feature information includes: performing initialization using the radar odometer information, associating the point cloud features with the depth values ​​of the image feature points through initialization to assign depth values ​​to the image feature points, thereby obtaining infrared image feature information; The infrared image feature information includes a set ratio of image feature points with depth values ​​and image feature points without depth values.

4. The instant positioning and map construction method according to claim 2 or 3, characterized in that: The initialization is performed using the radar odometer information, comprising: Determine depth points by obtaining the point cloud features in the radar odometry information, project the depth points onto a unit sphere centered on the infrared camera, evenly distribute the depth points on the unit sphere by downsampling, mark corresponding depth values ​​in a polar coordinate system, and construct a two-dimensional key value space binary tree; Projecting the image feature points onto the unit sphere; Find the three nearest depth points on the unit sphere to form a neighboring depth point group. If the ray connecting the image feature point and the center of the sphere intersects the plane formed by the neighboring depth point group in the Cartesian coordinate system, assign the depth value of the image feature point to the depth value of the corresponding intersection point. Determine whether the distance difference between the depth points in the neighboring depth point group exceeds a threshold; If the value exceeds the threshold, the depth value assignment result corresponding to the neighboring depth point group is discarded; if the value does not exceed the threshold, the depth value assignment result corresponding to the neighboring depth point group is retained; According to the depth value assignment result, if the number of the image feature points for which the depth values ​​are successfully assigned meets the set requirements, the initialization is successfully executed.

5. The instant positioning and map construction method according to claim 1, wherein: Before calculating the radar odometer information according to the point cloud features in the laser radar data, the method includes: During a formation period of a frame of point cloud data, dedistortion processing is performed on the point cloud features in the lidar data based on the pose estimation information and / or based on the infrared image feature information and the pose information output by the inertial data.

6. The instant positioning and map construction method according to claim 5, wherein: The step of performing dedistortion processing on the point cloud features in the lidar data based on the pose estimation information and / or the pose information output based on the infrared image feature information and the inertial data within a formation period of one frame of point cloud data includes: Determining a starting time and a plurality of sampling times included in the starting time according to a formation period of a frame of point cloud data; Calculating an initial pose of the laser radar at the starting moment according to the pose estimation information and / or the pose information output based on the infrared image feature information and the inertial data; For each sampling moment, determine the pose estimation information of the two frames before and after the sampling moment and / or the pose information output based on the infrared image feature information and the inertial data, and fit the lidar pose at the sampling moment by linear interpolation; According to the posture change of each sampling moment relative to the starting moment, the point cloud features of each sampling moment are projected onto the coordinate system of the initial posture of the lidar to complete the removal of point cloud distortion.

7. The instant positioning and map building method according to claim 1, wherein: Also includes: Constructing a bag-of-words model based on historical infrared images and / or historical lidar data; Performing loop closure detection based on the bag-of-words model using the infrared image and / or the lidar data, and performing feature descriptor matching between key frames and loop closure frames to determine matching points; If the number of matching points meets the minimum loop matching threshold, the key frame and the loop frame are spliced, and the matching points are connected to form a loop matching graph.

8. The instant positioning and map construction method according to claim 7, wherein: The step of performing feature descriptor matching on the key frame and the loop frame to determine a matching point includes: The infrared image is subjected to feature extraction based on the ORB algorithm to obtain a visual descriptor of the image feature points; Scan Context calculation is performed based on the lidar data to obtain a spatial descriptor of the point cloud feature; the key frame and the loop frame are matched with the visual descriptor and the spatial descriptor to determine the matching point based on the condition that the visual descriptor and the spatial descriptor match.

9. The instant positioning and map building method according to claim 1, wherein: The outputting fused pose information according to the infrared image feature information, the pose estimation information and the radar odometer information includes: Calculating infrared visual inertial odometry information based on the infrared image feature information and the pose estimation information; Based on adjacent key frame point cloud data of the current frame point cloud data in the laser radar data, the adjacent key frame point cloud data is converted into a map coordinate system to construct a local point cloud map; Calculate the pose change of the current frame point cloud data relative to the adjacent key frame point cloud data by using the pose estimation information and / or the infrared visual inertial odometry information between the current frame point cloud data and the adjacent key frame point cloud data; Extract the point cloud features in the current frame point cloud data and match them with the local point cloud map, obtain the lidar pose of the current frame point cloud data in the map coordinate system according to the matching result, optimize the lidar pose according to the pose change, and output fused pose information or update the fused pose information according to the optimization result.

10. A real-time positioning and map building device, characterized in that: include: An infrared data module, configured to acquire an infrared image through a single infrared camera and extract image feature points of the infrared image; A radar data module, configured to acquire lidar data and calculate radar odometry information based on point cloud features in the lidar data; A depth matching module is used to assign depth values ​​to the image feature points to obtain infrared image feature information; An inertial data module is used to obtain inertial data from an inertial sensor and calculate pose estimation information between two adjacent infrared images based on the inertial data; A fusion module is used to output fused pose information based on the infrared image feature information, the pose estimation information and the radar odometer information.

11. A real-time positioning and map building system, characterized in that: comprising a processor, a memory connected to the processor, a single infrared camera, a lidar, and an inertial sensor; The infrared camera is used to collect infrared images and send them to the processor; The inertial sensor is used to sense inertial data and send it to the processor; The laser radar is used to scan and collect laser radar data and send it to the processor; The memory stores a computer program that can be executed by the processor, and when the computer program is executed by the processor, the instant positioning and map construction method according to any one of claims 1 to 9 is implemented.

12. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the instant positioning and map construction method according to any one of claims 1 to 9 is implemented.

Images