Method and system for jointly initializing position of ultra-wideband base station

Abstract

The invention discloses a method and a system for jointly initializing the position of an ultra-wideband base station. The method comprises the following steps of: cooperatively working through a plurality of carriers carrying visual sensors and ultra-wideband tags; establishing an inter-carrier distance measurement model and a visual pose constraint; each carrier records the pose in the own coordinate system and recognizes the coordinate of the base station image; calculating a base station distance based on visual re-projection, and fusing ultra-wideband ranging to match a base station ID; broadcasting the position of the base station and synchronizing the position to each carrier coordinate system through coordinate conversion; and constructing a ranging residual model to iteratively optimize the position of the base station until convergence and initialization are completed. According to the invention, by fusing visual perception, inertial measurement and UWB ranging technologies, the problems of tedious deployment, low efficiency and the like of the UWB base station in an unknown environment are effectively solved. In addition, by constructing the relative constraint relationship among multiple carriers, the precision of pose estimation of each carrier is effectively improved, and the positioning accuracy of the UWB base station is further improved.

Description

Technical Field

[0001] This invention belongs to the field of multi-sensor combined positioning technology, specifically relating to a method and system for jointly initializing the location of an ultra-wideband base station. Background Technology

[0002] With the rapid development of technology and the acceleration of urbanization, high-rise buildings and complex structures are proliferating rapidly. However, these modern buildings pose enormous challenges to rescue efforts when faced with sudden disasters such as natural disasters, fires, or building collapses. Accurate location estimation is crucial for firefighters, not only as it is key to their efficient rescue operations but also as an important prerequisite for ensuring the safety of trapped personnel and their own lives. Ultra-wideband (UWB) wireless communication technology, with its advantages of high time resolution, resistance to multipath interference, and low power consumption, is widely used for high-precision indoor positioning. However, UWB base stations require advance deployment and accurate location, greatly increasing its complexity. Therefore, using sensor outputs on a carrier to perform online calibration of UWB anchor points will significantly improve the efficiency of UWB base station deployment.

[0003] In the deployment of existing ultra-wideband (UWB) positioning systems, base station coordinate calibration mainly relies on nonlinear least squares optimization algorithms based on ranging observations. This classic method exhibits good convergence in ideal line-of-sight (LOS) propagation environments. However, when the system is deployed in complex non-line-of-sight (NLOS) propagation scenarios with wall obstructions, metal reflections, etc., the ranging data will produce anomalous observations that deviate significantly from the true values ​​due to multipath effects and signal attenuation distortion. This ultimately leads to a decrease in positioning accuracy, making it difficult to meet the stringent real-time positioning accuracy requirements of emergency rescue scenarios. Summary of the Invention

[0004] The purpose of this invention is to solve the problem that base stations need to be deployed in advance and is cumbersome when using ultra-wideband positioning, and to provide a method and system for jointly initializing the location of ultra-wideband base stations.

[0005] The objective of this invention is achieved through the following technical solution:

[0006] A method for jointly initializing the location of an ultra-wideband base station includes the following steps:

[0007] A ranging model is constructed between ultra-wideband tags and between tags and base stations; each carrier is equipped with a visual sensor and an ultra-wideband tag;

[0008] The initial relative pose relationship between multiple carriers is determined by visual feature matching;

[0009] Each carrier records its pose information in its own world coordinate system, establishes mutual constraints among multiple carriers, and visually identifies the coordinates of the ultra-wideband base station in the image through a visual sensor.

[0010] The visual distance between each carrier and the detected ultra-wideband base station is calculated based on the visual reprojection model, and the base station ID is matched by combining the ultra-wideband ranging data.

[0011] The locations of UWB base stations detected by each carrier are broadcast via UWB tags, and other carriers convert the UWB base station locations to their own world coordinate system through inter-carrier coordinate transformation.

[0012] The ranging residual between the ultra-wideband base station and the carrier is constructed, and the base station location is refined through optimization algorithm. Initialization is completed when the preset conditions are met.

