SLAM method based on radar odometer and visual feature point depth filter

Abstract

The invention relates to an SLAM (Simultaneous Localization and Mapping) method based on a radar odometer and a visual feature point depth filter, which belongs to the technical field of mobile robot positioning and mapping, and comprises the following steps: S1, extracting visual feature points based on image data acquired by a monocular camera, and utilizing laser point cloud data acquired by a three-dimensional laser radar to obtain a visual feature point depth filter; laser point cloud information near the visual feature points is extracted, and estimated values and variances of real scales of the visual feature points are obtained; s2, in combination with a matching result of the radar odometer and the visual feature point, calculating an estimated value and a variance of a real scale of the visual feature point through a triangularization method; s3, using a depth filter to iteratively update the observation values obtained in the step S1 and the step S2, and optimizing the real scale estimation of the visual feature points; and S4, performing joint optimization based on the visual point cloud close to the true value and the pose of the radar odometer, correcting the accumulated drift of the radar odometer, and improving the positioning and map construction precision of the SLAM system.

Description

Technical Field

[0001] The invention belongs to the technical field of mobile robot positioning and mapping, and relates to a SLAM method based on a radar odometer and a visual feature point depth filter. Background Art

[0002] Mobile robots have been widely used in the fields of intelligent medical guidance robots, indoor and outdoor food delivery robots, unmanned delivery vehicles, and unmanned mining vehicles. As a key component of autonomous mobile robots, SLAM technology needs to estimate the robot's position relative to the environment in real time and generate or update the map of the current environment according to specific needs. In the SLAM system, monocular cameras and 3D laser radars can not only provide high-precision geometric information but also rich texture information, so they are widely used.

[0003] However, the existing SLAM system based on the fusion of monocular camera and 3D lidar has the following problems:

[0004] 1. Only using a single frame of laser point cloud or visual feature point triangulation to complete the depth estimation of visual feature points does not make full use of the temporal information of the image and radar, resulting in the depth estimation result of the visual feature points being far from the true value. In the subsequent BA optimization process, it is difficult to converge, resulting in information loss, which is not conducive to improving the positioning accuracy.

[0005] 2. Only using the laser point cloud to provide depth information of visual feature points causes a large amount of laser point cloud to be discarded, especially for mechanical rotating laser radar, thus reducing the robustness and accuracy of the system.

[0006] 3. The method of using visual odometry to provide initial values ​​of the radar odometry pose is prone to cause mismatching problems due to the lack of prior information of visual feature points. It cannot provide a good initial value for the optimization of the radar odometry pose and lacks degradation detection of the radar odometry.

[0007] Current research lacks a data association mechanism based on visual information between consecutive frames to effectively reduce the cumulative drift error of the radar odometer. Summary of the invention

[0008] In view of this, an object of the present invention is to provide a SLAM method based on radar odometer and visual feature point depth filter.

[0009] In order to achieve the above object, the present invention provides the following technical solutions:

[0010] A SLAM method based on radar odometer and visual feature point depth filter includes the following steps:

[0011] S1: Extract visual feature points based on the image data obtained by the monocular camera, and use the laser point cloud data obtained by the 3D laser radar to extract the laser point cloud information near the visual feature points to obtain the estimated value of the real scale of the visual feature points and its variance;

[0012] S2: Combine the matching results of radar odometer and visual feature points, and calculate the estimated value of the true scale of the visual feature points and its variance through triangulation method;

[0013] S3: Iteratively update the observation values ​​obtained in step S1 and step S2 using a deep filter to optimize the true scale estimation of the visual feature points;

[0014] S4: Jointly optimize the visual point cloud and radar odometer pose based on the close-to-true value to correct the accumulated drift of the radar odometer and improve the positioning and map construction accuracy of the SLAM system.

[0015] Further, step S1 specifically includes the following steps:

[0016] S1-1: First, the 3D laser point cloud According to the external parameters T of the 3D laser radar and monocular camera CL Transform to the camera coordinate system and project it onto the pixel plane using the intrinsic parameter K. Then, the pixel plane is counted based on the projection result and the set projection grid size LidarGird, which contains the laser point cloud in the camera coordinate system.

[0017] S1-2: Use the visual feature point extraction method to extract the visual feature points of the image, calculate the projection grid coordinates (Proj.x, Proj.y) corresponding to each visual feature point according to the projection grid divided by S1-1, and use the intrinsic parameter K to obtain its normalized vector in the camera coordinate system

