Feature layer fusion method and device based on multi-sensor data, equipment and medium

Abstract

The invention discloses a feature layer fusion method, device and equipment based on multi-sensor data and a medium, and relates to the field of navigation positioning and surveying and mapping, and the method comprises the steps: constructing data association among different sensors for plane features in an environment, and carrying out the extraction of a feature plane based on the assistance of a laser point cloud; correlation is constructed between the extracted feature plane and a historical feature plane, plane tracking is realized, and laser point cloud assists in extraction of the feature plane; constraint is constructed based on feature points in the laser point cloud and a feature plane in a visual system, and a laser, camera and inertial navigation fusion framework for feature layer fusion is realized. According to the application, by integrating information of multiple sensors, the robustness of the navigation system in a degraded environment is improved while high precision is maintained.

Description

Technical Field

[0001] The present application relates to the field of navigation positioning and surveying and mapping, and specifically to a feature layer fusion method, device, equipment and medium based on multi-sensor data. Background Art

[0002] For laser inertial odometers, although their pose estimation accuracy is high and they can obtain environmental point cloud maps, they are prone to failure in feature degradation scenarios. In contrast, although the pose estimation results of visual inertial odometers are relatively poor and visual sensors have certain requirements for environmental lighting conditions, they can use the rich texture information in the environment to provide effective pose estimation results in scenarios where laser inertial odometers are prone to degradation. Visual inertial odometers that take into account structured features can further use environmental information to improve the accuracy of odometers.

[0003] Considering that both laser inertial odometry and visual inertial odometry that takes into account structured information utilize structural features in the environment, how to fuse information from multiple sensors to improve robustness in degraded environments while maintaining high accuracy has become a problem that needs to be solved urgently. Summary of the invention

[0004] The present application provides a feature layer fusion method, device, equipment and medium based on multi-sensor data, which improves the robustness of the navigation system in a degraded environment while maintaining high accuracy by integrating multi-sensor information.

[0005] In a first aspect, an embodiment of the present application provides a feature layer fusion method based on multi-sensor data, and the feature layer fusion method based on multi-sensor data includes:

[0006] Build data associations between different sensors for plane features in the environment, and extract feature planes with the assistance of laser point clouds;

[0007] The extracted feature plane is associated with the historical feature plane to achieve plane tracking, and the feature plane is extracted with the aid of laser point cloud.

[0008] Based on the feature points in the laser point cloud and the feature planes in the vision system, constraints are constructed to realize the laser, camera, and inertial navigation fusion framework for feature layer fusion.

[0009] In combination with the first aspect, in one implementation,

[0010] The feature layer fusion method is implemented by using a Kalman filter method centered on the IMU, and includes a VIO subsystem and a LIO subsystem;

[0011] The LIO subsystem updates the state by constructing a point-to-feature plane distance constraint based on an error state iterative Kalman filtering method;

[0012] The VIO subsystem is developed based on the MSCKF framework and is used to extract and track feature points and feature planes. It estimates odometers by constructing reprojection and point-to-feature plane distance constraints, and introduces laser point cloud information in the feature plane extraction and state update process.

[0013] In combination with the first aspect, in one implementation, the extraction of the feature plane based on the assistance of the laser point cloud specifically includes:

[0014] Feature plane extraction is performed based on incremental feature point triangulation and two-dimensional Delaunay triangulation. The incremental feature point triangulation refers to incrementally constructing a linear system for feature point triangulation. When the linear system meets the requirements for solving the three-dimensional coordinates of the feature points, the position of the feature points in the global coordinate system is calculated, and the construction of the linear system lags behind the odometer backend by one frame.

[0015] For each observation, there is a transformation relationship between the feature points from the camera coordinate system to the global coordinate system:

[0016]

[0017] in, G P f Represents the coordinates of the feature point in the global coordinate system, Represents the coordinates of the feature point in the camera coordinate system, or the normalized vector of the feature point And the depth of the feature point in the camera coordinate system Represents the rotation matrix from the camera coordinate system to the global coordinate system, Represents the coordinates of the camera in the global coordinate system, G b f Represents the direction vector from the camera to the feature point in the global coordinate system;

[0018] Accumulating observations at different times gives a linear equation:

[0019]

[0020] in, G N i represents the orthogonal term, G N i = G P f ^, ^ represents the antisymmetric matrix of the vector;

[0021] Based on solving the linear equation, the three-dimensional coordinates of the feature point in the global coordinate system are obtained;

[0022] Select two groups of three grid points to form direction vectors G vec, and calculate the cross product of the two direction vectors to get the normal vector of the triangle mesh G norm;

[0023] For each vertex, obtain the normal vector of the triangle grid where it is located, and calculate the angle between the normal vectors. If the calculated angle is less than a given threshold, the current vertex is determined to be on a plane in space, and the current vertex is marked as a plane point. The average normal vector of the current vertex is calculated.

[0024] For each vertex marked as a plane point, the nearby plane points are obtained based on the association of the triangular mesh, and the plane parameters of the plane point set are calculated, and the distance from each plane point to the feature plane is detected.

[0025] In combination with the first aspect, in one implementation, the laser point cloud assists in extracting the feature plane, specifically including:

[0026] Project the point cloud of the global coordinate system of the previous frame to the current camera coordinate system, normalize the plane coordinates of each point in the point cloud under the camera coordinate system, and construct a 2D KD tree;

[0027] Calculate visual feature points based on feature points, search for the nearest feature points in the KD tree, restore 3D coordinates and fit plane parameters;

[0028] The coordinates of the feature points in the global coordinate system are calculated based on the restored plane parameters.

[0029] In combination with the first aspect, in one implementation, the constructing of constraints based on feature points in the laser point cloud and feature planes in the visual system specifically includes:

[0030] The camera provides the azimuth observation z of the three-dimensional point in the environment c , based on the measurement function h c Progress status x k renew:

[0031]

[0032] Among them, Λ represents the functional relationship of the feature point from the camera coordinate system to the pixel plane, Represents the rotation matrix from the IMU coordinate system to the global coordinate system, Represents the translation from the IMU coordinate system to the global coordinate system, n c represents the Gaussian white noise observed by the camera, G P represents the rigid body transformation relationship from the camera coordinate system to the IMU coordinate system, that is, the external parameters of the camera;

[0033] The Jacobian matrix of the IMU historical posture is obtained by linearization And the Jacobian matrix of the feature points

[0034]

[0035] Among them, x C Indicates the historical position state of IMU, p f Indicates the state of visual feature points;

[0036] EKF update is performed on the feature points of SLAM type and MSCKF type. The feature points of SLAM type are feature points whose tracking time exceeds the sliding window length, and the feature points of MSCKF type are feature points that fail to be tracked in the latest frame. For the feature points of MSCKF type, the Jacobian corresponding to the feature points in the linearization system is eliminated by projection into the left null space:

[0037]

[0038] Among them, N f is the Jacobian matrix of the feature points The left null space of , T represents the matrix transpose, which is obtained by QR decomposition of the Jacobian matrix;

[0039] For feature points and plane features, constraints are constructed by the distance from the point to the plane. For a plane feature G Π= G n G Feature points on d G P f ,exist:

[0040] z d =( G P f TG n- G d)+n d

[0041] Among them, n d represents the noise of the constraint, z d Represents the distance from the feature point to the plane, G n represents the normal vector of the plane feature in the global coordinate system, G d represents the distance from the origin of the global coordinate system to the plane;

[0042] The Jacobian matrix of feature points and plane features is obtained by linearization:

[0043]

[0044] in, represents the Jacobian of the function with respect to the characteristic point state, represents the Jacobian of the function with respect to the eigenstate of the plane;

[0045] Perform EKF update on SLAM-type and MSCKF-type plane features, where SLAM-type plane features are plane features associated with plane features that contain SLAM-type feature points, and MSCKF-type plane features are plane features that have been successfully tracked but have not yet been converted to SLAM-type plane features;

[0046] By combining plane features and feature points, a unified linear system is obtained:

[0047]

[0048] In combination with the first aspect, in one implementation, when implementing the laser, camera, and inertial navigation fusion framework of feature layer fusion, the fusion of the laser point cloud is specifically:

[0049] Obtain the correspondence between a pair of feature points and plane features. When there is a priori laser point cloud map, search for the nearest point in the point cloud map and fit the plane G Π fit = G n fit G d fit ;

[0050] When the fitting plane exists, establish the distance constraint from the feature point to the fitting plane, and the constraint from the nearest point to the plane feature:

[0051]

[0052] in, Represents the distance from the feature point to the laser fitting plane, G n fit represents the normal vector of the laser fitting plane, G d fit represents the distance from the origin of the global coordinate system to the laser fitting plane, represents the distance from the laser point to the characteristic plane used to fit the plane, G P i Indicates the coordinates of the laser point or feature point in the global coordinate system;

[0053] The characteristic point and plane characteristic Jacobian matrices are obtained by linearization:

[0054]

[0055] in, Represents the Jacobian of the feature point to fitting plane constraint function with respect to the feature point state, The Jacobian representing the distance constraint function from the laser point to the characteristic plane with respect to the characteristic plane state;

[0056] Whether to perform null space projection is determined according to the type of corresponding feature points and planes.

[0057] In combination with the first aspect, in one implementation, the feature layer fusion method further includes:

[0058] Laser point cloud distortion removal based on IMU assistance;

[0059] Compute the Kalman filter gains and compress the original linear system while applying noise whitening:

[0060]

[0061] Among them, Q represents the orthogonal matrix of the observation dimension size, R represents the upper triangular matrix, represents the IMU pose dimension, represents the observed distance from the point to the plane, represents the Jacobian of the point-to-plane distance constraint function with respect to the MU pose state, Indicates the IMU posture state, represents the compressed point-to-plane distance observation value, Represents the Jacobian of the compressed point-to-plane distance constraint function with respect to the IMU pose state, represents the compressed observation noise covariance;

[0062] Realize real-time update of LiDAR:

[0063]

[0064] Where K represents the Kalman filter gain matrix, P kk-1 represents the predicted IMU state covariance matrix, H represents the Jacobian of the compressed point-to-plane distance constraint function with respect to the IMU pose state, Indicates P kk-1 The transpose of .

[0065] In a second aspect, an embodiment of the present application provides a feature layer fusion device based on multi-sensor data, and the feature layer fusion device based on multi-sensor data includes:

