AGV positioning method and system based on 3D vision

Abstract

The application relates to the technical field of image processing, in particular to an AGV positioning method and system based on 3D vision, which comprises the following steps: acquiring images collected by AGV and constructing a source point cloud, performing iterative pose optimization, calculating the expected depth of a pixel point based on a current pose and a static map, calculating the depth residual and the uncertainty of the depth residual for each pixel point; fusing the two to determine the obstacle probability of the pixel point, performing 2D convolution on the obstacle probability to determine the spatial support factor; fusing the depth residual and the uncertainty and the spatial support factor through a fusion penalty model to determine the anti-occlusion confidence of the pixel point, taking the anti-occlusion confidence as a dynamic weight, performing a weighted ICP algorithm to solve a new pose, and outputting the positioning result of the AGV until convergence. The method suppresses the interference of dynamic obstacles and their edge artifacts on positioning, and improves the positioning accuracy of the AGV.

Description

Technical Field

[0001] This invention relates to the field of image processing technology. Specifically, it relates to an AGV positioning method and system based on 3D vision. Background Technology

[0002] In the production and manufacturing process of circuit breakers, AGVs (Automated Guided Vehicles) undertake high-frequency material handling tasks such as the transfer of key components, the loading of semi-finished products onto the production line, and the warehousing of finished products. AGVs are equipped with electromagnetic or optical automatic guidance devices, enabling them to travel along a prescribed guidance path. They have safety protection and various transfer functions. Under the command of the control system, they can automatically complete the entire process of picking up and placing materials and horizontal movement. AGV positioning refers to the process of determining the exact position and attitude of the AGV in the working environment or a pre-built map in real time. Accurate positioning is a prerequisite for achieving autonomous navigation and operation in complex environments.

[0003] Existing technologies typically rely on 3D vision for AGV positioning. These methods usually utilize 3D LiDAR or depth cameras to acquire source point clouds and use a pre-built static environment map as the target point cloud. The ICP (Iterative Closest Point) algorithm or its variants are used to iteratively match the source point cloud currently acquired by the sensor with the target point cloud to estimate the real-time pose of the AGV and achieve AGV positioning.

[0004] However, during the handling of circuit breaker parts on production lines or in warehouses, dynamic temporary obstacles often appear in the environment that are not included in the static map. Examples include circuit breaker parts accidentally dropped by AGVs while transporting materials, and temporarily stacked tooling equipment. When the AGV's 3D sensors detect these obstacles, the source point cloud collected by the sensors contains these 3D points that are not present in the static map. The core of the traditional ICP algorithm, when performing point cloud registration, is to indiscriminately find the closest Euclidean distance 3D point in the static map for each 3D point in the source point cloud as its corresponding point. Since obstacles do not have corresponding geometric entities in the static map, the algorithm may incorrectly bypass obstacles when processing 3D points on them, identifying 3D points on background objects behind the obstacles in the static map as the closest 3D points, thus establishing an incorrect matching relationship and attempting to fit the obstacles in the source point cloud into the background area of ​​the map. For example, it might match 3D points on temporarily stacked tooling equipment in the source point cloud with 3D points on walls or fixed shelves in the target point cloud.

[0005] The objective function of the ICP algorithm is to minimize the sum of matching errors between all mutually matching 3D points between the source point cloud and the target point cloud, and to perform iterative pose estimation. When such incorrect matching relationships exist, it will cause errors in the pose estimation of the AGV, which in turn will lead to inaccurate AGV positioning and affect the operating efficiency and safety of the AGV. Summary of the Invention

[0006] To address the problem that traditional ICP algorithms for AGV positioning are susceptible to interference from dynamic obstacles, leading to incorrect matching of 3D points of dynamic obstacles with background 3D points in a static map, resulting in errors in AGV pose estimation and consequently inaccurate AGV positioning, this invention proposes a 3D vision-based AGV positioning method and system.

[0007] In a first aspect, the present invention provides an AGV positioning method based on 3D vision, comprising:

[0008] Acquire the original image captured by the AGV and analyze the actual depth of each pixel. Construct a source point cloud through back projection and determine the corresponding pixel in the original image for each 3D point in the source point cloud. Obtain the initial pose estimate of the AGV as the current pose estimate and perform iterative pose optimization. Each iteration of pose optimization includes:

[0009] Based on the current pose estimation of the AGV and the static map, the expected depth of each pixel is determined. The depth residual and the uncertainty of the depth residual are calculated for each pixel according to the expected depth and the actual depth. The depth residual and the uncertainty of the depth residual are fused to determine the obstacle probability of the pixel.

[0010] The obstacle probabilities of all pixels are convolved in 2D to determine the spatial support factor of each pixel; the anti-occlusion confidence of each pixel is determined by fusing the depth residual and the uncertainty of the depth residual with the spatial support factor through a fusion penalty model.

