CN115718303A - A fusion relocalization method of camera and solid-state lidar for dynamic environment - Google Patents

Abstract

A camera and solid-state laser radar fusion relocation method facing a dynamic environment comprises the following steps: obtaining a priori dynamic object through target detection of deep learning YOLOv 5; extracting feature points of the image, calculating the dynamic probability of each feature point in a probability form by combining epipolar geometry and a Bayes formula, eliminating the dynamic feature points, and keeping static feature points; using a DBOW algorithm to represent and update word bags, wherein each image carries the pose calculated by the solid-state laser radar, the pose of the camera is obtained through external parameter calibration, and the point cloud information of the static part of the frame is recorded; and designing a two-stage registration mode to improve the robustness of the system, firstly carrying out similarity calculation on the feature points of the current frame and the previously stored feature points during relocation, and carrying out ICP registration on point cloud back projection in the candidate frame if the feature points are too little matched, so as to further improve the robustness of the system. The present invention combines vision and laser to ameliorate the disadvantages of repositioning in dynamic environments.

Description

A fusion relocalization method of camera and solid-state lidar for dynamic environment

technical field

The invention relates to the field of robot global positioning, in particular to a dynamic environment-oriented camera and solid-state laser radar fusion relocation method.

Background technique

The global positioning of the robot is a prerequisite for the robot to truly realize autonomous movement. The robot can quickly and accurately determine its own position after starting anytime and anywhere, which effectively improves the applicability and practicability of the robot; The ability to quickly re-recognize its current location in case of loss provides long-term stability and reliability for robot positioning.

In 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 the development of computer vision, visual SLAM (V-SLAM) has attracted the attention of many researchers and enterprises due to its low cost, low power consumption, and ability to provide rich landscape information. Although it has achieved excellent results in static environments, performance degradation and insufficient robustness in dynamic environments have become the main obstacles for its practical application. For example, the invention patent with the patent number CN110533722A proposes a fast relocation method and system for robots based on visual dictionaries, and the invention patent with the patent number CN110796683A proposes a relocation method based on visual features combined with laser SLAM, and the vision is sensitive to dynamic objects. The accuracy is much higher than that of sensors such as lidar. They use pure vision to achieve relocation in a static environment. It is a low-cost method, but when faced with a real environment, this method will fail. For example, the feature points of pedestrians, vehicles, etc.) are calculated together, which is nothing more than introducing interference into the system. Although the patent number CN110796683A will also record its own pose in the visual dictionary DBow and design some strategies, but for dynamic The interference of objects still does not help. The relocation method of laser radar and camera fusion, such as the invention patent of patent number CN112596064A, proposes an integrated indoor robot global positioning method for laser and vision fusion. This method simultaneously establishes a laser key frame retrieval library and a visual key frame retrieval library. Although the robustness It has been improved, but in the real dynamic environment, the movement of the object will cause the key frame to record the wrong feature points, and then when a large number of feature points change, it may cause the relocation to fail.

The current global positioning method for robots has the following defects and deficiencies: 1) The current method establishes a visual positioning map and a laser positioning map respectively, and then realizes the unification of the results of the two global positioning methods by aligning the two maps. However, this method is sensitive to the calibration accuracy of the two sensors, and it consumes a lot of resources to establish two maps respectively. 2) If only lidar is used for relocation, not only the point cloud registration time will be long, but also if there are scenes with similar structure or no structure, the relocation will fail. At present, some scholars will provide an initial pose for the robot to perform matching and repositioning in this range, but humans may not know the approximate position of the robot, and this is also human intervention, and there is no way to achieve a fully autonomous robot. 3) In the real environment, the existence of dynamic objects is inevitable. It will have an inestimable impact on relocation and even the entire SLAM system, and it is also one of the main problems hindering the further development of SLAM.

Contents of the invention

Aiming at the above-mentioned deficiencies in the prior art, the present invention provides a camera and solid-state laser radar fusion relocation method for dynamic environments, which can realize positioning in a dynamic real environment, and effectively combine solid-state laser radar and visual information at the same time, greatly The robustness of the positioning of the robot in a dynamic environment is greatly improved, and the repositioning does not require human intervention, and the speed is fast and the precision is high. Solid-state lidars are less expensive than spinning lidars and generate dense point clouds with a small FOV.

