Tight coupling laser inertial vision fusion method based on merged probability voxel map

Abstract

The invention discloses a tight coupling laser inertial vision fusion method based on a merged probability voxel map, and relates to the technical field of vision fusion. Comprising the steps of performing display parametric modeling based on radar measurement noise, and constructing a probability voxel plane model; combining the planes which may have a coplanar relationship, and adding the laser radar point clouds in the determined effective voxel planes into a global map; projecting the global map into an image frame to obtain a tracking point, tracking by using an LK optical flow method, and meanwhile, further eliminating an abnormal point by using a random sampling consensus algorithm based on a dynamic Bayesian network; and based on image information acquired by the camera, optimizing and maintaining the global map by minimizing frame-to-frame pixel errors and frame-to-map color errors and updating the global map. According to the method, the performance of the laser inertial visual odometer is improved by combining a framework of efficient coupling radar, IMU and camera information of the probability voxel map.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of visual fusion, and in particular to a tight coupling laser-inertial visual fusion method based on a merged probability voxel map. BACKGROUND

[0002] With the rapid development of industrial automation technology, various intelligent devices are deeply integrated into social production and life, significantly improving social productivity. This trend is particularly prominent in the field of robots. From unmanned aerial vehicle autonomous navigation systems to automated guided vehicles (AGV), from mobile service robots to autonomous vehicles, various intelligent robot devices are gradually expanding into industrial manufacturing, social services, logistics transportation and other multiple scenarios. With the continuous improvement of computer computing power and computing resources, such devices urgently need to have the ability of autonomous environment perception and real-time positioning to cope with the challenges of dynamic and complex scenarios. In this context, the simultaneous localization and mapping (SLAM) technology has become a key supporting technology for intelligent robots to perceive the external environment. This technology enables intelligent robot devices to perceive the surrounding environment through sensors, enabling the device to achieve real-time positioning and environment modeling in an environment lacking prior information, providing a core spatial cognition basis for autonomous decision-making of intelligent devices.

[0003] SLAM technology can be described as unmanned equipment such as unmanned aerial vehicles, unmanned cars, robots, and unmanned aerial vehicles collecting data through sensors carried by the body in an unknown environment, positioning its own position and attitude through repeatedly observed map features (such as corners, columns, etc.) during movement, and incrementally building a map based on its own position information, thereby achieving the purpose of simultaneous localization and mapping. Sensor data, as the core data source of the SLAM system, provides multi-scale surrounding environment information for positioning and mapping algorithms. According to the type of sensors used to collect environmental information, the current mainstream SLAM systems are mainly divided into laser SLAM, visual SLAM, and multi-sensor fusion SLAM technology assisted by various sensors.

[0004] A classic SLAM system is usually composed of four core modules: front-end odometry, back-end optimization, loop detection module, and environment modeling module. The data transmitted by the sensor is processed by the front-end odometry to obtain preliminary pose information containing errors; the back-end obtains optimized pose information through filtering or nonlinear optimization methods; the loop detection mechanism identifies historical access areas through scene feature matching, constructs a feedback correction loop to suppress cumulative errors, and finally generates a three-dimensional environment map with global consistency based on the optimized motion trajectory and environment feature point cloud.

[0005] As the basic data form of laser radar measurement, the point cloud map has the advantage of simple implementation, but its core defect is that it is difficult to effectively represent the map uncertainty caused by measurement noise. The traditional ICP algorithm and its improved method usually regard the fitted plane as a deterministic geometric feature, ignoring the probabilistic distribution characteristics of the radar point cloud. Although the existing method attempts to introduce a noise compensation mechanism in the process of plane fitting and point cloud registration, its reliability is limited by the accuracy of parameter estimation.

[0006] Therefore, a tightly coupled laser-inertial-vision fusion method based on merged probability voxel map is proposed to solve the difficulties existing in the prior art, which is a problem that a person skilled in the art urgently needs to solve. SUMMARY

[0007] Therefore, the present application provides a tightly coupled laser-inertial-vision fusion method based on merged probability voxel map, which improves the performance of laser-inertial-vision odometry by merging probability voxel map to efficiently couple radar, IMU and camera information.

[0008] In order to achieve the above purpose, the present application adopts the following technical solutions:

[0009] A tightly coupled laser-inertial-vision fusion method based on merged probability voxel map, comprising the following steps:

[0010] S1. Model construction: based on the radar measurement noise, the noise of the radar measurement is displayed and parameterized modeling, a fixed size probability voxel plane model is constructed, and the pose is optimized by minimizing the distance residual of the radar point cloud to the plane;

[0011] S2. Plane merging: merging the planes that may have coplanar relationship, adding the laser radar point cloud in the effective voxel plane determined to the global map;

[0012] S3. Tracking and identification: projecting the global map into the image frame to obtain tracking points, using LK optical flow method for tracking, and using a dynamic Bayesian network based on a random sample consensus algorithm to further remove abnormal points;

[0013] S4. Optimization and update: based on the image information obtained by the camera, the pose is optimized by minimizing the pixel error from frame to frame and the color error from frame to map, and finally the global RGB map is maintained and updated.

[0014] Optionally, the specific content of S1 is: using IMU pre-integration to compensate the laser radar point cloud, obtaining the laser radar point cloud after removing motion distortion, then dividing the laser radar point cloud into a fixed size voxel grid, and modeling the laser radar point cloud to construct a fixed size probability voxel plane model, and finally optimizing the pose by minimizing the distance residual of the radar point cloud to the plane.

[0015] Optionally, the specific contents of using IMU pre-integration to perform motion compensation on the lidar point cloud are as follows:

[0016] The continuous pose transformation between two frames of radar scans is obtained through IMU pre-integration. The current frame point cloud is dedistorted by forward propagation, and the starting point cloud is corrected by backpropagation to achieve precise compensation, thereby obtaining the lidar point cloud after eliminating motion distortion.

[0017] Optionally, the specific contents of probabilistic modeling of the lidar point cloud are as follows:

