Visual fusion positioning method based on prior 3D lidar point cloud map

Abstract

The present invention relates to multi-sensor fusion positioning technology for unmanned aerial vehicles, and proposes a visual fusion positioning algorithm based on a priori three-dimensional laser radar point cloud map to realize multi-sensor fusion positioning of a quad-rotor unmanned aerial vehicle in a GPS-denied environment. To this end, the technical solution adopted by the present invention is to reconstruct sparse three-dimensional feature points from image features based on the visual fusion positioning method based on a priori three-dimensional laser radar point cloud map, and then continuously match these points with laser feature points in the three-dimensional laser radar point cloud map to obtain the precise position information of the quad-rotor unmanned aerial vehicle in real time. The present invention is mainly used in the multi-sensor fusion positioning of unmanned aerial vehicles.

Description

Technical Field

[0001] The present invention relates to a multi-sensor fusion positioning technology, and in particular to a landing control method for three-dimensional laser radar and monocular vision in a GPS-denied environment. Background Art

[0002] The method proposed in this paper is related to the methods developed in computer vision and robotics in recent years. The vision-based methods rely on passive cameras, while the range information measurement-based methods rely on LiDAR sensors. The SLAM problem of these two alone has been a hot topic for many scholars. However, for algorithms that use both sensors for localization and mapping, the integration is mostly done in the backend of the algorithm, rather than directly matching the point clouds of the two.

[0003] LiDAR usually estimates pose by performing scan matching based on the ICP algorithm, and uses a feature matching-based method to detect closed loops (Conference: 2011IEEE / RSJ International Conference on Intelligent Robots and Systems; Author: Steder, B and Ruhnke, M and Grzonka, S, etc.; Publication Year: 2011; Article Title: Place recognition in 3D scans using a combination of bag of words and point feature based relative pose estimation; Pages: 1249-1255). Currently, mainstream LiDAR SLAM algorithms usually rely on graph optimization techniques to estimate trajectories and build 3D LiDAR maps. Since LiDAR can provide accurate range information and is not affected by lighting conditions, highly accurate maps of indoor and outdoor environments can be created. Positioning LiDAR in these maps can be achieved directly through scan matching or filtering methods.

[0004] Visual SLAM refers to a SLAM algorithm that uses images as the only source of information. In contrast to the processing method of the SfM (Structure from Motion) algorithm, the visual SLAM algorithm using keyframes is usually used in systems with real-time applications. Visual SLAM is based on the EKF (Extended Kalman Filter) algorithm to estimate the position, velocity and characteristics of the camera. The BA (Bundle Adjustment) algorithm is often used in the SfM algorithm for global optimization, but this method consumes too many resources for real-time operating systems. With the introduction of the PTAM (Parallel Tracking and Mapping) algorithm by Klein et al., it has become possible for visual SLAM algorithms to run on tiny embedded devices. The key idea of ​​the PTAM algorithm is to track and locate in parallel with map construction so that global optimization techniques that consume too many resources can be used without affecting real-time performance. Strasdat et al. subsequently stated that keyframe-based optimization methods outperform filtering methods in terms of accuracy per unit computational cost (Conference: 2010 IEEE International Conference on Robotics and Automation; Authors: Strasdat, H and Montiel, JMM and Davison, A J. et al.; Publication Year: 2010; Article Title: Real-time monocular SLAM: Why filter?; Pages: 2657-2664).

[0005] Compared with visual SLAM algorithms, visual odometry only focuses on the motion of the camera, that is, the pose information, and usually performs local bundle adjustment to reduce drift. Compared with methods based on stereo depth cameras, monocular visual odometry is susceptible to the unobservable effects of scale. Strasdat et al. pointed out that the scale is prone to drift over time, and described the relationship between Lie groups and similarity transformations. This paper relies on this formula to estimate the similarity transformation.