In order to achieve the above object, the present invention provides the following technical solution: a camera and solid-state lidar fusion relocation method for dynamic environments, comprising the following steps:

其中c代表相机坐标系，L代表雷达坐标系，标定相机，相机和雷达时间同步，去除图像畸变，获取去畸变后的图像；S1: External parameter calibration of solid-state lidar and camera to obtain the coordinate transformation matrix from camera to radar

Where c represents the camera coordinate system, L represents the radar coordinate system, calibrates the camera, synchronizes the camera and radar time, removes image distortion, and obtains the image after dedistortion;

S2: Use the pruned YOLOv5 to recognize the dynamic target in the image, and send the target frame of the obtained dynamic object to the visual assistance module;

The above pruning YOLOv5 is as follows:

Based on the YOLOV5 framework, the L1 regularization layer is applied to the scaling factor in batch normalization (BN) for pruning to achieve lightweight, so it is easy to implement and does not need to change the existing architecture.

Equation (1) is the calculation formula of the BN (Batch Normalization) layer, where z _in and z _out are the input and output of the BN layer respectively, B is the current minimum mini batch, μ _B and σ _B represent the input activation on B The mean and standard deviation of the function, γ and β are the trainable affine transformation parameters (scale and displacement). The activation size z _{out of} each channel is positively correlated with the coefficient γ. If γ is too small and close to zero, the activation value will be very small, which allows to eliminate channels (or neurons) where γ is close to zero. But under normal circumstances, after training a network, the coefficients of the BN layer are similar to a normal distribution. During normal training, γ is distributed in a histogram along with epochs, so there are very few channels where γ approaches zero, so there is no French pruning. Therefore, the value of the BN scaling factor should be pushed close to zero by L1 regularization, so that insignificant channels (or neurons) can be identified, because each scaling factor corresponds to a specific convolutional channel (or neuron in the fully connected layer Yuan).

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

S3: After the visual aid module extracts the feature points of the image, it needs to calculate the dynamic probability of the feature points, and regard the ones with higher probability values as interference points and remove them. In order to reduce the impact 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 point cloud of dynamic objects in the target frame obtained in S2 is clustered. Get him out.

The above dynamic probability calculation is as follows:

S31: Extract ORB feature points

S311: When the difference between the gray value I(x) of more than N points around the point P and the gray value I(p) of the point P in the image is greater than the threshold ε, the point is considered to be the target corner point, specifically expressed as:

S312: Maintain the direction invariance of the feature points by calculating the centroid, and form an image pyramid by scaling the original image sequence at a certain ratio to maintain the scale invariance of the feature points: at the same time, use the quadtree uniform algorithm to make the feature points evenly distributed in the image

S313: Calculate the BRIEF descriptor, and describe the information around the feature point through binary;

S32: First, calculate the fundamental matrix of the epipolar geometry of all points outside the bounding box identified by YOLO. The fundamental matrix maps the feature points in the next frame to the corresponding search domain in the current frame, that is, the epipolar line. Suppose P is Static point, P' is a dynamic point, and the distance between the dynamic point and the epipolar line is D. Let p ₁ and p ₂ denote the matching points of the previous frame and the current frame respectively, and P ₁ and P ₂ are homogeneous coordinate forms, and u and v are pixel coordinates of feature points in the image.

P ₁ =[u ₁ ,v ₁ ,1], P ₂ =[u ₂ ,v ₂ ,1] (4)

p ₁ =[u ₁ , v ₁ ], p ₂ =[u ₂ ,v ₂ ] (5)

The epipolar line is represented by I, and the calculation formula is:

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

Equation (7) indicates that a certain pairing point falls in the current frame after the basic matrix transformation in the previous frame, and the distance between it and the epipolar line I is D. For a stable feature point, in an ideal state, it will eventually fall on the epipolar line I after the basic matrix transformation, that is, the distance from 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 to be located on its corresponding epipolar line. Convert distance information into probability information. According to the above, it can be deduced that the longer the distance from the epipolar line, the greater the possibility of moving probability;

