Positioning calibration method based on UWB and vision and related equipment

Abstract

The invention relates to the technical field of positioning calibration, and provides a UWB and vision-based positioning calibration method and related equipment. The method comprises the following steps: when a calibration period arrives, acquiring UWB coordinates and visual coordinates of a target object at a plurality of moments; preprocessing all the UWB coordinates by using the infinite Kalman filtering to obtain final UWB coordinates at all moments; when the target object is not calibrated, calibrating the final UWB coordinate and the visual coordinate at the current moment according to all the final UWB coordinates and all the visual coordinates to obtain a first calibration coordinate of the target object at the current moment; and when the target object is calibrated, re-calibrating and smoothing the final UWB coordinate and the visual coordinate at the current moment to obtain a re-calibrated coordinate of the target object at each future time point in the future time period. According to the method, the positioning calibration precision can be improved, and positioning jump is avoided.

Description

Technical Field

[0001] This application relates to the technical field of positioning and calibration, and particularly to a positioning and calibration method and related devices based on UWB and vision. Background Art

[0002] In a multi-source positioning system, positioning based on ultra-wideband (UWB, Ultra-Wideband) and vision positioning information is affected by sensor noise, installation position deviation, environmental changes, etc., resulting in difficulty in maintaining the consistency between the initial coordinate system calibration and subsequent long-term operation positioning. Moreover, there is often a lack of a smooth transition mechanism during calibration or re-calibration, which easily leads to positioning jumps and accuracy attenuation. Compared with current related technologies, there is usually a lack of real-time detection of sensor drift or external environmental changes, or a "transient" phenomenon of data occurs during coordinate transformation update, resulting in problems of low positioning calibration accuracy and positioning jumps. Summary of the Invention

[0003] This application provides a positioning and calibration method and related devices based on UWB and vision, which can solve the problems of low positioning calibration accuracy and positioning jumps.

[0004] In a first aspect, this application provides a positioning and calibration method based on UWB and vision. The positioning and calibration method includes:

[0005] When the calibration period arrives, obtain the UWB coordinates and vision coordinates of the target object at multiple moments; the last moment among the multiple moments is the current moment;

[0006] Preprocess all UWB coordinates using an unscented Kalman filter to obtain the final UWB coordinates at all moments;

[0007] When the target object has not been calibrated, calibrate the final UWB coordinates and vision coordinates at the current moment according to all the final UWB coordinates and all the vision coordinates to obtain the first calibration coordinates of the target object at the current moment;

[0008] When the target object has been calibrated, re-calibrate and smooth the final UWB coordinates and vision coordinates at the current moment to obtain the re-calibration coordinates of the target object at each future time point in the future time period.

[0009] Optionally, calibrating the final UWB coordinates and vision coordinates at the current moment according to all the final UWB coordinates and all the vision coordinates to obtain the first calibration coordinates of the target object at the current moment includes:

[0010] Calculate the first centroid of all the final UWB coordinates, and convert each final UWB coordinate to a coordinate system centered on the first centroid to obtain the UWB conversion coordinates corresponding to each final UWB coordinate;

[0011] Calculate the second centroid of all visual coordinates, and transform each visual coordinate to the coordinate system centered at the second centroid to obtain the visual transformation coordinates corresponding to each visual coordinate;

[0012] Calculate the rotation matrix and translation vector based on all UWB transformation coordinates and all visual transformation coordinates;

[0013] Obtain the calibrated visual coordinates at the current moment based on the rotation matrix and translation vector;

[0014] Fuse the calibrated visual coordinates with the final UWB coordinates at the current moment to obtain the first calibrated coordinates of the target object at the current moment.

[0015] Optionally, calculate the first centroid of all final UWB coordinates, including:

[0016] Through the formula:

[0017]

[0018] Calculate the first centroid p c ;

[0019] where N represents the set of all time numbers, |N| represents the number of times, and p i represents the i-th final UWB coordinate;

[0020] Transform each final UWB coordinate to the coordinate system centered at the first centroid to obtain the UWB transformation coordinates corresponding to each final UWB coordinate, including:

[0021] Through the formula:

[0022] p′ i =p i -p c

[0023] Calculate the i-th UWB transformation coordinate p' i ;

[0024] Calculate the second centroid of all visual coordinates, including:

[0025] Through the formula:

[0026]

[0027] Calculate the second centroid v c ;

[0028] where v i represents the i-th final UWB coordinate;

[0029] Convert each visual coordinate to a coordinate system centered at the second centroid to obtain the visual transformation coordinates corresponding to each visual coordinate, including:

[0030] Through the formula:

[0031] v′ i = v i - v c

[0032] Calculate the i-th UWB transformation coordinate v' i .

[0033] Optionally, calculate the rotation matrix and translation vector based on all UWB transformation coordinates and all visual transformation coordinates, including:

[0034] Through the formula:

[0035] R = VU T

[0036] H = U∑V T =(V′) T P′

[0037] T = p c - Rv c

[0038] Calculate the rotation matrix R and translation vector T;

[0039] Among them, H represents the cross-covariance matrix, V represents the right singular vector matrix, U represents the left singular vector matrix, ∑ represents the singular value diagonal matrix, V′ represents the matrix composed of all visual transformation coordinates, and P′ represents the matrix composed of all UWB transformation coordinates;

[0040] Obtain the calibrated visual coordinates at the current moment based on the rotation matrix and translation vector, including:

[0041] Through the formula:

[0042]

[0043] Calculate the calibrated visual coordinates at the current moment

[0044] Among them, P uwb represents the final UWB coordinate at the current moment.

[0045] Optionally, fuse the calibrated visual coordinates with the final UWB coordinate at the current moment to obtain the first calibrated coordinate of the target object at the current moment, including:

[0046] Through the formula:

[0047]

[0048] Calculate the first calibration coordinates (x fused , y fused );

