A positioning calibration method based on UWB and vision and related equipment

Abstract

The application relates to the technical field of positioning calibration, and provides a positioning calibration method based on UWB and vision and related equipment. The method comprises the following steps: when a calibration period arrives, UWB coordinates and vision coordinates of a target object at multiple time points are acquired; all UWB coordinates are preprocessed by using an unscented Kalman filter to obtain final UWB coordinates at all time points; when the target object has not been calibrated, the final UWB coordinates and the vision coordinates at the current time point are calibrated according to all the final UWB coordinates and all the vision coordinates, so that first calibration coordinates of the target object at the current time point are obtained; when the target object has been calibrated, the final UWB coordinates and the vision coordinates at the current time point are recalibrated and smoothed, so that recalibration coordinates of the target object at each future time point in a future time period are obtained. The method can improve the positioning calibration precision and avoid positioning jump.

Description

Technical Field

[0001] This application relates to the field of positioning and calibration technology, and in particular to a positioning and calibration method and related equipment based on UWB and vision. Background Technology

[0002] In multi-source positioning systems, ultra-wideband (UWB) and visual positioning information are affected by sensor noise, installation position deviations, and environmental changes. This makes it difficult to maintain consistency between the initial coordinate system calibration and the positioning during long-term operation. Furthermore, there is often a lack of smooth transition mechanisms during calibration or recalibration, which can easily lead to positioning jumps and accuracy degradation. Compared to current related technologies, which typically lack real-time detection of sensor drift or changes in the external environment, or experience data "transient" phenomena during coordinate transformation updates, resulting in low positioning calibration accuracy and positioning jumps, these technologies also suffer from these problems. Summary of the Invention

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

[0004] Firstly, this application provides a localization and calibration method based on UWB and vision, which includes: When the calibration period arrives, the UWB coordinates and visual coordinates of the target object are obtained at multiple times; the last time among the multiple times is the current time. The boundless Kalman filter is used to preprocess all UWB coordinates to obtain the final UWB coordinates at all times. When the target object has not been calibrated, the final UWB coordinates and visual coordinates at the current moment are calibrated based on all final UWB coordinates and all visual coordinates to obtain the first calibration coordinates of the target object at the current moment. When the target object has already 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.

[0005] Optionally, the final UWB coordinates and visual coordinates at the current moment are calibrated based on all final UWB coordinates and all visual coordinates to obtain the initial calibration coordinates of the target object at the current moment, including: Calculate the first centroid of all final UWB coordinates, and transform each final UWB coordinate to a coordinate system centered on the first centroid, to obtain the UWB transformed coordinates corresponding to each final UWB coordinate; Calculate the second centroid of all visual coordinates, and transform each visual coordinate to a coordinate system centered on the second centroid, to obtain the visual transformed coordinates corresponding to each visual coordinate; Calculate the rotation matrix and translation vector based on all UWB transformation coordinates and all visual transformation coordinates; The current calibration visual coordinates are obtained based on the rotation matrix and translation vector; By fusing the calibrated visual coordinates with the final UWB coordinates at the current moment, the initial calibrated coordinates of the target object at the current moment are obtained.

[0006] Optionally, calculate the first centroid of all final UWB coordinates, including: Through the formula:

[0007] Calculate the first centroid ; in, Represents the set of indices for all times. Indicates the number of moments. Indicates the first One final UWB coordinate; Transform each final UWB coordinate to a coordinate system centered on the first centroid, obtaining the UWB transformed coordinates corresponding to each final UWB coordinate, including: Through the formula:

[0008] Calculate the first UWB transformed coordinates ; Calculate the second centroid for all visual coordinates, including: Through the formula:

[0009] Calculate the second centroid ; in, Indicates the first One final UWB coordinate; Transform each visual coordinate to a coordinate system centered on the second centroid, obtaining the visual transformed coordinates corresponding to each visual coordinate, including: Through the formula:

[0010] Calculate the first UWB transformed coordinates .

[0011] Optionally, the rotation matrix and translation vector are calculated based on all UWB transformation coordinates and all visual transformation coordinates, including: Through the formula:

[0012]

[0013]

[0014] Calculate the rotation matrix Translation vector ; in, Represents the cross covariance matrix. Describes a right singular vector matrix. Describes a left singular vector matrix. Represents a singular value diagonal matrix. This represents a matrix consisting of all visual transformation coordinates. This represents the matrix consisting of all UWB transformed coordinates; The calibrated visual coordinates at the current moment are obtained from the rotation matrix and translation vector, including: Through the formula:

