Multi-sensor based tightly coupled odometry method, device, equipment and storage medium

Abstract

The disclosure provides a multi-sensor-based tightly coupled odometry method, device, equipment and storage medium. In one embodiment of the disclosure, the multi-sensor-based tightly coupled odometry method comprises: acquiring an image frame from a camera, a laser frame from a laser radar and an IMU frame from an IMU; updating a system state variable by using a pre-integration constraint relationship of a current image frame, an inter-image frame constraint relationship of the current image frame and a constraint relationship between the current image frame and the laser frame, updating an RGB value of a laser point in a local point cloud map and an image feature point of the current image frame based on the updated system state variable; updating the system state variable by using a pre-integration constraint relationship of a current laser frame, an inter-laser frame constraint relationship of the current laser frame and a constraint relationship between the current laser frame and the image frame, and merging laser point cloud data of the current laser frame into a global point cloud map according to the updated system state variable. The disclosure can effectively improve the coupling degree of odometry, and reduce the error existing when estimating the pose of the sensor and constructing the surrounding environment map.

Description

Technical Field

[0001] The present disclosure relates to the field of autonomous driving technology, and in particular to a multi-sensor based tightly coupled odometer method, apparatus, device, and storage medium. Background Art

[0002] Simultaneous Localization And Mapping (SLAM) technology has been widely used in many fields such as autonomous driving, drones, robots, virtual reality (VR) and augmented reality (AR).

[0003] SLAM can be broadly divided into three parts: front-end odometry, back-end (non-linear) optimization, and loop closure detection. The front-end odometry is primarily responsible for processing raw data from various sensors, enabling real-time pose estimation and construction of a map of the surrounding environment. Back-end (non-linear) optimization and loop closure detection aim to eliminate errors in pose estimation and map construction. Since back-end (non-linear) optimization and loop closure detection can only partially eliminate errors in the front-end odometry, a high-precision odometry is crucial to SLAM technology.

[0004] Traditional odometry is primarily categorized as visual odometry and laser odometry. Visual odometry processes image information to estimate the sensor's pose in real time and construct a map of the surrounding environment in real time, while laser odometry processes laser point cloud data to achieve real-time pose estimation and map construction. Because visual odometry processes image data, system performance is severely impacted when the visual sensor is in scenes with sparse textures and drastic changes in lighting. Laser odometry performance degrades significantly in relatively open scenes due to insufficient collected laser point cloud data. Therefore, the question remains how to implement a tightly coupled odometry system based on multiple sensors to overcome the shortcomings of these different types of odometry. Summary of the Invention

[0005] In order to solve at least one of the above technical problems, the present disclosure provides a multi-sensor based tightly coupled odometer method, apparatus, device and storage medium, which can effectively improve the system performance of the entire odometer.

[0006] A first aspect of the present disclosure provides a multi-sensor based tightly coupled odometer method, comprising:

[0007] Get image frames from the camera, laser frames from the lidar, and IMU frames from the IMU;

[0008] updating the system state variables using the pre-integration constraint relationship of the current image frame, the inter-image frame constraint relationship of the current image frame, and the constraint relationship between the current image frame and the laser frame; and updating the RGB values ​​of the laser points in the local point cloud map and the image feature points of the current image frame based on the updated system state variables, wherein the pre-integration constraint relationship of the current image frame is established by the IMU frame associated with the current image frame;

[0009] The system state variables are updated using the pre-integration constraint relationship of the current laser frame, the inter-laser frame constraint relationship of the current laser frame, and the constraint relationship between the current laser frame and the image frame. The laser point cloud data of the current laser frame is merged into the global point cloud map according to the updated system state variables. The pre-integration constraint relationship of the current laser frame is established by the IMU frame associated with the current laser frame, and the constraint relationship between the current laser frame and the image frame is determined by the laser point of the current laser frame and the image feature point of the nearest image frame.

[0010] Some possible implementations of the first aspect further include: associating the image frame and the laser frame with the IMU frame according to their respective timestamps.

[0011] In some possible implementations of the first aspect, associating the image frame and the laser frame with the IMU frame according to their respective timestamps includes one of the following:

[0012] When there is no retained observation frame with a timestamp less than the current observation frame but there is an IMU frame with a timestamp less than the current observation frame, all IMU frames with a timestamp less than the current observation frame and the first IMU frame with a timestamp greater than or equal to the current observation frame are taken as the IMU frames associated with the current observation frame;

[0013] When there is a reserved observation frame with a timestamp less than the current observation frame, find the nearest observation frame with a timestamp less than the current observation frame, and take all IMU frames with a timestamp greater than the nearest observation frame and a timestamp less than the current observation frame and the first IMU frame with a timestamp greater than or equal to the current observation frame as the IMU frames associated with the current observation frame;

[0014] When there is no IMU frame with a timestamp less than the current observation frame, the current observation frame is discarded;

[0015] The observation frame is the image frame or the laser frame.

[0016] In some possible implementations of the first aspect, when the current image frame is an initial image frame, the image feature points of the current image frame are acquired in the following manner:

[0017] Projecting the laser point of the laser frame closest to the initial image frame into the initial image frame according to the external parameters between the camera and the laser radar to determine the laser pixel point of each laser point;

[0018] Divide each laser pixel into different image blocks;

[0019] Laser pixels are selected as image feature points of the initial image frame according to the quality of the laser pixels in each image block and the spatial position of the laser points corresponding to the laser pixels.

[0020] In some possible implementations of the first aspect, when the current image frame is not the initial image frame, the image feature points of the current image frame are updated in the following manner:

[0021] Projecting the laser point of the laser frame closest to the current image frame into the current image frame according to the external parameters between the camera and the laser radar to determine the laser pixel point of each laser point;

[0022] Dividing each of the laser pixel points and the image feature points existing in the current image frame into different image blocks of the current image frame;

[0023] According to the quality of the laser pixel point in each image block, the spatial position of the laser point corresponding to the laser pixel point, and the number and quality of existing image feature points, one of the laser pixel points or the existing image feature point is selected as the current image feature point of the image block.

[0024] In some possible implementations of the first aspect, the quality of the laser pixel point is judged based on a pixel gradient or an image entropy value of an image block centered on the laser pixel point.

[0025] In some possible implementations of the first aspect, selecting a laser pixel point or an existing image feature point as the current image feature point of the image block based on the quality of the laser pixel point in each image block, the spatial position of the laser point corresponding to the laser pixel point, and the number and quality of existing image feature points includes one of the following:

[0026] When there is an existing image feature point in the image block, retain the existing image feature point and use the existing image feature point as the current image feature point of the image block;

[0027] When there are multiple existing image feature points in the image block, retain the image feature point with the best quality among the multiple existing image feature points, and use the image feature point with the best quality as the current image feature point of the image block;

[0028] When there is no existing image feature point in the image block, a laser pixel point is selected as the image feature point of the image block according to the quality of the laser pixel point in the image block and the spatial position of the laser point corresponding to the laser pixel point.

[0029] In some possible implementations of the first aspect, the RGB value of the laser point in the local point cloud map is updated in the following manner:

[0030] Projecting the laser points in the local point cloud map within the field of view of the current image frame onto the current image frame based on the external parameters between the camera and the laser radar to determine the pixel coordinates corresponding to the laser points in the local point cloud map within the field of view of the current image frame;

[0031] The RGB value of the pixel coordinate corresponding to the laser point in the current image frame is added to the laser point.

[0032] In some possible implementations of the first aspect, when the current image frame is not the initial image frame, the RGB value of the laser point in the local point cloud map is updated in the following manner:

[0033] Projecting the laser points in the local point cloud map within the field of view of the current image frame onto the current image frame based on the external parameters between the camera and the laser radar to determine the pixel coordinates corresponding to the laser points in the local point cloud map within the field of view of the current image frame;

[0034] The RGB value at the pixel coordinate corresponding to the laser point in the current image frame is fused with the RGB value already attached to the laser point, and the RGB value already attached to the laser point is updated to the fused RGB value.

[0035] In some possible implementations of the first aspect, updating the system state variables using the pre-integration constraint relationship of the current image frame, the inter-image frame constraint relationship of the current image frame, and the constraint relationship between the current image frame and the laser frame includes:

[0036] Establishing a pre-integration constraint relationship between the current image frame and a previous observation frame through the IMU frame associated with the current image frame, wherein the previous observation frame is the previous image frame or the previous laser frame whose timestamp is closest to the current image frame;

[0037] Obtain the image feature point matching pairs between the current image frame and the previous image frame;

[0038] constructing an inter-image frame constraint relationship between the current image frame and the previous image frame based on the image feature point matching pairs, and performing a first joint optimization using the pre-integration constraint relationship and the inter-image frame constraint relationship between the current image frame and the previous observation frame to update the system state variables;

[0039] A constraint relationship between the current image frame and the nearest laser frame is constructed based on the system state variables obtained from the first joint optimization update and the image feature point matching pairs. A second joint optimization is performed using the pre-integration constraint relationship between the current image frame and the previous observation frame and the constraint relationship between the current image frame and the nearest laser frame to update the system state variables again.

[0040] In some possible implementations of the first aspect, the inter-image frame constraint relationship of the current image frame includes: the reprojection residual of the image feature point matching pair between the current image frame and the previous image frame and the RGB residual between the laser point corresponding to the image feature point matching pair and the laser pixel point obtained by projecting it on the current image frame.

[0041] In some possible implementations of the first aspect, the constraint relationship between the current image frame and the laser frame includes: the three-dimensional coordinate residual of the 3D point matching pair between the current image frame and the nearest laser frame, the 3D point matching pair including the triangulated point of the image feature point matching pair between the current image frame and the previous image frame and the laser point corresponding to the image feature point matching pair.

[0042] In some possible implementations of the first aspect, updating the system state variables using the pre-integration constraint relationship of the current laser frame, the inter-laser frame constraint relationship of the current laser frame, and the constraint relationship between the current laser frame and the image frame includes:

[0043] Establishing a pre-integration constraint relationship between the current laser frame and a previous observation frame through the IMU frame associated with the current laser frame, wherein the previous observation frame is the previous image frame or the previous laser frame whose timestamp is closest to the current laser frame;

[0044] Get the feature point matching relationship of the current laser frame;

[0045] constructing an inter-laser frame constraint relationship of the current laser frame according to the feature point matching relationship of the current laser frame, and performing a third joint optimization using the pre-integration constraint relationship of the current laser frame and the inter-laser frame constraint relationship to update system state variables;

[0046] A constraint relationship between the current laser frame and the nearest image frame is constructed based on the system state variables obtained by the third joint optimization update and the feature point matching relationship of the current laser frame. A fourth joint optimization is performed using the pre-integration constraint relationship of the current laser frame and the constraint relationship between it and the nearest image frame to update the system state variables again.

[0047] In some possible implementations of the first aspect, the inter-laser frame constraint relationship of the current laser frame includes: the point-to-surface distance residual of the surface formed by the surface feature points of the current laser frame and the matching laser points, and the point-to-line distance residual of the straight line formed by the line feature points of the current laser frame and the matching laser points.