S33: Dynamic is a continuous action, and it is unstable when processing a frame alone. Therefore, using Bayesian theorem, it is an idea of using past and current information to evaluate and eliminate dynamic key points. It is assumed that the initial value of the dynamic probability of each point is ω and the distance from the matching key point to its corresponding epipolar line is Gaussian distribution and the probability iteration is in line with the Markov property.

S331: Use the normal probability density function to calculate the movement probability of the key point, which is defined as:

The standard normal distribution is adopted, δ represents the standard deviation, its value is 1, and the expectation is zero, where c _pi represents whether the i-th point preliminarily screened by the epipolar geometric method is static or dynamic, and the expression is:

S332: In the present invention, the probability propagation formula of a point is:

更新概率是来自几何模型中，

更新概率是来自YOLO的边界框内。in

The update probabilities are derived from the geometric model,

The update probabilities are from within the bounding box of YOLO.

S333: where ω is a weight representing the confidence of the polar geometry model and the bounding box identified by YOLO respectively. ω is calculated as:

where N _c represents the number of outliers calculated by the epipolar geometric model, and N _s represents the number of points contained by the bounding box of YOLO. When the probability is greater than ω ₁ , it will be judged as a dynamic point and will be removed from the pairing of feature points that no longer participate in relocation.

S4: The solid-state lidar uses the traditional SLAM method to construct the environment map and calculate the robot pose T _wL , and calculate the camera pose in the world coordinate system according to the external parameters

S5: The camera continuously collects images during the movement and screens out the key frames, then uses the DBow algorithm to record the key frames that have removed the dynamic feature points and update the bag of words, and at the same time incorporates the pose information, and also projects to The point cloud outside the dynamic frame on the image is stored;

S51: the camera continuously collects images during the movement and filters out key frames;

是相机到雷达的外参变换矩阵。S52: Use the DBow algorithm to record the key frames that have removed the dynamic feature points and update the bag of words, and deduce the pose of the camera from the pose calculated by the laser SLAM according to the formula (13) and record it, where the formula (13) T _wc and T _wL in 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 camera to radar.

S54: project the point cloud of the solid-state lidar in the same frame onto the plane of the camera through the internal reference and the external reference;

S55: Use the recognition frame calculated by YOLOv5 and the solid-state lidar to project the point cloud on the camera plane for alignment, and then cluster and extract dynamic objects and remove them. At the same time, the remaining point cloud projected on the image is saved as an image.

S6: When the robot starts to relocate, it first extracts the feature points of S3, and uses the constructed bag of words to calculate the similarity with the key frames collected in S5. If the matching feature points are less than the threshold σ ₁ but greater than the threshold σ ₂ , the point cloud registration will be performed in the candidate frames matched in the previous step. The point cloud stored in the candidate frame is back-projected into a three-dimensional space point and the current point cloud is registered with the ICP to calculate the score, and finally the rough pose of the robot is obtained with the highest total score;

S61: Calculate the DBow vector of the current feature point, and calculate the similarity between it and the DBow vector of the historical record;

S62: If the matched points meet the rough pose estimation, go to the next step;

S63: If the matched points are less than the threshold σ ₁ but greater than the threshold σ ₂ , the point cloud recorded in the candidate frame calculated in S62 will be back-projected into the three-dimensional space, and the point cloud of the current frame will be sequentially ICP configured. Quasi-calculate the optimal candidate frame to obtain the rough pose of the robot;

S7: After the matching in S6, a rough pose of the robot will be obtained, and then this rough pose will be sent to the solid-state lidar preprocessing module for local point cloud matching to obtain an accurate robot pose.

The present invention obtains a priori dynamic objects through the target detection of deep learning YOLOv5; extracts feature points from the image, calculates the dynamic probability of each feature point in the form of probability in combination with epipolar geometry and Bayesian formula, and eliminates dynamic feature points. Keep the static feature points; use the DBOW algorithm to represent and update the bag of words, each image will carry the pose calculated by the solid-state lidar through external parameter calibration to obtain the pose of the camera, and record the point cloud information of the static part of this frame ; Design two-segment registration to improve the robustness of the system. When relocating, the feature points of the current frame will be calculated with the previously stored feature points. If there are too few feature points matching , and also perform ICP registration on the point cloud back projection in the candidate frame to further improve the robustness of the system. The invention combines vision and laser to improve the disadvantage of relocation in dynamic environment.

