Positioning Method and System for UWB and Vision Tightly Coupled SLAM Algorithm

Abstract

The present invention proposes a positioning method and system for a UWB and vision tightly coupled SLAM algorithm. A visible light camera and a UWB sensor are rigidly connected to obtain a positioning device, and UWB anchors are arranged at any position in space. First, an information frame composed of the distance information of the UWB anchors and the image information is constructed through the distance information and image information of the positioning device. Visual initialization is performed on the image data, and a UWB initialization queue is established to track the subsequently acquired information frames and process the information frames in the UWB initialization queue. For the subsequently acquired information frames, a joint optimization equation is established based on the reprojection error and the UWB distance error. By solving the joint optimization equation, the pose of the new information frame is obtained, and it is determined whether the new information frame is a key frame. After the new key frame is added to the key frame queue, a local map joint optimization equation is established to optimize the pose of the key frame, the map points, the anchor positions, and the scale factor. The present invention can effectively improve the positioning accuracy relying only on one UWB anchor.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous positioning of mobile robots and unmanned aerial vehicles, and particularly relates to a positioning method and system for a tightly coupled algorithm of UWB and visual SLAM. Background Art

[0002] In recent years, the autonomous positioning method of mobile devices has become the focus and difficulty of the research on mobile robot technology. Due to its characteristics of not relying on external devices and strong autonomy, SLAM technology has become the main solution for the autonomous positioning of mobile robots.

[0003] Visible light cameras have become the main sensors in SLAM solutions due to their low-cost sensors, small size, and light weight. Monocular visual SLAM obtains external environment information through a camera, and can estimate the pose of a mobile device in real time and build a sparse point cloud map of the external environment by processing the external environment information. Due to the singularity of the solution when solving the homogeneous linear equations during the initialization of a monocular visual SLAM system, there is scale uncertainty in the SLAM system.

[0004] Ultra Wide Band (UWB) positioning technology is a wireless communication technology with centimeter-level positioning accuracy. Its signals have characteristics such as extremely strong penetration power and extremely high time-domain resolution. Combined with some positioning algorithms, it has become one of the most widely used indoor positioning technologies. Existing UWB positioning technologies require at least 3 fixed anchors with determined placement positions to achieve 3D space positioning, which limits the use of this solution in unknown environments.

[0005] The tightly coupled SLAM of UWB and vision can eliminate the drift of the visual SLAM positioning algorithm by using UWB to measure distances, and improve the positioning accuracy. The positioning and mapping method and system for the fusion algorithm of UWB and visual SLAM proposed by Zhang Xinyu is based on at least 3 UWB anchors with known positions to achieve the positioning of UWB devices, and only uses UWB positioning information for relocalization without coupling UWB positioning information with visual information. The method for indoor positioning and navigation based on visual SLAM integrating UWB proposed by Yan Chenggang also uses 3 UWB anchors for UWB device positioning and uses UWB information to assist relocalization. The current solutions only use multiple UWB anchors with known positions to obtain device position information based on UWB anchors, directly use the position information, do not implement the tight coupling calculation of UWB measurement information and visual information, do not make full use of UWB measurement information, and the arrangement of multiple non-coplanar UWB anchors limits the implementation of the positioning solution. Summary of the Invention

[0006] To solve the problems existing in the prior art, the present invention proposes a positioning method and system for a UWB and vision tightly coupled SLAM algorithm. By rigidly connecting a visible light camera and a UWB sensor to obtain a positioning device, and arranging at least one UWB anchor point at any position in space, and tightly coupling the UWB measured distance data and visual information, the positioning accuracy can be effectively improved.

[0007] The technical solution of the present invention is as follows:

[0008] A positioning method for a UWB and vision tightly coupled SLAM algorithm, comprising the following steps:

[0009] Step 1: Place one or more UWB anchor points at any position in the environment, rigidly connect the UWB sensor and the camera to form a positioning device, and fixedly install the positioning device on the mobile device;

[0010] Step 2: Real-time collect the distance information between the positioning device and the UWB anchor point through the UWB sensor on the positioning device, and collect the environmental image information through the camera on the positioning device, and at the same time realize the time alignment of the environmental image information and the distance information. Based on the image frame, construct an information frame composed of the UWB anchor point distance and the image information;

[0011] Step 3: Perform visual initialization on the image data in the collected information frame, and estimate the relative pose of two consecutive information frames and the positions of the map points jointly observed by them;

[0012] Step 4: After completing the visual initialization, set the pose of the first visually initialized frame as the coordinate system of the system; establish a UWB initialization queue, and track the subsequent newly collected information frames, including establishing the matching relationship between the image feature points and the map points in the subsequent information frames, establishing a visual tracking optimization equation based on the reprojection error, calculating the pose of the subsequent frames, and determining whether to add the newly collected information frame to the UWB initialization queue;

[0013] Step 5: Process the information frames in the UWB initialization queue:

[0014] Step 5.1: When the number of information frames in the UWB initialization frame sequence is greater than the set UWB anchor point initialization threshold, establish an optimization equation based on the UWB distance error for the information frames in the UWB initialization frame queue. The UWB distance error refers to the difference between the calculated distance and the UWB measured distance in the information frame, where the calculated distance refers to the distance between the positioning device and the UWB anchor point calculated according to the pose estimated by the information frame; then solve the optimization equation to estimate the UWB anchor point position and the scaling factor between the visual SLAM scale and the metric unit;

[0015] Step 5.2: Using the scaling factor obtained in Step 5.1, multiply the pose of the information frame stored in the system and the position of the map point by the scaling factor, so as to restore the key frames and map points in the local map to the metric scale;