[0048] In some possible implementations of the first aspect, the constraint relationship between the current laser frame and the image frame includes: a three-dimensional coordinate residual of a laser point matching pair between the current laser frame and the nearest image frame; wherein the laser point matching pair includes a first laser point in the current laser frame and a laser point corresponding to a first image feature point in the nearest image frame of the current laser frame; wherein a laser pixel point obtained by projecting the first laser point onto the nearest image frame overlaps with the first image feature point of the nearest image frame.

[0049] A second aspect of the present disclosure provides a multi-sensor based tightly coupled odometer device, comprising:

[0050] An acquisition unit, configured to acquire image frames from a camera, laser frames from a lidar, and IMU frames from an IMU;

[0051] a first state updating unit, configured to update system state variables using a pre-integration constraint relationship of a current image frame, an inter-image frame constraint relationship of the current image frame, and a constraint relationship between the current image frame and a laser frame, wherein the pre-integration constraint relationship of the current image frame is established by the IMU frame associated with the current image frame;

[0052] a first map updating unit, configured to update the RGB value of the laser point in the local point cloud map based on the system state variable updated by the first state updating unit;

[0053] an image feature point updating unit, configured to update the image feature points of the current image frame based on the system state variables updated by the first state updating unit;

[0054] a second state updating unit, configured to update system state variables using a pre-integration constraint relationship of a current laser frame, an inter-laser frame constraint relationship of the current laser frame, and a constraint relationship between the current laser frame and an image frame, wherein the pre-integration constraint relationship of the current laser frame is established by the IMU frame associated with the current laser frame, and the constraint relationship between the current laser frame and the image frame is determined by a laser point of the current laser frame and an image feature point of a nearest image frame;

[0055] The second map updating unit is configured to merge the laser point cloud data of the current laser frame into the global point cloud map according to the updated system state variables.

[0056] Some possible implementations of the second aspect further include: an association unit, configured to associate the image frame and the laser frame with the IMU frame respectively according to their respective timestamps.

[0057] In some possible implementations of the second aspect, the image feature point updating unit is specifically used to obtain the image feature points of the current image frame when the current image frame is the initial image frame in the following manner: projecting the laser point of the laser frame closest to the initial image frame into the initial image frame according to the external parameters between the camera and the laser radar to determine the laser pixel point of each laser point; dividing each laser pixel point into different image blocks; and selecting the laser pixel point as the image feature point of the initial image frame according to the quality of the laser pixel point in each image block and the spatial position of the laser point corresponding to the laser pixel point.

[0058] In some possible implementations of the second aspect, the image feature point updating unit is specifically configured to update the image feature points of the current image frame in the following manner when the current image frame is not the initial image frame:

[0059] Projecting the laser point of the laser frame closest to the current image frame onto the initial image frame according to the external parameters between the camera and the laser radar to determine the laser pixel point of each laser point;

[0060] Dividing each of the laser pixel points and the image feature points existing in the current image frame into different image blocks of the current image frame;

[0061] According to the quality of the laser pixel point in each image block, the spatial position of the laser point corresponding to the laser pixel point, and the number and quality of existing image feature points, one of the laser pixel points or the existing image feature point is selected as the current image feature point of the image block.

[0062] In some possible implementations of the second aspect, the quality of the laser pixel point is judged based on a pixel gradient or an image entropy value of an image block centered on the laser pixel point.

[0063] In some possible implementations of the second aspect, the first map updating unit is specifically used to update the RGB value of the laser point in the local point cloud map in the following manner: based on the external parameters between the camera and the lidar, the laser point in the local point cloud map within the field of view of the current image frame is projected to the current image frame to determine the pixel coordinates corresponding to the laser point in the local point cloud map within the field of view of the current image frame; and the RGB value at the pixel coordinate corresponding to the laser point in the current image frame is appended to the laser point.

[0064] In some possible implementations of the second aspect, the first map updating unit is specifically configured to update the RGB value of the laser point in the local point cloud map in the following manner when the current image frame is not the initial image frame:

[0065] Projecting the laser points in the local point cloud map within the field of view of the current image frame onto the current image frame based on the external parameters between the camera and the laser radar to determine the pixel coordinates corresponding to the laser points in the local point cloud map within the field of view of the current image frame;

[0066] The RGB value at the pixel coordinate corresponding to the laser point in the current image frame is fused with the RGB value already attached to the laser point, and the RGB value already attached to the laser point is updated to the fused RGB value.

[0067] In some possible implementations of the second aspect, the first state updating unit is specifically configured to update the system state variable in the following manner:

[0068] Establishing a pre-integration constraint relationship between the current image frame and a previous observation frame through the IMU frame associated with the current image frame, wherein the previous observation frame is the previous image frame or the previous laser frame whose timestamp is closest to the current image frame;

[0069] Obtain the image feature point matching pairs between the current image frame and the previous image frame;

[0070] constructing an inter-image frame constraint relationship between the current image frame and the previous image frame based on the image feature point matching pairs, and performing a first joint optimization using the pre-integration constraint relationship and the inter-image frame constraint relationship between the current image frame and the previous observation frame to update the system state variables;

[0071] A constraint relationship between the current image frame and the nearest laser frame is constructed based on the system state variables obtained from the first joint optimization update and the image feature point matching pairs. A second joint optimization is performed using the pre-integration constraint relationship between the current image frame and the previous observation frame and the constraint relationship between the current image frame and the nearest laser frame to update the system state variables again.

[0072] In some possible implementations of the second aspect, the inter-image frame constraint relationship of the current image frame includes: the reprojection residual of the image feature point matching pair between the current image frame and the previous image frame and the RGB residual between the laser point corresponding to the image feature point matching pair and the laser pixel point projected on the current image frame.

[0073] In some possible implementations of the second aspect, the constraint relationship between the current image frame and the laser frame includes: the three-dimensional coordinate residual of the 3D point matching pair between the current image frame and the nearest laser frame, the 3D point matching pair includes the triangulated point of the image feature point matching pair between the current image frame and the previous image frame and the laser point corresponding to the image feature point matching pair.

[0074] In some possible implementations of the second aspect, the second state updating unit is specifically configured to update the system state variable in the following manner:

[0075] Establishing a pre-integration constraint relationship between the current laser frame and a previous observation frame through the IMU frame associated with the current laser frame, wherein the previous observation frame is the previous image frame or the previous laser frame whose timestamp is closest to the current laser frame;

[0076] Get the feature point matching relationship of the current laser frame;

[0077] constructing an inter-laser frame constraint relationship of the current laser frame according to the feature point matching relationship of the current laser frame, and performing a third joint optimization using the pre-integration constraint relationship of the current laser frame and the inter-laser frame constraint relationship to update system state variables;

[0078] A constraint relationship between the current laser frame and the nearest image frame is constructed based on the system state variables obtained by the third joint optimization update and the feature point matching relationship of the current laser frame. A fourth joint optimization is performed using the pre-integration constraint relationship of the current laser frame and the constraint relationship between it and the nearest image frame to update the system state variables again.

[0079] In some possible implementations of the second aspect, the inter-laser frame constraint relationship of the current laser frame includes: the point-to-surface distance residual of the surface formed by the surface feature points of the current laser frame and the matching laser points, and the point-to-line distance residual of the straight line formed by the line feature points of the current laser frame and the matching laser points.

[0080] In some possible implementations of the second aspect, the constraint relationship between the current laser frame and the image frame includes: a three-dimensional coordinate residual of a laser point matching pair between the current laser frame and the nearest image frame; wherein the laser point matching pair includes a first laser point in the current laser frame and a laser point corresponding to a first image feature point in the nearest image frame of the current laser frame; wherein a laser pixel point obtained by projecting the first laser point onto the nearest image frame overlaps with the first image feature point of the nearest image frame.

[0081] A third aspect of the present disclosure provides an electronic device, including:

[0082] a memory storing execution instructions; and

[0083] A processor executes the execution instructions stored in the memory, so that the processor executes the above-mentioned multi-sensor based tightly coupled odometer method.

[0084] A fourth aspect of the present disclosure provides a readable storage medium, wherein the readable storage medium stores execution instructions, and when the execution instructions are executed by a processor, they are used to implement the above-mentioned multi-sensor based tightly coupled odometer method.

[0085] The present disclosure jointly updates and maintains the same system state variables by establishing different constraint relationships between laser frames, between image frames, and between laser and image frames. It not only constructs optimization problems based on the constraint relationships between image frames, but also can find constraint relationships between image and laser frames to construct optimization problems. It not only can construct optimization problems based on the constraint relationships between laser frames, but also can find constraint relationships between laser and image frames to construct optimization problems. It can effectively improve the coupling degree of each sensor, thereby effectively reducing the errors in sensor pose estimation and surrounding environment map construction, and improving the system performance of the entire odometer. BRIEF DESCRIPTION OF THE DRAWINGS

[0086] The accompanying drawings illustrate exemplary embodiments of the present disclosure and together with the description serve to explain the principles of the present disclosure. These drawings are included to provide a further understanding of the present disclosure and are incorporated in and constitute a part of this specification.

[0087] Figure 1 is a flow chart of a tightly coupled odometer method based on multiple sensors according to an embodiment of the present disclosure;

[0088] Figure 2 Schematic diagram of the association principle between observation frames and IMU frames in one embodiment of the present disclosure;

[0089] Figure 3 This is a schematic diagram of the principle of obtaining image feature points of an initial image frame in one embodiment of the present disclosure;

[0090] Figure 4 1 is a schematic diagram showing the principle of a method for selecting image feature points to uniformly distribute the laser points in space in one embodiment of the present disclosure;

[0091] Figure 5 This is a flow chart of updating system state variables using the pre-integration constraint relationship of the current image frame, the inter-image frame constraint relationship of the current image frame, and the constraint relationship between the current image frame and the laser frame according to an embodiment of the present disclosure;

[0092] Figure 6 This is a schematic diagram of the principle of finding a constraint relationship between an image frame and a laser frame to construct an optimization problem according to an embodiment of the present disclosure;

[0093] Figure 7 This is a flow chart of updating system state variables using a pre-integration constraint relationship of a current laser frame, an inter-laser frame constraint relationship of the current laser frame, and a constraint relationship between the current laser frame and an image frame according to an embodiment of the present disclosure;

[0094] Figure 8 This is a schematic diagram of the principle of finding a constraint relationship between the current laser frame and the image frame to construct an optimization problem according to an embodiment of the present disclosure;

[0095] Figure 9 The figure is a schematic block diagram of a structure of a tightly coupled odometer device based on multiple sensors using a hardware implementation of a processing system according to an embodiment of the present disclosure. DETAILED DESCRIPTION

[0096] The present disclosure will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only intended to explain the relevant content and are not intended to limit the present disclosure. It should also be noted that, for ease of description, only the portions relevant to the present disclosure are shown in the accompanying drawings.

[0097] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in the present disclosure can be combined with each other. The technical solution of the present disclosure will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0098] Unless otherwise stated, the exemplary embodiments / examples shown are to be understood as providing exemplary features of various details of some ways in which the technical concepts of the present disclosure can be implemented in practice. Therefore, unless otherwise stated, the features of the various embodiments / examples may be further combined, separated, interchanged, and / or rearranged without departing from the technical concepts of the present disclosure.