[0011] The occlusion resistance confidence of the corresponding pixel in the original image for each 3D point in the source point cloud is used as the dynamic weight of that 3D point. The weighted ICP algorithm is executed based on the dynamic weights of all 3D points to obtain a new pose estimate and update it as the current pose estimate. The next iteration of pose optimization is then performed, and the current pose estimate when the iterative pose optimization meets the convergence condition is used as the localization result of the AGV.

[0012] This technical solution acquires the 3D point cloud corresponding to the original image observed by the AGV sensor and uses the initial pose estimation as the starting point for iteration. Then, an iterative pose optimization process begins. In each iteration, based on the current pose estimation and the static map, the expected depth of each pixel at its current position is determined. Subsequently, the solution compares the actual depth and expected depth of each pixel, calculates the physical difference between them, and transforms this physical difference into obstacle probability using a probabilistic model. A two-dimensional convolution operation is then used to analyze the spatial neighborhood characteristics of this probability, propagating obstacle attributes from their center to edge artifact regions, forming a coherent occlusion region criterion. Next, a fusion penalty model generates a final anti-occlusion confidence score for each pixel, serving as the dynamic weight for the corresponding 3D point. Finally, only these weighted 3D points are used to solve the pose, eliminating the impact of matching errors from 3D points originating from dynamic obstacles on localization, and retaining 3D points from the reliable background to dominate the pose estimation process, thus improving the accuracy of the AGV's localization results.

[0013] Optionally, the depth residual of each pixel is determined as follows: the difference between the expected depth and the actual depth of each pixel is used as the depth residual of that pixel.

[0014] Optionally, the uncertainty of the depth residual is determined as follows: the measurement variance of the actual depth of each pixel is obtained through the physical noise model of the 3D sensor, which is used as the measurement uncertainty of the actual depth; the pose covariance matrix of the current pose estimation is obtained, and the estimation uncertainty of the expected depth of each pixel is calculated through covariance propagation; the arithmetic square root of the sum of the measurement uncertainty of the actual depth and the estimation uncertainty of the expected depth is then used as the uncertainty of the depth residual of that pixel.

[0015] This technical solution clarifies two independent sources of uncertainty in depth residuals: one is the measurement uncertainty stemming from the physical characteristics of the sensors; the other is the estimation uncertainty caused by the inaccuracy of the AGV's current pose. Through covariance propagation, in the early stages of iteration, the pose covariance matrix is ​​large, and the system has a high tolerance for depth residuals, avoiding the erroneous elimination of static background points due to inaccurate initial poses. As the iteration converges, the system's tolerance for depth residuals decreases, making the system more sensitive and able to more accurately identify real obstacles, thus achieving statistical consistency throughout the optimization process.

[0016] Optionally, the obstacle probability of a pixel is determined by fusing the depth residual and its uncertainty, including: obtaining the depth residual and its uncertainty for each pixel; and performing a probability model calculation based on the depth residual and its uncertainty for each pixel and a preset positive bias value using a Gaussian error function to determine the obstacle probability of that pixel. The positive bias value is used in the probability model calculation to ensure that the input of the Gaussian error function is negative when the depth residual of the pixel is zero, so as to suppress the obstacle probability of the pixel.

[0017] This technical solution transforms depth residuals and their uncertainties into a statistically significant obstacle probability. It constructs a probabilistic model using a Gaussian error function. Whether a depth residual is considered an obstacle depends not only on its absolute value but also on its corresponding combination uncertainty, effectively avoiding misclassification of credible backgrounds as obstacles and improving the model's robustness. Optionally, 2D convolution is performed on the obstacle probabilities of all pixels to determine the spatial support factor for each pixel. This includes: obtaining the obstacle probabilities of all pixels to form an obstacle probability set; obtaining a normalized two-dimensional propagation kernel; performing a two-dimensional spatial convolution operation on the obstacle probability set using this kernel to obtain a convolution result image; and using the value at the same position as each pixel in the convolution result image as the spatial support factor for that pixel. The spatial support factor characterizes the probability that the neighborhood of that pixel is judged as an obstacle.

[0018] This technical solution performs a two-dimensional spatial convolution operation on the obstacle probability set and uses a normalized two-dimensional propagation kernel to calculate the spatial support factor of each pixel. The convolution kernel plays the role of probability propagation. Through the spatial convolution operation, the probability attributes of the obstacle are propagated from the center region of the obstacle to its edge artifact region, which helps to uniformly suppress the interference of the entire occluded area on the localization.

[0019] Optionally, the occlusion resistance confidence of a pixel is determined by fusing the depth residual and uncertainty of the depth residual, as well as the spatial support factor, of each pixel through a fusion penalty model. This includes: determining a regional linear penalty term by subtracting the spatial support factor from 1; determining a point Gaussian penalty term by applying a Gaussian penalty function to the depth residual and uncertainty of the pixel to characterize the significance of the depth residual of the pixel; and multiplying the regional linear penalty term by the point Gaussian penalty term to determine the occlusion resistance confidence of the pixel.

[0020] This technical solution constructs a dual penalty mechanism. It uses a region linear penalty term to reduce the confidence of pixels located in occluded areas, while retaining a point-based Gaussian penalty term to reduce the confidence of pixels with significant depth residuals. By fusing these two penalty terms, it ensures that only high-confidence pixels are used for subsequent localization calculations.