[0016] Step 6: For the newly acquired information frames in the subsequent process, establish the matching relationship between the image feature points and the map points in the newly acquired information frames, and establish the reprojection error of the feature points according to the matching relationship. At the same time, use the measured distance and the calculated distance in the newly acquired information frames to obtain the UWB distance error; establish a joint optimization equation based on the reprojection error and the UWB distance error, and obtain the pose of the new information frame by solving the joint optimization equation; and determine whether the new information frame is a key frame:

[0017] Calculate the relative pose difference, UWB measurement distance difference and acquisition time difference between the new information frame and its reference key frame. If any difference is greater than the set corresponding threshold, set the new information frame as a key frame, and add the new information frame and the map points observed by it to the local map and the global map;

[0018] Step 7: After a new key frame is added to the key frame queue of the local map, establish a local map joint optimization equation, and optimize the poses of the key frames in the key frame queue, the map points in the local map, the positions of the UWB anchors, and the scaling factor between the SLAM system and the metric unit by solving the joint optimization equation;

[0019] Step 8: Execute the loop detection thread for the key frame newly added to the global map. When it is detected that the key frame newly added to the global map matches the key frames in the global map, establish an optimization equation for the tight coupling of visual and UWB information between the matching key frames, perform relocalization, and eliminate the pose estimation drift generated during the operation of the mobile device.

[0020] Further, in Step 2, according to the UWB measurement frames before and after the image frame, use the method of linear interpolation to obtain the distance measurement value at the corresponding moment of the image frame, so as to align the ranging information and the image information in time.

[0021] Further, the process of aligning the ranging information and the image information in time is as follows:

[0022] Take the time of the second image frame received by the camera as the system initial time T0, and take the time stamp mean value of the two UWB measurement distance frames before and after the first image frame as the moment of the UWB measurement distance initial frame:

[0023]

[0024] The system timestamp of the subsequent Kth image frame And the system timestamp of the Kth UWB measurement frame is:

[0025]

[0026]

[0027] where T CK is the acquisition timestamp of the K-th frame image, and T C0 is the acquisition timestamp of the first frame image, and T UK is the acquisition timestamp of the K-th UWB measurement frame;

[0028] Save the UWB ranging information collected within the acquisition time interval between the (K - 1)-th frame image and the (K + 1)-th frame image, and retrieve two consecutive UWB ranging information frames that meet the conditions:

[0029]

[0030] where are respectively the timestamps of the previous and the next UWB measurement frames when acquiring the K-th frame image. Interpolate the measured distances of the n-th and the (n + 1)-th UWB measurement frames. The formula is as follows:

[0031]

[0032] where d n , and d n+1 are respectively the distances measured by the n-th and the (n + 1)-th UWB measurement frames. The interpolated d k is the distance measurement value corresponding to the K-th frame image. After achieving the time alignment of visual information and distance information, create a new information frame. The image of the information frame is the K-th frame image, and the UWB measured distance is d k .

[0033] Furthermore, in step 3, the specific process of visual initialization is as follows:

[0034] Step 3.1: For the image data in two consecutive information frames collected, extract the feature points therein respectively, and match the extracted feature points. If the number of matches is greater than the set initialization threshold, set the current two information frames as initialization frames;

[0035] Step 3.2: Based on the two information frames that are used as initialization frames in step 3.1, calculate the fundamental matrix and the homography matrix between the information frames respectively, and restore the poses of the information frames according to the fundamental matrix and the homography matrix respectively. And triangulate the matched feature point pairs in the images within the information frames according to the restored poses of the information frames to obtain the positions of the map points corresponding to the feature point pairs;

[0036] Step 3.3: Calculate the reprojection error of the feature points of the information frame based on the map point position and the pose of the information frame, and use the pose with a small reprojection error and the map point as the pose of the initialization frame and the map point position.

[0037] Further, the specific process of step 4 is as follows:

[0038] Step 4.1: Add the two vision initialization frames to the UWB initialization frame queue, set these two vision initialization frames as key frames, and add the key frames to the local map and the global map;

[0039] Step 4.2: Track the newly acquired information frames subsequently:

[0040] Step 4.2.1: Set the newly acquired information frame as the current frame, the newly created key frame as the reference key frame of the current frame, and set the pose transformation between the two frames before the current frame as the relative pose transformation in the system motion model; after the system receives the current frame, estimate the pose of the current frame using the system motion model, and calculate the distance from the estimated pose to the UWB anchor point. If the difference between the calculated distance and the UWB measured distance in the current frame is less than the set threshold, it means the system motion model is valid, otherwise the system motion model fails;

[0041] If the motion model is valid, establish the connection between the local map points and the image feature points in the current frame, and establish a bundle adjustment equation based on the reprojection error of the image feature points in the current frame to solve the pose of the current frame;

[0042] If the motion model fails, perform the matching of the image feature points in the current frame and the image feature points in its reference key frame, establish the corresponding relationship between the image feature points in the current frame and the map points according to the matching relationship, calculate the reprojection error of the feature points, and establish a bundle adjustment equation to solve the pose of the current frame;

[0043] Step 4.2.2: Calculate the relative pose, the difference in UWB measured distance, and the time difference between the current frame and the previous information frame respectively. If any of the differences in relative pose or measured distance difference and the time difference is greater than the set threshold, set the current frame as a key frame and store it in the local map and the global map;

[0044] Step 4.2.3: Calculate the difference between the UWB anchor point ranging information of the current frame and the ranging information of the latest added frame in the UWB initialization frame queue. If the ranging difference is greater than the set threshold, add the current frame to the UWB initialization frame sequence.

[0045] Further, in step 6, a matching relationship between the image feature points in the new information frame and the map points is established in a manner based on a uniform motion model or a reference key frame. The specific process is as follows:

