Factory agv positioning method based on single base station UWB and visual inertia

Abstract

The application discloses a factory AGV positioning method based on single base station UWB and visual inertia, S1: setting the origin of the base station coordinate system and defining a global coordinate system to obtain the starting coordinates of the AGV in the global coordinate system; S2: performing screening processing on a gray image, and then obtaining the pose between adjacent key frames; S3: pre-integrating the acceleration and angular velocity information obtained by the IMU sensor carried by the AGV; S4: establishing corresponding residual equations for the state quantities obtained by the monocular camera and the IMU sensor respectively, performing visual-inertial joint optimization, and then obtaining a visual-inertial pose; S5: a single base station UWB module carried by the AGV calculates the global UWB position information of the AGV; and S6: fusing the visual-inertial pose and the UWB position information, and then performing nonlinear optimization to obtain the position of the AGV. The method improves the positioning precision and robustness of the system.

Description

Technical Field

[0001] This patent relates to the field of AGV indoor navigation and positioning technology, specifically to a factory AGV positioning method based on single-base station UWB and visual inertia. Background Technology

[0002] With the rapid development of 5G and the Internet of Things (IoT), the demand for high-precision positioning is increasing. While global positioning and navigation systems (GPS) have become increasingly sophisticated and mature in outdoor environments, they struggle to meet the demand for high-precision positioning indoors due to factors such as building obstructions. Therefore, improving the quality of location services in indoor environments has become a major focus. Ultra-wideband (UWB) positioning technology stands out among indoor positioning technologies due to its low power consumption, high temporal resolution, strong multipath resistance, and high penetration capability. In more complex factory environments, facing challenges of low texture and high dynamics, achieving accurate positioning of a vehicle is even more difficult.

[0003] Currently, AGVs mainly include laser navigation, visual navigation, and visual inertial navigation that integrates GPS or UWB. (1) Laser navigation: Equipped with a multi-line laser sensor, it uses laser to scan the surrounding environment from various angles and directions to quickly and accurately obtain environmental information. (2) Visual inertial navigation: Equipped with a camera and IMU to obtain images and odometer information, it fuses the data from the two sensors to optimize the pose of the vehicle and achieve accurate navigation and positioning. (3) Visual inertial navigation that integrates GPS: The vehicle is equipped with a GPS signal receiving module. In outdoor open positioning scenarios, GPS signals are used as a global sensor to improve the positioning performance of the visual inertial navigation system. (4) Visual inertial navigation that integrates multi-base station UWB: Equipped with a UWB positioning module, in indoor scenarios where GPS signals are not ideal, it uses UWB positioning technology to obtain accurate UWB positioning information and fuses it to obtain high-precision and robust pose information.

[0004] Current AGV navigation technology still has problems: (1) It is easily affected by lighting and texture information. When the lighting and environment change drastically, the positioning performance of the system is affected; (2) It uses IMU and camera data for joint optimization. It is easily affected by IMU. After the positioning system of AGV has been running for a long time, the cumulative error continues to increase, resulting in trajectory drift; (3) GPS signals cannot provide accurate location information in indoor environments due to the obstruction of buildings. Therefore, GPS signals cannot be used as a fusion scheme in indoor environments; (4) Multi-base station UWB requires at least three to four base stations to be laid. The arrangement of base stations will affect the positioning effect. The location of the base stations needs to be known, so it has higher deployment costs and difficulties.

[0005] Therefore, it is necessary to develop a factory AGV positioning method based on single-base station UWB and visual inertia to improve the positioning accuracy and robustness of the system. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide a factory AGV positioning method based on single-base station UWB and visual inertial, which improves the positioning accuracy and robustness of the system.

[0007] To solve the above-mentioned technical problems, the technical solution of the present invention is as follows: a factory AGV positioning method based on single-base station UWB and visual inertial, the specific steps of which are as follows:

[0008] S1: Set the UWB positioning anchor point at a certain location in the positioning scene as the origin of the base station coordinate system, and define the coordinate system marked by the origin as the global coordinate system. Then calculate the starting coordinates of the AGV in the global coordinate system, and define the starting coordinates as the origin coordinates of the body coordinate system.

[0009] S2: The monocular camera acquires grayscale images, filters and processes the grayscale images, and then calculates the pose between adjacent keyframes.

[0010] S3: Pre-integrate the acceleration and angular velocity information acquired by the IMU sensor on the AGV to obtain the displacement, velocity and rotation between adjacent frames;

[0011] S4: Establish corresponding residual equations for the state variables obtained from the monocular camera and IMU sensor respectively, and obtain the visual-inertial pose after joint optimization of visual and inertial.

[0012] S5: The single-base station UWB module on the AGV vehicle calculates the global UWB position information of the AGV vehicle;

[0013] S6: The system backend subscribes to and fuses visual inertial pose and UWB position information, establishes an optimization equation, and obtains the position of the AGV vehicle after nonlinear optimization.

[0014] The above technical solution employs a random anchor point estimation algorithm to estimate and optimize the position of random anchor points. This method can estimate the position of randomly placed anchor points in an unknown environment, obtaining the vehicle's starting position in the global coordinate system. This avoids recalculating the base station position when entering a new environment. The vehicle acquires images through a monocular camera; the camera performs visual processing on the grayscale image; appropriate keyframes are selected based on parallax; the camera pose is calculated using reprojection error; the acceleration and angular velocity acquired by the IMU are pre-integrated; a residual function is established from the visual and IMU data for joint VI optimization; a bag-of-words model algorithm is used for loop closure detection; the single base station UWB module uses a TOA / AOA hybrid algorithm to calculate the vehicle position; and a loosely coupled optimization-based algorithm framework is proposed to jointly optimize the visual inertial pose and UWB coordinate information to obtain the optimal pose estimation. This method reduces the overhead of system computing resources, improves the system's robustness in complex environments, and reduces the cost and difficulty of deploying the UWB positioning system. Simultaneously, it proposes an optimization-based loosely coupled algorithm framework that uses UWB positioning information to provide global position constraints for the VINS, and uses the pose information of the VINS to eliminate the non-line-of-sight effects of UWB, thereby reducing the local cumulative error of the VINS system and improving the system's positioning accuracy.