[0006] Most research on visual localization focuses on matching the photometric features of the environment. This can be done by comparing image feature descriptions such as SIFT (Scale Invariant Feature Transform) or SURF (Speeded Up Robust Features), or by operating directly on image intensity values. However, one of the main problems with visual localization is that the luminosity of the environment changes significantly over time, especially across seasons. Churchill et al. solved this problem by storing multiple image sequences from the same place at different times. Naseer et al. solved this problem by using similarity matrix matching (Conference: 2015 IEEE / RSJ International Conference on Intelligent Robots and Systems; Author: Naseer, T and Ruhnke, M and Stachniss, C, et al.; Publication Year: 2015; Article Title: Robust visual SLAM across seasons; Pages: 2529-2535).

[0007] Compared to methods based on matching photometric measurements, methods for camera localization in geometric maps built from LiDAR maps are relatively rare in the literature. Wolcott et al. proposed a method for localizing autonomous vehicles in urban environments. Using the range information of the LiDAR, a synthetic view of the mapped ground plane is rendered and then matched to the camera image by the maximum normalized information (Conference: 2014 IEEE / RSJ International Conference on Intelligent Robots and Systems; Authors: Wolcott, R W. and Eustice, R M. et al.; Publication Year: 2014; Article Title: Visual localization within LiDAR maps for automated urban driving; Pages: 176-183). This method only provides a three-degree-of-freedom pose. The method proposed by Pascoe et al. estimates the full six-degree-of-freedom camera pose. The algorithm's prior map combines geometric and photometric data, and is subsequently matched to the live image by the minimum normalized information distance. Both methods perform matching in two-dimensional space, thus requiring expensive GPU hardware to support image rendering. In addition, the prior information of both algorithms includes the intensity and visual texture of the LiDAR. In contrast, the algorithm proposed in this paper relies only on geometric information and avoids extremely high floating-point calculations by directly matching 3D geometric bodies. Summary of the invention

[0008] In order to overcome the shortcomings of the prior art, the present invention aims to propose a visual fusion positioning algorithm based on a priori three-dimensional laser radar point cloud map to achieve multi-sensor fusion positioning of a quadrotor drone in a GPS-denied environment. To this end, the technical solution adopted by the present invention is to reconstruct sparse three-dimensional feature points from image features based on the visual fusion positioning method based on a priori three-dimensional laser radar point cloud map, based on the visual odometer process of local bundle adjustment in visual simultaneous localization and mapping SLAM (Simultaneous Localization And Mapping), and then continuously match these points with the laser feature points in the three-dimensional laser radar point cloud map to obtain the precise position information of the quadrotor drone in real time.

[0009] The specific steps are as follows:

[0010] Step 1) using homogeneous coordinates to represent rigid bodies and similarity transformations;

[0011] Define the rigid body T∈SE(3) and the similarity transformation A∈Sim(3). The specific description is as follows:

[0012]

[0013] in, R∈SO(3), point In homogeneous coordinates it is represented as To simplify the notation, define

[0014] Step 2) local reconstruction;

[0015] Define two sets ε and τ to represent the same keyframes and points included in the local reconstruction:

[0016]

[0017] τ={T i |T i ∈SE(3),i=1,…,T} (3)

[0018] Step 3) data association;

[0019] In order to determine the alignment of the local reconstruction with the lidar map, the set of relationships between the reconstructed points and the map points in ε is defined as γ:

[0020]

[0021] Alignment is performed in the ICP algorithm, that is, the current similarity transformation estimate is associated with the updated data through k=1,...,K iterations. For each reconstructed point, the adjacent points are found in the map and stored in the kd tree for fast search. If the adjacent point is close enough, it is added to the corresponding set:

[0022]

[0023] η k is the distance threshold, η max and η min is a positive real number. The linear threshold function is as follows:

[0024]

[0025] In order to analyze its local point distribution, the lidar map is voxelized into Δ, and the resolution depends on the density of voxelization. The mean value within each voxel in Perform a component analysis to determine the main directions of the distribution:

[0026]