[0018] S1-3: Count the number of laser point clouds in the camera coordinate system contained in the projection grid coordinates in S1-2 and its nearby grids (Proj.xN~Proj.x+N, Proj.yN~Proj.y+N), where N is the search radius;

[0019] If there is laser but the number of point clouds is less than the threshold Th num1 , then the inverse of the z value of the nearest laser point is taken as the estimation result, and its variance is calculated as

[0020] If the number of point clouds is greater than the threshold Th num1 But less than Th num2 , then normalize the laser point cloud under the camera coordinates contained, and use the kdtree tree to manage and search for P CnormThe three nearest normalized laser point clouds are used to fit the plane using the corresponding laser point clouds in the camera coordinate system. in Represents the coordinates of the normal vector of the plane in the camera coordinate system, d represents the distance between the camera optical center and the plane, and Get the scale estimate The corresponding variance is

[0021] If the number of point clouds exceeds the threshold Th num2 , then the laser point cloud in the camera coordinate system is sorted from small to large along the Z axis, divided into M intervals according to the maximum and minimum values, and the first interval greater than the threshold Th is found from small to large. num1 The scale estimate and variance are calculated based on the point-to-surface distance. If not found, the interval is expanded and re-divided.

[0022] Further, step S2 specifically includes the following steps:

[0023] The matching results of visual feature points are used to verify whether the radar odometer has degraded. If degradation has occurred, the depth estimation results of the visual feature points of the previous frame and the matching results of the current frame are used to estimate the pose of the current frame. If no degradation has occurred, the pose estimation results of the radar odometer and the visual feature points of the corresponding frames of all visual point clouds matched to the current frame are triangulated to obtain a new estimation result and calculate its variance.

[0024] Furthermore, in step S2, the odometer degradation is determined by calculating the basic matrix between adjacent frames:

[0025]

[0026] in represents the translation change between adjacent frames obtained by the radar odometer, R lc represents the rotation change between adjacent frames, [·] X Represents the antisymmetric matrix of a three-dimensional vector;

[0027] According to pixel l T Fpixel c The relationship between the modulus length and the threshold is calculated by counting the ratio of exceeding the threshold and not exceeding the threshold. If the ratio is greater than 1, it means that the radar odometer has degraded, otherwise it has not degraded.

[0028] Furthermore, in step S2, the matching of visual feature points of adjacent frames is performed using LK optical flow. If the radar odometer has not been degraded, the visual point cloud corresponding to all frames from the reference key frame of the current frame to the previous frame of the current frame is projected to the current frame and matched using the descriptor.

[0029] Furthermore, in step S2, the variance calculation includes two parts:

[0030] In the first part, the normalized direction vector in the camera coordinate system corresponding to the matched visual feature point is inner-producted to obtain the cosine value. The smaller the cosine value, the larger the parallax and the more reliable the triangulation. That is, the variance is

[0031] The second part performs pixel perturbation on the matching result to obtain a new triangulated result. The difference between the inverse depths of the two triangulated results is the variance of the second part. The final variance is obtained by accumulating the variances of the two parts.

[0032] Further, step S3 specifically includes the following steps:

[0033] S3-1: The depth modeling method of the visual feature points adopts the Gaussian uniform mixed distribution of the inverse depth, and uses the Gaussian Beta mixed distribution to approximate the Gaussian uniform mixed distribution. The parameter of the inverse depth of the feature point is the number of inner points a k , the number of external points b k , inverse depth mean μ k , the variance of the inverse depth Where k is the kth observation of the visual feature point, and the initial value of each parameter is set at the initial moment, namely a0, b0, μ0

[0034] S3-2: After the initial distribution of visual feature points is modeled, when the observation information is obtained, the inverse depth invD of the current visual feature point is obtained. k and variance When updating, use the following method:

[0035]

[0036]

[0037] μ k =C1×m+C2×μ k-1

[0038]

[0039] S3-3: When When , the depth information of the visual feature point converges, and a qualified visual point cloud can be generated, and its corresponding variance is retained for the joint optimization in the proposed step S4.

[0040] Further, step S4 specifically includes the following steps:

[0041] S4-1: Determine whether the number of visual point clouds of each local key frame is greater than a given threshold TcovisNum ,If the number of corresponding visual point clouds of some key frames is less than a given threshold, then traverse the unconverged key frame visual point clouds, arrange them from small to large according to the variance, and fill in the missing visual point clouds;

