A camera-solid-state LiDAR fusion relocation method for dynamic environments

Abstract

A camera-and-solid-state LiDAR fusion relocalization method for dynamic environments includes: obtaining prior dynamic objects through target detection using YOLOv5 deep learning; extracting feature points from the image, calculating the dynamic probability of each feature point using epipolar geometry and Bayes' theorem in probabilistic form, eliminating dynamic feature points, and retaining static feature points; updating the bag-of-words representation using the DBOW algorithm, where each image carries the pose calculated by the solid-state LiDAR and calibrated with extrinsic parameters to obtain the camera pose, and also records the point cloud information of the static portion of the frame; and designing a two-stage registration method to improve the robustness of the system. During relocalization, the similarity between the feature points of the current frame and the previously stored feature points is calculated. If there are too few matching feature points, the point clouds in the candidate frames are back-projected for ICP registration to further improve the robustness of the system. This invention combines vision and LiDAR to improve the shortcomings of relocalization in dynamic environments.

Description

Technical Field

[0001] This invention relates to the field of robot global localization, specifically a camera and solid-state LiDAR fusion relocalization method for dynamic environments. Background Technology

[0002] Global localization is a prerequisite for robots to achieve true autonomous movement. Once started, a robot can quickly and accurately determine its position, which effectively improves its applicability and practicality. On the other hand, if a robot loses its localization, it can quickly regain its current location, which provides long-term stability and reliability for robot localization.

[0003] Over the past three decades, with the rapid development of computer science and sensors, Simultaneous Localization and Mapping (SLAM) has become an indispensable technology in many fields, such as robotics, autonomous driving, and augmented reality (AR). Thanks to advancements in computer vision, Visual SLAM (V-SLAM) has attracted considerable attention from researchers and companies due to its advantages such as low cost, low power consumption, and the ability to provide rich landscape information. Despite its excellent performance in static environments, performance degradation and insufficient robustness in dynamic environments have become major obstacles to its practical application. For example, patent CN110533722A proposes a method and system for rapid robot relocalization based on a visual dictionary, while patent CN110796683A proposes a relocalization method based on visual features combined with laser SLAM. Vision is far more sensitive to dynamic objects than sensors like LiDAR. Using a purely visual method to achieve relocalization in a static environment is a low-cost approach, but this method will fail when facing a real environment. It will calculate the feature points of dynamic objects (such as pedestrians, vehicles, etc.), which simply introduces interference into the system. Although patent CN110796683A records its own pose in the visual dictionary DBow and designs some strategies, it is still ineffective against the interference of dynamic objects. The relocation method that combines lidar and camera, such as the invention patent with patent number CN112596064A, proposes an integrated global localization method for indoor robots that combines laser and vision. This method simultaneously establishes a laser keyframe retrieval library and a visual keyframe retrieval library. Although the robustness is improved, in real dynamic environments, the movement of objects can cause keyframes to record incorrect feature points. Consequently, when a large number of feature points change, relocation may fail.

[0004] Current global localization methods for robots suffer from the following drawbacks and limitations: 1) Current methods establish separate visual and laser localization maps, then align the two maps to unify the results of the two global localization methods. However, this method is highly sensitive to the calibration accuracy of the two sensors, and establishing two separate maps consumes significant resources. 2) If only laser radar is used for relocalization, not only is point cloud registration time long, but relocalization will also fail in scenarios with similar or unstructured structures. Currently, some researchers provide an initial pose for the robot to match and relocalize within this range, but humans may not know the robot's approximate location, and this constitutes human intervention, making it impossible to achieve fully autonomous robots. 3) In real-world environments, the presence of dynamic objects is unavoidable, and it has an immeasurable impact on relocalization and even the entire SLAM system, which is one of the main problems hindering the further development of SLAM. Summary of the Invention

[0005] This invention addresses the aforementioned shortcomings of existing technologies by providing a camera-and-solid-state LiDAR fusion relocalization method for dynamic environments. This method enables localization in dynamic real-world environments, effectively combining information from both solid-state LiDAR and vision, significantly improving the robustness of robot localization in dynamic environments. Relocalization requires no human intervention, is fast, and has high accuracy. Solid-state LiDAR is less expensive than rotating LiDAR and generates denser point clouds, but has a smaller field of view (FOV).