[0013] Furthermore, the construction of the ranging model includes: first, assigning a unique base station ID to each base station and a unique tag T_ID to each tag; starting the tag broadcast mechanism, starting from time T_ID=A, and the receiving end replying with messages according to the time sequence of "base station ID first, then tag T_ID"; calculating the distance between the base station and the tag based on the bilateral bidirectional ranging DS-TWR algorithm, and completing the mutual ranging of multiple base stations and multiple tags through a cyclic process.

[0014] Furthermore, in the mutual ranging of multiple base stations and multiple tags, after the last tag T_ID completes the message response, the current tag initiates a broadcast operation, calculates the distance between the base station and the tag based on the DS-TWR algorithm, and then sends the ranging value in the next reply to the tag. Subsequently, the tag switches to the receiving state to respond to the ranging requests of other tags, and each tag executes the operation in a loop.

[0015] Furthermore, the visual feature matching includes: extracting feature points from the visual sensor output image using the SIFT feature extraction algorithm; performing descriptor matching using the FLANN matching algorithm, combined with bidirectional matching and Lowe ratio to eliminate mismatches; eliminating mismatches again and calculating the essential matrix using the RANSAC algorithm; and finally restoring the relative pose relationship between the visual sensors using the ICP method.

[0016] Furthermore, pose information is defined as: the pose information of the j-th carrier:

[0017]

[0018] Download the body pose information for the k-th pose of the j-th carrier:

[0019]

[0020] in, and Let be the position, velocity, and acceleration of the k-th posture in the world coordinate system of the j-th carrier, respectively. and These are the gyroscope bias and accelerometer bias of the j-th carrier world coordinate system IMU, respectively;

[0021] Establish constraints between carriers:

[0022]

[0023] in, The observation equation between the carrier in the k-th attitude and the i-th carrier is represented by χ = [x1, x2, ..., x]. n ], Let be the position of the i-th carrier in its own world coordinate system. Let j be the position of the j-th carrier in its own world coordinate system. Let d be the pose change matrix of carriers i and j. ij The distance between the tags on carriers i and j is measured.

[0024] Each carrier detected the coordinates of the base station center in the image:

[0025]

[0026] in, and Let x and y be the x and y coordinates of j carriers in the visual sensor coordinate system, respectively.

[0027] Furthermore, the visual distance calculation between each carrier and the detected ultra-wideband base station, and the matching of base station IDs, are achieved by minimizing the distance error:

[0028]

[0029] Where x is the paired base station ID, d x The ranging data for the base station, This represents the difference between the carrier's location and the solved location of the ultra-wideband base station.

[0030] Furthermore, the information broadcast by the ultra-wideband tag includes: ranging values ​​d from n base stations. n The distance measurement value between the current carrier and the nth carrier. The base station detected by the current carrier The corresponding ID base station's position in the j-th world coordinate system Other carriers obtain the location of the base station through a transformation matrix. accomplish: The relative pose relationship is determined by visual feature matching.

[0031] Furthermore, the ranging residual is:

[0032]

[0033] in, The observation equation between the carrier and the j-th ultra-wideband base station is represented by the k-th attitude. This represents the location of the nth ultra-wideband base station in the current carrier world coordinate system. As the current posture of the carrier, Let be the extrinsic translation vector between the UWB tag and the IMU on the j-th carrier. Let K be the position of the current carrier in the k-th posture. The measured distance between the carrier and the nth UWB base station at that time; The carrier position deviation represents the time interval [t] from the camera image time to the UWB data time. k ,t j The IMU measurements within the [database] are used to predict the carrier position deviation:

[0034]

[0035] in, Let Δt be the carrier velocity. k,j The time offset corresponding to the time interval; g ω It is the acceleration due to gravity; This represents the IMU pre-integrated value within the time interval.

[0036] Furthermore, iterative optimization is performed using the Ceres function library. When the residual converges to the threshold, the optimization stops and the base station location is fixed.

[0037] A computer device / apparatus / system includes a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of a joint initialization method for ultra-wideband base station location.

[0038] The beneficial effects of this invention are as follows:

[0039] The beneficial effects of this invention are that by integrating visual perception, inertial measurement, and ultra-wideband (UWB) ranging technology, it effectively solves the problems of cumbersome and inefficient deployment of UWB base stations in unknown environments. Traditional nonlinear least-squares optimization methods based on ranging observations, while exhibiting good convergence in ideal line-of-sight (LOS) environments, are susceptible to multipath effects and signal attenuation distortion under complex non-line-of-sight (NLOS) propagation conditions such as wall obstruction and metal reflection, leading to significant errors in ranging data and consequently reducing base station positioning accuracy. This invention, based on a visual perception-based initialization strategy, avoids these positioning error problems caused by non-line-of-sight scenarios. Furthermore, compared to traditional methods relying on the pose of a single carrier, this invention effectively improves the accuracy of pose estimation for each carrier by constructing relative constraint relationships between multiple carriers, thereby enhancing the positioning accuracy of the UWB base station. Simultaneously, the multi-carrier collaborative operation mode significantly improves the deployment efficiency of UWB base stations and the overall robustness of the system in complex scenarios. Attached Figure Description

[0040] Figure 1 System flowchart

[0041] Figure 2 For DS-TWS ranging communication mechanism

[0042] Figure 3 This is a ranging communication mechanism between ultra-wideband tags and between tags and base stations. Detailed Implementation

[0043] The present invention will now be further described with reference to the accompanying drawings. It should be noted that, unless otherwise specified, the embodiments and features described in the embodiments of the present invention can be combined with each other.

[0044] This invention discloses a method and system for jointly initializing the location of an ultra-wideband base station. The specific flowchart of the method is shown below. Figure 1 As shown, it includes the following steps:

[0045] Step 1: Construct ranging models between ultra-wideband tags and between tags and base stations;

[0046] Combination Figure 2 and Figure 3 First, each base station is labeled with an ID (0, 1, ..., n), and each tag is labeled with a T_ID (A, B, ...). Tags have two states: ranging and receiving. A broadcast message is sent from tag A. Each base station and other tags receiving the broadcast message from tag A reply in sequence, first the base station, then the tag. After the last tag ID has replied, tag A sends a stop broadcast message, completing the ranging for that tag. Then, other tags continue this operation in ID order. The timing is controlled by base station 0. The communication mechanism follows the DS-TWR protocol for signal exchange, such as... Figure 2 As shown. Tag A broadcasts a signal, and base station 0 receives the broadcast signal and its system time counter at base station 0 counts after a value T. reply1 Afterwards, an acknowledgment signal is sent to tag A. Tag A receives the acknowledgment signal and its system time counter expires after a count value T. reply2 Afterwards, a termination signal is sent to base station 0. The system time counter for tag A accumulates a count value T from the time the interrogation signal is sent to the time the response signal is received. round1 When base station 0 receives the termination signal, the system time counter of base station 0 has accumulated a count value T from the time the acknowledgment signal was sent to the time the termination signal was received. round2 The formula for estimating the device counter value corresponding to the signal flight time based on these counter differences is as follows:

[0047]

[0048] Among them, T round The distance between the tag and the base station is calculated by multiplying the flight time in the air by the speed of light.

[0049] After the ranging operation for each tag is completed, the current tag switches states, marking one cycle for the current tag. The ranging result is then output, and ranging operations from other tags are accepted. When the ranging operation for the last ID tag is completed, the entire system completes its cycle. Figure 3 As shown.

[0050] Step 2: Determine the initial relative positional relationship between the carriers through visual feature matching.

[0051] Using image information from each camera, image feature points are first extracted using the SIFT feature extraction algorithm. Then, descriptor matching is performed using the FLANN matching algorithm, with bidirectional matching and Lowe ratio filtering to remove false matches. RANSAC is used to further remove false matches and calculate the essential matrix. Finally, the relative pose relationships between the cameras are recovered using the ICP method.

[0052] Step 3: Each carrier records its own pose information in its own world coordinate system. Using its own camera, a visual target detection algorithm is applied to identify the ultra-wideband base stations it detects. Simultaneously, each carrier records the coordinates of the ultra-wideband base stations it detects in the image.