[0021] Optionally, the weighted ICP algorithm can be used to solve the problem based on the dynamic weights of all 3D points by minimizing the following weighted objective function:

[0022] ;

[0023] For rotation matrix, It is a translation vector. For the number of iterations, and The first The rotation matrix and translation vector obtained from the iteration optimization form the first... The new pose estimate obtained from the optimization in the second iteration is updated to the first... Used in the current pose estimate of the next iteration. It is the first The pose covariance matrix obtained from the subsequent iterations describes the uncertainty in pose estimation. To find the one that minimizes the sum of distances and , For source point cloud, for The A three-dimensional point, for Dynamic weights, For the first During the next iteration of optimization, In a static map, the corresponding 3D point is represented by ||, where || is the symbol for calculating distance. This is the symbol for matrix multiplication.

[0024] This technical solution performs point cloud matching through the dynamic weight of each 3D point. For 3D points that are identified as dynamic obstacles or edge artifacts, their dynamic weights approach 0, eliminating the impact of these mismatches on pose estimation. For 3D points that are identified as credible static backgrounds, their dynamic weights approach 1, and the true matching error is completely preserved, thus ensuring the accuracy of the localization results.

[0025] Optionally, determining the expected depth of each pixel based on the current pose estimation of the AGV and the static map includes: taking each position in the static map as a map point, transforming the map point from the coordinate system of the static map to the sensor coordinate system of the AGV through the inverse transformation of the current pose estimation, projecting the map point in the sensor coordinate system onto the two-dimensional image plane formed by the sensor coordinate system to obtain the pixel point corresponding to the map point and the projection depth value of the map point, and determining the minimum projection depth value of each pixel in the original image through Z-Buffering depth buffering logic, so as to use it as the expected depth of the pixel.

[0026] Optionally, performing a weighted iterative nearest point ICP algorithm solution also includes: calculating the pose covariance matrix of the current pose estimation, and feeding the pose covariance matrix back to the estimated uncertainty of the expected depth when updating the uncertainty of the depth residual of the pixel in the next iterative pose optimization.

[0027] This technical solution establishes a crucial statistical closed loop between pose estimation and uncertainty estimation. The solution clarifies that while obtaining a new pose estimate, the pose covariance matrix must be calculated simultaneously. The pose covariance matrix, as a key piece of information, is fed back into the next iteration to update the estimated uncertainty of the expected depth value on which the combined uncertainty is relied upon. This ensures that the algorithm uses the most reasonable statistical model in each iteration, thereby improving the convergence speed and stability.

[0028] Secondly, the present invention also provides an AGV positioning system based on 3D vision, the AGV positioning system including a memory and a processor, the memory storing a computer program, and the processor executing the computer program to implement the steps of any of the AGV positioning methods.

[0029] The present invention has the following effects:

[0030] This invention calculates an anti-occlusion confidence level that can characterize dynamic occlusion by comprehensively utilizing depth residuals, combined uncertainties, and spatial neighborhood information at the 2D depth map level. This confidence level is then mapped back to the 3D source point cloud as a dynamic weight, which can effectively identify and suppress the interference of dynamic obstacles and their edge artifacts on the ICP algorithm, thereby improving the accuracy of AGV positioning. Attached Figure Description

[0031] Figure 1 This is a flowchart of the present invention;

[0032] Figure 2 This is a flowchart of step S2 of the present invention. Detailed Implementation

[0033] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.

[0034] Reference Figure 1 An AGV positioning method based on 3D vision specifically includes the following steps:

[0035] S1: Acquire the original image collected by the AGV and analyze the actual depth of each pixel. Construct the source point cloud through back projection, determine the corresponding pixel in the original image for each 3D point in the source point cloud, and obtain the initial pose estimate of the AGV as the current pose estimate.

[0036] This step is the starting point of the entire positioning process. Its core task is to acquire all the original sensor observation data and establish the mapping relationship between 2D pixel coordinates and 3D spatial coordinates, so as to provide a data foundation for subsequent 2D-3D joint optimization.

[0037] Specifically, a frame of image captured by the 3D sensor mounted on the AGV, namely a 3D LiDAR or depth camera, is used as the raw image. The horizontal axis of the raw image is then plotted using... The vertical axis is represented by... express, These are the coordinates of pixels, used to resolve the original image into a 2D actual depth map. The actual depth map is derived from the coordinates of pixels in the original image. The depth values ​​of the pixels at a given location constitute the 3D source point cloud. In 3D vision, the depth value of a pixel typically represents the distance from the surface of the real-world object corresponding to that pixel to the sensor plane. Simultaneously, based on the sensor's intrinsic parameters, all valid depth values ​​in the actual depth map are back-projected to construct the 3D source point cloud. A source point cloud is a collection of point clouds composed of a large number of three-dimensional points. The first point cloud in the source cloud Each 3D point is back-projected to a pixel in the actual depth map, while preserving the correspondence between each 3D point in the source point cloud and the pixel in the actual depth map. For example, in the actual depth map, the pixel at (100, 150) has a depth value of 3.5 meters. ,storage The mapping relationship between the pixel at (100, 150) and the pixel at (100, 150).