[0046] Based on the UWB measured distance in the new information frame and the calculated distance obtained from the estimated pose based on the uniform motion model, it is determined whether the uniform motion model can be used for rough tracking estimation. The determination formula is as follows: where is the UWB measured distance in the new information frame, is the calculated distance estimated based on the uniform motion model, and δ is the determination threshold of the uniform motion model;

[0047] If the determination formula is satisfied, indicating that the uniform motion model can be used for rough tracking estimation, then the pose of the new information frame is preliminarily estimated based on the relative pose change between the first two frames of the new information frame. The map points in the local map are projected into the image of the new information frame, and the matching relationship between the map points and the image feature points in the new information frame is established;

[0048] If the determination formula is not satisfied, indicating that the uniform motion model cannot be used for rough tracking estimation, then the matching relationship between the image feature points in the new information frame and the image feature points in its reference key frame is established, and then the connection between the image feature points in the new information frame and the map points is established according to the feature point matching relationship.

[0049] Further, in step 6, the specific process of obtaining the pose of the new information frame by establishing a joint optimization equation based on the reprojection error and the UWB distance error and solving the joint optimization equation is as follows:

[0050] The reprojection error and the UWB distance error are weighted and summed to establish the following joint optimization equation for the pose of the new information frame:

[0051]

[0052] In the formula is the reprojection error, e d is the UWB distance error, is the information matrix of the reprojection error, W d is the information matrix of the UWB distance error; Solving the joint optimization equation can obtain the pose of the new information frame, thereby estimating the real-time pose of the positioning device.

[0053] Further, the specific process of step 7 is as follows:

[0054] Step 7.1: The co-visible key frames of the key frames added to the key frame queue are used as optimized key frames and added to the optimized frame sequence. The map points observed by the optimized key frames in the optimized frame sequence are set as optimized map points, and the reprojection error between the optimized key frames and the optimized map points is established;

[0055] Step 7.2: When the number of optimized key frames in the optimized frame sequence is greater than the set value, establish the distance error between the optimized key frames and the UWB anchors, that is, calculate the distance from the optimized key frame pose to the UWB anchors, and obtain the distance error between the calculated distance and the measured distance in the optimized key frames;

[0056] Step 7.3: Perform a weighted sum of the reprojection error and the distance error to establish a local optimization joint optimization equation, and optimize the key frame poses, map point positions, UWB anchor positions, and the scaling factor between the system scale and the metric scale within the local map.

[0057] Further, the specific process of Step 8 is as follows:

[0058] Step 8.1: Use the bag-of-words model in the global map to retrieve the loop closure frames of the key frames newly added to the global map. When the loop closure frames are retrieved, match the co-visible frames of the loop closure frames and the co-visible frames of the key frames newly added to the global map, and determine whether the loop is valid according to whether the matching similarity is greater than the set threshold;

[0059] Step 8.2: When the loop is valid, calculate the similarity transformation optimization equation between the two frames of the valid loop, solve to obtain the similarity transformation matrix, and correct the pose of the key frames newly added to the global map; and establish and solve the global optimization equation of the key frames between the loops to correct the poses of the key frames between the loops, and if the scale factor of the similarity transformation matrix obtained when solving the global optimization equation is greater than the set threshold, perform optimization of the UWB anchors and the scaling factor between the system scale and the metric unit.

[0060] A mobile device positioning system, characterized in that it includes: an information frame generation module, a system initialization module, a tracking thread, a local optimization thread, and a loop closure detection thread;

[0061] Wherein:

[0062] The information frame generation module synchronizes the UWB ranging information and the acquired image information in real time, and generates an information frame containing image information and distance information;

[0063] The system initialization module first performs vision initialization based on the generated information frame. After completing the vision initialization, the system filters the information frames with vision poses that meet the system initialization requirements, and uses the filtered system initialization information frames to perform system initialization, estimating the anchor position and the scaling factor between the system scale and the metric scale;

[0064] The tracking thread, after completing the system initialization, the system establishes a joint optimization equation based on the relationship between the feature points of the current frame and the map points existing in the system, and estimates the pose of the current frame. The system determines whether to set the current frame as a key frame according to the pose of the current frame, its reference key frame, the UWB distance, and the time difference;

[0065] Local optimization thread. During the information frame tracking process, the set key frames are added to the sliding window of the local optimization thread. After a new key frame is added to the sliding window, local optimization is performed to optimize the poses of the key frames and the positions of the map points within the local sliding window.

[0066] Loop detection thread. For the key frames newly added to the global map, the bag-of-words model is used to match loop frames. After a loop key frame is matched, a spanning tree of the loop frame is established according to the co-visibility relationship, and a global optimization equation is established between the spanning tree and its key frames to optimize the poses of the key frames within the spanning tree, the positions of the observed map points, the scaling factor between the system scale and the metric scale, and the positions of the UWB anchors.

[0067] Beneficial effects

[0068] Compared with the prior art, the advantages of the present invention are as follows:

[0069] The method of the present invention can achieve high-precision positioning information of a mobile device relying only on one UWB anchor, and ensure that the positioning information has absolute scale information.

[0070] The present invention uses the UWB and vision tightly coupled SLAM method to estimate the positioning information of a mobile device, with only one UWB anchor placed in the external environment, and the position of the UWB anchor can be placed arbitrarily.

[0071] The present invention uses the joint optimization of UWB information and vision information to improve the accuracy of the vision SLAM system, accurately estimate the position of the UWB anchor, and achieve the positioning of unknown points in the environment.

[0072] The additional aspects and advantages of the present invention will be partially given in the following description, partially become obvious from the following description, or be understood through the practice of the present invention. Description of the drawings