[0015] Calculate the calibrated visual coordinates at the current moment. ; in, This represents the final UWB coordinates at the current moment.

[0016] Optionally, the calibrated visual coordinates are fused with the final UWB coordinates at the current moment to obtain the target object's initial calibrated coordinates at the current moment, including: Through the formula:

[0017]

[0018] Calculate the initial calibration coordinates ; in, Indicates the weight of UWB coordinates. , This represents the variance of the UWB position error. The weights representing visual coordinates, , This represents the variance of visual position error. This represents the x-coordinate in the final UWB coordinates at the current moment. This represents the ordinate in the final UWB coordinates at the current moment. This represents the x-coordinate in the calibrated visual coordinate system at the current moment. This represents the ordinate in the calibrated visual coordinates at the current moment.

[0019] Optionally, 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, including: Calculate the initial calibration coordinates at the current moment based on the rotation matrix and translation vector from the previous calibration. Determine whether the initial calibration coordinates meet the recalibration conditions; 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; Otherwise, the initial calibration coordinates will be used as the recalibrated coordinates for each future time point in the future time period.

[0020] Optional, the recalibration conditions are:

[0021] in, Indicates error. This represents the x-coordinate in the initial calibration coordinate system. This represents the ordinate in the initial calibration coordinate system. This represents the x-coordinate in the final UWB coordinates at the current moment. This represents the ordinate in the final UWB coordinates at the current moment. Represents a constant. This indicates the error threshold.

[0022] Optionally, based on the rotation matrix and translation vector at the current moment, the final UWB coordinates and visual coordinates are recalibrated and smoothed to obtain the recalibrated coordinates of the target object at each future time point in the future time period, including: For each future point in time within a future time period, perform the following steps: Calculate the difference ratio at future time points and determine whether the difference ratio is less than the difference threshold; If so, calculate the spherical difference based on the rotation matrix and the difference ratio at the current time, and convert the spherical difference into a rotation matrix at a future time point. Calculate the translation vector at a future time point based on the translation vector at the current time and the difference ratio. The calibration visual coordinates of the future time point are obtained based on the rotation matrix and translation vector of the future time point. By fusing the calibrated visual coordinates at future points in time with the final UWB coordinates at the current moment, the recalibrated coordinates of the target object at future points in time are obtained.

[0023] Optionally, the calculation of the difference ratio at future time points includes: Through the formula:

[0024] Calculate future time points The difference ratio ; in, Indicates the first future point in time. Indicates the duration of a future time period; The spherical difference is calculated based on the rotation matrix and the difference ratio at the current moment, including: Through the formula:

[0025] Calculate the rotation matrix at future time points ; in, This indicates the processing of spherical differences. The quaternion representing the rotation matrix at the current moment. The quaternion representing the rotation matrix of the last calibration; Calculate the translation vector at future time points based on the translation vector at the current time and the difference ratio, including: Through the formula:

[0026] Calculate the translation vector at future time points ; in, This represents the translation vector at the current moment. This represents the translation vector from the previous calibration.

[0027] Secondly, this application provides a positioning and calibration device based on UWB and vision, comprising: The acquisition module is used to acquire the UWB coordinates and visual coordinates of the target object at multiple moments when the calibration period arrives; the last moment among the multiple moments is the current moment. The preprocessing module is used to preprocess all UWB coordinates using an infinite Kalman filter to obtain the final UWB coordinates at all times. The calibration module is used to calibrate the final UWB coordinates and visual coordinates at the current moment based on all final UWB coordinates and all visual coordinates when the target object has not been calibrated before, so as to obtain the first calibration coordinates of the target object at the current moment. The recalibration module is used to recalibrate and smooth the final UWB coordinates and visual coordinates at the current moment when the target object has already been calibrated, so as to obtain the recalibrated coordinates of the target object at each future time point in the future time period.

[0028] Thirdly, embodiments of this application provide 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, it implements the aforementioned positioning and calibration method based on UWB and vision.

[0029] Fourthly, embodiments of this application provide a computer-readable storage medium storing a computer program that, when executed by a processor, implements the aforementioned positioning and calibration method based on UWB and vision.