[0038] Next, the initial pose estimate is obtained from the AGV's odometer. and the initial pose covariance matrix ,Will As the current pose estimate The iteration begins, where , It is a pair of symbols obtained from the AGV's odometer. This is the rotational component in the initial pose estimation, representing the initial attitude or orientation of the AGV. This is the translation component in the initial pose estimation, representing the initial position of the AGV. It is a pair of symbols. It is the first The rotation component in the nth iteration pose estimation represents the AGV's rotation in the nth iteration. The pose or orientation during the next iteration of pose estimation. It is the first The translation component in the nth iteration of pose estimation represents the AGV's translation in the nth iteration. Position during the next iteration of pose estimation.

[0039] S2: Perform one iteration of pose estimation optimization.

[0040] After obtaining the raw data and initial pose, the iterative pose optimization loop is entered. This loop is the core of the invention, and its purpose is to optimize the pose from an inaccurate initial pose through continuous iteration. To converge to a precise final pose, each pose optimization includes the following sub-steps:

[0041] Reference Figure 2 This optimization is a cyclical process. Until the pose estimation converges, each pose optimization is an iteration, including sub-steps S21 to S27:

[0042] S21: Expected depth based on AGV current pose estimation and static map synthesis.

[0043] In each iteration, the primary task is to establish a benchmark for comparison, which is based on the current pose estimation. and static maps The desired depth of each pixel in the original image is determined. The desired depth reflects what the sensor should see in the current pose, assuming there are no dynamic obstacles in the world.

[0044] In one embodiment, the desired depth map is synthesized based on the following method:

[0045] static map Each position in the map is treated as a map point, and the position is estimated using the current pose. The inverse transformation converts each map point to the AGV's sensor coordinate system, resulting in map points in the sensor coordinate system. For example, it transforms a static map... The first in Map points Map points converted to sensor coordinate system Map points in the sensor coordinate system Projecting the image onto the sensor's two-dimensional image plane, we obtain the coordinates and depth values ​​of the corresponding pixels on the map. Through Z-Buffering (depth buffering) logic, we determine the minimum depth value of each pixel in the two-dimensional image plane. Specifically, we initialize the expected depth of each pixel to infinity. We then iterate through all map points. For each map point, if the depth value of the pixel corresponding to the map point projected onto the sensor's two-dimensional plane is less than the expected depth of the pixel, we update the expected depth of the pixel to the depth of the pixel.

[0046] This step is functionally equivalent to a graphics rendering, simulating... In this pose, assuming no temporary obstacles in the world, this is the scene the sensor should see. For example, a fallen circuit breaker part 3 meters in front of the AGV creates a temporary obstacle that doesn't exist in the static map. 5 meters in front of the AGV (behind the temporary obstacle), there's a wall, a fixed structure of the warehouse that pre-exists in the static map. Thus, at the same pixel location on the surface of the temporary obstacle, two completely different depth values ​​are obtained. The depth value obtained from the actual depth map is 3 meters, which is what the sensor (such as a depth camera) actually observes; the sensor's line of sight is blocked by the obstacle, so it gets a depth value of 3 meters. The depth value obtained from the expected depth map is 5 meters because the static map doesn't consider such temporary obstacles and assumes only the fixed structure 5 meters in front of the AGV exists.

[0047] S22: For each pixel in the original image, calculate the depth residual and the uncertainty of the depth residual.

[0048] After obtaining the expected depth for each pixel, the difference between the actual depth and the expected depth is analyzed to reflect the discrepancy between the actual scene observed by the AGV and the expected scene. Therefore, this step is to determine this difference and evaluate its reliability.

[0049] By comparing the actual observed depth with the expected depth, the residual between the two is calculated. This residual is the key information for judging temporary obstacles. When there are no temporary obstacles, the residual between the actual observed depth and the expected depth is 0. When the residual is greater than 0, it indicates that there are temporary obstacles. In the subsequent positioning process, this key information is used to determine whether the AGV may be obstructed, so that it can be ignored during positioning to avoid being interfered with by temporary obstacles.

[0050] In one embodiment, the depth residual is determined based on the following:

[0051]

[0052] in, In the original image The depth residual of the pixel at that location. In the original image The expected depth of the pixel at that location. In the original image The actual depth of the pixel at that location.

[0053] Depth residual is the most direct physical quantity for determining obstacles; a significant positive residual indicates... This means that the actual observation point is closer than the expected observation point, which is consistent with the physical characteristics of dynamic obstacles.

[0054] In the original images captured by the AGV, pixels with larger depth residuals are more likely to match temporary obstacles. The more such pixels there are, the more likely temporary obstacles exist in the scene. On the other hand, pixels with smaller depth residuals are more likely to match fixed structures in the scene. The more such pixels there are, the less likely temporary obstacles exist in the scene.