[0066] The extraction module is used to build data associations between different sensors for the plane features in the environment and extract the feature planes based on the laser point cloud;

[0067] An association module is used to associate the extracted feature plane with the historical feature plane to achieve plane tracking and extract the feature plane with the assistance of laser point cloud;

[0068] The execution module is used to construct constraints based on the feature points in the laser point cloud and the feature planes in the visual system to realize the laser, camera, and inertial navigation fusion framework of feature layer fusion.

[0069] In a third aspect, an embodiment of the present application provides a feature layer fusion device based on multi-sensor data, wherein the feature layer fusion device based on multi-sensor data comprises a processor, a memory, and a feature layer fusion program based on multi-sensor data stored in the memory and executable by the processor, wherein when the feature layer fusion program based on multi-sensor data is executed by the processor, the steps of the feature layer fusion method based on multi-sensor data described above are implemented.

[0070] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, on which is stored a feature layer fusion program based on multi-sensor data. When the feature layer fusion program based on multi-sensor data is executed by a processor, the steps of the above-mentioned feature layer fusion method based on multi-sensor data are implemented.

[0071] The beneficial effects brought by the technical solution provided in the embodiments of the present application include:

[0072] Data associations between different sensors are constructed for the planar features in the environment, and feature planes are extracted with the assistance of laser point clouds. Then, associations are established between the extracted feature planes and historical feature planes to achieve plane tracking. Laser point clouds are used to extract feature planes, and then constraints are constructed based on feature points in the laser point clouds and feature planes in the visual system to achieve a laser, camera, and inertial navigation fusion framework with feature layer fusion. By integrating multi-sensor information, the robustness of the navigation system in degraded environments can be improved while maintaining high accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 This is a flow chart of the feature layer fusion method based on multi-sensor data in this application;

[0074] Figure 2 The flowchart of the mileage calculation method for feature layer fusion of LiDAR, vision and inertial navigation;

[0075] Figure 3 This is a schematic diagram of the functional modules of the feature layer fusion device based on multi-sensor data in this application;

[0076] Figure 4 This is a schematic diagram of the hardware structure of the feature layer fusion device based on multi-sensor data in this application. DETAILED DESCRIPTION

[0077] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0078] In order to make the objectives, technical solutions and advantages of the present application clearer, the implementation methods of the present application will be further described in detail below with reference to the accompanying drawings.

[0079] On the first aspect, the embodiments of the present application provide a feature layer fusion method based on multi-sensor data, which combines lidar, vision and inertial navigation to realize mileage calculation, and can fuse the information of multiple sensors to improve the robustness of the navigation system in a degraded environment while maintaining high accuracy.

[0080] In one embodiment, referring to Figure 1 , Figure 1 Schematic diagram of the process of the feature layer fusion method based on multi-sensor data in this application. Figure 1 As shown, the feature layer fusion method based on multi-sensor data includes:

[0081] S1: Build data associations between different sensors for the plane features in the environment, and extract feature planes with the assistance of laser point clouds;

[0082] It should be noted that the feature layer fusion method of the present application is implemented by a Kalman filter method centered on an IMU (Inertial Measurement Unit), and includes a VIO subsystem (visual inertial navigation system) and a LIO subsystem (lidar inertial navigation system). The VIO subsystem and the LIO subsystem jointly optimize the state of the IMU.

[0083] The LIO subsystem updates the state by constructing a point-to-feature plane distance constraint based on an error state iterative Kalman filtering method; at the same time, the LIO subsystem also takes into account the dynamic noise model of the point-to-plane distance constraint, and uses the higher-precision pose trajectory provided by VIO in the motion distortion removal stage.

[0084] The VIO subsystem is developed based on the MSCKF (Multi-State Constraint Kalman Filter) framework, which is used to extract and track feature points and feature planes, estimate odometer by constructing reprojection and point-to-feature plane distance constraints, and introduce laser point cloud information in the feature plane extraction and state update process.

[0085] Since the plane feature extraction and tracking process in the visual inertial odometry is easily affected by the noise of feature points, laser point cloud is used to assist in the extraction of feature planes. Specifically, the extraction of feature planes based on laser point cloud assistance includes:

[0086] S101: Extracting feature planes based on incremental feature point triangulation and two-dimensional Delaunay triangulation. The incremental feature point triangulation refers to incrementally constructing a linear system for feature point triangulation. When the linear system meets the requirements for solving the three-dimensional coordinates of the feature points, the positions of the feature points in the global coordinate system are calculated, and the construction of the linear system lags behind the odometer backend by one frame. Because the construction of the linear system requires the observation of feature points and the position and posture of the camera, the construction of the linear system lags behind the odometer backend by one frame.

[0087] S102: For each observation, there is a transformation relationship between the feature point from the camera coordinate system to the global coordinate system:

[0088]

[0089] in, G P f Represents the coordinates of the feature point in the global coordinate system, Represents the coordinates of the feature point in the camera coordinate system, or the normalized vector of the feature point And the depth of the feature point in the camera coordinate system Represents the rotation matrix from the camera coordinate system to the global coordinate system, Represents the coordinates of the camera in the global coordinate system, G b f Represents the direction vector from the camera to the feature point in the global coordinate system;