[0030] The above-mentioned solution in this application has the following beneficial effects: In the embodiments of this application, when the calibration period arrives, the UWB coordinates and visual coordinates of the target object at multiple times are obtained. Then, an infinite Kalman filter is used to preprocess all UWB coordinates to obtain the final UWB coordinates at all times. When the target object has not been calibrated, the final UWB coordinates and visual coordinates at the current time are calibrated based on all final UWB coordinates and all visual coordinates to obtain the initial calibration coordinates of the target object at the current time. When the target object has been calibrated, the final UWB coordinates and visual coordinates at the current time are recalibrated and smoothed to obtain the recalibrated coordinates of the target object at each future time point in the future time period. Among these methods, preprocessing the UWB coordinates can reduce the jitter error of the UWB coordinates; recalibrating the calibrated target object can correct the errors that may have occurred in the previous calibration, thereby improving the accuracy of the positioning calibration; and smoothing the positioning calibration can allow the calibration results to gradually transition to the new calibration results at future time points, avoiding positioning jumps.

[0031] Other beneficial effects of this application will be described in detail in the following detailed description section. Attached Figure Description

[0032] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0033] Figure 1 A flowchart illustrating a localization and calibration method based on UWB and vision provided in an embodiment of this application; Figure 2 A detailed flowchart of a positioning and calibration method provided in an embodiment of this application; Figure 3 A schematic diagram of the structure of a positioning and calibration device based on UWB and vision provided in an embodiment of this application; Figure 4 This is a schematic diagram of the structure of a terminal device provided in an embodiment of this application. Detailed Implementation

[0034] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.

[0035] It should be understood that, when used in this application specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or a collection thereof.

[0036] It should also be understood that the term “and / or” as used in this application specification and the appended claims means any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0037] As used in this application specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrase "if determined" or "if detected [the described condition or event]" may be interpreted, depending on the context, as meaning "once determined," "in response to determination," "once detected [the described condition or event]," or "in response to detection [the described condition or event]."

[0038] Furthermore, in the description of this application and the appended claims, the terms "first," "second," "third," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0039] References to "one embodiment" or "some embodiments" as described in this specification mean that one or more embodiments of this application include a specific feature, structure, or characteristic described in connection with that embodiment. Therefore, the phrases "in one embodiment," "in some embodiments," "in other embodiments," "in still other embodiments," etc., appearing in different parts of this specification do not necessarily refer to the same embodiment, but rather mean "one or more, but not all, embodiments," unless otherwise specifically emphasized. The terms "comprising," "including," "having," and variations thereof mean "including but not limited to," unless otherwise specifically emphasized.

[0040] To address the issues of low accuracy and location jumps in existing positioning calibration methods, this application provides a positioning calibration method based on UWB and vision. This method acquires the UWB and visual coordinates of the target object at multiple times when the calibration period arrives. Then, it preprocesses all UWB coordinates using an infinite Kalman filter to obtain the final UWB coordinates for all times. When the target object has not been calibrated, it calibrates the final UWB and visual coordinates at the current time based on all final UWB and visual coordinates to obtain the target object's initial calibration coordinates at the current time. When the target object has already been calibrated, it recalibrates and smooths the final UWB and visual coordinates at the current time to obtain the recalibrated coordinates of the target object at each future time point in the future time period. Preprocessing UWB coordinates can reduce UWB coordinate jitter error; recalibrating calibrated target objects can correct errors that may have occurred in the previous calibration, thereby improving the accuracy of positioning calibration; and smoothing positioning calibration can allow the calibration results to gradually transition to new calibration results at future time points, avoiding positioning jumps.

[0041] The following is an illustrative description of the localization and calibration method based on UWB and vision provided in this application.

[0042] like Figure 1 As shown, the localization and calibration method based on UWB and vision provided in this application includes the following steps: Step 11: When the calibration period arrives, obtain the UWB coordinates and visual coordinates of the target object at multiple times.

[0043] The last moment in a series of time points is the current moment, and the moments preceding the current moment are all historical moments. The target object mentioned above is the object that needs to be located and calibrated (such as drones in a drone swarm, autonomous vehicles, etc.).

[0044] In some embodiments of this application, UWB coordinates can be obtained using a UWB positioning system. UWB coordinates are coordinates in the UWB coordinate system. Image information of the target object can be obtained using a camera or the like, and then visual coordinates can be estimated using a computer vision algorithm (such as the SLAM algorithm). Visual coordinates are coordinates in the visual coordinate system.

[0045] For example, the calibration period is the time interval between two adjacent moments. If the current moment is the 5th second, and there are five moments in total, then the five moments are the 4th second, 4th second 250 milliseconds, 4th second 500 milliseconds, 4th second 750 milliseconds, and 5th second. With a calibration period of 250 milliseconds, the calibration period is from the 5th second to the 5th second 250 milliseconds. At the 5th second 250 millisecond, the 5th second 250 millisecond is taken as the current moment, and the target object at the 5th second 250 millisecond is located and calibrated. In practical applications, calibration often occurs within a very short time. For example, if the visual coordinate data output frequency is 200 Hz, then there can be two hundred data points per second, meaning there can be two hundred moments within one second. The calibration period and the number of moments can be set according to the data output frequency and actual calibration requirements in the actual application.

