A monocular VIO / UWB indoor combined positioning method based on ARKF

Through ARKF technology, a difference-resistant model is constructed, which solves the problems of indoor VIO drift and UWBNLOS error, and achieves high-precision and robust indoor positioning.

CN115183767BActive Publication Date: 2024-11-15LIAONING TECHNICAL UNIVERSITY
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210790610.0
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-07-06
Publication Date
2024-11-15
Estimated Expiration
2042-07-06

AI Technical Summary

Technical Problem

In indoor environments, it is difficult to obtain high-age and high-precision positioning results by relying solely on VIO or UWB positioning, and VIO is prone to drift and positioning error accumulation, so the accuracy and stability of UWB in NLOS environment are greatly reduced.

Method used

Using a monocular VIO/UWB indoor combined positioning method based on adaptive anti-difference Kalman filtering (ARKF), the state and Measuring the equation, using ARKF to solve the positioning results, reducing the impact of NLOS error.

Benefits of technology

Without relying on loopback detection, the drift and positioning error problems of monocular VIO are effectively solved, the robustness and positioning accuracy of the combined system are improved, and the average positioning accuracy reaches the sub-decimeter level, achieving high-precision indoor positioning.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115183767B_ABST
    Figure CN115183767B_ABST
Patent Text Reader

Abstract

The present invention discloses a monocular VIO / UWB indoor combined positioning method based on ARKF, which belongs to the field of navigation and positioning technology. The method comprises the following steps: the first step is to use ROS time tags to realize the time synchronization of different sensors in the combined system, and complete the combined system coordinate system one through a four-parameter conversion model; the second step is to obtain the measurement information of IMU, camera and UWB; the third step is to perform monocular VIO position solution; the fourth step is to construct a state model and measurement model of the monocular VIO / UWB combined system; the fifth step is to construct a monocular VIO / UWB combined system anti-error model; the sixth step is to use ARKF to solve the positioning result of the combined system. The present invention can effectively solve the problem of easy drift and large positioning error of monocular VIO without relying on loop detection, and reduce the influence of UWB's non-line-of-sight error on the combined system, so as to achieve indoor high-precision positioning.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of navigation and positioning, and specifically is a monocular VIO / UWB indoor combined positioning method based on ARKF. Background Art

[0002] With the development of the times, people have higher and higher requirements for positioning accuracy. People spend 80%-90% of their time indoors, so more positioning navigation and location service needs come from indoors. The indoor environment is complex, and it is difficult to meet the current positioning needs with a single sensor. Therefore, the combination of multiple sensors has become a research hotspot in the field of high-precision positioning. Due to the complementary advantages between cameras and inertial measurement unit (IMU) sensors, visual inertial odometry (VIO) based on different fusion methods has become the mainstream method to solve this problem. The fusion method of VIO is mainly divided into two methods: filtering-based and optimization-based. The former has a simple fusion framework, but the linearization error is large; the latter has more accurate positioning accuracy, but requires higher hardware computing power. Although VIO can obtain high positioning accuracy in a short time, due to the easy divergence of IMU positioning itself and the camera being easily affected by factors such as light and texture, tracking fails. The error of VIO itself will accumulate over time, which will lead to the cumulative error of the positioning result gradually increasing. Therefore, it is necessary to further introduce other auxiliary sensors to reduce the positioning error of VIO.

[0003] At present, commonly used indoor wireless positioning technologies include UWB, Wi-Fi, ultrasound, Bluetooth, etc. Among them, UWB positioning technology has the characteristics of low power spectrum density, narrow pulse width, high time resolution, and can obtain high ranging accuracy, so it is widely used in indoor high-precision positioning. However, in the process of pulse information propagation of UWB positioning technology, the existence of obstacles will block the pulse signal of UWB, causing the signal to be reflected, refracted and penetrated during the propagation process, forming a non-line of sight (NLOS) environment, resulting in NLOS errors, which greatly reduce the accuracy and stability of the UWB positioning system, and even make it impossible to measure distance and position in severe cases.