[0049] Among them, ω UWB represents the weight of the UWB coordinates, represents the variance of the UWB position error, ω Vision represents the weight of the visual coordinates, represents the variance of the visual position error, x uwb represents the abscissa in the final UWB coordinates at the current moment, y uwb represents the ordinate in the final UWB coordinates at the current moment, represents the abscissa in the calibrated visual coordinates at the current moment, represents the ordinate in the calibrated visual coordinates at the current moment.

[0050] Optionally, recalibrate and smooth the final UWB coordinates and visual coordinates at the current moment to obtain the recalibrated coordinates of the target object at each future time point in the future time period, including:

[0051] Calculate the initial calibration coordinates at the current moment according to the rotation matrix and translation vector calibrated last time;

[0052] Determine whether the initial calibration coordinates meet the recalibration conditions;

[0053] If so, calculate the rotation matrix and translation vector at the current moment, and based on the rotation matrix and translation vector at the current moment, recalibrate and smooth the final UWB coordinates and visual coordinates to obtain the recalibrated coordinates of the target object at each future time point in the future time period;

[0054] Otherwise, use the initial calibration coordinates as the recalibrated coordinates of the target object at each future time point in the future time period.

[0055] Optionally, the recalibration condition is:

[0056]

[0057] Among them, error_index represents the error, x fused* represents the abscissa in the initial calibration coordinates, y fused* represents the ordinate in the initial calibration coordinates, x uwb represents the abscissa in the final UWB coordinates at the current moment, y uwb represents the ordinate in the final UWB coordinates at the current moment, ∈ represents a constant, and th represents the error threshold.

[0058] Optionally, based on the rotation matrix and translation vector at the current moment, recalibrate and smooth the final UWB coordinates and visual coordinates to obtain the recalibrated coordinates of the target object at each future time point in the future time period, including:

[0059] For each future time point in the future time period, perform the following steps:

[0060] Calculate the difference ratio at the future time point and determine whether the difference ratio is less than the difference threshold;

[0061] If so, calculate the spherical difference based on the rotation matrix at the current moment and the difference ratio, convert the spherical difference to the rotation matrix at the future time point, and calculate the translation vector at the future time point based on the translation vector at the current moment and the difference ratio;

[0062] Obtain the calibrated visual coordinates at the future time point according to the rotation matrix at the future time point and the translation vector at the future time point;

[0063] Fuse the calibrated visual coordinates at the future time point with the final UWB coordinates at the current moment to obtain the recalibrated coordinates of the target object at the future time point.

[0064] Optionally, calculating the difference ratio at the future time point includes:

[0065] Through the formula:

[0066]

[0067] Calculate the difference ratio α at the future time point t;

[0068] Where t0 represents the first future time point and D represents the duration of the future time period;

[0069] Calculating the spherical difference based on the rotation matrix at the current moment and the difference ratio includes:

[0070] Through the formula:

[0071] q(t) = slerp(q old ,q new ,α)

[0072] Calculate the rotation matrix q(t) at the future time point;

[0073] Where slerp represents spherical difference processing, q new represents the quaternion of the rotation matrix at the current moment, q old represents the quaternion of the rotation matrix of the previous calibration;

[0074] Calculating the translation vector at the future time point based on the translation vector at the current moment and the difference ratio includes:

[0075] By the formula:

[0076] T(t) = T old + α·(T new - T old )

[0077] Calculate the translation vector T(t) at a future time point;

[0078] where, T new represents the translation vector at the current moment, and T old represents the translation vector calibrated last time.

[0079] In a second aspect, the present application provides a positioning and calibration device based on UWB and vision, including:

[0080] An acquisition module, configured to acquire the UWB coordinates and vision coordinates of a target object at multiple moments when a calibration period arrives; the last moment among the multiple moments is the current moment;

[0081] A preprocessing module, configured to preprocess all UWB coordinates by using an unscented Kalman filter to obtain the final UWB coordinates at all moments;

[0082] A calibration module, configured to calibrate the final UWB coordinates and vision coordinates at the current moment according to all the final UWB coordinates and all the vision coordinates to obtain the first calibrated coordinates of the target object at the current moment when the target object has not been calibrated;

[0083] A recalibration module, configured to perform recalibration and smoothing on the final UWB coordinates and vision coordinates at the current moment to obtain the recalibrated coordinates at each future time point in a future time period of the target object when the target object has been calibrated.

[0084] In a third aspect, an embodiment of the present application provides a terminal device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the above-mentioned positioning and calibration method based on UWB and vision is implemented.

[0085] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the above-mentioned positioning and calibration method based on UWB and vision is implemented.

[0086] The above solution of the present application has the following beneficial effects:

[0087] In an embodiment of the present application, when the calibration period arrives, the UWB coordinates and visual coordinates of the target object at multiple moments are obtained. Then, all UWB coordinates are preprocessed using the unscented Kalman filter to obtain the final UWB coordinates at all moments. When the target object has not been calibrated, the final UWB coordinates and visual coordinates at the current moment are calibrated based on all the final UWB coordinates and all the visual coordinates to obtain the first calibrated coordinates of the target object at the current moment. When the target object has been calibrated, the final UWB coordinates and visual coordinates at the current moment are recalibrated and smoothed to obtain the recalibrated coordinates of the target object at each future time point in the future time period. Among them, preprocessing the UWB coordinates can reduce the jitter error of the UWB coordinates. Recalibrating the calibrated target object can correct the possible errors in the previous calibration, thereby improving the accuracy of the positioning calibration. Smoothing the positioning calibration can make the calibration results gradually transition to the new calibration results at future time points, avoiding positioning jumps.

[0088] Other beneficial effects of the present application will be described in detail in the subsequent specific implementation part. BRIEF DESCRIPTION OF THE DRAWINGS

