Multi-source fusion positioning system-oriented UWB external parameter automatic calibration method and system

Abstract

The invention discloses a UWB external parameter automatic calibration method and system for a multi-source fusion positioning system, and relates to the technical field of positioning. The problems that UWB parameters are simply updated based on a filtering method in an existing automatic calibration method, but the convergence speed of the method is low, and calibration is inaccurate due to the fact that degeneration motion possibly occurs when calibration is directly carried out are solved. The method comprises the following steps: carrying out data acquisition according to a multi-source fusion positioning system, wherein the acquired data comprises image data, IMU measurement data and UWB distance measurement data; observability compensation is carried out on the UWB external parameter initialization process; and updating the UWB external parameters on line according to a filtering method, obtaining a final UWB external parameter value, and completing calibration. The method is applied to sensor positioning.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to a UWB external parameter automatic calibration method for a multi-source fusion positioning system. Background Art

[0002] In recent years, with the widespread application of autonomous robot systems in various scenarios, accurate and low-drift state estimation is essential for the stable operation of the system. The global satellite navigation and positioning system cannot be used under satellite denial conditions, resulting in cumulative drift errors in positioning. As a global radio positioning sensor, UWB can replace GPS in satellite denial environments to limit the divergence of drift. The multi-source fusion positioning system based on UWB sensors can take into account the long-term stability and rapid response of positioning, and can achieve low-cost, robust and accurate indoor positioning. In the multi-source fusion positioning system based on UWB, the external parameters of the UWB sensor have a great influence on the positioning accuracy, and this parameter is usually calibrated manually. The cumbersome steps limit the application of the system.

[0003] In addition, when the positioning system runs for a long time, the spatial offset of the UWB sensor will change, thus affecting the positioning accuracy.

[0004] In order to solve the problem of UWB extrinsic parameter calibration in multi-source fusion positioning systems, existing automatic calibration methods often simply update UWB parameters based on filtering methods, but this method has a slow convergence speed. In addition, the calibration observability of UWB extrinsic parameters is affected by the system motion mode, and direct calibration may encounter degraded motion conditions, resulting in inaccurate calibration. In summary, existing methods cannot quickly and robustly calibrate UWB extrinsic parameters. Summary of the invention

[0005] The present invention aims at the problem that the existing automatic calibration method often simply updates UWB parameters based on the filtering method, but the method has a slow convergence speed and may encounter degenerate motion conditions when directly performing calibration, resulting in inaccurate calibration. A UWB external parameter automatic calibration method for a multi-source fusion positioning system is proposed. The method includes:

[0006] Step S1: performing data collection according to a multi-source fusion positioning system, wherein the collected data includes: image data, IMU measurement data, and UWB ranging data;

[0007] Step S2: Observability compensation for the UWB external parameter initialization process;

[0008] Step S3: Update the UWB extrinsic parameters online according to the filtering method, obtain the final UWB extrinsic parameter value, and complete the calibration.

[0009] Furthermore, a preferred embodiment is proposed, in which the multi-source fusion positioning system in step S1 comprises:

[0010] The multi-source fusion positioning system in step S1 includes:

[0011] State quantity setting module, IMU information processing module, visual information processing module and UWB information processing module;

[0012] The state quantity setting module includes setting the IMU state quantity, the visual feature state quantity, the UWB anchor point state quantity, and the UWB external parameter state quantity:

[0013]

[0014] in is the carrier attitude state represented by the quaternion, and is the carrier velocity and position state quantity, b g,k and b a,k is the gyroscope and accelerometer bias, are the positions of all visual feature points in the global coordinate system, is the position of all UWB anchor points in the global coordinate system is the offset of the UWB tag on the carrier in the IMU coordinate system;

[0015] The IMU information processing includes:

[0016] The IMU state propagation equation participates in filtering. The state propagation equation is as follows:

[0017]

[0018] in, is the measurement update propagation from time k to time k+1, a m and ω m is the raw IMU measurement, f is the general nonlinear motion model of the IMU;

[0019] The covariance is calculated as follows:

[0020]

[0021] Where Φ and Q are the state transfer matrix and noise matrix respectively;

[0022] The visual information processing module comprises:

[0023] Maintain an additional IMU pose clone The length is S; the position of the feature point in the camera frame is C p f,j =[x j ,y j ,z j ],j=k+1,…,kS, the visual measurement is represented as the clone pose zj and feature location C p f,j Function:

[0024]

[0025] in G p f is the global position of the feature point, n z is Gaussian white noise, and C p I is the camera extrinsic parameter.

[0026] The UWB information processing module includes:

[0027] The UWB ranging model is:

[0028] z m =h m +n r =|| G p U - G p a ||+γ+n r ,

[0029] Among them, z m is the real distance measurement data. is the global position of the UWB tag on the carrier, G p a is the global position of the UWB anchor point, γ is the constant deviation of the ranging data, n r Gaussian white noise for ranging;