[0015] Preferably, in step S1, the AGV uses the TOA / AOA algorithm to obtain k sets of distance and angle data from the AGV to the positioning anchor point in the initial stage, and then uses a random anchor point estimation algorithm to optimize and obtain the distance and angle information with the minimum residual, thereby obtaining the starting coordinates (x0, y0) of the AGV in the global coordinate system, and defining the starting coordinates as the origin coordinates of the body coordinate system; wherein the specific steps of using the random anchor point estimation algorithm to obtain the starting coordinates (x0, y0) of the AGV in the global coordinate system are as follows:

[0016] S11: Calculate the group distance and angle data [l0, l1, ..., l] using the TOA / AOA algorithm. k ,θ0,θ1,…,θ k ];

[0017] S12: Initialize and calculate a set of distances l0 and angles θ0 as initial estimates; where l a =l0,θ a =θ0, construct an optimization problem, the formula is: e a =||l i -l a ||+||θ i -θ a ||,i∈[1,k](1);

[0018] S13: Thus, the minimized loss function is obtained, as shown in the formula:

[0019]

[0020] Where ρ(·) is the Huber function, and the optimized distance l is obtained according to formulas (1) and (2). anchor and angle θ anchor And define the anchor point as the origin of the global coordinate system, so as to restore the initial global coordinates (x0, y0) of the car.

[0021] Preferably, in step S2, a grayscale image is first acquired using a monocular camera, the grayscale image is then filtered, and the pose between adjacent keyframes is determined through feature point extraction, triangulation, and the PnP algorithm (perspective n-point method). The specific steps are as follows:

[0022] S21: The monocular camera first performs preliminary processing on the grayscale image, and improves the image contrast through adaptive histogram equalization; through optical flow tracing, it calculates the fundamental matrix, removes outliers, and ensures that the extraction of feature points can be spaced at a safe distance.

[0023] S22: Then extract features from the grayscale image to obtain geometric point feature information in the grayscale image, and perform feature matching on the same feature points in the grayscale image to obtain matching feature point pairs;

[0024] S23: According to the principle of grayscale invariance, let the grayscale of a certain pixel be I(x, y, t). Then, after time dt, the grayscale of the pixel is expressed as: I(x+dx, y+dy, t+dt)=I(x, y, t); after a first-order Taylor expansion, the derivatives of the pixel with respect to time in the x and y directions of the two-dimensional plane are:

[0025]

[0026] Let dx / dt and dy / dt represent the velocities I / x and I / y of the pixel in the x and y directions, respectively, and let I / y represent the gradient of the pixel. Solving the equations yields the velocities v of the pixel in the x and y directions. x v y .

[0027] S24: The matching feature point pairs obtained in the initialization phase are used to calculate the three-dimensional coordinates of the landmark points using epipolar geometry. Then, the reprojection error formula is constructed to calculate the pose changes of adjacent frames from n pairs of 2D-3D relationships.

[0028] Preferably, the specific steps of step S24 are as follows:

[0029] S241: Let the spatial coordinates of point P in the coordinate system of the first frame be P = [x, y, z]. TTwo pixels on the two frames are p1 and p2. The projection formulas for these pixels are s1p1 = KP and s2p2 = K(RP + t). The normalized coordinates obtained are: x1 = KP. -1 p1, x2 = K -1 p2; Based on the formulas, the homogeneous relation is obtained by solving the system of equations: x2 = Rx1 + t; After transformation, x2t is obtained: x2 = x2tRx1; The epipolar constraint x2t^Rx1 = 0 is obtained;

[0030] S242: Let the fundamental matrix be E = t^R; based on the normalized coordinates of the matching feature point pairs and the epipolar constraint, establish a system of linear equations to obtain the fundamental matrix E; perform singular value decomposition on the fundamental matrix E to recover the corresponding R and t;

[0031] S243: Using the normalized pixel coordinates of the matching feature point pairs obtained between two frames and the extrinsic parameters R and t obtained in step S241, triangulation is used to calculate the depth of the pixel, thereby obtaining the three-dimensional coordinates of the spatial point.

[0032] S244: Obtain the 3D coordinates of the feature points through step S243, and then use the PnP algorithm to solve for the pose.

[0033] Preferably, the specific steps of step S244 are as follows:

[0034] Suppose there are n points P in three-dimensional space and their projections p. We want to calculate the camera pose R and t, and denote R and t as the transformation matrix T. Now assume a certain point P in space... i =[X i Y i Z i ] T Its projected coordinates are p i =[u i v i ] T The positional relationship between pixels and spatial points is obtained using the following formula:

[0035]

[0036] Abbreviated as s i u i =KTP i Since there is an error between the projection of a 3D point and its observed position, i.e., a reprojection error, we minimize this error function to construct a least squares problem, as shown in the formula:

[0037]

[0038] Using the camera pose and landmark points as optimization variables, this nonlinear optimization problem is solved using the Gauss-Newton method. Specifically, the error term is first linearized, and the partial derivative of the error term with respect to the optimization variables is calculated using the formula: e(x+Δx)≈e(x)+J T Δx is further differentiated to obtain the first-order partial derivative e / δξ describing the reprojection error with respect to the camera pose; at the same time, the partial derivative e / P of the reprojection error with respect to the spatial point P can be obtained; using the derivative of the error term in the reprojection error with respect to the two optimization variables of camera pose and spatial point, the gradient direction is provided in the least squares, and the optimization is iterated to obtain the optimal pose estimate.

[0039] Preferably, the specific steps of step S3 are as follows:

[0040] S31: Perform pre-integration processing on the obtained IMU sensor data to obtain the relative displacement P, velocity V, and rotation Q of the AGV in the body coordinate system;

[0041] S32: Perform pre-integration on the IMU sensor data to calculate the residual of the IMU sensor. The residual equation is abbreviated as δz. k+1 =Fδz k +VQ, where Q represents the noise term covariance matrix of each state vector, and F and V represent the given matrices;