[0089] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings required for use in the embodiments or the description of the prior art. Obviously, the following drawings are only some embodiments of the present application. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0090] Figure 1 It is a flowchart of a positioning and calibration method based on UWB and vision provided by an embodiment of the present application;

[0091] Figure 2 It is a specific flowchart of the positioning and calibration method provided by an embodiment of the present application;

[0092] Figure 3 It is a schematic structural diagram of a positioning and calibration device based on UWB and vision provided by an embodiment of the present application;

[0093] Figure 4 It is a schematic structural diagram of a terminal device provided by an embodiment of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0094] In the following description, specific details such as specific system architectures, technologies, etc. are presented for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, those skilled in the art should clearly understand that the present application can also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to avoid unnecessary details from interfering with the description of the present application.

[0095] It should be understood that when used in the specification and appended claims of the present application, the term "comprising" indicates the presence of the described features, wholes, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components, and / or their combinations.

[0096] It should also be understood that the term "and / or" used in the specification and appended claims of the present application refers to any combination and all possible combinations of one or more of the associated listed items, and includes these combinations.

[0097] As used in the specification and appended claims of the present application, the term "if" can be interpreted as "when" or "once" or "in response to determining" or "in response to detecting" depending on the context. Similarly, the phrases "if determined" or "if [the described condition or event] is detected" can be interpreted as meaning "once determined" or "in response to determining" or "once [the described condition or event] is detected" or "in response to detecting [the described condition or event]" depending on the context.

[0098] In addition, in the description of the specification and appended claims of the present application, the terms "first", "second", "third", etc. are only used for distinguishing descriptions and cannot be understood as indicating or implying relative importance.

[0099] The reference to "one embodiment" or "some embodiments" or the like described in the specification of the present application means that a specific feature, structure, or characteristic described in connection with the embodiment is included in one or more embodiments of the present application. Thus, statements such as "in one embodiment", "in some embodiments", "in other some embodiments", "in still other embodiments", etc. that appear in different places in this specification do not necessarily all refer to the same embodiment, but mean "one or more but not all embodiments", unless otherwise specifically emphasized in other ways. The terms "comprising", "including", "having", and their variants all mean "including but not limited to", unless otherwise specifically emphasized in other ways.

[0100] In view of the problems of low accuracy and positioning jumps in the existing positioning and calibration, the embodiments of the present application provide a positioning and calibration method based on UWB and vision. When the calibration period arrives, the UWB coordinates and vision coordinates of the target object at multiple moments are obtained, and then the unscented Kalman filter is used to preprocess all UWB coordinates to obtain the final UWB coordinates at all moments. When the target object has not been calibrated, the final UWB coordinates and vision coordinates at the current moment are calibrated according to all the final UWB coordinates and all the vision coordinates to obtain the first calibrated coordinates of the target object at the current moment. When the target object has been calibrated, the final UWB coordinates and vision coordinates at the current moment are recalibrated and smoothed to obtain the recalibrated coordinates of the target object at each future time point in the future time period. Among them, preprocessing the UWB coordinates can reduce the jitter error of the UWB coordinates. Recalibrating the calibrated target object can correct the possible errors in the previous calibration, thereby improving the accuracy of the positioning and calibration. Smoothing the positioning and calibration can make the calibration results gradually transition to the new calibration results at future time points, avoiding positioning jumps.

[0101] Next, an exemplary description is given of the positioning and calibration method based on UWB and vision provided by the present application.

[0102] As Figure 1 shown, the positioning and calibration method based on UWB and vision provided by the present application includes the following steps:

[0103] Step 11, when the calibration period arrives, obtain the UWB coordinates and vision coordinates of the target object at multiple moments.

[0104] The last moment among the multiple moments is the current moment, and the multiple moments before the current moment among the multiple moments are all historical moments. The above target object is an object that needs to be positioned and calibrated (such as a drone in a drone formation, a vehicle for autonomous driving, etc.).

[0105] In some embodiments of the present application, the UWB coordinates can be obtained by using a UWB positioning system. The UWB coordinates are the coordinates in the UWB coordinate system. The image information of the target object can be obtained by using a camera, etc., and then the vision coordinates can be estimated by using a computer vision algorithm (such as the SLAM algorithm). The vision coordinates are the coordinates in the vision coordinate system.

[0106] Exemplarily, the length of the calibration period is the time length between two adjacent moments. The current moment is the 5th second. If there are multiple moments, and there are 5 moments, then the 5 moments are the 4th second, the 4th second and 250 milliseconds, the 4th second and 500 milliseconds, the 4th second and 750 milliseconds, and the 5th second respectively; the calibration period is 250 milliseconds. At this time, the calibration period is from the 5th second to the 5th second and 250 milliseconds. At the 5th second and 250 milliseconds, the 5th second and 250 milliseconds is used as the current moment, and the target object at the 5th second and 250 milliseconds is located and calibrated. In practical applications, calibration often occurs within a very short time. For example, if the data output frequency of the visual coordinate data is 200hz, then there can be two hundred data in one second, that is, it can be considered that there are two hundred moments in one second. The above calibration period and the number of moments can both be set according to the data output frequency and the actual calibration requirements during actual application.

[0107] It should be noted that in order to avoid inaccurate calibration caused by sampling in a static state, only when sufficient movement amount is detected (for example, when the difference between the visual coordinates at a certain moment and the visual coordinates at the previous moment is greater than or equal to the threshold of 0.025m), the UWB coordinates and visual coordinates at this moment will be obtained.

[0108] Step 12, use unscented Kalman filter to preprocess all UWB coordinates to obtain the final UWB coordinates at all moments.

[0109] Specifically, perform unscented Kalman filter (UKF, Unscented Kalman Filter) on each UWB coordinate respectively to obtain the final UWB coordinate corresponding to each UWB coordinate.