[0042] S4-2: Perform local BA optimization on all visual point clouds and visual key frames to obtain accurate poses and 3D visual point clouds. The initial pose of the visual key frame optimization is provided by the radar odometer. The laser point cloud of the radar odometer is registered in the camera coordinate system, and the pose transformation between different frames in the camera coordinate system is obtained.

[0043] S4-3: Optimize the pose graph by using the visual key frame pose optimized by S4-2 and the pose of the unoptimized visual common frame provided by the intermediate radar odometer

[0044] Furthermore, the local key frame in S4-1 refers to a visual key frame that has a co-viewing relationship with the current key frame, and the co-viewing relationship means that there is a projection of the same visual point cloud between the two key frames.

[0045] The beneficial effects of the present invention are as follows: the present invention solves the problems of inaccurate depth estimation of visual feature points, low reliability of pose estimation and accumulated drift of radar odometer in the prior art, and improves the accuracy and robustness of the SLAM system.

[0046] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0048] Figure 1 is a flow chart of the algorithm of the present invention;

[0049] Figure 2 This is a schematic diagram of radar odometer degradation detection;

[0050] Figure 3 Schematic diagram of reducing the accumulated drift of radar odometer by local BA optimization of visual keyframes. DETAILED DESCRIPTION

[0051] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0052] It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present invention, and thus the drawings only show components related to the present invention rather than being drawn according to the number, shape and size of components in actual implementation. In actual implementation, the type, quantity and proportion of each component may be changed arbitrarily, and the component layout may also be more complicated.

[0053] In the following description, numerous details are discussed to provide a more thorough explanation of the embodiments of the present invention. However, it is obvious to those skilled in the art that the embodiments of the present invention can be implemented without these specific details. In other embodiments, well-known structures and devices are shown in the form of block diagrams rather than in detail to avoid making the embodiments of the present invention difficult to understand.

[0054] The present invention provides a SLAM method based on radar odometer and visual feature point depth filter, such as Figure 1 As shown, the following steps are included:

[0055] Step S1: extracting visual feature points based on image data acquired by a monocular camera, and extracting laser point cloud information near the visual feature points using laser point cloud data acquired by a three-dimensional laser radar, to obtain an estimated value of the true scale of the visual feature points and its variance; the method for extracting laser point cloud information near the visual feature points includes:

[0056] S1-1: First, the 3D laser point cloud According to the external parameters T of the 3D laser radar and monocular camera CL Transform to the camera coordinate system and project it onto the pixel plane using the intrinsic parameter K. Then, the pixel plane is counted based on the projection result and the set projection grid size LidarGird, which contains the laser point cloud in the camera coordinate system.

[0057] S1-2: Extract the visual feature points of the image using any visual feature point extraction method and calculate the projection grid coordinates (Proj.x, Proj.y) corresponding to each visual feature point according to the projection grid divided by S1-1, and use the intrinsic parameter K to obtain its normalized vector in the camera coordinate system

[0058] S1-3: Count the number of laser point clouds in the camera coordinate system contained in the projection grid coordinates in S1-2 and its nearby grids (Proj.xN~Proj.x+N, Proj.yN~Proj.y+N), where N is the search radius. If there is a laser but the number of point clouds is less than the threshold Th num1 Then the inverse of the z value of the nearest laser point is taken as the estimation result and its variance is calculated as If the number of point clouds is greater than the threshold Th num1 But less than Th num2 , then normalize the laser point cloud under the camera coordinates contained, and use the kdtree tree to manage and search for P Corm The three nearest normalized laser point clouds are used to fit the plane using the corresponding laser point clouds in the camera coordinate system. in Represents the coordinates of the normal vector of the plane in the camera coordinate system, d represents the distance between the camera optical center and the plane, and Get the scale estimate The corresponding variance is If the number of point clouds exceeds the threshold Th num2 , then the laser point cloud in the camera coordinate system is sorted from small to large along the Z axis, divided into M intervals according to the maximum and minimum values, and the first interval greater than the threshold Th is found from small to large. num1 The scale estimate and variance are calculated according to the above point-to-surface distance method. If not found, the interval is expanded and re-divided.

[0059] Step S2: combining the matching results of the radar odometer and the visual feature points, and calculating the estimated value of the real scale of the visual feature points and its variance through a triangulation method; the specific implementation of the triangulation method includes:

[0060] S2-1: Use the matching results of visual feature points to verify whether the radar odometer has degraded. If degraded, use the depth estimation results of the visual feature points of the previous frame and the matching results of the current frame to estimate the pose of the current frame. If no degradation has occurred, use the pose estimation results of the radar odometer and the visual feature points of the corresponding frames of all visual point clouds matched to the current frame to triangulate, obtain a new estimation result and calculate its variance.