[0027] The operators ≤ and < represent component-by-component multiplication of matrices. 1 , There are two largest eigenvalues, and the orthogonal vector V 3 =V 1 ×V 2 Used to describe the voxel ∈ x,y,z = the distribution of the midpoint of (T,σ,N). The specific description is shown in equation (8) = (10):

[0028] R=(V 1 ,V 2 ,V 3 ),t=μ(Θ) (8)

[0029] A=Φ∑(θ)T -1 (9)

[0030] N=|Θ| (10)

[0031] Φ is the basic coordinate transformation matrix for principal component alignment in the voxel, σ represents the standard deviation of the feature points along the axis, and N represents the number of points in the voxel. Since the distribution of local points Λ is constructed offline, this calculation does not affect the online performance of this algorithm.

[0032]

[0033] In order to smooth the discretization effect, adjacent voxels are allowed to be compared, C′ k Specifically, it takes the following forms:

[0034]

[0035] Here, the operator ∨ ​​represents the domain of a voxel;

[0036] After each set of correspondences is determined, a set of estimated poses can be output through similar changes in local reconstruction and lidar map alignment. At each iteration, the refined subset C′ k Will be updated iteratively;

[0037]

[0038] By using the LM (Levenberg Marquardt, trust region) algorithm in g2o to solve the nonlinear least squares minimization problem, the estimated reference frame is output

[0039] For each key frame T c The estimated reference frame is output. Compared with the parameters in the lidar map reference frame, the optimized variables can better correspond to the expected The compensated drift dimension is further transformed on the refined subset to obtain the following set:

[0040]

[0041] The error function is the squared Euclidean distance between corresponding points, expressed as follows:

[0042]

[0043]

[0044]

[0045] Formula (17) is the Huber loss function, which increases the robustness when abnormal data appears and can make the objective function converge better;

[0046] When all iterations After being estimated, the joint similarity transform is as follows:

[0047]

[0048] For local reconstruction and lidar map alignment, we use all points d i By A * Mapped to keyframe T i Because A *is located in the estimated reference frame of the current keyframe, so A * Convert to the reference frame of the lidar map to output the global estimated pose. The conversion method is as follows:

[0049]

[0050]

[0051] The characteristics and beneficial effects of the present invention are:

[0052] 1. The present invention builds a three-dimensional laser radar map construction system based on a ground mobile robot platform to save and reuse PCD map files;

[0053] 2. The present invention designs a monocular vision fusion positioning algorithm based on visual SLAM and prior 3D laser radar point cloud map, uses a local bundle adjustment visual odometer system to reconstruct sparse 3D feature points from image features, and then continuously matches these points with the laser feature points in the 3D laser radar point cloud map to output the positioning information of the quadcopter in real time. The real-time positioning information is sent to the ground station via WiFi and visualized in the host computer;

[0054] 3. After proving the effectiveness of the algorithm designed in this paper through the KITTI public data set, the present invention built a quad-rotor UAV platform to conduct indoor and outdoor positioning experiments. The flight experiment shows that compared with the optical flow sensor, the algorithm designed in this paper has higher accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 It is a structural diagram of the control system of the present invention;

[0056] Figure 2 It is a curve diagram of the given disturbance value and the estimated disturbance value in the height direction in MATLAB simulation;

[0057] Figure 3 It is the experimental platform adopted by the present invention;

[0058] Figure 4 It is a graph of the given altitude and the altitude of the drone during the altitude direction trajectory tracking experiment without ground effect compensation;

[0059] Figure 5 It is a graph of the given altitude and the altitude of the drone during the altitude direction trajectory tracking experiment with ground effect compensation;

[0060] Figure 6 It is a curve diagram of the disturbance estimation values ​​in the three directions of xyz during the altitude direction trajectory tracking experiment with ground effect compensation;

[0061] Figure 7It is a graph of the given altitude and the altitude of the UAV in the landing control experiment without ground effect compensation;