[0004] From the above analysis, it can be seen that it is difficult to obtain high-efficiency and high-precision positioning results by relying solely on VIO or UWB for positioning in indoor environments. Therefore, in recent years, some experts and scholars have conducted research on the data fusion of VIO and UWB. In 2021, Nguyen et al. proposed a tightly coupled fusion scheme for VIO and UWB, which fuses the measurement data of VIO and UWB to obtain the robot's posture. (Reference: Nguyen TH, Nguyen TM, Xie L. Range-focused fusion of camera-IMU-UWB for accurate and drift-reduced localization[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 1678-1685.) In 2015, Nyqvist et al. proposed a method of using UWB to assist monocular VIO, and used extended Kalman filter (EKF) for fusion solution to obtain an improved drift-free global posture estimation. (Reference: Nyqvist HE, Skoglund MA, Hendeby G, et al. Pose estimation using monocular vision and inertial sensors aided with ultra wide band [C] / / 2015 international conference on indoor positioning and indoor navigation (IPIN). IEEE, 2015: 1-10.) In 2022, Shen Bingqi et al. proposed a loose coupling scheme of binocular VIO and UWB, using the position information output by binocular VIO and the position information output by UWB respectively, and obtained the optimal position estimation of the robot through EKF data fusion. This scheme adopts a loose coupling fusion method, and the robustness of the combined system is low. (Reference: Shen Bingqi, Zhang Zhiming, Shu Shaolong. Mobile robot UWB and VIO combined indoor positioning algorithm [J]. Computer Applications, 2022 (1): 1-8.) Although the above three fusion schemes can obtain good positioning results in a good environment, they have not fully considered the impact of UWB NLOS error on the combined system. To this end, the present invention proposes a monocular VIO / UWB indoor combined positioning method based on adaptive rosbust Kalman filter (ARKF). Summary of the invention

[0005] In view of the above technical problems, the present invention proposes a new monocular VIO / UWB indoor combined positioning method to effectively solve the problem of easy drift and large positioning error of monocular VIO in indoor environment, suppress the influence of UWB NLOS error on the combined system, and improve the robustness and positioning accuracy of the combined system.

[0006] To achieve the above object, the present invention provides a monocular VIO / UWB indoor combined positioning method based on ARKF, comprising the following steps:

[0007] Step 1: Use ROS time tags to achieve time synchronization of different sensors in the combined system, and complete the combined system coordinate system 1 through a four-parameter transformation model;

[0008] Step 2: Obtain measurement information of IMU, camera and UWB;

[0009] Step 3: Perform monocular VIO position calculation;

[0010] Step 4: Construct the state model and measurement model of the monocular VIO / UWB combined system;

[0011] Step 5: Construct a monocular VIO / UWB combined system robustness model;

[0012] Step 6: Use ARKF to solve the positioning result of the combined system.

[0013] In step 2, UWB uses a distance measurement model based on round-trip time (TW-TOF), and the distance measurement diagram is as follows: Figure 2 As shown, the specific formula is as follows:

[0014]

[0015] Where d is the distance between the UWB mobile station and the reference station, c is the speed of light, T round T is the round-trip time difference of the UWB pulse signal between the UWB mobile station and the base station, respond is the time difference from when the UWB base station receives the pulse signal to when it transmits the response pulse signal, n is the measurement noise, T1 is the time when the UWB mobile station sends the pulse signal, T2 is the time when the UWB base station receives the pulse signal, T3 is the time when the UWB base station sends the response pulse signal, and T4 is the time when the UWB mobile station receives the response pulse signal.

[0016] In step 3, based on the camera and IMU data obtained in step 2, the VINS framework is used to implement monocular VIO positioning. The positioning process is as follows: Figure 3 As shown;

[0017] The specific steps are as follows:

[0018] Step 3-1, extract the Harris corner points of the image and use the pyramid optical flow method to track adjacent frames;

[0019] Step 3-2, use the IMU data to calculate the pose of the current frame as the initial value, and the Jacobian matrix and covariance of the pre-integration increment and pre-integration error of the adjacent frames;