[0090] S103: Accumulate observations at different times to obtain a linear equation:

[0091]

[0092] in, G N i represents the orthogonal term, G N i = G P f ^, ∧ represents the antisymmetric matrix of the vector;

[0093] Since the depth is unknown, in order to eliminate the depth term, both sides of the above equation are multiplied by their orthogonal terms. G N i = G P f ^;

[0094] S104: based on solving the linear equation, the three-dimensional coordinates of the feature points in the global coordinate system are obtained; further, in order to ensure that the accuracy of the coordinates meets the requirements, unreliable coordinates can be screened out by judging the condition number of the coefficient matrix;

[0095] S105: Select two groups of three grid points to form direction vectors G vec, and calculate the cross product of the two direction vectors to get the normal vector of the triangle mesh G norm;

[0096] S106: For each vertex, obtain the normal vector of the triangle mesh where it is located, and calculate the angle between the normal vectors in pairs. If the calculated angle is less than a given threshold, it is determined that the current vertex is located on a plane in the space, the current vertex is marked as a plane point, and the average normal vector of the current vertex is calculated;

[0097] S107: For each vertex marked as a plane point, obtain the plane points near it based on the association of the triangular mesh, calculate the plane parameters of the plane point set, and detect the distance from each plane point to the feature plane.

[0098] For distance detection, specifically: G norm TG P i - G d i <Δd.

[0099] S2: Establish an association between the extracted feature plane and the historical feature plane to achieve plane tracking, and extract the feature plane with the assistance of laser point cloud;

[0100] For plane tracking, plane tracking refers to associating the extracted feature plane with the historical feature plane. The plane feature has a unique serial number as its identifier. Different from the one-to-one tracking method of feature points, the feature plane is judged by whether there is a common area, that is, the feature point. After the feature plane is extracted, the relationship between the current feature point and the feature plane can be obtained. The correspondence between all feature points and historical feature planes is counted. If there are points in the point set that have already matched the plane, the oldest plane is selected as the plane where the point set is located. If there are no points matching the plane, it is used as a new plane.

[0101] It should be noted that since the depth estimation error of visual triangulation increases quadratically with the depth, it is easy to lose many environmental feature planes when using visual estimated depth to extract planes. Therefore, referring to the idea of ​​using laser-assisted visual depth acquisition in the loosely coupled method, laser-assisted plane feature extraction is proposed. Specifically, the laser point cloud assists in extracting feature planes, which specifically includes:

[0102] S201: Project the point cloud of the global coordinate system of the previous frame to the current camera coordinate system, normalize the plane coordinates of each point in the point cloud in the camera coordinate system, and construct a 2D KD tree;

[0103] S202: Calculate visual feature points based on feature points, search for the nearest feature points in the KD tree, restore three-dimensional coordinates and fit plane parameters;

[0104] S203: Calculate the coordinates of the feature points in the global coordinate system based on the restored plane parameters.

[0105] Through the above steps, we test the hkust_campus_seq_00 data of R3LIVE, compare the performance of the two methods and observe the plane extraction effects of the two methods.

[0106] S3: A laser, camera, and inertial navigation fusion framework that builds constraints based on feature points in the laser point cloud and feature planes in the vision system to achieve feature layer fusion.

[0107] Furthermore, in one embodiment, constraints are constructed based on feature points in the laser point cloud and feature planes in the visual system, specifically including:

[0108] S301: Provide the orientation observation z of the three-dimensional point of the environment through the camera c , based on the measurement function h c Progress status x k renew:

[0109]

[0110] Among them, Λ represents the functional relationship from the camera coordinate system to the pixel plane, that is, the camera model, Represents the rotation matrix from the IMU coordinate system to the global coordinate system, Represents the translation from the IMU coordinate system to the global coordinate system, n c represents the Gaussian white noise observed by the camera, n c ~N(0,σ c 2 ), whose standard deviation σ c Usually 1 pixel, G P represents the rigid body transformation relationship from the camera coordinate system to the IMU coordinate system, that is, the camera's external parameters, which are set to fixed values;

[0111] S302: Obtain the Jacobian matrix of the IMU historical posture through linearization And the Jacobian matrix of the feature points

[0112]

[0113] Among them, x C Indicates the historical position state of IMU, p f Indicates the state of visual feature points;

[0114] S303: Perform EKF update on the SLAM type and MSCKF type feature points, wherein the SLAM type feature points are feature points whose tracking time exceeds the sliding window length, and the MSCKF type feature points are feature points that fail to be tracked in the latest frame, wherein, for the SLAM type feature points, the above formula can be directly used for EKF update; for the MSCKF type feature points, the Jacobian corresponding to the feature points in the linearization system is eliminated by projection into the left null space:

[0115]

[0116] Among them, N f is the Jacobian matrix of the feature points The left null space of , T represents the matrix transpose, which is obtained by QR decomposition of the Jacobian matrix;

[0117] S304: For feature points and plane features, constraints are constructed by the distance from the point to the plane. For a plane feature G Π= G n G Feature points on d G P f ,exist:

[0118] z d =( G P f TG n- G d)+n d

[0119] Among them, n d represents the noise of the constraint, z d Represents the distance from the feature point to the plane, G n represents the normal vector of the plane feature in the global coordinate system, G d represents the distance from the origin of the global coordinate system to the plane;