Compared with prior art, the beneficial effect of the present invention is:

The present invention is a camera and solid-state lidar fusion relocation method oriented to a dynamic environment, which solves the problem of failure of traditional laser SLAM relocation in a dynamic environment, and also realizes the robot's autonomous relocation function without human intervention. At the same time, it can achieve fast and accurate relocation effects in real dynamic environments, and also improves the robustness of traditional SLAM in dynamic environments.

Description of drawings

Figure 1 is a flow chart of the method of the present invention.

Fig. 2 is a pruning flowchart of the present invention.

Fig. 3 is a schematic diagram of the antipole geometry of the present invention.

Fig. 4 is a schematic diagram of the relationship between probability and dynamic degree in the present invention.

Fig. 5 is a schematic diagram of the effect of the dynamic feature point elimination of the present invention.

Figure 6 is a schematic diagram of the effect of dynamic feature point removal on the data set of Technical University of Munich.

Fig. 7 is a schematic diagram of the present invention before relocation.

Fig. 8 is a schematic diagram of the present invention after relocation.

Detailed ways

In order to make the purpose, technical solution and advantages of the present invention clearer, the present invention will be further described below in conjunction with the accompanying drawings. It should be understood that the implementation examples described this time are only used to explain the present invention, not to limit the present invention.

The present invention is a flow chart of a camera and solid-state lidar fusion relocation method for dynamic environments, as shown in FIG. 1 , including the following steps.

用Realsense d455自带的标定软件对相机进行标定，相机和雷达时间同步，去除图像畸变，获取去畸变后的图像；S1: The external parameter calibration of solid-state lidar and camera is to use the Livox Camera Calib algorithm of the University of Hong Kong to obtain the transformation matrix from the camera coordinate system to the radar coordinate system

Use the calibration software that comes with Realsense d455 to calibrate the camera, synchronize the camera and radar time, remove image distortion, and obtain the image after dedistortion;

S2: The YOLOv5 pruning flow chart is shown in Figure 7. Use the pruned YOLOv5 to recognize the dynamic target in the image, and send the obtained target frame of the dynamic object to the visual assistance module;

S3: After the visual assistance module extracts the feature points of the image, it needs to calculate the dynamic probability of the feature points, and regard those whose probability value is greater than ω ₁ =0.75 as interference points and remove them. In order to reduce the impact 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 point cloud of dynamic objects in the target frame obtained in the previous step is clustered. Remove him from the process. I did experiments on the data set and on my own data set, as shown in Figure 4, this is the effect on my own data set, where the green points are stable static points. As shown in Figure 5, this is verified on the data set of the famous Technical University of Munich. It can be seen that the form of probability retains the points in the YOLO target frame to further improve the robustness of the system. The figure is shown in red The points are the feature points retained in the frame, and the green ones are the stable feature points outside the frame.

S31: First put the feature points outside the YOLOv5 frame into the set P, and then use the points in the P set to match with the previous frame to get the corresponding matching points into the M set, and pair the points of the M set and the P set. The F matrix is obtained by calculating the polar geometric constraints;

S32: Utilize formula (7) to calculate the distance D from all feature points to the epipolar line;

S33: Assume that the distance from the point to the epipolar line obeys standard normal distribution, and the D calculated in S32 is brought into formula (8) to calculate the aggregate dynamic probability of the point;

S34: Calculate the ω weight, which represents the confidence of the polar geometric model and the bounding box identified by YOLO respectively. ω is calculated as:

where N _c represents the number of outliers calculated by the epipolar geometric model, and N _s represents the number of points contained by the bounding box of YOLO.

S35: Use past and current information to evaluate and eliminate dynamic key points, and further strengthen the effectiveness of dynamic point screening. Therefore, it is assumed that the initial value of the geometric probability of each point and the probability of points in the YOLOv5 frame is ω=0.5, using the formula (10) Probability iteration If the obtained probability is greater than ω ₁ =0.75, add it to the set of dynamic feature points S