[0018] In the construction of LiDAR point cloud maps, the accuracy of plane parameter estimation is affected by the noise of the point cloud data within the plane, that is, the uncertainty model of the points and the uncertainty model of the plane are constructed in sequence;

[0019] point W P i Uncertainty model:

[0020] The uncertainty of the radar point includes the range uncertainty and the azimuth uncertainty; then the radar point L P i Noise and covariance As shown in the following formula:

[0021]

[0022]

[0023] Among them, N(ω i )=[N1 N2] is the tangent plane ω i The orthonormal basis at represents the antisymmetric matrix, ω i Indicates the tangent plane where the radar point is located, represents the noise of the radar point along the tangent plane, d i Indicates the depth of the radar point, represents distance noise;

[0024] point W P i The uncertainty model is expressed as:

[0025]

[0026] in, and yes G R I and G p I Uncertainty in the tangent plane, (G R I , G p I ) represents the pose estimation result obtained by using IMU pre-integration to compensate the motion of the laser radar point cloud;

[0027] 3DoF plane uncertainty model:

[0028] The plane uncertainty model can be normalized along a certain principal axis direction, and formula (4) represents the normalized plane expression when the z-axis is used as the principal axis. At this time, n=[a,b,d] is used to represent the plane, that is, the 3DoF representation of the plane. T

[0029] When the z component of the plane normal approaches zero, the 3DoF representation of the plane is singular. By calculating the projection of the laser radar point cloud to each coordinate axis, that is, based on formula (4), formula (5) and formula (6) are obtained. The closed solution of n is obtained by using the least square method, as shown in formula (7), and the uncertainty model (8) of the plane is obtained.

[0030] ax+by+z+d=0 (4)

[0031]

[0032] where A * is the adjugate matrix of A, and A and e are expressed as:

[0033]

[0034] where denotes the partial derivative, T denotes the transpose matrix, x i denotes the coordinate position of the laser point cloud on the x-axis, y i denotes the coordinate position of the laser point cloud on the y-axis, and z i denotes the coordinate position of the laser point cloud on the z-axis.

[0035] Optionally, the specific content of S2 is that the de-distorted point cloud is first projected to the corresponding voxel space, and the possible coplanar relationship of the voxel planes in the voxel map is merged by using a hash table and a union-find set, and then the laser radar point cloud in the effective voxel plane is added to the global map.

[0036] The voxel map is constructed based on the discrete space division method of the voxel grid, and the specific content is that the original point cloud data is discretized into a fixed-size voxel grid structure, the spatial index is realized quickly by using a hash table, and the topological relationship between voxels is maintained by combining a union-find set data structure, so as to construct a dynamically updatable voxel map.

[0037] ​The specific content of merging voxel planes that may have a coplanar relationship is:

[0038] For the converged voxel plane exist Search around and Coplanar and converging voxel planes By introducing the Mahalanobis distance metric, as shown in formula (9), the similarity between planes is quantitatively analyzed. When the calculated result is lower than the value given by χ 2 When the distribution is below the critical threshold at the 95% confidence level, the geometric coplanarity condition is satisfied, and the voxels are and Merge to get For the merged voxels Use formula (10) and formula (11) to obtain the covariance of the corresponding plane and 3DoF representation When the plane of convergence and adjacent convergent planes When merging, the parent plane of the current plane Points to the parent plane of the neighboring plane Use pruning operations to make the height of the union-find structure ≤ 2 layers;

[0039]

[0040] Among them, || ||2 represents the two-norm.

[0041] Optionally, the specific content of determining a valid voxel plane is:

[0042] Through eigenvalue analysis, when Σ n The minimum eigenvalue of min <τ, when the threshold τ is set to 0.01, the voxel plane is determined to be valid.

[0043] Optionally, in S3, the global map is projected onto the image frame to obtain tracking points, and the LK optical flow method is used for tracking. At the same time, a random sampling consensus algorithm based on a dynamic Bayesian network is used to further eliminate abnormal points. The specific contents are as follows:

[0044] During iteration, a dynamic Bayesian network is used to update the inlier score of a single data point. Weighted sampling is achieved through the dynamically updated score during each iteration. At the same time, the termination condition of the algorithm is dynamically adjusted based on the inlier and inlier probability scores of the data point.

[0045] Optionally, in S4, based on the image information acquired by the camera, the global map is optimized by minimizing the frame-to-frame pixel error and the frame-to-map color error, thereby maintaining and updating the specific content of the global map as follows:

[0046] The specific content of the frame-to-frame pixel error is:

[0047] Suppose m map points M = {P1, …, Pm} are tracked in the last frame F k-1 m map points in the last frame F m The projection coordinates of the m map points in the last frame F k-1 are The pixel coordinates of the map points in the current image frame F k are obtained using the LK optical flow method

[0048] The re-projection error is defined as the Euclidean distance between the last frame projection point coordinates and the projection coordinates, and the ESIKF algorithm is used to minimize the re-projection error, for the s-th map point:

[0049]

[0050] wherein, represents the three-dimensional coordinates of the map point;

[0051] After the map point is projected into the camera coordinate system, its projection error is obtained through formula (14)

[0052]

[0053] wherein, ζ is a projection function, is a time correction factor, Δt k-1,k is the time interval between the last frame F k-1 and the current frame F k , and and represent the principal point and focal length of the camera, and the intrinsic parameters are also estimated and optimized in the process of minimizing the re-projection error;

[0054] The specific content of the frame-to-map color error is:

[0055] For the s-th map,

[0056] G P s ∈ M, the frame-to-map color error model is established by the following steps:

[0057] First, the map point is mapped to the current image plane through the projection model to obtain the corresponding pixel coordinates ρ;

[0058] Second, the RGB values of the sampling points in the pixel neighborhood are calculated using the linear interpolation algorithm, and the color information γ s of the projection point is estimated.

[0059] Finally, the color information c sColor error function between color information of projection point and color information of point s