[0020] Step 3-3: Perform camera initialization and camera inertial navigation joint initialization, calculate the pose of all frames in the sliding window and the depth of the landmark points, estimate the gravity, initial scale value, and zero bias of the gyroscope, and solve the external parameter matrix of the world coordinate system and the initial camera coordinate system;

[0021] Step 3-4: Use bundle adjustment to perform back-end nonlinear optimization and output the solved pose;

[0022] The specific formula is as follows:

[0023]

[0024] Among them, Ω is the total state quantity in the sliding window, n is the number of frames, m is the total number of feature points in the sliding window, and λ is the inverse depth of the feature points; X k is the IMU state corresponding to the k-th frame image, including position speed Rotation Acceleration bias b a , gyroscope bias b g ; is the camera external parameter, including the translation vector Rotation Vector

[0025] In the step S4, the state equation and measurement equation of the monocular VIO / UWB combination system are constructed;

[0026] The specific steps are as follows:

[0027] Step 4-1, establish an update model of the monocular VIO / UWB combined system;

[0028] The specific formula is as follows:

[0029]

[0030] Among them, t is the time interval of combined system sampling, δ is the representation of the corresponding mathematical correction number, (x k ,y k ) and (x k-1 ,y k-1 ) are the coordinates of the mobile platform at time k and time k-1, v x and vy are the speeds of the mobile platform in the x and y directions, a x and a y are the accelerations of the mobile platform in the x and y directions respectively;

[0031] Step 4-2: Linearize equation (3) to obtain the state equation of the monocular VIO / UWB combined system:

[0032] The specific formula is as follows:

[0033] X k =FX k-1 +W k-1 (4)

[0034] in, is the state transfer matrix, X k =[δx δy δv x δv y ] T is the state vector, is the system noise;

[0035] Step 4-3: Establish an observation model of the monocular VIO / UWB combined system;

[0036] The specific formula is as follows:

[0037]

[0038]

[0039] in, is the coordinate of UWB at time k, is the coordinate of the i-th reference station at time k, is the actual distance between the UWB carrier position obtained at time k and the i-th reference station, is the measured value, is the measured value of the displacement change of the monocular VIO at adjacent moments, and is the coordinates of the monocular VIO at time k and time k-1, is the observation noise sequence;

[0040] Step 4-4: Linearize equations (5) and (6) to obtain the measurement equation of the monocular VIO / UWB combined system;

[0041] The specific formula is as follows:

[0042] Y k =HX k +V k (7)

[0043] Among them, the observation vector Linearization noise Measurement coefficient matrix

[0044] In the step 5, a monocular VIO / UWB combined system robustness model is constructed;

[0045] The specific steps are as follows:

[0046] Step 5-1, constructing test information using the new information vector;

[0047] The specific formula is as follows:

[0048]

[0049] Among them, ε k is the new information vector, D k is the covariance matrix corresponding to the new information vector;

[0050] Step 5-2: Using the verification information Δε k,i Construct the robustness factor with Huber function;

[0051] The specific formula is as follows:

[0052]

[0053] Among them, L is the robustness threshold, which is given by Calculated;

[0054] Step 5-3, using the robustness factor to update the measurement information covariance matrix R;

[0055] The specific formula is as follows:

[0056]

[0057] Among them, α=diag[α1α1...α M-1 ];

[0058] Step 5-4, using the updated measurement information covariance matrix to calculate the gain matrix K;

[0059] The specific formula is as follows:

[0060]

[0061] Among them, P k,k-1 is the predicted state covariance matrix;

[0062] Step 5-5, introduce the Sage-Husa algorithm to update and correct the system noise covariance matrix Q in real time;

[0063] The specific formula is as follows:

[0064]

[0065] Among them, P k and P k-1 are the estimated state covariance matrices at time k and time k-1 respectively, b is the forgetting factor, 0<b<1; d k =(1-b) / (1-b k+1 ).

[0066] In the step 6, the positioning result of the combined positioning system is calculated by using ARKF;

[0067] The specific formula is as follows:

[0068]

[0069] in, and To update the corrected measurement information covariance matrix and system noise covariance matrix, is the predicted state vector, is the estimated state vector at time k, and I is the unit matrix.