[0061] S2-2: In S2-1, the method for determining odometer degradation is to calculate the basic matrix between adjacent frames. here represents the translation change between adjacent frames obtained by the radar odometer, R lc represents the rotation change between adjacent frames, [·] X Represents the antisymmetric matrix of a three-dimensional vector, based on pixel l T Fpixel c The relationship between the modulus length and the threshold is calculated by counting the ratio of those that exceed the threshold and those that do not. If the ratio is greater than 1, it means that the radar odometer has degraded, otherwise it has not degraded, such as Figure 2 As shown, the number of red arrows is greater than that of green arrows.

[0062] S2-3: In S2-1, the matching of visual feature points of adjacent frames is performed using LK optical flow. If the radar odometer has not degraded, the visual point cloud corresponding to all frames from the reference key frame of the current frame to the previous frame of the current frame is projected to the current frame, and matching is performed using the descriptor.

[0063] S2-4: The variance calculation in S2-1 includes two parts: the first part performs an inner product operation on the normalized direction vector in the camera coordinate system corresponding to the matched visual feature point to obtain a cosine value. The smaller the cosine value, the larger the parallax and the more reliable the triangulation. That is, the variance is The second part performs pixel perturbation on the matching result to obtain a new triangulated result. The difference between the inverse depths of the two triangulated results is the variance of the second part. The final variance is obtained by accumulating the variances of the two parts.

[0064] Step S3: using a depth filter to iteratively update the observation values ​​obtained in step 1 and step 2 to optimize the true scale estimation of the visual feature points; the updating method of the visual feature point depth filter includes:

[0065] S3-1: The depth modeling method of the visual feature points adopts the Gaussian uniform mixed distribution of the inverse depth, but uses the Gaussian Beta mixed distribution to approximate the Gaussian uniform mixed distribution, and the parameter of the inverse depth of the feature point is the number of inner points a k , the number of external points b k , inverse depth mean μ k , the variance of the inverse depth Here k is the kth observation of the visual feature point. At the initial moment, the initial value of each parameter needs to be set, i.e., a0, b0, μ0.

[0066] S3-2: After the initial distribution model of the above visual feature points is completed, when the observation information is obtained, the inverse depth invD of the current visual feature point is obtained.k and variance When updating, use the following method:

[0067]

[0068] μ k =C1×m+C2×μ k-1

[0069]

[0070] S3-3: When When , the depth information of the visual feature point converges to generate a qualified visual point cloud, and its corresponding variance is retained for the joint optimization in step S4.

[0071] Step S4: Jointly optimize the visual point cloud and the radar odometer pose that are closer to the true value to correct the accumulated drift of the radar odometer and improve the positioning and map construction accuracy of the SLAM system. The joint optimization method includes:

[0072] S4-1: Determine whether the number of visual point clouds of each local key frame is greater than a given threshold T covisNum ,If the number of corresponding visual point clouds of some key frames is less than a given threshold, the unconverged key frame visual point clouds are traversed, ,arranged from small to large according to the variance, and fill the vacant visual point clouds.

[0073] S4-2: In S4-1, the local key frame refers to a visual key frame that has a co-viewing relationship with the current key frame. The co-viewing relationship here means that there is a projection of the same visual point cloud between the two key frames.

[0074] S4-3: After S4-1, all visual point clouds and visual key frames will be locally optimized with BA to obtain more accurate poses and three-dimensional visual point clouds. The initial pose of the visual key frame optimization is provided by the radar odometer. The laser point cloud of the radar odometer is registered in the camera coordinate system, so what is obtained is the pose transformation between different frames in the camera coordinate system.

[0075] S4-4: Optimize the pose graph by using the visual key frame pose optimized by S4-3 and the pose of the unoptimized visual common frame provided by the intermediate radar odometer, such as Figure 3 As shown, to reduce the accumulated drift of the radar odometer.

[0076] In the above embodiments, the description's reference to "this embodiment" indicates that a particular feature, structure, or characteristic described in conjunction with the embodiment is included in at least some embodiments, but not necessarily all embodiments. Multiple occurrences of "this embodiment" do not necessarily all refer to the same embodiment.