[0060]

[0061] Via the technical solution, compared with the prior art, the application provides a tight coupling laser inertial vision fusion method based on a merged probability voxel map, which has the following beneficial effects:

[0062] (1) The application proposes a framework for efficiently coupling radar, IMU and camera information using a merged probability voxel map, aiming to improve the performance of laser inertial vision odometry. After removing motion distortion from radar point cloud using IMU pre-integration information, in order to consider the uncertainty of map points caused by noise of laser radar points, the uncertainty of laser radar is probabilistically modeled using voxel map to construct voxel plane, and 6DoF plane parameters are optimized to 3DoF plane parameters to improve system calculation efficiency. In addition, the use of union-find set and hash table can merge sub-planes with coplanar relationship, and experiments show that it effectively improves the matching speed of point to plane;

[0063] (2) The global map is projected into the image frame to directly obtain tracking points, ensuring the accuracy and stability of feature points. The method uses a dynamic Bayesian network-based random sample consensus algorithm to remove abnormal tracking points, which can effectively improve the speed and accuracy of feature matching. By minimizing the inter-frame reprojection error and frame-to-map RGB error, the system state is further accurately estimated, thereby optimizing visual tracking and motion estimation;

[0064] (3) The experimental results show that the tight coupling laser inertial vision odometry based on the merged probability voxel map has made effective progress in positioning accuracy and real-time performance, fully verifying the effectiveness of the framework. BRIEF DESCRIPTION OF DRAWINGS

[0065] In order to more clearly illustrate the technical solutions in the embodiments of the application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiment or prior art description. Obviously, the drawings in the following description are only embodiments of the application, and for those skilled in the art, other drawings can be obtained without creative labor on the basis of the provided drawings.

[0066] Figure 1 A tight coupling laser inertial vision fusion method based on a merged probability voxel map provided by the application is shown in the flowchart.

[0067] Figure 2 ​A probability voxel plane model provided by the present application, wherein 2a is a representation of a radar point in a voxel plane, and 2b is a radar point and plane uncertainty model;

[0068] Figure 3 A voxel plane merging diagram provided by the present application, wherein 3a is a simple case of plane merging, and 3b is a complex case of plane merging;

[0069] Figure 4 A minimum PnP re-projection error diagram provided by the present application;

[0070] Figure 5 An acquired RGB information diagram provided by the present application;

[0071] Figure 6 A running effect of an algorithm provided by the present application on a gate_01 sequence, wherein 6a is a point cloud diagram corresponding to the gate_01 sequence, and 6b is a representation of a voxel plane on the gate_01 sequence;

[0072] Figure 7 A trajectory estimation and real trajectory difference diagram of a method provided by the present application on the gate_01 sequence;

[0073] Figure 8 A gate_01 sequence trajectory comparison diagram provided by the present application;

[0074] Figure 9 A positioning error diagram on the gate_01 sequence provided by the present application;

[0075] Figure 10 A laser inertial odometry subsystem mapping effect diagram provided by the present application;

[0076] Figure 11 An RGB global map mapping effect diagram provided by the present application, wherein 11a is a hku_campus_seq_02 sequence mapping effect diagram, and 11b is an eee_01 sequence mapping effect diagram. DETAILED DESCRIPTION

[0077] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of the present application.

[0078] REFERENCE Figure 1As shown, the present invention discloses a tightly coupled laser inertial vision fusion method based on a combined probabilistic voxel map, comprising the following steps:

[0079] S1. Model Construction: Based on radar measurement noise, perform explicit parameterized modeling of radar measurement noise, construct a probabilistic voxel plane model of a fixed size, and optimize the pose by minimizing the distance residual from the radar point cloud to the plane.

[0080] S2. Plane merging: Merge planes that may be coplanar, and add the LiDAR point clouds that are determined to be in valid voxel planes to the global map.

[0081] S3. Tracking and Identification: Project the global map onto the image frame to obtain tracking points. Tracking is performed using the LK optical flow method. A random sampling consensus algorithm based on a dynamic Bayesian network is used to further eliminate outliers.

[0082] S4. Optimization and update: Based on the image information obtained by the camera, the pose is optimized by minimizing the frame-to-frame pixel error and the frame-to-map color error, and finally the global RGB map is maintained and updated.

[0083] Specifically, the laser inertial odometry first performs motion distortion correction on the input radar point cloud, and then establishes a state estimation model based on the error state iterative Kalman filter (ESIKF). The core of this model is to construct the point-surface residual equation: the point-surface residual is obtained by matching the dedistorted point cloud with the probabilistic voxel plane described by 3 degrees of freedom (3DoF) parameterization in the voxel map, and the system state vector is solved iteratively. In the voxel map maintenance phase, the dedistorted point cloud is first projected into the corresponding voxel space, and fast indexing is achieved through a hash table. The voxel planes with coplanar relationships are merged using a union-find data structure to maintain and update the voxel map. Finally, the radar point cloud that meets the plane constraint is added to the global map.

[0084] Further, such as Figure 2 The figure shows a schematic diagram of the probabilistic voxel plane model provided by the present invention, wherein 2a is the representation of the radar point in the voxel plane, and 2b is the uncertainty model of the radar point and plane. The specific content of S1 is: using IMU pre-integration to perform motion compensation on the lidar point cloud to obtain a lidar point cloud after eliminating motion distortion, then dividing the lidar point cloud into a fixed-size voxel grid, and performing probabilistic modeling on the lidar point cloud to construct a fixed-size probabilistic voxel plane model. Finally, the pose is optimized by minimizing the distance residual from the radar point cloud to the plane.

[0085] Furthermore, the specific content of using IMU pre-integration to perform motion compensation on the lidar point cloud is as follows:

[0086] The continuous pose transformation between two frames of radar scans is obtained by IMU pre-integration, the current frame of point cloud is de-warped by forward propagation, the starting time point cloud is corrected by backward propagation, accurate compensation is realized, and the laser radar point cloud after removing motion distortion is obtained.