[0055] After obtaining the depth residual of each pixel, it is further considered that a large depth residual does not necessarily mean an obstacle; it may also be caused by inaccurate pose estimation. Therefore, the uncertainty of the depth residual of each pixel is further obtained.

[0056] In one embodiment, the uncertainty of the depth residual for each pixel mainly comes from two aspects: one is the inaccuracy of the actual depth measurement, which is usually caused by physical noise in the sensor (depth camera) itself; the other is the deviation of the expected depth, which is because the expected depth is estimated based on an imperfect pose.

[0057] Therefore, the uncertainty of the depth residual of a pixel is determined by comprehensively considering these two aspects, specifically:

[0058] The measurement variance of the actual depth is obtained by using the physical noise model of the 3D sensor (depth camera). This variance represents the measurement uncertainty of the actual depth of each pixel. The larger the measurement variance, the greater the uncertainty of the measurement result. The measurement variance of a depth camera is usually proportional to the square of the actual depth value.

[0059] The estimated variance of the expected depth of a pixel is determined by the covariance between the current pose estimate of the AGV and the previous pose estimate (which reflects the uncertainty of the previous pose estimate). Specifically, the covariance (uncertainty of pose estimate) of the previous pose estimate is propagated to the expected depth of the pixel in the current pose estimate process through covariance propagation (e.g., using the Jacobian matrix), thus obtaining the estimated uncertainty of the expected depth of the pixel.

[0060] The measurement variance of the actual depth of a pixel and the estimated variance of the expected depth are added together, and the arithmetic square root is then taken as the uncertainty of the depth residual of the pixel.

[0061] This method is used to calculate the uncertainty of depth residuals because the uncertainty of depth residuals comes from the noise of actual measurements and the noise of expected rendering. Both must be considered simultaneously in order to accurately evaluate the significance of depth residuals. The arithmetic square root is used to make the uncertainty of depth residuals fit the subsequent Gaussian error function (the denominator is usually the standard deviation). By taking the arithmetic square root of the sum of the squares, a dimension in standard deviation format is obtained, which can be more reasonably used in subsequent analysis and calculation.

[0062] In this way, the uncertainty of the depth residual of a pixel becomes a dynamic feedback value that improves with each iteration. In the early stages of iteration, the pose estimation is inaccurate, and the covariance of the pose estimation is large, leading to significant uncertainty in the estimated depth and consequently, significant uncertainty in the depth residual of the pixel. At this point, the system has a high tolerance for the depth residual of the pixel, avoiding the erroneous removal of static background points due to inaccurate initial poses. As the iteration converges, the pose estimation gradually improves, and the covariance of the pose estimation slowly decreases, leading to a reduction in the uncertainty of the depth residual of the pixel. The system then becomes more sensitive in judging the depth residual of the pixel.

[0063] S23: Combine the depth residual of a pixel with the uncertainty of the depth residual to determine the obstacle probability of that pixel.

[0064] After obtaining the depth residual and its uncertainty, these two physical quantities are fused to calculate a statistically significant obstacle probability, reflecting how likely a pixel is to belong to an obstacle.

[0065] In one embodiment, the obstacle probability is determined by performing a probabilistic model calculation using a Gaussian error function:

[0066]

[0067] in, In the original image The pixel at point [0, 1] represents the posterior probability of the obstacle. It is a Gaussian error function. In the original image The depth residual of the pixel at that location. In the original image The uncertainty of the depth residual of the pixel at that location It is a preset positive bias value used to define the tolerable non-obstacle residuals. It is determined based on the sensor's physical noise model and the accuracy of the static map, and is set to... rice.

[0068] This formula is statistically equivalent to a one-sided Z-test, set as follows: In order to ensure that When approaching 0, The input is a negative number. It still approaches zero, meaning it's classified as background, rather than 0.5. hour, The closer the value is to 1, the more likely it is to be an obstacle.

[0069] S24: Perform 2D convolution on the obstacle probabilities of all pixels to determine the spatial support factor of each pixel.

[0070] Obstacle probabilities are calculated pixel by pixel, which may cause the obstacle probability of pixels in the artifact region at the edge of the obstacle to approach 0. To solve this problem, spatial neighborhood information is introduced, and the spatial support factor of the pixel is calculated by 2D convolution.

[0071] In one embodiment, the spatial support factor of a pixel is determined by performing a two-dimensional spatial convolution operation on the probability map formed by the obstacle probabilities of all pixels, and its calculation formula is as follows:

[0072]

[0073] in, In the original image The spatial support factor of a pixel represents the expected obstacle probability in the neighborhood of that pixel. It is a probability map composed of the obstacle probabilities of all pixels. It is a normalized 2D propagation kernel (a 5×5 mean kernel or Gaussian kernel) whose sum of all elements is 1. It is the window of the core. The index variables represent the summation process; their purpose is to traverse the kernel window. For each position within the image, this formula can be understood as a template overlay calculation, to calculate the original image within... The value of this point is determined by using a template (a 2D propagation kernel) and aligning its center with the original image. The position, then (m,n) here are the relative coordinates of the movement within the kernel window. When (m,n)=(0,0), the calculation... When (m,n)=(0,1), calculate This process continues, iterating through all points within the window. Finally, by adding up all these products, we obtain the final result. .