[0073] The above and / or additional aspects and advantages of the present invention will become obvious and easy to understand from the description of the embodiments in conjunction with the following drawings, where:

[0074] Figure 1 is the flowchart of the positioning method of the UWB and vision tightly coupled SLAM algorithm of the present invention. Detailed implementation manners

[0075] The embodiments of the present invention will be described in detail below. The embodiments are exemplary and are intended to explain the present invention, and should not be construed as a limitation of the present invention.

[0076] As Figure 1 shown, the positioning method of UWB and vision tightly coupled SLAM in this embodiment includes the following steps:

[0077] Step 1: Place a UWB anchor at any position in the environment. Rigidly connect the UWB sensor and the visible light camera to form a positioning device, and fixedly install the positioning device on the mobile device. Run the following steps to achieve real-time positioning.

[0078] Step 2: The positioning device receives UWB distance measurement information of different frequencies and visible light image information of the external environment.

[0079] Since the measurement frequency of the UWB sensor is higher than the image acquisition frequency of the visible light camera, and the tightly coupled algorithm needs to combine the original data of the two sensors at the same moment for calculation, it is necessary to achieve the time alignment of the ranging information and the image information, and based on the image frame, construct an information frame composed of the UWB anchor distance and the image information as the input of the positioning system.

[0080] Here, according to the UWB measurement frames before and after the image frame, the linear interpolation method is used to obtain the distance measurement value at the corresponding moment of the image frame. The specific process of achieving the time alignment of the ranging information and the image information is as follows:

[0081] Take the time of the second image frame received by the visible light camera as the system initial time T0, and take the average value of the timestamps of the two UWB measurement distance frames before and after the first image frame as the moment of the UWB measurement distance initial frame:

[0082]

[0083] The system timestamp of the subsequent Kth image frame And the system timestamp of the Kth UWB measurement frame Are:

[0084]

[0085]

[0086] Where T CK Is the acquisition timestamp of the Kth image frame, T C0 Is the acquisition timestamp of the first image frame, T UK Is the acquisition timestamp of the Kth UWB measurement frame.

[0087] Save the UWB ranging information collected in the acquisition time interval between the (K - 1)th image frame and the (K + 1)th image frame, and retrieve two consecutive UWB ranging information frames that meet the conditions:

[0088]

[0089] Where They are the timestamps of the previous and the next UWB measurement frames when the K-th frame of image is collected. Interpolate the measured distances of the n-th and (n + 1)-th UWB measurement frames. The formula is as follows:

[0090]

[0091] In the formula, d n , d n+1 are the distances measured by the n-th and (n + 1)-th UWB measurement frames respectively. The interpolated d k is the distance measurement value corresponding to the K-th frame of image. After achieving the time alignment of visual information and distance information, create a new information frame. The image of the information frame is the K-th frame of image, and the UWB measured distance is d k .

[0092] Step 3: Conduct visual initialization on the image data in the collected information frames, and estimate the relative poses of two consecutive information frames and the positions of the map points jointly observed by them. The specific processing steps are as follows:

[0093] Step 3.1: For the image data in two consecutive collected information frames, extract the feature points therein respectively, and match the extracted feature points. If the number of matches is greater than the set initialization threshold, set the current two information frames as initialization frames;

[0094] Step 3.2: According to the two information frames that are used as initialization frames in Step 3.1, calculate the fundamental matrix and the homography matrix between the information frames respectively, and restore the poses of the information frames according to the fundamental matrix and the homography matrix respectively. And triangulate the matching feature point pairs in the images within the information frames according to the restored poses of the information frames to obtain the positions of the map points corresponding to the feature point pairs.

[0095] Step 3.3: Calculate the reprojection error of the feature points of the information frames according to the map point positions and the information frame poses, and use the pose and the map points with small reprojection error as the pose and the map point positions of the initialization frames.

[0096] Step 4: After completing the visual initialization, set the pose of the first visually initialized frame as the coordinate system of the system; establish a UWB initialization queue, and track the subsequently newly collected information frames, including establishing the matching relationship between the image feature points and the map points in the subsequent information frames, establishing a visual tracking optimization equation based on the reprojection error, calculating the poses of the subsequent frames, and judging whether to add the newly collected information frames to the UWB initialization queue.

[0097] Since at least three non-coplanar information frames are required for the initialization of the UWB anchor position to establish an optimization equation based on distance error and there is noise in the UWB distance measurement, in order to reduce the influence of noise and realize the initialization of the UWB anchor position, frames with relatively large changes in relative pose are selected as UWB initialization frames based on the measured distance. Therefore, the difference in distance measurement values between consecutive information frames is calculated. When the difference is greater than the set UWB initialization frame selection threshold, this information frame is set as the UWB initialization frame and stored in the UWB initialization frame queue. The specific process is as follows:

[0098] Step 4.1: Add two visual initialization frames to the UWB initialization frame queue, set these two visual initialization frames as key frames, and add the key frames to the local map and the global map.

[0099] Step 4.2: Track the subsequently newly acquired information frames:

[0100] Step 4.2.1: Set the most recently acquired information frame as the current frame, the most recently created key frame as the reference key frame of the current frame, and set the pose transformation between the two frames before the current frame as the relative pose transformation in the system motion model; after the system receives the current frame, use the system motion model to estimate the pose of the current frame, and calculate the distance from the UWB anchor according to the estimated pose. If the difference between the calculated distance and the UWB measured distance in the current frame is less than the set threshold, it means that the system motion model is valid, otherwise the system motion model fails;

[0101] If the motion model is valid, establish the connection between the local map points and the image feature points in the current frame, and establish a bundle adjustment equation based on the reprojection error of the image feature points in the current frame to solve the pose of the current frame;