[0087] Further, the specific content of the probabilistic modeling of the laser radar point cloud is:

[0088] The specific content of the probabilistic modeling of the laser radar point cloud is:

[0089] In the construction of the laser radar point cloud map, the accuracy of the plane parameter estimation is affected by the in-plane point cloud data noise, that is, the uncertainty model of the point and the uncertainty model of the plane are constructed in turn;

[0090] The uncertainty model of the point W P i

[0091] The uncertainty of the radar point includes distance uncertainty and direction uncertainty; then the noise L P i and the covariance of the radar point are as follows:

[0092]

[0093] Wherein, N(ω i )=[N1 N2] is the standard orthogonal basis at the tangent plane ω i , denotes the skew-symmetric matrix, ω i denotes the tangent plane where the radar point is located, denotes the noise of the radar point along the tangent plane, d i denotes the depth of the radar point, denotes the distance noise;

[0094] The uncertainty model of the point W P i is expressed as:

[0095]

[0096] Wherein, and are G R I and G p I in the uncertainty of the tangent plane, ( G R I , G p I ​) represents the pose estimation result obtained by using IMU pre-integration to compensate the motion of the lidar point cloud;

[0097] 3DoF plane uncertainty model:

[0098] The plane uncertainty model can be normalized along a certain principal axis direction, and formula (4) represents the normalized plane expression when the z axis is used as the principal axis, at this time, n=[a,b,d] T three parameters are used to represent the plane, that is, the 3DoF representation of the plane;

[0099] When the z component of the plane normal approaches zero, the 3DoF representation of the plane is singular, and by calculating the projection of the lidar point cloud to each coordinate axis, that is, based on formula (4), formula (5) and formula (6) are obtained, and the closed solution of n is obtained by using the least square method, as shown in formula (7), and the uncertainty model (8) of the plane is obtained;

[0100] ax+by+z+d=0 (4)

[0101]

[0102] Where A * is the adjugate matrix of A, and A and e are expressed as:

[0103]

[0104]

[0105] Where, represents the partial derivative, T represents the transpose matrix, x i represents the coordinate position of the lidar point cloud in the x axis, y i represents the coordinate position of the lidar point cloud in the y axis, and z i represents the coordinate position of the lidar point cloud in the z axis.

[0106] Further, as Figure 3 shown is a voxel plane merging schematic diagram provided by the present application, wherein 3a is a simple case of plane merging, and 3b is a complex case of plane merging; the specific content of S2 is that: first, the de-distorted point cloud is projected into the corresponding voxel space, and the hash table and the union-find set are used to merge the voxel planes in the voxel map that may have coplanar relationship, and then the lidar point cloud in the effective voxel plane determined is added to the global map;

[0107] A voxel map is constructed based on a discrete spatial partitioning method based on voxelized grids. Specifically, the original point cloud data is discretized into a fixed-size voxel grid structure. Fast spatial indexing is achieved through a hash table. The topological relationship between voxels is maintained by combining the merge-and-find data structure to construct a dynamically updateable voxel map.

[0108] The specific content of merging voxel planes that may have a coplanar relationship is:

[0109] For the converged voxel plane exist Search around and Coplanar and converging voxel planes By introducing the Mahalanobis distance metric, as shown in formula (9), the similarity between planes is quantitatively analyzed. When the calculated result is lower than the value given by χ 2 When the distribution is below the critical threshold at the 95% confidence level, the geometric coplanarity condition is satisfied, and the voxels are and Merge to get For the merged voxels Use formula (10) and formula (11) to obtain the covariance of the corresponding plane and 3DoF representation When the plane of convergence and adjacent convergent planes When merging, the parent plane of the current plane Points to the parent plane of the neighboring plane Use pruning operations to make the height of the union-find structure ≤ 2 layers;

[0110]

[0111] Among them, || ||2 represents the two-norm.

[0112] Furthermore, the specific content of determining a valid voxel plane is:

[0113] Through eigenvalue analysis, when Σ n The minimum eigenvalue of min <τ, when the threshold τ=0.01 is set, the voxel plane is determined to be valid.

[0114] Specifically, the LiDAR point cloud is divided into fixed-size voxels (0.5×0.5×0.5m), and a voxel map is constructed using a discrete space partitioning method based on a voxel grid. Specifically, the original point cloud data is discretized into a fixed-size voxel grid structure, and a hash table is used to implement fast spatial indexing. The topological relationship between voxels is maintained by combining and querying the data structure, thereby constructing a dynamically updateable voxel map. For the first frame of the LiDAR point cloud, the corresponding voxel unit (such as Figure 3 As shown in Figure 3a), for the filled voxel, its plane parameters n = [a, b, d] are calculated based on formulas (6) and (8): T and its uncertainty Σ n Through eigenvalue analysis, when Σ n The minimum eigenvalue of min <τ, when the threshold τ is set to 0.01, the voxel plane is considered valid and its geometric parameters will be retained for subsequent processing.

[0115] For the processing of subsequent radar frames, the newly acquired radar point cloud will be dynamically registered to the existing voxel map. Specifically, when it is detected that a voxel has been established at the corresponding position of the current point cloud in the voxel map, and the voxel has not converged (defined as the number of point clouds contained is less than 50), the system will include the point cloud in the target voxel and incrementally update the plane parameters and uncertainty covariance matrix Σ of the voxel. n If no voxel has been established at the corresponding position, a new voxel is initialized and the plane parameters are calculated. To save system memory, the upper limit of the capacity of a single voxel is set to 50 point cloud data. When the voxel capacity is saturated, the update of the plane parameters of the voxel is immediately terminated and it is marked as a converged voxel. When the uncertainty of the plane parameters converges when the number of points reaches 50, the voxel storage is cleared to release memory resources, thereby effectively balancing computational efficiency and storage requirements.

[0116] Specifically, ESIKF state estimation of laser inertial odometry;