[0120] S305: Obtain the Jacobian matrix of feature points and plane features through linearization:

[0121]

[0122] in, represents the Jacobian of the function with respect to the characteristic point state, represents the Jacobian of the function with respect to the eigenstate of the plane;

[0123] S306: performing EKF (Extended Kalman Filter) update on SLAM (Simultaneous Localization and Mapping) type and MSCKF type plane features, wherein the SLAM type plane feature is a plane feature associated with a plane feature, and there are SLAM type feature points. This type of plane feature can add a long trajectory plane to the state quantity, provide reliable regularization information and constrain a large number of coplanar feature points. The MSCKF type plane feature is a plane feature that has been successfully tracked but has not yet been converted to a SLAM type.

[0124] S307: By combining the plane features and the feature points, a unified linear system is obtained:

[0125]

[0126] It should be noted that the above formula can be used directly for EKF update for SLAM type plane features and any type of feature points, but for MSCKF type plane features and feature points, since they are not located in the state quantity, it is necessary to eliminate the Jacobian corresponding to the linearized system features through the projection of the left null space. The problem of inconsistent diagonals of elements in the covariance matrix of the observed quantity due to different noise terms can be solved by noise whitening.

[0127] Furthermore, in one embodiment, when implementing the laser, camera, and inertial navigation fusion framework of feature layer fusion, the fusion of laser point cloud is specifically:

[0128] S311: Obtain the correspondence between a pair of feature points and plane features. When there is a priori laser point cloud map, search for the nearest point in the point cloud map and fit the plane. G Π fit = G n fit G d fit ;

[0129] S312: When the fitting plane exists, establish the distance constraint from the feature point to the fitting plane, and the constraint from the nearest point to the plane feature:

[0130]

[0131] in, Represents the distance from the feature point to the laser fitting plane, G n fit represents the normal vector of the laser fitting plane, G d fitrepresents the distance from the origin of the global coordinate system to the laser fitting plane, represents the distance from the laser point to the characteristic plane used to fit the plane, G P i Indicates the coordinates of the laser point or feature point in the global coordinate system;

[0132] S313: Obtain the Jacobian matrix of feature points and plane features through linearization:

[0133]

[0134] in, Represents the Jacobian of the feature point to fitting plane constraint function with respect to the feature point state, The Jacobian representing the distance constraint function from the laser point to the characteristic plane with respect to the characteristic plane state;

[0135] S314: Determine whether to perform null space projection according to the type of the corresponding feature point and the plane.

[0136] Furthermore, in one embodiment, the feature layer fusion method further includes:

[0137] S321: Laser point cloud distortion removal based on IMU assistance;

[0138] When using IMU to assist in dedistortion of laser point cloud, it is usually assumed that the state obtained by IMU recursion has high accuracy in a short period of time, but the position recursively obtained by IMU changes with time. However, since the residual motion distortion in the laser point cloud will not be removed again when the state is subsequently updated using the point cloud with motion distortion removed, some motion distortion will still be retained in the laser point cloud map. In order to further eliminate motion distortion, in a system that adds visual observation, the IMU state updated using visual observation can be used to dedistort the laser point cloud, because the observation frequency of camera data is usually greater than that of IMU but less than that of lidar.

[0139] S322: Calculate the Kalman filter gain and compress the original linear system while using noise whitening:

[0140]

[0141] Among them, Q represents the orthogonal matrix of the observation dimension size, R represents the upper triangular matrix, represents the IMU pose dimension, represents the observed distance from the point to the plane, Represents the Jacobian of the point-to-plane distance constraint function with respect to the IMU pose state, Indicates the IMU posture state, represents the compressed point-to-plane distance observation value, Represents the Jacobian of the compressed point-to-plane distance constraint function with respect to the IMU pose state, represents the compressed observation noise covariance;

[0142] Noise whitening can reduce the time consumption to matrix inversion of twice the state dimension. However, in the MSCKF framework, the dimension of the state quantity can reach hundreds of dimensions. At this time, using error iterative Kalman filtering for updating will reduce the real-time performance of the system. In order to solve this problem, it is necessary to calculate the Kalman filter gain and compress the original linear system while using noise whitening.

[0143] S323: Realize real-time update of LiDAR:

[0144]

[0145] Where K represents the Kalman filter gain matrix, P kk-1 represents the predicted IMU state covariance matrix, H represents the Jacobian of the compressed point-to-plane distance constraint function with respect to the IMU pose state, Indicates P kk-1 The transpose of .

[0146] See also Figure 2 The figure shows a flow chart of the mileage calculation method of feature layer fusion of LiDAR, vision and inertial navigation. Specifically, it includes data association, auxiliary plane extraction, constraint construction, and camera-assisted segmented motion distortion removal. First, data association is established between different sensors for plane features in the environment. Secondly, considering that plane feature extraction and tracking in visual inertial odometers are susceptible to noise, laser point cloud is used to assist plane extraction. Finally, in the filter state update, the laser point cloud is combined with the feature points and feature planes of the visual system to construct constraints, thus realizing a tightly coupled laser / camera / inertial navigation fusion framework for feature layer fusion. This application aims to integrate multi-sensor information to improve the robustness of the navigation system in degraded environments while maintaining high accuracy.