[0006] To achieve the above objectives, the present invention provides the following technical solution: a camera and solid-state lidar fusion relocation method for dynamic environments, comprising the following steps:

[0007] S1: The extrinsic parameter calibration of the solid-state lidar and camera yields the coordinate transformation matrix from camera to lidar. Where c represents the camera coordinate system and L represents the radar coordinate system, the camera is calibrated, the camera and radar are synchronized in time, image distortion is removed, and the distortion-removed image is obtained;

[0008] S2: Use the pruned YOLOv5 to identify dynamic targets in the image, and send the obtained target boxes of the dynamic objects into the vision assistance module respectively;

[0009] The specific pruning of YOLOv5 described above is as follows:

[0010] Based on the YOLOv5 framework, a lightweight approach is adopted by applying an L1 regularization layer to the scaling factor during batch normalization (BN) for pruning. This approach is easy to implement and does not require changes to the existing architecture.

[0011]

[0012] Equation (1) is the calculation formula for the BN (Batch Normalization) layer, where z in and z out These are the input and output of the Batch Normalization (BN) layer, respectively, where B is the current smallest mini-batch, and μ is the output. B and σ B γ and β represent the mean and standard deviation of the input activation function on B, respectively, and are trainable affine transformation parameters (scale and shift). The activation size z per channel. out The coefficient γ is positively correlated with the activation value. If γ is too small, close to zero, the activation value will also be very small, allowing the removal of channels (or neurons) with γ approaching zero. However, under normal circumstances, after training a network, the coefficients of the BN layer follow a normal distribution. During normal training, γ follows a histogram distribution with epochs, so there are very few channels with γ approaching zero, making pruning impossible. Therefore, L1 regularization is used to push the value of the BN scaling factor closer to zero, enabling the identification of irrelevant channels (or neurons), since each scaling factor corresponds to a specific convolutional channel (or neuron in a fully connected layer).

[0013]

[0014] The first part of equation (2) is the standard CNN loss function, and the second part is the constraint of the added regularization coefficient, where λ is the balance factor, W represents the trainable weights, (x, y) represents the training input and target, λ can be adjusted reasonably according to the dataset, and g(s)=|s| represents L1 regularization. Due to the gradient problem caused by the introduction of L1 norm, it can be replaced by smooth-L1.

[0015] S3: After the visual assistance module extracts feature points from the image, it needs to calculate the dynamic probability of the feature points and remove those with higher probability values ​​as interference points. In order to reduce the impact of dynamic objects on the localization and mapping of laser SLAM, the point cloud of the current frame is projected onto the image and the point cloud of dynamic objects within the target box obtained in S2 is clustered and removed during the mapping process.

[0016] The dynamic probability calculation described above is as follows:

[0017] S31: Extract ORB feature points

[0018] S311: When the gray values ​​I(x) of more than N points surrounding point P in an image differ from the gray value I(p) of point P by a value greater than a threshold ε, the point is considered a target corner point. Specifically, this is expressed as follows:

[0019]

[0020] S312: The orientation of feature points is preserved by calculating the centroid. An image pyramid is constructed by scaling the original image sequence by a certain ratio to maintain the scale invariance of feature points. At the same time, a quadtree uniform distribution algorithm is used to ensure that feature points are evenly distributed in the image.

[0021] S313: Calculate the BRIEF descriptor, which describes the information around the feature point in binary format;

[0022] S32: First, calculate the fundamental matrix of the epipolar geometry for all points outside the bounding box identified by YOLO. This fundamental matrix maps feature points from the previous frame to the corresponding search domain, i.e., the epipolar line, in the current frame. Assume P is a static point, P' is a dynamic point, and the distance between the dynamic point and the epipolar line is D. Let p1 and p2 represent the matching points in the previous and current frames, respectively, and P1 and P2 are in homogeneous coordinate form, while u and v are the pixel coordinates of the feature points in the image.

[0023] P1=[u1,v1,1],P2=[u2,v2,1] (4)

[0024] p1=[u1,v1],p2=[u2,v2] (5)

[0025] The polar line is represented by I, and the calculation formula is:

[0026]

[0027] Where X, Y, Z represent epipolar vectors, and F is the fundamental matrix.

[0028]