S4: The solid-state lidar uses the traditional SLAM method to construct the environment map and calculate the robot pose T _wL , and calculate the camera pose in the world coordinate system according to the external parameters

S5: The camera continuously collects images during the movement and screens out the key frames, then uses the DBow algorithm to record the key frames that have removed the dynamic feature points and update the word bag, and at the same time incorporate the pose information calculated in S4, The point cloud outside the dynamic frame projected onto the image will also be stored;

S51: the camera continuously collects images during the movement and filters out key frames;

是相机到雷达的外参变换矩阵。S52: Use the DBow algorithm to record the key frames that have removed the dynamic feature points and update the bag of words, and deduce the pose of the camera from the pose calculated by the laser SLAM according to the formula (13) and record it, where the formula (13) T _wc and T _wL in 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 camera to radar.

S54: project the point cloud of the solid-state lidar in the same frame onto the plane of the camera through the internal reference and the external reference;

S55: Use the recognition frame calculated by YOLOv5 and the solid-state lidar to project the point cloud on the camera plane for alignment, and then cluster and extract dynamic objects and remove them. At the same time, the remaining point cloud projected on the image is saved as an image.

S6: When the robot starts to relocate, it first extracts the feature points of S3, and uses the constructed bag of words to calculate the similarity with the key frames collected in S5. If the feature points matching the feature points are less than the threshold σ ₁ =500 but If it is greater than the threshold σ ₂ =200, it will match the candidate frame of the current feature point, back-project the stored point cloud into a three-dimensional space point and perform ICP pairing with the current point cloud to calculate the score, and finally the robot with the highest total score is the robot. rough pose of

S61: Calculate the DBow vector of the current feature point, and calculate the similarity between it and the DBow vector of the historical record;

S62: If the matched points meet the rough pose estimation, go to the next step;

S63: If the matched points are less than the threshold σ ₁ =500 but greater than the threshold σ ₂ =200, the point cloud recorded in the candidate frame calculated in S62 will be back-projected into the three-dimensional space, and the point cloud of the current frame will be Perform ICP registration in sequence to calculate the optimal candidate frame to obtain the rough pose of the robot;

S7: After the matching in S6, a rough pose of the robot will be obtained, and then this rough pose will be sent to the solid-state lidar preprocessing module for local point cloud matching to obtain an accurate robot pose.

You can see the comparison between attached drawing 6 and attached drawing 7, which is to verify the effect and speed of relocation. Figure 6 is the appearance before relocation. It can be seen that the red point on the right does not overlap with the white map point. When in In Figure 7, it can be seen that the red point cloud of the current frame has successfully overlapped with the white map point. It can be noticed that the WallTime time under the two pictures can be found that the relocation speed only needs 80ms to complete the relocation. Moreover, the map is formed after being disturbed by dynamic objects, and can still be relocated, which fully demonstrates the reliability and practicability of the present invention.

It should be emphasized that the embodiments described in the present invention are illustrative rather than restrictive, so the present invention includes and is not limited to the examples described in the specific implementation, and those skilled in the art according to the technology of the present invention Other implementations similar to those derived from the scheme also belong to the protection scope of the present invention.

Claims (7)

1. A camera and solid-state lidar fusion relocation method for dynamic environment, it is characterized in that, the method comprises: S1: External parameter calibration of solid-state lidar and camera to obtain the coordinate transformation matrix from camera to radar

Where c represents the camera coordinate system, L represents the radar coordinate system, calibrates the camera, synchronizes the camera and radar time, removes image distortion, and obtains the image after dedistortion; S2: Use the pruned YOLOv5 to recognize the dynamic target in the image, and send the target frame of the obtained dynamic object to the visual assistance module; S3: After the visual aid module extracts the feature points of the image, it needs to calculate the dynamic probability of the feature points, and regard the ones with higher probability values as interference points and remove them; in order to reduce the impact of dynamic objects on the positioning and mapping of laser SLAM, To this end, the point cloud of the current frame is projected onto the image and the point cloud of the dynamic object in the target frame obtained in S2 is clustered, and he is eliminated during the process of building the map; S4: The solid-state lidar uses the traditional SLAM method to construct the environment map and calculate the robot pose T _wL , and calculate the camera pose in the world coordinate system according to the external parameters