[0042] S33: Based on the residual equations obtained in step S2, differentiate the state vectors to obtain their respective Jacobian matrices. The iterative formula for the Jacobian matrix is ​​denoted as: J k+1 =FJ k The iterative equation for covariance is: P k+1 =FP k F T +VQ k V T Update the state variables, Jacobian matrix, and covariance matrix Q.

[0043] Preferably, the specific steps for performing joint visual-inertial optimization in step S4 are as follows:

[0044] S41: First, synchronize the high-frequency (200Hz) IMU sensor data and the low-frequency (20Hz) visual information according to the timestamp, and store the visual measurement information and IMU sensor measurement information in the same queue container.

[0045] S42: The visual inertial system backend performs nonlinear optimization on the IMU and image data. The required state vector of the system is: in This indicates the displacement, velocity, rotation, acceleration deviation, and gyroscope deviation in the IMU sensor; λ represents the position and rotation quaternion in the visual information; λ represents the inverse depth when the feature is first observed.

[0046] S43: Using the Bundle Adjustment (BA) method, the sum of the Mahalanobis norms of all prior measurement residuals, visual measurement residuals, and IMU sensor measurement residuals is minimized to construct the optimization objective function, as shown in the formula:

[0047]

[0048] S44: For the objective function in step S3, perform an edge-shifting strategy summary, determine whether the next newest frame is a key frame. If it is a key frame, put the latest frame into the sliding window for optimization, edge-shift the oldest frame in the sliding window, and convert the landmark points it sees and the associated IMU sensor data into prior information and add it to the overall objective function; if the next newest frame is not a key frame, directly discard the observation edge of the next newest frame.

[0049] S45: After determining the next newest frame as a keyframe, update the dictionary and the inverse document frequency (IDF) of all features, and recalculate the translated frequency-inverse document frequency (TF-IDF) description vectors of all images; calculate the difference between the translated frequency-inverse document frequency (TF-IDF) values ​​of the image and the historical images, and find the minimum difference S. min If the minimum difference S min If the value is less than the set threshold, a loop is detected; otherwise, no loop is detected.

[0050] S46: The nonlinear optimization problem of constructing the formula for the positional relationship between pixels and spatial points using prior information obtained from edge detection, visual residual function, and IMU sensor residual function is solved using the Gauss-Newton method. The steps are as follows: Given an initial value x0; for the k-th iteration, calculate the current Jacobian matrix J(x k ) and error; solve the incremental equation J(x) k )J T (x k )Δx k =-J(x) k f(x) k If Δx k Stop if it reaches a very small value; otherwise, let x... k+1 =x k +Δx k Then, starting from the k-th iteration, the loop is repeated to obtain the optimized VIO pose.

[0051] It should be noted that "very small" here refers to if Δxk The optimization stops when the value is closest to a local minimum.

[0052] Preferably, the specific steps of step S5 are as follows:

[0053] S51: The AGV is equipped with a single base station UWB module. The signal arrival angle of the positioning anchor point is obtained using the antenna array AOA algorithm. Let the incident angle of the ultra-wideband signal be θ, then the direction of the i-th antenna is: Where θ0 represents the spacing angle between the antenna beamlines, and k represents a constant for the half-power beamwidth; assuming the half-power beamwidth is α, we obtain k as: Let antenna n be the one with the strongest received pulse amplitude, antenna n-1 be the second strongest, and pulse strength be A. Then the received pulse amplitude of the antennas is: definition Taking the logarithm of both sides, the signal arrival angle is:

[0054] S52: Using the TOA algorithm, the time t from the positioning anchor point to the single base station is obtained. The speed of light is c. Then the distance from the positioning anchor point to the base station is: d = ct.

[0055] S53: Use the TOA / AOA hybrid positioning algorithm to find the coordinates of the AGV vehicle. Set the positioning anchor point in the initialization process to the origin (0, 0). Based on the angle θ and distance d obtained in steps S51 and S52, the coordinates of the AGV vehicle are obtained as (dcosθ, dsinθ), thus obtaining the two-dimensional plane coordinates of the AGV vehicle.

[0056] S54: Perform error Kalman filtering on the obtained two-dimensional plane coordinates to reduce positioning errors and output the two-dimensional coordinates of the AGV.

[0057] Preferably, the specific steps of loosely coupling optimization in step S6, where the system backend subscribes to the VIO pose of the visual-inertial module and the global two-dimensional position information of the single-base station UWB module through a loosely coupled module, are as follows:

[0058] S61: Time synchronization is performed on the VIO pose information of the visual inertial system obtained by the system backend through the loosely coupled module subscription and the global two-dimensional position information measured by the single base station UWB module;

[0059] S62: Define the transformation matrix for transforming the VIO coordinate system to the UWB global coordinate system as follows: Transformation matrix Initialize to an elementary matrix;

[0060] S63: Assume that the poses of VIO at time i and time i+1 are respectively The VIO inter-frame transformation matrix is ​​obtained as follows Based on the rotation quaternions in the VIO pose at time i and i+1, calculate the transformation matrix. Use the optimized state variable X at time i i Multiplying by the transformation matrix yields the predictor variable at time i+1. Subtracting the optimized variable and the observed value at time i+1 yields the corresponding VIO residual.

[0061] S64: Assume the optimization variable for the UWB position at time i is X. i The observations of a single base station UWB module are as follows Due to the presence of random errors in the system, the UWB residual of a single base station UWB module is defined as:

[0062] S65: The VIO residual obtained in step S3 and the UWB residual obtained in step S4 are jointly optimized. The loosely coupled joint optimization equation is:

[0063]

[0064] By iterating through nonlinear optimization until the error is minimized, the optimal position estimate is obtained.