[0046] It should be noted that, in order to avoid inaccurate calibration due to sampling in a static state, the UWB coordinates and visual coordinates at a certain moment will only be obtained when sufficient motion 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).

[0047] Step 12: Use the boundless Kalman filter to preprocess all UWB coordinates to obtain the final UWB coordinates at all times.

[0048] Specifically, an unscented Kalman filter (UKF) is applied to each UWB coordinate to obtain the final UWB coordinate corresponding to each UWB coordinate.

[0049] For example, the process of unscented Kalman filtering is as follows: Define the state variables and prediction equations for an unscented Kalman filter. :

[0050]

[0051] in, and These are the x and y coordinates in the UWB coordinate system, respectively. and These are the velocity components along the horizontal axis and the velocity components along the vertical axis, respectively. Indicates time interval, For the first State variables, To predict state variables.

[0052] Since this step only processes location information, only the location is observed, i.e.:

[0053]

[0054] in, For predicted state variables that only include location state variables, This indicates that only the x-coordinate of the state variable, including its position, is included. This indicates that the ordinate of the state variable only includes the position.

[0055] Generate sigma points:

[0056]

[0057]

[0058] in, Indicates the first The coordinate vectors of several Sigma points in the state space. UKF characterizes the Gaussian distribution under nonlinear transformations by selecting several weighted Sigma points. This indicates that at time step k, the mean state (posterior mean) after the measurement at that moment is incorporated. This represents the state covariance (posterior covariance) obtained after fusing measurements at time step k. The dimension of the state vector is represented by λ, which is a combination coefficient related to the hyperparameters α, β, and κ of the unscented transformation. Its calculation formula is λ = α^2 (n + κ). n is set to α=0.1, β=2, and κ=0, respectively, with the three hyperparameters set to α=0.1, β=2, and κ=0.

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

[0060] Calculate the prior mean and prior covariance: The prior mean is:

[0061] in, It is the weight of the mean.

[0062] The prior covariance is:

[0063] in, These are the weights of the covariance, where Q is the process noise covariance matrix. After completing this step, the prior estimate is obtained. Covariance .

[0064] Therefore, for each prior sigma point The measurement space can be calculated. :

[0065] Calculate the measurement prediction mean and measurement covariance: Measure the predicted mean for:

[0066] Measure the predicted variance for:

[0067] Where R is the measurement noise covariance.

[0068] Calculate cross covariance :

[0069] Calculate Kalman gain :

[0070] Calculate the posterior state mean :

[0071] Calculate the posterior covariance :

[0072] Finally, output the posterior state. The numbers of the first two dimensions are used as the plane coordinates (x, y) of the UWB to output the final UWB coordinates.

[0073] Step 13: When the target object has not been calibrated, calibrate the final UWB coordinates and visual coordinates at the current moment based on all final UWB coordinates and all visual coordinates to obtain the first calibration coordinates of the target object at the current moment.

[0074] In some embodiments of this application, the step of calibrating the final UWB coordinates and visual coordinates at the current moment based on all final UWB coordinates and all visual coordinates to obtain the initial calibration coordinates of the target object at the current moment includes: The first step is to calculate the first centroid of all final UWB coordinates and transform each final UWB coordinate to a coordinate system centered on the first centroid, thus obtaining the UWB transformed coordinates corresponding to each final UWB coordinate.

[0075] Specifically, through the formula:

[0076] Calculate the first centroid .

[0077] in, Represents the set of indices for all times. Indicates the number of moments. Indicates the first The final UWB coordinates.

[0078] Through the formula:

[0079] Calculate the first UWB transformed coordinates .

[0080] The second step is to calculate the second centroid of all visual coordinates and transform each visual coordinate to a coordinate system centered on the second centroid, thus obtaining the visual transformation coordinates corresponding to each visual coordinate.

[0081] Specifically, through the formula:

[0082] Calculate the second centroid .

[0083] in, Indicates the first The final UWB coordinates.

[0084] Through the formula:

[0085] Calculate the first UWB transformed coordinates .

[0086] The third step is to calculate the rotation matrix and translation vector based on all UWB transformation coordinates and all visual transformation coordinates.

[0087] Specifically, through the formula:

[0088]

[0089]

[0090] Calculate the rotation matrix Translation vector .