S5: The camera continuously collects images during the movement and screens out the key frames, then uses the DBow algorithm to record the key frames that have removed the dynamic feature points and update the bag of words, and at the same time incorporates the pose information, and also projects to The point cloud outside the dynamic frame on the image is stored; S6: When the robot starts to relocate, it first extracts the feature points of S3, and uses the constructed bag of words to calculate the similarity with the key frames collected in S5. If the matching feature points are less than the threshold σ ₁ but greater than the threshold σ ₂ , the point cloud registration will be performed in the candidate frame matched in the previous step; the point cloud stored in the candidate frame is back-projected into a three-dimensional space point and the current point cloud is used to perform ICP pairing to calculate the score, and finally the one with the highest total score is Coarse pose of the robot; S7: After the matching in S6, a rough pose of the robot will be obtained, and then this rough pose will be sent to the solid-state lidar preprocessing module for local point cloud matching to obtain an accurate robot pose. 2. A kind of dynamic environment-oriented camera and solid-state lidar fusion relocation method as claimed in claim 1, is characterized in that: the pruning YOLOv5 in S2 is specifically as follows: On the basis of the YOLOV5 framework, the L1 regularization layer is applied to the scaling factor in the batch normalization (BN) for pruning to achieve lightweight, so it is easy to implement and does not need to change the existing architecture;

Equation (1) is the calculation formula of the BN (Batch Normalization) layer, where z _in and z _out are the input and output of the BN layer respectively, B is the current minimum mini batch, μ _B and σ _B represent the input activation on B The mean and standard deviation of the function, γ and β are trainable affine transformation parameters (scale and displacement); the activation size z _out of each channel is positively correlated with the coefficient γ, if γ is too small and close to zero, then the activation The value will also be very small, allowing to eliminate channels (or neurons) where γ approaches zero; but under normal circumstances, after training a network, the coefficients of the BN layer are similar to a normal distribution, and during normal training γ is with The epochs are distributed in a histogram, so there are very few channels where γ approaches zero, so it cannot be pruned; therefore, the value of the BN scaling factor must be pushed to zero through L1 regularization, so that irrelevant channels can be identified ( or neurons), since each scaling factor corresponds to a specific convolutional channel (or neuron in a fully connected layer);

The first half of formula (2) is the standard CNN loss function part, and the second half is the constraint of the added regularization coefficient, where λ is the balance factor, W is the trainable weight, and l(x,y) is the training input and target , λ can be reasonably adjusted according to the data set, g(s)=∣s∣ represents L1 regularization; the gradient problem caused by the introduction of the L1 norm can be replaced by smooth-L1. 3. A kind of dynamic environment-oriented camera and solid-state lidar fusion relocation method as claimed in claim 1, is characterized in that: S3 is specifically as follows: S31: Extract ORB feature points S311: When the difference between the gray value I(x) of more than N points around the point P and the gray value I(p) of the point P in the image is greater than the threshold ε, the point is considered to be the target corner point, specifically expressed as:

S312: Maintain the direction invariance of the feature points by calculating the centroid, and form an image pyramid by scaling the original image sequence at a certain ratio to maintain the scale invariance of the feature points: at the same time, use the quadtree uniform algorithm to make the feature points evenly distributed in the image S313: Calculate the BRIEF descriptor, and describe the information around the feature point through binary; S32: First, calculate the fundamental matrix of the epipolar geometry of all points outside the bounding box identified by YOLO. The fundamental matrix maps the feature points in the next frame to the corresponding search domain in the current frame, that is, the epipolar line. Suppose P is Static point, P' is a dynamic point, and the distance between the dynamic point and the epipolar line is D; let p ₁ and p ₂ represent the matching points of the previous frame and the current frame respectively, and P ₁ and P ₂ are in the form of homogeneous coordinates , u, v are the pixel coordinates of the feature points in the image; P ₁ =[u ₁ ,v ₁ ,1], P ₂ =[u ₂ ,v ₂ ,1] (4) p ₁ =[u ₁ , v ₁ ], p ₂ =[u ₂ ,v ₂ ] (5) The epipolar line is represented by I, and the calculation formula is:

Among them, X, Y, and Z represent epipolar vectors, and F is the fundamental matrix;

Equation (7) indicates that a certain paired point falls in the current frame after the basic matrix transformation in the previous frame, and the distance between it and the epipolar line I is D; for stable feature points, in an ideal state, after the basic After the matrix transformation, it will eventually fall on the epipolar line I, that is, the distance from 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 to be located on its corresponding epipolar line; the distance information is transformed into It is probability information. According to the above, it can be deduced that the longer the distance from the epipolar line, the greater the possibility of moving probability; S33: Dynamic is a continuous action, and it is unstable when processing a frame alone. Therefore, using Bayesian theorem, it is an idea to evaluate and eliminate dynamic key points by using past and current information; assuming that every The initial value of the dynamic probability of a point is ω and the distance from the matching key point to its corresponding epipolar line satisfies the Gaussian distribution and the probability iteration conforms to the Markov property; S331: Use the normal probability density function to calculate the movement probability of the key point, which is defined as:

The standard normal distribution is adopted, δ represents the standard deviation, its value is 1, and the expectation is zero, where c _pi represents whether the i-th point preliminarily screened by the epipolar geometric method is static or dynamic, and the expression is:

S332: In the present invention, the probability propagation formula of a point is:

in

The update probabilities are derived from the geometric model,

Update probabilities are from within the bounding box of YOLO; S333: wherein ω is the confidence degree of the bounding box identified by the weight representing the pole geometry model and YOLO respectively; the calculation method of ω is:

Among them, N _c represents the number of outliers calculated by the epipolar geometric model, and N _s represents the number of points contained by the bounding box of YOLO; when the probability is greater than ω ₁ , it will be judged as a dynamic point and will be eliminated from participating in the Relocated feature point pairs. 4. A kind of dynamic environment-oriented camera and solid-state lidar fusion relocation method as claimed in claim 1, it is characterized in that: solid-state lidar uses traditional SLAM method to construct environment map and calculates robot pose _TwL , according to The external parameters calculate the pose of the camera in the world coordinate system

5. A kind of dynamic environment-oriented camera and solid-state lidar fusion relocation method as claimed in claim 1, it is characterized in that: in S5, the storage mode of camera and radar information is specifically as follows: S51: the camera continuously collects images during the movement and filters out key frames;

S52: Use the DBow algorithm to record the key frames that have removed the dynamic feature points and update the bag of words, and deduce the pose of the camera from the pose calculated by the laser SLAM according to the formula (13) and record it, where the formula (13) T _wc and T _wL in 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 camera to radar; S54: project the point cloud of the solid-state lidar in the same frame onto the plane of the camera through the internal reference and the external reference; S55: Use the recognition frame calculated by YOLOv5 and the solid-state lidar to project the point cloud on the camera plane for alignment, then extract the people clusters and remove them; at the same time, project the remaining point cloud on the image as an image form is preserved. 6. A kind of dynamic environment-oriented camera and solid-state lidar fusion relocation method as claimed in claim 1, is characterized in that: the relocation matching in S6 is specifically as follows: S61: Calculate the DBow vector of the current feature point, and calculate the similarity between it and the DBow vector of the historical record; S62: If the matched points meet the rough pose estimation, go to the next step; S63: If the matched points are less than the threshold σ ₁ but greater than the threshold σ ₂ , the point cloud recorded in the candidate frame calculated in S62 will be back-projected into the three-dimensional space, and the point cloud of the current frame will be sequentially ICP configured. Quasi-calculate the optimal candidate frame to get the rough pose of the robot. 7. A kind of dynamic environment-oriented camera and solid-state lidar fusion relocation method as claimed in claim 1, it is characterized in that relocation is specifically: the rough pose of a robot can be obtained through the matching of S6, and then this rough pose The pose will be sent to the solid-state lidar preprocessing module for local point cloud matching to obtain the precise pose of the robot.

Images