[0117] The observation equation for obtaining the radar point-to-plane distance in the i-th point-to-plane matching is as follows:

[0118]

[0119] Where Ω is a plane The normalized normal vector [a,b,1] T Combined with the prior state estimate obtained from the IMU state propagation and prior covariance The prior information is combined with the point-to-plane distance matching observation to construct a maximum a posteriori (MAP) model as shown in equation (18). The model is composed of two parts of prior state term and observation term, and the solving process is realized based on the error state iterative Kalman filtering framework.

[0120]

[0121] wherein, represents the observation error of the point-to-plane distance, the Jacobian matrix and the noise are calculated as follows:

[0122]

[0123]

[0124] wherein, is the observation error covariance of the convergence plane, is the observation error covariance of the point.

[0125] Specifically, in the implementation process of the visual inertial odometer, firstly, the prior estimation of the system state is obtained based on IMU pre-integration, and then the global Figure Three point cloud is projected onto the current image plane to establish feature association. The LK optical flow algorithm is used to realize cross-frame tracking of feature points, and the Bayesian network-based random sample consensus algorithm is used for abnormal point detection and elimination, so as to effectively suppress the interference of tracking error on state estimation. Then, the ESIKF is used to optimize the re-projection error and color error from frame to frame to further estimate the system state.

[0126] Further, in S3, the global map is projected into the image frame to obtain tracking points, and the LK optical flow method is used for tracking, and the specific content of further eliminating abnormal points using the dynamic Bayesian network-based random sample consensus algorithm is as follows:

[0127] In the iteration, the dynamic Bayesian network is used to update the inlier score of a single data point, and in each iteration process, the weighted sampling is realized through the dynamically updated score, and the algorithm termination condition is dynamically adjusted based on the inlier and outlier probability scores of the data points.

[0128] Specifically, referring to algorithm 1 shown in the figure, the input data of the algorithm is Q={x1,…,x N}, and the output is a hypothesis model θ*, and the inlier and outlier classification C * of the data Q using the hypothesis model. In the kth iteration, the inlier probability of each point in the k-1th iteration process is used as the weight for weighted sampling to obtain a set S kIn the fourth to sixth lines, we obtain the hypothesis model θ k , the number of outliers O k And the interior point classification C k , and find the best hypothesis model θ* and inlier classification C that makes as many inliers as possible * , the minimum number of outliers O * In the seventh line, the inlier probability of each point in the kth iteration is calculated based on the Bayesian network model. In the eighth line, the number of outliers whose inlier probability is less than the specified threshold θ (θ = 0.01) is obtained. In the ninth line The stopping criterion is triggered when , which means that the number of inliers in the current model is large enough and the accuracy is high enough, and the best hypothesis model and inlier classification are obtained at the same time.

[0129] Algorithm 1: Overview of random sampling consensus algorithm based on dynamic Bayesian network

[0130]

[0131] Furthermore, S4 optimizes the global map based on the image information obtained by the camera by minimizing the frame-to-frame pixel error and the frame-to-map color error. The specific content of maintaining and updating the global map is as follows:

[0132] like Figure 4 As shown, the specific content of the frame-to-frame pixel error is:

[0133] Assume that in the previous frame F k-1 In the track, m map points M={P1,…,P m}, m map points in the previous frame F k-1 The projection coordinates in are Use LK optical flow method to obtain the map point in the current image frame F k The pixel coordinates in are

[0134] The reprojection error is defined as the Euclidean distance between the projection point coordinates of the previous frame and the projection coordinates. The ESIKF algorithm is used to minimize the reprojection error. For the s-th map point:

[0135]

[0136] in, Represents the three-dimensional coordinates of a map point;

[0137] After the map point is projected into the camera coordinate system, its projection error is obtained by formula (14):

[0138]

[0139] where ζ is a projection function, is a time correction factor, Δt k-1,k is the previous frame F k-1 and the current frame F k time interval, and denote the principal point and focal length of the camera, which are also estimated and optimized in the minimization of the reprojection error process;

[0140] As shown in FIG. 4, the specific content of the frame-to-map color error is as follows: Figure 5

[0141] For the s-th map,

[0142] G P s ∈M, the frame-to-map color error model is established by the following steps:

[0143] Firstly, the map point is mapped to the current image plane through the projection model to obtain the corresponding pixel coordinates p;

[0144] Secondly, the RGB values of the sampling points in the pixel neighborhood are calculated by using the linear interpolation algorithm, and then the color information g s of the projection point is estimated.

[0145] Finally, the color error function between the color information c s of the map point and the color information g s of the projection point is constructed.

[0146]

[0147] Specifically, the visual-inertial odometer ESIKF state estimation is performed.

[0148] Combining the prior information obtained in the IMU state propagation, the pixel error between frames, and the color error between frames and maps, a maximum a posteriori estimation (MAP) of the state vector x k is formed.

[0149]

[0150] wherein, and respectively represent the observation noise of the pixel error and the color error, and the Jacobian matrix and the noise are calculated as follows:

[0151]

[0152] wherein, is the observation error covariance of the point, is the pixel observation error covariance,​ is the color observation error covariance. When an image frame arrives, the ESIKF successively minimizes the pixel error and color error of the tracking points to update the system state.

[0153] Specifically, (1) Radar Sub-Map: In the radar map sub, the radar point cloud is segmented and placed into a fixed-size voxel grid. When the point cloud in the voxel can form a plane, the plane parameters of this point are stored for subsequent point-to-plane matching and voxel plane merging. Union-Find set and hash table are used to merge and manage voxels. At the same time, in order to prevent the point cloud stored in each voxel from being too much, it is set that each voxel can store a maximum of 50 point clouds. When the voxel is full, it stops updating the voxel and discards all points in the voxel, while the plane parameters are retained.