[0091] in, Represents the cross covariance matrix. Describes a right singular vector matrix. Describes a left singular vector matrix. Represents a singular value diagonal matrix. This represents a matrix consisting of all visual transformation coordinates. This represents the matrix consisting of all UWB transformed coordinates.

[0092] The fourth step is to obtain the current calibration visual coordinates based on the rotation matrix and translation vector.

[0093] Through the formula:

[0094] Calculate the calibrated visual coordinates at the current moment. .

[0095] in, This represents the final UWB coordinates at the current moment.

[0096] The fifth step is to 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.

[0097] The coordinates used in the initial calibration are those in the UWB coordinate system.

[0098] Through the formula:

[0099]

[0100] Calculate the initial calibration coordinates .

[0101] in, Indicates the weight of UWB coordinates. , This represents the variance of the UWB position error. The weights representing visual coordinates, , This represents the variance of visual position error. This represents the x-coordinate in the final UWB coordinates at the current moment. This represents the ordinate in the final UWB coordinates at the current moment. This represents the x-coordinate in the calibrated visual coordinate system at the current moment. This represents the ordinate in the calibrated visual coordinates at the current moment.

[0102] 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.

[0103] The starting point of the aforementioned future time period is the current moment, and the ending point is the next moment for positioning and calibration (the future time period is a calibration cycle starting from the current moment, and multiple future time points are time points within this calibration cycle; for example, if the future time period is from the 5th second to the 5th second 250 milliseconds, there are a total of 5 future time points, namely the 5th second, the 5th second 62.5 milliseconds, the 5th second 125 milliseconds, the 5th second 187.5 milliseconds, and the 5th second 250 milliseconds). For the current moment when positioning and calibration are required, it is determined whether the target object has been calibrated before the current moment, and one of steps 13 and 14 is selected for execution. In practical applications, step 13 is only executed when the target object is initially calibrated; after the initial calibration, step 14 is executed for each subsequent calibration.

[0104] In some embodiments of this application, the above-mentioned 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 a future time period includes: The first step is to calculate the initial calibration coordinates at the current moment based on the rotation matrix and translation vector from the previous calibration.

[0105] Specifically, the calibration visual coordinates at the current moment are obtained based on the rotation matrix and translation vector of the previous calibration. The calibration visual coordinates are then fused with the final UWB coordinates at the current moment to obtain the initial calibration coordinates of the target object at the current moment.

[0106] It should be noted that the formula for calculating the calibration visual coordinates is the same as the formula for calculating the calibration visual coordinates in step 13. The rotation matrix and translation vector from the previous calibration are substituted into the formula for calculating the calibration visual coordinates in step 13 to obtain the calibration visual coordinates in this step. The formula for calculating the initial calibration coordinates is the same as the formula for calculating the first calibration coordinates in step 13. The calibration visual coordinates in this step and the final UWB coordinates at the current moment are substituted into the formula for calculating the first calibration coordinates in step 13 to obtain the initial calibration coordinates.

[0107] The second step is to determine whether the initial calibration coordinates meet the recalibration conditions.

[0108] The recalibration conditions are:

[0109] in, Indicates error. This represents the x-coordinate in the initial calibration coordinate system. This represents the ordinate in the initial calibration coordinate system. This represents the x-coordinate in the final UWB coordinates at the current moment. This represents the ordinate in the final UWB coordinates at the current moment. Represents a constant. This indicates the error threshold.

[0110] 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.

[0111] Otherwise, the initial calibration coordinates will be used as the recalibrated coordinates for each future time point in the future time period.

[0112] It should be noted that in practical applications, to prevent misjudgment of recalibration conditions or frequent recalibration, a preset time length can be set. The recalibration condition is considered satisfied only if it is continuously met within this preset time length. The rotation matrix and translation vector at the current moment are calculated using the steps in step 13 that calculate the rotation matrix and translation vector.

[0113] The steps described above, which involve recalibrating and smoothing the final UWB 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 as follows: For each future point in time within a future time period, perform the following steps: Calculate the difference ratio at future time points and determine whether the difference ratio is less than the difference threshold.

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

[0115] The calibration visual coordinates of the future time point are obtained based on the rotation matrix and translation vector of the future time point.

[0116] By fusing the calibrated visual coordinates at future points in time with the final UWB coordinates at the current moment, the recalibrated coordinates of the target object at future points in time are obtained.