[0030] For each distance measurement between the UWB anchor point a and the UWB tag U, the Jacobian matrix H z Obtained by the following formula:

[0031]

[0032] in, is the observed value of UWB ranging, is the posture state, is the position status, For UWB external reference, is the global posture of the carrier, for in is the global position of the anchor point.

[0033] Furthermore, a preferred method is proposed, wherein step S2 comprises:

[0034] The positioning result is obtained through the odometer, and the initial value of the UWB external parameters is set;

[0035] According to the global position of the UWB anchor point, the real distance measurement data between the UWB tag and the anchor point is obtained;

[0036] Construct the ranging residual model between UWB tag and UWB anchor point;

[0037] Calculate the Jacobian matrix of each residual block in the range finding residual model;

[0038] Compensate for the observability of the Jacobian matrix.

[0039] Furthermore, a preferred method is proposed, wherein the step of constructing a ranging residual model between a UWB tag and a UWB anchor point comprises:

[0040]

[0041] Among them, r t is the ranging residual model between the UWB tag and the UWB anchor point, d t is the actual ranging data between the UWB tag and the UWB anchor point, γ is the constant deviation of the ranging data, G p I,t and is the current positioning result of VIO. G p a is a known UWB anchor point a j The global position of is a good initial value of the UWB extrinsic parameter to be estimated.

[0042] Furthermore, a preferred method is proposed, wherein the compensating for the observability of the Jacobian matrix comprises:

[0043] Perform singular value decomposition on the Jacobian matrix to identify unobservable directions;

[0044] Calculate the eigenvalues ​​of the Hessian matrix, perform a threshold check on the eigenvalues ​​of the new data, and determine whether the new data is observable;

[0045] Add a priori constraints in unobservable directions;

[0046] Formulate optimization problems and solve them using the Levenberg-Marquardt algorithm;

[0047] After the algorithm stops, the final optimization results are evaluated, including the estimated values ​​of the extrinsic parameters and the size of the residuals.

[0048] Furthermore, a preferred method is proposed, wherein the optimization problem is constructed and the Levenberg-Marquardt algorithm is used to solve the optimization problem, including:

[0049] The optimization problem is:

[0050]

[0051] Among them, S r is the set of UWB tags and UWB anchor points at different time points, S e is the set of all unobservable directions, e t,i Prior constraints added for unobservable directions, T represents transposition;

[0052] Suppose the optimization problem is The optimization steps of the LM algorithm are:

[0053]

[0054] Among them, J k is the Jacobian matrix of the optimization problem, μ k is the damping factor of the algorithm to adjust the step size, is the covariance matrix of the estimated parameters and I is the identity matrix.

[0055] Furthermore, a preferred method is proposed, wherein step S3 comprises:

[0056]

[0057] in, is the observed value of UWB ranging, is the posture state, is the position status, For UWB external reference, is the global posture of the carrier, for in is the global position of the anchor point.

[0058] Based on the same inventive concept, the present invention also proposes a UWB external parameter automatic calibration system for a multi-source fusion positioning system, the system comprising:

[0059] A data acquisition unit, used for acquiring data according to a multi-source fusion positioning system, wherein the acquired data includes: image data, IMU measurement data, and UWB ranging data;

[0060] A compensation unit, used for observability compensation of the UWB external parameter initialization process;

[0061] The calibration unit is used to update the UWB extrinsic parameters online according to the filtering method, obtain the final UWB extrinsic parameter value, and complete the calibration.

[0062] Based on the same inventive concept, the present invention also proposes a computer device, including a memory and a processor, wherein the memory stores a computer program. When the processor runs the computer program stored in the memory, the processor executes a UWB external parameter automatic calibration method for a multi-source fusion positioning system as described in any one of the above items.

[0063] Based on the same inventive concept, the present invention also proposes a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of a UWB external parameter automatic calibration method for a multi-source fusion positioning system as described in any of the above items are executed.

[0064] The present invention is beneficial in that:

[0065] Traditional filtering methods have a slow convergence speed when updating UWB parameters. In the UWB external parameter automatic calibration method for a multi-source fusion positioning system proposed in the present invention, the efficiency of the calibration process is improved by introducing multi-source data fusion. During the calibration process, degenerate motion (such as low speed or static state) may be encountered, and traditional methods may lead to inaccurate calibration. The present invention overcomes this problem and improves the accuracy of calibration by fusing multi-source data. The present invention adopts an online update method so that the calibration of UWB external parameters can be carried out in real time to meet the application requirements in dynamic environments. By combining image data, IMU (inertial measurement unit) measurement data and UWB ranging data, a more comprehensive environmental perception is achieved, and the accuracy and reliability of calibration are improved. The automatic calibration feature of this method reduces manual intervention, improves the degree of automation of the system, and is convenient for large-scale application.

[0066] The invention is applied to the field of sensor positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 A flow chart of a UWB external parameter automatic calibration method for a multi-source fusion positioning system according to implementation mode 1;