[0154] (2) Global Map: For the global map point The global map not only stores the position of the point in the global coordinate system, but also stores the RGB information of the map point. After the ESIKF updates the system state in the visual odometry subsystem, the color information projected from the current image frame is combined with the existing color information in the map point using the Bayesian update-based method. This process can effectively update the color information of each map point, thereby improving the accuracy of color estimation. Subsequently, based on the updated system state of ESIKF, the map point is projected again and the pixel error and color error in the current image frame are calculated. If the error of some tracking points exceeds the set threshold, they will be removed to ensure the accuracy of the system state and the consistency of the map. This processing method not only enhances the robustness of the system, but also reduces the influence of errors caused by false tracking points on the final result.

[0155] In one specific embodiment, the following is included:

[0156] In order to verify the superiority of the proposed tightly coupled laser-inertial-visual odometry algorithm based on probabilistic voxel map, the proposed method and some of the best algorithms at this stage (such as FAST-LIO2, LIO-SAM, Voxelmap++, LVI-SAM, FAST-LIVO, R3live) were compared on the public dataset M2DGR and NTU-VIRAL on a computer configured as 12-core core TM i5-10400F CPU@2.9GHZ,32GB RAM,RTX2080Ti andUbuntu20.04. The proposed method was compared with some of the best algorithms at this stage (such as FAST-LIO2, LIO-SAM, Voxelmap++, LVI-SAM, FAST-LIVO, R3live) on the public dataset M2DGR and NTU-VIRAL on a computer configured as 12-core

[0157]

[0158] where, is the pose estimate, is the ground truth pose. Since the proposed method does not contain loop closure module, to keep the experiment fair, the loop closure module of LIO-SAM and LVI-SAM are closed in the following experiments.

[0159] (I) Positioning performance evaluation in M2DGR dataset, as shown in Table 1 and Figure 6 Fig. 6a is a point cloud graph corresponding to the gate_01 sequence, and Fig. 6b is a representation of the voxel plane on the gate_01 sequence.

[0160] Table 1. Root mean square error of absolute trajectory error in M2DGR dataset

[0161]

[0162]

[0163] The M2DGR dataset realizes data acquisition of indoor and outdoor complex scenes by a trolley carrying a Velodyne VLP-32C laser radar, a Realsense d435i camera, a Handsfree A9, and other sensors, and provides true values by RTK. As shown in Table 1, the proposed method obtains the best effect on most sequences, and the root mean square error of the average absolute trajectory error reaches 0.478 m. Since FAST-LIVO has strict data synchronization requirements for the dataset, MDGR dataset cannot run on FAST-LIVO, so FAST-LIVO is not included in Table 1. In indoor scene sequences such as door_*, hall_*, room_*, etc., almost the best effect is obtained, because there are a large number of continuous planes in the indoor scene, which can provide strong geometric constraints for pose estimation. In laser radar point cloud processing, since the point cloud data is discretized and assigned to a fixed size voxel grid, the continuous large-scale plane based on the probability plane model construction process will be discretized into multiple small size voxel planes that satisfy the coplanar relationship. To improve the efficiency of point cloud registration, the present application merges the voxel planes that have coplanar relationship through a voxel plane merging strategy (as shown in Fig. 6b). The fused plane features can provide effective geometric constraints for the pose estimation of the SLAM system, and improve the matching calculation efficiency of the radar point cloud and the plane model. Figure 6

[0164] ​In outdoor scenarios, the relative sparsity of map points leads to a relatively small number of available planar features, and the proposed algorithm performs relatively poorly on the street_* sequences, especially for long sequences such as street_02 and street_04. It is found that the voxel plane merging operation causes the gradual aggravation of the error accumulation effect, which is the main reason for the decline in system accuracy. However, for non-long sequence scenes, the proposed method exhibits good adaptability, especially compared to Voxelmap++, the visual-inertial odometry subsystem of the proposed method further corrects the accumulated error of the laser-inertial odometry subsystem, further improving the accuracy and robustness of the system.

[0165] Figure 7 The difference between the trajectory estimation of the proposed method and the true trajectory of the gate_01 sequence is shown. Figure 8 and Figure 9 The trajectories and positioning errors calculated by different algorithms for the gate_01 sequence are shown. It can be seen that the proposed method is closest to the true trajectory, but from Figure 8 (b), it can be seen that the proposed method has a risk of losing the trajectory under severe rotation, indicating that its robustness still needs to be further improved.

[0166] (II) Positioning performance evaluation of NTU-VIRAL dataset

[0167] The NTU-VIRAL dataset is collected by a drone equipped with multiple sensors to collect environmental information, and a centimeter-precision laser tracking total station is used to obtain the ground truth. As shown in Table 2, the proposed method performs best in overall performance, with an average root mean square error of absolute trajectory error of 0.185 meters. Experimental results show that better performance is achieved in indoor scene sequences nya_*, which benefits from the large number of continuous planar structures in limited space that can provide good geometric constraints for pose estimation. However, the performance in outdoor scene sequences eee_* and sbs_* is relatively poor, which is mainly due to two reasons: on the one hand, the continuous planar features are not rich enough in open scenes to provide sufficient effective constraints for pose estimation; on the other hand, the camera in the NTU-VIRAL dataset collects grayscale images of the surrounding environment, which makes the frame-to-map color alignment module in the visual-inertial odometry subsystem unable to be fully utilized, resulting in low utilization efficiency of the system. Despite this, compared to R3LIVE, which also uses a color alignment module, the proposed method still achieves significant improvement in positioning accuracy.

[0168] Table 2 Root mean square error of absolute trajectory error in NTU-VIRAL dataset

[0169]

[0170] (3) Point cloud map visualization