[0110] Exemplarily, the process of unscented Kalman filter is as follows:

[0111] Define the state quantity and prediction equation z of the unscented Kalman filter k :

[0112]

[0113] Among them, x and y are the abscissa and ordinate in the UWB coordinate system respectively, v x and v y are the velocity components in the horizontal axis direction and the vertical axis direction respectively, Δt represents the time interval, x k is the k-th state quantity, and x k+1 is the predicted state quantity.

[0114] Since this step only processes the position information, only the position is observed, that is:

[0115]

[0116] Among them, zk is the predicted state quantity of the state quantity that only includes the position, z x represents the abscissa in the state quantity that only includes the position, z y represents the ordinate in the state quantity that only includes the position.

[0117] Generate sigma points:

[0118]

[0119]

[0120] where, x i represents the coordinate vector of the i-th sigma point in the state space. The UKF characterizes the characteristics of the Gaussian distribution under non-linear transformation by selecting several weighted Sigma points, represents the state mean (posterior mean) after fusing the measurement at time step k, P k|k represents the state covariance (posterior covariance) obtained after fusing the measurement at time step k, n represents the dimension of the state vector, and λ is a combined coefficient related to the hyperparameters α, β, κ of the Unscented Transform. Its calculation formula is λ = α^2(n + κ) - n. When the three hyperparameters are set to α = 0.1, β = 2, κ = 0.

[0121] Apply the prediction equation to the sigma points, and solve the prediction equation for each sigma point to obtain the prior prediction result

[0122]

[0123] Calculate the prior mean and prior covariance:

[0124] The prior mean is:

[0125]

[0126] where, is the weight for the mean.

[0127] The prior covariance is:

[0128]

[0129] where, is the weight for the covariance, and Q is the process noise covariance matrix. After completing this step, the prior estimate value and the covariance P k+1|k .

[0130] Therefore, for each prior sigma point The measurement space can be calculated

[0131]

[0132] Calculate the measurement prediction mean and measurement covariance:

[0133] The measurement prediction mean is:

[0134]

[0135] The measurement prediction variance is:

[0136]

[0137] where R is the measurement noise covariance.

[0138] Calculate the cross covariance

[0139]

[0140] Calculate the Kalman gain K k+1 :

[0141]

[0142] Calculate the posterior state mean

[0143]

[0144] Calculate the posterior covariance P k+1|k+1 :

[0145]

[0146] Finally, output the first two dimensions of the numbers in the posterior state as the UWB planar coordinates (x, y) for output to obtain the final UWB coordinates.

[0147] Step 13, when the target object has not been calibrated, calibrate the final UWB coordinates and visual coordinates at the current moment according to all the final UWB coordinates and all the visual coordinates to obtain the first calibration coordinates of the target object at the current moment.

[0148] In some embodiments of the present application, the step of calibrating the final UWB coordinates and visual coordinates at the current moment according to all the final UWB coordinates and all the visual coordinates to obtain the first calibration coordinates of the target object at the current moment includes:

[0149] In the first step, calculate the first centroid of all the final UWB coordinates, and transform each final UWB coordinate into a coordinate system centered at the first centroid to obtain the UWB transformed coordinate corresponding to each final UWB coordinate.

[0150] Specifically, through the formula:

[0151]

[0152] calculate the first centroid p c .

[0153] where N represents the set of all time numbers, |N| represents the number of time instances, and p i represents the i-th final UWB coordinate.

[0154] Through the formula:

[0155] p′ i = p i - p c

[0156] calculate the i-th UWB transformed coordinate p′ i .

[0157] In the second step, calculate the second centroid of all the visual coordinates, and transform each visual coordinate into a coordinate system centered at the second centroid to obtain the visual transformed coordinate corresponding to each visual coordinate.

[0158] Specifically, through the formula:

[0159]

[0160] calculate the second centroid v c .

[0161] where v i represents the i-th final UWB coordinate.

[0162] Through the formula:

[0163] v′ i = v i - v c

[0164] calculate the i-th UWB transformed coordinate v' i .

[0165] In the third step, calculate the rotation matrix and the translation vector based on all the UWB transformed coordinates and all the visual transformed coordinates.

[0166] Specifically, through the formula:

[0167] R = VU T