[0147] This application first introduces the difference between the feature layer fusion scheme and other schemes at the data association level. The feature layer fusion scheme uses laser depth for assistance at the measurement data level and introduces constraints on similar feature states at the state estimation level; secondly, the F-LIVO method is proposed, and the system framework, the multi-sensor enhancement method in the visual subsystem and radar subsystem, and the implementation of the feature layer fusion scheme are introduced; finally, the positioning accuracy of R2LIVE, R3LIVE and F-LIVO are tested using open source data sets, proving that the present application has the same positioning accuracy as the measurement layer fusion algorithm in general scenarios, and its accuracy performance in degraded scenarios is slightly worse than that of the measurement layer fusion method, but it has higher robustness.

[0148] In the positioning and mapping process of this application, the use of planar features in the environment can effectively improve accuracy and robustness. Different sensors (such as lidar, visual sensors and inertial measurement units) have their own advantages and limitations in detecting and describing environmental features. By building data associations between different sensors, the advantages of each sensor can be comprehensively utilized to achieve more accurate positioning and mapping.

[0149] The feature layer fusion method based on multi-sensor data in the embodiment of the present application builds data associations between different sensors for planar features in the environment, extracts feature planes with the assistance of laser point clouds, and then builds associations between the extracted feature planes and historical feature planes to achieve plane tracking. The feature planes are extracted with the assistance of laser point clouds, and then constraints are built based on feature points in the laser point clouds and feature planes in the visual system to achieve a laser, camera, and inertial navigation fusion framework for feature layer fusion. By integrating multi-sensor information, the robustness of the navigation system in degraded environments is improved while maintaining high accuracy.

[0150] In a second aspect, an embodiment of the present application also provides a feature layer fusion device based on multi-sensor data.

[0151] In one embodiment, referring to Figure 3 , Figure 3 This is a functional module diagram of the feature layer fusion device based on multi-sensor data in this application. Figure 3 As shown, the feature layer fusion device based on multi-sensor data includes: an extraction module, an association module, and an execution module.

[0152] The extraction module is used to build data associations between different sensors for planar features in the environment, and to extract feature planes with the assistance of laser point clouds; the association module is used to build associations between the extracted feature planes and historical feature planes to achieve plane tracking and extract feature planes with the assistance of laser point clouds; the execution module is used to build constraints based on feature points in the laser point cloud and feature planes in the visual system to realize a laser, camera, and inertial navigation fusion framework for feature layer fusion.

[0153] In a third aspect, an embodiment of the present application provides a feature layer fusion device based on multi-sensor data. The feature layer fusion device based on multi-sensor data can be a personal computer (PC), a laptop computer, a server, or other device with data processing capabilities.

[0154] Reference Figure 4 , Figure 4The hardware structure diagram of the feature layer fusion device based on multi-sensor data involved in the embodiment of the present application is shown in FIG. In the embodiment of the present application, the feature layer fusion device based on multi-sensor data may include a processor, a memory, a communication interface and a communication bus.

[0155] The communication bus may be of any type and is used to interconnect the processor, the memory, and the communication interface.

[0156] The communication interface includes input / output (I / O) interface, physical interface and logical interface, etc., which are used to realize the interconnection of devices inside the feature layer fusion device based on multi-sensor data, and the interface used to realize the interconnection between the feature layer fusion device based on multi-sensor data and other devices (such as other computing devices or user devices). The physical interface can be an Ethernet interface, a fiber optic interface, an ATM interface, etc.; the user device can be a display, a keyboard, etc.

[0157] The memory can be various types of storage media, such as random access memory (RAM), read-only memory (ROM), non-volatile RAM (NVRAM), flash memory, optical storage, hard disk, programmable ROM (PROM), erasable PROM (EPROM), electrically erasable PROM (EEPROM), etc.

[0158] The processor may be a general-purpose processor, which may call a feature layer fusion program based on multi-sensor data stored in a memory and execute a feature layer fusion method based on multi-sensor data provided in an embodiment of the present application. For example, the general-purpose processor may be a central processing unit (CPU). The method executed when the feature layer fusion program based on multi-sensor data is called may refer to the various embodiments of the feature layer fusion method based on multi-sensor data of the present application, which will not be described in detail here.

[0159] Those skilled in the art will understand that Figure 4 The hardware structure shown in the figure does not constitute a limitation on the present application, and may include more or less components than shown in the figure, or combine certain components, or arrange the components differently.

[0160] In a fourth aspect, an embodiment of the present application also provides a computer-readable storage medium.

[0161] The computer-readable storage medium of the present application stores a feature layer fusion program based on multi-sensor data, wherein when the feature layer fusion program based on multi-sensor data is executed by a processor, the steps of the feature layer fusion method based on multi-sensor data as described above are implemented.

[0162] Among them, the method implemented when the feature layer fusion program based on multi-sensor data is executed can refer to the various embodiments of the feature layer fusion method based on multi-sensor data in this application, and will not be repeated here.

[0163] The terms "including" and "having" and any variations thereof in the specification and claims of this application and the above-mentioned drawings are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but optionally includes steps or units that are not listed, or optionally includes other steps or units inherent to these processes, methods, products or devices. The terms "first", "second" and "third" are used to distinguish different objects, etc., and do not represent a sequence, nor do they limit "first", "second" and "third" to different types.