[0102] If the motion model fails, perform the matching between the image feature points in the current frame and the image feature points in its reference key frame, establish the corresponding relationship between the image feature points in the current frame and the map points according to the matching relationship, calculate the reprojection error of the feature points, and establish a bundle adjustment equation to solve the pose of the current frame.

[0103] Step 4.2.2: Calculate the relative pose, the difference in UWB measured distance, and the time difference between the current frame and its reference key frame between the current frame and the previous information frame respectively. If the relative pose difference or any difference is greater than the corresponding threshold, set the current frame as a key frame and store it in the local map and the global map;

[0104] Step 4.2.3: Calculate the difference between the UWB anchor ranging information of the current frame and the ranging information of the most recently added frame in the UWB initialization frame queue. If the ranging difference is greater than the set threshold, add the current frame to the UWB initialization frame sequence.

[0105] Step 5: Process the information frames in the UWB initialization queue. The specific processing steps are as follows:

[0106] Step 5.1: When the number of information frames in the UWB initialization frame sequence is greater than the set UWB anchor initialization threshold, establish an optimization equation based on the UWB distance error for the information frames in the UWB initialization frame queue. The UWB distance error refers to the difference between the calculated distance and the UWB measured distance in the information frame, where the calculated distance is the distance between the positioning device and the UWB anchor calculated based on the estimated pose in the information frame; then solve the optimization equation to estimate the UWB anchor position and the scaling factor between the visual SLAM scale and the metric unit.

[0107] In this embodiment, when the number of information frames in the UWB initialization frame sequence is greater than 4, the system initialization requirements can be met. The optimization equation is:

[0108]

[0109]

[0110]

[0111] Where is the estimated distance between the positioning device and the UWB anchor, P a is the position coordinate of the UWB anchor, p k is the position of the positioning device estimated from the visual image of the k-th frame information frame, and s is the scaling factor between the visual SLAM scale and the metric unit. is the UWB measured distance value, is the estimated distance value.

[0112] Step 5.2: Use the scaling factor obtained in Step 5.1 to multiply the pose of the information frame and the position of the map point stored in the system by the scaling factor, so as to restore the key frames and map points in the local map to the metric scale.

[0113] Step 6: For the newly acquired information frames in the future, establish the matching relationship between the image feature points and the map points in the new information frame, and establish the reprojection error of the feature points according to the matching relationship. At the same time, use the measured distance and the calculated distance in the new information frame to obtain the UWB distance error of the information frame; establish a joint optimization equation based on the reprojection error and the UWB distance error, and obtain the pose of the new information frame by solving the joint optimization equation.

[0114] Since the UWB anchor position is known, the tracking of the information frame has both visual and distance measurement constraints at the same time. Compared with the pure visual reprojection constraint, the joint distance measurement here can eliminate the inherent drift characteristics of visual positioning.

[0115] Here, a matching relationship between the image feature points and map points in the new information frame is established based on a uniform motion model or by referring to key frames. The specific process is as follows:

[0116] Based on the UWB measured distance in the new information frame and the calculated distance obtained from the estimated pose based on the uniform motion model, it is determined whether the uniform motion model can be used for rough tracking estimation. The determination formula is: Where is the UWB measured distance in the new information frame, is the calculated distance estimated based on the uniform motion model, and δ is the determination threshold of the uniform motion model;

[0117] If the determination formula is satisfied, indicating that the uniform motion model can be used for rough tracking estimation, then the pose of the new information frame is initially estimated based on the relative pose change between the first two frames of the new information frame. The map points in the local map are projected into the image of the new information frame to establish a matching relationship between the map points and the image feature points in the new information frame;

[0118] If the determination formula is not satisfied, indicating that the uniform motion model cannot be used for rough tracking estimation, then a matching relationship between the image feature points in the new information frame and the image feature points in its reference key frame is established, and then the connection between the image feature points in the new information frame and the map points is established according to the feature point matching relationship.

[0119] The specific process of establishing a joint optimization equation based on the reprojection error and UWB distance error and obtaining the pose of the new information frame by solving the joint optimization equation is as follows:

[0120] The reprojection error and UWB distance error are weighted and summed to establish the following joint optimization equation for the pose of the new information frame:

[0121]

[0122]

[0123] e d =(d m ) 2 -(d e ) 2

[0124] In the formula is the reprojection error, e d is the UWB distance error, is the information matrix of the reprojection error, and W d is the information matrix of the UWB distance error. Solving the joint optimization equation can obtain the pose of the new information frame, thereby estimating the real-time pose of the positioning device.

[0125] Then, determine whether the new information frame is a key frame through the following process:

[0126] Calculate the relative pose difference, UWB measurement distance difference, and acquisition time difference between the new information frame and its reference key frame. If any of the differences is greater than the set corresponding threshold, set the new information frame as a key frame, and add the new information frame and the map points observed by it to the local map and the global map.

[0127] According to the set size of the key frame queue in the local map, when the number of key frames in the queue is equal to the queue size, delete the earliest added key frame in the queue and then add the new key frame.

[0128] Step 7: After adding the new key frame to the key frame queue of the local map, establish a local map joint optimization equation. By solving the joint optimization equation, optimize the poses of the key frames in the key frame queue, the map points in the local map, the positions of the UWB anchors, and the scaling factor between the SLAM system and the metric unit.

[0129] Since at least 3 information frames are required to update the position of the UWB anchor, applying multiple key frames in the local map to establish a distance error equation and adding the distance error to the graph optimization can optimize the position of the UWB anchor simultaneously, improve the accuracy of the UWB anchor position estimation, and thus improve the positioning accuracy of the system.