[0053] After obtaining the relative poses of multiple carriers, each carrier undergoes pure visual structure and motion reconstruction (SfM). Feature points are extracted within a sliding window, and scale-free relative camera poses and 3D structures are estimated. Simultaneously, IMU data within the window is pre-integrated to establish constraint equations between vision and inertial measurement. Solving linear closed-form solutions recovers the scale factor, initial velocity, gravity direction, and estimates the extrinsic parameters between the IMU and camera. Finally, using these initial values, a tightly coupled visual-inertial nonlinear optimization is performed to determine the system's initial state. Each carrier system then enters visual-inertial tracking, outputting its pose information X in n different postures. j , represented as:

[0054]

[0055] in, The k-th pose of the j-th carrier is:

[0056]

[0057] in, and Let be the position, velocity, and acceleration of the k-th posture in the world coordinate system of the j-th carrier, respectively. and is These are the gyroscope bias and accelerometer bias of the j-th carrier world coordinate system IMU, respectively.

[0058] After each carrier completes its visual inertial initialization, each carrier constructs a residual factor based on its own pose and the measured distance between itself and other carriers.

[0059]

[0060] in, Let χ = ​​[x1, x2, ..., xj] represent the observation equation between the i-th carrier at the k-th attitude and the j-th carrier in the current world coordinate system. n ], This represents the k-th carrier position in the current carrier world coordinate system. This is the matrix representing the change in relative pose. Let j be the position of the j-th carrier in the k-th posture. The distance between the i-th and j-th carriers at time i represents the measured distance at time j.

[0061] During the carrier's movement, the base station is randomly fixed to the wall surface. The carrier observes the base station using its onboard visual sensors. When a fixed base station is detected, the carrier's visual detection model, based on the YOLOv5 target detection algorithm, extracts the UWB base station pixel coordinates from the detected scene image sequence to determine the two-dimensional imaging position of the base station in each visual frame. This allows the acquisition of the pixel coordinates of the base station's center point in the image.

[0062] Step 4: Calculate the distance between the base station and the carrier using the visual reprojection model, solve for it using the Levenberg-Marquardt method, and match the corresponding ID based on the ultra-wideband measurement information.

[0063] Using any UWB base station as a spatial positioning reference node, a subset of images containing that base station is selected from the image set acquired by all carriers. For each visual measurement frame in the image subset, a spatial coordinate residual equation based on the camera projection model is constructed. A set of three-dimensional spatial position calculation equations for the UWB base station is established through multi-view geometric constraints, forming a multi-frame jointly optimized reprojection error constraint model.

[0064]

[0065] in, Let j be the expression for the measurement residual of the j-th carrier. Let n be the observation expression for the current location of the UWB base station in the nth frame image, where n is the total number of detected base station images. This represents the current location of the UWB base station in the j-th carrier world coordinate system. and Let be the extrinsic rotation matrix and extrinsic translation vector of the j-th carrier camera-IMU, respectively; and These represent the rotation and translation of the carrier corresponding to the i-th frame image in the local world coordinate system; This represents the pixel coordinates in the i-th image corresponding to the geometric center of the UWB base station detected by the j-th carrier.

[0066] The Levenberg-Marquardt method is used to solve the reprojection error constraint model to obtain the current location of the UWB base station in the j-th world coordinate system. And combined with the pose information of the corresponding camera carrier Calculate the visual distance between the current UWB base station and the camera carrier in the image.

[0067] Match the corresponding ID based on the ultra-wideband measurement information:

[0068]

[0069] Where x is the paired base station ID, d x The ranging data for the base station, This represents the difference between the carrier's location and the solved location of the ultra-wideband base station.

[0070] Step 5: Broadcast the UWB base station location calculated by each carrier. Other carriers will then convert the UWB base station location to their own world coordinate system through coordinate transformation between carriers.

[0071] The location of the ultra-wideband base station, calculated by each carrier, is broadcast via an ultra-wideband tag, which is represented as follows:

[0072]

[0073] Where, d n This represents the ranging value of the nth base station. This represents the distance measurement value between the current carrier and the nth carrier. The base station ID detected by the current carrier. This represents the position of the corresponding ID base station in the j-th world coordinate system.