[0164] In the description of the embodiments of the present application, "exemplary", "for example" or "for example" are used to indicate examples, illustrations or descriptions. Any embodiment or design described as "exemplary", "for example" or "for example" in the embodiments of the present application should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of words such as "exemplary", "for example" or "for example" is intended to present related concepts in a specific way.

[0165] In the description of the embodiments of the present application, unless otherwise specified, “ / ” means or, for example, A / B can mean A or B; the “and / or” in the text is merely a description of the association relationship of associated objects, indicating that three relationships may exist, for example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone. In addition, in the description of the embodiments of the present application, “multiple” refers to two or more than two.

[0166] In some processes described in the embodiments of the present application, multiple operations or steps that appear in a specific order are included, but it should be understood that these operations or steps may not be executed in the order in which they appear in the embodiments of the present application or in parallel, and the sequence number of the operation is only used to distinguish the different operations, and the sequence number itself does not represent any execution order. In addition, these processes may include more or fewer operations, and these operations or steps may be executed in sequence or in parallel, and these operations or steps may be combined.

[0167] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present application is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, disk, CD) as described above, and includes a number of instructions for a terminal device to execute the methods described in each embodiment of the present application.

[0168] The above are only preferred embodiments of the present application, and are not intended to limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A feature layer fusion method based on multi-sensor data, characterized in that: The feature layer fusion method based on multi-sensor data includes: Build data associations between different sensors for plane features in the environment, and extract feature planes with the assistance of laser point clouds; The extracted feature plane is associated with the historical feature plane to achieve plane tracking, and the feature plane is extracted with the aid of laser point cloud. Based on the feature points in the laser point cloud and the feature planes in the vision system, constraints are constructed to realize the laser, camera, and inertial navigation fusion framework for feature layer fusion.

2. A feature layer fusion method based on multi-sensor data as claimed in claim 1, characterized in that: The feature layer fusion method is implemented by using a Kalman filter method centered on the IMU, and includes a VIO subsystem and a LIO subsystem; The LIO subsystem updates the state by constructing a point-to-feature plane distance constraint based on an error state iterative Kalman filtering method; The VIO subsystem is developed based on the MSCKF framework and is used to extract and track feature points and feature planes. It estimates odometers by constructing reprojection and point-to-feature plane distance constraints, and introduces laser point cloud information in the feature plane extraction and state update process.

3. A feature layer fusion method based on multi-sensor data as claimed in claim 2, characterized in that: The extraction of feature planes based on laser point cloud assistance specifically includes: Feature plane extraction is performed based on incremental feature point triangulation and two-dimensional Delaunay triangulation. The incremental feature point triangulation refers to incrementally constructing a linear system for feature point triangulation. When the linear system meets the requirements for solving the three-dimensional coordinates of the feature points, the position of the feature points in the global coordinate system is calculated, and the construction of the linear system lags behind the odometer backend by one frame. For each observation, there is a transformation relationship between the feature points from the camera coordinate system to the global coordinate system: in, G P f Represents the coordinates of the feature point in the global coordinate system, Represents the coordinates of the feature point in the camera coordinate system, or the normalized vector of the feature point And the depth of the feature point in the camera coordinate system Represents the rotation matrix from the camera coordinate system to the global coordinate system, Represents the coordinates of the camera in the global coordinate system, G b f Represents the direction vector from the camera to the feature point in the global coordinate system; Accumulating observations at different times gives a linear equation: in, G N i represents the orthogonal term, G N i = G P f ^, ^ represents the antisymmetric matrix of the vector; Based on solving the linear equation, the three-dimensional coordinates of the feature point in the global coordinate system are obtained; Select two groups of three grid points to form direction vectors G vec, and calculate the cross product of the two direction vectors to get the normal vector of the triangle mesh G norm; For each vertex, obtain the normal vector of the triangle grid where it is located, and calculate the angle between the normal vectors. If the calculated angle is less than a given threshold, the current vertex is determined to be on a plane in space, and the current vertex is marked as a plane point. The average normal vector of the current vertex is calculated. For each vertex marked as a plane point, the nearby plane points are obtained based on the association of the triangular mesh, and the plane parameters of the plane point set are calculated, and the distance from each plane point to the feature plane is detected.

4. A feature layer fusion method based on multi-sensor data as claimed in claim 3, characterized in that: The laser point cloud assists in extracting the feature plane, specifically including: Project the point cloud of the global coordinate system of the previous frame to the current camera coordinate system, normalize the plane coordinates of each point in the point cloud under the camera coordinate system, and construct a 2D KD tree; Calculate visual feature points based on feature points, search for the nearest feature points in the KD tree, restore 3D coordinates and fit plane parameters; The coordinates of the feature points in the global coordinate system are calculated based on the restored plane parameters.