[0074] Through convolution operations It can propagate the probability value of the obstacle center to the edge artifact region. For example, at the obstacle center, Approaching 1, at the obstacle edge artifacts, even if the obstacle probability of a pixel is very small, approaching 0, the obstacle probability of pixels in its neighborhood is very high. After convolution, a large spatial support factor will be obtained, which helps subsequent steps to uniformly suppress the entire occlusion area.

[0075] S25: The occlusion resistance confidence of a pixel is determined by fusing the depth residual and uncertainty of the depth residual, as well as the spatial support factor, through a fusion penalty model.

[0076] After obtaining the spatial support factor of the pixel, it is used as the basis for judging the region occlusion. A fusion penalty model is constructed to generate the final anti-occlusion confidence score, which will be used as the weight of the weighted ICP.

[0077] In one embodiment, the anti-occlusion confidence of a pixel is determined by multiplying a region linear penalty term by a point-based Gaussian penalty term, calculated as follows:

[0078]

[0079] in, In the original image The occlusion resistance confidence of a pixel is [0,1]. The larger the occlusion resistance confidence, the closer it is to 1, indicating that the pixel is more likely to be a fixed structure in the scene and less likely to be a dynamic obstacle. Conversely, the smaller the occlusion resistance confidence, the closer it is to 0, indicating that the pixel is more likely to be a dynamic obstacle in the scene and less likely to be a fixed structure in the scene. In the original image The spatial support factor of a pixel represents the probability that the neighborhood of that pixel is identified as a dynamic obstacle. The larger the value, and the closer it is to 1, the more likely the pixel is supported by an obstacle region, and the more likely the pixel is to belong to a dynamic obstacle. In the original image The depth residual of a pixel represents the difference between the expected depth and the actual depth of that pixel. In the original image The uncertainty of the depth residual of the pixel at that location This is a scaling factor used to adjust the width of the Gaussian penalty, set to a multiple of the standard deviation of the Gaussian distribution. or These are standard settings, corresponding to tolerance for residuals within 95% or 99.7% confidence intervals, respectively. To ensure robustness, we select [the appropriate setting here]. .

[0080] in, It is a region-based linear penalty term, used to characterize the degree of occlusion in the neighborhood. It is a natural exponential function. Part of it is a point-based Gaussian penalty term, used to characterize the significance of the depth residual of that pixel. The function ensures that only positive residuals (obstacles) are penalized, and negative residuals (obstacle disappearance) are not penalized. This fusion penalty model implements double penalty:

[0081] The smaller the depth residual of a pixel, the more likely it is to be a pixel with a fixed structure in the scene. In this case, if the probability of obstacles for that pixel and its neighboring pixels is also lower, the support factor obtained after convolution will also be smaller. This leads to larger region linear penalty terms and point-based Gaussian penalty terms, both approaching 1, resulting in a higher confidence level for the pixel against occlusion, also approaching 1. Therefore, the confidence level is higher that the pixel is a pixel with a fixed structure in the scene. For example, the pixel might be located on a fixed wall or shelf.

[0082] If a pixel's depth residual is large, significantly greater than 0, and the obstacle probabilities of that pixel and its neighboring pixels are both high, approaching 1, then the spatial support factor obtained after convolution will also approach 1, and the region linear penalty term will approach 0. Simultaneously, because the depth residual is significantly greater than 0, the input to the point-based Gaussian penalty term is a large negative number, causing this term to also approach 0. Ultimately, the pixel's anti-occlusion confidence approaches 0, indicating that the pixel is doubly penalized with extremely low confidence, making it more likely to be located near dynamic obstacles.

[0083] If a pixel has a small depth residual but its neighboring pixels have a high probability of being an obstacle, the point-based Gaussian penalty term will approach 1 because the depth residual approaches 0. Since the neighboring pixels have a high probability of being an obstacle, after 2D convolution propagation, the spatial support factor of this pixel will still have a large value. Therefore, the region linear penalty term of this pixel will become very small. In the end, the anti-occlusion confidence of this pixel is still a low value, indicating that this pixel is mainly penalized by the region penalty term, and this pixel is more likely to be an edge pixel of an obstacle.

[0084] S26: Obtain the anti-occlusion confidence of the corresponding pixel in the original image for each 3D point of the source point cloud, and use it as the dynamic weight of that 3D point.

[0085] After calculating the confidence level of each pixel in the original image, the anti-occlusion confidence level of each pixel is transferred to the corresponding 3D point in the point cloud space through the correspondence between each pixel in the original image and each 3D point in the source point cloud as the dynamic weight of the 3D point. This results in a larger dynamic weight for 3D points in the source point cloud corresponding to fixed structures in the scene, and a smaller dynamic weight for 3D points corresponding to dynamic obstacles and their edge artifacts.