[0171] The M2DGR system uses the Velodyne VLP-32C mechanical rotating laser radar to collect environmental point clouds. The device uses 32 laser beams to achieve 360° horizontal scanning, but when used alone, its mapping effect (such as Figure 6 6b) is difficult to intuitively present the rich details of the surrounding environment, nor can it fully demonstrate the mapping advantages of the algorithm in this chapter. On the other hand, the NTU-VIRAL dataset lacks RGB information, and it is impossible to fully evaluate the construction quality of the global RGB map. In order to meet the dual needs of high-density point cloud detail reconstruction and color map visualization, the hku_campus_seq_02 sequence in the R3LIVE dataset was selected to demonstrate the mapping capabilities of the present invention. This sequence is not only equipped with a Livox Avia lidar with high-density point cloud acquisition capabilities (which can capture tiny structural features), but also synchronously equipped with an RGB camera, providing ideal hardware conditions for point cloud map visualization.

[0172] Experiments show that the mapping effect of the laser inertial odometry subsystem of the present invention on the hku_campus_seq_02 sequence is as follows: Figure 10 As shown, the surrounding trees, building structures and other spatial features can be clearly presented. In addition, the ability to build RGB global maps, Figure 11 Figure 11a shows the performance of the algorithm on this sequence: details such as text signs and poster boards in the map still retain high-resolution point cloud features, verifying the effectiveness of color reconstruction. To expand the verification dimension, RGB point cloud mapping tests were also conducted on the NTU-VIRAL dataset eee_01 sequence, which only contains grayscale images. The results are shown below. Figure 11 As shown in Figure 11b, the algorithm can still generate a point cloud map with good geometric accuracy. Comprehensive experimental results show that the present invention can build a globally consistent map framework and achieve real-time 3D rendering of complex environments while ensuring real-time positioning accuracy.

[0173] (4) Run time analysis

[0174] The average running time of FAST-LIVO, R3LIVE and the method proposed in the present application on the NTU-VIRAL dataset is statistically analyzed, as shown in Table 3. The method proposed in the present application has achieved good results in real-time performance, with an average time consumption of 36.19 ms. Thanks to the construction of the 3DoF incremental voxel map and the merging of the voxel planes, as well as the efficient query using the hash table, the laser-inertial odometry subsystem of the method proposed in the present application achieves the best results in running time, with an average processing time of 20.39 ms per frame. In FAST-LIVO and R3LIVE, the laser-inertial odometry subsystem uses a map management structure based on incremental KD-Tree. In this structure, the time complexity of updating the voxel map is O(NlogN), where N is the number of point clouds, and this complexity mainly comes from the neighbor search of point clouds and the dynamic balancing operation of tree structure. The method proposed in the present application uses union-find set and hash table to manage point cloud map, avoiding complex neighbor search operation. While ensuring the positioning accuracy, this method reduces the time complexity to O(N), significantly improving the calculation speed of the laser-inertial odometry subsystem.

[0175] Table 3 Average time consumption (MS / frame) on NTU-VIRAL dataset

[0176] FAST-LIVO R3LIVE Our Laser Inertial Odometry (MS) 24.06 27.99 20.39 Visual Inertial Odometry (MS) 9.17 30.25 15.8 Total Time Spent (MS) 33.23 58.24 36.19

[0177] The image frame size processed in the visual-inertial odometry subsystem is 752x480. On the one hand, the method proposed uses the random sample consensus algorithm based on dynamic Bayesian network with faster calculation speed, and on the other hand, it only adds the radar point cloud in the plane voxel judged in the laser-inertial odometry subsystem to the global map, which makes the calculation speed of the visual-inertial odometry subsystem improved compared with R3LIVE. Since FAST-LIVO only performs frame-to-frame visual alignment in the laser-inertial odometry module, the method proposed not only has a frame-to-frame visual alignment module, but also needs to calculate the RGB error from frame to map and maintain the global map, so the calculation speed of the laser-inertial odometry subsystem of the method proposed is not as good as FAST-LIVO, and the memory usage is also higher than FAST-LIVO.

[0178] The various embodiments in the specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0179] The foregoing description of the disclosed embodiments enables a person skilled in the art to make or use the application. Modifications of these embodiments will occur to persons of skill in the art, and that the appended claims are intended to cover all such modifications that do not depart from the true spirit and scope of the application. Therefore, the application is not limited to the embodiments shown but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A tightly coupled laser inertial vision fusion method based on merging probabilistic voxel maps, characterized in that: The following steps are involved: S1. Model Construction: Based on radar measurement noise, perform explicit parameterized modeling of radar measurement noise, construct a probabilistic voxel plane model of a fixed size, and optimize the pose by minimizing the distance residual from the radar point cloud to the plane. S2. Plane merging: Merge planes that may be coplanar, and add the LiDAR point clouds that are determined to be in valid voxel planes to the global map. S3. Tracking and Identification: Project the global map onto the image frame to obtain tracking points. Tracking is performed using the LK optical flow method. A random sampling consensus algorithm based on a dynamic Bayesian network is used to further eliminate outliers. S4. Optimization and update: Based on the image information obtained by the camera, the pose is optimized by minimizing the frame-to-frame pixel error and the frame-to-map color error, and finally the global RGB map is maintained and updated.

2. The tightly coupled laser inertial vision fusion method based on merging probabilistic voxel maps according to claim 1, characterized in that: The specific content of S1 is: use IMU pre-integration to perform motion compensation on the lidar point cloud to obtain the lidar point cloud after eliminating motion distortion, then divide the lidar point cloud into a fixed-size voxel grid, and perform probabilistic modeling on the lidar point cloud to construct a fixed-size probabilistic voxel plane model. Finally, optimize the posture by minimizing the distance residual from the radar point cloud to the plane.

3. The tightly coupled laser inertial vision fusion method based on merging probabilistic voxel maps according to claim 2, characterized in that: The specific content of using IMU pre-integration to perform motion compensation on the lidar point cloud is: The continuous pose transformation between two frames of radar scans is obtained through IMU pre-integration. The current frame point cloud is dedistorted by forward propagation, and the starting point cloud is corrected by backpropagation to achieve precise compensation, thereby obtaining the lidar point cloud after eliminating motion distortion.