[0068] Figure 2 This is a schematic diagram of the convergence of calibration of each axis of UWB extrinsic parameters in different calibration module combinations as described in implementation mode eleven, wherein truth represents the true value, intial+online represents the UWB extrinsic parameter calibration method of initialization + online calibration, intial represents the UWB extrinsic parameter calibration method with only initialization, online represents the UWB extrinsic parameter calibration method with only online calibration, Ablation represents the ablation experiment, UWB Offset x(m) represents the offset of the UWB extrinsic parameter x-axis, UWB Offset y(m) represents the offset of the UWB extrinsic parameter y-axis, and UWB Offset z(m) represents the offset of the UWB extrinsic parameter z-axis. DETAILED DESCRIPTION

[0069] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments.

[0070] Implementation method 1, see Figure 1 This embodiment describes a UWB external parameter automatic calibration method for a multi-source fusion positioning system, the method comprising:

[0071] Step S1: performing data collection according to a multi-source fusion positioning system, wherein the collected data includes: image data, IMU measurement data, and UWB ranging data;

[0072] Step S2: Observability compensation for the UWB external parameter initialization process;

[0073] Step S3: Update the UWB extrinsic parameters online according to the filtering method, obtain the final UWB extrinsic parameter value, and complete the calibration.

[0074] Traditional filtering methods have a slow convergence speed when updating UWB parameters, while the UWB external parameter automatic calibration method for a multi-source fusion positioning system proposed in this embodiment improves the efficiency of the calibration process by introducing multi-source data fusion. During the calibration process, degenerate motion (such as low speed or static state) may be encountered, and traditional methods may lead to inaccurate calibration. In this embodiment, this problem is overcome by fusing multi-source data, and the accuracy of calibration is improved. In this embodiment, an online update method is adopted, so that the calibration of UWB external parameters can be carried out in real time to meet the application requirements in dynamic environments. By combining image data, IMU (inertial measurement unit) measurement data and UWB ranging data, a more comprehensive environmental perception is achieved, and the accuracy and reliability of calibration are improved. The automatic calibration feature of this method reduces manual intervention, improves the degree of automation of the system, and is convenient for large-scale application.

[0075] The principle of the UWB external parameter automatic calibration method for a multi-source fusion positioning system proposed in this embodiment is based on multi-source data fusion and filtering update technology. The specific steps are as follows:

[0076] Data collection (step S1):

[0077] The multi-source fusion positioning system collects a variety of sensor data, including image data, IMU measurement data, and UWB ranging data. These data provide rich environmental information, which can complement each other and improve the overall performance of the system.

[0078] Observability compensation (step S2):

[0079] When initializing UWB external parameters, there may be insufficient observability. This step aims to compensate for this defect through a specific algorithm to ensure the accuracy of the initialization process. This can be done by analyzing existing data and identifying key observation information, thereby improving the reliability of initialization.

[0080] Online update (step S3):

[0081] This step applies a filtering method (such as Kalman filtering or extended Kalman filtering) to update the UWB extrinsic parameters in real time. By processing the real-time collected data, the algorithm can continuously adjust and optimize the UWB extrinsic parameters, and finally obtain accurate UWB extrinsic parameter values ​​to complete the calibration.

[0082] Embodiment 2: This embodiment further limits the UWB external parameter automatic calibration method for a multi-source fusion positioning system described in Embodiment 1. In step S1, the multi-source fusion positioning system includes:

[0083] State quantity setting module, IMU information processing module, visual information processing module and UWB information processing module;

[0084] The state quantity setting module includes setting the IMU state quantity, the visual feature state quantity, the UWB anchor point state quantity, and the UWB external parameter state quantity:

[0085]

[0086] in is the carrier attitude state represented by the quaternion, and is the carrier velocity and position state quantity, b g,k and b a,k is the gyroscope and accelerometer bias, are the positions of all visual feature points in the global coordinate system,

[0087] is the position of all UWB anchor points in the global coordinate system is the offset of the UWB tag on the carrier in the IMU coordinate system;

[0088] The IMU information processing includes:

[0089] The IMU state propagation equation participates in filtering. The state propagation equation is as follows:

[0090]

[0091] in, is the measurement update propagation from time k to time k+1, a m and ω m is the raw IMU measurement, f is the general nonlinear motion model of the IMU;

[0092] The covariance is calculated as follows:

[0093]

[0094] Where Φ and Q are the state transfer matrix and noise matrix respectively;

[0095] The visual information processing module comprises:

[0096] Maintain an additional IMU pose clone The length is S; the position of the feature point in the camera frame is C p f,j =[x j ,y j ,z j ],j=k+1,…,kS, the visual measurement is represented as the clone pose z j and feature location C p f,j Function:

[0097]

[0098] in G p f is the global position of the feature point, n z is Gaussian white noise, and C p I is the camera extrinsic parameter.

[0099] The UWB information processing module includes:

[0100] The UWB ranging model is:

[0101] z m =h m +n r =|| G p U - G p a ||+γ+n r ,