[0086] S27: Perform a weighted iterative nearest point ICP algorithm based on the dynamic weights of all 3D points to obtain a new pose estimate and pose covariance matrix.

[0087] After assigning dynamic weights to all 3D points, a weighted ICP algorithm is executed once to solve for a better new pose estimate that is no longer affected by obstacle points.

[0088] In one embodiment, this step is performed by minimizing the following weighted objective function:

[0089]

[0090] in, For the first The new pose estimate obtained in the nth iteration is used as the nth... The current pose estimate in the next iteration To find the rotation matrix that minimizes the sum of distances (Attitude) and translation vector (Location), Source Point Cloud The A three-dimensional point, for The occlusion resistance confidence is used here as Dynamic weights, For the first In the next iteration The corresponding 3D point in the static map The sign for calculating distance, This is the symbol for matrix multiplication. It is the first The pose covariance matrix is ​​obtained by the next iteration of optimization.

[0091] The design of this weighted objective function ensures that the optimizer completely ignores those parameters when calculating pose transformations. 3D points approaching zero are highly likely to be dynamic obstacles. Matching errors caused by these obstacle-related 3D points are avoided (because the error term is multiplied by 0), and matching is entirely dependent on... Three-dimensional points that approach 1 are highly likely to belong to fixed structures in the scene. By capturing the tiny errors of these three-dimensional points, the accuracy of the positioning results is ensured.

[0092] To obtain a new pose estimate At the same time, a new pose covariance matrix is ​​calculated. Solving the pose covariance matrix is ​​a standard mathematical step performed simultaneously with the weighted ICP algorithm for pose estimation. Its core is to obtain the uncertainty by constructing an information matrix and inverting it. The goal of the process is to minimize the weighted objective function to find the optimal pose. In the optimization iteration process, an information matrix is ​​first constructed based on all 3D points with dynamic weights. The size of this information matrix usually corresponds to the 6 degrees of freedom of the pose, reflecting how much constraint information all 3D points provide for the current pose estimation. This information matrix is ​​usually constructed in the form of a Jacobian matrix. The pose covariance matrix is ​​defined as the inverse of the information matrix. The larger the information matrix, the stronger the point cloud constraints. The more information there is, the smaller its inverse matrix, which represents the lower the uncertainty of the pose estimation.

[0093] pose covariance matrix Feedback will be provided in the next iteration, i.e., the [number]th iteration. During the next iteration, update the first... In the iteration, the estimation uncertainty relied upon when calculating the uncertainty of the depth residual of each pixel in the original image, i.e., the uncertainty in the 1st iteration... During the next iteration of pose estimation, This serves as the estimated uncertainty upon which the uncertainty in calculating the depth residual of each pixel in step S22 is based.

[0094] This feedback mechanism forms a statistically closed loop between localization optimization (solving pose) and uncertainty estimation. This closed loop ensures that the entire iterative process remains statistically consistent, allowing the uncertainty estimation to be dynamically optimized as the pose converges.

[0095] S3: Determine if the iteration has ended. If it has ended, output the current final pose estimate of the AGV to complete the accurate positioning of the AGV. If it has not ended, continue to the next iteration for pose estimation.

[0096] Will Update to current pose estimate, check and Is the transformation between them less than the preset convergence threshold? The convergence threshold should be scientifically set according to the positioning accuracy requirements of the AGV. For example, it can be set to a translation transformation of less than 1 mm and a rotation transformation of less than 0.01 degrees. If convergence is not achieved, return to step S21 and utilize... and Begin the next iteration of pose estimation. If the convergence condition is met, the iteration terminates, and the final pose estimate is obtained. As the positioning result of AGV Output, This is the final pose information of the AGV, including its final orientation, which is usually represented as a rotation matrix. It is the location information of the AGV, including the final position coordinates of the AGV on the map, which is usually represented as a translation vector.

[0097] This invention also provides an AGV positioning system based on 3D vision. The positioning system includes a processor and a memory. The memory stores computer program instructions, which, when executed by the processor, implement all or part of the steps of the AGV positioning method based on 3D vision in this application. The processor can be a central processing unit (CPU), a graphics processing unit (GPU), an application-specific integrated circuit (ASIC), or a field-programmable gate array (FPGA). Considering that this method involves a large amount of point cloud processing and image convolution operations, the processor is preferably a GPU to achieve parallel acceleration. The memory can include volatile memory (such as RAM) and non-volatile memory (such as ROM or flash memory).

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

Claims

1. An AGV positioning method based on 3D vision, characterized in that, include: The original image collected by the AGV is acquired and the actual depth of each pixel is analyzed. The source point cloud is constructed by back projection, and the pixel corresponding to each three-dimensional point in the source point cloud in the original image is determined. Obtain the initial pose estimate of the AGV as the current pose estimate, and perform iterative pose optimization. Each iteration of pose optimization includes: Based on the current pose estimation of the AGV and the static map, the expected depth of each pixel is determined. The depth residual and the uncertainty of the depth residual are calculated for each pixel according to the expected depth and the actual depth. The depth residual and the uncertainty of the depth residual are fused to determine the obstacle probability of the pixel. The obstacle probabilities of all pixels are convolved in 2D to determine the spatial support factor of each pixel; the anti-occlusion confidence of each pixel is determined by fusing the depth residual and the uncertainty of the depth residual with the spatial support factor through a fusion penalty model. The occlusion resistance confidence of the corresponding pixel in the original image for each 3D point in the source point cloud is used as the dynamic weight of that 3D point. The weighted ICP algorithm is executed based on the dynamic weights of all 3D points to obtain a new pose estimate and update it as the current pose estimate. The next iteration of pose optimization is then performed, and the current pose estimate when the iterative pose optimization meets the convergence condition is used as the localization result of the AGV.