[0065] S66: Convert the rotation quaternion of the VIO pose into a rotation matrix R. vio The position information of VIO pose is converted into vector T. vio The combined 4×4 VIO transformation matrix is ​​T. vio Convert the rotation quaternion of the optimized pose into a rotation matrix R. uwb The optimized pose position information is converted into a vector T. uwb Combined into a 4×4 UWB transformation matrix T uwb Transformation matrix T vio Inverse and left-multiply by T uwb Then complete the coordinate system transformation matrix. The AGV's position is obtained by updating the position of the AGV. The above technical solution uses an optimized loosely coupled algorithm framework, employing ultra-wideband positioning (UWB) information to provide global position constraints for the VINS, reducing the cumulative error caused by a single VINS over long periods of operation. By ensuring the continuity of the VINS in the local environment, the impact of non-line-of-sight effects on the UWB system in complex environments is reduced, improving the system's positioning accuracy and robustness.

[0066] The technical problem that this invention also aims to solve is to provide an AGV indoor positioning and navigation system based on single-base station UWB and visual inertial, which improves the robustness of the system in complex environments and reduces the deployment cost and difficulty of the UWB positioning system.

[0067] To address the aforementioned technical problems, the technical solution of this invention is as follows: An AGV indoor positioning and navigation system based on single-base station UWB and visual inertial navigation includes an AGV vehicle, an IMU sensor, a monocular camera, a UWB single base station, and a UWB tag serving as a positioning anchor point; wherein the UWB single base station, monocular camera, and IMU are connected to the AGV vehicle, and the measured data are transmitted to the AGV vehicle; the positioning anchor point is placed at any position in the positioning scene, and the anchor point position is estimated by the AGV vehicle during the initialization phase; during the positioning process, the AGV vehicle measures the data from each sensor, transmits it to the ROS terminal of the AGV vehicle for processing and calculation, and finally outputs the motion trajectory of the AGV vehicle.

[0068] Compared with existing technologies, the beneficial effects of this invention are as follows: This factory AGV positioning method based on single-base station UWB and visual inertial avoids recalculating the base station position when entering a new environment, reducing the overhead of system computing resources, improving the robustness of the system in complex environments, and reducing the deployment cost and difficulty of the UWB positioning system; it uses an optimized loosely coupled algorithm framework, using the location information of ultra-wideband positioning to provide global position constraints for VINS, reducing the cumulative error caused by a single VINS during long-term operation; by reducing the continuity of VINS in the local environment, it reduces the impact of non-line-of-sight effects of the UWB system in complex environments, improving the positioning accuracy and robustness of the system. Attached Figure Description

[0069] The accompanying drawings are for illustrative purposes only and are not intended to limit the scope of this patent.

[0070] Figure 1 This is a positioning structure diagram of the single-base station UWB based factory AGV positioning method of the present invention, which is based on single-base station UWB and visual inertial.

[0071] Figure 2 This is a flowchart of the factory AGV positioning method based on single-base station UWB and visual inertial perception according to the present invention;

[0072] Figure 3 This is a flowchart of the random anchor point estimation algorithm for the factory AGV positioning method based on single-base station UWB and visual inertial according to the present invention;

[0073] Figure 4 This is a flowchart of the visual information processing of the factory AGV positioning method based on single base station UWB and visual inertia of the present invention;

[0074] Figure 5 This is the APE and positioning trajectory diagram of the fusion algorithm of the factory AGV positioning method based on single base station UWB and visual inertial in this invention. Detailed Implementation

[0075] To further illustrate the technical means and effects adopted by this patent to achieve the intended purpose of the invention, the following, in conjunction with the accompanying drawings and embodiments, provides a detailed description of the specific implementation, structure, features, and effects of the factory AGV positioning method based on single-base station UWB and visual inertia proposed by this invention.

[0076] Example: Figure 1 As shown, this factory AGV positioning system based on single-base station UWB and visual inertial measurement includes a robot car, an IMU sensor, a monocular camera, a UWB single base station, and a UWB tag as a positioning anchor point. The UWB single base station, monocular camera, and IMU are directly connected to the car, and the measured data are transmitted to the car. The positioning anchor point is placed at any position in the positioning scene, and the position of the anchor point is estimated by the car during the initialization phase. During the positioning process, the car measures the data from each sensor, transmits it to the ROS terminal of the car for processing and calculation, and finally outputs the motion trajectory of the car.

[0077] like Figure 2 As shown, the specific steps of this factory AGV positioning method based on single-base station UWB and visual inertial are as follows:

[0078] S1: Set the UWB positioning anchor point at any location in the positioning scene as the origin of the base station coordinate system, and define the coordinate system marked by the origin as the global coordinate system. Then calculate the starting coordinates of the AGV in the global coordinate system, and define the starting coordinates as the origin coordinates of the body coordinate system.

[0079] like Figure 3 As shown, in step S1, the AGV uses the TOA / AOA algorithm to obtain k sets of distance and angle data from the AGV to the positioning anchor point in the initial stage. Then, it uses a random anchor point estimation algorithm to optimize and obtain the distance and angle information with the minimum residual, thereby obtaining the starting coordinates (x0, y0) of the AGV in the global coordinate system, and defining the starting coordinates as the origin coordinates of the body coordinate system. The specific steps of obtaining the starting coordinates (x0, y0) of the AGV in the global coordinate system using the random anchor point estimation algorithm are as follows:

[0080] S11: Calculate the group distance and angle data [l0, l1, ..., l] using the TOA / AOA algorithm. k ,θ0,θ1,…,θ k ];

[0081] S12: Initialize and calculate a set of distances l0 and angles θ0 as initial estimates; where l a =l0,θ a =θ0, construct an optimization problem, the formula is: e a =||li -l a ||+||θ i -θ a ||,i∈[1,k](1);

[0082] S13: Thus, the minimized loss function is obtained, as shown in the formula:

[0083]

[0084] Where ρ(·) is the Huber function, and the optimized distance l is obtained according to formulas (1) and (2). anchor and angle θ anchor And define the anchor point as the origin of the global coordinate system, so as to restore the initial global coordinates (x0, y0) of the car.

[0085] S2: The monocular camera acquires grayscale images, filters and processes the grayscale images, and then calculates the pose between adjacent keyframes.

[0086] In step S2, a grayscale image is first acquired using a monocular camera, and then the grayscale image is filtered. Next, the pose between adjacent keyframes is calculated using feature point extraction, triangulation, and the PnP algorithm (perspective n-point method). The specific steps are as follows:

[0087] S21: The monocular camera first performs preliminary processing on the grayscale image, and improves the image contrast through adaptive histogram equalization; through optical flow tracing, it calculates the fundamental matrix, removes outliers, and ensures that the extraction of feature points can be spaced at a safe distance.

[0088] S22: Then extract features from the grayscale image to obtain geometric point feature information in the grayscale image, and perform feature matching on the same feature points in the grayscale image to obtain matching feature point pairs;

[0089] S23: According to the principle of grayscale invariance, let the grayscale of a certain pixel be I(x, y, t). Then, after time dt, the grayscale of the pixel is expressed as: I(x+dx, y+dy, t+dt)=I(x, y, t); after a first-order Taylor expansion, the derivatives of the pixel with respect to time in the x and y directions of the two-dimensional plane are:

[0090]

[0091] Let dx / dt and dy / dt represent the velocities I / x and I / y of the pixel in the x and y directions, respectively, and let I / y represent the gradient of the pixel. Solving the equations yields the velocities v of the pixel in the x and y directions. x v y .

[0092] S24: The matching feature point pairs obtained in the initialization phase are used to calculate the three-dimensional coordinates of the landmark points using epipolar geometry. Then, the reprojection error formula is constructed to calculate the pose change of adjacent frames from n pairs of 2D-3D relationships.

[0093] The specific steps of step S24 are as follows:

[0094] S241: Let the spatial coordinates of point P in the coordinate system of the first frame be P = [x, y, z]. T Two pixels on the two frames are p1 and p2. The projection formulas for these pixels are s1p1 = KP and s2p2 = K(RP + t). The normalized coordinates obtained are: x1 = KP. -1 p1, x2 = K -1 p2; Based on the formulas, the homogeneous relation is obtained by solving the system of equations: x2 = Rx1 + t; After transformation, x2t is obtained: x2 = x2tRx1; The epipolar constraint x2t^Rx1 = 0 is obtained;

[0095] S242: Let the fundamental matrix be E = t^R; based on the normalized coordinates of the matching feature point pairs and the epipolar constraint, establish a system of linear equations to obtain the fundamental matrix E; perform singular value decomposition (SVD) on the fundamental matrix E to recover the corresponding R and t;

[0096] S243: Using the normalized pixel coordinates of the matching feature point pairs obtained between two frames and the extrinsic parameters R and t obtained in step S241, triangulation is used to calculate the depth of the pixel, thereby obtaining the three-dimensional coordinates of the spatial point.

[0097] S244: Obtain the 3D coordinates of the feature points through step S243, and then use the PnP algorithm to solve for the pose.

[0098] The specific steps of step S244 are as follows:

[0099] Suppose there are n points P in three-dimensional space and their projections p. We want to calculate the camera pose R and t, and denote R and t as the transformation matrix T. Now assume a certain point P in space... i =[X i Y i Z i ] T Its projected coordinates are p i =[u i v i ] T The positional relationship between pixels and spatial points is obtained using the following formula:

[0100]

[0101] Abbreviated as s i u i =KTPi Since there is an error between the projection of a 3D point and its observed position, i.e., a reprojection error, we minimize this error function to construct a least squares problem, as shown in the formula:

[0102]

[0103] Using the camera pose and landmark points as optimization variables, this nonlinear optimization problem is solved using the Gauss-Newton method. Specifically, the error term is first linearized, and the partial derivative of the error term with respect to the optimization variables is calculated using the formula: e(x+Δx)≈e(x)+J T Δx is further differentiated to obtain the first-order partial derivative e / δξ describing the reprojection error with respect to the camera pose; at the same time, the partial derivative e / P of the reprojection error with respect to the spatial point P can be obtained; using the derivative of the error term in the reprojection error with respect to the two optimization variables of camera pose and spatial point, the gradient direction is provided in the least squares, and the optimization is iterated to obtain the optimal pose estimate.

[0104] S3: Pre-integrate the acceleration and angular velocity information acquired by the IMU sensor on the AGV to obtain the displacement, velocity and rotation between adjacent frames;

[0105] The specific steps of step S3 are as follows:

[0106] S31: Perform pre-integration processing on the obtained IMU sensor data to obtain the relative displacement P, velocity V, and rotation Q of the AGV in the body coordinate system;

[0107] S32: Perform pre-integration on the IMU sensor data to calculate the residual of the IMU sensor. The residual equation is abbreviated as δz. k+1 =Fδz k +VQ, where Q represents the noise term covariance matrix of each state vector, and F and V represent the given matrices;

[0108] S33: Based on the residual equations obtained in step S2, differentiate the state vectors to obtain their respective Jacobian matrices. The iterative formula for the Jacobian matrix is ​​denoted as: J k+1 =FJ k The iterative equation for covariance is: P k+1 =FP k F T +VQ k V T Update the state variables, Jacobian matrix, and covariance matrix Q;

[0109] like Figure 4As shown, S4: Establish corresponding residual equations for the state variables obtained from the monocular camera and IMU sensor respectively, and obtain the visual-inertial pose after joint optimization of visual and inertial.

[0110] The specific steps for the visual-inertial joint optimization in step S4 are as follows:

[0111] S41: First, synchronize the high-frequency (200Hz) IMU sensor data and the low-frequency (20Hz) visual information according to the timestamp, and store the visual measurement information and IMU sensor measurement information in the same queue container.

[0112] S42: The visual inertial system backend performs nonlinear optimization on the IMU and image data. The required state vector of the system is: in This indicates the displacement, velocity, rotation, acceleration deviation, and gyroscope deviation in the IMU sensor; λ represents the position and rotation quaternion in the visual information; λ represents the inverse depth when the feature is first observed.

[0113] S43: Using the Bundle Adjustment (BA) method, the sum of the Mahalanobis norms of all prior measurement residuals, visual measurement residuals, and IMU sensor measurement residuals is minimized to construct the optimization objective function, as shown in the formula:

[0114]