[0029] Equation (7) indicates that a paired point falls into the current frame after the fundamental matrix transformation in the previous frame, and its distance from the epipolar line I is D. For stable feature points, in an ideal state, after the fundamental matrix transformation, they will eventually fall on the epipolar line I, that is, the distance from the point to the epipolar line is zero. Due to the presence of moving objects, each feature point in the image is not strictly constrained to be located on its corresponding epipolar line. By converting distance information into probability information, it can be deduced from the above that the longer the distance of the point from the epipolar line, the greater the probability of movement.

[0030] S33: Dynamics are a continuous action, and processing a single frame is unstable. Therefore, Bayes' theorem is used, which is an idea that uses past and current information to evaluate and eliminate dynamic keypoints. Assume that the initial dynamic probability of each point is ω, and the distance from the matched keypoint to its corresponding epipolar line follows a Gaussian distribution, and the probability iteration conforms to the Markov property.

[0031] S331: Calculate the movement probability of keypoints using the normal probability density function, which is defined as:

[0032]

[0033] The standard normal distribution is adopted, where δ represents the standard deviation, which is 1, and the expected value is zero, where c pi The expression representing whether the i-th point initially selected by epipolar geometry is static or dynamic is:

[0034]

[0035] S332: In this invention, the probability propagation formula for a point is:

[0036]

[0037]

[0038] in The update probability comes from the geometric model. The update probability comes from within the bounding box of YOLO.

[0039] S333: Where ω is the weight representing the confidence level of the bounding boxes identified by the epipolar geometry model and YOLO, respectively. ω is calculated as follows:

[0040]

[0041] Where N c N represents the number of exterior points calculated from the epipolar geometry model. s This represents the number of points contained in the YOLO bounding box. Points with a probability greater than ω1 are identified as dynamic points and removed from feature point pairing for relocalization.

[0042] S4: Solid-state LiDAR uses traditional SLAM methods to construct an environmental map and calculate the robot pose T. wL The camera pose in the world coordinate system is calculated based on the external parameters.

[0043] S5: The camera continuously acquires images and filters out key frames during the movement. Then, the DBow algorithm is used to record the key frames that have had dynamic feature points removed and update the bag of words. At the same time, pose information is incorporated, and the point cloud outside the dynamic bounding box projected onto the image is also stored.

[0044] S51: The camera continuously acquires images and selects key frames during movement;

[0045]

[0046] S52: Use the DBow algorithm to record the keyframes from which dynamic feature points have been removed and update the bag of words. Also, derive the camera pose from the pose calculated by laser SLAM according to formula (13) and record it. In formula (13), T wc and T wL These are the camera's pose in the world coordinate system and the radar's pose in the world coordinate system, respectively. It is the extrinsic transformation matrix from camera to radar.

[0047] S54: Project the point cloud of the solid-state lidar in the same frame onto the plane of the camera using intrinsic and extrinsic parameters.

[0048] S55: Align the bounding boxes calculated by YOLOv5 with the point cloud projected onto the camera plane by the solid-state LiDAR, then extract and remove dynamic objects by clustering. Simultaneously, the remaining point cloud projected onto the image is saved as an image.

[0049] S6: When the robot begins relocalization, feature point extraction from S3 is performed first. Similarity is calculated between the constructed bag-of-words and the keyframes acquired in S5. If the number of matched feature points is less than threshold σ1 but greater than threshold σ2, point cloud registration is performed on the candidate frames matched in the previous step. The point clouds stored in the candidate frames are back-projected into 3D spatial points and compared with the current point cloud using ICP registration to calculate the score. The robot's coarse pose is obtained by obtaining the highest total score.

[0050] S61: Calculate the DBow vector of the current feature point and calculate its similarity with the DBow vectors of the historical records;

[0051] S62: If the matched points satisfy the coarse pose estimation, proceed to the next step;

[0052] S63: If the number of matched points is less than the threshold σ1 but greater than the threshold σ2, the point cloud recorded in the candidate frame calculated in S62 will be back-projected into the three-dimensional space. The point cloud of the current frame will be sequentially registered by ICP to calculate the optimal candidate frame and obtain the robot's coarse pose.

[0053] S7: After matching in S6, a coarse pose of the robot will be obtained. This coarse pose will then be sent to the solid-state LiDAR preprocessing module for local point cloud matching to obtain the precise robot pose.