[0070] The characteristics and beneficial effects of the present invention are:

[0071] The present invention proposes a monocular VIO / UWB indoor combined positioning method based on ARKF, which can effectively solve the problem of easy drift and large positioning error of monocular VIO in indoor environment by fusing the measurement information of other sensors without relying on loop detection. At the same time, the anti-error model of the combined system is constructed by using the new information vector, which effectively reduces the influence of UWBNLOS error on the combined system, improves the positioning accuracy and robustness of the combined system, and the average positioning accuracy can reach sub-decimeter level, which can achieve high-precision indoor positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 It is a flow chart of a monocular VIO / UWB indoor combined positioning method based on ARKF according to an embodiment of the present invention;

[0073] Figure 2 It is a schematic diagram of the UWB ranging principle based on TW-TOF according to an embodiment of the present invention;

[0074] Figure 3 It is a monocular VIO positioning flow chart of an embodiment of the present invention;

[0075] Figure 4 It is a monocular VIO positioning trajectory and a reference trajectory diagram of an embodiment of the present invention;

[0076] Figure 5 It is a monocular VIO / UWB combined positioning trajectory and reference trajectory diagram based on EKF solution in one embodiment of the present invention;

[0077] Figure 6 It is a monocular VIO / UWB combined positioning trajectory and reference trajectory diagram based on ARKF solution according to an embodiment of the present invention;

[0078] Figure 7 It is the error statistics of the monocular VIO positioning result, the monocular VIO / UWB combined positioning result based on EKF, and the monocular VIO / UWB combined positioning result based on ARKF according to an embodiment of the present invention. DETAILED DESCRIPTION

[0079] Combine the following Figure 1 to Figure 6 The ARKF-based monocular VIO / UWB indoor combined positioning method of the present invention is described in detail.

[0080] In the embodiment of the present invention, a monocular VIO / UWB indoor combined positioning method based on ARKF is as follows: Figure 1 As shown, the following steps are included:

[0081] Step 1: Use ROS time tags to achieve time synchronization of different sensors in the combined system, and complete the combined system coordinate system 1 through a four-parameter transformation model;

[0082] In the example of the present invention, the monocular camera, IMU, UWB and prism are first fixed on the combined positioning experimental platform, the spatial coordinate system is unified through the four-parameter transformation model, and then the data collected by the monocular camera, IMU and UWB are synchronized using the ROS time tag.

[0083] Step 2: Obtain measurement information of IMU, camera and UWB;

[0084] In the embodiment of the present invention, the camera used is the Aca2500-20gc industrial monocular camera of Basler, equipped with a FL-BC1218A-VG fixed-focus lens, with a resolution of 648×512 and an image acquisition frequency of 5Hz. The IMU used is the Ellipse2-N of SBG-SYSTEMS, with a gyro accuracy of 10deg / h. The UWB is the PulsON440 ranging module produced by Time Domain, with a ranging accuracy of 5±1cm. The reference value is the coordinate value measured by the Leica TS50 total station, with a ranging accuracy of 1.5mm+2×10 -6 ×D, D is the distance measured by the total station;

[0085] In the present invention, UWB uses a distance measurement model based on round-trip time (TW-TOF), and the distance measurement schematic diagram is shown in FIG. Figure 2 As shown, the specific formula is as follows:

[0086]

[0087] Where d is the distance between the UWB mobile station and the reference station, c is the speed of light, T round T is the round-trip time difference of the UWB pulse signal between the UWB mobile station and the base station, respond is the time difference from when the UWB base station receives the pulse signal to when it transmits the response pulse signal, n is the measurement noise, T1 is the time when the UWB mobile station sends the pulse signal, T2 is the time when the UWB base station receives the pulse signal, T3 is the time when the UWB base station sends the response pulse signal, and T4 is the time when the UWB mobile station receives the response pulse signal.

[0088] Step 3: Based on the camera and IMU data obtained in step 2, use the VINS framework to achieve monocular VIO positioning. The positioning process is as follows: Figure 3 As shown;

[0089] The specific steps are as follows:

[0090] Step 3-1, extract the Harris corner points of the image and use the pyramid optical flow method to track adjacent frames;

[0091] Step 3-2, use the IMU data to calculate the pose of the current frame as the initial value, and the Jacobian matrix and covariance of the pre-integration increment and pre-integration error of the adjacent frames;

[0092] Step 3-3: Perform camera initialization and camera inertial navigation joint initialization, calculate the pose of all frames in the sliding window and the depth of the landmark points, estimate the gravity, initial scale value, and zero bias of the gyroscope, and solve the external parameter matrix of the world coordinate system and the initial camera coordinate system;

[0093] Step 3-4: Use bundle adjustment to perform back-end nonlinear optimization and output the solved pose;

[0094] The specific formula is as follows:

[0095]

[0096] Among them, Ω is the total state quantity in the sliding window, n is the number of frames, m is the total number of feature points in the sliding window, and λ is the inverse depth of the feature points; X k is the IMU state corresponding to the k-th frame image, including position speed Rotation Acceleration bias b a , gyroscope bias b g ; is the camera external parameter, including the translation vector Rotation Vector

[0097] Step 4: Construct the state equation and measurement equation of the combined system;

[0098] The specific steps are as follows:

[0099] Step 4-1, establish an update model of the monocular VIO / UWB combined system;

[0100] The specific formula is as follows:

[0101]

[0102] Among them, t is the time interval of combined system sampling, δ is the representation of the corresponding mathematical correction number, (x k ,y k ) and (x k-1 ,y k-1 ) are the coordinates of the mobile platform at time k and time k-1, v x and v y are the speeds of the mobile platform in the x and y directions, a x and a y are the accelerations of the mobile platform in the x and y directions respectively;

[0103] Step 4-2: Linearize equation (3) to obtain the state equation of the monocular VIO / UWB combined system;

[0104] The specific formula is as follows:

[0105] X k =FX k-1 +W k-1 (4)

[0106] in, is the state transfer matrix, X k =[δx δy δv x δv y ] T is the state vector, is the system noise;

[0107] Step 4-3: Establish an observation model of the monocular VIO / UWB combined system;

[0108] The specific formula is as follows:

[0109]

[0110]

[0111] in, is the coordinate of UWB at time k, is the coordinate of the i-th reference station at time k, is the actual distance between the UWB carrier position obtained at time k and the i-th reference station, is the measured value, is the measured value of the displacement change of the monocular VIO at adjacent moments, and is the coordinates of the monocular VIO at time k and time k-1, is the observation noise sequence;

[0112] Step 4-4: Linearize equations (5) and (6) to obtain the measurement equation of the monocular VIO / UWB combined system;

[0113] The specific formula is as follows:

[0114] Y k =HX k +V k (7)

[0115] Among them, the observation vector Linearized noise Measurement coefficient matrix

[0116] Step 5: Construct a combined system robustness model. The specific steps are as follows:

[0117] Step 5-1, constructing test information using the new information vector;

[0118] The specific formula is as follows:

[0119]

[0120] Among them, ε k is the new information vector, D k is the covariance matrix corresponding to the new information vector;

[0121] Step 5-2: Using the verification information Δε k,i Construct the robustness factor with Huber function;

[0122] The specific formula is as follows:

[0123]

[0124] Among them, L is the robustness threshold, which is given by Calculated;

[0125] Step 5-3, using the robustness factor to update the measurement information covariance matrix R;

[0126] The specific formula is as follows:

[0127]

[0128] Among them, α=diag[α1α1...α M-1 ];

[0129] Step 5-4, using the updated measurement information covariance matrix to calculate the gain matrix K;

[0130] The specific formula is as follows:

[0131]

[0132] Among them, P k,k-1 is the predicted state covariance matrix;

[0133] Step 5-5, introduce the Sage-Husa algorithm to update and correct the system noise covariance matrix Q in real time;

[0134] The specific formula is as follows:

[0135]

[0136] Among them, P k and P k-1 are the estimated state covariance matrices at time k and time k-1 respectively, b is the forgetting factor, 0<b<1; d k =(1-b) / (1-b k+1 ).