[0062] Figure 8 It is a graph of the given altitude and the altitude of the UAV in the landing control experiment with ground effect compensation;

[0063] Fig. 9 It is a curve diagram of the disturbance estimation values ​​in the three directions of xyz in the landing control experiment with ground effect compensation. DETAILED DESCRIPTION

[0064] In order to overcome the shortcomings of the prior art, the present invention aims to propose a visual fusion positioning algorithm based on a priori three-dimensional laser radar point cloud map to realize multi-sensor fusion positioning of a quadrotor drone in a GPS-denied environment. To this end, the technical solution adopted by the present invention is a visual fusion positioning method based on a priori three-dimensional laser radar point cloud map, comprising the following steps: using homogeneous coordinates to represent rigid bodies and similarity transformations, local reconstruction of key frames, and data association. The specific steps are as follows:

[0065] Step 1) using homogeneous coordinates to represent rigid bodies and similarity transformations;

[0066] Define the rigid body T∈SE(3) and the similarity transformation A∈Sim(3). The specific description is as follows:

[0067]

[0068] in, R∈SO(3), point In homogeneous coordinates it is represented as To simplify the notation, define

[0069] Step 2) local reconstruction;

[0070] The visual odometry system reconstructs the pose and 3D feature points of a keyframe by solving a local bundle adjustment problem. What is local is determined by an undirected weighted graph that describes how much of the image is visible between keyframes. All keyframes that are directly connected to the current keyframe through this graph, and all points observed by these keyframes, are variables of the visual odometry local estimation problem.

[0071] Define two sets ε and τ to represent the same keyframes and points included in the local reconstruction.

[0072]

[0073] τ={T i |T i ∈SE(3),i=1,…,T} (3)

[0074] Separating the keyframes and feature points into two sets has two benefits. First, because the bundle adjustment optimization is performed before alignment, it ensures that consistent local reconstructions are used. Second, the uniform transformation of local bundle adjustment does not affect the operation of visual odometry.

[0075] Step 3) data association;

[0076] In order to determine the alignment of the local reconstruction with the lidar map, the set of relationships between the reconstructed points and the map points in ε is defined as γ.

[0077]

[0078] Alignment is performed in the ICP algorithm, that is, the current similarity transformation estimate is associated with the updated data through k=1,…,K iterations. For each reconstructed point, the neighboring points are found in the map and stored in the kd tree for fast search. If the neighboring point is close enough, it is added to the corresponding set.

[0079]

[0080] η k is the distance threshold, η max and η min is a positive real number. The linear threshold function is as follows:

[0081]

[0082] In order to preserve high-quality associations, the thresholds are changed as the algorithm converges and the distances between points get smaller. A major problem with finding correspondences based purely on nearest neighbors is that the set of reconstructed points only partially overlaps the map. Therefore, even if point d i Even if the position of is perfectly estimated, there may be no meaningful correspondence in γ. Partial overlap is usually caused by incomplete mapping due to the limited vertical field of view of the lidar. Reconstructed points of unmapped geometry are associated with border points of the map, resulting in biased estimated transformations. Since these can significantly affect accuracy and convergence, the algorithm uses the map-local point distribution described below to refine the set of correspondences.

[0083] To analyze its local point distribution, the lidar map is voxelized into Δ, and the resolution depends on the density of the voxelization. The mean value within each voxel in Component analysis was performed to determine the main directions of the distribution.

[0084]

[0085] The operators ≤ and < represent component-by-component multiplication of matrices. 1 , There are two largest eigenvalues, and the orthogonal vector V 3 =V 1 ×V 2 Used to describe the voxel ∈ x,y,z = the distribution of points in (T,σ,N). The specific description is as follows.

[0086] R=(V 1 ,V 2 ,V 3 ),t=μ(Θ) (8)

[0087] A=Φ∑(θ)T -1 (9)

[0088] N=|Θ| (10)