[0130] The specific process is as follows:

[0131] Step 7.1: Use the co-visible key frames of the key frames added to the key frame queue as optimization key frames and add them to the optimization frame sequence. Set the map points observed by the optimization key frames in the optimization frame sequence as optimization map points, and establish the reprojection error between the optimization key frames and the optimization map points.

[0132] Step 7.2: When the number of optimization key frames in the optimization frame sequence is greater than 4, establish the distance error between the optimization key frames and the UWB anchors, that is, calculate the distance to the UWB anchors according to the poses of the optimization key frames, and obtain the distance error between the calculated distance and the measured distance in the optimization key frames.

[0133] Step 7.3: Weight and sum the reprojection error and the distance error to establish a local optimization joint optimization equation, and optimize the poses of the key frames in the local map, the positions of the map points, the positions of the UWB anchors, and the scaling factor between the system scale and the metric scale. The established local optimization joint optimization equation is:

[0134]

[0135]

[0136] Step 8: Execute the loop detection thread for the key frames newly added to the global map. When it is detected that the key frames newly added to the global map match the key frames in the global map, establish an optimization equation for the tight coupling of visual and UWB information between the matching key frames, perform relocalization, and eliminate the pose estimation drift generated during the operation of the mobile device. The specific process is as follows:

[0137] Step 8.1: Use the DBoW2 bag-of-words model in the global map to retrieve the loop frames of the key frames newly added to the global map. After retrieving the loop frames, match the co-visible frames of the loop frames with the co-visible frames of the key frames newly added to the global map, and determine whether the loop is valid according to whether the matching similarity is greater than the set threshold; among them, a frame generation tree data structure can be established according to the inter-frame co-visibility relationship, and the generation tree can be matched to determine whether the loop is valid.

[0138] Step 8.2: When the loop is valid, calculate the similarity transformation optimization equation between the two frames of the valid loop, solve to obtain the similarity transformation matrix, and correct the pose of the key frame newly added to the global map. The similarity transformation optimization equation is specifically

[0139]

[0140] where Q i and P i are the same map points matched by the loop frames, s is the scaling factor between the loop frames, R is the rotation matrix between the loop frames, and t is the translation amount between the loop frames.

[0141] In addition, a global optimization equation for the key frames between loops is established and solved to correct the poses of the key frames between loops, and when the scale factor of the similarity transformation matrix obtained when solving the global optimization equation is greater than the set threshold, optimize the UWB anchor points, system scale, and the scaling factor between the metric units.

[0142] Based on the above method, the mobile device positioning system of the UWB and vision tightly coupled SLAM method in the embodiments of the present invention includes: an information frame generation module, a system initialization module, a tracking thread, a local optimization thread, and a loop detection thread;

[0143] Among them:

[0144] The information frame generation module synchronizes the UWB ranging information and the acquired image information in real time, and then generates an information frame containing image information and distance information.

[0145] The system initialization module first performs vision initialization based on the generated information frame. After completing the vision initialization, the system filters the information frames with visual poses that meet the system initialization requirements, and uses the filtered system initialization information frames to perform system initialization to estimate the anchor point position, system scale, and the scaling factor between the metric scale.

[0146] Tracking thread: After the system initialization is completed, the system establishes a joint optimization equation based on the relationship between the feature points of the current frame and the map points in the system memory to estimate the pose of the current frame. The system determines whether to set the current frame as a key frame according to the pose of the current frame and its reference key frame, the UWB distance, and the time difference.

[0147] Local optimization thread: During the information frame tracking process, the set key frames are added to the sliding window of the local optimization thread. After a new key frame is added to the sliding window, local optimization is performed to optimize the poses of the key frames and the positions of the map points within the local sliding window.

[0148] Loop detection thread: For the key frames newly added to the global map, the bag-of-words model is used to match the loop frames. After a loop key frame is matched, a spanning tree of the loop frame is established according to the co-visibility relationship. A global optimization equation is established between the spanning tree and its key frames to optimize the poses of the key frames within the spanning tree, the positions of the observed map points, the scaling factor between the system scale and the metric scale, and the positions of the UWB anchors.

[0149] Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention without departing from the principles and spirit of the present invention.

Claims