[0137] Step 6: Use ARKF to solve the positioning result of the combined positioning system;

[0138] The specific formula is as follows:

[0139]

[0140] in, and To update the corrected measurement information covariance matrix and system noise covariance matrix, is the predicted state vector, is the estimated state vector at time k, and I is the unit matrix.

[0141] In the present invention, the method of the present invention is compared with the monocular VIO positioning result and the monocular VIO / UWB combined positioning result based on EKF. k Value and R kThe values ​​are the same, the robustness threshold L of the monocular VIO / UWB-ARKF combination model is set to 0.01, and the forgetting factor b in the Sage-Husa algorithm is set to 0.995.

[0142] like Figure 4 As shown in the figure, the combined positioning solution results are compared with the positioning results of the total station. In the initial stage of movement, the positioning accuracy of the monocular VIO is high, but as time goes by, the positioning trajectory gradually drifts, among which the drift at point B is the most serious, with a maximum positioning error of 0.384 m.

[0143] like Figure 5 As shown in the figure, the monocular VIO / UWB-EKF combined positioning solution results are compared with the total station positioning results. In the initial stage of the movement of the experimental platform, the positioning accuracy of the combined system is high, the drift at points A, B, C, and D is significantly reduced, and the overall movement trajectory is closer to the reference trajectory. However, due to the obstruction of obstacles during the movement of the experimental platform, the combined system is affected by the NLOS error. As time goes by, the error accumulation is still getting larger and larger, and its maximum positioning error is 0.269m.

[0144] like Figure 6 As shown in the figure, the monocular VIO / UWB-ARKF combined positioning solution results are compared with the total station positioning results. The positioning accuracy of the monocular VIO and UWB combined system solved by ARKF is better. It can not only overcome the problem that the monocular VIO is prone to drift at points A, B, C, and D, but also effectively eliminate the influence of the UWB NLOS error on the combined system, making the positioning trajectory of the combined system closer to the reference trajectory. The maximum positioning error of the combined system is 0.157m.

[0145] The statistical results of the positioning errors of the monocular VIO, monocular VIO / UWB-EKF combined model and monocular VIO / UWB-ARKF combined model are shown in Figure 2. Figure 7 As shown in the figure, the monocular VIO positioning error gradually increases over time. After the introduction of UWB, the positioning accuracy is significantly improved, but due to the existence of UWB's NLOS error, it still has a great impact on the positioning accuracy of the combined system. The introduction of the ARKF algorithm effectively suppresses the influence of UWB's NLOS error and improves the positioning accuracy and robustness of the combined system.

[0146] The above is only the most basic specific implementation of the present invention, but the protection scope of the present invention is not limited thereto. Any replacement that can be understood by a person skilled in the art within the technical scope disclosed by the present invention should be included in the scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A monocular VIO / UWB indoor combined positioning method based on ARKF, characterized in that: The steps include: Step 1: Use ROS time tags to achieve time synchronization of different sensors in the combined system, and complete the combined system coordinate system 1 through a four-parameter transformation model; Step 2: Obtain measurement information of IMU, camera and UWB; Step 3: Perform monocular VIO position calculation; Step 4: Construct the state model and measurement model of the monocular VIO / UWB combined system; Step 5: Construct a monocular VIO / UWB combined system robustness model. The specific steps are as follows: Step 5-1, constructing test information using the new information vector; The specific formula is as follows: Among them, e k is the new information vector, D k is the covariance matrix corresponding to the new information vector; Step 5-2: Use the verification information k,i Construct the robustness factor with Huber function; The specific formula is as follows: Among them, L is the robustness threshold, which is given by Calculated; Step 5-3, using the robustness factor to update the measurement information covariance matrix R; The specific formula is as follows: Where, α=diag[a1 a1...α M-1 ]; Step 5-4, using the updated measurement information covariance matrix to calculate the gain matrix K; The specific formula is as follows: Among them, P k,k-1 is the predicted state covariance matrix; Step 5-5, introduce the Sage-Husa algorithm to update and correct the system noise covariance matrix Q in real time; The specific formula is as follows: Among them, P k and P k-1 are the estimated state covariance matrices at time k and time k-1, b is the forgetting factor, 0 <b<1;d k =(1-b) / (1-b k+1 ); Step 6: Use ARKF to solve the positioning result of the combined system. The specific formula is as follows: in, and To update the corrected measurement information covariance matrix and system noise covariance matrix, is the predicted state vector, is the estimated state vector at time k, and I is the unit matrix.