[0099] The use of cross hatching and / or shading in the accompanying drawings is generally used to make the boundaries between adjacent components clear. As such, unless otherwise indicated, the presence or absence of cross hatching or shading does not convey or indicate any preference or requirement for the specific materials, material properties, dimensions, proportions, commonalities between the components shown, and / or any other characteristics, attributes, properties, etc. of the components. In addition, in the accompanying drawings, the sizes and relative sizes of the components may be exaggerated for clarity and / or descriptive purposes. When the exemplary embodiments can be implemented differently, the specific process sequence can be performed in a different order than described. For example, two successively described processes can be performed substantially simultaneously or in an order opposite to the order described. In addition, the same figure numbers represent the same components.

[0100] When a component is referred to as being “on,” “over,” “connected to,” or “coupled to” another component, the component may be directly on, directly connected to, or directly coupled to the other component, or intervening components may be present. However, when a component is referred to as being “directly on,” “directly connected to,” or “directly coupled to” another component, there are no intervening components present. For this purpose, the term “connected” may refer to a physical connection, an electrical connection, etc., with or without intervening components.

[0101] The terms used herein are for the purpose of describing specific embodiments and are not intended to be restrictive. As used herein, unless the context clearly indicates otherwise, the singular forms "one (kind, person)" and "said (the)" are also intended to include plural forms. In addition, when the terms "comprise" and / or "include" and their variations are used in this specification, the features, integral bodies, steps, operations, parts, assemblies and / or their groups stated are indicated, but the presence or addition of one or more other features, integral bodies, steps, operations, parts, assemblies and / or their groups is not excluded. It should also be noted that, as used herein, the terms "substantially", "approximately" and other similar terms are used as approximate terms and not as degree terms, and as such, they are used to explain the inherent deviations of the measured values, calculated values ​​and / or values ​​provided that will be recognized by those of ordinary skill in the art.

[0102] Brief description of related technologies:

[0103] Related Technology 1: "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual Tightly-Coupled State Estimation and Mapping Package" proposes a tightly coupled positioning and mapping method based on vision, laser, and IMU. The method includes two odometry units: visual-inertial odometry (VIO) and radar-inertial odometry (LIO). VIO obtains matching pairs of image feature points between the current image frame and the previous image frame through inter-frame optical flow tracking, and establishes residual equations (reprojection residual and RGB residual) based on the matching points. Finally, it uses the state error iterative Kalman filter (ESIKF) for iterative optimization to update the system state variables (for example, rotation, displacement, velocity, etc.). LIO uses an incremental k-dimensional tree (ikd-tree) search to obtain the matching relationship between the current laser frame point cloud and the local point cloud map, and establishes a residual equation (point-surface residual) based on the matching relationship. It then uses ESIKF to iteratively optimize and update the system state variables. Finally, it uses the optimized system state variables to publish the current frame laser frame point cloud to the global point cloud map. In this technology, the image feature points of VIO are directly obtained by projecting the laser points into the image frame according to the system state variables. The obtained image feature points and the laser points will form a one-to-one correspondence. When projecting the image feature points, the quality of the feature points is not considered, nor are the feature points homogenized. These will affect the accuracy of the optical flow tracking between VIO frames. In addition, in this technology, the constraint relationship is searched for and optimized between the image frames of VIO, and then the constraint relationship is searched for and optimized between the laser frames in LIO. There is no constraint relationship searched for and optimized between the laser frames and the image frames, and the degree of coupling is not high.

[0104] Related Technology 2: "FAST-LIVO: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry" discloses a tightly coupled odometry technology based on vision, laser, and IMU. Similar to Related Technology 1, this technology consists of two odometry units: the Vision Indicator (VIO) and the Light Indicator (LIO). The implementation of the Light Indicator (LIO) is similar to that of Technology 1, and the VIO is an odometry based on the direct method. In Related Technology 2, the laser point cloud is projected onto the image frame based on the system state variables to find and record the image blocks corresponding to each laser point. In the VIO, laser points within the field of view of the current image frame are located based on the prior pose provided by the IMU. A direct method is used to establish a residual equation for the photometric error between the laser points within the field of view of the current frame and the image blocks in the current image frame. Finally, an ESIKF is used to iteratively optimize and update the system state variables. Because the VIO residual equation is established based solely on the photometric error between image blocks to optimize the global state variables, system performance degrades in scenes with drastic changes in illumination. Furthermore, this related technology does not seek a constraint relationship between the laser frame and the image frame to construct an optimization problem, and still has the defect of low coupling degree.

[0105] Related technology 3: Chinese patent CN111561923 discloses a SLAM mapping method and system based on multi-sensor fusion. This technology uses lidar as a reference to calibrate the positional relationship between the camera, IMU, global navigation satellite system (GNSS) and lidar to obtain calibration information. Then, using GNSS as a reference, the time of the lidar, camera, and IMU is synchronized to the current GNSS time system. Then, data from the lidar, camera, IMU, and GNSS are collected, and real-time pose solution and construction of the surrounding environment map are achieved based on these data. Compared with technologies 1 and 2, this technology adds GNSS, but the GNSS satellite signals are transmitted from the sky tens of thousands of kilometers away. These signals are very weak when they reach the ground and cannot penetrate buildings. Therefore, this technology is only applicable to outdoor environments. In addition, this technology does not consider the problem of finding constraint relationships between laser frames and image frames to construct optimization problems, and also has the defect of low coupling degree.

[0106] In view of this, the present disclosure provides the following tightly coupled odometer method, device, equipment and storage medium based on multiple sensors to overcome the defect of low coupling degree between sensors in related technologies and reduce the error between the real-time pose estimation of the SLAM front-end odometer and the real-time construction of the surrounding environment map.

[0107] Before describing the specific embodiments of the present disclosure in detail, some relevant details of the present disclosure are explained as follows:

[0108] Time Base: Since cameras, lidars, and inertial measurement units (IMUs) are three different types of sensors, their output data may have different time bases. This disclosure uses the IMU time base to synchronize the camera and lidar times to the IMU time system.

[0109] Laser point: The laser points described in this disclosure all refer to 3D laser points.

[0110] Coordinate system: The coordinate systems involved in this disclosure are all Cartesian coordinate systems (right-hand systems);

[0111] In this disclosure, the camera, lidar, and IMU correspond to three different coordinate systems. Each coordinate system differs by a rotational external parameter and a translational external parameter. These rotational external parameters and translational external parameters are fixed and do not count as part of the system state variables. and The rotation external parameter and translation external parameter from the lidar coordinate system to the IMU coordinate system are: and The rotation extrinsic parameter and translation extrinsic parameter from the camera coordinate system to the IMU coordinate system are:

[0112] In this disclosure, the IMU coordinate system corresponding to the first frame of IMU data is defined as the world coordinate system (W);

[0113] In this disclosure, the system state variables may include one or more of the following: the rotation and translation from the IMU coordinate system to the world coordinate system, the system velocity, the IMU gyroscope and accelerometer zero bias, and the gravitational acceleration in the world coordinate system. In this disclosure, the system state variables may be denoted as x, in and It is the rotation and translation of the system's IMU coordinate system to the world coordinate system. W V is the speed of the system, b g with b a They are the IMU gyroscope and accelerometer zero bias, which are caused by Gaussian noise and Driven random walk, b g 、b a 、 W g is the acceleration of gravity in the world coordinate system, The system state variables are constantly changing at every moment of the system operation. Therefore, when the system receives IMU frames, image frames, and laser frames at different times, each IMU frame, image frame, and laser frame corresponds to its own system state variables.

[0114] In the present disclosure, the current image frame is the image frame being processed by the VIO system, and the current laser frame is the laser frame being processed by the LIO system.

[0115] Combined with the following Figures 1 to 9 The specific embodiments of the present disclosure are described in detail.

[0116] Figure 1 A schematic diagram of a multi-sensor tightly coupled odometer method according to an embodiment of the present disclosure is shown. Figure 1 As shown, in one embodiment, an exemplary implementation process S10 of a multi-sensor tightly coupled odometer method may include the following steps:

[0117] Step S12, acquiring image frames from the camera, laser frames from the lidar, and IMU frames from the IMU;

[0118] Considering the different output frequencies of the camera, lidar, and IMU, image frame data, laser frame data, and IMU frame data cannot be received simultaneously at the same time. Therefore, in some embodiments, step S12 may further include: pre-processing the image frame, laser frame, and IMU data frame to make them correspond to each other.

[0119] In some implementations, preprocessing the image frames, laser frames, and IMU data frames may include associating each image frame and each laser frame with an IMU frame according to their respective timestamps.

[0120] In some implementations, the IMU frame with the highest output frequency can be associated with the image frame and the laser frame respectively according to different timestamps. In this way, when an image frame is received, the VIO system is run to process the image frame and the IMU frame associated with the image frame; when a laser frame is received, the LIO system is run to process the laser frame and the IMU frame associated with the laser frame.

[0121] In some implementations, the method for associating image frames with IMU frames is the same as the method for associating laser frames with IMU frames.

[0122] In the following, image frames and laser frames are collectively referred to as observation frames, and their association method with IMU frames is detailed.

[0123] Figure 2 The diagram shows the principle of association between observation frame and IMU frame. Figure 2As shown, the association between the observation frame and the IMU frame can be divided into three cases: C1, C2, and C3. These three cases correspond to three observation frames F1, F2, and F3 respectively. In one implementation, the association between the observation frame and the IMU frame can include one or more of the following:

[0124] C1: If there is no IMU frame with a timestamp less than the current observation frame F1, the current observation frame F1 can be directly discarded and not processed in subsequent steps. Usually, observation frames like F1 only appear when the system starts running.

[0125] C2: When there is no retained observation frame with a timestamp less than the current observation frame F2 but there is an IMU frame with a timestamp less than the current observation frame F2, the observation frame F2 is retained and the subsequent steps are processed. At this time, all IMU frames with a timestamp less than F2 and the first IMU frame with a timestamp greater than or equal to F2 (that is, the closest IMU frame with a timestamp greater than or equal to F2) can be used as the associated IMU frame of F2. Figure 2 As shown in the figure, the three IMU frames enclosed by the dotted box at the observation frame F2 are the associated IMU frames of F2. Like F1, observation frames like F2 usually only appear when the system starts running.

[0126] C3: If there is a reserved observation frame F2 with a timestamp smaller than the current observation frame F3, the observation frame F3 is retained and the subsequent steps are processed. At this time, the observation frame with a timestamp smaller than F3 and closest to the timestamp of F3 is searched, that is, the observation frame closest to the current observation frame F3 is searched, such as Figure 2 As shown in the figure, the nearest observation frame of the current observation frame F3 is F2, and then all IMU frames with timestamps greater than F2 and less than F3 and the first IMU frame with timestamp greater than or equal to F3 (that is, the nearest IMU frame with timestamp greater than or equal to F3) are taken as the associated IMU frames of F3. Figure 2 The IMU frame enclosed by the dotted box at the F3 position in the figure is the associated IMU frame of F3. Generally, observation frames such as F3 are the most common during the operation of the entire system.

[0127] Step S14, updating the system state variables using the pre-integration constraint relationship of the current image frame, the inter-image frame constraint relationship of the current image frame, and the constraint relationship between the current image frame and the laser frame;