[0089] Φ is the basic coordinate transformation matrix for principal component alignment in the voxel, σ represents the standard deviation of the feature points along the axis, and N represents the number of points in the voxel. Since the distribution of local points Λ is constructed offline, this calculation does not affect the online performance of this algorithm.

[0090]

[0091] Determine a refined subset based on the local point distribution of the map Filter out the associated points d in areas not covered by the lidar map or on the boundaries i Secondly, check whether there are enough points in the voxel to ensure that the subset can properly cover the reconstructed point. For example, at the map border, if the distribution along the main axis is very uneven, the above criteria does not apply. Therefore, the algorithm also verifies whether the point lies within the standard deviation along the main axis of the voxel. In order to smooth the discretization effect, the algorithm also allows neighboring voxels to be compared. C′ k Specifically, it takes the following forms:

[0092]

[0093] Here, the operator ∨ ​​represents the neighborhood of a voxel.

[0094] After each set of correspondences is determined, a set of estimated poses can be output by similarly changing the local reconstruction and lidar map alignment. At each iteration, the refined subset C′ k Will be updated iteratively.

[0095]

[0096] By using the LM (Levenberg Marquardt, trust region) algorithm in g2o to solve the nonlinear least squares minimization problem, the estimated reference frame is output

[0097] For each key frame T c The estimated reference frame is output. Compared with the parameters in the lidar map reference frame, the optimized variables can better correspond to the expected The drift dimension of compensation. The refined subset is further transformed to obtain the following set:

[0098]

[0099] The error function is the squared Euclidean distance between corresponding points, expressed as follows:

[0100]

[0101]

[0102]

[0103] Formula (17) is the Huber loss function, which increases the robustness when abnormal data appears and can make the objective function converge better. Δ =η min , ensuring that the quadratic error of all iterations is within the constraints.

[0104] When all iterations After being estimated, the joint similarity transform is as follows:

[0105]

[0106] For local reconstruction and lidar map alignment, we use all points d i By A * Mapped to keyframe T i Because A * is located in the estimated reference frame of the current keyframe, so A * The global estimated pose can be output by converting it to the reference frame of the lidar map. The conversion method is as follows:

[0107]

[0108]

[0109] The technical problem to be solved by the present invention is that a single sensor of a quad-rotor drone cannot accurately locate itself in a GPS-denied environment due to defects.

[0110] The technical solution adopted by the present invention is: using homogeneous coordinates to represent rigid bodies and similarity transformations, and locally reconstructing and data associating key frames. The specific steps are as follows:

[0111] Step 1) using homogeneous coordinates to represent rigid bodies and similarity transformations;

[0112] Define the rigid body T∈SE(3) and the similarity transformation A∈Sim(3). The specific description is as follows:

[0113]

[0114] in, R∈SO(3), point In homogeneous coordinates it is represented as To simplify the notation, define

[0115] Step 2) local reconstruction;

[0116] The visual odometry system reconstructs the pose and 3D feature points of a keyframe by solving a local bundle adjustment problem. What is local is determined by an undirected weighted graph that describes how much of the image is visible between keyframes. All keyframes that are directly connected to the current keyframe through this graph, and all points observed by these keyframes, are variables of the visual odometry local estimation problem.

[0117] Define two sets ε and τ to represent the same keyframes and points included in the local reconstruction.

[0118]

[0119] τ={T i |T i ∈SE(3),i=1,…,T} (3)

[0120] Separating the keyframes and feature points into two sets has two benefits. First, because the bundle adjustment optimization is performed before alignment, it ensures that consistent local reconstructions are used. Second, the uniform transformation of local bundle adjustment does not affect the operation of visual odometry.

[0121] Step 3) data association;

[0122] In order to determine the alignment of the local reconstruction with the lidar map, the set of relationships between the reconstructed points and the map points in ε is defined as γ.

[0123]

[0124] Alignment is performed in the ICP algorithm, that is, the current similarity transformation estimate is associated with the updated data through k=1,…,K iterations. For each reconstructed point, the neighboring points are found in the map and stored in the kd tree for fast search. If the neighboring point is close enough, it is added to the corresponding set.