[0074] The location of a UWB base station, transformed into its own local world coordinate system, is represented as:

[0075]

[0076] in, This is for coordinate transformation between carriers.

[0077] Step 6: Construct the ranging residual between the ultra-wideband base station and the carrier, optimize it, further refine it, and complete the initialization when the conditions are met;

[0078] Each carrier constructs the state variables to be solved:

[0079]

[0080] in, Let be the location of the s-th UWB base station in the j-th carrier world coordinate system. Let be the location of the ns-th UWB base station in the i-th carrier world coordinate system, where n is the total number of UWB base stations, and s = 1, 2, ..., n.

[0081] Construct the expression for the measurement residual between the self-detected UWB base station and its own carrier:

[0082]

[0083] in, The observation equation between the carrier and the s-th ultra-wideband base station is represented by the k-th attitude. This represents the location of the s-th base station in the current carrier's world coordinate system. As the current posture of the carrier, The extrinsic translation vector between the UWB label and the IMU. Let K be the position of the current carrier in the k-th posture. This represents the ranging measurement between the carrier and the s-th UWB base station.

[0084] Construct an expression for the measurement residual between other carriers detecting UWB base stations and their own carriers:

[0085]

[0086] in, To detect the position of the nth UWB base station in the i-th world coordinate system for the i-th carrier. This represents the distance measurement between the carrier and the nth UWB base station.

[0087] The carrier position deviation represents the time interval [t] from the camera image time to the UWB data time. k ,t j The IMU measurements within the [database] are used to predict the carrier position deviation:

[0088]

[0089] in, Let Δt be the carrier velocity. k,j For the time interval [t] k ,t j The corresponding time offset; g ω It is the acceleration due to gravity; This represents the IMU pre-integrated value within the time interval.

[0090] Construct the measurement residual expressions for two different UWB base stations:

[0091]

[0092] in, To unify the position of the j-th carrier to the n-th UWB base station in its own world coordinate system, where n is the total number of UWB base stations. Let be the measurement residual of the distance between the i-th UWB base station and the j-th UWB base station in the current carrier coordinate system. Let i be the observation equation between the i-th UWB base station and the j-th UWB base station. and These are the positions of the i-th and j-th UWB base stations in their respective world coordinate systems. Let be the measured distance between the i-th UWB base station and the j-th UWB base station, i = 1, 2, ..., s, j = 1, 2, ..., s, i ≠ j.

[0093] Then, the data are fed into the respective carrier factor graph optimization functions, and the Levenberg-Marquardt method is used to perform nonlinear optimization to solve for the corresponding carrier position, velocity, attitude, and UWB base station information. When optimizing the UWB base station, the base station position is continuously refined through iteration. When the position change of the base station is less than 0.01 meters, the base station position is fixed and no further optimization is performed.

[0094] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for jointly initializing the location of an ultra-wideband base station, characterized in that: Includes the following steps: A ranging model is constructed between ultra-wideband tags and between tags and base stations; each carrier is equipped with a visual sensor and an ultra-wideband tag; The initial relative pose relationship between multiple carriers is determined by visual feature matching; Each carrier records its pose information in its own world coordinate system, establishes mutual constraints among multiple carriers, and visually identifies the coordinates of the ultra-wideband base station in the image through a visual sensor. The visual distance between each carrier and the detected ultra-wideband base station is calculated based on the visual reprojection model, and the base station ID is matched by combining the ultra-wideband ranging data. The locations of UWB base stations detected by each carrier are broadcast via UWB tags, and other carriers convert the UWB base station locations to their own world coordinate system through inter-carrier coordinate transformation. The ranging residual between the ultra-wideband base station and the carrier is constructed, and the base station location is refined through optimization algorithm. Initialization is completed when the preset conditions are met.