[0077] In the above-described embodiments, although the invention has been described in conjunction with specific embodiments of the invention, many substitutions, modifications, and variations of these embodiments will be apparent to those of ordinary skill in the art based on the foregoing description. For example, other storage structures (e.g., dynamic RAM (DRAM)) may use the embodiments discussed. Embodiments of the invention are intended to encompass all such substitutions, modifications, and variations that fall within the broad scope of the appended claims.

[0078] This embodiment further provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, any one of the methods in this embodiment is implemented.

[0079] This embodiment also provides an electronic terminal, including: a processor and a memory;

[0080] The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the terminal executes any one of the methods in this embodiment.

[0081] The computer-readable storage medium in this embodiment can be understood by ordinary technicians in this field: all or part of the steps of implementing the above-mentioned method embodiments can be completed by hardware related to the computer program. The aforementioned computer program can be stored in a computer-readable storage medium. When the program is executed, the steps of the above-mentioned method embodiments are executed; and the aforementioned storage medium includes: ROM, RAM, magnetic disk or optical disk and other media that can store program codes.

[0082] The electronic terminal provided in this embodiment includes a processor, a memory, a transceiver and a communication interface. The memory and the communication interface are connected to the processor and the transceiver and complete communication with each other. The memory is used to store computer programs, the communication interface is used to communicate, and the processor and the transceiver are used to run computer programs so that the electronic terminal executes each step of the above method.

[0083] In this embodiment, the memory may include a random access memory (RAM), and may also include a non-volatile memory (non-volatile memory), such as at least one disk memory.

[0084] The above-mentioned processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0085] The present invention can be used in many general or special computing system environments or configurations, such as personal computers, server computers, handheld or portable devices, tablet devices, multiprocessor systems, microprocessor-based systems, set-top boxes, programmable consumer electronic devices, network PCs, minicomputers, mainframe computers, distributed computing environments including any of the above systems or devices, and the like.

[0086] The present invention may be described in the general context of computer-executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc. that perform specific tasks or implement specific abstract data types. The present invention may also be practiced in distributed computing environments where tasks are performed by remote processing devices connected through a communication network. In a distributed computing environment, program modules may be located in local and remote computer storage media, including storage devices.

[0087] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A SLAM method based on radar odometer and visual feature point depth filter, characterized in that: The following steps are involved: S1: Extract visual feature points based on the image data obtained by the monocular camera, and use the laser point cloud data obtained by the 3D laser radar to extract the laser point cloud information near the visual feature points to obtain the estimated value of the real scale of the visual feature points and its variance; S2: Combine the matching results of radar odometer and visual feature points, and calculate the estimated value of the true scale of the visual feature points and its variance through triangulation method; S3: Iteratively update the observation values ​​obtained in step S1 and step S2 using a deep filter to optimize the true scale estimation of the visual feature points; S4: Jointly optimize the visual point cloud and radar odometer pose based on the close-to-true value to correct the accumulated drift of the radar odometer and improve the positioning and map construction accuracy of the SLAM system.

2. The SLAM method based on radar odometer and visual feature point depth filter according to claim 1, characterized in that: Step S1 specifically includes the following steps: S1-1: First, the 3D laser point cloud According to the external parameters T of the 3D laser radar and monocular camera CL Transform to the camera coordinate system and project it onto the pixel plane using the intrinsic parameter K. Then, the pixel plane is counted based on the projection result and the set projection grid size LidarGird, which contains the laser point cloud in the camera coordinate system. S1-2: Use the visual feature point extraction method to extract the visual feature points of the image, calculate the projection grid coordinates (Proj.x, Proj.y) corresponding to each visual feature point according to the projection grid divided by S1-1, and use the intrinsic parameter K to obtain its normalized vector in the camera coordinate system S1-3: Count the number of laser point clouds in the camera coordinate system contained in the projection grid coordinates in S1-2 and its nearby grids (Proj.xN~Proj.x+N, Proj.yN~Proj.y+N), where N is the search radius; If there is laser but the number of point clouds is less than the threshold Th num1 , then the inverse of the z value of the nearest laser point is taken as the estimation result, and its variance is calculated as If the number of point clouds is greater than the threshold Th num1 But less than Th num2 , then normalize the laser point cloud under the camera coordinates contained, and use the kdtree tree to manage and search for P Cnorm The three nearest normalized laser point clouds are used to fit the plane using the corresponding laser point clouds in the camera coordinate system. in Represents the coordinates of the normal vector of the plane in the camera coordinate system, d represents the distance between the camera optical center and the plane, and Get the scale estimate The corresponding variance is If the number of point clouds exceeds the threshold Th num2 , then the laser point cloud in the camera coordinate system is sorted from small to large along the Z axis, divided into M intervals according to the maximum and minimum values, and the first interval greater than the threshold Th is found from small to large. num1 The scale estimate and variance are calculated based on the point-to-surface distance. If not found, the interval is expanded and re-divided.