[0125]

[0126] η k is the distance threshold, η max and η min is a positive real number. The linear threshold function is as follows:

[0127]

[0128] In order to preserve high-quality associations, the thresholds are changed as the algorithm converges and the distances between points get smaller. A major problem with finding correspondences based purely on nearest neighbors is that the set of reconstructed points only partially overlaps the map. Therefore, even if point d i Even if the position of is perfectly estimated, there may be no meaningful correspondence in γ. Partial overlap is usually caused by incomplete mapping due to the limited vertical field of view of the lidar. Reconstructed points of unmapped geometry are associated with border points of the map, resulting in biased estimated transformations. Since these can significantly affect accuracy and convergence, the algorithm uses the map-local point distribution described below to refine the set of correspondences.

[0129] To analyze its local point distribution, the lidar map is voxelized into Δ, and the resolution depends on the density of the voxelization. The mean value within each voxel in Component analysis was performed to determine the main directions of the distribution.

[0130]

[0131] The operators ≤ and < represent component-by-component multiplication of matrices. 1 , There are two largest eigenvalues, and the orthogonal vector V 3 =V 1 ×V 2 Used to describe the voxel ∈ x,y,z = the distribution of points in (T,σ,N). The specific description is as follows.

[0132] R=(V 1 ,V 2 ,V 3 ),t=μ(Θ) (8)

[0133] A=Φ∑(θ)T -1 (9)

[0134] N=|Θ| (10)

[0135] Φ is the basic coordinate transformation matrix for principal component alignment in the voxel, σ represents the standard deviation of the feature points along the axis, and N represents the number of points in the voxel. Since the distribution of local points Λ is constructed offline, this calculation does not affect the online performance of this algorithm.

[0136]

[0137] Determine a refined subset based on the local point distribution of the map Filter out the associated points d in areas not covered by the lidar map or on the boundaries i Secondly, check whether there are enough points in the voxel to ensure that the subset can properly cover the reconstructed point. For example, at the map border, if the distribution along the main axis is very uneven, the above criteria does not apply. Therefore, the algorithm also verifies whether the point lies within the standard deviation along the main axis of the voxel. In order to smooth the discretization effect, the algorithm also allows neighboring voxels to be compared. C′ k Specifically, it takes the following forms:

[0138]

[0139] Here, the operator ∨ ​​represents the neighborhood of a voxel.

[0140] After each set of correspondences is determined, a set of estimated poses can be output by similarly changing the local reconstruction and lidar map alignment. At each iteration, the refined subset C′ k Will be updated iteratively.

[0141]

[0142] By using the LM (Levenberg Marquardt, trust region) algorithm in g2o to solve the nonlinear least squares minimization problem, the estimated reference frame is output

[0143] For each key frame T c The estimated reference frame is output. Compared with the parameters in the lidar map reference frame, the optimized variables can better correspond to the expected The drift dimension of compensation. The refined subset is further transformed to obtain the following set:

[0144]

[0145] The error function is the squared Euclidean distance between corresponding points, expressed as follows:

[0146]

[0147]

[0148]

[0149] Formula (17) is the Huber loss function, which increases the robustness when abnormal data appears and can make the objective function converge better. Δ =η min , ensuring that the quadratic error of all iterations is within the constraints.

[0150] When all iterations After being estimated, the joint similarity transform is as follows:

[0151]

[0152] For local reconstruction and lidar map alignment, we use all points d i By A * Mapped to keyframe T i Because A * is located in the estimated reference frame of the current keyframe, so A * The global estimated pose can be output by converting it to the reference frame of the lidar map. The conversion method is as follows:

[0153]

[0154]

[0155] In order to verify the effectiveness of the monocular vision fusion positioning algorithm based on visual SLAM and prior 3D lidar point cloud map proposed in this paper, the effectiveness of the fusion positioning algorithm designed in this paper is first verified by simulation of public data sets, and then an experimental platform is built for experimental verification. The system software architecture is as follows Figure 1 shown.