[0115] S44: For the objective function in step S3, perform an edge-shifting strategy summary, determine whether the next newest frame is a key frame. If it is a key frame, put the latest frame into the sliding window for optimization, edge-shift the oldest frame in the sliding window, and convert the landmark points it sees and the associated IMU sensor data into prior information and add it to the overall objective function; if the next newest frame is not a key frame, directly discard the observation edge of the next newest frame.

[0116] S45: After determining the next newest frame as a keyframe, update the dictionary and the inverse document frequency (IDF) of all features, and recalculate the translated frequency-inverse document frequency (TF-IDF) description vectors of all images; calculate the difference between the translated frequency-inverse document frequency (TF-IDF) values ​​of the image and the historical images, and find the minimum difference S. min If the minimum difference S min If the value is less than the set threshold, a loop is detected; otherwise, no loop is detected.

[0117] S46: The nonlinear optimization problem of constructing the formula for the positional relationship between pixels and spatial points using prior information obtained from edge detection, visual residual function, and IMU sensor residual function is solved using the Gauss-Newton method. The steps are as follows: Given an initial value x0; for the k-th iteration, calculate the current Jacobian matrix J(x k ) and error; solve the incremental equation J(x) k )J T (x k )Δx k =-J(x) k f(x) k If Δx k Stop if it reaches a very small value; otherwise, let x... k+1 =x k +Δx k Then, starting from the k-th iteration, the loop is repeated to obtain the optimized VIO pose;

[0118] S5: The single-base station UWB module on the AGV vehicle calculates the global UWB position information of the AGV vehicle;

[0119] The specific steps of step S5 are as follows:

[0120] S51: The AGV is equipped with a single-base station UWB module. The signal arrival angle of the positioning anchor point is obtained using the antenna array AOA algorithm. Let the incident angle of the ultra-wideband signal be θ, and the direction of the i-th antenna is obtained as follows: Where θ0 represents the spacing angle between the antenna beamlines, and k represents a constant for the half-power beamwidth; assuming the half-power beamwidth is α, we obtain k as: Let antenna n be the one with the strongest received pulse amplitude, antenna n-1 be the second strongest, and pulse strength be A. Then the received pulse amplitude of the antennas is: definition Taking the logarithm of both sides, the signal arrival angle is:

[0121] S52: Using the TOA algorithm, the time t from the positioning anchor point to the single base station is obtained. The speed of light is c. Then the distance from the positioning anchor point to the base station is: d = ct.

[0122] S53: Use the TOA / AOA hybrid positioning algorithm to find the coordinates of the AGV vehicle. Set the positioning anchor point in the initialization process to the origin (0, 0). Based on the angle θ and distance d obtained in steps S51 and S52, the coordinates of the AGV vehicle are obtained as (dcosθ, dsinθ), thus obtaining the two-dimensional plane coordinates of the AGV vehicle.

[0123] S54: Perform error Kalman filtering on the obtained two-dimensional plane coordinates to reduce positioning errors and output the two-dimensional coordinates of the AGV.

[0124] S6: The system backend subscribes to and fuses visual inertial pose and UWB position information, establishes an optimization equation, and performs nonlinear optimization to obtain the position of the AGV vehicle.

[0125] like Figure 5 As shown, the specific steps of loosely coupling optimization in step S6, where the system backend subscribes to the VIO pose of the visual-inertial module and the global two-dimensional position information of the single-base station UWB module through a loosely coupled module, are as follows:

[0126] S61: Time synchronization is performed on the VIO pose information of the visual inertial system obtained by the system backend through the loosely coupled module subscription and the global two-dimensional position information measured by the single base station UWB module;

[0127] S62: Define the transformation matrix for transforming the VIO coordinate system to the UWB global coordinate system as follows: Transformation matrix Initialize to an elementary matrix;

[0128] S63: Assume that the poses of VIO at time i and time i+1 are respectively The VIO inter-frame transformation matrix is ​​obtained as follows Based on the rotation quaternions in the VIO pose at time i and i+1, calculate the transformation matrix. Use the optimized state variable X at time i i Multiplying by the transformation matrix yields the predictor variable at time i+1. Subtracting the optimized variable and the observed value at time i+1 yields the corresponding VIO residual.

[0129] S64: Assume the optimization variable for the UWB position at time i is X. i The observations of a single base station UWB module are as follows Due to the presence of random errors in the system, the UWB residual of a single base station UWB module is defined as:

[0130]

[0131] S65: The VIO residual obtained in step S3 and the UWB residual obtained in step S4 are jointly optimized. The loosely coupled joint optimization equation is:

[0132]

[0133] By iterating through nonlinear optimization until the error is minimized, the optimal position estimate is obtained.

[0134] S66: Convert the rotation quaternion of the VIO pose into a rotation matrix R. vioThe position information of VIO pose is converted into vector T. vio The combined 4×4 VIO transformation matrix is ​​T. vio Convert the rotation quaternion of the optimized pose into a rotation matrix R. uwb The optimized pose position information is converted into a vector T. uwb Combined into a 4×4 UWB transformation matrix T uwb Transformation matrix T vio Inverse and left-multiply by T uwb Then complete the coordinate system transformation matrix. The update is used to obtain the position of the AGV, such as... Figure 5 As shown.

[0135] This technical solution proposes an optimization-based loosely coupled algorithm framework that jointly optimizes visual inertial pose and UWB coordinate information to obtain optimized position information. Using the EuRoC public dataset, the improved method is compared with other algorithms, and the comparison results are shown in Table 1 below.

[0136] Table 1 Comparison results of different algorithms

[0137] EuROC MH_01 0.186 0.178 0.102 EuROC MH_02 0.240 0.188 0.098 EuROC MH_03 0.271 0.260 0.207 EuROC MH_04 0.402 0.366 0.211 EuROC MH_05 0.388 0.291 0.191

[0138] The proposed method was tested on the EuRoC public dataset, and experimentally compared with the open-source frameworks VINS-Mono and VIR SLAM. The trajectory APE (Absolute Trajectory Error) and localization trajectory of the proposed method are shown below. Figure 5 As shown in Table 1, the positioning trajectory errors of the three frames were compared using the EVO evaluation tool. It can be seen from Table 1 that the method proposed in this invention (i.e., OURS) has smaller positioning errors and higher positioning accuracy and robustness compared with VINS-Mono and VIR SLAM.