[0117] It should be noted that if the difference ratio is greater than or equal to the difference threshold, the positioning and calibration is considered to be completed at that 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 quaternion conversion algorithm can be used to convert the spherical difference into the rotation matrix of the future time point. Using the formula for calculating the calibration visual coordinates in step 13, the rotation matrix and translation vector of the future time point are substituted into the formula to obtain the calibration visual coordinates of the future time point. Using the formula for calculating the initial calibration coordinates in step 13, the calibration visual coordinates of the future time point and the final UWB coordinates at the current time are substituted into the formula to obtain the recalibration coordinates of the target object at the future time point.

[0118] The specific process for calculating the difference ratio is as follows: Specifically, through the formula:

[0119] Calculate future time points The difference ratio .

[0120] in, Indicates the first future point in time. Indicates the duration of a future time period.

[0121] The specific process for calculating the spherical difference is as follows: Through the formula:

[0122] Calculate the rotation matrix at future time points .

[0123] in, This indicates the processing of spherical differences. The quaternion representing the rotation matrix at the current moment. This represents the quaternion of the rotation matrix that was last calibrated.

[0124] The specific process for calculating the translation vector at future time points is as follows: Through the formula:

[0125] Calculate the translation vector at future time points .

[0126] in, This represents the translation vector at the current moment. This represents the translation vector from the previous calibration.

[0127] For example, the process of converting a rotation matrix into a quaternion is as follows: Add all the elements on the main diagonal of the rotation matrix to obtain the trace, and determine the branch based on the trace: If the matrix trace Then a simplified formula can be used:

[0128] Calculate the real part of a quaternion and the virtual part , , .

[0129] If the matrix trace Then it is necessary to determine which term on the diagonal of the matrix is ​​the largest and apply the corresponding formula; for example, if If it is the largest, then:

[0130]