[0156] 1. Simulation Verification

[0157] In order to ensure the effectiveness of the algorithm designed in this paper and simplify the outdoor experimental debugging process, this chapter first uses the KITTI public dataset to verify the algorithm. The KITTI dataset is one of the most popular datasets for mobile robots and robots, collected by Karlsruhe Institute of Technology and Toyota Institute of Technology. This dataset uses a high-resolution RGB camera, a grayscale binocular camera, and a 3D lidar, and records several hours of actual road traffic scenes, with a total of about 180GB of data. This chapter selects the 3D lidar data and monocular camera data for simulation verification. The simulation platform is a Lenovo desktop computer, Inter(R) Core(TM) i5-6500 CPU, 16GB memory, Inter integrated graphics card HD530, equipped with Ubuntu18.04 system, ROS version 1.15.4.

[0158] First, we use the open source 3D LiDAR SLAM algorithm LeGO-LOAM to construct a 3D LiDAR point cloud map, and use pcl_viewer to view the generated PCD file. The effect is as follows: Figure 2 As shown. Secondly, copy the generated PCD file to the root directory of the visual fusion positioning algorithm, modify the corresponding path and file name in the parameter file, and ensure that the point cloud map file can be correctly parsed. Calibrate the camera and lidar, and also modify the corresponding parameter files. Since the visual fusion positioning algorithm outputs pose information, the corresponding trajectory cannot be seen directly. To facilitate algorithm debugging and visual query, this chapter uses RViz to display trajectories, point cloud maps, camera video information, and camera video information marked with feature points. The running results of the visual fusion positioning algorithm are shown in the figure below. Figure 3 As shown in the figure. The positioning and matching of this algorithm is performed in segments. The blue represents the lidar point cloud that is currently being matched, the red represents the lidar point cloud that is not matched in the algorithm, the pink line represents the trajectory displayed by the visual positioning algorithm, and the yellow box in the camera video represents the feature points extracted using the OpenVSLAM front-end visual odometer.

[0159] In order to verify the effectiveness of the monocular vision fusion positioning algorithm based on the prior three-dimensional lidar point cloud map, this chapter only uses the OpenVSLAM algorithm for visual positioning on the same monocular camera dataset, records the output pose information, and compares it with the output pose of the algorithm designed in this paper. Figure 4 The figure shows the result of processing the positioning data using MATLAB. The blue dotted line represents the trajectory of pose estimation using only OpenVSLAM, and the green solid line represents the trajectory of pose estimation using 3D LiDAR map and visual SLAM. It can be clearly seen from the figure that compared with the SLAM algorithm that only uses visual information, the algorithm designed in this paper has higher accuracy and stronger robustness.

[0160] 2. Experimental Verification

[0161] After verifying the effectiveness of the algorithm in the actual experiment through simulation of public data sets, this chapter selects the outdoor flight experiment between buildings with weak GPS signals. The relative position information output by the optical flow is used as the real value of positioning, while maintaining the stable flight of the quadcopter to ensure that the shaking amplitude of the fuselage is not too large. The outdoor experiment is a circular trajectory with the same starting and ending positions. The outdoor real scene of the experiment is as follows Figure 5 As shown in Figure 2, the 3D laser radar mapping effect is as follows: Figure 6 shown.

[0162] After the ground station records the positioning data through rosbag, the trajectory is restored through RViz, such as Figure 7 As shown. Use MATLAB to process monocular camera estimation data and optical flow relative positioning data. Figure 8 This is a three-dimensional comparison chart of positioning effect. Split the XYZ channels and draw positioning comparison curves respectively, such as Fig. 9 As shown in the figure, the maximum error in the X direction is 0.15 meters, the maximum error in the Y direction is 0.12 meters, and the maximum error in the Z direction is 0.23 meters.