[0139] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are 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. A factory AGV positioning method based on single-base station UWB and visual inertial, characterized in that, The specific steps are as follows: S1: Set the UWB positioning anchor point at a certain location in the positioning scene as the origin of the base station coordinate system, and define the coordinate system marked by the origin as the global coordinate system. Then calculate the starting coordinates of the AGV in the global coordinate system, and define the starting coordinates as the origin coordinates of the body coordinate system. S2: The monocular camera acquires grayscale images, filters and processes the grayscale images, and then calculates the pose between adjacent keyframes. S3: Pre-integrate the acceleration and angular velocity information acquired by the IMU sensor on the AGV to obtain the displacement, velocity and rotation between adjacent frames; S4: Establish corresponding residual equations for the state variables obtained from the monocular camera and IMU sensor respectively, and obtain the visual-inertial pose after joint optimization of visual and inertial. S5: The single-base station UWB module on the AGV vehicle calculates the global UWB position information of the AGV vehicle; S6: The system backend subscribes to and fuses visual inertial pose and UWB position information, establishes an optimization equation, and performs nonlinear optimization to obtain the position of the AGV vehicle. In step S1, the AGV uses the TOA / AOA algorithm to obtain k sets of distance and angle data from the AGV to the positioning anchor point in the initial stage. Then, it uses a random anchor point estimation algorithm to optimize and obtain the distance and angle information with the minimum residual, thereby obtaining the starting coordinates of the AGV in the global coordinate system. The initial coordinates are defined as the origin coordinates of the body coordinate system; wherein the initial coordinates of the AGV in the global coordinate system are obtained using a random anchor point estimation algorithm. The specific steps are as follows: S11: Calculate group distance and angle data using the TOA / AOA algorithm. ; S12: Initialize and calculate a set of distances and angle As the initial estimate; where , Let's construct an optimization problem with the following formula: (1); S13: Thus, the minimized loss function is obtained, as shown in the formula: （2）； in, The Huber function is used to obtain the optimized distance according to formulas (1) and (2). and angle And by defining the anchor point as the origin of the global coordinate system, the initial global coordinates of the car can be reconstructed. .

2. The factory AGV positioning method based on single-base station UWB and visual inertial as described in claim 1, characterized in that, In step S2, a grayscale image is first acquired using a monocular camera, and the grayscale image is then filtered. Next, the pose between adjacent keyframes is calculated using feature point extraction, triangulation, and the PnP algorithm. The specific steps are as follows: S21: The monocular camera first performs preliminary processing on the grayscale image, and improves the image contrast through adaptive histogram equalization; through optical flow tracing, it calculates the fundamental matrix, removes outliers, and ensures that the extraction of feature points can be spaced at a safe distance. S22: Then extract features from the grayscale image to obtain geometric point feature information in the grayscale image, and perform feature matching on the same feature points in the grayscale image to obtain matching feature point pairs; S23: According to the principle of grayscale invariance, let the grayscale of a certain pixel be... Then after The grayscale value of the pixel after time step 1 is represented as follows: The first-order Taylor expansion yields the time derivatives of this pixel in the x and y directions of the two-dimensional plane as follows: （3）； Reset , This indicates the velocity of the pixel in the x and y directions. , The gradient of the pixel is represented by the equation, and the velocity of the pixel in the x and y directions is obtained by solving the equation. , ; S24: The matching feature point pairs obtained in the initialization phase are used to calculate the three-dimensional coordinates of the landmark points using epipolar geometry. Then, the reprojection error formula is constructed to calculate the pose changes of adjacent frames from n pairs of 2D-3D relationships.

3. The factory AGV positioning method based on single-base station UWB and visual inertial as described in claim 2, characterized in that, The specific steps of step S24 are as follows: S241: Let the point in the coordinate system of the first frame be... Spatial coordinates are The two pixels on the two frames are , The formula for pixel projection is: , The normalized coordinates obtained from the calculation are as follows: , ; The homogeneous relation can be obtained by simultaneously solving the formulas: ; after transformation, obtain ; obtain epipolar constraints ; S242: Let the fundamental matrix be: Based on the normalized coordinates and epipolar constraints of the matched feature point pairs, a system of linear equations is established to obtain the fundamental matrix. E For the fundamental matrix E Perform singular value decomposition to recover the corresponding R,t ; S243: Normalized pixel coordinates of the matching feature point pairs obtained between the two frames and the extrinsic parameters obtained in step S241. , Triangulation is used to calculate the depth of a pixel, thereby obtaining the three-dimensional coordinates of the point in space. S244: Obtain the 3D coordinates of the feature points through step S243, and then use the PnP algorithm to solve for the pose.

4. The factory AGV positioning method based on single-base station UWB and visual inertial as described in claim 3, characterized in that, The specific steps of step S244 are as follows: Assume there are n three-dimensional points. and its projection We want to calculate the camera pose. , ,Will , The combination is denoted as the transformation matrix. Let's assume a certain spatial point Its projected coordinates are The positional relationship between pixels and spatial points is obtained using the following formula: （4）； Abbreviated as Since there is an error between the projection of a 3D point and its observed position, i.e., a reprojection error, we minimize this error function to construct a least squares problem, as shown in the formula: （5）； Using the camera pose and landmark points as optimization variables, this nonlinear optimization problem is solved using the Gauss-Newton method. Specifically, the error term is first linearized, and the partial derivative of the error term with respect to the optimization variables is calculated using the following formula: Further differentiation of this nonlinear optimization problem yields the first-order partial derivative describing the reprojection error with respect to the camera pose. Simultaneously, the partial derivative of the reprojection error with respect to spatial point P can be calculated. The gradient direction is provided in the least squares method by using the derivative of the error term in the reprojection error with respect to the two optimization variables of camera pose and spatial point, and iterative optimization is performed to obtain the best pose estimate.