2. The method for jointly initializing the location of an ultra-wideband base station according to claim 1, characterized in that: The ranging model construction includes: first, assigning a unique base station ID to each base station and a unique tag T_ID to each tag; starting from T_ID=A, initiating the tag broadcast mechanism, with the receiving end replying with messages in the order of "base station ID first, then tag T_ID"; calculating the distance between the base station and the tag based on the bilateral bidirectional ranging DS-TWR algorithm, and completing the mutual ranging of multiple base stations and multiple tags through a cyclic process.

3. The method for jointly initializing the location of an ultra-wideband base station according to claim 2, characterized in that: In the mutual ranging of multiple base stations and multiple tags, after the last tag T_ID completes the message response, the current tag initiates a broadcast operation, calculates the distance between the base station and the tag based on the DS-TWR algorithm, and then sends the ranging value in the next reply to the tag. Subsequently, the tag switches to the receiving state to respond to the ranging requests of other tags, and each tag executes the operation in a loop.

4. The method for jointly initializing the location of an ultra-wideband base station according to claim 1, characterized in that: The visual feature matching includes: extracting feature points from the visual sensor output image using the SIFT feature extraction algorithm; performing descriptor matching using the FLANN matching algorithm, combined with bidirectional matching and Lowe ratio to eliminate mismatches; eliminating mismatches again and calculating the essential matrix using the RANSAC algorithm; and finally restoring the relative pose relationship between visual sensors using the ICP method.

5. The method for jointly initializing the location of an ultra-wideband base station according to claim 1, characterized in that: Pose information is defined as: the pose information of the j-th carrier: Download the body pose information for the k-th pose of the j-th carrier: in, and Let be the position, velocity, and acceleration of the k-th posture in the world coordinate system of the j-th carrier, respectively. and These are the gyroscope bias and accelerometer bias of the j-th carrier world coordinate system IMU, respectively; Establish constraints between carriers: in, The observation equation between the carrier in the k-th attitude and the i-th carrier is represented by χ = [x1, x2, ..., x]. n ], Let be the position of the i-th carrier in its own world coordinate system. Let j be the position of the j-th carrier in its own world coordinate system. Let d be the pose change matrix of carriers i and j. ij The distance between the tags on carriers i and j is measured. Each carrier detected the coordinates of the base station center in the image: in, and Let x and y be the x and y coordinates of j carriers in the visual sensor coordinate system, respectively.

6. The method for jointly initializing the location of an ultra-wideband base station according to claim 5, characterized in that: The visual distance calculation between each carrier and the detected ultra-wideband base station, and the matching of base station IDs, are achieved by minimizing the distance error: Where x is the paired base station ID, d x For the ranging data of the base station, This represents the difference between the carrier's location and the solved location of the ultra-wideband base station.

7. The method for jointly initializing the location of an ultra-wideband base station according to claim 1, characterized in that: The information broadcast by the ultra-wideband tag includes: ranging values ​​d from n base stations. n The distance measurement value between the current carrier and the nth carrier. The base station detected by the current carrier The corresponding ID base station's position in the j-th world coordinate system Other carriers obtain the location of the base station through a transformation matrix. accomplish: The relative pose relationship is determined by visual feature matching.

8. The method for jointly initializing the location of an ultra-wideband base station according to claim 1, characterized in that: The ranging residual is: in, The observation equation between the carrier and the j-th ultra-wideband base station is represented by the k-th attitude. This represents the location of the nth ultra-wideband base station in the current carrier world coordinate system. As the current posture of the carrier, Let be the extrinsic translation vector between the UWB tag and the IMU on the j-th carrier. Let K be the position of the current carrier in the k-th posture. The measured distance between the carrier and the nth UWB base station at that time; The carrier position deviation represents the time interval [t] from the camera image time to the UWB data time. k ,t j The IMU measurements within the [database] are used to predict the carrier position deviation: in, Let Δt be the carrier velocity. k,j The time offset corresponding to the time interval; g ω It is the acceleration due to gravity; This represents the IMU pre-integrated value within the time interval.

9. The method for jointly initializing the location of an ultra-wideband base station according to claim 1, characterized in that: Iterative optimization is performed using the Ceres function library. When the residual converges to the threshold, the optimization stops and the base station location is fixed.

10. A computer device / equipment / system, comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 9.

Images