[0128] In some embodiments, when the current image frame is the initial image frame, system initialization needs to be performed in step S14 or before step S14. The system initialization may include: using the initial image frame to add RGB values ​​to the laser points in all received laser frames, and obtaining the image feature points of the initial image frame.

[0129] The entire system initialization phase requires the system to be in a static state. At this time, the system state variables corresponding to all laser frames and image frames received by the system are the same. Therefore, in some implementations, each laser point in the laser frame can be directly projected into the image frame based on the external parameters between the camera and the lidar, so as to attach RGB values ​​to each laser point and obtain image feature points.

[0130] In this disclosure, the image frame that successfully captures image feature points during system initialization is called the initial image frame. The initial image frame automatically meets one condition: a laser frame has already been received before the system receives the initial image frame. This is because only after the system receives the laser frame can the laser point of the laser frame be projected onto the image frame to complete the initialization operation.

[0131] It should be noted that system initialization is an optional step and is only performed when necessary. In this disclosure, as long as one image frame successfully acquires image feature points, the initialization process can be considered complete, and there is no need to perform system initialization again for subsequent image frames and laser frames.

[0132] In some embodiments, using the initial image frame to add RGB values ​​to the laser points in the nearest laser frame that has been received can include: based on the external parameters between the camera and the laser radar, projecting the laser points in all laser frames received during the system initialization process to the initial image frame to determine the pixel coordinates corresponding to the laser point in the initial image frame (i.e., determining the laser pixel point of the laser point), and adding the RGB values ​​of each laser point at the pixel coordinates corresponding to the initial image frame to the data of the laser point.

[0133] Figure 3 A schematic diagram showing the principle of obtaining image feature points of an initial image frame is shown.

[0134] If the best-quality laser pixels in each image block are directly selected as image feature points when acquiring image feature points, although the image feature points obtained in this way are evenly distributed in the image frame, the laser points corresponding to the image feature points may not be evenly distributed in space. Therefore, when selecting image feature points in each image block, it is necessary to consider not only the quality of each laser pixel, but also the distribution of the laser points corresponding to the laser pixels in space.

[0135] In some embodiments, obtaining image feature points of the initial image frame may include: step a1, projecting the laser point of the laser frame closest to the initial image frame into the initial image frame based on the external parameters between the camera and the laser radar to determine the pixel point corresponding to each laser point in the initial image frame, that is, determining the laser pixel point of each laser point. hereinafter, the corresponding pixel point of the laser point in the initial image frame is referred to as the laser pixel point; step a2, dividing each laser pixel point into different image blocks; step a3, selecting the laser pixel point as the image feature point of the initial image frame based on the quality of the laser pixel point in each image block and the spatial position of the laser point corresponding to the laser pixel point. In this way, the spatial distribution of the laser point corresponding to the image feature point and the distribution of the image feature point in the image frame can be uniformized while obtaining the image feature points.

[0136] In some embodiments, Figure 3 As shown, the process of dividing each laser pixel into different image blocks can include: calculating the variance of the coordinates of all laser pixels contained in each image block (initially, there is only one image block) in the x- and y-directions, then dividing the image block equally along the direction with the largest variance, and stopping the equal division of the image block when there is only one laser pixel in the image block. Finally, the equal division of the image block stops when the number of image blocks reaches a threshold of the required number of image feature points (e.g., 700).

[0137] In some implementations, the quadtree method in ORB-SLAM2 can also be used to divide the laser pixels into different image blocks.

[0138] In some embodiments, the quality of a laser pixel in each image block can be determined based on the pixel gradient or image entropy value of the image block centered on the laser pixel. The pixel gradient is the sum of the gradients of all pixels in an image block centered on the laser pixel; a higher gradient indicates better laser pixel quality. The image entropy is the entropy value of an image block centered on the laser pixel; a higher entropy value indicates better laser pixel quality.

[0139] In some implementations, the image entropy value can be calculated using the following formula (1).

[0140]

[0141] In formula (1), H is the image entropy value of an image block centered on the laser pixel, N is the number of pixels contained in an image block centered on the laser pixel, the subscript i is the pixel number, u is the pixel coordinate, I(·) represents the grayscale value of the pixel corresponding to u, and p(·) represents the probability of the grayscale value appearing in the image block. If the grayscale value distribution range of the image block pixels is large, the entropy value is large, and it is considered that the image block has rich texture information or the image pixel gradient changes significantly. If the grayscale value distribution range of the image block pixels is small, the entropy value is small, and it is considered that the image block has no texture information or the image pixel gradient changes are not obvious.

[0142] Figure 4 A schematic diagram showing the principle of selecting image feature points to make the spatial distribution of laser points uniform. Figure 4 In, F C Represents the initial image frame. The laser points of the nearest laser frame of the initial image frame are distributed in the space S. P1, P2, and P3 are three different laser pixel points. A1 and A2 are based on Figure 3 The described method divides the image blocks into different blocks. It is stipulated that the laser points corresponding to each image feature point cannot exist within a 5cm cube centered on the laser point corresponding to the image feature point. Laser pixel P1 is the highest quality laser pixel in image block A1. Since laser pixel P1 does not exist within any 5cm cube centered on the laser point corresponding to the image feature point, P1 is selected as the image feature point. C1 is the 5cm cube centered on the laser point corresponding to P1. In A2, laser pixel P2 has the highest quality, but the laser point corresponding to P2 is located in C1. Meanwhile, the laser point corresponding to P3, which has slightly lower quality than P2, does not exist within any 5cm cube centered on the laser point corresponding to the image feature point. In this case, P2 is not selected as the image feature point, but P3 is. This method obtains the image feature points of an image frame, ensuring a uniform distribution of the image feature points within the frame and a uniform spatial distribution of the laser points corresponding to the image feature points.

[0143] Figure 5 FIG. 4 shows a specific implementation process of step S14 in some embodiments. Figure 5 As shown, the specific implementation process of step S14 may include the following steps:

[0144] Step S52, establishing a pre-integration constraint relationship between the current image frame and the previous observation frame through the IMU frame associated with the current image frame;

[0145] Specifically, based on the IMU data associated with the current image frame, the system state variables corresponding to the current image frame are determined to provide initial values, as well as the pre-integration constraint relationship between the current image frame and the previous observation frame;

[0146] Specifically, the IMU data associated with the current image frame is processed, and initial values ​​for the system state variables corresponding to the current image frame are provided through IMU data integration. Furthermore, a pre-integration constraint relationship between the current image frame and the previous observation frame is established based on the IMU data associated with the current image frame. In this disclosure, the pre-integration constraint relationship between the current image frame and the previous observation frame is denoted as r(x).

[0147] For example, the integration method of IMU data and the method for establishing the pre-integration constraint between the current image frame and the previous observation frame are consistent with the relevant descriptions in the VINS-Mono paper.

[0148] Step S54, obtaining a matching pair of image feature points between the current image frame and the previous image frame;

[0149] In some implementations, optical flow tracking can be used to obtain feature point matching pairs between the previous image frame and the current image frame. In practical applications, however, this is not limited to using optical flow tracking to obtain feature point matching pairs between frames. Similar to the SVO approach, direct methods can also be used to minimize the photometric error of image blocks surrounding the feature points to obtain feature point matching pairs between image frames.

[0150] Step S56: construct an optimization problem between the previous image frame and the current image frame based on the image feature point matching pairs, and optimize and update the system state variables.

[0151] Specifically, an inter-image frame constraint relationship between the current image frame and the previous image frame is constructed based on the image feature point matching pairs obtained in step S54, and a first joint optimization is performed using the pre-integration constraint relationship and the inter-image frame constraint relationship between the current image frame and the previous observation frame to update the system state variables.

[0152] In some embodiments, the inter-frame constraint relationship of the current image frame includes: a reprojection residual of a matching pair of image feature points between the current image frame and the previous image frame, and an RGB residual between a laser point corresponding to the matching pair of image feature points and a laser pixel point projected onto the current image frame. In some embodiments, the reprojection residual can be determined using Equation (2), and the RGB residual can be determined using Equation (3).

[0153] In some implementations, the residual equation between the previous image frame and the current image frame may be constructed based on the following equations (2) to (3) according to the image feature point matching pairs:

[0154]

[0155] o(x, W P i )=O( W P i )-I(u i)i=1、2...N (3)

[0156] Formula (2) is the reprojection residual equation of the image feature point matching pair. In Formula (2), N represents the number of image feature point matching pairs, the subscript i represents the sequence number of the image feature point matching pair, and u represents the pixel coordinates of the image feature point of the current image frame in the image feature point matching pair. W P represents the coordinates of the laser point corresponding to the image feature point matching in the world coordinate system, K represents the intrinsic parameter matrix of the camera, c(x, W P i ,u i ) is the reprojection residual of the image feature point matching pair, and It is the rotation and translation of the system's IMU coordinate system to the world coordinate system. and The rotation extrinsic parameter and translation extrinsic parameter from the camera coordinate system to the IMU coordinate system are: for The transpose of for The transpose of .

[0157] Formula (3) is the RGB residual equation between the laser point corresponding to the image feature point matching pair and the laser pixel point projected on the current image frame. W The definition of P is consistent with that in formula (2), u i Represents the pixel coordinates of the laser pixel point obtained by projecting the corresponding laser point of the image feature point matching pair on the current image frame, and O(·) represents W The RGB value of the laser point corresponding to P is added, I(·) represents the RGB value of the pixel coordinate u in the current image frame, o(x, W P i ) is the RGB residual between the laser point corresponding to the image feature point matching pair and the laser pixel point projected on the current image frame.

[0158] In some embodiments, in step S56, the constraint relationship constructed based on the two residual equations of formulas (2) to (3) and the pre-integration constraint relationship between the current image frame and the previous observation frame can be jointly optimized using a graph optimization method to optimize and update the system state variables.

[0159] In some implementations, in step S56, the function shown in the following formula (4) can be used as the objective function, and the residual equations and pre-integration constraints of formulas (2) to (3) can be jointly optimized to optimize and update the system state variables.

[0160]

[0161] In formula (4), r(x) is the pre-integration constraint relationship between the current image frame and the previous observation frame, x is the system state variable, c(x, W P i ,u i ) is the reprojection residual of the image feature point matching pair, o(x, W P i ) is the RGB residual between the laser point corresponding to the image feature point matching pair and the laser pixel point projected on the current image frame, W P represents the coordinates of the laser point corresponding to the image feature point matching in the world coordinate system, u i It represents the pixel coordinates of the laser pixel point obtained by projecting the laser point corresponding to the image feature point matching pair onto the current image frame, N represents the number of image feature point matching pairs, and the subscript i represents the sequence number of the image feature point matching pair.

[0162] Step S58: Find the constraint relationship between the image frame and the laser frame to construct an optimization problem, and further optimize and update the system state variables.

[0163] In some embodiments, the constraint relationship between the current image frame and the nearest laser frame can be constructed based on the system state variables updated in step S56 (i.e., the first joint optimization) and the image feature point matching pairs obtained in step S54, and a second joint optimization can be performed using the pre-integration constraint relationship between the current image frame and the previous observation frame and the constraint relationship between the current image frame and the nearest laser frame to update the system state variables again.