2. The AGV positioning method according to claim 1, characterized in that, The depth residual of each pixel is determined as follows: the difference between the expected depth and the actual depth of each pixel is used as the depth residual of that pixel.

3. The AGV positioning method according to claim 1, characterized in that, The uncertainty of the depth residual is determined based on the following method: The measurement variance of the actual depth of each pixel is obtained through the physical noise model of the 3D sensor, which is used as the measurement uncertainty of the actual depth. The pose covariance matrix of the current pose estimation is obtained. Through covariance propagation, the estimation uncertainty of the expected depth of each pixel is calculated. The arithmetic square root of the sum of the measurement uncertainty of the actual depth and the estimation uncertainty of the expected depth is used as the uncertainty of the depth residual of the pixel.

4. The AGV positioning method according to claim 1, characterized in that, The obstacle probability of a pixel is determined by fusing the depth residual and its uncertainty. This includes: obtaining the depth residual and its uncertainty for each pixel; and performing a probability model calculation based on the depth residual and its uncertainty for each pixel, along with a preset positive bias value, using a Gaussian error function to determine the obstacle probability of that pixel. The positive bias value is used in the probability model calculation to ensure that the input of the Gaussian error function is negative when the depth residual of the pixel is zero, thereby suppressing the obstacle probability of that pixel.

5. The AGV positioning method according to claim 1, characterized in that, Perform 2D convolution on the obstacle probabilities of all pixels to determine the spatial support factor of each pixel, including: Obtain the obstacle probabilities of all pixels to form an obstacle probability set; obtain a normalized two-dimensional propagation kernel, and perform a two-dimensional spatial convolution operation on the obstacle probability set through the normalized two-dimensional propagation kernel to obtain a convolution result image. The value at the same position as each pixel in the convolution result image is used as the spatial support factor of the pixel. The spatial support factor is used to characterize the probability that the neighborhood of the pixel is judged as an obstacle.

6. The AGV positioning method according to claim 1, characterized in that, The occlusion resilience confidence of a pixel is determined by fusing the depth residual and uncertainty of the depth residual with the spatial support factor of each pixel using a fusion penalty model, including: A regional linear penalty term is determined by subtracting the spatial support factor from 1; a point Gaussian penalty term is determined by applying a Gaussian penalty function to the depth residual and its uncertainty of the pixel, to characterize the significance of the depth residual of the pixel; the regional linear penalty term is multiplied by the point Gaussian penalty term to determine the anti-occlusion confidence of the pixel.

7. The AGV positioning method according to claim 1, characterized in that, The weighted ICP algorithm, based on the dynamic weights of all 3D points, is solved by minimizing the following weighted objective function: , For rotation matrix, It is a translation vector. For the number of iterations, and The first The rotation matrix and translation vector obtained from the iteration optimization form the first... The new pose estimate obtained from the optimization in the second iteration is updated to the first... Used in the current pose estimate of the next iteration. It is the first The pose covariance matrix obtained from the subsequent iterations describes the uncertainty in pose estimation. To find the one that minimizes the sum of distances and , For source point cloud, for The A three-dimensional point, for Dynamic weights, For the first During the next iteration of optimization, In a static map, the corresponding 3D point is represented by ||, where || is the symbol for calculating distance. This is the symbol for matrix multiplication.

8. The AGV positioning method according to claim 1, characterized in that, Based on the current pose estimation of the AGV and the determination of the expected depth of each pixel using a static map, including: Each location in the static map is treated as a map point. Through the inverse transformation of the current pose estimation, the map point is transformed from the coordinate system of the static map to the sensor coordinate system of the AGV. The map point in the sensor coordinate system is projected onto the two-dimensional image plane formed by the sensor coordinate system to obtain the pixel point and the projection depth value of the map point. Through Z-Buffering depth buffering logic, the minimum projection depth value of each pixel in the original image is determined as the expected depth of the pixel.

9. The AGV positioning method according to claim 1, characterized in that, The process of solving the weighted ICP algorithm also includes: The pose covariance matrix of the current pose estimation is calculated, and the pose covariance matrix is ​​fed back to be used in the next iteration of pose optimization to update the uncertainty of the expected depth estimate on which the uncertainty of the depth residual of the pixel point is calculated.

10. An AGV positioning system based on 3D vision, characterized in that, The AGV positioning system includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps of the AGV positioning method as described in any one of claims 1-9.

Images