[0102] Among them, z m is the real distance measurement data. is the global position of the UWB tag on the carrier, G p a is the global position of the UWB anchor point, γ is the constant deviation of the ranging data, n r Gaussian white noise for ranging;

[0103] For each distance measurement between the UWB anchor point a and the UWB tag U, the Jacobian matrix H z Obtained by the following formula:

[0104]

[0105] in, is the observed value of UWB ranging, is the posture state, is the position status, For UWB external reference, is the global posture of the carrier, for in is the global position of the anchor point.

[0106] The multi-source fusion positioning system designed in this embodiment includes IMU, visual and UWB information processing, which promotes the scalability and maintainability of the system. Each module can be optimized independently, which improves the flexibility of the overall system. By setting a variety of state quantities (such as IMU state quantities, visual feature state quantities, UWB anchor point state quantities, etc.), it is ensured that all important parameters are considered during the calibration process, thereby improving the comprehensiveness and accuracy of the calibration. The IMU information processing module adopts a general nonlinear motion model, which can more accurately describe the actual motion state, thereby improving the filtering accuracy. This modeling method is more suitable for motion characteristics in complex dynamic environments. Maintaining the cloning of IMU poses and processing of feature points makes the fusion of visual information more accurate, which helps to improve the stability and reliability of visual measurement, especially in low-light or feature-sparse environments. The UWB information processing module improves the ranging accuracy by considering real ranging data, deviations and noise, and can effectively reduce the impact of the external environment on the ranging results. This model provides a more reliable basis for subsequent parameter optimization. The Jacobian matrix of UWB ranging is obtained through a clear formula, which ensures that the changes in system status can be quickly and accurately reflected when the status is updated, improving the real-time performance of the algorithm. By integrating multi-source data and performing collaborative processing, this method can effectively deal with various interferences and abnormal situations, enhance the robustness of the system in different environments, and ensure long-term stable operation. In multi-source fusion, the fusion of IMU, vision and UWB data not only improves the accuracy of calibration, but also speeds up data processing, enabling the system to perform better in real-time applications.

[0107] Implementation method 3: This implementation method further limits the UWB external parameter automatic calibration method for a multi-source fusion positioning system described in implementation method 1, and step S2 includes:

[0108] The positioning result is obtained through the odometer, and the initial value of the UWB external parameters is set;

[0109] According to the global position of the UWB anchor point, the real distance measurement data between the UWB tag and the anchor point is obtained;

[0110] Construct the ranging residual model between UWB tag and UWB anchor point;

[0111] Calculate the Jacobian matrix of each residual block in the range finding residual model;

[0112] Compensate for the observability of the Jacobian matrix.

[0113] In this implementation, the positioning result obtained by the odometer is used as the initial value of the UWB external parameter, which helps to improve the initial accuracy of the entire positioning system. The odometer can usually provide relatively reliable displacement information, and therefore can provide a more accurate starting reference for UWB positioning, reducing the impact of the initial value on the final result. Using the global position of the UWB anchor point to obtain real ranging data can provide more realistic positioning environment information. The use of this data can improve the accuracy of modeling of the positioning system and make the subsequent calibration process more accurate, especially in complex environments. The real ranging data can effectively reduce the impact of environmental noise and other interference on positioning accuracy.

[0114] Furthermore, in this embodiment, by constructing a ranging residual model between the UWB tag and the anchor point, it is helpful for the system to comprehensively analyze the source of ranging errors. By modeling the residual, the errors in the positioning process can be more clearly identified, and then corresponding measures can be taken to compensate and optimize, thereby improving the robustness of the system. Calculating the Jacobian matrix of each residual block in the ranging residual model can provide an important mathematical tool for subsequent optimization. The application of the Jacobian matrix in nonlinear least squares problems can effectively improve the convergence speed and accuracy of parameter estimation. By analyzing the Jacobian matrix, the relationship between system parameters can be better understood, thereby achieving more accurate calibration. Compensating for the observability of the Jacobian matrix can improve the adaptability and stability of the system under different conditions. By identifying and compensating for the unobservability problems that may occur in the Jacobian matrix, the risk of inaccurate calibration due to changes in the system state can be reduced, thereby improving the reliability of the entire positioning system. This method combines multiple data sources (such as odometers and UWB anchor points) and can obtain more comprehensive location information in complex environments by fusing positioning information. This multi-source data fusion strategy not only improves the reliability of the data, but also enhances the system's adaptability in dynamic environments, making the positioning results more accurate.

[0115] Implementation mode 4: This implementation mode is a further limitation of the UWB external parameter automatic calibration method for a multi-source fusion positioning system described in Implementation mode 3. The construction of the ranging residual model between the UWB tag and the UWB anchor point includes:

[0116]

[0117] Among them, r t is the ranging residual model between the UWB tag and the UWB anchor point, d t is the actual ranging data between the UWB tag and the UWB anchor point, γ is the constant deviation of the ranging data, G p I,t and is the current positioning result of VIO. G p a is a known UWB anchor point a j The global position of is a good initial value of the UWB extrinsic parameter to be estimated.