[0164] In some embodiments, the constraint relationship between the current image frame and the laser frame includes: the three-dimensional coordinate residual of the 3D point matching pair between the current image frame and the nearest laser frame, and the 3D point matching pair includes the triangulated point of the image feature point matching pair between the current image frame and the previous image frame and the laser point corresponding to the image feature point matching pair.

[0165] In some implementations, the three-dimensional coordinate residual of a 3D point matching pair can be determined using equation (5).

[0166] Figure 6 A schematic diagram showing the principle of constructing an optimization problem by finding the constraint relationship between the image frame and the laser frame.

[0167] Figure 6 middle, Indicates the current image frame, express The previous image frame of and The image feature points connected by the dotted line between the two frames are the image feature point matching pairs obtained in step S32 , and the two feature points in an image feature point matching pair correspond to the same laser point.

[0168] After the system state variables are updated through S56 optimization, triangulation and The image feature points between the two frames are matched to determine the triangulated points. The triangulated points (i.e., and The triangulated points of the image feature point matching pairs between the two frames and the laser points corresponding to the image feature point matching pairs form a 3D point matching pair. Figure 6 The points enclosed by the solid ellipse in the figure are these 3D point matching pairs. Since the laser points corresponding to different image feature point matching pairs may come from different laser frames, the connection between the image frame and the laser frame can be established by forming 3D point matching pairs with the laser points corresponding to the image feature point matching pairs.

[0169] If the system state variables optimized and updated in step S56 are highly accurate, the distance between 3D point matching pairs should be small, or even zero. If the system state variables optimized and updated in step S56 are not highly accurate, the distance between 3D point matching pairs will be relatively large. Therefore, in some implementations, a three-dimensional coordinate residual equation can be constructed using the 3D point matching pairs. Based on the constraints constructed from the residual equation and the pre-integrated constraints between the current image frame and the previous observation frame, an optimization problem can be constructed using graph optimization methods to optimize and update the system state variables, thereby improving their accuracy.

[0170] In some implementations, the three-dimensional coordinate residual equation between 3D point matching pairs can be expressed as the following equation (5).

[0171]

[0172] In formula (5), N is the number of 3D point matching pairs, and the subscript i is the sequence number of the 3D point matching pair. W P is the coordinate of the laser point corresponding to the image feature point matching pair in the 3D point matching pair in the world coordinate system, C P is the coordinate of the triangulated point in the current image frame coordinate system of the image feature point matching pair in the 3D point matching pair, n(x, W P i , C P i ) represents the three-dimensional coordinate residual between 3D point matching pairs, and It is the rotation and translation of the system's IMU coordinate system to the world coordinate system. and The rotation extrinsic parameter and translation extrinsic parameter from the camera coordinate system to the IMU coordinate system.

[0173] In some embodiments, the objective function for performing joint optimization based on the constraint relationship constructed based on the 3D coordinate residual equation of the 3D point matching pair and the pre-integrated constraint relationship between the current image frame and the previous observation frame can be the following formula (6):

[0174]

[0175] In formula (6), n(x, W P i , C P i ) represents the three-dimensional coordinate residual between 3D point matching pairs, r(x) is the pre-integration constraint relationship between the current image frame and the previous observation frame, N is the number of 3D point matching pairs, subscript i is the sequence number of the 3D point matching pair, and x is the system state variable. W P is the coordinate of the laser point corresponding to the image feature point matching pair in the 3D point matching pair in the world coordinate system, and cP is the coordinate of the point triangulated by the image feature point matching pair in the 3D point matching pair in the current image frame coordinate system.

[0176] Step S16, updating the system state variables using the pre-integration constraint relationship of the current laser frame, the inter-laser frame constraint relationship of the current laser frame, and the constraint relationship between the current laser frame and the image frame;

[0177] That is, step S1 searches for constraint relationships between laser frames and between laser and image frames to construct an optimization problem, and optimizes and updates system state variables.

[0178] Figure 7 FIG. 4 shows the specific implementation process of step S16 in some embodiments. Figure 7 As shown, the specific implementation process of step S16 may include the following steps:

[0179] Step S72, establishing a pre-integration constraint relationship between the current laser frame and the previous observation frame through the IMU frame associated with the current laser frame;

[0180] The last observation frame is the last image frame or the last laser frame whose timestamp is closest to the current laser frame.

[0181] In some embodiments, step S72 may include: determining, based on IMU data associated with the current laser frame, providing initial values ​​of system state variables corresponding to the current laser frame, and a pre-integration constraint relationship between the current laser frame and a previous observation frame.

[0182] In some implementations, the specific implementation of step S72 may be the same as that of step S52 and will not be repeated here.

[0183] Step S74, obtaining the feature point matching relationship of the current laser frame;

[0184] In some implementations, step S74 may include: extracting feature points of the current laser frame, searching and matching the feature points extracted from the current laser frame with a point cloud set of laser feature points of multiple frames through a kd-tree, and obtaining a matching relationship of the feature points of the current laser frame.

[0185] In some implementations, the relevant methods in the LOAM paper can be used to extract feature points, and the search and matching strategy can also be consistent with that in LOAM.

[0186] In some implementations, the extracted feature points may include surface feature points and line feature points. For surface feature points, the three closest laser points in the multi-frame point cloud of laser surface feature points are searched; for line feature points, the two closest laser points in the multi-frame point cloud of laser line feature points are searched. The multi-frame point cloud of laser feature points is maintained and updated based on the most recent five laser frames.

[0187] In some implementations, a kd-tree can be used to search and match feature points. In practice, real-time updates of a kd-tree are time-consuming. Therefore, a more efficient real-time update method (e.g., the kd-tree used in R3L1VE) can be used instead of a kd-tree.

[0188] Step S76: construct an optimization problem between laser frames based on the matching relationship of the laser feature points obtained in step S74, and optimize and update the system state variables.

[0189] In some embodiments, step S76 may include: constructing an inter-laser frame constraint relationship of the current laser frame based on the feature point matching relationship of the current laser frame, and performing a third joint optimization using the pre-integration constraint relationship and the inter-laser frame constraint relationship of the current laser frame to update the system state variables.

[0190] In some embodiments, the inter-laser frame constraint relationship of the current laser frame may include: the point-to-surface distance residual of the surface formed by the surface feature point of the current laser frame and the laser point that matches it, and the point-to-line distance residual of the straight line formed by the line feature point of the current laser frame and the laser point that matches it.

[0191] In some implementations, the inter-laser frame constraint relationship of the current laser frame can be expressed as the residual equations shown in the following equations (7) to (8).

[0192]

[0193]

[0194] Formula (7) is the residual equation of the point-to-surface distance between the surface feature point of the current laser frame and the surface composed of the three closest laser points it matches. In formula (7), N is the total number of surface feature points, subscript i is the serial number of each surface feature point, subscript z is the serial number of the three closest laser points matched with the surface feature point (z = 1, 2, 3), and n is the normal vector of the surface composed of the three closest laser points matched with the surface feature point. It is the coordinates of the three laser points closest to each surface feature point in the world coordinate system. L P i It is the coordinate of each surface feature point in the current laser coordinate system. Indicates the residual of the point-to-surface distance between the surface feature point of the current laser frame and the surface composed of the three laser points closest to it.

[0195] Formula (8) is the residual equation of the point-line distance between the line feature point of the current laser frame and the line formed by the two laser points closest to it. In formula (8), N is the total number of line feature points, and is the coordinates of the two laser points closest to each line feature point in the world coordinate system, and the subscript i is L P i The definition of is the same as that in formula (7), Represents the point-line distance residual of the line feature point in the current laser frame and the line formed by the two laser points closest to it.

[0196] In some embodiments, the objective function for performing joint optimization based on the constraint relationship constructed based on the feature point matching relationship of the current laser frame and the pre-integration constraint relationship between the current laser frame and the previous observation frame can be the following formula (9):

[0197]

[0198] In formula (9), r(x) is the pre-integration constraint relationship between the current laser frame and the previous observation frame, x is the system state variable, Indicates the residual of the point-line distance between the line feature point of the current laser frame and the two laser points closest to it. Indicates the residual distance between the surface feature point of the current laser frame and the surface composed of the three laser points closest to it. N in is the total number of surface feature points, N is the total number of line feature points.

[0199] Step S78 , searching for a constraint relationship between the current laser frame and the image frame closest to the current laser frame and constructing an optimization problem to further optimize and update the system state variables.

[0200] In some embodiments, step S78 may include: constructing a constraint relationship between the current laser frame and the nearest image frame based on the system state variables obtained by the third joint optimization update and the feature point matching relationship of the current laser frame, and performing a fourth joint optimization using the pre-integration constraint relationship of the current laser frame and the constraint relationship between it and the nearest image frame to update the system state variables again, thereby further improving the accuracy of the system state variables.

[0201] In some embodiments, the constraint relationship between the current laser frame and the image frame may include: the three-dimensional coordinate residual of the laser point matching pair between the current laser frame and the nearest image frame, the laser point matching pair including the first laser point in the current laser frame and the laser point corresponding to the first image feature point in the nearest image frame of the current laser frame, and the laser pixel point obtained by projecting the first laser point onto the nearest image frame coincides with the first image feature point of the nearest image frame.

[0202] In some embodiments, the three-dimensional coordinate residual of the laser point matching pair between the current laser frame and the nearest image frame can be determined by the following equation (10).

[0203] Figure 8 A schematic diagram showing the principle of constructing an optimization problem by finding the constraint relationship between the current laser frame and its nearest neighboring image frame.

[0204] Figure 8 middle, Represents the current laser frame, Represents the previous laser frame, represents the image frame closest to the timestamp of the current laser frame, express The previous image frame. Figure 8 middle and The image feature points connected by the dotted line between the two frames are the image feature point matching pairs obtained in step S32.

[0205] According to the system state variables optimized in step S76, the laser point of the current laser frame is projected to Find the projected The image feature points in the image frame overlap with the laser points in the current frame. Since each image feature point has a corresponding laser point, after finding the laser point in the current frame that overlaps with the image feature point after projection, the laser point corresponding to the image feature point and the laser point in the current frame that overlaps with the image feature point after projection are considered as a laser point matching pair and this matching relationship is recorded. Figure 8 The points enclosed by the solid ellipse are the laser point matching pairs.

[0206] like Figure 8 As shown, the laser point matching relationship is divided into three cases: C1, C2, and C3:

[0207] In C1, the image feature points corresponding to the laser point matching pair are The image feature points in are obtained by optical flow tracking, so the laser point corresponding to the image feature point may not be Observed, but This is equivalent to comparing the nearest image frame of the current laser frame with the Even The previous laser frame established the connection.

[0208] In C2, the image feature points corresponding to the laser point matching point pair are This is equivalent to the nearest image frame of the current laser frame. With the previous laser frame A connection was established.

[0209] In C3, multiple current frame laser points coincide with the same image feature point after projection. At this time, only the matching relationship between the current frame laser point closest to the laser point corresponding to the image feature point and the laser point corresponding to the image feature point is retained.

[0210] If the system state variables optimized in step S76 are highly accurate, the spacing between the laser point matching pairs obtained in this step will be very small, even approaching zero. If the system state variables optimized in step S76 are not accurately enough, the spacing between the laser point matching pairs will be very large. Therefore, in some embodiments, in step S78, a three-dimensional coordinate residual equation between the laser point matching pairs can be constructed. Based on the constraint relationship constructed from the three-dimensional coordinate residual equation between the laser points and the pre-integrated constraint relationship between the current laser frame and the previous observation frame, a joint optimization method is performed using graph optimization to update the system state variables, thereby further improving the accuracy of the system state variables.

[0211] In some embodiments, the three-dimensional coordinate residual equation between the laser point matching pairs may be the equation shown in equation (10).

[0212]

[0213] In formula (10), N is the number of laser point matching pairs, and the subscript i is the serial number of the laser point matching pair. W P i and L P i is the laser point matching pair, W P i is the laser point corresponding to the image feature point, L P i is the laser point of the current frame, W P iIn the world coordinate system, L P i Located in the current laser frame coordinate system. h(x, W P i , L P i ) represents the 3D coordinate residual between the matching pairs of laser points.

[0214] In some embodiments, the objective function for performing joint optimization based on the constraint relationship constructed by the laser point matching pair and the pre-integration constraint relationship between the current laser frame and the previous observation frame in step S78 can be the following formula (11):

[0215]

[0216] Among them, r(x) is the pre-integration constraint relationship between the current laser frame and the previous observation frame, x is the system state variable, h(x, W P i , L P i ) represents the 3D coordinate residual between the matching pairs of laser points.

[0217] Step S18: Based on the system state variables updated in step S14, the RGB values ​​of the laser points in the local point cloud map and the image feature points of the current image frame are updated.

[0218] In some embodiments, based on the system state variables obtained in step S14, laser points within the field of view of the current image frame in the local point cloud map can be projected into the current image frame to determine the pixel coordinates corresponding to the laser points in the current image frame, and the RGB values ​​at the pixel coordinates corresponding to the laser points in the current image frame can be added to the laser points. Typically, the local point cloud map is maintained by the most recently processed laser frames.

[0219] In some implementations, if some laser points in the local point cloud map already had RGB values ​​assigned to them when processing image frames prior to the current frame, the assigned RGB values ​​can be fused with the RGB values ​​of the corresponding pixel coordinates in the current frame, and the RGB values ​​assigned to the laser points can be updated to the fused RGB values. For example, the RGB value fusion method described in the R3LIVE paper can be used.

[0220] The method for updating the image feature points of the current image frame is substantially the same as the method for obtaining the image feature points of the initial image frame during system initialization described above, except that, before the image feature points are updated, the current image frame already contains image feature points tracked from the previous image frame using the optical flow method, whereas the initial image frame does not contain any image feature points before the image feature points are added. That is, in some embodiments, adding image feature points to the current image frame may include: adding image feature points to the current image frame based on the image feature points already existing in the current image frame that were tracked from the previous image frame using the optical flow method and the laser pixel points in the current image frame of the laser frame closest to the current image frame.

[0221] In some embodiments, adding image feature points to the current image frame may include: first projecting the laser point of the laser frame closest to the current image frame to the current image frame to obtain the laser pixel point of the laser frame closest to the current image frame in the current image frame, then dividing the existing image feature points of the current image frame and the laser pixel points tracked from the previous image frame by the optical flow method into different image blocks of the current image frame, and finally adding image feature points to each image block of the current image frame according to the image feature points and laser pixel points in each image block.

[0222] In some implementations, the laser pixel points and the image feature points already existing in the current image frame and tracked from the previous image frame using the optical flow method can be divided into different image blocks of the current image frame according to the division method in step S14.

[0223] In some implementations, adding image feature points to each image block of the current image frame based on the image feature points and laser pixel points in each image block may include the following three situations:

[0224] Case 1: When there is an existing image feature point in the image block, the existing image feature point is retained and used as the current image feature point of the image block. That is, when there is only one image feature point in the image block that is tracked from the previous image frame by the optical flow method, this image feature point is retained, and no image feature point is added to the image block by laser pixel points.

[0225] Case 2: When there are multiple existing image feature points in the image block, the image feature point with the best quality among the multiple existing image feature points is retained, and the image feature point with the best quality is used as the current image feature point of the image block. That is, when there are multiple image feature points in the image block that are tracked from the previous image frame through the optical flow method, the image feature point with the best quality is retained, the remaining image feature points are deleted, and no image feature points are added in the image block through laser pixel points.

[0226] Case 3: When there is no existing image feature point in the image block, a laser pixel point is selected as the image feature point of the image block based on the quality of the laser pixel point in the image block and the spatial position of the laser point corresponding to the laser pixel point. That is, when there is no image feature point in the image block that is tracked from the previous image frame by the optical flow method, the method of adding image feature points to the initial image frame is adopted, and image feature points are added to the current image frame through the laser pixel point.

[0227] Step S110: merging the laser point cloud data of the current laser frame into the global point cloud map according to the system state variables updated in step S16;

[0228] Since all laser points of the current laser frame are located in the laser coordinate system, the latest laser point cloud data (i.e., the laser point cloud of the current laser frame) can be converted from the current laser frame coordinate system to the world coordinate system according to the system state variables obtained in S16, and then the laser point cloud of the current laser frame converted to the world coordinate system is fused with the global point cloud map, thereby merging the laser point cloud data of the current laser frame into the global point cloud map.

[0229] Figure 9 The figure is a schematic block diagram of a structure of a tightly coupled odometer device based on multiple sensors using a hardware implementation of a processing system according to an embodiment of the present disclosure.

[0230] The device may include corresponding modules for executing each or several steps in the above flowchart. Therefore, each step or several steps in the above flowchart may be executed by a corresponding module, and the device may include one or more of these modules. The module may be one or more hardware modules specifically configured to execute the corresponding steps, or implemented by a processor configured to execute the corresponding steps, or stored in a computer-readable medium for execution by a processor, or implemented by some combination thereof.

[0231] The hardware structure can be implemented using a bus architecture. The bus architecture can include any number of interconnecting buses and bridges, depending on the specific application and overall design constraints of the hardware. Bus 1200 connects various circuits including one or more processors 1300, memory 1400, and / or hardware modules. Bus 1200 can also connect various other circuits 1500 such as peripherals, voltage regulators, power management circuits, external antennas, etc.

[0232] Bus 1200 may be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Component Architecture (EISA) bus. Buses can be classified as address buses, data buses, control buses, and the like. For ease of illustration, this figure shows only one connecting line, but this does not imply that there is only one bus or only one type of bus.

[0233] Any process or method description in the flowchart or otherwise described herein can be understood to represent a module, fragment or portion of code including one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present disclosure includes alternative implementations in which the functions may not be performed in the order shown or discussed, including performing the functions in a substantially simultaneous manner or in a reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present disclosure belong. The processor performs the various methods and processes described above. For example, the method embodiments of the present disclosure can be implemented as a software program that is tangibly contained in a machine-readable medium, such as a memory. In some embodiments, part or all of the software program can be loaded and / or installed via a memory and / or a communication interface. When the software program is loaded into the memory and executed by the processor, one or more steps in the method described above can be performed. Alternatively, in other embodiments, the processor can be configured to perform one of the above methods in any other appropriate manner (e.g., by means of firmware).

[0234] The logic and / or steps represented in the flowchart or otherwise described herein may be embodied in any readable storage medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other system that can fetch and execute instructions from an instruction execution system, apparatus, or device).