[0168] H = U∑V T = (V') T P'

[0169] T = p c -Rv c

[0170] Calculate the rotation matrix R and the translation vector T.

[0171] Among them, H represents the cross-covariance matrix, V represents the right singular vector matrix, U represents the left singular vector matrix, ∑ represents the singular value diagonal matrix, V′ represents the matrix composed of all visual transformation coordinates, and P′ represents the matrix composed of all UWB transformation coordinates.

[0172] In the fourth step, obtain the calibrated visual coordinates at the current moment according to the rotation matrix and the translation vector.

[0173] Through the formula:

[0174]

[0175] Calculate the calibrated visual coordinates at the current moment

[0176] Among them, P uwb represents the final UWB coordinates at the current moment.

[0177] In the fifth step, fuse the calibrated visual coordinates with the final UWB coordinates at the current moment to obtain the first calibrated coordinates of the target object at the current moment.

[0178] The above first calibrated coordinates are the coordinates in the UWB coordinate system.

[0179] Through the formula:

[0180]

[0181] Calculate the first calibrated coordinates (x fused , y fused ).

[0182] Among them, ω UWB represents the weight of the UWB coordinates, represents the variance of the UWB position error, ω Vision represents the weight of the visual coordinates, represents the variance of the visual position error, x uwb represents the abscissa in the final UWB coordinates at the current moment, y uwb represents the ordinate in the final UWB coordinates at the current moment, represents the abscissa in the calibrated visual coordinates at the current moment, Represents the ordinate in the calibrated visual coordinates at the current moment.

[0183] Step 14: When the target object has been calibrated, recalibrate and smooth the final UWB coordinates and visual coordinates at the current moment to obtain the recalibrated coordinates of the target object at each future time point in the future time period.

[0184] The starting time point of the above future time period is the current moment, and the ending time point is the next moment for positioning calibration (the future time period is a calibration cycle starting from the current moment, and multiple future time points are the time points in this calibration cycle. For example, the future time period is from the 5th second to the 5th second and 250 milliseconds, with a total of 5 future time points, namely the 5th second, the 5th second and 62.5 milliseconds, the 5th second and 125 milliseconds, the 5th second and 187.5 milliseconds, and the 5th second and 250 milliseconds). For the current moment that requires positioning calibration, one of Step 13 and Step 14 is executed by judging whether the target object has been calibrated before the current moment. In practical applications, only Step 13 is executed when the target object is initially calibrated, and Step 14 is executed every time calibration is performed after the initial calibration.

[0185] In some embodiments of the present application, the above recalibration and smoothing of the final UWB coordinates and visual coordinates at the current moment to obtain the recalibrated coordinates of the target object at each future time point in the future time period includes:

[0186] The first step: Calculate the initial calibrated coordinates at the current moment according to the rotation matrix and translation vector of the previous calibration.

[0187] Specifically, obtain the calibrated visual coordinates at the current moment according to the rotation matrix and translation vector of the previous calibration, and fuse the calibrated visual coordinates with the final UWB coordinates at the current moment to obtain the initial calibrated coordinates of the target object at the current moment.

[0188] It should be noted that the calculation formula for the calibrated visual coordinates is the formula for calculating the calibrated visual coordinates in Step 13. Substitute the rotation matrix and translation vector of the previous calibration into the formula for calculating the calibrated visual coordinates in Step 13 to obtain the calibrated visual coordinates in this step; the calculation formula for the initial calibrated coordinates is the formula for calculating the first calibrated coordinates in Step 13. Substitute the calibrated visual coordinates in this step and the final UWB coordinates at the current moment into the formula for calculating the first calibrated coordinates in Step 13 to obtain the initial calibrated coordinates.

[0189] The second step: Judge whether the initial calibrated coordinates meet the recalibration conditions.

[0190] The recalibration conditions are:

[0191]

[0192] Among them, error_index represents the error, x fused* represents the abscissa in the initial calibration coordinates, y fused* represents the ordinate in the initial calibration coordinates, x uwb represents the abscissa in the final UWB coordinates at the current moment, y uwb represents the ordinate in the final UWB coordinates at the current moment, ∈ represents a constant, and th represents the error threshold.

[0193] If so, calculate the rotation matrix and translation vector at the current moment, and based on the rotation matrix and translation vector at the current moment, recalibrate and smooth the final UWB coordinates and visual coordinates to obtain the recalibrated coordinates of the target object at each future time point in the future time period.

[0194] Otherwise, use the initial calibration coordinates as the recalibrated coordinates of each future time point in the future time period.

[0195] It should be noted that in practical applications, in order to prevent misjudgment of the recalibration conditions or frequent recalibration, a preset time length can be set, and it is considered to meet the recalibration conditions only when the recalibration conditions are continuously met within this preset time length. Calculate the rotation matrix and translation vector at the current moment using the steps for calculating the rotation matrix and translation vector in step 13.

[0196] The above steps of recalibrating and smoothing the final UWB coordinates and visual coordinates based on the rotation matrix and translation vector at the current moment to obtain the recalibrated coordinates of the target object at each future time point in the future time period are specifically as follows:

[0197] For each future time point in the future time period, perform the following steps:

[0198] Calculate the difference ratio at the future time point and determine whether the difference ratio is less than the difference threshold.

[0199] If so, calculate the spherical difference based on the rotation matrix at the current moment and the difference ratio, and convert the spherical difference into the rotation matrix at the future time point. Calculate the translation vector at the future time point based on the translation vector at the current moment and the difference ratio.

[0200] Obtain the calibrated visual coordinates at the future time point according to the rotation matrix at the future time point and the translation vector at the future time point.

[0201] Fuse the calibrated visual coordinates at the future time point with the final UWB coordinates at the current moment to obtain the recalibrated coordinates of the target object at the future time point.

[0202] It should be noted that if the difference ratio is greater than or equal to the difference threshold, it is considered that the positioning and calibration have ended at this future time point. At this time, the final coordinates of the target object have been obtained, and the rotation matrix and translation vector at this time are saved as the "rotation matrix of the previous calibration" and "translation vector of the previous calibration" in the next positioning and calibration process. The spherical difference can be converted into the rotation matrix at the future time point by using the algorithm of converting the rotation matrix with quaternions; substituting the rotation matrix at the future time point and the translation vector at the future time point into the formula for calculating the calibrated visual coordinates in step 13, the calibrated visual coordinates at the future time point can be obtained; substituting the calibrated visual coordinates at the future time point and the final UWB coordinates at the current moment into the formula for calculating the first calibration coordinates in step 13, the re-calibration coordinates of the target object at the future time point can be obtained.

[0203] The specific process of calculating the difference ratio is as follows:

[0204] Specifically, through the formula:

[0205]

[0206] Calculate the difference ratio α at the future time point t.

[0207] Among them, t0 represents the first future time point, and D represents the duration of the future time period.

[0208] The specific process of calculating the spherical difference is as follows:

[0209] Through the formula:

[0210] q(t) = slerp(q old , q new , α)

[0211] Calculate the rotation matrix q(t) at the future time point.

[0212] Among them, slerp represents spherical difference processing, q new represents the quaternion of the rotation matrix at the current moment, and q old represents the quaternion of the rotation matrix of the previous calibration.

[0213] The specific process of calculating the translation vector at the future time point is as follows:

[0214] Through the formula:

[0215] T(t) = T old + α · (T new - T old )

[0216] Calculate the translation vector T(t) at the future time point.

[0217] Among them, T new represents the translation vector at the current moment, and T old represents the translation vector calibrated last time.

[0218] Exemplarily, the process of converting the rotation matrix into a quaternion is as follows:

[0219] Add all the elements on the main diagonal of the rotation matrix to obtain the matrix trace, and judge the branch according to the matrix trace:

[0220] If the matrix trace tr(R)>0, the simplified formula can be used:

[0221]

[0222] Calculate the real part q w and the imaginary parts q x , q y , q z .

[0223] If the matrix trace tr(R)≤0, it is necessary to judge which item in the matrix diagonal is the largest and adopt the corresponding formula; for example, if R 00 is the largest, then:

[0224]

[0225] For other branches (R 11 or R 22 is the largest), it is similar.

[0226] Obtain the quaternion q = q w +q x i + q y j + q z k, and then normalize it to eliminate data errors.

[0227] The process of the above spherical interpolation processing is as follows:

[0228] 1. Preprocessing:

[0229] If the inner product of two quaternions q0·q1 < 0 (the inner product less than 0 means their included angle > 90°), q1 can be set to -q1 to obtain a smaller included angle difference and avoid inversion. Among them, q0 is the quaternion of the rotation matrix calibrated last time, and q1 is the quaternion of the rotation matrix at the current moment.

[0230] If q0·q1 is sufficiently close to 1, it means they are almost the same, and linear interpolation can be used for approximation.

[0231] 2. Calculate the inner product dot = q0·q1:

[0232] dot = q 0x q1x +q 0y q 1y +q 0z q 1z +q 0w q 1w

[0233] 3. Calculate θ0 and θ:

[0234] θ0 = arccos(dot)

[0235] θ = θ0·t

[0236] 4. Calculate the rotation matrix q(t):

[0237]

[0238] Exemplarily, after calibration or re - calibration is completed, the obtained coordinates are sent to downstream applications (such as robot control systems, visualization front - ends, etc.) to support downstream applications in performing downstream tasks such as control and monitoring.

[0239] The following uses a specific example to exemplarily illustrate the method of the present application.

[0240] As Figure 2 shown, first, system initialization, UWB positioning data buffering, and visual positioning data buffering are performed. Then, UWB and visual data are collected, the unscented Kalman filter state is updated, and the calibration or re - calibration process is entered. It is judged whether the sensor group reaches the motion threshold. If not, no processing is performed on the data. If so, a sufficient number of paired points are collected, and the rotation matrix R and translation vector T are calculated. Then, it is judged whether it is the initial calibration. If so, R and T are used. Otherwise, R and T are updated with a smooth transition. Then, the step of outputting the fused positioning result and error detection is entered. Coordinate system transformation and fused positioning result output are performed using R and T, and it is calculated and judged whether the error is greater than the threshold. If so, the step of judging whether the sensor group reaches the motion threshold is returned. Otherwise, the step of performing coordinate system transformation and fused positioning result output using R and T is returned.

[0241] It is worth mentioning that pre - processing the UWB coordinates can reduce the jitter error of the UWB coordinates. Re - calibrating the calibrated target object can correct the possible errors in the previous calibration, thereby improving the accuracy of the positioning calibration. Smoothing the positioning calibration can make the calibration result gradually transition to the new calibration result at future time points, avoiding positioning jumps.

[0242] In addition, the beneficial effects of the present application are also reflected in:

[0243] High degree of automation: Neither the initial calibration nor the recalibration process requires manual intervention. As long as a certain amount of movement is satisfied, paired points (UWB coordinates and corresponding visual coordinates) can be collected to obtain an accurate rotation matrix R and offset vector T.

[0244] Long-term accuracy maintenance: Since this method includes error detection and self-triggered recalibration, even if the device undergoes displacement, tilt, or environmental changes, it can autonomously correct the calibration, so that the fusion result of UWB and vision still maintains good consistency after long-term operation.

[0245] Smooth switching: The transition between the old and new calibration results uses quaternion slerp and linear interpolation, avoiding position jumps caused by instantaneous switching, making positioning or navigation more stable and reliable.

[0246] Scalability:

[0247] This method is not only applicable to two-dimensional plane positioning, but can also be used in three-dimensional scenarios (such as drones, AR / VR applications), with corresponding extensions to the rotation matrix and three-dimensional coordinate transformation.

[0248] It is also possible to replace the UKF with other filtering schemes (such as EKF, PF particle filter), or replace the vision module with other positioning methods (such as lidar SLAM), as long as the corresponding data fusion interface is adjusted.

[0249] Next, an exemplary description is provided for the positioning and calibration device based on UWB and vision provided in this application.

[0250] As Figure 3 shown, an embodiment of this application provides a positioning and calibration device based on UWB and vision. The positioning and calibration device 300 based on UWB and vision includes:

[0251] An acquisition module 301, configured to obtain the UWB coordinates and visual coordinates of a target object at multiple moments when the calibration period arrives; the last moment among the multiple moments is the current moment;

[0252] A preprocessing module 302, configured to preprocess all UWB coordinates using the unscented Kalman filter to obtain the final UWB coordinates at all moments;

[0253] A calibration module 303, configured to calibrate the final UWB coordinates and visual coordinates at the current moment according to all the final UWB coordinates and all the visual coordinates when the target object has not been calibrated, to obtain the initial calibration coordinates of the target object at the current moment;

[0254] A recalibration module 304, configured to perform recalibration and smoothing on the final UWB coordinates and visual coordinates at the current moment when the target object has been calibrated, to obtain the recalibration coordinates of the target object at each future time point in the future time period.

[0255] It should be noted that for the content such as information interaction and execution process between the above-mentioned devices / units, since it is based on the same concept as the method embodiment of the present application, for its specific functions and the technical effects brought, reference can be specifically made to the method embodiment part, and details will not be repeated here.

[0256] Those skilled in the art can clearly understand that for the convenience and conciseness of description, only the above-mentioned division of each functional unit and module is used as an example. In practical applications, the above-mentioned functions can be allocated to different functional units and modules according to needs, that is, the internal structure of the device is divided into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of a software functional unit. In addition, the specific names of each functional unit and module are only for the convenience of mutual distinction and do not limit the protection scope of the present application. The specific working process of the units and modules in the above system can refer to the corresponding process in the foregoing method embodiment, and details will not be repeated here.

[0257] As Figure 4 shown, an embodiment of the present application provides a terminal device. The terminal device D10 in this embodiment includes: at least one processor D100 ( Figure 4 only one processor is shown in the figure), a memory D101, and a computer program D102 stored in the memory D101 and executable on the at least one processor D100. When the processor D100 executes the computer program D102, it implements the steps in any of the above method embodiments.

[0258] Specifically, when the processor D100 executes the computer program D102, it obtains the UWB coordinates and visual coordinates of the target object at multiple moments when the calibration period arrives, and then preprocesses all the UWB coordinates using the unscented Kalman filter to obtain the final UWB coordinates at all moments. When the target object has not been calibrated, the final UWB coordinates and visual coordinates at the current moment are calibrated based on all the final UWB coordinates and all the visual coordinates to obtain the first calibrated coordinates of the target object at the current moment. When the target object has been calibrated, the final UWB coordinates and visual coordinates at the current moment are recalibrated and smoothed to obtain the recalibrated coordinates at each future time point in the future time period of the target object. Among them, preprocessing the UWB coordinates can reduce the jitter error of the UWB coordinates. Recalibrating the calibrated target object can correct the possible errors in the previous calibration, thereby improving the accuracy of the positioning calibration. Smoothing the positioning calibration can make the calibration result gradually transition to the new calibration result at future time points, avoiding positioning jumps.

[0259] The so-called processor D100 may be a central processing unit (CPU, Central Processing Unit), and this processor D100 may also be other general-purpose processors, digital signal processors (DSP, Digital Signal Processor), application-specific integrated circuits (ASIC, Application Specific Integrated Circuit), off-the-shelf programmable gate arrays (FPGA, Field-Programmable Gate Array) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or this processor may also be any conventional processor, etc.

[0260] In some embodiments, the memory D101 may be an internal storage unit of the terminal device D10, such as the hard disk or memory of the terminal device D10. In some other embodiments, the memory D101 may also be an external storage device of the terminal device D10, such as a plug-in hard disk equipped on the terminal device D10, a Smart Media Card (SMC), a Secure Digital (SD) card, a Flash Card, etc. Further, the memory D101 may also include both the internal storage unit of the terminal device D10 and the external storage device. The memory D101 is used to store an operating system, application programs, a BootLoader, data, and other programs, such as the program code of the computer program, etc. The memory D101 may also be used to temporarily store data that has been output or will be output.

[0261] An embodiment of the present application also provides a computer-readable storage medium storing a computer program, which when executed by a processor, can implement the steps in the above-mentioned method embodiments.

[0262] An embodiment of the present application provides a computer program product, which when running on a terminal device, enables the terminal device to execute the steps in the above-mentioned method embodiments.

[0263] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, to implement all or part of the processes in the above-mentioned method embodiments of the present application, a computer program can be used to instruct relevant hardware to complete. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by a processor, the steps in the above-mentioned method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in the form of source code, object code, an executable file, or some intermediate form, etc. The computer-readable medium may at least include: any entity or device that can carry the computer program code to the UWB and vision-based positioning and calibration method device / terminal device, a recording medium, a computer memory, a Read-Only Memory (ROM), a Random Access Memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium. For example, a USB flash drive, a mobile hard disk, a magnetic disk, or an optical disc, etc. In some jurisdictions, according to legislation and patent practice, the computer-readable medium may not be an electrical carrier signal and a telecommunication signal.

[0264] In the above embodiments, the descriptions of the various embodiments each have their own emphasis. For parts not detailed or recorded in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.

[0265] Those of ordinary skill in the art can realize that the units and algorithm steps of the examples described in combination with the embodiments disclosed herein can be implemented by electronic hardware or a combination of computer software and electronic hardware. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Professional technicians can use different methods for each specific application to implement the described functions, but such implementation should not be considered to exceed the scope of this application.

[0266] The above is the preferred embodiment of this application. It should be noted that for those of ordinary skill in the art of this technology, without departing from the principle described in this application, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of this application.

Claims

1. A positioning calibration method based on UWB and vision, characterized in that: include: When the calibration period arrives, the UWB coordinates and visual coordinates of the target object at multiple moments are obtained; the last moment of the multiple moments is the current moment; All UWB coordinates are preprocessed using the infinite Kalman filter to obtain the final UWB coordinates at all times; When the target object has not been calibrated, calibrate the final UWB coordinates and visual coordinates at the current moment according to all the final UWB coordinates and all the visual coordinates to obtain the first calibrated coordinates of the target object at the current moment; When the target object has been calibrated, the final UWB coordinates and visual coordinates at the current moment are recalibrated and smoothed to obtain the recalibrated coordinates of the target object at each future time point in the future time period.

2. The positioning calibration method according to claim 1, characterized in that: The step of calibrating the final UWB coordinates and the visual coordinates at the current moment according to all the final UWB coordinates and all the visual coordinates to obtain the first calibrated coordinates of the target object at the current moment includes: Calculate the first centroid of all final UWB coordinates, and transform each final UWB coordinate into a coordinate system centered at the first centroid to obtain a UWB transformed coordinate corresponding to each final UWB coordinate; Calculate the second centroid of all visual coordinates, and transform each visual coordinate into a coordinate system centered on the second centroid to obtain the visual transformation coordinate corresponding to each visual coordinate; Calculate the rotation matrix and translation vector based on all UWB transformed coordinates and all visual transformed coordinates; Obtaining the calibrated visual coordinates at the current moment according to the rotation matrix and the translation vector; The calibrated visual coordinates are merged with the final UWB coordinates at the current moment to obtain the first calibrated coordinates of the target object at the current moment.

3. The positioning calibration method according to claim 1, characterized in that: The first centroid of all final UWB coordinates is calculated, including: By formula: Calculate the first centroid p c ; Where N represents the set of numbers of all moments, |N| represents the number of moments, and p i represents the i-th final UWB coordinate; The step of converting each final UWB coordinate into a coordinate system centered at the first centroid to obtain a UWB conversion coordinate corresponding to each final UWB coordinate includes: By formula: p’ i =p i -p c Calculate the i-th UWB conversion coordinate p′ i ; The step of calculating the second centroid of all visual coordinates includes: By formula: Calculate the second centroid v c ; Among them, v i represents the i-th final UWB coordinate; The step of converting each visual coordinate into a coordinate system centered on the second centroid to obtain a visual conversion coordinate corresponding to each visual coordinate includes: By formula: v' i =v i -v c Calculate the i-th UWB conversion coordinate v′ i .

4. The positioning calibration method according to claim 3, characterized in that: The calculation of the rotation matrix and the translation vector according to all UWB conversion coordinates and all visual conversion coordinates includes: By formula: R=VU T H=U∑V T =(V′) T P′ T=p c -Rv c Calculate the rotation matrix R and translation vector T; Where H represents the cross-covariance matrix, V represents the right singular vector matrix, U represents the left singular vector matrix, ∑ represents the singular value diagonal matrix, V′ represents the matrix composed of all visual transformation coordinates, and P′ represents the matrix composed of all UWB transformation coordinates; The obtaining the calibrated visual coordinates at the current moment according to the rotation matrix and the translation vector includes: By formula: Calculate the calibrated visual coordinates at the current moment Among them, P uwb Indicates the final UWB coordinates at the current moment.

5. The positioning calibration method according to claim 4, characterized in that: The step of fusing the calibrated visual coordinates with the final UWB coordinates at the current moment to obtain the first calibrated coordinates of the target object at the current moment includes: By formula: Calculate the first calibration coordinates (x fused ,y fused ); Among them, ω UWB represents the weight of the UWB coordinates, represents the UWB position error variance, ω Vision represents the weight of the visual coordinates, represents the visual position error variance, x uwb Represents the horizontal coordinate in the final UWB coordinate at the current moment, y uwb Represents the ordinate in the final UWB coordinate at the current moment, Represents the horizontal coordinate in the calibrated visual coordinates at the current moment, Represents the ordinate in the calibrated visual coordinates at the current moment.

6. The positioning calibration method according to claim 1, characterized in that: The recalibrating and smoothing the final UWB coordinates and visual coordinates at the current moment to obtain the recalibrated coordinates of the target object at each future time point in the future time period includes: Calculate the initial calibration coordinates at the current moment based on the rotation matrix and translation vector of the last calibration; Determining whether the initial calibration coordinates meet the recalibration conditions; If yes, then calculate the rotation matrix and translation vector at the current moment, and recalibrate and smooth the final UWB coordinates and visual coordinates based on the rotation matrix and translation vector at the current moment to obtain the recalibrated coordinates of the target object at each future time point in the future time period; Otherwise, the initial calibration coordinates are used as the recalibrated coordinates of each future time point in the future time period.

7. The positioning calibration method according to claim 6, characterized in that: The recalibration conditions are: Among them, errir_index represents the error, x fused* Represents the horizontal coordinate in the initial calibration coordinate, y fused* Represents the ordinate in the initial calibration coordinates, x uwb Represents the horizontal coordinate in the final UWB coordinate at the current moment, y uwb represents the ordinate in the final UWB coordinate at the current moment, ∈ represents a constant, and th represents the error threshold.

8. The positioning calibration method according to claim 7, characterized in that: The recalibration and smoothing of the final UWB coordinates and visual coordinates based on the rotation matrix and translation vector at the current moment to obtain the recalibrated coordinates of the target object at each future time point in the future time period includes: For each future time point in the future time period, perform the following steps: Calculating the difference ratio at the future time point, and determining whether the difference ratio is less than a difference threshold; If yes, then calculating the spherical difference based on the rotation matrix at the current moment and the difference ratio, and converting the spherical difference into the rotation matrix at the future time point, and calculating the translation vector at the future time point based on the translation vector at the current moment and the difference ratio; Obtaining a calibrated visual coordinate at the future time point according to the rotation matrix at the future time point and the translation vector at the future time point; The calibrated visual coordinates at the future time point are merged with the final UWB coordinates at the current moment to obtain the recalibrated coordinates of the target object at the future time point.

9. The positioning calibration method according to claim 8, characterized in that: The calculating the difference ratio of the future time point includes: By formula: Calculate the difference ratio α at the future time point t; Among them, t0 represents the first future time point, and D represents the duration of the future time period; The calculating of the spherical difference value based on the rotation matrix at the current moment and the difference ratio includes: By formula: q(t)=slerp(q old ,q new ,α) Calculate the rotation matrix q(t) at the future time point; Among them, slerp represents spherical difference processing, q new The quaternion representing the rotation matrix at the current moment, q old The quaternion representing the rotation matrix of the last calibration; The calculating the translation vector at the future time point based on the translation vector at the current moment and the difference ratio comprises: By formula: T(t)=T old +α·(T new -T old ) Calculate the translation vector T(t) at the future time point; Among them, T new Represents the translation vector at the current moment, T old Indicates the translation vector of the last calibration.

10. A terminal device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the UWB and vision-based positioning and calibration method as described in any one of claims 1 to 9 is implemented.

Images