1. A positioning method for an ultra-wideband (UWB) and vision tightly coupled SLAM algorithm, characterized in that: It includes the following steps: Step 1: Place one or more UWB anchors at any position in the environment. Rigidly connect the UWB sensor and the camera to form a positioning device, and fixedly install the positioning device on the mobile device; Step 2: Real-time collect the distance information between the positioning device and the UWB anchors through the UWB sensor on the positioning device, and collect the environmental image information through the camera on the positioning device. At the same time, achieve the time alignment of the environmental image information and the distance information. Based on the image frames, construct an information frame composed of the UWB anchor distances and the image information; Step 3: Perform visual initialization on the image data in the collected information frames, and estimate the relative poses of two consecutive information frames and the positions of the map points jointly observed by them; Step 4: After completing the visual initialization, set the pose of the first visually initialized frame as the coordinate system of the system; establish a UWB initialization queue, and track the subsequently newly collected information frames, including establishing the matching relationship between the image feature points and the map points in the subsequent information frames, establishing a visual tracking optimization equation based on the reprojection error, calculating the poses of the subsequent frames, and determining whether to add the newly collected information frames to the UWB initialization queue; Step 5: Process the information frames in the UWB initialization queue: Step 5.1: When the number of information frames in the UWB initialization frame sequence is greater than the set UWB anchor initialization threshold, establish an optimization equation based on the UWB distance error for the information frames in the UWB initialization frame queue. The UWB distance error refers to the difference between the calculated distance and the UWB measured distance in the information frame, where the calculated distance refers to the distance between the positioning device and the UWB anchor calculated according to the pose estimated from the information frame; then solve the optimization equation to estimate the UWB anchor positions and the scaling factor between the visual SLAM scale and the metric unit; Step 5.2: Use the scaling factor obtained in Step 5.1 to multiply the pose of the information frame and the position of the map point stored in the system, so as to restore the key frames and map points in the local map to the metric scale; Step 6: For the newly collected subsequent information frames, establish the matching relationship between the image feature points and the map points in the new information frames, and establish the reprojection error of the feature points according to the matching relationship. At the same time, use the measured distance and the calculated distance in the new information frame to obtain the UWB distance error; establish a joint optimization equation based on the reprojection error and the UWB distance error, and obtain the pose of the new information frame by solving the joint optimization equation; and determine whether the new information frame is a key frame: Calculate the relative pose difference, UWB measurement distance difference, and acquisition time difference between the new information frame and its reference key frame. If any difference is greater than the set corresponding threshold, set the new information frame as a key frame, and add the new information frame and the map points it observes to the local map and the global map; Step 7: After adding the new key frame to the key frame queue of the local map, establish the local map joint optimization equation. By solving the joint optimization equation, optimize the key frame poses in the key frame queue, the map points in the local map, the positions of the UWB anchors, and the scaling factor between the SLAM system and the metric unit; Step 8: Execute the loop detection thread for the key frame newly added to the global map. When it is detected that the key frame newly added to the global map matches the key frames in the global map, establish an optimization equation for the tight coupling of visual and UWB information between the matching key frames, perform relocalization, and eliminate the pose estimation drift generated during the operation of the mobile device.

2. The positioning method of an ultra-wideband (UWB) and vision tightly coupled SLAM algorithm according to claim 1, characterized in that: In Step 2, according to the UWB measurement frames before and after the image frame, use the method of linear interpolation to obtain the distance measurement value at the corresponding moment of the image frame, and realize the time alignment of the ranging information and the image information.

3. The positioning method of an ultra-wideband (UWB) and vision tightly coupled SLAM algorithm according to claim 2, wherein: The process of realizing the time alignment of the ranging information and the image information is as follows: Take the time of the second image frame received by the camera as the system initial time T0, and take the time stamp mean of the two UWB measurement distance frames before and after the first image frame as the moment of the UWB measurement distance initial frame: System timestamp of the subsequent K-th frame image And the system timestamp of the K-th UWB measurement frame Is: where T CK is the acquisition timestamp of the K-th frame image, T C0 is the acquisition timestamp of the first frame image, T UK is the acquisition timestamp of the K-th UWB measurement frame; Save the UWB ranging information collected during the acquisition time interval between the (K - 1)-th image frame and the (K + 1)-th image frame, and retrieve two consecutive UWB ranging information frames that meet the conditions: where are the timestamps of the previous and the next UWB measurement frames when the K-th frame of the image is collected, respectively. Interpolate the measured distances of the n-th and the (n + 1)-th UWB measurement frames. The formula is as follows: where d n , d n+1 are the distances measured in the n-th frame and the (n + 1)-th UWB measurement frame respectively, and the interpolated d k is the distance measurement value corresponding to the K-th frame image. After achieving the temporal alignment of visual information and distance information, a new information frame is created. The image of the information frame is the K-th frame image, and the UWB measured distance is d k .

4. The positioning method of a UWB and vision tightly coupled SLAM algorithm according to claim 1, characterized in that: In Step 3, the specific process of visual initialization is as follows: Step 3.1: For the image data in two consecutive information frames collected, extract the feature points therein respectively, and match the extracted feature points. If the number of matches is greater than the set initialization threshold, set the current two information frames as the initialization frames; Step 3.2: According to the two information frames used as the initialization frames in Step 3.1, calculate the fundamental matrix and the homography matrix between the information frames respectively, and restore the poses of the information frames according to the fundamental matrix and the homography matrix respectively. And triangulate the matching feature point pairs in the images of the information frames according to the restored poses of the information frames to obtain the positions of the map points corresponding to the feature point pairs; Step 3.3: Calculate the reprojection error of the feature points of the information frames according to the map point positions and the information frame poses, and use the pose and map point positions with small reprojection error as the pose and map point positions of the initialization frames.

5. The positioning method of a UWB and vision tightly coupled SLAM algorithm according to claim 1, characterized in that: The specific process of Step 4 is as follows: Step 4.1: Add the two visual initialization frames to the UWB initialization frame queue, and set these two visual initialization frames as key frames, and add the key frames to the local map and the global map; Step 4.2: Track the newly acquired information frames subsequently: Step 4.2.1: Set the latest acquired information frame as the current frame, the latest created key frame as the reference key frame of the current frame, and set the pose transformation between the two frames before the current frame as the relative pose transformation in the system motion model; after the system receives the current frame, use the system motion model to estimate the pose of the current frame, and calculate the distance from the estimated pose to the UWB anchor. If the difference between the calculated distance and the UWB measurement distance in the current frame is less than the set threshold, it means that the system motion model is valid, otherwise the system motion model fails; If the motion model is valid, establish the connection between the local map points and the image feature points in the current frame, and establish a bundle adjustment equation based on the reprojection error of the image feature points in the current frame to solve the pose of the current frame; If the motion model fails, perform the matching between the image feature points in the current frame and the image feature points in its reference key frame, establish the corresponding relationship between the image feature points in the current frame and the map points according to the matching relationship, calculate the reprojection error of the feature points, and establish a bundle adjustment equation to solve and obtain the pose of the current frame; Step 4.2.2: Calculate the relative pose between the current frame and the previous information frame, the difference in UWB measurement distances, and the time difference between the current frame and its reference key frame respectively. If any of the differences in relative pose or measurement distance difference and time difference is greater than the set threshold, set the current frame as a key frame and store it in the local map and the global map; Step 4.2.3: Calculate the difference between the UWB anchor ranging information of the current frame and the ranging information of the latest added frame in the UWB initialization frame queue. If the ranging difference is greater than the set threshold, add the current frame to the UWB initialization frame sequence.