3. The SLAM method based on radar odometer and visual feature point depth filter according to claim 1, characterized in that: Step S2 specifically includes the following steps: The matching results of visual feature points are used to verify whether the radar odometer has degraded. If degradation has occurred, the depth estimation results of the visual feature points of the previous frame and the matching results of the current frame are used to estimate the pose of the current frame. If no degradation has occurred, the pose estimation results of the radar odometer and the visual feature points of the corresponding frames of all visual point clouds matched to the current frame are triangulated to obtain a new estimation result and calculate its variance.

4. The SLAM method based on radar odometer and visual feature point depth filter according to claim 1, characterized in that: In step S2, the odometer degradation is determined by calculating the basic matrix between adjacent frames: in represents the translation change between adjacent frames obtained by the radar odometer, R lc represents the rotation change between adjacent frames, [·] x Represents the antisymmetric matrix of a three-dimensional vector; According to pixel l T Fpixel c The relationship between the modulus length and the threshold is calculated by counting the ratio of exceeding the threshold and not exceeding the threshold. If the ratio is greater than 1, it means that the radar odometer has degraded, otherwise it has not degraded.

5. The SLAM method based on radar odometer and visual feature point depth filter according to claim 4, characterized in that: In step S2, the matching of visual feature points of adjacent frames is performed using LK optical flow. If the radar odometer has not been degraded, the visual point cloud corresponding to all frames from the reference key frame of the current frame to the previous frame of the current frame is projected to the current frame and matched using the descriptor.

6. The SLAM method based on radar odometer and visual feature point depth filter according to claim 5, characterized in that: In step S2, the variance calculation includes two parts: In the first part, the normalized direction vector in the camera coordinate system corresponding to the matched visual feature point is inner-producted to obtain the cosine value. The smaller the cosine value, the larger the parallax and the more reliable the triangulation. That is, the variance is The second part performs pixel perturbation on the matching result to obtain a new triangulated result. The difference between the inverse depths of the two triangulated results is the variance of the second part. The final variance is obtained by accumulating the variances of the two parts.

7. The SLAM method based on radar odometer and visual feature point depth filter according to claim 1, characterized in that: Step S3 specifically includes the following steps: S3-1: The depth modeling method of the visual feature points adopts the Gaussian uniform mixed distribution of the inverse depth, and uses the Gaussian Beta mixed distribution to approximate the Gaussian uniform mixed distribution. The parameter of the inverse depth of the feature point is the number of inner points a k , the number of external points b k , inverse depth mean μ k , the variance of the inverse depth Where k is the kth observation of the visual feature point, and the initial value of each parameter is set at the initial moment, that is, S3-2: After the initial distribution of visual feature points is modeled, when the observation information is obtained, the inverse depth invD of the current visual feature point is obtained. k and variance When updating, use the following method: m k =C1×m+C2×μ k-1 S3-3: When When , the depth information of the visual feature point converges, and a qualified visual point cloud can be generated, and its corresponding variance is retained for the joint optimization in the proposed step S4.

8. The SLAM method based on radar odometer and visual feature point depth filter according to claim 1, characterized in that: Step S4 The specific steps include: S4-1: Determine whether the number of visual point clouds of each local key frame is greater than a given threshold T covisNum ,If the number of corresponding visual point clouds of some key frames is less than a given threshold, then traverse the unconverged key frame visual point clouds, arrange them from small to large according to the variance, and fill in the missing visual point clouds; S4-2: Perform local BA optimization on all visual point clouds and visual key frames to obtain accurate poses and 3D visual point clouds; The initial pose of the visual keyframe optimization is provided by the radar odometer. The laser point cloud of the radar odometer is registered in the camera coordinate system, and the pose transformation between different frames in the camera coordinate system is obtained. S4-3: Optimize the pose graph by using the visual key frame pose optimized by S4-2 and the pose of the unoptimized visual ordinary frame provided by the intermediate radar odometer.

9. The SLAM method based on radar odometer and visual feature point depth filter according to claim 8, characterized in that: The local key frame in S4-1 refers to a visual key frame that has a co-viewing relationship with the current key frame, and the co-viewing relationship means that there is a projection of the same visual point cloud between the two key frames.

Images