[0118] This embodiment is explained in conjunction with the third embodiment. In this embodiment, by establishing a ranging residual model between the UWB tag and the UWB anchor point, the error in ranging can be effectively identified and corrected. This correction can eliminate the ranging deviation caused by environmental factors (such as multipath effects, reflections, etc.), thereby improving the accuracy of the overall positioning.

[0119] In this embodiment, the distance measurement residual model is combined with the VIO (visual inertial measurement) positioning result to achieve real-time external parameter calibration. This dynamic calibration method can adapt to environmental changes and reduce the time delay and accuracy reduction problems that may occur in traditional static calibration.

[0120] Since this method relies on real-time collected ranging data and VIO results, it can adapt to changes in different scenarios and environments. Whether it is a complex indoor environment or an open space, the system can continuously optimize external parameters based on new data to maintain high-performance positioning capabilities. By introducing a model of real ranging data and constant deviation, the system can maintain stable performance in the presence of noise and interference. In this way, the positioning system has better adaptability to changes and uncertainties in the external environment and reduces the failure rate.

[0121] Implementation mode 5: This implementation mode further limits the UWB external parameter automatic calibration method for a multi-source fusion positioning system described in implementation mode 3. The compensation for the observability of the Jacobian matrix includes:

[0122] Perform singular value decomposition on the Jacobian matrix to identify unobservable directions;

[0123] Calculate the eigenvalues ​​of the Hessian matrix, perform a threshold check on the eigenvalues ​​of the new data, and determine whether the new data is observable;

[0124] Add a priori constraints in unobservable directions;

[0125] Formulate optimization problems and solve them using the Levenberg-Marquardt algorithm;

[0126] After the algorithm stops, the final optimization results are evaluated, including the estimated values ​​of the extrinsic parameters and the size of the residuals.

[0127] In this embodiment, by performing singular value decomposition on the Jacobian matrix, the unobservable direction can be accurately identified. This process enables the system to understand which parameters cannot be effectively estimated through existing data, so that targeted optimization and improvement can be performed in subsequent processing. By calculating the eigenvalues ​​of the Hessian matrix and performing a threshold check, the observability of new data can be evaluated in real time. This dynamic adaptability enables the system to adjust the calibration strategy in a timely manner according to the quality and quantity of the data actually obtained, ensuring that the estimation results of the external parameters are always reliable.

[0128] Furthermore, adding prior constraints in unobservable directions can effectively utilize existing knowledge and information. This strategy can not only compensate for the problem caused by insufficient data, but also improve the overall robustness of the system, especially in complex environments, it can significantly improve positioning accuracy.

[0129] Furthermore, constructing the optimization problem and solving it using the Levenberg-Marquardt algorithm can achieve fast and efficient parameter estimation. With its good convergence and applicability, this algorithm has significant advantages in dealing with large-scale optimization problems, can reduce computing time, and is suitable for real-time applications. After the algorithm stops, evaluating the final optimization results, including the estimated values ​​of external parameters and the size of residuals, can provide important basis for subsequent decision-making. This evaluation mechanism helps to timely identify the credibility and accuracy of the calibration results, and provide data support for further optimization and adjustment of the system.

[0130] Implementation 6: This implementation is a further limitation of the UWB external parameter automatic calibration method for a multi-source fusion positioning system described in Implementation 5. The optimization problem is constructed and the Levenberg-Marquardt algorithm is used to solve the optimization problem, including:

[0131] The optimization problem is:

[0132]

[0133] Among them, S r is the set of UWB tags and UWB anchor points at different time points, S e is the set of all unobservable directions, e t,i Prior constraints added for unobservable directions, T represents transposition;

[0134] Suppose the optimization problem is The optimization steps of the LM algorithm are:

[0135]

[0136] Among them, J k is the Jacobian matrix of the optimization problem, μ k is the damping factor of the algorithm to adjust the step size, is the covariance matrix of the estimated parameters and I is the identity matrix.

[0137] The LM algorithm proposed in this embodiment combines the advantages of the Gauss-Newton method and the gradient descent method, and can converge quickly when dealing with nonlinear least squares problems. By adaptively adjusting the damping coefficient, the convergence speed under different conditions can be effectively coped with. The use of the Jacobian matrix makes the optimization direction and step size adjustment of each step more precise, ensuring the efficiency of parameter estimation. This method combines the data of the UWB tag and the anchor point, makes full use of the observation information at different time points, reduces the error caused by the single source information, and improves the accuracy of positioning. The prior constraints added in the unobservable direction help to reduce the uncertainty of the system and improve the stability and reliability of the final parameter estimation. Since this method is based on the construction of the optimization problem, it can adapt to different environments and application scenarios. Whether it is a dynamic scene or a static scene, optimization can be achieved by adjusting the model parameters and constraints.

[0138] Embodiment 7: This embodiment further limits the UWB external parameter automatic calibration method for a multi-source fusion positioning system described in Embodiment 1, and the step S3 includes:

[0139]

[0140] in, is the observed value of UWB ranging, is the posture state, is the position status, For UWB external reference, is the global posture of the carrier, in is the global position of the anchor point.

[0141] In this embodiment, the UWB external parameters are updated online through the filtering method, and the optimal external parameter value can be obtained in real time. This means that the system can continuously adjust and optimize the parameters according to the latest observation data, thereby improving the accuracy of calibration.

[0142] UWB technology has obvious advantages in high-precision positioning, and this method can further improve positioning accuracy and reduce positioning errors caused by inaccurate external parameters by optimizing external parameters. This calibration method can adapt to changes in different environments and conditions, especially in dynamic scenes. Due to the real-time update of external parameters, the system can quickly adapt to environmental changes and improve the stability and reliability of the positioning system. The method proposed in this embodiment combines a variety of information such as UWB ranging, attitude state and position state, and makes full use of multi-source data for fusion, thereby enhancing the system's anti-interference ability and overall performance. The use of automatic calibration methods reduces the need for manual intervention, making the entire calibration process more efficient and convenient. This not only saves labor costs, but also improves the consistency and repeatability of calibration.

[0143] Embodiment 8: A UWB external parameter automatic calibration system for a multi-source fusion positioning system described in this embodiment includes:

[0144] A data acquisition unit, used for acquiring data according to a multi-source fusion positioning system, wherein the acquired data includes: image data, IMU measurement data, and UWB ranging data;

[0145] A compensation unit, used for observability compensation of the UWB external parameter initialization process;

[0146] The calibration unit is used to update the UWB extrinsic parameters online according to the filtering method, obtain the final UWB extrinsic parameter value, and complete the calibration.

[0147] Embodiment 9. A computer device described in this embodiment includes a memory and a processor, wherein a computer program is stored in the memory. When the processor runs the computer program stored in the memory, the processor executes a UWB external parameter automatic calibration method for a multi-source fusion positioning system described in any one of embodiments 1 to 7.

[0148] Implementation method ten, a computer-readable storage medium described in this implementation method, on which a computer program is stored, and when the computer program is executed by a processor, the steps of a UWB external parameter automatic calibration method for a multi-source fusion positioning system as described in any one of implementation methods one to seven are executed.

[0149] Implementation Method XI: Participation Figure 2 This embodiment provides a specific example of the automatic calibration method of UWB external parameters for a multi-source fusion positioning system described in the first embodiment, and is also used to explain the second to seventh embodiments. Specifically:

[0150] In this embodiment, the multi-source fusion positioning system includes:

[0151] State quantity setting module, IMU information processing module, visual information processing module and UWB information processing module;

[0152] The state quantity setting module includes setting the IMU state quantity, the visual feature state quantity, the UWB anchor point state quantity, and the UWB external parameter state quantity:

[0153]

[0154] in is the carrier attitude state represented by the quaternion, and is the carrier velocity and position state quantity, b g,k and b a,k is the gyroscope and accelerometer bias, are the positions of all visual feature points in the global coordinate system,

[0155] is the position of all UWB anchor points in the global coordinate system is the offset of the UWB tag on the carrier in the IMU coordinate system;

[0156] The IMU information processing includes:

[0157] The IMU state propagation equation participates in filtering. The state propagation equation is as follows:

[0158]

[0159] in, is the measurement update propagation from time k to time k+1, a m and ω m is the raw IMU measurement, f is the general nonlinear motion model of the IMU;

[0160] The covariance is calculated as follows:

[0161]

[0162] Where Φ and Q are the state transfer matrix and noise matrix respectively;

[0163] The visual information processing module comprises:

[0164] Maintain an additional IMU pose clone The length is S; the position of the feature point in the camera frame is C p f,j =[x j ,y j ,z j ],j=k+1,…,kS, the visual measurement is represented as the clone pose z jand feature location C p f,j Function:

[0165]

[0166] in G p f is the global position of the feature point, n z is Gaussian white noise, and C p I is the camera extrinsic parameter.

[0167] The UWB information processing module includes:

[0168] The UWB ranging model is:

[0169] z m =h m +n r =|| G p U - G p a ||+γ+n r ,

[0170] Among them, z m is the real distance measurement data. is the global position of the UWB tag on the carrier, G p a is the global position of the UWB anchor point, γ is the constant deviation of the ranging data, n r Gaussian white noise for ranging;

[0171] For each distance measurement between the UWB anchor point a and the UWB tag U, the Jacobian matrix H z Obtained by the following formula:

[0172]

[0173] in, is the observed value of UWB ranging, is the posture state, is the position status, For UWB external reference, is the global posture of the carrier, for in is the global position of the anchor point.

[0174] In the short time after the multi-source fusion positioning system starts working, it is assumed that the positioning result obtained by the odometer is an accurate state estimation. Assuming that the required trajectory length takes time T, the odometer positioning result at each time t, t=1,…,T is used as the system positioning result, combined with the UWB ranging model, we can get:

[0175]

[0176] Among them, d t is the real distance measurement data between the UWB tag and the UWB anchor point, G p I,t and is the current positioning result of VIO. G p a is a known UWB anchor point a j The global position of is a good initial value of the UWB extrinsic parameter to be estimated.

[0177] All UWB ranging measurements within time T are superimposed to obtain the ranging residual model between the UWB tag and the UWB anchor point:

[0178]

[0179] The Jacobian of each residual block is:

[0180]

[0181] External parameter calibration is affected by motion, and the observability of the data needs to be checked and compensated to make the initialization more robust. First, by performing singular value decomposition on the Jacobian matrix of the optimization problem, the unobservable direction of the estimator can be observed. In practical applications, the eigenvalues ​​of the Hessian matrix can be calculated:

[0182]

[0183] Perform a threshold check on the eigenvalues ​​of the new data to determine its observability, and then add a priori constraints in the unobservable direction:

[0184]

[0185] in, I p U,0 (i) is the original initial value of UWB external parameters, is the direction vector of the unobservable direction.

[0186] The final optimization problem is:

[0187]

[0188] Among them, S r is the set of UWB tags and UWB anchor points at different time points, S e is the set of all unobservable directions. In this embodiment, the Levenberg-Marquardt (LM) algorithm is used to solve this optimization problem.

[0189] Assume that the optimization problem is The optimization steps of the LM algorithm are:

[0190]

[0191] Among them, J k is the Jacobian matrix of the optimization problem, μ k is the damping coefficient of the algorithm to adjust the step size, which is between the fastest descent direction and the Gauss-Newton direction. is the covariance matrix of the estimated parameters.

[0192] The algorithm stops when the change of the objective function falls below a certain tolerance or the maximum number of iterations is reached.

[0193] In this implementation, the UWB external parameters are updated online based on the filtering method, including:

[0194] For each distance measurement between the UWB anchor point a and the UWB tag U, the Jacobian matrix H z Obtained by the following formula:

[0195]

[0196] in, is the UWB external parameter value, Calculated by the following formula

[0197] A simulation experiment was conducted on the TUM public data set to simulate the noise and bias of the UWB extrinsic parameters and compare the visual inertial odometer OpenVINS, UVIO based on the former and UWB, and Proposed based on UVIO including the UWB extrinsic parameter automatic calibration method proposed in the present invention. The positioning accuracy is shown in Table 1 and is evaluated using the root mean square error of the complete trajectory error in meters.

[0198] Table 1 Positioning accuracy of each method under the influence of different UWB external parameter errors

[0199]

[0200]

[0201] Among them, openvins RMSE represents the positioning root mean square error of openvins, a filter-based visual-inertial odometry method; uvio RMSE represents the positioning root mean square error of the filter-based UWB visual-inertial fusion positioning method (without UWB extrinsic calibration); proposed RMSE represents the positioning root mean square error of the UWB visual-inertial fusion positioning method proposed in this paper that includes the UWB extrinsic calibration method.

[0202] The ablation experiment results of initialization and online calibration are as follows: Figure 2 As shown in the figure, the calibration of each axis of the UWB extrinsic parameter is shown. It can be seen that the UWB extrinsic parameter calibration that only includes initialization has an unremovable deviation, and the UWB extrinsic parameter calibration that only includes online calibration converges slowly, while the initialization + online calibration method proposed in this paper converges quickly and has a small deviation.

[0203] Those skilled in the art will appreciate that the embodiments of the present disclosure may be provided as methods, systems or computer program products. Therefore, the present disclosure may take the form of a complete hardware embodiment, a complete software embodiment or an embodiment combining software and hardware. Moreover, the present disclosure may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0204] The present disclosure is described with reference to flowcharts and / or block diagrams of methods, devices (systems) and computer program products according to embodiments of the present disclosure. It should be understood that each process and / or block in the flowchart and / or block diagram and the combination of processes and / or blocks in the flowchart and / or block diagram can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 These computer program instructions can also be stored in a computer-readable memory that can guide a computer or other programmable data processing device to work in a specific way, so that the instructions stored in the computer-readable memory produce a product including an instruction device, which implements the functions specified in the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0205] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0206] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present disclosure rather than to limit its protection scope. Although the present disclosure has been described in detail with reference to the above embodiments, ordinary technicians in the relevant field should understand that after reading the present disclosure, those skilled in the art can still make various changes, modifications or equivalent substitutions to the specific implementation methods of the invention, but these changes, modifications or equivalent substitutions are all within the protection scope of the disclosed claims to be approved.

Claims

1. A UWB extrinsic parameter automatic calibration method for a multi-source fusion positioning system, characterized in that: The method comprises: Step S1: performing data collection according to a multi-source fusion positioning system, wherein the collected data includes: image data, IMU measurement data, and UWB ranging data; Step S2: Observability compensation for the UWB external parameter initialization process; Step S3: Update the UWB extrinsic parameters online according to the filtering method, obtain the final UWB extrinsic parameter value, and complete the calibration.