2. The ARKF-based monocular VIO / UWB indoor combined positioning method according to claim 1, characterized in that: In step 2, UWB uses a ranging model based on round-trip time (TW-TOF), and the specific formula is as follows: Where d is the distance between the UWB mobile station and the reference station, c is the speed of light, T round T is the round-trip time difference of the UWB pulse signal between the UWB mobile station and the base station, respond is the time difference from when the UWB base station receives the pulse signal to when it transmits the response pulse signal, n is the measurement noise, T1 is the time when the UWB mobile station sends the pulse signal, T2 is the time when the UWB base station receives the pulse signal, T3 is the time when the UWB base station sends the response pulse signal, and T4 is the time when the UWB mobile station receives the response pulse signal.

3. The monocular VIO / UWB indoor combined positioning method based on ARKF according to claim 1, characterized in that: In step 3, based on the camera and IMU data obtained in step 2, the monocular VIO positioning is realized using the VINS framework. The positioning steps are as follows: Step 3-1, extract the Harris corner points of the image and use the pyramid optical flow method to track adjacent frames; Step 3-2, use the IMU data to calculate the pose of the current frame as the initial value, and the Jacobian matrix and covariance of the pre-integration increment and pre-integration error of the adjacent frames; Step 3-3: Perform camera initialization and camera inertial navigation joint initialization, calculate the pose of all frames in the sliding window and the depth of the landmark points, estimate the gravity, initial scale value, and zero bias of the gyroscope, and solve the external parameter matrix of the world coordinate system and the initial camera coordinate system; Step 3-4: Use bundle adjustment method to perform back-end nonlinear optimization and output the solved pose; The specific formula is as follows: Where W is the total state quantity in the sliding window, n is the number of frames, m is the total number of feature points in the sliding window, and λ is the inverse depth of the feature points; X k is the IMU state corresponding to the k-th frame image, including position speed Rotation Acceleration bias b a , gyroscope bias b g ; is the camera external parameter, including the translation vector Rotation Vector 4. The ARKF-based monocular VIO / UWB indoor combined positioning method according to claim 1, characterized in that: In step 4, the state equation and measurement equation of the monocular VIO / UWB combined system are constructed; The specific steps are as follows: Step 4-1, establish an update model of the monocular VIO / UWB combined system; The specific formula is as follows: Among them, t is the time interval of combined system sampling, δ is the representation of the corresponding mathematical correction number, (x k ,y k ) and (x k-1 ,y k-1 ) are the coordinates of the mobile platform at time k and time k-1, v x and v y are the speeds of the mobile platform in the x and y directions, a x and a y are the accelerations of the mobile platform in the x and y directions respectively; Step 4-2: Linearize equation (3) to obtain the state equation of the monocular VIO / UWB combined system: The specific formula is as follows: X k =FX k-1 +W k-1 (10) in, is the state transfer matrix, X k =[[dx dy dv x dv y ]] T is the state vector, is the system noise; Step 4-3: Establish an observation model of the monocular VIO / UWB combined system; The specific formula is as follows: in, is the coordinate of UWB at time k, is the coordinate of the i-th reference station at time k, is the actual distance between the UWB carrier position obtained at time k and the i-th reference station, is the measured value, is the measured value of the displacement change of the monocular VIO at adjacent moments, and is the coordinates of the monocular VIO at time k and time k-1, is the observation noise sequence; Step 4-4: Linearize equations (5) and (6) to obtain the measurement equation of the monocular VIO / UWB combination system: The specific formula is as follows: Y k =HX k +V k (13) Among them, the observation vector Linearization noise Measurement coefficient matrix