[0131] Other branches ( or (Largest) similar.

[0132] Obtain quaternions Then, it is normalized to eliminate data errors.

[0133] The above process for handling spherical differences is as follows: 1. Pretreatment: If two quaternions (A dot product less than 0 indicates that their included angle is >90°), so let This is done to obtain a smaller angle difference and avoid reversal. Among these, The quaternion of the rotation matrix from the previous calibration. Let be the quaternion of the rotation matrix at the current moment.

[0134] like If the two values ​​are close enough to 1, they are almost identical and can be approximated by a linear difference.

[0135] 2. Calculate the inner product :

[0136] 3. Calculation and :

[0137]

[0138] 4. Calculate the rotation matrix :

[0139] For example, after calibration or recalibration 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.

[0140] The method of this application will be illustrated below with a specific example.

[0141] like Figure 2 As shown, the system first initializes, buffers UWB positioning data and visual positioning data, then collects UWB and visual data, updates the unscented Kalman filter state, and enters the calibration or recalibration process. It determines whether the sensor group has reached the motion threshold. If not, no data processing is performed. If so, sufficient paired points are collected, and the rotation matrix R and translation vector T are calculated. Then, it determines whether it is the initial calibration. If so, R and T are used. Otherwise, R and T are updated smoothly. Then, it enters the steps of outputting the fused positioning result and error detection. It uses R and T to perform coordinate system transformation and output the fused positioning result. It calculates and determines whether the error is greater than the threshold. If so, it returns to the step of determining whether the sensor group has reached the motion threshold. Otherwise, it returns to the step of using R and T to perform coordinate system transformation and output the fused positioning result.

[0142] It is worth mentioning that preprocessing UWB coordinates can reduce the jitter error of UWB coordinates. Recalibrating the calibrated target object can correct the errors that may have occurred in the previous calibration, thereby improving the accuracy of positioning calibration. Smoothing the positioning calibration can allow the calibration results to gradually transition to the new calibration results at future points in time, avoiding positioning jumps.

[0143] Furthermore, the beneficial effects of this application are also reflected in: High degree of automation: The initial calibration and recalibration processes do not require manual intervention. As long as a certain amount of motion is met, the paired points (UWB coordinates and corresponding visual coordinates) can be collected to obtain the accurate rotation matrix R and offset vector T.

[0144] Maintaining accuracy over a long period: Because this method includes error detection and self-triggered recalibration, it can automatically correct the calibration even if the device is displaced, tilted, or the environment changes, thus ensuring that the fusion results of UWB and vision remain consistent even after long-term operation.

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

[0146] Scalability: This method is not only applicable to two-dimensional planar positioning, but can also be used in three-dimensional scenes (such as drones and AR / VR applications), by extending the rotation matrix and three-dimensional coordinate transformation accordingly.

[0147] UKF can also be replaced with other filtering schemes (such as EKF, PF particle filter), or the vision module can be replaced with other positioning methods (such as LiDAR SLAM), only the corresponding data fusion interface needs to be adjusted.

[0148] The following is an exemplary description of the positioning and calibration device based on UWB and vision provided in this application.

[0149] like Figure 3 As shown, this application embodiment provides a positioning and calibration device based on UWB and vision. The positioning and calibration device 300 based on UWB and vision includes: The acquisition module 301 is used to acquire the UWB coordinates and visual coordinates of the target object at multiple moments when the calibration period arrives; the last moment among the multiple moments is the current moment. The preprocessing module 302 is used to preprocess all UWB coordinates using an infinite Kalman filter to obtain the final UWB coordinates at all times. The calibration module 303 is used to calibrate the final UWB coordinates and visual coordinates at the current moment based on all final UWB coordinates and all visual coordinates when the target object has not been calibrated, so as to obtain the first calibration coordinates of the target object at the current moment. The recalibration module 304 is used to recalibrate and smooth the final UWB coordinates and visual coordinates at the current moment when the target object has been calibrated, so as to obtain the recalibrated coordinates of the target object at each future time point in the future time period.

[0150] It should be noted that the information interaction and execution process between the above-mentioned devices / units are based on the same concept as the method embodiments of this application. For details on their specific functions and technical effects, please refer to the method embodiments section, and they will not be repeated here.

[0151] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0152] like Figure 4 As shown, an embodiment of this application provides a terminal device, wherein the terminal device D10 of this embodiment includes: at least one processor D100 ( Figure 4 The diagram shows only one processor, a memory D101, and a computer program D102 stored in the memory D101 and executable on the at least one processor D100, wherein the processor D100 executes the computer program D102 to implement the steps in any of the above method embodiments.

[0153] Specifically, when the processor D100 executes the computer program D102, it acquires the UWB coordinates and visual coordinates of the target object at multiple times when the calibration period arrives. Then, it preprocesses all UWB coordinates using an infinite Kalman filter to obtain the final UWB coordinates for all times. When the target object has not been calibrated, it calibrates the final UWB coordinates and visual coordinates at the current time based on all final UWB coordinates and all visual coordinates to obtain the initial calibration coordinates of the target object at the current time. When the target object has already been calibrated, it recalibrates and smooths the final UWB coordinates and visual coordinates at the current time to obtain the recalibrated coordinates of the target object at each future time point in the future time period. Preprocessing the UWB coordinates reduces UWB coordinate jitter error; recalibrating the calibrated target object corrects errors that may have occurred in the previous calibration, thereby improving the accuracy of the positioning calibration; and smoothing the positioning calibration allows the calibration results to gradually transition to new calibration results at future time points, avoiding positioning jumps.

[0154] The processor D100 can be a central processing unit (CPU), or it can be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor.

[0155] In some embodiments, the memory D101 may be an internal storage unit of the terminal device D10, such as a hard disk or memory of the terminal device D10. In other embodiments, the memory D101 may be an external storage device of the terminal device D10, such as a plug-in hard disk, smart media card (SMC), secure digital card (SD), flash card, etc., equipped on the terminal device D10. Furthermore, the memory D101 may include both internal and external storage units of the terminal device D10. The memory D101 is used to store the operating system, applications, bootloader, data, and other programs, such as the program code of the computer program. The memory D101 can also be used to temporarily store data that has been output or will be output.

[0156] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps described in the various method embodiments above.

[0157] This application provides a computer program product that, when run on a terminal device, enables the terminal device to implement the steps described in the various method embodiments above.

[0158] If the integrated unit is implemented as 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, all or part of the processes in the above-described embodiments of this application can be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include at least: any entity or device capable of carrying the computer program code to a UWB-based 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. Examples include USB flash drives, portable hard drives, magnetic disks, or optical disks. In some jurisdictions, according to legislation and patent practice, computer-readable media cannot be electrical carrier signals or telecommunication signals.

[0159] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0160] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0161] The above description is the preferred embodiment of this application. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principles described in this application, and these improvements and modifications should also be considered within the scope of protection of this application.

Claims