2. The UWB extrinsic parameter automatic calibration method for a multi-source fusion positioning system according to claim 1, characterized in that: The multi-source fusion positioning system in step S1 includes: State quantity setting module, IMU information processing module, visual information processing module and UWB information processing module; The state quantity setting module includes: in, is the carrier attitude state represented by the quaternion, and is the carrier velocity and position state quantity, b g,k and b a,k is the gyroscope and accelerometer bias, are the positions of all visual feature points in the global coordinate system, is the position of all UWB anchor points in the global coordinate system It is the offset of the UWB tag on the carrier in the IMU coordinate system; The IMU information processing includes: The IMU state propagation equation participates in filtering. The state propagation equation is as follows: in, is the measurement update propagation from time k to time k+1, a m and ω m is the raw IMU measurement, f is the general nonlinear motion model of the IMU; The visual information processing module comprises: in, G p f is the global position of the feature point, n z is Gaussian white noise, and C p I is the camera extrinsic parameter, C p f,j is the position of the feature point in the camera frame; z j For cloning posture and C p f,j is the characteristic position; The UWB information processing module includes: The UWB ranging model is: z m =h m +n r =|| G p U - G p a ||+γ+n r Among them, z m is the real distance measurement data. is the global position of the UWB tag on the carrier, G p a is the global position of the UWB anchor point, γ is the constant deviation of the ranging data, n r Gaussian white noise for ranging.

3. The UWB extrinsic parameter automatic calibration method for a multi-source fusion positioning system according to claim 1, characterized in that: The step S2 comprises: The positioning result is obtained through the odometer, and the initial value of the UWB external parameters is set; According to the global position of the UWB anchor point, the real distance measurement data between the UWB tag and the anchor point is obtained; Construct the ranging residual model between UWB tag and UWB anchor point; Calculate the Jacobian matrix of each residual block in the range finding residual model; Compensate for the observability of the Jacobian matrix.

4. The UWB external parameter automatic calibration method for a multi-source fusion positioning system according to claim 3 is characterized in that: The step of constructing a ranging residual model between a UWB tag and a UWB anchor point includes: Among them, r t is the ranging residual model between the UWB tag and the UWB anchor point, d t is the actual ranging data between the UWB tag and the UWB anchor point, γ is the constant deviation of the ranging data, G p I,t and is the current positioning result of VIO. G p a is a known UWB anchor point a j The global position of is a good initial value of the UWB extrinsic parameter to be estimated.

5. The UWB external parameter automatic calibration method for a multi-source fusion positioning system according to claim 3 is characterized in that: The compensation for the observability of the Jacobian matrix includes: Perform singular value decomposition on the Jacobian matrix to identify unobservable directions; Calculate the eigenvalues ​​of the Hessian matrix, perform a threshold check on the eigenvalues ​​of the new data, and determine whether the new data is observable; Add a priori constraints in unobservable directions; Formulate optimization problems and solve them using the Levenberg-Marquardt algorithm; After the algorithm stops, the final optimization results are evaluated, including the estimated values ​​of the extrinsic parameters and the size of the residuals.

6. The UWB extrinsic parameter automatic calibration method for a multi-source fusion positioning system according to claim 5, characterized in that: The optimization problem is constructed and solved using the Levenberg-Marquardt algorithm, including: The optimization problem is: Among them, S r is the set of UWB tags and UWB anchor points at different time points, S e is the set of all unobservable directions, e t,i Prior constraints added for unobservable directions, T represents transposition; Suppose the optimization problem is The optimization steps of the LM algorithm are: Among them, J k is the Jacobian matrix of the optimization problem, μ k is the damping factor of the algorithm to adjust the step size, is the covariance matrix of the estimated parameters and I is the identity matrix.

7. The UWB extrinsic parameter automatic calibration method for a multi-source fusion positioning system according to claim 1, characterized in that: The step S3 comprises: in, is the observed value of UWB ranging, is the posture state, is the position status, For UWB external reference, is the global posture of the carrier, is the global position of the anchor point.

8. A UWB external parameter automatic calibration system for a multi-source fusion positioning system, characterized in that: The system comprises: A data acquisition unit, used for acquiring data according to a multi-source fusion positioning system, wherein the acquired data includes: image data, IMU measurement data, and UWB ranging data; A compensation unit, used for observability compensation of the UWB external parameter initialization process; The calibration unit is used to update the UWB extrinsic parameters online according to the filtering method, obtain the final UWB extrinsic parameter value, and complete the calibration.

9. A computer device, characterized in that: The invention comprises a memory and a processor, wherein a computer program is stored in the memory, and when the processor runs the computer program stored in the memory, the processor executes a UWB external parameter automatic calibration method for a multi-source fusion positioning system according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, which, when executed by a processor, executes the steps of a UWB external parameter automatic calibration method for a multi-source fusion positioning system as described in any one of claims 1 to 7.

Images