[0163] Through simulation verification of public data sets and actual indoor and outdoor flight experiments, it is proved that the monocular vision fusion positioning algorithm designed in this paper based on 3D lidar point cloud map has good accuracy and robustness. Under the ideal conditions of KITTI data set, the algorithm designed in this paper can achieve accurate positioning over a large range. Figure 3 and Figure 4 It can be seen that compared with the pure visual open source positioning algorithm OpenVSLAM, the algorithm designed in this paper does not have cumulative drift and can display the monocular camera trajectory with high accuracy in the point cloud map. Outdoor flight experiments show that the visual fusion positioning algorithm designed in this paper can output high-precision positioning information in a GPS-denied environment.

[0164] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A visual fusion positioning method based on a priori three-dimensional laser radar point cloud map, characterized in that: The visual odometry process of local bundle adjustment in SLAM based on visual simultaneous localization and mapping reconstructs sparse 3D feature points from image features, and then continuously matches these points with the laser feature points in the 3D lidar point cloud map to obtain the precise position information of the quadcopter in real time. The specific steps are as follows: Step 1) using homogeneous coordinates to represent rigid bodies and similarity transformations; Define the rigid body T∈SE(3) and the similarity transformation A∈Sim(3). The specific description is as follows: in, R∈SO(3), point In homogeneous coordinates it is represented as To simplify the notation, define Step 2) local reconstruction; Define two sets ε and τ to represent the same keyframes and points included in the local reconstruction: τ={T i |T i ∈SE(3),i=1,…,T} (3) Step 3) data association; In order to determine the alignment of the local reconstruction with the lidar map, the set of relationships between the reconstructed points and the map points in ε is defined as γ: Alignment is performed in the ICP algorithm, that is, the current similarity transformation estimate is associated with the updated data through k=1,...,K iterations. For each reconstructed point, the adjacent points are found in the map and stored in the kd tree for fast search. If the adjacent point is close enough, it is added to the corresponding set: η k is the distance threshold, η max and η min is a positive real number, the linear threshold function is as follows: In order to analyze its local point distribution, the lidar map is voxelized into Δ, and the resolution depends on the density of voxelization. The mean value within each voxel in Perform a component analysis to determine the main directions of the distribution: The operators ≤ and < represent component-by-component multiplication of matrices. There are two largest eigenvalues, and the orthogonal vector V3 = V1 × V2 is used to describe the voxel ∈ x,y,z = the distribution of the midpoint of (T, σ, N), which is described in Equations (8)-(10): N=|Θ| (10) Φ is the basic coordinate transformation matrix for principal component alignment in the voxel, σ represents the standard deviation of the feature points along the axis, and N represents the number of points in the voxel. Since the distribution of local points Λ is constructed offline, this calculation does not affect the online performance of this algorithm. In order to smooth the discretization effect, adjacent voxels are allowed to be compared, C′ k Specifically, it takes the following forms: Here, the operator ∨ ​​represents the domain of a voxel; After each set of correspondences is determined, a set of estimated poses can be output through similar changes in local reconstruction and lidar map alignment. At each iteration, the refined subset C′ k Will be updated iteratively; By using the trust region LM algorithm in g2o to solve the nonlinear least squares minimization problem, the estimated reference frame is output For each key frame T c The estimated reference frame is output, which optimizes the variables to better correspond to the expected parameters compared to the parameters in the lidar map reference frame. The compensated drift dimension is further transformed on the refined subset to obtain the following set: The error function is the squared Euclidean distance between corresponding points, expressed as follows: Formula (17) is the Huber loss function, which increases the robustness when abnormal data appears and makes the objective function converge better; When all iterations After being estimated, the joint similarity transform is as follows: For local reconstruction and lidar map alignment, all points d i By A * Mapped to keyframe T i Because A * is located in the estimated reference frame of the current keyframe, so A * Convert to the reference frame of the lidar map to output the global estimated pose. The conversion method is as follows:

Images