4. The tightly coupled laser inertial vision fusion method based on merging probabilistic voxel maps according to claim 2, characterized in that: The specific contents of probabilistic modeling of lidar point cloud are as follows: In the construction of LiDAR point cloud maps, the accuracy of plane parameter estimation is affected by the noise of the point cloud data within the plane, that is, the uncertainty model of the points and the uncertainty model of the plane are constructed in sequence; point W P i Uncertainty model: The uncertainty of the radar point includes the range uncertainty and the azimuth uncertainty; then the radar point L P i Noise and covariance As shown in the following formula: Among them, N(ω i )=[N1 N2] is the tangent plane ω i The orthonormal basis at represents the antisymmetric matrix, ω i Indicates the tangent plane where the radar point is located, represents the noise of the radar point along the tangent plane, d i Indicates the depth of the radar point, represents the distance noise; point W P i The uncertainty model is expressed as: in, and yes G R I and G p I Uncertainty in the tangent plane, ( G R I , G p I ) represents the pose estimation result obtained by motion compensation of the lidar point cloud using IMU pre-integration; Uncertainty model for 3DoF plane: The uncertainty model of the plane can be expressed in a normalized manner along a certain principal axis. For example, formula (4) represents the normalized plane representation with the z axis as the principal axis. In this case, n = [a, b, d] T Three parameters to represent the plane, i.e., the 3DoF representation of the plane; When the z component of the plane normal is close to zero, the 3DoF representation of the plane is singular. By calculating the projection of the lidar point cloud to each coordinate axis, that is, based on formula (4), formulas (5) and (6) are obtained. The closed solution of n is obtained using the least squares method, as shown in formula (7), and the uncertainty model of the plane is obtained (8); ax+by+z+d=0 (4) Among them, A * is the adjoint matrix of A, and A and e are expressed as: in, represents partial derivative, T represents transposed matrix, x i Indicates the coordinate position of the thunder point cloud on the x-axis, y i Indicates the coordinate position of the lightning point cloud on the y-axis, z i Indicates the coordinate position of the lightning point cloud on the z-axis.

5. The tightly coupled laser inertial vision fusion method based on merging probabilistic voxel maps according to claim 1, characterized in that: The specific content of S2 is as follows: first, the dedistorted point cloud is projected into the corresponding voxel space, and the voxel planes that may have coplanar relationships in the voxel map are merged using a hash table and union-find set. Then, the lidar point cloud that is determined to be in a valid voxel plane is added to the global map; A voxel map is constructed based on a discrete spatial partitioning method based on voxelized grids. Specifically, the original point cloud data is discretized into a fixed-size voxel grid structure. Fast spatial indexing is achieved through a hash table. The topological relationship between voxels is maintained by combining the merge-and-find data structure to construct a dynamically updateable voxel map. The specific content of merging voxel planes that may have a coplanar relationship is: For the converged voxel plane exist Search around and Coplanar and converging voxel planes By introducing the Mahalanobis distance metric, as shown in formula (9), the similarity between planes is quantitatively analyzed. When the calculated result is lower than the value given by χ 2 When the distribution is below the critical threshold at the 95% confidence level, the geometric coplanarity condition is satisfied, and the voxels are and Merge to get For the merged voxels Use formula (10) and formula (11) to obtain the covariance of the corresponding plane and 3DoF representation When the plane of convergence and adjacent convergent planes When merging, the parent plane of the current plane Points to the parent plane of the neighboring plane Use pruning operations to make the height of the union-find structure ≤ 2 layers; Among them, || ||2 represents the two-norm.

6. The tightly coupled laser inertial vision fusion method based on merging probabilistic voxel maps according to claim 5, characterized in that: The specific content of valid voxel plane is: Through eigenvalue analysis, when Σ n The minimum eigenvalue of min <τ, when the threshold τ=0.01 is set, the voxel plane is determined to be valid.

7. The tightly coupled laser inertial vision fusion method based on merging probabilistic voxel maps according to claim 1, characterized in that: In S3, the global map is projected onto the image frame to obtain tracking points. The LK optical flow method is used for tracking. At the same time, a random sampling consensus algorithm based on a dynamic Bayesian network is used to further eliminate outliers. The specific contents are as follows: During iteration, a dynamic Bayesian network is used to update the inlier score of a single data point. Weighted sampling is achieved through the dynamically updated score during each iteration. At the same time, the termination condition of the algorithm is dynamically adjusted based on the inlier and inlier probability scores of the data point.

8. The tightly coupled laser inertial vision fusion method based on merging probabilistic voxel maps according to claim 1, characterized in that: In S4, based on the image information obtained by the camera, the global map is optimized by minimizing the pixel error from frame to frame and the color error from frame to map. The specific contents of maintaining and updating the global map are as follows: The specific content of the frame-to-frame pixel error is: Assume that in the previous frame F k-1 In the track, m map points M={P1,…,P m }, m map points in the previous frame F k-1 The projection coordinates in are Use LK optical flow method to obtain the map point in the current image frame F k The pixel coordinates in are The reprojection error is defined as the Euclidean distance between the projection point coordinates of the previous frame and the projection coordinates. The ESIKF algorithm is used to minimize the reprojection error. For the s-th map point: in, Represents the three-dimensional coordinates of a map point; After the map point is projected into the camera coordinate system, its projection error is obtained by formula (14): Among them, ζ is a projection function, is the time correction factor, Δt k-1,k It is the previous frame F k -1 With the current frame F k time interval, and Represents the principal point and focal length of the camera, and the intrinsic parameters are also estimated and optimized in the process of minimizing the reprojection error; The specific content of the color error from frame to map is: For the sth map, G P s ∈M, the frame-to-map color error model is established through the following steps: First, the map point is mapped to the current image plane through the projection model to obtain the corresponding pixel coordinate ρ; Secondly, the linear interpolation algorithm is used to calculate the RGB value of the sampling point in the pixel neighborhood, and then the color information γ of the projection point is estimated. s ; Finally, construct the color information c of the map point s Color information of the projected point γ s Color error function between

Images