[0054] This invention obtains prior dynamic objects through object detection using YOLOv5 deep learning; it extracts feature points from images, calculates the dynamic probability of each feature point using epipolar geometry and Bayes' theorem, discards dynamic feature points, and retains static feature points; it uses the DBOW algorithm to update the bag-of-words representation, with each image carrying the pose calculated by a solid-state LiDAR and calibrated using extrinsic parameters to obtain the camera pose, and also records the point cloud information of the static portion of that frame; a two-stage registration method is designed to improve the robustness of the system. During relocalization, the similarity between the feature points of the current frame and the previously stored feature points is calculated. If there are too few matching feature points, the point clouds in the candidate frames are back-projected for ICP registration to further improve the robustness of the system. This invention combines vision and LiDAR to improve the shortcomings of relocalization in dynamic environments.

[0055] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0056] This invention presents a camera-and-solid-state LiDAR fusion relocalization method for dynamic environments, solving the problem of relocalization failure in traditional LiDAR SLAM under dynamic conditions. It also enables autonomous relocalization of robots without human intervention. Furthermore, it achieves fast and accurate relocalization in real-world dynamic environments, improving the robustness of traditional SLAM in dynamic conditions. Attached Figure Description

[0057] Figure 1 This is a flowchart of the method of the present invention.

[0058] Figure 2 This is a flowchart of the pruning process of the present invention.

[0059] Figure 3 This is a schematic diagram of the polar geometry of the present invention.

[0060] Figure 4 This is a schematic diagram illustrating the relationship between probability and dynamic degree in this invention.

[0061] Figure 5 This is a schematic diagram illustrating the effect of dynamic feature point removal in this invention.

[0062] Figure 6 This is a schematic diagram illustrating the effect of dynamic feature point removal on the Technical University of Munich dataset.

[0063] Figure 7 This is a schematic diagram of the present invention before repositioning.

[0064] Figure 8 This is a schematic diagram of the repositioning process of the present invention. Detailed Implementation

[0065] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings. It should be understood that the described embodiments are only for explaining the invention and are not intended to limit the invention.

[0066] This invention presents a flowchart of a camera and solid-state LiDAR fusion relocation method for dynamic environments, as shown below. Figure 1 As shown, it includes the following steps.

[0067] S1: The extrinsic parameter calibration of the solid-state LiDAR and camera was performed using the Livox Camera Calib algorithm from the University of Hong Kong to obtain the transformation matrix from the camera coordinate system to the LiDAR coordinate system. The camera was calibrated using the calibration software that comes with the RealSense d455, the camera and radar were synchronized in time, image distortion was removed, and the distortion-removed image was obtained.

[0068] S2: YOLOv5 pruning flowchart as shown Figure 7 As shown, the pruned YOLOv5 is used to identify dynamic targets in the image, and the target bounding boxes of the dynamic objects are sent to the vision assistance module respectively.

[0069] S3: After the visual assistance module extracts feature points from the image, it needs to calculate the dynamic probability of these feature points. Points with a probability value greater than ω1 = 0.75 are considered interference points and removed. To reduce the impact of dynamic objects on the localization and mapping of laser SLAM, the point cloud of the current frame is projected onto the image, and the point clouds of dynamic objects within the target bounding boxes obtained in the previous step are clustered and removed during the mapping process. Experiments were conducted on datasets such as [example datasets would be inserted here]. Figure 4 As shown, this is the result on my own dataset, where the green dots are stable, static points. See... Figure 5 The image shows the validation performed on the renowned Technical University of Munich dataset. It can be seen that the probabilistic form preserves the points within the YOLO bounding box, further improving the robustness of the system. The red points in the image are the feature points preserved within the bounding box, while the green points are the stable feature points outside the bounding box.

[0070] S31: First, put the feature points outside the YOLOv5 bounding box into set P. Then, use the points in set P to match with the previous frame to obtain the corresponding matching points and put them into set M. Perform epipolar geometric constraint calculation on the paired points in set M and set P to obtain the F matrix.

[0071]

[0072] S32: Calculate the distance D from all feature points to the epipolar line using formula (7);

[0073]