1. A localization and 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 among these multiple moments is the current moment. All UWB coordinates are preprocessed using unscented Kalman filtering to obtain the final UWB coordinates at all times. When the target object has not been calibrated, the final UWB coordinates and visual coordinates at the current moment are calibrated based on all final UWB coordinates and all visual coordinates to obtain the first calibration 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; The step of calibrating the final UWB coordinates and visual coordinates at the current moment based on all final UWB coordinates and all visual coordinates to obtain the initial calibration 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 to a coordinate system centered on the first centroid, to obtain the UWB transformed coordinates corresponding to each final UWB coordinate; Calculate the second centroid of all visual coordinates, and transform each visual coordinate to a coordinate system centered on the second centroid, to obtain the visual transformed coordinates corresponding to each visual coordinate; Calculate the rotation matrix and translation vector based on all UWB transformation coordinates and all visual transformation coordinates; The calibrated visual coordinates at the current moment are obtained based on the rotation matrix and the translation vector; The calibrated visual coordinates are fused with the final UWB coordinates at the current moment to obtain the initial calibrated coordinates of the target object at the current moment; The step of fusing 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 includes: Through the formula: Calculate the initial calibration coordinates ; in, Indicates the weight of UWB coordinates. , This represents the variance of the UWB position error. The weights representing visual coordinates, , This represents the variance of visual position error. This represents the x-coordinate in the final UWB coordinates at the current moment. This represents the ordinate in the final UWB coordinates at the current moment. This represents the x-coordinate in the calibrated visual coordinate system at the current moment. The vertical coordinate in the calibrated visual coordinate system at the current moment; The 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: Calculate the initial calibration coordinates at the current moment based on the rotation matrix and translation vector from the previous calibration. Determine whether the initial calibration coordinates meet the recalibration conditions; 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. Otherwise, the initial calibration coordinates will be used as the recalibrated coordinates for each future time point in the future time period; The recalibration conditions are as follows: in, Indicates error. This represents the x-coordinate in the initial calibration coordinate system. This represents the ordinate in the initial calibration coordinate system. This represents the x-coordinate in the final UWB coordinates at the current moment. This represents the ordinate in the final UWB coordinates at the current moment. Represents a constant. Indicates the error threshold; 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 point in time within the aforementioned future time period, perform the following steps: Calculate the difference ratio at the future time points and determine whether the difference ratio is less than the difference threshold; If so, then calculate the spherical difference based on the rotation matrix at the current time and the difference ratio, and convert the spherical difference into 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 time and the difference ratio; Based on the rotation matrix and translation vector of the future time point, the calibration visual coordinates of the future time point are obtained; The calibrated visual coordinates at the future time point are fused with the final UWB coordinates at the current time to obtain the recalibrated coordinates of the target object at the future time point.

2. The positioning and calibration method according to claim 1, characterized in that, The calculation of the first centroid of all final UWB coordinates includes: Through the formula: Calculate the first centroid ; in, Represents the set of indices for all times. Indicates the number of moments. Indicates the first One final UWB coordinate; The step of transforming each final UWB coordinate to a coordinate system centered on the first centroid to obtain the UWB transformed coordinates corresponding to each final UWB coordinate includes: Through the formula: Calculate the first UWB transformed coordinates ; The calculation of the second centroid of all visual coordinates includes: Through the formula: Calculate the second centroid ; in, Indicates the first One final UWB coordinate; The process of transforming each visual coordinate to a coordinate system centered on the second centroid to obtain the visual transformation coordinates corresponding to each visual coordinate includes: Through the formula: Calculate the first UWB transformed coordinates .

3. The positioning and calibration method according to claim 2, characterized in that, The calculation of the rotation matrix and translation vector based on all UWB transformation coordinates and all visual transformation coordinates includes: Through the formula: Calculate the rotation matrix Translation vector ; in, Represents the cross covariance matrix. Describes a right singular vector matrix. Describes a left singular vector matrix. Represents a singular value diagonal matrix. This represents a matrix consisting of all visual transformation coordinates. This represents the matrix consisting of all UWB transformed coordinates; The step of obtaining the current calibration visual coordinates based on the rotation matrix and the translation vector includes: Through the formula: Calculate the calibrated visual coordinates at the current moment. ; in, This represents the final UWB coordinates at the current moment.

4. The positioning and calibration method according to claim 1, characterized in that, The calculation of the difference ratio at the future time points includes: Through the formula: Calculate future time points The difference ratio ; in, Indicates the first future point in time. Indicates the duration of a future time period; The calculation of the spherical difference based on the rotation matrix at the current moment and the difference ratio includes: Through the formula: Calculate the rotation matrix at future time points ; in, This indicates the processing of spherical differences. The quaternion representing the rotation matrix at the current moment. The quaternion representing the rotation matrix of the last calibration; The calculation of the translation vector at the future time point based on the translation vector at the current time and the difference ratio includes: Through the formula: Calculate the translation vector at future time points ; in, This represents the translation vector at the current moment. This represents the translation vector from the previous calibration.

5. 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, it implements the UWB-based and vision-based localization and calibration method as described in any one of claims 1 to 4.

Images