6. The positioning method of a UWB and vision tightly coupled SLAM algorithm according to claim 1, characterized in that: In step 6, establish the matching relationship between the image feature points and the map points in the new information frame by using the uniform motion model or the reference key frame. The specific process is as follows: Based on the UWB measured distance in the new information frame and the calculated distance obtained from the estimated pose based on the uniform motion model, determine whether the uniform motion model can be used for rough tracking estimation. The judgment formula is: where is the UWB measured distance in the new information frame, is the calculated distance estimated based on the uniform motion model, and δ is the judgment threshold of the uniform motion model; If the judgment formula is satisfied, indicating that the uniform motion model can be used for rough tracking estimation, preliminarily estimate the pose of the new information frame based on the relative pose change between the first two frames of the new information frame, project the map points in the local map into the image of the new information frame, and establish the matching relationship between the map points and the image feature points in the new information frame; If the judgment formula is not satisfied, indicating that the uniform motion model cannot be used for rough tracking estimation, establish the matching relationship between the image feature points in the new information frame and the image feature points in its reference key frame, and then establish the connection between the image feature points in the new information frame and the map points according to the feature point matching relationship.

7. The positioning method of a UWB and vision tightly coupled SLAM algorithm according to claim 1, characterized in that: In step 6, establish a joint optimization equation based on the reprojection error and the UWB distance error. The specific process of obtaining the pose of the new information frame by solving the joint optimization equation is as follows: Weight and sum the reprojection error and the UWB distance error, so as to establish the following joint optimization equation for the pose of the new information frame: where is the reprojection error, e d is the UWB distance error, is the information matrix of the reprojection error, W d is the information matrix of the UWB distance error; Solving the joint optimization equation can obtain the pose of the new information frame, thereby estimating the real-time pose of the positioning device.

8. The positioning method of a UWB and vision tightly coupled SLAM algorithm according to claim 1, characterized in that: The specific process of step 7 is as follows: Step 7.1: Take the co-visible key frames of the key frames added to the key frame queue as the optimized key frames and add them to the optimized frame sequence. Set the map points observed by the optimized key frames in the optimized frame sequence as the optimized map points, and establish the reprojection error between the optimized key frames and the optimized map points; Step 7.2: When the number of optimized key frames in the optimized frame sequence is greater than the set value, establish the distance error between the optimized key frames and the UWB anchors, that is, calculate the distance from the optimized key frame pose to the UWB anchors, and obtain the distance error between the calculated distance and the measured distance in the optimized key frames; Step 7.3: Weightedly sum the reprojection error and the distance error to establish a local optimization joint optimization equation, and optimize the poses of key frames, the positions of map points, the positions of UWB anchors, and the scaling factor between the system scale and the metric scale within the local map.

9. The positioning method of a UWB and vision tightly coupled SLAM algorithm according to claim 1, characterized in that: The specific process of Step 8 is as follows: Step 8.1: Use the bag-of-words model in the global map to retrieve the loop closure frames of the key frames newly added to the global map. After retrieving the loop closure frames, match the co-visible frames of the loop closure frames with the co-visible frames of the key frames newly added to the global map, and determine whether the loop is valid according to whether the matching similarity is greater than the set threshold. Step 8.2: When the loop is valid, calculate the similarity transformation optimization equation between the two frames of the valid loop, solve to obtain the similarity transformation matrix, and correct the pose of the key frame newly added to the global map; and establish and solve the global optimization equation of the key frames between the loops to correct the poses of the key frames between the loops, and when the scale factor of the similarity transformation matrix obtained when solving the global optimization equation is greater than the set threshold, perform optimization on the UWB anchors and the scaling factor between the system scale and the metric unit.

10. A mobile device positioning system based on the method according to any one of claims 1 to 9, characterized in that: It includes: An information frame generation module, a system initialization module, a tracking thread, a local optimization thread, and a loop closure detection thread; Among them: The information frame generation module synchronizes the time of the UWB ranging information and the acquired image information obtained in real time, and generates an information frame containing the image information and the distance information. The system initialization module first performs vision initialization based on the generated information frame. After completing the vision initialization, the system filters the information frames with vision poses that meet the system initialization, and uses the filtered system initialization information frames to perform system initialization to estimate the anchor position and the scaling factor between the system scale and the metric scale. The tracking thread, after completing the system initialization, the system establishes a joint optimization equation according to the relationship between the feature points of the current frame and the map points existing in the system, and estimates the pose of the current frame; the system determines whether to set the current frame as a key frame according to the pose of the current frame, its reference key frame, the UWB distance, and the time difference. The local optimization thread, during the information frame tracking process, adds the set key frames to the sliding window of the local optimization thread. After the new key frames are added to the sliding window, local optimization is performed to optimize the poses of the key frames and the positions of the map points within the local sliding window. The loop closure detection thread uses the bag-of-words model to match the loop closure frames for the key frames newly added to the global map. After matching the loop closure key frames, a spanning tree of the loop closure frames is established according to the co-visibility relationship, and a global optimization equation is established between the spanning tree and its key frames to optimize the poses of the key frames within the spanning tree, the observed map point positions, the scaling factor between the system scale and the metric scale, and the UWB anchor positions.

Images