5. A feature layer fusion method based on multi-sensor data as claimed in claim 4, characterized in that: The construction of constraints based on the feature points in the laser point cloud and the feature planes in the visual system specifically includes: The camera provides the azimuth observation z of the three-dimensional point in the environment c , based on the measurement function h c Progress status x k renew: Among them, Λ represents the functional relationship of the feature point from the camera coordinate system to the pixel plane, Represents the rotation matrix from the IMU coordinate system to the global coordinate system, Represents the translation from the IMU coordinate system to the global coordinate system, n c represents the Gaussian white noise observed by the camera, G P represents the rigid body transformation relationship from the camera coordinate system to the IMU coordinate system, that is, the external parameters of the camera; The Jacobian matrix of the IMU historical posture is obtained by linearization And the Jacobian matrix of the feature points Among them, x C Indicates the historical position state of IMU, p f Indicates the state of visual feature points; EKF update is performed on the feature points of SLAM type and MSCKF type. The feature points of SLAM type are feature points whose tracking time exceeds the sliding window length, and the feature points of MSCKF type are feature points that fail to be tracked in the latest frame. For the feature points of MSCKF type, the Jacobian corresponding to the feature points in the linearization system is eliminated by projection into the left null space: Among them, N f is the Jacobian matrix of the feature points The left null space of , T represents the matrix transpose, which is obtained by QR decomposition of the Jacobian matrix; For feature points and plane features, constraints are constructed by the distance from the point to the plane. For a plane feature G Π= G n G Feature points on d G P f ,exist: Among them, n d represents the noise of the constraint, z d Represents the distance from the feature point to the plane, G n represents the normal vector of the plane feature in the global coordinate system, G d represents the distance from the origin of the global coordinate system to the plane; The Jacobian matrix of feature points and plane features is obtained by linearization: in, represents the Jacobian of the function with respect to the characteristic point state, represents the Jacobian of the function with respect to the eigenstate of the plane; Perform EKF update on SLAM-type and MSCKF-type plane features, where SLAM-type plane features are plane features associated with plane features that contain SLAM-type feature points, and MSCKF-type plane features are plane features that have been successfully tracked but have not yet been converted to SLAM-type plane features; By combining plane features and feature points, a unified linear system is obtained:

6. A feature layer fusion method based on multi-sensor data as claimed in claim 5, characterized in that: When implementing the laser, camera, and inertial navigation fusion framework of feature layer fusion, for the fusion of laser point cloud, specifically: Obtain the correspondence between a pair of feature points and plane features. When there is a priori laser point cloud map, search for the nearest point in the point cloud map and fit the plane G Π fit = G n fit G d fit ; When the fitting plane exists, establish the distance constraint from the feature point to the fitting plane, and the constraint from the nearest point to the plane feature: in, Represents the distance from the feature point to the laser fitting plane, G n fit represents the normal vector of the laser fitting plane, G d fit represents the distance from the origin of the global coordinate system to the laser fitting plane, represents the distance from the laser point to the characteristic plane used to fit the plane, G P i Indicates the coordinates of the laser point or feature point in the global coordinate system; The characteristic point and plane characteristic Jacobian matrices are obtained by linearization: in, Represents the Jacobian of the feature point to fitting plane constraint function with respect to the feature point state, The Jacobian representing the distance constraint function from the laser point to the characteristic plane with respect to the characteristic plane state; Whether to perform null space projection is determined according to the type of corresponding feature points and planes.

7. A feature layer fusion method based on multi-sensor data as claimed in claim 6, characterized in that: The feature layer fusion method further includes: Laser point cloud distortion removal based on IMU assistance; Compute the Kalman filter gains and compress the original linear system while applying noise whitening: Among them, Q represents the orthogonal matrix of the observation dimension size, R represents the upper triangular matrix, represents the IMU pose dimension, represents the observed distance from the point to the plane, Represents the Jacobian of the point-to-plane distance constraint function with respect to the IMU pose state, Indicates the IMU posture state, represents the compressed point-to-plane distance observation value, Represents the Jacobian of the compressed point-to-plane distance constraint function with respect to the IMU pose state, represents the compressed observation noise covariance; Realize real-time update of LiDAR: Where K represents the Kalman filter gain matrix, P k|k-1 represents the predicted IMU state covariance matrix, H represents the Jacobian of the compressed point-to-plane distance constraint function with respect to the IMU pose state, Indicates P k|k-1 The transpose of .

8. A feature layer fusion device based on multi-sensor data, characterized in that: The feature layer fusion device based on multi-sensor data includes: The extraction module is used to build data associations between different sensors for the plane features in the environment and extract the feature planes based on the laser point cloud; An association module is used to associate the extracted feature plane with the historical feature plane to achieve plane tracking and extract the feature plane with the assistance of laser point cloud; The execution module is used to construct constraints based on the feature points in the laser point cloud and the feature planes in the visual system to realize the laser, camera, and inertial navigation fusion framework of feature layer fusion.

9. A feature layer fusion device based on multi-sensor data, characterized in that: The feature layer fusion device based on multi-sensor data includes a processor, a memory, and a feature layer fusion program based on multi-sensor data stored in the memory and executable by the processor, wherein when the feature layer fusion program based on multi-sensor data is executed by the processor, the steps of the feature layer fusion method based on multi-sensor data as described in any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a feature layer fusion program based on multi-sensor data, wherein when the feature layer fusion program based on multi-sensor data is executed by a processor, the steps of the feature layer fusion method based on multi-sensor data according to any one of claims 1 to 7 are implemented.

Images