5. The factory AGV positioning method based on single-base station UWB and visual inertial as described in claim 3, characterized in that, The specific steps of step S3 are as follows: S31: Perform pre-integration processing on the obtained IMU sensor data to obtain the relative displacement P, velocity V, and rotation Q of the AGV in the body coordinate system; S32: Perform pre-integration on the IMU sensor data to calculate the residual of the IMU sensor. The residual equation is abbreviated as: ,in This represents the noise term covariance matrix of each state vector. , Represents a given matrix; S33: Based on the residual equations obtained in step S2, differentiate the state vectors to obtain their respective Jacobian matrices. The iterative formula for the Jacobian matrix is ​​denoted as: The iterative equation for covariance is: Update the state variables, Jacobian matrix, and covariance matrix. .

6. The factory AGV positioning method based on single-base station UWB and visual inertial as described in claim 5, characterized in that, The specific steps for the visual-inertial joint optimization in step S4 are as follows: S41: First, synchronize the high-frequency IMU sensor data and the low-frequency visual information according to the timestamp, and store the visual measurement information and the IMU sensor measurement information in the same queue container. S42: The visual inertial system backend performs nonlinear optimization on the IMU and image data. The required state vector of the system is: ,in This indicates the displacement, velocity, rotation, acceleration deviation, and gyroscope deviation in the IMU sensor; Represents position and rotation quaternions in visual information; Indicates the inverse depth at which the feature is first observed; S43: Using the bundle adjustment method, minimize the sum of the Mahalanobis norms of all prior measurement residuals, visual measurement residuals, and IMU sensor measurement residuals to construct the optimal objective function, as shown in the formula: （6）； S44: Summarize the objective function in step S3 by marginalization strategy, determine whether the next newest frame is a key frame. If it is a key frame, put the latest frame into the sliding window for optimization, marginalize the oldest frame in the sliding window, and convert the landmark points and associated IMU sensor data seen by it into prior information and add it to the overall objective function. If the next new frame is not a key frame, discard the observation edge of the next new frame directly; S45: After determining that the next newest frame is a keyframe, update the dictionary and the inverse document frequency (IVF) of all features, and recalculate the translated frequency-inverse document frequency (IVF) description vector for all images; calculate the difference between the translated frequency-inverse document frequency (IVF) values ​​of the image and the historical images, and find the smallest difference. If the minimum difference If the value is less than the set threshold, a loop is detected; otherwise, no loop is detected. S46: The nonlinear optimization problem of constructing the formula for the positional relationship between pixels and spatial points using prior information obtained from edge detection, visual residual function, and IMU sensor residual function is solved using the Gauss-Newton method. The steps are as follows: Given initial values... For the first k In the next iteration, the current Jacobian matrix is ​​obtained. Sum of errors; solve the incremental equations ;like Stop if it reaches a very small value; otherwise, let... Then proceed from the first k The next iteration begins the loop, resulting in the optimized VIO pose.

7. The factory AGV positioning method based on single-base station UWB and visual inertial as described in claim 5, characterized in that, The specific steps of step S5 are as follows: S51: The AGV is equipped with a single-base station UWB module, and obtains the signal arrival angle of the positioning anchor point by using the antenna array AOA algorithm; let the incident angle of the ultra-wideband signal be... The direction of the i-th antenna is obtained as follows: ,in Indicates the spacing angle of the antenna beam axis. k A constant representing the half-power beamwidth; let the half-power beamwidth be... ,get k for: ; Let the antenna with the strongest received pulse amplitude be... The second strongest antenna is an antenna. If the pulse intensity is A, then the amplitude of the received pulse by the antenna is: , ;definition ; Taking the logarithm of both sides, the signal arrival angle is: ; S52: Using the TOA algorithm to obtain the time t from the positioning anchor point to a single base station, and the speed of light is c, the distance from the positioning anchor point to the base station is: ; S53: Use the TOA / AOA hybrid positioning algorithm to calculate the coordinates of the AGV, and set the positioning anchor point during the initialization process as the origin. The angle obtained according to steps S51 and S52 and distance The coordinates of the AGV are obtained as follows: Thus, the two-dimensional planar coordinates of the AGV are obtained; S54: Perform error Kalman filtering on the obtained two-dimensional plane coordinates to reduce positioning errors and output the two-dimensional coordinates of the AGV.

8. The factory AGV positioning method based on single-base station UWB and visual inertial as described in claim 7, characterized in that, The specific steps for loose coupling optimization in step S6, where the system backend subscribes to the VIO pose of the visual-inertial module and the global two-dimensional position information of the single-base station UWB module through a loosely coupled module, are as follows: S61: Time synchronization is performed on the VIO pose information of the visual inertial system obtained by the system backend through the loosely coupled module subscription and the global two-dimensional position information measured by the single base station UWB module; S62: Define the transformation matrix for transforming the VIO coordinate system to the UWB global coordinate system as follows: Transformation matrix Initialize to an elementary matrix; S63: Assumption time, The poses of VIO at time points are respectively , The VIO inter-frame transformation matrix is ​​obtained as follows: ,according to time, Calculate the transformation matrix from the rotation quaternions in the VIO pose at time t. ,use Optimized state variables at time step Multiply by the transformation matrix to obtain Predictors at time ,Will The VIO residual is obtained by subtracting the optimization variable from the observed value at time step 1. ; S64: Assumption The optimization variables for the UWB position at time t are The observations of a single base station UWB module are as follows Since the system contains random errors, the UWB residual of a single base station UWB module is defined as: ; S65: The VIO residual obtained in step S3 and the UWB residual obtained in step S4 are jointly optimized. The loosely coupled joint optimization equation is: （7）； By iterating through nonlinear optimization until the error is minimized, the optimal position estimate is obtained. S66: Convert the rotation quaternion of the VIO pose into a rotation matrix. The position information of VIO pose is converted into a vector. , combined into The VIO transformation matrix is Convert the rotation quaternion of the optimized pose into a rotation matrix. The optimized pose position information is converted into a vector. , combined into UWB transformation matrix Transformation matrix Find the inverse and multiply on the left Then complete the coordinate system transformation matrix. The update is used to obtain the position of the AGV.

Images