[0074] S33: Assuming that the distance from a point to the polar line follows a standard normal distribution, substitute the D calculated in S32 into equation (8) to calculate the dynamic probability of the set of points;

[0075] S34: Calculate the weight ω, which represents the confidence level of the bounding boxes identified by the epipolar geometry model and YOLO, respectively. ω is calculated as follows:

[0076]

[0077] Where N c N represents the number of exterior points calculated from the epipolar geometry model. s This represents the number of points contained within the bounding box of YOLO.

[0078]

[0079] S35: Utilizing past and current information to evaluate and eliminate dynamic keypoints further enhances the effectiveness of dynamic point selection. Therefore, assuming the initial values ​​of the geometric probability of each point and the probability of points within the YOLOv5 bounding box are ω = 0.5, probability iteration is performed using equation (10). If the obtained probability is greater than ω1 = 0.75, it is added to the dynamic feature point set S.

[0080] S4: Solid-state LiDAR uses traditional SLAM methods to construct an environmental map and calculate the robot pose T. wL The camera pose in the world coordinate system is calculated based on the external parameters.

[0081] S5: The camera continuously acquires images and filters out key frames during the movement. Then, the DBow algorithm is used to record the key frames that have had dynamic feature points removed and update the bag of words. At the same time, the pose information calculated in S4 is incorporated, and the point cloud outside the dynamic box projected onto the image is also stored.

[0082] S51: The camera continuously acquires images and selects key frames during movement;

[0083]

[0084] S52: Use the DBow algorithm to record the keyframes from which dynamic feature points have been removed and update the bag of words. Also, derive the camera pose from the pose calculated by laser SLAM according to formula (13) and record it. In formula (13), T wc and T wL These are the camera's pose in the world coordinate system and the radar's pose in the world coordinate system, respectively. It is the extrinsic transformation matrix from camera to radar.

[0085] S54: Project the point cloud of the solid-state lidar in the same frame onto the plane of the camera using intrinsic and extrinsic parameters.

[0086] S55: Align the bounding boxes calculated by YOLOv5 with the point cloud projected onto the camera plane by the solid-state LiDAR, then extract and remove dynamic objects by clustering. Simultaneously, the remaining point cloud projected onto the image is saved as an image.

[0087] S6: When the robot starts relocalization, it first extracts the feature points from S3, and calculates the similarity between the constructed bag of words and the keyframes collected in S5. If the number of feature points that match the feature points is less than the threshold σ1 = 500 but greater than the threshold σ2 = 200, it will back-project the stored point cloud of the candidate frame that matches the current feature points into three-dimensional spatial points and perform ICP pairing with the current point cloud to calculate the score. Finally, the robot's coarse pose is the one with the highest total score.

[0088] S61: Calculate the DBow vector of the current feature point and calculate its similarity with the DBow vectors of the historical records;

[0089] S62: If the matched points satisfy the coarse pose estimation, proceed to the next step;

[0090] S63: If the number of matched points is less than the threshold σ1 = 500 but greater than the threshold σ2 = 200, the point cloud recorded in the candidate frame calculated in S62 will be back-projected into the three-dimensional space. The point cloud of the current frame will be sequentially registered by ICP to calculate the optimal candidate frame and obtain the robot's coarse pose.

[0091] S7: After matching in S6, a coarse pose of the robot will be obtained. This coarse pose will then be sent to the solid-state LiDAR preprocessing module for local point cloud matching to obtain the precise robot pose.

[0092] See attached Figure 6 and attached Figure 7 This comparison is to verify the effectiveness and speed of the relocation. Figure 6 This is what it looks like before relocation. You can see that the red dot on the right does not overlap with the white map dot. Figure 7 As can be seen, the red point cloud of the current frame has successfully overlapped with the white map points. Notice the WallTime values ​​below the two images; the relocalization process only takes 80ms. Furthermore, this map was formed after being disturbed by dynamic objects, yet relocalization was still achieved, fully demonstrating the reliability and practicality of this invention.

[0093] It should be emphasized that the embodiments described in this invention are illustrative rather than limiting. Therefore, this invention includes, but is not limited to, the embodiments described in the specific implementation schemes. Any other similar implementations derived by those skilled in the art based on the technical solutions of this invention also fall within the protection scope of this invention.

Claims