[0235] For the purposes of this specification, a "readable storage medium" can be any device that can contain, store, communicate, propagate or transmit a program for use with or in conjunction with an instruction execution system, device or apparatus. More specific examples (a non-exhaustive list) of readable storage media include the following: an electrical connection having one or more wires (electronic device), a portable computer disk cartridge (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and editable read-only memory (EPROM or flash memory), a fiber optic device, and a portable read-only memory (CDROM). In addition, the readable storage medium can even be paper or other suitable medium on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing it in other suitable ways as necessary, and then storing it in a memory.

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

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

[0238] Furthermore, the functional units in the various embodiments of the present disclosure may be integrated into a single processing module, each unit may exist physically separately, or two or more units may be integrated into a single module. The aforementioned integrated modules may be implemented in the form of hardware or software functional modules. If the integrated modules are implemented as software functional modules and sold or used as independent products, they may also be stored in a readable storage medium. The storage medium may be a read-only memory, a magnetic disk, or an optical disk, etc.

[0239] like Figure 9 As shown, a multi-sensor based tightly coupled odometer device 900 according to some embodiments of the present disclosure may include:

[0240] An acquisition unit 902 is configured to acquire image frames from a camera, laser frames from a lidar, and IMU frames from an IMU;

[0241] A first state updating unit 904 is configured to update system state variables using a pre-integration constraint relationship of a current image frame, an inter-image frame constraint relationship of the current image frame, and a constraint relationship between the current image frame and a laser frame, wherein the pre-integration constraint relationship of the current image frame is established by the IMU frame associated with the current image frame;

[0242] A first map updating unit 906 is configured to update the RGB value of the laser point in the local point cloud map based on the system state variable updated by the first state updating unit;

[0243] An image feature point updating unit 908, configured to update the image feature points of the current image frame based on the system state variables updated by the first state updating unit;

[0244] A second state updating unit 910 is configured to update system state variables using a pre-integration constraint relationship of a current laser frame, an inter-laser frame constraint relationship of the current laser frame, and a constraint relationship between the current laser frame and an image frame, wherein the pre-integration constraint relationship of the current laser frame is established by the IMU frame associated with the current laser frame, and the constraint relationship between the current laser frame and the image frame is determined by the laser point of the current laser frame and the image feature point of the nearest image frame;

[0245] The second map updating unit 912 is configured to merge the laser point cloud data of the current laser frame into the global point cloud map according to the updated system state variables.

[0246] In some implementations, the multi-sensor based tightly coupled odometer device 900 further includes: an associating unit 914, configured to associate the image frame and the laser frame with the IMU frame according to their respective timestamps.

[0247] In some embodiments, the image feature point updating unit 908 is specifically used to obtain the image feature points of the current image frame when the current image frame is the initial image frame in the following manner: projecting the laser point of the laser frame closest to the initial image frame into the initial image frame based on the external parameters between the camera and the laser radar to determine the laser pixel point of each laser point; dividing each laser pixel point into different image blocks; and selecting the laser pixel point as the image feature point of the initial image frame based on the quality of the laser pixel point in each image block and the spatial position of the laser point corresponding to the laser pixel point.

[0248] In some embodiments, the image feature point updating unit 908 is specifically used to update the image feature points of the current image frame when the current image frame is not the initial image frame, in the following manner: based on the external parameters between the camera and the laser radar, the laser point of the laser frame closest to the current image frame is projected into the initial image frame to determine the laser pixel point of each laser point; each laser pixel point and the existing image feature point in the current image frame are divided into different image blocks of the current image frame; based on the quality of the laser pixel point in each image block, the spatial position of the laser point corresponding to the laser pixel point, and the number and quality of the existing image feature points, one of the laser pixel points or the existing image feature point is selected as the current image feature point of the image block.

[0249] In some implementations, the quality of a laser pixel is determined based on a pixel gradient or an image entropy value of an image block centered on the laser pixel.

[0250] In some embodiments, the first map updating unit 906 is specifically used to update the RGB value of the laser point in the local point cloud map in the following manner: based on the external parameters between the camera and the lidar, the laser point in the local point cloud map within the field of view of the current image frame is projected to the current image frame to determine the pixel coordinates corresponding to the laser point in the local point cloud map within the field of view of the current image frame; and the RGB value at the pixel coordinate corresponding to the laser point in the current image frame is appended to the laser point.

[0251] In some embodiments, the first map updating unit 906 is specifically used to update the RGB value of the laser point in the local point cloud map when the current image frame is not the initial image frame in the following manner: based on the external parameters between the camera and the lidar, the laser point in the local point cloud map within the field of view of the current image frame is projected to the current image frame to determine the pixel coordinates corresponding to the laser point in the local point cloud map within the field of view of the current image frame; the RGB value at the pixel coordinate corresponding to the laser point in the current image frame and the RGB value already attached to the laser point are fused, and the RGB value already attached to the laser point is updated to the fused RGB value.

[0252] In some implementations, the first state updating unit 904 is specifically configured to update the system state variables in the following manner:

[0253] Establishing a pre-integration constraint relationship between the current image frame and a previous observation frame through the IMU frame associated with the current image frame, wherein the previous observation frame is the previous image frame or the previous laser frame whose timestamp is closest to the current image frame;

[0254] Obtain the image feature point matching pairs between the current image frame and the previous image frame;

[0255] constructing an inter-image frame constraint relationship between the current image frame and the previous image frame based on the image feature point matching pairs, and performing a first joint optimization using the pre-integration constraint relationship and the inter-image frame constraint relationship between the current image frame and the previous observation frame to update the system state variables;

[0256] A constraint relationship between the current image frame and the nearest laser frame is constructed based on the system state variables obtained from the first joint optimization update and the image feature point matching pairs. A second joint optimization is performed using the pre-integration constraint relationship between the current image frame and the previous observation frame and the constraint relationship between the current image frame and the nearest laser frame to update the system state variables again.

[0257] In some embodiments, the inter-image frame constraint relationship of the current image frame includes: the reprojection residual of the image feature point matching pair between the current image frame and the previous image frame and the RGB residual between the laser point corresponding to the image feature point matching pair and the laser pixel point obtained by projecting it on the current image frame.

[0258] In some embodiments, the constraint relationship between the current image frame and the laser frame includes: the three-dimensional coordinate residual of the 3D point matching pair between the current image frame and the nearest laser frame, the 3D point matching pair including the triangulated point of the image feature point matching pair between the current image frame and the previous image frame and the laser point corresponding to the image feature point matching pair.

[0259] In some implementations, the second state updating unit 910 is specifically configured to update the system state variables in the following manner:

[0260] Establishing a pre-integration constraint relationship between the current laser frame and a previous observation frame through the IMU frame associated with the current laser frame, wherein the previous observation frame is the previous image frame or the previous laser frame whose timestamp is closest to the current laser frame;

[0261] Get the feature point matching relationship of the current laser frame;

[0262] constructing an inter-laser frame constraint relationship of the current laser frame according to the feature point matching relationship of the current laser frame, and performing a third joint optimization using the pre-integration constraint relationship of the current laser frame and the inter-laser frame constraint relationship to update system state variables;

[0263] A constraint relationship between the current laser frame and the nearest image frame is constructed based on the system state variables obtained by the third joint optimization update and the feature point matching relationship of the current laser frame. A fourth joint optimization is performed using the pre-integration constraint relationship of the current laser frame and the constraint relationship between it and the nearest image frame to update the system state variables again.

[0264] In some embodiments, the inter-laser frame constraint relationship of the current laser frame includes: the point-to-surface distance residual of the surface formed by the surface feature points of the current laser frame and the matching laser points, and the point-to-line distance residual of the straight line formed by the line feature points of the current laser frame and the matching laser points.

[0265] In some embodiments, the constraint relationship between the current laser frame and the image frame includes: the three-dimensional coordinate residual of the laser point matching pair between the current laser frame and the nearest image frame; wherein the laser point matching pair includes the first laser point in the current laser frame and the laser point corresponding to the first image feature point in the nearest image frame of the current laser frame; wherein the laser pixel point obtained by projecting the first laser point onto the nearest image frame overlaps with the first image feature point of the nearest image frame.

[0266] The present disclosure also provides an electronic device, including: a memory, the memory storing execution instructions; and a processor or other hardware module, the processor or other hardware module executing the execution instructions stored in the memory, so that the processor or other hardware module executes the above-mentioned multi-sensor based tightly coupled odometer method.

[0267] In practical applications, electronic devices can be used in vehicles, logistics vehicles, robots or other scenarios.

[0268] The present disclosure also provides a readable storage medium, in which execution instructions are stored. When the execution instructions are executed by a processor, they are used to implement the above-mentioned multi-sensor based tightly coupled odometer method.

[0269] In the description of this specification, the description with reference to the terms "one embodiment / method", "some embodiments / methods", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment / method or example are included in at least one embodiment / method or example of the present application. In this specification, the schematic representations of the above terms are not necessarily the same embodiment / method or example. Moreover, the specific features, structures, materials or characteristics described may be combined in a suitable manner in any one or more embodiments / methods or examples. In addition, those skilled in the art may combine and combine the different embodiments / methods or examples described in this specification and the features of the different embodiments / methods or examples, unless they are mutually inconsistent.

[0270] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of the technical features being referred to. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of such features. Throughout the description of this application, "plurality" means at least two, for example, two, three, etc., unless otherwise specifically defined.

[0271] Those skilled in the art will appreciate that the above embodiments are merely intended to clearly illustrate the present disclosure and are not intended to limit the scope of the present disclosure. Other changes or modifications may be made based on the above disclosure, and such changes or modifications are still within the scope of the present disclosure.

Claims

1. A tightly coupled odometer method based on multiple sensors, characterized in that: include: Get image frames from the camera, laser frames from the lidar, and IMU frames from the IMU; updating the system state variables using the pre-integration constraint relationship of the current image frame, the inter-image frame constraint relationship of the current image frame, and the constraint relationship between the current image frame and the laser frame; and updating the RGB values ​​of the laser points in the local point cloud map and the image feature points of the current image frame based on the updated system state variables, wherein the pre-integration constraint relationship of the current image frame is established by the IMU frame associated with the current image frame; The system state variables are updated using the pre-integration constraint relationship of the current laser frame, the inter-laser frame constraint relationship of the current laser frame, and the constraint relationship between the current laser frame and the image frame. The laser point cloud data of the current laser frame is merged into the global point cloud map according to the updated system state variables. The pre-integration constraint relationship of the current laser frame is established by the IMU frame associated with the current laser frame, and the constraint relationship between the current laser frame and the image frame is determined by the laser point of the current laser frame and the image feature point of the nearest image frame.

2. The multi-sensor tightly coupled odometer method according to claim 1, characterized in that: Also includes: The image frame and the laser frame are respectively associated with the IMU frame according to their respective timestamps.

3. The multi-sensor tightly coupled odometer method according to claim 2, characterized in that: Associating the image frame and the laser frame with the IMU frame according to their respective timestamps, includes one of the following: When there is no retained observation frame with a timestamp less than the current observation frame but there is an IMU frame with a timestamp less than the current observation frame, all IMU frames with a timestamp less than the current observation frame and the first IMU frame with a timestamp greater than or equal to the current observation frame are taken as the IMU frames associated with the current observation frame; When there is a reserved observation frame with a timestamp less than the current observation frame, find the nearest observation frame with a timestamp less than the current observation frame, and take all IMU frames with a timestamp greater than the nearest observation frame and a timestamp less than the current observation frame and the first IMU frame with a timestamp greater than or equal to the current observation frame as the IMU frames associated with the current observation frame; When there is no IMU frame with a timestamp less than the current observation frame, the current observation frame is discarded; The observation frame is the image frame or the laser frame.

4. The multi-sensor tightly coupled odometer method according to claim 1, characterized in that: When the current image frame is an initial image frame, the image feature points of the current image frame are obtained in the following manner: Projecting the laser point of the laser frame closest to the initial image frame into the initial image frame according to the external parameters between the camera and the laser radar to determine the laser pixel point of each laser point; Divide each laser pixel into different image blocks; Laser pixels are selected as image feature points of the initial image frame according to the quality of the laser pixels in each image block and the spatial position of the laser points corresponding to the laser pixels.

5. The multi-sensor tightly coupled odometer method according to claim 1, characterized in that: When the current image frame is not the initial image frame, the image feature points of the current image frame are updated in the following manner: Projecting the laser point of the laser frame closest to the current image frame into the current image frame according to the external parameters between the camera and the laser radar to determine the laser pixel point of each laser point; Dividing each of the laser pixel points and the image feature points existing in the current image frame into different image blocks of the current image frame; According to the quality of the laser pixel point in each image block, the spatial position of the laser point corresponding to the laser pixel point, and the number and quality of existing image feature points, one of the laser pixel points or the existing image feature point is selected as the current image feature point of the image block.

6. The multi-sensor tightly coupled odometer method according to claim 4 or 5, characterized in that: The quality of the laser pixel point is judged based on the pixel gradient or image entropy value of the image block centered on the laser pixel point.

7. The multi-sensor tightly coupled odometer method according to claim 5, characterized in that: The selecting, based on the quality of the laser pixel points in each of the image blocks, the spatial position of the laser point corresponding to the laser pixel point, and the quantity and quality of the existing image feature points, a laser pixel point or an existing image feature point as the current image feature point of the image block includes one of the following: When there is an existing image feature point in the image block, retain the existing image feature point and use the existing image feature point as the current image feature point of the image block; When there are multiple existing image feature points in the image block, retain the image feature point with the best quality among the multiple existing image feature points, and use the image feature point with the best quality as the current image feature point of the image block; When there is no existing image feature point in the image block, a laser pixel point is selected as the image feature point of the image block according to the quality of the laser pixel point in the image block and the spatial position of the laser point corresponding to the laser pixel point.

8. The multi-sensor tightly coupled odometer method according to claim 1, characterized in that: The RGB values ​​of the laser points in the local point cloud map are updated as follows: Projecting the laser points in the local point cloud map within the field of view of the current image frame onto the current image frame based on the external parameters between the camera and the laser radar to determine the pixel coordinates corresponding to the laser points in the local point cloud map within the field of view of the current image frame; The RGB value of the pixel coordinate corresponding to the laser point in the current image frame is added to the laser point.

9. The multi-sensor tightly coupled odometer method according to claim 1, characterized in that: When the current image frame is not the initial image frame, the RGB value of the laser point in the local point cloud map is updated in the following manner: Projecting the laser points in the local point cloud map within the field of view of the current image frame onto the current image frame based on the external parameters between the camera and the laser radar to determine the pixel coordinates corresponding to the laser points in the local point cloud map within the field of view of the current image frame; The RGB value at the pixel coordinate corresponding to the laser point in the current image frame is fused with the RGB value already attached to the laser point, and the RGB value already attached to the laser point is updated to the fused RGB value.

10. The multi-sensor tightly coupled odometer method according to claim 1, characterized in that: The updating of the system state variables using the pre-integration constraint relationship of the current image frame, the inter-image frame constraint relationship of the current image frame, and the constraint relationship between the current image frame and the laser frame includes: Establishing a pre-integration constraint relationship between the current image frame and a previous observation frame through the IMU frame associated with the current image frame, where the previous observation frame is the previous image frame or the previous laser frame whose timestamp is closest to the current image frame; Obtain the image feature point matching pairs between the current image frame and the previous image frame; constructing an inter-image frame constraint relationship between the current image frame and the previous image frame based on the image feature point matching pairs, and performing a first joint optimization using the pre-integration constraint relationship and the inter-image frame constraint relationship between the current image frame and the previous observation frame to update the system state variables; A constraint relationship between the current image frame and the nearest laser frame is constructed based on the system state variables obtained from the first joint optimization update and the image feature point matching pairs. A second joint optimization is performed using the pre-integration constraint relationship between the current image frame and the previous observation frame and the constraint relationship between the current image frame and the nearest laser frame to update the system state variables again.

11. The multi-sensor tightly coupled odometer method according to claim 1 or 10, characterized in that: The inter-image frame constraint relationship of the current image frame includes: the reprojection residual of the image feature point matching pair between the current image frame and the previous image frame and the RGB residual between the laser point corresponding to the image feature point matching pair and the laser pixel point projected on the current image frame.

12. The multi-sensor tightly coupled odometer method according to claim 1 or 10, characterized in that: The constraint relationship between the current image frame and the laser frame includes: the three-dimensional coordinate residual of the 3D point matching pair between the current image frame and the nearest laser frame, and the 3D point matching pair includes the triangulated point of the image feature point matching pair between the current image frame and the previous image frame and the laser point corresponding to the image feature point matching pair.

13. The multi-sensor tightly coupled odometer method according to claim 1, characterized in that: The updating of the system state variables using the pre-integration constraint relationship of the current laser frame, the inter-laser frame constraint relationship of the current laser frame, and the constraint relationship between the current laser frame and the image frame includes: Establishing a pre-integration constraint relationship between the current laser frame and a previous observation frame through the IMU frame associated with the current laser frame, wherein the previous observation frame is the previous image frame or the previous laser frame whose timestamp is closest to the current laser frame; Get the feature point matching relationship of the current laser frame; constructing an inter-laser frame constraint relationship of the current laser frame according to the feature point matching relationship of the current laser frame, and performing a third joint optimization using the pre-integration constraint relationship of the current laser frame and the inter-laser frame constraint relationship to update system state variables; A constraint relationship between the current laser frame and the nearest image frame is constructed based on the system state variables obtained by the third joint optimization update and the feature point matching relationship of the current laser frame. A fourth joint optimization is performed using the pre-integration constraint relationship of the current laser frame and the constraint relationship between it and the nearest image frame to update the system state variables again.

14. The multi-sensor tightly coupled odometer method according to claim 1 or 13, characterized in that: The inter-laser frame constraint relationship of the current laser frame includes: the point-to-surface distance residual of the surface formed by the surface feature points of the current laser frame and the matching laser points, and the point-to-line distance residual of the straight line formed by the line feature points of the current laser frame and the matching laser points.

15. The multi-sensor tightly coupled odometer method according to claim 1 or 13, characterized in that: The constraint relationship between the current laser frame and the image frame includes: a three-dimensional coordinate residual of a laser point matching pair between the current laser frame and the nearest image frame; The laser point matching pair includes a first laser point in a current laser frame and a laser point corresponding to a first image feature point in an image frame closest to the current laser frame; The laser pixel point obtained by projecting the first laser point onto the nearest image frame overlaps with the first image feature point of the nearest image frame.

16. A tightly coupled odometer device based on multiple sensors, characterized in that: include: An acquisition unit, configured to acquire image frames from a camera, laser frames from a lidar, and IMU frames from an IMU; a first state updating unit, configured to update system state variables using a pre-integration constraint relationship of a current image frame, an inter-image frame constraint relationship of the current image frame, and a constraint relationship between the current image frame and a laser frame, wherein the pre-integration constraint relationship of the current image frame is established by the IMU frame associated with the current image frame; a first map updating unit, configured to update the RGB value of the laser point in the local point cloud map based on the system state variable updated by the first state updating unit; an image feature point updating unit, configured to update the image feature points of the current image frame based on the system state variables updated by the first state updating unit; a second state updating unit, configured to update system state variables using a pre-integration constraint relationship of a current laser frame, an inter-laser frame constraint relationship of the current laser frame, and a constraint relationship between the current laser frame and an image frame, wherein the pre-integration constraint relationship of the current laser frame is established by the IMU frame associated with the current laser frame, and the constraint relationship between the current laser frame and the image frame is determined by a laser point of the current laser frame and an image feature point of a nearest image frame; The second map updating unit is configured to merge the laser point cloud data of the current laser frame into the global point cloud map according to the updated system state variables.

17. The multi-sensor tightly coupled odometer device according to claim 16, characterized in that: Also includes: An associating unit is used to associate the image frame and the laser frame with the IMU frame according to their respective timestamps.

18. The multi-sensor tightly coupled odometer device according to claim 16, characterized in that: The image feature point updating unit is specifically configured to obtain the image feature points of the current image frame in the following manner when the current image frame is the initial image frame: Projecting the laser point of the laser frame closest to the initial image frame into the initial image frame according to the external parameters between the camera and the laser radar to determine the laser pixel point of each laser point; Divide each laser pixel into different image blocks; Laser pixels are selected as image feature points of the initial image frame according to the quality of the laser pixels in each image block and the spatial position of the laser points corresponding to the laser pixels.

19. The multi-sensor tightly coupled odometer device according to claim 16, characterized in that: The image feature point updating unit is specifically configured to update the image feature points of the current image frame in the following manner when the current image frame is not the initial image frame: Projecting the laser point of the laser frame closest to the current image frame onto the initial image frame according to the external parameters between the camera and the laser radar to determine the laser pixel point of each laser point; Dividing each of the laser pixel points and the image feature points existing in the current image frame into different image blocks of the current image frame; According to the quality of the laser pixel point in each image block, the spatial position of the laser point corresponding to the laser pixel point, and the number and quality of existing image feature points, one of the laser pixel points or the existing image feature point is selected as the current image feature point of the image block.

20. The multi-sensor based tightly coupled odometer device according to claim 18 or 19, characterized in that: The quality of the laser pixel point is judged based on the pixel gradient or image entropy value of the image block centered on the laser pixel point.

21. The multi-sensor tightly coupled odometer device according to claim 16, characterized in that: The first map updating unit is specifically configured to update the RGB value of the laser point in the local point cloud map in the following manner: Projecting the laser points in the local point cloud map within the field of view of the current image frame onto the current image frame based on the external parameters between the camera and the laser radar to determine the pixel coordinates corresponding to the laser points in the local point cloud map within the field of view of the current image frame; The RGB value of the pixel coordinate corresponding to the laser point in the current image frame is added to the laser point.

22. The multi-sensor tightly coupled odometer device according to claim 18 or 19, characterized in that: The first map updating unit is specifically configured to update the RGB value of the laser point in the local point cloud map in the following manner when the current image frame is not the initial image frame: Projecting the laser points in the local point cloud map within the field of view of the current image frame onto the current image frame based on the external parameters between the camera and the laser radar to determine the pixel coordinates corresponding to the laser points in the local point cloud map within the field of view of the current image frame; The RGB value at the pixel coordinate corresponding to the laser point in the current image frame is fused with the RGB value already attached to the laser point, and the RGB value already attached to the laser point is updated to the fused RGB value.

23. The multi-sensor tightly coupled odometer device according to claim 16, characterized in that: The first state updating unit is specifically configured to update the system state variables in the following manner: Establishing a pre-integration constraint relationship between the current image frame and a previous observation frame through the IMU frame associated with the current image frame, where the previous observation frame is the previous image frame or the previous laser frame whose timestamp is closest to the current image frame; Obtain the image feature point matching pairs between the current image frame and the previous image frame; constructing an inter-image frame constraint relationship between the current image frame and the previous image frame based on the image feature point matching pairs, and performing a first joint optimization using the pre-integration constraint relationship and the inter-image frame constraint relationship between the current image frame and the previous observation frame to update the system state variables; A constraint relationship between the current image frame and the nearest laser frame is constructed based on the system state variables obtained from the first joint optimization update and the image feature point matching pairs. A second joint optimization is performed using the pre-integration constraint relationship between the current image frame and the previous observation frame and the constraint relationship between the current image frame and the nearest laser frame to update the system state variables again.

24. The multi-sensor tightly coupled odometer device according to claim 16 or 23, characterized in that: The inter-image frame constraint relationship of the current image frame includes: the reprojection residual of the image feature point matching pair between the current image frame and the previous image frame and the RGB residual between the laser point corresponding to the image feature point matching pair and the laser pixel point projected on the current image frame.

25. The multi-sensor based tightly coupled odometer device according to claim 16 or 23, characterized in that: The constraint relationship between the current image frame and the laser frame includes: the three-dimensional coordinate residual of the 3D point matching pair between the current image frame and the nearest laser frame, and the 3D point matching pair includes the triangulated point of the image feature point matching pair between the current image frame and the previous image frame and the laser point corresponding to the image feature point matching pair.

26. The multi-sensor tightly coupled odometer device according to claim 16, characterized in that: The second state updating unit is specifically configured to update the system state variables in the following manner: Establishing a pre-integration constraint relationship between the current laser frame and a previous observation frame through the IMU frame associated with the current laser frame, wherein the previous observation frame is the previous image frame or the previous laser frame whose timestamp is closest to the current laser frame; Get the feature point matching relationship of the current laser frame; constructing an inter-laser frame constraint relationship of the current laser frame according to the feature point matching relationship of the current laser frame, and performing a third joint optimization using the pre-integration constraint relationship of the current laser frame and the inter-laser frame constraint relationship to update system state variables; A constraint relationship between the current laser frame and the nearest image frame is constructed based on the system state variables obtained by the third joint optimization update and the feature point matching relationship of the current laser frame. A fourth joint optimization is performed using the pre-integration constraint relationship of the current laser frame and the constraint relationship between it and the nearest image frame to update the system state variables again.

27. The multi-sensor tightly coupled odometer device according to claim 16 or 26, characterized in that: The inter-laser frame constraint relationship of the current laser frame includes: the point-to-surface distance residual of the surface formed by the surface feature points of the current laser frame and the matching laser points, and the point-to-line distance residual of the straight line formed by the line feature points of the current laser frame and the matching laser points.

28. The multi-sensor tightly coupled odometer device according to claim 16 or 26, characterized in that: The constraint relationship between the current laser frame and the image frame includes: a three-dimensional coordinate residual of a laser point matching pair between the current laser frame and the nearest image frame; The laser point matching pair includes a first laser point in a current laser frame and a laser point corresponding to a first image feature point in an image frame closest to the current laser frame; The laser pixel point obtained by projecting the first laser point onto the nearest image frame overlaps with the first image feature point of the nearest image frame.

29. An electronic device, characterized in that: include: a memory storing execution instructions; as well as A processor, wherein the processor executes the execution instructions stored in the memory, so that the processor executes the multi-sensor based tightly coupled odometer method according to any one of claims 1 to 15.

30. A readable storage medium, characterized in that The readable storage medium stores execution instructions, which, when executed by a processor, are used to implement the multi-sensor based tightly coupled odometer method according to any one of claims 1 to 15.

Images