1. A camera and solid-state laser radar fusion relocation method for a dynamic environment, characterized in that, The method comprises: S1: the external parameter calibration of the solid-state laser radar and the camera obtains a coordinate transformation matrix of the camera to the radar wherein c represents a camera coordinate system, L represents a radar coordinate system, a calibration camera, the camera and the radar are time-synchronized, image distortion is removed, and a de-distorted image is obtained; S2: using the pruned YOLOv5 to identify dynamic targets in the image, and respectively sending the target frame of the obtained dynamic objects into a visual auxiliary module; S3: the visual auxiliary module extracts feature points of the image, and needs to calculate dynamic probabilities of the feature points, and regards the probability value as a larger interference point and eliminates it; in order to reduce the influence of dynamic objects on the positioning and mapping of laser SLAM, the point cloud of the current frame is projected onto the image, and the dynamic object point cloud in the target frame obtained in S2 is clustered, and is eliminated in the process of mapping; S4: Construct the environment map and calculate the robot pose using traditional SLAM method for solid-state laser radar , the pose of the camera in the world coordinate system is calculated according to the external parameter = ; S5: the camera continuously collects images in the process of movement and screens out key frames, then uses the DBow algorithm to record and update the bag-of-words of the key frames with the dynamic feature points eliminated, and simultaneously integrates the pose information, and stores the point cloud projected onto the image outside the dynamic frame; S6: When the robot starts to relocate, first perform the feature point extraction of S3, and use the constructed bag of words to calculate the similarity with the key frames collected in S5. If the matched feature points are less than the threshold but greater than the threshold , then perform point cloud registration in the candidate frames matched in the previous step; project the stored point cloud in the candidate frames into three-dimensional space points and calculate the score by ICP pairing with the current point cloud, and finally the total score of the highest is the coarse pose of the robot; S7: the matching of S6 obtains a rough pose of the robot, and then the rough pose is sent into a solid-state laser radar preprocessing module to match the local point cloud and obtain an accurate pose of the robot.

2. The camera and solid-state lidar fusion relocation method for dynamic environment according to claim 1, characterized in that: The pruned YOLOv5 in S2 is as follows: On the basis of the YOLOV5 framework, L1 regularization layer is applied to the scaling factor in batch normalization to achieve pruning and light weight, so it is easy to implement and does not need to change the existing architecture; (1) is the formula of BN layer, where and are the input and output of BN layer, respectively, B is the current smallest minibatch, and represent the mean and standard deviation of the input activation function on B, and are trainable affine transformation parameters, representing scale and shift, respectively; each channel activation size and coefficient are positively correlated, if is too small close to zero, then the activation value will also be very small, which in turn allows to eliminate the channels that tend to be close to zero; but normally, after training a network, the coefficients of BN layer are similar to a normal distribution, and is a histogram distribution with epochs during normal training, so the channels that tend to be close to zero are very few, so it is not possible to prune; Therefore, the value of the BN scaling factor is pushed close to zero through L1 regularization, so that the irrelevant channels can be identified, because each scaling factor corresponds to a specific convolution channel; (2) The first half of formula (2) is the standard CNN loss function part, and the latter half is the added regularization coefficient constraint, where λ is the balance factor, W represents the trainable weight, represents the training input and target, λ is reasonably adjusted according to the data set, and g(s)=|s| represents the L1 regularization; Due to the gradient problem caused by the introduction of L1 norm, smooth-L1 is used instead.

3. The camera and solid-state lidar fusion relocation method for dynamic environment according to claim 1, wherein: S3 is as follows: S31: extracting ORB feature points S311: when the gray value I(x) of more than N points around the point P in the image is greater than the threshold value epsilon than the gray value I(p) of the point P, the point is considered as a target corner point, which is specifically expressed as: (3) S312: the centroid is calculated to keep the direction of the feature points unchanged, the image pyramid is constructed by scaling the original image sequence by a certain ratio to keep the scale of the feature points unchanged; and the four-tree uniform algorithm is used to uniformly distribute the feature points in the image S313: calculating the BRIEF descriptor, the information around the feature point is described by binary; S32: Firstly, the fundamental matrix of epipolar geometry of all points outside the bounding box recognized by YOLO is calculated, and the fundamental matrix maps the feature points in the next frame to the corresponding search domain in the current frame, i.e. the epipolar line. Assuming P is a static point, for dynamic points, the distance between dynamic points and the epipolar line is D; let , respectively represent the matching points of the previous frame and the current frame, and , is in homogeneous coordinates, u, v are the pixel coordinates of the feature points in the image; (4) (5) The epipolar line is represented by I, and the calculation formula is: (6) Where X, Y, Z represent the epipolar vector, and F is the fundamental matrix; (7) Formula (7) represents that a certain pair of points falls in the current frame after the basis matrix transformation of the last frame, and the distance between it and the epipolar line I is D; for stable feature points, in the ideal state, after the basis matrix transformation, they will finally fall on the epipolar line I, that is, the distance of the point to the epipolar line is zero; due to the existence of moving objects, each feature point in the image is not strictly constrained on the corresponding epipolar line; the distance information is converted into probability information, and according to the above, it can be deduced that the longer the distance of the point to the epipolar line, the greater the possibility of the moving probability; S33: Dynamics is a continuous action, and it is unstable when processing a frame alone, so the Bayesian theorem is used to evaluate and eliminate dynamic key points using past and current information; it is assumed that the initial value of the dynamic probability of each point is ω and the distance from the matched key point to its corresponding polar line satisfies the Gaussian distribution and the probability iteration is consistent with the Markov property; S331: The moving probability of the key point is calculated using the normal probability density function, which is defined as: (8) adopting a standard normal distribution, representing a standard deviation with a value of 1 and an expectation of zero, wherein representing whether the i-th point preliminarily filtered by the epipolar geometry is static or dynamic, expressed as: (9) S332: In the present application, the probability propagation formula for a point is: (10) (11) wherein The update probability is from the geometric model, The update probability is from the bounding box of YOLO; S333: wherein are weights representing the confidence of the bounding boxes respectively identified by the polar geometric model and YOLO; is calculated in the following way: (12) wherein represents the number of outliers calculated by the epipolar geometry model, represents the number of points contained by the bounding box of YOLO; when the probability is greater than it will be determined as a dynamic point and will be excluded and no longer participate in the feature point pairing of relocation.

4. The camera and solid-state lidar fusion relocation method for dynamic environment according to claim 1, wherein: A solid-state laser radar constructs an environment map and calculates a robot pose using a conventional SLAM method , and calculates a pose of a camera in a world coordinate system according to an external parameter = .

5. The camera and solid-state lidar fusion relocation method for dynamic environment according to claim 1, wherein: The storage mode of camera and radar information in S5 is as follows: S51: The camera continuously collects images during movement and selects key frames therefrom; = (13) S52: record the key frame which has eliminated dynamic feature points and update the bag of words with DBow algorithm, and deduce the pose of the camera according to formula (13) based on the pose calculated by laser SLAM, and record it, wherein and are the pose of the camera in the world coordinate system and the pose of the radar in the world coordinate system respectively, is the extrinsic transformation matrix from the camera to the radar; S54: The point cloud of the solid-state laser radar in the same frame is projected onto the plane of the camera through the intrinsic and extrinsic parameters; S55: The recognition frame calculated by YOLOv5 is aligned with the point cloud projected onto the camera plane by the solid-state laser radar, and then the people are clustered and extracted and removed; at the same time, the remaining point cloud projected on the image is saved in the form of an image.

6. The camera and solid-state lidar fusion relocation method for dynamic environment according to claim 1, wherein: The repositioning matching in S6 is as follows: S61: The DBow vector of the current feature point is calculated, and the similarity between it and the historical DBow vector is calculated; S62: If the matched points meet the coarse pose estimation, the next step is entered; S63: If the matched points are less than the threshold But greater than the threshold Then the point cloud recorded in the candidate frame calculated in S62 is back-projected into the three-dimensional space, and the point cloud of the current frame is sequentially ICP registered to calculate the optimal candidate frame to obtain the coarse pose of the robot.

7. The camera and solid-state lidar fusion relocation method for dynamic environment according to claim 1, wherein The repositioning is as follows: the matching in S6 will obtain a coarse pose of the robot, and then the coarse pose will be sent to the solid-state laser radar preprocessing module for local point cloud matching to obtain an accurate robot pose.

Images